Ford: close the loop on path curvature

Use the rolling path for pose and slow geometry while allocating jerk-limited requested curvature and bounded tracking error to the fast heading field. Prevent filtered C2 from reinforcing an unwind or reversal.

Assisted-by: Codex

openpilot/cereal/custom.capnp

@@ -382,6 +382,7 @@ struct CarControlSP @0xa5cd762cd951a455 {
   leadOne @2 :LeadData;
   leadTwo @3 :LeadData;
   intelligentCruiseButtonManagement @4 :IntelligentCruiseButtonManagement;
+  fordLateralPath @5 :FordLateralPath;
 
   struct Param {
     key @0 :Text;
@@ -402,6 +403,14 @@ struct CarControlSP @0xa5cd762cd951a455 {
   }
 }
 
+struct FordLateralPath {
+  pathOffset @0 :Float32;     # c0 [m]
+  pathAngle @1 :Float32;      # c1 [rad]
+  curvature @2 :Float32;      # c2 [1/m]
+  curvatureRate @3 :Float32;  # c3 [1/m^2]
+  valid @4 :Bool;
+}
+
 struct BackupManagerSP @0xf98d843bfd7004a3 {
   backupStatus @0 :Status;
   restoreStatus @1 :Status;

openpilot/common/params_keys.h

@@ -246,6 +246,7 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
 
     // mapd
     {"MapAdvisorySpeedLimit", {CLEAR_ON_ONROAD_TRANSITION, FLOAT}},
+    {"Mapd_ClearCache", {CLEAR_ON_MANAGER_START, BOOL}},
     {"MapdVersion", {PERSISTENT, STRING}},
     {"MapSpeedLimit", {CLEAR_ON_ONROAD_TRANSITION, FLOAT, "0.0"}},
     {"NextMapSpeedLimit", {CLEAR_ON_ONROAD_TRANSITION, JSON}},

openpilot/selfdrive/car/helpers.py

@@ -63,5 +63,6 @@ def convert_carControlSP(struct: capnp.lib.capnp._DynamicStructReader) -> struct
   struct_dataclass.intelligentCruiseButtonManagement = structs.IntelligentCruiseButtonManagement(
     **remove_deprecated(struct_dict.get('intelligentCruiseButtonManagement', {}))
   )
+  struct_dataclass.fordLateralPath = structs.FordLateralPath(**remove_deprecated(struct_dict.get('fordLateralPath', {})))
 
   return struct_dataclass

openpilot/selfdrive/controls/controlsd.py

@@ -13,6 +13,7 @@ from openpilot.common.swaglog import cloudlog
 from opendbc.car.car_helpers import interfaces
 from opendbc.car.vehicle_model import VehicleModel
 from openpilot.selfdrive.controls.lib.drive_helpers import clip_curvature
+from openpilot.selfdrive.controls.lib.ford_path import FordPath, FordPathController
 from openpilot.selfdrive.controls.lib.latcontrol import LatControl
 from openpilot.selfdrive.controls.lib.latcontrol_pid import LatControlPID
 from openpilot.selfdrive.controls.lib.latcontrol_angle import LatControlAngle, STEER_ANGLE_SATURATION_THRESHOLD
@@ -52,6 +53,8 @@ class Controls(ControlsExt):
     self.steer_limited_by_safety = False
     self.curvature = 0.0
     self.desired_curvature = 0.0
+    self.ford_path_controller = FordPathController()
+    self.ford_path = FordPath()
 
     self.pose_calibrator = PoseCalibrator()
     self.calibrated_pose: Pose | None = None
@@ -155,6 +158,12 @@ class Controls(ControlsExt):
       actuators.curvature = float(lateral_output)
     else:
       actuators.steeringAngleDeg = float(lateral_output)
+    if self.CP.brand == "ford":
+      self.ford_path = self.ford_path_controller.update(model_v2 if self.sm.valid['modelV2'] else None,
+                                                        self.desired_curvature, v_ego=CS.vEgo, active=CC.latActive,
+                                                        current_curvature=self.curvature, yaw_rate=CS.yawRate,
+                                                        actuator_delay=lat_delay)
+      actuators.curvature = float(self.ford_path.curvature)
     # Ensure no NaNs/Infs
     for p in ACTUATOR_FIELDS:
       attr = getattr(actuators, p)

openpilot/selfdrive/controls/lib/ford_path.py

@@ -0,0 +1,253 @@
+from dataclasses import dataclass
+import math
+
+import numpy as np
+
+
+DBC_OFFSET = (-5.12, 5.11)
+DBC_ANGLE = (-0.5, 0.5235)
+DBC_CURVATURE = (-0.02, 0.02)
+DBC_CURVATURE_RATE = (-0.001024, 0.001023)
+
+_FIT_DISTANCE_M = 7.0
+_COMMITTED_DISTANCE_M = 0.0
+_REFERENCE_SAMPLES = 17
+_C2_CURVATURE_LIMIT = 0.008
+_CURVATURE_RESIDUAL_DISTANCE_M = 4.0
+_CURVATURE_ERROR_DISTANCE_M = 2.0
+_CURVATURE_ERROR_LIMIT = 0.02
+_MAX_MEASURED_YAW_RATE = 1.0
+
+
+@dataclass(frozen=True)
+class FordPath:
+  valid: bool = False
+  path_offset: float = 0.0
+  path_angle: float = 0.0
+  curvature: float = 0.0
+  curvature_rate: float = 0.0
+
+
+def _finite(value: float) -> float:
+  return float(value) if math.isfinite(value) else 0.0
+
+
+def _model_path(model) -> tuple[np.ndarray, np.ndarray] | None:
+  try:
+    x = np.asarray(model.position.x, dtype=float)
+    y = np.asarray(model.position.y, dtype=float)
+  except (AttributeError, TypeError, ValueError):
+    return None
+  if len(x) < 3 or len(y) != len(x) or not np.isfinite(np.concatenate((x, y))).all():
+    return None
+
+  # LMC2 is y(x), so retain only the forward, monotonic part of a tight turn.
+  end = 1
+  while end < len(x) and x[end] > x[end - 1]:
+    end += 1
+  x = x[:end]
+  y = y[:end]
+  if len(x) < 3 or x[-1] - x[0] < 1.0:
+    return None
+  return x - x[0], y
+
+
+def _advance_path(x: np.ndarray, y: np.ndarray, speed: float, yaw_rate: float,
+                  dt: float) -> tuple[np.ndarray, np.ndarray]:
+  distance = max(speed, 0.0) * dt
+  yaw = yaw_rate * dt
+  if abs(yaw_rate) < 1e-9:
+    vehicle_x, vehicle_y = distance, 0.0
+  else:
+    radius = max(speed, 0.0) / yaw_rate
+    vehicle_x = radius * math.sin(yaw)
+    vehicle_y = radius * (1.0 - math.cos(yaw))
+  cosine = math.cos(yaw)
+  sine = math.sin(yaw)
+  relative_x = x - vehicle_x
+  relative_y = y - vehicle_y
+  return cosine * relative_x + sine * relative_y, -sine * relative_x + cosine * relative_y
+
+
+def _polynomial_reference(path: FordPath) -> tuple[np.ndarray, np.ndarray]:
+  x = np.linspace(0.0, 2.0 * _FIT_DISTANCE_M, _REFERENCE_SAMPLES)
+  y = (path.path_offset + math.tan(path.path_angle) * x + 0.5 * path.curvature * x ** 2 +
+       path.curvature_rate * x ** 3 / 6.0)
+  return x, y
+
+
+def _merge_reference(old: tuple[np.ndarray, np.ndarray], fresh: tuple[np.ndarray, np.ndarray],
+                     fallback: FordPath | None = None) -> tuple[np.ndarray, np.ndarray]:
+  old_x, old_y = old
+  fresh_x, fresh_y = fresh
+  old_end = 1
+  while old_end < len(old_x) and old_x[old_end] > old_x[old_end - 1] + 1e-3:
+    old_end += 1
+  old_x = old_x[:old_end]
+  old_y = old_y[:old_end]
+  if (len(old_x) < 2 or old_x[-1] < 1.0) and fallback is not None:
+    old_x, old_y = _polynomial_reference(fallback)
+  old_length = float(old_x[-1])
+  length = min(float(fresh_x[-1]), 2.0 * _FIT_DISTANCE_M)
+  if length < 1.0:
+    return fresh
+
+  x = np.linspace(0.0, length, _REFERENCE_SAMPLES)
+  old_sample_y = np.interp(x, old_x, old_y)
+  fresh_sample_y = np.interp(x, fresh_x, fresh_y)
+  blend_span = max(_FIT_DISTANCE_M - _COMMITTED_DISTANCE_M, 1e-3)
+  progress = np.clip((x - _COMMITTED_DISTANCE_M) / blend_span, 0.0, 1.0)
+  weight = progress * progress * (3.0 - 2.0 * progress)
+  weight[x > old_length] = 1.0
+  return x, old_sample_y + weight * (fresh_sample_y - old_sample_y)
+
+
+def _forward_prefix(path: tuple[np.ndarray, np.ndarray]) -> tuple[np.ndarray, np.ndarray]:
+  x, y = path
+  end = 1
+  while end < len(x) and x[end] > x[end - 1] + 1e-3:
+    end += 1
+  return x[:end], y[:end]
+
+
+def _local_geometry(path: tuple[np.ndarray, np.ndarray]) -> tuple[float, float] | None:
+  x, y = _forward_prefix(path)
+  forward = (x >= -0.25) & (x <= _FIT_DISTANCE_M)
+  x = x[forward]
+  y = y[forward]
+  if len(x) < 4 or x[-1] < 1.0:
+    return None
+  length = min(float(x[-1]), _FIT_DISTANCE_M)
+  sample_x = np.linspace(0.0, length, _REFERENCE_SAMPLES)
+  sample_y = np.interp(sample_x, x, y)
+  slopes = np.gradient(sample_y, sample_x, edge_order=2)
+  station = np.cumsum(np.hypot(np.diff(sample_x, prepend=sample_x[0]), np.diff(sample_y, prepend=sample_y[0])))
+  heading = np.unwrap(np.arctan(slopes))
+  geometry_samples = min(7, len(station))
+  heading_fit = np.polynomial.polynomial.polyfit(station[:geometry_samples], heading[:geometry_samples], 2)
+  geometric_curvature = float(heading_fit[1])
+  geometric_curvature_rate = float(2.0 * heading_fit[2])
+  return geometric_curvature, geometric_curvature_rate
+
+
+def _fit_path(reference: tuple[np.ndarray, np.ndarray], desired_curvature: float | None = None,
+              current_curvature: float | None = None) -> FordPath | None:
+  x, y = _forward_prefix(reference)
+  forward = (x >= -0.25) & (x <= _FIT_DISTANCE_M)
+  x = x[forward]
+  y = y[forward]
+  if len(x) < 4 or x[-1] < 1.0:
+    return None
+  length = min(float(x[-1]), _FIT_DISTANCE_M)
+  sample_x = np.linspace(0.0, length, _REFERENCE_SAMPLES)
+  sample_y = np.interp(sample_x, x, y)
+  slopes = np.gradient(sample_y, sample_x, edge_order=2)
+  local_geometry = _local_geometry(reference)
+  if local_geometry is None:
+    return None
+  geometric_curvature, geometric_curvature_rate = local_geometry
+  curvature = float(np.clip(geometric_curvature, -_C2_CURVATURE_LIMIT, _C2_CURVATURE_LIMIT))
+  curvature_rate = geometric_curvature_rate if abs(geometric_curvature) < _C2_CURVATURE_LIMIT else 0.0
+  requested_curvature = geometric_curvature
+  curvature_error = 0.0
+  if desired_curvature is not None:
+    requested_curvature = _finite(desired_curvature)
+    bounded_curvature = curvature
+    # C2 is filtered inside the PSCM. Never fill that slow channel beyond the
+    # curvature currently requested, or in the opposite direction during an unwind.
+    curvature = float(np.clip(curvature, min(0.0, requested_curvature), max(0.0, requested_curvature)))
+    if curvature != bounded_curvature:
+      curvature_rate = 0.0
+    if current_curvature is not None:
+      curvature_error = float(np.clip(requested_curvature - _finite(current_curvature),
+                                      -_CURVATURE_ERROR_LIMIT, _CURVATURE_ERROR_LIMIT))
+  curvature_rate = float(np.clip(curvature_rate, DBC_CURVATURE_RATE[0], DBC_CURVATURE_RATE[1]))
+
+  path_offset = float(np.clip(sample_y[0], DBC_OFFSET[0], DBC_OFFSET[1]))
+  local_heading = math.atan(float(slopes[0]))
+  # C0/C1 carry the rolling pose error and the fast part of the turn. The
+  # curvature error term continuously adds command while the vehicle is behind
+  # and countersteers while the PSCM's filtered C2 is draining.
+  path_angle = (local_heading + _CURVATURE_RESIDUAL_DISTANCE_M * (requested_curvature - curvature) +
+                _CURVATURE_ERROR_DISTANCE_M * curvature_error)
+  return FordPath(
+    valid=True,
+    path_offset=path_offset,
+    path_angle=float(np.clip(path_angle, DBC_ANGLE[0], DBC_ANGLE[1])),
+    curvature=curvature,
+    curvature_rate=curvature_rate,
+  )
+
+
+def _model_key(model) -> tuple[int, int] | int:
+  try:
+    frame_id = int(model.frameId)
+    timestamp = int(model.timestampEof)
+    if frame_id or timestamp:
+      return frame_id, timestamp
+  except (AttributeError, TypeError, ValueError):
+    pass
+  return id(model)
+
+
+class FordPathController:
+  """Track one rolling Ford path reference without modeling PSCM actuation."""
+
+  def __init__(self, dt: float = 0.01):
+    self.dt = dt
+    self._reference: tuple[np.ndarray, np.ndarray] | None = None
+    self._last_model_key: tuple[int, int] | int | None = None
+    self._last_path: FordPath | None = None
+
+  def reset(self) -> None:
+    self._reference = None
+    self._last_model_key = None
+    self._last_path = None
+
+  def update(self, model, desired_curvature: float | None = None, *, v_ego: float = 0.0, active: bool = True,
+             current_curvature: float | None = None, yaw_rate: float = 0.0, actuator_delay: float = 0.0) -> FordPath:
+    del actuator_delay
+    if not active or model is None:
+      self.reset()
+      return FordPath()
+
+    measured_yaw_rate = _finite(yaw_rate)
+    if abs(measured_yaw_rate) > _MAX_MEASURED_YAW_RATE:
+      measured_yaw_rate = 0.0
+    if self._reference is not None:
+      self._reference = _advance_path(self._reference[0], self._reference[1], max(_finite(v_ego), 0.0),
+                                      measured_yaw_rate, self.dt)
+    fresh = _model_path(model)
+    if fresh is None:
+      path = _fit_path(self._reference, desired_curvature, current_curvature) if self._reference is not None else None
+      if path is not None:
+        self._last_path = path
+        return path
+      self.reset()
+      return FordPath()
+
+    model_key = _model_key(model)
+    if self._reference is None:
+      self._reference = fresh
+    elif model_key != self._last_model_key:
+      self._reference = _merge_reference(self._reference, fresh, self._last_path)
+    self._last_model_key = model_key
+
+    path = _fit_path(self._reference, desired_curvature, current_curvature)
+    if path is None:
+      seed = _polynomial_reference(self._last_path) if self._last_path is not None else fresh
+      self._reference = _merge_reference(seed, fresh, self._last_path)
+      path = _fit_path(self._reference, desired_curvature, current_curvature)
+    if path is None:
+      self.reset()
+      return FordPath()
+    self._last_path = path
+    return path
+
+
+def encode_ford_path(model, t_prev: float, desired_curvature: float | None = None, *, v_ego: float = 0.0,
+                     current_curvature: float | None = None, yaw_rate: float = 0.0, actuator_delay: float = 0.0) -> FordPath:
+  """Stateless compatibility helper; live control uses FordPathController."""
+  del t_prev
+  return FordPathController().update(model, desired_curvature, v_ego=v_ego, current_curvature=current_curvature,
+                                     yaw_rate=yaw_rate, actuator_delay=actuator_delay)

openpilot/selfdrive/controls/tests/test_ford_path.py

@@ -0,0 +1,190 @@
+import math
+from types import SimpleNamespace
+
+import numpy as np
+
+from openpilot.cereal import custom
+from openpilot.selfdrive.car.helpers import convert_carControlSP
+from openpilot.selfdrive.controls.lib.ford_path import DBC_CURVATURE, FordPathController, encode_ford_path
+
+
+def _path(curvature: float, curvature_rate: float = 0.0, speed: float = 8.0):
+  t = np.linspace(0.0, 3.0, 61)
+  distance = speed * t
+  heading = curvature * distance + 0.5 * curvature_rate * distance ** 2
+  x = np.zeros_like(distance)
+  y = np.zeros_like(distance)
+  for i in range(1, len(distance)):
+    ds = distance[i] - distance[i - 1]
+    average_heading = 0.5 * (heading[i] + heading[i - 1])
+    x[i] = x[i - 1] + ds * math.cos(average_heading)
+    y[i] = y[i - 1] + ds * math.sin(average_heading)
+  return SimpleNamespace(
+    position=SimpleNamespace(t=t.tolist(), x=x.tolist(), y=y.tolist()),
+    orientation=SimpleNamespace(z=heading.tolist()),
+  )
+
+
+def _offset_path(offset: float, speed: float = 8.0):
+  t = np.linspace(0.0, 3.0, 61)
+  distance = speed * t
+  return SimpleNamespace(
+    position=SimpleNamespace(t=t.tolist(), x=distance.tolist(), y=np.full_like(distance, offset).tolist()),
+    orientation=SimpleNamespace(z=np.zeros_like(distance).tolist()),
+  )
+
+
+def test_gentle_arc_is_geometric_feedforward():
+  path = encode_ford_path(_path(0.008), 0.0, v_ego=8.0)
+
+  assert path.valid
+  assert abs(path.path_offset) < 1e-6
+  assert abs(path.path_angle) < 2e-4
+  assert np.isclose(path.curvature, 0.008, atol=5e-5)
+  assert abs(path.curvature_rate) < 1e-5
+
+
+def test_sunnypilot_path_message_round_trip():
+  message = custom.CarControlSP.new_message()
+  message.fordLateralPath.pathOffset = 0.3
+  message.fordLateralPath.pathAngle = -0.2
+  message.fordLateralPath.curvature = 0.008
+  message.fordLateralPath.curvatureRate = -0.0004
+  message.fordLateralPath.valid = True
+
+  path = convert_carControlSP(message.as_reader()).fordLateralPath
+  assert np.isclose(path.pathOffset, 0.3)
+  assert np.isclose(path.pathAngle, -0.2)
+  assert np.isclose(path.curvature, 0.008)
+  assert np.isclose(path.curvatureRate, -0.0004)
+  assert path.valid
+
+
+def test_tight_arc_caps_slow_feedforward_and_moves_residual_into_fast_heading():
+  left = encode_ford_path(_path(0.04), 0.0, v_ego=8.0)
+  right = encode_ford_path(_path(-0.04), 0.0, v_ego=8.0)
+
+  assert np.isclose(left.curvature, 0.008, atol=5e-5)
+  assert np.isclose(right.curvature, -0.008, atol=5e-5)
+  assert abs(left.curvature_rate) < 1e-4
+  assert abs(right.curvature_rate) < 1e-4
+  assert left.path_angle > 0.06
+  assert right.path_angle < -0.06
+  assert abs(left.path_offset) < 1e-9
+  assert abs(right.path_offset) < 1e-9
+
+
+def test_c2_does_not_increase_while_tight_curve_unwinds():
+  curvatures = (0.04, 0.018, 0.016, 0.014, 0.012, 0.010, 0.008, 0.006, 0.0)
+  commands = [encode_ford_path(_path(curvature), 0.0, v_ego=8.0).curvature for curvature in curvatures]
+
+  assert np.all(np.diff(commands) <= 1e-9)
+
+
+def test_lateral_delay_does_not_change_the_reference_polynomial():
+  early = FordPathController().update(_path(0.012, 0.0003), v_ego=10.0, current_curvature=-0.01, actuator_delay=0.1)
+  late = FordPathController().update(_path(0.012, 0.0003), v_ego=10.0, current_curvature=-0.01, actuator_delay=0.9)
+
+  assert early == late
+
+
+def test_reference_offset_is_not_erased_by_an_ego_anchored_replan():
+  controller = FordPathController(dt=0.05)
+  initial = controller.update(_offset_path(0.4), v_ego=8.0)
+  replanned = controller.update(_path(0.0), v_ego=8.0)
+
+  assert initial.path_offset > 0.39
+  assert replanned.path_offset > 0.35
+
+
+def test_s_turn_reverses_fast_fields_while_c2_is_bounded():
+  controller = FordPathController(dt=0.05)
+  controller.update(_path(0.04), v_ego=8.0, yaw_rate=0.0)
+  controller.update(_path(0.04), v_ego=8.0, yaw_rate=0.16)
+
+  outputs = [controller.update(_path(-0.02), -0.02, v_ego=8.0, current_curvature=0.02, yaw_rate=0.32) for _ in range(5)]
+
+  assert all(path.valid for path in outputs)
+  assert all(DBC_CURVATURE[0] <= path.curvature <= DBC_CURVATURE[1] for path in outputs)
+  assert all(path.curvature <= 0.0 for path in outputs)
+  assert outputs[0].path_angle < -0.03
+  assert outputs[-1].path_offset < 0.0
+
+
+def test_same_model_advances_reference_from_measured_motion():
+  model = _offset_path(0.25)
+  controller = FordPathController(dt=0.05)
+  before = controller.update(model, v_ego=8.0, current_curvature=0.0)
+  after = controller.update(model, v_ego=8.0, yaw_rate=0.16)
+
+  steering_controller = FordPathController(dt=0.05)
+  steering_controller.update(model, v_ego=8.0)
+  steering_only = steering_controller.update(model, v_ego=8.0, current_curvature=0.02)
+
+  assert before.valid and after.valid
+  assert after != before
+  assert steering_only != after
+
+
+def test_fresh_model_replenishes_the_rolling_horizon():
+  controller = FordPathController(dt=0.01)
+  for frame_id in range(100):
+    model = _path(0.008, speed=20.0)
+    model.frameId = frame_id + 1
+    model.timestampEof = frame_id + 1
+    assert controller.update(model, v_ego=20.0, yaw_rate=0.16).valid
+    assert controller._reference is not None
+    assert controller._reference[0][-1] >= 2.0 * 7.0 - 1e-6
+
+
+def test_invalid_ford_yaw_rate_does_not_rotate_the_reference():
+  model = _offset_path(0.25)
+  valid = FordPathController(dt=0.01)
+  invalid = FordPathController(dt=0.01)
+  valid.update(model, v_ego=8.0)
+  invalid.update(model, v_ego=8.0)
+
+  expected = valid.update(model, v_ego=8.0, yaw_rate=0.0)
+  sentinel = invalid.update(model, v_ego=8.0, yaw_rate=6.6066)
+
+  assert sentinel == expected
+
+
+def test_curvature_error_increases_only_the_fast_heading_command():
+  behind = FordPathController().update(_path(0.008), 0.008, v_ego=15.0, current_curvature=0.0)
+  tracking = FordPathController().update(_path(0.008), 0.008, v_ego=15.0, current_curvature=0.008)
+
+  assert behind.path_angle > tracking.path_angle + 0.015
+  assert np.isclose(behind.path_offset, tracking.path_offset)
+  assert np.isclose(behind.curvature, tracking.curvature)
+  assert np.isclose(behind.curvature_rate, tracking.curvature_rate)
+
+
+def test_c2_drains_with_requested_curvature_while_vehicle_unwinds():
+  controller = FordPathController()
+  controller.update(_path(0.02), 0.02, v_ego=15.0, current_curvature=0.02)
+  unwinding = controller.update(_path(0.02), 0.003, v_ego=15.0, current_curvature=0.01)
+
+  assert 0.0 <= unwinding.curvature <= 0.003
+  assert unwinding.path_angle < 0.0
+
+
+def test_tight_turn_from_stop_uses_fast_heading_command():
+  path = FordPathController().update(_path(0.04), 0.04, v_ego=0.0, current_curvature=0.0)
+
+  assert np.isclose(path.curvature, 0.008, atol=5e-5)
+  assert path.path_angle > 0.15
+
+
+def test_curvature_feedback_is_bounded_for_bad_measurement():
+  nominal = FordPathController().update(_path(0.008), 0.008, v_ego=15.0, current_curvature=0.008)
+  corrupted = FordPathController().update(_path(0.008), 0.008, v_ego=15.0, current_curvature=-1.0)
+
+  assert corrupted.path_angle <= nominal.path_angle + 0.04 + 1e-9
+
+
+def test_invalid_or_inactive_resets_reference():
+  controller = FordPathController()
+  assert controller.update(_path(0.0), v_ego=12.0).valid
+  assert not controller.update(_path(0.0), v_ego=12.0, active=False).valid
+  assert not controller.update(None, v_ego=12.0).valid

openpilot/selfdrive/ui/sunnypilot/layouts/settings/osm.py

@@ -8,7 +8,6 @@ import datetime
 import os
 import platform
 import requests
-import shutil
 import threading
 from pathlib import Path
 from time import monotonic
@@ -75,22 +74,12 @@ class OSMLayout(Widget):
   def _update_map_size(self):
     threading.Thread(target=self.calculate_size, daemon=True).start()
 
-  def _do_delete_maps(self):
-    if MAP_PATH.exists():
-      shutil.rmtree(MAP_PATH)
-
-    for param in ("OsmDownloadedDate", "OsmLocal", "OsmLocationName", "OsmLocationTitle", "OsmStateName", "OsmStateTitle"):
-      ui_state.params.remove(param)
-
+  def _on_confirm_delete_maps(self):
+    ui_state.params.put_bool("Mapd_ClearCache", True)
     self._delete_maps_btn.action_item.set_enabled(True)
     self._delete_maps_btn.action_item.set_text(tr("DELETE"))
     self._update_map_size()
 
-  def _on_confirm_delete_maps(self):
-    self._delete_maps_btn.action_item.set_enabled(False)
-    self._delete_maps_btn.action_item.set_text("DELETING...")
-    threading.Thread(target=self._do_delete_maps).start()
-
   def _delete_maps(self):
     self._show_confirm(tr("This will delete ALL downloaded maps\n\nAre you sure you want to delete all maps?"),
                        tr("Yes, delete all maps"), self._on_confirm_delete_maps)

openpilot/sunnypilot/mapd/mapd_manager.py

@@ -55,6 +55,19 @@ def cleanup_old_osm_data(files_to_remove: list[str]) -> None:
       shutil.rmtree(file, ignore_errors=False)
 
 
+def clear_downloaded_maps() -> None:
+  """Deletes downloaded OSM map data and resets params."""
+  path = f"{Paths.mapd_root()}/offline"
+  if os.path.exists(path):
+    shutil.rmtree(path, ignore_errors=True)
+
+  for param in ("OsmDownloadedDate", "OsmLocal", "OsmLocationName", "OsmLocationTitle",
+                "OsmStateName", "OsmStateTitle"):
+    params.remove(param)
+
+  cloudlog.info("mapd: downloaded maps cleared")
+
+
 def request_refresh_osm_location_data(nations: list[str], states: list[str] | None = None) -> None:
   params.put("OsmDownloadedDate", str(datetime.now().timestamp()), block=True)
   params.put_bool("OsmDbUpdatesCheck", False, block=True)
@@ -131,6 +144,10 @@ def main_thread():
     show_alert = bool(get_files_for_cleanup() and params.get_bool("OsmLocal"))
     set_offroad_alert("Offroad_OSMUpdateRequired", show_alert, "This alert will be cleared when new maps are downloaded.")
 
+    if params.get("Mapd_ClearCache"):
+      clear_downloaded_maps()
+      params.remove("Mapd_ClearCache")
+
     update_osm_db()
     live_map_sp.tick()
     rk.keep_time()

openpilot/sunnypilot/selfdrive/controls/controlsd_ext.py

@@ -104,6 +104,14 @@ class ControlsExt(ModelStateBase):
     CC_SP.intelligentCruiseButtonManagement.sendButton = icbm_src.sendButton
     CC_SP.intelligentCruiseButtonManagement.vTarget = icbm_src.vTarget
 
+    ford_path = getattr(self, 'ford_path', None)
+    if ford_path is not None:
+      CC_SP.fordLateralPath.valid = ford_path.valid
+      CC_SP.fordLateralPath.pathOffset = ford_path.path_offset
+      CC_SP.fordLateralPath.pathAngle = ford_path.path_angle
+      CC_SP.fordLateralPath.curvature = ford_path.curvature
+      CC_SP.fordLateralPath.curvatureRate = ford_path.curvature_rate
+
     return CC_SP
 
   @staticmethod