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/>.
29 #include "build/build_config.h"
30 #include "build/debug.h"
32 #include "common/axis.h"
33 #include "common/filter.h"
35 #include "config/config.h"
36 #include "config/config_reset.h"
37 #include "config/simplified_tuning.h"
39 #include "drivers/pwm_output.h"
40 #include "drivers/sound_beeper.h"
41 #include "drivers/time.h"
43 #include "fc/controlrate_profile.h"
46 #include "fc/rc_controls.h"
47 #include "fc/runtime_config.h"
49 #include "flight/gps_rescue.h"
50 #include "flight/imu.h"
51 #include "flight/mixer.h"
52 #include "flight/rpm_filter.h"
53 #include "flight/feedforward.h"
58 #include "pg/pg_ids.h"
60 #include "sensors/acceleration.h"
61 #include "sensors/battery.h"
62 #include "sensors/gyro.h"
72 const char pidNames
[] =
79 FAST_DATA_ZERO_INIT
uint32_t targetPidLooptime
;
80 FAST_DATA_ZERO_INIT pidAxisData_t pidData
[XYZ_AXIS_COUNT
];
81 FAST_DATA_ZERO_INIT pidRuntime_t pidRuntime
;
83 #if defined(USE_ABSOLUTE_CONTROL)
84 STATIC_UNIT_TESTED FAST_DATA_ZERO_INIT
float axisError
[XYZ_AXIS_COUNT
];
87 #if defined(USE_THROTTLE_BOOST)
88 FAST_DATA_ZERO_INIT
float throttleBoost
;
89 pt1Filter_t throttleLpf
;
92 PG_REGISTER_WITH_RESET_TEMPLATE(pidConfig_t
, pidConfig
, PG_PID_CONFIG
, 3);
94 #if defined(STM32F411xE)
95 #define PID_PROCESS_DENOM_DEFAULT 2
97 #define PID_PROCESS_DENOM_DEFAULT 1
100 #ifdef USE_RUNAWAY_TAKEOFF
101 PG_RESET_TEMPLATE(pidConfig_t
, pidConfig
,
102 .pid_process_denom
= PID_PROCESS_DENOM_DEFAULT
,
103 .runaway_takeoff_prevention
= true,
104 .runaway_takeoff_deactivate_throttle
= 20, // throttle level % needed to accumulate deactivation time
105 .runaway_takeoff_deactivate_delay
= 500, // Accumulated time (in milliseconds) before deactivation in successful takeoff
108 PG_RESET_TEMPLATE(pidConfig_t
, pidConfig
,
109 .pid_process_denom
= PID_PROCESS_DENOM_DEFAULT
,
113 #ifdef USE_ACRO_TRAINER
114 #define ACRO_TRAINER_LOOKAHEAD_RATE_LIMIT 500.0f // Max gyro rate for lookahead time scaling
115 #define ACRO_TRAINER_SETPOINT_LIMIT 1000.0f // Limit the correcting setpoint
116 #endif // USE_ACRO_TRAINER
118 #define CRASH_RECOVERY_DETECTION_DELAY_US 1000000 // 1 second delay before crash recovery detection is active after entering a self-level mode
120 #define LAUNCH_CONTROL_YAW_ITERM_LIMIT 50 // yaw iterm windup limit when launch mode is "FULL" (all axes)
122 PG_REGISTER_ARRAY_WITH_RESET_FN(pidProfile_t
, PID_PROFILE_COUNT
, pidProfiles
, PG_PID_PROFILE
, 6);
124 void resetPidProfile(pidProfile_t
*pidProfile
)
126 RESET_CONFIG(pidProfile_t
, pidProfile
,
128 [PID_ROLL
] = PID_ROLL_DEFAULT
,
129 [PID_PITCH
] = PID_PITCH_DEFAULT
,
130 [PID_YAW
] = PID_YAW_DEFAULT
,
131 [PID_LEVEL
] = { 50, 50, 75, 0 },
132 [PID_MAG
] = { 40, 0, 0, 0 },
134 .pidSumLimit
= PIDSUM_LIMIT
,
135 .pidSumLimitYaw
= PIDSUM_LIMIT_YAW
,
136 .yaw_lowpass_hz
= 100,
138 .dterm_notch_cutoff
= 0,
139 .itermWindupPointPercent
= 85,
140 .pidAtMinThrottle
= PID_STABILISATION_ON
,
141 .levelAngleLimit
= 55,
142 .feedforward_transition
= 0,
143 .yawRateAccelLimit
= 0,
145 .anti_gravity_gain
= 80,
146 .crash_time
= 500, // ms
147 .crash_delay
= 0, // ms
148 .crash_recovery_angle
= 10, // degrees
149 .crash_recovery_rate
= 100, // degrees/second
150 .crash_dthreshold
= 50, // degrees/second/second
151 .crash_gthreshold
= 400, // degrees/second
152 .crash_setpoint_threshold
= 350, // degrees/second
153 .crash_recovery
= PID_CRASH_RECOVERY_OFF
, // off by default
154 .horizon_tilt_effect
= 75,
155 .horizon_tilt_expert_mode
= false,
156 .crash_limit_yaw
= 200,
159 .throttle_boost_cutoff
= 15,
160 .iterm_rotation
= false,
161 .iterm_relax
= ITERM_RELAX_RP
,
162 .iterm_relax_cutoff
= ITERM_RELAX_CUTOFF_DEFAULT
,
163 .iterm_relax_type
= ITERM_RELAX_SETPOINT
,
164 .acro_trainer_angle_limit
= 20,
165 .acro_trainer_lookahead_ms
= 50,
166 .acro_trainer_debug_axis
= FD_ROLL
,
167 .acro_trainer_gain
= 75,
168 .abs_control_gain
= 0,
169 .abs_control_limit
= 90,
170 .abs_control_error_limit
= 20,
171 .abs_control_cutoff
= 11,
172 .dterm_lpf1_static_hz
= DTERM_LPF1_DYN_MIN_HZ_DEFAULT
,
173 // NOTE: dynamic lpf is enabled by default so this setting is actually
174 // overridden and the static lowpass 1 is disabled. We can't set this
175 // value to 0 otherwise Configurator versions 10.4 and earlier will also
176 // reset the lowpass filter type to PT1 overriding the desired BIQUAD setting.
177 .dterm_lpf2_static_hz
= DTERM_LPF2_HZ_DEFAULT
, // second Dterm LPF ON by default
178 .dterm_lpf1_type
= FILTER_PT1
,
179 .dterm_lpf2_type
= FILTER_PT1
,
180 .dterm_lpf1_dyn_min_hz
= DTERM_LPF1_DYN_MIN_HZ_DEFAULT
,
181 .dterm_lpf1_dyn_max_hz
= DTERM_LPF1_DYN_MAX_HZ_DEFAULT
,
182 .launchControlMode
= LAUNCH_CONTROL_MODE_NORMAL
,
183 .launchControlThrottlePercent
= 20,
184 .launchControlAngleLimit
= 0,
185 .launchControlGain
= 40,
186 .launchControlAllowTriggerReset
= true,
187 .use_integrated_yaw
= false,
188 .integrated_yaw_relax
= 200,
189 .thrustLinearization
= 0,
190 .d_min
= D_MIN_DEFAULT
,
193 .motor_output_limit
= 100,
194 .auto_profile_cell_count
= AUTO_PROFILE_CELL_COUNT_STAY
,
195 .transient_throttle_limit
= 0,
196 .profileName
= { 0 },
197 .dyn_idle_min_rpm
= 0,
198 .dyn_idle_p_gain
= 50,
199 .dyn_idle_i_gain
= 50,
200 .dyn_idle_d_gain
= 50,
201 .dyn_idle_max_increase
= 150,
202 .dyn_idle_start_increase
= 50,
203 .feedforward_averaging
= FEEDFORWARD_AVERAGING_OFF
,
204 .feedforward_max_rate_limit
= 90,
205 .feedforward_smooth_factor
= 25,
206 .feedforward_jitter_factor
= 7,
207 .feedforward_boost
= 15,
208 .dterm_lpf1_dyn_expo
= 5,
209 .level_race_mode
= false,
210 .vbat_sag_compensation
= 0,
211 .simplified_pids_mode
= PID_SIMPLIFIED_TUNING_RPY
,
212 .simplified_master_multiplier
= SIMPLIFIED_TUNING_DEFAULT
,
213 .simplified_roll_pitch_ratio
= SIMPLIFIED_TUNING_DEFAULT
,
214 .simplified_i_gain
= SIMPLIFIED_TUNING_DEFAULT
,
215 .simplified_d_gain
= SIMPLIFIED_TUNING_D_DEFAULT
,
216 .simplified_pi_gain
= SIMPLIFIED_TUNING_DEFAULT
,
217 .simplified_dmin_ratio
= SIMPLIFIED_TUNING_D_DEFAULT
,
218 .simplified_feedforward_gain
= SIMPLIFIED_TUNING_DEFAULT
,
219 .simplified_pitch_pi_gain
= SIMPLIFIED_TUNING_DEFAULT
,
220 .simplified_dterm_filter
= true,
221 .simplified_dterm_filter_multiplier
= SIMPLIFIED_TUNING_DEFAULT
,
222 .anti_gravity_cutoff_hz
= 5,
223 .anti_gravity_p_gain
= 100,
224 .tpa_mode
= TPA_MODE_D
,
226 .tpa_breakpoint
= 1350,
230 pidProfile
->pid
[PID_ROLL
].D
= 30;
231 pidProfile
->pid
[PID_PITCH
].D
= 32;
235 void pgResetFn_pidProfiles(pidProfile_t
*pidProfiles
)
237 for (int i
= 0; i
< PID_PROFILE_COUNT
; i
++) {
238 resetPidProfile(&pidProfiles
[i
]);
242 // Scale factors to make best use of range with D_LPF debugging, aiming for max +/-16K as debug values are 16 bit
243 #define D_LPF_RAW_SCALE 25
244 #define D_LPF_FILT_SCALE 22
247 void pidSetItermAccelerator(float newItermAccelerator
)
249 pidRuntime
.itermAccelerator
= newItermAccelerator
;
252 bool pidOsdAntiGravityActive(void)
254 return (pidRuntime
.itermAccelerator
> pidRuntime
.antiGravityOsdCutoff
);
257 void pidStabilisationState(pidStabilisationState_e pidControllerState
)
259 pidRuntime
.pidStabilisationEnabled
= (pidControllerState
== PID_STABILISATION_ON
) ? true : false;
262 const angle_index_t rcAliasToAngleIndexMap
[] = { AI_ROLL
, AI_PITCH
};
264 #ifdef USE_FEEDFORWARD
265 float pidGetFeedforwardTransitionFactor(void)
267 return pidRuntime
.feedforwardTransitionFactor
;
270 float pidGetFeedforwardSmoothFactor(void)
272 return pidRuntime
.feedforwardSmoothFactor
;
275 float pidGetFeedforwardJitterFactor(void)
277 return pidRuntime
.feedforwardJitterFactor
;
280 float pidGetFeedforwardBoostFactor(void)
282 return pidRuntime
.feedforwardBoostFactor
;
286 void pidResetIterm(void)
288 for (int axis
= 0; axis
< 3; axis
++) {
289 pidData
[axis
].I
= 0.0f
;
290 #if defined(USE_ABSOLUTE_CONTROL)
291 axisError
[axis
] = 0.0f
;
296 void pidUpdateTpaFactor(float throttle
)
298 pidProfile_t
*currentPidProfile
;
300 currentPidProfile
= pidProfilesMutable(systemConfig()->pidProfileIndex
);
301 const float tpaBreakpoint
= (currentPidProfile
->tpa_breakpoint
- 1000) / 1000.0f
;
302 float tpaRate
= currentPidProfile
->tpa_rate
/ 100.0f
;
304 if (throttle
> tpaBreakpoint
) {
305 if (throttle
< 1.0f
) {
306 tpaRate
*= (throttle
- tpaBreakpoint
) / (1.0f
- tpaBreakpoint
);
311 pidRuntime
.tpaFactor
= 1.0f
- tpaRate
;
314 void pidUpdateAntiGravityThrottleFilter(float throttle
)
316 static float previousThrottle
= 0.0f
;
317 const float throttleInv
= 1.0f
- throttle
;
318 float throttleDerivative
= fabsf(throttle
- previousThrottle
) * pidRuntime
.pidFrequency
;
319 DEBUG_SET(DEBUG_ANTI_GRAVITY
, 0, lrintf(throttleDerivative
* 100));
320 throttleDerivative
*= throttleInv
* throttleInv
;
321 // generally focus on the low throttle period
322 if (throttle
> previousThrottle
) {
323 throttleDerivative
*= throttleInv
* 0.5f
;
324 // when increasing throttle, focus even more on the low throttle range
326 previousThrottle
= throttle
;
327 throttleDerivative
= pt2FilterApply(&pidRuntime
.antiGravityLpf
, throttleDerivative
);
328 // lower cutoff suppresses peaks relative to troughs and prolongs the effects
329 // PT2 smoothing of throttle derivative.
330 // 6 is a typical value for the peak boost factor with default cutoff of 6Hz
331 DEBUG_SET(DEBUG_ANTI_GRAVITY
, 1, lrintf(throttleDerivative
* 100));
332 pidRuntime
.antiGravityThrottleD
= throttleDerivative
;
335 #ifdef USE_ACRO_TRAINER
336 void pidAcroTrainerInit(void)
338 pidRuntime
.acroTrainerAxisState
[FD_ROLL
] = 0;
339 pidRuntime
.acroTrainerAxisState
[FD_PITCH
] = 0;
341 #endif // USE_ACRO_TRAINER
343 #ifdef USE_THRUST_LINEARIZATION
344 float pidCompensateThrustLinearization(float throttle
)
346 if (pidRuntime
.thrustLinearization
!= 0.0f
) {
347 // for whoops where a lot of TL is needed, allow more throttle boost
348 const float throttleReversed
= (1.0f
- throttle
);
349 throttle
/= 1.0f
+ pidRuntime
.throttleCompensateAmount
* sq(throttleReversed
);
354 float pidApplyThrustLinearization(float motorOutput
)
356 if (pidRuntime
.thrustLinearization
!= 0.0f
) {
357 if (motorOutput
> 0.0f
) {
358 const float motorOutputReversed
= (1.0f
- motorOutput
);
359 motorOutput
*= 1.0f
+ sq(motorOutputReversed
) * pidRuntime
.thrustLinearization
;
367 // calculate the stick deflection while applying level mode expo
368 static float getLevelModeRcDeflection(uint8_t axis
)
370 const float stickDeflection
= getRcDeflection(axis
);
372 const float expof
= currentControlRateProfile
->levelExpo
[axis
] / 100.0f
;
373 return power3(stickDeflection
) * expof
+ stickDeflection
* (1 - expof
);
375 return stickDeflection
;
379 // calculates strength of horizon leveling; 0 = none, 1.0 = most leveling
380 STATIC_UNIT_TESTED FAST_CODE_NOINLINE
float calcHorizonLevelStrength(void)
382 // start with 1.0 at center stick, 0.0 at max stick deflection:
383 float horizonLevelStrength
= 1.0f
- MAX(fabsf(getLevelModeRcDeflection(FD_ROLL
)), fabsf(getLevelModeRcDeflection(FD_PITCH
)));
385 // 0 at level, 90 at vertical, 180 at inverted (degrees):
386 const float currentInclination
= MAX(abs(attitude
.values
.roll
), abs(attitude
.values
.pitch
)) / 10.0f
;
388 // horizonTiltExpertMode: 0 = leveling always active when sticks centered,
389 // 1 = leveling can be totally off when inverted
390 if (pidRuntime
.horizonTiltExpertMode
) {
391 if (pidRuntime
.horizonTransition
> 0 && pidRuntime
.horizonCutoffDegrees
> 0) {
392 // if d_level > 0 and horizonTiltEffect < 175
393 // horizonCutoffDegrees: 0 to 125 => 270 to 90 (represents where leveling goes to zero)
394 // inclinationLevelRatio (0.0 to 1.0) is smaller (less leveling)
395 // for larger inclinations; 0.0 at horizonCutoffDegrees value:
396 const float inclinationLevelRatio
= constrainf((pidRuntime
.horizonCutoffDegrees
-currentInclination
) / pidRuntime
.horizonCutoffDegrees
, 0, 1);
397 // apply configured horizon sensitivity:
398 // when stick is near center (horizonLevelStrength ~= 1.0)
399 // H_sensitivity value has little effect,
400 // when stick is deflected (horizonLevelStrength near 0.0)
401 // H_sensitivity value has more effect:
402 horizonLevelStrength
= (horizonLevelStrength
- 1) * 100 / pidRuntime
.horizonTransition
+ 1;
403 // apply inclination ratio, which may lower leveling
404 // to zero regardless of stick position:
405 horizonLevelStrength
*= inclinationLevelRatio
;
406 } else { // d_level=0 or horizon_tilt_effect>=175 means no leveling
407 horizonLevelStrength
= 0;
409 } else { // horizon_tilt_expert_mode = 0 (leveling always active when sticks centered)
411 if (pidRuntime
.horizonFactorRatio
< 1.0f
) { // if horizonTiltEffect > 0
412 // horizonFactorRatio: 1.0 to 0.0 (larger means more leveling)
413 // inclinationLevelRatio (0.0 to 1.0) is smaller (less leveling)
414 // for larger inclinations, goes to 1.0 at inclination==level:
415 const float inclinationLevelRatio
= (180 - currentInclination
) / 180 * (1.0f
- pidRuntime
.horizonFactorRatio
) + pidRuntime
.horizonFactorRatio
;
416 // apply ratio to configured horizon sensitivity:
417 sensitFact
= pidRuntime
.horizonTransition
* inclinationLevelRatio
;
418 } else { // horizonTiltEffect=0 for "old" functionality
419 sensitFact
= pidRuntime
.horizonTransition
;
422 if (sensitFact
<= 0) { // zero means no leveling
423 horizonLevelStrength
= 0;
425 // when stick is near center (horizonLevelStrength ~= 1.0)
426 // sensitFact value has little effect,
427 // when stick is deflected (horizonLevelStrength near 0.0)
428 // sensitFact value has more effect:
429 horizonLevelStrength
= ((horizonLevelStrength
- 1) * (100 / sensitFact
)) + 1;
433 return constrainf(horizonLevelStrength
, 0, 1);
436 // Use the FAST_CODE_NOINLINE directive to avoid this code from being inlined into ITCM RAM to avoid overflow.
437 // The impact is possibly slightly slower performance on F7/H7 but they have more than enough
438 // processing power that it should be a non-issue.
439 STATIC_UNIT_TESTED FAST_CODE_NOINLINE
float pidLevel(int axis
, const pidProfile_t
*pidProfile
, const rollAndPitchTrims_t
*angleTrim
,
440 float currentPidSetpoint
, float horizonLevelStrength
)
442 const float levelAngleLimit
= pidProfile
->levelAngleLimit
;
443 // calculate error angle and limit the angle to the max inclination
444 // rcDeflection is in range [-1.0, 1.0]
445 float angle
= levelAngleLimit
* getLevelModeRcDeflection(axis
);
446 #ifdef USE_GPS_RESCUE
447 angle
+= gpsRescueAngle
[axis
] / 100; // ANGLE IS IN CENTIDEGREES
449 angle
= constrainf(angle
, -levelAngleLimit
, levelAngleLimit
);
450 const float errorAngle
= angle
- ((attitude
.raw
[axis
] - angleTrim
->raw
[axis
]) / 10.0f
);
451 if (FLIGHT_MODE(ANGLE_MODE
) || FLIGHT_MODE(GPS_RESCUE_MODE
)) {
452 // ANGLE mode - control is angle based
453 const float setpointCorrection
= errorAngle
* pidRuntime
.levelGain
;
454 currentPidSetpoint
= pt3FilterApply(&pidRuntime
.attitudeFilter
[axis
], setpointCorrection
);
456 // HORIZON mode - mix of ANGLE and ACRO modes
457 // mix in errorAngle to currentPidSetpoint to add a little auto-level feel
458 const float setpointCorrection
= errorAngle
* pidRuntime
.horizonGain
* horizonLevelStrength
;
459 currentPidSetpoint
+= pt3FilterApply(&pidRuntime
.attitudeFilter
[axis
], setpointCorrection
);
461 return currentPidSetpoint
;
464 static FAST_CODE_NOINLINE
void handleCrashRecovery(
465 const pidCrashRecovery_e crash_recovery
, const rollAndPitchTrims_t
*angleTrim
,
466 const int axis
, const timeUs_t currentTimeUs
, const float gyroRate
, float *currentPidSetpoint
, float *errorRate
)
468 if (pidRuntime
.inCrashRecoveryMode
&& cmpTimeUs(currentTimeUs
, pidRuntime
.crashDetectedAtUs
) > pidRuntime
.crashTimeDelayUs
) {
469 if (crash_recovery
== PID_CRASH_RECOVERY_BEEP
) {
472 if (axis
== FD_YAW
) {
473 *errorRate
= constrainf(*errorRate
, -pidRuntime
.crashLimitYaw
, pidRuntime
.crashLimitYaw
);
475 // on roll and pitch axes calculate currentPidSetpoint and errorRate to level the aircraft to recover from crash
476 if (sensors(SENSOR_ACC
)) {
477 // errorAngle is deviation from horizontal
478 const float errorAngle
= -(attitude
.raw
[axis
] - angleTrim
->raw
[axis
]) / 10.0f
;
479 *currentPidSetpoint
= errorAngle
* pidRuntime
.levelGain
;
480 *errorRate
= *currentPidSetpoint
- gyroRate
;
483 // reset iterm, since accumulated error before crash is now meaningless
484 // and iterm windup during crash recovery can be extreme, especially on yaw axis
485 pidData
[axis
].I
= 0.0f
;
486 if (cmpTimeUs(currentTimeUs
, pidRuntime
.crashDetectedAtUs
) > pidRuntime
.crashTimeLimitUs
487 || (getMotorMixRange() < 1.0f
488 && fabsf(gyro
.gyroADCf
[FD_ROLL
]) < pidRuntime
.crashRecoveryRate
489 && fabsf(gyro
.gyroADCf
[FD_PITCH
]) < pidRuntime
.crashRecoveryRate
490 && fabsf(gyro
.gyroADCf
[FD_YAW
]) < pidRuntime
.crashRecoveryRate
)) {
491 if (sensors(SENSOR_ACC
)) {
492 // check aircraft nearly level
493 if (abs(attitude
.raw
[FD_ROLL
] - angleTrim
->raw
[FD_ROLL
]) < pidRuntime
.crashRecoveryAngleDeciDegrees
494 && abs(attitude
.raw
[FD_PITCH
] - angleTrim
->raw
[FD_PITCH
]) < pidRuntime
.crashRecoveryAngleDeciDegrees
) {
495 pidRuntime
.inCrashRecoveryMode
= false;
499 pidRuntime
.inCrashRecoveryMode
= false;
506 static FAST_CODE_NOINLINE
void detectAndSetCrashRecovery(
507 const pidCrashRecovery_e crash_recovery
, const int axis
,
508 const timeUs_t currentTimeUs
, const float delta
, const float errorRate
)
510 // if crash recovery is on and accelerometer enabled and there is no gyro overflow, then check for a crash
511 // no point in trying to recover if the crash is so severe that the gyro overflows
512 if ((crash_recovery
|| FLIGHT_MODE(GPS_RESCUE_MODE
)) && !gyroOverflowDetected()) {
513 if (ARMING_FLAG(ARMED
)) {
514 if (getMotorMixRange() >= 1.0f
&& !pidRuntime
.inCrashRecoveryMode
515 && fabsf(delta
) > pidRuntime
.crashDtermThreshold
516 && fabsf(errorRate
) > pidRuntime
.crashGyroThreshold
517 && fabsf(getSetpointRate(axis
)) < pidRuntime
.crashSetpointThreshold
) {
518 if (crash_recovery
== PID_CRASH_RECOVERY_DISARM
) {
519 setArmingDisabled(ARMING_DISABLED_CRASH_DETECTED
);
520 disarm(DISARM_REASON_CRASH_PROTECTION
);
522 pidRuntime
.inCrashRecoveryMode
= true;
523 pidRuntime
.crashDetectedAtUs
= currentTimeUs
;
526 if (pidRuntime
.inCrashRecoveryMode
&& cmpTimeUs(currentTimeUs
, pidRuntime
.crashDetectedAtUs
) < pidRuntime
.crashTimeDelayUs
&& (fabsf(errorRate
) < pidRuntime
.crashGyroThreshold
527 || fabsf(getSetpointRate(axis
)) > pidRuntime
.crashSetpointThreshold
)) {
528 pidRuntime
.inCrashRecoveryMode
= false;
531 } else if (pidRuntime
.inCrashRecoveryMode
) {
532 pidRuntime
.inCrashRecoveryMode
= false;
539 #ifdef USE_ACRO_TRAINER
541 int acroTrainerSign(float x
)
543 return x
> 0 ? 1 : -1;
546 // Acro Trainer - Manipulate the setPoint to limit axis angle while in acro mode
547 // There are three states:
548 // 1. Current angle has exceeded limit
549 // Apply correction to return to limit (similar to pidLevel)
550 // 2. Future overflow has been projected based on current angle and gyro rate
551 // Manage the setPoint to control the gyro rate as the actual angle approaches the limit (try to prevent overshoot)
552 // 3. If no potential overflow is detected, then return the original setPoint
554 // Use the FAST_CODE_NOINLINE directive to avoid this code from being inlined into ITCM RAM. We accept the
555 // performance decrease when Acro Trainer mode is active under the assumption that user is unlikely to be
556 // expecting ultimate flight performance at very high loop rates when in this mode.
557 static FAST_CODE_NOINLINE
float applyAcroTrainer(int axis
, const rollAndPitchTrims_t
*angleTrim
, float setPoint
)
559 float ret
= setPoint
;
561 if (!FLIGHT_MODE(ANGLE_MODE
) && !FLIGHT_MODE(HORIZON_MODE
) && !FLIGHT_MODE(GPS_RESCUE_MODE
)) {
562 bool resetIterm
= false;
563 float projectedAngle
= 0;
564 const int setpointSign
= acroTrainerSign(setPoint
);
565 const float currentAngle
= (attitude
.raw
[axis
] - angleTrim
->raw
[axis
]) / 10.0f
;
566 const int angleSign
= acroTrainerSign(currentAngle
);
568 if ((pidRuntime
.acroTrainerAxisState
[axis
] != 0) && (pidRuntime
.acroTrainerAxisState
[axis
] != setpointSign
)) { // stick has reversed - stop limiting
569 pidRuntime
.acroTrainerAxisState
[axis
] = 0;
572 // Limit and correct the angle when it exceeds the limit
573 if ((fabsf(currentAngle
) > pidRuntime
.acroTrainerAngleLimit
) && (pidRuntime
.acroTrainerAxisState
[axis
] == 0)) {
574 if (angleSign
== setpointSign
) {
575 pidRuntime
.acroTrainerAxisState
[axis
] = angleSign
;
580 if (pidRuntime
.acroTrainerAxisState
[axis
] != 0) {
581 ret
= constrainf(((pidRuntime
.acroTrainerAngleLimit
* angleSign
) - currentAngle
) * pidRuntime
.acroTrainerGain
, -ACRO_TRAINER_SETPOINT_LIMIT
, ACRO_TRAINER_SETPOINT_LIMIT
);
584 // Not currently over the limit so project the angle based on current angle and
585 // gyro angular rate using a sliding window based on gyro rate (faster rotation means larger window.
586 // If the projected angle exceeds the limit then apply limiting to minimize overshoot.
587 // Calculate the lookahead window by scaling proportionally with gyro rate from 0-500dps
588 float checkInterval
= constrainf(fabsf(gyro
.gyroADCf
[axis
]) / ACRO_TRAINER_LOOKAHEAD_RATE_LIMIT
, 0.0f
, 1.0f
) * pidRuntime
.acroTrainerLookaheadTime
;
589 projectedAngle
= (gyro
.gyroADCf
[axis
] * checkInterval
) + currentAngle
;
590 const int projectedAngleSign
= acroTrainerSign(projectedAngle
);
591 if ((fabsf(projectedAngle
) > pidRuntime
.acroTrainerAngleLimit
) && (projectedAngleSign
== setpointSign
)) {
592 ret
= ((pidRuntime
.acroTrainerAngleLimit
* projectedAngleSign
) - projectedAngle
) * pidRuntime
.acroTrainerGain
;
601 if (axis
== pidRuntime
.acroTrainerDebugAxis
) {
602 DEBUG_SET(DEBUG_ACRO_TRAINER
, 0, lrintf(currentAngle
* 10.0f
));
603 DEBUG_SET(DEBUG_ACRO_TRAINER
, 1, pidRuntime
.acroTrainerAxisState
[axis
]);
604 DEBUG_SET(DEBUG_ACRO_TRAINER
, 2, lrintf(ret
));
605 DEBUG_SET(DEBUG_ACRO_TRAINER
, 3, lrintf(projectedAngle
* 10.0f
));
611 #endif // USE_ACRO_TRAINER
613 static float accelerationLimit(int axis
, float currentPidSetpoint
)
615 static float previousSetpoint
[XYZ_AXIS_COUNT
];
616 const float currentVelocity
= currentPidSetpoint
- previousSetpoint
[axis
];
618 if (fabsf(currentVelocity
) > pidRuntime
.maxVelocity
[axis
]) {
619 currentPidSetpoint
= (currentVelocity
> 0) ? previousSetpoint
[axis
] + pidRuntime
.maxVelocity
[axis
] : previousSetpoint
[axis
] - pidRuntime
.maxVelocity
[axis
];
622 previousSetpoint
[axis
] = currentPidSetpoint
;
623 return currentPidSetpoint
;
626 static void rotateVector(float v
[XYZ_AXIS_COUNT
], float rotation
[XYZ_AXIS_COUNT
])
628 // rotate v around rotation vector rotation
629 // rotation in radians, all elements must be small
630 for (int i
= 0; i
< XYZ_AXIS_COUNT
; i
++) {
631 int i_1
= (i
+ 1) % 3;
632 int i_2
= (i
+ 2) % 3;
633 float newV
= v
[i_1
] + v
[i_2
] * rotation
[i
];
634 v
[i_2
] -= v
[i_1
] * rotation
[i
];
639 STATIC_UNIT_TESTED
void rotateItermAndAxisError(void)
641 if (pidRuntime
.itermRotation
642 #if defined(USE_ABSOLUTE_CONTROL)
643 || pidRuntime
.acGain
> 0 || debugMode
== DEBUG_AC_ERROR
646 const float gyroToAngle
= pidRuntime
.dT
* RAD
;
647 float rotationRads
[XYZ_AXIS_COUNT
];
648 for (int i
= FD_ROLL
; i
<= FD_YAW
; i
++) {
649 rotationRads
[i
] = gyro
.gyroADCf
[i
] * gyroToAngle
;
651 #if defined(USE_ABSOLUTE_CONTROL)
652 if (pidRuntime
.acGain
> 0 || debugMode
== DEBUG_AC_ERROR
) {
653 rotateVector(axisError
, rotationRads
);
656 if (pidRuntime
.itermRotation
) {
657 float v
[XYZ_AXIS_COUNT
];
658 for (int i
= 0; i
< XYZ_AXIS_COUNT
; i
++) {
661 rotateVector(v
, rotationRads
);
662 for (int i
= 0; i
< XYZ_AXIS_COUNT
; i
++) {
669 #ifdef USE_RC_SMOOTHING_FILTER
670 float FAST_CODE
applyRcSmoothingFeedforwardFilter(int axis
, float pidSetpointDelta
)
672 float ret
= pidSetpointDelta
;
673 if (axis
== pidRuntime
.rcSmoothingDebugAxis
) {
674 DEBUG_SET(DEBUG_RC_SMOOTHING
, 1, lrintf(pidSetpointDelta
* 100.0f
));
676 if (pidRuntime
.feedforwardLpfInitialized
) {
677 ret
= pt3FilterApply(&pidRuntime
.feedforwardPt3
[axis
], pidSetpointDelta
);
678 if (axis
== pidRuntime
.rcSmoothingDebugAxis
) {
679 DEBUG_SET(DEBUG_RC_SMOOTHING
, 2, lrintf(ret
* 100.0f
));
684 #endif // USE_RC_SMOOTHING_FILTER
686 #if defined(USE_ITERM_RELAX)
687 #if defined(USE_ABSOLUTE_CONTROL)
688 STATIC_UNIT_TESTED
void applyAbsoluteControl(const int axis
, const float gyroRate
, float *currentPidSetpoint
, float *itermErrorRate
)
690 if (pidRuntime
.acGain
> 0 || debugMode
== DEBUG_AC_ERROR
) {
691 const float setpointLpf
= pt1FilterApply(&pidRuntime
.acLpf
[axis
], *currentPidSetpoint
);
692 const float setpointHpf
= fabsf(*currentPidSetpoint
- setpointLpf
);
693 float acErrorRate
= 0;
694 const float gmaxac
= setpointLpf
+ 2 * setpointHpf
;
695 const float gminac
= setpointLpf
- 2 * setpointHpf
;
696 if (gyroRate
>= gminac
&& gyroRate
<= gmaxac
) {
697 const float acErrorRate1
= gmaxac
- gyroRate
;
698 const float acErrorRate2
= gminac
- gyroRate
;
699 if (acErrorRate1
* axisError
[axis
] < 0) {
700 acErrorRate
= acErrorRate1
;
702 acErrorRate
= acErrorRate2
;
704 if (fabsf(acErrorRate
* pidRuntime
.dT
) > fabsf(axisError
[axis
]) ) {
705 acErrorRate
= -axisError
[axis
] * pidRuntime
.pidFrequency
;
708 acErrorRate
= (gyroRate
> gmaxac
? gmaxac
: gminac
) - gyroRate
;
711 if (isAirmodeActivated()) {
712 axisError
[axis
] = constrainf(axisError
[axis
] + acErrorRate
* pidRuntime
.dT
,
713 -pidRuntime
.acErrorLimit
, pidRuntime
.acErrorLimit
);
714 const float acCorrection
= constrainf(axisError
[axis
] * pidRuntime
.acGain
, -pidRuntime
.acLimit
, pidRuntime
.acLimit
);
715 *currentPidSetpoint
+= acCorrection
;
716 *itermErrorRate
+= acCorrection
;
717 DEBUG_SET(DEBUG_AC_CORRECTION
, axis
, lrintf(acCorrection
* 10));
718 if (axis
== FD_ROLL
) {
719 DEBUG_SET(DEBUG_ITERM_RELAX
, 3, lrintf(acCorrection
* 10));
722 DEBUG_SET(DEBUG_AC_ERROR
, axis
, lrintf(axisError
[axis
] * 10));
727 STATIC_UNIT_TESTED
void applyItermRelax(const int axis
, const float iterm
,
728 const float gyroRate
, float *itermErrorRate
, float *currentPidSetpoint
)
730 const float setpointLpf
= pt1FilterApply(&pidRuntime
.windupLpf
[axis
], *currentPidSetpoint
);
731 const float setpointHpf
= fabsf(*currentPidSetpoint
- setpointLpf
);
733 if (pidRuntime
.itermRelax
) {
734 if (axis
< FD_YAW
|| pidRuntime
.itermRelax
== ITERM_RELAX_RPY
|| pidRuntime
.itermRelax
== ITERM_RELAX_RPY_INC
) {
735 const float itermRelaxFactor
= MAX(0, 1 - setpointHpf
/ ITERM_RELAX_SETPOINT_THRESHOLD
);
736 const bool isDecreasingI
=
737 ((iterm
> 0) && (*itermErrorRate
< 0)) || ((iterm
< 0) && (*itermErrorRate
> 0));
738 if ((pidRuntime
.itermRelax
>= ITERM_RELAX_RP_INC
) && isDecreasingI
) {
739 // Do Nothing, use the precalculed itermErrorRate
740 } else if (pidRuntime
.itermRelaxType
== ITERM_RELAX_SETPOINT
) {
741 *itermErrorRate
*= itermRelaxFactor
;
742 } else if (pidRuntime
.itermRelaxType
== ITERM_RELAX_GYRO
) {
743 *itermErrorRate
= fapplyDeadband(setpointLpf
- gyroRate
, setpointHpf
);
745 *itermErrorRate
= 0.0f
;
748 if (axis
== FD_ROLL
) {
749 DEBUG_SET(DEBUG_ITERM_RELAX
, 0, lrintf(setpointHpf
));
750 DEBUG_SET(DEBUG_ITERM_RELAX
, 1, lrintf(itermRelaxFactor
* 100.0f
));
751 DEBUG_SET(DEBUG_ITERM_RELAX
, 2, lrintf(*itermErrorRate
));
755 #if defined(USE_ABSOLUTE_CONTROL)
756 applyAbsoluteControl(axis
, gyroRate
, currentPidSetpoint
, itermErrorRate
);
762 #ifdef USE_AIRMODE_LPF
763 void pidUpdateAirmodeLpf(float currentOffset
)
765 if (pidRuntime
.airmodeThrottleOffsetLimit
== 0.0f
) {
769 float offsetHpf
= currentOffset
* 2.5f
;
770 offsetHpf
= offsetHpf
- pt1FilterApply(&pidRuntime
.airmodeThrottleLpf2
, offsetHpf
);
772 // During high frequency oscillation 2 * currentOffset averages to the offset required to avoid mirroring of the waveform
773 pt1FilterApply(&pidRuntime
.airmodeThrottleLpf1
, offsetHpf
);
774 // Bring offset up immediately so the filter only applies to the decline
775 if (currentOffset
* pidRuntime
.airmodeThrottleLpf1
.state
>= 0 && fabsf(currentOffset
) > pidRuntime
.airmodeThrottleLpf1
.state
) {
776 pidRuntime
.airmodeThrottleLpf1
.state
= currentOffset
;
778 pidRuntime
.airmodeThrottleLpf1
.state
= constrainf(pidRuntime
.airmodeThrottleLpf1
.state
, -pidRuntime
.airmodeThrottleOffsetLimit
, pidRuntime
.airmodeThrottleOffsetLimit
);
781 float pidGetAirmodeThrottleOffset(void)
783 return pidRuntime
.airmodeThrottleLpf1
.state
;
787 #ifdef USE_LAUNCH_CONTROL
788 #define LAUNCH_CONTROL_MAX_RATE 100.0f
789 #define LAUNCH_CONTROL_MIN_RATE 5.0f
790 #define LAUNCH_CONTROL_ANGLE_WINDOW 10.0f // The remaining angle degrees where rate dampening starts
792 // Use the FAST_CODE_NOINLINE directive to avoid this code from being inlined into ITCM RAM to avoid overflow.
793 // The impact is possibly slightly slower performance on F7/H7 but they have more than enough
794 // processing power that it should be a non-issue.
795 static FAST_CODE_NOINLINE
float applyLaunchControl(int axis
, const rollAndPitchTrims_t
*angleTrim
)
799 // Scale the rates based on stick deflection only. Fixed rates with a max of 100deg/sec
800 // reached at 50% stick deflection. This keeps the launch control positioning consistent
801 // regardless of the user's rates.
802 if ((axis
== FD_PITCH
) || (pidRuntime
.launchControlMode
!= LAUNCH_CONTROL_MODE_PITCHONLY
)) {
803 const float stickDeflection
= constrainf(getRcDeflection(axis
), -0.5f
, 0.5f
);
804 ret
= LAUNCH_CONTROL_MAX_RATE
* stickDeflection
* 2;
808 // If ACC is enabled and a limit angle is set, then try to limit forward tilt
809 // to that angle and slow down the rate as the limit is approached to reduce overshoot
810 if ((axis
== FD_PITCH
) && (pidRuntime
.launchControlAngleLimit
> 0) && (ret
> 0)) {
811 const float currentAngle
= (attitude
.raw
[axis
] - angleTrim
->raw
[axis
]) / 10.0f
;
812 if (currentAngle
>= pidRuntime
.launchControlAngleLimit
) {
815 //for the last 10 degrees scale the rate from the current input to 5 dps
816 const float angleDelta
= pidRuntime
.launchControlAngleLimit
- currentAngle
;
817 if (angleDelta
<= LAUNCH_CONTROL_ANGLE_WINDOW
) {
818 ret
= scaleRangef(angleDelta
, 0, LAUNCH_CONTROL_ANGLE_WINDOW
, LAUNCH_CONTROL_MIN_RATE
, ret
);
830 // Betaflight pid controller, which will be maintained in the future with additional features specialised for current (mini) multirotor usage.
831 // Based on 2DOF reference design (matlab)
832 void FAST_CODE
pidController(const pidProfile_t
*pidProfile
, timeUs_t currentTimeUs
)
834 static float previousGyroRateDterm
[XYZ_AXIS_COUNT
];
835 static float previousRawGyroRateDterm
[XYZ_AXIS_COUNT
];
838 const float tpaFactorKp
= (pidProfile
->tpa_mode
== TPA_MODE_PD
) ? pidRuntime
.tpaFactor
: 1.0f
;
840 const float tpaFactorKp
= pidRuntime
.tpaFactor
;
843 #ifdef USE_YAW_SPIN_RECOVERY
844 const bool yawSpinActive
= gyroYawSpinDetected();
847 const bool launchControlActive
= isLaunchControlActive();
850 static timeUs_t levelModeStartTimeUs
= 0;
851 static bool gpsRescuePreviousState
= false;
852 const rollAndPitchTrims_t
*angleTrim
= &accelerometerConfig()->accelerometerTrims
;
853 float horizonLevelStrength
= 0.0f
;
855 const bool gpsRescueIsActive
= FLIGHT_MODE(GPS_RESCUE_MODE
);
856 levelMode_e levelMode
;
857 if (FLIGHT_MODE(ANGLE_MODE
) || FLIGHT_MODE(HORIZON_MODE
) || gpsRescueIsActive
) {
858 if (pidRuntime
.levelRaceMode
&& !gpsRescueIsActive
) {
859 levelMode
= LEVEL_MODE_R
;
861 levelMode
= LEVEL_MODE_RP
;
864 // Keep track of when we entered a self-level mode so that we can
865 // add a guard time before crash recovery can activate.
866 // Also reset the guard time whenever GPS Rescue is activated.
867 if ((levelModeStartTimeUs
== 0) || (gpsRescueIsActive
&& !gpsRescuePreviousState
)) {
868 levelModeStartTimeUs
= currentTimeUs
;
871 // Calc horizonLevelStrength if needed
872 if (FLIGHT_MODE(HORIZON_MODE
)) {
873 horizonLevelStrength
= calcHorizonLevelStrength();
876 levelMode
= LEVEL_MODE_OFF
;
877 levelModeStartTimeUs
= 0;
880 gpsRescuePreviousState
= gpsRescueIsActive
;
883 UNUSED(currentTimeUs
);
887 if (pidRuntime
.antiGravityEnabled
) {
888 pidRuntime
.antiGravityThrottleD
*= pidRuntime
.antiGravityGain
;
889 // used later to increase pTerm
890 pidRuntime
.itermAccelerator
= pidRuntime
.antiGravityThrottleD
* ANTIGRAVITY_KI
;
892 pidRuntime
.antiGravityThrottleD
= 0.0f
;
893 pidRuntime
.itermAccelerator
= 0.0f
;
895 DEBUG_SET(DEBUG_ANTI_GRAVITY
, 2, lrintf((1 + (pidRuntime
.itermAccelerator
/ pidRuntime
.pidCoefficient
[FD_PITCH
].Ki
)) * 1000));
896 // amount of antigravity added relative to user's pitch iTerm coefficient
897 // used later to increase iTerm
899 // iTerm windup (attenuation of iTerm if motorMix range is large)
901 if (pidRuntime
.itermWindupPointInv
> 1.0f
) {
902 dynCi
= constrainf((1.0f
- getMotorMixRange()) * pidRuntime
.itermWindupPointInv
, 0.0f
, 1.0f
);
905 // Precalculate gyro delta for D-term here, this allows loop unrolling
906 float gyroRateDterm
[XYZ_AXIS_COUNT
];
907 for (int axis
= FD_ROLL
; axis
<= FD_YAW
; ++axis
) {
908 gyroRateDterm
[axis
] = gyro
.gyroADCf
[axis
];
909 // -----calculate raw, unfiltered D component
911 // Divide rate change by dT to get differential (ie dr/dt).
912 // dT is fixed and calculated from the target PID loop time
913 // This is done to avoid DTerm spikes that occur with dynamically
914 // calculated deltaT whenever another task causes the PID
915 // loop execution to be delayed.
917 // Log the unfiltered D for ROLL and PITCH
918 if (axis
!= FD_YAW
) {
919 const float delta
= (previousRawGyroRateDterm
[axis
] - gyroRateDterm
[axis
]) * pidRuntime
.pidFrequency
/ D_LPF_RAW_SCALE
;
920 previousRawGyroRateDterm
[axis
] = gyroRateDterm
[axis
];
921 DEBUG_SET(DEBUG_D_LPF
, axis
, lrintf(delta
));
924 gyroRateDterm
[axis
] = pidRuntime
.dtermNotchApplyFn((filter_t
*) &pidRuntime
.dtermNotch
[axis
], gyroRateDterm
[axis
]);
925 gyroRateDterm
[axis
] = pidRuntime
.dtermLowpassApplyFn((filter_t
*) &pidRuntime
.dtermLowpass
[axis
], gyroRateDterm
[axis
]);
926 gyroRateDterm
[axis
] = pidRuntime
.dtermLowpass2ApplyFn((filter_t
*) &pidRuntime
.dtermLowpass2
[axis
], gyroRateDterm
[axis
]);
929 rotateItermAndAxisError();
931 #ifdef USE_RPM_FILTER
935 #ifdef USE_FEEDFORWARD
936 const bool newRcFrame
= getShouldUpdateFeedforward();
939 // ----------PID controller----------
940 for (int axis
= FD_ROLL
; axis
<= FD_YAW
; ++axis
) {
942 float currentPidSetpoint
= getSetpointRate(axis
);
943 if (pidRuntime
.maxVelocity
[axis
]) {
944 currentPidSetpoint
= accelerationLimit(axis
, currentPidSetpoint
);
946 // Yaw control is GYRO based, direct sticks control is applied to rate PID
947 // When Race Mode is active PITCH control is also GYRO based in level or horizon mode
949 if ((levelMode
== LEVEL_MODE_R
&& axis
== FD_ROLL
)
950 || (levelMode
== LEVEL_MODE_RP
&& (axis
== FD_ROLL
|| axis
== FD_PITCH
)) ) {
951 currentPidSetpoint
= pidLevel(axis
, pidProfile
, angleTrim
, currentPidSetpoint
, horizonLevelStrength
);
952 DEBUG_SET(DEBUG_ATTITUDE
, axis
- FD_ROLL
+ 2, currentPidSetpoint
);
956 #ifdef USE_ACRO_TRAINER
957 if ((axis
!= FD_YAW
) && pidRuntime
.acroTrainerActive
&& !pidRuntime
.inCrashRecoveryMode
&& !launchControlActive
) {
958 currentPidSetpoint
= applyAcroTrainer(axis
, angleTrim
, currentPidSetpoint
);
960 #endif // USE_ACRO_TRAINER
962 #ifdef USE_LAUNCH_CONTROL
963 if (launchControlActive
) {
965 currentPidSetpoint
= applyLaunchControl(axis
, angleTrim
);
967 currentPidSetpoint
= applyLaunchControl(axis
, NULL
);
972 // Handle yaw spin recovery - zero the setpoint on yaw to aid in recovery
973 // It's not necessary to zero the set points for R/P because the PIDs will be zeroed below
974 #ifdef USE_YAW_SPIN_RECOVERY
975 if ((axis
== FD_YAW
) && yawSpinActive
) {
976 currentPidSetpoint
= 0.0f
;
978 #endif // USE_YAW_SPIN_RECOVERY
980 // -----calculate error rate
981 const float gyroRate
= gyro
.gyroADCf
[axis
]; // Process variable from gyro output in deg/sec
982 float errorRate
= currentPidSetpoint
- gyroRate
; // r - y
985 pidProfile
->crash_recovery
, angleTrim
, axis
, currentTimeUs
, gyroRate
,
986 ¤tPidSetpoint
, &errorRate
);
989 const float previousIterm
= pidData
[axis
].I
;
990 float itermErrorRate
= errorRate
;
991 #ifdef USE_ABSOLUTE_CONTROL
992 const float uncorrectedSetpoint
= currentPidSetpoint
;
995 #if defined(USE_ITERM_RELAX)
996 if (!launchControlActive
&& !pidRuntime
.inCrashRecoveryMode
) {
997 applyItermRelax(axis
, previousIterm
, gyroRate
, &itermErrorRate
, ¤tPidSetpoint
);
998 errorRate
= currentPidSetpoint
- gyroRate
;
1001 #ifdef USE_ABSOLUTE_CONTROL
1002 const float setpointCorrection
= currentPidSetpoint
- uncorrectedSetpoint
;
1005 // --------low-level gyro-based PID based on 2DOF PID controller. ----------
1006 // 2-DOF PID controller with optional filter on derivative term.
1007 // b = 1 and only c (feedforward weight) can be tuned (amount derivative on measurement or error).
1009 // -----calculate P component
1010 pidData
[axis
].P
= pidRuntime
.pidCoefficient
[axis
].Kp
* errorRate
* tpaFactorKp
;
1011 if (axis
== FD_YAW
) {
1012 pidData
[axis
].P
= pidRuntime
.ptermYawLowpassApplyFn((filter_t
*) &pidRuntime
.ptermYawLowpass
, pidData
[axis
].P
);
1015 // -----calculate I component
1016 float Ki
= pidRuntime
.pidCoefficient
[axis
].Ki
;
1017 #ifdef USE_LAUNCH_CONTROL
1018 // if launch control is active override the iterm gains and apply iterm windup protection to all axes
1019 if (launchControlActive
) {
1020 Ki
= pidRuntime
.launchControlKi
;
1024 if (axis
== FD_YAW
) {
1025 pidRuntime
.itermAccelerator
= 0.0f
; // no antigravity on yaw iTerm
1028 const float iTermChange
= (Ki
+ pidRuntime
.itermAccelerator
) * dynCi
* pidRuntime
.dT
* itermErrorRate
;
1029 pidData
[axis
].I
= constrainf(previousIterm
+ iTermChange
, -pidRuntime
.itermLimit
, pidRuntime
.itermLimit
);
1031 // -----calculate pidSetpointDelta
1032 float pidSetpointDelta
= 0;
1033 #ifdef USE_FEEDFORWARD
1034 pidSetpointDelta
= feedforwardApply(axis
, newRcFrame
, pidRuntime
.feedforwardAveraging
);
1036 pidRuntime
.previousPidSetpoint
[axis
] = currentPidSetpoint
;
1038 // -----calculate D component
1039 // disable D if launch control is active
1040 if ((pidRuntime
.pidCoefficient
[axis
].Kd
> 0) && !launchControlActive
) {
1042 // Divide rate change by dT to get differential (ie dr/dt).
1043 // dT is fixed and calculated from the target PID loop time
1044 // This is done to avoid DTerm spikes that occur with dynamically
1045 // calculated deltaT whenever another task causes the PID
1046 // loop execution to be delayed.
1048 - (gyroRateDterm
[axis
] - previousGyroRateDterm
[axis
]) * pidRuntime
.pidFrequency
;
1049 float preTpaD
= pidRuntime
.pidCoefficient
[axis
].Kd
* delta
;
1051 #if defined(USE_ACC)
1052 if (cmpTimeUs(currentTimeUs
, levelModeStartTimeUs
) > CRASH_RECOVERY_DETECTION_DELAY_US
) {
1053 detectAndSetCrashRecovery(pidProfile
->crash_recovery
, axis
, currentTimeUs
, delta
, errorRate
);
1057 #if defined(USE_D_MIN)
1058 float dMinFactor
= 1.0f
;
1059 if (pidRuntime
.dMinPercent
[axis
] > 0) {
1060 float dMinGyroFactor
= pt2FilterApply(&pidRuntime
.dMinRange
[axis
], delta
);
1061 dMinGyroFactor
= fabsf(dMinGyroFactor
) * pidRuntime
.dMinGyroGain
;
1062 const float dMinSetpointFactor
= (fabsf(pidSetpointDelta
)) * pidRuntime
.dMinSetpointGain
;
1063 dMinFactor
= MAX(dMinGyroFactor
, dMinSetpointFactor
);
1064 dMinFactor
= pidRuntime
.dMinPercent
[axis
] + (1.0f
- pidRuntime
.dMinPercent
[axis
]) * dMinFactor
;
1065 dMinFactor
= pt2FilterApply(&pidRuntime
.dMinLowpass
[axis
], dMinFactor
);
1066 dMinFactor
= MIN(dMinFactor
, 1.0f
);
1067 if (axis
== FD_ROLL
) {
1068 DEBUG_SET(DEBUG_D_MIN
, 0, lrintf(dMinGyroFactor
* 100));
1069 DEBUG_SET(DEBUG_D_MIN
, 1, lrintf(dMinSetpointFactor
* 100));
1070 DEBUG_SET(DEBUG_D_MIN
, 2, lrintf(pidRuntime
.pidCoefficient
[axis
].Kd
* dMinFactor
* 10 / DTERM_SCALE
));
1071 } else if (axis
== FD_PITCH
) {
1072 DEBUG_SET(DEBUG_D_MIN
, 3, lrintf(pidRuntime
.pidCoefficient
[axis
].Kd
* dMinFactor
* 10 / DTERM_SCALE
));
1076 // Apply the dMinFactor
1077 preTpaD
*= dMinFactor
;
1079 pidData
[axis
].D
= preTpaD
* pidRuntime
.tpaFactor
;
1081 // Log the value of D pre application of TPA
1082 preTpaD
*= D_LPF_FILT_SCALE
;
1084 if (axis
!= FD_YAW
) {
1085 DEBUG_SET(DEBUG_D_LPF
, axis
- FD_ROLL
+ 2, lrintf(preTpaD
));
1088 pidData
[axis
].D
= 0;
1089 if (axis
!= FD_YAW
) {
1090 DEBUG_SET(DEBUG_D_LPF
, axis
- FD_ROLL
+ 2, 0);
1094 previousGyroRateDterm
[axis
] = gyroRateDterm
[axis
];
1096 // -----calculate feedforward component
1097 #ifdef USE_ABSOLUTE_CONTROL
1098 // include abs control correction in feedforward
1099 pidSetpointDelta
+= setpointCorrection
- pidRuntime
.oldSetpointCorrection
[axis
];
1100 pidRuntime
.oldSetpointCorrection
[axis
] = setpointCorrection
;
1103 // no feedforward in launch control
1104 float feedforwardGain
= launchControlActive
? 0.0f
: pidRuntime
.pidCoefficient
[axis
].Kf
;
1105 if (feedforwardGain
> 0) {
1106 // halve feedforward in Level mode since stick sensitivity is weaker by about half
1107 feedforwardGain
*= FLIGHT_MODE(ANGLE_MODE
) ? 0.5f
: 1.0f
;
1108 // transition now calculated in feedforward.c when new RC data arrives
1109 float feedForward
= feedforwardGain
* pidSetpointDelta
* pidRuntime
.pidFrequency
;
1111 #ifdef USE_FEEDFORWARD
1112 pidData
[axis
].F
= shouldApplyFeedforwardLimits(axis
) ?
1113 applyFeedforwardLimit(axis
, feedForward
, pidRuntime
.pidCoefficient
[axis
].Kp
, currentPidSetpoint
) : feedForward
;
1115 pidData
[axis
].F
= feedForward
;
1117 #ifdef USE_RC_SMOOTHING_FILTER
1118 pidData
[axis
].F
= applyRcSmoothingFeedforwardFilter(axis
, pidData
[axis
].F
);
1119 #endif // USE_RC_SMOOTHING_FILTER
1121 pidData
[axis
].F
= 0;
1124 #ifdef USE_YAW_SPIN_RECOVERY
1125 if (yawSpinActive
) {
1126 pidData
[axis
].I
= 0; // in yaw spin always disable I
1127 if (axis
<= FD_PITCH
) {
1128 // zero PIDs on pitch and roll leaving yaw P to correct spin
1129 pidData
[axis
].P
= 0;
1130 pidData
[axis
].D
= 0;
1131 pidData
[axis
].F
= 0;
1134 #endif // USE_YAW_SPIN_RECOVERY
1136 #ifdef USE_LAUNCH_CONTROL
1137 // Disable P/I appropriately based on the launch control mode
1138 if (launchControlActive
) {
1139 // if not using FULL mode then disable I accumulation on yaw as
1140 // yaw has a tendency to windup. Otherwise limit yaw iterm accumulation.
1141 const int launchControlYawItermLimit
= (pidRuntime
.launchControlMode
== LAUNCH_CONTROL_MODE_FULL
) ? LAUNCH_CONTROL_YAW_ITERM_LIMIT
: 0;
1142 pidData
[FD_YAW
].I
= constrainf(pidData
[FD_YAW
].I
, -launchControlYawItermLimit
, launchControlYawItermLimit
);
1144 // for pitch-only mode we disable everything except pitch P/I
1145 if (pidRuntime
.launchControlMode
== LAUNCH_CONTROL_MODE_PITCHONLY
) {
1146 pidData
[FD_ROLL
].P
= 0;
1147 pidData
[FD_ROLL
].I
= 0;
1148 pidData
[FD_YAW
].P
= 0;
1149 // don't let I go negative (pitch backwards) as front motors are limited in the mixer
1150 pidData
[FD_PITCH
].I
= MAX(0.0f
, pidData
[FD_PITCH
].I
);
1155 // Add P boost from antiGravity when sticks are close to zero
1156 if (axis
!= FD_YAW
) {
1157 float agSetpointAttenuator
= fabsf(currentPidSetpoint
) / 50.0f
;
1158 agSetpointAttenuator
= MAX(agSetpointAttenuator
, 1.0f
);
1159 // attenuate effect if turning more than 50 deg/s, half at 100 deg/s
1160 const float antiGravityPBoost
= 1.0f
+ (pidRuntime
.antiGravityThrottleD
/ agSetpointAttenuator
) * pidRuntime
.antiGravityPGain
;
1161 pidData
[axis
].P
*= antiGravityPBoost
;
1162 if (axis
== FD_PITCH
) {
1163 DEBUG_SET(DEBUG_ANTI_GRAVITY
, 3, lrintf(antiGravityPBoost
* 1000));
1167 // calculating the PID sum
1168 const float pidSum
= pidData
[axis
].P
+ pidData
[axis
].I
+ pidData
[axis
].D
+ pidData
[axis
].F
;
1169 #ifdef USE_INTEGRATED_YAW_CONTROL
1170 if (axis
== FD_YAW
&& pidRuntime
.useIntegratedYaw
) {
1171 pidData
[axis
].Sum
+= pidSum
* pidRuntime
.dT
* 100.0f
;
1172 pidData
[axis
].Sum
-= pidData
[axis
].Sum
* pidRuntime
.integratedYawRelax
/ 100000.0f
* pidRuntime
.dT
/ 0.000125f
;
1176 pidData
[axis
].Sum
= pidSum
;
1180 // Disable PID control if at zero throttle or if gyro overflow detected
1181 // This may look very innefficient, but it is done on purpose to always show real CPU usage as in flight
1182 if (!pidRuntime
.pidStabilisationEnabled
|| gyroOverflowDetected()) {
1183 for (int axis
= FD_ROLL
; axis
<= FD_YAW
; ++axis
) {
1184 pidData
[axis
].P
= 0;
1185 pidData
[axis
].I
= 0;
1186 pidData
[axis
].D
= 0;
1187 pidData
[axis
].F
= 0;
1189 pidData
[axis
].Sum
= 0;
1191 } else if (pidRuntime
.zeroThrottleItermReset
) {
1196 bool crashRecoveryModeActive(void)
1198 return pidRuntime
.inCrashRecoveryMode
;
1201 #ifdef USE_ACRO_TRAINER
1202 void pidSetAcroTrainerState(bool newState
)
1204 if (pidRuntime
.acroTrainerActive
!= newState
) {
1206 pidAcroTrainerInit();
1208 pidRuntime
.acroTrainerActive
= newState
;
1211 #endif // USE_ACRO_TRAINER
1213 void pidSetAntiGravityState(bool newState
)
1215 if (newState
!= pidRuntime
.antiGravityEnabled
) {
1216 // reset the accelerator on state changes
1217 pidRuntime
.itermAccelerator
= 0.0f
;
1219 pidRuntime
.antiGravityEnabled
= newState
;
1222 bool pidAntiGravityEnabled(void)
1224 return pidRuntime
.antiGravityEnabled
;
1228 void dynLpfDTermUpdate(float throttle
)
1230 if (pidRuntime
.dynLpfFilter
!= DYN_LPF_NONE
) {
1232 if (pidRuntime
.dynLpfCurveExpo
> 0) {
1233 cutoffFreq
= dynLpfCutoffFreq(throttle
, pidRuntime
.dynLpfMin
, pidRuntime
.dynLpfMax
, pidRuntime
.dynLpfCurveExpo
);
1235 cutoffFreq
= fmaxf(dynThrottle(throttle
) * pidRuntime
.dynLpfMax
, pidRuntime
.dynLpfMin
);
1238 switch (pidRuntime
.dynLpfFilter
) {
1240 for (int axis
= 0; axis
< XYZ_AXIS_COUNT
; axis
++) {
1241 pt1FilterUpdateCutoff(&pidRuntime
.dtermLowpass
[axis
].pt1Filter
, pt1FilterGain(cutoffFreq
, pidRuntime
.dT
));
1244 case DYN_LPF_BIQUAD
:
1245 for (int axis
= 0; axis
< XYZ_AXIS_COUNT
; axis
++) {
1246 biquadFilterUpdateLPF(&pidRuntime
.dtermLowpass
[axis
].biquadFilter
, cutoffFreq
, targetPidLooptime
);
1250 for (int axis
= 0; axis
< XYZ_AXIS_COUNT
; axis
++) {
1251 pt2FilterUpdateCutoff(&pidRuntime
.dtermLowpass
[axis
].pt2Filter
, pt2FilterGain(cutoffFreq
, pidRuntime
.dT
));
1255 for (int axis
= 0; axis
< XYZ_AXIS_COUNT
; axis
++) {
1256 pt3FilterUpdateCutoff(&pidRuntime
.dtermLowpass
[axis
].pt3Filter
, pt3FilterGain(cutoffFreq
, pidRuntime
.dT
));
1264 float dynLpfCutoffFreq(float throttle
, uint16_t dynLpfMin
, uint16_t dynLpfMax
, uint8_t expo
)
1266 const float expof
= expo
/ 10.0f
;
1267 const float curve
= throttle
* (1 - throttle
) * expof
+ throttle
;
1268 return (dynLpfMax
- dynLpfMin
) * curve
+ dynLpfMin
;
1271 void pidSetItermReset(bool enabled
)
1273 pidRuntime
.zeroThrottleItermReset
= enabled
;
1276 float pidGetPreviousSetpoint(int axis
)
1278 return pidRuntime
.previousPidSetpoint
[axis
];
1281 float pidGetDT(void)
1283 return pidRuntime
.dT
;
1286 float pidGetPidFrequency(void)
1288 return pidRuntime
.pidFrequency
;