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

Revert "Change gyro_overflow_detect to use filtered instead of raw gyro data"

Browse source
This commit is contained in:
Michael Keller 2018-02-18 09:25:04 +13:00 • committed by GitHub
parent 3fbff5a700
commit e5364ee764
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
3 changed files with 35 additions and 27 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -153,9 +153,6 @@ static void gyroInitSensorFilters(gyroSensor_t *gyroSensor);
#define GYRO_SYNC_DENOM_DEFAULT 4
#endif
#define GYRO_OVERFLOW_TRIGGER_THRESHOLD 31980 // 97.5% full scale (1950dps)
#define GYRO_OVERFLOW_RESET_THRESHOLD 30340 // 92.5% full scale (1850dps)
PG_REGISTER_WITH_RESET_TEMPLATE(gyroConfig_t, gyroConfig, PG_GYRO_CONFIG, 1);
PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig,
@ -731,9 +728,13 @@ static void checkForOverflow(gyroSensor_t *gyroSensor, timeUs_t currentTimeUs)
// This can cause an overflow and sign reversal in the output.
// Overflow and sign reversal seems to result in a gyro value of +1996 or -1996.
if (gyroSensor->overflowDetected) {
if (abs(gyroSensor->gyroDev.gyroADC[X]) < GYRO_OVERFLOW_RESET_THRESHOLD
&& abs(gyroSensor->gyroDev.gyroADC[Y]) < GYRO_OVERFLOW_RESET_THRESHOLD
&& abs(gyroSensor->gyroDev.gyroADC[Z]) < GYRO_OVERFLOW_RESET_THRESHOLD) {
const float gyroRateX = gyroSensor->gyroDev.gyroADC[X] * gyroSensor->gyroDev.scale;
const float gyroRateY = gyroSensor->gyroDev.gyroADC[Y] * gyroSensor->gyroDev.scale;
const float gyroRateZ = gyroSensor->gyroDev.gyroADC[Z] * gyroSensor->gyroDev.scale;
static const int overflowResetThreshold = 1800;
if (abs(gyroRateX) < overflowResetThreshold
&& abs(gyroRateY) < overflowResetThreshold
&& abs(gyroRateZ) < overflowResetThreshold) {
// if we have 50ms of consecutive OK gyro vales, then assume yaw readings are OK again and reset overflowDetected
// reset requires good OK values on all axes
if (cmpTimeUs(currentTimeUs, gyroSensor->overflowTimeUs) > 50000) {
@ -743,25 +744,14 @@ static void checkForOverflow(gyroSensor_t *gyroSensor, timeUs_t currentTimeUs)
// not a consecutive OK value, so reset the overflow time
gyroSensor->overflowTimeUs = currentTimeUs;
}
} else {
#ifndef SIMULATOR_BUILD
// check for overflow in the axes set in overflowAxisMask
gyroOverflow_e overflowCheck = GYRO_OVERFLOW_NONE;
if (abs(gyroSensor->gyroDev.gyroADC[X]) > GYRO_OVERFLOW_TRIGGER_THRESHOLD) {
overflowCheck |= GYRO_OVERFLOW_X;
}
if (abs(gyroSensor->gyroDev.gyroADC[Y]) > GYRO_OVERFLOW_TRIGGER_THRESHOLD) {
overflowCheck |= GYRO_OVERFLOW_Y;
}
if (abs(gyroSensor->gyroDev.gyroADC[Z]) > GYRO_OVERFLOW_TRIGGER_THRESHOLD) {
overflowCheck |= GYRO_OVERFLOW_Z;
}
if (overflowCheck & overflowAxisMask) {
gyroSensor->overflowDetected = true;
gyroSensor->overflowTimeUs = currentTimeUs;
}
#endif // SIMULATOR_BUILD
}
#ifndef SIMULATOR_BUILD
// check for overflow in the axes set in overflowAxisMask
if (mpuGyroCheckOverflow(&gyroSensor->gyroDev) & overflowAxisMask) {
gyroSensor->overflowDetected = true;
gyroSensor->overflowTimeUs = currentTimeUs;
}
#endif // SIMULATOR_BUILD
#else
UNUSED(gyroSensor);
UNUSED(currentTimeUs);
@ -809,6 +799,9 @@ static FAST_CODE void gyroUpdateSensor(gyroSensor_t *gyroSensor, timeUs_t curren
accumulationLastTimeSampledUs = currentTimeUs;
accumulatedMeasurementTimeUs += sampleDeltaUs;
if (gyroConfig()->checkOverflow) {
checkForOverflow(gyroSensor, currentTimeUs);
}
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
@ -876,9 +869,6 @@ static FAST_CODE void gyroUpdateSensor(gyroSensor_t *gyroSensor, timeUs_t curren
}
}
}
if (gyroConfig()->checkOverflow) {
checkForOverflow(gyroSensor, currentTimeUs);
}
}
FAST_CODE void gyroUpdate(timeUs_t currentTimeUs)