2 * This file is part of Cleanflight and Betaflight.
4 * Cleanflight and Betaflight are free software. You can redistribute
5 * this software and/or modify this software under the terms of the
6 * GNU General Public License as published by the Free Software
7 * Foundation, either version 3 of the License, or (at your option)
10 * Cleanflight and Betaflight are distributed in the hope that they
11 * will be useful, but WITHOUT ANY WARRANTY; without even the implied
12 * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
13 * See the GNU General Public License for more details.
15 * You should have received a copy of the GNU General Public License
16 * along with this software.
18 * If not, see <http://www.gnu.org/licenses/>.
28 #include "build/build_config.h"
29 #include "build/debug.h"
31 #include "common/axis.h"
32 #include "common/maths.h"
33 #include "common/filter.h"
35 #include "config/config_reset.h"
37 #include "pg/pg_ids.h"
39 #include "drivers/sound_beeper.h"
40 #include "drivers/time.h"
42 #include "fc/fc_core.h"
45 #include "fc/rc_controls.h"
46 #include "fc/runtime_config.h"
48 #include "flight/pid.h"
49 #include "flight/imu.h"
50 #include "flight/gps_rescue.h"
51 #include "flight/mixer.h"
55 #include "sensors/gyro.h"
56 #include "sensors/acceleration.h"
59 FAST_RAM_ZERO_INIT
uint32_t targetPidLooptime
;
60 FAST_RAM_ZERO_INIT pidAxisData_t pidData
[XYZ_AXIS_COUNT
];
62 static FAST_RAM_ZERO_INIT
bool pidStabilisationEnabled
;
64 static FAST_RAM_ZERO_INIT
bool inCrashRecoveryMode
= false;
66 static FAST_RAM_ZERO_INIT
float dT
;
68 PG_REGISTER_WITH_RESET_TEMPLATE(pidConfig_t
, pidConfig
, PG_PID_CONFIG
, 2);
71 #define PID_PROCESS_DENOM_DEFAULT 1
72 #elif defined(USE_GYRO_SPI_MPU6000) || defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_ICM20689)
73 #define PID_PROCESS_DENOM_DEFAULT 4
75 #define PID_PROCESS_DENOM_DEFAULT 2
78 #ifdef USE_RUNAWAY_TAKEOFF
79 PG_RESET_TEMPLATE(pidConfig_t
, pidConfig
,
80 .pid_process_denom
= PID_PROCESS_DENOM_DEFAULT
,
81 .runaway_takeoff_prevention
= true,
82 .runaway_takeoff_deactivate_throttle
= 25, // throttle level % needed to accumulate deactivation time
83 .runaway_takeoff_deactivate_delay
= 500 // Accumulated time (in milliseconds) before deactivation in successful takeoff
86 PG_RESET_TEMPLATE(pidConfig_t
, pidConfig
,
87 .pid_process_denom
= PID_PROCESS_DENOM_DEFAULT
91 #ifdef USE_ACRO_TRAINER
92 #define ACRO_TRAINER_LOOKAHEAD_RATE_LIMIT 500.0f // Max gyro rate for lookahead time scaling
93 #define ACRO_TRAINER_SETPOINT_LIMIT 1000.0f // Limit the correcting setpoint
94 #endif // USE_ACRO_TRAINER
96 PG_REGISTER_ARRAY_WITH_RESET_FN(pidProfile_t
, MAX_PROFILE_COUNT
, pidProfiles
, PG_PID_PROFILE
, 4);
98 void resetPidProfile(pidProfile_t
*pidProfile
)
100 RESET_CONFIG(pidProfile_t
, pidProfile
,
102 [PID_ROLL
] = { 40, 40, 30 },
103 [PID_PITCH
] = { 58, 50, 35 },
104 [PID_YAW
] = { 70, 45, 20 },
105 [PID_ALT
] = { 50, 0, 0 },
106 [PID_POS
] = { 15, 0, 0 }, // POSHOLD_P * 100, POSHOLD_I * 100,
107 [PID_POSR
] = { 34, 14, 53 }, // POSHOLD_RATE_P * 10, POSHOLD_RATE_I * 100, POSHOLD_RATE_D * 1000,
108 [PID_NAVR
] = { 25, 33, 83 }, // NAV_P * 10, NAV_I * 100, NAV_D * 1000
109 [PID_LEVEL
] = { 50, 50, 75 },
110 [PID_MAG
] = { 40, 0, 0 },
111 [PID_VEL
] = { 55, 55, 75 }
114 .pidSumLimit
= PIDSUM_LIMIT
,
115 .pidSumLimitYaw
= PIDSUM_LIMIT_YAW
,
117 .dterm_lowpass_hz
= 100, // filtering ON by default
118 .dterm_lowpass2_hz
= 0, // second Dterm LPF OFF by default
119 .dterm_notch_hz
= 260,
120 .dterm_notch_cutoff
= 160,
121 .dterm_filter_type
= FILTER_BIQUAD
,
122 .itermWindupPointPercent
= 50,
123 .vbatPidCompensation
= 0,
124 .pidAtMinThrottle
= PID_STABILISATION_ON
,
125 .levelAngleLimit
= 55,
126 .setpointRelaxRatio
= 100,
127 .dtermSetpointWeight
= 0,
128 .yawRateAccelLimit
= 100,
130 .itermThrottleThreshold
= 350,
131 .itermAcceleratorGain
= 1000,
132 .crash_time
= 500, // ms
133 .crash_delay
= 0, // ms
134 .crash_recovery_angle
= 10, // degrees
135 .crash_recovery_rate
= 100, // degrees/second
136 .crash_dthreshold
= 50, // degrees/second/second
137 .crash_gthreshold
= 400, // degrees/second
138 .crash_setpoint_threshold
= 350, // degrees/second
139 .crash_recovery
= PID_CRASH_RECOVERY_OFF
, // off by default
140 .horizon_tilt_effect
= 75,
141 .horizon_tilt_expert_mode
= false,
142 .crash_limit_yaw
= 200,
145 .throttle_boost_cutoff
= 15,
146 .iterm_rotation
= false,
147 .smart_feedforward
= false,
148 .iterm_relax
= ITERM_RELAX_OFF
,
149 .iterm_relax_cutoff_low
= 3,
150 .iterm_relax_cutoff_high
= 30,
151 .acro_trainer_angle_limit
= 20,
152 .acro_trainer_lookahead_ms
= 50,
153 .acro_trainer_debug_axis
= FD_ROLL
,
154 .acro_trainer_gain
= 75,
155 .abs_control_gain
= 0,
156 .abs_control_limit
= 90,
157 .abs_control_error_limit
= 20,
161 void pgResetFn_pidProfiles(pidProfile_t
*pidProfiles
)
163 for (int i
= 0; i
< MAX_PROFILE_COUNT
; i
++) {
164 resetPidProfile(&pidProfiles
[i
]);
168 static void pidSetTargetLooptime(uint32_t pidLooptime
)
170 targetPidLooptime
= pidLooptime
;
171 dT
= (float)targetPidLooptime
* 0.000001f
;
174 static FAST_RAM
float itermAccelerator
= 1.0f
;
176 void pidSetItermAccelerator(float newItermAccelerator
)
178 itermAccelerator
= newItermAccelerator
;
181 float pidItermAccelerator(void)
183 return itermAccelerator
;
186 void pidStabilisationState(pidStabilisationState_e pidControllerState
)
188 pidStabilisationEnabled
= (pidControllerState
== PID_STABILISATION_ON
) ? true : false;
191 const angle_index_t rcAliasToAngleIndexMap
[] = { AI_ROLL
, AI_PITCH
};
193 typedef union dtermLowpass_u
{
194 pt1Filter_t pt1Filter
;
195 biquadFilter_t biquadFilter
;
198 static FAST_RAM_ZERO_INIT filterApplyFnPtr dtermNotchApplyFn
;
199 static FAST_RAM_ZERO_INIT biquadFilter_t dtermNotch
[2];
200 static FAST_RAM_ZERO_INIT filterApplyFnPtr dtermLowpassApplyFn
;
201 static FAST_RAM_ZERO_INIT dtermLowpass_t dtermLowpass
[2];
202 static FAST_RAM_ZERO_INIT filterApplyFnPtr dtermLowpass2ApplyFn
;
203 static FAST_RAM_ZERO_INIT pt1Filter_t dtermLowpass2
[2];
204 static FAST_RAM_ZERO_INIT filterApplyFnPtr ptermYawLowpassApplyFn
;
205 static FAST_RAM_ZERO_INIT pt1Filter_t ptermYawLowpass
;
206 #if defined(USE_ITERM_RELAX)
207 static FAST_RAM_ZERO_INIT pt1Filter_t windupLpf
[XYZ_AXIS_COUNT
][2];
208 static FAST_RAM_ZERO_INIT
uint8_t itermRelax
;
209 static FAST_RAM_ZERO_INIT
uint8_t itermRelaxCutoffLow
;
210 static FAST_RAM_ZERO_INIT
uint8_t itermRelaxCutoffHigh
;
213 void pidInitFilters(const pidProfile_t
*pidProfile
)
215 BUILD_BUG_ON(FD_YAW
!= 2); // only setting up Dterm filters on roll and pitch axes, so ensure yaw axis is 2
217 if (targetPidLooptime
== 0) {
218 // no looptime set, so set all the filters to null
219 dtermNotchApplyFn
= nullFilterApply
;
220 dtermLowpassApplyFn
= nullFilterApply
;
221 ptermYawLowpassApplyFn
= nullFilterApply
;
225 const uint32_t pidFrequencyNyquist
= (1.0f
/ dT
) / 2; // No rounding needed
227 uint16_t dTermNotchHz
;
228 if (pidProfile
->dterm_notch_hz
<= pidFrequencyNyquist
) {
229 dTermNotchHz
= pidProfile
->dterm_notch_hz
;
231 if (pidProfile
->dterm_notch_cutoff
< pidFrequencyNyquist
) {
232 dTermNotchHz
= pidFrequencyNyquist
;
238 if (dTermNotchHz
!= 0 && pidProfile
->dterm_notch_cutoff
!= 0) {
239 dtermNotchApplyFn
= (filterApplyFnPtr
)biquadFilterApply
;
240 const float notchQ
= filterGetNotchQ(dTermNotchHz
, pidProfile
->dterm_notch_cutoff
);
241 for (int axis
= FD_ROLL
; axis
<= FD_PITCH
; axis
++) {
242 biquadFilterInit(&dtermNotch
[axis
], dTermNotchHz
, targetPidLooptime
, notchQ
, FILTER_NOTCH
);
245 dtermNotchApplyFn
= nullFilterApply
;
248 //2nd Dterm Lowpass Filter
249 if (pidProfile
->dterm_lowpass2_hz
== 0 || pidProfile
->dterm_lowpass2_hz
> pidFrequencyNyquist
) {
250 dtermLowpass2ApplyFn
= nullFilterApply
;
252 dtermLowpass2ApplyFn
= (filterApplyFnPtr
)pt1FilterApply
;
253 for (int axis
= FD_ROLL
; axis
<= FD_PITCH
; axis
++) {
254 pt1FilterInit(&dtermLowpass2
[axis
], pt1FilterGain(pidProfile
->dterm_lowpass2_hz
, dT
));
258 if (pidProfile
->dterm_lowpass_hz
== 0 || pidProfile
->dterm_lowpass_hz
> pidFrequencyNyquist
) {
259 dtermLowpassApplyFn
= nullFilterApply
;
261 switch (pidProfile
->dterm_filter_type
) {
263 dtermLowpassApplyFn
= nullFilterApply
;
266 dtermLowpassApplyFn
= (filterApplyFnPtr
)pt1FilterApply
;
267 for (int axis
= FD_ROLL
; axis
<= FD_PITCH
; axis
++) {
268 pt1FilterInit(&dtermLowpass
[axis
].pt1Filter
, pt1FilterGain(pidProfile
->dterm_lowpass_hz
, dT
));
272 dtermLowpassApplyFn
= (filterApplyFnPtr
)biquadFilterApply
;
273 for (int axis
= FD_ROLL
; axis
<= FD_PITCH
; axis
++) {
274 biquadFilterInitLPF(&dtermLowpass
[axis
].biquadFilter
, pidProfile
->dterm_lowpass_hz
, targetPidLooptime
);
280 if (pidProfile
->yaw_lowpass_hz
== 0 || pidProfile
->yaw_lowpass_hz
> pidFrequencyNyquist
) {
281 ptermYawLowpassApplyFn
= nullFilterApply
;
283 ptermYawLowpassApplyFn
= (filterApplyFnPtr
)pt1FilterApply
;
284 pt1FilterInit(&ptermYawLowpass
, pt1FilterGain(pidProfile
->yaw_lowpass_hz
, dT
));
287 pt1FilterInit(&throttleLpf
, pt1FilterGain(pidProfile
->throttle_boost_cutoff
, dT
));
288 #if defined(USE_ITERM_RELAX)
290 for (int i
= 0; i
< XYZ_AXIS_COUNT
; i
++) {
291 pt1FilterInit(&windupLpf
[i
][0], pt1FilterGain(itermRelaxCutoffLow
, dT
));
292 pt1FilterInit(&windupLpf
[i
][1], pt1FilterGain(itermRelaxCutoffHigh
, dT
));
298 typedef struct pidCoefficient_s
{
304 static FAST_RAM_ZERO_INIT pidCoefficient_t pidCoefficient
[3];
305 static FAST_RAM_ZERO_INIT
float maxVelocity
[3];
306 static FAST_RAM_ZERO_INIT
float relaxFactor
;
307 static FAST_RAM_ZERO_INIT
float dtermSetpointWeight
;
308 static FAST_RAM_ZERO_INIT
float levelGain
, horizonGain
, horizonTransition
, horizonCutoffDegrees
, horizonFactorRatio
;
309 static FAST_RAM_ZERO_INIT
float ITermWindupPointInv
;
310 static FAST_RAM_ZERO_INIT
uint8_t horizonTiltExpertMode
;
311 static FAST_RAM_ZERO_INIT timeDelta_t crashTimeLimitUs
;
312 static FAST_RAM_ZERO_INIT timeDelta_t crashTimeDelayUs
;
313 static FAST_RAM_ZERO_INIT
int32_t crashRecoveryAngleDeciDegrees
;
314 static FAST_RAM_ZERO_INIT
float crashRecoveryRate
;
315 static FAST_RAM_ZERO_INIT
float crashDtermThreshold
;
316 static FAST_RAM_ZERO_INIT
float crashGyroThreshold
;
317 static FAST_RAM_ZERO_INIT
float crashSetpointThreshold
;
318 static FAST_RAM_ZERO_INIT
float crashLimitYaw
;
319 static FAST_RAM_ZERO_INIT
float itermLimit
;
320 FAST_RAM_ZERO_INIT
float throttleBoost
;
321 pt1Filter_t throttleLpf
;
322 static FAST_RAM_ZERO_INIT
bool itermRotation
;
324 #if defined(USE_SMART_FEEDFORWARD)
325 static FAST_RAM_ZERO_INIT
bool smartFeedforward
;
327 #if defined(USE_ABSOLUTE_CONTROL)
328 static FAST_RAM_ZERO_INIT
float axisError
[XYZ_AXIS_COUNT
];
329 static FAST_RAM_ZERO_INIT
float acGain
;
330 static FAST_RAM_ZERO_INIT
float acLimit
;
331 static FAST_RAM_ZERO_INIT
float acErrorLimit
;
334 void pidResetITerm(void)
336 for (int axis
= 0; axis
< 3; axis
++) {
337 pidData
[axis
].I
= 0.0f
;
338 #if defined(USE_ABSOLUTE_CONTROL)
339 axisError
[axis
] = 0.0f
;
344 #ifdef USE_ACRO_TRAINER
345 static FAST_RAM_ZERO_INIT
float acroTrainerAngleLimit
;
346 static FAST_RAM_ZERO_INIT
float acroTrainerLookaheadTime
;
347 static FAST_RAM_ZERO_INIT
uint8_t acroTrainerDebugAxis
;
348 static FAST_RAM_ZERO_INIT
bool acroTrainerActive
;
349 static FAST_RAM_ZERO_INIT
int acroTrainerAxisState
[2]; // only need roll and pitch
350 static FAST_RAM_ZERO_INIT
float acroTrainerGain
;
351 #endif // USE_ACRO_TRAINER
353 void pidInitConfig(const pidProfile_t
*pidProfile
)
355 for (int axis
= FD_ROLL
; axis
<= FD_YAW
; axis
++) {
356 pidCoefficient
[axis
].Kp
= PTERM_SCALE
* pidProfile
->pid
[axis
].P
;
357 pidCoefficient
[axis
].Ki
= ITERM_SCALE
* pidProfile
->pid
[axis
].I
;
358 pidCoefficient
[axis
].Kd
= DTERM_SCALE
* pidProfile
->pid
[axis
].D
;
361 dtermSetpointWeight
= pidProfile
->dtermSetpointWeight
/ 127.0f
;
362 if (pidProfile
->setpointRelaxRatio
== 0) {
365 relaxFactor
= 100.0f
/ pidProfile
->setpointRelaxRatio
;
367 levelGain
= pidProfile
->pid
[PID_LEVEL
].P
/ 10.0f
;
368 horizonGain
= pidProfile
->pid
[PID_LEVEL
].I
/ 10.0f
;
369 horizonTransition
= (float)pidProfile
->pid
[PID_LEVEL
].D
;
370 horizonTiltExpertMode
= pidProfile
->horizon_tilt_expert_mode
;
371 horizonCutoffDegrees
= (175 - pidProfile
->horizon_tilt_effect
) * 1.8f
;
372 horizonFactorRatio
= (100 - pidProfile
->horizon_tilt_effect
) * 0.01f
;
373 maxVelocity
[FD_ROLL
] = maxVelocity
[FD_PITCH
] = pidProfile
->rateAccelLimit
* 100 * dT
;
374 maxVelocity
[FD_YAW
] = pidProfile
->yawRateAccelLimit
* 100 * dT
;
375 const float ITermWindupPoint
= (float)pidProfile
->itermWindupPointPercent
/ 100.0f
;
376 ITermWindupPointInv
= 1.0f
/ (1.0f
- ITermWindupPoint
);
377 crashTimeLimitUs
= pidProfile
->crash_time
* 1000;
378 crashTimeDelayUs
= pidProfile
->crash_delay
* 1000;
379 crashRecoveryAngleDeciDegrees
= pidProfile
->crash_recovery_angle
* 10;
380 crashRecoveryRate
= pidProfile
->crash_recovery_rate
;
381 crashGyroThreshold
= pidProfile
->crash_gthreshold
;
382 crashDtermThreshold
= pidProfile
->crash_dthreshold
;
383 crashSetpointThreshold
= pidProfile
->crash_setpoint_threshold
;
384 crashLimitYaw
= pidProfile
->crash_limit_yaw
;
385 itermLimit
= pidProfile
->itermLimit
;
386 throttleBoost
= pidProfile
->throttle_boost
* 0.1f
;
387 itermRotation
= pidProfile
->iterm_rotation
;
388 #if defined(USE_SMART_FEEDFORWARD)
389 smartFeedforward
= pidProfile
->smart_feedforward
;
391 #if defined(USE_ITERM_RELAX)
392 itermRelax
= pidProfile
->iterm_relax
;
393 itermRelaxCutoffLow
= pidProfile
->iterm_relax_cutoff_low
;
394 itermRelaxCutoffHigh
= pidProfile
->iterm_relax_cutoff_high
;
397 #ifdef USE_ACRO_TRAINER
398 acroTrainerAngleLimit
= pidProfile
->acro_trainer_angle_limit
;
399 acroTrainerLookaheadTime
= (float)pidProfile
->acro_trainer_lookahead_ms
/ 1000.0f
;
400 acroTrainerDebugAxis
= pidProfile
->acro_trainer_debug_axis
;
401 acroTrainerGain
= (float)pidProfile
->acro_trainer_gain
/ 10.0f
;
402 #endif // USE_ACRO_TRAINER
404 #if defined(USE_ABSOLUTE_CONTROL)
405 acGain
= (float)pidProfile
->abs_control_gain
;
406 acLimit
= (float)pidProfile
->abs_control_limit
;
407 acErrorLimit
= (float)pidProfile
->abs_control_error_limit
;
411 void pidInit(const pidProfile_t
*pidProfile
)
413 pidSetTargetLooptime(gyro
.targetLooptime
* pidConfig()->pid_process_denom
); // Initialize pid looptime
414 pidInitFilters(pidProfile
);
415 pidInitConfig(pidProfile
);
418 #ifdef USE_ACRO_TRAINER
419 void pidAcroTrainerInit(void)
421 acroTrainerAxisState
[FD_ROLL
] = 0;
422 acroTrainerAxisState
[FD_PITCH
] = 0;
424 #endif // USE_ACRO_TRAINER
426 void pidCopyProfile(uint8_t dstPidProfileIndex
, uint8_t srcPidProfileIndex
)
428 if ((dstPidProfileIndex
< MAX_PROFILE_COUNT
-1 && srcPidProfileIndex
< MAX_PROFILE_COUNT
-1)
429 && dstPidProfileIndex
!= srcPidProfileIndex
431 memcpy(pidProfilesMutable(dstPidProfileIndex
), pidProfilesMutable(srcPidProfileIndex
), sizeof(pidProfile_t
));
435 // calculates strength of horizon leveling; 0 = none, 1.0 = most leveling
436 static float calcHorizonLevelStrength(void)
438 // start with 1.0 at center stick, 0.0 at max stick deflection:
439 float horizonLevelStrength
= 1.0f
- MAX(getRcDeflectionAbs(FD_ROLL
), getRcDeflectionAbs(FD_PITCH
));
441 // 0 at level, 90 at vertical, 180 at inverted (degrees):
442 const float currentInclination
= MAX(ABS(attitude
.values
.roll
), ABS(attitude
.values
.pitch
)) / 10.0f
;
444 // horizonTiltExpertMode: 0 = leveling always active when sticks centered,
445 // 1 = leveling can be totally off when inverted
446 if (horizonTiltExpertMode
) {
447 if (horizonTransition
> 0 && horizonCutoffDegrees
> 0) {
448 // if d_level > 0 and horizonTiltEffect < 175
449 // horizonCutoffDegrees: 0 to 125 => 270 to 90 (represents where leveling goes to zero)
450 // inclinationLevelRatio (0.0 to 1.0) is smaller (less leveling)
451 // for larger inclinations; 0.0 at horizonCutoffDegrees value:
452 const float inclinationLevelRatio
= constrainf((horizonCutoffDegrees
-currentInclination
) / horizonCutoffDegrees
, 0, 1);
453 // apply configured horizon sensitivity:
454 // when stick is near center (horizonLevelStrength ~= 1.0)
455 // H_sensitivity value has little effect,
456 // when stick is deflected (horizonLevelStrength near 0.0)
457 // H_sensitivity value has more effect:
458 horizonLevelStrength
= (horizonLevelStrength
- 1) * 100 / horizonTransition
+ 1;
459 // apply inclination ratio, which may lower leveling
460 // to zero regardless of stick position:
461 horizonLevelStrength
*= inclinationLevelRatio
;
462 } else { // d_level=0 or horizon_tilt_effect>=175 means no leveling
463 horizonLevelStrength
= 0;
465 } else { // horizon_tilt_expert_mode = 0 (leveling always active when sticks centered)
467 if (horizonFactorRatio
< 1.01f
) { // if horizonTiltEffect > 0
468 // horizonFactorRatio: 1.0 to 0.0 (larger means more leveling)
469 // inclinationLevelRatio (0.0 to 1.0) is smaller (less leveling)
470 // for larger inclinations, goes to 1.0 at inclination==level:
471 const float inclinationLevelRatio
= (180-currentInclination
)/180 * (1.0f
-horizonFactorRatio
) + horizonFactorRatio
;
472 // apply ratio to configured horizon sensitivity:
473 sensitFact
= horizonTransition
* inclinationLevelRatio
;
474 } else { // horizonTiltEffect=0 for "old" functionality
475 sensitFact
= horizonTransition
;
478 if (sensitFact
<= 0) { // zero means no leveling
479 horizonLevelStrength
= 0;
481 // when stick is near center (horizonLevelStrength ~= 1.0)
482 // sensitFact value has little effect,
483 // when stick is deflected (horizonLevelStrength near 0.0)
484 // sensitFact value has more effect:
485 horizonLevelStrength
= ((horizonLevelStrength
- 1) * (100 / sensitFact
)) + 1;
488 return constrainf(horizonLevelStrength
, 0, 1);
491 static float pidLevel(int axis
, const pidProfile_t
*pidProfile
, const rollAndPitchTrims_t
*angleTrim
, float currentPidSetpoint
) {
492 // calculate error angle and limit the angle to the max inclination
493 // rcDeflection is in range [-1.0, 1.0]
494 float angle
= pidProfile
->levelAngleLimit
* getRcDeflection(axis
);
495 #ifdef USE_GPS_RESCUE
496 angle
+= gpsRescueAngle
[axis
] / 100; // ANGLE IS IN CENTIDEGREES
498 angle
= constrainf(angle
, -pidProfile
->levelAngleLimit
, pidProfile
->levelAngleLimit
);
499 const float errorAngle
= angle
- ((attitude
.raw
[axis
] - angleTrim
->raw
[axis
]) / 10.0f
);
500 if (FLIGHT_MODE(ANGLE_MODE
) || FLIGHT_MODE(GPS_RESCUE_MODE
)) {
501 // ANGLE mode - control is angle based
502 currentPidSetpoint
= errorAngle
* levelGain
;
504 // HORIZON mode - mix of ANGLE and ACRO modes
505 // mix in errorAngle to currentPidSetpoint to add a little auto-level feel
506 const float horizonLevelStrength
= calcHorizonLevelStrength();
507 currentPidSetpoint
= currentPidSetpoint
+ (errorAngle
* horizonGain
* horizonLevelStrength
);
509 return currentPidSetpoint
;
512 static float accelerationLimit(int axis
, float currentPidSetpoint
)
514 static float previousSetpoint
[3];
515 const float currentVelocity
= currentPidSetpoint
- previousSetpoint
[axis
];
517 if (ABS(currentVelocity
) > maxVelocity
[axis
]) {
518 currentPidSetpoint
= (currentVelocity
> 0) ? previousSetpoint
[axis
] + maxVelocity
[axis
] : previousSetpoint
[axis
] - maxVelocity
[axis
];
521 previousSetpoint
[axis
] = currentPidSetpoint
;
522 return currentPidSetpoint
;
525 static timeUs_t crashDetectedAtUs
;
527 static void handleCrashRecovery(
528 const pidCrashRecovery_e crash_recovery
, const rollAndPitchTrims_t
*angleTrim
,
529 const int axis
, const timeUs_t currentTimeUs
, const float gyroRate
, float *currentPidSetpoint
, float *errorRate
)
531 if (inCrashRecoveryMode
&& cmpTimeUs(currentTimeUs
, crashDetectedAtUs
) > crashTimeDelayUs
) {
532 if (crash_recovery
== PID_CRASH_RECOVERY_BEEP
) {
535 if (axis
== FD_YAW
) {
536 *errorRate
= constrainf(*errorRate
, -crashLimitYaw
, crashLimitYaw
);
538 // on roll and pitch axes calculate currentPidSetpoint and errorRate to level the aircraft to recover from crash
539 if (sensors(SENSOR_ACC
)) {
540 // errorAngle is deviation from horizontal
541 const float errorAngle
= -(attitude
.raw
[axis
] - angleTrim
->raw
[axis
]) / 10.0f
;
542 *currentPidSetpoint
= errorAngle
* levelGain
;
543 *errorRate
= *currentPidSetpoint
- gyroRate
;
546 // reset ITerm, since accumulated error before crash is now meaningless
547 // and ITerm windup during crash recovery can be extreme, especially on yaw axis
548 pidData
[axis
].I
= 0.0f
;
549 if (cmpTimeUs(currentTimeUs
, crashDetectedAtUs
) > crashTimeLimitUs
550 || (getMotorMixRange() < 1.0f
551 && ABS(gyro
.gyroADCf
[FD_ROLL
]) < crashRecoveryRate
552 && ABS(gyro
.gyroADCf
[FD_PITCH
]) < crashRecoveryRate
553 && ABS(gyro
.gyroADCf
[FD_YAW
]) < crashRecoveryRate
)) {
554 if (sensors(SENSOR_ACC
)) {
555 // check aircraft nearly level
556 if (ABS(attitude
.raw
[FD_ROLL
] - angleTrim
->raw
[FD_ROLL
]) < crashRecoveryAngleDeciDegrees
557 && ABS(attitude
.raw
[FD_PITCH
] - angleTrim
->raw
[FD_PITCH
]) < crashRecoveryAngleDeciDegrees
) {
558 inCrashRecoveryMode
= false;
562 inCrashRecoveryMode
= false;
569 static void detectAndSetCrashRecovery(
570 const pidCrashRecovery_e crash_recovery
, const int axis
,
571 const timeUs_t currentTimeUs
, const float delta
, const float errorRate
)
573 // if crash recovery is on and accelerometer enabled and there is no gyro overflow, then check for a crash
574 // no point in trying to recover if the crash is so severe that the gyro overflows
575 if ((crash_recovery
|| FLIGHT_MODE(GPS_RESCUE_MODE
)) && !gyroOverflowDetected()) {
576 if (ARMING_FLAG(ARMED
)) {
577 if (getMotorMixRange() >= 1.0f
&& !inCrashRecoveryMode
578 && ABS(delta
) > crashDtermThreshold
579 && ABS(errorRate
) > crashGyroThreshold
580 && ABS(getSetpointRate(axis
)) < crashSetpointThreshold
) {
581 inCrashRecoveryMode
= true;
582 crashDetectedAtUs
= currentTimeUs
;
584 if (inCrashRecoveryMode
&& cmpTimeUs(currentTimeUs
, crashDetectedAtUs
) < crashTimeDelayUs
&& (ABS(errorRate
) < crashGyroThreshold
585 || ABS(getSetpointRate(axis
)) > crashSetpointThreshold
)) {
586 inCrashRecoveryMode
= false;
589 } else if (inCrashRecoveryMode
) {
590 inCrashRecoveryMode
= false;
596 static void rotateVector(float v
[XYZ_AXIS_COUNT
], float rotation
[XYZ_AXIS_COUNT
])
598 // rotate v around rotation vector rotation
599 // rotation in radians, all elements must be small
600 for (int i
= 0; i
< XYZ_AXIS_COUNT
; i
++) {
601 int i_1
= (i
+ 1) % 3;
602 int i_2
= (i
+ 2) % 3;
603 float newV
= v
[i_1
] + v
[i_2
] * rotation
[i
];
604 v
[i_2
] -= v
[i_1
] * rotation
[i
];
609 static void rotateITermAndAxisError()
612 #if defined(USE_ABSOLUTE_CONTROL)
616 const float gyroToAngle
= dT
* RAD
;
617 float rotationRads
[3];
618 for (int i
= FD_ROLL
; i
<= FD_YAW
; i
++) {
619 rotationRads
[i
] = gyro
.gyroADCf
[i
] * gyroToAngle
;
621 #if defined(USE_ABSOLUTE_CONTROL)
623 rotateVector(axisError
, rotationRads
);
628 for (int i
= 0; i
< XYZ_AXIS_COUNT
; i
++) {
631 rotateVector(v
, rotationRads
);
632 for (int i
= 0; i
< XYZ_AXIS_COUNT
; i
++) {
639 #ifdef USE_ACRO_TRAINER
641 int acroTrainerSign(float x
)
643 return x
> 0 ? 1 : -1;
646 // Acro Trainer - Manipulate the setPoint to limit axis angle while in acro mode
647 // There are three states:
648 // 1. Current angle has exceeded limit
649 // Apply correction to return to limit (similar to pidLevel)
650 // 2. Future overflow has been projected based on current angle and gyro rate
651 // Manage the setPoint to control the gyro rate as the actual angle approaches the limit (try to prevent overshoot)
652 // 3. If no potential overflow is detected, then return the original setPoint
654 // Use the FAST_CODE_NOINLINE directive to avoid this code from being inlined into ITCM RAM. We accept the
655 // performance decrease when Acro Trainer mode is active under the assumption that user is unlikely to be
656 // expecting ultimate flight performance at very high loop rates when in this mode.
657 static FAST_CODE_NOINLINE
float applyAcroTrainer(int axis
, const rollAndPitchTrims_t
*angleTrim
, float setPoint
)
659 float ret
= setPoint
;
661 if (!FLIGHT_MODE(ANGLE_MODE
) && !FLIGHT_MODE(HORIZON_MODE
) && !FLIGHT_MODE(GPS_RESCUE_MODE
)) {
662 bool resetIterm
= false;
663 float projectedAngle
= 0;
664 const int setpointSign
= acroTrainerSign(setPoint
);
665 const float currentAngle
= (attitude
.raw
[axis
] - angleTrim
->raw
[axis
]) / 10.0f
;
666 const int angleSign
= acroTrainerSign(currentAngle
);
668 if ((acroTrainerAxisState
[axis
] != 0) && (acroTrainerAxisState
[axis
] != setpointSign
)) { // stick has reversed - stop limiting
669 acroTrainerAxisState
[axis
] = 0;
672 // Limit and correct the angle when it exceeds the limit
673 if ((fabsf(currentAngle
) > acroTrainerAngleLimit
) && (acroTrainerAxisState
[axis
] == 0)) {
674 if (angleSign
== setpointSign
) {
675 acroTrainerAxisState
[axis
] = angleSign
;
680 if (acroTrainerAxisState
[axis
] != 0) {
681 ret
= constrainf(((acroTrainerAngleLimit
* angleSign
) - currentAngle
) * acroTrainerGain
, -ACRO_TRAINER_SETPOINT_LIMIT
, ACRO_TRAINER_SETPOINT_LIMIT
);
684 // Not currently over the limit so project the angle based on current angle and
685 // gyro angular rate using a sliding window based on gyro rate (faster rotation means larger window.
686 // If the projected angle exceeds the limit then apply limiting to minimize overshoot.
687 // Calculate the lookahead window by scaling proportionally with gyro rate from 0-500dps
688 float checkInterval
= constrainf(fabsf(gyro
.gyroADCf
[axis
]) / ACRO_TRAINER_LOOKAHEAD_RATE_LIMIT
, 0.0f
, 1.0f
) * acroTrainerLookaheadTime
;
689 projectedAngle
= (gyro
.gyroADCf
[axis
] * checkInterval
) + currentAngle
;
690 const int projectedAngleSign
= acroTrainerSign(projectedAngle
);
691 if ((fabsf(projectedAngle
) > acroTrainerAngleLimit
) && (projectedAngleSign
== setpointSign
)) {
692 ret
= ((acroTrainerAngleLimit
* projectedAngleSign
) - projectedAngle
) * acroTrainerGain
;
701 if (axis
== acroTrainerDebugAxis
) {
702 DEBUG_SET(DEBUG_ACRO_TRAINER
, 0, lrintf(currentAngle
* 10.0f
));
703 DEBUG_SET(DEBUG_ACRO_TRAINER
, 1, acroTrainerAxisState
[axis
]);
704 DEBUG_SET(DEBUG_ACRO_TRAINER
, 2, lrintf(ret
));
705 DEBUG_SET(DEBUG_ACRO_TRAINER
, 3, lrintf(projectedAngle
* 10.0f
));
711 #endif // USE_ACRO_TRAINER
713 // Betaflight pid controller, which will be maintained in the future with additional features specialised for current (mini) multirotor usage.
714 // Based on 2DOF reference design (matlab)
715 void FAST_CODE
pidController(const pidProfile_t
*pidProfile
, const rollAndPitchTrims_t
*angleTrim
, timeUs_t currentTimeUs
)
717 static float previousGyroRateDterm
[2];
718 static float previousPidSetpoint
[2];
720 const float tpaFactor
= getThrottlePIDAttenuation();
721 const float motorMixRange
= getMotorMixRange();
723 #ifdef USE_YAW_SPIN_RECOVERY
724 const bool yawSpinActive
= gyroYawSpinDetected();
727 // Dynamic i component,
728 // gradually scale back integration when above windup point
729 const float dynCi
= MIN((1.0f
- motorMixRange
) * ITermWindupPointInv
, 1.0f
) * dT
* itermAccelerator
;
731 // Dynamic d component, enable 2-DOF PID controller only for rate mode
732 const float dynCd
= flightModeFlags
? 0.0f
: dtermSetpointWeight
;
734 // Precalculate gyro deta for D-term here, this allows loop unrolling
735 float gyroRateDterm
[2];
736 for (int axis
= FD_ROLL
; axis
< FD_YAW
; ++axis
) {
737 gyroRateDterm
[axis
] = dtermNotchApplyFn((filter_t
*) &dtermNotch
[axis
], gyro
.gyroADCf
[axis
]);
738 gyroRateDterm
[axis
] = dtermLowpassApplyFn((filter_t
*) &dtermLowpass
[axis
], gyroRateDterm
[axis
]);
739 gyroRateDterm
[axis
] = dtermLowpass2ApplyFn((filter_t
*) &dtermLowpass2
[axis
], gyroRateDterm
[axis
]);
742 rotateITermAndAxisError();
744 // ----------PID controller----------
745 for (int axis
= FD_ROLL
; axis
<= FD_YAW
; ++axis
) {
746 float currentPidSetpoint
= getSetpointRate(axis
);
747 if (maxVelocity
[axis
]) {
748 currentPidSetpoint
= accelerationLimit(axis
, currentPidSetpoint
);
750 // Yaw control is GYRO based, direct sticks control is applied to rate PID
751 if ((FLIGHT_MODE(ANGLE_MODE
) || FLIGHT_MODE(HORIZON_MODE
) || FLIGHT_MODE(GPS_RESCUE_MODE
)) && axis
!= YAW
) {
752 currentPidSetpoint
= pidLevel(axis
, pidProfile
, angleTrim
, currentPidSetpoint
);
755 #ifdef USE_ACRO_TRAINER
756 if ((axis
!= FD_YAW
) && acroTrainerActive
&& !inCrashRecoveryMode
) {
757 currentPidSetpoint
= applyAcroTrainer(axis
, angleTrim
, currentPidSetpoint
);
759 #endif // USE_ACRO_TRAINER
761 // Handle yaw spin recovery - zero the setpoint on yaw to aid in recovery
762 // It's not necessary to zero the set points for R/P because the PIDs will be zeroed below
763 #ifdef USE_YAW_SPIN_RECOVERY
764 if ((axis
== FD_YAW
) && yawSpinActive
) {
765 currentPidSetpoint
= 0.0f
;
767 #endif // USE_YAW_SPIN_RECOVERY
769 #if defined(USE_ABSOLUTE_CONTROL)
770 // mix in a correction for accrued attitude error
771 float acCorrection
= 0;
773 acCorrection
= constrainf(axisError
[axis
] * acGain
, -acLimit
, acLimit
);
774 currentPidSetpoint
+= acCorrection
;
778 // -----calculate error rate
779 const float gyroRate
= gyro
.gyroADCf
[axis
]; // Process variable from gyro output in deg/sec
780 float errorRate
= currentPidSetpoint
- gyroRate
; // r - y
783 pidProfile
->crash_recovery
, angleTrim
, axis
, currentTimeUs
, gyroRate
,
784 ¤tPidSetpoint
, &errorRate
);
786 // --------low-level gyro-based PID based on 2DOF PID controller. ----------
787 // 2-DOF PID controller with optional filter on derivative term.
788 // b = 1 and only c (dtermSetpointWeight) can be tuned (amount derivative on measurement or error).
790 // -----calculate P component and add Dynamic Part based on stick input
791 pidData
[axis
].P
= pidCoefficient
[axis
].Kp
* errorRate
* tpaFactor
;
792 if (axis
== FD_YAW
) {
793 pidData
[axis
].P
= ptermYawLowpassApplyFn((filter_t
*) &ptermYawLowpass
, pidData
[axis
].P
);
796 // -----calculate I component
797 float itermErrorRate
= 0.0f
;
798 #if defined(USE_ABSOLUTE_CONTROL)
801 #if defined(USE_ITERM_RELAX)
802 if (itermRelax
&& (axis
< FD_YAW
|| itermRelax
== ITERM_RELAX_RPY
)) {
803 float gyroTargetLow
= pt1FilterApply(&windupLpf
[axis
][0], currentPidSetpoint
);
804 float gyroTargetHigh
= pt1FilterApply(&windupLpf
[axis
][1], currentPidSetpoint
);
805 #if defined(USE_ABSOLUTE_CONTROL)
806 gyroTargetLow
+= acCorrection
;
807 gyroTargetHigh
+= acCorrection
;
810 int itemOffset
= (axis
<< 1);
811 DEBUG_SET(DEBUG_ITERM_RELAX
, itemOffset
++, gyroTargetHigh
);
812 DEBUG_SET(DEBUG_ITERM_RELAX
, itemOffset
, gyroTargetLow
);
814 const float gmax
= MAX(gyroTargetHigh
, gyroTargetLow
);
815 const float gmin
= MIN(gyroTargetHigh
, gyroTargetLow
);
816 if (gyroRate
< gmin
|| gyroRate
> gmax
) {
817 itermErrorRate
= (gyroRate
> gmax
? gmax
: gmin
) - gyroRate
;
819 #if defined(USE_ABSOLUTE_CONTROL)
821 itermErrorRate
= acCorrection
;
823 acErrorRate
= itermErrorRate
- acCorrection
;
826 #endif // USE_ITERM_RELAX
828 itermErrorRate
= errorRate
;
829 #if defined(USE_ABSOLUTE_CONTROL)
830 acErrorRate
= errorRate
;
834 #if defined(USE_ABSOLUTE_CONTROL)
836 axisError
[axis
] = constrainf(axisError
[axis
] + acErrorRate
* dT
, -acErrorLimit
, acErrorLimit
);
840 const float ITerm
= pidData
[axis
].I
;
841 const float ITermNew
= constrainf(ITerm
+ pidCoefficient
[axis
].Ki
* itermErrorRate
* dynCi
, -itermLimit
, itermLimit
);
842 const bool outputSaturated
= mixerIsOutputSaturated(axis
, errorRate
);
843 if (outputSaturated
== false || ABS(ITermNew
) < ABS(ITerm
)) {
844 // Only increase ITerm if output is not saturated
845 pidData
[axis
].I
= ITermNew
;
848 // -----calculate D component
849 if (axis
!= FD_YAW
) {
850 // no transition if relaxFactor == 0
851 float transition
= relaxFactor
> 0 ? MIN(1.f
, getRcDeflectionAbs(axis
) * relaxFactor
) : 1;
853 // Divide rate change by dT to get differential (ie dr/dt).
854 // dT is fixed and calculated from the target PID loop time
855 // This is done to avoid DTerm spikes that occur with dynamically
856 // calculated deltaT whenever another task causes the PID
857 // loop execution to be delayed.
859 - (gyroRateDterm
[axis
] - previousGyroRateDterm
[axis
]) / dT
;
861 detectAndSetCrashRecovery(pidProfile
->crash_recovery
, axis
, currentTimeUs
, delta
, errorRate
);
863 pidData
[axis
].D
= pidCoefficient
[axis
].Kd
* delta
* tpaFactor
;
865 const float pidFeedForward
=
866 pidCoefficient
[axis
].Kd
* dynCd
* transition
*
867 (currentPidSetpoint
- previousPidSetpoint
[axis
]) * tpaFactor
/ dT
;
868 #if defined(USE_SMART_FEEDFORWARD)
869 bool addFeedforward
= true;
870 if (smartFeedforward
) {
871 if (pidData
[axis
].P
* pidFeedForward
> 0) {
872 if (ABS(pidFeedForward
) > ABS(pidData
[axis
].P
)) {
876 addFeedforward
= false;
883 pidData
[axis
].D
+= pidFeedForward
;
885 previousGyroRateDterm
[axis
] = gyroRateDterm
[axis
];
886 previousPidSetpoint
[axis
] = currentPidSetpoint
;
888 #ifdef USE_YAW_SPIN_RECOVERY
890 // zero PIDs on pitch and roll leaving yaw P to correct spin
895 #endif // USE_YAW_SPIN_RECOVERY
899 // calculating the PID sum
900 pidData
[FD_ROLL
].Sum
= pidData
[FD_ROLL
].P
+ pidData
[FD_ROLL
].I
+ pidData
[FD_ROLL
].D
;
901 pidData
[FD_PITCH
].Sum
= pidData
[FD_PITCH
].P
+ pidData
[FD_PITCH
].I
+ pidData
[FD_PITCH
].D
;
903 #ifdef USE_YAW_SPIN_RECOVERY
905 // yaw P alone to correct spin
906 pidData
[FD_YAW
].I
= 0;
908 #endif // USE_YAW_SPIN_RECOVERY
911 pidData
[FD_YAW
].Sum
= pidData
[FD_YAW
].P
+ pidData
[FD_YAW
].I
;
913 // Disable PID control if at zero throttle or if gyro overflow detected
914 // This may look very innefficient, but it is done on purpose to always show real CPU usage as in flight
915 if (!pidStabilisationEnabled
|| gyroOverflowDetected()) {
916 for (int axis
= FD_ROLL
; axis
<= FD_YAW
; ++axis
) {
921 pidData
[axis
].Sum
= 0;
926 bool crashRecoveryModeActive(void)
928 return inCrashRecoveryMode
;
931 #ifdef USE_ACRO_TRAINER
932 void pidSetAcroTrainerState(bool newState
)
934 if (acroTrainerActive
!= newState
) {
936 pidAcroTrainerInit();
938 acroTrainerActive
= newState
;
941 #endif // USE_ACRO_TRAINER