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

Toyota: more responsive start from stop (#1464) 

PERMIT_BRAKING is for the people
This commit is contained in:
Shane Smiskol 2024-11-08 23:03:25 -06:00 committed by GitHub
parent f1768748c0
commit 9b5f697a1e
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194
1 changed files with 11 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -172,16 +172,12 @@ class CarController(CarControllerBase):
# internal PCM gas command can get stuck unwinding from negative accel so we apply a generous rate limit
pcm_accel_cmd = min(actuators.accel, self.accel + ACCEL_WINDUP_LIMIT) if CC.longActive else 0.0
# calculate amount of acceleration PCM should apply to reach target, given pitch
accel_due_to_pitch = math.sin(CC.orientationNED[1]) * ACCELERATION_DUE_TO_GRAVITY if len(CC.orientationNED) == 3 else 0.0
net_acceleration_request = pcm_accel_cmd + accel_due_to_pitch
# 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:
# calculate amount of acceleration PCM should apply to reach target, given pitch
if len(CC.orientationNED) == 3:
accel_due_to_pitch = math.sin(CC.orientationNED[1]) * ACCELERATION_DUE_TO_GRAVITY
else:
accel_due_to_pitch = 0.0
net_acceleration_request = pcm_accel_cmd + accel_due_to_pitch
# let PCM handle stopping for now
pcm_accel_compensation = 0.0
if not stopping:
@ -194,16 +190,17 @@ class CarController(CarControllerBase):
self.pcm_accel_compensation = rate_limit(pcm_accel_compensation, self.pcm_accel_compensation, -0.03, 0.03)
pcm_accel_cmd = pcm_accel_cmd - self.pcm_accel_compensation
# Along with rate limiting positive jerk above, this greatly improves gas response time
# Consider the net acceleration request that the PCM should be applying (pitch included)
if net_acceleration_request < 0.1 or stopping:
self.permit_braking = True
elif net_acceleration_request > 0.2:
self.permit_braking = False
else:
self.pcm_accel_compensation = 0.0
self.permit_braking = True
# Along with rate limiting positive jerk above, this greatly improves gas response time
# Consider the net acceleration request that the PCM should be applying (pitch included)
if net_acceleration_request < 0.1 or stopping or not CC.longActive:
self.permit_braking = True
elif net_acceleration_request > 0.2:
self.permit_braking = False
pcm_accel_cmd = clip(pcm_accel_cmd, self.params.ACCEL_MIN, self.params.ACCEL_MAX)
can_sends.append(toyotacan.create_accel_command(self.packer, pcm_accel_cmd, pcm_cancel_cmd, self.permit_braking, self.standstill_req, lead,