From 661aa4d108703c065d56b52e809e901443a58bff Mon Sep 17 00:00:00 2001 From: firestar5683 <168790843+firestar5683@users.noreply.github.com> Date: Sun, 4 Oct 2026 04:18:28 -0500 Subject: [PATCH] Ford Steering Settings --- .../opendbc/car/ford/carcontroller.py | 1 + .../opendbc/car/ford/lateral_strategy.py | 5 ++ .../car/ford/tests/test_blend_settings.py | 80 +++++++++++++++++ .../ford/tests/test_blend_settings_startup.py | 87 +++++++++++++++++++ openpilot/common/params_keys.h | 3 + openpilot/starpilot/controller_extensions.py | 27 ++++++ tools/ci/run_vehicle_tests.py | 4 + 7 files changed, 207 insertions(+) create mode 100644 opendbc_repo/opendbc/car/ford/tests/test_blend_settings.py create mode 100644 opendbc_repo/opendbc/car/ford/tests/test_blend_settings_startup.py diff --git a/opendbc_repo/opendbc/car/ford/carcontroller.py b/opendbc_repo/opendbc/car/ford/carcontroller.py index 0e1737bc4a..2bd6de47cc 100644 --- a/opendbc_repo/opendbc/car/ford/carcontroller.py +++ b/opendbc_repo/opendbc/car/ford/carcontroller.py @@ -78,6 +78,7 @@ class CarController(CarControllerBase): can_sends = [] if (self.mache_lateral is not None or self.classic_lateral is not None) and self.manual_turn_inputs is not None: self.manual_turn_inputs.update() + self.manual_turn_inputs.apply_blend_settings(self) actuators = CC.actuators hud_control = CC.hudControl diff --git a/opendbc_repo/opendbc/car/ford/lateral_strategy.py b/opendbc_repo/opendbc/car/ford/lateral_strategy.py index 140531906f..ca47227c7e 100644 --- a/opendbc_repo/opendbc/car/ford/lateral_strategy.py +++ b/opendbc_repo/opendbc/car/ford/lateral_strategy.py @@ -171,6 +171,11 @@ class FordLateralController: self.path_angle_driver_cooldown = 0.0 self.desired_curvature_last = 0.0 + def set_blend_settings(self, low: float, high: float, lane_change: float): + self.curvature_blend_low = float(np.clip(low if np.isfinite(low) else 0.4, 0.0, 1.0)) + self.curvature_blend_high = float(np.clip(high if np.isfinite(high) else 0.4, 0.0, 1.0)) + self.curvature_lane_change_factor = float(np.clip(lane_change if np.isfinite(lane_change) else 0.85, 0.5, 1.25)) + def set_inputs(self, model, time_indices, lookahead: float, human_turn_enabled: bool): self.model = model self.time_indices = time_indices diff --git a/opendbc_repo/opendbc/car/ford/tests/test_blend_settings.py b/opendbc_repo/opendbc/car/ford/tests/test_blend_settings.py new file mode 100644 index 0000000000..1ee28572c3 --- /dev/null +++ b/opendbc_repo/opendbc/car/ford/tests/test_blend_settings.py @@ -0,0 +1,80 @@ +from pathlib import Path +from types import SimpleNamespace + +import pytest + +from opendbc.car.ford.lateral_strategy import FordLateralController +from opendbc.car.ford.values import CAR + + +@pytest.mark.parametrize("values,expected", ((None, (0.4, 0.4, 0.85)), + ((0.25, 0.65, 1.1), (0.25, 0.65, 1.1)), + ((-1.0, 2.0, 4.0), (0.0, 1.0, 1.25)), + ((2.0, -1.0, -4.0), (1.0, 0.0, 0.5)))) +def test_typed_saved_blends_reach_owner_without_model(values, expected, monkeypatch): + from openpilot.common.params import Params + from openpilot.common.prefix import OpenpilotPrefix + from openpilot.starpilot import controller_extensions + + model = SimpleNamespace(meta=SimpleNamespace(laneChangeState=0)) + sm = SimpleNamespace(update=lambda timeout: None, alive={"modelV2": False}, valid={"modelV2": False}) + + class Transport: + alive = sm.alive + valid = sm.valid + + def update(self, timeout): + return None + + def __getitem__(self, key): + return model + monkeypatch.setattr(controller_extensions.messaging, "SubMaster", lambda names: Transport()) + with OpenpilotPrefix(): + saved = Params() + keys = ("FordCurvatureBlendLow", "FordCurvatureBlendHigh", "FordCurvatureLaneChangeFactor") + if values is not None: + for key, value in zip(keys, values, strict=True): + saved.put(key, value, block=True) + inputs = controller_extensions.ManualTurnInputs(saved) + owner = FordLateralController(SimpleNamespace(carFingerprint=CAR.FORD_RANGER_MK2)) + calls = [] + setter = owner.set_blend_settings + + def counted_setter(*values): + calls.append(values) + setter(*values) + monkeypatch.setattr(owner, "set_blend_settings", counted_setter) + controller = SimpleNamespace(classic_lateral=owner) + inputs.apply_blend_settings(controller) + assert (owner.curvature_blend_low, owner.curvature_blend_high, owner.curvature_lane_change_factor) == expected + inputs.update() + inputs.apply_blend_settings(controller) + assert inputs.blend_settings == expected + assert len(calls) == 1 + for key, raw in zip(keys, (b"nan", b"not-a-float", b"inf"), strict=True): + Path(saved.get_param_path(key)).write_bytes(raw) + for _ in range(99): + inputs.update() + assert inputs.blend_settings == expected + inputs.update() + inputs.apply_blend_settings(controller) + assert inputs.blend_settings == (0.4, 0.4, 0.85) + assert len(calls) == (1 if expected == (0.4, 0.4, 0.85) else 2) + assert (owner.curvature_blend_low, owner.curvature_blend_high, owner.curvature_lane_change_factor) == (0.4, 0.4, 0.85) + + +@pytest.mark.parametrize("desired,predicted,direction", ((-0.0005, -0.002, 1), (0.0005, 0.002, 2), + (-0.001, -0.003, 2), (0.001, 0.003, 1))) +def test_saved_blends_preserve_source_units_and_lane_direction(desired, predicted, direction): + owner = FordLateralController(SimpleNamespace(carFingerprint=CAR.FORD_RANGER_MK2)) + owner.set_blend_settings(0.25, 0.65, 1.1) + owner.model = SimpleNamespace(meta=SimpleNamespace(laneChangeState=2, laneChangeDirection=direction)) + blend = 0.25 + min(abs(desired) / 0.001, 1.0) * 0.4 + expected = predicted * blend + desired * (1.0 - blend) + precision = 1 + if (direction == 1 and expected < 0.0) or (direction == 2 and expected > 0.0): + expected *= 0.95 + (20.0 - 4.4) / (40.23 - 4.4) * 0.15 + precision = 0 + result, actual_precision = owner._blend_and_scale(desired, predicted, v_ego=20.0, current=0.0) + assert result == pytest.approx(expected) + assert actual_precision == precision diff --git a/opendbc_repo/opendbc/car/ford/tests/test_blend_settings_startup.py b/opendbc_repo/opendbc/car/ford/tests/test_blend_settings_startup.py new file mode 100644 index 0000000000..8f4f72aeb1 --- /dev/null +++ b/opendbc_repo/opendbc/car/ford/tests/test_blend_settings_startup.py @@ -0,0 +1,87 @@ +import gc +import time + +import pytest + +from opendbc.car import structs +from opendbc.car.ford.generic_canfd_lateral import GENERIC_CANFD_CARS +from opendbc.car.ford.tests.test_three_ports import params + + +@pytest.mark.parametrize("identity", sorted(GENERIC_CANFD_CARS)) +def test_generic_actual_card_publication_reaches_blend_settings(identity, monkeypatch): + alpha = release = disable = False + from opendbc.car import car_helpers, gen_empty_fingerprint + from opendbc.car.ford.generic_canfd_lateral import qualified as generic_qualified + from openpilot.selfdrive.car import card as card_module + from openpilot.cereal import messaging + from openpilot.common.params import Params + from openpilot.common.prefix import OpenpilotPrefix + from openpilot.selfdrive.car.card import Car, alpha_long_requested + from openpilot.selfdrive.controls.controlsd import Controls + from openpilot.starpilot.controller_extensions import ManualTurnInputs + from openpilot.starpilot.lateral.tests.test_lane_runtime import feed + + monkeypatch.setenv('SIMULATION', '1') + monkeypatch.setenv('REPLAY', '1') + with OpenpilotPrefix(): + saved = Params() + saved.put_bool('OpenpilotEnabledToggle', True, block=True) + saved.put_bool('AlphaLongitudinalEnabled', alpha, block=True) + saved.put_bool('IsReleaseBranch', release, block=True) + saved.put_bool('DisableOpenpilotLongitudinal', disable, block=True) + saved.put_bool('FordHumanTurnDetection', False, block=True) + for key, value in (("FordCurvatureBlendLow", 0.25), ("FordCurvatureBlendHigh", 0.65), ("FordCurvatureLaneChangeFactor", 1.1)): + saved.put(key, value, block=True) + cp = params(identity, alpha_long_requested(saved, is_release=release), release) + cp.carVin = '0' * 17 + cp.carFw = [] + expected = cp.to_dict() + fingerprint = gen_empty_fingerprint() + fingerprint[0][0x5A] = 8 + fingerprint[2][0x3D6] = 8 + fingerprint[2][0x186] = 8 + observed = (identity, fingerprint, cp.carVin, [], cp.fingerprintSource, True) + subscriber = messaging.sub_sock('carParams', timeout=100, conflate=True) + ci = card = controls = None + try: + with monkeypatch.context() as discovery: + discovery.setattr(car_helpers, 'fingerprint', lambda *args: observed) + discovery.setattr(card_module, 'can_comm_callbacks', lambda *args: (lambda *args: [], lambda frames: None)) + initial_can = messaging.new_message('can', 1) + discovery.setattr(card_module.messaging, 'recv_one_retry', lambda socket: initial_can) + card = Car() + ci = card.CI + assert card.CP.to_dict() == expected + assert generic_qualified(card.CP) + owner = ci.CC.classic_lateral + assert owner is not None + assert (owner.curvature_blend_low, owner.curvature_blend_high, owner.curvature_lane_change_factor) == (0.25, 0.65, 1.1) + assert isinstance(ci.CC.manual_turn_inputs, ManualTurnInputs) + assert not ci.CC.manual_turn_inputs.enabled + assert not ci.CC.mache_extended_announced and not ci.CC.classic_extended_announced + with structs.CarParams.from_bytes(saved.get('CarParams')) as published: + assert published.to_dict() == expected + event = None + for _ in range(10): + card.car_params_published = False + card.state_publish(ci.update([]), None) + event = messaging.recv_one(subscriber) + if event is not None: + break + time.sleep(.01) + assert event is not None and event.valid and event.carParams.to_dict() == expected + controls = Controls() + assert controls.CP.to_dict() == expected + feed(controls, 1_000_000_000, 0, active=False, enabled=False, can_valid=False, can_timeout=True) + command, lateral_log = controls.state_control() + assert not command.enabled and not command.latActive and not command.longActive + controls.publish(command, lateral_log) + _, packets = ci.apply(command.as_reader(), 1_000_000_000) + assert packets + assert ci.CC.classic_extended_announced + assert not owner.human_turn_enabled + assert (owner.curvature_blend_low, owner.curvature_blend_high, owner.curvature_lane_change_factor) == (0.25, 0.65, 1.1) + finally: + del controls, card, ci, subscriber + gc.collect() diff --git a/openpilot/common/params_keys.h b/openpilot/common/params_keys.h index 0033f7b621..6b607aa905 100644 --- a/openpilot/common/params_keys.h +++ b/openpilot/common/params_keys.h @@ -23,6 +23,9 @@ inline static std::unordered_map keys = { {"NostalgiaMode", {PERSISTENT, BOOL, "0"}}, {"ToyotaAutoHold", {PERSISTENT, BOOL, "0"}}, {"FordHumanTurnDetection", {PERSISTENT, BOOL, "1"}}, + {"FordCurvatureBlendLow", {PERSISTENT, FLOAT, "0.4"}}, + {"FordCurvatureBlendHigh", {PERSISTENT, FLOAT, "0.4"}}, + {"FordCurvatureLaneChangeFactor", {PERSISTENT, FLOAT, "0.85"}}, {"OnroadCustomizations", {PERSISTENT | DONT_LOG, JSON}}, {"StarPilotFavoriteSlots", {PERSISTENT, JSON, R"([{"enabled":false,"show_onroad":false,"key":null,"label":""},{"enabled":false,"show_onroad":false,"key":null,"label":""},{"enabled":false,"show_onroad":false,"key":null,"label":""}])"}}, {"StarPilotControllerBindings", {PERSISTENT | DONT_LOG, JSON}}, diff --git a/openpilot/starpilot/controller_extensions.py b/openpilot/starpilot/controller_extensions.py index 4f0517e9a5..a6c4e481b6 100644 --- a/openpilot/starpilot/controller_extensions.py +++ b/openpilot/starpilot/controller_extensions.py @@ -18,11 +18,35 @@ class ManualTurnInputs: self.sm = messaging.SubMaster(["modelV2", "lateralDelay"]) self.enabled = bool(self.params.get("FordHumanTurnDetection", return_default=True)) self.frames = 0 + self.blend_settings = self._read_blend_settings() + self._applied_blends = {} + + def _read_blend_settings(self): + settings = [] + for key, default, low, high in (("FordCurvatureBlendLow", 0.4, 0.0, 1.0), + ("FordCurvatureBlendHigh", 0.4, 0.0, 1.0), + ("FordCurvatureLaneChangeFactor", 0.85, 0.5, 1.25)): + try: + value = float(self.params.get(key, return_default=True)) + if not math.isfinite(value): + value = default + except (TypeError, ValueError, OverflowError, RuntimeError): + value = default + settings.append(float(np.clip(value, low, high))) + return tuple(settings) + + def apply_blend_settings(self, controller): + for name in ("mache_lateral", "classic_lateral"): + owner = getattr(controller, name, None) + if owner is not None and self._applied_blends.get(owner) != self.blend_settings: + owner.set_blend_settings(*self.blend_settings) + self._applied_blends[owner] = self.blend_settings def update(self): self.sm.update(0) if self.frames % 100 == 0: self.enabled = bool(self.params.get("FordHumanTurnDetection", return_default=True)) + self.blend_settings = self._read_blend_settings() self.frames += 1 state = self.sm["modelV2"].meta.laneChangeState model_ready = self.sm.alive["modelV2"] and self.sm.valid["modelV2"] @@ -79,6 +103,9 @@ def configure_controller(CI, params): if controller is not None and (classic_qualified(cp) or generic_canfd_qualified(cp)) and getattr(controller, "classic_lateral", None) is not None: controller.manual_turn_inputs = ManualTurnInputs(params) + if controller is not None and cp.brand == "ford" and getattr(controller, "manual_turn_inputs", None) is not None: + controller.manual_turn_inputs.apply_blend_settings(controller) + from opendbc.car.hyundai.g90_lead import eligible as g90_lead_eligible if controller is not None and g90_lead_eligible(cp): from openpilot.starpilot.longitudinal.g90_lead import G90LeadInputs diff --git a/tools/ci/run_vehicle_tests.py b/tools/ci/run_vehicle_tests.py index c164e8c4db..5d28d6f691 100644 --- a/tools/ci/run_vehicle_tests.py +++ b/tools/ci/run_vehicle_tests.py @@ -21,6 +21,10 @@ sys.path.insert(0, str(ROOT)) from tools import test_runner RELEASE_TARGETS = ( + "opendbc_repo/opendbc/car/ford/tests/test_added_firmware_identity.py", + "opendbc_repo/opendbc/car/ford/tests/test_required_source_health.py", + "opendbc_repo/opendbc/car/ford/tests/test_blend_settings.py", + "opendbc_repo/opendbc/car/ford/tests/test_blend_settings_startup.py", "opendbc_repo/opendbc/car/ford/tests/test_production_extensions.py", "opendbc_repo/opendbc/car/ford/tests/test_generic_canfd.py", "opendbc_repo/opendbc/safety/tests/test_ford_generic_canfd_extended.py",