Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-23 00:05:33 +03:00
Code Issues Wiki Activity

Update autotune so it now actually changes the pid values.

Browse source 
This commit also fixes a few missing const keywords after the map file
was inspected.
This commit is contained in:
Dominic Clifton 2014-05-27 21:20:29 +01:00
parent 2cf686b36b
commit bc84f6d5d4
8 changed files with 105 additions and 59 deletions
Download patch file Download diff file Expand all files Collapse all files

27
docs/Autotune.md
View file

@ -4,31 +4,38 @@ Autotune helps to automatically tune your multirotor.
## Configuration.
Autotune only works in HORIZON or ANGLE mode.
Autotune only works in HORIZON or ANGLE mode, before using auto-tune it's best you setup so there is as little drift as possible.
Autotuning is best on a full battery in good flying conditions, i.e. no or minimal wind.
Configure a two position switch on your transmitter to activate the AUTOTUNE and HORIZON or ANGLE modes using the auxilary configuration.
You may find a momentary switch more suitable than a toggle switch.
Configure a switch on your transmitter to activate the AUTOTUNE and HORIZON or ANGLE modes using the auxilary configuration.
## Using autotuning
Turn off the autotune switch. If the autotune switch is on while not armed the warning LED will flash and you cannot arm.
Launch the multirotor.
Flip the autotune switch on your transmitter.
Turn on/hold the autotune switch on your transmitter.
Observe roll/left/right a few times.
Observe roll left/right. A beep code will sound on the buzzer.
Turn off/release the switch while still flying to stop this phase of tuning.
PID settings will have been updated for ROLL/YAW.
Disable the switch while still flying.
Turn on/hold the switch again.
Enable the switch again.
Observe pitch forwards/backwards. A beep code will sound on the buzzer.
Observe pitch forwards/backwards a few times.
Turn off/release the switch while still flying to stop this phase of tuning.
PID settings will have been updated for PITCH/YAW.
Disable the switch again.
PIDS are updated, fly and see if it's better.
PIDS are tunes, fly and see if it's better.
If it's worse, flip the switch again to restore previous pids that were present prior to arming.
Land.
@ -36,7 +43,7 @@ Verify results via an app while power still applied if desired.
Cutting the power will loose the unsaved pids.
If you're happy with the pids then while disarmed flip the autotune switch again to save all settings.
If you're happy with the pids then while disarmed flip the autotune switch again to save all settings then flip it back so you can arm again.
A beeper will sound indicating the settings are saved.

121
src/flight_autotune.c
View file

@ -24,6 +24,14 @@ extern int16_t debug[4];
#define AUTOTUNE_D_MULTIPLIER 1.2f
#define AUTOTUNE_SETTLING_DELAY_MS 250 // 1/4 of a second.
#define AUTOTUNE_INCREASE_MULTIPLIER 1.03f
#define AUTOTUNE_DECREASE_MULTIPLIER 0.97f
#define AUTOTUNE_MINIMUM_I_VALUE 0.001f
#define YAW_GAIN_MULTIPLIER 2.0f
typedef enum {
PHASE_IDLE = 0,
PHASE_TUNE_ROLL,
@ -31,6 +39,11 @@ typedef enum {
PHASE_SAVE_OR_RESTORE_PIDS,
} autotunePhase_e;
static const pidIndex_e angleIndexToPidIndexMap[] = {
PIDROLL,
PIDPITCH
};
#define AUTOTUNE_PHASE_MAX PHASE_SAVE_OR_RESTORE_PIDS
#define AUTOTUNE_PHASE_COUNT (AUTOTUNE_PHASE_MAX + 1)
@ -39,6 +52,7 @@ typedef enum {
static pidProfile_t *pidProfile;
static pidProfile_t pidBackup;
static uint8_t pidController;
static uint8_t pidIndex;
static bool rising;
static uint8_t cycleCount; // TODO can we replace this with an enum to improve readability.
static uint32_t timeoutAt;
@ -50,6 +64,19 @@ static float targetAngle = 0;
static float targetAngleAtPeak;
static float secondPeakAngle, firstPeakAngle; // deci dgrees, 180 deg = 1800
typedef struct fp_pid {
float p;
float i;
float d;
} fp_pid_t;
static fp_pid_t pid;
// These are used to convert between multiwii integer values to the float pid values used by the autotuner.
#define MULTIWII_P_MULTIPLIER 10.0f // e.g 0.4 * 10 = 40
#define MULTIWII_I_MULTIPLIER 1000.0f // e.g 0.030 * 1000 = 30
// Note there is no D multiplier since D values are stored and used AS-IS
bool isAutotuneIdle(void)
{
return phase == PHASE_IDLE;
@ -61,6 +88,11 @@ static void startNewCycle(void)
secondPeakAngle = firstPeakAngle = 0;
}
static void updatePidIndex(void)
{
pidIndex = angleIndexToPidIndexMap[autoTuneAngleIndex];
}
static void updateTargetAngle(void)
{
if (rising) {
@ -75,19 +107,26 @@ static void updateTargetAngle(void)
#endif
}
float autotune(angle_index_t angleIndex, rollAndPitchInclination_t *inclination, float errorAngle)
float autotune(angle_index_t angleIndex, const rollAndPitchInclination_t *inclination, float errorAngle)
{
float currentAngle;
if (!(phase == PHASE_TUNE_ROLL || phase == PHASE_TUNE_PITCH) || autoTuneAngleIndex != angleIndex) {
return errorAngle;
}
if (pidController == 2) {
// TODO support new baseflight pid controller
return errorAngle;
}
debug[0] = 0;
#if 0
debug[1] = inclination->rawAngles[angleIndex];
#endif
float currentAngle;
updatePidIndex();
if (rising) {
currentAngle = DECIDEGREES_TO_DEGREES(inclination->rawAngles[angleIndex]);
@ -115,16 +154,18 @@ float autotune(angle_index_t angleIndex, rollAndPitchInclination_t *inclination,
} else if (secondPeakAngle > 0) {
if (cycleCount == 0) {
// when checking the I value, we would like to overshoot the target position by half of the max oscillation.
// if (currentangle-targetangle<(FPAUTOTUNEMAXOSCILLATION>>1)) {
// currentivalueshifted=lib_fp_multiply(currentivalueshifted,AUTOTUNEINCREASEMULTIPLIER);
// } else {
// currentivalueshifted=lib_fp_multiply(currentivalueshifted,AUTOTUNEDECREASEMULTIPLIER);
// if (currentivalueshifted<AUTOTUNEMINIMUMIVALUE) currentivalueshifted=AUTOTUNEMINIMUMIVALUE;
// }
if (currentAngle - targetAngle < AUTOTUNE_MAX_OSCILLATION / 2) {
pid.i *= AUTOTUNE_INCREASE_MULTIPLIER;
} else {
pid.i *= AUTOTUNE_DECREASE_MULTIPLIER;
if (pid.i < AUTOTUNE_MINIMUM_I_VALUE) {
pid.i = AUTOTUNE_MINIMUM_I_VALUE;
}
}
// go back to checking P and D
cycleCount = 1;
// usersettings.pid_igain[autotuneindex]=0;
pidProfile->I8[pidIndex] = 0;
startNewCycle();
} else {
// we are checking P and D values
@ -152,7 +193,6 @@ float autotune(angle_index_t angleIndex, rollAndPitchInclination_t *inclination,
// analyze the data
// Our goal is to have zero overshoot and to have AUTOTUNEMAXOSCILLATION amplitude
if (secondPeakAngle > targetAngleAtPeak) {
// overshot
debug[0] = 1;
@ -161,16 +201,12 @@ float autotune(angle_index_t angleIndex, rollAndPitchInclination_t *inclination,
if (oscillationAmplitude > AUTOTUNE_MAX_OSCILLATION) {
// we have too much oscillation, so we can't increase D, so decrease P
#endif
// decrease P
//currentpvalueshifted=lib_fp_multiply(currentpvalueshifted, AUTOTUNEDECREASEMULTIPLIER); // TODO
pid.p *= AUTOTUNE_DECREASE_MULTIPLIER;
#ifdef PREFER_HIGH_GAIN_SOLUTION
} else {
// we don't have too much oscillation, so we can increase D"
// we don't have too much oscillation, so we can increase D
#endif
// increase D
//currentdvalueshifted=lib_fp_multiply(currentdvalueshifted, AUTOTUNEINCREASEMULTIPLIER); // TODO
pid.d *= AUTOTUNE_INCREASE_MULTIPLIER;
#ifdef PREFER_HIGH_GAIN_SOLUTION
}
#endif
@ -179,16 +215,16 @@ float autotune(angle_index_t angleIndex, rollAndPitchInclination_t *inclination,
debug[0] = 2;
if (oscillationAmplitude > AUTOTUNE_MAX_OSCILLATION) {
// we have too much oscillation, so we should lower D
//currentdvalueshifted=lib_fp_multiply(currentdvalueshifted, AUTOTUNEDECREASEMULTIPLIER); // TODO
// we have too much oscillation
pid.d *= AUTOTUNE_DECREASE_MULTIPLIER;
} else {
// we don't have too much oscillation, so we increase P
//currentpvalueshifted=lib_fp_multiply(currentpvalueshifted, AUTOTUNEINCREASEMULTIPLIER); // TODO
// we don't have too much oscillation
pid.p *= AUTOTUNE_INCREASE_MULTIPLIER;
}
}
//usersettings.pid_pgain[autotuneindex]=currentpvalueshifted>>AUTOTUNESHIFT; // TODO
//usersettings.pid_dgain[autotuneindex]=currentdvalueshifted>>AUTOTUNESHIFT; // TODO
pidProfile->P8[pidIndex] = pid.p * MULTIWII_P_MULTIPLIER;
pidProfile->D8[pidIndex] = pid.d;
// switch to the other direction and start a new cycle
startNewCycle();
@ -197,7 +233,7 @@ float autotune(angle_index_t angleIndex, rollAndPitchInclination_t *inclination,
// switch to testing I value
cycleCount = 0;
//usersettings.pid_igain[autotuneindex]=currentivalueshifted>>AUTOTUNESHIFT; // TODO
pidProfile->I8[pidIndex] = pid.i * MULTIWII_I_MULTIPLIER;
}
}
}
@ -254,30 +290,33 @@ void autotuneBeginNextPhase(pidProfile_t *pidProfileToTune, uint8_t pidControlle
pidProfile = pidProfileToTune;
pidController = pidControllerInUse;
updatePidIndex();
updateTargetAngle();
// TODO
// currentpvalueshifted=usersettings.pid_pgain[autotuneindex]<<AUTOTUNESHIFT;
// currentivalueshifted=usersettings.pid_igain[autotuneindex]<<AUTOTUNESHIFT;
// // divide by D multiplier to get our working value. We'll multiply by D multiplier when we are done.
// usersettings.pid_dgain[autotuneindex]=lib_fp_multiply(usersettings.pid_dgain[autotuneindex],FPONEOVERAUTOTUNE_D_MULTIPLIER);
// currentdvalueshifted=usersettings.pid_dgain[autotuneindex]<<AUTOTUNESHIFT;
//
// usersettings.pid_igain[autotuneindex]=0;
pid.p = pidProfile->P8[pidIndex] / MULTIWII_P_MULTIPLIER;
pid.i = pidProfile->I8[pidIndex] / MULTIWII_I_MULTIPLIER;
// divide by D multiplier to get our working value. We'll multiply by D multiplier when we are done.
pid.d = pidProfile->D8[pidIndex] * (1.0f / AUTOTUNE_D_MULTIPLIER);
pidProfile->D8[pidIndex] = pid.d;
pidProfile->I8[pidIndex] = 0;
}
void autotuneEndPhase(void)
{
// TODO
// usersettings.pid_igain[autotuneindex]=currentivalueshifted>>AUTOTUNESHIFT;
//
// // multiply by D multiplier. The best D is usually a little higher than what the algroithm produces.
// usersettings.pid_dgain[autotuneindex]=lib_fp_multiply(currentdvalueshifted,FPAUTOTUNE_D_MULTIPLIER)>>AUTOTUNESHIFT;
//
// usersettings.pid_igain[YAWINDEX]=usersettings.pid_igain[ROLLINDEX];
// usersettings.pid_dgain[YAWINDEX]=usersettings.pid_dgain[ROLLINDEX];
// usersettings.pid_pgain[YAWINDEX]=lib_fp_multiply(usersettings.pid_pgain[ROLLINDEX],YAWGAINMULTIPLIER);
if (phase == PHASE_TUNE_ROLL || phase == PHASE_TUNE_PITCH) {
// we leave P alone, just update I and D
pidProfile->I8[pidIndex] = pid.i * MULTIWII_I_MULTIPLIER;
// multiply by D multiplier. The best D is usually a little higher than what the algroithm produces.
pidProfile->D8[pidIndex] = (pid.d * AUTOTUNE_D_MULTIPLIER);
pidProfile->P8[PIDYAW] = pidProfile->P8[PIDROLL] * YAW_GAIN_MULTIPLIER;
pidProfile->I8[PIDYAW] = pidProfile->I8[PIDROLL];
pidProfile->D8[PIDYAW] = pidProfile->D8[PIDROLL];
}
if (phase == AUTOTUNE_PHASE_MAX) {
phase = PHASE_IDLE;

2
src/flight_autotune.h
View file

@ -2,7 +2,7 @@
void autotuneReset();
void autotuneBeginNextPhase(pidProfile_t *pidProfileToTune, uint8_t pidControllerInUse);
float autotune(angle_index_t angleIndex, rollAndPitchInclination_t *inclination, float errorAngle);
float autotune(angle_index_t angleIndex, const rollAndPitchInclination_t *inclination, float errorAngle);
void autotuneEndPhase();
bool isAutotuneIdle(void);

2
src/flight_common.c
View file

@ -56,7 +56,7 @@ void resetErrorGyro(void)
errorGyroIf[YAW] = 0;
}
angle_index_t rcAliasToAngleIndexMap[] = { AI_ROLL, AI_PITCH };
const angle_index_t rcAliasToAngleIndexMap[] = { AI_ROLL, AI_PITCH };
bool shouldAutotune(void)
{

4
src/flight_common.h
View file

@ -1,7 +1,7 @@
#pragma once
enum {
typedef enum {
PIDROLL,
PIDPITCH,
PIDYAW,
@ -13,7 +13,7 @@ enum {
PIDMAG,
PIDVEL,
PID_ITEM_COUNT
};
} pidIndex_e;
typedef struct pidProfile_s {
uint8_t P8[PID_ITEM_COUNT];

4
src/serial_common.c
View file

@ -78,12 +78,12 @@ typedef struct serialPortSearchResult_s {
uint8_t startSerialPortFunctionIndex; // used by findNextSerialPort
} serialPortSearchResult_t;
static serialPortFunctionList_t serialPortFunctionList = {
static const serialPortFunctionList_t serialPortFunctionList = {
4,
serialPortFunctions
};
serialPortFunctionList_t *getSerialPortFunctionList(void)
const serialPortFunctionList_t *getSerialPortFunctionList(void)
{
return &serialPortFunctionList;
}

2
src/serial_common.h
View file

@ -113,7 +113,7 @@ bool isSerialConfigValid(serialConfig_t *serialConfig);
bool doesConfigurationUsePort(serialConfig_t *serialConfig, serialPortIdentifier_e portIdentifier);
bool isSerialPortFunctionShared(serialPortFunction_e functionToUse, uint16_t functionMask);
serialPortFunctionList_t *getSerialPortFunctionList(void);
const serialPortFunctionList_t *getSerialPortFunctionList(void);
void evaluateOtherData(uint8_t sr);
void handleSerial(void);

2
src/serial_msp.c
View file

@ -316,7 +316,7 @@ static void openAllMSPSerialPorts(serialConfig_t *serialConfig)
} while (port);
// XXX this function might help with adding support for MSP on more than one port, if not delete it.
serialPortFunctionList_t *serialPortFunctionList = getSerialPortFunctionList();
const serialPortFunctionList_t *serialPortFunctionList = getSerialPortFunctionList();
UNUSED(serialPortFunctionList);
}