mirror of
https://github.com/firestar5683/StarPilot.git
synced 2026-11-08 05:23:41 +08:00
firestar5683
119 lines
6.2 KiB
Python
119 lines
6.2 KiB
Python
"""Actual Ford factory, parser, Card, SelfdriveD, Controls and native matrix.
|
|
|
|
CAN, perception, USB and clocks are synthetic; no physical acceptance claim.
|
|
"""
|
|
import pytest
|
|
from openpilot.starpilot.tests.ford_lifecycle_helpers import FORD_FACTORIES, ford_joined as ford_joined, run_actual_producer
|
|
|
|
|
|
@pytest.fixture(autouse=True)
|
|
def configuration_topology(monkeypatch, topology, request):
|
|
monkeypatch.ford_topology = topology
|
|
if request.node.name.startswith(('test_brake_retains_lateral_with_native_first_feedback', 'test_cancel_retains_lateral_with_native_first_feedback')):
|
|
monkeypatch.ford_native_10hz = True
|
|
monkeypatch.ford_angle = request.node.callspec.params['angle']
|
|
if request.node.name.startswith('test_recorded_radar_creep_enters_native_stopping'):
|
|
monkeypatch.ford_angle = request.node.callspec.params['angle']
|
|
if request.node.name.startswith('test_route49_first_main_engagement'):
|
|
monkeypatch.ford_first_main = True
|
|
monkeypatch.ford_angle = request.node.callspec.params['angle']
|
|
|
|
|
|
@pytest.mark.parametrize('identity', FORD_FACTORIES)
|
|
@pytest.mark.parametrize('alpha', [False, True])
|
|
@pytest.mark.parametrize('hands_free', [False])
|
|
@pytest.mark.parametrize('topology', ['automatic', 'manual', 'multi-panda', 'manual-multi'])
|
|
def test_actual_configuration_lifecycle(ford_joined, monkeypatch):
|
|
run_actual_producer(ford_joined, monkeypatch, scenario='configuration-parity')
|
|
|
|
|
|
@pytest.mark.parametrize('identity', [FORD_FACTORIES[11]])
|
|
@pytest.mark.parametrize('alpha', [False])
|
|
@pytest.mark.parametrize('hands_free', [False, True])
|
|
@pytest.mark.parametrize('topology', ['automatic', 'manual-multi'])
|
|
@pytest.mark.parametrize('angle', [False, True])
|
|
def test_route49_first_main_engagement_arms_before_cruise_cancel(ford_joined, monkeypatch, angle):
|
|
run_actual_producer(ford_joined, monkeypatch, scenario='route49')
|
|
|
|
|
|
@pytest.mark.parametrize('identity', [FORD_FACTORIES[11]])
|
|
@pytest.mark.parametrize('alpha', [True])
|
|
@pytest.mark.parametrize('hands_free', [False, True])
|
|
@pytest.mark.parametrize('angle', [False, True])
|
|
@pytest.mark.parametrize('topology', ['automatic', 'manual-multi'])
|
|
def test_brake_retains_lateral_with_native_first_feedback(ford_joined, monkeypatch, angle):
|
|
run_actual_producer(ford_joined, monkeypatch, scenario='brake-native-cadence')
|
|
|
|
|
|
@pytest.mark.parametrize('identity', [FORD_FACTORIES[11]])
|
|
@pytest.mark.parametrize('alpha', [False, True])
|
|
@pytest.mark.parametrize('hands_free', [False, True])
|
|
@pytest.mark.parametrize('angle', [False, True])
|
|
@pytest.mark.parametrize('topology', ['automatic', 'manual-multi'])
|
|
def test_cancel_retains_lateral_with_native_first_feedback(ford_joined, monkeypatch, angle):
|
|
run_actual_producer(ford_joined, monkeypatch, scenario='cancel-native-cadence')
|
|
|
|
|
|
@pytest.mark.parametrize('identity', [FORD_FACTORIES[11]])
|
|
@pytest.mark.parametrize('alpha', [True])
|
|
@pytest.mark.parametrize('hands_free', [True])
|
|
@pytest.mark.parametrize('topology', ['automatic', 'manual-multi'])
|
|
@pytest.mark.parametrize('angle', [False, True])
|
|
def test_recorded_radar_creep_enters_native_stopping(ford_joined, angle):
|
|
import json
|
|
import gzip
|
|
from pathlib import Path
|
|
from opendbc.car import structs
|
|
from opendbc.car.ford import fordcan
|
|
from opendbc.safety.tests.libsafety import libsafety_py
|
|
from openpilot.selfdrive.controls.lib.longitudinal_planner import LongitudinalPlanner
|
|
from openpilot.selfdrive.controls.lib.longcontrol import LongControl
|
|
from openpilot.starpilot.longitudinal.tests.test_lead_approach_planner import Frame, DRIVE_NS
|
|
|
|
recorded = json.loads(gzip.decompress((Path(__file__).parent / 'testdata/ford_mache_radar_creep.json.gz').read_bytes()))['frames']
|
|
joined = ford_joined
|
|
endpoint, safety = joined.native, joined.safety
|
|
endpoint.arm()
|
|
endpoint.pump(10, main=5, mask=3)
|
|
assert safety.aol_get_permission_mask() == 3
|
|
planner, controller = LongitudinalPlanner(joined.host.CP, init_v=recorded[0]['vEgo']), LongControl(joined.host.CP)
|
|
checked = 0
|
|
for index, sample in enumerate(recorded):
|
|
sm = Frame(index)
|
|
# Retain the route's actual cross-service age and validity, including radar cadence.
|
|
for service, age in sample['sourceAgeNs'].items():
|
|
sm.logMonoTime[service] = sm.now_ns - 10_000_000 - age
|
|
sm.valid[service] = sample['sourceValid'][service]
|
|
car = sm['carState']
|
|
car.vEgo, car.vEgoRaw, car.aEgo = sample['vEgo'], sample['vEgoRaw'], sample['aEgo']
|
|
car.standstill, car.gasPressed, car.brakePressed = sample['standstill'], sample['gas'], sample['brake']
|
|
car.gearShifter = 'drive'
|
|
car.cruiseState.available = car.cruiseState.enabled = True
|
|
sm['carControl'].longActive = sample['longActive']
|
|
sm['selfdriveState'].enabled = sample['enabled']
|
|
sm['selfdriveState'].experimentalMode = sample['experimental']
|
|
sm['controlsState'].longControlState = controller.long_control_state
|
|
sm['modelV2'].action.shouldStop = sample['shouldStop']
|
|
sm['modelV2'].action.desiredAcceleration = sample['aTarget']
|
|
for key, values in sample['model'].items():
|
|
getattr(sm['modelV2'], key).x = values
|
|
for lead, values in zip((sm['radarState'].leadOne, sm['radarState'].leadTwo), sample['leads'], strict=True):
|
|
for key, value in values.items():
|
|
setattr(lead, key, value)
|
|
planner.update(sm, now_ns=sm.now_ns, drive_id=DRIVE_NS)
|
|
assert planner.mpc.solution_status == 0
|
|
for _ in range(5):
|
|
output = controller.update(sample['longActive'], car, float(planner.output_a_target), planner.output_should_stop, (-3.5, 2.))
|
|
endpoint.pump(1, main=5, mask=3, speed=sample['vEgoRaw'])
|
|
stopping = controller.long_control_state == structs.CarControl.Actuators.LongControlState.stopping
|
|
active = sample['longActive']
|
|
gas = -5. if output < 0. or not active else float(output)
|
|
address, data, bus = fordcan.create_acc_msg(endpoint.packer, joined.host.CI.CC.CAN, active, gas,
|
|
float(output) if active else 0., stopping if active else False, bool(output < 0. and active), v_ego_kph=80.)
|
|
assert safety.safety_tx_hook(libsafety_py.make_CANPacket(address, bus - joined.offset, data))
|
|
if 821. <= sample['routeSeconds'] <= 828.:
|
|
assert planner.output_should_stop, ('Creeping toward stopped radar lead', sample['routeSeconds'])
|
|
assert stopping and output < 0.
|
|
checked += 1
|
|
assert checked > 500
|