diff --git a/opendbc_repo/opendbc/car/subaru/carcontroller.py b/opendbc_repo/opendbc/car/subaru/carcontroller.py index 47db23e36..9f1e2794f 100644 --- a/opendbc_repo/opendbc/car/subaru/carcontroller.py +++ b/opendbc_repo/opendbc/car/subaru/carcontroller.py @@ -16,7 +16,8 @@ _SNG_ACC_MIN_DIST = 3 _SNG_ACC_MAX_DIST = 4.5 _LEGACY_2025_MADS_MIN_SPEED = 0.44704 _LEGACY_2025_MADS_MAX_STEER_ANGLE = 120.0 -_LEGACY_2025_REENGAGE_MAX_STEER_RATE = 3.0 +_ANGLE_REENGAGE_MAX_STEER_RATE = 3.0 +_ANGLE_REENGAGE_SETTLE_FRAMES = 2 def get_safety_CP(): @@ -30,6 +31,7 @@ class CarController(CarControllerBase): self.apply_torque_last = 0 self.apply_steer_last = 0 self.driver_override = False + self.angle_reengage_settle_frames = 0 self.legacy_2025_lkas_active = False self.cruise_button_prev = 0 @@ -57,7 +59,7 @@ class CarController(CarControllerBase): CS.out.gearShifter == structs.CarState.GearShifter.drive and not CS.out.standstill manual_handoff = CS.out.steeringPressed or ( - not self.legacy_2025_lkas_active and abs(CS.out.steeringRateDeg) > _LEGACY_2025_REENGAGE_MAX_STEER_RATE + not self.legacy_2025_lkas_active and abs(CS.out.steeringRateDeg) > _ANGLE_REENGAGE_MAX_STEER_RATE ) lkas_active = lkas_available and not manual_handoff @@ -79,6 +81,17 @@ class CarController(CarControllerBase): abs_torque = abs(CS.out.steeringTorque) if abs_torque > self.p.STEER_OVERRIDE_TORQUE_HIGH: self.driver_override = True + self.angle_reengage_settle_frames = 0 + elif self.CP.carFingerprint == CAR.SUBARU_ASCENT_2023 and self.driver_override: + wheel_settled = abs(CS.out.steeringRateDeg) <= _ANGLE_REENGAGE_MAX_STEER_RATE + if abs_torque < self.p.STEER_OVERRIDE_TORQUE_LOW and wheel_settled: + self.angle_reengage_settle_frames += 1 + else: + self.angle_reengage_settle_frames = 0 + + if self.angle_reengage_settle_frames >= _ANGLE_REENGAGE_SETTLE_FRAMES: + self.driver_override = False + self.angle_reengage_settle_frames = 0 elif abs_torque < self.p.STEER_OVERRIDE_TORQUE_LOW: self.driver_override = False diff --git a/opendbc_repo/opendbc/car/subaru/tests/test_subaru.py b/opendbc_repo/opendbc/car/subaru/tests/test_subaru.py index 593d3a876..c2f0a15d3 100644 --- a/opendbc_repo/opendbc/car/subaru/tests/test_subaru.py +++ b/opendbc_repo/opendbc/car/subaru/tests/test_subaru.py @@ -308,6 +308,40 @@ def test_angle_controller_tracks_driver_override(): assert msg[0] == 0x124 +def test_ascent_angle_controller_waits_for_manual_steering_to_settle(): + CP = CarInterface.get_non_essential_params(CAR.SUBARU_ASCENT_2023) + controller = CarController({}, CP) + CC = SimpleNamespace(latActive=True, actuators=SimpleNamespace(steeringAngleDeg=-100.0)) + CS = SimpleNamespace(out=SimpleNamespace( + vEgoRaw=2.2, + steeringAngleDeg=-114.04, + steeringRateDeg=-90.0, + steeringTorque=-201.0, + )) + 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 + + CS.out.steeringAngleDeg = -216.05 + CS.out.steeringRateDeg = -133.0 + CS.out.steeringTorque = -148.0 + msg = controller.lateral_angle(CC, CS) + parser.update([(2, [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.steeringRateDeg = 2.0 + msg = controller.lateral_angle(CC, CS) + parser.update([(3, [msg])]) + assert parser.vl["ES_LKAS_ANGLE"]["LKAS_Request"] == 0 + + msg = controller.lateral_angle(CC, CS) + parser.update([(4, [msg])]) + assert parser.vl["ES_LKAS_ANGLE"]["LKAS_Request"] == 1 + + def test_lkas_hud_state_uses_lateral_active(): update_source = inspect.getsource(CarController.update)