IQ.Pilot Prebuilt Release @ da36e21

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

@@ -0,0 +1,118 @@
+"""
+Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
+
+Original concept ("Increased Stop Distance") by SpysyWeeb (github.com/SpysyWeeb)
+"""
+from types import SimpleNamespace
+
+from iqdbc.car.interfaces import ACCEL_MIN
+from iqpilot.selfdrive.iqmodeld.config import ModelConstants
+from iqpilot.selfdrive.controls.lib.custom_stop_distance import (
+  CustomStopDistance,
+  MIN_ADJUSTED_D_REL,
+)
+
+
+def _build(distance):
+  c = CustomStopDistance.__new__(CustomStopDistance)
+  c.frame = 0
+  c.distance = float(distance)
+  return c
+
+
+def _model_msg(stop_distance, end_velocity):
+  x = [0.0] * (ModelConstants.IDX_N - 1) + [stop_distance]
+  v = [0.0] * (ModelConstants.IDX_N - 1) + [end_velocity]
+  return SimpleNamespace(position=SimpleNamespace(x=x), velocity=SimpleNamespace(x=v))
+
+
+def test_zero_distance_is_a_no_op():
+  c = _build(0)
+  lead = {'status': True, 'dRel': 10.0, 'vLead': 0.0}
+  assert c.apply_lead(dict(lead)) == lead
+
+
+def test_positive_distance_reduces_reported_lead_distance():
+  c = _build(2)
+  lead = {'status': True, 'dRel': 10.0, 'vLead': 0.0}
+  out = c.apply_lead(dict(lead))
+  assert out['dRel'] == 8.0
+
+
+def test_negative_distance_increases_reported_lead_distance():
+  c = _build(-2)
+  lead = {'status': True, 'dRel': 10.0, 'vLead': 0.0}
+  out = c.apply_lead(dict(lead))
+  assert out['dRel'] == 12.0
+
+
+def test_positive_distance_never_reports_below_floor():
+  c = _build(2)
+  lead = {'status': True, 'dRel': 1.5, 'vLead': 0.0}
+  out = c.apply_lead(dict(lead))
+  assert out['dRel'] == MIN_ADJUSTED_D_REL
+
+
+def test_positive_distance_never_reports_further_than_reality():
+  c = _build(2)
+  lead = {'status': True, 'dRel': 0.5, 'vLead': 0.0}
+  out = c.apply_lead(dict(lead))
+  assert out['dRel'] == 0.5
+
+
+def test_offset_fades_out_as_lead_speeds_up():
+  c = _build(2)
+  lead = {'status': True, 'dRel': 10.0, 'vLead': 3.0}
+  out = c.apply_lead(dict(lead))
+  assert out['dRel'] == 10.0
+
+
+def test_no_lead_is_untouched():
+  c = _build(2)
+  lead = {'status': False, 'dRel': 10.0, 'vLead': 0.0}
+  out = c.apply_lead(dict(lead))
+  assert out['dRel'] == 10.0
+
+
+def test_e2e_negative_distance_is_a_no_op():
+  c = _build(-2)
+  a_target, should_stop = c.adjust_e2e_stop(-0.5, False, 0.2, _model_msg(3.0, 0.0))
+  assert (a_target, should_stop) == (-0.5, False)
+
+
+def test_e2e_zero_distance_is_a_no_op():
+  c = _build(0)
+  a_target, should_stop = c.adjust_e2e_stop(-0.5, False, 0.2, _model_msg(3.0, 0.0))
+  assert (a_target, should_stop) == (-0.5, False)
+
+
+def test_e2e_stop_sign_plans_are_untouched():
+  c = _build(2)
+  # model plan still moving at the end -> proceeding through (stop sign), not held
+  a_target, should_stop = c.adjust_e2e_stop(-0.5, False, 0.2, _model_msg(3.0, 5.0))
+  assert (a_target, should_stop) == (-0.5, False)
+
+
+def test_e2e_holds_short_of_model_stop_when_already_stopped():
+  c = _build(2)
+  a_target, should_stop = c.adjust_e2e_stop(0.0, False, 0.1, _model_msg(stop_distance=3.0, end_velocity=0.0))
+  assert should_stop is True
+
+
+def test_e2e_does_not_hold_once_past_offset_and_buffer():
+  c = _build(2)
+  a_target, should_stop = c.adjust_e2e_stop(0.0, False, 0.1, _model_msg(stop_distance=10.0, end_velocity=0.0))
+  assert should_stop is False
+
+
+def test_e2e_deepens_braking_already_in_progress():
+  c = _build(2)
+  a_target, should_stop = c.adjust_e2e_stop(-0.5, False, 5.0, _model_msg(stop_distance=10.0, end_velocity=0.0))
+  assert a_target < -0.5
+  assert a_target >= ACCEL_MIN
+
+
+def test_e2e_never_relaxes_braking():
+  c = _build(2)
+  a_target, should_stop = c.adjust_e2e_stop(0.0, False, 5.0, _model_msg(stop_distance=10.0, end_velocity=0.0))
+  assert a_target == 0.0

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

@@ -0,0 +1,62 @@
+"""
+Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
+"""
+
+from types import SimpleNamespace
+
+from iqpilot.common.realtime import DT_MDL
+from iqpilot.selfdrive.controls.lib.iq_longitudinal_planner import LongitudinalPlannerIQ
+
+
+class _FakeIQDynamic:
+  def __init__(self, requested=True, model_length=4.0, model_stop_time=5.0, minimum_force_stop_length=15.0):
+    self._requested = requested
+    self.model_length = model_length
+    self.model_stop_time = model_stop_time
+    self.minimum_force_stop_length = minimum_force_stop_length
+
+  def force_stop_requested(self):
+    return self._requested
+
+
+def _build_planner(iq_dynamic):
+  planner = LongitudinalPlannerIQ.__new__(LongitudinalPlannerIQ)
+  planner.iq_dynamic = iq_dynamic
+  planner.force_stop_timer = 0.0
+  planner.forcing_stop = False
+  planner.override_force_stop = False
+  planner.override_force_stop_timer = 0.0
+  planner.tracked_model_length = 0.0
+  return planner
+
+
+def _build_sm(gas_pressed=False, accel_pressed=False, standstill=False):
+  return {
+    "carState": SimpleNamespace(gasPressed=gas_pressed, standstill=standstill),
+    "iqCarState": SimpleNamespace(accelPressed=accel_pressed),
+  }
+
+
+def test_force_stop_uses_model_stop_time_as_ramp():
+  planner = _build_planner(_FakeIQDynamic(model_length=20.0, model_stop_time=5.0, minimum_force_stop_length=0.0))
+  sm = _build_sm()
+
+  output = 12.0
+  for _ in range(int(1.0 / DT_MDL)):
+    output = planner._apply_force_stop(12.0, 0.0, sm, True)
+
+  assert planner.forcing_stop
+  assert output == 4.0
+
+
+def test_force_stop_respects_minimum_force_stop_length():
+  planner = _build_planner(_FakeIQDynamic(model_length=4.0, model_stop_time=5.0, minimum_force_stop_length=15.0))
+  sm = _build_sm()
+
+  output = 12.0
+  for _ in range(int(1.0 / DT_MDL)):
+    output = planner._apply_force_stop(12.0, 0.0, sm, True)
+
+  assert planner.forcing_stop
+  assert planner.tracked_model_length == 15.0
+  assert output == 3.0

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,195 @@
+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 (
+  BLOCK_DEBOUNCE_S,
+  CLEAR_DEBOUNCE_S,
+  MAX_VALID_ROAD_EDGE_STD_M,
+  MIN_ACTIVE_SPEED_MPS,
+  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]
+
+
+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 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_two_sigma_bound_uses_std_in_metres() -> None:
+  measurement = evaluate_road_edge(edge_model(5.0).roadEdges[0], 0.2, log.LaneChangeDirection.left)
+  assert measurement.lateral_distance_m == 5.0
+  assert measurement.conservative_distance_m == 4.6
+  assert measurement.should_block is True
+
+
+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_block_debounce_rejects_a_single_clear_frame() -> None:
+  guard = LateralEdgeGuard()
+  blocking = edge_model(4.0)
+  clear = edge_model(6.0)
+  update_for(guard, blocking, BLOCK_DEBOUNCE_S - DT_MDL)
+  assert guard.block_for_direction(log.LaneChangeDirection.left) == custom.IQLateralEdgeBlock.none
+  guard.update(clear, MIN_ACTIVE_SPEED_MPS, DT_MDL)
+  update_for(guard, blocking, BLOCK_DEBOUNCE_S)
+  assert guard.block_for_direction(log.LaneChangeDirection.left) == custom.IQLateralEdgeBlock.left
+
+
+def test_clear_debounce_rejects_a_single_blocking_frame() -> None:
+  guard = LateralEdgeGuard()
+  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()
+  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_invalid_measurement_clears_through_release_debounce() -> None:
+  guard = LateralEdgeGuard()
+  update_for(guard, edge_model(4.0), BLOCK_DEBOUNCE_S)
+  invalid = edge_model(4.0, left_std_m=MAX_VALID_ROAD_EDGE_STD_M + 0.01)
+  update_for(guard, invalid, CLEAR_DEBOUNCE_S - DT_MDL)
+  assert guard.block_for_direction(log.LaneChangeDirection.left) == custom.IQLateralEdgeBlock.left
+  guard.update(invalid, 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()
+  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
+  update_for(guard, edge_model(4.0), BLOCK_DEBOUNCE_S, MIN_ACTIVE_SPEED_MPS)
+  assert guard.block_for_direction(log.LaneChangeDirection.left) == custom.IQLateralEdgeBlock.left
+
+
+def test_desire_helper_keeps_edge_block_out_of_blindspot_path() -> None:
+  helper = DesireHelper()
+  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)
+  blindspot_arguments: list[bool] = []
+
+  def record_blindspot(blindspot_detected: bool, brake_pressed: bool) -> None:
+    blindspot_arguments.append(blindspot_detected)
+
+  helper.alc.update_lane_change = record_blindspot
+  helper.update(CarState(left_blindspot=False), True, 1.0, modeldata=edge_model(4.0))
+  assert blindspot_arguments == [False]
+  assert helper.lateral_edge_block == custom.IQLateralEdgeBlock.left
+  assert helper.lane_change_state == log.LaneChangeState.preLaneChange
+
+  helper.update(CarState(left_blindspot=True), True, 1.0, modeldata=edge_model(4.0))
+  assert blindspot_arguments[-1] is True
+
+
+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_nav_exit_lane_change.py

@@ -0,0 +1,96 @@
+"""
+Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
+"""
+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
+LaneChangeDirection = log.LaneChangeDirection
+
+
+class DummyCarState:
+  def __init__(self, vEgo=25.0, leftBlinker=False, rightBlinker=False, leftBlindspot=False, rightBlindspot=False,
+               steeringPressed=False, steeringTorque=0, brakePressed=False):
+    self.vEgo = vEgo
+    self.leftBlinker = leftBlinker
+    self.rightBlinker = rightBlinker
+    self.leftBlindspot = leftBlindspot
+    self.rightBlindspot = rightBlindspot
+    self.steeringPressed = steeringPressed
+    self.steeringTorque = steeringTorque
+    self.brakePressed = brakePressed
+
+
+class DummyNavState:
+  def __init__(self, active=True, nextManeuverValid=True, nextManeuverType=int(ManeuverType.exit),
+               nextManeuverDistance=300.0, nextManeuverDirection=int(NavDirection.right)):
+    self.active = active
+    self.nextManeuverValid = nextManeuverValid
+    self.nextManeuverType = nextManeuverType
+    self.nextManeuverDistance = nextManeuverDistance
+    self.nextManeuverDirection = nextManeuverDirection
+
+
+def _make_dh(enabled: bool, enable_bsm: bool):
+  dh = DesireHelper()
+  dh.alc.lane_change_set_timer = AutoLaneChangeMode.NUDGE
+  dh.nav_exit._read_enabled = lambda: enabled  # bypass the (unregistered) param in tests
+  dh.nav_exit._enable_bsm = enable_bsm
+  return dh
+
+
+def _run(dh, carstate, nav_state, n=20):
+  for _ in range(n):
+    dh.update(carstate, True, 1.0, nav_state)
+  return dh.desire
+
+
+def test_feature_off_no_exit_lane_change():
+  dh = _make_dh(enabled=False, enable_bsm=True)
+  cs = DummyCarState(rightBlindspot=False)
+  assert _run(dh, cs, DummyNavState()) == log.Desire.none
+
+
+def test_no_bsm_requires_nudge_holds_without_one():
+  # No blindspot monitor: nav exit must NOT auto-start; without a nudge it stays in preLaneChange.
+  dh = _make_dh(enabled=True, enable_bsm=False)
+  cs = DummyCarState(steeringPressed=False)
+  assert _run(dh, cs, DummyNavState()) == log.Desire.none
+  assert dh.lane_change_state == LaneChangeState.preLaneChange
+  assert dh.lane_change_direction == LaneChangeDirection.right
+
+
+def test_no_bsm_starts_on_driver_nudge():
+  # Driver nudges the wheel toward the exit (right -> negative torque) -> lane change starts.
+  dh = _make_dh(enabled=True, enable_bsm=False)
+  cs = DummyCarState(steeringPressed=True, steeringTorque=-1)
+  assert _run(dh, cs, DummyNavState()) == log.Desire.laneChangeRight
+
+
+def test_bsm_auto_starts_when_clear():
+  dh = _make_dh(enabled=True, enable_bsm=True)
+  cs = DummyCarState(rightBlindspot=False)
+  assert _run(dh, cs, DummyNavState()) == log.Desire.laneChangeRight
+
+
+def test_bsm_holds_when_blindspot_occupied():
+  dh = _make_dh(enabled=True, enable_bsm=True)
+  cs = DummyCarState(rightBlindspot=True)
+  assert _run(dh, cs, DummyNavState()) == log.Desire.none
+
+
+def test_only_exit_maneuvers_trigger():
+  # A turn maneuver (not an exit) must not trigger the exit lane change.
+  dh = _make_dh(enabled=True, enable_bsm=True)
+  cs = DummyCarState(rightBlindspot=False)
+  nav = DummyNavState(nextManeuverType=int(ManeuverType.turn))
+  assert _run(dh, cs, nav) == log.Desire.none
+
+
+def test_too_far_does_not_trigger():
+  dh = _make_dh(enabled=True, enable_bsm=True)
+  cs = DummyCarState(rightBlindspot=False)
+  nav = DummyNavState(nextManeuverDistance=900.0)
+  assert _run(dh, cs, nav) == log.Desire.none

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

@@ -0,0 +1,463 @@
+"""
+Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
+"""
+
+from datetime import datetime
+from types import SimpleNamespace
+
+from iqpilot.common.constants import CV
+from iqpilot.common.slc_variables import OFFSET_MAP_IMPERIAL
+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:
+  def __init__(self):
+    self.values = {}
+
+  def get(self, key, encoding=None):
+    _ = encoding
+    return self.values.get(key)
+
+  def get_bool(self, key):
+    return bool(self.values.get(key, False))
+
+  def put_nonblocking(self, key, value):
+    self.values[key] = value
+
+  def put(self, key, value):
+    self.values[key] = value
+
+
+def _build_sm(v_cruise_cluster=100.0, v_ego_cluster=27.8, gas=False, enabled=True, iq_limit=0.0):
+  # vCruiseCluster is in kph in carState.
+  return {
+    "carState": SimpleNamespace(vCruiseCluster=v_cruise_cluster, vEgoCluster=v_ego_cluster, gasPressed=gas,
+                                steeringAngleDeg=0.0, buttonEvents=[]),
+    "iqCarState": SimpleNamespace(speedLimit=iq_limit, accelPressed=False, decelPressed=False),
+    "selfdriveState": SimpleNamespace(enabled=enabled),
+    "vehicleParameters": SimpleNamespace(angleOffsetDeg=0.0),
+  }
+
+
+class _FakeSLC:
+  def __init__(self):
+    self.target = 0.0
+    self.source = "None"
+    self.active_target = 0.0
+    self.active_source = "None"
+    self.unconfirmed_speed_limit = 0.0
+    self.overridden_speed = 0.0
+    self.pending_events = []
+    self.assist_state = None
+    self.output_a_target = 0.0
+    self.update_limits_calls = 0
+    self.update_override_calls = 0
+    self._offset = 0.0
+
+  def update_limits(self, *_args, **_kwargs):
+    self.update_limits_calls += 1
+
+  def update_override(self, *_args, **_kwargs):
+    self.update_override_calls += 1
+
+  def get_offset(self, _is_metric):
+    return self._offset
+
+
+def _base_slc_params_controller():
+  return {
+    "slc_policy": POLICY_MAP_DATA_PRIORITY,
+    "slc_auto_confirm": False,
+    "slc_fallback_previous_speed_limit": False,
+    "slc_fallback_set_speed": False,
+    "speed_limit_confirmation_higher": False,
+    "speed_limit_confirmation_lower": False,
+    "slc_online_filler": True,
+    "map_speed_lookahead_higher": 5.0,
+    "map_speed_lookahead_lower": 5.0,
+  }
+
+
+def test_speed_limit_controller_resolves_source_by_priority():
+  params = FakeParams()
+  controller = SpeedLimitController(params)
+  controller.update_gps = lambda _sm: None
+  controller._resolver.update_map_data = lambda *_args, **_kwargs: None
+  controller.get_mapbox_speed_limit = lambda *_args, **_kwargs: None
+  controller.mapbox_requests["total_requests"] = 0
+  controller.mapbox_requests["max_requests"] = 999999
+  controller.mapbox_limit = 22.0
+  controller._resolver.map_speed_limit = 18.0  # map data wins in map_data_priority policy
+
+  sm = _build_sm(iq_limit=25.0)
+  slc_params = _base_slc_params_controller()
+  slc_params["slc_policy"] = POLICY_MAP_DATA_PRIORITY
+
+  controller.update_limits(25.0, datetime.now(), True, 30.0, 27.0, sm, slc_params)
+  assert controller.active_source == "Map Data"
+  assert controller.active_target == 18.0
+
+
+def test_speed_limit_controller_combined_mode_prefers_smallest_limit():
+  params = FakeParams()
+  controller = SpeedLimitController(params)
+  controller.update_gps = lambda _sm: None
+  controller._resolver.update_map_data = lambda *_args, **_kwargs: None
+  controller.get_mapbox_speed_limit = lambda *_args, **_kwargs: None
+  controller.mapbox_requests["total_requests"] = 0
+  controller.mapbox_requests["max_requests"] = 999999
+  controller.mapbox_limit = 24.0
+  controller._resolver.map_speed_limit = 16.0  # smallest of: dashboard=28, mapbox=24, map_data=16
+
+  sm = _build_sm(iq_limit=28.0)
+  slc_params = _base_slc_params_controller()
+  slc_params["slc_policy"] = POLICY_COMBINED
+
+  controller.update_limits(28.0, datetime.now(), True, 31.0, 27.0, sm, slc_params)
+  assert controller.active_source == "Map Data"
+  assert controller.active_target == 16.0
+
+
+def test_slc_vcruise_applies_target_without_increasing_cruise():
+  slc = SLCVCruise()
+  slc.slc = _FakeSLC()
+  slc.slc.target = 23.0
+  slc.slc.source = "Dashboard"
+  slc.slc.active_target = 23.0
+  slc.slc.active_source = "Dashboard"
+  slc.slc._offset = 1.0
+
+  slc._get_slc_params = lambda: {
+    "speed_limit_controller": True,
+    "speed_limit_mode": 3,
+    "show_speed_limits": False,
+    "is_metric": True,
+    "slc_policy": POLICY_MAP_DATA_PRIORITY,
+    "slc_auto_confirm": False,
+    "speed_limit_confirmation_higher": False,
+    "speed_limit_confirmation_lower": False,
+    "map_speed_lookahead_higher": 5.0,
+    "map_speed_lookahead_lower": 5.0,
+    "slc_fallback_experimental_mode": False,
+    "slc_fallback_set_speed": False,
+    "slc_fallback_previous_speed_limit": False,
+    "speed_limit_controller_override_manual": True,
+    "speed_limit_controller_override_set_speed": False,
+    "slc_online_filler": False,
+  }
+
+  v_cruise = 30.0
+  sm = _build_sm(v_cruise_cluster=v_cruise * CV.MS_TO_KPH, v_ego_cluster=27.0, iq_limit=23.0)
+  out = slc.update(apply_enabled=True, now=None, time_validated=True, v_cruise=v_cruise, v_ego=27.0, sm=sm)
+
+  assert slc.slc.update_limits_calls == 1
+  assert slc.slc.update_override_calls == 1
+  assert out <= v_cruise
+  assert out >= CRUISING_SPEED
+
+
+def test_slc_vcruise_show_only_does_not_modify_cruise():
+  slc = SLCVCruise()
+  slc.slc = _FakeSLC()
+  slc.slc.target = 21.0
+  slc.slc.source = "Map Data"
+  slc.slc.active_target = 21.0
+  slc.slc.active_source = "Map Data"
+  slc._get_slc_params = lambda: {
+    "speed_limit_controller": False,
+    "speed_limit_mode": 1,
+    "show_speed_limits": True,
+    "is_metric": True,
+    "slc_policy": POLICY_MAP_DATA_PRIORITY,
+    "slc_auto_confirm": False,
+    "speed_limit_confirmation_higher": False,
+    "speed_limit_confirmation_lower": False,
+    "map_speed_lookahead_higher": 5.0,
+    "map_speed_lookahead_lower": 5.0,
+    "slc_fallback_experimental_mode": False,
+    "slc_fallback_set_speed": False,
+    "slc_fallback_previous_speed_limit": False,
+    "speed_limit_controller_override_manual": True,
+    "speed_limit_controller_override_set_speed": False,
+    "slc_online_filler": False,
+  }
+
+  v_cruise = 29.0
+  sm = _build_sm(v_cruise_cluster=v_cruise * CV.MS_TO_KPH, v_ego_cluster=26.0, iq_limit=21.0)
+  out = slc.update(apply_enabled=True, now=None, time_validated=True, v_cruise=v_cruise, v_ego=26.0, sm=sm)
+
+  assert slc.slc.update_limits_calls == 1
+  assert slc.slc.update_override_calls == 0
+  assert out == v_cruise
+
+
+def test_slc_vcruise_auto_raises_for_higher_limit_when_confirmation_disabled():
+  slc = SLCVCruise()
+  slc.slc = _FakeSLC()
+  slc.slc.target = 20.0
+  slc.slc.source = "Map Data"
+  slc.slc.active_target = 20.0
+  slc.slc.active_source = "Map Data"
+
+  slc._get_slc_params = lambda: {
+    "speed_limit_controller": True,
+    "speed_limit_mode": 3,
+    "show_speed_limits": False,
+    "is_metric": True,
+    "slc_policy": POLICY_MAP_DATA_PRIORITY,
+    "slc_auto_confirm": False,
+    "speed_limit_confirmation_higher": False,
+    "speed_limit_confirmation_lower": False,
+    "map_speed_lookahead_higher": 5.0,
+    "map_speed_lookahead_lower": 5.0,
+    "slc_fallback_experimental_mode": False,
+    "slc_fallback_set_speed": False,
+    "slc_fallback_previous_speed_limit": False,
+    "speed_limit_controller_override_manual": True,
+    "speed_limit_controller_override_set_speed": False,
+    "slc_online_filler": False,
+  }
+
+  v_cruise = 13.5
+  sm = _build_sm(v_cruise_cluster=v_cruise * CV.MS_TO_KPH, v_ego_cluster=13.5, iq_limit=20.0)
+  out = slc.update(apply_enabled=True, now=None, time_validated=True, v_cruise=v_cruise, v_ego=13.5, sm=sm)
+
+  assert out > v_cruise
+  assert out == 20.0
+
+
+def test_slc_vcruise_does_not_auto_raise_when_higher_confirmation_enabled():
+  slc = SLCVCruise()
+  slc.slc = _FakeSLC()
+  slc.slc.target = 20.0
+  slc.slc.source = "Map Data"
+  slc.slc.active_target = 20.0
+  slc.slc.active_source = "Map Data"
+
+  slc._get_slc_params = lambda: {
+    "speed_limit_controller": True,
+    "speed_limit_mode": 3,
+    "show_speed_limits": False,
+    "is_metric": True,
+    "slc_policy": POLICY_MAP_DATA_PRIORITY,
+    "slc_auto_confirm": False,
+    "speed_limit_confirmation_higher": True,
+    "speed_limit_confirmation_lower": False,
+    "map_speed_lookahead_higher": 5.0,
+    "map_speed_lookahead_lower": 5.0,
+    "slc_fallback_experimental_mode": False,
+    "slc_fallback_set_speed": False,
+    "slc_fallback_previous_speed_limit": False,
+    "speed_limit_controller_override_manual": True,
+    "speed_limit_controller_override_set_speed": False,
+    "slc_online_filler": False,
+  }
+
+  v_cruise = 13.5
+  sm = _build_sm(v_cruise_cluster=v_cruise * CV.MS_TO_KPH, v_ego_cluster=13.5, iq_limit=20.0)
+  out = slc.update(apply_enabled=True, now=None, time_validated=True, v_cruise=v_cruise, v_ego=13.5, sm=sm)
+
+  assert out == v_cruise
+
+
+class _FakeSM(dict):
+  def __init__(self, services, alive=None):
+    super().__init__(services)
+    self.alive = alive or {}
+
+
+def _construction_sm(active=True, alive=True, iq_limit=0.0):
+  sm = _FakeSM(_build_sm(iq_limit=iq_limit))
+  sm["iqConstructionZone"] = SimpleNamespace(active=active, orangeFraction=0.001, secondsSinceHit=1.0)
+  sm.alive = {"iqConstructionZone": alive}
+  return sm
+
+
+def _construction_controller():
+  params = FakeParams()
+  controller = SpeedLimitController(params)
+  controller.update_gps = lambda _sm: None
+  controller._resolver.update_map_data = lambda *_args, **_kwargs: None
+  controller.get_mapbox_speed_limit = lambda *_args, **_kwargs: None
+  controller.mapbox_requests["total_requests"] = 0
+  controller.mapbox_requests["max_requests"] = 999999
+  return controller
+
+
+def _construction_slc_params():
+  slc_params = _base_slc_params_controller()
+  slc_params["slc_online_filler"] = False
+  slc_params["construction_zone_assist"] = True
+  slc_params["construction_zone_speed"] = 60.0
+  slc_params["is_metric"] = False
+  return slc_params
+
+
+def test_construction_zone_clamps_higher_limit():
+  controller = _construction_controller()
+  controller._resolver.map_speed_limit = 31.3  # ~70 mph
+  sm = _construction_sm()
+
+  controller.update_limits(0.0, None, True, 33.0, 30.0, sm, _construction_slc_params())
+  assert controller.active_source == "Construction"
+  assert abs(controller.active_target - 60.0 * CV.MPH_TO_MS) < 1e-6
+
+
+def test_construction_zone_does_not_raise_lower_limit():
+  controller = _construction_controller()
+  controller._resolver.map_speed_limit = 20.0  # below the 60 mph clamp
+  sm = _construction_sm()
+
+  controller.update_limits(0.0, None, True, 33.0, 30.0, sm, _construction_slc_params())
+  assert controller.active_source == "Map Data"
+  assert controller.active_target == 20.0
+
+
+def test_construction_zone_applies_without_other_sources():
+  controller = _construction_controller()
+  controller._resolver.map_speed_limit = 0.0
+  sm = _construction_sm()
+
+  controller.update_limits(0.0, None, True, 33.0, 30.0, sm, _construction_slc_params())
+  assert controller.active_source == "Construction"
+  assert abs(controller.active_target - 60.0 * CV.MPH_TO_MS) < 1e-6
+
+
+def test_construction_zone_ignored_when_not_alive_or_inactive_or_disabled():
+  for kwargs, slc_toggle in (
+    (dict(alive=False), True),
+    (dict(active=False), True),
+    (dict(), False),
+  ):
+    controller = _construction_controller()
+    controller._resolver.map_speed_limit = 31.3
+    sm = _construction_sm(**kwargs)
+    slc_params = _construction_slc_params()
+    slc_params["construction_zone_assist"] = slc_toggle
+
+    controller.update_limits(0.0, None, True, 33.0, 30.0, sm, slc_params)
+    assert controller.active_source == "Map Data"
+    assert controller.active_target == 31.3
+
+
+def test_construction_zone_metric_speed_units():
+  controller = _construction_controller()
+  controller._resolver.map_speed_limit = 33.0
+  sm = _construction_sm()
+  slc_params = _construction_slc_params()
+  slc_params["is_metric"] = True
+  slc_params["construction_zone_speed"] = 100.0  # kph
+
+  controller.update_limits(0.0, None, True, 36.0, 33.0, sm, slc_params)
+  assert controller.active_source == "Construction"
+  assert abs(controller.active_target - 100.0 * CV.KPH_TO_MS) < 1e-6
+
+
+def test_construction_zone_never_raises_cruise_even_with_auto_raise():
+  slc = SLCVCruise()
+  slc.slc = _FakeSLC()
+  slc.slc.target = 60.0 * CV.MPH_TO_MS
+  slc.slc.source = "Construction"
+  slc.slc.active_target = slc.slc.target
+  slc.slc.active_source = "Construction"
+  slc.slc._offset = 2.0  # must be ignored for Construction
+
+  slc._get_slc_params = lambda: {
+    "speed_limit_controller": True,
+    "speed_limit_mode": 3,
+    "show_speed_limits": False,
+    "is_metric": False,
+    "slc_policy": POLICY_MAP_DATA_PRIORITY,
+    "slc_auto_confirm": False,
+    "speed_limit_confirmation_higher": False,  # auto-raise allowed
+    "speed_limit_confirmation_lower": False,
+    "map_speed_lookahead_higher": 5.0,
+    "map_speed_lookahead_lower": 5.0,
+    "slc_fallback_experimental_mode": False,
+    "slc_fallback_set_speed": False,
+    "slc_fallback_previous_speed_limit": False,
+    "speed_limit_controller_override_manual": True,
+    "speed_limit_controller_override_set_speed": False,
+    "slc_online_filler": False,
+    "construction_zone_assist": True,
+    "construction_zone_speed": 60.0,
+  }
+
+  # user cruising below the construction clamp: must not be raised to it
+  v_cruise = 22.0
+  sm = _build_sm(v_cruise_cluster=v_cruise * CV.MS_TO_KPH, v_ego_cluster=22.0)
+  out = slc.update(apply_enabled=True, now=None, time_validated=True, v_cruise=v_cruise, v_ego=22.0, sm=sm)
+  assert out == v_cruise
+  assert slc.slc_offset == 0
+
+  # user cruising above it: clamped down
+  v_cruise = 33.0
+  sm = _build_sm(v_cruise_cluster=v_cruise * CV.MS_TO_KPH, v_ego_cluster=33.0)
+  out = slc.update(apply_enabled=True, now=None, time_validated=True, v_cruise=v_cruise, v_ego=33.0, sm=sm)
+  assert abs(out - 60.0 * CV.MPH_TO_MS) < 1e-6
+
+
+def _offset_controller(pct1=10.0, pct2=5.0, pct3=8.0):
+  params = FakeParams()
+  params.put("speed_limit_offset1", pct1)
+  params.put("speed_limit_offset2", pct2)
+  params.put("speed_limit_offset3", pct3)
+  controller = SpeedLimitController(params)
+  controller._assist.source = "Map Data"
+  return controller
+
+
+def test_get_offset_percent_per_zone():
+  controller = _offset_controller()
+
+  controller._assist.target = 6.7  # ~15 mph -> zone 1
+  assert abs(controller.get_offset(False) - 6.7 * 0.10) < 1e-9
+
+  controller._assist.target = 13.4  # ~30 mph -> zone 2
+  assert abs(controller.get_offset(False) - 13.4 * 0.05) < 1e-9
+
+  controller._assist.target = 31.3  # ~70 mph -> zone 3 (open-ended)
+  assert abs(controller.get_offset(False) - 31.3 * 0.08) < 1e-9
+
+
+def test_get_offset_zone_lower_bound_inclusive():
+  controller = _offset_controller()
+  boundary = OFFSET_MAP_IMPERIAL[1][0]
+  controller._assist.target = boundary
+  assert abs(controller.get_offset(False) - boundary * 0.05) < 1e-9
+
+
+def test_get_offset_zero_without_real_limit_source():
+  for source in ("None", "Construction"):
+    controller = _offset_controller()
+    controller._assist.source = source
+    controller._assist.target = 30.0
+    assert controller.get_offset(False) == 0.0
+
+
+def test_get_offset_percent_clamped():
+  controller = _offset_controller(pct3=500.0)
+  controller._assist.target = 30.0
+  assert abs(controller.get_offset(False) - 30.0 * 0.50) < 1e-9
+
+
+def test_construction_zone_fires_event_once_per_zone_entry():
+  from iqpilot.cereal import custom
+  event = custom.IQOnroadEvent.EventName.constructionZoneDetected
+
+  controller = _construction_controller()
+  controller._resolver.map_speed_limit = 31.3
+  slc_params = _construction_slc_params()
+
+  controller.update_limits(0.0, None, True, 33.0, 30.0, _construction_sm(), slc_params)
+  assert event in controller.pending_events
+
+  controller.update_limits(0.0, None, True, 33.0, 30.0, _construction_sm(), slc_params)
+  assert event not in controller.pending_events
+
+  # zone releases, then a new zone: fires again
+  controller.update_limits(0.0, None, True, 33.0, 30.0, _construction_sm(active=False), slc_params)
+  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

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

@@ -0,0 +1,100 @@
+"""
+Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
+
+Original concept and implementation by SpysyWeeb (github.com/SpysyWeeb)
+"""
+from iqpilot.common.realtime import DT_CTRL
+from iqpilot.selfdrive.controls.lib.smooth_stops import (
+  SmoothStopController,
+  read_smooth_stops_enabled,
+  STANDSTILL_SPEED,
+  STANDSTILL_HOLD_SPEED,
+  SETTLE_DECEL,
+  TAPER_SPEED,
+  STOP_KISS_DECEL,
+  SETTLE_JERK,
+  EMERGENCY_DECEL,
+)
+
+JERK_STEP = SETTLE_JERK * DT_CTRL
+
+
+def _build(enabled=True):
+  c = SmoothStopController.__new__(SmoothStopController)
+  c.enabled = enabled
+  c._v_min = float("inf")
+  c._stall_s = 0.0
+  return c
+
+
+def test_unified_toggle_reads_force_stops():
+  seen = {}
+
+  class FakeParams:
+    def get_bool(self, key):
+      seen["key"] = key
+      return True
+
+  assert read_smooth_stops_enabled(FakeParams()) is True
+  assert seen["key"] == "IQForceStops"
+
+
+def test_hold_only_arms_at_standstill():
+  c = _build()
+  assert not c.want_hold(True, 0.5, False)
+  assert not c.want_hold(True, STANDSTILL_SPEED + 0.05, False)
+  assert not c.want_hold(True, 1.0, True)
+  assert not c.want_hold(True, STANDSTILL_HOLD_SPEED + 0.05, True)
+  assert c.want_hold(True, STANDSTILL_SPEED - 0.01, False)
+  assert c.want_hold(True, STANDSTILL_HOLD_SPEED - 0.01, True)
+  assert not c.want_hold(False, 0.0, True)
+
+
+def test_settle_feathers_toward_baseline():
+  c = _build()
+  out = c.settle(a_target=0.0, v_ego=1.0, lead_distance=0.0, has_lead=False, last_output=0.0)
+  assert out == -JERK_STEP
+
+
+def test_settle_never_softer_than_mpc():
+  c = _build()
+  out = c.settle(a_target=-2.0, v_ego=1.0, lead_distance=0.0, has_lead=False, last_output=-1.0)
+  assert out == -1.0 - JERK_STEP
+  assert out < -1.0
+
+
+def test_settle_emergency_bypasses_jerk_limit():
+  c = _build()
+  out = c.settle(a_target=-3.4, v_ego=2.0, lead_distance=0.0, has_lead=False, last_output=0.0)
+  assert out == -3.4
+  assert out <= -EMERGENCY_DECEL
+
+
+def test_settle_lead_firms_up_when_close():
+  c = _build()
+  assert c.settle(a_target=0.0, v_ego=1.0, lead_distance=50.0, has_lead=True, last_output=-SETTLE_DECEL) == -SETTLE_DECEL
+  c = _build()
+  assert c.settle(a_target=0.0, v_ego=1.0, lead_distance=3.0, has_lead=True, last_output=-1.0) == -1.0
+
+
+def test_settle_anti_creep_firms_up_when_not_slowing():
+  c = _build()
+  out = c.settle(a_target=0.0, v_ego=0.5, lead_distance=0.0, has_lead=False, last_output=-SETTLE_DECEL)
+  for _ in range(60):
+    out = c.settle(a_target=0.0, v_ego=0.5, lead_distance=0.0, has_lead=False, last_output=out)
+  assert out < -SETTLE_DECEL
+
+
+def test_settle_eases_off_near_stop():
+  c = _build()
+  near = c.settle(a_target=0.0, v_ego=0.1, lead_distance=0.0, has_lead=False, last_output=-0.305)
+  c = _build()
+  high = c.settle(a_target=0.0, v_ego=0.9, lead_distance=0.0, has_lead=False, last_output=-0.745)
+  assert near > high
+  assert near == -(STOP_KISS_DECEL + (SETTLE_DECEL - STOP_KISS_DECEL) * (0.1 / TAPER_SPEED))
+
+
+def test_settle_kiss_decel_at_stop():
+  c = _build()
+  out = c.settle(a_target=0.0, v_ego=0.0, lead_distance=0.0, has_lead=False, last_output=-STOP_KISS_DECEL)
+  assert out == -STOP_KISS_DECEL

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