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
= 0,
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_low
= 3,
151 .iterm_relax_cutoff_high
= 30,
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
][2];
210 static FAST_RAM_ZERO_INIT
uint8_t itermRelax
;
211 static FAST_RAM_ZERO_INIT
uint8_t itermRelaxCutoffLow
;
212 static FAST_RAM_ZERO_INIT
uint8_t itermRelaxCutoffHigh
;
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 pt1FilterInit(&throttleLpf
, pt1FilterGain(pidProfile
->throttle_boost_cutoff
, dT
));
298 #if defined(USE_ITERM_RELAX)
300 for (int i
= 0; i
< XYZ_AXIS_COUNT
; i
++) {
301 pt1FilterInit(&windupLpf
[i
][0], pt1FilterGain(itermRelaxCutoffLow
, dT
));
302 pt1FilterInit(&windupLpf
[i
][1], pt1FilterGain(itermRelaxCutoffHigh
, dT
));
308 #ifdef USE_RC_SMOOTHING_FILTER
309 void pidInitSetpointDerivativeLpf(uint16_t filterCutoff
, uint8_t debugAxis
, uint8_t filterType
)
311 rcSmoothingDebugAxis
= debugAxis
;
312 rcSmoothingFilterType
= filterType
;
313 if ((filterCutoff
> 0) && (rcSmoothingFilterType
!= RC_SMOOTHING_DERIVATIVE_OFF
)) {
314 setpointDerivativeLpfInitialized
= true;
315 for (int axis
= FD_ROLL
; axis
<= FD_PITCH
; axis
++) {
316 switch (rcSmoothingFilterType
) {
317 case RC_SMOOTHING_DERIVATIVE_PT1
:
318 pt1FilterInit(&setpointDerivativePt1
[axis
], pt1FilterGain(filterCutoff
, dT
));
320 case RC_SMOOTHING_DERIVATIVE_BIQUAD
:
321 biquadFilterInitLPF(&setpointDerivativeBiquad
[axis
], filterCutoff
, targetPidLooptime
);
327 #endif // USE_RC_SMOOTHING_FILTER
329 typedef struct pidCoefficient_s
{
335 static FAST_RAM_ZERO_INIT pidCoefficient_t pidCoefficient
[3];
336 static FAST_RAM_ZERO_INIT
float maxVelocity
[3];
337 static FAST_RAM_ZERO_INIT
float relaxFactor
;
338 static FAST_RAM_ZERO_INIT
float dtermSetpointWeight
;
339 static FAST_RAM_ZERO_INIT
float levelGain
, horizonGain
, horizonTransition
, horizonCutoffDegrees
, horizonFactorRatio
;
340 static FAST_RAM_ZERO_INIT
float ITermWindupPointInv
;
341 static FAST_RAM_ZERO_INIT
uint8_t horizonTiltExpertMode
;
342 static FAST_RAM_ZERO_INIT timeDelta_t crashTimeLimitUs
;
343 static FAST_RAM_ZERO_INIT timeDelta_t crashTimeDelayUs
;
344 static FAST_RAM_ZERO_INIT
int32_t crashRecoveryAngleDeciDegrees
;
345 static FAST_RAM_ZERO_INIT
float crashRecoveryRate
;
346 static FAST_RAM_ZERO_INIT
float crashDtermThreshold
;
347 static FAST_RAM_ZERO_INIT
float crashGyroThreshold
;
348 static FAST_RAM_ZERO_INIT
float crashSetpointThreshold
;
349 static FAST_RAM_ZERO_INIT
float crashLimitYaw
;
350 static FAST_RAM_ZERO_INIT
float itermLimit
;
351 FAST_RAM_ZERO_INIT
float throttleBoost
;
352 pt1Filter_t throttleLpf
;
353 static FAST_RAM_ZERO_INIT
bool itermRotation
;
355 #if defined(USE_SMART_FEEDFORWARD)
356 static FAST_RAM_ZERO_INIT
bool smartFeedforward
;
358 #if defined(USE_ABSOLUTE_CONTROL)
359 static FAST_RAM_ZERO_INIT
float axisError
[XYZ_AXIS_COUNT
];
360 static FAST_RAM_ZERO_INIT
float acGain
;
361 static FAST_RAM_ZERO_INIT
float acLimit
;
362 static FAST_RAM_ZERO_INIT
float acErrorLimit
;
365 void pidResetITerm(void)
367 for (int axis
= 0; axis
< 3; axis
++) {
368 pidData
[axis
].I
= 0.0f
;
369 #if defined(USE_ABSOLUTE_CONTROL)
370 axisError
[axis
] = 0.0f
;
375 #ifdef USE_ACRO_TRAINER
376 static FAST_RAM_ZERO_INIT
float acroTrainerAngleLimit
;
377 static FAST_RAM_ZERO_INIT
float acroTrainerLookaheadTime
;
378 static FAST_RAM_ZERO_INIT
uint8_t acroTrainerDebugAxis
;
379 static FAST_RAM_ZERO_INIT
bool acroTrainerActive
;
380 static FAST_RAM_ZERO_INIT
int acroTrainerAxisState
[2]; // only need roll and pitch
381 static FAST_RAM_ZERO_INIT
float acroTrainerGain
;
382 #endif // USE_ACRO_TRAINER
384 void pidInitConfig(const pidProfile_t
*pidProfile
)
386 for (int axis
= FD_ROLL
; axis
<= FD_YAW
; axis
++) {
387 pidCoefficient
[axis
].Kp
= PTERM_SCALE
* pidProfile
->pid
[axis
].P
;
388 pidCoefficient
[axis
].Ki
= ITERM_SCALE
* pidProfile
->pid
[axis
].I
;
389 pidCoefficient
[axis
].Kd
= DTERM_SCALE
* pidProfile
->pid
[axis
].D
;
392 dtermSetpointWeight
= pidProfile
->dtermSetpointWeight
/ 127.0f
;
393 if (pidProfile
->setpointRelaxRatio
== 0) {
396 relaxFactor
= 100.0f
/ pidProfile
->setpointRelaxRatio
;
398 levelGain
= pidProfile
->pid
[PID_LEVEL
].P
/ 10.0f
;
399 horizonGain
= pidProfile
->pid
[PID_LEVEL
].I
/ 10.0f
;
400 horizonTransition
= (float)pidProfile
->pid
[PID_LEVEL
].D
;
401 horizonTiltExpertMode
= pidProfile
->horizon_tilt_expert_mode
;
402 horizonCutoffDegrees
= (175 - pidProfile
->horizon_tilt_effect
) * 1.8f
;
403 horizonFactorRatio
= (100 - pidProfile
->horizon_tilt_effect
) * 0.01f
;
404 maxVelocity
[FD_ROLL
] = maxVelocity
[FD_PITCH
] = pidProfile
->rateAccelLimit
* 100 * dT
;
405 maxVelocity
[FD_YAW
] = pidProfile
->yawRateAccelLimit
* 100 * dT
;
406 const float ITermWindupPoint
= (float)pidProfile
->itermWindupPointPercent
/ 100.0f
;
407 ITermWindupPointInv
= 1.0f
/ (1.0f
- ITermWindupPoint
);
408 crashTimeLimitUs
= pidProfile
->crash_time
* 1000;
409 crashTimeDelayUs
= pidProfile
->crash_delay
* 1000;
410 crashRecoveryAngleDeciDegrees
= pidProfile
->crash_recovery_angle
* 10;
411 crashRecoveryRate
= pidProfile
->crash_recovery_rate
;
412 crashGyroThreshold
= pidProfile
->crash_gthreshold
;
413 crashDtermThreshold
= pidProfile
->crash_dthreshold
;
414 crashSetpointThreshold
= pidProfile
->crash_setpoint_threshold
;
415 crashLimitYaw
= pidProfile
->crash_limit_yaw
;
416 itermLimit
= pidProfile
->itermLimit
;
417 throttleBoost
= pidProfile
->throttle_boost
* 0.1f
;
418 itermRotation
= pidProfile
->iterm_rotation
;
419 #if defined(USE_SMART_FEEDFORWARD)
420 smartFeedforward
= pidProfile
->smart_feedforward
;
422 #if defined(USE_ITERM_RELAX)
423 itermRelax
= pidProfile
->iterm_relax
;
424 itermRelaxCutoffLow
= pidProfile
->iterm_relax_cutoff_low
;
425 itermRelaxCutoffHigh
= pidProfile
->iterm_relax_cutoff_high
;
428 #ifdef USE_ACRO_TRAINER
429 acroTrainerAngleLimit
= pidProfile
->acro_trainer_angle_limit
;
430 acroTrainerLookaheadTime
= (float)pidProfile
->acro_trainer_lookahead_ms
/ 1000.0f
;
431 acroTrainerDebugAxis
= pidProfile
->acro_trainer_debug_axis
;
432 acroTrainerGain
= (float)pidProfile
->acro_trainer_gain
/ 10.0f
;
433 #endif // USE_ACRO_TRAINER
435 #if defined(USE_ABSOLUTE_CONTROL)
436 acGain
= (float)pidProfile
->abs_control_gain
;
437 acLimit
= (float)pidProfile
->abs_control_limit
;
438 acErrorLimit
= (float)pidProfile
->abs_control_error_limit
;
442 void pidInit(const pidProfile_t
*pidProfile
)
444 pidSetTargetLooptime(gyro
.targetLooptime
* pidConfig()->pid_process_denom
); // Initialize pid looptime
445 pidInitFilters(pidProfile
);
446 pidInitConfig(pidProfile
);
449 #ifdef USE_ACRO_TRAINER
450 void pidAcroTrainerInit(void)
452 acroTrainerAxisState
[FD_ROLL
] = 0;
453 acroTrainerAxisState
[FD_PITCH
] = 0;
455 #endif // USE_ACRO_TRAINER
457 void pidCopyProfile(uint8_t dstPidProfileIndex
, uint8_t srcPidProfileIndex
)
459 if ((dstPidProfileIndex
< MAX_PROFILE_COUNT
-1 && srcPidProfileIndex
< MAX_PROFILE_COUNT
-1)
460 && dstPidProfileIndex
!= srcPidProfileIndex
462 memcpy(pidProfilesMutable(dstPidProfileIndex
), pidProfilesMutable(srcPidProfileIndex
), sizeof(pidProfile_t
));
466 // calculates strength of horizon leveling; 0 = none, 1.0 = most leveling
467 static float calcHorizonLevelStrength(void)
469 // start with 1.0 at center stick, 0.0 at max stick deflection:
470 float horizonLevelStrength
= 1.0f
- MAX(getRcDeflectionAbs(FD_ROLL
), getRcDeflectionAbs(FD_PITCH
));
472 // 0 at level, 90 at vertical, 180 at inverted (degrees):
473 const float currentInclination
= MAX(ABS(attitude
.values
.roll
), ABS(attitude
.values
.pitch
)) / 10.0f
;
475 // horizonTiltExpertMode: 0 = leveling always active when sticks centered,
476 // 1 = leveling can be totally off when inverted
477 if (horizonTiltExpertMode
) {
478 if (horizonTransition
> 0 && horizonCutoffDegrees
> 0) {
479 // if d_level > 0 and horizonTiltEffect < 175
480 // horizonCutoffDegrees: 0 to 125 => 270 to 90 (represents where leveling goes to zero)
481 // inclinationLevelRatio (0.0 to 1.0) is smaller (less leveling)
482 // for larger inclinations; 0.0 at horizonCutoffDegrees value:
483 const float inclinationLevelRatio
= constrainf((horizonCutoffDegrees
-currentInclination
) / horizonCutoffDegrees
, 0, 1);
484 // apply configured horizon sensitivity:
485 // when stick is near center (horizonLevelStrength ~= 1.0)
486 // H_sensitivity value has little effect,
487 // when stick is deflected (horizonLevelStrength near 0.0)
488 // H_sensitivity value has more effect:
489 horizonLevelStrength
= (horizonLevelStrength
- 1) * 100 / horizonTransition
+ 1;
490 // apply inclination ratio, which may lower leveling
491 // to zero regardless of stick position:
492 horizonLevelStrength
*= inclinationLevelRatio
;
493 } else { // d_level=0 or horizon_tilt_effect>=175 means no leveling
494 horizonLevelStrength
= 0;
496 } else { // horizon_tilt_expert_mode = 0 (leveling always active when sticks centered)
498 if (horizonFactorRatio
< 1.01f
) { // if horizonTiltEffect > 0
499 // horizonFactorRatio: 1.0 to 0.0 (larger means more leveling)
500 // inclinationLevelRatio (0.0 to 1.0) is smaller (less leveling)
501 // for larger inclinations, goes to 1.0 at inclination==level:
502 const float inclinationLevelRatio
= (180-currentInclination
)/180 * (1.0f
-horizonFactorRatio
) + horizonFactorRatio
;
503 // apply ratio to configured horizon sensitivity:
504 sensitFact
= horizonTransition
* inclinationLevelRatio
;
505 } else { // horizonTiltEffect=0 for "old" functionality
506 sensitFact
= horizonTransition
;
509 if (sensitFact
<= 0) { // zero means no leveling
510 horizonLevelStrength
= 0;
512 // when stick is near center (horizonLevelStrength ~= 1.0)
513 // sensitFact value has little effect,
514 // when stick is deflected (horizonLevelStrength near 0.0)
515 // sensitFact value has more effect:
516 horizonLevelStrength
= ((horizonLevelStrength
- 1) * (100 / sensitFact
)) + 1;
519 return constrainf(horizonLevelStrength
, 0, 1);
522 static float pidLevel(int axis
, const pidProfile_t
*pidProfile
, const rollAndPitchTrims_t
*angleTrim
, float currentPidSetpoint
) {
523 // calculate error angle and limit the angle to the max inclination
524 // rcDeflection is in range [-1.0, 1.0]
525 float angle
= pidProfile
->levelAngleLimit
* getRcDeflection(axis
);
526 #ifdef USE_GPS_RESCUE
527 angle
+= gpsRescueAngle
[axis
] / 100; // ANGLE IS IN CENTIDEGREES
529 angle
= constrainf(angle
, -pidProfile
->levelAngleLimit
, pidProfile
->levelAngleLimit
);
530 const float errorAngle
= angle
- ((attitude
.raw
[axis
] - angleTrim
->raw
[axis
]) / 10.0f
);
531 if (FLIGHT_MODE(ANGLE_MODE
) || FLIGHT_MODE(GPS_RESCUE_MODE
)) {
532 // ANGLE mode - control is angle based
533 currentPidSetpoint
= errorAngle
* levelGain
;
535 // HORIZON mode - mix of ANGLE and ACRO modes
536 // mix in errorAngle to currentPidSetpoint to add a little auto-level feel
537 const float horizonLevelStrength
= calcHorizonLevelStrength();
538 currentPidSetpoint
= currentPidSetpoint
+ (errorAngle
* horizonGain
* horizonLevelStrength
);
540 return currentPidSetpoint
;
543 static float accelerationLimit(int axis
, float currentPidSetpoint
)
545 static float previousSetpoint
[3];
546 const float currentVelocity
= currentPidSetpoint
- previousSetpoint
[axis
];
548 if (ABS(currentVelocity
) > maxVelocity
[axis
]) {
549 currentPidSetpoint
= (currentVelocity
> 0) ? previousSetpoint
[axis
] + maxVelocity
[axis
] : previousSetpoint
[axis
] - maxVelocity
[axis
];
552 previousSetpoint
[axis
] = currentPidSetpoint
;
553 return currentPidSetpoint
;
556 static timeUs_t crashDetectedAtUs
;
558 static void handleCrashRecovery(
559 const pidCrashRecovery_e crash_recovery
, const rollAndPitchTrims_t
*angleTrim
,
560 const int axis
, const timeUs_t currentTimeUs
, const float gyroRate
, float *currentPidSetpoint
, float *errorRate
)
562 if (inCrashRecoveryMode
&& cmpTimeUs(currentTimeUs
, crashDetectedAtUs
) > crashTimeDelayUs
) {
563 if (crash_recovery
== PID_CRASH_RECOVERY_BEEP
) {
566 if (axis
== FD_YAW
) {
567 *errorRate
= constrainf(*errorRate
, -crashLimitYaw
, crashLimitYaw
);
569 // on roll and pitch axes calculate currentPidSetpoint and errorRate to level the aircraft to recover from crash
570 if (sensors(SENSOR_ACC
)) {
571 // errorAngle is deviation from horizontal
572 const float errorAngle
= -(attitude
.raw
[axis
] - angleTrim
->raw
[axis
]) / 10.0f
;
573 *currentPidSetpoint
= errorAngle
* levelGain
;
574 *errorRate
= *currentPidSetpoint
- gyroRate
;
577 // reset ITerm, since accumulated error before crash is now meaningless
578 // and ITerm windup during crash recovery can be extreme, especially on yaw axis
579 pidData
[axis
].I
= 0.0f
;
580 if (cmpTimeUs(currentTimeUs
, crashDetectedAtUs
) > crashTimeLimitUs
581 || (getMotorMixRange() < 1.0f
582 && ABS(gyro
.gyroADCf
[FD_ROLL
]) < crashRecoveryRate
583 && ABS(gyro
.gyroADCf
[FD_PITCH
]) < crashRecoveryRate
584 && ABS(gyro
.gyroADCf
[FD_YAW
]) < crashRecoveryRate
)) {
585 if (sensors(SENSOR_ACC
)) {
586 // check aircraft nearly level
587 if (ABS(attitude
.raw
[FD_ROLL
] - angleTrim
->raw
[FD_ROLL
]) < crashRecoveryAngleDeciDegrees
588 && ABS(attitude
.raw
[FD_PITCH
] - angleTrim
->raw
[FD_PITCH
]) < crashRecoveryAngleDeciDegrees
) {
589 inCrashRecoveryMode
= false;
593 inCrashRecoveryMode
= false;
600 static void detectAndSetCrashRecovery(
601 const pidCrashRecovery_e crash_recovery
, const int axis
,
602 const timeUs_t currentTimeUs
, const float delta
, const float errorRate
)
604 // if crash recovery is on and accelerometer enabled and there is no gyro overflow, then check for a crash
605 // no point in trying to recover if the crash is so severe that the gyro overflows
606 if ((crash_recovery
|| FLIGHT_MODE(GPS_RESCUE_MODE
)) && !gyroOverflowDetected()) {
607 if (ARMING_FLAG(ARMED
)) {
608 if (getMotorMixRange() >= 1.0f
&& !inCrashRecoveryMode
609 && ABS(delta
) > crashDtermThreshold
610 && ABS(errorRate
) > crashGyroThreshold
611 && ABS(getSetpointRate(axis
)) < crashSetpointThreshold
) {
612 inCrashRecoveryMode
= true;
613 crashDetectedAtUs
= currentTimeUs
;
615 if (inCrashRecoveryMode
&& cmpTimeUs(currentTimeUs
, crashDetectedAtUs
) < crashTimeDelayUs
&& (ABS(errorRate
) < crashGyroThreshold
616 || ABS(getSetpointRate(axis
)) > crashSetpointThreshold
)) {
617 inCrashRecoveryMode
= false;
620 } else if (inCrashRecoveryMode
) {
621 inCrashRecoveryMode
= false;
627 static void rotateVector(float v
[XYZ_AXIS_COUNT
], float rotation
[XYZ_AXIS_COUNT
])
629 // rotate v around rotation vector rotation
630 // rotation in radians, all elements must be small
631 for (int i
= 0; i
< XYZ_AXIS_COUNT
; i
++) {
632 int i_1
= (i
+ 1) % 3;
633 int i_2
= (i
+ 2) % 3;
634 float newV
= v
[i_1
] + v
[i_2
] * rotation
[i
];
635 v
[i_2
] -= v
[i_1
] * rotation
[i
];
640 static void rotateITermAndAxisError()
643 #if defined(USE_ABSOLUTE_CONTROL)
647 const float gyroToAngle
= dT
* RAD
;
648 float rotationRads
[3];
649 for (int i
= FD_ROLL
; i
<= FD_YAW
; i
++) {
650 rotationRads
[i
] = gyro
.gyroADCf
[i
] * gyroToAngle
;
652 #if defined(USE_ABSOLUTE_CONTROL)
654 rotateVector(axisError
, rotationRads
);
659 for (int i
= 0; i
< XYZ_AXIS_COUNT
; i
++) {
662 rotateVector(v
, rotationRads
);
663 for (int i
= 0; i
< XYZ_AXIS_COUNT
; i
++) {
670 #ifdef USE_ACRO_TRAINER
672 int acroTrainerSign(float x
)
674 return x
> 0 ? 1 : -1;
677 // Acro Trainer - Manipulate the setPoint to limit axis angle while in acro mode
678 // There are three states:
679 // 1. Current angle has exceeded limit
680 // Apply correction to return to limit (similar to pidLevel)
681 // 2. Future overflow has been projected based on current angle and gyro rate
682 // Manage the setPoint to control the gyro rate as the actual angle approaches the limit (try to prevent overshoot)
683 // 3. If no potential overflow is detected, then return the original setPoint
685 // Use the FAST_CODE_NOINLINE directive to avoid this code from being inlined into ITCM RAM. We accept the
686 // performance decrease when Acro Trainer mode is active under the assumption that user is unlikely to be
687 // expecting ultimate flight performance at very high loop rates when in this mode.
688 static FAST_CODE_NOINLINE
float applyAcroTrainer(int axis
, const rollAndPitchTrims_t
*angleTrim
, float setPoint
)
690 float ret
= setPoint
;
692 if (!FLIGHT_MODE(ANGLE_MODE
) && !FLIGHT_MODE(HORIZON_MODE
) && !FLIGHT_MODE(GPS_RESCUE_MODE
)) {
693 bool resetIterm
= false;
694 float projectedAngle
= 0;
695 const int setpointSign
= acroTrainerSign(setPoint
);
696 const float currentAngle
= (attitude
.raw
[axis
] - angleTrim
->raw
[axis
]) / 10.0f
;
697 const int angleSign
= acroTrainerSign(currentAngle
);
699 if ((acroTrainerAxisState
[axis
] != 0) && (acroTrainerAxisState
[axis
] != setpointSign
)) { // stick has reversed - stop limiting
700 acroTrainerAxisState
[axis
] = 0;
703 // Limit and correct the angle when it exceeds the limit
704 if ((fabsf(currentAngle
) > acroTrainerAngleLimit
) && (acroTrainerAxisState
[axis
] == 0)) {
705 if (angleSign
== setpointSign
) {
706 acroTrainerAxisState
[axis
] = angleSign
;
711 if (acroTrainerAxisState
[axis
] != 0) {
712 ret
= constrainf(((acroTrainerAngleLimit
* angleSign
) - currentAngle
) * acroTrainerGain
, -ACRO_TRAINER_SETPOINT_LIMIT
, ACRO_TRAINER_SETPOINT_LIMIT
);
715 // Not currently over the limit so project the angle based on current angle and
716 // gyro angular rate using a sliding window based on gyro rate (faster rotation means larger window.
717 // If the projected angle exceeds the limit then apply limiting to minimize overshoot.
718 // Calculate the lookahead window by scaling proportionally with gyro rate from 0-500dps
719 float checkInterval
= constrainf(fabsf(gyro
.gyroADCf
[axis
]) / ACRO_TRAINER_LOOKAHEAD_RATE_LIMIT
, 0.0f
, 1.0f
) * acroTrainerLookaheadTime
;
720 projectedAngle
= (gyro
.gyroADCf
[axis
] * checkInterval
) + currentAngle
;
721 const int projectedAngleSign
= acroTrainerSign(projectedAngle
);
722 if ((fabsf(projectedAngle
) > acroTrainerAngleLimit
) && (projectedAngleSign
== setpointSign
)) {
723 ret
= ((acroTrainerAngleLimit
* projectedAngleSign
) - projectedAngle
) * acroTrainerGain
;
732 if (axis
== acroTrainerDebugAxis
) {
733 DEBUG_SET(DEBUG_ACRO_TRAINER
, 0, lrintf(currentAngle
* 10.0f
));
734 DEBUG_SET(DEBUG_ACRO_TRAINER
, 1, acroTrainerAxisState
[axis
]);
735 DEBUG_SET(DEBUG_ACRO_TRAINER
, 2, lrintf(ret
));
736 DEBUG_SET(DEBUG_ACRO_TRAINER
, 3, lrintf(projectedAngle
* 10.0f
));
742 #endif // USE_ACRO_TRAINER
744 // Betaflight pid controller, which will be maintained in the future with additional features specialised for current (mini) multirotor usage.
745 // Based on 2DOF reference design (matlab)
746 void FAST_CODE
pidController(const pidProfile_t
*pidProfile
, const rollAndPitchTrims_t
*angleTrim
, timeUs_t currentTimeUs
)
748 static float previousGyroRateDterm
[2];
749 static float previousPidSetpoint
[2];
751 const float tpaFactor
= getThrottlePIDAttenuation();
752 const float motorMixRange
= getMotorMixRange();
754 #ifdef USE_YAW_SPIN_RECOVERY
755 const bool yawSpinActive
= gyroYawSpinDetected();
758 // Dynamic i component,
759 // gradually scale back integration when above windup point
760 const float dynCi
= MIN((1.0f
- motorMixRange
) * ITermWindupPointInv
, 1.0f
) * dT
* itermAccelerator
;
762 // Dynamic d component, enable 2-DOF PID controller only for rate mode
763 const float dynCd
= flightModeFlags
? 0.0f
: dtermSetpointWeight
;
765 // Precalculate gyro deta for D-term here, this allows loop unrolling
766 float gyroRateDterm
[2];
767 for (int axis
= FD_ROLL
; axis
< FD_YAW
; ++axis
) {
768 gyroRateDterm
[axis
] = dtermNotchApplyFn((filter_t
*) &dtermNotch
[axis
], gyro
.gyroADCf
[axis
]);
769 gyroRateDterm
[axis
] = dtermLowpassApplyFn((filter_t
*) &dtermLowpass
[axis
], gyroRateDterm
[axis
]);
770 gyroRateDterm
[axis
] = dtermLowpass2ApplyFn((filter_t
*) &dtermLowpass2
[axis
], gyroRateDterm
[axis
]);
773 rotateITermAndAxisError();
775 // ----------PID controller----------
776 for (int axis
= FD_ROLL
; axis
<= FD_YAW
; ++axis
) {
777 float currentPidSetpoint
= getSetpointRate(axis
);
778 if (maxVelocity
[axis
]) {
779 currentPidSetpoint
= accelerationLimit(axis
, currentPidSetpoint
);
781 // Yaw control is GYRO based, direct sticks control is applied to rate PID
782 if ((FLIGHT_MODE(ANGLE_MODE
) || FLIGHT_MODE(HORIZON_MODE
) || FLIGHT_MODE(GPS_RESCUE_MODE
)) && axis
!= YAW
) {
783 currentPidSetpoint
= pidLevel(axis
, pidProfile
, angleTrim
, currentPidSetpoint
);
786 #ifdef USE_ACRO_TRAINER
787 if ((axis
!= FD_YAW
) && acroTrainerActive
&& !inCrashRecoveryMode
) {
788 currentPidSetpoint
= applyAcroTrainer(axis
, angleTrim
, currentPidSetpoint
);
790 #endif // USE_ACRO_TRAINER
792 // Handle yaw spin recovery - zero the setpoint on yaw to aid in recovery
793 // It's not necessary to zero the set points for R/P because the PIDs will be zeroed below
794 #ifdef USE_YAW_SPIN_RECOVERY
795 if ((axis
== FD_YAW
) && yawSpinActive
) {
796 currentPidSetpoint
= 0.0f
;
798 #endif // USE_YAW_SPIN_RECOVERY
800 #if defined(USE_ABSOLUTE_CONTROL)
801 // mix in a correction for accrued attitude error
802 float acCorrection
= 0;
804 acCorrection
= constrainf(axisError
[axis
] * acGain
, -acLimit
, acLimit
);
805 currentPidSetpoint
+= acCorrection
;
809 // -----calculate error rate
810 const float gyroRate
= gyro
.gyroADCf
[axis
]; // Process variable from gyro output in deg/sec
811 float errorRate
= currentPidSetpoint
- gyroRate
; // r - y
814 pidProfile
->crash_recovery
, angleTrim
, axis
, currentTimeUs
, gyroRate
,
815 ¤tPidSetpoint
, &errorRate
);
817 // --------low-level gyro-based PID based on 2DOF PID controller. ----------
818 // 2-DOF PID controller with optional filter on derivative term.
819 // b = 1 and only c (dtermSetpointWeight) can be tuned (amount derivative on measurement or error).
821 // -----calculate P component and add Dynamic Part based on stick input
822 pidData
[axis
].P
= pidCoefficient
[axis
].Kp
* errorRate
* tpaFactor
;
823 if (axis
== FD_YAW
) {
824 pidData
[axis
].P
= ptermYawLowpassApplyFn((filter_t
*) &ptermYawLowpass
, pidData
[axis
].P
);
827 // -----calculate I component
828 float itermErrorRate
= 0.0f
;
829 #if defined(USE_ABSOLUTE_CONTROL)
832 #if defined(USE_ITERM_RELAX)
833 if (itermRelax
&& (axis
< FD_YAW
|| itermRelax
== ITERM_RELAX_RPY
)) {
834 float gyroTargetLow
= pt1FilterApply(&windupLpf
[axis
][0], currentPidSetpoint
);
835 float gyroTargetHigh
= pt1FilterApply(&windupLpf
[axis
][1], currentPidSetpoint
);
836 #if defined(USE_ABSOLUTE_CONTROL)
837 gyroTargetLow
+= acCorrection
;
838 gyroTargetHigh
+= acCorrection
;
841 int itemOffset
= (axis
<< 1);
842 DEBUG_SET(DEBUG_ITERM_RELAX
, itemOffset
++, gyroTargetHigh
);
843 DEBUG_SET(DEBUG_ITERM_RELAX
, itemOffset
, gyroTargetLow
);
845 const float gmax
= MAX(gyroTargetHigh
, gyroTargetLow
);
846 const float gmin
= MIN(gyroTargetHigh
, gyroTargetLow
);
847 if (gyroRate
< gmin
|| gyroRate
> gmax
) {
848 itermErrorRate
= (gyroRate
> gmax
? gmax
: gmin
) - gyroRate
;
850 #if defined(USE_ABSOLUTE_CONTROL)
852 itermErrorRate
= acCorrection
;
854 acErrorRate
= itermErrorRate
- acCorrection
;
857 #endif // USE_ITERM_RELAX
859 itermErrorRate
= errorRate
;
860 #if defined(USE_ABSOLUTE_CONTROL)
861 acErrorRate
= errorRate
;
865 #if defined(USE_ABSOLUTE_CONTROL)
867 axisError
[axis
] = constrainf(axisError
[axis
] + acErrorRate
* dT
, -acErrorLimit
, acErrorLimit
);
871 const float ITerm
= pidData
[axis
].I
;
872 const float ITermNew
= constrainf(ITerm
+ pidCoefficient
[axis
].Ki
* itermErrorRate
* dynCi
, -itermLimit
, itermLimit
);
873 const bool outputSaturated
= mixerIsOutputSaturated(axis
, errorRate
);
874 if (outputSaturated
== false || ABS(ITermNew
) < ABS(ITerm
)) {
875 // Only increase ITerm if output is not saturated
876 pidData
[axis
].I
= ITermNew
;
879 // -----calculate D component
880 if (axis
!= FD_YAW
) {
881 // no transition if relaxFactor == 0
882 float transition
= relaxFactor
> 0 ? MIN(1.f
, getRcDeflectionAbs(axis
) * relaxFactor
) : 1;
884 // Divide rate change by dT to get differential (ie dr/dt).
885 // dT is fixed and calculated from the target PID loop time
886 // This is done to avoid DTerm spikes that occur with dynamically
887 // calculated deltaT whenever another task causes the PID
888 // loop execution to be delayed.
890 - (gyroRateDterm
[axis
] - previousGyroRateDterm
[axis
]) * pidFrequency
;
892 detectAndSetCrashRecovery(pidProfile
->crash_recovery
, axis
, currentTimeUs
, delta
, errorRate
);
894 pidData
[axis
].D
= pidCoefficient
[axis
].Kd
* delta
* tpaFactor
;
896 float pidSetpointDelta
= currentPidSetpoint
- previousPidSetpoint
[axis
];
898 #ifdef USE_RC_SMOOTHING_FILTER
899 if (axis
== rcSmoothingDebugAxis
) {
900 DEBUG_SET(DEBUG_RC_SMOOTHING
, 1, lrintf(pidSetpointDelta
* 100.0f
));
902 if ((dynCd
!= 0) && setpointDerivativeLpfInitialized
) {
903 switch (rcSmoothingFilterType
) {
904 case RC_SMOOTHING_DERIVATIVE_PT1
:
905 pidSetpointDelta
= pt1FilterApply(&setpointDerivativePt1
[axis
], pidSetpointDelta
);
907 case RC_SMOOTHING_DERIVATIVE_BIQUAD
:
908 pidSetpointDelta
= biquadFilterApply(&setpointDerivativeBiquad
[axis
], pidSetpointDelta
);
911 if (axis
== rcSmoothingDebugAxis
) {
912 DEBUG_SET(DEBUG_RC_SMOOTHING
, 2, lrintf(pidSetpointDelta
* 100.0f
));
915 #endif // USE_RC_SMOOTHING_FILTER
917 const float pidFeedForward
=
918 pidCoefficient
[axis
].Kd
* dynCd
* transition
* pidSetpointDelta
* tpaFactor
* pidFrequency
;
919 #if defined(USE_SMART_FEEDFORWARD)
920 bool addFeedforward
= true;
921 if (smartFeedforward
) {
922 if (pidData
[axis
].P
* pidFeedForward
> 0) {
923 if (ABS(pidFeedForward
) > ABS(pidData
[axis
].P
)) {
926 addFeedforward
= false;
933 pidData
[axis
].D
+= pidFeedForward
;
935 previousGyroRateDterm
[axis
] = gyroRateDterm
[axis
];
936 previousPidSetpoint
[axis
] = currentPidSetpoint
;
938 #ifdef USE_YAW_SPIN_RECOVERY
940 // zero PIDs on pitch and roll leaving yaw P to correct spin
945 #endif // USE_YAW_SPIN_RECOVERY
949 // calculating the PID sum
950 pidData
[FD_ROLL
].Sum
= pidData
[FD_ROLL
].P
+ pidData
[FD_ROLL
].I
+ pidData
[FD_ROLL
].D
;
951 pidData
[FD_PITCH
].Sum
= pidData
[FD_PITCH
].P
+ pidData
[FD_PITCH
].I
+ pidData
[FD_PITCH
].D
;
953 #ifdef USE_YAW_SPIN_RECOVERY
955 // yaw P alone to correct spin
956 pidData
[FD_YAW
].I
= 0;
958 #endif // USE_YAW_SPIN_RECOVERY
961 pidData
[FD_YAW
].Sum
= pidData
[FD_YAW
].P
+ pidData
[FD_YAW
].I
;
963 // Disable PID control if at zero throttle or if gyro overflow detected
964 // This may look very innefficient, but it is done on purpose to always show real CPU usage as in flight
965 if (!pidStabilisationEnabled
|| gyroOverflowDetected()) {
966 for (int axis
= FD_ROLL
; axis
<= FD_YAW
; ++axis
) {
971 pidData
[axis
].Sum
= 0;
976 bool crashRecoveryModeActive(void)
978 return inCrashRecoveryMode
;
981 #ifdef USE_ACRO_TRAINER
982 void pidSetAcroTrainerState(bool newState
)
984 if (acroTrainerActive
!= newState
) {
986 pidAcroTrainerInit();
988 acroTrainerActive
= newState
;
991 #endif // USE_ACRO_TRAINER