WIP on laser driver unit test

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
new 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;
+}