diff --git a/selfdrive/clearpilot/speed_logic.py b/selfdrive/clearpilot/speed_logic.py new file mode 100644 index 0000000..b5a1d4a --- /dev/null +++ b/selfdrive/clearpilot/speed_logic.py @@ -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 diff --git a/selfdrive/clearpilot/speed_logicd.py b/selfdrive/clearpilot/speed_logicd.py new file mode 100644 index 0000000..a55804e --- /dev/null +++ b/selfdrive/clearpilot/speed_logicd.py @@ -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() diff --git a/selfdrive/manager/process_config.py b/selfdrive/manager/process_config.py index cc638c0..74fd50e 100755 --- a/selfdrive/manager/process_config.py +++ b/selfdrive/manager/process_config.py @@ -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),