Skip to content

Commit d8c8628

Browse files
tracernztracernz
authored
Merge pull request #2042 from mlyle/mpl-justwindingyouup
stabilization: use antiwindup math for rate loop
2 parents c2da885 + 6a01dfc commit d8c8628

File tree

3 files changed

+73
-17
lines changed

3 files changed

+73
-17
lines changed

‎flight/Libraries/math/pid.c

+59-2
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

+4
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

+10-15
Original file line numberDiff line numberDiff line change
@@ -660,8 +660,7 @@ static void stabilizationTask(void* parameters)
660660
rateDesiredAxis[i] = bound_sym(raw_input[i], settings.ManualRate[i]);
661661

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

666665
break;
667666

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

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

711709
break;
712710

@@ -730,7 +728,7 @@ static void stabilizationTask(void* parameters)
730728
}
731729

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

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

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

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

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

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

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

804800
break;
805801

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

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

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

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

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

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

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

10621057
break;
10631058
case STABILIZATIONDESIRED_STABILIZATIONMODE_DISABLED:

0 commit comments

Comments
 (0)