sadmen
/
openpilot_comma
mirror of https://github.com/commaai/openpilot.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

camerad: cleaner queueing logic (#34786)

Browse Source
notmaster30
Adeeb Shihadeh 2 months ago committed by GitHub
parent f4d17cbfdd
commit 4bb0dfd59c
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194
2 changed files with 122 additions and 132 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 239
      system/camerad/cameras/spectra.cc
  2. 15
      system/camerad/cameras/spectra.h

239
system/camerad/cameras/spectra.cc
Unescape 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;
// raw as opposed to our re-indexed frame ID
uint64_t frame_id_raw = event_data->u.frame_msg.frame_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;
// 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;
}
//LOGD("handle cam %d, request id %lu -> %lu, frame id raw %lu", cc.camera_num, request_id_last, request_id, frame_id_raw);
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;
}
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;
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;
}
// 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));
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;
}
// 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;
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;
}
return false;
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::syncFirstFrame(int camera_id, uint64_t request_id, uint64_t raw_id, uint64_t timestamp) {
if (first_frame_synced) return true;
bool SpectraCamera::waitForFrameReady(uint64_t request_id) {
int buf_idx = request_id % ife_buf_depth;
assert(sync_objs_ife[buf_idx]);
// OX and OS cameras require a few frames for the FSIN to sync up
if (request_id < 3) {
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;
// 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
Unescape 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;
}

Write Preview
Loading…
Cancel
Save