src/main/navigation/navigation_pos_estimator_private.h

   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 #include <string.h>
  22
  23 #include "common/axis.h"
  24 #include "common/maths.h"
  25 #include "common/filter.h"
  26 #include "common/calibration.h"
  27
  28 #include "sensors/sensors.h"
  29
  30 #define INAV_GPS_DEFAULT_EPH                200.0f  // 2m GPS HDOP  (gives about 1.6s of dead-reckoning if GPS is temporary lost)
  31 #define INAV_GPS_DEFAULT_EPV                500.0f  // 5m GPS VDOP
  32
  33 #define INAV_GPS_ACCEPTANCE_EPE             500.0f  // 5m acceptance radius
  34
  35 #define INAV_ACC_BIAS_ACCEPTANCE_VALUE      (GRAVITY_CMSS * 0.25f)   // Max accepted bias correction of 0.25G - unlikely we are going to be that much off anyway
  36
  37 #define INAV_GPS_GLITCH_RADIUS              250.0f  // 2.5m GPS glitch radius
  38 #define INAV_GPS_GLITCH_ACCEL               1000.0f // 10m/s/s max possible acceleration for GPS glitch detection
  39
  40 #define INAV_POSITION_PUBLISH_RATE_HZ       50      // Publish position updates at this rate
  41 #define INAV_PITOT_UPDATE_RATE              10
  42 #define INAV_COG_UPDATE_RATE_HZ             20      // ground course update rate
  43
  44 #define INAV_GPS_TIMEOUT_MS                 1500    // GPS timeout
  45 #define INAV_BARO_TIMEOUT_MS                200     // Baro timeout
  46 #define INAV_SURFACE_TIMEOUT_MS             400     // Surface timeout    (missed 3 readings in a row)
  47 #define INAV_FLOW_TIMEOUT_MS                200
  48
  49 #define CALIBRATING_GRAVITY_TIME_MS         2000
  50
  51 // Time constants for calculating Baro/Sonar averages. Should be the same value to impose same amount of group delay
  52 #define INAV_BARO_AVERAGE_HZ                1.0f
  53 #define INAV_SURFACE_AVERAGE_HZ             1.0f
  54
  55 #define INAV_ACC_CLIPPING_RC_CONSTANT           (0.010f)    // Reduce acc weight for ~10ms after clipping
  56
  57 #define RANGEFINDER_RELIABILITY_RC_CONSTANT     (0.47802f)
  58 #define RANGEFINDER_RELIABILITY_LIGHT_THRESHOLD (0.15f)
  59 #define RANGEFINDER_RELIABILITY_LOW_THRESHOLD   (0.33f)
  60 #define RANGEFINDER_RELIABILITY_HIGH_THRESHOLD  (0.75f)
  61
  62 typedef struct {
  63     timeUs_t    lastTriggeredTime;
  64     timeUs_t    deltaTime;
  65 } navigationTimer_t;
  66
  67 typedef struct {
  68     timeUs_t    lastUpdateTime; // Last update time (us)
  69 #if defined(NAV_GPS_GLITCH_DETECTION)
  70     bool        glitchDetected;
  71     bool        glitchRecovery;
  72 #endif
  73     fpVector3_t pos;            // GPS position in NEU coordinate system (cm)
  74     fpVector3_t vel;            // GPS velocity (cms)
  75     float       eph;
  76     float       epv;
  77 } navPositionEstimatorGPS_t;
  78
  79 typedef struct {
  80     timeUs_t    lastUpdateTime; // Last update time (us)
  81     pt1Filter_t avgFilter;
  82     float       alt;            // Raw barometric altitude (cm)
  83     float       epv;
  84 } navPositionEstimatorBARO_t;
  85
  86 typedef struct {
  87     timeUs_t    lastUpdateTime; // Last update time (us)
  88     float       airspeed;            // airspeed (cm/s)
  89 } navPositionEstimatorPITOT_t;
  90
  91 typedef enum {
  92     SURFACE_QUAL_LOW,   // Surface sensor signal lost long ago - most likely surface distance is incorrect
  93     SURFACE_QUAL_MID,   // Surface sensor is not available but we can somewhat trust the estimate
  94     SURFACE_QUAL_HIGH   // All good
  95 } navAGLEstimateQuality_e;
  96
  97 typedef struct {
  98     timeUs_t    lastUpdateTime; // Last update time (us)
  99     pt1Filter_t avgFilter;
 100     float       alt;            // Raw altitude measurement (cm)
 101     float       reliability;
 102 } navPositionEstimatorSURFACE_t;
 103
 104 typedef struct {
 105     timeUs_t    lastUpdateTime; // Last update time (us)
 106     bool        isValid;
 107     float       quality;
 108     float       flowRate[2];
 109     float       bodyRate[2];
 110 } navPositionEstimatorFLOW_t;
 111
 112 typedef struct {
 113     timeUs_t    lastUpdateTime; // Last update time (us)
 114
 115     // 3D position, velocity and confidence
 116     fpVector3_t pos;
 117     fpVector3_t vel;
 118     float       eph;
 119     float       epv;
 120
 121     // AGL
 122     navAGLEstimateQuality_e aglQual;
 123     float                   aglOffset;  // Offset between surface and pos.Z
 124     float                   aglAlt;
 125     float                   aglVel;
 126
 127     // FLOW
 128     float                   flowCoordinates[2];
 129
 130     // COURSE
 131     int16_t     cog;    // course over ground (decidegrees)
 132 } navPositionEstimatorESTIMATE_t;
 133
 134 typedef struct {
 135      timeUs_t               lastUpdateTime;
 136     fpVector3_t             accelNEU;
 137     fpVector3_t             accelBias;
 138     float                   calibratedGravityCMSS;
 139     float                   accWeightFactor;
 140     zeroCalibrationScalar_t gravityCalibration;
 141 } navPosisitonEstimatorIMU_t;
 142
 143 typedef enum {
 144     EST_GPS_XY_VALID            = (1 << 0),
 145     EST_GPS_Z_VALID             = (1 << 1),
 146     EST_BARO_VALID              = (1 << 2),
 147     EST_SURFACE_VALID           = (1 << 3),
 148     EST_FLOW_VALID              = (1 << 4),
 149     EST_XY_VALID                = (1 << 5),
 150     EST_Z_VALID                 = (1 << 6),
 151 } navPositionEstimationFlags_e;
 152
 153 typedef struct {
 154     timeUs_t    baroGroundTimeout;
 155     float       baroGroundAlt;
 156     bool        isBaroGroundValid;
 157 } navPositionEstimatorSTATE_t;
 158
 159
 160 typedef struct {
 161     uint32_t                    flags;
 162
 163     // Data sources
 164     navPositionEstimatorGPS_t   gps;
 165     navPositionEstimatorBARO_t  baro;
 166     navPositionEstimatorSURFACE_t surface;
 167     navPositionEstimatorPITOT_t pitot;
 168     navPositionEstimatorFLOW_t  flow;
 169
 170     // IMU data
 171     navPosisitonEstimatorIMU_t  imu;
 172
 173     // Estimate
 174     navPositionEstimatorESTIMATE_t  est;
 175
 176     // Extra state variables
 177     navPositionEstimatorSTATE_t state;
 178 } navigationPosEstimator_t;
 179
 180 typedef struct {
 181     float dt;
 182     uint32_t newFlags;
 183     float newEPV;
 184     float newEPH;
 185     fpVector3_t estPosCorr;
 186     fpVector3_t estVelCorr;
 187     fpVector3_t accBiasCorr;
 188 } estimationContext_t;
 189
 190 extern navigationPosEstimator_t posEstimator;
 191
 192 extern float updateEPE(const float oldEPE, const float dt, const float newEPE, const float w);
 193 extern void estimationCalculateAGL(estimationContext_t * ctx);
 194 extern bool estimationCalculateCorrection_XY_FLOW(estimationContext_t * ctx);
 195 extern float navGetAccelerometerWeight(void);
 196