Merge pull request iNavFlight#10925 from breadoven/abo_posest_air_cushion_fix

Nav position estimator baro air cushion compensation fix

Author: breadoven

src/main/navigation/navigation_multicopter.c

@@ -834,14 +834,12 @@ bool isMulticopterLandingDetected(void)
     }
 #endif
 
-    bool throttleIsBelowMidHover = rcCommand[THROTTLE] < (0.5 * (currentBatteryProfile->nav.mc.hover_throttle + getThrottleIdleValue()));
-
     /* Basic condition to start looking for landing
      * Detection active during Failsafe only if throttle below mid hover throttle
      * and WP mission not active (except landing states).
      * Also active in non autonomous flight modes but only when thottle low */
     bool startCondition = (navGetCurrentStateFlags() & (NAV_CTL_LAND | NAV_CTL_EMERG))
-                          || (FLIGHT_MODE(FAILSAFE_MODE) && !FLIGHT_MODE(NAV_WP_MODE) && throttleIsBelowMidHover)
+                          || (FLIGHT_MODE(FAILSAFE_MODE) && !FLIGHT_MODE(NAV_WP_MODE) && !isMulticopterThrottleAboveMidHover())
                           || (!navigationIsFlyingAutonomousMode() && throttleStickIsLow());
 
     static timeMs_t landingDetectorStartedAt;
@@ -923,6 +921,11 @@ bool isMulticopterLandingDetected(void)
     }
 }
 
+bool isMulticopterThrottleAboveMidHover(void)
+{
+    return rcCommand[THROTTLE] > 0.5 * (currentBatteryProfile->nav.mc.hover_throttle + getThrottleIdleValue());
+}
+
 /*-----------------------------------------------------------
  * Multicopter emergency landing
  *-----------------------------------------------------------*/

src/main/navigation/navigation_pos_estimator.c

@@ -583,29 +583,34 @@ static bool estimationCalculateCorrection_Z(estimationContext_t * ctx)
     }
 
     if (ctx->newFlags & EST_BARO_VALID && wBaro) {
-        timeUs_t currentTimeUs = micros();
+        bool isAirCushionEffectDetected = false;
+        static float baroGroundAlt = 0.0f;
 
-        if (!ARMING_FLAG(ARMED)) {
-            posEstimator.state.baroGroundAlt = posEstimator.est.pos.z;
-            posEstimator.state.isBaroGroundValid = true;
-            posEstimator.state.baroGroundTimeout = currentTimeUs + 250000;   // 0.25 sec
-        }
-        else {
-            if (posEstimator.est.vel.z > 15) {
-                posEstimator.state.isBaroGroundValid = currentTimeUs > posEstimator.state.baroGroundTimeout ? false: true;
+        if (STATE(MULTIROTOR)) {
+            static bool isBaroGroundValid = false;
+
+            if (!ARMING_FLAG(ARMED)) {
+                baroGroundAlt = posEstimator.baro.alt;
+                isBaroGroundValid = true;
             }
-            else {
-                posEstimator.state.baroGroundTimeout = currentTimeUs + 250000;   // 0.25 sec
+            else if (isBaroGroundValid) {
+                // We might be experiencing air cushion effect during takeoff - use sonar or baro ground altitude to detect it
+                if (isMulticopterThrottleAboveMidHover()) {
+                    // Disable ground effect detection at lift off when est alt and baro alt converge. Always disable if baro alt > 1m.
+                    isBaroGroundValid = fabsf(posEstimator.est.pos.z - posEstimator.baro.alt) > 20.0f && posEstimator.baro.alt < 100.0f;
+                }
+
+                isAirCushionEffectDetected = (isEstimatedAglTrusted() && posEstimator.surface.alt < 20.0f) || posEstimator.baro.alt < baroGroundAlt + 20.0f;
             }
         }
 
-        // We might be experiencing air cushion effect during takeoff - use sonar or baro ground altitude to detect it
-        bool isAirCushionEffectDetected = ARMING_FLAG(ARMED) &&
-                                            (((ctx->newFlags & EST_SURFACE_VALID) && posEstimator.surface.alt < 20.0f && posEstimator.state.isBaroGroundValid) ||
-                                             ((ctx->newFlags & EST_BARO_VALID) && posEstimator.state.isBaroGroundValid && posEstimator.baro.alt < posEstimator.state.baroGroundAlt));
-
         // Altitude
-        const float baroAltResidual = wBaro * ((isAirCushionEffectDetected ? posEstimator.state.baroGroundAlt : posEstimator.baro.alt) - posEstimator.est.pos.z);
+        float baroAltResidual = wBaro * ((isAirCushionEffectDetected ? baroGroundAlt : posEstimator.baro.alt) - posEstimator.est.pos.z);
+
+        // Disable alt pos correction at point of lift off if ground effect active
+        if (isAirCushionEffectDetected && isMulticopterThrottleAboveMidHover()) {
+            baroAltResidual = 0.0f;
+        }
         const float baroVelZResidual = isAirCushionEffectDetected ? 0.0f : wBaro * (posEstimator.baro.baroAltRate - posEstimator.est.vel.z);
         const float w_z_baro_p = positionEstimationConfig()->w_z_baro_p;
 

src/main/navigation/navigation_pos_estimator_private.h

@@ -153,13 +153,6 @@ typedef enum {
     ALTITUDE_SOURCE_BARO_ONLY,
 } navDefaultAltitudeSensor_e;
 
-typedef struct {
-    timeUs_t    baroGroundTimeout;
-    float       baroGroundAlt;
-    bool        isBaroGroundValid;
-} navPositionEstimatorSTATE_t;
-
-
 typedef struct {
     uint32_t                    flags;
 
@@ -175,9 +168,6 @@ typedef struct {
 
     // Estimate
     navPositionEstimatorESTIMATE_t  est;
-
-    // Extra state variables
-    navPositionEstimatorSTATE_t state;
 } navigationPosEstimator_t;
 
 typedef struct {

src/main/navigation/navigation_private.h

@@ -582,6 +582,8 @@ bool isMulticopterLandingDetected(void);
 void calculateMulticopterInitialHoldPosition(fpVector3_t * pos);
 float getSqrtControllerVelocity(float targetAltitude, timeDelta_t deltaMicros);
 
+bool isMulticopterThrottleAboveMidHover(void);
+
 /* Fixed-wing specific functions */
 void setupFixedWingAltitudeController(void);