mawei/MoreTore
1
0
Fork 0
mirror of https://github.com/MoreTore/openpilot.git synced 2026-07-26 20:32:04 +08:00
Code Issues Packages Projects Releases Wiki Activity

fix sticky integrator

Browse Source
This commit is contained in:
MoreTore
2025-11-11 12:11:36 -06:00
parent 119502f73d
commit e47a41cc09
1 changed files with 10 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/controls/lib/pid.py
+10 -7
View File
@@ -50,14 +50,17 @@ class PIDController:
self.d = self.k_d * error_rate
self.f = self.k_f * feedforward
if not freeze_integrator:
i = self.i + self.k_i * self.i_dt * error
i_candidate = self.i if freeze_integrator else self.i + self.k_i * self.i_dt * error
u = self.p + i_candidate + self.d + self.f
u_sat = np.clip(u, self.neg_limit, self.pos_limit)
# Don't allow windup if already clipping
test_control = self.p + i + self.d + self.f
i_upperbound = self.i if test_control > self.pos_limit else self.pos_limit
i_lowerbound = self.i if test_control < self.neg_limit else self.neg_limit
self.i = np.clip(i, i_lowerbound, i_upperbound)
if u == u_sat:
self.i = i_candidate
else:
if u > self.pos_limit and error < 0:
self.i = i_candidate
elif u < self.neg_limit and error > 0:
self.i = i_candidate
control = self.p + self.i + self.d + self.f
self.control = np.clip(control, self.neg_limit, self.pos_limit)