mirror of
https://github.com/firestar5683/StarPilot.git
synced 2026-10-28 00:53:45 +08:00
FLM Calibration Lifecycle
Keep bounded automatic learning collection through valid live calibration refinements. Retire generations on resets or invalid calibration, preserve live physical transforms and existing controller tunes, and validate fitted coefficient bounds before publication. Exercise the current controller and historical wire paths.
This commit is contained in:
firestar5683
@@ -1233,6 +1233,7 @@ struct AutomaticLearningSourceReceipt {
|
||||
validUntilMonoTime @6 :UInt64;
|
||||
cpSha256 @7 :Text;
|
||||
calibrationBasis @8 :List(Float64);
|
||||
calibrationGeneration @9 :Text;
|
||||
}
|
||||
|
||||
struct LearningApplication {
|
||||
|
||||
@@ -16136,17 +16136,17 @@ const ::capnp::_::RawSchema s_ccffb4ab144bf131 = {
|
||||
0, 2, i_ccffb4ab144bf131, nullptr, nullptr, { &s_ccffb4ab144bf131, nullptr, nullptr, 0, 0, nullptr }, false
|
||||
};
|
||||
#endif // !CAPNP_LITE
|
||||
static const ::capnp::_::AlignedData<168> b_9fe2d23e75805149 = {
|
||||
static const ::capnp::_::AlignedData<185> b_9fe2d23e75805149 = {
|
||||
{ 0, 0, 0, 0, 5, 0, 6, 0,
|
||||
73, 81, 128, 117, 62, 210, 226, 159,
|
||||
13, 0, 0, 0, 1, 0, 3, 0,
|
||||
89, 10, 85, 29, 102, 186, 38, 181,
|
||||
4, 0, 7, 0, 0, 0, 0, 0,
|
||||
5, 0, 7, 0, 0, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
21, 0, 0, 0, 98, 1, 0, 0,
|
||||
41, 0, 0, 0, 7, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
37, 0, 0, 0, 255, 1, 0, 0,
|
||||
37, 0, 0, 0, 55, 2, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
99, 117, 115, 116, 111, 109, 46, 99,
|
||||
@@ -16156,70 +16156,77 @@ static const ::capnp::_::AlignedData<168> b_9fe2d23e75805149 = {
|
||||
117, 114, 99, 101, 82, 101, 99, 101,
|
||||
105, 112, 116, 0, 0, 0, 0, 0,
|
||||
0, 0, 0, 0, 1, 0, 1, 0,
|
||||
36, 0, 0, 0, 3, 0, 4, 0,
|
||||
40, 0, 0, 0, 3, 0, 4, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
0, 0, 1, 0, 0, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
237, 0, 0, 0, 66, 0, 0, 0,
|
||||
9, 1, 0, 0, 66, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
232, 0, 0, 0, 3, 0, 1, 0,
|
||||
244, 0, 0, 0, 2, 0, 1, 0,
|
||||
4, 1, 0, 0, 3, 0, 1, 0,
|
||||
16, 1, 0, 0, 2, 0, 1, 0,
|
||||
1, 0, 0, 0, 16, 0, 0, 0,
|
||||
0, 0, 1, 0, 1, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
241, 0, 0, 0, 66, 0, 0, 0,
|
||||
13, 1, 0, 0, 66, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
236, 0, 0, 0, 3, 0, 1, 0,
|
||||
248, 0, 0, 0, 2, 0, 1, 0,
|
||||
8, 1, 0, 0, 3, 0, 1, 0,
|
||||
20, 1, 0, 0, 2, 0, 1, 0,
|
||||
2, 0, 0, 0, 17, 0, 0, 0,
|
||||
0, 0, 1, 0, 2, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
245, 0, 0, 0, 58, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
240, 0, 0, 0, 3, 0, 1, 0,
|
||||
252, 0, 0, 0, 2, 0, 1, 0,
|
||||
3, 0, 0, 0, 0, 0, 0, 0,
|
||||
0, 0, 1, 0, 3, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
249, 0, 0, 0, 130, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
248, 0, 0, 0, 3, 0, 1, 0,
|
||||
4, 1, 0, 0, 2, 0, 1, 0,
|
||||
4, 0, 0, 0, 1, 0, 0, 0,
|
||||
0, 0, 1, 0, 4, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
1, 1, 0, 0, 98, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
0, 1, 0, 0, 3, 0, 1, 0,
|
||||
12, 1, 0, 0, 2, 0, 1, 0,
|
||||
5, 0, 0, 0, 1, 0, 0, 0,
|
||||
0, 0, 1, 0, 5, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
9, 1, 0, 0, 138, 0, 0, 0,
|
||||
17, 1, 0, 0, 58, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
12, 1, 0, 0, 3, 0, 1, 0,
|
||||
24, 1, 0, 0, 2, 0, 1, 0,
|
||||
3, 0, 0, 0, 0, 0, 0, 0,
|
||||
0, 0, 1, 0, 3, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
21, 1, 0, 0, 130, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
20, 1, 0, 0, 3, 0, 1, 0,
|
||||
32, 1, 0, 0, 2, 0, 1, 0,
|
||||
4, 0, 0, 0, 1, 0, 0, 0,
|
||||
0, 0, 1, 0, 4, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
29, 1, 0, 0, 98, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
28, 1, 0, 0, 3, 0, 1, 0,
|
||||
40, 1, 0, 0, 2, 0, 1, 0,
|
||||
5, 0, 0, 0, 1, 0, 0, 0,
|
||||
0, 0, 1, 0, 5, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
37, 1, 0, 0, 138, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
40, 1, 0, 0, 3, 0, 1, 0,
|
||||
52, 1, 0, 0, 2, 0, 1, 0,
|
||||
6, 0, 0, 0, 2, 0, 0, 0,
|
||||
0, 0, 1, 0, 6, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
21, 1, 0, 0, 154, 0, 0, 0,
|
||||
49, 1, 0, 0, 154, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
24, 1, 0, 0, 3, 0, 1, 0,
|
||||
36, 1, 0, 0, 2, 0, 1, 0,
|
||||
52, 1, 0, 0, 3, 0, 1, 0,
|
||||
64, 1, 0, 0, 2, 0, 1, 0,
|
||||
7, 0, 0, 0, 2, 0, 0, 0,
|
||||
0, 0, 1, 0, 7, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
33, 1, 0, 0, 74, 0, 0, 0,
|
||||
61, 1, 0, 0, 74, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
32, 1, 0, 0, 3, 0, 1, 0,
|
||||
44, 1, 0, 0, 2, 0, 1, 0,
|
||||
60, 1, 0, 0, 3, 0, 1, 0,
|
||||
72, 1, 0, 0, 2, 0, 1, 0,
|
||||
8, 0, 0, 0, 3, 0, 0, 0,
|
||||
0, 0, 1, 0, 8, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
41, 1, 0, 0, 138, 0, 0, 0,
|
||||
69, 1, 0, 0, 138, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
44, 1, 0, 0, 3, 0, 1, 0,
|
||||
72, 1, 0, 0, 2, 0, 1, 0,
|
||||
72, 1, 0, 0, 3, 0, 1, 0,
|
||||
100, 1, 0, 0, 2, 0, 1, 0,
|
||||
9, 0, 0, 0, 4, 0, 0, 0,
|
||||
0, 0, 1, 0, 9, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
97, 1, 0, 0, 178, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
100, 1, 0, 0, 3, 0, 1, 0,
|
||||
112, 1, 0, 0, 2, 0, 1, 0,
|
||||
118, 101, 114, 115, 105, 111, 110, 0,
|
||||
7, 0, 0, 0, 0, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
@@ -16303,16 +16310,26 @@ static const ::capnp::_::AlignedData<168> b_9fe2d23e75805149 = {
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
14, 0, 0, 0, 0, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
99, 97, 108, 105, 98, 114, 97, 116,
|
||||
105, 111, 110, 71, 101, 110, 101, 114,
|
||||
97, 116, 105, 111, 110, 0, 0, 0,
|
||||
12, 0, 0, 0, 0, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
12, 0, 0, 0, 0, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0, }
|
||||
};
|
||||
::capnp::word const* const bp_9fe2d23e75805149 = b_9fe2d23e75805149.words;
|
||||
#if !CAPNP_LITE
|
||||
static const uint16_t m_9fe2d23e75805149[] = {8, 7, 1, 2, 5, 3, 4, 6, 0};
|
||||
static const uint16_t i_9fe2d23e75805149[] = {0, 1, 2, 3, 4, 5, 6, 7, 8};
|
||||
static const uint16_t m_9fe2d23e75805149[] = {8, 9, 7, 1, 2, 5, 3, 4, 6, 0};
|
||||
static const uint16_t i_9fe2d23e75805149[] = {0, 1, 2, 3, 4, 5, 6, 7, 8, 9};
|
||||
const ::capnp::_::RawSchema s_9fe2d23e75805149 = {
|
||||
0x9fe2d23e75805149, b_9fe2d23e75805149.words, 168, nullptr, m_9fe2d23e75805149,
|
||||
0, 9, i_9fe2d23e75805149, nullptr, nullptr, { &s_9fe2d23e75805149, nullptr, nullptr, 0, 0, nullptr }, false
|
||||
0x9fe2d23e75805149, b_9fe2d23e75805149.words, 185, nullptr, m_9fe2d23e75805149,
|
||||
0, 10, i_9fe2d23e75805149, nullptr, nullptr, { &s_9fe2d23e75805149, nullptr, nullptr, 0, 0, nullptr }, false
|
||||
};
|
||||
#endif // !CAPNP_LITE
|
||||
static const ::capnp::_::AlignedData<168> b_82b6c8ea6a6efabd = {
|
||||
|
||||
@@ -1502,7 +1502,7 @@ struct AutomaticLearningSourceReceipt {
|
||||
class Pipeline;
|
||||
|
||||
struct _capnpPrivate {
|
||||
CAPNP_DECLARE_STRUCT_HEADER(9fe2d23e75805149, 3, 4)
|
||||
CAPNP_DECLARE_STRUCT_HEADER(9fe2d23e75805149, 3, 5)
|
||||
#if !CAPNP_LITE
|
||||
static constexpr ::capnp::_::RawBrandedSchema const* brand() { return &schema->defaultBrand; }
|
||||
#endif // !CAPNP_LITE
|
||||
@@ -10792,6 +10792,9 @@ public:
|
||||
inline bool hasCalibrationBasis() const;
|
||||
inline ::capnp::List<double, ::capnp::Kind::PRIMITIVE>::Reader getCalibrationBasis() const;
|
||||
|
||||
inline bool hasCalibrationGeneration() const;
|
||||
inline ::capnp::Text::Reader getCalibrationGeneration() const;
|
||||
|
||||
private:
|
||||
::capnp::_::StructReader _reader;
|
||||
template <typename, ::capnp::Kind>
|
||||
@@ -10864,6 +10867,13 @@ public:
|
||||
inline void adoptCalibrationBasis(::capnp::Orphan< ::capnp::List<double, ::capnp::Kind::PRIMITIVE>>&& value);
|
||||
inline ::capnp::Orphan< ::capnp::List<double, ::capnp::Kind::PRIMITIVE>> disownCalibrationBasis();
|
||||
|
||||
inline bool hasCalibrationGeneration();
|
||||
inline ::capnp::Text::Builder getCalibrationGeneration();
|
||||
inline void setCalibrationGeneration( ::capnp::Text::Reader value);
|
||||
inline ::capnp::Text::Builder initCalibrationGeneration(unsigned int size);
|
||||
inline void adoptCalibrationGeneration(::capnp::Orphan< ::capnp::Text>&& value);
|
||||
inline ::capnp::Orphan< ::capnp::Text> disownCalibrationGeneration();
|
||||
|
||||
private:
|
||||
::capnp::_::StructBuilder _builder;
|
||||
template <typename, ::capnp::Kind>
|
||||
@@ -26365,6 +26375,40 @@ inline ::capnp::Orphan< ::capnp::List<double, ::capnp::Kind::PRIMITIVE>> Automa
|
||||
::capnp::bounded<3>() * ::capnp::POINTERS));
|
||||
}
|
||||
|
||||
inline bool AutomaticLearningSourceReceipt::Reader::hasCalibrationGeneration() const {
|
||||
return !_reader.getPointerField(
|
||||
::capnp::bounded<4>() * ::capnp::POINTERS).isNull();
|
||||
}
|
||||
inline bool AutomaticLearningSourceReceipt::Builder::hasCalibrationGeneration() {
|
||||
return !_builder.getPointerField(
|
||||
::capnp::bounded<4>() * ::capnp::POINTERS).isNull();
|
||||
}
|
||||
inline ::capnp::Text::Reader AutomaticLearningSourceReceipt::Reader::getCalibrationGeneration() const {
|
||||
return ::capnp::_::PointerHelpers< ::capnp::Text>::get(_reader.getPointerField(
|
||||
::capnp::bounded<4>() * ::capnp::POINTERS));
|
||||
}
|
||||
inline ::capnp::Text::Builder AutomaticLearningSourceReceipt::Builder::getCalibrationGeneration() {
|
||||
return ::capnp::_::PointerHelpers< ::capnp::Text>::get(_builder.getPointerField(
|
||||
::capnp::bounded<4>() * ::capnp::POINTERS));
|
||||
}
|
||||
inline void AutomaticLearningSourceReceipt::Builder::setCalibrationGeneration( ::capnp::Text::Reader value) {
|
||||
::capnp::_::PointerHelpers< ::capnp::Text>::set(_builder.getPointerField(
|
||||
::capnp::bounded<4>() * ::capnp::POINTERS), value);
|
||||
}
|
||||
inline ::capnp::Text::Builder AutomaticLearningSourceReceipt::Builder::initCalibrationGeneration(unsigned int size) {
|
||||
return ::capnp::_::PointerHelpers< ::capnp::Text>::init(_builder.getPointerField(
|
||||
::capnp::bounded<4>() * ::capnp::POINTERS), size);
|
||||
}
|
||||
inline void AutomaticLearningSourceReceipt::Builder::adoptCalibrationGeneration(
|
||||
::capnp::Orphan< ::capnp::Text>&& value) {
|
||||
::capnp::_::PointerHelpers< ::capnp::Text>::adopt(_builder.getPointerField(
|
||||
::capnp::bounded<4>() * ::capnp::POINTERS), kj::mv(value));
|
||||
}
|
||||
inline ::capnp::Orphan< ::capnp::Text> AutomaticLearningSourceReceipt::Builder::disownCalibrationGeneration() {
|
||||
return ::capnp::_::PointerHelpers< ::capnp::Text>::disown(_builder.getPointerField(
|
||||
::capnp::bounded<4>() * ::capnp::POINTERS));
|
||||
}
|
||||
|
||||
inline ::uint16_t LearningApplication::Reader::getVersion() const {
|
||||
return _reader.getDataField< ::uint16_t>(
|
||||
::capnp::bounded<0>() * ::capnp::ELEMENTS);
|
||||
|
||||
Binary file not shown.
Binary file not shown.
@@ -0,0 +1,109 @@
|
||||
"""Calibration estimates share a vehicle frame until the producer resets it."""
|
||||
import math
|
||||
import uuid
|
||||
|
||||
|
||||
def calibrated_basis(message):
|
||||
try:
|
||||
basis_values, spread_values = message.rpyCalib, message.rpyCalibSpread
|
||||
if len(basis_values) != 3 or len(spread_values) != 3:
|
||||
return None
|
||||
basis = tuple(float(v) for v in basis_values)
|
||||
blocks = message.validBlocks
|
||||
if type(blocks) is not int:
|
||||
return None
|
||||
spread = tuple(float(v) for v in spread_values)
|
||||
if (str(message.calStatus) != 'calibrated' or blocks < 5 or blocks > 50 or
|
||||
len(basis) != 3 or not all(math.isfinite(v) for v in basis) or
|
||||
len(spread) != 3 or not all(math.isfinite(v) and v >= 0 for v in spread) or
|
||||
spread[1] > math.radians(4) or spread[2] > math.radians(2)):
|
||||
return None
|
||||
return basis, blocks
|
||||
except (AttributeError, TypeError, ValueError, OverflowError):
|
||||
return None
|
||||
|
||||
|
||||
def generation_valid(value):
|
||||
return type(value) is str and len(value) == 32 and all(c in '0123456789abcdef' for c in value)
|
||||
|
||||
|
||||
class ProducerCalibrationLifecycle:
|
||||
def __init__(self, token_factory=lambda: uuid.uuid4().hex):
|
||||
self.token_factory = token_factory
|
||||
self.token = ''
|
||||
self.blocks = None
|
||||
self.issued = set()
|
||||
|
||||
def observe(self, message, *, current):
|
||||
value = calibrated_basis(message) if current else None
|
||||
if value is None:
|
||||
self.token = ''
|
||||
self.blocks = None
|
||||
return None
|
||||
basis, blocks = value
|
||||
if self.blocks is not None and blocks < self.blocks:
|
||||
self.token = ''
|
||||
if not self.token:
|
||||
if len(self.issued) >= 32:
|
||||
return None # Bounded reset history; never recycle an old generation.
|
||||
try:
|
||||
token = self.token_factory()
|
||||
except Exception:
|
||||
return None
|
||||
if not generation_valid(token) or token in self.issued:
|
||||
return None
|
||||
self.token = token
|
||||
self.issued.add(token)
|
||||
self.blocks = blocks
|
||||
return self.token, basis
|
||||
|
||||
|
||||
class ConsumerCalibrationLifecycle:
|
||||
def __init__(self):
|
||||
self.generation = ''
|
||||
self.blocks = None
|
||||
self.rejected = set()
|
||||
self.exhausted = False
|
||||
self.current = False
|
||||
self.last_invalid_ns = 0
|
||||
|
||||
def retire(self, generation):
|
||||
if generation and generation not in self.rejected:
|
||||
if len(self.rejected) >= 32:
|
||||
self.exhausted = True
|
||||
else:
|
||||
self.rejected.add(generation)
|
||||
|
||||
def capture(self, generation):
|
||||
if not generation_valid(generation):
|
||||
return False
|
||||
if self.generation and generation != self.generation:
|
||||
self.retire(self.generation)
|
||||
self.generation = generation
|
||||
return not self.exhausted and generation not in self.rejected
|
||||
|
||||
def reject(self, *, now_ns=0):
|
||||
self.retire(self.generation)
|
||||
if type(now_ns) is int and now_ns > self.last_invalid_ns:
|
||||
self.last_invalid_ns = now_ns
|
||||
self.blocks = None
|
||||
self.current = False
|
||||
|
||||
def observe(self, message, *, current, now_ns=0):
|
||||
value = calibrated_basis(message) if current else None
|
||||
if value is None:
|
||||
self.reject(now_ns=now_ns)
|
||||
return False
|
||||
_, blocks = value
|
||||
if self.blocks is not None and blocks < self.blocks:
|
||||
self.reject(now_ns=now_ns)
|
||||
self.blocks = blocks
|
||||
self.current = True
|
||||
return True
|
||||
|
||||
def admit(self, generation, observed_ns):
|
||||
if (not self.current or self.exhausted or not generation_valid(generation) or
|
||||
type(observed_ns) is not int or observed_ns <= self.last_invalid_ns or
|
||||
generation in self.rejected):
|
||||
return False
|
||||
return self.capture(generation)
|
||||
@@ -42,6 +42,8 @@ class GlobalRuntime:
|
||||
def __init__(self, cp, ci, controller):
|
||||
# Construct AFTER actual LaC selection, startup native/manual configuration.
|
||||
self.cp, self.ci, self.controller = cp, ci, controller
|
||||
from openpilot.starpilot.flm.calibration_lifecycle import ConsumerCalibrationLifecycle
|
||||
self._calibration_lifecycle = ConsumerCalibrationLifecycle()
|
||||
self.health_services = ('carState', 'modelV2', 'vehicleParameters', 'lateralDelay', 'driverMonitoringState', 'extrinsicsCalibration')
|
||||
try:
|
||||
self.descriptor = selected_descriptor(cp, ci, controller)
|
||||
@@ -114,6 +116,29 @@ class GlobalRuntime:
|
||||
"""Bounded current source/admission checks; invoked immediately before LaC."""
|
||||
self.used, self.stage, self.frame, self.eligible = False, None, None, False
|
||||
self.source = None
|
||||
# Capture a fresh V2 generation and calibration interruption before ANY
|
||||
# unrelated adapter/native/vehicle health gate can return.
|
||||
tracker = self._calibration_lifecycle
|
||||
early_source = None
|
||||
try:
|
||||
if (type(now_ns) is int and now_ns > 0 and self.descriptor is not None and
|
||||
sm.seen[SERVICE] and sm.alive[SERVICE] and sm.valid[SERVICE] and sm.freq_ok[SERVICE] and
|
||||
0 <= now_ns - int(sm.recv_time[SERVICE] * 1e9) <= SOURCE_LEASE_NS):
|
||||
envelope = sm[SERVICE]
|
||||
if envelope.learning.sourceReceipt.version == 2:
|
||||
early_source = read_source_receipt(SimpleNamespace(valid=True, autoSteeringLearning=envelope),
|
||||
now_ns=now_ns, cp_sha256=self.descriptor.cp_sha256)
|
||||
if early_source is not None and not tracker.generation:
|
||||
tracker.capture(early_source.calibration_generation)
|
||||
stamp = int(sm.logMonoTime['extrinsicsCalibration'])
|
||||
receipt = int(sm.recv_time['extrinsicsCalibration'] * 1e9)
|
||||
current_calibration = bool(type(now_ns) is int and now_ns > 0 and
|
||||
sm.all_checks(['extrinsicsCalibration']) and
|
||||
0 < stamp <= now_ns <= stamp + 500_000_000 and
|
||||
0 < receipt <= now_ns <= receipt + 500_000_000)
|
||||
tracker.observe(sm['extrinsicsCalibration'], current=current_calibration, now_ns=now_ns)
|
||||
except (AttributeError, KeyError, TypeError, ValueError, OverflowError):
|
||||
tracker.reject(now_ns=now_ns)
|
||||
self.outcome = None
|
||||
self.centre_frame = None
|
||||
self.centre_raw = ()
|
||||
@@ -167,6 +192,8 @@ class GlobalRuntime:
|
||||
stamp = int(sm.logMonoTime[service])
|
||||
receipt = int(sm.recv_time[service] * 1e9)
|
||||
if not (sm.seen[service] and 0 < stamp <= now_ns <= stamp + maximum and 0 < receipt <= now_ns <= receipt + maximum):
|
||||
if service == 'extrinsicsCalibration' and hasattr(self, '_calibration_lifecycle'):
|
||||
self._calibration_lifecycle.reject()
|
||||
raise ValueError('stale_original_source')
|
||||
if self.controller.sat_time > 0:
|
||||
raise ValueError('controller_saturation')
|
||||
@@ -177,7 +204,7 @@ class GlobalRuntime:
|
||||
raise ValueError('stale_receipt')
|
||||
envelope = sm[SERVICE]
|
||||
event = SimpleNamespace(valid=True, autoSteeringLearning=envelope)
|
||||
source = read_source_receipt(event, now_ns=now_ns, cp_sha256=self.descriptor.cp_sha256)
|
||||
source = early_source if early_source is not None else read_source_receipt(event, now_ns=now_ns, cp_sha256=self.descriptor.cp_sha256)
|
||||
if (
|
||||
source is None
|
||||
or source.cp_sha256 != self.descriptor.cp_sha256
|
||||
@@ -185,7 +212,11 @@ class GlobalRuntime:
|
||||
):
|
||||
raise ValueError('source_identity_or_clock')
|
||||
calibration = sm['extrinsicsCalibration']
|
||||
if str(calibration.calStatus) != 'calibrated' or source.calibration != tuple(float(value) for value in calibration.rpyCalib):
|
||||
if source.calibration_generation:
|
||||
if (len(source.calibration) != 3 or not all(math.isfinite(value) for value in source.calibration) or
|
||||
not tracker.admit(source.calibration_generation, source.observed_ns)):
|
||||
raise ValueError('calibration_identity_changed')
|
||||
elif str(calibration.calStatus) != 'calibrated' or source.calibration != tuple(float(value) for value in calibration.rpyCalib):
|
||||
raise ValueError('calibration_identity_changed')
|
||||
if not self.settings_cache.install(source, received_ns=received) or self.settings_cache.current(now_ns=now_ns) is None:
|
||||
raise ValueError('source_cache')
|
||||
|
||||
@@ -223,6 +223,11 @@ def result_message(result, session, sequence, now_ns, *, replay_validation=None)
|
||||
raise ValueError('profile bound')
|
||||
if any(not finite(p.excitation_floor) or p.excitation_floor <= 0 for p in profile.parameters):
|
||||
raise ValueError('profile excitation floor')
|
||||
if any(not all(finite(v) for v in (p.low, p.high, p.max_step, p.slew_per_s, coefficient))
|
||||
or p.low > p.high or p.max_step <= 0 or p.slew_per_s <= 0
|
||||
or not p.low <= coefficient <= p.high or abs(coefficient) > p.max_step
|
||||
for p, coefficient in zip(profile.parameters, profile.coefficients, strict=True)):
|
||||
raise ValueError('profile parameter bound')
|
||||
if not replay_accepts(replay_validation, profile):
|
||||
raise ValueError('actual replay validation required')
|
||||
value.status = 'qualifiedFit' if result.status == 'qualified' else 'rejected'
|
||||
@@ -331,10 +336,12 @@ def write_source_receipt(event, source):
|
||||
not identity(source.cp_sha256) or type(source.observed_ns) is not int or not 0 < source.observed_ns <= source.expires_ns <= source.observed_ns + 500_000_000
|
||||
):
|
||||
raise ValueError('source receipt bounds')
|
||||
if source.calibration_generation and not _calibration_generation(source.calibration_generation):
|
||||
raise ValueError('calibration lifecycle generation')
|
||||
if source.enabled and (not identity(source.source_epoch) or not source.reset):
|
||||
raise ValueError('enabled source receipt identity')
|
||||
event.autoSteeringLearning.learning.sourceReceipt = {
|
||||
'version': VERSION,
|
||||
'version': 2 if source.calibration_generation else VERSION,
|
||||
'enabled': source.enabled,
|
||||
'manual': source.manual,
|
||||
'resetGeneration': source.reset,
|
||||
@@ -343,11 +350,13 @@ def write_source_receipt(event, source):
|
||||
'validUntilMonoTime': source.expires_ns,
|
||||
'cpSha256': source.cp_sha256,
|
||||
'calibrationBasis': source.calibration,
|
||||
'calibrationGeneration': source.calibration_generation,
|
||||
}
|
||||
|
||||
|
||||
def _read_native_source_receipt(row, now_ns, cp_sha256, source_type):
|
||||
if row.version != VERSION:
|
||||
version = row.version
|
||||
if version not in (VERSION, 2):
|
||||
return None
|
||||
cp = str(row.cpSha256)
|
||||
if cp != cp_sha256 or not identity(cp):
|
||||
@@ -382,7 +391,14 @@ def _read_native_source_receipt(row, now_ns, cp_sha256, source_type):
|
||||
manual = bool(row.manual)
|
||||
if not enabled:
|
||||
epoch = str(row.sourceEpoch)
|
||||
return source_type(enabled, manual, reset, epoch, observed, expires, cp, tuple(calibration))
|
||||
generation = str(row.calibrationGeneration) if version == 2 else ''
|
||||
if version == 2 and not _calibration_generation(generation):
|
||||
return None
|
||||
return source_type(enabled, manual, reset, epoch, observed, expires, cp, tuple(calibration), generation)
|
||||
|
||||
|
||||
def _calibration_generation(value):
|
||||
return type(value) is str and len(value) == 32 and all(c in '0123456789abcdef' for c in value)
|
||||
|
||||
|
||||
def read_source_receipt(event, *, now_ns, cp_sha256):
|
||||
@@ -394,8 +410,11 @@ def read_source_receipt(event, *, now_ns, cp_sha256):
|
||||
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)
|
||||
version = row.version
|
||||
if version == 2:
|
||||
return _read_native_source_receipt(row, now_ns, cp_sha256, SourceState)
|
||||
if (
|
||||
row.version != VERSION
|
||||
version != VERSION
|
||||
or str(row.cpSha256) != cp_sha256
|
||||
or not identity(str(row.cpSha256))
|
||||
or not 0 < row.observedMonoTime <= now_ns <= row.validUntilMonoTime <= row.observedMonoTime + 500_000_000
|
||||
|
||||
@@ -235,9 +235,10 @@ class SourceState:
|
||||
expires_ns: int
|
||||
cp_sha256: str
|
||||
calibration: tuple[float, ...] = ()
|
||||
calibration_generation: str = ''
|
||||
|
||||
|
||||
def read_source(cp, params, *, now_ns, cp_sha256=None, calibration=(), vehicle_session=""):
|
||||
def read_source(cp, params, *, now_ns, cp_sha256=None, calibration=(), vehicle_session="", calibration_generation=''):
|
||||
"""Only startup/separate-process settings work; errors disable learning."""
|
||||
cp_sha256 = car_params_digest(cp) if cp_sha256 is None else cp_sha256
|
||||
try:
|
||||
@@ -294,8 +295,13 @@ def read_source(cp, params, *, now_ns, cp_sha256=None, calibration=(), vehicle_s
|
||||
unit = vehicle_session
|
||||
else:
|
||||
unit = vin
|
||||
epoch = digest((tuple((key, None if raw is None else raw.hex()) for key, raw in snapshots), calibration, unit))
|
||||
return SourceState(bool(preference.valid and preference.enabled), manual, reset, epoch, now_ns, now_ns + SOURCE_LEASE_NS, cp_sha256, calibration)
|
||||
if type(calibration_generation) is not str or (calibration_generation and (
|
||||
len(calibration_generation) != 32 or any(c not in '0123456789abcdef' for c in calibration_generation))):
|
||||
raise ValueError('invalid calibration lifecycle')
|
||||
frame_identity = calibration if not calibration_generation else ('live_vehicle_frame_v2', calibration_generation)
|
||||
epoch = digest((tuple((key, None if raw is None else raw.hex()) for key, raw in snapshots), frame_identity, unit))
|
||||
return SourceState(bool(preference.valid and preference.enabled), manual, reset, epoch, now_ns,
|
||||
now_ns + SOURCE_LEASE_NS, cp_sha256, calibration, calibration_generation)
|
||||
except (AttributeError, UnicodeError, TypeError, ValueError, OSError, OverflowError):
|
||||
return SourceState(False, False, '', '', now_ns, now_ns + SOURCE_LEASE_NS, cp_sha256)
|
||||
|
||||
@@ -464,12 +470,19 @@ def physical_observer(sm, cp, collector, now_ns):
|
||||
state_ns, delay_ns = int(sm.logMonoTime['carState']), int(sm.logMonoTime['lateralDelay'])
|
||||
if not 0 < state_ns <= now_ns or now_ns - state_ns > 100_000_000 or not 0 < delay_ns <= now_ns or now_ns - delay_ns > 500_000_000:
|
||||
return None
|
||||
from openpilot.starpilot.flm.calibration_lifecycle import calibrated_basis
|
||||
generation = digest(tuple(float(v) for v in calibration.rpyCalib))
|
||||
estimate = generation
|
||||
lifecycle = getattr(collector, '_calibration_generation', '')
|
||||
if lifecycle:
|
||||
if calibrated_basis(calibration) is None:
|
||||
return None
|
||||
generation = digest(('live_vehicle_frame_v2', lifecycle))
|
||||
cache = getattr(collector, '_physical_calibrator', None)
|
||||
if cache is None or cache[0] != generation:
|
||||
if cache is None or cache[0] != estimate:
|
||||
calibrator = PoseCalibrator()
|
||||
calibrator.feed_extrinsics_calibration(calibration)
|
||||
collector._physical_calibrator = (generation, calibrator)
|
||||
collector._physical_calibrator = (estimate, calibrator)
|
||||
calibrator = collector._physical_calibrator[1]
|
||||
if not calibrator.calib_valid:
|
||||
return None
|
||||
|
||||
@@ -125,6 +125,7 @@ def run(
|
||||
unavailable_reason = 'actual_adapter_unavailable'
|
||||
return
|
||||
collector = FullrateCollector(adapter)
|
||||
collector._calibration_generation = source.calibration_generation
|
||||
collector.adapter.select_objective(active_key)
|
||||
# This records primitive actual frames/checkpoints ONLY. No selected
|
||||
# controller update, baseline probes or numerical fitting at100Hz.
|
||||
@@ -260,26 +261,34 @@ def main():
|
||||
cp_sha256 = car_params_digest(CP)
|
||||
source_cache = [None]
|
||||
vehicle_session = uuid.uuid4().hex
|
||||
from openpilot.starpilot.flm.calibration_lifecycle import ProducerCalibrationLifecycle
|
||||
calibration_lifecycle = ProducerCalibrationLifecycle()
|
||||
|
||||
def calibration_basis():
|
||||
calibration = sm["extrinsicsCalibration"]
|
||||
if not (
|
||||
sm.seen["extrinsicsCalibration"]
|
||||
and sm.valid["extrinsicsCalibration"]
|
||||
and sm.alive["extrinsicsCalibration"]
|
||||
and str(calibration.calStatus) == "calibrated"
|
||||
):
|
||||
return ()
|
||||
return tuple(float(value) for value in calibration.rpyCalib)
|
||||
def calibration_frame(now):
|
||||
stamp = int(sm.logMonoTime['extrinsicsCalibration'])
|
||||
receipt = int(sm.recv_time['extrinsicsCalibration'] * 1e9)
|
||||
current = bool(sm.all_checks(['extrinsicsCalibration']) and
|
||||
0 < stamp <= now <= stamp + 500_000_000 and
|
||||
0 < receipt <= now <= receipt + 500_000_000)
|
||||
return calibration_lifecycle.observe(sm['extrinsicsCalibration'], current=current)
|
||||
|
||||
def current_source(now, frame):
|
||||
return read_source(CP, params, now_ns=now, cp_sha256=cp_sha256,
|
||||
calibration=() if frame is None else frame[1], vehicle_session=vehicle_session,
|
||||
calibration_generation='' if frame is None else frame[0])
|
||||
|
||||
def source_reader(now):
|
||||
# Read once each4Hz receipt renewal, outside all controller/torqued processes.
|
||||
if source_cache[0] is None or now - source_cache[0].observed_ns >= 250_000_000:
|
||||
source_cache[0] = read_source(CP, params, now_ns=now, cp_sha256=cp_sha256, calibration=calibration_basis(), vehicle_session=vehicle_session)
|
||||
# Observe resets on every loop, but keep settings reads at receipt-renewal rate.
|
||||
frame = calibration_frame(now)
|
||||
generation = '' if frame is None else frame[0]
|
||||
if (source_cache[0] is None or generation != source_cache[0].calibration_generation or
|
||||
now - source_cache[0].observed_ns >= 250_000_000):
|
||||
source_cache[0] = current_source(now, frame)
|
||||
return source_cache[0]
|
||||
|
||||
def current_sources(identity, epoch):
|
||||
current = read_source(CP, params, now_ns=time.monotonic_ns(), cp_sha256=cp_sha256, calibration=calibration_basis(), vehicle_session=vehicle_session)
|
||||
now = time.monotonic_ns()
|
||||
current = current_source(now, calibration_frame(now))
|
||||
return (identity if persistent_unit and epoch == current.source_epoch else '', current.enabled, current.manual, current.reset)
|
||||
|
||||
persistence = PersistenceModule(params, current_sources)
|
||||
|
||||
@@ -1,225 +1,130 @@
|
||||
"""Composed automatic fit application; synthetic evidence, actual owners and wire."""
|
||||
|
||||
from contextlib import contextmanager
|
||||
"""Independent physical fit/replay into current bounded Controls learning stages."""
|
||||
from dataclasses import replace
|
||||
import json
|
||||
import math
|
||||
import os
|
||||
from pathlib import Path
|
||||
import struct
|
||||
import unittest
|
||||
from unittest import mock
|
||||
|
||||
from opendbc.car.car_helpers import interfaces
|
||||
from opendbc.car.gm.values import CAR as GM
|
||||
from opendbc.car.hyundai.values import CAR as HKG
|
||||
from openpilot.cereal import messaging
|
||||
from openpilot.common.params import Params
|
||||
from openpilot.common.prefix import OpenpilotPrefix
|
||||
from openpilot.selfdrive.controls.controlsd import Controls
|
||||
from openpilot.starpilot.flm.automatic import identity_digest
|
||||
from openpilot.starpilot.flm.learning_producer import AutomaticLearningProducer
|
||||
from openpilot.starpilot.flm.learning_source import (
|
||||
AutomaticLearningConsumer, MAX_AGE_NS, RESET_KEY, SERVICE, learning_payload,
|
||||
)
|
||||
from openpilot.starpilot.flm.tests import test_automatic as evidence
|
||||
from openpilot.starpilot.lateral.controller_selection import ControllerMode, DOCUMENT_KEY as CONTROLLER_KEY, policy_for, replace_mode
|
||||
from opendbc.car.toyota.values import CAR
|
||||
from openpilot.starpilot.flm.learning_wire import result_message
|
||||
from openpilot.starpilot.flm.tests.test_registered_controls_global import controls_for
|
||||
from openpilot.starpilot.flm.tests.test_registered_accepted_profile import computed_registered_fit
|
||||
from openpilot.starpilot.flm.tests.test_live_calibration_paths import live_frame, BASE
|
||||
from openpilot.starpilot.lateral.learning_preferences import KEY as PREFERENCE_KEY, replace_preference
|
||||
from openpilot.starpilot.lateral.tests.test_lane_runtime import feed, publish_corolla_params
|
||||
from openpilot.starpilot.lateral.torque_settings import DOCUMENT_KEY as TORQUE_KEY, replace_field, serialize_document
|
||||
|
||||
NOW = 400_000_000_000
|
||||
|
||||
def current_fit(controls):
|
||||
for tick in range(30):
|
||||
live_frame(controls, tick, 'a' * 32)
|
||||
return computed_registered_fit(controls.CP, controls.global_learning, BASE + 300_000_000)
|
||||
|
||||
|
||||
def float32(value):
|
||||
"""The controller's CarParams torque fields are Float32, not Python doubles."""
|
||||
return struct.unpack('<f', struct.pack('<f', value))[0]
|
||||
|
||||
|
||||
class AbsentCollection:
|
||||
"""Only exercise the real Params/identity refresh; no fabricated pose admission."""
|
||||
def all_checks(self, _services):
|
||||
return False
|
||||
|
||||
|
||||
class JoinedLearning:
|
||||
def __init__(self, test, controls):
|
||||
self.test, self.controls = test, controls
|
||||
self.now, self.tick = NOW, 0
|
||||
self.producer = self.new_producer()
|
||||
self.last_publication = None
|
||||
self.wire_readers = []
|
||||
|
||||
def new_producer(self):
|
||||
producer = AutomaticLearningProducer(self.controls.CP, self.controls.params)
|
||||
with mock.patch('openpilot.starpilot.flm.learning_producer.time.monotonic_ns', return_value=self.now), \
|
||||
mock.patch('openpilot.starpilot.flm.learning_producer.time.clock_gettime_ns', return_value=self.now):
|
||||
producer.update(AbsentCollection(), None)
|
||||
self.test.assertEqual(producer.optional_errors, 0)
|
||||
return producer
|
||||
|
||||
def fit(self, multiplier=1.05):
|
||||
identity = self.producer.identity
|
||||
self.test.assertIsNotNone(identity)
|
||||
factor = self.controls.CP.lateralTuning.torque.latAccelFactor * multiplier
|
||||
# Reuse the independently timed, direction/speed-balanced physical evidence
|
||||
# fixture. Bind it to the real current factory identity, not a fabricated fit.
|
||||
windows = []
|
||||
for window in evidence.windows():
|
||||
points = tuple(replace(point, lateral_accel=factor * point.applied_torque +
|
||||
math.copysign(.02, point.applied_torque)) for point in window.response)
|
||||
rows = tuple(replace(row, actual_la=point.lateral_accel, desired_la=point.lateral_accel)
|
||||
for row, point in zip(window.controls, points, strict=True))
|
||||
windows.append(replace(window, identity_sha256=identity_digest(identity), controls=rows, response=points))
|
||||
self.producer.windows.extend(windows)
|
||||
self.producer.block = len(windows)
|
||||
self.producer.evaluate(self.now)
|
||||
self.test.assertIsNotNone(self.producer.result)
|
||||
return self.producer.result
|
||||
|
||||
def step(self, *, publish=True, change=None, **health):
|
||||
controls = self.controls
|
||||
feed(controls, self.now, self.tick, **health)
|
||||
if publish and self.tick % 25 == 0:
|
||||
with mock.patch('openpilot.starpilot.flm.learning_producer.time.monotonic_ns', return_value=self.now):
|
||||
event = self.producer.get_msg()
|
||||
self.test.assertIsNotNone(event)
|
||||
self.test.assertTrue(event.valid, 'The real producer must mark its usable wire valid')
|
||||
if change is not None:
|
||||
change(learning_payload(event.autoSteeringLearning))
|
||||
reader = messaging.log_from_bytes(event.to_bytes())
|
||||
self.wire_readers.append(reader)
|
||||
controls.sm.update_msgs(self.now / 1e9, [reader])
|
||||
self.last_publication = self.now
|
||||
with mock.patch('openpilot.selfdrive.controls.controlsd.time.monotonic_ns', return_value=self.now):
|
||||
command, _ = controls.state_control()
|
||||
self.now += 10_000_000
|
||||
self.tick += 1
|
||||
return command
|
||||
|
||||
def drive(self, ticks, **kwargs):
|
||||
for _ in range(ticks):
|
||||
command = self.step(**kwargs)
|
||||
return command
|
||||
|
||||
|
||||
@contextmanager
|
||||
def joined(test, *, native_ack=False):
|
||||
with OpenpilotPrefix(), mock.patch.dict(os.environ, {'REPLAY': '1', 'TORQUE_REPLAY_RUNTIME': '0',
|
||||
'AOL_REPLAY_RUNTIME': '1' if native_ack else '0', 'LANE_CENTERING_REPLAY_RUNTIME': '0',
|
||||
'SIMULATION': '0'}), \
|
||||
mock.patch('openpilot.selfdrive.controls.controlsd.messaging.PubMaster'):
|
||||
params = Params()
|
||||
cp = publish_corolla_params(params)
|
||||
params.put(CONTROLLER_KEY, json.loads(replace_mode(None, cp, ControllerMode.STANDARD)), block=True)
|
||||
params.put(PREFERENCE_KEY, replace_preference(None, cp, True), block=True)
|
||||
controls = Controls()
|
||||
test.assertTrue(controls.automatic_learning.supported)
|
||||
test.assertTrue(controls.torque_learning_allowed)
|
||||
test.assertIsNone(controls.torque_host)
|
||||
yield JoinedLearning(test, controls)
|
||||
def apply_fit(controls, result, validation):
|
||||
command = None
|
||||
for tick in range(30, 71):
|
||||
command, _ = live_frame(controls, tick, 'a' * 32, result=result, validation=validation)
|
||||
return command
|
||||
|
||||
|
||||
class TestAutomaticLearningIntegration(unittest.TestCase):
|
||||
def params(self):
|
||||
return interfaces[CAR.TOYOTA_RAV4_TSS2].get_non_essential_params(CAR.TOYOTA_RAV4_TSS2)
|
||||
|
||||
def test_produced_fit_reaches_actual_controls_once_without_manual_document(self):
|
||||
with joined(self) as run:
|
||||
controls = run.controls
|
||||
with controls_for(self.params(), 'standard', True) as controls:
|
||||
before_cp = controls.CP.as_builder().to_bytes()
|
||||
basis = controls.CP.lateralTuning.torque
|
||||
offset, friction = basis.latAccelOffset, basis.friction
|
||||
command = run.drive(50)
|
||||
tune = controls.LaC.torque_params.to_dict()
|
||||
result, validation = current_fit(controls)
|
||||
self.assertIsNotNone(result.profile)
|
||||
self.assertIsNotNone(validation)
|
||||
command = apply_fit(controls, result, validation)
|
||||
runtime = controls.global_learning
|
||||
self.assertTrue(command.latActive)
|
||||
self.assertIsNone(controls.torque_host, 'Collecting alone must not instantiate an application owner')
|
||||
result = run.fit()
|
||||
self.assertEqual(result.status, 'qualified_fit')
|
||||
native_factor = controls.LaC.torque_params.latAccelFactor
|
||||
command = run.drive(100)
|
||||
self.assertTrue(controls.sm.all_checks([SERVICE]))
|
||||
self.assertFalse(controls.sm.simulation, 'Exercise actual frequency tracking, not simulation relaxation')
|
||||
host = controls.torque_host
|
||||
self.assertIsNotNone(host)
|
||||
self.assertIs(host.applied_controller, controls.LaC)
|
||||
self.assertGreater(controls.LaC.torque_params.latAccelFactor, native_factor)
|
||||
self.assertEqual(controls.LaC.torque_params.latAccelFactor, float32(dict(result.candidate)['latAccelFactor']))
|
||||
self.assertEqual(controls.LaC.torque_params.latAccelOffset, offset)
|
||||
self.assertEqual(controls.LaC.torque_params.friction, friction)
|
||||
self.assertTrue(runtime.used)
|
||||
self.assertEqual(runtime.reason, 'qualified')
|
||||
self.assertTrue(any(value != 0 for value in runtime.applied_coordinates))
|
||||
owner = runtime.torque_owner
|
||||
for tick in range(71, 80):
|
||||
live_frame(controls, tick, 'a' * 32, result=result, validation=validation)
|
||||
self.assertIs(runtime.torque_owner, owner, 'One stage owner survives subsequent receipts')
|
||||
self.assertEqual(controls.LaC.torque_params.to_dict(), tune)
|
||||
self.assertEqual(controls.CP.as_builder().to_bytes(), before_cp)
|
||||
self.assertIsNone(controls.params.get(TORQUE_KEY))
|
||||
self.assertTrue(command.latActive)
|
||||
run.drive(30)
|
||||
self.assertIs(controls.torque_host, host, 'One controller parameter writer must survive subsequent publications')
|
||||
self.assertFalse(controls.sm.simulation)
|
||||
self.assertTrue(controls.sm.all_checks(['autoSteeringLearning']))
|
||||
|
||||
def test_actual_application_expires_and_reset_restart_or_manual_choice_falls_back(self):
|
||||
for reason in ('stale', 'restart', 'off', 'corrupt', 'reset', 'manual'):
|
||||
with self.subTest(reason=reason), joined(self) as run:
|
||||
controls = run.controls
|
||||
self.assertEqual(run.fit().status, 'qualified_fit')
|
||||
run.drive(100)
|
||||
host = controls.torque_host
|
||||
self.assertIsNotNone(host)
|
||||
base = controls.CP.lateralTuning.torque.latAccelFactor
|
||||
self.assertGreater(controls.LaC.torque_params.latAccelFactor, base)
|
||||
if reason == 'restart':
|
||||
previous_session = run.producer.session
|
||||
run.producer = run.new_producer()
|
||||
self.assertNotEqual(run.producer.session, previous_session)
|
||||
self.assertIsNone(run.producer.result)
|
||||
elif reason == 'off':
|
||||
with self.subTest(reason=reason), controls_for(self.params(), 'standard', True) as controls:
|
||||
result, validation = current_fit(controls)
|
||||
self.assertIsNotNone(result.profile)
|
||||
apply_fit(controls, result, validation)
|
||||
runtime = controls.global_learning
|
||||
self.assertTrue(any(value != 0 for value in runtime.applied_coordinates))
|
||||
before_cp, tune = controls.CP.as_builder().to_bytes(), controls.LaC.torque_params.to_dict()
|
||||
if reason == 'off':
|
||||
controls.params.put(PREFERENCE_KEY, replace_preference(None, controls.CP, False), block=True)
|
||||
elif reason == 'corrupt':
|
||||
Path(controls.params.get_param_path(PREFERENCE_KEY)).write_bytes(b'malformed')
|
||||
elif reason == 'reset':
|
||||
controls.params.put(RESET_KEY, 'a' * 32, block=True)
|
||||
controls.params.put('AutoSteeringLearningReset', 'c' * 32, block=True)
|
||||
elif reason == 'manual':
|
||||
basis = controls.CP.lateralTuning.torque
|
||||
profile = replace_field({}, str(controls.CP.carFingerprint),
|
||||
(basis.latAccelFactor, basis.latAccelOffset, basis.friction), 'factor', 'custom', base * 1.2)
|
||||
(basis.latAccelFactor, basis.latAccelOffset, basis.friction), 'factor', 'custom', basis.latAccelFactor * 1.2)
|
||||
controls.params.put(TORQUE_KEY, json.loads(serialize_document(profile)), block=True)
|
||||
command = run.drive(220, publish=reason != 'stale')
|
||||
if reason == 'stale':
|
||||
self.assertGreater(run.now - run.last_publication, MAX_AGE_NS)
|
||||
self.assertTrue(command.latActive, 'Learning source loss must not manufacture a driving fault')
|
||||
self.assertIs(controls.torque_host, host)
|
||||
self.assertEqual(controls.LaC.torque_params.latAccelFactor, float32(base * (1.2 if reason == 'manual' else 1.)))
|
||||
self.assertEqual(host.selected.source.value, 'user' if reason == 'manual' else host.native_base.source.value)
|
||||
if reason in ('reset', 'off', 'corrupt', 'manual'):
|
||||
run.producer = run.new_producer()
|
||||
self.assertIsNone(run.producer.result, 'Changed saved identity cannot recover an earlier process fit')
|
||||
if reason in ('off', 'corrupt', 'manual'):
|
||||
self.assertIsNone(run.producer.identity)
|
||||
for tick in range(71, 101):
|
||||
command, _ = live_frame(controls, tick, 'b' * 32 if reason == 'restart' else 'a' * 32,
|
||||
result=None if reason == 'restart' else result, validation=None if reason == 'restart' else validation,
|
||||
publish=False if reason == 'stale' else None)
|
||||
self.assertEqual(runtime.applied_coordinates, (0., 0.))
|
||||
self.assertTrue(command.latActive, 'Learning loss must not create a driving fault')
|
||||
self.assertEqual(controls.CP.as_builder().to_bytes(), before_cp)
|
||||
self.assertEqual(controls.LaC.torque_params.to_dict(), tune)
|
||||
if reason in ('reset', 'manual'):
|
||||
self.assertNotEqual(runtime.identity, result.profile.identity)
|
||||
|
||||
def test_real_controller_bounds_and_authority_are_independent_of_qualified_wire(self):
|
||||
with joined(self, native_ack=True) as run:
|
||||
controls = run.controls
|
||||
with controls_for(self.params(), 'standard', True) as controls:
|
||||
before_cp = controls.CP.as_builder().to_bytes()
|
||||
self.assertEqual(run.fit().status, 'qualified_fit')
|
||||
commands = [run.step() for _ in range(100)]
|
||||
self.assertIsNotNone(controls.torque_host)
|
||||
self.assertTrue(all(math.isfinite(command.actuators.torque) and
|
||||
abs(command.actuators.torque) <= controls.LaC.steer_max for command in commands))
|
||||
for health in ({'native_allowed': False}, {'can_valid': False}, {'can_timeout': True}, {'fault': True},
|
||||
{'active': False, 'enabled': False}):
|
||||
command = run.step(**health)
|
||||
result, validation = current_fit(controls)
|
||||
command = apply_fit(controls, result, validation)
|
||||
self.assertTrue(command.latActive)
|
||||
self.assertLessEqual(abs(command.actuators.torque), controls.LaC.steer_max)
|
||||
runtime = controls.global_learning
|
||||
# A fitted correction never supplies a missing native authority witness.
|
||||
runtime.prepare(controls.sm, now_ns=BASE + 700_000_000, active=True, healthy=True,
|
||||
native_required=True, native_witness=None)
|
||||
self.assertEqual(runtime.applied_coordinates, (0., 0.))
|
||||
inactive_integral = controls.LaC.pid.i
|
||||
for tick, health in enumerate(({'fault': True}, {'active': False, 'enabled': False}), 71):
|
||||
command, _ = live_frame(controls, tick, 'a' * 32, result=result, validation=validation, **health)
|
||||
self.assertFalse(command.latActive)
|
||||
self.assertEqual(command.actuators.torque, 0.)
|
||||
self.assertEqual(controls.LaC.pid.i, 0.)
|
||||
# A qualified sender cannot escape consumer factor or identity admission.
|
||||
for field, value in (('latAccelFactor', float('nan')), ('latAccelFactor', 100.), ('acceptancePolicyId', 'unknown')):
|
||||
while run.tick % 25:
|
||||
run.step()
|
||||
command = run.step(change=lambda state, key=field, new=value: setattr(state, key, new))
|
||||
self.assertEqual(controls.torque_host.selected, controls.torque_host.native_base)
|
||||
self.assertEqual(controls.LaC.pid.i, inactive_integral, 'Inactive baseline PID state must not advance')
|
||||
# A sender cannot escape stage bounds or validation identity.
|
||||
invalid = replace(result.profile, coefficients=(100., 100.))
|
||||
invalid_profiles = (
|
||||
invalid,
|
||||
replace(result.profile, coefficients=(math.nan, 0.)),
|
||||
*(replace(result.profile, parameters=(replace(result.profile.parameters[0], **changes),
|
||||
result.profile.parameters[1]))
|
||||
for changes in ({'low': math.inf}, {'low': 1., 'high': -1.}, {'max_step': 0.}, {'slew_per_s': 0.})),
|
||||
)
|
||||
for invalid in invalid_profiles:
|
||||
with self.subTest(invalid=invalid), self.assertRaises(ValueError):
|
||||
result_message(replace(result, profile=invalid), 'invalid-sender', 1, BASE + 750_000_000,
|
||||
replay_validation=replace(validation, profile=invalid))
|
||||
|
||||
def corrupt_wire(event):
|
||||
event.autoSteeringLearning.learning.runtimeProfile.coefficients = [100., 100.]
|
||||
|
||||
for tick in range(73, 80):
|
||||
command, _ = live_frame(controls, tick, 'a' * 32, result=result,
|
||||
validation=validation, publish=tick == 75, wire_mutator=corrupt_wire)
|
||||
self.assertTrue(math.isfinite(command.actuators.torque))
|
||||
self.assertEqual(controls.CP.as_builder().to_bytes(), before_cp, 'Automatic fitting must never change native safety configuration')
|
||||
for vehicle, mode in ((HKG.HYUNDAI_IONIQ_6, ControllerMode.STARPILOT),
|
||||
(GM.CHEVROLET_SILVERADO, ControllerMode.STANDARD)):
|
||||
with self.subTest(vehicle=vehicle), OpenpilotPrefix():
|
||||
cp = interfaces[vehicle].get_non_essential_params(vehicle)
|
||||
params = Params()
|
||||
if policy_for(cp) is not None:
|
||||
params.put(CONTROLLER_KEY, json.loads(replace_mode(None, cp, mode)), block=True)
|
||||
else:
|
||||
self.assertIsNone(params.get(CONTROLLER_KEY))
|
||||
params.put(PREFERENCE_KEY, replace_preference(None, cp, True), block=True)
|
||||
consumer = AutomaticLearningConsumer(cp, interfaces[vehicle](cp), mode, params)
|
||||
self.assertFalse(consumer.supported, 'Saved On cannot qualify a rich or fixed custom conversion')
|
||||
self.assertLessEqual(abs(command.actuators.torque), controls.LaC.steer_max)
|
||||
self.assertEqual(runtime.applied_coordinates, (0., 0.))
|
||||
self.assertEqual(controls.CP.as_builder().to_bytes(), before_cp)
|
||||
|
||||
@@ -9,9 +9,11 @@ import capnp
|
||||
|
||||
from openpilot.cereal import custom, log, messaging
|
||||
from openpilot.starpilot.flm.learning_source import SERVICE
|
||||
from openpilot.starpilot.flm.tests import test_automatic_integration as integration
|
||||
from tools.ci.custom_schema_contract import describe, validate
|
||||
|
||||
NOW = 400_000_000_000
|
||||
|
||||
|
||||
# Exact occupied prefix from c2109496a72e85ca702ecef7aff53ff45adcfb8c,
|
||||
# cereal/custom.capnp. No learner pointer existed in these recordings.
|
||||
HISTORICAL_CAR_PARAMS = """@0xb526ba661d550a59;
|
||||
@@ -134,7 +136,7 @@ class TestAutomaticLearningWire(unittest.TestCase):
|
||||
old_bytes = old.to_bytes()
|
||||
readers = []
|
||||
for tick in range(100):
|
||||
now = integration.NOW + tick * 10_000_000
|
||||
now = NOW + tick * 10_000_000
|
||||
if tick % 25 == 0:
|
||||
event = messaging.new_message(SERVICE, valid=True)
|
||||
with custom.StarPilotCarParams.from_bytes(old_bytes) as envelope:
|
||||
@@ -178,9 +180,9 @@ class TestAutomaticLearningWire(unittest.TestCase):
|
||||
self.assertIsNotNone(identity)
|
||||
old = historical.AutoSteeringLearning.new_message(
|
||||
version=1, status='qualifiedFit', identity=identity, session='transient-c204', sequence=1,
|
||||
observedMonoTime=integration.NOW, validUntilMonoTime=integration.NOW + 500_000_000,
|
||||
sourceWindowStartMonoTime=integration.NOW - 10_000_000_000,
|
||||
sourceWindowEndMonoTime=integration.NOW - 1_000_000_000,
|
||||
observedMonoTime=NOW, validUntilMonoTime=NOW + 500_000_000,
|
||||
sourceWindowStartMonoTime=NOW - 10_000_000_000,
|
||||
sourceWindowEndMonoTime=NOW - 1_000_000_000,
|
||||
family='linearFactor', latAccelFactor=native_factor * 1.05,
|
||||
acceptancePolicyId=next(iter(legacy.policies)),
|
||||
trainWindows=10, heldoutWindows=8, trainBaselineLoss=.1, trainCandidateLoss=.09,
|
||||
@@ -191,7 +193,7 @@ class TestAutomaticLearningWire(unittest.TestCase):
|
||||
old_bytes = old.to_bytes()
|
||||
readers = []
|
||||
for tick in range(50):
|
||||
now = integration.NOW + tick * 10_000_000
|
||||
now = NOW + tick * 10_000_000
|
||||
if tick % 25 == 0:
|
||||
event = messaging.new_message(SERVICE, valid=True)
|
||||
with custom.StarPilotCarParams.from_bytes(old_bytes) as envelope:
|
||||
|
||||
@@ -0,0 +1,263 @@
|
||||
"""Explicit V2 live calibrated-frame lifecycle; V1 exact-basis remains supported."""
|
||||
import math
|
||||
from types import SimpleNamespace
|
||||
|
||||
import pytest
|
||||
from openpilot.cereal import messaging
|
||||
from openpilot.starpilot.flm.calibration_lifecycle import ProducerCalibrationLifecycle, ConsumerCalibrationLifecycle
|
||||
from openpilot.starpilot.flm.learning_wire import write_source_receipt, read_source_receipt
|
||||
from openpilot.starpilot.flm.runtime_adapter_registry import SourceState, read_source
|
||||
from openpilot.starpilot.flm.tests.test_fixed_manual_baseline import fixed_controls, BASE
|
||||
|
||||
|
||||
def calibration(*, basis=(0., 0., 0.), blocks=50, status='calibrated', spread=(0., 0., 0.)):
|
||||
return SimpleNamespace(rpyCalib=basis, validBlocks=blocks, calStatus=status, rpyCalibSpread=spread)
|
||||
|
||||
|
||||
def admitted(consumer, message, generation):
|
||||
consumer.observe(message, current=True)
|
||||
return consumer.admit(generation, BASE)
|
||||
|
||||
|
||||
def producer():
|
||||
values = iter(('1' * 32, '2' * 32, '3' * 32))
|
||||
return ProducerCalibrationLifecycle(lambda: next(values))
|
||||
|
||||
|
||||
def test_live_estimate_drift_does_not_change_frame_generation():
|
||||
owner = producer()
|
||||
first = owner.observe(calibration(blocks=5), current=True)[0]
|
||||
for tick in range(1600):
|
||||
result = owner.observe(calibration(basis=(0., tick * 1e-6, -tick * 1e-6),
|
||||
blocks=min(50, 5 + tick // 100)), current=True)
|
||||
assert result[0] == first
|
||||
assert result[1][1] == tick * 1e-6 # Actual live estimate still carried.
|
||||
|
||||
|
||||
@pytest.mark.parametrize('message,current', [
|
||||
(calibration(status='recalibrating', blocks=1), True),
|
||||
(calibration(status='invalid'), True), (calibration(blocks=4), True),
|
||||
(calibration(basis=(float('nan'), 0., 0.)), True),
|
||||
(calibration(spread=(0., math.radians(4) + 1e-9, 0.)), True),
|
||||
(calibration(spread=(0., 0., math.radians(2) + 1e-9)), True),
|
||||
(calibration(), False),
|
||||
])
|
||||
def test_lifecycle_interruption_rotates_and_consumer_cannot_resume_old_generation(message, current):
|
||||
owner = producer()
|
||||
first = owner.observe(calibration(), current=True)[0]
|
||||
consumer = ConsumerCalibrationLifecycle()
|
||||
assert admitted(consumer, calibration(), first)
|
||||
assert owner.observe(message, current=current) is None
|
||||
if current:
|
||||
assert not admitted(consumer, message, first)
|
||||
else:
|
||||
consumer.reject() # Core original-source stale guard performs this.
|
||||
second = owner.observe(calibration(), current=True)[0]
|
||||
assert second != first
|
||||
assert not admitted(consumer, calibration(), first)
|
||||
assert admitted(consumer, calibration(), second)
|
||||
|
||||
|
||||
def test_monotonic_blocks_regression_is_material_reset_even_when_status_claims_calibrated():
|
||||
owner = producer()
|
||||
first = owner.observe(calibration(), current=True)[0]
|
||||
consumer = ConsumerCalibrationLifecycle()
|
||||
assert admitted(consumer, calibration(), first)
|
||||
next_frame = owner.observe(calibration(blocks=5), current=True)[0]
|
||||
assert next_frame != first
|
||||
assert not admitted(consumer, calibration(blocks=5), first)
|
||||
assert admitted(consumer, calibration(blocks=5), next_frame)
|
||||
|
||||
|
||||
def test_exact_calibrationd_spread_boundary_and_malformed_vectors():
|
||||
owner = producer()
|
||||
assert owner.observe(calibration(spread=(0., math.radians(4), math.radians(2))), current=True)
|
||||
assert owner.observe(calibration(spread=(0., -1., 0.)), current=True) is None
|
||||
assert owner.observe(calibration(blocks=51), current=True) is None
|
||||
|
||||
|
||||
def test_v1_exact_basis_and_v2_generation_identity_and_real_wire_roundtrip():
|
||||
with fixed_controls() as (controls, _):
|
||||
args = {'now_ns': BASE, 'vehicle_session': 'lifecycle-test'}
|
||||
first = read_source(controls.CP, controls.params, calibration=(0., 0., 0.), **args)
|
||||
drift = read_source(controls.CP, controls.params, calibration=(0., .001, 0.), **args)
|
||||
assert first.enabled and first.source_epoch != drift.source_epoch
|
||||
v2 = read_source(controls.CP, controls.params, calibration=(0., 0., 0.),
|
||||
calibration_generation='a' * 32, **args)
|
||||
updated = read_source(controls.CP, controls.params, calibration=(0., .001, 0.),
|
||||
calibration_generation='a' * 32, **args)
|
||||
new_drive = read_source(controls.CP, controls.params, calibration=(0., .001, 0.),
|
||||
calibration_generation='b' * 32, **args)
|
||||
assert v2.enabled and v2.source_epoch == updated.source_epoch
|
||||
assert v2.calibration != updated.calibration
|
||||
assert new_drive.source_epoch != v2.source_epoch
|
||||
for source, version in ((first, 1), (v2, 2)):
|
||||
event = messaging.new_message('autoSteeringLearning', valid=True)
|
||||
write_source_receipt(event, source)
|
||||
assert event.autoSteeringLearning.learning.sourceReceipt.version == version
|
||||
# Native serialized reader and original generic builder paths.
|
||||
with _reader(event) as reader:
|
||||
for value in (event, reader):
|
||||
assert read_source_receipt(value, now_ns=BASE, cp_sha256=source.cp_sha256) == source
|
||||
# Old lifecycle never attaches via current_sources(identity, old_epoch).
|
||||
assert v2.source_epoch != new_drive.source_epoch
|
||||
|
||||
|
||||
def _reader(event):
|
||||
from contextlib import nullcontext
|
||||
return nullcontext(messaging.log_from_bytes(event.to_bytes()))
|
||||
|
||||
|
||||
@pytest.mark.parametrize('generation', ['', 'x' * 32, 'A' * 32, 'a' * 31, 'a' * 33])
|
||||
def test_malformed_v2_generation_never_admits(generation):
|
||||
source = SourceState(True, False, '0', 'c' * 64, BASE, BASE + 500_000_000,
|
||||
'd' * 64, (0., 0., 0.), 'a' * 32)
|
||||
event = messaging.new_message('autoSteeringLearning', valid=True)
|
||||
write_source_receipt(event, source)
|
||||
event.autoSteeringLearning.learning.sourceReceipt.calibrationGeneration = generation
|
||||
for value in (event, messaging.log_from_bytes(event.to_bytes())):
|
||||
assert read_source_receipt(value, now_ns=BASE, cp_sha256=source.cp_sha256) is None
|
||||
|
||||
|
||||
def test_retired_generation_cannot_return_after_later_generation_also_resets():
|
||||
consumer = ConsumerCalibrationLifecycle()
|
||||
assert admitted(consumer, calibration(), 'a' * 32)
|
||||
consumer.reject()
|
||||
assert admitted(consumer, calibration(), 'b' * 32)
|
||||
consumer.reject()
|
||||
assert not admitted(consumer, calibration(), 'a' * 32)
|
||||
assert not admitted(consumer, calibration(), 'b' * 32)
|
||||
assert admitted(consumer, calibration(), 'c' * 32)
|
||||
|
||||
|
||||
@pytest.mark.parametrize('token', ['', 'a', 'A' * 32, 'x' * 32, None])
|
||||
def test_producer_cannot_publish_malformed_token(token):
|
||||
owner = ProducerCalibrationLifecycle(lambda: token)
|
||||
assert owner.observe(calibration(), current=True) is None
|
||||
|
||||
|
||||
def test_producer_cannot_recycle_retired_token():
|
||||
owner = ProducerCalibrationLifecycle(lambda: 'a' * 32)
|
||||
assert owner.observe(calibration(), current=True)
|
||||
owner.observe(calibration(status='invalid'), current=True)
|
||||
assert owner.observe(calibration(), current=True) is None
|
||||
|
||||
|
||||
@pytest.mark.parametrize('blocks', [True, 5.1, '5', None])
|
||||
def test_malformed_block_count_cannot_be_silently_coerced(blocks):
|
||||
assert producer().observe(calibration(blocks=blocks), current=True) is None
|
||||
|
||||
|
||||
def test_token_factory_error_is_fail_closed():
|
||||
def failed():
|
||||
raise RuntimeError('unavailable token')
|
||||
assert ProducerCalibrationLifecycle(failed).observe(calibration(), current=True) is None
|
||||
|
||||
|
||||
@pytest.mark.parametrize('generation', [None, 0, False, [], 'invalid'])
|
||||
def test_read_source_does_not_fall_back_to_v1_for_malformed_generation(generation):
|
||||
with fixed_controls() as (controls, _):
|
||||
source = read_source(controls.CP, controls.params, now_ns=BASE, calibration=(0., 0., 0.),
|
||||
vehicle_session='lifecycle-test', calibration_generation=generation)
|
||||
assert not source.enabled and not source.source_epoch
|
||||
|
||||
|
||||
def test_first_invalid_calibration_captures_fresh_unadmitted_generation():
|
||||
consumer = ConsumerCalibrationLifecycle()
|
||||
assert consumer.capture('a' * 32)
|
||||
consumer.observe(calibration(status='invalid'), current=True, now_ns=BASE)
|
||||
consumer.observe(calibration(), current=True, now_ns=BASE + 10_000_000)
|
||||
assert not consumer.admit('a' * 32, BASE + 1)
|
||||
assert consumer.admit('b' * 32, BASE + 1)
|
||||
|
||||
|
||||
def test_first_invalid_without_receipt_rejects_old_receipt_observation():
|
||||
consumer = ConsumerCalibrationLifecycle()
|
||||
consumer.observe(calibration(), current=False, now_ns=BASE)
|
||||
consumer.observe(calibration(), current=True, now_ns=BASE + 10_000_000)
|
||||
assert not consumer.admit('a' * 32, BASE)
|
||||
assert not consumer.admit('a' * 32, BASE - 1)
|
||||
assert consumer.admit('b' * 32, BASE + 1)
|
||||
|
||||
|
||||
def test_generation_switch_alone_retires_previous_token():
|
||||
consumer = ConsumerCalibrationLifecycle()
|
||||
consumer.observe(calibration(), current=True)
|
||||
assert consumer.admit('a' * 32, BASE)
|
||||
assert consumer.admit('b' * 32, BASE + 1)
|
||||
assert not consumer.admit('a' * 32, BASE + 2)
|
||||
|
||||
|
||||
def test_actual_core_first_invalid_receipt_then_same_generation_cannot_resume():
|
||||
from openpilot.starpilot.flm.tests.test_live_calibration_paths import live_frame
|
||||
|
||||
with fixed_controls() as (controls, _):
|
||||
runtime = controls.global_learning
|
||||
live_frame(controls, 0, 'a' * 32, status='recalibrating', blocks=1, healthy=False)
|
||||
assert 'a' * 32 in runtime._calibration_lifecycle.rejected
|
||||
for tick in range(1, 31):
|
||||
live_frame(controls, tick, 'a' * 32)
|
||||
assert not runtime.eligible
|
||||
assert not runtime._calibration_lifecycle.admit('a' * 32, 1_000_000_000_000)
|
||||
for tick in range(31, 91):
|
||||
live_frame(controls, tick, 'b' * 32)
|
||||
assert runtime._calibration_lifecycle.generation == 'b' * 32
|
||||
assert 'b' * 32 not in runtime._calibration_lifecycle.rejected
|
||||
assert runtime.eligible
|
||||
|
||||
|
||||
def test_actual_main_source_reader_reuses_lifecycle_for_cache_and_persistence():
|
||||
from unittest import mock
|
||||
from openpilot.starpilot.flm import steerlearnerd
|
||||
|
||||
with fixed_controls() as (controls, _):
|
||||
cp = controls.CP.as_builder()
|
||||
cp.carVin = 'KMHM54AA6PA000001'
|
||||
controls.params.put('CarParams', cp.to_bytes(), block=True)
|
||||
cal = calibration()
|
||||
sm = type('Sources', (), {
|
||||
'__getitem__': lambda self, key: cal,
|
||||
'all_checks': lambda self, keys: True,
|
||||
})()
|
||||
sm.logMonoTime = {'extrinsicsCalibration': BASE}
|
||||
sm.recv_time = {'extrinsicsCalibration': BASE / 1e9}
|
||||
with mock.patch.object(steerlearnerd, 'Params', return_value=controls.params), \
|
||||
mock.patch.object(steerlearnerd.messaging, 'SubMaster', return_value=sm), \
|
||||
mock.patch.object(steerlearnerd.messaging, 'sub_sock'), \
|
||||
mock.patch.object(steerlearnerd.messaging, 'PubMaster'), \
|
||||
mock.patch.object(steerlearnerd, 'run') as run, \
|
||||
mock.patch.object(steerlearnerd.os, 'sched_setscheduler', create=True), \
|
||||
mock.patch.object(steerlearnerd.os, 'nice'):
|
||||
steerlearnerd.main()
|
||||
source_reader, persistence = run.call_args.args[7], run.call_args.args[11]
|
||||
first = source_reader(BASE)
|
||||
assert first.enabled and first.calibration_generation
|
||||
cal.rpyCalib = (0., .001, -.001)
|
||||
sm.logMonoTime['extrinsicsCalibration'] = BASE + 250_000_000
|
||||
sm.recv_time['extrinsicsCalibration'] = (BASE + 250_000_000) / 1e9
|
||||
refined = source_reader(BASE + 250_000_000)
|
||||
assert refined.source_epoch == first.source_epoch
|
||||
assert refined.calibration != first.calibration
|
||||
with mock.patch.object(steerlearnerd.time, 'monotonic_ns', return_value=BASE + 250_000_000):
|
||||
assert persistence.current_sources('current-identity', first.source_epoch) == ('current-identity', True, False, first.reset)
|
||||
cal.calStatus, cal.validBlocks = 'recalibrating', 1
|
||||
assert not source_reader(BASE + 260_000_000).enabled
|
||||
cal.calStatus, cal.validBlocks = 'calibrated', 50
|
||||
fresh = source_reader(BASE + 270_000_000)
|
||||
assert fresh.enabled and fresh.calibration_generation != first.calibration_generation
|
||||
with mock.patch.object(steerlearnerd.time, 'monotonic_ns', return_value=BASE + 270_000_000):
|
||||
assert persistence.current_sources('old-identity', first.source_epoch)[0] == ''
|
||||
assert persistence.current_sources('fresh-identity', fresh.source_epoch)[0] == 'fresh-identity'
|
||||
|
||||
|
||||
def test_oversized_calibration_vectors_are_rejected_before_iteration():
|
||||
class Oversized:
|
||||
def __len__(self):
|
||||
return 1_000_000
|
||||
|
||||
def __iter__(self):
|
||||
raise AssertionError('unbounded calibration work entered the control loop')
|
||||
|
||||
owner = producer()
|
||||
assert owner.observe(calibration(basis=Oversized()), current=True) is None
|
||||
assert owner.observe(calibration(spread=Oversized()), current=True) is None
|
||||
@@ -0,0 +1,191 @@
|
||||
"""Actual Controls admission and physical observer with live calibration estimates."""
|
||||
from types import SimpleNamespace
|
||||
import math
|
||||
from unittest import mock
|
||||
|
||||
import pytest
|
||||
from opendbc.car.car_helpers import interfaces
|
||||
from opendbc.car.toyota.values import CAR
|
||||
from openpilot.cereal import messaging
|
||||
from openpilot.starpilot.flm.learning_wire import result_message, write_source_receipt
|
||||
from openpilot.starpilot.flm.runtime_adapter_registry import read_source, physical_observer
|
||||
from openpilot.starpilot.flm.tests.test_registered_controls_global import controls_for, feed
|
||||
|
||||
BASE = 600_000_000_000
|
||||
|
||||
|
||||
def live_frame(controls, tick, generation, *, status='calibrated', blocks=50,
|
||||
basis=(0., 0., 0.), healthy=True, publish=None, result=None, validation=None, wire_mutator=None, **health):
|
||||
now = BASE + tick * 10_000_000
|
||||
original_message = messaging.new_message
|
||||
|
||||
def packet(service, *args, **kwargs):
|
||||
event = original_message(service, *args, **kwargs)
|
||||
if service == 'carState':
|
||||
event.carState.steeringAngleDeg = math.degrees(controls.VM.get_steer_from_curvature(-.0002, 30., 0.))
|
||||
if service == 'extrinsicsCalibration':
|
||||
event.extrinsicsCalibration.calStatus = status
|
||||
event.extrinsicsCalibration.validBlocks = blocks
|
||||
event.extrinsicsCalibration.rpyCalib = basis
|
||||
event.extrinsicsCalibration.rpyCalibSpread = [0., .001, .001]
|
||||
return event
|
||||
|
||||
# Keep the real 4 Hz calibration/source cadence, rather than injecting a
|
||||
# second 100 Hz calibration stream that correctly fails frequency checks.
|
||||
with mock.patch.object(messaging, 'new_message', side_effect=packet):
|
||||
feed(controls, now, tick, speed=30., calibration=basis, **health)
|
||||
if publish is None:
|
||||
publish = tick % 25 == 0
|
||||
if publish:
|
||||
source = read_source(controls.CP, controls.params, now_ns=now, calibration=basis,
|
||||
calibration_generation=generation, vehicle_session='current-test-drive')
|
||||
event = result_message(result, 'calibration-lifecycle-regression', tick + 1, now, replay_validation=validation)
|
||||
event.logMonoTime = now
|
||||
write_source_receipt(event, source)
|
||||
if wire_mutator is not None:
|
||||
wire_mutator(event)
|
||||
controls.sm.update_msgs(now / 1e9, [messaging.log_from_bytes(event.to_bytes())])
|
||||
controls._test_calibration_source_generation = generation
|
||||
if not healthy:
|
||||
controls.global_learning.prepare(controls.sm, now_ns=now, active=True, healthy=False)
|
||||
return
|
||||
with mock.patch('openpilot.selfdrive.controls.controlsd.time.monotonic_ns', return_value=now):
|
||||
return controls.state_control()
|
||||
|
||||
|
||||
def test_actual_controls_keeps_identity_through_eighty_seconds_of_calibration_refinement():
|
||||
cp = interfaces[CAR.TOYOTA_RAV4_TSS2].get_non_essential_params(CAR.TOYOTA_RAV4_TSS2)
|
||||
with controls_for(cp, 'standard', True) as controls:
|
||||
identity = None
|
||||
tune = controls.LaC.torque_params.to_dict()
|
||||
for tick in range(8000):
|
||||
# Refine the estimate every five seconds, just as in the reported route.
|
||||
basis = (0., (tick // 500) * .00002, -(tick // 500) * .00002)
|
||||
live_frame(controls, tick, 'a' * 32, basis=basis, publish=tick % 25 == 0)
|
||||
runtime = controls.global_learning
|
||||
if tick >= 30:
|
||||
assert runtime.eligible and runtime.reason != 'qualified', (tick, runtime.reason, vars(runtime._calibration_lifecycle))
|
||||
identity = runtime.identity if identity is None else identity
|
||||
assert runtime.identity == identity
|
||||
assert controls.LaC.torque_params.to_dict() == tune
|
||||
assert controls.global_learning.applied_coordinates == (0., 0.)
|
||||
|
||||
|
||||
@pytest.mark.parametrize('initial', [True, False])
|
||||
def test_actual_core_rejects_old_generation_after_calibration_failure_even_on_other_health_return(initial):
|
||||
cp = interfaces[CAR.TOYOTA_RAV4_TSS2].get_non_essential_params(CAR.TOYOTA_RAV4_TSS2)
|
||||
with controls_for(cp, 'standard', True) as controls:
|
||||
if not initial:
|
||||
for tick in range(30):
|
||||
live_frame(controls, tick, 'a' * 32)
|
||||
start = 0 if initial else 30
|
||||
live_frame(controls, start, 'a' * 32, status='recalibrating', blocks=1, healthy=False)
|
||||
for tick in range(start + 1, start + 31):
|
||||
live_frame(controls, tick, 'a' * 32)
|
||||
assert not controls.global_learning.eligible
|
||||
for tick in range(start + 31, start + 91):
|
||||
live_frame(controls, tick, 'b' * 32)
|
||||
assert controls.global_learning.eligible, (controls.global_learning.reason, vars(controls.global_learning._calibration_lifecycle))
|
||||
live_frame(controls, start + 91, 'a' * 32, publish=True)
|
||||
assert not controls.global_learning.eligible
|
||||
|
||||
|
||||
def test_actual_core_rejects_calibration_block_regression_until_new_generation():
|
||||
cp = interfaces[CAR.TOYOTA_RAV4_TSS2].get_non_essential_params(CAR.TOYOTA_RAV4_TSS2)
|
||||
with controls_for(cp, 'standard', True) as controls:
|
||||
for tick in range(30):
|
||||
live_frame(controls, tick, 'a' * 32)
|
||||
live_frame(controls, 30, 'a' * 32, blocks=5)
|
||||
assert not controls.global_learning.eligible
|
||||
for tick in range(31, 201):
|
||||
live_frame(controls, tick, 'b' * 32, blocks=5)
|
||||
assert controls.global_learning.eligible, (controls.global_learning.reason, vars(controls.global_learning._calibration_lifecycle))
|
||||
|
||||
|
||||
class PhysicalSources:
|
||||
def __init__(self, messages, now):
|
||||
self.messages = messages
|
||||
self.logMonoTime = dict.fromkeys(messages, now)
|
||||
self.recv_time = dict.fromkeys(messages, now / 1e9)
|
||||
|
||||
def __getitem__(self, name):
|
||||
return self.messages[name]
|
||||
|
||||
def all_checks(self, services):
|
||||
return True
|
||||
|
||||
|
||||
def physical_frame(collector, basis, now=BASE):
|
||||
motion = messaging.new_message('deviceMotion').deviceMotion
|
||||
motion.timestamp = now
|
||||
motion.inputsOK = motion.sensorsOK = motion.posenetOK = True
|
||||
for name, xyz in (('angularVelocityDevice', (.05, .1, .02)),
|
||||
('orientationNED', (.01, .02, .03))):
|
||||
value = getattr(motion, name)
|
||||
value.valid = True
|
||||
value.x, value.y, value.z = xyz
|
||||
cal = messaging.new_message('extrinsicsCalibration').extrinsicsCalibration
|
||||
cal.calStatus = 'calibrated'
|
||||
cal.validBlocks = 50
|
||||
cal.rpyCalib, cal.rpyCalibSpread = basis, [0., .001, .001]
|
||||
cs = SimpleNamespace(steeringPressed=False, brakePressed=False, gasPressed=False,
|
||||
regenBraking=False, buttonEvents=(), canTimeout=False, canValid=True,
|
||||
steerFaultTemporary=False, steerFaultPermanent=False, accFaulted=False)
|
||||
sm = PhysicalSources({'deviceMotion': motion, 'extrinsicsCalibration': cal,
|
||||
'carState': cs, 'lateralDelay': SimpleNamespace(lateralDelay=.1),
|
||||
'driverMonitoringState': SimpleNamespace(noResponseForceDecel=False)}, now)
|
||||
cp = SimpleNamespace(steerRatio=14., wheelbase=2.9, centerToFront=1.2, mass=2000.)
|
||||
with mock.patch('openpilot.starpilot.flm.runtime_adapter_registry.time.monotonic_ns', return_value=now), \
|
||||
mock.patch('openpilot.starpilot.flm.runtime_adapter_registry.time.clock_gettime_ns', return_value=now):
|
||||
return physical_observer(sm, cp, collector, now)
|
||||
|
||||
|
||||
def test_actual_physical_observer_updates_live_transform_without_splitting_generation():
|
||||
command = SimpleNamespace(native_identity='current-native', state_ns=BASE,
|
||||
replay_inputs=(('vm.sR', 14.), ('vm.cF', 100000.), ('vm.cR', 100000.),
|
||||
('vEgo', 30.), ('steeringAngleDeg', 0.)))
|
||||
collector = SimpleNamespace(context=('identity', 1, 'normalized_torque', 'stage', 'response', 'a' * 64),
|
||||
_calibration_generation='b' * 32, _learning_current_native_allowed=True,
|
||||
ready=[(command, SimpleNamespace(native_allowed=True, apply_ns=BASE))],
|
||||
adapter=SimpleNamespace(command_domain='normalized_torque'))
|
||||
first = physical_frame(collector, (0., 0., 0.))
|
||||
next_frame = physical_frame(collector, (0., .002, .001))
|
||||
assert first is not None and next_frame is not None
|
||||
assert first.calibration == next_frame.calibration
|
||||
assert first.yaw_rate != next_frame.yaw_rate
|
||||
assert first.roll_rad != next_frame.roll_rad
|
||||
assert first.native_allowed and next_frame.native_allowed
|
||||
|
||||
|
||||
def test_new_generation_at_block_regression_is_not_permanently_retired():
|
||||
cp = interfaces[CAR.TOYOTA_RAV4_TSS2].get_non_essential_params(CAR.TOYOTA_RAV4_TSS2)
|
||||
with controls_for(cp, 'standard', True) as controls:
|
||||
for tick in range(30):
|
||||
live_frame(controls, tick, 'a' * 32)
|
||||
live_frame(controls, 30, 'b' * 32, blocks=5, publish=True)
|
||||
# The first reset observation may pause learning, but its new generation
|
||||
# must be usable on a subsequent fresh receipt.
|
||||
for tick in range(31, 201):
|
||||
live_frame(controls, tick, 'b' * 32, blocks=5)
|
||||
assert controls.global_learning.eligible, (controls.global_learning.reason, vars(controls.global_learning._calibration_lifecycle))
|
||||
|
||||
|
||||
def test_computed_registered_correction_reaches_controller_with_live_calibration_generation():
|
||||
from openpilot.starpilot.flm.tests.test_registered_accepted_profile import computed_registered_fit
|
||||
|
||||
cp = interfaces[CAR.TOYOTA_RAV4_TSS2].get_non_essential_params(CAR.TOYOTA_RAV4_TSS2)
|
||||
with controls_for(cp, 'standard', True) as controls:
|
||||
for tick in range(30):
|
||||
live_frame(controls, tick, 'a' * 32)
|
||||
runtime = controls.global_learning
|
||||
identity = runtime.identity
|
||||
tune = controls.LaC.torque_params.to_dict()
|
||||
result, validation = computed_registered_fit(controls.CP, runtime, BASE + 300_000_000)
|
||||
assert result.profile is not None and validation is not None
|
||||
for tick in range(30, 71):
|
||||
live_frame(controls, tick, 'a' * 32, basis=(0., .00002, -.00002),
|
||||
result=result, validation=validation, publish=tick % 25 == 0)
|
||||
assert runtime.identity == identity
|
||||
assert runtime.reason == 'qualified'
|
||||
assert any(value != 0 for value in runtime.applied_coordinates)
|
||||
assert controls.LaC.torque_params.to_dict() == tune
|
||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
@@ -1,4 +1,4 @@
|
||||
#pragma once
|
||||
|
||||
extern const uint8_t gitversion[29];
|
||||
const uint8_t gitversion[29] = "DEV-63008e94-P92eb5651-DEBUG";
|
||||
const uint8_t gitversion[29] = "DEV-7e63c553-P92eb5651-DEBUG";
|
||||
|
||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
@@ -1 +1 @@
|
||||
DEV-63008e94-P92eb5651-DEBUG
|
||||
DEV-7e63c553-P92eb5651-DEBUG
|
||||
+1
-1
@@ -1 +1 @@
|
||||
{"protected_inventory_sha256": "0f7ccdbebc3383a1c589b1b73d5e36f6083dc63c14a3d4a25a6f994834ed0fbc", "schema_version": 1}
|
||||
{"protected_inventory_sha256": "7598dc8a2d426e413747c501920f079c3426a7c37816fcc80fadaccb6ffffcb0", "schema_version": 1}
|
||||
|
||||
Reference in New Issue
Block a user