brianhansonxyz
41c154014a
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.
115 lines
4.4 KiB
Python
115 lines
4.4 KiB
Python
"""
|
|
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
|