diff --git a/openpilot/selfdrive/controls/controlsd.py b/openpilot/selfdrive/controls/controlsd.py index f5a870c4fa..d33a19478b 100755 --- a/openpilot/selfdrive/controls/controlsd.py +++ b/openpilot/selfdrive/controls/controlsd.py @@ -160,7 +160,8 @@ class Controls(ControlsExt): actuators.steeringAngleDeg = float(lateral_output) if self.CP.brand == "ford": self.ford_path = self.ford_path_controller.update(model_v2 if self.sm.valid['modelV2'] else None, - active=CC.latActive) + self.desired_curvature, current_curvature=self.curvature, + v_ego=CS.vEgo, active=CC.latActive) actuators.curvature = float(self.ford_path.curvature) # Ensure no NaNs/Infs for p in ACTUATOR_FIELDS: diff --git a/openpilot/selfdrive/controls/lib/ford_path.py b/openpilot/selfdrive/controls/lib/ford_path.py index af363e00f3..e7b72d5610 100644 --- a/openpilot/selfdrive/controls/lib/ford_path.py +++ b/openpilot/selfdrive/controls/lib/ford_path.py @@ -11,6 +11,8 @@ DBC_CURVATURE_RATE = (-0.001024, 0.001023) _PATH_MIN_LOOKAHEAD = 7.0 _POSE_BLEND_CURVATURE = (0.006, 0.012) +_TRACKING_ERROR_DEADZONE = 0.0005 +_TRACKING_ERROR_LIMIT = 0.012 _PATH_OFFSET_RATE = 4.0 _PATH_ANGLE_RATE = 1.0 @@ -24,6 +26,10 @@ class FordPath: curvature_rate: float = 0.0 +def _finite(value: float) -> float: + return float(value) if math.isfinite(value) else 0.0 + + def _sample(distance: float, distances: list[float], values: list[float]) -> float: return float(np.interp(distance, distances, values)) @@ -33,7 +39,7 @@ def _blend_share(demand: float) -> float: return float(np.clip((demand - lower) / (upper - lower), 0.0, 1.0)) -def _model_path(model) -> tuple[list[float], list[float], list[float], list[float]] | None: +def _model_path(model) -> tuple[list[float], list[float], list[float]] | None: try: x = [float(value) for value in model.position.x] y = [float(value) for value in model.position.y] @@ -55,38 +61,36 @@ def _model_path(model) -> tuple[list[float], list[float], list[float], list[floa for value in heading[1:]: delta = (value - unwrapped_heading[-1] + math.pi) % (2.0 * math.pi) - math.pi unwrapped_heading.append(unwrapped_heading[-1] + delta) - return distance, x, y, unwrapped_heading + return distance, y, unwrapped_heading -def _encode_path(path: tuple[list[float], list[float], list[float], list[float]]) -> FordPath: - distance, _, y, heading = path - target_distance = min(_PATH_MIN_LOOKAHEAD, distance[-1]) - horizon = max(target_distance, 1e-3) +def _encode_path(path: tuple[list[float], list[float], list[float]], desired_curvature: float, + current_curvature: float, v_ego: float) -> FordPath: + distance, offset, heading = path + offset_horizon = min(_PATH_MIN_LOOKAHEAD, distance[-1]) + angle_horizon = min(max(v_ego, _PATH_MIN_LOOKAHEAD), distance[-1]) + model_offset = _sample(offset_horizon, distance, offset) + model_angle = _sample(angle_horizon, distance, heading) - model_offset = _sample(target_distance, distance, y) - model_angle = _sample(target_distance, distance, heading) - start_heading = _sample(0.0, distance, heading) - midpoint_heading = _sample(0.5 * horizon, distance, heading) - target_heading = _sample(target_distance, distance, heading) - near_curvature = 2.0 * (midpoint_heading - start_heading) / horizon - far_curvature = 2.0 * (target_heading - midpoint_heading) / horizon - model_curvature = 0.5 * (near_curvature + far_curvature) + offset_curvature = 2.0 * model_offset / offset_horizon ** 2 + angle_curvature = model_angle / angle_horizon + pose_share = _blend_share(max(abs(offset_curvature), abs(angle_curvature), abs(desired_curvature))) - offset_curvature = 2.0 * model_offset / horizon ** 2 - angle_curvature = model_angle / horizon - magnitude_share = _blend_share(max(abs(offset_curvature), abs(angle_curvature), abs(model_curvature))) - curvature_sum = abs(near_curvature) + abs(far_curvature) - change_share = min(abs(far_curvature - near_curvature) / curvature_sum, 1.0) if curvature_sum > 0.0 else 0.0 - pose_share = 1.0 - (1.0 - magnitude_share) * (1.0 - change_share) + tracking_error = desired_curvature - current_curvature + if tracking_error * desired_curvature > 0.0: + tracking_error = math.copysign(max(abs(tracking_error) - _TRACKING_ERROR_DEADZONE, 0.0), tracking_error) + tracking_error = float(np.clip(tracking_error, -_TRACKING_ERROR_LIMIT, _TRACKING_ERROR_LIMIT)) + else: + tracking_error = 0.0 - # All fields describe the same current-frame rolling model path. C2 carries - # steady gentle curvature; changing or larger remaining poses transfer - # continuously to the faster C0/C1 fields. - path_offset = pose_share * model_offset - path_angle = pose_share * model_angle + # C2 owns normal path following. As model pose demand grows, transfer the + # same path continuously to the faster C0/C1 fields. Measured shortfall is + # 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) * horizon - curvature = model_curvature * (1.0 - pose_share) + 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)), @@ -97,7 +101,7 @@ def _encode_path(path: tuple[list[float], list[float], list[float], list[float]] class FordPathController: - """Encode one current-frame rolling model path as C0/C1/C2.""" + """Blend normal C2 following into the model's forward C0/C1 pose.""" def __init__(self, dt: float = 0.01): self.dt = dt @@ -120,11 +124,13 @@ class FordPathController: ) return self._last_path - def update(self, model, *, active: bool = True) -> FordPath: + def update(self, model, desired_curvature: float, *, current_curvature: float = 0.0, + v_ego: float = 0.0, active: bool = True) -> FordPath: if not active: self._last_path = FordPath(valid=True) return FordPath() path = _model_path(model) if model is not None else None if path is None: return self._limit(FordPath(valid=True)) - return self._limit(_encode_path(path)) + return self._limit(_encode_path(path, _finite(desired_curvature), _finite(current_curvature), + max(_finite(v_ego), 0.0))) diff --git a/openpilot/selfdrive/controls/tests/test_ford_path.py b/openpilot/selfdrive/controls/tests/test_ford_path.py index c6b7f24889..d4008a4b91 100644 --- a/openpilot/selfdrive/controls/tests/test_ford_path.py +++ b/openpilot/selfdrive/controls/tests/test_ford_path.py @@ -1,4 +1,3 @@ -import inspect import math from types import SimpleNamespace @@ -45,8 +44,8 @@ def _changing_path(start_curvature: float, end_curvature: float, speed: float = ) -def _command(model): - return FordPathController(dt=1.0).update(model) +def _command(model, desired_curvature: float, *, current_curvature: float = 0.0, v_ego: float = 8.0): + return FordPathController(dt=1.0).update(model, desired_curvature, current_curvature=current_curvature, v_ego=v_ego) def _equivalent_curvature(command) -> float: @@ -54,32 +53,26 @@ def _equivalent_curvature(command) -> float: def test_gentle_path_uses_only_c2(): - command = _command(_path(0.004, speed=20.0)) + command = _command(_path(0.004, speed=20.0), 0.004, v_ego=20.0) assert command.valid - assert np.isclose(command.path_offset, 0.0) - assert np.isclose(command.path_angle, 0.0) + assert command.path_offset == 0.0 + assert command.path_angle == 0.0 assert np.isclose(command.curvature, 0.004) assert command.curvature_rate == 0.0 -def test_spatially_growing_path_adds_fast_pose_before_average_curvature_becomes_large(): - command = _command(_changing_path(0.0, 0.04)) +def test_spatially_growing_path_adds_fast_pose_before_action_becomes_large(): + controller = FordPathController(dt=1.0) + command = controller.update(_changing_path(0.0, 0.04), 0.012, current_curvature=0.0, v_ego=8.0) assert command.path_offset > 0.0 assert command.path_angle > 0.0 assert command.curvature < 0.012 assert command.curvature_rate == 0.0 -def test_gentle_curvature_ramp_transfers_its_changing_share_out_of_c2(): - command = _command(_changing_path(0.0, 0.008)) - assert command.path_offset > 0.0 - assert command.path_angle > 0.0 - assert 0.0 < command.curvature < 0.004 - - def test_growing_model_pose_adds_authority_but_c3_is_never_transmitted(): - constant = _command(_path(0.012)) - growing = _command(_changing_path(0.0, 0.04)) + constant = _command(_path(0.012), 0.012) + growing = _command(_changing_path(0.0, 0.04), 0.012) assert growing.path_offset > constant.path_offset assert growing.path_angle > constant.path_angle assert constant.curvature_rate == 0.0 @@ -87,58 +80,59 @@ def test_growing_model_pose_adds_authority_but_c3_is_never_transmitted(): def test_large_maneuver_uses_fast_pose_and_zeros_c2(): - command = _command(_path(0.04)) + command = _command(_path(0.04), 0.04) assert command.path_offset > 0.5 assert command.path_angle > 0.2 assert command.curvature == 0.0 assert command.curvature_rate == 0.0 -def test_model_pose_alone_defines_the_maneuver(): - command = _command(_path(0.04)) +def test_model_pose_can_trigger_maneuver_when_action_is_late(): + command = _command(_path(0.04), 0.002) assert command.path_offset > 0.5 assert command.path_angle > 0.2 assert command.curvature == 0.0 -def test_gentle_model_path_remains_c2_only(): - command = _command(_path(0.002)) - assert np.isclose(command.path_offset, 0.0) - assert np.isclose(command.path_angle, 0.0) - assert np.isclose(command.curvature, 0.002) +def test_action_can_trigger_maneuver_before_model_pose_grows(): + command = _command(_path(0.002), 0.04) + assert command.path_offset > 0.0 + assert command.path_angle > 0.0 + assert command.curvature == 0.0 def test_nearby_demands_blend_continuously_without_a_mode_threshold(): - low = _command(_path(0.0119)) - high = _command(_path(0.0121)) + low = _command(_path(0.0119), 0.0119) + high = _command(_path(0.0121), 0.0121) assert abs(high.path_offset - low.path_offset) < 0.05 assert abs(high.path_angle - low.path_angle) < 0.03 assert abs(high.curvature - low.curvature) < 0.001 def test_leaving_c2_normal_band_does_not_drop_total_authority(): - normal = _command(_path(0.006)) - transition = _command(_path(0.0061)) + normal = _command(_path(0.006), 0.006) + transition = _command(_path(0.0061), 0.0061) assert transition.curvature <= normal.curvature assert _equivalent_curvature(transition) >= _equivalent_curvature(normal) def test_low_speed_still_uses_available_model_pose(): - command = _command(_path(0.04, speed=2.0)) + command = _command(_path(0.04, speed=2.0), 0.04, v_ego=2.0) assert command.path_offset > 0.0 assert command.path_angle > 0.0 -def test_current_frame_controller_has_no_measured_response_or_delay_inputs(): - parameters = inspect.signature(FordPathController.update).parameters - assert "current_curvature" not in parameters - assert "v_ego" not in parameters - assert "actuator_delay" not in parameters +def test_higher_speed_extends_heading_horizon_without_moving_offset_horizon(): + 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) + assert fast.path_angle > slow.path_angle def test_short_model_uses_available_endpoint(): model = _path(0.04, speed=1.0) - command = _command(model) + command = _command(model, 0.04, v_ego=1.0) assert command.valid assert command.path_offset > 0.0 assert command.path_angle > 0.0 @@ -147,8 +141,8 @@ def test_short_model_uses_available_endpoint(): def test_turn_entry_coordinates_c2_release_with_fast_pose_attack(): controller = FordPathController(dt=0.01) for _ in range(20): - assert controller.update(_path(0.004)).curvature > 0.0 - outputs = [controller.update(_path(0.04)) for _ in range(100)] + assert controller.update(_path(0.004), 0.004, v_ego=8.0).curvature > 0.0 + outputs = [controller.update(_path(0.04), 0.04, current_curvature=0.01, v_ego=8.0) for _ in range(100)] assert 0.0 < outputs[0].curvature < 0.004 assert outputs[0].path_offset > 0.0 assert outputs[0].path_angle > 0.0 @@ -158,33 +152,44 @@ def test_turn_entry_coordinates_c2_release_with_fast_pose_attack(): def test_turn_exit_allows_c2_to_take_over_while_fast_pose_drains(): controller = FordPathController(dt=0.01) for _ in range(20): - controller.update(_path(0.04)) - outputs = [controller.update(_path(0.004)) for _ in range(100)] + controller.update(_path(0.04), 0.04, current_curvature=0.02, v_ego=8.0) + outputs = [controller.update(_path(0.004), 0.004, current_curvature=0.004, v_ego=8.0) for _ in range(100)] assert 0.0 < outputs[0].curvature < 0.004 assert outputs[0].path_offset != 0.0 or outputs[0].path_angle != 0.0 - assert np.isclose(outputs[-1].path_offset, 0.0) - assert np.isclose(outputs[-1].path_angle, 0.0) + assert outputs[-1].path_offset == 0.0 + assert outputs[-1].path_angle == 0.0 def test_100hz_handoff_preserves_total_authority_without_entry_drop_or_exit_overshoot(): controller = FordPathController(dt=0.01) - normal = controller.update(_path(0.006)) - entries = [controller.update(_path(0.04)) for _ in range(100)] + normal = controller.update(_path(0.006), 0.006, current_curvature=0.006, v_ego=8.0) + entries = [controller.update(_path(0.04), 0.04, current_curvature=0.01, v_ego=8.0) for _ in range(100)] entry_authority = np.asarray([_equivalent_curvature(command) for command in entries]) assert np.all(np.diff(entry_authority) >= -1e-9) assert entry_authority[0] >= _equivalent_curvature(normal) - exits = [controller.update(_path(0.004)) for _ in range(100)] + exits = [controller.update(_path(0.004), 0.004, current_curvature=0.004, v_ego=8.0) for _ in range(100)] exit_authority = np.asarray([_equivalent_curvature(command) for command in exits]) assert np.all(np.diff(exit_authority) <= 1e-9) assert np.all(exit_authority >= 0.004 - 1e-9) +def test_measured_undertracking_adds_fast_authority_without_overshoot_countersteer(): + model = _path(0.04) + under = _command(model, 0.04, current_curvature=0.005) + on_target = _command(model, 0.04, current_curvature=0.04) + over = _command(model, 0.04, current_curvature=0.05) + assert under.path_offset > on_target.path_offset + assert under.path_angle > on_target.path_angle + assert over.path_offset == on_target.path_offset + assert over.path_angle == on_target.path_angle + + def test_s_turn_reverses_model_pose_without_slow_c2(): controller = FordPathController(dt=0.05) for _ in range(10): - controller.update(_path(0.04)) - outputs = [controller.update(_path(-0.04)) for _ in range(10)] + controller.update(_path(0.04), 0.04, v_ego=8.0) + outputs = [controller.update(_path(-0.04), -0.04, v_ego=8.0) for _ in range(10)] assert all(command.curvature == 0.0 for command in outputs) assert np.all(np.diff([command.path_offset for command in outputs]) < 0.0) assert np.all(np.diff([command.path_angle for command in outputs]) < 0.0) @@ -194,7 +199,7 @@ def test_s_turn_reverses_model_pose_without_slow_c2(): def test_output_limits_and_rates_are_bounded(): controller = FordPathController() - outputs = [controller.update(_path(0.2)) for _ in range(100)] + outputs = [controller.update(_path(0.2), 0.2, v_ego=8.0) for _ in range(100)] assert all(DBC_OFFSET[0] <= command.path_offset <= DBC_OFFSET[1] for command in outputs) assert all(DBC_ANGLE[0] <= command.path_angle <= DBC_ANGLE[1] for command in outputs) assert all(DBC_CURVATURE[0] <= command.curvature <= DBC_CURVATURE[1] for command in outputs) @@ -206,7 +211,7 @@ 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) + 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) @@ -220,12 +225,12 @@ def test_clipped_path_angle_uses_available_offset_to_preserve_endpoint(): def test_invalid_model_ramps_pose_to_zero_and_inactive_resets(): controller = FordPathController(dt=0.01) for _ in range(20): - active = controller.update(_path(0.04)) - invalid = controller.update(None) + active = controller.update(_path(0.04), 0.04, v_ego=8.0) + invalid = controller.update(None, 0.0, v_ego=8.0) assert invalid.valid assert abs(invalid.path_offset) < abs(active.path_offset) assert abs(invalid.path_angle) < abs(active.path_angle) - assert not controller.update(_path(0.0), active=False).valid + assert not controller.update(_path(0.0), 0.0, v_ego=8.0, active=False).valid def test_sunnypilot_path_message_round_trip():