IQ.Pilot Release Commit @ f2a861c

iqpilot/selfdrive/selfdrived/alertmanager.py

@@ -0,0 +1,68 @@
+import copy
+import os
+import json
+from collections import defaultdict
+from dataclasses import dataclass
+
+from iqpilot.common.basedir import BASEDIR
+from iqpilot.common.params import Params
+from iqpilot.selfdrive.selfdrived.events import Alert
+from iqpilot.common.atlas_alerts import NULL_ALERT as EmptyAlert
+
+
+with open(os.path.join(BASEDIR, "iqpilot/selfdrive/selfdrived/alerts_offroad.json")) as f:
+  OFFROAD_ALERTS = json.load(f)
+
+
+def set_offroad_alert(alert: str, show_alert: bool, extra_text: str | None = None) -> None:
+  if show_alert:
+    a = copy.copy(OFFROAD_ALERTS[alert])
+    a['extra'] = extra_text or ''
+    Params().put(alert, a)
+  else:
+    Params().remove(alert)
+
+
+@dataclass
+class AlertEntry:
+  alert: Alert | None = None
+  start_frame: int = -1
+  end_frame: int = -1
+  added_frame: int = -1
+
+  def active(self, frame: int) -> bool:
+    return frame <= self.end_frame
+
+  def just_added(self, frame: int) -> bool:
+    return self.active(frame) and frame == (self.added_frame + 1)
+
+class AlertManager:
+  def __init__(self):
+    self.alerts: dict[str, AlertEntry] = defaultdict(AlertEntry)
+    self.current_alert = EmptyAlert
+
+  def add_many(self, frame: int, alerts: list[Alert]) -> None:
+    for alert in alerts:
+      entry = self.alerts[alert.alert_type]
+      entry.alert = alert
+      if not entry.just_added(frame):
+        entry.start_frame = frame
+      min_end_frame = entry.start_frame + alert.duration
+      entry.end_frame = max(frame + 1, min_end_frame)
+      entry.added_frame = frame
+
+  def process_alerts(self, frame: int, clear_event_types: set):
+    ae = AlertEntry()
+    for v in self.alerts.values():
+      if not v.alert:
+        continue
+
+      if v.alert.event_type in clear_event_types:
+        v.end_frame = -1
+
+      # sort by priority first and then by start_frame
+      greater = ae.alert is None or (v.alert.priority, v.start_frame) > (ae.alert.priority, ae.start_frame)
+      if v.active(frame) and greater:
+        ae = v
+
+    self.current_alert = ae.alert if ae.alert is not None else EmptyAlert

iqpilot/selfdrive/selfdrived/alerts_offroad.json

@@ -0,0 +1,86 @@
+{
+  "Offroad_TemperatureTooHigh": {
+    "text": "Device temperature too high. System cooling down before starting. Current internal component temperature: %1",
+    "severity": 0
+  },
+  "Offroad_ConnectivityNeededPrompt": {
+    "text": "Immediately connect to the internet to check for updates. If you do not connect to the internet, iqpilot won't engage in %1",
+    "severity": 0,
+    "_comment": "Set extra field to number of days"
+  },
+  "Offroad_ConnectivityNeeded": {
+    "text": "Connect to internet to check for updates. iqpilot won't automatically start until it connects to internet to check for updates.",
+    "severity": 1
+  },
+  "Offroad_UpdateFailed": {
+    "text": "Unable to download updates\n%1",
+    "severity": 1,
+    "_comment": "Set extra field to the failed reason."
+  },
+  "Offroad_IsTakingSnapshot": {
+    "text": "Taking camera snapshots. System won't start until finished.",
+    "severity": 0
+  },
+  "Offroad_NeosUpdate": {
+    "text": "An update to your device's operating system is downloading in the background. You will be prompted to update when it's ready to install.",
+    "severity": 0
+  },
+  "Offroad_UnregisteredHardware": {
+    "text": "Failed to register device with konn3kt. If you need assistance, contact IQ.Pilot support.",
+    "severity": 1
+  },
+  "Offroad_StorageMissing": {
+    "text": "NVMe drive not mounted.",
+    "severity": 1
+  },
+  "Offroad_CarUnrecognized": {
+    "text": "iqpilot was unable to identify your car. Your car is either unsupported or its ECUs are not recognized. Please submit a pull request to add the firmware versions to the proper vehicle.",
+    "severity": 0
+  },
+  "Offroad_Recalibration": {
+    "text": "iqpilot detected a change in the device's mounting position. Ensure the device is fully seated in the mount and the mount is firmly secured to the windshield.",
+    "severity": 0
+  },
+  "Offroad_OSMUpdateRequired": {
+    "text": "OpenStreetMap database is out of date. New maps must be downloaded if you wish to continue using OpenStreetMap data for Enhanced Speed Control and road name display.\n\n%1",
+    "severity": 0
+  },
+  "Offroad_ExcessiveActuation": {
+    "text": "Excessive %1 actuation detected on your last drive. Please contact IQ.Pilot support and share your device ID for troubleshooting.",
+    "severity": 1,
+    "_comment": "Set extra field to lateral or longitudinal."
+  },
+  "Offroad_TiciSupport": {
+    "text": "<b>Unsupported branch!</b> - The current version of <b><u>%1</u></b> is not marked as compatible with the Comma Three (3|tici). Please go to <b>[Device > Software]</b> and install a supported branch such as <b><u>release</u></b> or <b><u>beta</u></b> for the comma three.",
+    "severity": 1,
+    "_comment": "Set extra field to the current branch name."
+  },
+  "Offroad_EgpuNotDetected": {
+    "text": "eGPU dock not detected. Check USB and 12V connections.",
+    "severity": 0
+  },
+  "Offroad_EgpuFansObstructed": {
+    "text": "eGPU dock fans obstructed. Check the fans.",
+    "severity": 0
+  },
+  "Offroad_EgpuOverheated": {
+    "text": "eGPU dock overheated. Allow it to cool.",
+    "severity": 0
+  },
+  "Offroad_EgpuPcieUnavailable": {
+    "text": "eGPU dock PCIe unavailable. %1",
+    "severity": 0
+  },
+  "Offroad_EgpuUncompiled": {
+    "text": "eGPU big model not compiled. Keep ignition on and reboot the device.",
+    "severity": 0
+  },
+  "Offroad_EgpuUpdateFailed": {
+    "text": "eGPU dock update failed. Check the USB cable.",
+    "severity": 0
+  },
+  "Offroad_EgpuUsbSlow": {
+    "text": "eGPU dock USB link is slow. Check the USB cable. The current speed is %1.",
+    "severity": 0
+  }
+}

iqpilot/selfdrive/selfdrived/helpers.py

@@ -0,0 +1,55 @@
+import math
+from enum import StrEnum, auto
+
+from iqpilot.cereal import car, messaging
+from iqpilot.common.realtime import DT_CTRL
+from iqpilot.selfdrive.locationd.helpers import Pose
+from iqdbc.car import ACCELERATION_DUE_TO_GRAVITY
+from iqdbc.car.lateral import ISO_LATERAL_ACCEL
+from iqdbc.car.interfaces import ACCEL_MIN, ACCEL_MAX
+
+MIN_EXCESSIVE_ACTUATION_COUNT = int(0.25 / DT_CTRL)
+MIN_LATERAL_ENGAGE_BUFFER = int(1 / DT_CTRL)
+
+
+class ExcessiveActuationType(StrEnum):
+  LONGITUDINAL = auto()
+  LATERAL = auto()
+
+
+class ExcessiveActuationCheck:
+  def __init__(self):
+    self._excessive_counter = 0
+    self._engaged_counter = 0
+
+  def update(self, sm: messaging.SubMaster, CS: car.CarState, calibrated_pose: Pose) -> ExcessiveActuationType | None:
+    # CS.aEgo can be noisy to bumps in the road, transitioning from standstill, losing traction, etc.
+    # longitudinal
+    accel_calibrated = calibrated_pose.acceleration.x
+    excessive_long_actuation = sm['carControl'].longActive and ((not CS.gasPressed and accel_calibrated > ACCEL_MAX * 2) or
+                                                                accel_calibrated < ACCEL_MIN * 2)
+
+    # lateral
+    yaw_rate = calibrated_pose.angular_velocity.yaw
+    roll = sm['vehicleParameters'].roll
+    roll_compensated_lateral_accel = (CS.vEgo * yaw_rate) - (math.sin(roll) * ACCELERATION_DUE_TO_GRAVITY)
+
+    # Prevent false positives after overriding
+    excessive_lat_actuation = False
+    self._engaged_counter = self._engaged_counter + 1 if sm['carControl'].latActive and not CS.steeringPressed else 0
+    if self._engaged_counter > MIN_LATERAL_ENGAGE_BUFFER:
+      if abs(roll_compensated_lateral_accel) > ISO_LATERAL_ACCEL * 2:
+        excessive_lat_actuation = True
+
+    # deviceMotion acceleration can be noisy due to bad mounting or aliased deviceMotion measurements
+    livepose_valid = abs(CS.aEgo - accel_calibrated) < 2
+    self._excessive_counter = self._excessive_counter + 1 if livepose_valid and (excessive_long_actuation or excessive_lat_actuation) else 0
+
+    excessive_type = None
+    if self._excessive_counter > MIN_EXCESSIVE_ACTUATION_COUNT:
+      if excessive_long_actuation:
+        excessive_type = ExcessiveActuationType.LONGITUDINAL
+      else:
+        excessive_type = ExcessiveActuationType.LATERAL
+
+    return excessive_type

iqpilot/selfdrive/selfdrived/iq_events.py

@@ -0,0 +1,399 @@
+import iqpilot.cereal.messaging as messaging
+from iqpilot.cereal import log, car, custom
+from iqpilot.common.params import Params
+from iqpilot.common.constants import CV
+from iqpilot.common.atlas_alerts import EventBook as EventsBase, Tier as Priority, Tags as ET, AlertCard as Alert, \
+  NoEntryCard as NoEntryAlert, HardDisableCard as ImmediateDisableAlert, ChimeCard as EngagementAlert, \
+  BannerCard as NormalPermanentAlert, AlertFactory as AlertCallbackType, car_mode_entry_alert as wrong_car_mode_alert
+from iqpilot.selfdrive.controls.lib.speed_limit_controller import SpeedLimitAssistState
+
+
+AlertSize = log.SelfdriveState.AlertSize
+AlertStatus = log.SelfdriveState.AlertStatus
+VisualAlert = car.CarControl.HUDControl.VisualAlert
+AudibleAlert = car.CarControl.HUDControl.AudibleAlert
+AudibleAlertIQ = custom.IQState.AudibleAlert
+EventNameIQ = custom.IQOnroadEvent.EventName
+
+
+# get event name from enum
+EVENT_NAME_IQ = {v: k for k, v in EventNameIQ.schema.enumerants.items()}
+
+
+def _get_longitudinal_plan_ext(sm: messaging.SubMaster):
+  return sm['iqPlan']
+
+
+def speed_limit_adjust_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> Alert:
+  plan = _get_longitudinal_plan_ext(sm)
+  resolver = plan.speedLimit.resolver
+  assist = plan.speedLimit.assist
+  speed_conv = CV.MS_TO_KPH if metric else CV.MS_TO_MPH
+  speed = round(resolver.speedLimit * speed_conv)
+  unit = "km/h" if metric else "mph"
+  if assist.state == SpeedLimitAssistState.adapting:
+    message = f"Speed Limit: Adjusting to {speed} {unit}"
+  else:
+    message = f"Speed Limit: Active at {speed} {unit}"
+  return Alert(
+    message,
+    "",
+    AlertStatus.normal, AlertSize.small,
+    Priority.LOW, VisualAlert.none, AudibleAlert.none, 4.)
+
+
+def speed_limit_pre_active_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> Alert:
+  plan = _get_longitudinal_plan_ext(sm)
+  resolver = plan.speedLimit.resolver
+  speed_conv = CV.MS_TO_KPH if metric else CV.MS_TO_MPH
+  unit = "km/h" if metric else "mph"
+  pending_speed = round(resolver.speedLimit * speed_conv)
+  last_speed = resolver.speedLimitFinalLast * speed_conv
+  is_lower = pending_speed < last_speed or last_speed <= 0
+  confirm_hint = "SET" if is_lower else "RES"
+  return Alert(
+    f"Speed Limit: {pending_speed} {unit}",
+    f"Press {confirm_hint} to apply",
+    AlertStatus.normal, AlertSize.mid,
+    Priority.LOW, VisualAlert.none, AudibleAlertIQ.promptSingleLow, .1)
+
+
+def speed_limit_changed_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> Alert:
+  resolver = _get_longitudinal_plan_ext(sm).speedLimit.resolver
+  speed_conv = CV.MS_TO_KPH if metric else CV.MS_TO_MPH
+  speed = round(resolver.speedLimit * speed_conv)
+  unit = "km/h" if metric else "mph"
+  return Alert(
+    f"Speed Limit changed to {speed} {unit}",
+    "",
+    AlertStatus.normal, AlertSize.small,
+    Priority.LOW, VisualAlert.none, AudibleAlertIQ.promptSingleHigh, 3.)
+
+
+def construction_zone_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> Alert:
+  resolver = _get_longitudinal_plan_ext(sm).speedLimit.resolver
+  speed_conv = CV.MS_TO_KPH if metric else CV.MS_TO_MPH
+  speed = round(resolver.speedLimit * speed_conv)
+  unit = "KM/H" if metric else "MPH"
+  return Alert(
+    f"Construction Zone Detected: Speed {speed} {unit}",
+    "",
+    AlertStatus.userPrompt, AlertSize.small,
+    Priority.MID, VisualAlert.none, AudibleAlertIQ.promptSingleHigh, 4.)
+
+
+_CAMERA_LABELS = {
+  int(custom.IQNavState.CameraType.fixedSpeed): "Speed Camera",
+  int(custom.IQNavState.CameraType.mobileSpeed): "Mobile Speed Camera",
+  int(custom.IQNavState.CameraType.sectionStart): "Average-Speed Zone",
+  int(custom.IQNavState.CameraType.sectionEnd): "Average-Speed Zone Ends",
+  int(custom.IQNavState.CameraType.averageZone): "Average-Speed Zone",
+  int(custom.IQNavState.CameraType.redLight): "Red-Light Camera",
+  int(custom.IQNavState.CameraType.bump): "Speed Bump",
+  int(custom.IQNavState.CameraType.alpr): "Flock / ALPR Camera",
+  int(custom.IQNavState.CameraType.police): "Police Reported Ahead",
+}
+
+_POLICE_CHIMED_IDS: set[str] = set()
+_USA_REGION_CODES = frozenset(("US", "USA", "UNITED STATES", "UNITED STATES OF AMERICA"))
+
+
+def _configured_country_code() -> str:
+  try:
+    value = Params().get("OsmLocationName")
+  except Exception:
+    return ""
+  if isinstance(value, bytes):
+    value = value.decode("utf-8", "ignore")
+  return str(value or "").strip().upper()
+
+
+def _alpr_alert_labels(country_code: str) -> tuple[str, str]:
+  is_row = bool(country_code) and country_code not in _USA_REGION_CODES
+  if is_row:
+    return "Traffic / ALPR Camera", "Traffic / ALPR Camera Detected"
+  return "Flock / ALPR Camera", "Flock Camera Detected"
+
+
+def speed_camera_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> Alert:
+  nav = sm['iqNavState']
+  ctype = int(getattr(nav.cameraType, "raw", nav.cameraType))
+  label = _CAMERA_LABELS.get(ctype, "Speed Camera")
+  distance = float(nav.cameraDistance)
+  alpr_detected_label = "Flock Camera Detected"
+  if ctype == int(custom.IQNavState.CameraType.alpr):
+    label, alpr_detected_label = _alpr_alert_labels(_configured_country_code())
+  # RF (BLE/WiFi) Flock detection is a live proximity hit with no meaningful
+  # distance — flockd/navd flag it with distance 0 on the alpr camera type.
+  if ctype == int(custom.IQNavState.CameraType.alpr) and distance <= 0.0:
+    return Alert(
+      alpr_detected_label,
+      "",
+      AlertStatus.normal, AlertSize.small,
+      Priority.LOW, VisualAlert.none, AudibleAlert.prompt, .2)
+  if metric:
+    dist_str = f"{distance:.0f} m" if distance < 1000.0 else f"{distance / 1000.0:.1f} km"
+  else:
+    feet = distance * 3.28084
+    dist_str = f"{int(round(feet / 10.0) * 10)} ft" if feet < 1000.0 else f"{distance * 0.000621371:.1f} mi"
+  detail = dist_str
+  if float(nav.cameraSpeedLimit) > 0.0:
+    speed_conv = CV.MS_TO_KPH if metric else CV.MS_TO_MPH
+    unit = "km/h" if metric else "mph"
+    detail += f" • {round(float(nav.cameraSpeedLimit) * speed_conv)} {unit}"
+  audible = AudibleAlert.prompt
+  if ctype == int(custom.IQNavState.CameraType.police):
+    report_id = str(getattr(nav, "cameraAlertId", ""))
+    should_chime = bool(getattr(nav, "cameraChime", False)) and bool(report_id) and report_id not in _POLICE_CHIMED_IDS
+    if should_chime:
+      _POLICE_CHIMED_IDS.add(report_id)
+      if len(_POLICE_CHIMED_IDS) > 256:
+        _POLICE_CHIMED_IDS.clear()
+        _POLICE_CHIMED_IDS.add(report_id)
+    else:
+      audible = AudibleAlert.none
+  return Alert(
+    f"{label} • {detail}",
+    "",
+    AlertStatus.normal, AlertSize.small,
+    Priority.LOW if ctype == int(custom.IQNavState.CameraType.alpr) else Priority.HIGH,
+    VisualAlert.none, audible, .2)
+
+
+class IQEvents(EventsBase):
+  def __init__(self):
+    super().__init__()
+    self.event_counters = dict.fromkeys(EVENTS_IQ.keys(), 0)
+
+  def get_events_mapping(self) -> dict[int, dict[str, Alert | AlertCallbackType]]:
+    return EVENTS_IQ
+
+  def get_event_name(self, event: int):
+    return EVENT_NAME_IQ[event]
+
+  def get_event_msg_type(self):
+    return custom.IQOnroadEvent.Event
+
+
+EVENTS_IQ_TYPE = dict[int, dict[str, Alert | AlertCallbackType]]
+
+_GUIDANCE_EVENTS: EVENTS_IQ_TYPE = {
+  EventNameIQ.lateralEdgeBlocked: {
+    ET.WARNING: Alert(
+      "Lane Change Blocked",
+      "Road edge detected",
+      AlertStatus.userPrompt, AlertSize.mid,
+      Priority.LOW, VisualAlert.none, AudibleAlert.prompt, .1),
+  },
+
+  EventNameIQ.speedLimitActive: {
+    ET.WARNING: speed_limit_adjust_alert,
+  },
+
+  EventNameIQ.speedLimitPreActive: {
+    ET.WARNING: speed_limit_pre_active_alert,
+  },
+
+  EventNameIQ.speedLimitChanged: {
+    ET.WARNING: speed_limit_changed_alert,
+  },
+
+  EventNameIQ.speedCameraAhead: {
+    ET.WARNING: speed_camera_alert,
+  },
+
+  EventNameIQ.constructionZoneDetected: {
+    ET.WARNING: construction_zone_alert,
+  },
+
+  EventNameIQ.navExitLeft: {
+    ET.WARNING: Alert(
+      "Navigation: Exit Maneuver",
+      "Nudge the wheel left to change lanes",
+      AlertStatus.userPrompt, AlertSize.mid,
+      Priority.MID, VisualAlert.none, AudibleAlert.prompt, 1.5),
+  },
+
+  EventNameIQ.navExitRight: {
+    ET.WARNING: Alert(
+      "Navigation: Exit Maneuver",
+      "Nudge the wheel right to change lanes",
+      AlertStatus.userPrompt, AlertSize.mid,
+      Priority.MID, VisualAlert.none, AudibleAlert.prompt, 1.5),
+  },
+
+  EventNameIQ.navTurnLeft: {
+    ET.WARNING: Alert(
+      "Navigation: Turning Left",
+      "",
+      AlertStatus.normal, AlertSize.small,
+      Priority.MID, VisualAlert.none, AudibleAlert.none, 1.5),
+  },
+
+  EventNameIQ.navTurnRight: {
+    ET.WARNING: Alert(
+      "Navigation: Turning Right",
+      "",
+      AlertStatus.normal, AlertSize.small,
+      Priority.MID, VisualAlert.none, AudibleAlert.none, 1.5),
+  },
+
+  EventNameIQ.modelTurnLeft: {
+    ET.WARNING: Alert(
+      "Lane Turn Left",
+      "",
+      AlertStatus.normal, AlertSize.small,
+      Priority.LOW, VisualAlert.none, AudibleAlert.none, 1.),
+  },
+
+  EventNameIQ.modelTurnRight: {
+    ET.WARNING: Alert(
+      "Lane Turn Right",
+      "",
+      AlertStatus.normal, AlertSize.small,
+      Priority.LOW, VisualAlert.none, AudibleAlert.none, 1.),
+  },
+
+}
+
+_ENGAGE_EVENTS: EVENTS_IQ_TYPE = {
+  EventNameIQ.alcEngaged: {
+    ET.ENABLE: EngagementAlert(AudibleAlert.engage),
+  },
+
+  EventNameIQ.alcEngagedSilent: {
+    ET.ENABLE: EngagementAlert(AudibleAlert.none),
+  },
+
+  EventNameIQ.alcDisengaged: {
+    ET.USER_DISABLE: EngagementAlert(AudibleAlert.disengage),
+  },
+
+  EventNameIQ.alcDisengagedSilent: {
+    ET.USER_DISABLE: EngagementAlert(AudibleAlert.none),
+  },
+
+  EventNameIQ.steerManually: {
+    ET.USER_DISABLE: Alert(
+      "Lane Centering Off",
+      "Steer Manually",
+      AlertStatus.normal, AlertSize.mid,
+      Priority.LOW, VisualAlert.none, AudibleAlert.disengage, 1.),
+  },
+
+  EventNameIQ.speedManually: {
+    ET.WARNING: Alert(
+      "Adaptive Cruise Off",
+      "Control Speed Manually",
+      AlertStatus.normal, AlertSize.mid,
+      Priority.LOW, VisualAlert.none, AudibleAlert.none, 1.),
+  },
+
+  EventNameIQ.steeringOverrideReengageAlc: {
+    ET.WARNING: Alert(
+      "Steering Overridden By Driver",
+      "Double Tap SET or Cycle the Cruise Main to Re-Engage ALC",
+      AlertStatus.userPrompt, AlertSize.mid,
+      Priority.MID, VisualAlert.none, AudibleAlert.prompt, 2.0),
+  },
+
+  EventNameIQ.latMismatch: {
+    ET.IMMEDIATE_DISABLE: ImmediateDisableAlert("Lateral Controls Mismatch"),
+    ET.NO_ENTRY: NoEntryAlert("Lateral Controls Mismatch"),
+  },
+
+}
+
+_CABIN_BLOCK_EVENTS: EVENTS_IQ_TYPE = {
+  EventNameIQ.brakeHoldSilent: {
+    ET.WARNING: EngagementAlert(AudibleAlert.none),
+    ET.NO_ENTRY: NoEntryAlert("Brake Hold Engaged"),
+  },
+
+  EventNameIQ.gearNotDriveSilent: {
+    ET.WARNING: Alert(
+      "",
+      "",
+      AlertStatus.normal, AlertSize.none,
+      Priority.LOWEST, VisualAlert.none, AudibleAlert.none, 0.),
+    ET.NO_ENTRY: Alert(
+      "",
+      "",
+      AlertStatus.normal, AlertSize.none,
+      Priority.LOW, VisualAlert.none, AudibleAlert.none, 0.),
+  },
+
+  EventNameIQ.parkBrakeSilent: {
+    ET.WARNING: Alert(
+      "",
+      "",
+      AlertStatus.normal, AlertSize.none,
+      Priority.LOWEST, VisualAlert.none, AudibleAlert.none, 0.),
+    ET.NO_ENTRY: NoEntryAlert("Parking Brake On"),
+  },
+
+  EventNameIQ.doorAjarSilent: {
+    ET.WARNING: Alert(
+      "",
+      "",
+      AlertStatus.normal, AlertSize.none,
+      Priority.LOWEST, VisualAlert.none, AudibleAlert.none, 0.),
+    ET.NO_ENTRY: NoEntryAlert("Door Ajar"),
+  },
+
+  EventNameIQ.seatbeltUnbuckledSilent: {
+    ET.WARNING: Alert(
+      "",
+      "",
+      AlertStatus.normal, AlertSize.none,
+      Priority.LOWEST, VisualAlert.none, AudibleAlert.none, 0.),
+    ET.NO_ENTRY: NoEntryAlert("Seatbelt Unbuckled"),
+  },
+
+  EventNameIQ.reverseSilent: {
+    ET.PERMANENT: Alert(
+      "In\nReverse",
+      "",
+      AlertStatus.normal, AlertSize.full,
+      Priority.LOWEST, VisualAlert.none, AudibleAlert.none, .2, creation_delay=0.5),
+    ET.NO_ENTRY: NoEntryAlert("In Reverse"),
+  },
+
+}
+
+_NOTICE_EVENTS: EVENTS_IQ_TYPE = {
+  EventNameIQ.carModeMismatchNotice: {
+    ET.WARNING: wrong_car_mode_alert,
+  },
+
+  EventNameIQ.pedalHeldNotice: {
+    ET.WARNING: NoEntryAlert("Brake Pedal Held")
+  },
+
+  EventNameIQ.experimentalToggled: {
+    ET.WARNING: NormalPermanentAlert("Switched to IQ.Pilot End to End Control", duration=1.5)
+  },
+
+  EventNameIQ.e2eChime: {
+    ET.PERMANENT: Alert(
+      "",
+      "",
+      AlertStatus.normal, AlertSize.none,
+      Priority.MID, VisualAlert.none, AudibleAlert.prompt, 3.),
+  },
+
+  EventNameIQ.wideCamFaulty: {
+    ET.PERMANENT: NormalPermanentAlert("Wide Cam Faulty",
+                                       "IQ.Pilot still available, degraded via road cam only",
+                                       priority=Priority.LOW),
+  },
+
+  EventNameIQ.modelUpdating: {
+    ET.NO_ENTRY: NoEntryAlert("Update finishes while parked with internet",
+                              alert_text_1="Driving Model Updating",
+                              priority=Priority.MID),
+  },
+
+}
+
+EVENTS_IQ: EVENTS_IQ_TYPE = {**_GUIDANCE_EVENTS, **_ENGAGE_EVENTS, **_CABIN_BLOCK_EVENTS, **_NOTICE_EVENTS}

iqpilot/selfdrive/selfdrived/selfdrived.py

@@ -0,0 +1,807 @@
+#!/usr/bin/env python3
+import os
+import time
+import threading
+
+import iqpilot.cereal.messaging as messaging
+
+from iqpilot.cereal import car, log, custom
+from iqpilot.cereal.visionipc import VisionStreamType
+from msgq.visionipc import VisionIpcClient
+
+
+from iqpilot.common.params import Params
+from iqpilot.common.issue_debug import log_issue_limited
+from iqpilot.common.realtime import config_background_thread, config_realtime_process, lock_memory, Priority, Ratekeeper, DT_CTRL
+from iqpilot.common.swaglog import cloudlog
+from iqpilot.common.gps import get_gps_location_service
+
+from iqpilot.selfdrive.car.car_specific import CarSpecificEvents
+from iqpilot.selfdrive.locationd.helpers import PoseCalibrator, Pose
+from iqpilot.selfdrive.selfdrived.events import Events, ET
+from iqpilot.selfdrive.selfdrived.helpers import ExcessiveActuationCheck
+from iqpilot.selfdrive.selfdrived.state import StateMachine
+from iqpilot.selfdrive.selfdrived.alertmanager import AlertManager, set_offroad_alert
+from iqpilot.selfdrive.longitudinal_settings import get_runtime_personality
+
+from iqpilot.system.version import get_build_metadata
+from iqpilot.system.hardware import HARDWARE
+
+from iqpilot.sab.behavior import SteeringAssistanceBehavior
+from iqpilot.selfdrive.controls.lib.helpers.lane_change import NAV_EXIT_COMMIT_DISTANCE
+from iqpilot.vehicle.vehicle import VehicleEvents
+from iqpilot.selfdrive.car.gap_button_actions import GapButtonActions
+from iqpilot.selfdrive.selfdrived.iq_events import IQEvents
+
+REPLAY = "REPLAY" in os.environ
+SIMULATION = "SIMULATION" in os.environ
+TESTING_CLOSET = "TESTING_CLOSET" in os.environ
+
+WIDE_CAM_FAULTY_ALERT_FRAMES = int(300. / DT_CTRL)
+
+LONGITUDINAL_PERSONALITY_MAP = {v: k for k, v in log.LongitudinalPersonality.schema.enumerants.items()}
+
+ThermalStatus = log.DeviceState.ThermalStatus
+State = log.SelfdriveState.OpenpilotState
+PandaType = log.PandaState.PandaType
+LaneChangeState = log.LaneChangeState
+LaneChangeDirection = log.LaneChangeDirection
+EventName = log.OnroadEvent.EventName
+ButtonType = car.CarState.ButtonEvent.Type
+SafetyModel = car.CarParams.SafetyModel
+TurnDirection = custom.IQTurnSignalDirection
+
+IGNORED_SAFETY_MODES = (SafetyModel.silent, SafetyModel.noOutput)
+
+NON_BLOCKING_PROCESSES = {'mapd', 'iqmapd', 'navd', 'navincidentd', 'navrenderd'}
+
+
+def _cleanup_startup_params(CP: car.CarParams, params: Params) -> None:
+  if not CP.alphaLongitudinalAvailable or not CP.openpilotLongitudinalControl:
+    return
+
+
+class SelfdriveD(GapButtonActions):
+  def __init__(self, CP=None, CP_IQ=None):
+    self.params = Params()
+
+    # Ensure the current branch is cached, otherwise the first cycle lags
+    build_metadata = get_build_metadata()
+
+    if CP is None:
+      cloudlog.info("selfdrived is waiting for CarParams")
+      self.CP = messaging.log_from_bytes(self.params.get("CarParams", block=True), car.CarParams)
+      cloudlog.info("selfdrived got CarParams")
+    else:
+      self.CP = CP
+
+    if CP_IQ is None:
+      cloudlog.info("selfdrived is waiting for IQCarParams")
+      self.CP_IQ = messaging.log_from_bytes(self.params.get("IQCarParams", block=True), custom.IQCarParams)
+      cloudlog.info("selfdrived got IQCarParams")
+    else:
+      self.CP_IQ = CP_IQ
+
+    self.car_events = CarSpecificEvents(self.CP)
+
+    self.pose_calibrator = PoseCalibrator()
+    self.calibrated_pose: Pose | None = None
+    self.excessive_actuation_check = ExcessiveActuationCheck()
+    self.excessive_actuation = self.params.get("Offroad_ExcessiveActuation") is not None
+
+    # Setup sockets
+    self.pm = messaging.PubMaster(['selfdriveState', 'onroadEvents'] + ['iqState', 'iqOnroadEvents'])
+
+    self.gps_location_service = get_gps_location_service(self.params)
+    self.gps_packets = [self.gps_location_service]
+    self.sensor_packets = ["accelerometer", "gyroscope"]
+    self.camera_packets = ["roadCameraState", "driverCameraState", "wideRoadCameraState"]
+    if os.path.exists('/tmp/lite_hw'):
+      self.camera_packets.remove("driverCameraState")
+
+    # TODO: de-couple selfdrived with card/conflate on carState without introducing controls mismatches
+    self.car_state_sock = messaging.sub_sock('carState', timeout=20)
+
+    ignore = self.sensor_packets + self.gps_packets + ['alertDebug', 'lateralManeuverPlan', 'iqDriveModelData', 'iqNavState', 'vehicleParameters', 'driverAssistance', 'testJoystick']
+    if os.path.exists('/tmp/lite_hw'):
+      ignore += ['driverCameraState', 'driverMonitoringState']
+    if SIMULATION:
+      ignore += ['driverCameraState', 'managerState']
+    if REPLAY:
+      # no vipc in replay will make them ignored anyways
+      ignore += ['roadCameraState', 'wideRoadCameraState', 'userBookmark', 'iqPlan']
+    self.sm = messaging.SubMaster(['deviceState', 'pandaStates', 'peripheralState', 'modelV2', 'extrinsicsCalibration',
+                                   'carOutput', 'driverMonitoringState', 'longitudinalPlan', 'deviceMotion', 'lateralDelay',
+                                   'managerState', 'vehicleParameters', 'radarState', 'lateralTorqueParameters',
+                                   'controlsState', 'carControl', 'driverAssistance', 'alertDebug', 'userBookmark', 'audioFeedback',
+                                   'lateralManeuverPlan',
+                                   'iqDriveModelData', 'iqNavState', 'iqPlan', 'testJoystick'] + \
+                                   self.camera_packets + self.sensor_packets + self.gps_packets,
+                                  ignore_alive=ignore, ignore_avg_freq=ignore,
+                                  ignore_valid=ignore, frequency=int(1/DT_CTRL))
+
+    # read params
+    self.is_metric = self.params.get_bool("IsMetric")
+    self.is_ldw_enabled = self.params.get_bool("IsLdwEnabled")
+    self.disengage_on_accelerator = self.params.get_bool("DisengageOnAccelerator")
+    self.nav_exit_lane_change = self._read_nav_exit_lane_change()
+    self.model_download_pending = self.params.get("ModelManager_DownloadIndex") is not None
+
+    car_recognized = self.CP.brand != 'mock'
+
+    _cleanup_startup_params(self.CP, self.params)
+
+    self.CS_prev = car.CarState.new_message()
+    self.AM = AlertManager()
+    self.events = Events()
+
+    self.initialized = False
+    self.big_model_loading = False
+    self.big_model_active = False
+    self.big_model_failed = False
+    self.enabled = False
+    self.active = False
+    self.mismatch_counter = 0
+    self.cruise_mismatch_counter = 0
+    self.last_steering_pressed_frame = 0
+    self.distance_traveled = 0
+    self.last_functional_fan_frame = 0
+    self.events_prev = []
+    self.logged_comm_issue = None
+    self.not_running_prev = None
+    self.wide_cam_faulty = False
+    self.experimental_mode = False
+    self.personality = get_runtime_personality(self.params)
+    self.recalibrating_seen = False
+    self.state_machine = StateMachine()
+    self.rk = Ratekeeper(100, print_delay_threshold=None)
+
+    self.ignored_processes = set(NON_BLOCKING_PROCESSES)
+    nvme_expected = os.path.exists('/dev/nvme0n1') or (not os.path.isfile("/persist/comma/living-in-the-moment"))
+    if HARDWARE.get_device_type() == 'tici' and nvme_expected:
+      self.ignored_processes.add('loggerd')
+
+    # Determine startup event
+    is_remote = build_metadata.openpilot.comma_remote or build_metadata.openpilot.iqpilot_remote
+    self.startup_event = EventName.startup if is_remote and build_metadata.tested_channel else EventName.startupMaster
+    if HARDWARE.get_device_type() == 'mici':
+      self.startup_event = None
+    if not car_recognized:
+      self.startup_event = EventName.startupNoCar
+    elif car_recognized and self.CP.passive:
+      self.startup_event = EventName.startupNoControl
+    elif self.CP.secOcRequired and not self.CP.secOcKeyAvailable:
+      self.startup_event = EventName.startupNoSecOcKey
+
+    if not car_recognized:
+      self.events.add(EventName.carUnrecognized, static=True)
+      set_offroad_alert("Offroad_CarUnrecognized", True)
+    elif self.CP.passive:
+      self.events.add(EventName.dashcamMode, static=True)
+
+    self.events_iq = IQEvents()
+    self.events_iq_prev = []
+    self._cached_dm_event_names: tuple[int, ...] = ()
+    self._cached_plan_event_names: tuple[int, ...] = ()
+    self._cached_model_event_names: tuple[int, ...] = ()
+    self._cached_nav_event_names: tuple[int, ...] = ()
+
+    self.aol = SteeringAssistanceBehavior(self)
+    self.car_events_iq = VehicleEvents(self.CP, self.CP_IQ)
+
+    GapButtonActions.__init__(self, self.CP)
+
+  def _add_iq_event_names(self, event_names: tuple[int, ...] | list[int]) -> None:
+    for event_name in event_names:
+      self.events_iq.add(event_name)
+
+  def _add_event_names(self, event_names: tuple[int, ...] | list[int]) -> None:
+    for event_name in event_names:
+      self.events.add(event_name)
+
+  def _refresh_cached_plan_events(self) -> None:
+    if self.sm.updated['iqPlan']:
+      self._cached_plan_event_names = tuple(event.name.raw for event in self._get_longitudinal_plan_ext().events)
+
+  def _refresh_cached_dm_events(self) -> None:
+    if self.sm.updated['driverMonitoringState']:
+      self._cached_dm_event_names = tuple(event.name.raw for event in self.sm['driverMonitoringState'].events)
+
+  def _refresh_cached_model_events(self) -> None:
+    if not self.sm.updated['iqDriveModelData']:
+      return
+
+    model_data = self._get_model_data_ext()
+    model_events = []
+    if model_data.lateralEdgeBlock != custom.IQLateralEdgeBlock.none:
+      model_events.append(custom.IQOnroadEvent.EventName.lateralEdgeBlocked)
+
+    lane_turn_direction = model_data.turnSignalDirection
+    if lane_turn_direction == TurnDirection.turnLeft:
+      model_events.append(custom.IQOnroadEvent.EventName.modelTurnLeft)
+    elif lane_turn_direction == TurnDirection.turnRight:
+      model_events.append(custom.IQOnroadEvent.EventName.modelTurnRight)
+    self._cached_model_event_names = tuple(model_events)
+
+  def _read_nav_exit_lane_change(self) -> bool:
+    try:
+      return self.params.get_bool("NavExitLaneChange")
+    except Exception:
+      return False
+
+  def _refresh_cached_nav_events(self) -> None:
+    if not self.sm.updated['iqNavState']:
+      return
+
+    nav_state = self.sm['iqNavState']
+    nav_events: list[int] = []
+    if getattr(nav_state, 'active', False):
+      if getattr(nav_state, 'navTurnDesireDirection', 0) == 1:
+        nav_events.append(custom.IQOnroadEvent.EventName.navTurnLeft)
+      elif getattr(nav_state, 'navTurnDesireDirection', 0) == 2:
+        nav_events.append(custom.IQOnroadEvent.EventName.navTurnRight)
+      elif self.nav_exit_lane_change and \
+          getattr(nav_state, 'nextManeuverValid', False) and \
+          getattr(nav_state, 'nextManeuverType', custom.IQNavState.ManeuverType.none) == custom.IQNavState.ManeuverType.exit and \
+          0.0 < float(getattr(nav_state, 'nextManeuverDistance', 0.0)) <= NAV_EXIT_COMMIT_DISTANCE:
+        if getattr(nav_state, 'nextManeuverDirection', 0) == 1:
+          nav_events.append(custom.IQOnroadEvent.EventName.navExitLeft)
+        elif getattr(nav_state, 'nextManeuverDirection', 0) == 2:
+          nav_events.append(custom.IQOnroadEvent.EventName.navExitRight)
+
+    if getattr(nav_state, 'cameraValid', False):
+      nav_events.append(custom.IQOnroadEvent.EventName.speedCameraAhead)
+
+    self._cached_nav_event_names = tuple(nav_events)
+
+  def update_events(self, CS):
+    """Compute onroadEvents from carState"""
+
+    self.events.clear()
+    self.events_iq.clear()
+
+    if self.sm['controlsState'].lateralControlState.which() == 'debugState':
+      self.events.add(EventName.joystickDebug)
+      self.startup_event = None
+
+    if self.sm['deviceState'].egpuDockPresent or self.params.get_bool("IQEgpuEnabled") or self.big_model_active:
+      loading = self.params.get_bool("UsbGpuLoading")
+      self.big_model_loading = loading
+      if self.big_model_loading:
+        self.events.add(EventName.bigModelLoading)
+
+      big_active = self.params.get("UsbGpuActive")
+      dock_present = self.sm['deviceState'].egpuDockPresent
+      mac_active = self.params.get_bool("MacModelActive")
+      model_unavailable = big_active is True and self.sm.seen['modelV2'] and not self.sm.alive['modelV2']
+      # an explicit False before this session's first activation is just the selector arming
+      # (it pre-clears the param on startup); alerting on it pops "big model failed" on every
+      # return to the road until the latch warms up
+      big_failed = ((self.big_model_active and big_active is False) or model_unavailable
+                    or (self.big_model_active and not dock_present)) and not mac_active
+      if big_failed:
+        self.events.add(EventName.bigModelFailed)
+      self.big_model_failed = big_failed
+
+      # soft disable if the big model fails
+      if big_active:
+        self.big_model_active = True
+      if mac_active or (not self.enabled and not model_unavailable):
+        self.big_model_active = False
+
+    if self.sm.recv_frame['lateralManeuverPlan'] > 0:
+      self.events.add(EventName.lateralManeuver)
+      self.startup_event = None
+    elif self.sm.recv_frame['alertDebug'] > 0:
+      self.events.add(EventName.longitudinalManeuver)
+      self.startup_event = None
+
+    # Add startup event
+    if self.startup_event is not None:
+      self.events.add(self.startup_event)
+      self.startup_event = None
+
+    # Don't add any more events if not initialized
+    if not self.initialized:
+      self.events.add(EventName.selfdriveInitializing)
+      return
+
+    # Check for user bookmark press (bookmark button or end of LKAS button feedback)
+    if self.sm.updated['userBookmark']:
+      self.events.add(EventName.userBookmark)
+
+    if self.sm.updated['audioFeedback']:
+      self.events.add(EventName.audioFeedback)
+
+    # Don't add any more events while in dashcam mode
+    if self.CP.passive:
+      return
+
+    # Block resume if cruise never previously enabled
+    resume_pressed = any(be.type in (ButtonType.accelCruise, ButtonType.resumeCruise) for be in CS.buttonEvents)
+    volkswagen_op_long = self.CP.brand == "volkswagen" and self.CP.openpilotLongitudinalControl
+    if not self.CP.pcmCruise and CS.vCruise > 250 and resume_pressed and not volkswagen_op_long:
+      self.events.add(EventName.resumeBlocked)
+
+    if not self.CP.notCar:
+      self._refresh_cached_dm_events()
+      self._add_event_names(self._cached_dm_event_names)
+      self._refresh_cached_plan_events()
+      self._add_iq_event_names(self._cached_plan_event_names)
+
+    # Add car events, ignore if CAN isn't valid
+    if CS.canValid:
+      car_events = self.car_events.update(CS, self.CS_prev, self.sm['carControl']).to_msg()
+      self.events.add_from_msg(car_events)
+
+      car_events_iq = self.car_events_iq.update(CS, self.events)
+      self._add_iq_event_names(car_events_iq.names)
+
+      if self.CP.notCar:
+        # wait for everything to init first
+        if self.sm.frame > int(5. / DT_CTRL) and self.initialized:
+          # body always wants to enable
+          self.events.add(EventName.pcmEnable)
+
+      # Disable on rising edge of accelerator or brake. Also disable on brake when speed > 0
+      if (CS.gasPressed and not self.CS_prev.gasPressed and self.disengage_on_accelerator) or \
+        (CS.brakePressed and (not self.CS_prev.brakePressed or not CS.standstill)) or \
+        (CS.regenBraking and (not self.CS_prev.regenBraking or not CS.standstill)):
+        self.events.add(EventName.pedalPressed)
+
+    # Create events for temperature, disk space, and memory
+    if self.sm['deviceState'].thermalStatus >= ThermalStatus.red:
+      self.events.add(EventName.overheat)
+    if self.sm['deviceState'].freeSpacePercent < 7 and not SIMULATION:
+      self.events.add(EventName.outOfSpace)
+    if self.sm['deviceState'].memoryUsagePercent > 95 and not SIMULATION:
+      self.events.add(EventName.lowMemory)
+
+    # Alert if fan isn't spinning for 5 seconds
+    if self.sm['peripheralState'].pandaType != log.PandaState.PandaType.unknown:
+      if self.sm['peripheralState'].fanSpeedRpm < 500 and self.sm['deviceState'].fanSpeedPercentDesired > 50:
+        # allow enough time for the fan controller in the panda to recover from stalls
+        if (self.sm.frame - self.last_functional_fan_frame) * DT_CTRL > 15.0:
+          self.events.add(EventName.fanMalfunction)
+      else:
+        self.last_functional_fan_frame = self.sm.frame
+
+    # Handle calibration status
+    cal_status = self.sm['extrinsicsCalibration'].calStatus
+    if cal_status != log.ExtrinsicsCalibration.Status.calibrated:
+      if cal_status == log.ExtrinsicsCalibration.Status.uncalibrated:
+        self.events.add(EventName.calibrationIncomplete)
+      elif cal_status == log.ExtrinsicsCalibration.Status.recalibrating:
+        if not self.recalibrating_seen:
+          set_offroad_alert("Offroad_Recalibration", True)
+        self.recalibrating_seen = True
+        self.events.add(EventName.calibrationRecalibrating)
+      else:
+        self.events.add(EventName.calibrationInvalid)
+
+    # Lane departure warning
+    if self.is_ldw_enabled and self.sm.valid['driverAssistance']:
+      if self.sm['driverAssistance'].leftLaneDeparture or self.sm['driverAssistance'].rightLaneDeparture:
+        self.events.add(EventName.ldw)
+
+    # ******************************************************************************************
+    #  NOTE: To fork maintainers.
+    #  Disabling or nerfing safety features will get you and your users banned from our servers.
+    #  We recommend that you do not change these numbers from the defaults.
+    if self.sm.updated['extrinsicsCalibration']:
+      self.pose_calibrator.feed_live_calib(self.sm['extrinsicsCalibration'])
+    if self.sm.updated['deviceMotion']:
+      device_pose = Pose.from_live_pose(self.sm['deviceMotion'])
+      self.calibrated_pose = self.pose_calibrator.build_calibrated_pose(device_pose)
+
+    if self.calibrated_pose is not None:
+      excessive_actuation = self.excessive_actuation_check.update(self.sm, CS, self.calibrated_pose)
+      if not self.excessive_actuation and excessive_actuation is not None:
+        set_offroad_alert("Offroad_ExcessiveActuation", True, extra_text=str(excessive_actuation))
+        self.excessive_actuation = True
+
+    if self.excessive_actuation:
+      self.events.add(EventName.excessiveActuation)
+    # ******************************************************************************************
+
+    # Handle lane change
+    if self.sm['modelV2'].meta.laneChangeState == LaneChangeState.preLaneChange:
+      direction = self.sm['modelV2'].meta.laneChangeDirection
+      if (CS.leftBlindspot and direction == LaneChangeDirection.left) or \
+         (CS.rightBlindspot and direction == LaneChangeDirection.right):
+        self.events.add(EventName.laneChangeBlocked)
+      else:
+        if direction == LaneChangeDirection.left:
+          self.events.add(EventName.preLaneChangeLeft)
+        else:
+          self.events.add(EventName.preLaneChangeRight)
+    elif self.sm['modelV2'].meta.laneChangeState in (LaneChangeState.laneChangeStarting,
+                                                    LaneChangeState.laneChangeFinishing):
+      self.events.add(EventName.laneChange)
+
+    # Handle lane turn
+    self._refresh_cached_model_events()
+    self._add_iq_event_names(self._cached_model_event_names)
+
+    self._refresh_cached_nav_events()
+    self._add_iq_event_names(self._cached_nav_event_names)
+
+    for i, pandaState in enumerate(self.sm['pandaStates']):
+      # All pandas must match the list of safetyConfigs, and if outside this list, must be silent or noOutput
+      if i < len(self.CP.safetyConfigs):
+        safety_mismatch = pandaState.safetyModel != self.CP.safetyConfigs[i].safetyModel or \
+                          pandaState.safetyParam != self.CP.safetyConfigs[i].safetyParam or \
+                          pandaState.alternativeExperience != self.CP.alternativeExperience
+      else:
+        safety_mismatch = pandaState.safetyModel not in IGNORED_SAFETY_MODES
+
+      # safety mismatch allows some time for pandad to set the safety mode and publish it back from panda
+      if (safety_mismatch and self.sm.frame*DT_CTRL > 10.) or \
+         pandaState.safetyRxChecksInvalid or \
+         self.mismatch_counter >= 200:
+        self.events.add(EventName.controlsMismatch)
+
+      if log.PandaState.FaultType.relayMalfunction in pandaState.faults:
+        self.events.add(EventName.relayMalfunction)
+
+    # Handle HW and system malfunctions
+    # Order is very intentional here. Be careful when modifying this.
+    # All events here should at least have NO_ENTRY and SOFT_DISABLE.
+    num_events = len(self.events)
+
+    not_running = {p.name for p in self.sm['managerState'].processes if not p.running and p.shouldBeRunning}
+    if self.sm.recv_frame['managerState'] and len(not_running):
+      if not_running != self.not_running_prev:
+        cloudlog.event("process_not_running", not_running=not_running, error=True)
+      self.not_running_prev = not_running
+    if self.sm.recv_frame['managerState'] and (not_running - self.ignored_processes):
+      self.events.add(EventName.processNotRunning)
+      if 'iqmodeld' in not_running and self.model_download_pending:
+        self.events_iq.add(custom.IQOnroadEvent.EventName.modelUpdating)
+    else:
+      if not SIMULATION and not self.rk.lagging:
+        if not self.sm.all_alive(self.camera_packets):
+          self.events.add(EventName.cameraMalfunction)
+        elif not self.sm.all_freq_ok(self.camera_packets):
+          self.events.add(EventName.cameraFrameRate)
+
+    if self.sm.frame < WIDE_CAM_FAULTY_ALERT_FRAMES:
+      if self.sm.frame % int(1. / DT_CTRL) == 0:
+        self.wide_cam_faulty = self.params.get_bool("WideCamFaulty")
+      if self.wide_cam_faulty:
+        self.events_iq.add(custom.IQOnroadEvent.EventName.wideCamFaulty)
+
+    if not REPLAY and self.rk.lagging:
+      log_issue_limited(
+        "selfdrived_lagging",
+        "lag",
+        f"selfdrived lagging avg_dt={self.rk.avg_dt.get_average() * 1000:.2f}ms frame={self.sm.frame}",
+        interval_sec=1.0,
+      )
+      self.events.add(EventName.selfdrivedLagging)
+    if self.sm['radarState'].radarErrors.canError:
+      self.events.add(EventName.canError)
+    elif self.sm['radarState'].radarErrors.radarUnavailableTemporary:
+      self.events.add(EventName.radarTempUnavailable)
+    elif any(self.sm['radarState'].radarErrors.to_dict().values()):
+      self.events.add(EventName.radarFault)
+    if not self.sm.valid['pandaStates']:
+      self.events.add(EventName.usbError)
+    if CS.canTimeout:
+      self.events.add(EventName.canBusMissing)
+    elif not CS.canValid:
+      self.events.add(EventName.canError)
+
+    # generic catch-all. ideally, a more specific event should be added above instead
+    has_disable_events = self.events.contains(ET.NO_ENTRY) and (self.events.contains(ET.SOFT_DISABLE) or self.events.contains(ET.IMMEDIATE_DISABLE))
+    no_system_errors = (not has_disable_events) or (len(self.events) == num_events)
+    logs = self._comm_issue_logs()
+    has_not_alive = len(logs['not_alive']) > 0
+    has_not_freq = len(logs['not_freq_ok']) > 0
+    comm_issue_state = (tuple(logs['not_alive']), tuple(logs['not_freq_ok']))
+    if (has_not_alive or has_not_freq) and no_system_errors:
+      if has_not_alive:
+        self.events.add(EventName.commIssue)
+      else:
+        self.events.add(EventName.commIssueAvgFreq)
+
+      if comm_issue_state != self.logged_comm_issue:
+        cloudlog.event("commIssue", error=True, **logs)
+        self.logged_comm_issue = comm_issue_state
+    else:
+      self.logged_comm_issue = None
+
+    if not self.CP.notCar:
+      if not self.sm['deviceMotion'].posenetOK:
+        self.events.add(EventName.posenetInvalid)
+      if not self.sm['vehicleParameters'].valid and cal_status == log.ExtrinsicsCalibration.Status.calibrated and not TESTING_CLOSET and (not SIMULATION or REPLAY):
+        self.events.add(EventName.paramsdTemporaryError)
+
+    # conservative HW alert. if the data or frequency are off, locationd will throw an error
+    if any((self.sm.frame - self.sm.recv_frame[s])*DT_CTRL > 10. for s in self.sensor_packets):
+      self.events.add(EventName.sensorDataInvalid)
+
+    if not REPLAY:
+      # Check for mismatch between openpilot and car's PCM
+      cruise_mismatch = CS.cruiseState.enabled and (not self.enabled or not self.CP.pcmCruise)
+      self.cruise_mismatch_counter = self.cruise_mismatch_counter + 1 if cruise_mismatch else 0
+      if self.cruise_mismatch_counter > int(6. / DT_CTRL):
+        self.events.add(EventName.cruiseMismatch)
+
+    # Send a "steering required alert" if saturation count has reached the limit
+    if CS.steeringPressed:
+      self.last_steering_pressed_frame = self.sm.frame
+    recent_steer_pressed = (self.sm.frame - self.last_steering_pressed_frame)*DT_CTRL < 2.0
+    controlstate = self.sm['controlsState']
+    lac = getattr(controlstate.lateralControlState, controlstate.lateralControlState.which())
+    if lac.active and not recent_steer_pressed and not self.CP.notCar:
+      clipped_speed = max(CS.vEgo, 0.3)
+      actual_lateral_accel = controlstate.curvature * (clipped_speed**2)
+      desired_lateral_accel = self.sm['modelV2'].action.desiredCurvature * (clipped_speed**2)
+      undershooting = abs(desired_lateral_accel) / abs(1e-3 + actual_lateral_accel) > 1.2
+      turning = abs(desired_lateral_accel) > 1.0
+      # TODO: lac.saturated includes speed and other checks, should be pulled out
+      if undershooting and turning and lac.saturated:
+        self.events.add(EventName.steerSaturated)
+
+    # Check for FCW
+    stock_long_is_braking = self.enabled and not self.CP.openpilotLongitudinalControl and CS.aEgo < -1.25
+    model_fcw = self.sm['modelV2'].meta.hardBrakePredicted and not CS.brakePressed and not stock_long_is_braking
+    planner_fcw = self.sm['longitudinalPlan'].fcw and self.enabled
+    if (planner_fcw or model_fcw) and not self.CP.notCar:
+      self.events.add(EventName.fcw)
+
+    # GPS checks
+    gps_ok = self.sm.recv_frame[self.gps_location_service] > 0 and (self.sm.frame - self.sm.recv_frame[self.gps_location_service]) * DT_CTRL < 2.0
+    if not gps_ok and self.sm['deviceMotion'].inputsOK and (self.distance_traveled > 1500):
+      self.events.add(EventName.noGps)
+    if gps_ok:
+      self.distance_traveled = 0
+    self.distance_traveled += abs(CS.vEgo) * DT_CTRL
+
+    # TODO: fix simulator
+    if not SIMULATION or REPLAY:
+      if self.sm['modelV2'].frameDropPerc > 20:
+        log_issue_limited(
+          "modeld_frame_drop",
+          "lag",
+          f"modeld frame drops={self.sm['modelV2'].frameDropPerc:.1f}% frame={self.sm.frame}",
+          interval_sec=1.0,
+        )
+        self.events.add(EventName.modeldLagging)
+
+    # Mute canBusMissing in Park so standby guidance suppression does not create a false alarm.
+    if CS.gearShifter == car.CarState.GearShifter.park and self.aol.enabled:
+      self.events.remove(EventName.canBusMissing)
+
+    GapButtonActions.update(self, CS, self.events_iq, self.experimental_mode)
+
+    # decrement personality on distance button press
+    if self.CP.openpilotLongitudinalControl:
+      if any(not be.pressed and be.type == ButtonType.gapAdjustCruise for be in CS.buttonEvents):
+        if not self.experimental_mode_switched:
+          self.personality = (self.personality - 1) % 3
+          self.params.put_nonblocking('LongitudinalPersonality', self.personality)
+          self.events.add(EventName.personalityChanged)
+        self.experimental_mode_switched = False
+
+  def _get_model_data_ext(self):
+    return self.sm['iqDriveModelData']
+
+  def _get_longitudinal_plan_ext(self):
+    return self.sm['iqPlan']
+
+  def _comm_issue_logs(self):
+    ignore_freq = getattr(self.sm, "ignore_average_freq", [])
+    return {
+      'invalid': [s for s, valid in self.sm.valid.items() if not valid and s not in self.sm.ignore_valid],
+      'not_alive': [s for s, alive in self.sm.alive.items() if not alive and s not in self.sm.ignore_alive],
+      'not_freq_ok': [s for s, freq_ok in self.sm.freq_ok.items() if not freq_ok and s not in ignore_freq],
+    }
+
+  def data_sample(self):
+    started = time.monotonic()
+    _car_state = messaging.recv_one(self.car_state_sock)
+    car_state_wait_ms = (time.monotonic() - started) * 1000
+
+    started = time.monotonic()
+    CS = _car_state.carState if _car_state else self.CS_prev
+
+    self.sm.update(0)
+    sm_update_ms = (time.monotonic() - started) * 1000
+
+    if not self.initialized:
+      all_valid = CS.canValid and self.sm.all_checks()
+      timed_out = self.sm.frame * DT_CTRL > 6.
+      if all_valid or timed_out or (SIMULATION and not REPLAY):
+        available_streams = VisionIpcClient.available_streams("camerad", block=False)
+        if VisionStreamType.VISION_STREAM_ROAD not in available_streams:
+          self.sm.ignore_alive.append('roadCameraState')
+          self.sm.ignore_valid.append('roadCameraState')
+        if VisionStreamType.VISION_STREAM_WIDE_ROAD not in available_streams:
+          self.sm.ignore_alive.append('wideRoadCameraState')
+          self.sm.ignore_valid.append('wideRoadCameraState')
+
+        if REPLAY and any(ps.controlsAllowed for ps in self.sm['pandaStates']):
+          self.state_machine.state = State.enabled
+
+        self.initialized = True
+        comm_issue_logs = self._comm_issue_logs()
+        cloudlog.event(
+          "selfdrived.initialized",
+          dt=self.sm.frame*DT_CTRL,
+          timeout=timed_out,
+          canValid=CS.canValid,
+          invalid=comm_issue_logs['invalid'],
+          not_alive=comm_issue_logs['not_alive'],
+          not_freq_ok=comm_issue_logs['not_freq_ok'],
+          error=True,
+        )
+
+    # When the panda and selfdrived do not agree on controls_allowed
+    # we want to disengage openpilot. However the status from the panda goes through
+    # another socket other than the CAN messages and one can arrive earlier than the other.
+    # Therefore we allow a mismatch for two samples, then we trigger the disengagement.
+    if not self.enabled:
+      self.mismatch_counter = 0
+
+    # All pandas not in silent mode must have controlsAllowed when openpilot is enabled.
+    if self.enabled and any(not ps.controlsAllowed for ps in self.sm['pandaStates']
+           if ps.safetyModel not in IGNORED_SAFETY_MODES):
+      self.mismatch_counter += 1
+
+    sample_total_ms = car_state_wait_ms + sm_update_ms
+    if sample_total_ms > 7.0 or car_state_wait_ms > 5.0 or sm_update_ms > 2.0:
+      log_issue_limited(
+        "selfdrived_sample_breakdown",
+        "lag",
+        f"selfdrived sample slow total_ms={sample_total_ms:.2f} car_state_wait_ms={car_state_wait_ms:.2f} "
+        f"sm_update_ms={sm_update_ms:.2f}",
+        interval_sec=1.0,
+      )
+
+    return CS
+
+  def update_alerts(self, CS):
+    clear_event_types = set()
+    if ET.WARNING not in self.state_machine.current_alert_types:
+      clear_event_types.add(ET.WARNING)
+    if self.enabled:
+      clear_event_types.add(ET.NO_ENTRY)
+
+    pers = LONGITUDINAL_PERSONALITY_MAP[self.personality]
+    callback_args = [self.CP, CS, self.sm, self.is_metric,
+                     self.state_machine.soft_disable_timer, pers]
+
+    alerts = self.events.create_alerts(self.state_machine.current_alert_types, callback_args)
+    alerts_iq = self.events_iq.create_alerts(self.state_machine.current_alert_types, callback_args)
+
+    self.AM.add_many(self.sm.frame, alerts + alerts_iq)
+    self.AM.process_alerts(self.sm.frame, clear_event_types)
+
+  def publish_selfdriveState(self, CS):
+    # selfdriveState
+    ss_msg = messaging.new_message('selfdriveState')
+    ss_msg.valid = True
+    ss = ss_msg.selfdriveState
+    ss.enabled = self.enabled
+    ss.active = self.active
+    ss.state = self.state_machine.state
+    ss.engageable = not self.events.contains(ET.NO_ENTRY)
+    ss.experimentalMode = self.experimental_mode
+    ss.personality = self.personality
+
+    ss.alertText1 = self.AM.current_alert.alert_text_1
+    ss.alertText2 = self.AM.current_alert.alert_text_2
+    ss.alertSize = self.AM.current_alert.alert_size
+    ss.alertStatus = self.AM.current_alert.alert_status
+    ss.alertType = self.AM.current_alert.alert_type
+    ss.alertSound = self.AM.current_alert.audible_alert
+    ss.alertHudVisual = self.AM.current_alert.visual_alert
+
+    self.pm.send('selfdriveState', ss_msg)
+
+    # onroadEvents - logged every second or on change
+    if (self.sm.frame % int(1. / DT_CTRL) == 0) or (self.events.names != self.events_prev):
+      ce_send = messaging.new_message('onroadEvents', len(self.events))
+      ce_send.valid = True
+      ce_send.onroadEvents = self.events.to_msg()
+      self.pm.send('onroadEvents', ce_send)
+    self.events_prev = self.events.names.copy()
+
+    # iqState
+    iq_state_msg = messaging.new_message('iqState')
+    iq_state_msg.valid = True
+    iq_state = iq_state_msg.iqState
+    aol = iq_state.aol
+    aol.state = self.aol.state_machine.state
+    aol.enabled = self.aol.enabled
+    aol.active = self.aol.active
+    aol.available = self.aol.enabled_toggle
+    self.pm.send('iqState', iq_state_msg)
+
+    # iqOnroadEvents - logged every second or on change
+    if (self.sm.frame % int(1. / DT_CTRL) == 0) or (self.events_iq.names != self.events_iq_prev):
+      iq_events_msg = messaging.new_message('iqOnroadEvents')
+      iq_events_msg.valid = True
+      iq_events_msg.iqOnroadEvents.events = self.events_iq.to_msg()
+      self.pm.send('iqOnroadEvents', iq_events_msg)
+    self.events_iq_prev = self.events_iq.names.copy()
+
+  def step(self):
+    started = time.monotonic()
+    checkpoint = started
+
+    CS = self.data_sample()
+    sample_ms = (time.monotonic() - checkpoint) * 1000
+    checkpoint = time.monotonic()
+
+    self.update_events(CS)
+    events_ms = (time.monotonic() - checkpoint) * 1000
+    checkpoint = time.monotonic()
+
+    if not self.CP.passive and self.initialized:
+      self.enabled, self.active = self.state_machine.update(self.events)
+    state_ms = (time.monotonic() - checkpoint) * 1000
+    checkpoint = time.monotonic()
+
+    if not self.CP.notCar:
+      self.aol.update(CS)
+    aol_ms = (time.monotonic() - checkpoint) * 1000
+    checkpoint = time.monotonic()
+
+    self.update_alerts(CS)
+    alerts_ms = (time.monotonic() - checkpoint) * 1000
+    checkpoint = time.monotonic()
+
+    self.publish_selfdriveState(CS)
+    publish_ms = (time.monotonic() - checkpoint) * 1000
+
+    self.CS_prev = CS
+
+    total_ms = (time.monotonic() - started) * 1000
+    if total_ms > 8.0 or events_ms > 4.0 or publish_ms > 3.0:
+      log_issue_limited(
+        "selfdrived_step_slow",
+        "lag",
+        f"selfdrived step slow total_ms={total_ms:.2f} sample_ms={sample_ms:.2f} events_ms={events_ms:.2f} "
+        f"state_ms={state_ms:.2f} aol_ms={aol_ms:.2f} alerts_ms={alerts_ms:.2f} publish_ms={publish_ms:.2f} "
+        f"nav_active={getattr(self.sm['iqNavState'], 'active', False)}",
+        interval_sec=1.0,
+      )
+
+  def params_thread(self, evt):
+    config_background_thread()
+    while not evt.is_set():
+      self.is_metric = self.params.get_bool("IsMetric")
+      self.is_ldw_enabled = self.params.get_bool("IsLdwEnabled")
+      self.disengage_on_accelerator = self.params.get_bool("DisengageOnAccelerator")
+      self.experimental_mode = self.params.get_bool("ExperimentalMode") and self.CP.openpilotLongitudinalControl
+      self.personality = get_runtime_personality(self.params)
+      self.nav_exit_lane_change = self._read_nav_exit_lane_change()
+      self.model_download_pending = self.params.get("ModelManager_DownloadIndex") is not None
+
+      self.aol.read_params()
+      time.sleep(0.1)
+
+  def run(self):
+    e = threading.Event()
+    t = threading.Thread(target=self.params_thread, args=(e, ))
+    try:
+      t.start()
+      while True:
+        self.step()
+        self.rk.monitor_time()
+    finally:
+      e.set()
+      t.join()
+
+
+def main():
+  config_realtime_process(4, Priority.CTRL_HIGH)
+  lock_memory()
+  s = SelfdriveD()
+  s.run()
+
+if __name__ == "__main__":
+  main()

iqpilot/selfdrive/selfdrived/state.py

@@ -0,0 +1,98 @@
+from iqpilot.cereal import log
+from iqpilot.selfdrive.selfdrived.events import Events, ET
+from iqpilot.common.realtime import DT_CTRL
+
+State = log.SelfdriveState.OpenpilotState
+
+SOFT_DISABLE_TIME = 3  # seconds
+ACTIVE_STATES = (State.enabled, State.softDisabling, State.overriding)
+ENABLED_STATES = (State.preEnabled, *ACTIVE_STATES)
+
+class StateMachine:
+  def __init__(self):
+    self.current_alert_types = [ET.PERMANENT]
+    self.state = State.disabled
+    self.soft_disable_timer = 0
+
+  def update(self, events: Events):
+    # decrement the soft disable timer at every step, as it's reset on
+    # entrance in SOFT_DISABLING state
+    self.soft_disable_timer = max(0, self.soft_disable_timer - 1)
+
+    self.current_alert_types = [ET.PERMANENT]
+
+    # ENABLED, SOFT DISABLING, PRE ENABLING, OVERRIDING
+    if self.state != State.disabled:
+      # user and immediate disable always have priority in a non-disabled state
+      if events.contains(ET.USER_DISABLE):
+        self.state = State.disabled
+        self.current_alert_types.append(ET.USER_DISABLE)
+
+      elif events.contains(ET.IMMEDIATE_DISABLE):
+        self.state = State.disabled
+        self.current_alert_types.append(ET.IMMEDIATE_DISABLE)
+
+      else:
+        # ENABLED
+        if self.state == State.enabled:
+          if events.contains(ET.SOFT_DISABLE):
+            self.state = State.softDisabling
+            self.soft_disable_timer = int(SOFT_DISABLE_TIME / DT_CTRL)
+            self.current_alert_types.append(ET.SOFT_DISABLE)
+
+          elif events.contains(ET.OVERRIDE_LATERAL) or events.contains(ET.OVERRIDE_LONGITUDINAL):
+            self.state = State.overriding
+            self.current_alert_types += [ET.OVERRIDE_LATERAL, ET.OVERRIDE_LONGITUDINAL]
+
+        # SOFT DISABLING
+        elif self.state == State.softDisabling:
+          if not events.contains(ET.SOFT_DISABLE):
+            # no more soft disabling condition, so go back to ENABLED
+            self.state = State.enabled
+
+          elif self.soft_disable_timer > 0:
+            self.current_alert_types.append(ET.SOFT_DISABLE)
+
+          elif self.soft_disable_timer <= 0:
+            self.state = State.disabled
+
+        # PRE ENABLING
+        elif self.state == State.preEnabled:
+          if not events.contains(ET.PRE_ENABLE):
+            self.state = State.enabled
+          else:
+            self.current_alert_types.append(ET.PRE_ENABLE)
+
+        # OVERRIDING
+        elif self.state == State.overriding:
+          if events.contains(ET.SOFT_DISABLE):
+            self.state = State.softDisabling
+            self.soft_disable_timer = int(SOFT_DISABLE_TIME / DT_CTRL)
+            self.current_alert_types.append(ET.SOFT_DISABLE)
+          elif not (events.contains(ET.OVERRIDE_LATERAL) or events.contains(ET.OVERRIDE_LONGITUDINAL)):
+            self.state = State.enabled
+          else:
+            self.current_alert_types += [ET.OVERRIDE_LATERAL, ET.OVERRIDE_LONGITUDINAL]
+
+    # DISABLED
+    elif self.state == State.disabled:
+      if events.contains(ET.ENABLE):
+        if events.contains(ET.NO_ENTRY):
+          self.current_alert_types.append(ET.NO_ENTRY)
+
+        else:
+          if events.contains(ET.PRE_ENABLE):
+            self.state = State.preEnabled
+          elif events.contains(ET.OVERRIDE_LATERAL) or events.contains(ET.OVERRIDE_LONGITUDINAL):
+            self.state = State.overriding
+          else:
+            self.state = State.enabled
+          self.current_alert_types.append(ET.ENABLE)
+
+    # Check if openpilot is engaged and actuators are enabled
+    enabled = self.state in ENABLED_STATES
+    active = self.state in ACTIVE_STATES
+    if active:
+      self.current_alert_types.append(ET.WARNING)
+    return enabled, active
+

iqpilot/selfdrive/selfdrived/tests/test_alertmanager.py

@@ -0,0 +1,60 @@
+import random
+
+from iqpilot.selfdrive.selfdrived.events import Alert, EVENTS
+from iqpilot.selfdrive.selfdrived.alertmanager import AlertManager
+
+from iqpilot.common.atlas_alerts import NULL_ALERT as EmptyAlert
+
+
+class TestAlertManager:
+
+  def test_duration(self):
+    """
+      Enforce that an alert lasts for max(alert duration, duration the alert is added)
+    """
+    for duration in range(1, 100):
+      alert = None
+      while not isinstance(alert, Alert):
+        event = random.choice([e for e in EVENTS.values() if len(e)])
+        alert = random.choice(list(event.values()))
+
+      alert.duration = duration
+
+      # check two cases:
+      # - alert is added to AM for <= the alert's duration
+      # - alert is added to AM for > alert's duration
+      for greater in (True, False):
+        if greater:
+          add_duration = duration + random.randint(1, 10)
+        else:
+          add_duration = random.randint(1, duration)
+        show_duration = max(duration, add_duration)
+
+        AM = AlertManager()
+        for frame in range(duration+10):
+          if frame < add_duration:
+            AM.add_many(frame, [alert, ])
+          AM.process_alerts(frame, set())
+
+          shown = AM.current_alert != EmptyAlert
+          should_show = frame <= show_duration
+          assert shown == should_show, f"{frame=} {add_duration=} {duration=}"
+
+      # check one case:
+      # - if alert is re-added to AM before it ends the duration is extended
+      if duration > 1:
+        AM = AlertManager()
+        show_duration = duration * 2
+        for frame in range(duration * 2 + 10):
+          if frame == 0:
+            AM.add_many(frame, [alert, ])
+
+          if frame == duration:
+            # add alert one frame before it ends
+            assert AM.current_alert == alert
+            AM.add_many(frame, [alert, ])
+          AM.process_alerts(frame, set())
+
+          shown = AM.current_alert != EmptyAlert
+          should_show = frame <= show_duration
+          assert shown == should_show, f"{frame=} {duration=}"

iqpilot/selfdrive/selfdrived/tests/test_alerts.py

@@ -0,0 +1,163 @@
+import copy
+import json
+import os
+import random
+from PIL import Image, ImageDraw, ImageFont
+
+from iqpilot.cereal import log, car, custom
+from iqpilot.cereal.messaging import SubMaster
+from iqpilot.common.basedir import BASEDIR
+from iqpilot.common.params import Params
+from iqpilot.selfdrive.selfdrived.events import Alert, EVENTS, ET
+from iqpilot.selfdrive.selfdrived.iq_events import EVENTS_IQ
+from iqpilot.selfdrive.selfdrived.events import invalid_lkas_setting_alert, invalid_lkas_setting_no_entry_alert
+from iqpilot.selfdrive.selfdrived.alertmanager import set_offroad_alert
+from iqpilot.selfdrive.test.process_replay.process_replay import CONFIGS
+
+AlertSize = log.SelfdriveState.AlertSize
+
+OFFROAD_ALERTS_PATH = os.path.join(BASEDIR, "iqpilot/selfdrive/selfdrived/alerts_offroad.json")
+
+# TODO: add callback alerts
+ALERTS = []
+for event_types in EVENTS.values():
+  for alert in event_types.values():
+    ALERTS.append(alert)
+
+
+class TestAlerts:
+
+  def test_wrong_gear_alerts_are_silent_and_invisible(self):
+    wrong_gear_alerts = (EVENTS[log.OnroadEvent.EventName.wrongGear][ET.SOFT_DISABLE],
+                         EVENTS[log.OnroadEvent.EventName.wrongGear][ET.NO_ENTRY],
+                         EVENTS_IQ[custom.IQOnroadEvent.EventName.gearNotDriveSilent][ET.NO_ENTRY])
+
+    for alert in wrong_gear_alerts:
+      assert alert.alert_size == AlertSize.none
+      assert alert.audible_alert == car.CarControl.HUDControl.AudibleAlert.none
+      assert alert.alert_text_1 == ""
+      assert alert.alert_text_2 == ""
+
+  @classmethod
+  def setup_class(cls):
+    with open(OFFROAD_ALERTS_PATH) as f:
+      cls.offroad_alerts = json.loads(f.read())
+
+      # Create fake objects for callback
+      cls.CS = car.CarState.new_message()
+      cls.CP = car.CarParams.new_message()
+      cfg = [c for c in CONFIGS if c.proc_name == 'selfdrived'][0]
+      cls.sm = SubMaster(cfg.pubs)
+
+  def test_events_defined(self):
+    # Ensure all events in capnp schema are defined in events.py
+    events = log.OnroadEvent.EventName.schema.enumerants
+
+    for name, e in events.items():
+      if not name.endswith("DEPRECATED") and not name.startswith("eventReserved"):
+        fail_msg = f"{name} @{e} not in EVENTS"
+        assert e in EVENTS.keys(), fail_msg
+
+  # ensure alert text doesn't exceed allowed width
+  def test_alert_text_length(self):
+    font_path = os.path.join(BASEDIR, "iqpilot/selfdrive/assets/fonts")
+    regular_font_path = os.path.join(font_path, "Inter-SemiBold.ttf")
+    bold_font_path = os.path.join(font_path, "Inter-Bold.ttf")
+    semibold_font_path = os.path.join(font_path, "Inter-SemiBold.ttf")
+
+    max_text_width = 2160 - 300  # full screen width is usable, minus sidebar
+    draw = ImageDraw.Draw(Image.new('RGB', (0, 0)))
+
+    fonts = {
+      AlertSize.small: [ImageFont.truetype(semibold_font_path, 74)],
+      AlertSize.mid: [ImageFont.truetype(bold_font_path, 88),
+                      ImageFont.truetype(regular_font_path, 66)],
+    }
+
+    for alert in ALERTS:
+      if not isinstance(alert, Alert):
+        alert = alert(self.CP, self.CS, self.sm, metric=False, soft_disable_time=100, personality=log.LongitudinalPersonality.standard)
+
+      # for full size alerts, both text fields wrap the text,
+      # so it's unlikely that they  would go past the max width
+      if alert.alert_size in (AlertSize.none, AlertSize.full):
+        continue
+
+      for i, txt in enumerate([alert.alert_text_1, alert.alert_text_2]):
+        if i >= len(fonts[alert.alert_size]):
+          break
+
+        font = fonts[alert.alert_size][i]
+        left, _, right, _ = draw.textbbox((0, 0), txt, font)
+        width = right - left
+        msg = f"type: {alert.alert_type} msg: {txt}"
+        assert width <= max_text_width, msg
+
+  def test_alert_sanity_check(self):
+    for event_types in EVENTS.values():
+      for event_type, a in event_types.items():
+        # TODO: add callback alerts
+        if not isinstance(a, Alert):
+          continue
+
+        if a.alert_size == AlertSize.none:
+          assert len(a.alert_text_1) == 0
+          assert len(a.alert_text_2) == 0
+        elif a.alert_size == AlertSize.small:
+          assert len(a.alert_text_1) > 0
+          assert len(a.alert_text_2) == 0
+        elif a.alert_size == AlertSize.mid:
+          assert len(a.alert_text_1) > 0
+          assert len(a.alert_text_2) > 0
+        else:
+          assert len(a.alert_text_1) > 0
+
+        assert a.duration >= 0.
+
+        if event_type not in (ET.WARNING, ET.PERMANENT, ET.PRE_ENABLE):
+          assert a.creation_delay == 0.
+
+  def test_offroad_alerts(self):
+    params = Params()
+    for a in self.offroad_alerts:
+      # set the alert
+      alert = copy.copy(self.offroad_alerts[a])
+      set_offroad_alert(a, True)
+      alert['extra'] = ''
+      assert alert == params.get(a)
+
+      # then delete it
+      set_offroad_alert(a, False)
+      assert params.get(a) is None
+
+  def test_offroad_alerts_extra_text(self):
+    params = Params()
+    for i in range(50):
+      # set the alert
+      a = random.choice(list(self.offroad_alerts))
+      alert = self.offroad_alerts[a]
+      set_offroad_alert(a, True, extra_text="a"*i)
+
+      written_alert = params.get(a)
+      assert "a"*i == written_alert['extra']
+      assert alert["text"] == written_alert['text']
+
+  def test_invalid_lkas_setting_alert_tesla_dashcam_mode(self):
+    self.CP.brand = "tesla"
+    alert = invalid_lkas_setting_alert(self.CP, self.CS, self.sm, metric=False, soft_disable_time=100, personality=log.LongitudinalPersonality.standard)
+    no_entry = invalid_lkas_setting_no_entry_alert(self.CP, self.CS, self.sm, metric=False, soft_disable_time=100, personality=log.LongitudinalPersonality.standard)
+
+    assert alert.alert_text_1 == "Dashcam Mode"
+    assert alert.alert_text_2 == "FSD / Autosteer is active"
+    assert no_entry.alert_text_1 == "Dashcam Mode"
+    assert no_entry.alert_text_2 == "FSD / Autosteer is active"
+
+  def test_invalid_lkas_setting_alert_non_tesla_unchanged(self):
+    self.CP.brand = "mazda"
+    alert = invalid_lkas_setting_alert(self.CP, self.CS, self.sm, metric=False, soft_disable_time=100, personality=log.LongitudinalPersonality.standard)
+    no_entry = invalid_lkas_setting_no_entry_alert(self.CP, self.CS, self.sm, metric=False, soft_disable_time=100, personality=log.LongitudinalPersonality.standard)
+
+    assert alert.alert_text_1 == "Invalid LKAS setting"
+    assert alert.alert_text_2 == "Enable your car's LKAS to engage"
+    assert no_entry.alert_text_1 == "IQ.Pilot Unavailable"
+    assert no_entry.alert_text_2 == "Invalid LKAS setting"

iqpilot/selfdrive/selfdrived/tests/test_iq_camera_alerts.py

@@ -0,0 +1,79 @@
+import copy
+from types import SimpleNamespace
+
+from iqpilot.cereal import car, custom, log
+from iqpilot.common.atlas_alerts import HardDisableCard, Tags as ET, Tier as Priority
+from iqpilot.selfdrive.selfdrived.alertmanager import AlertManager
+from iqpilot.selfdrive.selfdrived.events import EVENTS
+from iqpilot.selfdrive.selfdrived import iq_events
+
+
+def alert(camera_type, *, report_id="", chime=False, distance=300.0):
+  nav = SimpleNamespace(
+    cameraType=camera_type,
+    cameraDistance=distance,
+    cameraSpeedLimit=25.0,
+    cameraAlertId=report_id,
+    cameraChime=chime,
+  )
+  return iq_events.speed_camera_alert(None, None, {"iqNavState": nav}, False, 0, None)
+
+
+def test_existing_camera_audio_is_unchanged():
+  result = alert(custom.IQNavState.CameraType.fixedSpeed)
+  assert result.audible_alert == car.CarControl.HUDControl.AudibleAlert.prompt
+
+
+def test_police_visual_mode_is_silent():
+  result = alert(custom.IQNavState.CameraType.police, report_id="visual", chime=False)
+  assert result.audible_alert == car.CarControl.HUDControl.AudibleAlert.none
+
+
+def test_police_chime_is_deduplicated_by_report():
+  iq_events._POLICE_CHIMED_IDS.clear()
+  first = alert(custom.IQNavState.CameraType.police, report_id="police-a", chime=True)
+  second = alert(custom.IQNavState.CameraType.police, report_id="police-a", chime=True)
+  assert first.audible_alert == car.CarControl.HUDControl.AudibleAlert.prompt
+  assert second.audible_alert == car.CarControl.HUDControl.AudibleAlert.none
+
+
+def test_alpr_wording_uses_configured_region(monkeypatch):
+  monkeypatch.setattr(iq_events, "_configured_country_code", lambda: "US")
+  assert alert(custom.IQNavState.CameraType.alpr).alert_text_1.startswith("Flock / ALPR Camera")
+  assert alert(custom.IQNavState.CameraType.alpr, distance=0.0).alert_text_1 == "Flock Camera Detected"
+
+  monkeypatch.setattr(iq_events, "_configured_country_code", lambda: "DE")
+  assert alert(custom.IQNavState.CameraType.alpr).alert_text_1.startswith("Traffic / ALPR Camera")
+  assert alert(custom.IQNavState.CameraType.alpr, distance=0.0).alert_text_1 == "Traffic / ALPR Camera Detected"
+
+
+def test_missing_region_is_safe_and_preserves_flock_wording(monkeypatch):
+  class UnavailableParams:
+    def get(self, key):
+      raise OSError(key)
+
+  monkeypatch.setattr(iq_events, "Params", UnavailableParams)
+  assert iq_events._configured_country_code() == ""
+  assert alert(custom.IQNavState.CameraType.alpr, distance=0.0).alert_text_1 == "Flock Camera Detected"
+
+
+def test_driver_attention_and_takeover_alerts_preempt_alpr():
+  flock = alert(custom.IQNavState.CameraType.alpr, distance=0.0)
+  pre_attention = copy.copy(EVENTS[log.OnroadEvent.EventName.preDriverDistracted][ET.PERMANENT])
+  prompt_attention = copy.copy(EVENTS[log.OnroadEvent.EventName.promptDriverDistracted][ET.PERMANENT])
+  takeover = copy.copy(EVENTS[log.OnroadEvent.EventName.driverDistracted][ET.PERMANENT])
+  immediate_disable = HardDisableCard("Regression Test")
+
+  assert flock.priority == Priority.LOW
+  assert pre_attention.priority == flock.priority + 1
+  assert prompt_attention.priority == flock.priority + 1
+  assert takeover.priority > flock.priority
+  assert immediate_disable.priority > flock.priority
+
+  for expected in (pre_attention, prompt_attention, takeover, immediate_disable):
+    manager = AlertManager()
+    flock.alert_type = "flock/warning"
+    expected.alert_type = f"expected/{expected.alert_text_1}"
+    manager.add_many(0, [flock, expected])
+    manager.process_alerts(0, set())
+    assert manager.current_alert is expected

iqpilot/selfdrive/selfdrived/tests/test_longitudinal_pref_persistence.py

@@ -0,0 +1,41 @@
+from iqpilot.cereal import car
+
+from iqpilot.selfdrive.longitudinal_settings import get_valid_personality
+from iqpilot.selfdrive.selfdrived.selfdrived import _cleanup_startup_params
+
+
+class DummyParams:
+  def __init__(self):
+    self.removed: list[str] = []
+
+  def remove(self, key: str) -> None:
+    self.removed.append(key)
+
+
+class TestLongitudinalPrefPersistence:
+  def test_startup_cleanup_preserves_persistent_longitudinal_preferences(self):
+    params = DummyParams()
+    cp = car.CarParams()
+    cp.alphaLongitudinalAvailable = False
+    cp.openpilotLongitudinalControl = False
+
+    _cleanup_startup_params(cp, params)
+
+    assert params.removed == []
+
+  def test_invalid_personality_is_clamped_before_use(self):
+    class ParamsWithInvalidPersonality:
+      def __init__(self):
+        self.value = 3
+
+      def get(self, key: str, return_default: bool = False) -> int:
+        assert key == "LongitudinalPersonality"
+        return self.value
+
+      def put(self, key: str, value: int) -> None:
+        assert key == "LongitudinalPersonality"
+        self.value = value
+
+    params = ParamsWithInvalidPersonality()
+    assert get_valid_personality(params) == 2
+    assert params.value == 2

iqpilot/selfdrive/selfdrived/tests/test_longitudinal_settings.py

@@ -0,0 +1,110 @@
+"""
+Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos/
+"""
+
+import pytest
+
+from iqpilot.selfdrive.longitudinal_settings import (
+  LONGITUDINAL_MODE_CHILL,
+  LONGITUDINAL_MODE_DYNAMIC,
+  LONGITUDINAL_MODE_STOCK,
+  PERSONALITY_AGGRESSIVE,
+  PERSONALITY_RELAXED,
+  PERSONALITY_STANDARD,
+  PERSONALITY_VALUES,
+  apply_longitudinal_mode,
+  get_follow_distance_state,
+  get_longitudinal_mode,
+  get_runtime_personality,
+  set_valid_personality,
+)
+
+
+class Params:
+  def __init__(self, personality=PERSONALITY_STANDARD):
+    self.values = {
+      "AlphaLongitudinalEnabled": True,
+      "ExperimentalMode": True,
+      "IQDynamicMode": True,
+      "LongitudinalPersonality": personality,
+    }
+    self.personality_writes = []
+
+  def get(self, key, return_default=False):
+    return self.values[key]
+
+  def get_bool(self, key):
+    return bool(self.values[key])
+
+  def put(self, key, value):
+    self.values[key] = value
+    if key == "LongitudinalPersonality":
+      self.personality_writes.append(value)
+
+  def put_bool(self, key, value):
+    self.values[key] = bool(value)
+
+
+def test_personality_writer_rejects_stock_value():
+  params = Params()
+
+  with pytest.raises(ValueError):
+    set_valid_personality(params, 3)
+
+  assert params.personality_writes == []
+
+
+def test_mode_paths_only_write_valid_personalities():
+  params = Params(PERSONALITY_AGGRESSIVE)
+
+  for mode in range(4):
+    apply_longitudinal_mode(params, mode)
+
+  assert all(value in PERSONALITY_VALUES for value in params.personality_writes)
+
+
+def test_stock_mode_preserves_personality_and_dynamic_restores_it():
+  params = Params(PERSONALITY_AGGRESSIVE)
+
+  apply_longitudinal_mode(params, LONGITUDINAL_MODE_STOCK)
+
+  assert get_follow_distance_state(params) == (None, False)
+  assert get_runtime_personality(params) == PERSONALITY_AGGRESSIVE
+  assert params.values["LongitudinalPersonality"] == PERSONALITY_AGGRESSIVE
+  assert params.personality_writes == []
+
+  apply_longitudinal_mode(params, LONGITUDINAL_MODE_DYNAMIC)
+
+  assert get_longitudinal_mode(params) == LONGITUDINAL_MODE_DYNAMIC
+  assert get_follow_distance_state(params) == (PERSONALITY_AGGRESSIVE, True)
+
+
+def test_stock_mode_sanitizes_legacy_stock_personality_value():
+  params = Params(3)
+
+  apply_longitudinal_mode(params, LONGITUDINAL_MODE_STOCK)
+
+  assert get_follow_distance_state(params) == (None, False)
+  assert params.values["LongitudinalPersonality"] == PERSONALITY_RELAXED
+  assert params.personality_writes == [PERSONALITY_RELAXED]
+
+
+def test_chill_mode_forces_relaxed_personality():
+  params = Params(PERSONALITY_AGGRESSIVE)
+
+  apply_longitudinal_mode(params, LONGITUDINAL_MODE_CHILL)
+
+  assert get_follow_distance_state(params) == (PERSONALITY_RELAXED, False)
+  assert get_runtime_personality(params) == PERSONALITY_RELAXED
+  assert params.values["LongitudinalPersonality"] == PERSONALITY_RELAXED
+  assert params.personality_writes == [PERSONALITY_RELAXED]
+
+
+def test_dynamic_and_pilot_enable_valid_personality_selection():
+  params = Params(PERSONALITY_STANDARD)
+
+  assert get_follow_distance_state(params) == (PERSONALITY_STANDARD, True)
+
+  params.values["IQDynamicMode"] = False
+
+  assert get_follow_distance_state(params) == (PERSONALITY_STANDARD, True)

iqpilot/selfdrive/selfdrived/tests/test_state_machine.py

@@ -0,0 +1,92 @@
+from iqpilot.cereal import log
+from iqpilot.common.realtime import DT_CTRL
+from iqpilot.selfdrive.selfdrived.state import StateMachine, SOFT_DISABLE_TIME
+from iqpilot.selfdrive.selfdrived.events import Events, ET, EVENTS, NormalPermanentAlert
+
+State = log.SelfdriveState.OpenpilotState
+
+# The event types that maintain the current state
+MAINTAIN_STATES = {State.enabled: (None,), State.disabled: (None,), State.softDisabling: (ET.SOFT_DISABLE,),
+                   State.preEnabled: (ET.PRE_ENABLE,), State.overriding: (ET.OVERRIDE_LATERAL, ET.OVERRIDE_LONGITUDINAL)}
+ALL_STATES = tuple(State.schema.enumerants.values())
+# The event types checked in DISABLED section of state machine
+ENABLE_EVENT_TYPES = (ET.ENABLE, ET.PRE_ENABLE, ET.OVERRIDE_LATERAL, ET.OVERRIDE_LONGITUDINAL)
+
+
+def make_event(event_types):
+  event = {}
+  for ev in event_types:
+    event[ev] = NormalPermanentAlert("alert")
+  EVENTS[0] = event
+  return 0
+
+
+class TestStateMachine:
+  def setup_method(self):
+    self.events = Events()
+    self.state_machine = StateMachine()
+    self.state_machine.soft_disable_timer = int(SOFT_DISABLE_TIME / DT_CTRL)
+
+  def test_immediate_disable(self):
+    for state in ALL_STATES:
+      for et in MAINTAIN_STATES[state]:
+        self.events.add(make_event([et, ET.IMMEDIATE_DISABLE]))
+        self.state_machine.state = state
+        self.state_machine.update(self.events)
+        assert State.disabled == self.state_machine.state
+        self.events.clear()
+
+  def test_user_disable(self):
+    for state in ALL_STATES:
+      for et in MAINTAIN_STATES[state]:
+        self.events.add(make_event([et, ET.USER_DISABLE]))
+        self.state_machine.state = state
+        self.state_machine.update(self.events)
+        assert State.disabled == self.state_machine.state
+        self.events.clear()
+
+  def test_soft_disable(self):
+    for state in ALL_STATES:
+      if state == State.preEnabled:  # preEnabled considers NO_ENTRY instead
+        continue
+      for et in MAINTAIN_STATES[state]:
+        self.events.add(make_event([et, ET.SOFT_DISABLE]))
+        self.state_machine.state = state
+        self.state_machine.update(self.events)
+        assert self.state_machine.state == State.disabled if state == State.disabled else State.softDisabling
+        self.events.clear()
+
+  def test_soft_disable_timer(self):
+    self.state_machine.state = State.enabled
+    self.events.add(make_event([ET.SOFT_DISABLE]))
+    self.state_machine.update(self.events)
+    for _ in range(int(SOFT_DISABLE_TIME / DT_CTRL)):
+      assert self.state_machine.state == State.softDisabling
+      self.state_machine.update(self.events)
+
+    assert self.state_machine.state == State.disabled
+
+  def test_no_entry(self):
+    # Make sure noEntry keeps us disabled
+    for et in ENABLE_EVENT_TYPES:
+      self.events.add(make_event([ET.NO_ENTRY, et]))
+      self.state_machine.update(self.events)
+      assert self.state_machine.state == State.disabled
+      self.events.clear()
+
+  def test_no_entry_pre_enable(self):
+    # preEnabled with noEntry event
+    self.state_machine.state = State.preEnabled
+    self.events.add(make_event([ET.NO_ENTRY, ET.PRE_ENABLE]))
+    self.state_machine.update(self.events)
+    assert self.state_machine.state == State.preEnabled
+
+  def test_maintain_states(self):
+    # Given current state's event type, we should maintain state
+    for state in ALL_STATES:
+      for et in MAINTAIN_STATES[state]:
+        self.state_machine.state = state
+        self.events.add(make_event([et]))
+        self.state_machine.update(self.events)
+        assert self.state_machine.state == state
+        self.events.clear()