diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py index e0649cd70..53bcfaf4a 100644 --- a/selfdrive/car/toyota/carcontroller.py +++ b/selfdrive/car/toyota/carcontroller.py @@ -6,7 +6,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 @@ -104,12 +104,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.TOYOTA_RAV4, CAR.TOYOTA_RAV4H, CAR.TOYOTA_HIGHLANDER): + if self.CP.carFingerprint == CAR.TOYOTA_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.TOYOTA_COROLLA,): + elif self.CP.carFingerprint == CAR.TOYOTA_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]) @@ -117,6 +117,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 ed6765060..5471d4932 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 @@ -42,8 +42,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): @@ -58,7 +56,6 @@ class CarInterface(CarInterfaceBase): and not (ret.flags & ToyotaFlags.SMART_DSU) if candidate == CAR.TOYOTA_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': @@ -66,14 +63,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.TOYOTA_AVALON, CAR.TOYOTA_AVALON_2019, CAR.TOYOTA_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.TOYOTA_AVALON - elif candidate in (CAR.TOYOTA_RAV4_TSS2, CAR.TOYOTA_RAV4_TSS2_2022, CAR.TOYOTA_RAV4_TSS2_2023): ret.lateralTuning.init('pid') ret.lateralTuning.pid.kiBP = [0.0] @@ -91,15 +82,6 @@ class CarInterface(CarInterfaceBase): ret.lateralTuning.pid.kf = 0.00004 break - elif candidate in (CAR.TOYOTA_CHR, CAR.TOYOTA_CAMRY, CAR.TOYOTA_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. @@ -143,7 +125,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 if candidate in TSS2_CAR or ret.enableGasInterceptor: diff --git a/selfdrive/car/toyota/values.py b/selfdrive/car/toyota/values.py index 2e781ad0a..617b65957 100644 --- a/selfdrive/car/toyota/values.py +++ b/selfdrive/car/toyota/values.py @@ -572,4 +572,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.TOYOTA_PRIUS, CAR.TOYOTA_PRIUS_V, CAR.TOYOTA_RAV4H, CAR.LEXUS_RX, CAR.TOYOTA_CHR, CAR.TOYOTA_CAMRY, CAR.TOYOTA_HIGHLANDER, + CAR.TOYOTA_SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_NX, CAR.TOYOTA_MIRAI, CAR.TOYOTA_AVALON_2019} + DBC = CAR.create_dbc_map()