Skip to content

Commit 76d07ef

Browse files
mlyleglowtape
mlyle
authored and
glowtape
committed
stabilization: use antiwindup math for rate loop
Woops! Seems we were not doing this before. Also introduce a version of the PID calls that does both deriv setpoint weighting and antiwindup.
1 parent c314916 commit 76d07ef

File tree

3 files changed

+73
-17
lines changed

3 files changed

+73
-17
lines changed

‎flight/Libraries/math/pid.c‎

Lines changed: 59 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -103,8 +103,8 @@ float pid_apply_antiwindup(struct pid *pid, const float err,
103103
dterm = pid->lastDer + dT / ( dT + deriv_tau) * ((diff * pid->d / dT) - pid->lastDer);
104104
pid->lastDer = dterm; // ^ set constant to 1/(2*pi*f_cutoff)
105105
} // 7.9577e-3 means 20 Hz f_cutoff
106-
107-
// Compute how much (if at all) the output is saturating
106+
107+
// Compute how much (if at all) the output is saturating
108108
float ideal_output = ((err * pid->p) + pid->iAccumulator + dterm);
109109
float saturation = 0;
110110
if (ideal_output > max_bound) {
@@ -114,6 +114,7 @@ float pid_apply_antiwindup(struct pid *pid, const float err,
114114
saturation = min_bound - ideal_output;
115115
ideal_output = min_bound;
116116
}
117+
117118
// Use Kt 10x Ki
118119
pid->iAccumulator += saturation * (pid->i * 10.0f * dT);
119120
pid->iAccumulator = bound_sym(pid->iAccumulator, pid->iLim);
@@ -168,6 +169,62 @@ float pid_apply_setpoint(struct pid *pid, struct pid_deadband *deadband, const f
168169
return ((err * pid->p) + pid->iAccumulator + dterm);
169170
}
170171

172+
float pid_apply_setpoint_antiwindup(struct pid *pid,
173+
struct pid_deadband *deadband, const float setpoint,
174+
const float measured, float min_bound, float max_bound)
175+
{
176+
float dT = pid->dT;
177+
178+
float err = setpoint - measured;
179+
float err_d = (deriv_gamma * setpoint - measured);
180+
181+
if(deadband && deadband->width > 0)
182+
{
183+
err = cubic_deadband(err, deadband->width, deadband->slope, deadband->cubic_weight,
184+
deadband->integrated_response);
185+
err_d = cubic_deadband(err_d, deadband->width, deadband->slope, deadband->cubic_weight,
186+
deadband->integrated_response);
187+
}
188+
189+
if (pid->i == 0) {
190+
// If Ki is zero, do not change the integrator. We do not reset to zero
191+
// because sometimes the accumulator term is set externally
192+
} else {
193+
pid->iAccumulator += err * (pid->i * dT);
194+
pid->iAccumulator = bound_sym(pid->iAccumulator, pid->iLim);
195+
}
196+
197+
// Calculate DT1 term,
198+
float dterm = 0;
199+
float diff = (err_d - pid->lastErr);
200+
pid->lastErr = err_d;
201+
if(pid->d && dT)
202+
{
203+
dterm = pid->lastDer + dT / ( dT + deriv_tau) * ((diff * pid->d / dT) - pid->lastDer);
204+
pid->lastDer = dterm; // ^ set constant to 1/(2*pi*f_cutoff)
205+
} // 7.9577e-3 means 20 Hz f_cutoff
206+
207+
// Compute how much (if at all) the output is saturating
208+
float ideal_output = ((err * pid->p) + pid->iAccumulator + dterm);
209+
float saturation = 0;
210+
if (ideal_output > max_bound) {
211+
saturation = max_bound - ideal_output;
212+
ideal_output = max_bound;
213+
} else if (ideal_output < min_bound) {
214+
saturation = min_bound - ideal_output;
215+
ideal_output = min_bound;
216+
}
217+
218+
if (pid->i == 0) {
219+
// If Ki is zero, do not change the integrator.
220+
} else {
221+
pid->iAccumulator += saturation * (pid->i * dT);
222+
pid->iAccumulator = bound_sym(pid->iAccumulator, pid->iLim);
223+
}
224+
225+
return ideal_output;
226+
}
227+
171228
/**
172229
* Reset a bit
173230
* @param[in] pid The pid to reset

‎flight/Libraries/math/pid.h‎

Lines changed: 4 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -56,11 +56,15 @@ struct pid {
5656
float pid_apply(struct pid *pid, const float err);
5757
float pid_apply_antiwindup(struct pid *pid, const float err, float min_bound, float max_bound);
5858
float pid_apply_setpoint(struct pid *pid, struct pid_deadband *deadband, const float setpoint, const float measured);
59+
float pid_apply_setpoint_antiwindup(struct pid *pid,
60+
struct pid_deadband *deadband, const float setpoint,
61+
const float measured, float min_bound, float max_bound);
5962
void pid_zero(struct pid *pid);
6063
void pid_configure(struct pid *pid, float p, float i, float d, float iLim, float dT);
6164
void pid_configure_derivative(float cutoff, float gamma);
6265
void pid_configure_deadband(struct pid_deadband *deadband, float width, float slope);
6366

67+
6468
#endif /* PID_H */
6569

6670
/**

‎flight/Modules/Stabilization/stabilization.c‎

Lines changed: 10 additions & 15 deletions
Original file line numberDiff line numberDiff line change
@@ -760,8 +760,7 @@ static void stabilizationTask(void* parameters)
760760
rateDesiredAxis[i] = bound_sym(raw_input[i], settings.ManualRate[i]);
761761

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

766765
break;
767766

@@ -805,8 +804,7 @@ static void stabilizationTask(void* parameters)
805804
rateDesiredAxis[i] = bound_sym(curve_cmd * max_rate_filtered[i], max_rate_filtered[i]);
806805

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

811809
break;
812810

@@ -830,7 +828,7 @@ static void stabilizationTask(void* parameters)
830828
}
831829

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

@@ -847,7 +845,7 @@ static void stabilizationTask(void* parameters)
847845
rateDesiredAxis[i] = bound_sym(rateDesiredAxis[i], settings.MaximumRate[i]);
848846

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

853851
break;
@@ -872,8 +870,7 @@ static void stabilizationTask(void* parameters)
872870

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

878875
break;
879876
}
@@ -898,8 +895,7 @@ static void stabilizationTask(void* parameters)
898895
rateDesiredAxis[i] = bound_sym(tmpRateDesired, settings.MaximumRate[i]);
899896
}
900897

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

904900
break;
905901

@@ -924,7 +920,7 @@ static void stabilizationTask(void* parameters)
924920
rateDesiredAxis[i] = bound_sym(rateDesiredAxis[i], settings.ManualRate[i]);
925921

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

930926
break;
@@ -974,13 +970,13 @@ static void stabilizationTask(void* parameters)
974970
rateDesiredAxis[i] = bound_sym(rateDesiredAxis[i], settings.MaximumRate[i]);
975971

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

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

986982
const float scale = settings.AutotuneActuationEffort[i];
@@ -1156,8 +1152,7 @@ static void stabilizationTask(void* parameters)
11561152
rateDesiredAxis[i] = bound_sym(rateDesiredAxis[i], settings.PoiMaximumRate[i]);
11571153

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

11621157
break;
11631158
case STABILIZATIONDESIRED_STABILIZATIONMODE_DISABLED:

0 commit comments

Comments
 (0)