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 #if defined(USE_NAV)
  25
  26 #include "build/build_config.h"
  27 #include "build/debug.h"
  28
  29 #include "common/axis.h"
  30 #include "common/filter.h"
  31 #include "common/maths.h"
  32
  33 #include "sensors/sensors.h"
  34 #include "sensors/acceleration.h"
  35 #include "sensors/boardalignment.h"
  36
  37 #include "flight/pid.h"
  38 #include "flight/imu.h"
  39
  40 #include "fc/config.h"
  41 #include "fc/runtime_config.h"
  42
  43 #include "navigation/navigation.h"
  44 #include "navigation/navigation_private.h"
  45
  46 #include "navigation/navigation_declination_gen.c"
  47
  48 static float get_lookup_table_val(unsigned lat_index, unsigned lon_index)
  49 {
  50     return declination_table[lat_index][lon_index];
  51 }
  52
  53 float geoCalculateMagDeclination(const gpsLocation_t * llh) // degrees units
  54 {
  55     /*
  56      * If the values exceed valid ranges, return zero as default
  57      * as we have no way of knowing what the closest real value
  58      * would be.
  59      */
  60     const float lat = llh->lat / 10000000.0f;
  61     const float lon = llh->lon / 10000000.0f;
  62
  63     if (lat < -90.0f || lat > 90.0f ||
  64         lon < -180.0f || lon > 180.0f) {
  65         return 0.0f;
  66     }
  67
  68     /* round down to nearest sampling resolution */
  69     int min_lat = (int)(lat / SAMPLING_RES) * SAMPLING_RES;
  70     int min_lon = (int)(lon / SAMPLING_RES) * SAMPLING_RES;
  71
  72     /* for the rare case of hitting the bounds exactly
  73      * the rounding logic wouldn't fit, so enforce it.
  74      */
  75
  76     /* limit to table bounds - required for maxima even when table spans full globe range */
  77     if (lat <= SAMPLING_MIN_LAT) {
  78         min_lat = SAMPLING_MIN_LAT;
  79     }
  80
  81     if (lat >= SAMPLING_MAX_LAT) {
  82         min_lat = (int)(lat / SAMPLING_RES) * SAMPLING_RES - SAMPLING_RES;
  83     }
  84
  85     if (lon <= SAMPLING_MIN_LON) {
  86         min_lon = SAMPLING_MIN_LON;
  87     }
  88
  89     if (lon >= SAMPLING_MAX_LON) {
  90         min_lon = (int)(lon / SAMPLING_RES) * SAMPLING_RES - SAMPLING_RES;
  91     }
  92
  93     /* find index of nearest low sampling point */
  94     const unsigned min_lat_index = (-(SAMPLING_MIN_LAT) + min_lat)  / SAMPLING_RES;
  95     const unsigned min_lon_index = (-(SAMPLING_MIN_LON) + min_lon) / SAMPLING_RES;
  96
  97     const float declination_sw = get_lookup_table_val(min_lat_index, min_lon_index);
  98     const float declination_se = get_lookup_table_val(min_lat_index, min_lon_index + 1);
  99     const float declination_ne = get_lookup_table_val(min_lat_index + 1, min_lon_index + 1);
 100     const float declination_nw = get_lookup_table_val(min_lat_index + 1, min_lon_index);
 101
 102     /* perform bilinear interpolation on the four grid corners */
 103
 104     const float declination_min = ((lon - min_lon) / SAMPLING_RES) * (declination_se - declination_sw) + declination_sw;
 105     const float declination_max = ((lon - min_lon) / SAMPLING_RES) * (declination_ne - declination_nw) + declination_nw;
 106
 107     return ((lat - min_lat) / SAMPLING_RES) * (declination_max - declination_min) + declination_min;
 108 }
 109
 110 void geoSetOrigin(gpsOrigin_t *origin, const gpsLocation_t *llh, geoOriginResetMode_e resetMode)
 111 {
 112     if (resetMode == GEO_ORIGIN_SET) {
 113         origin->valid = true;
 114         origin->lat = llh->lat;
 115         origin->lon = llh->lon;
 116         origin->alt = llh->alt;
 117         origin->scale = constrainf(cos_approx((ABS(origin->lat) / 10000000.0f) * 0.0174532925f), 0.01f, 1.0f);
 118     }
 119     else if (origin->valid && (resetMode == GEO_ORIGIN_RESET_ALTITUDE)) {
 120         origin->alt = llh->alt;
 121     }
 122 }
 123
 124 bool geoConvertGeodeticToLocal(fpVector3_t *pos, const gpsOrigin_t *origin, const gpsLocation_t *llh, geoAltitudeConversionMode_e altConv)
 125 {
 126     if (origin->valid) {
 127         pos->x = (llh->lat - origin->lat) * DISTANCE_BETWEEN_TWO_LONGITUDE_POINTS_AT_EQUATOR;
 128         pos->y = (llh->lon - origin->lon) * (DISTANCE_BETWEEN_TWO_LONGITUDE_POINTS_AT_EQUATOR * origin->scale);
 129
 130         // If flag GEO_ALT_RELATIVE, than llh altitude is already relative to origin
 131         if (altConv == GEO_ALT_RELATIVE) {
 132             pos->z = llh->alt;
 133         } else {
 134             pos->z = llh->alt - origin->alt;
 135         }
 136         return true;
 137     }
 138
 139     pos->x = 0.0f;
 140     pos->y = 0.0f;
 141     pos->z = 0.0f;
 142     return false;
 143 }
 144
 145 bool geoConvertGeodeticToLocalOrigin(fpVector3_t * pos, const gpsLocation_t *llh, geoAltitudeConversionMode_e altConv)
 146 {
 147     return geoConvertGeodeticToLocal(pos, &posControl.gpsOrigin, llh, altConv);
 148 }
 149
 150 bool geoConvertLocalToGeodetic(gpsLocation_t *llh, const gpsOrigin_t * origin, const fpVector3_t *pos)
 151 {
 152     float scaleLonDown;
 153
 154     if (origin->valid) {
 155         llh->lat = origin->lat;
 156         llh->lon = origin->lon;
 157         llh->alt = origin->alt;
 158         scaleLonDown = origin->scale;
 159     }
 160     else {
 161         llh->lat = 0;
 162         llh->lon = 0;
 163         llh->alt = 0;
 164         scaleLonDown = 1.0f;
 165     }
 166
 167     llh->lat += lrintf(pos->x / DISTANCE_BETWEEN_TWO_LONGITUDE_POINTS_AT_EQUATOR);
 168     llh->lon += lrintf(pos->y / (DISTANCE_BETWEEN_TWO_LONGITUDE_POINTS_AT_EQUATOR * scaleLonDown));
 169     llh->alt += lrintf(pos->z);
 170     return origin->valid;
 171 }
 172
 173
 174 #endif  // NAV