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()