Ford: add opt-in bounded assistance for large turn entries

This commit is contained in:
Isaac Barham
2026-09-21 08:09:52 -04:00
parent d203a109a7
commit 07affcf934
7 changed files with 309 additions and 6 deletions
+106
View File
@@ -0,0 +1,106 @@
# Ford large-turn entry assist
The coordinated encoder can select the same C0/C1 packet even when its angle
target increases: larger fields sometimes have the same predicted next state,
and its tie break favors the smaller fields. This does not prove that the live
PSCM responds identically. This opt-in trial adds a bounded command after that
selection, making the turn-entry experiment observable at the CAN output.
## Enable and revert
In sunnylink Ford settings, keep **Selected-Action Path Tracking** and
**Coordinated C0/C1 Steering** enabled, and **Model Geometry Reference** disabled.
Enable **Large-Turn Entry Assist (Experimental)**, then make an offroad-to-onroad
transition. The setting is default off and read once at startup. Disabling only
the new setting and cycling offroad/onroad restores the previous coordinated
controller. Disabling Selected-Action Path Tracking restores upstream Ford
control. No hot switching or added steering-loop parameter I/O.
## Exactly what changes
The original desired steering angle, inverse, target-trend preview and centering
trim remain. An additional command is weighted by three continuous ramps:
- Requested angle magnitude: zero through 30 degrees, full at 60 degrees.
- Wheel lag into that turn: zero through 25 degrees, full at 50 degrees.
- Growing filtered target rate: zero at steady/relaxing target, full at
30 degrees/second.
Multiply the three weights. At full weight, add **0.60 m C0 and 0.04 rad C1**
in the requested turn direction, then quantize and clip to the existing DBC
bounds. These are trial tuning values, not recovered Ford constants or a
guaranteed conversion to wheel angle. The 4× label refers to the earlier offline
starting budget of 0.15 m / 0.01 rad, not a multiplication of all steering.
Only the extra term is suppressed on steeringPressed, fresh PSCM limit >=2,
or a target already limited by the existing inverse acceleration allowance.
Existing disengagement, fault and explicit override gates still govern the
whole command. C2/C3 stay zero. No new integrator, retained maneuver state or
encoder search is added. The observer continues consuming actual packed commands.
The extra goes to zero when the filtered target stops growing. **This does not
clear the consequences of earlier commands:** the observer and encoder retain
their state, so later requests can change. Normal path following and physical
wheel release are not guaranteed unchanged. Delayed unwind is an accepted trial
tradeoff, not claimed fixed.
## Why this strength
Nine recorded routes were compared: 183, 185, 17c, 166, 16a, 172, 177, 175 and
17a. Each run used 556,776 input rows, actual adapter/packer calls, frozen model
and vehicle measurements, and full available histories. Compare against the
unchanged coordinated controller at `d203a109a`.
| Extra budget | Weak right 2: max total C0/C1 difference | Changed ordinary samples |
|---|---|---|
| 0.15 m / 0.01 rad | 0.26 m / 0.0185 rad | 2,923 / 236,385 (1.24%) |
| 0.30 m / 0.02 rad | 0.41 m / 0.0285 rad | 2,071 / 236,385 (0.88%) |
| **0.60 m / 0.04 rad** | **0.71 m / 0.0485 rad** | **1,493 / 236,385 (0.63%)** |
Total differences can exceed the immediate additive budget because earlier
commands alter subsequent encoder decisions. The ordinary screen requires
requests within ±30 degrees, engaged control, no touch/limitReached, and two
seconds of continuously qualifying data on each side. It measures command
differences, not lane error. Lower counts are not proof of better centering.
The 4× trial changes 76/334 and 104/326 samples in the two weak-right windows.
Their peak absolute fields stay below 3.10 m C0 and 0.225 rad C1. Roundabout
windows reach maximum differences of 0.63–0.66 m / 0.0425–0.0445 rad.
Nine of ten marked wobble windows match exactly. In route172's bookmark, 98/396
samples change, up to 0.21 m C0 / 0.007 rad C1. Command total variation is slightly
lower (C0 5.95→5.85 m; C1 0.3605→0.3545 rad); maximum individual steps are unchanged.
This does not prove that the real wobble improves or remains equal.
Across all nine routes the trial touches a DBC field bound on 29 samples versus
12 for baseline, including a short interval on175. It never exceeds those
bounds. It changes the marked good unwind's channel split substantially:
maximum C0/C1 differences of 2.16 m / 0.058 rad. That is not a measured wheel
release delay. Larger requests reaching CAN do not establish more wheel motion.
## Verification and interpretation
Tests exercise the real Ford adapter and packer: startup/fallback, default-off
parameter, both turn signs, no extra for small requests/errors, DBC clipping,
target limits, overrides, fault gates, and clearing the added term while retaining
the observer's real history. Full-route production verification compares both
settings directly against the frozen 4× research output, including state and
transmitted commands. UI source and generated schema are checked together.
The deployment gate additionally suppresses the term when the inverse target
is acceleration-limited; no active extra in these nine research tapes coincided
with that condition. This does not introduce a new acceleration allowance.
`Ford joint path tracking` diagnostics identify the enabled experiment as
`ford-joint-turn-entry-v1`, with `turn_entry_enabled`, `turn_entry_weight`,
`turn_entry_c0`, `turn_entry_c1`, `base_wire_command` and actual `wire_sent`.
The existing `predicted_curvature` field describes the encoder prediction before
the added term; `filtered_curvature` is the observer advanced from actual sends.
Logging cadence is unchanged.
These are offline numerical checks, not a closed-loop steering validation.
166/16a retain raw-yaw sensitivity; 183 has incomplete earlier history; 185 has
log gaps. The recovered older firmware model is not proven identical to the
live truck. The physical question for this trial is whether the stronger entry
requests move the wheel farther/faster without unacceptable normal-driving
changes. Neither turn completion nor preserved driving feel is established.
+1
View File
@@ -242,6 +242,7 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
{"FordC0TimeBased", {PERSISTENT | BACKUP, BOOL, "0"}},
{"FordGeometryReference", {PERSISTENT | BACKUP, BOOL, "0"}},
{"FordPscmJointControl", {PERSISTENT | BACKUP, BOOL, "0"}},
{"FordPscmTurnEntryAssist", {PERSISTENT | BACKUP, BOOL, "0"}},
{"HyundaiLongitudinalTuning", {PERSISTENT | BACKUP, INT, "0"}},
{"SubaruStopAndGo", {PERSISTENT | BACKUP, BOOL, "0"}},
{"SubaruStopAndGoManualParkingBrake", {PERSISTENT | BACKUP, BOOL, "0"}},
+36 -6
View File
@@ -9,12 +9,23 @@ import math
from opendbc.car.ford.values import FordFlags
# Small steering-wheel trim around the nominal inverse, in degrees. These are
# trial tuning values, not recovered PSCM constants. No extra proportional loop.
# trial tuning values, not recovered PSCM constants. Entry assist is separately gated.
ANGLE_TRIM_MAX = 2.0
ANGLE_TRIM_KI = 0.2 # 1/s; error clipping limits adaptation to 0.4 deg/s.
ANGLE_TRIM_ERROR_MAX = 5.0
ANGLE_TRIM_RATE_MAX = 5.0 # deg/s: do not learn a bias from fast wheel motion.
TARGET_PREVIEW = 0.1 # seconds; causal action-trend forecast, then hold.
TURN_ENTRY_C0_MAX = 0.60 # metres; experimental post-encoder correction budget.
TURN_ENTRY_C1_MAX = 0.04 # radians; not a recovered Ford calibration value.
def turn_entry_weight(target, angle, requested_rate):
"""Fade in only for a large, growing turn request with substantial wheel lag."""
direction = math.copysign(1.0, target)
size = max(0.0, min(1.0, (abs(target) - 30.0) / 30.0))
lag = max(0.0, min(1.0, (direction * (target - angle) - 25.0) / 25.0))
growth = max(0.0, min(1.0, direction * requested_rate / 30.0))
return size * lag * growth
def joint_control_enabled(CP, params):
@@ -30,17 +41,18 @@ def joint_control_enabled(CP, params):
def select_joint_control(CP, params):
# No calibration or native-library load on the normal/default path.
return FordJointControl(CP) if joint_control_enabled(CP, params) else None
return FordJointControl(CP, turn_entry_assist=params.get_bool('FordPscmTurnEntryAssist')) if joint_control_enabled(CP, params) else None
class FordJointControl:
def __init__(self, CP):
def __init__(self, CP, *, turn_entry_assist=False):
from opendbc.can import CANParser
from opendbc.car.ford.fordcan import CanBus
from openpilot.selfdrive.controls.lib.ford_joint.model import MainRequest
from openpilot.selfdrive.controls.lib.ford_joint.angle import AngleModel
from openpilot.selfdrive.controls.lib.ford_joint.encoder import PairedRelease
self.turn_entry_assist = turn_entry_assist
self.wheelbase, self.ratio = CP.wheelbase, CP.steerRatio
if not all(math.isfinite(v) and v > 0 for v in (self.wheelbase, self.ratio)):
raise ValueError('Joint control requires finite positive vehicle geometry')
@@ -80,7 +92,7 @@ class FordJointControl:
self.last_time = now
def prepare(self, CC, CC_SP, CS, now, *, fresh=True, pscm_status=None):
from openpilot.selfdrive.controls.lib.ford_joint.inverse import C0_BOUND, C1_BOUND, invert_angle
from openpilot.selfdrive.controls.lib.ford_joint.inverse import C0_BOUND, C1_BOUND, invert_angle, quantize
dt = now - self.last_time if self.last_time is not None else 0.0
self.advance(now)
@@ -150,7 +162,21 @@ class FordJointControl:
curvature_rate=self.requested_rate / inverse['slope_per_curvature'], preview=TARGET_PREVIEW,
curvature_bound=inverse['allowance'] / (speed / 3.6)**2,
)
base_command = command
entry_weight = 0.0
if self.turn_entry_assist and not CS.steeringPressed and not inverse['accel_limited'] and not (status_fresh and pscm_status.limit >= 2):
entry_weight = turn_entry_weight(target, angle, self.requested_rate)
if entry_weight:
extra = math.copysign(entry_weight, target)
command = quantize((command[0] + extra * TURN_ENTRY_C0_MAX, command[1] + extra * TURN_ENTRY_C1_MAX))
# Apply after selection so nominal slew equivalence cannot discard the
# added request. record_sent/advance still observe the actual packets;
# their retained state can affect later commands after this term clears.
details = {
'base_wire_command': tuple(map(float, base_command)),
'turn_entry_weight': entry_weight,
'turn_entry_c0': float(command[0] - base_command[0]),
'turn_entry_c1': float(command[1] - base_command[1]),
'requested_angle': target,
'trimmed_angle': trimmed_target,
'angle_trim': self.angle_trim,
@@ -161,7 +187,7 @@ class FordJointControl:
'target_preview_limited': info['preview_limited'],
'reachable_angle': inverse['reachable_target'],
'target_curvature': inverse['curvature'],
'predicted_curvature': float(info['first_state'][3]),
'predicted_curvature': float(info['first_state'][3]), # Encoder prediction before optional entry assist.
'accel_limited': inverse['accel_limited'],
}
# Learn only small residual errors while the wheel is settling. Large
@@ -189,7 +215,11 @@ class FordJointControl:
cc = CC.as_reader().as_builder() if hasattr(CC, 'as_reader') else CC.as_builder()
cc.latActive = active
self.diagnostics = {
'hypothesis': 'ford-joint-v24',
'hypothesis': 'ford-joint-turn-entry-v1' if self.turn_entry_assist else 'ford-joint-v24',
'turn_entry_enabled': self.turn_entry_assist,
'turn_entry_weight': 0.0,
'turn_entry_c0': 0.0,
'turn_entry_c1': 0.0,
'status': self.fault or ('active' if active else 'inactive'),
'driver_override': override,
'driver_pressed': bool(CS.steeringPressed),
@@ -0,0 +1,110 @@
"""Opt-in entry assist at the real Ford adapter and CAN packing boundary."""
from types import SimpleNamespace
import numpy as np
import pytest
from openpilot.common.params import Params, ParamKeyFlag
from openpilot.selfdrive.car.ford_joint_control import FordJointControl, select_joint_control
from openpilot.selfdrive.car.tests.test_ford_joint_control import Pipeline, cp
from openpilot.selfdrive.controls.lib.ford_joint.encoder import state
def assisted():
p = Pipeline()
p.joint = FordJointControl(cp(), turn_entry_assist=True)
return p
def test_turn_entry_is_default_off_and_requires_the_existing_joint_controller(tmp_path):
params = Params(str(tmp_path))
assert params.get_default_value('FordPscmTurnEntryAssist') is False
assert not params.get_bool('FordPscmTurnEntryAssist')
assert b'FordPscmTurnEntryAssist' in params.all_keys(ParamKeyFlag.PERSISTENT | ParamKeyFlag.BACKUP)
params.put_bool('FordPscmTurnEntryAssist', True, block=True)
assert select_joint_control(cp(), params) is None
params.put_bool('FordModelActionController', True, block=True)
assert select_joint_control(cp(), params) is None
params.put_bool('FordPscmJointControl', True, block=True)
assert select_joint_control(cp(), params).turn_entry_assist
params.put_bool('FordPscmTurnEntryAssist', False, block=True)
assert not select_joint_control(cp(), params).turn_entry_assist
params.put_bool('FordPscmTurnEntryAssist', True, block=True)
for key in ('FordGeometryReference', 'JoystickDebugMode'):
params.put_bool(key, True, block=True)
assert select_joint_control(cp(), params) is None
params.put_bool(key, False, block=True)
@pytest.mark.parametrize('sign', [-1., 1.])
def test_large_growing_undertracked_request_adds_to_real_wire_commands(sign):
base, trial = Pipeline(), assisted()
for p in (base, trial):
p.tick(1., 0.)
p.tick(1.01, sign * 120.)
np.testing.assert_allclose(np.array(trial.joint.sent[:2]) - base.joint.sent[:2], sign * np.array([.60, .04]), atol=1e-12)
assert trial.joint.diagnostics['requested_angle'] == sign * 120.
assert trial.joint.diagnostics['trimmed_angle'] == base.joint.diagnostics['trimmed_angle']
assert trial.joint.diagnostics['requested_rate'] == base.joint.diagnostics['requested_rate']
assert trial.joint.diagnostics['turn_entry_weight'] == 1.
assert trial.joint.sent[2] and base.joint.sent[2]
@pytest.mark.parametrize('sign', [-1., 1.])
def test_extra_clips_at_dbc_bounds_through_real_packer(sign, monkeypatch):
p = assisted()
p.tick(1., 0.)
choose = p.joint.encoder.choose
def near_bounds(*args, **kwargs):
_, info = choose(*args, **kwargs)
return (sign * 5.10, sign * .49), info
monkeypatch.setattr(p.joint.encoder, 'choose', near_bounds)
p.tick(1.01, sign * 120.)
assert p.joint.sent[:2] == pytest.approx((sign * 5.11, sign * .5))
def test_small_requests_and_small_errors_keep_full_history_identical():
base, trial = Pipeline(), assisted()
for i in range(1200):
target = 25 * np.sin(i * .02) if i < 600 else 100.
wheel = 0. if i < 600 else 90.
for p in (base, trial):
p.cs.steeringAngleDeg = wheel
p.tick(1. + i * .01, float(target))
assert trial.joint.sent == base.joint.sent
np.testing.assert_array_equal(state(trial.joint.request), state(base.joint.request))
assert trial.joint.angle_trim == base.joint.angle_trim
@pytest.mark.parametrize('sign', [-1., 1.])
def test_new_term_clears_on_relaxation_without_resetting_observed_state(sign):
p = assisted()
for i in range(100):
p.cs.steeringAngleDeg = sign * 20.
p.tick(1. + i * .01, sign * (100. + i))
assert p.joint.diagnostics['turn_entry_c0'] * sign > 0
p.tick(2., sign * 50.)
assert p.joint.diagnostics['turn_entry_c0'] == p.joint.diagnostics['turn_entry_c1'] == 0.
assert abs(p.joint.request.c0) + abs(p.joint.request.c1) > 0
assert p.joint.sent[2]
@pytest.mark.parametrize('gate', ['touch', 'limit', 'denied', 'fault', 'stale', 'inactive', 'accel'])
def test_extra_respects_intervention_health_and_target_limit_gates(gate):
p = assisted()
p.cs.vEgo = 30. if gate == 'accel' else 5.36
p.tick(1., 0.)
p.cs.steeringPressed = gate == 'touch'
p.cs.steerFaultTemporary = gate == 'fault'
status = SimpleNamespace(valid=True, canMonoTime=1_010_000_000, limit=2 if gate == 'limit' else 0, denied=gate == 'denied')
p.tick(1.01, 120., active=gate != 'inactive', fresh=gate != 'stale', pscm_status=status)
d = p.joint.diagnostics
assert d['turn_entry_c0'] == d['turn_entry_c1'] == 0.
if gate in ('touch', 'limit', 'accel'):
assert p.joint.sent[2] # Only the added term is suppressed.
else:
assert p.joint.sent == (0., 0., False)
if gate == 'accel':
assert d['accel_limited']
@@ -2231,6 +2231,34 @@
"equals": false
}
]
},
{
"key": "FordPscmTurnEntryAssist",
"widget": "toggle",
"needs_onroad_cycle": true,
"title": "Large-Turn Entry Assist (Experimental)",
"description": "Add bounded steering correction when a large turn request is growing and the wheel is behind.",
"details": "Requires Coordinated C0/C1 Steering and Selected-Action Path Tracking on, with Model Geometry Reference off. Default off. Leaves the normal steering target and centering logic in place. This trial may increase turn authority but can affect later centering and hold a turn longer; physical improvement has not been established. Turning it off returns to the existing coordinated controller after an offroad-to-onroad cycle. Turning Selected-Action Path Tracking off restores upstream Ford control.",
"enablement": [
{
"type": "offroad_only"
},
{
"type": "param",
"key": "FordModelActionController",
"equals": true
},
{
"type": "param",
"key": "FordPscmJointControl",
"equals": true
},
{
"type": "param",
"key": "FordGeometryReference",
"equals": false
}
]
}
]
},
@@ -43,6 +43,23 @@ sections:
- type: param
key: FordGeometryReference
equals: false
- key: FordPscmTurnEntryAssist
widget: toggle
needs_onroad_cycle: true
title: Large-Turn Entry Assist (Experimental)
description: Add bounded steering correction when a large turn request is growing and the wheel is behind.
details: Requires Coordinated C0/C1 Steering and Selected-Action Path Tracking on, with Model Geometry Reference off. Default off. Leaves the normal steering target and centering logic in place. This trial may increase turn authority but can affect later centering and hold a turn longer; physical improvement has not been established. Turning it off returns to the existing coordinated controller after an offroad-to-onroad cycle. Turning Selected-Action Path Tracking off restores upstream Ford control.
enablement:
- $ref: '#/macros/offroad'
- type: param
key: FordModelActionController
equals: true
- type: param
key: FordPscmJointControl
equals: true
- type: param
key: FordGeometryReference
equals: false
- id: hyundai
title: Hyundai / Kia / Genesis Settings
description: ''
@@ -317,6 +317,17 @@ class TestKnownVehicleSettings(OpenpilotTestCase):
with tempfile.TemporaryDirectory() as path:
assert Params(path).get_default_value("FordPscmJointControl") is False
def test_ford_turn_entry_is_separate_default_off_cycle_only_trial(self, schema):
items = _brand_items(schema["vehicle_settings"].get("ford"))
item = next(item for item in items if item["key"] == "FordPscmTurnEntryAssist")
assert item["widget"] == "toggle" and item["needs_onroad_cycle"] is True
assert item["enablement"] == [{"type": "offroad_only"},
{"type": "param", "key": "FordModelActionController", "equals": True},
{"type": "param", "key": "FordPscmJointControl", "equals": True},
{"type": "param", "key": "FordGeometryReference", "equals": False}]
with tempfile.TemporaryDirectory() as path:
assert Params(path).get_default_value("FordPscmTurnEntryAssist") is False
def test_hyundai_has_longitudinal_tuning(self, schema):
keys = {i["key"] for i in _brand_items(schema["vehicle_settings"].get("hyundai"))}
assert "HyundaiLongitudinalTuning" in keys