00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00046 #include <cstdint>
00047 #include <cmath>
00048 #include "rotors_gazebo_plugins/geo_mag_declination.h"
00049
00051 #define SAMPLING_RES 10.0f
00052 #define SAMPLING_MIN_LAT -60.0f
00053 #define SAMPLING_MAX_LAT 60.0f
00054 #define SAMPLING_MIN_LON -180.0f
00055 #define SAMPLING_MAX_LON 180.0f
00056
00057 static const int8_t declination_table[13][37] = \
00058 {
00059 { 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 },
00060 { 30, 30, 30, 30, 29, 29, 29, 29, 27, 24, 18, 11, 3, -3, -9, -12, -15, -17, -21, -26, -32, -39, -45, -51, -55, -57, -56, -53, -44, -31, -14, 0, 13, 21, 26, 29, 30 },
00061 { 21, 22, 22, 22, 22, 22, 22, 22, 21, 18, 13, 5, -3, -11, -17, -20, -21, -22, -23, -25, -29, -35, -40, -44, -45, -44, -40, -32, -22, -12, -3, 3, 9, 14, 18, 20, 21 },
00062 { 16, 17, 17, 17, 17, 17, 16, 16, 16, 13, 8, 0, -9, -16, -21, -24, -25, -25, -23, -20, -21, -24, -28, -31, -31, -29, -24, -17, -9, -3, 0, 4, 7, 10, 13, 15, 16 },
00063 { 12, 13, 13, 13, 13, 13, 12, 12, 11, 9, 3, -4, -12, -19, -23, -24, -24, -22, -17, -12, -9, -10, -13, -17, -18, -16, -13, -8, -3, 0, 1, 3, 6, 8, 10, 12, 12 },
00064 { 10, 10, 10, 10, 10, 10, 10, 9, 9, 6, 0, -6, -14, -20, -22, -22, -19, -15, -10, -6, -2, -2, -4, -7, -8, -8, -7, -4, 0, 1, 1, 2, 4, 6, 8, 10, 10 },
00065 { 9, 9, 9, 9, 9, 9, 8, 8, 7, 4, -1, -8, -15, -19, -20, -18, -14, -9, -5, -2, 0, 1, 0, -2, -3, -4, -3, -2, 0, 0, 0, 1, 3, 5, 7, 8, 9 },
00066 { 8, 8, 8, 9, 9, 9, 8, 8, 6, 2, -3, -9, -15, -18, -17, -14, -10, -6, -2, 0, 1, 2, 2, 0, -1, -1, -2, -1, 0, 0, 0, 0, 1, 3, 5, 7, 8 },
00067 { 8, 9, 9, 10, 10, 10, 10, 8, 5, 0, -5, -11, -15, -16, -15, -12, -8, -4, -1, 0, 2, 3, 2, 1, 0, 0, 0, 0, 0, -1, -2, -2, -1, 0, 3, 6, 8 },
00068 { 6, 9, 10, 11, 12, 12, 11, 9, 5, 0, -7, -12, -15, -15, -13, -10, -7, -3, 0, 1, 2, 3, 3, 3, 2, 1, 0, 0, -1, -3, -4, -5, -5, -2, 0, 3, 6 },
00069 { 5, 8, 11, 13, 15, 15, 14, 11, 5, -1, -9, -14, -17, -16, -14, -11, -7, -3, 0, 1, 3, 4, 5, 5, 5, 4, 3, 1, -1, -4, -7, -8, -8, -6, -2, 1, 5 },
00070 { 4, 8, 12, 15, 17, 18, 16, 12, 5, -3, -12, -18, -20, -19, -16, -13, -8, -4, -1, 1, 4, 6, 8, 9, 9, 9, 7, 3, -1, -6, -10, -12, -11, -9, -5, 0, 4 },
00071 { 3, 9, 14, 17, 20, 21, 19, 14, 4, -8, -19, -25, -26, -25, -21, -17, -12, -7, -2, 1, 5, 9, 13, 15, 16, 16, 13, 7, 0, -7, -12, -15, -14, -11, -6, -1, 3 },
00072 };
00073
00074 static float get_lookup_table_val(unsigned lat, unsigned lon);
00075
00076 float get_mag_declination(float lat_rad, float lon_rad)
00077 {
00078 float lat = lat_rad / M_PI * 180.0f;
00079 float lon = lon_rad / M_PI * 180.0f;
00080
00081
00082
00083
00084
00085
00086 if (lat < -90.0f || lat > 90.0f ||
00087 lon < -180.0f || lon > 180.0f) {
00088 return 0.0f;
00089 }
00090
00091
00092 int min_lat = (int)(lat / SAMPLING_RES) * SAMPLING_RES;
00093 int min_lon = (int)(lon / SAMPLING_RES) * SAMPLING_RES;
00094
00095
00096
00097
00098
00099
00100 if (lat <= SAMPLING_MIN_LAT) {
00101 min_lat = SAMPLING_MIN_LAT;
00102 }
00103
00104 if (lat >= SAMPLING_MAX_LAT) {
00105 min_lat = (int)(lat / SAMPLING_RES) * SAMPLING_RES - SAMPLING_RES;
00106 }
00107
00108 if (lon <= SAMPLING_MIN_LON) {
00109 min_lon = SAMPLING_MIN_LON;
00110 }
00111
00112 if (lon >= SAMPLING_MAX_LON) {
00113 min_lon = (int)(lon / SAMPLING_RES) * SAMPLING_RES - SAMPLING_RES;
00114 }
00115
00116
00117 unsigned min_lat_index = (-(SAMPLING_MIN_LAT) + min_lat) / SAMPLING_RES;
00118 unsigned min_lon_index = (-(SAMPLING_MIN_LON) + min_lon) / SAMPLING_RES;
00119
00120 float declination_sw = get_lookup_table_val(min_lat_index, min_lon_index);
00121 float declination_se = get_lookup_table_val(min_lat_index, min_lon_index + 1);
00122 float declination_ne = get_lookup_table_val(min_lat_index + 1, min_lon_index + 1);
00123 float declination_nw = get_lookup_table_val(min_lat_index + 1, min_lon_index);
00124
00125
00126
00127 float declination_min = ((lon - min_lon) / SAMPLING_RES) * (declination_se - declination_sw) + declination_sw;
00128 float declination_max = ((lon - min_lon) / SAMPLING_RES) * (declination_ne - declination_nw) + declination_nw;
00129
00130 float declination_ret = ((lat - min_lat) / SAMPLING_RES) * (declination_max - declination_min) + declination_min;
00131
00132 return declination_ret / 180.0f * M_PI;
00133 }
00134
00135 float get_lookup_table_val(unsigned lat_index, unsigned lon_index)
00136 {
00137 return declination_table[lat_index][lon_index];
00138 }