mirror of
https://github.com/firestar5683/StarPilot.git
synced 2026-10-28 00:53:45 +08:00
Separate armed AOL heartbeat from Panda siren activity
This commit is contained in:
firestar5683
@@ -0,0 +1,323 @@
|
||||
"""Host heartbeat -> native USB handlers -> safety RX -> actual 8 Hz siren tick.
|
||||
|
||||
Hardware IO is stubbed. The compiled tick and USB cases are extracted verbatim
|
||||
from production sources, and permissions come from real Toyota CAN RX hooks.
|
||||
"""
|
||||
import ctypes
|
||||
import re
|
||||
import shutil
|
||||
import subprocess
|
||||
from pathlib import Path
|
||||
|
||||
import pytest
|
||||
|
||||
from opendbc.safety.tests.libsafety import libsafety_py
|
||||
from opendbc.safety.tests.test_toyota_secoc_aol import checksum
|
||||
|
||||
ROOT = Path(__file__).resolve().parents[4]
|
||||
|
||||
|
||||
def usb_case(source, request):
|
||||
match = re.search(rf' case 0x{request}:.*?(?= //| case)', source, re.S)
|
||||
assert match is not None
|
||||
return match[0]
|
||||
|
||||
|
||||
@pytest.fixture(scope='module', params=[False, True], ids=['DEBUG', 'RELEASE'])
|
||||
def native_template(tmp_path_factory, request):
|
||||
tmp_path = tmp_path_factory.mktemp('siren-native')
|
||||
main = (ROOT / 'panda/board/main.c').read_text()
|
||||
tick = main[main.index('// go into SILENT'):main.index('\nint main(void)')]
|
||||
comms = (ROOT / 'panda/board/main_comms.h').read_text()
|
||||
cases = ''.join(usb_case(comms, r) for r in ('f3', 'f5', 'f6'))
|
||||
code = r'''
|
||||
#include "opendbc/safety/tests/libsafety/safety.c"
|
||||
uint32_t heartbeat_counter, heartbeat_tick_counter, uptime_cnt, ignition_can_cnt, wake_on_can_cnt;
|
||||
uint32_t voltage = 12000U;
|
||||
bool heartbeat_lost, heartbeat_disabled, siren_enabled, ignition_can, wake_on_can, power_save_enabled;
|
||||
bool ignition_line = true, siren_output;
|
||||
struct { uint32_t SR; } timer;
|
||||
#define TICK_TIMER (&timer)
|
||||
#define HARNESS_STATUS_NC 0U
|
||||
#define HARNESS_STATUS_FLIPPED 2U
|
||||
#define LED_GREEN 0U
|
||||
#define LED_BLUE 1U
|
||||
#define FAULT_RELAY_MALFUNCTION 1U
|
||||
struct { uint8_t status; } harness;
|
||||
void noop(void) {}
|
||||
uint32_t read_voltage(void) { return voltage; }
|
||||
void set_siren(bool enabled) { siren_output = enabled; }
|
||||
void set_ir_power(uint8_t value) { (void)value; }
|
||||
bool read_som_gpio(void) { return true; }
|
||||
struct { uint32_t (*read_voltage_mV)(void); void (*set_siren)(bool);
|
||||
void (*set_ir_power)(uint8_t); bool (*read_som_gpio)(void); } board = {
|
||||
read_voltage, set_siren, set_ir_power, read_som_gpio}, *current_board = &board;
|
||||
#define fan_tick noop
|
||||
#define harness_tick noop
|
||||
#define simple_watchdog_kick noop
|
||||
#define sound_tick noop
|
||||
#define can_init_all noop
|
||||
#define check_registers noop
|
||||
void fault_occurred(uint32_t f) { (void)f; }
|
||||
void fault_recovered(uint32_t f) { (void)f; }
|
||||
void can_set_orientation(bool flipped) { (void)flipped; }
|
||||
void led_set(uint8_t color, bool enabled) { (void)color; (void)enabled; }
|
||||
void bootkick_tick(bool started, bool recent, bool wake) { (void)started; (void)recent; (void)wake; }
|
||||
void fan_set_power(uint8_t power) { (void)power; }
|
||||
void print(const char *s) { (void)s; }
|
||||
void puth(uint32_t n) { (void)n; }
|
||||
bool panda_ignition_line(void) { return ignition_line; }
|
||||
bool is_car_safety_mode(uint16_t mode) {
|
||||
return mode != SAFETY_SILENT && mode != SAFETY_NOOUTPUT && mode != SAFETY_ALLOUTPUT && mode != SAFETY_ELM327;
|
||||
}
|
||||
void set_safety_mode(uint16_t mode, uint16_t param) { set_safety_hooks(mode, param); }
|
||||
void set_power_save_state(bool enabled) { power_save_enabled = enabled; }
|
||||
''' + tick + r'''
|
||||
void test_usb(uint16_t request, uint16_t param1) {
|
||||
struct { uint16_t param1; } packet = {param1}, *req = &packet;
|
||||
switch (request) {
|
||||
''' + cases + r'''
|
||||
default: abort();
|
||||
}
|
||||
}
|
||||
bool test_heartbeat_lost(void) { return heartbeat_lost; }
|
||||
void test_tick(bool powered, bool started) {
|
||||
voltage = powered ? 12000U : 5000U;
|
||||
ignition_line = started;
|
||||
timer.SR = 1U;
|
||||
tick_handler();
|
||||
}
|
||||
bool test_siren(void) { return siren_output; }
|
||||
'''
|
||||
path = tmp_path / 'siren.c'
|
||||
path.write_text(code)
|
||||
lib = tmp_path / 'siren.so'
|
||||
subprocess.run(['cc', '-shared', '-fPIC', '-std=gnu11', '-Wall', '-Werror',
|
||||
*([] if request.param else ['-DALLOW_DEBUG']), '-I', str(ROOT / 'opendbc_repo'),
|
||||
str(path), '-o', str(lib)], check=True)
|
||||
return lib
|
||||
|
||||
|
||||
@pytest.fixture
|
||||
def native(tmp_path, native_template):
|
||||
# A separate load path gives each case pristine static timer/native state.
|
||||
lib = tmp_path / 'siren.so'
|
||||
shutil.copyfile(native_template, lib)
|
||||
safety = libsafety_py.ffi.dlopen(str(lib))
|
||||
io = ctypes.CDLL(str(lib))
|
||||
io.test_usb.argtypes = [ctypes.c_uint16, ctypes.c_uint16]
|
||||
io.test_tick.argtypes = [ctypes.c_bool, ctypes.c_bool]
|
||||
io.test_siren.restype = ctypes.c_bool
|
||||
io.test_heartbeat_lost.restype = ctypes.c_bool
|
||||
yield safety, io
|
||||
|
||||
|
||||
@pytest.fixture(scope='module')
|
||||
def host(tmp_path_factory):
|
||||
"""Compile the actual pandad engaged decision and Panda USB serialization."""
|
||||
tmp = tmp_path_factory.mktemp('host-heartbeat')
|
||||
pandad = (ROOT / 'openpilot/selfdrive/pandad/pandad.cc').read_text()
|
||||
decision_match = re.search(r' if \(aol_write_ok\) \{.*?\n \}', pandad, re.S)
|
||||
assert decision_match is not None
|
||||
decision = decision_match[0]
|
||||
panda = (ROOT / 'openpilot/selfdrive/pandad/panda.cc').read_text()
|
||||
send_match = re.search(r'void Panda::send_heartbeat\(bool engaged\) \{.*?\n\}', panda, re.S)
|
||||
assert send_match is not None
|
||||
send = send_match[0]
|
||||
code = r'''
|
||||
#include "openpilot/starpilot/car/toyota/aol_policy.h"
|
||||
uint16_t usb_request, usb_value;
|
||||
const AolProfileRegistry registry{&TOYOTA_AOL_PROFILE, 1U};
|
||||
AolAxisNegotiator negotiator(registry);
|
||||
AolWritePlan pending;
|
||||
aol_safety_health_t snapshot;
|
||||
struct Handle { int control_write(uint16_t r, uint16_t v, uint16_t unused) {
|
||||
(void)unused; usb_request = r; usb_value = v; return 0; }} storage, *handle = &storage;
|
||||
''' + send.replace('Panda::', '') + r'''
|
||||
extern "C" void host_reset(void) { negotiator = AolAxisNegotiator(registry); }
|
||||
extern "C" uint64_t host_heartbeat(uint16_t word, uint8_t native_request, uint8_t native_permission,
|
||||
uint64_t now, uint8_t mask, bool retain, bool engaged, bool aol_write_ok) {
|
||||
snapshot = {AOL_SAFETY_PROTOCOL_MAGIC, AOL_SAFETY_PROTOCOL_VERSION, native_request,
|
||||
native_permission, 2U, word, 1U};
|
||||
AolAxisInput axis{true, true, "siren-session", now, now + 100000000ULL, now,
|
||||
(mask & 1U) != 0U, (mask & 2U) != 0U, retain};
|
||||
auto plan = negotiator.prepare(snapshot, axis, now, 2U);
|
||||
pending = plan;
|
||||
''' + decision + r'''
|
||||
send_heartbeat(engaged);
|
||||
return (uint64_t(plan.request_mask) << 32) | (uint32_t(usb_request) << 16) | usb_value;
|
||||
}
|
||||
extern "C" void host_complete(uint8_t native_request, uint8_t native_permission) {
|
||||
snapshot.request_mask = native_request;
|
||||
snapshot.permission_mask = native_permission;
|
||||
negotiator.complete(pending, true, snapshot);
|
||||
}
|
||||
'''
|
||||
source = tmp / 'host.cc'
|
||||
source.write_text(code)
|
||||
lib = tmp / 'host.so'
|
||||
subprocess.run(['c++', '-shared', '-fPIC', '-std=c++17', '-Wall', '-Werror', '-I', str(ROOT), '-I', str(ROOT / 'openpilot'),
|
||||
str(source), '-o', str(lib)], check=True)
|
||||
dll = ctypes.CDLL(str(lib))
|
||||
dll.host_heartbeat.argtypes = [ctypes.c_uint16, ctypes.c_uint8, ctypes.c_uint8, ctypes.c_uint64,
|
||||
ctypes.c_uint8, ctypes.c_bool, ctypes.c_bool, ctypes.c_bool]
|
||||
dll.host_heartbeat.restype = ctypes.c_uint64
|
||||
dll.host_complete.argtypes = [ctypes.c_uint8, ctypes.c_uint8]
|
||||
return dll
|
||||
|
||||
|
||||
class Loop:
|
||||
def __init__(self, native, host, word):
|
||||
self.safety, self.io = native
|
||||
self.host = host
|
||||
self.word = word
|
||||
self.host.host_reset()
|
||||
self.time = 1_000_000
|
||||
self.safety.init_tests()
|
||||
self.safety.set_alternative_experience(32)
|
||||
assert self.safety.set_safety_hooks(2, word) == 0
|
||||
|
||||
def rx(self, addr, data, crc=False):
|
||||
if crc:
|
||||
data = checksum(addr, data)
|
||||
assert self.safety.safety_rx_hook(libsafety_py.make_CANPacket(addr, 0, data))
|
||||
|
||||
def step(self, *, gear='drive', cruise=False, main=True, mask=1, retain=True,
|
||||
powered=True, started=True, heartbeat=True, ordinary=False, no_output=False):
|
||||
# Synthetic CAN at 5 ms, host negotiation/heartbeat at the actual 10 Hz,
|
||||
# and the production timer IRQ at 8 Hz.
|
||||
for _ in range(25):
|
||||
self.time += 5000
|
||||
self.safety.set_timer(self.time)
|
||||
if heartbeat and (self.time - 1_005_000) % 100_000 == 0:
|
||||
packed = self.host.host_heartbeat(self.word, self.safety.aol_get_request_mask(),
|
||||
self.safety.aol_get_permission_mask(), self.time * 1000,
|
||||
mask, retain, ordinary, True)
|
||||
self.io.test_usb(0xf5, packed >> 32)
|
||||
self.io.test_usb((packed >> 16) & 0xffff, packed & 0xffff)
|
||||
self.host.host_complete(self.safety.aol_get_request_mask(), self.safety.aol_get_permission_mask())
|
||||
self.rx(0xAA, bytes([0x1A, 0x6F] * 4))
|
||||
self.rx(0x260, bytes(8), True)
|
||||
self.rx(0x1D2, bytes([16 | (32 if cruise else 0)]) + bytes(7), True)
|
||||
self.rx(0x226, bytes(8))
|
||||
self.rx(0x1D3, bytes([0, 128 if main else 0]) + bytes(6), True)
|
||||
self.rx(0x620, bytes(8))
|
||||
self.rx(0x262, bytes(3) + bytes([2]) + bytes(4), True)
|
||||
self.rx(0x3BC, bytes([0, {'drive': 0, 'park': 32, 'reverse': 16}[gear]]) + bytes(6))
|
||||
if no_output:
|
||||
assert self.safety.set_safety_hooks(19, 0) == 0
|
||||
self.io.test_tick(powered, started)
|
||||
return self.io.test_siren()
|
||||
|
||||
|
||||
@pytest.mark.parametrize('word', [73, 585, 4169, 4681], ids=['RAV4-OP', 'RAV4-stock', 'Prius-OP', 'Prius-stock'])
|
||||
@pytest.mark.parametrize('gear', ['park', 'reverse'])
|
||||
def test_armed_pause_shutdown_quiet(native, host, word, gear):
|
||||
loop = Loop(native, host, word)
|
||||
# Active AOL, followed by an indefinitely armed physical pause. Include stale
|
||||
# PCM active to ensure native gear denial wins over ordinary enabled state.
|
||||
loop.step(cruise=True)
|
||||
assert loop.safety.aol_get_permission_mask() & 1
|
||||
for _ in range(48):
|
||||
assert not loop.step(gear=gear, cruise=True, mask=0, ordinary=True)
|
||||
assert loop.safety.aol_get_permission_mask() == 0
|
||||
assert loop.safety.get_controls_allowed()
|
||||
assert not any(loop.step(gear=gear, mask=0, heartbeat=False, powered=False, started=False) for _ in range(24))
|
||||
|
||||
|
||||
@pytest.mark.parametrize('word', [73, 585, 4169, 4681])
|
||||
@pytest.mark.parametrize('loss', ['power', 'heartbeat', 'park', 'noOutput'])
|
||||
def test_last_active_loss_still_warns(native, host, word, loss):
|
||||
loop = Loop(native, host, word)
|
||||
# First nine ticks inactive put the active boundary away from the 1 Hz sample.
|
||||
for _ in range(9):
|
||||
loop.step(gear='park', mask=0)
|
||||
loop.step(cruise=True, mask=3)
|
||||
assert loop.safety.aol_get_permission_mask() & 1
|
||||
alarms = []
|
||||
for _ in range(24 if loss == 'heartbeat' else 8):
|
||||
alarms.append(loop.step(gear='park' if loss == 'park' else 'drive', mask=0,
|
||||
heartbeat=False, powered=loss == 'heartbeat', started=False,
|
||||
no_output=loss == 'noOutput'))
|
||||
assert any(alarms)
|
||||
|
||||
|
||||
@pytest.mark.parametrize('word', [73, 585, 4169, 4681])
|
||||
def test_last_active_window_expires_without_disarming(native, host, word):
|
||||
loop = Loop(native, host, word)
|
||||
loop.step(cruise=True)
|
||||
assert loop.safety.aol_get_permission_mask() & 1
|
||||
for _ in range(40):
|
||||
assert not loop.step(gear='park', mask=0)
|
||||
assert not any(loop.step(gear='park', mask=0, heartbeat=False, powered=False, started=False) for _ in range(24))
|
||||
|
||||
|
||||
@pytest.mark.parametrize('word,mask', [(73, 1), (73, 2), (73, 3), (585, 1), (4169, 2), (4681, 1)])
|
||||
@pytest.mark.parametrize('paused_ticks,expected', [(0, True), (36, True), (40, False)])
|
||||
def test_exact_last_authority_window(native, host, word, mask, paused_ticks, expected):
|
||||
loop = Loop(native, host, word)
|
||||
loop.step(gear='park', mask=0)
|
||||
loop.step(cruise=True, mask=mask)
|
||||
assert loop.safety.aol_get_permission_mask() == mask
|
||||
for _ in range(paused_ticks):
|
||||
assert not loop.step(gear='park', mask=0)
|
||||
alarms = [loop.step(gear='park', mask=0, heartbeat=False, powered=False, started=False) for _ in range(8)]
|
||||
assert any(alarms) == expected
|
||||
|
||||
|
||||
@pytest.mark.parametrize('word,mask,gear', [(73, 0, 'drive'), (585, 2, 'drive'), (73, 3, 'park'), (585, 3, 'reverse')])
|
||||
def test_never_active_armed_or_denied_request(native, host, word, mask, gear):
|
||||
loop = Loop(native, host, word)
|
||||
for _ in range(48):
|
||||
assert not loop.step(gear=gear, mask=mask, cruise=True, ordinary=True)
|
||||
assert loop.safety.aol_get_permission_mask() == 0
|
||||
assert not any(loop.step(gear=gear, heartbeat=False, mask=0, powered=False, started=False) for _ in range(24))
|
||||
|
||||
|
||||
def test_no_output_stale_engaged_and_manual_siren(native, host):
|
||||
loop = Loop(native, host, 73)
|
||||
for _ in range(48):
|
||||
assert not loop.step(gear='park', ordinary=True, mask=0, no_output=True)
|
||||
assert not any(loop.step(heartbeat=False, powered=False, started=False, mask=0) for _ in range(24))
|
||||
loop.io.test_usb(0xf6, 1)
|
||||
assert any(loop.step(heartbeat=False, started=False, mask=0) for _ in range(8))
|
||||
|
||||
|
||||
def test_ordinary_non_aol_engagement_still_warns(native, host):
|
||||
loop = Loop(native, host, 73)
|
||||
loop.safety.set_alternative_experience(0)
|
||||
assert loop.safety.set_safety_hooks(2, 73) == 0
|
||||
loop.step(cruise=True, ordinary=True, retain=False, mask=0)
|
||||
assert loop.safety.get_controls_allowed()
|
||||
assert any(loop.step(heartbeat=False, powered=False, started=False, mask=0) for _ in range(8))
|
||||
|
||||
|
||||
@pytest.mark.parametrize('word', [73, 585, 4169, 4681])
|
||||
def test_driving_heartbeat_timeout_with_normal_voltage(native, host, word):
|
||||
loop = Loop(native, host, word)
|
||||
loop.step(cruise=True, mask=3)
|
||||
assert loop.safety.aol_get_permission_mask() & 1
|
||||
alarms = [loop.step(cruise=True, heartbeat=False, powered=True, started=True) for _ in range(48)]
|
||||
assert any(alarms)
|
||||
assert loop.io.test_heartbeat_lost()
|
||||
assert loop.safety.get_current_safety_mode() == 0
|
||||
|
||||
|
||||
@pytest.mark.parametrize('word', [73, 585, 4169, 4681])
|
||||
def test_armed_pause_drive_resume_restores_warning(native, host, word):
|
||||
loop = Loop(native, host, word)
|
||||
for _ in range(48):
|
||||
assert not loop.step(gear='park', mask=0)
|
||||
loop.step(cruise=True, mask=3)
|
||||
assert loop.safety.aol_get_permission_mask() == (1 if word in (585, 4681) else 3)
|
||||
assert any(loop.step(heartbeat=False, powered=False, started=False) for _ in range(8))
|
||||
|
||||
|
||||
@pytest.mark.parametrize('word', [73, 585])
|
||||
def test_main_off_workaround_expires_last_activity(native, host, word):
|
||||
loop = Loop(native, host, word)
|
||||
loop.step(cruise=True, mask=3)
|
||||
assert loop.safety.aol_get_permission_mask() & 1
|
||||
for _ in range(40):
|
||||
assert not loop.step(main=False, cruise=False, mask=0, retain=False)
|
||||
assert not any(loop.step(main=False, heartbeat=False, powered=False, started=False) for _ in range(24))
|
||||
+15
-8
@@ -118,6 +118,21 @@ static void tick_handler(void) {
|
||||
|
||||
heartbeat_tick_counter += 1U;
|
||||
|
||||
// AOL's heartbeat also retains an armed latch while both axes are paused.
|
||||
// Only native actuator authority refreshes its warning window. Preserve the
|
||||
// last active interval across Park, noOutput and heartbeat/authority loss;
|
||||
// sample at 8 Hz so a subsecond active interval is not missed at shutdown.
|
||||
bool warning_engaged = controls_allowed || heartbeat_engaged;
|
||||
if (aol_policy != NULL) {
|
||||
warning_engaged = aol_get_permission_mask() != 0U;
|
||||
}
|
||||
if (is_car_safety_mode(current_safety_mode) && warning_engaged) {
|
||||
controls_allowed_countdown = 5U * 8U;
|
||||
} else if (controls_allowed_countdown > 0U) {
|
||||
controls_allowed_countdown -= 1U;
|
||||
} else {
|
||||
}
|
||||
|
||||
// start the siren early on power loss
|
||||
if ((controls_allowed_countdown > 0U) && (heartbeat_tick_counter > 2U) && (current_board->read_voltage_mV() < 6000U)) {
|
||||
siren_countdown = 3U;
|
||||
@@ -184,14 +199,6 @@ static void tick_handler(void) {
|
||||
siren_countdown -= 1U;
|
||||
}
|
||||
|
||||
if (controls_allowed || heartbeat_engaged) {
|
||||
controls_allowed_countdown = 5U;
|
||||
} else if (controls_allowed_countdown > 0U) {
|
||||
controls_allowed_countdown -= 1U;
|
||||
} else {
|
||||
|
||||
}
|
||||
|
||||
// exit controls allowed if unused by openpilot for a few seconds
|
||||
if (controls_allowed && !heartbeat_engaged) {
|
||||
heartbeat_engaged_mismatches += 1U;
|
||||
|
||||
Reference in New Issue
Block a user