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