Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-16 21:05:35 +03:00
Code Issues Wiki Activity

Change ACC health check to use averaged rather than only the last (#5413)

Browse source 
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.
This commit is contained in:
etracer65 2018-03-14 09:19:13 -04:00 • committed by Michael Keller
parent eee15e7ca4
commit f674b01bc8
1 changed files with 5 additions and 8 deletions
Download patch file Download diff file Expand all files Collapse all files

13
src/main/flight/imu.c
View file

@ -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]),