commaai
/
opendbc
mirror of https://github.com/maweiyuweiwei/opendbc.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Match pid.py file from openpilot (#1514) 

* match pid

* fix
This commit is contained in:
Shane Smiskol 2024-11-20 19:26:21 -08:00 committed by GitHub
parent 0bfd38f4c9
commit b17502d658
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194
1 changed files with 26 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

37
opendbc/car/common/pid.py
View File

@ -1,15 +1,21 @@
import numpy as np
from numbers import Number from numbers import Number
from opendbc.car.common.numpy_fast import clip, interp from opendbc.car.common.numpy_fast import clip, interp
class PIDController: class PIDController:
def __init__(self, k_p, k_i, pos_limit=1e308, neg_limit=-1e308, rate=100): def __init__(self, k_p, k_i, k_f=0., k_d=0., pos_limit=1e308, neg_limit=-1e308, rate=100):
self._k_p = k_p self._k_p = k_p
self._k_i = k_i self._k_i = k_i
self._k_d = k_d
self.k_f = k_f # feedforward gain
if isinstance(self._k_p, Number): if isinstance(self._k_p, Number):
self._k_p = [[0], [self._k_p]] self._k_p = [[0], [self._k_p]]
if isinstance(self._k_i, Number): if isinstance(self._k_i, Number):
self._k_i = [[0], [self._k_i]] self._k_i = [[0], [self._k_i]]
if isinstance(self._k_d, Number):
self._k_d = [[0], [self._k_d]]
self.pos_limit = pos_limit self.pos_limit = pos_limit
self.neg_limit = neg_limit self.neg_limit = neg_limit
@ -28,6 +34,10 @@ class PIDController:
def k_i(self): def k_i(self):
return interp(self.speed, self._k_i[0], self._k_i[1]) return interp(self.speed, self._k_i[0], self._k_i[1])
@property
def k_d(self):
return interp(self.speed, self._k_d[0], self._k_d[1])
@property @property
def error_integral(self): def error_integral(self):
return self.i/self.k_i return self.i/self.k_i
@ -35,24 +45,29 @@ class PIDController:
def reset(self): def reset(self):
self.p = 0.0 self.p = 0.0
self.i = 0.0 self.i = 0.0
self.d = 0.0
self.f = 0.0
self.control = 0 self.control = 0
def update(self, error, speed=0.0, freeze_integrator=False): def update(self, error, error_rate=0.0, speed=0.0, override=False, feedforward=0., freeze_integrator=False):
self.speed = speed self.speed = speed
self.p = float(error) * self.k_p self.p = float(error) * self.k_p
self.f = feedforward * self.k_f
self.d = error_rate * self.k_d
i = self.i + error * self.k_i * self.i_rate if override:
control = self.p + i self.i -= self.i_unwind_rate * float(np.sign(self.i))
else:
if not freeze_integrator:
self.i = self.i + error * self.k_i * self.i_rate
# Update when changing i will move the control away from the limits # Clip i to prevent exceeding control limits
# or when i will move towards the sign of the error control_no_i = self.p + self.d + self.f
if ((error >= 0 and (control <= self.pos_limit or i < 0.0)) or control_no_i = clip(control_no_i, self.neg_limit, self.pos_limit)
(error <= 0 and (control >= self.neg_limit or i > 0.0))) and \ self.i = clip(self.i, self.neg_limit - control_no_i, self.pos_limit - control_no_i)
not freeze_integrator:
self.i = i
control = self.p + self.i control = self.p + self.i + self.d + self.f
self.control = clip(control, self.neg_limit, self.pos_limit) self.control = clip(control, self.neg_limit, self.pos_limit)
return self.control return self.control