mawei/sunnypilot
1
0
Fork 0
mirror of https://github.com/sunnypilot/sunnypilot.git synced 2026-02-19 11:13:53 +08:00
Code Issues Packages Projects Releases Wiki Activity

camerad: cleaner queueing logic (#34786)

Browse Source
This commit is contained in:
Adeeb Shihadeh
2025-03-05 17:12:30 -08:00
committed by GitHub
parent f4d17cbfdd
commit 4bb0dfd59c
2 changed files with 128 additions and 138 deletions
Download Patch File Download Diff File Expand all files Collapse all files

251
system/camerad/cameras/spectra.cc
View File

@@ -249,6 +249,7 @@ SpectraCamera::~SpectraCamera() {
}
int SpectraCamera::clear_req_queue() {
// for "non-realtime" BPS
if (icp_dev_handle > 0) {
struct cam_flush_dev_cmd cmd = {
.session_handle = session_handle,
@@ -260,6 +261,7 @@ int SpectraCamera::clear_req_queue() {
LOGD("flushed bps: %d", err);
}
// for "realtime" devices
struct cam_req_mgr_flush_info req_mgr_flush_request = {0};
req_mgr_flush_request.session_hdl = session_handle;
req_mgr_flush_request.link_hdl = link_handle;
@@ -304,14 +306,7 @@ void SpectraCamera::camera_open(VisionIpcServer *v, cl_device_id device_id, cl_c
LOGD("camera init %d", cc.camera_num);
buf.init(device_id, ctx, this, v, ife_buf_depth, cc.stream_type);
camera_map_bufs();
enqueue_req_multi(1, ife_buf_depth);
}
void SpectraCamera::enqueue_req_multi(uint64_t start, int n) {
for (uint64_t request_id = start; request_id < start + n; ++request_id) {
uint64_t idx = (request_id - 1) % ife_buf_depth;
enqueue_buffer(idx, request_id);
}
clearAndRequeue(1);
}
void SpectraCamera::sensors_start() {
@@ -901,59 +896,14 @@ void SpectraCamera::config_ife(int idx, int request_id, bool init) {
assert(ret == 0);
}
// Enqueue buffer for the given index and return true if the frame is ready
bool SpectraCamera::enqueue_buffer(int i, uint64_t request_id) {
int ret;
bool frame_ready = false;
// Before queuing up a new frame, wait for the
// previous one in this slot (index) to come in.
if (sync_objs_ife[i]) {
// TODO: write a test to stress test w/ a low timeout and check camera frame ids match
struct cam_sync_wait sync_wait = {0};
// *** Wait for IFE ***
// in RAW_OUTPUT mode, this is just the frame readout from the sensor
// in IFE_PROCESSED mode, this is both frame readout and image processing (~1ms)
sync_wait.sync_obj = sync_objs_ife[i];
sync_wait.timeout_ms = 100;
if (stress_test("IFE sync")) {
sync_wait.timeout_ms = 1;
}
ret = do_sync_control(m->cam_sync_fd, CAM_SYNC_WAIT, &sync_wait, sizeof(sync_wait));
if (ret != 0) {
LOGE("failed to wait for IFE sync: %d %d", ret, sync_wait.sync_obj);
}
// *** Wait for BPS ***
if (ret == 0 && sync_objs_bps[i]) {
sync_wait.sync_obj = sync_objs_bps[i];
sync_wait.timeout_ms = 50; // typically 7ms
if (stress_test("BPS sync")) {
sync_wait.timeout_ms = 1;
}
ret = do_sync_control(m->cam_sync_fd, CAM_SYNC_WAIT, &sync_wait, sizeof(sync_wait));
if (ret != 0) {
LOGE("failed to wait for BPS sync: %d %d", ret, sync_wait.sync_obj);
}
}
if (ret == 0) {
// all good, hand off frame
frame_ready = true;
destroySyncObjectAt(i);
} else {
// need to start over on sync failures,
// otherwise future frames will tear
clear_req_queue();
}
}
void SpectraCamera::enqueue_frame(uint64_t request_id) {
int i = request_id % ife_buf_depth;
assert(sync_objs_ife[i] == 0);
// create output fences
struct cam_sync_info sync_create = {0};
strcpy(sync_create.name, "NodeOutputPortFence");
ret = do_sync_control(m->cam_sync_fd, CAM_SYNC_CREATE, &sync_create, sizeof(sync_create));
int ret = do_sync_control(m->cam_sync_fd, CAM_SYNC_CREATE, &sync_create, sizeof(sync_create));
if (ret != 0) {
LOGE("failed to create fence: %d %d", ret, sync_create.sync_obj);
} else {
@@ -985,8 +935,6 @@ bool SpectraCamera::enqueue_buffer(int i, uint64_t request_id) {
// submit request to IFE and BPS
config_ife(i, request_id);
if (output_type == ISP_BPS_PROCESSED) config_bps(i, request_id);
return frame_ready;
}
void SpectraCamera::destroySyncObjectAt(int index) {
@@ -1371,90 +1319,129 @@ void SpectraCamera::camera_close() {
LOGD("destroyed session %d: %d", cc.camera_num, ret);
}
// Processes camera events and returns true if the frame is ready for further processing
bool SpectraCamera::handle_camera_event(const cam_req_mgr_message *event_data) {
if (stress_test("skipping handling camera event")) {
LOGW("skipping event");
/*
Handles camera SOF event. Returns true if the frame is valid for publishing.
*/
uint64_t request_id = event_data->u.frame_msg.request_id; // ID from the camera request manager
uint64_t frame_id_raw = event_data->u.frame_msg.frame_id; // raw as opposed to our re-indexed frame ID
uint64_t timestamp = event_data->u.frame_msg.timestamp; // timestamped in the kernel's SOF IRQ callback
//LOGD("handle cam %d ts %lu req id %lu frame id %lu", cc.camera_num, timestamp, request_id, frame_id_raw);
if (stress_test("skipping SOF event")) return false;
if (!validateEvent(request_id, frame_id_raw)) {
return false;
}
// ID from the qcom camera request manager
uint64_t request_id = event_data->u.frame_msg.request_id;
// Update tracking variables
if (request_id == request_id_last + 1) {
skip_expected = false;
}
frame_id_raw_last = frame_id_raw;
request_id_last = request_id;
// raw as opposed to our re-indexed frame ID
uint64_t frame_id_raw = event_data->u.frame_msg.frame_id;
//LOGD("handle cam %d, request id %lu -> %lu, frame id raw %lu", cc.camera_num, request_id_last, request_id, frame_id_raw);
if (request_id != 0) { // next ready
// check for skipped_last frames
if (frame_id_raw > frame_id_raw_last + 1 && !skipped_last) {
LOGE("camera %d realign", cc.camera_num);
clear_req_queue();
enqueue_req_multi(request_id + 1, ife_buf_depth - 1);
skipped_last = true;
} else if (frame_id_raw == frame_id_raw_last + 1) {
skipped_last = false;
}
// check for dropped requests
if (request_id > request_id_last + 1) {
LOGE("camera %d dropped requests %ld %ld", cc.camera_num, request_id, request_id_last);
enqueue_req_multi(request_id_last + 1 + ife_buf_depth, request_id - (request_id_last + 1));
}
// metas
frame_id_raw_last = frame_id_raw;
request_id_last = request_id;
int buf_idx = (request_id - 1) % ife_buf_depth;
uint64_t timestamp = event_data->u.frame_msg.timestamp; // this is timestamped in the kernel's SOF IRQ callback
if (syncFirstFrame(cc.camera_num, request_id, frame_id_raw, timestamp)) {
// wait for this frame's EOF, then queue up the next one
if (enqueue_buffer(buf_idx, request_id + ife_buf_depth)) {
// Frame is ready
// in IFE_PROCESSED mode, we can't know the true EOF, so recover it with sensor readout time
uint64_t timestamp_eof = timestamp + sensor->readout_time_ns;
// Update buffer and frame data
buf.cur_buf_idx = buf_idx;
buf.cur_frame_data = {
.frame_id = (uint32_t)(frame_id_raw - camera_sync_data[cc.camera_num].frame_id_offset),
.request_id = (uint32_t)request_id,
.timestamp_sof = timestamp,
.timestamp_eof = timestamp_eof,
.processing_time = float((nanos_since_boot() - timestamp_eof) * 1e-9)
};
return 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);
// 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) {
LOGE("camera %d reset after half second of no response", cc.camera_num);
clear_req_queue();
enqueue_req_multi(request_id_last + 1, ife_buf_depth);
frame_id_raw_last = frame_id_raw;
skipped_last = true;
}
// Wait until frame's fully read out and processed
if (!waitForFrameReady(request_id)) {
// Reset queue on sync failure to prevent frame tearing
LOGE("camera %d sync failure %ld %ld ", cc.camera_num, request_id, frame_id_raw);
clearAndRequeue(request_id + 1);
return false;
}
return false;
int buf_idx = request_id % ife_buf_depth;
bool ret = processFrame(buf_idx, request_id, frame_id_raw, timestamp);
destroySyncObjectAt(buf_idx);
enqueue_frame(request_id + ife_buf_depth); // request next frame for this slot
return ret;
}
bool SpectraCamera::validateEvent(uint64_t request_id, uint64_t frame_id_raw) {
// check if the request ID is even valid. this happens after queued
// requests are cleared. unclear if it happens any other time.
if (request_id == 0) {
if (invalid_request_count++ > 10) {
LOGE("camera %d reset after half second of invalid requests", cc.camera_num);
clearAndRequeue(request_id_last + 1);
invalid_request_count = 0;
}
return false;
}
invalid_request_count = 0;
// check for skips in frame_id or request_id
if (!skip_expected) {
if (frame_id_raw != frame_id_raw_last + 1) {
LOGE("camera %d frame ID skipped, %lu -> %lu", cc.camera_num, frame_id_raw_last, frame_id_raw);
clearAndRequeue(request_id + 1);
return false;
}
if (request_id != request_id_last + 1) {
LOGE("camera %d requests skipped %ld -> %ld", cc.camera_num, request_id_last, request_id);
clearAndRequeue(request_id_last + 1);
return false;
}
}
return true;
}
void SpectraCamera::clearAndRequeue(uint64_t from_request_id) {
// clear everything, then queue up a fresh set of frames
LOGW("clearing and requeuing camera %d from %lu", cc.camera_num, from_request_id);
clear_req_queue();
for (uint64_t id = from_request_id; id < from_request_id + ife_buf_depth; ++id) {
enqueue_frame(id);
}
skip_expected = true;
}
bool SpectraCamera::waitForFrameReady(uint64_t request_id) {
int buf_idx = request_id % ife_buf_depth;
assert(sync_objs_ife[buf_idx]);
auto waitForSync = [&](uint32_t sync_obj, int timeout_ms, const char *sync_type) {
struct cam_sync_wait sync_wait = {};
sync_wait.sync_obj = sync_obj;
sync_wait.timeout_ms = stress_test(sync_type) ? 1 : timeout_ms;
return do_sync_control(m->cam_sync_fd, CAM_SYNC_WAIT, &sync_wait, sizeof(sync_wait)) == 0;
};
// wait for frame from IFE
// - in RAW_OUTPUT mode, this time is just the frame readout from the sensor
// - in IFE_PROCESSED mode, this time also includes image processing (~1ms)
bool success = waitForSync(sync_objs_ife[buf_idx], 100, "IFE sync");
if (success && sync_objs_bps[buf_idx]) {
// BPS is typically 7ms
success = waitForSync(sync_objs_bps[buf_idx], 50, "BPS sync");
}
return success;
}
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)) {
return false;
}
// in IFE_PROCESSED mode, we can't know the true EOF, so recover it with sensor readout time
uint64_t timestamp_eof = timestamp + sensor->readout_time_ns;
// Update buffer and frame data
buf.cur_buf_idx = buf_idx;
buf.cur_frame_data = {
.frame_id = (uint32_t)(frame_id_raw - camera_sync_data[cc.camera_num].frame_id_offset),
.request_id = (uint32_t)request_id,
.timestamp_sof = timestamp,
.timestamp_eof = timestamp_eof,
.processing_time = float((nanos_since_boot() - timestamp_eof) * 1e-9)
};
return true;
}
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};
@@ -1468,7 +1455,7 @@ bool SpectraCamera::syncFirstFrame(int camera_id, uint64_t request_id, uint64_t
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 > 0.5*1e6) { // within 0.5ms
if (diff > 0.2*1e6) { // milliseconds
all_cams_synced = false;
}
}

15
system/camerad/cameras/spectra.h
View File

@@ -128,8 +128,7 @@ public:
void config_ife(int idx, int request_id, bool init=false);
int clear_req_queue();
bool enqueue_buffer(int i, uint64_t request_id);
void enqueue_req_multi(uint64_t start, int n);
void enqueue_frame(uint64_t request_id);
int sensors_init();
void sensors_start();
@@ -190,8 +189,8 @@ public:
int sync_objs_bps[MAX_IFE_BUFS] = {};
uint64_t request_id_last = 0;
uint64_t frame_id_raw_last = 0;
int64_t frame_id_offset = 0;
bool skipped_last = true;
int invalid_request_count = 0;
bool skip_expected = true;
SpectraOutputType output_type;
@@ -199,6 +198,10 @@ public:
SpectraMaster *m;
private:
void clearAndRequeue(uint64_t from_request_id);
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);
struct SyncData {
uint64_t timestamp;
@@ -208,11 +211,11 @@ private:
inline static bool first_frame_synced = false;
// a mode for stressing edge cases: realignment, sync failures, etc.
inline bool stress_test(const char* log, float prob=0.01) {
inline bool stress_test(const char* log, float prob=0.02) {
static bool enable = getenv("SPECTRA_STRESS_TEST") != nullptr;
bool triggered = enable && ((static_cast<double>(rand()) / RAND_MAX) < prob);
if (triggered) {
LOGE("stress test: %s", log);
LOGE("stress test (cam %d): %s", cc.camera_num, log);
}
return triggered;
}