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/>.
24 #include "build_config.h"
26 #include "common/axis.h"
27 #include "common/maths.h"
28 #include "common/filter.h"
30 #include "drivers/sensor.h"
31 #include "drivers/accgyro.h"
32 #include "drivers/gyro_sync.h"
34 #include "sensors/sensors.h"
35 #include "sensors/gyro.h"
36 #include "sensors/acceleration.h"
40 #include "io/rc_controls.h"
43 #include "flight/pid.h"
44 #include "flight/imu.h"
45 #include "flight/navigation.h"
46 #include "flight/gtune.h"
48 #include "config/runtime_config.h"
51 extern bool motorLimitReached
;
52 extern bool allowITermShrinkOnly
;
57 int32_t axisPID_P
[3], axisPID_I
[3], axisPID_D
[3];
60 // PIDweight is a scale factor for PIDs which is derived from the throttle and TPA setting, and 100 = 100% scale means no PID reduction
61 uint8_t dynP8
[3], dynI8
[3], dynD8
[3], PIDweight
[3];
63 static int32_t errorGyroI
[3] = { 0, 0, 0 };
64 static float errorGyroIf
[3] = { 0.0f
, 0.0f
, 0.0f
};
66 static void pidRewrite(pidProfile_t
*pidProfile
, controlRateConfig_t
*controlRateConfig
,
67 uint16_t max_angle_inclination
, rollAndPitchTrims_t
*angleTrim
, rxConfig_t
*rxConfig
);
69 typedef void (*pidControllerFuncPtr
)(pidProfile_t
*pidProfile
, controlRateConfig_t
*controlRateConfig
,
70 uint16_t max_angle_inclination
, rollAndPitchTrims_t
*angleTrim
, rxConfig_t
*rxConfig
); // pid controller function prototype
72 pidControllerFuncPtr pid_controller
= pidRewrite
; // which pid controller are we using, defaultMultiWii
74 void pidResetErrorGyro(void)
77 errorGyroI
[PITCH
] = 0;
80 errorGyroIf
[ROLL
] = 0.0f
;
81 errorGyroIf
[PITCH
] = 0.0f
;
82 errorGyroIf
[YAW
] = 0.0f
;
85 const angle_index_t rcAliasToAngleIndexMap
[] = { AI_ROLL
, AI_PITCH
};
87 static filterStatePt1_t DTermState
[3];
88 static filterStatePt1_t yawPTermState
;
90 static void pidLuxFloat(pidProfile_t
*pidProfile
, controlRateConfig_t
*controlRateConfig
,
91 uint16_t max_angle_inclination
, rollAndPitchTrims_t
*angleTrim
, rxConfig_t
*rxConfig
)
93 float RateError
, errorAngle
, AngleRate
, gyroRate
;
94 float ITerm
,PTerm
,DTerm
;
95 int32_t stickPosAil
, stickPosEle
, mostDeflectedPos
;
96 static float lastError
[3];
97 static float delta1
[3], delta2
[3];
98 float delta
, deltaSum
;
100 float horizonLevelStrength
= 1;
101 static float previousErrorGyroIf
[3] = { 0.0f
, 0.0f
, 0.0f
};
103 if (FLIGHT_MODE(HORIZON_MODE
)) {
105 // Figure out the raw stick positions
106 stickPosAil
= getRcStickDeflection(FD_ROLL
, rxConfig
->midrc
);
107 stickPosEle
= getRcStickDeflection(FD_PITCH
, rxConfig
->midrc
);
109 if(ABS(stickPosAil
) > ABS(stickPosEle
)){
110 mostDeflectedPos
= ABS(stickPosAil
);
113 mostDeflectedPos
= ABS(stickPosEle
);
116 // Progressively turn off the horizon self level strength as the stick is banged over
117 horizonLevelStrength
= (float)(500 - mostDeflectedPos
) / 500; // 1 at centre stick, 0 = max stick deflection
118 if(pidProfile
->H_sensitivity
== 0){
119 horizonLevelStrength
= 0;
121 horizonLevelStrength
= constrainf(((horizonLevelStrength
- 1) * (100 / pidProfile
->H_sensitivity
)) + 1, 0, 1);
125 // ----------PID controller----------
126 for (axis
= 0; axis
< 3; axis
++) {
127 // -----Get the desired angle rate depending on flight mode
128 uint8_t rate
= controlRateConfig
->rates
[axis
];
130 if (axis
== FD_YAW
) {
131 // YAW is always gyro-controlled (MAG correction is applied to rcCommand) 100dps to 1100dps max yaw rate
132 AngleRate
= (float)((rate
+ 10) * rcCommand
[YAW
]) / 50.0f
;
134 // calculate error and limit the angle to the max inclination
136 errorAngle
= (constrainf(((float)rcCommand
[axis
] * ((float)max_angle_inclination
/ 500.0f
)) + GPS_angle
[axis
], -((int) max_angle_inclination
),
137 +max_angle_inclination
) - attitude
.raw
[axis
] + angleTrim
->raw
[axis
]) / 10.0f
;
139 errorAngle
= (constrainf((float)rcCommand
[axis
] * ((float)max_angle_inclination
/ 500.0f
), -((int) max_angle_inclination
),
140 +max_angle_inclination
) - attitude
.raw
[axis
] + angleTrim
->raw
[axis
]) / 10.0f
;
143 if (FLIGHT_MODE(ANGLE_MODE
)) {
144 // it's the ANGLE mode - control is angle based, so control loop is needed
145 AngleRate
= errorAngle
* pidProfile
->A_level
;
147 //control is GYRO based (ACRO and HORIZON - direct sticks control is applied to rate PID
148 AngleRate
= (float)((rate
+ 20) * rcCommand
[axis
]) / 50.0f
; // 200dps to 1200dps max roll/pitch rate
149 if (FLIGHT_MODE(HORIZON_MODE
)) {
150 // mix up angle error to desired AngleRate to add a little auto-level feel
151 AngleRate
+= errorAngle
* pidProfile
->H_level
* horizonLevelStrength
;
156 gyroRate
= gyroADC
[axis
] * gyro
.scale
; // gyro output scaled to dps
158 // --------low-level gyro-based PID. ----------
159 // Used in stand-alone mode for ACRO, controlled by higher level regulators in other modes
160 // -----calculate scaled error.AngleRates
161 // multiplication of rcCommand corresponds to changing the sticks scaling here
162 RateError
= AngleRate
- gyroRate
;
164 // -----calculate P component
165 PTerm
= RateError
* pidProfile
->P_f
[axis
] * PIDweight
[axis
] / 100;
167 if (axis
== YAW
&& pidProfile
->yaw_pterm_cut_hz
) {
168 PTerm
= filterApplyPt1(PTerm
, &yawPTermState
, pidProfile
->yaw_pterm_cut_hz
, dT
);
171 // -----calculate I component.
172 errorGyroIf
[axis
] = constrainf(errorGyroIf
[axis
] + 0.5f
* (lastError
[axis
] + RateError
) * dT
* pidProfile
->I_f
[axis
] * 10, -250.0f
, 250.0f
);
174 if (allowITermShrinkOnly
|| motorLimitReached
) {
175 if (ABS(errorGyroIf
[axis
]) < ABS(previousErrorGyroIf
[axis
])) {
176 previousErrorGyroIf
[axis
] = errorGyroIf
[axis
];
178 errorGyroIf
[axis
] = constrain(errorGyroIf
[axis
], -ABS(previousErrorGyroIf
[axis
]), ABS(previousErrorGyroIf
[axis
]));
181 previousErrorGyroIf
[axis
] = errorGyroIf
[axis
];
184 // limit maximum integrator value to prevent WindUp - accumulating extreme values when system is saturated.
185 // I coefficient (I8) moved before integration to make limiting independent from PID settings
186 ITerm
= errorGyroIf
[axis
];
188 //-----calculate D-term
189 delta
= RateError
- lastError
[axis
];
190 lastError
[axis
] = RateError
;
192 // Correct difference by cycle time. Cycle time is jittery (can be different 2 times), so calculated difference
193 // would be scaled by different dt each time. Division by dT fixes that.
194 delta
*= (1.0f
/ dT
);
196 if (!pidProfile
->dterm_cut_hz
) {
197 // add moving average here to reduce noise
198 deltaSum
= (delta1
[axis
] + delta2
[axis
] + delta
) / 3;
199 delta2
[axis
] = delta1
[axis
];
200 delta1
[axis
] = delta
;
206 if (pidProfile
->dterm_cut_hz
) {
207 deltaSum
= filterApplyPt1(delta
, &DTermState
[axis
], pidProfile
->dterm_cut_hz
, dT
);
210 DTerm
= constrainf(deltaSum
* pidProfile
->D_f
[axis
] * PIDweight
[axis
] / 100, -300.0f
, 300.0f
);
212 // -----calculate total PID output
213 axisPID
[axis
] = constrain(lrintf(PTerm
+ ITerm
+ DTerm
), -1000, 1000);
216 if (FLIGHT_MODE(GTUNE_MODE
) && ARMING_FLAG(ARMED
)) {
217 calculate_Gtune(axis
);
222 axisPID_P
[axis
] = PTerm
;
223 axisPID_I
[axis
] = ITerm
;
224 axisPID_D
[axis
] = DTerm
;
229 static void pidRewrite(pidProfile_t
*pidProfile
, controlRateConfig_t
*controlRateConfig
, uint16_t max_angle_inclination
,
230 rollAndPitchTrims_t
*angleTrim
, rxConfig_t
*rxConfig
)
236 int32_t delta
, deltaSum
;
237 static int32_t delta1
[3], delta2
[3];
238 int32_t PTerm
, ITerm
, DTerm
;
239 static int32_t lastError
[3] = { 0, 0, 0 };
240 static int32_t previousErrorGyroI
[3] = { 0, 0, 0 };
241 int32_t AngleRateTmp
, RateError
;
243 int8_t horizonLevelStrength
= 100;
244 int32_t stickPosAil
, stickPosEle
, mostDeflectedPos
;
246 if (FLIGHT_MODE(HORIZON_MODE
)) {
248 // Figure out the raw stick positions
249 stickPosAil
= getRcStickDeflection(FD_ROLL
, rxConfig
->midrc
);
250 stickPosEle
= getRcStickDeflection(FD_PITCH
, rxConfig
->midrc
);
252 if(ABS(stickPosAil
) > ABS(stickPosEle
)){
253 mostDeflectedPos
= ABS(stickPosAil
);
256 mostDeflectedPos
= ABS(stickPosEle
);
259 // Progressively turn off the horizon self level strength as the stick is banged over
260 horizonLevelStrength
= (500 - mostDeflectedPos
) / 5; // 100 at centre stick, 0 = max stick deflection
262 // Using Level D as a Sensitivity for Horizon. 0 more level to 255 more rate. Default value of 100 seems to work fine.
263 // For more rate mode increase D and slower flips and rolls will be possible
264 horizonLevelStrength
= constrain((10 * (horizonLevelStrength
- 100) * (10 * pidProfile
->D8
[PIDLEVEL
] / 80) / 100) + 100, 0, 100);
267 // ----------PID controller----------
268 for (axis
= 0; axis
< 3; axis
++) {
269 uint8_t rate
= controlRateConfig
->rates
[axis
];
271 // -----Get the desired angle rate depending on flight mode
272 if (axis
== FD_YAW
) { // YAW is always gyro-controlled (MAG correction is applied to rcCommand)
273 AngleRateTmp
= (((int32_t)(rate
+ 27) * rcCommand
[YAW
]) >> 5);
275 // calculate error and limit the angle to max configured inclination
277 errorAngle
= constrain(2 * rcCommand
[axis
] + GPS_angle
[axis
], -((int) max_angle_inclination
),
278 +max_angle_inclination
) - attitude
.raw
[axis
] + angleTrim
->raw
[axis
]; // 16 bits is ok here
280 errorAngle
= constrain(2 * rcCommand
[axis
], -((int) max_angle_inclination
),
281 +max_angle_inclination
) - attitude
.raw
[axis
] + angleTrim
->raw
[axis
]; // 16 bits is ok here
284 if (!FLIGHT_MODE(ANGLE_MODE
)) { //control is GYRO based (ACRO and HORIZON - direct sticks control is applied to rate PID
285 AngleRateTmp
= ((int32_t)(rate
+ 27) * rcCommand
[axis
]) >> 4;
286 if (FLIGHT_MODE(HORIZON_MODE
)) {
287 // mix up angle error to desired AngleRateTmp to add a little auto-level feel. horizonLevelStrength is scaled to the stick input
288 AngleRateTmp
+= (errorAngle
* pidProfile
->I8
[PIDLEVEL
] * horizonLevelStrength
/ 100) >> 4;
290 } else { // it's the ANGLE mode - control is angle based, so control loop is needed
291 AngleRateTmp
= (errorAngle
* pidProfile
->P8
[PIDLEVEL
]) >> 4;
295 // --------low-level gyro-based PID. ----------
296 // Used in stand-alone mode for ACRO, controlled by higher level regulators in other modes
297 // -----calculate scaled error.AngleRates
298 // multiplication of rcCommand corresponds to changing the sticks scaling here
299 RateError
= AngleRateTmp
- (gyroADC
[axis
] / 4);
301 // -----calculate P component
302 PTerm
= (RateError
* pidProfile
->P8
[axis
] * PIDweight
[axis
] / 100) >> 7;
304 if (axis
== YAW
&& pidProfile
->yaw_pterm_cut_hz
) {
305 PTerm
= filterApplyPt1(PTerm
, &yawPTermState
, pidProfile
->yaw_pterm_cut_hz
, dT
);
308 // -----calculate I component
309 // there should be no division before accumulating the error to integrator, because the precision would be reduced.
310 // Precision is critical, as I prevents from long-time drift. Thus, 32 bits integrator is used.
311 // Time correction (to avoid different I scaling for different builds based on average cycle time)
312 // is normalized to cycle time = 2048.
313 errorGyroI
[axis
] = errorGyroI
[axis
] + ((((lastError
[axis
] + RateError
) / 2) * (uint16_t)targetLooptime
) >> 11) * pidProfile
->I8
[axis
];
315 // limit maximum integrator value to prevent WindUp - accumulating extreme values when system is saturated.
316 // I coefficient (I8) moved before integration to make limiting independent from PID settings
317 errorGyroI
[axis
] = constrain(errorGyroI
[axis
], (int32_t) - GYRO_I_MAX
<< 13, (int32_t) + GYRO_I_MAX
<< 13);
319 ITerm
= errorGyroI
[axis
] >> 13;
321 if (allowITermShrinkOnly
|| motorLimitReached
) {
322 if (ABS(errorGyroI
[axis
]) < ABS(previousErrorGyroI
[axis
])) {
323 previousErrorGyroI
[axis
] = errorGyroI
[axis
];
325 errorGyroI
[axis
] = constrain(errorGyroI
[axis
], -ABS(previousErrorGyroI
[axis
]), ABS(previousErrorGyroI
[axis
]));
328 previousErrorGyroI
[axis
] = errorGyroI
[axis
];
331 //-----calculate D-term
332 delta
= RateError
- lastError
[axis
]; // 16 bits is ok here, the dif between 2 consecutive gyro reads is limited to 800
333 lastError
[axis
] = RateError
;
335 // Correct difference by cycle time. Cycle time is jittery (can be different 2 times), so calculated difference
336 // would be scaled by different dt each time. Division by dT fixes that.
337 delta
= (delta
* ((uint16_t) 0xFFFF / ((uint16_t)targetLooptime
>> 4))) >> 6;
339 if (!pidProfile
->dterm_cut_hz
) {
340 // add moving average here to reduce noise
341 deltaSum
= delta1
[axis
] + delta2
[axis
] + delta
;
342 delta2
[axis
] = delta1
[axis
];
343 delta1
[axis
] = delta
;
345 deltaSum
= delta
* 2;
348 // Dterm delta low pass
349 if (pidProfile
->dterm_cut_hz
) {
350 deltaSum
= filterApplyPt1(deltaSum
, &DTermState
[axis
], pidProfile
->dterm_cut_hz
, dT
);
353 DTerm
= (deltaSum
* pidProfile
->D8
[axis
] * PIDweight
[axis
] / 100) >> 8;
355 // -----calculate total PID output
356 axisPID
[axis
] = PTerm
+ ITerm
+ DTerm
;
359 if (FLIGHT_MODE(GTUNE_MODE
) && ARMING_FLAG(ARMED
)) {
360 calculate_Gtune(axis
);
365 axisPID_P
[axis
] = PTerm
;
366 axisPID_I
[axis
] = ITerm
;
367 axisPID_D
[axis
] = DTerm
;
372 void pidSetController(pidControllerType_e type
)
376 case PID_CONTROLLER_MWREWRITE
:
377 pid_controller
= pidRewrite
;
379 case PID_CONTROLLER_LUX_FLOAT
:
380 pid_controller
= pidLuxFloat
;