diff --git a/panda b/panda index d1410f7f7..c10b82f8f 160000 --- a/panda +++ b/panda @@ -1 +1 @@ -Subproject commit d1410f7f7b061171c3702d84d975a3da3afce109 +Subproject commit c10b82f8ff03c7d677a9420c21c0c50126a5071e diff --git a/selfdrive/test/test_onroad.py b/selfdrive/test/test_onroad.py index 008b8ebe7..1129a1a2f 100644 --- a/selfdrive/test/test_onroad.py +++ b/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'), diff --git a/system/camerad/cameras/hw.h b/system/camerad/cameras/hw.h index f20a1b3ad..defe878e8 100644 --- a/system/camerad/cameras/hw.h +++ b/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}; diff --git a/system/camerad/cameras/spectra.cc b/system/camerad/cameras/spectra.cc index 73e0a78da..ab9d8e069 100644 --- a/system/camerad/cameras/spectra.cc +++ b/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; } } diff --git a/system/camerad/cameras/spectra.h b/system/camerad/cameras/spectra.h index a02b8a6ca..7dd113525 100644 --- a/system/camerad/cameras/spectra.h +++ b/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 camera_sync_data; inline static bool first_frame_synced = false; diff --git a/system/camerad/test/test_camerad.py b/system/camerad/test/test_camerad.py index 5f8de8689..3abe4db65 100644 --- a/system/camerad/test/test_camerad.py +++ b/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)