fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-28 05:53:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

geo_mag_declination: Fix interpolation when inputs are outside of sampling min and max.

Browse Source
This commit is contained in:
Stephan Brown
2017-02-04 08:52:04 +01:00
committed by Lorenz Meier
parent 20e7bd082a
commit 0d219caae3
1 changed files with 19 additions and 14 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/geo_lookup/geo_mag_declination.c
+19 -14
View File
@@ -34,9 +34,10 @@
/** /**
* @file geo_mag_declination.c * @file geo_mag_declination.c
* *
* Calculation / lookup table for earth magnetic field declination. * Calculation / lookup table for Earth's magnetic field declination.
* *
* Lookup table from Scott Ferguson <scottfromscott@gmail.com> * Lookup table from Scott Ferguson <scottfromscott@gmail.com> and
* Stephan Brown <stephan.brown.07@gmail.com>
* *
* XXX Lookup table currently too coarse in resolution (only full degrees) * XXX Lookup table currently too coarse in resolution (only full degrees)
* and lat/lon res - needs extension medium term. * and lat/lon res - needs extension medium term.
@@ -53,6 +54,8 @@
#define SAMPLING_MIN_LON -180.0f #define SAMPLING_MIN_LON -180.0f
#define SAMPLING_MAX_LON 180.0f #define SAMPLING_MAX_LON 180.0f
#define constrain(val, min, max) (val < min) ? min : ((val > max) ? max : val)
static const int8_t declination_table[13][37] = \ static const int8_t declination_table[13][37] = \
{ {
{ 47, 45, 44, 43, 41, 40, 38, 36, 33, 28, 23, 16, 10, 4, -1, -5, -9, -14, -19, -26, -33, -41, -48, -55, -61, -67, -71, -74, -75, -72, -61, -23, 23, 41, 46, 47, 47 }, { 47, 45, 44, 43, 41, 40, 38, 36, 33, 28, 23, 16, 10, 4, -1, -5, -9, -14, -19, -26, -33, -41, -48, -55, -61, -67, -71, -74, -75, -72, -61, -23, 23, 41, 46, 47, 47 },
@@ -71,24 +74,24 @@ static const int8_t declination_table[13][37] = \
}; };
static float get_lookup_table_val(unsigned lat, unsigned lon); static float get_lookup_table_val(unsigned lat, unsigned lon);
static unsigned get_lookup_table_index(float val, float min, float max); static unsigned get_lookup_table_index(float* val, float min, float max);
unsigned get_lookup_table_index(float val, float min, float max) unsigned get_lookup_table_index(float* val, float min, float max)
{ {
/* for the rare case of hitting the bounds exactly /* for the rare case of hitting the bounds exactly
* the rounding logic wouldn't fit, so enforce it. * the rounding logic wouldn't fit, so enforce it.
*/ */
/* limit to table bounds - required for maxima even when table spans full globe range */ /* limit to table bounds - required for maxima even when table spans full globe range */
if (val <= min) { if (*val < min) {
val = min; *val = min;
} }
/* limit to (table bounds - 1) because bilinear interpolation requires checking (index + 1) */ /* limit to (table bounds - 1) because bilinear interpolation requires checking (index + 1) */
if (val >= max) { if (*val > max) {
val = max - SAMPLING_RES; *val = max - SAMPLING_RES;
} }
return (-(min) + val) / SAMPLING_RES; return (-(min) + *val) / SAMPLING_RES;
} }
__EXPORT float get_mag_declination(float lat, float lon) __EXPORT float get_mag_declination(float lat, float lon)
@@ -108,8 +111,8 @@ __EXPORT float get_mag_declination(float lat, float lon)
float min_lon = (int)(lon / SAMPLING_RES) * SAMPLING_RES; float min_lon = (int)(lon / SAMPLING_RES) * SAMPLING_RES;
/* find index of nearest low sampling point */ /* find index of nearest low sampling point */
unsigned min_lat_index = get_lookup_table_index(min_lat, SAMPLING_MIN_LAT, SAMPLING_MAX_LAT); unsigned min_lat_index = get_lookup_table_index(&min_lat, SAMPLING_MIN_LAT, SAMPLING_MAX_LAT);
unsigned min_lon_index = get_lookup_table_index(min_lon, SAMPLING_MIN_LON, SAMPLING_MAX_LON); unsigned min_lon_index = get_lookup_table_index(&min_lon, SAMPLING_MIN_LON, SAMPLING_MAX_LON);
float declination_sw = get_lookup_table_val(min_lat_index, min_lon_index); float declination_sw = get_lookup_table_val(min_lat_index, min_lon_index);
float declination_se = get_lookup_table_val(min_lat_index, min_lon_index + 1); float declination_se = get_lookup_table_val(min_lat_index, min_lon_index + 1);
@@ -117,11 +120,13 @@ __EXPORT float get_mag_declination(float lat, float lon)
float declination_nw = get_lookup_table_val(min_lat_index + 1, min_lon_index); float declination_nw = get_lookup_table_val(min_lat_index + 1, min_lon_index);
/* perform bilinear interpolation on the four grid corners */ /* perform bilinear interpolation on the four grid corners */
float lat_scale = constrain((lat - min_lat) / SAMPLING_RES, 0.0f, 1.0f);
float lon_scale = constrain((lon - min_lon) / SAMPLING_RES, 0.0f, 1.0f);
float declination_min = ((lon - min_lon) / SAMPLING_RES) * (declination_se - declination_sw) + declination_sw; float declination_min = lon_scale * (declination_se - declination_sw) + declination_sw;
float declination_max = ((lon - min_lon) / SAMPLING_RES) * (declination_ne - declination_nw) + declination_nw; float declination_max = lon_scale * (declination_ne - declination_nw) + declination_nw;
return ((lat - min_lat) / SAMPLING_RES) * (declination_max - declination_min) + declination_min; return lat_scale * (declination_max - declination_min) + declination_min;
} }
float get_lookup_table_val(unsigned lat_index, unsigned lon_index) float get_lookup_table_val(unsigned lat_index, unsigned lon_index)