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
  33 #include "fc/runtime_config.h"
  34
  35 #include "flight/position.h"
  36 #include "flight/imu.h"
  37 #include "flight/pid.h"
  38
  39 #include "io/gps.h"
  40
  41 #include "scheduler/scheduler.h"
  42
  43 #include "sensors/sensors.h"
  44 #include "sensors/barometer.h"
  45
  46 #include "pg/pg.h"
  47 #include "pg/pg_ids.h"
  48
  49 typedef enum {
  50     DEFAULT = 0,
  51     BARO_ONLY,
  52     GPS_ONLY
  53 } altSource_e;
  54
  55 PG_REGISTER_WITH_RESET_TEMPLATE(positionConfig_t, positionConfig, PG_POSITION, 2);
  56
  57 PG_RESET_TEMPLATE(positionConfig_t, positionConfig,
  58     .altSource = DEFAULT,
  59     .altNumSatsGpsUse = POSITION_DEFAULT_ALT_NUM_SATS_GPS_USE,
  60     .altNumSatsBaroFallback = POSITION_DEFAULT_ALT_NUM_SATS_BARO_FALLBACK,
  61 );
  62
  63 static int32_t estimatedAltitudeCm = 0;                // in cm
  64
  65 #define BARO_UPDATE_FREQUENCY_40HZ (1000 * 25)
  66
  67 #ifdef USE_VARIO
  68 static int16_t estimatedVario = 0;                   // in cm/s
  69
  70 int16_t calculateEstimatedVario(int32_t baroAlt, const uint32_t dTime) {
  71     static float vel = 0;
  72     static int32_t lastBaroAlt = 0;
  73
  74     int32_t baroVel = 0;
  75
  76     baroVel = (baroAlt - lastBaroAlt) * 1000000.0f / dTime;
  77     lastBaroAlt = baroAlt;
  78
  79     baroVel = constrain(baroVel, -1500.0f, 1500.0f);
  80     baroVel = applyDeadband(baroVel, 10.0f);
  81
  82     vel = vel * CONVERT_PARAMETER_TO_FLOAT(barometerConfig()->baro_cf_vel) + baroVel * (1.0f - CONVERT_PARAMETER_TO_FLOAT(barometerConfig()->baro_cf_vel));
  83     int32_t vel_tmp = lrintf(vel);
  84     vel_tmp = applyDeadband(vel_tmp, 5.0f);
  85
  86     return constrain(vel_tmp, SHRT_MIN, SHRT_MAX);
  87 }
  88 #endif
  89
  90 #if defined(USE_BARO) || defined(USE_GPS)
  91 static bool altitudeOffsetSetBaro = false;
  92 static bool altitudeOffsetSetGPS = false;
  93
  94 void calculateEstimatedAltitude(timeUs_t currentTimeUs)
  95 {
  96     static timeUs_t previousTimeUs = 0;
  97     static int32_t baroAltOffset = 0;
  98     static int32_t gpsAltOffset = 0;
  99
 100     const uint32_t dTime = currentTimeUs - previousTimeUs;
 101     if (dTime < BARO_UPDATE_FREQUENCY_40HZ) {
 102         schedulerIgnoreTaskExecTime();
 103         return;
 104     }
 105     previousTimeUs = currentTimeUs;
 106
 107     int32_t baroAlt = 0;
 108     int32_t gpsAlt = 0;
 109     uint8_t gpsNumSat = 0;
 110
 111 #if defined(USE_GPS) && defined(USE_VARIO)
 112     int16_t gpsVertSpeed = 0;
 113 #endif
 114     float gpsTrust = 0.3; //conservative default
 115     bool haveBaroAlt = false;
 116     bool haveGpsAlt = false;
 117 #ifdef USE_BARO
 118     if (sensors(SENSOR_BARO)) {
 119         if (!baroIsCalibrationComplete()) {
 120             performBaroCalibrationCycle();
 121         } else {
 122             baroAlt = baroCalculateAltitude();
 123             haveBaroAlt = true;
 124         }
 125     }
 126 #endif
 127
 128 #ifdef USE_GPS
 129     if (sensors(SENSOR_GPS) && STATE(GPS_FIX)) {
 130         gpsAlt = gpsSol.llh.altCm;
 131         gpsNumSat = gpsSol.numSat;
 132 #ifdef USE_VARIO
 133         gpsVertSpeed = GPS_verticalSpeedInCmS;
 134 #endif
 135         haveGpsAlt = true;
 136
 137         if (gpsSol.hdop != 0) {
 138             gpsTrust = 100.0 / gpsSol.hdop;
 139         }
 140         // always use at least 10% of other sources besides gps if available
 141         gpsTrust = MIN(gpsTrust, 0.9f);
 142     }
 143 #endif
 144
 145     if (ARMING_FLAG(ARMED) && !altitudeOffsetSetBaro) {
 146         baroAltOffset = baroAlt;
 147         altitudeOffsetSetBaro = true;
 148     } else if (!ARMING_FLAG(ARMED) && altitudeOffsetSetBaro) {
 149         altitudeOffsetSetBaro = false;
 150     }
 151
 152     baroAlt -= baroAltOffset;
 153
 154     int goodGpsSats = 0;
 155     int badGpsSats = -1;
 156
 157     if (haveBaroAlt) {
 158         goodGpsSats = positionConfig()->altNumSatsGpsUse;
 159         badGpsSats = positionConfig()->altNumSatsBaroFallback;
 160     }
 161
 162     if (ARMING_FLAG(ARMED)) {
 163         if (!altitudeOffsetSetGPS && gpsNumSat >= goodGpsSats) {
 164             gpsAltOffset = gpsAlt - baroAlt;
 165             altitudeOffsetSetGPS = true;
 166         } else if (gpsNumSat <= badGpsSats) {
 167             altitudeOffsetSetGPS = false;
 168         }
 169     } else if (!ARMING_FLAG(ARMED) && altitudeOffsetSetGPS) {
 170         altitudeOffsetSetGPS = false;
 171     }
 172
 173     gpsAlt -= gpsAltOffset;
 174
 175     if (!altitudeOffsetSetGPS) {
 176         haveGpsAlt = false;
 177         gpsTrust = 0.0f;
 178     }
 179
 180     if (haveGpsAlt && haveBaroAlt && positionConfig()->altSource == DEFAULT) {
 181         if (ARMING_FLAG(ARMED)) {
 182             estimatedAltitudeCm = gpsAlt * gpsTrust + baroAlt * (1 - gpsTrust);
 183         } else {
 184             estimatedAltitudeCm = gpsAlt; //absolute altitude is shown before arming, ignore baro
 185         }
 186 #ifdef USE_VARIO
 187         // baro is a better source for vario, so ignore gpsVertSpeed
 188         estimatedVario = calculateEstimatedVario(baroAlt, dTime);
 189 #endif
 190     } else if (haveGpsAlt && (positionConfig()->altSource == GPS_ONLY || positionConfig()->altSource == DEFAULT )) {
 191         estimatedAltitudeCm = gpsAlt;
 192 #if defined(USE_VARIO) && defined(USE_GPS)
 193         estimatedVario = gpsVertSpeed;
 194 #endif
 195     } else if (haveBaroAlt && (positionConfig()->altSource == BARO_ONLY || positionConfig()->altSource == DEFAULT)) {
 196         estimatedAltitudeCm = baroAlt;
 197 #ifdef USE_VARIO
 198         estimatedVario = calculateEstimatedVario(baroAlt, dTime);
 199 #endif
 200     }
 201
 202
 203
 204     DEBUG_SET(DEBUG_ALTITUDE, 0, (int32_t)(100 * gpsTrust));
 205     DEBUG_SET(DEBUG_ALTITUDE, 1, baroAlt);
 206     DEBUG_SET(DEBUG_ALTITUDE, 2, gpsAlt);
 207 #ifdef USE_VARIO
 208     DEBUG_SET(DEBUG_ALTITUDE, 3, estimatedVario);
 209 #endif
 210 }
 211
 212 bool isAltitudeOffset(void)
 213 {
 214     return altitudeOffsetSetBaro || altitudeOffsetSetGPS;
 215 }
 216 #endif
 217
 218 int32_t getEstimatedAltitudeCm(void)
 219 {
 220     return estimatedAltitudeCm;
 221 }
 222
 223 #ifdef USE_VARIO
 224 int16_t getEstimatedVario(void)
 225 {
 226     return estimatedVario;
 227 }
 228 #endif