Keep calibrated configuration across learner receipt gaps

openpilot/starpilot/flm/steerlearnerd.py

@@ -95,6 +95,18 @@ def _collect_applied_output(collector, applied, sm, cp, now_ns):
   return collector.output(applied, now_ns)
 
 
+def _configuration_calibration(sm, calibration_lifecycle, now):
+  # Calibration describes the vehicle frame, not a per-message command lease.
+  # Like torqued's PoseCalibrator, retain a valid calibrated transform between
+  # deliveries. Sample freshness and current native authority are checked
+  # independently when admitting physical observations and applying offers.
+  stamp = int(sm.logMonoTime['extrinsicsCalibration'])
+  receipt = int(sm.recv_time['extrinsicsCalibration'] * 1e9)
+  if not (sm.seen['extrinsicsCalibration'] and sm.valid['extrinsicsCalibration'] and
+          0 < stamp <= now and 0 < receipt <= now):
+    return calibration_lifecycle.observe(None, current=False)
+  return calibration_lifecycle.observe(sm['extrinsicsCalibration'], current=True)
+
 def _source_config_binding(source):
   return (source.cp_sha256, source.source_epoch, source.reset, source.calibration_generation,
           source.enabled, source.manual, source.persistent_source_epoch, tuple(source.manual_baseline))
@@ -931,14 +943,7 @@ def main():
   calibration_lifecycle = ProducerCalibrationLifecycle()
 
   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)
-    if not sm.valid['extrinsicsCalibration'] or stamp > now or receipt > now:
-      return calibration_lifecycle.observe(None, current=False)
-    return calibration_lifecycle.observe(sm['extrinsicsCalibration'], current=current)
+    return _configuration_calibration(sm, calibration_lifecycle, now)
 
   def current_source(now, frame):
     return read_source(CP, params, now_ns=now, cp_sha256=cp_sha256,

openpilot/starpilot/flm/tests/test_calibration_configuration.py

@@ -0,0 +1,55 @@
+"""Calibration is retained configuration; physical samples keep their own gates."""
+from types import SimpleNamespace
+import pytest
+from openpilot.starpilot.flm.calibration_lifecycle import ProducerCalibrationLifecycle
+from openpilot.starpilot.flm.steerlearnerd import _configuration_calibration
+
+BASE = 632501433320
+
+
+class CalibrationSource:
+  def __init__(self, *, seen=True, valid=True, calibrated=True):
+    self.seen = {'extrinsicsCalibration': seen}
+    self.valid = {'extrinsicsCalibration': valid}
+    self.logMonoTime = {'extrinsicsCalibration': BASE}
+    self.recv_time = {'extrinsicsCalibration': BASE/1e9}
+    self.message = SimpleNamespace(rpyCalib=(-.0000129, -.0230993, -.0274958),
+      rpyCalibSpread=(.000135, .00904, .00597), validBlocks=50,
+      calStatus='calibrated' if calibrated else 'uncalibrated')
+
+  def __getitem__(self, service):
+    assert service == 'extrinsicsCalibration'
+    return self.message
+
+  def all_checks(self, services):
+    return False  # Receiver cadence is not the calibration's meaning.
+
+
+@pytest.mark.parametrize('age_ns', (20_000_000, 600_000_000, 5_000_000_000))
+def test_known_calibration_survives_missing_cadence_without_restamping(age_ns):
+  sm = CalibrationSource()
+  lifecycle = ProducerCalibrationLifecycle(token_factory=lambda:'a'*32)
+  frame = _configuration_calibration(sm, lifecycle, BASE+age_ns)
+  assert frame == ('a'*32, sm.message.rpyCalib)
+  assert sm.logMonoTime['extrinsicsCalibration'] == BASE
+  assert sm.recv_time['extrinsicsCalibration'] == BASE/1e9
+
+
+@pytest.mark.parametrize('seen,valid,calibrated,now', (
+  (False, True, True, BASE+1), (True, False, True, BASE+1),
+  (True, True, False, BASE+1), (True, True, True, BASE-1),
+))
+def test_unseen_invalid_uncalibrated_or_future_configuration_is_rejected(seen, valid, calibrated, now):
+  sm = CalibrationSource(seen=seen, valid=valid, calibrated=calibrated)
+  assert _configuration_calibration(sm, ProducerCalibrationLifecycle(), now) is None
+
+
+def test_explicit_calibration_reset_retires_prior_frame():
+  sm = CalibrationSource()
+  tokens = iter(('a'*32, 'b'*32))
+  lifecycle = ProducerCalibrationLifecycle(token_factory=lambda:next(tokens))
+  assert _configuration_calibration(sm, lifecycle, BASE+1)[0] == 'a'*32
+  sm.message.calStatus = 'uncalibrated'
+  assert _configuration_calibration(sm, lifecycle, BASE+2) is None
+  sm.message.calStatus = 'calibrated'
+  assert _configuration_calibration(sm, lifecycle, BASE+3)[0] == 'b'*32

prebuilt.json

@@ -1 +1 @@
-{"protected_inventory_sha256": "b145b4d62316af29a5981e42ba947f6cc8e86cdf8b30bb2cd1d745d9fb37f393", "schema_version": 1}
+{"protected_inventory_sha256": "70494d1578cab796bf9c7be8f5b0ba7c99e733f944628f0922a2f3e393cb2965", "schema_version": 1}