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/>.
27 #include "common/axis.h"
28 #include "common/maths.h"
30 #include "drivers/system.h"
31 #include "drivers/sensor.h"
32 #include "drivers/accgyro.h"
34 #include "sensors/sensors.h"
35 #include "sensors/acceleration.h"
37 #include "flight/pid.h"
38 #include "flight/imu.h"
40 #include "config/config.h"
41 #include "blackbox/blackbox.h"
43 extern int16_t debug
[4];
47 * Brad Quick - initial implementation in BradWii
48 * Dominic Clifton - baseflight port & cleanup.
50 * Autotune in BradWii thread here: http://www.rcgroups.com/forums/showthread.php?t=1922423
52 * We start with two input parameters. The first is our target angle. By default it's 20 degrees, so we will bank to 20 degrees,
53 * see how the system reacts, then bank to -20 degrees and evaluate again. We repeat this over and over. The second input is
54 * how much oscillation we can tolerate. This can range from 2 degrees to 5 or more degrees. This defaults to 4 degrees. The
55 * higher this value is, the more agressive the result of the tuning will be.
57 * First, we turn the I gain down to zero so that we don't have to try to figure out how much overshoot is caused by the I term
60 * Then, we move to the target of 20 degrees and analyze the results. Our goal is to have no overshoot and to keep the bounce
61 * back within the 4 degrees. By working to get zero overshoot, we can isolate the effects of the P and D terms. If we don't
62 * overshoot, then the P term never works in the opposite direction, so we know that any bounce we get is caused by the D term.
64 * If we overshoot the target 20 degrees, then we know our P term is too high or our D term is too low. We can determine
65 * which one to change by looking at how much bounce back (or the amplitude of the oscillation) we get. If it bounces back
66 * more than the 4 degrees, then our D is already too high, so we can't increase it, so instead we decrease P.
68 * If we undershoot, then either our P is too low or our D is too high. Again, we can determine which to change by looking at
69 * how much bounce we get.
71 * Once we have the P and D terms set, we then set the I term by repeating the same test above and measuring the overshoot.
72 * If our maximum oscillation is set to 4 degrees, then we keep increasing the I until we get an overshoot of 2 degrees, so
73 * that our oscillations are now centered around our target (in theory).
75 * In the BradWii software, it alternates between doing the P and D step and doing the I step so you can quit whenever you
76 * want without having to tell it specifically to do the I term. The sequence is actually P&D, P&D, I, P&D, P&D, I...
78 * Note: The 4 degrees mentioned above is the value of AUTOTUNE_MAX_OSCILLATION_ANGLE. In the BradWii code at the time of writing
79 * the default value was 1.0f instead of 4.0f.
81 * To adjust how aggressive the tuning is, adjust the AUTOTUNE_MAX_OSCILLATION_ANGLE value. A larger value will result in more
82 * aggressive tuning. A lower value will result in softer tuning. It will rock back and forth between -AUTOTUNE_TARGET_ANGLE
83 * and AUTOTUNE_TARGET_ANGLE degrees
84 * AUTOTUNE_D_MULTIPLIER is a multiplier that puts in a little extra D when autotuning is done. This helps damp the wobbles
85 * after a quick angle change.
86 * Always autotune on a full battery.
89 #define AUTOTUNE_MAX_OSCILLATION_ANGLE 2.0f
90 #define AUTOTUNE_TARGET_ANGLE 20.0f
91 #define AUTOTUNE_D_MULTIPLIER 1.2f
92 #define AUTOTUNE_SETTLING_DELAY_MS 250 // 1/4 of a second.
94 #define AUTOTUNE_INCREASE_MULTIPLIER 1.03f
95 #define AUTOTUNE_DECREASE_MULTIPLIER 0.97f
97 #define AUTOTUNE_MINIMUM_I_VALUE 0.001f
99 #define YAW_GAIN_MULTIPLIER 2.0f
106 PHASE_SAVE_OR_RESTORE_PIDS
,
115 static const pidIndex_e angleIndexToPidIndexMap
[] = {
120 #define AUTOTUNE_PHASE_MAX PHASE_SAVE_OR_RESTORE_PIDS
121 #define AUTOTUNE_PHASE_COUNT (AUTOTUNE_PHASE_MAX + 1)
123 #define FIRST_TUNE_PHASE PHASE_TUNE_ROLL
125 static pidProfile_t
*pidProfile
;
126 static pidProfile_t pidBackup
;
127 static uint8_t pidController
;
128 static uint8_t pidIndex
;
130 static autotuneCycle_e cycle
;
131 static uint32_t timeoutAt
;
132 static angle_index_t autoTuneAngleIndex
;
133 static autotunePhase_e phase
= PHASE_IDLE
;
134 static autotunePhase_e nextPhase
= FIRST_TUNE_PHASE
;
136 static float targetAngle
= 0;
137 static float targetAngleAtPeak
;
138 static float firstPeakAngle
, secondPeakAngle
; // in degrees
140 typedef struct fp_pid
{
148 // These are used to convert between multiwii integer values to the float pid values used by the autotuner.
149 #define MULTIWII_P_MULTIPLIER 10.0f // e.g 0.4 * 10 = 40
150 #define MULTIWII_I_MULTIPLIER 1000.0f // e.g 0.030 * 1000 = 30
151 // Note there is no D multiplier since D values are stored and used AS-IS
153 bool isAutotuneIdle(void)
155 return phase
== PHASE_IDLE
;
160 static void autotuneLogCycleStart()
162 if (feature(FEATURE_BLACKBOX
)) {
163 flightLogEvent_autotuneCycleStart_t eventData
;
165 eventData
.phase
= phase
;
166 eventData
.cycle
= cycle
;
167 eventData
.p
= pid
.p
* MULTIWII_P_MULTIPLIER
;
168 eventData
.i
= pid
.i
* MULTIWII_I_MULTIPLIER
;
170 eventData
.rising
= rising
? 1 : 0;
172 blackboxLogEvent(FLIGHT_LOG_EVENT_AUTOTUNE_CYCLE_START
, (flightLogEventData_t
*)&eventData
);
176 static void autotuneLogAngleTargets(float currentAngle
)
178 if (feature(FEATURE_BLACKBOX
)) {
179 flightLogEvent_autotuneTargets_t eventData
;
181 // targetAngle is always just -AUTOTUNE_TARGET_ANGLE or +AUTOTUNE_TARGET_ANGLE so no need for float precision:
182 eventData
.targetAngle
= (int) targetAngle
;
183 // and targetAngleAtPeak is set to targetAngle so it has the same small precision requirement:
184 eventData
.targetAngleAtPeak
= (int) targetAngleAtPeak
;
186 // currentAngle is integer decidegrees divided by 10, so just reverse that process to get an integer again:
187 eventData
.currentAngle
= round(currentAngle
* 10);
188 // the peak angles are only ever set to currentAngle, so they get the same treatment:
189 eventData
.firstPeakAngle
= round(firstPeakAngle
* 10);
190 eventData
.secondPeakAngle
= round(secondPeakAngle
* 10);
192 blackboxLogEvent(FLIGHT_LOG_EVENT_AUTOTUNE_TARGETS
, (flightLogEventData_t
*)&eventData
);
198 static void startNewCycle(void)
201 firstPeakAngle
= secondPeakAngle
= 0;
204 autotuneLogCycleStart();
208 static void updatePidIndex(void)
210 pidIndex
= angleIndexToPidIndexMap
[autoTuneAngleIndex
];
213 static void updateTargetAngle(void)
216 targetAngle
= AUTOTUNE_TARGET_ANGLE
;
218 targetAngle
= -AUTOTUNE_TARGET_ANGLE
;
222 debug
[2] = DEGREES_TO_DECIDEGREES(targetAngle
);
226 float autotune(angle_index_t angleIndex
, const rollAndPitchInclination_t
*inclination
, float errorAngle
)
231 if (!(phase
== PHASE_TUNE_ROLL
|| phase
== PHASE_TUNE_PITCH
) || autoTuneAngleIndex
!= angleIndex
) {
235 if (IS_PID_CONTROLLER_FP_BASED(pidController
)) {
236 // TODO support floating point based pid controllers
241 #ifdef DEBUG_AUTOTUNE
243 debug
[1] = inclination
->rawAngles
[angleIndex
];
249 currentAngle
= DECIDEGREES_TO_DEGREES(inclination
->raw
[angleIndex
]);
251 targetAngle
= -targetAngle
;
252 currentAngle
= DECIDEGREES_TO_DEGREES(-inclination
->raw
[angleIndex
]);
255 #ifdef DEBUG_AUTOTUNE
256 debug
[1] = DEGREES_TO_DECIDEGREES(currentAngle
);
257 debug
[2] = DEGREES_TO_DECIDEGREES(targetAngle
);
261 autotuneLogAngleTargets(currentAngle
);
264 if (secondPeakAngle
== 0) {
265 // The peak will be when our angular velocity is negative. To be sure we are in the right place,
266 // we also check to make sure our angle position is greater than zero.
268 if (currentAngle
> firstPeakAngle
) {
269 // we are still going up
270 firstPeakAngle
= currentAngle
;
271 targetAngleAtPeak
= targetAngle
;
273 debug
[3] = DEGREES_TO_DECIDEGREES(firstPeakAngle
);
275 } else if (firstPeakAngle
> 0) {
278 // when checking the I value, we would like to overshoot the target position by half of the max oscillation.
279 overshot
= currentAngle
- targetAngle
>= AUTOTUNE_MAX_OSCILLATION_ANGLE
/ 2;
282 pid
.i
*= AUTOTUNE_DECREASE_MULTIPLIER
;
283 if (pid
.i
< AUTOTUNE_MINIMUM_I_VALUE
) {
284 pid
.i
= AUTOTUNE_MINIMUM_I_VALUE
;
287 pid
.i
*= AUTOTUNE_INCREASE_MULTIPLIER
;
291 if (feature(FEATURE_BLACKBOX
)) {
292 flightLogEvent_autotuneCycleResult_t eventData
;
294 eventData
.flags
= overshot
? FLIGHT_LOG_EVENT_AUTOTUNE_FLAG_OVERSHOT
: 0;
295 eventData
.p
= pidProfile
->P8
[pidIndex
];
296 eventData
.i
= pidProfile
->I8
[pidIndex
];
297 eventData
.d
= pidProfile
->D8
[pidIndex
];
299 blackboxLogEvent(FLIGHT_LOG_EVENT_AUTOTUNE_CYCLE_RESULT
, (flightLogEventData_t
*)&eventData
);
303 // go back to checking P and D
304 cycle
= CYCLE_TUNE_PD
;
305 pidProfile
->I8
[pidIndex
] = 0;
311 // we are checking P and D values
312 // set up to look for the 2nd peak
313 secondPeakAngle
= currentAngle
;
314 timeoutAt
= millis() + AUTOTUNE_SETTLING_DELAY_MS
;
319 // We saw the first peak while tuning PD, looking for the second
321 if (currentAngle
< secondPeakAngle
) {
322 secondPeakAngle
= currentAngle
;
323 debug
[3] = DEGREES_TO_DECIDEGREES(secondPeakAngle
);
326 float oscillationAmplitude
= firstPeakAngle
- secondPeakAngle
;
328 uint32_t now
= millis();
329 int32_t signedDiff
= now
- timeoutAt
;
330 bool timedOut
= signedDiff
>= 0L;
332 // stop looking for the 2nd peak if we time out or if we change direction again after moving by more than half the maximum oscillation
333 if (timedOut
|| (oscillationAmplitude
> AUTOTUNE_MAX_OSCILLATION_ANGLE
/ 2 && currentAngle
> secondPeakAngle
)) {
335 // Our goal is to have zero overshoot and to have AUTOTUNE_MAX_OSCILLATION_ANGLE amplitude
337 overshot
= firstPeakAngle
> targetAngleAtPeak
;
339 #ifdef DEBUG_AUTOTUNE
343 #ifdef PREFER_HIGH_GAIN_SOLUTION
344 if (oscillationAmplitude
> AUTOTUNE_MAX_OSCILLATION_ANGLE
) {
345 // we have too much oscillation, so we can't increase D, so decrease P
346 pid
.p
*= AUTOTUNE_DECREASE_MULTIPLIER
;
348 // we don't have too much oscillation, so we can increase D
349 pid
.d
*= AUTOTUNE_INCREASE_MULTIPLIER
;
352 pid
.p
*= AUTOTUNE_DECREASE_MULTIPLIER
;
353 pid
.d
*= AUTOTUNE_INCREASE_MULTIPLIER
;
356 #ifdef DEBUG_AUTOTUNE
359 if (oscillationAmplitude
> AUTOTUNE_MAX_OSCILLATION_ANGLE
) {
360 // we have too much oscillation
361 pid
.d
*= AUTOTUNE_DECREASE_MULTIPLIER
;
363 // we don't have too much oscillation
364 pid
.p
*= AUTOTUNE_INCREASE_MULTIPLIER
;
368 pidProfile
->P8
[pidIndex
] = pid
.p
* MULTIWII_P_MULTIPLIER
;
369 pidProfile
->D8
[pidIndex
] = pid
.d
;
372 if (feature(FEATURE_BLACKBOX
)) {
373 flightLogEvent_autotuneCycleResult_t eventData
;
375 eventData
.flags
= (overshot
? FLIGHT_LOG_EVENT_AUTOTUNE_FLAG_OVERSHOT
: 0) | (timedOut
? FLIGHT_LOG_EVENT_AUTOTUNE_FLAG_TIMEDOUT
: 0);
376 eventData
.p
= pidProfile
->P8
[pidIndex
];
377 eventData
.i
= pidProfile
->I8
[pidIndex
];
378 eventData
.d
= pidProfile
->D8
[pidIndex
];
380 blackboxLogEvent(FLIGHT_LOG_EVENT_AUTOTUNE_CYCLE_RESULT
, (flightLogEventData_t
*)&eventData
);
384 if (cycle
== CYCLE_TUNE_PD2
) {
385 // switch to testing I value
386 cycle
= CYCLE_TUNE_I
;
388 pidProfile
->I8
[pidIndex
] = pid
.i
* MULTIWII_I_MULTIPLIER
;
390 cycle
= CYCLE_TUNE_PD2
;
393 // switch to the other direction for the new cycle
398 if (angleIndex
== AI_ROLL
) {
404 return targetAngle
- DECIDEGREES_TO_DEGREES(inclination
->raw
[angleIndex
]);
407 void autotuneReset(void)
411 nextPhase
= FIRST_TUNE_PHASE
;
414 void backupPids(pidProfile_t
*pidProfileToTune
)
416 memcpy(&pidBackup
, pidProfileToTune
, sizeof(pidBackup
));
419 void restorePids(pidProfile_t
*pidProfileToTune
)
421 memcpy(pidProfileToTune
, &pidBackup
, sizeof(pidBackup
));
424 void autotuneBeginNextPhase(pidProfile_t
*pidProfileToTune
)
428 if (phase
== PHASE_SAVE_OR_RESTORE_PIDS
) {
429 restorePids(pidProfileToTune
);
433 if (phase
== FIRST_TUNE_PHASE
) {
434 backupPids(pidProfileToTune
);
437 if (phase
== PHASE_TUNE_ROLL
) {
438 autoTuneAngleIndex
= AI_ROLL
;
439 } if (phase
== PHASE_TUNE_PITCH
) {
440 autoTuneAngleIndex
= AI_PITCH
;
444 cycle
= CYCLE_TUNE_PD
;
445 firstPeakAngle
= secondPeakAngle
= 0;
447 pidProfile
= pidProfileToTune
;
448 pidController
= pidProfile
->pidController
;
453 pid
.p
= pidProfile
->P8
[pidIndex
] / MULTIWII_P_MULTIPLIER
;
454 pid
.i
= pidProfile
->I8
[pidIndex
] / MULTIWII_I_MULTIPLIER
;
455 // divide by D multiplier to get our working value. We'll multiply by D multiplier when we are done.
456 pid
.d
= pidProfile
->D8
[pidIndex
] * (1.0f
/ AUTOTUNE_D_MULTIPLIER
);
458 pidProfile
->D8
[pidIndex
] = pid
.d
;
459 pidProfile
->I8
[pidIndex
] = 0;
462 autotuneLogCycleStart();
466 void autotuneEndPhase(void)
468 if (phase
== PHASE_TUNE_ROLL
|| phase
== PHASE_TUNE_PITCH
) {
470 // we leave P alone, just update I and D
472 pidProfile
->I8
[pidIndex
] = pid
.i
* MULTIWII_I_MULTIPLIER
;
474 // multiply by D multiplier. The best D is usually a little higher than what the algroithm produces.
475 pidProfile
->D8
[pidIndex
] = (pid
.d
* AUTOTUNE_D_MULTIPLIER
);
477 pidProfile
->P8
[PIDYAW
] = pidProfile
->P8
[PIDROLL
] * YAW_GAIN_MULTIPLIER
;
478 pidProfile
->I8
[PIDYAW
] = pidProfile
->I8
[PIDROLL
];
479 pidProfile
->D8
[PIDYAW
] = pidProfile
->D8
[PIDROLL
];
482 if (phase
== AUTOTUNE_PHASE_MAX
) {
484 nextPhase
= FIRST_TUNE_PHASE
;
490 bool havePidsBeenUpdatedByAutotune(void)
492 return targetAngle
!= 0;