toyota pedal

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

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),

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

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