add speed_logic daemon: write speed + cruise warning display params

Standalone managed process (selfdrive.clearpilot.speed_logicd) that ticks SpeedState at 2 Hz from cereal subscriptions. Decoupled from controlsd so self-driving timing isn't affected. Inputs: - gpsLocation.speed + hasFix (vehicle speed) - carState.cruiseState (cruise speed, enabled, standstill) - CarSpeedLimit memory param (will be populated when carstate.py CAN-FD decode is ported; 0 until then — speed-limit + warning logic naturally short-circuits) - IsMetric persistent param (read once at startup) Outputs (memory params, gated on change to avoid 2 Hz tmpfs churn): - ClearpilotSpeedDisplay, ClearpilotHasSpeed, ClearpilotSpeedUnit - ClearpilotSpeedLimitDisplay, ClearpilotIsMetric - ClearpilotCruiseWarning, ClearpilotCruiseWarningSpeed - ClearpilotPlayDing (one-shot trigger consumed by soundd) speed_logic.py copied verbatim from broken; speed_logicd.py is new glue. Registered as PythonProcess gated on only_onroad.
2026-05-03 22:52:39 -05:00
parent 5d2fe5248b
commit 41c154014a
3 changed files with 177 additions and 0 deletions
@@ -0,0 +1,114 @@
+"""
+ClearPilot speed processing module.
+
+Shared logic for converting raw speed and speed limit data into display-ready
+values. Called from controlsd (live mode) and bench_onroad (bench mode).
+
+Reads raw inputs, converts to display units (mph or kph based on car's CAN
+unit setting), detects speed limit changes, and writes results to params_memory
+for the onroad UI to read.
+"""
+import math
+import time
+from openpilot.common.params import Params
+from openpilot.common.conversions import Conversions as CV
+
+
+class SpeedState:
+  def __init__(self):
+    self.params_memory = Params("/dev/shm/params")
+    self.prev_speed_limit = 0
+
+    # Ding state tracking
+    self.last_ding_time = 0.0
+    self.prev_warning = ""
+    self.prev_warning_speed_limit = 0
+
+    # Cache last-written param values — each put() is mkstemp+fsync+flock+rename.
+    # Sentinel None so the first call always writes.
+    self._w_has_speed = None
+    self._w_speed_display = None
+    self._w_speed_limit_display = None
+    self._w_speed_unit = None
+    self._w_is_metric = None
+    self._w_cruise_warning = None
+    self._w_cruise_warning_speed = None
+
+  def _put_if_changed(self, key, value, attr):
+    if getattr(self, attr) != value:
+      self.params_memory.put(key, value)
+      setattr(self, attr, value)
+
+  def update(self, speed_ms: float, has_speed: bool, speed_limit_ms: float, is_metric: bool,
+             cruise_speed_ms: float = 0.0, cruise_active: bool = False, cruise_standstill: bool = False):
+    """
+    Convert raw m/s values to display-ready strings and write to params_memory.
+    """
+    now = time.monotonic()
+
+    if is_metric:
+      speed_display = speed_ms * CV.MS_TO_KPH
+      speed_limit_display = speed_limit_ms * CV.MS_TO_KPH
+      cruise_display = cruise_speed_ms * CV.MS_TO_KPH
+      unit = "km/h"
+    else:
+      speed_display = speed_ms * CV.MS_TO_MPH
+      speed_limit_display = speed_limit_ms * CV.MS_TO_MPH
+      cruise_display = cruise_speed_ms * CV.MS_TO_MPH
+      unit = "mph"
+
+    speed_int = int(math.floor(speed_display))
+    speed_limit_int = int(round(speed_limit_display))
+    cruise_int = int(round(cruise_display))
+
+    self.prev_speed_limit = speed_limit_int
+
+    # Write display-ready values to params_memory (gated on change)
+    self._put_if_changed("ClearpilotHasSpeed", "1" if has_speed and speed_int > 0 else "0", "_w_has_speed")
+    self._put_if_changed("ClearpilotSpeedDisplay", str(speed_int) if has_speed and speed_int > 0 else "", "_w_speed_display")
+    self._put_if_changed("ClearpilotSpeedLimitDisplay", str(speed_limit_int) if speed_limit_int > 0 else "0", "_w_speed_limit_display")
+    self._put_if_changed("ClearpilotSpeedUnit", unit, "_w_speed_unit")
+    self._put_if_changed("ClearpilotIsMetric", "1" if is_metric else "0", "_w_is_metric")
+
+    # Cruise warning logic
+    warning = ""
+    warning_speed = ""
+    cruise_engaged = cruise_active and not cruise_standstill
+
+    if speed_limit_int >= 20 and cruise_engaged and cruise_int > 0:
+      # Tiers (warning fires at >= limit + threshold):
+      #   limit >= 50:  +9 over ok, warn at +10   (e.g. 60 → warn at 70)
+      #   limit 26-49:  +6 over ok, warn at +7    (e.g. 35 → warn at 42)
+      #   limit <= 25:  +8 over ok, warn at +9    (e.g. 25 → warn at 34, so 33 is ok)
+      if speed_limit_int >= 50:
+        over_threshold = 10
+      elif speed_limit_int <= 25:
+        over_threshold = 9
+      else:
+        over_threshold = 7
+      if cruise_int >= speed_limit_int + over_threshold:
+        warning = "over"
+        warning_speed = str(cruise_int)
+      elif cruise_int <= speed_limit_int - 5:
+        warning = "under"
+        warning_speed = str(cruise_int)
+
+    self._put_if_changed("ClearpilotCruiseWarning", warning, "_w_cruise_warning")
+    self._put_if_changed("ClearpilotCruiseWarningSpeed", warning_speed, "_w_cruise_warning_speed")
+
+    # Ding logic: play when warning sign appears or speed limit changes while visible
+    should_ding = False
+    if warning:
+      if not self.prev_warning:
+        # Warning sign just appeared
+        should_ding = True
+      elif speed_limit_int != self.prev_warning_speed_limit:
+        # Speed limit changed while warning sign is visible
+        should_ding = True
+
+    if should_ding and now - self.last_ding_time >= 30:
+      self.params_memory.put("ClearpilotPlayDing", "1")
+      self.last_ding_time = now
+
+    self.prev_warning = warning
+    self.prev_warning_speed_limit = speed_limit_int if warning else 0
@@ -0,0 +1,60 @@
+#!/usr/bin/env python3
+"""
+ClearPilot speed/cruise daemon.
+
+Subscribes to gpsLocation (for vehicle speed display) and carState (for
+cruise state), reads CarSpeedLimit from /dev/shm/params, and ticks
+SpeedState at ~2 Hz. SpeedState writes the display params the onroad UI
+reads, and asserts ClearpilotPlayDing="1" on speed-limit warning
+transitions (soundd consumes that flag).
+
+Decoupled from controlsd so self-driving timing isn't affected.
+"""
+import sys
+
+import cereal.messaging as messaging
+from openpilot.common.params import Params
+from openpilot.common.realtime import Ratekeeper
+from openpilot.selfdrive.clearpilot.speed_logic import SpeedState
+
+
+def main():
+  print("CLP speed_logicd: starting", file=sys.stderr, flush=True)
+
+  params = Params()
+  params_memory = Params("/dev/shm/params")
+  speed_state = SpeedState()
+
+  is_metric = params.get_bool("IsMetric")
+
+  sm = messaging.SubMaster(['gpsLocation', 'carState'])
+  rk = Ratekeeper(2.0, print_delay_threshold=None)
+
+  while True:
+    sm.update(0)
+
+    gps = sm['gpsLocation']
+    has_gps = sm.valid['gpsLocation'] and gps.hasFix
+    speed_ms = float(gps.speed) if has_gps else 0.0
+
+    cs = sm['carState']
+    cruise_speed_ms = float(cs.cruiseState.speed)
+    cruise_active = bool(cs.cruiseState.enabled)
+    cruise_standstill = bool(cs.cruiseState.standstill)
+
+    # CarSpeedLimit is published by hyundai/carstate.py CAN-FD decode (when
+    # ported). Until then it's 0 / empty and the speed-limit + warning logic
+    # naturally short-circuits.
+    try:
+      speed_limit_ms = float(params_memory.get("CarSpeedLimit") or 0.0)
+    except (TypeError, ValueError):
+      speed_limit_ms = 0.0
+
+    speed_state.update(speed_ms, has_gps, speed_limit_ms, is_metric,
+                       cruise_speed_ms, cruise_active, cruise_standstill)
+
+    rk.keep_time()
+
+
+if __name__ == "__main__":
+  main()
@@ -86,6 +86,9 @@ procs = [
  # Uses Quectel modem AT commands via mmcli. Self-driving does NOT consume this; locationd
  # is patched to skip gpsLocation. Used only for system clock + UI speed + dashcam metadata.
  PythonProcess("gpsd", "system.clearpilot.gpsd", qcomgps, enabled=TICI),
+  # CLEARPILOT: speed/cruise overlay daemon. Reads gpsLocation + carState, writes display
+  # params for the onroad UI; asserts ClearpilotPlayDing on speed-limit warning transitions.
+  PythonProcess("speed_logicd", "selfdrive.clearpilot.speed_logicd", only_onroad),
  # PythonProcess("ugpsd", "system.ugpsd", only_onroad, enabled=TICI),
  #PythonProcess("navd", "selfdrive.navd.navd", only_onroad),
  PythonProcess("pandad", "selfdrive.boardd.pandad", always_run),