wmm.cpp
Go to the documentation of this file.
1 /*
2  * This program is free software: you can redistribute it and/or modify
3  * it under the terms of the GNU General Public License as published by
4  * the Free Software Foundation, version 3.
5  *
6  * This program is distributed in the hope that it will be useful, but
7  * WITHOUT ANY WARRANTY; without even the implied warranty of
8  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
9  * General Public License for more details.
10  *
11  * You should have received a copy of the GNU General Public License
12  * along with this program. If not, see <http://www.gnu.org/licenses/>.
13  *
14  * Author: Dmitry Ponomarev <ponomarevda96@gmail.com>
15  */
16 
17 #include "UavDynamics/math/wmm.hpp"
18 #include <cmath>
19 #include <stdint.h>
20 
21 
22 // *INDENT-OFF*
23 // Magnetic declination data in radians * 10^-4
24 // Model: WMM-2020,
25 // Version: 0.5.1.11,
26 // Date: 2023.3287,
27 static constexpr const int16_t declination_table[19][37] {
28  // LONGITUDE: -180, -170, -160, -150, -140, -130, -120, -110, -100, -90, -80, -70, -60, -50, -40, -30, -20, -10, 0, 10, 20, 30, 40, 50, 60, 70, 80, 90, 100, 110, 120, 130, 140, 150, 160, 170, 180,
29  /* LAT: -90 */ { 25967, 24222, 22477, 20731, 18986, 17240, 15495, 13750, 12005, 10259, 8514, 6769, 5023, 3278, 1533, -212, -1958, -3703, -5448, -7194, -8939,-10684,-12429,-14175,-15920,-17665,-19411,-21156,-22902,-24647,-26392,-28138,-29883, 31203, 29458, 27713, 25967, },
30  /* LAT: -80 */ { 22532, 20403, 18464, 16692, 15052, 13514, 12051, 10640, 9265, 7916, 6584, 5266, 3957, 2652, 1344, 24, -1318, -2691, -4102, -5555, -7051, -8589,-10168,-11788,-13454,-15173,-16960,-18833,-20817,-22937,-25212,-27647,-30215, 29979, 27363, 24859, 22532, },
31  /* LAT: -70 */ { 14982, 13583, 12455, 11491, 10620, 9787, 8944, 8056, 7102, 6083, 5013, 3920, 2836, 1782, 761, -249, -1287, -2394, -3598, -4901, -6285, -7718, -9168,-10611,-12038,-13455,-14885,-16373,-18000,-19920,-22451,-26265, 30648, 24116, 19620, 16853, 14982, },
32  /* LAT: -60 */ { 8438, 8189, 7905, 7627, 7371, 7115, 6804, 6369, 5752, 4931, 3929, 2818, 1699, 675, -202, -957, -1685, -2510, -3520, -4724, -6060, -7434, -8757, -9968,-11032,-11930,-12644,-13139,-13310,-12862,-10754, -3444, 4979, 7699, 8458, 8577, 8438, },
33  /* LAT: -50 */ { 5499, 5534, 5475, 5383, 5307, 5268, 5231, 5102, 4756, 4090, 3077, 1804, 465, -710, -1572, -2128, -2520, -2962, -3656, -4683, -5938, -7221, -8364, -9263, -9857,-10090, -9889, -9125, -7611, -5244, -2337, 412, 2522, 3948, 4822, 5296, 5499, },
34  /* LAT: -40 */ { 3965, 4057, 4063, 4015, 3953, 3917, 3921, 3908, 3733, 3195, 2167, 721, -841, -2148, -3000, -3440, -3614, -3670, -3846, -4436, -5432, -6517, -7400, -7932, -8030, -7641, -6742, -5354, -3645, -1948, -492, 728, 1773, 2642, 3301, 3733, 3965, },
35  /* LAT: -30 */ { 2992, 3078, 3106, 3088, 3027, 2947, 2885, 2851, 2723, 2241, 1198, -327, -1933, -3178, -3906, -4240, -4309, -4088, -3651, -3450, -3841, -4601, -5295, -5635, -5498, -4900, -3934, -2730, -1529, -592, 81, 680, 1306, 1913, 2426, 2791, 2992, },
36  /* LAT: -20 */ { 2349, 2395, 2409, 2408, 2363, 2266, 2156, 2079, 1933, 1434, 369, -1126, -2602, -3653, -4173, -4277, -4063, -3493, -2628, -1843, -1591, -1961, -2612, -3073, -3100, -2733, -2094, -1280, -512, -31, 227, 527, 971, 1455, 1882, 2194, 2349, },
37  /* LAT: -10 */ { 1956, 1949, 1925, 1918, 1887, 1799, 1686, 1593, 1408, 855, -214, -1595, -2864, -3682, -3934, -3693, -3110, -2325, -1480, -731, -278, -324, -801, -1304, -1511, -1415, -1100, -603, -112, 122, 165, 322, 697, 1141, 1541, 1833, 1956, },
38  /* LAT: 0 */ { 1742, 1708, 1649, 1637, 1623, 1551, 1442, 1325, 1070, 448, -596, -1824, -2872, -3446, -3431, -2920, -2149, -1369, -720, -187, 222, 325, 39, -378, -629, -683, -584, -325, -39, 44, -23, 62, 407, 858, 1284, 1610, 1742, },
39  /* LAT: 10 */ { 1604, 1612, 1567, 1580, 1603, 1551, 1425, 1235, 858, 141, -881, -1955, -2774, -3103, -2887, -2274, -1497, -788, -277, 101, 425, 569, 402, 79, -153, -261, -287, -210, -109, -155, -300, -276, 29, 491, 978, 1391, 1604, },
40  /* LAT: 20 */ { 1416, 1564, 1623, 1713, 1798, 1775, 1616, 1311, 766, -89, -1130, -2076, -2665, -2766, -2431, -1825, -1114, -467, -10, 297, 554, 697, 602, 359, 161, 42, -54, -124, -208, -402, -642, -702, -464, -11, 535, 1057, 1416, },
41  /* LAT: 30 */ { 1108, 1474, 1733, 1957, 2116, 2125, 1935, 1511, 784, -243, -1355, -2225, -2633, -2564, -2162, -1581, -926, -309, 150, 454, 684, 828, 804, 654, 506, 380, 218, -1, -289, -666, -1036, -1196, -1033, -603, -24, 586, 1108, },
42  /* LAT: 40 */ { 742, 1328, 1823, 2216, 2466, 2510, 2294, 1761, 851, -371, -1602, -2460, -2775, -2619, -2172, -1579, -925, -297, 211, 573, 842, 1038, 1128, 1116, 1040, 890, 615, 195, -350, -959, -1480, -1721, -1602, -1183, -585, 85, 742, },
43  /* LAT: 50 */ { 443, 1188, 1870, 2430, 2800, 2909, 2682, 2030, 894, -593, -2005, -2911, -3202, -3009, -2519, -1873, -1163, -466, 148, 652, 1068, 1421, 1698, 1866, 1882, 1691, 1238, 519, -383, -1287, -1962, -2243, -2116, -1675, -1044, -317, 443, },
44  /* LAT: 60 */ { 233, 1083, 1890, 2589, 3098, 3317, 3110, 2308, 810, -1130, -2833, -3808, -4061, -3803, -3233, -2489, -1664, -825, -19, 731, 1421, 2050, 2590, 2982, 3137, 2942, 2291, 1160, -263, -1582, -2443, -2746, -2579, -2089, -1403, -610, 233, },
45  /* LAT: 70 */ { -27, 905, 1801, 2603, 3222, 3525, 3289, 2187, 4, -2659, -4601, -5437, -5459, -4985, -4223, -3294, -2274, -1211, -138, 920, 1943, 2908, 3773, 4477, 4919, 4942, 4316, 2820, 607, -1504, -2794, -3233, -3075, -2549, -1808, -946, -27, },
46  /* LAT: 80 */ { -816, 104, 954, 1645, 2036, 1878, 760, -1675, -4727, -6835, -7634, -7551, -6956, -6063, -4988, -3801, -2543, -1244, 77, 1401, 2713, 3995, 5222, 6361, 7359, 8121, 8464, 8007, 6022, 2118, -1550, -3257, -3603, -3265, -2585, -1736, -816, },
47  /* LAT: 90 */ { -29541,-27795,-26050,-24304,-22559,-20814,-19068,-17323,-15578,-13833,-12087,-10342, -8597, -6852, -5107, -3362, -1616, 129, 1874, 3619, 5364, 7110, 8855, 10600, 12346, 14091, 15836, 17582, 19327, 21073, 22818, 24564, 26309, 28055, 29800,-31286,-29541, },
48 };
49 
50 // Magnetic inclination data in radians * 10^-4
51 // Model: WMM-2020,
52 // Version: 0.5.1.11,
53 // Date: 2023.3287,
54 static constexpr const int16_t inclination_table[19][37] {
55  // LONGITUDE: -180, -170, -160, -150, -140, -130, -120, -110, -100, -90, -80, -70, -60, -50, -40, -30, -20, -10, 0, 10, 20, 30, 40, 50, 60, 70, 80, 90, 100, 110, 120, 130, 140, 150, 160, 170, 180,
56  /* LAT: -90 */ { -12567,-12567,-12567,-12567,-12567,-12567,-12567,-12567,-12567,-12567,-12566,-12566,-12566,-12566,-12566,-12566,-12566,-12566,-12566,-12566,-12566,-12566,-12566,-12567,-12567,-12567,-12567,-12567,-12567,-12567,-12567,-12567,-12567,-12567,-12567,-12567,-12567, },
57  /* LAT: -80 */ { -13650,-13516,-13355,-13175,-12981,-12780,-12576,-12376,-12186,-12010,-11853,-11719,-11609,-11523,-11459,-11418,-11398,-11400,-11427,-11481,-11565,-11680,-11825,-11999,-12197,-12414,-12644,-12877,-13105,-13317,-13503,-13653,-13756,-13807,-13803,-13749,-13650, },
58  /* LAT: -70 */ { -14098,-13779,-13460,-13137,-12805,-12462,-12107,-11750,-11408,-11100,-10848,-10666,-10554,-10502,-10487,-10490,-10495,-10504,-10526,-10582,-10690,-10865,-11113,-11429,-11802,-12221,-12669,-13134,-13600,-14052,-14469,-14813,-14999,-14943,-14712,-14414,-14098, },
59  /* LAT: -60 */ { -13514,-13160,-12821,-12488,-12145,-11773,-11358,-10904,-10437,-10008, -9680, -9507, -9506, -9644, -9847,-10037,-10158,-10198,-10182,-10166,-10214,-10374,-10667,-11078,-11579,-12136,-12723,-13319,-13908,-14469,-14967,-15255,-15075,-14689,-14282,-13887,-13514, },
60  /* LAT: -50 */ { -12494,-12151,-11819,-11497,-11173,-10827,-10428, -9957, -9428, -8909, -8522, -8402, -8607, -9070, -9633,-10141,-10494,-10649,-10613,-10456,-10314,-10324,-10550,-10972,-11517,-12111,-12697,-13234,-13679,-13976,-14084,-14010,-13806,-13522,-13193,-12845,-12494, },
61  /* LAT: -40 */ { -11239,-10889,-10541,-10195, -9856, -9518, -9158, -8733, -8213, -7650, -7229, -7196, -7664, -8491, -9412,-10237,-10890,-11316,-11445,-11268,-10921,-10653,-10654,-10943,-11409,-11914,-12358,-12676,-12833,-12837,-12748,-12612,-12434,-12202,-11915,-11587,-11239, },
62  /* LAT: -30 */ { -9602, -9221, -8839, -8448, -8056, -7681, -7327, -6938, -6426, -5816, -5370, -5466, -6249, -7458, -8708, -9798,-10705,-11407,-11793,-11770,-11389,-10882,-10555,-10563,-10819,-11141,-11394,-11503,-11438,-11263,-11090,-10959,-10816,-10610,-10329, -9982, -9602, },
63  /* LAT: -20 */ { -7373, -6927, -6506, -6077, -5633, -5204, -4816, -4406, -3842, -3158, -2719, -2998, -4128, -5750, -7375, -8735, -9790,-10555,-10981,-11002,-10628,-10009, -9454, -9218, -9271, -9434, -9573, -9586, -9411, -9140, -8950, -8869, -8770, -8566, -8249, -7835, -7373, },
64  /* LAT: -10 */ { -4418, -3874, -3416, -2976, -2519, -2069, -1658, -1209, -588, 111, 462, 14, -1338, -3267, -5238, -6831, -7910, -8530, -8784, -8710, -8290, -7597, -6935, -6596, -6560, -6653, -6768, -6789, -6599, -6305, -6156, -6175, -6144, -5932, -5548, -5017, -4418, },
65  /* LAT: 0 */ { -910, -278, 193, 599, 1017, 1434, 1820, 2257, 2834, 3405, 3595, 3081, 1765, -159, -2199, -3831, -4819, -5232, -5281, -5102, -4651, -3927, -3222, -2856, -2797, -2865, -2988, -3060, -2929, -2696, -2648, -2804, -2879, -2699, -2274, -1640, -910, },
66  /* LAT: 10 */ { 2558, 3192, 3631, 3975, 4330, 4697, 5045, 5426, 5870, 6233, 6260, 5757, 4660, 3083, 1395, 35, -752, -984, -884, -642, -223, 423, 1056, 1389, 1453, 1412, 1313, 1222, 1264, 1365, 1282, 1003, 799, 866, 1219, 1825, 2558, },
67  /* LAT: 20 */ { 5414, 5947, 6330, 6627, 6941, 7284, 7624, 7966, 8294, 8488, 8393, 7922, 7077, 5976, 4853, 3954, 3437, 3330, 3487, 3735, 4070, 4540, 5001, 5254, 5313, 5299, 5253, 5195, 5181, 5160, 4980, 4639, 4333, 4239, 4412, 4838, 5414, },
68  /* LAT: 30 */ { 7568, 7943, 8262, 8546, 8855, 9202, 9557, 9894, 10167, 10276, 10126, 9699, 9067, 8354, 7696, 7191, 6906, 6872, 7019, 7234, 7483, 7785, 8074, 8248, 8306, 8322, 8327, 8317, 8292, 8207, 7981, 7623, 7263, 7041, 7025, 7221, 7568, },
69  /* LAT: 40 */ { 9266, 9487, 9743, 10029, 10355, 10715, 11081, 11418, 11667, 11745, 11593, 11231, 10757, 10285, 9891, 9611, 9466, 9467, 9581, 9745, 9922, 10108, 10281, 10406, 10484, 10547, 10604, 10637, 10619, 10505, 10261, 9908, 9538, 9252, 9110, 9122, 9266, },
70  /* LAT: 50 */ { 10802, 10923, 11124, 11393, 11715, 12068, 12421, 12735, 12955, 13012, 12872, 12574, 12210, 11867, 11593, 11409, 11319, 11318, 11387, 11492, 11607, 11724, 11841, 11957, 12076, 12200, 12314, 12387, 12376, 12248, 12000, 11671, 11331, 11046, 10856, 10776, 10802, },
71  /* LAT: 60 */ { 12320, 12391, 12540, 12757, 13026, 13325, 13626, 13891, 14066, 14093, 13958, 13710, 13423, 13153, 12933, 12778, 12689, 12659, 12677, 12726, 12795, 12881, 12988, 13121, 13281, 13457, 13622, 13730, 13734, 13612, 13386, 13106, 12828, 12592, 12422, 12330, 12320, },
72  /* LAT: 70 */ { 13757, 13798, 13892, 14032, 14210, 14412, 14619, 14800, 14905, 14887, 14753, 14553, 14337, 14134, 13961, 13827, 13733, 13679, 13661, 13674, 13717, 13790, 13894, 14031, 14199, 14387, 14571, 14710, 14755, 14681, 14519, 14320, 14126, 13961, 13841, 13772, 13757, },
73  /* LAT: 80 */ { 14994, 15005, 15041, 15099, 15174, 15258, 15336, 15381, 15367, 15292, 15182, 15060, 14938, 14825, 14725, 14644, 14582, 14542, 14523, 14527, 14554, 14604, 14676, 14769, 14881, 15008, 15144, 15276, 15384, 15427, 15386, 15297, 15201, 15115, 15050, 15009, 14994, },
74  /* LAT: 90 */ { 15398, 15398, 15398, 15398, 15398, 15398, 15398, 15398, 15398, 15398, 15398, 15398, 15398, 15398, 15398, 15398, 15398, 15398, 15398, 15398, 15398, 15398, 15398, 15398, 15398, 15398, 15398, 15398, 15398, 15398, 15398, 15398, 15398, 15398, 15398, 15398, 15398, },
75 };
76 
77 // Magnetic strength data in milli-Gauss * 10
78 // Model: WMM-2020,
79 // Version: 0.5.1.11,
80 // Date: 2023.3287,
81 static constexpr const int16_t strength_table[19][37] {
82  // LONGITUDE: -180, -170, -160, -150, -140, -130, -120, -110, -100, -90, -80, -70, -60, -50, -40, -30, -20, -10, 0, 10, 20, 30, 40, 50, 60, 70, 80, 90, 100, 110, 120, 130, 140, 150, 160, 170, 180,
83  /* LAT: -90 */ { 5447, 5447, 5447, 5447, 5447, 5447, 5447, 5447, 5447, 5447, 5447, 5447, 5447, 5447, 5447, 5447, 5447, 5447, 5447, 5447, 5447, 5447, 5447, 5447, 5447, 5447, 5447, 5447, 5447, 5447, 5447, 5447, 5447, 5447, 5447, 5447, 5447, },
84  /* LAT: -80 */ { 6053, 5989, 5910, 5818, 5714, 5602, 5484, 5363, 5240, 5120, 5006, 4901, 4807, 4728, 4665, 4620, 4596, 4595, 4616, 4663, 4734, 4828, 4943, 5075, 5219, 5369, 5519, 5662, 5793, 5907, 5999, 6068, 6112, 6131, 6126, 6100, 6053, },
85  /* LAT: -70 */ { 6298, 6164, 6013, 5847, 5668, 5476, 5270, 5056, 4837, 4622, 4419, 4237, 4080, 3949, 3846, 3769, 3722, 3710, 3739, 3816, 3945, 4126, 4355, 4623, 4919, 5226, 5530, 5814, 6064, 6267, 6418, 6513, 6554, 6546, 6496, 6410, 6298, },
86  /* LAT: -60 */ { 6183, 5990, 5788, 5578, 5359, 5124, 4867, 4589, 4297, 4007, 3741, 3514, 3335, 3201, 3103, 3029, 2979, 2959, 2985, 3077, 3246, 3499, 3827, 4214, 4637, 5072, 5493, 5877, 6201, 6449, 6612, 6690, 6691, 6627, 6513, 6361, 6183, },
87  /* LAT: -50 */ { 5841, 5610, 5377, 5146, 4913, 4668, 4397, 4094, 3765, 3434, 3132, 2893, 2733, 2642, 2594, 2559, 2524, 2498, 2503, 2576, 2752, 3044, 3442, 3915, 4423, 4929, 5403, 5821, 6160, 6401, 6538, 6578, 6535, 6424, 6261, 6062, 5841, },
88  /* LAT: -40 */ { 5391, 5145, 4899, 4659, 4424, 4186, 3931, 3647, 3333, 3007, 2709, 2488, 2372, 2346, 2365, 2386, 2389, 2376, 2363, 2393, 2525, 2804, 3225, 3741, 4289, 4812, 5278, 5665, 5957, 6143, 6231, 6233, 6163, 6031, 5849, 5631, 5391, },
89  /* LAT: -30 */ { 4877, 4636, 4396, 4161, 3934, 3715, 3496, 3265, 3009, 2733, 2476, 2294, 2226, 2251, 2318, 2389, 2454, 2502, 2522, 2534, 2604, 2807, 3173, 3664, 4196, 4692, 5109, 5424, 5625, 5725, 5751, 5724, 5644, 5511, 5331, 5114, 4877, },
90  /* LAT: -20 */ { 4320, 4107, 3897, 3692, 3496, 3313, 3144, 2981, 2802, 2604, 2413, 2280, 2241, 2286, 2376, 2487, 2614, 2741, 2827, 2861, 2887, 2986, 3230, 3618, 4069, 4495, 4842, 5074, 5178, 5185, 5155, 5108, 5026, 4898, 4731, 4534, 4320, },
91  /* LAT: -10 */ { 3790, 3628, 3475, 3328, 3192, 3072, 2968, 2876, 2778, 2662, 2540, 2442, 2398, 2424, 2512, 2642, 2797, 2955, 3076, 3137, 3151, 3179, 3306, 3561, 3888, 4210, 4473, 4634, 4668, 4615, 4548, 4484, 4396, 4272, 4122, 3958, 3790, },
92  /* LAT: 0 */ { 3412, 3318, 3234, 3161, 3105, 3067, 3040, 3021, 2995, 2946, 2867, 2773, 2694, 2665, 2709, 2813, 2945, 3080, 3194, 3268, 3299, 3321, 3398, 3557, 3768, 3981, 4161, 4265, 4270, 4202, 4114, 4022, 3911, 3779, 3645, 3520, 3412, },
93  /* LAT: 10 */ { 3282, 3251, 3230, 3226, 3250, 3297, 3352, 3404, 3438, 3428, 3359, 3244, 3119, 3025, 3001, 3044, 3125, 3224, 3324, 3409, 3472, 3535, 3624, 3743, 3880, 4019, 4138, 4208, 4209, 4147, 4037, 3893, 3731, 3572, 3436, 3339, 3282, },
94  /* LAT: 20 */ { 3399, 3401, 3427, 3480, 3571, 3692, 3820, 3936, 4016, 4028, 3954, 3813, 3647, 3509, 3435, 3424, 3460, 3533, 3630, 3728, 3819, 3918, 4029, 4141, 4251, 4363, 4466, 4531, 4541, 4482, 4344, 4141, 3913, 3703, 3540, 3439, 3399, },
95  /* LAT: 30 */ { 3722, 3728, 3781, 3880, 4022, 4192, 4367, 4523, 4630, 4656, 4584, 4429, 4241, 4078, 3975, 3930, 3935, 3987, 4074, 4173, 4271, 4377, 4494, 4612, 4732, 4859, 4981, 5067, 5092, 5033, 4876, 4636, 4361, 4107, 3907, 3778, 3722, },
96  /* LAT: 40 */ { 4222, 4218, 4282, 4404, 4570, 4757, 4939, 5096, 5200, 5226, 5158, 5011, 4825, 4652, 4526, 4451, 4427, 4450, 4511, 4591, 4678, 4776, 4891, 5025, 5176, 5338, 5490, 5600, 5638, 5582, 5426, 5187, 4914, 4654, 4443, 4297, 4222, },
97  /* LAT: 50 */ { 4832, 4822, 4876, 4985, 5130, 5289, 5438, 5560, 5634, 5644, 5581, 5455, 5293, 5131, 4994, 4898, 4844, 4833, 4859, 4910, 4980, 5071, 5190, 5341, 5516, 5701, 5867, 5984, 6027, 5981, 5850, 5654, 5430, 5215, 5035, 4905, 4832, },
98  /* LAT: 60 */ { 5392, 5378, 5404, 5465, 5550, 5644, 5731, 5798, 5832, 5824, 5771, 5678, 5558, 5430, 5312, 5217, 5151, 5118, 5116, 5144, 5201, 5288, 5406, 5554, 5720, 5888, 6035, 6138, 6181, 6158, 6075, 5950, 5804, 5662, 5540, 5448, 5392, },
99  /* LAT: 70 */ { 5726, 5704, 5700, 5711, 5732, 5758, 5782, 5798, 5798, 5780, 5742, 5685, 5616, 5540, 5467, 5403, 5356, 5328, 5324, 5344, 5389, 5459, 5551, 5660, 5777, 5891, 5989, 6062, 6100, 6103, 6074, 6021, 5954, 5884, 5819, 5765, 5726, },
100  /* LAT: 80 */ { 5790, 5772, 5757, 5744, 5734, 5724, 5715, 5703, 5689, 5671, 5649, 5624, 5598, 5571, 5547, 5527, 5513, 5509, 5514, 5529, 5555, 5590, 5633, 5681, 5730, 5778, 5821, 5855, 5879, 5892, 5894, 5887, 5873, 5853, 5832, 5810, 5790, },
101  /* LAT: 90 */ { 5684, 5684, 5684, 5684, 5684, 5684, 5684, 5684, 5684, 5684, 5684, 5684, 5684, 5684, 5684, 5684, 5684, 5684, 5684, 5684, 5684, 5684, 5684, 5684, 5684, 5684, 5684, 5684, 5684, 5684, 5684, 5684, 5684, 5684, 5684, 5684, 5684, },
102 };
103 
104 static const double latitudes[19] = {-90, -80, -70, -60, -50, -40, -30, -20, -10, 0,
105  10, 20, 30, 40, 50, 60, 70, 80, 90};
106 static const double longitudes[37] = {-180, -170, -160, -150, -140, -130, -120, -110, -100, -90,
107  -80, -70, -60, -50, -40, -30, -20, -10, 0, 10, 20, 30, 40,
108  50, 60, 70, 80, 90, 100, 110, 120, 130, 140, 150, 160, 170, 180};
109 
110 
111 // Function to perform linear interpolation
112 double interpolate(double v1, double v2, double factor) {
113  return v1 + (v2 - v1) * factor;
114 }
115 
116 // Helper function to find the closest lower index
117 int findLowerIndex(double value, const double* array, int size) {
118  for (int i = 0; i < size - 1; ++i) {
119  if (array[i] <= value && array[i + 1] >= value) {
120  return i;
121  }
122  }
123  return size - 2; // if value is beyond the array range, return the second last index
124 }
125 
126 void calculateMagneticFieldStrengthGauss(const double lat, const double lon, const double alt,
127  double& mag_east_gauss, double& mag_north_gauss, double& mag_up_gauss) {
128  (void)alt;
129 
130  int latIndex = findLowerIndex(lat, latitudes, 19);
131  int lonIndex = findLowerIndex(lon, longitudes, 37);
132 
133  double latFactor = (lat - latitudes[latIndex]) / (latitudes[latIndex + 1] - latitudes[latIndex]);
134  double lonFactor = (lon - longitudes[lonIndex]) / (longitudes[lonIndex + 1] - longitudes[lonIndex]);
135 
136  auto interpolateTable = [&](const int16_t table[19][37]) {
137  double v1 = interpolate(table[latIndex][lonIndex], table[latIndex][lonIndex + 1], lonFactor);
138  double v2 = interpolate(table[latIndex + 1][lonIndex], table[latIndex + 1][lonIndex + 1], lonFactor);
139  return interpolate(v1, v2, latFactor);
140  };
141 
142  double declination_rad = interpolateTable(declination_table) / 10000.0;
143  double inclination_rad = interpolateTable(inclination_table) / 10000.0;
144  double strength_gauss = interpolateTable(strength_table) / 10000.0;
145 
146  mag_east_gauss = strength_gauss * cos(inclination_rad) * sin(declination_rad);
147  mag_north_gauss = strength_gauss * cos(inclination_rad) * cos(declination_rad);
148  mag_up_gauss = strength_gauss * sin(inclination_rad);
149 }
calculateMagneticFieldStrengthGauss
void calculateMagneticFieldStrengthGauss(const double lat, const double lon, const double alt, double &mag_east_gauss, double &mag_north_gauss, double &mag_up_gauss)
Definition: wmm.cpp:126
interpolate
double interpolate(double v1, double v2, double factor)
Definition: wmm.cpp:112
inclination_table
static constexpr const int16_t inclination_table[19][37]
Definition: wmm.cpp:54
wmm.hpp
strength_table
static constexpr const int16_t strength_table[19][37]
Definition: wmm.cpp:81
longitudes
static const double longitudes[37]
Definition: wmm.cpp:106
latitudes
static const double latitudes[19]
Definition: wmm.cpp:104
declination_table
static constexpr const int16_t declination_table[19][37]
Definition: wmm.cpp:27
findLowerIndex
int findLowerIndex(double value, const double *array, int size)
Definition: wmm.cpp:117

inno_vtol_dynamics
Author(s): Roman Fedorenko, Dmitry Ponomarev, Ezra Tal, Winter Guerra
autogenerated on Mon Dec 9 2024 03:13:35