mirror of
https://github.com/firestar5683/StarPilot.git
synced 2026-11-08 13:33:42 +08:00
firestar5683
382 lines
18 KiB
Python
382 lines
18 KiB
Python
"""Angle-primary candidate: bounded conditioning and unchanged native contracts."""
|
|
import math
|
|
from types import SimpleNamespace
|
|
|
|
import numpy as np
|
|
import pytest
|
|
|
|
from opendbc.bluepilot_lateral.core.carcontroller_ext import InputTracker, SharedLateralState
|
|
from opendbc.bluepilot_lateral.core.lateral_angle_ext import LateralAngleExt
|
|
from opendbc.bluepilot_lateral.hosts.starpilot.safety_param import bp_safety_param, init_ford_safety_param_starpilot
|
|
from opendbc.car import gen_empty_fingerprint, structs
|
|
from opendbc.car.ford.interface import CarInterface
|
|
from opendbc.car.ford.tests.test_bluepilot_lateral import StraightRoadInputs, STARTUP, settings
|
|
from opendbc.car.ford.values import CAR
|
|
from openpilot.starpilot.car.ford.starpilot_controller import CurvatureConditioner, StarPilotFordController
|
|
|
|
|
|
def controller(kind=StarPilotFordController, speed=20.0, pinion=False, car=CAR.FORD_MUSTANG_MACH_E_MK1):
|
|
cp = CarInterface.get_params(car, gen_empty_fingerprint(), [], False, False, False)
|
|
init_ford_safety_param_starpilot(cp, STARTUP['on+pinion' if pinion else 'on'])
|
|
inputs = InputTracker(cp, bp_safety_param(cp))
|
|
inputs.update(StraightRoadInputs(settings(STARTUP['on'], True)).update().lateralInputs)
|
|
angle = kind(cp, inputs, SharedLateralState())
|
|
angle.update_angle_params(None)
|
|
state = structs.CarState(vEgoRaw=speed, vEgo=speed, canValid=True)
|
|
cs = SimpleNamespace(out=state, lat_ctl_lim_stat=0)
|
|
cc = structs.CarControl(latActive=True)
|
|
return angle, cc, cs
|
|
|
|
|
|
def set_curve(angle, cc, cs, curvature, model=None):
|
|
cc.actuators.curvature = curvature
|
|
cs.out.yawRate = -curvature * cs.out.vEgoRaw
|
|
angle.inputs.model.orientationRate.z = [(curvature if model is None else model) * cs.out.vEgoRaw] * 33
|
|
|
|
|
|
def test_small_signal_smoothing_has_no_dc_bias_or_unbounded_lag():
|
|
filt = CurvatureConditioner()
|
|
targets = np.array([.0003 + (-1) ** i * .0001 for i in range(200)])
|
|
outputs = np.array([filt.update(float(x), 25.) for x in targets])
|
|
assert np.mean(np.abs(np.diff(outputs[20:]))) < .4 * np.mean(np.abs(np.diff(targets[20:])))
|
|
assert np.max(np.abs(outputs - targets)) * 25. ** 2 <= .04 + 1e-12
|
|
for _ in range(100):
|
|
output = filt.update(.0003, 25.)
|
|
assert output == pytest.approx(.0003, abs=1e-12)
|
|
|
|
|
|
@pytest.mark.parametrize('sign', [-1., 1.])
|
|
def test_unwind_is_bounded_and_reversal_or_zero_clears_filter_history(sign):
|
|
filt = CurvatureConditioner()
|
|
filt.update(sign * .002, 20.)
|
|
assert abs(filt.update(sign * .001, 20.) - sign * .001) * 20. ** 2 <= .04 + 1e-12
|
|
assert filt.update(-sign * .001, 20.) == -sign * .001
|
|
assert filt.update(0., 20.) == 0.
|
|
|
|
|
|
def test_large_curve_has_bounded_lag_and_driver_lane_change_bypass_filter():
|
|
filt = CurvatureConditioner()
|
|
filt.update(0., 20.)
|
|
assert abs(filt.update(.004, 20.) - .004) * 20. ** 2 <= .04 + 1e-12
|
|
assert filt.update(.0001, 20., bypass=True) == .0001
|
|
filt.reset()
|
|
assert filt.update(-.0002, 20.) == -.0002
|
|
|
|
|
|
@pytest.mark.parametrize('speed', [3., 10., 20., 30.])
|
|
def test_passthrough_conditioner_matches_planner_primary_pinned_limit_chain(speed):
|
|
candidate, cc, cs = controller(speed=speed)
|
|
baseline, baseline_cc, baseline_cs = controller(LateralAngleExt, speed=speed)
|
|
candidate.conditioner.update = lambda target, speed, bypass=False: target
|
|
baseline.path_angle_blend_ratio = 0.0
|
|
for i in range(300):
|
|
curve = .004 * math.sin(i / 30.)
|
|
set_curve(candidate, cc, cs, curve)
|
|
set_curve(baseline, baseline_cc, baseline_cs, curve)
|
|
cc.latActive = baseline_cc.latActive = i >= 10 and not 130 <= i < 140
|
|
cs.out.steeringPressed = baseline_cs.out.steeringPressed = 150 <= i < 220
|
|
cs.out.steeringAngleDeg = baseline_cs.out.steeringAngleDeg = 60. if 155 <= i < 220 else 0.
|
|
actual = candidate.update_angle_strategy(cc, cs, cc.actuators)
|
|
expected = baseline.update_angle_strategy(baseline_cc, baseline_cs, baseline_cc.actuators)
|
|
assert actual == expected, i
|
|
assert candidate.bp_kappa_cmd == baseline.bp_kappa_cmd
|
|
assert candidate.angle_human_turn_active == baseline.angle_human_turn_active
|
|
assert candidate.angle_stall_blip_active == baseline.angle_stall_blip_active
|
|
|
|
|
|
def test_conditioned_shadow_stays_in_measured_envelope():
|
|
angle, cc, cs = controller()
|
|
for i in range(200):
|
|
set_curve(angle, cc, cs, .0003 + .0002 * math.sin(i))
|
|
cs.out.yawRate = 0.
|
|
angle.update_angle_strategy(cc, cs, cc.actuators)
|
|
assert abs(angle.bp_kappa_cmd) <= .002 + 1e-12
|
|
assert angle.conditioner.curvature == angle.bp_kappa_cmd
|
|
|
|
|
|
def test_override_inactive_and_pulse_clear_conditioner():
|
|
angle, cc, cs = controller()
|
|
set_curve(angle, cc, cs, .0003)
|
|
angle.update_angle_strategy(cc, cs, cc.actuators)
|
|
cs.out.steeringPressed = True
|
|
angle.update_angle_strategy(cc, cs, cc.actuators)
|
|
assert angle.conditioner.curvature is None
|
|
cs.out.steeringPressed = False
|
|
angle.stall_blip_frames_left = 6
|
|
for _ in range(6):
|
|
result = angle.update_angle_strategy(cc, cs, cc.actuators)
|
|
assert result.path_angle == 0. and angle.conditioner.curvature is None
|
|
cc.latActive = False
|
|
angle.update_angle_strategy(cc, cs, cc.actuators)
|
|
assert angle.conditioner.curvature is None and angle.shared.path_angle_last == 0.
|
|
|
|
|
|
@pytest.mark.parametrize('field,value', [
|
|
('vEgoRaw', float('nan')), ('yawRate', float('nan')), ('steeringAngleDeg', float('nan')),
|
|
('steeringTorque', float('nan')), ('canValid', False), ('canTimeout', True),
|
|
('vehicleSensorsInvalid', True), ('steerFaultTemporary', True), ('steerFaultPermanent', True),
|
|
])
|
|
def test_invalid_vehicle_inputs_zero_demand_and_shadow_remains_finite(field, value):
|
|
angle, cc, cs = controller()
|
|
setattr(cs.out, field, value)
|
|
assert not angle.inputs_valid(cs, cc.actuators)
|
|
result = angle.update_angle_strategy(cc, cs, cc.actuators)
|
|
assert result.path_angle == result.apply_curvature == 0.
|
|
assert math.isfinite(angle.bp_kappa_cmd)
|
|
|
|
|
|
def test_missing_model_or_nonfinite_demand_cannot_activate():
|
|
angle, cc, cs = controller()
|
|
angle.inputs.model = None
|
|
assert not angle.inputs_valid(cs, cc.actuators)
|
|
assert angle.update_angle_strategy(cc, cs, cc.actuators).path_angle == 0.
|
|
angle, cc, cs = controller()
|
|
cc.actuators.curvature = float('nan')
|
|
assert not angle.inputs_valid(cs, cc.actuators)
|
|
assert angle.update_angle_strategy(cc, cs, cc.actuators).path_angle == 0.
|
|
|
|
|
|
@pytest.mark.parametrize('car', [CAR.FORD_BRONCO_SPORT_MK1, CAR.FORD_F_150_MK14])
|
|
@pytest.mark.parametrize('startup', ['on', 'on+pinion'])
|
|
def test_candidate_packets_pass_existing_end_to_end_native_check(car, startup, monkeypatch):
|
|
from opendbc.car.ford.tests import test_bluepilot_lateral as original
|
|
from openpilot.starpilot.car.ford.bluepilot import FordBluePilotController
|
|
from openpilot.starpilot.car.ford.controller_selection import FordController
|
|
original_factory = original.CarController
|
|
|
|
def candidate_factory(dbc_names, cp):
|
|
output = original_factory(dbc_names, cp)
|
|
source = SimpleNamespace(controller_choice=FordController.STARPILOT, current_inputs_valid=True)
|
|
output.bp = FordBluePilotController(cp, output.packer, output.CAN, source)
|
|
return output
|
|
|
|
monkeypatch.setattr(original, 'CarController', candidate_factory)
|
|
original.test_every_steering_frame_passes_panda_safety(car, startup, True)
|
|
|
|
|
|
def test_candidate_host_packs_inactive_on_invalid_inputs():
|
|
from opendbc.car.ford.carcontroller import CarController
|
|
from opendbc.car.ford.values import DBC
|
|
from openpilot.starpilot.car.ford.bluepilot import FordBluePilotController
|
|
from openpilot.starpilot.car.ford.controller_selection import FordController
|
|
angle, cc, cs = controller()
|
|
cp = angle.CP
|
|
car = CarController(DBC[cp.carFingerprint], cp)
|
|
source = SimpleNamespace(controller_choice=FordController.STARPILOT, current_inputs_valid=True)
|
|
host = FordBluePilotController(cp, car.packer, car.CAN, source)
|
|
host.update(StraightRoadInputs(settings(STARTUP['on'], True)).update(), 0)
|
|
cc.actuators.curvature = .002
|
|
cs.out.canTimeout = True
|
|
packets, applied = host.step_lateral(cc, cs, cc.actuators, 0.)
|
|
assert applied == 0. and host.angle.bp_path_angle_final == 0.
|
|
for address, data, _ in packets:
|
|
if address == 0x3D6:
|
|
assert (data[0] >> 4) & 7 == 0
|
|
elif address == 0x3D3:
|
|
assert data[4] & 4 == 0
|
|
|
|
|
|
def test_small_signal_filter_is_left_right_symmetric():
|
|
left, right = CurvatureConditioner(), CurvatureConditioner()
|
|
for i in range(300):
|
|
target = .0004 * math.sin(i / 3.)
|
|
assert left.update(target, 25.) == -right.update(-target, 25.)
|
|
|
|
|
|
def test_candidate_uses_platform_defaults_not_hidden_bluepilot_tuning():
|
|
from opendbc.bluepilot_lateral.params.param_store import ParamStore
|
|
angle, _, _ = controller()
|
|
angle.update_angle_params(ParamStore([('FordHighSpeedFactor_ang', b'1.5'), ('enable_lane_positioning_ang', b'1')]))
|
|
assert angle.high_speed_curv_factor == 1.
|
|
assert not angle.enable_lane_positioning_ang
|
|
|
|
|
|
@pytest.mark.parametrize('phase', range(5))
|
|
@pytest.mark.parametrize('origin,target', [(0, 2), (1, 2), (2, 0), (2, 1)])
|
|
def test_controller_changes_wait_for_inactive_frame_and_preserve_native_shadow(origin, target, phase):
|
|
from opendbc.can import CANPacker
|
|
from opendbc.car.ford.fordcan import CanBus
|
|
from opendbc.safety.tests.libsafety import libsafety_py
|
|
from openpilot.starpilot.car.ford.bluepilot import FordBluePilotController
|
|
from openpilot.starpilot.car.ford.controller_selection import FordController
|
|
angle, cc, cs = controller()
|
|
cp, speed, curvature = angle.CP, 20., .004
|
|
packer = CANPacker('ford_lincoln_base_pt')
|
|
source = SimpleNamespace(controller_choice=FordController(origin), current_inputs_valid=True)
|
|
host = FordBluePilotController(cp, packer, CanBus(cp), source)
|
|
road = StraightRoadInputs(settings(STARTUP['on'], origin != 0))
|
|
road.curvature = curvature
|
|
cs.out.yawRate = -curvature * speed
|
|
cc.actuators.curvature = curvature
|
|
safety = libsafety_py.libsafety
|
|
safety.init_tests()
|
|
safety.set_alternative_experience(0)
|
|
assert safety.set_safety_hooks(structs.CarParams.SafetyModel.ford, int(cp.safetyConfigs[-1].safetyParam)) == 0
|
|
safety.set_controls_allowed(True)
|
|
applied = 0.
|
|
try:
|
|
for frame in range(100):
|
|
safety.set_timer(1_000_000 + frame * 10_000)
|
|
messages: list[tuple[str, dict[str, float]]] = [
|
|
('BrakeSysFeatures', {'Veh_V_ActlBrk': speed * 3.6, 'VehVActlBrk_D_Qf': 3, 'VehVActlBrk_No_Cnt': (frame // 2) % 16}),
|
|
('EngVehicleSpThrottle2', {'Veh_V_ActlEng': speed * 3.6, 'VehVActlEng_D_Qf': 3}),
|
|
('Yaw_Data_FD1', {'VehYaw_W_Actl': -curvature * speed, 'VehYawWActl_D_Qf': 3, 'VehRollYaw_No_Cnt': frame % 256}),
|
|
('DesiredTorqBrk', {'VehStop_D_Stat': 0}),
|
|
('EngVehicleSpThrottle', {'ApedPos_Pc_ActlArb': 0}),
|
|
('EngBrakeData', {'BpedDrvAppl_D_Actl': 1, 'CcStat_D_Actl': 5}),
|
|
]
|
|
for name, values in messages:
|
|
if name in ('BrakeSysFeatures', 'EngVehicleSpThrottle2') and frame % 2:
|
|
continue
|
|
address, data, bus = packer.make_can_msg(name, 0, values)
|
|
assert safety.safety_rx_hook(libsafety_py.make_CANPacket(address, bus, data))
|
|
if frame == 40 + phase:
|
|
source.controller_choice = FordController(target)
|
|
road.settings['FordPrefLateralControl'] = b'0' if target == 0 else b'1'
|
|
host.update(road.update(), frame)
|
|
if 40 + phase <= frame <= 60:
|
|
assert host.starpilot_selected == (origin == 2)
|
|
cc.latActive = 15 <= frame < 60 or frame >= 75
|
|
packets = []
|
|
if frame % 5 == 0:
|
|
sends, applied = host.step_lateral(cc, cs, cc.actuators, applied)
|
|
packets.extend(sends)
|
|
if frame % 3 == 0:
|
|
packets.append(host.step_lka(cc, cc.hudControl))
|
|
for address, data, bus in packets:
|
|
assert safety.safety_tx_hook(libsafety_py.make_CANPacket(address, bus, data)), (frame, hex(address), data.hex())
|
|
assert host.starpilot_selected == (target == 2)
|
|
finally:
|
|
safety.set_safety_hooks(structs.CarParams.SafetyModel.noOutput, 0)
|
|
|
|
|
|
@pytest.mark.parametrize('invalid', ['shape', 'nonfinite', 'flag', 'freshness'])
|
|
def test_new_invalid_frame_cannot_reuse_cached_turn_intent(invalid):
|
|
from opendbc.can import CANPacker
|
|
from opendbc.car.ford.fordcan import CanBus
|
|
from openpilot.starpilot.car.ford.bluepilot import FordBluePilotController
|
|
from openpilot.starpilot.car.ford.controller_selection import FordController
|
|
angle, cc, cs = controller()
|
|
source = SimpleNamespace(controller_choice=FordController.STARPILOT, current_inputs_valid=True)
|
|
host = FordBluePilotController(angle.CP, CANPacker('ford_lincoln_base_pt'), CanBus(angle.CP), source)
|
|
road = StraightRoadInputs(settings(STARTUP['on'], True))
|
|
road.curvature = cc.actuators.curvature = .002
|
|
host.update(road.update(), 0)
|
|
host.step_lateral(cc, cs, cc.actuators, 0.)
|
|
frame = road.update()
|
|
if invalid == 'shape':
|
|
frame.lateralInputs.pathYawRate = []
|
|
elif invalid == 'nonfinite':
|
|
frame.lateralInputs.pathYawRate[0] = float('nan')
|
|
elif invalid == 'flag':
|
|
frame.lateralInputs.valid = False
|
|
else:
|
|
source.current_inputs_valid = False
|
|
host.update(frame, 5)
|
|
assert host.inputs.model is not None # Upstream correctly retains its previous complete snapshot.
|
|
packets, _ = host.step_lateral(cc, cs, cc.actuators, 0.)
|
|
assert host.angle.conditioner.curvature is None
|
|
assert host.angle.bp_path_angle_final == 0.
|
|
assert all((data[0] >> 4) & 7 == 0 for address, data, _ in packets if address == 0x3D6)
|
|
|
|
|
|
def test_unselected_bluepilot_angle_packets_are_unchanged():
|
|
from opendbc.can import CANPacker
|
|
from opendbc.car.ford.fordcan import CanBus
|
|
from opendbc.bluepilot_lateral.hosts.starpilot.lateral import BluePilotLateral
|
|
from openpilot.starpilot.car.ford.bluepilot import FordBluePilotController
|
|
from openpilot.starpilot.car.ford.controller_selection import FordController
|
|
angle, cc, cs = controller()
|
|
packer, can = CANPacker('ford_lincoln_base_pt'), CanBus(angle.CP)
|
|
source = SimpleNamespace(controller_choice=FordController.BLUEPILOT_ANGLE, current_inputs_valid=True)
|
|
baseline = BluePilotLateral(angle.CP, packer, can)
|
|
host = FordBluePilotController(angle.CP, packer, can, source)
|
|
road = StraightRoadInputs(settings(STARTUP['on'], True))
|
|
for frame in range(100):
|
|
road.curvature = cc.actuators.curvature = .003 * math.sin(frame / 20.)
|
|
cs.out.yawRate = -road.curvature * cs.out.vEgoRaw
|
|
inputs = road.update()
|
|
baseline.update(inputs, frame)
|
|
host.update(inputs, frame)
|
|
if frame % 5 == 0:
|
|
assert baseline.step_lateral(cc, cs, cc.actuators, 0.) == host.step_lateral(cc, cs, cc.actuators, 0.)
|
|
if frame % 3 == 0:
|
|
assert baseline.step_lka(cc, cc.hudControl) == host.step_lka(cc, cc.hudControl)
|
|
|
|
|
|
@pytest.mark.parametrize('speed', [3., 12., 30.])
|
|
@pytest.mark.parametrize('pinion', [False, True])
|
|
@pytest.mark.parametrize('curvature', [0., .0003])
|
|
def test_ordinary_override_release_does_not_reset_steering(speed, pinion, curvature):
|
|
angle, cc, cs = controller(speed=speed, pinion=pinion)
|
|
set_curve(angle, cc, cs, curvature)
|
|
for tick in range(50):
|
|
cs.out.steeringPressed = 10 <= tick < 30
|
|
result = angle.update_angle_strategy(cc, cs, cc.actuators)
|
|
assert not angle.angle_human_turn_active
|
|
assert not angle.angle_stall_blip_active
|
|
assert angle.stall_blip_frames_left == angle.stall_blip_count == 0
|
|
assert math.isfinite(result.path_angle)
|
|
|
|
|
|
def test_natural_deviation_recovery_remains_bounded_and_native_admitted():
|
|
from opendbc.can import CANPacker
|
|
from opendbc.car.ford.fordcan import CanBus
|
|
from opendbc.safety.tests.libsafety import libsafety_py
|
|
from openpilot.starpilot.car.ford.bluepilot import FordBluePilotController
|
|
from openpilot.starpilot.car.ford.controller_selection import FordController
|
|
angle, cc, cs = controller(speed=12.)
|
|
cp, speed = angle.CP, 12.
|
|
packer = CANPacker('ford_lincoln_base_pt')
|
|
source = SimpleNamespace(controller_choice=FordController.STARPILOT, current_inputs_valid=True)
|
|
host = FordBluePilotController(cp, packer, CanBus(cp), source)
|
|
road = StraightRoadInputs(settings(STARTUP['on'], True))
|
|
road.curvature = cc.actuators.curvature = .01
|
|
safety = libsafety_py.libsafety
|
|
safety.init_tests()
|
|
safety.set_alternative_experience(0)
|
|
assert safety.set_safety_hooks(structs.CarParams.SafetyModel.ford, int(cp.safetyConfigs[-1].safetyParam)) == 0
|
|
safety.set_controls_allowed(True)
|
|
applied, modes, deviation_seen = 0., [], False
|
|
try:
|
|
for frame in range(200):
|
|
safety.set_timer(1_000_000 + frame * 10_000)
|
|
messages: list[tuple[str, dict[str, float]]] = [
|
|
('BrakeSysFeatures', {'Veh_V_ActlBrk': speed * 3.6, 'VehVActlBrk_D_Qf': 3, 'VehVActlBrk_No_Cnt': (frame // 2) % 16}),
|
|
('EngVehicleSpThrottle2', {'Veh_V_ActlEng': speed * 3.6, 'VehVActlEng_D_Qf': 3}),
|
|
('Yaw_Data_FD1', {'VehYaw_W_Actl': 0, 'VehYawWActl_D_Qf': 3, 'VehRollYaw_No_Cnt': frame % 256}),
|
|
('DesiredTorqBrk', {'VehStop_D_Stat': 0}),
|
|
('EngVehicleSpThrottle', {'ApedPos_Pc_ActlArb': 0}),
|
|
('EngBrakeData', {'BpedDrvAppl_D_Actl': 1, 'CcStat_D_Actl': 5}),
|
|
]
|
|
for name, values in messages:
|
|
if name in ('BrakeSysFeatures', 'EngVehicleSpThrottle2') and frame % 2:
|
|
continue
|
|
address, data, bus = packer.make_can_msg(name, 0, values)
|
|
assert safety.safety_rx_hook(libsafety_py.make_CANPacket(address, bus, data))
|
|
host.update(road.update(), frame)
|
|
cc.latActive = frame >= 15
|
|
packets = []
|
|
if frame % 5 == 0:
|
|
sends, applied = host.step_lateral(cc, cs, cc.actuators, applied)
|
|
packets.extend(sends)
|
|
deviation_seen |= host.shared.bp_curvature_deviation_limited
|
|
for address, data, _ in sends:
|
|
if address == 0x3D6 and cc.latActive:
|
|
mode = (data[0] >> 4) & 7
|
|
modes.append(mode)
|
|
if mode == 0:
|
|
assert deviation_seen and host.angle.angle_stall_blip_active
|
|
assert host.angle.bp_path_angle_final == 0.
|
|
if frame % 3 == 0:
|
|
packets.append(host.step_lka(cc, cc.hudControl))
|
|
for address, data, bus in packets:
|
|
assert safety.safety_tx_hook(libsafety_py.make_CANPacket(address, bus, data)), (frame, hex(address), data.hex())
|
|
zero_indices = [i for i, mode in enumerate(modes) if mode == 0]
|
|
assert len(zero_indices) == 6
|
|
assert zero_indices == list(range(zero_indices[0], zero_indices[0] + 6))
|
|
assert modes[-1] == 1 and host.angle.bp_path_angle_final > 0.
|
|
assert host.angle.stall_blip_count == 1 and host.angle.stall_blip_cooldown_s > 0.
|
|
finally:
|
|
safety.set_safety_hooks(structs.CarParams.SafetyModel.noOutput, 0)
|