From 24e8e091e3a30fa8f8cc8bb3d90a63e1a1d00708 Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Fri, 20 Sep 2024 20:14:24 -0700 Subject: [PATCH 1/3] Toyota PCM compensation: don't compensate in standstill (#1262) don't compensate if in standstill --- opendbc/car/toyota/carcontroller.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/opendbc/car/toyota/carcontroller.py b/opendbc/car/toyota/carcontroller.py index b7fde5b6c9..ee85edd049 100644 --- a/opendbc/car/toyota/carcontroller.py +++ b/opendbc/car/toyota/carcontroller.py @@ -106,7 +106,7 @@ def update(self, CC, CS, now_nanos): # *** gas and brake *** # For cars where we allow a higher max acceleration of 2.0 m/s^2, compensate for PCM request overshoot # TODO: validate PCM_CRUISE->ACCEL_NET for braking requests and compensate for imprecise braking as well - if self.CP.flags & ToyotaFlags.RAISED_ACCEL_LIMIT and CC.longActive: + 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 accel_due_to_pitch = math.sin(CS.slope_angle) * ACCELERATION_DUE_TO_GRAVITY net_acceleration_request = actuators.accel + accel_due_to_pitch From 4b7c90dadaa84d70d209d1b572febf1df9ec15b2 Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Fri, 20 Sep 2024 20:42:31 -0700 Subject: [PATCH 2/3] Lexus ES TSS2: reduce braking overshoot (#1261) * construct final CS.pcm_accel_net * compensate both ways (simpler) compensate both ways (simpler) * comment * cmt * todo --- opendbc/car/toyota/carcontroller.py | 10 +++++----- opendbc/car/toyota/carstate.py | 6 +++++- 2 files changed, 10 insertions(+), 6 deletions(-) diff --git a/opendbc/car/toyota/carcontroller.py b/opendbc/car/toyota/carcontroller.py index ee85edd049..89a31941dd 100644 --- a/opendbc/car/toyota/carcontroller.py +++ b/opendbc/car/toyota/carcontroller.py @@ -104,18 +104,18 @@ def update(self, CC, CS, now_nanos): lta_active, self.frame // 2, torque_wind_down)) # *** gas and brake *** - # For cars where we allow a higher max acceleration of 2.0 m/s^2, compensate for PCM request overshoot - # TODO: validate PCM_CRUISE->ACCEL_NET for braking requests and compensate for imprecise braking as well + # For cars where we allow a higher max acceleration of 2.0 m/s^2, compensate for PCM request overshoot and imprecise braking + # TODO: sometimes when switching from brake to gas quickly, CLUTCH->ACCEL_NET shows a slow unwind. make it go to 0 immediately 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 accel_due_to_pitch = math.sin(CS.slope_angle) * ACCELERATION_DUE_TO_GRAVITY net_acceleration_request = actuators.accel + accel_due_to_pitch - pcm_accel_compensation = 2.0 * (CS.pcm_accel_net - net_acceleration_request) if net_acceleration_request > 0 else 0.0 + pcm_accel_compensation = 2.0 * (CS.pcm_accel_net - net_acceleration_request) # prevent compensation windup - if actuators.accel - pcm_accel_compensation > self.params.ACCEL_MAX: - pcm_accel_compensation = actuators.accel - self.params.ACCEL_MAX + pcm_accel_compensation = clip(pcm_accel_compensation, actuators.accel - self.params.ACCEL_MAX, + actuators.accel - self.params.ACCEL_MIN) 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 diff --git a/opendbc/car/toyota/carstate.py b/opendbc/car/toyota/carstate.py index 9bad3ce404..4df37d1158 100644 --- a/opendbc/car/toyota/carstate.py +++ b/opendbc/car/toyota/carstate.py @@ -55,8 +55,12 @@ def update(self, cp, cp_cam, *_) -> structs.CarState: # Describes the acceleration request from the PCM if on flat ground, may be higher or lower if pitched # CLUTCH->ACCEL_NET is only accurate for gas, PCM_CRUISE->ACCEL_NET is only accurate for brake + # These signals only have meaning when ACC is active if self.CP.flags & ToyotaFlags.RAISED_ACCEL_LIMIT: - self.pcm_accel_net = cp.vl["CLUTCH"]["ACCEL_NET"] # - cp.vl["PCM_CRUISE"]["ACCEL_NET"] + # Sometimes ACC_BRAKING can be 1 while showing we're applying gas already + self.pcm_accel_net = max(cp.vl["CLUTCH"]["ACCEL_NET"], 0.0) + if cp.vl["PCM_CRUISE"]["ACC_BRAKING"]: + self.pcm_accel_net += min(cp.vl["PCM_CRUISE"]["ACCEL_NET"], 0.0) # filtered pitch estimate from the car, negative is a downward slope self.slope_angle = cp.vl["VSC1S07"]["ASLP"] * CV.DEG_TO_RAD From 0b7648ad2cb048b5f55a338dbd26cada01671124 Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Fri, 20 Sep 2024 20:46:59 -0700 Subject: [PATCH 3/3] Toyota: don't apply PCM compensation in the stopping state (#1264) check inside so we ramp down smoothly not cut out compensation --- opendbc/car/toyota/carcontroller.py | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/opendbc/car/toyota/carcontroller.py b/opendbc/car/toyota/carcontroller.py index 89a31941dd..d2ab902faf 100644 --- a/opendbc/car/toyota/carcontroller.py +++ b/opendbc/car/toyota/carcontroller.py @@ -10,6 +10,7 @@ UNSUPPORTED_DSU_CAR from opendbc.can.packer import CANPacker +LongCtrlState = structs.CarControl.Actuators.LongControlState SteerControlType = structs.CarParams.SteerControlType VisualAlert = structs.CarControl.HUDControl.VisualAlert @@ -111,7 +112,10 @@ def update(self, CC, CS, now_nanos): accel_due_to_pitch = math.sin(CS.slope_angle) * ACCELERATION_DUE_TO_GRAVITY net_acceleration_request = actuators.accel + accel_due_to_pitch - pcm_accel_compensation = 2.0 * (CS.pcm_accel_net - net_acceleration_request) + # let PCM handle stopping for now + pcm_accel_compensation = 0.0 + if actuators.longControlState != LongCtrlState.stopping: + pcm_accel_compensation = 2.0 * (CS.pcm_accel_net - net_acceleration_request) # prevent compensation windup pcm_accel_compensation = clip(pcm_accel_compensation, actuators.accel - self.params.ACCEL_MAX,