Quick Nav

system/manager/manager.py

@@ -45,6 +45,7 @@ from openpilot.starpilot.common.starpilot_variables import (
   LEGACY_STARPILOT_STATS_KEY_RENAMES,
   get_starpilot_toggles,
 )
+from openpilot.starpilot.navigation.destination_store import activate_next_drive_destination, is_next_drive_destination
 
 _MANAGER_IMPORT_DONE = time.monotonic()
 _manager_import_timing_line = (
@@ -135,7 +136,7 @@ def get_nav_offroad_clear_timeout_seconds(params) -> int:
 
 def update_nav_offroad_clear_state(params, started: bool, tracked_destination, tracked_started_at, now: float):
   nav_destination = params.get("NavDestination")
-  if started or not params.get_bool("ClearNavOnOffroad") or not nav_destination:
+  if started or not params.get_bool("ClearNavOnOffroad") or not nav_destination or is_next_drive_destination(nav_destination):
     return None, None
 
   if nav_destination != tracked_destination or tracked_started_at is None:
@@ -143,6 +144,9 @@ def update_nav_offroad_clear_state(params, started: bool, tracked_destination, t
     tracked_started_at = now
 
   if now - tracked_started_at >= get_nav_offroad_clear_timeout_seconds(params):
+    current_destination = params.get("NavDestination")
+    if current_destination != nav_destination or is_next_drive_destination(current_destination):
+      return None, None
     params.remove("NavDestination")
     return None, None
 
@@ -1152,6 +1156,9 @@ def manager_thread() -> None:
       # StarPilot variables
       params_memory.clear_all(ParamKeyFlag.CLEAR_ON_OFFROAD_TRANSITION)
 
+    if started and not started_prev:
+      activate_next_drive_destination(params)
+
     offroad_nav_destination, offroad_nav_started_at = update_nav_offroad_clear_state(
       params, started, offroad_nav_destination, offroad_nav_started_at, time.monotonic()
     )

system/manager/test/test_manager.py

@@ -12,6 +12,7 @@ import openpilot.system.manager.manager as manager
 from openpilot.system.manager.process import ensure_running
 from openpilot.system.manager.process_config import BigDeviceUIProcess, managed_processes, procs
 from openpilot.system.hardware import HARDWARE
+from openpilot.starpilot.navigation.destination_store import START_ON_NEXT_DRIVE_KEY
 
 os.environ['FAKEUPLOAD'] = "1"
 
@@ -71,6 +72,103 @@ class FileBackedFakeParams:
   def put_float(self, key, value):
     self.put(key, float(value))
 
+  def remove(self, key):
+    Path(self.get_param_path(key)).unlink(missing_ok=True)
+
+
+def test_offroad_navigation_cleanup_preserves_next_drive_destination(tmp_path):
+  destination = {
+    "name": "Home",
+    "place_name": "Home",
+    "latitude": 1.0,
+    "longitude": 2.0,
+    START_ON_NEXT_DRIVE_KEY: True,
+  }
+  params = FileBackedFakeParams(tmp_path / "params", {
+    "ClearNavOnOffroad": True,
+    "ClearNavOnOffroadTimeoutMinutes": 0,
+    "NavDestination": destination,
+  })
+
+  state = manager.update_nav_offroad_clear_state(params, False, None, None, 10.0)
+
+  assert state == (None, None)
+  assert json.loads(params.get("NavDestination")) == destination
+
+
+def test_next_drive_selection_disarms_delayed_cleanup_for_same_destination(tmp_path):
+  active_destination = {
+    "name": "Home",
+    "place_name": "Home",
+    "latitude": 1.0,
+    "longitude": 2.0,
+  }
+  params = FileBackedFakeParams(tmp_path / "params", {
+    "ClearNavOnOffroad": True,
+    "ClearNavOnOffroadTimeoutMinutes": 15,
+    "NavDestination": active_destination,
+  })
+
+  tracked = manager.update_nav_offroad_clear_state(params, False, None, None, 10.0)
+  assert tracked == (params.get("NavDestination"), 10.0)
+
+  params.put("NavDestination", {**active_destination, START_ON_NEXT_DRIVE_KEY: True})
+  state = manager.update_nav_offroad_clear_state(params, False, *tracked, 20.0)
+
+  assert state == (None, None)
+  assert json.loads(params.get("NavDestination"))[START_ON_NEXT_DRIVE_KEY] is True
+
+
+def test_unmarked_navigation_destination_still_clears_immediately_offroad(tmp_path):
+  params = FileBackedFakeParams(tmp_path / "params", {
+    "ClearNavOnOffroad": True,
+    "ClearNavOnOffroadTimeoutMinutes": 0,
+    "NavDestination": {"name": "Home", "latitude": 1.0, "longitude": 2.0},
+  })
+
+  state = manager.update_nav_offroad_clear_state(params, False, None, None, 10.0)
+
+  assert state == (None, None)
+  assert params.get("NavDestination") is None
+
+
+def test_offroad_cleanup_does_not_remove_destination_replaced_after_snapshot(tmp_path):
+  old_destination = json.dumps({"name": "Old", "latitude": 1.0, "longitude": 2.0})
+  next_drive_destination = {
+    "name": "Home",
+    "place_name": "Home",
+    "latitude": 3.0,
+    "longitude": 4.0,
+    START_ON_NEXT_DRIVE_KEY: True,
+  }
+
+  class SnapshotRaceParams(FileBackedFakeParams):
+    def __init__(self, root, values):
+      self._first_nav_read = old_destination
+      self.removed = []
+      super().__init__(root, values)
+
+    def get(self, key):
+      if key == "NavDestination" and self._first_nav_read is not None:
+        value, self._first_nav_read = self._first_nav_read, None
+        return value
+      return super().get(key)
+
+    def remove(self, key):
+      self.removed.append(key)
+      super().remove(key)
+
+  params = SnapshotRaceParams(tmp_path / "params", {
+    "ClearNavOnOffroad": True,
+    "ClearNavOnOffroadTimeoutMinutes": 0,
+    "NavDestination": next_drive_destination,
+  })
+
+  state = manager.update_nav_offroad_clear_state(params, False, None, None, 10.0)
+
+  assert state == (None, None)
+  assert "NavDestination" not in params.removed
+
 
 class FakeManagedProcess:
   def __init__(self):