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 /Tools/tests-host/sf0x_test.cpp | |
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
Diffstat (limited to 'Tools/tests-host/sf0x_test.cpp')
-rw-r--r-- | Tools/tests-host/sf0x_test.cpp | 137 |
1 files changed, 24 insertions, 113 deletions
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 { |