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

Merge pull request #4769 from martinbudden/bfa_gyro_trapezium_integrate

Browse source 
Changed gyro integration to use trapezium rule
This commit is contained in:
Michael Keller 2017-12-18 15:57:03 +13:00 • committed by GitHub
commit cbee16f6c3
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
1 changed files with 7 additions and 2 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -78,6 +78,7 @@ FAST_RAM gyro_t gyro;
static FAST_RAM uint8_t gyroDebugMode; static FAST_RAM uint8_t gyroDebugMode;
static FAST_RAM float accumulatedMeasurements[XYZ_AXIS_COUNT]; static FAST_RAM float accumulatedMeasurements[XYZ_AXIS_COUNT];
static FAST_RAM float gyroPrevious[XYZ_AXIS_COUNT];
static FAST_RAM timeUs_t accumulatedMeasurementTimeUs; static FAST_RAM timeUs_t accumulatedMeasurementTimeUs;
static FAST_RAM timeUs_t accumulationLastTimeSampledUs; static FAST_RAM timeUs_t accumulationLastTimeSampledUs;
@ -664,7 +665,9 @@ static FAST_CODE void gyroUpdateSensor(gyroSensor_t *gyroSensor, timeUs_t curren
gyroADCf = gyroSensor->notchFilter2ApplyFn(&gyroSensor->notchFilter2[axis], gyroADCf); gyroADCf = gyroSensor->notchFilter2ApplyFn(&gyroSensor->notchFilter2[axis], gyroADCf);
gyroADCf = gyroSensor->softLpfFilterApplyFn(gyroSensor->softLpfFilterPtr[axis], gyroADCf); gyroADCf = gyroSensor->softLpfFilterApplyFn(gyroSensor->softLpfFilterPtr[axis], gyroADCf);
gyro.gyroADCf[axis] = gyroADCf; gyro.gyroADCf[axis] = gyroADCf;
accumulatedMeasurements[axis] += gyroADCf * sampleDeltaUs; // integrate using trapezium rule to avoid bias
accumulatedMeasurements[axis] += 0.5f * (gyroPrevious[axis] + gyroADCf) * sampleDeltaUs;
gyroPrevious[axis] = gyroADCf;
} }
} else { } else {
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
@ -696,7 +699,9 @@ static FAST_CODE void gyroUpdateSensor(gyroSensor_t *gyroSensor, timeUs_t curren
gyroADCf = gyroSensor->softLpfFilterApplyFn(gyroSensor->softLpfFilterPtr[axis], gyroADCf); gyroADCf = gyroSensor->softLpfFilterApplyFn(gyroSensor->softLpfFilterPtr[axis], gyroADCf);
gyro.gyroADCf[axis] = gyroADCf; gyro.gyroADCf[axis] = gyroADCf;
accumulatedMeasurements[axis] += gyroADCf * sampleDeltaUs; // integrate using trapezium rule to avoid bias
accumulatedMeasurements[axis] += 0.5f * (gyroPrevious[axis] + gyroADCf) * sampleDeltaUs;
gyroPrevious[axis] = gyroADCf;
} }
} }
} }