Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-26 01:35:41 +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

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

@ -77,6 +77,9 @@
gyro_t gyro;
static uint8_t gyroDebugMode;
static float accumulator[XYZ_AXIS_COUNT];
static timeUs_t accumulatedTimeUs;
static timeUs_t lastTimeSampledUs;
typedef struct gyroCalibration_s {
int32_t sum[XYZ_AXIS_COUNT];
@ -613,7 +616,7 @@ int32_t gyroSlewLimiter(gyroSensor_t *gyroSensor, int axis)
}
#endif
void gyroUpdateSensor(gyroSensor_t *gyroSensor)
static void gyroUpdateSensor(gyroSensor_t *gyroSensor, timeUs_t currentTimeUs)
{
if (!gyroSensor->gyroDev.readFn(&gyroSensor->gyroDev)) {
return;
@ -646,6 +649,10 @@ void gyroUpdateSensor(gyroSensor_t *gyroSensor)
gyroDataAnalyse(&gyroSensor->gyroDev, gyroSensor->notchFilterDyn);
#endif
const timeDelta_t sampleDeltaUs = currentTimeUs - lastTimeSampledUs;
lastTimeSampledUs = currentTimeUs;
accumulatedTimeUs += sampleDeltaUs;
if (gyroDebugMode == DEBUG_NONE) {
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
// NOTE: this branch optimized for when there is no gyro debugging, ensure it is kept in step with non-optimized branch
@ -657,6 +664,7 @@ void gyroUpdateSensor(gyroSensor_t *gyroSensor)
gyroADCf = gyroSensor->notchFilter2ApplyFn(&gyroSensor->notchFilter2[axis], gyroADCf);
gyroADCf = gyroSensor->softLpfFilterApplyFn(gyroSensor->softLpfFilterPtr[axis], gyroADCf);
gyro.gyroADCf[axis] = gyroADCf;
accumulator[axis] += gyroADCf * sampleDeltaUs;
}
} else {
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
@ -688,13 +696,33 @@ void gyroUpdateSensor(gyroSensor_t *gyroSensor)
gyroADCf = gyroSensor->softLpfFilterApplyFn(gyroSensor->softLpfFilterPtr[axis], gyroADCf);
gyro.gyroADCf[axis] = gyroADCf;
accumulator[axis] += gyroADCf * sampleDeltaUs;
}
}
}
void gyroUpdate(void)
void gyroUpdate(timeUs_t currentTimeUs)
{
gyroUpdateSensor(&gyroSensor1);
gyroUpdateSensor(&gyroSensor1, currentTimeUs);
}
bool gyroGetAccumulationAverage(float *accumulation)
{
if (accumulatedTimeUs > 0) {
const float accumulatedTimeS = accumulatedTimeUs * 1e-6;
// If we have gyro data accumulated, calculate average rate that will yield the same rotation
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
accumulation[axis] = accumulator[axis] / accumulatedTimeS;
accumulator[axis] = 0.0f;
}
accumulatedTimeUs = 0;
return true;
} else {
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
accumulation[axis] = 0.0f;
}
return false;
}
}
void gyroReadTemperature(void)