diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2014-05-03 12:40:29 +0200 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2014-05-03 12:40:29 +0200 |
commit | 0be1f36571e74b407e312866b6eb77547b9e4c2b (patch) | |
tree | b10670f2106482852e55e82b2f5bb56aadc837ad /src/lib | |
parent | 22f262a241256e54aa6a03763689bdcdeb39be87 (diff) | |
parent | 5429b82ae0fe777e31a1a52fa98f472f343e33af (diff) | |
download | px4-firmware-0be1f36571e74b407e312866b6eb77547b9e4c2b.tar.gz px4-firmware-0be1f36571e74b407e312866b6eb77547b9e4c2b.tar.bz2 px4-firmware-0be1f36571e74b407e312866b6eb77547b9e4c2b.zip |
Merge branch 'autodeclination' of github.com:PX4/Firmware into autodeclination
Diffstat (limited to 'src/lib')
-rw-r--r-- | src/lib/geo/geo_mag_declination.c | 19 |
1 files changed, 9 insertions, 10 deletions
diff --git a/src/lib/geo/geo_mag_declination.c b/src/lib/geo/geo_mag_declination.c index b96f87721..09eac38f4 100644 --- a/src/lib/geo/geo_mag_declination.c +++ b/src/lib/geo/geo_mag_declination.c @@ -45,15 +45,14 @@ #include <geo/geo.h> - /** set this always to the sampling in degrees for the table below */ -#define SAMPLING_RES 10 +#define SAMPLING_RES 10.0f #define SAMPLING_MIN_LAT -60.0f #define SAMPLING_MAX_LAT 60.0f #define SAMPLING_MIN_LON -180.0f #define SAMPLING_MAX_LON 180.0f -static const char declination_table[13][37] = \ +static const int8_t declination_table[13][37] = \ { 46, 45, 44, 42, 41, 40, 38, 36, 33, 28, 23, 16, 10, 4, -1, -5, -9, -14, -19, -26, -33, -40, -48, -55, -61, \ -66, -71, -74, -75, -72, -61, -25, 22, 40, 45, 47, 46, 30, 30, 30, 30, 29, 29, 29, 29, 27, 24, 18, 11, 3, \ @@ -90,8 +89,8 @@ __EXPORT float get_mag_declination(float lat, float lon) } /* round down to nearest sampling resolution */ - int min_lat = (((int)lat) / SAMPLING_RES) * SAMPLING_RES; - int min_lon = (((int)lon) / SAMPLING_RES) * SAMPLING_RES; + int min_lat = (int)(lat / SAMPLING_RES) * SAMPLING_RES; + int min_lon = (int)(lon / SAMPLING_RES) * SAMPLING_RES; /* for the rare case of hitting the bounds exactly * the rounding logic wouldn't fit, so enforce it. @@ -103,7 +102,7 @@ __EXPORT float get_mag_declination(float lat, float lon) } if (lat >= SAMPLING_MAX_LAT) { - min_lat = (((int)lat) / SAMPLING_RES) * SAMPLING_RES - SAMPLING_RES; + min_lat = (int)(lat / SAMPLING_RES) * SAMPLING_RES - SAMPLING_RES; } if (lon <= SAMPLING_MIN_LON) { @@ -111,12 +110,12 @@ __EXPORT float get_mag_declination(float lat, float lon) } if (lon >= SAMPLING_MAX_LON) { - min_lon = (((int)lon) / SAMPLING_RES) * SAMPLING_RES - SAMPLING_RES; + min_lon = (int)(lon / SAMPLING_RES) * SAMPLING_RES - SAMPLING_RES; } /* find index of nearest low sampling point */ - unsigned min_lat_index = (90 + min_lat) / SAMPLING_RES; - unsigned min_lon_index = (180 + min_lat) / SAMPLING_RES; + unsigned min_lat_index = (-(SAMPLING_MIN_LAT) + min_lat) / SAMPLING_RES; + unsigned min_lon_index = (-(SAMPLING_MIN_LON) + min_lon) / SAMPLING_RES; 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); @@ -128,7 +127,7 @@ __EXPORT float get_mag_declination(float lat, float lon) float declination_min = ((lon - min_lon) / SAMPLING_RES) * (declination_se - declination_sw) + declination_sw; float declination_max = ((lon - min_lon) / SAMPLING_RES) * (declination_ne - declination_nw) + declination_nw; - return (lat - min_lat) / SAMPLING_RES * (declination_max - declination_min) + declination_min; + return ((lat - min_lat) / SAMPLING_RES) * (declination_max - declination_min) + declination_min; } float get_lookup_table_val(unsigned lat_index, unsigned lon_index) |