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"
29 #include "config/runtime_config.h"
33 #include "drivers/sensor.h"
34 #include "drivers/accgyro.h"
35 #include "sensors/sensors.h"
36 #include "sensors/gyro.h"
38 #include "io/rc_controls.h"
39 #include "flight/flight.h"
40 #include "flight/navigation.h"
41 #include "flight/autotune.h"
44 extern uint16_t cycleTime
;
45 extern uint8_t motorCount
;
47 int16_t heading
, magHold
;
51 int32_t axisPID_P
[3], axisPID_I
[3], axisPID_D
[3];
54 uint8_t dynP8
[3], dynI8
[3], dynD8
[3];
56 static int32_t errorGyroI
[3] = { 0, 0, 0 };
57 static float errorGyroIf
[3] = { 0.0f
, 0.0f
, 0.0f
};
58 static int32_t errorAngleI
[2] = { 0, 0 };
60 static void pidMultiWii(pidProfile_t
*pidProfile
, controlRateConfig_t
*controlRateConfig
,
61 uint16_t max_angle_inclination
, rollAndPitchTrims_t
*angleTrim
, rxConfig_t
*rxConfig
);
63 typedef void (*pidControllerFuncPtr
)(pidProfile_t
*pidProfile
, controlRateConfig_t
*controlRateConfig
,
64 uint16_t max_angle_inclination
, rollAndPitchTrims_t
*angleTrim
, rxConfig_t
*rxConfig
); // pid controller function prototype
66 pidControllerFuncPtr pid_controller
= pidMultiWii
; // which pid controller are we using, defaultMultiWii
68 void resetRollAndPitchTrims(rollAndPitchTrims_t
*rollAndPitchTrims
)
70 rollAndPitchTrims
->values
.roll
= 0;
71 rollAndPitchTrims
->values
.pitch
= 0;
74 void resetErrorAngle(void)
76 errorAngleI
[ROLL
] = 0;
77 errorAngleI
[PITCH
] = 0;
80 void resetErrorGyro(void)
83 errorGyroI
[PITCH
] = 0;
86 errorGyroIf
[ROLL
] = 0;
87 errorGyroIf
[PITCH
] = 0;
91 const angle_index_t rcAliasToAngleIndexMap
[] = { AI_ROLL
, AI_PITCH
};
94 bool shouldAutotune(void)
96 return ARMING_FLAG(ARMED
) && FLIGHT_MODE(AUTOTUNE_MODE
);
100 static void pidLuxFloat(pidProfile_t
*pidProfile
, controlRateConfig_t
*controlRateConfig
,
101 uint16_t max_angle_inclination
, rollAndPitchTrims_t
*angleTrim
, rxConfig_t
*rxConfig
)
103 float RateError
, errorAngle
, AngleRate
, gyroRate
;
104 float ITerm
,PTerm
,DTerm
;
105 int32_t stickPosAil
, stickPosEle
, mostDeflectedPos
;
106 static float lastGyroRate
[3];
107 static float delta1
[3], delta2
[3];
108 float delta
, deltaSum
;
111 float horizonLevelStrength
= 1;
113 dT
= (float)cycleTime
* 0.000001f
;
115 if (FLIGHT_MODE(HORIZON_MODE
)) {
117 // Figure out the raw stick positions
118 stickPosAil
= getRcStickDeflection(FD_ROLL
, rxConfig
->midrc
);
119 stickPosEle
= getRcStickDeflection(FD_PITCH
, rxConfig
->midrc
);
121 if(ABS(stickPosAil
) > ABS(stickPosEle
)){
122 mostDeflectedPos
= ABS(stickPosAil
);
125 mostDeflectedPos
= ABS(stickPosEle
);
128 // Progressively turn off the horizon self level strength as the stick is banged over
129 horizonLevelStrength
= (float)(500 - mostDeflectedPos
) / 500; // 1 at centre stick, 0 = max stick deflection
130 if(pidProfile
->H_sensitivity
== 0){
131 horizonLevelStrength
= 0;
133 horizonLevelStrength
= constrainf(((horizonLevelStrength
- 1) * (100 / pidProfile
->H_sensitivity
)) + 1, 0, 1);
137 // ----------PID controller----------
138 for (axis
= 0; axis
< 3; axis
++) {
139 // -----Get the desired angle rate depending on flight mode
140 if (axis
== FD_YAW
) {
141 // YAW is always gyro-controlled (MAG correction is applied to rcCommand) 100dps to 1100dps max yaw rate
142 AngleRate
= (float)((controlRateConfig
->yawRate
+ 10) * rcCommand
[YAW
]) / 50.0f
;
144 // calculate error and limit the angle to the max inclination
146 errorAngle
= (constrain(rcCommand
[axis
] + GPS_angle
[axis
], -((int) max_angle_inclination
),
147 +max_angle_inclination
) - inclination
.raw
[axis
] + angleTrim
->raw
[axis
]) / 10.0f
; // 16 bits is ok here
149 errorAngle
= (constrain(rcCommand
[axis
], -((int) max_angle_inclination
),
150 +max_angle_inclination
) - inclination
.raw
[axis
] + angleTrim
->raw
[axis
]) / 10.0f
; // 16 bits is ok here
154 if (shouldAutotune()) {
155 errorAngle
= autotune(rcAliasToAngleIndexMap
[axis
], &inclination
, errorAngle
);
159 if (FLIGHT_MODE(ANGLE_MODE
)) {
160 // it's the ANGLE mode - control is angle based, so control loop is needed
161 AngleRate
= errorAngle
* pidProfile
->A_level
;
163 //control is GYRO based (ACRO and HORIZON - direct sticks control is applied to rate PID
164 AngleRate
= (float)((controlRateConfig
->rollPitchRate
+ 20) * rcCommand
[axis
]) / 50.0f
; // 200dps to 1200dps max yaw rate
165 if (FLIGHT_MODE(HORIZON_MODE
)) {
166 // mix up angle error to desired AngleRate to add a little auto-level feel
167 AngleRate
+= errorAngle
* pidProfile
->H_level
* horizonLevelStrength
;
172 gyroRate
= gyroData
[axis
] * gyro
.scale
; // gyro output scaled to dps
174 // --------low-level gyro-based PID. ----------
175 // Used in stand-alone mode for ACRO, controlled by higher level regulators in other modes
176 // -----calculate scaled error.AngleRates
177 // multiplication of rcCommand corresponds to changing the sticks scaling here
178 RateError
= AngleRate
- gyroRate
;
180 // -----calculate P component
181 PTerm
= RateError
* pidProfile
->P_f
[axis
];
182 // -----calculate I component
183 errorGyroIf
[axis
] = constrainf(errorGyroIf
[axis
] + RateError
* dT
* pidProfile
->I_f
[axis
], -250.0f
, 250.0f
);
185 // limit maximum integrator value to prevent WindUp - accumulating extreme values when system is saturated.
186 // I coefficient (I8) moved before integration to make limiting independent from PID settings
187 ITerm
= errorGyroIf
[axis
];
189 //-----calculate D-term
190 delta
= gyroRate
- lastGyroRate
[axis
]; // 16 bits is ok here, the dif between 2 consecutive gyro reads is limited to 800
191 lastGyroRate
[axis
] = gyroRate
;
193 // Correct difference by cycle time. Cycle time is jittery (can be different 2 times), so calculated difference
194 // would be scaled by different dt each time. Division by dT fixes that.
195 delta
*= (1.0f
/ dT
);
196 // add moving average here to reduce noise
197 deltaSum
= delta1
[axis
] + delta2
[axis
] + delta
;
198 delta2
[axis
] = delta1
[axis
];
199 delta1
[axis
] = delta
;
200 DTerm
= constrainf((deltaSum
/ 3.0f
) * pidProfile
->D_f
[axis
], -300.0f
, 300.0f
);
202 // -----calculate total PID output
203 axisPID
[axis
] = constrain(lrintf(PTerm
+ ITerm
- DTerm
), -1000, 1000);
208 static void pidMultiWii(pidProfile_t
*pidProfile
, controlRateConfig_t
*controlRateConfig
,
209 uint16_t max_angle_inclination
, rollAndPitchTrims_t
*angleTrim
, rxConfig_t
*rxConfig
)
214 int32_t error
, errorAngle
;
215 int32_t PTerm
, ITerm
, PTermACC
= 0, ITermACC
= 0, PTermGYRO
= 0, ITermGYRO
= 0, DTerm
;
216 static int16_t lastGyro
[3] = { 0, 0, 0 };
217 static int32_t delta1
[3], delta2
[3];
221 UNUSED(controlRateConfig
);
223 // **** PITCH & ROLL & YAW PID ****
224 prop
= MIN(MAX(ABS(rcCommand
[PITCH
]), ABS(rcCommand
[ROLL
])), 500); // range [0;500]
226 for (axis
= 0; axis
< 3; axis
++) {
227 if ((FLIGHT_MODE(ANGLE_MODE
) || FLIGHT_MODE(HORIZON_MODE
)) && (axis
== FD_ROLL
|| axis
== FD_PITCH
)) { // MODE relying on ACC
228 // observe max inclination
230 errorAngle
= constrain(2 * rcCommand
[axis
] + GPS_angle
[axis
], -((int) max_angle_inclination
),
231 +max_angle_inclination
) - inclination
.raw
[axis
] + angleTrim
->raw
[axis
];
233 errorAngle
= constrain(2 * rcCommand
[axis
], -((int) max_angle_inclination
),
234 +max_angle_inclination
) - inclination
.raw
[axis
] + angleTrim
->raw
[axis
];
238 if (shouldAutotune()) {
239 errorAngle
= DEGREES_TO_DECIDEGREES(autotune(rcAliasToAngleIndexMap
[axis
], &inclination
, DECIDEGREES_TO_DEGREES(errorAngle
)));
243 PTermACC
= errorAngle
* pidProfile
->P8
[PIDLEVEL
] / 100; // 32 bits is needed for calculation: errorAngle*P8[PIDLEVEL] could exceed 32768 16 bits is ok for result
244 PTermACC
= constrain(PTermACC
, -pidProfile
->D8
[PIDLEVEL
] * 5, +pidProfile
->D8
[PIDLEVEL
] * 5);
246 errorAngleI
[axis
] = constrain(errorAngleI
[axis
] + errorAngle
, -10000, +10000); // WindUp
247 ITermACC
= (errorAngleI
[axis
] * pidProfile
->I8
[PIDLEVEL
]) >> 12;
249 if (!FLIGHT_MODE(ANGLE_MODE
) || FLIGHT_MODE(HORIZON_MODE
) || axis
== FD_YAW
) { // MODE relying on GYRO or YAW axis
250 error
= (int32_t) rcCommand
[axis
] * 10 * 8 / pidProfile
->P8
[axis
];
251 error
-= gyroData
[axis
] / 4;
253 PTermGYRO
= rcCommand
[axis
];
255 errorGyroI
[axis
] = constrain(errorGyroI
[axis
] + error
, -16000, +16000); // WindUp
256 if ((ABS(gyroData
[axis
]) > (640 * 4)) || (axis
== FD_YAW
&& ABS(rcCommand
[axis
]) > 100))
257 errorGyroI
[axis
] = 0;
259 ITermGYRO
= (errorGyroI
[axis
] / 125 * pidProfile
->I8
[axis
]) / 64;
261 if (FLIGHT_MODE(HORIZON_MODE
) && (axis
== FD_ROLL
|| axis
== FD_PITCH
)) {
262 PTerm
= (PTermACC
* (500 - prop
) + PTermGYRO
* prop
) / 500;
263 ITerm
= (ITermACC
* (500 - prop
) + ITermGYRO
* prop
) / 500;
265 if (FLIGHT_MODE(ANGLE_MODE
) && (axis
== FD_ROLL
|| axis
== FD_PITCH
)) {
274 PTerm
-= ((int32_t)gyroData
[axis
] / 4) * dynP8
[axis
] / 10 / 8; // 32 bits is needed for calculation
275 delta
= (gyroData
[axis
] - lastGyro
[axis
]) / 4;
276 lastGyro
[axis
] = gyroData
[axis
];
277 deltaSum
= delta1
[axis
] + delta2
[axis
] + delta
;
278 delta2
[axis
] = delta1
[axis
];
279 delta1
[axis
] = delta
;
280 DTerm
= (deltaSum
* dynD8
[axis
]) / 32;
281 axisPID
[axis
] = PTerm
+ ITerm
- DTerm
;
284 axisPID_P
[axis
] = PTerm
;
285 axisPID_I
[axis
] = ITerm
;
286 axisPID_D
[axis
] = -DTerm
;
291 #define GYRO_P_MAX 300
292 #define GYRO_I_MAX 256
294 static void pidMultiWii23(pidProfile_t
*pidProfile
, controlRateConfig_t
*controlRateConfig
, uint16_t max_angle_inclination
,
295 rollAndPitchTrims_t
*angleTrim
, rxConfig_t
*rxConfig
)
300 int32_t rc
, error
, errorAngle
;
301 int32_t PTerm
, ITerm
, PTermACC
, ITermACC
, DTerm
;
302 static int16_t lastGyro
[2] = { 0, 0 };
303 static int32_t delta1
[2] = { 0, 0 }, delta2
[2] = { 0, 0 };
306 if (FLIGHT_MODE(HORIZON_MODE
)) prop
= MIN(MAX(ABS(rcCommand
[PITCH
]), ABS(rcCommand
[ROLL
])), 512);
309 for (axis
= 0; axis
< 2; axis
++) {
310 rc
= rcCommand
[axis
] << 1;
311 error
= rc
- (gyroData
[axis
] / 4);
312 errorGyroI
[axis
] = constrain(errorGyroI
[axis
] + error
, -16000, +16000); // WindUp 16 bits is ok here
313 if (ABS(gyroData
[axis
]) > (640 * 4)) errorGyroI
[axis
] = 0;
315 ITerm
= (errorGyroI
[axis
] >> 7) * pidProfile
->I8
[axis
] >> 6; // 16 bits is ok here 16000/125 = 128 ; 128*250 = 32000
317 PTerm
= (int32_t)rc
* pidProfile
->P8
[axis
] >> 6;
319 if (FLIGHT_MODE(ANGLE_MODE
) || FLIGHT_MODE(HORIZON_MODE
)) { // axis relying on ACC
320 // 50 degrees max inclination
322 errorAngle
= constrain(2 * rcCommand
[axis
] + GPS_angle
[axis
], -((int) max_angle_inclination
),
323 +max_angle_inclination
) - inclination
.raw
[axis
] + angleTrim
->raw
[axis
];
325 errorAngle
= constrain(2 * rcCommand
[axis
], -((int) max_angle_inclination
),
326 +max_angle_inclination
) - inclination
.raw
[axis
] + angleTrim
->raw
[axis
];
330 if (shouldAutotune()) {
331 errorAngle
= DEGREES_TO_DECIDEGREES(autotune(rcAliasToAngleIndexMap
[axis
], &inclination
, DECIDEGREES_TO_DEGREES(errorAngle
)));
335 errorAngleI
[axis
] = constrain(errorAngleI
[axis
] + errorAngle
, -10000, +10000); // WindUp //16 bits is ok here
337 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
339 int16_t limit
= pidProfile
->D8
[PIDLEVEL
] * 5;
340 PTermACC
= constrain(PTermACC
, -limit
, +limit
);
342 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
344 ITerm
= ITermACC
+ ((ITerm
- ITermACC
) * prop
>> 9);
345 PTerm
= PTermACC
+ ((PTerm
- PTermACC
) * prop
>> 9);
348 PTerm
-= ((int32_t)(gyroData
[axis
] / 4) * dynP8
[axis
]) >> 6; // 32 bits is needed for calculation
350 delta
= (gyroData
[axis
] - lastGyro
[axis
]) / 4; // 16 bits is ok here, the dif between 2 consecutive gyro reads is limited to 800
351 lastGyro
[axis
] = gyroData
[axis
];
352 DTerm
= delta1
[axis
] + delta2
[axis
] + delta
;
353 delta2
[axis
] = delta1
[axis
];
354 delta1
[axis
] = delta
;
356 DTerm
= ((int32_t)DTerm
* dynD8
[axis
]) >> 5; // 32 bits is needed for calculation
358 axisPID
[axis
] = PTerm
+ ITerm
- DTerm
;
361 axisPID_P
[axis
] = PTerm
;
362 axisPID_I
[axis
] = ITerm
;
363 axisPID_D
[axis
] = -DTerm
;
368 rc
= (int32_t)rcCommand
[FD_YAW
] * (2 * controlRateConfig
->yawRate
+ 30) >> 5;
370 error
= rc
- gyroData
[FD_YAW
];
372 error
= rc
- (gyroData
[FD_YAW
] / 4);
374 errorGyroI
[FD_YAW
] += (int32_t)error
* pidProfile
->I8
[FD_YAW
];
375 errorGyroI
[FD_YAW
] = constrain(errorGyroI
[FD_YAW
], 2 - ((int32_t)1 << 28), -2 + ((int32_t)1 << 28));
376 if (ABS(rc
) > 50) errorGyroI
[FD_YAW
] = 0;
378 PTerm
= (int32_t)error
* pidProfile
->P8
[FD_YAW
] >> 6;
380 // Constrain YAW by D value if not servo driven in that case servolimits apply
382 int16_t limit
= GYRO_P_MAX
- pidProfile
->D8
[FD_YAW
];
383 PTerm
= constrain(PTerm
, -limit
, +limit
);
386 ITerm
= constrain((int16_t)(errorGyroI
[FD_YAW
] >> 13), -GYRO_I_MAX
, +GYRO_I_MAX
);
388 axisPID
[FD_YAW
] = PTerm
+ ITerm
;
391 axisPID_P
[FD_YAW
] = PTerm
;
392 axisPID_I
[FD_YAW
] = ITerm
;
393 axisPID_D
[FD_YAW
] = 0;
397 static void pidMultiWiiHybrid(pidProfile_t
*pidProfile
, controlRateConfig_t
*controlRateConfig
,
398 uint16_t max_angle_inclination
, rollAndPitchTrims_t
*angleTrim
, rxConfig_t
*rxConfig
)
403 int32_t rc
, error
, errorAngle
;
404 int32_t PTerm
, ITerm
, PTermACC
= 0, ITermACC
= 0, PTermGYRO
= 0, ITermGYRO
= 0, DTerm
;
405 static int16_t lastGyro
[2] = { 0, 0 };
406 static int32_t delta1
[2] = { 0, 0 }, delta2
[2] = { 0, 0 };
410 UNUSED(controlRateConfig
);
412 // **** PITCH & ROLL ****
413 prop
= MIN(MAX(ABS(rcCommand
[PITCH
]), ABS(rcCommand
[ROLL
])), 500); // range [0;500]
415 for (axis
= 0; axis
< 2; axis
++) {
416 if ((FLIGHT_MODE(ANGLE_MODE
) || FLIGHT_MODE(HORIZON_MODE
))) { // MODE relying on ACC
417 // observe max inclination
419 errorAngle
= constrain(2 * rcCommand
[axis
] + GPS_angle
[axis
], -((int) max_angle_inclination
),
420 +max_angle_inclination
) - inclination
.raw
[axis
] + angleTrim
->raw
[axis
];
422 errorAngle
= constrain(2 * rcCommand
[axis
], -((int) max_angle_inclination
),
423 +max_angle_inclination
) - inclination
.raw
[axis
] + angleTrim
->raw
[axis
];
427 if (shouldAutotune()) {
428 errorAngle
= DEGREES_TO_DECIDEGREES(autotune(rcAliasToAngleIndexMap
[axis
], &inclination
, DECIDEGREES_TO_DEGREES(errorAngle
)));
432 PTermACC
= errorAngle
* pidProfile
->P8
[PIDLEVEL
] / 100; // 32 bits is needed for calculation: errorAngle*P8[PIDLEVEL] could exceed 32768 16 bits is ok for result
433 PTermACC
= constrain(PTermACC
, -pidProfile
->D8
[PIDLEVEL
] * 5, +pidProfile
->D8
[PIDLEVEL
] * 5);
435 errorAngleI
[axis
] = constrain(errorAngleI
[axis
] + errorAngle
, -10000, +10000); // WindUp
436 ITermACC
= (errorAngleI
[axis
] * pidProfile
->I8
[PIDLEVEL
]) >> 12;
438 if (!FLIGHT_MODE(ANGLE_MODE
) || FLIGHT_MODE(HORIZON_MODE
)) { // MODE relying on GYRO
439 error
= (int32_t) rcCommand
[axis
] * 10 * 8 / pidProfile
->P8
[axis
];
440 error
-= gyroData
[axis
] / 4;
442 PTermGYRO
= rcCommand
[axis
];
444 errorGyroI
[axis
] = constrain(errorGyroI
[axis
] + error
, -16000, +16000); // WindUp
445 if (ABS(gyroData
[axis
]) > (640 * 4))
446 errorGyroI
[axis
] = 0;
448 ITermGYRO
= (errorGyroI
[axis
] / 125 * pidProfile
->I8
[axis
]) / 64;
450 if (FLIGHT_MODE(HORIZON_MODE
)) {
451 PTerm
= (PTermACC
* (500 - prop
) + PTermGYRO
* prop
) / 500;
452 ITerm
= (ITermACC
* (500 - prop
) + ITermGYRO
* prop
) / 500;
454 if (FLIGHT_MODE(ANGLE_MODE
)) {
463 PTerm
-= ((int32_t)gyroData
[axis
] / 4) * dynP8
[axis
] / 10 / 8; // 32 bits is needed for calculation
464 delta
= (gyroData
[axis
] - lastGyro
[axis
]) / 4;
465 lastGyro
[axis
] = gyroData
[axis
];
466 deltaSum
= delta1
[axis
] + delta2
[axis
] + delta
;
467 delta2
[axis
] = delta1
[axis
];
468 delta1
[axis
] = delta
;
469 DTerm
= (deltaSum
* dynD8
[axis
]) / 32;
470 axisPID
[axis
] = PTerm
+ ITerm
- DTerm
;
473 axisPID_P
[axis
] = PTerm
;
474 axisPID_I
[axis
] = ITerm
;
475 axisPID_D
[axis
] = -DTerm
;
479 rc
= (int32_t)rcCommand
[FD_YAW
] * (2 * controlRateConfig
->yawRate
+ 30) >> 5;
481 error
= rc
- gyroData
[FD_YAW
];
483 error
= rc
- (gyroData
[FD_YAW
] / 4);
485 errorGyroI
[FD_YAW
] += (int32_t)error
* pidProfile
->I8
[FD_YAW
];
486 errorGyroI
[FD_YAW
] = constrain(errorGyroI
[FD_YAW
], 2 - ((int32_t)1 << 28), -2 + ((int32_t)1 << 28));
487 if (ABS(rc
) > 50) errorGyroI
[FD_YAW
] = 0;
489 PTerm
= (int32_t)error
* pidProfile
->P8
[FD_YAW
] >> 6;
491 // Constrain YAW by D value if not servo driven in that case servolimits apply
493 int16_t limit
= GYRO_P_MAX
- pidProfile
->D8
[FD_YAW
];
494 PTerm
= constrain(PTerm
, -limit
, +limit
);
497 ITerm
= constrain((int16_t)(errorGyroI
[FD_YAW
] >> 13), -GYRO_I_MAX
, +GYRO_I_MAX
);
499 axisPID
[FD_YAW
] = PTerm
+ ITerm
;
503 axisPID_P
[FD_YAW
] = PTerm
;
504 axisPID_I
[FD_YAW
] = ITerm
;
505 axisPID_D
[FD_YAW
] = 0;
509 #define RCconstPI 0.159154943092f // 0.5f / M_PI;
510 #define MAIN_CUT_HZ 12.0f // (default 12Hz, Range 1-50Hz)
511 #define OLD_YAW 0 // [0/1] 0 = multiwii 2.3 yaw, 1 = older yaw.
513 static void pidHarakiri(pidProfile_t
*pidProfile
, controlRateConfig_t
*controlRateConfig
, uint16_t max_angle_inclination
,
514 rollAndPitchTrims_t
*angleTrim
, rxConfig_t
*rxConfig
)
518 float delta
, RCfactor
, rcCommandAxis
, MainDptCut
;
519 float PTerm
= 0, ITerm
= 0, DTerm
= 0, PTermACC
= 0, ITermACC
= 0, ITermGYRO
= 0, error
= 0, prop
= 0;
520 static float lastGyro
[2] = {0, 0}, lastDTerm
[2] = {0, 0};
524 float ACCDeltaTimeINS
= 0;
525 float FLOATcycleTime
= 0;
527 // MainDptCut = RCconstPI / (float)cfg.maincuthz; // Initialize Cut off frequencies for mainpid D
528 MainDptCut
= RCconstPI
/ MAIN_CUT_HZ
; // maincuthz (default 12Hz, Range 1-50Hz), hardcoded for now
529 FLOATcycleTime
= (float)constrain(cycleTime
, 1, 100000); // 1us - 100ms
530 ACCDeltaTimeINS
= FLOATcycleTime
* 0.000001f
; // ACCDeltaTimeINS is in seconds now
531 RCfactor
= ACCDeltaTimeINS
/ (MainDptCut
+ ACCDeltaTimeINS
); // used for pt1 element
533 if (FLIGHT_MODE(HORIZON_MODE
)) {
534 prop
= (float)MIN(MAX(ABS(rcCommand
[PITCH
]), ABS(rcCommand
[ROLL
])), 450) / 450.0f
;
537 for (axis
= 0; axis
< 2; axis
++) {
538 rcCommandAxis
= (float)rcCommand
[axis
]; // Calculate common values for pid controllers
539 if (FLIGHT_MODE(ANGLE_MODE
) || FLIGHT_MODE(HORIZON_MODE
)) {
541 error
= constrain(2.0f
* rcCommandAxis
+ GPS_angle
[axis
], -((int) max_angle_inclination
), +max_angle_inclination
) - inclination
.raw
[axis
] + angleTrim
->raw
[axis
];
543 error
= constrain(2.0f
* rcCommandAxis
, -((int) max_angle_inclination
), +max_angle_inclination
) - inclination
.raw
[axis
] + angleTrim
->raw
[axis
];
547 if (shouldAutotune()) {
548 error
= DEGREES_TO_DECIDEGREES(autotune(rcAliasToAngleIndexMap
[axis
], &inclination
, DECIDEGREES_TO_DEGREES(error
)));
551 PTermACC
= error
* (float)pidProfile
->P8
[PIDLEVEL
] * 0.008f
;
552 tmp0flt
= (float)pidProfile
->D8
[PIDLEVEL
] * 5.0f
;
553 PTermACC
= constrain(PTermACC
, -tmp0flt
, +tmp0flt
);
554 errorAngleI
[axis
] = constrain(errorAngleI
[axis
] + error
* ACCDeltaTimeINS
, -30.0f
, +30.0f
);
555 ITermACC
= errorAngleI
[axis
] * (float)pidProfile
->I8
[PIDLEVEL
] * 0.08f
;
558 if (!FLIGHT_MODE(ANGLE_MODE
)) {
559 if (ABS((int16_t)gyroData
[axis
]) > 2560) {
560 errorGyroI
[axis
] = 0.0f
;
562 error
= (rcCommandAxis
* 320.0f
/ (float)pidProfile
->P8
[axis
]) - gyroData
[axis
];
563 errorGyroI
[axis
] = constrain(errorGyroI
[axis
] + error
* ACCDeltaTimeINS
, -192.0f
, +192.0f
);
566 ITermGYRO
= errorGyroI
[axis
] * (float)pidProfile
->I8
[axis
] * 0.01f
;
568 if (FLIGHT_MODE(HORIZON_MODE
)) {
569 PTerm
= PTermACC
+ prop
* (rcCommandAxis
- PTermACC
);
570 ITerm
= ITermACC
+ prop
* (ITermGYRO
- ITermACC
);
572 PTerm
= rcCommandAxis
;
580 PTerm
-= gyroData
[axis
] * dynP8
[axis
] * 0.003f
;
581 delta
= (gyroData
[axis
] - lastGyro
[axis
]) / ACCDeltaTimeINS
;
583 lastGyro
[axis
] = gyroData
[axis
];
584 lastDTerm
[axis
] += RCfactor
* (delta
- lastDTerm
[axis
]);
585 DTerm
= lastDTerm
[axis
] * dynD8
[axis
] * 0.00007f
;
587 axisPID
[axis
] = lrintf(PTerm
+ ITerm
- DTerm
); // Round up result.
590 axisPID_P
[axis
] = PTerm
;
591 axisPID_I
[axis
] = ITerm
;
592 axisPID_D
[axis
] = -DTerm
;
596 tmp0flt
= (int32_t)FLOATcycleTime
& (int32_t)~3; // Filter last 2 bit jitter
599 if (OLD_YAW
) { // [0/1] 0 = multiwii 2.3 yaw, 1 = older yaw. hardcoded for now
600 PTerm
= ((int32_t)pidProfile
->P8
[FD_YAW
] * (100 - (int32_t)controlRateConfig
->yawRate
* (int32_t)ABS(rcCommand
[FD_YAW
]) / 500)) / 100;
601 tmp0
= lrintf(gyroData
[FD_YAW
] * 0.25f
);
602 PTerm
= rcCommand
[FD_YAW
] - tmp0
* PTerm
/ 80;
603 if ((ABS(tmp0
) > 640) || (ABS(rcCommand
[FD_YAW
]) > 100)) {
604 errorGyroI
[FD_YAW
] = 0;
606 error
= ((int32_t)rcCommand
[FD_YAW
] * 80 / (int32_t)pidProfile
->P8
[FD_YAW
]) - tmp0
;
607 errorGyroI
[FD_YAW
] = constrain(errorGyroI
[FD_YAW
] + (int32_t)(error
* tmp0flt
), -16000, +16000); // WindUp
608 ITerm
= (errorGyroI
[FD_YAW
] / 125 * pidProfile
->I8
[FD_YAW
]) >> 6;
611 tmp0
= ((int32_t)rcCommand
[FD_YAW
] * (((int32_t)controlRateConfig
->yawRate
<< 1) + 40)) >> 5;
612 error
= tmp0
- lrintf(gyroData
[FD_YAW
] * 0.25f
); // Less Gyrojitter works actually better
614 if (ABS(tmp0
) > 50) {
615 errorGyroI
[FD_YAW
] = 0;
617 errorGyroI
[FD_YAW
] = constrain(errorGyroI
[FD_YAW
] + (int32_t)(error
* (float)pidProfile
->I8
[FD_YAW
] * tmp0flt
), -268435454, +268435454);
620 ITerm
= constrain(errorGyroI
[FD_YAW
] >> 13, -GYRO_I_MAX
, +GYRO_I_MAX
);
621 PTerm
= ((int32_t)error
* (int32_t)pidProfile
->P8
[FD_YAW
]) >> 6;
623 if (motorCount
>= 4) { // Constrain FD_YAW by D value if not servo driven in that case servolimits apply
625 if (pidProfile
->D8
[FD_YAW
]) tmp0
-= (int32_t)pidProfile
->D8
[FD_YAW
];
626 PTerm
= constrain(PTerm
, -tmp0
, tmp0
);
629 axisPID
[FD_YAW
] = PTerm
+ ITerm
;
630 axisPID
[FD_YAW
] = lrintf(axisPID
[FD_YAW
]); // Round up result.
633 axisPID_P
[FD_YAW
] = PTerm
;
634 axisPID_I
[FD_YAW
] = ITerm
;
635 axisPID_D
[FD_YAW
] = 0;
639 static void pidRewrite(pidProfile_t
*pidProfile
, controlRateConfig_t
*controlRateConfig
, uint16_t max_angle_inclination
,
640 rollAndPitchTrims_t
*angleTrim
, rxConfig_t
*rxConfig
)
646 int32_t delta
, deltaSum
;
647 static int32_t delta1
[3], delta2
[3];
648 int32_t PTerm
, ITerm
, DTerm
;
649 static int32_t lastError
[3] = { 0, 0, 0 };
650 int32_t AngleRateTmp
, RateError
;
652 // ----------PID controller----------
653 for (axis
= 0; axis
< 3; axis
++) {
654 // -----Get the desired angle rate depending on flight mode
655 if (axis
== FD_YAW
) { // YAW is always gyro-controlled (MAG correction is applied to rcCommand)
656 AngleRateTmp
= (((int32_t)(controlRateConfig
->yawRate
+ 27) * rcCommand
[YAW
]) >> 5);
658 // calculate error and limit the angle to max configured inclination
660 errorAngle
= constrain(2 * rcCommand
[axis
] + GPS_angle
[axis
], -((int) max_angle_inclination
),
661 +max_angle_inclination
) - inclination
.raw
[axis
] + angleTrim
->raw
[axis
]; // 16 bits is ok here
663 errorAngle
= constrain(2 * rcCommand
[axis
], -((int) max_angle_inclination
),
664 +max_angle_inclination
) - inclination
.raw
[axis
] + angleTrim
->raw
[axis
]; // 16 bits is ok here
668 if (shouldAutotune()) {
669 errorAngle
= DEGREES_TO_DECIDEGREES(autotune(rcAliasToAngleIndexMap
[axis
], &inclination
, DECIDEGREES_TO_DEGREES(errorAngle
)));
673 if (!FLIGHT_MODE(ANGLE_MODE
)) { //control is GYRO based (ACRO and HORIZON - direct sticks control is applied to rate PID
674 AngleRateTmp
= ((int32_t)(controlRateConfig
->rollPitchRate
+ 27) * rcCommand
[axis
]) >> 4;
675 if (FLIGHT_MODE(HORIZON_MODE
)) {
676 // mix up angle error to desired AngleRateTmp to add a little auto-level feel
677 AngleRateTmp
+= (errorAngle
* pidProfile
->I8
[PIDLEVEL
]) >> 8;
679 } else { // it's the ANGLE mode - control is angle based, so control loop is needed
680 AngleRateTmp
= (errorAngle
* pidProfile
->P8
[PIDLEVEL
]) >> 4;
684 // --------low-level gyro-based PID. ----------
685 // Used in stand-alone mode for ACRO, controlled by higher level regulators in other modes
686 // -----calculate scaled error.AngleRates
687 // multiplication of rcCommand corresponds to changing the sticks scaling here
688 RateError
= AngleRateTmp
- (gyroData
[axis
] / 4);
690 // -----calculate P component
691 PTerm
= (RateError
* pidProfile
->P8
[axis
]) >> 7;
692 // -----calculate I component
693 // there should be no division before accumulating the error to integrator, because the precision would be reduced.
694 // Precision is critical, as I prevents from long-time drift. Thus, 32 bits integrator is used.
695 // Time correction (to avoid different I scaling for different builds based on average cycle time)
696 // is normalized to cycle time = 2048.
697 errorGyroI
[axis
] = errorGyroI
[axis
] + ((RateError
* cycleTime
) >> 11) * pidProfile
->I8
[axis
];
699 // limit maximum integrator value to prevent WindUp - accumulating extreme values when system is saturated.
700 // I coefficient (I8) moved before integration to make limiting independent from PID settings
701 errorGyroI
[axis
] = constrain(errorGyroI
[axis
], (int32_t) - GYRO_I_MAX
<< 13, (int32_t) + GYRO_I_MAX
<< 13);
702 ITerm
= errorGyroI
[axis
] >> 13;
704 //-----calculate D-term
705 delta
= RateError
- lastError
[axis
]; // 16 bits is ok here, the dif between 2 consecutive gyro reads is limited to 800
706 lastError
[axis
] = RateError
;
708 // Correct difference by cycle time. Cycle time is jittery (can be different 2 times), so calculated difference
709 // would be scaled by different dt each time. Division by dT fixes that.
710 delta
= (delta
* ((uint16_t) 0xFFFF / (cycleTime
>> 4))) >> 6;
711 // add moving average here to reduce noise
712 deltaSum
= delta1
[axis
] + delta2
[axis
] + delta
;
713 delta2
[axis
] = delta1
[axis
];
714 delta1
[axis
] = delta
;
715 DTerm
= (deltaSum
* pidProfile
->D8
[axis
]) >> 8;
717 // -----calculate total PID output
718 axisPID
[axis
] = PTerm
+ ITerm
+ DTerm
;
721 axisPID_P
[axis
] = PTerm
;
722 axisPID_I
[axis
] = ITerm
;
723 axisPID_D
[axis
] = DTerm
;
728 void setPIDController(int type
)
733 pid_controller
= pidMultiWii
;
736 pid_controller
= pidRewrite
;
739 pid_controller
= pidLuxFloat
;
742 pid_controller
= pidMultiWii23
;
745 pid_controller
= pidMultiWiiHybrid
;
748 pid_controller
= pidHarakiri
;