IQ.Pilot Prebuilt Release @ ab07000

2026-08-22 23:42:42 -05:00
commit 9f9c9a70cc
3729 changed files with 778697 additions and 0 deletions
--- a/iqpilot/sab/tests/test_sab.py
+++ b/iqpilot/sab/tests/test_sab.py
@@ -0,0 +1,175 @@
+"""
+Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
+"""
+
+from types import SimpleNamespace
+
+from cereal import custom
+from iqdbc.car import structs
+from iqdbc.car.hyundai.values import HyundaiFlags, HyundaiFlagsIQ
+from openpilot.iqpilot.sab.behavior import SteeringAssistanceBehavior
+from openpilot.iqpilot.selfdrive.selfdrived.events import IQEvents
+from openpilot.selfdrive.selfdrived.events import Events
+
+
+ButtonType = structs.CarState.ButtonEvent.Type
+EventNameIQ = custom.IQOnroadEvent.EventName
+
+
+class MockParams:
+  def __init__(self, main_cruise_allowed: bool = False, aol_enabled: bool = True):
+    self.main_cruise_allowed = main_cruise_allowed
+    self.aol_enabled = aol_enabled
+
+  def get_bool(self, key: str) -> bool:
+    return {
+      "AolEnabled": self.aol_enabled,
+      "AolMainCruiseAllowed": self.main_cruise_allowed,
+      "AolUnifiedEngagementMode": False,
+      "JoystickDebugMode": False,
+    }.get(key, False)
+
+  def get(self, key: str, return_default: bool = False):
+    if key == "AolSteeringMode":
+      return 0 if return_default else b"0"
+    return None
+
+  def remove(self, key: str) -> None:
+    return None
+
+
+def make_selfdrive(cp_flags: int, brand: str = "hyundai", main_cruise_allowed: bool = False,
+                   aol_enabled: bool = True, cp_iq_flags: int = 0):
+  cp = SimpleNamespace(
+    brand=brand,
+    flags=cp_flags,
+    passive=False,
+    notCar=False,
+    safetyModel=structs.CarParams.SafetyModel.noOutput,
+  )
+  cp_iq = SimpleNamespace(flags=cp_iq_flags)
+  return SimpleNamespace(
+    CP=cp,
+    CP_IQ=cp_iq,
+    params=MockParams(main_cruise_allowed, aol_enabled),
+    state_machine=SimpleNamespace(soft_disable_timer=0, current_alert_types=[]),
+    events=Events(),
+    events_iq=IQEvents(),
+    CS_prev=SimpleNamespace(
+      gasPressed=False,
+      cruiseState=SimpleNamespace(available=False),
+      lateralAvailable=False,
+    ),
+    enabled=False,
+    enabled_prev=False,
+    initialized=True,
+  )
+
+
+def make_car_state():
+  return SimpleNamespace(
+    started=True,
+    standstill=False,
+    doorOpen=False,
+    seatbeltUnlatched=False,
+    gearShifter=structs.CarState.GearShifter.drive,
+    vEgo=0.0,
+    gasPressed=False,
+    brakePressed=False,
+    cruiseState=SimpleNamespace(available=False),
+    lateralAvailable=False,
+    buttonEvents=[structs.CarState.ButtonEvent(pressed=True, type=ButtonType.lkas)],
+  )
+
+
+def make_vw_car_state(cruise_available: bool, cruise_fault_lateral: bool = False):
+  return SimpleNamespace(
+    started=True,
+    standstill=False,
+    doorOpen=False,
+    seatbeltUnlatched=False,
+    gearShifter=structs.CarState.GearShifter.drive,
+    vEgo=0.0,
+    gasPressed=False,
+    brakePressed=False,
+    cruiseState=SimpleNamespace(available=cruise_available),
+    lateralAvailable=cruise_available or cruise_fault_lateral,
+    cruiseFaultLateralMode=cruise_fault_lateral,
+    buttonEvents=[],
+  )
+
+
+def test_hyundai_lkas_button_can_arm_guidance_before_lateral_available():
+  selfdrive = make_selfdrive(0, cp_iq_flags=HyundaiFlagsIQ.HAS_LFA_BUTTON)
+  guidance = SteeringAssistanceBehavior(selfdrive)
+  car_state = make_car_state()
+  car_state.buttonEvents = [structs.CarState.ButtonEvent(pressed=True, type=ButtonType.lfaButton)]
+
+  guidance.update_events(car_state)
+
+  assert selfdrive.events_iq.has(EventNameIQ.alcEngaged)
+
+
+def test_hyundai_lkas_button_stays_inactive_without_platform_support():
+  selfdrive = make_selfdrive(0)
+  guidance = SteeringAssistanceBehavior(selfdrive)
+
+  guidance.update_events(make_car_state())
+
+  assert not selfdrive.events_iq.has(EventNameIQ.alcEngaged)
+
+
+def test_main_cruise_drop_cuts_guidance_even_if_lateral_signal_stays_true():
+  selfdrive = make_selfdrive(0, brand="volkswagen", main_cruise_allowed=True)
+  selfdrive.CS_prev = make_vw_car_state(cruise_available=True)
+  guidance = SteeringAssistanceBehavior(selfdrive)
+  guidance.enabled = True
+
+  guidance.update_events(make_vw_car_state(cruise_available=False))
+
+  assert selfdrive.events_iq.has(EventNameIQ.alcDisengaged)
+
+
+def test_faulted_lateral_mode_does_not_force_disable_guidance():
+  selfdrive = make_selfdrive(0, brand="volkswagen", main_cruise_allowed=True)
+  selfdrive.CS_prev = make_vw_car_state(cruise_available=True)
+  guidance = SteeringAssistanceBehavior(selfdrive)
+  guidance.enabled = True
+
+  guidance.update_events(make_vw_car_state(cruise_available=False, cruise_fault_lateral=True))
+
+  assert not selfdrive.events_iq.has(EventNameIQ.alcDisengaged)
+
+
+def test_main_switch_rising_edge_arms_guidance_during_faulted_cruise():
+  selfdrive = make_selfdrive(0, brand="volkswagen", main_cruise_allowed=True)
+  selfdrive.CS_prev = make_vw_car_state(cruise_available=False, cruise_fault_lateral=False)
+  guidance = SteeringAssistanceBehavior(selfdrive)
+
+  guidance.update_events(make_vw_car_state(cruise_available=False, cruise_fault_lateral=True))
+
+  assert selfdrive.events_iq.has(EventNameIQ.alcEngaged)
+
+
+def test_main_cruise_rising_edge_does_not_engage_when_toggle_is_off():
+  selfdrive = make_selfdrive(0, brand="volkswagen", main_cruise_allowed=True, aol_enabled=False)
+  selfdrive.CS_prev = make_vw_car_state(cruise_available=False)
+  guidance = SteeringAssistanceBehavior(selfdrive)
+
+  guidance.update(make_vw_car_state(cruise_available=True))
+
+  assert not selfdrive.events_iq.has(EventNameIQ.alcEngaged)
+  assert not guidance.active
+  assert not guidance.enabled
+  assert guidance.state_machine.state == custom.AlwaysOnLateral.AlwaysOnLateralState.disabled
+
+
+def test_main_cruise_rising_edge_engages_when_toggle_is_on():
+  selfdrive = make_selfdrive(0, brand="volkswagen", main_cruise_allowed=True, aol_enabled=True)
+  selfdrive.CS_prev = make_vw_car_state(cruise_available=False)
+  guidance = SteeringAssistanceBehavior(selfdrive)
+
+  guidance.update(make_vw_car_state(cruise_available=True))
+
+  assert selfdrive.events_iq.has(EventNameIQ.alcEngaged)
+  assert guidance.active
--- a/iqpilot/sab/tests/test_sab_state_machine.py
+++ b/iqpilot/sab/tests/test_sab_state_machine.py
@@ -0,0 +1,221 @@
+"""
+Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
+
+Table-driven checks for GuidanceStateMachine: every transition is one row of
+(start state, signals present, expected state), and the side effects (queued
+alert types, soft-disable timer arming) are asserted separately.
+"""
+import pytest
+
+from cereal import custom
+from openpilot.common.realtime import DT_CTRL
+from openpilot.selfdrive.selfdrived.events import ET
+from openpilot.selfdrive.selfdrived.state import SOFT_DISABLE_TIME
+from openpilot.iqpilot.sab.behavior import (GuidanceStateMachine, PAUSE_WITH_IQ_EVENTS,
+                                                 PAUSE_WITH_STOCK_EVENTS)
+
+State = custom.AlwaysOnLateral.AlwaysOnLateralState
+EventNameIQ = custom.IQOnroadEvent.EventName
+
+SOFT_DISABLE_FRAMES = int(SOFT_DISABLE_TIME / DT_CTRL)
+
+# signal aliases used in the table rows
+ENABLE = ET.ENABLE
+NO_ENTRY = ET.NO_ENTRY
+SOFT = ET.SOFT_DISABLE
+USER = ET.USER_DISABLE
+IMMEDIATE = ET.IMMEDIATE_DISABLE
+OVERRIDE = ET.OVERRIDE_LATERAL
+SILENT = "silent-disable"       # silentLkasDisable present in the IQ event bag
+PAUSE_OK = "pause-eligible"     # a gear/door/belt event from the pause lists is present
+
+
+class SignalBag:
+  """Stands in for both event buckets; the machine only probes membership."""
+
+  def __init__(self, types=(), names=()):
+    self._types = set(types)
+    self._names = set(names)
+
+  def contains(self, event_type):
+    return event_type in self._types
+
+  def has(self, name):
+    return name in self._names
+
+  def contains_in_list(self, names):
+    return any(n in self._names for n in names)
+
+
+class Host:
+  """Minimal stand-in for the sab/selfdrive plumbing the machine touches."""
+
+  class _SSM:
+    def __init__(self):
+      self.current_alert_types = []
+      self.soft_disable_timer = 0
+
+  def __init__(self, signals, selfdrive_enabled=False):
+    types = {s for s in signals if s in (ENABLE, NO_ENTRY, SOFT, USER, IMMEDIATE, OVERRIDE)}
+    names = set()
+    if SILENT in signals:
+      names.add(EventNameIQ.alcDisengagedSilent)
+    if PAUSE_OK in signals:
+      names.add(PAUSE_WITH_IQ_EVENTS[0])
+
+    self.enabled = selfdrive_enabled
+    self.state_machine = self._SSM()
+    self.events = SignalBag(types)
+    self.events_iq = SignalBag((), names)
+
+
+class Sab:
+  def __init__(self, host):
+    self.selfdrive = host
+
+
+def machine_at(state, signals, selfdrive_enabled=False):
+  host = Host(signals, selfdrive_enabled)
+  m = GuidanceStateMachine(Sab(host))
+  m.state = state
+  return m, host
+
+
+# (id, start state, signals, expected state)
+TRANSITIONS = [
+  # from disabled
+  ("idle stays idle", State.disabled, (), State.disabled),
+  ("engage", State.disabled, (ENABLE,), State.enabled),
+  ("engage while overriding", State.disabled, (ENABLE, OVERRIDE), State.overriding),
+  ("blocked entry", State.disabled, (ENABLE, NO_ENTRY), State.disabled),
+  ("blocked entry parks when pause-eligible", State.disabled, (ENABLE, NO_ENTRY, PAUSE_OK), State.paused),
+
+  # from enabled
+  ("cruise steady", State.enabled, (), State.enabled),
+  ("driver off switch", State.enabled, (USER,), State.disabled),
+  ("driver off switch, silent -> pause", State.enabled, (USER, SILENT), State.paused),
+  ("hard fault", State.enabled, (IMMEDIATE,), State.disabled),
+  ("grace period entry", State.enabled, (SOFT,), State.softDisabling),
+  ("hands on wheel", State.enabled, (OVERRIDE,), State.overriding),
+  ("user beats soft", State.enabled, (USER, SOFT), State.disabled),
+  ("hard beats soft", State.enabled, (IMMEDIATE, SOFT), State.disabled),
+
+  # from softDisabling (timer still armed -> stays; see timer tests for expiry)
+  ("condition cleared", State.softDisabling, (), State.enabled),
+  ("user during grace", State.softDisabling, (USER,), State.disabled),
+  ("hard during grace", State.softDisabling, (IMMEDIATE,), State.disabled),
+
+  # from paused
+  ("stays parked", State.paused, (), State.paused),
+  ("blocked resume", State.paused, (ENABLE, NO_ENTRY), State.paused),
+  ("resume", State.paused, (ENABLE,), State.enabled),
+  ("resume into override", State.paused, (ENABLE, OVERRIDE), State.overriding),
+  ("user kill while parked", State.paused, (USER,), State.disabled),
+  ("silent user kill re-parks", State.paused, (USER, SILENT), State.paused),
+  ("hard fault while parked", State.paused, (IMMEDIATE,), State.disabled),
+
+  # from overriding
+  ("override released", State.overriding, (), State.enabled),
+  ("override held", State.overriding, (OVERRIDE,), State.overriding),
+  ("override to grace", State.overriding, (SOFT,), State.softDisabling),
+  ("override user kill", State.overriding, (USER,), State.disabled),
+  ("override hard fault", State.overriding, (IMMEDIATE,), State.disabled),
+]
+
+
+@pytest.mark.parametrize("label,start,signals,expected", TRANSITIONS, ids=[t[0] for t in TRANSITIONS])
+def test_transition(label, start, signals, expected):
+  m, _ = machine_at(start, signals)
+  m.update()
+  assert m.state == expected
+
+
+@pytest.mark.parametrize("start,signals,expected_enabled,expected_active", [
+  (State.disabled, (), False, False),
+  (State.disabled, (ENABLE,), True, True),
+  (State.disabled, (ENABLE, NO_ENTRY, PAUSE_OK), True, False),   # paused: guidance armed, torque off
+  (State.enabled, (), True, True),
+  (State.enabled, (SOFT,), True, True),
+  (State.enabled, (USER,), False, False),
+  (State.overriding, (OVERRIDE,), True, True),
+])
+def test_update_outputs(start, signals, expected_enabled, expected_active):
+  m, _ = machine_at(start, signals)
+  enabled, active = m.update()
+  assert (enabled, active) == (expected_enabled, expected_active)
+
+
+class TestSoftDisableTimer:
+  def test_grace_period_arms_timer_when_solo(self):
+    m, host = machine_at(State.enabled, (SOFT,))
+    m.update()
+    assert m.state == State.softDisabling
+    assert host.state_machine.soft_disable_timer == SOFT_DISABLE_FRAMES
+    assert ET.SOFT_DISABLE in host.state_machine.current_alert_types
+
+  def test_grace_period_skips_timer_when_selfdrive_owns_it(self):
+    m, host = machine_at(State.enabled, (SOFT,), selfdrive_enabled=True)
+    m.update()
+    assert m.state == State.softDisabling
+    assert host.state_machine.soft_disable_timer == 0
+
+  def test_expiry_disables(self):
+    m, host = machine_at(State.softDisabling, (SOFT,))
+    host.state_machine.soft_disable_timer = 0
+    m.update()
+    assert m.state == State.disabled
+
+  def test_countdown_keeps_grace(self):
+    m, host = machine_at(State.softDisabling, (SOFT,))
+    host.state_machine.soft_disable_timer = 5
+    m.update()
+    assert m.state == State.softDisabling
+
+
+class TestAlertQueueing:
+  def test_alerts_only_queued_when_solo(self):
+    m, host = machine_at(State.disabled, (ENABLE,), selfdrive_enabled=True)
+    m.update()
+    assert host.state_machine.current_alert_types == []
+
+  def test_engage_alert_queued(self):
+    m, host = machine_at(State.disabled, (ENABLE,))
+    m.update()
+    assert ET.ENABLE in host.state_machine.current_alert_types
+    assert ET.WARNING in host.state_machine.current_alert_types  # active -> warning channel open
+
+  def test_no_entry_alert_queued(self):
+    m, host = machine_at(State.disabled, (ENABLE, NO_ENTRY))
+    m.update()
+    assert ET.NO_ENTRY in host.state_machine.current_alert_types
+
+  def test_user_disable_alert_always_queued(self):
+    # user disable bypasses the solo gate — the driver asked, the driver hears back
+    m, host = machine_at(State.enabled, (USER,), selfdrive_enabled=True)
+    m.update()
+    assert ET.USER_DISABLE in host.state_machine.current_alert_types
+
+  def test_override_alert_repeats_while_held(self):
+    m, host = machine_at(State.overriding, (OVERRIDE,), selfdrive_enabled=True)
+    m.update()
+    assert ET.OVERRIDE_LATERAL in host.state_machine.current_alert_types
+
+
+class TestPauseEligibility:
+  @pytest.mark.parametrize("event_name", PAUSE_WITH_IQ_EVENTS)
+  def test_each_iq_pause_event_parks(self, event_name):
+    host = Host((ENABLE, NO_ENTRY))
+    host.events_iq = SignalBag((), {event_name})
+    m = GuidanceStateMachine(Sab(host))
+    m.state = State.disabled
+    m.update()
+    assert m.state == State.paused
+
+  @pytest.mark.parametrize("event_name", PAUSE_WITH_STOCK_EVENTS)
+  def test_each_stock_pause_event_parks(self, event_name):
+    host = Host((ENABLE, NO_ENTRY))
+    host.events = SignalBag({ENABLE, NO_ENTRY}, {event_name})
+    m = GuidanceStateMachine(Sab(host))
+    m.state = State.disabled
+    m.update()
+    assert m.state == State.paused