stagger driver camera SOF (#37628)

selfdrive/test/test_onroad.py

@@ -342,10 +342,15 @@ class TestOnroad:
 
         start, end = min(first_fid), min(last_fid)
         for i in range(end-start):
-          ts = {c: round(self.ts[c]['timestampSof'][i]/1e6, 1) for c in cams}
+          # road and wide cameras (first two) should be synced within 2ms
+          ts = {c: round(self.ts[c]['timestampSof'][i]/1e6, 1) for c in cams[:2]}
           diff = (max(ts.values()) - min(ts.values()))
           assert diff < 2, f"Cameras not synced properly: frame_id={start+i}, {diff=:.1f}ms, {ts=}"
 
+          # driver camera should be staggered ~25ms from road camera
+          offset_ms = abs(self.ts[cams[2]]['timestampSof'][i] - self.ts[cams[0]]['timestampSof'][i]) / 1e6
+          assert 20 < offset_ms < 30, f"driver camera stagger out of range at frame {start+i}: {offset_ms:.1f}ms"
+
   def test_camera_encoder_matches(self, subtests):
     # sanity check that the frame metadata is consistent with the encoded frames
     pairs = [('roadCameraState', 'roadEncodeIdx'),

system/camerad/cameras/hw.h

@@ -25,6 +25,7 @@ struct CameraConfig {
   uint32_t phy;
   bool vignetting_correction;
   SpectraOutputType output_type;
+  bool staggered_sof;  // SOF is staggered (half-period offset) from other cameras
 };
 
 // NOTE: to be able to disable road and wide road, we still have to configure the sensor over i2c
@@ -39,6 +40,7 @@ const CameraConfig WIDE_ROAD_CAMERA_CONFIG = {
   .phy = CAM_ISP_IFE_IN_RES_PHY_0,
   .vignetting_correction = false,
   .output_type = ISP_IFE_PROCESSED,
+  .staggered_sof = false,
 };
 
 const CameraConfig ROAD_CAMERA_CONFIG = {
@@ -51,6 +53,7 @@ const CameraConfig ROAD_CAMERA_CONFIG = {
   .phy = CAM_ISP_IFE_IN_RES_PHY_1,
   .vignetting_correction = true,
   .output_type = ISP_IFE_PROCESSED,
+  .staggered_sof = false,
 };
 
 const CameraConfig DRIVER_CAMERA_CONFIG = {
@@ -63,6 +66,7 @@ const CameraConfig DRIVER_CAMERA_CONFIG = {
   .phy = CAM_ISP_IFE_IN_RES_PHY_2,
   .vignetting_correction = false,
   .output_type = ISP_BPS_PROCESSED,
+  .staggered_sof = true,
 };
 
 const CameraConfig ALL_CAMERA_CONFIGS[] = {WIDE_ROAD_CAMERA_CONFIG, ROAD_CAMERA_CONFIG, DRIVER_CAMERA_CONFIG};

system/camerad/cameras/spectra.cc

@@ -1436,7 +1436,7 @@ bool SpectraCamera::waitForFrameReady(uint64_t request_id) {
 }
 
 bool SpectraCamera::processFrame(int buf_idx, uint64_t request_id, uint64_t frame_id_raw, uint64_t timestamp) {
-  if (!syncFirstFrame(cc.camera_num, request_id, frame_id_raw, timestamp)) {
+  if (!syncFirstFrame(cc.camera_num, request_id, frame_id_raw, timestamp, cc.staggered_sof)) {
     return false;
   }
 
@@ -1455,23 +1455,31 @@ bool SpectraCamera::processFrame(int buf_idx, uint64_t request_id, uint64_t fram
   return true;
 }
 
-bool SpectraCamera::syncFirstFrame(int camera_id, uint64_t request_id, uint64_t raw_id, uint64_t timestamp) {
+bool SpectraCamera::syncFirstFrame(int camera_id, uint64_t request_id, uint64_t raw_id, uint64_t timestamp, bool staggered) {
   if (first_frame_synced) return true;
 
   // Store the frame data for this camera
-  camera_sync_data[camera_id] = SyncData{timestamp, raw_id + 1};
+  camera_sync_data[camera_id] = SyncData{timestamp, raw_id + 1, staggered};
 
   // Ensure all cameras are up
   int enabled_camera_count = std::count_if(std::begin(ALL_CAMERA_CONFIGS), std::end(ALL_CAMERA_CONFIGS),
                                            [](const auto &config) { return config.enabled; });
   bool all_cams_up = camera_sync_data.size() == enabled_camera_count;
 
-  // Wait until the timestamps line up
+  // Check that camera timestamps are properly aligned:
+  // - non-staggered cameras should be within 0.2ms of each other
+  // - staggered cameras should be within 0.2ms of a 25ms offset from non-staggered cameras
+  const uint64_t half_period_ns = 25 * 1000000ULL;  // 25ms
+  const uint64_t tolerance_ns = 200000ULL;           // 0.2ms
   bool all_cams_synced = true;
-  for (const auto &[_, sync_data] : camera_sync_data) {
+  for (const auto &[cam, sync_data] : camera_sync_data) {
+    if (cam == camera_id) continue;
     uint64_t diff = std::max(timestamp, sync_data.timestamp) -
                     std::min(timestamp, sync_data.timestamp);
-    if (diff > 0.2*1e6) { // milliseconds
+    bool pair_staggered = staggered != sync_data.staggered;
+    uint64_t expected_offset = pair_staggered ? half_period_ns : 0;
+    uint64_t error = (diff > expected_offset) ? diff - expected_offset : expected_offset - diff;
+    if (error > tolerance_ns) {
       all_cams_synced = false;
     }
   }

system/camerad/cameras/spectra.h

@@ -194,10 +194,11 @@ private:
   bool validateEvent(uint64_t request_id, uint64_t frame_id_raw);
   bool waitForFrameReady(uint64_t request_id);
   bool processFrame(int buf_idx, uint64_t request_id, uint64_t frame_id_raw, uint64_t timestamp);
-  static bool syncFirstFrame(int camera_id, uint64_t request_id, uint64_t raw_id, uint64_t timestamp);
+  static bool syncFirstFrame(int camera_id, uint64_t request_id, uint64_t raw_id, uint64_t timestamp, bool staggered);
   struct SyncData {
     uint64_t timestamp;
     uint64_t frame_id_offset = 0;
+    bool staggered = false;
   };
   inline static std::map<int, SyncData> camera_sync_data;
   inline static bool first_frame_synced = false;

system/camerad/test/test_camerad.py

@@ -105,17 +105,23 @@ class TestCamerad:
       assert set(np.diff(logs[c]['frameId'])) == {1, }, f"{c} has frame skips"
 
   def test_frame_sync(self, logs):
+    SYNCED_CAMS = ('roadCameraState', 'wideRoadCameraState')
     n = range(len(logs['roadCameraState']['t'][:-10]))
 
     frame_ids = {i: [logs[cam]['frameId'][i] for cam in CAMERAS] for i in n}
     assert all(len(set(v)) == 1 for v in frame_ids.values()), "frame IDs not aligned"
 
-    frame_times = {i: [logs[cam]['timestampSof'][i] for cam in CAMERAS] for i in n}
-    diffs = {i: (max(ts) - min(ts))/1e6 for i, ts in frame_times.items()}
-
+    # road and wide cameras should be synced within 1.1ms
+    synced_times = {i: [logs[cam]['timestampSof'][i] for cam in SYNCED_CAMS] for i in n}
+    diffs = {i: (max(ts) - min(ts))/1e6 for i, ts in synced_times.items()}
     laggy_frames = {k: v for k, v in diffs.items() if v > 1.1}
     assert len(laggy_frames) == 0, f"Frames not synced properly: {laggy_frames=}"
 
+    # driver camera should be staggered ~25ms from road camera
+    for i in n:
+      offset_ms = abs(logs['driverCameraState']['timestampSof'][i] - logs['roadCameraState']['timestampSof'][i]) / 1e6
+      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, logs):
     self._sanity_checks(logs)