From b112f8dba1d25b1a4cef505d362513bd0cdf9bfc Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Tue, 8 Mar 2022 13:42:50 +0000 Subject: [PATCH] AP_Declination: ensure indexing into declination tables is always correct add constants for table sizes --- libraries/AP_Declination/AP_Declination.cpp | 4 ++-- libraries/AP_Declination/AP_Declination.h | 9 ++++++--- libraries/AP_Declination/tables.cpp | 6 +++--- 3 files changed, 11 insertions(+), 8 deletions(-) diff --git a/libraries/AP_Declination/AP_Declination.cpp b/libraries/AP_Declination/AP_Declination.cpp index e68c69a384..550312000e 100644 --- a/libraries/AP_Declination/AP_Declination.cpp +++ b/libraries/AP_Declination/AP_Declination.cpp @@ -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 */ - uint32_t min_lat_index = static_cast((-(SAMPLING_MIN_LAT) + min_lat) / SAMPLING_RES); - uint32_t min_lon_index = static_cast((-(SAMPLING_MIN_LON) + min_lon) / SAMPLING_RES); + uint32_t min_lat_index = constrain_int32(static_cast((-(SAMPLING_MIN_LAT) + min_lat) / SAMPLING_RES), 0, LAT_TABLE_SIZE - 2); + uint32_t min_lon_index = constrain_int32(static_cast((-(SAMPLING_MIN_LON) + min_lon) / SAMPLING_RES), 0, LON_TABLE_SIZE -2); /* calculate intensity */ diff --git a/libraries/AP_Declination/AP_Declination.h b/libraries/AP_Declination/AP_Declination.h index a9dec3fea0..2832428f05 100644 --- a/libraries/AP_Declination/AP_Declination.h +++ b/libraries/AP_Declination/AP_Declination.h @@ -35,7 +35,10 @@ private: static const float SAMPLING_MIN_LON; static const float SAMPLING_MAX_LON; - static const float declination_table[19][37]; - static const float inclination_table[19][37]; - static const float intensity_table[19][37]; + static const uint32_t LAT_TABLE_SIZE = 19; + static const uint32_t LON_TABLE_SIZE = 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]; }; diff --git a/libraries/AP_Declination/tables.cpp b/libraries/AP_Declination/tables.cpp index 5879f03916..2b1542559f 100644 --- a/libraries/AP_Declination/tables.cpp +++ b/libraries/AP_Declination/tables.cpp @@ -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_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}, {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}, @@ -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} }; -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}, {-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}, @@ -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} }; -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.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},