mirror of
https://github.com/firestar5683/StarPilot.git
synced 2026-10-28 00:53:45 +08:00
firestar5683
80fbb4f6db
Register vehicle controls, planner modes, UI actions and Galaxy services after their providers exist. Wire the shared runtime entrypoints and cover their combined behavior. Co-authored-by: Adeeb Shihadeh <adeebshihadeh@gmail.com> Co-authored-by: Daniel Koepping <elkoled@gmail.com> Co-authored-by: Danny <dannyirwin31@yahoo.com> Co-authored-by: DimitriChrysafis <Dimitri.chrysafis@gmail.com> Co-authored-by: Dom <168790843+firestar5683@users.noreply.github.com> Co-authored-by: Harald Schäfer <harald.the.engineer@gmail.com> Co-authored-by: Kacper Rączy <gfw.kra@gmail.com> Co-authored-by: Prabhaav Pillai <prabhaav@msu.edu> Co-authored-by: dirwin31 <83434411+dirwin31@users.noreply.github.com> Co-authored-by: firestarsdog <229254897+firestarsdog@users.noreply.github.com> Co-authored-by: stef <19478336+stefpi@users.noreply.github.com> Co-authored-by: whoisdomi <dcantu83@gmail.com> Co-authored-by: Intelli <contact@intelli.software> Co-authored-by: haraschax <6804392+haraschax@users.noreply.github.com> Co-authored-by: prabhaavp <prabhaav@msu.edu>
100 lines
4.3 KiB
Python
100 lines
4.3 KiB
Python
import math
|
|
from types import SimpleNamespace as NS
|
|
|
|
import pytest
|
|
|
|
from openpilot.cereal import messaging
|
|
from openpilot.common.params import Params
|
|
from opendbc.car.honda.interface import CarInterface
|
|
from opendbc.car.honda.values import CAR
|
|
from openpilot.selfdrive.controls.lib.longitudinal_planner import LongitudinalPlanner
|
|
from openpilot.starpilot.controllers.coast import CoastOwner, CoastRuntime
|
|
from openpilot.starpilot.controllers.wheel_actions import WheelPublisher
|
|
from openpilot.starpilot.controllers.tests.test_wheel_actions import Publisher, DRIVE, NOW
|
|
from openpilot.starpilot.longitudinal.tests.test_cruise_ceiling import messages
|
|
|
|
|
|
def sample(owner, **changes):
|
|
values = {'admitted': True, 'active': True, 'gas': False, 'brake': False, 'speed_mps': 20.0,
|
|
'target_mps': 20.0, 'delta_mps': 2.0, 'pitch': 0.0, 'lead_relevant': False, 'stop_context': False}
|
|
values.update(changes)
|
|
return owner.sample(**values)
|
|
|
|
|
|
def test_original_pulse_hysteresis_grade_and_effective_target():
|
|
owner = CoastOwner()
|
|
assert owner.toggle(14, long_active=True)
|
|
assert sample(owner).speed_ceiling_mps == 18.0
|
|
assert sample(owner, speed_mps=18.26).pulse_coasting
|
|
assert not sample(owner, speed_mps=18.25).pulse_coasting
|
|
assert not sample(owner, speed_mps=19.74).pulse_coasting
|
|
assert sample(owner, speed_mps=19.75).pulse_coasting
|
|
assert not sample(owner, pitch=math.radians(3)).pulse_coasting
|
|
assert not sample(owner, pitch=None).pulse_coasting
|
|
assert not sample(owner, pitch=math.radians(2.51)).pulse_coasting
|
|
assert sample(owner, pitch=math.radians(2.5)).pulse_coasting
|
|
assert sample(owner, target_mps=15.0).speed_ceiling_mps == 13.0
|
|
|
|
|
|
@pytest.mark.parametrize('restriction', [{'lead_relevant': True}, {'stop_context': True}, {'target_mps': 5.0},
|
|
{'target_mps': 4.0}, {'delta_mps': 0.0}])
|
|
def test_pulse_cannot_coast_into_lead_stop_or_low_target(restriction):
|
|
owner = CoastOwner()
|
|
owner.toggle(14, long_active=True)
|
|
assert not sample(owner, **restriction).pulse_coasting
|
|
|
|
|
|
def test_force_coast_driver_reset_inactive_freeze_and_source_loss():
|
|
owner = CoastOwner()
|
|
owner.toggle(2, long_active=True)
|
|
owner.toggle(14, long_active=True)
|
|
assert sample(owner).force_decel
|
|
assert sample(owner, active=False).brake_floor is None
|
|
assert owner.force_coast and owner.pulse_glide
|
|
assert not sample(owner, gas=True).force_decel
|
|
assert owner.pulse_glide
|
|
sample(owner, admitted=False)
|
|
assert not owner.pulse_glide and not owner.force_coast
|
|
|
|
|
|
def run_planner(params, cp, *, action=0, lead=False):
|
|
class Services(dict):
|
|
pass
|
|
runtime, publisher = CoastRuntime(), WheelPublisher()
|
|
sender = Publisher()
|
|
planner = LongitudinalPlanner(cp, init_v=20.0)
|
|
for frame in range(65):
|
|
now = NOW + frame * 50_000_000
|
|
data, _ = messages(lead=lead)
|
|
sm = Services(data)
|
|
sm['deviceState'] = NS(started=True, startedMonoTime=DRIVE)
|
|
sm['carState'].canValid = True
|
|
sm['carState'].vCruise = 100.0
|
|
sm['carControl'].enabled = sm['carControl'].longActive = True
|
|
sm.logMonoTime = dict.fromkeys(sm, now)
|
|
sm.recv_time = dict.fromkeys(sm, now / 1e9)
|
|
sm.seen = sm.valid = sm.alive = dict.fromkeys(sm, True)
|
|
if frame == 0 and action:
|
|
params.put('DistanceButtonControl', action, block=True)
|
|
command = (('DistanceButtonControl', action),)
|
|
else:
|
|
command = (('', 0),)
|
|
publisher.publish(command, cp, sender, now_ns=now, drive_id=DRIVE, source_car_ns=now, source_control_ns=now)
|
|
runtime.receive(messaging.log_from_bytes(sender.events[-1][1]), params, cp, sm, now_ns=now)
|
|
planner.update(sm, now_ns=now, drive_id=DRIVE, wheel_coast=(runtime, params))
|
|
return planner, runtime
|
|
|
|
|
|
def test_actual_wheel_receipt_changes_cruise_but_preserves_native_mpc_lead_braking(tmp_path):
|
|
params = Params(str(tmp_path))
|
|
cp = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
|
|
ordinary, _ = run_planner(params, cp)
|
|
coast, owner = run_planner(params, cp, action=2)
|
|
assert owner.owner.force_coast
|
|
assert coast.a_cruise < ordinary.a_cruise
|
|
assert coast.mpc.params[0, 4] == ordinary.mpc.params[0, 4]
|
|
lead, _ = run_planner(params, cp, action=14, lead=True)
|
|
native_lead, _ = run_planner(params, cp, lead=True)
|
|
assert lead.output_a_target == pytest.approx(native_lead.output_a_target)
|
|
assert lead.output_a_target < lead.a_cruise
|