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

Merge pull request #5902 from mikeller/optimise_gyro_sanity_checks_f7

Browse source 
Optimised gyro sanity checks for F7.
This commit is contained in:
Michael Keller 2018-05-17 23:16:38 +12:00 • committed by GitHub
commit 82500afb1b
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
1 changed files with 43 additions and 34 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -971,27 +971,32 @@ FAST_CODE int32_t gyroSlewLimiter(gyroSensor_t *gyroSensor, int axis)
} }
#endif #endif
static void checkForOverflow(gyroSensor_t *gyroSensor, timeUs_t currentTimeUs)
{
#ifdef USE_GYRO_OVERFLOW_CHECK #ifdef USE_GYRO_OVERFLOW_CHECK
static NOINLINE void handleOverflow(gyroSensor_t *gyroSensor, timeUs_t currentTimeUs)
{
const float gyroOverflowResetRate = GYRO_OVERFLOW_RESET_THRESHOLD * gyroSensor->gyroDev.scale;
if ((abs(gyro.gyroADCf[X]) < gyroOverflowResetRate)
&& (abs(gyro.gyroADCf[Y]) < gyroOverflowResetRate)
&& (abs(gyro.gyroADCf[Z]) < gyroOverflowResetRate)) {
// 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) {
gyroSensor->overflowDetected = false;
}
} else {
// not a consecutive OK value, so reset the overflow time
gyroSensor->overflowTimeUs = currentTimeUs;
}
}
static FAST_CODE void checkForOverflow(gyroSensor_t *gyroSensor, timeUs_t currentTimeUs)
{
// check for overflow to handle Yaw Spin To The Moon (YSTTM) // check for overflow to handle Yaw Spin To The Moon (YSTTM)
// ICM gyros are specified to +/- 2000 deg/sec, in a crash they can go out of spec. // ICM gyros are specified to +/- 2000 deg/sec, in a crash they can go out of spec.
// This can cause an overflow and sign reversal in the output. // 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. // Overflow and sign reversal seems to result in a gyro value of +1996 or -1996.
if (gyroSensor->overflowDetected) { if (gyroSensor->overflowDetected) {
const float gyroOverflowResetRate = GYRO_OVERFLOW_RESET_THRESHOLD * gyroSensor->gyroDev.scale; handleOverflow(gyroSensor, currentTimeUs);
if ((abs(gyro.gyroADCf[X]) < gyroOverflowResetRate)
&& (abs(gyro.gyroADCf[Y]) < gyroOverflowResetRate)
&& (abs(gyro.gyroADCf[Z]) < gyroOverflowResetRate)) {
// 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) {
gyroSensor->overflowDetected = false;
}
} else {
// not a consecutive OK value, so reset the overflow time
gyroSensor->overflowTimeUs = currentTimeUs;
}
} else { } else {
#ifndef SIMULATOR_BUILD #ifndef SIMULATOR_BUILD
// check for overflow in the axes set in overflowAxisMask // check for overflow in the axes set in overflowAxisMask
@ -1015,15 +1020,26 @@ static void checkForOverflow(gyroSensor_t *gyroSensor, timeUs_t currentTimeUs)
} }
#endif // SIMULATOR_BUILD #endif // SIMULATOR_BUILD
} }
#else }
UNUSED(gyroSensor);
UNUSED(currentTimeUs);
#endif // USE_GYRO_OVERFLOW_CHECK #endif // USE_GYRO_OVERFLOW_CHECK
#ifdef USE_YAW_SPIN_RECOVERY
static NOINLINE void handleYawSpin(gyroSensor_t *gyroSensor, timeUs_t currentTimeUs)
{
const float yawSpinResetRate = gyroConfig()->yaw_spin_threshold - 100.0f;
if (abs(gyro.gyroADCf[Z]) < yawSpinResetRate) {
// testing whether 20ms of consecutive OK gyro yaw values is enough
if (cmpTimeUs(currentTimeUs, gyroSensor->yawSpinTimeUs) > 20000) {
gyroSensor->yawSpinDetected = false;
}
} else {
// reset the yaw spin time
gyroSensor->yawSpinTimeUs = currentTimeUs;
}
} }
static void checkForYawSpin(gyroSensor_t *gyroSensor, timeUs_t currentTimeUs) static FAST_CODE void checkForYawSpin(gyroSensor_t *gyroSensor, timeUs_t currentTimeUs)
{ {
#ifdef USE_YAW_SPIN_RECOVERY
// if not in overflow mode, handle yaw spins above threshold // if not in overflow mode, handle yaw spins above threshold
#ifdef USE_GYRO_OVERFLOW_CHECK #ifdef USE_GYRO_OVERFLOW_CHECK
if (gyroSensor->overflowDetected) { if (gyroSensor->overflowDetected) {
@ -1031,17 +1047,9 @@ static void checkForYawSpin(gyroSensor_t *gyroSensor, timeUs_t currentTimeUs)
return; return;
} }
#endif // USE_GYRO_OVERFLOW_CHECK #endif // USE_GYRO_OVERFLOW_CHECK
if (gyroSensor->yawSpinDetected) { if (gyroSensor->yawSpinDetected) {
const float yawSpinResetRate = gyroConfig()->yaw_spin_threshold - 100.0f; handleYawSpin(gyroSensor, currentTimeUs);
if (abs(gyro.gyroADCf[Z]) < yawSpinResetRate) {
// testing whether 20ms of consecutive OK gyro yaw values is enough
if (cmpTimeUs(currentTimeUs, gyroSensor->yawSpinTimeUs) > 20000) {
gyroSensor->yawSpinDetected = false;
}
} else {
// reset the yaw spin time
gyroSensor->yawSpinTimeUs = currentTimeUs;
}
} else { } else {
#ifndef SIMULATOR_BUILD #ifndef SIMULATOR_BUILD
// check for spin on yaw axis only // check for spin on yaw axis only
@ -1051,11 +1059,8 @@ static void checkForYawSpin(gyroSensor_t *gyroSensor, timeUs_t currentTimeUs)
} }
#endif // SIMULATOR_BUILD #endif // SIMULATOR_BUILD
} }
#else
UNUSED(gyroSensor);
UNUSED(currentTimeUs);
#endif // USE_YAW_SPIN_RECOVERY
} }
#endif // USE_YAW_SPIN_RECOVERY
static FAST_CODE NOINLINE void gyroUpdateSensor(gyroSensor_t *gyroSensor, timeUs_t currentTimeUs) static FAST_CODE NOINLINE void gyroUpdateSensor(gyroSensor_t *gyroSensor, timeUs_t currentTimeUs)
{ {
@ -1094,13 +1099,17 @@ static FAST_CODE NOINLINE void gyroUpdateSensor(gyroSensor_t *gyroSensor, timeUs
accumulationLastTimeSampledUs = currentTimeUs; accumulationLastTimeSampledUs = currentTimeUs;
accumulatedMeasurementTimeUs += sampleDeltaUs; accumulatedMeasurementTimeUs += sampleDeltaUs;
#ifdef USE_GYRO_OVERFLOW_CHECK
if (gyroConfig()->checkOverflow && !gyroHasOverflowProtection) { if (gyroConfig()->checkOverflow && !gyroHasOverflowProtection) {
checkForOverflow(gyroSensor, currentTimeUs); checkForOverflow(gyroSensor, currentTimeUs);
} }
#endif
#ifdef USE_YAW_SPIN_RECOVERY
if (gyroConfig()->yaw_spin_recovery) { if (gyroConfig()->yaw_spin_recovery) {
checkForYawSpin(gyroSensor, currentTimeUs); checkForYawSpin(gyroSensor, currentTimeUs);
} }
#endif
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++) {