modeld: revert to constant 20fps (except standby at standstill)
Disabled the variable-rate (4/10/20fps) logic — it caused persistent downstream freq/valid cascades that broke paramsd's angle-offset learner, calibrationd, and locationd's one-shot filterInitialized latch. Symptom: user's "TAKE CONTROL IMMEDIATELY / Communication Issue" banner during engaged driving, plus subtle right-pull (paramsd's fast angle-offset adaptation was frozen all session). Simpler model now: standstill → standby (0fps, paused) otherwise → 20fps (no variable rate) Republish-caching and FPCC.latRequested/noLatLaneChange plumbing left in place so re-enabling variable rate later is a one-line change (full_rate = True → the original full_rate expression). Co-Authored-By: Claude Opus 4.6 (1M context) <noreply@anthropic.com>
This commit is contained in:
@@ -23,7 +23,7 @@ from openpilot.selfdrive.car.car_helpers import get_startup_event
|
||||
from openpilot.selfdrive.car.card import CarD
|
||||
from openpilot.selfdrive.controls.lib.alertmanager import AlertManager, set_offroad_alert
|
||||
from openpilot.selfdrive.controls.lib.drive_helpers import VCruiseHelper, clip_curvature
|
||||
from openpilot.selfdrive.controls.lib.events import Events, ET
|
||||
from openpilot.selfdrive.controls.lib.events import Events, ET, EVENTS, EVENT_NAME
|
||||
from openpilot.selfdrive.controls.lib.latcontrol import LatControl, MIN_LATERAL_CONTROL_SPEED
|
||||
from openpilot.selfdrive.controls.lib.latcontrol_pid import LatControlPID
|
||||
from openpilot.selfdrive.controls.lib.latcontrol_angle import LatControlAngle, STEER_ANGLE_SATURATION_THRESHOLD
|
||||
@@ -172,6 +172,7 @@ class Controls:
|
||||
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
|
||||
self._dbg_prev_bad_events: tuple = () # CLEARPILOT: track set of alert-triggering events for debug dump
|
||||
# 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
|
||||
@@ -255,6 +256,11 @@ class Controls:
|
||||
|
||||
cloudlog.timestamp("Events updated")
|
||||
|
||||
# CLEARPILOT: debug dumper — find every cycle where an event with SOFT_DISABLE or
|
||||
# IMMEDIATE_DISABLE is active (both produce the "TAKE CONTROL IMMEDIATELY" banner)
|
||||
# and log full context when the set of triggering events changes.
|
||||
self._dbg_dump_alert_triggers(CS)
|
||||
|
||||
if not self.CP.passive and self.initialized:
|
||||
# Update control state
|
||||
self.state_transition(CS)
|
||||
@@ -320,6 +326,74 @@ class Controls:
|
||||
|
||||
return CS
|
||||
|
||||
def _dbg_dump_alert_triggers(self, CS):
|
||||
"""CLEARPILOT: log full context whenever the set of SOFT_DISABLE/IMMEDIATE_DISABLE
|
||||
events changes. Both alert types show 'TAKE CONTROL IMMEDIATELY' to the user.
|
||||
Emits only on set change to avoid spamming the log.
|
||||
"""
|
||||
bad_events = []
|
||||
for e in self.events.names:
|
||||
ets = EVENTS.get(e, {})
|
||||
if ET.SOFT_DISABLE in ets or ET.IMMEDIATE_DISABLE in ets:
|
||||
bad_events.append(EVENT_NAME.get(e, str(e)))
|
||||
|
||||
bad_tuple = tuple(sorted(bad_events))
|
||||
if bad_tuple == self._dbg_prev_bad_events:
|
||||
return
|
||||
self._dbg_prev_bad_events = bad_tuple
|
||||
|
||||
try:
|
||||
avg_dt = sum(self.rk._dts) / len(self.rk._dts) if len(self.rk._dts) else 0.0
|
||||
except Exception:
|
||||
avg_dt = 0.0
|
||||
|
||||
fpcc_latReq = False
|
||||
fpcc_noLatLC = False
|
||||
try:
|
||||
fpcc = self.sm['frogpilotCarControl']
|
||||
fpcc_latReq = bool(fpcc.latRequested)
|
||||
fpcc_noLatLC = bool(fpcc.noLatLaneChange)
|
||||
except Exception:
|
||||
pass
|
||||
|
||||
cloudlog.event("TAKE_CONTROL_TRIGGER",
|
||||
error=True,
|
||||
events=list(bad_tuple),
|
||||
state=self.state.name if hasattr(self.state, 'name') else str(self.state),
|
||||
enabled=self.enabled,
|
||||
active=self.active,
|
||||
# suppression flags (set in update_events before this is called)
|
||||
model_suppress=getattr(self, '_dbg_model_suppress', None),
|
||||
lat_engage_suppress=getattr(self, '_dbg_lat_engage_suppress', None),
|
||||
standby_ts_age_s=round(getattr(self, '_dbg_standby_ts_age', -1), 3),
|
||||
# loop health
|
||||
rk_lagging=self.rk.lagging,
|
||||
rk_avg_dt_ms=round(avg_dt * 1000, 2),
|
||||
rk_remaining_ms=round(self.rk.remaining * 1000, 2),
|
||||
# submaster aggregate health
|
||||
sm_all_alive=self.sm.all_alive(),
|
||||
sm_all_freq_ok=self.sm.all_freq_ok(),
|
||||
sm_all_valid=self.sm.all_valid(),
|
||||
sm_invalid=[s for s, v in self.sm.valid.items() if not v],
|
||||
sm_not_alive=[s for s, a in self.sm.alive.items() if not a],
|
||||
sm_not_freq_ok=[s for s, f in self.sm.freq_ok.items() if not f],
|
||||
# vehicle state
|
||||
vEgo=round(CS.vEgo, 2),
|
||||
gear=str(CS.gearShifter),
|
||||
cruise_enabled=CS.cruiseState.enabled,
|
||||
standstill=CS.standstill,
|
||||
steerFaultTemporary=CS.steerFaultTemporary,
|
||||
steerFaultPermanent=CS.steerFaultPermanent,
|
||||
canValid=CS.canValid,
|
||||
canTimeout=CS.canTimeout,
|
||||
# car control state (previous cycle)
|
||||
prev_latActive=self.CC.latActive if self.CC else False,
|
||||
prev_longActive=self.CC.longActive if self.CC else False,
|
||||
fpcc_latRequested=fpcc_latReq,
|
||||
fpcc_noLatLaneChange=fpcc_noLatLC,
|
||||
frame=self.sm.frame,
|
||||
)
|
||||
|
||||
def update_events(self, CS):
|
||||
"""Compute onroadEvents from carState"""
|
||||
|
||||
@@ -466,8 +540,16 @@ class Controls:
|
||||
# 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()
|
||||
self._dbg_model_suppress = model_suppress
|
||||
self._dbg_lat_engage_suppress = lat_engage_suppress
|
||||
self._dbg_standby_ts_age = now - standby_ts
|
||||
|
||||
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)
|
||||
|
||||
Reference in New Issue
Block a user