From 52b66a1c0c53087cd2719e3474070ddb99ee6576 Mon Sep 17 00:00:00 2001 From: Adeeb Shihadeh Date: Thu, 20 Feb 2025 14:15:38 -0800 Subject: [PATCH] Stricter cam sync tests (#34609) * strict * fix that --------- Co-authored-by: Comma Device --- selfdrive/test/test_onroad.py | 33 +++++++++++++++++++++++++++++ system/camerad/cameras/spectra.cc | 13 +++++++++--- system/camerad/cameras/spectra.h | 2 +- system/camerad/test/test_camerad.py | 8 +------ 4 files changed, 45 insertions(+), 11 deletions(-) diff --git a/selfdrive/test/test_onroad.py b/selfdrive/test/test_onroad.py index f21efcba12..8caa618e51 100644 --- a/selfdrive/test/test_onroad.py +++ b/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" diff --git a/system/camerad/cameras/spectra.cc b/system/camerad/cameras/spectra.cc index d578639322..9ca896785e 100644 --- a/system/camerad/cameras/spectra.cc +++ b/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; } } diff --git a/system/camerad/cameras/spectra.h b/system/camerad/cameras/spectra.h index 21b99d54a1..b27296fa57 100644 --- a/system/camerad/cameras/spectra.h +++ b/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; diff --git a/system/camerad/test/test_camerad.py b/system/camerad/test/test_camerad.py index c259fe788d..1c6bc8ee44 100644 --- a/system/camerad/test/test_camerad.py +++ b/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=}" + assert len(laggy_frames) == 0, f"Frames not synced properly: {laggy_frames=}"