adopt pre-modelrevert clearpilot tree (d639e28) as the new head
Discard the modelrevert tree adoption (8b4b7e0) and the in-process park short-circuits / cached-output / dashcam-idle work that came with it (0dc8002,37e095e). Restore the clearpilot tree as it stood atd639e28— the parked-controlsd manager-process split, the GPS-disable in locationd, the controlsd UI hooks, the boardd ignition-edge safety_setter_thread fix. After a full /data/params/d wipe and re-calibration drive, the modelrevert-tree variant overcorrected on turns; reverting to the parked-controlsd architecture (which Brian had previously vetted and documented in887b9c9+27cad05) and starting fresh. Single new commit, no merge — file state matchesd639e28byte-for-byte. Co-Authored-By: Claude Opus 4.7 (1M context) <noreply@anthropic.com>
This commit is contained in:
@@ -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,16 +478,13 @@ 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: 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_checks() or self.card.can_rcv_timeout) and no_system_errors:
|
||||
if not self.sm.all_alive():
|
||||
self.events.add(EventName.commIssue)
|
||||
else:
|
||||
self.events.add(EventName.commIssue) # can_rcv_timeout path
|
||||
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],
|
||||
@@ -509,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
|
||||
@@ -561,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)
|
||||
|
||||
|
||||
@@ -648,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)
|
||||
|
||||
@@ -665,30 +647,6 @@ 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
|
||||
# Call LaC.update with active=False so we get the right lac_log subtype
|
||||
# for this car's lateralTuning (torque vs pid vs angle). Internally it
|
||||
# early-returns when active is False — cheap.
|
||||
lp = self.sm['liveParameters']
|
||||
_, _, lac_log = self.LaC.update(False, CS, self.VM, lp, self.steer_limited, 0.0,
|
||||
self.sm['liveLocationKalman'], model_data=self.sm['modelV2'])
|
||||
return CC, lac_log
|
||||
|
||||
# Update VehicleModel
|
||||
lp = self.sm['liveParameters']
|
||||
x = max(lp.stiffnessFactor, 0.1)
|
||||
@@ -1287,40 +1245,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
|
||||
|
||||
Reference in New Issue
Block a user