2 * This file is part of Cleanflight.
4 * Cleanflight is free software: you can redistribute it and/or modify
5 * it under the terms of the GNU General Public License as published by
6 * the Free Software Foundation, either version 3 of the License, or
7 * (at your option) any later version.
9 * Cleanflight is distributed in the hope that it will be useful,
10 * but WITHOUT ANY WARRANTY; without even the implied warranty of
11 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
12 * GNU General Public License for more details.
14 * You should have received a copy of the GNU General Public License
15 * along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
25 #include "build/build_config.h"
27 #include "common/axis.h"
28 #include "common/maths.h"
29 #include "common/filter.h"
31 #include "config/parameter_group.h"
32 #include "config/parameter_group_ids.h"
33 #include "config/config_reset.h"
34 #include "config/profile.h"
36 #include "drivers/sensor.h"
37 #include "drivers/accgyro.h"
38 #include "drivers/gyro_sync.h"
40 #include "sensors/sensors.h"
41 #include "sensors/acceleration.h"
45 #include "fc/rc_controls.h"
46 #include "fc/rate_profile.h"
48 #include "flight/pid.h"
49 #include "flight/imu.h"
51 uint32_t targetPidLooptime
;
56 int32_t axisPID_P
[3], axisPID_I
[3], axisPID_D
[3];
59 // PIDweight is a scale factor for PIDs which is derived from the throttle and TPA setting, and 100 = 100% scale means no PID reduction
62 int32_t lastITerm
[3], ITermLimit
[3];
63 float lastITermf
[3], ITermLimitf
[3];
65 pt1Filter_t deltaFilter
[3];
66 pt1Filter_t yawFilter
;
67 biquadFilter_t dtermFilterLpf
[3];
68 biquadFilter_t dtermFilterNotch
[3];
69 bool dtermNotchInitialised
, dtermBiquadLpfInitialised
;
72 void pidLuxFloat(const pidProfile_t
*pidProfile
, const controlRateConfig_t
*controlRateConfig
,
73 uint16_t max_angle_inclination
, const rollAndPitchTrims_t
*angleTrim
, const rxConfig_t
*rxConfig
);
74 #ifdef USE_PID_MWREWRITE
75 void pidMultiWiiRewrite(const pidProfile_t
*pidProfile
, const controlRateConfig_t
*controlRateConfig
,
76 uint16_t max_angle_inclination
, const rollAndPitchTrims_t
*angleTrim
, const rxConfig_t
*rxConfig
);
79 void pidMultiWii23(const pidProfile_t
*pidProfile
, const controlRateConfig_t
*controlRateConfig
,
80 uint16_t max_angle_inclination
, const rollAndPitchTrims_t
*angleTrim
, const rxConfig_t
*rxConfig
);
83 pidControllerFuncPtr pid_controller
= pidLuxFloat
;
85 PG_REGISTER_PROFILE_WITH_RESET_TEMPLATE(pidProfile_t
, pidProfile
, PG_PID_PROFILE
, 0);
87 PG_RESET_TEMPLATE(pidProfile_t
, pidProfile
,
88 .pidController
= PID_CONTROLLER_LUX_FLOAT
,
101 .P8
[PIDPOS
] = 15, // POSHOLD_P * 100
102 .I8
[PIDPOS
] = 0, // POSHOLD_I * 100
104 .P8
[PIDPOSR
] = 34, // POSHOLD_RATE_P * 10
105 .I8
[PIDPOSR
] = 14, // POSHOLD_RATE_I * 100
106 .D8
[PIDPOSR
] = 53, // POSHOLD_RATE_D * 1000
107 .P8
[PIDNAVR
] = 25, // NAV_P * 10
108 .I8
[PIDNAVR
] = 33, // NAV_I * 100
109 .D8
[PIDNAVR
] = 83, // NAV_D * 1000
118 .yaw_p_limit
= YAW_P_LIMIT_MAX
,
120 .dterm_filter_type
= FILTER_BIQUAD
,
121 .dterm_lpf_hz
= 100, // filtering ON by default
122 .dterm_notch_hz
= 260,
123 .dterm_notch_cutoff
= 160,
124 .deltaMethod
= PID_DELTA_FROM_MEASUREMENT
,
125 .horizon_tilt_effect
= 75,
126 .horizon_tilt_mode
= HORIZON_TILT_MODE_SAFE
,
133 STATIC_UNIT_TESTED
void pidResetDt(void)
140 if (!dT
) dT
= (float)targetPidLooptime
* 0.000001f
;
145 void pidSetTargetLooptime(uint32_t pidLooptime
)
147 targetPidLooptime
= pidLooptime
;
150 void pidInitFilters(const pidProfile_t
*pidProfile
)
154 if (pidProfile
->dterm_notch_hz
&& !dtermNotchInitialised
) {
155 for (axis
= 0; axis
< 3; axis
++) biquadFilterInitNotch(&dtermFilterNotch
[axis
], pidProfile
->dterm_notch_hz
, targetPidLooptime
, pidProfile
->dterm_notch_hz
, pidProfile
->dterm_notch_cutoff
);
156 dtermNotchInitialised
= true;
159 if (pidProfile
->dterm_filter_type
== FILTER_BIQUAD
) {
160 if (pidProfile
->dterm_lpf_hz
&& !dtermBiquadLpfInitialised
) {
161 for (axis
= 0; axis
< 3; axis
++) biquadFilterInitLPF(&dtermFilterLpf
[axis
], pidProfile
->dterm_lpf_hz
, targetPidLooptime
);
162 dtermBiquadLpfInitialised
= true;
167 void pidResetITerm(void)
169 for (int axis
= 0; axis
< 3; axis
++) {
171 lastITermf
[axis
] = 0.0f
;
175 void pidSetController(pidControllerType_e type
)
179 case PID_CONTROLLER_LUX_FLOAT
:
180 pid_controller
= pidLuxFloat
;
182 #ifdef USE_PID_MWREWRITE
183 case PID_CONTROLLER_MWREWRITE
:
184 pid_controller
= pidMultiWiiRewrite
;
188 case PID_CONTROLLER_MW23
:
189 pid_controller
= pidMultiWii23
;
195 // calculates strength of horizon leveling; 0 = none, 100 = most leveling
196 int calcHorizonLevelStrength(uint16_t rxConfigMidrc
, int horizonTiltEffect
,
197 uint8_t horizonTiltMode
, int horizonSensitivity
)
199 // get raw stick positions (-500 to 500):
200 const int32_t stickPosAil
= getRcStickDeflection(FD_ROLL
, rxConfigMidrc
);
201 const int32_t stickPosEle
= getRcStickDeflection(FD_PITCH
, rxConfigMidrc
);
203 // 0 at center stick, 500 at max stick deflection:
204 const int32_t mostDeflectedPos
= MAX(ABS(stickPosAil
), ABS(stickPosEle
));
206 // start with 100 at center stick, 0 at max stick deflection:
207 int horizonLevelStrength
= (500 - mostDeflectedPos
) / 5;
209 // 0 at level, 900 at vertical, 1800 at inverted (degrees * 10)
210 const int currentInclination
= MAX(ABS(attitude
.values
.roll
),
211 ABS(attitude
.values
.pitch
));
213 // horizonTiltMode: SAFE = leveling always active when sticks centered,
214 // EXPERT = leveling can be totally off when inverted
215 if (horizonTiltMode
== HORIZON_TILT_MODE_EXPERT
) {
216 if (horizonTiltEffect
< 175) {
217 // horizonTiltEffect 0 to 125 => 2700 to 900
218 // (represents where leveling goes to zero):
219 const int cutoffDeciDegrees
= (175-horizonTiltEffect
) * 18;
220 // inclinationLevelRatio (0 to 100) is smaller (less leveling)
221 // for larger inclinations; 0 at cutoffDeciDegrees value:
222 const int inclinationLevelRatio
= constrain(
223 (((cutoffDeciDegrees
-currentInclination
)*10) /
224 (cutoffDeciDegrees
/10)), 0, 100);
225 // apply configured horizon sensitivity:
226 if (horizonSensitivity
<= 0) { // zero means no leveling
227 horizonLevelStrength
= 0;
229 // when stick is near center (horizonLevelStrength ~= 100)
230 // H_sensitivity value has little effect,
231 // when stick is deflected (horizonLevelStrength near 0)
232 // H_sensitivity value has more effect:
233 horizonLevelStrength
= (horizonLevelStrength
- 100) *
234 100 / horizonSensitivity
+ 100;
236 // apply inclination ratio, which may lower leveling
237 // to zero regardless of stick position:
238 horizonLevelStrength
= horizonLevelStrength
* inclinationLevelRatio
/ 100;
241 horizonLevelStrength
= 0;
242 } else { // horizon_tilt_mode = SAFE (leveling always active when sticks centered)
244 if (horizonTiltEffect
> 0) {
245 // ratio of 100 to 0 (larger means more leveling):
246 const int factorRatio
= 100 - horizonTiltEffect
;
247 // inclinationLevelRatio (0 to 100) is smaller (less leveling)
248 // for larger inclinations, goes to 100 at inclination level:
249 int inclinationLevelRatio
=
250 ((1800-currentInclination
)/18 * (100-factorRatio
)) / 100 + factorRatio
;
251 // apply ratio to configured horizon sensitivity:
252 sensitFact
= horizonSensitivity
* inclinationLevelRatio
/ 100;
254 else // horizonTiltEffect=0 for "old" functionality
255 sensitFact
= horizonSensitivity
;
257 if (sensitFact
<= 0) { // zero means no leveling
258 horizonLevelStrength
= 0;
260 // when stick is near center (horizonLevelStrength ~= 100)
261 // sensitFact value has little effect,
262 // when stick is deflected (horizonLevelStrength near 0)
263 // sensitFact value has more effect:
264 horizonLevelStrength
= (horizonLevelStrength
- 100) * 100 / sensitFact
+ 100;
267 return constrain(horizonLevelStrength
, 0, 100);