Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-25 01:05:27 +03:00
Code Issues Wiki Activity

Fix iterm relax interfering with crash recovery

Browse source 
Iterm Relax was inserted before crash recovery and this was causing the recovery to be ineffective or fail completely. The problem was that the subsequent iterm calculation was not using the calculated recovery error and instead was using the setpoint error. As such I ended up accumulating and opposing P based on recovery error. This prevented the efforts of crash recovery to level the quad and since it couldn't reach the level state crash recovery wouldn't shut off. This resulted in the pilot only having control over yaw and throttle while crash recovery was still ineffectually trying to control roll/pitch.

The fix is to move crash recovery handling ahead of the iterm relax calculations and make sure that iterm relax uses the error calculated from crash recovery if active.  This also allows crash recovery to reset the iterm accumulation as originally designed.
This commit is contained in:
Bruce Luckcuck 2018-09-02 09:58:16 -04:00
parent 09b52975fb
commit 960442e917
1 changed files with 5 additions and 6 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -898,6 +898,10 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, const rollAndPitchT
// -----calculate error rate
const float gyroRate = gyro.gyroADCf[axis]; // Process variable from gyro output in deg/sec
float errorRate = currentPidSetpoint - gyroRate; // r - y
handleCrashRecovery(
pidProfile->crash_recovery, angleTrim, axis, currentTimeUs, gyroRate,
&currentPidSetpoint, &errorRate);
#ifdef USE_ABSOLUTE_CONTROL
float acCorrection = 0;
@ -905,7 +909,7 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, const rollAndPitchT
#endif
const float ITerm = pidData[axis].I;
float itermErrorRate = currentPidSetpoint - gyroRate;
float itermErrorRate = errorRate;
#if defined(USE_ITERM_RELAX)
if (itermRelax && (axis < FD_YAW || itermRelax == ITERM_RELAX_RPY || itermRelax == ITERM_RELAX_RPY_INC)) {
@ -968,11 +972,6 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, const rollAndPitchT
}
#endif
float errorRate = currentPidSetpoint - gyroRate; // r - y
handleCrashRecovery(
pidProfile->crash_recovery, angleTrim, axis, currentTimeUs, gyroRate,
&currentPidSetpoint, &errorRate);
// --------low-level gyro-based PID based on 2DOF PID controller. ----------
// 2-DOF PID controller with optional filter on derivative term.
// b = 1 and only c (feedforward weight) can be tuned (amount derivative on measurement or error).