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"
57 #include "pg/pg_ids.h"
59 #include "sensors/acceleration.h"
60 #include "sensors/battery.h"
61 #include "sensors/gyro.h"
71 const char pidNames
[] =
78 FAST_DATA_ZERO_INIT
uint32_t targetPidLooptime
;
79 FAST_DATA_ZERO_INIT pidAxisData_t pidData
[XYZ_AXIS_COUNT
];
80 FAST_DATA_ZERO_INIT pidRuntime_t pidRuntime
;
82 #if defined(USE_ABSOLUTE_CONTROL)
83 STATIC_UNIT_TESTED FAST_DATA_ZERO_INIT
float axisError
[XYZ_AXIS_COUNT
];
86 #if defined(USE_THROTTLE_BOOST)
87 FAST_DATA_ZERO_INIT
float throttleBoost
;
88 pt1Filter_t throttleLpf
;
91 PG_REGISTER_WITH_RESET_TEMPLATE(pidConfig_t
, pidConfig
, PG_PID_CONFIG
, 3);
93 #if !defined(DEFAULT_PID_PROCESS_DENOM)
94 #if defined(STM32F411xE)
95 #define DEFAULT_PID_PROCESS_DENOM 2
97 #define DEFAULT_PID_PROCESS_DENOM 1
101 #ifdef USE_RUNAWAY_TAKEOFF
102 PG_RESET_TEMPLATE(pidConfig_t
, pidConfig
,
103 .pid_process_denom
= DEFAULT_PID_PROCESS_DENOM
,
104 .runaway_takeoff_prevention
= true,
105 .runaway_takeoff_deactivate_throttle
= 20, // throttle level % needed to accumulate deactivation time
106 .runaway_takeoff_deactivate_delay
= 500, // Accumulated time (in milliseconds) before deactivation in successful takeoff
109 PG_RESET_TEMPLATE(pidConfig_t
, pidConfig
,
110 .pid_process_denom
= DEFAULT_PID_PROCESS_DENOM
,
114 #ifdef USE_ACRO_TRAINER
115 #define ACRO_TRAINER_LOOKAHEAD_RATE_LIMIT 500.0f // Max gyro rate for lookahead time scaling
116 #define ACRO_TRAINER_SETPOINT_LIMIT 1000.0f // Limit the correcting setpoint
117 #endif // USE_ACRO_TRAINER
119 #define CRASH_RECOVERY_DETECTION_DELAY_US 1000000 // 1 second delay before crash recovery detection is active after entering a self-level mode
121 #define LAUNCH_CONTROL_YAW_ITERM_LIMIT 50 // yaw iterm windup limit when launch mode is "FULL" (all axes)
123 PG_REGISTER_ARRAY_WITH_RESET_FN(pidProfile_t
, PID_PROFILE_COUNT
, pidProfiles
, PG_PID_PROFILE
, 7);
125 void resetPidProfile(pidProfile_t
*pidProfile
)
127 RESET_CONFIG(pidProfile_t
, pidProfile
,
129 [PID_ROLL
] = PID_ROLL_DEFAULT
,
130 [PID_PITCH
] = PID_PITCH_DEFAULT
,
131 [PID_YAW
] = PID_YAW_DEFAULT
,
132 [PID_LEVEL
] = { 50, 75, 75, 50 },
133 [PID_MAG
] = { 40, 0, 0, 0 },
135 .pidSumLimit
= PIDSUM_LIMIT
,
136 .pidSumLimitYaw
= PIDSUM_LIMIT_YAW
,
137 .yaw_lowpass_hz
= 100,
139 .dterm_notch_cutoff
= 0,
140 .itermWindupPointPercent
= 85,
141 .pidAtMinThrottle
= PID_STABILISATION_ON
,
143 .feedforward_transition
= 0,
144 .yawRateAccelLimit
= 0,
146 .anti_gravity_gain
= 80,
147 .crash_time
= 500, // ms
148 .crash_delay
= 0, // ms
149 .crash_recovery_angle
= 10, // degrees
150 .crash_recovery_rate
= 100, // degrees/second
151 .crash_dthreshold
= 50, // degrees/second/second
152 .crash_gthreshold
= 400, // degrees/second
153 .crash_setpoint_threshold
= 350, // degrees/second
154 .crash_recovery
= PID_CRASH_RECOVERY_OFF
, // off by default
155 .horizon_limit_degrees
= 135,
156 .horizon_ignore_sticks
= false,
157 .crash_limit_yaw
= 200,
160 .throttle_boost_cutoff
= 15,
161 .iterm_rotation
= false,
162 .iterm_relax
= ITERM_RELAX_RP
,
163 .iterm_relax_cutoff
= ITERM_RELAX_CUTOFF_DEFAULT
,
164 .iterm_relax_type
= ITERM_RELAX_SETPOINT
,
165 .acro_trainer_angle_limit
= 20,
166 .acro_trainer_lookahead_ms
= 50,
167 .acro_trainer_debug_axis
= FD_ROLL
,
168 .acro_trainer_gain
= 75,
169 .abs_control_gain
= 0,
170 .abs_control_limit
= 90,
171 .abs_control_error_limit
= 20,
172 .abs_control_cutoff
= 11,
173 .dterm_lpf1_static_hz
= DTERM_LPF1_DYN_MIN_HZ_DEFAULT
,
174 // NOTE: dynamic lpf is enabled by default so this setting is actually
175 // overridden and the static lowpass 1 is disabled. We can't set this
176 // value to 0 otherwise Configurator versions 10.4 and earlier will also
177 // reset the lowpass filter type to PT1 overriding the desired BIQUAD setting.
178 .dterm_lpf2_static_hz
= DTERM_LPF2_HZ_DEFAULT
, // second Dterm LPF ON by default
179 .dterm_lpf1_type
= FILTER_PT1
,
180 .dterm_lpf2_type
= FILTER_PT1
,
181 .dterm_lpf1_dyn_min_hz
= DTERM_LPF1_DYN_MIN_HZ_DEFAULT
,
182 .dterm_lpf1_dyn_max_hz
= DTERM_LPF1_DYN_MAX_HZ_DEFAULT
,
183 .launchControlMode
= LAUNCH_CONTROL_MODE_NORMAL
,
184 .launchControlThrottlePercent
= 20,
185 .launchControlAngleLimit
= 0,
186 .launchControlGain
= 40,
187 .launchControlAllowTriggerReset
= true,
188 .use_integrated_yaw
= false,
189 .integrated_yaw_relax
= 200,
190 .thrustLinearization
= 0,
191 .d_min
= D_MIN_DEFAULT
,
194 .motor_output_limit
= 100,
195 .auto_profile_cell_count
= AUTO_PROFILE_CELL_COUNT_STAY
,
196 .transient_throttle_limit
= 0,
197 .profileName
= { 0 },
198 .dyn_idle_min_rpm
= 0,
199 .dyn_idle_p_gain
= 50,
200 .dyn_idle_i_gain
= 50,
201 .dyn_idle_d_gain
= 50,
202 .dyn_idle_max_increase
= 150,
203 .dyn_idle_start_increase
= 50,
204 .feedforward_averaging
= FEEDFORWARD_AVERAGING_OFF
,
205 .feedforward_max_rate_limit
= 90,
206 .feedforward_smooth_factor
= 25,
207 .feedforward_jitter_factor
= 7,
208 .feedforward_boost
= 15,
209 .dterm_lpf1_dyn_expo
= 5,
210 .level_race_mode
= false,
211 .vbat_sag_compensation
= 0,
212 .simplified_pids_mode
= PID_SIMPLIFIED_TUNING_RPY
,
213 .simplified_master_multiplier
= SIMPLIFIED_TUNING_DEFAULT
,
214 .simplified_roll_pitch_ratio
= SIMPLIFIED_TUNING_DEFAULT
,
215 .simplified_i_gain
= SIMPLIFIED_TUNING_DEFAULT
,
216 .simplified_d_gain
= SIMPLIFIED_TUNING_D_DEFAULT
,
217 .simplified_pi_gain
= SIMPLIFIED_TUNING_DEFAULT
,
218 .simplified_dmin_ratio
= SIMPLIFIED_TUNING_D_DEFAULT
,
219 .simplified_feedforward_gain
= SIMPLIFIED_TUNING_DEFAULT
,
220 .simplified_pitch_pi_gain
= SIMPLIFIED_TUNING_DEFAULT
,
221 .simplified_dterm_filter
= true,
222 .simplified_dterm_filter_multiplier
= SIMPLIFIED_TUNING_DEFAULT
,
223 .anti_gravity_cutoff_hz
= 5,
224 .anti_gravity_p_gain
= 100,
225 .tpa_mode
= TPA_MODE_D
,
227 .tpa_breakpoint
= 1350,
228 .angle_feedforward_smoothing_ms
= 80,
229 .angle_earth_ref
= 100,
230 .horizon_delay_ms
= 500, // 500ms time constant on any increase in horizon strength
234 pidProfile
->pid
[PID_ROLL
].D
= 30;
235 pidProfile
->pid
[PID_PITCH
].D
= 32;
239 void pgResetFn_pidProfiles(pidProfile_t
*pidProfiles
)
241 for (int i
= 0; i
< PID_PROFILE_COUNT
; i
++) {
242 resetPidProfile(&pidProfiles
[i
]);
246 // Scale factors to make best use of range with D_LPF debugging, aiming for max +/-16K as debug values are 16 bit
247 #define D_LPF_RAW_SCALE 25
248 #define D_LPF_FILT_SCALE 22
251 void pidSetItermAccelerator(float newItermAccelerator
)
253 pidRuntime
.itermAccelerator
= newItermAccelerator
;
256 bool pidOsdAntiGravityActive(void)
258 return (pidRuntime
.itermAccelerator
> pidRuntime
.antiGravityOsdCutoff
);
261 void pidStabilisationState(pidStabilisationState_e pidControllerState
)
263 pidRuntime
.pidStabilisationEnabled
= (pidControllerState
== PID_STABILISATION_ON
) ? true : false;
266 const angle_index_t rcAliasToAngleIndexMap
[] = { AI_ROLL
, AI_PITCH
};
268 void pidResetIterm(void)
270 for (int axis
= 0; axis
< 3; axis
++) {
271 pidData
[axis
].I
= 0.0f
;
272 #if defined(USE_ABSOLUTE_CONTROL)
273 axisError
[axis
] = 0.0f
;
278 void pidUpdateTpaFactor(float throttle
)
280 pidProfile_t
*currentPidProfile
;
282 currentPidProfile
= pidProfilesMutable(systemConfig()->pidProfileIndex
);
283 const float tpaBreakpoint
= (currentPidProfile
->tpa_breakpoint
- 1000) / 1000.0f
;
284 float tpaRate
= currentPidProfile
->tpa_rate
/ 100.0f
;
286 if (throttle
> tpaBreakpoint
) {
287 if (throttle
< 1.0f
) {
288 tpaRate
*= (throttle
- tpaBreakpoint
) / (1.0f
- tpaBreakpoint
);
293 pidRuntime
.tpaFactor
= 1.0f
- tpaRate
;
296 void pidUpdateAntiGravityThrottleFilter(float throttle
)
298 static float previousThrottle
= 0.0f
;
299 const float throttleInv
= 1.0f
- throttle
;
300 float throttleDerivative
= fabsf(throttle
- previousThrottle
) * pidRuntime
.pidFrequency
;
301 DEBUG_SET(DEBUG_ANTI_GRAVITY
, 0, lrintf(throttleDerivative
* 100));
302 throttleDerivative
*= throttleInv
* throttleInv
;
303 // generally focus on the low throttle period
304 if (throttle
> previousThrottle
) {
305 throttleDerivative
*= throttleInv
* 0.5f
;
306 // when increasing throttle, focus even more on the low throttle range
308 previousThrottle
= throttle
;
309 throttleDerivative
= pt2FilterApply(&pidRuntime
.antiGravityLpf
, throttleDerivative
);
310 // lower cutoff suppresses peaks relative to troughs and prolongs the effects
311 // PT2 smoothing of throttle derivative.
312 // 6 is a typical value for the peak boost factor with default cutoff of 6Hz
313 DEBUG_SET(DEBUG_ANTI_GRAVITY
, 1, lrintf(throttleDerivative
* 100));
314 pidRuntime
.antiGravityThrottleD
= throttleDerivative
;
317 #ifdef USE_ACRO_TRAINER
318 void pidAcroTrainerInit(void)
320 pidRuntime
.acroTrainerAxisState
[FD_ROLL
] = 0;
321 pidRuntime
.acroTrainerAxisState
[FD_PITCH
] = 0;
323 #endif // USE_ACRO_TRAINER
325 #ifdef USE_THRUST_LINEARIZATION
326 float pidCompensateThrustLinearization(float throttle
)
328 if (pidRuntime
.thrustLinearization
!= 0.0f
) {
329 // for whoops where a lot of TL is needed, allow more throttle boost
330 const float throttleReversed
= (1.0f
- throttle
);
331 throttle
/= 1.0f
+ pidRuntime
.throttleCompensateAmount
* sq(throttleReversed
);
336 float pidApplyThrustLinearization(float motorOutput
)
338 if (pidRuntime
.thrustLinearization
!= 0.0f
) {
339 if (motorOutput
> 0.0f
) {
340 const float motorOutputReversed
= (1.0f
- motorOutput
);
341 motorOutput
*= 1.0f
+ sq(motorOutputReversed
) * pidRuntime
.thrustLinearization
;
349 // Calculate strength of horizon leveling; 0 = none, 1.0 = most leveling
350 STATIC_UNIT_TESTED FAST_CODE_NOINLINE
float calcHorizonLevelStrength(void)
352 const float currentInclination
= MAX(abs(attitude
.values
.roll
), abs(attitude
.values
.pitch
)) * 0.1f
;
353 // 0 when level, 90 when vertical, 180 when inverted (degrees):
354 float absMaxStickDeflection
= MAX(fabsf(getRcDeflection(FD_ROLL
)), fabsf(getRcDeflection(FD_PITCH
)));
355 // 0-1, smoothed if RC smoothing is enabled
357 float horizonLevelStrength
= MAX((pidRuntime
.horizonLimitDegrees
- currentInclination
) * pidRuntime
.horizonLimitDegreesInv
, 0.0f
);
358 // 1.0 when attitude is 'flat', 0 when angle is equal to, or greater than, horizonLimitDegrees
359 horizonLevelStrength
*= MAX((pidRuntime
.horizonLimitSticks
- absMaxStickDeflection
) * pidRuntime
.horizonLimitSticksInv
, pidRuntime
.horizonIgnoreSticks
);
360 // use the value of horizonIgnoreSticks to enable/disable this effect.
361 // value should be 1.0 at center stick, 0.0 at max stick deflection:
362 horizonLevelStrength
*= pidRuntime
.horizonGain
;
364 if (pidRuntime
.horizonDelayMs
) {
365 const float horizonLevelStrengthSmoothed
= pt1FilterApply(&pidRuntime
.horizonSmoothingPt1
, horizonLevelStrength
);
366 horizonLevelStrength
= MIN(horizonLevelStrength
, horizonLevelStrengthSmoothed
);
368 return horizonLevelStrength
;
369 // 1 means full levelling, 0 means none
372 // Use the FAST_CODE_NOINLINE directive to avoid this code from being inlined into ITCM RAM to avoid overflow.
373 // The impact is possibly slightly slower performance on F7/H7 but they have more than enough
374 // processing power that it should be a non-issue.
375 STATIC_UNIT_TESTED FAST_CODE_NOINLINE
float pidLevel(int axis
, const pidProfile_t
*pidProfile
, const rollAndPitchTrims_t
*angleTrim
,
376 float currentPidSetpoint
, float horizonLevelStrength
)
378 // Applies only to axes that are in Angle mode
379 // We now use Acro Rates, transformed into the range +/- 1, to provide setpoints
380 const float angleLimit
= pidProfile
->angle_limit
;
381 float angleFeedforward
= 0.0f
;
383 #ifdef USE_FEEDFORWARD
384 angleFeedforward
= angleLimit
* getFeedforward(axis
) * pidRuntime
.angleFeedforwardGain
* pidRuntime
.maxRcRateInv
[axis
];
385 // angle feedforward must be heavily filtered, at the PID loop rate, with limited user control over time constant
386 // it MUST be very delayed to avoid early overshoot and being too aggressive
387 angleFeedforward
= pt3FilterApply(&pidRuntime
.angleFeedforwardPt3
[axis
], angleFeedforward
);
390 float angleTarget
= angleLimit
* currentPidSetpoint
* pidRuntime
.maxRcRateInv
[axis
];
391 // use acro rates for the angle target in both horizon and angle modes, converted to -1 to +1 range using maxRate
393 #ifdef USE_GPS_RESCUE
394 angleTarget
+= gpsRescueAngle
[axis
] / 100.0f
; // Angle is in centidegrees, stepped on roll at 10Hz but not on pitch
396 const float currentAngle
= (attitude
.raw
[axis
] - angleTrim
->raw
[axis
]) / 10.0f
; // stepped at 500hz with some 4ms flat spots
397 const float errorAngle
= angleTarget
- currentAngle
;
398 float angleRate
= errorAngle
* pidRuntime
.angleGain
+ angleFeedforward
;
400 // minimise cross-axis wobble due to faster yaw responses than roll or pitch, and make co-ordinated yaw turns
401 // by compensating for the effect of yaw on roll while pitched, and on pitch while rolled
402 // earthRef code here takes about 76 cycles, if conditional on angleEarthRef it takes about 100. sin_approx costs most of those cycles.
403 float sinAngle
= sin_approx(DEGREES_TO_RADIANS(pidRuntime
.angleTarget
[axis
== FD_ROLL
? FD_PITCH
: FD_ROLL
]));
404 sinAngle
*= (axis
== FD_ROLL
) ? -1.0f
: 1.0f
; // must be negative for Roll
405 angleRate
+= pidRuntime
.angleYawSetpoint
* sinAngle
* pidRuntime
.angleEarthRef
;
406 pidRuntime
.angleTarget
[axis
] = angleTarget
; // set target for alternate axis to current axis, for use in preceding calculation
408 // smooth final angle rate output to clean up attitude signal steps (500hz), GPS steps (10 or 100hz), RC steps etc
409 // this filter runs at ATTITUDE_CUTOFF_HZ, currently 50hz, so GPS roll may be a bit steppy
410 angleRate
= pt3FilterApply(&pidRuntime
.attitudeFilter
[axis
], angleRate
);
412 if (FLIGHT_MODE(ANGLE_MODE
) || FLIGHT_MODE(GPS_RESCUE_MODE
)) {
413 currentPidSetpoint
= angleRate
;
415 // can only be HORIZON mode - crossfade Angle rate and Acro rate
416 currentPidSetpoint
= currentPidSetpoint
* (1.0f
- horizonLevelStrength
) + angleRate
* horizonLevelStrength
;
420 if (axis
== FD_ROLL
) {
421 DEBUG_SET(DEBUG_ANGLE_MODE
, 0, lrintf(angleTarget
* 10.0f
)); // target angle
422 DEBUG_SET(DEBUG_ANGLE_MODE
, 1, lrintf(errorAngle
* pidRuntime
.angleGain
* 10.0f
)); // un-smoothed error correction in degrees
423 DEBUG_SET(DEBUG_ANGLE_MODE
, 2, lrintf(angleFeedforward
* 10.0f
)); // feedforward amount in degrees
424 DEBUG_SET(DEBUG_ANGLE_MODE
, 3, lrintf(currentAngle
* 10.0f
)); // angle returned
426 DEBUG_SET(DEBUG_ANGLE_TARGET
, 0, lrintf(angleTarget
* 10.0f
));
427 DEBUG_SET(DEBUG_ANGLE_TARGET
, 1, lrintf(sinAngle
* 10.0f
)); // modification factor from earthRef
428 // debug ANGLE_TARGET 2 is yaw attenuation
429 DEBUG_SET(DEBUG_ANGLE_TARGET
, 3, lrintf(currentAngle
* 10.0f
)); // angle returned
432 DEBUG_SET(DEBUG_CURRENT_ANGLE
, axis
, lrintf(currentAngle
* 10.0f
)); // current angle
433 return currentPidSetpoint
;
436 static FAST_CODE_NOINLINE
void handleCrashRecovery(
437 const pidCrashRecovery_e crash_recovery
, const rollAndPitchTrims_t
*angleTrim
,
438 const int axis
, const timeUs_t currentTimeUs
, const float gyroRate
, float *currentPidSetpoint
, float *errorRate
)
440 if (pidRuntime
.inCrashRecoveryMode
&& cmpTimeUs(currentTimeUs
, pidRuntime
.crashDetectedAtUs
) > pidRuntime
.crashTimeDelayUs
) {
441 if (crash_recovery
== PID_CRASH_RECOVERY_BEEP
) {
444 if (axis
== FD_YAW
) {
445 *errorRate
= constrainf(*errorRate
, -pidRuntime
.crashLimitYaw
, pidRuntime
.crashLimitYaw
);
447 // on roll and pitch axes calculate currentPidSetpoint and errorRate to level the aircraft to recover from crash
448 if (sensors(SENSOR_ACC
)) {
449 // errorAngle is deviation from horizontal
450 const float errorAngle
= -(attitude
.raw
[axis
] - angleTrim
->raw
[axis
]) / 10.0f
;
451 *currentPidSetpoint
= errorAngle
* pidRuntime
.angleGain
;
452 *errorRate
= *currentPidSetpoint
- gyroRate
;
455 // reset iterm, since accumulated error before crash is now meaningless
456 // and iterm windup during crash recovery can be extreme, especially on yaw axis
457 pidData
[axis
].I
= 0.0f
;
458 if (cmpTimeUs(currentTimeUs
, pidRuntime
.crashDetectedAtUs
) > pidRuntime
.crashTimeLimitUs
459 || (getMotorMixRange() < 1.0f
460 && fabsf(gyro
.gyroADCf
[FD_ROLL
]) < pidRuntime
.crashRecoveryRate
461 && fabsf(gyro
.gyroADCf
[FD_PITCH
]) < pidRuntime
.crashRecoveryRate
462 && fabsf(gyro
.gyroADCf
[FD_YAW
]) < pidRuntime
.crashRecoveryRate
)) {
463 if (sensors(SENSOR_ACC
)) {
464 // check aircraft nearly level
465 if (abs(attitude
.raw
[FD_ROLL
] - angleTrim
->raw
[FD_ROLL
]) < pidRuntime
.crashRecoveryAngleDeciDegrees
466 && abs(attitude
.raw
[FD_PITCH
] - angleTrim
->raw
[FD_PITCH
]) < pidRuntime
.crashRecoveryAngleDeciDegrees
) {
467 pidRuntime
.inCrashRecoveryMode
= false;
471 pidRuntime
.inCrashRecoveryMode
= false;
478 static FAST_CODE_NOINLINE
void detectAndSetCrashRecovery(
479 const pidCrashRecovery_e crash_recovery
, const int axis
,
480 const timeUs_t currentTimeUs
, const float delta
, const float errorRate
)
482 // if crash recovery is on and accelerometer enabled and there is no gyro overflow, then check for a crash
483 // no point in trying to recover if the crash is so severe that the gyro overflows
484 if ((crash_recovery
|| FLIGHT_MODE(GPS_RESCUE_MODE
)) && !gyroOverflowDetected()) {
485 if (ARMING_FLAG(ARMED
)) {
486 if (getMotorMixRange() >= 1.0f
&& !pidRuntime
.inCrashRecoveryMode
487 && fabsf(delta
) > pidRuntime
.crashDtermThreshold
488 && fabsf(errorRate
) > pidRuntime
.crashGyroThreshold
489 && fabsf(getSetpointRate(axis
)) < pidRuntime
.crashSetpointThreshold
) {
490 if (crash_recovery
== PID_CRASH_RECOVERY_DISARM
) {
491 setArmingDisabled(ARMING_DISABLED_CRASH_DETECTED
);
492 disarm(DISARM_REASON_CRASH_PROTECTION
);
494 pidRuntime
.inCrashRecoveryMode
= true;
495 pidRuntime
.crashDetectedAtUs
= currentTimeUs
;
498 if (pidRuntime
.inCrashRecoveryMode
&& cmpTimeUs(currentTimeUs
, pidRuntime
.crashDetectedAtUs
) < pidRuntime
.crashTimeDelayUs
&& (fabsf(errorRate
) < pidRuntime
.crashGyroThreshold
499 || fabsf(getSetpointRate(axis
)) > pidRuntime
.crashSetpointThreshold
)) {
500 pidRuntime
.inCrashRecoveryMode
= false;
503 } else if (pidRuntime
.inCrashRecoveryMode
) {
504 pidRuntime
.inCrashRecoveryMode
= false;
511 #ifdef USE_ACRO_TRAINER
513 int acroTrainerSign(float x
)
515 return x
> 0 ? 1 : -1;
518 // Acro Trainer - Manipulate the setPoint to limit axis angle while in acro mode
519 // There are three states:
520 // 1. Current angle has exceeded limit
521 // Apply correction to return to limit (similar to pidLevel)
522 // 2. Future overflow has been projected based on current angle and gyro rate
523 // Manage the setPoint to control the gyro rate as the actual angle approaches the limit (try to prevent overshoot)
524 // 3. If no potential overflow is detected, then return the original setPoint
526 // Use the FAST_CODE_NOINLINE directive to avoid this code from being inlined into ITCM RAM. We accept the
527 // performance decrease when Acro Trainer mode is active under the assumption that user is unlikely to be
528 // expecting ultimate flight performance at very high loop rates when in this mode.
529 static FAST_CODE_NOINLINE
float applyAcroTrainer(int axis
, const rollAndPitchTrims_t
*angleTrim
, float setPoint
)
531 float ret
= setPoint
;
533 if (!FLIGHT_MODE(ANGLE_MODE
) && !FLIGHT_MODE(HORIZON_MODE
) && !FLIGHT_MODE(GPS_RESCUE_MODE
)) {
534 bool resetIterm
= false;
535 float projectedAngle
= 0;
536 const int setpointSign
= acroTrainerSign(setPoint
);
537 const float currentAngle
= (attitude
.raw
[axis
] - angleTrim
->raw
[axis
]) / 10.0f
;
538 const int angleSign
= acroTrainerSign(currentAngle
);
540 if ((pidRuntime
.acroTrainerAxisState
[axis
] != 0) && (pidRuntime
.acroTrainerAxisState
[axis
] != setpointSign
)) { // stick has reversed - stop limiting
541 pidRuntime
.acroTrainerAxisState
[axis
] = 0;
544 // Limit and correct the angle when it exceeds the limit
545 if ((fabsf(currentAngle
) > pidRuntime
.acroTrainerAngleLimit
) && (pidRuntime
.acroTrainerAxisState
[axis
] == 0)) {
546 if (angleSign
== setpointSign
) {
547 pidRuntime
.acroTrainerAxisState
[axis
] = angleSign
;
552 if (pidRuntime
.acroTrainerAxisState
[axis
] != 0) {
553 ret
= constrainf(((pidRuntime
.acroTrainerAngleLimit
* angleSign
) - currentAngle
) * pidRuntime
.acroTrainerGain
, -ACRO_TRAINER_SETPOINT_LIMIT
, ACRO_TRAINER_SETPOINT_LIMIT
);
556 // Not currently over the limit so project the angle based on current angle and
557 // gyro angular rate using a sliding window based on gyro rate (faster rotation means larger window.
558 // If the projected angle exceeds the limit then apply limiting to minimize overshoot.
559 // Calculate the lookahead window by scaling proportionally with gyro rate from 0-500dps
560 float checkInterval
= constrainf(fabsf(gyro
.gyroADCf
[axis
]) / ACRO_TRAINER_LOOKAHEAD_RATE_LIMIT
, 0.0f
, 1.0f
) * pidRuntime
.acroTrainerLookaheadTime
;
561 projectedAngle
= (gyro
.gyroADCf
[axis
] * checkInterval
) + currentAngle
;
562 const int projectedAngleSign
= acroTrainerSign(projectedAngle
);
563 if ((fabsf(projectedAngle
) > pidRuntime
.acroTrainerAngleLimit
) && (projectedAngleSign
== setpointSign
)) {
564 ret
= ((pidRuntime
.acroTrainerAngleLimit
* projectedAngleSign
) - projectedAngle
) * pidRuntime
.acroTrainerGain
;
573 if (axis
== pidRuntime
.acroTrainerDebugAxis
) {
574 DEBUG_SET(DEBUG_ACRO_TRAINER
, 0, lrintf(currentAngle
* 10.0f
));
575 DEBUG_SET(DEBUG_ACRO_TRAINER
, 1, pidRuntime
.acroTrainerAxisState
[axis
]);
576 DEBUG_SET(DEBUG_ACRO_TRAINER
, 2, lrintf(ret
));
577 DEBUG_SET(DEBUG_ACRO_TRAINER
, 3, lrintf(projectedAngle
* 10.0f
));
583 #endif // USE_ACRO_TRAINER
585 static float accelerationLimit(int axis
, float currentPidSetpoint
)
587 static float previousSetpoint
[XYZ_AXIS_COUNT
];
588 const float currentVelocity
= currentPidSetpoint
- previousSetpoint
[axis
];
590 if (fabsf(currentVelocity
) > pidRuntime
.maxVelocity
[axis
]) {
591 currentPidSetpoint
= (currentVelocity
> 0) ? previousSetpoint
[axis
] + pidRuntime
.maxVelocity
[axis
] : previousSetpoint
[axis
] - pidRuntime
.maxVelocity
[axis
];
594 previousSetpoint
[axis
] = currentPidSetpoint
;
595 return currentPidSetpoint
;
598 static void rotateVector(float v
[XYZ_AXIS_COUNT
], float rotation
[XYZ_AXIS_COUNT
])
600 // rotate v around rotation vector rotation
601 // rotation in radians, all elements must be small
602 for (int i
= 0; i
< XYZ_AXIS_COUNT
; i
++) {
603 int i_1
= (i
+ 1) % 3;
604 int i_2
= (i
+ 2) % 3;
605 float newV
= v
[i_1
] + v
[i_2
] * rotation
[i
];
606 v
[i_2
] -= v
[i_1
] * rotation
[i
];
611 STATIC_UNIT_TESTED
void rotateItermAndAxisError(void)
613 if (pidRuntime
.itermRotation
614 #if defined(USE_ABSOLUTE_CONTROL)
615 || pidRuntime
.acGain
> 0 || debugMode
== DEBUG_AC_ERROR
618 const float gyroToAngle
= pidRuntime
.dT
* RAD
;
619 float rotationRads
[XYZ_AXIS_COUNT
];
620 for (int i
= FD_ROLL
; i
<= FD_YAW
; i
++) {
621 rotationRads
[i
] = gyro
.gyroADCf
[i
] * gyroToAngle
;
623 #if defined(USE_ABSOLUTE_CONTROL)
624 if (pidRuntime
.acGain
> 0 || debugMode
== DEBUG_AC_ERROR
) {
625 rotateVector(axisError
, rotationRads
);
628 if (pidRuntime
.itermRotation
) {
629 float v
[XYZ_AXIS_COUNT
];
630 for (int i
= 0; i
< XYZ_AXIS_COUNT
; i
++) {
633 rotateVector(v
, rotationRads
);
634 for (int i
= 0; i
< XYZ_AXIS_COUNT
; i
++) {
641 #if defined(USE_ITERM_RELAX)
642 #if defined(USE_ABSOLUTE_CONTROL)
643 STATIC_UNIT_TESTED
void applyAbsoluteControl(const int axis
, const float gyroRate
, float *currentPidSetpoint
, float *itermErrorRate
)
645 if (pidRuntime
.acGain
> 0 || debugMode
== DEBUG_AC_ERROR
) {
646 const float setpointLpf
= pt1FilterApply(&pidRuntime
.acLpf
[axis
], *currentPidSetpoint
);
647 const float setpointHpf
= fabsf(*currentPidSetpoint
- setpointLpf
);
648 float acErrorRate
= 0;
649 const float gmaxac
= setpointLpf
+ 2 * setpointHpf
;
650 const float gminac
= setpointLpf
- 2 * setpointHpf
;
651 if (gyroRate
>= gminac
&& gyroRate
<= gmaxac
) {
652 const float acErrorRate1
= gmaxac
- gyroRate
;
653 const float acErrorRate2
= gminac
- gyroRate
;
654 if (acErrorRate1
* axisError
[axis
] < 0) {
655 acErrorRate
= acErrorRate1
;
657 acErrorRate
= acErrorRate2
;
659 if (fabsf(acErrorRate
* pidRuntime
.dT
) > fabsf(axisError
[axis
]) ) {
660 acErrorRate
= -axisError
[axis
] * pidRuntime
.pidFrequency
;
663 acErrorRate
= (gyroRate
> gmaxac
? gmaxac
: gminac
) - gyroRate
;
666 if (isAirmodeActivated()) {
667 axisError
[axis
] = constrainf(axisError
[axis
] + acErrorRate
* pidRuntime
.dT
,
668 -pidRuntime
.acErrorLimit
, pidRuntime
.acErrorLimit
);
669 const float acCorrection
= constrainf(axisError
[axis
] * pidRuntime
.acGain
, -pidRuntime
.acLimit
, pidRuntime
.acLimit
);
670 *currentPidSetpoint
+= acCorrection
;
671 *itermErrorRate
+= acCorrection
;
672 DEBUG_SET(DEBUG_AC_CORRECTION
, axis
, lrintf(acCorrection
* 10));
673 if (axis
== FD_ROLL
) {
674 DEBUG_SET(DEBUG_ITERM_RELAX
, 3, lrintf(acCorrection
* 10));
677 DEBUG_SET(DEBUG_AC_ERROR
, axis
, lrintf(axisError
[axis
] * 10));
682 STATIC_UNIT_TESTED
void applyItermRelax(const int axis
, const float iterm
,
683 const float gyroRate
, float *itermErrorRate
, float *currentPidSetpoint
)
685 const float setpointLpf
= pt1FilterApply(&pidRuntime
.windupLpf
[axis
], *currentPidSetpoint
);
686 const float setpointHpf
= fabsf(*currentPidSetpoint
- setpointLpf
);
688 if (pidRuntime
.itermRelax
) {
689 if (axis
< FD_YAW
|| pidRuntime
.itermRelax
== ITERM_RELAX_RPY
|| pidRuntime
.itermRelax
== ITERM_RELAX_RPY_INC
) {
690 float itermRelaxThreshold
= ITERM_RELAX_SETPOINT_THRESHOLD
;
691 if (FLIGHT_MODE(ANGLE_MODE
)) {
692 itermRelaxThreshold
*= 0.2f
;
694 const float itermRelaxFactor
= MAX(0, 1 - setpointHpf
/ itermRelaxThreshold
);
695 const bool isDecreasingI
=
696 ((iterm
> 0) && (*itermErrorRate
< 0)) || ((iterm
< 0) && (*itermErrorRate
> 0));
697 if ((pidRuntime
.itermRelax
>= ITERM_RELAX_RP_INC
) && isDecreasingI
) {
698 // Do Nothing, use the precalculed itermErrorRate
699 } else if (pidRuntime
.itermRelaxType
== ITERM_RELAX_SETPOINT
) {
700 *itermErrorRate
*= itermRelaxFactor
;
701 } else if (pidRuntime
.itermRelaxType
== ITERM_RELAX_GYRO
) {
702 *itermErrorRate
= fapplyDeadband(setpointLpf
- gyroRate
, setpointHpf
);
704 *itermErrorRate
= 0.0f
;
707 if (axis
== FD_ROLL
) {
708 DEBUG_SET(DEBUG_ITERM_RELAX
, 0, lrintf(setpointHpf
));
709 DEBUG_SET(DEBUG_ITERM_RELAX
, 1, lrintf(itermRelaxFactor
* 100.0f
));
710 DEBUG_SET(DEBUG_ITERM_RELAX
, 2, lrintf(*itermErrorRate
));
714 #if defined(USE_ABSOLUTE_CONTROL)
715 applyAbsoluteControl(axis
, gyroRate
, currentPidSetpoint
, itermErrorRate
);
721 #ifdef USE_AIRMODE_LPF
722 void pidUpdateAirmodeLpf(float currentOffset
)
724 if (pidRuntime
.airmodeThrottleOffsetLimit
== 0.0f
) {
728 float offsetHpf
= currentOffset
* 2.5f
;
729 offsetHpf
= offsetHpf
- pt1FilterApply(&pidRuntime
.airmodeThrottleLpf2
, offsetHpf
);
731 // During high frequency oscillation 2 * currentOffset averages to the offset required to avoid mirroring of the waveform
732 pt1FilterApply(&pidRuntime
.airmodeThrottleLpf1
, offsetHpf
);
733 // Bring offset up immediately so the filter only applies to the decline
734 if (currentOffset
* pidRuntime
.airmodeThrottleLpf1
.state
>= 0 && fabsf(currentOffset
) > pidRuntime
.airmodeThrottleLpf1
.state
) {
735 pidRuntime
.airmodeThrottleLpf1
.state
= currentOffset
;
737 pidRuntime
.airmodeThrottleLpf1
.state
= constrainf(pidRuntime
.airmodeThrottleLpf1
.state
, -pidRuntime
.airmodeThrottleOffsetLimit
, pidRuntime
.airmodeThrottleOffsetLimit
);
740 float pidGetAirmodeThrottleOffset(void)
742 return pidRuntime
.airmodeThrottleLpf1
.state
;
746 #ifdef USE_LAUNCH_CONTROL
747 #define LAUNCH_CONTROL_MAX_RATE 100.0f
748 #define LAUNCH_CONTROL_MIN_RATE 5.0f
749 #define LAUNCH_CONTROL_ANGLE_WINDOW 10.0f // The remaining angle degrees where rate dampening starts
751 // Use the FAST_CODE_NOINLINE directive to avoid this code from being inlined into ITCM RAM to avoid overflow.
752 // The impact is possibly slightly slower performance on F7/H7 but they have more than enough
753 // processing power that it should be a non-issue.
754 static FAST_CODE_NOINLINE
float applyLaunchControl(int axis
, const rollAndPitchTrims_t
*angleTrim
)
758 // Scale the rates based on stick deflection only. Fixed rates with a max of 100deg/sec
759 // reached at 50% stick deflection. This keeps the launch control positioning consistent
760 // regardless of the user's rates.
761 if ((axis
== FD_PITCH
) || (pidRuntime
.launchControlMode
!= LAUNCH_CONTROL_MODE_PITCHONLY
)) {
762 const float stickDeflection
= constrainf(getRcDeflection(axis
), -0.5f
, 0.5f
);
763 ret
= LAUNCH_CONTROL_MAX_RATE
* stickDeflection
* 2;
767 // If ACC is enabled and a limit angle is set, then try to limit forward tilt
768 // to that angle and slow down the rate as the limit is approached to reduce overshoot
769 if ((axis
== FD_PITCH
) && (pidRuntime
.launchControlAngleLimit
> 0) && (ret
> 0)) {
770 const float currentAngle
= (attitude
.raw
[axis
] - angleTrim
->raw
[axis
]) / 10.0f
;
771 if (currentAngle
>= pidRuntime
.launchControlAngleLimit
) {
774 //for the last 10 degrees scale the rate from the current input to 5 dps
775 const float angleDelta
= pidRuntime
.launchControlAngleLimit
- currentAngle
;
776 if (angleDelta
<= LAUNCH_CONTROL_ANGLE_WINDOW
) {
777 ret
= scaleRangef(angleDelta
, 0, LAUNCH_CONTROL_ANGLE_WINDOW
, LAUNCH_CONTROL_MIN_RATE
, ret
);
789 // Betaflight pid controller, which will be maintained in the future with additional features specialised for current (mini) multirotor usage.
790 // Based on 2DOF reference design (matlab)
791 void FAST_CODE
pidController(const pidProfile_t
*pidProfile
, timeUs_t currentTimeUs
)
793 static float previousGyroRateDterm
[XYZ_AXIS_COUNT
];
794 static float previousRawGyroRateDterm
[XYZ_AXIS_COUNT
];
797 const float tpaFactorKp
= (pidProfile
->tpa_mode
== TPA_MODE_PD
) ? pidRuntime
.tpaFactor
: 1.0f
;
799 const float tpaFactorKp
= pidRuntime
.tpaFactor
;
802 #ifdef USE_YAW_SPIN_RECOVERY
803 const bool yawSpinActive
= gyroYawSpinDetected();
806 const bool launchControlActive
= isLaunchControlActive();
809 static timeUs_t levelModeStartTimeUs
= 0;
810 static bool gpsRescuePreviousState
= false;
811 const rollAndPitchTrims_t
*angleTrim
= &accelerometerConfig()->accelerometerTrims
;
812 float horizonLevelStrength
= 0.0f
;
814 const bool gpsRescueIsActive
= FLIGHT_MODE(GPS_RESCUE_MODE
);
815 levelMode_e levelMode
;
816 if (FLIGHT_MODE(ANGLE_MODE
) || FLIGHT_MODE(HORIZON_MODE
) || gpsRescueIsActive
) {
817 if (pidRuntime
.levelRaceMode
&& !gpsRescueIsActive
) {
818 levelMode
= LEVEL_MODE_R
;
820 levelMode
= LEVEL_MODE_RP
;
823 // Keep track of when we entered a self-level mode so that we can
824 // add a guard time before crash recovery can activate.
825 // Also reset the guard time whenever GPS Rescue is activated.
826 if ((levelModeStartTimeUs
== 0) || (gpsRescueIsActive
&& !gpsRescuePreviousState
)) {
827 levelModeStartTimeUs
= currentTimeUs
;
830 // Calc horizonLevelStrength if needed
831 if (FLIGHT_MODE(HORIZON_MODE
)) {
832 horizonLevelStrength
= calcHorizonLevelStrength();
835 levelMode
= LEVEL_MODE_OFF
;
836 levelModeStartTimeUs
= 0;
839 gpsRescuePreviousState
= gpsRescueIsActive
;
842 UNUSED(currentTimeUs
);
846 if (pidRuntime
.antiGravityEnabled
) {
847 pidRuntime
.antiGravityThrottleD
*= pidRuntime
.antiGravityGain
;
848 // used later to increase pTerm
849 pidRuntime
.itermAccelerator
= pidRuntime
.antiGravityThrottleD
* ANTIGRAVITY_KI
;
851 pidRuntime
.antiGravityThrottleD
= 0.0f
;
852 pidRuntime
.itermAccelerator
= 0.0f
;
854 DEBUG_SET(DEBUG_ANTI_GRAVITY
, 2, lrintf((1 + (pidRuntime
.itermAccelerator
/ pidRuntime
.pidCoefficient
[FD_PITCH
].Ki
)) * 1000));
855 // amount of antigravity added relative to user's pitch iTerm coefficient
856 // used later to increase iTerm
858 // iTerm windup (attenuation of iTerm if motorMix range is large)
860 if (pidRuntime
.itermWindupPointInv
> 1.0f
) {
861 dynCi
= constrainf((1.0f
- getMotorMixRange()) * pidRuntime
.itermWindupPointInv
, 0.0f
, 1.0f
);
864 // Precalculate gyro delta for D-term here, this allows loop unrolling
865 float gyroRateDterm
[XYZ_AXIS_COUNT
];
866 for (int axis
= FD_ROLL
; axis
<= FD_YAW
; ++axis
) {
867 gyroRateDterm
[axis
] = gyro
.gyroADCf
[axis
];
868 // -----calculate raw, unfiltered D component
870 // Divide rate change by dT to get differential (ie dr/dt).
871 // dT is fixed and calculated from the target PID loop time
872 // This is done to avoid DTerm spikes that occur with dynamically
873 // calculated deltaT whenever another task causes the PID
874 // loop execution to be delayed.
876 // Log the unfiltered D for ROLL and PITCH
877 if (axis
!= FD_YAW
) {
878 const float delta
= (previousRawGyroRateDterm
[axis
] - gyroRateDterm
[axis
]) * pidRuntime
.pidFrequency
/ D_LPF_RAW_SCALE
;
879 previousRawGyroRateDterm
[axis
] = gyroRateDterm
[axis
];
880 DEBUG_SET(DEBUG_D_LPF
, axis
, lrintf(delta
));
883 gyroRateDterm
[axis
] = pidRuntime
.dtermNotchApplyFn((filter_t
*) &pidRuntime
.dtermNotch
[axis
], gyroRateDterm
[axis
]);
884 gyroRateDterm
[axis
] = pidRuntime
.dtermLowpassApplyFn((filter_t
*) &pidRuntime
.dtermLowpass
[axis
], gyroRateDterm
[axis
]);
885 gyroRateDterm
[axis
] = pidRuntime
.dtermLowpass2ApplyFn((filter_t
*) &pidRuntime
.dtermLowpass2
[axis
], gyroRateDterm
[axis
]);
888 rotateItermAndAxisError();
890 #ifdef USE_RPM_FILTER
894 // ----------PID controller----------
895 for (int axis
= FD_ROLL
; axis
<= FD_YAW
; ++axis
) {
897 float currentPidSetpoint
= getSetpointRate(axis
);
898 if (pidRuntime
.maxVelocity
[axis
]) {
899 currentPidSetpoint
= accelerationLimit(axis
, currentPidSetpoint
);
901 // Yaw control is GYRO based, direct sticks control is applied to rate PID
902 // When Race Mode is active PITCH control is also GYRO based in level or horizon mode
904 pidRuntime
.axisInAngleMode
[axis
] = false;
906 if (levelMode
== LEVEL_MODE_RP
|| (levelMode
== LEVEL_MODE_R
&& axis
== FD_ROLL
)) {
907 pidRuntime
.axisInAngleMode
[axis
] = true;
908 currentPidSetpoint
= pidLevel(axis
, pidProfile
, angleTrim
, currentPidSetpoint
, horizonLevelStrength
);
910 } else { // yaw axis only
911 if (levelMode
== LEVEL_MODE_RP
) {
912 // if earth referencing is requested, attenuate yaw axis setpoint when pitched or rolled
913 // and send yawSetpoint to Angle code to modulate pitch and roll
914 // code cost is 107 cycles when earthRef enabled, 20 otherwise, nearly all in cos_approx
915 if (pidRuntime
.angleEarthRef
) {
916 pidRuntime
.angleYawSetpoint
= currentPidSetpoint
;
917 float maxAngleTargetAbs
= pidRuntime
.angleEarthRef
* fmaxf( fabsf(pidRuntime
.angleTarget
[FD_ROLL
]), fabsf(pidRuntime
.angleTarget
[FD_PITCH
]) );
918 maxAngleTargetAbs
*= (FLIGHT_MODE(HORIZON_MODE
)) ? horizonLevelStrength
: 1.0f
;
919 // reduce compensation whenever Horizon uses less levelling
920 currentPidSetpoint
*= cos_approx(DEGREES_TO_RADIANS(maxAngleTargetAbs
));
921 DEBUG_SET(DEBUG_ANGLE_TARGET
, 2, currentPidSetpoint
); // yaw setpoint after attenuation
927 #ifdef USE_ACRO_TRAINER
928 if ((axis
!= FD_YAW
) && pidRuntime
.acroTrainerActive
&& !pidRuntime
.inCrashRecoveryMode
&& !launchControlActive
) {
929 currentPidSetpoint
= applyAcroTrainer(axis
, angleTrim
, currentPidSetpoint
);
931 #endif // USE_ACRO_TRAINER
933 #ifdef USE_LAUNCH_CONTROL
934 if (launchControlActive
) {
936 currentPidSetpoint
= applyLaunchControl(axis
, angleTrim
);
938 currentPidSetpoint
= applyLaunchControl(axis
, NULL
);
943 // Handle yaw spin recovery - zero the setpoint on yaw to aid in recovery
944 // It's not necessary to zero the set points for R/P because the PIDs will be zeroed below
945 #ifdef USE_YAW_SPIN_RECOVERY
946 if ((axis
== FD_YAW
) && yawSpinActive
) {
947 currentPidSetpoint
= 0.0f
;
949 #endif // USE_YAW_SPIN_RECOVERY
951 // -----calculate error rate
952 const float gyroRate
= gyro
.gyroADCf
[axis
]; // Process variable from gyro output in deg/sec
953 float errorRate
= currentPidSetpoint
- gyroRate
; // r - y
956 pidProfile
->crash_recovery
, angleTrim
, axis
, currentTimeUs
, gyroRate
,
957 ¤tPidSetpoint
, &errorRate
);
960 const float previousIterm
= pidData
[axis
].I
;
961 float itermErrorRate
= errorRate
;
962 #ifdef USE_ABSOLUTE_CONTROL
963 const float uncorrectedSetpoint
= currentPidSetpoint
;
966 #if defined(USE_ITERM_RELAX)
967 if (!launchControlActive
&& !pidRuntime
.inCrashRecoveryMode
) {
968 applyItermRelax(axis
, previousIterm
, gyroRate
, &itermErrorRate
, ¤tPidSetpoint
);
969 errorRate
= currentPidSetpoint
- gyroRate
;
972 #ifdef USE_ABSOLUTE_CONTROL
973 const float setpointCorrection
= currentPidSetpoint
- uncorrectedSetpoint
;
976 // --------low-level gyro-based PID based on 2DOF PID controller. ----------
977 // 2-DOF PID controller with optional filter on derivative term.
978 // b = 1 and only c (feedforward weight) can be tuned (amount derivative on measurement or error).
980 // -----calculate P component
981 pidData
[axis
].P
= pidRuntime
.pidCoefficient
[axis
].Kp
* errorRate
* tpaFactorKp
;
982 if (axis
== FD_YAW
) {
983 pidData
[axis
].P
= pidRuntime
.ptermYawLowpassApplyFn((filter_t
*) &pidRuntime
.ptermYawLowpass
, pidData
[axis
].P
);
986 // -----calculate I component
987 float Ki
= pidRuntime
.pidCoefficient
[axis
].Ki
;
988 #ifdef USE_LAUNCH_CONTROL
989 // if launch control is active override the iterm gains and apply iterm windup protection to all axes
990 if (launchControlActive
) {
991 Ki
= pidRuntime
.launchControlKi
;
995 if (axis
== FD_YAW
) {
996 pidRuntime
.itermAccelerator
= 0.0f
; // no antigravity on yaw iTerm
999 const float iTermChange
= (Ki
+ pidRuntime
.itermAccelerator
) * dynCi
* pidRuntime
.dT
* itermErrorRate
;
1000 pidData
[axis
].I
= constrainf(previousIterm
+ iTermChange
, -pidRuntime
.itermLimit
, pidRuntime
.itermLimit
);
1002 // -----calculate D component
1004 float pidSetpointDelta
= 0;
1006 #ifdef USE_FEEDFORWARD
1007 if (FLIGHT_MODE(ANGLE_MODE
) && pidRuntime
.axisInAngleMode
[axis
]) {
1008 // this axis is fully under self-levelling control
1009 // it will already have stick based feedforward applied in the input to their angle setpoint
1010 // a simple setpoint Delta can be used to for PID feedforward element for motor lag on these axes
1011 // however RC steps come in, via angle setpoint
1012 // and setpoint RC smoothing must have a cutoff half normal to remove those steps completely
1013 // the RC stepping does not come in via the feedforward, which is very well smoothed already
1014 // if uncommented, and the forcing to zero is removed, the two following lines will restore PID feedforward to angle mode axes
1015 // but for now let's see how we go without it (which was the case before 4.5 anyway)
1016 // pidSetpointDelta = currentPidSetpoint - pidRuntime.previousPidSetpoint[axis];
1017 // pidSetpointDelta *= pidRuntime.pidFrequency * pidRuntime.angleFeedforwardGain;
1018 pidSetpointDelta
= 0.0f
;
1020 // the axis is operating as a normal acro axis, so use normal feedforard from rc.c
1021 pidSetpointDelta
= getFeedforward(axis
);
1024 pidRuntime
.previousPidSetpoint
[axis
] = currentPidSetpoint
; // this is the value sent to blackbox, and used for Dmin setpoint
1026 // disable D if launch control is active
1027 if ((pidRuntime
.pidCoefficient
[axis
].Kd
> 0) && !launchControlActive
) {
1028 // Divide rate change by dT to get differential (ie dr/dt).
1029 // dT is fixed and calculated from the target PID loop time
1030 // This is done to avoid DTerm spikes that occur with dynamically
1031 // calculated deltaT whenever another task causes the PID
1032 // loop execution to be delayed.
1034 - (gyroRateDterm
[axis
] - previousGyroRateDterm
[axis
]) * pidRuntime
.pidFrequency
;
1035 float preTpaD
= pidRuntime
.pidCoefficient
[axis
].Kd
* delta
;
1037 #if defined(USE_ACC)
1038 if (cmpTimeUs(currentTimeUs
, levelModeStartTimeUs
) > CRASH_RECOVERY_DETECTION_DELAY_US
) {
1039 detectAndSetCrashRecovery(pidProfile
->crash_recovery
, axis
, currentTimeUs
, delta
, errorRate
);
1043 #if defined(USE_D_MIN)
1044 float dMinFactor
= 1.0f
;
1045 if (pidRuntime
.dMinPercent
[axis
] > 0) {
1046 float dMinGyroFactor
= pt2FilterApply(&pidRuntime
.dMinRange
[axis
], delta
);
1047 dMinGyroFactor
= fabsf(dMinGyroFactor
) * pidRuntime
.dMinGyroGain
;
1048 const float dMinSetpointFactor
= (fabsf(pidSetpointDelta
)) * pidRuntime
.dMinSetpointGain
;
1049 dMinFactor
= MAX(dMinGyroFactor
, dMinSetpointFactor
);
1050 dMinFactor
= pidRuntime
.dMinPercent
[axis
] + (1.0f
- pidRuntime
.dMinPercent
[axis
]) * dMinFactor
;
1051 dMinFactor
= pt2FilterApply(&pidRuntime
.dMinLowpass
[axis
], dMinFactor
);
1052 dMinFactor
= MIN(dMinFactor
, 1.0f
);
1053 if (axis
== FD_ROLL
) {
1054 DEBUG_SET(DEBUG_D_MIN
, 0, lrintf(dMinGyroFactor
* 100));
1055 DEBUG_SET(DEBUG_D_MIN
, 1, lrintf(dMinSetpointFactor
* 100));
1056 DEBUG_SET(DEBUG_D_MIN
, 2, lrintf(pidRuntime
.pidCoefficient
[axis
].Kd
* dMinFactor
* 10 / DTERM_SCALE
));
1057 } else if (axis
== FD_PITCH
) {
1058 DEBUG_SET(DEBUG_D_MIN
, 3, lrintf(pidRuntime
.pidCoefficient
[axis
].Kd
* dMinFactor
* 10 / DTERM_SCALE
));
1062 // Apply the dMinFactor
1063 preTpaD
*= dMinFactor
;
1065 pidData
[axis
].D
= preTpaD
* pidRuntime
.tpaFactor
;
1067 // Log the value of D pre application of TPA
1068 preTpaD
*= D_LPF_FILT_SCALE
;
1070 if (axis
!= FD_YAW
) {
1071 DEBUG_SET(DEBUG_D_LPF
, axis
- FD_ROLL
+ 2, lrintf(preTpaD
));
1074 pidData
[axis
].D
= 0;
1075 if (axis
!= FD_YAW
) {
1076 DEBUG_SET(DEBUG_D_LPF
, axis
- FD_ROLL
+ 2, 0);
1080 previousGyroRateDterm
[axis
] = gyroRateDterm
[axis
];
1082 // -----calculate feedforward component
1083 #ifdef USE_ABSOLUTE_CONTROL
1084 // include abs control correction in feedforward
1085 pidSetpointDelta
+= setpointCorrection
- pidRuntime
.oldSetpointCorrection
[axis
];
1086 pidRuntime
.oldSetpointCorrection
[axis
] = setpointCorrection
;
1089 // no feedforward in launch control
1090 float feedforwardGain
= launchControlActive
? 0.0f
: pidRuntime
.pidCoefficient
[axis
].Kf
;
1091 if (feedforwardGain
> 0) {
1092 float feedForward
= feedforwardGain
* pidSetpointDelta
;
1093 pidData
[axis
].F
= feedForward
;
1095 pidData
[axis
].F
= 0;
1098 #ifdef USE_YAW_SPIN_RECOVERY
1099 if (yawSpinActive
) {
1100 pidData
[axis
].I
= 0; // in yaw spin always disable I
1101 if (axis
<= FD_PITCH
) {
1102 // zero PIDs on pitch and roll leaving yaw P to correct spin
1103 pidData
[axis
].P
= 0;
1104 pidData
[axis
].D
= 0;
1105 pidData
[axis
].F
= 0;
1108 #endif // USE_YAW_SPIN_RECOVERY
1110 #ifdef USE_LAUNCH_CONTROL
1111 // Disable P/I appropriately based on the launch control mode
1112 if (launchControlActive
) {
1113 // if not using FULL mode then disable I accumulation on yaw as
1114 // yaw has a tendency to windup. Otherwise limit yaw iterm accumulation.
1115 const int launchControlYawItermLimit
= (pidRuntime
.launchControlMode
== LAUNCH_CONTROL_MODE_FULL
) ? LAUNCH_CONTROL_YAW_ITERM_LIMIT
: 0;
1116 pidData
[FD_YAW
].I
= constrainf(pidData
[FD_YAW
].I
, -launchControlYawItermLimit
, launchControlYawItermLimit
);
1118 // for pitch-only mode we disable everything except pitch P/I
1119 if (pidRuntime
.launchControlMode
== LAUNCH_CONTROL_MODE_PITCHONLY
) {
1120 pidData
[FD_ROLL
].P
= 0;
1121 pidData
[FD_ROLL
].I
= 0;
1122 pidData
[FD_YAW
].P
= 0;
1123 // don't let I go negative (pitch backwards) as front motors are limited in the mixer
1124 pidData
[FD_PITCH
].I
= MAX(0.0f
, pidData
[FD_PITCH
].I
);
1129 // Add P boost from antiGravity when sticks are close to zero
1130 if (axis
!= FD_YAW
) {
1131 float agSetpointAttenuator
= fabsf(currentPidSetpoint
) / 50.0f
;
1132 agSetpointAttenuator
= MAX(agSetpointAttenuator
, 1.0f
);
1133 // attenuate effect if turning more than 50 deg/s, half at 100 deg/s
1134 const float antiGravityPBoost
= 1.0f
+ (pidRuntime
.antiGravityThrottleD
/ agSetpointAttenuator
) * pidRuntime
.antiGravityPGain
;
1135 pidData
[axis
].P
*= antiGravityPBoost
;
1136 if (axis
== FD_PITCH
) {
1137 DEBUG_SET(DEBUG_ANTI_GRAVITY
, 3, lrintf(antiGravityPBoost
* 1000));
1141 // calculating the PID sum
1142 const float pidSum
= pidData
[axis
].P
+ pidData
[axis
].I
+ pidData
[axis
].D
+ pidData
[axis
].F
;
1143 #ifdef USE_INTEGRATED_YAW_CONTROL
1144 if (axis
== FD_YAW
&& pidRuntime
.useIntegratedYaw
) {
1145 pidData
[axis
].Sum
+= pidSum
* pidRuntime
.dT
* 100.0f
;
1146 pidData
[axis
].Sum
-= pidData
[axis
].Sum
* pidRuntime
.integratedYawRelax
/ 100000.0f
* pidRuntime
.dT
/ 0.000125f
;
1150 pidData
[axis
].Sum
= pidSum
;
1154 // Disable PID control if at zero throttle or if gyro overflow detected
1155 // This may look very innefficient, but it is done on purpose to always show real CPU usage as in flight
1156 if (!pidRuntime
.pidStabilisationEnabled
|| gyroOverflowDetected()) {
1157 for (int axis
= FD_ROLL
; axis
<= FD_YAW
; ++axis
) {
1158 pidData
[axis
].P
= 0;
1159 pidData
[axis
].I
= 0;
1160 pidData
[axis
].D
= 0;
1161 pidData
[axis
].F
= 0;
1163 pidData
[axis
].Sum
= 0;
1165 } else if (pidRuntime
.zeroThrottleItermReset
) {
1170 bool crashRecoveryModeActive(void)
1172 return pidRuntime
.inCrashRecoveryMode
;
1175 #ifdef USE_ACRO_TRAINER
1176 void pidSetAcroTrainerState(bool newState
)
1178 if (pidRuntime
.acroTrainerActive
!= newState
) {
1180 pidAcroTrainerInit();
1182 pidRuntime
.acroTrainerActive
= newState
;
1185 #endif // USE_ACRO_TRAINER
1187 void pidSetAntiGravityState(bool newState
)
1189 if (newState
!= pidRuntime
.antiGravityEnabled
) {
1190 // reset the accelerator on state changes
1191 pidRuntime
.itermAccelerator
= 0.0f
;
1193 pidRuntime
.antiGravityEnabled
= newState
;
1196 bool pidAntiGravityEnabled(void)
1198 return pidRuntime
.antiGravityEnabled
;
1202 void dynLpfDTermUpdate(float throttle
)
1204 if (pidRuntime
.dynLpfFilter
!= DYN_LPF_NONE
) {
1206 if (pidRuntime
.dynLpfCurveExpo
> 0) {
1207 cutoffFreq
= dynLpfCutoffFreq(throttle
, pidRuntime
.dynLpfMin
, pidRuntime
.dynLpfMax
, pidRuntime
.dynLpfCurveExpo
);
1209 cutoffFreq
= fmaxf(dynThrottle(throttle
) * pidRuntime
.dynLpfMax
, pidRuntime
.dynLpfMin
);
1212 switch (pidRuntime
.dynLpfFilter
) {
1214 for (int axis
= 0; axis
< XYZ_AXIS_COUNT
; axis
++) {
1215 pt1FilterUpdateCutoff(&pidRuntime
.dtermLowpass
[axis
].pt1Filter
, pt1FilterGain(cutoffFreq
, pidRuntime
.dT
));
1218 case DYN_LPF_BIQUAD
:
1219 for (int axis
= 0; axis
< XYZ_AXIS_COUNT
; axis
++) {
1220 biquadFilterUpdateLPF(&pidRuntime
.dtermLowpass
[axis
].biquadFilter
, cutoffFreq
, targetPidLooptime
);
1224 for (int axis
= 0; axis
< XYZ_AXIS_COUNT
; axis
++) {
1225 pt2FilterUpdateCutoff(&pidRuntime
.dtermLowpass
[axis
].pt2Filter
, pt2FilterGain(cutoffFreq
, pidRuntime
.dT
));
1229 for (int axis
= 0; axis
< XYZ_AXIS_COUNT
; axis
++) {
1230 pt3FilterUpdateCutoff(&pidRuntime
.dtermLowpass
[axis
].pt3Filter
, pt3FilterGain(cutoffFreq
, pidRuntime
.dT
));
1238 float dynLpfCutoffFreq(float throttle
, uint16_t dynLpfMin
, uint16_t dynLpfMax
, uint8_t expo
)
1240 const float expof
= expo
/ 10.0f
;
1241 const float curve
= throttle
* (1 - throttle
) * expof
+ throttle
;
1242 return (dynLpfMax
- dynLpfMin
) * curve
+ dynLpfMin
;
1245 void pidSetItermReset(bool enabled
)
1247 pidRuntime
.zeroThrottleItermReset
= enabled
;
1250 float pidGetPreviousSetpoint(int axis
)
1252 return pidRuntime
.previousPidSetpoint
[axis
];
1255 float pidGetDT(void)
1257 return pidRuntime
.dT
;
1260 float pidGetPidFrequency(void)
1262 return pidRuntime
.pidFrequency
;