From 83aed16a35b016acb99d70b383dd40e1459cb5ac Mon Sep 17 00:00:00 2001 From: Brian Hanson Date: Thu, 16 Apr 2026 22:35:16 -0500 Subject: [PATCH] perf: suppress controlsdLagging on engage + gate frogpilotCarControl send MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 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) --- selfdrive/controls/controlsd.py | 22 +++++++++++++++++----- 1 file changed, 17 insertions(+), 5 deletions(-) diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 01214f7..1632a9e 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -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()