Ford: separate path feedforward from pose feedback

Keep the model's remaining path as feedforward while using the delay-aligned measured pose only as a bounded trim. Allocate common gentle model curvature to C2 and carry changing geometry in C0/C1 without allowing the action head to invent a path.

Assisted-by: Codex

.gitattributes

@@ -9,6 +9,7 @@
 *.ttf filter=lfs diff=lfs merge=lfs -text
 *.otf filter=lfs diff=lfs merge=lfs -text
 *.wav filter=lfs diff=lfs merge=lfs -text
+openpilot/selfdrive/assets/sounds/milestone.wav -filter -diff -merge -text
 
 openpilot/selfdrive/car/tests/test_models_segs.txt filter=lfs diff=lfs merge=lfs -text
 openpilot/common/hardware/comma/updater filter=lfs diff=lfs merge=lfs -text

.github/workflows/sunnypilot-build-model.yaml

@@ -121,7 +121,7 @@ jobs:
           if-no-files-found: error
 
   build_model:
-    runs-on: [self-hosted, chestnut]
+    runs-on: [self-hosted, "${{ inputs.target_hardware == 'chestnut' && 'chestnut' || 'tici' }}"]
     needs: get_model
     env:
       MODEL_NAME: ${{ inputs.custom_name || inputs.upstream_branch }} (${{ needs.get_model.outputs.model_date }})

openpilot/cereal/custom.capnp

@@ -383,6 +383,7 @@ struct CarControlSP @0xa5cd762cd951a455 {
   leadOne @2 :LeadData;
   leadTwo @3 :LeadData;
   intelligentCruiseButtonManagement @4 :IntelligentCruiseButtonManagement;
+  fordLateralPath @5 :FordLateralPath;
 
   struct Param {
     key @0 :Text;
@@ -403,6 +404,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;
@@ -470,7 +479,30 @@ struct ModelDataV2SP @0xa1680744031fdb2d {
   }
 }
 
-struct CustomReserved10 @0xcb9fd56c7057593a {
+struct AssistedDrivingMilestoneState @0xcb9fd56c7057593a {
+  enabled @0 :Bool;
+  madsDistanceMeters @1 :Float64;
+  fullAssistDistanceMeters @2 :Float64;
+  event @3 :Event;
+
+  struct Event {
+    id @0 :UInt64;
+    category @1 :Category;
+    distanceMeters @2 :Float64;
+    previousDistanceMeters @3 :Float64;
+    unit @4 :Unit;
+  }
+
+  enum Category {
+    none @0;
+    mads @1;
+    fullAssist @2;
+  }
+
+  enum Unit {
+    imperial @0;
+    metric @1;
+  }
 }
 
 struct CustomReserved11 @0xc2243c65e0340384 {

openpilot/cereal/log.capnp

@@ -2640,7 +2640,7 @@ struct Event {
     carStateSP @114 :Custom.CarStateSP;
     liveMapDataSP @115 :Custom.LiveMapDataSP;
     modelDataV2SP @116 :Custom.ModelDataV2SP;
-    customReserved10 @136 :Custom.CustomReserved10;
+    assistedDrivingMilestoneState @136 :Custom.AssistedDrivingMilestoneState;
     customReserved11 @137 :Custom.CustomReserved11;
     customReserved12 @138 :Custom.CustomReserved12;
     customReserved13 @139 :Custom.CustomReserved13;

openpilot/cereal/services.py

@@ -90,6 +90,7 @@ _services: dict[str, tuple] = {
   "carParamsSP": (True, 0.02, 1),
   "carControlSP": (True, 100., 10),
   "carStateSP": (True, 100., 10),
+  "assistedDrivingMilestoneState": (True, 10., 1),
   "liveMapDataSP": (True, 1., 1),
   "modelDataV2SP": (True, 20., None, QueueSize.BIG),
   "liveLocationKalman": (True, 20.),

openpilot/common/params.cc

@@ -97,6 +97,10 @@ Params::Params(const std::string &path) {
 }
 
 Params::~Params() {
+  flushNonBlockingWrites();
+}
+
+void Params::flushNonBlockingWrites() {
   if (future.valid()) {
     future.wait();
   }

openpilot/common/params.h

@@ -75,6 +75,7 @@ public:
     return put(key.c_str(), val ? "1" : "0", 1);
   }
   void putNonBlocking(const std::string &key, const std::string &val);
+  void flushNonBlockingWrites();
   inline void putBoolNonBlocking(const std::string &key, bool val) {
     putNonBlocking(key, val ? "1" : "0");
   }

openpilot/common/params.py

@@ -73,6 +73,7 @@ params_get = _bind("params_get", [ParamsHandle, ctypes.c_char_p, ctypes.c_bool],
 params_get_bool = _bind("params_get_bool", [ParamsHandle, ctypes.c_char_p, ctypes.c_bool], ctypes.c_bool)
 params_put = _bind("params_put", [ParamsHandle, ctypes.c_char_p, ctypes.c_char_p, ctypes.c_size_t, ctypes.c_bool], ctypes.c_int)
 params_put_bool = _bind("params_put_bool", [ParamsHandle, ctypes.c_char_p, ctypes.c_bool, ctypes.c_bool], ctypes.c_int)
+params_flush = _bind("params_flush", [ParamsHandle])
 params_remove = _bind("params_remove", [ParamsHandle, ctypes.c_char_p], ctypes.c_int)
 params_get_path = _bind("params_get_path", [ParamsHandle, ctypes.c_char_p, ctypes.c_size_t], ParamsBuffer)
 params_keys_size = _bind("params_keys_size", [ParamsHandle], ctypes.c_size_t)
@@ -178,6 +179,10 @@ class Params:
   def put_bool(self, key, val, block=False):
     params_put_bool(self.p, self.check_key(key), val, block)
 
+  def flush(self):
+    """Wait for all prior nonblocking writes from this Params instance."""
+    params_flush(self.p)
+
   def remove(self, key):
     params_remove(self.p, self.check_key(key))
 

openpilot/common/params_c.cc

@@ -133,6 +133,12 @@ int params_put_bool(ParamsHandle *handle, const char *key, bool value, bool bloc
   });
 }
 
+void params_flush(ParamsHandle *handle) noexcept {
+  translate_exceptions([&]() {
+    handle->params.flushNonBlockingWrites();
+  });
+}
+
 int params_remove(ParamsHandle *handle, const char *key) noexcept {
   return translate_exceptions(-1, [&]() {
     return handle->params.remove(key);

openpilot/common/params_keys.h

@@ -136,6 +136,8 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
 
     // --- sunnypilot params --- //
     {"ApiCache_DriveStats", {PERSISTENT, JSON}},
+    {"AssistedDrivingMilestonesEnabled", {PERSISTENT | BACKUP, BOOL, "1"}},
+    {"AssistedDrivingMilestoneState", {PERSISTENT, JSON, "{}"}},
     {"AutoLaneChangeBsmDelay", {PERSISTENT | BACKUP, BOOL, "0"}},
     {"AutoLaneChangeTimer", {PERSISTENT | BACKUP, INT, "0"}},
     {"BlinkerLateralReengageDelay", {PERSISTENT | BACKUP, INT, "0"}},  // seconds
@@ -156,6 +158,7 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
     {"DevUIInfo", {PERSISTENT | BACKUP, INT, "0"}},
     {"EnableCopyparty", {PERSISTENT | BACKUP, BOOL}},
     {"EnableGithubRunner", {PERSISTENT | BACKUP, BOOL}},
+    {"FullAssistDrivenDistanceMeters", {PERSISTENT, FLOAT, "0.0"}},
     {"GreenLightAlert", {PERSISTENT | BACKUP, BOOL, "0"}},
     {"GithubRunnerSufficientVoltage", {CLEAR_ON_MANAGER_START , BOOL}},
     {"HasAcceptedTermsSP", {PERSISTENT, STRING, "0"}},
@@ -165,7 +168,9 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
     {"IsDevelopmentBranch", {CLEAR_ON_MANAGER_START, BOOL}},
     {"IsReleaseSpBranch", {CLEAR_ON_MANAGER_START, BOOL}},
     {"LastGPSPositionLLK", {PERSISTENT, STRING}},
+    {"LastDriveAssistedDrivingSummary", {PERSISTENT, JSON, "{}"}},
     {"LeadDepartAlert", {PERSISTENT | BACKUP, BOOL, "0"}},
+    {"MadsDrivenDistanceMeters", {PERSISTENT, FLOAT, "0.0"}},
     {"MaxTimeOffroad", {PERSISTENT | BACKUP, INT, "1800"}},
     {"ModelRunnerTypeCache", {CLEAR_ON_ONROAD_TRANSITION, INT}},
     {"OffroadMode", {CLEAR_ON_MANAGER_START, BOOL}},

openpilot/common/tests/test_params.py

@@ -106,6 +106,13 @@ class TestParams(OpenpilotTestCase):
     assert q.get("CarParams") is None
     assert q.get("CarParams", True) == b"1"
 
+  def test_flush_non_blocking_writes(self):
+    self.params.put("DongleId", "first")
+    self.params.put("DongleId", "last")
+    self.params.flush()
+
+    assert self.params.get("DongleId") == "last"
+
   def test_params_all_keys(self):
     keys = Params().all_keys()
 

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, current_curvature=self.curvature,
+                                                        v_ego=CS.vEgo, active=CC.latActive)
+      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,194 @@
+from collections import deque
+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)
+
+DBC_OFFSET_RESOLUTION = 0.01
+DBC_ANGLE_RESOLUTION = 0.0005
+_PATH_MIN_LOOKAHEAD = 7.0
+_POSE_PREDICTION_TIME = 0.1
+_POSE_BLEND_CURVATURE = (0.006, 0.012)
+_PATH_OFFSET_RATE = 4.0
+_PATH_ANGLE_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 _sample(distance: float, distances: list[float], values: list[float]) -> float:
+  return float(np.interp(distance, distances, values))
+
+
+def _blend_share(demand: float) -> float:
+  lower, upper = _POSE_BLEND_CURVATURE
+  return float(np.clip((demand - lower) / (upper - lower), 0.0, 1.0))
+
+
+def _model_path(model) -> tuple[list[float], 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, x, y, unwrapped_heading
+
+
+def _predicted_pose(distance: float, current_curvature: float,
+                    curvature_delta: float) -> tuple[float, float, float]:
+  curvature = current_curvature + 0.5 * curvature_delta
+  heading = curvature * distance
+  if abs(curvature) < 1e-9:
+    return distance, 0.0, 0.0
+  return math.sin(heading) / curvature, (1.0 - math.cos(heading)) / curvature, heading
+
+
+def _relative_pose(target_distance: float, path: tuple[list[float], list[float], list[float], list[float]],
+                   vehicle_pose: tuple[float, float, float]) -> tuple[float, float]:
+  distance, x, y, heading = path
+  vehicle_x, vehicle_y, vehicle_heading = vehicle_pose
+  dx = _sample(target_distance, distance, x) - vehicle_x
+  dy = _sample(target_distance, distance, y) - vehicle_y
+  cosine = math.cos(vehicle_heading)
+  sine = math.sin(vehicle_heading)
+  offset = -sine * dx + cosine * dy
+  angle = math.atan2(math.sin(_sample(target_distance, distance, heading) - vehicle_heading),
+                     math.cos(_sample(target_distance, distance, heading) - vehicle_heading))
+  return offset, angle
+
+
+def _path_pose(target_distance: float,
+               path: tuple[list[float], list[float], list[float], list[float]]) -> tuple[float, float, float]:
+  distance, x, y, heading = path
+  return (_sample(target_distance, distance, x), _sample(target_distance, distance, y),
+          _sample(target_distance, distance, heading))
+
+
+def _bounded_feedback(feedforward: float, feedback: float, resolution: float, zero_path_limit: float) -> float:
+  quantization_threshold = 0.5 * resolution
+  limit = max(abs(feedforward) - resolution, 0.0) if abs(feedforward) >= quantization_threshold else zero_path_limit
+  return float(np.clip(feedback, -limit, limit))
+
+
+def _encode_path(path: tuple[list[float], list[float], list[float], list[float]], desired_curvature: float,
+                 current_curvature: float, curvature_delta: float, v_ego: float) -> FordPath:
+  distance, _, _, _ = path
+  advance = min(v_ego * _POSE_PREDICTION_TIME, distance[-1])
+  offset_horizon = min(_PATH_MIN_LOOKAHEAD, distance[-1] - advance)
+  angle_horizon = min(max(v_ego, _PATH_MIN_LOOKAHEAD), distance[-1] - advance)
+
+  # Keep the model's remaining path as feedforward. Measured vehicle motion is
+  # a separate, short delay-aligned correction, so catching the requested
+  # curvature cannot erase a turn that is still present in the model path.
+  model_pose = _path_pose(advance, path)
+  model_offset, _ = _relative_pose(advance + offset_horizon, path, model_pose)
+  _, model_angle = _relative_pose(advance + angle_horizon, path, model_pose)
+  vehicle_pose = _predicted_pose(advance, current_curvature, curvature_delta)
+  feedback_offset, feedback_angle = _relative_pose(advance, path, vehicle_pose)
+  gentle_curvature = _POSE_BLEND_CURVATURE[0]
+  feedback_offset = _bounded_feedback(model_offset, feedback_offset, DBC_OFFSET_RESOLUTION,
+                                       0.5 * gentle_curvature * advance ** 2)
+  feedback_angle = _bounded_feedback(model_angle, feedback_angle, DBC_ANGLE_RESOLUTION,
+                                      gentle_curvature * advance)
+
+  offset_curvature = 2.0 * model_offset / max(offset_horizon, 1e-3) ** 2
+  angle_curvature = model_angle / max(angle_horizon, 1e-3)
+  pose_share = _blend_share(max(abs(offset_curvature), abs(angle_curvature)))
+
+  # Only curvature common to both model offset and heading is steady enough
+  # for sticky C2. C0/C1 carry the changing remainder and measured pose error.
+  same_direction = offset_curvature * angle_curvature > 0.0
+  model_support = math.copysign(min(abs(offset_curvature), abs(angle_curvature)), model_angle) if same_direction else 0.0
+  curvature = float(np.clip(model_support, -_POSE_BLEND_CURVATURE[0], _POSE_BLEND_CURVATURE[0])) * (1.0 - pose_share)
+  if curvature * desired_curvature < 0.0:
+    curvature = 0.0
+
+  path_offset = model_offset - 0.5 * curvature * offset_horizon ** 2 + feedback_offset
+  path_angle = model_angle - curvature * angle_horizon + feedback_angle
+  if abs(path_offset) < 0.5 * DBC_OFFSET_RESOLUTION:
+    path_offset = 0.0
+  if abs(path_angle) < 0.5 * DBC_ANGLE_RESOLUTION:
+    path_angle = 0.0
+  limited_path_angle = float(np.clip(path_angle, *DBC_ANGLE))
+  path_offset += (path_angle - limited_path_angle) * offset_horizon
+  return FordPath(
+    valid=True,
+    path_offset=float(np.clip(path_offset, *DBC_OFFSET)),
+    path_angle=limited_path_angle,
+    curvature=float(np.clip(curvature, *DBC_CURVATURE)),
+    curvature_rate=0.0,
+  )
+
+
+class FordPathController:
+  """Blend normal C2 following into the model's forward C0/C1 pose."""
+
+  def __init__(self, dt: float = 0.01):
+    self.dt = dt
+    self._last_path = FordPath(valid=True)
+    self._curvature_history = deque(maxlen=max(round(_POSE_PREDICTION_TIME / dt) + 1, 2))
+
+  def _limit(self, target: FordPath) -> FordPath:
+    offset_delta = target.path_offset - self._last_path.path_offset
+    angle_delta = target.path_angle - self._last_path.path_angle
+    scale = min(
+      1.0,
+      _PATH_OFFSET_RATE * self.dt / abs(offset_delta) if offset_delta else 1.0,
+      _PATH_ANGLE_RATE * self.dt / abs(angle_delta) if angle_delta else 1.0,
+    )
+    self._last_path = FordPath(
+      True,
+      self._last_path.path_offset + scale * offset_delta,
+      self._last_path.path_angle + scale * angle_delta,
+      self._last_path.curvature + scale * (target.curvature - self._last_path.curvature),
+      0.0,
+    )
+    return self._last_path
+
+  def update(self, model, desired_curvature: float, *, current_curvature: float = 0.0,
+             v_ego: float = 0.0, active: bool = True) -> FordPath:
+    if not active:
+      self._last_path = FordPath(valid=True)
+      self._curvature_history.clear()
+      return FordPath()
+    current_curvature = _finite(current_curvature)
+    self._curvature_history.append(current_curvature)
+    curvature_delta = (current_curvature - self._curvature_history[0]
+                       if len(self._curvature_history) == self._curvature_history.maxlen else 0.0)
+    path = _model_path(model) if model is not None else None
+    if path is None:
+      return self._limit(FordPath(valid=True))
+    return self._limit(_encode_path(path, _finite(desired_curvature), current_curvature, curvature_delta,
+                                    max(_finite(v_ego), 0.0)))

openpilot/selfdrive/controls/tests/test_ford_path.py

@@ -0,0 +1,330 @@
+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_ANGLE, DBC_CURVATURE, DBC_OFFSET, FordPathController,
+                                                        _bounded_feedback, _encode_path, _model_path, _predicted_pose,
+                                                        _relative_pose)
+
+
+def _path(curvature: float, speed: float = 8.0):
+  t = np.linspace(0.0, 3.0, 61)
+  distance = speed * t
+  heading = curvature * distance
+  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 _changing_path(start_curvature: float, end_curvature: float, speed: float = 8.0):
+  t = np.linspace(0.0, 3.0, 61)
+  distance = speed * t
+  curvature = np.interp(distance, [distance[0], min(distance[-1], 7.0)], [start_curvature, end_curvature])
+  heading = np.zeros_like(distance)
+  x = np.zeros_like(distance)
+  y = np.zeros_like(distance)
+  for i in range(1, len(distance)):
+    ds = distance[i] - distance[i - 1]
+    heading[i] = heading[i - 1] + 0.5 * (curvature[i] + curvature[i - 1]) * ds
+    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 _command(model, desired_curvature: float, *, current_curvature: float = 0.0, v_ego: float = 8.0):
+  return FordPathController(dt=1.0).update(model, desired_curvature, current_curvature=current_curvature, v_ego=v_ego)
+
+
+def _equivalent_curvature(command) -> float:
+  return 2.0 * command.path_offset / 7.0 ** 2 + 2.0 * command.path_angle / 7.0 + command.curvature
+
+
+def test_gentle_path_uses_only_c2():
+  command = _command(_path(0.004, speed=20.0), 0.004, current_curvature=0.004, v_ego=20.0)
+  assert command.valid
+  assert command.path_offset == 0.0
+  assert command.path_angle == 0.0
+  assert np.isclose(command.curvature, 0.004, atol=1e-6)
+  assert command.curvature_rate == 0.0
+
+
+def test_spatially_growing_path_adds_fast_pose_before_action_becomes_large():
+  controller = FordPathController(dt=1.0)
+  command = controller.update(_changing_path(0.0, 0.04), 0.012, current_curvature=0.0, v_ego=8.0)
+  assert command.path_offset > 0.0
+  assert command.path_angle > 0.0
+  assert command.curvature < 0.012
+  assert command.curvature_rate == 0.0
+
+
+def test_growing_model_pose_adds_authority_but_c3_is_never_transmitted():
+  constant = _command(_path(0.012), 0.012)
+  growing = _command(_changing_path(0.0, 0.04), 0.012)
+  assert _equivalent_curvature(growing) > _equivalent_curvature(constant)
+  assert constant.curvature_rate == 0.0
+  assert growing.curvature_rate == 0.0
+
+
+def test_local_tracking_error_corrects_without_replacing_forward_pose():
+  model = _changing_path(0.0, 0.04)
+  local_curvature = 0.5 * 0.04 * 2.0 / 7.0
+  aligned = _command(model, 0.012, current_curvature=local_curvature)
+  under = _command(model, 0.012, current_curvature=0.0)
+  assert aligned.path_offset > 0.0
+  assert aligned.path_angle > 0.0
+  assert under.path_offset > aligned.path_offset
+  assert under.path_angle > aligned.path_angle
+
+
+def test_large_maneuver_uses_fast_pose_and_zeros_c2():
+  command = _command(_path(0.04), 0.04)
+  assert command.path_offset > 0.5
+  assert command.path_angle > 0.2
+  assert command.curvature == 0.0
+  assert command.curvature_rate == 0.0
+
+
+def test_model_pose_can_trigger_maneuver_when_action_is_late():
+  command = _command(_path(0.04), 0.002)
+  assert command.path_offset > 0.5
+  assert command.path_angle > 0.2
+  assert command.curvature == 0.0
+
+
+def test_model_pose_preserves_a_gentle_arc_when_the_action_collapses():
+  command = _command(_path(0.006), 0.002, current_curvature=0.006)
+  assert command.path_offset == 0.0
+  assert command.path_angle == 0.0
+  assert np.isclose(command.curvature, 0.006, atol=5e-6)
+
+
+def test_changing_gentle_curve_keeps_only_its_common_part_in_c2():
+  command = _command(_changing_path(0.0, 0.008), 0.004, current_curvature=0.0)
+  assert 0.0 < command.curvature < 0.004
+  assert command.path_angle > 0.0
+
+
+def test_action_cannot_invent_a_maneuver_missing_from_the_model_path():
+  command = _command(_path(0.002), 0.04)
+  assert command.path_offset == 0.0
+  assert 0.0 < command.path_angle < 0.002
+  assert np.isclose(command.curvature, 0.002)
+
+
+def test_nearby_demands_blend_continuously_without_a_mode_threshold():
+  low = _command(_path(0.0119), 0.0119)
+  high = _command(_path(0.0121), 0.0121)
+  assert abs(high.path_offset - low.path_offset) < 0.05
+  assert abs(high.path_angle - low.path_angle) < 0.03
+  assert abs(high.curvature - low.curvature) < 0.001
+
+
+def test_leaving_c2_normal_band_does_not_drop_total_authority():
+  normal = _command(_path(0.006), 0.006)
+  transition = _command(_path(0.0061), 0.0061)
+  assert transition.curvature <= normal.curvature
+  assert _equivalent_curvature(transition) >= _equivalent_curvature(normal)
+
+
+def test_low_speed_still_uses_available_model_pose():
+  command = _command(_path(0.04, speed=2.0), 0.04, v_ego=2.0)
+  assert command.path_offset > 0.0
+  assert command.path_angle > 0.0
+
+
+def test_higher_speed_advances_predicted_pose_and_extends_heading_horizon():
+  model = _changing_path(0.0, 0.015, speed=20.0)
+  slow = _command(model, 0.012, v_ego=7.0)
+  fast = _command(model, 0.012, v_ego=20.0)
+  assert fast.path_offset > slow.path_offset
+  assert fast.path_angle > slow.path_angle
+
+
+def test_short_model_uses_available_endpoint():
+  model = _path(0.04, speed=1.0)
+  command = _command(model, 0.04, v_ego=1.0)
+  assert command.valid
+  assert command.path_offset > 0.0
+  assert command.path_angle > 0.0
+
+
+def test_turn_entry_coordinates_c2_release_with_fast_pose_attack():
+  controller = FordPathController(dt=0.01)
+  for _ in range(20):
+    assert controller.update(_path(0.004), 0.004, v_ego=8.0).curvature > 0.0
+  outputs = [controller.update(_path(0.04), 0.04, current_curvature=0.01, v_ego=8.0) for _ in range(100)]
+  assert 0.0 < outputs[0].curvature < 0.004
+  assert outputs[0].path_offset > 0.0
+  assert outputs[0].path_angle > 0.0
+  assert outputs[-1].curvature == 0.0
+
+
+def test_turn_exit_allows_c2_to_take_over_while_fast_pose_drains():
+  controller = FordPathController(dt=0.01)
+  for _ in range(20):
+    controller.update(_path(0.04), 0.04, current_curvature=0.02, v_ego=8.0)
+  outputs = [controller.update(_path(0.004), 0.004, current_curvature=0.004, v_ego=8.0) for _ in range(100)]
+  assert 0.0 < outputs[0].curvature < 0.004
+  assert outputs[0].path_offset != 0.0 or outputs[0].path_angle != 0.0
+  assert outputs[-1].path_offset == 0.0
+  assert outputs[-1].path_angle == 0.0
+
+
+def test_100hz_handoff_preserves_total_authority_without_entry_drop_or_exit_overshoot():
+  controller = FordPathController(dt=0.01)
+  normal = controller.update(_path(0.006), 0.006, current_curvature=0.006, v_ego=8.0)
+  entries = [controller.update(_path(0.04), 0.04, current_curvature=0.01, v_ego=8.0) for _ in range(100)]
+  entry_authority = np.asarray([_equivalent_curvature(command) for command in entries])
+  assert np.all(np.diff(entry_authority) >= -1e-9)
+  assert entry_authority[0] >= _equivalent_curvature(normal)
+
+  exits = [controller.update(_path(0.004), 0.004, current_curvature=0.004, v_ego=8.0) for _ in range(100)]
+  exit_authority = np.asarray([_equivalent_curvature(command) for command in exits])
+  assert np.all(np.diff(exit_authority) <= 1e-9)
+  assert np.all(exit_authority >= 0.004 - 1e-9)
+
+
+def test_measured_tracking_error_closes_bidirectionally_without_abandoning_the_turn():
+  model = _path(0.04)
+  under = _command(model, 0.04, current_curvature=0.005)
+  on_target = _command(model, 0.04, current_curvature=0.04)
+  over = _command(model, 0.04, current_curvature=0.05)
+  assert under.path_offset > on_target.path_offset
+  assert under.path_angle > on_target.path_angle
+  assert 0.0 < over.path_offset < on_target.path_offset
+  assert 0.0 < over.path_angle < on_target.path_angle
+
+
+def test_gentle_curve_keeps_c2_and_adds_only_a_small_fast_tracking_trim():
+  model = _path(0.004)
+  under = _command(model, 0.004, current_curvature=0.002)
+  on_target = _command(model, 0.004, current_curvature=0.004)
+  over = _command(model, 0.004, current_curvature=0.006)
+  assert under.path_offset == on_target.path_offset == over.path_offset == 0.0
+  assert 0.0 < under.path_angle < 0.002
+  assert on_target.path_angle == 0.0
+  assert -0.002 < over.path_angle < 0.0
+  assert np.allclose([under.curvature, on_target.curvature, over.curvature], 0.004, atol=2e-6)
+
+
+def test_overshoot_trim_cannot_erase_a_modeled_turn():
+  model = _path(0.04)
+  on_target = _command(model, 0.04, current_curvature=0.04)
+  over = _command(model, 0.04, current_curvature=0.06)
+  assert over.path_offset > 0.95 * on_target.path_offset
+  assert over.path_angle > 0.9 * on_target.path_angle
+
+
+def test_corrupt_measured_curvature_cannot_reverse_a_modeled_turn():
+  command = _command(_path(0.04), 0.04, current_curvature=0.5)
+  assert command.path_offset > 0.0
+  assert command.path_angle > 0.0
+  assert command.curvature == 0.0
+
+
+def test_feedback_preserves_half_lsb_feedforward_direction():
+  for feedforward, resolution in ((0.006, 0.01), (0.0004, 0.0005)):
+    result = feedforward + _bounded_feedback(feedforward, -1.0, resolution, 1.0)
+    assert result >= 0.5 * resolution
+
+
+def test_recent_curvature_trend_advances_vehicle_pose_without_a_response_gain():
+  model = _model_path(_path(0.04))
+  assert model is not None
+  constant = _encode_path(model, 0.04, current_curvature=0.02, curvature_delta=0.0, v_ego=8.0)
+  rising = _encode_path(model, 0.04, current_curvature=0.02, curvature_delta=0.01, v_ego=8.0)
+  assert 0.0 < rising.path_offset < constant.path_offset
+  assert 0.0 < rising.path_angle < constant.path_angle
+
+
+def test_model_path_exit_zeros_lingering_c2_and_countersteers():
+  command = _command(_path(0.0), 0.004, current_curvature=0.006)
+  assert command.path_offset <= 0.0
+  assert command.path_angle < 0.0
+  assert command.curvature == 0.0
+
+
+def test_model_path_reversal_zeros_opposing_lingering_c2():
+  command = _command(_path(-0.004), 0.004, current_curvature=0.002)
+  assert command.path_offset < 0.0
+  assert command.path_angle < 0.0
+  assert command.curvature == 0.0
+
+
+def test_s_turn_reverses_model_pose_without_slow_c2():
+  controller = FordPathController(dt=0.05)
+  for _ in range(10):
+    controller.update(_path(0.04), 0.04, v_ego=8.0)
+  outputs = [controller.update(_path(-0.04), -0.04, v_ego=8.0) for _ in range(10)]
+  assert all(command.curvature == 0.0 for command in outputs)
+  assert np.all(np.diff([command.path_offset for command in outputs]) < 0.0)
+  assert np.all(np.diff([command.path_angle for command in outputs]) < 0.0)
+  assert outputs[-1].path_offset < 0.0
+  assert outputs[-1].path_angle < 0.0
+
+
+def test_output_limits_and_rates_are_bounded():
+  controller = FordPathController()
+  outputs = [controller.update(_path(0.2), 0.2, v_ego=8.0) for _ in range(100)]
+  assert all(DBC_OFFSET[0] <= command.path_offset <= DBC_OFFSET[1] for command in outputs)
+  assert all(DBC_ANGLE[0] <= command.path_angle <= DBC_ANGLE[1] for command in outputs)
+  assert all(DBC_CURVATURE[0] <= command.curvature <= DBC_CURVATURE[1] for command in outputs)
+  assert np.max(np.abs(np.diff([command.path_offset for command in outputs]))) <= 0.04 + 1e-9
+  assert np.max(np.abs(np.diff([command.path_angle for command in outputs]))) <= 0.01 + 1e-9
+
+
+def test_clipped_path_angle_uses_available_offset_to_preserve_endpoint():
+  horizon = 7.0
+  for curvature, angle_limit in ((-0.1, DBC_ANGLE[0]), (0.1, DBC_ANGLE[1])):
+    model = _path(curvature)
+    command = _command(model, curvature, current_curvature=curvature, v_ego=horizon)
+    path = _model_path(model)
+    assert path is not None
+    advance = 0.1 * horizon
+    model_offset, model_angle = _relative_pose(advance + horizon, path,
+                                                _predicted_pose(advance, curvature, 0.0))
+
+    assert command.path_angle == angle_limit
+    assert np.isclose(command.path_offset + horizon * command.path_angle,
+                      model_offset + horizon * model_angle)
+
+
+def test_invalid_model_ramps_pose_to_zero_and_inactive_resets():
+  controller = FordPathController(dt=0.01)
+  for _ in range(20):
+    active = controller.update(_path(0.04), 0.04, v_ego=8.0)
+  invalid = controller.update(None, 0.0, v_ego=8.0)
+  assert invalid.valid
+  assert abs(invalid.path_offset) < abs(active.path_offset)
+  assert abs(invalid.path_angle) < abs(active.path_angle)
+  assert not controller.update(_path(0.0), 0.0, v_ego=8.0, active=False).valid
+
+
+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

openpilot/selfdrive/selfdrived/selfdrived.py

@@ -32,7 +32,14 @@ from openpilot.sunnypilot.selfdrive.car.car_specific import CarSpecificEventsSP
 from openpilot.sunnypilot.selfdrive.car.cruise_helpers import CruiseHelper
 from openpilot.sunnypilot.selfdrive.car.intelligent_cruise_button_management.controller import IntelligentCruiseButtonManagement
 from openpilot.sunnypilot.selfdrive.selfdrived.button_state_tracker import ButtonStateTracker
+from openpilot.sunnypilot.selfdrive.selfdrived.assisted_driving_milestones import (
+  AssistCategory,
+  AssistedDrivingMilestones,
+  MilestoneEvent,
+  MilestoneStore,
+)
 from openpilot.sunnypilot.selfdrive.selfdrived.events import EventsSP
+from openpilot.sunnypilot.system.statsd import statlog
 
 REPLAY = "REPLAY" in os.environ
 SIMULATION = "SIMULATION" in os.environ
@@ -88,7 +95,8 @@ class SelfdriveD(CruiseHelper):
     self.big_model_ready_t = 0.
 
     # Setup sockets
-    self.pm = messaging.PubMaster(['selfdriveState', 'onroadEvents'] + ['selfdriveStateSP', 'onroadEventsSP'])
+    self.pm = messaging.PubMaster(['selfdriveState', 'onroadEvents'] +
+                                  ['selfdriveStateSP', 'onroadEventsSP', 'assistedDrivingMilestoneState'])
 
     self.gps_location_service = get_gps_location_service(self.params)
     self.gps_packets = [self.gps_location_service]
@@ -127,6 +135,7 @@ class SelfdriveD(CruiseHelper):
       self.params.remove("ExperimentalMode")
 
     self.CS_prev = car.CarState.new_message()
+    self.car_state_log_mono_time = 0
     self.AM = AlertManager()
     self.events = Events()
 
@@ -137,6 +146,11 @@ class SelfdriveD(CruiseHelper):
     self.cruise_mismatch_counter = 0
     self.last_steering_pressed_frame = 0
     self.distance_traveled = 0
+    self.assisted_driving_milestones = AssistedDrivingMilestones(MilestoneStore(self.params))
+    self.assisted_driving_milestones_enabled = bool(self.params.get("AssistedDrivingMilestonesEnabled", return_default=True))
+    self.assisted_driving_milestone_drive_id = ""
+    self._milestone_event: MilestoneEvent | None = None
+    self._milestone_event_expires_ns = 0
     self.last_functional_fan_frame = 0
     self.events_prev = []
     self.logged_comm_issue = None
@@ -528,6 +542,8 @@ class SelfdriveD(CruiseHelper):
   def data_sample(self):
     _car_state = messaging.recv_one(self.car_state_sock)
     CS = _car_state.carState if _car_state else self.CS_prev
+    if _car_state is not None:
+      self.car_state_log_mono_time = _car_state.logMonoTime
 
     self.sm.update(0)
 
@@ -646,6 +662,31 @@ class SelfdriveD(CruiseHelper):
       self.pm.send('onroadEventsSP', ce_send_sp)
     self.events_sp_prev = self.events_sp.names.copy()
 
+  def publish_assisted_driving_milestones(self, now_ns: int, event: MilestoneEvent | None) -> None:
+    if event is not None:
+      self._milestone_event = event
+      self._milestone_event_expires_ns = now_ns + 1_000_000_000
+    elif now_ns >= self._milestone_event_expires_ns:
+      self._milestone_event = None
+
+    if event is None and self.sm.frame % 10 != 0:
+      return
+
+    snapshot = self.assisted_driving_milestones.snapshot()
+    msg = messaging.new_message("assistedDrivingMilestoneState")
+    msg.valid = True
+    state = msg.assistedDrivingMilestoneState
+    state.enabled = self.assisted_driving_milestones_enabled
+    state.madsDistanceMeters = snapshot.distances_meters[AssistCategory.MADS]
+    state.fullAssistDistanceMeters = snapshot.distances_meters[AssistCategory.FULL_ASSIST]
+    if self._milestone_event is not None:
+      state.event.id = self._milestone_event.event_id
+      state.event.category = self._milestone_event.category.value
+      state.event.distanceMeters = self._milestone_event.distance_meters
+      state.event.previousDistanceMeters = self._milestone_event.previous_distance_meters
+      state.event.unit = self._milestone_event.unit.value
+    self.pm.send("assistedDrivingMilestoneState", msg)
+
   def step(self):
     CS = self.data_sample()
     self.update_events(CS)
@@ -655,6 +696,28 @@ class SelfdriveD(CruiseHelper):
       self.mads.update(CS)
     self.update_alerts(CS)
 
+    now_ns = time.monotonic_ns()
+    if not self.assisted_driving_milestone_drive_id:
+      self.assisted_driving_milestone_drive_id = self.params.get("CurrentRoute") or ""
+      self.assisted_driving_milestones.set_drive_id(self.assisted_driving_milestone_drive_id)
+    car_control = self.sm['carControl']
+    milestone_event = self.assisted_driving_milestones.update(
+      self.car_state_log_mono_time,
+      CS.vEgo,
+      lat_active=car_control.latActive,
+      long_active=car_control.longActive,
+      is_metric=self.is_metric,
+      enabled=self.assisted_driving_milestones_enabled,
+    )
+    if milestone_event is not None:
+      cloudlog.event("assisted_driving_milestone_reached",
+                     event_id=milestone_event.event_id,
+                     category=milestone_event.category.value,
+                     distance_meters=milestone_event.distance_meters)
+      statlog.gauge(f"assisted_driving_milestone.{milestone_event.category.value}.meters",
+                    milestone_event.distance_meters)
+    self.publish_assisted_driving_milestones(now_ns, milestone_event)
+
     self.button_state_tracker.update(CS)
     self.publish_selfdriveState(CS)
 
@@ -667,6 +730,7 @@ class SelfdriveD(CruiseHelper):
       self.disengage_on_accelerator = self.params.get_bool("DisengageOnAccelerator")
       self.experimental_mode = self.params.get_bool("ExperimentalMode") and self.CP.openpilotLongitudinalControl
       self.personality = self.params.get("LongitudinalPersonality", return_default=True)
+      self.assisted_driving_milestones_enabled = bool(self.params.get("AssistedDrivingMilestonesEnabled", return_default=True))
 
       self.mads.read_params()
       time.sleep(0.1)
@@ -680,6 +744,7 @@ class SelfdriveD(CruiseHelper):
         self.step()
         self.rk.monitor_time()
     finally:
+      self.assisted_driving_milestones.close()
       e.set()
       t.join()
 

openpilot/selfdrive/ui/mici/layouts/main.py

@@ -1,5 +1,8 @@
+import os
+
 import pyray as rl
 import openpilot.cereal.messaging as messaging
+from openpilot.common.hardware import PC
 from openpilot.selfdrive.ui.mici.layouts.home import MiciHomeLayout
 from openpilot.selfdrive.ui.mici.layouts.settings.settings import SettingsLayout
 from openpilot.selfdrive.ui.mici.layouts.offroad_alerts import MiciOffroadAlerts
@@ -61,7 +64,8 @@ class MiciMainLayout(Scroller):
 
     # Start onboarding if terms or training not completed, make sure to push after self
     self._onboarding_window = OnboardingWindow(lambda: gui_app.pop_widgets_to(self))
-    if not self._onboarding_window.completed:
+    skip_onboarding_for_milestone_preview = PC and os.getenv("SP_MILESTONE_PREVIEW") == "1"
+    if not self._onboarding_window.completed and not skip_onboarding_for_milestone_preview:
       gui_app.push_widget(self._onboarding_window)
 
     # initialize correct onroad layout
@@ -119,6 +123,8 @@ class MiciMainLayout(Scroller):
         self._onroad_time_delay = rl.get_time()
       else:
         self._scroll_to(self._home_layout)
+        if hasattr(self._home_layout, "request_drive_summary"):
+          self._home_layout.request_drive_summary()
 
     # FIXME: these two pops can interrupt user interacting in the settings
     if self._onroad_time_delay is not None and rl.get_time() - self._onroad_time_delay >= ONROAD_DELAY:

openpilot/selfdrive/ui/mici/layouts/settings/toggles.py

@@ -47,6 +47,7 @@ class TogglesLayoutMici(NavScroller):
     is_metric_toggle = BigParamControl("use metric units", "IsMetric")
     ldw_toggle = BigParamControl("lane departure warnings", "IsLdwEnabled")
     always_on_dm_toggle = BigParamControl("always-on driver monitor", "AlwaysOnDM")
+    milestone_celebrations_toggle = BigParamControl("assisted driving milestones", "AssistedDrivingMilestonesEnabled")
     record_front = BigParamControl("record & upload cabin camera", "RecordFront", toggle_callback=restart_needed_callback)
     record_mic = BigParamControl("record & upload mic audio", "RecordAudio", toggle_callback=restart_needed_callback)
     enable_openpilot = BigParamControl("enable sunnypilot", "OpenpilotEnabledToggle", toggle_callback=restart_needed_callback)
@@ -57,6 +58,7 @@ class TogglesLayoutMici(NavScroller):
       is_metric_toggle,
       ldw_toggle,
       always_on_dm_toggle,
+      milestone_celebrations_toggle,
       record_front,
       record_mic,
       enable_openpilot,
@@ -68,6 +70,7 @@ class TogglesLayoutMici(NavScroller):
       ("IsMetric", is_metric_toggle),
       ("IsLdwEnabled", ldw_toggle),
       ("AlwaysOnDM", always_on_dm_toggle),
+      ("AssistedDrivingMilestonesEnabled", milestone_celebrations_toggle),
       ("RecordFront", record_front),
       ("RecordAudio", record_mic),
       ("OpenpilotEnabledToggle", enable_openpilot),

openpilot/selfdrive/ui/mici/onroad/alert_renderer.py

@@ -20,6 +20,7 @@ AlertSize = log.SelfdriveState.AlertSize
 AlertStatus = log.SelfdriveState.AlertStatus
 
 ALERT_MARGIN = 18
+ALERT_BACKGROUND_OPACITY = 0.90
 
 ALERT_FONT_SMALL = 66 - 50
 ALERT_FONT_BIG = 88 - 40
@@ -279,7 +280,7 @@ class AlertRenderer(Widget, SpeedLimitAlertRenderer):
   def _draw_background(self, alert: Alert) -> None:
     # draw top gradient for alert text at top
     color = ALERT_COLORS.get(alert.status, ALERT_COLORS[AlertStatus.normal])
-    color = rl.Color(color.r, color.g, color.b, int(255 * 0.90 * self._alpha_filter.x))
+    color = rl.Color(color.r, color.g, color.b, int(255 * ALERT_BACKGROUND_OPACITY * self._alpha_filter.x))
     translucent_color = rl.Color(color.r, color.g, color.b, int(0 * self._alpha_filter.x))
 
     small_alert_height = round(self._rect.height * 0.583) # 140px at mici height

openpilot/selfdrive/ui/mici/onroad/augmented_road_view.py

@@ -19,10 +19,15 @@ from openpilot.common.transformations.camera import DEVICE_CAMERAS, DeviceCamera
 from openpilot.common.transformations.orientation import rot_from_euler
 from enum import IntEnum
 
+MILESTONE_CELEBRATION_ENABLED = gui_app.sunnypilot_ui()
+
 if gui_app.sunnypilot_ui():
   from openpilot.selfdrive.ui.sunnypilot.mici.onroad.hud_renderer import HudRendererSP as HudRenderer
   from openpilot.selfdrive.ui.sunnypilot.ui_state import OnroadTimerStatus
 
+  if MILESTONE_CELEBRATION_ENABLED:
+    from openpilot.selfdrive.ui.sunnypilot.onroad.milestone_celebration import MilestoneCelebration
+
 OpState = log.SelfdriveState.OpenpilotState
 CALIBRATED = log.ExtrinsicsCalibration.Status.calibrated
 NARROW_ROAD_CAM = VisionStreamType.VISION_STREAM_NARROW_ROAD
@@ -156,6 +161,7 @@ class AugmentedRoadView(CameraView):
     self._alert_renderer = AlertRenderer()
     self._driver_state_renderer = DriverStateRenderer()
     self._confidence_ball = ConfidenceBall()
+    self._milestone_celebration = self._child(MilestoneCelebration()) if MILESTONE_CELEBRATION_ENABLED else None
     self._offroad_label = UnifiedLabel("start the car to\nuse sunnypilot", 54, FontWeight.DISPLAY,
                                        text_color=rl.Color(255, 255, 255, int(255 * 0.9)),
                                        alignment=rl.GuiTextAlignment.TEXT_ALIGN_CENTER,
@@ -223,6 +229,12 @@ class AugmentedRoadView(CameraView):
 
     alert_to_render, not_animating_out = self._alert_renderer.will_render()
 
+    if self._milestone_celebration is not None:
+      if alert_to_render is not None:
+        self._milestone_celebration.cancel_for_alert()
+      else:
+        self._milestone_celebration.render(self._content_rect)
+
     # Hide DMoji when disengaged unless AlwaysOnDM is enabled
     should_draw_dmoji = (not self._hud_renderer.drawing_top_icons() and
                          (ui_state.status != UIStatus.DISENGAGED or ui_state.always_on_dm))
@@ -247,7 +259,6 @@ class AugmentedRoadView(CameraView):
     self._confidence_ball.render(self.rect)
 
     self._bookmark_icon.render(self.rect)
-
   def _switch_stream_if_needed(self, sm):
     if sm['selfdriveState'].experimentalMode and WIDE_CAM in self.available_streams:
       v_ego = sm['carState'].vEgo
@@ -355,10 +366,12 @@ class AugmentedRoadView(CameraView):
     return self._cached_matrix
 
   def show_event(self):
+    super().show_event()
     if gui_app.sunnypilot_ui():
       ui_state.reset_onroad_sleep_timer(OnroadTimerStatus.RESUME)
 
   def hide_event(self):
+    super().hide_event()
     if gui_app.sunnypilot_ui():
       ui_state.reset_onroad_sleep_timer(OnroadTimerStatus.PAUSE)
 

openpilot/selfdrive/ui/soundd.py

@@ -24,14 +24,8 @@ ALERT_RAMP_TIME = 4 # seconds to ramp to max volume for warningImmediate
 SELFDRIVE_STATE_TIMEOUT = 5 # 5 seconds
 FILTER_DT = 1. / (micd.SAMPLE_RATE / micd.FFT_SAMPLES)
 
-AMBIENT_DB = 26 # DB where MIN_VOLUME is applied
 DB_SCALE = 30 # AMBIENT_DB + DB_SCALE is where MAX_VOLUME is applied
 
-VOLUME_BASE = 20
-if HARDWARE.get_device_type() == "tizi":
-  AMBIENT_DB = 30
-  VOLUME_BASE = 10
-
 AudibleAlert = log.SelfdriveState.AudibleAlert
 AudibleAlertSP = custom.SelfdriveStateSP.AudibleAlert
 
@@ -53,6 +47,7 @@ sound_list: dict[int, tuple[str, int | None, float]] = {
   AudibleAlert.promptDistracted: ("dm_warning.wav", None, MAX_VOLUME),
 
   AudibleAlert.preAlert: ("pre_alert.wav", 1, MAX_VOLUME),
+  AudibleAlert.complete: ("milestone.wav", 1, MAX_VOLUME),
 
   AudibleAlert.warningSoft: ("critical.wav", None, MAX_VOLUME),
   AudibleAlert.warningImmediate: ("dm_critical.wav", None, MAX_VOLUME),
@@ -60,6 +55,14 @@ sound_list: dict[int, tuple[str, int | None, float]] = {
   **sound_list_sp,
 }
 
+
+def calculate_volume_for_device(weighted_db: float, device_type: str) -> float:
+  ambient_db = 30 if device_type in ("mici", "tizi") else 26
+  volume_base = 10 if device_type in ("mici", "tizi") else 20
+  volume_boost = 1.5 if device_type == "mici" else 1.0
+  volume = ((weighted_db - ambient_db) / DB_SCALE) * (MAX_VOLUME - MIN_VOLUME) + MIN_VOLUME
+  return min(MAX_VOLUME, volume_boost * math.pow(volume_base, (np.clip(volume, MIN_VOLUME, MAX_VOLUME) - 1)))
+
 def check_selfdrive_timeout_alert(sm):
   ss_missing = time.monotonic() - sm.recv_time['selfdriveState']
 
@@ -74,6 +77,7 @@ class Soundd(QuietMode):
   def __init__(self):
     super().__init__()
 
+    self.device_type = HARDWARE.get_device_type()
     self.load_sounds()
 
     self.current_alert = AudibleAlert.none
@@ -85,6 +89,7 @@ class Soundd(QuietMode):
 
     self.selfdrive_timeout_alert = False
     self.pending_stop = False
+    self.last_milestone_event_id = 0
 
     self.spl_filter_weighted = FirstOrderFilter(0, 2.5, FILTER_DT, initialized=False)
 
@@ -164,9 +169,19 @@ class Soundd(QuietMode):
       self.update_alert(AudibleAlert.none)
       self.selfdrive_timeout_alert = False
 
+  def update_milestone_alert(self, sm):
+    if not sm.updated['assistedDrivingMilestoneState']:
+      return
+    milestone_state = sm['assistedDrivingMilestoneState']
+    event_id = milestone_state.event.id
+    if not milestone_state.enabled or event_id == 0 or event_id == self.last_milestone_event_id:
+      return
+    self.last_milestone_event_id = event_id
+    if self.current_alert == AudibleAlert.none and not self.enabled:
+      self.update_alert(AudibleAlert.complete)
+
   def calculate_volume(self, weighted_db):
-    volume = ((weighted_db - AMBIENT_DB) / DB_SCALE) * (MAX_VOLUME - MIN_VOLUME) + MIN_VOLUME
-    return math.pow(VOLUME_BASE, (np.clip(volume, MIN_VOLUME, MAX_VOLUME) - 1))
+    return calculate_volume_for_device(weighted_db, self.device_type)
 
   @retry(attempts=10, delay=3)
   def get_stream(self, sd):
@@ -180,7 +195,7 @@ class Soundd(QuietMode):
     import sounddevice as sd
     micd.patch_sounddevice(sd)
 
-    sm = messaging.SubMaster(['selfdriveState', 'selfdriveStateSP', 'soundPressure'])
+    sm = messaging.SubMaster(['selfdriveState', 'selfdriveStateSP', 'soundPressure', 'assistedDrivingMilestoneState'])
 
     with self.get_stream(sd) as stream:
       rk = Ratekeeper(20)
@@ -198,6 +213,7 @@ class Soundd(QuietMode):
             self.current_volume = self.calculate_volume(float(self.spl_filter_weighted.x))
 
         self.get_audible_alert(sm)
+        self.update_milestone_alert(sm)
 
         # Ramp up immediate warning sound over 4s
         if self.current_alert == AudibleAlert.warningImmediate:

openpilot/selfdrive/ui/sunnypilot/mici/layouts/home.py

@@ -5,14 +5,28 @@ This file is part of sunnypilot and is licensed under the MIT License.
 See the LICENSE.md file in the root directory for more details.
 """
 import math
+import time
 
 import pyray as rl
 
 from openpilot.selfdrive.ui.mici.layouts.home import MiciHomeLayout
 from openpilot.selfdrive.ui.ui_state import ui_state, ChestnutState
 from openpilot.system.ui.lib.application import FontWeight
+from openpilot.system.ui.lib.multilang import tr
 from openpilot.system.ui.widgets.icon_widget import IconWidget
-from openpilot.system.ui.widgets.label import UnifiedLabel
+from openpilot.system.ui.widgets.label import UnifiedLabel, gui_label
+
+METERS_PER_MILE = 1609.344
+METERS_PER_KILOMETER = 1000.0
+SUMMARY_DURATION_SECONDS = 10.0
+SUMMARY_WAIT_SECONDS = 3.0
+
+
+def _nonnegative_float(value) -> float:
+  try:
+    return max(0.0, float(value))
+  except (TypeError, ValueError):
+    return 0.0
 
 
 class MiciHomeLayoutSP(MiciHomeLayout):
@@ -23,6 +37,52 @@ class MiciHomeLayoutSP(MiciHomeLayout):
     self._chestnut_loading_icon.set_visible(False)
     failed_idx = self._status_bar_layout.widgets.index(self._chestnut_failed_icon)
     self._status_bar_layout.widgets.insert(failed_idx + 1, self._chestnut_loading_icon)
+    initial_summary = ui_state.params.get("LastDriveAssistedDrivingSummary", return_default=True) or {}
+    self._last_summary_id = initial_summary.get("id", 0)
+    self._summary_wait_until = 0.0
+    self._summary_visible_until = 0.0
+    self._drive_summary = {}
+
+  def request_drive_summary(self) -> None:
+    self._summary_wait_until = time.monotonic() + SUMMARY_WAIT_SECONDS
+
+  def _render(self, _: rl.Rectangle) -> None:
+    super()._render(_)
+    now = time.monotonic()
+    if now < self._summary_wait_until:
+      summary = ui_state.params.get("LastDriveAssistedDrivingSummary", return_default=True) or {}
+      summary_id = summary.get("id", 0)
+      if summary_id and summary_id != self._last_summary_id:
+        self._last_summary_id = summary_id
+        distances = summary.get("distancesMeters", {})
+        enabled = ui_state.params.get_bool("AssistedDrivingMilestonesEnabled")
+        if enabled and any(_nonnegative_float(distances.get(category, 0.0)) > 0.0 for category in ("mads", "fullAssist")):
+          self._drive_summary = summary
+          self._summary_visible_until = now + SUMMARY_DURATION_SECONDS
+        self._summary_wait_until = 0.0
+
+    if now < self._summary_visible_until:
+      self._draw_drive_summary(_)
+
+  def _draw_drive_summary(self, rect: rl.Rectangle) -> None:
+    distances = self._drive_summary.get("distancesMeters", {})
+    metric = self._drive_summary.get("unit") == "metric"
+    meters_per_unit = METERS_PER_KILOMETER if metric else METERS_PER_MILE
+    unit = "KM" if metric else "MI"
+    mads = _nonnegative_float(distances.get("mads", 0.0)) / meters_per_unit
+    full_assist = _nonnegative_float(distances.get("fullAssist", 0.0)) / meters_per_unit
+
+    rl.draw_rectangle_rec(rect, rl.Color(0, 0, 0, 235))
+    gui_label(rl.Rectangle(rect.x, rect.y + 14, rect.width, 52), tr("DRIVE COMPLETE"), 42,
+              font_weight=FontWeight.SEMI_BOLD, alignment=rl.GuiTextAlignment.TEXT_ALIGN_CENTER)
+    gui_label(rl.Rectangle(rect.x + 20, rect.y + 78, rect.width / 2 - 30, 42), tr("MADS"), 28,
+              color=rl.Color(255, 255, 255, 184), alignment=rl.GuiTextAlignment.TEXT_ALIGN_CENTER)
+    gui_label(rl.Rectangle(rect.x + rect.width / 2 + 10, rect.y + 78, rect.width / 2 - 30, 42), tr("FULL ASSIST"), 28,
+              color=rl.Color(255, 255, 255, 184), alignment=rl.GuiTextAlignment.TEXT_ALIGN_CENTER)
+    gui_label(rl.Rectangle(rect.x + 20, rect.y + 116, rect.width / 2 - 30, 72), f"{mads:.1f} {unit}", 48,
+              font_weight=FontWeight.DISPLAY, alignment=rl.GuiTextAlignment.TEXT_ALIGN_CENTER)
+    gui_label(rl.Rectangle(rect.x + rect.width / 2 + 10, rect.y + 116, rect.width / 2 - 30, 72), f"{full_assist:.1f} {unit}", 48,
+              font_weight=FontWeight.DISPLAY, alignment=rl.GuiTextAlignment.TEXT_ALIGN_CENTER)
 
   def _set_chestnut_visibility(self):
     # stock has no loading tier: it shows green from the moment a big model is available. keep the

openpilot/selfdrive/ui/sunnypilot/onroad/milestone_celebration.py

@@ -0,0 +1,228 @@
+"""Render assisted-driving milestone celebrations over the on-road view."""
+
+import math
+import random
+import time
+from collections import deque
+from dataclasses import dataclass
+
+import pyray as rl
+
+from openpilot.cereal import custom
+from openpilot.selfdrive.ui.mici.onroad.alert_renderer import ALERT_BACKGROUND_OPACITY
+from openpilot.selfdrive.ui.mici.onroad.hud_renderer import FONT_SIZES
+from openpilot.selfdrive.ui.ui_state import ui_state
+from openpilot.system.ui.lib.application import FontWeight, gui_app
+from openpilot.system.ui.lib.multilang import tr
+from openpilot.system.ui.lib.text_measure import measure_text_cached
+from openpilot.system.ui.widgets import Widget
+
+
+CELEBRATION_DURATION = 4.5
+PARTICLE_COUNT = 150
+METERS_PER_MILE = 1609.344
+METERS_PER_KILOMETER = 1000.0
+
+CONFETTI_COLORS = (
+  rl.Color(255, 55, 95, 255),
+  rl.Color(255, 183, 3, 255),
+  rl.Color(48, 209, 88, 255),
+  rl.Color(36, 179, 255, 255),
+  rl.Color(112, 72, 232, 255),
+  rl.Color(255, 45, 196, 255),
+)
+
+
+@dataclass(frozen=True)
+class ConfettiParticle:
+  x: float
+  y: float
+  width: float
+  height: float
+  speed: float
+  drift: float
+  angle: float
+  spin: float
+  phase: float
+  color: rl.Color
+
+
+@dataclass(frozen=True)
+class CelebrationMilestone:
+  event_id: int
+  full_assist: bool
+  distance_meters: float
+  previous_distance_meters: float
+  metric: bool
+
+
+class MilestoneCelebration(Widget):
+  """Pure renderer for typed assisted-driving milestone events."""
+
+  def __init__(self):
+    super().__init__()
+    self._drive_started_time = -1.0
+    self._celebration_started_time: float | None = None
+    self._current_milestone: CelebrationMilestone | None = None
+    self._pending_milestones: deque[CelebrationMilestone] = deque()
+    self._last_event_id = 0
+    self._particles = self._make_particles()
+
+  @staticmethod
+  def _make_particles() -> list[ConfettiParticle]:
+    rng = random.Random(20260828)
+    return [
+      ConfettiParticle(
+        x=rng.random(),
+        y=rng.uniform(-0.25, 0.95),
+        width=rng.uniform(10, 24),
+        height=rng.uniform(24, 58),
+        speed=rng.uniform(0.12, 0.34),
+        drift=rng.uniform(-0.035, 0.035),
+        angle=rng.uniform(0, 360),
+        spin=rng.uniform(-150, 150),
+        phase=rng.uniform(0, math.tau),
+        color=CONFETTI_COLORS[rng.randrange(len(CONFETTI_COLORS))],
+      )
+      for _ in range(PARTICLE_COUNT)
+    ]
+
+  def _render(self, rect: rl.Rectangle, /) -> None:
+    now = time.monotonic()
+    if ui_state.started_time != self._drive_started_time:
+      self._drive_started_time = ui_state.started_time
+      self._celebration_started_time = None
+      self._current_milestone = None
+      self._pending_milestones.clear()
+
+    self._consume_event(suppress=False)
+
+    if self._current_milestone is None and self._pending_milestones:
+      self._current_milestone = self._pending_milestones.popleft()
+      self._celebration_started_time = now
+
+    if self._celebration_started_time is None or self._current_milestone is None:
+      return
+
+    elapsed = now - self._celebration_started_time
+    if elapsed >= CELEBRATION_DURATION:
+      self._celebration_started_time = None
+      self._current_milestone = None
+      return
+
+    alpha = min(1.0, elapsed / 0.2, (CELEBRATION_DURATION - elapsed) / 0.8)
+    self._draw_background_scrim(rect, alpha)
+    self._draw_confetti(rect, elapsed, alpha)
+    self._draw_milestone(rect, elapsed, alpha, self._current_milestone)
+
+  def cancel_for_alert(self) -> None:
+    self._consume_event(suppress=True)
+    self._celebration_started_time = None
+    self._current_milestone = None
+    self._pending_milestones.clear()
+
+  def _consume_event(self, suppress: bool) -> None:
+    if not ui_state.sm.updated["assistedDrivingMilestoneState"]:
+      return
+    state = ui_state.sm["assistedDrivingMilestoneState"]
+    event = state.event
+    if not state.enabled:
+      self._celebration_started_time = None
+      self._current_milestone = None
+      self._pending_milestones.clear()
+      return
+    if event.id == 0 or event.id == self._last_event_id:
+      return
+    self._last_event_id = event.id
+    if suppress:
+      return
+    self._pending_milestones.append(CelebrationMilestone(
+      event_id=event.id,
+      full_assist=event.category == custom.AssistedDrivingMilestoneState.Category.fullAssist,
+      distance_meters=event.distanceMeters,
+      previous_distance_meters=event.previousDistanceMeters,
+      metric=event.unit == custom.AssistedDrivingMilestoneState.Unit.metric,
+    ))
+
+  def _draw_confetti(self, rect: rl.Rectangle, elapsed: float, alpha: float) -> None:
+    travel_height = rect.height * 1.45
+    compact = rect.height <= 300
+    particle_scale = rect.height / 1080.0
+    particles = self._particles[:100] if compact else self._particles
+    for particle in particles:
+      x = rect.x + rect.width * (particle.x + particle.drift * elapsed + 0.012 * math.sin(elapsed * 3 + particle.phase))
+      y = rect.y - rect.height * 0.2 + (particle.y * travel_height + particle.speed * rect.height * elapsed) % travel_height
+      flip = 0.2 + 0.8 * abs(math.sin(elapsed * 5 + particle.phase))
+      particle_rect = rl.Rectangle(x, y, particle.width * particle_scale * flip, particle.height * particle_scale)
+      origin = rl.Vector2(particle_rect.width / 2, particle_rect.height / 2)
+      color = rl.Color(particle.color.r, particle.color.g, particle.color.b, int(255 * alpha))
+      rl.draw_rectangle_pro(particle_rect, origin, particle.angle + particle.spin * elapsed, color)
+
+  @staticmethod
+  def _draw_milestone(rect: rl.Rectangle, elapsed: float, alpha: float, milestone: CelebrationMilestone) -> None:
+    # Match the comma four set-speed hierarchy: DISPLAY number with a MAX-sized label.
+    scale = rect.height / 240.0
+    pulse = 1.0 + 0.025 * math.sin(min(elapsed, 0.6) / 0.6 * math.pi)
+    number_size = int(FONT_SIZES.set_speed * scale * pulse)
+    milestone_size = int(FONT_SIZES.max_speed * scale * pulse)
+    category_size = int(22 * scale * pulse)
+    unit_size = category_size
+
+    display_font = gui_app.font(FontWeight.DISPLAY)
+    semibold_font = gui_app.font(FontWeight.SEMI_BOLD)
+    tween_progress = min(elapsed / 0.85, 1.0)
+    tween_progress = 1.0 - (1.0 - tween_progress) ** 3
+    meters_per_unit = METERS_PER_KILOMETER if milestone.metric else METERS_PER_MILE
+    previous_distance = milestone.previous_distance_meters / meters_per_unit
+    milestone_distance = milestone.distance_meters / meters_per_unit
+    displayed_distance = previous_distance + (milestone_distance - previous_distance) * tween_progress
+    if tween_progress >= 1.0:
+      number = f"{round(milestone_distance):,}"
+    else:
+      number = f"{displayed_distance:,.1f}"
+    unit = tr("KM") if milestone.metric else tr("MI")
+    category = tr("FULL ASSIST") if milestone.full_assist else tr("MADS")
+    milestone_label = tr("MILESTONE")
+
+    unit_bounds = measure_text_cached(semibold_font, unit, unit_size)
+    number_bounds = measure_text_cached(display_font, number, number_size)
+    max_number_width = rect.width * 0.72 - unit_bounds.x - 8 * scale
+    if number_bounds.x > max_number_width:
+      number_size = max(1, int(number_size * max_number_width / number_bounds.x))
+      number_bounds = measure_text_cached(display_font, number, number_size)
+    category_bounds = measure_text_cached(semibold_font, category, category_size)
+    milestone_bounds = measure_text_cached(semibold_font, milestone_label, milestone_size)
+
+    center_x = rect.x + rect.width / 2
+    center_y = rect.y + rect.height / 2
+    text_color = rl.Color(255, 255, 255, int(255 * 0.9 * alpha))
+    secondary_color = rl.Color(255, 255, 255, int(255 * 0.72 * alpha))
+    number_line_width = number_bounds.x + 8 * scale + unit_bounds.x
+    number_x = center_x - number_line_width / 2
+    number_y = center_y - 76 * scale
+    unit_y = center_y + 14 * scale
+    category_y = center_y - 91 * scale
+    milestone_y = center_y + 50 * scale
+
+    rl.draw_text_ex(semibold_font, category, rl.Vector2(center_x - category_bounds.x / 2, category_y),
+                    category_size, 0, secondary_color)
+    rl.draw_text_ex(display_font, number, rl.Vector2(number_x, number_y), number_size, 0, text_color)
+    rl.draw_text_ex(semibold_font, unit, rl.Vector2(number_x + number_bounds.x + 8 * scale, unit_y),
+                    unit_size, 0, secondary_color)
+    rl.draw_text_ex(semibold_font, milestone_label, rl.Vector2(center_x - milestone_bounds.x / 2, milestone_y),
+                    milestone_size, 0, text_color)
+
+  @staticmethod
+  def _draw_background_scrim(rect: rl.Rectangle, alpha: float) -> None:
+    # Match the alert background: a mostly opaque black core fading to transparent.
+    fade_height = round(rect.height * 0.25)
+    solid_height = round(rect.height * 0.50)
+    solid_color = rl.Color(0, 0, 0, int(255 * ALERT_BACKGROUND_OPACITY * alpha))
+    transparent = rl.Color(0, 0, 0, 0)
+    x = int(rect.x)
+    y = int(rect.y)
+    width = int(rect.width)
+
+    rl.draw_rectangle_gradient_v(x, y, width, fade_height, transparent, solid_color)
+    rl.draw_rectangle(x, y + fade_height, width, solid_height, solid_color)
+    rl.draw_rectangle_gradient_v(x, y + fade_height + solid_height, width, fade_height, solid_color, transparent)

openpilot/selfdrive/ui/sunnypilot/ui_state.py

@@ -35,7 +35,8 @@ class UIStateSP:
     self.is_sp_release: bool = self.params.get_bool("IsReleaseSpBranch")
     self.sm_services_ext = [
       "modelManagerSP", "selfdriveStateSP", "longitudinalPlanSP", "backupManagerSP",
-      "gpsLocation", "lateralTorqueParameters", "carStateSP", "liveMapDataSP", "carParamsSP", "lateralDelay"
+      "gpsLocation", "lateralTorqueParameters", "carStateSP", "liveMapDataSP", "carParamsSP", "lateralDelay",
+      "assistedDrivingMilestoneState",
     ]
 
     self.sunnylink_state = SunnylinkState()

openpilot/selfdrive/ui/tests/generate_milestone_sound.py

@@ -0,0 +1,45 @@
+#!/usr/bin/env python3
+"""Generate the assisted-driving milestone celebration chime."""
+
+import math
+import wave
+from array import array
+from pathlib import Path
+
+
+SAMPLE_RATE = 48_000
+DURATION_SECONDS = 0.82
+NOTES = (
+  (0.00, 523.25),
+  (0.11, 659.25),
+  (0.22, 783.99),
+)
+
+
+def note_sample(age: float, frequency: float) -> float:
+  if not 0 <= age <= 0.58:
+    return 0.0
+  attack = min(age / 0.008, 1.0)
+  release = min((0.58 - age) / 0.15, 1.0)
+  envelope = attack * release * math.exp(-3.8 * age)
+  tone = math.sin(math.tau * frequency * age) + 0.16 * math.sin(math.tau * frequency * 2 * age)
+  return envelope * tone
+
+
+def main() -> None:
+  output = Path(__file__).parents[4] / "openpilot/selfdrive/assets/sounds/milestone.wav"
+  samples = array('h')
+  for frame in range(round(SAMPLE_RATE * DURATION_SECONDS)):
+    t = frame / SAMPLE_RATE
+    value = 0.38 * sum(note_sample(t - start, frequency) for start, frequency in NOTES)
+    samples.append(round(max(-1.0, min(1.0, value)) * 32767))
+
+  with wave.open(str(output), "wb") as wav:
+    wav.setnchannels(1)
+    wav.setsampwidth(2)
+    wav.setframerate(SAMPLE_RATE)
+    wav.writeframes(samples.tobytes())
+
+
+if __name__ == "__main__":
+  main()

openpilot/selfdrive/ui/tests/milestone_preview.py

@@ -0,0 +1,38 @@
+#!/usr/bin/env python3
+"""Publish deterministic milestone events for the local comma-four UI preview."""
+
+import itertools
+import time
+
+from openpilot.cereal import messaging
+
+
+def main() -> None:
+  pm = messaging.PubMaster(["assistedDrivingMilestoneState"])
+  milestones = itertools.cycle(((1, 0, "mads"), (2, 1, "fullAssist"), (5, 2, "mads"), (10, 5, "fullAssist")))
+  event_id = 0
+  milestone, previous_milestone, category = 0, 0, "mads"
+  next_event_time = time.monotonic() + 1.0
+
+  while True:
+    now = time.monotonic()
+    if now >= next_event_time:
+      event_id += 1
+      milestone, previous_milestone, category = next(milestones)
+      next_event_time = now + 6.0
+
+    msg = messaging.new_message("assistedDrivingMilestoneState")
+    state = msg.assistedDrivingMilestoneState
+    state.enabled = True
+    if event_id:
+      state.event.id = event_id
+      state.event.category = category
+      state.event.distanceMeters = milestone * 1609.344
+      state.event.previousDistanceMeters = previous_milestone * 1609.344
+      state.event.unit = "imperial"
+    pm.send("assistedDrivingMilestoneState", msg)
+    time.sleep(0.1)
+
+
+if __name__ == "__main__":
+  main()

openpilot/selfdrive/ui/tests/run_milestone_celebration.sh

@@ -0,0 +1,27 @@
+#!/usr/bin/env bash
+set -e
+
+repo_root="$(cd "$(dirname "${BASH_SOURCE[0]}")/../../../.." && pwd)"
+replay_pid=""
+preview_pid=""
+
+cleanup() {
+  for pid in "$preview_pid" "$replay_pid"; do
+    if [[ -n "$pid" ]]; then
+      kill "$pid" 2>/dev/null || true
+      wait "$pid" 2>/dev/null || true
+    fi
+  done
+}
+trap cleanup EXIT INT TERM
+
+export PATH="$repo_root/.venv/bin:$PATH"
+export SP_MILESTONE_PREVIEW=1
+playback="${SP_MILESTONE_PLAYBACK:-1}"
+
+"$repo_root/openpilot/tools/replay/replay" --demo --playback "$playback" &
+replay_pid=$!
+"$repo_root/.venv/bin/python" "$repo_root/openpilot/selfdrive/ui/tests/milestone_preview.py" &
+preview_pid=$!
+
+"$repo_root/.venv/bin/python" "$repo_root/openpilot/selfdrive/ui/mici/onroad/augmented_road_view.py"

openpilot/selfdrive/ui/tests/test_soundd.py

@@ -4,12 +4,63 @@ import time
 from openpilot.common.test import OpenpilotTestCase
 from openpilot.cereal import log, messaging
 from openpilot.cereal.messaging import SubMaster, PubMaster
-from openpilot.selfdrive.ui.soundd import SELFDRIVE_STATE_TIMEOUT, check_selfdrive_timeout_alert
+from openpilot.selfdrive.ui.soundd import SELFDRIVE_STATE_TIMEOUT, Soundd, calculate_volume_for_device, check_selfdrive_timeout_alert
 
 AudibleAlert = log.SelfdriveState.AudibleAlert
 
 
 class TestSoundd(OpenpilotTestCase):
+  @staticmethod
+  def milestone_submaster(event_id=42):
+    class SubMasterStub:
+      def __init__(self):
+        self.updated = {'assistedDrivingMilestoneState': True}
+        msg = messaging.new_message('assistedDrivingMilestoneState')
+        msg.assistedDrivingMilestoneState.enabled = True
+        msg.assistedDrivingMilestoneState.event.id = event_id
+        self.data = {'assistedDrivingMilestoneState': msg.assistedDrivingMilestoneState}
+
+      def __getitem__(self, service):
+        return self.data[service]
+
+    return SubMasterStub()
+
+  def test_comma_four_volume_is_50_percent_louder_than_comma_three_x(self):
+    for weighted_db in (20.0, 30.0, 40.0, 50.0):
+      with self.subTest(weighted_db=weighted_db):
+        comma_three_x_volume = calculate_volume_for_device(weighted_db, "tizi")
+        comma_four_volume = calculate_volume_for_device(weighted_db, "mici")
+        assert comma_four_volume == min(1.0, comma_three_x_volume * 1.5)
+
+  def test_milestone_chime_uses_typed_milestone_event_once(self):
+    soundd = Soundd()
+    sm = self.milestone_submaster()
+    soundd.update_milestone_alert(sm)
+
+    assert soundd.current_alert == AudibleAlert.complete
+    soundd.current_alert = AudibleAlert.none
+    soundd.update_milestone_alert(sm)
+    assert soundd.current_alert == AudibleAlert.none
+
+  def test_safety_alert_consumes_milestone_without_replaying_it(self):
+    soundd = Soundd()
+    sm = self.milestone_submaster()
+    soundd.current_alert = AudibleAlert.warningImmediate
+
+    soundd.update_milestone_alert(sm)
+    soundd.current_alert = AudibleAlert.none
+    soundd.update_milestone_alert(sm)
+
+    assert soundd.current_alert == AudibleAlert.none
+
+  def test_quiet_mode_consumes_milestone_without_playing_it(self):
+    soundd = Soundd()
+    soundd.enabled = True
+
+    soundd.update_milestone_alert(self.milestone_submaster())
+
+    assert soundd.current_alert == AudibleAlert.none
+
   def test_check_selfdrive_timeout_alert(self, mocker):
     sm = SubMaster(['selfdriveState', 'selfdriveStateSP'])
     pm = PubMaster(['selfdriveState', 'selfdriveStateSP'])

openpilot/sunnypilot/modeld_v2/modeld.py

@@ -8,22 +8,22 @@ See the LICENSE.md file in the root directory for more details.
 
 import os
 os.environ['GMMU'] = '0'
+import numpy as np
+import threading
+import time
+from setproctitle import setproctitle
+from tinygrad.tensor import Tensor
+
+import openpilot.cereal.messaging as messaging
 from openpilot.common.hardware import COMMA_HARDWARE
 from openpilot.selfdrive.modeld.helpers import chestnut_present, load_oob
-import time
-import numpy as np
-import openpilot.cereal.messaging as messaging
 from openpilot.cereal import log
 from opendbc.car.structs import car
 from openpilot.cereal.services import SERVICE_LIST
-from setproctitle import setproctitle
 from openpilot.cereal.messaging import PubMaster, SubMaster
 from openpilot.cereal.visionipc import VisionStreamType
 from msgq.visionipc import VisionIpcClient, VisionBuf
 from opendbc.car.car_helpers import get_demo_car_params
-
-from tinygrad.tensor import Tensor
-
 from openpilot.common.file_chunker import open_file_chunked
 from openpilot.common.swaglog import cloudlog
 from openpilot.common.params import Params
@@ -42,13 +42,13 @@ from openpilot.sunnypilot.modeld_v2.constants import Plan
 from openpilot.sunnypilot.modeld_v2.meta_helper import load_meta_constants
 from openpilot.sunnypilot.modeld_v2.camera_offset_helper import CameraOffsetHelper
 from openpilot.sunnypilot.modeld_v2.compile_modeld import derive_frame_skip, make_split_input_queues, make_supercombo_input_queues, WARP_INPUTS, POLICY_INPUTS
-
 from openpilot.sunnypilot.livedelay.helpers import get_lat_delay
 from openpilot.sunnypilot.modeld_v2.modeld_base import ModelStateBase
 from openpilot.sunnypilot.models.helpers import get_active_bundle
 from openpilot.sunnypilot.selfdrive.controls.lib.relc import RoadEdgeLaneChangeController
 
 PROCESS_NAME = "openpilot.selfdrive.modeld.modeld_tinygrad"
+BIG_MODEL_TIMEOUT = 60
 
 
 def _pkl_exists(path):
@@ -68,6 +68,7 @@ def _find_driving_pkl(bundle):
   pkl_path = os.path.join(model_root, pkl_name)
   if _pkl_exists(pkl_path):
     return pkl_path
+  return None
 
 
 class FrameMeta:
@@ -102,7 +103,7 @@ class ModelState(ModelStateBase):
     self.chestnut = chestnut
 
     pkl_path = _find_driving_pkl(model_bundle)
-    assert pkl_path is not None, "No driving pkl found — all models must be compiled with compile_modeld.py"
+    assert pkl_path is not None, f"No driving pkl found for {'chestnut' if chestnut else 'small model'} — all models must be compiled with compile_modeld.py"
     self._init_combined(pkl_path, cam_w, cam_h, model_bundle)
 
   def _init_combined(self, pkl_path, cam_w, cam_h, bundle):
@@ -185,9 +186,6 @@ class ModelState(ModelStateBase):
     else:
       self.warp(**{k: self.input_queues[k] for k in WARP_INPUTS}, frame=frame_tensor, big_frame=big_frame_tensor)
 
-    if self.chestnut:
-      self.warmup()
-
   def warmup(self) -> None:
     dummy_frames = {k: np.zeros(self.frame_buf_params[k][3], dtype=np.uint8) for k in self._vision_input_names}
     transforms = {k: np.eye(3, dtype=np.float32) for k in [self._road_key, self._wide_key] if k}
@@ -288,8 +286,7 @@ class ModelState(ModelStateBase):
       buf[0, -1, :] = outputs['desired_curvature'][0, :] if not self.mlsim else 0
 
     if self.chestnut and not np.all(np.isfinite(outputs.get('plan', np.array([0.])))):
-      cloudlog.error("model output not finite, dropping frame")
-      return None
+      raise RuntimeError("model output not finite")
 
     return outputs
 
@@ -363,21 +360,26 @@ def main(demo=False):
 
   model = None
   if CHESTNUT:
-    import threading
-    def load():
-      nonlocal model
-      model = ModelState(cam_w=vipc_client_main.width, cam_h=vipc_client_main.height, chestnut=True)
-    t = threading.Thread(target=load, daemon=True)
-    t.start()
-    t.join(60)
-    if model is None:
-      params.put_bool("ChestnutActive", False)
-      raise RuntimeError("chestnut model load failed or timed out (60s)")
-    params.put_bool("ChestnutActive", True)
-  else:
-    model = ModelState(cam_w=vipc_client_main.width, cam_h=vipc_client_main.height, chestnut=False)
+    big_model = None
+    def load_big():
+      nonlocal big_model
+      try:
+        m = ModelState(cam_w=vipc_client_main.width, cam_h=vipc_client_main.height, chestnut=True)
+        m.warmup()
+        big_model = m
+      except Exception:
+        cloudlog.exception("chestnut load failed")
+    loader = threading.Thread(target=load_big, daemon=True)
+    loader.start()
+    loader.join(BIG_MODEL_TIMEOUT)
+    model = big_model
+    params.put_bool("ChestnutActive", model is not None)
 
+  small_model = ModelState(cam_w=vipc_client_main.width, cam_h=vipc_client_main.height, chestnut=False) if model is None or CHESTNUT else None
+  if model is None:
+    model = small_model
   params.put_bool("ChestnutLoading", False)
+  assert model is not None
   cloudlog.warning(f"models loaded in {time.monotonic() - st:.1f}s, modeld starting")
 
   # messaging
@@ -386,7 +388,7 @@ def main(demo=False):
   sm = SubMaster(["deviceState", "carState", "narrowRoadCameraState", "extrinsicsCalibration", "driverMonitoringState", "carControl", "lateralDelay"])
 
   publish_state = PublishState()
-  chestnut_state = ChestnutState(pm, CHESTNUT) if CHESTNUT else None
+  chestnut_state = ChestnutState(pm, model.chestnut) if CHESTNUT else None
 
   # setup filter to track dropped frames
   frame_dropped_filter = FirstOrderFilter(0., 10., 1. / model.constants.MODEL_FREQ)
@@ -509,7 +511,19 @@ def main(demo=False):
       inputs['action_t'] = np.array([lat_action_t, long_action_t], dtype=np.float32)
 
     mt1 = time.perf_counter()
-    model_output = model.run(bufs, transforms, inputs, prepare_only)
+    try:
+      model_output = model.run(bufs, transforms, inputs, prepare_only)
+    except Exception:
+      if not params.get_bool("ChestnutActive"):
+        raise
+      cloudlog.exception("chestnut failed, falling back to small")
+      params.put_bool("ChestnutActive", False)
+      assert small_model is not None
+      model = small_model
+      if chestnut_state is not None:
+        chestnut_state.big = False
+      run_count = 0
+      model_output = None
     mt2 = time.perf_counter()
     model_execution_time = mt2 - mt1
 

openpilot/sunnypilot/modeld_v2/tests/test_fallback.py

@@ -0,0 +1,62 @@
+"""
+Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
+
+This file is part of sunnypilot and is licensed under the MIT License.
+See the LICENSE.md file in the root directory for more details.
+"""
+
+import io
+import requests
+
+from openpilot.common.file_chunker import get_chunk_name
+from openpilot.common.hardware import hw
+from openpilot.common.test import OpenpilotTestCase
+from openpilot.selfdrive.modeld.helpers import dump_oob
+import openpilot.sunnypilot.modeld_v2.modeld as modeld_module
+from openpilot.sunnypilot.modeld_v2.tests import helpers as tests_helpers
+from openpilot.sunnypilot.modeld_v2.tests.helpers import DummyModel, DummyBundle, CAM_W, CAM_H
+from openpilot.sunnypilot.models.fetcher import ModelParser, ModelFetcher
+
+tmp_path = tests_helpers.tmp_path
+
+
+class TestFallback(OpenpilotTestCase):
+  def test_find_dual_model_in_bundle(self, tmp_path, monkeypatch):
+    lebowski_file = 'driving_lebowski.pkl'
+    tsfdo_file = 'driving_tsfdo.pkl'
+    (tmp_path / lebowski_file).write_bytes(b'fkasdjfkljf')
+    (tmp_path / tsfdo_file).write_bytes(b'dskfajklsdjlsfka')
+
+    monkeypatch.setattr(hw.Paths, 'model_root', staticmethod(lambda: str(tmp_path)))
+    big_bundle = DummyBundle(models=[DummyModel('supercombo', lebowski_file)])
+    small_bundle = DummyBundle(models=[DummyModel('supercombo', tsfdo_file)])
+    big_pkl = modeld_module._find_driving_pkl(big_bundle)
+    small_pkl = modeld_module._find_driving_pkl(small_bundle)
+
+    assert big_pkl is not None and lebowski_file in big_pkl
+    assert small_pkl is not None and tsfdo_file in small_pkl
+
+  def test_download_models_and_init_modelstate_fallback(self, tmp_path, monkeypatch):
+    monkeypatch.setattr(hw.Paths, 'model_root', staticmethod(lambda: str(tmp_path)))
+    big_json = requests.get(ModelFetcher.MODEL_URL_CHESTNUT).json()
+    big_bundle = ModelParser.parse_models(big_json)[-1]
+    small_json = requests.get(ModelFetcher.MODEL_URL).json()
+    small_bundle = ModelParser.parse_models(small_json)[-1]
+
+    buf = io.BytesIO()
+    dump_oob(tests_helpers.make_pkl_data(tests_helpers.ARCHETYPES['supercombo_non20hz']), buf)
+    oob_bytes = buf.getvalue()
+
+    for bundle in (big_bundle, small_bundle):
+      artifact = bundle.models[0].artifact
+      for i in range(len(artifact.chunks)):
+        (tmp_path / get_chunk_name(artifact.fileName, i, len(artifact.chunks))).write_bytes(oob_bytes if i == 0 else b"")
+
+    monkeypatch.setattr(modeld_module, 'get_active_bundle', lambda params=None, *, chestnut=None: small_bundle)
+    assert modeld_module.ModelState(CAM_W, CAM_H, chestnut=False).chestnut is False
+
+    monkeypatch.setattr(modeld_module, 'get_active_bundle', lambda params=None, *, chestnut=None: big_bundle)
+    try:
+      assert modeld_module.ModelState(CAM_W, CAM_H, chestnut=True).chestnut is True
+    except Exception as e:
+      assert "AMD" in str(e) or "device" in str(e).lower()

openpilot/sunnypilot/models/default_model.py

@@ -1,11 +1,14 @@
 import argparse
 import os
 import hashlib
+import requests
+import re
 
 from openpilot.common.basedir import BASEDIR
 from openpilot.selfdrive.ui.ui_state import ui_state
 from openpilot.sunnypilot import get_file_hash
 from openpilot.sunnypilot.models.model_name import DEFAULT_MODEL, DEFAULT_BIG_MODEL
+from openpilot.sunnypilot.models.fetcher import ModelFetcher
 
 
 def get_default_model() -> str:
@@ -30,14 +33,29 @@ def update_model_hash():
   print(f"Generated and updated new combined model hash to {MODEL_HASH_PATH}")
 
 
+def get_ref_for_name(url: str, name: str) -> str:
+  response = requests.get(url, timeout=10)
+  if response.status_code == 200:
+    bundles = response.json()["bundles"]
+    matching = [b for b in bundles if re.search(name, f"{b['short_name']} {b['display_name']}", re.IGNORECASE)]
+    if matching:
+      return max(matching, key=lambda b: int(b["index"]))["ref"]
+  return ""
+
+
 def update_default_model_names(default_model_name: str, default_big_model_name: str):
   print("[CHANGE DEFAULT MODEL NAMES]")
+  small_ref = get_ref_for_name(ModelFetcher.MODEL_URL, default_model_name)
+  big_ref = get_ref_for_name(ModelFetcher.MODEL_URL_CHESTNUT, default_big_model_name)
+
   with open(DEFAULT_MODEL_NAME_PATH, "w") as f:
     f.write(f'DEFAULT_MODEL = "{default_model_name}"\n')
+    f.write(f'DEFAULT_MODEL_REF = "{small_ref}"\n')
     f.write(f'DEFAULT_BIG_MODEL = "{default_big_model_name}"\n')
+    f.write(f'DEFAULT_BIG_MODEL_REF = "{big_ref}"\n')
 
-  print(f'New default small model name: "{default_model_name}"')
-  print(f'New default big model name: "{default_big_model_name}"')
+  print(f'New default small model name: "{default_model_name}" (ref: {small_ref})')
+  print(f'New default big model name: "{default_big_model_name}" (ref: {big_ref})')
   print("[DONE]")
 
 

openpilot/sunnypilot/models/manager.py

@@ -328,6 +328,12 @@ class ModelManagerSP:
         validate_active_bundles(self.params, self.source_models)
         self.active_bundle = get_active_bundle(self.params, chestnut=self.chestnut_present)
 
+        if get_selected_bundle(self.params, "chestnut") is not None and get_selected_bundle(self.params, "qcom") is None:
+          if self.params.get("ModelManager_DownloadRef") is None:
+            from openpilot.sunnypilot.models.model_name import DEFAULT_MODEL_REF
+            if DEFAULT_MODEL_REF:
+              self.params.put("ModelManager_DownloadRef", DEFAULT_MODEL_REF)
+
         self._process_download_requests()
 
         if self.params.get("ModelManager_ClearCache"):

openpilot/sunnypilot/models/model_name.py

@@ -1,2 +1,4 @@
 DEFAULT_MODEL = "CD210"
+DEFAULT_MODEL_REF = "5b6436a90cf6902b8aaa71c2b6f3d7164d8ae391"
 DEFAULT_BIG_MODEL = "Lebowski"
+DEFAULT_BIG_MODEL_REF = "fa0c6876d3cf070e91e25e5353ceadc68a5b3285"

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

openpilot/sunnypilot/selfdrive/selfdrived/assisted_driving_milestones.py

@@ -0,0 +1,260 @@
+"""Authoritative assisted-driving distance and milestone tracking."""
+
+import math
+from collections.abc import Mapping
+from dataclasses import dataclass
+from enum import StrEnum
+
+from openpilot.common.params import Params
+
+
+METERS_PER_MILE = 1609.344
+METERS_PER_KILOMETER = 1000.0
+MAX_SAMPLE_INTERVAL_SECONDS = 0.5
+PERSIST_INTERVAL_NS = 10_000_000_000
+STATE_VERSION = 1
+STATE_PARAM = "AssistedDrivingMilestoneState"
+LAST_DRIVE_SUMMARY_PARAM = "LastDriveAssistedDrivingSummary"
+
+
+class AssistCategory(StrEnum):
+  MADS = "mads"
+  FULL_ASSIST = "fullAssist"
+
+
+class MilestoneUnit(StrEnum):
+  IMPERIAL = "imperial"
+  METRIC = "metric"
+
+
+@dataclass(frozen=True)
+class MilestoneEvent:
+  event_id: int
+  category: AssistCategory
+  distance_meters: float
+  previous_distance_meters: float
+  unit: MilestoneUnit
+
+
+@dataclass(frozen=True)
+class MilestoneSnapshot:
+  distances_meters: dict[AssistCategory, float]
+  drive_start_distances_meters: dict[AssistCategory, float]
+  next_event_id: int
+  next_summary_id: int
+  unit: MilestoneUnit
+  active_drive_id: str
+
+
+def assist_category(lat_active: bool, long_active: bool) -> AssistCategory | None:
+  if not lat_active:
+    return None
+  return AssistCategory.FULL_ASSIST if long_active else AssistCategory.MADS
+
+
+def _meters_per_unit(unit: MilestoneUnit) -> float:
+  return METERS_PER_KILOMETER if unit == MilestoneUnit.METRIC else METERS_PER_MILE
+
+
+def _next_ladder_value(value: float) -> float:
+  value = max(0.0, value)
+  magnitude = 10.0 ** math.floor(math.log10(max(1.0, value)))
+  for multiplier in (1.0, 2.0, 5.0):
+    candidate = multiplier * magnitude
+    if candidate > value + 1e-9:
+      return candidate
+  return 10.0 * magnitude
+
+
+def _previous_ladder_value(value: float) -> float:
+  if value <= 1.0:
+    return 0.0
+  magnitude = 10.0 ** math.floor(math.log10(value))
+  normalized = value / magnitude
+  if normalized <= 1.0 + 1e-9:
+    return 5.0 * magnitude / 10.0
+  if normalized <= 2.0 + 1e-9:
+    return magnitude
+  return 2.0 * magnitude
+
+
+def next_milestone_meters(distance_meters: float, unit: MilestoneUnit) -> float:
+  meters_per_unit = _meters_per_unit(unit)
+  return _next_ladder_value(distance_meters / meters_per_unit) * meters_per_unit
+
+
+class MilestoneStore:
+  def __init__(self, params: Params | None = None):
+    self._params = params or Params()
+
+  def load(self) -> MilestoneSnapshot:
+    raw = self._params.get(STATE_PARAM, return_default=True)
+    raw = raw if isinstance(raw, dict) else {}
+    raw_distances = raw.get("distancesMeters", {})
+    raw_distances = raw_distances if isinstance(raw_distances, dict) else {}
+    try:
+      unit = MilestoneUnit(raw.get("unit", MilestoneUnit.IMPERIAL))
+    except ValueError:
+      unit = MilestoneUnit.IMPERIAL
+
+    def distance(category: AssistCategory) -> float:
+      try:
+        return max(0.0, float(raw_distances.get(category.value, 0.0)))
+      except (TypeError, ValueError):
+        return 0.0
+
+    distances = {category: distance(category) for category in AssistCategory}
+    raw_drive_start = raw.get("driveStartDistancesMeters", {})
+    raw_drive_start = raw_drive_start if isinstance(raw_drive_start, dict) else {}
+
+    def drive_start_distance(category: AssistCategory) -> float:
+      try:
+        return max(0.0, min(float(raw_drive_start.get(category.value, distances[category])), distances[category]))
+      except (TypeError, ValueError):
+        return distances[category]
+
+    try:
+      next_event_id = max(1, int(raw.get("nextEventId", 1)))
+    except (TypeError, ValueError):
+      next_event_id = 1
+    try:
+      next_summary_id = max(1, int(raw.get("nextSummaryId", 1)))
+    except (TypeError, ValueError):
+      next_summary_id = 1
+
+    return MilestoneSnapshot(
+      distances_meters=distances,
+      drive_start_distances_meters={category: drive_start_distance(category) for category in AssistCategory},
+      next_event_id=next_event_id,
+      next_summary_id=next_summary_id,
+      unit=unit,
+      active_drive_id=str(raw.get("activeDriveId", "")),
+    )
+
+  def save(self, snapshot: MilestoneSnapshot, block: bool = False) -> None:
+    if block:
+      self._params.flush()
+    self._params.put(STATE_PARAM, {
+      "version": STATE_VERSION,
+      "distancesMeters": {category.value: max(0.0, snapshot.distances_meters.get(category, 0.0)) for category in AssistCategory},
+      "driveStartDistancesMeters": {
+        category.value: max(0.0, snapshot.drive_start_distances_meters.get(category, 0.0)) for category in AssistCategory
+      },
+      "nextEventId": max(1, snapshot.next_event_id),
+      "nextSummaryId": max(1, snapshot.next_summary_id),
+      "unit": snapshot.unit.value,
+      "activeDriveId": snapshot.active_drive_id,
+    }, block=block)
+
+  def save_drive_summary(self, summary_id: int, distances_meters: Mapping[AssistCategory, float], unit: MilestoneUnit) -> None:
+    self._params.put(LAST_DRIVE_SUMMARY_PARAM, {
+      "version": STATE_VERSION,
+      "id": summary_id,
+      "distancesMeters": {category.value: max(0.0, distances_meters.get(category, 0.0)) for category in AssistCategory},
+      "unit": unit.value,
+    }, block=True)
+
+
+class AssistedDrivingMilestones:
+  """Tracks, persists, and emits milestones through one small interface."""
+
+  def __init__(self, store: MilestoneStore | None = None):
+    self._store = store or MilestoneStore()
+    snapshot = self._store.load()
+    self._distances_meters = snapshot.distances_meters
+    self._drive_start_distances_meters = snapshot.drive_start_distances_meters
+    self._next_event_id = snapshot.next_event_id
+    self._next_summary_id = snapshot.next_summary_id
+    self._unit = snapshot.unit
+    self._active_drive_id = snapshot.active_drive_id
+    self._next_milestone_meters = {
+      category: next_milestone_meters(distance, self._unit)
+      for category, distance in self._distances_meters.items()
+    }
+    self._last_timestamp_ns: int | None = None
+    self._last_persist_timestamp_ns: int | None = None
+    self._last_speed_mps = 0.0
+    self._last_category: AssistCategory | None = None
+    self._enabled = False
+    self._closed = False
+
+  def snapshot(self) -> MilestoneSnapshot:
+    return MilestoneSnapshot(
+      self._distances_meters.copy(),
+      self._drive_start_distances_meters.copy(),
+      self._next_event_id,
+      self._next_summary_id,
+      self._unit,
+      self._active_drive_id,
+    )
+
+  def set_drive_id(self, drive_id: str) -> None:
+    if not drive_id or drive_id == self._active_drive_id:
+      return
+    self._active_drive_id = drive_id
+    self._drive_start_distances_meters = self._distances_meters.copy()
+    self._persist()
+
+  def update(self, timestamp_ns: int, speed_mps: float, *, lat_active: bool, long_active: bool,
+             is_metric: bool, enabled: bool) -> MilestoneEvent | None:
+    self._enabled = enabled
+    unit = MilestoneUnit.METRIC if is_metric else MilestoneUnit.IMPERIAL
+    if unit != self._unit:
+      self._unit = unit
+      self._next_milestone_meters = {
+        category: next_milestone_meters(distance, unit)
+        for category, distance in self._distances_meters.items()
+      }
+
+    speed_mps = max(0.0, speed_mps)
+    category = assist_category(lat_active, long_active) if enabled else None
+    event = None
+
+    if self._last_timestamp_ns is not None and timestamp_ns != self._last_timestamp_ns:
+      dt = (timestamp_ns - self._last_timestamp_ns) / 1e9
+      if 0 < dt <= MAX_SAMPLE_INTERVAL_SECONDS and self._last_category is not None:
+        active_category = self._last_category
+        self._distances_meters[active_category] += (self._last_speed_mps + speed_mps) / 2.0 * dt
+        threshold_meters = self._next_milestone_meters[active_category]
+        if self._distances_meters[active_category] >= threshold_meters:
+          meters_per_unit = _meters_per_unit(self._unit)
+          threshold_units = threshold_meters / meters_per_unit
+          event = MilestoneEvent(
+            event_id=self._next_event_id,
+            category=active_category,
+            distance_meters=threshold_meters,
+            previous_distance_meters=_previous_ladder_value(threshold_units) * meters_per_unit,
+            unit=self._unit,
+          )
+          self._next_event_id += 1
+          self._next_milestone_meters[active_category] = next_milestone_meters(threshold_meters, self._unit)
+          self._persist(timestamp_ns=timestamp_ns)
+
+    self._last_timestamp_ns = timestamp_ns
+    self._last_speed_mps = speed_mps
+    self._last_category = category
+
+    if self._last_persist_timestamp_ns is None:
+      self._last_persist_timestamp_ns = timestamp_ns
+    elif timestamp_ns - self._last_persist_timestamp_ns >= PERSIST_INTERVAL_NS:
+      self._persist(timestamp_ns=timestamp_ns)
+
+    return event
+
+  def close(self) -> None:
+    if self._closed:
+      return
+    self._closed = True
+    drive_distances = {
+      category: self._distances_meters[category] - self._drive_start_distances_meters[category]
+      for category in AssistCategory
+    }
+    summary_id = self._next_summary_id
+    self._next_summary_id += 1
+    self._persist(block=True)
+    if self._enabled:
+      self._store.save_drive_summary(summary_id, drive_distances, self._unit)
+
+  def _persist(self, block: bool = False, timestamp_ns: int | None = None) -> None:
+    self._store.save(self.snapshot(), block=block)
+    self._last_persist_timestamp_ns = self._last_timestamp_ns if timestamp_ns is None else timestamp_ns

openpilot/sunnypilot/selfdrive/selfdrived/tests/test_assisted_driving_milestones.py

@@ -0,0 +1,123 @@
+import unittest
+
+from openpilot.sunnypilot.selfdrive.selfdrived.assisted_driving_milestones import (
+  METERS_PER_MILE,
+  AssistCategory,
+  AssistedDrivingMilestones,
+  MilestoneStore,
+  MilestoneUnit,
+)
+
+
+class ParamsStub:
+  def __init__(self, state=None):
+    self.values = {"AssistedDrivingMilestoneState": state or {}}
+    self.writes = []
+
+  def get(self, key, return_default=False):
+    return self.values.get(key, {} if return_default else None)
+
+  def put(self, key, value, block=False):
+    self.values[key] = value
+    self.writes.append((key, value, block))
+
+  def flush(self):
+    pass
+
+
+class TestAssistedDrivingMilestones(unittest.TestCase):
+  def test_emits_and_asynchronously_persists_first_imperial_milestone(self):
+    params = ParamsStub({
+      "version": 1,
+      "distancesMeters": {"mads": METERS_PER_MILE - 5.0, "fullAssist": 0.0},
+      "nextEventId": 7,
+      "unit": "imperial",
+    })
+    milestones = AssistedDrivingMilestones(MilestoneStore(params))  # type: ignore[arg-type]
+
+    self.assertIsNone(milestones.update(0, 10.0, lat_active=True, long_active=False, is_metric=False, enabled=True))
+    event = milestones.update(500_000_000, 10.0, lat_active=True, long_active=False, is_metric=False, enabled=True)
+
+    self.assertIsNotNone(event)
+    assert event is not None
+    self.assertEqual(event.event_id, 7)
+    self.assertEqual(event.category, AssistCategory.MADS)
+    self.assertEqual(event.unit, MilestoneUnit.IMPERIAL)
+    self.assertAlmostEqual(event.distance_meters, METERS_PER_MILE)
+    self.assertFalse(params.writes[-1][2])
+
+  def test_switching_units_schedules_only_a_future_milestone(self):
+    params = ParamsStub({
+      "version": 1,
+      "distancesMeters": {"mads": 9_500.0, "fullAssist": 0.0},
+      "nextEventId": 2,
+      "unit": "imperial",
+    })
+    milestones = AssistedDrivingMilestones(MilestoneStore(params))  # type: ignore[arg-type]
+
+    self.assertIsNone(milestones.update(0, 1_000.0, lat_active=True, long_active=False, is_metric=True, enabled=True))
+    event = milestones.update(500_000_000, 1_000.0, lat_active=True, long_active=False, is_metric=True, enabled=True)
+
+    self.assertIsNotNone(event)
+    assert event is not None
+    self.assertEqual(event.unit, MilestoneUnit.METRIC)
+    self.assertAlmostEqual(event.distance_meters, 10_000.0)
+
+  def test_ignores_disabled_reverse_and_timestamp_gaps(self):
+    params = ParamsStub()
+    milestones = AssistedDrivingMilestones(MilestoneStore(params))  # type: ignore[arg-type]
+
+    milestones.update(0, 20.0, lat_active=True, long_active=False, is_metric=False, enabled=False)
+    milestones.update(500_000_000, 20.0, lat_active=True, long_active=False, is_metric=False, enabled=False)
+    milestones.update(1_000_000_000, -20.0, lat_active=True, long_active=False, is_metric=False, enabled=True)
+    milestones.update(2_000_000_000, 20.0, lat_active=True, long_active=False, is_metric=False, enabled=True)
+
+    self.assertEqual(milestones.snapshot().distances_meters[AssistCategory.MADS], 0.0)
+
+  def test_close_persists_totals_and_last_drive_summary(self):
+    params = ParamsStub()
+    milestones = AssistedDrivingMilestones(MilestoneStore(params))  # type: ignore[arg-type]
+    milestones.update(0, 10.0, lat_active=True, long_active=True, is_metric=False, enabled=True)
+    milestones.update(500_000_000, 10.0, lat_active=True, long_active=True, is_metric=False, enabled=True)
+
+    milestones.close()
+
+    summary = params.values["LastDriveAssistedDrivingSummary"]
+    self.assertAlmostEqual(summary["distancesMeters"]["fullAssist"], 5.0)
+    self.assertTrue(params.writes[-1][2])
+
+    write_count = len(params.writes)
+    milestones.close()
+    self.assertEqual(len(params.writes), write_count)
+
+  def test_process_restart_preserves_the_current_drive_start(self):
+    params = ParamsStub()
+    first_process = AssistedDrivingMilestones(MilestoneStore(params))  # type: ignore[arg-type]
+    first_process.set_drive_id("route-1")
+    first_process.update(0, 10.0, lat_active=True, long_active=False, is_metric=False, enabled=True)
+    first_process.update(500_000_000, 10.0, lat_active=True, long_active=False, is_metric=False, enabled=True)
+    first_process.close()
+
+    second_process = AssistedDrivingMilestones(MilestoneStore(params))  # type: ignore[arg-type]
+    second_process.set_drive_id("route-1")
+    second_process.update(1_000_000_000, 10.0, lat_active=True, long_active=False, is_metric=False, enabled=True)
+    second_process.update(1_500_000_000, 10.0, lat_active=True, long_active=False, is_metric=False, enabled=True)
+    second_process.close()
+
+    summary = params.values["LastDriveAssistedDrivingSummary"]
+    self.assertAlmostEqual(summary["distancesMeters"]["mads"], 10.0)
+
+  def test_disabled_feature_does_not_publish_drive_summary(self):
+    params = ParamsStub()
+    milestones = AssistedDrivingMilestones(MilestoneStore(params))  # type: ignore[arg-type]
+    milestones.update(0, 10.0, lat_active=True, long_active=False, is_metric=False, enabled=True)
+    milestones.update(500_000_000, 10.0, lat_active=True, long_active=False, is_metric=False, enabled=True)
+    milestones.update(1_000_000_000, 10.0, lat_active=True, long_active=False, is_metric=False, enabled=False)
+
+    milestones.close()
+
+    self.assertNotIn("LastDriveAssistedDrivingSummary", params.values)
+
+
+if __name__ == "__main__":
+  unittest.main()

openpilot/sunnypilot/sunnylink/settings_ui.json

@@ -1383,6 +1383,12 @@
               "title": "Steering Arc",
               "description": "Display steering arc on the driving screen when lateral control is enabled."
             },
+            {
+              "key": "AssistedDrivingMilestonesEnabled",
+              "widget": "toggle",
+              "title": "Assisted Driving Milestones",
+              "description": "Celebrate cumulative MADS and full-assist distance milestones while driving."
+            },
             {
               "key": "ShowTurnSignals",
               "widget": "toggle",

openpilot/sunnypilot/sunnylink/settings_ui_src/pages/visuals.yaml

@@ -20,6 +20,10 @@ sections:
     widget: toggle
     title: Steering Arc
     description: Display steering arc on the driving screen when lateral control is enabled.
+  - key: AssistedDrivingMilestonesEnabled
+    widget: toggle
+    title: Assisted Driving Milestones
+    description: Celebrate cumulative MADS and full-assist distance milestones while driving.
   - key: ShowTurnSignals
     widget: toggle
     title: Display Turn Signals

openpilot/sunnypilot/system/params_migration.py

@@ -103,6 +103,32 @@ def _migrate_model_bundle_slots(_params):
     cloudlog.exception(f"Error migrating model bundle slots: {e}")
 
 
+def _migrate_assisted_driving_milestones(_params):
+  try:
+    state = _params.get("AssistedDrivingMilestoneState", return_default=True)
+    if isinstance(state, dict) and state.get("version") == 1:
+      return
+
+    _params.put("AssistedDrivingMilestoneState", {
+      "version": 1,
+      "distancesMeters": {
+        "mads": max(0.0, _params.get("MadsDrivenDistanceMeters", return_default=True) or 0.0),
+        "fullAssist": max(0.0, _params.get("FullAssistDrivenDistanceMeters", return_default=True) or 0.0),
+      },
+      "driveStartDistancesMeters": {
+        "mads": max(0.0, _params.get("MadsDrivenDistanceMeters", return_default=True) or 0.0),
+        "fullAssist": max(0.0, _params.get("FullAssistDrivenDistanceMeters", return_default=True) or 0.0),
+      },
+      "nextEventId": 1,
+      "nextSummaryId": 1,
+      "unit": "metric" if _params.get_bool("IsMetric") else "imperial",
+      "activeDriveId": "",
+    }, block=True)
+    cloudlog.info("params_migration: migrated assisted-driving milestone state")
+  except Exception as e:
+    cloudlog.exception(f"Error migrating assisted-driving milestone state: {e}")
+
+
 def run_migration(_params):
   # migrate OnroadScreenOffBrightness
   if _params.get("OnroadScreenOffBrightnessMigrated") != ONROAD_BRIGHTNESS_MIGRATION_VERSION:
@@ -142,3 +168,5 @@ def run_migration(_params):
 
   # seed the chestnut model slot from the pre-split single slot
   _migrate_model_bundle_slots(_params)
+
+  _migrate_assisted_driving_milestones(_params)

openpilot/sunnypilot/system/tests/test_params_migration.py

@@ -7,7 +7,44 @@ See the LICENSE.md file in the root directory for more details.
 
 from openpilot.common.params import Params
 from openpilot.common.test import OpenpilotTestCase
-from openpilot.sunnypilot.system.params_migration import _migrate_model_bundle_slots
+from openpilot.sunnypilot.system.params_migration import _migrate_model_bundle_slots, run_migration
+
+
+class TestAssistedDrivingMilestoneMigration(OpenpilotTestCase):
+  def test_preserves_prototype_distances_once(self):
+    class ParamsStub:
+      def __init__(self):
+        self.values = {
+          "MadsDrivenDistanceMeters": 123.0,
+          "FullAssistDrivenDistanceMeters": 456.0,
+          "OnroadScreenOffBrightness": 0,
+          "OnroadScreenOffTimer": 15,
+          "AssistedDrivingMilestoneState": {},
+          "IsMetric": False,
+        }
+
+      def get(self, key, return_default=False):
+        return self.values.get(key)
+
+      def put(self, key, value, block=False):
+        self.values[key] = value
+
+      def get_bool(self, key):
+        return bool(self.values.get(key, False))
+
+    params = ParamsStub()
+
+    run_migration(params)
+
+    state = params.get("AssistedDrivingMilestoneState")
+    assert state["distancesMeters"] == {"mads": 123.0, "fullAssist": 456.0}
+
+    params.put("MadsDrivenDistanceMeters", 12.0, block=True)
+    params.put("FullAssistDrivenDistanceMeters", 34.0, block=True)
+    run_migration(params)
+
+    state = params.get("AssistedDrivingMilestoneState")
+    assert state["distancesMeters"] == {"mads": 123.0, "fullAssist": 456.0}
 
 
 class TestModelBundleSlotMigration(OpenpilotTestCase):

tools/ford_native_path.py

@@ -0,0 +1,498 @@
+#!/usr/bin/env python3
+"""Offline evaluation of Ford's native four-field path polynomial.
+
+The experiment deliberately does not alter the live controller. It rebases the
+model path into the vehicle pose expected at actuation time, fits one cubic over
+the remaining short path, and converts the cubic into the LMC2 C0/C1/C2/C3
+signals. A first-order C2 response envelope is included to expose commands that
+would look good only if the PSCM curvature channel were instantaneous.
+"""
+
+import argparse
+from collections import defaultdict
+from dataclasses import dataclass
+import glob
+import math
+from pathlib import Path
+
+import numpy as np
+
+from openpilot.tools.lib.logreader import LogReader
+
+
+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)
+MAX_LATERAL_ACCEL = 3.0 + 9.81 * 0.06
+MAX_LATERAL_JERK = 3.0 + 9.81 * 0.06
+@dataclass(frozen=True)
+class ModelPath:
+  x: np.ndarray
+  y: np.ndarray
+  heading: np.ndarray
+  distance: np.ndarray
+
+
+@dataclass(frozen=True)
+class Sample:
+  route: str
+  time: float
+  speed: float
+  curvature: float
+  steering_pressed: bool
+  path: ModelPath
+  sent_c0: float
+  sent_c1: float
+  sent_c2: float
+  sent_c3: float
+@dataclass(frozen=True)
+class NativePath:
+  c0: float
+  c1: float
+  c2: float
+  c3: float
+  fit_rmse: float
+  path_rms: float
+
+
+def _model_path(model) -> ModelPath | None:
+  try:
+    x = np.asarray(model.position.x, dtype=float)
+    y = np.asarray(model.position.y, dtype=float)
+    heading = np.unwrap(np.asarray(model.orientation.z, dtype=float))
+  except (AttributeError, TypeError, ValueError):
+    return None
+  if len(x) < 4 or len(x) != len(y) or len(x) != len(heading):
+    return None
+  if not np.isfinite(np.concatenate((x, y, heading))).all():
+    return None
+  distance = np.concatenate(([0.0], np.cumsum(np.hypot(np.diff(x), np.diff(y)))))
+  unique_distance, unique = np.unique(distance, return_index=True)
+  if len(unique_distance) < 4 or unique_distance[-1] <= 0.0:
+    return None
+  return ModelPath(x[unique], y[unique], heading[unique], unique_distance)
+
+
+def _arc_pose(distance: float, curvature: float) -> tuple[float, float, float]:
+  heading = curvature * distance
+  if abs(curvature) < 1e-9:
+    return distance, 0.0, 0.0
+  return math.sin(heading) / curvature, (1.0 - math.cos(heading)) / curvature, heading
+
+
+def _relative_points(path: ModelPath, vehicle_pose: tuple[float, float, float], start: float,
+                     horizon: float, count: int = 25) -> tuple[np.ndarray, np.ndarray]:
+  sample_distance = np.linspace(start, min(start + horizon, path.distance[-1]), count)
+  desired_x = np.interp(sample_distance, path.distance, path.x)
+  desired_y = np.interp(sample_distance, path.distance, path.y)
+  vehicle_x, vehicle_y, vehicle_heading = vehicle_pose
+  dx = desired_x - vehicle_x
+  dy = desired_y - vehicle_y
+  cosine = math.cos(vehicle_heading)
+  sine = math.sin(vehicle_heading)
+  return cosine * dx + sine * dy, -sine * dx + cosine * dy
+
+
+def _fit_points(path: ModelPath, speed: float, current_curvature: float, delay: float,
+                horizon: float) -> tuple[np.ndarray, np.ndarray] | None:
+  advance = min(max(speed, 0.0) * delay, path.distance[-1])
+  available = min(horizon, path.distance[-1] - advance)
+  if available <= 0.25:
+    return None
+  x, y = _relative_points(path, _arc_pose(advance, current_curvature), advance, available)
+  forward = (x >= -0.25) & (x <= horizon)
+  x = x[forward]
+  y = y[forward]
+  if len(x) < 4 or np.ptp(x) <= 0.25:
+    return None
+  return x, y
+
+
+def _wire_coefficients(c0: float, c1: float, c2: float, c3: float) -> tuple[float, float, float, float]:
+  slope = math.tan(c1)
+  slope_norm = 1.0 + slope ** 2
+  a2 = 0.5 * c2 * slope_norm ** 1.5
+  a3 = (c3 + 12.0 * slope * a2 ** 2 / slope_norm ** 3) * slope_norm ** 2 / 6.0
+  return c0, slope, a2, a3
+
+
+def _wire_rmse(command: tuple[float, float, float, float], x: np.ndarray, y: np.ndarray) -> float:
+  a0, a1, a2, a3 = _wire_coefficients(*command)
+  reconstructed = a0 + a1 * x + a2 * x ** 2 + a3 * x ** 3
+  return float(np.sqrt(np.mean((reconstructed - y) ** 2)))
+
+
+def fit_native_path(path: ModelPath, speed: float, current_curvature: float, *, delay: float,
+                    horizon: float) -> NativePath:
+  """Fit the delay-aligned path and return physical LMC2 fields.
+
+  C2 and C3 are curvature and curvature rate at the vehicle-frame origin, not
+  the raw quadratic and cubic polynomial coefficients.
+  """
+  points = _fit_points(path, speed, current_curvature, delay, horizon)
+  if points is None:
+    return NativePath(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)
+  x, y = points
+
+  # Scaling x before the least-squares solve keeps tight-turn fits well
+  # conditioned while preserving an ordinary cubic in vehicle coordinates.
+  scale = max(float(np.max(np.abs(x))), 1.0)
+  normalized_x = x / scale
+  design = np.column_stack((np.ones(len(x)), normalized_x, normalized_x ** 2, normalized_x ** 3))
+  scaled, *_ = np.linalg.lstsq(design, y, rcond=None)
+  a0, a1, a2, a3 = (float(scaled[index] / scale ** index) for index in range(4))
+  slope = a1
+  slope_norm = 1.0 + slope ** 2
+  curvature = 2.0 * a2 / slope_norm ** 1.5
+  curvature_rate = 6.0 * a3 / slope_norm ** 2 - 12.0 * slope * a2 ** 2 / slope_norm ** 3
+  command = (float(np.clip(a0, *DBC_OFFSET)),
+             float(np.clip(math.atan(slope), *DBC_ANGLE)),
+             float(np.clip(curvature, *DBC_CURVATURE)),
+             float(np.clip(curvature_rate, *DBC_CURVATURE_RATE)))
+  return NativePath(
+    *command,
+    _wire_rmse(command, x, y),
+    float(np.sqrt(np.mean(y ** 2))),
+  )
+
+
+def fit_c2_aware_path(path: ModelPath, speed: float, current_curvature: float, *, delay: float,
+                      horizon: float, target_c2: float, effective_c2: float,
+                      use_c3: bool = True) -> NativePath:
+  """Fit fast fields around the C2 curvature the PSCM is expected to realize."""
+  points = _fit_points(path, speed, current_curvature, delay, horizon)
+  if points is None:
+    return NativePath(0.0, 0.0, target_c2, 0.0, 0.0, 0.0)
+  x, y = points
+
+  slope = 0.0
+  a0 = a1 = a3 = 0.0
+  for _ in range(3):
+    a2 = 0.5 * effective_c2 * (1.0 + slope ** 2) ** 1.5
+    design = np.column_stack((np.ones(len(x)), x, x ** 3))
+    (a0, a1, a3), *_ = np.linalg.lstsq(design, y - a2 * x ** 2, rcond=None)
+    slope = float(a1)
+
+  slope_norm = 1.0 + slope ** 2
+  c3 = 6.0 * float(a3) / slope_norm ** 2 - 12.0 * slope * a2 ** 2 / slope_norm ** 3
+  c3 = float(np.clip(c3, *DBC_CURVATURE_RATE)) if use_c3 else 0.0
+  # Once C2 and C3 are fixed to what the hardware can realize, refit C0/C1 so
+  # their fast feedback preserves as much of the same path as possible.
+  _, _, fixed_a2, fixed_a3 = _wire_coefficients(0.0, math.atan(slope), effective_c2, c3)
+  (a0, a1), *_ = np.linalg.lstsq(np.column_stack((np.ones(len(x)), x)),
+                                  y - fixed_a2 * x ** 2 - fixed_a3 * x ** 3, rcond=None)
+  c0 = float(np.clip(a0, *DBC_OFFSET))
+  c1 = float(np.clip(math.atan(float(a1)), *DBC_ANGLE))
+  effective_command = (c0, c1, effective_c2, c3)
+  return NativePath(c0, c1, target_c2, c3, _wire_rmse(effective_command, x, y),
+                    float(np.sqrt(np.mean(y ** 2))))
+
+
+def _route(path: str) -> str:
+  return Path(path).name.split("--", 1)[0]
+
+
+def load_samples(paths: list[str], stride: int = 2) -> list[Sample]:
+  grouped: dict[str, list[str]] = defaultdict(list)
+  for path in paths:
+    grouped[_route(path)].append(path)
+
+  samples = []
+  for route, route_paths in sorted(grouped.items()):
+    events = []
+    for path in sorted(route_paths):
+      events.extend(LogReader(path))
+    events.sort(key=lambda event: event.logMonoTime)
+    if not events:
+      continue
+    start_time = events[0].logMonoTime
+    model_path = None
+    curvature = 0.0
+    lat_active = path_valid = False
+    sent = (0.0, 0.0, 0.0, 0.0)
+    car_state_count = 0
+    for event in events:
+      which = event.which()
+      if which == "modelV2":
+        model_path = _model_path(event.modelV2)
+      elif which == "controlsState":
+        curvature = float(event.controlsState.curvature)
+      elif which == "carControl":
+        lat_active = bool(event.carControl.latActive)
+      elif which == "carControlSP":
+        command = event.carControlSP.fordLateralPath
+        path_valid = bool(command.valid)
+        sent = (float(command.pathOffset), float(command.pathAngle),
+                float(command.curvature), float(command.curvatureRate))
+      elif which == "carState" and lat_active and path_valid and model_path is not None:
+        car_state_count += 1
+        if car_state_count % stride:
+          continue
+        samples.append(Sample(
+          route, (event.logMonoTime - start_time) * 1e-9, float(event.carState.vEgo), curvature,
+          bool(event.carState.steeringPressed), model_path, *sent,
+        ))
+  return samples
+
+
+def _percentile(values: np.ndarray, percentile: float, mask: np.ndarray | None = None) -> float:
+  selected = values if mask is None else values[mask]
+  return float(np.percentile(np.abs(selected), percentile)) if len(selected) else math.nan
+
+
+def _route_rate(samples: list[Sample], values: np.ndarray) -> np.ndarray:
+  rate = np.zeros(len(values))
+  for index in range(1, len(values)):
+    dt = samples[index].time - samples[index - 1].time
+    if samples[index].route == samples[index - 1].route and 0.005 <= dt <= 0.2:
+      rate[index] = (values[index] - values[index - 1]) / dt
+  return rate
+
+
+def _c2_response(samples: list[Sample], target: np.ndarray, tau_load: float,
+                 tau_unload: float) -> np.ndarray:
+  effective = np.zeros(len(target))
+  previous_route = None
+  previous_time = 0.0
+  state = 0.0
+  for index, sample in enumerate(samples):
+    if sample.route != previous_route:
+      state = 0.0
+      previous_time = sample.time
+    dt = float(np.clip(sample.time - previous_time, 0.005, 0.2))
+    loading = target[index] * state >= 0.0 and abs(target[index]) > abs(state)
+    tau = tau_load if loading else tau_unload
+    state += (1.0 - math.exp(-dt / tau)) * (target[index] - state)
+    effective[index] = state
+    previous_route, previous_time = sample.route, sample.time
+  return effective
+
+
+def _limit_c2_command(samples: list[Sample], target: np.ndarray) -> np.ndarray:
+  """Mirror the CAN-FD Ford curvature acceleration/jerk limiter."""
+  limited = np.zeros(len(target))
+  previous_route = None
+  previous_time = 0.0
+  previous = 0.0
+  for index, sample in enumerate(samples):
+    if sample.route != previous_route:
+      previous = 0.0
+      previous_time = sample.time
+    dt = float(np.clip(sample.time - previous_time, 0.005, 0.2))
+    speed = max(sample.speed, 1.0)
+    value = float(np.clip(target[index], -MAX_LATERAL_ACCEL / speed ** 2,
+                          MAX_LATERAL_ACCEL / speed ** 2))
+    step = MAX_LATERAL_JERK / speed ** 2 * dt
+    value = float(np.clip(value, previous - step, previous + step))
+    limited[index] = float(np.clip(value, *DBC_CURVATURE))
+    previous = limited[index]
+    previous_route, previous_time = sample.route, sample.time
+  return limited
+
+
+def _limit_fast_fields(samples: list[Sample], c0_target: np.ndarray,
+                       c1_target: np.ndarray) -> tuple[np.ndarray, np.ndarray]:
+  c0 = np.zeros(len(samples))
+  c1 = np.zeros(len(samples))
+  previous_route = None
+  previous_time = 0.0
+  previous_c0 = previous_c1 = 0.0
+  for index, sample in enumerate(samples):
+    if sample.route != previous_route:
+      previous_c0 = previous_c1 = 0.0
+      previous_time = sample.time
+    dt = float(np.clip(sample.time - previous_time, 0.005, 0.2))
+    c0[index] = np.clip(c0_target[index], previous_c0 - 4.0 * dt, previous_c0 + 4.0 * dt)
+    c1[index] = np.clip(c1_target[index], previous_c1 - 1.0 * dt, previous_c1 + 1.0 * dt)
+    previous_c0, previous_c1 = c0[index], c1[index]
+    previous_route, previous_time = sample.route, sample.time
+  return c0, c1
+
+
+def evaluate(samples: list[Sample], *, delay: float, horizon: float,
+             tau_load: float, tau_unload: float, horizon_time: float = 0.0,
+             assumed_tau_load: float | None = None, assumed_tau_unload: float | None = None,
+             use_c3: bool = True, c2_limit: float = DBC_CURVATURE[1]) -> dict[str, float]:
+  horizons = np.asarray([float(np.clip(sample.speed * horizon_time, 1.0, horizon))
+                         if horizon_time > 0.0 else horizon for sample in samples])
+  commands = [fit_native_path(sample.path, sample.speed, sample.curvature,
+                              delay=delay, horizon=sample_horizon)
+              for sample, sample_horizon in zip(samples, horizons, strict=True)]
+  c0 = np.asarray([command.c0 for command in commands])
+  c1 = np.asarray([command.c1 for command in commands])
+  raw_c2 = np.asarray([command.c2 for command in commands])
+  c2 = np.clip(raw_c2, -c2_limit, c2_limit)
+  c3 = np.asarray([command.c3 for command in commands])
+  fit_rmse = np.asarray([command.fit_rmse for command in commands])
+  path_rms = np.asarray([command.path_rms for command in commands])
+  transmitted_c2 = _limit_c2_command(samples, c2)
+  effective_c2 = _c2_response(samples, transmitted_c2, tau_load, tau_unload)
+  estimated_c2 = _c2_response(samples, transmitted_c2,
+                              tau_load if assumed_tau_load is None else assumed_tau_load,
+                              tau_unload if assumed_tau_unload is None else assumed_tau_unload)
+  compensated = [fit_c2_aware_path(sample.path, sample.speed, sample.curvature,
+                                   delay=delay, horizon=sample_horizon, target_c2=target,
+                                   effective_c2=estimated, use_c3=use_c3)
+                 for sample, sample_horizon, target, estimated in
+                 zip(samples, horizons, c2, estimated_c2, strict=True)]
+  compensated_c0 = np.asarray([command.c0 for command in compensated])
+  compensated_c1 = np.asarray([command.c1 for command in compensated])
+  compensated_c3 = np.asarray([command.c3 for command in compensated])
+  limited_c0, limited_c1 = _limit_fast_fields(samples, compensated_c0, compensated_c1)
+  estimated_compensated_rmse = np.asarray([command.fit_rmse for command in compensated])
+  compensated_rmse = []
+  for sample, sample_horizon, command, effective in zip(samples, horizons, compensated, effective_c2, strict=True):
+    points = _fit_points(sample.path, sample.speed, sample.curvature, delay, sample_horizon)
+    compensated_rmse.append(0.0 if points is None else _wire_rmse(
+      (command.c0, command.c1, effective, command.c3), *points))
+  compensated_rmse = np.asarray(compensated_rmse)
+  limited_compensated_rmse = []
+  for sample, sample_horizon, c0_value, c1_value, c3_value, effective in \
+      zip(samples, horizons, limited_c0, limited_c1, compensated_c3, effective_c2, strict=True):
+    points = _fit_points(sample.path, sample.speed, sample.curvature, delay, sample_horizon)
+    limited_compensated_rmse.append(0.0 if points is None else _wire_rmse(
+      (c0_value, c1_value, effective, c3_value), *points))
+  limited_compensated_rmse = np.asarray(limited_compensated_rmse)
+  missing_c2 = transmitted_c2 - effective_c2
+  # Compare channels by their lateral contribution at the fit horizon. This
+  # includes C3: treating it as zero would incorrectly blame C0/C1 for a
+  # curvature transition the native polynomial assigns to curvature rate.
+  fast = (2.0 * compensated_c0 / horizons ** 2 +
+          2.0 * np.tan(compensated_c1) / horizons +
+          compensated_c3 * horizons / 3.0)
+  lagging = np.abs(missing_c2) > 0.0005
+  unloading = lagging & (np.abs(c2) < 0.75 * np.abs(effective_c2))
+  pressed = np.asarray([sample.steering_pressed for sample in samples])
+  speed = np.asarray([sample.speed for sample in samples])
+  sent_c2 = np.asarray([sample.sent_c2 for sample in samples])
+  sent_transmitted_c2 = _limit_c2_command(samples, sent_c2)
+  sent_effective_c2 = _c2_response(samples, sent_transmitted_c2, tau_load, tau_unload)
+  sent_lpf_rmse = []
+  for sample, sample_horizon, effective in zip(samples, horizons, sent_effective_c2, strict=True):
+    points = _fit_points(sample.path, sample.speed, sample.curvature, delay, sample_horizon)
+    sent_lpf_rmse.append(0.0 if points is None else _wire_rmse(
+      (sample.sent_c0, sample.sent_c1, effective, sample.sent_c3), *points))
+  sent_lpf_rmse = np.asarray(sent_lpf_rmse)
+  raw_c2_rate = _route_rate(samples, c2)
+  c2_rate = _route_rate(samples, transmitted_c2)
+  sent_c2_rate = _route_rate(samples, sent_c2)
+  compensated_c0_rate = _route_rate(samples, compensated_c0)
+  compensated_c1_rate = _route_rate(samples, compensated_c1)
+  compensated_c3_rate = _route_rate(samples, compensated_c3)
+  normalized_fit = np.divide(fit_rmse, path_rms, out=np.zeros_like(fit_rmse), where=path_rms > 1e-4)
+  return {
+    "samples": float(len(samples)),
+    "delay": delay,
+    "horizon": horizon,
+    "horizon_time": horizon_time,
+    "assumed_tau_load": tau_load if assumed_tau_load is None else assumed_tau_load,
+    "assumed_tau_unload": tau_unload if assumed_tau_unload is None else assumed_tau_unload,
+    "use_c3": float(use_c3),
+    "c2_limit": c2_limit,
+    "actual_horizon_p50": _percentile(horizons, 50),
+    "actual_horizon_p95": _percentile(horizons, 95),
+    "fit_rmse_p50": _percentile(fit_rmse, 50),
+    "fit_rmse_p95": _percentile(fit_rmse, 95),
+    "normalized_fit_p95": _percentile(normalized_fit, 95),
+    "c2_aware_rmse_p50": _percentile(compensated_rmse, 50),
+    "c2_aware_rmse_p95": _percentile(compensated_rmse, 95),
+    "c2_aware_estimated_rmse_p95": _percentile(estimated_compensated_rmse, 95),
+    "c2_aware_limited_rmse_p95": _percentile(limited_compensated_rmse, 95),
+    "sent_lpf_rmse_p50": _percentile(sent_lpf_rmse, 50),
+    "sent_lpf_rmse_p95": _percentile(sent_lpf_rmse, 95),
+    "c0_p95": _percentile(c0, 95),
+    "c1_p95": _percentile(c1, 95),
+    "c2_p95": _percentile(c2, 95),
+    "c3_p95": _percentile(c3, 95),
+    "c0_clip_rate": float(np.mean((c0 <= DBC_OFFSET[0]) | (c0 >= DBC_OFFSET[1]))),
+    "c1_clip_rate": float(np.mean((c1 <= DBC_ANGLE[0]) | (c1 >= DBC_ANGLE[1]))),
+    "c2_clip_rate": float(np.mean((c2 <= DBC_CURVATURE[0]) | (c2 >= DBC_CURVATURE[1]))),
+    "c3_clip_rate": float(np.mean((c3 <= DBC_CURVATURE_RATE[0]) | (c3 >= DBC_CURVATURE_RATE[1]))),
+    "c2_aware_c0_p95": _percentile(compensated_c0, 95),
+    "c2_aware_c1_p95": _percentile(compensated_c1, 95),
+    "c2_aware_c3_p95": _percentile(compensated_c3, 95),
+    "c2_aware_c0_rate_p95": _percentile(compensated_c0_rate, 95),
+    "c2_aware_c1_rate_p95": _percentile(compensated_c1_rate, 95),
+    "c2_aware_c0_rate_limit_rate": float(np.mean(np.abs(compensated_c0_rate) > 4.0)),
+    "c2_aware_c1_rate_limit_rate": float(np.mean(np.abs(compensated_c1_rate) > 1.0)),
+    "c2_aware_c3_rate_p95": _percentile(compensated_c3_rate, 95),
+    "raw_c2_rate_p95": _percentile(raw_c2_rate, 95),
+    "c2_rate_p95": _percentile(c2_rate, 95),
+    "sent_c2_rate_p95": _percentile(sent_c2_rate, 95),
+    "c2_lag_p95": _percentile(missing_c2, 95),
+    "lag_samples": float(np.count_nonzero(lagging)),
+    "lag_fast_support_rate": float(np.mean(fast[lagging] * missing_c2[lagging] > 0.0)) if np.any(lagging) else math.nan,
+    "lag_fast_coverage_p50": _percentile(np.divide(fast, missing_c2, out=np.zeros_like(fast),
+                                                    where=np.abs(missing_c2) > 1e-6), 50, lagging),
+    "unload_samples": float(np.count_nonzero(unloading)),
+    "unload_fast_counter_rate": float(np.mean(fast[unloading] * effective_c2[unloading] < 0.0)) if np.any(unloading) else math.nan,
+    "unload_residual_c2_p95": _percentile(effective_c2 - c2, 95, unloading),
+    "pressed_c0_p95": _percentile(c0, 95, pressed),
+    "pressed_c1_p95": _percentile(c1, 95, pressed),
+    "low_speed_fit_p95": _percentile(fit_rmse, 95, speed < 5.0),
+    "road_speed_fit_p95": _percentile(fit_rmse, 95, speed >= 15.0),
+  }
+
+
+def _expand(patterns: list[str]) -> list[str]:
+  return sorted({path for pattern in patterns for path in glob.glob(pattern)})
+
+
+def _self_test() -> None:
+  distance = np.linspace(0.0, 20.0, 81)
+  coefficients = (0.2, 0.03, 0.004, -0.00005)
+  y = sum(coefficient * distance ** power for power, coefficient in enumerate(coefficients))
+  slope = coefficients[1] + 2.0 * coefficients[2] * distance + 3.0 * coefficients[3] * distance ** 2
+  heading = np.arctan(slope)
+  path = ModelPath(distance, y, heading, np.concatenate(([0.0], np.cumsum(np.hypot(np.diff(distance), np.diff(y))))))
+  command = fit_native_path(path, 0.0, 0.0, delay=0.1, horizon=7.0)
+  assert abs(command.c0 - coefficients[0]) < 2e-3
+  assert abs(command.c1 - math.atan(coefficients[1])) < 2e-3
+  expected_c2 = 2.0 * coefficients[2] / (1.0 + coefficients[1] ** 2) ** 1.5
+  assert abs(command.c2 - expected_c2) < 2e-4
+  assert command.fit_rmse < 1e-4
+
+
+def main() -> int:
+  parser = argparse.ArgumentParser(description=__doc__)
+  parser.add_argument("--logs", action="append", help="rlog glob", default=[])
+  parser.add_argument("--delay", type=float, default=0.1)
+  parser.add_argument("--horizon", type=float, action="append")
+  parser.add_argument("--time-horizon", type=float, default=0.0,
+                      help="if nonzero, use clamp(speed * seconds, 1 m, --horizon)")
+  parser.add_argument("--tau-load", type=float, default=0.75)
+  parser.add_argument("--tau-unload", type=float, default=1.3)
+  parser.add_argument("--assumed-tau-load", type=float)
+  parser.add_argument("--assumed-tau-unload", type=float)
+  parser.add_argument("--zero-c3", action="store_true")
+  parser.add_argument("--c2-limit", type=float, action="append",
+                      help="C2 cap to test; defaults to gentle 0.006 and full 0.02")
+  parser.add_argument("--self-test", action="store_true")
+  args = parser.parse_args()
+  if args.self_test:
+    _self_test()
+  paths = _expand(args.logs)
+  if not paths:
+    if args.self_test:
+      return 0
+    parser.error("at least one usable --logs glob is required")
+  samples = load_samples(paths)
+  if not samples:
+    parser.error("logs contain no active Ford path samples")
+  print(f"loaded_logs={len(paths)} samples={len(samples)} tau_load={args.tau_load} tau_unload={args.tau_unload}")
+  for horizon in args.horizon or [3.5, 5.0, 7.0, 10.0]:
+    for c2_limit in args.c2_limit or [0.006, DBC_CURVATURE[1]]:
+      result = evaluate(samples, delay=args.delay, horizon=horizon,
+                        tau_load=args.tau_load, tau_unload=args.tau_unload,
+                        horizon_time=args.time_horizon,
+                        assumed_tau_load=args.assumed_tau_load,
+                        assumed_tau_unload=args.assumed_tau_unload,
+                        use_c3=not args.zero_c3,
+                        c2_limit=c2_limit)
+      print(" ".join(f"{key}={value:.8g}" for key, value in result.items()))
+  return 0
+
+
+if __name__ == "__main__":
+  raise SystemExit(main())