Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_Declination: ensure indexing into declination tables is always correct 

add constants for table sizes
This commit is contained in:
Andy Piper 2022-03-08 13:42:50 +00:00 committed by Andrew Tridgell
parent 1a58b3b025
commit b112f8dba1
3 changed files with 11 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
libraries/AP_Declination/AP_Declination.cpp
View File

@ -66,8 +66,8 @@ bool AP_Declination::get_mag_field_ef(float latitude_deg, float longitude_deg, f
} }
/* find index of nearest low sampling point */ /* find index of nearest low sampling point */
uint32_t min_lat_index = static_cast<uint32_t>((-(SAMPLING_MIN_LAT) + min_lat) / SAMPLING_RES); uint32_t min_lat_index = constrain_int32(static_cast<uint32_t>((-(SAMPLING_MIN_LAT) + min_lat) / SAMPLING_RES), 0, LAT_TABLE_SIZE - 2);
uint32_t min_lon_index = static_cast<uint32_t>((-(SAMPLING_MIN_LON) + min_lon) / SAMPLING_RES); uint32_t min_lon_index = constrain_int32(static_cast<uint32_t>((-(SAMPLING_MIN_LON) + min_lon) / SAMPLING_RES), 0, LON_TABLE_SIZE -2);
/* calculate intensity */ /* calculate intensity */

9
libraries/AP_Declination/AP_Declination.h
View File

@ -35,7 +35,10 @@ private:
static const float SAMPLING_MIN_LON; static const float SAMPLING_MIN_LON;
static const float SAMPLING_MAX_LON; static const float SAMPLING_MAX_LON;
static const float declination_table[19][37]; static const uint32_t LAT_TABLE_SIZE = 19;
static const float inclination_table[19][37]; static const uint32_t LON_TABLE_SIZE = 37;
static const float intensity_table[19][37];
static const float declination_table[LAT_TABLE_SIZE][LON_TABLE_SIZE];
static const float inclination_table[LAT_TABLE_SIZE][LON_TABLE_SIZE];
static const float intensity_table[LAT_TABLE_SIZE][LON_TABLE_SIZE];
}; };

6
libraries/AP_Declination/tables.cpp
View File

@ -9,7 +9,7 @@ const float AP_Declination::SAMPLING_MAX_LAT = 90;
const float AP_Declination::SAMPLING_MIN_LON = -180; const float AP_Declination::SAMPLING_MIN_LON = -180;
const float AP_Declination::SAMPLING_MAX_LON = 180; const float AP_Declination::SAMPLING_MAX_LON = 180;
const float AP_Declination::declination_table[19][37] = { const float AP_Declination::declination_table[LAT_TABLE_SIZE][LON_TABLE_SIZE] = {
{149.10950f,139.10950f,129.10950f,119.10950f,109.10949f,99.10950f,89.10950f,79.10950f,69.10950f,59.10950f,49.10950f,39.10950f,29.10950f,19.10950f,9.10950f,-0.89050f,-10.89050f,-20.89050f,-30.89050f,-40.89050f,-50.89050f,-60.89050f,-70.89050f,-80.89050f,-90.89050f,-100.89050f,-110.89050f,-120.89050f,-130.89050f,-140.89050f,-150.89050f,-160.89050f,-170.89050f,179.10950f,169.10950f,159.10950f,149.10950f}, {149.10950f,139.10950f,129.10950f,119.10950f,109.10949f,99.10950f,89.10950f,79.10950f,69.10950f,59.10950f,49.10950f,39.10950f,29.10950f,19.10950f,9.10950f,-0.89050f,-10.89050f,-20.89050f,-30.89050f,-40.89050f,-50.89050f,-60.89050f,-70.89050f,-80.89050f,-90.89050f,-100.89050f,-110.89050f,-120.89050f,-130.89050f,-140.89050f,-150.89050f,-160.89050f,-170.89050f,179.10950f,169.10950f,159.10950f,149.10950f},
{129.37759f,117.14583f,106.01898f,95.84726f,86.44522f,77.63150f,69.24826f,61.16874f,53.29825f,45.57105f,37.94414f,30.38880f,22.88112f,15.39339f,7.88854f,0.31945f,-7.36677f,-15.22089f,-23.28322f,-31.57827f,-40.11442f,-48.88906f,-57.89765f,-67.14429f,-76.65158f,-86.46832f,-96.67422f,-107.38079f,-118.72599f,-130.85732f,-143.89431f,-157.86353f,-172.61739f,172.21319f,157.16190f,142.76170f,129.37759f}, {129.37759f,117.14583f,106.01898f,95.84726f,86.44522f,77.63150f,69.24826f,61.16874f,53.29825f,45.57105f,37.94414f,30.38880f,22.88112f,15.39339f,7.88854f,0.31945f,-7.36677f,-15.22089f,-23.28322f,-31.57827f,-40.11442f,-48.88906f,-57.89765f,-67.14429f,-76.65158f,-86.46832f,-96.67422f,-107.38079f,-118.72599f,-130.85732f,-143.89431f,-157.86353f,-172.61739f,172.21319f,157.16190f,142.76170f,129.37759f},
{85.60184f,77.69003f,71.32207f,65.86993f,60.92414f,56.17033f,51.35320f,46.28164f,40.84704f,35.03587f,28.92623f,22.66416f,16.41848f,10.31921f,4.39763f,-1.44271f,-7.40082f,-13.70324f,-20.51470f,-27.87783f,-35.70713f,-43.83304f,-52.06997f,-60.27655f,-68.39086f,-76.44339f,-84.56374f,-93.00460f,-102.21930f,-113.07088f,-127.37057f,-149.05145f,176.63172f,138.21637f,112.07842f,96.22737f,85.60184f}, {85.60184f,77.69003f,71.32207f,65.86993f,60.92414f,56.17033f,51.35320f,46.28164f,40.84704f,35.03587f,28.92623f,22.66416f,16.41848f,10.31921f,4.39763f,-1.44271f,-7.40082f,-13.70324f,-20.51470f,-27.87783f,-35.70713f,-43.83304f,-52.06997f,-60.27655f,-68.39086f,-76.44339f,-84.56374f,-93.00460f,-102.21930f,-113.07088f,-127.37057f,-149.05145f,176.63172f,138.21637f,112.07842f,96.22737f,85.60184f},
@ -31,7 +31,7 @@ const float AP_Declination::declination_table[19][37] = {
{-177.79784f,-167.79784f,-157.79784f,-147.79784f,-137.79784f,-127.79784f,-117.79784f,-107.79784f,-97.79784f,-87.79784f,-77.79784f,-67.79784f,-57.79784f,-47.79784f,-37.79784f,-27.79784f,-17.79784f,-7.79784f,2.20217f,12.20217f,22.20217f,32.20217f,42.20217f,52.20217f,62.20217f,72.20217f,82.20217f,92.20217f,102.20217f,112.20217f,122.20217f,132.20217f,142.20217f,152.20217f,162.20217f,172.20217f,-177.79784f} {-177.79784f,-167.79784f,-157.79784f,-147.79784f,-137.79784f,-127.79784f,-117.79784f,-107.79784f,-97.79784f,-87.79784f,-77.79784f,-67.79784f,-57.79784f,-47.79784f,-37.79784f,-27.79784f,-17.79784f,-7.79784f,2.20217f,12.20217f,22.20217f,32.20217f,42.20217f,52.20217f,62.20217f,72.20217f,82.20217f,92.20217f,102.20217f,112.20217f,122.20217f,132.20217f,142.20217f,152.20217f,162.20217f,172.20217f,-177.79784f}
}; };
const float AP_Declination::inclination_table[19][37] = { const float AP_Declination::inclination_table[LAT_TABLE_SIZE][LON_TABLE_SIZE] = {
{-72.08447f,-72.08447f,-72.08447f,-72.08447f,-72.08447f,-72.08447f,-72.08447f,-72.08447f,-72.08447f,-72.08447f,-72.08447f,-72.08447f,-72.08447f,-72.08447f,-72.08447f,-72.08447f,-72.08447f,-72.08447f,-72.08447f,-72.08447f,-72.08447f,-72.08447f,-72.08447f,-72.08447f,-72.08447f,-72.08447f,-72.08447f,-72.08447f,-72.08447f,-72.08447f,-72.08447f,-72.08447f,-72.08447f,-72.08447f,-72.08447f,-72.08447f,-72.08447f}, {-72.08447f,-72.08447f,-72.08447f,-72.08447f,-72.08447f,-72.08447f,-72.08447f,-72.08447f,-72.08447f,-72.08447f,-72.08447f,-72.08447f,-72.08447f,-72.08447f,-72.08447f,-72.08447f,-72.08447f,-72.08447f,-72.08447f,-72.08447f,-72.08447f,-72.08447f,-72.08447f,-72.08447f,-72.08447f,-72.08447f,-72.08447f,-72.08447f,-72.08447f,-72.08447f,-72.08447f,-72.08447f,-72.08447f,-72.08447f,-72.08447f,-72.08447f,-72.08447f},
{-78.33243f,-77.56645f,-76.64486f,-75.60941f,-74.49599f,-73.33711f,-72.16456f,-71.01082f,-69.90877f,-68.88978f,-67.98065f,-67.20063f,-66.55969f,-66.05909f,-65.69426f,-65.45930f,-65.35147f,-65.37404f,-65.53651f,-65.85220f,-66.33408f,-66.99021f,-67.82010f,-68.81276f,-69.94649f,-71.18994f,-72.50361f,-73.84119f,-75.15044f,-76.37388f,-77.45008f,-78.31699f,-78.91913f,-79.21830f,-79.20379f,-78.89480f,-78.33243f}, {-78.33243f,-77.56645f,-76.64486f,-75.60941f,-74.49599f,-73.33711f,-72.16456f,-71.01082f,-69.90877f,-68.88978f,-67.98065f,-67.20063f,-66.55969f,-66.05909f,-65.69426f,-65.45930f,-65.35147f,-65.37404f,-65.53651f,-65.85220f,-66.33408f,-66.99021f,-67.82010f,-68.81276f,-69.94649f,-71.18994f,-72.50361f,-73.84119f,-75.15044f,-76.37388f,-77.45008f,-78.31699f,-78.91913f,-79.21830f,-79.20379f,-78.89480f,-78.33243f},
{-80.91847f,-79.09801f,-77.26826f,-75.41050f,-73.49957f,-71.51974f,-69.48020f,-67.42760f,-65.44927f,-63.66181f,-62.18407f,-61.10090f,-60.43119f,-60.11709f,-60.04466f,-60.08935f,-60.16521f,-60.25535f,-60.41391f,-60.74312f,-61.35672f,-62.34264f,-63.73840f,-65.52698f,-67.65072f,-70.03207f,-72.58967f,-75.24472f,-77.91857f,-80.52353f,-82.93966f,-84.94483f,-86.05606f,-85.75384f,-84.42566f,-82.72116f,-80.91847f}, {-80.91847f,-79.09801f,-77.26826f,-75.41050f,-73.49957f,-71.51974f,-69.48020f,-67.42760f,-65.44927f,-63.66181f,-62.18407f,-61.10090f,-60.43119f,-60.11709f,-60.04466f,-60.08935f,-60.16521f,-60.25535f,-60.41391f,-60.74312f,-61.35672f,-62.34264f,-63.73840f,-65.52698f,-67.65072f,-70.03207f,-72.58967f,-75.24472f,-77.91857f,-80.52353f,-82.93966f,-84.94483f,-86.05606f,-85.75384f,-84.42566f,-82.72116f,-80.91847f},
@ -53,7 +53,7 @@ const float AP_Declination::inclination_table[19][37] = {
{88.07502f,88.07502f,88.07502f,88.07502f,88.07502f,88.07502f,88.07502f,88.07502f,88.07502f,88.07502f,88.07502f,88.07502f,88.07502f,88.07502f,88.07502f,88.07502f,88.07502f,88.07502f,88.07502f,88.07502f,88.07502f,88.07502f,88.07502f,88.07502f,88.07502f,88.07502f,88.07502f,88.07502f,88.07502f,88.07502f,88.07502f,88.07502f,88.07502f,88.07502f,88.07502f,88.07502f,88.07502f} {88.07502f,88.07502f,88.07502f,88.07502f,88.07502f,88.07502f,88.07502f,88.07502f,88.07502f,88.07502f,88.07502f,88.07502f,88.07502f,88.07502f,88.07502f,88.07502f,88.07502f,88.07502f,88.07502f,88.07502f,88.07502f,88.07502f,88.07502f,88.07502f,88.07502f,88.07502f,88.07502f,88.07502f,88.07502f,88.07502f,88.07502f,88.07502f,88.07502f,88.07502f,88.07502f,88.07502f,88.07502f}
}; };
const float AP_Declination::intensity_table[19][37] = { const float AP_Declination::intensity_table[LAT_TABLE_SIZE][LON_TABLE_SIZE] = {
{0.54677f,0.54677f,0.54677f,0.54677f,0.54677f,0.54677f,0.54677f,0.54677f,0.54677f,0.54677f,0.54677f,0.54677f,0.54677f,0.54677f,0.54677f,0.54677f,0.54677f,0.54677f,0.54677f,0.54677f,0.54677f,0.54677f,0.54677f,0.54677f,0.54677f,0.54677f,0.54677f,0.54677f,0.54677f,0.54677f,0.54677f,0.54677f,0.54677f,0.54677f,0.54677f,0.54677f,0.54677f}, {0.54677f,0.54677f,0.54677f,0.54677f,0.54677f,0.54677f,0.54677f,0.54677f,0.54677f,0.54677f,0.54677f,0.54677f,0.54677f,0.54677f,0.54677f,0.54677f,0.54677f,0.54677f,0.54677f,0.54677f,0.54677f,0.54677f,0.54677f,0.54677f,0.54677f,0.54677f,0.54677f,0.54677f,0.54677f,0.54677f,0.54677f,0.54677f,0.54677f,0.54677f,0.54677f,0.54677f,0.54677f},
{0.60733f,0.60103f,0.59321f,0.58408f,0.57385f,0.56274f,0.55099f,0.53886f,0.52664f,0.51464f,0.50318f,0.49258f,0.48311f,0.47506f,0.46864f,0.46409f,0.46158f,0.46131f,0.46341f,0.46797f,0.47499f,0.48434f,0.49579f,0.50895f,0.52332f,0.53833f,0.55334f,0.56771f,0.58086f,0.59227f,0.60156f,0.60848f,0.61292f,0.61488f,0.61448f,0.61189f,0.60733f}, {0.60733f,0.60103f,0.59321f,0.58408f,0.57385f,0.56274f,0.55099f,0.53886f,0.52664f,0.51464f,0.50318f,0.49258f,0.48311f,0.47506f,0.46864f,0.46409f,0.46158f,0.46131f,0.46341f,0.46797f,0.47499f,0.48434f,0.49579f,0.50895f,0.52332f,0.53833f,0.55334f,0.56771f,0.58086f,0.59227f,0.60156f,0.60848f,0.61292f,0.61488f,0.61448f,0.61189f,0.60733f},
{0.63154f,0.61845f,0.60363f,0.58729f,0.56950f,0.55031f,0.52986f,0.50843f,0.48660f,0.46508f,0.44473f,0.42628f,0.41025f,0.39690f,0.38632f,0.37857f,0.37385f,0.37260f,0.37540f,0.38291f,0.39557f,0.41347f,0.43621f,0.46292f,0.49236f,0.52306f,0.55344f,0.58192f,0.60704f,0.62760f,0.64283f,0.65244f,0.65659f,0.65582f,0.65087f,0.64254f,0.63154f}, {0.63154f,0.61845f,0.60363f,0.58729f,0.56950f,0.55031f,0.52986f,0.50843f,0.48660f,0.46508f,0.44473f,0.42628f,0.41025f,0.39690f,0.38632f,0.37857f,0.37385f,0.37260f,0.37540f,0.38291f,0.39557f,0.41347f,0.43621f,0.46292f,0.49236f,0.52306f,0.55344f,0.58192f,0.60704f,0.62760f,0.64283f,0.65244f,0.65659f,0.65582f,0.65087f,0.64254f,0.63154f},