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

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

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

@ -971,14 +971,9 @@ 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
// check for overflow to handle Yaw Spin To The Moon (YSTTM) static NOINLINE void handleOverflow(gyroSensor_t *gyroSensor, timeUs_t currentTimeUs)
// 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.
// Overflow and sign reversal seems to result in a gyro value of +1996 or -1996.
if (gyroSensor->overflowDetected) {
const float gyroOverflowResetRate = GYRO_OVERFLOW_RESET_THRESHOLD * gyroSensor->gyroDev.scale; const float gyroOverflowResetRate = GYRO_OVERFLOW_RESET_THRESHOLD * gyroSensor->gyroDev.scale;
if ((abs(gyro.gyroADCf[X]) < gyroOverflowResetRate) if ((abs(gyro.gyroADCf[X]) < gyroOverflowResetRate)
&& (abs(gyro.gyroADCf[Y]) < gyroOverflowResetRate) && (abs(gyro.gyroADCf[Y]) < gyroOverflowResetRate)
@ -992,6 +987,16 @@ static void checkForOverflow(gyroSensor_t *gyroSensor, timeUs_t currentTimeUs)
// not a consecutive OK value, so reset the overflow time // not a consecutive OK value, so reset the overflow time
gyroSensor->overflowTimeUs = currentTimeUs; 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)
// 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.
// Overflow and sign reversal seems to result in a gyro value of +1996 or -1996.
if (gyroSensor->overflowDetected) {
handleOverflow(gyroSensor, 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,23 +1020,12 @@ 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
static void checkForYawSpin(gyroSensor_t *gyroSensor, timeUs_t currentTimeUs)
{
#ifdef USE_YAW_SPIN_RECOVERY #ifdef USE_YAW_SPIN_RECOVERY
// if not in overflow mode, handle yaw spins above threshold static NOINLINE void handleYawSpin(gyroSensor_t *gyroSensor, timeUs_t currentTimeUs)
#ifdef USE_GYRO_OVERFLOW_CHECK {
if (gyroSensor->overflowDetected) {
gyroSensor->yawSpinDetected = false;
return;
}
#endif // USE_GYRO_OVERFLOW_CHECK
if (gyroSensor->yawSpinDetected) {
const float yawSpinResetRate = gyroConfig()->yaw_spin_threshold - 100.0f; const float yawSpinResetRate = gyroConfig()->yaw_spin_threshold - 100.0f;
if (abs(gyro.gyroADCf[Z]) < yawSpinResetRate) { if (abs(gyro.gyroADCf[Z]) < yawSpinResetRate) {
// testing whether 20ms of consecutive OK gyro yaw values is enough // testing whether 20ms of consecutive OK gyro yaw values is enough
@ -1042,6 +1036,20 @@ static void checkForYawSpin(gyroSensor_t *gyroSensor, timeUs_t currentTimeUs)
// reset the yaw spin time // reset the yaw spin time
gyroSensor->yawSpinTimeUs = currentTimeUs; gyroSensor->yawSpinTimeUs = currentTimeUs;
} }
}
static FAST_CODE void checkForYawSpin(gyroSensor_t *gyroSensor, timeUs_t currentTimeUs)
{
// if not in overflow mode, handle yaw spins above threshold
#ifdef USE_GYRO_OVERFLOW_CHECK
if (gyroSensor->overflowDetected) {
gyroSensor->yawSpinDetected = false;
return;
}
#endif // USE_GYRO_OVERFLOW_CHECK
if (gyroSensor->yawSpinDetected) {
handleYawSpin(gyroSensor, 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++) {