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

Add acc and gyro accumulators to improve attitude estimation

Browse source
This commit is contained in:
Martin Budden 2017-11-26 20:20:48 +00:00
parent d3d5b107cc
commit 22c672fa7d
9 changed files with 79 additions and 14 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -446,10 +446,13 @@ static void imuCalculateEstimatedAttitude(timeUs_t currentTimeUs)
// printf("[imu]deltaT = %u, imuDeltaT = %u, currentTimeUs = %u, micros64_real = %lu\n", deltaT, imuDeltaT, currentTimeUs, micros64_real());
deltaT = imuDeltaT;
#endif
float gyroAverage[XYZ_AXIS_COUNT];
gyroGetAccumulationAverage(gyroAverage);
float accAverage[XYZ_AXIS_COUNT];
accGetAccumulationAverage(accAverage);
imuMahonyAHRSupdate(deltaT * 1e-6f,
DEGREES_TO_RADIANS(gyro.gyroADCf[X]), DEGREES_TO_RADIANS(gyro.gyroADCf[Y]), DEGREES_TO_RADIANS(gyro.gyroADCf[Z]),
useAcc, acc.accSmooth[X], acc.accSmooth[Y], acc.accSmooth[Z],
DEGREES_TO_RADIANS(gyroAverage[X]), DEGREES_TO_RADIANS(gyroAverage[Y]), DEGREES_TO_RADIANS(gyroAverage[Z]),
useAcc, accAverage[X], accAverage[Y], accAverage[Z],
useMag, mag.magADC[X], mag.magADC[Y], mag.magADC[Z],
useYaw, rawYawError);