IQ.Pilot Release Commit @ b6534c0

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_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

@@ -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