Raise comma four alert volume

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

openpilot/cereal/custom.capnp

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

openpilot/common/params_keys.h

@@ -136,6 +136,7 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
 
     // --- sunnypilot params --- //
     {"ApiCache_DriveStats", {PERSISTENT, JSON}},
+    {"AssistedDistanceMilestoneResetVersion", {PERSISTENT, STRING, "0"}},
     {"AutoLaneChangeBsmDelay", {PERSISTENT | BACKUP, BOOL, "0"}},
     {"AutoLaneChangeTimer", {PERSISTENT | BACKUP, INT, "0"}},
     {"BlinkerLateralReengageDelay", {PERSISTENT | BACKUP, INT, "0"}},  // seconds
@@ -156,6 +157,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"}},
@@ -166,6 +168,7 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
     {"IsReleaseSpBranch", {CLEAR_ON_MANAGER_START, BOOL}},
     {"LastGPSPositionLLK", {PERSISTENT, STRING}},
     {"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/selfdrive/car/helpers.py

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

openpilot/selfdrive/controls/controlsd.py

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

openpilot/selfdrive/controls/lib/ford_path.py

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

openpilot/selfdrive/controls/tests/test_ford_path.py

@@ -0,0 +1,290 @@
+import math
+from types import SimpleNamespace
+
+import numpy as np
+
+from openpilot.cereal import custom
+from openpilot.selfdrive.car.helpers import convert_carControlSP
+from openpilot.selfdrive.controls.lib.ford_path import DBC_CURVATURE, FordPathController
+
+
+def _path(curvature: float, curvature_rate: float = 0.0, speed: float = 8.0):
+  t = np.linspace(0.0, 3.0, 61)
+  distance = speed * t
+  heading = curvature * distance + 0.5 * curvature_rate * distance ** 2
+  x = np.zeros_like(distance)
+  y = np.zeros_like(distance)
+  for i in range(1, len(distance)):
+    ds = distance[i] - distance[i - 1]
+    average_heading = 0.5 * (heading[i] + heading[i - 1])
+    x[i] = x[i - 1] + ds * math.cos(average_heading)
+    y[i] = y[i - 1] + ds * math.sin(average_heading)
+  return SimpleNamespace(
+    position=SimpleNamespace(t=t.tolist(), x=x.tolist(), y=y.tolist()),
+    orientation=SimpleNamespace(z=heading.tolist()),
+  )
+
+
+def _equivalent_curvature(path, distance: float = 7.0) -> float:
+  offset = path.path_offset + path.path_angle * distance + 0.5 * path.curvature * distance ** 2 + \
+    path.curvature_rate * distance ** 3 / 6.0
+  return 2.0 * offset / distance ** 2
+
+
+def _command(model, desired_curvature: float, *, v_ego: float = 0.0, current_curvature: float | None = None):
+  return FordPathController(dt=1.0).update(model, desired_curvature, v_ego=v_ego, current_curvature=current_curvature)
+
+
+def test_steady_arc_uses_c2_without_fast_pose_fields():
+  path = _command(_path(0.008), 0.008, v_ego=8.0)
+
+  assert path.valid
+  assert abs(path.path_offset) < 1e-9
+  assert abs(path.path_angle) < 1e-9
+  assert np.isclose(path.curvature, 0.008, atol=5e-5)
+  assert abs(path.curvature_rate) < 1e-5
+
+
+def test_sunnypilot_path_message_round_trip():
+  message = custom.CarControlSP.new_message()
+  message.fordLateralPath.pathOffset = 0.3
+  message.fordLateralPath.pathAngle = -0.2
+  message.fordLateralPath.curvature = 0.008
+  message.fordLateralPath.curvatureRate = -0.0004
+  message.fordLateralPath.valid = True
+
+  path = convert_carControlSP(message.as_reader()).fordLateralPath
+  assert np.isclose(path.pathOffset, 0.3)
+  assert np.isclose(path.pathAngle, -0.2)
+  assert np.isclose(path.curvature, 0.008)
+  assert np.isclose(path.curvatureRate, -0.0004)
+  assert path.valid
+
+
+def test_tight_arc_uses_signed_forward_pose_without_slow_c2():
+  left = _command(_path(0.04), 0.04, v_ego=8.0)
+  right = _command(_path(-0.04), -0.04, v_ego=8.0)
+
+  assert left.curvature == 0.0
+  assert right.curvature == 0.0
+  assert abs(left.curvature_rate) < 1e-4
+  assert abs(right.curvature_rate) < 1e-4
+  assert left.path_angle > 0.06
+  assert right.path_angle < -0.06
+  assert left.path_offset > 0.5
+  assert right.path_offset < -0.5
+
+
+def test_c2_does_not_increase_while_tight_curve_unwinds():
+  curvatures = (0.04, 0.018, 0.016, 0.014, 0.012, 0.010, 0.008, 0.006, 0.0)
+  measured = (0.04,) + curvatures[:-1]
+  commands = [_command(_path(curvature), curvature, v_ego=8.0, current_curvature=actual).curvature
+              for curvature, actual in zip(curvatures, measured, strict=True)]
+
+  assert np.all(np.diff(commands) <= 1e-9)
+
+
+def test_fresh_model_replaces_previous_path_without_hidden_state():
+  controller = FordPathController(dt=1.0)
+  initial = controller.update(_path(0.04), 0.04, v_ego=8.0)
+  replanned = controller.update(_path(0.0), 0.0, v_ego=8.0)
+
+  assert initial.path_offset > 0.5
+  assert replanned == FordPathController().update(_path(0.0), 0.0, v_ego=8.0)
+
+
+def test_s_turn_reverses_fast_fields_while_c2_is_bounded():
+  controller = FordPathController(dt=0.05)
+  controller.update(_path(0.04), 0.04, v_ego=8.0)
+  controller.update(_path(0.04), 0.04, v_ego=8.0)
+
+  outputs = []
+  for frame_id in range(5):
+    model = _path(-0.02)
+    model.frameId = frame_id + 1
+    model.timestampEof = frame_id + 1
+    outputs.append(controller.update(model, -0.02, v_ego=8.0, current_curvature=0.02))
+
+  assert all(path.valid for path in outputs)
+  assert all(DBC_CURVATURE[0] <= path.curvature <= DBC_CURVATURE[1] for path in outputs)
+  assert all(path.curvature <= 0.0 for path in outputs)
+  assert outputs[-1].path_angle < -0.03
+  assert outputs[-1].path_offset < 0.0
+
+
+def test_reversal_does_not_add_software_persistence_to_centering_c2():
+  controller = FordPathController()
+  assert controller.update(_path(0.002), 0.002, v_ego=8.0).curvature > 0.0
+
+  reversing = controller.update(_path(-0.02), -0.02, v_ego=8.0)
+
+  assert reversing.curvature <= 0.0
+
+
+def test_requested_turn_is_not_cancelled_by_previous_path():
+  controller = FordPathController(dt=1.0)
+  previous = _path(-0.02, speed=3.0)
+  previous.frameId = 1
+  previous.timestampEof = 1
+  controller.update(previous, -0.02, v_ego=3.0, current_curvature=-0.01)
+
+  requested = _path(0.02, speed=3.0)
+  requested.frameId = 2
+  requested.timestampEof = 2
+  command = controller.update(requested, 0.02, v_ego=3.0, current_curvature=0.007)
+
+  assert command.path_offset >= 0.0
+  assert command.path_angle >= 0.0
+  assert command.curvature >= 0.0
+  assert _equivalent_curvature(command) >= 0.02
+
+
+def test_short_low_speed_model_uses_available_path_endpoint():
+  command = FordPathController().update(_path(0.02, speed=1.0), 0.02, v_ego=1.0, current_curvature=0.0)
+
+  assert command.valid
+  assert command.path_offset > 0.0
+  assert command.path_angle > 0.0
+
+
+def test_action_demand_exposes_forward_path_authority():
+  command = FordPathController().update(_path(0.002), 0.0055, v_ego=8.0, current_curvature=0.0005)
+
+  assert _equivalent_curvature(command) >= 0.004
+
+
+def test_minor_curve_uses_c2_when_tracking_is_close():
+  command = FordPathController(dt=1.0).update(_path(0.005), 0.005, v_ego=8.0, current_curvature=0.0048)
+
+  assert abs(command.path_offset) < 1e-9
+  assert abs(command.path_angle) < 1e-9
+  assert command.curvature > 0.0049
+
+
+def test_minor_changing_curve_does_not_emit_ungated_c3():
+  command = FordPathController(dt=1.0).update(_path(0.005, 0.0003), 0.005, v_ego=8.0, current_curvature=0.0048)
+
+  assert abs(command.path_offset) < 1e-9
+  assert abs(command.path_angle) < 1e-9
+  assert command.curvature > 0.0049
+  assert command.curvature_rate == 0.0
+
+
+def test_c2_uses_stable_action_curvature_not_independent_model_fit():
+  controller = FordPathController(dt=1.0)
+  first = controller.update(_path(0.004), 0.002, v_ego=8.0, current_curvature=0.002)
+  second = controller.update(_path(0.006), 0.002, v_ego=8.0, current_curvature=0.002)
+
+  assert np.isclose(first.curvature, 0.002)
+  assert np.isclose(second.curvature, 0.002)
+
+
+def test_action_curvature_corrects_stale_opposing_model_at_low_speed():
+  command = FordPathController().update(_path(-0.001, speed=1.0), 0.005, v_ego=1.0, current_curvature=0.001)
+
+  assert command.path_offset > 0.0
+  assert command.path_angle > 0.0
+  assert command.curvature >= 0.0
+  assert _equivalent_curvature(command) >= 0.004
+
+
+def test_small_action_sign_noise_does_not_reverse_a_strong_model_path():
+  command = FordPathController(dt=1.0).update(_path(0.04), -0.0005, v_ego=6.0, current_curvature=0.02)
+
+  assert command.path_offset > 0.0
+  assert command.path_angle > 0.0
+  assert _equivalent_curvature(command) > 0.02
+
+
+def test_measured_curvature_after_path_exit_commands_countersteer():
+  command = FordPathController().update(_path(0.0), 0.0, v_ego=8.0, current_curvature=0.006)
+
+  assert command.curvature == 0.0
+  assert command.path_offset < 0.0
+  assert command.path_angle < 0.0
+
+
+def test_measured_curvature_countersteers_when_beyond_modeled_arc():
+  controller = FordPathController(dt=1.0)
+  command = controller.update(_path(0.004), 0.003, v_ego=8.0, current_curvature=0.012)
+  tracking = FordPathController(dt=1.0).update(_path(0.004), 0.003, v_ego=8.0, current_curvature=0.004)
+
+  assert command.path_offset < 0.0
+  assert command.path_angle < 0.0
+  assert command.path_angle < tracking.path_angle
+  assert command.curvature == 0.0
+
+
+def test_model_reversal_suppresses_old_c2_and_countersteers():
+  reversing = FordPathController().update(_path(-0.02), -0.0005, v_ego=8.0, current_curvature=0.01)
+
+  assert reversing.curvature <= 0.0
+  assert reversing.path_angle < 0.0
+
+
+def test_reversal_noise_band_is_continuous():
+  inside = FordPathController(dt=1.0).update(_path(0.02), -0.000099, v_ego=8.0, current_curvature=0.01)
+  outside = FordPathController(dt=1.0).update(_path(0.02), -0.000101, v_ego=8.0, current_curvature=0.01)
+
+  assert abs(outside.path_angle - inside.path_angle) < 0.005
+
+
+def test_curvature_error_increases_forward_pose_command_while_behind():
+  behind = FordPathController(dt=1.0).update(_path(0.008), 0.008, v_ego=15.0, current_curvature=0.0)
+  tracking = FordPathController(dt=1.0).update(_path(0.008), 0.008, v_ego=15.0, current_curvature=0.008)
+
+  assert behind.path_offset > tracking.path_offset + 0.01
+  assert behind.path_angle > tracking.path_angle + 0.015
+  assert behind.curvature == 0.0
+  assert tracking.curvature > 0.007
+  assert np.isclose(behind.curvature_rate, tracking.curvature_rate)
+
+
+def test_measured_wheel_beyond_action_countersteers_model_arc():
+  controller = FordPathController()
+  controller.update(_path(0.02), 0.02, v_ego=15.0, current_curvature=0.02)
+  outputs = [controller.update(_path(0.02), 0.003, v_ego=15.0, current_curvature=0.01) for _ in range(4)]
+  unwinding = outputs[-1]
+
+  assert unwinding.curvature == 0.0
+  assert unwinding.path_angle < 0.0
+
+
+def test_action_c2_remains_active_for_centering():
+  centering = FordPathController(dt=1.0).update(_path(0.002), 0.002, v_ego=15.0, current_curvature=0.002)
+
+  assert centering.curvature > 0.001
+  assert abs(centering.path_offset) < 1e-9
+  assert abs(centering.path_angle) < 1e-9
+
+
+def test_tight_turn_from_stop_builds_bounded_forward_pose_authority():
+  controller = FordPathController()
+  outputs = [controller.update(_path(0.04), 0.04, v_ego=0.0, current_curvature=0.0) for _ in range(20)]
+  path = outputs[-1]
+
+  assert path.curvature == 0.0
+  assert path.path_offset > 0.7
+  assert path.path_angle > 0.15
+  assert np.max(np.abs(np.diff([output.path_offset for output in outputs]))) <= 0.04 + 1e-9
+  assert np.max(np.abs(np.diff([output.path_angle for output in outputs]))) <= 0.01 + 1e-9
+
+
+def test_curvature_feedback_is_bounded_for_bad_measurement():
+  bounded = FordPathController().update(_path(0.008), 0.008, v_ego=15.0, current_curvature=-0.02)
+  corrupted = FordPathController().update(_path(0.008), 0.008, v_ego=15.0, current_curvature=-1.0)
+
+  assert np.isclose(corrupted.path_angle, bounded.path_angle)
+
+
+def test_invalid_model_ramps_pose_to_zero_while_remaining_in_extended_mode():
+  controller = FordPathController()
+  for _ in range(10):
+    active = controller.update(_path(0.04), 0.04, v_ego=12.0)
+  missing = controller.update(None, 0.0, v_ego=12.0)
+
+  assert active.path_offset > 0.0
+  assert missing.valid
+  assert np.isclose(active.path_offset - missing.path_offset, 0.04)
+  assert missing.curvature == 0.0
+  assert not controller.update(_path(0.0), 0.0, v_ego=12.0, active=False).valid

openpilot/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_local_prototype = PC and os.getenv("SP_MILESTONE_PROTOTYPE") == "1"
+    if not self._onboarding_window.completed and not skip_onboarding_for_local_prototype:
       gui_app.push_widget(self._onboarding_window)
 
     # initialize correct onroad layout

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_PROTOTYPE_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_PROTOTYPE_ENABLED:
+    from openpilot.selfdrive.ui.sunnypilot.onroad.milestone_celebration_prototype import MilestoneCelebrationPrototype
+
 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(MilestoneCelebrationPrototype()) if MILESTONE_PROTOTYPE_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,
@@ -221,6 +227,9 @@ class AugmentedRoadView(CameraView):
     # Fade out bottom of overlays for looks
     rl.draw_texture_ex(self._fade_texture, rl.Vector2(self._content_rect.x, self._content_rect.y), 0.0, 1.0, rl.WHITE)
 
+    if self._milestone_celebration is not None:
+      self._milestone_celebration.render(self._content_rect)
+
     alert_to_render, not_animating_out = self._alert_renderer.will_render()
 
     # Hide DMoji when disengaged unless AlwaysOnDM is enabled
@@ -247,6 +256,8 @@ class AugmentedRoadView(CameraView):
     self._confidence_ball.render(self.rect)
 
     self._bookmark_icon.render(self.rect)
+    if self._milestone_celebration is not None:
+      self._milestone_celebration.capture_screenshot()
 
   def _switch_stream_if_needed(self, sm):
     if sm['selfdriveState'].experimentalMode and WIDE_CAM in self.available_streams:
@@ -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

@@ -15,6 +15,7 @@ from openpilot.system import micd
 from openpilot.common.hardware import HARDWARE
 
 from openpilot.sunnypilot.selfdrive.ui.quiet_mode import QuietMode
+from openpilot.selfdrive.ui.sunnypilot.onroad.milestone_tracker_prototype import MILESTONE_EVENT_PAYLOAD
 
 SAMPLE_RATE = 48000
 SAMPLE_BUFFER = 4096 # (approx 100ms)
@@ -24,14 +25,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 +48,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 +56,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 +78,7 @@ class Soundd(QuietMode):
   def __init__(self):
     super().__init__()
 
+    self.device_type = HARDWARE.get_device_type()
     self.load_sounds()
 
     self.current_alert = AudibleAlert.none
@@ -164,9 +169,13 @@ class Soundd(QuietMode):
       self.update_alert(AudibleAlert.none)
       self.selfdrive_timeout_alert = False
 
+  def update_milestone_alert(self, sm):
+    milestone_event = sm.updated['customReservedRawData0'] and bytes(sm['customReservedRawData0']) == MILESTONE_EVENT_PAYLOAD
+    if milestone_event and 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 +189,7 @@ class Soundd(QuietMode):
     import sounddevice as sd
     micd.patch_sounddevice(sd)
 
-    sm = messaging.SubMaster(['selfdriveState', 'selfdriveStateSP', 'soundPressure'])
+    sm = messaging.SubMaster(['selfdriveState', 'selfdriveStateSP', 'soundPressure', 'customReservedRawData0'])
 
     with self.get_stream(sd) as stream:
       rk = Ratekeeper(20)
@@ -198,6 +207,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/onroad/milestone_celebration_prototype.py

@@ -0,0 +1,241 @@
+"""Tesla-style persistent assisted-distance milestones over the on-road view."""
+
+import math
+import os
+import random
+import time
+from collections import deque
+from dataclasses import dataclass
+
+import pyray as rl
+
+from openpilot.cereal import messaging
+from openpilot.common.hardware import PC
+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.selfdrive.ui.sunnypilot.onroad.milestone_tracker_prototype import (
+  AssistCategory,
+  AssistedDistanceMilestoneTracker,
+  DistanceMilestone,
+  MILESTONE_EVENT_PAYLOAD,
+  MilestoneStore,
+  assist_category,
+)
+from openpilot.system.ui.lib.application import FontWeight, gui_app
+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
+PERSIST_INTERVAL_SECONDS = 60.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
+
+
+class MilestoneCelebrationPrototype(Widget):
+  """Throwaway visual spike enabled on the sunnypilot comma four UI."""
+
+  def __init__(self):
+    super().__init__()
+    self._drive_started_time = -1.0
+    self._celebration_started_time: float | None = None
+    self._current_milestone: DistanceMilestone | None = None
+    self._pending_milestones: deque[DistanceMilestone] = deque()
+    self._store = MilestoneStore()
+    stored_distances = self._store.reset() if PC and os.getenv("SP_MILESTONE_RESET") == "1" else self._store.load()
+    self._tracker = AssistedDistanceMilestoneTracker(stored_distances)
+    self._pm = messaging.PubMaster(["customReservedRawData0"])
+    self._last_persisted_distances = stored_distances
+    self._last_persist_time = time.monotonic()
+    self._screenshot_taken = False
+    self._screenshot_ready = False
+    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._persist_distances(force=True)
+      self._drive_started_time = ui_state.started_time
+      self._celebration_started_time = None
+      self._current_milestone = None
+      self._pending_milestones.clear()
+      self._tracker.reset_sampling()
+      self._screenshot_taken = False
+      self._screenshot_ready = False
+
+    car_control = ui_state.sm["carControl"]
+    category = assist_category(car_control.latActive, car_control.longActive)
+
+    milestones = self._tracker.update(
+      ui_state.sm.logMonoTime["carState"],
+      ui_state.sm["carState"].vEgo,
+      category,
+    )
+    self._pending_milestones.extend(milestones)
+    self._persist_distances(force=bool(milestones))
+
+    if self._current_milestone is None and self._pending_milestones:
+      self._current_milestone = self._pending_milestones.popleft()
+      self._celebration_started_time = now
+      milestone_event = messaging.new_message("customReservedRawData0", size=len(MILESTONE_EVENT_PAYLOAD), valid=True)
+      milestone_event.customReservedRawData0 = MILESTONE_EVENT_PAYLOAD
+      self._pm.send("customReservedRawData0", milestone_event)
+
+    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)
+    self._screenshot_ready = elapsed >= 1.0
+
+  def hide_event(self) -> None:
+    self._persist_distances(force=True)
+    super().hide_event()
+
+  def _persist_distances(self, force: bool = False) -> None:
+    distances = self._tracker.distances_meters()
+    if distances == self._last_persisted_distances:
+      return
+    now = time.monotonic()
+    if force or now - self._last_persist_time >= PERSIST_INTERVAL_SECONDS:
+      self._store.save(distances)
+      self._last_persisted_distances = distances
+      self._last_persist_time = now
+
+  def capture_screenshot(self) -> None:
+    screenshot_path = os.getenv("SP_MILESTONE_SCREENSHOT")
+    if screenshot_path and self._screenshot_ready and not self._screenshot_taken:
+      rl.rl_draw_render_batch_active()
+      rl.take_screenshot(screenshot_path)
+      self._screenshot_taken = True
+
+  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: DistanceMilestone) -> 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
+    previous_distance = milestone.previous_distance_miles
+    displayed_distance = previous_distance + (milestone.distance_miles - previous_distance) * tween_progress
+    if tween_progress >= 1.0:
+      number = f"{round(milestone.distance_miles):,}"
+    else:
+      number = f"{displayed_distance:,.1f}"
+    unit = "MI"
+    category = "FULL ASSIST" if milestone.category == AssistCategory.FULL_ASSIST else "MADS"
+    milestone_label = "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/onroad/milestone_tracker_prototype.py

@@ -0,0 +1,140 @@
+"""Persistent assisted-distance 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
+MAX_SAMPLE_INTERVAL_SECONDS = 0.5
+MILESTONE_EVENT_PAYLOAD = b"sunnypilot-milestone-v1"
+
+
+class AssistCategory(StrEnum):
+  MADS = "mads"
+  FULL_ASSIST = "full_assist"
+
+
+PARAM_KEYS = {
+  AssistCategory.MADS: "MadsDrivenDistanceMeters",
+  AssistCategory.FULL_ASSIST: "FullAssistDrivenDistanceMeters",
+}
+
+
+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 next_milestone_miles(distance_miles: float) -> float:
+  """Return the next value in the 1, 2, 5 × 10ⁿ milestone ladder."""
+  distance_miles = max(0.0, distance_miles)
+  magnitude = 10.0 ** math.floor(math.log10(max(1.0, distance_miles)))
+  for multiplier in (1.0, 2.0, 5.0):
+    candidate = multiplier * magnitude
+    if candidate > distance_miles + 1e-9:
+      return candidate
+  return 10.0 * magnitude
+
+
+def previous_milestone_miles(milestone_miles: float) -> float:
+  if milestone_miles <= 1.0:
+    return 0.0
+  magnitude = 10.0 ** math.floor(math.log10(milestone_miles))
+  normalized = milestone_miles / 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
+
+
+@dataclass(frozen=True)
+class DistanceMilestone:
+  category: AssistCategory
+  distance_meters: float
+  previous_distance_meters: float
+
+  @property
+  def distance_miles(self) -> float:
+    return self.distance_meters / METERS_PER_MILE
+
+  @property
+  def previous_distance_miles(self) -> float:
+    return self.previous_distance_meters / METERS_PER_MILE
+
+
+class MilestoneStore:
+  def __init__(self, params: Params | None = None):
+    self._params = params or Params()
+
+  def load(self) -> dict[AssistCategory, float]:
+    return {
+      category: max(0.0, self._params.get(key, return_default=True) or 0.0)
+      for category, key in PARAM_KEYS.items()
+    }
+
+  def save(self, distances_meters: Mapping[AssistCategory, float]) -> None:
+    for category, key in PARAM_KEYS.items():
+      self._params.put(key, max(0.0, distances_meters.get(category, 0.0)))
+
+  def reset(self) -> dict[AssistCategory, float]:
+    distances = dict.fromkeys(AssistCategory, 0.0)
+    self.save(distances)
+    return distances
+
+
+class AssistedDistanceMilestoneTracker:
+  def __init__(self, initial_distances_meters: Mapping[AssistCategory, float] | None = None):
+    initial_distances_meters = initial_distances_meters or {}
+    self._distance_meters = {
+      category: max(0.0, initial_distances_meters.get(category, 0.0))
+      for category in AssistCategory
+    }
+    self._next_milestone_meters = {
+      category: next_milestone_miles(distance / METERS_PER_MILE) * METERS_PER_MILE
+      for category, distance in self._distance_meters.items()
+    }
+    self.reset_sampling()
+
+  def reset_sampling(self) -> None:
+    self._last_timestamp_ns: int | None = None
+    self._last_speed_mps = 0.0
+    self._last_category: AssistCategory | None = None
+
+  def distance_meters(self, category: AssistCategory) -> float:
+    return self._distance_meters[category]
+
+  def distances_meters(self) -> dict[AssistCategory, float]:
+    return self._distance_meters.copy()
+
+  def update(self, timestamp_ns: int, speed_mps: float, category: AssistCategory | None) -> list[DistanceMilestone]:
+    milestones: list[DistanceMilestone] = []
+    speed_mps = max(0.0, speed_mps)
+
+    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:
+        delta_meters = (self._last_speed_mps + speed_mps) / 2.0 * dt
+        active_category = self._last_category
+        self._distance_meters[active_category] += delta_meters
+
+        next_milestone = self._next_milestone_meters[active_category]
+        while self._distance_meters[active_category] >= next_milestone:
+          milestone_miles = next_milestone / METERS_PER_MILE
+          milestones.append(DistanceMilestone(
+            active_category,
+            next_milestone,
+            previous_milestone_miles(milestone_miles) * METERS_PER_MILE,
+          ))
+          next_milestone = next_milestone_miles(milestone_miles) * METERS_PER_MILE
+        self._next_milestone_meters[active_category] = next_milestone
+
+    self._last_timestamp_ns = timestamp_ns
+    self._last_speed_mps = speed_mps
+    self._last_category = category
+    return milestones

openpilot/selfdrive/ui/tests/generate_milestone_sound.py

@@ -0,0 +1,45 @@
+#!/usr/bin/env python3
+"""Generate the temporary 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/run_milestone_celebration_prototype.sh

@@ -0,0 +1,24 @@
+#!/usr/bin/env bash
+# PROTOTYPE: launch the demo replay and comma four milestone celebration together.
+set -e
+
+prototype_root="$(cd "$(dirname "${BASH_SOURCE[0]}")/../../../.." && pwd)"
+prototype_replay_pid=""
+
+cleanup_prototype() {
+  if [[ -n "$prototype_replay_pid" ]]; then
+    kill "$prototype_replay_pid" 2>/dev/null || true
+    wait "$prototype_replay_pid" 2>/dev/null || true
+  fi
+}
+trap cleanup_prototype EXIT INT TERM
+
+export PATH="$prototype_root/.venv/bin:$PATH"
+export SP_MILESTONE_PROTOTYPE=1
+export SP_MILESTONE_RESET=1
+prototype_playback="${SP_MILESTONE_PLAYBACK:-1}"
+
+"$prototype_root/openpilot/tools/replay/replay" --demo --playback "$prototype_playback" &
+prototype_replay_pid=$!
+
+"$prototype_root/.venv/bin/python" "$prototype_root/openpilot/selfdrive/ui/mici/onroad/augmented_road_view.py"

openpilot/selfdrive/ui/tests/test_milestone_tracker_prototype.py

@@ -0,0 +1,106 @@
+import unittest
+
+from openpilot.selfdrive.ui.sunnypilot.onroad.milestone_tracker_prototype import (
+  METERS_PER_MILE,
+  PARAM_KEYS,
+  AssistCategory,
+  AssistedDistanceMilestoneTracker,
+  MilestoneStore,
+  assist_category,
+  next_milestone_miles,
+)
+
+
+class TestAssistedDistanceMilestoneTracker(unittest.TestCase):
+  def test_store_round_trips_each_category(self):
+    class ParamsStub:
+      def __init__(self):
+        self.values = {
+          PARAM_KEYS[AssistCategory.MADS]: 123.0,
+          PARAM_KEYS[AssistCategory.FULL_ASSIST]: 456.0,
+        }
+
+      def get(self, key, return_default=False):
+        return self.values.get(key, 0.0 if return_default else None)
+
+      def put(self, key, value):
+        self.values[key] = value
+
+    params = ParamsStub()
+    store = MilestoneStore(params)  # type: ignore[arg-type]
+    self.assertEqual(store.load(), {
+      AssistCategory.MADS: 123.0,
+      AssistCategory.FULL_ASSIST: 456.0,
+    })
+
+    store.save({AssistCategory.MADS: 789.0, AssistCategory.FULL_ASSIST: 987.0})
+    self.assertEqual(store.load(), {
+      AssistCategory.MADS: 789.0,
+      AssistCategory.FULL_ASSIST: 987.0,
+    })
+
+  def test_classifies_actual_actuation(self):
+    self.assertIsNone(assist_category(False, False))
+    self.assertIsNone(assist_category(False, True))
+    self.assertEqual(assist_category(True, False), AssistCategory.MADS)
+    self.assertEqual(assist_category(True, True), AssistCategory.FULL_ASSIST)
+
+  def test_uses_a_one_two_five_milestone_ladder(self):
+    cases = (
+      (0.0, 1.0),
+      (1.0, 2.0),
+      (2.0, 5.0),
+      (5.0, 10.0),
+      (10.0, 20.0),
+      (49.9, 50.0),
+      (50.0, 100.0),
+      (999.0, 1000.0),
+      (1000.0, 2000.0),
+    )
+    for distance, expected in cases:
+      with self.subTest(distance=distance):
+        self.assertEqual(next_milestone_miles(distance), expected)
+
+  def test_tracks_categories_and_emits_dynamic_milestones(self):
+    tracker = AssistedDistanceMilestoneTracker({
+      AssistCategory.MADS: METERS_PER_MILE - 5.0,
+      AssistCategory.FULL_ASSIST: 2 * METERS_PER_MILE - 5.0,
+    })
+
+    self.assertEqual(tracker.update(0, 10.0, AssistCategory.MADS), [])
+    milestones = tracker.update(500_000_000, 10.0, AssistCategory.FULL_ASSIST)
+    self.assertEqual(len(milestones), 1)
+    self.assertEqual(milestones[0].category, AssistCategory.MADS)
+    self.assertAlmostEqual(milestones[0].previous_distance_miles, 0.0)
+    self.assertAlmostEqual(milestones[0].distance_miles, 1.0)
+
+    milestones = tracker.update(1_000_000_000, 10.0, AssistCategory.FULL_ASSIST)
+    self.assertEqual(len(milestones), 1)
+    self.assertEqual(milestones[0].category, AssistCategory.FULL_ASSIST)
+    self.assertAlmostEqual(milestones[0].previous_distance_miles, 1.0)
+    self.assertAlmostEqual(milestones[0].distance_miles, 2.0)
+
+  def test_does_not_count_unassisted_time_or_timestamp_gaps(self):
+    tracker = AssistedDistanceMilestoneTracker()
+
+    tracker.update(0, 20.0, None)
+    tracker.update(500_000_000, 20.0, AssistCategory.MADS)
+    self.assertEqual(tracker.distance_meters(AssistCategory.MADS), 0.0)
+
+    tracker.update(2_000_000_000, 20.0, AssistCategory.MADS)
+    self.assertEqual(tracker.distance_meters(AssistCategory.MADS), 0.0)
+
+  def test_drive_reset_preserves_persistent_distance(self):
+    tracker = AssistedDistanceMilestoneTracker()
+    tracker.update(0, 10.0, AssistCategory.MADS)
+    tracker.update(500_000_000, 10.0, AssistCategory.MADS)
+    distance_before_reset = tracker.distance_meters(AssistCategory.MADS)
+
+    tracker.reset_sampling()
+
+    self.assertEqual(tracker.distance_meters(AssistCategory.MADS), distance_before_reset)
+    self.assertEqual(tracker.update(1_000_000_000, 10.0, AssistCategory.MADS), [])
+
+
+if __name__ == "__main__":
+  unittest.main()

openpilot/selfdrive/ui/tests/test_soundd.py

@@ -4,12 +4,36 @@ 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
+from openpilot.selfdrive.ui.sunnypilot.onroad.milestone_tracker_prototype import MILESTONE_EVENT_PAYLOAD
 
 AudibleAlert = log.SelfdriveState.AudibleAlert
 
 
 class TestSoundd(OpenpilotTestCase):
+  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_ui_milestone_event(self):
+    soundd = Soundd()
+
+    class SubMasterStub:
+      def __init__(self):
+        self.updated = {'customReservedRawData0': True}
+        self.data = {'customReservedRawData0': MILESTONE_EVENT_PAYLOAD}
+
+      def __getitem__(self, service):
+        return self.data[service]
+
+    sm = SubMasterStub()
+    soundd.update_milestone_alert(sm)
+
+    assert soundd.current_alert == AudibleAlert.complete
+
   def test_check_selfdrive_timeout_alert(self, mocker):
     sm = SubMaster(['selfdriveState', 'selfdriveStateSP'])
     pm = PubMaster(['selfdriveState', 'selfdriveStateSP'])

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/system/params_migration.py

@@ -11,6 +11,7 @@ from openpilot.sunnypilot.selfdrive.car.sync_sunnylink_params import CAR_LIST_JS
 
 ONROAD_BRIGHTNESS_MIGRATION_VERSION: str = "1.0"
 ONROAD_BRIGHTNESS_TIMER_MIGRATION_VERSION: str = "1.0"
+ASSISTED_DISTANCE_MILESTONE_RESET_VERSION: str = "1"
 
 # index → seconds mapping for OnroadScreenOffTimer (SSoT)
 ONROAD_BRIGHTNESS_TIMER_VALUES = {0: 3, 1: 5, 2: 7, 3: 10, 4: 15, 5: 30, **{i: (i - 5) * 60 for i in range(6, 16)}}
@@ -99,6 +100,19 @@ def _migrate_model_bundle_slots(_params):
     cloudlog.exception(f"Error migrating model bundle slots: {e}")
 
 
+def _reset_assisted_distance_milestones(_params):
+  try:
+    if _params.get("AssistedDistanceMilestoneResetVersion", return_default=True) == ASSISTED_DISTANCE_MILESTONE_RESET_VERSION:
+      return
+
+    _params.put("MadsDrivenDistanceMeters", 0.0, block=True)
+    _params.put("FullAssistDrivenDistanceMeters", 0.0, block=True)
+    _params.put("AssistedDistanceMilestoneResetVersion", ASSISTED_DISTANCE_MILESTONE_RESET_VERSION, block=True)
+    cloudlog.info("params_migration: reset assisted-distance milestone counters")
+  except Exception as e:
+    cloudlog.exception(f"Error resetting assisted-distance milestone counters: {e}")
+
+
 def run_migration(_params):
   # migrate OnroadScreenOffBrightness
   if _params.get("OnroadScreenOffBrightnessMigrated") != ONROAD_BRIGHTNESS_MIGRATION_VERSION:
@@ -138,3 +152,6 @@ def run_migration(_params):
 
   # seed the usbgpu model slot from the pre-split single slot
   _migrate_model_bundle_slots(_params)
+
+  # reset prototype milestone counters once for the next test cycle
+  _reset_assisted_distance_milestones(_params)

openpilot/sunnypilot/system/tests/test_params_migration.py

@@ -7,7 +7,40 @@ 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 TestAssistedDistanceMilestoneReset(OpenpilotTestCase):
+  def test_resets_existing_distances_once(self):
+    class ParamsStub:
+      def __init__(self):
+        self.values = {
+          "MadsDrivenDistanceMeters": 123.0,
+          "FullAssistDrivenDistanceMeters": 456.0,
+          "OnroadScreenOffBrightness": 0,
+          "OnroadScreenOffTimer": 15,
+          "AssistedDistanceMilestoneResetVersion": "0",
+        }
+
+      def get(self, key, return_default=False):
+        return self.values.get(key)
+
+      def put(self, key, value, block=False):
+        self.values[key] = value
+
+    params = ParamsStub()
+
+    run_migration(params)
+
+    assert params.get("MadsDrivenDistanceMeters") == 0.0
+    assert params.get("FullAssistDrivenDistanceMeters") == 0.0
+
+    params.put("MadsDrivenDistanceMeters", 12.0, block=True)
+    params.put("FullAssistDrivenDistanceMeters", 34.0, block=True)
+    run_migration(params)
+
+    assert params.get("MadsDrivenDistanceMeters") == 12.0
+    assert params.get("FullAssistDrivenDistanceMeters") == 34.0
 
 
 class TestModelBundleSlotMigration(OpenpilotTestCase):