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

Add "auto" mode to yaw spin recovery

Browse source 
Changes yaw spin recovery from a simple on/off to on/off/auto. In auto mode the triggering threshold is automatically calculated based on the user's current max yaw rate.

So `yaw_spin_recovery` has the following options:
`OFF`: Feature is disabled
`ON`: Feature is enabled with a manually configured `yaw_spin_threshold`
`AUTO`: Feature is enabled and the trigger threshold is automatically calculated.

The auto calculation of the trigger threshold is:
```
MAX(yawRate * 0.25, 200)
```
The 25% of the user's rate is to provide a buffer for yaw overshoots during normal flight. Overall the threshold is constrained between 500 - 1950 to match the manual selection.

The default is changed from `ON` to `AUTO` so that users will get protection without needing to configure the threshold manually.

In `AUTO` mode the threshold is calculated dynamically so rate changes in flight (like rateprofile change) will set a new trigger threshold.
This commit is contained in:
Bruce Luckcuck 2020-02-05 17:33:33 -05:00
parent 97704dda58
commit 653fe308cb
4 changed files with 55 additions and 6 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -100,6 +100,8 @@ static FAST_RAM_ZERO_INIT uint8_t overflowAxisMask;
#endif
#ifdef USE_YAW_SPIN_RECOVERY
static FAST_RAM_ZERO_INIT bool yawSpinRecoveryEnabled;
static FAST_RAM_ZERO_INIT int yawSpinRecoveryThreshold;
static FAST_RAM_ZERO_INIT bool yawSpinDetected;
static FAST_RAM_ZERO_INIT timeUs_t yawSpinTimeUs;
#endif
@ -180,7 +182,7 @@ void pgResetFn_gyroConfig(gyroConfig_t *gyroConfig)
gyroConfig->gyro_soft_notch_cutoff_2 = 0;
gyroConfig->checkOverflow = GYRO_OVERFLOW_CHECK_ALL_AXES;
gyroConfig->gyro_offset_yaw = 0;
gyroConfig->yaw_spin_recovery = true;
gyroConfig->yaw_spin_recovery = YAW_SPIN_RECOVERY_AUTO;
gyroConfig->yaw_spin_threshold = 1950;
gyroConfig->dyn_lpf_gyro_min_hz = 200;
gyroConfig->dyn_lpf_gyro_max_hz = 500;
@ -998,7 +1000,7 @@ static FAST_CODE void checkForOverflow(timeUs_t currentTimeUs)
#ifdef USE_YAW_SPIN_RECOVERY
static FAST_CODE_NOINLINE void handleYawSpin(timeUs_t currentTimeUs)
{
const float yawSpinResetRate = gyroConfig()->yaw_spin_threshold - 100.0f;
const float yawSpinResetRate = yawSpinRecoveryThreshold - 100.0f;
if (fabsf(gyro.gyroADCf[Z]) < yawSpinResetRate) {
// testing whether 20ms of consecutive OK gyro yaw values is enough
if (cmpTimeUs(currentTimeUs, yawSpinTimeUs) > 20000) {
@ -1025,7 +1027,7 @@ static FAST_CODE void checkForYawSpin(timeUs_t currentTimeUs)
} else {
#ifndef SIMULATOR_BUILD
// check for spin on yaw axis only
if (abs((int)gyro.gyroADCf[Z]) > gyroConfig()->yaw_spin_threshold) {
if (abs((int)gyro.gyroADCf[Z]) > yawSpinRecoveryThreshold) {
yawSpinDetected = true;
yawSpinTimeUs = currentTimeUs;
}
@ -1181,7 +1183,7 @@ FAST_CODE void gyroFiltering(timeUs_t currentTimeUs)
#endif
#ifdef USE_YAW_SPIN_RECOVERY
if (gyroConfig()->yaw_spin_recovery) {
if (yawSpinRecoveryEnabled) {
checkForYawSpin(currentTimeUs);
}
#endif
@ -1310,3 +1312,30 @@ void dynLpfGyroUpdate(float throttle)
}
}
#endif
#ifdef USE_YAW_SPIN_RECOVERY
void initYawSpinRecovery(int maxYawRate)
{
bool enabledFlag;
int threshold;
switch (gyroConfig()->yaw_spin_recovery) {
case YAW_SPIN_RECOVERY_OFF:
enabledFlag = false;
threshold = YAW_SPIN_RECOVERY_THRESHOLD_MAX;
break;
case YAW_SPIN_RECOVERY_ON:
enabledFlag = true;
threshold = gyroConfig()->yaw_spin_threshold;
break;
case YAW_SPIN_RECOVERY_AUTO:
enabledFlag = true;
const int overshootAllowance = MAX(maxYawRate / 4, 200); // Allow a 25% or minimum 200dps overshoot tolerance
threshold = constrain(maxYawRate + overshootAllowance, YAW_SPIN_RECOVERY_THRESHOLD_MIN, YAW_SPIN_RECOVERY_THRESHOLD_MAX);
break;
}
yawSpinRecoveryEnabled = enabledFlag;
yawSpinRecoveryThreshold = threshold;
}
#endif