diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py index 0d60708..d92539c 100644 --- a/selfdrive/car/toyota/carcontroller.py +++ b/selfdrive/car/toyota/carcontroller.py @@ -7,7 +7,7 @@ from openpilot.selfdrive.car.interfaces import CarControllerBase 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 @@ -105,12 +105,12 @@ class CarController(CarControllerBase): 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]) @@ -118,6 +118,8 @@ class CarController(CarControllerBase): 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 self.CP.enableGasInterceptor and CC.longActive and self.CP.carFingerprint in STOP_AND_GO_CAR and actuators.accel > 0.0: + interceptor_gas_cmd = 0.12 if CS.out.standstill else 0. else: interceptor_gas_cmd = 0. pcm_accel_cmd = clip(actuators.accel, self.params.ACCEL_MIN, self.params.ACCEL_MAX) diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py index b6582ce..52ff134 100644 --- a/selfdrive/car/toyota/interface.py +++ b/selfdrive/car/toyota/interface.py @@ -2,7 +2,7 @@ from cereal import car, custom 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 create_button_events, get_safety_config from openpilot.selfdrive.car.disable_ecu import disable_ecu from openpilot.selfdrive.car.interfaces import CarInterfaceBase @@ -46,8 +46,6 @@ class CarInterface(CarInterfaceBase): ret.stoppingControl = False # Toyota starts braking more when it thinks you want to stop - stop_and_go = candidate in TSS2_CAR - # Detect smartDSU, which intercepts ACC_CMD from the DSU (or radar) allowing openpilot to send it # 0x2AA is sent by a similar device which intercepts the radar instead of DSU on NO_DSU_CARs if 0x2FF in fingerprint[0] or (0x2AA in fingerprint[0] and candidate in NO_DSU_CAR): @@ -67,7 +65,6 @@ class CarInterface(CarInterfaceBase): and not (ret.flags & ToyotaFlags.SMART_DSU) if candidate == CAR.PRIUS: - stop_and_go = True # Only give steer angle deadzone to for bad angle sensor prius for fw in car_fw: if fw.ecu == "eps" and not fw.fwVersion == b'8965B47060\x00\x00\x00\x00\x00\x00': @@ -75,14 +72,8 @@ class CarInterface(CarInterfaceBase): CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning, steering_angle_deadzone_deg=0.2) elif candidate in (CAR.LEXUS_RX, CAR.LEXUS_RX_TSS2): - stop_and_go = True ret.wheelSpeedFactor = 1.035 - 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 - elif candidate in (CAR.RAV4_TSS2, CAR.RAV4_TSS2_2022, CAR.RAV4_TSS2_2023): ret.lateralTuning.init('pid') ret.lateralTuning.pid.kiBP = [0.0] @@ -100,15 +91,6 @@ class CarInterface(CarInterfaceBase): ret.lateralTuning.pid.kf = 0.00004 break - elif candidate in (CAR.RAV4H, CAR.CHR, CAR.CAMRY, CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_NX): - # TODO: Some of these platforms are not advertised to have full range ACC, are they similar to SNG_WITHOUT_DSU cars? - stop_and_go = True - - # TODO: these models can do stop and go, but unclear if it requires sDSU or unplugging DSU. - # For now, don't list stop and go functionality in the docs - if ret.flags & ToyotaFlags.SNG_WITHOUT_DSU: - stop_and_go = stop_and_go or bool(ret.flags & ToyotaFlags.SMART_DSU.value) or (ret.enableDsu and not docs) - ret.centerToFront = ret.wheelbase * 0.44 # TODO: Some TSS-P platforms have BSM, but are flipped based on region or driving direction. @@ -153,7 +135,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 6b26e0e..9ad1896 100644 --- a/selfdrive/car/toyota/values.py +++ b/selfdrive/car/toyota/values.py @@ -612,4 +612,7 @@ ANGLE_CONTROL_CAR = CAR.with_flags(ToyotaFlags.ANGLE_CONTROL) # no resume button press required NO_STOP_TIMER_CAR = CAR.with_flags(ToyotaFlags.NO_STOP_TIMER) +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} + DBC = CAR.create_dbc_map()