toyota

opendbc_repo/opendbc/car/toyota/carcontroller.py

@@ -127,15 +127,19 @@ def limit_interceptor_pcm_accel(pcm_accel_cmd: float, target_accel: float, stopp
   return limited
 
 
-def limit_interceptor_stopping_accel(pcm_accel_cmd: float, stopping: bool, v_ego: float, lead_visible: bool) -> float:
-  if not stopping or lead_visible or pcm_accel_cmd >= 0.0 or v_ego >= 1.5:
+def limit_interceptor_stopping_accel(pcm_accel_cmd: float, target_accel: float, stopping: bool, v_ego: float, lead_visible: bool) -> float:
+  if not stopping or lead_visible or pcm_accel_cmd >= 0.0 or v_ego >= 2.5:
     return pcm_accel_cmd
 
   # Pedal/SDSU Toyotas can feel abrupt in the last few feet of a no-lead stop
-  # because stopping state holds onto a relatively strong negative accel. Keep
-  # real lead stops untouched, but soften the final crawl into standstill.
-  stop_floor = float(np.interp(v_ego, [0.0, 0.2, 0.5, 0.9, 1.5], [-0.90, -0.95, -1.05, -1.15, -1.30]))
-  return max(pcm_accel_cmd, stop_floor)
+  # because stopping state can hold onto a stale strong negative command even
+  # after the planner target has already softened. Keep real lead stops
+  # untouched, but let the last 2-3 mph of a no-lead stop unwind toward the
+  # current planner target instead of shoving through zero.
+  stop_floor = float(np.interp(v_ego, [0.0, 0.2, 0.5, 0.9, 1.5, 2.5], [-0.72, -0.76, -0.82, -0.92, -1.05, -1.20]))
+  target_buffer = float(np.interp(v_ego, [0.0, 0.5, 1.5, 2.5], [0.08, 0.10, 0.15, 0.20]))
+  planner_floor = float(target_accel) - target_buffer
+  return max(pcm_accel_cmd, max(stop_floor, planner_floor))
 
 
 class CarController(CarControllerBase):
@@ -405,7 +409,7 @@ class CarController(CarControllerBase):
 
         if self.CP.enableGasInterceptorDEPRECATED:
           pcm_accel_cmd = limit_interceptor_pcm_accel(pcm_accel_cmd, actuators.accel, stopping, CS.out.vEgo)
-          pcm_accel_cmd = limit_interceptor_stopping_accel(pcm_accel_cmd, stopping, CS.out.vEgo, bool(hud_control.leadVisible))
+          pcm_accel_cmd = limit_interceptor_stopping_accel(pcm_accel_cmd, actuators.accel, stopping, CS.out.vEgo, bool(hud_control.leadVisible))
 
         pcm_accel_cmd = float(np.clip(pcm_accel_cmd, self.params.ACCEL_MIN, self.params.ACCEL_MAX))
 

opendbc_repo/opendbc/car/toyota/interface.py

@@ -158,6 +158,7 @@ class CarInterface(CarInterfaceBase):
 
     if ret.enableGasInterceptorDEPRECATED:
       # Pedal/SDSU Toyotas feel best with a softer final stop clamp.
+      ret.longitudinalActuatorDelay = max(ret.longitudinalActuatorDelay, 0.2)
       ret.stopAccel = -1.5
 
     return ret

opendbc_repo/opendbc/car/toyota/tests/test_toyota.py

@@ -10,6 +10,7 @@ from opendbc.car.toyota import toyotacan
 from opendbc.car.toyota.carcontroller import CarController, limit_interceptor_pcm_accel, limit_interceptor_stopping_accel, update_permit_braking
 from opendbc.car.toyota.carstate import calculate_interceptor_gas_pressed
 from opendbc.car.toyota.fingerprints import FW_VERSIONS
+from opendbc.car.toyota.interface import CarInterface
 from opendbc.car.toyota.values import CAR, DBC, TSS2_CAR, ANGLE_CONTROL_CAR, RADAR_ACC_CAR, SECOC_CAR, \
                                                   FW_QUERY_CONFIG, PLATFORM_CODE_ECUS, FUZZY_EXCLUDED_PLATFORMS, \
                                                   get_platform_codes
@@ -353,21 +354,47 @@ class TestToyotaCarController:
     assert limited <= 0.0
 
   def test_interceptor_stopping_limit_softens_no_lead_final_crawl(self):
-    limited = limit_interceptor_stopping_accel(-1.48, True, 0.5, False)
+    limited = limit_interceptor_stopping_accel(-1.48, -1.48, True, 0.5, False)
 
     assert limited > -1.48
-    assert limited == -1.05
+    assert limited == -0.82
+
+  def test_interceptor_stopping_limit_tracks_softer_target_near_standstill(self):
+    limited = limit_interceptor_stopping_accel(-1.86, -0.63, True, 0.5, False)
+
+    assert limited > -1.0
+    assert limited == -0.73
 
   def test_interceptor_stopping_limit_keeps_visible_lead_stop_untouched(self):
-    limited = limit_interceptor_stopping_accel(-1.48, True, 0.5, True)
+    limited = limit_interceptor_stopping_accel(-1.48, -0.63, True, 0.5, True)
 
     assert limited == -1.48
 
   def test_interceptor_stopping_limit_keeps_higher_speed_stop_untouched(self):
-    limited = limit_interceptor_stopping_accel(-1.48, True, 2.0, False)
+    limited = limit_interceptor_stopping_accel(-1.48, -0.63, True, 3.0, False)
 
     assert limited == -1.48
 
+  def test_interceptor_stopping_limit_softens_low_speed_no_lead_stop_before_final_crawl(self):
+    limited = limit_interceptor_stopping_accel(-1.55, -0.66, True, 1.45, False)
+
+    assert abs(limited - (-0.8075)) < 1e-6
+
+  def test_avalon_pedal_params_raise_delay_and_soften_stop(self):
+    CP = CarInterface.get_params(
+      CAR.TOYOTA_AVALON_2019,
+      {0: {0x2FF: 8, 0x201: 8}},
+      [],
+      True,
+      False,
+      False,
+      None,
+    )
+
+    assert CP.enableGasInterceptorDEPRECATED
+    assert abs(CP.longitudinalActuatorDelay - 0.2) < 1e-6
+    assert CP.stopAccel == -1.5
+
 
 class TestToyotaCarState:
   def test_interceptor_gas_pressed_threshold(self):