diff options
Diffstat (limited to 'Tools')
-rw-r--r-- | Tools/tests-host/Makefile | 11 | ||||
-rw-r--r-- | Tools/tests-host/sf0x_data.txt | 33 | ||||
-rwxr-xr-x | Tools/tests-host/sf0x_test | bin | 0 -> 9228 bytes | |||
-rw-r--r-- | Tools/tests-host/sf0x_test.cpp | 157 |
4 files changed, 199 insertions, 2 deletions
diff --git a/Tools/tests-host/Makefile b/Tools/tests-host/Makefile index f0737ef88..b1547c23f 100644 --- a/Tools/tests-host/Makefile +++ b/Tools/tests-host/Makefile @@ -3,7 +3,7 @@ CC=g++ CFLAGS=-I. -I../../src/modules -I ../../src/include -I../../src/drivers \ -I../../src -I../../src/lib -D__EXPORT="" -Dnullptr="0" -lm -all: mixer_test sbus2_test autodeclination_test +all: mixer_test sbus2_test autodeclination_test sf0x_test MIXER_FILES=../../src/systemcmds/tests/test_mixer.cpp \ ../../src/systemcmds/tests/test_conv.cpp \ @@ -20,6 +20,10 @@ SBUS2_FILES=../../src/modules/px4iofirmware/sbus.c \ hrt.cpp \ sbus2_test.cpp +SF0X_FILES= \ + hrt.cpp \ + sf0x_test.cpp + AUTODECLINATION_FILES= ../../src/lib/geo/geo_mag_declination.c \ hrt.cpp \ autodeclination_test.cpp @@ -30,10 +34,13 @@ mixer_test: $(MIXER_FILES) sbus2_test: $(SBUS2_FILES) $(CC) -o sbus2_test $(SBUS2_FILES) $(CFLAGS) +sf0x_test: $(SF0X_FILES) + $(CC) -o sf0x_test $(SF0X_FILES) $(CFLAGS) + autodeclination_test: $(SBUS2_FILES) $(CC) -o autodeclination_test $(AUTODECLINATION_FILES) $(CFLAGS) .PHONY: clean clean: - rm -f $(ODIR)/*.o *~ core $(INCDIR)/*~ mixer_test sbus2_test autodeclination_test
\ No newline at end of file + rm -f $(ODIR)/*.o *~ core $(INCDIR)/*~ mixer_test sbus2_test autodeclination_test sf0x_test
\ No newline at end of file diff --git a/Tools/tests-host/sf0x_data.txt b/Tools/tests-host/sf0x_data.txt new file mode 100644 index 000000000..f61f59523 --- /dev/null +++ b/Tools/tests-host/sf0x_data.txt @@ -0,0 +1,33 @@ +0.02
+0.25
+0.03
+0.02
+0.25
+0.03
+0.02
+0.25
+0.03
+0.02
+0.25
+0.03
+0.25
+0.03
+0.02
+0.25
+0.03
+0.08
+0.40
+0.44
+
+
+0
+.
+0
+1
+
+0
+0.
+.0
+.01
+01
+1
\ No newline at end of file diff --git a/Tools/tests-host/sf0x_test b/Tools/tests-host/sf0x_test Binary files differnew file mode 100755 index 000000000..e56c35465 --- /dev/null +++ b/Tools/tests-host/sf0x_test diff --git a/Tools/tests-host/sf0x_test.cpp b/Tools/tests-host/sf0x_test.cpp new file mode 100644 index 000000000..d41f7e54a --- /dev/null +++ b/Tools/tests-host/sf0x_test.cpp @@ -0,0 +1,157 @@ + +#include <stdio.h> +#include <unistd.h> +#include <string.h> +#include <stdlib.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; +} + +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; + + 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) { + + printf("\n%s", _linebuf); + + int parse_ret; + + for (int i = 0; i < strlen(_linebuf); i++) + { + printf("%0x ", _linebuf[i]); + parse_ret = sf0x_parser(_linebuf[i], _parserbuf, &_parsebuf_index, &state, &dist_m); + + if (parse_ret == 0) { + printf("PARSED!"); + } + } + + 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 { + warnx("Test aborted, errno: %d", ret); + } + + return ret; +} |