mirror of
https://github.com/firestar5683/StarPilot.git
synced 2026-11-08 13:33:42 +08:00
firestar5683
87 lines
4.7 KiB
Python
87 lines
4.7 KiB
Python
"""Representable Ford path-angle commands across steering directions and buses."""
|
|
from types import SimpleNamespace
|
|
|
|
import pytest
|
|
|
|
from opendbc.can import CANPacker, CANParser
|
|
from opendbc.car import gen_empty_fingerprint, structs
|
|
from opendbc.car.ford.fordcan import CanBus
|
|
from opendbc.car.ford.interface import CarInterface
|
|
from opendbc.car.ford.tests.test_bluepilot_lateral import STARTUP, StraightRoadInputs, settings
|
|
from opendbc.car.ford.values import CAR, FordFlags
|
|
from opendbc.bluepilot_lateral.hosts.starpilot.safety_param import init_ford_safety_param_starpilot
|
|
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
|
|
|
|
|
|
@pytest.mark.parametrize('car', [CAR.FORD_ESCAPE_MK4, CAR.FORD_F_150_MK14, CAR.FORD_MUSTANG_MACH_E_MK1])
|
|
@pytest.mark.parametrize('sign', [-1., 1.])
|
|
@pytest.mark.parametrize('pinion', [False, True])
|
|
@pytest.mark.parametrize('approach', [.449, .469])
|
|
def test_path_angle_near_encoding_bounds_keeps_direction_and_passes_native(car, sign, pinion, approach):
|
|
cp = CarInterface.get_params(car, gen_empty_fingerprint(), [], False, False, False)
|
|
startup = STARTUP['on+pinion' if pinion else 'on']
|
|
init_ford_safety_param_starpilot(cp, startup)
|
|
packer, can = CANPacker('ford_lincoln_base_pt'), CanBus(cp)
|
|
source = SimpleNamespace(controller_choice=FordController.STARPILOT, current_inputs_valid=True)
|
|
host = FordBluePilotController(cp, packer, can, source)
|
|
road = StraightRoadInputs(settings(startup, True))
|
|
speed = 5.
|
|
cc = structs.CarControl(latActive=True)
|
|
cs = SimpleNamespace(out=structs.CarState(vEgo=speed, vEgoRaw=speed, canValid=True), lat_ctl_lim_stat=0)
|
|
message = 'LateralMotionControl2' if cp.flags & FordFlags.CANFD else 'LateralMotionControl'
|
|
parser = CANParser('ford_lincoln_base_pt', [(message, 0)], can.main)
|
|
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)
|
|
wire_angles = []
|
|
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}),
|
|
]
|
|
if pinion:
|
|
messages.append(('SteeringPinion_Data', {'StePinComp_An_Est': 0., 'StePinCompAnEst_D_Qf': 3, 'StePinAn_No_Cnt': frame % 16}))
|
|
for name, fields in messages:
|
|
if name in ('BrakeSysFeatures', 'EngVehicleSpThrottle2') and frame % 2:
|
|
continue
|
|
assert fields.keys() <= packer.dbc.name_to_msg[name].sigs.keys()
|
|
address, data, bus = packer.make_can_msg(name, can.main, fields)
|
|
assert safety.safety_rx_hook(libsafety_py.make_CANPacket(address, bus, data))
|
|
# Settle just below the saturation boundary, then request a stronger turn.
|
|
curvature = sign * (approach if frame < 125 else .8) / (speed * 1.3)
|
|
cc.actuators.curvature = curvature
|
|
inputs = road.update()
|
|
inputs.lateralInputs.pathYawRate = [curvature * speed] * 33
|
|
host.update(inputs, frame)
|
|
cc.latActive = frame >= 15
|
|
packets = []
|
|
if frame % 5 == 0:
|
|
lateral, _ = host.step_lateral(cc, cs, cc.actuators, 0.)
|
|
packets.extend(lateral)
|
|
for address, data, bus in lateral:
|
|
parser.update([(1_000_000_000 + frame * 10_000_000, [(address, data, bus)])])
|
|
wire = parser.vl[message]['LatCtlPath_An_Actl']
|
|
assert -.5 <= wire <= 2047 * .0005 - .5
|
|
assert wire == pytest.approx(-host.angle.bp_path_angle_final, abs=.00025)
|
|
if cc.latActive:
|
|
assert wire * sign <= 0.
|
|
wire_angles.append(wire)
|
|
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 max(abs(value) for value in wire_angles) >= approach - .00025
|
|
assert max(abs(b - a) for a, b in zip(wire_angles[:-1], wire_angles[1:], strict=True)) <= .0555
|
|
finally:
|
|
safety.set_safety_hooks(structs.CarParams.SafetyModel.noOutput, 0)
|