fargo levy

opendbc_repo/opendbc/car/subaru/carcontroller.py

@@ -28,6 +28,8 @@ _ASCENT_REENGAGE_MAX_STEER_RATE = 2.0
 _ASCENT_REENGAGE_MAX_ANGLE_DELTA = 1.0
 _ASCENT_RECLAIM_FRAMES = 36
 _ASCENT_RECLAIM_EXPONENT = 2.5
+_ASCENT_MADS_MIN_SPEED = 0.44704
+_ASCENT_MADS_MAX_STEER_ANGLE = 120.0
 
 
 def get_safety_CP():
@@ -223,8 +225,14 @@ class CarController(CarControllerBase):
       return subarucan.create_steering_control_angle(self.packer, apply_steer, lkas_active, self.angle_bus)
 
     if self.CP.carFingerprint == CAR.SUBARU_ASCENT_2023:
-      manual_handoff = self._ascent_manual_handoff(CS, CC.latActive)
-      lkas_active = CC.latActive and not manual_handoff
+      mads_only = CC.latActive and not CC.enabled
+      mads_only_ok = CS.out.vEgoRaw > _ASCENT_MADS_MIN_SPEED and \
+        abs(CS.out.steeringAngleDeg) < _ASCENT_MADS_MAX_STEER_ANGLE
+      lkas_available = CC.latActive and (not mads_only or mads_only_ok) and \
+        CS.out.gearShifter == structs.CarState.GearShifter.drive and not CS.out.standstill
+
+      manual_handoff = self._ascent_manual_handoff(CS, lkas_available)
+      lkas_active = lkas_available and not manual_handoff
 
       if lkas_active and not self.ascent_lkas_active:
         self.apply_steer_last = CS.out.steeringAngleDeg

opendbc_repo/opendbc/car/subaru/tests/test_subaru.py

@@ -440,13 +440,15 @@ def test_angle_controller_tracks_driver_override():
 def test_ascent_angle_controller_uses_fixed_angle_rate_limits():
   CP = CarInterface.get_non_essential_params(CAR.SUBARU_ASCENT_2023)
   controller = CarController({}, CP)
-  CC = SimpleNamespace(latActive=True, actuators=SimpleNamespace(steeringAngleDeg=-14.88))
+  CC = SimpleNamespace(enabled=True, latActive=True, actuators=SimpleNamespace(steeringAngleDeg=-14.88))
   CS = SimpleNamespace(out=SimpleNamespace(
     vEgoRaw=21.66,
     steeringAngleDeg=-25.77,
     steeringRateDeg=0.0,
     steeringTorque=-149.0,
     steeringPressed=False,
+    gearShifter=structs.CarState.GearShifter.drive,
+    standstill=False,
   ))
   parser = CANParser(DBC[CP.carFingerprint][Bus.pt], [("ES_LKAS_ANGLE", 0)], CanBus.main)
 
@@ -459,13 +461,15 @@ def test_ascent_angle_controller_uses_fixed_angle_rate_limits():
 def test_ascent_angle_controller_yields_until_manual_steering_settles():
   CP = CarInterface.get_non_essential_params(CAR.SUBARU_ASCENT_2023)
   controller = CarController({}, CP)
-  CC = SimpleNamespace(latActive=True, actuators=SimpleNamespace(steeringAngleDeg=-10.0))
+  CC = SimpleNamespace(enabled=True, latActive=True, actuators=SimpleNamespace(steeringAngleDeg=-10.0))
   CS = SimpleNamespace(out=SimpleNamespace(
     vEgoRaw=21.66,
     steeringAngleDeg=-25.06,
     steeringRateDeg=35.0,
     steeringTorque=-149.0,
     steeringPressed=True,
+    gearShifter=structs.CarState.GearShifter.drive,
+    standstill=False,
   ))
   parser = CANParser(DBC[CP.carFingerprint][Bus.pt], [("ES_LKAS_ANGLE", 0)], CanBus.main)
 
@@ -488,6 +492,39 @@ def test_ascent_angle_controller_yields_until_manual_steering_settles():
   assert parser.vl["ES_LKAS_ANGLE"]["LKAS_Output"] == pytest.approx(CS.out.steeringAngleDeg, abs=0.1)
 
 
+def test_ascent_angle_controller_blocks_parking_lot_aol_engagement():
+  CP = CarInterface.get_non_essential_params(CAR.SUBARU_ASCENT_2023)
+  controller = CarController({}, CP)
+  CC = SimpleNamespace(enabled=False, latActive=True, actuators=SimpleNamespace(steeringAngleDeg=-206.12))
+  CS = SimpleNamespace(out=SimpleNamespace(
+    vEgoRaw=1.57,
+    steeringAngleDeg=-260.44,
+    steeringRateDeg=96.0,
+    steeringTorque=7.0,
+    steeringPressed=False,
+    gearShifter=structs.CarState.GearShifter.drive,
+    standstill=False,
+  ))
+  parser = CANParser(DBC[CP.carFingerprint][Bus.pt], [("ES_LKAS_ANGLE", 0)], CanBus.main)
+
+  msg = controller.lateral_angle(CC, CS)
+  parser.update([(1, [msg])])
+  assert parser.vl["ES_LKAS_ANGLE"]["LKAS_Request"] == 0
+  assert parser.vl["ES_LKAS_ANGLE"]["LKAS_Output"] == pytest.approx(CS.out.steeringAngleDeg)
+
+  CS.out.steeringAngleDeg = -100.0
+  CS.out.steeringRateDeg = 0.0
+  msg = controller.lateral_angle(CC, CS)
+  parser.update([(2, [msg])])
+  assert parser.vl["ES_LKAS_ANGLE"]["LKAS_Request"] == 1
+
+  CS.out.gearShifter = structs.CarState.GearShifter.reverse
+  msg = controller.lateral_angle(CC, CS)
+  parser.update([(3, [msg])])
+  assert parser.vl["ES_LKAS_ANGLE"]["LKAS_Request"] == 0
+  assert parser.vl["ES_LKAS_ANGLE"]["LKAS_Output"] == pytest.approx(CS.out.steeringAngleDeg)
+
+
 def test_lkas_hud_state_uses_lateral_active():
   update_source = inspect.getsource(CarController.update)
 

selfdrive/controls/lib/longitudinal_planner.py

@@ -23,6 +23,7 @@ from openpilot.selfdrive.controls.lib.longitudinal_vehicle_tunes import (
   get_follow_prebrake_min_headway,
   get_force_stop_distance_bias,
   get_force_stop_handoff_distance,
+  allow_radar_standstill_gap_settle,
   is_gm_silverado_early_follow_lead,
   is_toyota_rav4_tss2_post_departure_tune,
   get_toyota_rav4_tss2_early_lead_cap,
@@ -2458,7 +2459,12 @@ class LongitudinalPlanner:
       slow_creep_depart_detected and
       self.slow_creep_lead_depart_elapsed >= STANDSTILL_LEAD_CREEP_RELEASE_CONFIRM_TIME
     )
-    radar_gap_settle_active = self.update_radar_standstill_gap_settle(sm, standstill_nudge_gap)
+    radar_gap_settle_active = False
+    if allow_radar_standstill_gap_settle(self.CP):
+      radar_gap_settle_active = self.update_radar_standstill_gap_settle(sm, standstill_nudge_gap)
+    else:
+      self.radar_standstill_gap_settle_elapsed = 0.0
+      self.radar_standstill_gap_settle_active = False
 
     standstill_stopped_lead_guard_cap = None
     standstill_guard_lead_present = any(bool(getattr(lead, "status", False)) for lead in (self.lead_one, self.lead_two))

selfdrive/controls/lib/longitudinal_vehicle_tunes.py

@@ -83,6 +83,14 @@ def get_toyota_rav4_tss2_early_lead_cap(CP, lead, v_ego, accel_min):
   return max(float(accel_min), -min(TOYOTA_RAV4_TSS2_EARLY_LEAD_MAX_DECEL, decel))
 
 
+def allow_radar_standstill_gap_settle(CP):
+  """Keep the generic stopped-lead gap nudge out of the early RAV4 TSS2 path."""
+  return not (
+    getattr(CP, "brand", "") == "toyota" and
+    str(getattr(CP, "carFingerprint", "")) == "TOYOTA_RAV4_TSS2"
+  )
+
+
 def get_far_follow_output_slew_rates(CP):
   if CP.brand == "honda" and str(CP.carFingerprint) == "HONDA_HRV_3G":
     return (

selfdrive/controls/tests/test_longitudinal_planner.py

@@ -19,6 +19,7 @@ from openpilot.selfdrive.controls.lib.longitudinal_planner import LongitudinalPl
 from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import LongitudinalMpc, soften_far_radar_lead_accel, should_trigger_planner_fcw
 from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import T_IDXS as T_IDXS_MPC
 from openpilot.selfdrive.controls.lib.longitudinal_vehicle_tunes import (
+  allow_radar_standstill_gap_settle,
   get_follow_prebrake_min_headway,
   get_toyota_rav4_tss2_early_lead_cap,
   get_toyota_sienna_post_departure_restop_cap,
@@ -2039,6 +2040,31 @@ def test_stationary_gap_settle_never_uses_vision_only_lead(model_version):
   assert planner.output_should_stop
 
 
+@pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14", "v15"])
+def test_rav4_tss2_does_not_release_a_stopped_lead_for_gap_settle(model_version):
+  CP = ToyotaCarInterface.get_non_essential_params(TOYOTA_CAR.TOYOTA_RAV4_TSS2)
+  planner = LongitudinalPlanner(CP, init_v=0.0)
+  sm = make_sm(
+    0.0,
+    desired_accel=-0.12,
+    min_accel=-0.5,
+    experimental_mode=True,
+    tracking_lead=False,
+    lead_one=make_lead(status=True, d_rel=6.4, v_lead=0.0, a_lead=0.0, radar=True, model_prob=1.0),
+  )
+  sm["carState"].standstill = True
+  sm["controlsState"].longControlState = LongCtrlState.stopping
+  sm["modelV2"].action.shouldStop = True
+
+  frames = int(round(longitudinal_planner_module.RADAR_STANDSTILL_GAP_SETTLE_CONFIRM_TIME / planner.dt)) + 2
+  for _ in range(max(frames, 1)):
+    planner.update(sm, make_toggles(model_version))
+
+  assert not allow_radar_standstill_gap_settle(CP)
+  assert not planner.radar_standstill_gap_settle_active
+  assert planner.output_should_stop
+
+
 @pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14", "v15"])
 def test_standstill_moving_lead_applies_resume_floor_once_stop_clears(model_version):
   v_ego = 0.0