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

perf: suppress controlsdLagging on engage + gate frogpilotCarControl send

Browse Source 
Two related fixes for the post-engage lag spike:

1. controlsdLagging suppressed during lat_engage_suppress window.
   Ratekeeper.lagging triggers when avg cycle duration over 100 cycles
   exceeds 11.1ms (90% of 10ms budget). The modeld 10→20fps ramp causes
   a legitimate transient where downstream services (plannerd, locationd,
   calibrationd, paramsd) each drain 2x the message rate, briefly pushing
   avg cycle time past the threshold. The underlying system isn't broken —
   it's correctly absorbing a scheduled workload transition.

2. frogpilotCarControl now sends only on change (+ 1Hz keepalive) instead
   of every 10ms. The message has 3 bool fields, of which speedLimitChanged
   code is entirely commented out, trafficModeActive flips only on UI
   button press, and alwaysOnLateral changes only on cruise/gear/brake
   edges. plannerd doesn't include frogpilotCarControl in its all_checks
   list so stale-freq detection isn't a concern. Saves ~7ms/sec of
   capnp build + zmq send work.

Co-Authored-By: Claude Opus 4.6 (1M context) <noreply@anthropic.com>
This commit is contained in:
Brian Hanson
2026-04-16 22:35:16 -05:00
parent 6dede984dc
commit 83aed16a35
1 changed files with 17 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files

22
selfdrive/controls/controlsd.py
View File

@@ -173,6 +173,9 @@ class Controls:
self.lat_requested_ts = 0 # CLEARPILOT: timestamp when lat was first requested (ramp-up delay + comm issue suppression)
self.prev_lat_requested = False
self.prev_no_lat_lane_change = None # CLEARPILOT: gate no_lat_lane_change write on change
# CLEARPILOT: gate frogpilotCarControl send on change (3 static bools, was publishing 100Hz)
self._prev_fpcc = (None, None, None)
self._last_fpcc_send_frame = 0
self.steer_limited = False
self.desired_curvature = 0.0
self.experimental_mode = False
@@ -478,7 +481,10 @@ class Controls:
self.events.add(EventName.cameraMalfunction)
elif not self.sm.all_freq_ok(self.camera_packets):
self.events.add(EventName.cameraFrameRate)
if not REPLAY and self.rk.lagging:
# CLEARPILOT: also gate on model_suppress + lat_engage_suppress — the fps transition
# on engage briefly pushes avg cycle time over the 11.1ms (90% of 10ms) threshold
# while downstream services (plannerd, locationd) drain the sudden 5x message rate
if not REPLAY and self.rk.lagging and not model_suppress and not lat_engage_suppress:
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)
@@ -1201,10 +1207,16 @@ class Controls:
self.FPCC.trafficModeActive = self.frogpilot_variables.traffic_mode and self.params_memory.get_bool("TrafficModeActive")
fpcc_send = messaging.new_message('frogpilotCarControl')
fpcc_send.valid = CS.canValid
fpcc_send.frogpilotCarControl = self.FPCC
self.pm.send('frogpilotCarControl', fpcc_send)
# CLEARPILOT: send only when one of the 3 fields changes, with 1Hz keepalive
# Saves ~7ms/sec of capnp+zmq work (was sending 100Hz despite static contents)
fpcc_tuple = (self.FPCC.alwaysOnLateral, self.FPCC.speedLimitChanged, self.FPCC.trafficModeActive)
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
if self.params_memory.get_bool("FrogPilotTogglesUpdated"):
self.update_frogpilot_params()