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

Toyota: use a PID for PCM compensation (#1517) 

* pid

* add pid
This commit is contained in:
Shane Smiskol 2024-11-21 21:01:38 -06:00 committed by GitHub
parent 70009fed0c
commit b4e8a4970f
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194
1 changed files with 9 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
opendbc/car/toyota/carcontroller.py
View File

@ -4,6 +4,7 @@ from opendbc.car import Bus, carlog, apply_meas_steer_torque_limits, apply_std_s
from opendbc.car.can_definitions import CanData
from opendbc.car.common.filter_simple import FirstOrderFilter
from opendbc.car.common.numpy_fast import clip
from opendbc.car.common.pid import PIDController
from opendbc.car.secoc import add_mac, build_sync_mac
from opendbc.car.interfaces import CarControllerBase
from opendbc.car.toyota import toyotacan
@ -52,6 +53,7 @@ class CarController(CarControllerBase):
self.pitch = FirstOrderFilter(0, 0.5, DT_CTRL)
self.net_acceleration_request = FirstOrderFilter(0, 0.15, DT_CTRL * 3)
self.accel_pid = PIDController(2.0, 0.5, 1 / (DT_CTRL * 3))
self.pcm_accel_compensation = FirstOrderFilter(0, 0.5, DT_CTRL * 3)
# the PCM's reported acceleration request can sometimes mismatch aEgo, close the loop
@ -218,11 +220,12 @@ class CarController(CarControllerBase):
# let PCM handle stopping for now, error correct on a delayed acceleration request
pcm_accel_compensation = 0.0
if not stopping:
pcm_accel_compensation = 2.0 * (new_pcm_accel_net - self.net_acceleration_request.x)
# prevent compensation windup
pcm_accel_compensation = clip(pcm_accel_compensation, pcm_accel_cmd - self.params.ACCEL_MAX,
pcm_accel_cmd - self.params.ACCEL_MIN)
# prevent compensation windup
self.accel_pid.neg_limit = pcm_accel_cmd - self.params.ACCEL_MAX
self.accel_pid.pos_limit = pcm_accel_cmd - self.params.ACCEL_MIN
pcm_accel_compensation = self.accel_pid.update(new_pcm_accel_net - self.net_acceleration_request.x)
else:
self.accel_pid.reset()
pcm_accel_cmd = pcm_accel_cmd - self.pcm_accel_compensation.update(pcm_accel_compensation)
@ -231,6 +234,7 @@ class CarController(CarControllerBase):
self.pcm_accel_net_offset.x = 0.0
self.net_acceleration_request.x = 0.0
self.pcm_accel_net.x = CS.pcm_accel_net
self.accel_pid.reset()
self.permit_braking = True
# Along with rate limiting positive jerk above, this greatly improves gas response time