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/filter.h"
33 #include "common/maths.h"
35 #include "config/config_reset.h"
37 #include "drivers/dshot_command.h"
38 #include "drivers/pwm_output.h"
39 #include "drivers/sound_beeper.h"
40 #include "drivers/time.h"
42 #include "fc/controlrate_profile.h"
45 #include "fc/rc_controls.h"
46 #include "fc/runtime_config.h"
48 #include "flight/gps_rescue.h"
49 #include "flight/imu.h"
50 #include "flight/mixer.h"
51 #include "flight/rpm_filter.h"
52 #include "flight/interpolated_setpoint.h"
57 #include "pg/pg_ids.h"
59 #include "sensors/acceleration.h"
60 #include "sensors/battery.h"
61 #include "sensors/gyro.h"
65 const char pidNames
[] =
72 FAST_RAM_ZERO_INIT
uint32_t targetPidLooptime
;
73 FAST_RAM_ZERO_INIT pidAxisData_t pidData
[XYZ_AXIS_COUNT
];
75 static FAST_RAM_ZERO_INIT
bool pidStabilisationEnabled
;
77 static FAST_RAM_ZERO_INIT
bool inCrashRecoveryMode
= false;
79 static FAST_RAM_ZERO_INIT
float dT
;
80 static FAST_RAM_ZERO_INIT
float pidFrequency
;
82 static FAST_RAM_ZERO_INIT
uint8_t antiGravityMode
;
83 static FAST_RAM_ZERO_INIT
float antiGravityThrottleHpf
;
84 static FAST_RAM_ZERO_INIT
uint16_t itermAcceleratorGain
;
85 static FAST_RAM_ZERO_INIT
float antiGravityOsdCutoff
;
86 static FAST_RAM_ZERO_INIT
bool antiGravityEnabled
;
87 static FAST_RAM_ZERO_INIT
bool zeroThrottleItermReset
;
89 PG_REGISTER_WITH_RESET_TEMPLATE(pidConfig_t
, pidConfig
, PG_PID_CONFIG
, 2);
92 #define PID_PROCESS_DENOM_DEFAULT 1
93 #elif defined(USE_GYRO_SPI_MPU6000) || defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_ICM20689)
94 #define PID_PROCESS_DENOM_DEFAULT 4
96 #define PID_PROCESS_DENOM_DEFAULT 2
98 #if defined(USE_D_MIN)
99 #define D_MIN_GAIN_FACTOR 0.00005f
100 #define D_MIN_SETPOINT_GAIN_FACTOR 0.00005f
101 #define D_MIN_RANGE_HZ 80 // Biquad lowpass input cutoff to peak D around propwash frequencies
102 #define D_MIN_LOWPASS_HZ 10 // PT1 lowpass cutoff to smooth the boost effect
105 #ifdef USE_RUNAWAY_TAKEOFF
106 PG_RESET_TEMPLATE(pidConfig_t
, pidConfig
,
107 .pid_process_denom
= PID_PROCESS_DENOM_DEFAULT
,
108 .runaway_takeoff_prevention
= true,
109 .runaway_takeoff_deactivate_throttle
= 20, // throttle level % needed to accumulate deactivation time
110 .runaway_takeoff_deactivate_delay
= 500 // Accumulated time (in milliseconds) before deactivation in successful takeoff
113 PG_RESET_TEMPLATE(pidConfig_t
, pidConfig
,
114 .pid_process_denom
= PID_PROCESS_DENOM_DEFAULT
118 #ifdef USE_ACRO_TRAINER
119 #define ACRO_TRAINER_LOOKAHEAD_RATE_LIMIT 500.0f // Max gyro rate for lookahead time scaling
120 #define ACRO_TRAINER_SETPOINT_LIMIT 1000.0f // Limit the correcting setpoint
121 #endif // USE_ACRO_TRAINER
123 #ifdef USE_AIRMODE_LPF
124 static FAST_RAM_ZERO_INIT
float airmodeThrottleOffsetLimit
;
127 #define ANTI_GRAVITY_THROTTLE_FILTER_CUTOFF 15 // The anti gravity throttle highpass filter cutoff
129 #define CRASH_RECOVERY_DETECTION_DELAY_US 1000000 // 1 second delay before crash recovery detection is active after entering a self-level mode
131 #define LAUNCH_CONTROL_YAW_ITERM_LIMIT 50 // yaw iterm windup limit when launch mode is "FULL" (all axes)
133 PG_REGISTER_ARRAY_WITH_RESET_FN(pidProfile_t
, PID_PROFILE_COUNT
, pidProfiles
, PG_PID_PROFILE
, 14);
135 void resetPidProfile(pidProfile_t
*pidProfile
)
137 RESET_CONFIG(pidProfile_t
, pidProfile
,
139 [PID_ROLL
] = { 42, 85, 35, 90 },
140 [PID_PITCH
] = { 46, 90, 38, 95 },
141 [PID_YAW
] = { 30, 90, 0, 90 },
142 [PID_LEVEL
] = { 50, 50, 75, 0 },
143 [PID_MAG
] = { 40, 0, 0, 0 },
145 .pidSumLimit
= PIDSUM_LIMIT
,
146 .pidSumLimitYaw
= PIDSUM_LIMIT_YAW
,
149 .dterm_notch_cutoff
= 0,
150 .itermWindupPointPercent
= 100,
151 .vbatPidCompensation
= 0,
152 .pidAtMinThrottle
= PID_STABILISATION_ON
,
153 .levelAngleLimit
= 55,
154 .feedForwardTransition
= 0,
155 .yawRateAccelLimit
= 0,
157 .itermThrottleThreshold
= 250,
158 .itermAcceleratorGain
= 3500,
159 .crash_time
= 500, // ms
160 .crash_delay
= 0, // ms
161 .crash_recovery_angle
= 10, // degrees
162 .crash_recovery_rate
= 100, // degrees/second
163 .crash_dthreshold
= 50, // degrees/second/second
164 .crash_gthreshold
= 400, // degrees/second
165 .crash_setpoint_threshold
= 350, // degrees/second
166 .crash_recovery
= PID_CRASH_RECOVERY_OFF
, // off by default
167 .horizon_tilt_effect
= 75,
168 .horizon_tilt_expert_mode
= false,
169 .crash_limit_yaw
= 200,
172 .throttle_boost_cutoff
= 15,
173 .iterm_rotation
= false,
174 .iterm_relax
= ITERM_RELAX_RP
,
175 .iterm_relax_cutoff
= ITERM_RELAX_CUTOFF_DEFAULT
,
176 .iterm_relax_type
= ITERM_RELAX_SETPOINT
,
177 .acro_trainer_angle_limit
= 20,
178 .acro_trainer_lookahead_ms
= 50,
179 .acro_trainer_debug_axis
= FD_ROLL
,
180 .acro_trainer_gain
= 75,
181 .abs_control_gain
= 0,
182 .abs_control_limit
= 90,
183 .abs_control_error_limit
= 20,
184 .abs_control_cutoff
= 11,
185 .antiGravityMode
= ANTI_GRAVITY_SMOOTH
,
186 .dterm_lowpass_hz
= 150, // NOTE: dynamic lpf is enabled by default so this setting is actually
187 // overridden and the static lowpass 1 is disabled. We can't set this
188 // value to 0 otherwise Configurator versions 10.4 and earlier will also
189 // reset the lowpass filter type to PT1 overriding the desired BIQUAD setting.
190 .dterm_lowpass2_hz
= 150, // second Dterm LPF ON by default
191 .dterm_filter_type
= FILTER_PT1
,
192 .dterm_filter2_type
= FILTER_PT1
,
193 .dyn_lpf_dterm_min_hz
= 70,
194 .dyn_lpf_dterm_max_hz
= 170,
195 .launchControlMode
= LAUNCH_CONTROL_MODE_NORMAL
,
196 .launchControlThrottlePercent
= 20,
197 .launchControlAngleLimit
= 0,
198 .launchControlGain
= 40,
199 .launchControlAllowTriggerReset
= true,
200 .use_integrated_yaw
= false,
201 .integrated_yaw_relax
= 200,
202 .thrustLinearization
= 0,
203 .d_min
= { 20, 22, 0 }, // roll, pitch, yaw
206 .motor_output_limit
= 100,
207 .auto_profile_cell_count
= AUTO_PROFILE_CELL_COUNT_STAY
,
208 .transient_throttle_limit
= 0,
209 .profileName
= { 0 },
211 .idle_adjustment_speed
= 50,
213 .idle_pid_limit
= 200,
214 .idle_max_increase
= 150,
215 .ff_interpolate_sp
= FF_INTERPOLATE_AVG2
,
216 .ff_spike_limit
= 60,
217 .ff_max_rate_limit
= 100,
218 .ff_smooth_factor
= 37,
220 .dyn_lpf_curve_expo
= 0,
221 .level_race_mode
= false,
224 pidProfile
->pid
[PID_ROLL
].D
= 30;
225 pidProfile
->pid
[PID_PITCH
].D
= 32;
229 void pgResetFn_pidProfiles(pidProfile_t
*pidProfiles
)
231 for (int i
= 0; i
< PID_PROFILE_COUNT
; i
++) {
232 resetPidProfile(&pidProfiles
[i
]);
236 static void pidSetTargetLooptime(uint32_t pidLooptime
)
238 targetPidLooptime
= pidLooptime
;
239 dT
= targetPidLooptime
* 1e-6f
;
240 pidFrequency
= 1.0f
/ dT
;
242 dshotSetPidLoopTime(targetPidLooptime
);
246 static FAST_RAM_ZERO_INIT
float itermAccelerator
;
248 void pidSetItermAccelerator(float newItermAccelerator
)
250 itermAccelerator
= newItermAccelerator
;
253 bool pidOsdAntiGravityActive(void)
255 return (itermAccelerator
> antiGravityOsdCutoff
);
258 void pidStabilisationState(pidStabilisationState_e pidControllerState
)
260 pidStabilisationEnabled
= (pidControllerState
== PID_STABILISATION_ON
) ? true : false;
263 const angle_index_t rcAliasToAngleIndexMap
[] = { AI_ROLL
, AI_PITCH
};
265 typedef union dtermLowpass_u
{
266 pt1Filter_t pt1Filter
;
267 biquadFilter_t biquadFilter
;
270 static FAST_RAM_ZERO_INIT
float previousPidSetpoint
[XYZ_AXIS_COUNT
];
272 static FAST_RAM_ZERO_INIT filterApplyFnPtr dtermNotchApplyFn
;
273 static FAST_RAM_ZERO_INIT biquadFilter_t dtermNotch
[XYZ_AXIS_COUNT
];
274 static FAST_RAM_ZERO_INIT filterApplyFnPtr dtermLowpassApplyFn
;
275 static FAST_RAM_ZERO_INIT dtermLowpass_t dtermLowpass
[XYZ_AXIS_COUNT
];
276 static FAST_RAM_ZERO_INIT filterApplyFnPtr dtermLowpass2ApplyFn
;
277 static FAST_RAM_ZERO_INIT dtermLowpass_t dtermLowpass2
[XYZ_AXIS_COUNT
];
278 static FAST_RAM_ZERO_INIT filterApplyFnPtr ptermYawLowpassApplyFn
;
279 static FAST_RAM_ZERO_INIT pt1Filter_t ptermYawLowpass
;
281 #if defined(USE_ITERM_RELAX)
282 static FAST_RAM_ZERO_INIT pt1Filter_t windupLpf
[XYZ_AXIS_COUNT
];
283 static FAST_RAM_ZERO_INIT
uint8_t itermRelax
;
284 static FAST_RAM_ZERO_INIT
uint8_t itermRelaxType
;
285 static uint8_t itermRelaxCutoff
;
286 static FAST_RAM_ZERO_INIT
float itermRelaxSetpointThreshold
;
289 #if defined(USE_ABSOLUTE_CONTROL)
290 STATIC_UNIT_TESTED FAST_RAM_ZERO_INIT
float axisError
[XYZ_AXIS_COUNT
];
291 static FAST_RAM_ZERO_INIT
float acGain
;
292 static FAST_RAM_ZERO_INIT
float acLimit
;
293 static FAST_RAM_ZERO_INIT
float acErrorLimit
;
294 static FAST_RAM_ZERO_INIT
float acCutoff
;
295 static FAST_RAM_ZERO_INIT pt1Filter_t acLpf
[XYZ_AXIS_COUNT
];
296 static FAST_RAM_ZERO_INIT
float oldSetpointCorrection
[XYZ_AXIS_COUNT
];
299 #if defined(USE_D_MIN)
300 static FAST_RAM_ZERO_INIT biquadFilter_t dMinRange
[XYZ_AXIS_COUNT
];
301 static FAST_RAM_ZERO_INIT pt1Filter_t dMinLowpass
[XYZ_AXIS_COUNT
];
304 #ifdef USE_RC_SMOOTHING_FILTER
305 static FAST_RAM_ZERO_INIT pt1Filter_t setpointDerivativePt1
[XYZ_AXIS_COUNT
];
306 static FAST_RAM_ZERO_INIT biquadFilter_t setpointDerivativeBiquad
[XYZ_AXIS_COUNT
];
307 static FAST_RAM_ZERO_INIT
bool setpointDerivativeLpfInitialized
;
308 static FAST_RAM_ZERO_INIT
uint8_t rcSmoothingDebugAxis
;
309 static FAST_RAM_ZERO_INIT
uint8_t rcSmoothingFilterType
;
310 #endif // USE_RC_SMOOTHING_FILTER
312 #ifdef USE_AIRMODE_LPF
313 static FAST_RAM_ZERO_INIT pt1Filter_t airmodeThrottleLpf1
;
314 static FAST_RAM_ZERO_INIT pt1Filter_t airmodeThrottleLpf2
;
317 static FAST_RAM_ZERO_INIT pt1Filter_t antiGravityThrottleLpf
;
319 static FAST_RAM_ZERO_INIT
float ffBoostFactor
;
320 static FAST_RAM_ZERO_INIT
float ffSmoothFactor
;
321 static FAST_RAM_ZERO_INIT
float ffSpikeLimitInverse
;
323 float pidGetSpikeLimitInverse()
325 return ffSpikeLimitInverse
;
329 float pidGetFfBoostFactor()
331 return ffBoostFactor
;
334 float pidGetFfSmoothFactor()
336 return ffSmoothFactor
;
340 void pidInitFilters(const pidProfile_t
*pidProfile
)
342 STATIC_ASSERT(FD_YAW
== 2, FD_YAW_incorrect
); // ensure yaw axis is 2
344 if (targetPidLooptime
== 0) {
345 // no looptime set, so set all the filters to null
346 dtermNotchApplyFn
= nullFilterApply
;
347 dtermLowpassApplyFn
= nullFilterApply
;
348 ptermYawLowpassApplyFn
= nullFilterApply
;
352 const uint32_t pidFrequencyNyquist
= pidFrequency
/ 2; // No rounding needed
354 uint16_t dTermNotchHz
;
355 if (pidProfile
->dterm_notch_hz
<= pidFrequencyNyquist
) {
356 dTermNotchHz
= pidProfile
->dterm_notch_hz
;
358 if (pidProfile
->dterm_notch_cutoff
< pidFrequencyNyquist
) {
359 dTermNotchHz
= pidFrequencyNyquist
;
365 if (dTermNotchHz
!= 0 && pidProfile
->dterm_notch_cutoff
!= 0) {
366 dtermNotchApplyFn
= (filterApplyFnPtr
)biquadFilterApply
;
367 const float notchQ
= filterGetNotchQ(dTermNotchHz
, pidProfile
->dterm_notch_cutoff
);
368 for (int axis
= FD_ROLL
; axis
<= FD_YAW
; axis
++) {
369 biquadFilterInit(&dtermNotch
[axis
], dTermNotchHz
, targetPidLooptime
, notchQ
, FILTER_NOTCH
);
372 dtermNotchApplyFn
= nullFilterApply
;
375 //1st Dterm Lowpass Filter
376 uint16_t dterm_lowpass_hz
= pidProfile
->dterm_lowpass_hz
;
379 if (pidProfile
->dyn_lpf_dterm_min_hz
) {
380 dterm_lowpass_hz
= pidProfile
->dyn_lpf_dterm_min_hz
;
384 if (dterm_lowpass_hz
> 0 && dterm_lowpass_hz
< pidFrequencyNyquist
) {
385 switch (pidProfile
->dterm_filter_type
) {
387 dtermLowpassApplyFn
= (filterApplyFnPtr
)pt1FilterApply
;
388 for (int axis
= FD_ROLL
; axis
<= FD_YAW
; axis
++) {
389 pt1FilterInit(&dtermLowpass
[axis
].pt1Filter
, pt1FilterGain(dterm_lowpass_hz
, dT
));
394 dtermLowpassApplyFn
= (filterApplyFnPtr
)biquadFilterApplyDF1
;
396 dtermLowpassApplyFn
= (filterApplyFnPtr
)biquadFilterApply
;
398 for (int axis
= FD_ROLL
; axis
<= FD_YAW
; axis
++) {
399 biquadFilterInitLPF(&dtermLowpass
[axis
].biquadFilter
, dterm_lowpass_hz
, targetPidLooptime
);
403 dtermLowpassApplyFn
= nullFilterApply
;
407 dtermLowpassApplyFn
= nullFilterApply
;
410 //2nd Dterm Lowpass Filter
411 if (pidProfile
->dterm_lowpass2_hz
== 0 || pidProfile
->dterm_lowpass2_hz
> pidFrequencyNyquist
) {
412 dtermLowpass2ApplyFn
= nullFilterApply
;
414 switch (pidProfile
->dterm_filter2_type
) {
416 dtermLowpass2ApplyFn
= (filterApplyFnPtr
)pt1FilterApply
;
417 for (int axis
= FD_ROLL
; axis
<= FD_YAW
; axis
++) {
418 pt1FilterInit(&dtermLowpass2
[axis
].pt1Filter
, pt1FilterGain(pidProfile
->dterm_lowpass2_hz
, dT
));
422 dtermLowpass2ApplyFn
= (filterApplyFnPtr
)biquadFilterApply
;
423 for (int axis
= FD_ROLL
; axis
<= FD_YAW
; axis
++) {
424 biquadFilterInitLPF(&dtermLowpass2
[axis
].biquadFilter
, pidProfile
->dterm_lowpass2_hz
, targetPidLooptime
);
428 dtermLowpass2ApplyFn
= nullFilterApply
;
433 if (pidProfile
->yaw_lowpass_hz
== 0 || pidProfile
->yaw_lowpass_hz
> pidFrequencyNyquist
) {
434 ptermYawLowpassApplyFn
= nullFilterApply
;
436 ptermYawLowpassApplyFn
= (filterApplyFnPtr
)pt1FilterApply
;
437 pt1FilterInit(&ptermYawLowpass
, pt1FilterGain(pidProfile
->yaw_lowpass_hz
, dT
));
440 #if defined(USE_THROTTLE_BOOST)
441 pt1FilterInit(&throttleLpf
, pt1FilterGain(pidProfile
->throttle_boost_cutoff
, dT
));
443 #if defined(USE_ITERM_RELAX)
445 for (int i
= 0; i
< XYZ_AXIS_COUNT
; i
++) {
446 pt1FilterInit(&windupLpf
[i
], pt1FilterGain(itermRelaxCutoff
, dT
));
450 #if defined(USE_ABSOLUTE_CONTROL)
452 for (int i
= 0; i
< XYZ_AXIS_COUNT
; i
++) {
453 pt1FilterInit(&acLpf
[i
], pt1FilterGain(acCutoff
, dT
));
457 #if defined(USE_D_MIN)
459 // Initialize the filters for all axis even if the d_min[axis] value is 0
460 // Otherwise if the pidProfile->d_min_xxx parameters are ever added to
461 // in-flight adjustments and transition from 0 to > 0 in flight the feature
462 // won't work because the filter wasn't initialized.
463 for (int axis
= FD_ROLL
; axis
<= FD_YAW
; axis
++) {
464 biquadFilterInitLPF(&dMinRange
[axis
], D_MIN_RANGE_HZ
, targetPidLooptime
);
465 pt1FilterInit(&dMinLowpass
[axis
], pt1FilterGain(D_MIN_LOWPASS_HZ
, dT
));
468 #if defined(USE_AIRMODE_LPF)
469 if (pidProfile
->transient_throttle_limit
) {
470 pt1FilterInit(&airmodeThrottleLpf1
, pt1FilterGain(7.0f
, dT
));
471 pt1FilterInit(&airmodeThrottleLpf2
, pt1FilterGain(20.0f
, dT
));
475 pt1FilterInit(&antiGravityThrottleLpf
, pt1FilterGain(ANTI_GRAVITY_THROTTLE_FILTER_CUTOFF
, dT
));
477 ffBoostFactor
= (float)pidProfile
->ff_boost
/ 10.0f
;
478 ffSpikeLimitInverse
= pidProfile
->ff_spike_limit
? 1.0f
/ ((float)pidProfile
->ff_spike_limit
/ 10.0f
) : 0.0f
;
481 #ifdef USE_RC_SMOOTHING_FILTER
482 void pidInitSetpointDerivativeLpf(uint16_t filterCutoff
, uint8_t debugAxis
, uint8_t filterType
)
484 rcSmoothingDebugAxis
= debugAxis
;
485 rcSmoothingFilterType
= filterType
;
486 if ((filterCutoff
> 0) && (rcSmoothingFilterType
!= RC_SMOOTHING_DERIVATIVE_OFF
)) {
487 setpointDerivativeLpfInitialized
= true;
488 for (int axis
= FD_ROLL
; axis
<= FD_YAW
; axis
++) {
489 switch (rcSmoothingFilterType
) {
490 case RC_SMOOTHING_DERIVATIVE_PT1
:
491 pt1FilterInit(&setpointDerivativePt1
[axis
], pt1FilterGain(filterCutoff
, dT
));
493 case RC_SMOOTHING_DERIVATIVE_BIQUAD
:
494 biquadFilterInitLPF(&setpointDerivativeBiquad
[axis
], filterCutoff
, targetPidLooptime
);
501 void pidUpdateSetpointDerivativeLpf(uint16_t filterCutoff
)
503 if ((filterCutoff
> 0) && (rcSmoothingFilterType
!= RC_SMOOTHING_DERIVATIVE_OFF
)) {
504 for (int axis
= FD_ROLL
; axis
<= FD_YAW
; axis
++) {
505 switch (rcSmoothingFilterType
) {
506 case RC_SMOOTHING_DERIVATIVE_PT1
:
507 pt1FilterUpdateCutoff(&setpointDerivativePt1
[axis
], pt1FilterGain(filterCutoff
, dT
));
509 case RC_SMOOTHING_DERIVATIVE_BIQUAD
:
510 biquadFilterUpdateLPF(&setpointDerivativeBiquad
[axis
], filterCutoff
, targetPidLooptime
);
516 #endif // USE_RC_SMOOTHING_FILTER
518 typedef struct pidCoefficient_s
{
525 static FAST_RAM_ZERO_INIT pidCoefficient_t pidCoefficient
[XYZ_AXIS_COUNT
];
526 static FAST_RAM_ZERO_INIT
float maxVelocity
[XYZ_AXIS_COUNT
];
527 static FAST_RAM_ZERO_INIT
float feedForwardTransition
;
528 static FAST_RAM_ZERO_INIT
float levelGain
, horizonGain
, horizonTransition
, horizonCutoffDegrees
, horizonFactorRatio
;
529 static FAST_RAM_ZERO_INIT
float itermWindupPointInv
;
530 static FAST_RAM_ZERO_INIT
uint8_t horizonTiltExpertMode
;
531 static FAST_RAM_ZERO_INIT timeDelta_t crashTimeLimitUs
;
532 static FAST_RAM_ZERO_INIT timeDelta_t crashTimeDelayUs
;
533 static FAST_RAM_ZERO_INIT
int32_t crashRecoveryAngleDeciDegrees
;
534 static FAST_RAM_ZERO_INIT
float crashRecoveryRate
;
535 static FAST_RAM_ZERO_INIT
float crashDtermThreshold
;
536 static FAST_RAM_ZERO_INIT
float crashGyroThreshold
;
537 static FAST_RAM_ZERO_INIT
float crashSetpointThreshold
;
538 static FAST_RAM_ZERO_INIT
float crashLimitYaw
;
539 static FAST_RAM_ZERO_INIT
float itermLimit
;
540 #if defined(USE_THROTTLE_BOOST)
541 FAST_RAM_ZERO_INIT
float throttleBoost
;
542 pt1Filter_t throttleLpf
;
544 static FAST_RAM_ZERO_INIT
bool itermRotation
;
546 #ifdef USE_LAUNCH_CONTROL
547 static FAST_RAM_ZERO_INIT
uint8_t launchControlMode
;
548 static FAST_RAM_ZERO_INIT
uint8_t launchControlAngleLimit
;
549 static FAST_RAM_ZERO_INIT
float launchControlKi
;
552 #ifdef USE_INTEGRATED_YAW_CONTROL
553 static FAST_RAM_ZERO_INIT
bool useIntegratedYaw
;
554 static FAST_RAM_ZERO_INIT
uint8_t integratedYawRelax
;
557 static FAST_RAM_ZERO_INIT
bool levelRaceMode
;
559 void pidResetIterm(void)
561 for (int axis
= 0; axis
< 3; axis
++) {
562 pidData
[axis
].I
= 0.0f
;
563 #if defined(USE_ABSOLUTE_CONTROL)
564 axisError
[axis
] = 0.0f
;
569 #ifdef USE_ACRO_TRAINER
570 static FAST_RAM_ZERO_INIT
float acroTrainerAngleLimit
;
571 static FAST_RAM_ZERO_INIT
float acroTrainerLookaheadTime
;
572 static FAST_RAM_ZERO_INIT
uint8_t acroTrainerDebugAxis
;
573 static FAST_RAM_ZERO_INIT
bool acroTrainerActive
;
574 static FAST_RAM_ZERO_INIT
int acroTrainerAxisState
[2]; // only need roll and pitch
575 static FAST_RAM_ZERO_INIT
float acroTrainerGain
;
576 #endif // USE_ACRO_TRAINER
578 #ifdef USE_THRUST_LINEARIZATION
579 FAST_RAM_ZERO_INIT
float thrustLinearization
;
580 FAST_RAM_ZERO_INIT
float thrustLinearizationReciprocal
;
581 FAST_RAM_ZERO_INIT
float thrustLinearizationB
;
584 void pidUpdateAntiGravityThrottleFilter(float throttle
)
586 if (antiGravityMode
== ANTI_GRAVITY_SMOOTH
) {
587 antiGravityThrottleHpf
= throttle
- pt1FilterApply(&antiGravityThrottleLpf
, throttle
);
592 static FAST_RAM
uint8_t dynLpfFilter
= DYN_LPF_NONE
;
593 static FAST_RAM_ZERO_INIT
uint16_t dynLpfMin
;
594 static FAST_RAM_ZERO_INIT
uint16_t dynLpfMax
;
595 static FAST_RAM_ZERO_INIT
uint8_t dynLpfCurveExpo
;
599 static FAST_RAM_ZERO_INIT
float dMinPercent
[XYZ_AXIS_COUNT
];
600 static FAST_RAM_ZERO_INIT
float dMinGyroGain
;
601 static FAST_RAM_ZERO_INIT
float dMinSetpointGain
;
604 #ifdef USE_INTERPOLATED_SP
605 static FAST_RAM_ZERO_INIT ffInterpolationType_t ffFromInterpolatedSetpoint
;
608 void pidInitConfig(const pidProfile_t
*pidProfile
)
610 if (pidProfile
->feedForwardTransition
== 0) {
611 feedForwardTransition
= 0;
613 feedForwardTransition
= 100.0f
/ pidProfile
->feedForwardTransition
;
615 for (int axis
= FD_ROLL
; axis
<= FD_YAW
; axis
++) {
616 pidCoefficient
[axis
].Kp
= PTERM_SCALE
* pidProfile
->pid
[axis
].P
;
617 pidCoefficient
[axis
].Ki
= ITERM_SCALE
* pidProfile
->pid
[axis
].I
;
618 pidCoefficient
[axis
].Kd
= DTERM_SCALE
* pidProfile
->pid
[axis
].D
;
619 pidCoefficient
[axis
].Kf
= FEEDFORWARD_SCALE
* (pidProfile
->pid
[axis
].F
/ 100.0f
);
621 #ifdef USE_INTEGRATED_YAW_CONTROL
622 if (!pidProfile
->use_integrated_yaw
)
625 pidCoefficient
[FD_YAW
].Ki
*= 2.5f
;
628 levelGain
= pidProfile
->pid
[PID_LEVEL
].P
/ 10.0f
;
629 horizonGain
= pidProfile
->pid
[PID_LEVEL
].I
/ 10.0f
;
630 horizonTransition
= (float)pidProfile
->pid
[PID_LEVEL
].D
;
631 horizonTiltExpertMode
= pidProfile
->horizon_tilt_expert_mode
;
632 horizonCutoffDegrees
= (175 - pidProfile
->horizon_tilt_effect
) * 1.8f
;
633 horizonFactorRatio
= (100 - pidProfile
->horizon_tilt_effect
) * 0.01f
;
634 maxVelocity
[FD_ROLL
] = maxVelocity
[FD_PITCH
] = pidProfile
->rateAccelLimit
* 100 * dT
;
635 maxVelocity
[FD_YAW
] = pidProfile
->yawRateAccelLimit
* 100 * dT
;
636 itermWindupPointInv
= 1.0f
;
637 if (pidProfile
->itermWindupPointPercent
< 100) {
638 const float itermWindupPoint
= pidProfile
->itermWindupPointPercent
/ 100.0f
;
639 itermWindupPointInv
= 1.0f
/ (1.0f
- itermWindupPoint
);
641 itermAcceleratorGain
= pidProfile
->itermAcceleratorGain
;
642 crashTimeLimitUs
= pidProfile
->crash_time
* 1000;
643 crashTimeDelayUs
= pidProfile
->crash_delay
* 1000;
644 crashRecoveryAngleDeciDegrees
= pidProfile
->crash_recovery_angle
* 10;
645 crashRecoveryRate
= pidProfile
->crash_recovery_rate
;
646 crashGyroThreshold
= pidProfile
->crash_gthreshold
;
647 crashDtermThreshold
= pidProfile
->crash_dthreshold
;
648 crashSetpointThreshold
= pidProfile
->crash_setpoint_threshold
;
649 crashLimitYaw
= pidProfile
->crash_limit_yaw
;
650 itermLimit
= pidProfile
->itermLimit
;
651 #if defined(USE_THROTTLE_BOOST)
652 throttleBoost
= pidProfile
->throttle_boost
* 0.1f
;
654 itermRotation
= pidProfile
->iterm_rotation
;
655 antiGravityMode
= pidProfile
->antiGravityMode
;
657 // Calculate the anti-gravity value that will trigger the OSD display.
658 // For classic AG it's either 1.0 for off and > 1.0 for on.
659 // For the new AG it's a continuous floating value so we want to trigger the OSD
660 // display when it exceeds 25% of its possible range. This gives a useful indication
661 // of AG activity without excessive display.
662 antiGravityOsdCutoff
= 0.0f
;
663 if (antiGravityMode
== ANTI_GRAVITY_SMOOTH
) {
664 antiGravityOsdCutoff
+= ((itermAcceleratorGain
- 1000) / 1000.0f
) * 0.25f
;
667 #if defined(USE_ITERM_RELAX)
668 itermRelax
= pidProfile
->iterm_relax
;
669 itermRelaxType
= pidProfile
->iterm_relax_type
;
670 itermRelaxCutoff
= pidProfile
->iterm_relax_cutoff
;
671 // adapt setpoint threshold to user changes from default cutoff value
672 itermRelaxSetpointThreshold
= ITERM_RELAX_SETPOINT_THRESHOLD
* ITERM_RELAX_CUTOFF_DEFAULT
/ itermRelaxCutoff
;
675 #ifdef USE_ACRO_TRAINER
676 acroTrainerAngleLimit
= pidProfile
->acro_trainer_angle_limit
;
677 acroTrainerLookaheadTime
= (float)pidProfile
->acro_trainer_lookahead_ms
/ 1000.0f
;
678 acroTrainerDebugAxis
= pidProfile
->acro_trainer_debug_axis
;
679 acroTrainerGain
= (float)pidProfile
->acro_trainer_gain
/ 10.0f
;
680 #endif // USE_ACRO_TRAINER
682 #if defined(USE_ABSOLUTE_CONTROL)
683 acGain
= (float)pidProfile
->abs_control_gain
;
684 acLimit
= (float)pidProfile
->abs_control_limit
;
685 acErrorLimit
= (float)pidProfile
->abs_control_error_limit
;
686 acCutoff
= (float)pidProfile
->abs_control_cutoff
;
687 for (int axis
= FD_ROLL
; axis
<= FD_YAW
; axis
++) {
688 float iCorrection
= -acGain
* PTERM_SCALE
/ ITERM_SCALE
* pidCoefficient
[axis
].Kp
;
689 pidCoefficient
[axis
].Ki
= MAX(0.0f
, pidCoefficient
[axis
].Ki
+ iCorrection
);
694 if (pidProfile
->dyn_lpf_dterm_min_hz
> 0) {
695 switch (pidProfile
->dterm_filter_type
) {
697 dynLpfFilter
= DYN_LPF_PT1
;
700 dynLpfFilter
= DYN_LPF_BIQUAD
;
703 dynLpfFilter
= DYN_LPF_NONE
;
707 dynLpfFilter
= DYN_LPF_NONE
;
709 dynLpfMin
= pidProfile
->dyn_lpf_dterm_min_hz
;
710 dynLpfMax
= pidProfile
->dyn_lpf_dterm_max_hz
;
711 dynLpfCurveExpo
= pidProfile
->dyn_lpf_curve_expo
;
714 #ifdef USE_LAUNCH_CONTROL
715 launchControlMode
= pidProfile
->launchControlMode
;
716 if (sensors(SENSOR_ACC
)) {
717 launchControlAngleLimit
= pidProfile
->launchControlAngleLimit
;
719 launchControlAngleLimit
= 0;
721 launchControlKi
= ITERM_SCALE
* pidProfile
->launchControlGain
;
724 #ifdef USE_INTEGRATED_YAW_CONTROL
725 useIntegratedYaw
= pidProfile
->use_integrated_yaw
;
726 integratedYawRelax
= pidProfile
->integrated_yaw_relax
;
729 #ifdef USE_THRUST_LINEARIZATION
730 thrustLinearization
= pidProfile
->thrustLinearization
/ 100.0f
;
731 if (thrustLinearization
!= 0.0f
) {
732 thrustLinearizationReciprocal
= 1.0f
/ thrustLinearization
;
733 thrustLinearizationB
= (1.0f
- thrustLinearization
) / (2.0f
* thrustLinearization
);
736 #if defined(USE_D_MIN)
737 for (int axis
= FD_ROLL
; axis
<= FD_YAW
; ++axis
) {
738 const uint8_t dMin
= pidProfile
->d_min
[axis
];
739 if ((dMin
> 0) && (dMin
< pidProfile
->pid
[axis
].D
)) {
740 dMinPercent
[axis
] = dMin
/ (float)(pidProfile
->pid
[axis
].D
);
742 dMinPercent
[axis
] = 0;
745 dMinGyroGain
= pidProfile
->d_min_gain
* D_MIN_GAIN_FACTOR
/ D_MIN_LOWPASS_HZ
;
746 dMinSetpointGain
= pidProfile
->d_min_gain
* D_MIN_SETPOINT_GAIN_FACTOR
* pidProfile
->d_min_advance
* pidFrequency
/ (100 * D_MIN_LOWPASS_HZ
);
747 // lowpass included inversely in gain since stronger lowpass decreases peak effect
749 #if defined(USE_AIRMODE_LPF)
750 airmodeThrottleOffsetLimit
= pidProfile
->transient_throttle_limit
/ 100.0f
;
752 #ifdef USE_INTERPOLATED_SP
753 ffFromInterpolatedSetpoint
= pidProfile
->ff_interpolate_sp
;
754 ffSmoothFactor
= 1.0f
- ((float)pidProfile
->ff_smooth_factor
) / 100.0f
;
755 interpolatedSpInit(pidProfile
);
758 levelRaceMode
= pidProfile
->level_race_mode
;
761 void pidInit(const pidProfile_t
*pidProfile
)
763 pidSetTargetLooptime(gyro
.targetLooptime
); // Initialize pid looptime
764 pidInitFilters(pidProfile
);
765 pidInitConfig(pidProfile
);
766 #ifdef USE_RPM_FILTER
767 rpmFilterInit(rpmFilterConfig());
771 #ifdef USE_ACRO_TRAINER
772 void pidAcroTrainerInit(void)
774 acroTrainerAxisState
[FD_ROLL
] = 0;
775 acroTrainerAxisState
[FD_PITCH
] = 0;
777 #endif // USE_ACRO_TRAINER
779 #ifdef USE_THRUST_LINEARIZATION
780 float pidCompensateThrustLinearization(float throttle
)
782 if (thrustLinearization
!= 0.0f
) {
783 throttle
= throttle
* (throttle
* thrustLinearization
+ 1.0f
- thrustLinearization
);
788 float pidApplyThrustLinearization(float motorOutput
)
790 if (thrustLinearization
!= 0.0f
) {
791 if (motorOutput
> 0.0f
) {
792 motorOutput
= sqrtf(motorOutput
* thrustLinearizationReciprocal
+
793 thrustLinearizationB
* thrustLinearizationB
) - thrustLinearizationB
;
800 void pidCopyProfile(uint8_t dstPidProfileIndex
, uint8_t srcPidProfileIndex
)
802 if (dstPidProfileIndex
< PID_PROFILE_COUNT
&& srcPidProfileIndex
< PID_PROFILE_COUNT
803 && dstPidProfileIndex
!= srcPidProfileIndex
) {
804 memcpy(pidProfilesMutable(dstPidProfileIndex
), pidProfilesMutable(srcPidProfileIndex
), sizeof(pidProfile_t
));
809 // calculates strength of horizon leveling; 0 = none, 1.0 = most leveling
810 STATIC_UNIT_TESTED
float calcHorizonLevelStrength(void)
812 // start with 1.0 at center stick, 0.0 at max stick deflection:
813 float horizonLevelStrength
= 1.0f
- MAX(getRcDeflectionAbs(FD_ROLL
), getRcDeflectionAbs(FD_PITCH
));
815 // 0 at level, 90 at vertical, 180 at inverted (degrees):
816 const float currentInclination
= MAX(ABS(attitude
.values
.roll
), ABS(attitude
.values
.pitch
)) / 10.0f
;
818 // horizonTiltExpertMode: 0 = leveling always active when sticks centered,
819 // 1 = leveling can be totally off when inverted
820 if (horizonTiltExpertMode
) {
821 if (horizonTransition
> 0 && horizonCutoffDegrees
> 0) {
822 // if d_level > 0 and horizonTiltEffect < 175
823 // horizonCutoffDegrees: 0 to 125 => 270 to 90 (represents where leveling goes to zero)
824 // inclinationLevelRatio (0.0 to 1.0) is smaller (less leveling)
825 // for larger inclinations; 0.0 at horizonCutoffDegrees value:
826 const float inclinationLevelRatio
= constrainf((horizonCutoffDegrees
-currentInclination
) / horizonCutoffDegrees
, 0, 1);
827 // apply configured horizon sensitivity:
828 // when stick is near center (horizonLevelStrength ~= 1.0)
829 // H_sensitivity value has little effect,
830 // when stick is deflected (horizonLevelStrength near 0.0)
831 // H_sensitivity value has more effect:
832 horizonLevelStrength
= (horizonLevelStrength
- 1) * 100 / horizonTransition
+ 1;
833 // apply inclination ratio, which may lower leveling
834 // to zero regardless of stick position:
835 horizonLevelStrength
*= inclinationLevelRatio
;
836 } else { // d_level=0 or horizon_tilt_effect>=175 means no leveling
837 horizonLevelStrength
= 0;
839 } else { // horizon_tilt_expert_mode = 0 (leveling always active when sticks centered)
841 if (horizonFactorRatio
< 1.01f
) { // if horizonTiltEffect > 0
842 // horizonFactorRatio: 1.0 to 0.0 (larger means more leveling)
843 // inclinationLevelRatio (0.0 to 1.0) is smaller (less leveling)
844 // for larger inclinations, goes to 1.0 at inclination==level:
845 const float inclinationLevelRatio
= (180-currentInclination
)/180 * (1.0f
-horizonFactorRatio
) + horizonFactorRatio
;
846 // apply ratio to configured horizon sensitivity:
847 sensitFact
= horizonTransition
* inclinationLevelRatio
;
848 } else { // horizonTiltEffect=0 for "old" functionality
849 sensitFact
= horizonTransition
;
852 if (sensitFact
<= 0) { // zero means no leveling
853 horizonLevelStrength
= 0;
855 // when stick is near center (horizonLevelStrength ~= 1.0)
856 // sensitFact value has little effect,
857 // when stick is deflected (horizonLevelStrength near 0.0)
858 // sensitFact value has more effect:
859 horizonLevelStrength
= ((horizonLevelStrength
- 1) * (100 / sensitFact
)) + 1;
862 return constrainf(horizonLevelStrength
, 0, 1);
865 // Use the FAST_CODE_NOINLINE directive to avoid this code from being inlined into ITCM RAM to avoid overflow.
866 // The impact is possibly slightly slower performance on F7/H7 but they have more than enough
867 // processing power that it should be a non-issue.
868 STATIC_UNIT_TESTED FAST_CODE_NOINLINE
float pidLevel(int axis
, const pidProfile_t
*pidProfile
, const rollAndPitchTrims_t
*angleTrim
, float currentPidSetpoint
) {
869 // calculate error angle and limit the angle to the max inclination
870 // rcDeflection is in range [-1.0, 1.0]
871 float angle
= pidProfile
->levelAngleLimit
* getRcDeflection(axis
);
872 #ifdef USE_GPS_RESCUE
873 angle
+= gpsRescueAngle
[axis
] / 100; // ANGLE IS IN CENTIDEGREES
875 angle
= constrainf(angle
, -pidProfile
->levelAngleLimit
, pidProfile
->levelAngleLimit
);
876 const float errorAngle
= angle
- ((attitude
.raw
[axis
] - angleTrim
->raw
[axis
]) / 10.0f
);
877 if (FLIGHT_MODE(ANGLE_MODE
) || FLIGHT_MODE(GPS_RESCUE_MODE
)) {
878 // ANGLE mode - control is angle based
879 currentPidSetpoint
= errorAngle
* levelGain
;
881 // HORIZON mode - mix of ANGLE and ACRO modes
882 // mix in errorAngle to currentPidSetpoint to add a little auto-level feel
883 const float horizonLevelStrength
= calcHorizonLevelStrength();
884 currentPidSetpoint
= currentPidSetpoint
+ (errorAngle
* horizonGain
* horizonLevelStrength
);
886 return currentPidSetpoint
;
889 static timeUs_t crashDetectedAtUs
;
891 static void handleCrashRecovery(
892 const pidCrashRecovery_e crash_recovery
, const rollAndPitchTrims_t
*angleTrim
,
893 const int axis
, const timeUs_t currentTimeUs
, const float gyroRate
, float *currentPidSetpoint
, float *errorRate
)
895 if (inCrashRecoveryMode
&& cmpTimeUs(currentTimeUs
, crashDetectedAtUs
) > crashTimeDelayUs
) {
896 if (crash_recovery
== PID_CRASH_RECOVERY_BEEP
) {
899 if (axis
== FD_YAW
) {
900 *errorRate
= constrainf(*errorRate
, -crashLimitYaw
, crashLimitYaw
);
902 // on roll and pitch axes calculate currentPidSetpoint and errorRate to level the aircraft to recover from crash
903 if (sensors(SENSOR_ACC
)) {
904 // errorAngle is deviation from horizontal
905 const float errorAngle
= -(attitude
.raw
[axis
] - angleTrim
->raw
[axis
]) / 10.0f
;
906 *currentPidSetpoint
= errorAngle
* levelGain
;
907 *errorRate
= *currentPidSetpoint
- gyroRate
;
910 // reset iterm, since accumulated error before crash is now meaningless
911 // and iterm windup during crash recovery can be extreme, especially on yaw axis
912 pidData
[axis
].I
= 0.0f
;
913 if (cmpTimeUs(currentTimeUs
, crashDetectedAtUs
) > crashTimeLimitUs
914 || (getMotorMixRange() < 1.0f
915 && fabsf(gyro
.gyroADCf
[FD_ROLL
]) < crashRecoveryRate
916 && fabsf(gyro
.gyroADCf
[FD_PITCH
]) < crashRecoveryRate
917 && fabsf(gyro
.gyroADCf
[FD_YAW
]) < crashRecoveryRate
)) {
918 if (sensors(SENSOR_ACC
)) {
919 // check aircraft nearly level
920 if (ABS(attitude
.raw
[FD_ROLL
] - angleTrim
->raw
[FD_ROLL
]) < crashRecoveryAngleDeciDegrees
921 && ABS(attitude
.raw
[FD_PITCH
] - angleTrim
->raw
[FD_PITCH
]) < crashRecoveryAngleDeciDegrees
) {
922 inCrashRecoveryMode
= false;
926 inCrashRecoveryMode
= false;
933 static void detectAndSetCrashRecovery(
934 const pidCrashRecovery_e crash_recovery
, const int axis
,
935 const timeUs_t currentTimeUs
, const float delta
, const float errorRate
)
937 // if crash recovery is on and accelerometer enabled and there is no gyro overflow, then check for a crash
938 // no point in trying to recover if the crash is so severe that the gyro overflows
939 if ((crash_recovery
|| FLIGHT_MODE(GPS_RESCUE_MODE
)) && !gyroOverflowDetected()) {
940 if (ARMING_FLAG(ARMED
)) {
941 if (getMotorMixRange() >= 1.0f
&& !inCrashRecoveryMode
942 && fabsf(delta
) > crashDtermThreshold
943 && fabsf(errorRate
) > crashGyroThreshold
944 && fabsf(getSetpointRate(axis
)) < crashSetpointThreshold
) {
945 if (crash_recovery
== PID_CRASH_RECOVERY_DISARM
) {
946 setArmingDisabled(ARMING_DISABLED_CRASH_DETECTED
);
947 disarm(DISARM_REASON_CRASH_PROTECTION
);
949 inCrashRecoveryMode
= true;
950 crashDetectedAtUs
= currentTimeUs
;
953 if (inCrashRecoveryMode
&& cmpTimeUs(currentTimeUs
, crashDetectedAtUs
) < crashTimeDelayUs
&& (fabsf(errorRate
) < crashGyroThreshold
954 || fabsf(getSetpointRate(axis
)) > crashSetpointThreshold
)) {
955 inCrashRecoveryMode
= false;
958 } else if (inCrashRecoveryMode
) {
959 inCrashRecoveryMode
= false;
966 #ifdef USE_ACRO_TRAINER
968 int acroTrainerSign(float x
)
970 return x
> 0 ? 1 : -1;
973 // Acro Trainer - Manipulate the setPoint to limit axis angle while in acro mode
974 // There are three states:
975 // 1. Current angle has exceeded limit
976 // Apply correction to return to limit (similar to pidLevel)
977 // 2. Future overflow has been projected based on current angle and gyro rate
978 // Manage the setPoint to control the gyro rate as the actual angle approaches the limit (try to prevent overshoot)
979 // 3. If no potential overflow is detected, then return the original setPoint
981 // Use the FAST_CODE_NOINLINE directive to avoid this code from being inlined into ITCM RAM. We accept the
982 // performance decrease when Acro Trainer mode is active under the assumption that user is unlikely to be
983 // expecting ultimate flight performance at very high loop rates when in this mode.
984 static FAST_CODE_NOINLINE
float applyAcroTrainer(int axis
, const rollAndPitchTrims_t
*angleTrim
, float setPoint
)
986 float ret
= setPoint
;
988 if (!FLIGHT_MODE(ANGLE_MODE
) && !FLIGHT_MODE(HORIZON_MODE
) && !FLIGHT_MODE(GPS_RESCUE_MODE
)) {
989 bool resetIterm
= false;
990 float projectedAngle
= 0;
991 const int setpointSign
= acroTrainerSign(setPoint
);
992 const float currentAngle
= (attitude
.raw
[axis
] - angleTrim
->raw
[axis
]) / 10.0f
;
993 const int angleSign
= acroTrainerSign(currentAngle
);
995 if ((acroTrainerAxisState
[axis
] != 0) && (acroTrainerAxisState
[axis
] != setpointSign
)) { // stick has reversed - stop limiting
996 acroTrainerAxisState
[axis
] = 0;
999 // Limit and correct the angle when it exceeds the limit
1000 if ((fabsf(currentAngle
) > acroTrainerAngleLimit
) && (acroTrainerAxisState
[axis
] == 0)) {
1001 if (angleSign
== setpointSign
) {
1002 acroTrainerAxisState
[axis
] = angleSign
;
1007 if (acroTrainerAxisState
[axis
] != 0) {
1008 ret
= constrainf(((acroTrainerAngleLimit
* angleSign
) - currentAngle
) * acroTrainerGain
, -ACRO_TRAINER_SETPOINT_LIMIT
, ACRO_TRAINER_SETPOINT_LIMIT
);
1011 // Not currently over the limit so project the angle based on current angle and
1012 // gyro angular rate using a sliding window based on gyro rate (faster rotation means larger window.
1013 // If the projected angle exceeds the limit then apply limiting to minimize overshoot.
1014 // Calculate the lookahead window by scaling proportionally with gyro rate from 0-500dps
1015 float checkInterval
= constrainf(fabsf(gyro
.gyroADCf
[axis
]) / ACRO_TRAINER_LOOKAHEAD_RATE_LIMIT
, 0.0f
, 1.0f
) * acroTrainerLookaheadTime
;
1016 projectedAngle
= (gyro
.gyroADCf
[axis
] * checkInterval
) + currentAngle
;
1017 const int projectedAngleSign
= acroTrainerSign(projectedAngle
);
1018 if ((fabsf(projectedAngle
) > acroTrainerAngleLimit
) && (projectedAngleSign
== setpointSign
)) {
1019 ret
= ((acroTrainerAngleLimit
* projectedAngleSign
) - projectedAngle
) * acroTrainerGain
;
1025 pidData
[axis
].I
= 0;
1028 if (axis
== acroTrainerDebugAxis
) {
1029 DEBUG_SET(DEBUG_ACRO_TRAINER
, 0, lrintf(currentAngle
* 10.0f
));
1030 DEBUG_SET(DEBUG_ACRO_TRAINER
, 1, acroTrainerAxisState
[axis
]);
1031 DEBUG_SET(DEBUG_ACRO_TRAINER
, 2, lrintf(ret
));
1032 DEBUG_SET(DEBUG_ACRO_TRAINER
, 3, lrintf(projectedAngle
* 10.0f
));
1038 #endif // USE_ACRO_TRAINER
1040 static float accelerationLimit(int axis
, float currentPidSetpoint
)
1042 static float previousSetpoint
[XYZ_AXIS_COUNT
];
1043 const float currentVelocity
= currentPidSetpoint
- previousSetpoint
[axis
];
1045 if (fabsf(currentVelocity
) > maxVelocity
[axis
]) {
1046 currentPidSetpoint
= (currentVelocity
> 0) ? previousSetpoint
[axis
] + maxVelocity
[axis
] : previousSetpoint
[axis
] - maxVelocity
[axis
];
1049 previousSetpoint
[axis
] = currentPidSetpoint
;
1050 return currentPidSetpoint
;
1053 static void rotateVector(float v
[XYZ_AXIS_COUNT
], float rotation
[XYZ_AXIS_COUNT
])
1055 // rotate v around rotation vector rotation
1056 // rotation in radians, all elements must be small
1057 for (int i
= 0; i
< XYZ_AXIS_COUNT
; i
++) {
1058 int i_1
= (i
+ 1) % 3;
1059 int i_2
= (i
+ 2) % 3;
1060 float newV
= v
[i_1
] + v
[i_2
] * rotation
[i
];
1061 v
[i_2
] -= v
[i_1
] * rotation
[i
];
1066 STATIC_UNIT_TESTED
void rotateItermAndAxisError()
1069 #if defined(USE_ABSOLUTE_CONTROL)
1070 || acGain
> 0 || debugMode
== DEBUG_AC_ERROR
1073 const float gyroToAngle
= dT
* RAD
;
1074 float rotationRads
[XYZ_AXIS_COUNT
];
1075 for (int i
= FD_ROLL
; i
<= FD_YAW
; i
++) {
1076 rotationRads
[i
] = gyro
.gyroADCf
[i
] * gyroToAngle
;
1078 #if defined(USE_ABSOLUTE_CONTROL)
1079 if (acGain
> 0 || debugMode
== DEBUG_AC_ERROR
) {
1080 rotateVector(axisError
, rotationRads
);
1083 if (itermRotation
) {
1084 float v
[XYZ_AXIS_COUNT
];
1085 for (int i
= 0; i
< XYZ_AXIS_COUNT
; i
++) {
1086 v
[i
] = pidData
[i
].I
;
1088 rotateVector(v
, rotationRads
);
1089 for (int i
= 0; i
< XYZ_AXIS_COUNT
; i
++) {
1090 pidData
[i
].I
= v
[i
];
1096 #ifdef USE_RC_SMOOTHING_FILTER
1097 float FAST_CODE
applyRcSmoothingDerivativeFilter(int axis
, float pidSetpointDelta
)
1099 float ret
= pidSetpointDelta
;
1100 if (axis
== rcSmoothingDebugAxis
) {
1101 DEBUG_SET(DEBUG_RC_SMOOTHING
, 1, lrintf(pidSetpointDelta
* 100.0f
));
1103 if (setpointDerivativeLpfInitialized
) {
1104 switch (rcSmoothingFilterType
) {
1105 case RC_SMOOTHING_DERIVATIVE_PT1
:
1106 ret
= pt1FilterApply(&setpointDerivativePt1
[axis
], pidSetpointDelta
);
1108 case RC_SMOOTHING_DERIVATIVE_BIQUAD
:
1109 ret
= biquadFilterApplyDF1(&setpointDerivativeBiquad
[axis
], pidSetpointDelta
);
1112 if (axis
== rcSmoothingDebugAxis
) {
1113 DEBUG_SET(DEBUG_RC_SMOOTHING
, 2, lrintf(ret
* 100.0f
));
1118 #endif // USE_RC_SMOOTHING_FILTER
1120 #if defined(USE_ITERM_RELAX)
1121 #if defined(USE_ABSOLUTE_CONTROL)
1122 STATIC_UNIT_TESTED
void applyAbsoluteControl(const int axis
, const float gyroRate
, float *currentPidSetpoint
, float *itermErrorRate
)
1124 if (acGain
> 0 || debugMode
== DEBUG_AC_ERROR
) {
1125 const float setpointLpf
= pt1FilterApply(&acLpf
[axis
], *currentPidSetpoint
);
1126 const float setpointHpf
= fabsf(*currentPidSetpoint
- setpointLpf
);
1127 float acErrorRate
= 0;
1128 const float gmaxac
= setpointLpf
+ 2 * setpointHpf
;
1129 const float gminac
= setpointLpf
- 2 * setpointHpf
;
1130 if (gyroRate
>= gminac
&& gyroRate
<= gmaxac
) {
1131 const float acErrorRate1
= gmaxac
- gyroRate
;
1132 const float acErrorRate2
= gminac
- gyroRate
;
1133 if (acErrorRate1
* axisError
[axis
] < 0) {
1134 acErrorRate
= acErrorRate1
;
1136 acErrorRate
= acErrorRate2
;
1138 if (fabsf(acErrorRate
* dT
) > fabsf(axisError
[axis
]) ) {
1139 acErrorRate
= -axisError
[axis
] * pidFrequency
;
1142 acErrorRate
= (gyroRate
> gmaxac
? gmaxac
: gminac
) - gyroRate
;
1145 if (isAirmodeActivated()) {
1146 axisError
[axis
] = constrainf(axisError
[axis
] + acErrorRate
* dT
,
1147 -acErrorLimit
, acErrorLimit
);
1148 const float acCorrection
= constrainf(axisError
[axis
] * acGain
, -acLimit
, acLimit
);
1149 *currentPidSetpoint
+= acCorrection
;
1150 *itermErrorRate
+= acCorrection
;
1151 DEBUG_SET(DEBUG_AC_CORRECTION
, axis
, lrintf(acCorrection
* 10));
1152 if (axis
== FD_ROLL
) {
1153 DEBUG_SET(DEBUG_ITERM_RELAX
, 3, lrintf(acCorrection
* 10));
1156 DEBUG_SET(DEBUG_AC_ERROR
, axis
, lrintf(axisError
[axis
] * 10));
1161 STATIC_UNIT_TESTED
void applyItermRelax(const int axis
, const float iterm
,
1162 const float gyroRate
, float *itermErrorRate
, float *currentPidSetpoint
)
1164 const float setpointLpf
= pt1FilterApply(&windupLpf
[axis
], *currentPidSetpoint
);
1165 const float setpointHpf
= fabsf(*currentPidSetpoint
- setpointLpf
);
1168 if (axis
< FD_YAW
|| itermRelax
== ITERM_RELAX_RPY
|| itermRelax
== ITERM_RELAX_RPY_INC
) {
1169 const float itermRelaxFactor
= MAX(0, 1 - setpointHpf
/ itermRelaxSetpointThreshold
);
1170 const bool isDecreasingI
=
1171 ((iterm
> 0) && (*itermErrorRate
< 0)) || ((iterm
< 0) && (*itermErrorRate
> 0));
1172 if ((itermRelax
>= ITERM_RELAX_RP_INC
) && isDecreasingI
) {
1173 // Do Nothing, use the precalculed itermErrorRate
1174 } else if (itermRelaxType
== ITERM_RELAX_SETPOINT
) {
1175 *itermErrorRate
*= itermRelaxFactor
;
1176 } else if (itermRelaxType
== ITERM_RELAX_GYRO
) {
1177 *itermErrorRate
= fapplyDeadband(setpointLpf
- gyroRate
, setpointHpf
);
1179 *itermErrorRate
= 0.0f
;
1182 if (axis
== FD_ROLL
) {
1183 DEBUG_SET(DEBUG_ITERM_RELAX
, 0, lrintf(setpointHpf
));
1184 DEBUG_SET(DEBUG_ITERM_RELAX
, 1, lrintf(itermRelaxFactor
* 100.0f
));
1185 DEBUG_SET(DEBUG_ITERM_RELAX
, 2, lrintf(*itermErrorRate
));
1189 #if defined(USE_ABSOLUTE_CONTROL)
1190 applyAbsoluteControl(axis
, gyroRate
, currentPidSetpoint
, itermErrorRate
);
1196 #ifdef USE_AIRMODE_LPF
1197 void pidUpdateAirmodeLpf(float currentOffset
)
1199 if (airmodeThrottleOffsetLimit
== 0.0f
) {
1203 float offsetHpf
= currentOffset
* 2.5f
;
1204 offsetHpf
= offsetHpf
- pt1FilterApply(&airmodeThrottleLpf2
, offsetHpf
);
1206 // During high frequency oscillation 2 * currentOffset averages to the offset required to avoid mirroring of the waveform
1207 pt1FilterApply(&airmodeThrottleLpf1
, offsetHpf
);
1208 // Bring offset up immediately so the filter only applies to the decline
1209 if (currentOffset
* airmodeThrottleLpf1
.state
>= 0 && fabsf(currentOffset
) > airmodeThrottleLpf1
.state
) {
1210 airmodeThrottleLpf1
.state
= currentOffset
;
1212 airmodeThrottleLpf1
.state
= constrainf(airmodeThrottleLpf1
.state
, -airmodeThrottleOffsetLimit
, airmodeThrottleOffsetLimit
);
1215 float pidGetAirmodeThrottleOffset()
1217 return airmodeThrottleLpf1
.state
;
1221 #ifdef USE_LAUNCH_CONTROL
1222 #define LAUNCH_CONTROL_MAX_RATE 100.0f
1223 #define LAUNCH_CONTROL_MIN_RATE 5.0f
1224 #define LAUNCH_CONTROL_ANGLE_WINDOW 10.0f // The remaining angle degrees where rate dampening starts
1226 // Use the FAST_CODE_NOINLINE directive to avoid this code from being inlined into ITCM RAM to avoid overflow.
1227 // The impact is possibly slightly slower performance on F7/H7 but they have more than enough
1228 // processing power that it should be a non-issue.
1229 static FAST_CODE_NOINLINE
float applyLaunchControl(int axis
, const rollAndPitchTrims_t
*angleTrim
)
1233 // Scale the rates based on stick deflection only. Fixed rates with a max of 100deg/sec
1234 // reached at 50% stick deflection. This keeps the launch control positioning consistent
1235 // regardless of the user's rates.
1236 if ((axis
== FD_PITCH
) || (launchControlMode
!= LAUNCH_CONTROL_MODE_PITCHONLY
)) {
1237 const float stickDeflection
= constrainf(getRcDeflection(axis
), -0.5f
, 0.5f
);
1238 ret
= LAUNCH_CONTROL_MAX_RATE
* stickDeflection
* 2;
1241 #if defined(USE_ACC)
1242 // If ACC is enabled and a limit angle is set, then try to limit forward tilt
1243 // to that angle and slow down the rate as the limit is approached to reduce overshoot
1244 if ((axis
== FD_PITCH
) && (launchControlAngleLimit
> 0) && (ret
> 0)) {
1245 const float currentAngle
= (attitude
.raw
[axis
] - angleTrim
->raw
[axis
]) / 10.0f
;
1246 if (currentAngle
>= launchControlAngleLimit
) {
1249 //for the last 10 degrees scale the rate from the current input to 5 dps
1250 const float angleDelta
= launchControlAngleLimit
- currentAngle
;
1251 if (angleDelta
<= LAUNCH_CONTROL_ANGLE_WINDOW
) {
1252 ret
= scaleRangef(angleDelta
, 0, LAUNCH_CONTROL_ANGLE_WINDOW
, LAUNCH_CONTROL_MIN_RATE
, ret
);
1264 // Betaflight pid controller, which will be maintained in the future with additional features specialised for current (mini) multirotor usage.
1265 // Based on 2DOF reference design (matlab)
1266 void FAST_CODE
pidController(const pidProfile_t
*pidProfile
, timeUs_t currentTimeUs
)
1268 static float previousGyroRateDterm
[XYZ_AXIS_COUNT
];
1269 #ifdef USE_INTERPOLATED_SP
1270 static FAST_RAM_ZERO_INIT
uint32_t lastFrameNumber
;
1273 #if defined(USE_ACC)
1274 static timeUs_t levelModeStartTimeUs
= 0;
1275 static bool gpsRescuePreviousState
= false;
1278 const float tpaFactor
= getThrottlePIDAttenuation();
1280 #if defined(USE_ACC)
1281 const rollAndPitchTrims_t
*angleTrim
= &accelerometerConfig()->accelerometerTrims
;
1284 UNUSED(currentTimeUs
);
1288 const float tpaFactorKp
= (currentControlRateProfile
->tpaMode
== TPA_MODE_PD
) ? tpaFactor
: 1.0f
;
1290 const float tpaFactorKp
= tpaFactor
;
1293 #ifdef USE_YAW_SPIN_RECOVERY
1294 const bool yawSpinActive
= gyroYawSpinDetected();
1297 const bool launchControlActive
= isLaunchControlActive();
1299 #if defined(USE_ACC)
1300 const bool gpsRescueIsActive
= FLIGHT_MODE(GPS_RESCUE_MODE
);
1301 const bool levelModeActive
= FLIGHT_MODE(ANGLE_MODE
) || FLIGHT_MODE(HORIZON_MODE
) || gpsRescueIsActive
;
1302 const bool levelRaceModeActive
= levelRaceMode
&& (FLIGHT_MODE(ANGLE_MODE
) || FLIGHT_MODE(HORIZON_MODE
)) && !gpsRescueIsActive
;
1304 // Keep track of when we entered a self-level mode so that we can
1305 // add a guard time before crash recovery can activate.
1306 // Also reset the guard time whenever GPS Rescue is activated.
1307 if (levelModeActive
) {
1308 if ((levelModeStartTimeUs
== 0) || (gpsRescueIsActive
&& !gpsRescuePreviousState
)) {
1309 levelModeStartTimeUs
= currentTimeUs
;
1312 levelModeStartTimeUs
= 0;
1314 gpsRescuePreviousState
= gpsRescueIsActive
;
1317 // Dynamic i component,
1318 if ((antiGravityMode
== ANTI_GRAVITY_SMOOTH
) && antiGravityEnabled
) {
1319 itermAccelerator
= fabsf(antiGravityThrottleHpf
) * 0.01f
* (itermAcceleratorGain
- 1000);
1320 DEBUG_SET(DEBUG_ANTI_GRAVITY
, 1, lrintf(antiGravityThrottleHpf
* 1000));
1322 DEBUG_SET(DEBUG_ANTI_GRAVITY
, 0, lrintf(itermAccelerator
* 1000));
1324 float agGain
= dT
* itermAccelerator
* AG_KI
;
1326 // gradually scale back integration when above windup point
1328 if (itermWindupPointInv
> 1.0f
) {
1329 dynCi
*= constrainf((1.0f
- getMotorMixRange()) * itermWindupPointInv
, 0.0f
, 1.0f
);
1332 // Precalculate gyro deta for D-term here, this allows loop unrolling
1333 float gyroRateDterm
[XYZ_AXIS_COUNT
];
1334 for (int axis
= FD_ROLL
; axis
<= FD_YAW
; ++axis
) {
1335 gyroRateDterm
[axis
] = gyro
.gyroADCf
[axis
];
1336 #ifdef USE_RPM_FILTER
1337 gyroRateDterm
[axis
] = rpmFilterDterm(axis
,gyroRateDterm
[axis
]);
1339 gyroRateDterm
[axis
] = dtermNotchApplyFn((filter_t
*) &dtermNotch
[axis
], gyroRateDterm
[axis
]);
1340 gyroRateDterm
[axis
] = dtermLowpassApplyFn((filter_t
*) &dtermLowpass
[axis
], gyroRateDterm
[axis
]);
1341 gyroRateDterm
[axis
] = dtermLowpass2ApplyFn((filter_t
*) &dtermLowpass2
[axis
], gyroRateDterm
[axis
]);
1344 rotateItermAndAxisError();
1345 #ifdef USE_RPM_FILTER
1349 #ifdef USE_INTERPOLATED_SP
1350 bool newRcFrame
= false;
1351 if (lastFrameNumber
!= getRcFrameNumber()) {
1352 lastFrameNumber
= getRcFrameNumber();
1357 // ----------PID controller----------
1358 for (int axis
= FD_ROLL
; axis
<= FD_YAW
; ++axis
) {
1360 float currentPidSetpoint
= getSetpointRate(axis
);
1361 if (maxVelocity
[axis
]) {
1362 currentPidSetpoint
= accelerationLimit(axis
, currentPidSetpoint
);
1364 // Yaw control is GYRO based, direct sticks control is applied to rate PID
1365 // When Race Mode is active PITCH control is also GYRO based in level or horizon mode
1366 #if defined(USE_ACC)
1367 if (levelModeActive
&& (axis
!= FD_YAW
) && !(levelRaceModeActive
&& (axis
== FD_PITCH
))) {
1368 currentPidSetpoint
= pidLevel(axis
, pidProfile
, angleTrim
, currentPidSetpoint
);
1372 #ifdef USE_ACRO_TRAINER
1373 if ((axis
!= FD_YAW
) && acroTrainerActive
&& !inCrashRecoveryMode
&& !launchControlActive
) {
1374 currentPidSetpoint
= applyAcroTrainer(axis
, angleTrim
, currentPidSetpoint
);
1376 #endif // USE_ACRO_TRAINER
1378 #ifdef USE_LAUNCH_CONTROL
1379 if (launchControlActive
) {
1380 #if defined(USE_ACC)
1381 currentPidSetpoint
= applyLaunchControl(axis
, angleTrim
);
1383 currentPidSetpoint
= applyLaunchControl(axis
, NULL
);
1388 // Handle yaw spin recovery - zero the setpoint on yaw to aid in recovery
1389 // It's not necessary to zero the set points for R/P because the PIDs will be zeroed below
1390 #ifdef USE_YAW_SPIN_RECOVERY
1391 if ((axis
== FD_YAW
) && yawSpinActive
) {
1392 currentPidSetpoint
= 0.0f
;
1394 #endif // USE_YAW_SPIN_RECOVERY
1396 // -----calculate error rate
1397 const float gyroRate
= gyro
.gyroADCf
[axis
]; // Process variable from gyro output in deg/sec
1398 float errorRate
= currentPidSetpoint
- gyroRate
; // r - y
1399 #if defined(USE_ACC)
1400 handleCrashRecovery(
1401 pidProfile
->crash_recovery
, angleTrim
, axis
, currentTimeUs
, gyroRate
,
1402 ¤tPidSetpoint
, &errorRate
);
1405 const float previousIterm
= pidData
[axis
].I
;
1406 float itermErrorRate
= errorRate
;
1407 #ifdef USE_ABSOLUTE_CONTROL
1408 float uncorrectedSetpoint
= currentPidSetpoint
;
1411 #if defined(USE_ITERM_RELAX)
1412 if (!launchControlActive
&& !inCrashRecoveryMode
) {
1413 applyItermRelax(axis
, previousIterm
, gyroRate
, &itermErrorRate
, ¤tPidSetpoint
);
1414 errorRate
= currentPidSetpoint
- gyroRate
;
1417 #ifdef USE_ABSOLUTE_CONTROL
1418 float setpointCorrection
= currentPidSetpoint
- uncorrectedSetpoint
;
1421 // --------low-level gyro-based PID based on 2DOF PID controller. ----------
1422 // 2-DOF PID controller with optional filter on derivative term.
1423 // b = 1 and only c (feedforward weight) can be tuned (amount derivative on measurement or error).
1425 // -----calculate P component
1426 pidData
[axis
].P
= pidCoefficient
[axis
].Kp
* errorRate
* tpaFactorKp
;
1427 if (axis
== FD_YAW
) {
1428 pidData
[axis
].P
= ptermYawLowpassApplyFn((filter_t
*) &ptermYawLowpass
, pidData
[axis
].P
);
1431 // -----calculate I component
1432 #ifdef USE_LAUNCH_CONTROL
1433 // if launch control is active override the iterm gains
1434 const float Ki
= launchControlActive
? launchControlKi
: pidCoefficient
[axis
].Ki
;
1436 const float Ki
= pidCoefficient
[axis
].Ki
;
1438 pidData
[axis
].I
= constrainf(previousIterm
+ (Ki
* dynCi
+ agGain
) * itermErrorRate
, -itermLimit
, itermLimit
);
1440 // -----calculate pidSetpointDelta
1441 float pidSetpointDelta
= 0;
1442 #ifdef USE_INTERPOLATED_SP
1443 if (ffFromInterpolatedSetpoint
) {
1444 pidSetpointDelta
= interpolatedSpApply(axis
, newRcFrame
, ffFromInterpolatedSetpoint
);
1446 pidSetpointDelta
= currentPidSetpoint
- previousPidSetpoint
[axis
];
1449 pidSetpointDelta
= currentPidSetpoint
- previousPidSetpoint
[axis
];
1451 previousPidSetpoint
[axis
] = currentPidSetpoint
;
1454 #ifdef USE_RC_SMOOTHING_FILTER
1455 pidSetpointDelta
= applyRcSmoothingDerivativeFilter(axis
, pidSetpointDelta
);
1456 #endif // USE_RC_SMOOTHING_FILTER
1458 // -----calculate D component
1459 // disable D if launch control is active
1460 if ((pidCoefficient
[axis
].Kd
> 0) && !launchControlActive
) {
1462 // Divide rate change by dT to get differential (ie dr/dt).
1463 // dT is fixed and calculated from the target PID loop time
1464 // This is done to avoid DTerm spikes that occur with dynamically
1465 // calculated deltaT whenever another task causes the PID
1466 // loop execution to be delayed.
1468 - (gyroRateDterm
[axis
] - previousGyroRateDterm
[axis
]) * pidFrequency
;
1470 #if defined(USE_ACC)
1471 if (cmpTimeUs(currentTimeUs
, levelModeStartTimeUs
) > CRASH_RECOVERY_DETECTION_DELAY_US
) {
1472 detectAndSetCrashRecovery(pidProfile
->crash_recovery
, axis
, currentTimeUs
, delta
, errorRate
);
1476 float dMinFactor
= 1.0f
;
1477 #if defined(USE_D_MIN)
1478 if (dMinPercent
[axis
] > 0) {
1479 float dMinGyroFactor
= biquadFilterApply(&dMinRange
[axis
], delta
);
1480 dMinGyroFactor
= fabsf(dMinGyroFactor
) * dMinGyroGain
;
1481 const float dMinSetpointFactor
= (fabsf(pidSetpointDelta
)) * dMinSetpointGain
;
1482 dMinFactor
= MAX(dMinGyroFactor
, dMinSetpointFactor
);
1483 dMinFactor
= dMinPercent
[axis
] + (1.0f
- dMinPercent
[axis
]) * dMinFactor
;
1484 dMinFactor
= pt1FilterApply(&dMinLowpass
[axis
], dMinFactor
);
1485 dMinFactor
= MIN(dMinFactor
, 1.0f
);
1486 if (axis
== FD_ROLL
) {
1487 DEBUG_SET(DEBUG_D_MIN
, 0, lrintf(dMinGyroFactor
* 100));
1488 DEBUG_SET(DEBUG_D_MIN
, 1, lrintf(dMinSetpointFactor
* 100));
1489 DEBUG_SET(DEBUG_D_MIN
, 2, lrintf(pidCoefficient
[axis
].Kd
* dMinFactor
* 10 / DTERM_SCALE
));
1490 } else if (axis
== FD_PITCH
) {
1491 DEBUG_SET(DEBUG_D_MIN
, 3, lrintf(pidCoefficient
[axis
].Kd
* dMinFactor
* 10 / DTERM_SCALE
));
1495 pidData
[axis
].D
= pidCoefficient
[axis
].Kd
* delta
* tpaFactor
* dMinFactor
;
1497 pidData
[axis
].D
= 0;
1499 previousGyroRateDterm
[axis
] = gyroRateDterm
[axis
];
1501 // -----calculate feedforward component
1502 #ifdef USE_ABSOLUTE_CONTROL
1503 // include abs control correction in FF
1504 pidSetpointDelta
+= setpointCorrection
- oldSetpointCorrection
[axis
];
1505 oldSetpointCorrection
[axis
] = setpointCorrection
;
1508 // Only enable feedforward for rate mode and if launch control is inactive
1509 const float feedforwardGain
= (flightModeFlags
|| launchControlActive
) ? 0.0f
: pidCoefficient
[axis
].Kf
;
1510 if (feedforwardGain
> 0) {
1511 // no transition if feedForwardTransition == 0
1512 float transition
= feedForwardTransition
> 0 ? MIN(1.f
, getRcDeflectionAbs(axis
) * feedForwardTransition
) : 1;
1513 float feedForward
= feedforwardGain
* transition
* pidSetpointDelta
* pidFrequency
;
1515 #ifdef USE_INTERPOLATED_SP
1516 pidData
[axis
].F
= shouldApplyFfLimits(axis
) ?
1517 applyFfLimit(axis
, feedForward
, pidCoefficient
[axis
].Kp
, currentPidSetpoint
) : feedForward
;
1519 pidData
[axis
].F
= feedForward
;
1522 pidData
[axis
].F
= 0;
1525 #ifdef USE_YAW_SPIN_RECOVERY
1526 if (yawSpinActive
) {
1527 pidData
[axis
].I
= 0; // in yaw spin always disable I
1528 if (axis
<= FD_PITCH
) {
1529 // zero PIDs on pitch and roll leaving yaw P to correct spin
1530 pidData
[axis
].P
= 0;
1531 pidData
[axis
].D
= 0;
1532 pidData
[axis
].F
= 0;
1535 #endif // USE_YAW_SPIN_RECOVERY
1537 #ifdef USE_LAUNCH_CONTROL
1538 // Disable P/I appropriately based on the launch control mode
1539 if (launchControlActive
) {
1540 // if not using FULL mode then disable I accumulation on yaw as
1541 // yaw has a tendency to windup. Otherwise limit yaw iterm accumulation.
1542 const int launchControlYawItermLimit
= (launchControlMode
== LAUNCH_CONTROL_MODE_FULL
) ? LAUNCH_CONTROL_YAW_ITERM_LIMIT
: 0;
1543 pidData
[FD_YAW
].I
= constrainf(pidData
[FD_YAW
].I
, -launchControlYawItermLimit
, launchControlYawItermLimit
);
1545 // for pitch-only mode we disable everything except pitch P/I
1546 if (launchControlMode
== LAUNCH_CONTROL_MODE_PITCHONLY
) {
1547 pidData
[FD_ROLL
].P
= 0;
1548 pidData
[FD_ROLL
].I
= 0;
1549 pidData
[FD_YAW
].P
= 0;
1550 // don't let I go negative (pitch backwards) as front motors are limited in the mixer
1551 pidData
[FD_PITCH
].I
= MAX(0.0f
, pidData
[FD_PITCH
].I
);
1555 // calculating the PID sum
1556 const float pidSum
= pidData
[axis
].P
+ pidData
[axis
].I
+ pidData
[axis
].D
+ pidData
[axis
].F
;
1557 #ifdef USE_INTEGRATED_YAW_CONTROL
1558 if (axis
== FD_YAW
&& useIntegratedYaw
) {
1559 pidData
[axis
].Sum
+= pidSum
* dT
* 100.0f
;
1560 pidData
[axis
].Sum
-= pidData
[axis
].Sum
* integratedYawRelax
/ 100000.0f
* dT
/ 0.000125f
;
1564 pidData
[axis
].Sum
= pidSum
;
1568 // Disable PID control if at zero throttle or if gyro overflow detected
1569 // This may look very innefficient, but it is done on purpose to always show real CPU usage as in flight
1570 if (!pidStabilisationEnabled
|| gyroOverflowDetected()) {
1571 for (int axis
= FD_ROLL
; axis
<= FD_YAW
; ++axis
) {
1572 pidData
[axis
].P
= 0;
1573 pidData
[axis
].I
= 0;
1574 pidData
[axis
].D
= 0;
1575 pidData
[axis
].F
= 0;
1577 pidData
[axis
].Sum
= 0;
1579 } else if (zeroThrottleItermReset
) {
1584 bool crashRecoveryModeActive(void)
1586 return inCrashRecoveryMode
;
1589 #ifdef USE_ACRO_TRAINER
1590 void pidSetAcroTrainerState(bool newState
)
1592 if (acroTrainerActive
!= newState
) {
1594 pidAcroTrainerInit();
1596 acroTrainerActive
= newState
;
1599 #endif // USE_ACRO_TRAINER
1601 void pidSetAntiGravityState(bool newState
)
1603 if (newState
!= antiGravityEnabled
) {
1604 // reset the accelerator on state changes
1605 itermAccelerator
= 0.0f
;
1607 antiGravityEnabled
= newState
;
1610 bool pidAntiGravityEnabled(void)
1612 return antiGravityEnabled
;
1616 void dynLpfDTermUpdate(float throttle
)
1618 static unsigned int cutoffFreq
;
1619 if (dynLpfFilter
!= DYN_LPF_NONE
) {
1620 if (dynLpfCurveExpo
> 0) {
1621 cutoffFreq
= dynDtermLpfCutoffFreq(throttle
, dynLpfMin
, dynLpfMax
, dynLpfCurveExpo
);
1623 cutoffFreq
= fmax(dynThrottle(throttle
) * dynLpfMax
, dynLpfMin
);
1626 if (dynLpfFilter
== DYN_LPF_PT1
) {
1627 for (int axis
= 0; axis
< XYZ_AXIS_COUNT
; axis
++) {
1628 pt1FilterUpdateCutoff(&dtermLowpass
[axis
].pt1Filter
, pt1FilterGain(cutoffFreq
, dT
));
1630 } else if (dynLpfFilter
== DYN_LPF_BIQUAD
) {
1631 for (int axis
= 0; axis
< XYZ_AXIS_COUNT
; axis
++) {
1632 biquadFilterUpdateLPF(&dtermLowpass
[axis
].biquadFilter
, cutoffFreq
, targetPidLooptime
);
1639 float dynDtermLpfCutoffFreq(float throttle
, uint16_t dynLpfMin
, uint16_t dynLpfMax
, uint8_t expo
) {
1640 const float expof
= expo
/ 10.0f
;
1642 curve
= 2 * throttle
* (1 - throttle
) * expof
+ powerf(throttle
, 2);
1643 return (dynLpfMax
- dynLpfMin
) * curve
+ dynLpfMin
;
1646 void pidSetItermReset(bool enabled
)
1648 zeroThrottleItermReset
= enabled
;
1651 float pidGetPreviousSetpoint(int axis
)
1653 return previousPidSetpoint
[axis
];
1661 float pidGetPidFrequency()
1663 return pidFrequency
;