4 Commits

Author SHA1 Message Date
brianhansonxyz 27cad05cd9 sessions: document baseline revert + parked-controlsd architecture
Briefing for the on-device agent covering commits 47321e3, f7e602c,
and 887b9c9: what was reverted vs kept, the UI re-wire, the parked
controlsd swap mechanism, build prerequisites, runtime verification
commands, and a troubleshooting playbook.

Co-Authored-By: Claude Opus 4.7 (1M context) <noreply@anthropic.com>
2026-04-26 09:18:34 -05:00
brianhansonxyz 887b9c9e12 parked-controlsd mode: shut down heavy stack while ignition+park
Adds a second controlsd variant that runs while ignition is on but the
car is in Park. It only listens to CAN and publishes carState — no
model, no planner, no lateral/long control, no actuator commands — so
modeld, locationd, calibrationd, plannerd, radard, paramsd, torqued,
dmonitoring*, soundd, loggerd all stay stopped while parked.

Manager swaps between the two via mutually-exclusive predicates:
  - controlsd_parked: ignition AND not started
  - controlsd (full): started (= ignition AND not_parked)

Thermald owns the swap. It already subscribes to carState; we add a
new onroad condition `not_parked` derived from gearShifter, with a
1.5s hysteresis on going into parked (R/P/D thrash protection) and
zero hysteresis on going out (instant wake on shift to D/R/N). At
boot we assume parked so the heavy stack waits for carState to
confirm gear has actually left Park.

Manager predicates can only see persistent Params, not pandaStates,
so thermald exposes ignition as a new IgnitionOn param (edge-written).
Reverse is treated as not-parked — driver is moving.

Files:
- selfdrive/controls/controlsd_parked.py (new, ~50 lines)
- selfdrive/thermald/thermald.py: not_parked condition + IgnitionOn
- selfdrive/manager/process_config.py: parked_only predicate + entry
- selfdrive/manager/manager.py: seed IgnitionOn=False
- common/params.cc: register IgnitionOn

The full controlsd is unchanged.

Co-Authored-By: Claude Opus 4.7 (1M context) <noreply@anthropic.com>
2026-04-26 09:13:41 -05:00
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
19 changed files with 622 additions and 397 deletions
+1
View File
@@ -138,6 +138,7 @@ std::unordered_map<std::string, uint32_t> keys = {
{"GsmRoaming", PERSISTENT},
{"HardwareSerial", PERSISTENT},
{"HasAcceptedTerms", PERSISTENT},
{"IgnitionOn", CLEAR_ON_MANAGER_START},
{"IMEI", PERSISTENT},
{"InstallDate", PERSISTENT},
{"IsDriverViewEnabled", CLEAR_ON_MANAGER_START},
+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)
+91 -210
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,11 +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 processing
# 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.radarless_model = self.params.get("Model", encoding='utf-8') in RADARLESS_MODELS
@@ -114,8 +118,7 @@ class Controls:
'carOutput', 'driverMonitoringState', 'longitudinalPlan', 'liveLocationKalman',
'managerState', 'liveParameters', 'radarState', 'liveTorqueParameters',
'testJoystick', 'frogpilotPlan', 'gpsLocation'] + self.camera_packets + self.sensor_packets,
ignore_alive=ignore+['gpsLocation'], ignore_avg_freq=ignore+['radarState', 'testJoystick', 'gpsLocation'],
ignore_valid=['testJoystick', 'gpsLocation'],
ignore_alive=ignore + ['gpsLocation'], ignore_avg_freq=ignore+['radarState', 'testJoystick', 'gpsLocation'], ignore_valid=['testJoystick', 'gpsLocation'],
frequency=int(1/DT_CTRL))
self.joystick_mode = self.params.get_bool("JoystickDebugMode")
@@ -170,26 +173,6 @@ class Controls:
self.current_alert_types = [ET.PERMANENT]
self.logged_comm_issue = None
self.not_running_prev = None
self.lat_requested_ts = 0 # CLEARPILOT: timestamp when lat was first requested (ramp-up delay + comm issue suppression)
self.prev_lat_requested = False
# CLEARPILOT: gate frogpilotCarControl send on change (3 static bools, was publishing 100Hz)
self._prev_fpcc = (None, None, None, None, None)
self._last_fpcc_send_frame = 0
# CLEARPILOT: hysteresis counters for transient-filter on cascade alerts. A blip
# under 50ms (5 cycles at 100Hz) is typical of MSGQ conflate + slow-polling-consumer
# freq-measurement artifacts — not a real comm issue. Persistent failure still
# alerts after 50ms, well inside a driver's reaction window.
self._hyst_commissue = 0
self._hyst_locationd_tmp = 0
self._hyst_paramsd_tmp = 0
self._hyst_posenet = 0
self.HYST_CYCLES = 5
# CLEARPILOT: parked-cycle skip — in park+ignition-on, run the full step() only
# every 10th cycle. CAN parse + CAN TX still happen every cycle; outer logic
# (events, state machine, PID/MPC, publishing) runs at 10Hz. Cached message
# bytes are re-published on skipped cycles so downstream freq_ok stays OK.
self._cached_controlsState_bytes = None
self._cached_carControl_bytes = None
self.steer_limited = False
self.desired_curvature = 0.0
self.experimental_mode = False
@@ -224,8 +207,6 @@ class Controls:
self.always_on_lateral_main = self.always_on_lateral and self.params.get_bool("AlwaysOnLateralMain")
self.drive_added = False
self.driving_gear = False
self.was_driving_gear = False
self.fcw_random_event_triggered = False
self.holiday_theme_alerted = False
self.onroad_distance_pressed = False
@@ -264,51 +245,27 @@ class Controls:
CS = self.data_sample()
cloudlog.timestamp("Data sampled")
# CLEARPILOT: handle debug-button press + display-mode transitions on every
# cycle — button edge events only live in the cycle's CS.buttonEvents and
# would otherwise be dropped on skipped cycles.
self.handle_screen_mode(CS)
self.update_events(CS)
self.update_frogpilot_events(CS)
self.update_clearpilot_events(CS)
# CLEARPILOT: in park, only run the full step() every 10th cycle. data_sample
# above still runs (CAN parse, button detection). Below, card.controls_update
# still runs (CAN TX heartbeat, counters increment). The skipped outer logic
# (events, state machine, PID/MPC, publishing) causes at most ~100ms lag on
# button→state transitions, which is fine in park. Cached message bytes are
# re-sent so downstream consumers see steady 100Hz.
parked = CS.gearShifter == car.CarState.GearShifter.park
full_cycle = (not parked) or (self.sm.frame % 10 == 0) or (self._cached_controlsState_bytes is None)
cloudlog.timestamp("Events updated")
if full_cycle:
self.update_events(CS)
self.update_frogpilot_events(CS)
self.update_clearpilot_events(CS)
if not self.CP.passive and self.initialized:
# Update control state
self.state_transition(CS)
cloudlog.timestamp("Events updated")
# Compute actuators (runs PID loops and lateral MPC)
CC, lac_log = self.state_control(CS)
CC = self.clearpilot_state_control(CC, CS)
if not self.CP.passive and self.initialized:
# Update control state
self.state_transition(CS)
# Publish data
self.publish_logs(CS, start_time, CC, lac_log)
# Compute actuators (runs PID loops and lateral MPC)
CC, lac_log = self.state_control(CS)
CC = self.clearpilot_state_control(CC, CS)
# Publish data (also sends CAN TX via card.controls_update inside)
self.publish_logs(CS, start_time, CC, lac_log)
self.CS_prev = CS
# Update FrogPilot variables
self.update_frogpilot_variables(CS)
else:
# CAN TX heartbeat: keep counters incrementing and CAN frames flowing to the car
if not self.CP.passive and self.initialized:
self.card.controls_update(self.CC, self.frogpilot_variables)
# Re-publish cached messages so downstream freq_ok checks don't trip
self.pm.send('controlsState', self._cached_controlsState_bytes)
self.pm.send('carControl', self._cached_carControl_bytes)
self.CS_prev = CS
self.CS_prev = CS
# Update FrogPilot variables
self.update_frogpilot_variables(CS)
def data_sample(self):
"""Receive data from sockets and update carState"""
@@ -494,24 +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
now = time.monotonic()
model_suppress = (now - standby_ts) < 2.0
# CLEARPILOT: suppress commIssue/location/params errors for 2s after lat was requested
# Model FPS jumps from 4 to 20 on engage, causing transient freq check failures
# lat_requested_ts is set in state_control on the False->True transition of LatRequested
# CLEARPILOT: 2s window — long enough to cover the modeld rate transition burst
# without hiding real comms issues. Extending this further risks masking a genuine
# fault that demands immediate driver takeover.
lat_engage_suppress = (now - self.lat_requested_ts) < 2.0
# CLEARPILOT: expose suppress flags + standby_ts for the debug dumper in step()
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)
@@ -524,17 +463,10 @@ class Controls:
self.events.add(EventName.cameraMalfunction)
elif not self.sm.all_freq_ok(self.camera_packets):
self.events.add(EventName.cameraFrameRate)
# CLEARPILOT: alert only when avg cycle time exceeds 20ms (≈50Hz effective).
# Stock rk.lagging fires at 11.1ms (90% of 10ms) which the Hyundai CAN load
# routinely crosses while driving — that's normal, not a fault. 50Hz control is
# still plenty responsive. `rk.lagging` is still used defensively elsewhere
# (lines ~479, 492) to skip secondary checks when slightly overloaded.
avg_dt = sum(self.rk._dts) / len(self.rk._dts)
alert_lagging = avg_dt > 0.020
if not REPLAY and alert_lagging and not model_suppress and not lat_engage_suppress:
if not REPLAY and self.rk.lagging:
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)
@@ -546,60 +478,34 @@ 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)
# CLEARPILOT: commIssue fires ONLY when messages actually aren't flowing. If all
# subscribers are alive and CAN isn't timing out, comms are fine — any `valid=False`
# is an upstream self-declaration that cascades from the MSGQ conflate +
# slow-polling-consumer freq_ok measurement artifact. The car drives correctly
# during these cascades (content is still usable), so we stop raising the banner.
# Genuine comms failures — missing messages or CAN RX timeout — still fire.
comms_really_broken = (not self.sm.all_alive()) or self.card.can_rcv_timeout
commissue_cond = comms_really_broken and no_system_errors and not model_suppress and not lat_engage_suppress
if commissue_cond:
self._hyst_commissue += 1
else:
self._hyst_commissue = 0
if self._hyst_commissue >= self.HYST_CYCLES:
self.events.add(EventName.commIssue)
if commissue_cond:
# Log ONCE per entry into comms-really-broken state — no hyst counter so it doesn't
# spam every cycle (hyst changes every cycle and would make logs dict always differ).
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():
self.events.add(EventName.commIssueAvgFreq)
else: # invalid or can_rcv_timeout.
self.events.add(EventName.commIssue)
logs = {
'invalid': [s for s, valid in self.sm.valid.items() if not valid],
'not_alive': [s for s, alive in self.sm.alive.items() if not alive],
'not_freq_ok': [s for s, freq_ok in self.sm.freq_ok.items() if not freq_ok],
'can_rcv_timeout': self.card.can_rcv_timeout,
}
if logs != self.logged_comm_issue:
import sys, json
print("CLP commIssue " + json.dumps(logs), file=sys.stderr, flush=True)
cloudlog.event("commIssue", error=True, **logs)
self.logged_comm_issue = logs
else:
self.logged_comm_issue = None
if not (self.CP.notCar and self.joystick_mode):
# CLEARPILOT: suppress locationd/paramsd "temporary error" when we're in the
# freq-only cascade (same root as the commIssue suppression above). These events
# cascade from upstream freq_ok artifacts — locationd's filterInitialized latches
# False if calibrationd tips invalid due to freq, which cascades here.
# Real failures still fire commIssue above via alive/valid/can_rcv_timeout gate.
freq_only_cascade = (not self.sm.all_checks()) and not comms_really_broken
# deviceFalling is real-physics (shock sensor), no hysteresis, no cascade suppression.
if not self.sm['liveLocationKalman'].posenetOK:
self.events.add(EventName.posenetInvalid)
if not self.sm['liveLocationKalman'].deviceStable:
self.events.add(EventName.deviceFalling)
posenet_bad = not self.sm['liveLocationKalman'].posenetOK and not model_suppress and not lat_engage_suppress and not freq_only_cascade
self._hyst_posenet = self._hyst_posenet + 1 if posenet_bad else 0
if self._hyst_posenet >= self.HYST_CYCLES:
self.events.add(EventName.posenetInvalid)
locd_bad = not self.sm['liveLocationKalman'].inputsOK and not model_suppress and not lat_engage_suppress and not freq_only_cascade
self._hyst_locationd_tmp = self._hyst_locationd_tmp + 1 if locd_bad else 0
if self._hyst_locationd_tmp >= self.HYST_CYCLES:
if not self.sm['liveLocationKalman'].inputsOK:
self.events.add(EventName.locationdTemporaryError)
paramsd_bad = not self.sm['liveParameters'].valid and not TESTING_CLOSET and (not SIMULATION or REPLAY) and not model_suppress and not lat_engage_suppress and not freq_only_cascade
self._hyst_paramsd_tmp = self._hyst_paramsd_tmp + 1 if paramsd_bad else 0
if self._hyst_paramsd_tmp >= self.HYST_CYCLES:
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
@@ -613,8 +519,16 @@ class Controls:
if self.cruise_mismatch_counter > int(6. / DT_CTRL):
self.events.add(EventName.cruiseMismatch)
# CLEARPILOT: FCW disabled — car's own radar AEB works better and triggers reliably.
# The comma FCW was producing false positives and adds nothing over the stock system.
# 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:
self.events.add(EventName.fcw)
# self.fcw_random_event_triggered = True
# elif self.fcw_random_event_triggered and self.random_events:
# self.events.add(EventName.yourFrogTriedToKillMe)
# self.fcw_random_event_triggered = False
for m in messaging.drain_sock(self.log_sock, wait_for_one=False):
try:
@@ -637,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)
@@ -724,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)
@@ -762,19 +668,8 @@ class Controls:
# Check which actuators can be enabled
standstill = CS.vEgo <= max(self.CP.minSteerSpeed, MIN_LATERAL_CONTROL_SPEED) or CS.standstill
# CLEARPILOT: lat_requested ignores momentary MDPS faults so modeld doesn't drop rate during
# brief steerFaultTemporary blips (stale predictions on recovery caused a fault feedback loop).
# lat_engaged gates the actual steering command and still respects the fault.
lat_requested = (self.active or self.FPCC.alwaysOnLateral) and self.speed_check and \
(not standstill or self.joystick_mode)
lat_engaged = lat_requested and not CS.steerFaultTemporary and not CS.steerFaultPermanent
now_ts = time.monotonic()
if lat_requested and not self.prev_lat_requested:
self.lat_requested_ts = now_ts
self.prev_lat_requested = lat_requested
self.FPCC.latRequested = lat_requested
CC.latActive = lat_engaged and (now_ts - self.lat_requested_ts) >= 0.25
CC.latActive = (self.active or self.FPCC.alwaysOnLateral) and self.speed_check and not CS.steerFaultTemporary and not CS.steerFaultPermanent and \
(not standstill or self.joystick_mode)
CC.longActive = self.enabled and not self.events.contains(ET.OVERRIDE_LONGITUDINAL) and self.CP.openpilotLongitudinalControl
actuators = CC.actuators
@@ -789,13 +684,14 @@ class Controls:
CC.leftBlinker = model_v2.meta.laneChangeDirection == LaneChangeDirection.left
CC.rightBlinker = model_v2.meta.laneChangeDirection == LaneChangeDirection.right
no_lat_lane_change = model_v2.meta.laneChangeState == LaneChangeState.laneChangeStarting and clearpilot_disable_lat_on_lane_change
if no_lat_lane_change:
if model_v2.meta.laneChangeState == LaneChangeState.laneChangeStarting and clearpilot_disable_lat_on_lane_change:
CC.latActive = False
# CLEARPILOT: noLatLaneChange now lives on frogpilotCarControl (see update_frogpilot_variables).
# Also mirrored to frogpilot_variables so in-process carcontroller reads it without IPC.
self.FPCC.noLatLaneChange = no_lat_lane_change
self.frogpilot_variables.no_lat_lane_change = no_lat_lane_change
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:
self.last_blinker_frame = self.sm.frame
@@ -1031,8 +927,6 @@ class Controls:
controlsState.lateralControlState.torqueState = lac_log
self.pm.send('controlsState', dat)
# CLEARPILOT: cache for re-publication on parked-skip cycles
self._cached_controlsState_bytes = dat.to_bytes()
# onroadEvents - logged every second or on change
if (self.sm.frame % int(1. / DT_CTRL) == 0) or (self.events.names != self.events_prev):
@@ -1047,7 +941,6 @@ class Controls:
cc_send.valid = CS.canValid
cc_send.carControl = CC
self.pm.send('carControl', cc_send)
self._cached_carControl_bytes = cc_send.to_bytes()
# copy CarControl to pass to CarInterface on the next iteration
self.CC = CC
@@ -1078,11 +971,6 @@ class Controls:
while True:
self.step()
self.rk.monitor_time()
# CLEARPILOT: accumulated debt from startup/blocking calls never recovers.
# Reset the rate keeper when catastrophically behind — prevents a one-time
# ~8s fingerprinting stall from poisoning the lag metric for the whole session.
if self.rk.remaining < -1.0:
self.rk._next_frame_time = time.monotonic() + self.rk._interval
except SystemExit:
e.set()
t.join()
@@ -1153,11 +1041,13 @@ class Controls:
elif self.sm['modelV2'].meta.turnDirection == Desire.turnRight:
self.events.add(EventName.turningRight)
# CLEARPILOT: check crash file once per second, not every cycle (stat syscall at 100Hz hurts)
if self.sm.frame % 100 == 0:
self._crashed_file_exists = os.path.isfile(os.path.join(sentry.CRASHES_DIR, 'error.txt'))
if getattr(self, '_crashed_file_exists', False) and self.crashed_timer < 10:
if os.path.isfile(os.path.join(sentry.CRASHES_DIR, 'error.txt')) and self.crashed_timer < 10:
self.events.add(EventName.openpilotCrashed)
# if self.random_events and not self.openpilot_crashed_triggered:
# self.events.add(EventName.openpilotCrashedRandomEvents)
# self.openpilot_crashed_triggered = True
self.crashed_timer += DT_CTRL
# if self.speed_limit_alert or self.speed_limit_confirmation:
@@ -1280,18 +1170,10 @@ class Controls:
self.FPCC.trafficModeActive = self.frogpilot_variables.traffic_mode and self.params_memory.get_bool("TrafficModeActive")
# CLEARPILOT: send only when any field changes, with 1Hz keepalive
# Saves ~7ms/sec of capnp+zmq work (was sending 100Hz despite static contents)
# latRequested and noLatLaneChange are edge-triggered too (rare flips)
fpcc_tuple = (self.FPCC.alwaysOnLateral, self.FPCC.speedLimitChanged, self.FPCC.trafficModeActive,
self.FPCC.latRequested, self.FPCC.noLatLaneChange)
if fpcc_tuple != self._prev_fpcc or (self.sm.frame - self._last_fpcc_send_frame) >= 100:
fpcc_send = messaging.new_message('frogpilotCarControl')
fpcc_send.valid = CS.canValid
fpcc_send.frogpilotCarControl = self.FPCC
self.pm.send('frogpilotCarControl', fpcc_send)
self._prev_fpcc = fpcc_tuple
self._last_fpcc_send_frame = self.sm.frame
fpcc_send = messaging.new_message('frogpilotCarControl')
fpcc_send.valid = CS.canValid
fpcc_send.frogpilotCarControl = self.FPCC
self.pm.send('frogpilotCarControl', fpcc_send)
if self.params_memory.get_bool("FrogPilotTogglesUpdated"):
self.update_frogpilot_params()
@@ -1364,53 +1246,52 @@ 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)
def handle_screen_mode(self, CS):
"""CLEARPILOT: tracks driving_gear, auto-resets display, and cycles
ScreenDisplayMode on debug-button presses. Must run every cycle so button
edge events aren't lost during parked-skip mode (edges happen in carstate
edge detection and only appear in that one cycle's CS.buttonEvents)."""
self.driving_gear = CS.gearShifter not in (GearShifter.neutral, GearShifter.park, GearShifter.reverse, GearShifter.unknown)
# 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)
# auto-reset display when shifting into drive from screen-off
if self.driving_gear and not self.was_driving_gear:
def clearpilot_state_control(self, CC, CS):
# 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)
def clearpilot_state_control(self, CC, CS):
# ClearPilot speed processing (~2 Hz at 100 Hz loop)
# 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
self.speed_state.update(speed_ms, has_gps, speed_limit_ms, is_metric,
cruise_speed_ms, cruise_active, cruise_standstill)
return CC
def main():
+54
View File
@@ -0,0 +1,54 @@
#!/usr/bin/env python3
"""
CLEARPILOT: minimal controlsd variant that runs while ignition is on but the
car is in Park. Keeps CAN parsing and carState publishing alive (so thermald
can see gearShifter and decide when to swap us out for the full controlsd),
but skips all of the heavy onroad work — no model, no planner, no lateral or
longitudinal control, no actuator commands.
Manager swaps between this and the full controlsd via predicate flips:
- this runs when: ignition AND not started
- full runs when: started (which requires ignition AND not_parked)
The two are mutually exclusive — only one publishes carState at a time.
"""
from types import SimpleNamespace
from openpilot.common.realtime import Priority, config_realtime_process
from openpilot.selfdrive.car.card import CarD
def _make_default_frogpilot_variables() -> SimpleNamespace:
"""Safe defaults for fields read inside CarInterface.update / CarState.update.
We're not actuating anything here; these only need to keep the update path
from raising AttributeError. False/0 across the board is the safe baseline."""
fv = SimpleNamespace()
fv.conditional_experimental_mode = False
fv.experimental_mode_via_distance = False
fv.traffic_mode = False
fv.sport_plus = False
fv.long_pitch = False
fv.no_lat_lane_change = False
return fv
def main():
config_realtime_process(4, Priority.CTRL_HIGH)
# CarD's __init__ blocks until it sees CAN + a pandaState, then calls get_car
# to fingerprint and write CarParams. Same path the full controlsd takes.
card = CarD()
card.initialize()
fv = _make_default_frogpilot_variables()
# state_update drains CAN, parses carState, publishes carState/carOutput/carParams.
# Internally blocks via drain_sock_raw(wait_for_one=True), so the loop is
# naturally paced by CAN traffic — no extra sleep needed.
while True:
card.state_update(fv)
if __name__ == "__main__":
main()
+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: {
+2 -33
View File
@@ -143,10 +143,6 @@ class LongitudinalPlanner:
self.params = Params()
self.params_memory = Params("/dev/shm/params")
# CLEARPILOT: track model FPS for dynamic dt adjustment
self.model_fps = 20
self._dbg_prev_valid = True # CLEARPILOT: diagnostic logging on valid transitions
self.radarless_model = self.params.get("Model", encoding='utf-8') in RADARLESS_MODELS
self.release = get_short_branch() == "clearpilot"
@@ -177,19 +173,6 @@ class LongitudinalPlanner:
return x, v, a, j
def update(self, sm):
# CLEARPILOT: read actual model FPS and adjust dt accordingly
model_fps_raw = self.params_memory.get("ModelFps")
if model_fps_raw is not None:
try:
fps = int(model_fps_raw)
if fps > 0 and fps != self.model_fps:
self.model_fps = fps
self.dt = 1.0 / fps
self.v_desired_filter.dt = self.dt
self.v_desired_filter.update_alpha(2.0) # rc=2.0, same as __init__
except (ValueError, ZeroDivisionError):
pass
self.mpc.mode = 'blended' if sm['controlsState'].experimentalMode else 'acc'
v_ego = sm['carState'].vEgo
@@ -256,10 +239,7 @@ class LongitudinalPlanner:
self.j_desired_trajectory = np.interp(ModelConstants.T_IDXS[:CONTROL_N], T_IDXS_MPC[:-1], self.mpc.j_solution)
# TODO counter is only needed because radar is glitchy, remove once radar is gone
# CLEARPILOT: scale crash_cnt threshold by FPS — at 20fps need 3 frames (0.15s),
# at 4fps need 1 frame (0.25s already exceeds 0.15s window)
fcw_threshold = max(0, int(0.15 * self.model_fps) - 1) # 20fps->2, 4fps->0
self.fcw = self.mpc.crash_cnt > fcw_threshold and not sm['carState'].standstill
self.fcw = self.mpc.crash_cnt > 2 and not sm['carState'].standstill
if self.fcw:
cloudlog.info("FCW triggered")
@@ -278,18 +258,7 @@ class LongitudinalPlanner:
def publish(self, sm, pm):
plan_send = messaging.new_message('longitudinalPlan')
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 longitudinalPlan 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
plan_send.valid = valid
plan_send.valid = sm.all_checks(service_list=['carState', 'controlsState'])
longitudinalPlan = plan_send.longitudinalPlan
longitudinalPlan.modelMonoTime = sm.logMonoTime['modelV2']
@@ -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)
+1 -8
View File
@@ -284,14 +284,7 @@ def main() -> NoReturn:
# 4Hz driven by cameraOdometry
if sm.frame % 5 == 0:
# CLEARPILOT: publish valid based on calibration status, not upstream sm.all_checks().
# Original openpilot gated valid on fresh inputs, but that caused a cascade:
# upstream freq glitches → liveCalibration.valid=False → locationd stays
# uninitialized → paramsd fed garbage → bogus steerRatio/stiffnessFactor → erratic
# steering. "valid" semantically means "calibration data is trustworthy"; that's a
# question about calibration convergence, not input freshness.
cal_valid = calibrator.cal_status == log.LiveCalibrationData.Status.calibrated
calibrator.send_data(pm, cal_valid)
calibrator.send_data(pm, sm.all_checks())
if __name__ == "__main__":
+1 -10
View File
@@ -187,7 +187,6 @@ def main():
avg_offset_valid = True
total_offset_valid = True
roll_valid = True
dbg_prev_valid = True # CLEARPILOT: track valid transitions
while True:
sm.update()
@@ -257,15 +256,7 @@ def main():
liveParameters.filterState.std = P.tolist()
liveParameters.filterState.valid = True
lp_valid = sm.all_checks()
# CLEARPILOT: on transition to invalid, log per-subscriber state to paramsd.log
if lp_valid != dbg_prev_valid and not lp_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 liveParameters valid=False: {' '.join(details)}", file=sys.stderr, flush=True)
dbg_prev_valid = lp_valid
msg.valid = lp_valid
msg.valid = sm.all_checks()
if sm.frame % 1200 == 0: # once a minute
params = {
+1 -11
View File
@@ -226,8 +226,6 @@ def main(demo=False):
with car.CarParams.from_bytes(params.get("CarParams", block=True)) as CP:
estimator = TorqueEstimator(CP)
dbg_prev_valid = True # CLEARPILOT: track valid transitions
while True:
sm.update()
if sm.all_checks():
@@ -238,15 +236,7 @@ def main(demo=False):
# 4Hz driven by liveLocationKalman
if sm.frame % 5 == 0:
tq_valid = sm.all_checks()
# CLEARPILOT: log per-sub detail on transition to invalid — goes to torqued.log
if tq_valid != dbg_prev_valid and not tq_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 liveTorqueParameters valid=False: {' '.join(details)}", file=sys.stderr, flush=True)
dbg_prev_valid = tq_valid
pm.send('liveTorqueParameters', estimator.get_msg(valid=tq_valid))
pm.send('liveTorqueParameters', estimator.get_msg(valid=sm.all_checks()))
# Cache points every 60 seconds while onroad
if sm.frame % 240 == 0:
+3
View File
@@ -363,6 +363,9 @@ def manager_init(frogpilot_functions) -> None:
params.put("GitRemote", get_origin())
params.put_bool("IsTestedBranch", is_tested_branch())
params.put_bool("IsReleaseBranch", is_release_branch())
# CLEARPILOT: thermald is the source of truth for IgnitionOn; seed False so
# the parked-controlsd predicate evaluates to False before thermald's first tick.
params.put_bool("IgnitionOn", False)
# set dongle id
reg_res = register(show_spinner=True)
+11
View File
@@ -43,6 +43,12 @@ def only_onroad(started: bool, params, CP: car.CarParams) -> bool:
def only_offroad(started, params, CP: car.CarParams) -> bool:
return not started
# CLEARPILOT: predicate for the parked controlsd variant. Runs while ignition
# is on but the car is in Park (so started=False because thermald has gated it
# off). Mutually exclusive with the full controlsd, which uses only_onroad.
def parked_only(started, params, CP: car.CarParams) -> bool:
return params.get_bool("IgnitionOn") and not started
# FrogPilot functions
def allow_logging(started, params, CP: car.CarParams) -> bool:
allow_logging = not (params.get_bool("DeviceManagement") and params.get_bool("NoLogging"))
@@ -82,6 +88,11 @@ procs = [
PythonProcess("calibrationd", "selfdrive.locationd.calibrationd", only_onroad),
PythonProcess("torqued", "selfdrive.locationd.torqued", only_onroad),
PythonProcess("controlsd", "selfdrive.controls.controlsd", only_onroad),
# CLEARPILOT: lightweight CAN listener that runs while ignition is on and the
# car is parked. Publishes carState (so thermald can see gear); does no model,
# planner, or actuator work. Manager swaps it out for the full controlsd as
# soon as gear leaves Park.
PythonProcess("controlsd_parked", "selfdrive.controls.controlsd_parked", parked_only),
PythonProcess("deleter", "system.loggerd.deleter", always_run),
PythonProcess("dmonitoringd", "selfdrive.monitoring.dmonitoringd", driverview, enabled=(not PC or WEBCAM)),
# PythonProcess("qcomgpsd", "system.qcomgpsd.qcomgpsd", qcomgps, enabled=TICI),
+2 -14
View File
@@ -128,14 +128,10 @@ def main():
assert vipc_client.is_connected()
cloudlog.warning(f"connected with buffer size: {vipc_client.buffer_len}")
sm = SubMaster(["liveCalibration", "carState"])
sm = SubMaster(["liveCalibration"])
pm = PubMaster(["driverStateV2"])
calib = np.zeros(CALIB_LEN, dtype=np.float32)
# CLEARPILOT: cache last model output so we can republish (not re-infer) at standstill.
# Saves ~7% CPU; downstream dmonitoringd sees a steady 10Hz stream with known-good
# last readings (driver can't become distracted relative to a stopped car).
last_model_output = None
# last = 0
while True:
@@ -147,16 +143,8 @@ def main():
if sm.updated["liveCalibration"]:
calib[:] = np.array(sm["liveCalibration"].rpyCalib)
standstill = sm["carState"].standstill
t1 = time.perf_counter()
if standstill and last_model_output is not None:
# CLEARPILOT: reuse last inference at standstill
model_output = last_model_output
dsp_execution_time = 0.0
else:
model_output, dsp_execution_time = model.run(buf, calib)
last_model_output = model_output
model_output, dsp_execution_time = model.run(buf, calib)
t2 = time.perf_counter()
pm.send("driverStateV2", get_driverstate_packet(model_output, vipc_client.frame_id, vipc_client.timestamp_sof, t2 - t1, dsp_execution_time))
+2 -36
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:
@@ -183,9 +179,6 @@ def main(demo=False):
model_transform_main = np.zeros((3, 3), dtype=np.float32)
model_transform_extra = np.zeros((3, 3), dtype=np.float32)
live_calib_seen = False
model_standby = False
last_standby_ts_write = 0
params_memory = Params("/dev/shm/params")
nav_features = np.zeros(ModelConstants.NAV_FEATURE_LEN, dtype=np.float32)
nav_instructions = np.zeros(ModelConstants.NAV_INSTRUCTION_LEN, dtype=np.float32)
buf_main, buf_extra = None, None
@@ -240,33 +233,6 @@ def main(demo=False):
meta_extra = meta_main
sm.update(0)
# CLEARPILOT: two-state modeld — 0fps only when parked (ignition-on means
# engine running in park; ignition-off stops modeld at the manager level).
# Standstill in drive (red light) keeps running so lateral stays responsive
# and liveCalibration/paramsd observations continue.
parked = sm['carState'].gearShifter == car.CarState.GearShifter.park
should_standby = parked
if should_standby and not model_standby:
params_memory.put_bool("ModelStandby", True)
params_memory.put("ModelFps", "0")
model_standby = True
cloudlog.warning("modeld: standby ON")
elif not should_standby and model_standby:
params_memory.put_bool("ModelStandby", False)
params_memory.put("ModelFps", "20")
model_standby = False
run_count = 0
frame_dropped_filter.x = 0.
cloudlog.warning("modeld: standby OFF")
if model_standby:
now = _time.monotonic()
if now - last_standby_ts_write > 1.0:
params_memory.put("ModelStandbyTs", str(now))
last_standby_ts_write = now
last_vipc_frame_id = meta_main.frame_id
continue
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,
+36
View File
@@ -170,7 +170,17 @@ def thermald_thread(end_event, hw_queue) -> None:
onroad_conditions: dict[str, bool] = {
"ignition": False,
# CLEARPILOT: park-aware gating. When False, manager runs controlsd_parked
# (CAN listener only) instead of the full onroad stack. Latched + hysteresis
# on going-into-parked to avoid R↔P↔D thrash; flips out of parked instantly.
# Initialized False (assume parked) so the full stack waits for carState
# to confirm gear has actually left Park before spinning up.
"not_parked": False,
}
is_parked = True
parked_since: float | None = None # monotonic ts when gear first read as Park
PARKED_HYSTERESIS_S = 1.5
ignition_param_prev: bool | None = None
startup_conditions: dict[str, bool] = {}
startup_conditions_prev: dict[str, bool] = {}
@@ -247,6 +257,32 @@ def thermald_thread(end_event, hw_queue) -> None:
onroad_conditions["ignition"] = False
cloudlog.error("panda timed out onroad")
# CLEARPILOT: derive is_parked from carState gearShifter. Whichever controlsd
# variant is currently running publishes carState; we just read the gear.
# Going INTO parked has hysteresis (PARKED_HYSTERESIS_S) so brief P touches
# during low-speed parking don't kick the heavy stack off. Going OUT of
# parked is instant so the full stack starts spinning up the moment the
# driver shifts to D/R/N.
if sm.updated['carState']:
gear = sm['carState'].gearShifter
gear_is_park = gear == car.CarState.GearShifter.park
now_mono = time.monotonic()
if gear_is_park:
if parked_since is None:
parked_since = now_mono
if (not is_parked) and (now_mono - parked_since) >= PARKED_HYSTERESIS_S:
is_parked = True
else:
parked_since = None
is_parked = False
onroad_conditions["not_parked"] = not is_parked
# CLEARPILOT: expose ignition as a Param so manager predicates (which only
# see persistent Params, not pandaStates) can gate controlsd_parked.
if ignition_param_prev != onroad_conditions["ignition"]:
params.put_bool("IgnitionOn", onroad_conditions["ignition"])
ignition_param_prev = onroad_conditions["ignition"]
try:
last_hw_state = hw_queue.get_nowait()
except queue.Empty:
@@ -0,0 +1,355 @@
# Session: 2026-04-26 — Baseline Revert + Parked-Controlsd Mode
## Context
This session was driven by a regression: the steering wheel "feels like
it pulls right" during normal driving, with no clear smoking gun. The
suspicion was that one of the variable-FPS / standstill-throttling
changes (added to reduce parked-state fan noise and CPU load) bled into
on-road driving behavior in a hard-to-isolate way.
Strategy: revert all driving-relevant logic to a known-good baseline
captured in `/projects/openpilot/archive/clearpilot` (HEAD `980f0aa`,
July 2024), keep all of the ClearPilot UI/dashcam/telemetry/bench-mode
infrastructure intact on top, then attack the parked-fan-noise problem
fresh from a different angle that doesn't touch driving logic at all.
Three commits landed in this order on branch `clearpilot`:
| SHA | Title |
|---|---|
| `47321e3` | restore driving logic to pre-variable-fps baseline |
| `f7e602c` | controlsd: re-wire UI hooks on top of restored baseline |
| `887b9c9` | parked-controlsd mode: shut down heavy stack while ignition+park |
Pre-revert tip was `62a403d`. The on-device agent should treat
**`62a403d` as "the broken version"** when looking at history.
---
## Commit 1: `47321e3` — Baseline restore
Reverted the following files wholesale to their `980f0aa` archive copy:
- `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 at the bottom of `update_canfd` — re-enable by
uncommenting; the `tlog` import is also commented out).
- `selfdrive/monitoring/dmonitoringd.py`
- `selfdrive/frogpilot/controls/frogpilot_planner.py`
- `common/realtime.py`
### Intentionally NOT restored (kept as the post-`62a403d` version)
- `selfdrive/thermald/*` — fan/power tuning kept as-is.
- `selfdrive/car/hyundai/carcontroller.py` and `hyundaicanfd.py`
reviewed; the only delta vs baseline is hoisting the
`no_lat_lane_change` Params read out of the 100Hz hot path
(~5% carcontroller CPU) by passing the bit as an argument. This is
a perf-only change with no behavioral effect outside lane changes.
- `cereal/services.py`, `cereal/custom.capnp` — additive only.
`custom.capnp` adds `latRequested @3` and `noLatLaneChange @4` fields
to `FrogPilotCarControl`; capnp tag numbers are append-only so this is
safe to leave in even though baseline code doesn't write to them.
- `selfdrive/manager/*`, `common/params.cc` — heavy ClearPilot
infrastructure (bench mode, log dir, dashcamd, gpsd, ClearPilot params).
- All `selfdrive/ui/`, `selfdrive/clearpilot/`, `system/clearpilot/`.
### Things removed by the restore (no longer in the tree)
- Standstill frame skipping in modeld (was: skip GPU inference 19/20
frames at standstill, report 0 dropped frames to fool controlsd).
- Standstill frame skipping in dmonitoringmodeld.
- Model standby logic + `ModelStandby`/`ModelStandbyTs` reads in
controlsd's comm-issue suppression path.
- Parked-cycle skip in `state_control()` (10Hz vs 100Hz when in Park).
- Calibrationd validity decoupling from `sm.all_checks()`.
- Post-engage 2s commIssue/location/params suppression window.
- Per-cycle carstate write-gating for `CarSpeedLimit`/`CarIsMetric` in
carstate.py.
- The diff-based carstate telemetry calls (preserved commented out).
These were the candidates for the steering-pull regression. The on-device
agent should **not re-introduce any of these** without a deliberate plan
and a test session. The user's intent is to get baseline driving feel
confirmed first, then re-introduce optimizations one at a time and drive
each.
---
## Commit 2: `f7e602c` — UI hooks on top of baseline
Baseline `controlsd.py` doesn't have the UI plumbing the ClearPilot UI
expects. This commit re-adds only the things needed for the existing UI
to keep working — pure params-write plumbing, no actuator effect.
### Changes (all in `selfdrive/controls/controlsd.py`)
1. **Import `SpeedState`** from `openpilot.selfdrive.clearpilot.speed_logic`.
2. **`Controls.__init__`**:
- `params_memory.put_bool("ScreenDisplayMode", 0)`
`params_memory.put_int("ScreenDisplayMode", 0)` (UI reads it as int).
- Added `self.speed_state = SpeedState()`, `self.speed_state_frame = 0`,
`self.was_driving_gear = False`.
3. **SubMaster** — added `gpsLocation` to the subscriber list, with
`ignore_alive` / `ignore_avg_freq` / `ignore_valid` so missing GPS
doesn't trigger commIssue.
4. **`clearpilot_state_control(...)`** rewritten from a simple 3-state
cycle into the documented 5-state ScreenDisplayMode machine:
- Auto-wake on park→drive edge if currently in screen-off (state 3).
- LFA button transitions in drive: `0→4, 1→2, 2→3, 3→4, 4→2`.
- LFA button transitions outside drive: any except 3 → 3, state 3 → 0.
- Speed/cruise-warning overlay tick at ~2Hz (`speed_state.update(...)`)
reading `gpsLocation`, `CarSpeedLimit` param, `self.is_metric`,
`CS.cruiseState`. This is what writes
`ClearpilotSpeedDisplay`/`ClearpilotSpeedLimitDisplay`/
`ClearpilotCruiseWarning` for the UI overlay.
5. **Lane-change suppression sync** — at the existing baseline
`clearpilot_disable_lat_on_lane_change` block (around line 687), in
addition to the existing `params_memory.put_bool("no_lat_lane_change", ...)`,
also set `self.frogpilot_variables.no_lat_lane_change = True/False`.
This is required because the kept (post-`62a403d`) carcontroller
reads off `frogpilot_variables`, not Params.
### What does NOT change
No edits to lateral or longitudinal control paths. No new actuator-side
behavior. The UI features (nightrider/screen-off/auto day-night, speed
overlay, cruise warning chime) get wired back up purely through param
writes.
---
## Commit 3: `887b9c9` — Parked-controlsd mode
Architectural fix for the original problem this whole session was
chasing: while ignition is on but the car is in Park, the entire onroad
stack (modeld, planner, control, locationd, calibrationd, paramsd,
torqued, dmonitoring*, soundd, loggerd) is running and burning CPU/fan
even though none of it is needed.
Solution: redefine "onroad" as **ignition AND not parked** instead of
just **ignition**. Reuse the existing `started`-based process gating in
manager. Add a tiny second controlsd variant that runs while parked,
just to keep CAN flowing so thermald can see when gear leaves Park.
### Files
#### NEW: `selfdrive/controls/controlsd_parked.py`
Minimal entry point. Roughly:
```python
def main():
config_realtime_process(4, Priority.CTRL_HIGH)
card = CarD() # blocks until first CAN, fingerprints car
card.initialize()
fv = _make_default_frogpilot_variables() # safe False/0 SimpleNamespace
while True:
card.state_update(fv) # publishes carState/carOutput/carParams
```
`CarD.state_update` blocks via `drain_sock_raw(wait_for_one=True)`, so
the loop is paced by CAN traffic — no extra sleep, no CPU spin.
The default `frogpilot_variables` sets these to safe values so
`CarInterfaceBase.update` doesn't `AttributeError`:
`conditional_experimental_mode`, `experimental_mode_via_distance`,
`traffic_mode`, `sport_plus`, `long_pitch`, `no_lat_lane_change` — all
False.
#### `selfdrive/thermald/thermald.py`
- `onroad_conditions` now also has `"not_parked"`. Initialized to
`False` (assume parked at boot) so the heavy stack waits for carState
to confirm gear has left Park before spinning up.
- New module-level loop variables: `is_parked = True`,
`parked_since: float | None = None`, `PARKED_HYSTERESIS_S = 1.5`,
`ignition_param_prev: bool | None = None`.
- New block right after the panda-disconnect check (around line 258 in
current state) reads `sm['carState'].gearShifter`:
- Gear == Park: latch `parked_since`, flip `is_parked = True` after
1.5s of continuous Park (hysteresis).
- Gear != Park: clear `parked_since`, `is_parked = False` immediately
(no hysteresis going out).
- Reverse is treated as **not parked** — driver is moving.
- `onroad_conditions["not_parked"] = not is_parked` every tick.
- New `IgnitionOn` Params write, edge-driven (only on change of
`onroad_conditions["ignition"]`) so we don't hammer the persistent
filesystem 2x/sec.
#### `selfdrive/manager/process_config.py`
- New predicate:
```python
def parked_only(started, params, CP):
return params.get_bool("IgnitionOn") and not started
```
- New process entry directly after the existing `controlsd` entry:
```python
PythonProcess("controlsd_parked", "selfdrive.controls.controlsd_parked", parked_only),
```
- `controlsd` is unchanged (still `only_onroad`). Mutually exclusive
with `parked_only` because `started` is the negation of the relevant
condition.
#### `selfdrive/manager/manager.py`
- Single line in `manager_init()` seeding `IgnitionOn=False` so the
predicate evaluates correctly before thermald's first tick.
#### `common/params.cc`
- New entry in the alphabetical I-block:
```cpp
{"IgnitionOn", CLEAR_ON_MANAGER_START},
```
- Manager predicates can only see persistent Params (not pandaStates
or `/dev/shm/params`), which is why thermald has to expose ignition
this way.
### State machine summary
| Ignition | Gear | Predicates true | What runs |
|----------|-------------|-----------------------------|-------------------------------|
| off | any | none | always_run only (ui, thermald, pandad, deleter, …) |
| on | Park (>1.5s)| parked_only | always_run + controlsd_parked |
| on | not Park | only_onroad (=> all `started`) | full onroad stack |
The transition between rows 2 and 3 is purely manager process
swapping driven by predicate flips — no IPC handshake, no
self-termination. Thermald sees gear change, flips `not_parked`,
`should_start = all(onroad_conditions.values())` flips, manager kills
the wrong variant and spawns the right one on its next tick.
---
## What the on-device agent needs to know / debug
### Build prerequisites
A new param key was added (`IgnitionOn`), so the C++ params whitelist
needs a fresh build. Per `CLAUDE.md` "Adding New Params":
```bash
chown -R comma:comma /data/openpilot
rm -f /data/openpilot/prebuilt /data/openpilot/common/params.o /data/openpilot/common/libcommon.a
su - comma -c "bash /data/openpilot/build_only.sh"
```
`build_only.sh` already deletes `prebuilt` but **does not** delete
`params.o` / `libcommon.a` — verify those are gone before building or
the new key won't be picked up and `Params().put_bool("IgnitionOn", …)`
will throw `UnknownKeyName` in manager_init or thermald.
### How to verify the swap is working
```bash
# 1. Watch which controlsd variant is running
watch -n 1 'ps -ef | grep -E "controlsd(_parked)?" | grep -v grep'
# 2. Watch the gating signals
watch -n 1 'echo "IgnitionOn:"; cat /data/params/d/IgnitionOn 2>/dev/null; \
echo; echo "deviceState.started:" ; \
python3 -c "import cereal.messaging as m; \
s=m.sub_sock(\"deviceState\", timeout=1000); \
print(m.recv_one(s).deviceState.started)"'
# 3. Watch gear via carState
python3 -c "
import cereal.messaging as m
s = m.sub_sock('carState', timeout=2000)
while True:
msg = m.recv_one(s)
if msg: print(msg.carState.gearShifter)
"
```
Expected behavior:
- Ignition off: neither variant in `ps`. `IgnitionOn` is `0`/missing.
- Ignition on, in Park: `controlsd_parked` in `ps`, `controlsd` is not.
`started` is False. After ~1.5s of confirmed Park, modeld and friends
should have stopped.
- Shift to Drive: `controlsd_parked` disappears within ~500ms, full
`controlsd` appears, all the onroad processes spin up. There will be
a brief carState gap during the swap (~0.52s).
### What to suspect first if something breaks
1. **Manager crash on startup** with `UnknownKeyName: IgnitionOn`. Means
`params.o`/`libcommon.a` weren't rebuilt. Delete them and rebuild.
2. **`controlsd_parked` keeps respawning / dying.** Check
`/data/log2/current/controlsd_parked.log`. Most likely either:
- `CarD.__init__` is hanging on `get_one_can` because pandad isn't
up yet — should only matter on the very first boot.
- A `frogpilot_variables` attribute we missed defaulting; add it to
`_make_default_frogpilot_variables` in `controlsd_parked.py`.
3. **Full controlsd never spawns after shift to Drive.** Check
`IgnitionOn` (should be `1`), check carState.gearShifter (should not
be `park`/`unknown`), check thermald.log for `should_start` logic.
Also check that `carState` is actually being published by
`controlsd_parked`.
4. **Steering still pulls right.** The whole point of commit 1 is to
rule out the variable-FPS work as the cause. If the symptom persists
on baseline-restored driving logic, the suspect list shifts to:
- Anything still on the kept side (carcontroller's
`no_lat_lane_change` plumbing, thermald-related changes,
custom.capnp additions).
- Hardware: a calibration that drifted, an alignment issue, panda
CAN bus issue, or torque tuning that was modified outside of these
files.
- The custom driving model selected (`Params("Model")`). Confirm
which `.thneed`/`.onnx` is loaded — none of the model files
themselves were changed in this session.
5. **Panda safety alerts during park transition.** If panda logs a
"lost heartbeat" or drops to NOOUTPUT mode in the swap window, we
need controlsd_parked to issue a no-op carcontrol heartbeat to keep
panda happy. Not implemented in this session — flag for follow-up.
### Open follow-up items (NOT done in this session)
- **Cold-start latency on shift-from-Park.** Modeld load + calibration
warmup may produce a noticeable gap before lateral/long are ready.
Anticipatory wake on `CS.brakePressed && in_park` is the planned
mitigation if needed.
- **Methodically reintroduce optimizations.** Once baseline driving
feels right, the standstill optimizations (modeld 1fps, fan clamps,
etc.) can come back one at a time, each with a drive test.
- **The CAN-FD telemetry block in `selfdrive/car/hyundai/carstate.py`**
is preserved as a commented block at the bottom of `update_canfd()`.
Re-enabling requires uncommenting + restoring the `tlog` import at
the top of the file.
---
## Key file index for fast navigation
```
selfdrive/controls/controlsd.py # full controlsd, restored to baseline + UI hooks re-added
selfdrive/controls/controlsd_parked.py # NEW: parked-only CAN listener
selfdrive/controls/clearpilot_state_control # in controlsd.py, ~line 1255 — 5-state ScreenDisplayMode + speed_state tick
selfdrive/thermald/thermald.py # gear-aware not_parked + IgnitionOn writer (~line 258)
selfdrive/manager/process_config.py # parked_only predicate + new entry
selfdrive/manager/manager.py # IgnitionOn seed in manager_init
common/params.cc # IgnitionOn registered (CLEAR_ON_MANAGER_START)
selfdrive/car/card.py # CarD class — used by both controlsd variants, unchanged
selfdrive/clearpilot/speed_logic.py # SpeedState class — unchanged, called from controlsd.clearpilot_state_control
selfdrive/car/hyundai/carstate.py # restored baseline + commented telemetry block at end of update_canfd
```
## Reproducing the diff per commit
```bash
git show 47321e3 # baseline restore
git show f7e602c # UI hooks
git show 887b9c9 # parked mode
git diff 62a403d..887b9c9 # full session delta
```