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"
49 #include "config/runtime_config.h"
51 extern uint8_t motorCount
;
53 extern bool motorLimitReached
;
58 int32_t axisPID_P
[3], axisPID_I
[3], axisPID_D
[3];
61 #define DELTA_MAX_SAMPLES 12
63 // 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 uint8_t dynP8
[3], dynI8
[3], dynD8
[3], PIDweight
[3];
66 static int32_t errorGyroI
[3], errorGyroILimit
[3];
67 static float errorGyroIf
[3], errorGyroIfLimit
[3];
68 static int32_t errorAngleI
[2];
69 static float errorAngleIf
[2];
70 static bool lowThrottlePidReduction
;
72 static void pidMultiWiiRewrite(pidProfile_t
*pidProfile
, controlRateConfig_t
*controlRateConfig
,
73 uint16_t max_angle_inclination
, rollAndPitchTrims_t
*angleTrim
, rxConfig_t
*rxConfig
);
75 typedef void (*pidControllerFuncPtr
)(pidProfile_t
*pidProfile
, controlRateConfig_t
*controlRateConfig
,
76 uint16_t max_angle_inclination
, rollAndPitchTrims_t
*angleTrim
, rxConfig_t
*rxConfig
); // pid controller function prototype
78 pidControllerFuncPtr pid_controller
= pidMultiWiiRewrite
; // which pid controller are we using, defaultMultiWii
80 void pidResetErrorAngle(void)
82 errorAngleI
[ROLL
] = 0;
83 errorAngleI
[PITCH
] = 0;
85 errorAngleIf
[ROLL
] = 0.0f
;
86 errorAngleIf
[PITCH
] = 0.0f
;
89 void pidResetErrorGyroState(uint8_t resetOption
)
91 if (resetOption
>= RESET_ITERM
) {
93 for (axis
= 0; axis
< 3; axis
++) {
95 errorGyroIf
[axis
] = 0.0f
;
99 if (resetOption
== RESET_ITERM_AND_REDUCE_PID
) {
100 lowThrottlePidReduction
= true;
102 lowThrottlePidReduction
= false;
106 void scaleItermToRcInput(int axis
, pidProfile_t
*pidProfile
) {
107 float rcCommandReflection
= (float)rcCommand
[axis
] / 500.0f
;
108 static float iTermScaler
[3] = {1.0f
, 1.0f
, 1.0f
};
109 static float antiWindUpIncrement
= 0;
111 if (!antiWindUpIncrement
) antiWindUpIncrement
= (0.001 / 500) * targetLooptime
; // Calculate increment for 500ms period
113 if (ABS(rcCommandReflection
) > 0.7f
&& (!flightModeFlags
)) { /* scaling should not happen in level modes */
114 /* Reset Iterm on high stick inputs. No scaling necessary here */
115 iTermScaler
[axis
] = 0.0f
;
116 errorGyroI
[axis
] = 0;
117 errorGyroIf
[axis
] = 0.0f
;
119 /* Prevent rapid windup during acro recoveries. Slowly enable Iterm for period of 500ms */
120 if (iTermScaler
[axis
] < 1) {
121 iTermScaler
[axis
] = constrainf(iTermScaler
[axis
] + antiWindUpIncrement
, 0.0f
, 1.0f
);
122 if (pidProfile
->pidController
!= PID_CONTROLLER_LUX_FLOAT
) {
123 errorGyroI
[axis
] *= iTermScaler
[axis
];
125 errorGyroIf
[axis
] *= iTermScaler
[axis
];
131 const angle_index_t rcAliasToAngleIndexMap
[] = { AI_ROLL
, AI_PITCH
};
133 static biquad_t deltaBiQuadState
[3];
134 static bool deltaStateIsSet
;
136 static void pidLuxFloat(pidProfile_t
*pidProfile
, controlRateConfig_t
*controlRateConfig
,
137 uint16_t max_angle_inclination
, rollAndPitchTrims_t
*angleTrim
, rxConfig_t
*rxConfig
)
139 float RateError
, AngleRate
, gyroRate
;
140 float ITerm
,PTerm
,DTerm
;
141 static float lastErrorForDelta
[3];
142 static float previousDelta
[3][DELTA_MAX_SAMPLES
];
143 float delta
, deltaSum
;
144 int axis
, deltaCount
;
145 float horizonLevelStrength
= 1;
147 float dT
= (float)targetLooptime
* 0.000001f
;
149 if (!deltaStateIsSet
&& pidProfile
->dterm_lpf_hz
) {
150 for (axis
= 0; axis
< 3; axis
++) BiQuadNewLpf(pidProfile
->dterm_lpf_hz
, &deltaBiQuadState
[axis
], targetLooptime
);
151 deltaStateIsSet
= true;
154 if (FLIGHT_MODE(HORIZON_MODE
)) {
155 // Figure out the raw stick positions
156 const int32_t stickPosAil
= ABS(getRcStickDeflection(FD_ROLL
, rxConfig
->midrc
));
157 const int32_t stickPosEle
= ABS(getRcStickDeflection(FD_PITCH
, rxConfig
->midrc
));
158 const int32_t mostDeflectedPos
= MAX(stickPosAil
, stickPosEle
);
159 // Progressively turn off the horizon self level strength as the stick is banged over
160 horizonLevelStrength
= (float)(500 - mostDeflectedPos
) / 500; // 1 at centre stick, 0 = max stick deflection
161 if(pidProfile
->H_sensitivity
== 0){
162 horizonLevelStrength
= 0;
164 horizonLevelStrength
= constrainf(((horizonLevelStrength
- 1) * (100 / pidProfile
->H_sensitivity
)) + 1, 0, 1);
168 // ----------PID controller----------
169 for (axis
= 0; axis
< 3; axis
++) {
170 uint8_t rate
= controlRateConfig
->rates
[axis
];
172 if (axis
== FD_YAW
) {
173 // YAW is always gyro-controlled (MAG correction is applied to rcCommand) 100dps to 1100dps max yaw rate
174 AngleRate
= (float)((rate
+ 10) * rcCommand
[YAW
]) / 50.0f
;
176 // ACRO mode, control is GYRO based, direct sticks control is applied to rate PID
177 AngleRate
= (float)((rate
+ 20) * rcCommand
[axis
]) / 50.0f
; // 200dps to 1200dps max roll/pitch rate
178 if (FLIGHT_MODE(ANGLE_MODE
) || FLIGHT_MODE(HORIZON_MODE
)) {
179 // calculate error angle and limit the angle to the max inclination
181 const float errorAngle
= (constrain(rcCommand
[axis
] + GPS_angle
[axis
], -((int) max_angle_inclination
),
182 +max_angle_inclination
) - attitude
.raw
[axis
] + angleTrim
->raw
[axis
]) / 10.0f
; // 16 bits is ok here
184 const float errorAngle
= (constrain(rcCommand
[axis
], -((int) max_angle_inclination
),
185 +max_angle_inclination
) - attitude
.raw
[axis
] + angleTrim
->raw
[axis
]) / 10.0f
; // 16 bits is ok here
187 if (FLIGHT_MODE(ANGLE_MODE
)) {
188 // ANGLE mode - control is angle based, so control loop is needed
189 AngleRate
= errorAngle
* pidProfile
->A_level
;
191 // HORIZON mode - direct sticks control is applied to rate PID
192 // mix up angle error to desired AngleRate to add a little auto-level feel
193 AngleRate
+= errorAngle
* pidProfile
->H_level
* horizonLevelStrength
;
198 gyroRate
= gyroADC
[axis
] * gyro
.scale
; // gyro output scaled to dps
200 // --------low-level gyro-based PID. ----------
201 // Used in stand-alone mode for ACRO, controlled by higher level regulators in other modes
202 // -----calculate scaled error.AngleRates
203 // multiplication of rcCommand corresponds to changing the sticks scaling here
204 RateError
= AngleRate
- gyroRate
;
206 // -----calculate P component
207 PTerm
= RateError
* pidProfile
->P_f
[axis
] * PIDweight
[axis
] / 100;
209 // -----calculate I component.
210 errorGyroIf
[axis
] = constrainf(errorGyroIf
[axis
] + RateError
* dT
* pidProfile
->I_f
[axis
] * 10, -250.0f
, 250.0f
);
212 if (IS_RC_MODE_ACTIVE(BOXAIRMODE
) || IS_RC_MODE_ACTIVE(BOXACROPLUS
)) {
213 scaleItermToRcInput(axis
, pidProfile
);
214 if (antiWindupProtection
|| motorLimitReached
) {
215 errorGyroIf
[axis
] = constrainf(errorGyroIf
[axis
], -errorGyroIfLimit
[axis
], errorGyroIfLimit
[axis
]);
217 errorGyroIfLimit
[axis
] = ABS(errorGyroIf
[axis
]);
221 // limit maximum integrator value to prevent WindUp - accumulating extreme values when system is saturated.
222 // I coefficient (I8) moved before integration to make limiting independent from PID settings
223 ITerm
= errorGyroIf
[axis
];
225 //-----calculate D-term
226 if (pidProfile
->deltaMethod
== DELTA_FROM_ERROR
) {
227 delta
= RateError
- lastErrorForDelta
[axis
];
228 lastErrorForDelta
[axis
] = RateError
;
230 delta
= -(gyroRate
- lastErrorForDelta
[axis
]); // 16 bits is ok here, the dif between 2 consecutive gyro reads is limited to 800
231 lastErrorForDelta
[axis
] = gyroRate
;
234 // Correct difference by cycle time. Cycle time is jittery (can be different 2 times), so calculated difference
235 // would be scaled by different dt each time. Division by dT fixes that.
236 delta
*= (1.0f
/ dT
);
238 if (deltaStateIsSet
) {
239 delta
= applyBiQuadFilter(delta
, &deltaBiQuadState
[axis
]);
241 // Apply moving average
243 for (deltaCount
= pidProfile
->dterm_average_count
-1; deltaCount
> 0; deltaCount
--) previousDelta
[axis
][deltaCount
] = previousDelta
[axis
][deltaCount
-1];
244 previousDelta
[axis
][0] = delta
;
245 for (deltaCount
= 0; deltaCount
< pidProfile
->dterm_average_count
; deltaCount
++) deltaSum
+= previousDelta
[axis
][deltaCount
];
246 delta
= (deltaSum
/ pidProfile
->dterm_average_count
);
249 DTerm
= constrainf(delta
* pidProfile
->D_f
[axis
] * PIDweight
[axis
] / 100, -300.0f
, 300.0f
);
251 // -----calculate total PID output
252 axisPID
[axis
] = constrain(lrintf(PTerm
+ ITerm
+ DTerm
), -1000, 1000);
254 if (lowThrottlePidReduction
) axisPID
[axis
] /= 4;
257 axisPID_P
[axis
] = PTerm
;
258 axisPID_I
[axis
] = ITerm
;
259 axisPID_D
[axis
] = DTerm
;
264 static void pidMultiWii23(pidProfile_t
*pidProfile
, controlRateConfig_t
*controlRateConfig
, uint16_t max_angle_inclination
,
265 rollAndPitchTrims_t
*angleTrim
, rxConfig_t
*rxConfig
)
269 int axis
, deltaCount
, prop
= 0;
270 int32_t rc
, error
, errorAngle
, delta
, gyroError
;
271 int32_t PTerm
, ITerm
, PTermACC
, ITermACC
, DTerm
;
272 static int16_t lastErrorForDelta
[2];
273 static int32_t previousDelta
[2][DELTA_MAX_SAMPLES
];
275 if (!deltaStateIsSet
&& pidProfile
->dterm_lpf_hz
) {
276 for (axis
= 0; axis
< 2; axis
++) BiQuadNewLpf(pidProfile
->dterm_lpf_hz
, &deltaBiQuadState
[axis
], targetLooptime
);
277 deltaStateIsSet
= true;
280 if (FLIGHT_MODE(HORIZON_MODE
)) {
281 prop
= MIN(MAX(ABS(rcCommand
[PITCH
]), ABS(rcCommand
[ROLL
])), 512);
285 for (axis
= 0; axis
< 2; axis
++) {
287 rc
= rcCommand
[axis
] << 1;
289 gyroError
= gyroADC
[axis
] / 4;
291 error
= rc
- gyroError
;
292 errorGyroI
[axis
] = constrain(errorGyroI
[axis
] + error
, -16000, +16000); // WindUp 16 bits is ok here
294 if (ABS(gyroADC
[axis
]) > (640 * 4)) {
295 errorGyroI
[axis
] = 0;
298 // Anti windup protection
299 if (IS_RC_MODE_ACTIVE(BOXAIRMODE
) || IS_RC_MODE_ACTIVE(BOXACROPLUS
)) {
300 scaleItermToRcInput(axis
, pidProfile
);
301 if (antiWindupProtection
|| motorLimitReached
) {
302 errorGyroI
[axis
] = constrain(errorGyroI
[axis
], -errorGyroILimit
[axis
], errorGyroILimit
[axis
]);
304 errorGyroILimit
[axis
] = ABS(errorGyroI
[axis
]);
308 ITerm
= (errorGyroI
[axis
] >> 7) * pidProfile
->I8
[axis
] >> 6; // 16 bits is ok here 16000/125 = 128 ; 128*250 = 32000
310 PTerm
= (int32_t)rc
* pidProfile
->P8
[axis
] >> 6;
312 if (FLIGHT_MODE(ANGLE_MODE
) || FLIGHT_MODE(HORIZON_MODE
)) { // axis relying on ACC
313 // 50 degrees max inclination
315 errorAngle
= constrain(2 * rcCommand
[axis
] + GPS_angle
[axis
], -((int) max_angle_inclination
),
316 +max_angle_inclination
) - attitude
.raw
[axis
] + angleTrim
->raw
[axis
];
318 errorAngle
= constrain(2 * rcCommand
[axis
], -((int) max_angle_inclination
),
319 +max_angle_inclination
) - attitude
.raw
[axis
] + angleTrim
->raw
[axis
];
322 errorAngleI
[axis
] = constrain(errorAngleI
[axis
] + errorAngle
, -10000, +10000); // WindUp //16 bits is ok here
324 PTermACC
= ((int32_t)errorAngle
* pidProfile
->P8
[PIDLEVEL
]) >> 7; // 32 bits is needed for calculation: errorAngle*P8 could exceed 32768 16 bits is ok for result
326 int16_t limit
= pidProfile
->D8
[PIDLEVEL
] * 5;
327 PTermACC
= constrain(PTermACC
, -limit
, +limit
);
329 ITermACC
= ((int32_t)errorAngleI
[axis
] * pidProfile
->I8
[PIDLEVEL
]) >> 12; // 32 bits is needed for calculation:10000*I8 could exceed 32768 16 bits is ok for result
331 ITerm
= ITermACC
+ ((ITerm
- ITermACC
) * prop
>> 9);
332 PTerm
= PTermACC
+ ((PTerm
- PTermACC
) * prop
>> 9);
335 PTerm
-= ((int32_t)gyroError
* dynP8
[axis
]) >> 6; // 32 bits is needed for calculation
337 //-----calculate D-term based on the configured approach (delta from measurement or deltafromError)
338 if (pidProfile
->deltaMethod
== DELTA_FROM_ERROR
) {
339 delta
= error
- lastErrorForDelta
[axis
];
340 lastErrorForDelta
[axis
] = error
;
341 } else { /* Delta from measurement */
342 delta
= -(gyroError
- lastErrorForDelta
[axis
]);
343 lastErrorForDelta
[axis
] = gyroError
;
346 if (deltaStateIsSet
) {
347 DTerm
= lrintf(applyBiQuadFilter((float) delta
, &deltaBiQuadState
[axis
])) * 3; // Keep same scaling as unfiltered delta
349 // Apply moving average
351 for (deltaCount
= pidProfile
->dterm_average_count
-1; deltaCount
> 0; deltaCount
--) previousDelta
[axis
][deltaCount
] = previousDelta
[axis
][deltaCount
-1];
352 previousDelta
[axis
][0] = delta
;
353 for (deltaCount
= 0; deltaCount
< pidProfile
->dterm_average_count
; deltaCount
++) DTerm
+= previousDelta
[axis
][deltaCount
];
354 delta
= (DTerm
/ pidProfile
->dterm_average_count
) * 3; // Keep same original scaling
357 DTerm
= ((int32_t)DTerm
* dynD8
[axis
]) >> 5; // 32 bits is needed for calculation
359 axisPID
[axis
] = PTerm
+ ITerm
+ DTerm
;
361 if (lowThrottlePidReduction
) axisPID
[axis
] /= 4;
365 axisPID_P
[axis
] = PTerm
;
366 axisPID_I
[axis
] = ITerm
;
367 axisPID_D
[axis
] = DTerm
;
372 rc
= (int32_t)rcCommand
[FD_YAW
] * (2 * controlRateConfig
->rates
[FD_YAW
] + 30) >> 5;
374 error
= rc
- gyroADC
[FD_YAW
];
376 error
= rc
- (gyroADC
[FD_YAW
] / 4);
378 errorGyroI
[FD_YAW
] += (int32_t)error
* pidProfile
->I8
[FD_YAW
];
379 errorGyroI
[FD_YAW
] = constrain(errorGyroI
[FD_YAW
], 2 - ((int32_t)1 << 28), -2 + ((int32_t)1 << 28));
380 if (ABS(rc
) > 50) errorGyroI
[FD_YAW
] = 0;
382 PTerm
= (int32_t)error
* pidProfile
->P8
[FD_YAW
] >> 6; // TODO: Bitwise shift on a signed integer is not recommended
384 // Constrain YAW by D value if not servo driven in that case servolimits apply
385 if(motorCount
>= 4 && pidProfile
->yaw_p_limit
< YAW_P_LIMIT_MAX
) {
386 PTerm
= constrain(PTerm
, -pidProfile
->yaw_p_limit
, pidProfile
->yaw_p_limit
);
389 ITerm
= constrain((int16_t)(errorGyroI
[FD_YAW
] >> 13), -GYRO_I_MAX
, +GYRO_I_MAX
);
391 axisPID
[FD_YAW
] = PTerm
+ ITerm
;
393 if (lowThrottlePidReduction
) axisPID
[FD_YAW
] /= 4;
396 axisPID_P
[FD_YAW
] = PTerm
;
397 axisPID_I
[FD_YAW
] = ITerm
;
398 axisPID_D
[FD_YAW
] = 0;
402 static void pidMultiWiiRewrite(pidProfile_t
*pidProfile
, controlRateConfig_t
*controlRateConfig
, uint16_t max_angle_inclination
,
403 rollAndPitchTrims_t
*angleTrim
, rxConfig_t
*rxConfig
)
407 int axis
, deltaCount
;
408 int32_t PTerm
, ITerm
, DTerm
, delta
, deltaSum
;
409 static int32_t lastErrorForDelta
[3] = { 0, 0, 0 };
410 static int32_t previousDelta
[3][DELTA_MAX_SAMPLES
];
411 int32_t AngleRateTmp
, RateError
, gyroRate
;
413 int8_t horizonLevelStrength
= 100;
415 if (!deltaStateIsSet
&& pidProfile
->dterm_lpf_hz
) {
416 for (axis
= 0; axis
< 3; axis
++) BiQuadNewLpf(pidProfile
->dterm_lpf_hz
, &deltaBiQuadState
[axis
], targetLooptime
);
417 deltaStateIsSet
= true;
420 if (FLIGHT_MODE(HORIZON_MODE
)) {
421 // Figure out the raw stick positions
422 const int32_t stickPosAil
= ABS(getRcStickDeflection(FD_ROLL
, rxConfig
->midrc
));
423 const int32_t stickPosEle
= ABS(getRcStickDeflection(FD_PITCH
, rxConfig
->midrc
));
424 const int32_t mostDeflectedPos
= MAX(stickPosAil
, stickPosEle
);
425 // Progressively turn off the horizon self level strength as the stick is banged over
426 horizonLevelStrength
= (500 - mostDeflectedPos
) / 5; // 100 at centre stick, 0 = max stick deflection
427 // Using Level D as a Sensitivity for Horizon. 0 more level to 255 more rate. Default value of 100 seems to work fine.
428 // For more rate mode increase D and slower flips and rolls will be possible
429 horizonLevelStrength
= constrain((10 * (horizonLevelStrength
- 100) * (10 * pidProfile
->D8
[PIDLEVEL
] / 80) / 100) + 100, 0, 100);
432 // ----------PID controller----------
433 for (axis
= 0; axis
< 3; axis
++) {
434 uint8_t rate
= controlRateConfig
->rates
[axis
];
436 // -----Get the desired angle rate depending on flight mode
437 if (axis
== FD_YAW
) {
438 // YAW is always gyro-controlled (MAG correction is applied to rcCommand)
439 AngleRateTmp
= ((int32_t)(rate
+ 27) * rcCommand
[YAW
]) >> 5;
441 AngleRateTmp
= ((int32_t)(rate
+ 27) * rcCommand
[axis
]) >> 4;
442 if (FLIGHT_MODE(ANGLE_MODE
) || FLIGHT_MODE(HORIZON_MODE
)) {
443 // calculate error angle and limit the angle to max configured inclination
445 const int32_t errorAngle
= constrain(2 * rcCommand
[axis
] + GPS_angle
[axis
], -((int) max_angle_inclination
),
446 +max_angle_inclination
) - attitude
.raw
[axis
] + angleTrim
->raw
[axis
];
448 const int32_t errorAngle
= constrain(2 * rcCommand
[axis
], -((int) max_angle_inclination
),
449 +max_angle_inclination
) - attitude
.raw
[axis
] + angleTrim
->raw
[axis
];
451 if (FLIGHT_MODE(ANGLE_MODE
)) {
452 // ANGLE mode - control is angle based, so control loop is needed
453 AngleRateTmp
= (errorAngle
* pidProfile
->P8
[PIDLEVEL
]) >> 4;
455 // HORIZON mode - mix up angle error to desired AngleRateTmp to add a little auto-level feel,
456 // horizonLevelStrength is scaled to the stick input
457 AngleRateTmp
+= (errorAngle
* pidProfile
->I8
[PIDLEVEL
] * horizonLevelStrength
/ 100) >> 4;
462 // --------low-level gyro-based PID. ----------
463 // Used in stand-alone mode for ACRO, controlled by higher level regulators in other modes
464 // -----calculate scaled error.AngleRates
465 // multiplication of rcCommand corresponds to changing the sticks scaling here
466 gyroRate
= gyroADC
[axis
] / 4;
467 RateError
= AngleRateTmp
- gyroRate
;
469 // -----calculate P component
470 PTerm
= (RateError
* pidProfile
->P8
[axis
] * PIDweight
[axis
] / 100) >> 7;
472 // -----calculate I component
473 // there should be no division before accumulating the error to integrator, because the precision would be reduced.
474 // Precision is critical, as I prevents from long-time drift. Thus, 32 bits integrator is used.
475 // Time correction (to avoid different I scaling for different builds based on average cycle time)
476 // is normalized to cycle time = 2048.
477 errorGyroI
[axis
] = errorGyroI
[axis
] + ((RateError
* (uint16_t)targetLooptime
) >> 11) * pidProfile
->I8
[axis
];
479 // limit maximum integrator value to prevent WindUp - accumulating extreme values when system is saturated.
480 // I coefficient (I8) moved before integration to make limiting independent from PID settings
481 errorGyroI
[axis
] = constrain(errorGyroI
[axis
], (int32_t) - GYRO_I_MAX
<< 13, (int32_t) + GYRO_I_MAX
<< 13);
483 if (IS_RC_MODE_ACTIVE(BOXAIRMODE
) || IS_RC_MODE_ACTIVE(BOXACROPLUS
)) {
484 scaleItermToRcInput(axis
, pidProfile
);
485 if (antiWindupProtection
|| motorLimitReached
) {
486 errorGyroI
[axis
] = constrain(errorGyroI
[axis
], -errorGyroILimit
[axis
], errorGyroILimit
[axis
]);
488 errorGyroILimit
[axis
] = ABS(errorGyroI
[axis
]);
492 ITerm
= errorGyroI
[axis
] >> 13;
494 //-----calculate D-term
495 if (pidProfile
->deltaMethod
== DELTA_FROM_ERROR
) {
496 delta
= RateError
- lastErrorForDelta
[axis
]; // 16 bits is ok here, the dif between 2 consecutive gyro reads is limited to 800
497 lastErrorForDelta
[axis
] = RateError
;
499 delta
= -(gyroRate
- lastErrorForDelta
[axis
]); // 16 bits is ok here, the dif between 2 consecutive gyro reads is limited to 800
500 lastErrorForDelta
[axis
] = gyroRate
;
503 // Correct difference by cycle time. Cycle time is jittery (can be different 2 times), so calculated difference
504 // would be scaled by different dt each time. Division by dT fixes that.
505 delta
= (delta
* ((uint16_t) 0xFFFF / ((uint16_t)targetLooptime
>> 4))) >> 6;
507 if (deltaStateIsSet
) {
508 delta
= lrintf(applyBiQuadFilter((float) delta
, &deltaBiQuadState
[axis
])) * 3; // Keep same scaling as unfiltered delta
510 // Apply moving average
512 for (deltaCount
= pidProfile
->dterm_average_count
-1; deltaCount
> 0; deltaCount
--) previousDelta
[axis
][deltaCount
] = previousDelta
[axis
][deltaCount
-1];
513 previousDelta
[axis
][0] = delta
;
514 for (deltaCount
= 0; deltaCount
< pidProfile
->dterm_average_count
; deltaCount
++) deltaSum
+= previousDelta
[axis
][deltaCount
];
515 delta
= (deltaSum
/ pidProfile
->dterm_average_count
) * 3; // Keep same original scaling
518 DTerm
= (delta
* pidProfile
->D8
[axis
] * PIDweight
[axis
] / 100) >> 8;
520 // -----calculate total PID output
521 axisPID
[axis
] = PTerm
+ ITerm
+ DTerm
;
523 if (lowThrottlePidReduction
) axisPID
[axis
] /= 4;
527 axisPID_P
[axis
] = PTerm
;
528 axisPID_I
[axis
] = ITerm
;
529 axisPID_D
[axis
] = DTerm
;
534 void pidSetController(pidControllerType_e type
)
538 case PID_CONTROLLER_MWREWRITE
:
539 pid_controller
= pidMultiWiiRewrite
;
541 case PID_CONTROLLER_LUX_FLOAT
:
542 pid_controller
= pidLuxFloat
;
544 case PID_CONTROLLER_MW23
:
545 pid_controller
= pidMultiWii23
;