park CPU savings + fix early shutdown on virtual battery capacity
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) <noreply@anthropic.com>
This commit is contained in:
@@ -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:
|
||||
|
||||
Reference in New Issue
Block a user