diff --git a/openpilot/selfdrive/controls/lib/ford_path.py b/openpilot/selfdrive/controls/lib/ford_path.py index d4aabc4479..e7b72d5610 100644 --- a/openpilot/selfdrive/controls/lib/ford_path.py +++ b/openpilot/selfdrive/controls/lib/ford_path.py @@ -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, ) diff --git a/openpilot/selfdrive/controls/tests/test_ford_path.py b/openpilot/selfdrive/controls/tests/test_ford_path.py index 43bd6f517e..d4008a4b91 100644 --- a/openpilot/selfdrive/controls/tests/test_ford_path.py +++ b/openpilot/selfdrive/controls/tests/test_ford_path.py @@ -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):