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 int32_t estimatedAltitudeCm = 0;                // in cm
  51 #ifdef USE_BARO
  52     static pt2Filter_t baroDerivativeLpf;
  53 #endif
  54
  55 typedef enum {
  56     DEFAULT = 0,
  57     BARO_ONLY,
  58     GPS_ONLY
  59 } altSource_e;
  60
  61 PG_REGISTER_WITH_RESET_TEMPLATE(positionConfig_t, positionConfig, PG_POSITION, 2);
  62
  63 PG_RESET_TEMPLATE(positionConfig_t, positionConfig,
  64     .altSource = DEFAULT,
  65     .altNumSatsGpsUse = POSITION_DEFAULT_ALT_NUM_SATS_GPS_USE,
  66     .altNumSatsBaroFallback = POSITION_DEFAULT_ALT_NUM_SATS_BARO_FALLBACK,
  67 );
  68
  69 #ifdef USE_VARIO
  70 static int16_t estimatedVario = 0;                   // in cm/s
  71
  72 int16_t calculateEstimatedVario(float baroAltVelocity)
  73 {
  74     baroAltVelocity = applyDeadband(baroAltVelocity, 10.0f); // cm/s, so ignore climb rates less than 0.1 m/s
  75     return constrain(lrintf(baroAltVelocity), -1500, 1500);
  76 }
  77 #endif
  78
  79 #if defined(USE_BARO) || defined(USE_GPS)
  80 static bool altitudeOffsetSetBaro = false;
  81 static bool altitudeOffsetSetGPS = false;
  82
  83 void calculateEstimatedAltitude()
  84 {
  85     static float baroAltOffset = 0;
  86     static float gpsAltOffset = 0;
  87
  88     float baroAlt = 0;
  89     float baroAltVelocity = 0;
  90     float gpsAlt = 0;
  91     uint8_t gpsNumSat = 0;
  92
  93 #if defined(USE_GPS) && defined(USE_VARIO)
  94     float gpsVertSpeed = 0;
  95 #endif
  96     float gpsTrust = 0.3; //conservative default
  97     bool haveBaroAlt = false;
  98     bool haveGpsAlt = false;
  99 #ifdef USE_BARO
 100     if (sensors(SENSOR_BARO)) {
 101         static float lastBaroAlt = 0;
 102         static bool initBaroFilter = false;
 103         if (!initBaroFilter) {
 104             const float cutoffHz = barometerConfig()->baro_vario_lpf / 100.0f;
 105             const float sampleTimeS = HZ_TO_INTERVAL(TASK_ALTITUDE_RATE_HZ);
 106             const float gain = pt2FilterGain(cutoffHz, sampleTimeS);
 107             pt2FilterInit(&baroDerivativeLpf, gain);
 108             initBaroFilter = true;
 109         }
 110         baroAlt = baroUpsampleAltitude();
 111         const float baroAltVelocityRaw = (baroAlt - lastBaroAlt) * TASK_ALTITUDE_RATE_HZ; // cm/s
 112         baroAltVelocity = pt2FilterApply(&baroDerivativeLpf, baroAltVelocityRaw);
 113         lastBaroAlt = baroAlt;
 114         if (baroIsCalibrated()) {
 115             haveBaroAlt = true;
 116         }
 117     }
 118 #endif
 119
 120 #ifdef USE_GPS
 121     if (sensors(SENSOR_GPS) && STATE(GPS_FIX)) {
 122         gpsAlt = gpsSol.llh.altCm;
 123         gpsNumSat = gpsSol.numSat;
 124 #ifdef USE_VARIO
 125         gpsVertSpeed = GPS_verticalSpeedInCmS;
 126 #endif
 127         haveGpsAlt = true;
 128
 129         if (gpsSol.hdop != 0) {
 130             gpsTrust = 100.0 / gpsSol.hdop;
 131         }
 132         // always use at least 10% of other sources besides gps if available
 133         gpsTrust = MIN(gpsTrust, 0.9f);
 134     }
 135 #endif
 136
 137     if (ARMING_FLAG(ARMED) && !altitudeOffsetSetBaro) {
 138         baroAltOffset = baroAlt;
 139         altitudeOffsetSetBaro = true;
 140     } else if (!ARMING_FLAG(ARMED) && altitudeOffsetSetBaro) {
 141         altitudeOffsetSetBaro = false;
 142     }
 143
 144     baroAlt -= baroAltOffset;
 145
 146     int goodGpsSats = 0;
 147     int badGpsSats = -1;
 148
 149     if (haveBaroAlt) {
 150         goodGpsSats = positionConfig()->altNumSatsGpsUse;
 151         badGpsSats = positionConfig()->altNumSatsBaroFallback;
 152     }
 153
 154     if (ARMING_FLAG(ARMED)) {
 155         if (!altitudeOffsetSetGPS && gpsNumSat >= goodGpsSats) {
 156             gpsAltOffset = gpsAlt - baroAlt;
 157             altitudeOffsetSetGPS = true;
 158         } else if (gpsNumSat <= badGpsSats) {
 159             altitudeOffsetSetGPS = false;
 160         }
 161     } else if (!ARMING_FLAG(ARMED) && altitudeOffsetSetGPS) {
 162         altitudeOffsetSetGPS = false;
 163     }
 164
 165     gpsAlt -= gpsAltOffset;
 166
 167     if (!altitudeOffsetSetGPS) {
 168         haveGpsAlt = false;
 169         gpsTrust = 0.0f;
 170     }
 171
 172     if (haveGpsAlt && haveBaroAlt && positionConfig()->altSource == DEFAULT) {
 173         if (ARMING_FLAG(ARMED)) {
 174             estimatedAltitudeCm = gpsAlt * gpsTrust + baroAlt * (1 - gpsTrust);
 175         } else {
 176             estimatedAltitudeCm = gpsAlt; //absolute altitude is shown before arming, ignore baro
 177         }
 178 #ifdef USE_VARIO
 179         // baro is a better source for vario, so ignore gpsVertSpeed
 180         estimatedVario = calculateEstimatedVario(baroAltVelocity);
 181 #endif
 182     } else if (haveGpsAlt && (positionConfig()->altSource == GPS_ONLY || positionConfig()->altSource == DEFAULT )) {
 183         estimatedAltitudeCm = gpsAlt;
 184 #if defined(USE_VARIO) && defined(USE_GPS)
 185         estimatedVario = gpsVertSpeed;
 186 #endif
 187     } else if (haveBaroAlt && (positionConfig()->altSource == BARO_ONLY || positionConfig()->altSource == DEFAULT)) {
 188         estimatedAltitudeCm = baroAlt;
 189 #ifdef USE_VARIO
 190         estimatedVario = calculateEstimatedVario(baroAltVelocity); // cm/s
 191 #endif
 192     }
 193
 194     DEBUG_SET(DEBUG_ALTITUDE, 0, (int32_t)(100 * gpsTrust));
 195     DEBUG_SET(DEBUG_ALTITUDE, 1, baroAlt);
 196     DEBUG_SET(DEBUG_ALTITUDE, 2, gpsAlt);
 197 #ifdef USE_VARIO
 198     DEBUG_SET(DEBUG_ALTITUDE, 3, estimatedVario);
 199 #endif
 200 }
 201
 202 bool isAltitudeOffset(void)
 203 {
 204     return altitudeOffsetSetBaro || altitudeOffsetSetGPS;
 205 }
 206 #endif
 207
 208 int32_t getEstimatedAltitudeCm(void)
 209 {
 210     return estimatedAltitudeCm;
 211 }
 212
 213 #ifdef USE_VARIO
 214 int16_t getEstimatedVario(void)
 215 {
 216     return estimatedVario;
 217 }
 218 #endif