CoCalc Logo Icon
StoreFeaturesDocsShareSupportNewsAboutSign UpSign In
Ardupilot

Real-time collaboration for Jupyter Notebooks, Linux Terminals, LaTeX, VS Code, R IDE, and more,
all in one place. Commercial Alternative to JupyterHub.

GitHub Repository: Ardupilot/ardupilot
 Path: blob/master/libraries/AP_Declination/tests/test_magfield.cpp
Views: 1799
1
#include <AP_gtest.h>

2


3
#include <AP_Math/AP_Math.h>

4
#include <AP_Declination/AP_Declination.h>

5


6
/*

7
  test data from AP_Declination/generate/testvectors.py

8
 */

9
static struct {

10
    float lat, lon;

11
    Vector3f field;

12
} test_data[] = {

13
{-5.347785, -55.396586, {240.284, -79.152, -18.954}},

14
{-49.307646, 128.384946, {116.253, 0.061, -644.547}},

15
{-54.681456, -36.248963, {154.192, -18.211, -234.732}},

16
{-62.435767, -62.106564, {194.290, 43.804, -294.683}},

17
{8.779390, 37.908713, {357.108, 14.172, 14.024}},

18
{41.538128, -148.450769, {227.080, 53.006, 387.268}},

19
{36.054359, -80.267792, {220.590, -32.594, 440.521}},

20
{34.199500, 43.418665, {289.166, 26.382, 369.560}},

21
{-59.875966, -81.614124, {195.629, 84.684, -312.342}},

22
{17.601001, -93.758586, {274.301, 7.775, 274.744}},

23
{-86.268095, 101.445984, {-99.788, -126.515, -534.840}},

24
{68.089832, 99.531480, {70.104, 3.974, 607.705}},

25
{-30.787050, 104.800267, {232.740, -28.398, -519.563}},

26
{-45.686919, 23.376860, {111.202, -77.903, -239.402}},

27
{42.344796, 170.972092, {256.985, 1.579, 361.160}},

28
{11.180749, 126.619946, {392.427, -12.519, 60.590}},

29
{16.085182, 45.400453, {367.527, 13.863, 138.729}},

30
{85.912578, -111.799061, {8.172, -16.213, 568.954}},

31
{61.702909, 71.317152, {115.475, 38.354, 577.707}},

32
{-18.117488, -107.440690, {267.590, 53.018, -104.495}},

33
{-62.458351, 93.834118, {20.921, -130.137, -583.142}},

34
{-4.456960, 24.690442, {268.996, 0.281, -181.715}},

35
{-11.938973, 57.584278, {286.506, -51.380, -249.702}},

36
{-17.077653, 68.903062, {276.594, -67.083, -330.818}},

37
{-0.641855, -85.362927, {273.764, -0.021, 93.528}},

38
{70.208175, -70.794011, {54.788, -33.030, 565.194}},

39
{65.974445, -37.033989, {110.056, -41.337, 525.313}},

40
{-12.136829, 86.003211, {342.692, -33.667, -313.206}},

41
{-85.859369, -148.301355, {-48.300, 152.537, -536.334}},

42
{-19.307265, 115.056146, {324.659, 3.429, -396.785}},

43
{63.409946, -125.963384, {102.250, 34.962, 561.223}},

44
{10.445067, 60.380511, {384.749, -5.779, 62.938}},

45
{-54.102628, 134.827917, {84.120, 9.249, -655.864}},

46
{-37.512392, -13.418317, {98.314, -39.221, -216.540}},

47
{72.345349, 113.696131, {53.912, -7.336, 602.335}},

48
{55.212086, -11.672616, {179.144, -14.309, 464.924}},

49
{-69.962700, -143.828634, {72.069, 139.770, -551.999}},

50
{87.579976, -163.341588, {-9.629, -20.622, 569.787}},

51
{73.111174, 143.018689, {72.564, -23.169, 586.064}},

52
{23.697573, -29.994152, {309.137, -54.381, 178.330}},

53
{8.771240, -117.029349, {293.100, 40.791, 152.298}},

54
{-17.705434, 2.904797, {141.603, -31.071, -250.994}},

55
{36.273922, 68.280726, {289.178, 20.771, 423.563}},

56
{-23.761563, 163.511301, {300.464, 67.082, -379.323}},

57
{79.029797, 176.355901, {47.596, -4.724, 577.193}},

58
{-52.405560, 22.440300, {122.338, -88.770, -252.169}},

59
{72.485297, -46.002536, {70.887, -38.208, 546.530}},

60
{-22.183293, -159.478002, {295.811, 77.374, -257.498}},

61
{-24.950420, 35.883896, {150.668, -57.547, -261.731}},

62
{34.311721, -178.681293, {263.416, 26.642, 291.758}},

63
{-61.490634, 130.934200, {30.474, -18.155, -665.682}},

64
{2.793989, -178.226296, {331.384, 56.367, 5.643}},

65
{51.905236, 130.494014, {204.801, -48.610, 530.005}},

66
{-25.962740, -60.589052, {188.450, -40.089, -113.336}},

67
{64.153460, 76.711770, {98.437, 34.425, 588.123}},

68
{85.273389, 80.411045, {8.157, 23.324, 574.387}},

69
{72.227644, -175.679117, {94.668, 2.434, 565.494}},

70
{-15.800057, -88.398789, {255.341, 26.361, -44.696}},

71
{-32.252553, 140.907109, {249.932, 37.054, -516.256}},

72
{0.990803, 19.645312, {302.237, 6.632, -135.887}},

73
{13.221787, -167.051664, {297.419, 47.578, 134.337}},

74
{-55.704793, -147.770348, {153.269, 119.486, -498.314}},

75
{-21.527164, 98.030817, {291.813, -24.330, -432.389}},

76
{-29.459735, -169.175151, {269.771, 84.511, -361.577}},

77
{-77.026676, -128.521814, {57.096, 156.826, -529.076}},

78
{59.744040, 154.466555, {174.852, -31.321, 530.246}},

79
{-8.667233, -84.596843, {263.264, 6.250, 19.318}},

80
{-44.970992, -90.210588, {203.908, 78.088, -237.970}},

81
{-81.281504, 53.655592, {53.564, -172.598, -484.541}},

82
{-28.899249, -168.937713, {272.725, 84.310, -354.906}},

83
{-5.495801, -115.945089, {294.242, 45.813, 2.378}},

84
{-56.855722, -27.312216, {150.561, -22.699, -243.436}},

85
{-5.160334, -104.515570, {287.811, 39.169, 23.270}},

86
{-4.765155, 27.513017, {270.761, 0.084, -179.570}},

87
{38.850519, -83.245266, {208.706, -24.590, 466.759}},

88
{82.903976, -111.774971, {15.131, -10.610, 569.636}},

89
{65.049275, 72.501496, {95.332, 38.164, 582.824}},

90
{62.904242, 91.633672, {102.309, 14.476, 603.680}},

91
{-26.703878, 172.749541, {285.797, 77.084, -385.509}},

92
{-33.226726, 75.903795, {173.490, -100.050, -456.333}},

93
{24.643218, -30.321324, {307.312, -53.948, 191.067}},

94
{16.206490, 22.434794, {358.048, 19.103, 94.086}},

95
{46.445304, -34.626939, {217.483, -45.670, 423.301}},

96
{19.266180, 125.974692, {377.297, -24.128, 183.176}},

97
{-24.732609, -162.086198, {289.118, 80.970, -291.171}},

98
{10.102379, -89.724793, {278.231, 3.333, 201.223}},

99
{44.251765, 81.116331, {239.108, 19.539, 512.551}},

100
{-72.418531, -130.000797, {82.558, 150.448, -523.607}},

101
{12.703973, 18.436378, {353.684, 14.320, 30.949}},

102
{29.149731, -105.686368, {248.213, 29.644, 376.837}},

103
{-17.956078, -35.751483, {164.178, -72.043, -167.376}},

104
{-38.321171, -141.763622, {232.842, 93.184, -359.596}},

105
{21.175405, 50.561510, {355.480, 13.624, 223.431}},

106
{54.947454, -169.310847, {199.460, 23.943, 468.886}},

107
{-23.361657, 44.446547, {177.462, -68.674, -281.151}},

108
{-24.280380, 19.644762, {122.126, -31.739, -246.213}},

109
{-63.110180, -122.152350, {143.859, 136.021, -464.339}},

110
{4.519890, 40.756994, {348.158, 6.163, -44.400}},

111
{-61.062289, 59.265124, {84.041, -166.618, -424.664}},

112
{-42.770992, -60.512815, {174.635, -6.856, -175.937}}};

113


114


115
TEST(MagField, test_field_error)

116
{

117
    for (const auto &d : test_data) {

118
        Location loc(d.lat*1.0e7, d.lon*1.0e7, 0, Location::AltFrame::ABSOLUTE);

119
        const Vector3f m = AP_Declination::get_earth_field_ga(loc);

120
        EXPECT_FLOAT_EQ(roundf(m.x*1000), roundf(d.field.x));

121
        EXPECT_FLOAT_EQ(roundf(m.y*1000), roundf(d.field.y));

122
        EXPECT_FLOAT_EQ(roundf(m.z*1000), roundf(d.field.z));

123
    }

124
}

125


126


127
AP_GTEST_MAIN()

128
int hal = 0;

129


130


131