IQ.Pilot Prebuilt Release @ 27f668a

iqpilot/sab/__init__.py

@@ -0,0 +1,22 @@
+"""
+Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
+"""
+from .behavior import (
+  SteeringAssistanceBehavior,
+  GuidanceStateMachine,
+  DriverInterventionMode,
+  BRANDS_WITHOUT_MAIN_CRUISE_TOGGLE,
+  apply_aol_brand_overrides,
+  apply_aol_experience_flags,
+  read_aol_enabled_pref,
+  read_joint_engagement_pref,
+  read_main_cruise_pref,
+  resolve_brake_intervention_mode,
+)
+
+__all__ = [
+  "SteeringAssistanceBehavior", "GuidanceStateMachine", "DriverInterventionMode",
+  "BRANDS_WITHOUT_MAIN_CRUISE_TOGGLE", "apply_aol_brand_overrides", "apply_aol_experience_flags",
+  "read_aol_enabled_pref", "read_joint_engagement_pref", "read_main_cruise_pref",
+  "resolve_brake_intervention_mode",
+]

iqpilot/sab/behavior.py

@@ -0,0 +1,525 @@
+"""
+Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
+"""
+from dataclasses import dataclass
+from typing import Optional
+
+from iqpilot.common.params import Params, UnknownKeyName
+from iqdbc.car import structs
+from iqpilot.common.realtime import DT_CTRL
+from iqdbc.safety import ALTERNATIVE_EXPERIENCE
+from iqpilot.selfdrive.selfdrived.events import ET
+from iqdbc.car.hyundai.values import HyundaiFlags, HyundaiFlagsIQ, HyundaiSafetyFlagsIQ
+from iqpilot.selfdrive.selfdrived.state import SOFT_DISABLE_TIME
+from iqpilot.cereal import log, custom
+
+State = custom.AlwaysOnLateral.AlwaysOnLateralState
+
+class DriverInterventionMode:
+  CONTINUE = 0
+  SUSPEND = 1
+  CANCEL = 2
+
+_FORCED_BRAKE_CANCEL = frozenset({"rivian"})
+BRANDS_WITHOUT_MAIN_CRUISE_TOGGLE = ("rivian", "tesla")
+_HYUNDAI_MAIN_CRUISE_FLAG_BRANDS = frozenset({"hyundai"})
+
+_EXPERIENCE_BY_BRAKE_MODE = {
+  DriverInterventionMode.CANCEL: ALTERNATIVE_EXPERIENCE.AOL_DISENGAGE_LATERAL_ON_BRAKE,
+  DriverInterventionMode.SUSPEND: ALTERNATIVE_EXPERIENCE.AOL_PAUSE_LATERAL_ON_BRAKE,
+}
+
+
+def uses_forced_brake_cancel(CP: structs.CarParams, CP_IQ: structs.IQCarParams):
+  del CP_IQ
+  return CP.brand in _FORCED_BRAKE_CANCEL
+
+
+def read_aol_enabled_pref(params: Params):
+  return params.get_bool("AolEnabled")
+
+
+def read_main_cruise_pref(params: Params):
+  return params.get_bool("AolMainCruiseAllowed")
+
+
+def read_joint_engagement_pref(params: Params):
+  return params.get_bool("AolUnifiedEngagementMode")
+
+
+def read_lateral_override_pause_pref(params: Params):
+  return params.get_bool("AolPauseOnSteeringOverride")
+
+
+def resolve_brake_intervention_mode(CP: structs.CarParams, CP_IQ: structs.IQCarParams, params: Params):
+  if uses_forced_brake_cancel(CP, CP_IQ):
+    return DriverInterventionMode.CANCEL
+  return params.get("AolSteeringMode", return_default=True)
+
+
+def apply_aol_experience_flags(CP: structs.CarParams, CP_IQ: structs.IQCarParams, params: Params):
+  if not read_aol_enabled_pref(params):
+    return
+  CP.alternativeExperience |= ALTERNATIVE_EXPERIENCE.ENABLE_AOL
+  mode = resolve_brake_intervention_mode(CP, CP_IQ, params)
+  CP.alternativeExperience |= _EXPERIENCE_BY_BRAKE_MODE.get(mode, 0)
+
+
+def apply_aol_brand_overrides(CP: structs.CarParams, CP_IQ: structs.IQCarParams, params: Params):
+  if CP.brand in _HYUNDAI_MAIN_CRUISE_FLAG_BRANDS:
+    CP_IQ.flags |= HyundaiFlagsIQ.MAIN_BTN_LONG_TOGGLE.value
+    CP_IQ.iqSafetyFlags |= HyundaiSafetyFlagsIQ.MAIN_BTN_LONG_TOGGLE
+
+  if uses_forced_brake_cancel(CP, CP_IQ):
+    params.put("AolSteeringMode", DriverInterventionMode.CANCEL)
+    params.put_bool("AolUnifiedEngagementMode", True)
+
+  if CP.brand in BRANDS_WITHOUT_MAIN_CRUISE_TOGGLE:
+    params.remove("AolMainCruiseAllowed")
+
+
+EventName = log.OnroadEvent.EventName
+EventNameIQ = custom.IQOnroadEvent.EventName
+TORQUE_DELIVERING_STATES = (State.overriding, State.enabled, State.softDisabling)
+LATERAL_CONTROLLED_STATES = (State.paused, *TORQUE_DELIVERING_STATES)
+GUIDANCE_AVAILABLE_SIGNAL = ET.ENABLE
+GUIDANCE_GATE_BLOCK_SIGNAL = ET.NO_ENTRY
+GUIDANCE_SUPPRESSION_SIGNAL = ET.SOFT_DISABLE
+GUIDANCE_OPERATOR_OFF_SIGNAL = ET.USER_DISABLE
+GUIDANCE_HARD_CUT_SIGNAL = ET.IMMEDIATE_DISABLE
+GUIDANCE_DRIVER_OVERRIDE_SIGNAL = ET.OVERRIDE_LATERAL
+GUIDANCE_ACTIVE_ALERT = ET.WARNING
+
+PAUSE_WITH_IQ_EVENTS = (
+  EventNameIQ.parkBrakeSilent,
+  EventNameIQ.seatbeltUnbuckledSilent,
+  EventNameIQ.doorAjarSilent,
+  EventNameIQ.brakeHoldSilent,
+  EventNameIQ.reverseSilent,
+  EventNameIQ.gearNotDriveSilent,
+)
+PAUSE_WITH_STOCK_EVENTS = (
+  EventName.parkBrake,
+  EventName.seatbeltNotLatched,
+  EventName.doorOpen,
+  EventName.brakeHold,
+  EventName.reverseGear,
+  EventName.wrongGear,
+)
+GEARS_ALLOW_PAUSED_SILENT = PAUSE_WITH_IQ_EVENTS
+GEARS_ALLOW_PAUSED = PAUSE_WITH_STOCK_EVENTS
+
+
+@dataclass(frozen=True)
+class GuidancePulse:
+  wake_ping: bool
+  gate_closed: bool
+  cooldown_call: bool
+  driver_kill: bool
+  hard_cut: bool
+  hands_on_wheel: bool
+  hush_cut: bool
+  pit_stop_ready: bool
+
+
+class GuidanceStateMachine:
+  def __init__(self, sab):
+    self.sab = sab
+    self.selfdrive = sab.selfdrive
+    self._sm_core = sab.selfdrive.state_machine
+    self._events = sab.selfdrive.events
+    self._events_iq = sab.selfdrive.events_iq
+    self.state = State.disabled
+
+  @property
+  def _parks_on_override(self) -> bool:
+    return bool(self.sab.pause_on_lateral_override)
+
+  def _hands_on_landing(self, pulse: GuidancePulse) -> State:
+    if not pulse.hands_on_wheel:
+      return State.enabled
+    return State.paused if self._parks_on_override else State.overriding
+
+  def _queue_alert_if_solo(self, alert_type: str):
+    if not self.selfdrive.enabled:
+      self._sm_core.current_alert_types.append(alert_type)
+
+  def _sees_event(self, event_type: str):
+    return self._events.contains(event_type) or self._events_iq.contains(event_type)
+
+  def _can_take_pit_stop(self):
+    return self._events.contains_in_list(PAUSE_WITH_STOCK_EVENTS) or self._events_iq.contains_in_list(PAUSE_WITH_IQ_EVENTS)
+
+  def _capture_pulse(self) -> GuidancePulse:
+    return GuidancePulse(
+      wake_ping=self._sees_event(GUIDANCE_AVAILABLE_SIGNAL),
+      gate_closed=self._sees_event(GUIDANCE_GATE_BLOCK_SIGNAL),
+      cooldown_call=self._sees_event(GUIDANCE_SUPPRESSION_SIGNAL),
+      driver_kill=self._sees_event(GUIDANCE_OPERATOR_OFF_SIGNAL),
+      hard_cut=self._sees_event(GUIDANCE_HARD_CUT_SIGNAL),
+      hands_on_wheel=self._sees_event(GUIDANCE_DRIVER_OVERRIDE_SIGNAL),
+      hush_cut=self._events_iq.has(EventNameIQ.alcDisengagedSilent),
+      pit_stop_ready=self._can_take_pit_stop(),
+    )
+
+  def _start_grace_period(self):
+    if not self.selfdrive.enabled:
+      self._sm_core.soft_disable_timer = int(SOFT_DISABLE_TIME / DT_CTRL)
+      self._sm_core.current_alert_types.append(GUIDANCE_SUPPRESSION_SIGNAL)
+
+  def _run_global_cutoffs(self, pulse: GuidancePulse) -> Optional[object]:
+    if pulse.driver_kill:
+      self._sm_core.current_alert_types.append(GUIDANCE_OPERATOR_OFF_SIGNAL)
+      return State.paused if pulse.hush_cut else State.disabled
+    if pulse.hard_cut:
+      self._queue_alert_if_solo(GUIDANCE_HARD_CUT_SIGNAL)
+      return State.disabled
+    return None
+
+  def _handle_disabled(self, pulse: GuidancePulse) -> State:
+    if not pulse.wake_ping:
+      return State.disabled
+    if pulse.gate_closed:
+      self._queue_alert_if_solo(GUIDANCE_GATE_BLOCK_SIGNAL)
+      return State.paused if pulse.pit_stop_ready else State.disabled
+    self._queue_alert_if_solo(GUIDANCE_AVAILABLE_SIGNAL)
+    return self._hands_on_landing(pulse)
+
+  def _handle_enabled(self, pulse: GuidancePulse) -> State:
+    forced_state = self._run_global_cutoffs(pulse)
+    if forced_state is not None:
+      return forced_state
+    if pulse.cooldown_call:
+      self._start_grace_period()
+      return State.softDisabling
+    if pulse.hands_on_wheel:
+      if self._parks_on_override:
+        return State.paused
+      self._queue_alert_if_solo(GUIDANCE_DRIVER_OVERRIDE_SIGNAL)
+      return State.overriding
+    return State.enabled
+
+  def _handle_soft_disabling(self, pulse: GuidancePulse) -> State:
+    forced_state = self._run_global_cutoffs(pulse)
+    if forced_state is not None:
+      return forced_state
+    if not pulse.cooldown_call:
+      return State.enabled
+    if self._sm_core.soft_disable_timer > 0:
+      self._queue_alert_if_solo(GUIDANCE_SUPPRESSION_SIGNAL)
+      return State.softDisabling
+    return State.disabled
+
+  def _handle_paused(self, pulse: GuidancePulse) -> State:
+    forced_state = self._run_global_cutoffs(pulse)
+    if forced_state is not None:
+      return forced_state
+    if not pulse.wake_ping:
+      return State.paused
+    if pulse.gate_closed:
+      self._queue_alert_if_solo(GUIDANCE_GATE_BLOCK_SIGNAL)
+      return State.paused
+    self._queue_alert_if_solo(GUIDANCE_AVAILABLE_SIGNAL)
+    return self._hands_on_landing(pulse)
+
+  def _handle_overriding(self, pulse: GuidancePulse) -> State:
+    forced_state = self._run_global_cutoffs(pulse)
+    if forced_state is not None:
+      return forced_state
+    if pulse.cooldown_call:
+      self._start_grace_period()
+      return State.softDisabling
+    if pulse.hands_on_wheel:
+      if self._parks_on_override:
+        return State.paused
+      self._sm_core.current_alert_types.append(GUIDANCE_DRIVER_OVERRIDE_SIGNAL)
+      return State.overriding
+    return State.enabled
+
+  def update(self):
+    pulse = self._capture_pulse()
+    handler = {
+      State.disabled: self._handle_disabled,
+      State.enabled: self._handle_enabled,
+      State.softDisabling: self._handle_soft_disabling,
+      State.paused: self._handle_paused,
+      State.overriding: self._handle_overriding,
+    }[self.state]
+
+    self.state = handler(pulse)
+    enabled = self.state in LATERAL_CONTROLLED_STATES
+    active = self.state in TORQUE_DELIVERING_STATES
+    if active:
+      self._queue_alert_if_solo(GUIDANCE_ACTIVE_ALERT)
+    return enabled, active
+
+_E = log.OnroadEvent.EventName
+_Q = custom.IQOnroadEvent.EventName
+_BTN = structs.CarState.ButtonEvent.Type
+_GEAR = structs.CarState.GearShifter
+
+_CRUISE_SET_TAPS = frozenset((_BTN.accelCruise, _BTN.resumeCruise, _BTN.decelCruise, _BTN.setCruise))
+_LATERAL_TOGGLE_BUTTONS = (_BTN.lkas, _BTN.lfaButton)
+_HYUNDAI_LDA_MASK = HyundaiFlags.CANFD
+
+_QUIET_SWAPS = (
+  (_Q.seatbeltUnbuckledSilent, _E.seatbeltNotLatched, True, None),
+  (_Q.doorAjarSilent, _E.doorOpen, True, None),
+  (_Q.reverseSilent, _E.reverseGear, False, None),
+  (_Q.parkBrakeSilent, _E.parkBrake, False, None),
+  (_Q.brakeHoldSilent, _E.brakeHold, False, None),
+  (_Q.gearNotDriveSilent, _E.wrongGear, False,
+   lambda cs: cs.vEgo < 2.5 or cs.gearShifter == _GEAR.reverse),
+)
+
+_DROP_ON_ENTRY = (_E.speedTooLow, _E.belowEngageSpeed, _E.preEnableStandstill,
+                  _E.manualRestart, _E.cruiseDisabled)
+_DROP_ON_EXIT = (_E.wrongCruiseMode, _E.pedalPressed, _E.buttonCancel, _E.pcmDisable)
+
+
+class SteeringAssistanceBehavior:
+  def __init__(self, selfdrive):
+    sd = selfdrive
+    self.selfdrive = sd
+    self.CP, self.CP_IQ, self.params = sd.CP, sd.CP_IQ, sd.params
+    self.events, self.events_iq = sd.events, sd.events_iq
+
+    self.enabled = self.active = self.available = False
+    self.pause_on_lateral_override = False
+    sd.enabled_prev = False
+    self.state_machine = GuidanceStateMachine(self)
+
+    self.disengage_on_accelerator = self.params.get_bool("DisengageOnAccelerator")
+    self._apply_brand_capabilities()
+    self._reload_preferences(full=True)
+
+  def _apply_brand_capabilities(self):
+    brand = self.CP.brand
+    self.no_main_cruise = brand in BRANDS_WITHOUT_MAIN_CRUISE_TOGGLE
+    lda_capable = bool(self.CP.flags & _HYUNDAI_LDA_MASK) or bool(self.CP_IQ.flags & HyundaiFlagsIQ.HAS_LFA_BUTTON)
+    self.hkg_allow = brand == "hyundai" and lda_capable
+
+  def _reload_preferences(self, full: bool = False):
+    self.main_enabled_toggle = read_main_cruise_pref(self.params)
+    self.unified_engagement_mode = read_joint_engagement_pref(self.params)
+    self.pause_on_lateral_override = read_lateral_override_pause_pref(self.params)
+    if full:
+      self.enabled_toggle = read_aol_enabled_pref(self.params)
+      self.steering_mode_on_brake = resolve_brake_intervention_mode(self.CP, self.CP_IQ, self.params)
+
+  def read_params(self):
+    self._reload_preferences()
+
+  def _has(self, ev):
+    return self.events.has(ev)
+
+  def _drop(self, ev):
+    self.events.remove(ev)
+
+  def _raise(self, ev):
+    self.events.add(ev)
+
+  def _emit(self, ev):
+    self.events_iq.add(ev)
+
+  def _retract(self, ev):
+    self.events_iq.remove(ev)
+
+  def _emitted(self, ev):
+    return self.events_iq.contains(ev)
+
+  def _emitted_any(self, evs):
+    return self.events_iq.contains_in_list(evs)
+
+  def _iq_has(self, ev):
+    return self.events_iq.has(ev)
+
+  def _brake_without_gas(self, cs):
+    prev_gas = self.selfdrive.CS_prev.gasPressed
+    gas_rising_edge = cs.gasPressed and not prev_gas
+    override_via_gas = gas_rising_edge and self.disengage_on_accelerator
+    return self._has(_E.pedalPressed) and not override_via_gas
+
+  def _lateral_overridden(self):
+    return self.events.contains(ET.OVERRIDE_LATERAL) or self.events_iq.contains(ET.OVERRIDE_LATERAL)
+
+  def _may_silently_resume(self, cs):
+    suspend_on_brake = self.steering_mode_on_brake == DriverInterventionMode.SUSPEND
+    if suspend_on_brake and self._brake_without_gas(cs):
+      return False
+    if self.pause_on_lateral_override and self._lateral_overridden():
+      return False
+    return not self._emitted_any(GEARS_ALLOW_PAUSED_SILENT)
+
+  @property
+  def _long_held_two_cycles(self):
+    sd = self.selfdrive
+    return bool(sd.enabled_prev and sd.enabled)
+
+  def _uem_blocks_engage(self):
+    if not self.unified_engagement_mode or self.enabled:
+      return True
+    return self._long_held_two_cycles
+
+  def _lateral_offered(self, cs):
+    return bool(cs.lateralAvailable or cs.cruiseState.available or self.hkg_allow or self.CP.brand == "tesla")
+
+  @staticmethod
+  def _main_cruise_live(cs):
+    cruise = getattr(cs, 'cruiseState', None)
+    if getattr(cruise, 'available', False):
+      return True
+    return bool(getattr(cs, 'cruiseFaultLateralMode', False))
+
+  def _swap_event(self, stock: int, silent: int):
+    self._drop(stock)
+    self._emit(silent)
+
+  def _flag_pause(self):
+    already_held = self.state_machine.state is State.paused
+    if not already_held:
+      self._emit(_Q.alcDisengagedSilent)
+
+  def _resolve_wrong_mode(self, alert_only: bool):
+    if not alert_only:
+      self._drop(_E.wrongCarMode)
+    elif self._has(_E.wrongCarMode):
+      self._swap_event(_E.wrongCarMode, _Q.carModeMismatchNotice)
+
+  def _consume_joystick_aol_request(self, cs) -> str | None:
+    if not self.params.get_bool("JoystickDebugMode"):
+      return None
+    try:
+      raw = self.params.get("JoystickAolRequest")
+    except UnknownKeyName:
+      return None
+    if not raw:
+      return None
+    try:
+      request = raw.decode("utf-8") if isinstance(raw, (bytes, bytearray)) else str(raw)
+    except Exception:
+      request = ""
+    try:
+      self.params.remove("JoystickAolRequest")
+    except UnknownKeyName:
+      return None
+
+    verb = request.strip().lower()
+    if verb not in ("enable", "disable"):
+      return None
+    if not getattr(cs, "started", False):
+      return None
+    if getattr(cs, "doorOpen", False) or getattr(cs, "seatbeltUnlatched", False):
+      return None
+    parked_or_reverse = getattr(cs, "gearShifter", _GEAR.unknown) in (_GEAR.park, _GEAR.reverse)
+    return None if parked_or_reverse else verb
+
+  def _phase_joystick(self, cs):
+    verb = self._consume_joystick_aol_request(cs)
+    if verb is not None:
+      self._emit(_Q.alcEngaged if verb == "enable" else _Q.alcDisengaged)
+
+  def _phase_soften_for_lateral_session(self, cs):
+    if self.selfdrive.enabled or not self.enabled:
+      return
+
+    for silent, stock, standstill_only, extra in _QUIET_SWAPS:
+      if standstill_only and not cs.standstill:
+        continue
+      if not self._has(stock):
+        continue
+      if extra is not None and not extra(cs):
+        continue
+      self._swap_event(stock, silent)
+      self._flag_pause()
+
+    if self.steering_mode_on_brake == DriverInterventionMode.SUSPEND and self._brake_without_gas(cs):
+      self._flag_pause()
+
+    for chatter in _DROP_ON_ENTRY:
+      self._drop(chatter)
+
+  _ENGAGE_TRIGGERS = (_E.pcmEnable, _E.buttonEnable)
+
+  def _phase_engagement(self, cs):
+    long_engage = any(self._has(trig) for trig in self._ENGAGE_TRIGGERS)
+    tapped_set = any(be.type in _CRUISE_SET_TAPS for be in cs.buttonEvents)
+    self._resolve_wrong_mode(long_engage or tapped_set)
+
+    if long_engage:
+      if self._brake_without_gas(cs):
+        self._emit(_Q.pedalHeldNotice)
+      if self._uem_blocks_engage():
+        self._drop(_E.pcmEnable)
+        self._drop(_E.buttonEnable)
+      return
+
+    if self.main_enabled_toggle and self._main_cruise_live(cs) and not self._main_cruise_live(self.selfdrive.CS_prev):
+      self._emit(_Q.alcEngaged)
+
+  def _phase_buttons(self, cs):
+    kill_all = False
+    long_dropped_out = self.selfdrive.enabled_prev and not self.selfdrive.enabled
+    for be in cs.buttonEvents:
+      if be.type == _BTN.cancel and long_dropped_out:
+        self._emit(_Q.speedManually)
+      if not (be.type in _LATERAL_TOGGLE_BUTTONS and be.pressed and self._lateral_offered(cs)):
+        continue
+      if not self.enabled:
+        self._emit(_Q.alcEngaged)
+        continue
+      self._emit(_Q.alcDisengaged)
+      if self.selfdrive.enabled:
+        kill_all = True
+    return kill_all
+
+  def _phase_availability(self, cs):
+    main_off = self.main_enabled_toggle and not self._main_cruise_live(cs)
+    if self.no_main_cruise or (self._lateral_offered(cs) and not main_off):
+      return
+    self._drop(_E.buttonEnable)
+    if self.enabled:
+      self._emit(_Q.alcDisengaged)
+
+  def _phase_brake_policy(self, cs):
+    if self.steering_mode_on_brake != DriverInterventionMode.CANCEL or not self._brake_without_gas(cs):
+      return
+    if self.enabled:
+      self._emit(_Q.alcDisengaged)
+    elif self._emitted(_Q.alcEngaged):
+      self._retract(_Q.alcEngaged)
+      self._emit(_Q.pedalHeldNotice)
+
+  def _phase_resume_from_pause(self, cs):
+    held = self.state_machine.state is State.paused
+    if held and self._may_silently_resume(cs):
+      self._emit(_Q.alcEngagedSilent)
+
+  def update_events(self, cs):
+    self._phase_joystick(cs)
+    self._phase_soften_for_lateral_session(cs)
+    self._phase_engagement(cs)
+    kill_all = self._phase_buttons(cs)
+    self._phase_availability(cs)
+    self._phase_brake_policy(cs)
+    self._phase_resume_from_pause(cs)
+
+    for chatter in _DROP_ON_EXIT:
+      self._drop(chatter)
+
+    if kill_all:
+      self._raise(_E.buttonCancel)
+
+  def update(self, cs):
+    if not self.enabled_toggle and not self.params.get_bool("JoystickDebugMode"):
+      return
+    self.update_events(cs)
+    self.update_state()
+
+  def update_state(self):
+    sd = self.selfdrive
+    sd.enabled_prev = sd.enabled
+    runnable = sd.initialized and not self.CP.passive
+    if runnable:
+      verdict = self.state_machine.update()
+      self.enabled, self.active = verdict

iqpilot/sab/tests/test_sab.py

@@ -0,0 +1,229 @@
+"""
+Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
+"""
+
+from types import SimpleNamespace
+
+from iqpilot.cereal import custom
+from iqdbc.car import structs
+from iqdbc.car.hyundai.values import HyundaiFlags, HyundaiFlagsIQ
+from iqpilot.sab.behavior import SteeringAssistanceBehavior
+from iqpilot.selfdrive.selfdrived.iq_events import IQEvents
+from iqpilot.selfdrive.selfdrived.events import Events
+from iqpilot.cereal import log
+
+
+ButtonType = structs.CarState.ButtonEvent.Type
+EventName = log.OnroadEvent.EventName
+EventNameIQ = custom.IQOnroadEvent.EventName
+GuidanceState = custom.AlwaysOnLateral.AlwaysOnLateralState
+
+
+class MockParams:
+  def __init__(self, main_cruise_allowed: bool = False, aol_enabled: bool = True,
+               pause_on_steering_override: bool = False):
+    self.main_cruise_allowed = main_cruise_allowed
+    self.aol_enabled = aol_enabled
+    self.pause_on_steering_override = pause_on_steering_override
+
+  def get_bool(self, key: str) -> bool:
+    return {
+      "AolEnabled": self.aol_enabled,
+      "AolMainCruiseAllowed": self.main_cruise_allowed,
+      "AolUnifiedEngagementMode": False,
+      "AolPauseOnSteeringOverride": self.pause_on_steering_override,
+      "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,
+                   pause_on_steering_override: bool = False):
+  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, pause_on_steering_override),
+    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 run_cycle(guidance, selfdrive, car_state, steering_pressed: bool):
+  selfdrive.events.clear()
+  selfdrive.events_iq.clear()
+  if steering_pressed:
+    selfdrive.events.add(EventName.steerOverride)
+  guidance.update(car_state)
+  selfdrive.CS_prev = car_state
+
+
+def make_engaged_guidance(pause_on_steering_override: bool):
+  selfdrive = make_selfdrive(0, brand="volkswagen", pause_on_steering_override=pause_on_steering_override)
+  selfdrive.CS_prev = make_vw_car_state(cruise_available=True)
+  guidance = SteeringAssistanceBehavior(selfdrive)
+  guidance.enabled = True
+  guidance.state_machine.state = GuidanceState.enabled
+  return guidance, selfdrive
+
+
+def test_steering_override_parks_guidance_when_enabled():
+  guidance, selfdrive = make_engaged_guidance(True)
+
+  run_cycle(guidance, selfdrive, make_vw_car_state(cruise_available=True), steering_pressed=True)
+
+  assert guidance.state_machine.state == GuidanceState.paused
+  assert guidance.enabled
+  assert not guidance.active
+
+
+def test_guidance_resumes_once_steering_is_released():
+  guidance, selfdrive = make_engaged_guidance(True)
+
+  run_cycle(guidance, selfdrive, make_vw_car_state(cruise_available=True), steering_pressed=True)
+  run_cycle(guidance, selfdrive, make_vw_car_state(cruise_available=True), steering_pressed=False)
+
+  assert guidance.state_machine.state == GuidanceState.enabled
+  assert guidance.active
+
+
+def test_steering_override_keeps_torque_when_option_is_off():
+  guidance, selfdrive = make_engaged_guidance(False)
+
+  run_cycle(guidance, selfdrive, make_vw_car_state(cruise_available=True), steering_pressed=True)
+
+  assert guidance.state_machine.state == GuidanceState.overriding
+  assert guidance.active
+
+
+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

iqpilot/sab/tests/test_sab_state_machine.py

@@ -0,0 +1,234 @@
+"""
+Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
+"""
+import pytest
+
+from iqpilot.cereal import custom
+from iqpilot.common.realtime import DT_CTRL
+from iqpilot.selfdrive.selfdrived.events import ET
+from iqpilot.selfdrive.selfdrived.state import SOFT_DISABLE_TIME
+from 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)
+
+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"
+PAUSE_OK = "pause-eligible"
+
+
+class SignalBag:
+  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:
+  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, pause_on_lateral_override=False):
+    self.selfdrive = host
+    self.pause_on_lateral_override = pause_on_lateral_override
+
+
+def machine_at(state, signals, selfdrive_enabled=False, pause_on_lateral_override=False):
+  host = Host(signals, selfdrive_enabled)
+  m = GuidanceStateMachine(Sab(host, pause_on_lateral_override))
+  m.state = state
+  return m, host
+
+
+TRANSITIONS = [
+  ("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),
+  ("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),
+  ("condition cleared", State.softDisabling, (), State.enabled),
+  ("user during grace", State.softDisabling, (USER,), State.disabled),
+  ("hard during grace", State.softDisabling, (IMMEDIATE,), State.disabled),
+  ("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),
+  ("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),
+  (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
+
+  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):
+    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
+
+OVERRIDE_PAUSE_TRANSITIONS = [
+  ("hands on wheel parks", State.enabled, (OVERRIDE,), State.paused),
+  ("override held stays parked", State.overriding, (OVERRIDE,), State.paused),
+  ("engage while overriding parks", State.disabled, (ENABLE, OVERRIDE), State.paused),
+  ("parked resume waits for release", State.paused, (ENABLE, OVERRIDE), State.paused),
+  ("release resumes", State.paused, (ENABLE,), State.enabled),
+  ("hands off keeps steering", State.enabled, (), State.enabled),
+  ("user kill still kills", State.enabled, (OVERRIDE, USER), State.disabled),
+  ("hard fault still faults", State.enabled, (OVERRIDE, IMMEDIATE), State.disabled),
+  ("grace beats override", State.enabled, (OVERRIDE, SOFT), State.softDisabling),
+]
+
+@pytest.mark.parametrize("label,start,signals,expected", OVERRIDE_PAUSE_TRANSITIONS,
+                         ids=[t[0] for t in OVERRIDE_PAUSE_TRANSITIONS])
+def test_override_pause_transition(label, start, signals, expected):
+  m, _ = machine_at(start, signals, pause_on_lateral_override=True)
+  m.update()
+  assert m.state == expected
+
+
+class TestOverridePauseOutputs:
+  def test_torque_stops_while_overriding(self):
+    m, _ = machine_at(State.enabled, (OVERRIDE,), pause_on_lateral_override=True)
+    enabled, active = m.update()
+    assert (enabled, active) == (True, False)
+
+  def test_torque_returns_on_release(self):
+    m, _ = machine_at(State.paused, (ENABLE,), pause_on_lateral_override=True)
+    enabled, active = m.update()
+    assert (enabled, active) == (True, True)
+
+  def test_override_alert_not_queued_while_parked(self):
+    m, host = machine_at(State.enabled, (OVERRIDE,), pause_on_lateral_override=True)
+    m.update()
+    assert ET.OVERRIDE_LATERAL not in host.state_machine.current_alert_types