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

Ford: restore full C2 gentle path following

Browse Source 
Assisted-by: Codex
This commit is contained in:
Isaac Barham
2026-08-31 11:41:29 -04:00
parent 7f371b8acd
commit b7f0e3fbdc
2 changed files with 16 additions and 16 deletions
Download Patch File Download Diff File Expand all files Collapse all files
openpilot/selfdrive/controls/lib/ford_path.py
+1 -1
View File
@@ -14,7 +14,7 @@ DBC_CURVATURE_RATE = (-0.001024, 0.001023)
_PATH_HORIZON = 7.0
_CURVATURE_RATE_HORIZONS = (3.5, 5.0, 7.0)
_FAST_POSE_CURVATURE_BAND = (0.009, 0.012)
_CENTERING_CURVATURE_SHARE = 0.65
_CENTERING_CURVATURE_SHARE = 1.0
_CURVATURE_NOISE_FLOOR = 0.0005
_PATH_RATES = (4.0, 1.0, math.inf, math.inf)
openpilot/selfdrive/controls/tests/test_ford_path.py
+15 -15
View File
@@ -39,22 +39,22 @@ def _command(model, desired_curvature: float, *, v_ego: float = 0.0, current_cur
return FordPathController(dt=1.0).update(model, desired_curvature, v_ego=v_ego, current_curvature=current_curvature)
def test_steady_arc_splits_hold_between_c2_and_fast_pose():
def test_steady_gentle_arc_uses_full_c2_without_fast_pose():
path = _command(_path(0.008), 0.008, v_ego=8.0, current_curvature=0.008)
assert path.valid
assert np.isclose(_fast_curvatures(path)[0], 0.0028)
assert np.isclose(_fast_curvatures(path)[1], 0.0028)
assert np.isclose(path.curvature, 0.0052, atol=5e-5)
assert np.isclose(_fast_curvatures(path)[0], 0.0)
assert np.isclose(_fast_curvatures(path)[1], 0.0)
assert np.isclose(path.curvature, 0.008, atol=5e-5)
assert abs(path.curvature_rate) < 1e-5
def test_gentle_changing_curve_keeps_continuous_pose_authority():
def test_gentle_changing_curve_stays_on_full_c2_when_tracking():
path = _command(_path(0.004, 0.00015), 0.004, v_ego=8.0, current_curvature=0.004)
assert abs(path.path_offset) > 0.0001
assert abs(path.path_angle) > 0.0001
assert 0.0 < path.curvature < 0.004
assert path.path_offset == 0.0
assert path.path_angle == 0.0
assert np.isclose(path.curvature, 0.004)
def test_c2_unloads_before_near_horizon_curve_exit():
@@ -245,8 +245,8 @@ def test_c2_uses_stable_action_curvature_not_independent_model_fit():
first = controller.update(_path(0.004), 0.002, v_ego=8.0, current_curvature=0.002)
second = controller.update(_path(0.006), 0.002, v_ego=8.0, current_curvature=0.002)
assert np.isclose(first.curvature, 0.0013)
assert np.isclose(second.curvature, 0.0013)
assert np.isclose(first.curvature, 0.002)
assert np.isclose(second.curvature, 0.002)
def test_action_curvature_corrects_stale_opposing_model_at_low_speed():
@@ -332,8 +332,8 @@ def test_action_c2_remains_active_for_centering():
centering = FordPathController(dt=1.0).update(_path(0.002), 0.002, v_ego=15.0, current_curvature=0.002)
assert centering.curvature > 0.001
assert centering.path_offset > 0.0
assert centering.path_angle > 0.0
assert centering.path_offset == 0.0
assert centering.path_angle == 0.0
def test_tight_turn_from_stop_builds_bounded_forward_pose_authority():
@@ -406,6 +406,6 @@ def test_gentle_c2_share_is_subtracted_once_from_fast_feedforward():
command = _command(_path(0.004), 0.004, current_curvature=0.004)
offset_curvature, angle_curvature = _fast_curvatures(command)
assert np.isclose(command.curvature, 0.0026)
assert np.isclose(offset_curvature, 0.0014)
assert np.isclose(angle_curvature, 0.0014)
assert np.isclose(command.curvature, 0.004)
assert np.isclose(offset_curvature, 0.0)
assert np.isclose(angle_curvature, 0.0)