diff --git a/openpilot/starpilot/aol/tests/test_panda_siren.py b/openpilot/starpilot/aol/tests/test_panda_siren.py new file mode 100644 index 0000000000..4bdf9543d0 --- /dev/null +++ b/openpilot/starpilot/aol/tests/test_panda_siren.py @@ -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)) diff --git a/panda/board/main.c b/panda/board/main.c index 7e0bf6646b..0546033072 100644 --- a/panda/board/main.c +++ b/panda/board/main.c @@ -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;