Line data    Source code

       1             : /*
       2             :    This program is free software: you can redistribute it and/or modify
       3             :    it under the terms of the GNU General Public License as published by
       4             :    the Free Software Foundation, either version 3 of the License, or
       5             :    (at your option) any later version.
       6             : 
       7             :    This program is distributed in the hope that it will be useful,
       8             :    but WITHOUT ANY WARRANTY; without even the implied warranty of
       9             :    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
      10             :    GNU General Public License for more details.
      11             : 
      12             :    You should have received a copy of the GNU General Public License
      13             :    along with this program.  If not, see <http://www.gnu.org/licenses/>.
      14             :  */
      15             : 
      16             : /*
      17             :  *      Adam M Rivera
      18             :  *      With direction from: Andrew Tridgell, Jason Short, Justin Beech
      19             :  *
      20             :  *      Adapted from: http://www.societyofrobots.com/robotforum/index.php?topic=11855.0
      21             :  *      Scott Ferguson
      22             :  *      [email protected]
      23             :  *
      24             :  */
      25             : #include "AP_Declination.h"
      26             : 
      27             : #include <cmath>
      28             : 
      29             : #include <AP_Common/AP_Common.h>
      30             : #include <AP_Math/AP_Math.h>
      31             : 
      32             : /*
      33             :   calculate magnetic field intensity and orientation
      34             : */
      35   187779012 : bool AP_Declination::get_mag_field_ef(float latitude_deg, float longitude_deg, float &intensity_gauss, float &declination_deg, float &inclination_deg)
      36             : {
      37   187779012 :     bool valid_input_data = true;
      38             : 
      39             :     /* round down to nearest sampling resolution. On some platforms (e.g. clang on macOS),
      40             :         the behaviour of implicit casts from int32 to float can be undefined thus making it explicit here. */
      41   187779012 :     float min_lat = float(static_cast<int32_t>(static_cast<int32_t>(floorf(latitude_deg / SAMPLING_RES)) * SAMPLING_RES));
      42   187779012 :     float min_lon = float(static_cast<int32_t>(static_cast<int32_t>(floorf(longitude_deg / SAMPLING_RES)) * SAMPLING_RES));
      43             : 
      44             :     /* for the rare case of hitting the bounds exactly
      45             :      * the rounding logic wouldn't fit, so enforce it.
      46             :      */
      47             : 
      48             :     /* limit to table bounds - required for maxima even when table spans full globe range */
      49   187779012 :     if (latitude_deg <= SAMPLING_MIN_LAT) {
      50           0 :         min_lat = float(static_cast<int32_t>(SAMPLING_MIN_LAT));
      51           0 :         valid_input_data = false;
      52             :     }
      53             : 
      54   187779012 :     if (latitude_deg >= SAMPLING_MAX_LAT) {
      55           0 :         min_lat = float(static_cast<int32_t>(static_cast<int32_t>(latitude_deg / SAMPLING_RES) * SAMPLING_RES - SAMPLING_RES));
      56           0 :         valid_input_data = false;
      57             :     }
      58             : 
      59   187779012 :     if (longitude_deg <= SAMPLING_MIN_LON) {
      60           0 :         min_lon = float(static_cast<int32_t>(SAMPLING_MIN_LON));
      61           0 :         valid_input_data = false;
      62             :     }
      63             : 
      64   187779012 :     if (longitude_deg >= SAMPLING_MAX_LON) {
      65           0 :         min_lon = float(static_cast<int32_t>(static_cast<int32_t>(longitude_deg / SAMPLING_RES) * SAMPLING_RES - SAMPLING_RES));
      66           0 :         valid_input_data = false;
      67             :     }
      68             : 
      69             :     /* find index of nearest low sampling point */
      70   187779012 :     uint32_t min_lat_index = constrain_int32(static_cast<uint32_t>((-(SAMPLING_MIN_LAT) + min_lat)  / SAMPLING_RES), 0, LAT_TABLE_SIZE - 2);
      71   187779012 :     uint32_t min_lon_index = constrain_int32(static_cast<uint32_t>((-(SAMPLING_MIN_LON) + min_lon) / SAMPLING_RES), 0, LON_TABLE_SIZE -2);
      72             : 
      73             :     /* calculate intensity */
      74             : 
      75   187779012 :     float data_sw = intensity_table[min_lat_index][min_lon_index];
      76   187779012 :     float data_se = intensity_table[min_lat_index][min_lon_index + 1];
      77   187779012 :     float data_ne = intensity_table[min_lat_index + 1][min_lon_index + 1];
      78   187779012 :     float data_nw = intensity_table[min_lat_index + 1][min_lon_index];
      79             : 
      80             :     /* perform bilinear interpolation on the four grid corners */
      81             : 
      82   187779012 :     float data_min = ((longitude_deg - min_lon) / SAMPLING_RES) * (data_se - data_sw) + data_sw;
      83   187779012 :     float data_max = ((longitude_deg - min_lon) / SAMPLING_RES) * (data_ne - data_nw) + data_nw;
      84             : 
      85   187779012 :     intensity_gauss = ((latitude_deg - min_lat) / SAMPLING_RES) * (data_max - data_min) + data_min;
      86             : 
      87             :     /* calculate declination */
      88             : 
      89   187779012 :     data_sw = declination_table[min_lat_index][min_lon_index];
      90   187779012 :     data_se = declination_table[min_lat_index][min_lon_index + 1];
      91   187779012 :     data_ne = declination_table[min_lat_index + 1][min_lon_index + 1];
      92   187779012 :     data_nw = declination_table[min_lat_index + 1][min_lon_index];
      93             : 
      94             :     /* perform bilinear interpolation on the four grid corners */
      95             : 
      96   187779012 :     data_min = ((longitude_deg - min_lon) / SAMPLING_RES) * (data_se - data_sw) + data_sw;
      97   187779012 :     data_max = ((longitude_deg - min_lon) / SAMPLING_RES) * (data_ne - data_nw) + data_nw;
      98             : 
      99   187779012 :     declination_deg = ((latitude_deg - min_lat) / SAMPLING_RES) * (data_max - data_min) + data_min;
     100             : 
     101             :     /* calculate inclination */
     102             : 
     103   187779012 :     data_sw = inclination_table[min_lat_index][min_lon_index];
     104   187779012 :     data_se = inclination_table[min_lat_index][min_lon_index + 1];
     105   187779012 :     data_ne = inclination_table[min_lat_index + 1][min_lon_index + 1];
     106   187779012 :     data_nw = inclination_table[min_lat_index + 1][min_lon_index];
     107             : 
     108             :     /* perform bilinear interpolation on the four grid corners */
     109             : 
     110   187779012 :     data_min = ((longitude_deg - min_lon) / SAMPLING_RES) * (data_se - data_sw) + data_sw;
     111   187779012 :     data_max = ((longitude_deg - min_lon) / SAMPLING_RES) * (data_ne - data_nw) + data_nw;
     112             : 
     113   187779012 :     inclination_deg = ((latitude_deg - min_lat) / SAMPLING_RES) * (data_max - data_min) + data_min;
     114             : 
     115   187779012 :     return valid_input_data;
     116             : }
     117             : 
     118             : 
     119             : /*
     120             :  calculate magnetic field intensity and orientation
     121             : */
     122      225679 : float AP_Declination::get_declination(float latitude_deg, float longitude_deg)
     123             : {
     124      225679 :     float declination_deg=0, inclination_deg=0, intensity_gauss=0;
     125             : 
     126      225679 :     get_mag_field_ef(latitude_deg, longitude_deg, intensity_gauss, declination_deg, inclination_deg);
     127             : 
     128      225679 :     return declination_deg;
     129             : }
     130             : 
     131             : /*
     132             :   get earth field as a Vector3f in Gauss given a Location
     133             : */
     134      277837 : Vector3f AP_Declination::get_earth_field_ga(const Location &loc)
     135             : {
     136      277837 :     float declination_deg=0, inclination_deg=0, intensity_gauss=0;
     137      277837 :     get_mag_field_ef(loc.lat*1.0e-7f, loc.lng*1.0e-7f, intensity_gauss, declination_deg, inclination_deg);
     138             : 
     139             :     // create earth field
     140      277837 :     Vector3f mag_ef = Vector3f(intensity_gauss, 0.0, 0.0);
     141      277837 :     Matrix3f R;
     142             : 
     143      277837 :     R.from_euler(0.0f, -radians(inclination_deg), radians(declination_deg));
     144      277837 :     mag_ef = R * mag_ef;
     145      277837 :     return mag_ef;
     146             : }