camerad: improve IFE robustness to lags

openpilot/system/camerad/cameras/camera_qcom2.cc

@@ -294,7 +294,13 @@ void camerad_thread() {
         for (auto &cam : cams) {
           if (event_data->session_hdl == cam->camera.session_handle) {
             if (cam->camera.handle_camera_event(event_data)) {
+              assert(cam->camera.buf.cur_frame_data.request_id + 1 == cam->camera.next_request_id - cam->camera.requests_in_flight);
               cam->sendState();
+              if (cam->camera.held_buf_idx >= 0) {
+                assert(cam->camera.next_request_id % cam->camera.ife_buf_depth == cam->camera.held_buf_idx);
+                cam->camera.enqueue_frame();
+              }
+              cam->camera.held_buf_idx = cam->camera.buf.cur_buf_idx;
             }
             break;
           }

openpilot/system/camerad/cameras/spectra.cc

@@ -297,7 +297,7 @@ void SpectraCamera::camera_open(VisionIpcServer *v) {
   LOGD("camera init %d", cc.camera_num);
   buf.init(this, v, ife_buf_depth, cc.stream_type);
   camera_map_bufs();
-  clearAndRequeue(1);
+  clearAndRequeue();
 }
 
 void SpectraCamera::sensors_start() {
@@ -942,7 +942,11 @@ void SpectraCamera::config_ife(int idx, int request_id, bool init) {
   assert(ret == 0);
 }
 
-void SpectraCamera::enqueue_frame(uint64_t request_id) {
+void SpectraCamera::enqueue_frame() {
+  // The kernel reports only requests newer than reported_req_id, which a flush does not reset.
+  // Always allocate a new ID so recovery never reuses one the kernel has already seen.
+  // https://github.com/commaai/agnos-kernel-sdm845/blob/93ddd472ce522ab8669456b11bf3924ad32e9882/drivers/media/platform/msm/camera/cam_isp/cam_isp_context.c#L609-L613
+  uint64_t request_id = next_request_id++;
   int i = request_id % ife_buf_depth;
   assert(sync_objs_ife[i] == 0);
 
@@ -981,6 +985,7 @@ void SpectraCamera::enqueue_frame(uint64_t request_id) {
   // submit request to IFE and BPS
   config_ife(i, request_id);
   if (cc.output_type == ISP_BPS_PROCESSED) config_bps(i, request_id);
+  requests_in_flight++;
 }
 
 void SpectraCamera::destroySyncObjectAt(int index) {
@@ -1416,26 +1421,25 @@ bool SpectraCamera::handle_camera_event(const cam_req_mgr_message *event_data) {
   if (!validateEvent(request_id, ife_frame_id)) {
     return false;
   }
+  assert(requests_in_flight > 0);
+  requests_in_flight--;
 
   // Update tracking variables
-  if (request_id == last_valid_request_id + 1) {
-    skip_expected = false;
-  }
   last_valid_ife_frame_id = ife_frame_id;
-  last_valid_request_id = request_id;
 
   // Wait until frame's fully read out and processed
   if (!waitForFrameReady(request_id)) {
     // Reset queue on sync failure to prevent frame tearing
     LOGE("camera %d sync failure %ld %ld ", cc.camera_num, request_id, ife_frame_id);
-    clearAndRequeue(request_id + 1);
+    clearAndRequeue();
     return false;
   }
 
   int buf_idx = request_id % ife_buf_depth;
   bool ret = processFrame(buf_idx, request_id, ife_frame_id, timestamp);
   destroySyncObjectAt(buf_idx);
-  enqueue_frame(request_id + ife_buf_depth);  // request next frame for this slot
+  if (!ret) enqueue_frame();
+  if (stress_test("publish delay")) util::sleep_for(1200);
   return ret;
 }
 
@@ -1445,39 +1449,42 @@ bool SpectraCamera::validateEvent(uint64_t request_id, uint64_t ife_frame_id) {
   if (request_id == 0) {
     if (invalid_request_count++ > ife_buf_depth+2) {
       LOGE("camera %d reset after half second of invalid requests", cc.camera_num);
-      clearAndRequeue(last_valid_request_id + 1);
+      clearAndRequeue();
       invalid_request_count = 0;
     }
     return false;
   }
   invalid_request_count = 0;
 
-  // check for skips in frame_id or request_id
-  if (!skip_expected) {
-    if (ife_frame_id != last_valid_ife_frame_id + 1) {
-      LOGE("camera %d frame ID skipped, %lu -> %lu", cc.camera_num, last_valid_ife_frame_id, ife_frame_id);
-      clearAndRequeue(request_id + 1);
-      return false;
-    }
+  uint64_t expected_request_id = next_request_id - requests_in_flight;
+  if (request_id != expected_request_id) {
+    LOGE("camera %d request ID mismatch, expected %lu, got %lu", cc.camera_num, expected_request_id, request_id);
+    clearAndRequeue();
+    return false;
+  }
 
-    if (request_id != last_valid_request_id + 1) {
-      LOGE("camera %d requests skipped %ld -> %ld", cc.camera_num, last_valid_request_id, request_id);
-      clearAndRequeue(request_id + 1);
-      return false;
-    }
+  if (last_valid_ife_frame_id != 0 && ife_frame_id != last_valid_ife_frame_id + 1) {
+    LOGE("camera %d frame ID skipped, %lu -> %lu", cc.camera_num, last_valid_ife_frame_id, ife_frame_id);
+    clearAndRequeue();
+    return false;
   }
   return true;
 }
 
-void SpectraCamera::clearAndRequeue(uint64_t from_request_id) {
+void SpectraCamera::clearAndRequeue() {
   // clear everything, then queue up a fresh set of frames
-  LOGW("clearing and requeuing camera %d from %lu", cc.camera_num, from_request_id);
+  LOGW("clearing and requeuing camera %d from %lu", cc.camera_num, next_request_id);
   clear_req_queue();
+  requests_in_flight = 0;
   last_requeue_ts = nanos_since_boot();
-  for (uint64_t id = from_request_id; id < from_request_id + ife_buf_depth; ++id) {
-    enqueue_frame(id);
+  if (held_buf_idx >= 0) {
+    assert(next_request_id % ife_buf_depth == held_buf_idx);
+    next_request_id++;  // keep the last published buffer out of the hardware queue
+  }
+  for (int i = 0; i < ife_buf_depth - (held_buf_idx >= 0); ++i) {
+    enqueue_frame();
   }
-  skip_expected = true;
+  last_valid_ife_frame_id = 0;  // accept any IFE frame ID after requeue
 }
 
 bool SpectraCamera::waitForFrameReady(uint64_t request_id) {
@@ -1490,10 +1497,11 @@ bool SpectraCamera::waitForFrameReady(uint64_t request_id) {
   }
 
   auto waitForSync = [&](uint32_t sync_obj, int timeout_ms, const char *sync_type) {
+    if (stress_test(sync_type)) return false;
     double st = millis_since_boot();
     struct cam_sync_wait sync_wait = {};
     sync_wait.sync_obj = sync_obj;
-    sync_wait.timeout_ms = stress_test(sync_type) ? 1 : timeout_ms;
+    sync_wait.timeout_ms = timeout_ms;
     bool ret = do_sync_control(m->cam_sync_fd, CAM_SYNC_WAIT, &sync_wait, sizeof(sync_wait)) == 0;
     double et = millis_since_boot();
     if (!ret) LOGE("camera %d %s failed after %.2fms", cc.camera_num, sync_type, et-st);

openpilot/system/camerad/cameras/spectra.h

@@ -144,7 +144,7 @@ class SpectraCamera {
   void config_ife(int idx, int request_id, bool init=false);
 
   int clear_req_queue();
-  void enqueue_frame(uint64_t request_id);
+  void enqueue_frame();
 
   int sensors_init();
   void sensors_start();
@@ -205,17 +205,18 @@ class SpectraCamera {
   int buf_handle_raw[MAX_IFE_BUFS] = {};
   int sync_objs_ife[MAX_IFE_BUFS] = {};
   int sync_objs_bps[MAX_IFE_BUFS] = {};
-  uint64_t last_valid_request_id = 0;
+  uint64_t next_request_id = 1;
+  int requests_in_flight = 0;
+  int held_buf_idx = -1;
   uint64_t last_requeue_ts = 0;
   uint64_t last_valid_ife_frame_id = 0;
   int invalid_request_count = 0;
-  bool skip_expected = true;
 
   CameraBuf buf;
   SpectraMaster *m;
 
 private:
-  void clearAndRequeue(uint64_t from_request_id);
+  void clearAndRequeue();
   bool validateEvent(uint64_t request_id, uint64_t ife_frame_id);
   bool waitForFrameReady(uint64_t request_id);
   bool processFrame(int buf_idx, uint64_t request_id, uint64_t ife_frame_id, uint64_t timestamp);
@@ -230,10 +231,12 @@ class SpectraCamera {
 
   // a mode for stressing edge cases: realignment, sync failures, etc.
   inline bool stress_test(std::string log) {
-    static double last_trigger = 0;
+    static double last_trigger = millis_since_boot();
     static double prob = std::stod(util::getenv("SPECTRA_ERROR_PROB", "-1"));
     static double dt = std::stod(util::getenv("SPECTRA_ERROR_DT", "1"));
-    bool triggered = (prob > 0) && \
+    static std::string filter = util::getenv("SPECTRA_ERROR_FILTER");
+    static int camera = std::stoi(util::getenv("SPECTRA_ERROR_CAMERA", "-1"));
+    bool triggered = (camera < 0 || camera == cc.camera_num) && (filter.empty() || filter == log) && (prob > 0) && \
                      ((static_cast<double>(rand()) / RAND_MAX) < prob) && \
                      (millis_since_boot() - last_trigger) > dt;
     if (triggered) {

openpilot/system/camerad/sensors/os04c10.cc

@@ -1,4 +1,5 @@
 #include <cmath>
+#include <cstdlib>
 
 #include "system/camerad/sensors/sensor.h"
 #include <media/msm_camsensor_sdk.h>
@@ -37,6 +38,9 @@ OS04C10::OS04C10() {
 
   start_reg_array.assign(std::begin(start_reg_array_os04c10), std::end(start_reg_array_os04c10));
   init_reg_array.assign(std::begin(init_array_os04c10), std::end(init_array_os04c10));
+  if (std::getenv("SPECTRA_TEST_PATTERN")) {
+    init_reg_array.push_back({0x5080, 0xc4});
+  }
   probe_reg_addr = 0x300a;
   probe_expected_data = 0x5304;
   bits_per_pixel = 12;

openpilot/system/camerad/sensors/ox03c10.cc

@@ -1,4 +1,5 @@
 #include <cmath>
+#include <cstdlib>
 
 #include "system/camerad/sensors/sensor.h"
 #include <media/msm_camsensor_sdk.h>
@@ -37,6 +38,12 @@ OX03C10::OX03C10() {
 
   start_reg_array.assign(std::begin(start_reg_array_ox03c10), std::end(start_reg_array_ox03c10));
   init_reg_array.assign(std::begin(init_array_ox03c10), std::end(init_array_ox03c10));
+  if (std::getenv("SPECTRA_TEST_PATTERN")) {
+    init_reg_array.insert(init_reg_array.end(), {
+      {0x5004, 0x1f}, {0x5005, 0x1f}, {0x5006, 0x1f}, {0x5007, 0x1f},
+      {0x5240, 0x03}, {0x5440, 0x03}, {0x5640, 0x03}, {0x5840, 0x03},
+    });
+  }
   probe_reg_addr = 0x300a;
   probe_expected_data = 0x5803;
   bits_per_pixel = 12;

openpilot/system/camerad/test/test_camerad.py

@@ -3,8 +3,10 @@
 import os
 import time
 import unittest
+from unittest.mock import patch
 import numpy as np
 
+from msgq.visionipc import VisionIpcClient, VisionStreamType
 from openpilot.common.parameterized import parameterized
 from openpilot.common.test import OpenpilotTestCase
 from openpilot.cereal.services import SERVICE_LIST
@@ -17,6 +19,12 @@
 EXPOSURE_STABLE_COUNT = 3
 EXPOSURE_RANGE = (0.15, 0.35)
 MAX_TEST_TIME = 25
+STRESS_ERRORS = (
+  ("skip_sof", "skipping SOF event"),
+  ("processing_delay", "sync sleep time"),
+  ("ife_timeout", "IFE sync"),
+  ("bps_timeout", "BPS sync"),
+)
 
 
 def _numpy_rgb2gray(im):
@@ -37,6 +45,39 @@ def _exposure_stable(results):
     for v in results.values()
   )
 
+def _pattern_sample(client):
+  buf = client.recv(1000)
+  if buf is None:
+    return None
+  y = np.asarray(buf.data[:buf.uv_offset], dtype=np.uint8).reshape((-1, buf.stride))[:buf.height:4, :buf.width:4]
+  profile = y.mean(1)
+  padded = np.pad(profile, (4, 4), mode="edge")
+  neighbors = [padded[i:i + len(profile)] for i in range(9) if i != 4]
+  residual = profile - np.median(neighbors, axis=0)
+  position = int(np.argmax(residual))
+  if residual[position] <= 10:
+    return None
+  return client.timestamp_sof, client.frame_id, position / len(profile)
+
+def _sanity_checks(ts):
+  for camera in CAMERAS:
+    assert camera in ts
+    assert len(ts[camera]['t']) > 20
+    assert 0 not in ts[camera]['requestId']
+
+    frame_steps = np.diff(ts[camera]['frameId'])
+    assert np.all(frame_steps > 0)
+    assert np.all(np.diff(ts[camera]['requestId']) > 0)
+    # Skipped frame IDs must account for the same number of frame periods in SOF time.
+    expected_sof_steps = frame_steps * 1e9 / SERVICE_LIST[camera].frequency
+    sof_step_errors = np.diff(ts[camera]['timestampSof']) - expected_sof_steps
+    assert np.all(np.abs(sof_step_errors) < 2e6), f"{camera} frame/SOF steps disagree: {sof_step_errors[np.abs(sof_step_errors) >= 2e6]}"
+
+    assert np.all((ts[camera]['timestampEof'] - ts[camera]['timestampSof']) > 0)
+    assert np.all((ts[camera]['t'] - ts[camera]['timestampSof']/1e9) > 1e-7)
+    assert np.mean((ts[camera]['t'] - ts[camera]['timestampEof']/1e9) > 1e-7) > 0.7
+    assert np.all((ts[camera]['t'] - ts[camera]['timestampEof']/1e9) > -0.10)
+
 
 def run_and_log(procs, services, duration):
   with processes_context(procs):
@@ -124,43 +165,59 @@ def test_frame_sync(self):
       assert 20 < offset_ms < 30, f"driver camera stagger out of range at frame {i}: {offset_ms:.1f}ms (expected ~25ms)"
 
   def test_sanity_checks(self):
-    self._sanity_checks(self.logs)
-
-  def _sanity_checks(self, ts):
-    for c in CAMERAS:
-      assert c in ts
-      assert len(ts[c]['t']) > 20
-
-      # not a valid request id
-      assert 0 not in ts[c]['requestId']
+    _sanity_checks(self.logs)
 
-      # should monotonically increase
-      assert np.all(np.diff(ts[c]['frameId']) >= 1)
-      assert np.all(np.diff(ts[c]['requestId']) >= 1)
 
-      # EOF > SOF
-      assert np.all((ts[c]['timestampEof'] - ts[c]['timestampSof']) > 0)
-
-      # logMonoTime > SOF
-      assert np.all((ts[c]['t'] - ts[c]['timestampSof']/1e9) > 1e-7)
-
-      # logMonoTime > EOF, needs some tolerance since EOF is (SOF + readout time) but there is noise in the SOF timestamping (done via IRQ)
-      assert np.mean((ts[c]['t'] - ts[c]['timestampEof']/1e9) > 1e-7) > 0.7  # should be mostly logMonoTime > EOF
-      assert np.all((ts[c]['t'] - ts[c]['timestampEof']/1e9) > -0.10)        # when EOF > logMonoTime, it should never be more than two frames
+class TestCameradStress(OpenpilotTestCase):
+  TICI_TEST = True
 
-  def test_stress_test(self):
-    os.environ['SPECTRA_ERROR_PROB'] = '0.008'
-    try:
-      logs = run_and_log(["camerad", ], CAMERAS, 10)
-    finally:
-      del os.environ['SPECTRA_ERROR_PROB']
+  @parameterized.expand(STRESS_ERRORS, ids=lambda name, _: name)
+  def test_stress_test(self, _, error):
+    env = {'SPECTRA_ERROR_FILTER': error, 'SPECTRA_ERROR_PROB': '1', 'SPECTRA_ERROR_DT': '2000'}
+    with patch.dict(os.environ, env):
+      logs = run_and_log(["camerad"], CAMERAS, 6)
     ts = msgs_to_time_series(logs)
 
-    # we should see some jumps from introduced errors
-    assert np.max([ np.max(np.diff(ts[c]['frameId'])) for c in CAMERAS ]) > 1
-    assert np.max([ np.max(np.diff(ts[c]['requestId'])) for c in CAMERAS ]) > 1
-
-    self._sanity_checks(ts)
+    assert max(np.max(np.diff(ts[c]['frameId'])) for c in CAMERAS) > 1
+    assert max(np.max(np.diff(ts[c]['requestId'])) for c in CAMERAS) > 1
+    _sanity_checks(ts)
+
+  def test_frame_data_alignment(self):
+    env = {'SPECTRA_TEST_PATTERN': '1', 'SPECTRA_ERROR_FILTER': 'publish delay', 'SPECTRA_ERROR_CAMERA': '1',
+           'SPECTRA_ERROR_PROB': '1', 'SPECTRA_ERROR_DT': '6000'}
+    samples = {'road': [], 'wide': []}
+    with patch.dict(os.environ, env), processes_context(["camerad"]), log_collector(CAMERAS) as (raw_logs, lock):
+      clients = {
+        'road': VisionIpcClient("camerad", VisionStreamType.VISION_STREAM_ROAD, True),
+        'wide': VisionIpcClient("camerad", VisionStreamType.VISION_STREAM_WIDE_ROAD, True),
+      }
+      for client in clients.values():
+        client.connect(True)
+      end = time.monotonic() + 10
+      while time.monotonic() < end:
+        for camera, client in clients.items():
+          sample = _pattern_sample(client)
+          if sample is not None:
+            samples[camera].append(sample)
+
+    with lock:
+      ts = msgs_to_time_series(raw_logs)
+    assert all(samples.values())
+    offsets = []
+    for timestamp, frame_id, phase in samples['road']:
+      wide = min(samples['wide'], key=lambda sample: abs(sample[0] - timestamp))
+      if abs(wide[0] - timestamp) < 1.1e6:
+        offsets.append((frame_id, (phase - wide[2]) % 1))
+    assert len(offsets) > 20
+    expected_offsets = {offset for _, offset in offsets[:len(offsets) // 2]}
+    # Allow small sensor-specific phase jitter while rejecting a substituted frame.
+    tolerance = 1 / 32
+    unexpected = {frame_id: offset for frame_id, offset in offsets[len(offsets) // 2:]
+                  if all(abs((offset - expected + .5) % 1 - .5) > tolerance for expected in expected_offsets)}
+    assert not unexpected, f"road/wide pixels disagree for synchronized frames: {unexpected}"
+
+    assert max(np.max(np.diff(ts[c]['frameId'])) for c in CAMERAS) > 1
+    _sanity_checks(ts)
 
 
 if __name__ == "__main__":