From 6fe645e21c0f68e378cdfa668f5909b4e6fdf59c Mon Sep 17 00:00:00 2001 From: FrogAi <91348155+FrogAi@users.noreply.github.com> Date: Tue, 27 Feb 2024 16:34:47 -0700 Subject: [PATCH] Toyota: Improve comma pedal Stop-and-Go Co-Authored-By: Irene <12470297+cydia2020@users.noreply.github.com> --- selfdrive/car/toyota/carcontroller.py | 11 +++++++---- selfdrive/car/toyota/carstate.py | 11 ++++++++++- selfdrive/car/toyota/interface.py | 16 ++-------------- selfdrive/car/toyota/values.py | 4 ++++ 4 files changed, 23 insertions(+), 19 deletions(-) diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py index bea9269..9ebf54e 100644 --- a/selfdrive/car/toyota/carcontroller.py +++ b/selfdrive/car/toyota/carcontroller.py @@ -6,7 +6,7 @@ from openpilot.selfdrive.car import apply_meas_steer_torque_limits, apply_std_st from openpilot.selfdrive.car.toyota import toyotacan from openpilot.selfdrive.car.toyota.values import CAR, STATIC_DSU_MSGS, NO_STOP_TIMER_CAR, TSS2_CAR, \ MIN_ACC_SPEED, PEDAL_TRANSITION, CarControllerParams, ToyotaFlags, \ - UNSUPPORTED_DSU_CAR + UNSUPPORTED_DSU_CAR, STOP_AND_GO_CAR from opendbc.can.packer import CANPacker SteerControlType = car.CarParams.SteerControlType @@ -123,12 +123,12 @@ class CarController: lta_active, self.frame // 2, torque_wind_down)) # *** gas and brake *** - if self.CP.enableGasInterceptor and CC.longActive: + if self.CP.enableGasInterceptor and CC.longActive and self.CP.carFingerprint not in STOP_AND_GO_CAR: MAX_INTERCEPTOR_GAS = 0.5 # RAV4 has very sensitive gas pedal - if self.CP.carFingerprint in (CAR.RAV4, CAR.RAV4H, CAR.HIGHLANDER): + if self.CP.carFingerprint == CAR.RAV4: PEDAL_SCALE = interp(CS.out.vEgo, [0.0, MIN_ACC_SPEED, MIN_ACC_SPEED + PEDAL_TRANSITION], [0.15, 0.3, 0.0]) - elif self.CP.carFingerprint in (CAR.COROLLA,): + elif self.CP.carFingerprint == CAR.COROLLA: PEDAL_SCALE = interp(CS.out.vEgo, [0.0, MIN_ACC_SPEED, MIN_ACC_SPEED + PEDAL_TRANSITION], [0.3, 0.4, 0.0]) else: PEDAL_SCALE = interp(CS.out.vEgo, [0.0, MIN_ACC_SPEED, MIN_ACC_SPEED + PEDAL_TRANSITION], [0.4, 0.5, 0.0]) @@ -136,6 +136,9 @@ class CarController: pedal_offset = interp(CS.out.vEgo, [0.0, 2.3, MIN_ACC_SPEED + PEDAL_TRANSITION], [-.4, 0.0, 0.2]) pedal_command = PEDAL_SCALE * (actuators.accel + pedal_offset) interceptor_gas_cmd = clip(pedal_command, 0., MAX_INTERCEPTOR_GAS) + elif ((CC.longActive and actuators.accel > 0.) or (not self.CP.openpilotLongitudinalControl and CS.stock_resume_ready)) \ + and self.CP.carFingerprint in STOP_AND_GO_CAR and self.CP.enableGasInterceptor and CS.out.vEgo < 1e-3: + interceptor_gas_cmd = 0.12 else: interceptor_gas_cmd = 0. diff --git a/selfdrive/car/toyota/carstate.py b/selfdrive/car/toyota/carstate.py index a4f3035..83fc210 100644 --- a/selfdrive/car/toyota/carstate.py +++ b/selfdrive/car/toyota/carstate.py @@ -175,6 +175,12 @@ class CarState(CarStateBase): if self.CP.carFingerprint != CAR.PRIUS_V: self.lkas_hud = copy.copy(cp_cam.vl["LKAS_HUD"]) + # if openpilot does not control longitudinal, in this case, assume 0x343 is on bus0 + # 1) the car is no longer sending standstill + # 2) the car is still in standstill + if not self.CP.openpilotLongitudinalControl: + self.stock_resume_ready = cp.vl["ACC_CONTROL"]["RELEASE_STANDSTILL"] == 1 + # FrogPilot functions if self.CP.carFingerprint in (TSS2_CAR - RADAR_ACC_CAR) or (self.CP.flags & ToyotaFlags.SMART_DSU and not self.CP.flags & ToyotaFlags.RADAR_CAN_FILTER): # distance button is wired to the ACC module (camera or radar) @@ -303,7 +309,7 @@ class CarState(CarStateBase): if CP.enableBsm: messages.append(("BSM", 1)) - if CP.carFingerprint in RADAR_ACC_CAR and not CP.flags & ToyotaFlags.DISABLE_RADAR.value: + if not CP.openpilotLongitudinalControl or (CP.carFingerprint in RADAR_ACC_CAR and not CP.flags & ToyotaFlags.DISABLE_RADAR.value): if not CP.flags & ToyotaFlags.SMART_DSU.value: messages += [ ("ACC_CONTROL", 33), @@ -345,4 +351,7 @@ class CarState(CarStateBase): ("PCS_HUD", 1), ] + if not CP.openpilotLongitudinalControl and CP.carFingerprint not in (TSS2_CAR, UNSUPPORTED_DSU_CAR): + messages.append(("ACC_CONTROL", 33)) + return CANParser(DBC[CP.carFingerprint]["pt"], messages, 2) diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py index d7571a2..e3d1b6d 100644 --- a/selfdrive/car/toyota/interface.py +++ b/selfdrive/car/toyota/interface.py @@ -3,7 +3,7 @@ from openpilot.common.conversions import Conversions as CV from panda import Panda from panda.python import uds from openpilot.selfdrive.car.toyota.values import Ecu, CAR, DBC, ToyotaFlags, CarControllerParams, TSS2_CAR, RADAR_ACC_CAR, NO_DSU_CAR, \ - MIN_ACC_SPEED, EPS_SCALE, UNSUPPORTED_DSU_CAR, NO_STOP_TIMER_CAR, ANGLE_CONTROL_CAR + MIN_ACC_SPEED, EPS_SCALE, UNSUPPORTED_DSU_CAR, NO_STOP_TIMER_CAR, ANGLE_CONTROL_CAR, STOP_AND_GO_CAR from openpilot.selfdrive.car import get_safety_config from openpilot.selfdrive.car.disable_ecu import disable_ecu from openpilot.selfdrive.car.interfaces import CarInterfaceBase @@ -56,7 +56,6 @@ class CarInterface(CarInterfaceBase): ret.flags |= ToyotaFlags.RADAR_CAN_FILTER.value if candidate == CAR.PRIUS: - stop_and_go = True ret.wheelbase = 2.70 ret.steerRatio = 15.74 # unknown end-to-end spec ret.tireStiffnessFactor = 0.6371 # hand-tune @@ -68,14 +67,12 @@ class CarInterface(CarInterfaceBase): CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning, steering_angle_deadzone_deg=0.2) elif candidate == CAR.PRIUS_V: - stop_and_go = True ret.wheelbase = 2.78 ret.steerRatio = 17.4 ret.tireStiffnessFactor = 0.5533 ret.mass = 3340. * CV.LB_TO_KG elif candidate in (CAR.RAV4, CAR.RAV4H): - stop_and_go = True if (candidate in CAR.RAV4H) else False ret.wheelbase = 2.65 ret.steerRatio = 16.88 # 14.5 is spec end-to-end ret.tireStiffnessFactor = 0.5533 @@ -88,7 +85,6 @@ class CarInterface(CarInterfaceBase): ret.mass = 2860. * CV.LB_TO_KG # mean between normal and hybrid elif candidate in (CAR.LEXUS_RX, CAR.LEXUS_RX_TSS2): - stop_and_go = True ret.wheelbase = 2.79 ret.steerRatio = 16. # 14.8 is spec end-to-end ret.wheelSpeedFactor = 1.035 @@ -96,14 +92,12 @@ class CarInterface(CarInterfaceBase): ret.mass = 4481. * CV.LB_TO_KG # mean between min and max elif candidate in (CAR.CHR, CAR.CHR_TSS2): - stop_and_go = True ret.wheelbase = 2.63906 ret.steerRatio = 13.6 ret.tireStiffnessFactor = 0.7933 ret.mass = 3300. * CV.LB_TO_KG elif candidate in (CAR.CAMRY, CAR.CAMRY_TSS2): - stop_and_go = True ret.wheelbase = 2.82448 ret.steerRatio = 13.7 ret.tireStiffnessFactor = 0.7933 @@ -111,7 +105,6 @@ class CarInterface(CarInterfaceBase): elif candidate in (CAR.HIGHLANDER, CAR.HIGHLANDER_TSS2): # TODO: TSS-P models can do stop and go, but unclear if it requires sDSU or unplugging DSU - stop_and_go = True ret.wheelbase = 2.8194 # average of 109.8 and 112.2 in ret.steerRatio = 16.0 ret.tireStiffnessFactor = 0.8 @@ -120,7 +113,6 @@ class CarInterface(CarInterfaceBase): elif candidate in (CAR.AVALON, CAR.AVALON_2019, CAR.AVALON_TSS2): # starting from 2019, all Avalon variants have stop and go # https://engage.toyota.com/static/images/toyota_safety_sense/TSS_Applicability_Chart.pdf - stop_and_go = candidate != CAR.AVALON ret.wheelbase = 2.82 ret.steerRatio = 14.8 # Found at https://pressroom.toyota.com/releases/2016+avalon+product+specs.download ret.tireStiffnessFactor = 0.7983 @@ -160,7 +152,6 @@ class CarInterface(CarInterfaceBase): ret.mass = 3677. * CV.LB_TO_KG # mean between min and max elif candidate == CAR.SIENNA: - stop_and_go = True ret.wheelbase = 3.03 ret.steerRatio = 15.5 ret.tireStiffnessFactor = 0.444 @@ -179,14 +170,12 @@ class CarInterface(CarInterfaceBase): ret.mass = 4034. * CV.LB_TO_KG elif candidate == CAR.LEXUS_CTH: - stop_and_go = True ret.wheelbase = 2.60 ret.steerRatio = 18.6 ret.tireStiffnessFactor = 0.517 ret.mass = 3108 * CV.LB_TO_KG # mean between min and max elif candidate in (CAR.LEXUS_NX, CAR.LEXUS_NX_TSS2): - stop_and_go = True ret.wheelbase = 2.66 ret.steerRatio = 14.7 ret.tireStiffnessFactor = 0.444 # not optimized yet @@ -205,7 +194,6 @@ class CarInterface(CarInterfaceBase): ret.mass = 3115. * CV.LB_TO_KG elif candidate == CAR.MIRAI: - stop_and_go = True ret.wheelbase = 2.91 ret.steerRatio = 14.8 ret.tireStiffnessFactor = 0.8 @@ -271,7 +259,7 @@ class CarInterface(CarInterfaceBase): # min speed to enable ACC. if car can do stop and go, then set enabling speed # to a negative value, so it won't matter. - ret.minEnableSpeed = -1. if (stop_and_go or ret.enableGasInterceptor) else MIN_ACC_SPEED + ret.minEnableSpeed = -1. if (candidate in STOP_AND_GO_CAR or ret.enableGasInterceptor) else MIN_ACC_SPEED tune = ret.longitudinalTuning tune.deadzoneBP = [0., 9.] diff --git a/selfdrive/car/toyota/values.py b/selfdrive/car/toyota/values.py index 97c84a5..8fec1ee 100644 --- a/selfdrive/car/toyota/values.py +++ b/selfdrive/car/toyota/values.py @@ -501,3 +501,7 @@ ANGLE_CONTROL_CAR = {CAR.RAV4_TSS2_2023} # no resume button press required NO_STOP_TIMER_CAR = TSS2_CAR | {CAR.PRIUS_V, CAR.RAV4H, CAR.HIGHLANDER, CAR.SIENNA} + +# stop and go +STOP_AND_GO_CAR = TSS2_CAR | {CAR.PRIUS, CAR.PRIUS_V, CAR.RAV4H, CAR.LEXUS_RX, CAR.CHR, CAR.CAMRY, CAR.HIGHLANDER, + CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_NX, CAR.MIRAI, CAR.AVALON_2019}