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

Fix to acc gyro accumulation code

Browse source
This commit is contained in:
Martin Budden 2017-11-28 15:10:11 +00:00
parent 94d3549299
commit c0b1572175
3 changed files with 29 additions and 30 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -449,7 +449,9 @@ static void imuCalculateEstimatedAttitude(timeUs_t currentTimeUs)
float gyroAverage[XYZ_AXIS_COUNT]; float gyroAverage[XYZ_AXIS_COUNT];
gyroGetAccumulationAverage(gyroAverage); gyroGetAccumulationAverage(gyroAverage);
float accAverage[XYZ_AXIS_COUNT]; float accAverage[XYZ_AXIS_COUNT];
accGetAccumulationAverage(accAverage); if (!accGetAccumulationAverage(accAverage)) {
useAcc = false;
}
imuMahonyAHRSupdate(deltaT * 1e-6f, imuMahonyAHRSupdate(deltaT * 1e-6f,
DEGREES_TO_RADIANS(gyroAverage[X]), DEGREES_TO_RADIANS(gyroAverage[Y]), DEGREES_TO_RADIANS(gyroAverage[Z]), DEGREES_TO_RADIANS(gyroAverage[X]), DEGREES_TO_RADIANS(gyroAverage[Y]), DEGREES_TO_RADIANS(gyroAverage[Z]),
useAcc, accAverage[X], accAverage[Y], accAverage[Z], useAcc, accAverage[X], accAverage[Y], accAverage[Z],

26
src/main/sensors/acceleration.c
View file

@ -69,9 +69,8 @@
acc_t acc; // acc access functions acc_t acc; // acc access functions
static float accumulator[XYZ_AXIS_COUNT]; static float accumulatedMeasurements[XYZ_AXIS_COUNT];
static timeUs_t accumulatedTimeUs; static int accumulatedMeasurementCount;
static timeUs_t lastTimeSampledUs;
static uint16_t calibratingA = 0; // the calibration is done is the main loop. Calibrating decreases at each cycle down to 0, then we enter in a normal mode. static uint16_t calibratingA = 0; // the calibration is done is the main loop. Calibrating decreases at each cycle down to 0, then we enter in a normal mode.
@ -469,6 +468,8 @@ static void applyAccelerationTrims(const flightDynamicsTrims_t *accelerationTrim
void accUpdate(timeUs_t currentTimeUs, rollAndPitchTrims_t *rollAndPitchTrims) void accUpdate(timeUs_t currentTimeUs, rollAndPitchTrims_t *rollAndPitchTrims)
{ {
UNUSED(currentTimeUs);
if (!acc.dev.readFn(&acc.dev)) { if (!acc.dev.readFn(&acc.dev)) {
return; return;
} }
@ -497,28 +498,25 @@ void accUpdate(timeUs_t currentTimeUs, rollAndPitchTrims_t *rollAndPitchTrims)
applyAccelerationTrims(accelerationTrims); applyAccelerationTrims(accelerationTrims);
const timeDelta_t sampleDeltaUs = currentTimeUs - lastTimeSampledUs; ++accumulatedMeasurementCount;
lastTimeSampledUs = currentTimeUs;
accumulatedTimeUs += sampleDeltaUs;
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
accumulator[axis] += acc.accSmooth[axis] * sampleDeltaUs; accumulatedMeasurements[axis] += acc.accSmooth[axis];
} }
} }
bool accGetAccumulationAverage(float *accumulation) bool accGetAccumulationAverage(float *accumulationAverage)
{ {
if (accumulatedTimeUs > 0) { if (accumulatedMeasurementCount > 0) {
const float accumulatedTimeS = accumulatedTimeUs * 1e-6;
// If we have gyro data accumulated, calculate average rate that will yield the same rotation // If we have gyro data accumulated, calculate average rate that will yield the same rotation
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
accumulation[axis] = accumulator[axis] / accumulatedTimeS; accumulationAverage[axis] = accumulatedMeasurements[axis] / accumulatedMeasurementCount;
accumulator[axis] = 0.0f; accumulatedMeasurements[axis] = 0.0f;
} }
accumulatedTimeUs = 0; accumulatedMeasurementCount = 0;
return true; return true;
} else { } else {
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
accumulation[axis] = 0.0f; accumulationAverage[axis] = 0.0f;
} }
return false; return false;
} }

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

@ -77,9 +77,9 @@
gyro_t gyro; gyro_t gyro;
static uint8_t gyroDebugMode; static uint8_t gyroDebugMode;
static float accumulator[XYZ_AXIS_COUNT]; static float accumulatedMeasurements[XYZ_AXIS_COUNT];
static timeUs_t accumulatedTimeUs; static timeUs_t accumulatedMeasurementTimeUs;
static timeUs_t lastTimeSampledUs; static timeUs_t accumulationLastTimeSampledUs;
typedef struct gyroCalibration_s { typedef struct gyroCalibration_s {
int32_t sum[XYZ_AXIS_COUNT]; int32_t sum[XYZ_AXIS_COUNT];
@ -649,9 +649,9 @@ static void gyroUpdateSensor(gyroSensor_t *gyroSensor, timeUs_t currentTimeUs)
gyroDataAnalyse(&gyroSensor->gyroDev, gyroSensor->notchFilterDyn); gyroDataAnalyse(&gyroSensor->gyroDev, gyroSensor->notchFilterDyn);
#endif #endif
const timeDelta_t sampleDeltaUs = currentTimeUs - lastTimeSampledUs; const timeDelta_t sampleDeltaUs = currentTimeUs - accumulationLastTimeSampledUs;
lastTimeSampledUs = currentTimeUs; accumulationLastTimeSampledUs = currentTimeUs;
accumulatedTimeUs += sampleDeltaUs; accumulatedMeasurementTimeUs += sampleDeltaUs;
if (gyroDebugMode == DEBUG_NONE) { if (gyroDebugMode == DEBUG_NONE) {
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
@ -664,7 +664,7 @@ static void gyroUpdateSensor(gyroSensor_t *gyroSensor, timeUs_t currentTimeUs)
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;
accumulator[axis] += gyroADCf * sampleDeltaUs; accumulatedMeasurements[axis] += gyroADCf * sampleDeltaUs;
} }
} else { } else {
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
@ -696,7 +696,7 @@ static void gyroUpdateSensor(gyroSensor_t *gyroSensor, timeUs_t currentTimeUs)
gyroADCf = gyroSensor->softLpfFilterApplyFn(gyroSensor->softLpfFilterPtr[axis], gyroADCf); gyroADCf = gyroSensor->softLpfFilterApplyFn(gyroSensor->softLpfFilterPtr[axis], gyroADCf);
gyro.gyroADCf[axis] = gyroADCf; gyro.gyroADCf[axis] = gyroADCf;
accumulator[axis] += gyroADCf * sampleDeltaUs; accumulatedMeasurements[axis] += gyroADCf * sampleDeltaUs;
} }
} }
} }
@ -706,20 +706,19 @@ void gyroUpdate(timeUs_t currentTimeUs)
gyroUpdateSensor(&gyroSensor1, currentTimeUs); gyroUpdateSensor(&gyroSensor1, currentTimeUs);
} }
bool gyroGetAccumulationAverage(float *accumulation) bool gyroGetAccumulationAverage(float *accumulationAverage)
{ {
if (accumulatedTimeUs > 0) { if (accumulatedMeasurementTimeUs > 0) {
const float accumulatedTimeS = accumulatedTimeUs * 1e-6;
// If we have gyro data accumulated, calculate average rate that will yield the same rotation // If we have gyro data accumulated, calculate average rate that will yield the same rotation
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
accumulation[axis] = accumulator[axis] / accumulatedTimeS; accumulationAverage[axis] = accumulatedMeasurements[axis] / accumulatedMeasurementTimeUs;
accumulator[axis] = 0.0f; accumulatedMeasurements[axis] = 0.0f;
} }
accumulatedTimeUs = 0; accumulatedMeasurementTimeUs = 0;
return true; return true;
} else { } else {
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
accumulation[axis] = 0.0f; accumulationAverage[axis] = 0.0f;
} }
return false; return false;
} }