diff --git a/common/params.cc b/common/params.cc index b05b46b..1f4908a 100755 --- a/common/params.cc +++ b/common/params.cc @@ -201,6 +201,7 @@ std::unordered_map keys = { {"TelemetryEnabled", PERSISTENT}, {"Timezone", PERSISTENT}, {"TrainingVersion", PERSISTENT}, + {"LatRequested", PERSISTENT}, {"ModelFps", PERSISTENT}, {"ModelStandby", PERSISTENT}, {"ModelStandbyTs", PERSISTENT}, diff --git a/selfdrive/clearpilot/dashcamd b/selfdrive/clearpilot/dashcamd index 828d6f5..8b1d7ef 100755 Binary files a/selfdrive/clearpilot/dashcamd and b/selfdrive/clearpilot/dashcamd differ diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 22bb698..09fe5d3 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -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 diff --git a/selfdrive/manager/manager.py b/selfdrive/manager/manager.py index dd6b810..953acea 100755 --- a/selfdrive/manager/manager.py +++ b/selfdrive/manager/manager.py @@ -89,6 +89,7 @@ def manager_init(frogpilot_functions) -> None: params_memory.put("VpnEnabled", "1") params_memory.put("DashcamFrames", "0") params_memory.put("DashcamState", "stopped") + params_memory.put("LatRequested", "0") params_memory.put("ModelFps", "20") params_memory.put("ModelStandby", "0") params_memory.put("ShutdownTouchReset", "0") diff --git a/selfdrive/modeld/modeld.py b/selfdrive/modeld/modeld.py index 10fc514..fd9c790 100755 --- a/selfdrive/modeld/modeld.py +++ b/selfdrive/modeld/modeld.py @@ -242,10 +242,12 @@ def main(demo=False): sm.update(0) # CLEARPILOT: power saving — three modes based on driving state - # Full 20fps: lat active or lane changing - # Reduced 4fps: not lat active, not standstill (driving without cruise) + # Full 20fps: lat requested or lane changing + # Reduced 4fps: not lat requested, not standstill (driving without cruise) # Standby 0fps: standstill - lat_active = sm['carControl'].latActive + # Uses LatRequested (not carControl.latActive) so modeld ramps to 20fps BEFORE + # controlsd actually engages steering — gives downstream services time to stabilize. + lat_active = params_memory.get_bool("LatRequested") lane_changing = params_memory.get_bool("no_lat_lane_change") standstill = sm['carState'].standstill calibrating = sm['liveCalibration'].calStatus != log.LiveCalibrationData.Status.calibrated diff --git a/selfdrive/ui/qt/spinner b/selfdrive/ui/qt/spinner index 0f2b116..53b0b3e 100755 Binary files a/selfdrive/ui/qt/spinner and b/selfdrive/ui/qt/spinner differ