AP_Declination: added a test suite for AP_Declination

use test vectors from MAVProxy mavextra

libraries/AP_Declination/generate/testvectors.py

@@ -0,0 +1,20 @@
+#!/usr/bin/env python3
+'''
+generate some test vectors for autotest
+'''
+
+
+from pymavlink import mavextra
+import random
+import argparse
+parser = argparse.ArgumentParser(description='generate test vectors')
+parser.add_argument('--num-samples', type=int, default=100, help='number of samples')
+
+args = parser.parse_args()
+
+for i in range(args.num_samples):
+    lat = random.uniform(-89,89)
+    lon = random.uniform(-180,180)
+    mavextra.earth_field = None
+    m = mavextra.expected_earth_field_lat_lon(lat, lon)
+    print("{%f, %f, {%.3f, %.3f, %.3f}}," % (lat, lon, m.x, m.y, m.z))

libraries/AP_Declination/tests/test_magfield.cpp

@@ -0,0 +1,129 @@
+#include <AP_gtest.h>
+
+#include <AP_Math/AP_Math.h>
+#include <AP_Declination/AP_Declination.h>
+
+/*
+  test data from AP_Declination/generate/testvectors.py
+ */
+static struct {
+    float lat, lon;
+    Vector3f field;
+} test_data[] = {
+{65.815374, -143.062333, {107.860, 35.113, 556.328}},
+{4.841310, -64.150382, {266.887, -64.218, 111.937}},
+{45.369500, 48.968768, {222.477, 32.849, 462.635}},
+{56.434560, -25.336510, {167.362, -35.285, 476.757}},
+{66.808874, 79.148521, {82.702, 33.023, 589.400}},
+{-7.916276, -44.424167, {220.765, -86.936, -83.784}},
+{64.118293, -0.153519, {134.901, -3.555, 500.255}},
+{-35.352160, 99.449185, {191.986, -54.353, -541.436}},
+{-42.283291, -97.597266, {210.008, 85.539, -250.617}},
+{83.930834, 5.940172, {47.596, 4.145, 554.006}},
+{47.012991, 36.119569, {213.213, 29.443, 453.806}},
+{-21.339327, 36.712924, {166.444, -45.453, -261.021}},
+{-26.958910, 135.309269, {283.842, 26.564, -468.649}},
+{76.337075, -77.042161, {30.209, -27.702, 564.548}},
+{-68.792368, -26.587185, {184.541, -12.654, -318.117}},
+{42.204177, -164.412272, {236.899, 39.169, 370.136}},
+{73.424992, -118.704385, {43.023, 8.787, 575.713}},
+{65.283281, 2.411133, {129.587, -0.609, 504.668}},
+{34.510044, 135.655983, {309.899, -40.362, 353.492}},
+{68.831024, -173.333644, {115.391, 10.915, 556.720}},
+{-65.176537, 164.992638, {19.605, 85.294, -639.778}},
+{-57.434603, 152.791413, {72.536, 58.406, -647.895}},
+{15.880162, 66.262931, {387.182, -2.057, 146.598}},
+{67.203361, -177.608822, {128.854, 7.288, 548.788}},
+{-30.523857, 29.118199, {113.791, -54.955, -248.678}},
+{31.809160, -119.132163, {244.130, 49.844, 376.711}},
+{-15.663087, -80.002773, {247.203, 5.676, -31.897}},
+{-58.047622, 52.078363, {101.589, -155.733, -380.717}},
+{88.215263, -126.430760, {-3.492, -18.889, 566.858}},
+{-14.693858, 100.649221, {343.282, -9.516, -348.166}},
+{37.646886, -40.902135, {250.225, -59.227, 358.666}},
+{-73.309744, 171.966445, {-32.052, 91.220, -623.196}},
+{66.381521, 93.066045, {81.832, 15.501, 601.880}},
+{-51.800132, -48.161067, {166.155, -10.418, -218.457}},
+{-1.470683, -101.101444, {289.312, 35.209, 67.787}},
+{30.018412, -128.584173, {250.048, 54.126, 338.710}},
+{60.430386, -85.104119, {90.333, -20.598, 572.518}},
+{65.859137, -119.499590, {78.769, 25.428, 573.031}},
+{49.072308, -47.606189, {188.603, -58.266, 465.766}},
+{77.503573, -60.423956, {38.556, -33.783, 556.409}},
+{82.714485, 128.717387, {19.657, 10.220, 580.422}},
+{38.801986, 91.587255, {282.915, 4.742, 471.180}},
+{-31.287123, -94.113422, {226.905, 60.645, -171.111}},
+{25.047470, 65.541191, {352.871, 8.642, 282.449}},
+{14.206388, 26.081254, {358.987, 18.786, 66.843}},
+{-29.299607, -166.418773, {271.109, 83.632, -352.255}},
+{26.591442, -73.547420, {251.844, -46.851, 346.171}},
+{88.640245, 169.476288, {-18.244, 11.586, 567.557}},
+{33.894442, 72.675222, {307.721, 15.765, 398.253}},
+{-88.031320, 74.684074, {-37.224, -164.448, -519.832}},
+{79.537032, 32.735513, {58.741, 24.554, 552.941}},
+{45.437015, 80.470038, {231.120, 22.130, 515.505}},
+{49.325371, -172.704252, {224.668, 24.546, 424.141}},
+{14.903743, -64.771466, {269.743, -64.464, 209.310}},
+{17.835966, 174.249220, {303.993, 38.460, 147.669}},
+{7.298126, 118.399877, {406.420, -6.502, 8.517}},
+{-63.751920, 68.614309, {61.074, -168.670, -474.255}},
+{-38.960700, 10.784171, {92.427, -43.852, -222.347}},
+{53.775617, 81.590013, {168.976, 26.522, 565.427}},
+{43.838294, -155.062495, {226.785, 48.323, 397.111}},
+{44.222391, 169.462356, {252.609, -1.206, 379.944}},
+{17.959121, 54.928441, {369.427, 6.915, 175.187}},
+{76.044596, -122.732672, {35.189, 6.653, 573.863}},
+{52.971416, -26.099079, {185.527, -36.203, 458.872}},
+{-74.070371, -138.645368, {53.523, 150.678, -546.192}},
+{-81.736068, -31.431979, {189.895, 4.156, -439.712}},
+{10.192549, -127.948140, {292.468, 45.734, 152.605}},
+{-47.221957, 78.382924, {103.034, -130.997, -499.588}},
+{51.576442, -154.944501, {201.013, 45.570, 460.479}},
+{-63.852951, 35.163443, {127.325, -135.698, -341.812}},
+{28.300507, -163.781419, {260.632, 42.939, 261.746}},
+{0.595936, 141.700759, {374.127, 18.689, -98.455}},
+{-64.367741, 95.249244, {11.465, -130.514, -585.559}},
+{-69.280536, -143.001117, {77.504, 139.710, -549.996}},
+{48.335410, 70.288703, {206.898, 31.805, 519.597}},
+{18.370932, -0.992613, {343.193, -6.545, 96.215}},
+{-33.668004, 31.933840, {115.929, -68.241, -254.100}},
+{-66.283464, 136.355865, {35.080, 29.372, -658.843}},
+{87.188467, 80.272993, {6.995, 22.295, 569.340}},
+{-37.843441, -37.567972, {133.282, -46.568, -190.835}},
+{69.911723, 4.695994, {108.423, 1.595, 519.605}},
+{8.137470, 11.523994, {335.731, 0.997, -49.510}},
+{49.732747, 90.330000, {198.706, 11.573, 559.178}},
+{-29.391961, -88.979254, {225.191, 49.068, -144.567}},
+{35.156541, 43.801954, {283.633, 25.974, 375.362}},
+{49.352233, -56.328211, {177.143, -59.472, 486.532}},
+{68.219860, 29.192947, {109.116, 27.847, 527.438}},
+{-16.720512, 171.687827, {328.473, 68.966, -270.540}},
+{-65.879394, 156.212441, {-0.674, 68.128, -650.680}},
+{59.992056, 167.198631, {178.767, -11.904, 516.533}},
+{15.571870, -163.519672, {290.435, 46.049, 160.239}},
+{-4.927750, 48.044786, {309.256, -21.794, -163.621}},
+{8.440285, -48.490401, {273.723, -89.165, 76.441}},
+{31.432914, 128.102195, {327.297, -38.624, 341.520}},
+{-76.539795, 114.525530, {-79.373, -91.953, -597.405}},
+{37.595668, 10.343255, {273.196, 12.709, 353.344}},
+{7.995321, 66.515830, {392.070, -12.382, 18.917}},
+{14.426346, -7.141914, {336.552, -21.023, 32.344}},
+{-25.434922, -161.299687, {286.912, 80.742, -297.084}},
+{67.878431, 32.272372, {109.027, 30.505, 529.484}}};
+
+
+TEST(MagField, test_field_error)
+{
+    for (const auto &d : test_data) {
+        Location loc(d.lat*1.0e7, d.lon*1.0e7, 0, Location::AltFrame::ABSOLUTE);
+        const Vector3f m = AP_Declination::get_earth_field_ga(loc);
+        EXPECT_FLOAT_EQ(roundf(m.x*1000), roundf(d.field.x));
+        EXPECT_FLOAT_EQ(roundf(m.y*1000), roundf(d.field.y));
+        EXPECT_FLOAT_EQ(roundf(m.z*1000), roundf(d.field.z));
+    }
+}
+
+
+AP_GTEST_MAIN()
+int hal = 0;
+

libraries/AP_Declination/tests/wscript

@@ -0,0 +1,7 @@
+#!/usr/bin/env python
+# encoding: utf-8
+
+def build(bld):
+    bld.ap_find_tests(
+        use='ap',
+    )