Skip to content

Commit

Permalink
Merge pull request #2042 from mlyle/mpl-justwindingyouup
Browse files Browse the repository at this point in the history 
stabilization: use antiwindup math for rate loop
  • Loading branch information
@tracernz
tracernz authored Jan 30, 2018
2 parents c2da885 + 6a01dfc commit d8c8628
Show file tree
Hide file tree
Showing 3 changed files with 73 additions and 17 deletions.
61 changes: 59 additions & 2 deletions flight/Libraries/math/pid.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -103,8 +103,8 @@ float pid_apply_antiwindup(struct pid *pid, const float err,
dterm = pid->lastDer + dT / ( dT + deriv_tau) * ((diff * pid->d / dT) - pid->lastDer);
pid->lastDer = dterm; // ^ set constant to 1/(2*pi*f_cutoff)
} // 7.9577e-3 means 20 Hz f_cutoff
// Compute how much (if at all) the output is saturating

// Compute how much (if at all) the output is saturating
float ideal_output = ((err * pid->p) + pid->iAccumulator + dterm);
float saturation = 0;
if (ideal_output > max_bound) {
Expand All @@ -114,6 +114,7 @@ float pid_apply_antiwindup(struct pid *pid, const float err,
saturation = min_bound - ideal_output;
ideal_output = min_bound;
}

// Use Kt 10x Ki
pid->iAccumulator += saturation * (pid->i * 10.0f * dT);
pid->iAccumulator = bound_sym(pid->iAccumulator, pid->iLim);
Expand Down Expand Up @@ -168,6 +169,62 @@ float pid_apply_setpoint(struct pid *pid, struct pid_deadband *deadband, const f
return ((err * pid->p) + pid->iAccumulator + dterm);
}

float pid_apply_setpoint_antiwindup(struct pid *pid,
struct pid_deadband *deadband, const float setpoint,
const float measured, float min_bound, float max_bound)
{
float dT = pid->dT;

float err = setpoint - measured;
float err_d = (deriv_gamma * setpoint - measured);

if(deadband && deadband->width > 0)
{
err = cubic_deadband(err, deadband->width, deadband->slope, deadband->cubic_weight,
deadband->integrated_response);
err_d = cubic_deadband(err_d, deadband->width, deadband->slope, deadband->cubic_weight,
deadband->integrated_response);
}

if (pid->i == 0) {
// If Ki is zero, do not change the integrator. We do not reset to zero
// because sometimes the accumulator term is set externally
} else {
pid->iAccumulator += err * (pid->i * dT);
pid->iAccumulator = bound_sym(pid->iAccumulator, pid->iLim);
}

// Calculate DT1 term,
float dterm = 0;
float diff = (err_d - pid->lastErr);
pid->lastErr = err_d;
if(pid->d && dT)
{
dterm = pid->lastDer + dT / ( dT + deriv_tau) * ((diff * pid->d / dT) - pid->lastDer);
pid->lastDer = dterm; // ^ set constant to 1/(2*pi*f_cutoff)
} // 7.9577e-3 means 20 Hz f_cutoff

// Compute how much (if at all) the output is saturating
float ideal_output = ((err * pid->p) + pid->iAccumulator + dterm);
float saturation = 0;
if (ideal_output > max_bound) {
saturation = max_bound - ideal_output;
ideal_output = max_bound;
} else if (ideal_output < min_bound) {
saturation = min_bound - ideal_output;
ideal_output = min_bound;
}

if (pid->i == 0) {
// If Ki is zero, do not change the integrator.
} else {
pid->iAccumulator += saturation * (pid->i * dT);
pid->iAccumulator = bound_sym(pid->iAccumulator, pid->iLim);
}

return ideal_output;
}

/**
* Reset a bit
* @param[in] pid The pid to reset
Expand Down
4 changes: 4 additions & 0 deletions flight/Libraries/math/pid.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -56,11 +56,15 @@ struct pid {
float pid_apply(struct pid *pid, const float err);
float pid_apply_antiwindup(struct pid *pid, const float err, float min_bound, float max_bound);
float pid_apply_setpoint(struct pid *pid, struct pid_deadband *deadband, const float setpoint, const float measured);
float pid_apply_setpoint_antiwindup(struct pid *pid,
struct pid_deadband *deadband, const float setpoint,
const float measured, float min_bound, float max_bound);
void pid_zero(struct pid *pid);
void pid_configure(struct pid *pid, float p, float i, float d, float iLim, float dT);
void pid_configure_derivative(float cutoff, float gamma);
void pid_configure_deadband(struct pid_deadband *deadband, float width, float slope);


#endif /* PID_H */

/**
Expand Down
25 changes: 10 additions & 15 deletions flight/Modules/Stabilization/stabilization.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -660,8 +660,7 @@ static void stabilizationTask(void* parameters)
rateDesiredAxis[i] = bound_sym(raw_input[i], settings.ManualRate[i]);

// Compute the inner loop
actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_GROUP_RATE + i], get_deadband(i), rateDesiredAxis[i], gyro_filtered[i]);
actuatorDesiredAxis[i] = bound_sym(actuatorDesiredAxis[i],1.0f);
actuatorDesiredAxis[i] = pid_apply_setpoint_antiwindup(&pids[PID_GROUP_RATE + i], get_deadband(i), rateDesiredAxis[i], gyro_filtered[i], -1.0f, 1.0f);

break;

Expand Down Expand Up @@ -705,8 +704,7 @@ static void stabilizationTask(void* parameters)
rateDesiredAxis[i] = bound_sym(curve_cmd * max_rate_filtered[i], max_rate_filtered[i]);

// Compute the inner loop
actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_GROUP_RATE + i], get_deadband(i), rateDesiredAxis[i], gyro_filtered[i]);
actuatorDesiredAxis[i] = bound_sym(actuatorDesiredAxis[i], 1.0f);
actuatorDesiredAxis[i] = pid_apply_setpoint_antiwindup(&pids[PID_GROUP_RATE + i], get_deadband(i), rateDesiredAxis[i], gyro_filtered[i], -1.0f, 1.0f);

break;

Expand All @@ -730,7 +728,7 @@ static void stabilizationTask(void* parameters)
}

// Compute the inner loop
actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_GROUP_RATE + i], get_deadband(i), rateDesiredAxis[i], gyro_filtered[i]);
actuatorDesiredAxis[i] = pid_apply_setpoint_antiwindup(&pids[PID_GROUP_RATE + i], get_deadband(i), rateDesiredAxis[i], gyro_filtered[i], -1.0f, 1.0f);
actuatorDesiredAxis[i] = factor * raw_input[i] + (1.0f - factor) * actuatorDesiredAxis[i];
actuatorDesiredAxis[i] = bound_sym(actuatorDesiredAxis[i], 1.0f);

Expand All @@ -747,7 +745,7 @@ static void stabilizationTask(void* parameters)
rateDesiredAxis[i] = bound_sym(rateDesiredAxis[i], settings.MaximumRate[i]);

// Compute the inner loop
actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_GROUP_RATE + i], get_deadband(i), rateDesiredAxis[i], gyro_filtered[i]);
actuatorDesiredAxis[i] = pid_apply_setpoint_antiwindup(&pids[PID_GROUP_RATE + i], get_deadband(i), rateDesiredAxis[i], gyro_filtered[i], -1.0f, 1.0f);
actuatorDesiredAxis[i] = bound_sym(actuatorDesiredAxis[i],1.0f);

break;
Expand All @@ -772,8 +770,7 @@ static void stabilizationTask(void* parameters)

// Compute desired rate as input biased towards leveling
rateDesiredAxis[i] = raw_input[i] + weak_leveling;
actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_GROUP_RATE + i], get_deadband(i), rateDesiredAxis[i], gyro_filtered[i]);
actuatorDesiredAxis[i] = bound_sym(actuatorDesiredAxis[i],1.0f);
actuatorDesiredAxis[i] = pid_apply_setpoint_antiwindup(&pids[PID_GROUP_RATE + i], get_deadband(i), rateDesiredAxis[i], gyro_filtered[i], -1.0f, 1.0f);

break;
}
Expand All @@ -798,8 +795,7 @@ static void stabilizationTask(void* parameters)
rateDesiredAxis[i] = bound_sym(tmpRateDesired, settings.MaximumRate[i]);
}

actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_GROUP_RATE + i], get_deadband(i), rateDesiredAxis[i], gyro_filtered[i]);
actuatorDesiredAxis[i] = bound_sym(actuatorDesiredAxis[i],1.0f);
actuatorDesiredAxis[i] = pid_apply_setpoint_antiwindup(&pids[PID_GROUP_RATE + i], get_deadband(i), rateDesiredAxis[i], gyro_filtered[i], -1.0f, 1.0f);

break;

Expand All @@ -824,7 +820,7 @@ static void stabilizationTask(void* parameters)
rateDesiredAxis[i] = bound_sym(rateDesiredAxis[i], settings.ManualRate[i]);

// Compute the inner loop
actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_GROUP_RATE + i], get_deadband(i), rateDesiredAxis[i], gyro_filtered[i]);
actuatorDesiredAxis[i] = pid_apply_setpoint_antiwindup(&pids[PID_GROUP_RATE + i], get_deadband(i), rateDesiredAxis[i], gyro_filtered[i], -1.0f, 1.0f);
actuatorDesiredAxis[i] = bound_sym(actuatorDesiredAxis[i],1.0f);

break;
Expand Down Expand Up @@ -874,13 +870,13 @@ static void stabilizationTask(void* parameters)
rateDesiredAxis[i] = bound_sym(rateDesiredAxis[i], settings.MaximumRate[i]);

// Compute the inner loop
actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_GROUP_RATE + i], get_deadband(i), rateDesiredAxis[i], gyro_filtered[i]);
actuatorDesiredAxis[i] = pid_apply_setpoint_antiwindup(&pids[PID_GROUP_RATE + i], get_deadband(i), rateDesiredAxis[i], gyro_filtered[i], -1.0f, 1.0f);
} else {
// Get the desired rate. yaw is always in rate mode in system ident.
rateDesiredAxis[i] = bound_sym(raw_input[i], settings.ManualRate[i]);

// Compute the inner loop only for yaw
actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_GROUP_RATE + i], get_deadband(i), rateDesiredAxis[i], gyro_filtered[i]);
actuatorDesiredAxis[i] = pid_apply_setpoint_antiwindup(&pids[PID_GROUP_RATE + i], get_deadband(i), rateDesiredAxis[i], gyro_filtered[i], -1.0f, 1.0f);
}

const float scale = settings.AutotuneActuationEffort[i];
Expand Down Expand Up @@ -1056,8 +1052,7 @@ static void stabilizationTask(void* parameters)
rateDesiredAxis[i] = bound_sym(rateDesiredAxis[i], settings.PoiMaximumRate[i]);

// Compute the inner loop
actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_GROUP_RATE + i], get_deadband(i), rateDesiredAxis[i], gyro_filtered[i]);
actuatorDesiredAxis[i] = bound_sym(actuatorDesiredAxis[i],1.0f);
actuatorDesiredAxis[i] = pid_apply_setpoint_antiwindup(&pids[PID_GROUP_RATE + i], get_deadband(i), rateDesiredAxis[i], gyro_filtered[i], -1.0f, 1.0f);

break;
case STABILIZATIONDESIRED_STABILIZATIONMODE_DISABLED:
Expand Down

0 comments on commit d8c8628

Please sign in to comment.