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"
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 const char pidNames
[] =
66 FAST_RAM_ZERO_INIT
uint32_t targetPidLooptime
;
67 FAST_RAM_ZERO_INIT pidAxisData_t pidData
[XYZ_AXIS_COUNT
];
69 static FAST_RAM_ZERO_INIT
bool pidStabilisationEnabled
;
71 static FAST_RAM_ZERO_INIT
bool inCrashRecoveryMode
= false;
73 static FAST_RAM_ZERO_INIT
float dT
;
74 static FAST_RAM_ZERO_INIT
float pidFrequency
;
76 static FAST_RAM_ZERO_INIT
uint8_t antiGravityMode
;
77 static FAST_RAM_ZERO_INIT
float antiGravityThrottleHpf
;
78 static FAST_RAM_ZERO_INIT
uint16_t itermAcceleratorGain
;
79 static FAST_RAM
float antiGravityOsdCutoff
= 1.0f
;
80 static FAST_RAM_ZERO_INIT
bool antiGravityEnabled
;
82 PG_REGISTER_WITH_RESET_TEMPLATE(pidConfig_t
, pidConfig
, PG_PID_CONFIG
, 2);
85 #define PID_PROCESS_DENOM_DEFAULT 1
86 #elif defined(USE_GYRO_SPI_MPU6000) || defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_ICM20689)
87 #define PID_PROCESS_DENOM_DEFAULT 4
89 #define PID_PROCESS_DENOM_DEFAULT 2
92 #ifdef USE_RUNAWAY_TAKEOFF
93 PG_RESET_TEMPLATE(pidConfig_t
, pidConfig
,
94 .pid_process_denom
= PID_PROCESS_DENOM_DEFAULT
,
95 .runaway_takeoff_prevention
= true,
96 .runaway_takeoff_deactivate_throttle
= 25, // throttle level % needed to accumulate deactivation time
97 .runaway_takeoff_deactivate_delay
= 500 // Accumulated time (in milliseconds) before deactivation in successful takeoff
100 PG_RESET_TEMPLATE(pidConfig_t
, pidConfig
,
101 .pid_process_denom
= PID_PROCESS_DENOM_DEFAULT
105 #ifdef USE_ACRO_TRAINER
106 #define ACRO_TRAINER_LOOKAHEAD_RATE_LIMIT 500.0f // Max gyro rate for lookahead time scaling
107 #define ACRO_TRAINER_SETPOINT_LIMIT 1000.0f // Limit the correcting setpoint
108 #endif // USE_ACRO_TRAINER
110 #define ANTI_GRAVITY_THROTTLE_FILTER_CUTOFF 15 // The anti gravity throttle highpass filter cutoff
112 PG_REGISTER_ARRAY_WITH_RESET_FN(pidProfile_t
, MAX_PROFILE_COUNT
, pidProfiles
, PG_PID_PROFILE
, 5);
114 void resetPidProfile(pidProfile_t
*pidProfile
)
116 RESET_CONFIG(pidProfile_t
, pidProfile
,
118 [PID_ROLL
] = { 46, 65, 25, 60 },
119 [PID_PITCH
] = { 50, 75, 27, 60 },
120 [PID_YAW
] = { 45, 100, 0, 100 },
121 [PID_LEVEL
] = { 50, 50, 75, 0 },
122 [PID_MAG
] = { 40, 0, 0, 0 },
124 .pidSumLimit
= PIDSUM_LIMIT
,
125 .pidSumLimitYaw
= PIDSUM_LIMIT_YAW
,
128 .dterm_notch_cutoff
= 0,
129 .itermWindupPointPercent
= 100,
130 .vbatPidCompensation
= 0,
131 .pidAtMinThrottle
= PID_STABILISATION_ON
,
132 .levelAngleLimit
= 55,
133 .feedForwardTransition
= 0,
134 .yawRateAccelLimit
= 0,
136 .itermThrottleThreshold
= 250,
137 .itermAcceleratorGain
= 5000,
138 .crash_time
= 500, // ms
139 .crash_delay
= 0, // ms
140 .crash_recovery_angle
= 10, // degrees
141 .crash_recovery_rate
= 100, // degrees/second
142 .crash_dthreshold
= 50, // degrees/second/second
143 .crash_gthreshold
= 400, // degrees/second
144 .crash_setpoint_threshold
= 350, // degrees/second
145 .crash_recovery
= PID_CRASH_RECOVERY_OFF
, // off by default
146 .horizon_tilt_effect
= 75,
147 .horizon_tilt_expert_mode
= false,
148 .crash_limit_yaw
= 200,
151 .throttle_boost_cutoff
= 15,
152 .iterm_rotation
= true,
153 .smart_feedforward
= false,
154 .iterm_relax
= ITERM_RELAX_RP
,
155 .iterm_relax_cutoff
= 20,
156 .iterm_relax_type
= ITERM_RELAX_SETPOINT
,
157 .acro_trainer_angle_limit
= 20,
158 .acro_trainer_lookahead_ms
= 50,
159 .acro_trainer_debug_axis
= FD_ROLL
,
160 .acro_trainer_gain
= 75,
161 .abs_control_gain
= 0,
162 .abs_control_limit
= 90,
163 .abs_control_error_limit
= 20,
164 .antiGravityMode
= ANTI_GRAVITY_SMOOTH
,
165 .dyn_lpf_dterm_max_hz
= 200,
166 .dyn_lpf_dterm_idle
= 20,
167 .dterm_lowpass_hz
= 100, // dual PT1 filtering ON by default
168 .dterm_lowpass2_hz
= 200, // second Dterm LPF ON by default
169 .dterm_filter_type
= FILTER_PT1
,
170 .dterm_filter2_type
= FILTER_PT1
,
173 pidProfile
->dterm_lowpass_hz
= 120;
174 pidProfile
->dterm_lowpass2_hz
= 180;
175 pidProfile
->dterm_filter_type
= FILTER_BIQUAD
;
176 pidProfile
->dterm_filter2_type
= FILTER_BIQUAD
;
180 void pgResetFn_pidProfiles(pidProfile_t
*pidProfiles
)
182 for (int i
= 0; i
< MAX_PROFILE_COUNT
; i
++) {
183 resetPidProfile(&pidProfiles
[i
]);
187 static void pidSetTargetLooptime(uint32_t pidLooptime
)
189 targetPidLooptime
= pidLooptime
;
190 dT
= targetPidLooptime
* 1e-6f
;
191 pidFrequency
= 1.0f
/ dT
;
194 static FAST_RAM
float itermAccelerator
= 1.0f
;
196 void pidSetItermAccelerator(float newItermAccelerator
)
198 itermAccelerator
= newItermAccelerator
;
201 bool pidOsdAntiGravityActive(void)
203 return (itermAccelerator
> antiGravityOsdCutoff
);
206 void pidStabilisationState(pidStabilisationState_e pidControllerState
)
208 pidStabilisationEnabled
= (pidControllerState
== PID_STABILISATION_ON
) ? true : false;
211 const angle_index_t rcAliasToAngleIndexMap
[] = { AI_ROLL
, AI_PITCH
};
213 typedef union dtermLowpass_u
{
214 pt1Filter_t pt1Filter
;
215 biquadFilter_t biquadFilter
;
218 static FAST_RAM_ZERO_INIT filterApplyFnPtr dtermNotchApplyFn
;
219 static FAST_RAM_ZERO_INIT biquadFilter_t dtermNotch
[XYZ_AXIS_COUNT
];
220 static FAST_RAM_ZERO_INIT filterApplyFnPtr dtermLowpassApplyFn
;
221 static FAST_RAM_ZERO_INIT dtermLowpass_t dtermLowpass
[XYZ_AXIS_COUNT
];
222 static FAST_RAM_ZERO_INIT filterApplyFnPtr dtermLowpass2ApplyFn
;
223 static FAST_RAM_ZERO_INIT dtermLowpass_t dtermLowpass2
[XYZ_AXIS_COUNT
];
224 static FAST_RAM_ZERO_INIT filterApplyFnPtr ptermYawLowpassApplyFn
;
225 static FAST_RAM_ZERO_INIT pt1Filter_t ptermYawLowpass
;
226 #if defined(USE_ITERM_RELAX)
227 static FAST_RAM_ZERO_INIT pt1Filter_t windupLpf
[XYZ_AXIS_COUNT
];
228 static FAST_RAM_ZERO_INIT
uint8_t itermRelax
;
229 static FAST_RAM_ZERO_INIT
uint8_t itermRelaxType
;
230 static FAST_RAM_ZERO_INIT
uint8_t itermRelaxCutoff
;
233 #ifdef USE_RC_SMOOTHING_FILTER
234 static FAST_RAM_ZERO_INIT pt1Filter_t setpointDerivativePt1
[XYZ_AXIS_COUNT
];
235 static FAST_RAM_ZERO_INIT biquadFilter_t setpointDerivativeBiquad
[XYZ_AXIS_COUNT
];
236 static FAST_RAM_ZERO_INIT
bool setpointDerivativeLpfInitialized
;
237 static FAST_RAM_ZERO_INIT
uint8_t rcSmoothingDebugAxis
;
238 static FAST_RAM_ZERO_INIT
uint8_t rcSmoothingFilterType
;
239 #endif // USE_RC_SMOOTHING_FILTER
241 static FAST_RAM_ZERO_INIT pt1Filter_t antiGravityThrottleLpf
;
243 void pidInitFilters(const pidProfile_t
*pidProfile
)
245 STATIC_ASSERT(FD_YAW
== 2, FD_YAW_incorrect
); // ensure yaw axis is 2
247 if (targetPidLooptime
== 0) {
248 // no looptime set, so set all the filters to null
249 dtermNotchApplyFn
= nullFilterApply
;
250 dtermLowpassApplyFn
= nullFilterApply
;
251 ptermYawLowpassApplyFn
= nullFilterApply
;
255 const uint32_t pidFrequencyNyquist
= pidFrequency
/ 2; // No rounding needed
257 uint16_t dTermNotchHz
;
258 if (pidProfile
->dterm_notch_hz
<= pidFrequencyNyquist
) {
259 dTermNotchHz
= pidProfile
->dterm_notch_hz
;
261 if (pidProfile
->dterm_notch_cutoff
< pidFrequencyNyquist
) {
262 dTermNotchHz
= pidFrequencyNyquist
;
268 if (dTermNotchHz
!= 0 && pidProfile
->dterm_notch_cutoff
!= 0) {
269 dtermNotchApplyFn
= (filterApplyFnPtr
)biquadFilterApply
;
270 const float notchQ
= filterGetNotchQ(dTermNotchHz
, pidProfile
->dterm_notch_cutoff
);
271 for (int axis
= FD_ROLL
; axis
<= FD_YAW
; axis
++) {
272 biquadFilterInit(&dtermNotch
[axis
], dTermNotchHz
, targetPidLooptime
, notchQ
, FILTER_NOTCH
);
275 dtermNotchApplyFn
= nullFilterApply
;
278 //1st Dterm Lowpass Filter
279 if (pidProfile
->dterm_lowpass_hz
== 0 || pidProfile
->dterm_lowpass_hz
> pidFrequencyNyquist
) {
280 dtermLowpassApplyFn
= nullFilterApply
;
282 switch (pidProfile
->dterm_filter_type
) {
284 dtermLowpassApplyFn
= (filterApplyFnPtr
)pt1FilterApply
;
285 for (int axis
= FD_ROLL
; axis
<= FD_YAW
; axis
++) {
286 pt1FilterInit(&dtermLowpass
[axis
].pt1Filter
, pt1FilterGain(pidProfile
->dterm_lowpass_hz
, dT
));
291 dtermLowpassApplyFn
= (filterApplyFnPtr
)biquadFilterApplyDF1
;
293 dtermLowpassApplyFn
= (filterApplyFnPtr
)biquadFilterApply
;
295 for (int axis
= FD_ROLL
; axis
<= FD_YAW
; axis
++) {
296 biquadFilterInitLPF(&dtermLowpass
[axis
].biquadFilter
, pidProfile
->dterm_lowpass_hz
, targetPidLooptime
);
300 dtermLowpassApplyFn
= nullFilterApply
;
305 //2nd Dterm Lowpass Filter
306 if (pidProfile
->dterm_lowpass2_hz
== 0 || pidProfile
->dterm_lowpass2_hz
> pidFrequencyNyquist
) {
307 dtermLowpass2ApplyFn
= nullFilterApply
;
309 switch (pidProfile
->dterm_filter2_type
) {
311 dtermLowpass2ApplyFn
= (filterApplyFnPtr
)pt1FilterApply
;
312 for (int axis
= FD_ROLL
; axis
<= FD_YAW
; axis
++) {
313 pt1FilterInit(&dtermLowpass2
[axis
].pt1Filter
, pt1FilterGain(pidProfile
->dterm_lowpass2_hz
, dT
));
317 dtermLowpass2ApplyFn
= (filterApplyFnPtr
)biquadFilterApply
;
318 for (int axis
= FD_ROLL
; axis
<= FD_YAW
; axis
++) {
319 biquadFilterInitLPF(&dtermLowpass2
[axis
].biquadFilter
, pidProfile
->dterm_lowpass2_hz
, targetPidLooptime
);
323 dtermLowpass2ApplyFn
= nullFilterApply
;
328 if (pidProfile
->yaw_lowpass_hz
== 0 || pidProfile
->yaw_lowpass_hz
> pidFrequencyNyquist
) {
329 ptermYawLowpassApplyFn
= nullFilterApply
;
331 ptermYawLowpassApplyFn
= (filterApplyFnPtr
)pt1FilterApply
;
332 pt1FilterInit(&ptermYawLowpass
, pt1FilterGain(pidProfile
->yaw_lowpass_hz
, dT
));
335 #if defined(USE_THROTTLE_BOOST)
336 pt1FilterInit(&throttleLpf
, pt1FilterGain(pidProfile
->throttle_boost_cutoff
, dT
));
338 #if defined(USE_ITERM_RELAX)
340 for (int i
= 0; i
< XYZ_AXIS_COUNT
; i
++) {
341 pt1FilterInit(&windupLpf
[i
], pt1FilterGain(itermRelaxCutoff
, dT
));
346 pt1FilterInit(&antiGravityThrottleLpf
, pt1FilterGain(ANTI_GRAVITY_THROTTLE_FILTER_CUTOFF
, dT
));
349 #ifdef USE_RC_SMOOTHING_FILTER
350 void pidInitSetpointDerivativeLpf(uint16_t filterCutoff
, uint8_t debugAxis
, uint8_t filterType
)
352 rcSmoothingDebugAxis
= debugAxis
;
353 rcSmoothingFilterType
= filterType
;
354 if ((filterCutoff
> 0) && (rcSmoothingFilterType
!= RC_SMOOTHING_DERIVATIVE_OFF
)) {
355 setpointDerivativeLpfInitialized
= true;
356 for (int axis
= FD_ROLL
; axis
<= FD_YAW
; axis
++) {
357 switch (rcSmoothingFilterType
) {
358 case RC_SMOOTHING_DERIVATIVE_PT1
:
359 pt1FilterInit(&setpointDerivativePt1
[axis
], pt1FilterGain(filterCutoff
, dT
));
361 case RC_SMOOTHING_DERIVATIVE_BIQUAD
:
362 biquadFilterInitLPF(&setpointDerivativeBiquad
[axis
], filterCutoff
, targetPidLooptime
);
369 void pidUpdateSetpointDerivativeLpf(uint16_t filterCutoff
)
371 if ((filterCutoff
> 0) && (rcSmoothingFilterType
!= RC_SMOOTHING_DERIVATIVE_OFF
)) {
372 for (int axis
= FD_ROLL
; axis
<= FD_YAW
; axis
++) {
373 switch (rcSmoothingFilterType
) {
374 case RC_SMOOTHING_DERIVATIVE_PT1
:
375 pt1FilterUpdateCutoff(&setpointDerivativePt1
[axis
], pt1FilterGain(filterCutoff
, dT
));
377 case RC_SMOOTHING_DERIVATIVE_BIQUAD
:
378 biquadFilterUpdateLPF(&setpointDerivativeBiquad
[axis
], filterCutoff
, targetPidLooptime
);
384 #endif // USE_RC_SMOOTHING_FILTER
386 typedef struct pidCoefficient_s
{
393 static FAST_RAM_ZERO_INIT pidCoefficient_t pidCoefficient
[XYZ_AXIS_COUNT
];
394 static FAST_RAM_ZERO_INIT
float maxVelocity
[XYZ_AXIS_COUNT
];
395 static FAST_RAM_ZERO_INIT
float feedForwardTransition
;
396 static FAST_RAM_ZERO_INIT
float levelGain
, horizonGain
, horizonTransition
, horizonCutoffDegrees
, horizonFactorRatio
;
397 static FAST_RAM_ZERO_INIT
float itermWindupPointInv
;
398 static FAST_RAM_ZERO_INIT
uint8_t horizonTiltExpertMode
;
399 static FAST_RAM_ZERO_INIT timeDelta_t crashTimeLimitUs
;
400 static FAST_RAM_ZERO_INIT timeDelta_t crashTimeDelayUs
;
401 static FAST_RAM_ZERO_INIT
int32_t crashRecoveryAngleDeciDegrees
;
402 static FAST_RAM_ZERO_INIT
float crashRecoveryRate
;
403 static FAST_RAM_ZERO_INIT
float crashDtermThreshold
;
404 static FAST_RAM_ZERO_INIT
float crashGyroThreshold
;
405 static FAST_RAM_ZERO_INIT
float crashSetpointThreshold
;
406 static FAST_RAM_ZERO_INIT
float crashLimitYaw
;
407 static FAST_RAM_ZERO_INIT
float itermLimit
;
408 #if defined(USE_THROTTLE_BOOST)
409 FAST_RAM_ZERO_INIT
float throttleBoost
;
410 pt1Filter_t throttleLpf
;
412 static FAST_RAM_ZERO_INIT
bool itermRotation
;
414 #if defined(USE_SMART_FEEDFORWARD)
415 static FAST_RAM_ZERO_INIT
bool smartFeedforward
;
417 #if defined(USE_ABSOLUTE_CONTROL)
418 STATIC_UNIT_TESTED FAST_RAM_ZERO_INIT
float axisError
[XYZ_AXIS_COUNT
];
419 static FAST_RAM_ZERO_INIT
float acGain
;
420 static FAST_RAM_ZERO_INIT
float acLimit
;
421 static FAST_RAM_ZERO_INIT
float acErrorLimit
;
424 void pidResetIterm(void)
426 for (int axis
= 0; axis
< 3; axis
++) {
427 pidData
[axis
].I
= 0.0f
;
428 #if defined(USE_ABSOLUTE_CONTROL)
429 axisError
[axis
] = 0.0f
;
434 #ifdef USE_ACRO_TRAINER
435 static FAST_RAM_ZERO_INIT
float acroTrainerAngleLimit
;
436 static FAST_RAM_ZERO_INIT
float acroTrainerLookaheadTime
;
437 static FAST_RAM_ZERO_INIT
uint8_t acroTrainerDebugAxis
;
438 static FAST_RAM_ZERO_INIT
bool acroTrainerActive
;
439 static FAST_RAM_ZERO_INIT
int acroTrainerAxisState
[2]; // only need roll and pitch
440 static FAST_RAM_ZERO_INIT
float acroTrainerGain
;
441 #endif // USE_ACRO_TRAINER
443 void pidUpdateAntiGravityThrottleFilter(float throttle
)
445 if (antiGravityMode
== ANTI_GRAVITY_SMOOTH
) {
446 antiGravityThrottleHpf
= throttle
- pt1FilterApply(&antiGravityThrottleLpf
, throttle
);
451 static FAST_RAM
int8_t dynLpfFilter
= DYN_LPF_NONE
;
452 static FAST_RAM_ZERO_INIT
float dynLpfIdle
;
453 static FAST_RAM_ZERO_INIT
float dynLpfIdlePoint
;
454 static FAST_RAM_ZERO_INIT
float dynLpfInvIdlePointScaled
;
455 static FAST_RAM_ZERO_INIT
uint16_t dynLpfMin
;
458 void pidInitConfig(const pidProfile_t
*pidProfile
)
460 if (pidProfile
->feedForwardTransition
== 0) {
461 feedForwardTransition
= 0;
463 feedForwardTransition
= 100.0f
/ pidProfile
->feedForwardTransition
;
465 for (int axis
= FD_ROLL
; axis
<= FD_YAW
; axis
++) {
466 pidCoefficient
[axis
].Kp
= PTERM_SCALE
* pidProfile
->pid
[axis
].P
;
467 pidCoefficient
[axis
].Ki
= ITERM_SCALE
* pidProfile
->pid
[axis
].I
;
468 pidCoefficient
[axis
].Kd
= DTERM_SCALE
* pidProfile
->pid
[axis
].D
;
469 pidCoefficient
[axis
].Kf
= FEEDFORWARD_SCALE
* (pidProfile
->pid
[axis
].F
/ 100.0f
);
472 levelGain
= pidProfile
->pid
[PID_LEVEL
].P
/ 10.0f
;
473 horizonGain
= pidProfile
->pid
[PID_LEVEL
].I
/ 10.0f
;
474 horizonTransition
= (float)pidProfile
->pid
[PID_LEVEL
].D
;
475 horizonTiltExpertMode
= pidProfile
->horizon_tilt_expert_mode
;
476 horizonCutoffDegrees
= (175 - pidProfile
->horizon_tilt_effect
) * 1.8f
;
477 horizonFactorRatio
= (100 - pidProfile
->horizon_tilt_effect
) * 0.01f
;
478 maxVelocity
[FD_ROLL
] = maxVelocity
[FD_PITCH
] = pidProfile
->rateAccelLimit
* 100 * dT
;
479 maxVelocity
[FD_YAW
] = pidProfile
->yawRateAccelLimit
* 100 * dT
;
480 itermWindupPointInv
= 1.0f
;
481 if (pidProfile
->itermWindupPointPercent
< 100) {
482 const float itermWindupPoint
= pidProfile
->itermWindupPointPercent
/ 100.0f
;
483 itermWindupPointInv
= 1.0f
/ (1.0f
- itermWindupPoint
);
485 itermAcceleratorGain
= pidProfile
->itermAcceleratorGain
;
486 crashTimeLimitUs
= pidProfile
->crash_time
* 1000;
487 crashTimeDelayUs
= pidProfile
->crash_delay
* 1000;
488 crashRecoveryAngleDeciDegrees
= pidProfile
->crash_recovery_angle
* 10;
489 crashRecoveryRate
= pidProfile
->crash_recovery_rate
;
490 crashGyroThreshold
= pidProfile
->crash_gthreshold
;
491 crashDtermThreshold
= pidProfile
->crash_dthreshold
;
492 crashSetpointThreshold
= pidProfile
->crash_setpoint_threshold
;
493 crashLimitYaw
= pidProfile
->crash_limit_yaw
;
494 itermLimit
= pidProfile
->itermLimit
;
495 #if defined(USE_THROTTLE_BOOST)
496 throttleBoost
= pidProfile
->throttle_boost
* 0.1f
;
498 itermRotation
= pidProfile
->iterm_rotation
;
499 antiGravityMode
= pidProfile
->antiGravityMode
;
501 // Calculate the anti-gravity value that will trigger the OSD display.
502 // For classic AG it's either 1.0 for off and > 1.0 for on.
503 // For the new AG it's a continuous floating value so we want to trigger the OSD
504 // display when it exceeds 25% of its possible range. This gives a useful indication
505 // of AG activity without excessive display.
506 antiGravityOsdCutoff
= 1.0f
;
507 if (antiGravityMode
== ANTI_GRAVITY_SMOOTH
) {
508 antiGravityOsdCutoff
+= ((itermAcceleratorGain
- 1000) / 1000.0f
) * 0.25f
;
511 #if defined(USE_SMART_FEEDFORWARD)
512 smartFeedforward
= pidProfile
->smart_feedforward
;
514 #if defined(USE_ITERM_RELAX)
515 itermRelax
= pidProfile
->iterm_relax
;
516 itermRelaxType
= pidProfile
->iterm_relax_type
;
517 itermRelaxCutoff
= pidProfile
->iterm_relax_cutoff
;
520 #ifdef USE_ACRO_TRAINER
521 acroTrainerAngleLimit
= pidProfile
->acro_trainer_angle_limit
;
522 acroTrainerLookaheadTime
= (float)pidProfile
->acro_trainer_lookahead_ms
/ 1000.0f
;
523 acroTrainerDebugAxis
= pidProfile
->acro_trainer_debug_axis
;
524 acroTrainerGain
= (float)pidProfile
->acro_trainer_gain
/ 10.0f
;
525 #endif // USE_ACRO_TRAINER
527 #if defined(USE_ABSOLUTE_CONTROL)
528 acGain
= (float)pidProfile
->abs_control_gain
;
529 acLimit
= (float)pidProfile
->abs_control_limit
;
530 acErrorLimit
= (float)pidProfile
->abs_control_error_limit
;
534 if (pidProfile
->dyn_lpf_dterm_idle
> 0 && pidProfile
->dyn_lpf_dterm_max_hz
> 0) {
535 if (pidProfile
->dterm_lowpass_hz
> 0 ) {
536 dynLpfMin
= pidProfile
->dterm_lowpass_hz
;
537 switch (pidProfile
->dterm_filter_type
) {
539 dynLpfFilter
= DYN_LPF_PT1
;
542 dynLpfFilter
= DYN_LPF_BIQUAD
;
545 dynLpfFilter
= DYN_LPF_NONE
;
550 dynLpfFilter
= DYN_LPF_NONE
;
552 dynLpfIdle
= pidProfile
->dyn_lpf_dterm_idle
/ 100.0f
;
553 dynLpfIdlePoint
= (dynLpfIdle
- (dynLpfIdle
* dynLpfIdle
* dynLpfIdle
) / 3.0f
) * 1.5f
;
554 dynLpfInvIdlePointScaled
= 1 / (1 - dynLpfIdlePoint
) * (pidProfile
->dyn_lpf_dterm_max_hz
- dynLpfMin
);
558 void pidInit(const pidProfile_t
*pidProfile
)
560 pidSetTargetLooptime(gyro
.targetLooptime
* pidConfig()->pid_process_denom
); // Initialize pid looptime
561 pidInitFilters(pidProfile
);
562 pidInitConfig(pidProfile
);
565 #ifdef USE_ACRO_TRAINER
566 void pidAcroTrainerInit(void)
568 acroTrainerAxisState
[FD_ROLL
] = 0;
569 acroTrainerAxisState
[FD_PITCH
] = 0;
571 #endif // USE_ACRO_TRAINER
573 void pidCopyProfile(uint8_t dstPidProfileIndex
, uint8_t srcPidProfileIndex
)
575 if ((dstPidProfileIndex
< MAX_PROFILE_COUNT
-1 && srcPidProfileIndex
< MAX_PROFILE_COUNT
-1)
576 && dstPidProfileIndex
!= srcPidProfileIndex
578 memcpy(pidProfilesMutable(dstPidProfileIndex
), pidProfilesMutable(srcPidProfileIndex
), sizeof(pidProfile_t
));
582 // calculates strength of horizon leveling; 0 = none, 1.0 = most leveling
583 STATIC_UNIT_TESTED
float calcHorizonLevelStrength(void)
585 // start with 1.0 at center stick, 0.0 at max stick deflection:
586 float horizonLevelStrength
= 1.0f
- MAX(getRcDeflectionAbs(FD_ROLL
), getRcDeflectionAbs(FD_PITCH
));
588 // 0 at level, 90 at vertical, 180 at inverted (degrees):
589 const float currentInclination
= MAX(ABS(attitude
.values
.roll
), ABS(attitude
.values
.pitch
)) / 10.0f
;
591 // horizonTiltExpertMode: 0 = leveling always active when sticks centered,
592 // 1 = leveling can be totally off when inverted
593 if (horizonTiltExpertMode
) {
594 if (horizonTransition
> 0 && horizonCutoffDegrees
> 0) {
595 // if d_level > 0 and horizonTiltEffect < 175
596 // horizonCutoffDegrees: 0 to 125 => 270 to 90 (represents where leveling goes to zero)
597 // inclinationLevelRatio (0.0 to 1.0) is smaller (less leveling)
598 // for larger inclinations; 0.0 at horizonCutoffDegrees value:
599 const float inclinationLevelRatio
= constrainf((horizonCutoffDegrees
-currentInclination
) / horizonCutoffDegrees
, 0, 1);
600 // apply configured horizon sensitivity:
601 // when stick is near center (horizonLevelStrength ~= 1.0)
602 // H_sensitivity value has little effect,
603 // when stick is deflected (horizonLevelStrength near 0.0)
604 // H_sensitivity value has more effect:
605 horizonLevelStrength
= (horizonLevelStrength
- 1) * 100 / horizonTransition
+ 1;
606 // apply inclination ratio, which may lower leveling
607 // to zero regardless of stick position:
608 horizonLevelStrength
*= inclinationLevelRatio
;
609 } else { // d_level=0 or horizon_tilt_effect>=175 means no leveling
610 horizonLevelStrength
= 0;
612 } else { // horizon_tilt_expert_mode = 0 (leveling always active when sticks centered)
614 if (horizonFactorRatio
< 1.01f
) { // if horizonTiltEffect > 0
615 // horizonFactorRatio: 1.0 to 0.0 (larger means more leveling)
616 // inclinationLevelRatio (0.0 to 1.0) is smaller (less leveling)
617 // for larger inclinations, goes to 1.0 at inclination==level:
618 const float inclinationLevelRatio
= (180-currentInclination
)/180 * (1.0f
-horizonFactorRatio
) + horizonFactorRatio
;
619 // apply ratio to configured horizon sensitivity:
620 sensitFact
= horizonTransition
* inclinationLevelRatio
;
621 } else { // horizonTiltEffect=0 for "old" functionality
622 sensitFact
= horizonTransition
;
625 if (sensitFact
<= 0) { // zero means no leveling
626 horizonLevelStrength
= 0;
628 // when stick is near center (horizonLevelStrength ~= 1.0)
629 // sensitFact value has little effect,
630 // when stick is deflected (horizonLevelStrength near 0.0)
631 // sensitFact value has more effect:
632 horizonLevelStrength
= ((horizonLevelStrength
- 1) * (100 / sensitFact
)) + 1;
635 return constrainf(horizonLevelStrength
, 0, 1);
638 STATIC_UNIT_TESTED
float pidLevel(int axis
, const pidProfile_t
*pidProfile
, const rollAndPitchTrims_t
*angleTrim
, float currentPidSetpoint
) {
639 // calculate error angle and limit the angle to the max inclination
640 // rcDeflection is in range [-1.0, 1.0]
641 float angle
= pidProfile
->levelAngleLimit
* getRcDeflection(axis
);
642 #ifdef USE_GPS_RESCUE
643 angle
+= gpsRescueAngle
[axis
] / 100; // ANGLE IS IN CENTIDEGREES
645 angle
= constrainf(angle
, -pidProfile
->levelAngleLimit
, pidProfile
->levelAngleLimit
);
646 const float errorAngle
= angle
- ((attitude
.raw
[axis
] - angleTrim
->raw
[axis
]) / 10.0f
);
647 if (FLIGHT_MODE(ANGLE_MODE
) || FLIGHT_MODE(GPS_RESCUE_MODE
)) {
648 // ANGLE mode - control is angle based
649 currentPidSetpoint
= errorAngle
* levelGain
;
651 // HORIZON mode - mix of ANGLE and ACRO modes
652 // mix in errorAngle to currentPidSetpoint to add a little auto-level feel
653 const float horizonLevelStrength
= calcHorizonLevelStrength();
654 currentPidSetpoint
= currentPidSetpoint
+ (errorAngle
* horizonGain
* horizonLevelStrength
);
656 return currentPidSetpoint
;
659 static float accelerationLimit(int axis
, float currentPidSetpoint
)
661 static float previousSetpoint
[XYZ_AXIS_COUNT
];
662 const float currentVelocity
= currentPidSetpoint
- previousSetpoint
[axis
];
664 if (ABS(currentVelocity
) > maxVelocity
[axis
]) {
665 currentPidSetpoint
= (currentVelocity
> 0) ? previousSetpoint
[axis
] + maxVelocity
[axis
] : previousSetpoint
[axis
] - maxVelocity
[axis
];
668 previousSetpoint
[axis
] = currentPidSetpoint
;
669 return currentPidSetpoint
;
672 static timeUs_t crashDetectedAtUs
;
674 static void handleCrashRecovery(
675 const pidCrashRecovery_e crash_recovery
, const rollAndPitchTrims_t
*angleTrim
,
676 const int axis
, const timeUs_t currentTimeUs
, const float gyroRate
, float *currentPidSetpoint
, float *errorRate
)
678 if (inCrashRecoveryMode
&& cmpTimeUs(currentTimeUs
, crashDetectedAtUs
) > crashTimeDelayUs
) {
679 if (crash_recovery
== PID_CRASH_RECOVERY_BEEP
) {
682 if (axis
== FD_YAW
) {
683 *errorRate
= constrainf(*errorRate
, -crashLimitYaw
, crashLimitYaw
);
685 // on roll and pitch axes calculate currentPidSetpoint and errorRate to level the aircraft to recover from crash
686 if (sensors(SENSOR_ACC
)) {
687 // errorAngle is deviation from horizontal
688 const float errorAngle
= -(attitude
.raw
[axis
] - angleTrim
->raw
[axis
]) / 10.0f
;
689 *currentPidSetpoint
= errorAngle
* levelGain
;
690 *errorRate
= *currentPidSetpoint
- gyroRate
;
693 // reset iterm, since accumulated error before crash is now meaningless
694 // and iterm windup during crash recovery can be extreme, especially on yaw axis
695 pidData
[axis
].I
= 0.0f
;
696 if (cmpTimeUs(currentTimeUs
, crashDetectedAtUs
) > crashTimeLimitUs
697 || (getMotorMixRange() < 1.0f
698 && ABS(gyro
.gyroADCf
[FD_ROLL
]) < crashRecoveryRate
699 && ABS(gyro
.gyroADCf
[FD_PITCH
]) < crashRecoveryRate
700 && ABS(gyro
.gyroADCf
[FD_YAW
]) < crashRecoveryRate
)) {
701 if (sensors(SENSOR_ACC
)) {
702 // check aircraft nearly level
703 if (ABS(attitude
.raw
[FD_ROLL
] - angleTrim
->raw
[FD_ROLL
]) < crashRecoveryAngleDeciDegrees
704 && ABS(attitude
.raw
[FD_PITCH
] - angleTrim
->raw
[FD_PITCH
]) < crashRecoveryAngleDeciDegrees
) {
705 inCrashRecoveryMode
= false;
709 inCrashRecoveryMode
= false;
716 static void detectAndSetCrashRecovery(
717 const pidCrashRecovery_e crash_recovery
, const int axis
,
718 const timeUs_t currentTimeUs
, const float delta
, const float errorRate
)
720 // if crash recovery is on and accelerometer enabled and there is no gyro overflow, then check for a crash
721 // no point in trying to recover if the crash is so severe that the gyro overflows
722 if ((crash_recovery
|| FLIGHT_MODE(GPS_RESCUE_MODE
)) && !gyroOverflowDetected()) {
723 if (ARMING_FLAG(ARMED
)) {
724 if (getMotorMixRange() >= 1.0f
&& !inCrashRecoveryMode
725 && ABS(delta
) > crashDtermThreshold
726 && ABS(errorRate
) > crashGyroThreshold
727 && ABS(getSetpointRate(axis
)) < crashSetpointThreshold
) {
728 inCrashRecoveryMode
= true;
729 crashDetectedAtUs
= currentTimeUs
;
731 if (inCrashRecoveryMode
&& cmpTimeUs(currentTimeUs
, crashDetectedAtUs
) < crashTimeDelayUs
&& (ABS(errorRate
) < crashGyroThreshold
732 || ABS(getSetpointRate(axis
)) > crashSetpointThreshold
)) {
733 inCrashRecoveryMode
= false;
736 } else if (inCrashRecoveryMode
) {
737 inCrashRecoveryMode
= false;
743 static void rotateVector(float v
[XYZ_AXIS_COUNT
], float rotation
[XYZ_AXIS_COUNT
])
745 // rotate v around rotation vector rotation
746 // rotation in radians, all elements must be small
747 for (int i
= 0; i
< XYZ_AXIS_COUNT
; i
++) {
748 int i_1
= (i
+ 1) % 3;
749 int i_2
= (i
+ 2) % 3;
750 float newV
= v
[i_1
] + v
[i_2
] * rotation
[i
];
751 v
[i_2
] -= v
[i_1
] * rotation
[i
];
756 STATIC_UNIT_TESTED
void rotateItermAndAxisError()
759 #if defined(USE_ABSOLUTE_CONTROL)
763 const float gyroToAngle
= dT
* RAD
;
764 float rotationRads
[XYZ_AXIS_COUNT
];
765 for (int i
= FD_ROLL
; i
<= FD_YAW
; i
++) {
766 rotationRads
[i
] = gyro
.gyroADCf
[i
] * gyroToAngle
;
768 #if defined(USE_ABSOLUTE_CONTROL)
770 rotateVector(axisError
, rotationRads
);
774 float v
[XYZ_AXIS_COUNT
];
775 for (int i
= 0; i
< XYZ_AXIS_COUNT
; i
++) {
778 rotateVector(v
, rotationRads
);
779 for (int i
= 0; i
< XYZ_AXIS_COUNT
; i
++) {
786 #ifdef USE_ACRO_TRAINER
788 int acroTrainerSign(float x
)
790 return x
> 0 ? 1 : -1;
793 // Acro Trainer - Manipulate the setPoint to limit axis angle while in acro mode
794 // There are three states:
795 // 1. Current angle has exceeded limit
796 // Apply correction to return to limit (similar to pidLevel)
797 // 2. Future overflow has been projected based on current angle and gyro rate
798 // Manage the setPoint to control the gyro rate as the actual angle approaches the limit (try to prevent overshoot)
799 // 3. If no potential overflow is detected, then return the original setPoint
801 // Use the FAST_CODE_NOINLINE directive to avoid this code from being inlined into ITCM RAM. We accept the
802 // performance decrease when Acro Trainer mode is active under the assumption that user is unlikely to be
803 // expecting ultimate flight performance at very high loop rates when in this mode.
804 static FAST_CODE_NOINLINE
float applyAcroTrainer(int axis
, const rollAndPitchTrims_t
*angleTrim
, float setPoint
)
806 float ret
= setPoint
;
808 if (!FLIGHT_MODE(ANGLE_MODE
) && !FLIGHT_MODE(HORIZON_MODE
) && !FLIGHT_MODE(GPS_RESCUE_MODE
)) {
809 bool resetIterm
= false;
810 float projectedAngle
= 0;
811 const int setpointSign
= acroTrainerSign(setPoint
);
812 const float currentAngle
= (attitude
.raw
[axis
] - angleTrim
->raw
[axis
]) / 10.0f
;
813 const int angleSign
= acroTrainerSign(currentAngle
);
815 if ((acroTrainerAxisState
[axis
] != 0) && (acroTrainerAxisState
[axis
] != setpointSign
)) { // stick has reversed - stop limiting
816 acroTrainerAxisState
[axis
] = 0;
819 // Limit and correct the angle when it exceeds the limit
820 if ((fabsf(currentAngle
) > acroTrainerAngleLimit
) && (acroTrainerAxisState
[axis
] == 0)) {
821 if (angleSign
== setpointSign
) {
822 acroTrainerAxisState
[axis
] = angleSign
;
827 if (acroTrainerAxisState
[axis
] != 0) {
828 ret
= constrainf(((acroTrainerAngleLimit
* angleSign
) - currentAngle
) * acroTrainerGain
, -ACRO_TRAINER_SETPOINT_LIMIT
, ACRO_TRAINER_SETPOINT_LIMIT
);
831 // Not currently over the limit so project the angle based on current angle and
832 // gyro angular rate using a sliding window based on gyro rate (faster rotation means larger window.
833 // If the projected angle exceeds the limit then apply limiting to minimize overshoot.
834 // Calculate the lookahead window by scaling proportionally with gyro rate from 0-500dps
835 float checkInterval
= constrainf(fabsf(gyro
.gyroADCf
[axis
]) / ACRO_TRAINER_LOOKAHEAD_RATE_LIMIT
, 0.0f
, 1.0f
) * acroTrainerLookaheadTime
;
836 projectedAngle
= (gyro
.gyroADCf
[axis
] * checkInterval
) + currentAngle
;
837 const int projectedAngleSign
= acroTrainerSign(projectedAngle
);
838 if ((fabsf(projectedAngle
) > acroTrainerAngleLimit
) && (projectedAngleSign
== setpointSign
)) {
839 ret
= ((acroTrainerAngleLimit
* projectedAngleSign
) - projectedAngle
) * acroTrainerGain
;
848 if (axis
== acroTrainerDebugAxis
) {
849 DEBUG_SET(DEBUG_ACRO_TRAINER
, 0, lrintf(currentAngle
* 10.0f
));
850 DEBUG_SET(DEBUG_ACRO_TRAINER
, 1, acroTrainerAxisState
[axis
]);
851 DEBUG_SET(DEBUG_ACRO_TRAINER
, 2, lrintf(ret
));
852 DEBUG_SET(DEBUG_ACRO_TRAINER
, 3, lrintf(projectedAngle
* 10.0f
));
858 #endif // USE_ACRO_TRAINER
860 #ifdef USE_RC_SMOOTHING_FILTER
861 float FAST_CODE
applyRcSmoothingDerivativeFilter(int axis
, float pidSetpointDelta
)
863 float ret
= pidSetpointDelta
;
864 if (axis
== rcSmoothingDebugAxis
) {
865 DEBUG_SET(DEBUG_RC_SMOOTHING
, 1, lrintf(pidSetpointDelta
* 100.0f
));
867 if (setpointDerivativeLpfInitialized
) {
868 switch (rcSmoothingFilterType
) {
869 case RC_SMOOTHING_DERIVATIVE_PT1
:
870 ret
= pt1FilterApply(&setpointDerivativePt1
[axis
], pidSetpointDelta
);
872 case RC_SMOOTHING_DERIVATIVE_BIQUAD
:
873 ret
= biquadFilterApplyDF1(&setpointDerivativeBiquad
[axis
], pidSetpointDelta
);
876 if (axis
== rcSmoothingDebugAxis
) {
877 DEBUG_SET(DEBUG_RC_SMOOTHING
, 2, lrintf(ret
* 100.0f
));
882 #endif // USE_RC_SMOOTHING_FILTER
884 #ifdef USE_SMART_FEEDFORWARD
885 void FAST_CODE
applySmartFeedforward(int axis
)
887 if (smartFeedforward
) {
888 if (pidData
[axis
].P
* pidData
[axis
].F
> 0) {
889 if (ABS(pidData
[axis
].F
) > ABS(pidData
[axis
].P
)) {
897 #endif // USE_SMART_FEEDFORWARD
899 #if defined(USE_ABSOLUTE_CONTROL)
900 STATIC_UNIT_TESTED
void applyAbsoluteControl(const int axis
, const float gyroRate
, const bool itermRelaxIsEnabled
,
901 const float setpointLpf
, const float setpointHpf
,
902 float *currentPidSetpoint
, float *itermErrorRate
)
905 float acErrorRate
= 0;
906 if (itermRelaxIsEnabled
) {
907 const float gmaxac
= setpointLpf
+ 2 * setpointHpf
;
908 const float gminac
= setpointLpf
- 2 * setpointHpf
;
909 if (gyroRate
>= gminac
&& gyroRate
<= gmaxac
) {
910 const float acErrorRate1
= gmaxac
- gyroRate
;
911 const float acErrorRate2
= gminac
- gyroRate
;
912 if (acErrorRate1
* axisError
[axis
] < 0) {
913 acErrorRate
= acErrorRate1
;
915 acErrorRate
= acErrorRate2
;
917 if (fabsf(acErrorRate
* dT
) > fabsf(axisError
[axis
]) ) {
918 acErrorRate
= -axisError
[axis
] * pidFrequency
;
921 acErrorRate
= (gyroRate
> gmaxac
? gmaxac
: gminac
) - gyroRate
;
924 acErrorRate
= *itermErrorRate
;
927 if (isAirmodeActivated()) {
928 axisError
[axis
] = constrainf(axisError
[axis
] + acErrorRate
* dT
,
929 -acErrorLimit
, acErrorLimit
);
930 const float acCorrection
= constrainf(axisError
[axis
] * acGain
, -acLimit
, acLimit
);
931 *currentPidSetpoint
+= acCorrection
;
932 *itermErrorRate
+= acCorrection
;
933 if (axis
== FD_ROLL
) {
934 DEBUG_SET(DEBUG_ITERM_RELAX
, 3, lrintf(axisError
[axis
] * 10));
941 #if defined(USE_ITERM_RELAX)
942 STATIC_UNIT_TESTED
void applyItermRelax(const int axis
, const float iterm
,
943 const float gyroRate
, float *itermErrorRate
, float *currentPidSetpoint
)
945 const float setpointLpf
= pt1FilterApply(&windupLpf
[axis
], *currentPidSetpoint
);
946 const float setpointHpf
= fabsf(*currentPidSetpoint
- setpointLpf
);
947 bool itermRelaxIsEnabled
= false;
950 (axis
< FD_YAW
|| itermRelax
== ITERM_RELAX_RPY
||
951 itermRelax
== ITERM_RELAX_RPY_INC
)) {
952 itermRelaxIsEnabled
= true;
953 const float itermRelaxFactor
= 1 - setpointHpf
/ ITERM_RELAX_SETPOINT_THRESHOLD
;
955 const bool isDecreasingI
=
956 ((iterm
> 0) && (*itermErrorRate
< 0)) || ((iterm
< 0) && (*itermErrorRate
> 0));
957 if ((itermRelax
>= ITERM_RELAX_RP_INC
) && isDecreasingI
) {
958 // Do Nothing, use the precalculed itermErrorRate
959 } else if (itermRelaxType
== ITERM_RELAX_SETPOINT
&& setpointHpf
< ITERM_RELAX_SETPOINT_THRESHOLD
) {
960 *itermErrorRate
*= itermRelaxFactor
;
961 } else if (itermRelaxType
== ITERM_RELAX_GYRO
) {
962 *itermErrorRate
= fapplyDeadband(setpointLpf
- gyroRate
, setpointHpf
);
964 *itermErrorRate
= 0.0f
;
967 if (axis
== FD_ROLL
) {
968 DEBUG_SET(DEBUG_ITERM_RELAX
, 0, lrintf(setpointHpf
));
969 DEBUG_SET(DEBUG_ITERM_RELAX
, 1, lrintf(itermRelaxFactor
* 100.0f
));
970 DEBUG_SET(DEBUG_ITERM_RELAX
, 2, lrintf(*itermErrorRate
));
973 #if defined(USE_ABSOLUTE_CONTROL)
974 applyAbsoluteControl(axis
, gyroRate
, setpointLpf
, setpointHpf
, itermRelaxIsEnabled
, currentPidSetpoint
, itermErrorRate
);
976 UNUSED(itermRelaxIsEnabled
);
982 // Betaflight pid controller, which will be maintained in the future with additional features specialised for current (mini) multirotor usage.
983 // Based on 2DOF reference design (matlab)
984 void FAST_CODE
pidController(const pidProfile_t
*pidProfile
, const rollAndPitchTrims_t
*angleTrim
, timeUs_t currentTimeUs
)
986 static float previousGyroRateDterm
[XYZ_AXIS_COUNT
];
987 static float previousPidSetpoint
[XYZ_AXIS_COUNT
];
989 const float tpaFactor
= getThrottlePIDAttenuation();
991 #ifdef USE_YAW_SPIN_RECOVERY
992 const bool yawSpinActive
= gyroYawSpinDetected();
995 // Dynamic i component,
996 if ((antiGravityMode
== ANTI_GRAVITY_SMOOTH
) && antiGravityEnabled
) {
997 itermAccelerator
= 1 + fabsf(antiGravityThrottleHpf
) * 0.01f
* (itermAcceleratorGain
- 1000);
998 DEBUG_SET(DEBUG_ANTI_GRAVITY
, 1, lrintf(antiGravityThrottleHpf
* 1000));
1000 DEBUG_SET(DEBUG_ANTI_GRAVITY
, 0, lrintf(itermAccelerator
* 1000));
1002 // gradually scale back integration when above windup point
1003 float dynCi
= dT
* itermAccelerator
;
1004 if (itermWindupPointInv
> 1.0f
) {
1005 dynCi
*= constrainf((1.0f
- getMotorMixRange()) * itermWindupPointInv
, 0.0f
, 1.0f
);
1008 // Precalculate gyro deta for D-term here, this allows loop unrolling
1009 float gyroRateDterm
[XYZ_AXIS_COUNT
];
1010 for (int axis
= FD_ROLL
; axis
<= FD_YAW
; ++axis
) {
1011 gyroRateDterm
[axis
] = dtermNotchApplyFn((filter_t
*) &dtermNotch
[axis
], gyro
.gyroADCf
[axis
]);
1012 gyroRateDterm
[axis
] = dtermLowpassApplyFn((filter_t
*) &dtermLowpass
[axis
], gyroRateDterm
[axis
]);
1013 gyroRateDterm
[axis
] = dtermLowpass2ApplyFn((filter_t
*) &dtermLowpass2
[axis
], gyroRateDterm
[axis
]);
1016 rotateItermAndAxisError();
1018 // ----------PID controller----------
1019 for (int axis
= FD_ROLL
; axis
<= FD_YAW
; ++axis
) {
1021 float currentPidSetpoint
= getSetpointRate(axis
);
1022 if (maxVelocity
[axis
]) {
1023 currentPidSetpoint
= accelerationLimit(axis
, currentPidSetpoint
);
1025 // Yaw control is GYRO based, direct sticks control is applied to rate PID
1026 if ((FLIGHT_MODE(ANGLE_MODE
) || FLIGHT_MODE(HORIZON_MODE
) || FLIGHT_MODE(GPS_RESCUE_MODE
)) && axis
!= FD_YAW
) {
1027 currentPidSetpoint
= pidLevel(axis
, pidProfile
, angleTrim
, currentPidSetpoint
);
1030 #ifdef USE_ACRO_TRAINER
1031 if ((axis
!= FD_YAW
) && acroTrainerActive
&& !inCrashRecoveryMode
) {
1032 currentPidSetpoint
= applyAcroTrainer(axis
, angleTrim
, currentPidSetpoint
);
1034 #endif // USE_ACRO_TRAINER
1036 // Handle yaw spin recovery - zero the setpoint on yaw to aid in recovery
1037 // It's not necessary to zero the set points for R/P because the PIDs will be zeroed below
1038 #ifdef USE_YAW_SPIN_RECOVERY
1039 if ((axis
== FD_YAW
) && yawSpinActive
) {
1040 currentPidSetpoint
= 0.0f
;
1042 #endif // USE_YAW_SPIN_RECOVERY
1044 // -----calculate error rate
1045 const float gyroRate
= gyro
.gyroADCf
[axis
]; // Process variable from gyro output in deg/sec
1046 float errorRate
= currentPidSetpoint
- gyroRate
; // r - y
1047 handleCrashRecovery(
1048 pidProfile
->crash_recovery
, angleTrim
, axis
, currentTimeUs
, gyroRate
,
1049 ¤tPidSetpoint
, &errorRate
);
1051 const float iterm
= pidData
[axis
].I
;
1052 float itermErrorRate
= errorRate
;
1054 #if defined(USE_ITERM_RELAX)
1055 applyItermRelax(axis
, iterm
, gyroRate
, &itermErrorRate
, ¤tPidSetpoint
);
1058 // --------low-level gyro-based PID based on 2DOF PID controller. ----------
1059 // 2-DOF PID controller with optional filter on derivative term.
1060 // b = 1 and only c (feedforward weight) can be tuned (amount derivative on measurement or error).
1062 // -----calculate P component
1063 pidData
[axis
].P
= pidCoefficient
[axis
].Kp
* errorRate
* tpaFactor
;
1064 if (axis
== FD_YAW
) {
1065 pidData
[axis
].P
= ptermYawLowpassApplyFn((filter_t
*) &ptermYawLowpass
, pidData
[axis
].P
);
1068 // -----calculate I component
1069 pidData
[axis
].I
= constrainf(iterm
+ pidCoefficient
[axis
].Ki
* itermErrorRate
* dynCi
, -itermLimit
, itermLimit
);
1071 // -----calculate D component
1072 if (pidCoefficient
[axis
].Kd
> 0) {
1074 // Divide rate change by dT to get differential (ie dr/dt).
1075 // dT is fixed and calculated from the target PID loop time
1076 // This is done to avoid DTerm spikes that occur with dynamically
1077 // calculated deltaT whenever another task causes the PID
1078 // loop execution to be delayed.
1080 - (gyroRateDterm
[axis
] - previousGyroRateDterm
[axis
]) * pidFrequency
;
1082 detectAndSetCrashRecovery(pidProfile
->crash_recovery
, axis
, currentTimeUs
, delta
, errorRate
);
1084 pidData
[axis
].D
= pidCoefficient
[axis
].Kd
* delta
* tpaFactor
;
1086 pidData
[axis
].D
= 0;
1088 previousGyroRateDterm
[axis
] = gyroRateDterm
[axis
];
1090 // -----calculate feedforward component
1092 // Only enable feedforward for rate mode
1093 const float feedforwardGain
= flightModeFlags
? 0.0f
: pidCoefficient
[axis
].Kf
;
1095 if (feedforwardGain
> 0) {
1097 // no transition if feedForwardTransition == 0
1098 float transition
= feedForwardTransition
> 0 ? MIN(1.f
, getRcDeflectionAbs(axis
) * feedForwardTransition
) : 1;
1100 float pidSetpointDelta
= currentPidSetpoint
- previousPidSetpoint
[axis
];
1102 #ifdef USE_RC_SMOOTHING_FILTER
1103 pidSetpointDelta
= applyRcSmoothingDerivativeFilter(axis
, pidSetpointDelta
);
1104 #endif // USE_RC_SMOOTHING_FILTER
1106 pidData
[axis
].F
= feedforwardGain
* transition
* pidSetpointDelta
* pidFrequency
;
1108 #if defined(USE_SMART_FEEDFORWARD)
1109 applySmartFeedforward(axis
);
1112 pidData
[axis
].F
= 0;
1114 previousPidSetpoint
[axis
] = currentPidSetpoint
;
1116 #ifdef USE_YAW_SPIN_RECOVERY
1117 if (yawSpinActive
) {
1118 pidData
[axis
].I
= 0; // in yaw spin always disable I
1119 if (axis
<= FD_PITCH
) {
1120 // zero PIDs on pitch and roll leaving yaw P to correct spin
1121 pidData
[axis
].P
= 0;
1122 pidData
[axis
].D
= 0;
1123 pidData
[axis
].F
= 0;
1126 #endif // USE_YAW_SPIN_RECOVERY
1128 // calculating the PID sum
1129 pidData
[axis
].Sum
= pidData
[axis
].P
+ pidData
[axis
].I
+ pidData
[axis
].D
+ pidData
[axis
].F
;
1132 // Disable PID control if at zero throttle or if gyro overflow detected
1133 // This may look very innefficient, but it is done on purpose to always show real CPU usage as in flight
1134 if (!pidStabilisationEnabled
|| gyroOverflowDetected()) {
1135 for (int axis
= FD_ROLL
; axis
<= FD_YAW
; ++axis
) {
1136 pidData
[axis
].P
= 0;
1137 pidData
[axis
].I
= 0;
1138 pidData
[axis
].D
= 0;
1139 pidData
[axis
].F
= 0;
1141 pidData
[axis
].Sum
= 0;
1146 bool crashRecoveryModeActive(void)
1148 return inCrashRecoveryMode
;
1151 #ifdef USE_ACRO_TRAINER
1152 void pidSetAcroTrainerState(bool newState
)
1154 if (acroTrainerActive
!= newState
) {
1156 pidAcroTrainerInit();
1158 acroTrainerActive
= newState
;
1161 #endif // USE_ACRO_TRAINER
1163 void pidSetAntiGravityState(bool newState
)
1165 if (newState
!= antiGravityEnabled
) {
1166 // reset the accelerator on state changes
1167 itermAccelerator
= 1.0f
;
1169 antiGravityEnabled
= newState
;
1172 bool pidAntiGravityEnabled(void)
1174 return antiGravityEnabled
;
1178 void dynLpfDTermUpdate(float throttle
)
1180 if (dynLpfFilter
!= DYN_LPF_NONE
) {
1181 uint16_t cutoffFreq
= dynLpfMin
;
1182 if (throttle
> dynLpfIdle
) {
1183 const float dynThrottle
= (throttle
- (throttle
* throttle
* throttle
) / 3.0f
) * 1.5f
;
1184 cutoffFreq
+= (dynThrottle
- dynLpfIdlePoint
) * dynLpfInvIdlePointScaled
;
1187 if (dynLpfFilter
== DYN_LPF_PT1
) {
1188 for (int axis
= 0; axis
< XYZ_AXIS_COUNT
; axis
++) {
1189 pt1FilterUpdateCutoff(&dtermLowpass
[axis
].pt1Filter
, pt1FilterGain(cutoffFreq
, dT
));
1192 for (int axis
= 0; axis
< XYZ_AXIS_COUNT
; axis
++) {
1193 biquadFilterUpdateLPF(&dtermLowpass
[axis
].biquadFilter
, cutoffFreq
, targetPidLooptime
);