Stricter cam sync tests (#34609)

* strict

* fix that

---------

Co-authored-by: Comma Device <device@comma.ai>

selfdrive/test/test_onroad.py

@@ -333,6 +333,39 @@ class TestOnroad:
     result += "------------------------------------------------\n"
     print(result)
 
+  def test_camera_sync(self, subtests):
+    cam_states = ['roadCameraState', 'wideRoadCameraState', 'driverCameraState']
+    encode_cams = ['roadEncodeIdx', 'wideRoadEncodeIdx', 'driverEncodeIdx']
+    for cams in (cam_states, encode_cams):
+      # TODO: driverEncodeIdx has an issue that needs to be fixed
+      if 'driverEncodeIdx' in cams:
+        continue
+
+      # sanity checks within a single cam
+      for cam in cams:
+        with subtests.test(test="frame_skips", camera=cam):
+          print(self.ts[cam]['frameId'])
+          assert set(np.diff(self.ts[cam]['frameId'])) == {1, }, "Frame ID skips"
+
+          # EOF > SOF
+          eof_sof_diff = self.ts[cam]['timestampEof'] - self.ts[cam]['timestampSof']
+          assert np.all(eof_sof_diff > 0)
+          assert np.all(eof_sof_diff < 50*1e6)
+
+      # camerad guarantees that all cams start on the same frame ID
+      first_fid = {c: min(self.ts[c]['frameId']) for c in cams}
+      assert len(set(first_fid.values())) == 1, "Cameras don't start on same frame ID"
+
+      # we don't do a full segment rotation, so these might not match exactly
+      last_fid = {c: max(self.ts[c]['frameId']) for c in cams}
+      assert max(last_fid.values()) - min(last_fid.values()) < 10
+
+      start, end = min(first_fid.values()), min(last_fid.values())
+      for i in range(end-start):
+        ts = {c: round(self.ts[c]['timestampSof'][i]/1e6, 1) for c in cams}
+        diff = (max(ts.values()) - min(ts.values()))
+        assert diff < 2, f"Cameras not synced properly: frame_id={start+i}, {diff=:.1f}ms, {ts=}"
+
   def test_mpc_execution_timings(self):
     result = "\n"
     result += "------------------------------------------------\n"

system/camerad/cameras/spectra.cc

@@ -1401,7 +1401,7 @@ void SpectraCamera::handle_camera_event(const cam_req_mgr_message *event_data) {
     request_id_last = request_id;
 
     uint64_t timestamp = event_data->u.frame_msg.timestamp;  // this is timestamped in the kernel's SOF IRQ callback
-    if (syncFirstFrame(cc.camera_num, frame_id_raw, timestamp)) {
+    if (syncFirstFrame(cc.camera_num, request_id, frame_id_raw, timestamp)) {
       auto &meta_data = buf.frame_metadata[buf_idx];
       meta_data.frame_id = frame_id_raw - camera_sync_data[cc.camera_num].frame_id_offset;
       meta_data.request_id = request_id;
@@ -1409,9 +1409,11 @@ void SpectraCamera::handle_camera_event(const cam_req_mgr_message *event_data) {
 
       // wait for this frame's EOF, then queue up the next one
       enqueue_req_multi(request_id + ife_buf_depth, 1, true);
+      //LOGW("camerad %d synced req %d fid %d, publishing ts %.2f cereal_frame_id %d", cc.camera_num, (int)request_id, (int)frame_id_raw, (double)(timestamp)*1e-6, meta_data.frame_id);
     } else {
       // Frames not yet synced
       enqueue_req_multi(request_id + ife_buf_depth, 1, false);
+      //LOGW("camerad %d not synced req %d fid %d", cc.camera_num, (int)request_id, (int)frame_id_raw);
     }
   } else { // not ready
     if (frame_id_raw > frame_id_raw_last + 10) {
@@ -1424,9 +1426,14 @@ void SpectraCamera::handle_camera_event(const cam_req_mgr_message *event_data) {
   }
 }
 
-bool SpectraCamera::syncFirstFrame(int camera_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) {
   if (first_frame_synced) return true;
 
+  // OX and OS cameras require a few frames for the FSIN to sync up
+  if (request_id < 3) {
+    return false;
+  }
+
   // Store the frame data for this camera
   camera_sync_data[camera_id] = SyncData{timestamp, raw_id + 1};
 
@@ -1440,7 +1447,7 @@ bool SpectraCamera::syncFirstFrame(int camera_id, uint64_t raw_id, uint64_t time
   for (const auto &[_, sync_data] : camera_sync_data) {
     uint64_t diff = std::max(timestamp, sync_data.timestamp) -
                     std::min(timestamp, sync_data.timestamp);
-    if (diff > 2*1e6) {  // within 2ms
+    if (diff > 0.5*1e6) {  // within 0.5ms
       all_cams_synced = false;
     }
   }

system/camerad/cameras/spectra.h

@@ -200,7 +200,7 @@ public:
   SpectraMaster *m;
 
 private:
-  static bool syncFirstFrame(int camera_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);
   struct SyncData {
     uint64_t timestamp;
     uint64_t frame_id_offset = 0;

system/camerad/test/test_camerad.py

@@ -76,10 +76,4 @@ class TestCamerad:
       cam_times = [(m.which(), getattr(m, m.which()).timestampSof/1e6) for m in self.log_by_frame_id[fid]]
       return (diff, cam_times)
     laggy_frames = {k: get_desc(k, v) for k, v in diffs.items() if v > LAG_FRAME_TOLERANCE[self.sensor_type]}
-
-    def in_tol(diff):
-      return 50 - LAG_FRAME_TOLERANCE[self.sensor_type] < diff and diff < 50 + LAG_FRAME_TOLERANCE[self.sensor_type]
-    if len(laggy_frames) != 0 and all( in_tol(laggy_frames[lf][0]) for lf in laggy_frames):
-      print("TODO: handle camera out of sync")
-    else:
     assert len(laggy_frames) == 0, f"Frames not synced properly: {laggy_frames=}"