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

Toyota: prevent overshoot w/ new tune (#1515) 

* use filter

* fix

* try pid

* Revert "try pid"

This reverts commit f47acb0b2c.

* improve start from stop

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

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

@ -50,6 +50,7 @@ class CarController(CarControllerBase):
self.distance_button = 0
self.pitch = FirstOrderFilter(0, 0.5, DT_CTRL)
self.net_acceleration_request = FirstOrderFilter(0, 0.15, DT_CTRL * 3)
self.pcm_accel_compensation = FirstOrderFilter(0, 0.5, DT_CTRL * 3)
@ -199,6 +200,7 @@ class CarController(CarControllerBase):
# calculate amount of acceleration PCM should apply to reach target, given pitch
accel_due_to_pitch = math.sin(self.pitch.x) * ACCELERATION_DUE_TO_GRAVITY
net_acceleration_request = pcm_accel_cmd + accel_due_to_pitch
self.net_acceleration_request.update(net_acceleration_request)
# For cars where we allow a higher max acceleration of 2.0 m/s^2, compensate for PCM request overshoot and imprecise braking
if self.CP.flags & ToyotaFlags.RAISED_ACCEL_LIMIT and CC.longActive and not CS.out.cruiseState.standstill:
@ -213,10 +215,10 @@ class CarController(CarControllerBase):
else:
new_pcm_accel_net -= self.pcm_accel_net_offset.update((self.pcm_accel_net.x - accel_due_to_pitch) - CS.out.aEgo)
# let PCM handle stopping for now
# 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 - net_acceleration_request)
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,
@ -227,6 +229,7 @@ class CarController(CarControllerBase):
else:
self.pcm_accel_compensation.x = 0.0
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.permit_braking = True