diff options
Diffstat (limited to 'unittests')
-rw-r--r-- | unittests/autodeclination_test.cpp | 36 |
1 files changed, 30 insertions, 6 deletions
diff --git a/unittests/autodeclination_test.cpp b/unittests/autodeclination_test.cpp index 93bc340bb..1bda6eb79 100644 --- a/unittests/autodeclination_test.cpp +++ b/unittests/autodeclination_test.cpp @@ -3,26 +3,50 @@ #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 "../../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* 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(argv[1], &p_end); - float lon = strtod(argv[2], &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, dec: %f\n", lat, lon, declination); + 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; } |