mirror of
https://github.com/firestar5683/StarPilot.git
synced 2026-11-01 19:43:51 +08:00
Compare commits
1 Commits
| Author | SHA1 | Date | |
|---|---|---|---|
| 829aed275e |
@@ -1,6 +0,0 @@
|
||||
github: firestar5683
|
||||
patreon: firestar4430
|
||||
custom:
|
||||
- "https://cash.app/$domkthompson"
|
||||
- "https://account.venmo.com/u/firestar4430"
|
||||
- "https://www.paypal.com/paypalme/firestar4430"
|
||||
@@ -287,8 +287,6 @@ def prune_cache_dir(target=None, source=None, env=None):
|
||||
|
||||
# ********** start building stuff **********
|
||||
|
||||
SConscript(['opendbc_repo/opendbc/can/SConscript'])
|
||||
|
||||
# Build common module
|
||||
SConscript(['openpilot/common/SConscript'])
|
||||
Import('_common')
|
||||
|
||||
Binary file not shown.
@@ -1,6 +0,0 @@
|
||||
Import('envCython')
|
||||
|
||||
raw_value = envCython.Clone(SHLIBPREFIX='', SHLIBSUFFIX='.so')
|
||||
raw_value.Append(CFLAGS=['-std=c11'], CCFLAGS=['-O3', '-Werror'])
|
||||
raw_value.Replace(SHLINKFLAGS=['$LINKFLAGS'])
|
||||
raw_value.SharedLibrary('_raw_value', ['_raw_value.c'])
|
||||
@@ -1,135 +0,0 @@
|
||||
#define PY_SSIZE_T_CLEAN
|
||||
#include <Python.h>
|
||||
#include <stdint.h>
|
||||
|
||||
typedef struct {
|
||||
PyObject *names[4];
|
||||
} ExtractState;
|
||||
|
||||
static PyObject *get_raw_value(PyObject *self, PyObject *args) {
|
||||
ExtractState *state = PyModule_GetState(self);
|
||||
PyObject *signal, *payload, *expected;
|
||||
PyObject *msb_obj = NULL, *lsb_obj = NULL, *size_obj = NULL, *little_obj = NULL;
|
||||
const unsigned char *data;
|
||||
Py_ssize_t length;
|
||||
long msb, lsb, size, little, first, last;
|
||||
if (!PyArg_UnpackTuple(args, "get_raw_value", 3, 3, &signal, &payload, &expected)) return NULL;
|
||||
if (state == NULL || state->names[0] == NULL || state->names[1] == NULL ||
|
||||
state->names[2] == NULL || state->names[3] == NULL) goto unsupported;
|
||||
if (!PyType_Check(expected) || Py_TYPE(signal) != (PyTypeObject *)expected) goto unsupported;
|
||||
if (!PyBytes_CheckExact(payload) && !PyByteArray_CheckExact(payload)) goto unsupported;
|
||||
msb_obj = PyObject_GetAttr(signal, state->names[0]);
|
||||
if (msb_obj == NULL) goto unsupported;
|
||||
lsb_obj = PyObject_GetAttr(signal, state->names[1]);
|
||||
if (lsb_obj == NULL) goto unsupported;
|
||||
size_obj = PyObject_GetAttr(signal, state->names[2]);
|
||||
if (size_obj == NULL) goto unsupported;
|
||||
little_obj = PyObject_GetAttr(signal, state->names[3]);
|
||||
if (little_obj == NULL) goto unsupported;
|
||||
if (!PyLong_CheckExact(msb_obj) || !PyLong_CheckExact(lsb_obj) ||
|
||||
!PyLong_CheckExact(size_obj) || !PyBool_Check(little_obj)) goto unsupported;
|
||||
msb = PyLong_AsLong(msb_obj);
|
||||
if (PyErr_Occurred()) goto unsupported;
|
||||
lsb = PyLong_AsLong(lsb_obj);
|
||||
if (PyErr_Occurred()) goto unsupported;
|
||||
size = PyLong_AsLong(size_obj);
|
||||
if (PyErr_Occurred()) goto unsupported;
|
||||
little = little_obj == Py_True;
|
||||
/* Attribute access may execute Python; acquire mutable storage only afterward. */
|
||||
if (PyBytes_CheckExact(payload)) {
|
||||
data = (const unsigned char *)PyBytes_AS_STRING(payload);
|
||||
length = PyBytes_GET_SIZE(payload);
|
||||
} else {
|
||||
data = (const unsigned char *)PyByteArray_AS_STRING(payload);
|
||||
length = PyByteArray_GET_SIZE(payload);
|
||||
}
|
||||
if (msb < 0 || msb >= 512 || lsb < 0 || lsb >= 512 || size <= 0 || size > 64) goto unsupported;
|
||||
if (little) {
|
||||
first = lsb / 8;
|
||||
last = msb / 8;
|
||||
if (size != msb - lsb + 1) goto unsupported;
|
||||
} else {
|
||||
first = msb / 8;
|
||||
last = lsb / 8;
|
||||
if (size != 8 * (last - first) + msb % 8 - lsb % 8 + 1) goto unsupported;
|
||||
}
|
||||
if (first < 0 || first > last || last >= length) goto unsupported;
|
||||
uint64_t result = 0;
|
||||
long remaining = size;
|
||||
for (long byte = msb / 8; remaining > 0; byte += little ? -1 : 1) {
|
||||
if (byte < 0 || byte >= length) goto unsupported;
|
||||
const long low = lsb / 8 == byte ? lsb : byte * 8;
|
||||
const long high = msb / 8 == byte ? msb : (byte + 1) * 8 - 1;
|
||||
const long width = high - low + 1;
|
||||
const long bit_shift = low - byte * 8;
|
||||
const long result_shift = remaining - width;
|
||||
if (width <= 0 || width > 8 || bit_shift < 0 || bit_shift > 7 ||
|
||||
result_shift < 0 || result_shift > 63) goto unsupported;
|
||||
const unsigned int fragment = (data[byte] >> bit_shift) & ((1U << width) - 1U);
|
||||
result |= (uint64_t)fragment << result_shift;
|
||||
remaining -= width;
|
||||
}
|
||||
Py_DECREF(msb_obj);
|
||||
Py_DECREF(lsb_obj);
|
||||
Py_DECREF(size_obj);
|
||||
Py_DECREF(little_obj);
|
||||
return PyLong_FromUnsignedLongLong((unsigned long long)result);
|
||||
|
||||
unsupported:
|
||||
Py_XDECREF(msb_obj);
|
||||
Py_XDECREF(lsb_obj);
|
||||
Py_XDECREF(size_obj);
|
||||
Py_XDECREF(little_obj);
|
||||
PyErr_Clear();
|
||||
Py_INCREF(Py_NotImplemented);
|
||||
return Py_NotImplemented;
|
||||
}
|
||||
|
||||
static PyMethodDef methods[] = {
|
||||
{"get_raw_value", get_raw_value, METH_VARARGS, "Extract an exact complete <=64-bit CAN signal, or NotImplemented."},
|
||||
{NULL, NULL, 0, NULL},
|
||||
};
|
||||
static int module_traverse(PyObject *self, visitproc visit, void *arg) {
|
||||
ExtractState *state = PyModule_GetState(self);
|
||||
if (state != NULL) {
|
||||
for (int i = 0; i < 4; ++i) Py_VISIT(state->names[i]);
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
static int module_clear(PyObject *self) {
|
||||
ExtractState *state = PyModule_GetState(self);
|
||||
if (state != NULL) {
|
||||
for (int i = 0; i < 4; ++i) Py_CLEAR(state->names[i]);
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
static void module_free(void *self) {
|
||||
(void)module_clear((PyObject *)self);
|
||||
}
|
||||
|
||||
static struct PyModuleDef module = {
|
||||
PyModuleDef_HEAD_INIT,
|
||||
.m_name = "_raw_value",
|
||||
.m_doc = "Complete raw CAN signal bit extraction.",
|
||||
.m_size = sizeof(ExtractState),
|
||||
.m_methods = methods,
|
||||
.m_traverse = module_traverse,
|
||||
.m_clear = module_clear,
|
||||
.m_free = module_free,
|
||||
};
|
||||
PyMODINIT_FUNC PyInit__raw_value(void) {
|
||||
PyObject *created = PyModule_Create(&module);
|
||||
if (created == NULL) return NULL;
|
||||
ExtractState *state = PyModule_GetState(created);
|
||||
static const char *names[4] = {"msb", "lsb", "size", "is_little_endian"};
|
||||
for (int i = 0; i < 4; ++i) {
|
||||
state->names[i] = PyUnicode_InternFromString(names[i]);
|
||||
if (state->names[i] == NULL) {
|
||||
Py_DECREF(created);
|
||||
return NULL;
|
||||
}
|
||||
}
|
||||
return created;
|
||||
}
|
||||
Binary file not shown.
@@ -6,11 +6,6 @@ from functools import cache
|
||||
|
||||
from opendbc import DBC_PATH, get_generated_dbcs
|
||||
|
||||
try:
|
||||
from opendbc.can._raw_value import get_raw_value as _native_raw_value
|
||||
except ImportError:
|
||||
_native_raw_value = None
|
||||
|
||||
# TODO: these should just be passed in along with the DBC file
|
||||
from opendbc.car.honda.hondacan import honda_checksum
|
||||
from opendbc.car.toyota.toyotacan import toyota_checksum
|
||||
@@ -66,10 +61,6 @@ class Signal:
|
||||
checksum_fields: tuple['Signal', ...] = ()
|
||||
|
||||
def get_raw_value(self, dat: bytes | bytearray) -> int:
|
||||
if _native_raw_value is not None:
|
||||
value = _native_raw_value(self, dat, Signal)
|
||||
if value is not NotImplemented:
|
||||
return value
|
||||
msb_byte = self.msb // 8
|
||||
lsb_byte = self.lsb // 8
|
||||
if self.is_little_endian:
|
||||
|
||||
@@ -56,18 +56,6 @@ class CANPacker:
|
||||
|
||||
|
||||
def set_value(msg: bytearray, sig: Signal, ival: int) -> None:
|
||||
if type(msg) is bytearray and type(sig) is Signal and type(ival) is int:
|
||||
lsb = getattr(sig, 'lsb', None)
|
||||
size = getattr(sig, 'size', None)
|
||||
little_endian = getattr(sig, 'is_little_endian', None)
|
||||
if type(lsb) is int and type(size) is int and type(little_endian) is bool:
|
||||
i, shift = lsb // 8, lsb % 8
|
||||
if 0 < size <= 8 - shift and 0 <= i < len(msg):
|
||||
field_mask = (1 << size) - 1
|
||||
mask = field_mask << shift
|
||||
msg[i] = (msg[i] & ~mask) | ((ival & field_mask) << shift)
|
||||
return
|
||||
|
||||
i = sig.lsb // 8
|
||||
bits = sig.size
|
||||
if sig.size < 64:
|
||||
|
||||
@@ -1,146 +0,0 @@
|
||||
import builtins
|
||||
import importlib.util
|
||||
import sys
|
||||
|
||||
import pytest
|
||||
|
||||
from opendbc.can import dbc
|
||||
from opendbc.can.tests.test_signal_raw_value import make_signal, raw_reference
|
||||
|
||||
|
||||
@pytest.fixture(params=['python', 'native'])
|
||||
def decoder(request, monkeypatch):
|
||||
native = dbc._native_raw_value
|
||||
if request.param == 'native' and native is None:
|
||||
pytest.skip('optional raw CAN extension is not built')
|
||||
monkeypatch.setattr(dbc, '_native_raw_value', native if request.param == 'native' else None)
|
||||
return dbc.Signal
|
||||
|
||||
|
||||
@pytest.fixture
|
||||
def native():
|
||||
if dbc._native_raw_value is None:
|
||||
pytest.skip('optional raw CAN extension is not built')
|
||||
return dbc._native_raw_value
|
||||
|
||||
|
||||
@pytest.mark.parametrize('little', [False, True])
|
||||
@pytest.mark.parametrize('container', [bytes, bytearray])
|
||||
def test_raw_widths_offsets_and_unsigned_high_bit(decoder, little, container):
|
||||
data = container(bytes(range(64)))
|
||||
for base in (0, 16, 55):
|
||||
for alignment in range(8):
|
||||
start = base * 8 + alignment
|
||||
available = 512 - start if little else alignment + 1 + 8 * (63 - base)
|
||||
for size in range(1, min(64, available) + 1):
|
||||
signal = make_signal(start, size, little)
|
||||
assert signal.get_raw_value(data) == raw_reference(data, start, size, little)
|
||||
signal = make_signal(7 if little else 0, 64, little)
|
||||
assert signal.get_raw_value(container(b'\xff' * 9)) == 2**64 - 1
|
||||
|
||||
|
||||
@pytest.mark.parametrize('msb,lsb,size,little,payload,expected', [
|
||||
(23, 0, 24, True, b'\xab\xcd', 0),
|
||||
(7, 16, 24, False, b'\xab\xcd', 0xABCD00),
|
||||
(15, 0, 8, True, b'\xab\xcd', 0xCD),
|
||||
(7, 16, 16, False, b'\xab\xcd\xef', 0xABCD),
|
||||
(7, 0, 0, True, b'\xff', 0),
|
||||
(7, 0, -1, False, b'\xff', 0),
|
||||
(23, 24, 16, False, b'\xab\xcd', 0),
|
||||
(7, 0, 8, True, b'', 0),
|
||||
])
|
||||
def test_raw_partial_and_noncanonical_fallback(decoder, msb, lsb, size, little, payload, expected):
|
||||
signal = decoder('TEST', msb, msb, lsb, size, False, 1., 0., little)
|
||||
assert signal.get_raw_value(payload) == expected
|
||||
|
||||
|
||||
def test_raw_full_width_python_fallback(decoder):
|
||||
for little in (False, True):
|
||||
signal = make_signal(0 if little else 7, 512, little)
|
||||
assert signal.get_raw_value(b'\xff' * 64) == 2**512 - 1
|
||||
|
||||
|
||||
def test_raw_noncanonical_exception_order(decoder):
|
||||
for msb, lsb, size, little in ((7, 4, 2, True), (3, 0, 2, False)):
|
||||
signal = decoder('TEST', msb, msb, lsb, size, False, 1., 0., little)
|
||||
with pytest.raises(ValueError, match='negative shift count'):
|
||||
signal.get_raw_value(b'\xff')
|
||||
|
||||
|
||||
def test_raw_geometry_updates_and_custom_values(decoder):
|
||||
class EqualInt(int):
|
||||
pass
|
||||
|
||||
signal = make_signal(0, 16, True)
|
||||
assert signal.get_raw_value(b'\xab\xcd') == 0xCDAB
|
||||
signal.size = 8
|
||||
assert signal.get_raw_value(b'\xab\xcd') == 0xCD
|
||||
signal.__dict__.update(msb=7, lsb=0, size=8, is_little_endian=True)
|
||||
assert signal.get_raw_value(b'\xab\xcd') == 0xAB
|
||||
signal.__dict__['size'] = EqualInt(8)
|
||||
assert signal.get_raw_value(b'\xab\xcd') == 0xAB
|
||||
signal.__dict__['msb'] = 2**100
|
||||
assert signal.get_raw_value(b'\xab\xcd') == 0
|
||||
|
||||
|
||||
@pytest.mark.parametrize('container', [memoryview, list])
|
||||
def test_raw_custom_payload_falls_back(decoder, container):
|
||||
signal = make_signal(0, 16, True)
|
||||
assert signal.get_raw_value(container(b'\xab\xcd')) == 0xCDAB
|
||||
|
||||
|
||||
def test_native_rejects_subclass_without_reading_fields(native):
|
||||
class CustomSignal(dbc.Signal):
|
||||
def __getattribute__(self, name):
|
||||
if name in ('msb', 'lsb', 'size', 'is_little_endian'):
|
||||
raise AssertionError('native must not read subclass fields')
|
||||
return super().__getattribute__(name)
|
||||
|
||||
signal = CustomSignal('CUSTOM', 0, 7, 0, 8, False, 1., 0., True)
|
||||
assert native(signal, b'\xab', dbc.Signal) is NotImplemented
|
||||
|
||||
|
||||
def test_native_keeps_mutable_payload_after_attribute_callbacks(native):
|
||||
class CallbackSignal:
|
||||
lsb, size, is_little_endian = 0, 8, True
|
||||
|
||||
def __init__(self, payload, length):
|
||||
self.payload, self.length = payload, length
|
||||
|
||||
@property
|
||||
def msb(self):
|
||||
self.payload[:] = b'\xcd' * self.length
|
||||
return 7
|
||||
|
||||
for length in (0, 4096):
|
||||
payload = bytearray(b'\xab' * 64)
|
||||
result = native(CallbackSignal(payload, length), payload, CallbackSignal)
|
||||
assert result is NotImplemented if length == 0 else result == 0xCD
|
||||
|
||||
|
||||
def test_native_unsupported_keeps_attribute_references(native):
|
||||
value = int('1000000000000000000000000000000000000000')
|
||||
signal = dbc.Signal('REFS', 0, value, 0, 8, False, 1., 0., True)
|
||||
before = sys.getrefcount(value)
|
||||
for _ in range(1000):
|
||||
assert native(signal, b'\xff', dbc.Signal) is NotImplemented
|
||||
assert sys.getrefcount(value) == before
|
||||
|
||||
|
||||
def test_python_import_without_native_extension(monkeypatch):
|
||||
original_import = builtins.__import__
|
||||
|
||||
def without_native(name, module_globals=None, module_locals=None, fromlist=(), level=0):
|
||||
if name == 'opendbc.can._raw_value':
|
||||
raise ImportError('optional extension unavailable')
|
||||
return original_import(name, module_globals, module_locals, fromlist, level)
|
||||
|
||||
name = '_dbc_python_fallback_test'
|
||||
spec = importlib.util.spec_from_file_location(name, dbc.__file__)
|
||||
module = importlib.util.module_from_spec(spec)
|
||||
monkeypatch.setitem(sys.modules, name, module)
|
||||
monkeypatch.setattr(builtins, '__import__', without_native)
|
||||
spec.loader.exec_module(module)
|
||||
assert module._native_raw_value is None
|
||||
signal = module.Signal('FALLBACK', 0, 15, 0, 16, False, 1., 0., True)
|
||||
assert signal.get_raw_value(b'\xab\xcd') == 0xCDAB
|
||||
@@ -1,186 +0,0 @@
|
||||
"""Single-byte packing preserves bits and the original fallback semantics."""
|
||||
|
||||
import unittest
|
||||
|
||||
from opendbc.can.dbc import Signal
|
||||
from opendbc.can.packer import set_value
|
||||
|
||||
|
||||
def reference_set_value(msg, sig, ival):
|
||||
# Frozen pre-optimization setter; the private AST checker binds this to source.
|
||||
i = sig.lsb // 8
|
||||
bits = sig.size
|
||||
if sig.size < 64:
|
||||
ival &= (1 << sig.size) - 1
|
||||
while 0 <= i < len(msg) and bits > 0:
|
||||
shift = sig.lsb % 8 if (sig.lsb // 8) == i else 0
|
||||
size = min(bits, 8 - shift)
|
||||
mask = ((1 << size) - 1) << shift
|
||||
msg[i] &= ~mask
|
||||
msg[i] |= (ival & ((1 << size) - 1)) << shift
|
||||
bits -= size
|
||||
ival >>= size
|
||||
i = i + 1 if sig.is_little_endian else i - 1
|
||||
|
||||
|
||||
def signal(lsb, size, little=True, kind=Signal):
|
||||
return kind('TEST', lsb, lsb + size - 1, lsb, size, False, 1., 0., little)
|
||||
|
||||
|
||||
def observed(function, factory):
|
||||
msg, sig, value, events = factory()
|
||||
try:
|
||||
result = function(msg, sig, value)
|
||||
error = None
|
||||
except Exception as exc:
|
||||
result, error = None, (type(exc).__name__, str(exc))
|
||||
return bytes(msg), result, error, events
|
||||
|
||||
|
||||
class TestPackerSingleByte(unittest.TestCase):
|
||||
def compare(self, factory):
|
||||
self.assertEqual(observed(set_value, factory), observed(reference_set_value, factory))
|
||||
|
||||
def test_contained_fields_preserve_neighbor_bits_both_endiannesses(self):
|
||||
values = (0, 1, -1, 0x55, 0xAA, 1 << 96, -(1 << 96))
|
||||
for little in (False, True):
|
||||
for initial in (0, 0xA5, 255):
|
||||
for shift in range(8):
|
||||
for size in range(1, 9 - shift):
|
||||
for value in values:
|
||||
with self.subTest(little=little, initial=initial, shift=shift, size=size, value=value):
|
||||
msg = bytearray((0x3C, initial, 0xC3))
|
||||
mask = ((1 << size) - 1) << shift
|
||||
self.assertIsNone(set_value(msg, signal(8 + shift, size, little), value))
|
||||
self.assertEqual(msg, bytearray((0x3C, (initial & ~mask) | ((value << shift) & mask), 0xC3)))
|
||||
|
||||
def test_cross_byte_wide_zero_negative_and_clipped_geometry(self):
|
||||
geometry = ((0, 0), (0, -1), (-1, 1), (-8, 8), (8, 1), (24, 8),
|
||||
(0, 9), (7, 2), (4, 16), (0, 63), (0, 64), (7, 65), (16, 128))
|
||||
for little in (False, True):
|
||||
for lsb, size in geometry:
|
||||
for length in (0, 1, 3, 17):
|
||||
for value in (-1, 0, 0x55, 1 << 130, -(1 << 130)):
|
||||
with self.subTest(little=little, lsb=lsb, size=size, length=length, value=value):
|
||||
self.compare(lambda length=length, lsb=lsb, size=size, little=little, value=value:
|
||||
(bytearray([0xA5] * length), signal(lsb, size, little), value, []))
|
||||
|
||||
def test_mutated_geometry_is_read_for_each_call(self):
|
||||
sig = signal(0, 1)
|
||||
for lsb, size, little in ((0, 1, True), (9, 6, False), (7, 9, True), (16, 64, False)):
|
||||
sig.lsb, sig.size, sig.is_little_endian = lsb, size, little
|
||||
expected, actual = bytearray(b'\xA5\x5A\x96'), bytearray(b'\xA5\x5A\x96')
|
||||
reference_set_value(expected, sig, -7)
|
||||
set_value(actual, sig, -7)
|
||||
self.assertEqual(actual, expected)
|
||||
|
||||
def test_nonexact_buffers_preserve_fallback_writes_and_errors(self):
|
||||
class Buffer(bytearray):
|
||||
def __getitem__(self, key):
|
||||
self.events.append(('get', key))
|
||||
return super().__getitem__(key)
|
||||
|
||||
def __setitem__(self, key, value):
|
||||
self.events.append(('set', key, value))
|
||||
return super().__setitem__(key, value)
|
||||
|
||||
def tracked():
|
||||
msg = Buffer(b'\xA5\x5A')
|
||||
msg.events = []
|
||||
return msg, signal(2, 3), 7, msg.events
|
||||
|
||||
self.compare(tracked)
|
||||
for container in (bytes, list, lambda data: memoryview(bytearray(data))):
|
||||
self.compare(lambda container=container: (container(b'\xA5\x5A'), signal(2, 3), 7, []))
|
||||
|
||||
def test_signal_subclass_attribute_side_effects_keep_original_order(self):
|
||||
class TrackedSignal(Signal):
|
||||
def __getattribute__(self, name):
|
||||
if name in ('lsb', 'size', 'is_little_endian'):
|
||||
object.__getattribute__(self, 'events').append(name)
|
||||
return super().__getattribute__(name)
|
||||
|
||||
def factory():
|
||||
sig = signal(3, 10, False, TrackedSignal)
|
||||
sig.events = []
|
||||
return bytearray(b'\xA5\x5A'), sig, -1, sig.events
|
||||
|
||||
self.compare(factory)
|
||||
|
||||
def test_nonexact_integer_values_and_fields_keep_original_operations(self):
|
||||
class Integer(int):
|
||||
def __and__(self, other):
|
||||
self.events.append(('and', other))
|
||||
return int(self) & other
|
||||
|
||||
def __floordiv__(self, other):
|
||||
self.events.append(('floordiv', other))
|
||||
return int(self) // other
|
||||
|
||||
def __mod__(self, other):
|
||||
self.events.append(('mod', other))
|
||||
return int(self) % other
|
||||
|
||||
def __lt__(self, other):
|
||||
self.events.append(('lt', other))
|
||||
return int(self) < other
|
||||
|
||||
for field in ('value', 'lsb', 'size'):
|
||||
def factory(field=field):
|
||||
events = []
|
||||
number = Integer({'value': 7, 'lsb': 2, 'size': 3}[field])
|
||||
number.events = events
|
||||
sig = signal(number if field == 'lsb' else 2, number if field == 'size' else 3)
|
||||
return bytearray(b'\xA5\x5A'), sig, number if field == 'value' else 7, events
|
||||
self.compare(factory)
|
||||
for value in (True, False, 1.5, None):
|
||||
self.compare(lambda value=value: (bytearray(b'\xA5'), signal(2, 3), value, []))
|
||||
for lsb, size, little in ((True, 3, True), (2, True, False), (2., 3, True),
|
||||
(2, 3., True), (2, 3, 1), (2, 3, None)):
|
||||
self.compare(lambda lsb=lsb, size=size, little=little: (bytearray(b'\xA5\x5A'), signal(lsb, size, little), 7, []))
|
||||
|
||||
def test_nonboolean_endian_truth_side_effects_keep_fallback_order(self):
|
||||
class Truth:
|
||||
def __init__(self, events):
|
||||
self.events = events
|
||||
|
||||
def __bool__(self):
|
||||
self.events.append('bool')
|
||||
return False
|
||||
|
||||
def factory():
|
||||
events = []
|
||||
return bytearray(b'\xA5\x5A'), signal(8, 9, Truth(events)), 5, events
|
||||
|
||||
self.compare(factory)
|
||||
|
||||
def test_deleted_signal_fields_preserve_errors_and_unread_endian(self):
|
||||
for field in ('lsb', 'size', 'is_little_endian'):
|
||||
for lsb, size, length in ((2, 3, 1), (2, 0, 1), (16, 3, 1), (2, 3, 0)):
|
||||
def factory(lsb=lsb, size=size, field=field, length=length):
|
||||
sig = signal(lsb, size)
|
||||
delattr(sig, field)
|
||||
return bytearray([0xA5] * length), sig, 7, []
|
||||
with self.subTest(field=field, lsb=lsb, size=size, length=length):
|
||||
self.compare(factory)
|
||||
|
||||
class DivisionProbe:
|
||||
def __init__(self, events):
|
||||
self.events = events
|
||||
|
||||
def __floordiv__(self, other):
|
||||
self.events.append(('floordiv', other))
|
||||
raise RuntimeError('original lsb operation precedes missing size')
|
||||
|
||||
def missing_size():
|
||||
events = []
|
||||
sig = signal(2, 3)
|
||||
sig.lsb = DivisionProbe(events)
|
||||
del sig.size
|
||||
return bytearray(b'\xA5'), sig, 7, events
|
||||
|
||||
self.compare(missing_size)
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
unittest.main()
|
||||
@@ -124,14 +124,13 @@ class TestOrdinaryAscm(unittest.TestCase):
|
||||
from opendbc.car.gm.longitudinal import AscmStopEvidence
|
||||
controls, now, offset = controls_fixture()
|
||||
cp = params(CAR.CHEVROLET_MALIBU_ASCM, sascm=True, alpha=True).as_reader()
|
||||
owner = LongitudinalInputs(cp, controls.params, lambda: controls.sm)
|
||||
owner = LongitudinalInputs(cp, None, lambda: controls.sm)
|
||||
owner.gm_ascm_boot_offset_ns = offset
|
||||
self.assertEqual(owner.optional_services, ['radarState', 'deviceState'])
|
||||
controls.sm['radarState'].leadOne.present = controls.sm['longitudinalPlan'].hasLead = True
|
||||
with patch('openpilot.starpilot.longitudinal.inputs.clock_pair_ns', return_value=(now, now + offset)), \
|
||||
patch.dict('os.environ', {'REPLAY': '0'}):
|
||||
self.assertIsInstance(owner.context(True).vehicle_stop_evidence, AscmStopEvidence)
|
||||
self.assertFalse(owner.faster_takeoffs)
|
||||
controls.sm['longitudinalPlan'].hasLead = False
|
||||
self.assertIsNone(owner.context(True).vehicle_stop_evidence)
|
||||
controls.sm['longitudinalPlan'].hasLead = True
|
||||
|
||||
@@ -11,7 +11,6 @@ from openpilot.starpilot.longitudinal.output_max import final_output
|
||||
from openpilot.selfdrive.controls.lib.longcontrol import LongControl
|
||||
from openpilot.starpilot.longitudinal.extension import LongitudinalContext
|
||||
from openpilot.starpilot.longitudinal.inputs import LongitudinalInputs
|
||||
from openpilot.starpilot.longitudinal.tests.extension_helpers import empty_params
|
||||
|
||||
State = car.CarControl.Actuators.LongControlState
|
||||
|
||||
@@ -79,61 +78,16 @@ class TestBlendedOutput(unittest.TestCase):
|
||||
|
||||
messages = Messages(longitudinalPlan=SimpleNamespace(hasLead=True),
|
||||
selfdriveState=SimpleNamespace(experimentalMode=True))
|
||||
owner = LongitudinalInputs(self.cp, empty_params(self), lambda: messages)
|
||||
self.assertEqual(owner.optional_services, ['radarState', 'deviceState'])
|
||||
owner = LongitudinalInputs(self.cp, SimpleNamespace(), lambda: messages)
|
||||
self.assertEqual(owner.optional_services, [])
|
||||
context = owner.context(True)
|
||||
self.assertTrue(context.has_lead)
|
||||
self.assertTrue(context.experimental_mode)
|
||||
self.assertFalse(context.faster_takeoffs)
|
||||
self.assertIsNone(context.start_evidence)
|
||||
messages.fresh = False
|
||||
context = owner.context(True)
|
||||
self.assertIsNone(context.has_lead)
|
||||
self.assertIsNone(context.experimental_mode)
|
||||
|
||||
def test_unseen_optional_takeoff_sources_keep_native_context_but_cannot_start(self):
|
||||
from openpilot.cereal import messaging
|
||||
|
||||
optional = ['radarState', 'deviceState']
|
||||
required = ['carState', 'longitudinalPlan', 'selfdriveState']
|
||||
messages = messaging.SubMaster(required + optional, ignore_alive=optional,
|
||||
ignore_valid=optional, ignore_avg_freq=optional)
|
||||
plan = messaging.new_message('longitudinalPlan', valid=True, logMonoTime=5_020_000_000)
|
||||
plan.longitudinalPlan.hasLead = True
|
||||
state = messaging.new_message('selfdriveState', valid=True, logMonoTime=5_020_000_000)
|
||||
state.selfdriveState.experimentalMode = True
|
||||
car_state = messaging.new_message('carState', valid=True, logMonoTime=5_020_000_000)
|
||||
messages.update_msgs(5.02, [plan.as_reader(), state.as_reader(), car_state.as_reader()])
|
||||
for name in required:
|
||||
messages.alive[name] = messages.valid[name] = messages.freq_ok[name] = True
|
||||
params = empty_params(self)
|
||||
owner = LongitudinalInputs(self.cp, params, lambda: messages)
|
||||
self.assertEqual(owner.optional_services, optional)
|
||||
self.assertFalse(any(messages.seen[name] for name in optional))
|
||||
self.assertTrue(messages.all_checks())
|
||||
baseline = owner.context(True)
|
||||
self.assertTrue(baseline.has_lead and baseline.experimental_mode)
|
||||
self.assertFalse(baseline.faster_takeoffs)
|
||||
self.assertIsNone(baseline.start_evidence)
|
||||
params.put_bool('FasterLeadTakeoff', True, block=True)
|
||||
assert owner.takeoff_preferences is not None
|
||||
owner.takeoff_preferences.next_ns = 0
|
||||
with patch('openpilot.starpilot.longitudinal.inputs.clock_pair_ns', side_effect=[
|
||||
(5_000_000_000, 5_500_000_000), (5_050_000_000, 5_550_000_000)]):
|
||||
for _ in range(2):
|
||||
context = owner.context(True)
|
||||
self.assertTrue(messages.all_checks())
|
||||
self.assertTrue(context.has_lead and context.experimental_mode and context.faster_takeoffs)
|
||||
self.assertFalse(context.start_evidence.car_fresh or context.start_evidence.plan_fresh)
|
||||
self.assertIsNone(context.start_evidence.lead_clear)
|
||||
control = LongControl(self.cp)
|
||||
self.cs.vEgo = 0.
|
||||
control.update(True, self.cs, 1.8, False, (-3.5, 2.), context=context)
|
||||
self.assertFalse(control.takeoff_starting)
|
||||
messages.valid['longitudinalPlan'] = False
|
||||
self.assertFalse(messages.all_checks())
|
||||
self.assertIsNone(owner.context(True).has_lead)
|
||||
|
||||
def test_cp_selected_alpha_limits_and_shared_default_output_ceiling(self):
|
||||
for topology in ('hdai', 'hdaii'):
|
||||
stock = params(topology)
|
||||
|
||||
@@ -194,11 +194,6 @@ static bool aol_ioniq6_session_started = false;
|
||||
static bool aol_ioniq6_request_seen = false;
|
||||
static bool aol_ioniq6_lateral_token_claimed = false;
|
||||
static uint32_t aol_ioniq6_gesture_ts = 0U;
|
||||
static bool aol_ioniq6_enable_counter_seen = false;
|
||||
static bool aol_ioniq6_enable_pending = false;
|
||||
static bool aol_ioniq6_enable_neutral = false;
|
||||
static bool aol_ioniq6_enable_release = false;
|
||||
static uint32_t aol_ioniq6_enable_release_ts = 0U;
|
||||
static bool hyundai_canfd_ioniq6_lfa_unpaired = false;
|
||||
static uint32_t hyundai_canfd_ioniq6_lfa_ts = 0U;
|
||||
static bool hyundai_canfd_ioniq6_heartbeat_seen = false;
|
||||
@@ -223,16 +218,8 @@ static uint8_t hyundai_canfd_angle_model = 0U;
|
||||
static uint8_t hyundai_canfd_carnival_consumed_counter = 0U;
|
||||
static uint8_t hyundai_canfd_carnival_source[16] = {0};
|
||||
|
||||
static void aol_ioniq6_enable_reset(void) {
|
||||
aol_ioniq6_enable_counter_seen = false;
|
||||
aol_ioniq6_enable_neutral = false;
|
||||
aol_ioniq6_enable_pending = false;
|
||||
aol_ioniq6_enable_release = false;
|
||||
}
|
||||
|
||||
static void aol_ioniq6_reset(void) {
|
||||
aol_ioniq6_long = false;
|
||||
aol_ioniq6_enable_reset();
|
||||
aol_optional_release_capable = false;
|
||||
aol_optional_release_policy = false;
|
||||
aol_optional_release_neutral = false;
|
||||
@@ -289,7 +276,6 @@ static uint8_t aol_ioniq6_request_mask(void) {
|
||||
if (relay_malfunction || safety_rx_checks_invalid ||
|
||||
(aol_ioniq6_session_started && !heartbeat_engaged) || pre_session_expired || request_expired) {
|
||||
aol_ioniq6_lateral_latch = false;
|
||||
aol_ioniq6_enable_reset();
|
||||
aol_optional_release_policy = false;
|
||||
aol_optional_release_neutral = false;
|
||||
aol_optional_release_counter_seen = false;
|
||||
@@ -304,12 +290,6 @@ static uint8_t aol_ioniq6_request_mask(void) {
|
||||
aol_host_axis_mask = 0U;
|
||||
} else if (heartbeat_engaged) {
|
||||
aol_ioniq6_session_started = true;
|
||||
if (hyundai_canfd_ioniq6_long && controls_allowed && aol_ioniq6_enable_release &&
|
||||
((aol_host_axis_mask & 0x3U) == 0x3U) &&
|
||||
(safety_get_ts_elapsed(now, aol_ioniq6_enable_release_ts) <= AOL_HOST_REQUEST_TIMEOUT_US)) {
|
||||
aol_ioniq6_lateral_latch = true;
|
||||
aol_ioniq6_enable_release = false;
|
||||
}
|
||||
if (((aol_host_axis_mask & 0x1U) != 0U) && aol_ioniq6_lateral_latch) {
|
||||
aol_ioniq6_lateral_token_claimed = true;
|
||||
}
|
||||
@@ -324,7 +304,6 @@ static uint8_t aol_ioniq6_permission_mask(void) {
|
||||
uint8_t permission = 0U;
|
||||
if (!aol_rx_healthy()) {
|
||||
aol_ioniq6_lateral_latch = false;
|
||||
aol_ioniq6_enable_reset();
|
||||
aol_optional_release_policy = false;
|
||||
aol_optional_release_neutral = false;
|
||||
aol_optional_release_counter_seen = false;
|
||||
@@ -358,7 +337,6 @@ static uint8_t hyundai_angle_aol_permission_mask(void) {
|
||||
}
|
||||
|
||||
static void aol_ioniq6_rx_invalid(void) {
|
||||
aol_ioniq6_enable_reset();
|
||||
aol_optional_release_policy = false;
|
||||
aol_optional_release_neutral = false;
|
||||
aol_optional_release_counter_seen = false;
|
||||
@@ -537,40 +515,7 @@ static void hyundai_canfd_rx_hook(const CANPacket_t *msg) {
|
||||
main_button = GET_BIT(msg, 34U);
|
||||
}
|
||||
const bool previous_controls_allowed = controls_allowed;
|
||||
const int previous_cruise_button = cruise_button_prev;
|
||||
hyundai_common_cruise_buttons_check(cruise_button, main_button);
|
||||
if (hyundai_canfd_ioniq6_long && aol_ioniq6_long) {
|
||||
static uint8_t aol_ioniq6_enable_counter = 0U;
|
||||
static uint32_t aol_ioniq6_enable_ts = 0U;
|
||||
const uint8_t counter = hyundai_canfd_get_counter(msg);
|
||||
const uint32_t now = microsecond_timer_get();
|
||||
const uint32_t elapsed = safety_get_ts_elapsed(now, aol_ioniq6_enable_ts);
|
||||
const bool fresh = aol_ioniq6_enable_counter_seen &&
|
||||
(counter == ((aol_ioniq6_enable_counter + 1U) & 0xFU)) && (elapsed > 0U) && (elapsed <= 100000U);
|
||||
if (!fresh || main_button || GET_BIT(msg, 23U)) {
|
||||
aol_ioniq6_enable_pending = false;
|
||||
aol_ioniq6_enable_release = false;
|
||||
} else if (aol_ioniq6_enable_neutral && (previous_cruise_button == HYUNDAI_BTN_NONE) &&
|
||||
((cruise_button == HYUNDAI_BTN_SET) || (cruise_button == HYUNDAI_BTN_RESUME))) {
|
||||
aol_ioniq6_enable_pending = true;
|
||||
} else if (aol_ioniq6_enable_pending && (cruise_button == HYUNDAI_BTN_NONE) &&
|
||||
((previous_cruise_button == HYUNDAI_BTN_SET) || (previous_cruise_button == HYUNDAI_BTN_RESUME))) {
|
||||
if (controls_allowed && aol_rx_healthy()) {
|
||||
heartbeat_engaged_mismatches = 0U;
|
||||
aol_ioniq6_enable_release = true;
|
||||
aol_ioniq6_enable_release_ts = now;
|
||||
}
|
||||
aol_ioniq6_enable_pending = false;
|
||||
} else if ((cruise_button != HYUNDAI_BTN_SET) && (cruise_button != HYUNDAI_BTN_RESUME)) {
|
||||
aol_ioniq6_enable_pending = false;
|
||||
} else {
|
||||
// A held enable button does not renew the heartbeat grace.
|
||||
}
|
||||
aol_ioniq6_enable_neutral = !main_button && !GET_BIT(msg, 23U) && (cruise_button == HYUNDAI_BTN_NONE);
|
||||
aol_ioniq6_enable_counter_seen = true;
|
||||
aol_ioniq6_enable_counter = counter;
|
||||
aol_ioniq6_enable_ts = now;
|
||||
}
|
||||
if (hyundai_canfd_ev9_long && !previous_controls_allowed && controls_allowed) {
|
||||
hyundai_ev9_inactive_accel_count = 0U;
|
||||
}
|
||||
@@ -609,7 +554,6 @@ static void hyundai_canfd_rx_hook(const CANPacket_t *msg) {
|
||||
}
|
||||
const bool gesture = main_button || (hyundai_canfd_alt_buttons ? GET_BIT(msg, 39U) : GET_BIT(msg, 23U)); // LDA/LKAS button
|
||||
if (cruise_button == HYUNDAI_BTN_CANCEL) {
|
||||
aol_ioniq6_enable_reset();
|
||||
// Exact DEBUG torque LONG AOL keeps only previously authorized lateral.
|
||||
// Common cruise handling above still withdraws ordinary LONG; CANCEL
|
||||
// cannot create a gesture, refresh a lease, or claim a lateral token.
|
||||
@@ -1138,7 +1082,6 @@ static bool hyundai_canfd_tx_hook(const CANPacket_t *msg) {
|
||||
static void hyundai_canfd_reset_ioniq6_state(void) {
|
||||
hyundai_canfd_ioniq6_long = false;
|
||||
aol_ioniq6_long = false;
|
||||
aol_ioniq6_enable_reset();
|
||||
aol_optional_release_capable = false;
|
||||
aol_optional_release_policy = false;
|
||||
aol_optional_release_neutral = false;
|
||||
|
||||
@@ -90,14 +90,6 @@ void set_controls_allowed(bool c){
|
||||
controls_allowed = c;
|
||||
}
|
||||
|
||||
void set_heartbeat_engaged_mismatches(uint32_t count) {
|
||||
heartbeat_engaged_mismatches = count;
|
||||
}
|
||||
|
||||
uint32_t get_heartbeat_engaged_mismatches(void) {
|
||||
return heartbeat_engaged_mismatches;
|
||||
}
|
||||
|
||||
void set_aol_test_heartbeat(bool engaged) {
|
||||
heartbeat_engaged = engaged;
|
||||
}
|
||||
|
||||
@@ -823,105 +823,6 @@ class TestHyundaiIoniq6Long(unittest.TestCase):
|
||||
self.safety.aol_set_host_request(1)
|
||||
self.assertEqual(self.safety.aol_get_permission_mask(), 0) # no stale latch after cancel
|
||||
|
||||
def test_full_engagement_without_lkas_retains_only_lateral_after_brake(self):
|
||||
if self.release:
|
||||
self.skipTest("RELEASE denies the Ioniq 6 AOL profiles")
|
||||
for topology, raw in RAW_AOL.items():
|
||||
with self.subTest(topology=topology):
|
||||
_, cp = params(topology)
|
||||
packer = CANPacker(DBC[cp.carFingerprint][Bus.pt])
|
||||
self.mode(raw)
|
||||
self.refresh_required_rx(packer, 1, 1_000_000)
|
||||
self.safety.set_aol_test_heartbeat(True)
|
||||
self.safety.set_timer(1_005_000)
|
||||
self.assertTrue(self.safety.safety_rx_hook(self.packet(packer.make_can_msg(
|
||||
"CRUISE_BUTTONS", 1, {"COUNTER": 2, "CRUISE_BUTTONS": 2}))))
|
||||
self.safety.set_timer(1_010_000)
|
||||
self.assertTrue(self.safety.safety_rx_hook(self.packet(packer.make_can_msg(
|
||||
"CRUISE_BUTTONS", 1, {"COUNTER": 3, "CRUISE_BUTTONS": 0}))))
|
||||
self.assertTrue(self.safety.get_controls_allowed())
|
||||
self.safety.aol_set_host_request(3)
|
||||
self.assertEqual(self.safety.aol_get_permission_mask(), 3)
|
||||
self.safety.set_timer(1_020_000)
|
||||
self.assertTrue(self.safety.safety_rx_hook(self.packet(packer.make_can_msg(
|
||||
"TCS", 1, {"DriverBraking": 1, "COUNTER": 2}))))
|
||||
self.assertFalse(self.safety.get_controls_allowed())
|
||||
self.safety.aol_set_host_request(1)
|
||||
self.assertEqual(self.safety.aol_get_permission_mask(), 1)
|
||||
for frame in create_steering_messages(packer, cp, CanBus(cp), True, True, 2):
|
||||
self.assertTrue(self.safety.safety_tx_hook(self.packet(frame)))
|
||||
_, state, controller = controller_fixture(topology == "lkas_alt", aol=True)
|
||||
command = structs.CarControl(enabled=True, longActive=True)
|
||||
command.actuators.accel = .1
|
||||
command.actuators.longControlState = LongState.starting
|
||||
_, frames = controller.update(command.as_reader(), state, 1_020_000_000)
|
||||
active_scc = next(frame for frame in frames if frame[0] == 0x1A0)
|
||||
self.assertFalse(self.safety.safety_tx_hook(self.packet(active_scc)))
|
||||
for counter in range(4, 35):
|
||||
self.refresh_required_rx(packer, counter, 1_020_000 + counter * 10_000)
|
||||
self.safety.aol_set_host_request(0)
|
||||
self.assertEqual(self.safety.aol_get_permission_mask(), 0)
|
||||
self.safety.aol_set_host_request(1)
|
||||
self.assertEqual(self.safety.aol_get_permission_mask(), 1)
|
||||
|
||||
def test_full_engagement_release_credit_cannot_survive_native_revocation(self):
|
||||
if self.release:
|
||||
self.skipTest("RELEASE denies the Ioniq 6 AOL profiles")
|
||||
for topology, raw in RAW_AOL.items():
|
||||
for reset in ("heartbeat", "timeout", "unclaimed", "long_only", "zero_only"):
|
||||
with self.subTest(topology=topology, reset=reset):
|
||||
_, cp = params(topology)
|
||||
packer = CANPacker(DBC[cp.carFingerprint][Bus.pt])
|
||||
self.mode(raw)
|
||||
self.refresh_required_rx(packer, 1, 1_000_000)
|
||||
self.safety.set_aol_test_heartbeat(True)
|
||||
for counter, button in ((2, 2), (3, 0)):
|
||||
self.safety.set_timer(1_000_000 + counter * 5_000)
|
||||
self.assertTrue(self.safety.safety_rx_hook(self.packet(packer.make_can_msg(
|
||||
"CRUISE_BUTTONS", 1, {"COUNTER": counter, "CRUISE_BUTTONS": button}))))
|
||||
self.safety.aol_set_host_request(0 if reset in ("zero_only", "unclaimed") else 2 if reset == "long_only" else 3)
|
||||
self.assertEqual(self.safety.aol_get_permission_mask(), 0 if reset in ("zero_only", "unclaimed") else 2 if reset == "long_only" else 3)
|
||||
if reset == "heartbeat":
|
||||
self.safety.set_aol_test_heartbeat(False)
|
||||
self.assertEqual(self.safety.aol_get_permission_mask(), 0)
|
||||
self.safety.set_aol_test_heartbeat(True)
|
||||
elif reset in ("timeout", "unclaimed"):
|
||||
self.safety.set_timer(1_400_000)
|
||||
self.assertEqual(self.safety.aol_get_permission_mask(), 0)
|
||||
stamp = 1_410_000 if reset in ("timeout", "unclaimed") else 1_020_000
|
||||
self.refresh_required_rx(packer, 4, stamp)
|
||||
if reset in ("heartbeat", "timeout", "unclaimed"):
|
||||
self.safety.aol_set_host_request(3)
|
||||
self.assertEqual(self.safety.aol_get_permission_mask(), 3)
|
||||
self.safety.set_timer(stamp + 5_000)
|
||||
self.assertTrue(self.safety.safety_rx_hook(self.packet(packer.make_can_msg(
|
||||
"TCS", 1, {"DriverBraking": 1, "COUNTER": 5}))))
|
||||
self.safety.aol_set_host_request(1)
|
||||
self.assertEqual(self.safety.aol_get_permission_mask(), 0)
|
||||
|
||||
def test_new_physical_enable_release_restarts_engaged_heartbeat_grace(self):
|
||||
if self.release:
|
||||
self.skipTest("RELEASE denies the Ioniq 6 AOL profiles")
|
||||
for topology, raw in RAW_AOL.items():
|
||||
for button in (1, 2):
|
||||
with self.subTest(topology=topology, button=button):
|
||||
_, cp = params(topology)
|
||||
packer = CANPacker(DBC[cp.carFingerprint][Bus.pt])
|
||||
self.mode(raw)
|
||||
self.refresh_required_rx(packer, 1, 1_000_000)
|
||||
self.safety.set_controls_allowed(True)
|
||||
self.safety.set_aol_test_heartbeat(False)
|
||||
self.safety.set_heartbeat_engaged_mismatches(2)
|
||||
self.safety.set_timer(1_005_000)
|
||||
self.assertTrue(self.safety.safety_rx_hook(self.packet(packer.make_can_msg(
|
||||
"CRUISE_BUTTONS", 1, {"COUNTER": 2, "CRUISE_BUTTONS": button}))))
|
||||
self.assertEqual(self.safety.get_heartbeat_engaged_mismatches(), 2)
|
||||
self.safety.set_timer(1_010_000)
|
||||
self.assertTrue(self.safety.safety_rx_hook(self.packet(packer.make_can_msg(
|
||||
"CRUISE_BUTTONS", 1, {"COUNTER": 3, "CRUISE_BUTTONS": 0}))))
|
||||
self.assertTrue(self.safety.get_controls_allowed())
|
||||
self.assertEqual(self.safety.get_heartbeat_engaged_mismatches(), 0)
|
||||
|
||||
def test_aol_long_permission_requires_explicit_host_long_request(self):
|
||||
if self.release:
|
||||
self.skipTest("RELEASE denies the Ioniq 6 AOL profiles")
|
||||
|
||||
@@ -180,8 +180,8 @@ class FrequencyTracker:
|
||||
|
||||
class SubMaster:
|
||||
def __init__(self, services: list[str], poll: str | None = None,
|
||||
ignore_alive: list[str] | set[str] | None = None, ignore_avg_freq: list[str] | set[str] | None = None,
|
||||
ignore_valid: list[str] | set[str] | None = None, addr: str = "127.0.0.1", frequency: float | None = None):
|
||||
ignore_alive: list[str] | None = None, ignore_avg_freq: list[str] | None = None,
|
||||
ignore_valid: list[str] | None = None, addr: str = "127.0.0.1", frequency: float | None = None):
|
||||
self.frame = -1
|
||||
self.services = services
|
||||
self.seen = dict.fromkeys(services, False)
|
||||
@@ -268,43 +268,13 @@ class SubMaster:
|
||||
self.freq_ok[s] = self.freq_tracker[s].valid or self.simulation
|
||||
|
||||
def all_alive(self, service_list: list[str] | None = None) -> bool:
|
||||
for s in (service_list or self.services):
|
||||
try:
|
||||
if s in self.ignore_alive:
|
||||
continue
|
||||
value = self.alive[s]
|
||||
except StopIteration as exc:
|
||||
raise RuntimeError('generator raised StopIteration') from exc
|
||||
if not value:
|
||||
return False
|
||||
del value
|
||||
return True
|
||||
return all(self.alive[s] for s in (service_list or self.services) if s not in self.ignore_alive)
|
||||
|
||||
def all_freq_ok(self, service_list: list[str] | None = None) -> bool:
|
||||
for s in (service_list or self.services):
|
||||
try:
|
||||
if not self._check_avg_freq(s):
|
||||
continue
|
||||
value = self.freq_ok[s]
|
||||
except StopIteration as exc:
|
||||
raise RuntimeError('generator raised StopIteration') from exc
|
||||
if not value:
|
||||
return False
|
||||
del value
|
||||
return True
|
||||
return all(self.freq_ok[s] for s in (service_list or self.services) if self._check_avg_freq(s))
|
||||
|
||||
def all_valid(self, service_list: list[str] | None = None) -> bool:
|
||||
for s in (service_list or self.services):
|
||||
try:
|
||||
if s in self.ignore_valid:
|
||||
continue
|
||||
value = self.valid[s]
|
||||
except StopIteration as exc:
|
||||
raise RuntimeError('generator raised StopIteration') from exc
|
||||
if not value:
|
||||
return False
|
||||
del value
|
||||
return True
|
||||
return all(self.valid[s] for s in (service_list or self.services) if s not in self.ignore_valid)
|
||||
|
||||
def all_checks(self, service_list: list[str] | None = None) -> bool:
|
||||
return self.all_alive(service_list) and self.all_freq_ok(service_list) and self.all_valid(service_list)
|
||||
|
||||
@@ -1,144 +0,0 @@
|
||||
"""Health aggregation is fresh and preserves lookup and exception ordering."""
|
||||
import random
|
||||
import unittest
|
||||
|
||||
from openpilot.cereal.messaging import SubMaster
|
||||
from openpilot.cereal.services import SERVICE_LIST
|
||||
|
||||
SERVICES = ['carState', 'deviceState', 'carParams']
|
||||
METHODS = ('all_alive', 'all_freq_ok', 'all_valid', 'all_checks')
|
||||
|
||||
|
||||
class Reference:
|
||||
def _check_avg_freq(self, s):
|
||||
return SERVICE_LIST[s].frequency > 0.99 and (s not in self.ignore_average_freq) and (s not in self.ignore_alive)
|
||||
|
||||
def all_alive(self, service_list=None):
|
||||
return all(self.alive[s] for s in (service_list or self.services) if s not in self.ignore_alive)
|
||||
|
||||
def all_freq_ok(self, service_list=None):
|
||||
return all(self.freq_ok[s] for s in (service_list or self.services) if self._check_avg_freq(s))
|
||||
|
||||
def all_valid(self, service_list=None):
|
||||
return all(self.valid[s] for s in (service_list or self.services) if s not in self.ignore_valid)
|
||||
|
||||
def all_checks(self, service_list=None):
|
||||
return self.all_alive(service_list) and self.all_freq_ok(service_list) and self.all_valid(service_list)
|
||||
|
||||
|
||||
def host(kind, seed=0):
|
||||
obj, rng = kind.__new__(kind), random.Random(seed)
|
||||
obj.services, obj.events = SERVICES[:], []
|
||||
for name in ('alive', 'freq_ok', 'valid'):
|
||||
setattr(obj, name, {s: bool(rng.getrandbits(1)) for s in SERVICES})
|
||||
for name in ('ignore_alive', 'ignore_average_freq', 'ignore_valid'):
|
||||
setattr(obj, name, [s for s in SERVICES if rng.randrange(4) == 0])
|
||||
return obj
|
||||
|
||||
|
||||
def outcome(obj, method, selected):
|
||||
try:
|
||||
return getattr(obj, method)(selected), obj.events
|
||||
except Exception as exc:
|
||||
return (type(exc).__name__, str(exc), type(exc.__cause__).__name__, exc.__suppress_context__), obj.events
|
||||
|
||||
|
||||
def traced(kind, fail=None, error=KeyError, first_false=False):
|
||||
obj = host(kind)
|
||||
def touch(operation, key):
|
||||
obj.events.append((operation, key))
|
||||
if operation == fail and key == 'deviceState':
|
||||
raise error('probe')
|
||||
class Truth:
|
||||
def __init__(self, key): self.key = key
|
||||
def __bool__(self):
|
||||
touch('truth', self.key)
|
||||
return not (first_false and self.key == 'carState')
|
||||
class Mapping(dict):
|
||||
def __getitem__(self, key):
|
||||
touch('get', key)
|
||||
return super().__getitem__(key)
|
||||
class Ignored(list):
|
||||
def __contains__(self, key):
|
||||
touch('contains', key)
|
||||
return super().__contains__(key)
|
||||
for name in ('alive', 'freq_ok', 'valid'):
|
||||
setattr(obj, name, Mapping({s: Truth(s) for s in SERVICES}))
|
||||
for name in ('ignore_alive', 'ignore_average_freq', 'ignore_valid'):
|
||||
setattr(obj, name, Ignored())
|
||||
return obj
|
||||
|
||||
|
||||
class TestHealthAggregation(unittest.TestCase):
|
||||
def test_deterministic_maps_selection_ignores_and_mutation(self):
|
||||
for seed in range(16):
|
||||
for selected in (None, [], SERVICES[:2], [SERVICES[0], SERVICES[0]]):
|
||||
a, b = host(SubMaster, seed), host(Reference, seed)
|
||||
for method in METHODS:
|
||||
self.assertEqual(outcome(a, method, selected), outcome(b, method, selected))
|
||||
for name in ('alive', 'freq_ok', 'valid'):
|
||||
getattr(a, name)['carState'] = not getattr(a, name)['carState']
|
||||
getattr(b, name)['carState'] = not getattr(b, name)['carState']
|
||||
self.assertEqual(outcome(a, method, selected), outcome(b, method, selected))
|
||||
|
||||
def test_custom_lookup_truth_membership_and_first_failure_order(self):
|
||||
for fail in (None, 'get', 'contains', 'truth'):
|
||||
for error in (KeyError, StopIteration):
|
||||
for first_false in (False, True):
|
||||
for method in METHODS:
|
||||
a, b = (traced(kind, fail, error, first_false) for kind in (SubMaster, Reference))
|
||||
self.assertEqual(outcome(a, method, None), outcome(b, method, None))
|
||||
for method in METHODS:
|
||||
self.assertEqual(outcome(traced(SubMaster), method, ['unknown']), outcome(traced(Reference), method, ['unknown']))
|
||||
|
||||
def test_selection_stopiteration_stays_outside_generator(self):
|
||||
class TruthRaises(list):
|
||||
def __bool__(self): raise StopIteration('selection truth')
|
||||
class IterRaises(list):
|
||||
def __iter__(self): raise StopIteration('selection iter')
|
||||
for selected in (TruthRaises(SERVICES), IterRaises(SERVICES)):
|
||||
for method in METHODS:
|
||||
self.assertEqual(outcome(host(SubMaster), method, selected), outcome(host(Reference), method, selected))
|
||||
|
||||
def test_ephemeral_truthy_value_is_released_before_next_lookup(self):
|
||||
def factory(kind):
|
||||
obj = host(kind)
|
||||
class Ephemeral:
|
||||
def __init__(self, key): self.key = key
|
||||
def __bool__(self):
|
||||
obj.events.append(('truth', self.key))
|
||||
return True
|
||||
def __del__(self):
|
||||
obj.events.append(('del', self.key))
|
||||
class Mapping:
|
||||
def __getitem__(self, key):
|
||||
obj.events.append(('get', key))
|
||||
return Ephemeral(key)
|
||||
for name in ('alive', 'freq_ok', 'valid'):
|
||||
setattr(obj, name, Mapping())
|
||||
for name in ('ignore_alive', 'ignore_average_freq', 'ignore_valid'):
|
||||
setattr(obj, name, [])
|
||||
return obj
|
||||
for method in METHODS:
|
||||
a, b = factory(SubMaster), factory(Reference)
|
||||
self.assertEqual(outcome(a, method, None), outcome(b, method, None))
|
||||
self.assertEqual(a.events[:6], [('get', 'carState'), ('truth', 'carState'), ('del', 'carState'),
|
||||
('get', 'deviceState'), ('truth', 'deviceState'), ('del', 'deviceState')])
|
||||
|
||||
def test_all_checks_retains_subclass_dispatch_and_short_circuit(self):
|
||||
selected = SERVICES[:1]
|
||||
for stop in range(4):
|
||||
results = []
|
||||
for base in (SubMaster, Reference):
|
||||
def override(index, stop=stop):
|
||||
def call(obj, services):
|
||||
obj.events.append((METHODS[index], services is selected))
|
||||
return index != stop
|
||||
return call
|
||||
cls = type('Overridden', (base,), {name: override(i) for i, name in enumerate(METHODS[:3])})
|
||||
results.append(outcome(host(cls), 'all_checks', selected))
|
||||
self.assertEqual(*results)
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
unittest.main()
|
||||
@@ -13,9 +13,6 @@ public:
|
||||
static std::string get_name() { return ""; }
|
||||
static cereal::InitData::DeviceType get_device_type() { return cereal::InitData::DeviceType::UNKNOWN; }
|
||||
|
||||
static int get_voltage() { return 0; }
|
||||
static int get_current() { return 0; }
|
||||
|
||||
static std::string get_serial() { return "cccccc"; }
|
||||
|
||||
static std::map<std::string, std::string> get_init_logs(bool route_log = false) {
|
||||
|
||||
@@ -1,7 +1,6 @@
|
||||
#pragma once
|
||||
|
||||
#include <cassert>
|
||||
#include <cstdlib>
|
||||
#include <fstream>
|
||||
#include <map>
|
||||
#include <string>
|
||||
@@ -31,9 +30,6 @@ public:
|
||||
return it->second;
|
||||
}
|
||||
|
||||
static int get_voltage() { return std::atoi(util::read_file("/sys/class/hwmon/hwmon1/in1_input").c_str()); }
|
||||
static int get_current() { return std::atoi(util::read_file("/sys/class/hwmon/hwmon1/curr1_input").c_str()); }
|
||||
|
||||
static std::string get_serial() {
|
||||
static std::string serial("");
|
||||
if (serial.empty()) {
|
||||
|
||||
Binary file not shown.
Binary file not shown.
@@ -43,7 +43,6 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
|
||||
{"StarPilotFavoriteSlots", {PERSISTENT, JSON, R"([{"enabled":false,"show_onroad":false,"key":null,"label":""},{"enabled":false,"show_onroad":false,"key":null,"label":""},{"enabled":false,"show_onroad":false,"key":null,"label":""}])"}},
|
||||
{"StarPilotControllerBindings", {PERSISTENT | DONT_LOG, JSON}},
|
||||
{"UseStarPilotLongitudinalPlanner", {PERSISTENT, BOOL, "1"}},
|
||||
{"UseVOACC", {PERSISTENT, BOOL, "0"}},
|
||||
{"CustomPersonalities", {PERSISTENT, BOOL, "0"}},
|
||||
{"SettingsRetirementState", {PERSISTENT | DONT_LOG, JSON}},
|
||||
{"RetiredSettingsEvidence", {PERSISTENT | DONT_LOG, JSON}},
|
||||
@@ -290,7 +289,6 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
|
||||
{"LockDoors", {PERSISTENT, BOOL, "0"}},
|
||||
{"UnlockDoors", {PERSISTENT, BOOL, "0"}},
|
||||
{"ShowDebugInfo", {PERSISTENT, BOOL}},
|
||||
{"DeveloperSidebar", {PERSISTENT, BOOL, "0"}},
|
||||
{"ShowBrakeStatus", {PERSISTENT, BOOL, "0"}},
|
||||
{"HideDMIcon", {PERSISTENT, BOOL, "0"}},
|
||||
{"HidePersonalityAlerts", {PERSISTENT, BOOL, "0"}},
|
||||
|
||||
@@ -1,16 +0,0 @@
|
||||
"""Scalar float clipping with NumPy fallback for all other cases."""
|
||||
|
||||
import math
|
||||
|
||||
import numpy as np
|
||||
|
||||
|
||||
def clip_scalar(value, lower, upper):
|
||||
if ((type(value) is float or type(value) is np.float64) and
|
||||
(type(lower) is float or type(lower) is np.float64) and
|
||||
(type(upper) is float or type(upper) is np.float64)):
|
||||
scalar, lo, hi = float(value), float(lower), float(upper)
|
||||
if (math.isfinite(scalar) and math.isfinite(lo) and math.isfinite(hi) and
|
||||
lo < hi and scalar != lo and scalar != hi):
|
||||
return lo if scalar < lo else hi if scalar > hi else scalar
|
||||
return float(np.clip(value, lower, upper))
|
||||
@@ -1,57 +0,0 @@
|
||||
import math
|
||||
import random
|
||||
import struct
|
||||
import unittest
|
||||
import warnings
|
||||
|
||||
import numpy as np
|
||||
|
||||
from openpilot.common.scalar_clip import clip_scalar
|
||||
|
||||
|
||||
class TestScalarClip(unittest.TestCase):
|
||||
def compare(self, value, lower, upper):
|
||||
results = []
|
||||
for function in (lambda v, lo, hi: float(np.clip(v, lo, hi)), clip_scalar):
|
||||
with warnings.catch_warnings(record=True) as seen:
|
||||
warnings.simplefilter('always')
|
||||
try:
|
||||
result = ('value', struct.pack('>d', function(value, lower, upper)))
|
||||
except Exception as error:
|
||||
result = ('error', type(error), str(error))
|
||||
results.append((result, [(warning.category, str(warning.message)) for warning in seen]))
|
||||
self.assertEqual(results[0], results[1], (value, lower, upper))
|
||||
|
||||
def test_binary64_inputs_match_numpy_exactly(self):
|
||||
randomizer = random.Random(20261015)
|
||||
def finite():
|
||||
while True:
|
||||
value = struct.unpack('>d', randomizer.getrandbits(64).to_bytes(8, 'big'))[0]
|
||||
if math.isfinite(value):
|
||||
return value
|
||||
for _ in range(3000):
|
||||
lower, upper = sorted((finite(), finite()))
|
||||
value = finite()
|
||||
self.compare(value, lower, upper)
|
||||
self.compare(np.float64(value), lower, upper)
|
||||
|
||||
def test_boundary_bits_and_nonfinite_inputs(self):
|
||||
values = (-0.0, 0.0, -math.ulp(0.0), math.ulp(0.0), -1.0, 1.0,
|
||||
math.nextafter(1.0, -math.inf), math.nextafter(1.0, math.inf),
|
||||
-math.inf, math.inf, math.nan)
|
||||
bounds = ((-1.0, 1.0), (-0.0, 1.0), (0.0, 1.0), (-1.0, -0.0), (-1.0, 0.0),
|
||||
(-0.0, 0.0), (0.0, -0.0), (1.0, 1.0), (1.0, -1.0),
|
||||
(-math.inf, math.inf), (math.nan, 1.0), (-1.0, math.nan))
|
||||
for lower, upper in bounds:
|
||||
for value in values:
|
||||
self.compare(value, lower, upper)
|
||||
self.compare(np.float64(value), lower, upper)
|
||||
|
||||
def test_other_types_keep_numpy_results_errors_and_warnings(self):
|
||||
class FloatSubclass(float):
|
||||
pass
|
||||
values = (0, True, None, '0', np.float32(0.2), np.int64(2), FloatSubclass(0.2),
|
||||
np.array(0.2), np.array([0.2]), np.array([0.2, 0.5]))
|
||||
for value in values:
|
||||
for lower, upper in ((-1.0, 1.0), (-1, 1), (np.float32(-1), np.float32(1))):
|
||||
self.compare(value, lower, upper)
|
||||
@@ -1,136 +0,0 @@
|
||||
"""Scalar clipping and drive limits preserve NumPy bits, types, and callbacks."""
|
||||
from decimal import Decimal
|
||||
from fractions import Fraction
|
||||
from itertools import product
|
||||
import math
|
||||
import struct
|
||||
import unittest
|
||||
from unittest.mock import patch
|
||||
import warnings
|
||||
|
||||
import numpy as np
|
||||
|
||||
from openpilot.common import scalar_clip
|
||||
from openpilot.selfdrive.controls.lib import drive_helpers as drive
|
||||
from openpilot.selfdrive.controls.lib.drive_helpers import (
|
||||
ACCELERATION_DUE_TO_GRAVITY, DT_CTRL, MAX_CURVATURE, MAX_LATERAL_ACCEL_NO_ROLL, MAX_LATERAL_JERK, MIN_SPEED,
|
||||
)
|
||||
|
||||
|
||||
def original_clip(value, lower, upper):
|
||||
return float(np.clip(value, lower, upper))
|
||||
|
||||
|
||||
def clamp(val, min_val, max_val):
|
||||
clamped_val = float(np.clip(val, min_val, max_val))
|
||||
return clamped_val, clamped_val != val
|
||||
|
||||
|
||||
def original_clip_curvature(v_ego, prev_curvature, new_curvature, roll):
|
||||
v_ego = max(v_ego, MIN_SPEED)
|
||||
max_curvature_rate = MAX_LATERAL_JERK / (v_ego ** 2)
|
||||
new_curvature = np.clip(new_curvature,
|
||||
prev_curvature - max_curvature_rate * DT_CTRL,
|
||||
prev_curvature + max_curvature_rate * DT_CTRL)
|
||||
roll_compensation = roll * ACCELERATION_DUE_TO_GRAVITY
|
||||
max_lat_accel = MAX_LATERAL_ACCEL_NO_ROLL + roll_compensation
|
||||
min_lat_accel = -MAX_LATERAL_ACCEL_NO_ROLL + roll_compensation
|
||||
new_curvature, limited_accel = clamp(new_curvature, min_lat_accel / v_ego ** 2, max_lat_accel / v_ego ** 2)
|
||||
new_curvature, limited_max_curv = clamp(new_curvature, -MAX_CURVATURE, MAX_CURVATURE)
|
||||
return float(new_curvature), limited_accel or limited_max_curv
|
||||
|
||||
|
||||
def encoded(value):
|
||||
if isinstance(value, tuple):
|
||||
return type(value), tuple(encoded(item) for item in value)
|
||||
if isinstance(value, np.ndarray):
|
||||
return type(value), value.dtype.str, value.shape, value.tobytes()
|
||||
if isinstance(value, np.generic):
|
||||
return type(value), value.dtype.str, value.tobytes()
|
||||
if type(value) is float:
|
||||
return float, struct.pack('!d', value)
|
||||
return type(value), value
|
||||
|
||||
|
||||
def outcome(function, *args):
|
||||
with warnings.catch_warnings(record=True) as seen:
|
||||
warnings.simplefilter('always')
|
||||
try:
|
||||
result = ('return', encoded(function(*args)))
|
||||
except Exception as exc:
|
||||
result = ('raise', type(exc), str(exc))
|
||||
return result, [(item.category, str(item.message)) for item in seen]
|
||||
|
||||
|
||||
class TestScalarDriveClip(unittest.TestCase):
|
||||
def test_exact_float64_bounds_bit_parity_at_boundaries_and_special_values(self):
|
||||
triples = ((.5, 0., 1.), (-2., -1., 1.), (2., -1., 1.),
|
||||
(-0., 0., 1.), (0., -0., 1.), (-0., -1., 0.), (0., -1., -0.),
|
||||
(1., 1., 1.), (0., 1., -1.), (math.nan, -1., 1.),
|
||||
(.5, math.nan, 1.), (.5, -1., math.nan), (math.inf, -1., 1.),
|
||||
(-math.inf, -1., 1.), (.5, -math.inf, math.inf))
|
||||
for value_type, lower_type, upper_type in product((float, np.float64), repeat=3):
|
||||
for value, lower, upper in triples:
|
||||
args = value_type(value), lower_type(lower), upper_type(upper)
|
||||
self.assertEqual(outcome(scalar_clip.clip_scalar, *args), outcome(original_clip, *args))
|
||||
|
||||
def test_unsupported_types_use_numpy_fallback_with_identical_warnings_and_errors(self):
|
||||
class FloatSubclass(float):
|
||||
pass
|
||||
class IntSubclass(int):
|
||||
pass
|
||||
cases = ((np.float32(.5), 0., 1.), (.5, np.float32(0), 1.),
|
||||
(np.longdouble('1.000000000000000001'), np.longdouble(0), np.longdouble(2)),
|
||||
(1, 0, 2), (1 << 4096, -1., 1.), (True, False, True),
|
||||
(FloatSubclass(.5), 0., 1.), (.5, FloatSubclass(0), 1.), (IntSubclass(1), 0, 2),
|
||||
(Fraction(1, 3), Fraction(0), Fraction(1)), (Decimal('.5'), Decimal(0), Decimal(1)),
|
||||
(np.array(.5), 0., 1.), (np.array([.5]), 0., 1.), (np.array([.5, 2.]), 0., 1.),
|
||||
(None, 0., 1.), (object(), 0., 1.), ('invalid', 0., 1.))
|
||||
numpy_clip = np.clip
|
||||
for args in cases:
|
||||
expected = outcome(original_clip, *args)
|
||||
with patch.object(scalar_clip.np, 'clip', wraps=numpy_clip) as fallback:
|
||||
self.assertEqual(outcome(scalar_clip.clip_scalar, *args), expected)
|
||||
fallback.assert_called_once()
|
||||
|
||||
def test_clamp_preserves_comparison_type_shape_and_side_effect_order(self):
|
||||
for args in ((.5, 0., 1.), (np.float64(2), np.float64(0), np.float64(1)),
|
||||
(np.float32(.5), 0., 1.), (-0., 0., 1.), (math.nan, -1., 1.),
|
||||
(np.array(.5), 0., 1.), (np.array([.5]), 0., 1.), (np.array([.5, 2.]), 0., 1.)):
|
||||
self.assertEqual(outcome(drive.clamp, *args), outcome(clamp, *args))
|
||||
expected = clamp(np.float64(2), 0., 1.)
|
||||
actual = drive.clamp(np.float64(2), 0., 1.)
|
||||
self.assertIs(type(actual[1]), type(expected[1]))
|
||||
self.assertIs(type(actual[1]), np.bool_)
|
||||
class Callback:
|
||||
def __init__(self, events):
|
||||
self.events = events
|
||||
def __array_function__(self, function, types, args, kwargs):
|
||||
self.events.append(('clip', function.__name__))
|
||||
return np.float64(.25)
|
||||
def __ne__(self, other):
|
||||
self.events.append(('ne', encoded(other)))
|
||||
return np.array([False, True])
|
||||
for function, reference in ((scalar_clip.clip_scalar, original_clip), (drive.clamp, clamp)):
|
||||
actual_events, expected_events = [], []
|
||||
actual = outcome(function, Callback(actual_events), 0., 1.)
|
||||
expected = outcome(reference, Callback(expected_events), 0., 1.)
|
||||
self.assertEqual(actual, expected)
|
||||
self.assertEqual(actual_events, expected_events)
|
||||
|
||||
def test_actual_curvature_grid_matches_frozen_body_and_retains_first_numpy_clip(self):
|
||||
for args in product((0., 1., 20., 100., np.float64(20)), (-.25, 0., .25), (-.2, .0002, .2), (-.1, 0., .1)):
|
||||
self.assertEqual(outcome(drive.clip_curvature, *args), outcome(original_clip_curvature, *args))
|
||||
for args in ((20., 0., math.nan, 0.), (math.inf, 0., .001, 0.), (20., .001, math.inf, math.nan)):
|
||||
self.assertEqual(outcome(drive.clip_curvature, *args), outcome(original_clip_curvature, *args))
|
||||
numpy_clip = np.clip
|
||||
args = (np.float64(20), .001, .002, np.float64(.01))
|
||||
expected = outcome(original_clip_curvature, *args)
|
||||
with patch.object(drive.np, 'clip', wraps=numpy_clip) as clip:
|
||||
self.assertEqual(outcome(drive.clip_curvature, *args), expected)
|
||||
self.assertGreaterEqual(clip.call_count, 1)
|
||||
self.assertIs(clip.call_args_list[0].args[0], args[2])
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
unittest.main()
|
||||
@@ -53,7 +53,7 @@ from opendbc.car.ford.aol import (
|
||||
from opendbc.car.hyundai.canfd_angle_aol import native_observation, temporary_restriction, qualified as qualified_angle_aol
|
||||
from opendbc.car.hyundai.canfd_owner import config_index as hyundai_canfd_config_index
|
||||
from opendbc.car.hyundai.captured_adrv_startup import CapturedADRVStartup
|
||||
from openpilot.starpilot.car.hyundai.aol import aol_safety_config as hyundai_aol_safety_config
|
||||
from openpilot.starpilot.car.hyundai.aol import aol_safety_config as hyundai_aol_safety_config, qualified_ioniq6
|
||||
from openpilot.starpilot.aol.runtime import AXIS_MAX_AGE_NS, current_axis, current_native
|
||||
from openpilot.starpilot.aol.vehicle import create_intent as create_aol_intent, native_latch_rejected, policy_for as aol_policy_for
|
||||
from openpilot.starpilot.aol.wire import IntentState, encode_intent
|
||||
@@ -161,7 +161,7 @@ class Car:
|
||||
self.conditional_replay = os.getenv('CONDITIONAL_MODE_REPLAY_RUNTIME') == '1' or feature_requested(self.params, 'conditional')
|
||||
subscribed_services = ['pandaStates', 'carControl', 'onroadEvents', 'deviceState']
|
||||
if self.aol_replay:
|
||||
subscribed_services.extend(('aolSafetyWire', 'managerState', 'aolAxisState', 'selfdriveState'))
|
||||
subscribed_services.extend(('aolSafetyWire', 'managerState', 'aolAxisState'))
|
||||
if self.slc_replay:
|
||||
subscribed_services.append('slcState')
|
||||
self.sm = messaging.SubMaster(subscribed_services)
|
||||
@@ -227,7 +227,7 @@ class Car:
|
||||
enroll_missing_preference(self.params, cp)
|
||||
if not self.aol_replay and feature_requested(self.params, 'aol'):
|
||||
self.aol_replay = True
|
||||
subscribed_services.extend(('aolSafetyWire', 'managerState', 'aolAxisState', 'selfdriveState'))
|
||||
subscribed_services.extend(('aolSafetyWire', 'managerState', 'aolAxisState'))
|
||||
self.sm = messaging.SubMaster(subscribed_services)
|
||||
self.pm.sock['aolIntentWire'] = messaging.pub_sock('aolIntentWire')
|
||||
|
||||
@@ -349,7 +349,7 @@ class Car:
|
||||
aol_policy = aol_policy_for(self.CP)
|
||||
if aol_policy.full_axis_runtime_required and not self.aol_replay:
|
||||
self.aol_replay = True
|
||||
subscribed_services.extend(('aolSafetyWire', 'managerState', 'aolAxisState', 'selfdriveState'))
|
||||
subscribed_services.extend(('aolSafetyWire', 'managerState', 'aolAxisState'))
|
||||
self.sm = messaging.SubMaster(subscribed_services)
|
||||
self.pm.sock['aolIntentWire'] = messaging.pub_sock('aolIntentWire')
|
||||
initialize_aol_preference(self.CP)
|
||||
@@ -614,23 +614,15 @@ class Car:
|
||||
fault_active = True
|
||||
rejected = native_reset or native_latch_rejected(self.CP, native, state=CS if angle_panda_ready else None)
|
||||
rejection_ns = now_ns if native_reset else int(native.observedMonoTime) if rejected else 0
|
||||
engagement_observation = ({"standard_active": bool(host_enabled and
|
||||
(self.sm['carControl'].latActive or self.sm['carControl'].longActive))}
|
||||
if getattr(self.aol_card_intent, 'observe_active_engagement', False) else {})
|
||||
observe_physical = getattr(self.aol_card_intent, 'observe_physical_samples', None)
|
||||
if observe_physical is not None:
|
||||
observe_physical(self.CI.CS, now_ns=now_ns)
|
||||
self.aol_card_intent.update(CS, fault_active=fault_active, now_ns=now_ns, native_rejection_ns=rejection_ns,
|
||||
standard_enabled=(host_enabled if getattr(self.aol_card_intent, 'observe_stock_engagement', False)
|
||||
else host_control_enabled))
|
||||
# Observe ordinary state directly: independent AOL actuator activity is
|
||||
# not evidence that ordinary preEnabled has become active engagement.
|
||||
engagement_ns = int(self.sm.logMonoTime.get('selfdriveState', 0))
|
||||
engagement_seen = self.sm.seen.get('selfdriveState', False)
|
||||
engagement = self.sm['selfdriveState'] if engagement_seen else None
|
||||
self.aol_card_intent.observe_full_engagement(
|
||||
CS, enabled=bool(engagement is not None and engagement.enabled),
|
||||
active=bool(engagement is not None and engagement.active), source_ns=engagement_ns,
|
||||
now_ns=now_ns, fault_active=fault_active, native_rejection_ns=rejection_ns,
|
||||
observation_valid=bool(engagement_seen and self.sm.valid['selfdriveState'] and self.sm.alive['selfdriveState'] and
|
||||
0 < engagement_ns <= now_ns and now_ns - engagement_ns <= AXIS_MAX_AGE_NS))
|
||||
else host_control_enabled), **engagement_observation)
|
||||
if self.aol_card_intent is not None and preap_aol:
|
||||
self.CI.CS.preap_lateral_authorized = bool(
|
||||
native is not None and native.lateralAllowed and self.aol_card_intent.allowed_latch and
|
||||
@@ -655,8 +647,7 @@ class Car:
|
||||
manual_snapshot = self.manual_settings_owner.refresh(now_ns)
|
||||
manual_drive_id = int(self.sm['deviceState'].startedMonoTime)
|
||||
manual_verdict = self.manual_settings_owner.verdict(manual_snapshot, now_mono_ns=now_ns, drive_id=manual_drive_id)
|
||||
manual_fingerprint = (settings_fingerprint(manual_snapshot, owner=self.manual_settings_owner)
|
||||
if manual_verdict.status == 'ready' and manual_verdict.safe_mode is False and
|
||||
manual_fingerprint = (settings_fingerprint(manual_snapshot) if manual_verdict.status == 'ready' and manual_verdict.safe_mode is False and
|
||||
manual_verdict.selection is not None and manual_verdict.selection.choice in (ModeChoice.CEM, ModeChoice.CCM)
|
||||
else None)
|
||||
self.manual_button_tracker.keep_claim(manual_drive_id, manual_fingerprint)
|
||||
@@ -782,18 +773,25 @@ class Car:
|
||||
|
||||
def aol_honda_brake_source(self, CS, now_ns: int) -> int:
|
||||
observation = getattr(self, '_aol_can_observation', None)
|
||||
if (not CS.brakePressed or not qualified_honda_stock_aol(self.CP, marked_only=True) or
|
||||
observation is None or observation[0] is not CS):
|
||||
if not CS.brakePressed or observation is None or observation[0] is not CS:
|
||||
return 0
|
||||
from opendbc.car import Bus
|
||||
from opendbc.car.honda.values import HondaFlags
|
||||
parser = self.CI.can_parsers[Bus.pt]
|
||||
message = 'BRAKE_MODULE' if self.CP.flags & HondaFlags.BOSCH_ALT_BRAKE else 'POWERTRAIN_DATA'
|
||||
signals = parser.vl.get(message, {})
|
||||
# Match the real parser's applied-brake or two-sample switch decision.
|
||||
brake = bool(signals.get('BRAKE_PRESSED', 0) or message == 'POWERTRAIN_DATA' and
|
||||
signals.get('BRAKE_SWITCH', 0) and self.CI.CS.brake_switch_active)
|
||||
stamp = int(parser.ts_nanos.get(message, {}).get('BRAKE_PRESSED', 0))
|
||||
if qualified_honda_stock_aol(self.CP, marked_only=True):
|
||||
from opendbc.car.honda.values import HondaFlags
|
||||
message = 'BRAKE_MODULE' if self.CP.flags & HondaFlags.BOSCH_ALT_BRAKE else 'POWERTRAIN_DATA'
|
||||
signals = parser.vl.get(message, {})
|
||||
# Match the real parser's applied-brake or two-sample switch decision.
|
||||
brake = bool(signals.get('BRAKE_PRESSED', 0) or message == 'POWERTRAIN_DATA' and
|
||||
signals.get('BRAKE_SWITCH', 0) and self.CI.CS.brake_switch_active)
|
||||
stamp = int(parser.ts_nanos.get(message, {}).get('BRAKE_PRESSED', 0))
|
||||
elif qualified_ioniq6(self.CP):
|
||||
# Panda revokes LONG on this brake one frame before the host withdraws its
|
||||
# request; the source lets that window retain LAT intent like Honda's.
|
||||
brake = parser.vl.get('TCS', {}).get('DriverBraking', 0) == 1
|
||||
stamp = int(parser.ts_nanos.get('TCS', {}).get('DriverBraking', 0))
|
||||
else:
|
||||
return 0
|
||||
_, received_ns, can_source_ns, boot_ns = observation
|
||||
# CANParser/CAN batch use BOOTTIME; retain the original MONOTONIC receipt.
|
||||
if (not brake or not 0 < received_ns <= now_ns or now_ns - received_ns > 30_000_000 or
|
||||
@@ -827,8 +825,7 @@ class Car:
|
||||
events = [event for event in events if not
|
||||
(event.name in AVAILABILITY_EVENTS and event.softDisable and not event.immediateDisable)]
|
||||
if getattr(self.aol_transport_feedback, 'temporary_host_qualified', False):
|
||||
events = [event for event in events if not (event.name in
|
||||
(log.OnroadEvent.EventName.locationdTemporaryError, log.OnroadEvent.EventName.commIssue) and
|
||||
events = [event for event in events if not (event.name == log.OnroadEvent.EventName.locationdTemporaryError and
|
||||
event.softDisable and not event.immediateDisable)]
|
||||
return intent_disarming_fault(self.CP, events, CS,
|
||||
startup_calibration_pending=bool(getattr(self.aol_card_intent, 'startup_calibration_pending', False) and
|
||||
|
||||
@@ -8,7 +8,6 @@ from openpilot.cereal import log
|
||||
from opendbc.car.structs import car
|
||||
import openpilot.cereal.messaging as messaging
|
||||
from openpilot.common.constants import CV
|
||||
from openpilot.common.hardware import HARDWARE
|
||||
from openpilot.common.params import Params
|
||||
from openpilot.common.realtime import config_realtime_process, DT_CTRL, Priority, Ratekeeper, set_core_affinity
|
||||
from openpilot.common.swaglog import cloudlog
|
||||
@@ -76,9 +75,8 @@ class Controls:
|
||||
(['aolAxisState', 'aolSafetyWire'] if self.aol_replay or self.ordinary_axis_ack_required else []))
|
||||
optional = list(dict.fromkeys(self.longitudinal_inputs.optional_services + ['deviceState', 'autoSteeringLearning']))
|
||||
if optional:
|
||||
ignore = set(optional)
|
||||
self.sm = messaging.SubMaster(services + optional, poll='selfdriveState',
|
||||
ignore_alive=ignore, ignore_valid=ignore, ignore_avg_freq=ignore)
|
||||
ignore_alive=optional, ignore_valid=optional, ignore_avg_freq=optional)
|
||||
else:
|
||||
self.sm = messaging.SubMaster(services, poll='selfdriveState')
|
||||
self.pm = messaging.PubMaster(['carControl', 'controlsState', 'starpilotLateralState'])
|
||||
@@ -266,12 +264,11 @@ class Controls:
|
||||
self.LoC.reset()
|
||||
|
||||
# accel PID loop
|
||||
publish_takeoff_state = getattr(self.LoC, 'takeoff_starting', False)
|
||||
pid_accel_limits = self.CI.get_pid_accel_limits(self.CP, CS.vEgo, CS.vCruise * CV.KPH_TO_MS)
|
||||
longitudinal_output = self.LoC.update(CC.longActive, CS, long_plan.aTarget, long_plan.shouldStop, pid_accel_limits,
|
||||
context=self.longitudinal_inputs.context(CC.longActive))
|
||||
actuators.accel = final_output(longitudinal_output)
|
||||
if self.longitudinal_inputs.publish_state or publish_takeoff_state:
|
||||
if self.longitudinal_inputs.publish_state:
|
||||
# Publish the selected launch state to the vehicle controller in this same frame.
|
||||
actuators.longControlState = self.LoC.long_control_state
|
||||
|
||||
@@ -343,7 +340,10 @@ class Controls:
|
||||
not any(event.softDisable or event.immediateDisable for event in self.sm['onroadEvents']) and
|
||||
not self.sm['driverMonitoringState'].noResponseForceDecel), controller=self.LaC,
|
||||
native_required=bool(self.aol_replay or getattr(self, 'ordinary_axis_ack_required', False)),
|
||||
native_witness=getattr(self, 'flm_native_witness', None))
|
||||
native_witness=getattr(self, 'flm_native_witness', None),
|
||||
manual_current=bool(self.torque_host is not None and
|
||||
(self.torque_host.settings.user_factor is not None or self.torque_host.settings.user_friction is not None) or
|
||||
self.lateral_gain_owner is not None and self.lateral_gain_owner.current_table != self.lateral_gain_owner.basis.source_table))
|
||||
automatic.begin_frame(CS, self.VM, lp, active=bool(CC.latActive),
|
||||
safety_limited=self.steer_limited_by_safety, desired_curvature=self.desired_curvature,
|
||||
curvature_limited=curvature_limited, lat_delay=lat_delay,
|
||||
@@ -496,8 +496,6 @@ def main():
|
||||
config_realtime_process(4, Priority.CTRL_HIGH)
|
||||
controls = Controls()
|
||||
set_core_affinity([4, 5])
|
||||
if HARDWARE.get_device_type() in ("tici", "tizi", "mici"):
|
||||
os.sched_setscheduler(0, os.SCHED_FIFO, os.sched_param(Priority.CTRL_HIGH - 1))
|
||||
controls.run()
|
||||
|
||||
|
||||
|
||||
@@ -1,7 +1,6 @@
|
||||
import numpy as np
|
||||
from openpilot.common.constants import ACCELERATION_DUE_TO_GRAVITY
|
||||
from openpilot.common.realtime import DT_CTRL, DT_MDL
|
||||
from openpilot.common.scalar_clip import clip_scalar
|
||||
|
||||
MIN_SPEED = 1.0
|
||||
CONTROL_N = 17
|
||||
@@ -18,7 +17,7 @@ def should_stop(v_ego: float, a_target: float) -> bool:
|
||||
return bool(v_ego < 0.3 and a_target < 0.1)
|
||||
|
||||
def clamp(val, min_val, max_val):
|
||||
clamped_val = clip_scalar(val, min_val, max_val)
|
||||
clamped_val = float(np.clip(val, min_val, max_val))
|
||||
return clamped_val, clamped_val != val
|
||||
|
||||
def smooth_value(val, prev_val, tau, dt=DT_MDL):
|
||||
|
||||
@@ -5,7 +5,6 @@ from openpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N
|
||||
from openpilot.common.pid import PIDController
|
||||
from openpilot.selfdrive.modeld.constants import ModelConstants
|
||||
from openpilot.starpilot.longitudinal.extension import LongitudinalContext, create_extension
|
||||
from openpilot.starpilot.longitudinal.takeoffs import TakeoffStart, eligible as takeoff_eligible
|
||||
|
||||
CONTROL_N_T_IDX = ModelConstants.T_IDXS[:CONTROL_N]
|
||||
|
||||
@@ -48,8 +47,6 @@ class LongControl:
|
||||
else (CP.longitudinalTuning.kiBP, CP.longitudinalTuning.kiV)),
|
||||
rate=1 / DT_CTRL)
|
||||
self.last_output_accel = 0.0
|
||||
self.takeoff_start = TakeoffStart() if takeoff_eligible(CP) else None
|
||||
self.takeoff_starting = False
|
||||
|
||||
def reset(self, *, reset_start: bool = True):
|
||||
self.pid.reset()
|
||||
@@ -62,28 +59,15 @@ class LongControl:
|
||||
self.pid.neg_limit = accel_limits[0]
|
||||
self.pid.pos_limit = accel_limits[1]
|
||||
|
||||
planner_target = a_target
|
||||
previous_state = self.long_control_state
|
||||
enabled_takeoff = bool(context.faster_takeoffs and self.takeoff_start is not None)
|
||||
ioniq_start = self.extension is not None and self.extension.ioniq6_start is not None
|
||||
native_previous = (LongCtrlState.pid if previous_state == LongCtrlState.starting and
|
||||
(self.takeoff_starting or enabled_takeoff and ioniq_start) else previous_state)
|
||||
native_state = long_control_state_trans(active, native_previous, should_stop,
|
||||
native_state = long_control_state_trans(active, previous_state, should_stop,
|
||||
CS.brakePressed, CS.cruiseState.standstill and
|
||||
not (self.extension is not None and self.extension.ignore_cruise_standstill))
|
||||
owned_start = self.extension is not None and any((self.extension.gm_start, self.extension.vehicle_stop,
|
||||
self.extension.stopping_policy))
|
||||
selected = bool(enabled_takeoff and not owned_start and self.takeoff_start.select(
|
||||
active=active, CS=CS, target=a_target, should_stop=should_stop, limits=accel_limits,
|
||||
evidence=context.start_evidence, stock_hold_allowed=ioniq_start))
|
||||
if selected:
|
||||
native_state = LongCtrlState.starting
|
||||
self.long_control_state = native_state
|
||||
if self.extension is not None:
|
||||
self.long_control_state = self.extension.transition(
|
||||
native_state, previous_state, active, CS, a_target, should_stop, accel_limits,
|
||||
context=context)
|
||||
self.takeoff_starting = selected and self.long_control_state == LongCtrlState.starting
|
||||
if self.long_control_state == LongCtrlState.off:
|
||||
self.reset()
|
||||
output_accel = 0.
|
||||
@@ -115,7 +99,5 @@ class LongControl:
|
||||
output_accel = self.extension.shape_output(output_accel, a_target, error, CS, self.last_output_accel,
|
||||
previous_state, should_stop, context)
|
||||
|
||||
if self.takeoff_starting:
|
||||
output_accel = min(output_accel, planner_target)
|
||||
self.last_output_accel = np.clip(output_accel, accel_limits[0], accel_limits[1])
|
||||
return self.last_output_accel
|
||||
|
||||
@@ -48,8 +48,6 @@ NATIVE_PLAN_INPUTS = ('carControl', 'carState', 'controlsState', 'vehicleParamet
|
||||
# Lookup table for turns
|
||||
_A_TOTAL_MAX_V = [1.7, 3.2]
|
||||
_A_TOTAL_MAX_BP = [20., 40.]
|
||||
_TAKEOFF_TOTAL_BP = [0., 2., 5., 20., 40.]
|
||||
_TAKEOFF_TOTAL_MAX = [3.5, 3.5, 1.7, 1.7, 3.2]
|
||||
|
||||
def get_max_accel(v_ego):
|
||||
return np.interp(v_ego, A_CRUISE_MAX_BP, A_CRUISE_MAX_VALS)
|
||||
@@ -59,7 +57,7 @@ def get_coast_accel(pitch):
|
||||
|
||||
def get_cruise_accel(e2e, v_cruise, v_ego, a_cruise_prev, angle_steers, CP, dt, accel_coast, allow_throttle,
|
||||
profile: AppliedProfile | None = None, cruise_brake_floor: float | None = None,
|
||||
base_brake_floor: float = A_CRUISE_MIN, selected_acceleration_max: float | None = None, faster_takeoffs: bool = False):
|
||||
base_brake_floor: float = A_CRUISE_MIN, selected_acceleration_max: float | None = None):
|
||||
max_accel = ACCEL_MAX if e2e else get_max_accel(v_ego)
|
||||
if selected_acceleration_max is not None and not e2e:
|
||||
max_accel = selected_acceleration_max
|
||||
@@ -70,7 +68,7 @@ def get_cruise_accel(e2e, v_cruise, v_ego, a_cruise_prev, angle_steers, CP, dt,
|
||||
max_accel = min(max_accel, selected_acceleration_max, ACCEL_MAX)
|
||||
|
||||
if not e2e:
|
||||
a_total_max = np.interp(v_ego, _TAKEOFF_TOTAL_BP, _TAKEOFF_TOTAL_MAX) if faster_takeoffs else np.interp(v_ego, _A_TOTAL_MAX_BP, _A_TOTAL_MAX_V)
|
||||
a_total_max = np.interp(v_ego, _A_TOTAL_MAX_BP, _A_TOTAL_MAX_V)
|
||||
a_y = v_ego ** 2 * angle_steers * CV.DEG_TO_RAD / (CP.steerRatio * CP.wheelbase)
|
||||
a_x_allowed = math.sqrt(max(a_total_max ** 2 - a_y ** 2, 0.))
|
||||
max_accel = min(max_accel, a_x_allowed)
|
||||
@@ -137,7 +135,6 @@ class LongitudinalPlanner:
|
||||
if curve_ceiling is not None and curve_provider is not None:
|
||||
raise ValueError('choose explicit Curve ceiling or same-cycle provider')
|
||||
self.plan_drive_id = drive_id
|
||||
faster_takeoffs = bool(faster_lead_takeoff)
|
||||
sample_now_ns = self.clock_ns() if now_ns is None else now_ns
|
||||
if len(sm['carControl'].orientationNED) == 3:
|
||||
accel_coast = get_coast_accel(sm['carControl'].orientationNED[1])
|
||||
@@ -370,7 +367,7 @@ class LongitudinalPlanner:
|
||||
self.a_cruise = get_cruise_accel(sm['selfdriveState'].experimentalMode, v_cruise, v_ego,
|
||||
self.a_cruise, steer_angle_without_offset, self.CP, self.dt,
|
||||
accel_coast, self.allow_throttle, cruise_profile, slc_floor,
|
||||
base_brake_floor=global_floor, selected_acceleration_max=selected_acceleration, faster_takeoffs=faster_takeoffs)
|
||||
base_brake_floor=global_floor, selected_acceleration_max=selected_acceleration)
|
||||
cruise_should_stop = should_stop(v_ego, self.a_cruise)
|
||||
|
||||
output_a_target_e2e = self.accel_boost.update(sm, output_a_target_e2e, output_a_target_mpc, self.a_cruise)
|
||||
@@ -412,7 +409,7 @@ class LongitudinalPlanner:
|
||||
takeoff_max = min(takeoff_max, cruise_profile.acceleration_max)
|
||||
if selected_acceleration is not None:
|
||||
takeoff_max = min(takeoff_max, selected_acceleration)
|
||||
a_total_max = np.interp(v_ego, _TAKEOFF_TOTAL_BP, _TAKEOFF_TOTAL_MAX) if faster_takeoffs else np.interp(v_ego, _A_TOTAL_MAX_BP, _A_TOTAL_MAX_V)
|
||||
a_total_max = np.interp(v_ego, _A_TOTAL_MAX_BP, _A_TOTAL_MAX_V)
|
||||
a_y = v_ego ** 2 * steer_angle_without_offset * CV.DEG_TO_RAD / (self.CP.steerRatio * self.CP.wheelbase)
|
||||
takeoff_max = min(takeoff_max, math.sqrt(max(a_total_max ** 2 - a_y ** 2, 0.)), max(0., v_cruise - v_ego))
|
||||
takeoff_frame = project_takeoff(
|
||||
|
||||
@@ -20,7 +20,6 @@ from openpilot.starpilot.conditional_mode.adjacent import (
|
||||
AdjacentFrame, AdjacentObservation, AdjacentTrack, MonoSource, UNKNOWN, evaluate,
|
||||
)
|
||||
from openpilot.starpilot.feature_runtime import enabled as feature_enabled
|
||||
from openpilot.starpilot.longitudinal.voacc import selected as voacc_selected
|
||||
|
||||
|
||||
# Default lead acceleration decay set to 50% at 1s
|
||||
@@ -201,8 +200,7 @@ def get_lead(v_ego: float, ready: bool, tracks: dict[int, Track], lead_msg: capn
|
||||
|
||||
class RadarD:
|
||||
def __init__(self, delay: float = 0.0, *, adjacent_enabled: bool = False, radar_available: bool = False,
|
||||
clock_pair_fn=None, honda_bosch_a_radar: bool = False, use_voacc: bool = False):
|
||||
self.use_voacc = use_voacc
|
||||
clock_pair_fn=None, honda_bosch_a_radar: bool = False):
|
||||
self.tracks: dict[int, Track] = {}
|
||||
self.honda_bosch_a = None
|
||||
if honda_bosch_a_radar:
|
||||
@@ -347,12 +345,7 @@ class RadarD:
|
||||
else:
|
||||
self.lead_prob_filters[i].update(lead_prob)
|
||||
|
||||
if self.use_voacc:
|
||||
self.radar_state.leadOne = get_lead(self.v_ego, self.ready, {}, leads_v3[0], model_v_ego,
|
||||
self.lead_prob_filters[0].x, low_speed_override=False)
|
||||
self.radar_state.leadTwo = get_lead(self.v_ego, self.ready, {}, leads_v3[1], model_v_ego,
|
||||
self.lead_prob_filters[1].x, low_speed_override=False)
|
||||
elif self.honda_bosch_a is not None:
|
||||
if self.honda_bosch_a is not None:
|
||||
self.radar_state.leadOne, self.radar_state.leadTwo = self.honda_bosch_a.leads(sm, self, model_v_ego)
|
||||
else:
|
||||
self.radar_state.leadOne = get_lead(self.v_ego, self.ready, self.tracks, leads_v3[0], model_v_ego, self.lead_prob_filters[0].x, low_speed_override=True)
|
||||
@@ -415,7 +408,7 @@ def main() -> None:
|
||||
from opendbc.car.honda.bosch_a_radar import VERIFIED_BOSCH_A_CARS
|
||||
honda_bosch_a = CP.brand == "honda" and CP.carFingerprint in VERIFIED_BOSCH_A_CARS and not CP.radarUnavailable
|
||||
RD = RadarD(CP.radarDelay, adjacent_enabled=adjacent_enabled, radar_available=not CP.radarUnavailable,
|
||||
honda_bosch_a_radar=honda_bosch_a, use_voacc=voacc_selected(params, CP))
|
||||
honda_bosch_a_radar=honda_bosch_a)
|
||||
set_core_affinity([4, 5])
|
||||
|
||||
while 1:
|
||||
|
||||
@@ -1,54 +0,0 @@
|
||||
from unittest import mock
|
||||
from types import SimpleNamespace
|
||||
|
||||
import pytest
|
||||
|
||||
from openpilot.selfdrive.controls import controlsd
|
||||
|
||||
|
||||
@pytest.mark.parametrize("device", ("mici", "tizi", "tici", "pc", "unknown"))
|
||||
def test_controls_main_priority_is_applied_after_initialization_and_affinity(device):
|
||||
calls = []
|
||||
controls = mock.Mock()
|
||||
controls.run.side_effect = lambda: calls.append(("run",))
|
||||
|
||||
def priority_changed(tid, policy, param):
|
||||
calls.append(("priority", tid, policy, param.sched_priority))
|
||||
with (mock.patch.object(controlsd, "config_realtime_process", side_effect=lambda core, priority: calls.append(("configure", core, priority))),
|
||||
mock.patch.object(controlsd, "Controls", side_effect=lambda: calls.append(("construct",)) or controls),
|
||||
mock.patch.object(controlsd, "set_core_affinity", side_effect=lambda cores: calls.append(("affinity", cores))),
|
||||
mock.patch.object(controlsd.HARDWARE, "get_device_type", return_value=device),
|
||||
mock.patch.object(controlsd.os, "SCHED_FIFO", getattr(controlsd.os, "SCHED_FIFO", 1), create=True),
|
||||
mock.patch.object(controlsd.os, "sched_param", getattr(controlsd.os, "sched_param",
|
||||
lambda priority: SimpleNamespace(sched_priority=priority)), create=True),
|
||||
mock.patch.object(controlsd.os, "sched_setscheduler", create=True, side_effect=priority_changed)):
|
||||
controlsd.main()
|
||||
fifo = controlsd.os.SCHED_FIFO
|
||||
expected = [("configure", 4, controlsd.Priority.CTRL_HIGH), ("construct",), ("affinity", [4, 5])]
|
||||
if device in ("tici", "tizi", "mici"):
|
||||
expected.append(("priority", 0, fifo, 52))
|
||||
assert calls == [*expected, ("run",)]
|
||||
|
||||
|
||||
def test_constructor_failure_does_not_reprioritize_existing_threads():
|
||||
with (mock.patch.object(controlsd, "config_realtime_process"),
|
||||
mock.patch.object(controlsd, "Controls", side_effect=RuntimeError("initialization failed")),
|
||||
mock.patch.object(controlsd.os, "sched_setscheduler", create=True) as priority,
|
||||
pytest.raises(RuntimeError, match="initialization failed")):
|
||||
controlsd.main()
|
||||
priority.assert_not_called()
|
||||
|
||||
|
||||
def test_scheduler_failure_does_not_start_controls():
|
||||
controls = mock.Mock()
|
||||
with (mock.patch.object(controlsd, "config_realtime_process"),
|
||||
mock.patch.object(controlsd, "Controls", return_value=controls),
|
||||
mock.patch.object(controlsd, "set_core_affinity"),
|
||||
mock.patch.object(controlsd.HARDWARE, "get_device_type", return_value="mici"),
|
||||
mock.patch.object(controlsd.os, "SCHED_FIFO", getattr(controlsd.os, "SCHED_FIFO", 1), create=True),
|
||||
mock.patch.object(controlsd.os, "sched_param", getattr(controlsd.os, "sched_param",
|
||||
lambda priority: SimpleNamespace(sched_priority=priority)), create=True),
|
||||
mock.patch.object(controlsd.os, "sched_setscheduler", create=True, side_effect=PermissionError("priority denied")),
|
||||
pytest.raises(PermissionError, match="priority denied")):
|
||||
controlsd.main()
|
||||
controls.run.assert_not_called()
|
||||
@@ -1,112 +0,0 @@
|
||||
"""Actual RadarD lead selection without changing radar health or input data."""
|
||||
|
||||
from types import SimpleNamespace
|
||||
from typing import cast
|
||||
from pathlib import Path
|
||||
import tempfile
|
||||
import unittest
|
||||
from unittest.mock import Mock, patch
|
||||
|
||||
from openpilot.selfdrive.controls import radard
|
||||
from openpilot.selfdrive.controls.radard import RadarD
|
||||
from openpilot.selfdrive.controls.tests.test_adjacent_radar_transport import Captured, MONO, Sources
|
||||
from openpilot.starpilot.longitudinal.voacc import selected
|
||||
from openpilot.starpilot.car.honda.radar import BoschALeadPolicy
|
||||
|
||||
|
||||
class VoaccTest(unittest.TestCase):
|
||||
def source(self, *, vision=True):
|
||||
source = Sources(MONO, points=((17, 5., 0., 0.),))
|
||||
source.data['radarTracks'].init('errors')
|
||||
source.data['carState'].vEgo = 0.
|
||||
for lead in source.data['modelV2'].leadsV3:
|
||||
lead.prob = .99 if vision else 0.
|
||||
lead.x, lead.y, lead.v, lead.a = [20.], [0.], [15.], [0.]
|
||||
lead.xStd, lead.yStd, lead.vStd = [1.], [1.], [1.]
|
||||
return source
|
||||
|
||||
def test_vision_only_both_leads_ignore_low_speed_radar_override(self):
|
||||
source = self.source()
|
||||
raw = {name: value.to_dict() for name, value in source.data.items()}
|
||||
normal, vision = RadarD(), RadarD(use_voacc=True)
|
||||
normal.update(source, source['radarTracks'])
|
||||
vision.update(source, source['radarTracks'])
|
||||
self.assertTrue(normal.radar_state.leadOne.radar)
|
||||
self.assertEqual(normal.radar_state.leadOne.dRel, 5.)
|
||||
for lead in (vision.radar_state.leadOne, vision.radar_state.leadTwo):
|
||||
self.assertTrue(lead.present)
|
||||
self.assertFalse(lead.radar)
|
||||
self.assertEqual(lead.radarTrackId, -1)
|
||||
self.assertAlmostEqual(lead.dRel, 18.48, delta=1e-6)
|
||||
self.assertEqual(vision.tracks[17].dRel, 5.)
|
||||
self.assertEqual(raw, {name: value.to_dict() for name, value in source.data.items()})
|
||||
|
||||
def test_no_vision_lead_does_not_use_radar_only_lead(self):
|
||||
source = self.source(vision=False)
|
||||
radar = RadarD(use_voacc=True)
|
||||
radar.update(source, source['radarTracks'])
|
||||
self.assertFalse(radar.radar_state.leadOne.present)
|
||||
self.assertFalse(radar.radar_state.leadTwo.present)
|
||||
|
||||
def test_radar_errors_and_message_validity_are_preserved(self):
|
||||
for field in ('canError', 'radarFault', 'wrongConfig', 'radarUnavailableTemporary'):
|
||||
with self.subTest(field=field):
|
||||
source = self.source()
|
||||
setattr(source.data['radarTracks'].errors, field, True)
|
||||
source.valid['radarTracks'] = False
|
||||
radar = RadarD(use_voacc=True)
|
||||
radar.update(source, source['radarTracks'])
|
||||
publisher = Captured()
|
||||
radar.publish(publisher)
|
||||
result = publisher.messages['radarState']
|
||||
self.assertFalse(result.valid)
|
||||
self.assertTrue(getattr(result.radarState.radarErrors, field))
|
||||
|
||||
def test_specialized_honda_radar_leads_are_bypassed_but_tracks_still_update(self):
|
||||
source = self.source()
|
||||
radar = RadarD(honda_bosch_a_radar=True, use_voacc=True)
|
||||
policy = SimpleNamespace(update_tracks=Mock(), leads=Mock(side_effect=AssertionError("radar lead path")))
|
||||
radar.honda_bosch_a = cast(BoschALeadPolicy, policy)
|
||||
radar.update(source, source['radarTracks'])
|
||||
policy.update_tracks.assert_called_once_with(source, source['radarTracks'], radar)
|
||||
policy.leads.assert_not_called()
|
||||
self.assertFalse(radar.radar_state.leadOne.radar)
|
||||
|
||||
def test_default_off_keeps_normal_output(self):
|
||||
source = self.source()
|
||||
default, disabled = RadarD(), RadarD(use_voacc=False)
|
||||
default.update(source, source['radarTracks'])
|
||||
disabled.update(source, source['radarTracks'])
|
||||
self.assertEqual(default.radar_state.to_dict(), disabled.radar_state.to_dict())
|
||||
|
||||
def test_main_latches_setting_until_next_drive(self):
|
||||
with tempfile.TemporaryDirectory() as directory:
|
||||
path = Path(directory) / 'UseVOACC'
|
||||
path.write_bytes(b'1')
|
||||
params = SimpleNamespace(get=lambda *args, **kwargs: b'cp', get_param_path=lambda key: str(path))
|
||||
cp = SimpleNamespace(brand='toyota', carFingerprint='test', radarDelay=0., radarUnavailable=False,
|
||||
openpilotLongitudinalControl=True, notCar=False, dashcamOnly=False, passive=False)
|
||||
source = self.source()
|
||||
source.update = Mock(side_effect=(None, StopIteration))
|
||||
instances = []
|
||||
|
||||
def create(*args, **kwargs):
|
||||
result = RadarD(*args, **kwargs)
|
||||
instances.append(result)
|
||||
path.write_bytes(b'0')
|
||||
return result
|
||||
|
||||
with patch.object(radard, 'Params', return_value=params), \
|
||||
patch.object(radard, 'config_realtime_process'), patch.object(radard, 'set_core_affinity'), \
|
||||
patch.object(radard, 'feature_enabled', return_value=False), \
|
||||
patch.object(radard.messaging, 'log_from_bytes', return_value=cp), \
|
||||
patch.object(radard.messaging, 'SubMaster', return_value=source), \
|
||||
patch.object(radard.messaging, 'PubMaster', return_value=Captured()), \
|
||||
patch.object(radard, 'voacc_selected', wraps=selected) as read, \
|
||||
patch.object(radard, 'RadarD', side_effect=create):
|
||||
with self.assertRaises(StopIteration):
|
||||
radard.main()
|
||||
read.assert_called_once_with(params, cp)
|
||||
self.assertTrue(instances[0].use_voacc)
|
||||
self.assertFalse(instances[0].radar_state.leadOne.radar)
|
||||
self.assertFalse(selected(params, cp), 'Next drive reads the newly saved Off')
|
||||
@@ -19,12 +19,3 @@ if arch != "Darwin":
|
||||
|
||||
if GetOption('extras') or any(target.startswith('openpilot/selfdrive/pandad/tests/') for target in COMMAND_LINE_TARGETS):
|
||||
env.Program('tests/test_panda_owner', ['tests/test_panda_owner.cc', 'panda_safety.cc'], LIBS=[panda] + libs)
|
||||
|
||||
# Exercise the real publisher/worker with injected Panda and sysfs I/O only.
|
||||
if GetOption('extras') or any(target.startswith('openpilot/selfdrive/pandad/tests/') for target in COMMAND_LINE_TARGETS):
|
||||
power_test = env.Clone()
|
||||
power_test.Append(LINKFLAGS=[
|
||||
'-Wl,--wrap=_ZN4util9read_fileERKNSt7__cxx1112basic_stringIcSt11char_traitsIcESaIcEEE',
|
||||
'-Wl,--wrap=_ZN9PubMaster4sendEPKcR14MessageBuilder',
|
||||
])
|
||||
power_test.Program('tests/test_peripheral_state', ['tests/test_peripheral_state.cc', power_test.Object('tests/peripheral_panda_safety.o', 'panda_safety.cc')], LIBS=[panda] + libs)
|
||||
|
||||
@@ -143,16 +143,7 @@ public:
|
||||
axis.retain_lateral_arm};
|
||||
}
|
||||
}
|
||||
// A short command lease expiring must withdraw both actuator requests.
|
||||
// A fresh Card arm, bound to this already-negotiated session, may still
|
||||
// retain the physical latch within the existing axis-message age limit.
|
||||
// It cannot negotiate a new session or preserve any command permission.
|
||||
const bool retain_arm = !reset_pending_ && axis.alive_valid && axis.qualified &&
|
||||
!session_.empty() && axis.session == session_ && axis.retain_lateral_arm &&
|
||||
axis.observed_ns > 0U && axis.message_ns == axis.observed_ns &&
|
||||
axis.observed_ns <= now_ns && axis.valid_until_ns >= axis.observed_ns &&
|
||||
now_ns - axis.message_ns <= AOL_AXIS_MESSAGE_MAX_AGE_NS;
|
||||
return {true, 0U, retain_arm};
|
||||
return {true, 0U};
|
||||
}
|
||||
|
||||
AolOutcome complete(const AolWritePlan &plan, bool write_ok,
|
||||
|
||||
@@ -93,19 +93,12 @@ inline std::vector<uint8_t> encode_aol_safety_wire(const AolSafetyWireFields &fi
|
||||
// Card's armed bit keeps a previously authorized session alive while its axis
|
||||
// request is zero. It never grants an actuator request or native permission.
|
||||
inline bool aol_armed_intent(kj::ArrayPtr<const capnp::byte> bytes, uint64_t message_ns,
|
||||
uint64_t car_state_ns, uint64_t now_ns, bool optional_release = false,
|
||||
const std::string &producer_session = {}) {
|
||||
uint64_t car_state_ns, uint64_t now_ns, bool optional_release = false) {
|
||||
constexpr uint64_t MAX_AGE_NS = 30000000ULL;
|
||||
// A newer, current Card packet can preserve its own arm across a delayed
|
||||
// axis publication. Bind it to the Card producer acknowledged by that axis;
|
||||
// the negotiator separately withdraws expired actuator requests.
|
||||
const bool newer_armed_source = !optional_release && !producer_session.empty() &&
|
||||
message_ns >= car_state_ns && message_ns - car_state_ns <= 200000000ULL;
|
||||
if (bytes.size() < 16U || bytes.size() > 512U || bytes.size() % 8U != 0U ||
|
||||
message_ns == 0U || car_state_ns == 0U || message_ns > now_ns ||
|
||||
car_state_ns > now_ns || now_ns - message_ns > MAX_AGE_NS ||
|
||||
(!newer_armed_source &&
|
||||
(message_ns > car_state_ns ? message_ns - car_state_ns : car_state_ns - message_ns) > MAX_AGE_NS)) return false;
|
||||
(message_ns > car_state_ns ? message_ns - car_state_ns : car_state_ns - message_ns) > MAX_AGE_NS) return false;
|
||||
uint32_t segments = 0U, words = 0U;
|
||||
std::memcpy(&segments, bytes.begin(), sizeof(segments));
|
||||
std::memcpy(&words, bytes.begin() + 4U, sizeof(words));
|
||||
@@ -121,9 +114,6 @@ inline bool aol_armed_intent(kj::ArrayPtr<const capnp::byte> bytes, uint64_t mes
|
||||
auto producer = intent.getProducerSessionId();
|
||||
return intent.getKind() == 2U && intent.getVersion() == 1U &&
|
||||
producer.size() > 0U && producer.size() <= 96U &&
|
||||
(producer_session.empty() || (producer_session.size() == producer.size() &&
|
||||
std::memcmp(producer_session.data(), producer.begin(), producer.size()) == 0)) &&
|
||||
(!newer_armed_source || (intent.getSequence() > 0U && intent.getObservedMonoTime() == message_ns)) &&
|
||||
intent.getSettingsQualified() && (optional_release ? intent.getOptionalSetRelease() : intent.getLateralArmed()) &&
|
||||
intent.getCarStateLogMonoTime() == message_ns &&
|
||||
intent.getObservedMonoTime() <= now_ns &&
|
||||
|
||||
Binary file not shown.
Binary file not shown.
@@ -11,7 +11,6 @@
|
||||
|
||||
#include <algorithm>
|
||||
#include <array>
|
||||
#include <atomic>
|
||||
#include <bitset>
|
||||
#include <cassert>
|
||||
#include <cerrno>
|
||||
@@ -35,20 +34,6 @@
|
||||
|
||||
ExitHandler do_exit;
|
||||
|
||||
// One atomic pair keeps voltage/current from the same hwmon sample together.
|
||||
std::atomic<uint64_t> hwmon_power{0};
|
||||
|
||||
void hwmon_thread() {
|
||||
if (Hardware::get_device_type() != cereal::InitData::DeviceType::TICI) return;
|
||||
util::set_thread_name("pandad_hwmon");
|
||||
while (!do_exit) {
|
||||
const uint32_t voltage = Hardware::get_voltage();
|
||||
const uint32_t current = Hardware::get_current();
|
||||
hwmon_power.store((uint64_t(voltage) << 32) | current);
|
||||
util::sleep_for(500);
|
||||
}
|
||||
}
|
||||
|
||||
bool check_all_connected(const std::vector<Panda *> &pandas) {
|
||||
for (const auto& panda : pandas) {
|
||||
if (!panda->connected()) {
|
||||
@@ -314,13 +299,9 @@ void send_peripheral_state(Panda *panda, PubMaster *pm) {
|
||||
auto ps = evt.initPeripheralState();
|
||||
ps.setPandaType(panda->hw_type);
|
||||
|
||||
const health_t &health = *health_opt;
|
||||
// The original comma 3 measures input power with its SOM hwmon sensor.
|
||||
// Its DOS Panda ADC is not the source used by Dom. Keep slow sysfs reads
|
||||
// in the helper thread; newer devices continue to report Panda power.
|
||||
const uint64_t power = hwmon_power.load();
|
||||
ps.setVoltage(power ? uint32_t(power >> 32) : health.voltage_pkt);
|
||||
ps.setCurrent(power ? uint32_t(power) : health.current_pkt);
|
||||
health_t health = *health_opt;
|
||||
ps.setVoltage(health.voltage_pkt);
|
||||
ps.setCurrent(health.current_pkt);
|
||||
|
||||
uint16_t fan_speed_rpm = panda->get_fan_speed();
|
||||
ps.setFanSpeedRpm(fan_speed_rpm);
|
||||
@@ -462,11 +443,6 @@ void pandad_run(std::vector<Panda *> &pandas) {
|
||||
|
||||
// Start helper thread for event-driven sendcan.
|
||||
std::thread send_thread(can_send_thread, pandas, fake_send);
|
||||
hwmon_power.store(0);
|
||||
std::thread hardware_thread;
|
||||
if (Hardware::get_device_type() == cereal::InitData::DeviceType::TICI) {
|
||||
hardware_thread = std::thread(hwmon_thread);
|
||||
}
|
||||
|
||||
RateKeeper rk("pandad", 100);
|
||||
SubMaster sm({"selfdriveState", "deviceState", "aolAxisState", "aolIntentWire"});
|
||||
@@ -520,8 +496,7 @@ void pandad_run(std::vector<Panda *> &pandas) {
|
||||
if (is_onroad && sm.allAliveAndValid({"deviceState", "aolIntentWire"})) {
|
||||
auto intent_event = sm["aolIntentWire"];
|
||||
axis_input.retain_lateral_arm = aol_armed_intent(intent_event.getAolIntentWire(),
|
||||
intent_event.getLogMonoTime(), axis.getSourceCarStateMonoTime(), now_ns,
|
||||
false, axis.getIntentProducerSessionId().cStr());
|
||||
intent_event.getLogMonoTime(), axis.getSourceCarStateMonoTime(), now_ns);
|
||||
axis_input.optional_set_release = axis.getOptionalSetRelease() &&
|
||||
aol_armed_intent(intent_event.getAolIntentWire(), intent_event.getLogMonoTime(),
|
||||
axis.getSourceCarStateMonoTime(), now_ns, true);
|
||||
@@ -599,7 +574,6 @@ void pandad_run(std::vector<Panda *> &pandas) {
|
||||
}
|
||||
|
||||
send_thread.join();
|
||||
if (hardware_thread.joinable()) hardware_thread.join();
|
||||
}
|
||||
|
||||
void pandad_main_thread(std::vector<std::string> serials) {
|
||||
|
||||
@@ -615,63 +615,6 @@ int main() {
|
||||
result = run(paused_negotiator, paused_transport, paused);
|
||||
assert(result.plan.request_mask == 0U && !result.plan.retain_lateral_arm);
|
||||
|
||||
// Route 78511c37de32c375/0000002f--338c7a2e1f/1: the latest axis
|
||||
// expired 1.19 ms before pandad's tick while a newer Card arm was current.
|
||||
// Expire both commands, retain only the existing physical arm, and require
|
||||
// a new current axis before steering can resume. LONG never resumes here.
|
||||
AolAxisNegotiator gap_negotiator(TEST_REGISTRY);
|
||||
FakeTransport gap_transport;
|
||||
queue(gap_transport, status(), status());
|
||||
result = run(gap_negotiator, gap_transport, axis("gap-session"));
|
||||
assert(result.plan.request_mask == 0U && result.outcome.compatible);
|
||||
queue(gap_transport, status(), status(3U, 3U));
|
||||
result = run(gap_negotiator, gap_transport, axis("gap-session", true, true));
|
||||
assert(result.plan.request_mask == 3U);
|
||||
auto expired_axis = axis("gap-session", true, true);
|
||||
expired_axis.observed_ns = 91533516787ULL;
|
||||
expired_axis.message_ns = expired_axis.observed_ns;
|
||||
expired_axis.valid_until_ns = 91563516787ULL;
|
||||
expired_axis.retain_lateral_arm = true;
|
||||
constexpr uint64_t GAP_NOW = 91564707308ULL;
|
||||
queue(gap_transport, status(3U, 3U), status());
|
||||
result = run(gap_negotiator, gap_transport, expired_axis, GAP_NOW);
|
||||
assert(result.plan.capable && result.plan.request_mask == 0U && result.plan.retain_lateral_arm);
|
||||
for (int refusal = 0; refusal < 9; ++refusal) {
|
||||
auto denied = expired_axis;
|
||||
if (refusal == 0) denied.retain_lateral_arm = false;
|
||||
if (refusal == 1) denied.alive_valid = false;
|
||||
if (refusal == 2) denied.qualified = false;
|
||||
if (refusal == 3) denied.session = "replacement-session";
|
||||
if (refusal == 4) {
|
||||
denied.observed_ns = denied.message_ns = GAP_NOW - AOL_AXIS_MESSAGE_MAX_AGE_NS - 1U;
|
||||
denied.valid_until_ns = denied.observed_ns + 30000000ULL;
|
||||
}
|
||||
if (refusal == 5) {
|
||||
denied.observed_ns = denied.message_ns = GAP_NOW + 1U;
|
||||
denied.valid_until_ns = denied.observed_ns + 30000000ULL;
|
||||
}
|
||||
if (refusal == 6) denied.valid_until_ns = denied.observed_ns - 1U;
|
||||
if (refusal == 7) denied.observed_ns = 0U;
|
||||
if (refusal == 8) denied.message_ns -= 1U;
|
||||
queue(gap_transport, status(), status());
|
||||
result = run(gap_negotiator, gap_transport, denied, GAP_NOW);
|
||||
assert(result.plan.request_mask == 0U && !result.plan.retain_lateral_arm);
|
||||
}
|
||||
auto resumed = axis("gap-session", true, false);
|
||||
resumed.observed_ns = resumed.message_ns = GAP_NOW;
|
||||
resumed.valid_until_ns = GAP_NOW + 30000000ULL;
|
||||
queue(gap_transport, status(), status(1U, 1U));
|
||||
result = run(gap_negotiator, gap_transport, resumed, GAP_NOW);
|
||||
assert(result.plan.request_mask == 1U && (result.plan.request_mask & 2U) == 0U);
|
||||
gap_transport.write_ok = false;
|
||||
gap_transport.replies.push_back(bytes(status(1U, 1U)));
|
||||
result = run(gap_negotiator, gap_transport, expired_axis, GAP_NOW);
|
||||
assert(!result.outcome.compatible);
|
||||
gap_transport.write_ok = true;
|
||||
queue(gap_transport, status(), status());
|
||||
result = run(gap_negotiator, gap_transport, expired_axis, GAP_NOW);
|
||||
assert(result.plan.request_mask == 0U && !result.plan.retain_lateral_arm);
|
||||
|
||||
AolAxisNegotiator boot_negotiator(TEST_REGISTRY);
|
||||
FakeTransport boot_transport;
|
||||
queue(boot_transport, status(), status());
|
||||
|
||||
@@ -6,9 +6,6 @@
|
||||
#include <string>
|
||||
|
||||
static constexpr uint64_t now = 1000000000ULL;
|
||||
static constexpr uint64_t GAP_NOW = 91564707308ULL;
|
||||
static constexpr uint64_t CARD_NS = 91560387360ULL;
|
||||
static constexpr uint64_t AXIS_SOURCE_NS = 91529338245ULL;
|
||||
|
||||
int main() {
|
||||
const AolSafetyWireFields fields = {1, true, 100, 200, 5, 34, true, false, true, false, "panda", "axis",
|
||||
@@ -114,47 +111,5 @@ int main() {
|
||||
malformed[0] = 0;
|
||||
malformed[8] = 0xff; // malformed root pointer
|
||||
assert(!aol_armed_intent(kj::arrayPtr(malformed.data(), malformed.size()), now - 10000000ULL, now - 10000000ULL, now));
|
||||
|
||||
// The exact route2f/1 Card packet is 4.32 ms old, but 31.05 ms newer
|
||||
// than the delayed axis source. Only its acknowledged producer can retain
|
||||
// the physical arm. Optional release and all command leases stay unchanged.
|
||||
intent.setSequence(5206);
|
||||
intent.setCarStateLogMonoTime(CARD_NS);
|
||||
intent.setObservedMonoTime(CARD_NS);
|
||||
intent.setValidUntilMonoTime(CARD_NS + 200000000ULL);
|
||||
intent.setLateralArmed(true);
|
||||
intent.setOptionalSetRelease(true);
|
||||
auto retained = [&](const std::string &producer = "card-session", bool optional = false,
|
||||
uint64_t stamp = CARD_NS, uint64_t source = AXIS_SOURCE_NS, uint64_t clock = GAP_NOW) {
|
||||
auto retained_flat = capnp::messageToFlatArray(intent_builder);
|
||||
return aol_armed_intent(retained_flat.asBytes(), stamp, source, clock, optional, producer);
|
||||
};
|
||||
assert(retained());
|
||||
assert(retained("card-session", false, CARD_NS, AXIS_SOURCE_NS, CARD_NS + 30000000ULL));
|
||||
assert(retained("card-session", false, CARD_NS, CARD_NS - 200000000ULL));
|
||||
assert(!retained(""));
|
||||
assert(!retained("different-card-session"));
|
||||
assert(!retained("card-session", true));
|
||||
assert(!retained("card-session", false, CARD_NS, CARD_NS + 30000001ULL, CARD_NS + 30000001ULL));
|
||||
assert(!retained("card-session", false, CARD_NS, CARD_NS - 200000001ULL));
|
||||
assert(!retained("card-session", false, CARD_NS, AXIS_SOURCE_NS, CARD_NS + 30000001ULL));
|
||||
assert(!retained("card-session", false, CARD_NS + 1U));
|
||||
intent.setSequence(0);
|
||||
assert(!retained());
|
||||
intent.setSequence(5206);
|
||||
intent.setObservedMonoTime(CARD_NS - 1U);
|
||||
assert(!retained());
|
||||
intent.setObservedMonoTime(CARD_NS);
|
||||
intent.setCarStateLogMonoTime(CARD_NS - 1U);
|
||||
assert(!retained());
|
||||
intent.setCarStateLogMonoTime(CARD_NS);
|
||||
intent.setValidUntilMonoTime(GAP_NOW - 1U);
|
||||
assert(!retained());
|
||||
intent.setValidUntilMonoTime(CARD_NS + 200000000ULL);
|
||||
intent.setSettingsQualified(false);
|
||||
assert(!retained());
|
||||
intent.setSettingsQualified(true);
|
||||
intent.setLateralArmed(false);
|
||||
assert(!retained());
|
||||
return 0;
|
||||
}
|
||||
|
||||
@@ -1,132 +0,0 @@
|
||||
// Compile the actual publisher and hwmon worker, replacing only external I/O.
|
||||
#include "common/hardware/hw.h"
|
||||
#include "common/hardware/comma/hardware.h"
|
||||
#undef Hardware
|
||||
#define Hardware HardwareComma
|
||||
#include "selfdrive/pandad/pandad.cc"
|
||||
#include "common/tests/native_test.h"
|
||||
|
||||
#include <condition_variable>
|
||||
#include <mutex>
|
||||
|
||||
namespace {
|
||||
std::string device;
|
||||
std::atomic<unsigned> hwmon_reads{0};
|
||||
std::thread::id publisher_thread;
|
||||
std::atomic<uint32_t> input_voltage{12345}, input_current{812};
|
||||
std::mutex sensor_mutex;
|
||||
std::condition_variable sensor_changed;
|
||||
bool block_sensor = false, sensor_waiting = false;
|
||||
kj::Array<capnp::word> last_message;
|
||||
unsigned publications = 0;
|
||||
}
|
||||
|
||||
std::string wrapped_read_file(const std::string &) asm("__wrap__ZN4util9read_fileERKNSt7__cxx1112basic_stringIcSt11char_traitsIcESaIcEEE");
|
||||
std::string wrapped_read_file(const std::string &path) {
|
||||
if (path == "/sys/firmware/devicetree/base/model") return "comma " + device;
|
||||
CHECK(path == "/sys/class/hwmon/hwmon1/in1_input" || path == "/sys/class/hwmon/hwmon1/curr1_input");
|
||||
CHECK(std::this_thread::get_id() != publisher_thread);
|
||||
++hwmon_reads;
|
||||
std::unique_lock lock(sensor_mutex);
|
||||
if (block_sensor) {
|
||||
sensor_waiting = true;
|
||||
sensor_changed.notify_all();
|
||||
sensor_changed.wait(lock, [] { return !block_sensor; });
|
||||
}
|
||||
return std::to_string(path == "/sys/class/hwmon/hwmon1/in1_input" ? input_voltage.load() : input_current.load()) + "\n";
|
||||
}
|
||||
|
||||
int wrapped_send(PubMaster *, const char *, MessageBuilder &) asm("__wrap__ZN9PubMaster4sendEPKcR14MessageBuilder");
|
||||
int wrapped_send(PubMaster *, const char *name, MessageBuilder &message) {
|
||||
CHECK(std::string(name) == "peripheralState");
|
||||
last_message = capnp::messageToFlatArray(message);
|
||||
++publications;
|
||||
return 0;
|
||||
}
|
||||
|
||||
class PowerHandle : public PandaCommsHandle {
|
||||
public:
|
||||
health_t health{};
|
||||
bool fail_health = false;
|
||||
PowerHandle() { health.voltage_pkt = 300; health.current_pkt = 17; health.faults_pkt = 8; }
|
||||
int control_write(uint8_t, uint16_t, uint16_t, unsigned int) override { return 0; }
|
||||
int control_read(uint8_t request, uint16_t, uint16_t, unsigned char *data, uint16_t length, unsigned int) override {
|
||||
if (request == 0xd2 && length == sizeof(health)) {
|
||||
if (fail_health) return -1;
|
||||
std::memcpy(data, &health, length);
|
||||
return length;
|
||||
}
|
||||
if (request == 0xb2 && length == 2) { uint16_t fan = 1234; std::memcpy(data, &fan, length); return length; }
|
||||
return -1;
|
||||
}
|
||||
int bulk_write(unsigned char, unsigned char *, int, unsigned int) override { return 0; }
|
||||
int bulk_read(unsigned char, unsigned char *, int, unsigned int) override { return 0; }
|
||||
void cleanup() override {}
|
||||
};
|
||||
class PowerPanda : public Panda {
|
||||
public:
|
||||
PowerPanda(std::unique_ptr<PowerHandle> comms, cereal::PandaState::PandaType type) : Panda(std::move(comms), type) {}
|
||||
};
|
||||
|
||||
void test_power() {
|
||||
publisher_thread = std::this_thread::get_id();
|
||||
using Type = cereal::PandaState::PandaType;
|
||||
auto handle = std::make_unique<PowerHandle>();
|
||||
auto *raw = handle.get();
|
||||
const auto type = device == "tici" ? Type::DOS : device == "tizi" ? Type::TRES : Type::CUATRO;
|
||||
PowerPanda panda(std::move(handle), type);
|
||||
PubMaster publisher({});
|
||||
auto check = [&](uint32_t voltage, uint32_t current, bool valid = true) {
|
||||
send_peripheral_state(&panda, &publisher);
|
||||
capnp::FlatArrayMessageReader reader(last_message.asPtr());
|
||||
auto event = reader.getRoot<cereal::Event>();
|
||||
CHECK(event.getValid() == valid && event.isPeripheralState());
|
||||
auto state = event.getPeripheralState();
|
||||
CHECK(state.getVoltage() == voltage && state.getCurrent() == current);
|
||||
CHECK(state.getPandaType() == type && state.getFanSpeedRpm() == 1234);
|
||||
CHECK(raw->health.voltage_pkt == 300 && raw->health.current_pkt == 17 && raw->health.faults_pkt == 8);
|
||||
};
|
||||
check(300, 17); // No sample yet: retain the current Panda fallback.
|
||||
if (device != "tici") {
|
||||
hwmon_thread(); // Returns before touching absent INA sensors on 3x/4.
|
||||
CHECK(hwmon_reads == 0 && hwmon_power == 0);
|
||||
check(300, 17);
|
||||
return;
|
||||
}
|
||||
std::thread worker(hwmon_thread);
|
||||
struct Join {
|
||||
std::thread &worker;
|
||||
~Join() {
|
||||
{ std::lock_guard lock(sensor_mutex); block_sensor = false; }
|
||||
sensor_changed.notify_all(); do_exit = true; worker.join();
|
||||
}
|
||||
} join{worker};
|
||||
for (int i = 0; i < 2000 && hwmon_power == 0; ++i) std::this_thread::sleep_for(std::chrono::milliseconds(1));
|
||||
CHECK(hwmon_power != 0 && hwmon_reads >= 2);
|
||||
check(12345, 812);
|
||||
{
|
||||
std::unique_lock lock(sensor_mutex); block_sensor = true;
|
||||
CHECK(sensor_changed.wait_for(lock, std::chrono::seconds(3), [] { return sensor_waiting; }));
|
||||
}
|
||||
// A blocked background sensor cannot hold the actual peripheral publisher.
|
||||
check(12345, 812);
|
||||
// Verify paired publication and Dom's zero/zero fallback without voltage clamps.
|
||||
for (auto pair : {std::pair{0U, 0U}, {2300U, 15U}, {0U, 50U}, {14000U, 0U}}) {
|
||||
hwmon_power.store((uint64_t(pair.first) << 32) | pair.second);
|
||||
check(pair.first || pair.second ? pair.first : 300, pair.first || pair.second ? pair.second : 17);
|
||||
}
|
||||
raw->comms_healthy = false;
|
||||
check(14000, 0, false);
|
||||
raw->comms_healthy = true;
|
||||
raw->fail_health = true;
|
||||
auto before = publications;
|
||||
send_peripheral_state(&panda, &publisher);
|
||||
CHECK(publications == before);
|
||||
}
|
||||
|
||||
int main(int argc, char **argv) {
|
||||
CHECK(argc == 2);
|
||||
device = argv[1];
|
||||
CHECK(device == "tici" || device == "tizi" || device == "mici");
|
||||
return run_native_test(test_power);
|
||||
}
|
||||
@@ -42,7 +42,7 @@ from openpilot.starpilot.aol.source_availability import (
|
||||
ordinary_source_availability, fresh_ordinary_lag_continuation, stock_lateral_role, stock_lateral_fault_proof,
|
||||
)
|
||||
from openpilot.starpilot.aol.runtime import (INTENT_MAX_AGE_NS, SAFETY_MAX_AGE_NS, AxisDecision, current_intent, current_native, decide_axes,
|
||||
ordinary_lateral_requested, decide_ordinary_axis, known_reverse_pause)
|
||||
ordinary_lateral_requested, decide_ordinary_axis)
|
||||
from openpilot.starpilot.aol.vehicle import policy_for as axis_policy_for, ordinary_axis_request_allowed
|
||||
from openpilot.starpilot.conditional_mode.consumer import ConsumerResult, ModeConsumer
|
||||
from openpilot.starpilot.conditional_mode.effective_status import publish_ack
|
||||
@@ -180,7 +180,6 @@ class SelfdriveD:
|
||||
from openpilot.starpilot.car.gm.radar_recovery import capability as radar_recovery_capability
|
||||
radar_notifications = ['radarTracks'] if radar_recovery_capability(self.CP) is not None else []
|
||||
ignore += radar_notifications
|
||||
ignore = set(ignore)
|
||||
self.sm = messaging.SubMaster(['deviceState', 'pandaStates', 'peripheralState', 'modelV2', 'extrinsicsCalibration',
|
||||
'carOutput', 'driverMonitoringState', 'longitudinalPlan', 'deviceMotion', 'lateralDelay',
|
||||
'managerState', 'vehicleParameters', 'radarState', 'lateralTorqueParameters',
|
||||
@@ -757,11 +756,11 @@ class SelfdriveD:
|
||||
if all_valid or timed_out or (SIMULATION and not REPLAY):
|
||||
available_streams = VisionIpcClient.available_streams("camerad", block=False)
|
||||
if VisionStreamType.VISION_STREAM_NARROW_ROAD not in available_streams:
|
||||
self.sm.ignore_alive.add('narrowRoadCameraState')
|
||||
self.sm.ignore_valid.add('narrowRoadCameraState')
|
||||
self.sm.ignore_alive.append('narrowRoadCameraState')
|
||||
self.sm.ignore_valid.append('narrowRoadCameraState')
|
||||
if VisionStreamType.VISION_STREAM_WIDE_ROAD not in available_streams:
|
||||
self.sm.ignore_alive.add('wideRoadCameraState')
|
||||
self.sm.ignore_valid.add('wideRoadCameraState')
|
||||
self.sm.ignore_alive.append('wideRoadCameraState')
|
||||
self.sm.ignore_valid.append('wideRoadCameraState')
|
||||
|
||||
if REPLAY and any(ps.controlsAllowed for ps in self.sm['pandaStates']):
|
||||
self.state_machine.state = State.enabled
|
||||
@@ -1540,9 +1539,6 @@ class SelfdriveD:
|
||||
from openpilot.starpilot.car.gm.aol import physical_cancel_lateral_allowed
|
||||
from openpilot.starpilot.aol.dm import lateral_event_context
|
||||
retained_dm_lateral, lateral_events = lateral_event_context(self, CS, intent, native, now_ns, ordinary_lag)
|
||||
reverse_pause = known_reverse_pause(CS, getattr(self, 'car_state_samples', ()))
|
||||
if reverse_pause:
|
||||
lateral_events = [event for event in lateral_events if event not in (EventName.wrongGear, EventName.reverseGear)]
|
||||
return decide_axes(
|
||||
standard_lateral=self.active, standard_longitudinal=bool(self.enabled and self.CP.openpilotLongitudinalControl and
|
||||
not any(sample.seatbeltUnlatched for sample, _ in getattr(self, 'car_state_samples', ()))),
|
||||
@@ -1563,7 +1559,6 @@ class SelfdriveD:
|
||||
self.sm['driverMonitoringState'].alwaysOnLockout or
|
||||
self.sm['driverMonitoringState'].alertLevel == AlertLevel.three)),
|
||||
pause_brake_mps=self.aol_settings.pause_brake_mps if self.aol_settings is not None else 0.0,
|
||||
gear_pause=reverse_pause,
|
||||
lateral_inhibit=self.aol_dm_lateral_inhibit or any(
|
||||
sample.steerFaultTemporary or sample.brakePressed and not sample.standstill and
|
||||
sample.vEgo < (self.aol_settings.pause_brake_mps if self.aol_settings is not None else 0.0)
|
||||
@@ -1770,9 +1765,7 @@ class SelfdriveD:
|
||||
# Temporary EPS withdraws steering commands without revoking the armed
|
||||
# latch. Keep the event and ordinary soft-disable behavior unchanged.
|
||||
from openpilot.starpilot.aol.seatbelt import seatbelt_lateral_soft_event_allowed
|
||||
reverse_pause = known_reverse_pause(CS, getattr(self, 'car_state_samples', ()))
|
||||
aol_hard_soft_fault = any(event.softDisable and not (
|
||||
event.name == EventName.wrongGear and reverse_pause or
|
||||
event.name == EventName.steerTempUnavailable and CS.steerFaultTemporary and not CS.steerFaultPermanent or
|
||||
seatbelt_lateral_soft_event_allowed(self.CP, CS, intent, native, event,
|
||||
source_ns=self.aol_car_state_log_ns, now_ns=now_ns, axis_session_id=self.aol_session_id,
|
||||
|
||||
@@ -229,12 +229,11 @@ class AugmentedRoadView(CameraView):
|
||||
def render_camera_model_layer(self, rect: rl.Rectangle, *, camera_view: int = CAMERA_VIEW_AUTO,
|
||||
reverse_driver: bool = False, lead_indicator: bool = False,
|
||||
lead_info_mode: int = 0, lead_info_metric: bool | None = None,
|
||||
lateral_active: bool = False, road_style: dict | None = None, paint: bool = True,
|
||||
poll_camera: bool = True) -> None:
|
||||
lateral_active: bool = False, road_style: dict | None = None, paint: bool = True) -> None:
|
||||
"""Draw the existing calibrated camera/model without stock HUD or alert."""
|
||||
if not ui_state.started and ui_state.replay_clock is None:
|
||||
return
|
||||
if poll_camera and (camera_view == CAMERA_VIEW_DRIVER or reverse_driver):
|
||||
if camera_view == CAMERA_VIEW_DRIVER or reverse_driver:
|
||||
self._ensure_connection()
|
||||
target = self._switch_stream_if_needed(ui_state.sm, camera_view, reverse_driver)
|
||||
if target is None:
|
||||
@@ -242,7 +241,7 @@ class AugmentedRoadView(CameraView):
|
||||
rl.draw_rectangle_rec(rect, rl.BLACK)
|
||||
return
|
||||
driver_transition = DRIVER_CAM in (self.stream_type, target, self._target_stream_type)
|
||||
if poll_camera and self._switching and driver_transition:
|
||||
if self._switching and driver_transition:
|
||||
self._handle_switch()
|
||||
if (target == DRIVER_CAM and DRIVER_CAM not in self.available_streams) or \
|
||||
(driver_transition and self.stream_type != target):
|
||||
@@ -255,7 +254,7 @@ class AugmentedRoadView(CameraView):
|
||||
self._update_calibration()
|
||||
self._content_rect = rect
|
||||
self._model_renderer.road_style = road_style or {}
|
||||
gui_app.measure_frame_phase("camera", CameraView._render, self, rect, paint=paint, poll_camera=poll_camera)
|
||||
gui_app.measure_frame_phase("camera", CameraView._render, self, rect, paint=paint)
|
||||
if target != DRIVER_CAM and self.frame is not None:
|
||||
gui_app.measure_frame_phase("model", self._model_renderer.render_with_lead, rect, lead_indicator,
|
||||
lead_info_mode, lead_info_metric, lateral_active=lateral_active, paint=paint)
|
||||
@@ -293,9 +292,9 @@ class AugmentedRoadView(CameraView):
|
||||
if not self._bookmark_icon.interacting():
|
||||
super()._handle_mouse_release(mouse_pos)
|
||||
|
||||
def _render(self, rect: rl.Rectangle, /, *, paint: bool = True, poll_camera: bool = True):
|
||||
def _render(self, rect: rl.Rectangle, /, *, paint: bool = True):
|
||||
if not paint:
|
||||
super()._render(rect, paint=False, poll_camera=poll_camera)
|
||||
super()._render(rect, paint=False)
|
||||
return
|
||||
# Draw text if not onroad
|
||||
if not ui_state.started:
|
||||
@@ -326,7 +325,7 @@ class AugmentedRoadView(CameraView):
|
||||
)
|
||||
|
||||
# Render the base camera view
|
||||
super()._render(self._content_rect, poll_camera=poll_camera)
|
||||
super()._render(self._content_rect)
|
||||
|
||||
# Draw all UI overlays
|
||||
self._model_renderer.render(self._content_rect)
|
||||
|
||||
@@ -227,24 +227,25 @@ class CameraView(Widget):
|
||||
[0.0, 0.0, 1.0]
|
||||
])
|
||||
|
||||
def _poll_camera(self) -> bool:
|
||||
def _render(self, rect: rl.Rectangle, /, *, paint: bool = True):
|
||||
if self._switching:
|
||||
self._handle_switch()
|
||||
|
||||
if not self._ensure_connection():
|
||||
return False
|
||||
if paint:
|
||||
self._draw_placeholder(rect)
|
||||
return
|
||||
|
||||
# Try to get a new buffer without blocking
|
||||
buffer = self.client.recv(timeout_ms=0)
|
||||
if buffer:
|
||||
self._texture_needs_update = True
|
||||
self.frame = buffer
|
||||
elif not self.client.is_connected():
|
||||
# ensure we clear the displayed frame when the connection is lost
|
||||
self.frame = None
|
||||
|
||||
return self.frame is not None
|
||||
|
||||
def _render(self, rect: rl.Rectangle, /, *, paint: bool = True, poll_camera: bool = True):
|
||||
if (poll_camera and not self._poll_camera()) or self.frame is None:
|
||||
if not self.frame:
|
||||
if paint:
|
||||
self._draw_placeholder(rect)
|
||||
return
|
||||
|
||||
@@ -7,11 +7,11 @@ from opendbc.car.structs import car
|
||||
from dataclasses import dataclass, field
|
||||
from types import SimpleNamespace
|
||||
from openpilot.common.params import Params
|
||||
from openpilot.starpilot.ui.appearance_preferences import format_lead_info
|
||||
from openpilot.starpilot.ui.rainbow_path import RainbowPath
|
||||
from openpilot.starpilot.ui.road_colors import lane_color, path_mode, solid_gradient
|
||||
from openpilot.starpilot.ui.onroad_customization import ROAD_COLORS
|
||||
from openpilot.starpilot.lateral.lane_feedback import BLUE, direction as lane_centering_direction
|
||||
from openpilot.common.constants import CV
|
||||
from openpilot.common.filter_simple import FirstOrderFilter
|
||||
from openpilot.selfdrive.locationd.calibrationd import HEIGHT_INIT
|
||||
from openpilot.selfdrive.controls.radard import RADAR_TO_CAMERA
|
||||
@@ -221,7 +221,18 @@ class ModelRenderer(Widget):
|
||||
|
||||
@staticmethod
|
||||
def _format_lead_info(lead, mode: int, metric: bool | None) -> str:
|
||||
return format_lead_info(lead, mode, metric)
|
||||
if lead is None or not lead.present or mode not in (1, 2) or metric is None:
|
||||
return ""
|
||||
try:
|
||||
value = float(lead.dRel if mode == 1 else lead.vLead)
|
||||
except (AttributeError, TypeError, ValueError, OverflowError):
|
||||
return ""
|
||||
if not math.isfinite(value):
|
||||
return ""
|
||||
value = max(value, 0.0)
|
||||
if mode == 1:
|
||||
return f"{round(value if metric else value * 3.28084)} {'m' if metric else 'ft'}"
|
||||
return f"{round(value * (CV.MS_TO_KPH if metric else CV.MS_TO_MPH))} {'km/h' if metric else 'mph'}"
|
||||
|
||||
@staticmethod
|
||||
def _valid_lead_numbers(lead) -> bool:
|
||||
|
||||
@@ -48,8 +48,7 @@ class AugmentedRoadView(CameraView):
|
||||
self.alert_renderer = AlertRenderer()
|
||||
self.driver_state_renderer = DriverStateRenderer()
|
||||
|
||||
def render_camera_model_layer(self, rect: rl.Rectangle, *, road_style: dict | None = None,
|
||||
lead_display: tuple[bool, int, bool | None] | None = None, poll_camera: bool = True) -> None:
|
||||
def render_camera_model_layer(self, rect: rl.Rectangle, *, road_style: dict | None = None) -> None:
|
||||
"""Draw the existing calibrated camera/model without stock HUD or alert."""
|
||||
if not ui_state.started and ui_state.replay_clock is None:
|
||||
return
|
||||
@@ -60,11 +59,10 @@ class AugmentedRoadView(CameraView):
|
||||
if ("pathEdge" in getattr(self.model_renderer, "road_style", {})) != ("pathEdge" in style):
|
||||
self.model_renderer._transform_dirty = True
|
||||
self.model_renderer.road_style = style
|
||||
self.model_renderer.lead_display = lead_display
|
||||
gui_app.measure_frame_phase("camera", CameraView._render, self, rect, poll_camera=poll_camera)
|
||||
gui_app.measure_frame_phase("camera", CameraView._render, self, rect)
|
||||
gui_app.measure_frame_phase("model", self.model_renderer.render, rect)
|
||||
|
||||
def _render(self, rect, *, poll_camera: bool = True):
|
||||
def _render(self, rect):
|
||||
# Only render when system is started to avoid invalid data access
|
||||
if not ui_state.started:
|
||||
return
|
||||
@@ -92,7 +90,7 @@ class AugmentedRoadView(CameraView):
|
||||
)
|
||||
|
||||
# Render the base camera view
|
||||
super()._render(self._content_rect, poll_camera=poll_camera)
|
||||
super()._render(self._content_rect)
|
||||
|
||||
# Draw all UI overlays
|
||||
self.model_renderer.render(self._content_rect)
|
||||
|
||||
@@ -29,8 +29,8 @@ class CabinCameraDialog(CameraView):
|
||||
def __del__(self):
|
||||
self.close()
|
||||
|
||||
def _render(self, rect, *, poll_camera: bool = True):
|
||||
super()._render(rect, poll_camera=poll_camera)
|
||||
def _render(self, rect):
|
||||
super()._render(rect)
|
||||
|
||||
if not self.frame:
|
||||
gui_label(
|
||||
|
||||
@@ -213,24 +213,24 @@ class CameraView(Widget):
|
||||
[0.0, 0.0, 1.0]
|
||||
])
|
||||
|
||||
def _poll_camera(self) -> bool:
|
||||
def _render(self, rect: rl.Rectangle):
|
||||
if self._switching:
|
||||
self._handle_switch()
|
||||
|
||||
if not self._ensure_connection():
|
||||
return False
|
||||
self._draw_placeholder(rect)
|
||||
return
|
||||
|
||||
# Try to get a new buffer without blocking
|
||||
buffer = self.client.recv(timeout_ms=0)
|
||||
if buffer:
|
||||
self._texture_needs_update = True
|
||||
self.frame = buffer
|
||||
elif not self.client.is_connected():
|
||||
# ensure we clear the displayed frame when the connection is lost
|
||||
self.frame = None
|
||||
|
||||
return self.frame is not None
|
||||
|
||||
def _render(self, rect: rl.Rectangle, *, poll_camera: bool = True):
|
||||
if (poll_camera and not self._poll_camera()) or self.frame is None:
|
||||
if not self.frame:
|
||||
self._draw_placeholder(rect)
|
||||
return
|
||||
|
||||
|
||||
@@ -6,15 +6,13 @@ from opendbc.car.structs import car
|
||||
from dataclasses import dataclass, field
|
||||
from openpilot.common.filter_simple import FirstOrderFilter
|
||||
from openpilot.common.params import Params
|
||||
from openpilot.starpilot.ui.appearance_preferences import format_lead_info
|
||||
from openpilot.starpilot.ui.rainbow_path import RainbowPath
|
||||
from openpilot.starpilot.ui.road_colors import edge_gradient, lane_color, path_mode, solid_gradient
|
||||
from openpilot.starpilot.ui.onroad_customization import ROAD_COLORS
|
||||
from openpilot.starpilot.lateral.lane_feedback import BLUE, direction as lane_centering_direction
|
||||
from openpilot.selfdrive.locationd.calibrationd import HEIGHT_INIT
|
||||
from openpilot.selfdrive.ui.ui_state import ui_state
|
||||
from openpilot.system.ui.lib.application import FontWeight, gui_app
|
||||
from openpilot.system.ui.lib.text_measure import measure_text_cached
|
||||
from openpilot.system.ui.lib.application import gui_app
|
||||
from openpilot.system.ui.lib.shader_polygon import draw_polygon, Gradient
|
||||
from openpilot.system.ui.widgets import Widget
|
||||
|
||||
@@ -61,8 +59,6 @@ class ModelRenderer(Widget):
|
||||
self._lane_centering_direction = 0
|
||||
self._road_edge_stds = np.zeros(2, dtype=np.float32)
|
||||
self._lead_vehicles = [LeadVehicle(), LeadVehicle()]
|
||||
# (show marker, LeadInfoMode, metric) from a screen with its own lead choices; None keeps the stock chevrons.
|
||||
self.lead_display: tuple[bool, int, bool | None] | None = None
|
||||
self._path_offset_z = HEIGHT_INIT[0]
|
||||
|
||||
# Initialize ModelPoints objects
|
||||
@@ -127,9 +123,7 @@ class ModelRenderer(Widget):
|
||||
model = sm['modelV2']
|
||||
radar_state = sm['radarState'] if sm.valid['radarState'] else None
|
||||
lead_one = radar_state.leadOne if radar_state else None
|
||||
lead_display = self.lead_display
|
||||
show_lead = self._longitudinal_control if lead_display is None else lead_display[0]
|
||||
render_lead_indicator = show_lead and radar_state is not None
|
||||
render_lead_indicator = self._longitudinal_control and radar_state is not None
|
||||
|
||||
# Update model data when needed
|
||||
model_updated = sm.updated['modelV2']
|
||||
@@ -152,10 +146,6 @@ class ModelRenderer(Widget):
|
||||
|
||||
if render_lead_indicator and radar_state:
|
||||
self._draw_lead_indicator()
|
||||
if lead_display is not None and self._lead_vehicles[0].chevron:
|
||||
text = format_lead_info(lead_one, lead_display[1], lead_display[2])
|
||||
if text:
|
||||
self._draw_lead_info(self._lead_vehicles[0], text)
|
||||
|
||||
def _update_raw_points(self, model):
|
||||
"""Update raw 3D points from model data"""
|
||||
@@ -377,17 +367,6 @@ class ModelRenderer(Widget):
|
||||
rl.draw_triangle_fan(lead.glow, len(lead.glow), rl.Color(218, 202, 37, 255))
|
||||
rl.draw_triangle_fan(lead.chevron, len(lead.chevron), rl.Color(201, 34, 49, lead.fill_alpha))
|
||||
|
||||
def _draw_lead_info(self, lead: LeadVehicle, text: str) -> None:
|
||||
# Centered just above the chevron's glow, kept on screen.
|
||||
font = gui_app.font(FontWeight.SEMI_BOLD)
|
||||
size = measure_text_cached(font, text, 48)
|
||||
tip_x, tip_y = lead.glow[1]
|
||||
x = min(max(tip_x - size.x / 2, self._rect.x), self._rect.x + self._rect.width - size.x)
|
||||
y = max(tip_y - size.y - 12, self._rect.y)
|
||||
for dx, dy in ((-1, -1), (1, -1), (-1, 1), (1, 1)):
|
||||
rl.draw_text_ex(font, text, rl.Vector2(x + dx, y + dy), 48, 0, rl.BLACK)
|
||||
rl.draw_text_ex(font, text, rl.Vector2(x, y), 48, 0, rl.WHITE)
|
||||
|
||||
@staticmethod
|
||||
def _get_path_length_idx(pos_x_array: np.ndarray, path_distance: float) -> int:
|
||||
"""Get the index corresponding to the given path distance"""
|
||||
|
||||
@@ -1,7 +1,6 @@
|
||||
#!/usr/bin/env python3
|
||||
import os
|
||||
import sys
|
||||
import time
|
||||
|
||||
from openpilot.cereal import messaging
|
||||
from openpilot.common.hardware import COMMA_HARDWARE, HARDWARE
|
||||
@@ -16,12 +15,8 @@ from openpilot.starpilot.ui.startup import select_ui
|
||||
BIG_UI = gui_app.big_ui()
|
||||
|
||||
|
||||
def update_frame(preview=None, controllers=None, star=None):
|
||||
def update_frame(preview=None, controllers=None):
|
||||
gui_app.measure_frame_phase("ui_state", ui_state.update)
|
||||
if star is not None:
|
||||
# Android Auto bookmarks must be consumed even when the comma screen is off
|
||||
# or another widget covers the StarPilot page.
|
||||
star._poll_projection_bookmark(time.monotonic_ns())
|
||||
if controllers is not None:
|
||||
gui_app.measure_frame_phase("controllers", controllers.poll)
|
||||
if preview is not None:
|
||||
@@ -60,9 +55,8 @@ def main():
|
||||
controllers = None
|
||||
|
||||
pm = messaging.PubMaster(['uiDebug'])
|
||||
star = layout.star if star_ui else None
|
||||
try:
|
||||
for should_render, frame_time, cpu_time in gui_app.render(before_frame=lambda: update_frame(preview, controllers, star)):
|
||||
for should_render, frame_time, cpu_time in gui_app.render(before_frame=lambda: update_frame(preview, controllers)):
|
||||
if should_render:
|
||||
if camera_core_ui and not main_thread_configured:
|
||||
# Share the camera core with normal scheduling, without FIFO display
|
||||
|
||||
@@ -153,10 +153,6 @@ class AolCardIntent:
|
||||
self._fault_inhibit = False
|
||||
self._last_latch_edge_ns = 0
|
||||
self._native_rejection_ns = 0
|
||||
self._engagement_source_ns = 0
|
||||
self._engagement_consumed = True
|
||||
self._engagement_withdrawal_ns = 0
|
||||
self._engagement_observed_ns = 0
|
||||
self._calibration_inhibit = False
|
||||
self._calibration_fault = False
|
||||
self._calibration_rearm_required = False
|
||||
@@ -180,67 +176,13 @@ class AolCardIntent:
|
||||
if action == AOL_TOGGLE and self.settings.enabled and not self.explicit_latch:
|
||||
self._record_latch_clear('configured_toggle')
|
||||
self.allowed_latch = not self.allowed_latch
|
||||
if not self.allowed_latch:
|
||||
self._withdraw_full_engagement()
|
||||
if self.allowed_latch:
|
||||
self.pause_lateral = False
|
||||
elif action == PAUSE_LATERAL:
|
||||
self.pause_lateral = not self.pause_lateral
|
||||
if self.pause_lateral:
|
||||
self._withdraw_full_engagement()
|
||||
elif action == PAUSE_LONGITUDINAL:
|
||||
self.pause_longitudinal = not self.pause_longitudinal
|
||||
|
||||
def _withdraw_full_engagement(self) -> None:
|
||||
self._engagement_consumed = True
|
||||
self._engagement_withdrawal_ns = max(self._engagement_withdrawal_ns, self._engagement_observed_ns)
|
||||
|
||||
def observe_full_engagement(self, CS, *, enabled: bool, active: bool, source_ns: int,
|
||||
observation_valid: bool, now_ns: int, fault_active: bool, native_rejection_ns: int = 0) -> None:
|
||||
"""Consume one successful activation per freshly observed enabled epoch.
|
||||
|
||||
Missing/reordered control packets do not invent a disengagement. This is
|
||||
driver intent only; the existing vehicle and native owners retain authority.
|
||||
"""
|
||||
self._engagement_observed_ns = now_ns
|
||||
manual = any(event.type not in (ButtonType.setCruise, ButtonType.accelCruise,
|
||||
ButtonType.decelCruise, ButtonType.resumeCruise)
|
||||
for event in CS.buttonEvents)
|
||||
blocked = bool(manual or self._main_held or self._lkas_held or
|
||||
any(self._held.values()) or getattr(self, 'cancel_held', False) or
|
||||
getattr(self, 'screen_held', False) or getattr(self, '_lkas_down', False) or
|
||||
getattr(self, '_engagement_manual_withdrawal', False) or
|
||||
not self.settings.enabled or fault_active or not CS.canValid or CS.canTimeout or CS.steerFaultPermanent or
|
||||
CS.accFaulted or self._fault_inhibit or self._calibration_inhibit or
|
||||
self._calibration_rearm_required or native_rejection_ns)
|
||||
# A local withdrawal also cancels pending preEnabled intent when Controls
|
||||
# has not published another packet yet. It cannot be undone by delayed ACKs.
|
||||
if blocked:
|
||||
self._withdraw_full_engagement()
|
||||
if not observation_valid or not self._engagement_source_ns < source_ns <= now_ns:
|
||||
return
|
||||
blocked = blocked or 0 < source_ns <= self._native_rejection_ns
|
||||
first = self._engagement_source_ns == 0
|
||||
self._engagement_source_ns = source_ns
|
||||
if first:
|
||||
# A process starting during engagement has no new driver gesture.
|
||||
self._engagement_consumed = bool(enabled)
|
||||
if not enabled:
|
||||
self._engagement_consumed = source_ns <= self._engagement_withdrawal_ns
|
||||
return
|
||||
if self._engagement_consumed or not active and not blocked:
|
||||
return
|
||||
self._engagement_consumed = True
|
||||
if (blocked or not active or not self.settings.enabled or not CS.cruiseState.available or
|
||||
CS.gearShifter in (GearShifter.park, GearShifter.neutral, GearShifter.reverse, GearShifter.unknown)):
|
||||
return
|
||||
self._arm_from_full_engagement(CS, source_ns=source_ns)
|
||||
|
||||
def _arm_from_full_engagement(self, CS, *, source_ns: int) -> None:
|
||||
self.allowed_latch = True
|
||||
self.pause_lateral = False
|
||||
self._last_latch_edge_ns = source_ns
|
||||
|
||||
def update(self, CS, *, consumed_buttons: frozenset[int] = frozenset(), fault_active: bool | None = None,
|
||||
now_ns: int = 0, native_rejection_ns: int = 0, standard_enabled: bool = False) -> None:
|
||||
if self.explicit_latch and self._native_rejection_ns < native_rejection_ns <= now_ns:
|
||||
|
||||
@@ -160,23 +160,14 @@ def current_native(sm, CP, *, now_ns: int, axis_session_id: str | None = None):
|
||||
return state
|
||||
|
||||
|
||||
def known_reverse_pause(car_state, samples=()) -> bool:
|
||||
states = [car_state, *(state for state, _ in samples)]
|
||||
gears = car.CarState.GearShifter
|
||||
return bool(all(valid for _, valid in samples) and
|
||||
any(state.gearShifter == gears.reverse for state in states) and
|
||||
all(state.canValid and not state.canTimeout and state.gearShifter in
|
||||
(gears.drive, gears.low, gears.sport, gears.brake, gears.reverse) for state in states))
|
||||
|
||||
|
||||
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, gear_pause: bool = False) -> AxisDecision:
|
||||
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 = not gear_pause and gear not in (car.CarState.GearShifter.neutral, car.CarState.GearShifter.park,
|
||||
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
|
||||
|
||||
@@ -117,34 +117,3 @@ def test_queue_reorder_bound_and_pair_loss_still_withdraw(failure):
|
||||
assert str(axis.faultReason) == 'critical' and sd.aol_authority_lost
|
||||
if failure != 'missing_latest_pair':
|
||||
assert axis.sourceCarStateMonoTime == 0 and not sd.conditional_car_state_valid
|
||||
|
||||
|
||||
@pytest.mark.parametrize('first_gear,valid,retained', [('reverse', True, True), ('unknown', True, False),
|
||||
('reverse', False, False), ('eco', True, False)])
|
||||
def test_reverse_queue_pause_requires_known_valid_gears(first_gear, valid, retained):
|
||||
from openpilot.starpilot.aol.runtime import known_reverse_pause
|
||||
first, last = fixture.car_state(), fixture.car_state()
|
||||
first.gearShifter = first_gear
|
||||
last.gearShifter = 'reverse' if first_gear != 'reverse' else 'drive'
|
||||
assert known_reverse_pause(last, ((first, valid), (last, True))) == retained
|
||||
|
||||
|
||||
def test_reverse_then_drive_in_one_batch_preserves_latch_without_commanding_either_axis():
|
||||
case = fixture.IoniqAvailabilityTests(methodName='runTest')
|
||||
with ExitStack() as cleanup:
|
||||
cleanup.callback(case.doCleanups)
|
||||
with OpenpilotPrefix():
|
||||
sd, _, step, card, receive = case._ioniq_availability_callers()
|
||||
reverse = fixture.car_state(brake=True)
|
||||
reverse.gearShifter = 'reverse'
|
||||
with mock.patch('openpilot.selfdrive.selfdrived.selfdrived.time.monotonic_ns', return_value=10_024_292_276):
|
||||
sd.publish_axis_proof(sd.CS_prev)
|
||||
receive(10_024_292_276, axis=sd.pm.messages['aolAxisState'], event=sd.pm.messages['onroadEvents'])
|
||||
axis, _, command = queued_step(sd, step, [message(10_040_000_000, reverse), message(10_051_448_441)])
|
||||
assert not axis.desiredLateral and not axis.desiredLongitudinal
|
||||
assert not axis.lateralActive and not axis.longitudinalActive and not command.latActive and not command.longActive
|
||||
assert axis.nativeAcknowledged and str(axis.faultReason) == 'none'
|
||||
assert not sd.enabled and not sd.active
|
||||
assert log.OnroadEvent.EventName.reverseGear in sd.events.names
|
||||
assert receive(10_054_292_276, axis=sd.pm.messages['aolAxisState'], event=sd.pm.messages['onroadEvents'])
|
||||
assert card.aol_card_intent.allowed_latch
|
||||
|
||||
@@ -149,11 +149,7 @@ class TestClassicSccProfiles(unittest.TestCase):
|
||||
setattr(state, field, True)
|
||||
owner.update(state, now_ns=4, standard_enabled=False, fault_active=False)
|
||||
self.assertFalse(owner.allowed_latch)
|
||||
owner.observe_full_engagement(state, enabled=False, active=False, source_ns=4,
|
||||
observation_valid=True, now_ns=4, fault_active=False)
|
||||
owner.update(state, now_ns=5, standard_enabled=True, fault_active=False)
|
||||
owner.observe_full_engagement(state, enabled=True, active=True, source_ns=5,
|
||||
observation_valid=True, now_ns=5, fault_active=False)
|
||||
self.assertEqual(owner.allowed_latch, field is None)
|
||||
# Cancel and a deliberate LKAS press take precedence over auto engage.
|
||||
for button in ('cancel', 'lkas'):
|
||||
|
||||
@@ -98,13 +98,9 @@ class TestEv6ConfiguredDesire(unittest.TestCase):
|
||||
state.cruiseState.available = available
|
||||
state.cruiseState.enabled = cruise_enabled
|
||||
state.accFaulted = acc_fault
|
||||
now = getattr(owner, '_test_clock_ns', 90) + 10
|
||||
owner._test_clock_ns = now
|
||||
owner.observe_physical_samples(SimpleNamespace(ev6_aol_sample_stamp_ns=now,
|
||||
ev6_aol_source_healthy=True, ev6_aol_timeout_ns=1_000_000_000, ev6_aol_samples=samples), now_ns=now)
|
||||
owner.update(state, now_ns=now, fault_active=fault)
|
||||
owner.observe_full_engagement(state, enabled=active, active=active, source_ns=now,
|
||||
observation_valid=True, now_ns=now, fault_active=bool(fault))
|
||||
owner.observe_physical_samples(SimpleNamespace(ev6_aol_sample_stamp_ns=100,
|
||||
ev6_aol_source_healthy=True, ev6_aol_timeout_ns=1_000_000_000, ev6_aol_samples=samples), now_ns=100)
|
||||
owner.update(state, now_ns=100, fault_active=fault, standard_active=active)
|
||||
return state
|
||||
|
||||
def test_exact_main_and_lkas_action_ownership(self):
|
||||
@@ -149,32 +145,6 @@ class TestEv6ConfiguredDesire(unittest.TestCase):
|
||||
self.sample(owner, [], active=True)
|
||||
self.assertEqual(owner.allowed_latch, lkas_action == AOL_TOGGLE)
|
||||
|
||||
def test_raw_manual_off_wins_over_full_engagement_without_net_button_event(self):
|
||||
owner = self.owner(main=AOL_TOGGLE)
|
||||
self.sample(owner, [(0, 0, Buttons.NONE)])
|
||||
self.sample(owner, [(0, 1, Buttons.NONE), (0, 0, Buttons.NONE)])
|
||||
self.sample(owner, [(1, 0, Buttons.NONE), (0, 0, Buttons.NONE)])
|
||||
self.assertTrue(owner.allowed_latch and owner.physical.authorized)
|
||||
cs = self.sample(owner, [(1, 0, Buttons.NONE), (0, 0, Buttons.NONE)], active=True)
|
||||
self.assertTrue(owner.physical.authorized)
|
||||
self.assertFalse(cs.buttonEvents)
|
||||
self.assertFalse(owner.allowed_latch or owner.output(cs)[0])
|
||||
self.sample(owner, [], active=True)
|
||||
self.assertFalse(owner.allowed_latch)
|
||||
|
||||
def test_auxiliary_off_after_pending_activation_cannot_rearm_same_engagement(self):
|
||||
owner = self.owner(lkas=AOL_TOGGLE)
|
||||
self.sample(owner, [(0, 0, Buttons.NONE)])
|
||||
cs = self.sample(owner, [(0, 1, Buttons.NONE), (0, 0, Buttons.NONE)])
|
||||
self.assertTrue(owner.allowed_latch and owner.physical.authorized)
|
||||
now = owner._test_clock_ns + 1
|
||||
owner.observe_full_engagement(cs, enabled=True, active=False, source_ns=now,
|
||||
observation_valid=True, now_ns=now, fault_active=False)
|
||||
owner._perform(AOL_TOGGLE)
|
||||
self.assertFalse(owner.allowed_latch)
|
||||
self.sample(owner, [], active=True)
|
||||
self.assertFalse(owner.allowed_latch)
|
||||
|
||||
def test_cold_held_no_source_and_unphysical_auxiliary_cannot_create_output(self):
|
||||
for main_action, lkas_action in ((0, 0), (AOL_TOGGLE, 0), (0, AOL_TOGGLE),
|
||||
(PAUSE_LATERAL, PAUSE_LONGITUDINAL)):
|
||||
|
||||
@@ -1,179 +0,0 @@
|
||||
"""Shared full-engagement intent, independent of native steering permission."""
|
||||
from dataclasses import replace
|
||||
from types import SimpleNamespace
|
||||
|
||||
import pytest
|
||||
from opendbc.car.structs import car
|
||||
from openpilot.starpilot.aol.intent import AolCardIntent, AolSettings, AOL_TOGGLE
|
||||
from openpilot.starpilot.car.ford.aol import FordCardIntent
|
||||
from openpilot.starpilot.car.gm.aol import GmAolCardIntent
|
||||
from openpilot.starpilot.car.honda.stock_intent import HondaStockCardIntent
|
||||
from openpilot.starpilot.car.hyundai.classic_long_intent import ClassicLongCardIntent
|
||||
from openpilot.starpilot.car.hyundai.classic_scc_intent import ClassicSccCardIntent
|
||||
from openpilot.starpilot.car.hyundai.ev6_intent import Ev6CardIntent
|
||||
from openpilot.starpilot.car.hyundai.forte_intent import ForteCardIntent
|
||||
from openpilot.starpilot.car.mazda.stock_intent import MazdaStockCardIntent
|
||||
from openpilot.starpilot.car.tesla.screen_intent import TeslaScreenCardIntent
|
||||
from openpilot.starpilot.car.tesla.stock_intent import TeslaPreapCardIntent
|
||||
from openpilot.starpilot.car.toyota.aol import ToyotaCardIntent
|
||||
|
||||
SETTINGS = AolSettings(True, 0., AOL_TOGGLE, 0, (0, 0, 0), (0, 0, 0))
|
||||
CP = SimpleNamespace(flags=0, safetyConfigs=[SimpleNamespace(safetyParam=0)])
|
||||
|
||||
|
||||
def state():
|
||||
cs = car.CarState(canValid=True, gearShifter='drive')
|
||||
cs.cruiseState.available = True
|
||||
return cs
|
||||
|
||||
|
||||
def observe(owner, cs, stamp, *, enabled=True, active=True, valid=True, fault=False, rejection=0):
|
||||
owner.observe_full_engagement(cs, enabled=enabled, active=active, source_ns=stamp,
|
||||
observation_valid=valid, now_ns=max(1, stamp),
|
||||
fault_active=fault, native_rejection_ns=rejection)
|
||||
|
||||
|
||||
@pytest.mark.parametrize('factory', [
|
||||
lambda: AolCardIntent(SETTINGS, explicit_latch=True),
|
||||
lambda: FordCardIntent(CP, SETTINGS), lambda: GmAolCardIntent(SETTINGS),
|
||||
lambda: HondaStockCardIntent(CP, SETTINGS), lambda: MazdaStockCardIntent(CP, SETTINGS),
|
||||
lambda: ClassicLongCardIntent(CP, SETTINGS), lambda: ClassicSccCardIntent(CP, SETTINGS),
|
||||
lambda: ForteCardIntent(CP, SETTINGS), lambda: ToyotaCardIntent(SETTINGS),
|
||||
lambda: TeslaScreenCardIntent(CP, SETTINGS),
|
||||
])
|
||||
def test_full_engagement_arms_each_intent_owner_and_brake_does_not_clear_it(factory):
|
||||
owner, cs = factory(), state()
|
||||
observe(owner, cs, 1, enabled=False, active=False)
|
||||
observe(owner, cs, 2, active=False) # preEnabled is not active engagement.
|
||||
assert not owner.allowed_latch
|
||||
observe(owner, cs, 3)
|
||||
assert owner.allowed_latch and not owner.pause_lateral
|
||||
cs.brakePressed = True
|
||||
owner.update(cs, now_ns=4, fault_active=False)
|
||||
observe(owner, cs, 4, enabled=False, active=False)
|
||||
assert owner.output(cs)[0]
|
||||
assert not owner.pause_longitudinal
|
||||
|
||||
|
||||
def test_startup_or_replayed_enabled_packet_is_not_an_engagement():
|
||||
owner, cs = AolCardIntent(SETTINGS, explicit_latch=True), state()
|
||||
for stamp in (10, 11, 12):
|
||||
observe(owner, cs, stamp)
|
||||
assert not owner.allowed_latch
|
||||
observe(owner, cs, 13, enabled=False, active=False)
|
||||
observe(owner, cs, 14)
|
||||
assert owner.allowed_latch
|
||||
|
||||
|
||||
@pytest.mark.parametrize('valid,stamp', [(False, 3), (True, 2), (True, 1)])
|
||||
def test_missing_duplicate_or_reordered_control_cannot_rearm_after_manual_off(valid, stamp):
|
||||
owner, cs = AolCardIntent(SETTINGS, explicit_latch=True), state()
|
||||
observe(owner, cs, 1, enabled=False, active=False)
|
||||
observe(owner, cs, 2)
|
||||
assert owner.allowed_latch
|
||||
owner.allowed_latch, owner.pause_lateral = False, True
|
||||
observe(owner, cs, stamp, enabled=False, active=False, valid=valid)
|
||||
observe(owner, cs, 4, active=False)
|
||||
observe(owner, cs, 5)
|
||||
assert not owner.allowed_latch and owner.pause_lateral
|
||||
observe(owner, cs, 6, enabled=False, active=False)
|
||||
observe(owner, cs, 7)
|
||||
assert owner.allowed_latch and not owner.pause_lateral
|
||||
|
||||
|
||||
@pytest.mark.parametrize('valid,stamp', [(False, 3), (True, 2), (True, 1)])
|
||||
def test_manual_off_during_pending_activation_consumes_the_epoch(valid, stamp):
|
||||
owner, cs = AolCardIntent(SETTINGS, explicit_latch=True), state()
|
||||
observe(owner, cs, 1, enabled=False, active=False)
|
||||
observe(owner, cs, 2, active=False)
|
||||
cs.buttonEvents = [car.CarState.ButtonEvent(type='lkas', pressed=True)]
|
||||
observe(owner, cs, stamp, active=False, valid=valid)
|
||||
cs.buttonEvents = []
|
||||
observe(owner, cs, 4)
|
||||
assert not owner.allowed_latch
|
||||
|
||||
|
||||
@pytest.mark.parametrize('veto', ['disabled', 'fault', 'rejection', 'calibration', 'cancel', 'main', 'lkas'])
|
||||
def test_current_veto_consumes_engagement_without_replay(veto):
|
||||
owner, cs = AolCardIntent(SETTINGS, explicit_latch=True), state()
|
||||
observe(owner, cs, 10, enabled=False, active=False)
|
||||
if veto == 'disabled':
|
||||
owner.settings = replace(SETTINGS, enabled=False)
|
||||
if veto == 'calibration':
|
||||
owner._calibration_rearm_required = True
|
||||
if veto in ('cancel', 'main', 'lkas'):
|
||||
cs.buttonEvents = [car.CarState.ButtonEvent(type='mainCruise' if veto == 'main' else veto, pressed=True)]
|
||||
observe(owner, cs, 20, fault=veto == 'fault', rejection=15 if veto == 'rejection' else 0)
|
||||
assert not owner.allowed_latch
|
||||
owner.settings, owner._calibration_rearm_required = SETTINGS, False
|
||||
cs.buttonEvents = []
|
||||
observe(owner, cs, 21)
|
||||
assert not owner.allowed_latch
|
||||
|
||||
|
||||
@pytest.mark.parametrize('factory,field', [
|
||||
(lambda: GmAolCardIntent(SETTINGS), '_main_cycle_required'),
|
||||
(lambda: ForteCardIntent(CP, SETTINGS), 'fault_rearm'),
|
||||
])
|
||||
def test_vehicle_fault_rearm_cannot_be_bypassed(factory, field):
|
||||
owner, cs = factory(), state()
|
||||
observe(owner, cs, 1, enabled=False, active=False)
|
||||
setattr(owner, field, True)
|
||||
observe(owner, cs, 2)
|
||||
assert not owner.allowed_latch
|
||||
|
||||
|
||||
def test_physical_owners_cannot_be_forged_by_host_engagement():
|
||||
for owner in (Ev6CardIntent(SETTINGS), TeslaPreapCardIntent(CP, SETTINGS)):
|
||||
cs = state()
|
||||
observe(owner, cs, 1, enabled=False, active=False)
|
||||
observe(owner, cs, 2)
|
||||
assert not owner.allowed_latch and not owner.output(cs)[0]
|
||||
if isinstance(owner, Ev6CardIntent):
|
||||
assert not owner.physical.authorized and owner._last_latch_edge_ns == 0
|
||||
|
||||
|
||||
def test_tesla_screen_retains_explicit_brake_disengage_and_gear_policy():
|
||||
for gear, brake in (('sport', False), ('drive', True)):
|
||||
owner = TeslaScreenCardIntent(SimpleNamespace(safetyConfigs=[SimpleNamespace(safetyParam=1024)]), SETTINGS)
|
||||
cs = state()
|
||||
observe(owner, cs, 1, enabled=False, active=False)
|
||||
cs.gearShifter, cs.brakePressed = gear, brake
|
||||
owner.update(cs, now_ns=2, fault_active=False)
|
||||
observe(owner, cs, 2)
|
||||
assert not owner.allowed_latch
|
||||
|
||||
|
||||
def test_delayed_disabled_and_active_before_manual_off_cannot_reopen_epoch():
|
||||
owner, cs = AolCardIntent(SETTINGS, explicit_latch=True), state()
|
||||
observe(owner, cs, 80, enabled=False, active=False)
|
||||
cs.buttonEvents = [car.CarState.ButtonEvent(type='lkas', pressed=True)]
|
||||
owner.observe_full_engagement(cs, enabled=False, active=False, source_ns=90,
|
||||
observation_valid=True, now_ns=100, fault_active=False)
|
||||
cs.buttonEvents = []
|
||||
owner.observe_full_engagement(cs, enabled=True, active=True, source_ns=95,
|
||||
observation_valid=True, now_ns=110, fault_active=False)
|
||||
assert not owner.allowed_latch
|
||||
observe(owner, cs, 120, enabled=False, active=False)
|
||||
observe(owner, cs, 130)
|
||||
assert owner.allowed_latch
|
||||
|
||||
|
||||
def test_missing_source_does_not_consume_pending_activation():
|
||||
owner, cs = AolCardIntent(SETTINGS, explicit_latch=True), state()
|
||||
observe(owner, cs, 1, enabled=False, active=False)
|
||||
observe(owner, cs, 2, active=False)
|
||||
observe(owner, cs, 0, valid=False)
|
||||
observe(owner, cs, 3)
|
||||
assert owner.allowed_latch
|
||||
|
||||
|
||||
def test_auxiliary_off_after_pending_observation_cannot_be_undone_by_activation():
|
||||
owner, cs = AolCardIntent(SETTINGS, explicit_latch=False), state()
|
||||
observe(owner, cs, 1, enabled=False, active=False)
|
||||
observe(owner, cs, 2, active=False)
|
||||
owner.allowed_latch = True
|
||||
owner._perform(AOL_TOGGLE)
|
||||
assert not owner.allowed_latch
|
||||
observe(owner, cs, 3)
|
||||
assert not owner.allowed_latch
|
||||
@@ -206,7 +206,7 @@ class IoniqAvailabilityTests(unittest.TestCase):
|
||||
def step(tick, *, timeout=False, old_intent=False, allowed=True, pause=False, latched=True,
|
||||
brake=False, steering=False, gas=False, user_disable=False, observed_state=None, authority_unavailable=False, companion=True,
|
||||
long_allowed=True, pause_long=False, state_stamp=None, intent_message=None, offset_ns=0,
|
||||
native_request_lat=True, native_request_long=None, native_valid=True, panda_fault=False, panda_controls_allowed=None,
|
||||
native_request_lat=True, native_request_long=None, native_valid=True, panda_fault=False,
|
||||
critical_event=None, calibrated=True, car_output_valid=True, relay_fault=False):
|
||||
now = base + tick * 10_000_000 + offset_ns
|
||||
clock.return_value = now
|
||||
@@ -215,8 +215,6 @@ class IoniqAvailabilityTests(unittest.TestCase):
|
||||
state = car_state(brake=brake) if observed_state is None else observed_state
|
||||
state.cruiseState.available = True
|
||||
state.steeringPressed, state.gasPressed = steering, gas
|
||||
if panda_controls_allowed is None:
|
||||
panda_controls_allowed = full_op if native_request_long is None else native_request_long
|
||||
state_event = messaging.new_message('carState', valid=True, logMonoTime=now if state_stamp is None else state_stamp)
|
||||
state_event.carState = state
|
||||
recv.return_value = None if timeout else state_event.as_reader()
|
||||
@@ -236,7 +234,7 @@ class IoniqAvailabilityTests(unittest.TestCase):
|
||||
for panda, config in zip(pandas.pandaStates, cp.safetyConfigs, strict=True):
|
||||
panda.safetyModel, panda.safetyParam = config.safetyModel, config.safetyParam
|
||||
panda.alternativeExperience = cp.alternativeExperience
|
||||
panda.controlsAllowed = panda_controls_allowed
|
||||
panda.controlsAllowed = full_op
|
||||
panda.safetyRxChecksInvalid = panda_fault
|
||||
if relay_fault:
|
||||
panda.faults = ['relayMalfunction']
|
||||
@@ -250,12 +248,12 @@ class IoniqAvailabilityTests(unittest.TestCase):
|
||||
native = messaging.new_message('aolSafetyWire', 0, valid=native_valid, logMonoTime=now)
|
||||
native.aolSafetyWire = encode_safety(SafetyState(1, True, now, now + 200_000_000,
|
||||
int(cp.safetyConfigs[-1].safetyModel.raw), cp.safetyConfigs[-1].safetyParam,
|
||||
allowed, (full_op or native_request_long is True) and long_allowed, native_request_lat,
|
||||
allowed, full_op and long_allowed, native_request_lat,
|
||||
full_op if native_request_long is None else native_request_long, 'authority-panda', sd.aol_session_id,
|
||||
now if genuine_events else 0, tuple(
|
||||
PandaSlot(index, 'authority-panda' if index == len(cp.safetyConfigs) - 1 else f'silent-panda-{index}',
|
||||
int(config.safetyModel.raw), config.safetyParam,
|
||||
cp.alternativeExperience, panda_controls_allowed, panda_fault, False,
|
||||
cp.alternativeExperience, full_op, panda_fault, False,
|
||||
1 if relay_fault else 0)
|
||||
for index, config in enumerate(cp.safetyConfigs)) if genuine_events else ()))
|
||||
if companion:
|
||||
@@ -298,7 +296,7 @@ class IoniqAvailabilityTests(unittest.TestCase):
|
||||
return sd.pm.messages['aolAxisState'].aolAxisState, sd.pm.messages['selfdriveState'].selfdriveState, command
|
||||
return sd, controls, step
|
||||
|
||||
def _ioniq_availability_callers(self, full_op=True, *, initially_armed=True):
|
||||
def _ioniq_availability_callers(self, full_op=True):
|
||||
from opendbc.car.hyundai.interface import CarInterface
|
||||
from opendbc.car.hyundai.radar_interface import RadarInterface
|
||||
from openpilot.selfdrive.car.card import Car
|
||||
@@ -326,11 +324,10 @@ class IoniqAvailabilityTests(unittest.TestCase):
|
||||
cs = car_state()
|
||||
cs.cruiseState.available = True
|
||||
card.aol_card_intent.update(cs, now_ns=9_980_000_000)
|
||||
if initially_armed:
|
||||
cs.buttonEvents = [car.CarState.ButtonEvent(type=car.CarState.ButtonEvent.Type.lkas, pressed=True)]
|
||||
card.aol_card_intent.update(cs, now_ns=9_990_000_000)
|
||||
cs.buttonEvents = []
|
||||
self.assertEqual(card.aol_card_intent.allowed_latch, initially_armed)
|
||||
cs.buttonEvents = [car.CarState.ButtonEvent(type=car.CarState.ButtonEvent.Type.lkas, pressed=True)]
|
||||
card.aol_card_intent.update(cs, now_ns=9_990_000_000)
|
||||
cs.buttonEvents = []
|
||||
self.assertTrue(card.aol_card_intent.allowed_latch)
|
||||
|
||||
def receive(now, *, axis=None, event=None, state=None):
|
||||
frames = []
|
||||
@@ -341,7 +338,7 @@ class IoniqAvailabilityTests(unittest.TestCase):
|
||||
setattr(message, service, sd.sm[service])
|
||||
# Card receives the exact envelope bytes, including validity and stamps.
|
||||
frames.append(messaging.log_from_bytes(message.to_bytes()))
|
||||
card.sm.update_msgs(now / 1e9, frames + [m for m in (axis, event, sd.pm.messages.get('selfdriveState')) if m is not None])
|
||||
card.sm.update_msgs(now / 1e9, frames + [m for m in (axis, event) if m is not None])
|
||||
observed = state if state is not None else car_state()
|
||||
observed.cruiseState.available = True
|
||||
card.CC_prev = card.sm['carControl']
|
||||
@@ -364,85 +361,12 @@ class IoniqAvailabilityTests(unittest.TestCase):
|
||||
if full_op:
|
||||
self.assertTrue(axis.lateralActive and axis.longitudinalActive and command.latActive and command.longActive)
|
||||
else:
|
||||
self.assertEqual(bool(axis.lateralActive and command.latActive), initially_armed)
|
||||
self.assertTrue(axis.lateralActive and command.latActive)
|
||||
self.assertFalse(sd.enabled or sd.active or axis.longitudinalActive or command.longActive)
|
||||
self.assertEqual(receive(10_000_000_000 + tick * 10_000_000,
|
||||
axis=sd.pm.messages['aolAxisState'], event=sd.pm.messages['onroadEvents']), initially_armed)
|
||||
self.assertTrue(receive(10_000_000_000 + tick * 10_000_000,
|
||||
axis=sd.pm.messages['aolAxisState'], event=sd.pm.messages['onroadEvents']))
|
||||
return sd, controls, drive, card, receive
|
||||
|
||||
def test_full_set_engagement_arms_aol_and_brake_keeps_lateral_without_lkas(self):
|
||||
from opendbc.car.hyundai.values import Buttons
|
||||
|
||||
with OpenpilotPrefix():
|
||||
sd, _, step, card, receive = self._ioniq_availability_callers(full_op=False, initially_armed=False)
|
||||
self.assertFalse(card.aol_card_intent.allowed_latch)
|
||||
for tick, previous, current in ((2, Buttons.NONE, Buttons.SET_DECEL),
|
||||
(3, Buttons.SET_DECEL, Buttons.NONE)):
|
||||
cs = car_state()
|
||||
cs.buttonEvents = card.CI.CS.create_cruise_button_events(current, previous)
|
||||
cs.buttonEnable = card.CI.CS.update_button_enable(cs.buttonEvents)
|
||||
step(tick, observed_state=cs, native_request_long=False)
|
||||
receive(10_000_000_000 + tick * 10_000_000, state=cs,
|
||||
axis=sd.pm.messages['aolAxisState'], event=sd.pm.messages['onroadEvents'])
|
||||
self.assertIn(log.OnroadEvent.EventName.buttonEnable, sd.events.names)
|
||||
self.assertTrue(sd.enabled and sd.active)
|
||||
# PandaStates updates at 10 Hz. Grant LONG only once its actual fresh
|
||||
# health observation agrees, retaining the production latest-Panda veto.
|
||||
for tick in range(4, 13):
|
||||
granted = tick >= 10
|
||||
axis, _, command = step(tick, native_request_long=granted)
|
||||
self.assertTrue(axis.lateralActive and command.latActive)
|
||||
self.assertEqual(bool(axis.longitudinalActive and command.longActive), granted)
|
||||
receive(10_000_000_000 + tick * 10_000_000,
|
||||
axis=sd.pm.messages['aolAxisState'], event=sd.pm.messages['onroadEvents'])
|
||||
self.assertTrue(card.aol_card_intent.allowed_latch)
|
||||
for tick in (13, 14, 15):
|
||||
axis, _, command = step(tick, brake=tick == 13, native_request_long=False)
|
||||
self.assertTrue(axis.lateralActive and command.latActive)
|
||||
self.assertFalse(sd.enabled or sd.active or axis.longitudinalActive or command.longActive)
|
||||
self.assertTrue(receive(10_000_000_000 + tick * 10_000_000, state=car_state(brake=tick == 13),
|
||||
axis=sd.pm.messages['aolAxisState'], event=sd.pm.messages['onroadEvents']))
|
||||
self.assertFalse(any(e.type == car.CarState.ButtonEvent.Type.lkas for e in card.CI.CS.out.buttonEvents))
|
||||
|
||||
def test_reverse_keeps_actual_card_armed_without_output_and_drive_requires_ack(self):
|
||||
from openpilot.starpilot.aol.wire import decode_intent
|
||||
|
||||
with OpenpilotPrefix():
|
||||
sd, _, step, card, receive = self._ioniq_availability_callers(full_op=False)
|
||||
card.pm = type(sd.pm)()
|
||||
card.aol_sequence = 2
|
||||
for tick in range(2, 45):
|
||||
reverse = tick < 42
|
||||
cs = car_state(brake=reverse)
|
||||
cs.gearShifter = car.CarState.GearShifter.reverse if reverse else car.CarState.GearShifter.drive
|
||||
now = 10_000_000_000 + tick * 10_000_000
|
||||
original_message = messaging.new_message
|
||||
def current_message(*args, _original=original_message, _now=now, **kwargs):
|
||||
message = _original(*args, **kwargs)
|
||||
message.logMonoTime = _now
|
||||
return message
|
||||
with mock.patch('openpilot.selfdrive.car.card.messaging.new_message', side_effect=current_message):
|
||||
card.state_publish(cs, None)
|
||||
intent_message = card.pm.messages['aolIntentWire'].as_builder()
|
||||
intent = decode_intent(bytes(intent_message.aolIntentWire))
|
||||
self.assertTrue(intent.lateralArmed)
|
||||
self.assertEqual(intent.allowedLatch, not reverse)
|
||||
requested = tick == 2 or tick >= 43
|
||||
axis, state, command = step(tick, observed_state=cs, intent_message=intent_message,
|
||||
native_request_lat=requested, allowed=requested, native_request_long=False)
|
||||
self.assertEqual(bool(axis.lateralActive), tick >= 43)
|
||||
self.assertEqual(bool(command.latActive), tick >= 43)
|
||||
self.assertFalse(axis.longitudinalActive or command.longActive or sd.enabled or sd.active)
|
||||
self.assertTrue(axis.nativeAcknowledged, (tick, sd.events.names,
|
||||
[(str(sample.gearShifter), sample.canValid, sample.canTimeout) for sample, _ in sd.car_state_samples], axis.to_dict()))
|
||||
self.assertEqual(str(axis.faultReason), 'none')
|
||||
if reverse:
|
||||
self.assertIn(log.OnroadEvent.EventName.reverseGear, sd.events.names)
|
||||
self.assertIn(log.OnroadEvent.EventName.wrongGear, sd.events.names)
|
||||
self.assertFalse(axis.desiredLateral or axis.desiredLongitudinal)
|
||||
self.assertTrue(receive(now, state=cs, axis=sd.pm.messages['aolAxisState'],
|
||||
event=sd.pm.messages['onroadEvents']))
|
||||
|
||||
def test_ioniq_caroutput_communication_fault_joined_delivery_and_recovery(self):
|
||||
for order in ('axis_first', 'event_first', 'together'):
|
||||
with self.subTest(order=order), OpenpilotPrefix():
|
||||
|
||||
@@ -283,9 +283,7 @@ class _Ev6CancelCaller:
|
||||
now = self.last_now if now is None else now
|
||||
owner = self.card.aol_card_intent
|
||||
owner.observe_physical_samples(self.card.CI.CS, now_ns=now)
|
||||
owner.update(self.cs, now_ns=now, fault_active=fault, standard_enabled=active)
|
||||
owner.observe_full_engagement(self.cs, enabled=active, active=active, source_ns=now,
|
||||
observation_valid=True, now_ns=now, fault_active=fault)
|
||||
owner.update(self.cs, now_ns=now, fault_active=fault, standard_active=active, standard_enabled=active)
|
||||
return owner
|
||||
|
||||
def arm(self):
|
||||
@@ -296,7 +294,7 @@ class _Ev6CancelCaller:
|
||||
assert not self.intent().allowed_latch
|
||||
self.parse(Buttons.NONE)
|
||||
assert not self.intent().allowed_latch
|
||||
assert self.intent(active=True, now=self.last_now + 1).allowed_latch
|
||||
assert self.intent(active=True).allowed_latch
|
||||
assert self.card.aol_card_intent.physical.authorized
|
||||
|
||||
def evidence(self, *, partial=False, lat_only=False, now=None, mutation=None, event=None):
|
||||
|
||||
@@ -50,9 +50,8 @@ def test_actual_temporary_host_event_keeps_only_armed_intent(event_name):
|
||||
assert leases(card.aol_transport_feedback) == original
|
||||
|
||||
|
||||
@pytest.mark.parametrize('coincident_communication', [False, True])
|
||||
@pytest.mark.parametrize('fault', ['native_denial', 'panda_fault', 'permanent_eps', 'invalid_can', 'hard_event', 'overheat'])
|
||||
def test_recorded_temporary_host_path_never_masks_native_or_hard_fault(fault, coincident_communication):
|
||||
@pytest.mark.parametrize('fault', ['native_denial', 'panda_fault', 'permanent_eps', 'invalid_can', 'hard_event'])
|
||||
def test_recorded_temporary_host_path_never_masks_native_or_hard_fault(fault):
|
||||
case = fixture.IoniqAvailabilityTests(methodName='runTest')
|
||||
with ExitStack() as cleanup:
|
||||
cleanup.callback(case.doCleanups)
|
||||
@@ -63,17 +62,11 @@ def test_recorded_temporary_host_path_never_masks_native_or_hard_fault(fault, co
|
||||
state = fixture.car_state()
|
||||
state.steerFaultPermanent = fault == 'permanent_eps'
|
||||
state.canValid = fault != 'invalid_can'
|
||||
proof, event = sd.pm.messages['aolAxisState'], sd.pm.messages['onroadEvents'].as_builder()
|
||||
extra = []
|
||||
if coincident_communication:
|
||||
extra.append({'name': 'commIssue', 'softDisable': True})
|
||||
if fault == 'overheat':
|
||||
extra.append({'name': 'overheat', 'softDisable': True})
|
||||
if extra:
|
||||
event.onroadEvents = [entry.to_dict() for entry in event.onroadEvents] + extra
|
||||
proof, event = sd.pm.messages['aolAxisState'], sd.pm.messages['onroadEvents']
|
||||
if fault == 'hard_event':
|
||||
event = event.as_builder()
|
||||
event.onroadEvents[0].immediateDisable = True
|
||||
assert not receive(10_020_000_000, axis=proof, event=event.as_reader(), state=state)
|
||||
assert not receive(10_020_000_000, axis=proof, event=event, state=state)
|
||||
assert not command.longActive
|
||||
|
||||
|
||||
@@ -213,9 +206,7 @@ def test_verified_eps_marker_cannot_cross_owner_reset(reset):
|
||||
assert not command.latActive and not command.longActive
|
||||
|
||||
|
||||
@pytest.mark.parametrize('coincident_communication', [False, True])
|
||||
@pytest.mark.parametrize('recovery_tick', [40, 303])
|
||||
def test_full_long_location_error_event_first_withdraw_recover_brake_and_lkas_off(coincident_communication, recovery_tick):
|
||||
def test_full_long_location_error_event_first_withdraw_recover_brake_and_lkas_off():
|
||||
case = fixture.IoniqAvailabilityTests(methodName='runTest')
|
||||
with ExitStack() as cleanup:
|
||||
cleanup.callback(case.doCleanups)
|
||||
@@ -227,8 +218,6 @@ def test_full_long_location_error_event_first_withdraw_recover_brake_and_lkas_of
|
||||
def events(cs):
|
||||
result = actual_events(cs)
|
||||
if location_error[0]:
|
||||
if coincident_communication:
|
||||
sd.events.add(log.OnroadEvent.EventName.commIssue)
|
||||
sd.events.add(log.OnroadEvent.EventName.locationdTemporaryError)
|
||||
return result
|
||||
|
||||
@@ -283,8 +272,7 @@ def test_full_long_location_error_event_first_withdraw_recover_brake_and_lkas_of
|
||||
axis, state, command = drive_with_sources(2)
|
||||
assert state.enabled and state.active
|
||||
assert state.state == log.SelfdriveState.OpenpilotState.softDisabling
|
||||
expected_alert = 'commIssue' if coincident_communication else 'locationdTemporaryError'
|
||||
assert state.alertType == expected_alert + '/softDisable'
|
||||
assert state.alertType == 'locationdTemporaryError/softDisable'
|
||||
assert not axis.lateralActive and not axis.longitudinalActive
|
||||
assert not command.latActive and not command.longActive
|
||||
assert sd.state_machine.soft_disable_timer == 300
|
||||
@@ -295,36 +283,32 @@ def test_full_long_location_error_event_first_withdraw_recover_brake_and_lkas_of
|
||||
assert receive(10_020_100_000, axis=sd.pm.messages['aolAxisState'], event=sd.pm.messages['onroadEvents'])
|
||||
assert card.aol_card_intent.allowed_latch
|
||||
# Follow the unchanged ordinary SOFT countdown, not a forced fixture disable.
|
||||
for tick in range(3, recovery_tick):
|
||||
for tick in range(3, 303):
|
||||
axis, state, command = drive_with_sources(tick)
|
||||
assert publish_to_card(tick, axis=sd.pm.messages['aolAxisState'], event=sd.pm.messages['onroadEvents'])
|
||||
ordinary_still_enabled = recovery_tick < 303
|
||||
assert state.enabled == state.active == ordinary_still_enabled
|
||||
assert not state.enabled and not state.active
|
||||
assert not axis.lateralActive and not axis.longitudinalActive
|
||||
assert not command.latActive and not command.longActive
|
||||
assert card.aol_card_intent.allowed_latch
|
||||
location_error[0] = False
|
||||
axis, state, command = drive_with_sources(recovery_tick, native_request_long=ordinary_still_enabled)
|
||||
assert state.enabled == state.active == ordinary_still_enabled
|
||||
assert axis.lateralActive and axis.longitudinalActive == ordinary_still_enabled
|
||||
assert command.latActive and command.longActive == ordinary_still_enabled
|
||||
assert publish_to_card(recovery_tick, axis=sd.pm.messages['aolAxisState'], event=sd.pm.messages['onroadEvents'])
|
||||
brake_tick = recovery_tick + 1
|
||||
axis, state, command = drive_with_sources(brake_tick, brake=True, native_request_long=False)
|
||||
axis, state, command = drive_with_sources(303, native_request_long=False)
|
||||
assert not state.enabled and not state.active
|
||||
assert axis.lateralActive and not axis.longitudinalActive
|
||||
assert command.latActive and not command.longActive
|
||||
assert publish_to_card(303, axis=sd.pm.messages['aolAxisState'], event=sd.pm.messages['onroadEvents'])
|
||||
axis, state, command = drive_with_sources(304, brake=True, native_request_long=False)
|
||||
assert not state.enabled and not axis.longitudinalActive and not command.longActive
|
||||
assert axis.lateralActive and command.latActive
|
||||
assert publish_to_card(brake_tick, axis=sd.pm.messages['aolAxisState'], event=sd.pm.messages['onroadEvents'],
|
||||
assert publish_to_card(304, axis=sd.pm.messages['aolAxisState'], event=sd.pm.messages['onroadEvents'],
|
||||
state=fixture.car_state(brake=True))
|
||||
from opendbc.car.structs import car
|
||||
release = fixture.car_state()
|
||||
release.buttonEvents = [car.CarState.ButtonEvent(type=car.CarState.ButtonEvent.Type.lkas, pressed=False)]
|
||||
brake_ns = 10_000_000_000 + brake_tick * 10_000_000
|
||||
assert receive(brake_ns + 50_000, state=release)
|
||||
assert receive(13_040_050_000, state=release)
|
||||
assert not card.aol_card_intent._lkas_held
|
||||
off = fixture.car_state()
|
||||
off.buttonEvents = [car.CarState.ButtonEvent(type=car.CarState.ButtonEvent.Type.lkas, pressed=True)]
|
||||
assert not receive(brake_ns + 100_000, state=off)
|
||||
assert not receive(13_040_100_000, state=off)
|
||||
assert not card.aol_card_intent.allowed_latch
|
||||
axis, state, command = drive_with_sources(brake_tick + 1, native_request_long=False)
|
||||
axis, state, command = drive_with_sources(305, native_request_long=False)
|
||||
assert not axis.lateralActive and not axis.longitudinalActive
|
||||
assert not command.latActive and not command.longActive and not state.enabled
|
||||
|
||||
@@ -1532,7 +1532,7 @@ class IpcAxisContractTests(unittest.TestCase):
|
||||
sm = SM(aolSafetyWire=b'', pandaStates=[],
|
||||
driverMonitoringState=SimpleNamespace(alertLevel=0, lockout=False, alwaysOnLockout=False),
|
||||
extrinsicsCalibration=SimpleNamespace(calStatus=log.ExtrinsicsCalibration.Status.calibrated))
|
||||
sm.ignore_alive, sm.ignore_valid = set(), set()
|
||||
sm.ignore_alive, sm.ignore_valid = [], []
|
||||
sm.seen = dict(SM.seen)
|
||||
self.enterContext(mock.patch.object(sd, 'sm', sm, create=True))
|
||||
sd.car_state_sock = object()
|
||||
@@ -1780,7 +1780,7 @@ class IpcAxisContractTests(unittest.TestCase):
|
||||
sm = SM(aolSafetyWire=b'', pandaStates=[],
|
||||
driverMonitoringState=SimpleNamespace(alertLevel=0, lockout=False, alwaysOnLockout=False),
|
||||
extrinsicsCalibration=SimpleNamespace(calStatus=log.ExtrinsicsCalibration.Status.calibrated))
|
||||
sm.ignore_alive, sm.ignore_valid = set(), set()
|
||||
sm.ignore_alive, sm.ignore_valid = [], []
|
||||
sm.seen = dict(SM.seen)
|
||||
self.enterContext(mock.patch.object(sd, 'sm', sm, create=True))
|
||||
sd.car_state_sock = object()
|
||||
|
||||
@@ -6,7 +6,6 @@ import pytest
|
||||
from openpilot.cereal import log
|
||||
from openpilot.common.prefix import OpenpilotPrefix
|
||||
from openpilot.starpilot.aol.tests import test_ioniq_availability as fixture
|
||||
from openpilot.starpilot.aol.wire import decode_safety
|
||||
|
||||
|
||||
@pytest.mark.parametrize(('full_op', 'gas'), [(False, True), (True, False)],
|
||||
@@ -134,13 +133,7 @@ def test_persistent_lag_keeps_latch_but_ordinary_countdown_still_disables():
|
||||
sd, _, drive, card, receive = case._ioniq_availability_callers(full_op=True)
|
||||
with mock.patch.object(type(sd.rk), 'lagging', new_callable=mock.PropertyMock, return_value=True):
|
||||
for tick in range(2, 304):
|
||||
axis, state, command = drive(tick, native_request_long=False, panda_controls_allowed=True)
|
||||
native = decode_safety(sd.sm['aolSafetyWire'])
|
||||
assert not native.requestedLongitudinal
|
||||
assert all(p.controlsAllowed for p in native.pandaInventory)
|
||||
assert all(p.controlsAllowed for p in sd.sm['pandaStates'])
|
||||
assert sd.mismatch_counter == 0
|
||||
assert log.OnroadEvent.EventName.controlsMismatch not in sd.events.names
|
||||
axis, state, command = drive(tick, native_request_long=False)
|
||||
ordinary_active = tick < 302
|
||||
assert state.enabled == state.active == ordinary_active
|
||||
assert sd.state_machine.soft_disable_timer == max(0, 302 - tick)
|
||||
@@ -157,43 +150,12 @@ def test_persistent_lag_keeps_latch_but_ordinary_countdown_still_disables():
|
||||
axis=sd.pm.messages['aolAxisState'], event=sd.pm.messages['onroadEvents'])
|
||||
assert not state.enabled and not state.active and sd.state_machine.soft_disable_timer == 0
|
||||
assert card.aol_card_intent.allowed_latch
|
||||
axis, state, command = drive(304, native_request_long=False, panda_controls_allowed=True)
|
||||
axis, state, command = drive(304, native_request_long=False)
|
||||
assert not state.enabled and not command.longActive and not axis.longitudinalActive
|
||||
# A newly current lateral native ACK alone can restore lateral output.
|
||||
assert axis.lateralActive and command.latActive
|
||||
|
||||
|
||||
def test_persistent_lag_does_not_mask_native_controls_mismatch():
|
||||
case = fixture.IoniqAvailabilityTests(methodName='runTest')
|
||||
with ExitStack() as cleanup:
|
||||
cleanup.callback(case.doCleanups)
|
||||
cleanup.enter_context(OpenpilotPrefix())
|
||||
sd, _, drive, card, receive = case._ioniq_availability_callers(full_op=True)
|
||||
with mock.patch.object(type(sd.rk), 'lagging', new_callable=mock.PropertyMock, return_value=True):
|
||||
for tick in range(2, 210):
|
||||
axis, state, command = drive(tick, native_request_long=False, panda_controls_allowed=False)
|
||||
native = decode_safety(sd.sm['aolSafetyWire'])
|
||||
assert not native.requestedLongitudinal
|
||||
assert not any(p.controlsAllowed for p in native.pandaInventory)
|
||||
# The real 10 Hz Panda publication first reports denial at tick 10.
|
||||
assert sd.mismatch_counter == max(0, tick - 9)
|
||||
assert sd.state_machine.soft_disable_timer == 302 - tick
|
||||
assert state.enabled == state.active == (tick < 209)
|
||||
assert not axis.longitudinalActive and not command.longActive
|
||||
if tick < 209:
|
||||
assert not sd.events.contains(fixture.ET.IMMEDIATE_DISABLE)
|
||||
assert receive(10_000_000_000 + tick*10_000_000,
|
||||
axis=sd.pm.messages['aolAxisState'], event=sd.pm.messages['onroadEvents'])
|
||||
else:
|
||||
assert sd.mismatch_counter == 200 and sd.state_machine.soft_disable_timer == 93
|
||||
assert log.OnroadEvent.EventName.controlsMismatch in sd.events.names
|
||||
assert sd.events.contains(fixture.ET.IMMEDIATE_DISABLE)
|
||||
assert not axis.lateralActive and not command.latActive
|
||||
assert not receive(10_000_000_000 + tick*10_000_000,
|
||||
axis=sd.pm.messages['aolAxisState'], event=sd.pm.messages['onroadEvents'])
|
||||
assert not card.aol_card_intent.allowed_latch
|
||||
|
||||
|
||||
@pytest.mark.parametrize('failure', ['missing_event', 'hard_event', 'wrong_event', 'session', 'inventory'])
|
||||
def test_axis_first_critical_join_never_masks_missing_or_hard_occurrence(failure):
|
||||
case = fixture.IoniqAvailabilityTests(methodName='runTest')
|
||||
|
||||
@@ -275,11 +275,7 @@ class TransportPauseFeedback:
|
||||
events = sm['onroadEvents']
|
||||
faults = [event for event in events if event.immediateDisable or event.softDisable]
|
||||
names = (log.OnroadEvent.EventName.selfdrivedLagging, log.OnroadEvent.EventName.locationdTemporaryError)
|
||||
location_fault = bool(any(event.name == log.OnroadEvent.EventName.locationdTemporaryError for event in faults) and
|
||||
all(event.softDisable and not event.immediateDisable and event.name in
|
||||
(log.OnroadEvent.EventName.locationdTemporaryError, log.OnroadEvent.EventName.commIssue) for event in faults))
|
||||
temporary = location_fault or bool(len(faults) == 1 and faults[0].name in names and
|
||||
faults[0].softDisable and not faults[0].immediateDisable)
|
||||
temporary = bool(len(faults) == 1 and faults[0].name in names and faults[0].softDisable and not faults[0].immediateDisable)
|
||||
eps_recovery = bool(not cs.steerFaultTemporary and getattr(self, 'temporary_eps_event_ns', 0) and
|
||||
axis.faultEventMonoTime == self.temporary_eps_event_ns and
|
||||
all(not event.immediateDisable and
|
||||
@@ -287,7 +283,8 @@ class TransportPauseFeedback:
|
||||
users = [event for event in events if event.userDisable]
|
||||
pedal = bool(cs.brakePressed and users and all(event.name == log.OnroadEvent.EventName.pedalPressed and
|
||||
not event.softDisable and not event.immediateDisable for event in users) and not faults)
|
||||
if axis.longitudinalActive and not location_fault or not (temporary or eps_recovery or pedal):
|
||||
location_only = bool(temporary and faults[0].name == log.OnroadEvent.EventName.locationdTemporaryError)
|
||||
if axis.longitudinalActive and not location_only or not (temporary or eps_recovery or pedal):
|
||||
return False
|
||||
if (not latched or self.failed or self.paused or self.pending_event or self.retention_started_ns or
|
||||
not self.session or str(axis.sessionId) != self.session or
|
||||
@@ -490,12 +487,17 @@ class TransportPauseFeedback:
|
||||
return True
|
||||
|
||||
def _honda_brake_lateral_current(self, sm, cp, cs, axis, now_ns, *, latched, brake_source_ns):
|
||||
"""Preserve existing LAT intent through brake's LONG withdrawal, never output."""
|
||||
"""Preserve existing LAT intent through brake's LONG withdrawal, never output.
|
||||
|
||||
Honda stock AOL and the Ioniq 6 both see Panda revoke LONG on brake before
|
||||
the host withdraws its LONG request."""
|
||||
from opendbc.car.honda.stock_aol import qualified
|
||||
from opendbc.car.structs import car
|
||||
from openpilot.starpilot.car.hyundai.aol import qualified_ioniq6
|
||||
|
||||
source_ns = self.honda_brake_ns or (brake_source_ns if cs.brakePressed else 0)
|
||||
if (not qualified(cp, marked_only=True) or not latched or self.failed or self.paused or self.pending_event or
|
||||
if (not (qualified(cp, marked_only=True) or qualified_ioniq6(cp)) or
|
||||
not latched or self.failed or self.paused or self.pending_event or
|
||||
not self.session or str(axis.sessionId) != self.session or
|
||||
not self.healthy_feedback_ns or not 0 <= now_ns - self.healthy_feedback_ns <= AXIS_MAX_AGE_NS or
|
||||
not 0 < source_ns <= now_ns or now_ns - source_ns > SAFETY_MAX_AGE_NS or
|
||||
|
||||
@@ -76,7 +76,7 @@ def _bounded_id(value: str) -> str:
|
||||
return value
|
||||
|
||||
|
||||
def _payload(raw_value, schema, kind: int, materialize, versions=(WIRE_VERSION,)):
|
||||
def _payload(raw_value, schema, kind: int, versions=(WIRE_VERSION,)):
|
||||
if not isinstance(raw_value, (bytes, bytearray, memoryview)):
|
||||
return None
|
||||
byte_count = raw_value.nbytes if isinstance(raw_value, memoryview) else len(raw_value)
|
||||
@@ -92,7 +92,7 @@ def _payload(raw_value, schema, kind: int, materialize, versions=(WIRE_VERSION,)
|
||||
with schema.from_bytes(raw, traversal_limit_in_words=MAX_WIRE_WORDS, nesting_limit=4) as decoded:
|
||||
if int(decoded.kind) != kind or int(decoded.version) not in versions:
|
||||
return None
|
||||
return materialize(decoded)
|
||||
return decoded.to_dict()
|
||||
except (ValueError, OverflowError, RuntimeError, UnicodeDecodeError, capnp.KjException):
|
||||
return None
|
||||
|
||||
@@ -112,52 +112,49 @@ def _memoized(decode):
|
||||
return wrapper
|
||||
|
||||
|
||||
def _read_safety(value) -> SafetyState | None:
|
||||
try:
|
||||
source_ns = int(value.sourcePandaStatesMonoTime)
|
||||
inventory = tuple(PandaSlot(int(slot.slotIndex), _bounded_id(str(slot.hardwareSerial)),
|
||||
int(slot.safetyModel), int(slot.safetyParam), int(slot.alternativeExperience),
|
||||
bool(slot.controlsAllowed), bool(slot.safetyRxChecksInvalid),
|
||||
bool(slot.heartbeatLost), int(slot.faults))
|
||||
for slot in value.pandaInventory)
|
||||
if (int(value.version) == WIRE_VERSION and (source_ns or inventory) or
|
||||
int(value.version) == SAFETY_INVENTORY_VERSION and
|
||||
(source_ns <= 0 or not 1 <= len(inventory) <= MAX_PANDA_SLOTS)):
|
||||
return None
|
||||
result = SafetyState(int(value.protocolVersion), bool(value.compatible), int(value.observedMonoTime),
|
||||
int(value.validUntilMonoTime), int(value.safetyModel), int(value.safetyParam),
|
||||
bool(value.lateralAllowed), bool(value.longitudinalAllowed), bool(value.requestedLateral),
|
||||
bool(value.requestedLongitudinal), _bounded_id(str(value.pandaSerial)),
|
||||
_bounded_id(str(value.axisSessionId)), source_ns, inventory)
|
||||
if result.validUntilMonoTime < result.observedMonoTime:
|
||||
return None
|
||||
return result
|
||||
except (KeyError, ValueError, OverflowError, RuntimeError, capnp.KjException):
|
||||
return None
|
||||
|
||||
|
||||
def _read_intent(value) -> IntentState | None:
|
||||
try:
|
||||
result = IntentState(_bounded_id(str(value.producerSessionId)), int(value.sequence),
|
||||
int(value.carStateLogMonoTime), int(value.observedMonoTime), int(value.validUntilMonoTime),
|
||||
bool(value.allowedLatch), bool(value.pauseLateral), bool(value.pauseLongitudinal),
|
||||
bool(value.settingsQualified), bool(value.lateralArmed), bool(value.optionalSetRelease))
|
||||
if result.validUntilMonoTime < result.observedMonoTime:
|
||||
return None
|
||||
return result
|
||||
except (KeyError, ValueError, OverflowError, RuntimeError, capnp.KjException):
|
||||
return None
|
||||
|
||||
|
||||
@_memoized
|
||||
def decode_safety(raw: bytes) -> SafetyState | None:
|
||||
return _payload(raw, custom.AolAxisState.SafetyWire, SAFETY_KIND, _read_safety,
|
||||
(WIRE_VERSION, SAFETY_INVENTORY_VERSION))
|
||||
value = _payload(raw, custom.AolAxisState.SafetyWire, SAFETY_KIND, (WIRE_VERSION, SAFETY_INVENTORY_VERSION))
|
||||
if value is None:
|
||||
return None
|
||||
try:
|
||||
source_ns = int(value.get('sourcePandaStatesMonoTime', 0))
|
||||
inventory = tuple(PandaSlot(int(slot['slotIndex']), _bounded_id(str(slot.get('hardwareSerial', ''))),
|
||||
int(slot['safetyModel']), int(slot['safetyParam']), int(slot['alternativeExperience']),
|
||||
bool(slot['controlsAllowed']), bool(slot['safetyRxChecksInvalid']),
|
||||
bool(slot['heartbeatLost']), int(slot['faults']))
|
||||
for slot in value.get('pandaInventory', ()))
|
||||
if (int(value['version']) == WIRE_VERSION and (source_ns or inventory) or
|
||||
int(value['version']) == SAFETY_INVENTORY_VERSION and
|
||||
(source_ns <= 0 or not 1 <= len(inventory) <= MAX_PANDA_SLOTS)):
|
||||
return None
|
||||
result = SafetyState(int(value['protocolVersion']), bool(value['compatible']), int(value['observedMonoTime']),
|
||||
int(value['validUntilMonoTime']), int(value['safetyModel']), int(value['safetyParam']),
|
||||
bool(value['lateralAllowed']), bool(value['longitudinalAllowed']), bool(value['requestedLateral']),
|
||||
bool(value['requestedLongitudinal']), _bounded_id(str(value.get('pandaSerial', ''))),
|
||||
_bounded_id(str(value.get('axisSessionId', ''))), source_ns, inventory)
|
||||
if result.validUntilMonoTime < result.observedMonoTime:
|
||||
return None
|
||||
return result
|
||||
except (KeyError, ValueError, OverflowError, RuntimeError, capnp.KjException):
|
||||
return None
|
||||
|
||||
|
||||
@_memoized
|
||||
def decode_intent(raw: bytes) -> IntentState | None:
|
||||
return _payload(raw, custom.AolAxisState.IntentWire, INTENT_KIND, _read_intent)
|
||||
value = _payload(raw, custom.AolAxisState.IntentWire, INTENT_KIND)
|
||||
if value is None:
|
||||
return None
|
||||
try:
|
||||
result = IntentState(_bounded_id(str(value.get('producerSessionId', ''))), int(value['sequence']),
|
||||
int(value['carStateLogMonoTime']), int(value['observedMonoTime']), int(value['validUntilMonoTime']),
|
||||
bool(value['allowedLatch']), bool(value['pauseLateral']), bool(value['pauseLongitudinal']),
|
||||
bool(value['settingsQualified']), bool(value.get('lateralArmed', False)), bool(value.get('optionalSetRelease', False)))
|
||||
if result.validUntilMonoTime < result.observedMonoTime:
|
||||
return None
|
||||
return result
|
||||
except (KeyError, ValueError, OverflowError, RuntimeError, capnp.KjException):
|
||||
return None
|
||||
|
||||
|
||||
def encode_intent(value: IntentState) -> bytes:
|
||||
|
||||
@@ -52,10 +52,6 @@ class GmAolCardIntent(AolCardIntent):
|
||||
self.allowed_latch = False
|
||||
self.startup_calibration_pending = False
|
||||
|
||||
def _arm_from_full_engagement(self, CS, *, source_ns):
|
||||
if not self._main_cycle_required:
|
||||
super()._arm_from_full_engagement(CS, source_ns=source_ns)
|
||||
|
||||
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.
|
||||
|
||||
@@ -5,6 +5,7 @@ from openpilot.starpilot.aol.intent import AolCardIntent, AOL_TOGGLE, ButtonType
|
||||
|
||||
class HondaStockCardIntent(AolCardIntent):
|
||||
observe_stock_engagement = True
|
||||
observe_active_engagement = True
|
||||
|
||||
def __init__(self, cp, settings):
|
||||
super().__init__(settings, explicit_latch=True)
|
||||
@@ -14,6 +15,7 @@ class HondaStockCardIntent(AolCardIntent):
|
||||
self.main_pulse = False
|
||||
self.cancel_held = False
|
||||
self.cancel_ticks = 0
|
||||
self._active_previous = False
|
||||
self.auto_main = settings.lkas_action != AOL_TOGGLE and settings.main_action != AOL_TOGGLE
|
||||
|
||||
def cancel_action(self, action):
|
||||
@@ -24,7 +26,11 @@ class HondaStockCardIntent(AolCardIntent):
|
||||
else:
|
||||
self._perform(action)
|
||||
|
||||
def update(self, CS, **kwargs):
|
||||
def update(self, CS, *, standard_active=False, **kwargs):
|
||||
engagement_started = bool(standard_active and not self._active_previous)
|
||||
self._active_previous = bool(standard_active)
|
||||
now_ns = kwargs.get("now_ns", 0)
|
||||
rejected = self._native_rejection_ns < kwargs.get("native_rejection_ns", 0) <= now_ns
|
||||
state = CS
|
||||
events = []
|
||||
for event in CS.buttonEvents:
|
||||
@@ -77,3 +83,14 @@ class HondaStockCardIntent(AolCardIntent):
|
||||
self.cancel_action(self.settings.cancel_actions[1])
|
||||
elif self.cancel_ticks == CRUISE_LONG_PRESS * 5:
|
||||
self.cancel_action(self.settings.cancel_actions[2])
|
||||
manual = self._main_held or self._lkas_held or self.cancel_held or any(
|
||||
event.type in (ButtonType.mainCruise, ButtonType.lkas, ButtonType.cancel) for event in CS.buttonEvents)
|
||||
driving = state.gearShifter not in (structs.CarState.GearShifter.park, structs.CarState.GearShifter.neutral,
|
||||
structs.CarState.GearShifter.reverse, structs.CarState.GearShifter.unknown)
|
||||
if (engagement_started and self.settings.enabled and self.settings.lkas_action == AOL_TOGGLE and
|
||||
available and healthy and driving and not state.accFaulted and not state.steerFaultPermanent and
|
||||
not self._fault_inhibit and not rejected and not manual):
|
||||
# Match the normal active engagement edge from Dom. Temporary restrictions
|
||||
# and existing pause flags still gate output without erasing this intent.
|
||||
self.allowed_latch = True
|
||||
self._last_latch_edge_ns = now_ns
|
||||
|
||||
@@ -272,6 +272,10 @@ def create_intent(CP, settings):
|
||||
from openpilot.starpilot.car.hyundai.forte_intent import ForteCardIntent
|
||||
|
||||
return ForteCardIntent(CP, settings)
|
||||
if qualified_ioniq6(CP):
|
||||
from openpilot.starpilot.car.hyundai.ioniq6_intent import Ioniq6CardIntent
|
||||
|
||||
return Ioniq6CardIntent(settings)
|
||||
from openpilot.starpilot.aol.intent import AolCardIntent
|
||||
|
||||
return AolCardIntent(settings, explicit_latch=policy_for(CP).explicit_latch)
|
||||
|
||||
@@ -1,4 +1,5 @@
|
||||
from openpilot.starpilot.aol.intent import AolCardIntent
|
||||
from opendbc.car.hyundai.values import HyundaiFlags
|
||||
from openpilot.starpilot.aol.intent import AolCardIntent, AOL_TOGGLE
|
||||
|
||||
|
||||
class ClassicLongCardIntent(AolCardIntent):
|
||||
@@ -6,7 +7,37 @@ class ClassicLongCardIntent(AolCardIntent):
|
||||
super().__init__(settings, explicit_latch=True)
|
||||
self.requires_fault_observation = True
|
||||
self.observe_stock_engagement = True
|
||||
self.has_lkas = bool(cp.flags & HyundaiFlags.HAS_LDA_BUTTON)
|
||||
self._normal_previous = False
|
||||
|
||||
def _arm_from_full_engagement(self, CS, *, source_ns):
|
||||
if not (CS.steerFaultTemporary or CS.gasPressed or CS.brakePressed):
|
||||
super()._arm_from_full_engagement(CS, source_ns=source_ns)
|
||||
def update(self, CS, *, consumed_buttons=frozenset(), fault_active=None, now_ns=0, native_rejection_ns=0, standard_enabled=False):
|
||||
rejected = self._native_rejection_ns < native_rejection_ns <= now_ns
|
||||
super().update(
|
||||
CS,
|
||||
consumed_buttons=consumed_buttons,
|
||||
fault_active=fault_active,
|
||||
now_ns=now_ns,
|
||||
native_rejection_ns=native_rejection_ns,
|
||||
standard_enabled=standard_enabled,
|
||||
)
|
||||
normal = bool(standard_enabled and CS.canValid and not CS.canTimeout and CS.cruiseState.available)
|
||||
deliberate = any(event.pressed for event in CS.buttonEvents)
|
||||
if (
|
||||
normal
|
||||
and not self._normal_previous
|
||||
and self.has_lkas
|
||||
and self.settings.enabled
|
||||
and self.settings.lkas_action == AOL_TOGGLE
|
||||
and not deliberate
|
||||
and not rejected
|
||||
and not fault_active
|
||||
and not CS.steerFaultPermanent
|
||||
and not CS.steerFaultTemporary
|
||||
and not CS.gasPressed
|
||||
and not CS.brakePressed
|
||||
and not CS.accFaulted
|
||||
):
|
||||
self.allowed_latch = True
|
||||
self.pause_lateral = False
|
||||
self._last_latch_edge_ns = now_ns
|
||||
self._normal_previous = normal
|
||||
|
||||
@@ -1,10 +1,36 @@
|
||||
"""Stock SCC engagement intent over the shared physical lateral owner."""
|
||||
from opendbc.car.structs import car
|
||||
from openpilot.starpilot.aol.intent import AOL_TOGGLE
|
||||
from openpilot.starpilot.car.hyundai.forte_intent import ForteCardIntent
|
||||
|
||||
ButtonType = car.CarState.ButtonEvent.Type
|
||||
|
||||
|
||||
class ClassicSccCardIntent(ForteCardIntent):
|
||||
observe_stock_engagement = True
|
||||
|
||||
def _arm_from_full_engagement(self, CS, *, source_ns):
|
||||
if not (CS.steerFaultTemporary or CS.gasPressed or CS.brakePressed):
|
||||
super()._arm_from_full_engagement(CS, source_ns=source_ns)
|
||||
def __init__(self, cp, settings):
|
||||
super().__init__(cp, settings)
|
||||
self._normal_previous = False
|
||||
|
||||
def update(self, CS, *, consumed_buttons=frozenset(), fault_active=None,
|
||||
now_ns=0, native_rejection_ns=0, standard_enabled=False):
|
||||
rejected = self._native_rejection_ns < native_rejection_ns <= now_ns
|
||||
super().update(CS, consumed_buttons=consumed_buttons, fault_active=fault_active,
|
||||
now_ns=now_ns, native_rejection_ns=native_rejection_ns,
|
||||
standard_enabled=standard_enabled)
|
||||
normal = bool(standard_enabled and CS.cruiseState.enabled and CS.canValid and not CS.canTimeout)
|
||||
# A deliberate physical press wins over automatic engagement in this frame.
|
||||
pressed = any(event.pressed and event.type in (ButtonType.lkas, ButtonType.cancel)
|
||||
for event in CS.buttonEvents)
|
||||
if (normal and not self._normal_previous and self.has_lkas and self.settings.enabled and
|
||||
self.settings.lkas_action == AOL_TOGGLE and not pressed and not rejected and
|
||||
not fault_active and not self.fault_rearm and not CS.steerFaultTemporary and
|
||||
not CS.steerFaultPermanent and not CS.gasPressed and not CS.brakePressed):
|
||||
# This is intent only: native SCC/recent-button controls authority and the
|
||||
# current heartbeat/request acknowledgement still gate actual steering.
|
||||
self.physical_latch = True
|
||||
self.allowed_latch = True
|
||||
self.pause_lateral = False
|
||||
self._last_latch_edge_ns = now_ns
|
||||
self._normal_previous = normal
|
||||
|
||||
@@ -6,6 +6,7 @@ from openpilot.starpilot.aol.intent import AolCardIntent, AOL_TOGGLE, ButtonType
|
||||
|
||||
class Ev6CardIntent(AolCardIntent):
|
||||
observe_stock_engagement = True
|
||||
observe_active_engagement = True
|
||||
|
||||
def __init__(self, settings):
|
||||
# Explicit identifies the native-ACK contract, not a universal button mapping.
|
||||
@@ -13,10 +14,10 @@ class Ev6CardIntent(AolCardIntent):
|
||||
self.physical = Ev6PhysicalAuthorization(lkas_on_engage=settings.enabled and settings.lkas_action == AOL_TOGGLE)
|
||||
self._samples = ()
|
||||
self._buttons_current = False
|
||||
self._active_previous = False
|
||||
self._raw_neutral_seen = False
|
||||
self._acc_faulted = False
|
||||
self._controller_override = None
|
||||
self._engagement_manual_withdrawal = False
|
||||
|
||||
def observe_physical_samples(self, state, *, now_ns):
|
||||
stamp = int(state.ev6_aol_sample_stamp_ns)
|
||||
@@ -29,7 +30,6 @@ class Ev6CardIntent(AolCardIntent):
|
||||
if self.allowed_latch:
|
||||
# Withdrawal is always allowed, including the press that removes physical authority.
|
||||
self.allowed_latch = False
|
||||
self._withdraw_full_engagement()
|
||||
elif (self.settings.enabled and self._buttons_current and self.physical.authorized
|
||||
and self._last_latch_edge_ns > 0 and not self._fault_inhibit and not self._acc_faulted):
|
||||
self.allowed_latch = True
|
||||
@@ -51,8 +51,7 @@ class Ev6CardIntent(AolCardIntent):
|
||||
return True
|
||||
|
||||
def update(self, CS, *, consumed_buttons=frozenset(), fault_active=None, now_ns=0, native_rejection_ns=0,
|
||||
standard_enabled=False):
|
||||
self._engagement_manual_withdrawal = False
|
||||
standard_enabled=False, standard_active=False):
|
||||
self.physical.lkas_on_engage = self.settings.enabled and self.settings.lkas_action == AOL_TOGGLE
|
||||
self._acc_faulted = bool(CS.accFaulted)
|
||||
ordinary = CS.as_reader().as_builder()
|
||||
@@ -84,7 +83,6 @@ class Ev6CardIntent(AolCardIntent):
|
||||
self.physical.observe(main, lkas, cruise, main_available=CS.cruiseState.available)
|
||||
main_press = bool(main and not previous_main)
|
||||
lkas_press = bool(lkas and not previous_lkas)
|
||||
self._engagement_manual_withdrawal |= main_press or lkas_press
|
||||
engagement_release = (self.physical.lkas_on_engage and cruise != previous_cruise
|
||||
and previous_cruise in (Buttons.SET_DECEL, Buttons.RES_ACCEL))
|
||||
if main_press or lkas_press or engagement_release:
|
||||
@@ -100,17 +98,16 @@ class Ev6CardIntent(AolCardIntent):
|
||||
self.physical.main_on = False
|
||||
if auto_main:
|
||||
self.allowed_latch = bool(CS.cruiseState.available and self._controller_override is not False)
|
||||
engagement_started = bool(standard_active and not self._active_previous)
|
||||
self._active_previous = bool(standard_active)
|
||||
if (engagement_started and lkas_managed and healthy and not rejected and not self._fault_inhibit
|
||||
and not CS.accFaulted and self.physical.authorized and self._last_latch_edge_ns > 0):
|
||||
self.allowed_latch = True
|
||||
# Retain current fail-closed startup/reset contracts in addition to Dom's gate.
|
||||
if not (healthy and self.physical.authorized and self._last_latch_edge_ns > 0):
|
||||
self.allowed_latch = False
|
||||
self._samples = ()
|
||||
|
||||
def _arm_from_full_engagement(self, CS, *, source_ns):
|
||||
if (self._buttons_current and self.physical.authorized and self._last_latch_edge_ns > 0
|
||||
and self._controller_override is not False and not self._acc_faulted and not self.pause_lateral):
|
||||
# Keep the raw physical edge credit; host engagement cannot manufacture it.
|
||||
self.allowed_latch = True
|
||||
|
||||
def output(self, CS):
|
||||
desired, pause_lateral, pause_longitudinal = super().output(CS)
|
||||
return (desired and self._buttons_current and self.physical.authorized and self._last_latch_edge_ns > 0
|
||||
|
||||
@@ -82,8 +82,3 @@ class ForteCardIntent(AolCardIntent):
|
||||
self.fault_rearm = False
|
||||
allowed = CS.cruiseState.available if main_managed else self.physical_latch
|
||||
self.allowed_latch = bool(settings.enabled and allowed and not invalid and not fatal and not self.fault_rearm)
|
||||
|
||||
def _arm_from_full_engagement(self, CS, *, source_ns):
|
||||
if not self.fault_rearm:
|
||||
self.physical_latch = True
|
||||
super()._arm_from_full_engagement(CS, source_ns=source_ns)
|
||||
|
||||
@@ -0,0 +1,25 @@
|
||||
"""Ioniq 6 AOL desire: a full engagement also arms LKAS-managed AOL."""
|
||||
from openpilot.starpilot.aol.intent import AolCardIntent, AOL_TOGGLE
|
||||
|
||||
|
||||
class Ioniq6CardIntent(AolCardIntent):
|
||||
def __init__(self, settings):
|
||||
super().__init__(settings, explicit_latch=True)
|
||||
self._engaged_previous = False
|
||||
|
||||
def update(self, CS, *, consumed_buttons=frozenset(), fault_active=None, now_ns=0, native_rejection_ns=0,
|
||||
standard_enabled=False):
|
||||
super().update(CS, consumed_buttons=consumed_buttons, fault_active=fault_active, now_ns=now_ns,
|
||||
native_rejection_ns=native_rejection_ns, standard_enabled=standard_enabled)
|
||||
# A stale carControl can drop standard_enabled for a frame while cruise stays
|
||||
# engaged; requiring both to fall keeps that from reading as a new engagement.
|
||||
engaged = bool(standard_enabled or CS.cruiseState.enabled)
|
||||
engagement_started = engaged and not self._engaged_previous
|
||||
self._engaged_previous = engaged
|
||||
if (engagement_started and self.settings.enabled and self.lkas_action == AOL_TOGGLE and
|
||||
CS.canValid and not CS.canTimeout and not CS.accFaulted and not self._fault_inhibit):
|
||||
# Full engagement means LAT and LONG: arm AOL so brake withdraws only LONG,
|
||||
# and LKAS can then drop the remaining LAT.
|
||||
self.allowed_latch = True
|
||||
self.pause_lateral = False
|
||||
self._last_latch_edge_ns = now_ns
|
||||
@@ -0,0 +1,174 @@
|
||||
"""Ioniq 6 AOL: full engagement arms AOL, and brake's LONG withdrawal keeps LAT intent."""
|
||||
from types import SimpleNamespace
|
||||
from typing import Any
|
||||
|
||||
import pytest
|
||||
|
||||
from opendbc.car import Bus, structs
|
||||
from openpilot.cereal import messaging
|
||||
from openpilot.selfdrive.car.card import Car
|
||||
from openpilot.selfdrive.car.tests.test_hyundai_aol import candidate
|
||||
from openpilot.starpilot.aol.intent import AOL_TOGGLE, AolSettings
|
||||
from openpilot.starpilot.aol.transport_pause import TransportPauseFeedback, _healthy_transport_native, independent_lateral_health
|
||||
from openpilot.starpilot.aol.wire import PandaSlot, SafetyState, encode_safety
|
||||
from openpilot.starpilot.car.hyundai.aol import create_intent
|
||||
from openpilot.starpilot.car.hyundai.ioniq6_intent import Ioniq6CardIntent
|
||||
|
||||
ButtonType = structs.CarState.ButtonEvent.Type
|
||||
NOW = 10_000_000_000
|
||||
|
||||
|
||||
def ioniq6(alt=False):
|
||||
_, cp = candidate(alt)
|
||||
cp.safetyConfigs[0].safetyParam = 0x8895 if alt else 0x8815
|
||||
return cp
|
||||
|
||||
|
||||
def settings(lkas_action=AOL_TOGGLE, enabled=True):
|
||||
return AolSettings(enabled, 0.0, lkas_action, 0, (0, 0, 0), (0, 0, 0))
|
||||
|
||||
|
||||
class Drive:
|
||||
def __init__(self, intent):
|
||||
self.intent = intent
|
||||
self.cs = structs.CarState(canValid=True, gearShifter=structs.CarState.GearShifter.drive, vEgo=20.0)
|
||||
self.now = NOW
|
||||
self.step()
|
||||
|
||||
def step(self, *, engaged=None, cruise=None, buttons=(), standard=None):
|
||||
if cruise is not None:
|
||||
self.cs.cruiseState.enabled = cruise
|
||||
self.cs.buttonEvents = [structs.CarState.ButtonEvent(type=b, pressed=p) for b, p in buttons]
|
||||
self.now += 10_000_000
|
||||
enabled = engaged if standard is None else standard
|
||||
self.intent.update(self.cs, now_ns=self.now, standard_enabled=bool(enabled))
|
||||
return self.intent.output(self.cs)[:2]
|
||||
|
||||
def engage(self):
|
||||
return self.step(engaged=True, cruise=True)
|
||||
|
||||
def brake(self):
|
||||
return self.step(engaged=False, cruise=False)
|
||||
|
||||
def lkas(self, engaged):
|
||||
self.step(engaged=engaged, buttons=((ButtonType.lkas, True),))
|
||||
return self.step(engaged=engaged, buttons=((ButtonType.lkas, False),))
|
||||
|
||||
|
||||
@pytest.mark.parametrize('alt', [False, True])
|
||||
def test_qualified_ioniq6_uses_engagement_intent(alt):
|
||||
assert isinstance(create_intent(ioniq6(alt), settings()), Ioniq6CardIntent)
|
||||
|
||||
|
||||
def test_full_engagement_arms_aol_so_brake_only_drops_long():
|
||||
drive = Drive(create_intent(ioniq6(), settings()))
|
||||
assert drive.step(engaged=False) == (False, False)
|
||||
assert drive.engage() == (True, False)
|
||||
assert drive.brake() == (True, False)
|
||||
# LKAS then drops the remaining AOL lateral.
|
||||
assert drive.lkas(engaged=False) == (False, True)
|
||||
# A fresh SET engagement is LAT and LONG again.
|
||||
assert drive.engage() == (True, False)
|
||||
|
||||
|
||||
def test_lkas_in_full_op_pauses_lateral_and_stale_control_does_not_rearm():
|
||||
drive = Drive(create_intent(ioniq6(), settings()))
|
||||
drive.engage()
|
||||
assert drive.lkas(engaged=True) == (False, True)
|
||||
# carControl briefly stale while cruise stays engaged is not a new engagement.
|
||||
assert drive.step(standard=False) == (False, True)
|
||||
assert drive.step(standard=True) == (False, True)
|
||||
# Brake keeps the driver's lateral pause; LKAS restores AOL.
|
||||
assert drive.brake() == (False, True)
|
||||
assert drive.lkas(engaged=False) == (True, False)
|
||||
|
||||
|
||||
@pytest.mark.parametrize('config', ['lkas_not_aol', 'aol_disabled', 'acc_fault', 'can_invalid'])
|
||||
def test_engagement_does_not_arm_without_lkas_aol(config):
|
||||
intent = create_intent(ioniq6(), settings(lkas_action=0 if config == 'lkas_not_aol' else AOL_TOGGLE,
|
||||
enabled=config != 'aol_disabled'))
|
||||
drive = Drive(intent)
|
||||
drive.cs.accFaulted = config == 'acc_fault'
|
||||
drive.cs.canValid = config != 'can_invalid'
|
||||
drive.engage()
|
||||
assert not intent.allowed_latch
|
||||
|
||||
|
||||
class Peers:
|
||||
def __init__(self, messages):
|
||||
self.messages = {m.which(): m for m in messages}
|
||||
self.logMonoTime = {n: int(m.logMonoTime) for n, m in self.messages.items()}
|
||||
self.seen = self.valid = self.alive = self.updated = dict.fromkeys(self.messages, True)
|
||||
|
||||
def __getitem__(self, name):
|
||||
return getattr(self.messages[name], name)
|
||||
|
||||
|
||||
def transport_fixture(*, long_requested=True):
|
||||
cp = ioniq6(alt=True)
|
||||
config = cp.safetyConfigs[0]
|
||||
slots = (PandaSlot(0, 'unit-ioniq', config.safetyModel.raw, config.safetyParam, cp.alternativeExperience, False),)
|
||||
# Brake revoked LONG at the Panda; the host has not yet withdrawn its LONG request.
|
||||
native = SafetyState(1, True, NOW - 20_000_000, NOW + 180_000_000, config.safetyModel.raw, config.safetyParam,
|
||||
True, False, True, long_requested, 'unit-ioniq', 'unit-session',
|
||||
sourcePandaStatesMonoTime=NOW - 21_000_000, pandaInventory=slots)
|
||||
safety = messaging.new_message('aolSafetyWire', 0, valid=True, logMonoTime=native.observedMonoTime)
|
||||
safety.aolSafetyWire = encode_safety(native)
|
||||
pandas = messaging.new_message('pandaStates', 1, valid=True, logMonoTime=NOW)
|
||||
p = pandas.pandaStates[0]
|
||||
p.safetyModel, p.safetyParam, p.alternativeExperience = config.safetyModel, config.safetyParam, cp.alternativeExperience
|
||||
axis = messaging.new_message('aolAxisState', valid=True, logMonoTime=NOW - 1_000_000)
|
||||
a = axis.aolAxisState
|
||||
a.sessionId = a.faultSessionId = 'unit-session'
|
||||
a.qualified, a.sequence, a.faultReason = True, 2, 'none'
|
||||
a.observedMonoTime, a.validUntilMonoTime = axis.logMonoTime, axis.logMonoTime + 30_000_000
|
||||
a.sourceCarStateMonoTime = axis.logMonoTime - 1000
|
||||
a.desiredLateral = a.lateralActive = a.nativeAcknowledged = True
|
||||
events = messaging.new_message('onroadEvents', 1, valid=True, logMonoTime=NOW - 1_000_000)
|
||||
events.onroadEvents[0].name = 'pedalPressed'
|
||||
events.onroadEvents[0].userDisable = True
|
||||
sm = Peers([safety.as_reader(), pandas.as_reader(), axis.as_reader(), events.as_reader()])
|
||||
cs = structs.CarState(canValid=True, brakePressed=True, vEgo=13.5, gearShifter=structs.CarState.GearShifter.drive)
|
||||
feedback = TransportPauseFeedback(cp)
|
||||
# An already observed healthy, acknowledged LAT+LONG baseline one frame earlier.
|
||||
feedback.session = native.axisSessionId
|
||||
feedback.healthy_feedback_ns = NOW - 10_000_000
|
||||
feedback.healthy_native_ns = native.observedMonoTime
|
||||
feedback.healthy_inventory = (native.pandaSerial, tuple(s.hardwareSerial for s in slots))
|
||||
return cp, cs, sm, native, feedback
|
||||
|
||||
|
||||
def test_brake_long_revoke_window_retains_lateral_intent():
|
||||
cp, cs, sm, native, feedback = transport_fixture()
|
||||
# Recorded 2026-10-14 drive, 586.87 s: this window used to hard-reset the AOL latch.
|
||||
assert _healthy_transport_native(sm, cp, cs, 'unit-session', NOW) is None
|
||||
assert independent_lateral_health(sm, cp, cs, 'unit-session', NOW) == native
|
||||
assert not feedback.observe(sm, cp, cs, NOW, latched=True, brake_source_ns=NOW - 1000)
|
||||
assert feedback.honda_brake_ns == NOW - 1000
|
||||
|
||||
|
||||
@pytest.mark.parametrize('mutation', ['no_source', 'no_brake', 'stale_source', 'no_baseline'])
|
||||
def test_brake_window_without_its_conditions_still_fails_closed(mutation):
|
||||
cp, cs, sm, _, feedback = transport_fixture()
|
||||
source = {'no_source': 0, 'stale_source': NOW - 30_000_001}.get(mutation, NOW - 1000)
|
||||
cs.brakePressed = mutation != 'no_brake'
|
||||
if mutation == 'no_baseline':
|
||||
feedback.healthy_feedback_ns = 0
|
||||
assert feedback.observe(sm, cp, cs, NOW, latched=True, brake_source_ns=source)
|
||||
assert feedback.honda_brake_ns == 0
|
||||
|
||||
|
||||
def test_card_ioniq6_brake_source_reads_tcs():
|
||||
cp = ioniq6()
|
||||
cs = structs.CarState(canValid=True, brakePressed=True)
|
||||
card: Any = Car.__new__(Car)
|
||||
card.CP = cp
|
||||
parser = SimpleNamespace(vl={'TCS': {'DriverBraking': 1}}, ts_nanos={'TCS': {'DriverBraking': NOW + 9_000_000_000}})
|
||||
card.CI = SimpleNamespace(can_parsers={Bus.pt: parser})
|
||||
card._aol_can_observation = (cs, NOW - 1000, NOW + 9_000_000_000, NOW + 9_000_001_000)
|
||||
assert card.aol_honda_brake_source(cs, NOW) == NOW - 1000
|
||||
parser.ts_nanos['TCS']['DriverBraking'] -= 30_000_001
|
||||
assert card.aol_honda_brake_source(cs, NOW) == 0
|
||||
parser.ts_nanos['TCS']['DriverBraking'] += 30_000_001
|
||||
parser.vl['TCS']['DriverBraking'] = 0
|
||||
assert card.aol_honda_brake_source(cs, NOW) == 0
|
||||
@@ -53,12 +53,6 @@ class TeslaScreenCardIntent(AolCardIntent):
|
||||
not self.brake_blocked and not fault and not cancel and self.settings.enabled)
|
||||
self.previous_enabled, self.previous_available = engaged, available
|
||||
|
||||
def _arm_from_full_engagement(self, CS, *, source_ns):
|
||||
if not (self.brake_blocked or self.brake_disengage and CS.brakePressed or
|
||||
CS.steeringDisengage or CS.doorOpen or CS.gearShifter != structs.CarState.GearShifter.drive):
|
||||
self.screen_override = True
|
||||
super()._arm_from_full_engagement(CS, source_ns=source_ns)
|
||||
|
||||
@property
|
||||
def settings(self):
|
||||
return self._settings
|
||||
|
||||
@@ -21,10 +21,6 @@ class TeslaPreapCardIntent(AolCardIntent):
|
||||
self.pull_held = False
|
||||
self.pull_neutral = False
|
||||
|
||||
def _arm_from_full_engagement(self, CS, *, source_ns):
|
||||
# Set already arms through the physical stalk owner, including neutral credit.
|
||||
pass
|
||||
|
||||
def update(self, CS, **kwargs):
|
||||
state = structs.CarState(**CS.to_dict())
|
||||
pull = False
|
||||
|
||||
@@ -51,7 +51,7 @@ def conditional_manual_candidate(CS: car.CarState, tracker: ButtonTracker, param
|
||||
if choice not in (ModeChoice.CEM, ModeChoice.CCM) or snapshot.preferences is None:
|
||||
tracker.invalidate_media()
|
||||
return None
|
||||
fingerprint = settings_fingerprint(snapshot, owner=settings_owner)
|
||||
fingerprint = settings_fingerprint(snapshot)
|
||||
if fingerprint is None:
|
||||
tracker.invalidate_media()
|
||||
return None
|
||||
@@ -113,7 +113,7 @@ def conditional_traffic_candidate(CS: car.CarState, tracker: ButtonTracker, para
|
||||
return None
|
||||
snapshot = settings_owner.refresh(now_ns)
|
||||
verdict = settings_owner.verdict(snapshot, now_mono_ns=now_ns, drive_id=drive_id)
|
||||
fingerprint = settings_fingerprint(snapshot, owner=settings_owner) if verdict.status == 'ready' and verdict.safe_mode is False else None
|
||||
fingerprint = settings_fingerprint(snapshot) if verdict.status == 'ready' and verdict.safe_mode is False else None
|
||||
if fingerprint is None:
|
||||
tracker.invalidate_media()
|
||||
return None
|
||||
|
||||
@@ -9,9 +9,8 @@ Replay continues to use the synchronous source-clock owner.
|
||||
from dataclasses import dataclass
|
||||
from typing import Any
|
||||
|
||||
from openpilot.starpilot.conditional_mode.preferences import CCMOptions, CEMOptions, ModeSelection, SavedPreferences
|
||||
from openpilot.starpilot.conditional_mode.runtime_settings import (
|
||||
REVERIFY_NS, ConditionalSettingsOwner, DocumentState, SafeModeState, SettingsSnapshot,
|
||||
REVERIFY_NS, ConditionalSettingsOwner, SettingsSnapshot,
|
||||
)
|
||||
|
||||
|
||||
@@ -29,14 +28,10 @@ class CardSettingsView(ConditionalSettingsOwner):
|
||||
super().__init__(None)
|
||||
self.owner_token = owner_token
|
||||
self.unavailable = self._unavailable(0)
|
||||
self._selection_cache = None
|
||||
self._fingerprint_cache = None
|
||||
|
||||
def adopt(self, observation: CardSettingsObservation) -> None:
|
||||
# Latch the whole immutable source once per frame. The worker never writes
|
||||
# this view; all consumers within that frame see the same revision.
|
||||
self._selection_cache = None
|
||||
self._fingerprint_cache = None
|
||||
snapshot = observation.snapshot
|
||||
self.current = (snapshot if observation.status == 'verified' and
|
||||
type(snapshot) is SettingsSnapshot and snapshot.owner_token == self.owner_token
|
||||
@@ -52,58 +47,6 @@ class CardSettingsView(ConditionalSettingsOwner):
|
||||
0 <= now_mono_ns - current.verified_mono_ns < REVERIFY_NS and
|
||||
super().affirm(snapshot, now_mono_ns=now_mono_ns))
|
||||
|
||||
@staticmethod
|
||||
def _preference_fields(snapshot: SettingsSnapshot):
|
||||
try:
|
||||
preferences = snapshot.preferences
|
||||
if (type(preferences) is not SavedPreferences or type(preferences.cem) is not CEMOptions or
|
||||
type(preferences.ccm) is not CCMOptions):
|
||||
return None
|
||||
cem, ccm = preferences.cem, preferences.ccm
|
||||
# Each successful derivation validated exact immutable scalars. Identity
|
||||
# checks also notice deliberate object.__setattr__ fixture mutations.
|
||||
return (preferences, cem, ccm, preferences.version, preferences.mode,
|
||||
cem.speed_mps, cem.speed_with_lead_mps, cem.signal_speed_mps,
|
||||
cem.open_road, cem.curves, cem.curves_with_lead, cem.lead, cem.slower_lead,
|
||||
cem.stopped_lead, cem.stop_lights, cem.model_stop_s, cem.signal_lane_detection,
|
||||
cem.signal_lane_width_m, cem.persist_manual, ccm.speed_mps, ccm.speed_with_lead_mps,
|
||||
ccm.set_speed_margin_mps, ccm.lead, ccm.launch_assist, ccm.persist_manual)
|
||||
except AttributeError:
|
||||
return None
|
||||
|
||||
def _selection_for_drive(self, snapshot: SettingsSnapshot, drive_id: int) -> ModeSelection | None:
|
||||
# verdict still checks the current owner, revision, SafeMode and clock on
|
||||
# every call. Only its pure post-affirmation derivation may be reused.
|
||||
if (type(self) is not CardSettingsView or type(snapshot) is not SettingsSnapshot or
|
||||
snapshot is not self.current or type(drive_id) is not int or drive_id <= 0):
|
||||
return super()._selection_for_drive(snapshot, drive_id)
|
||||
key = self._preference_fields(snapshot)
|
||||
if key is None:
|
||||
return super()._selection_for_drive(snapshot, drive_id)
|
||||
cached = self._selection_cache
|
||||
if cached is not None and cached[0] == drive_id and all(a is b for a, b in zip(key, cached[1], strict=True)):
|
||||
return cached[2]
|
||||
selection = super()._selection_for_drive(snapshot, drive_id)
|
||||
self._selection_cache = (drive_id, key, selection)
|
||||
return selection
|
||||
|
||||
def fingerprint(self, snapshot: SettingsSnapshot | None) -> str | None:
|
||||
from openpilot.starpilot.conditional_mode.status import _settings_fingerprint
|
||||
|
||||
if (type(self) is not CardSettingsView or type(snapshot) is not SettingsSnapshot or
|
||||
snapshot is not self.current or type(snapshot.document_state) is not DocumentState or
|
||||
type(snapshot.safe_mode_state) is not SafeModeState or
|
||||
snapshot.document_raw is not None and type(snapshot.document_raw) is not bytes or
|
||||
snapshot.safe_mode_raw is not None and type(snapshot.safe_mode_raw) is not bytes):
|
||||
return _settings_fingerprint(snapshot)
|
||||
key = (snapshot.document_state, snapshot.safe_mode_state, snapshot.document_raw, snapshot.safe_mode_raw)
|
||||
cached = self._fingerprint_cache
|
||||
if cached is not None and all(a is b for a, b in zip(key, cached[0], strict=True)):
|
||||
return cached[1]
|
||||
fingerprint = _settings_fingerprint(snapshot)
|
||||
self._fingerprint_cache = (key, fingerprint)
|
||||
return fingerprint
|
||||
|
||||
|
||||
class CardSettingsWorker:
|
||||
"""One worker-owned reader and an atomically replaced immutable publication."""
|
||||
|
||||
@@ -198,7 +198,7 @@ class ConditionalSettingsOwner:
|
||||
current.document_state in (DocumentState.ABSENT, DocumentState.VALID) and
|
||||
current.safe_mode_state in (SafeModeState.ABSENT_FALSE, SafeModeState.FALSE) and
|
||||
snapshot.verified_mono_ns <= current.verified_mono_ns and
|
||||
(snapshot is current or replace(snapshot, verified_mono_ns=current.verified_mono_ns) == current) and
|
||||
replace(snapshot, verified_mono_ns=current.verified_mono_ns) == current and
|
||||
type(now_mono_ns) is int and verified <= now_mono_ns <= verified + REFRESH_NS
|
||||
)
|
||||
|
||||
@@ -218,11 +218,7 @@ class ConditionalSettingsOwner:
|
||||
current.observed_mono_ns if current is not None else None,
|
||||
None, None, None, safe)
|
||||
assert snapshot is not None and snapshot.preferences is not None
|
||||
selection = self._selection_for_drive(snapshot, drive_id)
|
||||
selection = saved_selection_for_drive(snapshot.preferences, drive_id)
|
||||
return SettingsVerdict('ready' if selection is not None else 'invalid_drive', snapshot.revision,
|
||||
snapshot.observed_mono_ns, selection, snapshot.preferences.cem,
|
||||
snapshot.preferences.ccm, False)
|
||||
|
||||
def _selection_for_drive(self, snapshot: SettingsSnapshot, drive_id: int) -> ModeSelection | None:
|
||||
assert snapshot.preferences is not None
|
||||
return saved_selection_for_drive(snapshot.preferences, drive_id)
|
||||
|
||||
@@ -124,23 +124,6 @@ def model_widths(model: object) -> tuple[float, float] | None:
|
||||
return (left, right) if left is not None and right is not None else None
|
||||
|
||||
|
||||
def _model_widths_valid(model: object) -> bool:
|
||||
from capnp.lib.capnp import _DynamicStructReader
|
||||
from openpilot.cereal import log
|
||||
|
||||
if type(model) is not _DynamicStructReader or model.schema != log.ModelDataV2.schema:
|
||||
return model_widths(model) is not None
|
||||
try:
|
||||
lines, edges = model.laneLines, model.roadEdges
|
||||
if len(lines) != 4 or len(edges) != 2:
|
||||
return False
|
||||
geometry = (_line(lines[0]), _line(lines[1]), _line(edges[0]),
|
||||
_line(lines[3]), _line(lines[2]), _line(edges[1]))
|
||||
except (AttributeError, TypeError, ValueError, OverflowError):
|
||||
return False
|
||||
return all(line is not None for line in geometry)
|
||||
|
||||
|
||||
class SignalLaneTracker:
|
||||
"""One drive-session owner; call once per newly received model update."""
|
||||
|
||||
@@ -167,13 +150,13 @@ class SignalLaneTracker:
|
||||
self._count = 0
|
||||
self._widths = (0.0, 0.0)
|
||||
elif repeated:
|
||||
if not _model_widths_valid(frame.model):
|
||||
if model_widths(frame.model) is None:
|
||||
self._widths = None
|
||||
elif not repeated:
|
||||
self._count += 1
|
||||
if self._count % 4 == 0:
|
||||
self._widths = model_widths(frame.model)
|
||||
elif not _model_widths_valid(frame.model):
|
||||
elif model_widths(frame.model) is None:
|
||||
# A malformed intervening model frame cannot extend prior evidence.
|
||||
self._widths = None
|
||||
if not frame.left_blinker and not frame.right_blinker:
|
||||
|
||||
@@ -22,16 +22,7 @@ CHOICES = {ModeChoice.STOCK: 'stock', ModeChoice.CEM: 'conditionalExperimental',
|
||||
_HEX = re.compile(r'[0-9a-f]{64}\Z')
|
||||
|
||||
|
||||
def settings_fingerprint(snapshot: SettingsSnapshot | None, *, owner=None) -> str | None:
|
||||
if owner is not None:
|
||||
from openpilot.starpilot.conditional_mode.card_settings import CardSettingsView
|
||||
|
||||
if type(owner) is CardSettingsView:
|
||||
return owner.fingerprint(snapshot)
|
||||
return _settings_fingerprint(snapshot)
|
||||
|
||||
|
||||
def _settings_fingerprint(snapshot: SettingsSnapshot | None) -> str | None:
|
||||
def settings_fingerprint(snapshot: SettingsSnapshot | None) -> str | None:
|
||||
"""Bind two independent readers to the exact saved document and SafeMode."""
|
||||
if (
|
||||
snapshot is None
|
||||
|
||||
@@ -1,231 +0,0 @@
|
||||
"""Frame-local pure derivations; live settings/authority checks still run."""
|
||||
from dataclasses import fields, replace
|
||||
from pathlib import Path
|
||||
from types import SimpleNamespace
|
||||
from unittest.mock import patch
|
||||
|
||||
import pytest
|
||||
|
||||
from openpilot.common.params import Params
|
||||
from openpilot.starpilot.conditional_mode import runtime_settings, status
|
||||
from openpilot.starpilot.conditional_mode.card_settings import CardSettingsObservation, CardSettingsView, CardSettingsWorker
|
||||
from openpilot.starpilot.conditional_mode.policy import ModeChoice
|
||||
from openpilot.starpilot.conditional_mode.preferences import CCMOptions, CEMOptions, SavedPreferences
|
||||
from openpilot.starpilot.conditional_mode.runtime_settings import ConditionalSettingsOwner, DocumentState, REVERIFY_NS, SafeModeState
|
||||
|
||||
NOW = 10_000_000_000
|
||||
|
||||
|
||||
@pytest.fixture
|
||||
def worker(tmp_path):
|
||||
result = CardSettingsWorker(Params(str(tmp_path)))
|
||||
result.refresh(NOW)
|
||||
result.adopt()
|
||||
return result
|
||||
|
||||
|
||||
class UncachedCardSettingsView(CardSettingsView):
|
||||
# The live Card's 500ms source-age bound must remain in the reference too;
|
||||
# a generic owner has a different one-second bound. Only derivation is uncached.
|
||||
_selection_for_drive = ConditionalSettingsOwner._selection_for_drive
|
||||
|
||||
|
||||
def reference(view):
|
||||
owner = UncachedCardSettingsView(view.owner_token)
|
||||
owner.current = view.current
|
||||
return owner
|
||||
|
||||
|
||||
def outcome(owner, snapshot, now=NOW, drive=1):
|
||||
try:
|
||||
return owner.verdict(snapshot, now_mono_ns=now, drive_id=drive)
|
||||
except Exception as error:
|
||||
return type(error), str(error)
|
||||
|
||||
|
||||
def test_reuses_only_pure_derivations_and_affirms_every_call(worker):
|
||||
view, snapshot = worker.view, worker.view.current
|
||||
expected = reference(view).verdict(snapshot, now_mono_ns=NOW, drive_id=1)
|
||||
with patch.object(runtime_settings, 'saved_selection_for_drive', wraps=runtime_settings.saved_selection_for_drive) as selection, \
|
||||
patch.object(status, '_settings_fingerprint', wraps=status._settings_fingerprint) as fingerprint, \
|
||||
patch.object(view, 'affirm', wraps=view.affirm) as affirm, \
|
||||
patch.object(runtime_settings, 'read_mode_sources', side_effect=AssertionError('main-thread I/O')):
|
||||
for _ in range(4):
|
||||
assert view.refresh(NOW) is snapshot
|
||||
assert view.verdict(snapshot, now_mono_ns=NOW, drive_id=1) == expected
|
||||
assert status.settings_fingerprint(snapshot, owner=view) == status.settings_fingerprint(snapshot)
|
||||
assert selection.call_count == 1 and affirm.call_count == 4
|
||||
assert fingerprint.call_count == 5 # One cached miss plus four uncached references.
|
||||
|
||||
|
||||
@pytest.mark.parametrize('now', [0, NOW - 1, NOW + REVERIFY_NS - 1, NOW + REVERIFY_NS, NOW + 1_000_000_001, True, 1.0])
|
||||
def test_warm_cache_does_not_bypass_clock_and_stale_gates(worker, now):
|
||||
view, snapshot = worker.view, worker.view.current
|
||||
view.verdict(snapshot, now_mono_ns=NOW, drive_id=1)
|
||||
assert outcome(view, snapshot, now) == outcome(reference(view), snapshot, now)
|
||||
|
||||
|
||||
@pytest.mark.parametrize('drive', [2, 0, -1, True, 1.0, '1', None])
|
||||
def test_warm_cache_does_not_reuse_another_drive_or_accept_invalid_drive(worker, drive):
|
||||
view, snapshot = worker.view, worker.view.current
|
||||
view.verdict(snapshot, now_mono_ns=NOW, drive_id=1)
|
||||
assert outcome(view, snapshot, drive=drive) == outcome(reference(view), snapshot, drive=drive)
|
||||
|
||||
|
||||
def test_adoption_clears_both_caches_even_for_the_same_snapshot(worker):
|
||||
view, snapshot = worker.view, worker.view.current
|
||||
with patch.object(runtime_settings, 'saved_selection_for_drive', wraps=runtime_settings.saved_selection_for_drive) as selection, \
|
||||
patch.object(status, '_settings_fingerprint', wraps=status._settings_fingerprint) as fingerprint:
|
||||
for _ in range(2):
|
||||
worker.adopt()
|
||||
view.verdict(snapshot, now_mono_ns=NOW, drive_id=1)
|
||||
status.settings_fingerprint(snapshot, owner=view)
|
||||
assert selection.call_count == fingerprint.call_count == 2
|
||||
|
||||
|
||||
PREFERENCE_FIELDS = [('preferences', field.name) for field in fields(SavedPreferences)] + \
|
||||
[('cem', field.name) for field in fields(CEMOptions)] + [('ccm', field.name) for field in fields(CCMOptions)]
|
||||
|
||||
|
||||
@pytest.mark.parametrize('section,name', PREFERENCE_FIELDS)
|
||||
def test_frozen_input_fixture_mutation_cannot_hide_behind_identity(worker, section, name):
|
||||
view, snapshot = worker.view, worker.view.current
|
||||
view.verdict(snapshot, now_mono_ns=NOW, drive_id=1)
|
||||
preferences = snapshot.preferences
|
||||
target = preferences if section == 'preferences' else getattr(preferences, section)
|
||||
previous = getattr(target, name)
|
||||
if type(previous) is bool:
|
||||
changed = not previous
|
||||
elif type(previous) in (int, float):
|
||||
changed = previous + 0.125
|
||||
elif type(previous) is ModeChoice:
|
||||
changed = ModeChoice.CCM if previous is not ModeChoice.CCM else ModeChoice.CEM
|
||||
elif type(previous) is CEMOptions:
|
||||
changed = replace(previous, curves=not previous.curves)
|
||||
elif type(previous) is CCMOptions:
|
||||
changed = replace(previous, lead=not previous.lead)
|
||||
else:
|
||||
raise AssertionError(type(previous))
|
||||
try:
|
||||
object.__setattr__(target, name, changed)
|
||||
assert outcome(view, snapshot) == outcome(reference(view), snapshot)
|
||||
finally:
|
||||
object.__setattr__(target, name, previous)
|
||||
|
||||
|
||||
@pytest.mark.parametrize('preferences', [SimpleNamespace(), replace(SavedPreferences(), cem=SimpleNamespace()),
|
||||
replace(SavedPreferences(), ccm=SimpleNamespace()),
|
||||
replace(SavedPreferences(), version='invalid')])
|
||||
def test_custom_or_malformed_preferences_follow_original_failure(worker, preferences):
|
||||
view, snapshot = worker.view, worker.view.current
|
||||
view.verdict(snapshot, now_mono_ns=NOW, drive_id=1)
|
||||
changed = replace(snapshot, preferences=preferences)
|
||||
view.adopt(CardSettingsObservation(changed, 'verified', NOW))
|
||||
assert outcome(view, changed) == outcome(reference(view), changed)
|
||||
|
||||
|
||||
@pytest.mark.parametrize('field,value', [('document_raw', b'{}'), ('safe_mode_raw', b'0'),
|
||||
('document_state', DocumentState.INVALID), ('safe_mode_state', SafeModeState.TRUE),
|
||||
('safe_mode_raw', bytearray(b'0'))])
|
||||
def test_fingerprint_reads_current_values_and_preserves_fallback(worker, field, value):
|
||||
view, snapshot = worker.view, worker.view.current
|
||||
status.settings_fingerprint(snapshot, owner=view)
|
||||
previous = getattr(snapshot, field)
|
||||
try:
|
||||
object.__setattr__(snapshot, field, value)
|
||||
assert status.settings_fingerprint(snapshot, owner=view) == status.settings_fingerprint(snapshot)
|
||||
finally:
|
||||
object.__setattr__(snapshot, field, previous)
|
||||
|
||||
|
||||
def test_foreign_snapshots_and_custom_view_subclasses_do_not_use_cache(worker):
|
||||
view, snapshot = worker.view, worker.view.current
|
||||
view.verdict(snapshot, now_mono_ns=NOW, drive_id=1)
|
||||
foreign = replace(snapshot, owner_token='foreign')
|
||||
assert outcome(view, foreign) == outcome(reference(view), foreign)
|
||||
|
||||
class CustomView(CardSettingsView):
|
||||
pass
|
||||
|
||||
custom = CustomView(view.owner_token)
|
||||
custom.adopt(worker.published)
|
||||
generic = reference(custom)
|
||||
with patch.object(runtime_settings, 'saved_selection_for_drive', wraps=runtime_settings.saved_selection_for_drive) as selection:
|
||||
for _ in range(2):
|
||||
assert custom.verdict(snapshot, now_mono_ns=NOW, drive_id=1) == generic.verdict(snapshot, now_mono_ns=NOW, drive_id=1)
|
||||
assert selection.call_count == 4
|
||||
assert custom._selection_cache is None and custom._fingerprint_cache is None
|
||||
assert status.settings_fingerprint(snapshot, owner=object()) == status.settings_fingerprint(snapshot)
|
||||
|
||||
|
||||
def test_deleted_nested_fixture_field_preserves_original_error_order(worker):
|
||||
view, snapshot = worker.view, worker.view.current
|
||||
view.verdict(snapshot, now_mono_ns=NOW, drive_id=1)
|
||||
preferences = snapshot.preferences
|
||||
previous = preferences.cem
|
||||
try:
|
||||
object.__setattr__(preferences, 'version', 'invalid')
|
||||
object.__delattr__(preferences, 'cem')
|
||||
assert outcome(view, snapshot) == outcome(reference(view), snapshot)
|
||||
finally:
|
||||
object.__setattr__(preferences, 'version', 1)
|
||||
object.__setattr__(preferences, 'cem', previous)
|
||||
|
||||
|
||||
@pytest.mark.parametrize('field,value', [('safe_mode_state', SafeModeState.TRUE),
|
||||
('safe_mode_state', SafeModeState.INVALID),
|
||||
('document_state', DocumentState.READ_ERROR),
|
||||
('owner_token', 'foreign')])
|
||||
def test_current_source_gate_changes_after_warm_cache_still_deny(worker, field, value):
|
||||
view, snapshot = worker.view, worker.view.current
|
||||
view.verdict(snapshot, now_mono_ns=NOW, drive_id=1)
|
||||
changed = replace(snapshot, **{field: value})
|
||||
# Deliberately replace the live source without adopt to verify the cache is
|
||||
# subordinate to current owner/source gates, not just adoption bookkeeping.
|
||||
view.current = changed
|
||||
assert outcome(view, changed) == outcome(reference(view), changed)
|
||||
assert outcome(view, snapshot) == outcome(reference(view), snapshot)
|
||||
|
||||
|
||||
@pytest.mark.parametrize('full_op', [False, True])
|
||||
def test_actual_card_adoption_and_multiple_consumers_keep_live_authority(full_op):
|
||||
from openpilot.common.prefix import OpenpilotPrefix
|
||||
from openpilot.selfdrive.car import card as module
|
||||
from openpilot.starpilot.aol.tests.test_ioniq_availability import IoniqAvailabilityTests
|
||||
from openpilot.starpilot.conditional_mode.manual import ButtonTracker, IoniqMediaMapCache
|
||||
|
||||
fixture = IoniqAvailabilityTests()
|
||||
try:
|
||||
with OpenpilotPrefix():
|
||||
sd, _, drive, card, receive = fixture._ioniq_availability_callers(full_op=full_op)
|
||||
card.conditional_replay = True
|
||||
card.manual_button_tracker = ButtonTracker()
|
||||
card.traffic_button_tracker = ButtonTracker()
|
||||
card.ioniq_media_map = IoniqMediaMapCache()
|
||||
card.manual_settings_owner = ConditionalSettingsOwner(card.params)
|
||||
with patch.object(module, 'REPLAY', False):
|
||||
card.configure_conditional_settings_worker()
|
||||
worker = card.conditional_settings_worker
|
||||
for tick in (2, 3):
|
||||
now = NOW + tick * 10_000_000
|
||||
worker.refresh(now)
|
||||
axis, _, command = drive(tick)
|
||||
with patch.object(runtime_settings, 'saved_selection_for_drive', wraps=runtime_settings.saved_selection_for_drive) as selection, \
|
||||
patch.object(worker.view, 'affirm', wraps=worker.view.affirm) as affirm:
|
||||
assert receive(now, axis=sd.pm.messages['aolAxisState'], event=sd.pm.messages['onroadEvents'])
|
||||
assert selection.call_count == 1
|
||||
assert affirm.call_count >= (2 if full_op else 1)
|
||||
assert axis.lateralActive and command.latActive
|
||||
assert bool(axis.longitudinalActive) is full_op and bool(command.longActive) is full_op
|
||||
# A fresh restrictive worker publication must revoke optional proposals.
|
||||
Path(card.params.get_param_path('ConditionalModeConfig')).write_bytes(b'{')
|
||||
now = NOW + 40_000_000
|
||||
worker.refresh(now)
|
||||
axis, _, command = drive(4)
|
||||
assert receive(now, axis=sd.pm.messages['aolAxisState'], event=sd.pm.messages['onroadEvents'])
|
||||
assert worker.view.verdict(worker.view.current, now_mono_ns=now, drive_id=NOW - 1_000_000_000).status == 'unavailable_document'
|
||||
assert card.manual_receipt is None and card.traffic_receipt is None and card.switchback_receipt is None
|
||||
assert axis.lateralActive and command.latActive
|
||||
assert bool(axis.longitudinalActive) is full_op and bool(command.longActive) is full_op
|
||||
finally:
|
||||
fixture.doCleanups()
|
||||
@@ -1,159 +0,0 @@
|
||||
"""Native validation skips width math while retaining cadence and revocation."""
|
||||
|
||||
from dataclasses import replace
|
||||
import math
|
||||
from types import SimpleNamespace
|
||||
import unittest
|
||||
from unittest.mock import patch
|
||||
|
||||
from openpilot.cereal import messaging
|
||||
from openpilot.starpilot.conditional_mode import signal_lane as lane
|
||||
from openpilot.starpilot.conditional_mode.tests.test_signal_lane import BOOT, MONO, frame, wire_model
|
||||
|
||||
|
||||
def native_event(index=1, *, line=None, defect=None, **geometry):
|
||||
"""Reusable serialized ModelDataV2 event; line0..3 lanes,4..5 road edges."""
|
||||
event = messaging.new_message('modelV2', valid=True, logMonoTime=MONO + index * 50_000_000)
|
||||
event.modelV2 = wire_model(**geometry)
|
||||
event.modelV2.timestampEof = BOOT + index * 50_000_000
|
||||
if line is not None:
|
||||
selected = event.modelV2.laneLines[line] if line < 4 else event.modelV2.roadEdges[line - 4]
|
||||
if defect == 'length':
|
||||
selected.y = list(selected.y)[:-1]
|
||||
elif defect == 'nan':
|
||||
selected.y[10] = math.nan
|
||||
elif defect == 'nonincreasing':
|
||||
selected.x[10] = selected.x[9]
|
||||
elif defect == 'origin_roundoff':
|
||||
selected.x[0] = -1e-8
|
||||
else:
|
||||
raise ValueError('unknown geometry defect')
|
||||
return messaging.log_from_bytes(event.to_bytes())
|
||||
|
||||
|
||||
def native_model(index=1, **changes):
|
||||
return native_event(index, **changes).modelV2
|
||||
|
||||
|
||||
def native_frame(index=1, *, model=None, **changes):
|
||||
return replace(frame(index, model=native_model(index) if model is None else model), **changes)
|
||||
|
||||
|
||||
def warmed_tracker():
|
||||
tracker = lane.SignalLaneTracker()
|
||||
for index in range(1, 5):
|
||||
tracker.update(native_frame(index))
|
||||
return tracker
|
||||
|
||||
|
||||
class TestSignalLaneValidation(unittest.TestCase):
|
||||
def test_actual_native_valid_edge_override_origin_and_shape(self):
|
||||
for geometry in ({}, {'left_edge': -2.0}, {'right_edge': 2.0}):
|
||||
model = native_model(**geometry)
|
||||
expected = lane.model_widths(model)
|
||||
self.assertIsNotNone(expected)
|
||||
with patch.object(lane, '_line', wraps=lane._line) as lines, \
|
||||
patch.object(lane.np, 'interp', side_effect=AssertionError('validation interpolated')), \
|
||||
patch.object(lane.np, 'median', side_effect=AssertionError('validation computed median')):
|
||||
self.assertTrue(lane._model_widths_valid(model))
|
||||
self.assertEqual(lines.call_count, 6)
|
||||
for index in range(6):
|
||||
model = native_model(line=index, defect='origin_roundoff')
|
||||
self.assertEqual(lane._model_widths_valid(model), lane.model_widths(model) is not None)
|
||||
for field, length in (('laneLines', 3), ('roadEdges', 1)):
|
||||
event = messaging.new_message('modelV2', valid=True)
|
||||
event.modelV2 = native_model()
|
||||
event.modelV2.init(field, length)
|
||||
model = messaging.log_from_bytes(event.to_bytes()).modelV2
|
||||
self.assertFalse(lane._model_widths_valid(model))
|
||||
self.assertIsNone(lane.model_widths(model))
|
||||
backing = native_event(4).as_builder()
|
||||
alias = backing.modelV2.as_reader()
|
||||
tracker = warmed_tracker()
|
||||
self.assertTrue(lane._model_widths_valid(alias))
|
||||
self.assertTrue(tracker.update(native_frame(4, model=alias)).lane_available)
|
||||
backing.modelV2.roadEdges[1].y[10] = math.nan
|
||||
self.assertFalse(lane._model_widths_valid(alias))
|
||||
self.assertIsNone(lane.model_widths(alias))
|
||||
self.assertIsNone(tracker.update(native_frame(4, model=alias)).lane_available)
|
||||
boundary = native_event().as_builder()
|
||||
xs = [0.0, math.ldexp(1.0, -149), *map(float, range(1, 31)), 500.0]
|
||||
for index, line in enumerate((*boundary.modelV2.laneLines, *boundary.modelV2.roadEdges)):
|
||||
line.x = xs
|
||||
line.y = [-30.0 if index % 2 == 0 else 30.0] * 33
|
||||
model = messaging.log_from_bytes(boundary.to_bytes()).modelV2
|
||||
self.assertEqual(lane._model_widths_valid(model), lane.model_widths(model) is not None)
|
||||
|
||||
def test_each_native_lane_and_edge_rejects_length_nan_and_nonincreasing_x(self):
|
||||
for index in range(6):
|
||||
for defect in ('length', 'nan', 'nonincreasing'):
|
||||
with self.subTest(line=index, defect=defect):
|
||||
model = native_model(line=index, defect=defect)
|
||||
self.assertIsNone(lane.model_widths(model))
|
||||
self.assertFalse(lane._model_widths_valid(model))
|
||||
|
||||
def test_custom_builder_and_wrong_schema_use_full_fallback(self):
|
||||
reader = native_model()
|
||||
wrong_schema = messaging.new_message('carState').as_reader().carState
|
||||
custom = SimpleNamespace(schema=reader.schema, laneLines=reader.laneLines, roadEdges=reader.roadEdges)
|
||||
for model in (None, custom, reader.as_builder(), wrong_schema):
|
||||
for result in (None, (3.6, 3.6)):
|
||||
with self.subTest(model=type(model), result=result), patch.object(lane, 'model_widths', return_value=result) as full:
|
||||
self.assertEqual(lane._model_widths_valid(model), result is not None)
|
||||
full.assert_called_once_with(model)
|
||||
with patch.object(lane, 'model_widths', side_effect=RuntimeError('custom fallback')):
|
||||
with self.assertRaisesRegex(RuntimeError, 'custom fallback'):
|
||||
lane._model_widths_valid(custom)
|
||||
|
||||
def test_three_intervening_and_repeated_native_frames_skip_math_fourth_retained(self):
|
||||
tracker = lane.SignalLaneTracker()
|
||||
with patch.object(lane, 'model_widths', wraps=lane.model_widths) as full, \
|
||||
patch.object(lane.np, 'interp', wraps=lane.np.interp) as interp, \
|
||||
patch.object(lane.np, 'median', wraps=lane.np.median) as median:
|
||||
for index in range(1, 4):
|
||||
self.assertIsNone(tracker.update(native_frame(index)).lane_available)
|
||||
self.assertEqual((full.call_count, interp.call_count, median.call_count), (0, 0, 0))
|
||||
measured = tracker.update(native_frame(4))
|
||||
self.assertTrue(measured.lane_available)
|
||||
assert measured.left_width_m is not None
|
||||
self.assertAlmostEqual(measured.left_width_m, 3.6, places=5)
|
||||
self.assertEqual((full.call_count, interp.call_count, median.call_count), (1, 4, 4))
|
||||
repeated = tracker.update(native_frame(4))
|
||||
self.assertEqual(repeated, measured)
|
||||
self.assertEqual(tracker._count, 4)
|
||||
for index in range(5, 8):
|
||||
model = native_model(index, left_outer=-3.8)
|
||||
self.assertTrue(tracker.update(native_frame(index, model=model)).lane_available)
|
||||
self.assertEqual((full.call_count, interp.call_count, median.call_count), (1, 4, 4))
|
||||
changed = tracker.update(native_frame(8, model=native_model(8, left_outer=-3.8)))
|
||||
self.assertFalse(changed.lane_available)
|
||||
assert changed.left_width_m is not None
|
||||
self.assertAlmostEqual(changed.left_width_m, 2.0, places=5)
|
||||
self.assertEqual((full.call_count, interp.call_count, median.call_count), (2, 8, 8))
|
||||
|
||||
def test_bad_intervening_repeated_and_stale_sources_revoke_cached_widths(self):
|
||||
for index in (4, 5):
|
||||
tracker = warmed_tracker()
|
||||
malformed = native_frame(index, model=native_model(index, line=5, defect='nan'))
|
||||
with patch.object(lane.np, 'interp', side_effect=AssertionError('malformed validation interpolated')):
|
||||
invalid = tracker.update(malformed)
|
||||
self.assertIsNone(invalid.left_width_m)
|
||||
self.assertIsNone(invalid.right_width_m)
|
||||
self.assertIsNone(invalid.lane_available)
|
||||
self.assertIsNone(invalid.signal_scene)
|
||||
for next_index in range(index + 1, 8):
|
||||
self.assertIsNone(tracker.update(native_frame(next_index)).lane_available)
|
||||
self.assertTrue(tracker.update(native_frame(8)).lane_available)
|
||||
current = native_frame(5)
|
||||
stale = replace(current, now_mono_ns=current.now_mono_ns + lane.MODEL_MAX_AGE_NS + 1,
|
||||
now_boot_ns=current.now_boot_ns + lane.MODEL_MAX_AGE_NS + 1)
|
||||
bad_car = replace(current, car_source=lane.MonoSource(MONO - 1, current.now_mono_ns))
|
||||
for invalid in (stale, bad_car, replace(current, model_eof_boot_ns=BOOT - 1), native_frame(3)):
|
||||
tracker = warmed_tracker()
|
||||
self.assertEqual(tracker.update(invalid), lane.UNKNOWN)
|
||||
self.assertIsNone(tracker._widths)
|
||||
self.assertEqual(tracker._count, 0)
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
unittest.main()
|
||||
@@ -101,29 +101,9 @@ class CoastRuntime:
|
||||
return False
|
||||
|
||||
def sample(self, params, cp, sm, *, now_ns, target_mps, lead_relevant, stop_context):
|
||||
from openpilot.starpilot.controllers.wheel_actions import eligible, LIFETIME_NS, cp_fingerprint, WheelConsumer
|
||||
from openpilot.starpilot.controllers.wheel_actions import eligible, LIFETIME_NS, cp_fingerprint
|
||||
from openpilot.starpilot.conditional_mode.ui_action import fresh_service
|
||||
from openpilot.starpilot.saved_source import read_saved
|
||||
# A zero accepted-drive/heartbeat cannot satisfy the admission below.
|
||||
if (type(self) is CoastRuntime and type(getattr(self, 'owner', None)) is CoastOwner and
|
||||
type(getattr(self, 'consumer', None)) is WheelConsumer and 'sample' not in vars(self.owner) and
|
||||
type(getattr(self, 'drive', None)) is int and type(getattr(self.consumer, 'last_heartbeat', None)) is int and
|
||||
(self.drive == 0 or self.consumer.last_heartbeat == 0) and
|
||||
type(stop_context) is bool and type(getattr(self, 'conditional_stop', None)) is bool):
|
||||
from capnp.lib.capnp import _DynamicStructReader
|
||||
from opendbc.car.structs import car
|
||||
from openpilot.cereal import log, messaging
|
||||
from openpilot.common.params import Params
|
||||
if (type(params) is Params and type(sm) is messaging.SubMaster and type(getattr(sm, 'data', None)) is dict and
|
||||
type(cp) is _DynamicStructReader and cp.schema == car.CarParams.schema):
|
||||
device = sm.data.get('deviceState')
|
||||
state = sm.data.get('carState')
|
||||
control = sm.data.get('carControl')
|
||||
if (type(device) is _DynamicStructReader and device.schema == log.DeviceState.schema and
|
||||
type(state) is _DynamicStructReader and state.schema == car.CarState.schema and
|
||||
type(control) is _DynamicStructReader and control.schema == car.CarControl.schema):
|
||||
self.owner.reset()
|
||||
return CoastPlan()
|
||||
drive = int(sm["deviceState"].startedMonoTime)
|
||||
raw, readable = read_saved(params, "PulseGlideSpeedDelta", 32)
|
||||
units, unit_readable = read_saved(params, "IsMetric", 8)
|
||||
|
||||
@@ -1,210 +0,0 @@
|
||||
"""Idle shortcut is narrow; accepted wheel traffic keeps live admission checks."""
|
||||
|
||||
from contextlib import contextmanager
|
||||
import copy
|
||||
import tempfile
|
||||
import unittest
|
||||
from unittest.mock import Mock, patch
|
||||
|
||||
from capnp.lib.capnp import KjException
|
||||
from opendbc.car import gen_empty_fingerprint, structs
|
||||
from opendbc.car.honda.interface import CarInterface
|
||||
from opendbc.car.honda.values import CAR
|
||||
from openpilot.cereal import messaging
|
||||
from openpilot.common.params import Params
|
||||
from openpilot.common.prefix import OpenpilotPrefix
|
||||
from openpilot.starpilot.controllers.coast import CoastOwner, CoastPlan, CoastRuntime
|
||||
from openpilot.starpilot.controllers import wheel_actions
|
||||
|
||||
DRIVE, NOW = 1_000_000_000, 3_000_000_000
|
||||
SERVICES = ('deviceState', 'carState', 'carControl')
|
||||
|
||||
|
||||
@contextmanager
|
||||
def coast_fixture():
|
||||
"""Reusable actual Params + reader CP + actual SubMaster, with private IPC."""
|
||||
with OpenpilotPrefix(), tempfile.TemporaryDirectory() as directory:
|
||||
params = Params(directory)
|
||||
params.put_bool('SafeMode', False, block=True)
|
||||
cp = CarInterface.get_params(CAR.HONDA_CIVIC_BOSCH, gen_empty_fingerprint(), [], True, False, False).as_reader()
|
||||
sm = messaging.SubMaster(list(SERVICES), poll='carState')
|
||||
frames = []
|
||||
for service in SERVICES:
|
||||
message = messaging.new_message(service, valid=True, logMonoTime=NOW)
|
||||
if service == 'deviceState':
|
||||
message.deviceState.started, message.deviceState.startedMonoTime = True, DRIVE
|
||||
elif service == 'carState':
|
||||
message.carState.canValid, message.carState.vEgo = True, 20.0
|
||||
else:
|
||||
message.carControl.enabled = message.carControl.longActive = True
|
||||
message.carControl.orientationNED = [0.0, 0.0, 0.0]
|
||||
frames.append(message.as_reader())
|
||||
sm.update_msgs(NOW / 1e9, frames)
|
||||
yield params, cp, sm
|
||||
|
||||
|
||||
def sample(runtime, params, cp, sm, **changes):
|
||||
values = {'now_ns': NOW, 'target_mps': 20.0, 'lead_relevant': False, 'stop_context': False}
|
||||
values.update(changes)
|
||||
return runtime.sample(params, cp, sm, **values)
|
||||
|
||||
|
||||
def deliver_action(runtime, params, cp, sm, action=2):
|
||||
"""Use the actual WheelPublisher and serialized receipt, without CAN or IPC send."""
|
||||
class Sink:
|
||||
def send(self, _service, message):
|
||||
self.raw = message.to_bytes()
|
||||
sink = Sink()
|
||||
params.put('DistanceButtonControl', action, block=True)
|
||||
publisher = wheel_actions.WheelPublisher()
|
||||
publisher.publish((('DistanceButtonControl', action),), cp, sink, now_ns=NOW, drive_id=DRIVE,
|
||||
source_car_ns=NOW, source_control_ns=NOW)
|
||||
return runtime.receive(messaging.log_from_bytes(sink.raw), params, cp, sm, now_ns=NOW)
|
||||
|
||||
|
||||
class FallbackReached(Exception):
|
||||
pass
|
||||
|
||||
|
||||
class TestCoastIdle(unittest.TestCase):
|
||||
def test_zero_admission_resets_all_flags_preserves_bookkeeping_and_skips_work(self):
|
||||
with coast_fixture() as (params, cp, sm):
|
||||
for drive, heartbeat in ((0, 0), (0, NOW), (DRIVE, 0)):
|
||||
for stop in (False, True):
|
||||
runtime = CoastRuntime()
|
||||
runtime.drive, runtime.consumer.last_heartbeat = drive, heartbeat
|
||||
runtime.session, runtime.cp_signature, runtime.traffic_toggles = 'kept', 'kept', 7
|
||||
runtime.consumer.session, runtime.consumer.sequence = 'kept-consumer', 23
|
||||
runtime.consumer.retired.add('retired')
|
||||
runtime.conditional_stop = stop
|
||||
for flag in ('force_coast', 'pulse_glide', 'coasting', 'hill_paused'):
|
||||
setattr(runtime.owner, flag, True)
|
||||
bookkeeping = {key: value for key, value in vars(runtime).items() if key not in ('owner', 'consumer')}
|
||||
consumer = copy.deepcopy(vars(runtime.consumer))
|
||||
with patch('openpilot.starpilot.saved_source.read_saved', side_effect=AssertionError('idle read')) as reads, \
|
||||
patch.object(wheel_actions, 'cp_fingerprint', side_effect=AssertionError('idle hash')) as fingerprint:
|
||||
first = sample(runtime, params, cp, sm, stop_context=stop)
|
||||
second = sample(runtime, params, cp, sm, stop_context=stop)
|
||||
self.assertEqual(first, CoastPlan())
|
||||
self.assertIsNot(first, second)
|
||||
self.assertFalse(any(vars(runtime.owner).values()))
|
||||
self.assertEqual(bookkeeping, {key: value for key, value in vars(runtime).items() if key not in ('owner', 'consumer')})
|
||||
self.assertEqual(vars(runtime.consumer), consumer)
|
||||
reads.assert_not_called()
|
||||
fingerprint.assert_not_called()
|
||||
|
||||
def test_nonexact_sentinels_and_runtime_owners_keep_fallback(self):
|
||||
class Integer(int):
|
||||
pass
|
||||
class RuntimeSubclass(CoastRuntime):
|
||||
pass
|
||||
class OwnerSubclass(CoastOwner):
|
||||
pass
|
||||
class ConsumerSubclass(wheel_actions.WheelConsumer):
|
||||
pass
|
||||
with coast_fixture() as (params, cp, sm):
|
||||
for field in ('drive', 'heartbeat', 'stop_context', 'conditional_stop'):
|
||||
for value in (False, 0.0, Integer(0), None):
|
||||
if field in ('stop_context', 'conditional_stop') and type(value) is bool:
|
||||
continue
|
||||
runtime, changes = CoastRuntime(), {}
|
||||
if field == 'heartbeat':
|
||||
setattr(runtime.consumer, f'last_{field}', value)
|
||||
elif field == 'stop_context':
|
||||
changes[field] = value
|
||||
else:
|
||||
setattr(runtime, field, value)
|
||||
with self.subTest(field=field, value=value), patch('openpilot.starpilot.saved_source.read_saved', side_effect=FallbackReached):
|
||||
with self.assertRaises(FallbackReached):
|
||||
sample(runtime, params, cp, sm, **changes)
|
||||
runtime_cases = (RuntimeSubclass(), CoastRuntime(), CoastRuntime())
|
||||
runtime_cases[1].owner, runtime_cases[2].consumer = OwnerSubclass(), ConsumerSubclass()
|
||||
for runtime in runtime_cases:
|
||||
with patch('openpilot.starpilot.saved_source.read_saved', side_effect=FallbackReached):
|
||||
with self.assertRaises(FallbackReached):
|
||||
sample(runtime, params, cp, sm)
|
||||
|
||||
def test_instance_sample_override_is_observed(self):
|
||||
with coast_fixture() as (params, cp, sm):
|
||||
runtime, marker = CoastRuntime(), object()
|
||||
runtime.owner.sample = Mock(return_value=marker)
|
||||
with patch('openpilot.starpilot.saved_source.read_saved', return_value=(None, True)), \
|
||||
patch.object(wheel_actions, 'cp_fingerprint', return_value='different'):
|
||||
self.assertIs(sample(runtime, params, cp, sm), marker)
|
||||
runtime.owner.sample.assert_called_once()
|
||||
|
||||
def test_fake_contexts_builders_wrong_schemas_and_missing_readers_fall_back(self):
|
||||
class Proxy:
|
||||
def __init__(self, target):
|
||||
self.target = target
|
||||
def __getattr__(self, name):
|
||||
return getattr(self.target, name)
|
||||
def __getitem__(self, name):
|
||||
return self.target[name]
|
||||
class Mapping(dict):
|
||||
pass
|
||||
with coast_fixture() as (params, cp, sm):
|
||||
cases = ((Proxy(params), cp, sm), (params, cp, Proxy(sm)),
|
||||
(params, cp.as_builder(), sm), (params, structs.CarState().as_reader(), sm))
|
||||
for actual_params, actual_cp, actual_sm in cases:
|
||||
with patch('openpilot.starpilot.saved_source.read_saved', side_effect=FallbackReached):
|
||||
with self.assertRaises(FallbackReached):
|
||||
sample(CoastRuntime(), actual_params, actual_cp, actual_sm)
|
||||
original = sm.data
|
||||
try:
|
||||
sm.data = Mapping(original)
|
||||
with patch('openpilot.starpilot.saved_source.read_saved', side_effect=FallbackReached):
|
||||
with self.assertRaises(FallbackReached):
|
||||
sample(CoastRuntime(), params, cp, sm)
|
||||
for name in SERVICES:
|
||||
for replacement in (None, structs.CarParams().as_reader(), original[name].as_builder()):
|
||||
sm.data = dict(original)
|
||||
if replacement is None:
|
||||
del sm.data[name]
|
||||
else:
|
||||
sm.data[name] = replacement
|
||||
expected = KeyError if replacement is None and name == 'deviceState' else \
|
||||
(AttributeError, KjException) if name == 'deviceState' and replacement.schema == structs.CarParams.schema else FallbackReached
|
||||
with self.subTest(name=name, replacement=type(replacement)), \
|
||||
patch('openpilot.starpilot.saved_source.read_saved', side_effect=FallbackReached):
|
||||
with self.assertRaises(expected):
|
||||
sample(CoastRuntime(), params, cp, sm)
|
||||
finally:
|
||||
sm.data = original
|
||||
|
||||
def test_real_published_action_ends_shortcut_and_live_rejections_still_reset(self):
|
||||
from openpilot.starpilot.saved_source import read_saved
|
||||
for fault in ('safe_mode', 'cp_mutation', 'expiry', 'can_invalid', 'can_timeout'):
|
||||
with self.subTest(fault=fault), coast_fixture() as (params, cp, sm):
|
||||
runtime = CoastRuntime()
|
||||
self.assertEqual(sample(runtime, params, cp, sm), CoastPlan())
|
||||
self.assertTrue(deliver_action(runtime, params, cp, sm))
|
||||
self.assertGreater(runtime.drive, 0)
|
||||
self.assertGreater(runtime.consumer.last_heartbeat, 0)
|
||||
with patch('openpilot.starpilot.saved_source.read_saved', wraps=read_saved) as reads, \
|
||||
patch.object(wheel_actions, 'cp_fingerprint', wraps=wheel_actions.cp_fingerprint) as fingerprint:
|
||||
self.assertTrue(sample(runtime, params, cp, sm).force_decel)
|
||||
self.assertGreater(reads.call_count, 0)
|
||||
fingerprint.assert_called()
|
||||
changes = {}
|
||||
if fault == 'safe_mode':
|
||||
params.put_bool('SafeMode', True, block=True)
|
||||
elif fault == 'cp_mutation':
|
||||
changed = cp.as_builder()
|
||||
changed.carVin = 'changed-after-receipt'
|
||||
cp = changed.as_reader()
|
||||
elif fault == 'expiry':
|
||||
changes['now_ns'] = NOW + wheel_actions.LIFETIME_NS + 1
|
||||
else:
|
||||
changed = sm.data['carState'].as_builder()
|
||||
if fault == 'can_invalid':
|
||||
changed.canValid = False
|
||||
else:
|
||||
changed.canTimeout = True
|
||||
sm.data['carState'] = changed.as_reader()
|
||||
self.assertEqual(sample(runtime, params, cp, sm, **changes), CoastPlan())
|
||||
self.assertFalse(any(vars(runtime.owner).values()))
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
unittest.main()
|
||||
@@ -5,13 +5,11 @@ import math
|
||||
import time
|
||||
|
||||
from openpilot.cereal.services import SERVICE_LIST
|
||||
from openpilot.common.realtime import DT_MDL
|
||||
from openpilot.starpilot.curve_speed.runtime import Frame, INPUT_MAX_AGE_NS, MODEL_MAX_AGE_NS, Runtime
|
||||
from openpilot.starpilot.curve_speed.target import CurveProfile
|
||||
from openpilot.starpilot.lead_tracking import FILTER_THRESHOLD, FILTER_TIME_S, LeadDetector
|
||||
from openpilot.starpilot.lead_tracking import LeadDetector
|
||||
|
||||
CLOCK_PAIR_MAX_SKEW_NS = 5_000_000
|
||||
LEAD_WARMUP_FRAMES = math.ceil(math.log(1 - FILTER_THRESHOLD) / math.log(1 - DT_MDL / (FILTER_TIME_S + DT_MDL)))
|
||||
|
||||
|
||||
def clock_pair_ns() -> tuple[int, int] | None:
|
||||
@@ -69,14 +67,12 @@ class CurveHost:
|
||||
self.boot_offset_ns: int | None = None
|
||||
self.source_floor_ns = 0
|
||||
self.leads = LeadDetector()
|
||||
self.lead_samples = 0
|
||||
self.lead_drive_id = 0
|
||||
self.previous_long_active = False
|
||||
self.previous_active_headway: float | None = None
|
||||
|
||||
def _reset_leads(self) -> None:
|
||||
self.leads.reset()
|
||||
self.lead_samples = 0
|
||||
self.previous_long_active = False
|
||||
self.previous_active_headway = None
|
||||
|
||||
@@ -105,20 +101,22 @@ class CurveHost:
|
||||
required = ('modelV2', 'carState', 'carControl', 'controlsState', 'selfdriveState', 'radarState')
|
||||
try:
|
||||
camera_eof = sm['modelV2'].timestampEof
|
||||
camera_age = pair[1] - camera_eof if type(camera_eof) is int and camera_eof > 0 else -1
|
||||
camera_current = (0 <= camera_age <= MODEL_MAX_AGE_NS and camera_eof <= now_ns + offset)
|
||||
camera_current = (type(camera_eof) is int and 0 < camera_eof <= pair[1] and
|
||||
pair[1] - camera_eof <= MODEL_MAX_AGE_NS)
|
||||
except (KeyError, AttributeError, TypeError, ValueError, OverflowError):
|
||||
camera_current = False
|
||||
# A newly published model envelope cannot renew the age of its camera
|
||||
# input. EOF uses BOOTTIME; only the bracketed clock pair may compare it.
|
||||
# Offline replay intentionally keeps its separate recorded timeline.
|
||||
if (not camera_current or not 0 < frame.model_ns <= now_ns or
|
||||
pair[0] - frame.model_ns > MODEL_MAX_AGE_NS or not 0 <= pair[0] - now_ns <= INPUT_MAX_AGE_NS):
|
||||
if (not camera_current or not 0 <= pair[0] - now_ns <= INPUT_MAX_AGE_NS or
|
||||
not all(_fresh(sm, name, pair[0]) for name in required)):
|
||||
frame = replace(frame, inputs_fresh=False)
|
||||
if rearmed or any(type(sm.logMonoTime.get(name, 0)) is not int or sm.logMonoTime[name] <= self.source_floor_ns
|
||||
for name in required):
|
||||
frame = replace(frame, inputs_fresh=False)
|
||||
if frame.inputs_fresh and frame.profile is not None:
|
||||
if not frame.inputs_fresh or frame.profile is None:
|
||||
self._reset_leads()
|
||||
else:
|
||||
try:
|
||||
# Tracking precedes headway reset: the first inactive tick retains the prior headway,
|
||||
# then uses zero. Active control requires this cycle's MPC headway.
|
||||
@@ -133,16 +131,9 @@ class CurveHost:
|
||||
observed_mono_s=frame.model_ns / 1e9, now_mono_s=now_ns / 1e9,
|
||||
radar_fresh=_fresh(sm, 'radarState', now_ns), model_fresh=_fresh(sm, 'modelV2', now_ns),
|
||||
car_fresh=_fresh(sm, 'carState', now_ns), headway_fresh=headway_fresh)
|
||||
if self.leads.last_observed_mono_s is None:
|
||||
self.lead_samples = 0
|
||||
elif observed.tracked is not None and not sm['carState'].standstill:
|
||||
self.lead_samples += 1
|
||||
tracked = observed.tracked
|
||||
following = (observed.following if frame.long_active else
|
||||
False if observed.tracked is not None else None)
|
||||
if self.lead_samples < LEAD_WARMUP_FRAMES and not tracked and observed.raw_present:
|
||||
tracked = following = None
|
||||
frame = replace(frame, tracking_lead=tracked, following_lead=following)
|
||||
frame = replace(frame, tracking_lead=observed.tracked, following_lead=following)
|
||||
self.previous_long_active = frame.long_active if observed.tracked is not None else False
|
||||
self.previous_active_headway = (float(detector_headway) if detector_headway is not None and frame.long_active and headway_fresh and
|
||||
observed.tracked is not None else None)
|
||||
|
||||
@@ -76,7 +76,6 @@ class Runtime:
|
||||
self.filter = TargetFilter()
|
||||
self.last_ns = 0
|
||||
self.last_model_ns = 0
|
||||
self.last_valid_ns = 0
|
||||
self.drive_id = 0
|
||||
self.was_controlling = False
|
||||
self.was_long_active = False
|
||||
@@ -95,7 +94,6 @@ class Runtime:
|
||||
self.pending_ns = 0
|
||||
self.pending_target: float | None = None
|
||||
self.pending_cap = False
|
||||
self.pending_hold = False
|
||||
self.pending_cruise = 0.0
|
||||
self.pending_ego = 0.0
|
||||
self.glow = False
|
||||
@@ -112,7 +110,6 @@ class Runtime:
|
||||
self.filter.reset()
|
||||
self.last_ns = 0
|
||||
self.last_model_ns = 0
|
||||
self.last_valid_ns = 0
|
||||
self.drive_id = drive_id
|
||||
self.was_controlling = False
|
||||
self.was_long_active = False
|
||||
@@ -130,7 +127,6 @@ class Runtime:
|
||||
self.pending_ns = 0
|
||||
self.pending_target = None
|
||||
self.pending_cap = False
|
||||
self.pending_hold = False
|
||||
self.pending_cruise = 0.0
|
||||
self.pending_ego = 0.0
|
||||
self.glow = False
|
||||
@@ -153,8 +149,6 @@ class Runtime:
|
||||
self.glow = False
|
||||
return False
|
||||
self.confirmed_ns = frame_ns
|
||||
if self.pending_hold:
|
||||
return True
|
||||
target = self.pending_target
|
||||
selected = applied and self.pending_cap and target is not None
|
||||
self.was_controlling = bool(selected and target < self.pending_cruise - 1.0 and
|
||||
@@ -236,31 +230,24 @@ class Runtime:
|
||||
self.pending_ns = now
|
||||
self.pending_target = None
|
||||
self.pending_cap = False
|
||||
self.pending_hold = False
|
||||
valid_profile = (frame.inputs_fresh and frame.profile is not None and
|
||||
type(frame.model_ns) is int and 0 < frame.model_ns <= now and
|
||||
now - frame.model_ns <= MODEL_MAX_AGE_NS and
|
||||
frame.profile.observed_ns == frame.model_ns and
|
||||
frame.model_ns > self.last_model_ns)
|
||||
if not valid_profile:
|
||||
self.filter.reset()
|
||||
self.watch_until_ns = 0
|
||||
self.last_training_ns = 0
|
||||
self.pending_hold = (self.filter.target is not None and
|
||||
now - self.last_valid_ns <= int(MAX_STEP_SECONDS * 1e9))
|
||||
if self.pending_hold:
|
||||
self.pending_target = self.filter.target
|
||||
else:
|
||||
self.filter.reset()
|
||||
self.nudged = False
|
||||
self.override = False
|
||||
self.was_controlling = False
|
||||
self.glow = False
|
||||
self.was_long_active = frame.long_active
|
||||
self.was_gas = frame.gas_pressed
|
||||
self.was_brake = frame.brake_pressed
|
||||
self.nudged = False
|
||||
self.override = False
|
||||
self.was_controlling = False
|
||||
self.glow = False
|
||||
self.was_long_active = frame.long_active
|
||||
self.was_gas = frame.gas_pressed
|
||||
self.was_brake = frame.brake_pressed
|
||||
return self._result(None, None, 0.0, 'stale_or_invalid_evidence', False, now)
|
||||
self.last_model_ns = frame.model_ns
|
||||
self.last_valid_ns = now
|
||||
if (not finite_number(frame.cruise_mps) or frame.cruise_mps <= 0 or
|
||||
not finite_number(frame.ego_mps) or frame.ego_mps < 0 or
|
||||
frame.ego_mps > 100.0 or not finite_number(frame.measured_curvature) or
|
||||
|
||||
@@ -50,8 +50,7 @@ class StatusPublisher:
|
||||
'documentValid': bool(runtime.document_valid),
|
||||
'hasCandidate': result.candidate_mps is not None, 'candidateMps': float(result.candidate_mps or 0.0),
|
||||
'hasCeiling': result.ceiling_mps is not None, 'ceilingMps': float(result.ceiling_mps or 0.0),
|
||||
'applied': bool(planner.last_curve_ceiling_applied),
|
||||
'controlling': bool(runtime.was_controlling and planner.last_curve_ceiling_applied),
|
||||
'applied': bool(planner.last_curve_ceiling_applied), 'controlling': bool(runtime.was_controlling),
|
||||
'training': bool(result.training), 'calibrationProgress': float(result.progress),
|
||||
'comfortAccel': float(runtime.curve.average_comfort), 'bindingDistance': float(result.binding_distance_m),
|
||||
'reason': result.reason if result.ceiling_mps is not None else result.learning_reason,
|
||||
|
||||
@@ -101,50 +101,6 @@ class TestCurveHost(unittest.TestCase):
|
||||
self.assertFalse(seen[-1][1].following)
|
||||
self.assertFalse(seen[-1][1].tracked)
|
||||
|
||||
def capture_frames(self, host):
|
||||
frames = []
|
||||
original = host.runtime.step
|
||||
def runtime_capture(frame, **kwargs):
|
||||
frames.append(frame)
|
||||
return original(frame, **kwargs)
|
||||
self.enterContext(patch.object(host.runtime, 'step', side_effect=runtime_capture))
|
||||
return frames
|
||||
|
||||
def present_lead(self, d_rel=30.0):
|
||||
radar = self.sm.messages['radarState'].leadOne
|
||||
radar.present = True
|
||||
radar.dRel = d_rel
|
||||
|
||||
def test_stale_frame_keeps_lead_consensus(self):
|
||||
host = CurveHost(enabled=True, no_lead=True, replay=True)
|
||||
frames = self.capture_frames(host)
|
||||
self.present_lead()
|
||||
for n in range(1, 17):
|
||||
stamp = NOW + n * 50_000_000
|
||||
self.sm.logMonoTime = dict.fromkeys(self.sm.messages, stamp)
|
||||
host.sample(self.sm, self.cp, stamp, follow_time_s=1.45)
|
||||
self.assertTrue(frames[-1].following_lead)
|
||||
stamp += 45_000_000
|
||||
self.sm.logMonoTime = dict.fromkeys(self.sm.messages, stamp)
|
||||
self.sm.logMonoTime['carState'] = stamp - 30_000_000
|
||||
host.sample(self.sm, self.cp, stamp, follow_time_s=1.45)
|
||||
stamp += 45_000_000
|
||||
self.sm.logMonoTime = dict.fromkeys(self.sm.messages, stamp)
|
||||
host.sample(self.sm, self.cp, stamp, follow_time_s=1.45)
|
||||
self.assertTrue(frames[-1].tracking_lead)
|
||||
self.assertTrue(frames[-1].following_lead)
|
||||
|
||||
def test_present_lead_is_unknown_until_detector_warms_up(self):
|
||||
host = CurveHost(enabled=True, no_lead=True, replay=True)
|
||||
frames = self.capture_frames(host)
|
||||
self.present_lead()
|
||||
for n in range(1, 21):
|
||||
stamp = NOW + n * 50_000_000
|
||||
self.sm.logMonoTime = dict.fromkeys(self.sm.messages, stamp)
|
||||
self.assertIsNone(host.sample(self.sm, self.cp, stamp, follow_time_s=1.45).ceiling_mps)
|
||||
self.assertNotIn(False, [frame.following_lead for frame in frames])
|
||||
self.assertTrue(frames[-1].following_lead)
|
||||
|
||||
def test_actual_serialized_schema_projection(self):
|
||||
events = {name: messaging.new_message(name) for name in self.sm.messages}
|
||||
events['modelV2'].modelV2.orientationRate.z = [0.25] * 33
|
||||
@@ -222,33 +178,6 @@ class TestCurveHost(unittest.TestCase):
|
||||
self.assertFalse(result.training)
|
||||
self.assertEqual(result.reason, 'stale_or_invalid_evidence')
|
||||
|
||||
def test_live_planner_latency_does_not_age_out_loop_inputs(self):
|
||||
offset = 2_000_000_000
|
||||
for latency, input_age, camera_age, expected in (
|
||||
(35_000_000, 5_000_000, 60_000_000, True),
|
||||
(35_000_000, 21_000_000, 60_000_000, False),
|
||||
(35_000_000, 5_000_000, 115_000_000, True),
|
||||
(35_000_000, 5_000_000, 115_000_001, False),
|
||||
(35_000_000, 5_000_000, 140_000_000, False),
|
||||
(35_000_000, 5_000_000, -1, False),
|
||||
(35_000_000, 5_000_000, 150_000_001, False),
|
||||
(150_000_001, 5_000_000, 60_000_000, False),
|
||||
):
|
||||
with self.subTest(latency=latency, input_age=input_age, camera_age=camera_age):
|
||||
tick = [NOW]
|
||||
host = CurveHost(enabled=True, clock_pair=lambda tick=tick, latency=latency: (tick[0] + latency, tick[0] + latency + offset))
|
||||
result = None
|
||||
for _ in range(4):
|
||||
tick[0] += 50_000_000
|
||||
self.sm.logMonoTime = dict.fromkeys(self.sm.messages, tick[0] - input_age)
|
||||
self.sm.logMonoTime['modelV2'] = tick[0] - 3_000_000
|
||||
self.sm.logMonoTime['radarState'] = tick[0] - 40_000_000
|
||||
self.sm.messages['modelV2'].timestampEof = tick[0] + offset - camera_age
|
||||
result = host.sample(self.sm, self.cp, tick[0])
|
||||
self.assertEqual(result.ceiling_mps is not None, expected)
|
||||
if not expected:
|
||||
self.assertEqual(result.reason, 'stale_or_invalid_evidence')
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
unittest.main()
|
||||
|
||||
@@ -1,9 +1,7 @@
|
||||
import unittest
|
||||
from dataclasses import replace
|
||||
from types import SimpleNamespace as NS
|
||||
|
||||
from openpilot.starpilot.curve_speed.runtime import DriverEvent, Frame, Runtime
|
||||
from openpilot.starpilot.curve_speed.status import StatusPublisher, observation
|
||||
from openpilot.starpilot.curve_speed.target import CurveProfile
|
||||
|
||||
|
||||
@@ -86,59 +84,6 @@ class TestCurveRuntime(unittest.TestCase):
|
||||
self.assertIsNotNone(rt.step(frame(4)).ceiling_mps)
|
||||
self.assertIsNotNone(rt.step(frame(20)).ceiling_mps) # discontinuity resets filter, then fresh frame can rearm
|
||||
|
||||
def test_isolated_stale_frame_holds_target_without_ceiling(self):
|
||||
rt = Runtime(enabled=True)
|
||||
arm_selected(rt)
|
||||
held = rt.filter.target
|
||||
assert held is not None
|
||||
stale = frame(27, inputs_fresh=False)
|
||||
self.assertIsNone(rt.step(stale).ceiling_mps)
|
||||
rt.confirm_applied(stale.now_ns, applied=False)
|
||||
self.assertTrue(rt.was_controlling)
|
||||
self.assertTrue(rt.glow)
|
||||
self.assertEqual(rt.filter.target, held)
|
||||
resumed = rt.step(frame(28)).ceiling_mps
|
||||
assert resumed is not None
|
||||
self.assertLessEqual(resumed, held)
|
||||
|
||||
def test_stale_hold_publishes_valid_glowing_status(self):
|
||||
rt = Runtime(enabled=True)
|
||||
arm_selected(rt)
|
||||
stale = frame(27, inputs_fresh=False)
|
||||
result = rt.step(stale)
|
||||
rt.confirm_applied(stale.now_ns, applied=False)
|
||||
planner = NS(last_curve_ceiling_applied=False, last_curve_ceiling_status='absent')
|
||||
event = StatusPublisher().attach(None, NS(runtime=rt), result, planner, now_ns=stale.now_ns,
|
||||
model_ns=frame(26).now_ns, persistence_status='idle')
|
||||
status = observation(event.slcState, stale.now_ns)
|
||||
assert status is not None
|
||||
self.assertTrue(status.glow)
|
||||
self.assertFalse(status.controlling)
|
||||
|
||||
def test_stale_hold_expires_after_max_step(self):
|
||||
rt = Runtime(enabled=True)
|
||||
arm_selected(rt)
|
||||
for n in range(27, 31):
|
||||
item = frame(n, inputs_fresh=False)
|
||||
rt.step(item)
|
||||
rt.confirm_applied(item.now_ns, applied=False)
|
||||
self.assertIsNotNone(rt.filter.target)
|
||||
item = frame(31, inputs_fresh=False)
|
||||
rt.step(item)
|
||||
rt.confirm_applied(item.now_ns, applied=False)
|
||||
self.assertIsNone(rt.filter.target)
|
||||
self.assertFalse(rt.glow)
|
||||
self.assertFalse(rt.was_controlling)
|
||||
|
||||
def test_gas_override_during_stale_hold_still_latches(self):
|
||||
rt = Runtime(enabled=True)
|
||||
arm_selected(rt)
|
||||
stale = frame(27, inputs_fresh=False, gas_pressed=True)
|
||||
rt.step(stale)
|
||||
rt.confirm_applied(stale.now_ns, applied=False)
|
||||
self.assertIsNone(rt.step(frame(28, gas_pressed=True)).ceiling_mps)
|
||||
self.assertTrue(rt.override)
|
||||
|
||||
def test_invalid_document_never_learns_or_persists(self):
|
||||
rt = Runtime({'version': 1, 'buckets': {'nan': {'average': 2.0, 'count': 1}}}, enabled=True)
|
||||
self.assertFalse(rt.document_valid)
|
||||
|
||||
@@ -78,10 +78,9 @@ class FavoritesOwner:
|
||||
def __init__(self, params, actions: Callable[[], Mapping[str, FavoriteAction]], configurable: Callable[[], bool]):
|
||||
self.params, self.actions, self.configurable = params, actions, configurable
|
||||
|
||||
def snapshot(self, *, include_options=True):
|
||||
def snapshot(self):
|
||||
saved, raw, readable, valid = read_slots(self.params)
|
||||
# Empty render slots need no action metadata until the picker is opened.
|
||||
options = self.actions() if include_options or any(slot["key"] is not None for slot in saved) else {}
|
||||
options = self.actions()
|
||||
token = revision(raw)
|
||||
slots = []
|
||||
for index, slot in enumerate(saved):
|
||||
|
||||
@@ -8,13 +8,10 @@ controller selection is inferred here. Domain adapters qualify response inverse.
|
||||
from collections import OrderedDict, deque
|
||||
from dataclasses import dataclass, replace
|
||||
import math
|
||||
import re
|
||||
import uuid
|
||||
|
||||
from openpilot.starpilot.flm.adaptive_profile import Block, Sample, Result, evaluate, replay_accepts, readmit_profile
|
||||
|
||||
_IDENTITY_HEX64 = re.compile(r'[0-9a-f]{64}')
|
||||
|
||||
MAX_PENDING = 128
|
||||
MAX_DRAIN = 32
|
||||
MAX_POINTS = 240
|
||||
@@ -42,7 +39,7 @@ def replay_values(value, bound):
|
||||
|
||||
|
||||
def identity(value):
|
||||
return type(value) is str and len(value) == 64 and _IDENTITY_HEX64.fullmatch(value) is not None
|
||||
return type(value) is str and len(value) == 64 and all(c in '0123456789abcdef' for c in value)
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
|
||||
@@ -121,9 +121,7 @@ class GlobalRuntime:
|
||||
self.torque_result = (0.0, 0.0)
|
||||
self.stage_effect = False
|
||||
self.centre_ack = None
|
||||
policy = getattr(getattr(self.controller, 'starpilot_extension', None), 'policy', None)
|
||||
self.live_manual = bool(manual_current or getattr(self.controller, 'flm_surface', None) is not None or
|
||||
getattr(policy, 'surface', None) is not None)
|
||||
self.live_manual = bool(manual_current)
|
||||
self.native_required = bool(native_required)
|
||||
self.native_witness = native_witness
|
||||
key = native_identity_key(native_witness)
|
||||
|
||||
@@ -1,7 +1,6 @@
|
||||
"""Typed custom child snapshots; legacy Event fields and linear fit unchanged."""
|
||||
|
||||
from capnp.lib.capnp import _DynamicStructReader
|
||||
from openpilot.cereal import custom, messaging
|
||||
from openpilot.cereal import messaging
|
||||
from openpilot.starpilot.flm.adaptive_profile import Parameter, Profile, ReplayValidation, replay_accepts
|
||||
from openpilot.starpilot.flm.fullrate_collector import ControlEvidence, AppliedEvidence, identity, finite
|
||||
|
||||
@@ -346,54 +345,12 @@ def write_source_receipt(event, source):
|
||||
}
|
||||
|
||||
|
||||
def _read_native_source_receipt(row, now_ns, cp_sha256, source_type):
|
||||
if row.version != VERSION:
|
||||
return None
|
||||
cp = str(row.cpSha256)
|
||||
if cp != cp_sha256 or not identity(cp):
|
||||
return None
|
||||
observed = row.observedMonoTime
|
||||
if not 0 < observed <= now_ns:
|
||||
return None
|
||||
expires = row.validUntilMonoTime
|
||||
if not now_ns <= expires <= observed + 500_000_000:
|
||||
return None
|
||||
enabled = bool(row.enabled)
|
||||
if enabled:
|
||||
epoch = str(row.sourceEpoch)
|
||||
if not identity(epoch):
|
||||
return None
|
||||
reset = str(row.resetGeneration)
|
||||
if not reset:
|
||||
return None
|
||||
else:
|
||||
reset = str(row.resetGeneration)
|
||||
if len(reset) > 64:
|
||||
return None
|
||||
basis = row.calibrationBasis
|
||||
if len(basis) != 3:
|
||||
return None
|
||||
calibration = []
|
||||
for item in basis:
|
||||
value = float(item)
|
||||
if not finite(value):
|
||||
return None
|
||||
calibration.append(value)
|
||||
manual = bool(row.manual)
|
||||
if not enabled:
|
||||
epoch = str(row.sourceEpoch)
|
||||
return source_type(enabled, manual, reset, epoch, observed, expires, cp, tuple(calibration))
|
||||
|
||||
|
||||
def read_source_receipt(event, *, now_ns, cp_sha256):
|
||||
from openpilot.starpilot.flm.runtime_adapter_registry import SourceState
|
||||
|
||||
if not event.valid:
|
||||
return None
|
||||
row = event.autoSteeringLearning.learning.sourceReceipt
|
||||
if (type(row) is _DynamicStructReader and row.schema == custom.AutomaticLearningSourceReceipt.schema
|
||||
and type(now_ns) is int and type(cp_sha256) is str):
|
||||
return _read_native_source_receipt(row, now_ns, cp_sha256, SourceState)
|
||||
if (
|
||||
row.version != VERSION
|
||||
or str(row.cpSha256) != cp_sha256
|
||||
|
||||
@@ -267,21 +267,27 @@ def read_source(cp, params, *, now_ns, cp_sha256=None, calibration=(), vehicle_s
|
||||
tune = cp.lateralTuning.torque
|
||||
if profile.basis != (float(tune.latAccelFactor), float(tune.latAccelOffset), float(tune.friction)):
|
||||
raise ValueError('incompatible saved tuning basis')
|
||||
# Scalar and geometry choices are immutable baseline inputs. Automatic
|
||||
# FF residuals never write them; a manually owned FLM surface stays fixed.
|
||||
manual = profile.flm is not None
|
||||
manual = (
|
||||
profile.factor.mode == 'custom'
|
||||
or profile.friction.mode == 'custom'
|
||||
or profile.proportional_gain.mode == 'custom'
|
||||
or profile.flm is not None
|
||||
or profile.geometry is not None
|
||||
and (profile.geometry.ratio.mode == 'custom' or profile.geometry.full_delay.mode == 'custom')
|
||||
)
|
||||
if profile.geometry is not None and profile.geometry.learning == 'force_off':
|
||||
preference = type(preference)(False, preference.valid, preference.source, preference.raw)
|
||||
else:
|
||||
# Validate legacy scalar choices and bind their exact bytes to the source
|
||||
# epoch. They stay fixed baseline inputs, not automatic profile ownership.
|
||||
# Legacy numeric manual intent preempts optional learning. False legacy
|
||||
# switches themselves remain source-bound, but are not numeric overrides.
|
||||
if cp.lateralTuning.which() == 'torque':
|
||||
tune = cp.lateralTuning.torque
|
||||
interpret_legacy(
|
||||
choice = interpret_legacy(
|
||||
{key: raw_by_key[key] for key in LEGACY_KEYS},
|
||||
(float(tune.latAccelFactor), float(tune.latAccelOffset), float(tune.friction)),
|
||||
fingerprint=str(cp.carFingerprint),
|
||||
)
|
||||
manual = choice.factor is not None or choice.friction is not None
|
||||
calibration = tuple(calibration)
|
||||
if len(calibration) != 3 or not all(math.isfinite(v) for v in calibration):
|
||||
raise ValueError("uncertain calibration")
|
||||
|
||||
@@ -44,11 +44,9 @@ def run(
|
||||
cached_reports = {}
|
||||
prior_attached = {}
|
||||
previous_publish = publication_sequence = 0
|
||||
unavailable_reason = 'no_current_controller_evidence'
|
||||
|
||||
def clear(reason):
|
||||
nonlocal collector, raw_context, active_key, pending_key, pending_token, unavailable_reason
|
||||
unavailable_reason = reason
|
||||
nonlocal collector, raw_context, active_key, pending_key, pending_token
|
||||
if collector is not None:
|
||||
collector.split(reason, reset=True)
|
||||
collector = None
|
||||
@@ -75,12 +73,10 @@ def run(
|
||||
now = clock()
|
||||
source = source_reader(now)
|
||||
if not source.enabled or source.manual or not source.observed_ns <= now <= source.expires_ns:
|
||||
clear('learning_disabled' if not source.enabled else 'manual_profile_override' if source.manual else 'settings_receipt_expired')
|
||||
elif collector is None:
|
||||
unavailable_reason = 'no_current_controller_evidence'
|
||||
clear('current_settings_receipt_unavailable')
|
||||
|
||||
def control(event, current, stamp, *, source=source, now=now):
|
||||
nonlocal collector, raw_context, active_key, unavailable_reason
|
||||
nonlocal collector, raw_context, active_key
|
||||
if event.which() == 'starpilotLateralState' and event.starpilotLateralState.learningEvidence.version == 0:
|
||||
if collector is not None:
|
||||
collector.split('no_current_learning_evidence')
|
||||
@@ -92,7 +88,7 @@ def run(
|
||||
or evidence.source_epoch != source.source_epoch
|
||||
or not 0 < evidence.source_receipt_ns <= evidence.control_ns <= now <= source.expires_ns
|
||||
):
|
||||
clear('learning_disabled' if not source.enabled else 'manual_profile_override' if source.manual else 'controller_source_receipt_mismatch')
|
||||
clear('current_settings_receipt_unavailable')
|
||||
return
|
||||
incoming = (
|
||||
evidence.identity,
|
||||
@@ -121,7 +117,6 @@ def run(
|
||||
if collector is None:
|
||||
adapter = adapter_for_evidence(CP, evidence, active_key)
|
||||
if adapter is None:
|
||||
unavailable_reason = 'actual_adapter_unavailable'
|
||||
return
|
||||
collector = FullrateCollector(adapter)
|
||||
collector.adapter.select_objective(active_key)
|
||||
@@ -198,7 +193,7 @@ def run(
|
||||
else:
|
||||
message = messaging.new_message('autoSteeringLearning')
|
||||
value = message.autoSteeringLearning.learning
|
||||
value.version, value.status, value.reason, value.family = 1, 'unavailable', unavailable_reason, 'runtimeProfile'
|
||||
value.version, value.status, value.reason, value.family = 1, 'unavailable', 'actual_adapter_unavailable', 'runtimeProfile'
|
||||
value.observedMonoTime, value.validUntilMonoTime = now, now + 500_000_000
|
||||
message.valid = True
|
||||
write_source_receipt(message, source)
|
||||
|
||||
@@ -1,190 +0,0 @@
|
||||
"""Manual scalar fields stay fixed while the separate automatic FF profile learns."""
|
||||
from contextlib import contextmanager
|
||||
from dataclasses import replace
|
||||
import json
|
||||
import os
|
||||
from pathlib import Path
|
||||
from types import SimpleNamespace
|
||||
from unittest import mock
|
||||
|
||||
import pytest
|
||||
|
||||
from opendbc.car.car_helpers import interfaces
|
||||
from opendbc.car.hyundai.values import CAR
|
||||
from openpilot.common.params import Params
|
||||
from openpilot.common.prefix import OpenpilotPrefix
|
||||
from openpilot.selfdrive.controls.controlsd import Controls
|
||||
from openpilot.starpilot.flm.controller_replay import capture_snapshot
|
||||
from openpilot.starpilot.flm.learner_factory import from_evidence
|
||||
from openpilot.starpilot.flm.runtime_adapter_registry import read_source
|
||||
from openpilot.starpilot.flm.tests.test_registered_controls_global import published_cp, frame
|
||||
from openpilot.starpilot.flm.tests.test_registered_accepted_profile import computed_registered_fit, accepted_frame
|
||||
from openpilot.starpilot.lateral.controller_selection import DOCUMENT_KEY as CONTROLLER_KEY, ControllerMode, replace_mode
|
||||
from openpilot.starpilot.lateral.learning_preferences import KEY, replace_preference
|
||||
from openpilot.starpilot.lateral.torque_settings import DOCUMENT_KEY, FieldChoice, PlatformProfile, serialize_document
|
||||
|
||||
|
||||
BASE = 400_000_000_000
|
||||
|
||||
|
||||
def profile_for(cp, factor=True, friction=True):
|
||||
tune = cp.lateralTuning.torque
|
||||
basis = (float(tune.latAccelFactor), float(tune.latAccelOffset), float(tune.friction))
|
||||
return PlatformProfile(basis, FieldChoice('custom', basis[0] * 1.04) if factor else FieldChoice(),
|
||||
FieldChoice('custom', basis[2] * 1.10) if friction else FieldChoice())
|
||||
|
||||
|
||||
@contextmanager
|
||||
def fixed_controls(*, factor=True, friction=True, legacy=False, learning=True):
|
||||
cp = interfaces[CAR.HYUNDAI_IONIQ_6].get_non_essential_params(CAR.HYUNDAI_IONIQ_6)
|
||||
with (OpenpilotPrefix(),
|
||||
mock.patch.dict(os.environ, {'REPLAY': '1', 'SIMULATION': '0', 'TORQUE_REPLAY_RUNTIME': '0',
|
||||
'AOL_REPLAY_RUNTIME': '0', 'LANE_CENTERING_REPLAY_RUNTIME': '0'}),
|
||||
mock.patch('openpilot.selfdrive.controls.controlsd.messaging.PubMaster')):
|
||||
params = Params()
|
||||
selected = published_cp(cp, params)
|
||||
params.put('CarParams', selected.to_bytes(), block=True)
|
||||
params.put(KEY, replace_preference(None, cp, learning), block=True)
|
||||
params.put(CONTROLLER_KEY, json.loads(replace_mode(None, cp, ControllerMode.STARPILOT)), block=True)
|
||||
profile = profile_for(cp, factor, friction)
|
||||
if legacy:
|
||||
if factor:
|
||||
params.put('SteerLatAccel', profile.factor.custom_value, block=True)
|
||||
if friction:
|
||||
params.put('SteerFriction', profile.friction.custom_value, block=True)
|
||||
else:
|
||||
params.put(DOCUMENT_KEY, json.loads(serialize_document({str(cp.carFingerprint): profile})), block=True)
|
||||
yield Controls(), profile
|
||||
|
||||
|
||||
@pytest.mark.parametrize('factor,friction', ((True, False), (False, True), (True, True)))
|
||||
@pytest.mark.parametrize('legacy', (False, True))
|
||||
def test_manual_scalar_source_is_a_fixed_baseline_not_an_auto_profile_override(factor, friction, legacy):
|
||||
with fixed_controls(factor=factor, friction=friction, legacy=legacy) as (controls, profile):
|
||||
params = controls.params
|
||||
keys = (DOCUMENT_KEY, 'SteerLatAccel', 'SteerFriction', CONTROLLER_KEY)
|
||||
saved = {key: params.get(key) for key in keys}
|
||||
source = read_source(controls.CP, params, now_ns=BASE, calibration=(0., 0., 0.), vehicle_session='fixed-test')
|
||||
assert source.enabled and not source.manual and source.source_epoch
|
||||
for tick in range(100):
|
||||
frame(controls, BASE + tick * 10_000_000, tick)
|
||||
runtime = controls.global_learning
|
||||
assert runtime.eligible and runtime.descriptor.controller_mode == 'starpilot'
|
||||
assert runtime.descriptor.policy.startswith('ioniq6:')
|
||||
assert controls.LaC.controller_policy == 'ioniq6'
|
||||
assert {key: params.get(key) for key in keys} == saved
|
||||
if factor:
|
||||
assert controls.torque_host.selected.lat_accel_factor == profile.factor.custom_value
|
||||
if friction:
|
||||
assert controls.torque_host.selected.friction == profile.friction.custom_value
|
||||
# The independent old scalar learner remains disabled for the custom policy.
|
||||
assert not controls.torque_learning_allowed
|
||||
|
||||
|
||||
def test_automatic_profile_changes_output_without_writing_manual_fields_and_expires():
|
||||
with fixed_controls() as (baseline, _), fixed_controls() as (candidate, profile):
|
||||
for tick in range(105):
|
||||
now = BASE + tick * 10_000_000
|
||||
frame(baseline, now, tick)
|
||||
frame(candidate, now, tick)
|
||||
runtime = candidate.global_learning
|
||||
assert runtime.eligible and runtime.identity
|
||||
saved = candidate.params.get(DOCUMENT_KEY)
|
||||
fixed_torque = candidate.LaC.torque_params.to_dict()
|
||||
fixed_gain = [list(row) for row in candidate.LaC.pid._k_p]
|
||||
fixed_cp = candidate.CP.as_builder().to_bytes()
|
||||
assert candidate.torque_host.selected.lat_accel_factor == profile.factor.custom_value
|
||||
assert candidate.torque_host.selected.friction == profile.friction.custom_value
|
||||
result, validation = computed_registered_fit(candidate.CP, runtime, BASE + 1_050_000_000, mode='starpilot',
|
||||
baseline_snapshot=capture_snapshot(candidate.LaC, candidate.VM))
|
||||
changed = False
|
||||
for tick in range(105, 156):
|
||||
now = BASE + tick * 10_000_000
|
||||
reference, _ = frame(baseline, now, tick)
|
||||
actual, _ = accepted_frame(candidate, now, tick, result, validation)
|
||||
assert runtime.reason == 'qualified' and runtime.used
|
||||
assert runtime.outcome.status == 'applied'
|
||||
changed |= actual.actuators.torque != reference.actuators.torque
|
||||
assert candidate.LaC.torque_params.to_dict() == fixed_torque
|
||||
assert candidate.LaC.pid._k_p == fixed_gain
|
||||
assert candidate.CP.as_builder().to_bytes() == fixed_cp
|
||||
assert candidate.params.get(DOCUMENT_KEY) == saved
|
||||
assert actual.latActive == reference.latActive and actual.longActive == reference.longActive
|
||||
assert changed
|
||||
assert now == result.profile.expires_ns
|
||||
last_actual_stage = runtime.stage
|
||||
runtime.prepare(candidate.sm, now_ns=now + 1, active=True, healthy=True, controller=candidate.LaC,
|
||||
native_required=bool(candidate.aol_replay or candidate.ordinary_axis_ack_required),
|
||||
native_witness=candidate.flm_native_witness)
|
||||
assert runtime.torque_residual(last_actual_stage, stage_revision=runtime.descriptor.stage_revision) == (0.0, 0.0)
|
||||
assert runtime.applied_coordinates == (0.0, 0.0)
|
||||
assert candidate.LaC.torque_params.to_dict() == fixed_torque
|
||||
# Continue the real 100 Hz caller; duplicating a CarState timestamp is a
|
||||
# separate ordinary TorqueHost clock reset and must not be hidden here.
|
||||
accepted_frame(candidate, now + 10_000_000, 156, result, validation)
|
||||
assert runtime.applied_coordinates == (0.0, 0.0)
|
||||
assert candidate.LaC.torque_params.to_dict() == fixed_torque
|
||||
assert candidate.params.get(DOCUMENT_KEY) == saved
|
||||
|
||||
|
||||
def test_actual_controller_checkpoint_reconstructs_the_fixed_custom_baseline():
|
||||
with fixed_controls() as (controls, _):
|
||||
runtime = controls.global_learning
|
||||
for tick in range(151):
|
||||
now = BASE + tick * 10_000_000
|
||||
frame(controls, now, tick)
|
||||
if tick > 100 and runtime.snapshot and runtime.eligible:
|
||||
evidence = runtime.capture_evidence(control_ns=now + 1, state_ns=int(controls.sm.logMonoTime['selfdriveState']),
|
||||
active=True, limited=False)
|
||||
assert evidence is not None and evidence.replay_snapshot
|
||||
adapter = from_evidence(controls.CP, evidence)
|
||||
assert adapter is not None
|
||||
assert adapter.baseline_revision == runtime.baseline_revision
|
||||
assert adapter.identity == runtime.identity
|
||||
assert adapter.prepare_control(evidence) is evidence
|
||||
return
|
||||
raise pytest.fail.Exception('No source-bound fixed manual baseline checkpoint reached the real learner factory')
|
||||
|
||||
|
||||
@pytest.mark.parametrize('raw', (b'nan', b'99999', b'not-a-number'))
|
||||
def test_corrupt_legacy_scalar_settings_still_refuse_learning(raw):
|
||||
with fixed_controls(legacy=True) as (controls, _):
|
||||
Path(controls.params.get_param_path('SteerLatAccel')).write_bytes(raw)
|
||||
source = read_source(controls.CP, controls.params, now_ns=BASE, calibration=(0., 0., 0.), vehicle_session='fixed-test')
|
||||
assert not source.enabled and not source.source_epoch
|
||||
|
||||
|
||||
def test_editing_a_fixed_manual_value_changes_the_source_identity():
|
||||
with fixed_controls() as (controls, profile):
|
||||
before = read_source(controls.CP, controls.params, now_ns=BASE, calibration=(0., 0., 0.), vehicle_session='fixed-test')
|
||||
updated = replace(profile, factor=FieldChoice('custom', profile.factor.custom_value * 1.01))
|
||||
controls.params.put(DOCUMENT_KEY, json.loads(serialize_document({str(controls.CP.carFingerprint): updated})), block=True)
|
||||
after = read_source(controls.CP, controls.params, now_ns=BASE + 1, calibration=(0., 0., 0.), vehicle_session='fixed-test')
|
||||
assert before.enabled and after.enabled and not before.manual and not after.manual
|
||||
assert before.source_epoch != after.source_epoch
|
||||
|
||||
|
||||
def test_explicit_learning_off_still_blocks_the_custom_controller():
|
||||
with fixed_controls(learning=False) as (controls, _):
|
||||
for tick in range(30):
|
||||
frame(controls, BASE + tick * 10_000_000, tick)
|
||||
assert controls.global_learning.descriptor.controller_mode == 'starpilot'
|
||||
assert not controls.global_learning.eligible
|
||||
assert controls.global_learning.applied_coordinates == (0.0, 0.0)
|
||||
|
||||
|
||||
@pytest.mark.parametrize('location', ('controller', 'policy'))
|
||||
def test_explicit_manual_surface_still_blocks_automatic_profiles(location):
|
||||
with fixed_controls() as (controls, _):
|
||||
for tick in range(100):
|
||||
now = BASE + tick * 10_000_000
|
||||
frame(controls, now, tick)
|
||||
runtime = controls.global_learning
|
||||
assert runtime.eligible
|
||||
target = controls.LaC if location == 'controller' else controls.LaC.starpilot_extension.policy
|
||||
attribute = 'flm_surface' if location == 'controller' else 'surface'
|
||||
with mock.patch.object(target, attribute, SimpleNamespace(), create=True):
|
||||
runtime.prepare(controls.sm, now_ns=now + 1, active=True, healthy=True, controller=controls.LaC,
|
||||
native_required=False)
|
||||
assert runtime.live_manual and not runtime.eligible
|
||||
assert runtime.applied_coordinates == (0.0, 0.0)
|
||||
@@ -1,28 +0,0 @@
|
||||
import random
|
||||
import unittest
|
||||
|
||||
from openpilot.starpilot.flm.fullrate_collector import identity
|
||||
|
||||
|
||||
class TestIdentity(unittest.TestCase):
|
||||
def test_exact_lowercase_hex_language(self):
|
||||
allowed = '0123456789abcdef'
|
||||
base = 'a' * 64
|
||||
for code in range(256):
|
||||
character = chr(code)
|
||||
for position in range(64):
|
||||
value = base[:position] + character + base[position + 1:]
|
||||
self.assertIs(identity(value), character in allowed)
|
||||
randomizer = random.Random(64)
|
||||
for _ in range(1000):
|
||||
self.assertIs(identity(''.join(randomizer.choice(allowed) for _ in range(64))), True)
|
||||
|
||||
def test_strict_types_lengths_and_unicode(self):
|
||||
class StringSubclass(str):
|
||||
pass
|
||||
for value in (None, False, 0, b'a' * 64, bytearray(b'a' * 64), StringSubclass('a' * 64)):
|
||||
self.assertIs(identity(value), False)
|
||||
for length in (0, 1, 63, 65, 10000):
|
||||
self.assertIs(identity('a' * length), False)
|
||||
for character in ('\n', '\0', 'é', 'A', '𝟢', '٠', '\ud800'):
|
||||
self.assertIs(identity('a' * 63 + character), False)
|
||||
@@ -1,79 +0,0 @@
|
||||
"""Existing status publications must name settings blocks accurately."""
|
||||
from dataclasses import replace
|
||||
from types import SimpleNamespace
|
||||
from unittest import mock
|
||||
|
||||
import pytest
|
||||
|
||||
from openpilot.starpilot.flm import steerlearnerd
|
||||
from openpilot.starpilot.flm.runtime_adapter_registry import SourceState
|
||||
|
||||
|
||||
NOW = 1_000_000_000
|
||||
|
||||
|
||||
def publication(source, evidence=None, adapter=None, sources=None):
|
||||
sources = [source] if sources is None else sources
|
||||
sm = mock.Mock()
|
||||
sm.update.side_effect = [*([None] * len(sources)), StopIteration()]
|
||||
pm, fitter = mock.Mock(), mock.Mock()
|
||||
control_socket, applied_socket = object(), object()
|
||||
factory = mock.Mock(return_value=adapter)
|
||||
source_reader = mock.Mock(side_effect=sources)
|
||||
clock = mock.Mock(side_effect=[NOW + index * 250_000_000 for index in range(len(sources))])
|
||||
|
||||
def drain(socket, callback, receive, sink, now):
|
||||
if socket is control_socket and evidence is not None:
|
||||
event = SimpleNamespace(which=lambda: 'starpilotLateralState',
|
||||
starpilotLateralState=SimpleNamespace(learningEvidence=SimpleNamespace(version=1)))
|
||||
callback(event, now, now)
|
||||
|
||||
with mock.patch.object(steerlearnerd, 'drain', drain), mock.patch.object(steerlearnerd, 'read_control', return_value=evidence):
|
||||
with pytest.raises(StopIteration):
|
||||
steerlearnerd.run(SimpleNamespace(), sm, control_socket, applied_socket, pm, factory, mock.Mock(),
|
||||
source_reader, lambda item: (('feedforward', 'low', 'steady'),),
|
||||
lambda item: ('feedforward', 'low', 'steady'), mock.Mock(), mock.Mock(), fitter,
|
||||
mock.Mock(), clock=clock)
|
||||
fitter.close.assert_called_once_with('learner_parent_exit')
|
||||
factory_calls = factory.call_count
|
||||
message = pm.send.call_args.args[1]
|
||||
assert message.valid
|
||||
assert str(message.autoSteeringLearning.learning.status) == 'unavailable'
|
||||
assert message.autoSteeringLearning.learning.sourceReceipt.manual == sources[-1].manual
|
||||
return str(message.autoSteeringLearning.learning.reason), factory_calls
|
||||
|
||||
|
||||
def source():
|
||||
return SourceState(True, False, '0', 'a' * 64, NOW, NOW + 500_000_000, 'b' * 64, (0., 0., 0.))
|
||||
|
||||
|
||||
@pytest.mark.parametrize('change,reason', (({'enabled': False}, 'learning_disabled'),
|
||||
({'manual': True}, 'manual_profile_override'),
|
||||
({'observed_ns': NOW - 600_000_000, 'expires_ns': NOW - 100_000_000},
|
||||
'settings_receipt_expired')))
|
||||
def test_settings_block_is_reported_without_claiming_an_unsupported_controller(change, reason):
|
||||
actual, factory_calls = publication(replace(source(), **change))
|
||||
assert actual == reason
|
||||
assert factory_calls == 0
|
||||
|
||||
|
||||
def test_no_controller_evidence_is_distinct_from_adapter_rejection():
|
||||
reason, factory_calls = publication(source())
|
||||
assert reason == 'no_current_controller_evidence'
|
||||
assert factory_calls == 0
|
||||
|
||||
|
||||
def test_cleared_manual_block_does_not_leave_a_stale_status_reason():
|
||||
reason, factory_calls = publication(source(), sources=[replace(source(), manual=True), source()])
|
||||
assert reason == 'no_current_controller_evidence'
|
||||
assert factory_calls == 0
|
||||
|
||||
|
||||
def test_actual_adapter_unavailable_is_reserved_for_a_real_factory_refusal():
|
||||
evidence = SimpleNamespace(source_epoch='a' * 64, source_receipt_ns=NOW, control_ns=NOW,
|
||||
identity='c' * 64, generation=1, domain='normalized_torque', stage_revision='stage',
|
||||
response_revision='response', controller_mode='starpilot', policy='ioniq6',
|
||||
baseline_revision='baseline', controller_revision='controller', turn_assist=False)
|
||||
reason, factory_calls = publication(source(), evidence)
|
||||
assert reason == 'actual_adapter_unavailable'
|
||||
assert factory_calls == 1
|
||||
@@ -64,7 +64,7 @@ def refresh_registered_sources(ci, cs, stamp, tick):
|
||||
ci.CS.out = cs.as_reader()
|
||||
|
||||
|
||||
def computed_registered_fit(cp, runtime, observed_ns, *, mode="standard", baseline_snapshot=None):
|
||||
def computed_registered_fit(cp, runtime, observed_ns, *, mode="standard"):
|
||||
"""Fit independent plant, actual stage sensitivities, then exact heldout replay."""
|
||||
objective = Objective('feedforward', 'high', 'steady')
|
||||
# Predeclared independent plant goal: a bounded 2.5% FF reduction.
|
||||
@@ -112,9 +112,6 @@ def computed_registered_fit(cp, runtime, observed_ns, *, mode="standard", baseli
|
||||
for block in range(8):
|
||||
sign = 1 if block % 2 == 0 else -1
|
||||
controller, vm = factory.create()
|
||||
if baseline_snapshot is not None:
|
||||
from openpilot.starpilot.flm.controller_replay import restore_snapshot
|
||||
restore_snapshot(controller, vm, baseline_snapshot)
|
||||
params = SimpleNamespace(angleOffsetDeg=0.0, roll=0.0)
|
||||
# Parameters are independent left/right coordinates. Repeat the same
|
||||
# per-sign maneuver in each independent cohort so heldout improvement
|
||||
@@ -291,9 +288,7 @@ def test_registered_controls_computed_profile_is_used_and_revoked(vehicle, mode,
|
||||
frame(candidate, now, tick)
|
||||
assert candidate.params.check_key('AutomaticSteeringProfiles')
|
||||
assert candidate.params.get_type('AutomaticSteeringProfiles') == ParamKeyType.BYTES
|
||||
assert candidate.params.check_key('UseVOACC')
|
||||
assert candidate.params.check_key('DeveloperSidebar')
|
||||
assert len(candidate.params.all_keys()) == 347
|
||||
assert len(candidate.params.all_keys()) == 345
|
||||
runtime = candidate.global_learning
|
||||
assert runtime.identity and runtime.eligible
|
||||
assert candidate.sm['selfdriveState'].active and candidate.sm['selfdriveState'].enabled
|
||||
|
||||
@@ -1,220 +0,0 @@
|
||||
"""Source receipt leaf decoding and unchanged generic fallback contracts."""
|
||||
import math
|
||||
import struct
|
||||
from types import SimpleNamespace
|
||||
from typing import TypedDict
|
||||
import unittest
|
||||
|
||||
from capnp.lib.capnp import KjException, _DynamicStructReader
|
||||
from openpilot.cereal import custom, log, messaging
|
||||
from openpilot.starpilot.flm import learning_wire
|
||||
from openpilot.starpilot.flm.runtime_adapter_registry import SourceState
|
||||
|
||||
NOW = 10_000_000_000
|
||||
CP = 'c' * 64
|
||||
|
||||
|
||||
class ReceiptFields(TypedDict):
|
||||
version: int
|
||||
enabled: bool
|
||||
manual: bool
|
||||
resetGeneration: str
|
||||
sourceEpoch: str
|
||||
observedMonoTime: int
|
||||
validUntilMonoTime: int
|
||||
cpSha256: str
|
||||
calibrationBasis: list[float]
|
||||
|
||||
|
||||
FIELDS: ReceiptFields = {'version': 1, 'enabled': True, 'manual': False, 'resetGeneration': 'reset-1', 'sourceEpoch': 'b' * 64,
|
||||
'observedMonoTime': NOW, 'validUntilMonoTime': NOW + 500_000_000, 'cpSha256': CP,
|
||||
'calibrationBasis': [0.0, 0.0, 0.0]}
|
||||
VALID_LOG = [
|
||||
'event.valid', 'event.autoSteeringLearning', 'outer.learning', 'learning.sourceReceipt',
|
||||
'row.version', 'row.cpSha256', 'row.cpSha256', 'row.observedMonoTime',
|
||||
'row.validUntilMonoTime', 'row.observedMonoTime', 'row.enabled', 'row.sourceEpoch',
|
||||
'row.resetGeneration', 'row.resetGeneration', 'row.calibrationBasis', 'row.calibrationBasis',
|
||||
'row.enabled', 'row.manual', 'row.resetGeneration', 'row.sourceEpoch', 'row.observedMonoTime',
|
||||
'row.validUntilMonoTime', 'row.cpSha256', 'row.calibrationBasis',
|
||||
]
|
||||
|
||||
|
||||
def native(*, reader=True, **changes):
|
||||
event = messaging.new_message('autoSteeringLearning', valid=True, logMonoTime=NOW)
|
||||
event.autoSteeringLearning.learning.sourceReceipt = {**FIELDS, **changes}
|
||||
return event.as_reader() if reader else event
|
||||
|
||||
|
||||
def envelope(row, valid=True):
|
||||
return SimpleNamespace(valid=valid, autoSteeringLearning=SimpleNamespace(learning=SimpleNamespace(sourceReceipt=row)))
|
||||
|
||||
|
||||
def expected(**changes):
|
||||
row: ReceiptFields = {**FIELDS, **changes}
|
||||
return SourceState(row['enabled'], row['manual'], row['resetGeneration'], row['sourceEpoch'],
|
||||
row['observedMonoTime'], row['validUntilMonoTime'], row['cpSha256'], tuple(row['calibrationBasis']))
|
||||
|
||||
|
||||
def signature(value):
|
||||
if value is None:
|
||||
return None
|
||||
return (type(value), value.enabled, value.manual, value.reset, value.source_epoch,
|
||||
value.observed_ns, value.expires_ns, value.cp_sha256,
|
||||
tuple(struct.pack('!d', item) for item in value.calibration))
|
||||
|
||||
|
||||
class Logged:
|
||||
def __init__(self, values, accesses, label):
|
||||
self.values, self.accesses, self.label = values, accesses, label
|
||||
|
||||
def __getattr__(self, name):
|
||||
self.accesses.append(self.label + '.' + name)
|
||||
if name not in self.values:
|
||||
raise AttributeError(name)
|
||||
value = self.values[name]
|
||||
if isinstance(value, Exception):
|
||||
raise value
|
||||
return value() if callable(value) else value
|
||||
|
||||
|
||||
def traced(accesses, *, valid=True, **changes):
|
||||
row = Logged({**FIELDS, **changes}, accesses, 'row')
|
||||
learning = Logged({'sourceReceipt': row}, accesses, 'learning')
|
||||
outer = Logged({'learning': learning}, accesses, 'outer')
|
||||
return Logged({'valid': valid, 'autoSteeringLearning': outer}, accesses, 'event')
|
||||
|
||||
|
||||
class TestSourceReceipt(unittest.TestCase):
|
||||
def assert_receipt(self, changes, wanted, *, now=NOW, cp=CP):
|
||||
builder = native(reader=False, **changes)
|
||||
# Builders retain the original generic body; both paths must meet an explicit result.
|
||||
for event in (builder.as_reader(), builder):
|
||||
value = learning_wire.read_source_receipt(event, now_ns=now, cp_sha256=cp)
|
||||
self.assertEqual(signature(value), signature(wanted))
|
||||
return value
|
||||
|
||||
def test_native_schemas_and_enabled_manual_flags(self):
|
||||
event = native()
|
||||
row = event.autoSteeringLearning.learning.sourceReceipt
|
||||
self.assertIs(type(row), _DynamicStructReader)
|
||||
self.assertEqual(event.schema, log.Event.schema)
|
||||
self.assertEqual(event.autoSteeringLearning.schema, custom.StarPilotCarParams.schema)
|
||||
self.assertEqual(row.schema, custom.AutomaticLearningSourceReceipt.schema)
|
||||
for enabled, manual in ((True, False), (True, True), (False, False), (False, True)):
|
||||
with self.subTest(enabled=enabled, manual=manual):
|
||||
changes = {'enabled': enabled, 'manual': manual}
|
||||
self.assert_receipt(changes, expected(**changes))
|
||||
|
||||
def test_version_identity_reset_and_time_boundaries(self):
|
||||
rows = (
|
||||
{'version': 0}, {'version': 2}, {'cpSha256': 'd' * 64}, {'cpSha256': 'invalid'},
|
||||
{'sourceEpoch': ''}, {'sourceEpoch': 'B' * 64}, {'resetGeneration': ''},
|
||||
{'resetGeneration': 'r' * 65}, {'enabled': False, 'resetGeneration': 'r' * 65},
|
||||
{'observedMonoTime': 0}, {'observedMonoTime': NOW + 1},
|
||||
{'validUntilMonoTime': NOW - 1}, {'validUntilMonoTime': NOW + 500_000_001},
|
||||
)
|
||||
for changes in rows:
|
||||
with self.subTest(changes=changes):
|
||||
self.assert_receipt(changes, None)
|
||||
for changes in ({'enabled': False, 'sourceEpoch': '', 'resetGeneration': ''},
|
||||
{'resetGeneration': 'r' * 64}, {'validUntilMonoTime': NOW}):
|
||||
self.assert_receipt(changes, expected(**changes))
|
||||
for now, wanted in ((NOW - 1, None), (NOW, expected()),
|
||||
(NOW + 500_000_000, expected()), (NOW + 500_000_001, None)):
|
||||
with self.subTest(now=now):
|
||||
self.assert_receipt({}, wanted, now=now)
|
||||
|
||||
def test_calibration_lengths_nonfinite_and_signed_zero(self):
|
||||
for basis in ([], [0.0], [0.0, 0.0], [0.0] * 4, [math.nan, 0.0, 0.0],
|
||||
[0.0, math.inf, 0.0], [0.0, 0.0, -math.inf]):
|
||||
with self.subTest(basis=basis):
|
||||
self.assert_receipt({'calibrationBasis': basis}, None)
|
||||
changes = {'calibrationBasis': [-0.0, 0.0, -0.0]}
|
||||
self.assert_receipt(changes, expected(**changes))
|
||||
|
||||
def test_borrowed_reader_mutations_and_fresh_result_objects(self):
|
||||
builder = native(reader=False)
|
||||
borrowed = builder.as_reader()
|
||||
first = learning_wire.read_source_receipt(borrowed, now_ns=NOW, cp_sha256=CP)
|
||||
second = learning_wire.read_source_receipt(borrowed, now_ns=NOW, cp_sha256=CP)
|
||||
self.assertEqual(signature(first), signature(expected()))
|
||||
self.assertEqual(signature(second), signature(expected()))
|
||||
self.assertIsNot(first, second)
|
||||
rows = (('enabled', False), ('manual', True), ('version', 0), ('cpSha256', 'd' * 64),
|
||||
('sourceEpoch', 'e' * 64), ('resetGeneration', 'reset-2'),
|
||||
('observedMonoTime', NOW - 1), ('validUntilMonoTime', NOW + 1),
|
||||
('calibrationBasis', [-0.0, 0.25, -0.5]))
|
||||
for field, value in rows:
|
||||
with self.subTest(field=field):
|
||||
builder.autoSteeringLearning.learning.sourceReceipt = FIELDS
|
||||
setattr(builder.autoSteeringLearning.learning.sourceReceipt, field, value)
|
||||
wanted = None if field in ('version', 'cpSha256', 'observedMonoTime') else expected(**{field: value})
|
||||
actual = learning_wire.read_source_receipt(borrowed, now_ns=NOW, cp_sha256=CP)
|
||||
self.assertEqual(signature(actual), signature(wanted))
|
||||
builder.autoSteeringLearning.learning.sourceReceipt = FIELDS
|
||||
self.assertEqual(signature(learning_wire.read_source_receipt(borrowed, now_ns=NOW, cp_sha256=CP)), signature(expected()))
|
||||
|
||||
def test_custom_schema_spoof_and_wrong_native_schema_fallback(self):
|
||||
row = SimpleNamespace(**FIELDS, schema=custom.AutomaticLearningSourceReceipt.schema)
|
||||
actual = learning_wire.read_source_receipt(envelope(row), now_ns=NOW, cp_sha256=CP)
|
||||
self.assertEqual(signature(actual), signature(expected()))
|
||||
wrong = messaging.new_message('carState').as_reader().carState
|
||||
with self.assertRaises((AttributeError, ValueError, KjException)):
|
||||
learning_wire.read_source_receipt(envelope(wrong), now_ns=NOW, cp_sha256=CP)
|
||||
with self.assertRaises(AttributeError):
|
||||
learning_wire.read_source_receipt(envelope(None), now_ns=NOW, cp_sha256=CP)
|
||||
|
||||
def test_custom_getter_short_circuit_order_and_exception(self):
|
||||
cases = (({'valid': False}, 1), ({'version': 0}, 5), ({'cpSha256': 'd' * 64}, 6),
|
||||
({'observedMonoTime': NOW + 1}, 8), ({'sourceEpoch': ''}, 12),
|
||||
({'resetGeneration': ''}, 13), ({'calibrationBasis': []}, 15))
|
||||
for changes, length in cases:
|
||||
with self.subTest(changes=changes):
|
||||
accesses = []
|
||||
self.assertIsNone(learning_wire.read_source_receipt(traced(accesses, **changes), now_ns=NOW, cp_sha256=CP))
|
||||
self.assertEqual(accesses, VALID_LOG[:length])
|
||||
accesses = []
|
||||
with self.assertRaisesRegex(RuntimeError, '^manual getter$'):
|
||||
learning_wire.read_source_receipt(traced(accesses, manual=RuntimeError('manual getter')), now_ns=NOW, cp_sha256=CP)
|
||||
self.assertEqual(accesses, VALID_LOG[:18])
|
||||
|
||||
def test_stateful_custom_field_keeps_original_repeated_reads(self):
|
||||
accesses = []
|
||||
values = iter((CP, 'd' * 64, 'e' * 64))
|
||||
event = traced(accesses, cpSha256=lambda: next(values))
|
||||
actual = learning_wire.read_source_receipt(event, now_ns=NOW, cp_sha256=CP)
|
||||
self.assertEqual(signature(actual), signature(expected(cpSha256='e' * 64)))
|
||||
self.assertEqual(accesses, VALID_LOG)
|
||||
|
||||
def test_nonbuiltin_clock_and_cp_arguments(self):
|
||||
class IntSubclass(int):
|
||||
pass
|
||||
|
||||
class StrSubclass(str):
|
||||
pass
|
||||
|
||||
class Clock:
|
||||
def __init__(self, accesses):
|
||||
self.accesses = accesses
|
||||
|
||||
def __ge__(self, other):
|
||||
self.accesses.append(('ge', other))
|
||||
return NOW >= other
|
||||
|
||||
def __le__(self, other):
|
||||
self.accesses.append(('le', other))
|
||||
return NOW <= other
|
||||
|
||||
for now, cp, wanted in ((float(NOW), CP, expected()), (IntSubclass(NOW), CP, expected()),
|
||||
(True, CP, None), (0.0, CP, None), (-0.0, CP, None), (math.nan, CP, None),
|
||||
(NOW, StrSubclass(CP), expected()), (NOW, None, None)):
|
||||
with self.subTest(now=now, cp=cp):
|
||||
self.assert_receipt({}, wanted, now=now, cp=cp)
|
||||
accesses = []
|
||||
value = learning_wire.read_source_receipt(native(), now_ns=Clock(accesses), cp_sha256=CP)
|
||||
self.assertEqual(signature(value), signature(expected()))
|
||||
self.assertEqual(accesses, [('ge', NOW), ('le', NOW + 500_000_000)])
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
unittest.main()
|
||||
Some files were not shown because too many files have changed in this diff Show More
Reference in New Issue
Block a user