Merge pull request #876 from PX4/autodeclination

Added automatic declination lookup

8 files changed, 143 insertions, 35 deletions

diff --git a/Tools/tests-host/.gitignore b/Tools/tests-host/.gitignore
index 61e091551..87b314c61 100644
--- a/Tools/tests-host/.gitignore
+++ b/Tools/tests-host/.gitignore
@@ -1,2 +1,4 @@
 ./obj/*
 mixer_test
+sbus2_test
+autodeclination_test
diff --git a/Tools/tests-host/Makefile b/Tools/tests-host/Makefile
index 7ab1454f0..f0737ef88 100644
--- a/Tools/tests-host/Makefile
+++ b/Tools/tests-host/Makefile
@@ -1,47 +1,39 @@
 
 CC=g++
-CFLAGS=-I. -I../../src/modules -I ../../src/include -I../../src/drivers -I../../src -D__EXPORT="" -Dnullptr="0"
+CFLAGS=-I. -I../../src/modules -I ../../src/include -I../../src/drivers \
+	-I../../src -I../../src/lib -D__EXPORT="" -Dnullptr="0" -lm
 
-ODIR=obj
-LDIR =../lib
+all: mixer_test sbus2_test autodeclination_test
 
-LIBS=-lm
+MIXER_FILES=../../src/systemcmds/tests/test_mixer.cpp \
+		../../src/systemcmds/tests/test_conv.cpp \
+		../../src/modules/systemlib/mixer/mixer_simple.cpp \
+		../../src/modules/systemlib/mixer/mixer_multirotor.cpp \
+		../../src/modules/systemlib/mixer/mixer.cpp \
+		../../src/modules/systemlib/mixer/mixer_group.cpp \
+		../../src/modules/systemlib/mixer/mixer_load.c \
+		../../src/modules/systemlib/pwm_limit/pwm_limit.c \
+		hrt.cpp \
+		mixer_test.cpp
 
-#_DEPS = test.h
-#DEPS = $(patsubst %,$(IDIR)/%,$(_DEPS))
+SBUS2_FILES=../../src/modules/px4iofirmware/sbus.c \
+		hrt.cpp \
+		sbus2_test.cpp
 
-_OBJ = mixer_test.o test_mixer.o mixer_simple.o mixer_multirotor.o \
-	mixer.o mixer_group.o mixer_load.o test_conv.o pwm_limit.o hrt.o
-OBJ = $(patsubst %,$(ODIR)/%,$(_OBJ))
+AUTODECLINATION_FILES= ../../src/lib/geo/geo_mag_declination.c \
+		hrt.cpp \
+		autodeclination_test.cpp
 
-#$(DEPS)
-$(ODIR)/%.o: %.cpp
-	mkdir -p obj
-	$(CC) -c -o $@ $< $(CFLAGS)
+mixer_test: $(MIXER_FILES)
+	$(CC) -o mixer_test $(MIXER_FILES) $(CFLAGS)
 
-$(ODIR)/%.o: ../../src/systemcmds/tests/%.cpp
-	$(CC) -c -o $@ $< $(CFLAGS)
+sbus2_test: $(SBUS2_FILES)
+	$(CC) -o sbus2_test $(SBUS2_FILES) $(CFLAGS)
 
-$(ODIR)/%.o: ../../src/modules/systemlib/%.cpp
-	$(CC) -c -o $@ $< $(CFLAGS)
-
-$(ODIR)/%.o: ../../src/modules/systemlib/mixer/%.cpp
-	$(CC) -c -o $@ $< $(CFLAGS)
-
-$(ODIR)/%.o: ../../src/modules/systemlib/pwm_limit/%.cpp
-	$(CC) -c -o $@ $< $(CFLAGS)
-
-$(ODIR)/%.o: ../../src/modules/systemlib/pwm_limit/%.c
-	$(CC) -c -o $@ $< $(CFLAGS)
-
-$(ODIR)/%.o: ../../src/modules/systemlib/mixer/%.c
-	$(CC) -c -o $@ $< $(CFLAGS)
-
-#
-mixer_test: $(OBJ)
-	g++ -o $@ $^ $(CFLAGS) $(LIBS)
+autodeclination_test: $(SBUS2_FILES)
+	$(CC) -o autodeclination_test $(AUTODECLINATION_FILES) $(CFLAGS)
 
 .PHONY: clean
 
 clean:
-	rm -f $(ODIR)/*.o *~ core $(INCDIR)/*~
\ No newline at end of file
+	rm -f $(ODIR)/*.o *~ core $(INCDIR)/*~ mixer_test sbus2_test autodeclination_test
\ No newline at end of file
diff --git a/Tools/tests-host/autodeclination_test.cpp b/Tools/tests-host/autodeclination_test.cpp
new file mode 100644
index 000000000..93bc340bb
--- /dev/null
+++ b/Tools/tests-host/autodeclination_test.cpp
@@ -0,0 +1,28 @@
+
+#include <stdio.h>
+#include <stdlib.h> 
+#include <unistd.h>
+#include <string.h>
+#include <systemlib/mixer/mixer.h>
+#include <systemlib/err.h>
+#include <drivers/drv_hrt.h>
+#include <px4iofirmware/px4io.h>
+#include "../../src/systemcmds/tests/tests.h"
+#include <geo/geo.h>
+
+int main(int argc, char *argv[]) {
+	warnx("autodeclination test started");
+
+	if (argc < 3)
+		errx(1, "Need lat/lon!");
+
+	char* p_end;
+
+	float lat = strtod(argv[1], &p_end);
+	float lon = strtod(argv[2], &p_end);
+
+	float declination = get_mag_declination(lat, lon);
+
+	printf("lat: %f lon: %f, dec: %f\n", lat, lon, declination);
+
+}
diff --git a/Tools/tests-host/board_config.h b/Tools/tests-host/board_config.h
new file mode 100644
index 000000000..e69de29bb
--- /dev/null
+++ b/Tools/tests-host/board_config.h
diff --git a/Tools/tests-host/debug.h b/Tools/tests-host/debug.h
new file mode 100644
index 000000000..9824d13fc
--- /dev/null
+++ b/Tools/tests-host/debug.h
@@ -0,0 +1,5 @@
+
+#pragma once
+
+#include <systemlib/err.h>
+#define lowsyslog warnx
\ No newline at end of file
diff --git a/Tools/tests-host/mixer_test.cpp b/Tools/tests-host/mixer_test.cpp
index e311617f9..06499afd0 100644
--- a/Tools/tests-host/mixer_test.cpp
+++ b/Tools/tests-host/mixer_test.cpp
@@ -11,4 +11,4 @@ int main(int argc, char *argv[]) {
 	test_mixer(3, args);
 
 	test_conv(1, args);
-}
\ No newline at end of file
+}
diff --git a/Tools/tests-host/run_tests.sh b/Tools/tests-host/run_tests.sh
new file mode 100755
index 000000000..ff5ee509a
--- /dev/null
+++ b/Tools/tests-host/run_tests.sh
@@ -0,0 +1,6 @@
+#!/bin/sh
+
+make clean
+make all
+./mixer_test
+./sbus2_test ../../../../data/sbus2/sbus2_r7008SB_gps_baro_tx_off.txt
\ No newline at end of file
diff --git a/Tools/tests-host/sbus2_test.cpp b/Tools/tests-host/sbus2_test.cpp
new file mode 100644
index 000000000..d8fcb695d
--- /dev/null
+++ b/Tools/tests-host/sbus2_test.cpp
@@ -0,0 +1,75 @@
+
+#include <stdio.h>
+#include <unistd.h>
+#include <string.h>
+#include <systemlib/mixer/mixer.h>
+#include <systemlib/err.h>
+#include <drivers/drv_hrt.h>
+#include <px4iofirmware/px4io.h>
+#include "../../src/systemcmds/tests/tests.h"
+
+int main(int argc, char *argv[]) {
+	warnx("SBUS2 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");
+
+	float f;
+	unsigned x;
+	int ret;
+
+	// Trash the first 20 lines
+	for (unsigned i = 0; i < 20; i++) {
+		(void)fscanf(fp, "%f,%x,,", &f, &x);
+	}
+
+	// Init the parser
+	uint8_t frame[30];
+	unsigned partial_frame_count = 0;
+	uint16_t rc_values[18];
+	uint16_t num_values;
+	bool sbus_failsafe;
+	bool sbus_frame_drop;
+	uint16_t max_channels = sizeof(rc_values) / sizeof(rc_values[0]);
+
+	float last_time = 0;
+
+	while (EOF != (ret = fscanf(fp, "%f,%x,,", &f, &x))) {
+		if (((f - last_time) * 1000 * 1000) > 3000) {
+			partial_frame_count = 0;
+			warnx("FRAME RESET\n\n");
+		}
+
+		frame[partial_frame_count] = x;
+		partial_frame_count++;
+
+		//warnx("%f: 0x%02x, first: 0x%02x, last: 0x%02x, pcount: %u", (double)f, x, frame[0], frame[24], partial_frame_count);
+
+		if (partial_frame_count == sizeof(frame))
+			partial_frame_count = 0;
+
+		last_time = f;
+
+		// Pipe the data into the parser
+		hrt_abstime now = hrt_absolute_time();
+
+		//if (partial_frame_count % 25 == 0)
+			//sbus_parse(now, frame, &partial_frame_count, rc_values, &num_values, &sbus_failsafe, &sbus_frame_drop, max_channels);
+	}
+
+	if (ret == EOF) {
+		warnx("Test finished, reached end of file");
+	} else {
+		warnx("Test aborted, errno: %d", ret);
+	}
+
+}