diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 5e91138..1f330e2 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -88,9 +88,18 @@ class Controls: # exiting park, stay in keepalive-tick mode until SubMaster reports # everything healthy again, with an 8-second hard cap so we never get # stuck if some service is permanently broken. + # + # Park mode is suppressed for the first 10 seconds after init completes: + # card.initialize() runs in data_sample's not-initialized branch and is + # what hooks up CarInterface so controls_update actually generates CAN + # messages. If we entered park mode before that, the "heartbeat" would + # be silent — and only_onroad_active processes would never get a chance + # to come up cleanly the first time. self.park_mode = False self.park_exit_frame = -1 + self.startup_complete_frame = -1 self.PARK_GRACE_MAX_FRAMES = int(8.0 / DT_CTRL) + self.PARK_STARTUP_DELAY_FRAMES = int(10.0 / DT_CTRL) self.radarless_model = self.params.get("Model", encoding='utf-8') in RADARLESS_MODELS @@ -1259,9 +1268,24 @@ class Controls: if (len(CS.buttonEvents) > 0): print (CS.buttonEvents) + def _park_mode_allowed(self): + # Don't allow park mode until init has completed AND we've run at least + # 10s of normal step() after init. card.initialize() (which hooks + # CarInterface up to send CAN) only runs on the init path, so we need + # to take that path at least once before short-circuiting into the + # minimal tick. The 10s buffer also lets only_onroad_active processes + # come up cleanly the first time before we ask manager to pause them. + if not self.initialized: + return False + if self.startup_complete_frame < 0: + self.startup_complete_frame = self.sm.frame + return (self.sm.frame - self.startup_complete_frame) >= self.PARK_STARTUP_DELAY_FRAMES + def _update_park_mode(self, CS): # CLEARPILOT: track the gear-park flag and write ParkMode for manager. # On park→drive transition, capture the frame so the grace window starts. + if not self._park_mode_allowed(): + return in_park = CS.gearShifter == GearShifter.park if in_park != self.park_mode: self.park_mode = in_park