From 0593e53b048f95e586c65794a2aeb172669a5860 Mon Sep 17 00:00:00 2001 From: firestar5683 <168790843+firestar5683@users.noreply.github.com> Date: Sun, 16 Aug 2026 17:32:22 -0500 Subject: [PATCH] fargo levy --- .../opendbc/car/subaru/carcontroller.py | 12 +++++- .../opendbc/car/subaru/tests/test_subaru.py | 41 ++++++++++++++++++- .../controls/lib/longitudinal_planner.py | 8 +++- .../lib/longitudinal_vehicle_tunes.py | 8 ++++ .../tests/test_longitudinal_planner.py | 26 ++++++++++++ 5 files changed, 90 insertions(+), 5 deletions(-) diff --git a/opendbc_repo/opendbc/car/subaru/carcontroller.py b/opendbc_repo/opendbc/car/subaru/carcontroller.py index 16046c76e..0a5ee1b30 100644 --- a/opendbc_repo/opendbc/car/subaru/carcontroller.py +++ b/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 diff --git a/opendbc_repo/opendbc/car/subaru/tests/test_subaru.py b/opendbc_repo/opendbc/car/subaru/tests/test_subaru.py index bc80294a4..74e3ac684 100644 --- a/opendbc_repo/opendbc/car/subaru/tests/test_subaru.py +++ b/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) diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index d47946070..4a0eb9eae 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/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)) diff --git a/selfdrive/controls/lib/longitudinal_vehicle_tunes.py b/selfdrive/controls/lib/longitudinal_vehicle_tunes.py index 865a281d6..202ced61c 100644 --- a/selfdrive/controls/lib/longitudinal_vehicle_tunes.py +++ b/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 ( diff --git a/selfdrive/controls/tests/test_longitudinal_planner.py b/selfdrive/controls/tests/test_longitudinal_planner.py index c883e7e8d..3ba70b77e 100644 --- a/selfdrive/controls/tests/test_longitudinal_planner.py +++ b/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