Skip to content

[AUTOTUNE] Fix incorrect logic modifying D-gain instead of FF-gain #4870

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
digitalentity merged 1 commit into from
Jun 24, 2019
Merged

[AUTOTUNE] Fix incorrect logic modifying D-gain instead of FF-gain #4870

Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
29 changes: 15 additions & 14 deletions src/main/flight/pid_autotune.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -74,7 +74,7 @@ typedef struct {
bool pidSaturated;
float gainP;
float gainI;
float gainD;
float gainFF;
} pidAutotuneData_t;

#define AUTOTUNE_SAVE_PERIOD 5000 // Save interval is 5 seconds - when we turn off autotune we'll restore values from previous update at most 5 sec ago
Expand All @@ -90,7 +90,8 @@ void autotuneUpdateGains(pidAutotuneData_t * data)
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
pidBankMutable()->pid[axis].P = lrintf(data[axis].gainP);
pidBankMutable()->pid[axis].I = lrintf(data[axis].gainI);
pidBankMutable()->pid[axis].D = lrintf(data[axis].gainD);
pidBankMutable()->pid[axis].D = 0;
pidBankMutable()->pid[axis].FF = lrintf(data[axis].gainFF);
}
schedulePidGainsUpdate();
}
Expand All @@ -114,7 +115,7 @@ void autotuneStart(void)
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
tuneCurrent[axis].gainP = pidBank()->pid[axis].P;
tuneCurrent[axis].gainI = pidBank()->pid[axis].I;
tuneCurrent[axis].gainD = pidBank()->pid[axis].D;
tuneCurrent[axis].gainFF = pidBank()->pid[axis].FF;
tuneCurrent[axis].pidSaturated = false;
tuneCurrent[axis].stateEnterTime = millis();
tuneCurrent[axis].state = DEMAND_TOO_LOW;
Expand Down Expand Up @@ -200,18 +201,18 @@ void autotuneFixedWingUpdate(const flight_dynamics_index_t axis, float desiredRa
break;
case DEMAND_OVERSHOOT:
if (stateTimeMs >= pidAutotuneConfig()->fw_overshoot_time) {
tuneCurrent[axis].gainD = tuneCurrent[axis].gainD * (100 - AUTOTUNE_FIXED_WING_DECREASE_STEP) / 100.0f;
if (tuneCurrent[axis].gainD < AUTOTUNE_FIXED_WING_MIN_FF) {
tuneCurrent[axis].gainD = AUTOTUNE_FIXED_WING_MIN_FF;
tuneCurrent[axis].gainFF = tuneCurrent[axis].gainFF * (100 - AUTOTUNE_FIXED_WING_DECREASE_STEP) / 100.0f;
if (tuneCurrent[axis].gainFF < AUTOTUNE_FIXED_WING_MIN_FF) {
tuneCurrent[axis].gainFF = AUTOTUNE_FIXED_WING_MIN_FF;
}
gainsUpdated = true;
}
break;
case DEMAND_UNDERSHOOT:
if (stateTimeMs >= pidAutotuneConfig()->fw_undershoot_time && !tuneCurrent[axis].pidSaturated) {
tuneCurrent[axis].gainD = tuneCurrent[axis].gainD * (100 + AUTOTUNE_FIXED_WING_INCREASE_STEP) / 100.0f;
if (tuneCurrent[axis].gainD > AUTOTUNE_FIXED_WING_MAX_FF) {
tuneCurrent[axis].gainD = AUTOTUNE_FIXED_WING_MAX_FF;
tuneCurrent[axis].gainFF = tuneCurrent[axis].gainFF * (100 + AUTOTUNE_FIXED_WING_INCREASE_STEP) / 100.0f;
if (tuneCurrent[axis].gainFF > AUTOTUNE_FIXED_WING_MAX_FF) {
tuneCurrent[axis].gainFF = AUTOTUNE_FIXED_WING_MAX_FF;
}
gainsUpdated = true;
}
Expand All @@ -220,24 +221,24 @@ void autotuneFixedWingUpdate(const flight_dynamics_index_t axis, float desiredRa

if (gainsUpdated) {
// Set P-gain to 10% of FF gain (quite agressive - FIXME)
tuneCurrent[axis].gainP = tuneCurrent[axis].gainD * (pidAutotuneConfig()->fw_ff_to_p_gain / 100.0f);
tuneCurrent[axis].gainP = tuneCurrent[axis].gainFF * (pidAutotuneConfig()->fw_ff_to_p_gain / 100.0f);

// Set integrator gain to reach the same response as FF gain in 0.667 second
tuneCurrent[axis].gainI = (tuneCurrent[axis].gainD / FP_PID_RATE_FF_MULTIPLIER) * (1000.0f / pidAutotuneConfig()->fw_ff_to_i_time_constant) * FP_PID_RATE_I_MULTIPLIER;
tuneCurrent[axis].gainI = (tuneCurrent[axis].gainFF / FP_PID_RATE_FF_MULTIPLIER) * (1000.0f / pidAutotuneConfig()->fw_ff_to_i_time_constant) * FP_PID_RATE_I_MULTIPLIER;
tuneCurrent[axis].gainI = constrainf(tuneCurrent[axis].gainI, 2.0f, 50.0f);
autotuneUpdateGains(tuneCurrent);

switch (axis) {
case FD_ROLL:
blackboxLogAutotuneEvent(ADJUSTMENT_ROLL_D, tuneCurrent[axis].gainD);
blackboxLogAutotuneEvent(ADJUSTMENT_ROLL_D, tuneCurrent[axis].gainFF);
break;

case FD_PITCH:
blackboxLogAutotuneEvent(ADJUSTMENT_PITCH_D, tuneCurrent[axis].gainD);
blackboxLogAutotuneEvent(ADJUSTMENT_PITCH_D, tuneCurrent[axis].gainFF);
break;

case FD_YAW:
blackboxLogAutotuneEvent(ADJUSTMENT_YAW_D, tuneCurrent[axis].gainD);
blackboxLogAutotuneEvent(ADJUSTMENT_YAW_D, tuneCurrent[axis].gainFF);
break;
}
}
Expand Down