src/main/navigation/navigation_pos_estimator.c

   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 "platform.h"
  24
  25 #include "build/build_config.h"
  26 #include "build/debug.h"
  27
  28 #include "common/axis.h"
  29 #include "common/log.h"
  30 #include "common/maths.h"
  31
  32 #include "config/parameter_group.h"
  33 #include "config/parameter_group_ids.h"
  34
  35 #include "drivers/time.h"
  36
  37 #include "fc/config.h"
  38 #include "fc/settings.h"
  39
  40 #include "flight/imu.h"
  41 #include "flight/secondary_imu.h"
  42
  43 #include "io/gps.h"
  44
  45 #include "navigation/navigation.h"
  46 #include "navigation/navigation_private.h"
  47 #include "navigation/navigation_pos_estimator_private.h"
  48
  49 #include "sensors/acceleration.h"
  50 #include "sensors/barometer.h"
  51 #include "sensors/compass.h"
  52 #include "sensors/pitotmeter.h"
  53 #include "sensors/opflow.h"
  54
  55 navigationPosEstimator_t posEstimator;
  56
  57 PG_REGISTER_WITH_RESET_TEMPLATE(positionEstimationConfig_t, positionEstimationConfig, PG_POSITION_ESTIMATION_CONFIG, 5);
  58
  59 PG_RESET_TEMPLATE(positionEstimationConfig_t, positionEstimationConfig,
  60         // Inertial position estimator parameters
  61         .automatic_mag_declination = SETTING_INAV_AUTO_MAG_DECL_DEFAULT,
  62         .reset_altitude_type = SETTING_INAV_RESET_ALTITUDE_DEFAULT,
  63         .reset_home_type = SETTING_INAV_RESET_HOME_DEFAULT,
  64         .gravity_calibration_tolerance = SETTING_INAV_GRAVITY_CAL_TOLERANCE_DEFAULT,  // 5 cm/s/s calibration error accepted (0.5% of gravity)
  65         .use_gps_velned = SETTING_INAV_USE_GPS_VELNED_DEFAULT,                        // "Disabled" is mandatory with gps_dyn_model = Pedestrian
  66         .use_gps_no_baro = SETTING_INAV_USE_GPS_NO_BARO_DEFAULT,                      // Use GPS altitude if no baro is available on all aircrafts
  67         .allow_dead_reckoning = SETTING_INAV_ALLOW_DEAD_RECKONING_DEFAULT,
  68
  69         .max_surface_altitude = SETTING_INAV_MAX_SURFACE_ALTITUDE_DEFAULT,
  70
  71         .w_xyz_acc_p = SETTING_INAV_W_XYZ_ACC_P_DEFAULT,
  72
  73         .w_z_baro_p = SETTING_INAV_W_Z_BARO_P_DEFAULT,
  74
  75         .w_z_surface_p = SETTING_INAV_W_Z_SURFACE_P_DEFAULT,
  76         .w_z_surface_v = SETTING_INAV_W_Z_SURFACE_V_DEFAULT,
  77
  78         .w_z_gps_p = SETTING_INAV_W_Z_GPS_P_DEFAULT,
  79         .w_z_gps_v = SETTING_INAV_W_Z_GPS_V_DEFAULT,
  80
  81         .w_xy_gps_p = SETTING_INAV_W_XY_GPS_P_DEFAULT,
  82         .w_xy_gps_v = SETTING_INAV_W_XY_GPS_V_DEFAULT,
  83
  84         .w_xy_flow_p = SETTING_INAV_W_XY_FLOW_P_DEFAULT,
  85         .w_xy_flow_v = SETTING_INAV_W_XY_FLOW_V_DEFAULT,
  86
  87         .w_z_res_v = SETTING_INAV_W_Z_RES_V_DEFAULT,
  88         .w_xy_res_v = SETTING_INAV_W_XY_RES_V_DEFAULT,
  89
  90         .w_acc_bias = SETTING_INAV_W_ACC_BIAS_DEFAULT,
  91
  92         .max_eph_epv = SETTING_INAV_MAX_EPH_EPV_DEFAULT,
  93         .baro_epv = SETTING_INAV_BARO_EPV_DEFAULT
  94 );
  95
  96 #define resetTimer(tim, currentTimeUs) { (tim)->deltaTime = 0; (tim)->lastTriggeredTime = currentTimeUs; }
  97 #define getTimerDeltaMicros(tim) ((tim)->deltaTime)
  98 static bool updateTimer(navigationTimer_t * tim, timeUs_t interval, timeUs_t currentTimeUs)
  99 {
 100     if ((currentTimeUs - tim->lastTriggeredTime) >= interval) {
 101         tim->deltaTime = currentTimeUs - tim->lastTriggeredTime;
 102         tim->lastTriggeredTime = currentTimeUs;
 103         return true;
 104     }
 105     else {
 106         return false;
 107     }
 108 }
 109
 110 static bool shouldResetReferenceAltitude(void)
 111 {
 112     switch ((nav_reset_type_e)positionEstimationConfig()->reset_altitude_type) {
 113         case NAV_RESET_NEVER:
 114             return false;
 115         case NAV_RESET_ON_FIRST_ARM:
 116             return !ARMING_FLAG(ARMED) && !ARMING_FLAG(WAS_EVER_ARMED);
 117         case NAV_RESET_ON_EACH_ARM:
 118             return !ARMING_FLAG(ARMED);
 119     }
 120
 121     return false;
 122 }
 123
 124 #if defined(USE_GPS)
 125 /* Why is this here: Because GPS will be sending at quiet a nailed rate (if not overloaded by junk tasks at the brink of its specs)
 126  * but we might read out with timejitter because Irq might be off by a few us so we do a +-10% margin around the time between GPS
 127  * datasets representing the most common Hz-rates today. You might want to extend the list or find a smarter way.
 128  * Don't overload your GPS in its config with trash, choose a Hz rate that it can deliver at a sustained rate.
 129  * (c) CrashPilot1000
 130  */
 131 static timeUs_t getGPSDeltaTimeFilter(timeUs_t dTus)
 132 {
 133     if (dTus >= 225000 && dTus <= 275000) return HZ2US(4);       //  4Hz Data 250ms
 134     if (dTus >= 180000 && dTus <= 220000) return HZ2US(5);       //  5Hz Data 200ms
 135     if (dTus >=  90000 && dTus <= 110000) return HZ2US(10);      // 10Hz Data 100ms
 136     if (dTus >=  45000 && dTus <=  55000) return HZ2US(20);      // 20Hz Data  50ms
 137     if (dTus >=  30000 && dTus <=  36000) return HZ2US(30);      // 30Hz Data  33ms
 138     if (dTus >=  23000 && dTus <=  27000) return HZ2US(40);      // 40Hz Data  25ms
 139     if (dTus >=  18000 && dTus <=  22000) return HZ2US(50);      // 50Hz Data  20ms
 140     return dTus;                                                 // Filter failed. Set GPS Hz by measurement
 141 }
 142
 143 #if defined(NAV_GPS_GLITCH_DETECTION)
 144 static bool detectGPSGlitch(timeUs_t currentTimeUs)
 145 {
 146     static timeUs_t previousTime = 0;
 147     static fpVector3_t lastKnownGoodPosition;
 148     static fpVector3_t lastKnownGoodVelocity;
 149
 150     bool isGlitching = false;
 151
 152     if (previousTime == 0) {
 153         isGlitching = false;
 154     }
 155     else {
 156         fpVector3_t predictedGpsPosition;
 157         float gpsDistance;
 158         float dT = US2S(currentTimeUs - previousTime);
 159
 160         /* We predict new position based on previous GPS velocity and position */
 161         predictedGpsPosition.x = lastKnownGoodPosition.x + lastKnownGoodVelocity.x * dT;
 162         predictedGpsPosition.y = lastKnownGoodPosition.y + lastKnownGoodVelocity.y * dT;
 163
 164         /* New pos is within predefined radius of predicted pos, radius is expanded exponentially */
 165         gpsDistance = fast_fsqrtf(sq(predictedGpsPosition.x - lastKnownGoodPosition.x) + sq(predictedGpsPosition.y - lastKnownGoodPosition.y));
 166         if (gpsDistance <= (INAV_GPS_GLITCH_RADIUS + 0.5f * INAV_GPS_GLITCH_ACCEL * dT * dT)) {
 167             isGlitching = false;
 168         }
 169         else {
 170             isGlitching = true;
 171         }
 172     }
 173
 174     if (!isGlitching) {
 175         previousTime = currentTimeUs;
 176         lastKnownGoodPosition = posEstimator.gps.pos;
 177         lastKnownGoodVelocity = posEstimator.gps.vel;
 178     }
 179
 180     return isGlitching;
 181 }
 182 #endif
 183
 184 /**
 185  * Update GPS topic
 186  *  Function is called on each GPS update
 187  */
 188 void onNewGPSData(void)
 189 {
 190     static timeUs_t lastGPSNewDataTime;
 191     static int32_t previousLat;
 192     static int32_t previousLon;
 193     static int32_t previousAlt;
 194     static bool isFirstGPSUpdate = true;
 195
 196     gpsLocation_t newLLH;
 197     const timeUs_t currentTimeUs = micros();
 198
 199     newLLH.lat = gpsSol.llh.lat;
 200     newLLH.lon = gpsSol.llh.lon;
 201     newLLH.alt = gpsSol.llh.alt;
 202
 203     if (sensors(SENSOR_GPS)) {
 204         if (!STATE(GPS_FIX)) {
 205             isFirstGPSUpdate = true;
 206             return;
 207         }
 208
 209         if ((currentTimeUs - lastGPSNewDataTime) > MS2US(INAV_GPS_TIMEOUT_MS)) {
 210             isFirstGPSUpdate = true;
 211         }
 212
 213         /* Automatic magnetic declination calculation - do this once */
 214         if(STATE(GPS_FIX_HOME)){
 215             static bool magDeclinationSet = false;
 216             if (positionEstimationConfig()->automatic_mag_declination && !magDeclinationSet) {
 217                 const float declination = geoCalculateMagDeclination(&newLLH);
 218                 imuSetMagneticDeclination(declination);
 219 #ifdef USE_SECONDARY_IMU
 220                 secondaryImuSetMagneticDeclination(declination);
 221 #endif
 222                 magDeclinationSet = true;
 223             }
 224         }
 225         /* Process position update if GPS origin is already set, or precision is good enough */
 226         // FIXME: Add HDOP check for acquisition of GPS origin
 227         /* Set GPS origin or reset the origin altitude - keep initial pre-arming altitude at zero */
 228         if (!posControl.gpsOrigin.valid) {
 229             geoSetOrigin(&posControl.gpsOrigin, &newLLH, GEO_ORIGIN_SET);
 230         }
 231         else if (shouldResetReferenceAltitude()) {
 232             /* If we were never armed - keep altitude at zero */
 233             geoSetOrigin(&posControl.gpsOrigin, &newLLH, GEO_ORIGIN_RESET_ALTITUDE);
 234         }
 235
 236         if (posControl.gpsOrigin.valid) {
 237             /* Convert LLH position to local coordinates */
 238             geoConvertGeodeticToLocal(&posEstimator.gps.pos, &posControl.gpsOrigin, &newLLH, GEO_ALT_ABSOLUTE);
 239
 240             /* If not the first update - calculate velocities */
 241             if (!isFirstGPSUpdate) {
 242                 float dT = US2S(getGPSDeltaTimeFilter(currentTimeUs - lastGPSNewDataTime));
 243
 244                 /* Use VELNED provided by GPS if available, calculate from coordinates otherwise */
 245                 float gpsScaleLonDown = constrainf(cos_approx((ABS(gpsSol.llh.lat) / 10000000.0f) * 0.0174532925f), 0.01f, 1.0f);
 246                 if (positionEstimationConfig()->use_gps_velned && gpsSol.flags.validVelNE) {
 247                     posEstimator.gps.vel.x = gpsSol.velNED[X];
 248                     posEstimator.gps.vel.y = gpsSol.velNED[Y];
 249                 }
 250                 else {
 251                     posEstimator.gps.vel.x = (posEstimator.gps.vel.x + (DISTANCE_BETWEEN_TWO_LONGITUDE_POINTS_AT_EQUATOR * (gpsSol.llh.lat - previousLat) / dT)) / 2.0f;
 252                     posEstimator.gps.vel.y = (posEstimator.gps.vel.y + (gpsScaleLonDown * DISTANCE_BETWEEN_TWO_LONGITUDE_POINTS_AT_EQUATOR * (gpsSol.llh.lon - previousLon) / dT)) / 2.0f;
 253                 }
 254
 255                 if (positionEstimationConfig()->use_gps_velned && gpsSol.flags.validVelD) {
 256                     posEstimator.gps.vel.z = -gpsSol.velNED[Z];   // NEU
 257                 }
 258                 else {
 259                     posEstimator.gps.vel.z = (posEstimator.gps.vel.z + (gpsSol.llh.alt - previousAlt) / dT) / 2.0f;
 260                 }
 261
 262 #if defined(NAV_GPS_GLITCH_DETECTION)
 263                 /* GPS glitch protection. We have local coordinates and local velocity for current GPS update. Check if they are sane */
 264                 if (detectGPSGlitch(currentTimeUs)) {
 265                     posEstimator.gps.glitchRecovery = false;
 266                     posEstimator.gps.glitchDetected = true;
 267                 }
 268                 else {
 269                     /* Store previous glitch flag in glitchRecovery to indicate a valid reading after a glitch */
 270                     posEstimator.gps.glitchRecovery = posEstimator.gps.glitchDetected;
 271                     posEstimator.gps.glitchDetected = false;
 272                 }
 273 #endif
 274
 275                 /* FIXME: use HDOP/VDOP */
 276                 if (gpsSol.flags.validEPE) {
 277                     posEstimator.gps.eph = gpsSol.eph;
 278                     posEstimator.gps.epv = gpsSol.epv;
 279                 }
 280                 else {
 281                     posEstimator.gps.eph = INAV_GPS_DEFAULT_EPH;
 282                     posEstimator.gps.epv = INAV_GPS_DEFAULT_EPV;
 283                 }
 284
 285                 /* Indicate a last valid reading of Pos/Vel */
 286                 posEstimator.gps.lastUpdateTime = currentTimeUs;
 287             }
 288
 289             previousLat = gpsSol.llh.lat;
 290             previousLon = gpsSol.llh.lon;
 291             previousAlt = gpsSol.llh.alt;
 292             isFirstGPSUpdate = false;
 293
 294             lastGPSNewDataTime = currentTimeUs;
 295         }
 296     }
 297     else {
 298         posEstimator.gps.lastUpdateTime = 0;
 299     }
 300 }
 301 #endif
 302
 303 #if defined(USE_BARO)
 304 /**
 305  * Read BARO and update alt/vel topic
 306  *  Function is called from TASK_BARO
 307  */
 308 void updatePositionEstimator_BaroTopic(timeUs_t currentTimeUs)
 309 {
 310     static float initialBaroAltitudeOffset = 0.0f;
 311     float newBaroAlt = baroCalculateAltitude();
 312
 313     /* If we are required - keep altitude at zero */
 314     if (shouldResetReferenceAltitude()) {
 315         initialBaroAltitudeOffset = newBaroAlt;
 316     }
 317
 318     if (sensors(SENSOR_BARO) && baroIsCalibrationComplete()) {
 319         const timeUs_t baroDtUs = currentTimeUs - posEstimator.baro.lastUpdateTime;
 320
 321         posEstimator.baro.alt = newBaroAlt - initialBaroAltitudeOffset;
 322         posEstimator.baro.epv = positionEstimationConfig()->baro_epv;
 323         posEstimator.baro.lastUpdateTime = currentTimeUs;
 324
 325         if (baroDtUs <= MS2US(INAV_BARO_TIMEOUT_MS)) {
 326             pt1FilterApply3(&posEstimator.baro.avgFilter, posEstimator.baro.alt, US2S(baroDtUs));
 327         }
 328     }
 329     else {
 330         posEstimator.baro.alt = 0;
 331         posEstimator.baro.lastUpdateTime = 0;
 332     }
 333 }
 334 #endif
 335
 336 #if defined(USE_PITOT)
 337 /**
 338  * Read Pitot and update airspeed topic
 339  *  Function is called at main loop rate, updates happen at reduced rate
 340  */
 341 void updatePositionEstimator_PitotTopic(timeUs_t currentTimeUs)
 342 {
 343     posEstimator.pitot.airspeed = pitot.airSpeed;
 344     posEstimator.pitot.lastUpdateTime = currentTimeUs;
 345 }
 346 #endif
 347
 348 /**
 349  * Update IMU topic
 350  *  Function is called at main loop rate
 351  */
 352 static void restartGravityCalibration(void)
 353 {
 354     zeroCalibrationStartS(&posEstimator.imu.gravityCalibration, CALIBRATING_GRAVITY_TIME_MS, positionEstimationConfig()->gravity_calibration_tolerance, false);
 355 }
 356
 357 static bool gravityCalibrationComplete(void)
 358 {
 359     return zeroCalibrationIsCompleteS(&posEstimator.imu.gravityCalibration);
 360 }
 361
 362 static void updateIMUEstimationWeight(const float dt)
 363 {
 364     bool isAccClipped = accIsClipped();
 365
 366     // If accelerometer measurement is clipped - drop the acc weight to zero
 367     // and gradually restore weight back to 1.0 over time
 368     if (isAccClipped) {
 369         posEstimator.imu.accWeightFactor = 0.0f;
 370     }
 371     else {
 372         const float relAlpha = dt / (dt + INAV_ACC_CLIPPING_RC_CONSTANT);
 373         posEstimator.imu.accWeightFactor = posEstimator.imu.accWeightFactor * (1.0f - relAlpha) + 1.0f * relAlpha;
 374     }
 375
 376     // DEBUG_VIBE[0-3] are used in IMU
 377     DEBUG_SET(DEBUG_VIBE, 4, posEstimator.imu.accWeightFactor * 1000);
 378 }
 379
 380 float navGetAccelerometerWeight(void)
 381 {
 382     const float accWeightScaled = posEstimator.imu.accWeightFactor * positionEstimationConfig()->w_xyz_acc_p;
 383     DEBUG_SET(DEBUG_VIBE, 5, accWeightScaled * 1000);
 384
 385     return accWeightScaled;
 386 }
 387
 388 static void updateIMUTopic(timeUs_t currentTimeUs)
 389 {
 390     const float dt = US2S(currentTimeUs - posEstimator.imu.lastUpdateTime);
 391     posEstimator.imu.lastUpdateTime = currentTimeUs;
 392
 393     if (!isImuReady()) {
 394         posEstimator.imu.accelNEU.x = 0;
 395         posEstimator.imu.accelNEU.y = 0;
 396         posEstimator.imu.accelNEU.z = 0;
 397
 398         restartGravityCalibration();
 399     }
 400     else {
 401         /* Update acceleration weight based on vibration levels and clipping */
 402         updateIMUEstimationWeight(dt);
 403
 404         fpVector3_t accelBF;
 405
 406         /* Read acceleration data in body frame */
 407         accelBF.x = imuMeasuredAccelBF.x;
 408         accelBF.y = imuMeasuredAccelBF.y;
 409         accelBF.z = imuMeasuredAccelBF.z;
 410
 411         /* Correct accelerometer bias */
 412         accelBF.x -= posEstimator.imu.accelBias.x;
 413         accelBF.y -= posEstimator.imu.accelBias.y;
 414         accelBF.z -= posEstimator.imu.accelBias.z;
 415
 416         /* Rotate vector to Earth frame - from Forward-Right-Down to North-East-Up*/
 417         imuTransformVectorBodyToEarth(&accelBF);
 418
 419         /* Read acceleration data in NEU frame from IMU */
 420         posEstimator.imu.accelNEU.x = accelBF.x;
 421         posEstimator.imu.accelNEU.y = accelBF.y;
 422         posEstimator.imu.accelNEU.z = accelBF.z;
 423
 424         /* When unarmed, assume that accelerometer should measure 1G. Use that to correct accelerometer gain */
 425         if (!ARMING_FLAG(ARMED) && !gravityCalibrationComplete()) {
 426             zeroCalibrationAddValueS(&posEstimator.imu.gravityCalibration, posEstimator.imu.accelNEU.z);
 427
 428             if (gravityCalibrationComplete()) {
 429                 zeroCalibrationGetZeroS(&posEstimator.imu.gravityCalibration, &posEstimator.imu.calibratedGravityCMSS);
 430                 LOG_D(POS_ESTIMATOR, "Gravity calibration complete (%d)", (int)lrintf(posEstimator.imu.calibratedGravityCMSS));
 431             }
 432         }
 433
 434         /* If calibration is incomplete - report zero acceleration */
 435         if (gravityCalibrationComplete()) {
 436             posEstimator.imu.accelNEU.z -= posEstimator.imu.calibratedGravityCMSS;
 437         }
 438         else {
 439             posEstimator.imu.accelNEU.x = 0;
 440             posEstimator.imu.accelNEU.y = 0;
 441             posEstimator.imu.accelNEU.z = 0;
 442         }
 443
 444         /* Update blackbox values */
 445         navAccNEU[X] = posEstimator.imu.accelNEU.x;
 446         navAccNEU[Y] = posEstimator.imu.accelNEU.y;
 447         navAccNEU[Z] = posEstimator.imu.accelNEU.z;
 448     }
 449 }
 450
 451 float updateEPE(const float oldEPE, const float dt, const float newEPE, const float w)
 452 {
 453     return oldEPE + (newEPE - oldEPE) * w * dt;
 454 }
 455
 456 static bool navIsAccelerationUsable(void)
 457 {
 458     return true;
 459 }
 460
 461 static bool navIsHeadingUsable(void)
 462 {
 463     if (sensors(SENSOR_GPS)) {
 464         // If we have GPS - we need true IMU north (valid heading)
 465         return isImuHeadingValid();
 466     }
 467     else {
 468         // If we don't have GPS - we may use whatever we have, other sensors are operating in body frame
 469         return isImuHeadingValid() || positionEstimationConfig()->allow_dead_reckoning;
 470     }
 471 }
 472
 473 static uint32_t calculateCurrentValidityFlags(timeUs_t currentTimeUs)
 474 {
 475     /* Figure out if we have valid position data from our data sources */
 476     uint32_t newFlags = 0;
 477
 478     if (sensors(SENSOR_GPS) && posControl.gpsOrigin.valid &&
 479         ((currentTimeUs - posEstimator.gps.lastUpdateTime) <= MS2US(INAV_GPS_TIMEOUT_MS)) &&
 480         (posEstimator.gps.eph < positionEstimationConfig()->max_eph_epv)) {
 481         if (posEstimator.gps.epv < positionEstimationConfig()->max_eph_epv) {
 482             newFlags |= EST_GPS_XY_VALID | EST_GPS_Z_VALID;
 483         }
 484         else {
 485             newFlags |= EST_GPS_XY_VALID;
 486         }
 487     }
 488
 489     if (sensors(SENSOR_BARO) && ((currentTimeUs - posEstimator.baro.lastUpdateTime) <= MS2US(INAV_BARO_TIMEOUT_MS))) {
 490         newFlags |= EST_BARO_VALID;
 491     }
 492
 493     if (sensors(SENSOR_RANGEFINDER) && ((currentTimeUs - posEstimator.surface.lastUpdateTime) <= MS2US(INAV_SURFACE_TIMEOUT_MS))) {
 494         newFlags |= EST_SURFACE_VALID;
 495     }
 496
 497     if (sensors(SENSOR_OPFLOW) && posEstimator.flow.isValid && ((currentTimeUs - posEstimator.flow.lastUpdateTime) <= MS2US(INAV_FLOW_TIMEOUT_MS))) {
 498         newFlags |= EST_FLOW_VALID;
 499     }
 500
 501     if (posEstimator.est.eph < positionEstimationConfig()->max_eph_epv) {
 502         newFlags |= EST_XY_VALID;
 503     }
 504
 505     if (posEstimator.est.epv < positionEstimationConfig()->max_eph_epv) {
 506         newFlags |= EST_Z_VALID;
 507     }
 508
 509     return newFlags;
 510 }
 511
 512 static void estimationPredict(estimationContext_t * ctx)
 513 {
 514     const float accWeight = navGetAccelerometerWeight();
 515
 516     /* Prediction step: Z-axis */
 517     if ((ctx->newFlags & EST_Z_VALID)) {
 518         posEstimator.est.pos.z += posEstimator.est.vel.z * ctx->dt;
 519         posEstimator.est.pos.z += posEstimator.imu.accelNEU.z * sq(ctx->dt) / 2.0f * accWeight;
 520         posEstimator.est.vel.z += posEstimator.imu.accelNEU.z * ctx->dt * sq(accWeight);
 521     }
 522
 523     /* Prediction step: XY-axis */
 524     if ((ctx->newFlags & EST_XY_VALID)) {
 525         // Predict based on known velocity
 526         posEstimator.est.pos.x += posEstimator.est.vel.x * ctx->dt;
 527         posEstimator.est.pos.y += posEstimator.est.vel.y * ctx->dt;
 528
 529         // If heading is valid, accelNEU is valid as well. Account for acceleration
 530         if (navIsHeadingUsable() && navIsAccelerationUsable()) {
 531             posEstimator.est.pos.x += posEstimator.imu.accelNEU.x * sq(ctx->dt) / 2.0f * accWeight;
 532             posEstimator.est.pos.y += posEstimator.imu.accelNEU.y * sq(ctx->dt) / 2.0f * accWeight;
 533             posEstimator.est.vel.x += posEstimator.imu.accelNEU.x * ctx->dt * sq(accWeight);
 534             posEstimator.est.vel.y += posEstimator.imu.accelNEU.y * ctx->dt * sq(accWeight);
 535         }
 536     }
 537 }
 538
 539 static bool estimationCalculateCorrection_Z(estimationContext_t * ctx)
 540 {
 541     if (ctx->newFlags & EST_BARO_VALID) {
 542         timeUs_t currentTimeUs = micros();
 543
 544         if (!ARMING_FLAG(ARMED)) {
 545             posEstimator.state.baroGroundAlt = posEstimator.est.pos.z;
 546             posEstimator.state.isBaroGroundValid = true;
 547             posEstimator.state.baroGroundTimeout = currentTimeUs + 250000;   // 0.25 sec
 548         }
 549         else {
 550             if (posEstimator.est.vel.z > 15) {
 551                 if (currentTimeUs > posEstimator.state.baroGroundTimeout) {
 552                     posEstimator.state.isBaroGroundValid = false;
 553                 }
 554             }
 555             else {
 556                 posEstimator.state.baroGroundTimeout = currentTimeUs + 250000;   // 0.25 sec
 557             }
 558         }
 559
 560         // We might be experiencing air cushion effect - use sonar or baro groung altitude to detect it
 561         bool isAirCushionEffectDetected = ARMING_FLAG(ARMED) &&
 562                                             (((ctx->newFlags & EST_SURFACE_VALID) && posEstimator.surface.alt < 20.0f && posEstimator.state.isBaroGroundValid) ||
 563                                              ((ctx->newFlags & EST_BARO_VALID) && posEstimator.state.isBaroGroundValid && posEstimator.baro.alt < posEstimator.state.baroGroundAlt));
 564
 565         // Altitude
 566         const float baroAltResidual = (isAirCushionEffectDetected ? posEstimator.state.baroGroundAlt : posEstimator.baro.alt) - posEstimator.est.pos.z;
 567         ctx->estPosCorr.z += baroAltResidual * positionEstimationConfig()->w_z_baro_p * ctx->dt;
 568         ctx->estVelCorr.z += baroAltResidual * sq(positionEstimationConfig()->w_z_baro_p) * ctx->dt;
 569
 570         // If GPS is available - also use GPS climb rate
 571         if (ctx->newFlags & EST_GPS_Z_VALID) {
 572             // Trust GPS velocity only if residual/error is less than 2.5 m/s, scale weight according to gaussian distribution
 573             const float gpsRocResidual = posEstimator.gps.vel.z - posEstimator.est.vel.z;
 574             const float gpsRocScaler = bellCurve(gpsRocResidual, 250.0f);
 575             ctx->estVelCorr.z += gpsRocResidual * positionEstimationConfig()->w_z_gps_v * gpsRocScaler * ctx->dt;
 576         }
 577
 578         ctx->newEPV = updateEPE(posEstimator.est.epv, ctx->dt, posEstimator.baro.epv, positionEstimationConfig()->w_z_baro_p);
 579
 580         // Accelerometer bias
 581         if (!isAirCushionEffectDetected) {
 582             ctx->accBiasCorr.z -= baroAltResidual * sq(positionEstimationConfig()->w_z_baro_p);
 583         }
 584
 585         return true;
 586     }
 587     else if ((STATE(FIXED_WING_LEGACY) || positionEstimationConfig()->use_gps_no_baro) && (ctx->newFlags & EST_GPS_Z_VALID)) {
 588         // If baro is not available - use GPS Z for correction on a plane
 589         // Reset current estimate to GPS altitude if estimate not valid
 590         if (!(ctx->newFlags & EST_Z_VALID)) {
 591             ctx->estPosCorr.z += posEstimator.gps.pos.z - posEstimator.est.pos.z;
 592             ctx->estVelCorr.z += posEstimator.gps.vel.z - posEstimator.est.vel.z;
 593             ctx->newEPV = posEstimator.gps.epv;
 594         }
 595         else {
 596             // Altitude
 597             const float gpsAltResudual = posEstimator.gps.pos.z - posEstimator.est.pos.z;
 598
 599             ctx->estPosCorr.z += gpsAltResudual * positionEstimationConfig()->w_z_gps_p * ctx->dt;
 600             ctx->estVelCorr.z += gpsAltResudual * sq(positionEstimationConfig()->w_z_gps_p) * ctx->dt;
 601             ctx->estVelCorr.z += (posEstimator.gps.vel.z - posEstimator.est.vel.z) * positionEstimationConfig()->w_z_gps_v * ctx->dt;
 602             ctx->newEPV = updateEPE(posEstimator.est.epv, ctx->dt, MAX(posEstimator.gps.epv, gpsAltResudual), positionEstimationConfig()->w_z_gps_p);
 603
 604             // Accelerometer bias
 605             ctx->accBiasCorr.z -= gpsAltResudual * sq(positionEstimationConfig()->w_z_gps_p);
 606         }
 607
 608         return true;
 609     }
 610
 611     DEBUG_SET(DEBUG_ALTITUDE, 0, posEstimator.est.pos.z);       // Position estimate
 612     DEBUG_SET(DEBUG_ALTITUDE, 2, posEstimator.baro.alt);        // Baro altitude
 613     DEBUG_SET(DEBUG_ALTITUDE, 4, posEstimator.gps.pos.z);       // GPS altitude
 614     DEBUG_SET(DEBUG_ALTITUDE, 6, accGetVibrationLevel());       // Vibration level
 615     DEBUG_SET(DEBUG_ALTITUDE, 1, posEstimator.est.vel.z);       // Vertical speed estimate
 616     DEBUG_SET(DEBUG_ALTITUDE, 3, posEstimator.imu.accelNEU.z);  // Vertical acceleration on earth frame
 617     DEBUG_SET(DEBUG_ALTITUDE, 5, posEstimator.gps.vel.z);       // GPS vertical speed
 618     DEBUG_SET(DEBUG_ALTITUDE, 7, accGetClipCount());            // Clip count
 619
 620     return false;
 621 }
 622
 623 static bool estimationCalculateCorrection_XY_GPS(estimationContext_t * ctx)
 624 {
 625     if (ctx->newFlags & EST_GPS_XY_VALID) {
 626         /* If GPS is valid and our estimate is NOT valid - reset it to GPS coordinates and velocity */
 627         if (!(ctx->newFlags & EST_XY_VALID)) {
 628             ctx->estPosCorr.x += posEstimator.gps.pos.x - posEstimator.est.pos.x;
 629             ctx->estPosCorr.y += posEstimator.gps.pos.y - posEstimator.est.pos.y;
 630             ctx->estVelCorr.x += posEstimator.gps.vel.x - posEstimator.est.vel.x;
 631             ctx->estVelCorr.y += posEstimator.gps.vel.y - posEstimator.est.vel.y;
 632             ctx->newEPH = posEstimator.gps.eph;
 633         }
 634         else {
 635             const float gpsPosXResidual = posEstimator.gps.pos.x - posEstimator.est.pos.x;
 636             const float gpsPosYResidual = posEstimator.gps.pos.y - posEstimator.est.pos.y;
 637             const float gpsVelXResidual = posEstimator.gps.vel.x - posEstimator.est.vel.x;
 638             const float gpsVelYResidual = posEstimator.gps.vel.y - posEstimator.est.vel.y;
 639             const float gpsPosResidualMag = fast_fsqrtf(sq(gpsPosXResidual) + sq(gpsPosYResidual));
 640
 641             //const float gpsWeightScaler = scaleRangef(bellCurve(gpsPosResidualMag, INAV_GPS_ACCEPTANCE_EPE), 0.0f, 1.0f, 0.1f, 1.0f);
 642             const float gpsWeightScaler = 1.0f;
 643
 644             const float w_xy_gps_p = positionEstimationConfig()->w_xy_gps_p * gpsWeightScaler;
 645             const float w_xy_gps_v = positionEstimationConfig()->w_xy_gps_v * sq(gpsWeightScaler);
 646
 647             // Coordinates
 648             ctx->estPosCorr.x += gpsPosXResidual * w_xy_gps_p * ctx->dt;
 649             ctx->estPosCorr.y += gpsPosYResidual * w_xy_gps_p * ctx->dt;
 650
 651             // Velocity from coordinates
 652             ctx->estVelCorr.x += gpsPosXResidual * sq(w_xy_gps_p) * ctx->dt;
 653             ctx->estVelCorr.y += gpsPosYResidual * sq(w_xy_gps_p) * ctx->dt;
 654
 655             // Velocity from direct measurement
 656             ctx->estVelCorr.x += gpsVelXResidual * w_xy_gps_v * ctx->dt;
 657             ctx->estVelCorr.y += gpsVelYResidual * w_xy_gps_v * ctx->dt;
 658
 659             // Accelerometer bias
 660             ctx->accBiasCorr.x -= gpsPosXResidual * sq(w_xy_gps_p);
 661             ctx->accBiasCorr.y -= gpsPosYResidual * sq(w_xy_gps_p);
 662
 663             /* Adjust EPH */
 664             ctx->newEPH = updateEPE(posEstimator.est.eph, ctx->dt, MAX(posEstimator.gps.eph, gpsPosResidualMag), w_xy_gps_p);
 665         }
 666
 667         return true;
 668     }
 669
 670     return false;
 671 }
 672
 673 /**
 674  * Calculate next estimate using IMU and apply corrections from reference sensors (GPS, BARO etc)
 675  *  Function is called at main loop rate
 676  */
 677 static void updateEstimatedTopic(timeUs_t currentTimeUs)
 678 {
 679     estimationContext_t ctx;
 680
 681     /* Calculate dT */
 682     ctx.dt = US2S(currentTimeUs - posEstimator.est.lastUpdateTime);
 683     posEstimator.est.lastUpdateTime = currentTimeUs;
 684
 685     /* If IMU is not ready we can't estimate anything */
 686     if (!isImuReady()) {
 687         posEstimator.est.eph = positionEstimationConfig()->max_eph_epv + 0.001f;
 688         posEstimator.est.epv = positionEstimationConfig()->max_eph_epv + 0.001f;
 689         posEstimator.flags = 0;
 690         return;
 691     }
 692
 693     /* Calculate new EPH and EPV for the case we didn't update postion */
 694     ctx.newEPH = posEstimator.est.eph * ((posEstimator.est.eph <= positionEstimationConfig()->max_eph_epv) ? 1.0f + ctx.dt : 1.0f);
 695     ctx.newEPV = posEstimator.est.epv * ((posEstimator.est.epv <= positionEstimationConfig()->max_eph_epv) ? 1.0f + ctx.dt : 1.0f);
 696     ctx.newFlags = calculateCurrentValidityFlags(currentTimeUs);
 697     vectorZero(&ctx.estPosCorr);
 698     vectorZero(&ctx.estVelCorr);
 699     vectorZero(&ctx.accBiasCorr);
 700
 701     /* AGL estimation - separate process, decouples from Z coordinate */
 702     estimationCalculateAGL(&ctx);
 703
 704     /* Prediction stage: X,Y,Z */
 705     estimationPredict(&ctx);
 706
 707     /* Correction stage: Z */
 708     const bool estZCorrectOk =
 709         estimationCalculateCorrection_Z(&ctx);
 710
 711     /* Correction stage: XY: GPS, FLOW */
 712     // FIXME: Handle transition from FLOW to GPS and back - seamlessly fly indoor/outdoor
 713     const bool estXYCorrectOk =
 714         estimationCalculateCorrection_XY_GPS(&ctx) ||
 715         estimationCalculateCorrection_XY_FLOW(&ctx);
 716
 717     // If we can't apply correction or accuracy is off the charts - decay velocity to zero
 718     if (!estXYCorrectOk || ctx.newEPH > positionEstimationConfig()->max_eph_epv) {
 719         ctx.estVelCorr.x = (0.0f - posEstimator.est.vel.x) * positionEstimationConfig()->w_xy_res_v * ctx.dt;
 720         ctx.estVelCorr.y = (0.0f - posEstimator.est.vel.y) * positionEstimationConfig()->w_xy_res_v * ctx.dt;
 721     }
 722
 723     if (!estZCorrectOk || ctx.newEPV > positionEstimationConfig()->max_eph_epv) {
 724         ctx.estVelCorr.z = (0.0f - posEstimator.est.vel.z) * positionEstimationConfig()->w_z_res_v * ctx.dt;
 725     }
 726
 727     // Apply corrections
 728     vectorAdd(&posEstimator.est.pos, &posEstimator.est.pos, &ctx.estPosCorr);
 729     vectorAdd(&posEstimator.est.vel, &posEstimator.est.vel, &ctx.estVelCorr);
 730
 731     /* Correct accelerometer bias */
 732     if (positionEstimationConfig()->w_acc_bias > 0.0f) {
 733         const float accelBiasCorrMagnitudeSq = sq(ctx.accBiasCorr.x) + sq(ctx.accBiasCorr.y) + sq(ctx.accBiasCorr.z);
 734         if (accelBiasCorrMagnitudeSq < sq(INAV_ACC_BIAS_ACCEPTANCE_VALUE)) {
 735             /* transform error vector from NEU frame to body frame */
 736             imuTransformVectorEarthToBody(&ctx.accBiasCorr);
 737
 738             /* Correct accel bias */
 739             posEstimator.imu.accelBias.x += ctx.accBiasCorr.x * positionEstimationConfig()->w_acc_bias * ctx.dt;
 740             posEstimator.imu.accelBias.y += ctx.accBiasCorr.y * positionEstimationConfig()->w_acc_bias * ctx.dt;
 741             posEstimator.imu.accelBias.z += ctx.accBiasCorr.z * positionEstimationConfig()->w_acc_bias * ctx.dt;
 742         }
 743     }
 744
 745     /* Update uncertainty */
 746     posEstimator.est.eph = ctx.newEPH;
 747     posEstimator.est.epv = ctx.newEPV;
 748
 749     // Keep flags for further usage
 750     posEstimator.flags = ctx.newFlags;
 751 }
 752
 753 /**
 754  * Examine estimation error and update navigation system if estimate is good enough
 755  *  Function is called at main loop rate, but updates happen less frequently - at a fixed rate
 756  */
 757 static void publishEstimatedTopic(timeUs_t currentTimeUs)
 758 {
 759     static navigationTimer_t posPublishTimer;
 760
 761     /* IMU operates in decidegrees while INAV operates in deg*100 */
 762     updateActualHeading(navIsHeadingUsable(), DECIDEGREES_TO_CENTIDEGREES(attitude.values.yaw));
 763
 764     /* Position and velocity are published with INAV_POSITION_PUBLISH_RATE_HZ */
 765     if (updateTimer(&posPublishTimer, HZ2US(INAV_POSITION_PUBLISH_RATE_HZ), currentTimeUs)) {
 766         /* Publish position update */
 767         if (posEstimator.est.eph < positionEstimationConfig()->max_eph_epv) {
 768             // FIXME!!!!!
 769             updateActualHorizontalPositionAndVelocity(true, true, posEstimator.est.pos.x, posEstimator.est.pos.y, posEstimator.est.vel.x, posEstimator.est.vel.y);
 770         }
 771         else {
 772             updateActualHorizontalPositionAndVelocity(false, false, posEstimator.est.pos.x, posEstimator.est.pos.y, 0, 0);
 773         }
 774
 775         /* Publish altitude update and set altitude validity */
 776         if (posEstimator.est.epv < positionEstimationConfig()->max_eph_epv) {
 777             navigationEstimateStatus_e aglStatus = (posEstimator.est.aglQual == SURFACE_QUAL_LOW) ? EST_USABLE : EST_TRUSTED;
 778             updateActualAltitudeAndClimbRate(true, posEstimator.est.pos.z, posEstimator.est.vel.z, posEstimator.est.aglAlt, posEstimator.est.aglVel, aglStatus);
 779         }
 780         else {
 781             updateActualAltitudeAndClimbRate(false, posEstimator.est.pos.z, 0, posEstimator.est.aglAlt, 0, EST_NONE);
 782         }
 783
 784         //Update Blackbox states
 785         navEPH = posEstimator.est.eph;
 786         navEPV = posEstimator.est.epv;
 787     }
 788 }
 789
 790 #if defined(NAV_GPS_GLITCH_DETECTION)
 791 bool isGPSGlitchDetected(void)
 792 {
 793     return posEstimator.gps.glitchDetected;
 794 }
 795 #endif
 796
 797 /**
 798  * Initialize position estimator
 799  *  Should be called once before any update occurs
 800  */
 801 void initializePositionEstimator(void)
 802 {
 803     int axis;
 804
 805     posEstimator.est.eph = positionEstimationConfig()->max_eph_epv + 0.001f;
 806     posEstimator.est.epv = positionEstimationConfig()->max_eph_epv + 0.001f;
 807
 808     posEstimator.imu.lastUpdateTime = 0;
 809     posEstimator.gps.lastUpdateTime = 0;
 810     posEstimator.baro.lastUpdateTime = 0;
 811     posEstimator.surface.lastUpdateTime = 0;
 812
 813     posEstimator.est.aglAlt = 0;
 814     posEstimator.est.aglVel = 0;
 815
 816     posEstimator.est.flowCoordinates[X] = 0;
 817     posEstimator.est.flowCoordinates[Y] = 0;
 818
 819     posEstimator.imu.accWeightFactor = 0;
 820
 821     restartGravityCalibration();
 822
 823     for (axis = 0; axis < 3; axis++) {
 824         posEstimator.imu.accelBias.v[axis] = 0;
 825         posEstimator.est.pos.v[axis] = 0;
 826         posEstimator.est.vel.v[axis] = 0;
 827     }
 828
 829     pt1FilterInit(&posEstimator.baro.avgFilter, INAV_BARO_AVERAGE_HZ, 0.0f);
 830     pt1FilterInit(&posEstimator.surface.avgFilter, INAV_SURFACE_AVERAGE_HZ, 0.0f);
 831 }
 832
 833 /**
 834  * Update estimator
 835  *  Update rate: loop rate (>100Hz)
 836  */
 837 void updatePositionEstimator(void)
 838 {
 839     static bool isInitialized = false;
 840
 841     if (!isInitialized) {
 842         initializePositionEstimator();
 843         isInitialized = true;
 844     }
 845
 846     const timeUs_t currentTimeUs = micros();
 847
 848     /* Read updates from IMU, preprocess */
 849     updateIMUTopic(currentTimeUs);
 850
 851     /* Update estimate */
 852     updateEstimatedTopic(currentTimeUs);
 853
 854     /* Publish estimate */
 855     publishEstimatedTopic(currentTimeUs);
 856 }
 857
 858 bool navIsCalibrationComplete(void)
 859 {
 860     return gravityCalibrationComplete();
 861 }