diff --git a/docs/ford_joint_supervisor_endpoint.md b/docs/ford_joint_supervisor_endpoint.md new file mode 100644 index 0000000000..ad25e6555e --- /dev/null +++ b/docs/ford_joint_supervisor_endpoint.md @@ -0,0 +1,38 @@ +# Ford joint endpoint: hold C0 at the supervisor saturation + +The equal-arrival endpoint (`C0 = 1.5 t`, `C1 = 0.1 t`) holds 2-4 m of C0 in large +turns. In the reference ML3V-14D003-BD / ML34-14D007-EDL calibration the path +supervisor centre is + +``` +S(v) * (clip(0.5 * held_C0, +-0.5) + clip(10 * held_C1, +-0.349609)) +``` + +with width `0.399994 * S + 5` below 40 km/h. Its C0 term saturates at 1.0 m, so +held C0 above that adds primary curvature but no turn-direction allowance. On +release, zero demand stays excluded until held C0 slews (1.5 m/s) below about +0.41 m while C1 is saturated, so extra C0 adds (C0 - 0.41) / 1.5 s of forced +old-direction output. + +`static_pair` therefore caps its equal-arrival C0 at 1.0 m and gives the rest to +C1; the remainder returns to C0 only when C1 reaches its bound. Endpoints with +equal-arrival C0 of at most 1.0 m are unchanged. The paired selector, dynamics, +quantization, preview, gates, stop hold and DBC bounds are unchanged; it still +builds with both fields, then trades C0 for C1 at constant curvature. + +Offline evidence (reference firmware, not the live RL38 calibration; no wheel, +assist or vehicle model): + +- Native zero-demand supervisor probe, controlled 15/20/25 km/h large steps: + zero excluded after release 1.48/1.88/2.17 s -> 0.39/0.39/0.48 s. The first + 50%/90% of the rise is unchanged; the last 1% can settle up to 0.5 s later. +- Native probe on replayed routes 194/195/196 packets: forced old-direction + output in the 3 s after large releases 24.9 -> 6.4, 19.6 -> 8.4, + 15.0 -> 3.6, 16.6 -> 8.3, 11.0 -> 4.4 unit*s (one driver-involved release + 9.6 -> 9.2). +- 81,340 replay updates per policy: with no |target| >= 30 deg within 10 s, + packets are identical on 194/196 and 98.9% identical on 195. + +These are firmware-stage predictions. A physical A/B must show whether the wheel +releases large turns earlier; diagnostics report `allocation: +equal-arrival-supervisor-c0`. diff --git a/openpilot/selfdrive/car/ford_joint_control.py b/openpilot/selfdrive/car/ford_joint_control.py index 1ecda08476..424831a813 100644 --- a/openpilot/selfdrive/car/ford_joint_control.py +++ b/openpilot/selfdrive/car/ford_joint_control.py @@ -253,7 +253,7 @@ class FordJointControl: 'driver_override': override, 'driver_pressed': bool(CS.steeringPressed), 'stop_hold': bool(stop_hold), - 'allocation': 'equal-arrival', + 'allocation': 'equal-arrival-supervisor-c0', '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 index 7bf43d6ad5..5041cdd260 100644 --- a/openpilot/selfdrive/car/tests/test_ford_joint_allocation.py +++ b/openpilot/selfdrive/car/tests/test_ford_joint_allocation.py @@ -3,7 +3,7 @@ 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.inverse import C0_BOUND, C0_SUPERVISOR_SATURATION, C1_BOUND, invert_angle, static_pair from openpilot.selfdrive.controls.lib.ford_joint.model import MainRequest @@ -40,10 +40,64 @@ def test_static_allocation_uses_available_combined_field_range(speed, fraction): @pytest.mark.parametrize('sign', [-1., 1.]) def test_unsaturated_pair_reaches_both_endpoints_together(sign): + # Below the supervisor's C0 saturation the endpoint is still equal arrival. 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) + inverse = invert_angle(AngleModel(request.cal), speed, sign * 40., 0., 0., 0., 3.7, 16.9) c0, c1 = static_pair(request, inverse['curvature'], speed) + assert abs(c0) < C0_SUPERVISOR_SATURATION 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']) + + +@pytest.mark.parametrize('speed', [8., 15., 19.312128, 24.14016, 28.968192, 50., 100., 180.]) +@pytest.mark.parametrize('fraction', [.05, .2, .4, .6, .8, .95, 1.]) +@pytest.mark.parametrize('sign', [-1., 1.]) +def test_endpoint_holds_c0_at_supervisor_saturation(speed, fraction, sign): + request = MainRequest(native_lookup=True) + probe = request.step(speed, 0., 0., freeze_i=True) + g0, g1 = probe['g0'], probe['g1'] + curvature = sign * fraction * (g0 * C0_BOUND + g1 * C1_BOUND) + c0, c1 = static_pair(request, curvature, speed) + assert g0 * c0 + g1 * c1 == pytest.approx(curvature, abs=1e-12) + # Held C0 exceeds the supervisor saturation only when C1 alone cannot carry the rest. + assert abs(c0) <= C0_SUPERVISOR_SATURATION + 1e-12 or abs(c1) == C1_BOUND + assert static_pair(request, -curvature, speed) == pytest.approx((-c0, -c1), abs=1e-15) + + +def _equal_arrival(request, curvature, speed_kmh, *, gains): + r0, r1 = request.cal.f(0xFEF259F8), request.cal.f(0xFEF25A08) + duration = curvature / (gains[0] * r0 + gains[1] * r1) + return duration * r0, duration * r1 + + +@pytest.mark.parametrize('speed,curvature', [(15., .06), (20., .05), (25., .04)]) +def test_capped_endpoint_keeps_entry_and_parks_c0_before_release(monkeypatch, speed, curvature): + """Production selector, quantized packets: same rise, less held C0 at the end of the hold.""" + from openpilot.selfdrive.controls.lib.ford_joint import encoder + from openpilot.selfdrive.controls.lib.ford_joint.inverse import quantize + + def run(): + m = MainRequest(native_lookup=True) + selector = PairedRelease(m) + sent, phase, rise = (0., 0.), 0., None + for i in range(500): # 5 s step-and-hold at 100 Hz + phase += .01 + ticks = int((phase + 1e-12) / .008) + phase -= ticks * .008 + for _ in range(ticks): + m.step(speed, *sent, freeze_i=True) + if rise is None and m.filtered >= .9 * curvature: + rise = i * .01 + count = max(1, min(2, int((phase + .01 + 1e-12) / .008))) + command, _ = selector.choose(speed, curvature, phase=0 if count == 2 else 2) + sent = quantize(command) + return rise, m.c0, m.filtered + + capped = run() + monkeypatch.setattr(encoder, 'static_pair', _equal_arrival) + equal = run() + assert capped[0] == pytest.approx(equal[0], abs=.02) + assert capped[2] == pytest.approx(curvature, rel=.01) + assert abs(capped[1]) <= C0_SUPERVISOR_SATURATION + .15 < abs(equal[1]) diff --git a/openpilot/selfdrive/car/tests/test_ford_joint_control.py b/openpilot/selfdrive/car/tests/test_ford_joint_control.py index 060b8d8c69..f266af643d 100644 --- a/openpilot/selfdrive/car/tests/test_ford_joint_control.py +++ b/openpilot/selfdrive/car/tests/test_ford_joint_control.py @@ -473,10 +473,10 @@ 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), 4.719272529061328), - (20., 3., .4, .03, False, -.03, 2, (2.98, .399), 142.8506441410823), + (20., 0., 0., 0., False, .03, 0, (.03, .002), 5.206693787207076), + (20., 3., .4, .03, False, -.03, 2, (2.98, .399), 149.0975016923494), (40., 3., .4, .03, True, 0., 0, (2.95, .395), 12.167867914611671), - (6., 5.11, .5, .1, False, -.1, 0, (5.08, .498), 1371.556512359005), + (6., 5.11, .5, .1, False, -.1, 0, (5.08, .498), 1423.8735141422555), (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), ]) @@ -484,7 +484,8 @@ def test_optimized_selection_matches_frozen_cases(speed, c0, c1, filtered, fast, # 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. + # field bounds, then for the supervisor-saturation C0 endpoint (its return + # path differs). 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/lib/ford_joint/inverse.py b/openpilot/selfdrive/controls/lib/ford_joint/inverse.py index d230c0ae47..d856dab94f 100644 --- a/openpilot/selfdrive/controls/lib/ford_joint/inverse.py +++ b/openpilot/selfdrive/controls/lib/ford_joint/inverse.py @@ -7,6 +7,11 @@ from openpilot.selfdrive.controls.lib.ford_joint.model import clip, interp_int, C0_BOUND = 5.11 C1_BOUND = 0.5 +# Reference-calibration supervisor centre term clip(0.5 * held C0, +-0.5) saturates +# here. Held C0 beyond it adds primary curvature but no turn-direction supervisor +# allowance, and on release the supervisor keeps excluding zero demand until held +# C0 slews (1.5 m/s) back below about 0.41 m. +C0_SUPERVISOR_SATURATION = 1.0 def invert_angle(output, speed_kmh, target_angle, angle, yaw, accel, wheelbase, ratio, accel_allowance=3.0): @@ -56,7 +61,8 @@ def invert_angle(output, speed_kmh, target_angle, angle, yaw, accel, wheelbase, def static_pair(request, curvature, speed_kmh, *, gains=None): - """Allocate equal nominal buildup times, then use remaining field capacity.""" + """Allocate equal nominal buildup times, hold C0 at most at the supervisor's C0 + saturation, 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'] @@ -65,6 +71,11 @@ def static_pair(request, curvature, speed_kmh, *, gains=None): r0, r1 = request.cal.f(0xFEF259F8), request.cal.f(0xFEF25A08) duration = curvature / (gains[0] * r0 + gains[1] * r1) p0, p1 = duration * r0, duration * r1 + if abs(p0) > C0_SUPERVISOR_SATURATION and gains[1]: + # Only the held endpoint moves: the selector still builds with both fields, + # then trades C0 for C1 at constant curvature before a release is needed. + p0 = math.copysign(C0_SUPERVISOR_SATURATION, p0) + p1 = (curvature - gains[0] * p0) / gains[1] 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.