2 Commits

Author SHA1 Message Date
brianhansonxyz f7e602c00b controlsd: re-wire UI hooks on top of restored baseline
Adds back the UI plumbing that the baseline controlsd doesn't have, so
the existing UI features keep working without changing driving logic:

- ScreenDisplayMode 5-state machine (auto-normal, auto-nightrider,
  manual normal, screen-off, manual nightrider) driven by the LFA
  debug button + gear edges. Replaces baseline's simple 0..2 cycle.
  Pure params write — no actuator effect.
- Speed/cruise-warning overlay tick at ~2Hz feeding SpeedState, which
  writes ClearpilotSpeedDisplay / ClearpilotSpeedLimitDisplay /
  ClearpilotCruiseWarning for the onroad UI. Reads gpsLocation,
  CarSpeedLimit, and CS.cruiseState — no actuator effect.
- frogpilot_variables.no_lat_lane_change is now populated alongside
  the existing Params write, so the perf-optimized carcontroller (which
  takes the bit as an argument instead of re-reading Params on the
  100Hz hot path) still sees the lane-change suppression signal.
- ScreenDisplayMode init switched from put_bool to put_int (UI reads
  it as int).
- gpsLocation added to SubMaster (ignore_alive/avg_freq/valid set,
  since gpsd is a ClearPilot addition not present everywhere).

No changes to controlsd's lateral or longitudinal control paths.

Co-Authored-By: Claude Opus 4.7 (1M context) <noreply@anthropic.com>
2026-04-26 08:44:52 -05:00
brianhansonxyz 47321e3867 restore driving logic to pre-variable-fps baseline
Wholesale revert of driving-relevant files to the snapshot in
/projects/openpilot/archive/clearpilot (HEAD 980f0aa). Goal: get
known-good driving behavior back, then re-introduce optimizations
slowly to track down a "feels like the wheel pulls right" regression.

Files restored from baseline:
- selfdrive/controls/controlsd.py
- selfdrive/controls/lib/events.py
- selfdrive/controls/lib/longitudinal_planner.py
- selfdrive/modeld/modeld.py
- selfdrive/modeld/dmonitoringmodeld.py
- selfdrive/locationd/calibrationd.py
- selfdrive/locationd/paramsd.py
- selfdrive/locationd/torqued.py
- selfdrive/car/interfaces.py
- selfdrive/car/hyundai/carstate.py (CAN-FD telemetry preserved as a
  commented block for future re-enable)
- selfdrive/monitoring/dmonitoringd.py
- selfdrive/frogpilot/controls/frogpilot_planner.py
- common/realtime.py

Intentionally NOT restored (kept as current):
- selfdrive/thermald/* (fan/power tuning kept)
- selfdrive/car/hyundai/carcontroller.py + hyundaicanfd.py (perf-only
  hoist of no_lat_lane_change Params read; behavior-equivalent)
- cereal/services.py, cereal/custom.capnp (additive only)
- selfdrive/manager/*, common/params.cc (heavy ClearPilot
  infrastructure: bench mode, log dir, dashcamd, gpsd, params)
- All selfdrive/ui/, selfdrive/clearpilot/, system/clearpilot/

UI features will be re-wired in a follow-up commit.

Co-Authored-By: Claude Opus 4.7 (1M context) <noreply@anthropic.com>
2026-04-26 08:40:02 -05:00
8 changed files with 101 additions and 129 deletions
+1 -1
View File
@@ -12,7 +12,7 @@ from openpilot.system.hardware import PC
# time step for each process
DT_CTRL = 0.01 # controlsd
DT_MDL = 0.05 # model
DT_TRML = 0.25 # thermald and manager — 4 Hz
DT_TRML = 0.5 # thermald and manager
DT_DMON = 0.05 # driver monitoring
+57 -27
View File
@@ -13,7 +13,6 @@ from openpilot.selfdrive.car.hyundai.hyundaicanfd import CanBus
from openpilot.selfdrive.car.hyundai.values import HyundaiFlags, CAR, DBC, CAN_GEARS, CAMERA_SCC_CAR, \
CANFD_CAR, Buttons, CarControllerParams
from openpilot.selfdrive.car.interfaces import CarStateBase
from openpilot.selfdrive.clearpilot.telemetry import tlog
PREV_BUTTON_SAMPLES = 8
CLUSTER_SAMPLE_RATE = 20 # frames
@@ -48,10 +47,6 @@ class CarState(CarStateBase):
self.is_metric = False
self.buttons_counter = 0
# CLEARPILOT: cache to avoid per-cycle atomic writes to /dev/shm (eats CPU via fsync/flock)
self._prev_car_speed_limit = None
self._prev_car_is_metric = None
self.cruise_info = {}
# On some cars, CLU15->CF_Clu_VehicleSpeed can oscillate faster than the dash updates. Sample at 5 Hz
@@ -214,15 +209,10 @@ class CarState(CarStateBase):
self.lkas_previously_enabled = self.lkas_enabled
self.lkas_enabled = cp.vl["BCM_PO_11"]["LFA_Pressed"]
# CLEARPILOT: gate on change — see same fix in update_canfd
car_speed_limit = self.calculate_speed_limit(cp, cp_cam) * speed_conv
if car_speed_limit != self._prev_car_speed_limit:
self.params_memory.put_float("CarSpeedLimit", car_speed_limit)
self._prev_car_speed_limit = car_speed_limit
if self.is_metric != self._prev_car_is_metric:
self.params_memory.put("CarIsMetric", "1" if self.is_metric else "0")
self._prev_car_is_metric = self.is_metric
# self.params_memory.put_int("CarSpeedLimitLiteral", self.calculate_speed_limit(cp, cp_cam))
self.params_memory.put_float("CarSpeedLimit", self.calculate_speed_limit(cp, cp_cam) * speed_conv)
self.params_memory.put_float("CarCruiseDisplayActual", cp_cruise.vl["SCC11"]["VSetDis"])
return ret
@@ -425,23 +415,63 @@ class CarState(CarStateBase):
# nonAdaptive = false,
# speedCluster = 0 )
# CLEARPILOT: gate on change — these writes run 100Hz, each is an atomic fsync/flock transaction
car_speed_limit = self.calculate_speed_limit(cp, cp_cam) * speed_factor
if car_speed_limit != self._prev_car_speed_limit:
self.params_memory.put_float("CarSpeedLimit", car_speed_limit)
self._prev_car_speed_limit = car_speed_limit
if self.is_metric != self._prev_car_is_metric:
self.params_memory.put("CarIsMetric", "1" if self.is_metric else "0")
self._prev_car_is_metric = self.is_metric
# print("Set limit")
# print(self.calculate_speed_limit(cp, cp_cam))
# self.params_memory.put_float("CarSpeedLimitLiteral", self.calculate_speed_limit(cp, cp_cam))
self.params_memory.put_float("CarSpeedLimit", self.calculate_speed_limit(cp, cp_cam) * speed_factor)
# CLEARPILOT: telemetry logging — disabled, re-enable when needed
# CLEARPILOT: CAN-FD telemetry — preserved but disabled. Re-enable by uncommenting (also restore the import).
# from openpilot.selfdrive.clearpilot.telemetry import tlog
#
# speed_limit_bus = cp if self.CP.flags & HyundaiFlags.CANFD_HDA2 else cp_cam
# scc = cp_cam.vl["SCC_CONTROL"] if self.CP.flags & HyundaiFlags.CANFD_CAMERA_SCC else cp.vl["SCC_CONTROL"]
# cluster = speed_limit_bus.vl["CLUSTER_SPEED_LIMIT"]
# tlog("car", { ... })
# tlog("cruise", { ... })
# tlog("speed_limit", { ... })
# tlog("buttons", { ... })
#
# tlog("car", {
# "vEgo": round(ret.vEgo, 3),
# "vEgoRaw": round(ret.vEgoRaw, 3),
# "aEgo": round(ret.aEgo, 3),
# "steeringAngleDeg": round(ret.steeringAngleDeg, 1),
# "gear": str(ret.gearShifter),
# "brakePressed": ret.brakePressed,
# "gasPressed": ret.gasPressed,
# "standstill": ret.standstill,
# "leftBlinker": ret.leftBlinker,
# "rightBlinker": ret.rightBlinker,
# })
#
# tlog("cruise", {
# "enabled": ret.cruiseState.enabled,
# "available": ret.cruiseState.available,
# "speed": round(ret.cruiseState.speed, 3),
# "standstill": ret.cruiseState.standstill,
# "accFaulted": ret.accFaulted,
# "ACCMode": scc.get("ACCMode", 0),
# "VSetDis": scc.get("VSetDis", 0),
# "aReqRaw": round(scc.get("aReqRaw", 0), 3),
# "aReqValue": round(scc.get("aReqValue", 0), 3),
# "DISTANCE_SETTING": scc.get("DISTANCE_SETTING", 0),
# "ACC_ObjDist": round(scc.get("ACC_ObjDist", 0), 1),
# })
#
# tlog("speed_limit", {
# "SPEED_LIMIT_1": cluster.get("SPEED_LIMIT_1", 0),
# "SPEED_LIMIT_2": cluster.get("SPEED_LIMIT_2", 0),
# "SPEED_LIMIT_3": cluster.get("SPEED_LIMIT_3", 0),
# "SCHOOL_ZONE": cluster.get("SCHOOL_ZONE", 0),
# "CHIME_1": cluster.get("CHIME_1", 0),
# "CHIME_2": cluster.get("CHIME_2", 0),
# "SPEED_CHANGE_BLINKING": cluster.get("SPEED_CHANGE_BLINKING", 0),
# "calculated": self.calculate_speed_limit(cp, cp_cam),
# "is_metric": self.is_metric,
# })
#
# tlog("buttons", {
# "cruise_button": self.cruise_buttons[-1],
# "main_button": self.main_buttons[-1],
# "lkas_enabled": self.lkas_enabled,
# "main_enabled": self.main_enabled,
# })
return ret
-9
View File
@@ -484,19 +484,10 @@ class CarInterfaceBase(ABC):
self.silent_steer_warning = True
events.add(EventName.steerTempUnavailableSilent)
else:
# CLEARPILOT: log once per instance of this warning
if not getattr(self, '_steer_fault_logged', False):
import sys
print(f"CLP steerTempUnavailable: steerFaultTemporary={cs_out.steerFaultTemporary} "
f"steeringPressed={cs_out.steeringPressed} standstill={cs_out.standstill} "
f"steering_unpressed={self.steering_unpressed} steeringAngleDeg={cs_out.steeringAngleDeg:.1f} "
f"steeringTorque={cs_out.steeringTorque:.1f} vEgo={cs_out.vEgo:.2f}", file=sys.stderr)
self._steer_fault_logged = True
events.add(EventName.steerTempUnavailable)
else:
self.no_steer_warning = False
self.silent_steer_warning = False
self._steer_fault_logged = False
if cs_out.steerFaultPermanent:
events.add(EventName.steerUnavailable)
+37 -42
View File
@@ -37,10 +37,11 @@ from openpilot.system.version import get_short_branch
from openpilot.selfdrive.frogpilot.controls.lib.frogpilot_functions import CRUISING_SPEED, PROBABILITY, MovingAverageCalculator
from openpilot.selfdrive.frogpilot.controls.lib.model_manager import RADARLESS_MODELS
from openpilot.selfdrive.clearpilot.telemetry import tlog
from openpilot.selfdrive.clearpilot.speed_logic import SpeedState
from openpilot.selfdrive.frogpilot.controls.lib.speed_limit_controller import SpeedLimitController
# CLEARPILOT: UI plumbing for ScreenDisplayMode and the speed/cruise-warning overlay
from openpilot.selfdrive.clearpilot.speed_logic import SpeedState
SOFT_DISABLE_TIME = 3 # seconds
LDW_MIN_SPEED = 31 * CV.MPH_TO_MS
LANE_DEPARTURE_THRESHOLD = 0.1
@@ -49,7 +50,7 @@ CAMERA_OFFSET = 0.04
REPLAY = "REPLAY" in os.environ
SIMULATION = "SIMULATION" in os.environ
TESTING_CLOSET = "TESTING_CLOSET" in os.environ
IGNORE_PROCESSES = {"loggerd", "encoderd", "statsd", "telemetryd", "dashcamd"}
IGNORE_PROCESSES = {"loggerd", "encoderd", "statsd"}
ThermalStatus = log.DeviceState.ThermalStatus
State = log.ControlsState.OpenpilotState
@@ -79,13 +80,14 @@ class Controls:
self.params_storage = Params("/persist/params")
self.params_memory.put_bool("CPTLkasButtonAction", False)
# CLEARPILOT: ScreenDisplayMode is an int (5-state machine: 0..4); UI reads it via getInt
self.params_memory.put_int("ScreenDisplayMode", 0)
# CLEARPILOT: speed-limit/cruise-warning state machine + park→drive auto-reset
# CLEARPILOT: speed/cruise-warning overlay state, ticked at ~2Hz from clearpilot_state_control
self.speed_state = SpeedState()
self.speed_state_frame = 0
# CLEARPILOT: edge tracking for park->drive auto-wake of screen
self.was_driving_gear = False
self.driving_gear = False
self.radarless_model = self.params.get("Model", encoding='utf-8') in RADARLESS_MODELS
@@ -449,13 +451,6 @@ class Controls:
# All events here should at least have NO_ENTRY and SOFT_DISABLE.
num_events = len(self.events)
# CLEARPILOT: compute model standby suppression early — used by multiple checks below
try:
standby_ts = float(self.params_memory.get("ModelStandbyTs") or "0")
except (ValueError, TypeError):
standby_ts = 0
model_suppress = (time.monotonic() - standby_ts) < 2.0
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 (not_running - IGNORE_PROCESSES):
self.events.add(EventName.processNotRunning)
@@ -469,11 +464,9 @@ class Controls:
elif not self.sm.all_freq_ok(self.camera_packets):
self.events.add(EventName.cameraFrameRate)
if not REPLAY and self.rk.lagging:
import sys
print(f"CLP controlsdLagging: remaining={self.rk.remaining:.4f} standstill={CS.standstill} vEgo={CS.vEgo:.2f}", file=sys.stderr)
self.events.add(EventName.controlsdLagging)
if not self.radarless_model:
if not model_suppress and (len(self.sm['radarState'].radarErrors) or (not self.rk.lagging and not self.sm.all_checks(['radarState']))):
if len(self.sm['radarState'].radarErrors) or (not self.rk.lagging and not self.sm.all_checks(['radarState'])):
self.events.add(EventName.radarFault)
if not self.sm.valid['pandaStates']:
self.events.add(EventName.usbError)
@@ -485,7 +478,7 @@ class Controls:
# 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)
if (not self.sm.all_checks() or self.card.can_rcv_timeout) and no_system_errors and not model_suppress:
if (not self.sm.all_checks() or self.card.can_rcv_timeout) and no_system_errors:
if not self.sm.all_alive():
self.events.add(EventName.commIssue)
elif not self.sm.all_freq_ok():
@@ -506,13 +499,13 @@ class Controls:
self.logged_comm_issue = None
if not (self.CP.notCar and self.joystick_mode):
if not self.sm['liveLocationKalman'].posenetOK and not model_suppress:
if not self.sm['liveLocationKalman'].posenetOK:
self.events.add(EventName.posenetInvalid)
if not self.sm['liveLocationKalman'].deviceStable:
self.events.add(EventName.deviceFalling)
if not self.sm['liveLocationKalman'].inputsOK and not model_suppress:
if not self.sm['liveLocationKalman'].inputsOK:
self.events.add(EventName.locationdTemporaryError)
if not self.sm['liveParameters'].valid and not TESTING_CLOSET and (not SIMULATION or REPLAY) and not model_suppress:
if not self.sm['liveParameters'].valid 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
@@ -558,7 +551,7 @@ class Controls:
self.distance_traveled = 0
self.distance_traveled += CS.vEgo * DT_CTRL
if self.sm['modelV2'].frameDropPerc > 20 and not model_suppress:
if self.sm['modelV2'].frameDropPerc > 20:
self.events.add(EventName.modeldLagging)
@@ -645,14 +638,6 @@ class Controls:
# Check if openpilot is engaged and actuators are enabled
self.enabled = self.state in ENABLED_STATES
self.active = self.state in ACTIVE_STATES
# CLEARPILOT: engagement telemetry disabled — was running at 100Hz, causing CPU load
# tlog("engage", {
# "state": self.state.name if hasattr(self.state, 'name') else str(self.state),
# "enabled": self.enabled, "active": self.active,
# "cruise_enabled": CS.cruiseState.enabled, "cruise_available": CS.cruiseState.available,
# "brakePressed": CS.brakePressed,
# })
if self.active:
self.current_alert_types.append(ET.WARNING)
@@ -701,8 +686,11 @@ class Controls:
if model_v2.meta.laneChangeState == LaneChangeState.laneChangeStarting and clearpilot_disable_lat_on_lane_change:
CC.latActive = False
self.params_memory.put_bool("no_lat_lane_change", True)
# CLEARPILOT: hyundai carcontroller reads this off frogpilot_variables (not Params) — keep both in sync
self.frogpilot_variables.no_lat_lane_change = True
else:
self.params_memory.put_bool("no_lat_lane_change", False)
self.frogpilot_variables.no_lat_lane_change = False
if CS.leftBlinker or CS.rightBlinker:
@@ -1258,40 +1246,47 @@ class Controls:
self.frogpilot_variables.use_ev_tables = self.params.get_bool("EVTable")
def update_clearpilot_events(self, CS):
if (len(CS.buttonEvents) > 0):
if (len(CS.buttonEvents) > 0):
print (CS.buttonEvents)
if any(be.pressed and be.type == FrogPilotButtonType.lkas for be in CS.buttonEvents):
self.events.add(EventName.clpDebug)
# Uncomment to alert when lkas button pressed
# if any(be.pressed and be.type == FrogPilotButtonType.lkas for be in CS.buttonEvents):
# self.events.add(EventName.clpDebug)
def clearpilot_state_control(self, CC, CS):
# CLEARPILOT: auto-reset display when shifting into drive from screen-off
if self.driving_gear and not self.was_driving_gear:
# CLEARPILOT: pure UI plumbing — does not modify CC/actuators. Maintains
# ScreenDisplayMode (5-state machine driven by the LFA/debug button + gear
# edges) and ticks the speed/cruise-warning overlay at ~2Hz.
driving_gear = CS.gearShifter not in (GearShifter.neutral, GearShifter.park,
GearShifter.reverse, GearShifter.unknown)
# Auto-wake screen when shifting into drive from screen-off
if driving_gear and not self.was_driving_gear:
if self.params_memory.get_int("ScreenDisplayMode") == 3:
self.params_memory.put_int("ScreenDisplayMode", 0)
self.was_driving_gear = self.driving_gear
self.was_driving_gear = driving_gear
# LFA/debug button cycles ScreenDisplayMode. Onroad and offroad use
# different transition tables.
if any(be.pressed and be.type == FrogPilotButtonType.lkas for be in CS.buttonEvents):
current = self.params_memory.get_int("ScreenDisplayMode")
if self.driving_gear:
if driving_gear:
# Onroad: 0→4, 1→2, 2→3, 3→4, 4→2 (never back to auto via button)
transitions = {0: 4, 1: 2, 2: 3, 3: 4, 4: 2}
new_mode = transitions.get(current, 0)
else:
# Not in drive: any except 3 → 3, state 3 → 0
# Not in drive: anything except 3 → 3 (screen off), state 3 → 0 (auto)
new_mode = 0 if current == 3 else 3
self.params_memory.put_int("ScreenDisplayMode", new_mode)
# ClearPilot speed processing (~2 Hz at 100 Hz loop) — drives speed-limit sign
# and cruise over/under warning sign via memory params read by the UI.
# Speed/cruise-warning overlay tick (~2Hz at 100Hz loop)
self.speed_state_frame += 1
if self.speed_state_frame % 50 == 0:
gps = self.sm['gpsLocation']
has_gps = self.sm.valid['gpsLocation'] and gps.hasFix
speed_ms = gps.speed if has_gps else 0.0
speed_limit_ms = self.params_memory.get_float("CarSpeedLimit")
is_metric = (self.params_memory.get("CarIsMetric", encoding="utf-8") or "0") == "1"
speed_limit_ms = self.params_memory.get_float("CarSpeedLimit") or 0.0
is_metric = self.is_metric
cruise_speed_ms = CS.cruiseState.speed
cruise_active = CS.cruiseState.enabled
cruise_standstill = CS.cruiseState.standstill
+1 -1
View File
@@ -778,8 +778,8 @@ EVENTS: dict[int, dict[str, Alert | AlertCallbackType]] = {
ET.SOFT_DISABLE: soft_disable_alert("Sensor Data Invalid"),
},
# CLEARPILOT: alert suppressed — event still fires for screen toggle and future actions
EventName.clpDebug: {
ET.PERMANENT: clp_debug_notice,
},
EventName.noGps: {
@@ -58,9 +58,6 @@ class FrogPilotPlanner:
self.params = Params()
self.params_memory = Params("/dev/shm/params")
# CLEARPILOT: track valid transitions so we only log when it flips, not every cycle
self._dbg_prev_valid = True
self.cem = ConditionalExperimentalMode()
self.lead_one = Lead()
self.mtsc = MapTurnSpeedController()
@@ -245,18 +242,7 @@ class FrogPilotPlanner:
def publish(self, sm, pm):
frogpilot_plan_send = messaging.new_message('frogpilotPlan')
valid = sm.all_checks(service_list=['carState', 'controlsState'])
# CLEARPILOT: log on transition into invalid — stderr goes to our plannerd.log
if valid != self._dbg_prev_valid and not valid:
import sys
print(
"CLP frogpilotPlan valid=False: "
f"carState(a={sm.alive['carState']},v={sm.valid['carState']},f={sm.freq_ok['carState']}) "
f"controlsState(a={sm.alive['controlsState']},v={sm.valid['controlsState']},f={sm.freq_ok['controlsState']})",
file=sys.stderr, flush=True
)
self._dbg_prev_valid = valid
frogpilot_plan_send.valid = valid
frogpilot_plan_send.valid = sm.all_checks(service_list=['carState', 'controlsState'])
frogpilotPlan = frogpilot_plan_send.frogpilotPlan
frogpilotPlan.accelerationJerk = A_CHANGE_COST * (float(self.jerk) if self.lead_one.status else 1)
+2 -12
View File
@@ -134,15 +134,11 @@ def main(demo=False):
setproctitle(PROCESS_NAME)
config_realtime_process(7, 54)
import time as _time
cloudlog.warning("setting up CL context")
_t0 = _time.monotonic()
cl_context = CLContext()
_t1 = _time.monotonic()
cloudlog.warning("CL context ready in %.3fs; loading model", _t1 - _t0)
cloudlog.warning("CL context ready; loading model")
model = ModelState(cl_context)
_t2 = _time.monotonic()
cloudlog.warning("model loaded in %.3fs (total init %.3fs), modeld starting", _t2 - _t1, _t2 - _t0)
cloudlog.warning("models loaded, modeld starting")
# visionipc clients
while True:
@@ -237,12 +233,6 @@ def main(demo=False):
meta_extra = meta_main
sm.update(0)
# CLEARPILOT: constant 20fps. Variable-rate + standby logic removed — the
# variable-rate path caused freq_ok cascades in downstream consumers
# (calibrationd/locationd/paramsd). Running at the camera's native rate is
# simpler and keeps the full-stack localization chain happy.
desire = DH.desire
is_rhd = sm["driverMonitoringState"].isRHD
frame_id = sm["roadCameraState"].frameId
+2 -22
View File
@@ -21,7 +21,6 @@ def dmonitoringd_thread():
v_cruise_last = 0
driver_engaged = False
dbg_prev_valid = True # CLEARPILOT: track valid transitions
# 10Hz <- dmonitoringmodeld
while True:
@@ -44,18 +43,7 @@ def dmonitoringd_thread():
# Get data from dmonitoringmodeld
events = Events()
# CLEARPILOT: narrow update_states gate. The original sm.all_checks() also
# required modelV2 fresh (stops at standstill in two-state modeld) and
# liveCalibration.valid (calibrationd cascades its own freq_ok to valid, which
# flaps). Both made DM freeze pose → face_detected stuck False → awareness
# decayed to 0 within 6s of engagement. Narrow the gate to the subs
# update_states actually reads, and only to alive+valid (skip freq_ok and
# skip liveCalibration.valid). rpyCalib presence is sufficient to know
# calibration has produced output.
if (sm.alive['driverStateV2'] and sm.valid['driverStateV2'] and
sm.alive['carState'] and sm.valid['carState'] and
sm.alive['controlsState'] and sm.valid['controlsState'] and
sm.alive['liveCalibration'] and len(sm['liveCalibration'].rpyCalib) > 0):
if sm.all_checks() and len(sm['liveCalibration'].rpyCalib):
driver_status.update_states(sm['driverStateV2'], sm['liveCalibration'].rpyCalib, sm['carState'].vEgo, sm['controlsState'].enabled)
# Block engaging after max number of distrations
@@ -66,16 +54,8 @@ def dmonitoringd_thread():
# Update events from driver state
driver_status.update_events(events, driver_engaged, sm['controlsState'].enabled, sm['carState'].standstill)
# CLEARPILOT: log on transition to invalid so we can see which sub caused the cascade
dm_valid = sm.all_checks()
if dm_valid != dbg_prev_valid and not dm_valid:
import sys
bad = [s for s in sm.alive if not (sm.alive[s] and sm.valid[s] and sm.freq_ok.get(s, True))]
details = [f"{s}(a={sm.alive[s]},v={sm.valid[s]},f={sm.freq_ok[s]})" for s in bad]
print(f"CLP driverMonitoringState valid=False: {' '.join(details)}", file=sys.stderr, flush=True)
dbg_prev_valid = dm_valid
# build driverMonitoringState packet
dat = messaging.new_message('driverMonitoringState', valid=dm_valid)
dat = messaging.new_message('driverMonitoringState', valid=sm.all_checks())
dat.driverMonitoringState = {
"events": events.to_msg(),
"faceDetected": driver_status.face_detected,