avalon surge

opendbc_repo/opendbc/car/toyota/carcontroller.py

@@ -32,6 +32,7 @@ TOYOTA_COAST_BRAKE_MIN_SPEED = 15.0  # m/s
 TOYOTA_COAST_BRAKE_ENABLE_ACCEL = -0.10  # m/s^2
 TOYOTA_COAST_BRAKE_DISABLE_ACCEL = -0.06  # m/s^2
 TOYOTA_NO_LEAD_COAST_BRAKE_ACCEL = -0.30  # m/s^2
+TOYOTA_INTERCEPTOR_COMFORT_TARGET_ACCEL = 1.0  # m/s^2
 
 # LKA limits
 # EPS faults if you apply torque while the steering rate is above 100 deg/s for too long
@@ -89,6 +90,28 @@ def update_permit_braking(current: bool, net_acceleration_request_min: float, st
   return current
 
 
+def limit_interceptor_pcm_accel(pcm_accel_cmd: float, target_accel: float, stopping: bool, v_ego: float) -> float:
+  if stopping or abs(target_accel) > TOYOTA_INTERCEPTOR_COMFORT_TARGET_ACCEL:
+    return pcm_accel_cmd
+
+  lower_delta = float(np.interp(v_ego, [0.0, 5.0, 15.0], [0.20, 0.25, 0.35]))
+  upper_delta = float(np.interp(v_ego, [0.0, 5.0, 15.0], [0.25, 0.30, 0.40]))
+  limited = float(np.clip(pcm_accel_cmd, target_accel - lower_delta, target_accel + upper_delta))
+
+  # Pedal/SDSU cars can feel especially surgey if the Toyota longitudinal controller
+  # swings far away from the planner target. Bias comfort-zone requests back toward
+  # the planner while still allowing some controller correction.
+  target_weight = float(np.interp(v_ego, [0.0, 5.0, 15.0], [0.8, 0.65, 0.5]))
+  limited = (target_weight * target_accel) + ((1.0 - target_weight) * limited)
+
+  if target_accel > 0.15:
+    limited = max(limited, float(np.interp(v_ego, [0.0, 5.0, 15.0], [0.10, 0.05, 0.0])))
+  elif target_accel < -0.15:
+    limited = min(limited, float(np.interp(v_ego, [0.0, 5.0, 15.0], [-0.10, -0.05, 0.0])))
+
+  return limited
+
+
 class CarController(CarControllerBase):
   def __init__(self, dbc_names, CP):
     super().__init__(dbc_names, CP)
@@ -347,6 +370,9 @@ class CarController(CarControllerBase):
                                                     CS.out.vEgo,
                                                     lead)
 
+        if self.CP.enableGasInterceptorDEPRECATED:
+          pcm_accel_cmd = limit_interceptor_pcm_accel(pcm_accel_cmd, actuators.accel, stopping, CS.out.vEgo)
+
         pcm_accel_cmd = float(np.clip(pcm_accel_cmd, self.params.ACCEL_MIN, self.params.ACCEL_MAX))
 
         main_accel_cmd = 0. if self.CP.flags & ToyotaFlags.SECOC.value else pcm_accel_cmd

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

@@ -7,7 +7,7 @@ from opendbc.can import CANPacker, CANParser
 from opendbc.car.structs import CarParams
 from opendbc.car.fw_versions import build_fw_dict
 from opendbc.car.toyota import toyotacan
-from opendbc.car.toyota.carcontroller import CarController, update_permit_braking
+from opendbc.car.toyota.carcontroller import CarController, limit_interceptor_pcm_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.values import CAR, DBC, TSS2_CAR, ANGLE_CONTROL_CAR, RADAR_ACC_CAR, SECOC_CAR, \
@@ -324,6 +324,24 @@ class TestToyotaCarController:
 
     assert gas_cmd == 0.0
 
+  def test_interceptor_comfort_limit_keeps_positive_target_out_of_coast(self):
+    limited = limit_interceptor_pcm_accel(-0.30, 0.70, False, 8.5)
+
+    assert limited > 0.0
+    assert limited < 0.70
+
+  def test_interceptor_comfort_limit_keeps_mild_brake_request_negative(self):
+    limited = limit_interceptor_pcm_accel(0.25, -0.35, False, 8.5)
+
+    assert limited < 0.0
+    assert limited > -0.35
+
+  def test_interceptor_comfort_limit_bypasses_harder_braking(self):
+    original = -1.80
+    limited = limit_interceptor_pcm_accel(original, -1.20, False, 8.5)
+
+    assert limited == original
+
 
 class TestToyotaCarState:
   def test_interceptor_gas_pressed_threshold(self):