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
;
67 static FAST_RAM_ZERO_INIT
float pidFrequency
;
69 PG_REGISTER_WITH_RESET_TEMPLATE(pidConfig_t
, pidConfig
, PG_PID_CONFIG
, 2);
72 #define PID_PROCESS_DENOM_DEFAULT 1
73 #elif defined(USE_GYRO_SPI_MPU6000) || defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_ICM20689)
74 #define PID_PROCESS_DENOM_DEFAULT 4
76 #define PID_PROCESS_DENOM_DEFAULT 2
79 #ifdef USE_RUNAWAY_TAKEOFF
80 PG_RESET_TEMPLATE(pidConfig_t
, pidConfig
,
81 .pid_process_denom
= PID_PROCESS_DENOM_DEFAULT
,
82 .runaway_takeoff_prevention
= true,
83 .runaway_takeoff_deactivate_throttle
= 25, // throttle level % needed to accumulate deactivation time
84 .runaway_takeoff_deactivate_delay
= 500 // Accumulated time (in milliseconds) before deactivation in successful takeoff
87 PG_RESET_TEMPLATE(pidConfig_t
, pidConfig
,
88 .pid_process_denom
= PID_PROCESS_DENOM_DEFAULT
92 #ifdef USE_ACRO_TRAINER
93 #define ACRO_TRAINER_LOOKAHEAD_RATE_LIMIT 500.0f // Max gyro rate for lookahead time scaling
94 #define ACRO_TRAINER_SETPOINT_LIMIT 1000.0f // Limit the correcting setpoint
95 #endif // USE_ACRO_TRAINER
97 PG_REGISTER_ARRAY_WITH_RESET_FN(pidProfile_t
, MAX_PROFILE_COUNT
, pidProfiles
, PG_PID_PROFILE
, 4);
99 void resetPidProfile(pidProfile_t
*pidProfile
)
101 RESET_CONFIG(pidProfile_t
, pidProfile
,
103 [PID_ROLL
] = { 46, 45, 25 },
104 [PID_PITCH
] = { 50, 50, 27 },
105 [PID_YAW
] = { 65, 45, 0 },
106 [PID_ALT
] = { 50, 0, 0 },
107 [PID_POS
] = { 15, 0, 0 }, // POSHOLD_P * 100, POSHOLD_I * 100,
108 [PID_POSR
] = { 34, 14, 53 }, // POSHOLD_RATE_P * 10, POSHOLD_RATE_I * 100, POSHOLD_RATE_D * 1000,
109 [PID_NAVR
] = { 25, 33, 83 }, // NAV_P * 10, NAV_I * 100, NAV_D * 1000
110 [PID_LEVEL
] = { 50, 50, 75 },
111 [PID_MAG
] = { 40, 0, 0 },
112 [PID_VEL
] = { 55, 55, 75 }
115 .pidSumLimit
= PIDSUM_LIMIT
,
116 .pidSumLimitYaw
= PIDSUM_LIMIT_YAW
,
118 .dterm_lowpass_hz
= 100, // dual PT1 filtering ON by default
119 .dterm_lowpass2_hz
= 200, // second Dterm LPF ON by default
121 .dterm_notch_cutoff
= 160,
122 .dterm_filter_type
= FILTER_PT1
,
123 .itermWindupPointPercent
= 40,
124 .vbatPidCompensation
= 0,
125 .pidAtMinThrottle
= PID_STABILISATION_ON
,
126 .levelAngleLimit
= 55,
127 .setpointRelaxRatio
= 0,
128 .dtermSetpointWeight
= 60,
129 .yawRateAccelLimit
= 100,
131 .itermThrottleThreshold
= 350,
132 .itermAcceleratorGain
= 5000,
133 .crash_time
= 500, // ms
134 .crash_delay
= 0, // ms
135 .crash_recovery_angle
= 10, // degrees
136 .crash_recovery_rate
= 100, // degrees/second
137 .crash_dthreshold
= 50, // degrees/second/second
138 .crash_gthreshold
= 400, // degrees/second
139 .crash_setpoint_threshold
= 350, // degrees/second
140 .crash_recovery
= PID_CRASH_RECOVERY_OFF
, // off by default
141 .horizon_tilt_effect
= 75,
142 .horizon_tilt_expert_mode
= false,
143 .crash_limit_yaw
= 200,
146 .throttle_boost_cutoff
= 15,
147 .iterm_rotation
= true,
148 .smart_feedforward
= false,
149 .iterm_relax
= ITERM_RELAX_OFF
,
150 .iterm_relax_cutoff
= 11,
151 .iterm_relax_type
= ITERM_RELAX_GYRO
,
152 .acro_trainer_angle_limit
= 20,
153 .acro_trainer_lookahead_ms
= 50,
154 .acro_trainer_debug_axis
= FD_ROLL
,
155 .acro_trainer_gain
= 75,
156 .abs_control_gain
= 0,
157 .abs_control_limit
= 90,
158 .abs_control_error_limit
= 20,
162 void pgResetFn_pidProfiles(pidProfile_t
*pidProfiles
)
164 for (int i
= 0; i
< MAX_PROFILE_COUNT
; i
++) {
165 resetPidProfile(&pidProfiles
[i
]);
169 static void pidSetTargetLooptime(uint32_t pidLooptime
)
171 targetPidLooptime
= pidLooptime
;
172 dT
= targetPidLooptime
* 1e-6f
;
173 pidFrequency
= 1.0f
/ dT
;
176 static FAST_RAM
float itermAccelerator
= 1.0f
;
178 void pidSetItermAccelerator(float newItermAccelerator
)
180 itermAccelerator
= newItermAccelerator
;
183 float pidItermAccelerator(void)
185 return itermAccelerator
;
188 void pidStabilisationState(pidStabilisationState_e pidControllerState
)
190 pidStabilisationEnabled
= (pidControllerState
== PID_STABILISATION_ON
) ? true : false;
193 const angle_index_t rcAliasToAngleIndexMap
[] = { AI_ROLL
, AI_PITCH
};
195 typedef union dtermLowpass_u
{
196 pt1Filter_t pt1Filter
;
197 biquadFilter_t biquadFilter
;
200 static FAST_RAM_ZERO_INIT filterApplyFnPtr dtermNotchApplyFn
;
201 static FAST_RAM_ZERO_INIT biquadFilter_t dtermNotch
[2];
202 static FAST_RAM_ZERO_INIT filterApplyFnPtr dtermLowpassApplyFn
;
203 static FAST_RAM_ZERO_INIT dtermLowpass_t dtermLowpass
[2];
204 static FAST_RAM_ZERO_INIT filterApplyFnPtr dtermLowpass2ApplyFn
;
205 static FAST_RAM_ZERO_INIT pt1Filter_t dtermLowpass2
[2];
206 static FAST_RAM_ZERO_INIT filterApplyFnPtr ptermYawLowpassApplyFn
;
207 static FAST_RAM_ZERO_INIT pt1Filter_t ptermYawLowpass
;
208 #if defined(USE_ITERM_RELAX)
209 static FAST_RAM_ZERO_INIT pt1Filter_t windupLpf
[XYZ_AXIS_COUNT
];
210 static FAST_RAM_ZERO_INIT
uint8_t itermRelax
;
211 static FAST_RAM_ZERO_INIT
uint8_t itermRelaxType
;
212 static FAST_RAM_ZERO_INIT
uint8_t itermRelaxCutoff
;
215 #ifdef USE_RC_SMOOTHING_FILTER
216 static FAST_RAM_ZERO_INIT pt1Filter_t setpointDerivativePt1
[2];
217 static FAST_RAM_ZERO_INIT biquadFilter_t setpointDerivativeBiquad
[2];
218 static FAST_RAM_ZERO_INIT
bool setpointDerivativeLpfInitialized
;
219 static FAST_RAM_ZERO_INIT
uint8_t rcSmoothingDebugAxis
;
220 static FAST_RAM_ZERO_INIT
uint8_t rcSmoothingFilterType
;
221 #endif // USE_RC_SMOOTHING_FILTER
223 void pidInitFilters(const pidProfile_t
*pidProfile
)
225 BUILD_BUG_ON(FD_YAW
!= 2); // only setting up Dterm filters on roll and pitch axes, so ensure yaw axis is 2
227 if (targetPidLooptime
== 0) {
228 // no looptime set, so set all the filters to null
229 dtermNotchApplyFn
= nullFilterApply
;
230 dtermLowpassApplyFn
= nullFilterApply
;
231 ptermYawLowpassApplyFn
= nullFilterApply
;
235 const uint32_t pidFrequencyNyquist
= pidFrequency
/ 2; // No rounding needed
237 uint16_t dTermNotchHz
;
238 if (pidProfile
->dterm_notch_hz
<= pidFrequencyNyquist
) {
239 dTermNotchHz
= pidProfile
->dterm_notch_hz
;
241 if (pidProfile
->dterm_notch_cutoff
< pidFrequencyNyquist
) {
242 dTermNotchHz
= pidFrequencyNyquist
;
248 if (dTermNotchHz
!= 0 && pidProfile
->dterm_notch_cutoff
!= 0) {
249 dtermNotchApplyFn
= (filterApplyFnPtr
)biquadFilterApply
;
250 const float notchQ
= filterGetNotchQ(dTermNotchHz
, pidProfile
->dterm_notch_cutoff
);
251 for (int axis
= FD_ROLL
; axis
<= FD_PITCH
; axis
++) {
252 biquadFilterInit(&dtermNotch
[axis
], dTermNotchHz
, targetPidLooptime
, notchQ
, FILTER_NOTCH
);
255 dtermNotchApplyFn
= nullFilterApply
;
258 //2nd Dterm Lowpass Filter
259 if (pidProfile
->dterm_lowpass2_hz
== 0 || pidProfile
->dterm_lowpass2_hz
> pidFrequencyNyquist
) {
260 dtermLowpass2ApplyFn
= nullFilterApply
;
262 dtermLowpass2ApplyFn
= (filterApplyFnPtr
)pt1FilterApply
;
263 for (int axis
= FD_ROLL
; axis
<= FD_PITCH
; axis
++) {
264 pt1FilterInit(&dtermLowpass2
[axis
], pt1FilterGain(pidProfile
->dterm_lowpass2_hz
, dT
));
268 if (pidProfile
->dterm_lowpass_hz
== 0 || pidProfile
->dterm_lowpass_hz
> pidFrequencyNyquist
) {
269 dtermLowpassApplyFn
= nullFilterApply
;
271 switch (pidProfile
->dterm_filter_type
) {
273 dtermLowpassApplyFn
= nullFilterApply
;
276 dtermLowpassApplyFn
= (filterApplyFnPtr
)pt1FilterApply
;
277 for (int axis
= FD_ROLL
; axis
<= FD_PITCH
; axis
++) {
278 pt1FilterInit(&dtermLowpass
[axis
].pt1Filter
, pt1FilterGain(pidProfile
->dterm_lowpass_hz
, dT
));
282 dtermLowpassApplyFn
= (filterApplyFnPtr
)biquadFilterApply
;
283 for (int axis
= FD_ROLL
; axis
<= FD_PITCH
; axis
++) {
284 biquadFilterInitLPF(&dtermLowpass
[axis
].biquadFilter
, pidProfile
->dterm_lowpass_hz
, targetPidLooptime
);
290 if (pidProfile
->yaw_lowpass_hz
== 0 || pidProfile
->yaw_lowpass_hz
> pidFrequencyNyquist
) {
291 ptermYawLowpassApplyFn
= nullFilterApply
;
293 ptermYawLowpassApplyFn
= (filterApplyFnPtr
)pt1FilterApply
;
294 pt1FilterInit(&ptermYawLowpass
, pt1FilterGain(pidProfile
->yaw_lowpass_hz
, dT
));
297 #if defined(USE_THROTTLE_BOOST)
298 pt1FilterInit(&throttleLpf
, pt1FilterGain(pidProfile
->throttle_boost_cutoff
, dT
));
300 #if defined(USE_ITERM_RELAX)
302 for (int i
= 0; i
< XYZ_AXIS_COUNT
; i
++) {
303 pt1FilterInit(&windupLpf
[i
], pt1FilterGain(itermRelaxCutoff
, dT
));
309 #ifdef USE_RC_SMOOTHING_FILTER
310 void pidInitSetpointDerivativeLpf(uint16_t filterCutoff
, uint8_t debugAxis
, uint8_t filterType
)
312 rcSmoothingDebugAxis
= debugAxis
;
313 rcSmoothingFilterType
= filterType
;
314 if ((filterCutoff
> 0) && (rcSmoothingFilterType
!= RC_SMOOTHING_DERIVATIVE_OFF
)) {
315 setpointDerivativeLpfInitialized
= true;
316 for (int axis
= FD_ROLL
; axis
<= FD_PITCH
; axis
++) {
317 switch (rcSmoothingFilterType
) {
318 case RC_SMOOTHING_DERIVATIVE_PT1
:
319 pt1FilterInit(&setpointDerivativePt1
[axis
], pt1FilterGain(filterCutoff
, dT
));
321 case RC_SMOOTHING_DERIVATIVE_BIQUAD
:
322 biquadFilterInitLPF(&setpointDerivativeBiquad
[axis
], filterCutoff
, targetPidLooptime
);
329 void pidUpdateSetpointDerivativeLpf(uint16_t filterCutoff
)
331 if ((filterCutoff
> 0) && (rcSmoothingFilterType
!= RC_SMOOTHING_DERIVATIVE_OFF
)) {
332 for (int axis
= FD_ROLL
; axis
<= FD_PITCH
; axis
++) {
333 switch (rcSmoothingFilterType
) {
334 case RC_SMOOTHING_DERIVATIVE_PT1
:
335 pt1FilterUpdateCutoff(&setpointDerivativePt1
[axis
], pt1FilterGain(filterCutoff
, dT
));
337 case RC_SMOOTHING_DERIVATIVE_BIQUAD
:
338 biquadFilterUpdateLPF(&setpointDerivativeBiquad
[axis
], filterCutoff
, targetPidLooptime
);
344 #endif // USE_RC_SMOOTHING_FILTER
346 typedef struct pidCoefficient_s
{
352 static FAST_RAM_ZERO_INIT pidCoefficient_t pidCoefficient
[3];
353 static FAST_RAM_ZERO_INIT
float maxVelocity
[3];
354 static FAST_RAM_ZERO_INIT
float relaxFactor
;
355 static FAST_RAM_ZERO_INIT
float dtermSetpointWeight
;
356 static FAST_RAM_ZERO_INIT
float levelGain
, horizonGain
, horizonTransition
, horizonCutoffDegrees
, horizonFactorRatio
;
357 static FAST_RAM_ZERO_INIT
float ITermWindupPointInv
;
358 static FAST_RAM_ZERO_INIT
uint8_t horizonTiltExpertMode
;
359 static FAST_RAM_ZERO_INIT timeDelta_t crashTimeLimitUs
;
360 static FAST_RAM_ZERO_INIT timeDelta_t crashTimeDelayUs
;
361 static FAST_RAM_ZERO_INIT
int32_t crashRecoveryAngleDeciDegrees
;
362 static FAST_RAM_ZERO_INIT
float crashRecoveryRate
;
363 static FAST_RAM_ZERO_INIT
float crashDtermThreshold
;
364 static FAST_RAM_ZERO_INIT
float crashGyroThreshold
;
365 static FAST_RAM_ZERO_INIT
float crashSetpointThreshold
;
366 static FAST_RAM_ZERO_INIT
float crashLimitYaw
;
367 static FAST_RAM_ZERO_INIT
float itermLimit
;
368 #if defined(USE_THROTTLE_BOOST)
369 FAST_RAM_ZERO_INIT
float throttleBoost
;
370 pt1Filter_t throttleLpf
;
372 static FAST_RAM_ZERO_INIT
bool itermRotation
;
374 #if defined(USE_SMART_FEEDFORWARD)
375 static FAST_RAM_ZERO_INIT
bool smartFeedforward
;
377 #if defined(USE_ABSOLUTE_CONTROL)
378 static FAST_RAM_ZERO_INIT
float axisError
[XYZ_AXIS_COUNT
];
379 static FAST_RAM_ZERO_INIT
float acGain
;
380 static FAST_RAM_ZERO_INIT
float acLimit
;
381 static FAST_RAM_ZERO_INIT
float acErrorLimit
;
384 void pidResetITerm(void)
386 for (int axis
= 0; axis
< 3; axis
++) {
387 pidData
[axis
].I
= 0.0f
;
388 #if defined(USE_ABSOLUTE_CONTROL)
389 axisError
[axis
] = 0.0f
;
394 #ifdef USE_ACRO_TRAINER
395 static FAST_RAM_ZERO_INIT
float acroTrainerAngleLimit
;
396 static FAST_RAM_ZERO_INIT
float acroTrainerLookaheadTime
;
397 static FAST_RAM_ZERO_INIT
uint8_t acroTrainerDebugAxis
;
398 static FAST_RAM_ZERO_INIT
bool acroTrainerActive
;
399 static FAST_RAM_ZERO_INIT
int acroTrainerAxisState
[2]; // only need roll and pitch
400 static FAST_RAM_ZERO_INIT
float acroTrainerGain
;
401 #endif // USE_ACRO_TRAINER
403 void pidInitConfig(const pidProfile_t
*pidProfile
)
405 for (int axis
= FD_ROLL
; axis
<= FD_YAW
; axis
++) {
406 pidCoefficient
[axis
].Kp
= PTERM_SCALE
* pidProfile
->pid
[axis
].P
;
407 pidCoefficient
[axis
].Ki
= ITERM_SCALE
* pidProfile
->pid
[axis
].I
;
408 pidCoefficient
[axis
].Kd
= DTERM_SCALE
* pidProfile
->pid
[axis
].D
;
411 dtermSetpointWeight
= pidProfile
->dtermSetpointWeight
/ 100.0f
;
412 if (pidProfile
->setpointRelaxRatio
== 0) {
415 relaxFactor
= 100.0f
/ pidProfile
->setpointRelaxRatio
;
417 levelGain
= pidProfile
->pid
[PID_LEVEL
].P
/ 10.0f
;
418 horizonGain
= pidProfile
->pid
[PID_LEVEL
].I
/ 10.0f
;
419 horizonTransition
= (float)pidProfile
->pid
[PID_LEVEL
].D
;
420 horizonTiltExpertMode
= pidProfile
->horizon_tilt_expert_mode
;
421 horizonCutoffDegrees
= (175 - pidProfile
->horizon_tilt_effect
) * 1.8f
;
422 horizonFactorRatio
= (100 - pidProfile
->horizon_tilt_effect
) * 0.01f
;
423 maxVelocity
[FD_ROLL
] = maxVelocity
[FD_PITCH
] = pidProfile
->rateAccelLimit
* 100 * dT
;
424 maxVelocity
[FD_YAW
] = pidProfile
->yawRateAccelLimit
* 100 * dT
;
425 const float ITermWindupPoint
= (float)pidProfile
->itermWindupPointPercent
/ 100.0f
;
426 ITermWindupPointInv
= 1.0f
/ (1.0f
- ITermWindupPoint
);
427 crashTimeLimitUs
= pidProfile
->crash_time
* 1000;
428 crashTimeDelayUs
= pidProfile
->crash_delay
* 1000;
429 crashRecoveryAngleDeciDegrees
= pidProfile
->crash_recovery_angle
* 10;
430 crashRecoveryRate
= pidProfile
->crash_recovery_rate
;
431 crashGyroThreshold
= pidProfile
->crash_gthreshold
;
432 crashDtermThreshold
= pidProfile
->crash_dthreshold
;
433 crashSetpointThreshold
= pidProfile
->crash_setpoint_threshold
;
434 crashLimitYaw
= pidProfile
->crash_limit_yaw
;
435 itermLimit
= pidProfile
->itermLimit
;
436 #if defined(USE_THROTTLE_BOOST)
437 throttleBoost
= pidProfile
->throttle_boost
* 0.1f
;
439 itermRotation
= pidProfile
->iterm_rotation
;
440 #if defined(USE_SMART_FEEDFORWARD)
441 smartFeedforward
= pidProfile
->smart_feedforward
;
443 #if defined(USE_ITERM_RELAX)
444 itermRelax
= pidProfile
->iterm_relax
;
445 itermRelaxType
= pidProfile
->iterm_relax_type
;
446 itermRelaxCutoff
= pidProfile
->iterm_relax_cutoff
;
449 #ifdef USE_ACRO_TRAINER
450 acroTrainerAngleLimit
= pidProfile
->acro_trainer_angle_limit
;
451 acroTrainerLookaheadTime
= (float)pidProfile
->acro_trainer_lookahead_ms
/ 1000.0f
;
452 acroTrainerDebugAxis
= pidProfile
->acro_trainer_debug_axis
;
453 acroTrainerGain
= (float)pidProfile
->acro_trainer_gain
/ 10.0f
;
454 #endif // USE_ACRO_TRAINER
456 #if defined(USE_ABSOLUTE_CONTROL)
457 acGain
= (float)pidProfile
->abs_control_gain
;
458 acLimit
= (float)pidProfile
->abs_control_limit
;
459 acErrorLimit
= (float)pidProfile
->abs_control_error_limit
;
463 void pidInit(const pidProfile_t
*pidProfile
)
465 pidSetTargetLooptime(gyro
.targetLooptime
* pidConfig()->pid_process_denom
); // Initialize pid looptime
466 pidInitFilters(pidProfile
);
467 pidInitConfig(pidProfile
);
470 #ifdef USE_ACRO_TRAINER
471 void pidAcroTrainerInit(void)
473 acroTrainerAxisState
[FD_ROLL
] = 0;
474 acroTrainerAxisState
[FD_PITCH
] = 0;
476 #endif // USE_ACRO_TRAINER
478 void pidCopyProfile(uint8_t dstPidProfileIndex
, uint8_t srcPidProfileIndex
)
480 if ((dstPidProfileIndex
< MAX_PROFILE_COUNT
-1 && srcPidProfileIndex
< MAX_PROFILE_COUNT
-1)
481 && dstPidProfileIndex
!= srcPidProfileIndex
483 memcpy(pidProfilesMutable(dstPidProfileIndex
), pidProfilesMutable(srcPidProfileIndex
), sizeof(pidProfile_t
));
487 // calculates strength of horizon leveling; 0 = none, 1.0 = most leveling
488 static float calcHorizonLevelStrength(void)
490 // start with 1.0 at center stick, 0.0 at max stick deflection:
491 float horizonLevelStrength
= 1.0f
- MAX(getRcDeflectionAbs(FD_ROLL
), getRcDeflectionAbs(FD_PITCH
));
493 // 0 at level, 90 at vertical, 180 at inverted (degrees):
494 const float currentInclination
= MAX(ABS(attitude
.values
.roll
), ABS(attitude
.values
.pitch
)) / 10.0f
;
496 // horizonTiltExpertMode: 0 = leveling always active when sticks centered,
497 // 1 = leveling can be totally off when inverted
498 if (horizonTiltExpertMode
) {
499 if (horizonTransition
> 0 && horizonCutoffDegrees
> 0) {
500 // if d_level > 0 and horizonTiltEffect < 175
501 // horizonCutoffDegrees: 0 to 125 => 270 to 90 (represents where leveling goes to zero)
502 // inclinationLevelRatio (0.0 to 1.0) is smaller (less leveling)
503 // for larger inclinations; 0.0 at horizonCutoffDegrees value:
504 const float inclinationLevelRatio
= constrainf((horizonCutoffDegrees
-currentInclination
) / horizonCutoffDegrees
, 0, 1);
505 // apply configured horizon sensitivity:
506 // when stick is near center (horizonLevelStrength ~= 1.0)
507 // H_sensitivity value has little effect,
508 // when stick is deflected (horizonLevelStrength near 0.0)
509 // H_sensitivity value has more effect:
510 horizonLevelStrength
= (horizonLevelStrength
- 1) * 100 / horizonTransition
+ 1;
511 // apply inclination ratio, which may lower leveling
512 // to zero regardless of stick position:
513 horizonLevelStrength
*= inclinationLevelRatio
;
514 } else { // d_level=0 or horizon_tilt_effect>=175 means no leveling
515 horizonLevelStrength
= 0;
517 } else { // horizon_tilt_expert_mode = 0 (leveling always active when sticks centered)
519 if (horizonFactorRatio
< 1.01f
) { // if horizonTiltEffect > 0
520 // horizonFactorRatio: 1.0 to 0.0 (larger means more leveling)
521 // inclinationLevelRatio (0.0 to 1.0) is smaller (less leveling)
522 // for larger inclinations, goes to 1.0 at inclination==level:
523 const float inclinationLevelRatio
= (180-currentInclination
)/180 * (1.0f
-horizonFactorRatio
) + horizonFactorRatio
;
524 // apply ratio to configured horizon sensitivity:
525 sensitFact
= horizonTransition
* inclinationLevelRatio
;
526 } else { // horizonTiltEffect=0 for "old" functionality
527 sensitFact
= horizonTransition
;
530 if (sensitFact
<= 0) { // zero means no leveling
531 horizonLevelStrength
= 0;
533 // when stick is near center (horizonLevelStrength ~= 1.0)
534 // sensitFact value has little effect,
535 // when stick is deflected (horizonLevelStrength near 0.0)
536 // sensitFact value has more effect:
537 horizonLevelStrength
= ((horizonLevelStrength
- 1) * (100 / sensitFact
)) + 1;
540 return constrainf(horizonLevelStrength
, 0, 1);
543 static float pidLevel(int axis
, const pidProfile_t
*pidProfile
, const rollAndPitchTrims_t
*angleTrim
, float currentPidSetpoint
) {
544 // calculate error angle and limit the angle to the max inclination
545 // rcDeflection is in range [-1.0, 1.0]
546 float angle
= pidProfile
->levelAngleLimit
* getRcDeflection(axis
);
547 #ifdef USE_GPS_RESCUE
548 angle
+= gpsRescueAngle
[axis
] / 100; // ANGLE IS IN CENTIDEGREES
550 angle
= constrainf(angle
, -pidProfile
->levelAngleLimit
, pidProfile
->levelAngleLimit
);
551 const float errorAngle
= angle
- ((attitude
.raw
[axis
] - angleTrim
->raw
[axis
]) / 10.0f
);
552 if (FLIGHT_MODE(ANGLE_MODE
) || FLIGHT_MODE(GPS_RESCUE_MODE
)) {
553 // ANGLE mode - control is angle based
554 currentPidSetpoint
= errorAngle
* levelGain
;
556 // HORIZON mode - mix of ANGLE and ACRO modes
557 // mix in errorAngle to currentPidSetpoint to add a little auto-level feel
558 const float horizonLevelStrength
= calcHorizonLevelStrength();
559 currentPidSetpoint
= currentPidSetpoint
+ (errorAngle
* horizonGain
* horizonLevelStrength
);
561 return currentPidSetpoint
;
564 static float accelerationLimit(int axis
, float currentPidSetpoint
)
566 static float previousSetpoint
[3];
567 const float currentVelocity
= currentPidSetpoint
- previousSetpoint
[axis
];
569 if (ABS(currentVelocity
) > maxVelocity
[axis
]) {
570 currentPidSetpoint
= (currentVelocity
> 0) ? previousSetpoint
[axis
] + maxVelocity
[axis
] : previousSetpoint
[axis
] - maxVelocity
[axis
];
573 previousSetpoint
[axis
] = currentPidSetpoint
;
574 return currentPidSetpoint
;
577 static timeUs_t crashDetectedAtUs
;
579 static void handleCrashRecovery(
580 const pidCrashRecovery_e crash_recovery
, const rollAndPitchTrims_t
*angleTrim
,
581 const int axis
, const timeUs_t currentTimeUs
, const float gyroRate
, float *currentPidSetpoint
, float *errorRate
)
583 if (inCrashRecoveryMode
&& cmpTimeUs(currentTimeUs
, crashDetectedAtUs
) > crashTimeDelayUs
) {
584 if (crash_recovery
== PID_CRASH_RECOVERY_BEEP
) {
587 if (axis
== FD_YAW
) {
588 *errorRate
= constrainf(*errorRate
, -crashLimitYaw
, crashLimitYaw
);
590 // on roll and pitch axes calculate currentPidSetpoint and errorRate to level the aircraft to recover from crash
591 if (sensors(SENSOR_ACC
)) {
592 // errorAngle is deviation from horizontal
593 const float errorAngle
= -(attitude
.raw
[axis
] - angleTrim
->raw
[axis
]) / 10.0f
;
594 *currentPidSetpoint
= errorAngle
* levelGain
;
595 *errorRate
= *currentPidSetpoint
- gyroRate
;
598 // reset ITerm, since accumulated error before crash is now meaningless
599 // and ITerm windup during crash recovery can be extreme, especially on yaw axis
600 pidData
[axis
].I
= 0.0f
;
601 if (cmpTimeUs(currentTimeUs
, crashDetectedAtUs
) > crashTimeLimitUs
602 || (getMotorMixRange() < 1.0f
603 && ABS(gyro
.gyroADCf
[FD_ROLL
]) < crashRecoveryRate
604 && ABS(gyro
.gyroADCf
[FD_PITCH
]) < crashRecoveryRate
605 && ABS(gyro
.gyroADCf
[FD_YAW
]) < crashRecoveryRate
)) {
606 if (sensors(SENSOR_ACC
)) {
607 // check aircraft nearly level
608 if (ABS(attitude
.raw
[FD_ROLL
] - angleTrim
->raw
[FD_ROLL
]) < crashRecoveryAngleDeciDegrees
609 && ABS(attitude
.raw
[FD_PITCH
] - angleTrim
->raw
[FD_PITCH
]) < crashRecoveryAngleDeciDegrees
) {
610 inCrashRecoveryMode
= false;
614 inCrashRecoveryMode
= false;
621 static void detectAndSetCrashRecovery(
622 const pidCrashRecovery_e crash_recovery
, const int axis
,
623 const timeUs_t currentTimeUs
, const float delta
, const float errorRate
)
625 // if crash recovery is on and accelerometer enabled and there is no gyro overflow, then check for a crash
626 // no point in trying to recover if the crash is so severe that the gyro overflows
627 if ((crash_recovery
|| FLIGHT_MODE(GPS_RESCUE_MODE
)) && !gyroOverflowDetected()) {
628 if (ARMING_FLAG(ARMED
)) {
629 if (getMotorMixRange() >= 1.0f
&& !inCrashRecoveryMode
630 && ABS(delta
) > crashDtermThreshold
631 && ABS(errorRate
) > crashGyroThreshold
632 && ABS(getSetpointRate(axis
)) < crashSetpointThreshold
) {
633 inCrashRecoveryMode
= true;
634 crashDetectedAtUs
= currentTimeUs
;
636 if (inCrashRecoveryMode
&& cmpTimeUs(currentTimeUs
, crashDetectedAtUs
) < crashTimeDelayUs
&& (ABS(errorRate
) < crashGyroThreshold
637 || ABS(getSetpointRate(axis
)) > crashSetpointThreshold
)) {
638 inCrashRecoveryMode
= false;
641 } else if (inCrashRecoveryMode
) {
642 inCrashRecoveryMode
= false;
648 static void rotateVector(float v
[XYZ_AXIS_COUNT
], float rotation
[XYZ_AXIS_COUNT
])
650 // rotate v around rotation vector rotation
651 // rotation in radians, all elements must be small
652 for (int i
= 0; i
< XYZ_AXIS_COUNT
; i
++) {
653 int i_1
= (i
+ 1) % 3;
654 int i_2
= (i
+ 2) % 3;
655 float newV
= v
[i_1
] + v
[i_2
] * rotation
[i
];
656 v
[i_2
] -= v
[i_1
] * rotation
[i
];
661 static void rotateITermAndAxisError()
664 #if defined(USE_ABSOLUTE_CONTROL)
668 const float gyroToAngle
= dT
* RAD
;
669 float rotationRads
[3];
670 for (int i
= FD_ROLL
; i
<= FD_YAW
; i
++) {
671 rotationRads
[i
] = gyro
.gyroADCf
[i
] * gyroToAngle
;
673 #if defined(USE_ABSOLUTE_CONTROL)
675 rotateVector(axisError
, rotationRads
);
680 for (int i
= 0; i
< XYZ_AXIS_COUNT
; i
++) {
683 rotateVector(v
, rotationRads
);
684 for (int i
= 0; i
< XYZ_AXIS_COUNT
; i
++) {
691 #ifdef USE_ACRO_TRAINER
693 int acroTrainerSign(float x
)
695 return x
> 0 ? 1 : -1;
698 // Acro Trainer - Manipulate the setPoint to limit axis angle while in acro mode
699 // There are three states:
700 // 1. Current angle has exceeded limit
701 // Apply correction to return to limit (similar to pidLevel)
702 // 2. Future overflow has been projected based on current angle and gyro rate
703 // Manage the setPoint to control the gyro rate as the actual angle approaches the limit (try to prevent overshoot)
704 // 3. If no potential overflow is detected, then return the original setPoint
706 // Use the FAST_CODE_NOINLINE directive to avoid this code from being inlined into ITCM RAM. We accept the
707 // performance decrease when Acro Trainer mode is active under the assumption that user is unlikely to be
708 // expecting ultimate flight performance at very high loop rates when in this mode.
709 static FAST_CODE_NOINLINE
float applyAcroTrainer(int axis
, const rollAndPitchTrims_t
*angleTrim
, float setPoint
)
711 float ret
= setPoint
;
713 if (!FLIGHT_MODE(ANGLE_MODE
) && !FLIGHT_MODE(HORIZON_MODE
) && !FLIGHT_MODE(GPS_RESCUE_MODE
)) {
714 bool resetIterm
= false;
715 float projectedAngle
= 0;
716 const int setpointSign
= acroTrainerSign(setPoint
);
717 const float currentAngle
= (attitude
.raw
[axis
] - angleTrim
->raw
[axis
]) / 10.0f
;
718 const int angleSign
= acroTrainerSign(currentAngle
);
720 if ((acroTrainerAxisState
[axis
] != 0) && (acroTrainerAxisState
[axis
] != setpointSign
)) { // stick has reversed - stop limiting
721 acroTrainerAxisState
[axis
] = 0;
724 // Limit and correct the angle when it exceeds the limit
725 if ((fabsf(currentAngle
) > acroTrainerAngleLimit
) && (acroTrainerAxisState
[axis
] == 0)) {
726 if (angleSign
== setpointSign
) {
727 acroTrainerAxisState
[axis
] = angleSign
;
732 if (acroTrainerAxisState
[axis
] != 0) {
733 ret
= constrainf(((acroTrainerAngleLimit
* angleSign
) - currentAngle
) * acroTrainerGain
, -ACRO_TRAINER_SETPOINT_LIMIT
, ACRO_TRAINER_SETPOINT_LIMIT
);
736 // Not currently over the limit so project the angle based on current angle and
737 // gyro angular rate using a sliding window based on gyro rate (faster rotation means larger window.
738 // If the projected angle exceeds the limit then apply limiting to minimize overshoot.
739 // Calculate the lookahead window by scaling proportionally with gyro rate from 0-500dps
740 float checkInterval
= constrainf(fabsf(gyro
.gyroADCf
[axis
]) / ACRO_TRAINER_LOOKAHEAD_RATE_LIMIT
, 0.0f
, 1.0f
) * acroTrainerLookaheadTime
;
741 projectedAngle
= (gyro
.gyroADCf
[axis
] * checkInterval
) + currentAngle
;
742 const int projectedAngleSign
= acroTrainerSign(projectedAngle
);
743 if ((fabsf(projectedAngle
) > acroTrainerAngleLimit
) && (projectedAngleSign
== setpointSign
)) {
744 ret
= ((acroTrainerAngleLimit
* projectedAngleSign
) - projectedAngle
) * acroTrainerGain
;
753 if (axis
== acroTrainerDebugAxis
) {
754 DEBUG_SET(DEBUG_ACRO_TRAINER
, 0, lrintf(currentAngle
* 10.0f
));
755 DEBUG_SET(DEBUG_ACRO_TRAINER
, 1, acroTrainerAxisState
[axis
]);
756 DEBUG_SET(DEBUG_ACRO_TRAINER
, 2, lrintf(ret
));
757 DEBUG_SET(DEBUG_ACRO_TRAINER
, 3, lrintf(projectedAngle
* 10.0f
));
763 #endif // USE_ACRO_TRAINER
765 // Betaflight pid controller, which will be maintained in the future with additional features specialised for current (mini) multirotor usage.
766 // Based on 2DOF reference design (matlab)
767 void FAST_CODE
pidController(const pidProfile_t
*pidProfile
, const rollAndPitchTrims_t
*angleTrim
, timeUs_t currentTimeUs
)
769 static float previousGyroRateDterm
[2];
770 static float previousPidSetpoint
[2];
772 const float tpaFactor
= getThrottlePIDAttenuation();
773 const float motorMixRange
= getMotorMixRange();
775 #ifdef USE_YAW_SPIN_RECOVERY
776 const bool yawSpinActive
= gyroYawSpinDetected();
779 // Dynamic i component,
780 // gradually scale back integration when above windup point
781 const float dynCi
= MIN((1.0f
- motorMixRange
) * ITermWindupPointInv
, 1.0f
) * dT
* itermAccelerator
;
783 // Dynamic d component, enable 2-DOF PID controller only for rate mode
784 const float dynCd
= flightModeFlags
? 0.0f
: dtermSetpointWeight
;
786 // Precalculate gyro deta for D-term here, this allows loop unrolling
787 float gyroRateDterm
[2];
788 for (int axis
= FD_ROLL
; axis
< FD_YAW
; ++axis
) {
789 gyroRateDterm
[axis
] = dtermNotchApplyFn((filter_t
*) &dtermNotch
[axis
], gyro
.gyroADCf
[axis
]);
790 gyroRateDterm
[axis
] = dtermLowpassApplyFn((filter_t
*) &dtermLowpass
[axis
], gyroRateDterm
[axis
]);
791 gyroRateDterm
[axis
] = dtermLowpass2ApplyFn((filter_t
*) &dtermLowpass2
[axis
], gyroRateDterm
[axis
]);
794 rotateITermAndAxisError();
796 // ----------PID controller----------
797 for (int axis
= FD_ROLL
; axis
<= FD_YAW
; ++axis
) {
798 float currentPidSetpoint
= getSetpointRate(axis
);
799 if (maxVelocity
[axis
]) {
800 currentPidSetpoint
= accelerationLimit(axis
, currentPidSetpoint
);
802 // Yaw control is GYRO based, direct sticks control is applied to rate PID
803 if ((FLIGHT_MODE(ANGLE_MODE
) || FLIGHT_MODE(HORIZON_MODE
) || FLIGHT_MODE(GPS_RESCUE_MODE
)) && axis
!= YAW
) {
804 currentPidSetpoint
= pidLevel(axis
, pidProfile
, angleTrim
, currentPidSetpoint
);
807 #ifdef USE_ACRO_TRAINER
808 if ((axis
!= FD_YAW
) && acroTrainerActive
&& !inCrashRecoveryMode
) {
809 currentPidSetpoint
= applyAcroTrainer(axis
, angleTrim
, currentPidSetpoint
);
811 #endif // USE_ACRO_TRAINER
813 // Handle yaw spin recovery - zero the setpoint on yaw to aid in recovery
814 // It's not necessary to zero the set points for R/P because the PIDs will be zeroed below
815 #ifdef USE_YAW_SPIN_RECOVERY
816 if ((axis
== FD_YAW
) && yawSpinActive
) {
817 currentPidSetpoint
= 0.0f
;
819 #endif // USE_YAW_SPIN_RECOVERY
821 // -----calculate error rate
822 const float gyroRate
= gyro
.gyroADCf
[axis
]; // Process variable from gyro output in deg/sec
824 #ifdef USE_ABSOLUTE_CONTROL
825 float acCorrection
= 0;
828 float itermErrorRate
= 0.0f
;
830 #if defined(USE_ITERM_RELAX)
831 if (itermRelax
&& (axis
< FD_YAW
|| itermRelax
== ITERM_RELAX_RPY
)) {
832 const float setpointLpf
= pt1FilterApply(&windupLpf
[axis
], currentPidSetpoint
);
833 const float setpointHpf
= fabsf(currentPidSetpoint
- setpointLpf
);
834 const float itermRelaxFactor
= 1 - setpointHpf
/ 30.0f
;
835 if (itermRelaxType
== ITERM_RELAX_SETPOINT
&& setpointHpf
< 30) {
836 itermErrorRate
= itermRelaxFactor
* (currentPidSetpoint
- gyroRate
);
837 } else if (itermRelaxType
== ITERM_RELAX_GYRO
) {
838 itermErrorRate
= fapplyDeadband(setpointLpf
- gyroRate
, setpointHpf
);
841 if (axis
== FD_ROLL
) {
842 DEBUG_SET(DEBUG_ITERM_RELAX
, 0, lrintf(setpointHpf
));
843 DEBUG_SET(DEBUG_ITERM_RELAX
, 1, lrintf(itermRelaxFactor
* 100.0f
));
844 DEBUG_SET(DEBUG_ITERM_RELAX
, 2, lrintf(itermErrorRate
));
847 #if defined(USE_ABSOLUTE_CONTROL)
848 const float gmaxac
= setpointLpf
+ 2 * setpointHpf
;
849 const float gminac
= setpointLpf
- 2 * setpointHpf
;
850 if (gyroRate
>= gminac
&& gyroRate
<= gmaxac
) {
851 float acErrorRate1
= gmaxac
- gyroRate
;
852 float acErrorRate2
= gminac
- gyroRate
;
853 if (acErrorRate1
* axisError
[axis
] < 0) {
854 acErrorRate
= acErrorRate1
;
856 acErrorRate
= acErrorRate2
;
858 if (fabsf(acErrorRate
* dT
) > fabsf(axisError
[axis
]) ) {
859 acErrorRate
= -axisError
[axis
] / dT
;
862 acErrorRate
= (gyroRate
> gmaxac
? gmaxac
: gminac
) - gyroRate
;
864 #endif // USE_ABSOLUTE_CONTROL
866 #endif // USE_ITERM_RELAX
868 itermErrorRate
= currentPidSetpoint
- gyroRate
;
869 #if defined(USE_ABSOLUTE_CONTROL)
870 acErrorRate
= itermErrorRate
;
871 #endif // USE_ABSOLUTE_CONTROL
874 #if defined(USE_ABSOLUTE_CONTROL)
875 if (acGain
> 0 && isAirmodeActivated()) {
876 axisError
[axis
] = constrainf(axisError
[axis
] + acErrorRate
* dT
, -acErrorLimit
, acErrorLimit
);
877 acCorrection
= constrainf(axisError
[axis
] * acGain
, -acLimit
, acLimit
);
878 currentPidSetpoint
+= acCorrection
;
879 itermErrorRate
+= acCorrection
;
880 if (axis
== FD_ROLL
) {
881 DEBUG_SET(DEBUG_ITERM_RELAX
, 3, lrintf(axisError
[axis
] * 10));
886 float errorRate
= currentPidSetpoint
- gyroRate
; // r - y
888 pidProfile
->crash_recovery
, angleTrim
, axis
, currentTimeUs
, gyroRate
,
889 ¤tPidSetpoint
, &errorRate
);
891 // --------low-level gyro-based PID based on 2DOF PID controller. ----------
892 // 2-DOF PID controller with optional filter on derivative term.
893 // b = 1 and only c (dtermSetpointWeight) can be tuned (amount derivative on measurement or error).
895 // -----calculate P component and add Dynamic Part based on stick input
896 pidData
[axis
].P
= pidCoefficient
[axis
].Kp
* errorRate
* tpaFactor
;
897 if (axis
== FD_YAW
) {
898 pidData
[axis
].P
= ptermYawLowpassApplyFn((filter_t
*) &ptermYawLowpass
, pidData
[axis
].P
);
901 // -----calculate I component
903 const float ITerm
= pidData
[axis
].I
;
904 const float ITermNew
= constrainf(ITerm
+ pidCoefficient
[axis
].Ki
* itermErrorRate
* dynCi
, -itermLimit
, itermLimit
);
905 const bool outputSaturated
= mixerIsOutputSaturated(axis
, errorRate
);
906 if (outputSaturated
== false || ABS(ITermNew
) < ABS(ITerm
)) {
907 // Only increase ITerm if output is not saturated
908 pidData
[axis
].I
= ITermNew
;
911 // -----calculate D component
912 if (axis
!= FD_YAW
) {
913 // no transition if relaxFactor == 0
914 float transition
= relaxFactor
> 0 ? MIN(1.f
, getRcDeflectionAbs(axis
) * relaxFactor
) : 1;
916 // Divide rate change by dT to get differential (ie dr/dt).
917 // dT is fixed and calculated from the target PID loop time
918 // This is done to avoid DTerm spikes that occur with dynamically
919 // calculated deltaT whenever another task causes the PID
920 // loop execution to be delayed.
922 - (gyroRateDterm
[axis
] - previousGyroRateDterm
[axis
]) * pidFrequency
;
924 detectAndSetCrashRecovery(pidProfile
->crash_recovery
, axis
, currentTimeUs
, delta
, errorRate
);
926 pidData
[axis
].D
= pidCoefficient
[axis
].Kd
* delta
* tpaFactor
;
928 float pidSetpointDelta
= currentPidSetpoint
- previousPidSetpoint
[axis
];
930 #ifdef USE_RC_SMOOTHING_FILTER
931 if (axis
== rcSmoothingDebugAxis
) {
932 DEBUG_SET(DEBUG_RC_SMOOTHING
, 1, lrintf(pidSetpointDelta
* 100.0f
));
934 if ((dynCd
!= 0) && setpointDerivativeLpfInitialized
) {
935 switch (rcSmoothingFilterType
) {
936 case RC_SMOOTHING_DERIVATIVE_PT1
:
937 pidSetpointDelta
= pt1FilterApply(&setpointDerivativePt1
[axis
], pidSetpointDelta
);
939 case RC_SMOOTHING_DERIVATIVE_BIQUAD
:
940 pidSetpointDelta
= biquadFilterApplyDF1(&setpointDerivativeBiquad
[axis
], pidSetpointDelta
);
943 if (axis
== rcSmoothingDebugAxis
) {
944 DEBUG_SET(DEBUG_RC_SMOOTHING
, 2, lrintf(pidSetpointDelta
* 100.0f
));
947 #endif // USE_RC_SMOOTHING_FILTER
949 const float pidFeedForward
=
950 pidCoefficient
[axis
].Kd
* dynCd
* transition
* pidSetpointDelta
* tpaFactor
* pidFrequency
;
951 #if defined(USE_SMART_FEEDFORWARD)
952 bool addFeedforward
= true;
953 if (smartFeedforward
) {
954 if (pidData
[axis
].P
* pidFeedForward
> 0) {
955 if (ABS(pidFeedForward
) > ABS(pidData
[axis
].P
)) {
958 addFeedforward
= false;
965 pidData
[axis
].D
+= pidFeedForward
;
967 previousGyroRateDterm
[axis
] = gyroRateDterm
[axis
];
968 previousPidSetpoint
[axis
] = currentPidSetpoint
;
970 #ifdef USE_YAW_SPIN_RECOVERY
972 // zero PIDs on pitch and roll leaving yaw P to correct spin
977 #endif // USE_YAW_SPIN_RECOVERY
981 // calculating the PID sum
982 pidData
[FD_ROLL
].Sum
= pidData
[FD_ROLL
].P
+ pidData
[FD_ROLL
].I
+ pidData
[FD_ROLL
].D
;
983 pidData
[FD_PITCH
].Sum
= pidData
[FD_PITCH
].P
+ pidData
[FD_PITCH
].I
+ pidData
[FD_PITCH
].D
;
985 #ifdef USE_YAW_SPIN_RECOVERY
987 // yaw P alone to correct spin
988 pidData
[FD_YAW
].I
= 0;
990 #endif // USE_YAW_SPIN_RECOVERY
993 pidData
[FD_YAW
].Sum
= pidData
[FD_YAW
].P
+ pidData
[FD_YAW
].I
;
995 // Disable PID control if at zero throttle or if gyro overflow detected
996 // This may look very innefficient, but it is done on purpose to always show real CPU usage as in flight
997 if (!pidStabilisationEnabled
|| gyroOverflowDetected()) {
998 for (int axis
= FD_ROLL
; axis
<= FD_YAW
; ++axis
) {
1000 pidData
[axis
].I
= 0;
1001 pidData
[axis
].D
= 0;
1003 pidData
[axis
].Sum
= 0;
1008 bool crashRecoveryModeActive(void)
1010 return inCrashRecoveryMode
;
1013 #ifdef USE_ACRO_TRAINER
1014 void pidSetAcroTrainerState(bool newState
)
1016 if (acroTrainerActive
!= newState
) {
1018 pidAcroTrainerInit();
1020 acroTrainerActive
= newState
;
1023 #endif // USE_ACRO_TRAINER