Ford: add low-speed model delay preview

Add up to 0.4 seconds to lateral model preview at or below 15 mph, tapering to zero at 30 mph. Apply only when the Ford CAN-FD C0/C1 controller is enabled, using the same startup toggle semantics as controlsd. Preserve the underlying delay estimate and longitudinal action time.

Validation: 397 helper, Ford controller, model recovery and parser tests passed; lint and whitespace checks passed. Actual model delay construction matches the offline +0.4-second sweep across 2,560 recorded frames at float32 and model float16 precision.

openpilot/selfdrive/modeld/modeld.py

@@ -35,7 +35,7 @@ from openpilot.common.hardware.usb import CHESTNUT_USB_IDS
 from openpilot.selfdrive.modeld.constants import ModelConstants, Plan
 from openpilot.selfdrive.modeld.helpers import chestnut_present, chestnut_compiled, chestnut_ready, modeld_pkl_path, load_oob
 
-from openpilot.sunnypilot.livedelay.helpers import get_lat_delay
+from openpilot.sunnypilot.livedelay.helpers import get_ford_delay_offset, get_lat_delay
 from openpilot.sunnypilot.modeld_v2.modeld_base import ModelStateBase
 from openpilot.sunnypilot.selfdrive.controls.lib.relc import RoadEdgeLaneChangeController
 
@@ -344,6 +344,7 @@ def main(demo=False):
   else:
     CP = messaging.log_from_bytes(params.get("CarParams", block=True), car.CarParams)
   cloudlog.info("modeld got CarParams: %s", CP.brand)
+  ford_model_action = params.get_bool("FordModelActionController")
 
   # TODO this needs more thought, use .2s extra for now to estimate other delays
   # TODO Move smooth seconds to action function
@@ -393,6 +394,7 @@ def main(demo=False):
     v_ego = max(sm["carState"].vEgo, 0.)
     model.lat_delay = get_lat_delay(params, sm["lateralDelay"].lateralDelay)
     lat_delay = sm["lateralDelay"].lateralDelay + LAT_SMOOTH_SECONDS
+    lat_delay += get_ford_delay_offset(CP, ford_model_action, v_ego)
     if sm.updated["extrinsicsCalibration"] and sm.seen['narrowRoadCameraState'] and sm.seen['deviceState']:
       device_from_calib_euler = np.array(sm["extrinsicsCalibration"].rpyCalib, dtype=np.float32)
       dc = DEVICE_CAMERAS[(str(sm['deviceState'].deviceType), str(sm['narrowRoadCameraState'].sensor))]

openpilot/sunnypilot/livedelay/helpers.py

@@ -4,6 +4,11 @@ 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 math
+
+from opendbc.car import structs
+from opendbc.car.ford.values import FordFlags
+from opendbc.car.common.conversions import Conversions as CV
 from openpilot.common.params import Params
 
 
@@ -15,3 +20,11 @@ def get_lat_delay(params: Params, stock_lat_delay: float) -> float:
     return stock_lat_delay
 
   return float(params.get("LagdValueCache", return_default=True))
+
+
+def get_ford_delay_offset(CP: structs.CarParams, enabled: bool, v_ego: float) -> float:
+  """Extra model preview for the opt-in C0/C1 controller; never alters learned delay."""
+  if not enabled or CP.brand != 'ford' or not CP.flags & FordFlags.CANFD or not math.isfinite(v_ego):
+    return 0.
+  # Route 146 trial: full preview through 15 mph, fading to zero at 30 mph.
+  return .4 * max(0., min(1., (30. - v_ego / CV.MPH_TO_MS) / 15.))

openpilot/sunnypilot/livedelay/test_helpers.py

@@ -0,0 +1,55 @@
+from types import SimpleNamespace
+
+import numpy as np
+import pytest
+
+from opendbc.car.common.conversions import Conversions as CV
+from opendbc.car.ford.values import CAR, FordFlags
+from openpilot.sunnypilot.livedelay.helpers import get_ford_delay_offset, get_lat_delay
+
+
+@pytest.mark.parametrize('mph,expected', [(-1, .4), (0, .4), (10, .4), (15, .4), (20, .4*2/3),
+                                          (22.5, .2), (25, .4/3), (30, 0), (45, 0), (100, 0)])
+def test_preview_schedule(mph, expected):
+  cp = SimpleNamespace(brand='ford', flags=FordFlags.CANFD)
+  assert get_ford_delay_offset(cp, True, mph*CV.MPH_TO_MS) == pytest.approx(expected)
+
+
+@pytest.mark.parametrize('enabled', [False, True])
+@pytest.mark.parametrize('brand,flags', [('ford', 0), ('ford', 8), ('ford', FordFlags.CANFD),
+                                       ('ford', FordFlags.CANFD | 8), ('toyota', FordFlags.CANFD)])
+def test_only_enabled_ford_canfd_has_preview(enabled, brand, flags):
+  cp = SimpleNamespace(brand=brand, flags=flags)
+  expected = .4 if enabled and brand == 'ford' and flags & FordFlags.CANFD else 0.
+  assert get_ford_delay_offset(cp, enabled, 0.) == expected
+
+
+@pytest.mark.parametrize('vehicle', list(CAR))
+def test_all_ford_platforms_follow_canfd_gate(vehicle):
+  cp = SimpleNamespace(brand='ford', flags=vehicle.config.flags)
+  assert get_ford_delay_offset(cp, True, 5.) == (.4 if vehicle.config.flags & FordFlags.CANFD else 0.)
+
+
+@pytest.mark.parametrize('v_ego', [float('nan'), float('inf'), -float('inf')])
+def test_invalid_speed_does_not_add_preview(v_ego):
+  assert get_ford_delay_offset(SimpleNamespace(brand='ford', flags=FordFlags.CANFD), True, v_ego) == 0.
+
+
+def test_preview_is_continuous_and_recomputed_from_current_speed():
+  cp = SimpleNamespace(brand='ford', flags=FordFlags.CANFD)
+  mph = np.linspace(0, 60, 12001)
+  delays = np.array([get_ford_delay_offset(cp, True, v*CV.MPH_TO_MS) for v in mph])
+  assert np.all((0 <= delays) & (delays <= .4))
+  assert np.all(np.diff(delays) <= 0)
+  assert np.max(np.abs(np.diff(delays))) <= .4/15*.005 + 1e-14
+  assert [get_ford_delay_offset(cp, True, v*CV.MPH_TO_MS) for v in [10, 40, 10]] == [.4, 0., .4]
+
+
+@pytest.mark.parametrize('learning,expected', [(True, .16894637), (False, .32)])
+def test_preview_does_not_replace_or_write_the_base_delay(learning, expected):
+  params = SimpleNamespace(get_bool=lambda key: learning, get=lambda key, **kwargs: .32)
+  cp = SimpleNamespace(brand='ford', flags=FordFlags.CANFD)
+  base = get_lat_delay(params, .16894637)
+  assert base == expected
+  assert base + get_ford_delay_offset(cp, True, 0.) == pytest.approx(expected+.4)
+  assert get_lat_delay(params, .16894637) == expected

openpilot/sunnypilot/modeld_v2/modeld.py

@@ -50,7 +50,7 @@ from openpilot.sunnypilot.modeld_v2.camera_offset_helper import CameraOffsetHelp
 from openpilot.sunnypilot.modeld_v2.compile_modeld import (derive_frame_skip, make_split_input_queues,
                                                            make_supercombo_input_queues, nv12_copy_size,
                                                            WARP_INPUTS, POLICY_INPUTS)
-from openpilot.sunnypilot.livedelay.helpers import get_lat_delay
+from openpilot.sunnypilot.livedelay.helpers import get_ford_delay_offset, get_lat_delay
 from openpilot.sunnypilot.modeld_v2.modeld_base import ModelStateBase
 from openpilot.sunnypilot.modeld_v2.helpers import load_oob
 from openpilot.sunnypilot.models.helpers import get_active_bundle
@@ -414,6 +414,7 @@ def main(demo=False):
   else:
     CP = messaging.log_from_bytes(params.get("CarParams", block=True), car.CarParams)
   cloudlog.info("modeld got CarParams: %s", CP.brand)
+  ford_model_action = params.get_bool("FordModelActionController")
 
   # TODO Move smooth seconds to action function
   long_delay = CP.longitudinalActuatorDelay + model.LONG_SMOOTH_SECONDS
@@ -466,6 +467,7 @@ def main(demo=False):
       model.PLANPLUS_CONTROL = params.get("PlanplusControl", return_default=True)
       camera_offset_helper.set_offset(params.get("CameraOffset", return_default=True))
     lat_delay = model.lat_delay + model.LAT_SMOOTH_SECONDS
+    lat_delay += get_ford_delay_offset(CP, ford_model_action, v_ego)
     if sm.updated["extrinsicsCalibration"] and sm.seen['narrowRoadCameraState'] and sm.seen['deviceState']:
       device_from_calib_euler = np.array(sm["extrinsicsCalibration"].rpyCalib, dtype=np.float32)
       dc = DEVICE_CAMERAS[(str(sm['deviceState'].deviceType), str(sm['narrowRoadCameraState'].sensor))]