mawei/sunnypilot
1
0
Fork 0
mirror of https://github.com/sunnypilot/sunnypilot.git synced 2026-09-02 01:53:45 +08:00
Code Issues Packages Projects Releases Wiki Activity

Ford: preserve pose authority when C1 clips

Browse Source 
Move heading authority lost at the DBC angle limit into available C0 endpoint authority while retaining the coordinated output limiter.

Assisted-by: Codex
This commit is contained in:
Isaac Barham
2026-09-01 01:09:47 -04:00
parent 0729ce7c08
commit a1dcec490f
2 changed files with 19 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
openpilot/selfdrive/controls/lib/ford_path.py
+3 -1
View File
@@ -88,11 +88,13 @@ def _encode_path(path: tuple[list[float], list[float], list[float]], desired_cur
# expressed in those same pose units and cannot initiate the transfer.
path_offset = pose_share * (model_offset + 0.5 * tracking_error * offset_horizon ** 2)
path_angle = pose_share * (model_angle + tracking_error * angle_horizon)
limited_path_angle = float(np.clip(path_angle, *DBC_ANGLE))
path_offset += (path_angle - limited_path_angle) * offset_horizon
curvature = desired_curvature * (1.0 - pose_share)
return FordPath(
valid=True,
path_offset=float(np.clip(path_offset, *DBC_OFFSET)),
path_angle=float(np.clip(path_angle, *DBC_ANGLE)),
path_angle=limited_path_angle,
curvature=float(np.clip(curvature, *DBC_CURVATURE)),
curvature_rate=0.0,
)
openpilot/selfdrive/controls/tests/test_ford_path.py
+16 -1
View File
@@ -123,7 +123,7 @@ def test_low_speed_still_uses_available_model_pose():
def test_higher_speed_extends_heading_horizon_without_moving_offset_horizon():
model = _changing_path(0.0, 0.04, speed=20.0)
model = _changing_path(0.0, 0.015, speed=20.0)
slow = _command(model, 0.012, v_ego=7.0)
fast = _command(model, 0.012, v_ego=20.0)
assert np.isclose(fast.path_offset, slow.path_offset)
@@ -207,6 +207,21 @@ def test_output_limits_and_rates_are_bounded():
assert np.max(np.abs(np.diff([command.path_angle for command in outputs]))) <= 0.01 + 1e-9
def test_clipped_path_angle_uses_available_offset_to_preserve_endpoint():
horizon = 7.0
for curvature, angle_limit in ((-0.1, DBC_ANGLE[0]), (0.1, DBC_ANGLE[1])):
model = _path(curvature)
command = _command(model, curvature, current_curvature=curvature, v_ego=horizon)
distance = np.concatenate(([0.0], np.cumsum(np.hypot(np.diff(model.position.x), np.diff(model.position.y)))))
model_offset = np.interp(horizon, distance, model.position.y)
model_angle = np.interp(horizon, distance, model.orientation.z)
assert command.path_angle == angle_limit
assert np.isclose(command.path_offset + horizon * command.path_angle,
model_offset + horizon * model_angle)
def test_invalid_model_ramps_pose_to_zero_and_inactive_resets():
controller = FordPathController(dt=0.01)
for _ in range(20):