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

Yaw spin recovery optimize (#3)

Browse source 
* PID controller unittest

* Clean code for yaw spin recovery

* Yaw spin recovery optimizations

* Flash size optimizations, use 50% throttle when airmode is off, and override pidsum_limit_yaw

Also rebasing from betaflight/master
This commit is contained in:
Bruce Luckcuck 2018-04-23 20:04:39 -04:00 • committed by Michael Keller
parent 41fb37a264
commit 0a0add8c56
5 changed files with 28 additions and 13 deletions
Download patch file Download diff file Expand all files Collapse all files

22
src/main/flight/mixer.c
View file

@ -58,7 +58,6 @@
#include "rx/rx.h"
#include "sensors/battery.h"
#include "sensors/gyro.h"
PG_REGISTER_WITH_RESET_TEMPLATE(mixerConfig_t, mixerConfig, PG_MIXER_CONFIG, 0);
@ -751,8 +750,19 @@ NOINLINE void mixTable(timeUs_t currentTimeUs, uint8_t vbatPidCompensation)
constrainf(pidData[FD_ROLL].Sum, -currentPidProfile->pidSumLimit, currentPidProfile->pidSumLimit) / PID_MIXER_SCALING;
const float scaledAxisPidPitch =
constrainf(pidData[FD_PITCH].Sum, -currentPidProfile->pidSumLimit, currentPidProfile->pidSumLimit) / PID_MIXER_SCALING;
uint16_t yawPidSumLimit = currentPidProfile->pidSumLimitYaw;
#ifdef USE_YAW_SPIN_RECOVERY
const bool yawSpinDetected = gyroYawSpinDetected();
if (yawSpinDetected) {
yawPidSumLimit = PIDSUM_LIMIT_MAX; // Set to the maximum limit during yaw spin recovery to prevent limiting motor authority
}
#endif // USE_YAW_SPIN_RECOVERY
float scaledAxisPidYaw =
constrainf(pidData[FD_YAW].Sum, -currentPidProfile->pidSumLimitYaw, currentPidProfile->pidSumLimitYaw) / PID_MIXER_SCALING;
constrainf(pidData[FD_YAW].Sum, -yawPidSumLimit, yawPidSumLimit) / PID_MIXER_SCALING;
if (!mixerConfig()->yaw_motors_reversed) {
scaledAxisPidYaw = -scaledAxisPidYaw;
}
@ -765,11 +775,11 @@ NOINLINE void mixTable(timeUs_t currentTimeUs, uint8_t vbatPidCompensation)
throttle = applyThrottleLimit(throttle);
}
// Handle yaw spin recovery - throttle is set to zero to prevent flyaway
// and to give the mixer full authority to stop the spin
#ifdef USE_YAW_SPIN_RECOVERY
if (gyroYawSpinDetected()) {
throttle = 0.0f;
// 50% throttle provides the maximum authority for yaw recovery when airmode is not active.
// When airmode is active the throttle setting doesn't impact recovery authority.
if (yawSpinDetected && !isAirmodeActive()) {
throttle = 0.5f; //
}
#endif // USE_YAW_SPIN_RECOVERY