Merge pull request #4681 from iNavFlight/de_yg_accel_weight_scaling

Ignore acceleration in estimated position when clipping or high vibration

Author: digitalentity

src/main/flight/imu.c

@@ -580,6 +580,7 @@ void imuCheckVibrationLevels(void)
     DEBUG_SET(DEBUG_VIBE, 1, accVibeLevels.y * 100);
     DEBUG_SET(DEBUG_VIBE, 2, accVibeLevels.z * 100);
     DEBUG_SET(DEBUG_VIBE, 3, accClipCount);
+    // DEBUG_VIBE values 4-7 are used by NAV estimator
 }
 
 void FAST_CODE NOINLINE imuUpdateAttitude(timeUs_t currentTimeUs)

src/main/navigation/navigation_pos_estimator.c

@@ -25,6 +25,7 @@
 #if defined(USE_NAV)
 
 #include "build/build_config.h"
+#include "build/debug.h"
 
 #include "common/axis.h"
 #include "common/log.h"
@@ -354,8 +355,37 @@ static bool gravityCalibrationComplete(void)
     return zeroCalibrationIsCompleteS(&posEstimator.imu.gravityCalibration);
 }
 
-static void updateIMUTopic(void)
+static void updateIMUEstimationWeight(const float dt)
 {
+    bool isAccClipped = accIsClipped();
+
+    // If accelerometer measurement is clipped - drop the acc weight to zero
+    // and gradually restore weight back to 1.0 over time
+    if (isAccClipped) {
+        posEstimator.imu.accWeightFactor = 0.0f;
+    }
+    else {
+        const float relAlpha = dt / (dt + INAV_ACC_CLIPPING_RC_CONSTANT);
+        posEstimator.imu.accWeightFactor = posEstimator.imu.accWeightFactor * (1.0f - relAlpha) + 1.0f * relAlpha;
+    }
+
+    // DEBUG_VIBE[0-3] are used in IMU
+    DEBUG_SET(DEBUG_VIBE, 4, posEstimator.imu.accWeightFactor * 1000);
+}
+
+float navGetAccelerometerWeight(void)
+{
+    const float accWeightScaled = posEstimator.imu.accWeightFactor * positionEstimationConfig()->w_xyz_acc_p;
+    DEBUG_SET(DEBUG_VIBE, 5, accWeightScaled * 1000);
+
+    return accWeightScaled;
+}
+
+static void updateIMUTopic(timeUs_t currentTimeUs)
+{
+    const float dt = US2S(currentTimeUs - posEstimator.imu.lastUpdateTime);
+    posEstimator.imu.lastUpdateTime = currentTimeUs;
+
     if (!isImuReady()) {
         posEstimator.imu.accelNEU.x = 0;
         posEstimator.imu.accelNEU.y = 0;
@@ -364,6 +394,9 @@ static void updateIMUTopic(void)
         restartGravityCalibration();
     }
     else {
+        /* Update acceleration weight based on vibration levels and clipping */
+        updateIMUEstimationWeight(dt);
+
         fpVector3_t accelBF;
 
         /* Read acceleration data in body frame */
@@ -435,12 +468,6 @@ static bool navIsHeadingUsable(void)
     }
 }
 
-float navGetAccelerometerWeight(void)
-{
-    // TODO(digitalentity): consider accelerometer health in weight calculation
-    return positionEstimationConfig()->w_xyz_acc_p;
-}
-
 static uint32_t calculateCurrentValidityFlags(timeUs_t currentTimeUs)
 {
     /* Figure out if we have valid position data from our data sources */
@@ -768,6 +795,7 @@ void initializePositionEstimator(void)
     posEstimator.est.eph = positionEstimationConfig()->max_eph_epv + 0.001f;
     posEstimator.est.epv = positionEstimationConfig()->max_eph_epv + 0.001f;
 
+    posEstimator.imu.lastUpdateTime = 0;
     posEstimator.gps.lastUpdateTime = 0;
     posEstimator.baro.lastUpdateTime = 0;
     posEstimator.surface.lastUpdateTime = 0;
@@ -778,6 +806,8 @@ void initializePositionEstimator(void)
     posEstimator.est.flowCoordinates[X] = 0;
     posEstimator.est.flowCoordinates[Y] = 0;
 
+    posEstimator.imu.accWeightFactor = 0;
+
     restartGravityCalibration();
 
     for (axis = 0; axis < 3; axis++) {
@@ -806,7 +836,7 @@ void FAST_CODE NOINLINE updatePositionEstimator(void)
     const timeUs_t currentTimeUs = micros();
 
     /* Read updates from IMU, preprocess */
-    updateIMUTopic();
+    updateIMUTopic(currentTimeUs);
 
     /* Update estimate */
     updateEstimatedTopic(currentTimeUs);

src/main/navigation/navigation_pos_estimator_private.h

@@ -51,6 +51,8 @@
 #define INAV_BARO_AVERAGE_HZ                1.0f
 #define INAV_SURFACE_AVERAGE_HZ             1.0f
 
+#define INAV_ACC_CLIPPING_RC_CONSTANT           (0.010f)    // Reduce acc weight for ~10ms after clipping
+
 #define RANGEFINDER_RELIABILITY_RC_CONSTANT     (0.47802f)
 #define RANGEFINDER_RELIABILITY_LIGHT_THRESHOLD (0.15f)
 #define RANGEFINDER_RELIABILITY_LOW_THRESHOLD   (0.33f)
@@ -126,9 +128,11 @@ typedef struct {
 } navPositionEstimatorESTIMATE_t;
 
 typedef struct {
+     timeUs_t               lastUpdateTime;
     fpVector3_t             accelNEU;
     fpVector3_t             accelBias;
     float                   calibratedGravityCMSS;
+    float                   accWeightFactor;
     zeroCalibrationScalar_t gravityCalibration;
 } navPosisitonEstimatorIMU_t;
 

src/main/sensors/acceleration.c

@@ -541,8 +541,12 @@ void accUpdate(void)
 
     // Before filtering check for clipping and vibration levels
     if (fabsf(acc.accADCf[X]) > ACC_CLIPPING_THRESHOLD_G || fabsf(acc.accADCf[Y]) > ACC_CLIPPING_THRESHOLD_G || fabsf(acc.accADCf[Z]) > ACC_CLIPPING_THRESHOLD_G) {
+        acc.isClipped = true;
         acc.accClipCount++;
     }
+    else {
+        acc.isClipped = false;
+    }
 
     // Calculate vibration levels
     for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
@@ -587,6 +591,11 @@ uint32_t accGetClipCount(void)
     return acc.accClipCount;
 }
 
+bool accIsClipped(void)
+{
+    return acc.isClipped;
+}
+
 void accSetCalibrationValues(void)
 {
     if ((accelerometerConfig()->accZero.raw[X] == 0) && (accelerometerConfig()->accZero.raw[Y] == 0) && (accelerometerConfig()->accZero.raw[Z] == 0) &&

src/main/sensors/acceleration.h

@@ -55,6 +55,7 @@ typedef struct acc_s {
     float accADCf[XYZ_AXIS_COUNT]; // acceleration in g
     float accVibeSq[XYZ_AXIS_COUNT];
     uint32_t accClipCount;
+    bool isClipped;
 } acc_t;
 
 extern acc_t acc;
@@ -79,6 +80,7 @@ void accGetMeasuredAcceleration(fpVector3_t *measuredAcc);
 void accGetVibrationLevels(fpVector3_t *accVibeLevels);
 float accGetVibrationLevel(void);
 uint32_t accGetClipCount(void);
+bool accIsClipped(void);
 void accUpdate(void);
 void accSetCalibrationValues(void);
 void accInitFilters(void);