From b30793bfb1d70b552f7ca7b8f09c867f7318b79c Mon Sep 17 00:00:00 2001 From: Isaac Barham Date: Mon, 21 Sep 2026 22:55:37 -0400 Subject: [PATCH] Ford: coordinate endpoint buildup and retain stop requests through speed undershoot --- docs/ford_joint_equal_arrival_trial.md | 39 +++++++++++++++ openpilot/selfdrive/car/ford_joint_control.py | 11 +++-- .../car/tests/test_ford_joint_allocation.py | 49 +++++++++++++++++++ .../car/tests/test_ford_joint_control.py | 34 +++++++++++-- openpilot/selfdrive/controls/controlsd.py | 5 +- .../controls/lib/ford_joint/inverse.py | 29 ++++++----- openpilot/selfdrive/controls/lib/ford_path.py | 10 ++++ .../tests/test_ford_model_action_adapter.py | 31 +++++++++++- 8 files changed, 182 insertions(+), 26 deletions(-) create mode 100644 docs/ford_joint_equal_arrival_trial.md create mode 100644 openpilot/selfdrive/car/tests/test_ford_joint_allocation.py diff --git a/docs/ford_joint_equal_arrival_trial.md b/docs/ford_joint_equal_arrival_trial.md new file mode 100644 index 0000000000..cdaf441585 --- /dev/null +++ b/docs/ford_joint_equal_arrival_trial.md @@ -0,0 +1,39 @@ +# Ford coordinated allocation trial + +The existing `FordModelActionController` + `FordPscmJointControl` gate now uses +the recovered normal C0/C1 slew rates to choose its steady endpoint split: + +``` +t = desired_internal_curvature / (g0 * c0_rate + g1 * c1_rate) +c0 = t * c0_rate +c1 = t * c1_rate +``` + +When a field clips, allocate its missing contribution to the other field's +remaining capacity. The existing dynamic selector still handles held state, +heading-dependent filtering, target preview and release. No new gain, control +mode, search horizon or native-library ABI is added. DBC bounds and C2=C3=0 remain. +The normal upstream controller is unchanged with the existing master toggle off. + +This is the exact endpoint policy tested against ML3V-14D003-BD / +ML34-14D007-EDL instructions. At 18 mph, the approximately 204-degree internal +target settles within 2.5 degrees in 2.864 rather than 3.384 seconds. The first +90% of its rise is unchanged. The approximately 25-degree release takes .624 +rather than .456 seconds to settle: this is a known trial tradeoff, not a proven +improvement in all driving. These are internal target results, not wheel-motion +or closed-loop smoothness predictions. Actual steering feedback must assess both +large turns and ordinary correction/release. + +For stopping, controlsd and card share the same normalization: a filtered speed +between -0.3 and 0 m/s counts as zero only when raw speed is in [0, 0.3) m/s and +reverse is not selected. This prevents small speed-filter undershoot from +invalidating and clearing the last active packet. Larger/inconsistent negative +speeds, reverse, stale inputs, faults, override and disengagement still release. +No global CarState speed or PSCM speed message is altered. Retaining the request +does not bypass the firmware's separate near-zero-speed output gate. + +Validation: 926 Ford tests and two subtests passed; lint and diff checks passed. +The shipped encoder reproduces all 9,000 saved candidate command samples from +the native-verified step/release fixture (maximum difference 8.89e-16). Native +instruction validation and the underlying fixtures are recorded in the local +PSCM authority report. Diagnostics include `allocation: equal-arrival`. diff --git a/openpilot/selfdrive/car/ford_joint_control.py b/openpilot/selfdrive/car/ford_joint_control.py index b6745c1d4e..1ecda08476 100644 --- a/openpilot/selfdrive/car/ford_joint_control.py +++ b/openpilot/selfdrive/car/ford_joint_control.py @@ -7,6 +7,7 @@ It estimates older-firmware state; it cannot observe ECU RAM or prove acceptance import math from opendbc.car.ford.values import FordFlags +from openpilot.selfdrive.controls.lib.ford_path import joint_control_speed # Small steering-wheel trim around the nominal inverse, in degrees. These are # trial tuning values, not recovered PSCM constants. Entry assist is separately gated. @@ -102,18 +103,19 @@ class FordJointControl: self.advance(now) path = CC_SP.fordLateralPath target = float(CC.actuators.steeringAngleDeg) + speed_ms = joint_control_speed(CS) # controlsd publishes calibrated car-frame motion and gates the path on its # health/freshness. Raw Ford CAN yaw has a zero offset on the audited truck. # Calibrated Z is opposite the CAN/pinion sign used by the recovered model. yaw = -float(CC.angularVelocity[2]) if len(CC.angularVelocity) == 3 else math.nan - finite = all(math.isfinite(v) for v in (CS.vEgo, CS.steeringAngleDeg, CS.yawRate, yaw, target)) + finite = all(math.isfinite(v) for v in (speed_ms, CS.steeringAngleDeg, CS.yawRate, yaw, target)) if finite: # Ford does not populate CarState.steeringRateDeg. Derive motion from the # measured angle; 0.1 s filtering suppresses its 0.1-degree quantization. if 0.0 < dt <= 0.1: rate = (CS.steeringAngleDeg - self.measurement[1]) / dt self.wheel_rate += dt / (0.1 + dt) * (rate - self.wheel_rate) - self.measurement = (max(0.0, CS.vEgo * 3.6), CS.steeringAngleDeg, yaw) + self.measurement = (max(0.0, speed_ms * 3.6), CS.steeringAngleDeg, yaw) else: self.wheel_rate = 0.0 status_fresh = pscm_status is not None and pscm_status.valid and pscm_status.canMonoTime > 0 and -0.005 <= now - pscm_status.canMonoTime * 1e-9 <= 0.15 @@ -124,7 +126,7 @@ class FordJointControl: fresh and CS.canValid and finite - and 0.0 <= CS.vEgo <= 55 + and 0.0 <= speed_ms <= 55 and abs(CS.yawRate) <= 3 and abs(yaw) <= 3 and path.enabled @@ -142,7 +144,7 @@ class FordJointControl: self.requested_rate = 0.0 command = (0.0, 0.0) details = {} - stop_hold = active and CS.vEgo < 0.3 + stop_hold = active and speed_ms < 0.3 if stop_hold: # The moving inverse divides by v**2. At a stop retain the last actual # active packet, not an inactive/zero request or a fictitious speed. @@ -251,6 +253,7 @@ class FordJointControl: 'driver_override': override, 'driver_pressed': bool(CS.steeringPressed), 'stop_hold': bool(stop_hold), + 'allocation': 'equal-arrival', 'yaw_source': 'calibrated_pose', 'yaw_rate': yaw if math.isfinite(yaw) else None, 'can_yaw_rate': float(CS.yawRate) if math.isfinite(CS.yawRate) else None, diff --git a/openpilot/selfdrive/car/tests/test_ford_joint_allocation.py b/openpilot/selfdrive/car/tests/test_ford_joint_allocation.py new file mode 100644 index 0000000000..7bf43d6ad5 --- /dev/null +++ b/openpilot/selfdrive/car/tests/test_ford_joint_allocation.py @@ -0,0 +1,49 @@ +"""A field reaching its bound must not discard a jointly reachable target.""" +import pytest + +from openpilot.selfdrive.controls.lib.ford_joint.angle import AngleModel +from openpilot.selfdrive.controls.lib.ford_joint.encoder import PairedRelease +from openpilot.selfdrive.controls.lib.ford_joint.inverse import C0_BOUND, C1_BOUND, invert_angle, static_pair +from openpilot.selfdrive.controls.lib.ford_joint.model import MainRequest + + +@pytest.mark.parametrize('sign', [-1., 1.]) +def test_joint_encoder_preserves_large_reachable_target(sign): + request = MainRequest(native_lookup=True) + speed = 15 * 1.609344 + inverse = invert_angle(AngleModel(request.cal), speed, sign * 280., 0., 0., 0., 3.7, 16.9) + assert not inverse['accel_limited'] + probe = MainRequest(native_lookup=True).step(speed, 0., 0., freeze_i=True) + c0, c1 = static_pair(request, inverse['curvature'], speed) + assert abs(c1) <= C1_BOUND + assert abs(c0) <= C0_BOUND + assert probe['g0'] * c0 + probe['g1'] * c1 == pytest.approx(inverse['curvature'], abs=1e-12) + + _, info = PairedRelease(request).choose(speed, inverse['curvature']) + # The planned target is the CAN-quantized steady pair, not next-tick wheel motion. + quantization_error = probe['g0'] * .005 + probe['g1'] * .00025 + assert abs(info['planned_target'] - inverse['curvature']) <= quantization_error + 1e-12 + + +@pytest.mark.parametrize('speed', [8., 15., 19.312128, 24.14016, 28.968192, 50., 100., 180.]) +@pytest.mark.parametrize('fraction', [-2., -1., -.8, -.3, 0., .3, .8, 1., 2.]) +def test_static_allocation_uses_available_combined_field_range(speed, fraction): + request = MainRequest(native_lookup=True) + probe = request.step(speed, 0., 0., freeze_i=True) + g0, g1 = probe['g0'], probe['g1'] + capacity = g0 * C0_BOUND + g1 * C1_BOUND + c0, c1 = static_pair(request, fraction * capacity, speed) + assert abs(c0) <= C0_BOUND and abs(c1) <= C1_BOUND + expected = max(-capacity, min(capacity, fraction * capacity)) + assert g0 * c0 + g1 * c1 == pytest.approx(expected, abs=1e-12) + + +@pytest.mark.parametrize('sign', [-1., 1.]) +def test_unsaturated_pair_reaches_both_endpoints_together(sign): + request = MainRequest(native_lookup=True) + speed = 15 * 1.609344 + inverse = invert_angle(AngleModel(request.cal), speed, sign * 180., 0., 0., 0., 3.7, 16.9) + c0, c1 = static_pair(request, inverse['curvature'], speed) + assert c0 / request.cal.f(0xFEF259F8) == pytest.approx(c1 / request.cal.f(0xFEF25A08)) + probe = request.step(speed, 0., 0., freeze_i=True) + assert probe['g0'] * c0 + probe['g1'] * c1 == pytest.approx(inverse['curvature']) diff --git a/openpilot/selfdrive/car/tests/test_ford_joint_control.py b/openpilot/selfdrive/car/tests/test_ford_joint_control.py index 799b6dd3a4..060b8d8c69 100644 --- a/openpilot/selfdrive/car/tests/test_ford_joint_control.py +++ b/openpilot/selfdrive/car/tests/test_ford_joint_control.py @@ -133,6 +133,28 @@ def test_stop_repeats_transmitted_request_and_resume_keeps_target(sign): assert not p.joint.fault +@pytest.mark.parametrize('sign', [-1., 1.]) +def test_stop_speed_filter_undershoot_keeps_actual_transmitted_hold(sign): + p = Pipeline() + p.cs.gearShifter = structs.CarState.GearShifter.drive + for i in range(30): + p.tick(1+i*.01, sign*120.) + sent = p.joint.sent + assert sent[2] and any(abs(v) > .01 for v in sent[:2]) + p.cs.vEgoRaw, p.cs.standstill = 0., True + for i, speed in enumerate([.01, 0., -.01, -.04, -.01, 0., .01, .29]): + p.cs.vEgo = speed + assert p.tick(1.3+i*.01, sign*120.).latActive + assert p.joint.sent == sent + assert p.joint.diagnostics['stop_hold'] + p.cs.vEgo = p.cs.vEgoRaw = .3 + assert p.tick(1.38, sign*120.).latActive + assert p.joint.sent[0]*sign > 0 and p.joint.sent[1]*sign > 0 + p.cs.gearShifter = structs.CarState.GearShifter.reverse + assert not p.tick(1.39, sign*120.).latActive + assert p.joint.sent == (0., 0., False) + + @pytest.mark.parametrize('failure', ['disengage', 'stale', 'can', 'steering_fault', 'override', 'denied']) def test_stopped_hold_still_releases_and_does_not_resurrect_old_command(failure): p = Pipeline() @@ -451,16 +473,18 @@ def test_candidate_does_not_mutate_state_and_native_step_matches_python(): @pytest.mark.parametrize('speed,c0,c1,filtered,fast,target,phase,pair,cost', [ - (20., 0., 0., 0., False, .03, 0, (.03, .002), 5.6554469893989605), - (20., 3., .4, .03, False, -.03, 2, (2.98, .399), 151.49476621872228), + (20., 0., 0., 0., False, .03, 0, (.03, .002), 4.719272529061328), + (20., 3., .4, .03, False, -.03, 2, (2.98, .399), 142.8506441410823), (40., 3., .4, .03, True, 0., 0, (2.95, .395), 12.167867914611671), - (6., 5.11, .5, .1, False, -.1, 0, (5.08, .498), 1391.5462859829215), - (80., -5.11, -.5, -.1, True, .03, 2, (-5.08, -.4975), 34.51646821335084), + (6., 5.11, .5, .1, False, -.1, 0, (5.08, .498), 1371.556512359005), + (80., -5.11, -.5, -.1, True, .03, 2, (-5.08, -.4975), 39.42744549117561), (40., 3., -.4, 0., False, 0., 2, (3.02, -.399), 6.184073685045232), ]) -def test_optimized_selection_preserves_v24_commands(speed, c0, c1, filtered, fast, target, phase, pair, cost): +def test_optimized_selection_matches_frozen_cases(speed, c0, c1, filtered, fast, target, phase, pair, cost): # Frozen outputs from 528ed3615 before pruning/caching the full-return search. # Cover entry, reversal, release, saturation, cancellation and both tick counts. + # Costs refreshed for the equal-buildup endpoint and residual allocation at + # field bounds. All six immediate selected packets remain unchanged. m = MainRequest(native_lookup=True) m.c0, m.c1, m.filtered, m.fast = c0, c1, filtered, fast command, info = PairedRelease(m).choose(speed, target, phase) diff --git a/openpilot/selfdrive/controls/controlsd.py b/openpilot/selfdrive/controls/controlsd.py index 7b4ddd2512..95fe07bd66 100755 --- a/openpilot/selfdrive/controls/controlsd.py +++ b/openpilot/selfdrive/controls/controlsd.py @@ -180,7 +180,10 @@ class Controls(ControlsExt): reference_service = 'lateralManeuverPlan' if self.sm.valid['lateralManeuverPlan'] else 'modelV2' now = time.monotonic() yaw_rate, motion_valid = -CS.yawRate, True + ford_speed = CS.vEgo if self.ford_path_controller.joint_control: + from openpilot.selfdrive.controls.lib.ford_path import joint_control_speed + ford_speed = joint_control_speed(CS) # card's inverse uses the calibrated yaw published in carControl. # Never keep steering from a stale pose or fall back to raw CAN yaw. motion = self.sm['deviceMotion'] @@ -190,7 +193,7 @@ class Controls(ControlsExt): and -0.005 <= now - self.sm.logMonoTime['deviceMotion'] * 1e-9 <= 0.15) yaw_rate = float(self.calibrated_pose.angular_velocity.xyz[2]) if motion_valid else math.nan self.ford_path = self.ford_path_controller.update( - ford_model, self.desired_curvature, current_curvature=self.curvature, yaw_rate=yaw_rate, speed=CS.vEgo, now=now, + ford_model, self.desired_curvature, current_curvature=self.curvature, yaw_rate=yaw_rate, speed=ford_speed, now=now, # Roll/angle offset cancel in the error; retain the normal steering-angle conversion's speed and stiffness effects. curvature_scale=self.VM.get_steer_from_curvature(1., CS.vEgo, 0.) / (self.CP.steerRatio*self.CP.wheelbase), measurement_time=self.sm.logMonoTime['carState'] * 1e-9, diff --git a/openpilot/selfdrive/controls/lib/ford_joint/inverse.py b/openpilot/selfdrive/controls/lib/ford_joint/inverse.py index da7fa578fe..d230c0ae47 100644 --- a/openpilot/selfdrive/controls/lib/ford_joint/inverse.py +++ b/openpilot/selfdrive/controls/lib/ford_joint/inverse.py @@ -55,25 +55,24 @@ def invert_angle(output, speed_kmh, target_angle, angle, yaw, accel, wheelbase, } -def arc_pair(curvature, speed_kmh): - """Current base geometry only; no current OP proportional/integral terms.""" - half = 0.5 * 7 * curvature - sinc = math.sin(half) / half if half else 1.0 - c1 = max(7.0, speed_kmh / 3.6) * curvature - clipped_c1 = clip(c1, -C1_BOUND, C1_BOUND) - c0 = 0.5 * curvature * 49 * sinc * sinc + 7 * (c1 - clipped_c1) - return clip(c0, -C0_BOUND, C0_BOUND), clipped_c1 - - def static_pair(request, curvature, speed_kmh, *, gains=None): - """Preserve the base's channel proportion, solve its static curvature sum.""" + """Allocate equal nominal buildup times, then use remaining field capacity.""" if gains is None: probe = copy.copy(request).step(speed_kmh, request.c0, request.c1, freeze_i=True) gains = probe['g0'], probe['g1'] - p0, p1 = arc_pair(curvature, speed_kmh) - total = gains[0] * p0 + gains[1] * p1 - scale = curvature / total if total else 1.0 - return clip(p0 * scale, -C0_BOUND, C0_BOUND), clip(p1 * scale, -C1_BOUND, C1_BOUND) + # Recovered normal held-input rates; the dynamic selector still accounts for + # current held states, the fast latch, heading-dependent filter and release. + r0, r1 = request.cal.f(0xFEF259F8), request.cal.f(0xFEF25A08) + duration = curvature / (gains[0] * r0 + gains[1] * r1) + p0, p1 = duration * r0, duration * r1 + c0, c1 = clip(p0, -C0_BOUND, C0_BOUND), clip(p1, -C1_BOUND, C1_BOUND) + if abs(p0) > C0_BOUND or abs(p1) > C1_BOUND: + # Clipping one field must not silently lower a target the pair can encode. + if gains[0]: + c0 = clip((curvature - gains[1] * c1) / gains[0], -C0_BOUND, C0_BOUND) + if gains[1]: + c1 = clip((curvature - gains[0] * c0) / gains[1], -C1_BOUND, C1_BOUND) + return c0, c1 def quantize(pair): diff --git a/openpilot/selfdrive/controls/lib/ford_path.py b/openpilot/selfdrive/controls/lib/ford_path.py index 3c923fd609..b35a94dc39 100644 --- a/openpilot/selfdrive/controls/lib/ford_path.py +++ b/openpilot/selfdrive/controls/lib/ford_path.py @@ -5,6 +5,7 @@ import math import numpy as np from opendbc.car.ford.values import CarControllerParams +from opendbc.car.structs import CarState DBC_OFFSET = (-5.12, 5.11) @@ -41,6 +42,15 @@ class FordPath: curvature_rate: float = 0.0 +def joint_control_speed(CS): + """Treat small speed-filter undershoot as stopped only near raw standstill.""" + if CS.gearShifter == CarState.GearShifter.reverse: + return math.nan + if -0.3 <= CS.vEgo < 0.0 and 0.0 <= CS.vEgoRaw < 0.3: + return 0.0 + return CS.vEgo + + @dataclass(frozen=True) class FordPscmState: path_offset: float = 0.0 diff --git a/openpilot/selfdrive/controls/tests/test_ford_model_action_adapter.py b/openpilot/selfdrive/controls/tests/test_ford_model_action_adapter.py index 6aaa894aa8..25aef24713 100644 --- a/openpilot/selfdrive/controls/tests/test_ford_model_action_adapter.py +++ b/openpilot/selfdrive/controls/tests/test_ford_model_action_adapter.py @@ -269,7 +269,8 @@ def test_joint_calibrated_motion_gate_only_affects_joint_mode(pipeline, joint, f controls.calibrated_pose.angular_velocity.xyz[2] = math.nan if failure == 'nan' else 3.1 cc = structs.CarControl(latActive=True) - cs = SimpleNamespace(vEgo=20., yawRate=-.028, canValid=True, steeringPressed=False, steeringTorque=0.) + cs = SimpleNamespace(vEgo=20., vEgoRaw=20., gearShifter=structs.CarState.GearShifter.drive, + yawRate=-.028, canValid=True, steeringPressed=False, steeringTorque=0.) model = straight() model.action = SimpleNamespace(desiredCurvature=.001) exec(pipeline[0], {'self': controls, 'CS': cs, 'CC': cc, 'actuators': cc.actuators, 'model_v2': model, @@ -281,6 +282,34 @@ def test_joint_calibrated_motion_gate_only_affects_joint_mode(pipeline, joint, f assert msg.fordLateralPath.valid == cc.latActive +@pytest.mark.parametrize('speed,raw,reverse,expected', [ + (-.01, 0., False, True), (-.29, .1, False, True), (0., 0., False, True), + (-.31, 0., False, False), (-.01, 1., False, False), (-.01, math.nan, False, False), + (-.01, 0., True, False), (.1, .1, True, False), +]) +def test_joint_controlsd_stop_speed_undershoot(pipeline, speed, raw, reverse, expected): + settings = {'FordModelActionController', 'FordPscmJointControl'} + controls = startup(params=SimpleNamespace(get_bool=lambda key: key in settings)) + sm = Subscriptions(False) + controls.sm, controls.desired_curvature, controls.curvature = sm, 0., 0. + controls.calibrated_pose = SimpleNamespace(angular_velocity=SimpleNamespace(xyz=[0., 0., 0.])) + controls.pose_calibrator = SimpleNamespace(calib_valid=True) + sm.messages['deviceMotion'] = SimpleNamespace(inputsOK=True, sensorsOK=True, angularVelocityDevice=SimpleNamespace(valid=True)) + sm.logMonoTime['deviceMotion'] = 980_000_000 + cs = structs.CarState(vEgo=speed, vEgoRaw=raw, canValid=True, standstill=True, + gearShifter=structs.CarState.GearShifter.reverse if reverse else structs.CarState.GearShifter.drive) + cc = structs.CarControl(latActive=True) + model = straight() + model.action = SimpleNamespace(desiredCurvature=.001) + exec(pipeline[0], {'self': controls, 'CS': cs, 'CC': cc, 'actuators': cc.actuators, 'model_v2': model, + 'lp': SimpleNamespace(roll=0.), 'math': math, 'clip_curvature': clip_curvature, + 'time': SimpleNamespace(monotonic=lambda: 1.)}) + assert cc.latActive == controls.ford_path.valid == expected + msg = custom.CarControlSP.new_message() + exec(pipeline[1], {'self': controls, 'CC_SP': msg}) + assert msg.fordLateralPath.valid == cc.latActive + + @pytest.mark.parametrize('sign', [-1., 1.]) def test_feedback_through_actual_controlsd_publication_and_100hz_sender(pipeline, sign): call, publication = pipeline