IQ.Pilot Release Commit @ f2a861c

iqpilot/selfdrive/controls/lib/tests/test_curvature_lookahead.py

@@ -0,0 +1,27 @@
+from types import SimpleNamespace
+
+import numpy as np
+
+from iqpilot.selfdrive.controls.lib.curvature_lookahead import LOOKAHEAD_SECONDS, get_lookahead_curvature
+from iqpilot.selfdrive.controls.lib.drive_helpers import get_curvature_from_plan
+from iqpilot.selfdrive.iqmodeld.config import ModelConstants
+
+
+def test_lookahead_samples_total_delay_horizon():
+  yaws = np.square(np.asarray(ModelConstants.T_IDXS)) * 0.02
+  yaw_rates = np.asarray(ModelConstants.T_IDXS) * 0.04
+  model_v2 = SimpleNamespace(
+    orientation=SimpleNamespace(z=yaws.tolist()),
+    orientationRate=SimpleNamespace(z=yaw_rates.tolist()),
+  )
+  lat_delay = 0.3
+  expected = get_curvature_from_plan(yaws, yaw_rates, ModelConstants.T_IDXS, 20.0, lat_delay + LOOKAHEAD_SECONDS)
+  assert get_lookahead_curvature(model_v2, 20.0, lat_delay) == expected
+
+
+def test_invalid_trajectory_falls_back_to_none():
+  model_v2 = SimpleNamespace(
+    orientation=SimpleNamespace(z=[0.0]),
+    orientationRate=SimpleNamespace(z=[0.0]),
+  )
+  assert get_lookahead_curvature(model_v2, 20.0, 0.3) is None

iqpilot/selfdrive/controls/lib/tests/test_custom_stop_distance.py

@@ -6,8 +6,8 @@ Original concept ("Increased Stop Distance") by SpysyWeeb (github.com/SpysyWeeb)
 from types import SimpleNamespace
 
 from iqdbc.car.interfaces import ACCEL_MIN
-from openpilot.selfdrive.modeld.constants import ModelConstants
-from openpilot.iqpilot.selfdrive.controls.lib.custom_stop_distance import (
+from iqpilot.selfdrive.iqmodeld.config import ModelConstants
+from iqpilot.selfdrive.controls.lib.custom_stop_distance import (
   CustomStopDistance,
   MIN_ADJUSTED_D_REL,
 )

iqpilot/selfdrive/controls/lib/tests/test_iq_dynamic_force_stop.py

@@ -4,8 +4,8 @@ Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, license
 
 from types import SimpleNamespace
 
-from openpilot.common.realtime import DT_MDL
-from openpilot.iqpilot.selfdrive.controls.lib.longitudinal_planner import LongitudinalPlannerIQ
+from iqpilot.common.realtime import DT_MDL
+from iqpilot.selfdrive.controls.lib.iq_longitudinal_planner import LongitudinalPlannerIQ
 
 
 class _FakeIQDynamic:

iqpilot/selfdrive/controls/lib/tests/test_lateral_acceleration_slew_limiter.py

@@ -0,0 +1,87 @@
+import numpy as np
+
+from iqpilot.selfdrive.controls.lib.lateral_acceleration_slew_limiter import (
+  A_LAT_MAX,
+  AVOIDANCE_BYPASS_ACCEL_DELTA,
+  LateralAccelerationSlewLimiter,
+)
+
+
+def test_disabled_is_exact_passthrough_without_state_change():
+  limiter = LateralAccelerationSlewLimiter(False)
+  limiter.reset(1.25)
+  rng = np.random.default_rng(0)
+  for curvature in rng.standard_normal(100):
+    assert limiter.update(curvature, 25.0, 0.01) is curvature
+    assert limiter.a_lim == 1.25
+
+
+def test_step_is_limited_by_speed_scheduled_jerk():
+  limiter = LateralAccelerationSlewLimiter(True)
+  limiter.reset(0.0)
+  v_ego = 20.0
+  dt = 0.01
+  target = 1.5 / v_ego ** 2
+  previous = limiter.a_lim
+  for _ in range(100):
+    limiter.update(target, v_ego, dt)
+    assert abs(limiter.a_lim - previous) <= limiter.jerk_max(v_ego) * dt + 1e-12
+    previous = limiter.a_lim
+
+
+def test_converges_to_held_target():
+  limiter = LateralAccelerationSlewLimiter(True)
+  v_ego = 20.0
+  target_accel = 1.0
+  limiter.reset(0.0)
+  for _ in range(100):
+    limiter.update(target_accel / v_ego ** 2, v_ego, 0.01)
+  assert limiter.a_lim == target_accel
+
+
+def test_reset_prevents_reengagement_jump():
+  limiter = LateralAccelerationSlewLimiter(True)
+  v_ego = 20.0
+  target_accel = 1.0
+  limiter.reset(target_accel)
+  curvature = limiter.update(target_accel / v_ego ** 2, v_ego, 0.01)
+  assert curvature == target_accel / v_ego ** 2
+  assert limiter.a_lim == target_accel
+
+
+def test_low_speed_passes_through_and_resets():
+  limiter = LateralAccelerationSlewLimiter(True)
+  limiter.reset(-1.0)
+  curvature = 0.2
+  assert limiter.update(curvature, 4.0, 0.01) == curvature
+  assert limiter.a_lim == A_LAT_MAX
+
+
+def test_speed_schedule_changes_slew_rate():
+  limiter = LateralAccelerationSlewLimiter(True)
+  limiter.reset(0.0)
+  limiter.update(1.0 / 8.0 ** 2, 8.0, 0.01)
+  low_speed_step = limiter.a_lim
+  limiter.reset(0.0)
+  limiter.update(1.0 / 35.0 ** 2, 35.0, 0.01)
+  high_speed_step = limiter.a_lim
+  assert low_speed_step > high_speed_step
+
+
+def test_sharp_avoidance_bypasses_limiter():
+  limiter = LateralAccelerationSlewLimiter(True)
+  v_ego = 20.0
+  limiter.reset(0.0)
+  target_accel = AVOIDANCE_BYPASS_ACCEL_DELTA + 0.1
+  curvature = limiter.update(target_accel / v_ego ** 2, v_ego, 0.01)
+  assert limiter.a_lim == target_accel
+  assert curvature == target_accel / v_ego ** 2
+
+
+def test_acceleration_space_couples_speed_and_curvature_changes():
+  limiter = LateralAccelerationSlewLimiter(True)
+  limiter.reset(0.5)
+  limiter.update(0.005, 10.0, 0.01)
+  previous = limiter.a_lim
+  limiter.update(0.003, 20.0, 0.01)
+  assert limiter.a_lim - previous <= limiter.jerk_max(20.0) * 0.01 + 1e-12

iqpilot/selfdrive/controls/lib/tests/test_lateral_edge_guard.py

@@ -0,0 +1,236 @@
+"""
+Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos/
+"""
+
+from __future__ import annotations
+
+from dataclasses import dataclass
+import math
+
+from iqpilot.cereal import custom, log
+import iqpilot.cereal.messaging as messaging
+from iqpilot.common.realtime import DT_MDL
+from iqpilot.selfdrive.controls.lib.desire_helper import DesireHelper
+from iqpilot.selfdrive.controls.lib.helpers.lane_change import AutoLaneChangeMode
+from iqpilot.selfdrive.controls.lib.helpers.lateral_edge_guard import (
+  ADJACENT_LANE_LINE_PROB,
+  BLOCK_DEBOUNCE_S,
+  CLEAR_DEBOUNCE_S,
+  LANE_CENTER_OFFSET_M,
+  MAX_MEASURED_LANE_WIDTH_M,
+  MAX_VALID_ROAD_EDGE_STD_M,
+  MIN_ACTIVE_SPEED_MPS,
+  MIN_MEASURED_LANE_WIDTH_M,
+  REQUIRED_ROAD_EDGE_DISTANCE_M,
+  UNAVAILABLE_HOLD_S,
+  LateralEdgeGuard,
+  RoadEdgeDataState,
+  evaluate_road_edge,
+)
+from iqpilot.selfdrive.selfdrived.iq_events import EVENTS_IQ, ET
+from iqpilot.selfdrive.selfdrived.selfdrived import SelfdriveD
+
+
+@dataclass
+class Edge:
+  x: list[float]
+  y: list[float]
+
+
+@dataclass
+class ModelData:
+  roadEdges: list[Edge]
+  roadEdgeStds: list[float]
+
+
+@dataclass
+class LaneModelData:
+  roadEdges: list[Edge]
+  roadEdgeStds: list[float]
+  laneLines: list[Edge]
+  laneLineProbs: list[float]
+
+
+class CarState:
+  def __init__(self, left_blindspot: bool = False) -> None:
+    self.vEgo = MIN_ACTIVE_SPEED_MPS + 1.0
+    self.leftBlinker = True
+    self.rightBlinker = False
+    self.leftBlindspot = left_blindspot
+    self.rightBlindspot = False
+    self.steeringPressed = True
+    self.steeringTorque = 1.0
+    self.brakePressed = False
+    self.standstill = False
+
+
+def edge_model(left_distance_m: float = 6.0, right_distance_m: float = 6.0,
+               left_std_m: float = 0.0, right_std_m: float = 0.0) -> ModelData:
+  xs = [5.0, 20.0, 40.0]
+  return ModelData(
+    [Edge(xs, [-left_distance_m] * len(xs)), Edge(xs, [right_distance_m] * len(xs))],
+    [left_std_m, right_std_m],
+  )
+
+
+def lane_model(left_distance_m: float = 4.0, outer_prob: float = 0.0,
+               ego_width_m: float = 3.5, ego_prob: float = 0.9) -> LaneModelData:
+  xs = [5.0, 20.0, 40.0]
+  base = edge_model(left_distance_m, left_distance_m)
+  half = ego_width_m / 2.0
+  lines = [Edge(xs, [-(half + 3.0)] * 3), Edge(xs, [-half] * 3),
+           Edge(xs, [half] * 3), Edge(xs, [half + 3.0] * 3)]
+  return LaneModelData(base.roadEdges, base.roadEdgeStds, lines,
+                       [outer_prob, ego_prob, ego_prob, outer_prob])
+
+
+def cycles(duration_s: float) -> int:
+  return math.ceil(duration_s / DT_MDL)
+
+
+def update_for(guard: LateralEdgeGuard, modeldata: ModelData | None, duration_s: float,
+               speed_mps: float = MIN_ACTIVE_SPEED_MPS) -> None:
+  for _ in range(cycles(duration_s)):
+    guard.update(modeldata, speed_mps, DT_MDL)
+
+
+def test_valid_geometry_blocks_and_clear_geometry_does_not_block() -> None:
+  blocked = evaluate_road_edge(edge_model(4.0).roadEdges[0], 0.2, log.LaneChangeDirection.left)
+  clear = evaluate_road_edge(edge_model(6.0).roadEdges[0], 0.2, log.LaneChangeDirection.left)
+  assert blocked.state == RoadEdgeDataState.VALID
+  assert blocked.should_block is True
+  assert clear.state == RoadEdgeDataState.VALID
+  assert clear.should_block is False
+
+
+def test_unavailable_and_invalid_are_distinct() -> None:
+  unavailable = evaluate_road_edge(Edge([5.0], []), 0.2, log.LaneChangeDirection.left)
+  invalid = evaluate_road_edge(edge_model().roadEdges[0], MAX_VALID_ROAD_EDGE_STD_M + 0.01,
+                               log.LaneChangeDirection.left)
+  assert unavailable.state == RoadEdgeDataState.UNAVAILABLE
+  assert unavailable.lateral_distance_m is None
+  assert invalid.state == RoadEdgeDataState.INVALID
+  assert invalid.should_block is None
+
+
+def test_distance_threshold_on_either_side() -> None:
+  epsilon_m = 0.001
+  for direction, edge_index in ((log.LaneChangeDirection.left, 0), (log.LaneChangeDirection.right, 1)):
+    below = edge_model(REQUIRED_ROAD_EDGE_DISTANCE_M - epsilon_m, REQUIRED_ROAD_EDGE_DISTANCE_M - epsilon_m)
+    above = edge_model(REQUIRED_ROAD_EDGE_DISTANCE_M + epsilon_m, REQUIRED_ROAD_EDGE_DISTANCE_M + epsilon_m)
+    assert evaluate_road_edge(below.roadEdges[edge_index], 0.0, direction).should_block is True
+    assert evaluate_road_edge(above.roadEdges[edge_index], 0.0, direction).should_block is False
+
+
+def test_disabled_guard_never_blocks() -> None:
+  guard = LateralEdgeGuard(enabled=False)
+  update_for(guard, edge_model(4.0), BLOCK_DEBOUNCE_S * 2)
+  assert guard.block_for_direction(log.LaneChangeDirection.left) == custom.IQLateralEdgeBlock.none
+
+
+def test_enabled_guard_blocks_after_debounce() -> None:
+  guard = LateralEdgeGuard(enabled=True)
+  update_for(guard, edge_model(4.0), BLOCK_DEBOUNCE_S)
+  assert guard.block_for_direction(log.LaneChangeDirection.left) == custom.IQLateralEdgeBlock.left
+
+
+def test_parameter_refresh_controls_guard() -> None:
+  class EdgeGuardParams:
+    enabled = True
+
+    def get_bool(self, key: str) -> bool:
+      assert key == "IQEdgeGuard"
+      return self.enabled
+
+  params = EdgeGuardParams()
+  guard = LateralEdgeGuard(enabled=False)
+  guard._params = params
+  guard._param_refresh_frame = 0
+  update_for(guard, edge_model(4.0), BLOCK_DEBOUNCE_S)
+  assert guard.block_for_direction(log.LaneChangeDirection.left) == custom.IQLateralEdgeBlock.left
+
+  params.enabled = False
+  guard._param_refresh_frame = 50
+  guard.update(edge_model(4.0), MIN_ACTIVE_SPEED_MPS, DT_MDL)
+  assert guard.block_for_direction(log.LaneChangeDirection.left) == custom.IQLateralEdgeBlock.none
+
+
+def test_clear_debounce_rejects_a_single_blocking_frame() -> None:
+  guard = LateralEdgeGuard(enabled=True)
+  update_for(guard, edge_model(4.0), BLOCK_DEBOUNCE_S)
+  update_for(guard, edge_model(6.0), CLEAR_DEBOUNCE_S - DT_MDL)
+  assert guard.block_for_direction(log.LaneChangeDirection.left) == custom.IQLateralEdgeBlock.left
+  guard.update(edge_model(4.0), MIN_ACTIVE_SPEED_MPS, DT_MDL)
+  update_for(guard, edge_model(6.0), CLEAR_DEBOUNCE_S)
+  assert guard.block_for_direction(log.LaneChangeDirection.left) == custom.IQLateralEdgeBlock.none
+
+
+def test_unavailable_holds_then_falls_back_to_not_blocking() -> None:
+  guard = LateralEdgeGuard(enabled=True)
+  update_for(guard, edge_model(4.0), BLOCK_DEBOUNCE_S)
+  update_for(guard, None, UNAVAILABLE_HOLD_S - DT_MDL)
+  assert guard.left_measurement.state == RoadEdgeDataState.UNAVAILABLE
+  assert guard.block_for_direction(log.LaneChangeDirection.left) == custom.IQLateralEdgeBlock.left
+  guard.update(None, MIN_ACTIVE_SPEED_MPS, DT_MDL)
+  assert guard.block_for_direction(log.LaneChangeDirection.left) == custom.IQLateralEdgeBlock.none
+
+
+def test_speed_gate_is_inactive_below_threshold() -> None:
+  guard = LateralEdgeGuard(enabled=True)
+  update_for(guard, edge_model(4.0), BLOCK_DEBOUNCE_S, MIN_ACTIVE_SPEED_MPS - 0.01)
+  assert guard.block_for_direction(log.LaneChangeDirection.left) == custom.IQLateralEdgeBlock.none
+
+
+def test_visible_outer_lane_line_overrides_edge_block() -> None:
+  guard = LateralEdgeGuard(enabled=True)
+  update_for(guard, lane_model(4.0, outer_prob=ADJACENT_LANE_LINE_PROB + 0.2), BLOCK_DEBOUNCE_S * 4)
+  assert guard.block_for_direction(log.LaneChangeDirection.left) == custom.IQLateralEdgeBlock.none
+
+
+def test_measured_lane_width_is_clamped_and_falls_back() -> None:
+  assert LateralEdgeGuard._measured_lane_width(None) == LANE_CENTER_OFFSET_M
+  assert LateralEdgeGuard._measured_lane_width(edge_model(4.0)) == LANE_CENTER_OFFSET_M
+  assert LateralEdgeGuard._measured_lane_width(lane_model(4.0, ego_prob=0.1)) == LANE_CENTER_OFFSET_M
+  assert LateralEdgeGuard._measured_lane_width(lane_model(4.0, ego_width_m=9.0)) == MAX_MEASURED_LANE_WIDTH_M
+  assert LateralEdgeGuard._measured_lane_width(lane_model(4.0, ego_width_m=0.5)) == MIN_MEASURED_LANE_WIDTH_M
+  assert LateralEdgeGuard._measured_lane_width(lane_model(4.0, ego_width_m=3.2)) == 3.2
+
+
+def test_desire_helper_blocks_only_when_edge_guard_is_enabled() -> None:
+  helper = DesireHelper()
+  helper.lateral_edge_guard = LateralEdgeGuard(enabled=True)
+  helper.alc.lane_change_set_timer = AutoLaneChangeMode.NUDGE
+  helper.lane_change_state = log.LaneChangeState.preLaneChange
+  helper.lane_change_direction = log.LaneChangeDirection.left
+  update_for(helper.lateral_edge_guard, edge_model(4.0), BLOCK_DEBOUNCE_S)
+  helper.update(CarState(), True, 1.0, modeldata=edge_model(4.0))
+  assert helper.lateral_edge_block == custom.IQLateralEdgeBlock.left
+  assert helper.lane_change_state == log.LaneChangeState.preLaneChange
+
+  helper.lateral_edge_guard = LateralEdgeGuard(enabled=False)
+  helper.update(CarState(), True, 1.0, modeldata=edge_model(4.0))
+  assert helper.lateral_edge_block == custom.IQLateralEdgeBlock.none
+  assert helper.lane_change_state == log.LaneChangeState.laneChangeStarting
+
+
+def test_published_edge_block_maps_to_distinct_event_and_alert() -> None:
+  message = messaging.new_message("iqDriveModelData")
+  message.iqDriveModelData.lateralEdgeBlock = custom.IQLateralEdgeBlock.right
+
+  class SubMaster:
+    updated = {"iqDriveModelData": True}
+
+    def __getitem__(self, service: str):
+      assert service == "iqDriveModelData"
+      return message.iqDriveModelData
+
+  selfdrived = SelfdriveD.__new__(SelfdriveD)
+  selfdrived.sm = SubMaster()
+  selfdrived._cached_model_event_names = ()
+  selfdrived._refresh_cached_model_events()
+
+  event_name = custom.IQOnroadEvent.EventName.lateralEdgeBlocked
+  assert selfdrived._cached_model_event_names == (event_name,)
+  alert = EVENTS_IQ[event_name][ET.WARNING]
+  assert alert.alert_text_1 == "Lane Change Blocked"
+  assert alert.alert_text_2 == "Road edge detected"

iqpilot/selfdrive/controls/lib/tests/test_longitudinal_planner_accel_state.py

@@ -0,0 +1,190 @@
+"""
+Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
+"""
+import numpy as np
+import pytest
+
+from iqdbc.car.honda.interface import CarInterface
+from iqdbc.car.honda.values import CAR
+from iqpilot.cereal import custom, log
+import iqpilot.cereal.messaging as messaging
+from iqpilot.selfdrive.controls.lib.longcontrol import LongCtrlState
+from iqpilot.selfdrive.controls.lib.longitudinal_planner import LongitudinalPlanner
+from iqpilot.selfdrive.iqmodeld.config import ModelConstants
+
+CRUISE = "cruise"
+SPEED_LIMIT_ASSIST = "speedLimitAssist"
+NAV = "nav"
+SOURCES = [CRUISE, SPEED_LIMIT_ASSIST, NAV]
+PLAN_SOURCE = custom.IQPlan.LongitudinalPlanSource
+
+V_CRUISE_MS = 25.0
+NAV_SPEED_TARGET = 11.0
+SLC_SPEED_TARGET = 12.0
+
+APPROACH_V_EGO = 11.2
+APPROACH_D_REL = 100.0
+APPROACH_STEPS = 250
+MIN_SAFE_GAP = 2.0
+COAST_THROTTLE_PROB = 0.1
+
+
+def build_planner(init_v=V_CRUISE_MS, init_a=0.0):
+  CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
+  CP_IQ = CarInterface.get_non_essential_params_iq(CP, CAR.HONDA_CIVIC)
+  return LongitudinalPlanner(CP, CP_IQ, init_v=init_v, init_a=init_a)
+
+
+def build_sm(v_ego, d_rel, v_lead, source, enabled=True, throttle_prob=1.0, a_ego=0.0, v_cruise=V_CRUISE_MS):
+  radar = messaging.new_message('radarState')
+  control = messaging.new_message('controlsState')
+  ss = messaging.new_message('selfdriveState')
+  car_state = messaging.new_message('carState')
+  car_control = messaging.new_message('carControl')
+  vehicle_params = messaging.new_message('vehicleParameters')
+  model = messaging.new_message('modelV2')
+  iq_car_state = messaging.new_message('iqCarState')
+  iq_nav_state = messaging.new_message('iqNavState')
+  iq_live_data = messaging.new_message('iqLiveData')
+  gps = messaging.new_message('gpsLocation')
+
+  lead = log.RadarState.LeadData.new_message()
+  lead.dRel = float(d_rel)
+  lead.vRel = float(v_lead - v_ego)
+  lead.vLead = float(v_lead)
+  lead.vLeadK = float(v_lead)
+  lead.status = True
+  lead.modelProb = 1.0
+  radar.radarState.leadOne = lead
+
+  t_idxs = np.array(ModelConstants.T_IDXS)
+  position = log.XYZTData.new_message()
+  position.x = [float(x) for x in v_ego * t_idxs]
+  model.modelV2.position = position
+  velocity = log.XYZTData.new_message()
+  velocity.x = [float(v_ego) for _ in t_idxs]
+  model.modelV2.velocity = velocity
+  acceleration = log.XYZTData.new_message()
+  acceleration.x = [0.0 for _ in t_idxs]
+  model.modelV2.acceleration = acceleration
+  model.modelV2.action.desiredAcceleration = 0.0
+  model.modelV2.meta.disengagePredictions.gasPressProbs = [float(throttle_prob) for _ in range(6)]
+
+  lead_times = np.array(ModelConstants.LEAD_T_IDXS)
+  for lead_prediction in model.modelV2.leadsV3:
+    lead_prediction.prob = 1.0
+    lead_prediction.x = [float(d_rel + v_lead * t) for t in lead_times]
+    lead_prediction.v = [float(v_lead) for _ in lead_times]
+
+  control.controlsState.longControlState = LongCtrlState.pid if enabled else LongCtrlState.off
+  ss.selfdriveState.enabled = enabled
+  car_state.carState.vEgo = float(v_ego)
+  car_state.carState.aEgo = float(a_ego)
+  car_state.carState.standstill = bool(v_ego < 0.01)
+  car_state.carState.vCruise = float(v_cruise * 3.6)
+  car_control.carControl.orientationNED = [0.0, 0.0, 0.0]
+
+  if source == NAV:
+    iq_nav_state.iqNavState.longitudinalEngaged = True
+    iq_nav_state.iqNavState.valid = True
+    iq_nav_state.iqNavState.speedTarget = NAV_SPEED_TARGET
+    iq_nav_state.iqNavState.accelTarget = 0.0
+
+  return {
+    'radarState': radar.radarState,
+    'carState': car_state.carState,
+    'carControl': car_control.carControl,
+    'controlsState': control.controlsState,
+    'selfdriveState': ss.selfdriveState,
+    'vehicleParameters': vehicle_params.vehicleParameters,
+    'modelV2': model.modelV2,
+    'iqCarState': iq_car_state.iqCarState,
+    'iqNavState': iq_nav_state.iqNavState,
+    'iqLiveData': iq_live_data.iqLiveData,
+    'gpsLocation': gps.gpsLocation,
+  }
+
+
+def stub_speed_limit_assist(planner):
+  planner.slimit.update = lambda *args, **kwargs: SLC_SPEED_TARGET
+
+
+def run_approach(planner, source, v_ego_0=APPROACH_V_EGO, d_rel_0=APPROACH_D_REL,
+                 steps=APPROACH_STEPS, throttle_prob=COAST_THROTTLE_PROB):
+  if source == SPEED_LIMIT_ASSIST:
+    stub_speed_limit_assist(planner)
+
+  v_ego = v_ego_0
+  d_rel = d_rel_0
+  prev_output_a_target = None
+  trace = []
+  for _ in range(steps):
+    planner.update(build_sm(v_ego, d_rel, 0.0, source, throttle_prob=throttle_prob))
+    trace.append({
+      'v_ego': v_ego,
+      'd_rel': d_rel,
+      'accels_0': float(planner.a_desired_trajectory[0]),
+      'prev_output_a_target': prev_output_a_target,
+      'output_a_target': float(planner.output_a_target),
+    })
+    prev_output_a_target = float(planner.output_a_target)
+    v_ego = max(0.0, v_ego + prev_output_a_target * planner.dt)
+    d_rel = max(0.0, d_rel - v_ego * planner.dt)
+  return trace
+
+
+@pytest.mark.parametrize("source", SOURCES)
+def test_mpc_initial_accel_state_carries_previous_command(source):
+  planner = build_planner(init_v=APPROACH_V_EGO)
+  trace = run_approach(planner, source)
+
+  for i, step in enumerate(trace):
+    if step['prev_output_a_target'] is None:
+      continue
+    assert step['accels_0'] == pytest.approx(step['prev_output_a_target'], abs=1e-6), (
+      f"step {i} source={source}: MPC initial accel state was {step['accels_0']:.4f} "
+      f"but the previous commanded accel was {step['prev_output_a_target']:.4f}"
+    )
+
+
+@pytest.mark.parametrize("source", SOURCES)
+def test_brakes_for_stopped_lead(source):
+  planner = build_planner(init_v=APPROACH_V_EGO)
+  trace = run_approach(planner, source)
+
+  min_gap = min(step['d_rel'] for step in trace)
+  assert min_gap > MIN_SAFE_GAP, (
+    f"source={source}: closed to {min_gap:.2f} m of a stopped lead first seen at "
+    f"{APPROACH_D_REL:.0f} m while coasting from {APPROACH_V_EGO:.1f} m/s"
+  )
+
+
+@pytest.mark.parametrize("source,expected", [
+  (CRUISE, V_CRUISE_MS),
+  (SPEED_LIMIT_ASSIST, SLC_SPEED_TARGET),
+  (NAV, NAV_SPEED_TARGET),
+])
+def test_speed_source_arbitration_unchanged(source, expected):
+  planner = build_planner(init_v=APPROACH_V_EGO)
+  if source == SPEED_LIMIT_ASSIST:
+    stub_speed_limit_assist(planner)
+  planner.update(build_sm(APPROACH_V_EGO, APPROACH_D_REL, 0.0, source))
+
+  assert planner.output_v_target == pytest.approx(expected, abs=1e-6)
+  assert planner.source == getattr(PLAN_SOURCE, source)
+
+
+def test_cruise_accel_initializes_from_planner_accel():
+  planner = build_planner(init_a=-0.35)
+
+  assert planner.a_cruise == pytest.approx(-0.35)
+
+
+def test_cruise_accel_resets_from_measured_accel():
+  a_ego = -0.45
+  v_ego = 20.0
+  planner = build_planner(init_v=v_ego)
+  planner.a_cruise = 0.5
+  planner.update(build_sm(v_ego, APPROACH_D_REL, v_ego, CRUISE, enabled=False, a_ego=a_ego, v_cruise=v_ego + a_ego))
+
+  assert planner.a_cruise == pytest.approx(a_ego, abs=1e-6)

iqpilot/selfdrive/controls/lib/tests/test_nav_exit_lane_change.py

@@ -1,9 +1,9 @@
 """
 Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
 """
-from cereal import custom, log
-from openpilot.selfdrive.controls.lib.desire_helper import DesireHelper, LaneChangeState
-from openpilot.iqpilot.selfdrive.controls.lib.helpers.lane_change import AutoLaneChangeMode
+from iqpilot.cereal import custom, log
+from iqpilot.selfdrive.controls.lib.desire_helper import DesireHelper, LaneChangeState
+from iqpilot.selfdrive.controls.lib.helpers.lane_change import AutoLaneChangeMode
 
 ManeuverType = custom.IQNavState.ManeuverType
 NavDirection = custom.NavDirection

iqpilot/selfdrive/controls/lib/tests/test_slc_speed_limit_control.py

@@ -5,10 +5,15 @@ Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed
 from datetime import datetime
 from types import SimpleNamespace
 
-from openpilot.common.constants import CV
-from openpilot.iqpilot.common.slc_variables import OFFSET_MAP_IMPERIAL
-from openpilot.iqpilot.selfdrive.controls.lib.slc_vcruise import SLCVCruise, CRUISING_SPEED
-from openpilot.iqpilot.selfdrive.controls.lib.speed_limit_controller import SpeedLimitController, POLICY_MAP_DATA_PRIORITY, POLICY_COMBINED
+import pytest
+
+from iqpilot.cereal import car, custom
+from iqpilot.common.constants import CV
+from iqpilot.common.slc_variables import OFFSET_MAP_IMPERIAL
+from iqpilot.selfdrive.car.cruise import VCruiseHelper
+from iqpilot.selfdrive.controls.lib.iq_longitudinal_planner import LongitudinalPlannerIQ
+from iqpilot.selfdrive.controls.lib.slc_vcruise import SLCVCruise, CRUISING_SPEED
+from iqpilot.selfdrive.controls.lib.speed_limit_controller import SpeedLimitController, POLICY_MAP_DATA_PRIORITY, POLICY_COMBINED
 
 
 class FakeParams:
@@ -36,7 +41,7 @@ def _build_sm(v_cruise_cluster=100.0, v_ego_cluster=27.8, gas=False, enabled=Tru
                                 steeringAngleDeg=0.0, buttonEvents=[]),
     "iqCarState": SimpleNamespace(speedLimit=iq_limit, accelPressed=False, decelPressed=False),
     "selfdriveState": SimpleNamespace(enabled=enabled),
-    "liveParameters": SimpleNamespace(angleOffsetDeg=0.0),
+    "vehicleParameters": SimpleNamespace(angleOffsetDeg=0.0),
   }
 
 
@@ -61,6 +66,9 @@ class _FakeSLC:
   def update_override(self, *_args, **_kwargs):
     self.update_override_calls += 1
 
+  def reset_override(self, _sm):
+    self.overridden_speed = 0.0
+
   def get_offset(self, _is_metric):
     return self._offset
 
@@ -261,6 +269,200 @@ def test_slc_vcruise_does_not_auto_raise_when_higher_confirmation_enabled():
   assert out == v_cruise
 
 
+@pytest.fixture(params=[True, False], ids=["metric", "imperial"])
+def set_speed_slc(request, monkeypatch):
+  monkeypatch.setattr("iqpilot.selfdrive.controls.lib.slc_vcruise.Params", FakeParams)
+  slc = SLCVCruise()
+  slc._maybe_log_debug = lambda *_args: None
+  slc.slc.update_gps = lambda _sm: None
+  slc.slc._resolver.update_map_data = lambda *_args: None
+  params = _base_slc_params_controller() | {
+    "speed_limit_controller": True,
+    "speed_limit_mode": 3,
+    "show_speed_limits": False,
+    "is_metric": request.param,
+    "slc_online_filler": False,
+    "slc_fallback_experimental_mode": False,
+    "speed_limit_controller_override_manual": False,
+    "speed_limit_controller_override_set_speed": True,
+  }
+  slc._get_slc_params = lambda: params
+  unit = CV.KPH_TO_MS if request.param else CV.MPH_TO_MS
+  slc.slc._resolver.map_speed_limit = 50 * unit
+  sm = _FakeSM(_build_sm(v_ego_cluster=50 * unit))
+  sm["iqCarState"].slcSetSpeedRequestId = 0
+  sm["iqCarState"].slcSetSpeedGestureId = 0
+  sm["iqCarState"].slcSetSpeedRequestKph = 0.0
+
+  def step(speed, increase=False, new_gesture=False):
+    sm["carState"].vCruiseCluster = speed * unit * CV.MS_TO_KPH
+    if new_gesture:
+      sm["iqCarState"].slcSetSpeedGestureId += 1
+    if increase:
+      sm["iqCarState"].slcSetSpeedRequestId += 1
+      sm["iqCarState"].slcSetSpeedRequestKph = sm["carState"].vCruiseCluster
+    target = slc.update(sm["selfdriveState"].enabled, None, True, speed * unit, 50 * unit, sm)
+    return min(speed * unit, target) / unit
+
+  step(50)
+  return SimpleNamespace(slc=slc, params=params, sm=sm, unit=unit, step=step)
+
+
+@pytest.mark.parametrize("confirm_higher", [False, True])
+def test_set_speed_override_tracks_driver_adjustments(set_speed_slc, confirm_higher):
+  system = set_speed_slc
+  system.params["speed_limit_confirmation_higher"] = confirm_higher
+  assert system.step(50, new_gesture=True) == pytest.approx(50)
+  assert system.step(55, increase=True) == pytest.approx(55)
+  assert system.step(60, increase=True) == pytest.approx(60)
+  assert system.step(60) == pytest.approx(60)
+  assert system.step(55) == pytest.approx(55)
+  assert system.step(50) == pytest.approx(50)
+  assert not system.slc.slc.override_slc
+  assert system.step(45) == pytest.approx(45)
+  assert system.step(60) == pytest.approx(50)
+  assert system.step(61, increase=True, new_gesture=True) == pytest.approx(61)
+
+
+def test_set_speed_override_ignores_automatic_speed_changes(set_speed_slc):
+  system = set_speed_slc
+  assert system.step(80) == pytest.approx(50)
+  system.slc.slc._resolver.map_speed_limit = 60 * system.unit
+  assert system.step(60) == pytest.approx(60)
+  assert system.step(80) == pytest.approx(60)
+  assert system.slc.slc.overridden_speed == 0
+
+
+@pytest.mark.parametrize("limit", [40, 55])
+def test_set_speed_override_resets_on_accepted_limit(set_speed_slc, limit):
+  system = set_speed_slc
+  assert system.step(60, increase=True, new_gesture=True) == pytest.approx(60)
+  system.slc.slc._resolver.map_speed_limit = limit * system.unit
+  assert system.step(65, increase=True) == pytest.approx(limit)
+  assert system.step(70, increase=True) == pytest.approx(limit)
+  assert system.step(71, increase=True, new_gesture=True) == pytest.approx(71)
+
+
+@pytest.mark.parametrize("limit,button", [(40, "decelCruise"), (55, "accelCruise")])
+def test_set_speed_override_does_not_reuse_confirmation_gesture(set_speed_slc, limit, button):
+  system = set_speed_slc
+  system.params["speed_limit_confirmation_higher"] = True
+  system.params["speed_limit_confirmation_lower"] = True
+  system.slc.slc._resolver.map_speed_limit = limit * system.unit
+  system.step(50, new_gesture=True)
+  assert system.slc.assist_state == custom.IQPlan.SpeedLimit.AssistState.preActive
+  assert system.step(60, increase=True) == pytest.approx(50)
+  system.sm["carState"].buttonEvents = [car.CarState.ButtonEvent(type=button, pressed=False)]
+  assert system.step(61, increase=True) == pytest.approx(limit)
+  system.sm["carState"].buttonEvents = []
+  assert system.step(65, increase=True) == pytest.approx(limit)
+  assert system.step(66, increase=True, new_gesture=True) == pytest.approx(66)
+
+
+@pytest.mark.parametrize("reset", ["disengage", "information", "off", "missing_limit"])
+def test_set_speed_override_cannot_survive_reset(set_speed_slc, reset):
+  system = set_speed_slc
+  assert system.step(60, increase=True, new_gesture=True) == pytest.approx(60)
+  if reset == "disengage":
+    system.sm["selfdriveState"].enabled = False
+  elif reset in ("information", "off"):
+    system.params["speed_limit_controller"] = False
+    system.params["show_speed_limits"] = reset == "information"
+  else:
+    system.slc.slc._resolver.map_speed_limit = 0
+  system.step(60)
+  assert system.slc.slc.overridden_speed == 0
+  system.sm["selfdriveState"].enabled = True
+  system.params["speed_limit_controller"] = True
+  system.slc.slc._resolver.map_speed_limit = 50 * system.unit
+  assert system.step(60) == pytest.approx(50)
+  assert system.step(65, increase=True) == pytest.approx(50)
+  assert system.step(66, increase=True, new_gesture=True) == pytest.approx(66)
+
+
+def test_set_speed_override_respects_offset(set_speed_slc):
+  system = set_speed_slc
+  system.slc.slc.params.put("speed_limit_offset1", 10)
+  system.slc.slc.params.put("speed_limit_offset2", 10)
+  system.slc.slc.params.put("speed_limit_offset3", 10)
+  system.slc.slc._offset_cache.clear()
+  system.step(50, new_gesture=True)
+  assert system.step(52, increase=True) == pytest.approx(52)
+  assert not system.slc.slc.override_slc
+  assert system.step(56, increase=True) == pytest.approx(56)
+  assert system.step(55) == pytest.approx(55)
+  assert not system.slc.slc.override_slc
+
+
+def test_manual_override_still_requires_accelerator(set_speed_slc):
+  system = set_speed_slc
+  system.params["speed_limit_controller_override_set_speed"] = False
+  system.params["speed_limit_controller_override_manual"] = True
+  assert system.step(60, increase=True, new_gesture=True) == pytest.approx(50)
+  system.sm["carState"].gasPressed = True
+  system.sm["carState"].vEgoCluster = 55 * system.unit
+  system.slc.slc.update_override(60 * system.unit, 0, 55 * system.unit, 0, system.sm, system.params, system.params["is_metric"])
+  assert system.slc.slc.overridden_speed == pytest.approx(55 * system.unit)
+  system.sm["carState"].gasPressed = False
+  system.slc.slc.update_override(60 * system.unit, 0, 55 * system.unit, 0, system.sm, system.params, system.params["is_metric"])
+  assert system.slc.slc.overridden_speed == pytest.approx(55 * system.unit)
+
+
+@pytest.mark.parametrize("pcm_cruise", [False, True])
+def test_driver_increase_reaches_slc_without_transient_button_events(set_speed_slc, pcm_cruise):
+  system = set_speed_slc
+  helper = VCruiseHelper(car.CarParams(pcmCruise=pcm_cruise), custom.IQCarParams(pcmCruiseSpeed=True))
+  helper.set_speed_to_limit = False
+  helper.v_cruise_kph = helper.v_cruise_cluster_kph = 50 * system.unit * CV.MS_TO_KPH
+  state = car.CarState(cruiseState={"available": True, "speed": 50 * system.unit, "speedCluster": 50 * system.unit})
+  helper.update_v_cruise(state, True, system.params["is_metric"])
+  state.buttonEvents = [car.CarState.ButtonEvent(type="accelCruise", pressed=True)]
+  helper.update_v_cruise(state, True, system.params["is_metric"])
+  state.buttonEvents = [car.CarState.ButtonEvent(type="accelCruise", pressed=False)]
+  if pcm_cruise:
+    state.cruiseState.speed = state.cruiseState.speedCluster = 51 * system.unit
+  helper.update_v_cruise(state, True, system.params["is_metric"])
+  state.buttonEvents = []
+  for _ in range(5):
+    helper.update_v_cruise(state, True, system.params["is_metric"])
+  system.sm["iqCarState"] = custom.IQCarState.new_message(
+    slcSetSpeedRequestId=helper.slc_set_speed_request_id,
+    slcSetSpeedGestureId=helper.slc_set_speed_gesture_id,
+    slcSetSpeedRequestKph=helper.slc_set_speed_request_kph,
+  )
+  requested = helper.v_cruise_kph * CV.KPH_TO_MS / system.unit
+  assert requested > 50
+  assert system.step(requested) == pytest.approx(requested)
+
+
+def test_set_speed_override_keeps_navigation_constraint(set_speed_slc):
+  system = set_speed_slc
+  assert system.step(60, increase=True, new_gesture=True) == pytest.approx(60)
+  planner = LongitudinalPlannerIQ.__new__(LongitudinalPlannerIQ)
+  planner.slimit = system.slc
+  planner.iq_dynamic = SimpleNamespace(
+    set_slc_experimental_mode=lambda _mode: None, update=lambda _sm: None, force_stop_requested=lambda: False)
+  planner.force_stop_timer = 0.0
+  planner.override_force_stop_timer = 0.0
+  planner.override_force_stop = False
+  system.sm["iqNavState"] = SimpleNamespace(longitudinalEngaged=False, valid=False)
+  assert planner.update_targets(system.sm, 50 * system.unit, 60 * system.unit) == pytest.approx(60 * system.unit)
+  system.sm["iqNavState"] = SimpleNamespace(longitudinalEngaged=True, valid=True, speedTarget=40 * system.unit)
+  assert planner.update_targets(system.sm, 50 * system.unit, 60 * system.unit) == pytest.approx(40 * system.unit)
+
+
+def test_set_speed_override_cannot_bypass_construction_zone(set_speed_slc):
+  system = set_speed_slc
+  assert system.step(60, increase=True, new_gesture=True) == pytest.approx(60)
+  system.params["construction_zone_assist"] = True
+  system.params["construction_zone_speed"] = 40
+  system.sm["iqConstructionZone"] = SimpleNamespace(active=True)
+  system.sm.alive["iqConstructionZone"] = True
+  assert system.step(60) == pytest.approx(40)
+  assert system.step(65, increase=True, new_gesture=True) == pytest.approx(40)
+  assert not system.slc.slc.override_slc
+
+
 class _FakeSM(dict):
   def __init__(self, services, alive=None):
     super().__init__(services)
@@ -443,7 +645,7 @@ def test_get_offset_percent_clamped():
 
 
 def test_construction_zone_fires_event_once_per_zone_entry():
-  from cereal import custom
+  from iqpilot.cereal import custom
   event = custom.IQOnroadEvent.EventName.constructionZoneDetected
 
   controller = _construction_controller()
@@ -461,3 +663,51 @@ def test_construction_zone_fires_event_once_per_zone_entry():
   assert event not in controller.pending_events
   controller.update_limits(0.0, None, True, 33.0, 30.0, _construction_sm(), slc_params)
   assert event in controller.pending_events
+
+
+@pytest.mark.parametrize("alive,valid,limit_valid,limit", [
+  (False, True, True, 25.0), (True, False, True, 25.0), (True, True, False, 25.0),
+  (True, True, True, 0.0), (True, True, True, float("nan")), (True, True, True, float("inf")),
+])
+def test_navigation_mapbox_limit_requires_fresh_valid_data(alive, valid, limit_valid, limit):
+  controller = _construction_controller()
+  controller.get_tomtom_speed_limit = lambda *_args: None
+  controller.mapbox_limit = 20.0
+  sm = _FakeSM(_build_sm())
+  sm["iqNavState"] = custom.IQNavState.new_message(mapboxSpeedLimit=limit, mapboxSpeedLimitValid=limit_valid)
+  sm.alive["iqNavState"] = alive
+  sm.valid = {"iqNavState": valid}
+  controller.update_limits(0, datetime.now(), True, 30, 20, sm, _base_slc_params_controller())
+  assert controller.target == pytest.approx(20.0)
+  assert controller.source == "Mapbox"
+
+
+@pytest.mark.parametrize("policy,expected", [(0, 0.0), (1, 25.0), (2, 25.0)])
+@pytest.mark.parametrize("online_filler", [False, True])
+def test_navigation_mapbox_only_limit_obeys_slc_policy(policy, expected, online_filler):
+  controller = _construction_controller()
+  controller.get_tomtom_speed_limit = lambda *_args: None
+  sm = _FakeSM(_build_sm())
+  sm["iqNavState"] = custom.IQNavState.new_message(mapboxSpeedLimit=25.0, mapboxSpeedLimitValid=True)
+  sm.alive["iqNavState"] = True
+  sm.valid = {"iqNavState": True}
+  params = _base_slc_params_controller() | {"slc_policy": policy, "slc_online_filler": online_filler}
+  controller.update_limits(0, datetime.now(), True, 30, 20, sm, params)
+  assert controller.target == pytest.approx(expected)
+
+
+def test_navigation_mapbox_limit_requires_confirmation_before_override(set_speed_slc):
+  system = set_speed_slc
+  system.params["speed_limit_confirmation_higher"] = True
+  system.slc.slc._resolver.map_speed_limit = 0
+  system.sm["iqNavState"] = custom.IQNavState.new_message(mapboxSpeedLimit=60 * system.unit, mapboxSpeedLimitValid=True)
+  system.sm.alive["iqNavState"] = True
+  system.sm.valid = {"iqNavState": True}
+  system.step(50, new_gesture=True)
+  assert system.slc.assist_state == custom.IQPlan.SpeedLimit.AssistState.preActive
+  assert system.step(70, increase=True) == pytest.approx(50)
+  system.sm["carState"].buttonEvents = [car.CarState.ButtonEvent(type="accelCruise", pressed=False)]
+  assert system.step(71, increase=True) == pytest.approx(60)
+  system.sm["carState"].buttonEvents = []
+  assert system.step(72, increase=True) == pytest.approx(60)
+  assert system.step(73, increase=True, new_gesture=True) == pytest.approx(73)

iqpilot/selfdrive/controls/lib/tests/test_smooth_stops.py

@@ -3,8 +3,8 @@ Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed
 
 Original concept and implementation by SpysyWeeb (github.com/SpysyWeeb)
 """
-from openpilot.common.realtime import DT_CTRL
-from openpilot.iqpilot.selfdrive.controls.lib.smooth_stops import (
+from iqpilot.common.realtime import DT_CTRL
+from iqpilot.selfdrive.controls.lib.smooth_stops import (
   SmoothStopController,
   read_smooth_stops_enabled,
   STANDSTILL_SPEED,

iqpilot/selfdrive/controls/lib/tests/test_turn_desire_cycling.py

@@ -0,0 +1,78 @@
+from types import SimpleNamespace
+
+import pytest
+
+from iqpilot.cereal import custom, log
+from iqpilot.common.realtime import DT_MDL
+from iqpilot.selfdrive.controls.lib.desire_helper import (
+  DesireHelper,
+  TURN_DESIRE_STOP_CYCLE_TIME,
+  TURN_DESIRE_STOP_HOLD_TIME,
+)
+
+
+TurnDirection = custom.IQTurnSignalDirection
+
+
+def helper(v_ego=0.0, yaw_rate=0.0):
+  result = DesireHelper.__new__(DesireHelper)
+  result._last_carstate = SimpleNamespace(vEgo=v_ego, yawRate=yaw_rate)
+  result.turn_desire_stop_timer = 0.0
+  result.turn_desire_stop_active = False
+  result.turn_desire_cycle_input = log.Desire.none
+  result.turn_desire_committed = False
+  result.nav_turn_direction = TurnDirection.none
+  result.lane_turn_direction = TurnDirection.none
+  result.lane_change_direction = log.LaneChangeDirection.none
+  result.lane_change_state = log.LaneChangeState.off
+  result.desire = log.Desire.none
+  return result
+
+
+@pytest.mark.parametrize("source", ["manual", "nav"])
+def test_manual_and_nav_turn_desires_receive_rising_edges(source):
+  h = helper()
+  if source == "manual":
+    h.lane_turn_direction = TurnDirection.turnLeft
+  else:
+    h.nav_turn_direction = TurnDirection.turnLeft
+
+  outputs = []
+  for _ in range(round((TURN_DESIRE_STOP_CYCLE_TIME + 2 * DT_MDL) / DT_MDL)):
+    h._pick_desire_output()
+    outputs.append(h.desire)
+
+  gap_index = next(i for i, output in enumerate(outputs) if output == log.Desire.none)
+  assert gap_index * DT_MDL == pytest.approx(TURN_DESIRE_STOP_HOLD_TIME, abs=DT_MDL * 1.1)
+  assert log.Desire.turnLeft in outputs[gap_index + 1:]
+
+
+def test_creeping_restarts_stopped_turn_cycle():
+  h = helper()
+  for _ in range(round(TURN_DESIRE_STOP_HOLD_TIME / DT_MDL)):
+    h._cycle_turn_desire_when_stopped(log.Desire.turnRight)
+
+  h._last_carstate.vEgo = 3.0
+  assert h._cycle_turn_desire_when_stopped(log.Desire.turnRight) == log.Desire.turnRight
+  h._last_carstate.vEgo = 0.0
+  assert h._cycle_turn_desire_when_stopped(log.Desire.turnRight) == log.Desire.turnRight
+  assert h.turn_desire_stop_timer == pytest.approx(DT_MDL)
+
+
+def test_measured_turn_commitment_stops_cycling():
+  h = helper()
+  h._last_carstate.yawRate = -0.1
+  assert h._cycle_turn_desire_when_stopped(log.Desire.turnLeft) == log.Desire.turnLeft
+  h._last_carstate.yawRate = 0.0
+
+  outputs = [h._cycle_turn_desire_when_stopped(log.Desire.turnLeft) for _ in range(300)]
+  assert set(outputs) == {log.Desire.turnLeft}
+
+
+def test_new_turn_direction_rearms_cycle_after_commitment():
+  h = helper(yaw_rate=0.1)
+  h._cycle_turn_desire_when_stopped(log.Desire.turnLeft)
+  h._last_carstate.yawRate = 0.0
+  h._cycle_turn_desire_when_stopped(log.Desire.turnRight)
+  assert h.turn_desire_committed is False
+  assert h.turn_desire_cycle_input == log.Desire.turnRight