ford: narrow lateral path interface

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/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,11 @@ 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)
+      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,166 @@
+from dataclasses import dataclass, fields
+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)
+
+_PATH_OFFSET_DISTANCE = 7.0
+_PATH_MIN_LOOKAHEAD = 7.0
+_CURVATURE_RATE_HORIZONS = (3.5, 5.0, 7.0)
+_CENTERING_CURVATURE_BASEBAND = (0.003, 0.006)
+_TRACKING_ERROR_DEADZONE = 0.0005
+_TRACKING_ERROR_LIMIT = 0.012
+_PATH_RATES = (4.0, 1.0, math.inf, 0.002)
+
+
+@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 _sample(distance: float, distances: list[float], values: list[float]) -> float:
+  return float(np.interp(distance, distances, values))
+
+
+def _model_path(model) -> tuple[list[float], list[float], list[float]] | None:
+  try:
+    x = [float(value) for value in model.position.x]
+    y = [float(value) for value in model.position.y]
+    heading = [float(value) for value in model.orientation.z]
+  except (AttributeError, TypeError, ValueError):
+    return None
+  if len(x) < 2 or len(x) != len(y) or len(x) != len(heading):
+    return None
+  if not all(math.isfinite(value) for values in (x, y, heading) for value in values):
+    return None
+
+  distance = [0.0]
+  for i in range(1, len(x)):
+    distance.append(distance[-1] + math.hypot(x[i] - x[i - 1], y[i] - y[i - 1]))
+  if distance[-1] <= 0.0:
+    return None
+
+  unwrapped_heading = [heading[0]]
+  for value in heading[1:]:
+    delta = (value - unwrapped_heading[-1] + math.pi) % (2.0 * math.pi) - math.pi
+    unwrapped_heading.append(unwrapped_heading[-1] + delta)
+  return distance, y, unwrapped_heading
+
+
+def _curvature_rate(path: tuple[list[float], list[float], list[float]]) -> float:
+  distance, _, heading = path
+  rates = []
+  for requested_horizon in _CURVATURE_RATE_HORIZONS:
+    horizon = min(requested_horizon, distance[-1])
+    start = _sample(0.0, distance, heading)
+    midpoint = _sample(0.5 * horizon, distance, heading)
+    end = _sample(horizon, distance, heading)
+    rates.append(4.0 * (start - 2.0 * midpoint + end) / horizon ** 2)
+
+  magnitude = sum(abs(rate) for rate in rates)
+  if magnitude == 0.0:
+    return 0.0
+  return sorted(rates)[1] * abs(sum(rates)) / magnitude
+
+
+def _curvature(path: tuple[list[float], list[float], list[float]]) -> float:
+  distance, _, heading = path
+  horizon = min(_PATH_MIN_LOOKAHEAD, distance[-1])
+  return (_sample(horizon, distance, heading) - _sample(0.0, distance, heading)) / horizon
+
+
+def _encode_path(model, desired_curvature: float, v_ego: float, current_curvature: float | None) -> FordPath:
+  path = _model_path(model)
+  if path is None:
+    return FordPath()
+
+  distance, offset, heading = path
+  lookahead = max(_finite(v_ego), _PATH_MIN_LOOKAHEAD)
+  path_offset = _sample(_PATH_OFFSET_DISTANCE, distance, offset)
+  path_angle = _sample(lookahead, distance, heading)
+  model_curvature = _curvature(path)
+  model_curvature_rate = _curvature_rate(path)
+  action_curvature = _finite(desired_curvature)
+  requested_curvature = max((model_curvature, action_curvature), key=abs)
+  maneuver_residual = requested_curvature - model_curvature
+  path_offset += 0.5 * maneuver_residual * _PATH_OFFSET_DISTANCE ** 2
+  path_angle += maneuver_residual * lookahead
+  correction = 0.0
+  tracking_demand = 0.0
+  wheel_beyond_target = False
+  if current_curvature is not None:
+    target_curvature = action_curvature
+    measured_curvature = _finite(current_curvature)
+    tracking_error = target_curvature - measured_curvature
+    correction = math.copysign(max(abs(tracking_error) - _TRACKING_ERROR_DEADZONE, 0.0), tracking_error)
+    tracking_demand = abs(correction)
+    wheel_beyond_target = target_curvature * measured_curvature > 0.0 and \
+      abs(target_curvature) + _TRACKING_ERROR_DEADZONE < abs(measured_curvature)
+    correction_limit = _TRACKING_ERROR_LIMIT
+    if correction * target_curvature < 0.0:
+      correction_limit = 0.5 * abs(target_curvature)
+    correction = float(np.clip(correction, -correction_limit, correction_limit))
+    correction_offset = 0.5 * correction * _PATH_OFFSET_DISTANCE ** 2
+    correction_angle = correction * lookahead
+    if wheel_beyond_target:
+      path_offset = correction_offset
+      path_angle = correction_angle
+    else:
+      path_offset += correction_offset
+      path_angle += correction_angle
+
+  spatial_demand = abs(model_curvature_rate) * lookahead / 3.0
+  overflow_demand = max(abs(requested_curvature) - DBC_CURVATURE[1], 0.0)
+  maneuver_demand = max(spatial_demand, overflow_demand, tracking_demand)
+  maneuver_share = 1.0 if wheel_beyond_target else \
+    float(np.interp(maneuver_demand, _CENTERING_CURVATURE_BASEBAND, (0.0, 1.0)))
+  path_offset *= maneuver_share
+  path_angle *= maneuver_share
+  centering_curvature = action_curvature
+
+  return FordPath(
+    valid=True,
+    path_offset=float(np.clip(path_offset, *DBC_OFFSET)),
+    path_angle=float(np.clip(path_angle, *DBC_ANGLE)),
+    curvature=float(np.clip(centering_curvature * (1.0 - maneuver_share), *DBC_CURVATURE)),
+    curvature_rate=float(np.clip(model_curvature_rate * maneuver_share, *DBC_CURVATURE_RATE)),
+  )
+
+
+class FordPathController:
+  """Convert the model path directly into one vehicle-independent Ford path command."""
+
+  def __init__(self, dt: float = 0.01):
+    self.dt = dt
+    self._last_path = FordPath(valid=True)
+
+  def _limit(self, target: FordPath) -> FordPath:
+    values = []
+    for field, rate in zip(fields(FordPath)[1:], _PATH_RATES, strict=True):
+      previous = getattr(self._last_path, field.name)
+      value = getattr(target, field.name)
+      values.append(float(np.clip(value, previous - rate * self.dt, previous + rate * self.dt)))
+    self._last_path = FordPath(True, *values)
+    return self._last_path
+
+  def update(self, model, desired_curvature: float, *, v_ego: float = 0.0, active: bool = True,
+             current_curvature: float | None = None) -> FordPath:
+    if not active:
+      self._last_path = FordPath(valid=True)
+      return FordPath()
+    if model is None:
+      return self._limit(FordPath(valid=True))
+    return self._limit(_encode_path(model, desired_curvature, v_ego, current_curvature))

openpilot/selfdrive/controls/tests/test_ford_path.py

@@ -0,0 +1,290 @@
+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
+
+
+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 _equivalent_curvature(path, distance: float = 7.0) -> float:
+  offset = path.path_offset + path.path_angle * distance + 0.5 * path.curvature * distance ** 2 + \
+    path.curvature_rate * distance ** 3 / 6.0
+  return 2.0 * offset / distance ** 2
+
+
+def _command(model, desired_curvature: float, *, v_ego: float = 0.0, current_curvature: float | None = None):
+  return FordPathController(dt=1.0).update(model, desired_curvature, v_ego=v_ego, current_curvature=current_curvature)
+
+
+def test_steady_arc_uses_c2_without_fast_pose_fields():
+  path = _command(_path(0.008), 0.008, v_ego=8.0)
+
+  assert path.valid
+  assert abs(path.path_offset) < 1e-9
+  assert abs(path.path_angle) < 1e-9
+  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_uses_signed_forward_pose_without_slow_c2():
+  left = _command(_path(0.04), 0.04, v_ego=8.0)
+  right = _command(_path(-0.04), -0.04, v_ego=8.0)
+
+  assert left.curvature == 0.0
+  assert right.curvature == 0.0
+  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 left.path_offset > 0.5
+  assert right.path_offset < -0.5
+
+
+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)
+  measured = (0.04,) + curvatures[:-1]
+  commands = [_command(_path(curvature), curvature, v_ego=8.0, current_curvature=actual).curvature
+              for curvature, actual in zip(curvatures, measured, strict=True)]
+
+  assert np.all(np.diff(commands) <= 1e-9)
+
+
+def test_fresh_model_replaces_previous_path_without_hidden_state():
+  controller = FordPathController(dt=1.0)
+  initial = controller.update(_path(0.04), 0.04, v_ego=8.0)
+  replanned = controller.update(_path(0.0), 0.0, v_ego=8.0)
+
+  assert initial.path_offset > 0.5
+  assert replanned == FordPathController().update(_path(0.0), 0.0, v_ego=8.0)
+
+
+def test_s_turn_reverses_fast_fields_while_c2_is_bounded():
+  controller = FordPathController(dt=0.05)
+  controller.update(_path(0.04), 0.04, v_ego=8.0)
+  controller.update(_path(0.04), 0.04, v_ego=8.0)
+
+  outputs = []
+  for frame_id in range(5):
+    model = _path(-0.02)
+    model.frameId = frame_id + 1
+    model.timestampEof = frame_id + 1
+    outputs.append(controller.update(model, -0.02, v_ego=8.0, current_curvature=0.02))
+
+  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[-1].path_angle < -0.03
+  assert outputs[-1].path_offset < 0.0
+
+
+def test_reversal_does_not_add_software_persistence_to_centering_c2():
+  controller = FordPathController()
+  assert controller.update(_path(0.002), 0.002, v_ego=8.0).curvature > 0.0
+
+  reversing = controller.update(_path(-0.02), -0.02, v_ego=8.0)
+
+  assert reversing.curvature <= 0.0
+
+
+def test_requested_turn_is_not_cancelled_by_previous_path():
+  controller = FordPathController(dt=1.0)
+  previous = _path(-0.02, speed=3.0)
+  previous.frameId = 1
+  previous.timestampEof = 1
+  controller.update(previous, -0.02, v_ego=3.0, current_curvature=-0.01)
+
+  requested = _path(0.02, speed=3.0)
+  requested.frameId = 2
+  requested.timestampEof = 2
+  command = controller.update(requested, 0.02, v_ego=3.0, current_curvature=0.007)
+
+  assert command.path_offset >= 0.0
+  assert command.path_angle >= 0.0
+  assert command.curvature >= 0.0
+  assert _equivalent_curvature(command) >= 0.02
+
+
+def test_short_low_speed_model_uses_available_path_endpoint():
+  command = FordPathController().update(_path(0.02, speed=1.0), 0.02, v_ego=1.0, current_curvature=0.0)
+
+  assert command.valid
+  assert command.path_offset > 0.0
+  assert command.path_angle > 0.0
+
+
+def test_action_demand_exposes_forward_path_authority():
+  command = FordPathController().update(_path(0.002), 0.0055, v_ego=8.0, current_curvature=0.0005)
+
+  assert _equivalent_curvature(command) >= 0.004
+
+
+def test_minor_curve_uses_c2_when_tracking_is_close():
+  command = FordPathController(dt=1.0).update(_path(0.005), 0.005, v_ego=8.0, current_curvature=0.0048)
+
+  assert abs(command.path_offset) < 1e-9
+  assert abs(command.path_angle) < 1e-9
+  assert command.curvature > 0.0049
+
+
+def test_minor_changing_curve_does_not_emit_ungated_c3():
+  command = FordPathController(dt=1.0).update(_path(0.005, 0.0003), 0.005, v_ego=8.0, current_curvature=0.0048)
+
+  assert abs(command.path_offset) < 1e-9
+  assert abs(command.path_angle) < 1e-9
+  assert command.curvature > 0.0049
+  assert command.curvature_rate == 0.0
+
+
+def test_c2_uses_stable_action_curvature_not_independent_model_fit():
+  controller = FordPathController(dt=1.0)
+  first = controller.update(_path(0.004), 0.002, v_ego=8.0, current_curvature=0.002)
+  second = controller.update(_path(0.006), 0.002, v_ego=8.0, current_curvature=0.002)
+
+  assert np.isclose(first.curvature, 0.002)
+  assert np.isclose(second.curvature, 0.002)
+
+
+def test_action_curvature_corrects_stale_opposing_model_at_low_speed():
+  command = FordPathController().update(_path(-0.001, speed=1.0), 0.005, v_ego=1.0, current_curvature=0.001)
+
+  assert command.path_offset > 0.0
+  assert command.path_angle > 0.0
+  assert command.curvature >= 0.0
+  assert _equivalent_curvature(command) >= 0.004
+
+
+def test_small_action_sign_noise_does_not_reverse_a_strong_model_path():
+  command = FordPathController(dt=1.0).update(_path(0.04), -0.0005, v_ego=6.0, current_curvature=0.02)
+
+  assert command.path_offset > 0.0
+  assert command.path_angle > 0.0
+  assert _equivalent_curvature(command) > 0.02
+
+
+def test_measured_curvature_after_path_exit_commands_countersteer():
+  command = FordPathController().update(_path(0.0), 0.0, v_ego=8.0, current_curvature=0.006)
+
+  assert command.curvature == 0.0
+  assert command.path_offset < 0.0
+  assert command.path_angle < 0.0
+
+
+def test_measured_curvature_countersteers_when_beyond_modeled_arc():
+  controller = FordPathController(dt=1.0)
+  command = controller.update(_path(0.004), 0.003, v_ego=8.0, current_curvature=0.012)
+  tracking = FordPathController(dt=1.0).update(_path(0.004), 0.003, v_ego=8.0, current_curvature=0.004)
+
+  assert command.path_offset < 0.0
+  assert command.path_angle < 0.0
+  assert command.path_angle < tracking.path_angle
+  assert command.curvature == 0.0
+
+
+def test_model_reversal_suppresses_old_c2_and_countersteers():
+  reversing = FordPathController().update(_path(-0.02), -0.0005, v_ego=8.0, current_curvature=0.01)
+
+  assert reversing.curvature <= 0.0
+  assert reversing.path_angle < 0.0
+
+
+def test_reversal_noise_band_is_continuous():
+  inside = FordPathController(dt=1.0).update(_path(0.02), -0.000099, v_ego=8.0, current_curvature=0.01)
+  outside = FordPathController(dt=1.0).update(_path(0.02), -0.000101, v_ego=8.0, current_curvature=0.01)
+
+  assert abs(outside.path_angle - inside.path_angle) < 0.005
+
+
+def test_curvature_error_increases_forward_pose_command_while_behind():
+  behind = FordPathController(dt=1.0).update(_path(0.008), 0.008, v_ego=15.0, current_curvature=0.0)
+  tracking = FordPathController(dt=1.0).update(_path(0.008), 0.008, v_ego=15.0, current_curvature=0.008)
+
+  assert behind.path_offset > tracking.path_offset + 0.01
+  assert behind.path_angle > tracking.path_angle + 0.015
+  assert behind.curvature == 0.0
+  assert tracking.curvature > 0.007
+  assert np.isclose(behind.curvature_rate, tracking.curvature_rate)
+
+
+def test_measured_wheel_beyond_action_countersteers_model_arc():
+  controller = FordPathController()
+  controller.update(_path(0.02), 0.02, v_ego=15.0, current_curvature=0.02)
+  outputs = [controller.update(_path(0.02), 0.003, v_ego=15.0, current_curvature=0.01) for _ in range(4)]
+  unwinding = outputs[-1]
+
+  assert unwinding.curvature == 0.0
+  assert unwinding.path_angle < 0.0
+
+
+def test_action_c2_remains_active_for_centering():
+  centering = FordPathController(dt=1.0).update(_path(0.002), 0.002, v_ego=15.0, current_curvature=0.002)
+
+  assert centering.curvature > 0.001
+  assert abs(centering.path_offset) < 1e-9
+  assert abs(centering.path_angle) < 1e-9
+
+
+def test_tight_turn_from_stop_builds_bounded_forward_pose_authority():
+  controller = FordPathController()
+  outputs = [controller.update(_path(0.04), 0.04, v_ego=0.0, current_curvature=0.0) for _ in range(20)]
+  path = outputs[-1]
+
+  assert path.curvature == 0.0
+  assert path.path_offset > 0.7
+  assert path.path_angle > 0.15
+  assert np.max(np.abs(np.diff([output.path_offset for output in outputs]))) <= 0.04 + 1e-9
+  assert np.max(np.abs(np.diff([output.path_angle for output in outputs]))) <= 0.01 + 1e-9
+
+
+def test_curvature_feedback_is_bounded_for_bad_measurement():
+  bounded = FordPathController().update(_path(0.008), 0.008, v_ego=15.0, current_curvature=-0.02)
+  corrupted = FordPathController().update(_path(0.008), 0.008, v_ego=15.0, current_curvature=-1.0)
+
+  assert np.isclose(corrupted.path_angle, bounded.path_angle)
+
+
+def test_invalid_model_ramps_pose_to_zero_while_remaining_in_extended_mode():
+  controller = FordPathController()
+  for _ in range(10):
+    active = controller.update(_path(0.04), 0.04, v_ego=12.0)
+  missing = controller.update(None, 0.0, v_ego=12.0)
+
+  assert active.path_offset > 0.0
+  assert missing.valid
+  assert np.isclose(active.path_offset - missing.path_offset, 0.04)
+  assert missing.curvature == 0.0
+  assert not controller.update(_path(0.0), 0.0, v_ego=12.0, active=False).valid

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