diff --git a/system/camerad/cameras/hw.h b/system/camerad/cameras/hw.h index f9ebc07052..bc40e01345 100644 --- a/system/camerad/cameras/hw.h +++ b/system/camerad/cameras/hw.h @@ -53,3 +53,5 @@ const CameraConfig DRIVER_CAMERA_CONFIG = { .phy = CAM_ISP_IFE_IN_RES_PHY_2, .vignetting_correction = false, }; + +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 34bd565e29..a66fb652b5 100644 --- a/system/camerad/cameras/spectra.cc +++ b/system/camerad/cameras/spectra.cc @@ -1,5 +1,6 @@ #include "cdm.h" +#include #include #include #include @@ -1377,9 +1378,6 @@ void SpectraCamera::handle_camera_event(const cam_req_mgr_message *event_data) { uint64_t frame_id_raw = event_data->u.frame_msg.frame_id; if (request_id != 0) { // next ready - if (request_id == 1) { - frame_id_offset = frame_id_raw; - } int buf_idx = (request_id - 1) % ife_buf_depth; // check for skipped_last frames @@ -1402,13 +1400,19 @@ void SpectraCamera::handle_camera_event(const cam_req_mgr_message *event_data) { frame_id_raw_last = frame_id_raw; request_id_last = request_id; - auto &meta_data = buf.frame_metadata[buf_idx]; - meta_data.frame_id = frame_id_raw - frame_id_offset; - meta_data.request_id = request_id; - meta_data.timestamp_sof = event_data->u.frame_msg.timestamp; // this is timestamped in the kernel's SOF IRQ callback + 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)) { + 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; + meta_data.timestamp_sof = timestamp; - // wait for this frame's EOF, then queue up the next one - enqueue_req_multi(request_id + ife_buf_depth, 1, 1); + // wait for this frame's EOF, then queue up the next one + enqueue_req_multi(request_id + ife_buf_depth, 1, true); + } else { + // Frames not yet synced + enqueue_req_multi(request_id + ife_buf_depth, 1, false); + } } else { // not ready if (frame_id_raw > frame_id_raw_last + 10) { LOGE("camera %d reset after half second of no response", cc.camera_num); @@ -1419,3 +1423,46 @@ 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) { + std::lock_guard lk(frame_sync_mutex); + + if (first_frame_synced) return true; + + // Store the frame data for this camera + camera_sync_data[camera_id] = SyncData{raw_id, timestamp, raw_id + 1}; + + // Ensure all cameras are up + bool all_cams_up = true; + for (const auto& config : ALL_CAMERA_CONFIGS) { + if (camera_sync_data.find(config.camera_num) == camera_sync_data.end()) { + all_cams_up = false; + } + } + + // Wait until the timestamps line up + bool all_cams_synced = true; + uint64_t reference_timestamp = camera_sync_data.begin()->second.timestamp; + for (const auto &[_, sync_data] : camera_sync_data) { + uint64_t diff = std::max(reference_timestamp, sync_data.timestamp) - + std::min(reference_timestamp, sync_data.timestamp); + if (diff > 2*1e6) { // within 2ms + all_cams_synced = false; + } + } + + if (all_cams_up && all_cams_synced) { + first_frame_synced = true; + for (auto &[cam, sync_data] : camera_sync_data) { + LOGW("camera %d synced on frame_id_offset %ld timestamp %lu", cam, camera_sync_data[cam].frame_id_offset, camera_sync_data[cam].timestamp); + } + } + + // Timeout in case the timestamps never line up + if (raw_id > 40) { + LOGE("camera first frame sync timed out"); + first_frame_synced = true; + } + + return false; +} diff --git a/system/camerad/cameras/spectra.h b/system/camerad/cameras/spectra.h index 71113d07b3..91984cefab 100644 --- a/system/camerad/cameras/spectra.h +++ b/system/camerad/cameras/spectra.h @@ -3,6 +3,7 @@ #include #include #include +#include #include #include @@ -189,7 +190,7 @@ public: uint64_t request_ids[MAX_IFE_BUFS] = {}; uint64_t request_id_last = 0; uint64_t frame_id_raw_last = 0; - uint64_t frame_id_offset = 0; + int64_t frame_id_offset = 0; bool skipped_last = true; SpectraOutputType output_type; @@ -197,4 +198,15 @@ public: CameraBuf buf; MemoryManager mm; SpectraMaster *m; + +private: + static bool syncFirstFrame(int camera_id, uint64_t raw_id, uint64_t timestamp); + struct SyncData { + uint64_t raw_id; + uint64_t timestamp; + uint64_t frame_id_offset = 0; + }; + inline static std::map camera_sync_data; + inline static bool first_frame_synced = false; + inline static std::mutex frame_sync_mutex; };