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

Ford: strengthen bounded path tracking feedback

Browse Source 
Keep action curvature authoritative while increasing bounded C0/C1 feedback when measured curvature is behind. Keep C2 allocation tied to maneuver demand instead of tracking error.

Assisted-by: Codex <codex@openai.com>
This commit is contained in:
Isaac Barham
2026-08-30 11:28:19 -04:00
parent 405407c252
commit 24c858e618
2 changed files with 10 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
openpilot/selfdrive/controls/lib/ford_path.py
+3 -2
View File
@@ -15,6 +15,7 @@ _CURVATURE_RATE_HORIZONS = (3.5, 5.0, 7.0)
_FAST_POSE_CURVATURE_BAND = (0.009, 0.012)
_CENTERING_CURVATURE_SHARE = 0.65
_TRACKING_ERROR_DEADZONE = 0.0005
_TRACKING_ERROR_GAIN = 1.5
_TRACKING_ERROR_LIMIT = 0.012
_PATH_RATES = (4.0, 1.0, math.inf, math.inf)
@@ -91,7 +92,7 @@ def _encode_path(model, desired_curvature: float, v_ego: float, current_curvatur
target_curvature = action_curvature
measured_curvature = _finite(current_curvature)
tracking_error = target_curvature - measured_curvature
correction = math.copysign(max(abs(tracking_error) - _TRACKING_ERROR_DEADZONE, 0.0), tracking_error)
correction = _TRACKING_ERROR_GAIN * math.copysign(max(abs(tracking_error) - _TRACKING_ERROR_DEADZONE, 0.0), tracking_error)
wheel_beyond_target = target_curvature * measured_curvature > 0.0 and \
abs(target_curvature) + _TRACKING_ERROR_DEADZONE < abs(measured_curvature)
correction_limit = _TRACKING_ERROR_LIMIT
@@ -103,7 +104,7 @@ def _encode_path(model, desired_curvature: float, v_ego: float, current_curvatur
sustained_curvature = 0.0
if action_curvature * future_curvature > 0.0 and abs(future_curvature) > _TRACKING_ERROR_DEADZONE:
sustained_curvature = math.copysign(min(abs(action_curvature), abs(future_curvature)), action_curvature)
maneuver_demand = max(abs(action_curvature), abs(correction))
maneuver_demand = abs(action_curvature)
maneuver_share = float(np.interp(maneuver_demand, _FAST_POSE_CURVATURE_BAND, (0.0, 1.0)))
centering_curvature = 0.0 if wheel_beyond_target else \
sustained_curvature * _CENTERING_CURVATURE_SHARE * (1.0 - maneuver_share)
openpilot/selfdrive/controls/tests/test_ford_path.py
+7
View File
@@ -306,6 +306,13 @@ def test_curvature_error_increases_forward_pose_command_while_behind():
assert np.isclose(behind.curvature_rate, tracking.curvature_rate)
def test_major_turn_undertracking_uses_bounded_fast_feedback_authority():
command = FordPathController(dt=1.0).update(_path(0.02), 0.02, v_ego=8.0, current_curvature=0.01)
assert command.curvature == 0.0
assert command.path_angle >= 0.25
def test_measured_wheel_beyond_action_countersteers_model_arc():
controller = FordPathController()
controller.update(_path(0.02), 0.02, v_ego=15.0, current_curvature=0.02)