mirror of
https://github.com/firestar5683/StarPilot.git
synced 2026-11-08 05:23:41 +08:00
firestar5683
e4f3d6661c
Implement acceleration profiles, FollowJerk, force-stop and lead-departure owners. Keep selected acceleration limits and hazard arbitration explicit at their shared boundaries. Co-authored-by: whoisdomi <dcantu83@gmail.com>
217 lines
10 KiB
Python
217 lines
10 KiB
Python
"""One opt-in, shared launch admission and unchanged vehicle-owned limits."""
|
|
from dataclasses import replace
|
|
from unittest.mock import patch
|
|
|
|
import pytest
|
|
import numpy as np
|
|
from opendbc.car.honda.interface import CarInterface as Honda
|
|
from opendbc.car.honda.values import CAR as H
|
|
from opendbc.car.toyota.interface import CarInterface as Toyota
|
|
from opendbc.car.toyota.values import CAR as T
|
|
from openpilot.common.params import Params
|
|
from openpilot.cereal import messaging
|
|
from openpilot.starpilot.galaxy.settings import AuthorityContext, SettingsGateway
|
|
from openpilot.selfdrive.controls.lib.longcontrol import LongControl, LongCtrlState
|
|
from openpilot.selfdrive.controls.lib.longitudinal_planner import LongitudinalPlanner, get_cruise_accel
|
|
from openpilot.starpilot.longitudinal.extension import LongitudinalContext
|
|
from openpilot.starpilot.longitudinal.inputs import LongitudinalInputs
|
|
from openpilot.starpilot.longitudinal.ioniq6_start import StartEvidence
|
|
from openpilot.starpilot.longitudinal.profile_runtime import SelectedProfileTuning
|
|
from openpilot.starpilot.longitudinal.tests.test_ioniq6_start import candidate, car_state
|
|
from openpilot.starpilot.longitudinal.tests.test_conditional_handoff import Frame, NOW, DRIVE
|
|
from openpilot.starpilot.longitudinal.takeoffs import eligible
|
|
|
|
|
|
def evidence(tick=0, **changes):
|
|
return replace(StartEvidence(True, True, False, 1, 1_000_000_000 + tick * 10_000_000), **changes)
|
|
|
|
|
|
def context(tick=0, **changes):
|
|
return LongitudinalContext(faster_takeoffs=True, start_evidence=evidence(tick, **changes))
|
|
|
|
|
|
@pytest.mark.parametrize('factory', [lambda: Honda.get_non_essential_params(H.HONDA_CIVIC),
|
|
lambda: Toyota.get_non_essential_params(T.TOYOTA_COROLLA_TSS2),
|
|
lambda: candidate()[1]])
|
|
def test_actual_factory_longcontrol_first_start_target_bounds_and_ordinary_exit(factory):
|
|
cp = factory()
|
|
assert eligible(cp)
|
|
ctrl = LongControl(cp)
|
|
cs = car_state(standstill=False)
|
|
output = ctrl.update(True, cs, 1.8, False, (np.float64(-3.), np.float64(1.5)), context=context())
|
|
assert ctrl.takeoff_starting and ctrl.long_control_state == LongCtrlState.starting
|
|
assert output <= 1.5
|
|
ctrl.update(True, car_state(speed=.11, standstill=False), .8, False, (-3., 2.), context=context(1))
|
|
assert not ctrl.takeoff_starting and ctrl.long_control_state == LongCtrlState.pid
|
|
ctrl.update(False, cs, .8, False, (-3., 2.), context=context(2))
|
|
assert ctrl.long_control_state == LongCtrlState.off
|
|
|
|
|
|
@pytest.mark.parametrize('kind', ['off', 'missing', 'stale', 'unknown_lead', 'brake', 'gas', 'can', 'timeout', 'stop', 'weak'])
|
|
def test_optin_never_creates_authority_from_missing_evidence_or_pedals(kind):
|
|
ctrl = LongControl(candidate()[1])
|
|
cs = car_state(standstill=False)
|
|
target, stop = .9, False
|
|
ctx = context()
|
|
if kind == 'off':
|
|
ctx = replace(ctx, faster_takeoffs=False)
|
|
if kind == 'missing':
|
|
ctx = replace(ctx, start_evidence=None)
|
|
if kind == 'stale':
|
|
ctx = context(plan_fresh=False)
|
|
if kind == 'unknown_lead':
|
|
ctx = context(lead_clear=None)
|
|
if kind == 'brake':
|
|
cs = car_state(brake=True)
|
|
if kind == 'gas':
|
|
cs = car_state(gas=True)
|
|
if kind == 'can':
|
|
cs = car_state(valid=False)
|
|
if kind == 'timeout':
|
|
cs = car_state(timeout=True)
|
|
if kind == 'stop':
|
|
stop = True
|
|
if kind == 'weak':
|
|
target = .74
|
|
ctrl.update(True, cs, target, stop, (-3., 2.), context=ctx)
|
|
assert not ctrl.takeoff_starting and ctrl.long_control_state != LongCtrlState.starting
|
|
|
|
|
|
@pytest.mark.parametrize('loss', ['toggle', 'evidence', 'duplicate', 'drive'])
|
|
def test_generic_starting_unwinds_when_optin_or_evidence_is_lost(loss):
|
|
ctrl = LongControl(Honda.get_non_essential_params(H.HONDA_CIVIC))
|
|
cs = car_state(standstill=False)
|
|
ctrl.update(True, cs, .9, False, (-3., 2.), context=context())
|
|
assert ctrl.takeoff_starting
|
|
ctx = context(1)
|
|
if loss == 'toggle':
|
|
ctx = replace(ctx, faster_takeoffs=False)
|
|
if loss == 'evidence':
|
|
ctx = replace(ctx, start_evidence=None)
|
|
if loss == 'duplicate':
|
|
ctx = context()
|
|
if loss == 'drive':
|
|
ctx = context(1, drive_id=2)
|
|
ctrl.update(True, cs, .9, False, (-3., 2.), context=ctx)
|
|
assert not ctrl.takeoff_starting and ctrl.long_control_state == LongCtrlState.pid
|
|
|
|
|
|
def test_native_stock_standstill_and_non_longitudinal_configuration_still_win():
|
|
cp = Honda.get_non_essential_params(H.HONDA_CIVIC)
|
|
ctrl = LongControl(cp)
|
|
ctrl.update(True, car_state(standstill=True), .9, False, (-3., 2.), context=context())
|
|
assert not ctrl.takeoff_starting and ctrl.long_control_state == LongCtrlState.stopping
|
|
cp.openpilotLongitudinalControl = False
|
|
assert not eligible(cp)
|
|
assert LongControl(cp).takeoff_start is None
|
|
|
|
|
|
def test_takeoff_budget_is_off_by_default_tapers_by_5ms_and_respects_selected_caps():
|
|
cp = Honda.get_non_essential_params(H.HONDA_CIVIC)
|
|
args = (False, 30., 0., 1.7, 0., cp, 1., 0., True)
|
|
assert get_cruise_accel(*args, selected_acceleration_max=2.) == 1.7
|
|
assert get_cruise_accel(*args, selected_acceleration_max=2., faster_takeoffs=True) == 2.
|
|
assert get_cruise_accel(*args, selected_acceleration_max=.5, faster_takeoffs=True) == .5
|
|
for speed in (5., 10., 20., 40.):
|
|
args = (False, 50., speed, 4., 0., cp, .05, 0., True)
|
|
assert get_cruise_accel(*args, faster_takeoffs=True) == get_cruise_accel(*args)
|
|
|
|
|
|
def test_actual_native_planner_uses_same_toggle_for_launch_budget():
|
|
cp = Honda.get_non_essential_params(H.HONDA_CIVIC)
|
|
off, enabled = LongitudinalPlanner(cp), LongitudinalPlanner(cp)
|
|
for tick in range(65):
|
|
sm = Frame()
|
|
stamp = NOW + tick * 50_000_000
|
|
sm.logMonoTime = dict.fromkeys(sm, stamp - 5_000_000)
|
|
sm.recv_time = dict.fromkeys(sm, (stamp - 5_000_000) / 1e9)
|
|
sm['carState'].vEgo, sm['carState'].standstill = 0., True
|
|
sm['selfdriveState'].experimentalMode = False
|
|
sm['carControl'].longActive = True
|
|
sm['modelV2'].meta.disengagePredictions.gasPressProbs = [1., 1.]
|
|
for planner, switch in ((off, False), (enabled, True)):
|
|
planner.update(sm, now_ns=stamp, selected_profiles=SelectedProfileTuning(acceleration_max=2.),
|
|
faster_lead_takeoff=switch, takeoff_drive_id=DRIVE)
|
|
assert planner.output_a_target <= 2.
|
|
assert enabled.a_cruise > off.a_cruise
|
|
assert off.a_cruise <= 1.7 and enabled.a_cruise <= 2.
|
|
|
|
|
|
def test_galaxy_single_toggle_roundtrip_global_and_both_runtime_readers(tmp_path):
|
|
params = Params(str(tmp_path))
|
|
cp = Honda.get_non_essential_params(H.HONDA_CIVIC)
|
|
class ParkedContext:
|
|
def sample(self) -> AuthorityContext:
|
|
return AuthorityContext(True, cp, b'cp')
|
|
gateway = SettingsGateway(params, ParkedContext(), clock=lambda: 10.)
|
|
page = gateway.page('profiles', 'session', b'generation')
|
|
rows = [(i, row) for i, row in enumerate(page['rows']) if row['label'] == 'Faster Takeoffs']
|
|
assert len(rows) == 1
|
|
index, row = rows[0]
|
|
assert row['label'] == 'Faster Takeoffs' and row['value'] == 'Off'
|
|
intent = gateway.preview(page['view'], index, 0, 'session', b'generation', value='On')
|
|
assert gateway.confirm(intent['intent'], 'session', b'generation')
|
|
assert params.get_bool('FasterLeadTakeoff')
|
|
inputs = LongitudinalInputs(cp, params, lambda: None)
|
|
assert inputs.takeoff_preferences.sample(1_000_000_000)
|
|
from openpilot.starpilot.longitudinal.lead_takeoff_preferences import TakeoffPreferences
|
|
assert TakeoffPreferences(params).sample(1_000_000_000)
|
|
|
|
|
|
def test_real_ioniq_host_starting_and_packed_scc_same_first_tick():
|
|
from opendbc.can import CANParser
|
|
from opendbc.car import Bus, structs
|
|
from opendbc.car.hyundai.values import DBC
|
|
from opendbc.car.hyundai.tests.test_ioniq6_longitudinal import controller_fixture
|
|
for alternate in (False, True):
|
|
cp, state, controller = controller_fixture(alternate)
|
|
host = LongControl(cp)
|
|
parser = CANParser(DBC[cp.carFingerprint][Bus.pt], [('SCC_CONTROL', 0)], 1)
|
|
state.out.canValid, state.out.canTimeout = True, False
|
|
state.out.vEgo, state.out.aEgo = 0., 0.
|
|
control = structs.CarControl(enabled=True, longActive=True)
|
|
for tick in range(8):
|
|
target = 1.8
|
|
output = host.update(True, state.out, target, False, (-3.5, 2.), context=context(tick))
|
|
assert host.long_control_state == LongCtrlState.starting and output == target
|
|
control.actuators.longControlState = host.long_control_state
|
|
control.actuators.accel = float(output)
|
|
sent, frames = controller.update(control.as_reader(), state, 1_000_000_000 + tick * 10_000_000)
|
|
scc = [message for message in frames if message[0] == 0x1A0]
|
|
if scc:
|
|
parser.update((1_000_000_000 + tick * 10_000_000, scc))
|
|
assert abs(parser.vl['SCC_CONTROL']['aReqValue'] - sent.accel) <= .0051
|
|
assert sent.accel <= target
|
|
assert controller.ioniq6_longitudinal.state.launch_active
|
|
|
|
|
|
@pytest.mark.parametrize('factory', [lambda: Honda.get_non_essential_params(H.HONDA_CIVIC),
|
|
lambda: Toyota.get_non_essential_params(T.TOYOTA_COROLLA_TSS2),
|
|
lambda: candidate()[1]])
|
|
def test_actual_inputs_qualify_current_serialized_drive_and_plan(factory, tmp_path):
|
|
cp = factory()
|
|
params = Params(str(tmp_path))
|
|
params.put_bool('FasterLeadTakeoff', True, block=True)
|
|
sm = Frame()
|
|
sm['longitudinalPlan'] = messaging.new_message('longitudinalPlan').longitudinalPlan
|
|
sm.seen['longitudinalPlan'] = sm.alive['longitudinalPlan'] = sm.valid['longitudinalPlan'] = True
|
|
sm.logMonoTime['longitudinalPlan'] = NOW - 5_000_000
|
|
sm.recv_time['longitudinalPlan'] = (NOW - 5_000_000) / 1e9
|
|
inputs = LongitudinalInputs(cp, params, lambda: sm)
|
|
assert 'radarState' in inputs.optional_services and 'deviceState' in inputs.optional_services
|
|
first = NOW - 100_000_000
|
|
with patch('openpilot.starpilot.longitudinal.inputs.clock_pair_ns', return_value=(first, first + 500_000_000)):
|
|
assert not inputs.context(True).start_evidence.plan_fresh
|
|
with patch('openpilot.starpilot.longitudinal.inputs.clock_pair_ns', return_value=(NOW, NOW + 500_000_000)):
|
|
ctx = inputs.context(True)
|
|
assert ctx.faster_takeoffs and ctx.start_evidence.car_fresh and ctx.start_evidence.plan_fresh
|
|
assert ctx.start_evidence.drive_id == DRIVE
|
|
ctrl = LongControl(cp)
|
|
ctrl.update(True, car_state(standstill=False), 1.5, False, (-3., 2.), context=ctx)
|
|
assert ctrl.takeoff_starting and ctrl.long_control_state == LongCtrlState.starting
|
|
sm.logMonoTime['longitudinalPlan'] = NOW - 150_000_001
|
|
assert not inputs.context(True).start_evidence.plan_fresh
|
|
sm.logMonoTime['longitudinalPlan'] = NOW - 5_000_000
|
|
sm.valid['radarState'] = False
|
|
assert inputs.context(True).start_evidence.lead_clear is None
|