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, 1);
  56
  57 PG_RESET_TEMPLATE(positionConfig_t, positionConfig,
  58     .altSource = DEFAULT,
  59 );
  60
  61 static int32_t estimatedAltitudeCm = 0;                // in cm
  62
  63 #define BARO_UPDATE_FREQUENCY_40HZ (1000 * 25)
  64
  65 #ifdef USE_VARIO
  66 static int16_t estimatedVario = 0;                   // in cm/s
  67
  68 int16_t calculateEstimatedVario(int32_t baroAlt, const uint32_t dTime) {
  69     static float vel = 0;
  70     static int32_t lastBaroAlt = 0;
  71
  72     int32_t baroVel = 0;
  73
  74     baroVel = (baroAlt - lastBaroAlt) * 1000000.0f / dTime;
  75     lastBaroAlt = baroAlt;
  76
  77     baroVel = constrain(baroVel, -1500.0f, 1500.0f);
  78     baroVel = applyDeadband(baroVel, 10.0f);
  79
  80     vel = vel * CONVERT_PARAMETER_TO_FLOAT(barometerConfig()->baro_cf_vel) + baroVel * (1.0f - CONVERT_PARAMETER_TO_FLOAT(barometerConfig()->baro_cf_vel));
  81     int32_t vel_tmp = lrintf(vel);
  82     vel_tmp = applyDeadband(vel_tmp, 5.0f);
  83
  84     return constrain(vel_tmp, SHRT_MIN, SHRT_MAX);
  85 }
  86 #endif
  87
  88 #if defined(USE_BARO) || defined(USE_GPS)
  89 static bool altitudeOffsetSet = false;
  90
  91 void calculateEstimatedAltitude(timeUs_t currentTimeUs)
  92 {
  93     static timeUs_t previousTimeUs = 0;
  94     static int32_t baroAltOffset = 0;
  95     static int32_t gpsAltOffset = 0;
  96
  97     const uint32_t dTime = currentTimeUs - previousTimeUs;
  98     if (dTime < BARO_UPDATE_FREQUENCY_40HZ) {
  99         schedulerIgnoreTaskExecTime();
 100         return;
 101     }
 102     previousTimeUs = currentTimeUs;
 103
 104     int32_t baroAlt = 0;
 105     int32_t gpsAlt = 0;
 106
 107 #if defined(USE_GPS) && defined(USE_VARIO)
 108     int16_t gpsVertSpeed = 0;
 109 #endif
 110     float gpsTrust = 0.3; //conservative default
 111     bool haveBaroAlt = false;
 112     bool haveGpsAlt = false;
 113 #ifdef USE_BARO
 114     if (sensors(SENSOR_BARO)) {
 115         if (!baroIsCalibrationComplete()) {
 116             performBaroCalibrationCycle();
 117         } else {
 118             baroAlt = baroCalculateAltitude();
 119             haveBaroAlt = true;
 120         }
 121     }
 122 #endif
 123
 124 #ifdef USE_GPS
 125     if (sensors(SENSOR_GPS) && STATE(GPS_FIX)) {
 126         gpsAlt = gpsSol.llh.altCm;
 127 #ifdef USE_VARIO
 128         gpsVertSpeed = GPS_verticalSpeedInCmS;
 129 #endif
 130         haveGpsAlt = true;
 131
 132         if (gpsSol.hdop != 0) {
 133             gpsTrust = 100.0 / gpsSol.hdop;
 134         }
 135         // always use at least 10% of other sources besides gps if available
 136         gpsTrust = MIN(gpsTrust, 0.9f);
 137     }
 138 #endif
 139
 140     if (ARMING_FLAG(ARMED) && !altitudeOffsetSet) {
 141         baroAltOffset = baroAlt;
 142         gpsAltOffset = gpsAlt;
 143         altitudeOffsetSet = true;
 144     } else if (!ARMING_FLAG(ARMED) && altitudeOffsetSet) {
 145         altitudeOffsetSet = false;
 146     }
 147     baroAlt -= baroAltOffset;
 148     gpsAlt -= gpsAltOffset;
 149
 150
 151     if (haveGpsAlt && haveBaroAlt && positionConfig()->altSource == DEFAULT) {
 152         if (ARMING_FLAG(ARMED)) {
 153             estimatedAltitudeCm = gpsAlt * gpsTrust + baroAlt * (1 - gpsTrust);
 154         } else {
 155             estimatedAltitudeCm = gpsAlt; //absolute altitude is shown before arming, ignore baro
 156         }
 157 #ifdef USE_VARIO
 158         // baro is a better source for vario, so ignore gpsVertSpeed
 159         estimatedVario = calculateEstimatedVario(baroAlt, dTime);
 160 #endif
 161     } else if (haveGpsAlt && (positionConfig()->altSource == GPS_ONLY || positionConfig()->altSource == DEFAULT )) {
 162         estimatedAltitudeCm = gpsAlt;
 163 #if defined(USE_VARIO) && defined(USE_GPS)
 164         estimatedVario = gpsVertSpeed;
 165 #endif
 166     } else if (haveBaroAlt && (positionConfig()->altSource == BARO_ONLY || positionConfig()->altSource == DEFAULT)) {
 167         estimatedAltitudeCm = baroAlt;
 168 #ifdef USE_VARIO
 169         estimatedVario = calculateEstimatedVario(baroAlt, dTime);
 170 #endif
 171     }
 172
 173
 174
 175     DEBUG_SET(DEBUG_ALTITUDE, 0, (int32_t)(100 * gpsTrust));
 176     DEBUG_SET(DEBUG_ALTITUDE, 1, baroAlt);
 177     DEBUG_SET(DEBUG_ALTITUDE, 2, gpsAlt);
 178 #ifdef USE_VARIO
 179     DEBUG_SET(DEBUG_ALTITUDE, 3, estimatedVario);
 180 #endif
 181 }
 182
 183 bool isAltitudeOffset(void)
 184 {
 185     return altitudeOffsetSet;
 186 }
 187 #endif
 188
 189 int32_t getEstimatedAltitudeCm(void)
 190 {
 191     return estimatedAltitudeCm;
 192 }
 193
 194 #ifdef USE_VARIO
 195 int16_t getEstimatedVario(void)
 196 {
 197     return estimatedVario;
 198 }
 199 #endif