From 76eece252bf5d0f726b76d4ecf01df3eabd3fd62 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 3 Sep 2021 13:13:10 +1000 Subject: [PATCH] AP_Declination: added a test suite for AP_Declination use test vectors from MAVProxy mavextra --- .../AP_Declination/generate/testvectors.py | 20 +++ .../AP_Declination/tests/test_magfield.cpp | 129 ++++++++++++++++++ libraries/AP_Declination/tests/wscript | 7 + 3 files changed, 156 insertions(+) create mode 100755 libraries/AP_Declination/generate/testvectors.py create mode 100644 libraries/AP_Declination/tests/test_magfield.cpp create mode 100644 libraries/AP_Declination/tests/wscript diff --git a/libraries/AP_Declination/generate/testvectors.py b/libraries/AP_Declination/generate/testvectors.py new file mode 100755 index 0000000000..9314bed9b6 --- /dev/null +++ b/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)) diff --git a/libraries/AP_Declination/tests/test_magfield.cpp b/libraries/AP_Declination/tests/test_magfield.cpp new file mode 100644 index 0000000000..b0f22fc377 --- /dev/null +++ b/libraries/AP_Declination/tests/test_magfield.cpp @@ -0,0 +1,129 @@ +#include + +#include +#include + +/* + 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; + diff --git a/libraries/AP_Declination/tests/wscript b/libraries/AP_Declination/tests/wscript new file mode 100644 index 0000000000..cd3e5e3ce7 --- /dev/null +++ b/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', + )