feat: ramp-up delay on lat engagement to prevent commIssue flash
Decouples "tell modeld to go fast" from "steering actually active": - New LatRequested memory param — controlsd writes when lat would be active - modeld reads LatRequested (not carControl.latActive) for FPS decision, so it switches to 20fps immediately on engage request - controlsd delays CC.latActive becoming true by 250ms (5 frames @ 20fps) after LatRequested goes true, giving downstream services (longitudinalPlan, liveCalibration, etc.) time to stabilize at the new rate Co-Authored-By: Claude Opus 4.6 (1M context) <noreply@anthropic.com>
This commit is contained in:
@@ -172,6 +172,8 @@ class Controls:
|
||||
self.not_running_prev = None
|
||||
self.last_lat_engage_ts = 0 # CLEARPILOT: timestamp of most recent lat engage transition
|
||||
self.prev_lat_active = False
|
||||
self.lat_requested_ts = 0 # CLEARPILOT: timestamp when lat was first requested (for ramp-up delay)
|
||||
self.prev_lat_requested = False
|
||||
self.steer_limited = False
|
||||
self.desired_curvature = 0.0
|
||||
self.experimental_mode = False
|
||||
@@ -695,8 +697,17 @@ class Controls:
|
||||
|
||||
# Check which actuators can be enabled
|
||||
standstill = CS.vEgo <= max(self.CP.minSteerSpeed, MIN_LATERAL_CONTROL_SPEED) or CS.standstill
|
||||
CC.latActive = (self.active or self.FPCC.alwaysOnLateral) and self.speed_check and not CS.steerFaultTemporary and not CS.steerFaultPermanent and \
|
||||
(not standstill or self.joystick_mode)
|
||||
lat_requested = (self.active or self.FPCC.alwaysOnLateral) and self.speed_check and not CS.steerFaultTemporary and not CS.steerFaultPermanent and \
|
||||
(not standstill or self.joystick_mode)
|
||||
|
||||
# CLEARPILOT: tell modeld early so it can ramp to 20fps, then delay latActive by
|
||||
# 250ms (5 frames at 20fps) so downstream services stabilize before steering engages.
|
||||
self.params_memory.put_bool("LatRequested", lat_requested)
|
||||
now_ts = time.monotonic()
|
||||
if lat_requested and not self.prev_lat_requested:
|
||||
self.lat_requested_ts = now_ts
|
||||
self.prev_lat_requested = lat_requested
|
||||
CC.latActive = lat_requested and (now_ts - self.lat_requested_ts) >= 0.25
|
||||
CC.longActive = self.enabled and not self.events.contains(ET.OVERRIDE_LONGITUDINAL) and self.CP.openpilotLongitudinalControl
|
||||
|
||||
actuators = CC.actuators
|
||||
|
||||
Reference in New Issue
Block a user