src/main/flight/position.c

   1 /*
   2  * This file is part of Cleanflight and Betaflight.
   3  *
   4  * Cleanflight and Betaflight are free software. You can redistribute
   5  * this software and/or modify this software under the terms of the
   6  * GNU General Public License as published by the Free Software
   7  * Foundation, either version 3 of the License, or (at your option)
   8  * any later version.
   9  *
  10  * Cleanflight and Betaflight are distributed in the hope that they
  11  * will be useful, but WITHOUT ANY WARRANTY; without even the implied
  12  * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
  13  * See the GNU General Public License for more details.
  14  *
  15  * You should have received a copy of the GNU General Public License
  16  * along with this software.
  17  *
  18  * If not, see <http://www.gnu.org/licenses/>.
  19  */
  20
  21 #include <stdbool.h>
  22 #include <stdint.h>
  23 #include <stdlib.h>
  24 #include <math.h>
  25 #include <limits.h>
  26
  27 #include "platform.h"
  28
  29 #include "build/debug.h"
  30
  31 #include "common/maths.h"
  32 #include "common/filter.h"
  33
  34 #include "fc/runtime_config.h"
  35
  36 #include "flight/position.h"
  37 #include "flight/imu.h"
  38 #include "flight/pid.h"
  39
  40 #include "io/gps.h"
  41
  42 #include "scheduler/scheduler.h"
  43
  44 #include "sensors/sensors.h"
  45 #include "sensors/barometer.h"
  46
  47 #include "pg/pg.h"
  48 #include "pg/pg_ids.h"
  49
  50 static float displayAltitudeCm = 0.0f;
  51 static float zeroedAltitudeCm = 0.0f;
  52
  53 #if defined(USE_BARO) || defined(USE_GPS)
  54 static float zeroedAltitudeDerivative = 0.0f;
  55 #endif
  56
  57 static pt2Filter_t altitudeLpf;
  58 static pt2Filter_t altitudeDerivativeLpf;
  59 #ifdef USE_VARIO
  60 static int16_t estimatedVario = 0; // in cm/s
  61 #endif
  62
  63 void positionInit(void)
  64 {
  65     const float sampleTimeS = HZ_TO_INTERVAL(TASK_ALTITUDE_RATE_HZ);
  66
  67     const float altitudeCutoffHz = positionConfig()->altitude_lpf / 100.0f;
  68     const float altitudeGain = pt2FilterGain(altitudeCutoffHz, sampleTimeS);
  69     pt2FilterInit(&altitudeLpf, altitudeGain);
  70
  71     const float altitudeDerivativeCutoffHz = positionConfig()->altitude_d_lpf / 100.0f;
  72     const float altitudeDerivativeGain = pt2FilterGain(altitudeDerivativeCutoffHz, sampleTimeS);
  73     pt2FilterInit(&altitudeDerivativeLpf, altitudeDerivativeGain);
  74 }
  75
  76 typedef enum {
  77     DEFAULT = 0,
  78     BARO_ONLY,
  79     GPS_ONLY
  80 } altitudeSource_e;
  81
  82 PG_REGISTER_WITH_RESET_TEMPLATE(positionConfig_t, positionConfig, PG_POSITION, 4);
  83
  84 PG_RESET_TEMPLATE(positionConfig_t, positionConfig,
  85     .altitude_source = DEFAULT,
  86     .altitude_prefer_baro = 100, // percentage 'trust' of baro data
  87     .altitude_lpf = 300,
  88     .altitude_d_lpf = 100,
  89 );
  90
  91 #if defined(USE_BARO) || defined(USE_GPS)
  92 void calculateEstimatedAltitude(void)
  93 {
  94     static bool wasArmed = false;
  95     static bool useZeroedGpsAltitude = false; // whether a zero for the GPS altitude value exists
  96     static float gpsAltCm = 0.0f; // will hold last value on transient loss of 3D fix
  97     static float gpsAltOffsetCm = 0.0f;
  98     static float baroAltOffsetCm = 0.0f;
  99     static float newBaroAltOffsetCm = 0.0f;
 100
 101     float baroAltCm = 0.0f;
 102     float gpsTrust = 0.3f; // if no pDOP value, use 0.3, intended range 0-1;
 103     bool haveBaroAlt = false; // true if baro exists and has been calibrated on power up
 104     bool haveGpsAlt = false; // true if GPS is connected and while it has a 3D fix, set each run to false
 105
 106     // *** Get sensor data
 107 #ifdef USE_BARO
 108     if (sensors(SENSOR_BARO)) {
 109         baroAltCm = getBaroAltitude();
 110         haveBaroAlt = true; // false only if there is no sensor on the board, or it has failed
 111     }
 112 #endif
 113 #ifdef USE_GPS
 114     if (sensors(SENSOR_GPS) && STATE(GPS_FIX)) {
 115         // GPS_FIX means a 3D fix, which requires min 4 sats.
 116         // On loss of 3D fix, gpsAltCm remains at the last value, haveGpsAlt becomes false, and gpsTrust goes to zero.
 117         gpsAltCm = gpsSol.llh.altCm; // static, so hold last altitude value if 3D fix is lost to prevent fly to moon
 118         haveGpsAlt = true; // stays false if no 3D fix
 119         if (gpsSol.dop.pdop != 0) {
 120             // pDOP of 1.0 is good.  100 is very bad.  Our gpsSol.dop.pdop values are *100
 121             // When pDOP is a value less than 3.3, GPS trust will be stronger than default.
 122             gpsTrust = 100.0f / gpsSol.dop.pdop;
 123             // *** TO DO - investigate if we should use vDOP or vACC with UBlox units;
 124         }
 125         // always use at least 10% of other sources besides gps if available
 126         gpsTrust = MIN(gpsTrust, 0.9f);
 127     }
 128 #endif
 129
 130     //  ***  DISARMED  ***
 131     if (!ARMING_FLAG(ARMED)) {
 132         if (wasArmed) { // things to run once, on disarming, after being armed
 133             useZeroedGpsAltitude = false; // reset, and wait for valid GPS data to zero the GPS signal
 134             wasArmed = false;
 135         }
 136
 137         newBaroAltOffsetCm = 0.2f * baroAltCm + 0.8f * newBaroAltOffsetCm; // smooth some recent baro samples
 138         displayAltitudeCm = baroAltCm - baroAltOffsetCm; // if no GPS altitude, show un-smoothed Baro altitude in OSD and sensors tab, using most recent offset.
 139
 140         if (haveGpsAlt) { // watch for valid GPS altitude data to get a zero value from
 141             gpsAltOffsetCm = gpsAltCm; // update the zero offset value with the most recent valid gps altitude reading
 142             useZeroedGpsAltitude = true; // we can use this offset to zero the GPS altitude on arming
 143             if (!(positionConfig()->altitude_source == BARO_ONLY)) {
 144                 displayAltitudeCm = gpsAltCm; // estimatedAltitude shows most recent ASL GPS altitude in OSD and sensors, while disarmed
 145             }
 146         }
 147         zeroedAltitudeCm = 0.0f; // always hold relativeAltitude at zero while disarmed
 148         DEBUG_SET(DEBUG_ALTITUDE, 2, gpsAltCm / 100.0f); // Absolute altitude ASL in metres, max 32,767m
 149     //  ***  ARMED  ***
 150     } else {
 151         if (!wasArmed) { // things to run once, on arming, after being disarmed
 152             baroAltOffsetCm = newBaroAltOffsetCm;
 153             wasArmed = true;
 154         }
 155
 156         baroAltCm -= baroAltOffsetCm; // use smoothed baro with most recent zero from disarm period
 157
 158         if (haveGpsAlt) { // update relativeAltitude with every new gpsAlt value, or hold the previous value until 3D lock recovers
 159             if (!useZeroedGpsAltitude && haveBaroAlt) { // armed without zero offset, can use baro values to zero later
 160                 gpsAltOffsetCm = gpsAltCm - baroAltCm; // not very accurate
 161                 useZeroedGpsAltitude = true;
 162             }
 163             if (useZeroedGpsAltitude) { // normal situation
 164                 zeroedAltitudeCm = gpsAltCm - gpsAltOffsetCm; // now that we have a GPS offset value, we can use it to zero relativeAltitude
 165             }
 166         } else {
 167             gpsTrust = 0.0f;
 168             // TO DO - smoothly reduce GPS trust, rather than immediately dropping to zero for what could be only a very brief loss of 3D fix
 169         }
 170         DEBUG_SET(DEBUG_ALTITUDE, 2, lrintf(zeroedAltitudeCm / 10.0f)); // Relative altitude above takeoff, to 0.1m, rolls over at 3,276.7m
 171
 172         // Empirical mixing of GPS and Baro altitudes
 173         if (useZeroedGpsAltitude && (positionConfig()->altitude_source == DEFAULT || positionConfig()->altitude_source == GPS_ONLY)) {
 174             if (haveBaroAlt && positionConfig()->altitude_source == DEFAULT) {
 175                 // mix zeroed GPS with Baro altitude data, if Baro data exists if are in default altitude control mode
 176                 const float absDifferenceM = fabsf(zeroedAltitudeCm - baroAltCm) / 100.0f * positionConfig()->altitude_prefer_baro / 100.0f;
 177                 if (absDifferenceM > 1.0f) { // when there is a large difference, favour Baro
 178                     gpsTrust /=  absDifferenceM;
 179                 }
 180                 zeroedAltitudeCm = zeroedAltitudeCm * gpsTrust + baroAltCm * (1.0f - gpsTrust);
 181             }
 182         } else if (haveBaroAlt && (positionConfig()->altitude_source == DEFAULT || positionConfig()->altitude_source == BARO_ONLY)) {
 183             zeroedAltitudeCm = baroAltCm; // use Baro if no GPS data, or we want Baro only
 184         }
 185     }
 186
 187     zeroedAltitudeCm = pt2FilterApply(&altitudeLpf, zeroedAltitudeCm);
 188     // NOTE: this filter must receive 0 as its input, for the whole disarmed time, to ensure correct zeroed values on arming
 189
 190     if (wasArmed) {
 191         displayAltitudeCm = zeroedAltitudeCm; // while armed, show filtered relative altitude in OSD / sensors tab
 192     }
 193
 194     // *** calculate Vario signal
 195     static float previousZeroedAltitudeCm = 0.0f;
 196     zeroedAltitudeDerivative = (zeroedAltitudeCm - previousZeroedAltitudeCm) * TASK_ALTITUDE_RATE_HZ; // cm/s
 197     previousZeroedAltitudeCm = zeroedAltitudeCm;
 198
 199     zeroedAltitudeDerivative = pt2FilterApply(&altitudeDerivativeLpf, zeroedAltitudeDerivative);
 200
 201 #ifdef USE_VARIO
 202     estimatedVario = lrintf(zeroedAltitudeDerivative);
 203     estimatedVario = applyDeadband(estimatedVario, 10); // ignore climb rates less than 0.1 m/s
 204 #endif
 205
 206     // *** set debugs
 207     DEBUG_SET(DEBUG_ALTITUDE, 0, (int32_t)(100 * gpsTrust));
 208     DEBUG_SET(DEBUG_ALTITUDE, 1, lrintf(baroAltCm / 10.0f)); // Relative altitude above takeoff, to 0.1m, rolls over at 3,276.7m
 209 #ifdef USE_VARIO
 210     DEBUG_SET(DEBUG_ALTITUDE, 3, estimatedVario);
 211 #endif
 212     DEBUG_SET(DEBUG_RTH, 1, lrintf(displayAltitudeCm / 10.0f));
 213 }
 214 #endif //defined(USE_BARO) || defined(USE_GPS)
 215
 216 int32_t getEstimatedAltitudeCm(void)
 217 {
 218     return lrintf(displayAltitudeCm);
 219 }
 220
 221 float getAltitude(void)
 222 {
 223     return zeroedAltitudeCm;
 224 }
 225
 226 #ifdef USE_VARIO
 227 int16_t getEstimatedVario(void)
 228 {
 229     return estimatedVario;
 230 }
 231 #endif