IQ.Pilot Release Commit @ b6534c0

iqpilot/selfdrive/controls/lib/helpers/__init__.py

@@ -0,0 +1,3 @@
+"""
+Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
+"""

iqpilot/selfdrive/controls/lib/helpers/blinker_pause.py

@@ -0,0 +1,82 @@
+"""
+Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
+"""
+from iqpilot.cereal import car
+
+from iqpilot.common.constants import CV
+from iqpilot.common.params import Params
+
+
+class SignalPauseEngine:
+  _KEY_ON = "IQBlinkerPauseLateral"
+  _KEY_UNIT = "IsMetric"
+  _KEY_GATE = "IQBlinkerMinLateralSpeed"
+
+  def __init__(self):
+    self._kv = Params()
+    self._state = {"on": False, "metric": False, "gate": 0.0}
+    self.reload_setup()
+
+  @staticmethod
+  def _one_signal(cs: car.CarState) -> bool:
+    return bool(cs.leftBlinker) ^ bool(cs.rightBlinker)
+
+  @staticmethod
+  def _as_float(raw) -> float:
+    try:
+      return float(raw) if raw is not None else 0.0
+    except (TypeError, ValueError):
+      return 0.0
+
+  def _pull_setup(self) -> None:
+    self._state["on"] = self._kv.get_bool(self._KEY_ON)
+    self._state["metric"] = self._kv.get_bool(self._KEY_UNIT)
+    self._state["gate"] = self._as_float(self._kv.get(self._KEY_GATE))
+
+  def _gate_mps(self) -> float:
+    factor = CV.KPH_TO_MS if self._state["metric"] else CV.MPH_TO_MS
+    return self._state["gate"] * factor
+
+  def reload_setup(self) -> None:
+    self._pull_setup()
+
+  def heartbeat(self) -> None:
+    self._pull_setup()
+
+  def is_paused(self, cs: car.CarState) -> bool:
+    return bool(self._state["on"] and self._one_signal(cs) and cs.vEgo < self._gate_mps())
+
+  @property
+  def enabled(self):
+    return self._state["on"]
+
+  @enabled.setter
+  def enabled(self, value):
+    self._state["on"] = bool(value)
+
+  @property
+  def is_metric(self):
+    return self._state["metric"]
+
+  @is_metric.setter
+  def is_metric(self, value):
+    self._state["metric"] = bool(value)
+
+  @property
+  def min_speed(self):
+    return self._state["gate"]
+
+  @min_speed.setter
+  def min_speed(self, value):
+    self._state["gate"] = float(value)
+
+
+class IQSignalPauseController(SignalPauseEngine):
+  def __init__(self):
+    super().__init__()
+
+  def get_params(self) -> None:
+    self.reload_setup()
+
+  def update(self, cs: car.CarState) -> bool:
+    return self.is_paused(cs)

iqpilot/selfdrive/controls/lib/helpers/e2e_alerts.py

@@ -0,0 +1,143 @@
+"""
+Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
+"""
+
+from iqpilot.cereal import messaging, custom
+
+from iqpilot.common.params import Params
+from iqpilot.common.realtime import DT_MDL
+from iqpilot.selfdrive.selfdrived.iq_events import IQEvents
+
+PARAM_PATH = "EndToEndAlert"
+PARAM_LEAD = "EndToEndLeadAlert"
+PARAM_STRIDE_S = 2.0
+
+SETTLE_S = 1.0
+CONFIRM_S = 0.4
+ROLL_MPS = 0.3
+
+HORIZON_TAIL = 5
+PATH_SPEED_MPS = 3.0
+
+LEAD_QUEUE_M = 12.0
+LEAD_SPEED_MPS = 1.0
+LEAD_GAP_M = 0.5
+
+
+class _Confirm:
+  def __init__(self, window_s: float):
+    self._window = window_s
+    self._held = 0.0
+    self._spent = False
+
+  def clear(self) -> None:
+    self._held = 0.0
+    self._spent = False
+
+  def poll(self, holds: bool) -> bool:
+    if self._spent:
+      return False
+    self._held = self._held + DT_MDL if holds else 0.0
+    if self._held < self._window:
+      return False
+    self._spent = True
+    return True
+
+
+class _Dwell:
+  def __init__(self):
+    self.seconds = 0.0
+    self.lead_floor = float('inf')
+
+  def clear(self) -> None:
+    self.seconds = 0.0
+    self.lead_floor = float('inf')
+
+  def tick(self, lead_range: float | None) -> None:
+    self.seconds += DT_MDL
+    if lead_range is not None:
+      self.lead_floor = min(self.lead_floor, lead_range)
+
+  @property
+  def settled(self) -> bool:
+    return self.seconds >= SETTLE_S
+
+  @property
+  def queued(self) -> bool:
+    return self.lead_floor < LEAD_QUEUE_M
+
+
+class EndToEndAlertEngine:
+  def __init__(self):
+    self._params = Params()
+    self._on = {"path": False, "lead": False}
+    self._elapsed_since_read = PARAM_STRIDE_S
+    self._dwell = _Dwell()
+    self._confirm = {"path": _Confirm(CONFIRM_S), "lead": _Confirm(CONFIRM_S)}
+    self._fired = {"path": False, "lead": False}
+
+  def _refresh_params(self) -> None:
+    self._elapsed_since_read += DT_MDL
+    if self._elapsed_since_read < PARAM_STRIDE_S:
+      return
+    self._elapsed_since_read = 0.0
+    self._on["path"] = self._params.get_bool(PARAM_PATH)
+    self._on["lead"] = self._params.get_bool(PARAM_LEAD)
+
+  @staticmethod
+  def _car_holds_long(sm: messaging.SubMaster) -> bool:
+    # AOL steers without raising selfdriveState.enabled, so the pair reads as long authority
+    return bool(sm['selfdriveState'].enabled or sm['carState'].cruiseState.enabled)
+
+  @staticmethod
+  def _halted(cs) -> bool:
+    return bool(cs.standstill) or abs(cs.vEgo) < ROLL_MPS
+
+  @staticmethod
+  def _horizon_speed(model) -> float:
+    # capnp list readers reject slices
+    samples = model.velocity.x
+    count = len(samples)
+    if count < HORIZON_TAIL:
+      return 0.0
+    return sum(samples[i] for i in range(count - HORIZON_TAIL, count)) / HORIZON_TAIL
+
+  def _rearm(self) -> None:
+    self._dwell.clear()
+    for gate in self._confirm.values():
+      gate.clear()
+
+  def update(self, sm: messaging.SubMaster, iq_events: IQEvents) -> None:
+    self._refresh_params()
+    self._fired["path"] = self._fired["lead"] = False
+
+    cs = sm['carState']
+    lead = sm['radarState'].leadOne
+    lead_range = float(lead.dRel) if lead.status else None
+
+    if not self._halted(cs) or cs.gasPressed or self._car_holds_long(sm):
+      self._rearm()
+      return
+
+    self._dwell.tick(lead_range)
+    if not self._dwell.settled:
+      return
+
+    if self._on["path"] and lead_range is None:
+      opened = self._horizon_speed(sm['modelV2']) > PATH_SPEED_MPS
+      self._fired["path"] = self._confirm["path"].poll(opened)
+
+    if self._on["lead"] and lead_range is not None and self._dwell.queued:
+      pulling = lead.vLead > LEAD_SPEED_MPS and (lead_range - self._dwell.lead_floor) > LEAD_GAP_M
+      self._fired["lead"] = self._confirm["lead"].poll(pulling)
+
+    if self._fired["path"] or self._fired["lead"]:
+      iq_events.add(custom.IQOnroadEvent.EventName.e2eChime)
+
+  @property
+  def path_alert(self) -> bool:
+    return self._fired["path"]
+
+  @property
+  def lead_alert(self) -> bool:
+    return self._fired["lead"]

iqpilot/selfdrive/controls/lib/helpers/lane_change.py

@@ -0,0 +1,293 @@
+"""
+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.common.params import Params
+from iqpilot.common.realtime import DT_MDL
+
+NAV_EXIT_COMMIT_DISTANCE = 500.0  # m before a route exit to begin moving into the exit lane
+_ManeuverType = custom.IQNavState.ManeuverType
+_NavDirection = custom.NavDirection
+
+
+class LaneSwapPreset:
+  DISABLED = -1
+  STEERING_NUDGE = 0
+  DIRECT = 1
+  DELAY_HALF = 2
+  DELAY_ONE = 3
+  DELAY_TWO = 4
+  DELAY_THREE = 5
+  OFF = DISABLED
+  NUDGE = STEERING_NUDGE
+  NUDGELESS = DIRECT
+  HALF_SECOND = DELAY_HALF
+  ONE_SECOND = DELAY_ONE
+  TWO_SECONDS = DELAY_TWO
+  THREE_SECONDS = DELAY_THREE
+
+
+PRESET_SECONDS = {
+  LaneSwapPreset.DISABLED: 0.0,
+  LaneSwapPreset.STEERING_NUDGE: 0.0,
+  LaneSwapPreset.DIRECT: 0.05,
+  LaneSwapPreset.DELAY_HALF: 0.5,
+  LaneSwapPreset.DELAY_ONE: 1.0,
+  LaneSwapPreset.DELAY_TWO: 2.0,
+  LaneSwapPreset.DELAY_THREE: 3.0,
+}
+
+LANE_SWAP_SECONDS = dict(PRESET_SECONDS)
+BLINDSPOT_WAIT_OFFSET = -1
+
+
+class LaneSwapEngine:
+  def __init__(self, desire_hub):
+    self._hub = desire_hub
+    self._kv = Params()
+    self._mem = {
+      "sec": 0.0,
+      "tick": 0,
+      "gate": 0.0,
+      "preset": self._kv.get("IQLaneChangeTimer", return_default=True),
+      "bsm_hold": False,
+      "braked": False,
+      "ready": False,
+      "used": False,
+    }
+    self.reload_setup()
+
+  def _pull_setup(self) -> None:
+    self._mem["bsm_hold"] = self._kv.get_bool("IQLaneChangeBsmDelay")
+    self._mem["preset"] = self._kv.get("IQLaneChangeTimer", return_default=True)
+
+  def _idle_phase(self) -> bool:
+    return (
+      self._hub.lane_change_state == log.LaneChangeState.off and
+      self._hub.lane_change_direction == log.LaneChangeDirection.none
+    )
+
+  def _seconds_for_preset(self) -> float:
+    picked = self._mem["preset"]
+    return PRESET_SECONDS.get(picked, PRESET_SECONDS[LaneSwapPreset.STEERING_NUDGE])
+
+  def _auto_preset_active(self) -> bool:
+    picked = self._mem["preset"]
+    return picked not in (LaneSwapPreset.DISABLED, LaneSwapPreset.STEERING_NUDGE)
+
+  def _advance_clock(self, blindspot_now: bool) -> None:
+    wait_s = self._seconds_for_preset()
+    self._mem["gate"] = wait_s
+    self._mem["sec"] += DT_MDL
+    if self._mem["bsm_hold"] and blindspot_now and wait_s > 0.0:
+      if wait_s == PRESET_SECONDS[LaneSwapPreset.DIRECT]:
+        self._mem["sec"] = BLINDSPOT_WAIT_OFFSET
+      else:
+        self._mem["sec"] = wait_s + BLINDSPOT_WAIT_OFFSET
+
+  def _ready_to_fire(self) -> bool:
+    return (
+      self._auto_preset_active() and
+      (not self._mem["braked"]) and
+      (not self._mem["used"]) and
+      (self._mem["sec"] > self._mem["gate"])
+    )
+
+  def reload_setup(self) -> None:
+    self._pull_setup()
+
+  def heartbeat(self) -> None:
+    if (self._mem["tick"] % 50) == 0:
+      self._pull_setup()
+    self._mem["tick"] += 1
+
+  def sample(self, blindspot_now: bool = False, brake_now: bool = False, **legacy) -> None:
+    blindspot_now = bool(legacy.get("blindspot_detected", blindspot_now))
+    brake_now = bool(legacy.get("brake_pressed", brake_now))
+    self._mem["braked"] = self._mem["braked"] or brake_now
+    self._advance_clock(blindspot_now)
+    self._mem["ready"] = self._ready_to_fire()
+
+  def finalize(self) -> None:
+    started = self._hub.lane_change_state == log.LaneChangeState.laneChangeStarting
+    self._mem["used"] = self._mem["used"] or started
+    if self._idle_phase():
+      self._mem["sec"] = 0.0
+      self._mem["braked"] = False
+      self._mem["used"] = False
+
+  @property
+  def ready(self):
+    return self._mem["ready"]
+
+  @property
+  def delay(self):
+    return self._mem["gate"]
+
+  @property
+  def elapsed(self):
+    return self._mem["sec"]
+
+  @property
+  def preset(self):
+    return self._mem["preset"]
+
+  @preset.setter
+  def preset(self, value):
+    self._mem["preset"] = value
+
+  @property
+  def bsm_hold(self):
+    return self._mem["bsm_hold"]
+
+  @bsm_hold.setter
+  def bsm_hold(self, value):
+    self._mem["bsm_hold"] = bool(value)
+
+  @property
+  def braked(self):
+    return self._mem["braked"]
+
+  @braked.setter
+  def braked(self, value):
+    self._mem["braked"] = bool(value)
+
+  @property
+  def used(self):
+    return self._mem["used"]
+
+  @used.setter
+  def used(self, value):
+    self._mem["used"] = bool(value)
+
+
+class NavExitLaneChangeController:
+  def __init__(self, enable_bsm: bool):
+    self._params = Params()
+    self._enable_bsm = bool(enable_bsm)
+    self.enabled = self._read_enabled()
+    self._tick = 0
+    self.active = False
+    self.direction = log.LaneChangeDirection.none
+    self.auto_allowed = False
+
+  def _read_enabled(self) -> bool:
+    try:
+      return self._params.get_bool("NavExitLaneChange")
+    except Exception:
+      return False
+
+  def update_params(self) -> None:
+    if self._tick % 50 == 0:
+      self.enabled = self._read_enabled()
+    self._tick += 1
+
+  @staticmethod
+  def _raw(value):
+    return getattr(value, "raw", value)
+
+  def update(self, nav_state, carstate) -> None:
+    self.active = False
+    self.direction = log.LaneChangeDirection.none
+    self.auto_allowed = False
+
+    if not self.enabled or nav_state is None or not getattr(nav_state, "active", False):
+      return
+    if not getattr(nav_state, "nextManeuverValid", False):
+      return
+    if self._raw(getattr(nav_state, "nextManeuverType", _ManeuverType.none)) != int(_ManeuverType.exit):
+      return
+    distance = float(getattr(nav_state, "nextManeuverDistance", 0.0))
+    if not 0.0 < distance <= NAV_EXIT_COMMIT_DISTANCE:
+      return
+
+    direction = self._raw(getattr(nav_state, "nextManeuverDirection", _NavDirection.none))
+    if direction == int(_NavDirection.left):
+      self.direction = log.LaneChangeDirection.left
+    elif direction == int(_NavDirection.right):
+      self.direction = log.LaneChangeDirection.right
+    else:
+      return
+
+    self.active = True
+    blindspot = carstate.leftBlindspot if self.direction == log.LaneChangeDirection.left else carstate.rightBlindspot
+    self.auto_allowed = (not blindspot) if self._enable_bsm else False
+
+
+AutoLaneChangeMode = LaneSwapPreset
+AUTO_LANE_CHANGE_TIMER = LANE_SWAP_SECONDS
+ONE_SECOND_DELAY = BLINDSPOT_WAIT_OFFSET
+
+
+class IQLaneSwapController(LaneSwapEngine):
+  def __init__(self, desire_helper):
+    super().__init__(desire_helper)
+
+  def reset(self) -> None:
+    self.finalize()
+
+  def update_params(self) -> None:
+    self.heartbeat()
+
+  def update_lane_change(self, blindspot_detected: bool, brake_pressed: bool) -> None:
+    self.sample(blindspot_now=blindspot_detected, brake_now=brake_pressed)
+
+  def update_state(self) -> None:
+    self.finalize()
+
+  @property
+  def lane_change_wait_timer(self):
+    return self.elapsed
+
+  @lane_change_wait_timer.setter
+  def lane_change_wait_timer(self, value):
+    self._mem["sec"] = float(value)
+
+  @property
+  def lane_change_delay(self):
+    return self.delay
+
+  @lane_change_delay.setter
+  def lane_change_delay(self, value):
+    self._mem["gate"] = float(value)
+
+  @property
+  def lane_change_set_timer(self):
+    return self.preset
+
+  @lane_change_set_timer.setter
+  def lane_change_set_timer(self, value):
+    self.preset = value
+
+  @property
+  def lane_change_bsm_delay(self):
+    return self.bsm_hold
+
+  @lane_change_bsm_delay.setter
+  def lane_change_bsm_delay(self, value):
+    self.bsm_hold = value
+
+  @property
+  def prev_brake_pressed(self):
+    return self.braked
+
+  @prev_brake_pressed.setter
+  def prev_brake_pressed(self, value):
+    self.braked = value
+
+  @property
+  def auto_lane_change_allowed(self):
+    return self.ready
+
+  @auto_lane_change_allowed.setter
+  def auto_lane_change_allowed(self, value):
+    self._mem["ready"] = bool(value)
+
+  @property
+  def prev_lane_change(self):
+    return self.used
+
+  @prev_lane_change.setter
+  def prev_lane_change(self, value):
+    self.used = value

iqpilot/selfdrive/controls/lib/helpers/lane_turn.py

@@ -0,0 +1,157 @@
+"""
+Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos/
+"""
+
+from __future__ import annotations
+
+from dataclasses import dataclass
+
+from iqpilot.cereal import custom
+
+from iqpilot.common.constants import CV
+from iqpilot.common.params import Params
+
+TurnDirection = custom.IQTurnSignalDirection
+
+TURN_TRIGGER_MPS = 20 * CV.MPH_TO_MS
+TURN_SPEED_GATE_MPS = TURN_TRIGGER_MPS
+LANE_CHANGE_SPEED_MIN = TURN_SPEED_GATE_MPS
+
+
+@dataclass
+class _TurnGateState:
+  active: bool = False
+  speed_limit_mps: float = TURN_TRIGGER_MPS
+  outcome: int = TurnDirection.none
+  refresh_tick: int = 0
+
+
+def _mph_param_to_mps(raw_value) -> float:
+  try:
+    return float(raw_value) * CV.MPH_TO_MS
+  except (TypeError, ValueError):
+    return TURN_TRIGGER_MPS
+
+
+def _resolve_signal_choice(speed_mps: float,
+                           speed_limit_mps: float,
+                           left_signal: bool,
+                           right_signal: bool,
+                           left_blocked: bool,
+                           right_blocked: bool) -> int:
+  if speed_mps >= speed_limit_mps:
+    return TurnDirection.none
+  if left_signal and not right_signal and not left_blocked:
+    return TurnDirection.turnLeft
+  if right_signal and not left_signal and not right_blocked:
+    return TurnDirection.turnRight
+  return TurnDirection.none
+
+
+class TurnSignalPlanner:
+  _REFRESH_STRIDE = 50
+
+  def __init__(self, desire_hub):
+    self._desire_hub = desire_hub
+    self._params = Params()
+    self._state = _TurnGateState()
+    self.reload_setup()
+
+  def _refresh_from_params(self) -> None:
+    requested_gate = _mph_param_to_mps(self._params.get("IQLaneTurnValue", return_default=True))
+    self._state.active = self._params.get_bool("IQLaneTurnDesire")
+    self._state.speed_limit_mps = min(TURN_TRIGGER_MPS, requested_gate)
+
+  def _consume_legacy_kwargs(self, **legacy) -> tuple[bool, bool, bool, bool, float]:
+    return (
+      bool(legacy.get("blindspot_left", False)),
+      bool(legacy.get("blindspot_right", False)),
+      bool(legacy.get("left_blinker", False)),
+      bool(legacy.get("right_blinker", False)),
+      float(legacy.get("v_ego", 0.0)),
+    )
+
+  def reload_setup(self):
+    self._refresh_from_params()
+
+  def heartbeat(self) -> None:
+    if self._state.refresh_tick % self._REFRESH_STRIDE == 0:
+      self._refresh_from_params()
+    self._state.refresh_tick += 1
+
+  def sample(self,
+             blocked_l: bool = False,
+             blocked_r: bool = False,
+             blink_l: bool = False,
+             blink_r: bool = False,
+             speed_mps: float = 0.0,
+             **legacy) -> None:
+    if legacy:
+      blocked_l, blocked_r, blink_l, blink_r, speed_mps = self._consume_legacy_kwargs(**legacy)
+    self._state.outcome = _resolve_signal_choice(speed_mps,
+                                                 self._state.speed_limit_mps,
+                                                 blink_l,
+                                                 blink_r,
+                                                 blocked_l,
+                                                 blocked_r)
+
+  def output(self):
+    return self._state.outcome if self._state.active else TurnDirection.none
+
+  @property
+  def enabled(self):
+    return self._state.active
+
+  @enabled.setter
+  def enabled(self, value):
+    self._state.active = bool(value)
+
+  @property
+  def speed_gate(self):
+    return self._state.speed_limit_mps
+
+  @speed_gate.setter
+  def speed_gate(self, value):
+    self._state.speed_limit_mps = float(value)
+
+  @property
+  def turn_direction(self):
+    return self._state.outcome
+
+  @turn_direction.setter
+  def turn_direction(self, value):
+    self._state.outcome = value
+
+
+class IQNavTurnController(TurnSignalPlanner):
+  def __init__(self, desire_helper):
+    super().__init__(desire_helper)
+
+  def read_params(self):
+    self.reload_setup()
+
+  def update_params(self) -> None:
+    self.heartbeat()
+
+  def update_lane_turn(self,
+                       blindspot_left: bool,
+                       blindspot_right: bool,
+                       left_blinker: bool,
+                       right_blinker: bool,
+                       v_ego: float) -> None:
+    self.sample(blocked_l=blindspot_left,
+                blocked_r=blindspot_right,
+                blink_l=left_blinker,
+                blink_r=right_blinker,
+                speed_mps=v_ego)
+
+  def get_turn_direction(self):
+    return self.output()
+
+  @property
+  def lane_turn_value(self):
+    return self.speed_gate
+
+  @lane_turn_value.setter
+  def lane_turn_value(self, value):
+    self.speed_gate = value

iqpilot/selfdrive/controls/lib/helpers/nav_torque_pulse.py

@@ -0,0 +1,83 @@
+"""
+Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
+
+Short, decaying steering-torque nudges that lean the car through navigation
+turns and highway exits. This is a lateral-control add-on driven by iqNavState;
+it is independent of the feed-forward model and is off by default.
+"""
+import numpy as np
+import iqpilot.cereal.messaging as messaging
+from iqpilot.cereal import custom
+
+TURN_NUDGE_TORQUE = 0.8
+EXIT_NUDGE_TORQUE = 0.6
+TURN_PULSE_FRAMES = 50
+EXIT_PULSE_FRAMES = 75
+
+# Master switch — nav torque influence is experimental and shipped off.
+IQP_NAV_TORQUE_INFLUENCE_ENABLED = False
+
+_LEFT = 1   # turnDesireDirection / lanePositioningDirection: 1 == left
+
+
+class NavTorquePulseBrain:
+  def __init__(self, lac_torque):
+    self._controller = lac_torque
+    self._nav_sm = messaging.SubMaster(["iqNavState"], poll="iqNavState")
+    self._nav_key = ""
+    self._nav_pulse_sign = 0.0
+    self._nav_pulse_frames = 0
+
+  def _lookup_nav_pulse(self):
+    if not IQP_NAV_TORQUE_INFLUENCE_ENABLED:
+      return "", 0.0, 0
+
+    self._nav_sm.update(0)
+    nav_state = self._nav_sm["iqNavState"]
+    phase = getattr(nav_state, "maneuverPhase", custom.IQNavState.ManeuverPhase.none)
+    maneuver_direction = getattr(nav_state, "maneuverDirection", custom.NavDirection.none)
+
+    # left nudges negative, otherwise positive
+    def turn(tag, direction):
+      return f"turn{tag}:{direction}", -TURN_NUDGE_TORQUE if direction == _LEFT else TURN_NUDGE_TORQUE, TURN_PULSE_FRAMES
+
+    def keep(tag, direction):
+      return f"{tag}:{direction}", -EXIT_NUDGE_TORQUE if direction == _LEFT else EXIT_NUDGE_TORQUE, EXIT_PULSE_FRAMES
+
+    if phase == custom.IQNavState.ManeuverPhase.turnActive:
+      return turn("-phase", getattr(nav_state, "turnDesireDirection", 0))
+    if phase == custom.IQNavState.ManeuverPhase.highwayCommit and maneuver_direction in (custom.NavDirection.left, custom.NavDirection.right):
+      return keep("highway-phase", getattr(nav_state, "lanePositioningDirection", 0))
+    if getattr(nav_state, "shouldSendTurnDesire", False):
+      return turn("", getattr(nav_state, "turnDesireDirection", 0))
+    if getattr(nav_state, "shouldSendLanePositioning", False):
+      return keep("keep", getattr(nav_state, "lanePositioningDirection", 0))
+    return "", 0.0, 0
+
+  def nudge_output_torque(self, active: bool, car_state, output_torque: float) -> float:
+    if not IQP_NAV_TORQUE_INFLUENCE_ENABLED:
+      self._nav_pulse_frames = 0
+      self._nav_key = ""
+      return output_torque
+
+    nav_key, pulse_sign, pulse_frames = self._lookup_nav_pulse()
+
+    if not active or getattr(car_state, "steeringPressed", False):
+      self._nav_pulse_frames = 0
+      if not nav_key:
+        self._nav_key = ""
+      return output_torque
+
+    if nav_key and nav_key != self._nav_key:
+      self._nav_key = nav_key
+      self._nav_pulse_sign = pulse_sign
+      self._nav_pulse_frames = pulse_frames
+    elif not nav_key and self._nav_pulse_frames == 0:
+      self._nav_key = ""
+
+    if self._nav_pulse_frames > 0:
+      self._nav_pulse_frames -= 1
+      steer_max = float(getattr(self._controller, "steer_max", 1.0))
+      output_torque = float(np.clip(output_torque + self._nav_pulse_sign, -steer_max, steer_max))
+
+    return output_torque

iqpilot/selfdrive/controls/lib/helpers/tests/__init__.py

@@ -0,0 +1,3 @@
+"""
+Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
+"""

iqpilot/selfdrive/controls/lib/helpers/tests/test_e2e_alerts.py

@@ -0,0 +1,133 @@
+"""
+Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
+"""
+from types import SimpleNamespace
+
+import pytest
+
+import iqpilot.cereal.messaging as messaging
+from iqpilot.cereal import custom
+from iqpilot.common.realtime import DT_MDL
+from iqpilot.selfdrive.controls.lib.helpers.e2e_alerts import (
+  EndToEndAlertEngine, CONFIRM_S, SETTLE_S, HORIZON_TAIL, PATH_SPEED_MPS, LEAD_SPEED_MPS, LEAD_GAP_M)
+
+E2E_CHIME = custom.IQOnroadEvent.EventName.e2eChime
+
+
+class _Events(list):
+  def add(self, name):
+    self.append(name)
+
+
+def _model(horizon):
+  msg = messaging.new_message('modelV2')
+  msg.modelV2.velocity.x = [0.0] * (33 - HORIZON_TAIL) + [horizon] * HORIZON_TAIL
+  return msg.as_reader().modelV2
+
+
+def _sm(*, v_ego=0.0, standstill=True, gas=False, enabled=False, cruise=False,
+        horizon=0.0, lead=None):
+  lead = lead or SimpleNamespace(status=False, dRel=0.0, vLead=0.0)
+  return {
+    'carState': SimpleNamespace(vEgo=v_ego, standstill=standstill, gasPressed=gas,
+                                cruiseState=SimpleNamespace(enabled=cruise)),
+    'selfdriveState': SimpleNamespace(enabled=enabled),
+    'radarState': SimpleNamespace(leadOne=lead),
+    'modelV2': _model(horizon),
+  }
+
+
+def _engine(path=True, lead=True):
+  engine = EndToEndAlertEngine()
+  engine._refresh_params = lambda: None
+  engine._on = {"path": path, "lead": lead}
+  return engine
+
+
+def _run(engine, sm, seconds):
+  events = _Events()
+  chimes = 0
+  for _ in range(int(seconds / DT_MDL)):
+    engine.update(sm, events)
+    chimes += events.count(E2E_CHIME)
+    events.clear()
+  return chimes
+
+
+def _lead(d_rel, v_lead=0.0):
+  return SimpleNamespace(status=True, dRel=d_rel, vLead=v_lead)
+
+
+def test_path_opens_chimes_once():
+  engine = _engine()
+  assert _run(engine, _sm(horizon=0.0), SETTLE_S + 1.0) == 0
+  assert _run(engine, _sm(horizon=PATH_SPEED_MPS + 2.0), CONFIRM_S + 1.0) == 1
+  assert _run(engine, _sm(horizon=PATH_SPEED_MPS + 2.0), 5.0) == 0
+
+
+def test_path_needs_the_settle_dwell():
+  engine = _engine()
+  assert _run(engine, _sm(horizon=PATH_SPEED_MPS + 2.0), SETTLE_S - 0.2) == 0
+
+
+def test_path_silent_while_openpilot_long_is_engaged():
+  engine = _engine()
+  _run(engine, _sm(horizon=0.0, enabled=True), SETTLE_S + 1.0)
+  assert _run(engine, _sm(horizon=PATH_SPEED_MPS + 2.0, enabled=True), 5.0) == 0
+
+
+def test_path_silent_while_stock_acc_holds_the_car():
+  engine = _engine()
+  _run(engine, _sm(horizon=0.0, cruise=True), SETTLE_S + 1.0)
+  assert _run(engine, _sm(horizon=PATH_SPEED_MPS + 2.0, cruise=True), 5.0) == 0
+
+
+def test_path_chimes_under_aol():
+  engine = _engine()
+  _run(engine, _sm(horizon=0.0), SETTLE_S + 1.0)
+  assert _run(engine, _sm(horizon=PATH_SPEED_MPS + 2.0), CONFIRM_S + 1.0) == 1
+
+
+def test_path_ignores_a_visible_lead():
+  engine = _engine(lead=False)
+  _run(engine, _sm(horizon=0.0, lead=_lead(6.0)), SETTLE_S + 1.0)
+  assert _run(engine, _sm(horizon=PATH_SPEED_MPS + 2.0, lead=_lead(6.0)), 5.0) == 0
+
+
+def test_lead_pullaway_chimes_once():
+  engine = _engine()
+  assert _run(engine, _sm(lead=_lead(6.0)), SETTLE_S + 1.0) == 0
+  moving = _sm(lead=_lead(6.0 + LEAD_GAP_M + 0.5, LEAD_SPEED_MPS + 1.0))
+  assert _run(engine, moving, CONFIRM_S + 1.0) == 1
+  assert _run(engine, moving, 5.0) == 0
+
+
+def test_lead_creep_inside_the_gap_stays_silent():
+  engine = _engine()
+  _run(engine, _sm(lead=_lead(6.0)), SETTLE_S + 1.0)
+  assert _run(engine, _sm(lead=_lead(6.0 + LEAD_GAP_M / 2, LEAD_SPEED_MPS + 1.0)), 5.0) == 0
+
+
+def test_lead_far_ahead_is_not_a_queue():
+  engine = _engine()
+  _run(engine, _sm(lead=_lead(40.0)), SETTLE_S + 1.0)
+  assert _run(engine, _sm(lead=_lead(44.0, LEAD_SPEED_MPS + 1.0)), 5.0) == 0
+
+
+def test_gas_and_motion_rearm_the_dwell():
+  engine = _engine()
+  _run(engine, _sm(horizon=0.0), SETTLE_S + 1.0)
+  _run(engine, _sm(v_ego=5.0, standstill=False, horizon=8.0), 2.0)
+  assert _run(engine, _sm(horizon=PATH_SPEED_MPS + 2.0), SETTLE_S - 0.2) == 0
+  assert _run(engine, _sm(horizon=PATH_SPEED_MPS + 2.0), CONFIRM_S + 1.0) == 1
+
+
+@pytest.mark.parametrize("param_off", ["path", "lead"])
+def test_each_param_gates_only_its_own_trigger(param_off):
+  engine = _engine(path=param_off != "path", lead=param_off != "lead")
+  if param_off == "path":
+    _run(engine, _sm(horizon=0.0), SETTLE_S + 1.0)
+    assert _run(engine, _sm(horizon=PATH_SPEED_MPS + 2.0), 5.0) == 0
+  else:
+    _run(engine, _sm(lead=_lead(6.0)), SETTLE_S + 1.0)
+    assert _run(engine, _sm(lead=_lead(8.0, LEAD_SPEED_MPS + 1.0)), 5.0) == 0

iqpilot/selfdrive/controls/lib/helpers/tests/test_nav_torque_pulse.py

@@ -0,0 +1,105 @@
+"""
+Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
+"""
+from types import SimpleNamespace
+
+import numpy as np
+import pytest
+
+from iqpilot.cereal import custom
+import iqpilot.selfdrive.controls.lib.helpers.nav_torque_pulse as nav_pulse
+from iqpilot.selfdrive.controls.lib.helpers.nav_torque_pulse import (
+  NavTorquePulseBrain, TURN_PULSE_FRAMES, EXIT_PULSE_FRAMES)
+
+
+@pytest.fixture
+def influence_on():
+  nav_pulse.IQP_NAV_TORQUE_INFLUENCE_ENABLED = True
+  try:
+    yield
+  finally:
+    nav_pulse.IQP_NAV_TORQUE_INFLUENCE_ENABLED = False
+
+
+def _fixed_nav_sm(**fields):
+  nav = SimpleNamespace(
+    maneuverPhase=custom.IQNavState.ManeuverPhase.none,
+    maneuverDirection=custom.NavDirection.none,
+    shouldSendTurnDesire=False,
+    turnDesireDirection=0,
+    shouldSendLanePositioning=False,
+    lanePositioningDirection=0,
+  )
+  for k, v in fields.items():
+    setattr(nav, k, v)
+
+  class SM:
+    def update(self, _):
+      return None
+
+    def __getitem__(self, _):
+      return nav
+  return SM()
+
+
+def _brain(nav_sm=None, steer_max=1.0):
+  brain = NavTorquePulseBrain(SimpleNamespace(steer_max=steer_max))
+  if nav_sm is not None:
+    brain._nav_sm = nav_sm
+  return brain
+
+
+def test_passthrough_when_disabled():
+  brain = _brain()
+  cs = SimpleNamespace(steeringPressed=False)
+  assert brain.nudge_output_torque(True, cs, 0.42) == 0.42
+
+
+class TestPulseSign:
+  @pytest.mark.parametrize("direction,expect_negative", [(1, True), (2, False)])
+  def test_turn_desire_direction(self, influence_on, direction, expect_negative):
+    brain = _brain(_fixed_nav_sm(shouldSendTurnDesire=True, turnDesireDirection=direction))
+    cs = SimpleNamespace(steeringPressed=False)
+    first = brain.nudge_output_torque(True, cs, 0.0)
+    assert (first < 0.0) == expect_negative
+
+  def test_turn_active_phase_uses_turn_frames(self, influence_on):
+    brain = _brain(_fixed_nav_sm(maneuverPhase=custom.IQNavState.ManeuverPhase.turnActive,
+                                 turnDesireDirection=1))
+    cs = SimpleNamespace(steeringPressed=False)
+    outs = [brain.nudge_output_torque(True, cs, 0.0) for _ in range(TURN_PULSE_FRAMES + 2)]
+    assert outs[TURN_PULSE_FRAMES - 1] < 0.0
+    assert outs[TURN_PULSE_FRAMES] == 0.0
+
+
+class TestPulseLifecycle:
+  def test_pulse_expires_after_its_frame_count(self, influence_on):
+    brain = _brain(_fixed_nav_sm(shouldSendTurnDesire=True, turnDesireDirection=1))
+    cs = SimpleNamespace(steeringPressed=False)
+    outs = [brain.nudge_output_torque(True, cs, 0.0) for _ in range(TURN_PULSE_FRAMES + 3)]
+    assert all(o < 0.0 for o in outs[:TURN_PULSE_FRAMES])
+    assert all(o == 0.0 for o in outs[TURN_PULSE_FRAMES:])
+    assert all(np.isfinite(o) for o in outs)
+
+  def test_steering_press_suppresses_pulse(self, influence_on):
+    brain = _brain(_fixed_nav_sm(shouldSendTurnDesire=True, turnDesireDirection=1))
+    cs = SimpleNamespace(steeringPressed=True)
+    assert brain.nudge_output_torque(True, cs, 0.4) == 0.4
+
+  def test_inactive_suppresses_pulse(self, influence_on):
+    brain = _brain(_fixed_nav_sm(shouldSendTurnDesire=True, turnDesireDirection=1))
+    cs = SimpleNamespace(steeringPressed=False)
+    assert brain.nudge_output_torque(False, cs, 0.4) == 0.4
+
+  def test_output_clamped_to_steer_max(self, influence_on):
+    brain = _brain(_fixed_nav_sm(shouldSendTurnDesire=True, turnDesireDirection=2), steer_max=0.5)
+    cs = SimpleNamespace(steeringPressed=False)
+    out = brain.nudge_output_torque(True, cs, 0.4)  # 0.4 + 0.8 nudge, clamped to 0.5
+    assert out == pytest.approx(0.5)
+
+  def test_lane_positioning_uses_exit_frames(self, influence_on):
+    brain = _brain(_fixed_nav_sm(shouldSendLanePositioning=True, lanePositioningDirection=1))
+    cs = SimpleNamespace(steeringPressed=False)
+    outs = [brain.nudge_output_torque(True, cs, 0.0) for _ in range(EXIT_PULSE_FRAMES + 2)]
+    assert outs[EXIT_PULSE_FRAMES - 1] < 0.0
+    assert outs[EXIT_PULSE_FRAMES] == 0.0