diff --git a/openpilot/selfdrive/controls/controlsd.py b/openpilot/selfdrive/controls/controlsd.py index c5bf0db2f3..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, - self.desired_curvature, v_ego=CS.vEgo, 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 1a27e252bd..d4aabc4479 100644 --- a/openpilot/selfdrive/controls/lib/ford_path.py +++ b/openpilot/selfdrive/controls/lib/ford_path.py @@ -9,10 +9,10 @@ DBC_ANGLE = (-0.5, 0.5235) DBC_CURVATURE = (-0.02, 0.02) DBC_CURVATURE_RATE = (-0.001024, 0.001023) -_PATH_LOOKAHEAD_TIME = 1.0 _PATH_MIN_LOOKAHEAD = 7.0 -_MANEUVER_ENTER_CURVATURE = 0.018 -_MANEUVER_EXIT_CURVATURE = 0.012 +_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 @@ -34,6 +34,11 @@ def _sample(distance: float, distances: list[float], values: list[float]) -> flo return float(np.interp(distance, distances, values)) +def _blend_share(demand: float) -> float: + lower, upper = _POSE_BLEND_CURVATURE + return float(np.clip((demand - lower) / (upper - lower), 0.0, 1.0)) + + def _model_path(model) -> tuple[list[float], list[float], list[float]] | None: try: x = [float(value) for value in model.position.x] @@ -59,56 +64,71 @@ def _model_path(model) -> tuple[list[float], list[float], list[float]] | None: return distance, y, unwrapped_heading -def _encode_path(model_offset: float, model_angle: float, desired_curvature: float, maneuver: bool) -> FordPath: - action_curvature = _finite(desired_curvature) +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) + 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))) + + 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 + + # 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) + curvature = desired_curvature * (1.0 - pose_share) return FordPath( valid=True, - path_offset=float(np.clip(model_offset, *DBC_OFFSET)) if maneuver else 0.0, - path_angle=float(np.clip(model_angle, *DBC_ANGLE)) if maneuver else 0.0, - curvature=0.0 if maneuver else float(np.clip(action_curvature, *DBC_CURVATURE)), + path_offset=float(np.clip(path_offset, *DBC_OFFSET)), + path_angle=float(np.clip(path_angle, *DBC_ANGLE)), + curvature=float(np.clip(curvature, *DBC_CURVATURE)), curvature_rate=0.0, ) class FordPathController: - """Convert the model path directly into one vehicle-independent Ford path command.""" + """Blend normal C2 following into the model's forward C0/C1 pose.""" def __init__(self, dt: float = 0.01): self.dt = dt self._last_path = FordPath(valid=True) - self._maneuver = False def _limit(self, target: FordPath) -> FordPath: - path_offset = float(np.clip(target.path_offset, - self._last_path.path_offset - _PATH_OFFSET_RATE * self.dt, - self._last_path.path_offset + _PATH_OFFSET_RATE * self.dt)) - path_angle = float(np.clip(target.path_angle, - self._last_path.path_angle - _PATH_ANGLE_RATE * self.dt, - self._last_path.path_angle + _PATH_ANGLE_RATE * self.dt)) - # Never stack the normal C2 path on a maneuver pose while C0/C1 are slewing out. - curvature = target.curvature if path_offset == 0.0 and path_angle == 0.0 else 0.0 - self._last_path = FordPath(True, path_offset, path_angle, curvature, 0.0) + offset_delta = target.path_offset - self._last_path.path_offset + angle_delta = target.path_angle - self._last_path.path_angle + scale = min( + 1.0, + _PATH_OFFSET_RATE * self.dt / abs(offset_delta) if offset_delta else 1.0, + _PATH_ANGLE_RATE * self.dt / abs(angle_delta) if angle_delta else 1.0, + ) + self._last_path = FordPath( + True, + self._last_path.path_offset + scale * offset_delta, + self._last_path.path_angle + scale * angle_delta, + self._last_path.curvature + scale * (target.curvature - self._last_path.curvature), + 0.0, + ) return self._last_path - def update(self, model, desired_curvature: float, *, v_ego: float = 0.0, 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) - self._maneuver = False return FordPath() path = _model_path(model) if model is not None else None if path is None: - self._maneuver = False return self._limit(FordPath(valid=True)) - - distance, offset, heading = path - lookahead = min(max(_finite(v_ego) * _PATH_LOOKAHEAD_TIME, _PATH_MIN_LOOKAHEAD), distance[-1]) - model_offset = _sample(lookahead, distance, offset) - model_angle = _sample(lookahead, distance, heading) - demand = max(abs(_finite(desired_curvature)), abs(model_angle / lookahead)) - if self._maneuver: - self._maneuver = demand >= _MANEUVER_EXIT_CURVATURE - else: - self._maneuver = demand >= _MANEUVER_ENTER_CURVATURE - - return self._limit(_encode_path(model_offset, model_angle, desired_curvature, self._maneuver)) + 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 c41191d6f6..43bd6f517e 100644 --- a/openpilot/selfdrive/controls/tests/test_ford_path.py +++ b/openpilot/selfdrive/controls/tests/test_ford_path.py @@ -25,35 +25,66 @@ def _path(curvature: float, speed: float = 8.0): ) -def _model_pose(model, lookahead: float) -> tuple[float, float]: - x = np.asarray(model.position.x) - y = np.asarray(model.position.y) - heading = np.asarray(model.orientation.z) - distance = np.concatenate(([0.0], np.cumsum(np.hypot(np.diff(x), np.diff(y))))) - horizon = min(lookahead, distance[-1]) - return float(np.interp(horizon, distance, y)), float(np.interp(horizon, distance, heading)) +def _changing_path(start_curvature: float, end_curvature: float, speed: float = 8.0): + t = np.linspace(0.0, 3.0, 61) + distance = speed * t + curvature = np.interp(distance, [distance[0], min(distance[-1], 7.0)], [start_curvature, end_curvature]) + heading = np.zeros_like(distance) + x = np.zeros_like(distance) + y = np.zeros_like(distance) + for i in range(1, len(distance)): + ds = distance[i] - distance[i - 1] + heading[i] = heading[i - 1] + 0.5 * (curvature[i] + curvature[i - 1]) * ds + average_heading = 0.5 * (heading[i] + heading[i - 1]) + x[i] = x[i - 1] + ds * math.cos(average_heading) + y[i] = y[i - 1] + ds * math.sin(average_heading) + return SimpleNamespace( + position=SimpleNamespace(t=t.tolist(), x=x.tolist(), y=y.tolist()), + orientation=SimpleNamespace(z=heading.tolist()), + ) -def _command(model, desired_curvature: float, *, v_ego: float = 8.0): - return FordPathController(dt=1.0).update(model, desired_curvature, v_ego=v_ego) +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: + return 2.0 * command.path_offset / 7.0 ** 2 + 2.0 * command.path_angle / 7.0 + command.curvature def test_gentle_path_uses_only_c2(): - command = _command(_path(0.008, speed=20.0), 0.008, v_ego=20.0) + command = _command(_path(0.004, speed=20.0), 0.004, v_ego=20.0) assert command.valid assert command.path_offset == 0.0 assert command.path_angle == 0.0 - assert np.isclose(command.curvature, 0.008) + assert np.isclose(command.curvature, 0.004) assert command.curvature_rate == 0.0 -def test_large_maneuver_uses_model_pose_and_immediately_zeros_c2(): - model = _path(0.04) - expected_offset, expected_angle = _model_pose(model, 8.0) - command = _command(model, 0.04) - assert np.isclose(command.path_offset, expected_offset) - assert np.isclose(command.path_angle, expected_angle) +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_growing_model_pose_adds_authority_but_c3_is_never_transmitted(): + 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 + assert growing.curvature_rate == 0.0 + + +def test_large_maneuver_uses_fast_pose_and_zeros_c2(): + 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_can_trigger_maneuver_when_action_is_late(): @@ -70,78 +101,88 @@ def test_action_can_trigger_maneuver_before_model_pose_grows(): assert command.curvature == 0.0 -def test_large_offset_does_not_trigger_maneuver_without_heading_or_action_demand(): - model = _path(0.008) - model.position.y = (np.asarray(model.position.y) + np.linspace(0.0, 4.0, len(model.position.y))).tolist() - command = _command(model, 0.008) - assert command.path_offset == 0.0 - assert command.path_angle == 0.0 - assert np.isclose(command.curvature, 0.008) +def test_nearby_demands_blend_continuously_without_a_mode_threshold(): + 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_maneuver_hysteresis_prevents_mode_chatter(): - controller = FordPathController(dt=1.0) - entry = controller.update(_path(0.02), 0.02, v_ego=8.0) - held = controller.update(_path(0.015), 0.015, v_ego=8.0) - exited = controller.update(_path(0.01), 0.01, v_ego=8.0) - assert entry.path_angle != 0.0 and entry.curvature == 0.0 - assert held.path_angle != 0.0 and held.curvature == 0.0 - assert exited.path_offset == 0.0 and exited.path_angle == 0.0 - assert np.isclose(exited.curvature, 0.01) +def test_leaving_c2_normal_band_does_not_drop_total_authority(): + 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_uses_seven_meter_lookahead_for_both_pose_fields(): - model = _path(0.04, speed=2.0) - expected_offset, expected_angle = _model_pose(model, 7.0) - command = _command(model, 0.04, v_ego=2.0) - assert np.isclose(command.path_offset, expected_offset) - assert np.isclose(command.path_angle, expected_angle) +def test_low_speed_still_uses_available_model_pose(): + 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_speed_uses_one_second_lookahead_for_both_pose_fields(): - model = _path(0.02, speed=15.0) - expected_offset, expected_angle = _model_pose(model, 15.0) - command = _command(model, 0.02, v_ego=15.0) - assert np.isclose(command.path_offset, expected_offset) - assert np.isclose(command.path_angle, expected_angle) +def test_higher_speed_extends_heading_horizon_without_moving_offset_horizon(): + model = _changing_path(0.0, 0.04, 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) - expected_offset, expected_angle = _model_pose(model, math.inf) command = _command(model, 0.04, v_ego=1.0) - assert np.isclose(command.path_offset, expected_offset) - assert np.isclose(command.path_angle, expected_angle) + assert command.valid + assert command.path_offset > 0.0 + assert command.path_angle > 0.0 -def test_turn_entry_does_not_retain_previous_gentle_c2(): - controller = FordPathController() - for _ in range(20): - assert controller.update(_path(0.004), 0.004, v_ego=8.0).curvature > 0.0 - turning = controller.update(_path(0.04), 0.04, v_ego=8.0) - assert turning.curvature == 0.0 - - -def test_turn_exit_never_stacks_pose_and_c2(): +def test_turn_entry_coordinates_c2_release_with_fast_pose_attack(): controller = FordPathController(dt=0.01) for _ in range(20): - controller.update(_path(0.04), 0.04, v_ego=8.0) - outputs = [controller.update(_path(0.008), 0.008, v_ego=8.0) for _ in range(100)] - assert all(command.curvature == 0.0 or (command.path_offset == 0.0 and command.path_angle == 0.0) for command in outputs) - assert np.isclose(outputs[-1].curvature, 0.008) + 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 + assert outputs[-1].curvature == 0.0 -def test_inactive_and_invalid_model_reset_maneuver_latch(): - controller = FordPathController(dt=1.0) - controller.update(_path(0.02), 0.02, v_ego=8.0) - controller.update(None, 0.0, v_ego=8.0) - after_invalid = controller.update(_path(0.015), 0.015, v_ego=8.0) - assert after_invalid.path_angle == 0.0 and np.isclose(after_invalid.curvature, 0.015) +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), 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 outputs[-1].path_offset == 0.0 + assert outputs[-1].path_angle == 0.0 - controller.update(_path(0.02), 0.02, v_ego=8.0) - controller.update(_path(0.0), 0.0, v_ego=8.0, active=False) - after_inactive = controller.update(_path(0.015), 0.015, v_ego=8.0) - assert after_inactive.path_angle == 0.0 and np.isclose(after_inactive.curvature, 0.015) + +def test_100hz_handoff_preserves_total_authority_without_entry_drop_or_exit_overshoot(): + controller = FordPathController(dt=0.01) + 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), 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():