From f674b01bc83a61f2883c73ad57e076a932933c14 Mon Sep 17 00:00:00 2001 From: etracer65 Date: Wed, 14 Mar 2018 09:19:13 -0400 Subject: [PATCH] Change ACC health check to use averaged rather than only the last (#5413) The function imuIsAccelerometerHealthy() was only using the last ACC sample to determine if the sensor was returning "healthy" data. This then controlled whether the IMU attitude calculation considered ACC data at all. There were two problems with this: 1. A single sample from the ACC can be very noisy and exceed the check threshold resulting in a false negative on the health of the sensor. 2. The attitude calculations exclusively used the averaged ACC samples so there was an inconsistency in checking only the last sample to determine if the data was useful. This change should lead to fewer occurences of the ACC sensor data being ignored in the attitude estimation which should in turn improve the overall estimate. --- src/main/flight/imu.c | 13 +++++-------- 1 file changed, 5 insertions(+), 8 deletions(-) diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c index aa71a5ba50..87705b5c0e 100644 --- a/src/main/flight/imu.c +++ b/src/main/flight/imu.c @@ -393,11 +393,11 @@ STATIC_UNIT_TESTED void imuUpdateEulerAngles(void) } } -static bool imuIsAccelerometerHealthy(void) +static bool imuIsAccelerometerHealthy(float *accAverage) { float accMagnitude = 0; for (int axis = 0; axis < 3; axis++) { - const float a = acc.accADC[axis]; + const float a = accAverage[axis]; accMagnitude += a * a; } @@ -418,10 +418,6 @@ static void imuCalculateEstimatedAttitude(timeUs_t currentTimeUs) const timeDelta_t deltaT = currentTimeUs - previousIMUUpdateTime; previousIMUUpdateTime = currentTimeUs; - if (imuIsAccelerometerHealthy()) { - useAcc = true; - } - #ifdef USE_MAG if (sensors(SENSOR_MAG) && compassIsHealthy()) { useMag = true; @@ -437,6 +433,7 @@ static void imuCalculateEstimatedAttitude(timeUs_t currentTimeUs) #if defined(SIMULATOR_BUILD) && defined(SKIP_IMU_CALC) UNUSED(imuMahonyAHRSupdate); + UNUSED(imuIsAccelerometerHealthy); UNUSED(useAcc); UNUSED(useMag); UNUSED(useYaw); @@ -450,8 +447,8 @@ static void imuCalculateEstimatedAttitude(timeUs_t currentTimeUs) float gyroAverage[XYZ_AXIS_COUNT]; gyroGetAccumulationAverage(gyroAverage); float accAverage[XYZ_AXIS_COUNT]; - if (!accGetAccumulationAverage(accAverage)) { - useAcc = false; + if (accGetAccumulationAverage(accAverage)) { + useAcc = imuIsAccelerometerHealthy(accAverage); } imuMahonyAHRSupdate(deltaT * 1e-6f, DEGREES_TO_RADIANS(gyroAverage[X]), DEGREES_TO_RADIANS(gyroAverage[Y]), DEGREES_TO_RADIANS(gyroAverage[Z]),