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
] = { 40, 40, 30 },
104 [PID_PITCH
] = { 58, 50, 35 },
105 [PID_YAW
] = { 70, 45, 20 },
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, // filtering ON by default
119 .dterm_lowpass2_hz
= 0, // second Dterm LPF OFF by default
120 .dterm_notch_hz
= 260,
121 .dterm_notch_cutoff
= 160,
122 .dterm_filter_type
= FILTER_BIQUAD
,
123 .itermWindupPointPercent
= 50,
124 .vbatPidCompensation
= 0,
125 .pidAtMinThrottle
= PID_STABILISATION_ON
,
126 .levelAngleLimit
= 55,
127 .setpointRelaxRatio
= 100,
128 .dtermSetpointWeight
= 45,
129 .yawRateAccelLimit
= 100,
131 .itermThrottleThreshold
= 350,
132 .itermAcceleratorGain
= 1000,
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
= false,
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
);
328 #endif // USE_RC_SMOOTHING_FILTER
330 typedef struct pidCoefficient_s
{
336 static FAST_RAM_ZERO_INIT pidCoefficient_t pidCoefficient
[3];
337 static FAST_RAM_ZERO_INIT
float maxVelocity
[3];
338 static FAST_RAM_ZERO_INIT
float relaxFactor
;
339 static FAST_RAM_ZERO_INIT
float dtermSetpointWeight
;
340 static FAST_RAM_ZERO_INIT
float levelGain
, horizonGain
, horizonTransition
, horizonCutoffDegrees
, horizonFactorRatio
;
341 static FAST_RAM_ZERO_INIT
float ITermWindupPointInv
;
342 static FAST_RAM_ZERO_INIT
uint8_t horizonTiltExpertMode
;
343 static FAST_RAM_ZERO_INIT timeDelta_t crashTimeLimitUs
;
344 static FAST_RAM_ZERO_INIT timeDelta_t crashTimeDelayUs
;
345 static FAST_RAM_ZERO_INIT
int32_t crashRecoveryAngleDeciDegrees
;
346 static FAST_RAM_ZERO_INIT
float crashRecoveryRate
;
347 static FAST_RAM_ZERO_INIT
float crashDtermThreshold
;
348 static FAST_RAM_ZERO_INIT
float crashGyroThreshold
;
349 static FAST_RAM_ZERO_INIT
float crashSetpointThreshold
;
350 static FAST_RAM_ZERO_INIT
float crashLimitYaw
;
351 static FAST_RAM_ZERO_INIT
float itermLimit
;
352 #if defined(USE_THROTTLE_BOOST)
353 FAST_RAM_ZERO_INIT
float throttleBoost
;
354 pt1Filter_t throttleLpf
;
356 static FAST_RAM_ZERO_INIT
bool itermRotation
;
358 #if defined(USE_SMART_FEEDFORWARD)
359 static FAST_RAM_ZERO_INIT
bool smartFeedforward
;
361 #if defined(USE_ABSOLUTE_CONTROL)
362 static FAST_RAM_ZERO_INIT
float axisError
[XYZ_AXIS_COUNT
];
363 static FAST_RAM_ZERO_INIT
float acGain
;
364 static FAST_RAM_ZERO_INIT
float acLimit
;
365 static FAST_RAM_ZERO_INIT
float acErrorLimit
;
368 void pidResetITerm(void)
370 for (int axis
= 0; axis
< 3; axis
++) {
371 pidData
[axis
].I
= 0.0f
;
372 #if defined(USE_ABSOLUTE_CONTROL)
373 axisError
[axis
] = 0.0f
;
378 #ifdef USE_ACRO_TRAINER
379 static FAST_RAM_ZERO_INIT
float acroTrainerAngleLimit
;
380 static FAST_RAM_ZERO_INIT
float acroTrainerLookaheadTime
;
381 static FAST_RAM_ZERO_INIT
uint8_t acroTrainerDebugAxis
;
382 static FAST_RAM_ZERO_INIT
bool acroTrainerActive
;
383 static FAST_RAM_ZERO_INIT
int acroTrainerAxisState
[2]; // only need roll and pitch
384 static FAST_RAM_ZERO_INIT
float acroTrainerGain
;
385 #endif // USE_ACRO_TRAINER
387 void pidInitConfig(const pidProfile_t
*pidProfile
)
389 for (int axis
= FD_ROLL
; axis
<= FD_YAW
; axis
++) {
390 pidCoefficient
[axis
].Kp
= PTERM_SCALE
* pidProfile
->pid
[axis
].P
;
391 pidCoefficient
[axis
].Ki
= ITERM_SCALE
* pidProfile
->pid
[axis
].I
;
392 pidCoefficient
[axis
].Kd
= DTERM_SCALE
* pidProfile
->pid
[axis
].D
;
395 dtermSetpointWeight
= pidProfile
->dtermSetpointWeight
/ 100.0f
;
396 if (pidProfile
->setpointRelaxRatio
== 0) {
399 relaxFactor
= 100.0f
/ pidProfile
->setpointRelaxRatio
;
401 levelGain
= pidProfile
->pid
[PID_LEVEL
].P
/ 10.0f
;
402 horizonGain
= pidProfile
->pid
[PID_LEVEL
].I
/ 10.0f
;
403 horizonTransition
= (float)pidProfile
->pid
[PID_LEVEL
].D
;
404 horizonTiltExpertMode
= pidProfile
->horizon_tilt_expert_mode
;
405 horizonCutoffDegrees
= (175 - pidProfile
->horizon_tilt_effect
) * 1.8f
;
406 horizonFactorRatio
= (100 - pidProfile
->horizon_tilt_effect
) * 0.01f
;
407 maxVelocity
[FD_ROLL
] = maxVelocity
[FD_PITCH
] = pidProfile
->rateAccelLimit
* 100 * dT
;
408 maxVelocity
[FD_YAW
] = pidProfile
->yawRateAccelLimit
* 100 * dT
;
409 const float ITermWindupPoint
= (float)pidProfile
->itermWindupPointPercent
/ 100.0f
;
410 ITermWindupPointInv
= 1.0f
/ (1.0f
- ITermWindupPoint
);
411 crashTimeLimitUs
= pidProfile
->crash_time
* 1000;
412 crashTimeDelayUs
= pidProfile
->crash_delay
* 1000;
413 crashRecoveryAngleDeciDegrees
= pidProfile
->crash_recovery_angle
* 10;
414 crashRecoveryRate
= pidProfile
->crash_recovery_rate
;
415 crashGyroThreshold
= pidProfile
->crash_gthreshold
;
416 crashDtermThreshold
= pidProfile
->crash_dthreshold
;
417 crashSetpointThreshold
= pidProfile
->crash_setpoint_threshold
;
418 crashLimitYaw
= pidProfile
->crash_limit_yaw
;
419 itermLimit
= pidProfile
->itermLimit
;
420 #if defined(USE_THROTTLE_BOOST)
421 throttleBoost
= pidProfile
->throttle_boost
* 0.1f
;
423 itermRotation
= pidProfile
->iterm_rotation
;
424 #if defined(USE_SMART_FEEDFORWARD)
425 smartFeedforward
= pidProfile
->smart_feedforward
;
427 #if defined(USE_ITERM_RELAX)
428 itermRelax
= pidProfile
->iterm_relax
;
429 itermRelaxType
= pidProfile
->iterm_relax_type
;
430 itermRelaxCutoff
= pidProfile
->iterm_relax_cutoff
;
433 #ifdef USE_ACRO_TRAINER
434 acroTrainerAngleLimit
= pidProfile
->acro_trainer_angle_limit
;
435 acroTrainerLookaheadTime
= (float)pidProfile
->acro_trainer_lookahead_ms
/ 1000.0f
;
436 acroTrainerDebugAxis
= pidProfile
->acro_trainer_debug_axis
;
437 acroTrainerGain
= (float)pidProfile
->acro_trainer_gain
/ 10.0f
;
438 #endif // USE_ACRO_TRAINER
440 #if defined(USE_ABSOLUTE_CONTROL)
441 acGain
= (float)pidProfile
->abs_control_gain
;
442 acLimit
= (float)pidProfile
->abs_control_limit
;
443 acErrorLimit
= (float)pidProfile
->abs_control_error_limit
;
447 void pidInit(const pidProfile_t
*pidProfile
)
449 pidSetTargetLooptime(gyro
.targetLooptime
* pidConfig()->pid_process_denom
); // Initialize pid looptime
450 pidInitFilters(pidProfile
);
451 pidInitConfig(pidProfile
);
454 #ifdef USE_ACRO_TRAINER
455 void pidAcroTrainerInit(void)
457 acroTrainerAxisState
[FD_ROLL
] = 0;
458 acroTrainerAxisState
[FD_PITCH
] = 0;
460 #endif // USE_ACRO_TRAINER
462 void pidCopyProfile(uint8_t dstPidProfileIndex
, uint8_t srcPidProfileIndex
)
464 if ((dstPidProfileIndex
< MAX_PROFILE_COUNT
-1 && srcPidProfileIndex
< MAX_PROFILE_COUNT
-1)
465 && dstPidProfileIndex
!= srcPidProfileIndex
467 memcpy(pidProfilesMutable(dstPidProfileIndex
), pidProfilesMutable(srcPidProfileIndex
), sizeof(pidProfile_t
));
471 // calculates strength of horizon leveling; 0 = none, 1.0 = most leveling
472 static float calcHorizonLevelStrength(void)
474 // start with 1.0 at center stick, 0.0 at max stick deflection:
475 float horizonLevelStrength
= 1.0f
- MAX(getRcDeflectionAbs(FD_ROLL
), getRcDeflectionAbs(FD_PITCH
));
477 // 0 at level, 90 at vertical, 180 at inverted (degrees):
478 const float currentInclination
= MAX(ABS(attitude
.values
.roll
), ABS(attitude
.values
.pitch
)) / 10.0f
;
480 // horizonTiltExpertMode: 0 = leveling always active when sticks centered,
481 // 1 = leveling can be totally off when inverted
482 if (horizonTiltExpertMode
) {
483 if (horizonTransition
> 0 && horizonCutoffDegrees
> 0) {
484 // if d_level > 0 and horizonTiltEffect < 175
485 // horizonCutoffDegrees: 0 to 125 => 270 to 90 (represents where leveling goes to zero)
486 // inclinationLevelRatio (0.0 to 1.0) is smaller (less leveling)
487 // for larger inclinations; 0.0 at horizonCutoffDegrees value:
488 const float inclinationLevelRatio
= constrainf((horizonCutoffDegrees
-currentInclination
) / horizonCutoffDegrees
, 0, 1);
489 // apply configured horizon sensitivity:
490 // when stick is near center (horizonLevelStrength ~= 1.0)
491 // H_sensitivity value has little effect,
492 // when stick is deflected (horizonLevelStrength near 0.0)
493 // H_sensitivity value has more effect:
494 horizonLevelStrength
= (horizonLevelStrength
- 1) * 100 / horizonTransition
+ 1;
495 // apply inclination ratio, which may lower leveling
496 // to zero regardless of stick position:
497 horizonLevelStrength
*= inclinationLevelRatio
;
498 } else { // d_level=0 or horizon_tilt_effect>=175 means no leveling
499 horizonLevelStrength
= 0;
501 } else { // horizon_tilt_expert_mode = 0 (leveling always active when sticks centered)
503 if (horizonFactorRatio
< 1.01f
) { // if horizonTiltEffect > 0
504 // horizonFactorRatio: 1.0 to 0.0 (larger means more leveling)
505 // inclinationLevelRatio (0.0 to 1.0) is smaller (less leveling)
506 // for larger inclinations, goes to 1.0 at inclination==level:
507 const float inclinationLevelRatio
= (180-currentInclination
)/180 * (1.0f
-horizonFactorRatio
) + horizonFactorRatio
;
508 // apply ratio to configured horizon sensitivity:
509 sensitFact
= horizonTransition
* inclinationLevelRatio
;
510 } else { // horizonTiltEffect=0 for "old" functionality
511 sensitFact
= horizonTransition
;
514 if (sensitFact
<= 0) { // zero means no leveling
515 horizonLevelStrength
= 0;
517 // when stick is near center (horizonLevelStrength ~= 1.0)
518 // sensitFact value has little effect,
519 // when stick is deflected (horizonLevelStrength near 0.0)
520 // sensitFact value has more effect:
521 horizonLevelStrength
= ((horizonLevelStrength
- 1) * (100 / sensitFact
)) + 1;
524 return constrainf(horizonLevelStrength
, 0, 1);
527 static float pidLevel(int axis
, const pidProfile_t
*pidProfile
, const rollAndPitchTrims_t
*angleTrim
, float currentPidSetpoint
) {
528 // calculate error angle and limit the angle to the max inclination
529 // rcDeflection is in range [-1.0, 1.0]
530 float angle
= pidProfile
->levelAngleLimit
* getRcDeflection(axis
);
531 #ifdef USE_GPS_RESCUE
532 angle
+= gpsRescueAngle
[axis
] / 100; // ANGLE IS IN CENTIDEGREES
534 angle
= constrainf(angle
, -pidProfile
->levelAngleLimit
, pidProfile
->levelAngleLimit
);
535 const float errorAngle
= angle
- ((attitude
.raw
[axis
] - angleTrim
->raw
[axis
]) / 10.0f
);
536 if (FLIGHT_MODE(ANGLE_MODE
) || FLIGHT_MODE(GPS_RESCUE_MODE
)) {
537 // ANGLE mode - control is angle based
538 currentPidSetpoint
= errorAngle
* levelGain
;
540 // HORIZON mode - mix of ANGLE and ACRO modes
541 // mix in errorAngle to currentPidSetpoint to add a little auto-level feel
542 const float horizonLevelStrength
= calcHorizonLevelStrength();
543 currentPidSetpoint
= currentPidSetpoint
+ (errorAngle
* horizonGain
* horizonLevelStrength
);
545 return currentPidSetpoint
;
548 static float accelerationLimit(int axis
, float currentPidSetpoint
)
550 static float previousSetpoint
[3];
551 const float currentVelocity
= currentPidSetpoint
- previousSetpoint
[axis
];
553 if (ABS(currentVelocity
) > maxVelocity
[axis
]) {
554 currentPidSetpoint
= (currentVelocity
> 0) ? previousSetpoint
[axis
] + maxVelocity
[axis
] : previousSetpoint
[axis
] - maxVelocity
[axis
];
557 previousSetpoint
[axis
] = currentPidSetpoint
;
558 return currentPidSetpoint
;
561 static timeUs_t crashDetectedAtUs
;
563 static void handleCrashRecovery(
564 const pidCrashRecovery_e crash_recovery
, const rollAndPitchTrims_t
*angleTrim
,
565 const int axis
, const timeUs_t currentTimeUs
, const float gyroRate
, float *currentPidSetpoint
, float *errorRate
)
567 if (inCrashRecoveryMode
&& cmpTimeUs(currentTimeUs
, crashDetectedAtUs
) > crashTimeDelayUs
) {
568 if (crash_recovery
== PID_CRASH_RECOVERY_BEEP
) {
571 if (axis
== FD_YAW
) {
572 *errorRate
= constrainf(*errorRate
, -crashLimitYaw
, crashLimitYaw
);
574 // on roll and pitch axes calculate currentPidSetpoint and errorRate to level the aircraft to recover from crash
575 if (sensors(SENSOR_ACC
)) {
576 // errorAngle is deviation from horizontal
577 const float errorAngle
= -(attitude
.raw
[axis
] - angleTrim
->raw
[axis
]) / 10.0f
;
578 *currentPidSetpoint
= errorAngle
* levelGain
;
579 *errorRate
= *currentPidSetpoint
- gyroRate
;
582 // reset ITerm, since accumulated error before crash is now meaningless
583 // and ITerm windup during crash recovery can be extreme, especially on yaw axis
584 pidData
[axis
].I
= 0.0f
;
585 if (cmpTimeUs(currentTimeUs
, crashDetectedAtUs
) > crashTimeLimitUs
586 || (getMotorMixRange() < 1.0f
587 && ABS(gyro
.gyroADCf
[FD_ROLL
]) < crashRecoveryRate
588 && ABS(gyro
.gyroADCf
[FD_PITCH
]) < crashRecoveryRate
589 && ABS(gyro
.gyroADCf
[FD_YAW
]) < crashRecoveryRate
)) {
590 if (sensors(SENSOR_ACC
)) {
591 // check aircraft nearly level
592 if (ABS(attitude
.raw
[FD_ROLL
] - angleTrim
->raw
[FD_ROLL
]) < crashRecoveryAngleDeciDegrees
593 && ABS(attitude
.raw
[FD_PITCH
] - angleTrim
->raw
[FD_PITCH
]) < crashRecoveryAngleDeciDegrees
) {
594 inCrashRecoveryMode
= false;
598 inCrashRecoveryMode
= false;
605 static void detectAndSetCrashRecovery(
606 const pidCrashRecovery_e crash_recovery
, const int axis
,
607 const timeUs_t currentTimeUs
, const float delta
, const float errorRate
)
609 // if crash recovery is on and accelerometer enabled and there is no gyro overflow, then check for a crash
610 // no point in trying to recover if the crash is so severe that the gyro overflows
611 if ((crash_recovery
|| FLIGHT_MODE(GPS_RESCUE_MODE
)) && !gyroOverflowDetected()) {
612 if (ARMING_FLAG(ARMED
)) {
613 if (getMotorMixRange() >= 1.0f
&& !inCrashRecoveryMode
614 && ABS(delta
) > crashDtermThreshold
615 && ABS(errorRate
) > crashGyroThreshold
616 && ABS(getSetpointRate(axis
)) < crashSetpointThreshold
) {
617 inCrashRecoveryMode
= true;
618 crashDetectedAtUs
= currentTimeUs
;
620 if (inCrashRecoveryMode
&& cmpTimeUs(currentTimeUs
, crashDetectedAtUs
) < crashTimeDelayUs
&& (ABS(errorRate
) < crashGyroThreshold
621 || ABS(getSetpointRate(axis
)) > crashSetpointThreshold
)) {
622 inCrashRecoveryMode
= false;
625 } else if (inCrashRecoveryMode
) {
626 inCrashRecoveryMode
= false;
632 static void rotateVector(float v
[XYZ_AXIS_COUNT
], float rotation
[XYZ_AXIS_COUNT
])
634 // rotate v around rotation vector rotation
635 // rotation in radians, all elements must be small
636 for (int i
= 0; i
< XYZ_AXIS_COUNT
; i
++) {
637 int i_1
= (i
+ 1) % 3;
638 int i_2
= (i
+ 2) % 3;
639 float newV
= v
[i_1
] + v
[i_2
] * rotation
[i
];
640 v
[i_2
] -= v
[i_1
] * rotation
[i
];
645 static void rotateITermAndAxisError()
648 #if defined(USE_ABSOLUTE_CONTROL)
652 const float gyroToAngle
= dT
* RAD
;
653 float rotationRads
[3];
654 for (int i
= FD_ROLL
; i
<= FD_YAW
; i
++) {
655 rotationRads
[i
] = gyro
.gyroADCf
[i
] * gyroToAngle
;
657 #if defined(USE_ABSOLUTE_CONTROL)
659 rotateVector(axisError
, rotationRads
);
664 for (int i
= 0; i
< XYZ_AXIS_COUNT
; i
++) {
667 rotateVector(v
, rotationRads
);
668 for (int i
= 0; i
< XYZ_AXIS_COUNT
; i
++) {
675 #ifdef USE_ACRO_TRAINER
677 int acroTrainerSign(float x
)
679 return x
> 0 ? 1 : -1;
682 // Acro Trainer - Manipulate the setPoint to limit axis angle while in acro mode
683 // There are three states:
684 // 1. Current angle has exceeded limit
685 // Apply correction to return to limit (similar to pidLevel)
686 // 2. Future overflow has been projected based on current angle and gyro rate
687 // Manage the setPoint to control the gyro rate as the actual angle approaches the limit (try to prevent overshoot)
688 // 3. If no potential overflow is detected, then return the original setPoint
690 // Use the FAST_CODE_NOINLINE directive to avoid this code from being inlined into ITCM RAM. We accept the
691 // performance decrease when Acro Trainer mode is active under the assumption that user is unlikely to be
692 // expecting ultimate flight performance at very high loop rates when in this mode.
693 static FAST_CODE_NOINLINE
float applyAcroTrainer(int axis
, const rollAndPitchTrims_t
*angleTrim
, float setPoint
)
695 float ret
= setPoint
;
697 if (!FLIGHT_MODE(ANGLE_MODE
) && !FLIGHT_MODE(HORIZON_MODE
) && !FLIGHT_MODE(GPS_RESCUE_MODE
)) {
698 bool resetIterm
= false;
699 float projectedAngle
= 0;
700 const int setpointSign
= acroTrainerSign(setPoint
);
701 const float currentAngle
= (attitude
.raw
[axis
] - angleTrim
->raw
[axis
]) / 10.0f
;
702 const int angleSign
= acroTrainerSign(currentAngle
);
704 if ((acroTrainerAxisState
[axis
] != 0) && (acroTrainerAxisState
[axis
] != setpointSign
)) { // stick has reversed - stop limiting
705 acroTrainerAxisState
[axis
] = 0;
708 // Limit and correct the angle when it exceeds the limit
709 if ((fabsf(currentAngle
) > acroTrainerAngleLimit
) && (acroTrainerAxisState
[axis
] == 0)) {
710 if (angleSign
== setpointSign
) {
711 acroTrainerAxisState
[axis
] = angleSign
;
716 if (acroTrainerAxisState
[axis
] != 0) {
717 ret
= constrainf(((acroTrainerAngleLimit
* angleSign
) - currentAngle
) * acroTrainerGain
, -ACRO_TRAINER_SETPOINT_LIMIT
, ACRO_TRAINER_SETPOINT_LIMIT
);
720 // Not currently over the limit so project the angle based on current angle and
721 // gyro angular rate using a sliding window based on gyro rate (faster rotation means larger window.
722 // If the projected angle exceeds the limit then apply limiting to minimize overshoot.
723 // Calculate the lookahead window by scaling proportionally with gyro rate from 0-500dps
724 float checkInterval
= constrainf(fabsf(gyro
.gyroADCf
[axis
]) / ACRO_TRAINER_LOOKAHEAD_RATE_LIMIT
, 0.0f
, 1.0f
) * acroTrainerLookaheadTime
;
725 projectedAngle
= (gyro
.gyroADCf
[axis
] * checkInterval
) + currentAngle
;
726 const int projectedAngleSign
= acroTrainerSign(projectedAngle
);
727 if ((fabsf(projectedAngle
) > acroTrainerAngleLimit
) && (projectedAngleSign
== setpointSign
)) {
728 ret
= ((acroTrainerAngleLimit
* projectedAngleSign
) - projectedAngle
) * acroTrainerGain
;
737 if (axis
== acroTrainerDebugAxis
) {
738 DEBUG_SET(DEBUG_ACRO_TRAINER
, 0, lrintf(currentAngle
* 10.0f
));
739 DEBUG_SET(DEBUG_ACRO_TRAINER
, 1, acroTrainerAxisState
[axis
]);
740 DEBUG_SET(DEBUG_ACRO_TRAINER
, 2, lrintf(ret
));
741 DEBUG_SET(DEBUG_ACRO_TRAINER
, 3, lrintf(projectedAngle
* 10.0f
));
747 #endif // USE_ACRO_TRAINER
749 // Betaflight pid controller, which will be maintained in the future with additional features specialised for current (mini) multirotor usage.
750 // Based on 2DOF reference design (matlab)
751 void FAST_CODE
pidController(const pidProfile_t
*pidProfile
, const rollAndPitchTrims_t
*angleTrim
, timeUs_t currentTimeUs
)
753 static float previousGyroRateDterm
[2];
754 static float previousPidSetpoint
[2];
756 const float tpaFactor
= getThrottlePIDAttenuation();
757 const float motorMixRange
= getMotorMixRange();
759 #ifdef USE_YAW_SPIN_RECOVERY
760 const bool yawSpinActive
= gyroYawSpinDetected();
763 // Dynamic i component,
764 // gradually scale back integration when above windup point
765 const float dynCi
= MIN((1.0f
- motorMixRange
) * ITermWindupPointInv
, 1.0f
) * dT
* itermAccelerator
;
767 // Dynamic d component, enable 2-DOF PID controller only for rate mode
768 const float dynCd
= flightModeFlags
? 0.0f
: dtermSetpointWeight
;
770 // Precalculate gyro deta for D-term here, this allows loop unrolling
771 float gyroRateDterm
[2];
772 for (int axis
= FD_ROLL
; axis
< FD_YAW
; ++axis
) {
773 gyroRateDterm
[axis
] = dtermNotchApplyFn((filter_t
*) &dtermNotch
[axis
], gyro
.gyroADCf
[axis
]);
774 gyroRateDterm
[axis
] = dtermLowpassApplyFn((filter_t
*) &dtermLowpass
[axis
], gyroRateDterm
[axis
]);
775 gyroRateDterm
[axis
] = dtermLowpass2ApplyFn((filter_t
*) &dtermLowpass2
[axis
], gyroRateDterm
[axis
]);
778 rotateITermAndAxisError();
780 // ----------PID controller----------
781 for (int axis
= FD_ROLL
; axis
<= FD_YAW
; ++axis
) {
782 float currentPidSetpoint
= getSetpointRate(axis
);
783 if (maxVelocity
[axis
]) {
784 currentPidSetpoint
= accelerationLimit(axis
, currentPidSetpoint
);
786 // Yaw control is GYRO based, direct sticks control is applied to rate PID
787 if ((FLIGHT_MODE(ANGLE_MODE
) || FLIGHT_MODE(HORIZON_MODE
) || FLIGHT_MODE(GPS_RESCUE_MODE
)) && axis
!= YAW
) {
788 currentPidSetpoint
= pidLevel(axis
, pidProfile
, angleTrim
, currentPidSetpoint
);
791 #ifdef USE_ACRO_TRAINER
792 if ((axis
!= FD_YAW
) && acroTrainerActive
&& !inCrashRecoveryMode
) {
793 currentPidSetpoint
= applyAcroTrainer(axis
, angleTrim
, currentPidSetpoint
);
795 #endif // USE_ACRO_TRAINER
797 // Handle yaw spin recovery - zero the setpoint on yaw to aid in recovery
798 // It's not necessary to zero the set points for R/P because the PIDs will be zeroed below
799 #ifdef USE_YAW_SPIN_RECOVERY
800 if ((axis
== FD_YAW
) && yawSpinActive
) {
801 currentPidSetpoint
= 0.0f
;
803 #endif // USE_YAW_SPIN_RECOVERY
805 // -----calculate error rate
806 const float gyroRate
= gyro
.gyroADCf
[axis
]; // Process variable from gyro output in deg/sec
808 #ifdef USE_ABSOLUTE_CONTROL
809 float acCorrection
= 0;
812 float itermErrorRate
= 0.0f
;
814 #if defined(USE_ITERM_RELAX)
815 if (itermRelax
&& (axis
< FD_YAW
|| itermRelax
== ITERM_RELAX_RPY
)) {
816 const float gyroTarget
= pt1FilterApply(&windupLpf
[axis
], currentPidSetpoint
);
817 const float gyroHpf
= fabsf(currentPidSetpoint
- gyroTarget
);
818 if (itermRelaxType
== ITERM_RELAX_SETPOINT
) {
819 itermErrorRate
= (1 - MIN(1, fabsf(gyroHpf
) / 60.0f
)) * (currentPidSetpoint
- gyroRate
);
821 const float tolerance
= gyroHpf
* 1.0f
;
822 if (axis
== FD_ROLL
) {
823 DEBUG_SET(DEBUG_ITERM_RELAX
, 0, gyroTarget
);
824 DEBUG_SET(DEBUG_ITERM_RELAX
, 1, gyroTarget
+ tolerance
);
825 DEBUG_SET(DEBUG_ITERM_RELAX
, 2, gyroTarget
- tolerance
);
826 DEBUG_SET(DEBUG_ITERM_RELAX
, 3, axisError
[axis
] * 10);
828 if (itermRelaxType
== ITERM_RELAX_GYRO
) {
829 const float gmax
= gyroTarget
+ tolerance
;
830 const float gmin
= gyroTarget
- tolerance
;
832 if (gyroRate
>= gmin
&& gyroRate
<= gmax
) {
835 itermErrorRate
= (gyroRate
> gmax
? gmax
: gmin
) - gyroRate
;
839 #if defined(USE_ABSOLUTE_CONTROL)
840 const float gmaxac
= gyroTarget
+ 2 * tolerance
;
841 const float gminac
= gyroTarget
- 2 * tolerance
;
842 if (gyroRate
>= gminac
&& gyroRate
<= gmaxac
) {
843 float acErrorRate1
= gmaxac
- gyroRate
;
844 float acErrorRate2
= gminac
- gyroRate
;
845 if (acErrorRate1
* axisError
[axis
] < 0) {
846 acErrorRate
= acErrorRate1
;
848 acErrorRate
= acErrorRate2
;
850 if (fabsf(acErrorRate
*dT
) > fabsf(axisError
[axis
]) ) {
851 acErrorRate
= -axisError
[axis
]/dT
;
854 acErrorRate
= (gyroRate
> gmaxac
? gmaxac
: gminac
) - gyroRate
;
856 #endif // USE_ABSOLUTE_CONTROL
858 #endif // USE_ITERM_RELAX
860 itermErrorRate
= currentPidSetpoint
- gyroRate
;
861 #if defined(USE_ABSOLUTE_CONTROL)
862 acErrorRate
= itermErrorRate
;
863 #endif // USE_ABSOLUTE_CONTROL
866 #if defined(USE_ABSOLUTE_CONTROL)
867 if (acGain
> 0 && isAirmodeActivated()) {
868 axisError
[axis
] = constrainf(axisError
[axis
] + acErrorRate
* dT
, -acErrorLimit
, acErrorLimit
);
869 acCorrection
= constrainf(axisError
[axis
] * acGain
, -acLimit
, acLimit
);
870 currentPidSetpoint
+= acCorrection
;
871 itermErrorRate
+= acCorrection
;
874 float errorRate
= currentPidSetpoint
- gyroRate
; // r - y
876 pidProfile
->crash_recovery
, angleTrim
, axis
, currentTimeUs
, gyroRate
,
877 ¤tPidSetpoint
, &errorRate
);
879 // --------low-level gyro-based PID based on 2DOF PID controller. ----------
880 // 2-DOF PID controller with optional filter on derivative term.
881 // b = 1 and only c (dtermSetpointWeight) can be tuned (amount derivative on measurement or error).
883 // -----calculate P component and add Dynamic Part based on stick input
884 pidData
[axis
].P
= pidCoefficient
[axis
].Kp
* errorRate
* tpaFactor
;
885 if (axis
== FD_YAW
) {
886 pidData
[axis
].P
= ptermYawLowpassApplyFn((filter_t
*) &ptermYawLowpass
, pidData
[axis
].P
);
889 // -----calculate I component
891 const float ITerm
= pidData
[axis
].I
;
892 const float ITermNew
= constrainf(ITerm
+ pidCoefficient
[axis
].Ki
* itermErrorRate
* dynCi
, -itermLimit
, itermLimit
);
893 const bool outputSaturated
= mixerIsOutputSaturated(axis
, errorRate
);
894 if (outputSaturated
== false || ABS(ITermNew
) < ABS(ITerm
)) {
895 // Only increase ITerm if output is not saturated
896 pidData
[axis
].I
= ITermNew
;
899 // -----calculate D component
900 if (axis
!= FD_YAW
) {
901 // no transition if relaxFactor == 0
902 float transition
= relaxFactor
> 0 ? MIN(1.f
, getRcDeflectionAbs(axis
) * relaxFactor
) : 1;
904 // Divide rate change by dT to get differential (ie dr/dt).
905 // dT is fixed and calculated from the target PID loop time
906 // This is done to avoid DTerm spikes that occur with dynamically
907 // calculated deltaT whenever another task causes the PID
908 // loop execution to be delayed.
910 - (gyroRateDterm
[axis
] - previousGyroRateDterm
[axis
]) * pidFrequency
;
912 detectAndSetCrashRecovery(pidProfile
->crash_recovery
, axis
, currentTimeUs
, delta
, errorRate
);
914 pidData
[axis
].D
= pidCoefficient
[axis
].Kd
* delta
* tpaFactor
;
916 float pidSetpointDelta
= currentPidSetpoint
- previousPidSetpoint
[axis
];
918 #ifdef USE_RC_SMOOTHING_FILTER
919 if (axis
== rcSmoothingDebugAxis
) {
920 DEBUG_SET(DEBUG_RC_SMOOTHING
, 1, lrintf(pidSetpointDelta
* 100.0f
));
922 if ((dynCd
!= 0) && setpointDerivativeLpfInitialized
) {
923 switch (rcSmoothingFilterType
) {
924 case RC_SMOOTHING_DERIVATIVE_PT1
:
925 pidSetpointDelta
= pt1FilterApply(&setpointDerivativePt1
[axis
], pidSetpointDelta
);
927 case RC_SMOOTHING_DERIVATIVE_BIQUAD
:
928 pidSetpointDelta
= biquadFilterApply(&setpointDerivativeBiquad
[axis
], pidSetpointDelta
);
931 if (axis
== rcSmoothingDebugAxis
) {
932 DEBUG_SET(DEBUG_RC_SMOOTHING
, 2, lrintf(pidSetpointDelta
* 100.0f
));
935 #endif // USE_RC_SMOOTHING_FILTER
937 const float pidFeedForward
=
938 pidCoefficient
[axis
].Kd
* dynCd
* transition
* pidSetpointDelta
* tpaFactor
* pidFrequency
;
939 #if defined(USE_SMART_FEEDFORWARD)
940 bool addFeedforward
= true;
941 if (smartFeedforward
) {
942 if (pidData
[axis
].P
* pidFeedForward
> 0) {
943 if (ABS(pidFeedForward
) > ABS(pidData
[axis
].P
)) {
946 addFeedforward
= false;
953 pidData
[axis
].D
+= pidFeedForward
;
955 previousGyroRateDterm
[axis
] = gyroRateDterm
[axis
];
956 previousPidSetpoint
[axis
] = currentPidSetpoint
;
958 #ifdef USE_YAW_SPIN_RECOVERY
960 // zero PIDs on pitch and roll leaving yaw P to correct spin
965 #endif // USE_YAW_SPIN_RECOVERY
969 // calculating the PID sum
970 pidData
[FD_ROLL
].Sum
= pidData
[FD_ROLL
].P
+ pidData
[FD_ROLL
].I
+ pidData
[FD_ROLL
].D
;
971 pidData
[FD_PITCH
].Sum
= pidData
[FD_PITCH
].P
+ pidData
[FD_PITCH
].I
+ pidData
[FD_PITCH
].D
;
973 #ifdef USE_YAW_SPIN_RECOVERY
975 // yaw P alone to correct spin
976 pidData
[FD_YAW
].I
= 0;
978 #endif // USE_YAW_SPIN_RECOVERY
981 pidData
[FD_YAW
].Sum
= pidData
[FD_YAW
].P
+ pidData
[FD_YAW
].I
;
983 // Disable PID control if at zero throttle or if gyro overflow detected
984 // This may look very innefficient, but it is done on purpose to always show real CPU usage as in flight
985 if (!pidStabilisationEnabled
|| gyroOverflowDetected()) {
986 for (int axis
= FD_ROLL
; axis
<= FD_YAW
; ++axis
) {
991 pidData
[axis
].Sum
= 0;
996 bool crashRecoveryModeActive(void)
998 return inCrashRecoveryMode
;
1001 #ifdef USE_ACRO_TRAINER
1002 void pidSetAcroTrainerState(bool newState
)
1004 if (acroTrainerActive
!= newState
) {
1006 pidAcroTrainerInit();
1008 acroTrainerActive
= newState
;
1011 #endif // USE_ACRO_TRAINER