mirror of
https://github.com/firestar5683/StarPilot.git
synced 2026-11-14 00:13:42 +08:00
Runtime AOL
Register vehicle AOL adapters and runtime dispatch after the vehicle factories and native contracts exist. Co-authored-by: Dom <168790843+firestar5683@users.noreply.github.com>
This commit is contained in:
firestar5683
@@ -0,0 +1,198 @@
|
||||
"""Actual classic-Bosch Honda CP/controller frames against the AOL safety hooks."""
|
||||
|
||||
import unittest
|
||||
from types import SimpleNamespace
|
||||
|
||||
from opendbc.car import gen_empty_fingerprint, structs
|
||||
from opendbc.car.honda.carcontroller import CarController
|
||||
from opendbc.car.honda.interface import CarInterface
|
||||
from opendbc.car.honda.values import CAR, CarControllerParams, HondaSafetyFlags
|
||||
from opendbc.safety.tests.libsafety import libsafety_py
|
||||
from opendbc.safety.tests.test_honda import Btn
|
||||
from openpilot.starpilot.car.honda.aol import CLASSIC_BOSCH_AOL_CARS, qualified_honda
|
||||
from openpilot.starpilot.aol.runtime import decide_axes
|
||||
|
||||
|
||||
def _frames(candidate, cp, *, lat: bool, long: bool):
|
||||
cc = structs.CarControl()
|
||||
cc.enabled = lat or long
|
||||
cc.latActive = lat
|
||||
cc.longActive = long
|
||||
cc.actuators.torque = 0.01
|
||||
cc.actuators.accel = 1.0
|
||||
state = SimpleNamespace(out=structs.CarState(), v_cruise_factor=1.0, is_metric=True, acc_hud={}, lkas_hud={})
|
||||
state.out.vEgo = 20.0
|
||||
state.out.cruiseState.available = True
|
||||
controller = CarController(candidate.config.dbc_dict, cp)
|
||||
_, sent = controller.update(cc.as_reader(), state, 0)
|
||||
return {msg[0]: msg for msg in sent if isinstance(msg, tuple)}
|
||||
|
||||
|
||||
def _packet(frame):
|
||||
address, data, bus = frame
|
||||
return libsafety_py.make_CANPacket(address, bus, data)
|
||||
|
||||
|
||||
class TestHondaAolFamily(unittest.TestCase):
|
||||
def test_real_controller_frames_obey_native_four_axis_masks(self):
|
||||
if libsafety_py.libsafety.set_safety_hooks(structs.CarParams.SafetyModel.allOutput, 0) != 0:
|
||||
self.skipTest('Independent AOL axes require ALLOW_DEBUG firmware')
|
||||
from opendbc.safety.tests.test_honda import TestHondaBoschLongSafety
|
||||
gas_lookup = list(CarControllerParams.BOSCH_GAS_LOOKUP_V)
|
||||
try:
|
||||
for candidate in sorted(CLASSIC_BOSCH_AOL_CARS, key=str):
|
||||
for detected_alt in ((False, True) if candidate == CAR.HONDA_ACCORD else (False,)):
|
||||
with self.subTest(candidate=candidate, detected_alt=detected_alt):
|
||||
CarControllerParams.BOSCH_GAS_LOOKUP_V = list(gas_lookup)
|
||||
fingerprint = gen_empty_fingerprint()
|
||||
if detected_alt:
|
||||
fingerprint[1][0x1BE] = 3
|
||||
cp = CarInterface.get_params(candidate, fingerprint, [], True, False, False)
|
||||
self.assertTrue(qualified_honda(cp))
|
||||
self.assertEqual(bool(cp.safetyConfigs[0].safetyParam & HondaSafetyFlags.ALT_BRAKE),
|
||||
candidate in (CAR.HONDA_CRV_5G, CAR.ACURA_RDX_3G) or detected_alt)
|
||||
|
||||
for mode, lat, long in (("off", False, False), ("lateralOnly", True, False),
|
||||
("longitudinalOnly", False, True), ("combined", True, True)):
|
||||
with self.subTest(mode=mode):
|
||||
native = SimpleNamespace(requestedLateral=lat, requestedLongitudinal=long,
|
||||
lateralAllowed=lat, longitudinalAllowed=long)
|
||||
state = structs.CarState(canValid=True, gearShifter=structs.CarState.GearShifter.drive, vEgo=20.0)
|
||||
intent = SimpleNamespace(allowedLatch=lat, pauseLateral=not lat, pauseLongitudinal=not long)
|
||||
decision = decide_axes(standard_lateral=False, standard_longitudinal=long, intent=intent,
|
||||
native=native, car_state=state, initialized=True, model_ready=True,
|
||||
no_entry=False, immediate_disable=False, dm_lockout=False, pause_brake_mps=5.0)
|
||||
self.assertEqual(decision.mode, mode)
|
||||
frames = _frames(candidate, cp, lat=decision.lateral_active, long=decision.longitudinal_active)
|
||||
self.assertEqual(frames[0xE4][2], 1)
|
||||
self.assertEqual(frames[0x1DF][2], 1)
|
||||
|
||||
fixture = TestHondaBoschLongSafety('test_diagnostics')
|
||||
fixture.setUp()
|
||||
safety = fixture.safety
|
||||
safety.set_safety_hooks(structs.CarParams.SafetyModel.hondaBosch,
|
||||
int(cp.safetyConfigs[0].safetyParam) | int(HondaSafetyFlags.AOL_BOSCH_LONG))
|
||||
safety.init_tests()
|
||||
safety.set_timer(1_000_000)
|
||||
safety.set_aol_test_heartbeat(True)
|
||||
fixture._rx(fixture._acc_state_msg(True))
|
||||
fixture._rx(fixture._button_msg(Btn.NONE))
|
||||
fixture._rx(fixture._speed_msg(20))
|
||||
fixture._rx(fixture._powertrain_data_msg())
|
||||
if cp.safetyConfigs[0].safetyParam & HondaSafetyFlags.ALT_BRAKE:
|
||||
fixture._rx(fixture._alt_brake_msg(0))
|
||||
if long:
|
||||
fixture._rx(fixture._button_msg(Btn.SET))
|
||||
fixture._rx(fixture._button_msg(Btn.NONE))
|
||||
self.assertTrue(safety.get_controls_allowed())
|
||||
safety.aol_set_host_request(int(lat) | (int(long) << 1))
|
||||
self.assertEqual(safety.aol_get_permission_mask(), int(lat) | (int(long) << 1))
|
||||
steer = _packet(frames[0xE4])
|
||||
acc = _packet(frames[0x1DF])
|
||||
self.assertTrue(safety.safety_tx_hook(steer))
|
||||
self.assertTrue(safety.safety_tx_hook(acc))
|
||||
|
||||
active_steer = _frames(candidate, cp, lat=True, long=False)[0xE4]
|
||||
active_acc = _frames(candidate, cp, lat=False, long=True)[0x1DF]
|
||||
if not lat:
|
||||
self.assertFalse(safety.safety_tx_hook(_packet(active_steer)))
|
||||
if not long:
|
||||
self.assertFalse(safety.safety_tx_hook(_packet(active_acc)))
|
||||
finally:
|
||||
CarControllerParams.BOSCH_GAS_LOOKUP_V = gas_lookup
|
||||
|
||||
def test_native_main_brake_source_and_health_for_both_safety_params(self):
|
||||
if libsafety_py.libsafety.set_safety_hooks(structs.CarParams.SafetyModel.allOutput, 0) != 0:
|
||||
self.skipTest('Independent AOL axes require ALLOW_DEBUG firmware')
|
||||
from opendbc.safety.tests.test_honda import TestHondaBoschLongSafety
|
||||
|
||||
for alt_brake in (False, True):
|
||||
with self.subTest(alt_brake=alt_brake):
|
||||
fixture = TestHondaBoschLongSafety('test_diagnostics')
|
||||
fixture.setUp()
|
||||
safety = fixture.safety
|
||||
safety.set_safety_hooks(structs.CarParams.SafetyModel.hondaBosch,
|
||||
int(HondaSafetyFlags.BOSCH_LONG | HondaSafetyFlags.AOL_BOSCH_LONG |
|
||||
(HondaSafetyFlags.ALT_BRAKE if alt_brake else 0)))
|
||||
safety.init_tests()
|
||||
safety.set_timer(1_000_000)
|
||||
safety.set_aol_test_heartbeat(True)
|
||||
fixture._rx(fixture._acc_state_msg(True))
|
||||
fixture._rx(fixture._button_msg(Btn.NONE))
|
||||
fixture._rx(fixture._speed_msg(20))
|
||||
fixture._rx(fixture._powertrain_data_msg())
|
||||
safety.aol_set_host_request(1)
|
||||
self.assertEqual(safety.aol_get_permission_mask(), 0 if alt_brake else 1)
|
||||
if alt_brake:
|
||||
wrong_bus = fixture._alt_brake_msg(0)
|
||||
wrong_bus[0].bus = 0
|
||||
fixture._rx(wrong_bus)
|
||||
self.assertEqual(safety.aol_get_permission_mask(), 0)
|
||||
bad_checksum = fixture._alt_brake_msg(0)
|
||||
bad_checksum[0].data[2] ^= 1
|
||||
self.assertFalse(fixture._rx(bad_checksum))
|
||||
self.assertEqual(safety.aol_get_permission_mask(), 0)
|
||||
safety.set_safety_hooks(structs.CarParams.SafetyModel.hondaBosch,
|
||||
int(HondaSafetyFlags.BOSCH_LONG | HondaSafetyFlags.AOL_BOSCH_LONG |
|
||||
HondaSafetyFlags.ALT_BRAKE))
|
||||
safety.init_tests()
|
||||
safety.set_timer(1_000_000)
|
||||
safety.set_aol_test_heartbeat(True)
|
||||
fixture._rx(fixture._acc_state_msg(True))
|
||||
fixture._rx(fixture._button_msg(Btn.NONE))
|
||||
fixture._rx(fixture._speed_msg(20))
|
||||
fixture._rx(fixture._powertrain_data_msg())
|
||||
fixture._rx(fixture._alt_brake_msg(0))
|
||||
safety.aol_set_host_request(1)
|
||||
self.assertEqual(safety.aol_get_permission_mask(), 1)
|
||||
fixture._rx(fixture._alt_brake_msg(1))
|
||||
self.assertTrue(safety.get_brake_pressed_prev())
|
||||
self.assertEqual(safety.aol_get_permission_mask(), 1) # native lateral permission is separate from brake-paused host intent
|
||||
fixture._rx(fixture._alt_brake_msg(0))
|
||||
safety.aol_set_host_request(1)
|
||||
self.assertEqual(safety.aol_get_permission_mask(), 1)
|
||||
safety.set_timer(1_300_001)
|
||||
self.assertEqual(safety.aol_get_permission_mask(), 0)
|
||||
safety.set_timer(1_400_000)
|
||||
fixture._rx(fixture._acc_state_msg(True))
|
||||
fixture._rx(fixture._button_msg(Btn.NONE))
|
||||
fixture._rx(fixture._speed_msg(20))
|
||||
fixture._rx(fixture._powertrain_data_msg())
|
||||
if alt_brake:
|
||||
fixture._rx(fixture._alt_brake_msg(0))
|
||||
safety.aol_set_host_request(1)
|
||||
self.assertEqual(safety.aol_get_permission_mask(), 1)
|
||||
fixture._rx(fixture._button_msg(Btn.CANCEL))
|
||||
self.assertEqual(safety.aol_get_permission_mask(), 0)
|
||||
safety.aol_set_host_request(1)
|
||||
safety.set_aol_test_heartbeat(False)
|
||||
self.assertEqual(safety.aol_get_permission_mask(), 0)
|
||||
safety.set_aol_test_heartbeat(True)
|
||||
fixture._rx(fixture._acc_state_msg(False))
|
||||
safety.aol_set_host_request(1)
|
||||
self.assertEqual(safety.aol_get_permission_mask(), 0)
|
||||
|
||||
|
||||
class TestHondaAolFamilyRelease(unittest.TestCase):
|
||||
def setUp(self):
|
||||
self.gas_lookup = list(CarControllerParams.BOSCH_GAS_LOOKUP_V)
|
||||
self.addCleanup(setattr, CarControllerParams, 'BOSCH_GAS_LOOKUP_V', self.gas_lookup)
|
||||
|
||||
def test_release_firmware_never_advertises_family_axis_permission(self):
|
||||
safety = libsafety_py.libsafety
|
||||
if safety.set_safety_hooks(structs.CarParams.SafetyModel.allOutput, 0) == 0:
|
||||
self.skipTest('Release-only denial contract')
|
||||
for candidate in sorted(CLASSIC_BOSCH_AOL_CARS, key=str):
|
||||
with self.subTest(candidate=candidate):
|
||||
CarControllerParams.BOSCH_GAS_LOOKUP_V = list(self.gas_lookup)
|
||||
cp = CarInterface.get_params(candidate, gen_empty_fingerprint(), [], True, False, False)
|
||||
self.assertTrue(qualified_honda(cp))
|
||||
safety.set_safety_hooks(structs.CarParams.SafetyModel.hondaBosch,
|
||||
int(cp.safetyConfigs[0].safetyParam) | int(HondaSafetyFlags.AOL_BOSCH_LONG))
|
||||
safety.init_tests()
|
||||
safety.set_aol_test_heartbeat(True)
|
||||
safety.aol_set_host_request(3)
|
||||
self.assertEqual(safety.aol_get_permission_mask(), 0)
|
||||
frames = _frames(candidate, cp, lat=True, long=True)
|
||||
self.assertFalse(safety.safety_tx_hook(_packet(frames[0xE4])))
|
||||
self.assertFalse(safety.safety_tx_hook(_packet(frames[0x1DF])))
|
||||
@@ -0,0 +1,34 @@
|
||||
import unittest
|
||||
|
||||
from opendbc.car import structs
|
||||
from opendbc.car.gm.tests.test_bolt_cc import fixture, feed, control
|
||||
from opendbc.car.gm.values import CAR
|
||||
from openpilot.selfdrive.car.car_events import CarEvents, EventName
|
||||
from openpilot.selfdrive.selfdrived.events import EVENTS, ET
|
||||
|
||||
|
||||
SUPPORTED_NON_ACC = (CAR.CHEVROLET_BOLT_CC_2017, CAR.CHEVROLET_BOLT_CC_2018_2021, CAR.CHEVROLET_BOLT_CC_2022_2023)
|
||||
|
||||
|
||||
class TestBoltCruiseModeEvent(unittest.TestCase):
|
||||
def test_actual_bolt_ci_state_does_not_block_engagement_for_wrong_cruise_mode(self):
|
||||
for identity in SUPPORTED_NON_ACC:
|
||||
for removed in (False, True):
|
||||
cp, ci, packer = fixture(identity, removed=removed)
|
||||
state, _ = feed(ci, packer, 1_000_000_000, active=True, speed=20)
|
||||
self.assertTrue(state.canValid)
|
||||
self.assertFalse(cp.pcmCruise)
|
||||
self.assertTrue(state.cruiseState.enabled)
|
||||
events = CarEvents(cp).update(state.as_reader(), structs.CarState.new_message().as_reader(), control().as_reader())
|
||||
self.assertNotIn(EventName.wrongCruiseMode, events.names, (identity, removed))
|
||||
self.assertFalse(state.cruiseState.nonAdaptive)
|
||||
|
||||
def test_wrong_mode_still_produces_stock_owner_no_entry_and_user_disable(self):
|
||||
cp, _, _ = fixture(CAR.CHEVROLET_BOLT_EUV)
|
||||
self.assertTrue(cp.pcmCruise)
|
||||
state = structs.CarState.new_message()
|
||||
state.cruiseState.nonAdaptive = True
|
||||
events = CarEvents(cp).update(state.as_reader(), structs.CarState.new_message().as_reader(), control().as_reader())
|
||||
self.assertIn(EventName.wrongCruiseMode, events.names)
|
||||
self.assertIn(ET.NO_ENTRY, EVENTS[EventName.wrongCruiseMode])
|
||||
self.assertIn(ET.USER_DISABLE, EVENTS[EventName.wrongCruiseMode])
|
||||
@@ -0,0 +1,109 @@
|
||||
"""Saved cruise intervals alter only Card's non-PCM button increments."""
|
||||
|
||||
import tempfile
|
||||
import unittest
|
||||
from pathlib import Path
|
||||
|
||||
from opendbc.car.structs import car
|
||||
from openpilot.common.params import Params
|
||||
from openpilot.selfdrive.car.cruise import CRUISE_LONG_PRESS, IMPERIAL_INCREMENT, SlcPendingConfirmation, VCruiseHelper
|
||||
from openpilot.starpilot.longitudinal.cruise_intervals import CruiseIntervals, read_cruise_intervals
|
||||
|
||||
|
||||
Button = car.CarState.ButtonEvent.Type
|
||||
|
||||
|
||||
def state(button=None, pressed=False, *, standstill=False):
|
||||
events = [] if button is None else [car.CarState.ButtonEvent(type=button, pressed=pressed)]
|
||||
return car.CarState(cruiseState={"available": True, "standstill": standstill}, buttonEvents=events)
|
||||
|
||||
|
||||
class TestCruiseIntervals(unittest.TestCase):
|
||||
def setUp(self):
|
||||
temp = tempfile.TemporaryDirectory()
|
||||
self.addCleanup(temp.cleanup)
|
||||
self.params = Params(temp.name)
|
||||
|
||||
def test_saved_children_ignore_retired_parent_and_keep_pcm_gate(self):
|
||||
self.params.put("CustomCruise", 5.0, block=True)
|
||||
self.params.put("CustomCruiseLong", 1.0, block=True)
|
||||
self.assertEqual(read_cruise_intervals(self.params, pcm_cruise=False), CruiseIntervals(5, 1))
|
||||
self.params.put_bool("QOLLongitudinal", True, block=True)
|
||||
self.assertEqual(read_cruise_intervals(self.params, pcm_cruise=False), CruiseIntervals(5, 1))
|
||||
self.assertEqual(read_cruise_intervals(self.params, pcm_cruise=True), CruiseIntervals())
|
||||
self.params.put_bool("QOLLongitudinal", False, block=True)
|
||||
self.assertEqual(read_cruise_intervals(self.params, pcm_cruise=False), CruiseIntervals(5, 1))
|
||||
|
||||
def test_invalid_children_fall_back_without_enabling_other_features(self):
|
||||
self.params.put_bool("QOLLongitudinal", True, block=True)
|
||||
Path(self.params.get_param_path("CustomCruise")).write_bytes(b"nan")
|
||||
Path(self.params.get_param_path("CustomCruiseLong")).write_bytes(b"200")
|
||||
self.assertEqual(read_cruise_intervals(self.params, pcm_cruise=False), CruiseIntervals())
|
||||
|
||||
def helper(self, *, metric=True, custom=True, initial=67.0, pcm=False):
|
||||
helper = VCruiseHelper(car.CarParams(pcmCruise=pcm))
|
||||
helper.v_cruise_kph = initial
|
||||
if custom:
|
||||
helper.intervals = CruiseIntervals(5, 1)
|
||||
return helper
|
||||
|
||||
@staticmethod
|
||||
def step(helper, button=None, pressed=False, *, metric=True, pending=None, standstill=False):
|
||||
helper.update_v_cruise(state(button, pressed, standstill=standstill), True, metric, pending)
|
||||
|
||||
def test_metric_short_and_held_accel_and_decel(self):
|
||||
for button, short_target, held_target in ((Button.accelCruise, 70, 71), (Button.decelCruise, 65, 64)):
|
||||
with self.subTest(button=button):
|
||||
helper = self.helper()
|
||||
self.step(helper, button, True)
|
||||
self.step(helper, button, False)
|
||||
self.assertEqual(helper.v_cruise_kph, short_target)
|
||||
self.assertEqual(helper.slc_cruise_change[2:], ("accel" if button == Button.accelCruise else "decel", False))
|
||||
self.step(helper, button, True)
|
||||
for _ in range(CRUISE_LONG_PRESS - 1):
|
||||
self.step(helper)
|
||||
self.step(helper)
|
||||
self.assertEqual(helper.v_cruise_kph, held_target)
|
||||
self.step(helper, button, False)
|
||||
self.assertEqual(helper.v_cruise_kph, held_target)
|
||||
self.assertIsNone(helper.slc_cruise_change)
|
||||
|
||||
def test_imperial_display_unit_increments(self):
|
||||
helper = self.helper(initial=50 * IMPERIAL_INCREMENT)
|
||||
self.step(helper, Button.accelCruise, True, metric=False)
|
||||
self.step(helper, Button.accelCruise, False, metric=False)
|
||||
self.assertAlmostEqual(helper.v_cruise_kph, 55 * IMPERIAL_INCREMENT)
|
||||
self.step(helper, Button.accelCruise, True, metric=False)
|
||||
for _ in range(CRUISE_LONG_PRESS):
|
||||
self.step(helper, metric=False)
|
||||
self.assertAlmostEqual(helper.v_cruise_kph, 56 * IMPERIAL_INCREMENT)
|
||||
|
||||
def test_default_pcm_standstill_and_slc_ownership(self):
|
||||
default = self.helper(custom=False, initial=60)
|
||||
self.step(default, Button.accelCruise, True)
|
||||
self.step(default, Button.accelCruise, False)
|
||||
self.assertEqual(default.v_cruise_kph, 61)
|
||||
|
||||
pcm = self.helper(pcm=True, initial=60)
|
||||
self.step(pcm, Button.accelCruise, True)
|
||||
self.step(pcm, Button.accelCruise, False)
|
||||
self.assertEqual(pcm.v_cruise_kph, 255)
|
||||
for pressed in (True, False):
|
||||
pcm_state = state(Button.accelCruise, pressed)
|
||||
pcm_state.cruiseState.speed = 20.0
|
||||
pcm_state.cruiseState.speedCluster = 21.0
|
||||
pcm.update_v_cruise(pcm_state, True, True)
|
||||
self.assertEqual(pcm.v_cruise_kph, 72.0)
|
||||
self.assertAlmostEqual(pcm.v_cruise_cluster_kph, 75.6)
|
||||
|
||||
standstill = self.helper(initial=60)
|
||||
self.step(standstill, Button.accelCruise, True, standstill=True)
|
||||
self.step(standstill, Button.accelCruise, False)
|
||||
self.assertEqual(standstill.v_cruise_kph, 60)
|
||||
|
||||
slc = self.helper(initial=60)
|
||||
pending = SlcPendingConfirmation("s", 1, 1)
|
||||
self.step(slc, Button.accelCruise, True, pending=pending)
|
||||
self.step(slc, Button.accelCruise, False)
|
||||
self.assertEqual(slc.v_cruise_kph, 60)
|
||||
self.assertTrue(slc.slc_released_suppressed)
|
||||
@@ -0,0 +1,261 @@
|
||||
"""Physical distance gestures reserve only their mapped custom action."""
|
||||
|
||||
import tempfile
|
||||
import unittest
|
||||
from types import SimpleNamespace as NS
|
||||
from unittest.mock import MagicMock, patch
|
||||
|
||||
from opendbc.car.structs import car
|
||||
from openpilot.cereal import messaging
|
||||
from openpilot.common.params import Params
|
||||
from openpilot.selfdrive.car.card import Car
|
||||
from openpilot.selfdrive.car.cruise import VCruiseHelper, CRUISE_LONG_PRESS
|
||||
from openpilot.selfdrive.selfdrived.events import EventName, Events
|
||||
from openpilot.selfdrive.selfdrived.selfdrived import SelfdriveD
|
||||
from openpilot.starpilot.aol.intent import AolCardIntent, read_settings as read_aol_settings
|
||||
from openpilot.selfdrive.car.tests.test_hyundai_aol import candidate as ioniq6_candidate
|
||||
from openpilot.starpilot.conditional_mode.manual import ButtonTracker
|
||||
|
||||
|
||||
ButtonType = car.CarState.ButtonEvent.Type
|
||||
|
||||
|
||||
def state(*events):
|
||||
return car.CarState(canValid=True, buttonEvents=[car.CarState.ButtonEvent(type=ButtonType.gapAdjustCruise, pressed=pressed)
|
||||
for pressed in events])
|
||||
|
||||
|
||||
class Publisher:
|
||||
def __init__(self):
|
||||
self.messages = []
|
||||
|
||||
def send(self, service, message):
|
||||
self.messages.append((service, message.to_bytes()))
|
||||
|
||||
|
||||
class TestDistancePersonalityClaim(unittest.TestCase):
|
||||
def setUp(self):
|
||||
temp = tempfile.TemporaryDirectory()
|
||||
self.addCleanup(temp.cleanup)
|
||||
self.params = Params(temp.name)
|
||||
_, long_cp = ioniq6_candidate(False)
|
||||
self.cp = long_cp
|
||||
self.publisher = Publisher()
|
||||
self.card = Car.__new__(Car)
|
||||
self.enterContext(patch.multiple(self.card, create=True, params=self.params, CP=self.cp,
|
||||
distance_personality_tracker=ButtonTracker(), aol_card_intent=None,
|
||||
sm=NS(frame=1, all_checks=lambda _: True), pm=self.publisher,
|
||||
slc_replay=False, curve_replay=False, conditional_replay=False, aol_replay=False,
|
||||
last_actuators_output=car.CarControl.Actuators(), can_rcv_cum_timeout_counter=0,
|
||||
rk=NS(remaining=0.0), v_cruise_helper=VCruiseHelper(self.cp), slc_receipts=[]))
|
||||
Car.observe_distance_personality(self.card, state(), 1)
|
||||
|
||||
def observe(self, physical, tick):
|
||||
Car.observe_distance_personality(self.card, physical, tick)
|
||||
return physical
|
||||
|
||||
def published(self, physical):
|
||||
self.publisher.messages.clear()
|
||||
Car.state_publish(self.card, physical, None)
|
||||
return next(messaging.log_from_bytes(payload).carState for service, payload in self.publisher.messages
|
||||
if service == 'carState')
|
||||
|
||||
def test_custom_short_and_conditional_code_reserve_release(self):
|
||||
for code in (3, 4, 5):
|
||||
with self.subTest(code=code):
|
||||
self.params.put('DistanceButtonControl', code, block=True)
|
||||
self.observe(state(True), 10)
|
||||
release = self.observe(state(False), 11)
|
||||
self.assertEqual(len(self.published(release).buttonEvents), 0)
|
||||
self.assertEqual(len(release.buttonEvents), 1)
|
||||
self.params.put('DistanceButtonControl', 0, block=True)
|
||||
self.observe(state(True), 12)
|
||||
release = self.observe(state(False), 13)
|
||||
self.assertEqual(len(self.published(release).buttonEvents), 1)
|
||||
|
||||
def test_long_mapping_preserves_short_fallback_and_claims_later_release(self):
|
||||
self.params.put('LongDistanceButtonControl', 3, block=True)
|
||||
self.observe(state(True), 10)
|
||||
self.assertEqual(len(self.published(self.observe(state(False), 11)).buttonEvents), 1)
|
||||
self.observe(state(True), 20)
|
||||
for tick in range(CRUISE_LONG_PRESS - 1):
|
||||
self.observe(state(), 21 + tick)
|
||||
self.params.put('LongDistanceButtonControl', 0, block=True)
|
||||
release = self.observe(state(False), 100)
|
||||
self.assertEqual(len(self.published(release).buttonEvents), 0)
|
||||
self.observe(state(True), 101)
|
||||
self.assertEqual(len(self.published(self.observe(state(False), 102)).buttonEvents), 1)
|
||||
|
||||
def test_very_long_and_unqualified_keep_unmapped_fallback(self):
|
||||
self.params.put('VeryLongDistanceButtonControl', 5, block=True)
|
||||
self.observe(state(True), 10)
|
||||
for tick in range(CRUISE_LONG_PRESS * 5 - 1):
|
||||
self.observe(state(), 11 + tick)
|
||||
self.assertEqual(len(self.published(self.observe(state(False), 300)).buttonEvents), 0)
|
||||
stock, _ = ioniq6_candidate(False)
|
||||
self.card.CP = stock
|
||||
self.card.distance_personality_tracker = None
|
||||
self.assertEqual(len(self.published(state(False)).buttonEvents), 1)
|
||||
|
||||
def test_base_ioniq_long_without_aol_profile_reserves_conditional_gesture(self):
|
||||
self.card.CP.safetyConfigs[0].safetyParam &= ~0x800
|
||||
self.params.put('DistanceButtonControl', 5, block=True)
|
||||
self.observe(state(True), 10)
|
||||
self.assertEqual(len(self.published(self.observe(state(False), 11)).buttonEvents), 0)
|
||||
|
||||
def test_startup_held_and_invalid_can_do_not_inherit_a_claim(self):
|
||||
self.params.put('DistanceButtonControl', 3, block=True)
|
||||
self.params.put_bool('AlwaysOnLateral', True, block=True)
|
||||
self.card.aol_card_intent = AolCardIntent(read_aol_settings(self.params), explicit_latch=True)
|
||||
self.card.distance_personality_tracker = ButtonTracker()
|
||||
pressed = state(True)
|
||||
self.card.aol_card_intent.update(pressed)
|
||||
self.observe(pressed, 10)
|
||||
release = state(False)
|
||||
self.card.aol_card_intent.update(release)
|
||||
self.observe(release, 11)
|
||||
self.assertFalse(self.card.aol_card_intent.pause_lateral)
|
||||
self.assertEqual(len(self.published(release).buttonEvents), 1)
|
||||
|
||||
self.observe(state(), 20)
|
||||
self.card.aol_card_intent.update(state())
|
||||
self.params.put('LongDistanceButtonControl', 3, block=True)
|
||||
self.observe(state(True), 21)
|
||||
for tick in range(CRUISE_LONG_PRESS - 1):
|
||||
self.observe(state(), 22 + tick)
|
||||
self.assertIsNotNone(self.card.distance_personality_tracker.distance_claim)
|
||||
self.observe(car.CarState(canValid=False), 80)
|
||||
self.assertIsNone(self.card.distance_personality_tracker.distance_claim)
|
||||
self.assertEqual(len(self.published(self.observe(state(False), 81)).buttonEvents), 1)
|
||||
self.observe(state(), 82)
|
||||
self.card.aol_card_intent.update(state())
|
||||
self.observe(state(True), 83)
|
||||
self.card.aol_card_intent.update(state(True))
|
||||
self.observe(state(False), 84)
|
||||
self.card.aol_card_intent.update(state(False))
|
||||
self.assertTrue(self.card.aol_card_intent.pause_lateral)
|
||||
|
||||
self.observe(state(True), 90)
|
||||
self.card.aol_card_intent.update(state(True))
|
||||
timeout = car.CarState(canValid=True, canTimeout=True)
|
||||
self.observe(timeout, 91)
|
||||
self.card.aol_card_intent.update(timeout)
|
||||
self.assertIsNone(self.card.distance_personality_tracker.distance_claim)
|
||||
self.observe(state(False), 92)
|
||||
self.card.aol_card_intent.update(state(False))
|
||||
self.assertTrue(self.card.aol_card_intent.pause_lateral)
|
||||
|
||||
def test_mid_press_saved_change_keeps_sampled_aol_action_reserved(self):
|
||||
self.params.put('DistanceButtonControl', 3, block=True)
|
||||
self.params.put_bool('AlwaysOnLateral', True, block=True)
|
||||
self.card.aol_card_intent = AolCardIntent(read_aol_settings(self.params), explicit_latch=True)
|
||||
self.card.aol_card_intent.update(state())
|
||||
pressed = state(True)
|
||||
self.card.aol_card_intent.update(pressed)
|
||||
self.observe(pressed, 10)
|
||||
self.params.put('DistanceButtonControl', 0, block=True)
|
||||
release = state(False)
|
||||
self.card.aol_card_intent.update(release)
|
||||
self.observe(release, 11)
|
||||
self.assertTrue(self.card.aol_card_intent.pause_lateral)
|
||||
self.assertEqual(len(self.published(release).buttonEvents), 0)
|
||||
|
||||
def test_lkas_first_startup_interleaving_does_not_diverge_from_tracker(self):
|
||||
self.params.put('DistanceButtonControl', 3, block=True)
|
||||
self.params.put_bool('AlwaysOnLateral', True, block=True)
|
||||
self.card.aol_card_intent = AolCardIntent(read_aol_settings(self.params), explicit_latch=True)
|
||||
self.card.distance_personality_tracker = ButtonTracker()
|
||||
lkas = car.CarState(canValid=True, buttonEvents=[car.CarState.ButtonEvent(type=ButtonType.lkas, pressed=True)])
|
||||
self.card.aol_card_intent.update(lkas)
|
||||
self.observe(lkas, 10)
|
||||
pressed = state(True)
|
||||
self.card.aol_card_intent.update(pressed)
|
||||
self.observe(pressed, 11)
|
||||
release = state(False)
|
||||
self.card.aol_card_intent.update(release)
|
||||
self.observe(release, 12)
|
||||
self.assertFalse(self.card.aol_card_intent.pause_lateral)
|
||||
self.assertEqual(len(self.published(release).buttonEvents), 1)
|
||||
|
||||
def test_selfdrived_only_cycles_for_unclaimed_serialized_release(self):
|
||||
self.params.put('LongDistanceButtonControl', 3, block=True)
|
||||
self.observe(state(True), 10)
|
||||
short = self.published(self.observe(state(False), 11))
|
||||
self.observe(state(True), 20)
|
||||
for tick in range(CRUISE_LONG_PRESS - 1):
|
||||
self.observe(state(), 21 + tick)
|
||||
claimed = self.published(self.observe(state(False), 100))
|
||||
self.assertEqual((len(short.buttonEvents), len(claimed.buttonEvents)), (1, 0))
|
||||
drive = SelfdriveD.__new__(SelfdriveD)
|
||||
drive.CP = car.CarParams(openpilotLongitudinalControl=True, notCar=True)
|
||||
drive.params = MagicMock()
|
||||
drive.events = Events()
|
||||
drive.personality = 1
|
||||
drive.initialized = True
|
||||
drive.startup_event = None
|
||||
drive.big_model_loading = False
|
||||
drive.big_model_active = False
|
||||
drive.big_model_failed = False
|
||||
drive.big_model_ready_t = 0.0
|
||||
drive.enabled = False
|
||||
drive.nostalgia_enabled = False
|
||||
drive.CS_prev = car.CarState.new_message()
|
||||
drive.dm_lockout_set = False
|
||||
drive.dm_uncertain_alerted = False
|
||||
drive.distance_traveled = 0.0
|
||||
drive.last_functional_fan_frame = 0
|
||||
drive.recalibrating_seen = False
|
||||
drive.is_ldw_enabled = False
|
||||
drive.calibrated_pose = None
|
||||
drive.excessive_actuation = False
|
||||
drive.not_running_prev = None
|
||||
drive.logged_comm_issue = None
|
||||
drive.sensor_packets = []
|
||||
drive.camera_packets = []
|
||||
drive.cruise_mismatch_counter = 0
|
||||
drive.last_steering_pressed_frame = 0
|
||||
drive.gps_location_service = 'gpsLocationExternal'
|
||||
services = ('controlsState', 'deviceState', 'driverMonitoringState', 'peripheralState',
|
||||
'extrinsicsCalibration', 'modelV2', 'managerState', 'radarState', 'longitudinalPlan',
|
||||
'deviceMotion', 'vehicleParameters', 'carControl')
|
||||
messages = {service: getattr(messaging.new_message(service), service) for service in services}
|
||||
messages['pandaStates'] = []
|
||||
class FakeMaster:
|
||||
frame = 1
|
||||
|
||||
def __init__(self):
|
||||
self.updated = {}
|
||||
self.seen = {}
|
||||
self.valid = {}
|
||||
self.alive = {}
|
||||
self.freq_ok = {}
|
||||
self.recv_frame = {}
|
||||
|
||||
def __getitem__(self, key):
|
||||
return messages[key]
|
||||
|
||||
def all_checks(self, *_):
|
||||
return True
|
||||
|
||||
def all_alive(self, *_):
|
||||
return True
|
||||
|
||||
def all_freq_ok(self, *_):
|
||||
return True
|
||||
|
||||
self.enterContext(patch.multiple(drive, create=True, sm=FakeMaster(), rk=NS(lagging=False),
|
||||
car_events=NS(update=lambda *_: NS(to_msg=list))))
|
||||
for service in (*services, 'userBookmark', 'alertDebug', 'lateralManeuverPlan', 'managerState',
|
||||
'gpsLocationExternal', 'deviceMotion', 'vehicleParameters'):
|
||||
drive.sm.updated[service] = False
|
||||
drive.sm.seen[service] = False
|
||||
drive.sm.valid[service] = True
|
||||
drive.sm.alive[service] = True
|
||||
drive.sm.freq_ok[service] = True
|
||||
drive.sm.recv_frame[service] = 0
|
||||
drive.update_events(short)
|
||||
self.assertEqual(drive.personality, 0)
|
||||
self.assertIn(EventName.personalityChanged, drive.events.names)
|
||||
drive.update_events(claimed)
|
||||
self.assertEqual(drive.personality, 0)
|
||||
self.assertNotIn(EventName.personalityChanged, drive.events.names)
|
||||
@@ -0,0 +1,249 @@
|
||||
"""Honda AOL host qualification and serialized integration tests."""
|
||||
|
||||
import time
|
||||
import unittest
|
||||
from unittest import mock
|
||||
|
||||
from openpilot.cereal import log, messaging
|
||||
from openpilot.common.prefix import OpenpilotPrefix
|
||||
from opendbc.car.structs import car
|
||||
from opendbc.car import gen_empty_fingerprint
|
||||
from opendbc.car.interfaces import RadarInterfaceBase
|
||||
from opendbc.car.honda.interface import CarInterface
|
||||
from opendbc.car.honda.values import CAR, CarControllerParams, HondaFlags
|
||||
from openpilot.common.params import Params
|
||||
from openpilot.selfdrive.car.card import Car
|
||||
from openpilot.selfdrive.selfdrived.selfdrived import SelfdriveD
|
||||
from openpilot.selfdrive.controls.controlsd import Controls
|
||||
from openpilot.starpilot.aol.intent import read_settings
|
||||
from openpilot.starpilot.aol.wire import SafetyState, decode_intent, encode_safety
|
||||
from openpilot.starpilot.car.honda.aol import CLASSIC_BOSCH_AOL_CARS, qualified_honda
|
||||
|
||||
|
||||
def car_state(*, brake=False):
|
||||
return car.CarState(canValid=True, gearShifter=car.CarState.GearShifter.drive,
|
||||
vEgo=20.0, brakePressed=brake)
|
||||
|
||||
|
||||
class HondaFamilyQualificationTests(unittest.TestCase):
|
||||
def setUp(self):
|
||||
# Some upstream interfaces set this shared table while constructing CP.
|
||||
self.addCleanup(setattr, CarControllerParams, 'BOSCH_GAS_LOOKUP_V', list(CarControllerParams.BOSCH_GAS_LOOKUP_V))
|
||||
|
||||
def test_exact_classic_bosch_cp_family_and_rejections(self):
|
||||
self.assertEqual(len(CLASSIC_BOSCH_AOL_CARS), 10)
|
||||
all_honda = tuple(candidate for candidate in CAR if candidate.config.flags & HondaFlags.BOSCH)
|
||||
for candidate in all_honda:
|
||||
with self.subTest(candidate=candidate):
|
||||
cp = CarInterface.get_params(candidate, gen_empty_fingerprint(), [], True, False, False)
|
||||
self.assertEqual(qualified_honda(cp), candidate in CLASSIC_BOSCH_AOL_CARS)
|
||||
if candidate in CLASSIC_BOSCH_AOL_CARS:
|
||||
self.assertEqual(cp.brand, 'honda')
|
||||
self.assertTrue(cp.openpilotLongitudinalControl)
|
||||
self.assertFalse(cp.pcmCruise)
|
||||
self.assertEqual(len(cp.safetyConfigs), 1)
|
||||
self.assertIn(cp.safetyConfigs[0].safetyParam, (2, 3))
|
||||
cp.safetyConfigs[0].safetyParam |= 32
|
||||
self.assertTrue(qualified_honda(cp))
|
||||
cp.safetyConfigs[0].safetyParam |= 4
|
||||
self.assertFalse(qualified_honda(cp))
|
||||
|
||||
for candidate in CLASSIC_BOSCH_AOL_CARS:
|
||||
with self.subTest(stock=candidate):
|
||||
cp = CarInterface.get_params(candidate, gen_empty_fingerprint(), [], False, False, False)
|
||||
self.assertFalse(qualified_honda(cp))
|
||||
|
||||
cp = CarInterface.get_params(CAR.HONDA_ACCORD, gen_empty_fingerprint(), [], True, False, False)
|
||||
cp.passive = True
|
||||
self.assertFalse(qualified_honda(cp))
|
||||
cp.passive = False
|
||||
cp.dashcamOnly = True
|
||||
self.assertFalse(qualified_honda(cp))
|
||||
cp.dashcamOnly = False
|
||||
cp.brand = 'other'
|
||||
self.assertFalse(qualified_honda(cp))
|
||||
cp.brand = 'honda'
|
||||
cp.notCar = True
|
||||
self.assertFalse(qualified_honda(cp))
|
||||
cp.notCar = False
|
||||
cp.flags |= int(HondaFlags.BOSCH_ALT_RADAR)
|
||||
self.assertFalse(qualified_honda(cp))
|
||||
cp.flags &= ~int(HondaFlags.BOSCH_ALT_RADAR)
|
||||
cp.safetyConfigs[0].safetyModel = car.CarParams.SafetyModel.hondaNidec
|
||||
self.assertFalse(qualified_honda(cp))
|
||||
cp.safetyConfigs[0].safetyModel = car.CarParams.SafetyModel.hondaBosch
|
||||
cp.safetyConfigs = [car.CarParams.SafetyConfig(safetyModel=car.CarParams.SafetyModel.hondaBosch, safetyParam=2),
|
||||
car.CarParams.SafetyConfig(safetyModel=car.CarParams.SafetyModel.noOutput, safetyParam=0)]
|
||||
self.assertFalse(qualified_honda(cp))
|
||||
|
||||
def test_card_serializes_opt_in_only_for_actual_eligible_cp(self):
|
||||
class FakeHonda:
|
||||
CC = object()
|
||||
CS = object()
|
||||
|
||||
def __init__(self, candidate, alpha_long):
|
||||
self.CP = CarInterface.get_params(candidate, gen_empty_fingerprint(), [], alpha_long, False, False)
|
||||
|
||||
def init(self, _cp, _recv, _send):
|
||||
pass
|
||||
|
||||
class FakeRadar(RadarInterfaceBase):
|
||||
def update(self, can_packets):
|
||||
return None
|
||||
|
||||
for candidate in sorted(CLASSIC_BOSCH_AOL_CARS, key=str):
|
||||
for alpha_long in (False, True):
|
||||
with self.subTest(candidate=candidate, alpha_long=alpha_long), OpenpilotPrefix(), \
|
||||
mock.patch.dict('os.environ', {'AOL_REPLAY_RUNTIME': '1', 'SIMULATION': '1'}):
|
||||
messaging.reset_context()
|
||||
params = Params()
|
||||
params.put_bool('OpenpilotEnabledToggle', True, block=True)
|
||||
params.put_bool('AlwaysOnLateral', True, block=True)
|
||||
honda = FakeHonda(candidate, alpha_long)
|
||||
card = Car(honda, FakeRadar(honda.CP))
|
||||
self.assertEqual(card.aol_qualified, alpha_long)
|
||||
self.assertEqual(bool(card.CP.safetyConfigs[0].safetyParam & 32), alpha_long)
|
||||
with car.CarParams.from_bytes(params.get('CarParams')) as published:
|
||||
self.assertEqual(bool(published.safetyConfigs[0].safetyParam & 32), alpha_long)
|
||||
|
||||
|
||||
class HondaHostTests(unittest.TestCase):
|
||||
def test_real_car_params_selfdrive_controls_chain_fails_closed(self):
|
||||
class FakeHonda:
|
||||
CC = object()
|
||||
CS = object()
|
||||
|
||||
def __init__(self):
|
||||
self.CP = CarInterface.get_params(CAR.HONDA_CIVIC_BOSCH, gen_empty_fingerprint(), [], True, False, False)
|
||||
|
||||
def update(self, _can):
|
||||
state = car_state()
|
||||
state.cruiseState.available = True
|
||||
return state
|
||||
|
||||
def init(self, _cp, _recv, _send):
|
||||
pass
|
||||
|
||||
def apply(self, _control, _now_ns):
|
||||
return car.CarControl.Actuators(), []
|
||||
|
||||
class FakeRadar(RadarInterfaceBase):
|
||||
def update(self, can_packets):
|
||||
return None
|
||||
|
||||
with OpenpilotPrefix(), mock.patch.dict('os.environ', {'AOL_REPLAY_RUNTIME': '1', 'SIMULATION': '1'}):
|
||||
messaging.reset_context()
|
||||
params = Params()
|
||||
params.put_bool('OpenpilotEnabledToggle', True, block=True)
|
||||
params.put_bool('AlwaysOnLateral', True, block=True)
|
||||
honda = FakeHonda()
|
||||
card = Car(honda, FakeRadar(honda.CP))
|
||||
self.assertEqual(card.CP.safetyConfigs[-1].safetyParam & 0x20, 0x20)
|
||||
with car.CarParams.from_bytes(params.get('CarParams')) as published:
|
||||
self.assertEqual(published.safetyConfigs[-1].safetyParam & 0x20, 0x20)
|
||||
selfdrive = SelfdriveD() # reads the actual card-published CarParams
|
||||
controls = Controls() # reads the same serialized CarParams
|
||||
self.assertTrue(selfdrive.aol_replay and controls.aol_replay)
|
||||
can = messaging.PubMaster(['can'])
|
||||
intent = messaging.SubMaster(['aolIntentWire'])
|
||||
can.send('can', messaging.new_message('can', 1))
|
||||
card.step()
|
||||
intent.update(100)
|
||||
self.assertTrue(intent.seen['aolIntentWire'])
|
||||
self.assertTrue(decode_intent(intent['aolIntentWire']).allowedLatch)
|
||||
selfdrive.step()
|
||||
controls.sm.update(100)
|
||||
cc, _ = controls.state_control()
|
||||
self.assertFalse(cc.latActive or cc.longActive) # no native capability/ack
|
||||
self.assertEqual(selfdrive.aol_axis_decision.mode, 'off')
|
||||
|
||||
source = messaging.PubMaster(['aolSafetyWire', 'modelV2', 'extrinsicsCalibration', 'driverMonitoringState'])
|
||||
time.sleep(0.05) # allow native IPC subscriptions to connect before the first publication
|
||||
model = messaging.new_message('modelV2')
|
||||
model.valid = True
|
||||
calibration = messaging.new_message('extrinsicsCalibration')
|
||||
calibration.valid = True
|
||||
calibration.extrinsicsCalibration.calStatus = log.ExtrinsicsCalibration.Status.calibrated
|
||||
dm = messaging.new_message('driverMonitoringState')
|
||||
dm.valid = True
|
||||
def publish_native(*, requested_long=False):
|
||||
native = messaging.new_message('aolSafetyWire', 0)
|
||||
native.valid = True
|
||||
observed = time.monotonic_ns()
|
||||
native.aolSafetyWire = encode_safety(SafetyState(
|
||||
1, True, observed, observed + 200_000_000, int(car.CarParams.SafetyModel.hondaBosch),
|
||||
card.CP.safetyConfigs[-1].safetyParam, True, requested_long, True, requested_long,
|
||||
'test-panda', selfdrive.aol_session_id))
|
||||
source.send('aolSafetyWire', native)
|
||||
|
||||
for _ in range(3):
|
||||
source.send('modelV2', model)
|
||||
model.clear_write_flag()
|
||||
source.send('extrinsicsCalibration', calibration)
|
||||
calibration.clear_write_flag()
|
||||
source.send('driverMonitoringState', dm)
|
||||
dm.clear_write_flag()
|
||||
publish_native()
|
||||
selfdrive.sm.update(100)
|
||||
controls.sm.update(0)
|
||||
selfdrive.initialized = True
|
||||
with mock.patch.object(selfdrive, 'update_events', side_effect=lambda _cs: selfdrive.events.clear()):
|
||||
can.send('can', messaging.new_message('can', 1))
|
||||
card.step()
|
||||
selfdrive.step()
|
||||
self.assertEqual(selfdrive.aol_axis_decision.mode, 'lateralOnly')
|
||||
controls.sm.update(0)
|
||||
cc, _ = controls.state_control()
|
||||
self.assertTrue(cc.latActive)
|
||||
self.assertFalse(cc.longActive)
|
||||
# The native long request may still reflect a previous combined mode;
|
||||
# this must not interrupt unchanged lateral permission.
|
||||
publish_native(requested_long=True)
|
||||
controls.sm.update(0)
|
||||
cc, _ = controls.state_control()
|
||||
self.assertTrue(cc.latActive)
|
||||
self.assertFalse(cc.longActive)
|
||||
# On the reverse transition, an old lateral-only acknowledgment keeps
|
||||
# lateral active while the newly requested longitudinal axis waits.
|
||||
publish_native()
|
||||
axis_msg = messaging.new_message('aolAxisState')
|
||||
axis_msg.valid = True
|
||||
axis = axis_msg.aolAxisState
|
||||
axis.sessionId = selfdrive.aol_session_id
|
||||
axis.qualified = True
|
||||
axis.desiredLateral = True
|
||||
axis.desiredLongitudinal = True
|
||||
axis.lateralActive = True
|
||||
axis.longitudinalActive = False
|
||||
axis.nativeAcknowledged = True
|
||||
axis.observedMonoTime = time.monotonic_ns()
|
||||
axis.validUntilMonoTime = axis.observedMonoTime + 30_000_000
|
||||
selfdrive.pm.send('aolAxisState', axis_msg)
|
||||
controls.sm.update(0)
|
||||
cc, _ = controls.state_control()
|
||||
self.assertTrue(cc.latActive)
|
||||
self.assertFalse(cc.longActive)
|
||||
|
||||
dm.driverMonitoringState.lockout = True
|
||||
source.send('driverMonitoringState', dm)
|
||||
with mock.patch.object(selfdrive, 'update_events', side_effect=lambda _cs: selfdrive.events.clear()):
|
||||
can.send('can', messaging.new_message('can', 1))
|
||||
card.step()
|
||||
selfdrive.step()
|
||||
self.assertEqual(selfdrive.aol_axis_decision.mode, 'off')
|
||||
controls.sm.update(100)
|
||||
cc, _ = controls.state_control()
|
||||
self.assertFalse(cc.latActive or cc.longActive)
|
||||
|
||||
# Saved AOL off retains the negotiated protocol while ending extra lateral autonomy.
|
||||
params.put_bool('AlwaysOnLateral', False, block=True)
|
||||
assert card.aol_card_intent is not None
|
||||
card.aol_card_intent.settings = read_settings(params)
|
||||
selfdrive.aol_settings = read_settings(params)
|
||||
can.send('can', messaging.new_message('can', 1))
|
||||
card.step()
|
||||
selfdrive.step()
|
||||
controls.sm.update(100)
|
||||
cc, _ = controls.state_control()
|
||||
self.assertFalse(cc.latActive or cc.longActive)
|
||||
self.assertTrue(card.aol_qualified)
|
||||
@@ -0,0 +1,410 @@
|
||||
"""Ioniq 6 host requests remain gated by a separate exact native AOL profile."""
|
||||
|
||||
import unittest
|
||||
import time
|
||||
from types import SimpleNamespace
|
||||
from unittest import mock
|
||||
|
||||
from opendbc.can import CANParser
|
||||
from opendbc.car import Bus, gen_empty_fingerprint
|
||||
from opendbc.car.hyundai.tests.test_ioniq6_longitudinal import controller_fixture
|
||||
from opendbc.car.hyundai.interface import CarInterface
|
||||
from opendbc.car.hyundai.ioniq6_handoff import build_ioniq6_hda2_long_candidate
|
||||
from opendbc.car.hyundai.values import CAR, DBC, HyundaiFlags
|
||||
from opendbc.car.structs import car
|
||||
from openpilot.cereal import messaging
|
||||
from openpilot.common.params import Params
|
||||
from openpilot.common.prefix import OpenpilotPrefix
|
||||
from openpilot.selfdrive.controls.controlsd import Controls
|
||||
from openpilot.starpilot.aol.intent import AolCardIntent, AolSettings
|
||||
from openpilot.starpilot.aol.runtime import current_native, decide_axes, monitoring_lateral_engaged
|
||||
from openpilot.starpilot.aol.vehicle import native_latch_rejected, policy_for
|
||||
from openpilot.starpilot.aol.wire import SafetyState, encode_safety
|
||||
from openpilot.starpilot.car.hyundai.aol import ioniq6_settings_capable, qualified_ioniq6
|
||||
|
||||
|
||||
def candidate(alt: bool):
|
||||
fp = gen_empty_fingerprint()
|
||||
steering, support = ((0x110, 0x362) if alt else (0x50, 0x2A4))
|
||||
fp[2][steering] = 32 if alt else 16
|
||||
fp[2][support] = 32 if alt else 24
|
||||
fp[0][0x3A5] = 24
|
||||
fp[1].update({0x1CF: 8, 0x1AA: 16, 0x35: 32, 0x175: 24, 0xA0: 24, 0xEA: 24,
|
||||
0x1BA: 24, 0x1E5: 16, 0x36A: 16})
|
||||
stock = CarInterface.get_params(CAR.HYUNDAI_IONIQ_6, fp, [], False, False, False)
|
||||
long_cp = build_ioniq6_hda2_long_candidate(stock, fp)
|
||||
assert long_cp is not None
|
||||
return stock, long_cp
|
||||
|
||||
|
||||
def state():
|
||||
return car.CarState(canValid=True, gearShifter=car.CarState.GearShifter.drive, vEgo=20.0)
|
||||
|
||||
|
||||
class Ioniq6HostTests(unittest.TestCase):
|
||||
def test_native_token_loss_resynchronizes_card_without_changing_native_safety(self):
|
||||
from opendbc.safety.tests.test_hyundai_ioniq6_long import TestHyundaiIoniq6Long
|
||||
|
||||
native_test = TestHyundaiIoniq6Long()
|
||||
native_test.setUp()
|
||||
if native_test.release:
|
||||
self.skipTest("RELEASE denies the Ioniq 6 AOL profiles")
|
||||
from opendbc.can import CANPacker
|
||||
for alt, raw in ((False, 0x8815), (True, 0x8895)):
|
||||
with self.subTest(alt=alt), OpenpilotPrefix():
|
||||
_, cp = candidate(alt)
|
||||
cp.safetyConfigs[0].safetyParam = raw
|
||||
native_test.mode(raw)
|
||||
safety = native_test.safety
|
||||
packer = CANPacker(DBC[cp.carFingerprint][Bus.pt])
|
||||
native_test.refresh_required_rx(packer, 1, 1_000_000)
|
||||
safety.set_aol_test_heartbeat(True)
|
||||
owner = AolCardIntent(AolSettings(True, 0, 0, 0, (0, 0, 0), (0, 0, 0)), explicit_latch=True)
|
||||
cs = state()
|
||||
owner.update(cs, now_ns=1_000_000_000)
|
||||
sm = messaging.SubMaster(['aolSafetyWire'])
|
||||
|
||||
def buttons(counter, pressed, safety=safety, packer=packer, cs=cs, owner=owner):
|
||||
self.assertTrue(safety.safety_rx_hook(native_test.packet(packer.make_can_msg(
|
||||
'CRUISE_BUTTONS', 1, {'COUNTER': counter, 'LDA_BTN': int(pressed)}))))
|
||||
cs.buttonEvents = [car.CarState.ButtonEvent(type=car.CarState.ButtonEvent.Type.lkas, pressed=pressed)]
|
||||
owner.update(cs, now_ns=1_000_000_000 + counter * 10_000_000)
|
||||
|
||||
def receipt(stamp, safety=safety, cp=cp, raw=raw, sm=sm):
|
||||
requested, allowed = safety.aol_get_request_mask(), safety.aol_get_permission_mask()
|
||||
message = messaging.new_message('aolSafetyWire', 0, valid=True)
|
||||
message.logMonoTime = stamp
|
||||
message.aolSafetyWire = encode_safety(SafetyState(
|
||||
1, True, stamp, stamp + 200_000_000, int(cp.safetyConfigs[0].safetyModel.raw), raw,
|
||||
bool(allowed & 1), bool(allowed & 2), bool(requested & 1), bool(requested & 2), 'panda', 'selfdrived'))
|
||||
sm.update_msgs(stamp / 1e9, [message.as_reader()])
|
||||
return current_native(sm, cp, now_ns=stamp)
|
||||
|
||||
buttons(2, True)
|
||||
safety.aol_set_host_request(1)
|
||||
self.assertEqual(safety.aol_get_permission_mask(), 1)
|
||||
buttons(3, False)
|
||||
self.assertTrue(owner.allowed_latch)
|
||||
# Brake/gas override affect neither the physical AOL token nor its host latch.
|
||||
for name, values in (('TCS', {'DriverBraking': 1}), ('ACCELERATOR', {'ACCELERATOR_PEDAL': 100})):
|
||||
self.assertTrue(safety.safety_rx_hook(native_test.packet(packer.make_can_msg(name, 1, values))))
|
||||
self.assertEqual(safety.aol_get_permission_mask(), 1)
|
||||
native = receipt(1_040_000_000)
|
||||
self.assertFalse(native_latch_rejected(cp, native))
|
||||
cs.buttonEvents = []
|
||||
cs.gasPressed, cs.brakePressed = True, True
|
||||
owner.update(cs, now_ns=1_040_000_000)
|
||||
self.assertTrue(owner.allowed_latch)
|
||||
safety.set_aol_test_heartbeat(False)
|
||||
self.assertEqual(safety.aol_get_permission_mask(), 0)
|
||||
safety.set_aol_test_heartbeat(True)
|
||||
safety.aol_set_host_request(1)
|
||||
native = receipt(1_050_000_000)
|
||||
self.assertTrue(native_latch_rejected(cp, native))
|
||||
owner.update(cs, now_ns=1_050_000_000, native_rejection_ns=native.observedMonoTime)
|
||||
self.assertFalse(owner.allowed_latch)
|
||||
self.assertEqual(safety.aol_get_permission_mask(), 0)
|
||||
# Native neutral after its reset, then one real LKAS edge restores both.
|
||||
buttons(6, False)
|
||||
buttons(7, True)
|
||||
safety.aol_set_host_request(1)
|
||||
self.assertTrue(owner.allowed_latch)
|
||||
self.assertEqual(safety.aol_get_permission_mask(), 1)
|
||||
|
||||
def test_latch_feedback_requires_fresh_exact_native_receipt(self):
|
||||
_, cp = candidate(False)
|
||||
cp.safetyConfigs[0].safetyParam = 0x8815
|
||||
now = 2_000_000_000
|
||||
with OpenpilotPrefix():
|
||||
sm = messaging.SubMaster(['aolSafetyWire'])
|
||||
for requested, allowed in ((False, False), (False, True), (True, True), (True, False)):
|
||||
now += 100_000_000
|
||||
msg = messaging.new_message('aolSafetyWire', 0, valid=True)
|
||||
msg.logMonoTime = now
|
||||
msg.aolSafetyWire = encode_safety(SafetyState(1, True, now, now + 200_000_000,
|
||||
int(cp.safetyConfigs[0].safetyModel.raw), 0x8815, allowed, False, requested, False, 'panda', 'session'))
|
||||
sm.update_msgs(now / 1e9, [msg.as_reader()])
|
||||
self.assertEqual(native_latch_rejected(cp, current_native(sm, cp, now_ns=now)), requested and not allowed)
|
||||
self.assertFalse(native_latch_rejected(cp, current_native(sm, cp, now_ns=now + 201_000_000)))
|
||||
self.assertFalse(native_latch_rejected(cp, current_native(sm, cp, now_ns=now - 1)))
|
||||
wrong = cp.as_reader().as_builder()
|
||||
wrong.safetyConfigs[0].safetyParam = 0x8895
|
||||
self.assertFalse(native_latch_rejected(wrong, current_native(sm, wrong, now_ns=now)))
|
||||
|
||||
def test_only_exact_tagged_hda2_long_cp_qualifies(self):
|
||||
for alt, raw in ((False, 0x8815), (True, 0x8895)):
|
||||
with self.subTest(alt=alt):
|
||||
stock, long_cp = candidate(alt)
|
||||
self.assertFalse(qualified_ioniq6(stock))
|
||||
self.assertFalse(qualified_ioniq6(long_cp))
|
||||
self.assertFalse(policy_for(long_cp).runtime_supported)
|
||||
cp = long_cp.as_reader().as_builder()
|
||||
cp.safetyConfigs[0].safetyParam = raw
|
||||
self.assertTrue(qualified_ioniq6(cp))
|
||||
self.assertTrue(policy_for(cp).runtime_supported)
|
||||
for wrong in (raw ^ 0x80, raw | 0x20, raw | 0x100, raw & ~0x8000, raw & ~0x800):
|
||||
cp.safetyConfigs[0].safetyParam = wrong
|
||||
self.assertFalse(qualified_ioniq6(cp), hex(wrong))
|
||||
cp.safetyConfigs[0].safetyParam = raw
|
||||
for field, value in (('pcmCruise', True), ('openpilotLongitudinalControl', False),
|
||||
('radarUnavailable', True), ('notCar', True), ('passive', True), ('dashcamOnly', True)):
|
||||
altered = cp.as_reader().as_builder()
|
||||
setattr(altered, field, value)
|
||||
self.assertFalse(qualified_ioniq6(altered), field)
|
||||
altered = cp.as_reader().as_builder()
|
||||
altered.flags |= int(HyundaiFlags.CANFD_ALT_BUTTONS)
|
||||
self.assertFalse(qualified_ioniq6(altered))
|
||||
altered = cp.as_reader().as_builder()
|
||||
altered.safetyConfigs[0].safetyModel = car.CarParams.SafetyModel.noOutput
|
||||
self.assertFalse(qualified_ioniq6(altered))
|
||||
|
||||
def test_stock_settings_capability_is_separate_from_runtime(self):
|
||||
for alt in (False, True):
|
||||
stock, long_cp = candidate(alt)
|
||||
self.assertTrue(ioniq6_settings_capable(stock))
|
||||
self.assertFalse(policy_for(stock).runtime_supported)
|
||||
self.assertTrue(ioniq6_settings_capable(long_cp)) # LONG saved choices do not require AOL's separate bit.
|
||||
tagged = long_cp.as_reader().as_builder()
|
||||
tagged.safetyConfigs[0].safetyParam |= 0x800
|
||||
self.assertTrue(ioniq6_settings_capable(tagged))
|
||||
self.assertTrue(policy_for(tagged).runtime_supported)
|
||||
changed = stock.as_reader().as_builder()
|
||||
changed.safetyConfigs[0].safetyParam |= 0x20
|
||||
self.assertFalse(ioniq6_settings_capable(changed))
|
||||
changed = stock.as_reader().as_builder()
|
||||
changed.flags |= int(HyundaiFlags.CANFD_ALT_BUTTONS)
|
||||
self.assertFalse(ioniq6_settings_capable(changed))
|
||||
with mock.patch('openpilot.starpilot.car.hyundai.aol.IONIQ6_LONG_PREARM_ENABLED', False):
|
||||
self.assertFalse(ioniq6_settings_capable(stock))
|
||||
|
||||
def test_tcs_availability_never_arms_explicit_lateral_latch(self):
|
||||
settings = AolSettings(True, 0.0, 0, 0, (0, 0, 0), (0, 0, 0))
|
||||
owner = AolCardIntent(settings, explicit_latch=True)
|
||||
cs = state()
|
||||
cs.cruiseState.available = True
|
||||
owner.update(cs)
|
||||
self.assertFalse(owner.output(cs)[0])
|
||||
cs.buttonEvents = [car.CarState.ButtonEvent(type=car.CarState.ButtonEvent.Type.lkas, pressed=True)]
|
||||
owner.update(cs)
|
||||
self.assertTrue(owner.output(cs)[0])
|
||||
cs.buttonEvents = [car.CarState.ButtonEvent(type=car.CarState.ButtonEvent.Type.lkas, pressed=False)]
|
||||
owner.update(cs)
|
||||
cs.buttonEvents = []
|
||||
cs.brakePressed = True
|
||||
cs.gasPressed = True
|
||||
owner.update(cs)
|
||||
self.assertTrue(owner.output(cs)[0])
|
||||
cs.buttonEvents = [car.CarState.ButtonEvent(type=car.CarState.ButtonEvent.Type.cancel, pressed=True)]
|
||||
owner.update(cs)
|
||||
self.assertFalse(owner.output(cs)[0])
|
||||
cs.buttonEvents = [car.CarState.ButtonEvent(type=car.CarState.ButtonEvent.Type.mainCruise, pressed=True)]
|
||||
owner.update(cs)
|
||||
self.assertTrue(owner.output(cs)[0])
|
||||
cs.buttonEvents = []
|
||||
cs.accFaulted = True
|
||||
owner.update(cs)
|
||||
self.assertFalse(owner.output(cs)[0])
|
||||
cs.accFaulted = False
|
||||
cs.canValid = False
|
||||
owner.update(cs)
|
||||
self.assertFalse(owner.output(cs)[0])
|
||||
|
||||
def test_startup_held_and_simultaneous_buttons_have_one_combined_edge(self):
|
||||
settings = AolSettings(True, 0.0, 0, 0, (0, 0, 0), (0, 0, 0))
|
||||
main = car.CarState.ButtonEvent.Type.mainCruise
|
||||
lkas = car.CarState.ButtonEvent.Type.lkas
|
||||
cs = state()
|
||||
owner = AolCardIntent(settings, explicit_latch=True)
|
||||
cs.buttonEvents = [car.CarState.ButtonEvent(type=main, pressed=True)]
|
||||
owner.update(cs)
|
||||
self.assertFalse(owner.output(cs)[0]) # held at initialization is ignored
|
||||
cs.buttonEvents = []
|
||||
owner.update(cs)
|
||||
self.assertFalse(owner.output(cs)[0])
|
||||
cs.buttonEvents = [car.CarState.ButtonEvent(type=main, pressed=False)]
|
||||
owner.update(cs)
|
||||
self.assertFalse(owner.output(cs)[0])
|
||||
cs.buttonEvents = [car.CarState.ButtonEvent(type=main, pressed=True),
|
||||
car.CarState.ButtonEvent(type=lkas, pressed=True)]
|
||||
owner.update(cs)
|
||||
self.assertTrue(owner.output(cs)[0]) # one combined gesture, not two toggles
|
||||
cs.buttonEvents = [car.CarState.ButtonEvent(type=main, pressed=False)]
|
||||
owner.update(cs)
|
||||
self.assertTrue(owner.output(cs)[0])
|
||||
cs.buttonEvents = [car.CarState.ButtonEvent(type=lkas, pressed=False)]
|
||||
owner.update(cs)
|
||||
self.assertTrue(owner.output(cs)[0])
|
||||
cs.buttonEvents = [car.CarState.ButtonEvent(type=lkas, pressed=True)]
|
||||
owner.update(cs)
|
||||
self.assertFalse(owner.output(cs)[0])
|
||||
|
||||
def test_native_reset_token_never_grants_without_matching_host_request(self):
|
||||
settings = AolSettings(True, 0.0, 0, 0, (0, 0, 0), (0, 0, 0))
|
||||
owner = AolCardIntent(settings, explicit_latch=True)
|
||||
cs = state()
|
||||
owner.update(cs) # neutral baseline
|
||||
button = car.CarState.ButtonEvent.Type.lkas
|
||||
cs.buttonEvents = [car.CarState.ButtonEvent(type=button, pressed=True)]
|
||||
owner.update(cs)
|
||||
self.assertTrue(owner.output(cs)[0])
|
||||
cs.buttonEvents = [car.CarState.ButtonEvent(type=button, pressed=False)]
|
||||
owner.update(cs)
|
||||
# Native can revoke its idempotent token independently after heartbeat/RX
|
||||
# loss; host's next deliberate toggle requests off, so a new native token
|
||||
# cannot grant actuation until another explicit host-on gesture.
|
||||
cs.buttonEvents = [car.CarState.ButtonEvent(type=button, pressed=True)]
|
||||
owner.update(cs)
|
||||
self.assertFalse(owner.output(cs)[0])
|
||||
|
||||
def test_four_axes_require_exact_native_readback(self):
|
||||
for alt, raw in ((False, 0x8815), (True, 0x8895)):
|
||||
_, cp = candidate(alt)
|
||||
cp.safetyConfigs[0].safetyParam = raw
|
||||
stamp = 1_000_000_000
|
||||
for standard_long, lateral_latch, expected in ((False, False, 'off'), (False, True, 'lateralOnly'),
|
||||
(True, False, 'longitudinalOnly'), (True, True, 'combined')):
|
||||
cp.safetyConfigs[0].safetyParam = raw
|
||||
desired_lat, desired_long = lateral_latch, standard_long
|
||||
wire = encode_safety(SafetyState(1, True, stamp, stamp + 200_000_000,
|
||||
int(car.CarParams.SafetyModel.hyundaiCanfd), raw,
|
||||
desired_lat, desired_long, desired_lat, desired_long,
|
||||
'panda', 'drive-session'))
|
||||
class SafetySM:
|
||||
valid = {'aolSafetyWire': True}
|
||||
alive = {'aolSafetyWire': True}
|
||||
seen = {'aolSafetyWire': True}
|
||||
logMonoTime = {'aolSafetyWire': stamp}
|
||||
|
||||
def __init__(self, payload):
|
||||
self.payload = payload
|
||||
|
||||
def __getitem__(self, key):
|
||||
return self.payload
|
||||
|
||||
sm = SafetySM(wire)
|
||||
native = current_native(sm, cp, now_ns=stamp + 1_000_000, axis_session_id='drive-session')
|
||||
self.assertIsNotNone(native)
|
||||
intent = SimpleNamespace(allowedLatch=lateral_latch, pauseLateral=False, pauseLongitudinal=False)
|
||||
decision = decide_axes(standard_lateral=False, standard_longitudinal=standard_long,
|
||||
intent=intent, native=native, car_state=state(), initialized=True,
|
||||
model_ready=True, no_entry=False, immediate_disable=False, dm_lockout=False,
|
||||
pause_brake_mps=0.0)
|
||||
self.assertEqual(decision.mode, expected)
|
||||
self.assertEqual((decision.desired_lateral, decision.desired_longitudinal), (desired_lat, desired_long))
|
||||
cp.safetyConfigs[0].safetyParam = raw ^ 0x800
|
||||
self.assertIsNone(current_native(sm, cp, now_ns=stamp + 1_000_000, axis_session_id='drive-session'))
|
||||
|
||||
def test_monitoring_requires_fresh_matching_native_lateral_permission(self):
|
||||
stamp = 1_000_000_000
|
||||
class MonitorSM:
|
||||
valid = {'aolAxisState': True, 'aolSafetyWire': True}
|
||||
alive = {'aolAxisState': True, 'aolSafetyWire': True}
|
||||
seen = {'aolAxisState': True, 'aolSafetyWire': True}
|
||||
logMonoTime = {'aolAxisState': stamp, 'aolSafetyWire': stamp}
|
||||
|
||||
def __init__(self):
|
||||
self.axis = SimpleNamespace(qualified=True, sessionId='drive-session', lateralActive=True,
|
||||
nativeAcknowledged=True, observedMonoTime=stamp, validUntilMonoTime=stamp + 30_000_000)
|
||||
self.payload = b''
|
||||
|
||||
def __getitem__(self, key):
|
||||
return self.axis if key == 'aolAxisState' else self.payload
|
||||
|
||||
sm = MonitorSM()
|
||||
for raw in (0x8815, 0x8895):
|
||||
sm.payload = encode_safety(SafetyState(1, True, stamp, stamp + 200_000_000,
|
||||
int(car.CarParams.SafetyModel.hyundaiCanfd), raw,
|
||||
True, False, True, False, 'panda', 'drive-session'))
|
||||
self.assertTrue(monitoring_lateral_engaged(sm, now_ns=stamp + 1_000_000))
|
||||
sm.payload = encode_safety(SafetyState(1, True, stamp, stamp + 200_000_000,
|
||||
int(car.CarParams.SafetyModel.hyundaiCanfd), raw,
|
||||
False, False, True, False, 'panda', 'drive-session'))
|
||||
self.assertFalse(monitoring_lateral_engaged(sm, now_ns=stamp + 1_000_000))
|
||||
self.assertFalse(monitoring_lateral_engaged(sm, now_ns=stamp + 31_000_000))
|
||||
sm.payload = encode_safety(SafetyState(1, True, stamp, stamp + 200_000_000,
|
||||
int(car.CarParams.SafetyModel.hyundaiCanfd), 0x8015,
|
||||
True, False, True, False, 'panda', 'drive-session'))
|
||||
self.assertFalse(monitoring_lateral_engaged(sm, now_ns=stamp + 1_000_000))
|
||||
|
||||
def test_actual_controls_lateral_only_command_has_active_steering_status(self):
|
||||
for alt, raw in ((False, 0x8815), (True, 0x8895)):
|
||||
with self.subTest(alt=alt), OpenpilotPrefix(), mock.patch.dict('os.environ', {'AOL_REPLAY_RUNTIME': '1', 'SIMULATION': '1'}):
|
||||
cp, controller_cs, controller = controller_fixture(alt, aol=True)
|
||||
self.assertEqual(cp.safetyConfigs[0].safetyParam, raw)
|
||||
Params().put('CarParams', cp.to_bytes(), block=True)
|
||||
controls = Controls()
|
||||
self.assertTrue(controls.aol_replay)
|
||||
cs = state()
|
||||
cs.cruiseState.available = True
|
||||
controls.sm.data['carState'] = cs.as_reader()
|
||||
axis = SimpleNamespace(sessionId='drive-session', nativeAcknowledged=True, lateralActive=True,
|
||||
longitudinalActive=False, desiredLateral=True, desiredLongitudinal=False)
|
||||
native = SimpleNamespace(requestedLateral=True, requestedLongitudinal=False,
|
||||
lateralAllowed=True, longitudinalAllowed=False)
|
||||
with mock.patch('openpilot.selfdrive.controls.controlsd.current_axis', return_value=axis), \
|
||||
mock.patch('openpilot.selfdrive.controls.controlsd.current_native', return_value=native):
|
||||
command, _ = controls.state_control()
|
||||
self.assertFalse(command.enabled)
|
||||
self.assertTrue(command.latActive)
|
||||
self.assertFalse(command.longActive)
|
||||
_, frames = controller.update(command.as_reader(), controller_cs, 1_000_000_000)
|
||||
lfa = next(frame for frame in frames if frame[0] == 0x12A)
|
||||
parser = CANParser(DBC[cp.carFingerprint][Bus.pt], [('LFA', 0)], 1)
|
||||
parser.update((1_000_000_000, [lfa]))
|
||||
self.assertEqual(parser.vl['LFA']['LKA_SysIndReq'], 2)
|
||||
self.assertEqual(parser.vl['LFA']['ActToiSta'], 1)
|
||||
scc = next(frame for frame in frames if frame[0] == 0x1A0)
|
||||
parser = CANParser(DBC[cp.carFingerprint][Bus.pt], [('SCC_CONTROL', 0)], 1)
|
||||
parser.update((1_000_000_000, [scc]))
|
||||
self.assertEqual(parser.vl['SCC_CONTROL']['ACCMode'], 0)
|
||||
self.assertEqual(parser.vl['SCC_CONTROL']['aReqValue'], 0.)
|
||||
self.assertEqual(parser.vl['SCC_CONTROL']['aReqRaw'], 0.)
|
||||
|
||||
def test_serialized_ioniq_axis_and_native_receipts_drive_both_controls_and_expire(self):
|
||||
with OpenpilotPrefix(), mock.patch.dict('os.environ', {'AOL_REPLAY_RUNTIME': '0', 'SIMULATION': '1'}):
|
||||
cp, _, _ = controller_fixture(True, aol=True)
|
||||
params = Params()
|
||||
params.put('CarParams', cp.to_bytes(), block=True)
|
||||
params.put_bool('AlwaysOnLateral', True, block=True)
|
||||
controls = Controls()
|
||||
self.assertTrue(controls.aol_replay)
|
||||
base = time.monotonic_ns()
|
||||
|
||||
def command(stamp: int, *, longitudinal: bool, native_receipt: bool = True):
|
||||
car_msg = messaging.new_message('carState', valid=True)
|
||||
car_msg.logMonoTime = stamp
|
||||
car_msg.carState = state()
|
||||
drive_msg = messaging.new_message('selfdriveState', valid=True)
|
||||
drive_msg.logMonoTime = stamp
|
||||
drive_msg.selfdriveState.enabled = longitudinal
|
||||
axis_msg = messaging.new_message('aolAxisState', valid=True)
|
||||
axis_msg.logMonoTime = stamp
|
||||
axis = axis_msg.aolAxisState
|
||||
axis.qualified = axis.nativeAcknowledged = axis.lateralActive = axis.desiredLateral = True
|
||||
axis.longitudinalActive = axis.desiredLongitudinal = longitudinal
|
||||
axis.sessionId = 'ioniq-drive'
|
||||
axis.sourceCarStateMonoTime = stamp
|
||||
axis.observedMonoTime = stamp
|
||||
axis.validUntilMonoTime = stamp + 30_000_000
|
||||
events = [car_msg, drive_msg, axis_msg]
|
||||
if native_receipt:
|
||||
safety_msg = messaging.new_message('aolSafetyWire', 0, valid=True)
|
||||
safety_msg.logMonoTime = stamp
|
||||
safety_msg.aolSafetyWire = encode_safety(SafetyState(
|
||||
1, True, stamp, stamp + 200_000_000,
|
||||
int(car.CarParams.SafetyModel.hyundaiCanfd), 0x8895,
|
||||
True, longitudinal, True, longitudinal, 'panda', 'ioniq-drive'))
|
||||
events.append(safety_msg)
|
||||
controls.sm.update_msgs(stamp / 1e9, [messaging.log_from_bytes(event.to_bytes()) for event in events])
|
||||
with mock.patch('openpilot.selfdrive.controls.controlsd.time.monotonic_ns', return_value=stamp + 1_000_000):
|
||||
return controls.state_control()[0]
|
||||
|
||||
combined = command(base, longitudinal=True)
|
||||
self.assertTrue(combined.latActive and combined.longActive)
|
||||
lateral_only = command(base + 10_000_000, longitudinal=False)
|
||||
self.assertTrue(lateral_only.latActive)
|
||||
self.assertFalse(lateral_only.longActive)
|
||||
restored = command(base + 20_000_000, longitudinal=True)
|
||||
self.assertTrue(restored.latActive and restored.longActive)
|
||||
lost = command(base + 250_000_000, longitudinal=True, native_receipt=False)
|
||||
self.assertFalse(lost.latActive or lost.longActive)
|
||||
@@ -0,0 +1,115 @@
|
||||
import unittest
|
||||
from types import SimpleNamespace
|
||||
from unittest.mock import Mock, patch
|
||||
|
||||
from opendbc.car import gen_empty_fingerprint
|
||||
from opendbc.car.can_definitions import CanData
|
||||
from opendbc.car.gm.interface import CarInterface as GMInterface
|
||||
from opendbc.car.gm.values import CAR as GMCar, GMFlags
|
||||
from openpilot.cereal import messaging
|
||||
from openpilot.selfdrive.car.card import Car, EventName
|
||||
from openpilot.selfdrive.pandad import can_capnp_to_list
|
||||
|
||||
|
||||
class CardInitLifecycleTest(unittest.TestCase):
|
||||
def test_bolt_pedal_waits_for_control_then_initializes_once_before_output(self):
|
||||
fingerprint = gen_empty_fingerprint()
|
||||
fingerprint[0][0x201] = 6
|
||||
with patch('opendbc.car.gm.interface.Params') as saved:
|
||||
saved.return_value.get_bool.return_value = True
|
||||
cp = GMInterface.get_params(GMCar.CHEVROLET_BOLT_ACC_2022_2023_PEDAL,
|
||||
fingerprint, [], False, False, False)
|
||||
self.assertTrue(cp.flags & GMFlags.PEDAL_LONG.value)
|
||||
|
||||
order = []
|
||||
card = Car.__new__(Car)
|
||||
card.CP = cp
|
||||
card.ci_initialized = False
|
||||
card.initialized_prev = False
|
||||
card.ioniq6_long_prearmed = False
|
||||
card.ioniq6_long_selected = False
|
||||
card.can_callbacks = (lambda wait_for_one=False: [], lambda frames: None)
|
||||
|
||||
def apply(*_):
|
||||
order.append('apply')
|
||||
return object(), [CanData(0x200, b'\x00' * 8, 0)]
|
||||
|
||||
fake_ci = SimpleNamespace(
|
||||
init=Mock(side_effect=lambda *_: order.append('init')),
|
||||
apply=Mock(side_effect=apply),
|
||||
)
|
||||
self.enterContext(patch.object(card, 'CI', fake_ci, create=True))
|
||||
self.enterContext(patch.object(card, 'params', SimpleNamespace(put_bool=lambda key, value: order.append((key, value))), create=True))
|
||||
sent = []
|
||||
|
||||
def publish(service, packet):
|
||||
assert service == 'sendcan'
|
||||
sent.append(packet)
|
||||
order.append(('sendcan', tuple(can_capnp_to_list([packet], msgtype='sendcan')[0][1]),
|
||||
bool(messaging.log_from_bytes(packet).valid)))
|
||||
|
||||
self.enterContext(patch.object(card, 'pm', SimpleNamespace(send=publish), create=True))
|
||||
card.state_publish = Mock(side_effect=lambda *_: order.append('carState'))
|
||||
cs = SimpleNamespace(canValid=True, canTimeout=False)
|
||||
card.state_update = Mock(return_value=(cs, None))
|
||||
control = object()
|
||||
|
||||
class SubMaster:
|
||||
valid = {'carControl': False}
|
||||
alive = {'carControl': False}
|
||||
seen = {'onroadEvents': False}
|
||||
events = []
|
||||
|
||||
def __getitem__(self, key):
|
||||
return self.events if key == 'onroadEvents' else control
|
||||
|
||||
def all_alive(self, services):
|
||||
return all(self.alive[name] for name in services)
|
||||
|
||||
sm = SubMaster()
|
||||
self.enterContext(patch.object(card, 'sm', sm, create=True))
|
||||
for _ in range(30):
|
||||
card.step()
|
||||
self.assertNotIn('init', order)
|
||||
sm.seen['onroadEvents'] = True
|
||||
sm.events = [SimpleNamespace(name=EventName.selfdriveInitializing)]
|
||||
card.step()
|
||||
self.assertNotIn('init', order)
|
||||
|
||||
sm.events = []
|
||||
card.step() # Controls still have no live producer.
|
||||
self.assertNotIn('init', order)
|
||||
sm.valid['carControl'] = True
|
||||
sm.alive['carControl'] = True
|
||||
cs.canValid = False
|
||||
card.step() # A live controller cannot initialize against invalid CAN.
|
||||
self.assertNotIn('init', order)
|
||||
|
||||
cs.canValid = True
|
||||
card.step()
|
||||
self.assertEqual(order[-5:-1], ['carState', 'init', ('ControlsReady', True), 'apply'])
|
||||
self.assertEqual(order[-1][0], 'sendcan')
|
||||
self.assertEqual([(frame[0], frame[2]) for frame in order[-1][1]], [(0x200, 0)])
|
||||
self.assertTrue(order[-1][2])
|
||||
self.assertTrue(all(not can_capnp_to_list([packet], msgtype='sendcan')[0][1] for packet in sent[:-1]))
|
||||
self.assertTrue(all(not messaging.log_from_bytes(packet).valid for packet in sent[:-1]))
|
||||
card.step()
|
||||
fake_ci.init.assert_called_once_with(cp, *card.can_callbacks)
|
||||
self.assertEqual(order.count(('ControlsReady', True)), 1)
|
||||
|
||||
applied = fake_ci.apply.call_count
|
||||
sent_before = len(sent)
|
||||
sm.valid['carControl'] = False
|
||||
card.step() # A live but invalid native command cannot renew CAN output.
|
||||
self.assertEqual(fake_ci.apply.call_count, applied)
|
||||
self.assertEqual(len(sent), sent_before)
|
||||
sm.valid['carControl'] = True
|
||||
card.step()
|
||||
self.assertEqual(fake_ci.apply.call_count, applied + 1)
|
||||
self.assertEqual(len(sent), sent_before + 1)
|
||||
self.assertTrue(messaging.log_from_bytes(sent[-1]).valid)
|
||||
fake_ci.init.assert_called_once_with(cp, *card.can_callbacks)
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
unittest.main()
|
||||
@@ -0,0 +1,28 @@
|
||||
"""Capability-aware preference enrollment, separate from runtime authority."""
|
||||
from pathlib import Path
|
||||
|
||||
from openpilot.starpilot.aol.vehicle import policy_for
|
||||
from openpilot.starpilot.state_migration import _atomic_write, _params_lock, _read_value
|
||||
|
||||
|
||||
def default_enabled(cp) -> bool:
|
||||
if cp is None or getattr(cp, 'notCar', True) or getattr(cp, 'passive', True) or getattr(cp, 'dashcamOnly', True):
|
||||
return False
|
||||
policy = policy_for(cp)
|
||||
return bool(policy.settings_supported and policy.intent_supported)
|
||||
|
||||
|
||||
def enroll_missing_preference(params, cp) -> bool:
|
||||
if not default_enabled(cp):
|
||||
return False
|
||||
try:
|
||||
namespace = Path(params.get_param_path()).absolute()
|
||||
with _params_lock(namespace):
|
||||
try:
|
||||
_read_value(namespace / 'AlwaysOnLateral')
|
||||
except FileNotFoundError:
|
||||
_atomic_write(namespace / 'AlwaysOnLateral', b'1')
|
||||
return True
|
||||
except (OSError, ValueError):
|
||||
return False
|
||||
return False
|
||||
@@ -0,0 +1,63 @@
|
||||
def armed_lateral_current(cp, cs, intent, native, *, source_ns, now_ns, axis_session_id) -> bool:
|
||||
"""Retain only a current deliberately armed native-permitted lateral owner."""
|
||||
from opendbc.car.structs import car
|
||||
from openpilot.starpilot.aol.vehicle import policy_for
|
||||
from openpilot.starpilot.aol.runtime import AXIS_MAX_AGE_NS, SAFETY_MAX_AGE_NS, native_inventory_matches_cp
|
||||
try:
|
||||
return bool(policy_for(cp).intent_supported and
|
||||
cs.canValid and not cs.canTimeout and not cs.accFaulted and not cs.doorOpen and
|
||||
not cs.steerFaultTemporary and not cs.steerFaultPermanent and cs.cruiseState.available and
|
||||
cs.gearShifter in (car.CarState.GearShifter.drive, car.CarState.GearShifter.low,
|
||||
car.CarState.GearShifter.sport, car.CarState.GearShifter.brake) and
|
||||
intent is not None and intent.settingsQualified and intent.allowedLatch and intent.lateralArmed and
|
||||
intent.producerSessionId and intent.carStateLogMonoTime == source_ns and
|
||||
0 < source_ns <= intent.observedMonoTime <= now_ns <= intent.validUntilMonoTime and
|
||||
now_ns - source_ns <= AXIS_MAX_AGE_NS and
|
||||
intent.validUntilMonoTime - intent.observedMonoTime <= SAFETY_MAX_AGE_NS and
|
||||
native is not None and native.compatible and int(native.protocolVersion) == 1 and
|
||||
axis_session_id and native.axisSessionId == axis_session_id and
|
||||
native.requestedLateral and native.lateralAllowed and native.pandaInventory and
|
||||
native_inventory_matches_cp(native, cp) and
|
||||
0 < native.observedMonoTime <= now_ns <= native.validUntilMonoTime and
|
||||
native.validUntilMonoTime - native.observedMonoTime <= SAFETY_MAX_AGE_NS)
|
||||
except (AttributeError, KeyError, IndexError, TypeError, ValueError):
|
||||
return False
|
||||
|
||||
|
||||
def lateral_event_context(self, CS, intent, native, now_ns, ordinary_lag):
|
||||
from openpilot.selfdrive.selfdrived.events import EventName
|
||||
from openpilot.cereal import log
|
||||
dm = self.sm['driverMonitoringState']
|
||||
needs_exception = bool(dm.alertLevel == log.DriverMonitoringState.AlertLevel.three or
|
||||
dm.lockout or dm.alwaysOnLockout or EventName.tooDistracted in self.events.names)
|
||||
retained_dm_lateral = bool(needs_exception and intent is not None and
|
||||
getattr(self, 'aol_dm_retained_owner', None) == (intent.producerSessionId, self.aol_session_id) and
|
||||
self.sm.all_checks(['driverMonitoringState']) and
|
||||
armed_lateral_current(self.CP, CS, intent, native, source_ns=self.aol_car_state_log_ns,
|
||||
now_ns=now_ns, axis_session_id=self.aol_session_id))
|
||||
lateral_events = [name for name in self.events.names if not (
|
||||
name == EventName.selfdrivedLagging and ordinary_lag or
|
||||
name == EventName.tooDistracted and retained_dm_lateral)]
|
||||
return retained_dm_lateral, lateral_events
|
||||
|
||||
|
||||
def observe_owner(self, intent, previous_intent):
|
||||
from openpilot.cereal import log
|
||||
AlertLevel = log.DriverMonitoringState.AlertLevel
|
||||
if (intent is None or not intent.allowedLatch or previous_intent is None or
|
||||
intent.producerSessionId != previous_intent.producerSessionId):
|
||||
self.aol_dm_retained_owner = None
|
||||
elif self.aol_axis_decision.lateral_active and previous_intent.allowedLatch:
|
||||
self.aol_dm_retained_owner = (intent.producerSessionId, self.aol_session_id)
|
||||
if (self.sm['driverMonitoringState'].alertLevel == AlertLevel.three or
|
||||
self.sm['driverMonitoringState'].lockout or self.sm['driverMonitoringState'].alwaysOnLockout):
|
||||
self.aol_availability_long_inhibit = True
|
||||
self.aol_dm_lateral_inhibit = False
|
||||
elif intent is not None and not intent.allowedLatch:
|
||||
self.aol_dm_lateral_inhibit = False
|
||||
|
||||
|
||||
def dm_clear(self):
|
||||
from openpilot.cereal import log
|
||||
return (self.sm['driverMonitoringState'].alertLevel != log.DriverMonitoringState.AlertLevel.three and
|
||||
not self.sm['driverMonitoringState'].lockout and not self.sm['driverMonitoringState'].alwaysOnLockout)
|
||||
@@ -0,0 +1,171 @@
|
||||
"""Original carState companions from Card's nonconflating intent transport."""
|
||||
from dataclasses import replace
|
||||
|
||||
from openpilot.cereal import messaging
|
||||
from openpilot.starpilot.aol.runtime import INTENT_MAX_AGE_NS, SAFETY_MAX_AGE_NS
|
||||
from openpilot.starpilot.aol.wire import INTENT_SERVICE, decode_intent
|
||||
|
||||
MAX_COMPANIONS = 16
|
||||
MAX_DRAIN = 32
|
||||
|
||||
|
||||
class IntentCompanion:
|
||||
def __init__(self, sock):
|
||||
self.sock = sock
|
||||
self.samples = {}
|
||||
self.latest = None
|
||||
self.session = None
|
||||
self.sequence = -1
|
||||
self.sequence_stamp = 0
|
||||
self.integrity_epoch = 0
|
||||
self.verified_sm_projection = None
|
||||
self.raw_projections = {}
|
||||
self.last_rejection = None
|
||||
self.last_drain_count = 0
|
||||
self.last_drain_ok = True
|
||||
|
||||
def _observe(self, stamp, valid, raw):
|
||||
stamp = int(stamp)
|
||||
intent = decode_intent(raw)
|
||||
valid = bool(valid and intent is not None and intent.carStateLogMonoTime == stamp)
|
||||
previous = self.samples.get(stamp)
|
||||
if self.latest is not None and stamp < self.latest[0] and previous is None:
|
||||
valid = False
|
||||
if intent is not None and (self.latest is None or stamp >= self.latest[0]):
|
||||
if self.session is not None and intent.producerSessionId != self.session:
|
||||
self.integrity_epoch += 1
|
||||
self.verified_sm_projection = None
|
||||
self.raw_projections.clear()
|
||||
self.samples.clear()
|
||||
self.sequence = -1
|
||||
self.sequence_stamp = 0
|
||||
self.session = intent.producerSessionId
|
||||
if previous is not None:
|
||||
valid = bool(valid and previous[1] and previous[2] == intent)
|
||||
if valid and intent is not None:
|
||||
if (stamp > self.sequence_stamp and intent.producerSessionId == self.session and
|
||||
intent.sequence <= self.sequence):
|
||||
valid = False
|
||||
for other_stamp, other_valid, other in self.samples.values():
|
||||
if other_valid and other is not None and other.producerSessionId == intent.producerSessionId and (
|
||||
other_stamp < stamp and other.sequence >= intent.sequence or
|
||||
other_stamp > stamp and other.sequence <= intent.sequence):
|
||||
valid = False
|
||||
if valid and intent is not None and (self.latest is None or stamp >= self.latest[0]):
|
||||
self.sequence = max(self.sequence, intent.sequence)
|
||||
self.sequence_stamp = max(self.sequence_stamp, stamp)
|
||||
if not valid:
|
||||
self.integrity_epoch += 1
|
||||
self.verified_sm_projection = None
|
||||
self.raw_projections.clear()
|
||||
sample = (stamp, valid, intent)
|
||||
self.samples[stamp] = sample
|
||||
if self.latest is None or stamp >= self.latest[0]:
|
||||
self.latest = sample
|
||||
while len(self.samples) > MAX_COMPANIONS:
|
||||
del self.samples[min(self.samples)]
|
||||
|
||||
@staticmethod
|
||||
def _fresh(sample, now_ns):
|
||||
stamp, valid, intent = sample
|
||||
return bool(valid and intent is not None and intent.settingsQualified and intent.producerSessionId and
|
||||
0 < stamp <= now_ns and now_ns - stamp <= INTENT_MAX_AGE_NS and
|
||||
intent.observedMonoTime <= now_ns <= intent.validUntilMonoTime and
|
||||
now_ns - intent.observedMonoTime <= INTENT_MAX_AGE_NS)
|
||||
|
||||
def drain(self, *, max_messages=MAX_DRAIN):
|
||||
"""Read bounded transport before sampling the decision clock."""
|
||||
self.last_drain_count = 0
|
||||
self.last_drain_ok = False
|
||||
for _ in range(max(0, min(MAX_DRAIN, max_messages))):
|
||||
message = messaging.recv_one_or_none(self.sock)
|
||||
if message is None:
|
||||
break
|
||||
self.last_drain_count += 1
|
||||
self._observe(message.logMonoTime, message.valid, message.aolIntentWire)
|
||||
sample = self.samples.get(int(message.logMonoTime))
|
||||
if sample is not None and sample[1]:
|
||||
self.raw_projections[sample[0]] = (sample[2], self.integrity_epoch, self.session)
|
||||
while len(self.raw_projections) > MAX_COMPANIONS:
|
||||
del self.raw_projections[min(self.raw_projections)]
|
||||
else:
|
||||
self.integrity_epoch += 1
|
||||
self.verified_sm_projection = None
|
||||
self.raw_projections.clear()
|
||||
self.samples.clear()
|
||||
if self.latest is not None:
|
||||
self.latest = (self.latest[0], False, self.latest[2])
|
||||
self.samples[self.latest[0]] = self.latest
|
||||
self.last_rejection = 'drain_overrun'
|
||||
return False
|
||||
self.last_drain_ok = True
|
||||
return True
|
||||
|
||||
def _reject(self, reason):
|
||||
self.last_rejection = reason
|
||||
return None
|
||||
|
||||
def current(self, sm, *, source_ns, now_ns, drain=True):
|
||||
self.last_rejection = None
|
||||
if drain and not self.drain():
|
||||
return self._reject('drain_overrun')
|
||||
if not self.last_drain_ok:
|
||||
return self._reject('drain_overrun')
|
||||
projection = None
|
||||
if sm.seen.get(INTENT_SERVICE, False):
|
||||
stamp = int(sm.logMonoTime[INTENT_SERVICE])
|
||||
raw_verified = self.raw_projections.get(stamp)
|
||||
raw_current = bool(raw_verified is not None and raw_verified[0].settingsQualified and
|
||||
raw_verified[0].producerSessionId and 0 < raw_verified[0].observedMonoTime <= now_ns <=
|
||||
raw_verified[0].validUntilMonoTime)
|
||||
verified = ((stamp, *raw_verified) if raw_current else self.verified_sm_projection)
|
||||
if (verified is not None and verified[0] == stamp and verified[2] == self.integrity_epoch and
|
||||
verified[3] == self.session and sm.valid.get(INTENT_SERVICE, False) and
|
||||
sm.alive.get(INTENT_SERVICE, False) and 0 < stamp <= now_ns <= stamp + SAFETY_MAX_AGE_NS and
|
||||
(stamp < source_ns or (source_ns == 0 or source_ns == stamp) and
|
||||
0 < stamp <= now_ns <= stamp + SAFETY_MAX_AGE_NS) and
|
||||
now_ns - stamp > INTENT_MAX_AGE_NS and decode_intent(sm[INTENT_SERVICE]) == verified[1]):
|
||||
# This exact previously accepted SM identity contributes restrictions
|
||||
# only; it never replaces the fresh raw source pair or latest sample.
|
||||
projection = (stamp, True, verified[1])
|
||||
else:
|
||||
self._observe(stamp, sm.valid.get(INTENT_SERVICE, False), sm[INTENT_SERVICE])
|
||||
for stamp in tuple(self.samples):
|
||||
if not 0 < stamp <= now_ns or now_ns - stamp > INTENT_MAX_AGE_NS:
|
||||
del self.samples[stamp]
|
||||
for stamp in tuple(self.raw_projections):
|
||||
if not 0 < stamp <= now_ns or now_ns - stamp > SAFETY_MAX_AGE_NS:
|
||||
del self.raw_projections[stamp]
|
||||
if not (sm.seen.get(INTENT_SERVICE, False) and sm.valid.get(INTENT_SERVICE, False) and
|
||||
sm.alive.get(INTENT_SERVICE, False)):
|
||||
return self._reject('sm_unavailable')
|
||||
pair = self.samples.get(source_ns)
|
||||
sm_sample = projection if projection is not None else self.samples.get(int(sm.logMonoTime[INTENT_SERVICE]))
|
||||
if pair is None or self.latest is None or sm_sample is None:
|
||||
return self._reject('missing_pair_latest_or_sm')
|
||||
fresh_samples = (pair, self.latest) if projection is not None else (pair, self.latest, sm_sample)
|
||||
for label, sample in zip(('pair', 'latest', 'sm'), fresh_samples, strict=False):
|
||||
if not self._fresh(sample, now_ns):
|
||||
stamp, valid, value = sample
|
||||
reason = ('invalid' if not valid or value is None else
|
||||
'settings_or_session' if not value.settingsQualified or not value.producerSessionId else
|
||||
'future' if stamp > now_ns or value.observedMonoTime > now_ns else
|
||||
'nonpositive_stamp' if stamp <= 0 else
|
||||
'expired')
|
||||
return self._reject(label + '_' + reason)
|
||||
if self.latest[0] < source_ns:
|
||||
return self._reject('latest_older_than_source')
|
||||
intent = pair[2]
|
||||
for sample in (self.latest, sm_sample):
|
||||
_, _, latest = sample
|
||||
if latest.producerSessionId != intent.producerSessionId:
|
||||
return self._reject('producer_session_mismatch')
|
||||
intent = replace(intent, allowedLatch=intent.allowedLatch and latest.allowedLatch,
|
||||
pauseLateral=intent.pauseLateral or latest.pauseLateral,
|
||||
pauseLongitudinal=intent.pauseLongitudinal or latest.pauseLongitudinal,
|
||||
lateralArmed=intent.lateralArmed and latest.lateralArmed,
|
||||
optionalSetRelease=intent.optionalSetRelease and latest.optionalSetRelease)
|
||||
if projection is None:
|
||||
# Save only after every ordinary fresh pair/latest/SM guard succeeded.
|
||||
self.verified_sm_projection = (sm_sample[0], sm_sample[2], self.integrity_epoch, self.session)
|
||||
return intent
|
||||
@@ -0,0 +1,239 @@
|
||||
"""Bounded AOL axis requests and native acknowledgment checks.
|
||||
|
||||
The requester and the actuator are deliberately separate: a native permission
|
||||
cannot be required before the host sends its first desired-axis request.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from dataclasses import dataclass
|
||||
|
||||
from opendbc.car.structs import car
|
||||
from openpilot.starpilot.aol.vehicle import native_admission_key, native_matches_cp, native_profile_supported
|
||||
from openpilot.starpilot.aol.wire import INTENT_SERVICE, SAFETY_SERVICE, IntentState, decode_intent, decode_safety
|
||||
|
||||
INTENT_MAX_AGE_NS = 30_000_000
|
||||
SAFETY_MAX_AGE_NS = 200_000_000
|
||||
AXIS_MAX_AGE_NS = 30_000_000
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class AxisDecision:
|
||||
desired_lateral: bool = False
|
||||
desired_longitudinal: bool = False
|
||||
lateral_active: bool = False
|
||||
longitudinal_active: bool = False
|
||||
native_acknowledged: bool = False
|
||||
|
||||
@property
|
||||
def mode(self) -> str:
|
||||
if self.lateral_active and self.longitudinal_active:
|
||||
return 'combined'
|
||||
if self.lateral_active:
|
||||
return 'lateralOnly'
|
||||
if self.longitudinal_active:
|
||||
return 'longitudinalOnly'
|
||||
return 'off'
|
||||
|
||||
|
||||
def current_intent(sm, *, car_state_ns: int, now_ns: int, previous: IntentState | None = None, companion=None, drain=True):
|
||||
if companion is not None:
|
||||
return companion.current(sm, source_ns=car_state_ns, now_ns=now_ns, drain=drain)
|
||||
if not (sm.valid[INTENT_SERVICE] and sm.alive[INTENT_SERVICE] and sm.seen[INTENT_SERVICE]):
|
||||
return None
|
||||
intent = decode_intent(sm[INTENT_SERVICE])
|
||||
if intent is None:
|
||||
return None
|
||||
# Intent is published before its carState wakeup. If the carState socket
|
||||
# times out exactly between those sends, only the already accepted companion
|
||||
# for that *same retained carState* may bridge the publication boundary.
|
||||
# Revalidate its original lease; never accept a newer intent for older CAN.
|
||||
if (intent.carStateLogMonoTime > car_state_ns and previous is not None and
|
||||
previous.carStateLogMonoTime == car_state_ns and intent.settingsQualified and
|
||||
intent.producerSessionId == previous.producerSessionId and
|
||||
int(sm.logMonoTime[INTENT_SERVICE]) == intent.carStateLogMonoTime and
|
||||
intent.observedMonoTime <= now_ns <= intent.validUntilMonoTime and
|
||||
now_ns - intent.observedMonoTime <= INTENT_MAX_AGE_NS):
|
||||
intent = previous
|
||||
message_ns = int(previous.carStateLogMonoTime)
|
||||
else:
|
||||
message_ns = int(sm.logMonoTime[INTENT_SERVICE])
|
||||
observed = int(intent.observedMonoTime)
|
||||
produced_for = int(intent.carStateLogMonoTime)
|
||||
if (not intent.settingsQualified or not intent.producerSessionId or produced_for <= 0 or
|
||||
message_ns != produced_for or
|
||||
produced_for > car_state_ns or car_state_ns - produced_for > INTENT_MAX_AGE_NS or
|
||||
observed > now_ns or now_ns > int(intent.validUntilMonoTime) or
|
||||
now_ns - observed > INTENT_MAX_AGE_NS):
|
||||
return None
|
||||
return intent
|
||||
|
||||
|
||||
def native_inventory_matches_cp(state, CP) -> bool:
|
||||
"""Bind the selected serial to the ordered same-read configuration inventory.
|
||||
|
||||
Historical v1 wires retain single-Panda compatibility only; they carry no
|
||||
inventory qualification. Multi-Panda admission requires coherent production v2
|
||||
with the exact configuration and one owner.
|
||||
The source PandaStates stamp is BOOTTIME provenance, never a monotonic lease.
|
||||
"""
|
||||
if not state.pandaInventory:
|
||||
return state.sourcePandaStatesMonoTime == 0 and len(CP.safetyConfigs) == 1
|
||||
try:
|
||||
slots = state.pandaInventory
|
||||
configs = CP.safetyConfigs
|
||||
if state.sourcePandaStatesMonoTime <= 0 or not 1 <= len(slots) <= 2 or len(slots) != len(configs):
|
||||
return False
|
||||
serials = [slot.hardwareSerial for slot in slots]
|
||||
if any(not serial for serial in serials) or len(set(serials)) != len(serials):
|
||||
return False
|
||||
owners = []
|
||||
for index, (slot, config) in enumerate(zip(slots, configs, strict=True)):
|
||||
if slot.safetyRxChecksInvalid or slot.heartbeatLost or slot.faults:
|
||||
return False
|
||||
if (slot.slotIndex != index or slot.safetyModel != int(config.safetyModel.raw) or
|
||||
slot.safetyParam != int(config.safetyParam) or slot.alternativeExperience != int(CP.alternativeExperience)):
|
||||
return False
|
||||
if str(config.safetyModel) in ('silent', 'noOutput'):
|
||||
if slot.safetyParam != 0 or slot.controlsAllowed:
|
||||
return False
|
||||
else:
|
||||
owners.append(slot)
|
||||
return bool(len(owners) == 1 and owners[0].hardwareSerial == state.pandaSerial and
|
||||
owners[0].safetyModel == state.safetyModel and owners[0].safetyParam == state.safetyParam)
|
||||
except (AttributeError, TypeError, ValueError, OverflowError, IndexError):
|
||||
return False
|
||||
|
||||
|
||||
def _clear_native_admission(sm):
|
||||
try:
|
||||
sm._aol_native_admission = None
|
||||
except Exception:
|
||||
# Custom/slotted owners may not support this optional optimization.
|
||||
pass
|
||||
|
||||
|
||||
def _native_admission_matches(sm, CP, state, raw):
|
||||
key = None
|
||||
try:
|
||||
cp_key = native_admission_key(CP) if state.pandaInventory else None
|
||||
if cp_key is not None and isinstance(raw, bytes):
|
||||
key = (raw, int(sm.logMonoTime[SAFETY_SERVICE]), cp_key)
|
||||
if getattr(sm, '_aol_native_admission', None) == key:
|
||||
return True
|
||||
except Exception:
|
||||
# Cache plumbing may never replace or prevent the original validation.
|
||||
key = None
|
||||
matches = (native_matches_cp(CP, int(state.safetyModel), int(state.safetyParam)) and
|
||||
native_inventory_matches_cp(state, CP))
|
||||
try:
|
||||
# Re-read mutable CP on a miss before binding the successful result.
|
||||
sm._aol_native_admission = key if matches and key is not None and native_admission_key(CP) == key[2] else None
|
||||
except Exception:
|
||||
_clear_native_admission(sm)
|
||||
return matches
|
||||
|
||||
|
||||
def current_native(sm, CP, *, now_ns: int, axis_session_id: str | None = None):
|
||||
if not (sm.valid[SAFETY_SERVICE] and sm.alive[SAFETY_SERVICE] and sm.seen[SAFETY_SERVICE]):
|
||||
_clear_native_admission(sm)
|
||||
return None
|
||||
raw = sm[SAFETY_SERVICE]
|
||||
raw = bytes(raw) if isinstance(raw, (bytes, bytearray, memoryview)) else raw
|
||||
state = decode_safety(raw)
|
||||
if state is None:
|
||||
_clear_native_admission(sm)
|
||||
return None
|
||||
if (not _native_admission_matches(sm, CP, state, raw) or
|
||||
not state.compatible or int(state.protocolVersion) != 1 or not state.pandaSerial or not state.axisSessionId or
|
||||
(axis_session_id is not None and str(state.axisSessionId) != axis_session_id) or
|
||||
int(sm.logMonoTime[SAFETY_SERVICE]) > now_ns or
|
||||
now_ns - int(sm.logMonoTime[SAFETY_SERVICE]) > SAFETY_MAX_AGE_NS or
|
||||
int(state.observedMonoTime) > now_ns or now_ns > int(state.validUntilMonoTime)):
|
||||
_clear_native_admission(sm)
|
||||
return None
|
||||
if state.pandaInventory and (int(sm.logMonoTime[SAFETY_SERVICE]) != state.observedMonoTime or
|
||||
state.observedMonoTime <= 0 or
|
||||
state.validUntilMonoTime - state.observedMonoTime > SAFETY_MAX_AGE_NS):
|
||||
_clear_native_admission(sm)
|
||||
return None
|
||||
return state
|
||||
|
||||
|
||||
def decide_axes(*, standard_lateral: bool, standard_longitudinal: bool, intent,
|
||||
native, car_state, initialized: bool, model_ready: bool,
|
||||
no_entry: bool, immediate_disable: bool, dm_lockout: bool,
|
||||
pause_brake_mps: float, lateral_inhibit: bool = False) -> AxisDecision:
|
||||
if intent is None or not initialized or not car_state.canValid or immediate_disable or no_entry or dm_lockout:
|
||||
return AxisDecision()
|
||||
gear = car_state.gearShifter
|
||||
driving = gear not in (car.CarState.GearShifter.neutral, car.CarState.GearShifter.park,
|
||||
car.CarState.GearShifter.reverse, car.CarState.GearShifter.unknown)
|
||||
lateral_safe = (driving and model_ready and not lateral_inhibit and not car_state.steerFaultPermanent and
|
||||
not car_state.steerFaultTemporary and
|
||||
(pause_brake_mps <= 0 or not car_state.brakePressed or car_state.vEgo >= pause_brake_mps or car_state.standstill))
|
||||
desired_lateral = bool(lateral_safe and not intent.pauseLateral and (standard_lateral or intent.allowedLatch))
|
||||
desired_longitudinal = bool(driving and standard_longitudinal and not car_state.seatbeltUnlatched and not intent.pauseLongitudinal)
|
||||
lateral_ack = native is not None and bool(native.requestedLateral) == desired_lateral
|
||||
longitudinal_ack = native is not None and bool(native.requestedLongitudinal) == desired_longitudinal
|
||||
return AxisDecision(
|
||||
desired_lateral=desired_lateral, desired_longitudinal=desired_longitudinal,
|
||||
lateral_active=bool(desired_lateral and lateral_ack and native.lateralAllowed),
|
||||
longitudinal_active=bool(desired_longitudinal and longitudinal_ack and native.longitudinalAllowed),
|
||||
native_acknowledged=bool(native is not None),
|
||||
)
|
||||
|
||||
|
||||
def current_axis(sm, *, now_ns: int):
|
||||
if not (sm.valid['aolAxisState'] and sm.alive['aolAxisState'] and sm.seen['aolAxisState']):
|
||||
return None
|
||||
state = sm['aolAxisState']
|
||||
if (not state.qualified or not state.sessionId or
|
||||
int(sm.logMonoTime['aolAxisState']) > now_ns or
|
||||
now_ns - int(sm.logMonoTime['aolAxisState']) > AXIS_MAX_AGE_NS or
|
||||
int(state.observedMonoTime) > now_ns or now_ns > int(state.validUntilMonoTime)):
|
||||
return None
|
||||
return state
|
||||
|
||||
|
||||
def monitoring_lateral_engaged(sm, *, now_ns: int) -> bool:
|
||||
axis = current_axis(sm, now_ns=now_ns)
|
||||
if axis is None or not axis.lateralActive or not axis.nativeAcknowledged:
|
||||
return False
|
||||
if not (sm.valid[SAFETY_SERVICE] and sm.seen[SAFETY_SERVICE] and sm.alive[SAFETY_SERVICE]):
|
||||
return False
|
||||
native = decode_safety(sm[SAFETY_SERVICE])
|
||||
if native is None:
|
||||
return False
|
||||
return bool(native.compatible and int(native.protocolVersion) == 1 and
|
||||
native.axisSessionId == axis.sessionId and
|
||||
native_profile_supported(int(native.safetyModel), int(native.safetyParam)) and
|
||||
int(sm.logMonoTime[SAFETY_SERVICE]) <= now_ns and
|
||||
now_ns - int(sm.logMonoTime[SAFETY_SERVICE]) <= SAFETY_MAX_AGE_NS and
|
||||
int(native.observedMonoTime) <= now_ns <= int(native.validUntilMonoTime) and
|
||||
native.requestedLateral and native.lateralAllowed)
|
||||
|
||||
|
||||
def ordinary_lateral_requested(active: bool, CS, CP) -> bool:
|
||||
"""The standard lateral output gate, shared by request and actuation owners."""
|
||||
standstill = abs(CS.vEgo) <= max(CP.minSteerSpeed, 0.3) or CS.standstill
|
||||
return bool(active and not CS.steerFaultTemporary and not CS.steerFaultPermanent and
|
||||
(not standstill or CP.steerAtStandstill))
|
||||
|
||||
|
||||
def decide_ordinary_axis(*, requested: bool, native) -> AxisDecision:
|
||||
"""Acknowledgment cannot create driver intent or an independent axis."""
|
||||
acknowledged = bool(native is not None and native.requestedLateral == requested and
|
||||
not native.requestedLongitudinal)
|
||||
return AxisDecision(desired_lateral=bool(requested),
|
||||
lateral_active=bool(requested and acknowledged and native.lateralAllowed),
|
||||
native_acknowledged=acknowledged)
|
||||
|
||||
|
||||
def ordinary_axis_acknowledged(sm, CP, *, now_ns: int) -> bool:
|
||||
axis = current_axis(sm, now_ns=now_ns)
|
||||
native = current_native(sm, CP, now_ns=now_ns,
|
||||
axis_session_id=str(axis.sessionId) if axis is not None else None)
|
||||
return bool(axis is not None and native is not None and axis.nativeAcknowledged and
|
||||
axis.desiredLateral and axis.lateralActive and not axis.desiredLongitudinal and
|
||||
native.requestedLateral and not native.requestedLongitudinal and native.lateralAllowed)
|
||||
@@ -0,0 +1,30 @@
|
||||
def seatbelt_lateral_soft_event_allowed(cp, cs, intent, native, event, *, source_ns, now_ns, axis_session_id, pending_lateral_request=False) -> bool:
|
||||
"""A fresh native-OFF baseline may arm LAT while the belt remains a LONG lock."""
|
||||
from opendbc.car.structs import car
|
||||
from openpilot.starpilot.aol.vehicle import policy_for
|
||||
from openpilot.cereal import log
|
||||
from openpilot.starpilot.aol.runtime import AXIS_MAX_AGE_NS, SAFETY_MAX_AGE_NS, native_inventory_matches_cp
|
||||
try:
|
||||
pending_native = bool(pending_lateral_request and native is not None and
|
||||
not native.requestedLateral and not native.lateralAllowed)
|
||||
return bool(policy_for(cp).intent_supported and event.name == log.OnroadEvent.EventName.seatbeltNotLatched and event.softDisable and
|
||||
event.noEntry and not event.immediateDisable and not event.userDisable and
|
||||
cs.seatbeltUnlatched and cs.canValid and not cs.canTimeout and not cs.accFaulted and not cs.doorOpen and
|
||||
not cs.steerFaultTemporary and not cs.steerFaultPermanent and cs.cruiseState.available and
|
||||
cs.gearShifter in (car.CarState.GearShifter.drive, car.CarState.GearShifter.low,
|
||||
car.CarState.GearShifter.sport, car.CarState.GearShifter.brake) and
|
||||
intent is not None and intent.settingsQualified and
|
||||
(intent.allowedLatch and intent.lateralArmed or
|
||||
pending_native and not intent.allowedLatch and not intent.lateralArmed) and
|
||||
intent.producerSessionId and intent.carStateLogMonoTime == source_ns and
|
||||
0 < source_ns <= intent.observedMonoTime <= now_ns <= intent.validUntilMonoTime and
|
||||
now_ns - source_ns <= AXIS_MAX_AGE_NS and
|
||||
intent.validUntilMonoTime - intent.observedMonoTime <= SAFETY_MAX_AGE_NS and
|
||||
native is not None and native.compatible and int(native.protocolVersion) == 1 and
|
||||
axis_session_id and native.axisSessionId == axis_session_id and
|
||||
(native.requestedLateral and native.lateralAllowed or pending_native) and native.pandaInventory and
|
||||
native_inventory_matches_cp(native, cp) and
|
||||
0 < native.observedMonoTime <= now_ns <= native.validUntilMonoTime and
|
||||
native.validUntilMonoTime - native.observedMonoTime <= SAFETY_MAX_AGE_NS)
|
||||
except (AttributeError, KeyError, IndexError, TypeError, ValueError):
|
||||
return False
|
||||
@@ -0,0 +1,275 @@
|
||||
"""Classify ordinary host source loss without granting commands or engagement."""
|
||||
from opendbc.car.structs import car
|
||||
from openpilot.cereal import log
|
||||
from openpilot.selfdrive.selfdrived.events import ET
|
||||
from openpilot.starpilot.aol.intent_companion import MAX_COMPANIONS
|
||||
from openpilot.starpilot.aol.runtime import INTENT_MAX_AGE_NS, SAFETY_MAX_AGE_NS, current_native
|
||||
from openpilot.starpilot.aol.transport_pause import (cp_identity, healthy_transport, HOST_SOURCE_AVAILABILITY_EVENTS,
|
||||
host_source_availability_only, stock_lateral_off, stock_lateral_user_event)
|
||||
from openpilot.starpilot.nostalgia import physical_cancel
|
||||
|
||||
EventName = log.OnroadEvent.EventName
|
||||
AlertLevel = log.DriverMonitoringState.AlertLevel
|
||||
|
||||
|
||||
def stock_lateral_role(self, CS, native, intent=None):
|
||||
"""Current stock USER role while an established independent LAT owner remains."""
|
||||
rules = self.events.event_rules
|
||||
stock_user = stock_lateral_user_event(self.CP)
|
||||
users = [name for name in self.events.names if ET.USER_DISABLE in rules.get(name, {})]
|
||||
if stock_user is None or not users or any(name != stock_user or
|
||||
ET.SOFT_DISABLE in rules.get(name, {}) or ET.IMMEDIATE_DISABLE in rules.get(name, {}) for name in users):
|
||||
return False
|
||||
anchor = getattr(self, 'aol_ordinary_source_anchor', None)
|
||||
if (anchor is None or not anchor[0][8] or anchor[0][7] or anchor[0][9] or self.enabled or self.active or
|
||||
self.state_machine.state != log.SelfdriveState.OpenpilotState.disabled or
|
||||
anchor[0][0] != self.aol_session_id or cp_identity(self.CP) != anchor[3] or
|
||||
getattr(self, 'aol_dm_lateral_inhibit', False) or
|
||||
not self.sm.all_checks(['modelV2', 'extrinsicsCalibration', 'driverMonitoringState']) or
|
||||
self.sm['extrinsicsCalibration'].calStatus != log.ExtrinsicsCalibration.Status.calibrated or
|
||||
self.sm['driverMonitoringState'].alertLevel == AlertLevel.three or
|
||||
self.sm['driverMonitoringState'].lockout or self.sm['driverMonitoringState'].alwaysOnLockout or
|
||||
not stock_lateral_off(self.CP, CS, native)):
|
||||
return False
|
||||
accepted = intent if intent is not None else anchor[2]
|
||||
companion = getattr(self, 'aol_intent_companion', None)
|
||||
return bool(accepted is not None and accepted.settingsQualified and accepted.allowedLatch and accepted.lateralArmed and
|
||||
not accepted.pauseLateral and not accepted.pauseLongitudinal and
|
||||
accepted.producerSessionId == anchor[0][4] and companion is not None and companion.last_drain_ok and
|
||||
companion.integrity_epoch == anchor[0][5])
|
||||
|
||||
|
||||
def stock_lateral_fault_proof(self, CS, message, now_ns):
|
||||
"""Capture the stock role of this successfully published inactive fault."""
|
||||
published = self.aol_published_fault_proof
|
||||
intent = getattr(self, 'aol_last_intent', None)
|
||||
native = getattr(self, 'aol_decision_native', None)
|
||||
anchor = getattr(self, 'aol_ordinary_source_anchor', None)
|
||||
axis = message.aolAxisState
|
||||
if (published is None or anchor is None or anchor[1] is None or not message.valid or not axis.qualified or published[1] != 'critical' or
|
||||
any(published[4:]) or not stock_lateral_role(self, CS, native, intent) or
|
||||
self.aol_authority_lost or getattr(self, 'aol_availability_long_inhibit', False) or
|
||||
getattr(self, 'car_state_queue_invalid', False) or getattr(self, 'aol_selected_pair_future', False) or
|
||||
not getattr(self, 'aol_step_intent_drain_ok', True) or
|
||||
getattr(self, 'aol_step_intent_drain_count', 0) > MAX_COMPANIONS or
|
||||
not getattr(self, 'conditional_car_state_valid', False) or intent != anchor[2] or CS is not anchor[4] or
|
||||
axis.sourceCarStateMonoTime != intent.carStateLogMonoTime or
|
||||
not 0 < axis.sourceCarStateMonoTime <= axis.observedMonoTime <= now_ns <= axis.validUntilMonoTime or
|
||||
now_ns - axis.sourceCarStateMonoTime > INTENT_MAX_AGE_NS or
|
||||
native is not getattr(self.aol_transport_pause, 'observed_native', None) or
|
||||
not 0 < native.observedMonoTime <= now_ns <= native.validUntilMonoTime or
|
||||
native.observedMonoTime < anchor[1].observedMonoTime or native.pandaSerial != anchor[1].pandaSerial or
|
||||
tuple(p.hardwareSerial for p in native.pandaInventory) != tuple(p.hardwareSerial for p in anchor[1].pandaInventory) or
|
||||
any(self.events.contains(kind) for kind in (ET.ENABLE, ET.OVERRIDE_LATERAL, ET.OVERRIDE_LONGITUDINAL))):
|
||||
return None
|
||||
events = self.events.to_msg()
|
||||
stock_user = stock_lateral_user_event(self.CP)
|
||||
host = [e for e in events if not (e.name == stock_user and e.userDisable and
|
||||
not e.softDisable and not e.immediateDisable)]
|
||||
if not host_source_availability_only(host) or not any(e.name == EventName.selfdrivedLagging for e in host):
|
||||
return None
|
||||
samples = getattr(self, 'car_state_samples', ()) or ((CS, True),)
|
||||
if any(not valid or not sample.canValid or sample.canTimeout or sample.steerFaultPermanent or
|
||||
sample.steerFaultTemporary or sample.accFaulted or sample.doorOpen or sample.seatbeltUnlatched or
|
||||
sample.brakePressed or sample.regenBraking or sample.gasPressed or physical_cancel(sample) or
|
||||
sample.gearShifter not in (car.CarState.GearShifter.drive, car.CarState.GearShifter.sport,
|
||||
car.CarState.GearShifter.low, car.CarState.GearShifter.brake) for sample, valid in samples):
|
||||
return None
|
||||
return (published, stock_user)
|
||||
|
||||
|
||||
def ordinary_source_availability(self, CS, now_ns, native=None, *, replay: bool, native_provider=current_native):
|
||||
"""Classify lost host source; never renew a command or change engagement."""
|
||||
anchor = getattr(self, 'aol_ordinary_source_anchor', None)
|
||||
companion = getattr(self, 'aol_intent_companion', None)
|
||||
events = self.events.to_msg()
|
||||
stock_user = None
|
||||
if any(event.name in (EventName.pcmDisable, EventName.cruiseDisabled) and event.userDisable for event in events):
|
||||
# Resolve the native value once, as this classifier already does below.
|
||||
# Normal healthy frames never enter this nominal stock-role branch.
|
||||
native = native or native_provider(self.sm, self.CP, now_ns=now_ns, axis_session_id=self.aol_session_id)
|
||||
if stock_lateral_role(self, CS, native):
|
||||
stock_user = stock_lateral_user_event(self.CP)
|
||||
events = [event for event in events if not (event.name == stock_user and event.userDisable and
|
||||
not event.softDisable and not event.immediateDisable)]
|
||||
if (replay or not self.aol_replay or not self.axis_transport_required or anchor is None or
|
||||
not self.initialized or getattr(self, 'aol_availability_long_inhibit', False) or self.aol_authority_lost or
|
||||
getattr(self, 'aol_fault_reason', 'none') not in ('none', 'availabilityPause', 'critical') or
|
||||
any(event.userDisable or event.immediateDisable for event in events) or
|
||||
any(e.softDisable and e.name not in HOST_SOURCE_AVAILABILITY_EVENTS
|
||||
for e in events) or self.events.contains(ET.ENABLE) or
|
||||
getattr(self, 'car_state_queue_invalid', False) or
|
||||
not getattr(self, 'aol_step_intent_drain_ok', True) or
|
||||
getattr(self, 'aol_step_intent_drain_count', 0) > MAX_COMPANIONS or companion is None or not companion.last_drain_ok or
|
||||
getattr(self, 'aol_selected_pair_future', False) or
|
||||
not self.sm.all_checks(['modelV2', 'extrinsicsCalibration', 'driverMonitoringState']) or
|
||||
self.sm['extrinsicsCalibration'].calStatus != log.ExtrinsicsCalibration.Status.calibrated or
|
||||
self.sm['driverMonitoringState'].alertLevel == AlertLevel.three or
|
||||
self.sm['driverMonitoringState'].lockout or self.sm['driverMonitoringState'].alwaysOnLockout):
|
||||
return False
|
||||
proof, prior_native, accepted, identity, accepted_state = anchor
|
||||
if accepted is None:
|
||||
return False
|
||||
# A source admitted by normal runtime in THIS step can expire only while
|
||||
# publishing its first host-SOFT fault. It has no prior critical publication.
|
||||
# Require the exact admitted object/state and original completed baseline;
|
||||
# stale historical inputs cannot enter this path or grant a new command.
|
||||
admitted_expiry = bool(getattr(self, 'aol_published_fault_reason', '') == 'none' and
|
||||
getattr(self, 'aol_step_input_proof', None) == proof and
|
||||
getattr(self, 'aol_last_intent', None) is accepted and
|
||||
getattr(self, 'conditional_car_state_valid', False) and CS is accepted_state and
|
||||
self.aol_car_state_log_ns == accepted.carStateLogMonoTime and
|
||||
host_source_availability_only(events))
|
||||
if getattr(self, 'aol_fault_reason', 'none') == 'critical' and not admitted_expiry:
|
||||
# A prior OFF/critical proof may describe ordinary SOFT loop lag rather
|
||||
# than an authority fault. Prove that exact published episode; never infer
|
||||
# it from a current healthy native snapshot or clear authority_lost here.
|
||||
published = getattr(self, 'aol_published_fault_proof', None)
|
||||
if (published is None or len(published) != 9 or published[0] != self.aol_session_id or
|
||||
published[1] != 'critical' or published[2] != getattr(self, 'aol_fault_event_ns', 0) or
|
||||
published[3] != tuple(getattr(self, 'events_prev', ()))):
|
||||
return False
|
||||
prior_names = published[3]
|
||||
stock_record = getattr(self, 'aol_published_stock_lateral_proof', None)
|
||||
if (stock_user is not None and stock_record is not None and len(stock_record) == 2 and
|
||||
stock_record[0] == published and stock_record[1] == stock_user):
|
||||
# This USER was qualified at the actual completed prior publication,
|
||||
# not inferred from today's healthy physical/native snapshot.
|
||||
prior_names = tuple(name for name in prior_names if name != stock_user)
|
||||
rules = self.events.event_rules
|
||||
prior_faults = [name for name in prior_names if any(kind in rules.get(name, {})
|
||||
for kind in (ET.SOFT_DISABLE, ET.IMMEDIATE_DISABLE, ET.USER_DISABLE))]
|
||||
if (getattr(self, 'aol_published_fault_reason', '') != 'critical' or
|
||||
not getattr(self, 'aol_fault_event_ns', 0) or
|
||||
EventName.selfdrivedLagging not in prior_faults or
|
||||
not all(name in HOST_SOURCE_AVAILABILITY_EVENTS and
|
||||
not any(kind in rules.get(name, {}) for kind in (ET.IMMEDIATE_DISABLE, ET.USER_DISABLE))
|
||||
for name in prior_faults) or not host_source_availability_only(events) or
|
||||
any(published[4:])):
|
||||
return False
|
||||
source = self.aol_car_state_log_ns or getattr(self, 'car_state_retained_source_ns', 0)
|
||||
held_metadata = bool(not getattr(self, 'car_state_samples', ()) and CS is accepted_state and
|
||||
getattr(self, 'aol_publication_retained_intent', None) is accepted)
|
||||
if not source and held_metadata:
|
||||
source = int(accepted.carStateLogMonoTime)
|
||||
if (proof[0] != self.aol_session_id or not (proof[8] or proof[9]) or accepted is None or source != int(accepted.carStateLogMonoTime) or
|
||||
not 0 < source <= now_ns or now_ns - source <= INTENT_MAX_AGE_NS or
|
||||
cp_identity(self.CP) != identity or companion.integrity_epoch != proof[5] or
|
||||
accepted is None or accepted.producerSessionId != proof[4] or
|
||||
prior_native is None or not prior_native.pandaInventory):
|
||||
return False
|
||||
samples = getattr(self, 'car_state_samples', ()) or ((CS,
|
||||
bool(getattr(self, 'car_state_retained_valid', False) or held_metadata)),)
|
||||
if any(not valid or not sample.canValid or sample.canTimeout or sample.steerFaultPermanent or
|
||||
sample.steerFaultTemporary or sample.accFaulted or sample.doorOpen or sample.seatbeltUnlatched or
|
||||
sample.brakePressed or sample.regenBraking or sample.gasPressed or physical_cancel(sample) or
|
||||
sample.gearShifter not in (car.CarState.GearShifter.drive, car.CarState.GearShifter.sport,
|
||||
car.CarState.GearShifter.low, car.CarState.GearShifter.brake)
|
||||
for sample, valid in samples):
|
||||
return False
|
||||
def valid_metadata(stamp, value):
|
||||
if (value is None or value.carStateLogMonoTime != stamp or
|
||||
value.producerSessionId != proof[4] or not value.settingsQualified or
|
||||
value.allowedLatch != accepted.allowedLatch or value.lateralArmed != accepted.lateralArmed or
|
||||
value.pauseLongitudinal or value.pauseLateral and (proof[8] or not accepted.pauseLateral)):
|
||||
return False
|
||||
# Only the exact originally accepted tuple is identity after expiry.
|
||||
# Every newer raw/SM tuple keeps its own original shape and 200ms lease;
|
||||
# none supplies an active command or a renewed source/output deadline.
|
||||
return bool(stamp == accepted.carStateLogMonoTime and value == accepted or
|
||||
0 < stamp <= value.observedMonoTime <= now_ns <= value.validUntilMonoTime and
|
||||
0 < value.validUntilMonoTime - value.observedMonoTime <= SAFETY_MAX_AGE_NS)
|
||||
|
||||
# The raw-only cache retains every distinct tuple in a <=16-message step.
|
||||
# Its exact validated receipts remain metadata when current() legitimately
|
||||
# prunes the separate 30ms source-pair cache. Check every original restriction
|
||||
# directly, including one no longer present in the conflated SM/sample cache.
|
||||
accounted = 0
|
||||
for stamp, (value, epoch, producer) in companion.raw_projections.items():
|
||||
if stamp < accepted.carStateLogMonoTime:
|
||||
continue
|
||||
if epoch != proof[5] or producer != proof[4] or not valid_metadata(stamp, value):
|
||||
return False
|
||||
accounted += 1
|
||||
if accounted < getattr(self, 'aol_step_intent_drain_count', 0):
|
||||
return False
|
||||
# A newer conflated SM sample may add a restriction but cannot remove any
|
||||
# already checked raw restriction. Invalid/reordered bursts also retain the
|
||||
# existing integrity-epoch and cumulative-drain vetoes above.
|
||||
for stamp, valid, value in companion.samples.values():
|
||||
if stamp > accepted.carStateLogMonoTime and (not valid or not valid_metadata(stamp, value)):
|
||||
return False
|
||||
latest = companion.latest
|
||||
raw = latest[2] if latest is not None and latest[1] else None
|
||||
if (latest is None or not accepted.carStateLogMonoTime <= latest[0] <= now_ns or
|
||||
not valid_metadata(latest[0], raw)):
|
||||
return False
|
||||
native = native or native_provider(self.sm, self.CP, now_ns=now_ns, axis_session_id=self.aol_session_id)
|
||||
lateral_intent_only = bool(proof[8] and not proof[9] and not self.enabled)
|
||||
if (native is None or lateral_intent_only and not (accepted.allowedLatch and accepted.lateralArmed and
|
||||
not accepted.pauseLateral and not getattr(self, 'aol_dm_lateral_inhibit', False) and
|
||||
native.requestedLateral and native.lateralAllowed) or
|
||||
not healthy_transport(self.sm, self.CP, CS, self.aol_session_id, now_ns, native) or
|
||||
native.observedMonoTime < prior_native.observedMonoTime or
|
||||
native.pandaSerial != prior_native.pandaSerial or
|
||||
tuple(p.hardwareSerial for p in native.pandaInventory) != tuple(p.hardwareSerial for p in prior_native.pandaInventory)):
|
||||
return False
|
||||
self.aol_ordinary_source_native = native
|
||||
return True
|
||||
|
||||
|
||||
def fresh_ordinary_lag_continuation(self, CS, intent, native, now_ns):
|
||||
"""Continue established standard axes; native/inputs still own every grant."""
|
||||
if (EventName.selfdrivedLagging not in self.events.names or not self.enabled or not self.active or
|
||||
self.state_machine.state != log.SelfdriveState.OpenpilotState.softDisabling or
|
||||
not self.initialized or not self.aol_replay or not self.axis_transport_required or
|
||||
self.aol_authority_lost or getattr(self, 'aol_availability_long_inhibit', False)):
|
||||
return False
|
||||
rules = self.events.event_rules
|
||||
faults = [name for name in self.events.names if any(kind in rules.get(name, {})
|
||||
for kind in (ET.USER_DISABLE, ET.SOFT_DISABLE, ET.IMMEDIATE_DISABLE))]
|
||||
lag_rules = rules.get(EventName.selfdrivedLagging, {})
|
||||
if (faults != [EventName.selfdrivedLagging] or ET.SOFT_DISABLE not in lag_rules or
|
||||
ET.IMMEDIATE_DISABLE in lag_rules or ET.USER_DISABLE in lag_rules or self.events.contains(ET.ENABLE) or
|
||||
self.events.contains(ET.OVERRIDE_LATERAL) or self.events.contains(ET.OVERRIDE_LONGITUDINAL) or
|
||||
any(name != EventName.selfdrivedLagging and ET.NO_ENTRY in self.events.event_rules.get(name, {})
|
||||
for name in self.events.names)):
|
||||
return False
|
||||
anchor = getattr(self, 'aol_ordinary_source_anchor', None)
|
||||
companion = getattr(self, 'aol_intent_companion', None)
|
||||
if (anchor is None or intent is None or companion is None or not (anchor[0][8] or anchor[0][9]) or
|
||||
anchor[0][0] != self.aol_session_id or companion.integrity_epoch != anchor[0][5] or
|
||||
intent != anchor[2] or CS is not anchor[4] or intent.producerSessionId != anchor[0][4] or
|
||||
cp_identity(self.CP) != anchor[3] or not getattr(self, 'conditional_car_state_valid', False) or
|
||||
not 0 < self.aol_car_state_log_ns == intent.carStateLogMonoTime <= now_ns or
|
||||
now_ns - self.aol_car_state_log_ns > INTENT_MAX_AGE_NS or
|
||||
getattr(self, 'car_state_queue_invalid', False) or getattr(self, 'aol_selected_pair_future', False) or
|
||||
not getattr(self, 'aol_step_intent_drain_ok', True) or not companion.last_drain_ok or
|
||||
getattr(self, 'aol_step_intent_drain_count', 0) > MAX_COMPANIONS or
|
||||
not self.sm.all_checks(['modelV2', 'extrinsicsCalibration', 'driverMonitoringState']) or
|
||||
self.sm['extrinsicsCalibration'].calStatus != log.ExtrinsicsCalibration.Status.calibrated or
|
||||
self.sm['driverMonitoringState'].alertLevel == AlertLevel.three or
|
||||
self.sm['driverMonitoringState'].lockout or self.sm['driverMonitoringState'].alwaysOnLockout or
|
||||
native is None or native is not getattr(self.aol_transport_pause, 'observed_native', None) or
|
||||
native.observedMonoTime < anchor[1].observedMonoTime or native.pandaSerial != anchor[1].pandaSerial or
|
||||
tuple(p.hardwareSerial for p in native.pandaInventory) != tuple(p.hardwareSerial for p in anchor[1].pandaInventory)):
|
||||
return False
|
||||
if intent.pauseLongitudinal or intent.pauseLateral and anchor[0][8]:
|
||||
return False
|
||||
for stamp, (value, epoch, producer) in companion.raw_projections.items():
|
||||
if stamp < anchor[0][1]:
|
||||
continue
|
||||
if (epoch != anchor[0][5] or producer != anchor[0][4] or value is None or
|
||||
value.producerSessionId != anchor[0][4] or not value.settingsQualified or
|
||||
value.allowedLatch != intent.allowedLatch or value.lateralArmed != intent.lateralArmed or
|
||||
value.pauseLongitudinal or value.pauseLateral and (anchor[0][8] or not intent.pauseLateral) or
|
||||
value.carStateLogMonoTime != stamp or not 0 < stamp <= value.observedMonoTime <= now_ns <= value.validUntilMonoTime or
|
||||
not 0 < value.validUntilMonoTime - value.observedMonoTime <= SAFETY_MAX_AGE_NS):
|
||||
return False
|
||||
samples = getattr(self, 'car_state_samples', ()) or ((CS, True),)
|
||||
return not any(not valid or not sample.canValid or sample.canTimeout or sample.steerFaultPermanent or
|
||||
sample.steerFaultTemporary or sample.accFaulted or sample.doorOpen or sample.seatbeltUnlatched or
|
||||
sample.brakePressed or sample.regenBraking or sample.gasPressed or physical_cancel(sample) or
|
||||
sample.gearShifter not in (car.CarState.GearShifter.drive, car.CarState.GearShifter.sport,
|
||||
car.CarState.GearShifter.low, car.CarState.GearShifter.brake)
|
||||
for sample, valid in samples)
|
||||
@@ -0,0 +1,262 @@
|
||||
"""Synthetic parsed CAN and native axis/controller projection, not vehicle replay."""
|
||||
from dataclasses import replace
|
||||
from types import SimpleNamespace
|
||||
import unittest
|
||||
from unittest.mock import patch
|
||||
|
||||
from opendbc.can import CANPacker, CANParser
|
||||
from opendbc.car import Bus, structs
|
||||
from opendbc.car.hyundai.interface import CarInterface
|
||||
from opendbc.car.hyundai.values import CAR, DBC, HyundaiFlags
|
||||
from opendbc.car.vehicle_model import VehicleModel
|
||||
from opendbc.safety.tests.common import make_msg
|
||||
from opendbc.safety.tests.libsafety import libsafety_py
|
||||
from opendbc.safety.tests.test_hyundai import checksum
|
||||
from openpilot.starpilot.aol.tests.test_forte_intent import params, settings
|
||||
from openpilot.starpilot.car.hyundai.aol import create_intent, policy_for
|
||||
from openpilot.starpilot.aol.runtime import current_native, decide_axes
|
||||
from openpilot.starpilot.aol.wire import SAFETY_SERVICE, SafetyState, encode_safety
|
||||
from openpilot.cereal import log
|
||||
from openpilot.selfdrive.controls.lib.latcontrol_torque import LatControlTorque
|
||||
from openpilot.starpilot.lateral.controller_selection import ControllerMode
|
||||
|
||||
|
||||
class Samples(dict):
|
||||
def __init__(self, native, stamp):
|
||||
super().__init__({SAFETY_SERVICE: encode_safety(native)})
|
||||
self.valid = self.alive = self.seen = {SAFETY_SERVICE: True}
|
||||
self.logMonoTime = {SAFETY_SERVICE: stamp}
|
||||
|
||||
|
||||
class ForteStream:
|
||||
def __init__(self, *, main_action=0, source=0x391, aol=True, car=CAR.KIA_FORTE_2019_NON_SCC, controller_mode=None):
|
||||
self.source = source
|
||||
self.cp = params(car, source=source)
|
||||
policy = policy_for(self.cp)
|
||||
if aol:
|
||||
self.cp.safetyConfigs[0].safetyParam |= policy.safety_param_addition
|
||||
self.cp.alternativeExperience |= policy.alternative_experience_addition
|
||||
self.ci = CarInterface(self.cp)
|
||||
self.ci.update([])
|
||||
self.packer = CANPacker(DBC[self.cp.carFingerprint][Bus.pt])
|
||||
self.output_parser = CANParser(DBC[self.cp.carFingerprint][Bus.pt], [('LKAS11', 100)], 0)
|
||||
self.lateral_controller = (LatControlTorque(self.cp.as_reader(), self.ci, .01, controller_mode=controller_mode)
|
||||
if controller_mode is not None else None)
|
||||
self.vehicle_model = VehicleModel(self.cp)
|
||||
self.vehicle_parameters = log.VehicleParameters.new_message(angleOffsetDeg=0., roll=0.)
|
||||
self.desired_curvature = 0.
|
||||
self.last_torque = 0.
|
||||
self.last_normalized_torque = 0.
|
||||
self.intent = create_intent(self.cp, settings(main=main_action))
|
||||
self.intent.settings = replace(self.intent.settings, enabled=aol)
|
||||
self.safety = libsafety_py.libsafety # Harness preloads the qualified candidate.
|
||||
self.now = 1_000_000_000
|
||||
self.count = 0
|
||||
self.reset()
|
||||
|
||||
def reset(self):
|
||||
self.safety.init_tests()
|
||||
self.safety.set_alternative_experience(self.cp.alternativeExperience)
|
||||
assert self.safety.set_safety_hooks(int(self.cp.safetyConfigs[0].safetyModel.raw),
|
||||
self.cp.safetyConfigs[0].safetyParam) == 0
|
||||
self.safety.set_timer(self.now // 1000)
|
||||
self.safety.set_aol_test_heartbeat(True) # Synthetic transport envelope only.
|
||||
|
||||
def tick(self, *, main=False, cruise=False, bcm=False, clu=False, gear='D', eps=False,
|
||||
fatal=False, rejection=0, cruise_button=0):
|
||||
self.now += 10_000_000
|
||||
n = self.count
|
||||
self.count += 1
|
||||
gears = self.ci.CS.shifter_values
|
||||
gear_code = next(code for code, name in gears.items() if name == gear)
|
||||
values: dict[str, dict[str, float]] = {
|
||||
'EMS16': {'CRUISE_LAMP_M': int(main), 'CRUISE_LAMP_S': int(cruise), 'AliveCounter': n % 4},
|
||||
'WHL_SPD11': {'WHL_SPD_FL': 72, 'WHL_SPD_FR': 72, 'WHL_SPD_RL': 72, 'WHL_SPD_RR': 72,
|
||||
'WHL_SPD_AliveCounter_LSB': n % 4, 'WHL_SPD_AliveCounter_MSB': (n // 4) % 4},
|
||||
'TCS13': {'AliveCounterTCS': n % 8},
|
||||
'MDPS12': {'CF_Mdps_ToiUnavail': int(eps), 'CR_Mdps_StrColTq': 0., 'CR_Mdps_OutTq': 0.},
|
||||
'SAS11': {'SAS_Angle': 0., 'SAS_Speed': 0.},
|
||||
'CLU11': {'CF_Clu_AliveCnt1': n % 16, 'CF_Clu_CruiseSwState': cruise_button}, 'LVR12': {'CF_Lvr_Gear': gear_code},
|
||||
'BCM_PO_11': {'LDA_BTN': int(bcm)}, 'CLU13': {'CF_Clu_LdwsLkasSW': int(clu)},
|
||||
'CGW1': {'CF_Gway_DrvSeatBeltSw': 1},
|
||||
}
|
||||
if self.source == 0x50c:
|
||||
del values['BCM_PO_11']
|
||||
frames = []
|
||||
for name in ('EMS12', 'TCS11', 'TCS15', 'CLU15', 'ESP12', 'CGW2', *values):
|
||||
frame = self.packer.make_can_msg(name, 0, values.get(name, {}))
|
||||
if name in ('EMS16', 'WHL_SPD11', 'TCS13'):
|
||||
frame = checksum(frame)
|
||||
frames.append(frame)
|
||||
frames.append(self.packer.make_can_msg('LKAS11', 2, {'CF_Lkas_MsgCount': n % 16}))
|
||||
if not self.cp.flags & HyundaiFlags.NON_SCC_NO_FCA:
|
||||
frames.append(self.packer.make_can_msg('FCA11', 2, {}))
|
||||
self.safety.set_timer(self.now // 1000)
|
||||
for address, data, bus in frames:
|
||||
self.safety.safety_rx_hook(make_msg(bus, address, len(data), data))
|
||||
self.safety.safety_tick()
|
||||
with patch('opendbc.car.hyundai.non_scc_aol.time.CLOCK_BOOTTIME', 7, create=True), \
|
||||
patch('opendbc.car.hyundai.non_scc_aol.time.clock_gettime_ns', return_value=self.now):
|
||||
self.cs = self.ci.update([(self.now, frames)])
|
||||
self.intent.update(self.cs, now_ns=self.now, fault_active=fatal, native_rejection_ns=rejection)
|
||||
return self.axis()
|
||||
|
||||
def axis(self, receipt=None):
|
||||
intent = SimpleNamespace(allowedLatch=self.intent.allowed_latch, pauseLateral=self.intent.pause_lateral,
|
||||
pauseLongitudinal=self.intent.pause_longitudinal)
|
||||
kwargs: dict = {'standard_lateral': False, 'standard_longitudinal': False, 'intent': intent,
|
||||
'car_state': self.cs, 'initialized': True, 'model_ready': True, 'no_entry': False,
|
||||
'immediate_disable': False, 'dm_lockout': False, 'pause_brake_mps': 0.}
|
||||
desired = decide_axes(native=None, **kwargs)
|
||||
self.safety.aol_set_host_request(int(desired.desired_lateral))
|
||||
permission = self.safety.aol_get_permission_mask()
|
||||
native = SafetyState(1, True, self.now, self.now + 30_000_000,
|
||||
int(self.cp.safetyConfigs[0].safetyModel.raw), self.cp.safetyConfigs[0].safetyParam,
|
||||
bool(permission & 1), False, bool(self.safety.aol_get_request_mask() & 1), False, 'synthetic-panda', 'session')
|
||||
if receipt is not None:
|
||||
native = receipt(native)
|
||||
acknowledged = current_native(Samples(native, self.now), self.cp, now_ns=self.now, axis_session_id='session')
|
||||
decision = decide_axes(native=acknowledged, **kwargs)
|
||||
command = structs.CarControl()
|
||||
command.latActive = decision.lateral_active
|
||||
if self.lateral_controller is not None:
|
||||
torque, _, _ = self.lateral_controller.update(
|
||||
decision.lateral_active, self.cs, self.vehicle_model, self.vehicle_parameters, False,
|
||||
self.desired_curvature, False, self.cp.steerActuatorDelay)
|
||||
command.actuators.torque = float(torque)
|
||||
else:
|
||||
command.actuators.torque = 0.01 if decision.lateral_active else 0.
|
||||
self.last_normalized_torque = float(command.actuators.torque)
|
||||
_, packets = self.ci.apply(command.as_reader(), self.now)
|
||||
lkas = [packet for packet in packets if packet[0] == 0x340]
|
||||
assert lkas, 'Actual controller must emit LKAS'
|
||||
self.output_parser.update((self.now, lkas))
|
||||
self.last_torque = self.output_parser.vl['LKAS11']['CR_Lkas_StrToqReq']
|
||||
for address, data, bus in lkas:
|
||||
assert self.safety.safety_tx_hook(make_msg(bus, address, len(data), data)), 'Actual LKAS rejected'
|
||||
return decision
|
||||
|
||||
|
||||
class TestForteIntegration(unittest.TestCase):
|
||||
def tearDown(self):
|
||||
libsafety_py.libsafety.set_alternative_experience(0)
|
||||
|
||||
def warm(self, stream, **kwargs):
|
||||
for _ in range(12):
|
||||
result = stream.tick(**kwargs)
|
||||
self.assertTrue(stream.cs.canValid)
|
||||
return result
|
||||
|
||||
def test_both_forte_controllers_reach_native_with_original_torque_limit(self):
|
||||
for car in (CAR.KIA_FORTE_2019_NON_SCC, CAR.KIA_FORTE_2021_NON_SCC):
|
||||
for source in (0, 0x391):
|
||||
for mode in (ControllerMode.STANDARD, ControllerMode.STARPILOT):
|
||||
for direction in (-1., 1.):
|
||||
with self.subTest(car=car, source=source, mode=mode, direction=direction):
|
||||
case = f'{car}, source={source:#x}, controller={mode}, direction={direction}'
|
||||
stream = ForteStream(car=car, source=source, main_action=9, controller_mode=mode)
|
||||
self.assertEqual(stream.cp.safetyConfigs[0].safetyParam, 0x1c00 if source else 0x1400)
|
||||
self.assertEqual(stream.cp.alternativeExperience, 32)
|
||||
self.assertEqual(stream.lateral_controller.controller_mode, mode)
|
||||
self.assertEqual(bool(stream.lateral_controller.starpilot_extension), mode == ControllerMode.STARPILOT)
|
||||
self.assertTrue(self.warm(stream, main=True).lateral_active, case)
|
||||
self.assertGreater(stream.cs.vEgo, 19., case)
|
||||
self.assertEqual(stream.cs.steeringAngleDeg, 0., case)
|
||||
self.assertEqual(stream.cs.steeringTorque, 0., case)
|
||||
self.assertFalse(stream.cs.steeringPressed, case)
|
||||
stream.desired_curvature = direction * .01
|
||||
for _ in range(180):
|
||||
self.assertTrue(stream.tick(main=True).lateral_active, case)
|
||||
self.assertLessEqual(abs(stream.last_torque), 255, case)
|
||||
context = (f'{case}; vEgo={stream.cs.vEgo}, angle={stream.cs.steeringAngleDeg}, ' +
|
||||
f'driverTorque={stream.cs.steeringTorque}, normalized={stream.last_normalized_torque}, ' +
|
||||
f'sentTorque={stream.last_torque}, factor={stream.lateral_controller.torque_params.latAccelFactor}')
|
||||
self.assertGreater(abs(stream.last_torque), 200, context)
|
||||
self.assertLess(stream.last_torque * direction, 0., context)
|
||||
if mode == ControllerMode.STANDARD:
|
||||
self.assertEqual(stream.last_torque, -direction * 255, context)
|
||||
self.assertFalse(stream.tick(main=False).lateral_active, case)
|
||||
self.assertEqual(stream.last_torque, 0, case)
|
||||
|
||||
def test_native_reset_requires_neutral_and_new_physical_gesture(self):
|
||||
s = ForteStream()
|
||||
self.warm(s)
|
||||
self.assertTrue(s.tick(bcm=True).lateral_active)
|
||||
s.reset()
|
||||
self.assertFalse(self.warm(s, bcm=True).lateral_active)
|
||||
s.tick(bcm=True, rejection=s.now + 10_000_000)
|
||||
self.assertFalse(s.tick(bcm=True).lateral_active)
|
||||
s.tick()
|
||||
self.assertTrue(s.tick(bcm=True).lateral_active)
|
||||
|
||||
def test_main_boot_both_button_orders_and_combined_sources(self):
|
||||
s = ForteStream(main_action=9)
|
||||
self.assertTrue(self.warm(s, main=True).lateral_active)
|
||||
self.assertFalse(s.tick(main=True, cruise=True, bcm=True).lateral_active)
|
||||
s.tick(main=True, cruise=True)
|
||||
self.assertTrue(s.tick(main=True, cruise=True, clu=True).lateral_active)
|
||||
t = ForteStream()
|
||||
self.warm(t)
|
||||
self.assertTrue(t.tick(bcm=True).lateral_active)
|
||||
self.assertTrue(t.tick(bcm=True, clu=False).lateral_active)
|
||||
self.assertTrue(t.tick(bcm=True, clu=False).lateral_active)
|
||||
t.tick()
|
||||
self.assertFalse(t.tick(clu=True).lateral_active)
|
||||
|
||||
def test_wrong_session_stale_receipt_and_output_pause(self):
|
||||
s = ForteStream(main_action=9)
|
||||
self.assertTrue(self.warm(s, main=True).lateral_active)
|
||||
self.assertFalse(s.axis(lambda n: replace(n, axisSessionId='old')).lateral_active)
|
||||
self.assertFalse(s.axis(lambda n: replace(n, validUntilMonoTime=s.now - 1)).lateral_active)
|
||||
self.assertFalse(s.tick(main=True, eps=True).lateral_active)
|
||||
self.assertTrue(s.intent.allowed_latch)
|
||||
self.assertFalse(s.tick(main=True, gear='R').lateral_active)
|
||||
self.assertTrue(s.tick(main=True).lateral_active)
|
||||
self.assertFalse(s.tick(main=True, fatal=True).lateral_active)
|
||||
self.assertFalse(s.tick(main=True).lateral_active)
|
||||
s.tick(main=False)
|
||||
self.assertTrue(s.tick(main=True).lateral_active)
|
||||
|
||||
def test_50c_only_ordinary_profile_does_not_require_absent_bcm(self):
|
||||
s = ForteStream(source=0x50c, aol=False)
|
||||
self.assertEqual(s.cp.safetyConfigs[0].safetyParam, 0x1000)
|
||||
self.assertEqual(s.cp.alternativeExperience, 0)
|
||||
self.assertFalse(self.warm(s, main=True).lateral_active)
|
||||
s.tick(main=True, cruise_button=2) # Actual SET interaction precedes cruise rising.
|
||||
self.assertFalse(s.tick(main=True, cruise=True).lateral_active)
|
||||
self.assertTrue(s.cs.cruiseState.available)
|
||||
self.assertTrue(s.cs.cruiseState.enabled)
|
||||
self.assertTrue(s.safety.get_controls_allowed())
|
||||
self.assertEqual(s.safety.aol_get_permission_mask(), 0)
|
||||
|
||||
def test_invalid_main_off_cannot_rearm_and_old_denial_preserves_new_edge(self):
|
||||
s = ForteStream(main_action=9)
|
||||
self.warm(s, main=True)
|
||||
s.tick(main=True, fatal=True)
|
||||
# A malformed/unhealthy state envelope is not confirmed main-OFF evidence.
|
||||
# Parsed CAN remains the source; the health failure is explicitly injected.
|
||||
s.cs.canValid = False
|
||||
s.cs.cruiseState.available = False
|
||||
s.intent.update(s.cs, now_ns=s.now + 1, fault_active=False)
|
||||
self.assertFalse(s.tick(main=True).lateral_active)
|
||||
s.tick(main=False)
|
||||
self.assertTrue(s.tick(main=True).lateral_active)
|
||||
t = ForteStream()
|
||||
self.warm(t)
|
||||
old_denial = t.now
|
||||
self.assertTrue(t.tick(bcm=True).lateral_active)
|
||||
self.assertTrue(t.tick(bcm=True, rejection=old_denial).lateral_active)
|
||||
|
||||
def test_unselected_clu_source_grants_only_after_its_own_fresh_neutral(self):
|
||||
s = ForteStream()
|
||||
# BCM arrives first and remains the required native-health alternative.
|
||||
# CLU's independent neutral and press must still reach the port owner.
|
||||
self.warm(s)
|
||||
self.assertFalse(s.safety.get_controls_allowed())
|
||||
self.assertTrue(s.tick(clu=True).lateral_active)
|
||||
self.assertTrue(s.intent.allowed_latch)
|
||||
self.assertEqual(s.safety.aol_get_permission_mask(), 1)
|
||||
self.assertFalse(s.safety.get_controls_allowed())
|
||||
self.assertTrue(s.tick(clu=True, bcm=False).lateral_active)
|
||||
s.tick()
|
||||
self.assertFalse(s.tick(clu=True).lateral_active)
|
||||
@@ -0,0 +1,223 @@
|
||||
import unittest
|
||||
from unittest.mock import patch
|
||||
|
||||
from opendbc.can import CANPacker
|
||||
from opendbc.car import Bus, gen_empty_fingerprint, structs
|
||||
from opendbc.car.hyundai.interface import CarInterface
|
||||
from opendbc.car.hyundai.forte_aol import FORTE_IDS, FORTE_AOL_MARKER, FORTE_AOL_EXPERIENCE, ForteLkasSources
|
||||
from opendbc.car.hyundai.values import CAR, DBC, HyundaiFlags
|
||||
from openpilot.starpilot.aol.intent import AolSettings
|
||||
from openpilot.starpilot.aol.runtime import decide_axes
|
||||
from openpilot.starpilot.car.hyundai.aol import policy_for, create_intent, native_profile_supported, native_accepts_cp
|
||||
from types import SimpleNamespace
|
||||
|
||||
Button = structs.CarState.ButtonEvent.Type
|
||||
|
||||
|
||||
def params(car=CAR.KIA_FORTE_2019_NON_SCC, source=0x391):
|
||||
fp = gen_empty_fingerprint()
|
||||
if source:
|
||||
fp[0][source] = 8
|
||||
return CarInterface.get_params(car, fp, [], False, False, False)
|
||||
|
||||
|
||||
def settings(lkas=9, main=0):
|
||||
return AolSettings(True, 0., lkas, main, (0, 0, 0), (0, 0, 0))
|
||||
|
||||
|
||||
def state(main=False, cruise=False, events=()):
|
||||
cs = structs.CarState()
|
||||
cs.canValid = True
|
||||
cs.gearShifter = 'drive'
|
||||
cs.cruiseState.available, cs.cruiseState.enabled = main, cruise
|
||||
cs.buttonEvents = [structs.CarState.ButtonEvent(type=button, pressed=pressed) for button, pressed in events]
|
||||
return cs
|
||||
|
||||
|
||||
class TestForteIntent(unittest.TestCase):
|
||||
def test_actual_cp_profile_additions_and_exact_family_isolation(self):
|
||||
for car in FORTE_IDS:
|
||||
for source in (0, 0x391, 0x50c):
|
||||
cp = params(car, source)
|
||||
policy = policy_for(cp)
|
||||
self.assertTrue(policy.intent_supported)
|
||||
self.assertEqual(policy.safety_param_addition, FORTE_AOL_MARKER | (0x800 if source else 0))
|
||||
self.assertEqual(cp.safetyConfigs[0].safetyParam, 0x1800 if source == 0x391 else 0x1000)
|
||||
self.assertEqual(policy.alternative_experience_addition, FORTE_AOL_EXPERIENCE)
|
||||
cp.safetyConfigs[0].safetyParam |= policy.safety_param_addition
|
||||
cp.alternativeExperience |= policy.alternative_experience_addition
|
||||
self.assertTrue(native_profile_supported(int(cp.safetyConfigs[0].safetyModel.raw), cp.safetyConfigs[0].safetyParam))
|
||||
self.assertTrue(native_accepts_cp(cp, int(cp.safetyConfigs[0].safetyModel.raw), cp.safetyConfigs[0].safetyParam))
|
||||
bad = cp.as_reader().as_builder()
|
||||
bad.safetyConfigs[0].safetyParam ^= 0x800
|
||||
self.assertFalse(policy_for(bad).intent_supported)
|
||||
for flag in (HyundaiFlags.LEGACY, HyundaiFlags.CAMERA_SCC, HyundaiFlags.CANFD, HyundaiFlags.FCEV, HyundaiFlags.CCNC):
|
||||
bad = cp.as_reader().as_builder()
|
||||
bad.flags |= int(flag)
|
||||
self.assertFalse(policy_for(bad).intent_supported)
|
||||
for field in ('passive', 'dashcamOnly', 'notCar'):
|
||||
bad = cp.as_reader().as_builder()
|
||||
setattr(bad, field, True)
|
||||
self.assertFalse(policy_for(bad).intent_supported)
|
||||
from opendbc.car.hyundai.non_scc_aol import qualified
|
||||
self.assertFalse(qualified(params(CAR.KIA_FORTE)))
|
||||
self.assertFalse(native_profile_supported(int(structs.CarParams.SafetyModel.hyundai), 0x1000))
|
||||
|
||||
def test_main_boot_and_assignment_defaults_and_symmetric_combined_pause(self):
|
||||
for car in FORTE_IDS:
|
||||
managed = create_intent(params(car), settings(main=9))
|
||||
managed.update(state(main=True), now_ns=1)
|
||||
self.assertTrue(managed.allowed_latch)
|
||||
managed.update(state(main=True, cruise=True, events=((Button.lkas, False),)), now_ns=2)
|
||||
for tick in (3, 5):
|
||||
managed.update(state(main=True, cruise=True, events=((Button.lkas, True),)), now_ns=tick)
|
||||
self.assertEqual(managed.pause_lateral, tick == 3)
|
||||
self.assertTrue(managed.allowed_latch)
|
||||
managed.update(state(main=True, cruise=True, events=((Button.lkas, False),)), now_ns=tick + 1)
|
||||
for source, expected in ((0, True), (0x391, False), (0x50c, False)):
|
||||
implicit = create_intent(params(car, source), settings(lkas=0))
|
||||
implicit.update(state(main=True), now_ns=1)
|
||||
self.assertEqual(implicit.allowed_latch, expected)
|
||||
managed.settings = settings(lkas=0, main=0)
|
||||
managed.update(state(main=True), now_ns=8)
|
||||
self.assertFalse(managed.allowed_latch)
|
||||
managed.settings = settings(lkas=0, main=9)
|
||||
managed.update(state(main=True), now_ns=9)
|
||||
self.assertTrue(managed.allowed_latch)
|
||||
|
||||
def test_lkas_native_denial_fatal_rearm_and_temporary_output_pause(self):
|
||||
intent = create_intent(params(), settings())
|
||||
intent.update(state(events=((Button.lkas, False),)), now_ns=1)
|
||||
intent.update(state(events=((Button.lkas, True),)), now_ns=2)
|
||||
self.assertTrue(intent.allowed_latch)
|
||||
cs = state()
|
||||
cs.steerFaultTemporary = True
|
||||
intent.update(cs, now_ns=3)
|
||||
self.assertTrue(intent.allowed_latch)
|
||||
cs.gearShifter = 'reverse'
|
||||
intent.update(cs, now_ns=4)
|
||||
self.assertTrue(intent.allowed_latch)
|
||||
self.assertFalse(intent.output(cs)[0])
|
||||
intent.update(state(), now_ns=5, native_rejection_ns=5)
|
||||
self.assertFalse(intent.allowed_latch)
|
||||
intent.update(state(events=((Button.lkas, True),)), now_ns=6)
|
||||
self.assertFalse(intent.allowed_latch)
|
||||
intent.update(state(events=((Button.lkas, False),)), now_ns=7)
|
||||
intent.update(state(events=((Button.lkas, True),)), now_ns=8)
|
||||
self.assertTrue(intent.allowed_latch)
|
||||
intent.update(state(), now_ns=9, fault_active=True)
|
||||
self.assertFalse(intent.allowed_latch)
|
||||
intent.update(state(), now_ns=10)
|
||||
self.assertFalse(intent.allowed_latch)
|
||||
|
||||
intent.update(state(events=((Button.lkas, False),)), now_ns=11)
|
||||
intent.update(state(events=((Button.lkas, True),)), now_ns=20)
|
||||
self.assertTrue(intent.allowed_latch)
|
||||
intent.update(state(), now_ns=21, native_rejection_ns=19)
|
||||
self.assertTrue(intent.allowed_latch) # Older denial cannot revoke a newer physical gesture.
|
||||
managed = create_intent(params(), settings(main=9))
|
||||
managed.update(state(main=True), now_ns=1)
|
||||
managed.update(state(main=True), now_ns=2, fault_active=True)
|
||||
bad = state(main=False)
|
||||
bad.canValid = False
|
||||
managed.update(bad, now_ns=3)
|
||||
managed.update(state(main=True), now_ns=4)
|
||||
self.assertFalse(managed.allowed_latch)
|
||||
managed.settings = settings(lkas=0, main=9)
|
||||
managed.update(state(main=True, events=((Button.lkas, False),)), now_ns=5)
|
||||
managed.update(state(main=True, events=((Button.lkas, True),)), now_ns=6)
|
||||
self.assertFalse(managed.allowed_latch)
|
||||
managed.update(state(main=False), now_ns=7)
|
||||
managed.update(state(main=True), now_ns=8)
|
||||
self.assertTrue(managed.allowed_latch)
|
||||
|
||||
def test_transient_fault_preserves_released_input_without_replaying_held_input(self):
|
||||
for fault in ('invalid', 'fatal', 'rejected'):
|
||||
for held in (False, True):
|
||||
with self.subTest(fault=fault, held=held):
|
||||
intent = create_intent(params(), settings())
|
||||
intent.update(state(events=((Button.lkas, False),)), now_ns=1)
|
||||
if held:
|
||||
intent.update(state(events=((Button.lkas, True),)), now_ns=2)
|
||||
self.assertTrue(intent.allowed_latch)
|
||||
failed = state()
|
||||
failed.canValid = fault != 'invalid'
|
||||
intent.update(failed, now_ns=3, fault_active=fault == 'fatal', native_rejection_ns=3 if fault == 'rejected' else 0)
|
||||
self.assertFalse(intent.allowed_latch)
|
||||
intent.update(state(), now_ns=4, fault_active=False)
|
||||
self.assertFalse(intent.allowed_latch)
|
||||
intent.update(state(events=((Button.lkas, True),)), now_ns=5, fault_active=False)
|
||||
self.assertEqual(intent.allowed_latch, not held)
|
||||
if held:
|
||||
intent.update(state(events=((Button.lkas, False),)), now_ns=6, fault_active=False)
|
||||
intent.update(state(events=((Button.lkas, True),)), now_ns=7, fault_active=False)
|
||||
self.assertTrue(intent.allowed_latch)
|
||||
|
||||
def test_actual_ci_ordered_physical_or_and_held_initialization(self):
|
||||
for car in FORTE_IDS:
|
||||
cp = params(car)
|
||||
ci = CarInterface(cp)
|
||||
packer = CANPacker(DBC[car][Bus.pt])
|
||||
ci.update([])
|
||||
def packet(address, pressed, packer=packer):
|
||||
return packer.make_can_msg('BCM_PO_11' if address == 0x391 else 'CLU13', 0,
|
||||
{'LDA_BTN' if address == 0x391 else 'CF_Clu_LdwsLkasSW': int(pressed)})
|
||||
clock = 1_000_000_000
|
||||
def update(samples, ci=ci, packet=packet):
|
||||
nonlocal clock
|
||||
packets = []
|
||||
for address, pressed in samples:
|
||||
clock += 1_000_000
|
||||
packets.append((clock, [packet(address, pressed)]))
|
||||
with patch('opendbc.car.hyundai.non_scc_aol.time.CLOCK_BOOTTIME', 7, create=True), \
|
||||
patch('opendbc.car.hyundai.non_scc_aol.time.clock_gettime_ns', return_value=clock):
|
||||
ret = ci.update(packets)
|
||||
return [event.pressed for event in ret.buttonEvents if event.type == Button.lkas]
|
||||
self.assertEqual(update(((0x391, True), (0x50c, False))), [])
|
||||
self.assertEqual(update(((0x391, False),)), [False])
|
||||
self.assertEqual(update(((0x391, True), (0x50c, False), (0x391, True))), [True])
|
||||
self.assertEqual(update(((0x391, False),)), [False])
|
||||
self.assertEqual(update(((0x50c, True), (0x50c, False))), [True, False])
|
||||
clock += 300_000_001
|
||||
self.assertEqual(update(((0x391, True),)), [])
|
||||
self.assertEqual(update(((0x391, True),)), [])
|
||||
self.assertEqual(update(((0x391, False), (0x391, True), (0x391, False))), [False, True, False])
|
||||
|
||||
def test_new_or_expired_source_must_show_own_neutral_without_phantom_edges(self):
|
||||
owner = ForteLkasSources()
|
||||
clock = 1_000_000_000
|
||||
def update(samples):
|
||||
nonlocal clock
|
||||
packets = []
|
||||
for address, pressed in samples:
|
||||
clock += 1_000_000
|
||||
data = bytearray(8)
|
||||
data[0 if address == 0x391 else 7] = 0x10 if address == 0x391 and pressed else int(pressed)
|
||||
packets.append((clock, [(address, bytes(data), 0)]))
|
||||
with patch('opendbc.car.hyundai.non_scc_aol.time.CLOCK_BOOTTIME', 7, create=True), \
|
||||
patch('opendbc.car.hyundai.non_scc_aol.time.clock_gettime_ns', return_value=clock):
|
||||
owner.update(packets)
|
||||
return owner.edges
|
||||
self.assertEqual(update(((0x391, False),)), [False])
|
||||
self.assertEqual(update(((0x50c, True),)), [])
|
||||
self.assertEqual(update(((0x50c, False),)), [False])
|
||||
self.assertEqual(update(((0x50c, True), (0x50c, False))), [True, False])
|
||||
self.assertEqual(update(((0x391, True),)), [True])
|
||||
clock += 300_000_001 # Lost release: expiry cannot fabricate a replacement gesture.
|
||||
self.assertEqual(update(((0x391, True), (0x50c, False))), [])
|
||||
self.assertEqual(update(((0x391, False), (0x391, True))), [False, True])
|
||||
|
||||
def test_unacknowledged_native_denial_and_wrong_request_remain_neutral(self):
|
||||
cs = state(main=True)
|
||||
intent = SimpleNamespace(pauseLateral=False, pauseLongitudinal=False, allowedLatch=True)
|
||||
common: dict = {'standard_lateral': False, 'standard_longitudinal': False, 'intent': intent, 'car_state': cs,
|
||||
'initialized': True, 'model_ready': True, 'no_entry': False, 'immediate_disable': False, 'dm_lockout': False, 'pause_brake_mps': 0.}
|
||||
for native in (None, SimpleNamespace(requestedLateral=True, requestedLongitudinal=False, lateralAllowed=False),
|
||||
SimpleNamespace(requestedLateral=False, requestedLongitudinal=False, lateralAllowed=True)):
|
||||
result = decide_axes(native=native, **common)
|
||||
self.assertTrue(result.desired_lateral)
|
||||
self.assertFalse(result.lateral_active)
|
||||
native = SimpleNamespace(requestedLateral=True, requestedLongitudinal=False, lateralAllowed=True)
|
||||
self.assertTrue(decide_axes(native=native, **common).lateral_active)
|
||||
cs.steerFaultTemporary = True
|
||||
self.assertFalse(decide_axes(native=native, **common).lateral_active)
|
||||
@@ -0,0 +1,78 @@
|
||||
"""Independent axes use the finalized ordinary ASCM vehicle owner."""
|
||||
|
||||
import unittest
|
||||
|
||||
from opendbc.car.gm.aol import qualified_gm
|
||||
from opendbc.car.gm.tests.test_ascm_intercept import params
|
||||
from opendbc.car.gm.values import ORDINARY_ASCM_CAR
|
||||
from openpilot.starpilot.aol.vehicle import policy_for, create_intent
|
||||
from openpilot.starpilot.aol.intent import AolSettings
|
||||
|
||||
|
||||
class TestGmAscmAol(unittest.TestCase):
|
||||
def test_actual_finalized_cp_and_default_off(self):
|
||||
settings = AolSettings(False, 0., 0, 0, (0, 0, 0), (0, 0, 0))
|
||||
for identity in ORDINARY_ASCM_CAR:
|
||||
for release in (False, True):
|
||||
for alpha in (False, True):
|
||||
for c9 in (False, True):
|
||||
for radar in (False, True):
|
||||
cp = params(identity, sascm=True, alpha=alpha, release=release, accelerator=not c9, radar=radar)
|
||||
self.assertTrue(qualified_gm(cp))
|
||||
self.assertEqual(cp.alternativeExperience, 0)
|
||||
policy = policy_for(cp)
|
||||
self.assertTrue(policy.normal_runtime_supported)
|
||||
intent = create_intent(cp, settings, policy)
|
||||
self.assertFalse(intent.allowed_latch)
|
||||
cp.alternativeExperience = 32
|
||||
self.assertTrue(qualified_gm(cp))
|
||||
cp.alternativeExperience = 33
|
||||
self.assertFalse(qualified_gm(cp))
|
||||
|
||||
def test_profile_integrity_and_finite_tune_boundaries(self):
|
||||
for identity in ORDINARY_ASCM_CAR:
|
||||
cp = params(identity, sascm=True)
|
||||
for field in ('passive', 'dashcamOnly', 'notCar'):
|
||||
setattr(cp, field, True)
|
||||
self.assertFalse(qualified_gm(cp))
|
||||
setattr(cp, field, False)
|
||||
cp.safetyConfigs[0].safetyParam |= 4
|
||||
self.assertFalse(qualified_gm(cp))
|
||||
cp = params(identity, sascm=True)
|
||||
if cp.lateralTuning.which() == 'torque':
|
||||
cp.lateralTuning.torque.friction = float('nan')
|
||||
else:
|
||||
cp.lateralTuning.pid.kpV = [float('nan')]
|
||||
self.assertFalse(qualified_gm(cp))
|
||||
|
||||
def test_actual_controller_cruise_off_lateral_only_and_default_off(self):
|
||||
from opendbc.can import CANPacker
|
||||
from opendbc.car import Bus, structs
|
||||
from opendbc.car.gm.interface import CarInterface
|
||||
from opendbc.car.gm.tests.test_volt_camera_control import feed_camera
|
||||
from opendbc.car.gm.values import DBC
|
||||
for identity in ORDINARY_ASCM_CAR:
|
||||
for alpha in (False, True):
|
||||
for alternative in (0, 32):
|
||||
cp = params(identity, sascm=True, alpha=alpha)
|
||||
cp.alternativeExperience = alternative
|
||||
ci = CarInterface(cp)
|
||||
packer = CANPacker(DBC[identity][Bus.pt])
|
||||
observed = []
|
||||
for tick in range(6):
|
||||
now = 1_000_000_000 + tick * 40_000_000
|
||||
out, sources = feed_camera(ci, packer, now, counter=tick % 4, active=False)
|
||||
sources = [packet for packet in sources if packet[0] != 0x370]
|
||||
sources.append(packer.make_can_msg('ASCMActiveCruiseControlStatus', 2, {'ACCCruiseState': 0}))
|
||||
out = ci.update([(now + 1, sources)])
|
||||
now += 1
|
||||
self.assertTrue(out.canValid)
|
||||
self.assertTrue(out.cruiseState.available)
|
||||
self.assertFalse(out.cruiseState.enabled)
|
||||
command = structs.CarControl(enabled=False, latActive=alternative == 32, longActive=False)
|
||||
command.actuators.torque = .1
|
||||
_, packets = ci.apply(command.as_reader(), now)
|
||||
observed.extend(packet for packet in packets if packet[0] == 0x180)
|
||||
self.assertTrue(observed)
|
||||
nonneutral = any(((packet[1][0] & 7) << 8 | packet[1][1]) != 0 for packet in observed)
|
||||
self.assertEqual(nonneutral, alternative == 32)
|
||||
@@ -0,0 +1,133 @@
|
||||
"""Exercise the real cross-socket wakeup between card and selfdrived."""
|
||||
|
||||
import time
|
||||
from types import SimpleNamespace
|
||||
from unittest.mock import patch
|
||||
|
||||
import pytest
|
||||
|
||||
from opendbc.car.structs import car
|
||||
from openpilot.cereal import messaging
|
||||
from openpilot.common.prefix import OpenpilotPrefix
|
||||
from openpilot.selfdrive.car.card import Car
|
||||
from openpilot.selfdrive.selfdrived.selfdrived import SelfdriveD
|
||||
from openpilot.starpilot.aol.runtime import current_intent, current_native, decide_axes
|
||||
from openpilot.starpilot.aol.wire import IntentState, SafetyState, encode_intent, encode_safety
|
||||
|
||||
|
||||
@pytest.mark.parametrize('pause_lateral,qualified,expected', [(False, True, 'combined'), (True, True, 'longitudinalOnly'),
|
||||
(False, False, 'off')])
|
||||
def test_card_companion_is_available_when_car_state_wakes_selfdrive(pause_lateral, qualified, expected):
|
||||
with OpenpilotPrefix():
|
||||
messaging.reset_context()
|
||||
pm = messaging.PubMaster(['carState', 'carOutput', 'aolIntentWire', 'aolSafetyWire'])
|
||||
sd = SelfdriveD.__new__(SelfdriveD)
|
||||
sd.sm = messaging.SubMaster(['aolIntentWire', 'aolSafetyWire'])
|
||||
sd.car_state_sock = messaging.sub_sock('carState', timeout=100)
|
||||
sd.initialized, sd.enabled = True, False
|
||||
sd.aol_car_state_log_ns, sd.conditional_car_state_valid = 0, False
|
||||
sd.CS_prev = car.CarState.new_message(canValid=True, gearShifter=car.CarState.GearShifter.drive, vEgo=20.)
|
||||
stamp = time.monotonic_ns()
|
||||
prior = stamp - 29_350_000 # Route: 29 ms CAN gap plus 2.7 ms consumer latency.
|
||||
old = messaging.new_message('aolIntentWire', 0, valid=True)
|
||||
old.logMonoTime = prior
|
||||
old.aolIntentWire = encode_intent(IntentState('card', 1, prior, prior, prior + 200_000_000,
|
||||
True, False, False, True, True))
|
||||
pm.send('aolIntentWire', old)
|
||||
sd.sm.update(100)
|
||||
assert sd.sm.seen['aolIntentWire']
|
||||
# Native permission is current and unchanged throughout the race.
|
||||
cp = car.CarParams.new_message()
|
||||
cp.safetyConfigs = [car.CarParams.SafetyConfig(safetyModel=car.CarParams.SafetyModel.hondaBosch, safetyParam=34)]
|
||||
native = messaging.new_message('aolSafetyWire', 0, valid=True)
|
||||
native.logMonoTime = stamp
|
||||
native.aolSafetyWire = encode_safety(SafetyState(1, True, stamp, stamp + 200_000_000,
|
||||
int(car.CarParams.SafetyModel.hondaBosch), 34, True, True, True, True, 'panda', 'drive'))
|
||||
pm.send('aolSafetyWire', native)
|
||||
sd.sm.update(100)
|
||||
assert sd.sm.seen['aolSafetyWire']
|
||||
decisions = []
|
||||
|
||||
def send(service, message):
|
||||
pm.send(service, message)
|
||||
if service == 'carState':
|
||||
# Force the consumer to run at the earliest possible publication
|
||||
# boundary, before card can execute its next line. Both receive paths
|
||||
# are real sockets and data_sample retains its nonblocking SM update.
|
||||
state = sd.data_sample()
|
||||
now = stamp + 2_800_000
|
||||
intent = current_intent(sd.sm, car_state_ns=sd.aol_car_state_log_ns, now_ns=now)
|
||||
ack = current_native(sd.sm, cp, now_ns=now, axis_session_id='drive')
|
||||
assert ack is not None
|
||||
if qualified:
|
||||
assert intent is not None
|
||||
assert intent.carStateLogMonoTime == sd.aol_car_state_log_ns == stamp
|
||||
else:
|
||||
assert intent is None
|
||||
decisions.append(decide_axes(standard_lateral=True, standard_longitudinal=True,
|
||||
intent=intent, native=ack, car_state=state, initialized=True, model_ready=True,
|
||||
no_entry=False, immediate_disable=False, dm_lockout=False, pause_brake_mps=0))
|
||||
# The original 30 ms contract remains exact; no arrival-side renewal.
|
||||
assert current_intent(sd.sm, car_state_ns=stamp, now_ns=stamp + 30_000_001) is None
|
||||
|
||||
card = Car.__new__(Car)
|
||||
card.__dict__.update(sm=SimpleNamespace(frame=1, all_checks=lambda _: True), pm=SimpleNamespace(send=send),
|
||||
CP=cp, car_params_published=True, slc_replay=False, curve_replay=False, conditional_replay=False,
|
||||
last_actuators_output=car.CarControl.Actuators.new_message(), can_rcv_cum_timeout_counter=0,
|
||||
rk=SimpleNamespace(remaining=0), aol_replay=True, aol_qualified=qualified, aol_sequence=1,
|
||||
slc_producer_session='card', aol_card_intent=SimpleNamespace(allowed_latch=True, output=lambda _: (True, pause_lateral, False)),
|
||||
v_cruise_helper=SimpleNamespace(slc_cruise_change=None))
|
||||
create = messaging.new_message
|
||||
|
||||
def stamped(service, *args, **kwargs):
|
||||
message = create(service, *args, **kwargs)
|
||||
if service == 'carState':
|
||||
message.logMonoTime = stamp
|
||||
return message
|
||||
|
||||
with patch('openpilot.selfdrive.car.card.messaging.new_message', side_effect=stamped):
|
||||
card.state_publish(sd.CS_prev, None)
|
||||
assert len(decisions) == 1
|
||||
assert decisions[0].mode == expected
|
||||
|
||||
|
||||
def test_future_companion_only_reuses_same_car_state_original_lease():
|
||||
with OpenpilotPrefix():
|
||||
messaging.reset_context()
|
||||
pm = messaging.PubMaster(['carState', 'aolIntentWire'])
|
||||
sd = SelfdriveD.__new__(SelfdriveD)
|
||||
sd.sm = messaging.SubMaster(['aolIntentWire'])
|
||||
sd.car_state_sock = messaging.sub_sock('carState', timeout=1)
|
||||
sd.initialized, sd.enabled = True, False
|
||||
sd.aol_car_state_log_ns, sd.conditional_car_state_valid = 0, False
|
||||
sd.CS_prev = car.CarState.new_message(canValid=True, gearShifter=car.CarState.GearShifter.drive, vEgo=20.)
|
||||
stamp = time.monotonic_ns()
|
||||
|
||||
def publish_intent(at, *, valid=True, qualified=True, session='card'):
|
||||
message = messaging.new_message('aolIntentWire', 0, valid=valid)
|
||||
message.logMonoTime = at
|
||||
message.aolIntentWire = encode_intent(IntentState(session, 1, at, at, at + 200_000_000,
|
||||
True, True, False, qualified, True))
|
||||
pm.send('aolIntentWire', message)
|
||||
|
||||
publish_intent(stamp)
|
||||
state = messaging.new_message('carState', valid=True)
|
||||
state.logMonoTime, state.carState = stamp, sd.CS_prev
|
||||
pm.send('carState', state)
|
||||
sd.CS_prev = sd.data_sample()
|
||||
previous = current_intent(sd.sm, car_state_ns=stamp, now_ns=stamp + 1_000_000)
|
||||
assert previous is not None
|
||||
# Card has sent the next intent, but not its carState. Exercise the real
|
||||
# carState socket timeout and the real nonblocking SubMaster receive.
|
||||
publish_intent(stamp + 20_000_000)
|
||||
with patch('openpilot.selfdrive.selfdrived.selfdrived.time.monotonic_ns', return_value=stamp + 23_000_000):
|
||||
assert sd.data_sample() is sd.CS_prev
|
||||
assert sd.aol_car_state_log_ns == stamp
|
||||
assert current_intent(sd.sm, car_state_ns=stamp, now_ns=stamp + 23_000_000) is None
|
||||
assert current_intent(sd.sm, car_state_ns=stamp, now_ns=stamp + 23_000_000, previous=previous) == previous
|
||||
assert current_intent(sd.sm, car_state_ns=stamp, now_ns=stamp + 30_000_001, previous=previous) is None
|
||||
assert current_intent(sd.sm, car_state_ns=stamp - 1, now_ns=stamp + 23_000_000, previous=previous) is None
|
||||
for changes in ({'valid': False}, {'qualified': False}, {'session': 'new-card'}):
|
||||
publish_intent(stamp + 20_000_000, **changes)
|
||||
sd.sm.update(100)
|
||||
assert current_intent(sd.sm, car_state_ns=stamp, now_ns=stamp + 23_000_000, previous=previous) is None
|
||||
@@ -0,0 +1,86 @@
|
||||
"""Exact stock Ioniq AOL admission and saved-request cleanup regression."""
|
||||
import ast
|
||||
from pathlib import Path
|
||||
from types import SimpleNamespace
|
||||
import pytest
|
||||
from opendbc.car.structs import car
|
||||
from opendbc.car.hyundai.values import CAR, HyundaiFlags
|
||||
from openpilot.starpilot.car.hyundai.aol import (policy_for, qualified_ioniq6, ioniq6_settings_capable,
|
||||
native_profile_supported, native_accepts_cp)
|
||||
from openpilot.starpilot.aol.intent import AolCardIntent, AolSettings
|
||||
|
||||
|
||||
def stock(alternate):
|
||||
cp = car.CarParams.new_message()
|
||||
cp.brand = 'hyundai'
|
||||
cp.carFingerprint = CAR.HYUNDAI_IONIQ_6
|
||||
cp.flags = int(HyundaiFlags.CANFD | HyundaiFlags.EV | HyundaiFlags.CANFD_LKA_STEER_MSG)
|
||||
if alternate:
|
||||
cp.flags |= int(HyundaiFlags.CANFD_LKA_STEER_MSG_ALT)
|
||||
cp.pcmCruise = True
|
||||
cp.openpilotLongitudinalControl = False
|
||||
cp.radarUnavailable = False
|
||||
cp.safetyConfigs = [{'safetyModel': car.CarParams.SafetyModel.hyundaiCanfd,
|
||||
'safetyParam': 0x91 if alternate else 0x11}]
|
||||
return cp
|
||||
|
||||
|
||||
@pytest.mark.parametrize('alternate', [False, True])
|
||||
def test_stock_marker_is_lateral_only_and_native_exact(alternate):
|
||||
cp = stock(alternate)
|
||||
original = int(cp.safetyConfigs[0].safetyParam)
|
||||
policy = policy_for(cp)
|
||||
assert policy.intent_supported and policy.runtime_supported and policy.normal_runtime_supported
|
||||
assert policy.explicit_latch and policy.safety_param_addition == 0x0800
|
||||
assert not policy.distance_personality
|
||||
assert ioniq6_settings_capable(cp) and not qualified_ioniq6(cp)
|
||||
assert not native_accepts_cp(cp, int(car.CarParams.SafetyModel.hyundaiCanfd), original)
|
||||
intent = AolCardIntent(AolSettings(True, 0., 0, 0, (0,0,0), (0,0,0)), explicit_latch=policy.explicit_latch)
|
||||
assert intent.explicit_latch and not intent.allowed_latch
|
||||
cp.safetyConfigs[0].safetyParam |= policy.safety_param_addition
|
||||
assert qualified_ioniq6(cp) and ioniq6_settings_capable(cp)
|
||||
assert native_profile_supported(int(car.CarParams.SafetyModel.hyundaiCanfd), original | 0x800)
|
||||
assert native_accepts_cp(cp, int(car.CarParams.SafetyModel.hyundaiCanfd), original | 0x800)
|
||||
assert cp.pcmCruise and not cp.openpilotLongitudinalControl and not cp.alphaLongitudinalAvailable
|
||||
cp.flags |= int(HyundaiFlags.CANFD_ALT_BUTTONS)
|
||||
assert not policy_for(cp).runtime_supported and not qualified_ioniq6(cp)
|
||||
|
||||
|
||||
@pytest.mark.parametrize('alternate', [False, True])
|
||||
def test_existing_long_profile_retained(alternate):
|
||||
cp = stock(alternate)
|
||||
cp.pcmCruise = False
|
||||
cp.openpilotLongitudinalControl = True
|
||||
cp.safetyConfigs[0].safetyParam = 0x8895 if alternate else 0x8815
|
||||
assert qualified_ioniq6(cp)
|
||||
assert policy_for(cp).intent_supported and policy_for(cp).explicit_latch
|
||||
|
||||
|
||||
def cleanup(cp):
|
||||
# Execute the actual saved-request cleanup nodes without initializing processes.
|
||||
source = Path(__file__).parents[3] / 'selfdrive/selfdrived/selfdrived.py'
|
||||
tree = ast.parse(source.read_text())
|
||||
cls = next(node for node in tree.body if isinstance(node, ast.ClassDef) and node.name == 'SelfdriveD')
|
||||
method = next(node for node in cls.body if isinstance(node, ast.FunctionDef) and node.name == '__init__')
|
||||
# Execute the real startup cleanup condition; constructor side effects stay absent.
|
||||
nodes: list[ast.stmt] = [node for node in method.body if isinstance(node, ast.If) and any(
|
||||
isinstance(call, ast.Call) and isinstance(call.func, ast.Attribute) and call.func.attr == 'remove'
|
||||
and call.args and isinstance(call.args[0], ast.Constant)
|
||||
and call.args[0].value in ('AlphaLongitudinalEnabled', 'ExperimentalMode') for call in ast.walk(node))]
|
||||
calls = []
|
||||
host = SimpleNamespace(CP=cp, params=SimpleNamespace(remove=calls.append))
|
||||
exec(compile(ast.Module(body=nodes, type_ignores=[]), str(source), 'exec'), {'self': host})
|
||||
return calls
|
||||
|
||||
|
||||
def test_actual_cleanup_preserves_request_without_enabling_long():
|
||||
cp = stock(True)
|
||||
assert cleanup(cp) == ['ExperimentalMode']
|
||||
cp.passive = True
|
||||
cp.dashcamOnly = True
|
||||
assert not qualified_ioniq6(cp) and not ioniq6_settings_capable(cp)
|
||||
assert cleanup(cp) == ['ExperimentalMode']
|
||||
cp.carFingerprint = CAR.HYUNDAI_IONIQ_5
|
||||
assert cleanup(cp) == ['ExperimentalMode']
|
||||
cp.openpilotLongitudinalControl = True
|
||||
assert cleanup(cp) == []
|
||||
@@ -0,0 +1,706 @@
|
||||
import time
|
||||
import unittest
|
||||
from unittest import mock
|
||||
from types import SimpleNamespace
|
||||
from pathlib import Path
|
||||
|
||||
from openpilot.cereal import log, messaging
|
||||
from openpilot.common.prefix import OpenpilotPrefix
|
||||
from opendbc.car.structs import car
|
||||
from openpilot.selfdrive.car.cruise import CRUISE_LONG_PRESS
|
||||
from openpilot.common.params import Params
|
||||
from openpilot.selfdrive.selfdrived.selfdrived import SelfdriveD
|
||||
from openpilot.selfdrive.selfdrived.alertmanager import AlertManager
|
||||
from openpilot.selfdrive.selfdrived.events import Events, ET
|
||||
from openpilot.selfdrive.selfdrived.state import StateMachine
|
||||
from openpilot.starpilot.aol.intent import AolCardIntent, AolSettings, disarming_fault, read_settings
|
||||
from openpilot.starpilot.aol.runtime import AxisDecision, current_intent, current_native, decide_axes
|
||||
from openpilot.starpilot.aol.wire import IntentState, SafetyState, encode_intent, encode_safety
|
||||
|
||||
|
||||
def car_state(*, brake=False):
|
||||
return car.CarState(canValid=True, gearShifter=car.CarState.GearShifter.drive,
|
||||
vEgo=20.0, brakePressed=brake)
|
||||
|
||||
|
||||
class CardIntentTests(unittest.TestCase):
|
||||
def test_temporary_eps_and_known_gears_preserve_latch_with_zero_steering(self):
|
||||
owner = AolCardIntent(AolSettings(True, 0, 0, 0, (0, 0, 0), (0, 0, 0)), explicit_latch=True)
|
||||
state = car_state()
|
||||
owner.update(state)
|
||||
state.buttonEvents = [car.CarState.ButtonEvent(type=car.CarState.ButtonEvent.Type.lkas, pressed=True)]
|
||||
owner.update(state)
|
||||
state.buttonEvents = [car.CarState.ButtonEvent(type=car.CarState.ButtonEvent.Type.lkas, pressed=False)]
|
||||
owner.update(state)
|
||||
state.buttonEvents = []
|
||||
for gear, temporary, name in (
|
||||
(car.CarState.GearShifter.drive, True, log.OnroadEvent.EventName.steerTempUnavailable),
|
||||
(car.CarState.GearShifter.neutral, False, log.OnroadEvent.EventName.wrongGear),
|
||||
(car.CarState.GearShifter.reverse, False, log.OnroadEvent.EventName.wrongGear),
|
||||
(car.CarState.GearShifter.park, False, log.OnroadEvent.EventName.wrongGear)):
|
||||
with self.subTest(gear=gear, temporary=temporary):
|
||||
state.gearShifter = gear
|
||||
state.steerFaultTemporary = temporary
|
||||
events = Events()
|
||||
events.add(name)
|
||||
fault = disarming_fault(events.to_msg(), state)
|
||||
self.assertFalse(fault)
|
||||
for _ in range(100):
|
||||
owner.update(state, fault_active=fault)
|
||||
self.assertTrue(owner.allowed_latch)
|
||||
allowed, pause_lat, pause_long = owner.output(state)
|
||||
decision = decide_axes(standard_lateral=False, standard_longitudinal=False,
|
||||
intent=SimpleNamespace(allowedLatch=allowed, pauseLateral=pause_lat, pauseLongitudinal=pause_long),
|
||||
native=SimpleNamespace(requestedLateral=False, requestedLongitudinal=False, lateralAllowed=False, longitudinalAllowed=False),
|
||||
car_state=state, initialized=True, model_ready=True, no_entry=True, immediate_disable=False,
|
||||
dm_lockout=False, pause_brake_mps=0)
|
||||
self.assertFalse(decision.desired_lateral)
|
||||
self.assertFalse(decision.lateral_active)
|
||||
state.gearShifter = car.CarState.GearShifter.drive
|
||||
state.steerFaultTemporary = False
|
||||
owner.update(state, fault_active=False)
|
||||
self.assertTrue(owner.output(state)[0])
|
||||
decision = decide_axes(standard_lateral=False, standard_longitudinal=False,
|
||||
intent=SimpleNamespace(allowedLatch=owner.output(state)[0], pauseLateral=False, pauseLongitudinal=False),
|
||||
native=SimpleNamespace(requestedLateral=True, requestedLongitudinal=False, lateralAllowed=True, longitudinalAllowed=False),
|
||||
car_state=state, initialized=True, model_ready=True, no_entry=False, immediate_disable=False,
|
||||
dm_lockout=False, pause_brake_mps=0)
|
||||
self.assertTrue(decision.lateral_active)
|
||||
|
||||
def test_low_speed_brake_suppresses_output_without_destroying_explicit_latch(self):
|
||||
owner = AolCardIntent(AolSettings(True, 5.0, 0, 0, (0, 0, 0), (0, 0, 0)), explicit_latch=True)
|
||||
state = car_state()
|
||||
owner.update(state)
|
||||
state.buttonEvents = [car.CarState.ButtonEvent(type=car.CarState.ButtonEvent.Type.lkas, pressed=True)]
|
||||
owner.update(state)
|
||||
state.buttonEvents = []
|
||||
events = Events()
|
||||
events.add(log.OnroadEvent.EventName.pedalPressed)
|
||||
for speed, brake, expected in ((2., True, False), (2., False, True), (6., True, True)):
|
||||
state.vEgo, state.brakePressed = speed, brake
|
||||
owner.update(state, fault_active=disarming_fault(events.to_msg(), state))
|
||||
self.assertTrue(owner.allowed_latch)
|
||||
allowed, pause_lat, pause_long = owner.output(state)
|
||||
axes = decide_axes(standard_lateral=False, standard_longitudinal=False,
|
||||
intent=SimpleNamespace(allowedLatch=allowed, pauseLateral=pause_lat, pauseLongitudinal=pause_long),
|
||||
native=SimpleNamespace(requestedLateral=expected, requestedLongitudinal=False,
|
||||
lateralAllowed=expected, longitudinalAllowed=False),
|
||||
car_state=state, initialized=True, model_ready=True, no_entry=False, immediate_disable=False,
|
||||
dm_lockout=False, pause_brake_mps=5.0)
|
||||
self.assertEqual(axes.lateral_active, expected)
|
||||
self.assertFalse(axes.longitudinal_active)
|
||||
|
||||
def test_permanent_and_unrelated_faults_still_disarm(self):
|
||||
state = car_state()
|
||||
for name in (log.OnroadEvent.EventName.steerUnavailable, log.OnroadEvent.EventName.controlsMismatch,
|
||||
log.OnroadEvent.EventName.overheat):
|
||||
events = Events()
|
||||
events.add(name)
|
||||
self.assertTrue(disarming_fault(events.to_msg(), state))
|
||||
events = Events()
|
||||
events.add(log.OnroadEvent.EventName.wrongGear)
|
||||
self.assertFalse(disarming_fault(events.to_msg(), state)) # Events may lag the return to Drive.
|
||||
state.gearShifter = car.CarState.GearShifter.unknown
|
||||
self.assertTrue(disarming_fault(events.to_msg(), state))
|
||||
state.steerFaultPermanent = True
|
||||
self.assertTrue(disarming_fault([], state))
|
||||
owner = AolCardIntent(AolSettings(True, 0, 0, 0, (0, 0, 0), (0, 0, 0)), explicit_latch=True)
|
||||
owner.allowed_latch = True
|
||||
owner.update(state)
|
||||
self.assertFalse(owner.allowed_latch)
|
||||
|
||||
def test_native_rejection_rearms_with_one_new_press_without_replaying_old_receipt(self):
|
||||
owner = AolCardIntent(AolSettings(True, 0, 0, 0, (0, 0, 0), (0, 0, 0)), explicit_latch=True)
|
||||
state = car_state()
|
||||
lkas = car.CarState.ButtonEvent.Type.lkas
|
||||
owner.update(state, now_ns=100)
|
||||
state.buttonEvents = [car.CarState.ButtonEvent(type=lkas, pressed=True)]
|
||||
owner.update(state, now_ns=200)
|
||||
self.assertTrue(owner.allowed_latch)
|
||||
state.buttonEvents = [car.CarState.ButtonEvent(type=lkas, pressed=False)]
|
||||
owner.update(state, now_ns=300)
|
||||
state.buttonEvents = []
|
||||
owner.update(state, now_ns=400, native_rejection_ns=350)
|
||||
self.assertFalse(owner.allowed_latch)
|
||||
state.buttonEvents = [car.CarState.ButtonEvent(type=lkas, pressed=True)]
|
||||
owner.update(state, now_ns=500, native_rejection_ns=350)
|
||||
self.assertTrue(owner.allowed_latch)
|
||||
state.buttonEvents = []
|
||||
owner.update(state, now_ns=600, native_rejection_ns=450)
|
||||
self.assertTrue(owner.allowed_latch) # Delayed denial predates the new press.
|
||||
owner.update(state, now_ns=700, native_rejection_ns=650)
|
||||
self.assertFalse(owner.allowed_latch)
|
||||
owner.update(state, now_ns=800)
|
||||
self.assertFalse(owner.allowed_latch) # The held physical button cannot rearm.
|
||||
state.buttonEvents = [car.CarState.ButtonEvent(type=lkas, pressed=False)]
|
||||
owner.update(state, now_ns=900)
|
||||
state.buttonEvents = [car.CarState.ButtonEvent(type=lkas, pressed=True)]
|
||||
owner.update(state, now_ns=1000)
|
||||
self.assertTrue(owner.allowed_latch)
|
||||
|
||||
def test_native_rejection_and_fresh_press_same_frame_preserve_fault_inhibit(self):
|
||||
for fault in (False, True):
|
||||
owner = AolCardIntent(AolSettings(True, 0, 0, 0, (0, 0, 0), (0, 0, 0)), explicit_latch=True)
|
||||
state = car_state()
|
||||
owner.update(state, now_ns=100)
|
||||
owner.allowed_latch = True
|
||||
state.buttonEvents = [car.CarState.ButtonEvent(type=car.CarState.ButtonEvent.Type.lkas, pressed=True)]
|
||||
owner.update(state, now_ns=200, native_rejection_ns=150, fault_active=fault)
|
||||
self.assertEqual(owner.allowed_latch, not fault)
|
||||
|
||||
def test_explicit_latch_requires_new_gesture_after_fault(self):
|
||||
owner = AolCardIntent(AolSettings(True, 0, 0, 0, (0, 0, 0), (0, 0, 0)), explicit_latch=True)
|
||||
state = car_state()
|
||||
lkas = car.CarState.ButtonEvent.Type.lkas
|
||||
owner.update(state, fault_active=False)
|
||||
state.buttonEvents = [car.CarState.ButtonEvent(type=lkas, pressed=True)]
|
||||
owner.update(state)
|
||||
self.assertTrue(owner.allowed_latch)
|
||||
|
||||
state.buttonEvents = []
|
||||
owner.update(state, fault_active=True)
|
||||
self.assertFalse(owner.allowed_latch)
|
||||
state.buttonEvents = [car.CarState.ButtonEvent(type=lkas, pressed=False)]
|
||||
owner.update(state, fault_active=True)
|
||||
state.buttonEvents = [car.CarState.ButtonEvent(type=lkas, pressed=True)]
|
||||
owner.update(state, fault_active=True)
|
||||
self.assertFalse(owner.allowed_latch)
|
||||
state.buttonEvents = [car.CarState.ButtonEvent(type=lkas, pressed=False)]
|
||||
owner.update(state)
|
||||
state.buttonEvents = [car.CarState.ButtonEvent(type=lkas, pressed=True)]
|
||||
owner.update(state) # No fresh clear event yet.
|
||||
self.assertFalse(owner.allowed_latch)
|
||||
|
||||
# A held button at recovery is not a new request. The driver must release it first.
|
||||
owner.update(state, fault_active=False)
|
||||
self.assertFalse(owner.allowed_latch)
|
||||
state.buttonEvents = [car.CarState.ButtonEvent(type=lkas, pressed=False)]
|
||||
owner.update(state)
|
||||
state.buttonEvents = [car.CarState.ButtonEvent(type=lkas, pressed=True)]
|
||||
owner.update(state)
|
||||
self.assertTrue(owner.allowed_latch)
|
||||
|
||||
def test_persisted_preferences_and_fresh_defaults(self):
|
||||
with OpenpilotPrefix():
|
||||
params = Params()
|
||||
fresh = read_settings(params)
|
||||
self.assertFalse(fresh.enabled)
|
||||
self.assertEqual(fresh.pause_brake_mps, 0.0)
|
||||
self.assertEqual(fresh.distance_actions, (0, 0, 0))
|
||||
params.put_bool('AlwaysOnLateral', True, block=True)
|
||||
params.put('PauseAOLOnBrake', 10.0, block=True)
|
||||
params.put('DistanceButtonControl', 3, block=True)
|
||||
saved = read_settings(params)
|
||||
self.assertTrue(saved.enabled)
|
||||
self.assertEqual(saved.pause_brake_mps, 10.0)
|
||||
self.assertEqual(saved.distance_actions, (3, 0, 0))
|
||||
|
||||
def test_brake_pause_units_preserve_legacy_until_explicit_si_override(self):
|
||||
with OpenpilotPrefix():
|
||||
params = Params()
|
||||
params.put_bool('AlwaysOnLateral', True, block=True)
|
||||
params.put('PauseAOLOnBrake', 10.0, block=True)
|
||||
for metric in (False, True):
|
||||
params.put_bool('IsMetric', metric, block=True)
|
||||
self.assertEqual(read_settings(params).pause_brake_mps, 10.0)
|
||||
params.put('AolBrakePauseSpeedMps', 4.4704, block=True)
|
||||
self.assertAlmostEqual(read_settings(params).pause_brake_mps, 4.4704)
|
||||
self.assertTrue(read_settings(params).enabled)
|
||||
params.put('AolBrakePauseSpeedMps', 0.0, block=True)
|
||||
self.assertEqual(read_settings(params).pause_brake_mps, 0.0)
|
||||
params.put('AolBrakePauseSpeedMps', -1.0, block=True)
|
||||
self.assertFalse(read_settings(params).enabled)
|
||||
self.assertTrue(params.get_bool('AlwaysOnLateral'))
|
||||
params.remove('AolBrakePauseSpeedMps')
|
||||
self.assertEqual(read_settings(params).pause_brake_mps, 10.0)
|
||||
|
||||
def test_invalid_threshold_never_falls_back_to_permissive_value(self):
|
||||
with OpenpilotPrefix():
|
||||
params = Params()
|
||||
params.put_bool('AlwaysOnLateral', True, block=True)
|
||||
params.put('PauseAOLOnBrake', 10.0, block=True)
|
||||
for key in ('AolBrakePauseSpeedMps', 'PauseAOLOnBrake'):
|
||||
path = Path(params.get_param_path(key))
|
||||
for value in (b'', b'bad', b'\xff', b'nan', b'inf', b'-1', b'100.1'):
|
||||
with self.subTest(key=key, value=value):
|
||||
path.write_bytes(value)
|
||||
self.assertFalse(read_settings(params).enabled)
|
||||
self.assertTrue(params.get_bool('AlwaysOnLateral'))
|
||||
path.unlink()
|
||||
self.assertTrue(read_settings(params).enabled)
|
||||
self.assertEqual(read_settings(params).pause_brake_mps, 0.0)
|
||||
|
||||
def test_saved_threshold_controls_actual_brake_boundary(self):
|
||||
with OpenpilotPrefix():
|
||||
params = Params()
|
||||
params.put_bool('AlwaysOnLateral', True, block=True)
|
||||
params.put('PauseAOLOnBrake', 10.0, block=True)
|
||||
state = car_state(brake=True)
|
||||
native = SimpleNamespace(requestedLateral=True, requestedLongitudinal=False,
|
||||
lateralAllowed=True, longitudinalAllowed=False)
|
||||
intent = SimpleNamespace(allowedLatch=True, lateralArmed=True, pauseLateral=False, pauseLongitudinal=False)
|
||||
for metric in (False, True):
|
||||
params.put_bool('IsMetric', metric, block=True)
|
||||
for speed, mode in ((9.9, 'off'), (10.0, 'lateralOnly')):
|
||||
state.vEgo = speed
|
||||
result = decide_axes(standard_lateral=False, standard_longitudinal=False, intent=intent,
|
||||
native=native, car_state=state, initialized=True, model_ready=True,
|
||||
no_entry=False, immediate_disable=False, dm_lockout=False,
|
||||
pause_brake_mps=read_settings(params).pause_brake_mps)
|
||||
self.assertEqual(result.mode, mode)
|
||||
|
||||
def test_honda_main_lkas_pause_and_cancel_ownership(self):
|
||||
settings = AolSettings(True, 5.0, 9, 0, (3, 4, 0), (3, 0, 0))
|
||||
owner = AolCardIntent(settings)
|
||||
state = car_state()
|
||||
state.cruiseState.available = True
|
||||
owner.update(state)
|
||||
self.assertFalse(owner.output(state)[0]) # LKAS button owns the latch
|
||||
|
||||
state.buttonEvents = [car.CarState.ButtonEvent(type=car.CarState.ButtonEvent.Type.lkas, pressed=True)]
|
||||
owner.update(state)
|
||||
self.assertTrue(owner.output(state)[0])
|
||||
|
||||
state.buttonEvents = [car.CarState.ButtonEvent(type=car.CarState.ButtonEvent.Type.cancel, pressed=True)]
|
||||
owner.update(state)
|
||||
self.assertFalse(owner.pause_lateral) # Honda cancel never doubles as pause
|
||||
|
||||
state.buttonEvents = [car.CarState.ButtonEvent(type=car.CarState.ButtonEvent.Type.gapAdjustCruise, pressed=True)]
|
||||
owner.update(state)
|
||||
state.buttonEvents = [car.CarState.ButtonEvent(type=car.CarState.ButtonEvent.Type.gapAdjustCruise, pressed=False)]
|
||||
owner.update(state)
|
||||
self.assertTrue(owner.pause_lateral)
|
||||
self.assertEqual(owner.output(state), (True, True, False))
|
||||
|
||||
def test_invalid_can_and_settings_disable_intent(self):
|
||||
owner = AolCardIntent(AolSettings(True, 0, 0, 0, (0, 0, 0), (0, 0, 0)))
|
||||
state = car_state()
|
||||
state.cruiseState.available = True
|
||||
owner.update(state)
|
||||
self.assertTrue(owner.output(state)[0])
|
||||
state.canValid = False
|
||||
owner.update(state)
|
||||
self.assertFalse(owner.output(state)[0])
|
||||
|
||||
def test_interrupted_distance_hold_does_not_replay_and_keeps_deliberate_pause(self):
|
||||
settings = AolSettings(True, 0, 0, 0, (3, 4, 0), (0, 0, 0))
|
||||
owner = AolCardIntent(settings)
|
||||
state = car_state()
|
||||
state.cruiseState.available = True
|
||||
button = car.CarState.ButtonEvent.Type.gapAdjustCruise
|
||||
owner.update(state)
|
||||
state.buttonEvents = [car.CarState.ButtonEvent(type=button, pressed=True)]
|
||||
owner.update(state)
|
||||
state.buttonEvents = []
|
||||
for _ in range(5):
|
||||
owner.update(state)
|
||||
state.canValid = False
|
||||
owner.update(state)
|
||||
self.assertFalse(owner._held[int(button)])
|
||||
self.assertEqual(owner._timers[int(button)], 0)
|
||||
state.canValid = True
|
||||
state.buttonEvents = [car.CarState.ButtonEvent(type=button, pressed=False)]
|
||||
owner.update(state)
|
||||
self.assertFalse(owner.pause_lateral or owner.pause_longitudinal)
|
||||
|
||||
# A complete user pause remains latched through CAN loss.
|
||||
state.buttonEvents = [car.CarState.ButtonEvent(type=button, pressed=True)]
|
||||
owner.update(state)
|
||||
state.buttonEvents = [car.CarState.ButtonEvent(type=button, pressed=False)]
|
||||
owner.update(state)
|
||||
self.assertTrue(owner.pause_lateral)
|
||||
state.canValid = False
|
||||
owner.update(state)
|
||||
self.assertTrue(owner.pause_lateral)
|
||||
|
||||
# If another owner consumed a release, it cannot complete the held gesture.
|
||||
state.canValid = True
|
||||
state.buttonEvents = [car.CarState.ButtonEvent(type=button, pressed=True)]
|
||||
owner.update(state)
|
||||
state.buttonEvents = [car.CarState.ButtonEvent(type=button, pressed=False)]
|
||||
owner.update(state, consumed_buttons=frozenset((int(button),)))
|
||||
self.assertFalse(owner._held[int(button)])
|
||||
self.assertTrue(owner.pause_lateral)
|
||||
|
||||
# CAN loss just before the long-press boundary cannot finish that gesture.
|
||||
state.buttonEvents = [car.CarState.ButtonEvent(type=button, pressed=True)]
|
||||
owner.update(state)
|
||||
state.buttonEvents = []
|
||||
for _ in range(CRUISE_LONG_PRESS - 2):
|
||||
owner.update(state)
|
||||
self.assertFalse(owner.pause_longitudinal)
|
||||
state.canValid = False
|
||||
owner.update(state)
|
||||
state.canValid = True
|
||||
for _ in range(3):
|
||||
owner.update(state)
|
||||
self.assertFalse(owner.pause_longitudinal)
|
||||
self.assertTrue(owner.pause_lateral)
|
||||
|
||||
# An owner can reserve the button on a tick with no new button event.
|
||||
state.buttonEvents = [car.CarState.ButtonEvent(type=button, pressed=True)]
|
||||
owner.update(state)
|
||||
state.buttonEvents = []
|
||||
for _ in range(CRUISE_LONG_PRESS - 2):
|
||||
owner.update(state)
|
||||
owner.update(state, consumed_buttons=frozenset((int(button),)))
|
||||
state.buttonEvents = [car.CarState.ButtonEvent(type=button, pressed=False)]
|
||||
owner.update(state)
|
||||
self.assertFalse(owner.pause_longitudinal)
|
||||
self.assertTrue(owner.pause_lateral)
|
||||
|
||||
def test_held_at_start_distance_waits_for_neutral_before_all_press_lengths(self):
|
||||
button = car.CarState.ButtonEvent.Type.gapAdjustCruise
|
||||
for held_ticks, actions in ((1, (3, 0, 0)), (CRUISE_LONG_PRESS, (0, 4, 0)),
|
||||
(CRUISE_LONG_PRESS * 5, (0, 0, 3))):
|
||||
with self.subTest(held_ticks=held_ticks):
|
||||
settings = AolSettings(True, 0, 0, 0, actions, (0, 0, 0))
|
||||
owner = AolCardIntent(settings, explicit_latch=True)
|
||||
state = car_state()
|
||||
state.buttonEvents = [car.CarState.ButtonEvent(type=button, pressed=True)]
|
||||
owner.update(state)
|
||||
state.buttonEvents = []
|
||||
for _ in range(held_ticks):
|
||||
owner.update(state)
|
||||
state.buttonEvents = [car.CarState.ButtonEvent(type=button, pressed=False)]
|
||||
owner.update(state)
|
||||
self.assertFalse(owner.pause_lateral or owner.pause_longitudinal)
|
||||
state.buttonEvents = [car.CarState.ButtonEvent(type=button, pressed=True)]
|
||||
owner.update(state)
|
||||
state.buttonEvents = []
|
||||
for _ in range(held_ticks - 1):
|
||||
owner.update(state)
|
||||
state.buttonEvents = [car.CarState.ButtonEvent(type=button, pressed=False)]
|
||||
owner.update(state)
|
||||
self.assertEqual((owner.pause_lateral, owner.pause_longitudinal),
|
||||
(held_ticks != CRUISE_LONG_PRESS, held_ticks == CRUISE_LONG_PRESS))
|
||||
|
||||
|
||||
|
||||
|
||||
class IpcAxisContractTests(unittest.TestCase):
|
||||
def test_car_state_timeout_retains_only_original_fresh_valid_sample(self):
|
||||
sd = SelfdriveD.__new__(SelfdriveD)
|
||||
sd.car_state_sock = object()
|
||||
sd.CS_prev = car_state()
|
||||
sd.aol_car_state_log_ns = 0
|
||||
sd.conditional_car_state_valid = False
|
||||
sd.initialized = True
|
||||
sd.enabled = False
|
||||
self.enterContext(mock.patch.object(sd, 'sm', SimpleNamespace(update=mock.Mock()), create=True))
|
||||
stamp = 1_000_000_000
|
||||
message = messaging.new_message('carState')
|
||||
message.logMonoTime = stamp
|
||||
message.valid = True
|
||||
message.carState = sd.CS_prev
|
||||
with (mock.patch('openpilot.selfdrive.selfdrived.selfdrived.messaging.recv_one', return_value=message) as recv,
|
||||
mock.patch('openpilot.selfdrive.selfdrived.selfdrived.time.monotonic_ns', return_value=stamp) as now):
|
||||
sd.CS_prev = sd.data_sample()
|
||||
self.assertEqual(sd.aol_car_state_log_ns, stamp)
|
||||
recv.return_value = None # 20 ms socket timeout, still inside the existing 30 ms lease.
|
||||
now.return_value = stamp + 23_000_000
|
||||
self.assertIs(sd.data_sample(), sd.CS_prev)
|
||||
self.assertEqual(sd.aol_car_state_log_ns, stamp)
|
||||
self.assertTrue(sd.conditional_car_state_valid)
|
||||
now.return_value = stamp + 30_000_001
|
||||
sd.data_sample()
|
||||
self.assertEqual(sd.aol_car_state_log_ns, 0)
|
||||
self.assertFalse(sd.conditional_car_state_valid)
|
||||
|
||||
# A next-drive/new process has no retained source, even if CS_prev exists.
|
||||
sd.data_sample()
|
||||
self.assertEqual(sd.aol_car_state_log_ns, 0)
|
||||
message.logMonoTime = stamp + 24_000_000
|
||||
recv.return_value = message
|
||||
sd.CS_prev = sd.data_sample()
|
||||
self.assertEqual(sd.aol_car_state_log_ns, stamp + 24_000_000)
|
||||
for invalid_kind in ('valid', 'canValid', 'canTimeout', 'clock'):
|
||||
with self.subTest(invalid_kind=invalid_kind):
|
||||
message.valid = invalid_kind != 'valid'
|
||||
message.carState.canValid = invalid_kind != 'canValid'
|
||||
message.carState.canTimeout = invalid_kind == 'canTimeout'
|
||||
recv.return_value = message
|
||||
sd.CS_prev = sd.data_sample()
|
||||
recv.return_value = None
|
||||
now.return_value = message.logMonoTime + (-1 if invalid_kind == 'clock' else 20_000_000)
|
||||
sd.data_sample()
|
||||
self.assertEqual(sd.aol_car_state_log_ns, 0)
|
||||
self.assertFalse(sd.conditional_car_state_valid)
|
||||
|
||||
def test_lateral_only_receipt_loss_keeps_take_control_alert(self):
|
||||
class SM:
|
||||
frame = 0
|
||||
|
||||
def __getitem__(self, service):
|
||||
if service == 'driverMonitoringState':
|
||||
return SimpleNamespace(alertLevel=0, lockout=False, alwaysOnLockout=False)
|
||||
if service == 'extrinsicsCalibration':
|
||||
return SimpleNamespace(calStatus=log.ExtrinsicsCalibration.Status.calibrated)
|
||||
raise KeyError(service)
|
||||
|
||||
def all_checks(self, _services):
|
||||
return True
|
||||
|
||||
sd = SelfdriveD.__new__(SelfdriveD)
|
||||
sd.CP = SimpleNamespace(passive=False, openpilotLongitudinalControl=True)
|
||||
self.enterContext(mock.patch.object(sd, 'sm', SM(), create=True))
|
||||
sd.events = Events()
|
||||
sd.state_machine = StateMachine()
|
||||
sd.AM = AlertManager()
|
||||
sd.personality = 1
|
||||
sd.is_metric = False
|
||||
sd.enabled = sd.active = False
|
||||
sd.initialized = True
|
||||
sd.aol_replay = True
|
||||
sd.aol_car_state_log_ns = 0
|
||||
sd.aol_session_id = 'drive-session'
|
||||
sd.aol_axis_decision = AxisDecision()
|
||||
sd.aol_dm_lateral_inhibit = False
|
||||
sd.aol_settings = None
|
||||
sd.nostalgia_paddle_cancel = False
|
||||
self.enterContext(mock.patch.object(sd, 'data_sample', car_state))
|
||||
self.enterContext(mock.patch.object(sd, 'update_events', lambda _cs: sd.events.clear()))
|
||||
sd.update_conditional_mode = mock.Mock()
|
||||
sd.publish_selfdriveState = mock.Mock()
|
||||
|
||||
native = SimpleNamespace(requestedLateral=True, requestedLongitudinal=False,
|
||||
lateralAllowed=True, longitudinalAllowed=False)
|
||||
intent = SimpleNamespace(allowedLatch=True, lateralArmed=True, pauseLateral=False, pauseLongitudinal=False)
|
||||
with mock.patch('openpilot.selfdrive.selfdrived.selfdrived.current_intent', return_value=intent), \
|
||||
mock.patch('openpilot.selfdrive.selfdrived.selfdrived.current_native', side_effect=(None, native, None, None)):
|
||||
sd.sm.frame = 1
|
||||
sd.step()
|
||||
self.assertEqual(sd.aol_axis_decision.mode, 'off')
|
||||
self.assertFalse(sd.enabled)
|
||||
self.assertNotIn(ET.IMMEDIATE_DISABLE, sd.state_machine.current_alert_types)
|
||||
self.assertNotEqual(sd.AM.current_alert.alert_text_1, 'TAKE CONTROL IMMEDIATELY')
|
||||
|
||||
sd.sm.frame = 2
|
||||
sd.step()
|
||||
self.assertEqual(sd.aol_axis_decision.mode, 'lateralOnly')
|
||||
self.assertFalse(sd.enabled)
|
||||
|
||||
sd.sm.frame = 3
|
||||
sd.step()
|
||||
self.assertEqual(sd.aol_axis_decision.mode, 'off')
|
||||
self.assertFalse(sd.enabled)
|
||||
self.assertIn(ET.IMMEDIATE_DISABLE, sd.state_machine.current_alert_types)
|
||||
self.assertEqual(sd.AM.current_alert.alert_text_1, 'TAKE CONTROL IMMEDIATELY')
|
||||
self.assertEqual(sd.AM.current_alert.alert_text_2, 'Controls Mismatch')
|
||||
alert_end_frame = sd.AM.alerts['controlsMismatch/immediateDisable'].end_frame
|
||||
self.assertGreater(alert_end_frame, sd.sm.frame)
|
||||
|
||||
sd.sm.frame = 4
|
||||
sd.step()
|
||||
self.assertNotIn(ET.IMMEDIATE_DISABLE, sd.state_machine.current_alert_types)
|
||||
self.assertEqual(sd.AM.current_alert.alert_text_1, 'TAKE CONTROL IMMEDIATELY')
|
||||
self.assertEqual(sd.AM.alerts['controlsMismatch/immediateDisable'].end_frame, alert_end_frame)
|
||||
|
||||
def test_lateral_only_temporary_fault_uses_existing_orange_warning(self):
|
||||
class SM:
|
||||
frame = 1
|
||||
|
||||
def __getitem__(self, service):
|
||||
if service == 'driverMonitoringState':
|
||||
return SimpleNamespace(alertLevel=0, lockout=False, alwaysOnLockout=False)
|
||||
if service == 'extrinsicsCalibration':
|
||||
return SimpleNamespace(calStatus=log.ExtrinsicsCalibration.Status.calibrated)
|
||||
raise KeyError(service)
|
||||
|
||||
def all_checks(self, _services):
|
||||
return True
|
||||
|
||||
sd = SelfdriveD.__new__(SelfdriveD)
|
||||
sd.CP = SimpleNamespace(passive=False, openpilotLongitudinalControl=True)
|
||||
self.enterContext(mock.patch.object(sd, 'sm', SM(), create=True))
|
||||
sd.events, sd.state_machine, sd.AM = Events(), StateMachine(), AlertManager()
|
||||
sd.personality, sd.is_metric = 1, False
|
||||
sd.enabled = sd.active = False
|
||||
sd.initialized = sd.aol_replay = True
|
||||
sd.aol_car_state_log_ns = 0
|
||||
sd.aol_session_id = 'drive-session'
|
||||
sd.aol_axis_decision = AxisDecision()
|
||||
sd.aol_dm_lateral_inhibit = False
|
||||
sd.aol_settings = None
|
||||
sd.nostalgia_paddle_cancel = False
|
||||
state = car_state()
|
||||
state.steerFaultTemporary = True
|
||||
self.enterContext(mock.patch.object(sd, 'data_sample', return_value=state))
|
||||
self.enterContext(mock.patch.object(sd, 'update_events', lambda _cs: sd.events.clear()))
|
||||
sd.update_conditional_mode = mock.Mock()
|
||||
sd.publish_selfdriveState = mock.Mock()
|
||||
native = SimpleNamespace(requestedLateral=False, requestedLongitudinal=False,
|
||||
lateralAllowed=False, longitudinalAllowed=False)
|
||||
intent = SimpleNamespace(allowedLatch=True, lateralArmed=True, pauseLateral=False, pauseLongitudinal=False)
|
||||
with (mock.patch('openpilot.selfdrive.selfdrived.selfdrived.current_intent', return_value=intent),
|
||||
mock.patch('openpilot.selfdrive.selfdrived.selfdrived.current_native', return_value=native)):
|
||||
sd.step()
|
||||
self.assertFalse(sd.enabled)
|
||||
self.assertFalse(sd.aol_axis_decision.desired_lateral)
|
||||
self.assertFalse(sd.aol_axis_decision.lateral_active)
|
||||
self.assertEqual(sd.AM.current_alert.alert_type, 'steerTempUnavailableSilent/warning')
|
||||
self.assertEqual(sd.AM.current_alert.alert_status, log.SelfdriveState.AlertStatus.userPrompt)
|
||||
self.assertEqual(sd.AM.current_alert.alert_text_1, 'Steering Assist Temporarily Unavailable')
|
||||
state.steerFaultTemporary = False
|
||||
native.requestedLateral = native.lateralAllowed = True
|
||||
sd.sm.frame += 1
|
||||
sd.step()
|
||||
self.assertTrue(sd.aol_axis_decision.lateral_active)
|
||||
self.assertNotIn(log.OnroadEvent.EventName.steerTempUnavailableSilent, sd.events.names)
|
||||
|
||||
def test_native_receipt_gates_engagement_and_disables_on_loss(self):
|
||||
class SM:
|
||||
def __getitem__(self, service):
|
||||
if service == 'driverMonitoringState':
|
||||
return SimpleNamespace(alertLevel=0, lockout=False, alwaysOnLockout=False)
|
||||
if service == 'extrinsicsCalibration':
|
||||
return SimpleNamespace(calStatus=log.ExtrinsicsCalibration.Status.calibrated)
|
||||
raise KeyError(service)
|
||||
|
||||
def all_checks(self, _services):
|
||||
return True
|
||||
|
||||
def driver(aol_enabled):
|
||||
sd = SelfdriveD.__new__(SelfdriveD)
|
||||
sd.CP = SimpleNamespace(passive=False, openpilotLongitudinalControl=True)
|
||||
self.enterContext(mock.patch.object(sd, 'sm', SM(), create=True))
|
||||
sd.events = Events()
|
||||
sd.state_machine = StateMachine()
|
||||
sd.enabled = sd.active = False
|
||||
sd.initialized = True
|
||||
sd.aol_replay = aol_enabled
|
||||
sd.aol_car_state_log_ns = 0
|
||||
sd.aol_session_id = 'drive-session'
|
||||
sd.aol_axis_decision = AxisDecision()
|
||||
sd.aol_dm_lateral_inhibit = False
|
||||
sd.aol_settings = None
|
||||
sd.nostalgia_paddle_cancel = False
|
||||
self.enterContext(mock.patch.object(sd, 'data_sample', car_state))
|
||||
sd.update_alerts = mock.Mock()
|
||||
sd.update_conditional_mode = mock.Mock()
|
||||
sd.publish_selfdriveState = mock.Mock()
|
||||
self.enterContext(mock.patch.object(sd, 'update_events',
|
||||
lambda _cs: (sd.events.clear(), sd.events.add(log.OnroadEvent.EventName.buttonEnable))))
|
||||
return sd
|
||||
|
||||
native = SimpleNamespace(requestedLateral=True, requestedLongitudinal=True,
|
||||
lateralAllowed=True, longitudinalAllowed=True)
|
||||
intent = SimpleNamespace(allowedLatch=True, lateralArmed=True, pauseLateral=False, pauseLongitudinal=False)
|
||||
sd = driver(True)
|
||||
with mock.patch('openpilot.selfdrive.selfdrived.selfdrived.current_intent', return_value=intent), \
|
||||
mock.patch('openpilot.selfdrive.selfdrived.selfdrived.current_native', side_effect=(None, native, None)):
|
||||
sd.step()
|
||||
self.assertFalse(sd.enabled)
|
||||
self.assertTrue(sd.events.contains(ET.NO_ENTRY))
|
||||
self.assertEqual(sd.aol_axis_decision.mode, 'off')
|
||||
sd.publish_selfdriveState.assert_called_once() # disabled bootstrap still publishes the zero-axis request
|
||||
sd.step()
|
||||
self.assertTrue(sd.enabled and sd.active)
|
||||
self.assertEqual(sd.aol_axis_decision.mode, 'combined')
|
||||
sd.step()
|
||||
self.assertFalse(sd.enabled or sd.active)
|
||||
self.assertTrue(sd.events.contains(ET.IMMEDIATE_DISABLE))
|
||||
alerts = sd.events.create_alerts(sd.state_machine.current_alert_types)
|
||||
self.assertTrue(any(alert.alert_text_1 == 'TAKE CONTROL IMMEDIATELY' and
|
||||
alert.alert_text_2 == 'Controls Mismatch' for alert in alerts))
|
||||
|
||||
stock = driver(False)
|
||||
with mock.patch('openpilot.selfdrive.selfdrived.selfdrived.current_native') as read_native:
|
||||
stock.step()
|
||||
read_native.assert_not_called()
|
||||
self.assertTrue(stock.enabled and stock.active)
|
||||
|
||||
def test_independent_ack_keeps_unchanged_axis(self):
|
||||
state = car_state()
|
||||
old_combined = SimpleNamespace(requestedLateral=True, requestedLongitudinal=True,
|
||||
lateralAllowed=True, longitudinalAllowed=True)
|
||||
intent = SimpleNamespace(allowedLatch=True, lateralArmed=True, pauseLateral=False, pauseLongitudinal=False)
|
||||
def decide(standard_long, native):
|
||||
return decide_axes(standard_lateral=False, standard_longitudinal=standard_long,
|
||||
intent=intent, native=native, car_state=state, initialized=True,
|
||||
model_ready=True, no_entry=False, immediate_disable=False,
|
||||
dm_lockout=False, pause_brake_mps=5.0)
|
||||
self.assertEqual(decide(True, old_combined).mode, 'combined')
|
||||
self.assertEqual(decide(False, old_combined).mode, 'lateralOnly')
|
||||
old_lateral = SimpleNamespace(requestedLateral=True, requestedLongitudinal=False,
|
||||
lateralAllowed=True, longitudinalAllowed=False)
|
||||
self.assertEqual(decide(True, old_lateral).mode, 'lateralOnly')
|
||||
intent.pauseLateral = True
|
||||
self.assertEqual(decide(True, old_combined).mode, 'longitudinalOnly')
|
||||
intent.pauseLateral = False
|
||||
intent.pauseLongitudinal = True
|
||||
self.assertEqual(decide(True, old_combined).mode, 'lateralOnly')
|
||||
intent.pauseLongitudinal = False
|
||||
old_long = SimpleNamespace(requestedLateral=False, requestedLongitudinal=True,
|
||||
lateralAllowed=False, longitudinalAllowed=True)
|
||||
self.assertEqual(decide(True, old_long).mode, 'longitudinalOnly')
|
||||
|
||||
|
||||
def test_card_selfdrive_panda_cadence_and_four_modes(self):
|
||||
with OpenpilotPrefix():
|
||||
messaging.reset_context()
|
||||
pm = messaging.PubMaster(['carState', 'aolIntentWire', 'aolAxisState', 'aolSafetyWire'])
|
||||
sm = messaging.SubMaster(['carState', 'aolIntentWire', 'aolAxisState', 'aolSafetyWire'])
|
||||
base = time.monotonic_ns() - 50_000_000
|
||||
CP = car.CarParams()
|
||||
CP.safetyConfigs = [car.CarParams.SafetyConfig(safetyModel=car.CarParams.SafetyModel.hondaBosch, safetyParam=34)]
|
||||
|
||||
def publish_intent(stamp, *, pause_lat=False, pause_long=False):
|
||||
message = messaging.new_message('aolIntentWire', 0)
|
||||
message.logMonoTime = stamp
|
||||
message.valid = True
|
||||
message.aolIntentWire = encode_intent(IntentState(
|
||||
'card-session', stamp - base + 1, stamp, stamp, stamp + 100_000_000,
|
||||
True, pause_lat, pause_long, True))
|
||||
pm.send('aolIntentWire', message)
|
||||
|
||||
def publish_native(stamp, lat, long, session='drive-session'):
|
||||
message = messaging.new_message('aolSafetyWire', 0)
|
||||
message.logMonoTime = stamp
|
||||
message.valid = True
|
||||
message.aolSafetyWire = encode_safety(SafetyState(
|
||||
1, True, stamp, stamp + 200_000_000, int(car.CarParams.SafetyModel.hondaBosch),
|
||||
34, lat, long, lat, long, 'panda', session))
|
||||
pm.send('aolSafetyWire', message)
|
||||
|
||||
for index, (standard_long, pause_lat, pause_long, expected) in enumerate((
|
||||
(False, False, False, 'lateralOnly'),
|
||||
(True, False, False, 'combined'),
|
||||
(True, True, False, 'longitudinalOnly'),
|
||||
(False, True, False, 'off'),
|
||||
)):
|
||||
stamp = base + index * 10_000_000
|
||||
state_msg = messaging.new_message('carState')
|
||||
state_msg.logMonoTime = stamp
|
||||
state_msg.valid = True
|
||||
pm.send('carState', state_msg)
|
||||
publish_intent(stamp, pause_lat=pause_lat, pause_long=pause_long)
|
||||
desired_lat = not pause_lat
|
||||
desired_long = standard_long and not pause_long
|
||||
publish_native(stamp, desired_lat, desired_long)
|
||||
sm.update(100)
|
||||
intent = current_intent(sm, car_state_ns=stamp, now_ns=stamp + 1_000_000)
|
||||
native = current_native(sm, CP, now_ns=stamp + 1_000_000, axis_session_id='drive-session')
|
||||
decision = decide_axes(standard_lateral=False, standard_longitudinal=standard_long,
|
||||
intent=intent, native=native, car_state=car_state(),
|
||||
initialized=True, model_ready=True, no_entry=False,
|
||||
immediate_disable=False, dm_lockout=False, pause_brake_mps=5.0)
|
||||
self.assertEqual(decision.mode, expected)
|
||||
|
||||
self.assertIsNone(current_native(sm, CP, now_ns=base + 500_000_000, axis_session_id='drive-session'))
|
||||
self.assertIsNone(current_native(sm, CP, now_ns=base + 31_000_000, axis_session_id='new-session'))
|
||||
|
||||
def test_no_native_ack_still_requests_but_does_not_actuate(self):
|
||||
intent = SimpleNamespace(pauseLateral=False, pauseLongitudinal=False, allowedLatch=True)
|
||||
decision = decide_axes(standard_lateral=False, standard_longitudinal=True,
|
||||
intent=intent, native=None, car_state=car_state(), initialized=True,
|
||||
model_ready=True, no_entry=False, immediate_disable=False,
|
||||
dm_lockout=False, pause_brake_mps=5.0)
|
||||
self.assertTrue(decision.desired_lateral)
|
||||
self.assertTrue(decision.desired_longitudinal)
|
||||
self.assertEqual(decision.mode, 'off')
|
||||
self.assertFalse(decision.native_acknowledged)
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
unittest.main()
|
||||
@@ -0,0 +1,71 @@
|
||||
"""Joined adapter-factory regression for stock CAN-FD and classic lateral policies."""
|
||||
import unittest
|
||||
|
||||
from opendbc.car.structs import car
|
||||
from opendbc.car.hyundai.values import CAR, HyundaiFlags
|
||||
from openpilot.starpilot.aol.intent import AolSettings
|
||||
from openpilot.starpilot.aol.vehicle import create_intent, native_matches_cp, policy_for
|
||||
from openpilot.starpilot.car.hyundai.forte_intent import ForteCardIntent
|
||||
from openpilot.starpilot.aol.tests.test_forte_intent import params as forte_params, state as forte_state
|
||||
|
||||
Button = car.CarState.ButtonEvent.Type
|
||||
|
||||
|
||||
def stock(alternate):
|
||||
cp = car.CarParams.new_message()
|
||||
cp.brand, cp.carFingerprint = 'hyundai', CAR.HYUNDAI_IONIQ_6
|
||||
cp.flags = int(HyundaiFlags.CANFD | HyundaiFlags.EV | HyundaiFlags.CANFD_LKA_STEER_MSG)
|
||||
if alternate:
|
||||
cp.flags |= int(HyundaiFlags.CANFD_LKA_STEER_MSG_ALT)
|
||||
cp.pcmCruise, cp.openpilotLongitudinalControl, cp.radarUnavailable = True, False, False
|
||||
cp.safetyConfigs = [{'safetyModel': car.CarParams.SafetyModel.hyundaiCanfd,
|
||||
'safetyParam': 0x91 if alternate else 0x11}]
|
||||
return cp
|
||||
|
||||
|
||||
def settings(lkas=9, main=0):
|
||||
return AolSettings(True, 0., lkas, main, (0, 0, 0), (0, 0, 0))
|
||||
|
||||
|
||||
class TestStockForteFactory(unittest.TestCase):
|
||||
def test_actual_dispatcher_stock_factory_retains_latch_before_marker(self):
|
||||
for alternate in (False, True):
|
||||
with self.subTest(alternate=alternate):
|
||||
cp = stock(alternate)
|
||||
policy = policy_for(cp)
|
||||
intent = create_intent(cp, settings(), policy)
|
||||
self.assertTrue(policy.intent_supported and policy.explicit_latch)
|
||||
self.assertTrue(intent.explicit_latch)
|
||||
self.assertFalse(intent.allowed_latch)
|
||||
model, original = int(cp.safetyConfigs[0].safetyModel.raw), int(cp.safetyConfigs[0].safetyParam)
|
||||
self.assertFalse(native_matches_cp(cp, model, original))
|
||||
cp.safetyConfigs[0].safetyParam |= policy.safety_param_addition
|
||||
serialized = cp.as_reader().as_builder()
|
||||
self.assertEqual(serialized.safetyConfigs[0].safetyParam, original | 0x0800)
|
||||
self.assertTrue(native_matches_cp(serialized, model, original | 0x0800))
|
||||
self.assertFalse(native_matches_cp(serialized, model, original))
|
||||
self.assertTrue(serialized.pcmCruise)
|
||||
self.assertFalse(serialized.openpilotLongitudinalControl)
|
||||
disabled = create_intent(stock(alternate), AolSettings(False, 0., 9, 0, (0, 0, 0), (0, 0, 0)), policy)
|
||||
self.assertTrue(disabled.explicit_latch)
|
||||
self.assertFalse(disabled.allowed_latch)
|
||||
|
||||
def test_forte_dispatcher_retains_direct_button_state_machine(self):
|
||||
for platform in (CAR.KIA_FORTE_2019_NON_SCC, CAR.KIA_FORTE_2021_NON_SCC):
|
||||
for source in (0, 0x391, 0x50c):
|
||||
for main_action in (0, 9):
|
||||
with self.subTest(platform=platform, source=source, main_action=main_action):
|
||||
cp = forte_params(platform, source)
|
||||
configured = settings(main=main_action)
|
||||
dispatched = create_intent(cp, configured, policy_for(cp))
|
||||
direct = ForteCardIntent(cp, configured)
|
||||
self.assertIs(type(dispatched), ForteCardIntent)
|
||||
for tick, events in enumerate(((), ((Button.lkas, False),), ((Button.lkas, True),),
|
||||
((Button.lkas, False),), ((Button.lkas, True),),
|
||||
((Button.lkas, False),)), 1):
|
||||
cs = forte_state(main=True, cruise=tick > 1, events=events)
|
||||
for intent in (dispatched, direct):
|
||||
intent.update(cs, now_ns=tick)
|
||||
for attribute in ('allowed_latch', 'pause_lateral', 'pause_longitudinal',
|
||||
'physical_latch', 'fault_rearm', 'neutral_seen'):
|
||||
self.assertEqual(getattr(dispatched, attribute), getattr(direct, attribute), attribute)
|
||||
@@ -0,0 +1,20 @@
|
||||
"""Unknown vehicles have no implicit AOL capability or native receipt."""
|
||||
|
||||
import unittest
|
||||
from types import SimpleNamespace
|
||||
|
||||
from openpilot.starpilot.aol.vehicle import AolVehiclePolicy, native_matches_cp, native_profile_supported, policy_for
|
||||
|
||||
|
||||
class GenericVehiclePolicyTests(unittest.TestCase):
|
||||
def test_unknown_and_malformed_cp_fail_closed(self):
|
||||
denied = AolVehiclePolicy()
|
||||
for cp in (None, SimpleNamespace(), SimpleNamespace(brand='ford'),
|
||||
SimpleNamespace(brand='honda'), SimpleNamespace(brand='hyundai')):
|
||||
with self.subTest(cp=cp):
|
||||
self.assertEqual(policy_for(cp), denied)
|
||||
self.assertFalse(native_matches_cp(cp, 0, 0))
|
||||
|
||||
def test_no_unregistered_native_profile(self):
|
||||
self.assertFalse(native_profile_supported(0, 0))
|
||||
self.assertFalse(native_profile_supported(999, 0x8815))
|
||||
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,96 @@
|
||||
"""Fail-closed dispatch to vehicle-owned AOL capabilities."""
|
||||
|
||||
from openpilot.starpilot.aol.policy import AolVehiclePolicy
|
||||
from openpilot.starpilot.aol.intent import AolCardIntent
|
||||
from openpilot.starpilot.car.honda import aol as honda
|
||||
from openpilot.starpilot.car.hyundai import aol as hyundai
|
||||
from openpilot.starpilot.car.gm import aol as gm
|
||||
from openpilot.starpilot.car.ford import aol as ford
|
||||
from openpilot.starpilot.car.mazda import aol as mazda
|
||||
from openpilot.starpilot.car.tesla import aol as tesla
|
||||
from openpilot.starpilot.car.toyota import aol as toyota
|
||||
|
||||
_PORTS = {'honda': honda, 'hyundai': hyundai, 'gm': gm, 'ford': ford, 'mazda': mazda, 'tesla': tesla, 'toyota': toyota}
|
||||
|
||||
|
||||
def policy_for(CP) -> AolVehiclePolicy:
|
||||
try:
|
||||
port = _PORTS.get(CP.brand)
|
||||
return port.policy_for(CP) if port is not None else AolVehiclePolicy()
|
||||
except (AttributeError, TypeError, ValueError, OverflowError, IndexError):
|
||||
return AolVehiclePolicy()
|
||||
|
||||
|
||||
def native_profile_supported(model: int, param: int) -> bool:
|
||||
return any(port.native_profile_supported(model, param) for port in _PORTS.values())
|
||||
|
||||
|
||||
def native_matches_cp(CP, model: int, param: int) -> bool:
|
||||
try:
|
||||
if CP.brand == 'hyundai':
|
||||
config = hyundai.aol_safety_config(CP)
|
||||
return bool(config is not None and hyundai.native_accepts_cp(CP, model, param) and
|
||||
model == int(config.safetyModel.raw) and param == int(config.safetyParam))
|
||||
return bool(any(port.native_accepts_cp(CP, model, param) for port in _PORTS.values()) and
|
||||
model == int(CP.safetyConfigs[0].safetyModel.raw) and
|
||||
param == int(CP.safetyConfigs[-1].safetyParam))
|
||||
except (AttributeError, TypeError, ValueError, OverflowError, IndexError):
|
||||
return False
|
||||
|
||||
|
||||
def native_admission_key(CP):
|
||||
"""Observation reuse is scoped to the exact Ioniq single-Panda contract."""
|
||||
try:
|
||||
return hyundai.native_admission_key(CP) if CP.brand == 'hyundai' else None
|
||||
except (AttributeError, TypeError, ValueError, OverflowError, IndexError):
|
||||
return None
|
||||
|
||||
|
||||
def native_latch_rejected(CP, native, *, state=None) -> bool:
|
||||
if native is None:
|
||||
return False
|
||||
try:
|
||||
port = _PORTS.get(CP.brand)
|
||||
retain = getattr(port, 'retain_on_native_denial', None)
|
||||
rejected = getattr(port, 'native_latch_rejected', None)
|
||||
if retain is None and rejected is None:
|
||||
return False
|
||||
matched = native_matches_cp(CP, native.safetyModel, native.safetyParam)
|
||||
if retain is not None and matched and retain(CP, native, state):
|
||||
return False
|
||||
return bool(rejected is not None and matched and rejected(CP, native))
|
||||
except (AttributeError, TypeError, ValueError, OverflowError, IndexError):
|
||||
return False
|
||||
|
||||
|
||||
def create_intent(CP, settings, policy):
|
||||
port = _PORTS.get(CP.brand)
|
||||
factory = getattr(port, 'create_intent', None)
|
||||
if factory is not None:
|
||||
return factory(CP, settings)
|
||||
return AolCardIntent(settings, explicit_latch=policy.explicit_latch)
|
||||
|
||||
|
||||
def ordinary_axis_request_allowed(CP, CS) -> bool:
|
||||
"""Required ordinary transport has no permission without its port owner."""
|
||||
try:
|
||||
port = _PORTS.get(CP.brand)
|
||||
allowed = getattr(port, 'ordinary_axis_request_allowed', None)
|
||||
return bool(allowed is not None and allowed(CP, CS))
|
||||
except (AttributeError, TypeError, ValueError, OverflowError, IndexError):
|
||||
return False
|
||||
|
||||
|
||||
def allow_lateral_onset(CP, *, requested, normal_enabled, steering_pressed, previous_active):
|
||||
"""Optional vehicle-owned onset restriction; never creates permission."""
|
||||
port = _PORTS.get(CP.brand)
|
||||
owner = getattr(port, 'allow_lateral_onset', None)
|
||||
if owner is None:
|
||||
return bool(requested)
|
||||
return bool(requested and owner(CP, requested=requested, normal_enabled=normal_enabled,
|
||||
steering_pressed=steering_pressed, previous_active=previous_active))
|
||||
|
||||
|
||||
def configuration_settings_policy(CP):
|
||||
from openpilot.starpilot.car.hyundai.settings import configuration_wheel_policy
|
||||
return configuration_wheel_policy(CP) or toyota.configuration_settings_policy(CP) or gm.configuration_settings_policy(CP)
|
||||
@@ -1,15 +1,20 @@
|
||||
"""GM-owned AOL capabilities; exact CP admission lives with the vehicle."""
|
||||
|
||||
import math
|
||||
|
||||
from opendbc.car.structs import CarParams
|
||||
from opendbc.car.gm.values import CAR, control_flags, camera_acc_pedal_profile, is_volt_ascm_longitudinal
|
||||
from opendbc.car.gm.aol import GM_AOL_ALTERNATIVE_EXPERIENCE, GM_AOL_WORDS, qualified_gm
|
||||
from openpilot.starpilot.aol.policy import AolVehiclePolicy
|
||||
from openpilot.starpilot.aol.intent import AolCardIntent
|
||||
from openpilot.starpilot.aol.intent import AOL_TOGGLE, AolCardIntent
|
||||
|
||||
|
||||
def policy_for(cp) -> AolVehiclePolicy:
|
||||
qualified = qualified_gm(cp)
|
||||
return AolVehiclePolicy(intent_supported=qualified, settings_supported=qualified,
|
||||
runtime_supported=qualified, normal_runtime_supported=qualified,
|
||||
lkas_button_supported=False,
|
||||
full_axis_runtime_required=qualified and int(cp.alternativeExperience) == GM_AOL_ALTERNATIVE_EXPERIENCE,
|
||||
alternative_experience_addition=GM_AOL_ALTERNATIVE_EXPERIENCE if qualified else 0)
|
||||
|
||||
|
||||
@@ -25,10 +30,13 @@ def native_accepts_cp(cp, model: int, param: int) -> bool:
|
||||
class GmAolCardIntent(AolCardIntent):
|
||||
"""Main-following intent with explicit main-cycle recovery after fatal faults."""
|
||||
|
||||
def __init__(self, settings):
|
||||
super().__init__(settings)
|
||||
def __init__(self, settings, cp=None):
|
||||
super().__init__(settings, lkas_button_supported=False)
|
||||
self.cp = cp
|
||||
self.requires_fault_observation = True
|
||||
self._main_cycle_required = False
|
||||
self.startup_calibration_pending = True
|
||||
self._startup_incomplete = False
|
||||
|
||||
def update(self, CS, *, consumed_buttons=frozenset(), fault_active=None,
|
||||
now_ns=0, native_rejection_ns=0, standard_enabled=False):
|
||||
@@ -42,7 +50,229 @@ class GmAolCardIntent(AolCardIntent):
|
||||
standard_enabled=standard_enabled)
|
||||
if self._main_cycle_required:
|
||||
self.allowed_latch = False
|
||||
self.startup_calibration_pending = False
|
||||
|
||||
def observe_calibration(self, CS, *, events=None, standard_enabled=False):
|
||||
from openpilot.cereal import log
|
||||
# Cold calibration readiness is not a previously revoked driver gesture.
|
||||
# The original calibration inhibit still withdraws all optional output.
|
||||
prior_incomplete = self._startup_incomplete
|
||||
cold_safe = True
|
||||
if events is not None:
|
||||
from openpilot.starpilot.aol.intent import disarming_fault
|
||||
remaining = [event for event in _intent_fault_events(self.cp, events) if
|
||||
event.name != log.OnroadEvent.EventName.calibrationIncomplete or event.immediateDisable]
|
||||
cold_safe = not disarming_fault(remaining, CS,
|
||||
temporary_selfdrive_lagging=qualified_gm(self.cp))
|
||||
self._startup_incomplete = bool(cold_safe and
|
||||
any(event.name == log.OnroadEvent.EventName.calibrationIncomplete for event in events))
|
||||
cold_readiness = bool(self.startup_calibration_pending and cold_safe and
|
||||
(prior_incomplete or self._startup_incomplete) and
|
||||
self.settings.enabled and self.settings.main_action != AOL_TOGGLE and
|
||||
not CS.steerFaultPermanent and not self._main_cycle_required)
|
||||
if cold_readiness:
|
||||
self._calibration_rearm_required = False
|
||||
super().observe_calibration(CS, events=events, standard_enabled=standard_enabled)
|
||||
if cold_readiness:
|
||||
self._calibration_rearm_required = False
|
||||
if (events is not None and CS.canValid and not CS.canTimeout and not self._startup_incomplete and
|
||||
not any(event.name == log.OnroadEvent.EventName.selfdriveInitializing for event in events)):
|
||||
self.startup_calibration_pending = False
|
||||
|
||||
def output(self, CS):
|
||||
allowed, pause_lateral, pause_longitudinal = super().output(CS)
|
||||
return (bool(allowed and not CS.canTimeout and CS.cruiseState.available),
|
||||
pause_lateral, pause_longitudinal)
|
||||
|
||||
|
||||
def create_intent(cp, settings):
|
||||
return GmAolCardIntent(settings) if qualified_gm(cp) else None
|
||||
return GmAolCardIntent(settings, cp) if qualified_gm(cp) else None
|
||||
|
||||
|
||||
def configuration_settings_policy(cp):
|
||||
"""Saving stock Silverado lateral intent does not establish its physical source graph."""
|
||||
try:
|
||||
if (cp.brand == 'gm' and cp.carFingerprint == CAR.CHEVROLET_MALIBU_HYBRID_CC and
|
||||
not cp.notCar and not cp.passive and cp.lateralTuning.which() == 'torque' and
|
||||
cp.steerControlType == CarParams.SteerControlType.torque and
|
||||
int(cp.alternativeExperience) in (0, GM_AOL_ALTERNATIVE_EXPERIENCE)):
|
||||
return AolVehiclePolicy(settings_supported=True, lkas_button_supported=False)
|
||||
if (cp.brand != 'gm' or cp.carFingerprint != CAR.CHEVROLET_SILVERADO_CC or
|
||||
cp.notCar or cp.passive or cp.networkLocation != CarParams.NetworkLocation.gateway or
|
||||
cp.transmissionType != CarParams.TransmissionType.automatic or not cp.radarUnavailable or
|
||||
not cp.pcmCruise or cp.openpilotLongitudinalControl or cp.alphaLongitudinalAvailable or
|
||||
control_flags(cp) != 0 or int(cp.alternativeExperience) not in (0, GM_AOL_ALTERNATIVE_EXPERIENCE) or
|
||||
len(cp.safetyConfigs) != 1 or cp.safetyConfigs[0].safetyModel != CarParams.SafetyModel.gm or
|
||||
int(cp.safetyConfigs[0].safetyParam) != 16 or cp.steerControlType != CarParams.SteerControlType.torque or
|
||||
cp.lateralTuning.which() != 'torque'):
|
||||
return None
|
||||
tune = cp.lateralTuning.torque
|
||||
if (not all(math.isfinite(value) for value in (tune.latAccelFactor, tune.latAccelOffset, tune.friction)) or
|
||||
tune.latAccelFactor <= 0 or tune.friction < 0):
|
||||
return None
|
||||
return AolVehiclePolicy(settings_supported=True, lkas_button_supported=False)
|
||||
except (AttributeError, IndexError, TypeError, ValueError, OverflowError):
|
||||
return None
|
||||
|
||||
|
||||
def _intent_fault_events(cp, events):
|
||||
from openpilot.cereal import log
|
||||
if qualified_gm(cp):
|
||||
availability = (log.OnroadEvent.EventName.commIssue, log.OnroadEvent.EventName.commIssueAvgFreq)
|
||||
events = [event for event in events if not
|
||||
(event.name in availability and event.softDisable and not event.immediateDisable)]
|
||||
if qualified_gm(cp) and camera_acc_pedal_profile(cp) is None and is_volt_ascm_longitudinal(cp):
|
||||
events = [event for event in events if event.name != log.OnroadEvent.EventName.accFaulted]
|
||||
return events
|
||||
|
||||
|
||||
def intent_disarming_fault(cp, events, state, **context):
|
||||
"""An ASCM Volt ACC fault withdraws axes without erasing main-armed intent."""
|
||||
from openpilot.cereal import log
|
||||
from openpilot.starpilot.aol.intent import disarming_fault
|
||||
cold_calibration = context.pop('startup_calibration_pending', False)
|
||||
if qualified_gm(cp) and cold_calibration and not state.steerFaultPermanent:
|
||||
events = [event for event in events if not (
|
||||
event.name == log.OnroadEvent.EventName.calibrationIncomplete and not event.immediateDisable)]
|
||||
return disarming_fault(_intent_fault_events(cp, events), state, **context)
|
||||
|
||||
|
||||
def physical_longitudinal_withdrawal_source(cp, cs, parser, observation, now_ns) -> int:
|
||||
"""Bind a Bolt pedal-owner withdrawal to its parsed CAN and local receipt."""
|
||||
from opendbc.car.gm.values import CruiseButtons, is_bolt_present_no_acc_pedal_profile
|
||||
from opendbc.car.structs import car
|
||||
try:
|
||||
if (not qualified_gm(cp) or not is_bolt_present_no_acc_pedal_profile(cp) or
|
||||
int(cp.alternativeExperience) != GM_AOL_ALTERNATIVE_EXPERIENCE or
|
||||
observation is None or observation[0] is not cs or not cs.canValid or cs.canTimeout):
|
||||
return 0
|
||||
fields = []
|
||||
if cs.brakePressed and parser.vl['ECMEngineStatus']['BrakePressed']:
|
||||
fields.append(('ECMEngineStatus', 'BrakePressed'))
|
||||
if cs.regenBraking and parser.vl['EBCMRegenPaddle']['RegenPaddle']:
|
||||
fields.append(('EBCMRegenPaddle', 'RegenPaddle'))
|
||||
if (parser.vl['ASCMSteeringButton']['ACCButtons'] == CruiseButtons.CANCEL and
|
||||
any(e.type == car.CarState.ButtonEvent.Type.cancel and e.pressed for e in cs.buttonEvents)):
|
||||
fields.append(('ASCMSteeringButton', 'ACCButtons'))
|
||||
_, received_ns, can_source_ns, boot_ns = observation
|
||||
if not fields or not 0 < received_ns <= now_ns or now_ns - received_ns > 30_000_000:
|
||||
return 0
|
||||
# Parser/CAN batch use BOOTTIME. The fixed deadline uses the original
|
||||
# local MONOTONIC receipt; neither clock is converted or restamped.
|
||||
stamps = [int(parser.ts_nanos[message][signal]) for message, signal in fields]
|
||||
if not all(0 < stamp <= can_source_ns <= boot_ns and boot_ns - stamp <= 30_000_000 and
|
||||
can_source_ns - stamp <= 30_000_000 for stamp in stamps):
|
||||
return 0
|
||||
return received_ns
|
||||
except (AttributeError, KeyError, IndexError, TypeError, ValueError):
|
||||
return 0
|
||||
|
||||
|
||||
def retained_physical_longitudinal_intent(feedback, sm, cp, cs, axis, now_ns, *, latched, source_ns, producer):
|
||||
"""Retain accepted LAT intent through a bounded physical LONG withdrawal."""
|
||||
from opendbc.car.gm.values import is_bolt_present_no_acc_pedal_profile
|
||||
from opendbc.car.structs import car
|
||||
from openpilot.cereal import log
|
||||
from openpilot.starpilot.aol.runtime import AXIS_MAX_AGE_NS, SAFETY_MAX_AGE_NS
|
||||
from openpilot.starpilot.aol.transport_pause import independent_lateral_health
|
||||
occurrence = feedback.gm_withdrawal_ns or source_ns
|
||||
if (not qualified_gm(cp) or not is_bolt_present_no_acc_pedal_profile(cp) or
|
||||
int(cp.alternativeExperience) != GM_AOL_ALTERNATIVE_EXPERIENCE or
|
||||
not latched or feedback.failed or feedback.paused or feedback.pending_event or
|
||||
feedback.retention_started_ns or not feedback.session or str(axis.sessionId) != feedback.session or
|
||||
not feedback.healthy_feedback_ns or not 0 <= now_ns - feedback.healthy_feedback_ns <= AXIS_MAX_AGE_NS or
|
||||
not 0 < occurrence <= now_ns or now_ns - occurrence > SAFETY_MAX_AGE_NS or
|
||||
not feedback.gm_withdrawal_ns and now_ns - occurrence > AXIS_MAX_AGE_NS or
|
||||
not producer or producer != feedback.availability_producer or
|
||||
str(axis.intentProducerSessionId) != producer or int(axis.intentIntegrityEpoch) != feedback.availability_epoch or
|
||||
cs.accFaulted or cs.doorOpen or not cs.cruiseState.available or
|
||||
cs.gearShifter not in (car.CarState.GearShifter.drive, car.CarState.GearShifter.low,
|
||||
car.CarState.GearShifter.sport, car.CarState.GearShifter.brake) or
|
||||
str(axis.faultReason) != 'none' or not axis.desiredLateral or not axis.lateralActive or
|
||||
not axis.nativeAcknowledged or
|
||||
not 0 < axis.sourceCarStateMonoTime <= axis.observedMonoTime <= now_ns or
|
||||
now_ns - axis.sourceCarStateMonoTime > AXIS_MAX_AGE_NS):
|
||||
return None
|
||||
# The saved LAT contract treats a parsed unlatched belt as a LONG lock.
|
||||
# After rebuckling, accept only a prior producer's matching belt event and
|
||||
# LAT-only proof within the already established physical occurrence. Events
|
||||
# publish sparsely, so their stamp binds to faultEventMonoTime, not the newer
|
||||
# car-state source. The original axis/native leases remain unchanged.
|
||||
event_ns = int(sm.logMonoTime['onroadEvents'])
|
||||
prior_belt_proof = bool(feedback.gm_withdrawal_ns and
|
||||
feedback.gm_withdrawal_ns <= event_ns <= axis.observedMonoTime < now_ns and
|
||||
event_ns == axis.faultEventMonoTime and str(axis.faultSessionId) == feedback.session and
|
||||
int(sm.logMonoTime['aolAxisState']) == axis.observedMonoTime and
|
||||
now_ns <= axis.validUntilMonoTime <= axis.observedMonoTime + AXIS_MAX_AGE_NS and
|
||||
not axis.desiredLongitudinal and not axis.longitudinalActive)
|
||||
if (not sm.seen['onroadEvents'] or not sm.valid['onroadEvents'] or not sm.alive['onroadEvents'] or
|
||||
not 0 < event_ns <= now_ns or now_ns - event_ns > 1_500_000_000 or
|
||||
any(e.immediateDisable or e.softDisable and not
|
||||
((cs.seatbeltUnlatched or prior_belt_proof) and e.name == log.OnroadEvent.EventName.seatbeltNotLatched and
|
||||
e.noEntry and not e.userDisable) for e in sm['onroadEvents'])):
|
||||
return None
|
||||
native = independent_lateral_health(sm, cp, cs, feedback.session, now_ns)
|
||||
if (native is None or not native.pandaInventory or not native.requestedLongitudinal or
|
||||
not any(not p.controlsAllowed for p in sm['pandaStates']) or
|
||||
native.observedMonoTime < feedback.healthy_native_ns or
|
||||
(native.pandaSerial, tuple(p.hardwareSerial for p in native.pandaInventory)) != feedback.healthy_inventory):
|
||||
return None
|
||||
# One occurrence spans the held native request. Held/released samples and
|
||||
# duplicate Card calls cannot renew it; output/native leases remain original.
|
||||
feedback.gm_withdrawal_ns = occurrence
|
||||
return native
|
||||
|
||||
|
||||
def physical_cancel_lateral_allowed(cp, cs, intent, native, *, source_ns, now_ns) -> bool:
|
||||
"""Exact armed Bolt Cancel removes only the lateral NO_ENTRY restriction."""
|
||||
from opendbc.car.gm.values import is_bolt_present_no_acc_pedal_profile
|
||||
from opendbc.car.structs import car
|
||||
from openpilot.starpilot.aol.runtime import AXIS_MAX_AGE_NS, SAFETY_MAX_AGE_NS, native_inventory_matches_cp
|
||||
try:
|
||||
return bool(qualified_gm(cp) and is_bolt_present_no_acc_pedal_profile(cp) and
|
||||
int(cp.alternativeExperience) == GM_AOL_ALTERNATIVE_EXPERIENCE and
|
||||
intent is not None and intent.settingsQualified and intent.allowedLatch and intent.lateralArmed and
|
||||
intent.producerSessionId and intent.carStateLogMonoTime == source_ns and
|
||||
0 < source_ns <= intent.observedMonoTime <= now_ns <= intent.validUntilMonoTime and
|
||||
now_ns - source_ns <= AXIS_MAX_AGE_NS and
|
||||
# Card declares a 200ms wire lease; admission and this source remain 30ms fresh.
|
||||
intent.validUntilMonoTime - intent.observedMonoTime <= SAFETY_MAX_AGE_NS and
|
||||
cs.canValid and not cs.canTimeout and not cs.steerFaultTemporary and not cs.steerFaultPermanent and
|
||||
any(e.type == car.CarState.ButtonEvent.Type.cancel for e in cs.buttonEvents) and
|
||||
native is not None and native.requestedLateral and native.lateralAllowed and
|
||||
native.pandaInventory and native_inventory_matches_cp(native, cp) and
|
||||
0 < native.observedMonoTime <= now_ns <= native.validUntilMonoTime)
|
||||
except (AttributeError, KeyError, IndexError, TypeError, ValueError):
|
||||
return False
|
||||
|
||||
|
||||
def seatbelt_lateral_soft_event_allowed(cp, cs, intent, native, event, *, source_ns, now_ns, axis_session_id) -> bool:
|
||||
"""The qualified saved LAT owner treats only its ordinary belt event as a LONG lock."""
|
||||
if not qualified_gm(cp):
|
||||
return False
|
||||
from opendbc.car.gm.values import is_bolt_present_no_acc_pedal_profile
|
||||
from opendbc.car.structs import car
|
||||
from openpilot.cereal import log
|
||||
from openpilot.starpilot.aol.runtime import AXIS_MAX_AGE_NS, SAFETY_MAX_AGE_NS, native_inventory_matches_cp
|
||||
try:
|
||||
return bool(is_bolt_present_no_acc_pedal_profile(cp) and
|
||||
int(cp.alternativeExperience) == GM_AOL_ALTERNATIVE_EXPERIENCE and
|
||||
event.name == log.OnroadEvent.EventName.seatbeltNotLatched and event.softDisable and
|
||||
event.noEntry and not event.immediateDisable and not event.userDisable and
|
||||
cs.seatbeltUnlatched and cs.canValid and not cs.canTimeout and not cs.accFaulted and not cs.doorOpen and
|
||||
not cs.steerFaultTemporary and not cs.steerFaultPermanent and cs.cruiseState.available and
|
||||
cs.gearShifter in (car.CarState.GearShifter.drive, car.CarState.GearShifter.low,
|
||||
car.CarState.GearShifter.sport, car.CarState.GearShifter.brake) and
|
||||
intent is not None and intent.settingsQualified and intent.allowedLatch and intent.lateralArmed and
|
||||
intent.producerSessionId and intent.carStateLogMonoTime == source_ns and
|
||||
0 < source_ns <= intent.observedMonoTime <= now_ns <= intent.validUntilMonoTime and
|
||||
now_ns - source_ns <= AXIS_MAX_AGE_NS and
|
||||
intent.validUntilMonoTime - intent.observedMonoTime <= SAFETY_MAX_AGE_NS and
|
||||
native is not None and native.compatible and int(native.protocolVersion) == 1 and
|
||||
axis_session_id and native.axisSessionId == axis_session_id and
|
||||
native.requestedLateral and native.lateralAllowed and native.pandaInventory and
|
||||
native_inventory_matches_cp(native, cp) and
|
||||
0 < native.observedMonoTime <= now_ns <= native.validUntilMonoTime and
|
||||
native.validUntilMonoTime - native.observedMonoTime <= SAFETY_MAX_AGE_NS)
|
||||
except (AttributeError, KeyError, IndexError, TypeError, ValueError):
|
||||
return False
|
||||
|
||||
@@ -0,0 +1,59 @@
|
||||
"""Exact carState companions from the nonconflating steering-authority transport."""
|
||||
from openpilot.cereal import messaging
|
||||
from openpilot.starpilot.aol.runtime import INTENT_MAX_AGE_NS
|
||||
|
||||
MAX_COMPANIONS = 16
|
||||
MAX_DRAIN = 32
|
||||
SERVICE = 'starpilotCarState'
|
||||
|
||||
|
||||
class SteeringCompanion:
|
||||
def __init__(self, sock=None):
|
||||
self.sock = sock
|
||||
self.samples = {}
|
||||
self.latest = None
|
||||
|
||||
def _observe(self, stamp, valid, companion):
|
||||
sample = (int(stamp), bool(valid), int(companion.sourceCarStateMonoTime),
|
||||
bool(companion.lateralAuthorityUnavailable))
|
||||
previous = self.samples.get(sample[0])
|
||||
if previous is not None:
|
||||
sample = (sample[0], previous[1] and sample[1] and previous[2] == sample[2],
|
||||
sample[2], previous[3] or sample[3])
|
||||
self.samples[sample[0]] = sample
|
||||
if self.latest is None or sample[0] >= self.latest[0]:
|
||||
self.latest = sample
|
||||
while len(self.samples) > MAX_COMPANIONS:
|
||||
del self.samples[min(self.samples)]
|
||||
|
||||
def current(self, sm, *, source_ns, now_ns):
|
||||
if self.sock is not None:
|
||||
for _ in range(MAX_DRAIN):
|
||||
message = messaging.recv_one_or_none(self.sock)
|
||||
if message is None:
|
||||
break
|
||||
self._observe(message.logMonoTime, message.valid, message.starpilotCarState)
|
||||
else:
|
||||
return None
|
||||
elif sm.seen.get(SERVICE, False):
|
||||
self._observe(sm.logMonoTime[SERVICE], sm.valid.get(SERVICE, False), sm[SERVICE])
|
||||
|
||||
for stamp in tuple(self.samples):
|
||||
if not 0 < stamp <= now_ns or now_ns - stamp > INTENT_MAX_AGE_NS:
|
||||
del self.samples[stamp]
|
||||
if not (sm.seen.get(SERVICE, False) and sm.valid.get(SERVICE, False) and sm.alive.get(SERVICE, False)):
|
||||
return None
|
||||
sm_stamp = int(sm.logMonoTime[SERVICE])
|
||||
sm_companion = sm[SERVICE]
|
||||
if not (int(sm_companion.sourceCarStateMonoTime) == sm_stamp and
|
||||
0 < source_ns <= sm_stamp <= now_ns and now_ns - sm_stamp <= INTENT_MAX_AGE_NS):
|
||||
return None
|
||||
latest = self.latest
|
||||
pair = self.samples.get(source_ns)
|
||||
if latest is None or pair is None:
|
||||
return None
|
||||
stamp, valid, source, unavailable = latest
|
||||
if not (valid and source == stamp and source_ns <= stamp <= now_ns and
|
||||
now_ns - stamp <= INTENT_MAX_AGE_NS and pair[1] and pair[2] == source_ns):
|
||||
return None
|
||||
return bool(pair[3] or unavailable or sm_companion.lateralAuthorityUnavailable)
|
||||
@@ -0,0 +1,87 @@
|
||||
import os
|
||||
import unittest
|
||||
from unittest.mock import patch
|
||||
|
||||
from opendbc.car.car_helpers import interfaces
|
||||
from opendbc.car.hyundai.values import CAR
|
||||
from openpilot.cereal import messaging
|
||||
from openpilot.common.params import Params
|
||||
from openpilot.common.prefix import OpenpilotPrefix
|
||||
from openpilot.selfdrive.controls.controlsd import Controls
|
||||
from openpilot.starpilot.car.hyundai.lateral_fault import LateralFaultLatch
|
||||
from openpilot.starpilot.lateral.tests.test_lane_runtime import feed
|
||||
|
||||
|
||||
class TestHyundaiLateralFault(unittest.TestCase):
|
||||
def test_actual_controls_requires_rearm_after_elantra_hybrid_fault(self):
|
||||
for model in (CAR.HYUNDAI_ELANTRA_HEV_2024, CAR.HYUNDAI_ELANTRA_2024, CAR.HYUNDAI_IONIQ_6):
|
||||
with self.subTest(model=model), OpenpilotPrefix(), \
|
||||
patch.dict(os.environ, {'REPLAY': '1', 'AOL_REPLAY_RUNTIME': '0'}), \
|
||||
patch('openpilot.selfdrive.controls.controlsd.messaging.PubMaster'):
|
||||
cp = interfaces[model].get_non_essential_params(model)
|
||||
Params().put('CarParams', cp.to_bytes(), block=True)
|
||||
controls = Controls()
|
||||
latched_model = model == CAR.HYUNDAI_ELANTRA_HEV_2024
|
||||
cases = [(True, False, False, False, True),
|
||||
(True, True, False, False, False),
|
||||
(True, False, False, False, not latched_model),
|
||||
(True, False, True, False, True),
|
||||
(True, True, True, False, False),
|
||||
(False, False, False, False, False),
|
||||
(True, False, False, False, True),
|
||||
(True, False, False, True, False)]
|
||||
for tick, (requested, temporary, cruise, permanent, expected) in enumerate(cases):
|
||||
now_ns = 1_000_000_000 + tick * 10_000_000
|
||||
feed(controls, now_ns, tick, active=requested, enabled=requested, fault=temporary)
|
||||
event = messaging.new_message('carState', valid=True, logMonoTime=now_ns)
|
||||
event.carState = controls.sm['carState']
|
||||
event.carState.cruiseState.enabled = cruise
|
||||
event.carState.steerFaultPermanent = permanent
|
||||
controls.sm.data['carState'] = event.carState.as_reader()
|
||||
command, _ = controls.state_control()
|
||||
self.assertEqual(command.latActive, expected)
|
||||
self.assertEqual(command.enabled, requested)
|
||||
if not expected:
|
||||
self.assertEqual(command.actuators.torque, 0.0)
|
||||
|
||||
def test_actual_axis_intent_survives_fault_and_missing_permission_without_admission(self):
|
||||
from openpilot.starpilot.aol.runtime import current_axis, current_native
|
||||
|
||||
with OpenpilotPrefix(), patch.dict(os.environ, {'REPLAY': '1', 'AOL_REPLAY_RUNTIME': '0'}), \
|
||||
patch('openpilot.selfdrive.controls.controlsd.messaging.PubMaster'):
|
||||
cp = interfaces[CAR.HYUNDAI_ELANTRA_HEV_2024].get_non_essential_params(CAR.HYUNDAI_ELANTRA_HEV_2024)
|
||||
Params().put('CarParams', cp.to_bytes(), block=True)
|
||||
controls = Controls()
|
||||
self.assertFalse(controls.aol_replay)
|
||||
# Exercise the reader boundary without granting this vehicle AOL safety.
|
||||
controls.aol_replay = True
|
||||
controls.sm = messaging.SubMaster([*controls.sm.services, 'aolAxisState', 'aolSafetyWire'], frequency=100)
|
||||
for tick, (desired, active, fault, fresh, expected_latch) in enumerate(
|
||||
((True, True, True, True, True), (True, False, False, True, True),
|
||||
(True, False, False, False, True), (False, False, False, True, False))):
|
||||
now_ns = 1_000_000_000 + tick * 10_000_000
|
||||
feed(controls, now_ns, tick, fault=fault, aol_lat_only=True)
|
||||
event = messaging.new_message('aolAxisState', valid=True, logMonoTime=now_ns)
|
||||
event.aolAxisState = controls.sm['aolAxisState']
|
||||
event.aolAxisState.desiredLateral = desired
|
||||
event.aolAxisState.lateralActive = active
|
||||
controls.sm.data['aolAxisState'] = event.aolAxisState.as_reader()
|
||||
controls.sm.valid['aolAxisState'] = fresh
|
||||
axis = current_axis(controls.sm, now_ns=now_ns)
|
||||
self.assertEqual(axis is not None, fresh)
|
||||
self.assertIsNone(current_native(controls.sm, cp, now_ns=now_ns))
|
||||
command, _ = controls.state_control()
|
||||
self.assertEqual(controls.hyundai_lateral_fault.faulted, expected_latch)
|
||||
self.assertFalse(command.latActive)
|
||||
self.assertEqual(command.actuators.torque, 0.0)
|
||||
|
||||
def test_fault_at_cruise_rising_edge_still_suppresses_output(self):
|
||||
latch = LateralFaultLatch(CAR.HYUNDAI_ELANTRA_HEV_2024)
|
||||
self.assertFalse(latch.update(requested=True, temporary_fault=True, cruise_enabled=True))
|
||||
self.assertTrue(latch.update(requested=True, temporary_fault=True, cruise_enabled=True))
|
||||
self.assertTrue(latch.update(requested=True, temporary_fault=False, cruise_enabled=True))
|
||||
self.assertFalse(latch.update(requested=False, temporary_fault=False, cruise_enabled=True))
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
unittest.main()
|
||||
Reference in New Issue
Block a user