controlsd: suppress controlsdLagging for 3s after engagement
The state transition into State.enabled briefly bumps the controlsd loop time over its budget, which trips rk.lagging and triggers the 'Controls Process Lagging: Reboot Your Device' NoEntryAlert right as the user takes their hands off the wheel. The lag clears within a couple cycles and self-driving continues fine. Track the rising edge of self.enabled, and skip the controlsdLagging event for the first 3 seconds after engagement. All other lag triggers (unrelated to engagement) still fire normally.
This commit is contained in:
@@ -174,6 +174,12 @@ class Controls:
|
|||||||
self.state = State.disabled
|
self.state = State.disabled
|
||||||
self.enabled = False
|
self.enabled = False
|
||||||
self.active = False
|
self.active = False
|
||||||
|
# CLEARPILOT: track engagement edge for the post-engage controlsdLagging
|
||||||
|
# suppression window (the loop briefly lags during state transition into
|
||||||
|
# enabled, which would otherwise pop "Controls Process Lagging: Reboot
|
||||||
|
# Your Device" right as the user lets the wheel go).
|
||||||
|
self.last_engaged_frame = -1
|
||||||
|
self.POST_ENGAGE_LAG_GRACE_FRAMES = int(3.0 / DT_CTRL)
|
||||||
self.soft_disable_timer = 0
|
self.soft_disable_timer = 0
|
||||||
self.mismatch_counter = 0
|
self.mismatch_counter = 0
|
||||||
self.cruise_mismatch_counter = 0
|
self.cruise_mismatch_counter = 0
|
||||||
@@ -486,7 +492,14 @@ class Controls:
|
|||||||
elif not self.sm.all_freq_ok(self.camera_packets):
|
elif not self.sm.all_freq_ok(self.camera_packets):
|
||||||
self.events.add(EventName.cameraFrameRate)
|
self.events.add(EventName.cameraFrameRate)
|
||||||
if not REPLAY and self.rk.lagging:
|
if not REPLAY and self.rk.lagging:
|
||||||
self.events.add(EventName.controlsdLagging)
|
# CLEARPILOT: suppress controlsdLagging for the first 3 seconds after
|
||||||
|
# engagement — the loop briefly lags during the state transition into
|
||||||
|
# enabled, which otherwise pops a scary "Reboot Your Device" alert
|
||||||
|
# just as the user is letting their hands off the wheel.
|
||||||
|
in_post_engage_lag_grace = (self.last_engaged_frame >= 0
|
||||||
|
and (self.sm.frame - self.last_engaged_frame) < self.POST_ENGAGE_LAG_GRACE_FRAMES)
|
||||||
|
if not in_post_engage_lag_grace:
|
||||||
|
self.events.add(EventName.controlsdLagging)
|
||||||
if not self.radarless_model:
|
if not self.radarless_model:
|
||||||
if len(self.sm['radarState'].radarErrors) or (not self.rk.lagging and not self.sm.all_checks(['radarState'])):
|
if len(self.sm['radarState'].radarErrors) or (not self.rk.lagging and not self.sm.all_checks(['radarState'])):
|
||||||
self.events.add(EventName.radarFault)
|
self.events.add(EventName.radarFault)
|
||||||
@@ -658,8 +671,12 @@ class Controls:
|
|||||||
self.v_cruise_helper.initialize_v_cruise(CS, self.experimental_mode, self.sm['frogpilotPlan'].unconfirmedSlcSpeedLimit, self.frogpilot_variables)
|
self.v_cruise_helper.initialize_v_cruise(CS, self.experimental_mode, self.sm['frogpilotPlan'].unconfirmedSlcSpeedLimit, self.frogpilot_variables)
|
||||||
|
|
||||||
# Check if openpilot is engaged and actuators are enabled
|
# Check if openpilot is engaged and actuators are enabled
|
||||||
|
enabled_prev = self.enabled
|
||||||
self.enabled = self.state in ENABLED_STATES
|
self.enabled = self.state in ENABLED_STATES
|
||||||
self.active = self.state in ACTIVE_STATES
|
self.active = self.state in ACTIVE_STATES
|
||||||
|
# CLEARPILOT: capture rising edge of engagement for the lag-grace window.
|
||||||
|
if self.enabled and not enabled_prev:
|
||||||
|
self.last_engaged_frame = self.sm.frame
|
||||||
if self.active:
|
if self.active:
|
||||||
self.current_alert_types.append(ET.WARNING)
|
self.current_alert_types.append(ET.WARNING)
|
||||||
|
|
||||||
|
|||||||
Reference in New Issue
Block a user