1 files changed, 52 insertions, 0 deletions
diff --git a/unittests/autodeclination_test.cpp b/unittests/autodeclination_test.cpp
new file mode 100644
index 000000000..1bda6eb79
--- /dev/null
+++ b/unittests/autodeclination_test.cpp
@@ -0,0 +1,52 @@
+
+#include <stdio.h>
+#include <stdlib.h> 
+#include <unistd.h>
+#include <string.h>
+#include <math.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");
+
+	char* latstr = 0;
+	char* lonstr = 0;
+	char* declstr = 0;
+
+	if (argc < 4) {
+		warnx("Too few arguments. Using default lat / lon and declination");
+		latstr = "47.0";
+		lonstr = "8.0";
+		declstr = "0.6";
+	} else {
+		latstr = argv[1];
+		lonstr = argv[2];
+		declstr = argv[3];
+	}
+
+	char* p_end;
+
+	float lat = strtod(latstr, &p_end);
+	float lon = strtod(lonstr, &p_end);
+	float decl_truth = strtod(declstr, &p_end);
+
+	float declination = get_mag_declination(lat, lon);
+
+	printf("lat: %f lon: %f, expected dec: %f, estimated dec: %f\n", lat, lon, declination, decl_truth);
+
+	int ret = 0;
+
+	// Fail if the declination differs by more than one degree
+	float decldiff = fabs(decl_truth - declination);
+	if (decldiff > 0.5f) {
+		warnx("declination differs more than 1 degree: difference: %12.8f", decldiff);
+		ret = 1;
+	}
+
+	return ret;
+}