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

use standstill, permit braking uses un rate limited request

This commit is contained in:
Shane Smiskol 2024-11-12 21:52:55 -08:00
parent f5aa4c6d04
commit 78b78eb257
1 changed files with 6 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -64,7 +64,7 @@ class CarController(CarControllerBase):
lat_active = CC.latActive and abs(CS.out.steeringTorque) < MAX_USER_TORQUE
# update pitch within sane bounds
if abs(CS.out.aEgo) < 0.5 and abs(CS.out.vEgo) > 0.2 and len(CC.orientationNED) == 3:
if abs(CS.out.aEgo) < 0.5 and not CS.out.standstill and len(CC.orientationNED) == 3:
self.pitch.update(CC.orientationNED[1])
# *** control msgs ***
@ -179,7 +179,7 @@ class CarController(CarControllerBase):
pcm_accel_cmd = min(actuators.accel, self.accel + ACCEL_WINDUP_LIMIT / 3) 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
accel_due_to_pitch = math.sin(self.pitch.x) * ACCELERATION_DUE_TO_GRAVITY
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
@ -200,10 +200,11 @@ class CarController(CarControllerBase):
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:
# Consider the net acceleration request that the PCM should be applying before rate limiting (pitch included)
net_acceleration_request_raw = actuators.accel + accel_due_to_pitch
if net_acceleration_request_raw < 0.1 or stopping or not CC.longActive:
self.permit_braking = True
elif net_acceleration_request > 0.2:
elif net_acceleration_request_raw > 0.2:
self.permit_braking = False
pcm_accel_cmd = clip(pcm_accel_cmd, self.params.ACCEL_MIN, self.params.ACCEL_MAX)