From 1f84395539d45bac5aa9bbc921a45872e11ffcbb Mon Sep 17 00:00:00 2001 From: firestar5683 <168790843+firestar5683@users.noreply.github.com> Date: Tue, 19 May 2026 12:42:41 -0500 Subject: [PATCH] toyota pedal --- .../opendbc/car/toyota/carcontroller.py | 28 +++++++++++- opendbc_repo/opendbc/car/toyota/carstate.py | 9 +++- opendbc_repo/opendbc/car/toyota/interface.py | 5 ++- .../opendbc/car/toyota/tests/test_toyota.py | 45 ++++++++++++++++++- 4 files changed, 81 insertions(+), 6 deletions(-) diff --git a/opendbc_repo/opendbc/car/toyota/carcontroller.py b/opendbc_repo/opendbc/car/toyota/carcontroller.py index 4c1e6b18f..f96eaa904 100644 --- a/opendbc_repo/opendbc/car/toyota/carcontroller.py +++ b/opendbc_repo/opendbc/car/toyota/carcontroller.py @@ -1,6 +1,6 @@ import math import numpy as np -from opendbc.car import Bus, make_tester_present_msg, rate_limit, structs, ACCELERATION_DUE_TO_GRAVITY, DT_CTRL +from opendbc.car import Bus, create_gas_interceptor_command, make_tester_present_msg, rate_limit, structs, ACCELERATION_DUE_TO_GRAVITY, DT_CTRL from opendbc.car.lateral import apply_meas_steer_torque_limits, apply_std_steer_angle_limits, common_fault_avoidance from opendbc.car.can_definitions import CanData from opendbc.car.carlog import carlog @@ -9,7 +9,7 @@ from opendbc.car.common.pid import PIDController from opendbc.car.secoc import add_mac, build_sync_mac from opendbc.car.interfaces import CarControllerBase from opendbc.car.toyota import toyotacan -from opendbc.car.toyota.values import CAR, NO_STOP_TIMER_CAR, TSS2_CAR, \ +from opendbc.car.toyota.values import CAR, MIN_ACC_SPEED, NO_STOP_TIMER_CAR, PEDAL_TRANSITION, TSS2_CAR, \ CarControllerParams, ToyotaFlags, \ UNSUPPORTED_DSU_CAR from opendbc.can import CANPacker @@ -121,6 +121,25 @@ class CarController(CarControllerBase): self.doors_locked = False + def _compute_interceptor_gas_cmd(self, CC, CS): + if not (self.CP.enableGasInterceptorDEPRECATED and self.CP.openpilotLongitudinalControl and CC.longActive): + return 0.0 + + if self.CP.minEnableSpeed < 0.0: + return 0.12 if CS.out.standstill and self.accel > 0.0 else 0.0 + + max_interceptor_gas = 0.5 + if self.CP.carFingerprint == CAR.TOYOTA_RAV4: + pedal_scale = float(np.interp(CS.out.vEgo, [0.0, MIN_ACC_SPEED, MIN_ACC_SPEED + PEDAL_TRANSITION], [0.15, 0.3, 0.0])) + elif self.CP.carFingerprint == CAR.TOYOTA_COROLLA: + pedal_scale = float(np.interp(CS.out.vEgo, [0.0, MIN_ACC_SPEED, MIN_ACC_SPEED + PEDAL_TRANSITION], [0.3, 0.4, 0.0])) + else: + pedal_scale = float(np.interp(CS.out.vEgo, [0.0, MIN_ACC_SPEED, MIN_ACC_SPEED + PEDAL_TRANSITION], [0.4, 0.5, 0.0])) + + pedal_offset = float(np.interp(CS.out.vEgo, [0.0, 2.3, MIN_ACC_SPEED + PEDAL_TRANSITION], [-0.4, 0.0, 0.2])) + pedal_command = pedal_scale * (self.accel + pedal_offset) + return float(np.clip(pedal_command, 0.0, max_interceptor_gas)) + def _update_standstill_request(self, CC, CS, actuators, starpilot_toggles): # Older TSS-P platforms need a standstill latch pulse, then an explicit release to move again. if self.CP.carFingerprint not in NO_STOP_TIMER_CAR: @@ -239,12 +258,17 @@ class CarController(CarControllerBase): # *** gas and brake *** self._update_standstill_request(CC, CS, actuators, starpilot_toggles) + interceptor_gas_cmd = self._compute_interceptor_gas_cmd(CC, CS) # handle UI messages fcw_alert = hud_control.visualAlert == VisualAlert.fcw steer_alert = hud_control.visualAlert in (VisualAlert.steerRequired, VisualAlert.ldw) lead = hud_control.leadVisible or CS.out.vEgo < 12. # at low speed we always assume the lead is present so ACC can be engaged + if self.frame % 2 == 0 and self.CP.enableGasInterceptorDEPRECATED and self.CP.openpilotLongitudinalControl: + # Send an explicit zero when off so the interceptor does not rescale around a stale command. + can_sends.append(create_gas_interceptor_command(self.packer, interceptor_gas_cmd, self.frame // 2)) + if self.CP.openpilotLongitudinalControl: if self.frame % 3 == 0: # Press distance button until we are at the correct bar length. Only change while enabled to avoid skipping startup popup diff --git a/opendbc_repo/opendbc/car/toyota/carstate.py b/opendbc_repo/opendbc/car/toyota/carstate.py index 2af378a19..596ea50d0 100644 --- a/opendbc_repo/opendbc/car/toyota/carstate.py +++ b/opendbc_repo/opendbc/car/toyota/carstate.py @@ -99,7 +99,11 @@ class CarState(CarStateBase): ret.gasPressed = cp.vl["GAS_PEDAL"]["GAS_PEDAL_USER"] > 0 can_gear = int(cp.vl["GEAR_PACKET_HYBRID"]["GEAR"]) else: - ret.gasPressed = cp.vl["PCM_CRUISE"]["GAS_RELEASED"] == 0 # TODO: these also have GAS_PEDAL, come back and unify + if self.CP.enableGasInterceptorDEPRECATED: + ret.gas = (cp.vl["GAS_SENSOR"]["INTERCEPTOR_GAS"] + cp.vl["GAS_SENSOR"]["INTERCEPTOR_GAS2"]) / 2 + ret.gasPressed = ret.gas > 805 + else: + ret.gasPressed = cp.vl["PCM_CRUISE"]["GAS_RELEASED"] == 0 # TODO: these also have GAS_PEDAL, come back and unify can_gear = int(cp.vl["GEAR_PACKET"]["GEAR"]) if not self.CP.flags & ToyotaFlags.DISABLE_RADAR.value: ret.stockAeb = bool(cp_acc.vl["PRE_COLLISION"]["PRECOLLISION_ACTIVE"] and cp_acc.vl["PRE_COLLISION"]["FORCE"] < -1e-5) @@ -269,6 +273,9 @@ class CarState(CarStateBase): ("BLINKERS_STATE", float('nan')), ] + if CP.enableGasInterceptorDEPRECATED: + pt_messages.append(("GAS_SENSOR", 50)) + return { Bus.pt: CANParser(DBC[CP.carFingerprint][Bus.pt], pt_messages, 0), Bus.cam: CANParser(DBC[CP.carFingerprint][Bus.pt], [], 2), diff --git a/opendbc_repo/opendbc/car/toyota/interface.py b/opendbc_repo/opendbc/car/toyota/interface.py index 09dc9679b..7c30138fa 100644 --- a/opendbc_repo/opendbc/car/toyota/interface.py +++ b/opendbc_repo/opendbc/car/toyota/interface.py @@ -136,15 +136,16 @@ class CarInterface(CarInterfaceBase): bool(ret.flags & ToyotaFlags.DISABLE_RADAR.value)) ret.autoResumeSng = ret.openpilotLongitudinalControl and candidate in NO_STOP_TIMER_CAR + ret.enableGasInterceptorDEPRECATED = 0x201 in fingerprint[0] and ret.openpilotLongitudinalControl if not ret.openpilotLongitudinalControl: ret.safetyConfigs[0].safetyParam |= ToyotaSafetyFlags.STOCK_LONGITUDINAL.value # 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 else MIN_ACC_SPEED + ret.minEnableSpeed = -1. if (stop_and_go or ret.enableGasInterceptorDEPRECATED) else MIN_ACC_SPEED - if candidate in TSS2_CAR: + if candidate in TSS2_CAR or ret.enableGasInterceptorDEPRECATED: ret.flags |= ToyotaFlags.RAISED_ACCEL_LIMIT.value ret.vEgoStopping = 0.25 diff --git a/opendbc_repo/opendbc/car/toyota/tests/test_toyota.py b/opendbc_repo/opendbc/car/toyota/tests/test_toyota.py index 51ff27de9..e2d8c713c 100644 --- a/opendbc_repo/opendbc/car/toyota/tests/test_toyota.py +++ b/opendbc_repo/opendbc/car/toyota/tests/test_toyota.py @@ -174,9 +174,15 @@ class TestToyotaCarController: @staticmethod def _make_controller(*, standstill_req=False, last_standstill=False): controller = CarController.__new__(CarController) - controller.CP = SimpleNamespace(carFingerprint=CAR.TOYOTA_PRIUS) + controller.CP = SimpleNamespace( + carFingerprint=CAR.TOYOTA_PRIUS, + enableGasInterceptorDEPRECATED=False, + openpilotLongitudinalControl=True, + minEnableSpeed=-1.0, + ) controller.standstill_req = standstill_req controller.last_standstill = last_standstill + controller.accel = 0.0 return controller @staticmethod @@ -253,3 +259,40 @@ class TestToyotaCarController: ) assert controller.standstill_req is False + + def test_interceptor_stop_and_go_holds_small_launch_at_standstill(self): + controller = self._make_controller() + controller.CP.enableGasInterceptorDEPRECATED = True + controller.accel = 0.3 + + gas_cmd = controller._compute_interceptor_gas_cmd( + SimpleNamespace(longActive=True), + SimpleNamespace(out=SimpleNamespace(standstill=True, vEgo=0.0)), + ) + + assert gas_cmd == 0.12 + + def test_interceptor_non_stop_and_go_scales_with_accel_request(self): + controller = self._make_controller() + controller.CP.enableGasInterceptorDEPRECATED = True + controller.CP.carFingerprint = CAR.TOYOTA_AVALON_2019 + controller.CP.minEnableSpeed = 8.5 + controller.accel = 0.8 + + gas_cmd = controller._compute_interceptor_gas_cmd( + SimpleNamespace(longActive=True), + SimpleNamespace(out=SimpleNamespace(standstill=False, vEgo=8.0)), + ) + + assert 0.0 < gas_cmd <= 0.5 + + def test_interceptor_disabled_returns_zero(self): + controller = self._make_controller() + controller.accel = 1.0 + + gas_cmd = controller._compute_interceptor_gas_cmd( + SimpleNamespace(longActive=True), + SimpleNamespace(out=SimpleNamespace(standstill=False, vEgo=8.0)), + ) + + assert gas_cmd == 0.0