2 * This file is part of Cleanflight and Betaflight.
4 * Cleanflight and Betaflight are free software. You can redistribute
5 * this software and/or modify this software under the terms of the
6 * GNU General Public License as published by the Free Software
7 * Foundation, either version 3 of the License, or (at your option)
10 * Cleanflight and Betaflight are distributed in the hope that they
11 * will be useful, but WITHOUT ANY WARRANTY; without even the implied
12 * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
13 * See the GNU General Public License for more details.
15 * You should have received a copy of the GNU General Public License
16 * along with this software.
18 * If not, see <http://www.gnu.org/licenses/>.
28 #include "build/build_config.h"
29 #include "build/debug.h"
31 #include "common/axis.h"
32 #include "common/maths.h"
33 #include "common/filter.h"
35 #include "config/config_reset.h"
37 #include "pg/pg_ids.h"
39 #include "drivers/sound_beeper.h"
40 #include "drivers/time.h"
42 #include "fc/fc_core.h"
45 #include "fc/rc_controls.h"
46 #include "fc/runtime_config.h"
48 #include "flight/pid.h"
49 #include "flight/imu.h"
50 #include "flight/gps_rescue.h"
51 #include "flight/mixer.h"
55 #include "sensors/gyro.h"
56 #include "sensors/acceleration.h"
59 FAST_RAM_ZERO_INIT
uint32_t targetPidLooptime
;
60 FAST_RAM_ZERO_INIT pidAxisData_t pidData
[XYZ_AXIS_COUNT
];
62 static FAST_RAM_ZERO_INIT
bool pidStabilisationEnabled
;
64 static FAST_RAM_ZERO_INIT
bool inCrashRecoveryMode
= false;
66 static FAST_RAM_ZERO_INIT
float dT
;
68 PG_REGISTER_WITH_RESET_TEMPLATE(pidConfig_t
, pidConfig
, PG_PID_CONFIG
, 2);
71 #define PID_PROCESS_DENOM_DEFAULT 1
72 #elif defined(USE_GYRO_SPI_MPU6000) || defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_ICM20689)
73 #define PID_PROCESS_DENOM_DEFAULT 4
75 #define PID_PROCESS_DENOM_DEFAULT 2
78 #ifdef USE_RUNAWAY_TAKEOFF
79 PG_RESET_TEMPLATE(pidConfig_t
, pidConfig
,
80 .pid_process_denom
= PID_PROCESS_DENOM_DEFAULT
,
81 .runaway_takeoff_prevention
= true,
82 .runaway_takeoff_deactivate_throttle
= 25, // throttle level % needed to accumulate deactivation time
83 .runaway_takeoff_deactivate_delay
= 500 // Accumulated time (in milliseconds) before deactivation in successful takeoff
86 PG_RESET_TEMPLATE(pidConfig_t
, pidConfig
,
87 .pid_process_denom
= PID_PROCESS_DENOM_DEFAULT
91 PG_REGISTER_ARRAY_WITH_RESET_FN(pidProfile_t
, MAX_PROFILE_COUNT
, pidProfiles
, PG_PID_PROFILE
, 3);
93 void resetPidProfile(pidProfile_t
*pidProfile
)
95 RESET_CONFIG(pidProfile_t
, pidProfile
,
97 [PID_ROLL
] = { 40, 40, 30 },
98 [PID_PITCH
] = { 58, 50, 35 },
99 [PID_YAW
] = { 70, 45, 20 },
100 [PID_ALT
] = { 50, 0, 0 },
101 [PID_POS
] = { 15, 0, 0 }, // POSHOLD_P * 100, POSHOLD_I * 100,
102 [PID_POSR
] = { 34, 14, 53 }, // POSHOLD_RATE_P * 10, POSHOLD_RATE_I * 100, POSHOLD_RATE_D * 1000,
103 [PID_NAVR
] = { 25, 33, 83 }, // NAV_P * 10, NAV_I * 100, NAV_D * 1000
104 [PID_LEVEL
] = { 50, 50, 75 },
105 [PID_MAG
] = { 40, 0, 0 },
106 [PID_VEL
] = { 55, 55, 75 }
109 .pidSumLimit
= PIDSUM_LIMIT
,
110 .pidSumLimitYaw
= PIDSUM_LIMIT_YAW
,
112 .dterm_lowpass_hz
= 100, // filtering ON by default
113 .dterm_lowpass2_hz
= 0, // second Dterm LPF OFF by default
114 .dterm_notch_hz
= 260,
115 .dterm_notch_cutoff
= 160,
116 .dterm_filter_type
= FILTER_BIQUAD
,
117 .itermWindupPointPercent
= 50,
118 .vbatPidCompensation
= 0,
119 .pidAtMinThrottle
= PID_STABILISATION_ON
,
120 .levelAngleLimit
= 55,
121 .setpointRelaxRatio
= 100,
122 .dtermSetpointWeight
= 0,
123 .yawRateAccelLimit
= 100,
125 .itermThrottleThreshold
= 350,
126 .itermAcceleratorGain
= 1000,
127 .crash_time
= 500, // ms
128 .crash_delay
= 0, // ms
129 .crash_recovery_angle
= 10, // degrees
130 .crash_recovery_rate
= 100, // degrees/second
131 .crash_dthreshold
= 50, // degrees/second/second
132 .crash_gthreshold
= 400, // degrees/second
133 .crash_setpoint_threshold
= 350, // degrees/second
134 .crash_recovery
= PID_CRASH_RECOVERY_OFF
, // off by default
135 .horizon_tilt_effect
= 75,
136 .horizon_tilt_expert_mode
= false,
137 .crash_limit_yaw
= 200,
140 .throttle_boost_cutoff
= 15,
141 .iterm_rotation
= false,
142 .smart_feedforward
= false,
143 .iterm_relax
= false,
144 .iterm_relax_cutoff_low
= 3,
145 .iterm_relax_cutoff_high
= 15,
149 void pgResetFn_pidProfiles(pidProfile_t
*pidProfiles
)
151 for (int i
= 0; i
< MAX_PROFILE_COUNT
; i
++) {
152 resetPidProfile(&pidProfiles
[i
]);
156 static void pidSetTargetLooptime(uint32_t pidLooptime
)
158 targetPidLooptime
= pidLooptime
;
159 dT
= (float)targetPidLooptime
* 0.000001f
;
162 void pidResetITerm(void)
164 for (int axis
= 0; axis
< 3; axis
++) {
165 pidData
[axis
].I
= 0.0f
;
169 static FAST_RAM
float itermAccelerator
= 1.0f
;
171 void pidSetItermAccelerator(float newItermAccelerator
)
173 itermAccelerator
= newItermAccelerator
;
176 float pidItermAccelerator(void)
178 return itermAccelerator
;
181 void pidStabilisationState(pidStabilisationState_e pidControllerState
)
183 pidStabilisationEnabled
= (pidControllerState
== PID_STABILISATION_ON
) ? true : false;
186 const angle_index_t rcAliasToAngleIndexMap
[] = { AI_ROLL
, AI_PITCH
};
188 typedef union dtermLowpass_u
{
189 pt1Filter_t pt1Filter
;
190 biquadFilter_t biquadFilter
;
191 #if defined(USE_FIR_FILTER_DENOISE)
192 firFilterDenoise_t denoisingFilter
;
196 static FAST_RAM_ZERO_INIT filterApplyFnPtr dtermNotchApplyFn
;
197 static FAST_RAM_ZERO_INIT biquadFilter_t dtermNotch
[2];
198 static FAST_RAM_ZERO_INIT filterApplyFnPtr dtermLowpassApplyFn
;
199 static FAST_RAM_ZERO_INIT dtermLowpass_t dtermLowpass
[2];
200 static FAST_RAM_ZERO_INIT filterApplyFnPtr dtermLowpass2ApplyFn
;
201 static FAST_RAM_ZERO_INIT pt1Filter_t dtermLowpass2
[2];
202 static FAST_RAM_ZERO_INIT filterApplyFnPtr ptermYawLowpassApplyFn
;
203 static FAST_RAM_ZERO_INIT pt1Filter_t ptermYawLowpass
;
204 static FAST_RAM_ZERO_INIT pt1Filter_t windupLpf
[3][2];
205 static FAST_RAM_ZERO_INIT
bool itermRelax
;
206 static FAST_RAM_ZERO_INIT
uint8_t itermRelaxCutoffLow
;
207 static FAST_RAM_ZERO_INIT
uint8_t itermRelaxCutoffHigh
;
209 void pidInitFilters(const pidProfile_t
*pidProfile
)
211 BUILD_BUG_ON(FD_YAW
!= 2); // only setting up Dterm filters on roll and pitch axes, so ensure yaw axis is 2
213 if (targetPidLooptime
== 0) {
214 // no looptime set, so set all the filters to null
215 dtermNotchApplyFn
= nullFilterApply
;
216 dtermLowpassApplyFn
= nullFilterApply
;
217 ptermYawLowpassApplyFn
= nullFilterApply
;
221 const uint32_t pidFrequencyNyquist
= (1.0f
/ dT
) / 2; // No rounding needed
223 uint16_t dTermNotchHz
;
224 if (pidProfile
->dterm_notch_hz
<= pidFrequencyNyquist
) {
225 dTermNotchHz
= pidProfile
->dterm_notch_hz
;
227 if (pidProfile
->dterm_notch_cutoff
< pidFrequencyNyquist
) {
228 dTermNotchHz
= pidFrequencyNyquist
;
234 if (dTermNotchHz
!= 0 && pidProfile
->dterm_notch_cutoff
!= 0) {
235 dtermNotchApplyFn
= (filterApplyFnPtr
)biquadFilterApply
;
236 const float notchQ
= filterGetNotchQ(dTermNotchHz
, pidProfile
->dterm_notch_cutoff
);
237 for (int axis
= FD_ROLL
; axis
<= FD_PITCH
; axis
++) {
238 biquadFilterInit(&dtermNotch
[axis
], dTermNotchHz
, targetPidLooptime
, notchQ
, FILTER_NOTCH
);
241 dtermNotchApplyFn
= nullFilterApply
;
244 //2nd Dterm Lowpass Filter
245 if (pidProfile
->dterm_lowpass2_hz
== 0 || pidProfile
->dterm_lowpass2_hz
> pidFrequencyNyquist
) {
246 dtermLowpass2ApplyFn
= nullFilterApply
;
248 dtermLowpass2ApplyFn
= (filterApplyFnPtr
)pt1FilterApply
;
249 for (int axis
= FD_ROLL
; axis
<= FD_PITCH
; axis
++) {
250 pt1FilterInit(&dtermLowpass2
[axis
], pt1FilterGain(pidProfile
->dterm_lowpass2_hz
, dT
));
254 if (pidProfile
->dterm_lowpass_hz
== 0 || pidProfile
->dterm_lowpass_hz
> pidFrequencyNyquist
) {
255 dtermLowpassApplyFn
= nullFilterApply
;
257 switch (pidProfile
->dterm_filter_type
) {
259 dtermLowpassApplyFn
= nullFilterApply
;
262 dtermLowpassApplyFn
= (filterApplyFnPtr
)pt1FilterApply
;
263 for (int axis
= FD_ROLL
; axis
<= FD_PITCH
; axis
++) {
264 pt1FilterInit(&dtermLowpass
[axis
].pt1Filter
, pt1FilterGain(pidProfile
->dterm_lowpass_hz
, dT
));
268 dtermLowpassApplyFn
= (filterApplyFnPtr
)biquadFilterApply
;
269 for (int axis
= FD_ROLL
; axis
<= FD_PITCH
; axis
++) {
270 biquadFilterInitLPF(&dtermLowpass
[axis
].biquadFilter
, pidProfile
->dterm_lowpass_hz
, targetPidLooptime
);
273 #if defined(USE_FIR_FILTER_DENOISE)
275 dtermLowpassApplyFn
= (filterApplyFnPtr
)firFilterDenoiseUpdate
;
276 for (int axis
= FD_ROLL
; axis
<= FD_PITCH
; axis
++) {
277 firFilterDenoiseInit(&dtermLowpass
[axis
].denoisingFilter
, pidProfile
->dterm_lowpass_hz
, targetPidLooptime
);
284 if (pidProfile
->yaw_lowpass_hz
== 0 || pidProfile
->yaw_lowpass_hz
> pidFrequencyNyquist
) {
285 ptermYawLowpassApplyFn
= nullFilterApply
;
287 ptermYawLowpassApplyFn
= (filterApplyFnPtr
)pt1FilterApply
;
288 pt1FilterInit(&ptermYawLowpass
, pt1FilterGain(pidProfile
->yaw_lowpass_hz
, dT
));
291 pt1FilterInit(&throttleLpf
, pt1FilterGain(pidProfile
->throttle_boost_cutoff
, dT
));
293 for (int i
= 0; i
< 3; i
++) {
294 pt1FilterInit(&windupLpf
[i
][0], pt1FilterGain(itermRelaxCutoffLow
, dT
));
295 pt1FilterInit(&windupLpf
[i
][1], pt1FilterGain(itermRelaxCutoffHigh
, dT
));
299 typedef struct pidCoefficient_s
{
305 static FAST_RAM_ZERO_INIT pidCoefficient_t pidCoefficient
[3];
306 static FAST_RAM_ZERO_INIT
float maxVelocity
[3];
307 static FAST_RAM_ZERO_INIT
float relaxFactor
;
308 static FAST_RAM_ZERO_INIT
float dtermSetpointWeight
;
309 static FAST_RAM_ZERO_INIT
float levelGain
, horizonGain
, horizonTransition
, horizonCutoffDegrees
, horizonFactorRatio
;
310 static FAST_RAM_ZERO_INIT
float ITermWindupPointInv
;
311 static FAST_RAM_ZERO_INIT
uint8_t horizonTiltExpertMode
;
312 static FAST_RAM_ZERO_INIT timeDelta_t crashTimeLimitUs
;
313 static FAST_RAM_ZERO_INIT timeDelta_t crashTimeDelayUs
;
314 static FAST_RAM_ZERO_INIT
int32_t crashRecoveryAngleDeciDegrees
;
315 static FAST_RAM_ZERO_INIT
float crashRecoveryRate
;
316 static FAST_RAM_ZERO_INIT
float crashDtermThreshold
;
317 static FAST_RAM_ZERO_INIT
float crashGyroThreshold
;
318 static FAST_RAM_ZERO_INIT
float crashSetpointThreshold
;
319 static FAST_RAM_ZERO_INIT
float crashLimitYaw
;
320 static FAST_RAM_ZERO_INIT
float itermLimit
;
321 FAST_RAM_ZERO_INIT
float throttleBoost
;
322 pt1Filter_t throttleLpf
;
323 static FAST_RAM_ZERO_INIT
bool itermRotation
;
324 static FAST_RAM_ZERO_INIT
bool smartFeedforward
;
326 void pidInitConfig(const pidProfile_t
*pidProfile
)
328 for (int axis
= FD_ROLL
; axis
<= FD_YAW
; axis
++) {
329 pidCoefficient
[axis
].Kp
= PTERM_SCALE
* pidProfile
->pid
[axis
].P
;
330 pidCoefficient
[axis
].Ki
= ITERM_SCALE
* pidProfile
->pid
[axis
].I
;
331 pidCoefficient
[axis
].Kd
= DTERM_SCALE
* pidProfile
->pid
[axis
].D
;
334 dtermSetpointWeight
= pidProfile
->dtermSetpointWeight
/ 127.0f
;
335 if (pidProfile
->setpointRelaxRatio
== 0) {
338 relaxFactor
= 100.0f
/ pidProfile
->setpointRelaxRatio
;
340 levelGain
= pidProfile
->pid
[PID_LEVEL
].P
/ 10.0f
;
341 horizonGain
= pidProfile
->pid
[PID_LEVEL
].I
/ 10.0f
;
342 horizonTransition
= (float)pidProfile
->pid
[PID_LEVEL
].D
;
343 horizonTiltExpertMode
= pidProfile
->horizon_tilt_expert_mode
;
344 horizonCutoffDegrees
= (175 - pidProfile
->horizon_tilt_effect
) * 1.8f
;
345 horizonFactorRatio
= (100 - pidProfile
->horizon_tilt_effect
) * 0.01f
;
346 maxVelocity
[FD_ROLL
] = maxVelocity
[FD_PITCH
] = pidProfile
->rateAccelLimit
* 100 * dT
;
347 maxVelocity
[FD_YAW
] = pidProfile
->yawRateAccelLimit
* 100 * dT
;
348 const float ITermWindupPoint
= (float)pidProfile
->itermWindupPointPercent
/ 100.0f
;
349 ITermWindupPointInv
= 1.0f
/ (1.0f
- ITermWindupPoint
);
350 crashTimeLimitUs
= pidProfile
->crash_time
* 1000;
351 crashTimeDelayUs
= pidProfile
->crash_delay
* 1000;
352 crashRecoveryAngleDeciDegrees
= pidProfile
->crash_recovery_angle
* 10;
353 crashRecoveryRate
= pidProfile
->crash_recovery_rate
;
354 crashGyroThreshold
= pidProfile
->crash_gthreshold
;
355 crashDtermThreshold
= pidProfile
->crash_dthreshold
;
356 crashSetpointThreshold
= pidProfile
->crash_setpoint_threshold
;
357 crashLimitYaw
= pidProfile
->crash_limit_yaw
;
358 itermLimit
= pidProfile
->itermLimit
;
359 throttleBoost
= pidProfile
->throttle_boost
* 0.1f
;
360 itermRotation
= pidProfile
->iterm_rotation
;
361 smartFeedforward
= pidProfile
->smart_feedforward
;
362 itermRelax
= pidProfile
->iterm_relax
;
363 itermRelaxCutoffLow
= pidProfile
->iterm_relax_cutoff_low
;
364 itermRelaxCutoffHigh
= pidProfile
->iterm_relax_cutoff_high
;
367 void pidInit(const pidProfile_t
*pidProfile
)
369 pidSetTargetLooptime(gyro
.targetLooptime
* pidConfig()->pid_process_denom
); // Initialize pid looptime
370 pidInitFilters(pidProfile
);
371 pidInitConfig(pidProfile
);
375 void pidCopyProfile(uint8_t dstPidProfileIndex
, uint8_t srcPidProfileIndex
)
377 if ((dstPidProfileIndex
< MAX_PROFILE_COUNT
-1 && srcPidProfileIndex
< MAX_PROFILE_COUNT
-1)
378 && dstPidProfileIndex
!= srcPidProfileIndex
380 memcpy(pidProfilesMutable(dstPidProfileIndex
), pidProfilesMutable(srcPidProfileIndex
), sizeof(pidProfile_t
));
384 // calculates strength of horizon leveling; 0 = none, 1.0 = most leveling
385 static float calcHorizonLevelStrength(void)
387 // start with 1.0 at center stick, 0.0 at max stick deflection:
388 float horizonLevelStrength
= 1.0f
- MAX(getRcDeflectionAbs(FD_ROLL
), getRcDeflectionAbs(FD_PITCH
));
390 // 0 at level, 90 at vertical, 180 at inverted (degrees):
391 const float currentInclination
= MAX(ABS(attitude
.values
.roll
), ABS(attitude
.values
.pitch
)) / 10.0f
;
393 // horizonTiltExpertMode: 0 = leveling always active when sticks centered,
394 // 1 = leveling can be totally off when inverted
395 if (horizonTiltExpertMode
) {
396 if (horizonTransition
> 0 && horizonCutoffDegrees
> 0) {
397 // if d_level > 0 and horizonTiltEffect < 175
398 // horizonCutoffDegrees: 0 to 125 => 270 to 90 (represents where leveling goes to zero)
399 // inclinationLevelRatio (0.0 to 1.0) is smaller (less leveling)
400 // for larger inclinations; 0.0 at horizonCutoffDegrees value:
401 const float inclinationLevelRatio
= constrainf((horizonCutoffDegrees
-currentInclination
) / horizonCutoffDegrees
, 0, 1);
402 // apply configured horizon sensitivity:
403 // when stick is near center (horizonLevelStrength ~= 1.0)
404 // H_sensitivity value has little effect,
405 // when stick is deflected (horizonLevelStrength near 0.0)
406 // H_sensitivity value has more effect:
407 horizonLevelStrength
= (horizonLevelStrength
- 1) * 100 / horizonTransition
+ 1;
408 // apply inclination ratio, which may lower leveling
409 // to zero regardless of stick position:
410 horizonLevelStrength
*= inclinationLevelRatio
;
411 } else { // d_level=0 or horizon_tilt_effect>=175 means no leveling
412 horizonLevelStrength
= 0;
414 } else { // horizon_tilt_expert_mode = 0 (leveling always active when sticks centered)
416 if (horizonFactorRatio
< 1.01f
) { // if horizonTiltEffect > 0
417 // horizonFactorRatio: 1.0 to 0.0 (larger means more leveling)
418 // inclinationLevelRatio (0.0 to 1.0) is smaller (less leveling)
419 // for larger inclinations, goes to 1.0 at inclination==level:
420 const float inclinationLevelRatio
= (180-currentInclination
)/180 * (1.0f
-horizonFactorRatio
) + horizonFactorRatio
;
421 // apply ratio to configured horizon sensitivity:
422 sensitFact
= horizonTransition
* inclinationLevelRatio
;
423 } else { // horizonTiltEffect=0 for "old" functionality
424 sensitFact
= horizonTransition
;
427 if (sensitFact
<= 0) { // zero means no leveling
428 horizonLevelStrength
= 0;
430 // when stick is near center (horizonLevelStrength ~= 1.0)
431 // sensitFact value has little effect,
432 // when stick is deflected (horizonLevelStrength near 0.0)
433 // sensitFact value has more effect:
434 horizonLevelStrength
= ((horizonLevelStrength
- 1) * (100 / sensitFact
)) + 1;
437 return constrainf(horizonLevelStrength
, 0, 1);
440 static float pidLevel(int axis
, const pidProfile_t
*pidProfile
, const rollAndPitchTrims_t
*angleTrim
, float currentPidSetpoint
) {
441 // calculate error angle and limit the angle to the max inclination
442 // rcDeflection is in range [-1.0, 1.0]
443 float angle
= pidProfile
->levelAngleLimit
* getRcDeflection(axis
);
444 #ifdef USE_GPS_RESCUE
445 angle
+= gpsRescueAngle
[axis
] / 100; // ANGLE IS IN CENTIDEGREES
447 angle
= constrainf(angle
, -pidProfile
->levelAngleLimit
, pidProfile
->levelAngleLimit
);
448 const float errorAngle
= angle
- ((attitude
.raw
[axis
] - angleTrim
->raw
[axis
]) / 10.0f
);
449 if (FLIGHT_MODE(ANGLE_MODE
) || FLIGHT_MODE(GPS_RESCUE_MODE
)) {
450 // ANGLE mode - control is angle based
451 currentPidSetpoint
= errorAngle
* levelGain
;
453 // HORIZON mode - mix of ANGLE and ACRO modes
454 // mix in errorAngle to currentPidSetpoint to add a little auto-level feel
455 const float horizonLevelStrength
= calcHorizonLevelStrength();
456 currentPidSetpoint
= currentPidSetpoint
+ (errorAngle
* horizonGain
* horizonLevelStrength
);
458 return currentPidSetpoint
;
461 static float accelerationLimit(int axis
, float currentPidSetpoint
)
463 static float previousSetpoint
[3];
464 const float currentVelocity
= currentPidSetpoint
- previousSetpoint
[axis
];
466 if (ABS(currentVelocity
) > maxVelocity
[axis
]) {
467 currentPidSetpoint
= (currentVelocity
> 0) ? previousSetpoint
[axis
] + maxVelocity
[axis
] : previousSetpoint
[axis
] - maxVelocity
[axis
];
470 previousSetpoint
[axis
] = currentPidSetpoint
;
471 return currentPidSetpoint
;
474 static timeUs_t crashDetectedAtUs
;
476 static void handleCrashRecovery(
477 const pidCrashRecovery_e crash_recovery
, const rollAndPitchTrims_t
*angleTrim
,
478 const int axis
, const timeUs_t currentTimeUs
, const float gyroRate
, float *currentPidSetpoint
, float *errorRate
)
480 if (inCrashRecoveryMode
&& cmpTimeUs(currentTimeUs
, crashDetectedAtUs
) > crashTimeDelayUs
) {
481 if (crash_recovery
== PID_CRASH_RECOVERY_BEEP
) {
484 if (axis
== FD_YAW
) {
485 *errorRate
= constrainf(*errorRate
, -crashLimitYaw
, crashLimitYaw
);
487 // on roll and pitch axes calculate currentPidSetpoint and errorRate to level the aircraft to recover from crash
488 if (sensors(SENSOR_ACC
)) {
489 // errorAngle is deviation from horizontal
490 const float errorAngle
= -(attitude
.raw
[axis
] - angleTrim
->raw
[axis
]) / 10.0f
;
491 *currentPidSetpoint
= errorAngle
* levelGain
;
492 *errorRate
= *currentPidSetpoint
- gyroRate
;
495 // reset ITerm, since accumulated error before crash is now meaningless
496 // and ITerm windup during crash recovery can be extreme, especially on yaw axis
497 pidData
[axis
].I
= 0.0f
;
498 if (cmpTimeUs(currentTimeUs
, crashDetectedAtUs
) > crashTimeLimitUs
499 || (getMotorMixRange() < 1.0f
500 && ABS(gyro
.gyroADCf
[FD_ROLL
]) < crashRecoveryRate
501 && ABS(gyro
.gyroADCf
[FD_PITCH
]) < crashRecoveryRate
502 && ABS(gyro
.gyroADCf
[FD_YAW
]) < crashRecoveryRate
)) {
503 if (sensors(SENSOR_ACC
)) {
504 // check aircraft nearly level
505 if (ABS(attitude
.raw
[FD_ROLL
] - angleTrim
->raw
[FD_ROLL
]) < crashRecoveryAngleDeciDegrees
506 && ABS(attitude
.raw
[FD_PITCH
] - angleTrim
->raw
[FD_PITCH
]) < crashRecoveryAngleDeciDegrees
) {
507 inCrashRecoveryMode
= false;
511 inCrashRecoveryMode
= false;
518 static void detectAndSetCrashRecovery(
519 const pidCrashRecovery_e crash_recovery
, const int axis
,
520 const timeUs_t currentTimeUs
, const float delta
, const float errorRate
)
522 // if crash recovery is on and accelerometer enabled and there is no gyro overflow, then check for a crash
523 // no point in trying to recover if the crash is so severe that the gyro overflows
524 if ((crash_recovery
|| FLIGHT_MODE(GPS_RESCUE_MODE
)) && !gyroOverflowDetected()) {
525 if (ARMING_FLAG(ARMED
)) {
526 if (getMotorMixRange() >= 1.0f
&& !inCrashRecoveryMode
527 && ABS(delta
) > crashDtermThreshold
528 && ABS(errorRate
) > crashGyroThreshold
529 && ABS(getSetpointRate(axis
)) < crashSetpointThreshold
) {
530 inCrashRecoveryMode
= true;
531 crashDetectedAtUs
= currentTimeUs
;
533 if (inCrashRecoveryMode
&& cmpTimeUs(currentTimeUs
, crashDetectedAtUs
) < crashTimeDelayUs
&& (ABS(errorRate
) < crashGyroThreshold
534 || ABS(getSetpointRate(axis
)) > crashSetpointThreshold
)) {
535 inCrashRecoveryMode
= false;
538 } else if (inCrashRecoveryMode
) {
539 inCrashRecoveryMode
= false;
545 static void handleItermRotation()
547 // rotate old I to the new coordinate system
548 const float gyroToAngle
= dT
* RAD
;
549 for (int i
= FD_ROLL
; i
<= FD_YAW
; i
++) {
550 int i_1
= (i
+ 1) % 3;
551 int i_2
= (i
+ 2) % 3;
552 float angle
= gyro
.gyroADCf
[i
] * gyroToAngle
;
553 float newPID_I_i_1
= pidData
[i_1
].I
+ pidData
[i_2
].I
* angle
;
554 pidData
[i_2
].I
-= pidData
[i_1
].I
* angle
;
555 pidData
[i_1
].I
= newPID_I_i_1
;
559 // Betaflight pid controller, which will be maintained in the future with additional features specialised for current (mini) multirotor usage.
560 // Based on 2DOF reference design (matlab)
561 void FAST_CODE
pidController(const pidProfile_t
*pidProfile
, const rollAndPitchTrims_t
*angleTrim
, timeUs_t currentTimeUs
)
563 static float previousGyroRateDterm
[2];
564 static float previousPidSetpoint
[2];
566 const float tpaFactor
= getThrottlePIDAttenuation();
567 const float motorMixRange
= getMotorMixRange();
569 #ifdef USE_YAW_SPIN_RECOVERY
570 const bool yawSpinActive
= gyroYawSpinDetected();
573 // Dynamic i component,
574 // gradually scale back integration when above windup point
575 const float dynCi
= MIN((1.0f
- motorMixRange
) * ITermWindupPointInv
, 1.0f
) * dT
* itermAccelerator
;
577 // Dynamic d component, enable 2-DOF PID controller only for rate mode
578 const float dynCd
= flightModeFlags
? 0.0f
: dtermSetpointWeight
;
580 // Precalculate gyro deta for D-term here, this allows loop unrolling
581 float gyroRateDterm
[2];
582 for (int axis
= FD_ROLL
; axis
< FD_YAW
; ++axis
) {
583 gyroRateDterm
[axis
] = dtermNotchApplyFn((filter_t
*) &dtermNotch
[axis
], gyro
.gyroADCf
[axis
]);
584 gyroRateDterm
[axis
] = dtermLowpassApplyFn((filter_t
*) &dtermLowpass
[axis
], gyroRateDterm
[axis
]);
585 gyroRateDterm
[axis
] = dtermLowpass2ApplyFn((filter_t
*) &dtermLowpass2
[axis
], gyroRateDterm
[axis
]);
589 handleItermRotation();
592 // ----------PID controller----------
593 for (int axis
= FD_ROLL
; axis
<= FD_YAW
; ++axis
) {
594 float currentPidSetpoint
= getSetpointRate(axis
);
595 if (maxVelocity
[axis
]) {
596 currentPidSetpoint
= accelerationLimit(axis
, currentPidSetpoint
);
598 // Yaw control is GYRO based, direct sticks control is applied to rate PID
599 if ((FLIGHT_MODE(ANGLE_MODE
) || FLIGHT_MODE(HORIZON_MODE
) || FLIGHT_MODE(GPS_RESCUE_MODE
)) && axis
!= YAW
) {
600 currentPidSetpoint
= pidLevel(axis
, pidProfile
, angleTrim
, currentPidSetpoint
);
603 // Handle yaw spin recovery - zero the setpoint on yaw to aid in recovery
604 // It's not necessary to zero the set points for R/P because the PIDs will be zeroed below
605 #ifdef USE_YAW_SPIN_RECOVERY
606 if ((axis
== FD_YAW
) && yawSpinActive
) {
607 currentPidSetpoint
= 0.0f
;
609 #endif // USE_YAW_SPIN_RECOVERY
611 // -----calculate error rate
612 const float gyroRate
= gyro
.gyroADCf
[axis
]; // Process variable from gyro output in deg/sec
613 float errorRate
= currentPidSetpoint
- gyroRate
; // r - y
616 pidProfile
->crash_recovery
, angleTrim
, axis
, currentTimeUs
, gyroRate
,
617 ¤tPidSetpoint
, &errorRate
);
619 // --------low-level gyro-based PID based on 2DOF PID controller. ----------
620 // 2-DOF PID controller with optional filter on derivative term.
621 // b = 1 and only c (dtermSetpointWeight) can be tuned (amount derivative on measurement or error).
623 // -----calculate P component and add Dynamic Part based on stick input
624 pidData
[axis
].P
= pidCoefficient
[axis
].Kp
* errorRate
* tpaFactor
;
625 if (axis
== FD_YAW
) {
626 pidData
[axis
].P
= ptermYawLowpassApplyFn((filter_t
*) &ptermYawLowpass
, pidData
[axis
].P
);
629 // -----calculate I component
630 float itermErrorRate
;
632 const float gyroTargetHigh
= pt1FilterApply(&windupLpf
[axis
][0], currentPidSetpoint
);
633 const float gyroTargetLow
= pt1FilterApply(&windupLpf
[axis
][1], currentPidSetpoint
);
634 DEBUG_SET(DEBUG_ITERM_RELAX
, 0, gyroTargetHigh
);
635 DEBUG_SET(DEBUG_ITERM_RELAX
, 1, gyroTargetLow
);
636 const float gmax
= MAX(gyroTargetHigh
, gyroTargetLow
);
637 const float gmin
= MIN(gyroTargetHigh
, gyroTargetLow
);
638 if (gyroRate
>= gmin
&& gyroRate
<= gmax
)
639 itermErrorRate
= 0.0f
;
641 itermErrorRate
= (gyroRate
> gmax
? gmax
: gmin
) - gyroRate
;
643 else itermErrorRate
= errorRate
;
645 const float ITerm
= pidData
[axis
].I
;
646 const float ITermNew
= constrainf(ITerm
+ pidCoefficient
[axis
].Ki
* itermErrorRate
* dynCi
, -itermLimit
, itermLimit
);
647 const bool outputSaturated
= mixerIsOutputSaturated(axis
, errorRate
);
648 if (outputSaturated
== false || ABS(ITermNew
) < ABS(ITerm
)) {
649 // Only increase ITerm if output is not saturated
650 pidData
[axis
].I
= ITermNew
;
653 // -----calculate D component
654 if (axis
!= FD_YAW
) {
655 // no transition if relaxFactor == 0
656 float transition
= relaxFactor
> 0 ? MIN(1.f
, getRcDeflectionAbs(axis
) * relaxFactor
) : 1;
658 // Divide rate change by dT to get differential (ie dr/dt).
659 // dT is fixed and calculated from the target PID loop time
660 // This is done to avoid DTerm spikes that occur with dynamically
661 // calculated deltaT whenever another task causes the PID
662 // loop execution to be delayed.
664 - (gyroRateDterm
[axis
] - previousGyroRateDterm
[axis
]) / dT
;
666 detectAndSetCrashRecovery(pidProfile
->crash_recovery
, axis
, currentTimeUs
, delta
, errorRate
);
668 pidData
[axis
].D
= pidCoefficient
[axis
].Kd
* delta
* tpaFactor
;
670 const float pidFeedForward
=
671 pidCoefficient
[axis
].Kd
* dynCd
* transition
*
672 (currentPidSetpoint
- previousPidSetpoint
[axis
]) * tpaFactor
/ dT
;
673 bool addFeedforward
= true;
674 if (smartFeedforward
) {
675 if (pidData
[axis
].P
* pidFeedForward
> 0) {
676 if (ABS(pidFeedForward
) > ABS(pidData
[axis
].P
)) {
680 addFeedforward
= false;
684 if (addFeedforward
) {
685 pidData
[axis
].D
+= pidFeedForward
;
687 previousGyroRateDterm
[axis
] = gyroRateDterm
[axis
];
688 previousPidSetpoint
[axis
] = currentPidSetpoint
;
690 #ifdef USE_YAW_SPIN_RECOVERY
692 // zero PIDs on pitch and roll leaving yaw P to correct spin
697 #endif // USE_YAW_SPIN_RECOVERY
701 // calculating the PID sum
702 pidData
[FD_ROLL
].Sum
= pidData
[FD_ROLL
].P
+ pidData
[FD_ROLL
].I
+ pidData
[FD_ROLL
].D
;
703 pidData
[FD_PITCH
].Sum
= pidData
[FD_PITCH
].P
+ pidData
[FD_PITCH
].I
+ pidData
[FD_PITCH
].D
;
705 #ifdef USE_YAW_SPIN_RECOVERY
707 // yaw P alone to correct spin
708 pidData
[FD_YAW
].I
= 0;
710 #endif // USE_YAW_SPIN_RECOVERY
713 pidData
[FD_YAW
].Sum
= pidData
[FD_YAW
].P
+ pidData
[FD_YAW
].I
;
715 // Disable PID control if at zero throttle or if gyro overflow detected
716 // This may look very innefficient, but it is done on purpose to always show real CPU usage as in flight
717 if (!pidStabilisationEnabled
|| gyroOverflowDetected()) {
718 for (int axis
= FD_ROLL
; axis
<= FD_YAW
; ++axis
) {
723 pidData
[axis
].Sum
= 0;
728 bool crashRecoveryModeActive(void)
730 return inCrashRecoveryMode
;