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

ford: gate curvature rate with maneuver demand

Browse Source 
Assisted-by: Codex
This commit is contained in:
Isaac Barham
2026-08-27 19:44:19 -04:00
parent 7e2000e909
commit 3e020e321f
2 changed files with 10 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files
openpilot/selfdrive/controls/lib/ford_path.py
+1 -1
View File
@@ -136,7 +136,7 @@ def _encode_path(model, desired_curvature: float | None, v_ego: float, current_c
path_offset=float(np.clip(path_offset, *DBC_OFFSET)),
path_angle=float(np.clip(path_angle, *DBC_ANGLE)),
curvature=float(np.clip(centering_curvature * (1.0 - maneuver_share), *DBC_CURVATURE)),
curvature_rate=float(np.clip(model_curvature_rate, *DBC_CURVATURE_RATE)),
curvature_rate=float(np.clip(model_curvature_rate * maneuver_share, *DBC_CURVATURE_RATE)),
)
openpilot/selfdrive/controls/tests/test_ford_path.py
+9
View File
@@ -173,6 +173,15 @@ def test_minor_curve_uses_c2_when_tracking_is_close():
assert command.curvature > 0.0049
def test_minor_changing_curve_does_not_emit_ungated_c3():
command = FordPathController(dt=1.0).update(_path(0.005, 0.0003), 0.005, v_ego=8.0, current_curvature=0.0048)
assert abs(command.path_offset) < 1e-9
assert abs(command.path_angle) < 1e-9
assert command.curvature > 0.0049
assert command.curvature_rate == 0.0
def test_action_curvature_corrects_stale_opposing_model_at_low_speed():
command = FordPathController().update(_path(-0.001, speed=1.0), 0.005, v_ego=1.0, current_curvature=0.001)