diff --git a/opendbc_repo/opendbc/car/subaru/carcontroller.py b/opendbc_repo/opendbc/car/subaru/carcontroller.py index 50fdd1257..16046c76e 100644 --- a/opendbc_repo/opendbc/car/subaru/carcontroller.py +++ b/opendbc_repo/opendbc/car/subaru/carcontroller.py @@ -22,8 +22,12 @@ _LEGACY_2025_REENGAGE_MAX_STEER_RATE = 2.0 _LEGACY_2025_REENGAGE_MAX_ANGLE_DELTA = 1.0 _LEGACY_2025_RECLAIM_FRAMES = 36 _LEGACY_2025_RECLAIM_EXPONENT = 2.5 -_ANGLE_REENGAGE_MAX_STEER_RATE = 3.0 -_ANGLE_REENGAGE_SETTLE_FRAMES = 2 +_ASCENT_OVERRIDE_HOLD_FRAMES = 10 +_ASCENT_REENGAGE_SETTLE_FRAMES = 8 +_ASCENT_REENGAGE_MAX_STEER_RATE = 2.0 +_ASCENT_REENGAGE_MAX_ANGLE_DELTA = 1.0 +_ASCENT_RECLAIM_FRAMES = 36 +_ASCENT_RECLAIM_EXPONENT = 2.5 def get_safety_CP(): @@ -37,7 +41,6 @@ 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.legacy_2025_handoff_active = False self.legacy_2025_override_hold_frames = 0 @@ -45,6 +48,13 @@ class CarController(CarControllerBase): self.legacy_2025_reengage_reference_angle = 0.0 self.legacy_2025_reclaim_frames = 0 self.legacy_2025_reclaim_start_angle = 0.0 + self.ascent_lkas_active = False + self.ascent_handoff_active = False + self.ascent_override_hold_frames = 0 + self.ascent_reengage_settle_frames = 0 + self.ascent_reengage_reference_angle = 0.0 + self.ascent_reclaim_frames = 0 + self.ascent_reclaim_start_angle = 0.0 self.cruise_button_prev = 0 self.steer_rate_counter = 0 @@ -125,6 +135,69 @@ class CarController(CarControllerBase): self.legacy_2025_reclaim_frames -= 1 return target_angle + def _reset_ascent_handoff(self): + self.ascent_handoff_active = False + self.ascent_override_hold_frames = 0 + self.ascent_reengage_settle_frames = 0 + self.ascent_reengage_reference_angle = 0.0 + self.ascent_reclaim_frames = 0 + self.ascent_reclaim_start_angle = 0.0 + + def _ascent_manual_handoff(self, CS, lat_active): + if not lat_active: + self._reset_ascent_handoff() + return False + + if CS.out.steeringPressed: + self.ascent_handoff_active = True + self.ascent_override_hold_frames = _ASCENT_OVERRIDE_HOLD_FRAMES + self.ascent_reengage_settle_frames = 0 + self.ascent_reengage_reference_angle = CS.out.steeringAngleDeg + self.ascent_reclaim_frames = 0 + return True + + if not self.ascent_handoff_active and not self.ascent_lkas_active and \ + abs(CS.out.steeringRateDeg) > _ASCENT_REENGAGE_MAX_STEER_RATE: + self.ascent_handoff_active = True + self.ascent_reengage_reference_angle = CS.out.steeringAngleDeg + + if not self.ascent_handoff_active: + return False + + if self.ascent_override_hold_frames > 0: + self.ascent_override_hold_frames -= 1 + if self.ascent_override_hold_frames == 0: + self.ascent_reengage_reference_angle = CS.out.steeringAngleDeg + return True + + wheel_stable = abs(CS.out.steeringRateDeg) <= _ASCENT_REENGAGE_MAX_STEER_RATE and \ + abs(CS.out.steeringAngleDeg - self.ascent_reengage_reference_angle) <= _ASCENT_REENGAGE_MAX_ANGLE_DELTA + if wheel_stable: + self.ascent_reengage_settle_frames += 1 + else: + self.ascent_reengage_settle_frames = 0 + self.ascent_reengage_reference_angle = CS.out.steeringAngleDeg + + if self.ascent_reengage_settle_frames < _ASCENT_REENGAGE_SETTLE_FRAMES: + return True + + self.ascent_handoff_active = False + self.ascent_reengage_settle_frames = 0 + self.ascent_reclaim_frames = _ASCENT_RECLAIM_FRAMES + self.ascent_reclaim_start_angle = CS.out.steeringAngleDeg + return True + + def _ascent_reclaim_target(self, target_angle): + if self.ascent_reclaim_frames <= 0: + return target_angle + + progress = (_ASCENT_RECLAIM_FRAMES - self.ascent_reclaim_frames + 1) / _ASCENT_RECLAIM_FRAMES + eased_progress = progress ** _ASCENT_RECLAIM_EXPONENT + target_angle = self.ascent_reclaim_start_angle + eased_progress * \ + (target_angle - self.ascent_reclaim_start_angle) + self.ascent_reclaim_frames -= 1 + return target_angle + def lateral_angle(self, CC, CS): if self.CP.carFingerprint == CAR.SUBARU_LEGACY_2025: mads_only = CC.latActive and not CC.enabled @@ -136,9 +209,6 @@ class CarController(CarControllerBase): manual_handoff = self._legacy_2025_manual_handoff(CS, lkas_available) lkas_active = lkas_available and not manual_handoff - if lkas_active and not self.legacy_2025_lkas_active: - self.apply_steer_last = CS.out.steeringAngleDeg - steer_target = self._legacy_2025_reclaim_target(CC.actuators.steeringAngleDeg) if lkas_active else CC.actuators.steeringAngleDeg apply_steer = apply_std_steer_angle_limits( steer_target, @@ -152,20 +222,29 @@ class CarController(CarControllerBase): self.legacy_2025_lkas_active = lkas_active 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 + + if lkas_active and not self.ascent_lkas_active: + self.apply_steer_last = CS.out.steeringAngleDeg + + steer_target = self._ascent_reclaim_target(CC.actuators.steeringAngleDeg) if lkas_active else CC.actuators.steeringAngleDeg + apply_steer = apply_std_steer_angle_limits( + steer_target, + self.apply_steer_last, + CS.out.vEgoRaw, + CS.out.steeringAngleDeg, + lkas_active, + self.p.FIXED_ANGLE_LIMITS, + ) + self.apply_steer_last = apply_steer + self.ascent_lkas_active = lkas_active + return subarucan.create_steering_control_angle(self.packer, apply_steer, lkas_active, self.angle_bus) + 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/interface.py b/opendbc_repo/opendbc/car/subaru/interface.py index 9432bcb8c..59dd14422 100644 --- a/opendbc_repo/opendbc/car/subaru/interface.py +++ b/opendbc_repo/opendbc/car/subaru/interface.py @@ -40,8 +40,8 @@ class CarInterface(CarInterfaceBase): ret.safetyConfigs[0].safetyParam |= SubaruSafetyFlags.D_PLATFORM.value if ret.flags & SubaruFlags.D_PLATFORM_CAMERA: ret.safetyConfigs[0].safetyParam |= SubaruSafetyFlags.D_PLATFORM_CAMERA.value - if candidate == CAR.SUBARU_LEGACY_2025: - ret.safetyConfigs[0].safetyParam |= SubaruSafetyFlags.LEGACY_2025_ANGLE_LIMITS.value + if candidate in (CAR.SUBARU_LEGACY_2025, CAR.SUBARU_ASCENT_2023): + ret.safetyConfigs[0].safetyParam |= SubaruSafetyFlags.FIXED_ANGLE_LIMITS.value ret.steerLimitTimer = 0.4 ret.steerActuatorDelay = 0.1 diff --git a/opendbc_repo/opendbc/car/subaru/tests/test_subaru.py b/opendbc_repo/opendbc/car/subaru/tests/test_subaru.py index 072941102..bc9239454 100644 --- a/opendbc_repo/opendbc/car/subaru/tests/test_subaru.py +++ b/opendbc_repo/opendbc/car/subaru/tests/test_subaru.py @@ -185,7 +185,7 @@ def test_legacy_2025_uses_gen2_angle_bus_layout(): assert not (CP.safetyConfigs[0].safetyParam & SubaruSafetyFlags.D_PLATFORM) assert not (CP.flags & SubaruFlags.D_PLATFORM_CAMERA) assert not (CP.safetyConfigs[0].safetyParam & SubaruSafetyFlags.D_PLATFORM_CAMERA) - assert CP.safetyConfigs[0].safetyParam & SubaruSafetyFlags.LEGACY_2025_ANGLE_LIMITS + assert CP.safetyConfigs[0].safetyParam & SubaruSafetyFlags.FIXED_ANGLE_LIMITS assert CanBus.main_for_cp(CP) == CanBus.main assert CanBus.angle_for_cp(CP) == CanBus.main assert parsers[Bus.pt].bus == CanBus.main @@ -201,7 +201,7 @@ def test_legacy_2025_uses_validated_angle_request_limits(): controller = CarController({}, CP) CC = SimpleNamespace( enabled=False, - latActive=True, + latActive=False, actuators=SimpleNamespace(steeringAngleDeg=-73.05), ) CS = SimpleNamespace(out=SimpleNamespace( @@ -213,6 +213,8 @@ def test_legacy_2025_uses_validated_angle_request_limits(): standstill=False, )) + controller.lateral_angle(CC, CS) + CC.latActive = True msg = controller.lateral_angle(CC, CS) parser = CANParser(DBC[CP.carFingerprint][Bus.pt], [("ES_LKAS_ANGLE", 0)], CanBus.main) parser.update([(1, [msg])]) @@ -228,6 +230,39 @@ def test_legacy_2025_uses_validated_angle_request_limits(): assert parser.vl["ES_LKAS_ANGLE"]["LKAS_Output"] == pytest.approx(CS.out.steeringAngleDeg) +def test_legacy_2025_engagement_continues_from_last_sent_angle(): + CP = CarInterface.get_non_essential_params(CAR.SUBARU_LEGACY_2025) + controller = CarController({}, CP) + CC = SimpleNamespace( + enabled=False, + latActive=False, + actuators=SimpleNamespace(steeringAngleDeg=3.17), + ) + CS = SimpleNamespace(out=SimpleNamespace( + vEgoRaw=13.9, + steeringAngleDeg=-0.14, + steeringRateDeg=0.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(-0.14) + + # CarState can advance between the last inactive command and the first active one. + # Continue from the command panda accepted rather than skipping ahead to the newer sample. + CS.out.steeringAngleDeg = -0.09 + CC.latActive = True + msg = controller.lateral_angle(CC, CS) + parser.update([(2, [msg])]) + assert parser.vl["ES_LKAS_ANGLE"]["LKAS_Request"] == 1 + assert parser.vl["ES_LKAS_ANGLE"]["LKAS_Output"] == pytest.approx(0.47) + + def test_legacy_2025_waits_for_manual_steering_to_settle_before_reengaging(): CP = CarInterface.get_non_essential_params(CAR.SUBARU_LEGACY_2025) controller = CarController({}, CP) @@ -339,6 +374,7 @@ def test_ascent_2023_uses_gen2_angle_bus_layout(): assert not (CP.safetyConfigs[0].safetyParam & SubaruSafetyFlags.D_PLATFORM) assert not (CP.flags & SubaruFlags.D_PLATFORM_CAMERA) assert not (CP.safetyConfigs[0].safetyParam & SubaruSafetyFlags.D_PLATFORM_CAMERA) + assert CP.safetyConfigs[0].safetyParam & SubaruSafetyFlags.FIXED_ANGLE_LIMITS assert CanBus.main_for_cp(CP) == CanBus.main assert CanBus.angle_for_cp(CP) == CanBus.main assert parsers[Bus.pt].bus == CanBus.main @@ -373,38 +409,55 @@ def test_angle_controller_tracks_driver_override(): assert msg[0] == 0x124 -def test_ascent_angle_controller_waits_for_manual_steering_to_settle(): +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=-100.0)) + CC = SimpleNamespace(latActive=True, actuators=SimpleNamespace(steeringAngleDeg=-14.88)) CS = SimpleNamespace(out=SimpleNamespace( - vEgoRaw=2.2, - steeringAngleDeg=-114.04, - steeringRateDeg=-90.0, - steeringTorque=-201.0, + vEgoRaw=21.66, + steeringAngleDeg=-25.77, + steeringRateDeg=0.0, + steeringTorque=-149.0, + steeringPressed=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"] == 1 + assert CS.out.steeringAngleDeg < parser.vl["ES_LKAS_ANGLE"]["LKAS_Output"] < -25.0 + + +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)) + CS = SimpleNamespace(out=SimpleNamespace( + vEgoRaw=21.66, + steeringAngleDeg=-25.06, + steeringRateDeg=35.0, + steeringTorque=-149.0, + steeringPressed=True, )) 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 + CS.out.steeringPressed = False + CS.out.steeringAngleDeg = -17.91 + CS.out.steeringRateDeg = 0.0 + for i in range(18): + msg = controller.lateral_angle(CC, CS) + parser.update([(2 + i, [msg])]) + assert parser.vl["ES_LKAS_ANGLE"]["LKAS_Request"] == 0 msg = controller.lateral_angle(CC, CS) - parser.update([(4, [msg])]) + parser.update([(20, [msg])]) assert parser.vl["ES_LKAS_ANGLE"]["LKAS_Request"] == 1 + assert parser.vl["ES_LKAS_ANGLE"]["LKAS_Output"] == pytest.approx(CS.out.steeringAngleDeg, abs=0.1) def test_lkas_hud_state_uses_lateral_active(): diff --git a/opendbc_repo/opendbc/car/subaru/values.py b/opendbc_repo/opendbc/car/subaru/values.py index 1586899f9..bb23533d2 100644 --- a/opendbc_repo/opendbc/car/subaru/values.py +++ b/opendbc_repo/opendbc/car/subaru/values.py @@ -19,11 +19,12 @@ class CarControllerParams: MAX_LATERAL_JERK=3.0 + (ACCELERATION_DUE_TO_GRAVITY * 0.06), MAX_ANGLE_RATE=1, ) - LEGACY_2025_ANGLE_LIMITS: AngleSteeringLimits = AngleSteeringLimits( + FIXED_ANGLE_LIMITS: AngleSteeringLimits = AngleSteeringLimits( 545, ([0., 5., 35.], [5., .8, .15]), ([0., 5., 35.], [5., .8, .15]), ) + LEGACY_2025_ANGLE_LIMITS = FIXED_ANGLE_LIMITS def __init__(self, CP): self.STEER_STEP = 2 # how often we update the steer cmd @@ -81,7 +82,8 @@ class SubaruSafetyFlags(IntFlag): LKAS_ANGLE = 16 D_PLATFORM = 32 D_PLATFORM_CAMERA = 64 - LEGACY_2025_ANGLE_LIMITS = 128 + FIXED_ANGLE_LIMITS = 128 + LEGACY_2025_ANGLE_LIMITS = FIXED_ANGLE_LIMITS class SubaruFlags(IntFlag): diff --git a/opendbc_repo/opendbc/safety/modes/subaru.h b/opendbc_repo/opendbc/safety/modes/subaru.h index c8e735ac2..699334e75 100644 --- a/opendbc_repo/opendbc/safety/modes/subaru.h +++ b/opendbc_repo/opendbc/safety/modes/subaru.h @@ -107,7 +107,7 @@ static bool subaru_longitudinal = false; static bool subaru_stop_and_go = false; static bool subaru_lkas_angle = false; static bool subaru_d_platform = false; -static bool subaru_legacy_2025_angle_limits = false; +static bool subaru_fixed_angle_limits = false; static uint32_t subaru_get_checksum(const CANPacket_t *msg) { return (uint8_t)msg->data[0]; @@ -192,7 +192,7 @@ static bool subaru_tx_hook(const CANPacket_t *msg) { .frequency = 50U, }; - const AngleSteeringLimits SUBARU_LEGACY_2025_ANGLE_STEERING_LIMITS = { + const AngleSteeringLimits SUBARU_FIXED_ANGLE_STEERING_LIMITS = { .max_angle = 545 * 100, .angle_deg_to_can = 100., .angle_rate_up_lookup = { @@ -241,8 +241,8 @@ static bool subaru_tx_hook(const CANPacket_t *msg) { desired_angle = -1 * to_signed(desired_angle, 17); bool lkas_request = GET_BIT(msg, 12U); - if (subaru_legacy_2025_angle_limits) { - violation |= steer_angle_cmd_checks(desired_angle, lkas_request, SUBARU_LEGACY_2025_ANGLE_STEERING_LIMITS); + if (subaru_fixed_angle_limits) { + violation |= steer_angle_cmd_checks(desired_angle, lkas_request, SUBARU_FIXED_ANGLE_STEERING_LIMITS); } else { violation |= steer_angle_cmd_checks_vm(desired_angle, lkas_request, SUBARU_ANGLE_STEERING_LIMITS, SUBARU_ANGLE_STEERING_PARAMS); } @@ -375,8 +375,8 @@ static safety_config subaru_init(uint16_t param) { const uint16_t SUBARU_PARAM_D_PLATFORM_CAMERA = 64; const bool subaru_d_platform_camera = GET_FLAG(param, SUBARU_PARAM_D_PLATFORM_CAMERA); - const uint16_t SUBARU_PARAM_LEGACY_2025_ANGLE_LIMITS = 128; - subaru_legacy_2025_angle_limits = GET_FLAG(param, SUBARU_PARAM_LEGACY_2025_ANGLE_LIMITS); + const uint16_t SUBARU_PARAM_FIXED_ANGLE_LIMITS = 128; + subaru_fixed_angle_limits = GET_FLAG(param, SUBARU_PARAM_FIXED_ANGLE_LIMITS); #ifdef ALLOW_DEBUG const uint16_t SUBARU_PARAM_LONGITUDINAL = 2; diff --git a/opendbc_repo/opendbc/safety/tests/test_subaru.py b/opendbc_repo/opendbc/safety/tests/test_subaru.py index 1cabd5756..41e2624bb 100755 --- a/opendbc_repo/opendbc/safety/tests/test_subaru.py +++ b/opendbc_repo/opendbc/safety/tests/test_subaru.py @@ -358,8 +358,8 @@ class TestSubaruGen2AngleStockLongitudinalSafety(TestSubaruStockLongitudinalSafe TX_MSGS = lkas_tx_msgs(SUBARU_ALT_BUS, SubaruMsg.ES_LKAS_ANGLE) -class TestSubaruGen2Legacy2025AngleSafety(TestSubaruGen2AngleStockLongitudinalSafety): - FLAGS = SubaruSafetyFlags.GEN2 | SubaruSafetyFlags.LKAS_ANGLE | SubaruSafetyFlags.LEGACY_2025_ANGLE_LIMITS +class TestSubaruGen2FixedAngleSafety(TestSubaruGen2AngleStockLongitudinalSafety): + FLAGS = SubaruSafetyFlags.GEN2 | SubaruSafetyFlags.LKAS_ANGLE | SubaruSafetyFlags.FIXED_ANGLE_LIMITS STEER_ANGLE_MAX = 545 ANGLE_RATE_BP = [0., 5., 35.] ANGLE_RATE_UP = [5., .8, .15] diff --git a/panda/board/obj/gitversion.h b/panda/board/obj/gitversion.h index 874cb83a4..b7f6f1ba7 100644 --- a/panda/board/obj/gitversion.h +++ b/panda/board/obj/gitversion.h @@ -1,2 +1,2 @@ extern const uint8_t gitversion[19]; -const uint8_t gitversion[19] = "DEV-cbf7f35c-DEBUG"; +const uint8_t gitversion[19] = "DEV-8a5cef0f-DEBUG"; diff --git a/panda/board/obj/version b/panda/board/obj/version index f2f9378a1..468f2a785 100644 --- a/panda/board/obj/version +++ b/panda/board/obj/version @@ -1 +1 @@ -DEV-cbf7f35c-DEBUG \ No newline at end of file +DEV-8a5cef0f-DEBUG \ No newline at end of file diff --git a/selfdrive/controls/lib/longcontrol_vehicle_tunes.py b/selfdrive/controls/lib/longcontrol_vehicle_tunes.py index 64423247b..c9450b365 100644 --- a/selfdrive/controls/lib/longcontrol_vehicle_tunes.py +++ b/selfdrive/controls/lib/longcontrol_vehicle_tunes.py @@ -20,8 +20,8 @@ BOLT_ACC_PEDAL_START_HANDOFF_FLOOR_V = [0.22, 0.18, 0.10] NEGATIVE_TARGET_CREEP_GUARD_SPEED = 0.35 NEGATIVE_TARGET_CREEP_GUARD_DECEL = 0.40 GM_TRUCK_TARGET_FILTER_MIN_SPEED = 12.0 -GM_TRUCK_TARGET_FILTER_UP_TAU = 0.10 -GM_TRUCK_TARGET_FILTER_DOWN_TAU = 0.06 +GM_TRUCK_TARGET_FILTER_UP_TAU = 0.20 +GM_TRUCK_TARGET_FILTER_DOWN_TAU = 0.14 GM_TRUCK_TARGET_FILTER_BRAKE_BYPASS = -0.65 GM_TRUCK_TARGET_FILTER_DROP_BYPASS = 0.45 TOYOTA_SIENNA_TARGET_FILTER_MIN_SPEED = 12.0 diff --git a/selfdrive/controls/tests/test_longcontrol.py b/selfdrive/controls/tests/test_longcontrol.py index dc26fe0dd..b85a3cedd 100644 --- a/selfdrive/controls/tests/test_longcontrol.py +++ b/selfdrive/controls/tests/test_longcontrol.py @@ -1160,6 +1160,22 @@ def test_gm_stock_truck_target_filter_smooths_mild_follow_reversals(): assert filtered_brake < filtered_accel < 0.25 +def test_gm_stock_truck_target_filter_uses_comfort_slew_for_mild_braking(): + CP = make_longcontrol_cp( + brand="gm", + carFingerprint=CAR.CHEVROLET_SILVERADO, + enableGasInterceptorDEPRECATED=False, + ) + tuning = LongControl(CP).vehicle_tuning + + tuning.shape_gm_truck_accel_target(0.30, 25.0, False) + filtered = tuning.shape_gm_truck_accel_target(-0.10, 25.0, False) + expected = 0.30 + vehicle_tunes.DT_CTRL / (vehicle_tunes.GM_TRUCK_TARGET_FILTER_DOWN_TAU + vehicle_tunes.DT_CTRL) * (-0.40) + + assert filtered == pytest.approx(expected) + assert filtered > -0.10 + + def test_gm_stock_truck_target_filter_bypasses_urgent_braking(): CP = make_longcontrol_cp( brand="gm", diff --git a/selfdrive/ui/lib/starpilot_status.py b/selfdrive/ui/lib/starpilot_status.py index 98ac23486..9a4510cbd 100644 --- a/selfdrive/ui/lib/starpilot_status.py +++ b/selfdrive/ui/lib/starpilot_status.py @@ -14,6 +14,17 @@ EXPERIMENTAL_COLOR = rl.Color(218, 111, 37, 255) CEM_OVERRIDE_COLOR = rl.Color(255, 214, 0, 255) SWITCHBACK_COLOR = rl.Color(139, 108, 197, 255) TRAFFIC_COLOR = rl.Color(201, 34, 49, 255) +LONGITUDINAL_ONLY_COLOR = rl.Color(255, 105, 180, 255) + + +def is_longitudinal_only_active(state: UIState) -> bool: + """Return true when control is enabled but lateral control is inactive. + + Do not use carControl.longActive here: stock-cruise cars intentionally leave + that field false while the vehicle's own longitudinal controller is active. + """ + car_control = state.sm["carControl"] + return bool(state.sm["selfdriveState"].enabled and not car_control.latActive) def get_border_color(state: UIState): @@ -21,6 +32,8 @@ def get_border_color(state: UIState): lateral_active = enabled or state.always_on_lateral_active if state.status == UIStatus.OVERRIDE: return OVERRIDE_COLOR + if is_longitudinal_only_active(state): + return LONGITUDINAL_ONLY_COLOR if state.switchback_mode_enabled and lateral_active: return SWITCHBACK_COLOR if state.traffic_mode_enabled and enabled: @@ -48,6 +61,8 @@ def get_screen_edge_color(state: UIState): lateral_active = enabled or state.always_on_lateral_active if state.status == UIStatus.OVERRIDE: return OVERRIDE_COLOR + if is_longitudinal_only_active(state): + return LONGITUDINAL_ONLY_COLOR if state.switchback_mode_enabled and lateral_active: return SWITCHBACK_COLOR if state.always_on_lateral_active: diff --git a/selfdrive/ui/mici/onroad/starpilot_status.py b/selfdrive/ui/mici/onroad/starpilot_status.py index d9c6912e7..f9343d856 100644 --- a/selfdrive/ui/mici/onroad/starpilot_status.py +++ b/selfdrive/ui/mici/onroad/starpilot_status.py @@ -7,6 +7,7 @@ from openpilot.selfdrive.ui.lib.starpilot_status import ( DISENGAGED_COLOR, ENGAGED_COLOR, EXPERIMENTAL_COLOR, + LONGITUDINAL_ONLY_COLOR, OVERRIDE_COLOR, SWITCHBACK_COLOR, TRAFFIC_COLOR, @@ -25,6 +26,7 @@ __all__ = [ "DISENGAGED_COLOR", "ENGAGED_COLOR", "EXPERIMENTAL_COLOR", + "LONGITUDINAL_ONLY_COLOR", "OVERRIDE_COLOR", "SWITCHBACK_COLOR", "TRAFFIC_COLOR", diff --git a/selfdrive/ui/tests/test_starpilot_status.py b/selfdrive/ui/tests/test_starpilot_status.py new file mode 100644 index 000000000..ea94c24ab --- /dev/null +++ b/selfdrive/ui/tests/test_starpilot_status.py @@ -0,0 +1,42 @@ +from types import SimpleNamespace + +from openpilot.selfdrive.ui.lib.starpilot_status import ( + DISENGAGED_COLOR, + ENGAGED_COLOR, + LONGITUDINAL_ONLY_COLOR, + AOL_COLOR, + get_border_color, + get_screen_edge_color, +) +from openpilot.selfdrive.ui.ui_state import UIStatus + + +def _state(*, enabled=False, lat_active=False, aol=False): + return SimpleNamespace( + sm={ + "selfdriveState": SimpleNamespace(enabled=enabled, experimentalMode=False), + "carControl": SimpleNamespace(latActive=lat_active), + }, + status=UIStatus.ENGAGED if enabled else UIStatus.DISENGAGED, + always_on_lateral_active=aol, + switchback_mode_enabled=False, + traffic_mode_enabled=False, + conditional_status=0, + ) + + +def _rgb(color): + return color.r, color.g, color.b + + +def test_cruise_only_uses_pink_for_border_and_screen_edge(): + state = _state(enabled=True, lat_active=False) + + assert _rgb(get_border_color(state)) == _rgb(LONGITUDINAL_ONLY_COLOR) + assert _rgb(get_screen_edge_color(state)) == _rgb(LONGITUDINAL_ONLY_COLOR) + + +def test_lateral_active_colors_remain_unchanged(): + assert _rgb(get_border_color(_state(enabled=True, lat_active=True))) == _rgb(ENGAGED_COLOR) + assert _rgb(get_border_color(_state(aol=True))) == _rgb(AOL_COLOR) + assert _rgb(get_border_color(_state())) == _rgb(DISENGAGED_COLOR) diff --git a/starpilot/system/the_galaxy/assets/components/tools/toggles.js b/starpilot/system/the_galaxy/assets/components/tools/toggles.js index 971e747e1..9d7b40b98 100644 --- a/starpilot/system/the_galaxy/assets/components/tools/toggles.js +++ b/starpilot/system/the_galaxy/assets/components/tools/toggles.js @@ -7,7 +7,9 @@ const FACTORY_RESET_STATUS_POLL_INTERVAL_MS = 1000 const state = reactive({ showResetDefaultModal: false, showSaveMeModal: false, + showDeleteRoutesModal: false, factoryResetBusy: false, + routeDeleteBusy: false, factoryResetStatus: null, }) @@ -199,6 +201,29 @@ export function ToggleControl() { state.showSaveMeModal = true; } + function confirmDeleteRoutes() { + state.showDeleteRoutesModal = true; + } + + async function deleteAllRoutes() { + if (state.routeDeleteBusy || state.factoryResetBusy) return + + state.showDeleteRoutesModal = false + state.routeDeleteBusy = true + try { + const response = await fetch("/api/routes/delete_all", { method: "POST" }) + const payload = await response.json().catch(() => ({})) + if (!response.ok) { + throw new Error(payload.error || response.statusText || "Failed to delete driving routes") + } + showSnackbar(payload.message || "All local driving routes deleted.") + } catch (error) { + showSnackbar(error?.message || "Failed to delete driving routes.", "error") + } finally { + state.routeDeleteBusy = false + } + } + async function runFactoryReset() { if (state.factoryResetBusy) return @@ -262,6 +287,15 @@ export function ToggleControl() { disabled="${() => state.factoryResetBusy}"> ${() => state.factoryResetBusy ? "Starting..." : "SAVE ME"} +
+ Delete local route recordings without changing settings or rebooting the device. +
+ ${() => state.factoryResetStatus ? html`