dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

world_magnetic_model: move from ecl

This commit is contained in:
Daniel Agar 2021-07-14 17:09:03 -04:00
parent b5430c22c4
commit fedf8f0266
18 changed files with 168 additions and 158 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
src/examples/fake_magnetometer/FakeMagnetometer.cpp
View File

@ -33,7 +33,7 @@
#include "FakeMagnetometer.hpp"
#include <lib/ecl/geo_lookup/geo_mag_declination.h>
#include <lib/world_magnetic_model/geo_mag_declination.h>
using namespace matrix;
using namespace time_literals;

1
src/lib/CMakeLists.txt
View File

@ -70,3 +70,4 @@ add_subdirectory(terrain_estimation)
add_subdirectory(tunes)
add_subdirectory(version)
add_subdirectory(weather_vane)
add_subdirectory(world_magnetic_model)

6
src/lib/ecl/CMakeLists.txt
View File

@ -97,9 +97,12 @@ if(CMAKE_SOURCE_DIR STREQUAL PROJECT_SOURCE_DIR)
# temporary
configure_file(${CMAKE_CURRENT_SOURCE_DIR}/../mathlib/math/filter/AlphaFilter.hpp ${CMAKE_CURRENT_BINARY_DIR}/mathlib/math/filter/AlphaFilter.hpp COPYONLY)
include_directories(${CMAKE_CURRENT_BINARY_DIR})
include_directories(${CMAKE_CURRENT_SOURCE_DIR})
add_subdirectory(${CMAKE_CURRENT_SOURCE_DIR}/../world_magnetic_model world_magnetic_model)
include_directories(${CMAKE_CURRENT_SOURCE_DIR}/../..)
if(MSVC)
add_compile_options(
/W4
@ -178,7 +181,6 @@ endif()
add_subdirectory(airdata)
add_subdirectory(EKF)
add_subdirectory(geo)
add_subdirectory(geo_lookup)
if(BUILD_TESTING AND CMAKE_SOURCE_DIR STREQUAL PROJECT_SOURCE_DIR)
add_subdirectory(test)

2
src/lib/ecl/EKF/CMakeLists.txt
View File

@ -56,7 +56,7 @@ add_library(ecl_EKF
add_dependencies(ecl_EKF prebuild_targets)
target_compile_definitions(ecl_EKF PRIVATE -DMODULE_NAME="ecl/EKF")
target_include_directories(ecl_EKF PUBLIC ${ECL_SOURCE_DIR})
target_link_libraries(ecl_EKF PRIVATE ecl_geo ecl_geo_lookup)
target_link_libraries(ecl_EKF PRIVATE ecl_geo world_magnetic_model)
set_target_properties(ecl_EKF PROPERTIES PUBLIC_HEADER "ekf.h")

2
src/lib/ecl/EKF/gps_checks.cpp
View File

@ -42,7 +42,7 @@
#include "ekf.h"
#include <ecl.h>
#include <geo_lookup/geo_mag_declination.h>
#include <lib/world_magnetic_model/geo_mag_declination.h>
#include <mathlib/mathlib.h>
// GPS pre-flight check bit locations

125
src/lib/ecl/geo_lookup/geo_magnetic_tables.hpp
View File

@ -1,125 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2020 Estimation and Control Library (ECL). All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name ECL nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#include <stdint.h>
static constexpr float SAMPLING_RES = 10;
static constexpr float SAMPLING_MIN_LAT = -90;
static constexpr float SAMPLING_MAX_LAT = 90;
static constexpr float SAMPLING_MIN_LON = -180;
static constexpr float SAMPLING_MAX_LON = 180;
static constexpr int LAT_DIM = 19;
static constexpr int LON_DIM = 37;
// Magnetic declination data in radians * 10^-4
// Model: WMM-2020,
// Version: 0.5.1.11,
// Date: 2021.2356,
static constexpr const int16_t declination_table[19][37] {
// 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,
/* LAT: -90 */ { 26020, 24274, 22529, 20784, 19038, 17293, 15548, 13802, 12057, 10312, 8566, 6821, 5076, 3331, 1585, -160, -1905, -3650, -5396, -7141, -8886,-10632,-12377,-14122,-15868,-17613,-19358,-21104,-22849,-24594,-26340,-28085,-29831, 31256, 29511, 27765, 26020, },
/* LAT: -80 */ { 22590, 20454, 18510, 16733, 15090, 13550, 12085, 10673, 9297, 7947, 6614, 5295, 3985, 2681, 1373, 55, -1284, -2654, -4061, -5510, -7002, -8536,-10111,-11727,-13389,-15105,-16888,-18758,-20738,-22854,-25126,-27559,-30128, 30061, 27438, 24926, 22590, },
/* LAT: -70 */ { 14966, 13570, 12448, 11490, 10626, 9798, 8959, 8074, 7123, 6105, 5035, 3941, 2853, 1796, 772, -238, -1274, -2375, -3570, -4863, -6237, -7662, -9106,-10545,-11970,-13384,-14809,-16291,-17907,-19809,-22308,-26075, 30840, 24200, 19629, 16839, 14966, },
/* LAT: -60 */ { 8375, 8139, 7867, 7601, 7355, 7108, 6806, 6378, 5768, 4952, 3954, 2842, 1719, 685, -205, -969, -1699, -2516, -3508, -4692, -6012, -7374, -8691, -9902,-10968,-11869,-12588,-13084,-13256,-12806,-10720, -3613, 4760, 7561, 8359, 8500, 8375, },
/* LAT: -50 */ { 5450, 5491, 5441, 5358, 5291, 5259, 5230, 5108, 4770, 4114, 3110, 1841, 497, -694, -1581, -2161, -2566, -3002, -3670, -4660, -5884, -7151, -8291, -9198, -9806,-10057, -9876, -9132, -7634, -5282, -2383, 363, 2471, 3894, 4768, 5244, 5450, },
/* LAT: -40 */ { 3929, 4024, 4037, 3998, 3944, 3915, 3924, 3917, 3749, 3223, 2211, 777, -790, -2118, -3002, -3473, -3672, -3739, -3895, -4430, -5375, -6439, -7328, -7880, -8005, -7647, -6775, -5398, -3683, -1973, -507, 716, 1757, 2618, 3270, 3698, 3929, },
/* LAT: -30 */ { 2962, 3049, 3085, 3078, 3026, 2953, 2897, 2866, 2743, 2274, 1254, -254, -1864, -3134, -3895, -4259, -4356, -4167, -3737, -3482, -3801, -4530, -5236, -5605, -5500, -4932, -3983, -2774, -1554, -598, 85, 687, 1307, 1904, 2407, 2765, 2962, },
/* LAT: -20 */ { 2319, 2367, 2392, 2402, 2367, 2278, 2176, 2102, 1959, 1472, 433, -1044, -2527, -3608, -4165, -4302, -4118, -3581, -2728, -1909, -1595, -1923, -2571, -3053, -3107, -2762, -2133, -1312, -525, -25, 244, 548, 984, 1454, 1869, 2170, 2319, },
/* LAT: -10 */ { 1926, 1921, 1908, 1913, 1892, 1815, 1712, 1625, 1442, 899, -148, -1518, -2799, -3650, -3945, -3741, -3181, -2408, -1561, -795, -311, -321, -779, -1289, -1514, -1433, -1124, -622, -116, 135, 191, 354, 719, 1147, 1531, 1811, 1926, },
/* LAT: 0 */ { 1713, 1682, 1633, 1632, 1629, 1567, 1471, 1364, 1112, 497, -535, -1760, -2826, -3435, -3462, -2982, -2222, -1438, -780, -238, 183, 310, 44, -372, -635, -695, -597, -332, -35, 62, 9, 101, 436, 867, 1277, 1589, 1713, },
/* LAT: 10 */ { 1581, 1591, 1555, 1578, 1610, 1568, 1456, 1277, 904, 188, -829, -1909, -2750, -3111, -2930, -2338, -1565, -848, -325, 61, 389, 547, 396, 75, -164, -273, -293, -207, -98, -133, -266, -236, 60, 503, 975, 1375, 1581, },
/* LAT: 20 */ { 1405, 1555, 1620, 1716, 1808, 1794, 1647, 1352, 810, -46, -1090, -2050, -2663, -2792, -2480, -1887, -1177, -523, -55, 260, 521, 673, 590, 348, 145, 28, -57, -116, -190, -375, -607, -665, -436, 3, 537, 1050, 1405, },
/* LAT: 30 */ { 1115, 1482, 1744, 1970, 2133, 2148, 1966, 1549, 824, -206, -1331, -2222, -2653, -2606, -2217, -1642, -988, -366, 103, 414, 648, 800, 786, 639, 488, 364, 214, 12, -263, -634, -1001, -1162, -1008, -588, -15, 593, 1115, },
/* LAT: 40 */ { 770, 1358, 1852, 2244, 2493, 2539, 2326, 1796, 885, -348, -1599, -2483, -2819, -2677, -2236, -1646, -993, -361, 155, 525, 800, 1004, 1104, 1096, 1020, 874, 614, 215, -315, -918, -1441, -1690, -1579, -1166, -568, 108, 770, },
/* LAT: 50 */ { 495, 1244, 1924, 2478, 2843, 2948, 2718, 2062, 916, -594, -2035, -2968, -3273, -3087, -2599, -1954, -1243, -542, 79, 590, 1013, 1374, 1661, 1836, 1859, 1678, 1245, 551, -332, -1234, -1920, -2215, -2096, -1655, -1015, -276, 495, },
/* LAT: 60 */ { 320, 1177, 1983, 2674, 3173, 3379, 3158, 2336, 802, -1187, -2929, -3922, -4178, -3917, -3342, -2593, -1763, -918, -105, 651, 1347, 1983, 2532, 2935, 3104, 2930, 2310, 1214, -189, -1513, -2395, -2715, -2553, -2054, -1351, -539, 320, },
/* LAT: 70 */ { 125, 1068, 1968, 2765, 3374, 3659, 3388, 2214, -98, -2882, -4855, -5670, -5663, -5165, -4384, -3440, -2407, -1334, -254, 812, 1842, 2812, 3686, 4400, 4858, 4906, 4323, 2886, 720, -1388, -2701, -3154, -2995, -2455, -1694, -812, 125, },
/* LAT: 80 */ { -447, 481, 1339, 2035, 2416, 2203, 901, -1946, -5362, -7492, -8178, -7990, -7318, -6369, -5256, -4040, -2761, -1445, -112, 1221, 2540, 3825, 5055, 6194, 7191, 7952, 8302, 7884, 6049, 2457, -1054, -2796, -3195, -2887, -2220, -1372, -447, },
/* LAT: 90 */ { -30329,-28584,-26838,-25093,-23347,-21602,-19857,-18111,-16366,-14621,-12875,-11130, -9385, -7640, -5895, -4149, -2404, -659, 1086, 2831, 4576, 6322, 8067, 9812, 11558, 13303, 15048, 16794, 18539, 20284, 22030, 23775, 25521, 27266, 29012, 30757,-30329, },
};
// Magnetic inclination data in radians * 10^-4
// Model: WMM-2020,
// Version: 0.5.1.11,
// Date: 2021.2356,
static constexpr const int16_t inclination_table[19][37] {
// 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,
/* LAT: -90 */ { -12580,-12580,-12580,-12580,-12580,-12580,-12580,-12580,-12580,-12580,-12580,-12580,-12580,-12579,-12579,-12579,-12579,-12579,-12579,-12579,-12579,-12580,-12580,-12580,-12580,-12580,-12580,-12580,-12580,-12580,-12580,-12580,-12580,-12580,-12580,-12580,-12580, },
/* LAT: -80 */ { -13667,-13533,-13373,-13192,-12998,-12796,-12592,-12391,-12199,-12022,-11865,-11731,-11620,-11534,-11470,-11429,-11409,-11412,-11440,-11494,-11577,-11691,-11835,-12008,-12206,-12422,-12651,-12884,-13112,-13325,-13512,-13663,-13768,-13820,-13818,-13765,-13667, },
/* LAT: -70 */ { -14117,-13798,-13479,-13156,-12823,-12479,-12123,-11764,-11420,-11109,-10855,-10670,-10556,-10503,-10490,-10495,-10504,-10516,-10540,-10597,-10704,-10877,-11121,-11433,-11804,-12220,-12667,-13131,-13597,-14050,-14469,-14817,-15010,-14960,-14730,-14433,-14117, },
/* LAT: -60 */ { -13525,-13173,-12835,-12503,-12159,-11787,-11371,-10916,-10446,-10013, -9679, -9499, -9492, -9626, -9831,-10028,-10160,-10211,-10203,-10191,-10237,-10391,-10675,-11078,-11572,-12126,-12711,-13307,-13895,-14456,-14953,-15244,-15075,-14693,-14289,-13897,-13525, },
/* LAT: -50 */ { -12498,-12158,-11829,-11507,-11184,-10837,-10438, -9965, -9434, -8909, -8513, -8378, -8570, -9026, -9591,-10109,-10479,-10655,-10637,-10493,-10350,-10349,-10559,-10966,-11502,-12092,-12679,-13220,-13668,-13968,-14078,-14006,-13803,-13520,-13193,-12847,-12498, },
/* LAT: -40 */ { -11240,-10895,-10550,-10206, -9866, -9527, -9165, -8738, -8215, -7647, -7212, -7155, -7600, -8416, -9339,-10175,-10844,-11293,-11451,-11301,-10965,-10687,-10665,-10934,-11389,-11893,-12342,-12670,-12833,-12840,-12749,-12610,-12430,-12197,-11911,-11584,-11240, },
/* LAT: -30 */ { -9601, -9228, -8853, -8464, -8071, -7694, -7335, -6941, -6426, -5812, -5349, -5410, -6156, -7347, -8600, -9704,-10627,-11351,-11771,-11784,-11428,-10921,-10575,-10562,-10806,-11127,-11388,-11508,-11450,-11274,-11097,-10958,-10809,-10600,-10319, -9975, -9601, },
/* LAT: -20 */ { -7369, -6938, -6528, -6103, -5658, -5226, -4831, -4412, -3842, -3153, -2691, -2922, -4002, -5600, -7232, -8616, -9697,-10489,-10948,-11003,-10657,-10051, -9488, -9233, -9274, -9435, -9580, -9604, -9435, -9162, -8963, -8869, -8757, -8546, -8229, -7820, -7369, },
/* LAT: -10 */ { -4411, -3889, -3445, -3013, -2556, -2104, -1685, -1221, -587, 124, 503, 108, -1188, -3088, -5069, -6700, -7820, -8475, -8757, -8710, -8317, -7645, -6980, -6625, -6580, -6673, -6793, -6821, -6636, -6338, -6177, -6174, -6122, -5900, -5517, -4994, -4411, },
/* LAT: 0 */ { -901, -294, 160, 557, 974, 1392, 1787, 2242, 2838, 3426, 3645, 3177, 1911, 20, -2028, -3703, -4741, -5192, -5265, -5106, -4680, -3977, -3272, -2893, -2830, -2902, -3030, -3105, -2977, -2739, -2676, -2804, -2850, -2657, -2233, -1610, -901, },
/* LAT: 10 */ { 2566, 3178, 3602, 3938, 4291, 4659, 5016, 5414, 5878, 6258, 6308, 5840, 4780, 3230, 1538, 143, -687, -956, -877, -649, -249, 379, 1012, 1354, 1416, 1366, 1263, 1171, 1215, 1322, 1255, 1004, 828, 908, 1259, 1854, 2566, },
/* LAT: 20 */ { 5419, 5937, 6310, 6601, 6914, 7258, 7605, 7961, 8305, 8514, 8437, 7988, 7166, 6078, 4951, 4030, 3484, 3350, 3489, 3727, 4051, 4509, 4968, 5225, 5280, 5257, 5205, 5149, 5140, 5127, 4960, 4640, 4353, 4269, 4440, 4858, 5419, },
/* LAT: 30 */ { 7569, 7938, 8252, 8533, 8841, 9190, 9551, 9898, 10182, 10302, 10165, 9752, 9130, 8420, 7756, 7238, 6937, 6885, 7020, 7227, 7469, 7764, 8051, 8225, 8280, 8289, 8288, 8279, 8260, 8182, 7966, 7621, 7272, 7055, 7038, 7230, 7569, },
/* LAT: 40 */ { 9266, 9485, 9740, 10025, 10352, 10715, 11086, 11429, 11686, 11774, 11629, 11273, 10801, 10327, 9928, 9640, 9485, 9475, 9580, 9738, 9910, 10091, 10262, 10387, 10463, 10521, 10573, 10606, 10592, 10486, 10249, 9903, 9539, 9256, 9113, 9124, 9266, },
/* LAT: 50 */ { 10801, 10923, 11125, 11396, 11720, 12076, 12433, 12753, 12978, 13041, 12904, 12607, 12241, 11894, 11616, 11426, 11329, 11321, 11384, 11484, 11595, 11709, 11824, 11939, 12056, 12177, 12288, 12361, 12355, 12235, 11992, 11667, 11328, 11044, 10854, 10774, 10801, },
/* LAT: 60 */ { 12318, 12393, 12545, 12766, 13038, 13341, 13645, 13913, 14091, 14119, 13983, 13732, 13442, 13169, 12946, 12787, 12694, 12660, 12673, 12719, 12785, 12868, 12973, 13103, 13261, 13435, 13600, 13711, 13720, 13604, 13382, 13103, 12824, 12587, 12416, 12325, 12318, },
/* LAT: 70 */ { 13759, 13804, 13902, 14046, 14228, 14434, 14645, 14827, 14931, 14909, 14768, 14564, 14344, 14139, 13964, 13828, 13733, 13677, 13657, 13668, 13709, 13779, 13881, 14016, 14181, 14368, 14552, 14694, 14743, 14674, 14515, 14317, 14122, 13957, 13837, 13770, 13759, },
/* LAT: 80 */ { 15003, 15017, 15057, 15118, 15197, 15285, 15365, 15410, 15387, 15303, 15187, 15060, 14936, 14821, 14721, 14638, 14576, 14534, 14515, 14518, 14544, 14593, 14664, 14755, 14866, 14992, 15127, 15258, 15365, 15411, 15376, 15293, 15200, 15117, 15053, 15015, 15003, },
/* LAT: 90 */ { 15391, 15391, 15391, 15391, 15391, 15391, 15391, 15391, 15391, 15391, 15391, 15391, 15391, 15391, 15391, 15391, 15391, 15391, 15391, 15391, 15391, 15391, 15391, 15391, 15391, 15391, 15391, 15391, 15391, 15391, 15391, 15391, 15391, 15391, 15391, 15391, 15391, },
};
// Magnetic strength data in milli-Gauss * 10
// Model: WMM-2020,
// Version: 0.5.1.11,
// Date: 2021.2356,
static constexpr const int16_t strength_table[19][37] {
// 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,
/* LAT: -90 */ { 5459, 5459, 5459, 5459, 5459, 5459, 5459, 5459, 5459, 5459, 5459, 5459, 5459, 5459, 5459, 5459, 5459, 5459, 5459, 5459, 5459, 5459, 5459, 5459, 5459, 5459, 5459, 5459, 5459, 5459, 5459, 5459, 5459, 5459, 5459, 5459, 5459, },
/* LAT: -80 */ { 6065, 6003, 5924, 5833, 5731, 5619, 5502, 5380, 5258, 5138, 5024, 4918, 4824, 4744, 4681, 4635, 4610, 4608, 4629, 4674, 4744, 4837, 4951, 5082, 5225, 5375, 5524, 5667, 5798, 5912, 6005, 6075, 6119, 6139, 6136, 6111, 6065, },
/* LAT: -70 */ { 6310, 6178, 6029, 5866, 5688, 5496, 5292, 5077, 4857, 4642, 4439, 4256, 4097, 3966, 3861, 3784, 3736, 3723, 3751, 3825, 3952, 4130, 4357, 4624, 4918, 5225, 5528, 5812, 6062, 6267, 6419, 6515, 6557, 6551, 6503, 6420, 6310, },
/* LAT: -60 */ { 6194, 6004, 5804, 5597, 5379, 5145, 4889, 4611, 4318, 4028, 3760, 3531, 3351, 3215, 3116, 3042, 2992, 2971, 2996, 3085, 3251, 3500, 3824, 4208, 4629, 5063, 5484, 5869, 6194, 6444, 6609, 6689, 6692, 6630, 6518, 6369, 6194, },
/* LAT: -50 */ { 5850, 5622, 5391, 5162, 4931, 4687, 4417, 4114, 3785, 3453, 3149, 2908, 2745, 2652, 2603, 2570, 2538, 2512, 2517, 2587, 2757, 3042, 3434, 3903, 4408, 4914, 5390, 5810, 6150, 6393, 6533, 6575, 6534, 6425, 6265, 6068, 5850, },
/* LAT: -40 */ { 5398, 5153, 4910, 4672, 4438, 4202, 3948, 3664, 3350, 3024, 2724, 2500, 2380, 2353, 2372, 2395, 2402, 2392, 2381, 2408, 2533, 2801, 3213, 3723, 4269, 4794, 5263, 5654, 5947, 6136, 6226, 6230, 6161, 6032, 5852, 5635, 5398, },
/* LAT: -30 */ { 4881, 4642, 4405, 4171, 3946, 3728, 3509, 3278, 3023, 2748, 2489, 2305, 2232, 2255, 2322, 2394, 2461, 2514, 2539, 2552, 2615, 2806, 3159, 3642, 4172, 4671, 5094, 5414, 5619, 5720, 5747, 5721, 5642, 5511, 5332, 5117, 4881, },
/* LAT: -20 */ { 4323, 4112, 3905, 3701, 3505, 3323, 3155, 2992, 2815, 2618, 2427, 2291, 2247, 2287, 2375, 2485, 2614, 2745, 2838, 2877, 2900, 2990, 3221, 3599, 4047, 4475, 4827, 5066, 5174, 5183, 5153, 5106, 5023, 4896, 4730, 4534, 4323, },
/* LAT: -10 */ { 3791, 3632, 3480, 3335, 3199, 3079, 2977, 2886, 2790, 2677, 2556, 2455, 2406, 2425, 2508, 2636, 2792, 2953, 3080, 3146, 3162, 3185, 3302, 3548, 3871, 4193, 4460, 4627, 4665, 4614, 4546, 4482, 4393, 4268, 4120, 3957, 3791, },
/* LAT: 0 */ { 3412, 3321, 3238, 3166, 3111, 3073, 3049, 3032, 3010, 2963, 2885, 2789, 2706, 2670, 2707, 2808, 2940, 3077, 3194, 3271, 3304, 3325, 3396, 3549, 3755, 3968, 4149, 4257, 4266, 4200, 4112, 4019, 3907, 3775, 3642, 3519, 3412, },
/* LAT: 10 */ { 3283, 3253, 3234, 3231, 3257, 3305, 3362, 3418, 3455, 3447, 3378, 3263, 3134, 3036, 3005, 3042, 3122, 3220, 3321, 3407, 3471, 3532, 3618, 3734, 3867, 4004, 4124, 4196, 4200, 4140, 4031, 3889, 3727, 3568, 3433, 3338, 3283, },
/* LAT: 20 */ { 3400, 3404, 3432, 3488, 3581, 3703, 3834, 3953, 4036, 4049, 3976, 3834, 3665, 3522, 3442, 3426, 3459, 3530, 3625, 3722, 3812, 3909, 4019, 4130, 4237, 4347, 4448, 4515, 4527, 4471, 4335, 4135, 3908, 3699, 3537, 3437, 3400, },
/* LAT: 30 */ { 3723, 3732, 3789, 3891, 4036, 4208, 4385, 4543, 4653, 4680, 4607, 4451, 4259, 4091, 3982, 3933, 3934, 3982, 4067, 4164, 4260, 4365, 4480, 4598, 4717, 4842, 4961, 5048, 5076, 5021, 4866, 4628, 4355, 4101, 3903, 3776, 3723, },
/* LAT: 40 */ { 4223, 4224, 4292, 4418, 4587, 4776, 4961, 5118, 5223, 5249, 5180, 5030, 4840, 4663, 4532, 4453, 4425, 4444, 4503, 4580, 4665, 4761, 4876, 5009, 5160, 5320, 5471, 5581, 5623, 5571, 5418, 5181, 4907, 4648, 4437, 4294, 4223, },
/* LAT: 50 */ { 4831, 4828, 4888, 5001, 5149, 5310, 5460, 5580, 5653, 5662, 5597, 5468, 5304, 5137, 4998, 4898, 4841, 4827, 4850, 4899, 4967, 5056, 5175, 5324, 5499, 5683, 5850, 5969, 6016, 5975, 5845, 5649, 5424, 5207, 5027, 4899, 4831, },
/* LAT: 60 */ { 5392, 5383, 5415, 5481, 5569, 5664, 5750, 5815, 5847, 5836, 5781, 5685, 5562, 5432, 5312, 5214, 5146, 5111, 5108, 5134, 5190, 5275, 5392, 5539, 5705, 5874, 6022, 6128, 6174, 6154, 6071, 5945, 5797, 5653, 5531, 5442, 5392, },
/* LAT: 70 */ { 5726, 5709, 5709, 5723, 5746, 5773, 5796, 5809, 5807, 5787, 5746, 5687, 5615, 5538, 5463, 5398, 5349, 5321, 5315, 5334, 5378, 5447, 5538, 5647, 5764, 5879, 5979, 6053, 6093, 6097, 6068, 6015, 5947, 5877, 5812, 5761, 5726, },
/* LAT: 80 */ { 5789, 5772, 5759, 5748, 5738, 5729, 5719, 5707, 5691, 5672, 5649, 5623, 5595, 5567, 5542, 5521, 5506, 5501, 5505, 5520, 5545, 5580, 5623, 5671, 5721, 5769, 5812, 5847, 5871, 5885, 5887, 5880, 5867, 5848, 5828, 5808, 5789, },
/* LAT: 90 */ { 5679, 5679, 5679, 5679, 5679, 5679, 5679, 5679, 5679, 5679, 5679, 5679, 5679, 5679, 5679, 5679, 5679, 5679, 5679, 5679, 5679, 5679, 5679, 5679, 5679, 5679, 5679, 5679, 5679, 5679, 5679, 5679, 5679, 5679, 5679, 5679, 5679, },
};

1
src/lib/ecl/test/CMakeLists.txt
View File

@ -53,7 +53,6 @@ set(SRCS
test_EKF_terrain_estimator.cpp
test_SensorRangeFinder.cpp
test_geo.cpp
test_geo_lookup.cpp
test_EKF_utils.cpp
)
add_executable(ECL_GTESTS ${SRCS})

15
src/lib/ecl/geo_lookup/CMakeLists.txt → src/lib/world_magnetic_model/CMakeLists.txt
View File

@ -1,6 +1,6 @@
############################################################################
#
# Copyright (c) 2018 ECL Development Team. All rights reserved.
# Copyright (c) 2018-2021 PX4 Development Team. All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
@ -12,7 +12,7 @@
# notice, this list of conditions and the following disclaimer in
# the documentation and/or other materials provided with the
# distribution.
# 3. Neither the name ECL nor the names of its contributors may be
# 3. Neither the name PX4 nor the names of its contributors may be
# used to endorse or promote products derived from this software
# without specific prior written permission.
#
@ -31,11 +31,14 @@
#
############################################################################
add_library(ecl_geo_lookup
add_library(world_magnetic_model
geo_mag_declination.cpp
geo_mag_declination.h
geo_magnetic_tables.hpp
)
add_dependencies(ecl_geo_lookup prebuild_targets)
target_compile_definitions(ecl_geo_lookup PRIVATE -DMODULE_NAME="ecl/geo_lookup")
target_include_directories(ecl_geo_lookup PUBLIC ${ECL_SOURCE_DIR})
target_compile_options(world_magnetic_model PRIVATE ${MAX_CUSTOM_OPT_LEVEL})
if(CMAKE_PROJECT_NAME MATCHES "px4") # temporary
add_compile_options(-Wno-double-promotion)
px4_add_unit_gtest(SRC test_geo_lookup.cpp LINKLIBS world_magnetic_model)
endif()

4
src/lib/ecl/geo_lookup/fetch_noaa_table.py → src/lib/world_magnetic_model/fetch_noaa_table.py
View File

@ -1,7 +1,7 @@
#!/usr/bin/env python3
############################################################################
#
# Copyright (c) 2020 Estimation and Control Library (ECL). All rights reserved.
# Copyright (c) 2020-2021 PX4 Development Team. All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
@ -13,7 +13,7 @@
# notice, this list of conditions and the following disclaimer in
# the documentation and/or other materials provided with the
# distribution.
# 3. Neither the name ECL nor the names of its contributors may be
# 3. Neither the name PX4 nor the names of its contributors may be
# used to endorse or promote products derived from this software
# without specific prior written permission.
#

4
src/lib/ecl/geo_lookup/generate_gtest.py → src/lib/world_magnetic_model/generate_gtest.py
View File

@ -1,7 +1,7 @@
#!/usr/bin/env python3
############################################################################
#
# Copyright (c) 2020 Estimation and Control Library (ECL). All rights reserved.
# Copyright (c) 2020-2021 PX4 Development Team. All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
@ -13,7 +13,7 @@
# notice, this list of conditions and the following disclaimer in
# the documentation and/or other materials provided with the
# distribution.
# 3. Neither the name ECL nor the names of its contributors may be
# 3. Neither the name PX4 nor the names of its contributors may be
# used to endorse or promote products derived from this software
# without specific prior written permission.
#

16
src/lib/ecl/geo_lookup/geo_mag_declination.cpp → src/lib/world_magnetic_model/geo_mag_declination.cpp
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2014-2020 Estimation and Control Library (ECL). All rights reserved.
* Copyright (c) 2014-2021 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@ -12,7 +12,7 @@
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name ECL nor the names of its contributors may be
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
@ -101,19 +101,23 @@ static constexpr float get_table_data(float lat, float lon, const int16_t table[
return lat_scale * (data_max - data_min) + data_min;
}
float get_mag_declination_radians(float lat, float lon) {
float get_mag_declination_radians(float lat, float lon)
{
return get_table_data(lat, lon, declination_table) * 1e-4f; // declination table stored as 10^-4 radians
}
float get_mag_declination_degrees(float lat, float lon) {
float get_mag_declination_degrees(float lat, float lon)
{
return math::degrees(get_mag_declination_radians(lat, lon));
}
float get_mag_inclination_radians(float lat, float lon) {
float get_mag_inclination_radians(float lat, float lon)
{
return get_table_data(lat, lon, inclination_table) * 1e-4f; // inclination table stored as 10^-4 radians
}
float get_mag_inclination_degrees(float lat, float lon) {
float get_mag_inclination_degrees(float lat, float lon)
{
return math::degrees(get_mag_inclination_radians(lat, lon));
}

4
src/lib/ecl/geo_lookup/geo_mag_declination.h → src/lib/world_magnetic_model/geo_mag_declination.h
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2014-2020 Estimation and Control Library (ECL). All rights reserved.
* Copyright (c) 2014-2021 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@ -12,7 +12,7 @@
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name ECL nor the names of its contributors may be
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*

125
src/lib/world_magnetic_model/geo_magnetic_tables.hpp Normal file
View File

@ -0,0 +1,125 @@
/****************************************************************************
*
* Copyright (c) 2020 Estimation and Control Library (ECL). All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name ECL nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#include <stdint.h>
static constexpr float SAMPLING_RES = 10;
static constexpr float SAMPLING_MIN_LAT = -90;
static constexpr float SAMPLING_MAX_LAT = 90;
static constexpr float SAMPLING_MIN_LON = -180;
static constexpr float SAMPLING_MAX_LON = 180;
static constexpr int LAT_DIM = 19;
static constexpr int LON_DIM = 37;
// Magnetic declination data in radians * 10^-4
// Model: WMM-2020,
// Version: 0.5.1.11,
// Date: 2021.2356,
static constexpr const int16_t declination_table[19][37] {
// 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,
/* LAT: -90 */ { 26020, 24274, 22529, 20784, 19038, 17293, 15548, 13802, 12057, 10312, 8566, 6821, 5076, 3331, 1585, -160, -1905, -3650, -5396, -7141, -8886, -10632, -12377, -14122, -15868, -17613, -19358, -21104, -22849, -24594, -26340, -28085, -29831, 31256, 29511, 27765, 26020, },
/* LAT: -80 */ { 22590, 20454, 18510, 16733, 15090, 13550, 12085, 10673, 9297, 7947, 6614, 5295, 3985, 2681, 1373, 55, -1284, -2654, -4061, -5510, -7002, -8536, -10111, -11727, -13389, -15105, -16888, -18758, -20738, -22854, -25126, -27559, -30128, 30061, 27438, 24926, 22590, },
/* LAT: -70 */ { 14966, 13570, 12448, 11490, 10626, 9798, 8959, 8074, 7123, 6105, 5035, 3941, 2853, 1796, 772, -238, -1274, -2375, -3570, -4863, -6237, -7662, -9106, -10545, -11970, -13384, -14809, -16291, -17907, -19809, -22308, -26075, 30840, 24200, 19629, 16839, 14966, },
/* LAT: -60 */ { 8375, 8139, 7867, 7601, 7355, 7108, 6806, 6378, 5768, 4952, 3954, 2842, 1719, 685, -205, -969, -1699, -2516, -3508, -4692, -6012, -7374, -8691, -9902, -10968, -11869, -12588, -13084, -13256, -12806, -10720, -3613, 4760, 7561, 8359, 8500, 8375, },
/* LAT: -50 */ { 5450, 5491, 5441, 5358, 5291, 5259, 5230, 5108, 4770, 4114, 3110, 1841, 497, -694, -1581, -2161, -2566, -3002, -3670, -4660, -5884, -7151, -8291, -9198, -9806, -10057, -9876, -9132, -7634, -5282, -2383, 363, 2471, 3894, 4768, 5244, 5450, },
/* LAT: -40 */ { 3929, 4024, 4037, 3998, 3944, 3915, 3924, 3917, 3749, 3223, 2211, 777, -790, -2118, -3002, -3473, -3672, -3739, -3895, -4430, -5375, -6439, -7328, -7880, -8005, -7647, -6775, -5398, -3683, -1973, -507, 716, 1757, 2618, 3270, 3698, 3929, },
/* LAT: -30 */ { 2962, 3049, 3085, 3078, 3026, 2953, 2897, 2866, 2743, 2274, 1254, -254, -1864, -3134, -3895, -4259, -4356, -4167, -3737, -3482, -3801, -4530, -5236, -5605, -5500, -4932, -3983, -2774, -1554, -598, 85, 687, 1307, 1904, 2407, 2765, 2962, },
/* LAT: -20 */ { 2319, 2367, 2392, 2402, 2367, 2278, 2176, 2102, 1959, 1472, 433, -1044, -2527, -3608, -4165, -4302, -4118, -3581, -2728, -1909, -1595, -1923, -2571, -3053, -3107, -2762, -2133, -1312, -525, -25, 244, 548, 984, 1454, 1869, 2170, 2319, },
/* LAT: -10 */ { 1926, 1921, 1908, 1913, 1892, 1815, 1712, 1625, 1442, 899, -148, -1518, -2799, -3650, -3945, -3741, -3181, -2408, -1561, -795, -311, -321, -779, -1289, -1514, -1433, -1124, -622, -116, 135, 191, 354, 719, 1147, 1531, 1811, 1926, },
/* LAT: 0 */ { 1713, 1682, 1633, 1632, 1629, 1567, 1471, 1364, 1112, 497, -535, -1760, -2826, -3435, -3462, -2982, -2222, -1438, -780, -238, 183, 310, 44, -372, -635, -695, -597, -332, -35, 62, 9, 101, 436, 867, 1277, 1589, 1713, },
/* LAT: 10 */ { 1581, 1591, 1555, 1578, 1610, 1568, 1456, 1277, 904, 188, -829, -1909, -2750, -3111, -2930, -2338, -1565, -848, -325, 61, 389, 547, 396, 75, -164, -273, -293, -207, -98, -133, -266, -236, 60, 503, 975, 1375, 1581, },
/* LAT: 20 */ { 1405, 1555, 1620, 1716, 1808, 1794, 1647, 1352, 810, -46, -1090, -2050, -2663, -2792, -2480, -1887, -1177, -523, -55, 260, 521, 673, 590, 348, 145, 28, -57, -116, -190, -375, -607, -665, -436, 3, 537, 1050, 1405, },
/* LAT: 30 */ { 1115, 1482, 1744, 1970, 2133, 2148, 1966, 1549, 824, -206, -1331, -2222, -2653, -2606, -2217, -1642, -988, -366, 103, 414, 648, 800, 786, 639, 488, 364, 214, 12, -263, -634, -1001, -1162, -1008, -588, -15, 593, 1115, },
/* LAT: 40 */ { 770, 1358, 1852, 2244, 2493, 2539, 2326, 1796, 885, -348, -1599, -2483, -2819, -2677, -2236, -1646, -993, -361, 155, 525, 800, 1004, 1104, 1096, 1020, 874, 614, 215, -315, -918, -1441, -1690, -1579, -1166, -568, 108, 770, },
/* LAT: 50 */ { 495, 1244, 1924, 2478, 2843, 2948, 2718, 2062, 916, -594, -2035, -2968, -3273, -3087, -2599, -1954, -1243, -542, 79, 590, 1013, 1374, 1661, 1836, 1859, 1678, 1245, 551, -332, -1234, -1920, -2215, -2096, -1655, -1015, -276, 495, },
/* LAT: 60 */ { 320, 1177, 1983, 2674, 3173, 3379, 3158, 2336, 802, -1187, -2929, -3922, -4178, -3917, -3342, -2593, -1763, -918, -105, 651, 1347, 1983, 2532, 2935, 3104, 2930, 2310, 1214, -189, -1513, -2395, -2715, -2553, -2054, -1351, -539, 320, },
/* LAT: 70 */ { 125, 1068, 1968, 2765, 3374, 3659, 3388, 2214, -98, -2882, -4855, -5670, -5663, -5165, -4384, -3440, -2407, -1334, -254, 812, 1842, 2812, 3686, 4400, 4858, 4906, 4323, 2886, 720, -1388, -2701, -3154, -2995, -2455, -1694, -812, 125, },
/* LAT: 80 */ { -447, 481, 1339, 2035, 2416, 2203, 901, -1946, -5362, -7492, -8178, -7990, -7318, -6369, -5256, -4040, -2761, -1445, -112, 1221, 2540, 3825, 5055, 6194, 7191, 7952, 8302, 7884, 6049, 2457, -1054, -2796, -3195, -2887, -2220, -1372, -447, },
/* LAT: 90 */ { -30329, -28584, -26838, -25093, -23347, -21602, -19857, -18111, -16366, -14621, -12875, -11130, -9385, -7640, -5895, -4149, -2404, -659, 1086, 2831, 4576, 6322, 8067, 9812, 11558, 13303, 15048, 16794, 18539, 20284, 22030, 23775, 25521, 27266, 29012, 30757, -30329, },
};
// Magnetic inclination data in radians * 10^-4
// Model: WMM-2020,
// Version: 0.5.1.11,
// Date: 2021.2356,
static constexpr const int16_t inclination_table[19][37] {
// 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,
/* LAT: -90 */ { -12580, -12580, -12580, -12580, -12580, -12580, -12580, -12580, -12580, -12580, -12580, -12580, -12580, -12579, -12579, -12579, -12579, -12579, -12579, -12579, -12579, -12580, -12580, -12580, -12580, -12580, -12580, -12580, -12580, -12580, -12580, -12580, -12580, -12580, -12580, -12580, -12580, },
/* LAT: -80 */ { -13667, -13533, -13373, -13192, -12998, -12796, -12592, -12391, -12199, -12022, -11865, -11731, -11620, -11534, -11470, -11429, -11409, -11412, -11440, -11494, -11577, -11691, -11835, -12008, -12206, -12422, -12651, -12884, -13112, -13325, -13512, -13663, -13768, -13820, -13818, -13765, -13667, },
/* LAT: -70 */ { -14117, -13798, -13479, -13156, -12823, -12479, -12123, -11764, -11420, -11109, -10855, -10670, -10556, -10503, -10490, -10495, -10504, -10516, -10540, -10597, -10704, -10877, -11121, -11433, -11804, -12220, -12667, -13131, -13597, -14050, -14469, -14817, -15010, -14960, -14730, -14433, -14117, },
/* LAT: -60 */ { -13525, -13173, -12835, -12503, -12159, -11787, -11371, -10916, -10446, -10013, -9679, -9499, -9492, -9626, -9831, -10028, -10160, -10211, -10203, -10191, -10237, -10391, -10675, -11078, -11572, -12126, -12711, -13307, -13895, -14456, -14953, -15244, -15075, -14693, -14289, -13897, -13525, },
/* LAT: -50 */ { -12498, -12158, -11829, -11507, -11184, -10837, -10438, -9965, -9434, -8909, -8513, -8378, -8570, -9026, -9591, -10109, -10479, -10655, -10637, -10493, -10350, -10349, -10559, -10966, -11502, -12092, -12679, -13220, -13668, -13968, -14078, -14006, -13803, -13520, -13193, -12847, -12498, },
/* LAT: -40 */ { -11240, -10895, -10550, -10206, -9866, -9527, -9165, -8738, -8215, -7647, -7212, -7155, -7600, -8416, -9339, -10175, -10844, -11293, -11451, -11301, -10965, -10687, -10665, -10934, -11389, -11893, -12342, -12670, -12833, -12840, -12749, -12610, -12430, -12197, -11911, -11584, -11240, },
/* LAT: -30 */ { -9601, -9228, -8853, -8464, -8071, -7694, -7335, -6941, -6426, -5812, -5349, -5410, -6156, -7347, -8600, -9704, -10627, -11351, -11771, -11784, -11428, -10921, -10575, -10562, -10806, -11127, -11388, -11508, -11450, -11274, -11097, -10958, -10809, -10600, -10319, -9975, -9601, },
/* LAT: -20 */ { -7369, -6938, -6528, -6103, -5658, -5226, -4831, -4412, -3842, -3153, -2691, -2922, -4002, -5600, -7232, -8616, -9697, -10489, -10948, -11003, -10657, -10051, -9488, -9233, -9274, -9435, -9580, -9604, -9435, -9162, -8963, -8869, -8757, -8546, -8229, -7820, -7369, },
/* LAT: -10 */ { -4411, -3889, -3445, -3013, -2556, -2104, -1685, -1221, -587, 124, 503, 108, -1188, -3088, -5069, -6700, -7820, -8475, -8757, -8710, -8317, -7645, -6980, -6625, -6580, -6673, -6793, -6821, -6636, -6338, -6177, -6174, -6122, -5900, -5517, -4994, -4411, },
/* LAT: 0 */ { -901, -294, 160, 557, 974, 1392, 1787, 2242, 2838, 3426, 3645, 3177, 1911, 20, -2028, -3703, -4741, -5192, -5265, -5106, -4680, -3977, -3272, -2893, -2830, -2902, -3030, -3105, -2977, -2739, -2676, -2804, -2850, -2657, -2233, -1610, -901, },
/* LAT: 10 */ { 2566, 3178, 3602, 3938, 4291, 4659, 5016, 5414, 5878, 6258, 6308, 5840, 4780, 3230, 1538, 143, -687, -956, -877, -649, -249, 379, 1012, 1354, 1416, 1366, 1263, 1171, 1215, 1322, 1255, 1004, 828, 908, 1259, 1854, 2566, },
/* LAT: 20 */ { 5419, 5937, 6310, 6601, 6914, 7258, 7605, 7961, 8305, 8514, 8437, 7988, 7166, 6078, 4951, 4030, 3484, 3350, 3489, 3727, 4051, 4509, 4968, 5225, 5280, 5257, 5205, 5149, 5140, 5127, 4960, 4640, 4353, 4269, 4440, 4858, 5419, },
/* LAT: 30 */ { 7569, 7938, 8252, 8533, 8841, 9190, 9551, 9898, 10182, 10302, 10165, 9752, 9130, 8420, 7756, 7238, 6937, 6885, 7020, 7227, 7469, 7764, 8051, 8225, 8280, 8289, 8288, 8279, 8260, 8182, 7966, 7621, 7272, 7055, 7038, 7230, 7569, },
/* LAT: 40 */ { 9266, 9485, 9740, 10025, 10352, 10715, 11086, 11429, 11686, 11774, 11629, 11273, 10801, 10327, 9928, 9640, 9485, 9475, 9580, 9738, 9910, 10091, 10262, 10387, 10463, 10521, 10573, 10606, 10592, 10486, 10249, 9903, 9539, 9256, 9113, 9124, 9266, },
/* LAT: 50 */ { 10801, 10923, 11125, 11396, 11720, 12076, 12433, 12753, 12978, 13041, 12904, 12607, 12241, 11894, 11616, 11426, 11329, 11321, 11384, 11484, 11595, 11709, 11824, 11939, 12056, 12177, 12288, 12361, 12355, 12235, 11992, 11667, 11328, 11044, 10854, 10774, 10801, },
/* LAT: 60 */ { 12318, 12393, 12545, 12766, 13038, 13341, 13645, 13913, 14091, 14119, 13983, 13732, 13442, 13169, 12946, 12787, 12694, 12660, 12673, 12719, 12785, 12868, 12973, 13103, 13261, 13435, 13600, 13711, 13720, 13604, 13382, 13103, 12824, 12587, 12416, 12325, 12318, },
/* LAT: 70 */ { 13759, 13804, 13902, 14046, 14228, 14434, 14645, 14827, 14931, 14909, 14768, 14564, 14344, 14139, 13964, 13828, 13733, 13677, 13657, 13668, 13709, 13779, 13881, 14016, 14181, 14368, 14552, 14694, 14743, 14674, 14515, 14317, 14122, 13957, 13837, 13770, 13759, },
/* LAT: 80 */ { 15003, 15017, 15057, 15118, 15197, 15285, 15365, 15410, 15387, 15303, 15187, 15060, 14936, 14821, 14721, 14638, 14576, 14534, 14515, 14518, 14544, 14593, 14664, 14755, 14866, 14992, 15127, 15258, 15365, 15411, 15376, 15293, 15200, 15117, 15053, 15015, 15003, },
/* LAT: 90 */ { 15391, 15391, 15391, 15391, 15391, 15391, 15391, 15391, 15391, 15391, 15391, 15391, 15391, 15391, 15391, 15391, 15391, 15391, 15391, 15391, 15391, 15391, 15391, 15391, 15391, 15391, 15391, 15391, 15391, 15391, 15391, 15391, 15391, 15391, 15391, 15391, 15391, },
};
// Magnetic strength data in milli-Gauss * 10
// Model: WMM-2020,
// Version: 0.5.1.11,
// Date: 2021.2356,
static constexpr const int16_t strength_table[19][37] {
// 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,
/* LAT: -90 */ { 5459, 5459, 5459, 5459, 5459, 5459, 5459, 5459, 5459, 5459, 5459, 5459, 5459, 5459, 5459, 5459, 5459, 5459, 5459, 5459, 5459, 5459, 5459, 5459, 5459, 5459, 5459, 5459, 5459, 5459, 5459, 5459, 5459, 5459, 5459, 5459, 5459, },
/* LAT: -80 */ { 6065, 6003, 5924, 5833, 5731, 5619, 5502, 5380, 5258, 5138, 5024, 4918, 4824, 4744, 4681, 4635, 4610, 4608, 4629, 4674, 4744, 4837, 4951, 5082, 5225, 5375, 5524, 5667, 5798, 5912, 6005, 6075, 6119, 6139, 6136, 6111, 6065, },
/* LAT: -70 */ { 6310, 6178, 6029, 5866, 5688, 5496, 5292, 5077, 4857, 4642, 4439, 4256, 4097, 3966, 3861, 3784, 3736, 3723, 3751, 3825, 3952, 4130, 4357, 4624, 4918, 5225, 5528, 5812, 6062, 6267, 6419, 6515, 6557, 6551, 6503, 6420, 6310, },
/* LAT: -60 */ { 6194, 6004, 5804, 5597, 5379, 5145, 4889, 4611, 4318, 4028, 3760, 3531, 3351, 3215, 3116, 3042, 2992, 2971, 2996, 3085, 3251, 3500, 3824, 4208, 4629, 5063, 5484, 5869, 6194, 6444, 6609, 6689, 6692, 6630, 6518, 6369, 6194, },
/* LAT: -50 */ { 5850, 5622, 5391, 5162, 4931, 4687, 4417, 4114, 3785, 3453, 3149, 2908, 2745, 2652, 2603, 2570, 2538, 2512, 2517, 2587, 2757, 3042, 3434, 3903, 4408, 4914, 5390, 5810, 6150, 6393, 6533, 6575, 6534, 6425, 6265, 6068, 5850, },
/* LAT: -40 */ { 5398, 5153, 4910, 4672, 4438, 4202, 3948, 3664, 3350, 3024, 2724, 2500, 2380, 2353, 2372, 2395, 2402, 2392, 2381, 2408, 2533, 2801, 3213, 3723, 4269, 4794, 5263, 5654, 5947, 6136, 6226, 6230, 6161, 6032, 5852, 5635, 5398, },
/* LAT: -30 */ { 4881, 4642, 4405, 4171, 3946, 3728, 3509, 3278, 3023, 2748, 2489, 2305, 2232, 2255, 2322, 2394, 2461, 2514, 2539, 2552, 2615, 2806, 3159, 3642, 4172, 4671, 5094, 5414, 5619, 5720, 5747, 5721, 5642, 5511, 5332, 5117, 4881, },
/* LAT: -20 */ { 4323, 4112, 3905, 3701, 3505, 3323, 3155, 2992, 2815, 2618, 2427, 2291, 2247, 2287, 2375, 2485, 2614, 2745, 2838, 2877, 2900, 2990, 3221, 3599, 4047, 4475, 4827, 5066, 5174, 5183, 5153, 5106, 5023, 4896, 4730, 4534, 4323, },
/* LAT: -10 */ { 3791, 3632, 3480, 3335, 3199, 3079, 2977, 2886, 2790, 2677, 2556, 2455, 2406, 2425, 2508, 2636, 2792, 2953, 3080, 3146, 3162, 3185, 3302, 3548, 3871, 4193, 4460, 4627, 4665, 4614, 4546, 4482, 4393, 4268, 4120, 3957, 3791, },
/* LAT: 0 */ { 3412, 3321, 3238, 3166, 3111, 3073, 3049, 3032, 3010, 2963, 2885, 2789, 2706, 2670, 2707, 2808, 2940, 3077, 3194, 3271, 3304, 3325, 3396, 3549, 3755, 3968, 4149, 4257, 4266, 4200, 4112, 4019, 3907, 3775, 3642, 3519, 3412, },
/* LAT: 10 */ { 3283, 3253, 3234, 3231, 3257, 3305, 3362, 3418, 3455, 3447, 3378, 3263, 3134, 3036, 3005, 3042, 3122, 3220, 3321, 3407, 3471, 3532, 3618, 3734, 3867, 4004, 4124, 4196, 4200, 4140, 4031, 3889, 3727, 3568, 3433, 3338, 3283, },
/* LAT: 20 */ { 3400, 3404, 3432, 3488, 3581, 3703, 3834, 3953, 4036, 4049, 3976, 3834, 3665, 3522, 3442, 3426, 3459, 3530, 3625, 3722, 3812, 3909, 4019, 4130, 4237, 4347, 4448, 4515, 4527, 4471, 4335, 4135, 3908, 3699, 3537, 3437, 3400, },
/* LAT: 30 */ { 3723, 3732, 3789, 3891, 4036, 4208, 4385, 4543, 4653, 4680, 4607, 4451, 4259, 4091, 3982, 3933, 3934, 3982, 4067, 4164, 4260, 4365, 4480, 4598, 4717, 4842, 4961, 5048, 5076, 5021, 4866, 4628, 4355, 4101, 3903, 3776, 3723, },
/* LAT: 40 */ { 4223, 4224, 4292, 4418, 4587, 4776, 4961, 5118, 5223, 5249, 5180, 5030, 4840, 4663, 4532, 4453, 4425, 4444, 4503, 4580, 4665, 4761, 4876, 5009, 5160, 5320, 5471, 5581, 5623, 5571, 5418, 5181, 4907, 4648, 4437, 4294, 4223, },
/* LAT: 50 */ { 4831, 4828, 4888, 5001, 5149, 5310, 5460, 5580, 5653, 5662, 5597, 5468, 5304, 5137, 4998, 4898, 4841, 4827, 4850, 4899, 4967, 5056, 5175, 5324, 5499, 5683, 5850, 5969, 6016, 5975, 5845, 5649, 5424, 5207, 5027, 4899, 4831, },
/* LAT: 60 */ { 5392, 5383, 5415, 5481, 5569, 5664, 5750, 5815, 5847, 5836, 5781, 5685, 5562, 5432, 5312, 5214, 5146, 5111, 5108, 5134, 5190, 5275, 5392, 5539, 5705, 5874, 6022, 6128, 6174, 6154, 6071, 5945, 5797, 5653, 5531, 5442, 5392, },
/* LAT: 70 */ { 5726, 5709, 5709, 5723, 5746, 5773, 5796, 5809, 5807, 5787, 5746, 5687, 5615, 5538, 5463, 5398, 5349, 5321, 5315, 5334, 5378, 5447, 5538, 5647, 5764, 5879, 5979, 6053, 6093, 6097, 6068, 6015, 5947, 5877, 5812, 5761, 5726, },
/* LAT: 80 */ { 5789, 5772, 5759, 5748, 5738, 5729, 5719, 5707, 5691, 5672, 5649, 5623, 5595, 5567, 5542, 5521, 5506, 5501, 5505, 5520, 5545, 5580, 5623, 5671, 5721, 5769, 5812, 5847, 5871, 5885, 5887, 5880, 5867, 5848, 5828, 5808, 5789, },
/* LAT: 90 */ { 5679, 5679, 5679, 5679, 5679, 5679, 5679, 5679, 5679, 5679, 5679, 5679, 5679, 5679, 5679, 5679, 5679, 5679, 5679, 5679, 5679, 5679, 5679, 5679, 5679, 5679, 5679, 5679, 5679, 5679, 5679, 5679, 5679, 5679, 5679, 5679, 5679, },
};

12
src/lib/ecl/test/test_geo_lookup.cpp → src/lib/world_magnetic_model/test_geo_lookup.cpp
View File

@ -35,10 +35,10 @@
#include <math.h>
#include <mathlib/mathlib.h>
#include <geo_lookup/geo_mag_declination.h>
#include "geo_mag_declination.h"
TEST(GeoLookupTest, declination) {
TEST(GeoLookupTest, declination)
{
EXPECT_NEAR(get_mag_declination_degrees(-50, -180), 31.22632, 0.40062 + 2);
EXPECT_NEAR(get_mag_declination_degrees(-50, -175), 31.43125, 0.39369 + 2);
EXPECT_NEAR(get_mag_declination_degrees(-50, -170), 31.45948, 0.38791 + 2);
@ -1720,7 +1720,8 @@ TEST(GeoLookupTest, declination) {
EXPECT_NEAR(get_mag_declination_degrees(60, 180), 1.83383, 0.40746 + 1);
}
TEST(GeoLookupTest, inclination) {
TEST(GeoLookupTest, inclination)
{
EXPECT_NEAR(get_mag_inclination_degrees(-50, -180), -71.61084, 0.21 + 2);
EXPECT_NEAR(get_mag_inclination_degrees(-50, -175), -70.62874, 0.21 + 2);
EXPECT_NEAR(get_mag_inclination_degrees(-50, -170), -69.66156, 0.21 + 2);
@ -3402,7 +3403,8 @@ TEST(GeoLookupTest, inclination) {
EXPECT_NEAR(get_mag_inclination_degrees(60, 180), 70.57639, 0.21 + 1.2);
}
TEST(GeoLookupTest, strength) {
TEST(GeoLookupTest, strength)
{
EXPECT_NEAR(get_mag_strength_tesla(-50, -180) * 1e9, 58500.4, 145 + 500);
EXPECT_NEAR(get_mag_strength_tesla(-50, -175) * 1e9, 57365.7, 145 + 500);
EXPECT_NEAR(get_mag_strength_tesla(-50, -170) * 1e9, 56217.8, 145 + 500);

2
src/modules/attitude_estimator_q/CMakeLists.txt
View File

@ -39,6 +39,6 @@ px4_add_module(
SRCS
attitude_estimator_q_main.cpp
DEPENDS
ecl_geo_lookup
world_magnetic_model
)

2
src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp
View File

@ -43,7 +43,7 @@
#include <drivers/drv_hrt.h>
#include <lib/ecl/geo/geo.h>
#include <lib/ecl/geo_lookup/geo_mag_declination.h>
#include <lib/world_magnetic_model/geo_mag_declination.h>
#include <lib/mathlib/mathlib.h>
#include <lib/parameters/param.h>
#include <matrix/math.hpp>

2
src/modules/commander/mag_calibration.cpp
View File

@ -53,7 +53,7 @@
#include <lib/sensor_calibration/Magnetometer.hpp>
#include <lib/sensor_calibration/Utilities.hpp>
#include <lib/conversion/rotation.h>
#include <lib/ecl/geo_lookup/geo_mag_declination.h>
#include <lib/world_magnetic_model/geo_mag_declination.h>
#include <lib/systemlib/mavlink_log.h>
#include <lib/parameters/param.h>
#include <lib/systemlib/err.h>

1
src/systemcmds/tests/CMakeLists.txt
View File

@ -104,7 +104,6 @@ px4_add_module(
SRCS
${srcs}
DEPENDS
ecl_geo_lookup # TODO: move this
output_limit
version
)