Accelerometer health improvements

src/main/fc/settings.yaml

@@ -1196,6 +1196,9 @@ groups:
     type: positionEstimationConfig_t
     condition: USE_NAV
     members:
+      - name: inav_avoid_acc_clipping
+        field: avoid_accel_clipping
+        type: bool
       - name: inav_auto_mag_decl
         field: automatic_mag_declination
         type: bool

src/main/io/osd.c

@@ -2147,7 +2147,18 @@ static bool osdDrawSingleElement(uint8_t item)
                 // messages to show.
                 const char *messages[5];
                 unsigned messageCount = 0;
-                if (FLIGHT_MODE(FAILSAFE_MODE)) {
+                if (isUsingNavigationModes() || failsafeMayRequireNavigationMode()) {
+                    // If we configure any navigation modes - show navigation-related warnings
+                    uint32_t navWarn = navGetPositionEstimatorWarnings();
+
+                    if (navWarn & NAV_WARNING_ACCELEROMETER_HEALTH) {
+                        messages[messageCount++] = "(ACCEL UNHEALTHY)";
+                    }
+
+                    if (navWarn & NAV_WARNING_HEADING_UNKNOWN) {
+                        messages[messageCount++] = "(HEADING UNHEALTHY)";
+                    }
+                } else if (FLIGHT_MODE(FAILSAFE_MODE)) {
                     // In FS mode while being armed too
                     const char *failsafePhaseMessage = osdFailsafePhaseMessage();
                     const char *failsafeInfoMessage = osdFailsafeInfoMessage();

src/main/navigation/navigation.h

@@ -110,6 +110,7 @@ typedef struct positionEstimationConfig_s {
     uint8_t gravity_calibration_tolerance;    // Tolerance of gravity calibration (cm/s/s)
     uint8_t use_gps_velned;
     uint8_t allow_dead_reckoning;
+    uint8_t avoid_accel_clipping;
 
     uint16_t max_surface_altitude;
 
@@ -488,6 +489,14 @@ bool isAdjustingHeading(void);
  */
 int32_t navigationGetHomeHeading(void);
 
+/* Notify navigation system that accelerometer is not healthy (navigation modes may misbehave) */
+uint32_t navGetPositionEstimatorWarnings(void);
+
+typedef enum {
+    NAV_WARNING_ACCELEROMETER_HEALTH    = (1 << 0),
+    NAV_WARNING_HEADING_UNKNOWN         = (1 << 1),
+} navGetPositionEstimatorWarnings_e;
+
 /* Compatibility data */
 extern navSystemStatus_t    NAV_Status;
 

src/main/navigation/navigation_pos_estimator.c

@@ -54,7 +54,7 @@
 
 navigationPosEstimator_t posEstimator;
 
-PG_REGISTER_WITH_RESET_TEMPLATE(positionEstimationConfig_t, positionEstimationConfig, PG_POSITION_ESTIMATION_CONFIG, 4);
+PG_REGISTER_WITH_RESET_TEMPLATE(positionEstimationConfig_t, positionEstimationConfig, PG_POSITION_ESTIMATION_CONFIG, 5);
 
 PG_RESET_TEMPLATE(positionEstimationConfig_t, positionEstimationConfig,
         // Inertial position estimator parameters
@@ -64,6 +64,7 @@ PG_RESET_TEMPLATE(positionEstimationConfig_t, positionEstimationConfig,
         .gravity_calibration_tolerance = 5,     // 5 cm/s/s calibration error accepted (0.5% of gravity)
         .use_gps_velned = 1,         // "Disabled" is mandatory with gps_dyn_model = Pedestrian
         .allow_dead_reckoning = 0,
+        .avoid_accel_clipping = 1,
 
         .max_surface_altitude = 200,
 
@@ -362,6 +363,7 @@ static void updateIMUEstimationWeight(const float dt)
     // and gradually restore weight back to 1.0 over time
     if (isAccClipped) {
         posEstimator.imu.accWeightFactor = 0.0f;
+        posEstimator.imu.lastClippingTimeMs = millis();
     }
     else {
         const float relAlpha = dt / (dt + INAV_ACC_CLIPPING_RC_CONSTANT);
@@ -374,10 +376,14 @@ static void updateIMUEstimationWeight(const float dt)
 
 float navGetAccelerometerWeight(void)
 {
-    const float accWeightScaled = posEstimator.imu.accWeightFactor * positionEstimationConfig()->w_xyz_acc_p;
-    DEBUG_SET(DEBUG_VIBE, 5, accWeightScaled * 1000);
-
-    return accWeightScaled;
+    if (positionEstimationConfig()->avoid_accel_clipping) {
+        const float accWeightScaled = posEstimator.imu.accWeightFactor * positionEstimationConfig()->w_xyz_acc_p;
+        DEBUG_SET(DEBUG_VIBE, 5, accWeightScaled * 1000);
+        return accWeightScaled;
+    }
+    else {
+        return positionEstimationConfig()->w_xyz_acc_p;
+    }
 }
 
 static void updateIMUTopic(timeUs_t currentTimeUs)
@@ -806,6 +812,7 @@ void initializePositionEstimator(void)
     posEstimator.est.flowCoordinates[Y] = 0;
 
     posEstimator.imu.accWeightFactor = 0;
+    posEstimator.imu.lastClippingTimeMs = 0;
 
     restartGravityCalibration();
 
@@ -849,4 +856,19 @@ bool navIsCalibrationComplete(void)
     return gravityCalibrationComplete();
 }
 
+uint32_t navGetPositionEstimatorWarnings(void)
+{
+    uint32_t warningBits = 0;
+
+    if (!navIsAccelerationUsable() || (millis() - posEstimator.imu.lastClippingTimeMs < INAV_ACC_CLIPPING_WARNING_TIMEOUT_MS)) {
+        warningBits |= NAV_WARNING_ACCELEROMETER_HEALTH;
+    }
+
+    if (!navIsHeadingUsable()) {
+        warningBits |= NAV_WARNING_HEADING_UNKNOWN;
+    }
+
+    return warningBits;
+}
+
 #endif

src/main/navigation/navigation_pos_estimator_private.h

@@ -52,6 +52,7 @@
 #define INAV_SURFACE_AVERAGE_HZ             1.0f
 
 #define INAV_ACC_CLIPPING_RC_CONSTANT           (0.010f)    // Reduce acc weight for ~10ms after clipping
+#define INAV_ACC_CLIPPING_WARNING_TIMEOUT_MS    500
 
 #define RANGEFINDER_RELIABILITY_RC_CONSTANT     (0.47802f)
 #define RANGEFINDER_RELIABILITY_LIGHT_THRESHOLD (0.15f)
@@ -128,7 +129,8 @@ typedef struct {
 } navPositionEstimatorESTIMATE_t;
 
 typedef struct {
-     timeUs_t               lastUpdateTime;
+    timeUs_t                lastUpdateTime;
+    timeMs_t                lastClippingTimeMs;
     fpVector3_t             accelNEU;
     fpVector3_t             accelBias;
     float                   calibratedGravityCMSS;