From b720e9f1bb805c69e5560f13dd86034d4bf50260 Mon Sep 17 00:00:00 2001 From: Isaac Barham Date: Mon, 14 Sep 2026 23:29:37 -0400 Subject: [PATCH] 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 | 4 +- openpilot/sunnypilot/livedelay/helpers.py | 13 +++++ .../sunnypilot/livedelay/test_helpers.py | 55 +++++++++++++++++++ openpilot/sunnypilot/modeld_v2/modeld.py | 4 +- 4 files changed, 74 insertions(+), 2 deletions(-) create mode 100644 openpilot/sunnypilot/livedelay/test_helpers.py diff --git a/openpilot/selfdrive/modeld/modeld.py b/openpilot/selfdrive/modeld/modeld.py index 7e7fa7f583..7a80e1677e 100755 --- a/openpilot/selfdrive/modeld/modeld.py +++ b/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))] diff --git a/openpilot/sunnypilot/livedelay/helpers.py b/openpilot/sunnypilot/livedelay/helpers.py index 7b36b57af1..9e7c924fc8 100644 --- a/openpilot/sunnypilot/livedelay/helpers.py +++ b/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.)) diff --git a/openpilot/sunnypilot/livedelay/test_helpers.py b/openpilot/sunnypilot/livedelay/test_helpers.py new file mode 100644 index 0000000000..4d40450556 --- /dev/null +++ b/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 diff --git a/openpilot/sunnypilot/modeld_v2/modeld.py b/openpilot/sunnypilot/modeld_v2/modeld.py index c80249c571..db7bb01a4d 100755 --- a/openpilot/sunnypilot/modeld_v2/modeld.py +++ b/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))]