Updated and fixed parser for SF02/F laser sensor, test harness runs clean

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
deleted 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