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

Clean code for yaw spin recovery

Browse source
This commit is contained in:
ctzsnooze 2018-04-22 22:45:29 +10:00 • committed by Michael Keller
parent a2c6264d55
commit 41fb37a264
6 changed files with 111 additions and 7 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -571,6 +571,14 @@ void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *an
currentPidSetpoint = pidLevel(axis, pidProfile, angleTrim, currentPidSetpoint);
}
// 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()) {
currentPidSetpoint = 0.0f;
}
#endif // USE_YAW_SPIN_RECOVERY
// -----calculate error rate
const float gyroRate = gyro.gyroADCf[axis]; // Process variable from gyro output in deg/sec
float errorRate = currentPidSetpoint - gyroRate; // r - y
@ -618,6 +626,15 @@ void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *an
detectAndSetCrashRecovery(pidProfile->crash_recovery, axis, currentTimeUs, delta, errorRate);
pidData[axis].D = pidCoefficient[axis].Kd * delta * tpaFactor;
#ifdef USE_YAW_SPIN_RECOVERY
if (gyroYawSpinDetected()) {
// zero PIDs on pitch and roll leaving yaw P to correct spin
pidData[axis].P = 0;
pidData[axis].I = 0;
pidData[axis].D = 0;
}
#endif // USE_YAW_SPIN_RECOVERY
}
}
@ -625,6 +642,13 @@ void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *an
pidData[FD_ROLL].Sum = pidData[FD_ROLL].P + pidData[FD_ROLL].I + pidData[FD_ROLL].D;
pidData[FD_PITCH].Sum = pidData[FD_PITCH].P + pidData[FD_PITCH].I + pidData[FD_PITCH].D;
#ifdef USE_YAW_SPIN_RECOVERY
if (gyroYawSpinDetected()) {
// yaw P alone to correct spin
pidData[FD_YAW].I = 0;
}
#endif // USE_YAW_SPIN_RECOVERY
// YAW has no D
pidData[FD_YAW].Sum = pidData[FD_YAW].P + pidData[FD_YAW].I;