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 "drivers/sensor.h"
30 #include "drivers/accgyro.h"
32 #include "sensors/sensors.h"
33 #include "sensors/gyro.h"
34 #include "sensors/acceleration.h"
38 #include "io/rc_controls.h"
41 #include "flight/flight.h"
42 #include "flight/imu.h"
43 #include "flight/navigation.h"
44 #include "flight/autotune.h"
46 #include "config/runtime_config.h"
48 extern uint16_t cycleTime
;
49 extern uint8_t motorCount
;
51 int16_t heading
, magHold
;
55 int32_t axisPID_P
[3], axisPID_I
[3], axisPID_D
[3];
58 uint8_t dynP8
[3], dynI8
[3], dynD8
[3];
60 static int32_t errorGyroI
[3] = { 0, 0, 0 };
61 static float errorGyroIf
[3] = { 0.0f
, 0.0f
, 0.0f
};
62 static int32_t errorAngleI
[2] = { 0, 0 };
64 static void pidMultiWii(pidProfile_t
*pidProfile
, controlRateConfig_t
*controlRateConfig
,
65 uint16_t max_angle_inclination
, rollAndPitchTrims_t
*angleTrim
, rxConfig_t
*rxConfig
);
67 typedef void (*pidControllerFuncPtr
)(pidProfile_t
*pidProfile
, controlRateConfig_t
*controlRateConfig
,
68 uint16_t max_angle_inclination
, rollAndPitchTrims_t
*angleTrim
, rxConfig_t
*rxConfig
); // pid controller function prototype
70 pidControllerFuncPtr pid_controller
= pidMultiWii
; // which pid controller are we using, defaultMultiWii
72 void resetErrorAngle(void)
74 errorAngleI
[ROLL
] = 0;
75 errorAngleI
[PITCH
] = 0;
78 void resetErrorGyro(void)
81 errorGyroI
[PITCH
] = 0;
84 errorGyroIf
[ROLL
] = 0;
85 errorGyroIf
[PITCH
] = 0;
89 const angle_index_t rcAliasToAngleIndexMap
[] = { AI_ROLL
, AI_PITCH
};
92 bool shouldAutotune(void)
94 return ARMING_FLAG(ARMED
) && FLIGHT_MODE(AUTOTUNE_MODE
);
98 static void pidLuxFloat(pidProfile_t
*pidProfile
, controlRateConfig_t
*controlRateConfig
,
99 uint16_t max_angle_inclination
, rollAndPitchTrims_t
*angleTrim
, rxConfig_t
*rxConfig
)
101 float RateError
, errorAngle
, AngleRate
, gyroRate
;
102 float ITerm
,PTerm
,DTerm
;
103 int32_t stickPosAil
, stickPosEle
, mostDeflectedPos
;
104 static float lastGyroRate
[3];
105 static float delta1
[3], delta2
[3];
106 float delta
, deltaSum
;
109 float horizonLevelStrength
= 1;
111 dT
= (float)cycleTime
* 0.000001f
;
113 if (FLIGHT_MODE(HORIZON_MODE
)) {
115 // Figure out the raw stick positions
116 stickPosAil
= getRcStickDeflection(FD_ROLL
, rxConfig
->midrc
);
117 stickPosEle
= getRcStickDeflection(FD_PITCH
, rxConfig
->midrc
);
119 if(ABS(stickPosAil
) > ABS(stickPosEle
)){
120 mostDeflectedPos
= ABS(stickPosAil
);
123 mostDeflectedPos
= ABS(stickPosEle
);
126 // Progressively turn off the horizon self level strength as the stick is banged over
127 horizonLevelStrength
= (float)(500 - mostDeflectedPos
) / 500; // 1 at centre stick, 0 = max stick deflection
128 if(pidProfile
->H_sensitivity
== 0){
129 horizonLevelStrength
= 0;
131 horizonLevelStrength
= constrainf(((horizonLevelStrength
- 1) * (100 / pidProfile
->H_sensitivity
)) + 1, 0, 1);
135 // ----------PID controller----------
136 for (axis
= 0; axis
< 3; axis
++) {
137 // -----Get the desired angle rate depending on flight mode
138 if (axis
== FD_YAW
) {
139 // YAW is always gyro-controlled (MAG correction is applied to rcCommand) 100dps to 1100dps max yaw rate
140 AngleRate
= (float)((controlRateConfig
->yawRate
+ 10) * rcCommand
[YAW
]) / 50.0f
;
142 // calculate error and limit the angle to the max inclination
144 errorAngle
= (constrain(rcCommand
[axis
] + GPS_angle
[axis
], -((int) max_angle_inclination
),
145 +max_angle_inclination
) - inclination
.raw
[axis
] + angleTrim
->raw
[axis
]) / 10.0f
; // 16 bits is ok here
147 errorAngle
= (constrain(rcCommand
[axis
], -((int) max_angle_inclination
),
148 +max_angle_inclination
) - inclination
.raw
[axis
] + angleTrim
->raw
[axis
]) / 10.0f
; // 16 bits is ok here
152 if (shouldAutotune()) {
153 errorAngle
= autotune(rcAliasToAngleIndexMap
[axis
], &inclination
, errorAngle
);
157 if (FLIGHT_MODE(ANGLE_MODE
)) {
158 // it's the ANGLE mode - control is angle based, so control loop is needed
159 AngleRate
= errorAngle
* pidProfile
->A_level
;
161 //control is GYRO based (ACRO and HORIZON - direct sticks control is applied to rate PID
162 AngleRate
= (float)((controlRateConfig
->rollPitchRate
+ 20) * rcCommand
[axis
]) / 50.0f
; // 200dps to 1200dps max yaw rate
163 if (FLIGHT_MODE(HORIZON_MODE
)) {
164 // mix up angle error to desired AngleRate to add a little auto-level feel
165 AngleRate
+= errorAngle
* pidProfile
->H_level
* horizonLevelStrength
;
170 gyroRate
= gyroData
[axis
] * gyro
.scale
; // gyro output scaled to dps
172 // --------low-level gyro-based PID. ----------
173 // Used in stand-alone mode for ACRO, controlled by higher level regulators in other modes
174 // -----calculate scaled error.AngleRates
175 // multiplication of rcCommand corresponds to changing the sticks scaling here
176 RateError
= AngleRate
- gyroRate
;
178 // -----calculate P component
179 PTerm
= RateError
* pidProfile
->P_f
[axis
];
180 // -----calculate I component
181 errorGyroIf
[axis
] = constrainf(errorGyroIf
[axis
] + RateError
* dT
* pidProfile
->I_f
[axis
], -250.0f
, 250.0f
);
183 // limit maximum integrator value to prevent WindUp - accumulating extreme values when system is saturated.
184 // I coefficient (I8) moved before integration to make limiting independent from PID settings
185 ITerm
= errorGyroIf
[axis
];
187 //-----calculate D-term
188 delta
= gyroRate
- lastGyroRate
[axis
]; // 16 bits is ok here, the dif between 2 consecutive gyro reads is limited to 800
189 lastGyroRate
[axis
] = gyroRate
;
191 // Correct difference by cycle time. Cycle time is jittery (can be different 2 times), so calculated difference
192 // would be scaled by different dt each time. Division by dT fixes that.
193 delta
*= (1.0f
/ dT
);
194 // add moving average here to reduce noise
195 deltaSum
= delta1
[axis
] + delta2
[axis
] + delta
;
196 delta2
[axis
] = delta1
[axis
];
197 delta1
[axis
] = delta
;
198 DTerm
= constrainf((deltaSum
/ 3.0f
) * pidProfile
->D_f
[axis
], -300.0f
, 300.0f
);
200 // -----calculate total PID output
201 axisPID
[axis
] = constrain(lrintf(PTerm
+ ITerm
- DTerm
), -1000, 1000);
204 axisPID_P
[axis
] = PTerm
;
205 axisPID_I
[axis
] = ITerm
;
206 axisPID_D
[axis
] = -DTerm
;
211 static void pidMultiWii(pidProfile_t
*pidProfile
, controlRateConfig_t
*controlRateConfig
,
212 uint16_t max_angle_inclination
, rollAndPitchTrims_t
*angleTrim
, rxConfig_t
*rxConfig
)
217 int32_t error
, errorAngle
;
218 int32_t PTerm
, ITerm
, PTermACC
= 0, ITermACC
= 0, PTermGYRO
= 0, ITermGYRO
= 0, DTerm
;
219 static int16_t lastGyro
[3] = { 0, 0, 0 };
220 static int32_t delta1
[3], delta2
[3];
224 UNUSED(controlRateConfig
);
226 // **** PITCH & ROLL & YAW PID ****
227 prop
= MIN(MAX(ABS(rcCommand
[PITCH
]), ABS(rcCommand
[ROLL
])), 500); // range [0;500]
229 for (axis
= 0; axis
< 3; axis
++) {
230 if ((FLIGHT_MODE(ANGLE_MODE
) || FLIGHT_MODE(HORIZON_MODE
)) && (axis
== FD_ROLL
|| axis
== FD_PITCH
)) { // MODE relying on ACC
231 // observe max inclination
233 errorAngle
= constrain(2 * rcCommand
[axis
] + GPS_angle
[axis
], -((int) max_angle_inclination
),
234 +max_angle_inclination
) - inclination
.raw
[axis
] + angleTrim
->raw
[axis
];
236 errorAngle
= constrain(2 * rcCommand
[axis
], -((int) max_angle_inclination
),
237 +max_angle_inclination
) - inclination
.raw
[axis
] + angleTrim
->raw
[axis
];
241 if (shouldAutotune()) {
242 errorAngle
= DEGREES_TO_DECIDEGREES(autotune(rcAliasToAngleIndexMap
[axis
], &inclination
, DECIDEGREES_TO_DEGREES(errorAngle
)));
246 PTermACC
= errorAngle
* pidProfile
->P8
[PIDLEVEL
] / 100; // 32 bits is needed for calculation: errorAngle*P8[PIDLEVEL] could exceed 32768 16 bits is ok for result
247 PTermACC
= constrain(PTermACC
, -pidProfile
->D8
[PIDLEVEL
] * 5, +pidProfile
->D8
[PIDLEVEL
] * 5);
249 errorAngleI
[axis
] = constrain(errorAngleI
[axis
] + errorAngle
, -10000, +10000); // WindUp
250 ITermACC
= (errorAngleI
[axis
] * pidProfile
->I8
[PIDLEVEL
]) >> 12;
252 if (!FLIGHT_MODE(ANGLE_MODE
) || FLIGHT_MODE(HORIZON_MODE
) || axis
== FD_YAW
) { // MODE relying on GYRO or YAW axis
253 error
= (int32_t) rcCommand
[axis
] * 10 * 8 / pidProfile
->P8
[axis
];
254 error
-= gyroData
[axis
] / 4;
256 PTermGYRO
= rcCommand
[axis
];
258 errorGyroI
[axis
] = constrain(errorGyroI
[axis
] + error
, -16000, +16000); // WindUp
259 if ((ABS(gyroData
[axis
]) > (640 * 4)) || (axis
== FD_YAW
&& ABS(rcCommand
[axis
]) > 100))
260 errorGyroI
[axis
] = 0;
262 ITermGYRO
= (errorGyroI
[axis
] / 125 * pidProfile
->I8
[axis
]) / 64;
264 if (FLIGHT_MODE(HORIZON_MODE
) && (axis
== FD_ROLL
|| axis
== FD_PITCH
)) {
265 PTerm
= (PTermACC
* (500 - prop
) + PTermGYRO
* prop
) / 500;
266 ITerm
= (ITermACC
* (500 - prop
) + ITermGYRO
* prop
) / 500;
268 if (FLIGHT_MODE(ANGLE_MODE
) && (axis
== FD_ROLL
|| axis
== FD_PITCH
)) {
277 PTerm
-= ((int32_t)gyroData
[axis
] / 4) * dynP8
[axis
] / 10 / 8; // 32 bits is needed for calculation
278 delta
= (gyroData
[axis
] - lastGyro
[axis
]) / 4;
279 lastGyro
[axis
] = gyroData
[axis
];
280 deltaSum
= delta1
[axis
] + delta2
[axis
] + delta
;
281 delta2
[axis
] = delta1
[axis
];
282 delta1
[axis
] = delta
;
283 DTerm
= (deltaSum
* dynD8
[axis
]) / 32;
284 axisPID
[axis
] = PTerm
+ ITerm
- DTerm
;
287 axisPID_P
[axis
] = PTerm
;
288 axisPID_I
[axis
] = ITerm
;
289 axisPID_D
[axis
] = -DTerm
;
294 #define GYRO_P_MAX 300
295 #define GYRO_I_MAX 256
297 static void pidMultiWii23(pidProfile_t
*pidProfile
, controlRateConfig_t
*controlRateConfig
, uint16_t max_angle_inclination
,
298 rollAndPitchTrims_t
*angleTrim
, rxConfig_t
*rxConfig
)
303 int32_t rc
, error
, errorAngle
;
304 int32_t PTerm
, ITerm
, PTermACC
, ITermACC
, DTerm
;
305 static int16_t lastGyro
[2] = { 0, 0 };
306 static int32_t delta1
[2] = { 0, 0 }, delta2
[2] = { 0, 0 };
309 if (FLIGHT_MODE(HORIZON_MODE
)) prop
= MIN(MAX(ABS(rcCommand
[PITCH
]), ABS(rcCommand
[ROLL
])), 512);
312 for (axis
= 0; axis
< 2; axis
++) {
313 rc
= rcCommand
[axis
] << 1;
314 error
= rc
- (gyroData
[axis
] / 4);
315 errorGyroI
[axis
] = constrain(errorGyroI
[axis
] + error
, -16000, +16000); // WindUp 16 bits is ok here
316 if (ABS(gyroData
[axis
]) > (640 * 4)) errorGyroI
[axis
] = 0;
318 ITerm
= (errorGyroI
[axis
] >> 7) * pidProfile
->I8
[axis
] >> 6; // 16 bits is ok here 16000/125 = 128 ; 128*250 = 32000
320 PTerm
= (int32_t)rc
* pidProfile
->P8
[axis
] >> 6;
322 if (FLIGHT_MODE(ANGLE_MODE
) || FLIGHT_MODE(HORIZON_MODE
)) { // axis relying on ACC
323 // 50 degrees max inclination
325 errorAngle
= constrain(2 * rcCommand
[axis
] + GPS_angle
[axis
], -((int) max_angle_inclination
),
326 +max_angle_inclination
) - inclination
.raw
[axis
] + angleTrim
->raw
[axis
];
328 errorAngle
= constrain(2 * rcCommand
[axis
], -((int) max_angle_inclination
),
329 +max_angle_inclination
) - inclination
.raw
[axis
] + angleTrim
->raw
[axis
];
333 if (shouldAutotune()) {
334 errorAngle
= DEGREES_TO_DECIDEGREES(autotune(rcAliasToAngleIndexMap
[axis
], &inclination
, DECIDEGREES_TO_DEGREES(errorAngle
)));
338 errorAngleI
[axis
] = constrain(errorAngleI
[axis
] + errorAngle
, -10000, +10000); // WindUp //16 bits is ok here
340 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
342 int16_t limit
= pidProfile
->D8
[PIDLEVEL
] * 5;
343 PTermACC
= constrain(PTermACC
, -limit
, +limit
);
345 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
347 ITerm
= ITermACC
+ ((ITerm
- ITermACC
) * prop
>> 9);
348 PTerm
= PTermACC
+ ((PTerm
- PTermACC
) * prop
>> 9);
351 PTerm
-= ((int32_t)(gyroData
[axis
] / 4) * dynP8
[axis
]) >> 6; // 32 bits is needed for calculation
353 delta
= (gyroData
[axis
] - lastGyro
[axis
]) / 4; // 16 bits is ok here, the dif between 2 consecutive gyro reads is limited to 800
354 lastGyro
[axis
] = gyroData
[axis
];
355 DTerm
= delta1
[axis
] + delta2
[axis
] + delta
;
356 delta2
[axis
] = delta1
[axis
];
357 delta1
[axis
] = delta
;
359 DTerm
= ((int32_t)DTerm
* dynD8
[axis
]) >> 5; // 32 bits is needed for calculation
361 axisPID
[axis
] = PTerm
+ ITerm
- DTerm
;
364 axisPID_P
[axis
] = PTerm
;
365 axisPID_I
[axis
] = ITerm
;
366 axisPID_D
[axis
] = -DTerm
;
371 rc
= (int32_t)rcCommand
[FD_YAW
] * (2 * controlRateConfig
->yawRate
+ 30) >> 5;
373 error
= rc
- gyroData
[FD_YAW
];
375 error
= rc
- (gyroData
[FD_YAW
] / 4);
377 errorGyroI
[FD_YAW
] += (int32_t)error
* pidProfile
->I8
[FD_YAW
];
378 errorGyroI
[FD_YAW
] = constrain(errorGyroI
[FD_YAW
], 2 - ((int32_t)1 << 28), -2 + ((int32_t)1 << 28));
379 if (ABS(rc
) > 50) errorGyroI
[FD_YAW
] = 0;
381 PTerm
= (int32_t)error
* pidProfile
->P8
[FD_YAW
] >> 6;
383 // Constrain YAW by D value if not servo driven in that case servolimits apply
385 int16_t limit
= GYRO_P_MAX
- pidProfile
->D8
[FD_YAW
];
386 PTerm
= constrain(PTerm
, -limit
, +limit
);
389 ITerm
= constrain((int16_t)(errorGyroI
[FD_YAW
] >> 13), -GYRO_I_MAX
, +GYRO_I_MAX
);
391 axisPID
[FD_YAW
] = PTerm
+ ITerm
;
394 axisPID_P
[FD_YAW
] = PTerm
;
395 axisPID_I
[FD_YAW
] = ITerm
;
396 axisPID_D
[FD_YAW
] = 0;
400 static void pidMultiWiiHybrid(pidProfile_t
*pidProfile
, controlRateConfig_t
*controlRateConfig
,
401 uint16_t max_angle_inclination
, rollAndPitchTrims_t
*angleTrim
, rxConfig_t
*rxConfig
)
406 int32_t rc
, error
, errorAngle
;
407 int32_t PTerm
, ITerm
, PTermACC
= 0, ITermACC
= 0, PTermGYRO
= 0, ITermGYRO
= 0, DTerm
;
408 static int16_t lastGyro
[2] = { 0, 0 };
409 static int32_t delta1
[2] = { 0, 0 }, delta2
[2] = { 0, 0 };
413 UNUSED(controlRateConfig
);
415 // **** PITCH & ROLL ****
416 prop
= MIN(MAX(ABS(rcCommand
[PITCH
]), ABS(rcCommand
[ROLL
])), 500); // range [0;500]
418 for (axis
= 0; axis
< 2; axis
++) {
419 if ((FLIGHT_MODE(ANGLE_MODE
) || FLIGHT_MODE(HORIZON_MODE
))) { // MODE relying on ACC
420 // observe max inclination
422 errorAngle
= constrain(2 * rcCommand
[axis
] + GPS_angle
[axis
], -((int) max_angle_inclination
),
423 +max_angle_inclination
) - inclination
.raw
[axis
] + angleTrim
->raw
[axis
];
425 errorAngle
= constrain(2 * rcCommand
[axis
], -((int) max_angle_inclination
),
426 +max_angle_inclination
) - inclination
.raw
[axis
] + angleTrim
->raw
[axis
];
430 if (shouldAutotune()) {
431 errorAngle
= DEGREES_TO_DECIDEGREES(autotune(rcAliasToAngleIndexMap
[axis
], &inclination
, DECIDEGREES_TO_DEGREES(errorAngle
)));
435 PTermACC
= errorAngle
* pidProfile
->P8
[PIDLEVEL
] / 100; // 32 bits is needed for calculation: errorAngle*P8[PIDLEVEL] could exceed 32768 16 bits is ok for result
436 PTermACC
= constrain(PTermACC
, -pidProfile
->D8
[PIDLEVEL
] * 5, +pidProfile
->D8
[PIDLEVEL
] * 5);
438 errorAngleI
[axis
] = constrain(errorAngleI
[axis
] + errorAngle
, -10000, +10000); // WindUp
439 ITermACC
= (errorAngleI
[axis
] * pidProfile
->I8
[PIDLEVEL
]) >> 12;
441 if (!FLIGHT_MODE(ANGLE_MODE
) || FLIGHT_MODE(HORIZON_MODE
)) { // MODE relying on GYRO
442 error
= (int32_t) rcCommand
[axis
] * 10 * 8 / pidProfile
->P8
[axis
];
443 error
-= gyroData
[axis
] / 4;
445 PTermGYRO
= rcCommand
[axis
];
447 errorGyroI
[axis
] = constrain(errorGyroI
[axis
] + error
, -16000, +16000); // WindUp
448 if (ABS(gyroData
[axis
]) > (640 * 4))
449 errorGyroI
[axis
] = 0;
451 ITermGYRO
= (errorGyroI
[axis
] / 125 * pidProfile
->I8
[axis
]) / 64;
453 if (FLIGHT_MODE(HORIZON_MODE
)) {
454 PTerm
= (PTermACC
* (500 - prop
) + PTermGYRO
* prop
) / 500;
455 ITerm
= (ITermACC
* (500 - prop
) + ITermGYRO
* prop
) / 500;
457 if (FLIGHT_MODE(ANGLE_MODE
)) {
466 PTerm
-= ((int32_t)gyroData
[axis
] / 4) * dynP8
[axis
] / 10 / 8; // 32 bits is needed for calculation
467 delta
= (gyroData
[axis
] - lastGyro
[axis
]) / 4;
468 lastGyro
[axis
] = gyroData
[axis
];
469 deltaSum
= delta1
[axis
] + delta2
[axis
] + delta
;
470 delta2
[axis
] = delta1
[axis
];
471 delta1
[axis
] = delta
;
472 DTerm
= (deltaSum
* dynD8
[axis
]) / 32;
473 axisPID
[axis
] = PTerm
+ ITerm
- DTerm
;
476 axisPID_P
[axis
] = PTerm
;
477 axisPID_I
[axis
] = ITerm
;
478 axisPID_D
[axis
] = -DTerm
;
482 rc
= (int32_t)rcCommand
[FD_YAW
] * (2 * controlRateConfig
->yawRate
+ 30) >> 5;
484 error
= rc
- gyroData
[FD_YAW
];
486 error
= rc
- (gyroData
[FD_YAW
] / 4);
488 errorGyroI
[FD_YAW
] += (int32_t)error
* pidProfile
->I8
[FD_YAW
];
489 errorGyroI
[FD_YAW
] = constrain(errorGyroI
[FD_YAW
], 2 - ((int32_t)1 << 28), -2 + ((int32_t)1 << 28));
490 if (ABS(rc
) > 50) errorGyroI
[FD_YAW
] = 0;
492 PTerm
= (int32_t)error
* pidProfile
->P8
[FD_YAW
] >> 6;
494 // Constrain YAW by D value if not servo driven in that case servolimits apply
496 int16_t limit
= GYRO_P_MAX
- pidProfile
->D8
[FD_YAW
];
497 PTerm
= constrain(PTerm
, -limit
, +limit
);
500 ITerm
= constrain((int16_t)(errorGyroI
[FD_YAW
] >> 13), -GYRO_I_MAX
, +GYRO_I_MAX
);
502 axisPID
[FD_YAW
] = PTerm
+ ITerm
;
506 axisPID_P
[FD_YAW
] = PTerm
;
507 axisPID_I
[FD_YAW
] = ITerm
;
508 axisPID_D
[FD_YAW
] = 0;
512 #define RCconstPI 0.159154943092f // 0.5f / M_PI;
513 #define MAIN_CUT_HZ 12.0f // (default 12Hz, Range 1-50Hz)
514 #define OLD_YAW 0 // [0/1] 0 = multiwii 2.3 yaw, 1 = older yaw.
516 static void pidHarakiri(pidProfile_t
*pidProfile
, controlRateConfig_t
*controlRateConfig
, uint16_t max_angle_inclination
,
517 rollAndPitchTrims_t
*angleTrim
, rxConfig_t
*rxConfig
)
521 float delta
, RCfactor
, rcCommandAxis
, MainDptCut
;
522 float PTerm
= 0, ITerm
= 0, DTerm
= 0, PTermACC
= 0, ITermACC
= 0, ITermGYRO
= 0, error
= 0, prop
= 0;
523 static float lastGyro
[2] = {0, 0}, lastDTerm
[2] = {0, 0};
527 float ACCDeltaTimeINS
= 0;
528 float FLOATcycleTime
= 0;
530 // MainDptCut = RCconstPI / (float)cfg.maincuthz; // Initialize Cut off frequencies for mainpid D
531 MainDptCut
= RCconstPI
/ MAIN_CUT_HZ
; // maincuthz (default 12Hz, Range 1-50Hz), hardcoded for now
532 FLOATcycleTime
= (float)constrain(cycleTime
, 1, 100000); // 1us - 100ms
533 ACCDeltaTimeINS
= FLOATcycleTime
* 0.000001f
; // ACCDeltaTimeINS is in seconds now
534 RCfactor
= ACCDeltaTimeINS
/ (MainDptCut
+ ACCDeltaTimeINS
); // used for pt1 element
536 if (FLIGHT_MODE(HORIZON_MODE
)) {
537 prop
= (float)MIN(MAX(ABS(rcCommand
[PITCH
]), ABS(rcCommand
[ROLL
])), 450) / 450.0f
;
540 for (axis
= 0; axis
< 2; axis
++) {
541 rcCommandAxis
= (float)rcCommand
[axis
]; // Calculate common values for pid controllers
542 if (FLIGHT_MODE(ANGLE_MODE
) || FLIGHT_MODE(HORIZON_MODE
)) {
544 error
= constrain(2.0f
* rcCommandAxis
+ GPS_angle
[axis
], -((int) max_angle_inclination
), +max_angle_inclination
) - inclination
.raw
[axis
] + angleTrim
->raw
[axis
];
546 error
= constrain(2.0f
* rcCommandAxis
, -((int) max_angle_inclination
), +max_angle_inclination
) - inclination
.raw
[axis
] + angleTrim
->raw
[axis
];
550 if (shouldAutotune()) {
551 error
= DEGREES_TO_DECIDEGREES(autotune(rcAliasToAngleIndexMap
[axis
], &inclination
, DECIDEGREES_TO_DEGREES(error
)));
554 PTermACC
= error
* (float)pidProfile
->P8
[PIDLEVEL
] * 0.008f
;
555 tmp0flt
= (float)pidProfile
->D8
[PIDLEVEL
] * 5.0f
;
556 PTermACC
= constrain(PTermACC
, -tmp0flt
, +tmp0flt
);
557 errorAngleI
[axis
] = constrain(errorAngleI
[axis
] + error
* ACCDeltaTimeINS
, -30.0f
, +30.0f
);
558 ITermACC
= errorAngleI
[axis
] * (float)pidProfile
->I8
[PIDLEVEL
] * 0.08f
;
561 if (!FLIGHT_MODE(ANGLE_MODE
)) {
562 if (ABS((int16_t)gyroData
[axis
]) > 2560) {
563 errorGyroI
[axis
] = 0.0f
;
565 error
= (rcCommandAxis
* 320.0f
/ (float)pidProfile
->P8
[axis
]) - gyroData
[axis
];
566 errorGyroI
[axis
] = constrain(errorGyroI
[axis
] + error
* ACCDeltaTimeINS
, -192.0f
, +192.0f
);
569 ITermGYRO
= errorGyroI
[axis
] * (float)pidProfile
->I8
[axis
] * 0.01f
;
571 if (FLIGHT_MODE(HORIZON_MODE
)) {
572 PTerm
= PTermACC
+ prop
* (rcCommandAxis
- PTermACC
);
573 ITerm
= ITermACC
+ prop
* (ITermGYRO
- ITermACC
);
575 PTerm
= rcCommandAxis
;
583 PTerm
-= gyroData
[axis
] * dynP8
[axis
] * 0.003f
;
584 delta
= (gyroData
[axis
] - lastGyro
[axis
]) / ACCDeltaTimeINS
;
586 lastGyro
[axis
] = gyroData
[axis
];
587 lastDTerm
[axis
] += RCfactor
* (delta
- lastDTerm
[axis
]);
588 DTerm
= lastDTerm
[axis
] * dynD8
[axis
] * 0.00007f
;
590 axisPID
[axis
] = lrintf(PTerm
+ ITerm
- DTerm
); // Round up result.
593 axisPID_P
[axis
] = PTerm
;
594 axisPID_I
[axis
] = ITerm
;
595 axisPID_D
[axis
] = -DTerm
;
599 tmp0flt
= (int32_t)FLOATcycleTime
& (int32_t)~3; // Filter last 2 bit jitter
602 if (OLD_YAW
) { // [0/1] 0 = multiwii 2.3 yaw, 1 = older yaw. hardcoded for now
603 PTerm
= ((int32_t)pidProfile
->P8
[FD_YAW
] * (100 - (int32_t)controlRateConfig
->yawRate
* (int32_t)ABS(rcCommand
[FD_YAW
]) / 500)) / 100;
604 tmp0
= lrintf(gyroData
[FD_YAW
] * 0.25f
);
605 PTerm
= rcCommand
[FD_YAW
] - tmp0
* PTerm
/ 80;
606 if ((ABS(tmp0
) > 640) || (ABS(rcCommand
[FD_YAW
]) > 100)) {
607 errorGyroI
[FD_YAW
] = 0;
609 error
= ((int32_t)rcCommand
[FD_YAW
] * 80 / (int32_t)pidProfile
->P8
[FD_YAW
]) - tmp0
;
610 errorGyroI
[FD_YAW
] = constrain(errorGyroI
[FD_YAW
] + (int32_t)(error
* tmp0flt
), -16000, +16000); // WindUp
611 ITerm
= (errorGyroI
[FD_YAW
] / 125 * pidProfile
->I8
[FD_YAW
]) >> 6;
614 tmp0
= ((int32_t)rcCommand
[FD_YAW
] * (((int32_t)controlRateConfig
->yawRate
<< 1) + 40)) >> 5;
615 error
= tmp0
- lrintf(gyroData
[FD_YAW
] * 0.25f
); // Less Gyrojitter works actually better
617 if (ABS(tmp0
) > 50) {
618 errorGyroI
[FD_YAW
] = 0;
620 errorGyroI
[FD_YAW
] = constrain(errorGyroI
[FD_YAW
] + (int32_t)(error
* (float)pidProfile
->I8
[FD_YAW
] * tmp0flt
), -268435454, +268435454);
623 ITerm
= constrain(errorGyroI
[FD_YAW
] >> 13, -GYRO_I_MAX
, +GYRO_I_MAX
);
624 PTerm
= ((int32_t)error
* (int32_t)pidProfile
->P8
[FD_YAW
]) >> 6;
626 if (motorCount
>= 4) { // Constrain FD_YAW by D value if not servo driven in that case servolimits apply
628 if (pidProfile
->D8
[FD_YAW
]) tmp0
-= (int32_t)pidProfile
->D8
[FD_YAW
];
629 PTerm
= constrain(PTerm
, -tmp0
, tmp0
);
632 axisPID
[FD_YAW
] = PTerm
+ ITerm
;
633 axisPID
[FD_YAW
] = lrintf(axisPID
[FD_YAW
]); // Round up result.
636 axisPID_P
[FD_YAW
] = PTerm
;
637 axisPID_I
[FD_YAW
] = ITerm
;
638 axisPID_D
[FD_YAW
] = 0;
642 static void pidRewrite(pidProfile_t
*pidProfile
, controlRateConfig_t
*controlRateConfig
, uint16_t max_angle_inclination
,
643 rollAndPitchTrims_t
*angleTrim
, rxConfig_t
*rxConfig
)
649 int32_t delta
, deltaSum
;
650 static int32_t delta1
[3], delta2
[3];
651 int32_t PTerm
, ITerm
, DTerm
;
652 static int32_t lastError
[3] = { 0, 0, 0 };
653 int32_t AngleRateTmp
, RateError
;
655 // ----------PID controller----------
656 for (axis
= 0; axis
< 3; axis
++) {
657 // -----Get the desired angle rate depending on flight mode
658 if (axis
== FD_YAW
) { // YAW is always gyro-controlled (MAG correction is applied to rcCommand)
659 AngleRateTmp
= (((int32_t)(controlRateConfig
->yawRate
+ 27) * rcCommand
[YAW
]) >> 5);
661 // calculate error and limit the angle to max configured inclination
663 errorAngle
= constrain(2 * rcCommand
[axis
] + GPS_angle
[axis
], -((int) max_angle_inclination
),
664 +max_angle_inclination
) - inclination
.raw
[axis
] + angleTrim
->raw
[axis
]; // 16 bits is ok here
666 errorAngle
= constrain(2 * rcCommand
[axis
], -((int) max_angle_inclination
),
667 +max_angle_inclination
) - inclination
.raw
[axis
] + angleTrim
->raw
[axis
]; // 16 bits is ok here
671 if (shouldAutotune()) {
672 errorAngle
= DEGREES_TO_DECIDEGREES(autotune(rcAliasToAngleIndexMap
[axis
], &inclination
, DECIDEGREES_TO_DEGREES(errorAngle
)));
676 if (!FLIGHT_MODE(ANGLE_MODE
)) { //control is GYRO based (ACRO and HORIZON - direct sticks control is applied to rate PID
677 AngleRateTmp
= ((int32_t)(controlRateConfig
->rollPitchRate
+ 27) * rcCommand
[axis
]) >> 4;
678 if (FLIGHT_MODE(HORIZON_MODE
)) {
679 // mix up angle error to desired AngleRateTmp to add a little auto-level feel
680 AngleRateTmp
+= (errorAngle
* pidProfile
->I8
[PIDLEVEL
]) >> 8;
682 } else { // it's the ANGLE mode - control is angle based, so control loop is needed
683 AngleRateTmp
= (errorAngle
* pidProfile
->P8
[PIDLEVEL
]) >> 4;
687 // --------low-level gyro-based PID. ----------
688 // Used in stand-alone mode for ACRO, controlled by higher level regulators in other modes
689 // -----calculate scaled error.AngleRates
690 // multiplication of rcCommand corresponds to changing the sticks scaling here
691 RateError
= AngleRateTmp
- (gyroData
[axis
] / 4);
693 // -----calculate P component
694 PTerm
= (RateError
* pidProfile
->P8
[axis
]) >> 7;
695 // -----calculate I component
696 // there should be no division before accumulating the error to integrator, because the precision would be reduced.
697 // Precision is critical, as I prevents from long-time drift. Thus, 32 bits integrator is used.
698 // Time correction (to avoid different I scaling for different builds based on average cycle time)
699 // is normalized to cycle time = 2048.
700 errorGyroI
[axis
] = errorGyroI
[axis
] + ((RateError
* cycleTime
) >> 11) * pidProfile
->I8
[axis
];
702 // limit maximum integrator value to prevent WindUp - accumulating extreme values when system is saturated.
703 // I coefficient (I8) moved before integration to make limiting independent from PID settings
704 errorGyroI
[axis
] = constrain(errorGyroI
[axis
], (int32_t) - GYRO_I_MAX
<< 13, (int32_t) + GYRO_I_MAX
<< 13);
705 ITerm
= errorGyroI
[axis
] >> 13;
707 //-----calculate D-term
708 delta
= RateError
- lastError
[axis
]; // 16 bits is ok here, the dif between 2 consecutive gyro reads is limited to 800
709 lastError
[axis
] = RateError
;
711 // Correct difference by cycle time. Cycle time is jittery (can be different 2 times), so calculated difference
712 // would be scaled by different dt each time. Division by dT fixes that.
713 delta
= (delta
* ((uint16_t) 0xFFFF / (cycleTime
>> 4))) >> 6;
714 // add moving average here to reduce noise
715 deltaSum
= delta1
[axis
] + delta2
[axis
] + delta
;
716 delta2
[axis
] = delta1
[axis
];
717 delta1
[axis
] = delta
;
718 DTerm
= (deltaSum
* pidProfile
->D8
[axis
]) >> 8;
720 // -----calculate total PID output
721 axisPID
[axis
] = PTerm
+ ITerm
+ DTerm
;
724 axisPID_P
[axis
] = PTerm
;
725 axisPID_I
[axis
] = ITerm
;
726 axisPID_D
[axis
] = DTerm
;
731 void setPIDController(int type
)
736 pid_controller
= pidMultiWii
;
739 pid_controller
= pidRewrite
;
742 pid_controller
= pidLuxFloat
;
745 pid_controller
= pidMultiWii23
;
748 pid_controller
= pidMultiWiiHybrid
;
751 pid_controller
= pidHarakiri
;