From 6a79996a14c877015a4f8801d6aea1b20d497ba8 Mon Sep 17 00:00:00 2001 From: Brian Hanson Date: Sat, 18 Apr 2026 13:46:33 -0500 Subject: [PATCH] park CPU savings + fix early shutdown on virtual battery capacity MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit controlsd: in park+ignition-on, run full step() only every 10th cycle. data_sample still runs every cycle (CAN parse, button detection) and card.controls_update still runs every cycle (CAN TX heartbeat, counter increments). Skipped cycles re-send cached controlsState/carControl so downstream freq_ok stays OK. Button edge handling + display-mode transitions extracted to handle_screen_mode() and called every cycle so debug-button presses aren't dropped. controlsd: 55% → 30% CPU in park. dmonitoringmodeld: subscribe to carState; at standstill, skip model.run and re-publish last inference. driverStateV2 continues flowing at 10Hz with known-good last face data (driver can't become distracted relative to a stopped car). ~5% CPU saved. power_monitoring: remove the `car_battery_capacity_uWh <= 0` shutdown trigger. That virtual capacity counter floor-limits to 3e6 µWh on boot and drains in ~12 min at typical device power, so a short drive (that doesn't fully recharge the 30e6 µWh virtual cap) followed by a quick store stop would trip shutdown well before the 30-min idle timer. The real car-battery-voltage protection (low_voltage_shutdown at 11.8V with 60s debounce) is kept. Co-Authored-By: Claude Opus 4.7 (1M context) --- selfdrive/controls/controlsd.py | 74 ++++++++++++++++++++------ selfdrive/modeld/dmonitoringmodeld.py | 16 +++++- selfdrive/thermald/power_monitoring.py | 8 ++- 3 files changed, 78 insertions(+), 20 deletions(-) diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 5674a5b..44a32a9 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -184,6 +184,12 @@ class Controls: self._hyst_paramsd_tmp = 0 self._hyst_posenet = 0 self.HYST_CYCLES = 5 + # CLEARPILOT: parked-cycle skip — in park+ignition-on, run the full step() only + # every 10th cycle. CAN parse + CAN TX still happen every cycle; outer logic + # (events, state machine, PID/MPC, publishing) runs at 10Hz. Cached message + # bytes are re-published on skipped cycles so downstream freq_ok stays OK. + self._cached_controlsState_bytes = None + self._cached_carControl_bytes = None self.steer_limited = False self.desired_curvature = 0.0 self.experimental_mode = False @@ -258,27 +264,50 @@ class Controls: CS = self.data_sample() cloudlog.timestamp("Data sampled") - self.update_events(CS) - self.update_frogpilot_events(CS) - self.update_clearpilot_events(CS) + # CLEARPILOT: handle debug-button press + display-mode transitions on every + # cycle — button edge events only live in the cycle's CS.buttonEvents and + # would otherwise be dropped on skipped cycles. + self.handle_screen_mode(CS) - cloudlog.timestamp("Events updated") + # CLEARPILOT: in park, only run the full step() every 10th cycle. data_sample + # above still runs (CAN parse, button detection). Below, card.controls_update + # still runs (CAN TX heartbeat, counters increment). The skipped outer logic + # (events, state machine, PID/MPC, publishing) causes at most ~100ms lag on + # button→state transitions, which is fine in park. Cached message bytes are + # re-sent so downstream consumers see steady 100Hz. + parked = CS.gearShifter == car.CarState.GearShifter.park + full_cycle = (not parked) or (self.sm.frame % 10 == 0) or (self._cached_controlsState_bytes is None) - if not self.CP.passive and self.initialized: - # Update control state - self.state_transition(CS) + if full_cycle: + self.update_events(CS) + self.update_frogpilot_events(CS) + self.update_clearpilot_events(CS) - # Compute actuators (runs PID loops and lateral MPC) - CC, lac_log = self.state_control(CS) - CC = self.clearpilot_state_control(CC, CS) + cloudlog.timestamp("Events updated") - # Publish data - self.publish_logs(CS, start_time, CC, lac_log) + if not self.CP.passive and self.initialized: + # Update control state + self.state_transition(CS) - self.CS_prev = CS + # Compute actuators (runs PID loops and lateral MPC) + CC, lac_log = self.state_control(CS) + CC = self.clearpilot_state_control(CC, CS) - # Update FrogPilot variables - self.update_frogpilot_variables(CS) + # Publish data (also sends CAN TX via card.controls_update inside) + self.publish_logs(CS, start_time, CC, lac_log) + + self.CS_prev = CS + + # Update FrogPilot variables + self.update_frogpilot_variables(CS) + else: + # CAN TX heartbeat: keep counters incrementing and CAN frames flowing to the car + if not self.CP.passive and self.initialized: + self.card.controls_update(self.CC, self.frogpilot_variables) + # Re-publish cached messages so downstream freq_ok checks don't trip + self.pm.send('controlsState', self._cached_controlsState_bytes) + self.pm.send('carControl', self._cached_carControl_bytes) + self.CS_prev = CS def data_sample(self): @@ -1002,6 +1031,8 @@ class Controls: controlsState.lateralControlState.torqueState = lac_log self.pm.send('controlsState', dat) + # CLEARPILOT: cache for re-publication on parked-skip cycles + self._cached_controlsState_bytes = dat.to_bytes() # onroadEvents - logged every second or on change if (self.sm.frame % int(1. / DT_CTRL) == 0) or (self.events.names != self.events_prev): @@ -1016,6 +1047,7 @@ class Controls: cc_send.valid = CS.canValid cc_send.carControl = CC self.pm.send('carControl', cc_send) + self._cached_carControl_bytes = cc_send.to_bytes() # copy CarControl to pass to CarInterface on the next iteration self.CC = CC @@ -1337,8 +1369,14 @@ class Controls: 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 + def handle_screen_mode(self, CS): + """CLEARPILOT: tracks driving_gear, auto-resets display, and cycles + ScreenDisplayMode on debug-button presses. Must run every cycle so button + edge events aren't lost during parked-skip mode (edges happen in carstate + edge detection and only appear in that one cycle's CS.buttonEvents).""" + self.driving_gear = CS.gearShifter not in (GearShifter.neutral, GearShifter.park, GearShifter.reverse, GearShifter.unknown) + + # auto-reset display when shifting into drive from screen-off if self.driving_gear and not self.was_driving_gear: if self.params_memory.get_int("ScreenDisplayMode") == 3: self.params_memory.put_int("ScreenDisplayMode", 0) @@ -1357,6 +1395,8 @@ class Controls: self.params_memory.put_int("ScreenDisplayMode", new_mode) + def clearpilot_state_control(self, CC, CS): + # ClearPilot speed processing (~2 Hz at 100 Hz loop) self.speed_state_frame += 1 if self.speed_state_frame % 50 == 0: diff --git a/selfdrive/modeld/dmonitoringmodeld.py b/selfdrive/modeld/dmonitoringmodeld.py index ef403b4..9476fba 100755 --- a/selfdrive/modeld/dmonitoringmodeld.py +++ b/selfdrive/modeld/dmonitoringmodeld.py @@ -128,10 +128,14 @@ def main(): assert vipc_client.is_connected() cloudlog.warning(f"connected with buffer size: {vipc_client.buffer_len}") - sm = SubMaster(["liveCalibration"]) + sm = SubMaster(["liveCalibration", "carState"]) pm = PubMaster(["driverStateV2"]) calib = np.zeros(CALIB_LEN, dtype=np.float32) + # CLEARPILOT: cache last model output so we can republish (not re-infer) at standstill. + # Saves ~7% CPU; downstream dmonitoringd sees a steady 10Hz stream with known-good + # last readings (driver can't become distracted relative to a stopped car). + last_model_output = None # last = 0 while True: @@ -143,8 +147,16 @@ def main(): if sm.updated["liveCalibration"]: calib[:] = np.array(sm["liveCalibration"].rpyCalib) + standstill = sm["carState"].standstill + t1 = time.perf_counter() - model_output, dsp_execution_time = model.run(buf, calib) + if standstill and last_model_output is not None: + # CLEARPILOT: reuse last inference at standstill + model_output = last_model_output + dsp_execution_time = 0.0 + else: + model_output, dsp_execution_time = model.run(buf, calib) + last_model_output = model_output t2 = time.perf_counter() pm.send("driverStateV2", get_driverstate_packet(model_output, vipc_client.frame_id, vipc_client.timestamp_sof, t2 - t1, dsp_execution_time)) diff --git a/selfdrive/thermald/power_monitoring.py b/selfdrive/thermald/power_monitoring.py index 3163ac2..0d7d82c 100755 --- a/selfdrive/thermald/power_monitoring.py +++ b/selfdrive/thermald/power_monitoring.py @@ -122,7 +122,13 @@ class PowerMonitoring: offroad_time > VOLTAGE_SHUTDOWN_MIN_OFFROAD_TIME_S) should_shutdown |= offroad_time > self.device_shutdown_time should_shutdown |= low_voltage_shutdown - should_shutdown |= (self.car_battery_capacity_uWh <= 0) + # CLEARPILOT: removed `car_battery_capacity_uWh <= 0` trigger. That's a virtual + # capacity counter floor-limited to 3e6 µWh on boot which drains in ~12 min at + # typical device power. With a short drive that doesn't fully recharge (charges + # at 45W, cap 30e6 µWh = 36 min to full), a quick store stop could trip shutdown + # well before the intended 30-min idle timer. The real protection we want here + # is the car battery voltage check (kept above) — the virtual counter is now + # retained only for telemetry. should_shutdown &= not ignition should_shutdown &= (not self.params.get_bool("DisablePowerDown")) should_shutdown &= in_car