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