mirror of
https://github.com/firestar5683/StarPilot.git
synced 2026-11-08 05:23:41 +08:00
firestar5683
315 lines
18 KiB
Python
315 lines
18 KiB
Python
"""Screen gestures through current Tesla parsing, Card, Controls and native safety."""
|
|
import os
|
|
from dataclasses import replace
|
|
from itertools import product
|
|
from types import SimpleNamespace
|
|
import unittest
|
|
from unittest.mock import patch
|
|
|
|
from opendbc.can import CANPacker
|
|
from opendbc.car import gen_empty_fingerprint, structs
|
|
from opendbc.car.tesla.interface import CarInterface
|
|
from opendbc.car.tesla.values import CAR
|
|
from opendbc.safety.tests.libsafety import libsafety_py
|
|
from openpilot.cereal import messaging
|
|
from openpilot.common.params import Params
|
|
from openpilot.common.prefix import OpenpilotPrefix
|
|
from openpilot.selfdrive.car.card import Car
|
|
from openpilot.selfdrive.controls.controlsd import Controls
|
|
from openpilot.selfdrive.selfdrived.selfdrived import SelfdriveD
|
|
from openpilot.selfdrive.selfdrived.events import EventName
|
|
from openpilot.starpilot.aol.wire import SafetyState, encode_safety
|
|
from openpilot.starpilot.aol.runtime import current_native
|
|
from openpilot.starpilot.lateral.tests.test_lane_runtime import feed
|
|
from openpilot.starpilot.tests.tesla_fixture import DBC, EPS, FW_VERSIONS, TeslaFixture
|
|
|
|
|
|
def screen_params(platform, marker, *, longitudinal=False, release=False):
|
|
fingerprint = gen_empty_fingerprint()
|
|
fingerprint[1][0x3DF] = 8
|
|
fingerprint[2][0x293] = 8
|
|
fingerprint[0 if marker == 'marker054' else 2][0x054 if marker == 'marker054' else 0x489] = 8
|
|
fw = structs.CarParams.CarFw(ecu=EPS[0], address=EPS[1], brand='tesla', fwVersion=FW_VERSIONS[platform][EPS][0])
|
|
return CarInterface.get_params(platform, fingerprint, [fw], longitudinal and not release, release, False)
|
|
|
|
|
|
def start_card(cp, *, after_snapshot=None):
|
|
def create(*args, pre_create_hook, **kwargs):
|
|
if after_snapshot is not None:
|
|
after_snapshot()
|
|
return CarInterface(pre_create_hook(cp.as_reader().as_builder(), cp.carFingerprint, {}, []))
|
|
with patch('openpilot.selfdrive.car.card.messaging.recv_one_retry', return_value=SimpleNamespace(can=[1])), \
|
|
patch('openpilot.selfdrive.car.card.get_car', side_effect=create):
|
|
return Car()
|
|
|
|
|
|
def selfdrive_step(sd, published, native, previous_command, cp, safety, now):
|
|
peers = []
|
|
for service in sd.sm.data:
|
|
if service in ('alertDebug', 'lateralManeuverPlan', 'laneChangeAssistWire'):
|
|
continue
|
|
if service == 'pandaStates' and service in published:
|
|
peers.append(published[service])
|
|
continue
|
|
if service == 'aolIntentWire':
|
|
peers.append(published[service])
|
|
continue
|
|
if service == 'aolSafetyWire':
|
|
peers.append(native.as_reader())
|
|
continue
|
|
size = 1 if service == 'pandaStates' else None
|
|
event = messaging.new_message(service, size, valid=True, logMonoTime=now)
|
|
value = getattr(event, service)
|
|
if service == 'pandaStates':
|
|
value[0].safetyModel = cp.safetyConfigs[0].safetyModel
|
|
value[0].safetyParam = cp.safetyConfigs[0].safetyParam
|
|
value[0].alternativeExperience = cp.alternativeExperience
|
|
value[0].controlsAllowed = bool(safety.get_controls_allowed())
|
|
value[0].safetyRxChecksInvalid = not bool(safety.safety_config_valid())
|
|
elif service == 'extrinsicsCalibration':
|
|
value.calStatus, value.rpyCalib = 'calibrated', [0., 0., 0.]
|
|
elif service == 'vehicleParameters':
|
|
value.stiffnessFactor, value.steerRatio, value.valid = 1., cp.steerRatio, True
|
|
elif service == 'deviceMotion':
|
|
value.inputsOK = value.posenetOK = True
|
|
value.velocityDevice.x = published['carState'].carState.vEgo
|
|
elif service == 'controlsState':
|
|
value.lateralControlState.init('angleState')
|
|
elif service == 'carControl':
|
|
value.from_dict(previous_command.to_dict())
|
|
elif service == 'deviceState':
|
|
value.freeSpacePercent = 50.
|
|
peers.append(event.as_reader())
|
|
output = {}
|
|
with patch('openpilot.selfdrive.selfdrived.selfdrived.REPLAY', False), \
|
|
patch('openpilot.selfdrive.selfdrived.selfdrived.time.monotonic_ns', return_value=now), \
|
|
patch('openpilot.selfdrive.selfdrived.selfdrived.messaging.recv_one', return_value=published['carState']), \
|
|
patch.object(sd.sm, 'update', side_effect=lambda _: sd.sm.update_msgs(now / 1e9, peers)), \
|
|
patch.object(sd.pm, 'send', side_effect=lambda service, msg: output.update({service: msg.as_reader()})):
|
|
sd.step()
|
|
return output
|
|
|
|
|
|
def transport_receipt(native, tick, now):
|
|
if tick == 48:
|
|
return replace(native, validUntilMonoTime=now - 1)
|
|
if tick == 49:
|
|
return replace(native, safetyParam=native.safetyParam ^ 1024)
|
|
if tick == 50:
|
|
return replace(native, safetyModel=39, safetyParam=0)
|
|
if tick == 51:
|
|
return replace(native, axisSessionId='other-session')
|
|
if tick == 52:
|
|
return replace(native, safetyParam=native.safetyParam ^ 1)
|
|
if tick == 53:
|
|
return replace(native, safetyParam=514)
|
|
return native
|
|
|
|
|
|
class TestTeslaScreenIntegration(unittest.TestCase):
|
|
def test_screen_gesture_baseline_order_brake_and_cruise_ownership(self):
|
|
self.exercise_screen_runtime(master_off=False)
|
|
|
|
def test_marked_screen_restart_without_aol_preserves_ordinary_axes(self):
|
|
self.exercise_screen_runtime(master_off=True)
|
|
|
|
def exercise_screen_runtime(self, *, master_off):
|
|
safety = libsafety_py.libsafety
|
|
release = safety.set_safety_hooks(structs.CarParams.SafetyModel.allOutput, 0) != 0
|
|
for platform in (CAR.TESLA_MODEL_3, CAR.TESLA_MODEL_Y):
|
|
for marker in ('marker054', 'marker489'):
|
|
for brake_option, longitudinal in product((False, True), repeat=2):
|
|
with self.subTest(platform=platform, marker=marker, brake_option=brake_option, longitudinal=longitudinal), OpenpilotPrefix(), \
|
|
patch.dict(os.environ, {'SIMULATION': '1', 'REPLAY': '1', 'AOL_REPLAY_RUNTIME': '0'}):
|
|
saved = Params()
|
|
for key, value in (('OpenpilotEnabledToggle', True), ('AlwaysOnLateral', True),
|
|
('TeslaAOLScreenTap', True), ('TeslaAOLDisengageOnBrake', brake_option),
|
|
('IsReleaseBranch', release), ('SafeMode', False)):
|
|
saved.put_bool(key, value, block=True)
|
|
if master_off:
|
|
from openpilot.selfdrive.car import card as card_module
|
|
requested = card_module.feature_requested
|
|
saved.put_bool('AlwaysOnLateral', False, block=True)
|
|
def sample_request(params, feature, requested=requested, saved=saved):
|
|
result = requested(params, feature)
|
|
if feature == 'aol':
|
|
saved.put_bool('AlwaysOnLateral', True, block=True)
|
|
return result
|
|
with patch.object(card_module, 'feature_requested', side_effect=sample_request), \
|
|
patch.object(card_module, 'can_comm_callbacks', wraps=card_module.can_comm_callbacks) as callbacks:
|
|
selected = start_card(screen_params(platform, marker, longitudinal=longitudinal, release=release),
|
|
after_snapshot=lambda saved=saved: saved.put_bool('AlwaysOnLateral', False, block=True))
|
|
self.assertIs(callbacks.call_args.args[1], selected.pm.sock['sendcan'])
|
|
self.assertIn('aolSafetyWire', selected.sm.data)
|
|
saved.put_bool('AlwaysOnLateral', False, block=True)
|
|
iterations = iter((False, True))
|
|
with patch('openpilot.selfdrive.car.card.time.sleep'):
|
|
selected.params_thread(SimpleNamespace(is_set=lambda iterations=iterations: next(iterations)))
|
|
else:
|
|
selected = start_card(screen_params(platform, marker, longitudinal=longitudinal, release=release))
|
|
cp, ci = selected.CP, selected.CI
|
|
self.assertEqual(cp.safetyConfigs[0].safetyParam, (1536 if brake_option else 512) + int(cp.openpilotLongitudinalControl))
|
|
self.assertEqual(cp.alternativeExperience, 32)
|
|
self.assertEqual(cp.openpilotLongitudinalControl, longitudinal and not release)
|
|
self.assertTrue(cp.pcmCruise)
|
|
self.assertIsNotNone(selected.aol_card_intent)
|
|
controls = Controls()
|
|
with patch('openpilot.selfdrive.selfdrived.selfdrived.REPLAY', False):
|
|
sd = SelfdriveD(CP=cp)
|
|
self.assertTrue(controls.aol_replay)
|
|
self.assertTrue(sd.aol_replay)
|
|
self.assertEqual(controls.CP.to_dict(), cp.to_dict())
|
|
packer = CANPacker(DBC)
|
|
safety.init_tests()
|
|
safety.set_alternative_experience(cp.alternativeExperience)
|
|
self.assertEqual(safety.set_safety_hooks(cp.safetyConfigs[0].safetyModel.raw, cp.safetyConfigs[0].safetyParam), 0)
|
|
observed = {}
|
|
previous_receipt = None
|
|
previous_command = structs.CarControl()
|
|
selfdriven = {}
|
|
for tick in range(250):
|
|
now = 1_000_000_000 + tick * 10_000_000
|
|
cruise = 2 if 140 <= tick < 160 else 0
|
|
brake = 100 <= tick < 110
|
|
unbuckled = 40 <= tick < 60 or 150 <= tick < 160
|
|
counts = (3,) if tick < 35 or tick in (40, 60, 90) or tick == 120 and brake_option else (0,)
|
|
# Authority loss needs a published withdrawal, then a fresh
|
|
# physical rearm and one native request acknowledgement cycle.
|
|
if not master_off and tick in (53, 54):
|
|
counts = (3,) if tick == 53 else (0, 3, 0)
|
|
if tick == 80:
|
|
counts = (3, 0, 3, 0)
|
|
if 170 <= tick <= 200 or 205 <= tick < 220 or tick == 235:
|
|
counts = (3,)
|
|
incoming = []
|
|
for bus, name, period, defaults in TeslaFixture.INPUTS:
|
|
if tick % period:
|
|
continue
|
|
values = dict(defaults)
|
|
if name == 'DI_state':
|
|
values['DI_cruiseState'] = cruise
|
|
if name == 'ESP_status':
|
|
values['ESP_driverBrakeApply'] = 2 if brake else 1
|
|
if name == 'UI_warning':
|
|
values['buckleStatus'] = 0 if unbuckled else 1
|
|
if name == 'EPAS3S_sysStatus':
|
|
values['EPAS3S_handsOnLevel'] = 3 if 170 <= tick < 190 or 220 <= tick < 230 else 0
|
|
incoming.append(packer.make_can_msg(name, bus, values))
|
|
incoming.extend((0x3DF, bytes((0, 0, 0, count, 0, 0, 0, 0)), 1) for count in counts)
|
|
safety.set_timer(now // 1000)
|
|
for address, payload, bus in incoming:
|
|
self.assertTrue(safety.safety_rx_hook(libsafety_py.make_CANPacket(address, bus, payload)))
|
|
safety.safety_tick()
|
|
safety.set_aol_test_heartbeat(True)
|
|
peers = []
|
|
control = messaging.new_message('carControl', valid=True, logMonoTime=now)
|
|
control.carControl = previous_command
|
|
peers.append(control.as_reader())
|
|
events = messaging.new_message('onroadEvents', 0, valid=True, logMonoTime=now)
|
|
peers.append(events.as_reader())
|
|
if previous_receipt is not None:
|
|
peers.append(previous_receipt.as_reader())
|
|
selected.sm.update_msgs(now / 1e9, peers)
|
|
selected.CC_prev = previous_command
|
|
published = {}
|
|
new_message = messaging.new_message
|
|
def stamped_message(service, size=None, *, factory=new_message, stamp=now, **kwargs):
|
|
return factory(service, size, **({'logMonoTime': stamp} | kwargs))
|
|
with patch('openpilot.selfdrive.car.card.messaging.drain_sock_raw', return_value=[b'can']), \
|
|
patch('openpilot.selfdrive.car.card.can_capnp_to_list', return_value=[(now, incoming)]), \
|
|
patch('openpilot.selfdrive.car.card.time.monotonic_ns', return_value=now), \
|
|
patch('openpilot.selfdrive.car.card.REPLAY', False), patch.object(selected.sm, 'update'):
|
|
cs, radar = selected.state_update()
|
|
with patch('openpilot.selfdrive.car.card.messaging.new_message', side_effect=stamped_message), \
|
|
patch.object(selected.pm, 'send', side_effect=lambda service, msg, published=published: published.update({service: msg.as_reader()})):
|
|
selected.state_publish(cs, radar)
|
|
ordinary = bool(cs.cruiseState.enabled)
|
|
mask = safety.aol_get_permission_mask()
|
|
native = SafetyState(1, True, now, now + 30_000_000, int(cp.safetyConfigs[0].safetyModel.raw),
|
|
cp.safetyConfigs[0].safetyParam, bool(mask & 1), bool(mask & 2),
|
|
bool(safety.aol_get_request_mask() & 1), bool(safety.aol_get_request_mask() & 2),
|
|
'tesla-panda', sd.aol_session_id)
|
|
receipt = messaging.new_message('aolSafetyWire', 0, valid=True, logMonoTime=now)
|
|
receipt.aolSafetyWire = encode_safety(transport_receipt(native, tick, now))
|
|
selfdriven.update(selfdrive_step(sd, published, receipt, previous_command, cp, safety, now))
|
|
wanted = sd.aol_axis_decision
|
|
safety.aol_set_host_request(int(wanted.desired_lateral) | (int(wanted.desired_longitudinal) << 1))
|
|
mask = safety.aol_get_permission_mask()
|
|
native = SafetyState(1, True, now, now + 30_000_000, int(cp.safetyConfigs[0].safetyModel.raw),
|
|
cp.safetyConfigs[0].safetyParam, bool(mask & 1), bool(mask & 2),
|
|
wanted.desired_lateral, wanted.desired_longitudinal, 'tesla-panda', sd.aol_session_id)
|
|
feed(controls, now, tick, active=ordinary, enabled=ordinary)
|
|
state = published['carState']
|
|
receipt = messaging.new_message('aolSafetyWire', 0, valid=True, logMonoTime=now)
|
|
receipt.aolSafetyWire = encode_safety(transport_receipt(native, tick, now))
|
|
controls.sm.update_msgs((now + 1_000) / 1e9, [state, selfdriven['aolAxisState'], selfdriven['selfdriveState'], receipt.as_reader(),
|
|
selfdriven['onroadEvents']])
|
|
if tick == 55:
|
|
self.assertIsNotNone(current_native(controls.sm, cp, now_ns=now, axis_session_id=sd.aol_session_id))
|
|
for mutate in ('unmarked', 'duplicate'):
|
|
invalid = cp.as_reader().as_builder()
|
|
if mutate == 'unmarked':
|
|
invalid.alternativeExperience = 0
|
|
else:
|
|
invalid.safetyConfigs = [cp.safetyConfigs[0].to_dict(), cp.safetyConfigs[0].to_dict()]
|
|
self.assertIsNone(current_native(controls.sm, invalid, now_ns=now, axis_session_id=sd.aol_session_id))
|
|
command, lateral_log = controls.state_control()
|
|
controls.publish(command, lateral_log)
|
|
previous_command, previous_receipt = command, receipt
|
|
self.assertEqual(command.latActive, wanted.lateral_active)
|
|
self.assertEqual(command.longActive, wanted.longitudinal_active)
|
|
_, outgoing = ci.apply(command.as_reader(), now)
|
|
for address, payload, bus in outgoing:
|
|
self.assertTrue(safety.safety_tx_hook(libsafety_py.make_CANPacket(address, bus, payload)), (tick, hex(address)))
|
|
self.assertEqual(safety.safety_fwd_hook(1, 0x3DF), -1)
|
|
self.assertEqual(safety.safety_fwd_hook(0, 0x3DF), 2)
|
|
observed[tick] = (command.latActive, [event.pressed for event in cs.buttonEvents], cs.canValid,
|
|
{'permission': mask, 'desired': wanted.desired_lateral, 'events': sd.events.names,
|
|
'latch': selected.aol_card_intent.allowed_latch, 'axis_ack': wanted.native_acknowledged,
|
|
'native_healthy': bool(safety.safety_config_valid()), 'long_active': command.longActive,
|
|
'unbuckled': cs.seatbeltUnlatched, 'cancel': command.cruiseControl.cancel,
|
|
'ordinary_active': sd.active, 'ordinary_enabled': sd.enabled,
|
|
'authority_lost': sd.aol_authority_lost})
|
|
self.assertTrue(all(row[2] for tick, row in observed.items() if tick >= 30))
|
|
self.assertFalse(observed[34][0])
|
|
self.assertEqual(observed[34][1], [])
|
|
self.assertEqual(observed[45][0], not master_off, observed[45])
|
|
events = observed[45][3]['events']
|
|
assert isinstance(events, list)
|
|
self.assertIn(EventName.wrongCarMode, events)
|
|
self.assertFalse(observed[45][3]['ordinary_active'] or observed[45][3]['ordinary_enabled'])
|
|
self.assertTrue(observed[45][3]['unbuckled'])
|
|
self.assertIn(EventName.seatbeltNotLatched, events)
|
|
self.assertFalse(observed[45][3]['long_active'])
|
|
for tick in (48, 49, 50, 51, 52, 53):
|
|
self.assertFalse(observed[tick][0], observed[tick])
|
|
if not master_off:
|
|
self.assertTrue(observed[52][3]['authority_lost'])
|
|
self.assertEqual(observed[53][1], [True])
|
|
self.assertFalse(observed[53][3]['latch'] or observed[53][3]['desired'])
|
|
self.assertFalse(observed[53][3]['authority_lost'])
|
|
self.assertEqual(observed[54][1], [False, True, False])
|
|
self.assertTrue(observed[54][3]['latch'] and observed[54][3]['desired'])
|
|
self.assertFalse(observed[54][0]) # Rearm has not yet been acknowledged.
|
|
self.assertFalse(observed[54][3]['authority_lost'])
|
|
self.assertFalse(observed[55][3]['authority_lost'])
|
|
self.assertEqual(observed[55][0], not master_off)
|
|
self.assertFalse(observed[65][0])
|
|
self.assertEqual(observed[80][1], [True, False, True, False])
|
|
self.assertFalse(observed[85][0])
|
|
self.assertEqual(observed[95][0], not master_off)
|
|
self.assertEqual(observed[105][0], not master_off and not brake_option)
|
|
self.assertEqual(observed[125][0], not master_off)
|
|
self.assertTrue(observed[145][0])
|
|
self.assertEqual(observed[145][3]['long_active'], cp.openpilotLongitudinalControl)
|
|
self.assertTrue(observed[155][3]['unbuckled'])
|
|
self.assertTrue(observed[155][0], observed[155])
|
|
self.assertFalse(observed[155][3]['long_active'])
|
|
self.assertFalse(observed[165][0])
|
|
self.assertFalse(any(observed[tick][0] for tick in range(170, 205)))
|
|
self.assertEqual(observed[210][0], not master_off)
|
|
self.assertFalse(any(observed[tick][0] for tick in range(220, 235)))
|
|
self.assertEqual(observed[240][0], not master_off)
|
|
if master_off:
|
|
self.assertFalse(any(row[3]['latch'] for row in observed.values()))
|