Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-25 17:25:20 +03:00
Code Issues Wiki Activity

Correct IMU gyro input to be based on sample loop time rather than actual delta time

Browse source 
The current calculation accumulates gyro data between IMU updates based on the actual time delta betwwn samples. The problem with this is that loop time jitter or delays will decrease the accuracy of the gyro rotational input into the attitude calculation. The sensor samples based on a hardware driven clock that matches the `gyro.targetLooptime`. The sensor captures samples at these intervals regardless of when we read them so the calculation should be based on the duration between samples rather then the duration between processing loops.

Will improve the accuracy of the IMU attitude estimate - particularly in cases of higher CPU load and increased looptime jitter. Bench testing shows reduced drift. Also flight tested and behaved as expected.
This commit is contained in:
Bruce Luckcuck 2019-07-26 16:32:03 -04:00
parent 0f230c42a1
commit 50ce338390
1 changed files with 6 additions and 8 deletions
Download patch file Download diff file Expand all files Collapse all files

14
src/main/sensors/gyro.c
View file

@ -100,8 +100,7 @@ static FAST_RAM_ZERO_INIT bool yawSpinDetected;
static FAST_RAM_ZERO_INIT float accumulatedMeasurements[XYZ_AXIS_COUNT];
static FAST_RAM_ZERO_INIT float gyroPrevious[XYZ_AXIS_COUNT];
static FAST_RAM_ZERO_INIT timeUs_t accumulatedMeasurementTimeUs;
static FAST_RAM_ZERO_INIT timeUs_t accumulationLastTimeSampledUs;
static FAST_RAM_ZERO_INIT int accumulatedMeasurementCount;
static FAST_RAM_ZERO_INIT int16_t gyroSensorTemperature;
@ -1092,9 +1091,6 @@ static FAST_CODE FAST_CODE_NOINLINE void gyroUpdateSensor(gyroSensor_t *gyroSens
FAST_CODE void gyroUpdate(timeUs_t currentTimeUs)
{
const timeDelta_t sampleDeltaUs = currentTimeUs - accumulationLastTimeSampledUs;
accumulationLastTimeSampledUs = currentTimeUs;
accumulatedMeasurementTimeUs += sampleDeltaUs;
switch (gyroToUse) {
case GYRO_CONFIG_USE_GYRO_1:
@ -1187,22 +1183,24 @@ FAST_CODE void gyroUpdate(timeUs_t currentTimeUs)
if (!overflowDetected) {
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
// integrate using trapezium rule to avoid bias
accumulatedMeasurements[axis] += 0.5f * (gyroPrevious[axis] + gyro.gyroADCf[axis]) * sampleDeltaUs;
accumulatedMeasurements[axis] += 0.5f * (gyroPrevious[axis] + gyro.gyroADCf[axis]) * gyro.targetLooptime;
gyroPrevious[axis] = gyro.gyroADCf[axis];
}
accumulatedMeasurementCount++;
}
}
bool gyroGetAccumulationAverage(float *accumulationAverage)
{
if (accumulatedMeasurementTimeUs > 0) {
if (accumulatedMeasurementCount) {
// If we have gyro data accumulated, calculate average rate that will yield the same rotation
const timeUs_t accumulatedMeasurementTimeUs = accumulatedMeasurementCount * gyro.targetLooptime;
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
accumulationAverage[axis] = accumulatedMeasurements[axis] / accumulatedMeasurementTimeUs;
accumulatedMeasurements[axis] = 0.0f;
}
accumulatedMeasurementTimeUs = 0;
accumulatedMeasurementCount = 0;
return true;
} else {
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {