Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-19 14:25:20 +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

10
src/main/flight/pid.c
View file

@ -541,6 +541,10 @@ void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *an
const float tpaFactor = getThrottlePIDAttenuation();
const float motorMixRange = getMotorMixRange();
#ifdef USE_YAW_SPIN_RECOVERY
const bool yawSpinActive = gyroYawSpinDetected();
#endif
// Dynamic i component,
// gradually scale back integration when above windup point
const float dynCi = MIN((1.0f - motorMixRange) * ITermWindupPointInv, 1.0f) * dT * itermAccelerator;
@ -574,7 +578,7 @@ void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *an
// Handle yaw spin recovery - zero the setpoint on yaw to aid in recovery
// It's not necessary to zero the set points for R/P because the PIDs will be zeroed below
#ifdef USE_YAW_SPIN_RECOVERY
if ((axis == FD_YAW) && gyroYawSpinDetected()) {
if ((axis == FD_YAW) && yawSpinActive) {
currentPidSetpoint = 0.0f;
}
#endif // USE_YAW_SPIN_RECOVERY
@ -628,7 +632,7 @@ void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *an
pidData[axis].D = pidCoefficient[axis].Kd * delta * tpaFactor;
#ifdef USE_YAW_SPIN_RECOVERY
if (gyroYawSpinDetected()) {
if (yawSpinActive) {
// zero PIDs on pitch and roll leaving yaw P to correct spin
pidData[axis].P = 0;
pidData[axis].I = 0;
@ -643,7 +647,7 @@ void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *an
pidData[FD_PITCH].Sum = pidData[FD_PITCH].P + pidData[FD_PITCH].I + pidData[FD_PITCH].D;
#ifdef USE_YAW_SPIN_RECOVERY
if (gyroYawSpinDetected()) {
if (yawSpinActive) {
// yaw P alone to correct spin
pidData[FD_YAW].I = 0;
}