From e134537ae8f892ba3ae4a2f9c771bcfa62f905c8 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 28 Apr 2014 17:44:29 +0200 Subject: [PATCH 01/13] Added automatic declination lookup --- src/lib/geo/geo.h | 2 + src/lib/geo/geo_mag_declination.c | 135 ++++++++++++++++++++++++++++++ src/lib/geo/geo_mag_declination.h | 47 +++++++++++ src/lib/geo/module.mk | 3 +- 4 files changed, 186 insertions(+), 1 deletion(-) create mode 100644 src/lib/geo/geo_mag_declination.c create mode 100644 src/lib/geo/geo_mag_declination.h diff --git a/src/lib/geo/geo.h b/src/lib/geo/geo.h index 0a3f85d970..d987afe337 100644 --- a/src/lib/geo/geo.h +++ b/src/lib/geo/geo.h @@ -48,6 +48,8 @@ #include "uORB/topics/fence.h" #include "uORB/topics/vehicle_global_position.h" +#include "geo/geo_mag_declination.h" + __BEGIN_DECLS #include diff --git a/src/lib/geo/geo_mag_declination.c b/src/lib/geo/geo_mag_declination.c new file mode 100644 index 0000000000..7b4aa69a2f --- /dev/null +++ b/src/lib/geo/geo_mag_declination.c @@ -0,0 +1,135 @@ +/**************************************************************************** + * + * Copyright (c) 2014 MAV GEO Library (MAVGEO). 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 MAVGEO 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 geo_mag_declination.c +* +* Calculation / lookup table for earth magnetic field declination. +* +* Lookup table from Scott Ferguson +* +* XXX Lookup table currently too coarse in resolution (only full degrees) +* and lat/lon res - needs extension medium term. +* +*/ + + +/** set this always to the sampling in degrees for the table below */ +#define SAMPLING_RES 10 +#define SAMPLING_MIN_LAT -60.0f +#define SAMPLING_MAX_LAT 60.0f +#define SAMPLING_MIN_LON -180.0f +#define SAMPLING_MAX_LON 180.0f + +static const char declination_table[13][37] = \ +{ + 46, 45, 44, 42, 41, 40, 38, 36, 33, 28, 23, 16, 10, 4, -1, -5, -9, -14, -19, -26, -33, -40, -48, -55, -61, \ + -66, -71, -74, -75, -72, -61, -25, 22, 40, 45, 47, 46, 30, 30, 30, 30, 29, 29, 29, 29, 27, 24, 18, 11, 3, \ + -3, -9, -12, -15, -17, -21, -26, -32, -39, -45, -51, -55, -57, -56, -53, -44, -31, -14, 0, 13, 21, 26, \ + 29, 30, 21, 22, 22, 22, 22, 22, 22, 22, 21, 18, 13, 5, -3, -11, -17, -20, -21, -22, -23, -25, -29, -35, \ + -40, -44, -45, -44, -40, -32, -22, -12, -3, 3, 9, 14, 18, 20, 21, 16, 17, 17, 17, 17, 17, 16, 16, 16, 13, \ + 8, 0, -9, -16, -21, -24, -25, -25, -23, -20, -21, -24, -28, -31, -31, -29, -24, -17, -9, -3, 0, 4, 7, \ + 10, 13, 15, 16, 12, 13, 13, 13, 13, 13, 12, 12, 11, 9, 3, -4, -12, -19, -23, -24, -24, -22, -17, -12, -9, \ + -10, -13, -17, -18, -16, -13, -8, -3, 0, 1, 3, 6, 8, 10, 12, 12, 10, 10, 10, 10, 10, 10, 10, 9, 9, 6, 0, -6, \ + -14, -20, -22, -22, -19, -15, -10, -6, -2, -2, -4, -7, -8, -8, -7, -4, 0, 1, 1, 2, 4, 6, 8, 10, 10, 9, 9, 9, \ + 9, 9, 9, 8, 8, 7, 4, -1, -8, -15, -19, -20, -18, -14, -9, -5, -2, 0, 1, 0, -2, -3, -4, -3, -2, 0, 0, 0, 1, 3, 5, \ + 7, 8, 9, 8, 8, 8, 9, 9, 9, 8, 8, 6, 2, -3, -9, -15, -18, -17, -14, -10, -6, -2, 0, 1, 2, 2, 0, -1, -1, -2, -1, 0, \ + 0, 0, 0, 1, 3, 5, 7, 8, 8, 9, 9, 10, 10, 10, 10, 8, 5, 0, -5, -11, -15, -16, -15, -12, -8, -4, -1, 0, 2, 3, 2, 1, 0, \ + 0, 0, 0, 0, -1, -2, -2, -1, 0, 3, 6, 8, 6, 9, 10, 11, 12, 12, 11, 9, 5, 0, -7, -12, -15, -15, -13, -10, -7, -3, \ + 0, 1, 2, 3, 3, 3, 2, 1, 0, 0, -1, -3, -4, -5, -5, -2, 0, 3, 6, 5, 8, 11, 13, 15, 15, 14, 11, 5, -1, -9, -14, -17, \ + -16, -14, -11, -7, -3, 0, 1, 3, 4, 5, 5, 5, 4, 3, 1, -1, -4, -7, -8, -8, -6, -2, 1, 5, 4, 8, 12, 15, 17, 18, 16, \ + 12, 5, -3, -12, -18, -20, -19, -16, -13, -8, -4, -1, 1, 4, 6, 8, 9, 9, 9, 7, 3, -1, -6, -10, -12, -11, -9, -5, \ + 0, 4, 3, 9, 14, 17, 20, 21, 19, 14, 4, -8, -19, -25, -26, -25, -21, -17, -12, -7, -2, 1, 5, 9, 13, 15, 16, 16, \ + 13, 7, 0, -7, -12, -15, -14, -11, -6, -1, 3 +}; + +static float get_lookup_table_val(unsigned lat, unsigned lon); + +__EXPORT float get_mag_declination(float lat, float lon) +{ + /* + * If the values exceed valid ranges, return zero as default + * as we have no way of knowing what the closest real value + * would be. + */ + if (lat < -90.0f || lat > 90.0f || + lon < -180.0f || lon > 180.0f) { + return 0.0f; + } + + /* round down to nearest sampling resolution */ + int min_lat = (((int)lat) / SAMPLING_RES) * SAMPLING_RES; + int min_lon = (((int)lon) / SAMPLING_RES) * SAMPLING_RES; + + /* for the rare case of hitting the bounds exactly + * the rounding logic wouldn't fit, so enforce it. + */ + + /* limit to table bounds - required for maxima even when table spans full globe range */ + if (lat <= SAMPLING_MIN_LAT) { + min_lat = SAMPLING_MIN_LAT; + } + + if (lat >= SAMPLING_MAX_LAT) { + min_lat = (((int)lat) / SAMPLING_RES) * SAMPLING_RES - SAMPLING_RES; + } + + if (lon <= SAMPLING_MIN_LON) { + min_lon = SAMPLING_MIN_LON; + } + + if (lon >= SAMPLING_MAX_LON) { + min_lon = (((int)lon) / SAMPLING_RES) * SAMPLING_RES - SAMPLING_RES; + } + + /* find index of nearest low sampling point */ + unsigned min_lat_index = (90 + min_lat) / SAMPLING_RES; + unsigned min_lon_index = (180 + min_lat) / SAMPLING_RES; + + float declination_sw = get_lookup_table_val(min_lat_index, min_lon_index); + float declination_se = get_lookup_table_val(min_lat_index, min_lon_index + 1); + float declination_ne = get_lookup_table_val(min_lat_index + 1, min_lon_index + 1); + float declination_nw = get_lookup_table_val(min_lat_index + 1, min_lon_index); + + /* perform bilinear interpolation on the four grid corners */ + + float declination_min = ((lon - min_lon) / SAMPLING_RES) * (declination_se - declination_sw) + declination_sw; + float declination_max = ((lon - min_lon) / SAMPLING_RES) * (declination_ne - declination_nw) + declination_nw; + + return (lat - min_lat) / SAMPLING_RES * (declination_max - declination_min) + declination_min; +} + +float get_lookup_table_val(unsigned lat_index, unsigned lon_index) +{ + return declination_table[lat_index][lon_index]; +} \ No newline at end of file diff --git a/src/lib/geo/geo_mag_declination.h b/src/lib/geo/geo_mag_declination.h new file mode 100644 index 0000000000..0ac062d6d4 --- /dev/null +++ b/src/lib/geo/geo_mag_declination.h @@ -0,0 +1,47 @@ +/**************************************************************************** + * + * Copyright (c) 2014 MAV GEO Library (MAVGEO). 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 MAVGEO 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 geo_mag_declination.h +* +* Calculation / lookup table for earth magnetic field declination. +* +*/ + +#pragma once + +__BEGIN_DECLS + +__EXPORT float get_mag_declination(float lat, float lon); + +__END_DECLS diff --git a/src/lib/geo/module.mk b/src/lib/geo/module.mk index 30a2dc99fd..9500a2bccf 100644 --- a/src/lib/geo/module.mk +++ b/src/lib/geo/module.mk @@ -35,4 +35,5 @@ # Geo library # -SRCS = geo.c +SRCS = geo.c \ + geo_mag_declination.c From c179863b1e35628a75efef624d2df0b0e85ad923 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 23 Feb 2014 07:20:54 -0800 Subject: [PATCH 02/13] Improve testing infrastructure for mixer and SBUS2 --- Tools/tests-host/.gitignore | 1 + Tools/tests-host/Makefile | 57 +++++++++---------------- Tools/tests-host/board_config.h | 0 Tools/tests-host/debug.h | 5 +++ Tools/tests-host/run_tests.sh | 6 +++ Tools/tests-host/sbus2_test.cpp | 75 +++++++++++++++++++++++++++++++++ 6 files changed, 108 insertions(+), 36 deletions(-) create mode 100644 Tools/tests-host/board_config.h create mode 100644 Tools/tests-host/debug.h create mode 100755 Tools/tests-host/run_tests.sh create mode 100644 Tools/tests-host/sbus2_test.cpp diff --git a/Tools/tests-host/.gitignore b/Tools/tests-host/.gitignore index 61e0915515..1618faf581 100644 --- a/Tools/tests-host/.gitignore +++ b/Tools/tests-host/.gitignore @@ -1,2 +1,3 @@ ./obj/* mixer_test +sbus2_test \ No newline at end of file diff --git a/Tools/tests-host/Makefile b/Tools/tests-host/Makefile index 7ab1454f01..15ccf15945 100644 --- a/Tools/tests-host/Makefile +++ b/Tools/tests-host/Makefile @@ -1,47 +1,32 @@ 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 -D__EXPORT="" -Dnullptr="0" -lm -ODIR=obj -LDIR =../lib +all: mixer_test sbus2_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)) +mixer_test: $(MIXER_FILES) + $(CC) -o mixer_test $(MIXER_FILES) $(CFLAGS) -#$(DEPS) -$(ODIR)/%.o: %.cpp - mkdir -p obj - $(CC) -c -o $@ $< $(CFLAGS) - -$(ODIR)/%.o: ../../src/systemcmds/tests/%.cpp - $(CC) -c -o $@ $< $(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) +sbus2_test: $(SBUS2_FILES) + $(CC) -o sbus2_test $(SBUS2_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 \ No newline at end of file diff --git a/Tools/tests-host/board_config.h b/Tools/tests-host/board_config.h new file mode 100644 index 0000000000..e69de29bb2 diff --git a/Tools/tests-host/debug.h b/Tools/tests-host/debug.h new file mode 100644 index 0000000000..9824d13fc3 --- /dev/null +++ b/Tools/tests-host/debug.h @@ -0,0 +1,5 @@ + +#pragma once + +#include +#define lowsyslog warnx \ 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 0000000000..ff5ee509ac --- /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 0000000000..e3b0122267 --- /dev/null +++ b/Tools/tests-host/sbus2_test.cpp @@ -0,0 +1,75 @@ + +#include +#include +#include +#include +#include +#include +#include +#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); + } + +} \ No newline at end of file From db304910510ed2ae8c262097fa6e8df1db965d5e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 28 Apr 2014 19:20:14 +0200 Subject: [PATCH 03/13] Add missing newline --- Tools/tests-host/.gitignore | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Tools/tests-host/.gitignore b/Tools/tests-host/.gitignore index 1618faf581..a6ce91d694 100644 --- a/Tools/tests-host/.gitignore +++ b/Tools/tests-host/.gitignore @@ -1,3 +1,3 @@ ./obj/* mixer_test -sbus2_test \ No newline at end of file +sbus2_test From 002ff7da7e792010c4eba5903075af83cb1ebd3e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 23 Feb 2014 07:21:13 -0800 Subject: [PATCH 04/13] Add missing header in HRT --- src/drivers/drv_hrt.h | 1 + 1 file changed, 1 insertion(+) diff --git a/src/drivers/drv_hrt.h b/src/drivers/drv_hrt.h index d130d68b34..8bfc90c64b 100644 --- a/src/drivers/drv_hrt.h +++ b/src/drivers/drv_hrt.h @@ -41,6 +41,7 @@ #include #include +#include #include #include From 3959d0c1c9cf32ac1a2fa4df63fa0f0f77cc17a5 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 28 Apr 2014 19:24:44 +0200 Subject: [PATCH 05/13] Disable sbus2_test for now, just leverage the testing infrastructure --- Tools/tests-host/sbus2_test.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Tools/tests-host/sbus2_test.cpp b/Tools/tests-host/sbus2_test.cpp index e3b0122267..134a71b809 100644 --- a/Tools/tests-host/sbus2_test.cpp +++ b/Tools/tests-host/sbus2_test.cpp @@ -63,7 +63,7 @@ int main(int argc, char *argv[]) { 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); + //sbus_parse(now, frame, &partial_frame_count, rc_values, &num_values, &sbus_failsafe, &sbus_frame_drop, max_channels); } if (ret == EOF) { From da525f29f13e5bdae717fbf189c5a3b4ab46794c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 28 Apr 2014 19:25:13 +0200 Subject: [PATCH 06/13] Add missing header in mixer load command --- src/modules/systemlib/mixer/mixer_load.c | 1 + 1 file changed, 1 insertion(+) diff --git a/src/modules/systemlib/mixer/mixer_load.c b/src/modules/systemlib/mixer/mixer_load.c index b05273c0de..bf3428a50b 100644 --- a/src/modules/systemlib/mixer/mixer_load.c +++ b/src/modules/systemlib/mixer/mixer_load.c @@ -41,6 +41,7 @@ #include #include #include +#include #include "mixer_load.h" From 238a3636faae13c4ba52ab9581a305c7a069276e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 28 Apr 2014 20:29:13 +0200 Subject: [PATCH 07/13] Add autodeclination --- Tools/tests-host/.gitignore | 1 + 1 file changed, 1 insertion(+) diff --git a/Tools/tests-host/.gitignore b/Tools/tests-host/.gitignore index a6ce91d694..87b314c614 100644 --- a/Tools/tests-host/.gitignore +++ b/Tools/tests-host/.gitignore @@ -1,3 +1,4 @@ ./obj/* mixer_test sbus2_test +autodeclination_test From 7aefcb7a09a12fae2dcbe2bc2360948aefbb66a4 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 28 Apr 2014 20:29:45 +0200 Subject: [PATCH 08/13] Add autodeclination testing - no-op right now --- Tools/tests-host/Makefile | 11 +++++++++-- Tools/tests-host/autodeclination_test.cpp | 19 +++++++++++++++++++ Tools/tests-host/sbus2_test.cpp | 2 +- 3 files changed, 29 insertions(+), 3 deletions(-) create mode 100644 Tools/tests-host/autodeclination_test.cpp diff --git a/Tools/tests-host/Makefile b/Tools/tests-host/Makefile index 15ccf15945..fd001e4d7f 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 -D__EXPORT="" -Dnullptr="0" -lm -all: mixer_test sbus2_test +all: mixer_test sbus2_test autodeclination_test MIXER_FILES=../../src/systemcmds/tests/test_mixer.cpp \ ../../src/systemcmds/tests/test_conv.cpp \ @@ -20,13 +20,20 @@ SBUS2_FILES=../../src/modules/px4iofirmware/sbus.c \ hrt.cpp \ sbus2_test.cpp +AUTODECLINATION_FILES=../../src/lib/geo/geo_mag_declination.c \ + hrt.cpp \ + autodeclination_test.cpp + mixer_test: $(MIXER_FILES) $(CC) -o mixer_test $(MIXER_FILES) $(CFLAGS) sbus2_test: $(SBUS2_FILES) $(CC) -o sbus2_test $(SBUS2_FILES) $(CFLAGS) +autodeclination_test: $(SBUS2_FILES) + $(CC) -o autodeclination_test $(SBUS2_FILES) $(CFLAGS) + .PHONY: clean clean: - rm -f $(ODIR)/*.o *~ core $(INCDIR)/*~ mixer_test sbus2_test \ 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 0000000000..b1b30f5d97 --- /dev/null +++ b/Tools/tests-host/autodeclination_test.cpp @@ -0,0 +1,19 @@ + +#include +#include +#include +#include +#include +#include +#include +#include "../../src/systemcmds/tests/tests.h" + +int main(int argc, char *argv[]) { + warnx("autodeclination test started"); + + if (argc < 3) + errx(1, "Need lat/lon!"); + + + +} \ No newline at end of file diff --git a/Tools/tests-host/sbus2_test.cpp b/Tools/tests-host/sbus2_test.cpp index 134a71b809..281903cf68 100644 --- a/Tools/tests-host/sbus2_test.cpp +++ b/Tools/tests-host/sbus2_test.cpp @@ -62,7 +62,7 @@ int main(int argc, char *argv[]) { // Pipe the data into the parser hrt_abstime now = hrt_absolute_time(); - if (partial_frame_count % 25 == 0) + //if (partial_frame_count % 25 == 0) //sbus_parse(now, frame, &partial_frame_count, rc_values, &num_values, &sbus_failsafe, &sbus_frame_drop, max_channels); } From 9c81ab113e73c8862cc3f4e81411cb69dbec14ee Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 28 Apr 2014 20:42:46 +0200 Subject: [PATCH 09/13] Updated outo-test --- Tools/tests-host/Makefile | 6 +++--- Tools/tests-host/autodeclination_test.cpp | 11 ++++++++++- 2 files changed, 13 insertions(+), 4 deletions(-) diff --git a/Tools/tests-host/Makefile b/Tools/tests-host/Makefile index fd001e4d7f..f0737ef88c 100644 --- a/Tools/tests-host/Makefile +++ b/Tools/tests-host/Makefile @@ -1,7 +1,7 @@ CC=g++ CFLAGS=-I. -I../../src/modules -I ../../src/include -I../../src/drivers \ - -I../../src -D__EXPORT="" -Dnullptr="0" -lm + -I../../src -I../../src/lib -D__EXPORT="" -Dnullptr="0" -lm all: mixer_test sbus2_test autodeclination_test @@ -20,7 +20,7 @@ SBUS2_FILES=../../src/modules/px4iofirmware/sbus.c \ hrt.cpp \ sbus2_test.cpp -AUTODECLINATION_FILES=../../src/lib/geo/geo_mag_declination.c \ +AUTODECLINATION_FILES= ../../src/lib/geo/geo_mag_declination.c \ hrt.cpp \ autodeclination_test.cpp @@ -31,7 +31,7 @@ sbus2_test: $(SBUS2_FILES) $(CC) -o sbus2_test $(SBUS2_FILES) $(CFLAGS) autodeclination_test: $(SBUS2_FILES) - $(CC) -o autodeclination_test $(SBUS2_FILES) $(CFLAGS) + $(CC) -o autodeclination_test $(AUTODECLINATION_FILES) $(CFLAGS) .PHONY: clean diff --git a/Tools/tests-host/autodeclination_test.cpp b/Tools/tests-host/autodeclination_test.cpp index b1b30f5d97..6c751dc1e7 100644 --- a/Tools/tests-host/autodeclination_test.cpp +++ b/Tools/tests-host/autodeclination_test.cpp @@ -1,5 +1,6 @@ #include +#include #include #include #include @@ -7,6 +8,7 @@ #include #include #include "../../src/systemcmds/tests/tests.h" +#include int main(int argc, char *argv[]) { warnx("autodeclination test started"); @@ -14,6 +16,13 @@ int main(int argc, char *argv[]) { 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); } \ No newline at end of file From ec50f73cbe4c88a57f92f888d764a678f6796dd2 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 28 Apr 2014 20:44:11 +0200 Subject: [PATCH 10/13] Updated geo lib C/C++ interfacing --- src/lib/geo/geo.h | 4 ++-- src/lib/geo/geo_mag_declination.c | 2 ++ 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/src/lib/geo/geo.h b/src/lib/geo/geo.h index d987afe337..e2f3da6f80 100644 --- a/src/lib/geo/geo.h +++ b/src/lib/geo/geo.h @@ -48,10 +48,10 @@ #include "uORB/topics/fence.h" #include "uORB/topics/vehicle_global_position.h" -#include "geo/geo_mag_declination.h" - __BEGIN_DECLS +#include "geo/geo_mag_declination.h" + #include #define CONSTANTS_ONE_G 9.80665f /* m/s^2 */ diff --git a/src/lib/geo/geo_mag_declination.c b/src/lib/geo/geo_mag_declination.c index 7b4aa69a2f..b96f877214 100644 --- a/src/lib/geo/geo_mag_declination.c +++ b/src/lib/geo/geo_mag_declination.c @@ -43,6 +43,8 @@ * */ +#include + /** set this always to the sampling in degrees for the table below */ #define SAMPLING_RES 10 From 48a9ba39afec8443ad4cb7d34f42e0cfc37d4e1e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 30 Apr 2014 08:13:42 +0200 Subject: [PATCH 11/13] Fixed typos in declination table lookup --- src/lib/geo/geo_mag_declination.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/lib/geo/geo_mag_declination.c b/src/lib/geo/geo_mag_declination.c index b96f877214..cfee6b423d 100644 --- a/src/lib/geo/geo_mag_declination.c +++ b/src/lib/geo/geo_mag_declination.c @@ -115,8 +115,8 @@ __EXPORT float get_mag_declination(float lat, float lon) } /* find index of nearest low sampling point */ - unsigned min_lat_index = (90 + min_lat) / SAMPLING_RES; - unsigned min_lon_index = (180 + min_lat) / SAMPLING_RES; + unsigned min_lat_index = (-(SAMPLING_MIN_LAT) + min_lat) / SAMPLING_RES; + unsigned min_lon_index = (-(SAMPLING_MIN_LON) + min_lon) / SAMPLING_RES; float declination_sw = get_lookup_table_val(min_lat_index, min_lon_index); float declination_se = get_lookup_table_val(min_lat_index, min_lon_index + 1); From 5429b82ae0fe777e31a1a52fa98f472f343e33af Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 30 Apr 2014 08:53:22 +0200 Subject: [PATCH 12/13] Fix last data type and casting compiler nuisances --- src/lib/geo/geo_mag_declination.c | 15 +++++++-------- 1 file changed, 7 insertions(+), 8 deletions(-) diff --git a/src/lib/geo/geo_mag_declination.c b/src/lib/geo/geo_mag_declination.c index cfee6b423d..09eac38f44 100644 --- a/src/lib/geo/geo_mag_declination.c +++ b/src/lib/geo/geo_mag_declination.c @@ -45,15 +45,14 @@ #include - /** set this always to the sampling in degrees for the table below */ -#define SAMPLING_RES 10 +#define SAMPLING_RES 10.0f #define SAMPLING_MIN_LAT -60.0f #define SAMPLING_MAX_LAT 60.0f #define SAMPLING_MIN_LON -180.0f #define SAMPLING_MAX_LON 180.0f -static const char declination_table[13][37] = \ +static const int8_t declination_table[13][37] = \ { 46, 45, 44, 42, 41, 40, 38, 36, 33, 28, 23, 16, 10, 4, -1, -5, -9, -14, -19, -26, -33, -40, -48, -55, -61, \ -66, -71, -74, -75, -72, -61, -25, 22, 40, 45, 47, 46, 30, 30, 30, 30, 29, 29, 29, 29, 27, 24, 18, 11, 3, \ @@ -90,8 +89,8 @@ __EXPORT float get_mag_declination(float lat, float lon) } /* round down to nearest sampling resolution */ - int min_lat = (((int)lat) / SAMPLING_RES) * SAMPLING_RES; - int min_lon = (((int)lon) / SAMPLING_RES) * SAMPLING_RES; + int min_lat = (int)(lat / SAMPLING_RES) * SAMPLING_RES; + int min_lon = (int)(lon / SAMPLING_RES) * SAMPLING_RES; /* for the rare case of hitting the bounds exactly * the rounding logic wouldn't fit, so enforce it. @@ -103,7 +102,7 @@ __EXPORT float get_mag_declination(float lat, float lon) } if (lat >= SAMPLING_MAX_LAT) { - min_lat = (((int)lat) / SAMPLING_RES) * SAMPLING_RES - SAMPLING_RES; + min_lat = (int)(lat / SAMPLING_RES) * SAMPLING_RES - SAMPLING_RES; } if (lon <= SAMPLING_MIN_LON) { @@ -111,7 +110,7 @@ __EXPORT float get_mag_declination(float lat, float lon) } if (lon >= SAMPLING_MAX_LON) { - min_lon = (((int)lon) / SAMPLING_RES) * SAMPLING_RES - SAMPLING_RES; + min_lon = (int)(lon / SAMPLING_RES) * SAMPLING_RES - SAMPLING_RES; } /* find index of nearest low sampling point */ @@ -128,7 +127,7 @@ __EXPORT float get_mag_declination(float lat, float lon) float declination_min = ((lon - min_lon) / SAMPLING_RES) * (declination_se - declination_sw) + declination_sw; float declination_max = ((lon - min_lon) / SAMPLING_RES) * (declination_ne - declination_nw) + declination_nw; - return (lat - min_lat) / SAMPLING_RES * (declination_max - declination_min) + declination_min; + return ((lat - min_lat) / SAMPLING_RES) * (declination_max - declination_min) + declination_min; } float get_lookup_table_val(unsigned lat_index, unsigned lon_index) From 22f262a241256e54aa6a03763689bdcdeb39be87 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 3 May 2014 12:40:11 +0200 Subject: [PATCH 13/13] host tests: Fix missing newlines --- Tools/tests-host/autodeclination_test.cpp | 2 +- Tools/tests-host/mixer_test.cpp | 2 +- Tools/tests-host/sbus2_test.cpp | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/Tools/tests-host/autodeclination_test.cpp b/Tools/tests-host/autodeclination_test.cpp index 6c751dc1e7..93bc340bb3 100644 --- a/Tools/tests-host/autodeclination_test.cpp +++ b/Tools/tests-host/autodeclination_test.cpp @@ -25,4 +25,4 @@ int main(int argc, char *argv[]) { printf("lat: %f lon: %f, dec: %f\n", lat, lon, declination); -} \ No newline at end of file +} diff --git a/Tools/tests-host/mixer_test.cpp b/Tools/tests-host/mixer_test.cpp index e311617f96..06499afd0e 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/sbus2_test.cpp b/Tools/tests-host/sbus2_test.cpp index 281903cf68..d8fcb695d8 100644 --- a/Tools/tests-host/sbus2_test.cpp +++ b/Tools/tests-host/sbus2_test.cpp @@ -72,4 +72,4 @@ int main(int argc, char *argv[]) { warnx("Test aborted, errno: %d", ret); } -} \ No newline at end of file +}