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/>.
24 #include "common/time.h"
25 #include "common/filter.h"
28 #define MAX_PID_PROCESS_DENOM 16
29 #define PID_CONTROLLER_BETAFLIGHT 1
30 #define PID_MIXER_SCALING 1000.0f
31 #define PID_SERVO_MIXER_SCALING 0.7f
32 #define PIDSUM_LIMIT 500
33 #define PIDSUM_LIMIT_YAW 400
34 #define PIDSUM_LIMIT_MIN 100
35 #define PIDSUM_LIMIT_MAX 1000
37 // Scaling factors for Pids for better tunable range in configurator for betaflight pid controller. The scaling is based on legacy pid controller or previous float
38 #define PTERM_SCALE 0.032029f
39 #define ITERM_SCALE 0.244381f
40 #define DTERM_SCALE 0.000529f
42 // The constant scale factor to replace the Kd component of the feedforward calculation.
43 // This value gives the same "feel" as the previous Kd default of 26 (26 * DTERM_SCALE)
44 #define FEEDFORWARD_SCALE 0.013754f
61 SUPEREXPO_YAW_OFF
= 0,
67 PID_STABILISATION_OFF
= 0,
69 } pidStabilisationState_e
;
72 PID_CRASH_RECOVERY_OFF
= 0,
73 PID_CRASH_RECOVERY_ON
,
74 PID_CRASH_RECOVERY_BEEP
77 typedef struct pidf_s
{
102 typedef struct pidProfile_s
{
103 uint16_t yaw_lowpass_hz
; // Additional yaw filter when yaw axis too noisy
104 uint16_t dterm_lowpass_hz
; // Delta Filter in hz
105 uint16_t dterm_notch_hz
; // Biquad dterm notch hz
106 uint16_t dterm_notch_cutoff
; // Biquad dterm notch low cutoff
108 pidf_t pid
[PID_ITEM_COUNT
];
110 uint8_t dterm_filter_type
; // Filter selection for dterm
111 uint8_t itermWindupPointPercent
; // Experimental ITerm windup threshold, percent motor saturation
112 uint16_t pidSumLimit
;
113 uint16_t pidSumLimitYaw
;
114 uint8_t pidAtMinThrottle
; // Disable/Enable pids on zero throttle. Normally even without airmode P and D would be active.
115 uint8_t levelAngleLimit
; // Max angle in degrees in level mode
117 uint8_t horizon_tilt_effect
; // inclination factor for Horizon mode
118 uint8_t horizon_tilt_expert_mode
; // OFF or ON
120 // Betaflight PID controller parameters
121 uint8_t antiGravityMode
; // type of anti gravity method
122 uint16_t itermThrottleThreshold
; // max allowed throttle delta before iterm accelerated in ms
123 uint16_t itermAcceleratorGain
; // Iterm Accelerator Gain when itermThrottlethreshold is hit
124 uint16_t yawRateAccelLimit
; // yaw accel limiter for deg/sec/ms
125 uint16_t rateAccelLimit
; // accel limiter roll/pitch deg/sec/ms
126 uint16_t crash_dthreshold
; // dterm crash value
127 uint16_t crash_gthreshold
; // gyro crash value
128 uint16_t crash_setpoint_threshold
; // setpoint must be below this value to detect crash, so flips and rolls are not interpreted as crashes
129 uint16_t crash_time
; // ms
130 uint16_t crash_delay
; // ms
131 uint8_t crash_recovery_angle
; // degrees
132 uint8_t crash_recovery_rate
; // degree/second
133 uint8_t vbatPidCompensation
; // Scale PIDsum to battery voltage
134 uint8_t feedForwardTransition
; // Feed forward weight transition
135 uint16_t crash_limit_yaw
; // limits yaw errorRate, so crashes don't cause huge throttle increase
137 uint16_t dterm_lowpass2_hz
; // Extra PT1 Filter on D in hz
138 uint8_t crash_recovery
; // off, on, on and beeps when it is in crash recovery mode
139 uint8_t throttle_boost
; // how much should throttle be boosted during transient changes 0-100, 100 adds 10x hpf filtered throttle
140 uint8_t throttle_boost_cutoff
; // Which cutoff frequency to use for throttle boost. higher cutoffs keep the boost on for shorter. Specified in hz.
141 uint8_t iterm_rotation
; // rotates iterm to translate world errors to local coordinate system
142 uint8_t smart_feedforward
; // takes only the larger of P and the D weight feed forward term if they have the same sign.
143 uint8_t iterm_relax_type
; // Specifies type of relax algorithm
144 uint8_t iterm_relax_cutoff
; // This cutoff frequency specifies a low pass filter which predicts average response of the quad to setpoint
145 uint8_t iterm_relax
; // Enable iterm suppression during stick input
146 uint8_t acro_trainer_angle_limit
; // Acro trainer roll/pitch angle limit in degrees
147 uint8_t acro_trainer_debug_axis
; // The axis for which record debugging values are captured 0=roll, 1=pitch
148 uint8_t acro_trainer_gain
; // The strength of the limiting. Raising may reduce overshoot but also lead to oscillation around the angle limit
149 uint16_t acro_trainer_lookahead_ms
; // The lookahead window in milliseconds used to reduce overshoot
150 uint8_t abs_control_gain
; // How strongly should the absolute accumulated error be corrected for
151 uint8_t abs_control_limit
; // Limit to the correction
152 uint8_t abs_control_error_limit
; // Limit to the accumulated error
155 #ifndef USE_OSD_SLAVE
156 PG_DECLARE_ARRAY(pidProfile_t
, MAX_PROFILE_COUNT
, pidProfiles
);
159 typedef struct pidConfig_s
{
160 uint8_t pid_process_denom
; // Processing denominator for PID controller vs gyro sampling rate
161 uint8_t runaway_takeoff_prevention
; // off, on - enables pidsum runaway disarm logic
162 uint16_t runaway_takeoff_deactivate_delay
; // delay in ms for "in-flight" conditions before deactivation (successful flight)
163 uint8_t runaway_takeoff_deactivate_throttle
; // minimum throttle percent required during deactivation phase
166 PG_DECLARE(pidConfig_t
, pidConfig
);
168 union rollAndPitchTrims_u
;
169 void pidController(const pidProfile_t
*pidProfile
, const union rollAndPitchTrims_u
*angleTrim
, timeUs_t currentTimeUs
);
171 typedef struct pidAxisData_s
{
180 extern pidAxisData_t pidData
[3];
182 extern uint32_t targetPidLooptime
;
184 extern float throttleBoost
;
185 extern pt1Filter_t throttleLpf
;
187 void pidResetITerm(void);
188 void pidStabilisationState(pidStabilisationState_e pidControllerState
);
189 void pidSetItermAccelerator(float newItermAccelerator
);
190 void pidInitFilters(const pidProfile_t
*pidProfile
);
191 void pidInitConfig(const pidProfile_t
*pidProfile
);
192 void pidInit(const pidProfile_t
*pidProfile
);
193 void pidCopyProfile(uint8_t dstPidProfileIndex
, uint8_t srcPidProfileIndex
);
194 bool crashRecoveryModeActive(void);
195 void pidAcroTrainerInit(void);
196 void pidSetAcroTrainerState(bool newState
);
197 void pidInitSetpointDerivativeLpf(uint16_t filterCutoff
, uint8_t debugAxis
, uint8_t filterType
);
198 void pidUpdateSetpointDerivativeLpf(uint16_t filterCutoff
);
199 void pidUpdateAntiGravityThrottleFilter(float throttle
);
200 bool pidOsdAntiGravityActive(void);
201 bool pidOsdAntiGravityMode(void);
202 void pidSetAntiGravityState(bool newState
);
203 bool pidAntiGravityEnabled(void);