diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2014-10-07 12:46:09 +0200 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2014-10-07 12:46:09 +0200 |
commit | 72fbd76c84f8ecfc6844b3d23fa90ce0c3bc1227 (patch) | |
tree | 2a4d15496cafe4b7642a962006b0734096bcf8cd | |
parent | 8d187cc2fa093c0784c330d2480a17b9d24c6ef1 (diff) | |
download | px4-firmware-72fbd76c84f8ecfc6844b3d23fa90ce0c3bc1227.tar.gz px4-firmware-72fbd76c84f8ecfc6844b3d23fa90ce0c3bc1227.tar.bz2 px4-firmware-72fbd76c84f8ecfc6844b3d23fa90ce0c3bc1227.zip |
Updated and fixed parser for SF02/F laser sensor, test harness runs clean
-rw-r--r-- | Tools/tests-host/.gitignore | 1 | ||||
-rw-r--r-- | Tools/tests-host/Makefile | 5 | ||||
-rwxr-xr-x | Tools/tests-host/sf0x_test | bin | 9228 -> 0 bytes | |||
-rw-r--r-- | Tools/tests-host/sf0x_test.cpp | 137 | ||||
-rw-r--r-- | src/drivers/sf0x/sf0x_parser.cpp | 143 | ||||
-rw-r--r-- | src/drivers/sf0x/sf0x_parser.h | 51 |
6 files changed, 222 insertions, 115 deletions
diff --git a/Tools/tests-host/.gitignore b/Tools/tests-host/.gitignore index 87b314c61..ec11e0fec 100644 --- a/Tools/tests-host/.gitignore +++ b/Tools/tests-host/.gitignore @@ -1,4 +1,5 @@ ./obj/* mixer_test +sf0x_test sbus2_test autodeclination_test diff --git a/Tools/tests-host/Makefile b/Tools/tests-host/Makefile index b1547c23f..903c3a4ca 100644 --- a/Tools/tests-host/Makefile +++ b/Tools/tests-host/Makefile @@ -22,9 +22,10 @@ SBUS2_FILES=../../src/modules/px4iofirmware/sbus.c \ SF0X_FILES= \ hrt.cpp \ - sf0x_test.cpp + sf0x_test.cpp \ + ../../src/drivers/sf0x/sf0x_parser.cpp -AUTODECLINATION_FILES= ../../src/lib/geo/geo_mag_declination.c \ +AUTODECLINATION_FILES= ../../src/lib/geo_lookup/geo_mag_declination.c \ hrt.cpp \ autodeclination_test.cpp diff --git a/Tools/tests-host/sf0x_test b/Tools/tests-host/sf0x_test Binary files differdeleted file mode 100755 index e56c35465..000000000 --- a/Tools/tests-host/sf0x_test +++ /dev/null diff --git a/Tools/tests-host/sf0x_test.cpp b/Tools/tests-host/sf0x_test.cpp index d41f7e54a..ebc893b60 100644 --- a/Tools/tests-host/sf0x_test.cpp +++ b/Tools/tests-host/sf0x_test.cpp @@ -1,152 +1,63 @@ -#include <stdio.h> #include <unistd.h> #include <string.h> #include <stdlib.h> +#include <stdio.h> #include <systemlib/err.h> #include <drivers/drv_hrt.h> -enum SF0X_PARSE_STATE { - SF0X_PARSE_STATE0_UNSYNC = 0, - SF0X_PARSE_STATE1_SYNC, - SF0X_PARSE_STATE2_GOT_DIGIT0, - SF0X_PARSE_STATE3_GOT_DOT, - SF0X_PARSE_STATE4_GOT_DIGIT1, - SF0X_PARSE_STATE5_GOT_DIGIT2, - SF0X_PARSE_STATE6_GOT_CARRIAGE_RETURN -}; - -int sf0x_parser(char c, char *parserbuf, unsigned *parserbuf_index, enum *SF0X_PARSE_STATE state, float *dist) -{ - int ret = -1; - - unsigned len = strlen(parserbuf); - switch (state) { - case SF0X_PARSE_STATE0_UNSYNC: - if (c == '\n') { - *state = SF0X_PARSE_STATE1_SYNC; - *parserbuf_index = 0; - } - break; - - case SF0X_PARSE_STATE1_SYNC: - if (c >= '0' && c <= '9') { - *state = SF0X_PARSE_STATE2_GOT_DIGIT0; - parserbuf[parserbuf_index] = c; - parserbuf_index++; - } - break; - - case SF0X_PARSE_STATE2_GOT_DIGIT0: - if (c >= '0' && c <= '9') { - *state = SF0X_PARSE_STATE2_GOT_DIGIT0; - parserbuf[parserbuf_index] = c; - parserbuf_index++; - } else if (c == '.') { - *state = SF0X_PARSE_STATE3_GOT_DOT; - parserbuf[parserbuf_index] = c; - parserbuf_index++; - } else { - *state = SF0X_PARSE_STATE0_UNSYNC; - } - break; - - case SF0X_PARSE_STATE3_GOT_DOT: - if (c >= '0' && c <= '9') { - *state = SF0X_PARSE_STATE4_GOT_DIGIT1; - parserbuf[parserbuf_index] = c; - parserbuf_index++; - } else { - *state = SF0X_PARSE_STATE0_UNSYNC; - } - break; - - case SF0X_PARSE_STATE4_GOT_DIGIT1: - if (c >= '0' && c <= '9') { - *state = SF0X_PARSE_STATE4_GOT_DIGIT2; - parserbuf[parserbuf_index] = c; - parserbuf_index++; - } else { - *state = SF0X_PARSE_STATE0_UNSYNC; - } - break; - - case SF0X_PARSE_STATE5_GOT_DIGIT2: - if (c == '\r') { - *state = SF0X_PARSE_STATE6_GOT_CARRIAGE_RETURN; - } else { - *state = SF0X_PARSE_STATE0_UNSYNC; - } - break; - - case SF0X_PARSE_STATE6_GOT_CARRIAGE_RETURN: - if (c == '\n') { - parserbuf[parserbuf_index] = '\0'; - *dist = strtod(linebuf, &end); - *state = SF0X_PARSE_STATE0_SYNC; - *parserbuf_index = 0; - ret = 0; - } else { - *state = SF0X_PARSE_STATE0_UNSYNC; - } - break; - } - - - - return ret; -} +#include <drivers/sf0x/sf0x_parser.h> int main(int argc, char *argv[]) { warnx("SF0X test started"); - if (argc < 2) - errx(1, "Need a filename for the input file"); - - warnx("loading data from: %s", argv[1]); - - FILE *fp; - - fp = fopen(argv[1],"rt"); - - if (!fp) - errx(1, "failed opening file"); - int ret = 0; const char LINE_MAX = 20; char _linebuf[LINE_MAX]; _linebuf[0] = '\0'; - char *end; + const char *lines[] = {"0.01\r\n", + "0.02\r\n", + "0.03\r\n", + "0.04\r\n", + "0", + ".", + "0", + "5", + "\r", + "\n", + "0", + "3\r", + "\n" + "\r\n", + "0.06", + "\r\n" + }; enum SF0X_PARSE_STATE state = SF0X_PARSE_STATE0_UNSYNC; float dist_m; char _parserbuf[LINE_MAX]; unsigned _parsebuf_index = 0; - while (fgets(_linebuf, LINE_MAX, fp) != NULL) { + for (unsigned l = 0; l < sizeof(lines) / sizeof(lines[0]); l++) { printf("\n%s", _linebuf); int parse_ret; - for (int i = 0; i < strlen(_linebuf); i++) + for (int i = 0; i < strlen(lines[l]); i++) { - printf("%0x ", _linebuf[i]); - parse_ret = sf0x_parser(_linebuf[i], _parserbuf, &_parsebuf_index, &state, &dist_m); + parse_ret = sf0x_parser(lines[l][i], _parserbuf, &_parsebuf_index, &state, &dist_m); if (parse_ret == 0) { - printf("PARSED!"); + printf("\nparsed: %f %s\n", dist_m, (parse_ret == 0) ? "OK" : ""); } } + printf("%s", lines[l]); - printf("\nparsed: %f %s\n", dist_m, (parse_ret == 0) ? "OK" : ""); } - // Init the parser - - if (ret == EOF) { warnx("Test finished, reached end of file"); } else { diff --git a/src/drivers/sf0x/sf0x_parser.cpp b/src/drivers/sf0x/sf0x_parser.cpp new file mode 100644 index 000000000..b9ceb52f5 --- /dev/null +++ b/src/drivers/sf0x/sf0x_parser.cpp @@ -0,0 +1,143 @@ +/**************************************************************************** + * + * Copyright (c) 2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file sf0x_parser.cpp + * @author Lorenz Meier <lm@inf.ethz.ch> + * + * Driver for the Lightware SF0x laser rangefinder series + */ + +#include "sf0x_parser.h" +#include <string.h> +#include <stdlib.h> + +//#define SF0X_DEBUG + +#ifdef SF0X_DEBUG +#include <stdio.h> + +const char* parser_state[] = { + "0_UNSYNC", + "1_SYNC", + "2_GOT_DIGIT0", + "3_GOT_DOT", + "4_GOT_DIGIT1", + "5_GOT_DIGIT2", + "6_GOT_CARRIAGE_RETURN" +}; +#endif + +int sf0x_parser(char c, char *parserbuf, unsigned *parserbuf_index, enum SF0X_PARSE_STATE *state, float *dist) +{ + int ret = -1; + char *end; + + unsigned len = strlen(parserbuf); + switch (*state) { + case SF0X_PARSE_STATE0_UNSYNC: + if (c == '\n') { + *state = SF0X_PARSE_STATE1_SYNC; + (*parserbuf_index) = 0; + } + break; + + case SF0X_PARSE_STATE1_SYNC: + if (c >= '0' && c <= '9') { + *state = SF0X_PARSE_STATE2_GOT_DIGIT0; + parserbuf[*parserbuf_index] = c; + (*parserbuf_index)++; + } + break; + + case SF0X_PARSE_STATE2_GOT_DIGIT0: + if (c >= '0' && c <= '9') { + *state = SF0X_PARSE_STATE2_GOT_DIGIT0; + parserbuf[*parserbuf_index] = c; + (*parserbuf_index)++; + } else if (c == '.') { + *state = SF0X_PARSE_STATE3_GOT_DOT; + parserbuf[*parserbuf_index] = c; + (*parserbuf_index)++; + } else { + *state = SF0X_PARSE_STATE0_UNSYNC; + } + break; + + case SF0X_PARSE_STATE3_GOT_DOT: + if (c >= '0' && c <= '9') { + *state = SF0X_PARSE_STATE4_GOT_DIGIT1; + parserbuf[*parserbuf_index] = c; + (*parserbuf_index)++; + } else { + *state = SF0X_PARSE_STATE0_UNSYNC; + } + break; + + case SF0X_PARSE_STATE4_GOT_DIGIT1: + if (c >= '0' && c <= '9') { + *state = SF0X_PARSE_STATE5_GOT_DIGIT2; + parserbuf[*parserbuf_index] = c; + (*parserbuf_index)++; + } else { + *state = SF0X_PARSE_STATE0_UNSYNC; + } + break; + + case SF0X_PARSE_STATE5_GOT_DIGIT2: + if (c == '\r') { + *state = SF0X_PARSE_STATE6_GOT_CARRIAGE_RETURN; + } else { + *state = SF0X_PARSE_STATE0_UNSYNC; + } + break; + + case SF0X_PARSE_STATE6_GOT_CARRIAGE_RETURN: + if (c == '\n') { + parserbuf[*parserbuf_index] = '\0'; + *dist = strtod(parserbuf, &end); + *state = SF0X_PARSE_STATE1_SYNC; + *parserbuf_index = 0; + ret = 0; + } else { + *state = SF0X_PARSE_STATE0_UNSYNC; + } + break; + } + +#ifdef SF0X_DEBUG + printf("state: SF0X_PARSE_STATE%s\n", parser_state[*state]); +#endif + + return ret; +}
\ No newline at end of file diff --git a/src/drivers/sf0x/sf0x_parser.h b/src/drivers/sf0x/sf0x_parser.h new file mode 100644 index 000000000..20892d50e --- /dev/null +++ b/src/drivers/sf0x/sf0x_parser.h @@ -0,0 +1,51 @@ +/**************************************************************************** + * + * Copyright (c) 2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file sf0x_parser.cpp + * @author Lorenz Meier <lm@inf.ethz.ch> + * + * Declarations of parser for the Lightware SF0x laser rangefinder series + */ + +enum SF0X_PARSE_STATE { + SF0X_PARSE_STATE0_UNSYNC = 0, + SF0X_PARSE_STATE1_SYNC, + SF0X_PARSE_STATE2_GOT_DIGIT0, + SF0X_PARSE_STATE3_GOT_DOT, + SF0X_PARSE_STATE4_GOT_DIGIT1, + SF0X_PARSE_STATE5_GOT_DIGIT2, + SF0X_PARSE_STATE6_GOT_CARRIAGE_RETURN +}; + +int sf0x_parser(char c, char *parserbuf, unsigned *parserbuf_index, enum SF0X_PARSE_STATE *state, float *dist);
\ No newline at end of file |