brianhansonxyz/clearpilot
1
0
Fork 0
Code Issues Pull Requests Actions 62 Packages Projects Releases Wiki Activity

adopt modelrevert tree as the new clearpilot baseline

Browse Source 
Replaces clearpilot's working state wholesale with the modelrevert branch's
tree (modelrevert tip cea422b). Discards the parked-controlsd manager-process
split and the two session READMEs that documented it; keeps the simpler
in-process park short-circuits (controlsd state_control, plannerd, frogpilot_process)
and the cached-output decimation (modeld, dmonitoringmodeld) that achieve
the same goal with less moving parts. Also brings in the locationd GPS
ignore, the calibrationd valid=calStatus gate, and the model-revert lineage's
controlsd / paramsd / torqued / events.py / carstate.py / interfaces.py.

This is a single new commit on clearpilot (no merge), so the branch advances
linearly while the file state matches modelrevert exactly.

Co-Authored-By: Claude Opus 4.7 (1M context) <noreply@anthropic.com>
This commit is contained in:
Brian Hanson
2026-04-26 12:17:35 -05:00
parent d639e28057
commit 8b4b7e04b5
21 changed files with 204 additions and 941 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/controls/controlsd.py
+68 -38
View File
@@ -37,10 +37,9 @@ 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.frogpilot.controls.lib.speed_limit_controller import SpeedLimitController
# CLEARPILOT: UI plumbing for ScreenDisplayMode and the speed/cruise-warning overlay
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
SOFT_DISABLE_TIME = 3 # seconds
LDW_MIN_SPEED = 31 * CV.MPH_TO_MS
@@ -50,7 +49,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"}
IGNORE_PROCESSES = {"loggerd", "encoderd", "statsd", "telemetryd", "dashcamd"}
ThermalStatus = log.DeviceState.ThermalStatus
State = log.ControlsState.OpenpilotState
@@ -80,14 +79,13 @@ 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/cruise-warning overlay state, ticked at ~2Hz from clearpilot_state_control
# CLEARPILOT: speed-limit/cruise-warning state machine + park→drive auto-reset
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
@@ -451,6 +449,13 @@ 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)
@@ -464,9 +469,11 @@ 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 len(self.sm['radarState'].radarErrors) or (not self.rk.lagging and not self.sm.all_checks(['radarState'])):
if not model_suppress and (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)
@@ -478,13 +485,16 @@ 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:
# CLEARPILOT: fire commIssue ONLY when messages actually aren't flowing (not_alive)
# or CAN RX is timing out. Don't fire on self-declared valid=False — that's the
# polling-pattern / all_checks cascade that paramsd/torqued/plannerd/frogpilot
# propagate even while their publish rate and content are fine.
comms_really_broken = (not self.sm.all_alive()) or self.card.can_rcv_timeout
if comms_really_broken and no_system_errors and not model_suppress:
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)
else:
self.events.add(EventName.commIssue) # can_rcv_timeout path
logs = {
'invalid': [s for s, valid in self.sm.valid.items() if not valid],
@@ -499,13 +509,13 @@ class Controls:
self.logged_comm_issue = None
if not (self.CP.notCar and self.joystick_mode):
if not self.sm['liveLocationKalman'].posenetOK:
if not self.sm['liveLocationKalman'].posenetOK and not model_suppress:
self.events.add(EventName.posenetInvalid)
if not self.sm['liveLocationKalman'].deviceStable:
self.events.add(EventName.deviceFalling)
if not self.sm['liveLocationKalman'].inputsOK:
if not self.sm['liveLocationKalman'].inputsOK and not model_suppress:
self.events.add(EventName.locationdTemporaryError)
if not self.sm['liveParameters'].valid and not TESTING_CLOSET and (not SIMULATION or REPLAY):
if not self.sm['liveParameters'].valid and not TESTING_CLOSET and (not SIMULATION or REPLAY) and not model_suppress:
self.events.add(EventName.paramsdTemporaryError)
# conservative HW alert. if the data or frequency are off, locationd will throw an error
@@ -551,7 +561,7 @@ class Controls:
self.distance_traveled = 0
self.distance_traveled += CS.vEgo * DT_CTRL
if self.sm['modelV2'].frameDropPerc > 20:
if self.sm['modelV2'].frameDropPerc > 20 and not model_suppress:
self.events.add(EventName.modeldLagging)
@@ -638,6 +648,14 @@ 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)
@@ -647,6 +665,25 @@ class Controls:
def state_control(self, CS):
"""Given the state, this function returns a CarControl packet"""
# CLEARPILOT: short-circuit while parked. Skip LaC/LoC PID, MPC, model_v2
# reads, lane-change logic — none of it matters when the car isn't moving.
# publish_logs still runs and still triggers carcontroller.apply via
# card.controls_update, so the sendcan heartbeats / tester-present messages
# keep flowing at 100Hz and the car doesn't fault. Saves ~30% controlsd CPU
# in park.
if CS.gearShifter == car.CarState.GearShifter.park:
CC = car.CarControl.new_message()
CC.enabled = False
CC.latActive = False
CC.longActive = False
CC.actuators.longControlState = self.LoC.long_control_state
self.LaC.reset()
self.LoC.reset(v_pid=CS.vEgo)
self.frogpilot_variables.no_lat_lane_change = False
self.FPCC.noLatLaneChange = False
lac_log = log.ControlsState.LateralDebugState.new_message()
return CC, lac_log
# Update VehicleModel
lp = self.sm['liveParameters']
x = max(lp.stiffnessFactor, 0.1)
@@ -1245,47 +1282,40 @@ 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)
# 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)
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: 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:
# CLEARPILOT: auto-reset display when shifting into drive from screen-off
if self.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 = driving_gear
self.was_driving_gear = self.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 driving_gear:
if self.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: anything except 3 → 3 (screen off), state 3 → 0 (auto)
# Not in drive: any except 3 → 3, state 3 → 0
new_mode = 0 if current == 3 else 3
self.params_memory.put_int("ScreenDisplayMode", new_mode)
# Speed/cruise-warning overlay tick (~2Hz at 100Hz loop)
# 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.
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") or 0.0
is_metric = self.is_metric
speed_limit_ms = self.params_memory.get_float("CarSpeedLimit")
is_metric = (self.params_memory.get("CarIsMetric", encoding="utf-8") or "0") == "1"
cruise_speed_ms = CS.cruiseState.speed
cruise_active = CS.cruiseState.enabled
cruise_standstill = CS.cruiseState.standstill