src/main/navigation/navigation_geo.c

   1 /*
   2  * This file is part of Cleanflight.
   3  *
   4  * Cleanflight is free software: you can redistribute it and/or modify
   5  * it under the terms of the GNU General Public License as published by
   6  * the Free Software Foundation, either version 3 of the License, or
   7  * (at your option) any later version.
   8  *
   9  * Cleanflight is distributed in the hope that it will be useful,
  10  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  12  * GNU General Public License for more details.
  13  *
  14  * You should have received a copy of the GNU General Public License
  15  * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
  16  */
  17
  18 #include <stdbool.h>
  19 #include <stdint.h>
  20 #include <math.h>
  21
  22 #include "platform.h"
  23
  24 #include "build/build_config.h"
  25 #include "build/debug.h"
  26
  27 #include "common/axis.h"
  28 #include "common/filter.h"
  29 #include "common/maths.h"
  30
  31 #include "sensors/sensors.h"
  32 #include "sensors/acceleration.h"
  33 #include "sensors/boardalignment.h"
  34
  35 #include "flight/pid.h"
  36 #include "flight/imu.h"
  37
  38 #include "fc/config.h"
  39 #include "fc/runtime_config.h"
  40
  41 #include "navigation/navigation.h"
  42 #include "navigation/navigation_private.h"
  43
  44 #include "navigation/navigation_declination_gen.c"
  45
  46 static float get_lookup_table_val(unsigned lat_index, unsigned lon_index)
  47 {
  48     return declination_table[lat_index][lon_index];
  49 }
  50
  51 float geoCalculateMagDeclination(const gpsLocation_t * llh) // degrees units
  52 {
  53     /*
  54      * If the values exceed valid ranges, return zero as default
  55      * as we have no way of knowing what the closest real value
  56      * would be.
  57      */
  58     const float lat = llh->lat / 10000000.0f;
  59     const float lon = llh->lon / 10000000.0f;
  60
  61     if (lat < -90.0f || lat > 90.0f ||
  62         lon < -180.0f || lon > 180.0f) {
  63         return 0.0f;
  64     }
  65
  66     /* round down to nearest sampling resolution */
  67     int min_lat = (int)(lat / SAMPLING_RES) * SAMPLING_RES;
  68     int min_lon = (int)(lon / SAMPLING_RES) * SAMPLING_RES;
  69
  70     /* for the rare case of hitting the bounds exactly
  71      * the rounding logic wouldn't fit, so enforce it.
  72      */
  73
  74     /* limit to table bounds - required for maxima even when table spans full globe range */
  75     if (lat <= SAMPLING_MIN_LAT) {
  76         min_lat = SAMPLING_MIN_LAT;
  77     }
  78
  79     if (lat >= SAMPLING_MAX_LAT) {
  80         min_lat = (int)(lat / SAMPLING_RES) * SAMPLING_RES - SAMPLING_RES;
  81     }
  82
  83     if (lon <= SAMPLING_MIN_LON) {
  84         min_lon = SAMPLING_MIN_LON;
  85     }
  86
  87     if (lon >= SAMPLING_MAX_LON) {
  88         min_lon = (int)(lon / SAMPLING_RES) * SAMPLING_RES - SAMPLING_RES;
  89     }
  90
  91     /* find index of nearest low sampling point */
  92     const unsigned min_lat_index = (-(SAMPLING_MIN_LAT) + min_lat)  / SAMPLING_RES;
  93     const unsigned min_lon_index = (-(SAMPLING_MIN_LON) + min_lon) / SAMPLING_RES;
  94
  95     const float declination_sw = get_lookup_table_val(min_lat_index, min_lon_index);
  96     const float declination_se = get_lookup_table_val(min_lat_index, min_lon_index + 1);
  97     const float declination_ne = get_lookup_table_val(min_lat_index + 1, min_lon_index + 1);
  98     const float declination_nw = get_lookup_table_val(min_lat_index + 1, min_lon_index);
  99
 100     /* perform bilinear interpolation on the four grid corners */
 101
 102     const float declination_min = ((lon - min_lon) / SAMPLING_RES) * (declination_se - declination_sw) + declination_sw;
 103     const float declination_max = ((lon - min_lon) / SAMPLING_RES) * (declination_ne - declination_nw) + declination_nw;
 104
 105     return ((lat - min_lat) / SAMPLING_RES) * (declination_max - declination_min) + declination_min;
 106 }
 107
 108 void geoSetOrigin(gpsOrigin_t *origin, const gpsLocation_t *llh, geoOriginResetMode_e resetMode)
 109 {
 110     if (resetMode == GEO_ORIGIN_SET) {
 111         origin->valid = true;
 112         origin->lat = llh->lat;
 113         origin->lon = llh->lon;
 114         origin->alt = llh->alt;
 115         origin->scale = constrainf(cos_approx((ABS(origin->lat) / 10000000.0f) * 0.0174532925f), 0.01f, 1.0f);
 116     }
 117     else if (origin->valid && (resetMode == GEO_ORIGIN_RESET_ALTITUDE)) {
 118         origin->alt = llh->alt;
 119     }
 120 }
 121
 122 bool geoConvertGeodeticToLocal(fpVector3_t *pos, const gpsOrigin_t *origin, const gpsLocation_t *llh, geoAltitudeConversionMode_e altConv)
 123 {
 124     if (origin->valid) {
 125         pos->x = (llh->lat - origin->lat) * DISTANCE_BETWEEN_TWO_LONGITUDE_POINTS_AT_EQUATOR;
 126         pos->y = (llh->lon - origin->lon) * (DISTANCE_BETWEEN_TWO_LONGITUDE_POINTS_AT_EQUATOR * origin->scale);
 127
 128         // If flag GEO_ALT_RELATIVE, than llh altitude is already relative to origin
 129         if (altConv == GEO_ALT_RELATIVE) {
 130             pos->z = llh->alt;
 131         } else {
 132             pos->z = llh->alt - origin->alt;
 133         }
 134         return true;
 135     }
 136
 137     pos->x = 0.0f;
 138     pos->y = 0.0f;
 139     pos->z = 0.0f;
 140     return false;
 141 }
 142
 143 bool geoConvertGeodeticToLocalOrigin(fpVector3_t * pos, const gpsLocation_t *llh, geoAltitudeConversionMode_e altConv)
 144 {
 145     return geoConvertGeodeticToLocal(pos, &posControl.gpsOrigin, llh, altConv);
 146 }
 147
 148 bool geoConvertLocalToGeodetic(gpsLocation_t *llh, const gpsOrigin_t * origin, const fpVector3_t *pos)
 149 {
 150     float scaleLonDown;
 151
 152     if (origin->valid) {
 153         llh->lat = origin->lat;
 154         llh->lon = origin->lon;
 155         llh->alt = origin->alt;
 156         scaleLonDown = origin->scale;
 157     }
 158     else {
 159         llh->lat = 0;
 160         llh->lon = 0;
 161         llh->alt = 0;
 162         scaleLonDown = 1.0f;
 163     }
 164
 165     llh->lat += lrintf(pos->x / DISTANCE_BETWEEN_TWO_LONGITUDE_POINTS_AT_EQUATOR);
 166     llh->lon += lrintf(pos->y / (DISTANCE_BETWEEN_TWO_LONGITUDE_POINTS_AT_EQUATOR * scaleLonDown));
 167     llh->alt += lrintf(pos->z);
 168     return origin->valid;
 169 }