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"
27 #include "common/axis.h"
28 #include "common/maths.h"
29 #include "common/filter.h"
31 #include "drivers/sensor.h"
32 #include "drivers/gyro_sync.h"
34 #include "drivers/accgyro.h"
35 #include "sensors/sensors.h"
36 #include "sensors/gyro.h"
37 #include "sensors/acceleration.h"
41 #include "io/rc_controls.h"
44 #include "flight/pid.h"
45 #include "flight/imu.h"
46 #include "flight/navigation.h"
47 #include "flight/gtune.h"
49 #include "config/runtime_config.h"
52 extern bool motorLimitReached
;
53 extern bool allowITermShrinkOnly
;
58 int32_t axisPID_P
[3], axisPID_I
[3], axisPID_D
[3];
61 // PIDweight is a scale factor for PIDs which is derived from the throttle and TPA setting, and 100 = 100% scale means no PID reduction
64 static int32_t errorGyroI
[3] = { 0, 0, 0 };
65 static float errorGyroIf
[3] = { 0.0f
, 0.0f
, 0.0f
};
67 static void pidRewrite(pidProfile_t
*pidProfile
, controlRateConfig_t
*controlRateConfig
,
68 uint16_t max_angle_inclination
, rollAndPitchTrims_t
*angleTrim
, rxConfig_t
*rxConfig
);
70 typedef void (*pidControllerFuncPtr
)(pidProfile_t
*pidProfile
, controlRateConfig_t
*controlRateConfig
,
71 uint16_t max_angle_inclination
, rollAndPitchTrims_t
*angleTrim
, rxConfig_t
*rxConfig
); // pid controller function prototype
73 pidControllerFuncPtr pid_controller
= pidRewrite
; // which pid controller are we using, defaultMultiWii
75 void pidResetErrorGyro(void)
78 errorGyroI
[PITCH
] = 0;
81 errorGyroIf
[ROLL
] = 0.0f
;
82 errorGyroIf
[PITCH
] = 0.0f
;
83 errorGyroIf
[YAW
] = 0.0f
;
86 void airModePlus(airModePlus_t
*axisState
, int axis
, pidProfile_t
*pidProfile
) {
87 float rcCommandReflection
= (float)rcCommand
[axis
] / 500.0f
;
88 axisState
->wowFactor
= 1;
89 axisState
->factor
= 0;
91 if (ABS(rcCommandReflection
) > 0.7f
&& (!flightModeFlags
)) { /* scaling should not happen in level modes */
93 axisState
->iTermScaler
= 0.0f
;
95 /* Prevent rapid windup during acro recoveries */
96 if (axisState
->iTermScaler
< 1) {
97 axisState
->iTermScaler
= constrainf(axisState
->iTermScaler
+ 0.001f
, 0.0f
, 1.0f
);
99 axisState
->iTermScaler
= 1;
103 /* acro plus factor handling */
104 if (axis
!= YAW
&& pidProfile
->airModeInsaneAcrobilityFactor
&& (!flightModeFlags
)) {
105 axisState
->wowFactor
= ABS(rcCommandReflection
) * ((float)pidProfile
->airModeInsaneAcrobilityFactor
/ 100.0f
); //0-1f
106 axisState
->factor
= axisState
->wowFactor
* rcCommandReflection
* 1000;
107 axisState
->wowFactor
= 1.0f
- axisState
->wowFactor
;
111 const angle_index_t rcAliasToAngleIndexMap
[] = { AI_ROLL
, AI_PITCH
};
113 static airModePlus_t airModePlusAxisState
[3];
114 static biquad_t deltaBiQuadState
[3];
115 static bool deltaStateIsSet
;
117 static void pidLuxFloat(pidProfile_t
*pidProfile
, controlRateConfig_t
*controlRateConfig
,
118 uint16_t max_angle_inclination
, rollAndPitchTrims_t
*angleTrim
, rxConfig_t
*rxConfig
)
120 float RateError
, AngleRate
, gyroRate
;
121 float ITerm
,PTerm
,DTerm
;
122 static float lastError
[3];
125 float horizonLevelStrength
= 1;
126 static float previousErrorGyroIf
[3] = { 0.0f
, 0.0f
, 0.0f
};
128 if (!deltaStateIsSet
&& pidProfile
->dterm_lpf_hz
) {
129 for (axis
= 0; axis
< 3; axis
++) BiQuadNewLpf(pidProfile
->dterm_lpf_hz
, &deltaBiQuadState
[axis
], 0);
130 deltaStateIsSet
= true;
133 if (FLIGHT_MODE(HORIZON_MODE
)) {
134 // Figure out the raw stick positions
135 const int32_t stickPosAil
= ABS(getRcStickDeflection(FD_ROLL
, rxConfig
->midrc
));
136 const int32_t stickPosEle
= ABS(getRcStickDeflection(FD_PITCH
, rxConfig
->midrc
));
137 const int32_t mostDeflectedPos
= MAX(stickPosAil
, stickPosEle
);
138 // Progressively turn off the horizon self level strength as the stick is banged over
139 horizonLevelStrength
= (float)(500 - mostDeflectedPos
) / 500; // 1 at centre stick, 0 = max stick deflection
140 if(pidProfile
->H_sensitivity
== 0){
141 horizonLevelStrength
= 0;
143 horizonLevelStrength
= constrainf(((horizonLevelStrength
- 1) * (100 / pidProfile
->H_sensitivity
)) + 1, 0, 1);
147 // ----------PID controller----------
148 for (axis
= 0; axis
< 3; axis
++) {
150 // -----Get the desired angle rate depending on flight mode
151 if (axis
== YAW
|| !pidProfile
->airModeInsaneAcrobilityFactor
|| !IS_RC_MODE_ACTIVE(BOXAIRMODE
)) {
152 rate
= controlRateConfig
->rates
[axis
];
155 if (axis
== FD_YAW
) {
156 // YAW is always gyro-controlled (MAG correction is applied to rcCommand) 100dps to 1100dps max yaw rate
157 AngleRate
= (float)((rate
+ 10) * rcCommand
[YAW
]) / 50.0f
;
159 // ACRO mode, control is GYRO based, direct sticks control is applied to rate PID
160 AngleRate
= (float)((rate
+ 20) * rcCommand
[axis
]) / 50.0f
; // 200dps to 1200dps max roll/pitch rate
161 if (FLIGHT_MODE(ANGLE_MODE
) || FLIGHT_MODE(HORIZON_MODE
)) {
162 // calculate error angle and limit the angle to the max inclination
164 const float errorAngle
= (constrain(rcCommand
[axis
] + GPS_angle
[axis
], -((int) max_angle_inclination
),
165 +max_angle_inclination
) - attitude
.raw
[axis
] + angleTrim
->raw
[axis
]) / 10.0f
; // 16 bits is ok here
167 const float errorAngle
= (constrain(rcCommand
[axis
], -((int) max_angle_inclination
),
168 +max_angle_inclination
) - attitude
.raw
[axis
] + angleTrim
->raw
[axis
]) / 10.0f
; // 16 bits is ok here
170 if (FLIGHT_MODE(ANGLE_MODE
)) {
171 // ANGLE mode - control is angle based, so control loop is needed
172 AngleRate
= errorAngle
* pidProfile
->A_level
;
174 // HORIZON mode - direct sticks control is applied to rate PID
175 // mix up angle error to desired AngleRate to add a little auto-level feel
176 AngleRate
+= errorAngle
* pidProfile
->H_level
* horizonLevelStrength
;
181 gyroRate
= gyroADC
[axis
] * gyro
.scale
; // gyro output scaled to dps
183 // --------low-level gyro-based PID. ----------
184 // Used in stand-alone mode for ACRO, controlled by higher level regulators in other modes
185 // -----calculate scaled error.AngleRates
186 // multiplication of rcCommand corresponds to changing the sticks scaling here
187 RateError
= AngleRate
- gyroRate
;
189 // -----calculate P component
190 PTerm
= RateError
* pidProfile
->P_f
[axis
] * PIDweight
[axis
] / 100;
192 // -----calculate I component.
193 errorGyroIf
[axis
] = constrainf(errorGyroIf
[axis
] + RateError
* dT
* pidProfile
->I_f
[axis
] * 10, -250.0f
, 250.0f
);
195 if (IS_RC_MODE_ACTIVE(BOXAIRMODE
)) {
196 airModePlus(&airModePlusAxisState
[axis
], axis
, pidProfile
);
197 errorGyroIf
[axis
] *= airModePlusAxisState
[axis
].iTermScaler
;
200 if (allowITermShrinkOnly
|| motorLimitReached
) {
201 if (ABS(errorGyroIf
[axis
]) < ABS(previousErrorGyroIf
[axis
])) {
202 previousErrorGyroIf
[axis
] = errorGyroIf
[axis
];
204 errorGyroIf
[axis
] = constrain(errorGyroIf
[axis
], -ABS(previousErrorGyroIf
[axis
]), ABS(previousErrorGyroIf
[axis
]));
207 previousErrorGyroIf
[axis
] = errorGyroIf
[axis
];
210 // limit maximum integrator value to prevent WindUp - accumulating extreme values when system is saturated.
211 // I coefficient (I8) moved before integration to make limiting independent from PID settings
212 ITerm
= errorGyroIf
[axis
];
214 //-----calculate D-term
215 delta
= RateError
- lastError
[axis
];
216 lastError
[axis
] = RateError
;
218 if (deltaStateIsSet
) {
219 delta
= applyBiQuadFilter(delta
, &deltaBiQuadState
[axis
]);
222 // Correct difference by cycle time. Cycle time is jittery (can be different 2 times), so calculated difference
223 // would be scaled by different dt each time. Division by dT fixes that.
224 delta
*= (1.0f
/ dT
);
226 DTerm
= constrainf(delta
* pidProfile
->D_f
[axis
] * PIDweight
[axis
] / 100, -300.0f
, 300.0f
);
228 // -----calculate total PID output
229 axisPID
[axis
] = constrain(lrintf(PTerm
+ ITerm
+ DTerm
), -1000, 1000);
231 if (IS_RC_MODE_ACTIVE(BOXAIRMODE
)) {
232 axisPID
[axis
] = lrintf(airModePlusAxisState
[axis
].factor
+ airModePlusAxisState
[axis
].wowFactor
* axisPID
[axis
]);
236 if (FLIGHT_MODE(GTUNE_MODE
) && ARMING_FLAG(ARMED
)) {
237 calculate_Gtune(axis
);
242 axisPID_P
[axis
] = PTerm
;
243 axisPID_I
[axis
] = ITerm
;
244 axisPID_D
[axis
] = DTerm
;
249 static void pidRewrite(pidProfile_t
*pidProfile
, controlRateConfig_t
*controlRateConfig
, uint16_t max_angle_inclination
,
250 rollAndPitchTrims_t
*angleTrim
, rxConfig_t
*rxConfig
)
255 int32_t PTerm
, ITerm
, DTerm
, delta
;
256 static int32_t lastError
[3] = { 0, 0, 0 };
257 static int32_t previousErrorGyroI
[3] = { 0, 0, 0 };
258 int32_t AngleRateTmp
, RateError
, gyroRate
;
260 int8_t horizonLevelStrength
= 100;
262 if (!deltaStateIsSet
&& pidProfile
->dterm_lpf_hz
) {
263 for (axis
= 0; axis
< 3; axis
++) BiQuadNewLpf(pidProfile
->dterm_lpf_hz
, &deltaBiQuadState
[axis
], 0);
264 deltaStateIsSet
= true;
267 if (FLIGHT_MODE(HORIZON_MODE
)) {
268 // Figure out the raw stick positions
269 const int32_t stickPosAil
= ABS(getRcStickDeflection(FD_ROLL
, rxConfig
->midrc
));
270 const int32_t stickPosEle
= ABS(getRcStickDeflection(FD_PITCH
, rxConfig
->midrc
));
271 const int32_t mostDeflectedPos
= MAX(stickPosAil
, stickPosEle
);
272 // Progressively turn off the horizon self level strength as the stick is banged over
273 horizonLevelStrength
= (500 - mostDeflectedPos
) / 5; // 100 at centre stick, 0 = max stick deflection
274 // Using Level D as a Sensitivity for Horizon. 0 more level to 255 more rate. Default value of 100 seems to work fine.
275 // For more rate mode increase D and slower flips and rolls will be possible
276 horizonLevelStrength
= constrain((10 * (horizonLevelStrength
- 100) * (10 * pidProfile
->D8
[PIDLEVEL
] / 80) / 100) + 100, 0, 100);
279 // ----------PID controller----------
280 for (axis
= 0; axis
< 3; axis
++) {
282 // -----Get the desired angle rate depending on flight mode
283 if (axis
== YAW
|| !pidProfile
->airModeInsaneAcrobilityFactor
|| !IS_RC_MODE_ACTIVE(BOXAIRMODE
)) {
284 rate
= controlRateConfig
->rates
[axis
];
287 // -----Get the desired angle rate depending on flight mode
288 if (axis
== FD_YAW
) {
289 // YAW is always gyro-controlled (MAG correction is applied to rcCommand)
290 AngleRateTmp
= ((int32_t)(rate
+ 27) * rcCommand
[YAW
]) >> 5;
292 AngleRateTmp
= ((int32_t)(rate
+ 27) * rcCommand
[axis
]) >> 4;
293 if (FLIGHT_MODE(ANGLE_MODE
) || FLIGHT_MODE(HORIZON_MODE
)) {
294 // calculate error angle and limit the angle to max configured inclination
296 const int32_t errorAngle
= constrain(2 * rcCommand
[axis
] + GPS_angle
[axis
], -((int) max_angle_inclination
),
297 +max_angle_inclination
) - attitude
.raw
[axis
] + angleTrim
->raw
[axis
];
299 const int32_t errorAngle
= constrain(2 * rcCommand
[axis
], -((int) max_angle_inclination
),
300 +max_angle_inclination
) - attitude
.raw
[axis
] + angleTrim
->raw
[axis
];
302 if (FLIGHT_MODE(ANGLE_MODE
)) {
303 // ANGLE mode - control is angle based, so control loop is needed
304 AngleRateTmp
= (errorAngle
* pidProfile
->P8
[PIDLEVEL
]) >> 4;
306 // HORIZON mode - mix up angle error to desired AngleRateTmp to add a little auto-level feel,
307 // horizonLevelStrength is scaled to the stick input
308 AngleRateTmp
+= (errorAngle
* pidProfile
->I8
[PIDLEVEL
] * horizonLevelStrength
/ 100) >> 4;
313 // --------low-level gyro-based PID. ----------
314 // Used in stand-alone mode for ACRO, controlled by higher level regulators in other modes
315 // -----calculate scaled error.AngleRates
316 // multiplication of rcCommand corresponds to changing the sticks scaling here
317 gyroRate
= gyroADC
[axis
] / 4;
318 RateError
= AngleRateTmp
- gyroRate
;
320 // -----calculate P component
321 PTerm
= (RateError
* pidProfile
->P8
[axis
] * PIDweight
[axis
] / 100) >> 7;
323 // -----calculate I component
324 // there should be no division before accumulating the error to integrator, because the precision would be reduced.
325 // Precision is critical, as I prevents from long-time drift. Thus, 32 bits integrator is used.
326 // Time correction (to avoid different I scaling for different builds based on average cycle time)
327 // is normalized to cycle time = 2048.
328 errorGyroI
[axis
] = errorGyroI
[axis
] + ((RateError
* (uint16_t)targetLooptime
) >> 11) * pidProfile
->I8
[axis
];
330 // limit maximum integrator value to prevent WindUp - accumulating extreme values when system is saturated.
331 // I coefficient (I8) moved before integration to make limiting independent from PID settings
332 errorGyroI
[axis
] = constrain(errorGyroI
[axis
], (int32_t) - GYRO_I_MAX
<< 13, (int32_t) + GYRO_I_MAX
<< 13);
334 ITerm
= errorGyroI
[axis
] >> 13;
336 if (IS_RC_MODE_ACTIVE(BOXAIRMODE
)) {
337 airModePlus(&airModePlusAxisState
[axis
], axis
, pidProfile
);
338 errorGyroI
[axis
] *= airModePlusAxisState
[axis
].iTermScaler
;
341 if (allowITermShrinkOnly
|| motorLimitReached
) {
342 if (ABS(errorGyroI
[axis
]) < ABS(previousErrorGyroI
[axis
])) {
343 previousErrorGyroI
[axis
] = errorGyroI
[axis
];
345 errorGyroI
[axis
] = constrain(errorGyroI
[axis
], -ABS(previousErrorGyroI
[axis
]), ABS(previousErrorGyroI
[axis
]));
348 previousErrorGyroI
[axis
] = errorGyroI
[axis
];
351 //-----calculate D-term
352 delta
= RateError
- lastError
[axis
]; // 16 bits is ok here, the dif between 2 consecutive gyro reads is limited to 800
353 lastError
[axis
] = RateError
;
355 if (deltaStateIsSet
) {
356 delta
= lrintf(applyBiQuadFilter((float) delta
, &deltaBiQuadState
[axis
]));
359 // Correct difference by cycle time. Cycle time is jittery (can be different 2 times), so calculated difference
360 // would be scaled by different dt each time. Division by dT fixes that.
361 delta
= (delta
* ((uint16_t) 0xFFFF / ((uint16_t)targetLooptime
>> 4))) >> 6;
363 DTerm
= (delta
* 3 * pidProfile
->D8
[axis
] * PIDweight
[axis
] / 100) >> 8;
365 // -----calculate total PID output
366 axisPID
[axis
] = PTerm
+ ITerm
+ DTerm
;
368 if (IS_RC_MODE_ACTIVE(BOXAIRMODE
)) {
369 axisPID
[axis
] = lrintf(airModePlusAxisState
[axis
].factor
+ airModePlusAxisState
[axis
].wowFactor
* axisPID
[axis
]);
373 if (FLIGHT_MODE(GTUNE_MODE
) && ARMING_FLAG(ARMED
)) {
374 calculate_Gtune(axis
);
379 axisPID_P
[axis
] = PTerm
;
380 axisPID_I
[axis
] = ITerm
;
381 axisPID_D
[axis
] = DTerm
;
386 void pidSetController(pidControllerType_e type
)
390 case PID_CONTROLLER_MWREWRITE
:
391 pid_controller
= pidRewrite
;
393 case PID_CONTROLLER_LUX_FLOAT
:
394 pid_controller
= pidLuxFloat
;