diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c index 6ca875e337..e99bdc601b 100644 --- a/src/main/flight/imu.c +++ b/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) diff --git a/src/main/navigation/navigation_pos_estimator.c b/src/main/navigation/navigation_pos_estimator.c index 389118bfc8..563ee5dc67 100755 --- a/src/main/navigation/navigation_pos_estimator.c +++ b/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); diff --git a/src/main/navigation/navigation_pos_estimator_private.h b/src/main/navigation/navigation_pos_estimator_private.h index 19e3f83fde..7306936ff6 100644 --- a/src/main/navigation/navigation_pos_estimator_private.h +++ b/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; diff --git a/src/main/sensors/acceleration.c b/src/main/sensors/acceleration.c index 9d38c8526d..590d95bf59 100644 --- a/src/main/sensors/acceleration.c +++ b/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) && diff --git a/src/main/sensors/acceleration.h b/src/main/sensors/acceleration.h index 0e02737b28..4532d3c389 100644 --- a/src/main/sensors/acceleration.h +++ b/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);