From cf9f10a16da05be39ccb5507de3083a80ce3e909 Mon Sep 17 00:00:00 2001 From: rav4kumar Date: Sat, 28 Sep 2024 05:28:36 -0700 Subject: [PATCH] Re-apply "Lexus ES TSS2: improve start from stop response time" (#1303) Beta --- selfdrive/car/toyota/carcontroller.py | 13 ++++++++++--- selfdrive/car/toyota/toyotacan.py | 4 ++-- 2 files changed, 12 insertions(+), 5 deletions(-) diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py index a22b013ef5..6793361855 100644 --- a/selfdrive/car/toyota/carcontroller.py +++ b/selfdrive/car/toyota/carcontroller.py @@ -53,6 +53,7 @@ class CarController(CarControllerBase): self.standstill_req = False self.steer_rate_counter = 0 self.pcm_accel_compensation = 0.0 + self.permit_braking = 0.0 self.distance_button = 0 #self.pid = PIDController(k_p=1.0, k_i=0.25, k_f=0) @@ -168,9 +169,16 @@ class CarController(CarControllerBase): self.pcm_accel_compensation = rate_limit(pcm_accel_compensation, self.pcm_accel_compensation, -0.01, 0.01) pcm_accel_cmd = actuators.accel - self.pcm_accel_compensation + # Along with rate limiting positive jerk below, 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: + self.permit_braking = True + elif net_acceleration_request > 0.2: + self.permit_braking = False else: self.pcm_accel_compensation = 0.0 pcm_accel_cmd = actuators.accel + self.permit_braking = True pcm_accel_cmd = clip(pcm_accel_cmd, self.params.ACCEL_MIN, self.params.ACCEL_MAX) @@ -236,11 +244,10 @@ 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(pcm_accel_cmd, self.accel + ACCEL_WINDUP_LIMIT) if CC.longActive else 0.0 - can_sends.append(toyotacan.create_accel_command(self.packer, pcm_accel_cmd, pcm_cancel_cmd, self.standstill_req, lead, CS.acc_type, fcw_alert, - self.distance_button, reverse_acc)) + can_sends.append(toyotacan.create_accel_command(self.packer, pcm_accel_cmd, pcm_cancel_cmd, self.permit_braking, self.standstill_req, lead, CS.acc_type, fcw_alert, self.distance_button, reverse_acc)) self.accel = pcm_accel_cmd else: - can_sends.append(toyotacan.create_accel_command(self.packer, 0, pcm_cancel_cmd, False, lead, CS.acc_type, False, self.distance_button, reverse_acc)) + can_sends.append(toyotacan.create_accel_command(self.packer, 0, pcm_cancel_cmd, True, False, lead, CS.acc_type, False, self.distance_button, reverse_acc)) if self.frame % 2 == 0 and self.CP.enableGasInterceptorDEPRECATED and self.CP.openpilotLongitudinalControl: # send exactly zero if gas cmd is zero. Interceptor will send the max between read value and gas cmd. diff --git a/selfdrive/car/toyota/toyotacan.py b/selfdrive/car/toyota/toyotacan.py index 979879e368..bc680eb4f0 100644 --- a/selfdrive/car/toyota/toyotacan.py +++ b/selfdrive/car/toyota/toyotacan.py @@ -34,14 +34,14 @@ def create_lta_steer_command(packer, steer_control_type, steer_angle, steer_req, return packer.make_can_msg("STEERING_LTA", 0, values) -def create_accel_command(packer, accel, pcm_cancel, standstill_req, lead, acc_type, fcw_alert, distance, reverse_acc): +def create_accel_command(packer, accel, pcm_cancel, permit_braking, standstill_req, lead, acc_type, fcw_alert, distance, reverse_acc): # TODO: find the exact canceling bit that does not create a chime values = { "ACCEL_CMD": 0 if pcm_cancel else accel, "ACC_TYPE": acc_type, "DISTANCE": distance, "MINI_CAR": lead, - "PERMIT_BRAKING": 1, + "PERMIT_BRAKING": permit_braking, "RELEASE_STANDSTILL": not standstill_req, "CANCEL_REQ": pcm_cancel, "ALLOW_LONG_PRESS": reverse_acc,