Ford Steering Settings

2026-11-03 11:09:55 +08:00 · 2026-10-04 04:18:28 -05:00
parent e244fd106e
commit 661aa4d108
7 changed files with 207 additions and 0 deletions
@@ -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
@@ -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
@@ -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
@@ -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()
@@ -23,6 +23,9 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> 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}},
@@ -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
@@ -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",