mirror of
https://github.com/firestar5683/StarPilot.git
synced 2026-11-08 13:33:42 +08:00
firestar5683
96 lines
4.1 KiB
Python
96 lines
4.1 KiB
Python
"""Planner authority and bounded smoothing across operating points."""
|
|
import math
|
|
|
|
import numpy as np
|
|
import pytest
|
|
|
|
from opendbc.car.ford.values import CAR
|
|
|
|
from openpilot.starpilot.car.ford.starpilot_controller import CurvatureConditioner
|
|
from openpilot.starpilot.tests.test_ford_starpilot_angle import controller, set_curve
|
|
|
|
|
|
@pytest.mark.parametrize('speed', [5., 15., 25., 35.])
|
|
@pytest.mark.parametrize('center', [-2., -.9, -.6, -.4, -.2, 0., .2, .4, .6, .9, 2.])
|
|
@pytest.mark.parametrize('frequency', [.25, .5, 1., 2., 4.])
|
|
def test_unclipped_sweeper_ripple_preserves_mean(speed, center, frequency):
|
|
time = np.arange(0., 20., .05)
|
|
target = (center + .04 * np.sin(2. * np.pi * frequency * time)) / speed ** 2
|
|
filt = CurvatureConditioner()
|
|
output = np.array([filt.update(float(value), speed) for value in target])
|
|
settled = time >= 12.
|
|
assert abs(float(np.mean(output[settled] - target[settled]))) * speed ** 2 < 1e-10
|
|
assert np.max(np.abs(output - target)) * speed ** 2 <= .04 + 1e-12
|
|
assert np.std(output[settled]) < np.std(target[settled])
|
|
wave = np.exp(-2j * np.pi * frequency * time[settled])
|
|
response = np.sum((output[settled] - center / speed ** 2) * wave) / np.sum((target[settled] - center / speed ** 2) * wave)
|
|
expected = .5 / (1. - .5 * np.exp(-2j * np.pi * frequency * .05))
|
|
assert response == pytest.approx(expected, abs=1e-10)
|
|
|
|
|
|
@pytest.mark.parametrize('sign', [-1., 1.])
|
|
@pytest.mark.parametrize('speed', [5., 15., 25., 35.])
|
|
def test_smoothing_step_and_exit_are_bounded_without_overshoot(sign, speed):
|
|
filt = CurvatureConditioner()
|
|
filt.update(0., speed)
|
|
target = sign * 1.5 / speed ** 2
|
|
previous = 0.
|
|
for _ in range(40):
|
|
output = filt.update(target, speed)
|
|
assert min(previous, target) <= output <= max(previous, target)
|
|
assert abs(output - target) * speed ** 2 <= .04 + 1e-12
|
|
previous = output
|
|
assert output == pytest.approx(target, abs=1e-12)
|
|
for magnitude in np.linspace(1.5, .15, 40):
|
|
target = sign * float(magnitude) / speed ** 2
|
|
output = filt.update(target, speed)
|
|
assert min(previous, target) <= output <= max(previous, target)
|
|
assert abs(output - target) * speed ** 2 <= .04 + 1e-12
|
|
previous = output
|
|
assert filt.update(0., speed) == 0.
|
|
assert filt.update(-sign * 1.5 / speed ** 2, speed) == -sign * 1.5 / speed ** 2
|
|
|
|
|
|
def test_speed_changes_do_not_expand_request_error_budget():
|
|
filt = CurvatureConditioner()
|
|
for index in range(1000):
|
|
speed = 1. + 34. * (1. + math.sin(index / 7.)) / 2.
|
|
target = .003 * math.sin(index / 13.)
|
|
output = filt.update(target, speed)
|
|
assert math.isfinite(output)
|
|
assert abs(output - target) * max(speed, 1.) ** 2 <= .04 + 1e-12
|
|
|
|
|
|
@pytest.mark.parametrize('speed', [5., 15., 25., 35.])
|
|
@pytest.mark.parametrize('desired', [-.001, 0., .001])
|
|
@pytest.mark.parametrize('forecast', [-.01, 0., .01])
|
|
@pytest.mark.parametrize('car', [CAR.FORD_ESCAPE_MK4, CAR.FORD_F_150_MK14, CAR.FORD_MUSTANG_MACH_E_MK1])
|
|
@pytest.mark.parametrize('pinion', [False, True])
|
|
def test_forecast_disagreement_cannot_cancel_or_create_planner_demand(speed, desired, forecast, car, pinion):
|
|
angle, cc, cs = controller(speed=speed, car=car, pinion=pinion)
|
|
set_curve(angle, cc, cs, desired, model=forecast)
|
|
for _ in range(30):
|
|
result = angle.update_angle_strategy(cc, cs, cc.actuators)
|
|
assert angle.bp_kappa_cmd == cc.actuators.curvature
|
|
assert angle.bp_curvature_lookup_time == 0.
|
|
if desired == 0.:
|
|
assert result.path_angle == 0.
|
|
else:
|
|
assert result.path_angle * desired > 0.
|
|
|
|
|
|
@pytest.mark.parametrize('sign', [-1., 1.])
|
|
def test_planner_correction_has_unity_steady_gain_before_native_limits(sign):
|
|
angle, cc, cs = controller(speed=20.)
|
|
base, correction = sign * .001, sign * .0002
|
|
set_curve(angle, cc, cs, base, model=base)
|
|
for _ in range(30):
|
|
angle.update_angle_strategy(cc, cs, cc.actuators)
|
|
baseline = angle.bp_kappa_cmd
|
|
base_request = cc.actuators.curvature
|
|
set_curve(angle, cc, cs, base + correction, model=base)
|
|
for _ in range(40):
|
|
angle.update_angle_strategy(cc, cs, cc.actuators)
|
|
assert angle.bp_kappa_cmd - baseline == pytest.approx(cc.actuators.curvature - base_request, abs=1e-12)
|
|
assert not angle.enable_lane_positioning_ang
|