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
, 4);
93 #ifndef DEFAULT_PID_PROCESS_DENOM
94 #define DEFAULT_PID_PROCESS_DENOM 1
97 #ifdef USE_RUNAWAY_TAKEOFF
98 PG_RESET_TEMPLATE(pidConfig_t
, pidConfig
,
99 .pid_process_denom
= DEFAULT_PID_PROCESS_DENOM
,
100 .runaway_takeoff_prevention
= true,
101 .runaway_takeoff_deactivate_throttle
= 20, // throttle level % needed to accumulate deactivation time
102 .runaway_takeoff_deactivate_delay
= 500, // Accumulated time (in milliseconds) before deactivation in successful takeoff
105 PG_RESET_TEMPLATE(pidConfig_t
, pidConfig
,
106 .pid_process_denom
= DEFAULT_PID_PROCESS_DENOM
,
110 #ifdef USE_ACRO_TRAINER
111 #define ACRO_TRAINER_LOOKAHEAD_RATE_LIMIT 500.0f // Max gyro rate for lookahead time scaling
112 #define ACRO_TRAINER_SETPOINT_LIMIT 1000.0f // Limit the correcting setpoint
113 #endif // USE_ACRO_TRAINER
115 #define CRASH_RECOVERY_DETECTION_DELAY_US 1000000 // 1 second delay before crash recovery detection is active after entering a self-level mode
117 #define LAUNCH_CONTROL_YAW_ITERM_LIMIT 50 // yaw iterm windup limit when launch mode is "FULL" (all axes)
119 PG_REGISTER_ARRAY_WITH_RESET_FN(pidProfile_t
, PID_PROFILE_COUNT
, pidProfiles
, PG_PID_PROFILE
, 9);
121 void resetPidProfile(pidProfile_t
*pidProfile
)
123 RESET_CONFIG(pidProfile_t
, pidProfile
,
125 [PID_ROLL
] = PID_ROLL_DEFAULT
,
126 [PID_PITCH
] = PID_PITCH_DEFAULT
,
127 [PID_YAW
] = PID_YAW_DEFAULT
,
128 [PID_LEVEL
] = { 50, 75, 75, 50, 0 },
129 [PID_MAG
] = { 40, 0, 0, 0, 0 },
131 .pidSumLimit
= PIDSUM_LIMIT
,
132 .pidSumLimitYaw
= PIDSUM_LIMIT_YAW
,
133 .yaw_lowpass_hz
= 100,
135 .dterm_notch_cutoff
= 0,
136 .itermWindupPointPercent
= 85,
137 .pidAtMinThrottle
= PID_STABILISATION_ON
,
139 .feedforward_transition
= 0,
140 .yawRateAccelLimit
= 0,
142 .anti_gravity_gain
= 80,
143 .crash_time
= 500, // ms
144 .crash_delay
= 0, // ms
145 .crash_recovery_angle
= 10, // degrees
146 .crash_recovery_rate
= 100, // degrees/second
147 .crash_dthreshold
= 50, // degrees/second/second
148 .crash_gthreshold
= 400, // degrees/second
149 .crash_setpoint_threshold
= 350, // degrees/second
150 .crash_recovery
= PID_CRASH_RECOVERY_OFF
, // off by default
151 .horizon_limit_degrees
= 135,
152 .horizon_ignore_sticks
= false,
153 .crash_limit_yaw
= 200,
156 .throttle_boost_cutoff
= 15,
157 .iterm_rotation
= false,
158 .iterm_relax
= ITERM_RELAX_RP
,
159 .iterm_relax_cutoff
= ITERM_RELAX_CUTOFF_DEFAULT
,
160 .iterm_relax_type
= ITERM_RELAX_SETPOINT
,
161 .acro_trainer_angle_limit
= 20,
162 .acro_trainer_lookahead_ms
= 50,
163 .acro_trainer_debug_axis
= FD_ROLL
,
164 .acro_trainer_gain
= 75,
165 .abs_control_gain
= 0,
166 .abs_control_limit
= 90,
167 .abs_control_error_limit
= 20,
168 .abs_control_cutoff
= 11,
169 .dterm_lpf1_static_hz
= DTERM_LPF1_DYN_MIN_HZ_DEFAULT
,
170 // NOTE: dynamic lpf is enabled by default so this setting is actually
171 // overridden and the static lowpass 1 is disabled. We can't set this
172 // value to 0 otherwise Configurator versions 10.4 and earlier will also
173 // reset the lowpass filter type to PT1 overriding the desired BIQUAD setting.
174 .dterm_lpf2_static_hz
= DTERM_LPF2_HZ_DEFAULT
, // second Dterm LPF ON by default
175 .dterm_lpf1_type
= FILTER_PT1
,
176 .dterm_lpf2_type
= FILTER_PT1
,
177 .dterm_lpf1_dyn_min_hz
= DTERM_LPF1_DYN_MIN_HZ_DEFAULT
,
178 .dterm_lpf1_dyn_max_hz
= DTERM_LPF1_DYN_MAX_HZ_DEFAULT
,
179 .launchControlMode
= LAUNCH_CONTROL_MODE_PITCHONLY
,
180 .launchControlThrottlePercent
= 20,
181 .launchControlAngleLimit
= 0,
182 .launchControlGain
= 40,
183 .launchControlAllowTriggerReset
= true,
184 .use_integrated_yaw
= false,
185 .integrated_yaw_relax
= 200,
186 .thrustLinearization
= 0,
187 .d_min
= D_MIN_DEFAULT
,
190 .motor_output_limit
= 100,
191 .auto_profile_cell_count
= AUTO_PROFILE_CELL_COUNT_STAY
,
192 .transient_throttle_limit
= 0,
193 .profileName
= { 0 },
194 .dyn_idle_min_rpm
= 0,
195 .dyn_idle_p_gain
= 50,
196 .dyn_idle_i_gain
= 50,
197 .dyn_idle_d_gain
= 50,
198 .dyn_idle_max_increase
= 150,
199 .dyn_idle_start_increase
= 50,
200 .feedforward_averaging
= FEEDFORWARD_AVERAGING_OFF
,
201 .feedforward_max_rate_limit
= 90,
202 .feedforward_smooth_factor
= 25,
203 .feedforward_jitter_factor
= 7,
204 .feedforward_boost
= 15,
205 .dterm_lpf1_dyn_expo
= 5,
206 .level_race_mode
= false,
207 .vbat_sag_compensation
= 0,
208 .simplified_pids_mode
= PID_SIMPLIFIED_TUNING_RPY
,
209 .simplified_master_multiplier
= SIMPLIFIED_TUNING_DEFAULT
,
210 .simplified_roll_pitch_ratio
= SIMPLIFIED_TUNING_DEFAULT
,
211 .simplified_i_gain
= SIMPLIFIED_TUNING_DEFAULT
,
212 .simplified_d_gain
= SIMPLIFIED_TUNING_D_DEFAULT
,
213 .simplified_pi_gain
= SIMPLIFIED_TUNING_DEFAULT
,
214 .simplified_dmin_ratio
= SIMPLIFIED_TUNING_D_DEFAULT
,
215 .simplified_feedforward_gain
= SIMPLIFIED_TUNING_DEFAULT
,
216 .simplified_pitch_pi_gain
= SIMPLIFIED_TUNING_DEFAULT
,
217 .simplified_dterm_filter
= true,
218 .simplified_dterm_filter_multiplier
= SIMPLIFIED_TUNING_DEFAULT
,
219 .anti_gravity_cutoff_hz
= 5,
220 .anti_gravity_p_gain
= 100,
221 .tpa_mode
= TPA_MODE_D
,
223 .tpa_breakpoint
= 1350,
224 .angle_feedforward_smoothing_ms
= 80,
225 .angle_earth_ref
= 100,
226 .horizon_delay_ms
= 500, // 500ms time constant on any increase in horizon strength
228 .tpa_low_breakpoint
= 1050,
230 .ez_landing_threshold
= 25,
231 .ez_landing_limit
= 15,
232 .ez_landing_speed
= 50,
234 .tpa_gravity_thr0
= 0,
235 .tpa_gravity_thr100
= 0,
236 .spa_center
= { 0, 0, 0 },
237 .spa_width
= { 0, 0, 0 },
238 .spa_mode
= { 0, 0, 0 },
239 .ez_landing_disarm_threshold
= 0 ,
243 pidProfile
->pid
[PID_ROLL
].D
= 30;
244 pidProfile
->pid
[PID_PITCH
].D
= 32;
248 void pgResetFn_pidProfiles(pidProfile_t
*pidProfiles
)
250 for (int i
= 0; i
< PID_PROFILE_COUNT
; i
++) {
251 resetPidProfile(&pidProfiles
[i
]);
255 // Scale factors to make best use of range with D_LPF debugging, aiming for max +/-16K as debug values are 16 bit
256 #define D_LPF_RAW_SCALE 25
257 #define D_LPF_PRE_TPA_SCALE 10
260 void pidSetItermAccelerator(float newItermAccelerator
)
262 pidRuntime
.itermAccelerator
= newItermAccelerator
;
265 bool pidOsdAntiGravityActive(void)
267 return (pidRuntime
.itermAccelerator
> pidRuntime
.antiGravityOsdCutoff
);
270 void pidStabilisationState(pidStabilisationState_e pidControllerState
)
272 pidRuntime
.pidStabilisationEnabled
= (pidControllerState
== PID_STABILISATION_ON
) ? true : false;
275 const angle_index_t rcAliasToAngleIndexMap
[] = { AI_ROLL
, AI_PITCH
};
277 void pidResetIterm(void)
279 for (int axis
= 0; axis
< 3; axis
++) {
280 pidData
[axis
].I
= 0.0f
;
281 #if defined(USE_ABSOLUTE_CONTROL)
282 axisError
[axis
] = 0.0f
;
288 static float getWingTpaArgument(float throttle
)
290 const float pitchFactorAdjustment
= scaleRangef(throttle
, 0.0f
, 1.0f
, pidRuntime
.tpaGravityThr0
, pidRuntime
.tpaGravityThr100
);
291 const float pitchAngleFactor
= getSinPitchAngle() * pitchFactorAdjustment
;
292 DEBUG_SET(DEBUG_TPA
, 1, lrintf(pitchAngleFactor
* 1000.0f
));
294 float tpaArgument
= throttle
+ pitchAngleFactor
;
295 const float maxTpaArgument
= MAX(1.0 + pidRuntime
.tpaGravityThr100
, pidRuntime
.tpaGravityThr0
);
296 tpaArgument
= tpaArgument
/ maxTpaArgument
;
297 tpaArgument
= pt2FilterApply(&pidRuntime
.tpaLpf
, tpaArgument
);
298 DEBUG_SET(DEBUG_TPA
, 2, lrintf(tpaArgument
* 1000.0f
));
302 #endif // #ifndef USE_WING
304 void pidUpdateTpaFactor(float throttle
)
306 static bool isTpaLowFaded
= false;
307 // don't permit throttle > 1 & throttle < 0 ? is this needed ? can throttle be > 1 or < 0 at this point
308 throttle
= constrainf(throttle
, 0.0f
, 1.0f
);
311 const float tpaArgument
= isFixedWing() ? getWingTpaArgument(throttle
) : throttle
;
313 const float tpaArgument
= throttle
;
316 bool isThrottlePastTpaLowBreakpoint
= (tpaArgument
>= pidRuntime
.tpaLowBreakpoint
|| pidRuntime
.tpaLowBreakpoint
<= 0.01f
);
317 float tpaRate
= 0.0f
;
318 if (isThrottlePastTpaLowBreakpoint
|| isTpaLowFaded
) {
319 tpaRate
= pidRuntime
.tpaMultiplier
* fmaxf(tpaArgument
- pidRuntime
.tpaBreakpoint
, 0.0f
);
320 if (!pidRuntime
.tpaLowAlways
&& !isTpaLowFaded
) {
321 isTpaLowFaded
= true;
324 tpaRate
= pidRuntime
.tpaLowMultiplier
* (pidRuntime
.tpaLowBreakpoint
- tpaArgument
);
327 float tpaFactor
= 1.0f
- tpaRate
;
328 DEBUG_SET(DEBUG_TPA
, 0, lrintf(tpaFactor
* 1000));
329 pidRuntime
.tpaFactor
= tpaFactor
;
332 void pidUpdateAntiGravityThrottleFilter(float throttle
)
334 static float previousThrottle
= 0.0f
;
335 const float throttleInv
= 1.0f
- throttle
;
336 float throttleDerivative
= fabsf(throttle
- previousThrottle
) * pidRuntime
.pidFrequency
;
337 DEBUG_SET(DEBUG_ANTI_GRAVITY
, 0, lrintf(throttleDerivative
* 100));
338 throttleDerivative
*= throttleInv
* throttleInv
;
339 // generally focus on the low throttle period
340 if (throttle
> previousThrottle
) {
341 throttleDerivative
*= throttleInv
* 0.5f
;
342 // when increasing throttle, focus even more on the low throttle range
344 previousThrottle
= throttle
;
345 throttleDerivative
= pt2FilterApply(&pidRuntime
.antiGravityLpf
, throttleDerivative
);
346 // lower cutoff suppresses peaks relative to troughs and prolongs the effects
347 // PT2 smoothing of throttle derivative.
348 // 6 is a typical value for the peak boost factor with default cutoff of 6Hz
349 DEBUG_SET(DEBUG_ANTI_GRAVITY
, 1, lrintf(throttleDerivative
* 100));
350 pidRuntime
.antiGravityThrottleD
= throttleDerivative
;
353 #ifdef USE_ACRO_TRAINER
354 void pidAcroTrainerInit(void)
356 pidRuntime
.acroTrainerAxisState
[FD_ROLL
] = 0;
357 pidRuntime
.acroTrainerAxisState
[FD_PITCH
] = 0;
359 #endif // USE_ACRO_TRAINER
361 #ifdef USE_THRUST_LINEARIZATION
362 float pidCompensateThrustLinearization(float throttle
)
364 if (pidRuntime
.thrustLinearization
!= 0.0f
) {
365 // for whoops where a lot of TL is needed, allow more throttle boost
366 const float throttleReversed
= (1.0f
- throttle
);
367 throttle
/= 1.0f
+ pidRuntime
.throttleCompensateAmount
* sq(throttleReversed
);
372 float pidApplyThrustLinearization(float motorOutput
)
374 motorOutput
*= 1.0f
+ pidRuntime
.thrustLinearization
* sq(1.0f
- motorOutput
);
380 // Calculate strength of horizon leveling; 0 = none, 1.0 = most leveling
381 STATIC_UNIT_TESTED FAST_CODE_NOINLINE
float calcHorizonLevelStrength(void)
383 const float currentInclination
= MAX(abs(attitude
.values
.roll
), abs(attitude
.values
.pitch
)) * 0.1f
;
384 // 0 when level, 90 when vertical, 180 when inverted (degrees):
385 float absMaxStickDeflection
= MAX(fabsf(getRcDeflection(FD_ROLL
)), fabsf(getRcDeflection(FD_PITCH
)));
386 // 0-1, smoothed if RC smoothing is enabled
388 float horizonLevelStrength
= MAX((pidRuntime
.horizonLimitDegrees
- currentInclination
) * pidRuntime
.horizonLimitDegreesInv
, 0.0f
);
389 // 1.0 when attitude is 'flat', 0 when angle is equal to, or greater than, horizonLimitDegrees
390 horizonLevelStrength
*= MAX((pidRuntime
.horizonLimitSticks
- absMaxStickDeflection
) * pidRuntime
.horizonLimitSticksInv
, pidRuntime
.horizonIgnoreSticks
);
391 // use the value of horizonIgnoreSticks to enable/disable this effect.
392 // value should be 1.0 at center stick, 0.0 at max stick deflection:
393 horizonLevelStrength
*= pidRuntime
.horizonGain
;
395 if (pidRuntime
.horizonDelayMs
) {
396 const float horizonLevelStrengthSmoothed
= pt1FilterApply(&pidRuntime
.horizonSmoothingPt1
, horizonLevelStrength
);
397 horizonLevelStrength
= MIN(horizonLevelStrength
, horizonLevelStrengthSmoothed
);
399 return horizonLevelStrength
;
400 // 1 means full levelling, 0 means none
403 // Use the FAST_CODE_NOINLINE directive to avoid this code from being inlined into ITCM RAM to avoid overflow.
404 // The impact is possibly slightly slower performance on F7/H7 but they have more than enough
405 // processing power that it should be a non-issue.
406 STATIC_UNIT_TESTED FAST_CODE_NOINLINE
float pidLevel(int axis
, const pidProfile_t
*pidProfile
, const rollAndPitchTrims_t
*angleTrim
,
407 float currentPidSetpoint
, float horizonLevelStrength
)
409 // Applies only to axes that are in Angle mode
410 // We now use Acro Rates, transformed into the range +/- 1, to provide setpoints
411 const float angleLimit
= pidProfile
->angle_limit
;
412 float angleFeedforward
= 0.0f
;
414 #ifdef USE_FEEDFORWARD
415 angleFeedforward
= angleLimit
* getFeedforward(axis
) * pidRuntime
.angleFeedforwardGain
* pidRuntime
.maxRcRateInv
[axis
];
416 // angle feedforward must be heavily filtered, at the PID loop rate, with limited user control over time constant
417 // it MUST be very delayed to avoid early overshoot and being too aggressive
418 angleFeedforward
= pt3FilterApply(&pidRuntime
.angleFeedforwardPt3
[axis
], angleFeedforward
);
421 float angleTarget
= angleLimit
* currentPidSetpoint
* pidRuntime
.maxRcRateInv
[axis
];
422 // use acro rates for the angle target in both horizon and angle modes, converted to -1 to +1 range using maxRate
424 #ifdef USE_GPS_RESCUE
425 angleTarget
+= gpsRescueAngle
[axis
] / 100.0f
; // Angle is in centidegrees, stepped on roll at 10Hz but not on pitch
427 const float currentAngle
= (attitude
.raw
[axis
] - angleTrim
->raw
[axis
]) / 10.0f
; // stepped at 500hz with some 4ms flat spots
428 const float errorAngle
= angleTarget
- currentAngle
;
429 float angleRate
= errorAngle
* pidRuntime
.angleGain
+ angleFeedforward
;
431 // minimise cross-axis wobble due to faster yaw responses than roll or pitch, and make co-ordinated yaw turns
432 // by compensating for the effect of yaw on roll while pitched, and on pitch while rolled
433 // earthRef code here takes about 76 cycles, if conditional on angleEarthRef it takes about 100. sin_approx costs most of those cycles.
434 float sinAngle
= sin_approx(DEGREES_TO_RADIANS(pidRuntime
.angleTarget
[axis
== FD_ROLL
? FD_PITCH
: FD_ROLL
]));
435 sinAngle
*= (axis
== FD_ROLL
) ? -1.0f
: 1.0f
; // must be negative for Roll
436 const float earthRefGain
= FLIGHT_MODE(GPS_RESCUE_MODE
) ? 1.0f
: pidRuntime
.angleEarthRef
;
437 angleRate
+= pidRuntime
.angleYawSetpoint
* sinAngle
* earthRefGain
;
438 pidRuntime
.angleTarget
[axis
] = angleTarget
; // set target for alternate axis to current axis, for use in preceding calculation
440 // smooth final angle rate output to clean up attitude signal steps (500hz), GPS steps (10 or 100hz), RC steps etc
441 // this filter runs at ATTITUDE_CUTOFF_HZ, currently 50hz, so GPS roll may be a bit steppy
442 angleRate
= pt3FilterApply(&pidRuntime
.attitudeFilter
[axis
], angleRate
);
444 if (FLIGHT_MODE(ANGLE_MODE
) || FLIGHT_MODE(GPS_RESCUE_MODE
)) {
445 currentPidSetpoint
= angleRate
;
447 // can only be HORIZON mode - crossfade Angle rate and Acro rate
448 currentPidSetpoint
= currentPidSetpoint
* (1.0f
- horizonLevelStrength
) + angleRate
* horizonLevelStrength
;
452 if (axis
== FD_ROLL
) {
453 DEBUG_SET(DEBUG_ANGLE_MODE
, 0, lrintf(angleTarget
* 10.0f
)); // target angle
454 DEBUG_SET(DEBUG_ANGLE_MODE
, 1, lrintf(errorAngle
* pidRuntime
.angleGain
* 10.0f
)); // un-smoothed error correction in degrees
455 DEBUG_SET(DEBUG_ANGLE_MODE
, 2, lrintf(angleFeedforward
* 10.0f
)); // feedforward amount in degrees
456 DEBUG_SET(DEBUG_ANGLE_MODE
, 3, lrintf(currentAngle
* 10.0f
)); // angle returned
458 DEBUG_SET(DEBUG_ANGLE_TARGET
, 0, lrintf(angleTarget
* 10.0f
));
459 DEBUG_SET(DEBUG_ANGLE_TARGET
, 1, lrintf(sinAngle
* 10.0f
)); // modification factor from earthRef
460 // debug ANGLE_TARGET 2 is yaw attenuation
461 DEBUG_SET(DEBUG_ANGLE_TARGET
, 3, lrintf(currentAngle
* 10.0f
)); // angle returned
464 DEBUG_SET(DEBUG_CURRENT_ANGLE
, axis
, lrintf(currentAngle
* 10.0f
)); // current angle
465 return currentPidSetpoint
;
468 static FAST_CODE_NOINLINE
void handleCrashRecovery(
469 const pidCrashRecovery_e crash_recovery
, const rollAndPitchTrims_t
*angleTrim
,
470 const int axis
, const timeUs_t currentTimeUs
, const float gyroRate
, float *currentPidSetpoint
, float *errorRate
)
472 if (pidRuntime
.inCrashRecoveryMode
&& cmpTimeUs(currentTimeUs
, pidRuntime
.crashDetectedAtUs
) > pidRuntime
.crashTimeDelayUs
) {
473 if (crash_recovery
== PID_CRASH_RECOVERY_BEEP
) {
476 if (axis
== FD_YAW
) {
477 *errorRate
= constrainf(*errorRate
, -pidRuntime
.crashLimitYaw
, pidRuntime
.crashLimitYaw
);
479 // on roll and pitch axes calculate currentPidSetpoint and errorRate to level the aircraft to recover from crash
480 if (sensors(SENSOR_ACC
)) {
481 // errorAngle is deviation from horizontal
482 const float errorAngle
= -(attitude
.raw
[axis
] - angleTrim
->raw
[axis
]) / 10.0f
;
483 *currentPidSetpoint
= errorAngle
* pidRuntime
.angleGain
;
484 *errorRate
= *currentPidSetpoint
- gyroRate
;
487 // reset iterm, since accumulated error before crash is now meaningless
488 // and iterm windup during crash recovery can be extreme, especially on yaw axis
489 pidData
[axis
].I
= 0.0f
;
490 if (cmpTimeUs(currentTimeUs
, pidRuntime
.crashDetectedAtUs
) > pidRuntime
.crashTimeLimitUs
491 || (getMotorMixRange() < 1.0f
492 && fabsf(gyro
.gyroADCf
[FD_ROLL
]) < pidRuntime
.crashRecoveryRate
493 && fabsf(gyro
.gyroADCf
[FD_PITCH
]) < pidRuntime
.crashRecoveryRate
494 && fabsf(gyro
.gyroADCf
[FD_YAW
]) < pidRuntime
.crashRecoveryRate
)) {
495 if (sensors(SENSOR_ACC
)) {
496 // check aircraft nearly level
497 if (abs(attitude
.raw
[FD_ROLL
] - angleTrim
->raw
[FD_ROLL
]) < pidRuntime
.crashRecoveryAngleDeciDegrees
498 && abs(attitude
.raw
[FD_PITCH
] - angleTrim
->raw
[FD_PITCH
]) < pidRuntime
.crashRecoveryAngleDeciDegrees
) {
499 pidRuntime
.inCrashRecoveryMode
= false;
503 pidRuntime
.inCrashRecoveryMode
= false;
510 static FAST_CODE_NOINLINE
void detectAndSetCrashRecovery(
511 const pidCrashRecovery_e crash_recovery
, const int axis
,
512 const timeUs_t currentTimeUs
, const float delta
, const float errorRate
)
514 // if crash recovery is on and accelerometer enabled and there is no gyro overflow, then check for a crash
515 // no point in trying to recover if the crash is so severe that the gyro overflows
516 if ((crash_recovery
|| FLIGHT_MODE(GPS_RESCUE_MODE
)) && !gyroOverflowDetected()) {
517 if (ARMING_FLAG(ARMED
)) {
518 if (getMotorMixRange() >= 1.0f
&& !pidRuntime
.inCrashRecoveryMode
519 && fabsf(delta
) > pidRuntime
.crashDtermThreshold
520 && fabsf(errorRate
) > pidRuntime
.crashGyroThreshold
521 && fabsf(getSetpointRate(axis
)) < pidRuntime
.crashSetpointThreshold
) {
522 if (crash_recovery
== PID_CRASH_RECOVERY_DISARM
) {
523 setArmingDisabled(ARMING_DISABLED_CRASH_DETECTED
);
524 disarm(DISARM_REASON_CRASH_PROTECTION
);
526 pidRuntime
.inCrashRecoveryMode
= true;
527 pidRuntime
.crashDetectedAtUs
= currentTimeUs
;
530 if (pidRuntime
.inCrashRecoveryMode
&& cmpTimeUs(currentTimeUs
, pidRuntime
.crashDetectedAtUs
) < pidRuntime
.crashTimeDelayUs
&& (fabsf(errorRate
) < pidRuntime
.crashGyroThreshold
531 || fabsf(getSetpointRate(axis
)) > pidRuntime
.crashSetpointThreshold
)) {
532 pidRuntime
.inCrashRecoveryMode
= false;
535 } else if (pidRuntime
.inCrashRecoveryMode
) {
536 pidRuntime
.inCrashRecoveryMode
= false;
543 #ifdef USE_ACRO_TRAINER
545 int acroTrainerSign(float x
)
547 return x
> 0 ? 1 : -1;
550 // Acro Trainer - Manipulate the setPoint to limit axis angle while in acro mode
551 // There are three states:
552 // 1. Current angle has exceeded limit
553 // Apply correction to return to limit (similar to pidLevel)
554 // 2. Future overflow has been projected based on current angle and gyro rate
555 // Manage the setPoint to control the gyro rate as the actual angle approaches the limit (try to prevent overshoot)
556 // 3. If no potential overflow is detected, then return the original setPoint
558 // Use the FAST_CODE_NOINLINE directive to avoid this code from being inlined into ITCM RAM. We accept the
559 // performance decrease when Acro Trainer mode is active under the assumption that user is unlikely to be
560 // expecting ultimate flight performance at very high loop rates when in this mode.
561 static FAST_CODE_NOINLINE
float applyAcroTrainer(int axis
, const rollAndPitchTrims_t
*angleTrim
, float setPoint
)
563 float ret
= setPoint
;
565 if (!FLIGHT_MODE(ANGLE_MODE
) && !FLIGHT_MODE(HORIZON_MODE
) && !FLIGHT_MODE(GPS_RESCUE_MODE
)) {
566 bool resetIterm
= false;
567 float projectedAngle
= 0;
568 const int setpointSign
= acroTrainerSign(setPoint
);
569 const float currentAngle
= (attitude
.raw
[axis
] - angleTrim
->raw
[axis
]) / 10.0f
;
570 const int angleSign
= acroTrainerSign(currentAngle
);
572 if ((pidRuntime
.acroTrainerAxisState
[axis
] != 0) && (pidRuntime
.acroTrainerAxisState
[axis
] != setpointSign
)) { // stick has reversed - stop limiting
573 pidRuntime
.acroTrainerAxisState
[axis
] = 0;
576 // Limit and correct the angle when it exceeds the limit
577 if ((fabsf(currentAngle
) > pidRuntime
.acroTrainerAngleLimit
) && (pidRuntime
.acroTrainerAxisState
[axis
] == 0)) {
578 if (angleSign
== setpointSign
) {
579 pidRuntime
.acroTrainerAxisState
[axis
] = angleSign
;
584 if (pidRuntime
.acroTrainerAxisState
[axis
] != 0) {
585 ret
= constrainf(((pidRuntime
.acroTrainerAngleLimit
* angleSign
) - currentAngle
) * pidRuntime
.acroTrainerGain
, -ACRO_TRAINER_SETPOINT_LIMIT
, ACRO_TRAINER_SETPOINT_LIMIT
);
588 // Not currently over the limit so project the angle based on current angle and
589 // gyro angular rate using a sliding window based on gyro rate (faster rotation means larger window.
590 // If the projected angle exceeds the limit then apply limiting to minimize overshoot.
591 // Calculate the lookahead window by scaling proportionally with gyro rate from 0-500dps
592 float checkInterval
= constrainf(fabsf(gyro
.gyroADCf
[axis
]) / ACRO_TRAINER_LOOKAHEAD_RATE_LIMIT
, 0.0f
, 1.0f
) * pidRuntime
.acroTrainerLookaheadTime
;
593 projectedAngle
= (gyro
.gyroADCf
[axis
] * checkInterval
) + currentAngle
;
594 const int projectedAngleSign
= acroTrainerSign(projectedAngle
);
595 if ((fabsf(projectedAngle
) > pidRuntime
.acroTrainerAngleLimit
) && (projectedAngleSign
== setpointSign
)) {
596 ret
= ((pidRuntime
.acroTrainerAngleLimit
* projectedAngleSign
) - projectedAngle
) * pidRuntime
.acroTrainerGain
;
605 if (axis
== pidRuntime
.acroTrainerDebugAxis
) {
606 DEBUG_SET(DEBUG_ACRO_TRAINER
, 0, lrintf(currentAngle
* 10.0f
));
607 DEBUG_SET(DEBUG_ACRO_TRAINER
, 1, pidRuntime
.acroTrainerAxisState
[axis
]);
608 DEBUG_SET(DEBUG_ACRO_TRAINER
, 2, lrintf(ret
));
609 DEBUG_SET(DEBUG_ACRO_TRAINER
, 3, lrintf(projectedAngle
* 10.0f
));
615 #endif // USE_ACRO_TRAINER
617 static float accelerationLimit(int axis
, float currentPidSetpoint
)
619 static float previousSetpoint
[XYZ_AXIS_COUNT
];
620 const float currentVelocity
= currentPidSetpoint
- previousSetpoint
[axis
];
622 if (fabsf(currentVelocity
) > pidRuntime
.maxVelocity
[axis
]) {
623 currentPidSetpoint
= (currentVelocity
> 0) ? previousSetpoint
[axis
] + pidRuntime
.maxVelocity
[axis
] : previousSetpoint
[axis
] - pidRuntime
.maxVelocity
[axis
];
626 previousSetpoint
[axis
] = currentPidSetpoint
;
627 return currentPidSetpoint
;
630 static void rotateVector(float v
[XYZ_AXIS_COUNT
], const float rotation
[XYZ_AXIS_COUNT
])
632 // rotate v around rotation vector rotation
633 // rotation in radians, all elements must be small
634 for (int i
= 0; i
< XYZ_AXIS_COUNT
; i
++) {
635 int i_1
= (i
+ 1) % 3;
636 int i_2
= (i
+ 2) % 3;
637 float newV
= v
[i_1
] + v
[i_2
] * rotation
[i
];
638 v
[i_2
] -= v
[i_1
] * rotation
[i
];
643 STATIC_UNIT_TESTED
void rotateItermAndAxisError(void)
645 if (pidRuntime
.itermRotation
646 #if defined(USE_ABSOLUTE_CONTROL)
647 || pidRuntime
.acGain
> 0 || debugMode
== DEBUG_AC_ERROR
650 const float gyroToAngle
= pidRuntime
.dT
* RAD
;
651 float rotationRads
[XYZ_AXIS_COUNT
];
652 for (int i
= FD_ROLL
; i
<= FD_YAW
; i
++) {
653 rotationRads
[i
] = gyro
.gyroADCf
[i
] * gyroToAngle
;
655 #if defined(USE_ABSOLUTE_CONTROL)
656 if (pidRuntime
.acGain
> 0 || debugMode
== DEBUG_AC_ERROR
) {
657 rotateVector(axisError
, rotationRads
);
660 if (pidRuntime
.itermRotation
) {
661 float v
[XYZ_AXIS_COUNT
];
662 for (int i
= 0; i
< XYZ_AXIS_COUNT
; i
++) {
665 rotateVector(v
, rotationRads
);
666 for (int i
= 0; i
< XYZ_AXIS_COUNT
; i
++) {
673 #if defined(USE_ITERM_RELAX)
674 #if defined(USE_ABSOLUTE_CONTROL)
675 STATIC_UNIT_TESTED
void applyAbsoluteControl(const int axis
, const float gyroRate
, float *currentPidSetpoint
, float *itermErrorRate
)
677 if (pidRuntime
.acGain
> 0 || debugMode
== DEBUG_AC_ERROR
) {
678 const float setpointLpf
= pt1FilterApply(&pidRuntime
.acLpf
[axis
], *currentPidSetpoint
);
679 const float setpointHpf
= fabsf(*currentPidSetpoint
- setpointLpf
);
680 float acErrorRate
= 0;
681 const float gmaxac
= setpointLpf
+ 2 * setpointHpf
;
682 const float gminac
= setpointLpf
- 2 * setpointHpf
;
683 if (gyroRate
>= gminac
&& gyroRate
<= gmaxac
) {
684 const float acErrorRate1
= gmaxac
- gyroRate
;
685 const float acErrorRate2
= gminac
- gyroRate
;
686 if (acErrorRate1
* axisError
[axis
] < 0) {
687 acErrorRate
= acErrorRate1
;
689 acErrorRate
= acErrorRate2
;
691 if (fabsf(acErrorRate
* pidRuntime
.dT
) > fabsf(axisError
[axis
]) ) {
692 acErrorRate
= -axisError
[axis
] * pidRuntime
.pidFrequency
;
695 acErrorRate
= (gyroRate
> gmaxac
? gmaxac
: gminac
) - gyroRate
;
698 if (isAirmodeActivated()) {
699 axisError
[axis
] = constrainf(axisError
[axis
] + acErrorRate
* pidRuntime
.dT
,
700 -pidRuntime
.acErrorLimit
, pidRuntime
.acErrorLimit
);
701 const float acCorrection
= constrainf(axisError
[axis
] * pidRuntime
.acGain
, -pidRuntime
.acLimit
, pidRuntime
.acLimit
);
702 *currentPidSetpoint
+= acCorrection
;
703 *itermErrorRate
+= acCorrection
;
704 DEBUG_SET(DEBUG_AC_CORRECTION
, axis
, lrintf(acCorrection
* 10));
705 if (axis
== FD_ROLL
) {
706 DEBUG_SET(DEBUG_ITERM_RELAX
, 3, lrintf(acCorrection
* 10));
709 DEBUG_SET(DEBUG_AC_ERROR
, axis
, lrintf(axisError
[axis
] * 10));
714 STATIC_UNIT_TESTED
void applyItermRelax(const int axis
, const float iterm
,
715 const float gyroRate
, float *itermErrorRate
, float *currentPidSetpoint
)
717 const float setpointLpf
= pt1FilterApply(&pidRuntime
.windupLpf
[axis
], *currentPidSetpoint
);
718 const float setpointHpf
= fabsf(*currentPidSetpoint
- setpointLpf
);
720 if (pidRuntime
.itermRelax
) {
721 if (axis
< FD_YAW
|| pidRuntime
.itermRelax
== ITERM_RELAX_RPY
|| pidRuntime
.itermRelax
== ITERM_RELAX_RPY_INC
) {
722 float itermRelaxThreshold
= ITERM_RELAX_SETPOINT_THRESHOLD
;
723 if (FLIGHT_MODE(ANGLE_MODE
)) {
724 itermRelaxThreshold
*= 0.2f
;
726 const float itermRelaxFactor
= MAX(0, 1 - setpointHpf
/ itermRelaxThreshold
);
727 const bool isDecreasingI
=
728 ((iterm
> 0) && (*itermErrorRate
< 0)) || ((iterm
< 0) && (*itermErrorRate
> 0));
729 if ((pidRuntime
.itermRelax
>= ITERM_RELAX_RP_INC
) && isDecreasingI
) {
730 // Do Nothing, use the precalculed itermErrorRate
731 } else if (pidRuntime
.itermRelaxType
== ITERM_RELAX_SETPOINT
) {
732 *itermErrorRate
*= itermRelaxFactor
;
733 } else if (pidRuntime
.itermRelaxType
== ITERM_RELAX_GYRO
) {
734 *itermErrorRate
= fapplyDeadband(setpointLpf
- gyroRate
, setpointHpf
);
736 *itermErrorRate
= 0.0f
;
739 if (axis
== FD_ROLL
) {
740 DEBUG_SET(DEBUG_ITERM_RELAX
, 0, lrintf(setpointHpf
));
741 DEBUG_SET(DEBUG_ITERM_RELAX
, 1, lrintf(itermRelaxFactor
* 100.0f
));
742 DEBUG_SET(DEBUG_ITERM_RELAX
, 2, lrintf(*itermErrorRate
));
746 #if defined(USE_ABSOLUTE_CONTROL)
747 applyAbsoluteControl(axis
, gyroRate
, currentPidSetpoint
, itermErrorRate
);
753 #ifdef USE_AIRMODE_LPF
754 void pidUpdateAirmodeLpf(float currentOffset
)
756 if (pidRuntime
.airmodeThrottleOffsetLimit
== 0.0f
) {
760 float offsetHpf
= currentOffset
* 2.5f
;
761 offsetHpf
= offsetHpf
- pt1FilterApply(&pidRuntime
.airmodeThrottleLpf2
, offsetHpf
);
763 // During high frequency oscillation 2 * currentOffset averages to the offset required to avoid mirroring of the waveform
764 pt1FilterApply(&pidRuntime
.airmodeThrottleLpf1
, offsetHpf
);
765 // Bring offset up immediately so the filter only applies to the decline
766 if (currentOffset
* pidRuntime
.airmodeThrottleLpf1
.state
>= 0 && fabsf(currentOffset
) > pidRuntime
.airmodeThrottleLpf1
.state
) {
767 pidRuntime
.airmodeThrottleLpf1
.state
= currentOffset
;
769 pidRuntime
.airmodeThrottleLpf1
.state
= constrainf(pidRuntime
.airmodeThrottleLpf1
.state
, -pidRuntime
.airmodeThrottleOffsetLimit
, pidRuntime
.airmodeThrottleOffsetLimit
);
772 float pidGetAirmodeThrottleOffset(void)
774 return pidRuntime
.airmodeThrottleLpf1
.state
;
778 static FAST_CODE_NOINLINE
void disarmOnImpact(void)
780 // if all sticks are within 5% of center, and throttle low, check accDelta for impacts
781 // threshold should be high enough to avoid unwanted disarms in the air on throttle chops
782 if (isAirmodeActivated() && getMaxRcDeflectionAbs() < 0.05f
&& mixerGetRcThrottle() < 0.05f
&&
783 fabsf(acc
.accDelta
) > pidRuntime
.ezLandingDisarmThreshold
) {
784 // disarm on accDelta transients
785 setArmingDisabled(ARMING_DISABLED_ARM_SWITCH
);
786 disarm(DISARM_REASON_LANDING
);
788 DEBUG_SET(DEBUG_EZLANDING
, 6, lrintf(getMaxRcDeflectionAbs() * 100.0f
));
789 DEBUG_SET(DEBUG_EZLANDING
, 7, lrintf(acc
.accDelta
));
792 #ifdef USE_LAUNCH_CONTROL
793 #define LAUNCH_CONTROL_MAX_RATE 100.0f
794 #define LAUNCH_CONTROL_MIN_RATE 5.0f
795 #define LAUNCH_CONTROL_ANGLE_WINDOW 10.0f // The remaining angle degrees where rate dampening starts
797 // Use the FAST_CODE_NOINLINE directive to avoid this code from being inlined into ITCM RAM to avoid overflow.
798 // The impact is possibly slightly slower performance on F7/H7 but they have more than enough
799 // processing power that it should be a non-issue.
800 static FAST_CODE_NOINLINE
float applyLaunchControl(int axis
, const rollAndPitchTrims_t
*angleTrim
)
804 // Scale the rates based on stick deflection only. Fixed rates with a max of 100deg/sec
805 // reached at 50% stick deflection. This keeps the launch control positioning consistent
806 // regardless of the user's rates.
807 if ((axis
== FD_PITCH
) || (pidRuntime
.launchControlMode
!= LAUNCH_CONTROL_MODE_PITCHONLY
)) {
808 const float stickDeflection
= constrainf(getRcDeflection(axis
), -0.5f
, 0.5f
);
809 ret
= LAUNCH_CONTROL_MAX_RATE
* stickDeflection
* 2;
813 // If ACC is enabled and a limit angle is set, then try to limit forward tilt
814 // to that angle and slow down the rate as the limit is approached to reduce overshoot
815 if ((axis
== FD_PITCH
) && (pidRuntime
.launchControlAngleLimit
> 0) && (ret
> 0)) {
816 const float currentAngle
= (attitude
.raw
[axis
] - angleTrim
->raw
[axis
]) / 10.0f
;
817 if (currentAngle
>= pidRuntime
.launchControlAngleLimit
) {
820 //for the last 10 degrees scale the rate from the current input to 5 dps
821 const float angleDelta
= pidRuntime
.launchControlAngleLimit
- currentAngle
;
822 if (angleDelta
<= LAUNCH_CONTROL_ANGLE_WINDOW
) {
823 ret
= scaleRangef(angleDelta
, 0, LAUNCH_CONTROL_ANGLE_WINDOW
, LAUNCH_CONTROL_MIN_RATE
, ret
);
835 static float getSterm(int axis
, const pidProfile_t
*pidProfile
)
838 const float sTerm
= getSetpointRate(axis
) / getMaxRcRate(axis
) * 1000.0f
*
839 (float)pidProfile
->pid
[axis
].S
/ 100.0f
;
841 DEBUG_SET(DEBUG_S_TERM
, axis
, lrintf(sTerm
));
851 NOINLINE
static void calculateSpaValues(const pidProfile_t
*pidProfile
)
854 for (int axis
= 0; axis
< XYZ_AXIS_COUNT
; axis
++) {
855 float currentRate
= getSetpointRate(axis
);
856 pidRuntime
.spa
[axis
] = 1.0f
- smoothStepUpTransition(
857 fabsf(currentRate
), pidProfile
->spa_center
[axis
], pidProfile
->spa_width
[axis
]);
858 DEBUG_SET(DEBUG_SPA
, axis
, lrintf(pidRuntime
.spa
[axis
] * 1000));
862 #endif // #ifdef USE_WING ... #else
865 NOINLINE
static void applySpa(int axis
, const pidProfile_t
*pidProfile
)
868 switch(pidProfile
->spa_mode
[axis
]){
870 pidData
[axis
].P
*= pidRuntime
.spa
[axis
];
871 pidData
[axis
].D
*= pidRuntime
.spa
[axis
];
872 pidData
[axis
].I
*= pidRuntime
.spa
[axis
];
875 pidData
[axis
].I
*= pidRuntime
.spa
[axis
];
877 case SPA_MODE_PD_I_FREEZE
:
878 pidData
[axis
].P
*= pidRuntime
.spa
[axis
];
879 pidData
[axis
].D
*= pidRuntime
.spa
[axis
];
881 case SPA_MODE_I_FREEZE
:
889 #endif // #ifdef USE_WING ... #else
892 // Betaflight pid controller, which will be maintained in the future with additional features specialised for current (mini) multirotor usage.
893 // Based on 2DOF reference design (matlab)
894 void FAST_CODE
pidController(const pidProfile_t
*pidProfile
, timeUs_t currentTimeUs
)
896 static float previousGyroRateDterm
[XYZ_AXIS_COUNT
];
897 static float previousRawGyroRateDterm
[XYZ_AXIS_COUNT
];
899 calculateSpaValues(pidProfile
);
902 const float tpaFactorKp
= (pidProfile
->tpa_mode
== TPA_MODE_PD
) ? pidRuntime
.tpaFactor
: 1.0f
;
904 const float tpaFactorKp
= pidRuntime
.tpaFactor
;
907 #ifdef USE_YAW_SPIN_RECOVERY
908 const bool yawSpinActive
= gyroYawSpinDetected();
911 const bool launchControlActive
= isLaunchControlActive();
914 static timeUs_t levelModeStartTimeUs
= 0;
915 static bool gpsRescuePreviousState
= false;
916 const rollAndPitchTrims_t
*angleTrim
= &accelerometerConfig()->accelerometerTrims
;
917 float horizonLevelStrength
= 0.0f
;
919 const bool gpsRescueIsActive
= FLIGHT_MODE(GPS_RESCUE_MODE
);
920 levelMode_e levelMode
;
921 if (FLIGHT_MODE(ANGLE_MODE
) || FLIGHT_MODE(HORIZON_MODE
) || gpsRescueIsActive
) {
922 if (pidRuntime
.levelRaceMode
&& !gpsRescueIsActive
) {
923 levelMode
= LEVEL_MODE_R
;
925 levelMode
= LEVEL_MODE_RP
;
928 // Keep track of when we entered a self-level mode so that we can
929 // add a guard time before crash recovery can activate.
930 // Also reset the guard time whenever GPS Rescue is activated.
931 if ((levelModeStartTimeUs
== 0) || (gpsRescueIsActive
&& !gpsRescuePreviousState
)) {
932 levelModeStartTimeUs
= currentTimeUs
;
935 // Calc horizonLevelStrength if needed
936 if (FLIGHT_MODE(HORIZON_MODE
)) {
937 horizonLevelStrength
= calcHorizonLevelStrength();
940 levelMode
= LEVEL_MODE_OFF
;
941 levelModeStartTimeUs
= 0;
944 gpsRescuePreviousState
= gpsRescueIsActive
;
947 UNUSED(currentTimeUs
);
951 if (pidRuntime
.antiGravityEnabled
) {
952 pidRuntime
.antiGravityThrottleD
*= pidRuntime
.antiGravityGain
;
953 // used later to increase pTerm
954 pidRuntime
.itermAccelerator
= pidRuntime
.antiGravityThrottleD
* ANTIGRAVITY_KI
;
956 pidRuntime
.antiGravityThrottleD
= 0.0f
;
957 pidRuntime
.itermAccelerator
= 0.0f
;
959 DEBUG_SET(DEBUG_ANTI_GRAVITY
, 2, lrintf((1 + (pidRuntime
.itermAccelerator
/ pidRuntime
.pidCoefficient
[FD_PITCH
].Ki
)) * 1000));
960 // amount of antigravity added relative to user's pitch iTerm coefficient
961 // used later to increase iTerm
963 // iTerm windup (attenuation of iTerm if motorMix range is large)
965 if (pidRuntime
.itermWindupPointInv
> 1.0f
) {
966 dynCi
= constrainf((1.0f
- getMotorMixRange()) * pidRuntime
.itermWindupPointInv
, 0.0f
, 1.0f
);
969 // Precalculate gyro delta for D-term here, this allows loop unrolling
970 float gyroRateDterm
[XYZ_AXIS_COUNT
];
971 for (int axis
= FD_ROLL
; axis
<= FD_YAW
; ++axis
) {
972 gyroRateDterm
[axis
] = gyro
.gyroADCf
[axis
];
974 // Log the unfiltered D for ROLL and PITCH
975 if (debugMode
== DEBUG_D_LPF
&& axis
!= FD_YAW
) {
976 const float delta
= (previousRawGyroRateDterm
[axis
] - gyroRateDterm
[axis
]) * pidRuntime
.pidFrequency
/ D_LPF_RAW_SCALE
;
977 previousRawGyroRateDterm
[axis
] = gyroRateDterm
[axis
];
978 DEBUG_SET(DEBUG_D_LPF
, axis
, lrintf(delta
)); // debug d_lpf 2 and 3 used for pre-TPA D
981 gyroRateDterm
[axis
] = pidRuntime
.dtermNotchApplyFn((filter_t
*) &pidRuntime
.dtermNotch
[axis
], gyroRateDterm
[axis
]);
982 gyroRateDterm
[axis
] = pidRuntime
.dtermLowpassApplyFn((filter_t
*) &pidRuntime
.dtermLowpass
[axis
], gyroRateDterm
[axis
]);
983 gyroRateDterm
[axis
] = pidRuntime
.dtermLowpass2ApplyFn((filter_t
*) &pidRuntime
.dtermLowpass2
[axis
], gyroRateDterm
[axis
]);
986 rotateItermAndAxisError();
988 #ifdef USE_RPM_FILTER
992 if (pidRuntime
.useEzDisarm
) {
996 // ----------PID controller----------
997 for (int axis
= FD_ROLL
; axis
<= FD_YAW
; ++axis
) {
999 float currentPidSetpoint
= getSetpointRate(axis
);
1000 if (pidRuntime
.maxVelocity
[axis
]) {
1001 currentPidSetpoint
= accelerationLimit(axis
, currentPidSetpoint
);
1003 // Yaw control is GYRO based, direct sticks control is applied to rate PID
1004 // When Race Mode is active PITCH control is also GYRO based in level or horizon mode
1005 #if defined(USE_ACC)
1006 pidRuntime
.axisInAngleMode
[axis
] = false;
1007 if (axis
< FD_YAW
) {
1008 if (levelMode
== LEVEL_MODE_RP
|| (levelMode
== LEVEL_MODE_R
&& axis
== FD_ROLL
)) {
1009 pidRuntime
.axisInAngleMode
[axis
] = true;
1010 currentPidSetpoint
= pidLevel(axis
, pidProfile
, angleTrim
, currentPidSetpoint
, horizonLevelStrength
);
1012 } else { // yaw axis only
1013 if (levelMode
== LEVEL_MODE_RP
) {
1014 // if earth referencing is requested, attenuate yaw axis setpoint when pitched or rolled
1015 // and send yawSetpoint to Angle code to modulate pitch and roll
1016 // code cost is 107 cycles when earthRef enabled, 20 otherwise, nearly all in cos_approx
1017 const float earthRefGain
= FLIGHT_MODE(GPS_RESCUE_MODE
) ? 1.0f
: pidRuntime
.angleEarthRef
;
1019 pidRuntime
.angleYawSetpoint
= currentPidSetpoint
;
1020 float maxAngleTargetAbs
= earthRefGain
* fmaxf( fabsf(pidRuntime
.angleTarget
[FD_ROLL
]), fabsf(pidRuntime
.angleTarget
[FD_PITCH
]) );
1021 maxAngleTargetAbs
*= (FLIGHT_MODE(HORIZON_MODE
)) ? horizonLevelStrength
: 1.0f
;
1022 // reduce compensation whenever Horizon uses less levelling
1023 currentPidSetpoint
*= cos_approx(DEGREES_TO_RADIANS(maxAngleTargetAbs
));
1024 DEBUG_SET(DEBUG_ANGLE_TARGET
, 2, currentPidSetpoint
); // yaw setpoint after attenuation
1030 #ifdef USE_ACRO_TRAINER
1031 if ((axis
!= FD_YAW
) && pidRuntime
.acroTrainerActive
&& !pidRuntime
.inCrashRecoveryMode
&& !launchControlActive
) {
1032 currentPidSetpoint
= applyAcroTrainer(axis
, angleTrim
, currentPidSetpoint
);
1034 #endif // USE_ACRO_TRAINER
1036 #ifdef USE_LAUNCH_CONTROL
1037 if (launchControlActive
) {
1038 #if defined(USE_ACC)
1039 currentPidSetpoint
= applyLaunchControl(axis
, angleTrim
);
1041 currentPidSetpoint
= applyLaunchControl(axis
, NULL
);
1046 // Handle yaw spin recovery - zero the setpoint on yaw to aid in recovery
1047 // It's not necessary to zero the set points for R/P because the PIDs will be zeroed below
1048 #ifdef USE_YAW_SPIN_RECOVERY
1049 if ((axis
== FD_YAW
) && yawSpinActive
) {
1050 currentPidSetpoint
= 0.0f
;
1052 #endif // USE_YAW_SPIN_RECOVERY
1054 // -----calculate error rate
1055 const float gyroRate
= gyro
.gyroADCf
[axis
]; // Process variable from gyro output in deg/sec
1056 float errorRate
= currentPidSetpoint
- gyroRate
; // r - y
1057 #if defined(USE_ACC)
1058 handleCrashRecovery(
1059 pidProfile
->crash_recovery
, angleTrim
, axis
, currentTimeUs
, gyroRate
,
1060 ¤tPidSetpoint
, &errorRate
);
1063 const float previousIterm
= pidData
[axis
].I
;
1064 float itermErrorRate
= errorRate
;
1066 #ifdef USE_ABSOLUTE_CONTROL
1067 const float uncorrectedSetpoint
= currentPidSetpoint
;
1070 #if defined(USE_ITERM_RELAX)
1071 if (!launchControlActive
&& !pidRuntime
.inCrashRecoveryMode
) {
1072 applyItermRelax(axis
, previousIterm
, gyroRate
, &itermErrorRate
, ¤tPidSetpoint
);
1073 errorRate
= currentPidSetpoint
- gyroRate
;
1076 #ifdef USE_ABSOLUTE_CONTROL
1077 const float setpointCorrection
= currentPidSetpoint
- uncorrectedSetpoint
;
1080 // --------low-level gyro-based PID based on 2DOF PID controller. ----------
1082 // -----calculate P component
1083 pidData
[axis
].P
= pidRuntime
.pidCoefficient
[axis
].Kp
* errorRate
* tpaFactorKp
;
1084 if (axis
== FD_YAW
) {
1085 pidData
[axis
].P
= pidRuntime
.ptermYawLowpassApplyFn((filter_t
*) &pidRuntime
.ptermYawLowpass
, pidData
[axis
].P
);
1089 // -----calculate I component
1090 float Ki
= pidRuntime
.pidCoefficient
[axis
].Ki
;
1091 #ifdef USE_LAUNCH_CONTROL
1092 // if launch control is active override the iterm gains and apply iterm windup protection to all axes
1093 if (launchControlActive
) {
1094 Ki
= pidRuntime
.launchControlKi
;
1098 if (axis
== FD_YAW
) {
1099 pidRuntime
.itermAccelerator
= 0.0f
; // no antigravity on yaw iTerm
1103 float iTermChange
= (Ki
+ pidRuntime
.itermAccelerator
) * dynCi
* pidRuntime
.dT
* itermErrorRate
;
1106 if (pidProfile
->spa_mode
[axis
] != SPA_MODE_OFF
) {
1107 // slowing down I-term change, or even making it zero if setpoint is high enough
1108 iTermChange
*= pidRuntime
.spa
[axis
];
1110 #endif // #ifdef USE_WING
1112 pidData
[axis
].I
= constrainf(previousIterm
+ iTermChange
, -pidRuntime
.itermLimit
, pidRuntime
.itermLimit
);
1114 // -----calculate D component
1116 float pidSetpointDelta
= 0;
1118 #ifdef USE_FEEDFORWARD
1119 if (FLIGHT_MODE(ANGLE_MODE
) && pidRuntime
.axisInAngleMode
[axis
]) {
1120 // this axis is fully under self-levelling control
1121 // it will already have stick based feedforward applied in the input to their angle setpoint
1122 // a simple setpoint Delta can be used to for PID feedforward element for motor lag on these axes
1123 // however RC steps come in, via angle setpoint
1124 // and setpoint RC smoothing must have a cutoff half normal to remove those steps completely
1125 // the RC stepping does not come in via the feedforward, which is very well smoothed already
1126 // if uncommented, and the forcing to zero is removed, the two following lines will restore PID feedforward to angle mode axes
1127 // but for now let's see how we go without it (which was the case before 4.5 anyway)
1128 // pidSetpointDelta = currentPidSetpoint - pidRuntime.previousPidSetpoint[axis];
1129 // pidSetpointDelta *= pidRuntime.pidFrequency * pidRuntime.angleFeedforwardGain;
1130 pidSetpointDelta
= 0.0f
;
1132 // the axis is operating as a normal acro axis, so use normal feedforard from rc.c
1133 pidSetpointDelta
= getFeedforward(axis
);
1136 pidRuntime
.previousPidSetpoint
[axis
] = currentPidSetpoint
; // this is the value sent to blackbox, and used for Dmin setpoint
1138 // disable D if launch control is active
1139 if ((pidRuntime
.pidCoefficient
[axis
].Kd
> 0) && !launchControlActive
) {
1140 // Divide rate change by dT to get differential (ie dr/dt).
1141 // dT is fixed and calculated from the target PID loop time
1142 // This is done to avoid DTerm spikes that occur with dynamically
1143 // calculated deltaT whenever another task causes the PID
1144 // loop execution to be delayed.
1145 const float delta
= - (gyroRateDterm
[axis
] - previousGyroRateDterm
[axis
]) * pidRuntime
.pidFrequency
;
1146 float preTpaD
= pidRuntime
.pidCoefficient
[axis
].Kd
* delta
;
1148 #if defined(USE_ACC)
1149 if (cmpTimeUs(currentTimeUs
, levelModeStartTimeUs
) > CRASH_RECOVERY_DETECTION_DELAY_US
) {
1150 detectAndSetCrashRecovery(pidProfile
->crash_recovery
, axis
, currentTimeUs
, delta
, errorRate
);
1154 #if defined(USE_D_MIN)
1155 float dMinFactor
= 1.0f
;
1156 if (pidRuntime
.dMinPercent
[axis
] > 0) {
1157 float dMinGyroFactor
= pt2FilterApply(&pidRuntime
.dMinRange
[axis
], delta
);
1158 dMinGyroFactor
= fabsf(dMinGyroFactor
) * pidRuntime
.dMinGyroGain
;
1159 const float dMinSetpointFactor
= (fabsf(pidSetpointDelta
)) * pidRuntime
.dMinSetpointGain
;
1160 dMinFactor
= MAX(dMinGyroFactor
, dMinSetpointFactor
);
1161 dMinFactor
= pidRuntime
.dMinPercent
[axis
] + (1.0f
- pidRuntime
.dMinPercent
[axis
]) * dMinFactor
;
1162 dMinFactor
= pt2FilterApply(&pidRuntime
.dMinLowpass
[axis
], dMinFactor
);
1163 dMinFactor
= MIN(dMinFactor
, 1.0f
);
1164 if (axis
== FD_ROLL
) {
1165 DEBUG_SET(DEBUG_D_MIN
, 0, lrintf(dMinGyroFactor
* 100));
1166 DEBUG_SET(DEBUG_D_MIN
, 1, lrintf(dMinSetpointFactor
* 100));
1167 DEBUG_SET(DEBUG_D_MIN
, 2, lrintf(pidRuntime
.pidCoefficient
[axis
].Kd
* dMinFactor
* 10 / DTERM_SCALE
));
1168 } else if (axis
== FD_PITCH
) {
1169 DEBUG_SET(DEBUG_D_MIN
, 3, lrintf(pidRuntime
.pidCoefficient
[axis
].Kd
* dMinFactor
* 10 / DTERM_SCALE
));
1173 // Apply the dMinFactor
1174 preTpaD
*= dMinFactor
;
1177 pidData
[axis
].D
= preTpaD
* pidRuntime
.tpaFactor
;
1179 // Log the value of D pre application of TPA
1180 if (axis
!= FD_YAW
) {
1181 DEBUG_SET(DEBUG_D_LPF
, axis
- FD_ROLL
+ 2, lrintf(preTpaD
* D_LPF_PRE_TPA_SCALE
));
1184 pidData
[axis
].D
= 0;
1185 if (axis
!= FD_YAW
) {
1186 DEBUG_SET(DEBUG_D_LPF
, axis
- FD_ROLL
+ 2, 0);
1190 previousGyroRateDterm
[axis
] = gyroRateDterm
[axis
];
1192 // -----calculate feedforward component
1194 #ifdef USE_ABSOLUTE_CONTROL
1195 // include abs control correction in feedforward
1196 pidSetpointDelta
+= setpointCorrection
- pidRuntime
.oldSetpointCorrection
[axis
];
1197 pidRuntime
.oldSetpointCorrection
[axis
] = setpointCorrection
;
1199 // no feedforward in launch control
1200 const float feedforwardGain
= launchControlActive
? 0.0f
: pidRuntime
.pidCoefficient
[axis
].Kf
;
1201 pidData
[axis
].F
= feedforwardGain
* pidSetpointDelta
;
1203 #ifdef USE_YAW_SPIN_RECOVERY
1204 if (yawSpinActive
) {
1205 pidData
[axis
].I
= 0; // in yaw spin always disable I
1206 if (axis
<= FD_PITCH
) {
1207 // zero PIDs on pitch and roll leaving yaw P to correct spin
1208 pidData
[axis
].P
= 0;
1209 pidData
[axis
].D
= 0;
1210 pidData
[axis
].F
= 0;
1211 pidData
[axis
].S
= 0;
1214 #endif // USE_YAW_SPIN_RECOVERY
1216 #ifdef USE_LAUNCH_CONTROL
1217 // Disable P/I appropriately based on the launch control mode
1218 if (launchControlActive
) {
1219 // if not using FULL mode then disable I accumulation on yaw as
1220 // yaw has a tendency to windup. Otherwise limit yaw iterm accumulation.
1221 const int launchControlYawItermLimit
= (pidRuntime
.launchControlMode
== LAUNCH_CONTROL_MODE_FULL
) ? LAUNCH_CONTROL_YAW_ITERM_LIMIT
: 0;
1222 pidData
[FD_YAW
].I
= constrainf(pidData
[FD_YAW
].I
, -launchControlYawItermLimit
, launchControlYawItermLimit
);
1224 // for pitch-only mode we disable everything except pitch P/I
1225 if (pidRuntime
.launchControlMode
== LAUNCH_CONTROL_MODE_PITCHONLY
) {
1226 pidData
[FD_ROLL
].P
= 0;
1227 pidData
[FD_ROLL
].I
= 0;
1228 pidData
[FD_YAW
].P
= 0;
1229 // don't let I go negative (pitch backwards) as front motors are limited in the mixer
1230 pidData
[FD_PITCH
].I
= MAX(0.0f
, pidData
[FD_PITCH
].I
);
1235 // Add P boost from antiGravity when sticks are close to zero
1236 if (axis
!= FD_YAW
) {
1237 float agSetpointAttenuator
= fabsf(currentPidSetpoint
) / 50.0f
;
1238 agSetpointAttenuator
= MAX(agSetpointAttenuator
, 1.0f
);
1239 // attenuate effect if turning more than 50 deg/s, half at 100 deg/s
1240 const float antiGravityPBoost
= 1.0f
+ (pidRuntime
.antiGravityThrottleD
/ agSetpointAttenuator
) * pidRuntime
.antiGravityPGain
;
1241 pidData
[axis
].P
*= antiGravityPBoost
;
1242 if (axis
== FD_PITCH
) {
1243 DEBUG_SET(DEBUG_ANTI_GRAVITY
, 3, lrintf(antiGravityPBoost
* 1000));
1247 pidData
[axis
].S
= getSterm(axis
, pidProfile
);
1248 applySpa(axis
, pidProfile
);
1250 // calculating the PID sum
1251 const float pidSum
= pidData
[axis
].P
+ pidData
[axis
].I
+ pidData
[axis
].D
+ pidData
[axis
].F
+ pidData
[axis
].S
;
1252 #ifdef USE_INTEGRATED_YAW_CONTROL
1253 if (axis
== FD_YAW
&& pidRuntime
.useIntegratedYaw
) {
1254 pidData
[axis
].Sum
+= pidSum
* pidRuntime
.dT
* 100.0f
;
1255 pidData
[axis
].Sum
-= pidData
[axis
].Sum
* pidRuntime
.integratedYawRelax
/ 100000.0f
* pidRuntime
.dT
/ 0.000125f
;
1259 pidData
[axis
].Sum
= pidSum
;
1263 // Disable PID control if at zero throttle or if gyro overflow detected
1264 // This may look very innefficient, but it is done on purpose to always show real CPU usage as in flight
1265 if (!pidRuntime
.pidStabilisationEnabled
|| gyroOverflowDetected()) {
1266 for (int axis
= FD_ROLL
; axis
<= FD_YAW
; ++axis
) {
1267 pidData
[axis
].P
= 0;
1268 pidData
[axis
].I
= 0;
1269 pidData
[axis
].D
= 0;
1270 pidData
[axis
].F
= 0;
1271 pidData
[axis
].S
= 0;
1273 pidData
[axis
].Sum
= 0;
1275 } else if (pidRuntime
.zeroThrottleItermReset
) {
1280 bool crashRecoveryModeActive(void)
1282 return pidRuntime
.inCrashRecoveryMode
;
1285 #ifdef USE_ACRO_TRAINER
1286 void pidSetAcroTrainerState(bool newState
)
1288 if (pidRuntime
.acroTrainerActive
!= newState
) {
1290 pidAcroTrainerInit();
1292 pidRuntime
.acroTrainerActive
= newState
;
1295 #endif // USE_ACRO_TRAINER
1297 void pidSetAntiGravityState(bool newState
)
1299 if (newState
!= pidRuntime
.antiGravityEnabled
) {
1300 // reset the accelerator on state changes
1301 pidRuntime
.itermAccelerator
= 0.0f
;
1303 pidRuntime
.antiGravityEnabled
= newState
;
1306 bool pidAntiGravityEnabled(void)
1308 return pidRuntime
.antiGravityEnabled
;
1312 void dynLpfDTermUpdate(float throttle
)
1314 if (pidRuntime
.dynLpfFilter
!= DYN_LPF_NONE
) {
1316 if (pidRuntime
.dynLpfCurveExpo
> 0) {
1317 cutoffFreq
= dynLpfCutoffFreq(throttle
, pidRuntime
.dynLpfMin
, pidRuntime
.dynLpfMax
, pidRuntime
.dynLpfCurveExpo
);
1319 cutoffFreq
= fmaxf(dynThrottle(throttle
) * pidRuntime
.dynLpfMax
, pidRuntime
.dynLpfMin
);
1322 switch (pidRuntime
.dynLpfFilter
) {
1324 for (int axis
= 0; axis
< XYZ_AXIS_COUNT
; axis
++) {
1325 pt1FilterUpdateCutoff(&pidRuntime
.dtermLowpass
[axis
].pt1Filter
, pt1FilterGain(cutoffFreq
, pidRuntime
.dT
));
1328 case DYN_LPF_BIQUAD
:
1329 for (int axis
= 0; axis
< XYZ_AXIS_COUNT
; axis
++) {
1330 biquadFilterUpdateLPF(&pidRuntime
.dtermLowpass
[axis
].biquadFilter
, cutoffFreq
, targetPidLooptime
);
1334 for (int axis
= 0; axis
< XYZ_AXIS_COUNT
; axis
++) {
1335 pt2FilterUpdateCutoff(&pidRuntime
.dtermLowpass
[axis
].pt2Filter
, pt2FilterGain(cutoffFreq
, pidRuntime
.dT
));
1339 for (int axis
= 0; axis
< XYZ_AXIS_COUNT
; axis
++) {
1340 pt3FilterUpdateCutoff(&pidRuntime
.dtermLowpass
[axis
].pt3Filter
, pt3FilterGain(cutoffFreq
, pidRuntime
.dT
));
1348 float dynLpfCutoffFreq(float throttle
, uint16_t dynLpfMin
, uint16_t dynLpfMax
, uint8_t expo
)
1350 const float expof
= expo
/ 10.0f
;
1351 const float curve
= throttle
* (1 - throttle
) * expof
+ throttle
;
1352 return (dynLpfMax
- dynLpfMin
) * curve
+ dynLpfMin
;
1355 void pidSetItermReset(bool enabled
)
1357 pidRuntime
.zeroThrottleItermReset
= enabled
;
1360 float pidGetPreviousSetpoint(int axis
)
1362 return pidRuntime
.previousPidSetpoint
[axis
];
1365 float pidGetDT(void)
1367 return pidRuntime
.dT
;
1370 float pidGetPidFrequency(void)
1372 return pidRuntime
.pidFrequency
;