camera_qcom2 refactor (#23946)
* convert camerastate to a class * more refactor * do the refactor in file * put that one back * those changes can wait * very minor Co-authored-by: Comma Device <device@comma.ai> old-commit-hash: 0adfef0b3423291101520424b3bb2c3a1b39dc6c
This commit is contained in:
George Hotz
@@ -56,7 +56,7 @@ const int EXPOSURE_TIME_MIN = 2; // with HDR, fastest ss
|
||||
const int EXPOSURE_TIME_MAX = 1904; // with HDR, slowest ss
|
||||
|
||||
// ************** low level camera helpers ****************
|
||||
int cam_control(int fd, int op_code, void *handle, int size) {
|
||||
int do_cam_control(int fd, int op_code, void *handle, int size) {
|
||||
struct cam_control camcontrol = {0};
|
||||
camcontrol.op_code = op_code;
|
||||
camcontrol.handle = (uint64_t)handle;
|
||||
@@ -83,7 +83,7 @@ std::optional<int32_t> device_acquire(int fd, int32_t session_handle, void *data
|
||||
.num_resources = (uint32_t)(data ? 1 : 0),
|
||||
.resource_hdl = (uint64_t)data,
|
||||
};
|
||||
int err = cam_control(fd, CAM_ACQUIRE_DEV, &cmd, sizeof(cmd));
|
||||
int err = do_cam_control(fd, CAM_ACQUIRE_DEV, &cmd, sizeof(cmd));
|
||||
return err == 0 ? std::make_optional(cmd.dev_handle) : std::nullopt;
|
||||
};
|
||||
|
||||
@@ -93,13 +93,13 @@ int device_config(int fd, int32_t session_handle, int32_t dev_handle, uint64_t p
|
||||
.dev_handle = dev_handle,
|
||||
.packet_handle = packet_handle,
|
||||
};
|
||||
return cam_control(fd, CAM_CONFIG_DEV, &cmd, sizeof(cmd));
|
||||
return do_cam_control(fd, CAM_CONFIG_DEV, &cmd, sizeof(cmd));
|
||||
}
|
||||
|
||||
int device_control(int fd, int op_code, int session_handle, int dev_handle) {
|
||||
// start stop and release are all the same
|
||||
struct cam_start_stop_dev_cmd cmd { .session_handle = session_handle, .dev_handle = dev_handle };
|
||||
return cam_control(fd, op_code, &cmd, sizeof(cmd));
|
||||
return do_cam_control(fd, op_code, &cmd, sizeof(cmd));
|
||||
}
|
||||
|
||||
void *alloc_w_mmu_hdl(int video0_fd, int len, uint32_t *handle, int align = 8, int flags = CAM_MEM_FLAG_KMD_ACCESS | CAM_MEM_FLAG_UMD_ACCESS | CAM_MEM_FLAG_CMD_BUF_TYPE,
|
||||
@@ -118,7 +118,7 @@ void *alloc_w_mmu_hdl(int video0_fd, int len, uint32_t *handle, int align = 8, i
|
||||
mem_mgr_alloc_cmd.num_hdl++;
|
||||
}
|
||||
|
||||
cam_control(video0_fd, CAM_REQ_MGR_ALLOC_BUF, &mem_mgr_alloc_cmd, sizeof(mem_mgr_alloc_cmd));
|
||||
do_cam_control(video0_fd, CAM_REQ_MGR_ALLOC_BUF, &mem_mgr_alloc_cmd, sizeof(mem_mgr_alloc_cmd));
|
||||
*handle = mem_mgr_alloc_cmd.out.buf_handle;
|
||||
|
||||
void *ptr = NULL;
|
||||
@@ -137,7 +137,7 @@ void release(int video0_fd, uint32_t handle) {
|
||||
struct cam_mem_mgr_release_cmd mem_mgr_release_cmd = {0};
|
||||
mem_mgr_release_cmd.buf_handle = handle;
|
||||
|
||||
ret = cam_control(video0_fd, CAM_REQ_MGR_RELEASE_BUF, &mem_mgr_release_cmd, sizeof(mem_mgr_release_cmd));
|
||||
ret = do_cam_control(video0_fd, CAM_REQ_MGR_RELEASE_BUF, &mem_mgr_release_cmd, sizeof(mem_mgr_release_cmd));
|
||||
assert(ret == 0);
|
||||
}
|
||||
|
||||
@@ -153,34 +153,39 @@ void clear_req_queue(int fd, int32_t session_hdl, int32_t link_hdl) {
|
||||
req_mgr_flush_request.link_hdl = link_hdl;
|
||||
req_mgr_flush_request.flush_type = CAM_REQ_MGR_FLUSH_TYPE_ALL;
|
||||
int ret;
|
||||
ret = cam_control(fd, CAM_REQ_MGR_FLUSH_REQ, &req_mgr_flush_request, sizeof(req_mgr_flush_request));
|
||||
ret = do_cam_control(fd, CAM_REQ_MGR_FLUSH_REQ, &req_mgr_flush_request, sizeof(req_mgr_flush_request));
|
||||
// LOGD("flushed all req: %d", ret);
|
||||
}
|
||||
|
||||
// ************** high level camera helpers ****************
|
||||
|
||||
void sensors_poke(struct CameraState *s, int request_id) {
|
||||
void CameraState::sensors_start() {
|
||||
int start_reg_len = sizeof(start_reg_array) / sizeof(struct i2c_random_wr_payload);
|
||||
sensors_i2c(start_reg_array, start_reg_len, CAM_SENSOR_PACKET_OPCODE_SENSOR_CONFIG);
|
||||
}
|
||||
|
||||
void CameraState::sensors_poke(int request_id) {
|
||||
uint32_t cam_packet_handle = 0;
|
||||
int size = sizeof(struct cam_packet);
|
||||
struct cam_packet *pkt = (struct cam_packet *)alloc_w_mmu_hdl(s->multi_cam_state->video0_fd, size, &cam_packet_handle);
|
||||
struct cam_packet *pkt = (struct cam_packet *)alloc_w_mmu_hdl(multi_cam_state->video0_fd, size, &cam_packet_handle);
|
||||
pkt->num_cmd_buf = 0;
|
||||
pkt->kmd_cmd_buf_index = -1;
|
||||
pkt->header.size = size;
|
||||
pkt->header.op_code = 0x7f;
|
||||
pkt->header.request_id = request_id;
|
||||
|
||||
int ret = device_config(s->sensor_fd, s->session_handle, s->sensor_dev_handle, cam_packet_handle);
|
||||
int ret = device_config(sensor_fd, session_handle, sensor_dev_handle, cam_packet_handle);
|
||||
assert(ret == 0);
|
||||
|
||||
munmap(pkt, size);
|
||||
release_fd(s->multi_cam_state->video0_fd, cam_packet_handle);
|
||||
release_fd(multi_cam_state->video0_fd, cam_packet_handle);
|
||||
}
|
||||
|
||||
void sensors_i2c(struct CameraState *s, struct i2c_random_wr_payload* dat, int len, int op_code) {
|
||||
void CameraState::sensors_i2c(struct i2c_random_wr_payload* dat, int len, int op_code) {
|
||||
// LOGD("sensors_i2c: %d", len);
|
||||
uint32_t cam_packet_handle = 0;
|
||||
int size = sizeof(struct cam_packet)+sizeof(struct cam_cmd_buf_desc)*1;
|
||||
struct cam_packet *pkt = (struct cam_packet *)alloc_w_mmu_hdl(s->multi_cam_state->video0_fd, size, &cam_packet_handle);
|
||||
struct cam_packet *pkt = (struct cam_packet *)alloc_w_mmu_hdl(multi_cam_state->video0_fd, size, &cam_packet_handle);
|
||||
pkt->num_cmd_buf = 1;
|
||||
pkt->kmd_cmd_buf_index = -1;
|
||||
pkt->header.size = size;
|
||||
@@ -190,7 +195,7 @@ void sensors_i2c(struct CameraState *s, struct i2c_random_wr_payload* dat, int l
|
||||
buf_desc[0].size = buf_desc[0].length = sizeof(struct i2c_rdwr_header) + len*sizeof(struct i2c_random_wr_payload);
|
||||
buf_desc[0].type = CAM_CMD_BUF_I2C;
|
||||
|
||||
struct cam_cmd_i2c_random_wr *i2c_random_wr = (struct cam_cmd_i2c_random_wr *)alloc_w_mmu_hdl(s->multi_cam_state->video0_fd, buf_desc[0].size, (uint32_t*)&buf_desc[0].mem_handle);
|
||||
struct cam_cmd_i2c_random_wr *i2c_random_wr = (struct cam_cmd_i2c_random_wr *)alloc_w_mmu_hdl(multi_cam_state->video0_fd, buf_desc[0].size, (uint32_t*)&buf_desc[0].mem_handle);
|
||||
i2c_random_wr->header.count = len;
|
||||
i2c_random_wr->header.op_code = 1;
|
||||
i2c_random_wr->header.cmd_type = CAMERA_SENSOR_CMD_TYPE_I2C_RNDM_WR;
|
||||
@@ -198,14 +203,15 @@ void sensors_i2c(struct CameraState *s, struct i2c_random_wr_payload* dat, int l
|
||||
i2c_random_wr->header.addr_type = CAMERA_SENSOR_I2C_TYPE_WORD;
|
||||
memcpy(i2c_random_wr->random_wr_payload, dat, len*sizeof(struct i2c_random_wr_payload));
|
||||
|
||||
int ret = device_config(s->sensor_fd, s->session_handle, s->sensor_dev_handle, cam_packet_handle);
|
||||
int ret = device_config(sensor_fd, session_handle, sensor_dev_handle, cam_packet_handle);
|
||||
assert(ret == 0);
|
||||
|
||||
munmap(i2c_random_wr, buf_desc[0].size);
|
||||
release_fd(s->multi_cam_state->video0_fd, buf_desc[0].mem_handle);
|
||||
release_fd(multi_cam_state->video0_fd, buf_desc[0].mem_handle);
|
||||
munmap(pkt, size);
|
||||
release_fd(s->multi_cam_state->video0_fd, cam_packet_handle);
|
||||
release_fd(multi_cam_state->video0_fd, cam_packet_handle);
|
||||
}
|
||||
|
||||
static cam_cmd_power *power_set_wait(cam_cmd_power *power, int16_t delay_ms) {
|
||||
cam_cmd_unconditional_wait *unconditional_wait = (cam_cmd_unconditional_wait *)((char *)power + (sizeof(struct cam_cmd_power) + (power->count - 1) * sizeof(struct cam_power_settings)));
|
||||
unconditional_wait->cmd_type = CAMERA_SENSOR_CMD_TYPE_WAIT;
|
||||
@@ -214,7 +220,8 @@ static cam_cmd_power *power_set_wait(cam_cmd_power *power, int16_t delay_ms) {
|
||||
return (struct cam_cmd_power *)(unconditional_wait + 1);
|
||||
};
|
||||
|
||||
void sensors_init(int video0_fd, int sensor_fd, int camera_num) {
|
||||
void CameraState::sensors_init() {
|
||||
int video0_fd = multi_cam_state->video0_fd;
|
||||
uint32_t cam_packet_handle = 0;
|
||||
int size = sizeof(struct cam_packet)+sizeof(struct cam_cmd_buf_desc)*2;
|
||||
struct cam_packet *pkt = (struct cam_packet *)alloc_w_mmu_hdl(video0_fd, size, &cam_packet_handle);
|
||||
@@ -333,7 +340,7 @@ void sensors_init(int video0_fd, int sensor_fd, int camera_num) {
|
||||
power->power_settings[2].power_seq_type = 3;
|
||||
|
||||
LOGD("probing the sensor");
|
||||
int ret = cam_control(sensor_fd, CAM_SENSOR_PROBE_CMD, (void *)(uintptr_t)cam_packet_handle, 0);
|
||||
int ret = do_cam_control(sensor_fd, CAM_SENSOR_PROBE_CMD, (void *)(uintptr_t)cam_packet_handle, 0);
|
||||
assert(ret == 0);
|
||||
|
||||
munmap(i2c_info, buf_desc[0].size);
|
||||
@@ -344,13 +351,13 @@ void sensors_init(int video0_fd, int sensor_fd, int camera_num) {
|
||||
release_fd(video0_fd, cam_packet_handle);
|
||||
}
|
||||
|
||||
void config_isp(struct CameraState *s, int io_mem_handle, int fence, int request_id, int buf0_mem_handle, int buf0_offset) {
|
||||
void CameraState::config_isp(int io_mem_handle, int fence, int request_id, int buf0_mem_handle, int buf0_offset) {
|
||||
uint32_t cam_packet_handle = 0;
|
||||
int size = sizeof(struct cam_packet)+sizeof(struct cam_cmd_buf_desc)*2;
|
||||
if (io_mem_handle != 0) {
|
||||
size += sizeof(struct cam_buf_io_cfg);
|
||||
}
|
||||
struct cam_packet *pkt = (struct cam_packet *)alloc_w_mmu_hdl(s->multi_cam_state->video0_fd, size, &cam_packet_handle);
|
||||
struct cam_packet *pkt = (struct cam_packet *)alloc_w_mmu_hdl(multi_cam_state->video0_fd, size, &cam_packet_handle);
|
||||
pkt->num_cmd_buf = 2;
|
||||
pkt->kmd_cmd_buf_index = 0;
|
||||
// YUV has kmd_cmd_buf_offset = 1780
|
||||
@@ -445,7 +452,7 @@ void config_isp(struct CameraState *s, int io_mem_handle, int fence, int request
|
||||
buf_desc[1].length = buf_desc[1].size - buf_desc[1].offset;
|
||||
buf_desc[1].type = CAM_CMD_BUF_GENERIC;
|
||||
buf_desc[1].meta_data = CAM_ISP_PACKET_META_GENERIC_BLOB_COMMON;
|
||||
uint32_t *buf2 = (uint32_t *)alloc_w_mmu_hdl(s->multi_cam_state->video0_fd, buf_desc[1].size, (uint32_t*)&buf_desc[1].mem_handle, 0x20);
|
||||
uint32_t *buf2 = (uint32_t *)alloc_w_mmu_hdl(multi_cam_state->video0_fd, buf_desc[1].size, (uint32_t*)&buf_desc[1].mem_handle, 0x20);
|
||||
memcpy(buf2, &tmp, sizeof(tmp));
|
||||
|
||||
if (io_mem_handle != 0) {
|
||||
@@ -475,133 +482,133 @@ void config_isp(struct CameraState *s, int io_mem_handle, int fence, int request
|
||||
io_cfg[0].framedrop_pattern = 0x1;
|
||||
}
|
||||
|
||||
int ret = device_config(s->multi_cam_state->isp_fd, s->session_handle, s->isp_dev_handle, cam_packet_handle);
|
||||
int ret = device_config(multi_cam_state->isp_fd, session_handle, isp_dev_handle, cam_packet_handle);
|
||||
assert(ret == 0);
|
||||
if (ret != 0) {
|
||||
printf("ISP CONFIG FAILED\n");
|
||||
}
|
||||
|
||||
munmap(buf2, buf_desc[1].size);
|
||||
release_fd(s->multi_cam_state->video0_fd, buf_desc[1].mem_handle);
|
||||
// release_fd(s->multi_cam_state->video0_fd, buf_desc[0].mem_handle);
|
||||
release_fd(multi_cam_state->video0_fd, buf_desc[1].mem_handle);
|
||||
// release_fd(multi_cam_state->video0_fd, buf_desc[0].mem_handle);
|
||||
munmap(pkt, size);
|
||||
release_fd(s->multi_cam_state->video0_fd, cam_packet_handle);
|
||||
release_fd(multi_cam_state->video0_fd, cam_packet_handle);
|
||||
}
|
||||
|
||||
void enqueue_buffer(struct CameraState *s, int i, bool dp) {
|
||||
void CameraState::enqueue_buffer(int i, bool dp) {
|
||||
int ret;
|
||||
int request_id = s->request_ids[i];
|
||||
int request_id = request_ids[i];
|
||||
|
||||
if (s->buf_handle[i]) {
|
||||
release(s->multi_cam_state->video0_fd, s->buf_handle[i]);
|
||||
if (buf_handle[i]) {
|
||||
release(multi_cam_state->video0_fd, buf_handle[i]);
|
||||
// wait
|
||||
struct cam_sync_wait sync_wait = {0};
|
||||
sync_wait.sync_obj = s->sync_objs[i];
|
||||
sync_wait.sync_obj = sync_objs[i];
|
||||
sync_wait.timeout_ms = 50; // max dt tolerance, typical should be 23
|
||||
ret = cam_control(s->multi_cam_state->video1_fd, CAM_SYNC_WAIT, &sync_wait, sizeof(sync_wait));
|
||||
ret = do_cam_control(multi_cam_state->video1_fd, CAM_SYNC_WAIT, &sync_wait, sizeof(sync_wait));
|
||||
// LOGD("fence wait: %d %d", ret, sync_wait.sync_obj);
|
||||
|
||||
s->buf.camera_bufs_metadata[i].timestamp_eof = (uint64_t)nanos_since_boot(); // set true eof
|
||||
if (dp) s->buf.queue(i);
|
||||
buf.camera_bufs_metadata[i].timestamp_eof = (uint64_t)nanos_since_boot(); // set true eof
|
||||
if (dp) buf.queue(i);
|
||||
|
||||
// destroy old output fence
|
||||
struct cam_sync_info sync_destroy = {0};
|
||||
strcpy(sync_destroy.name, "NodeOutputPortFence");
|
||||
sync_destroy.sync_obj = s->sync_objs[i];
|
||||
ret = cam_control(s->multi_cam_state->video1_fd, CAM_SYNC_DESTROY, &sync_destroy, sizeof(sync_destroy));
|
||||
sync_destroy.sync_obj = sync_objs[i];
|
||||
ret = do_cam_control(multi_cam_state->video1_fd, CAM_SYNC_DESTROY, &sync_destroy, sizeof(sync_destroy));
|
||||
// LOGD("fence destroy: %d %d", ret, sync_destroy.sync_obj);
|
||||
}
|
||||
|
||||
// do stuff
|
||||
struct cam_req_mgr_sched_request req_mgr_sched_request = {0};
|
||||
req_mgr_sched_request.session_hdl = s->session_handle;
|
||||
req_mgr_sched_request.link_hdl = s->link_handle;
|
||||
req_mgr_sched_request.session_hdl = session_handle;
|
||||
req_mgr_sched_request.link_hdl = link_handle;
|
||||
req_mgr_sched_request.req_id = request_id;
|
||||
ret = cam_control(s->multi_cam_state->video0_fd, CAM_REQ_MGR_SCHED_REQ, &req_mgr_sched_request, sizeof(req_mgr_sched_request));
|
||||
ret = do_cam_control(multi_cam_state->video0_fd, CAM_REQ_MGR_SCHED_REQ, &req_mgr_sched_request, sizeof(req_mgr_sched_request));
|
||||
// LOGD("sched req: %d %d", ret, request_id);
|
||||
|
||||
// create output fence
|
||||
struct cam_sync_info sync_create = {0};
|
||||
strcpy(sync_create.name, "NodeOutputPortFence");
|
||||
ret = cam_control(s->multi_cam_state->video1_fd, CAM_SYNC_CREATE, &sync_create, sizeof(sync_create));
|
||||
ret = do_cam_control(multi_cam_state->video1_fd, CAM_SYNC_CREATE, &sync_create, sizeof(sync_create));
|
||||
// LOGD("fence req: %d %d", ret, sync_create.sync_obj);
|
||||
s->sync_objs[i] = sync_create.sync_obj;
|
||||
sync_objs[i] = sync_create.sync_obj;
|
||||
|
||||
// configure ISP to put the image in place
|
||||
struct cam_mem_mgr_map_cmd mem_mgr_map_cmd = {0};
|
||||
mem_mgr_map_cmd.mmu_hdls[0] = s->multi_cam_state->device_iommu;
|
||||
mem_mgr_map_cmd.mmu_hdls[0] = multi_cam_state->device_iommu;
|
||||
mem_mgr_map_cmd.num_hdl = 1;
|
||||
mem_mgr_map_cmd.flags = CAM_MEM_FLAG_HW_READ_WRITE;
|
||||
mem_mgr_map_cmd.fd = s->buf.camera_bufs[i].fd;
|
||||
ret = cam_control(s->multi_cam_state->video0_fd, CAM_REQ_MGR_MAP_BUF, &mem_mgr_map_cmd, sizeof(mem_mgr_map_cmd));
|
||||
// LOGD("map buf req: (fd: %d) 0x%x %d", s->bufs[i].fd, mem_mgr_map_cmd.out.buf_handle, ret);
|
||||
s->buf_handle[i] = mem_mgr_map_cmd.out.buf_handle;
|
||||
mem_mgr_map_cmd.fd = buf.camera_bufs[i].fd;
|
||||
ret = do_cam_control(multi_cam_state->video0_fd, CAM_REQ_MGR_MAP_BUF, &mem_mgr_map_cmd, sizeof(mem_mgr_map_cmd));
|
||||
// LOGD("map buf req: (fd: %d) 0x%x %d", bufs[i].fd, mem_mgr_map_cmd.out.buf_handle, ret);
|
||||
buf_handle[i] = mem_mgr_map_cmd.out.buf_handle;
|
||||
|
||||
// poke sensor
|
||||
sensors_poke(s, request_id);
|
||||
sensors_poke(request_id);
|
||||
// LOGD("Poked sensor");
|
||||
|
||||
// push the buffer
|
||||
config_isp(s, s->buf_handle[i], s->sync_objs[i], request_id, s->buf0_handle, 65632*(i+1));
|
||||
config_isp(buf_handle[i], sync_objs[i], request_id, buf0_handle, 65632*(i+1));
|
||||
}
|
||||
|
||||
void enqueue_req_multi(struct CameraState *s, int start, int n, bool dp) {
|
||||
void CameraState::enqueue_req_multi(int start, int n, bool dp) {
|
||||
for (int i=start;i<start+n;++i) {
|
||||
s->request_ids[(i - 1) % FRAME_BUF_COUNT] = i;
|
||||
enqueue_buffer(s, (i - 1) % FRAME_BUF_COUNT, dp);
|
||||
request_ids[(i - 1) % FRAME_BUF_COUNT] = i;
|
||||
enqueue_buffer((i - 1) % FRAME_BUF_COUNT, dp);
|
||||
}
|
||||
}
|
||||
|
||||
// ******************* camera *******************
|
||||
|
||||
static void camera_init(MultiCameraState *multi_cam_state, VisionIpcServer * v, CameraState *s, int camera_id, int camera_num, unsigned int fps, cl_device_id device_id, cl_context ctx, VisionStreamType rgb_type, VisionStreamType yuv_type) {
|
||||
void CameraState::camera_init(MultiCameraState *multi_cam_state_, VisionIpcServer * v, int camera_id, int camera_num_, unsigned int fps, cl_device_id device_id, cl_context ctx, VisionStreamType rgb_type, VisionStreamType yuv_type) {
|
||||
LOGD("camera init %d", camera_num);
|
||||
s->multi_cam_state = multi_cam_state;
|
||||
multi_cam_state = multi_cam_state_;
|
||||
assert(camera_id < std::size(cameras_supported));
|
||||
s->ci = cameras_supported[camera_id];
|
||||
assert(s->ci.frame_width != 0);
|
||||
ci = cameras_supported[camera_id];
|
||||
assert(ci.frame_width != 0);
|
||||
|
||||
s->camera_num = camera_num;
|
||||
camera_num = camera_num_;
|
||||
|
||||
s->request_id_last = 0;
|
||||
s->skipped = true;
|
||||
request_id_last = 0;
|
||||
skipped = true;
|
||||
|
||||
s->min_ev = EXPOSURE_TIME_MIN * sensor_analog_gains[ANALOG_GAIN_MIN_IDX];
|
||||
s->max_ev = EXPOSURE_TIME_MAX * sensor_analog_gains[ANALOG_GAIN_MAX_IDX] * DC_GAIN;
|
||||
s->target_grey_fraction = 0.3;
|
||||
min_ev = EXPOSURE_TIME_MIN * sensor_analog_gains[ANALOG_GAIN_MIN_IDX];
|
||||
max_ev = EXPOSURE_TIME_MAX * sensor_analog_gains[ANALOG_GAIN_MAX_IDX] * DC_GAIN;
|
||||
target_grey_fraction = 0.3;
|
||||
|
||||
s->dc_gain_enabled = false;
|
||||
s->gain_idx = ANALOG_GAIN_REC_IDX;
|
||||
s->exposure_time = 5;
|
||||
s->cur_ev[0] = s->cur_ev[1] = s->cur_ev[2] = (s->dc_gain_enabled ? DC_GAIN : 1) * sensor_analog_gains[s->gain_idx] * s->exposure_time;
|
||||
dc_gain_enabled = false;
|
||||
gain_idx = ANALOG_GAIN_REC_IDX;
|
||||
exposure_time = 5;
|
||||
cur_ev[0] = cur_ev[1] = cur_ev[2] = (dc_gain_enabled ? DC_GAIN : 1) * sensor_analog_gains[gain_idx] * exposure_time;
|
||||
|
||||
s->buf.init(device_id, ctx, s, v, FRAME_BUF_COUNT, rgb_type, yuv_type);
|
||||
buf.init(device_id, ctx, this, v, FRAME_BUF_COUNT, rgb_type, yuv_type);
|
||||
}
|
||||
|
||||
static void camera_open(CameraState *s) {
|
||||
s->sensor_fd = open_v4l_by_name_and_index("cam-sensor-driver", s->camera_num);
|
||||
assert(s->sensor_fd >= 0);
|
||||
LOGD("opened sensor for %d", s->camera_num);
|
||||
void CameraState::camera_open() {
|
||||
sensor_fd = open_v4l_by_name_and_index("cam-sensor-driver", camera_num);
|
||||
assert(sensor_fd >= 0);
|
||||
LOGD("opened sensor for %d", camera_num);
|
||||
|
||||
// probe the sensor
|
||||
LOGD("-- Probing sensor %d", s->camera_num);
|
||||
sensors_init(s->multi_cam_state->video0_fd, s->sensor_fd, s->camera_num);
|
||||
LOGD("-- Probing sensor %d", camera_num);
|
||||
sensors_init();
|
||||
|
||||
// create session
|
||||
struct cam_req_mgr_session_info session_info = {};
|
||||
int ret = cam_control(s->multi_cam_state->video0_fd, CAM_REQ_MGR_CREATE_SESSION, &session_info, sizeof(session_info));
|
||||
int ret = do_cam_control(multi_cam_state->video0_fd, CAM_REQ_MGR_CREATE_SESSION, &session_info, sizeof(session_info));
|
||||
LOGD("get session: %d 0x%X", ret, session_info.session_hdl);
|
||||
s->session_handle = session_info.session_hdl;
|
||||
session_handle = session_info.session_hdl;
|
||||
|
||||
// access the sensor
|
||||
LOGD("-- Accessing sensor");
|
||||
auto sensor_dev_handle = device_acquire(s->sensor_fd, s->session_handle, nullptr);
|
||||
assert(sensor_dev_handle);
|
||||
s->sensor_dev_handle = *sensor_dev_handle;
|
||||
auto sensor_dev_handle_ = device_acquire(sensor_fd, session_handle, nullptr);
|
||||
assert(sensor_dev_handle_);
|
||||
sensor_dev_handle = *sensor_dev_handle_;
|
||||
LOGD("acquire sensor dev");
|
||||
|
||||
struct cam_isp_in_port_info in_port_info = {
|
||||
.res_type = (uint32_t[]){CAM_ISP_IFE_IN_RES_PHY_0, CAM_ISP_IFE_IN_RES_PHY_1, CAM_ISP_IFE_IN_RES_PHY_2}[s->camera_num],
|
||||
.res_type = (uint32_t[]){CAM_ISP_IFE_IN_RES_PHY_0, CAM_ISP_IFE_IN_RES_PHY_1, CAM_ISP_IFE_IN_RES_PHY_2}[camera_num],
|
||||
|
||||
.lane_type = CAM_ISP_LANE_TYPE_DPHY,
|
||||
.lane_num = 4,
|
||||
@@ -649,29 +656,29 @@ static void camera_open(CameraState *s) {
|
||||
.length = sizeof(in_port_info),
|
||||
};
|
||||
|
||||
auto isp_dev_handle = device_acquire(s->multi_cam_state->isp_fd, s->session_handle, &isp_resource);
|
||||
assert(isp_dev_handle);
|
||||
s->isp_dev_handle = *isp_dev_handle;
|
||||
auto isp_dev_handle_ = device_acquire(multi_cam_state->isp_fd, session_handle, &isp_resource);
|
||||
assert(isp_dev_handle_);
|
||||
isp_dev_handle = *isp_dev_handle_;
|
||||
LOGD("acquire isp dev");
|
||||
|
||||
s->csiphy_fd = open_v4l_by_name_and_index("cam-csiphy-driver", s->camera_num);
|
||||
assert(s->csiphy_fd >= 0);
|
||||
LOGD("opened csiphy for %d", s->camera_num);
|
||||
csiphy_fd = open_v4l_by_name_and_index("cam-csiphy-driver", camera_num);
|
||||
assert(csiphy_fd >= 0);
|
||||
LOGD("opened csiphy for %d", camera_num);
|
||||
|
||||
struct cam_csiphy_acquire_dev_info csiphy_acquire_dev_info = {.combo_mode = 0};
|
||||
auto csiphy_dev_handle = device_acquire(s->csiphy_fd, s->session_handle, &csiphy_acquire_dev_info);
|
||||
assert(csiphy_dev_handle);
|
||||
s->csiphy_dev_handle = *csiphy_dev_handle;
|
||||
auto csiphy_dev_handle_ = device_acquire(csiphy_fd, session_handle, &csiphy_acquire_dev_info);
|
||||
assert(csiphy_dev_handle_);
|
||||
csiphy_dev_handle = *csiphy_dev_handle_;
|
||||
LOGD("acquire csiphy dev");
|
||||
|
||||
// config ISP
|
||||
alloc_w_mmu_hdl(s->multi_cam_state->video0_fd, 984480, (uint32_t*)&s->buf0_handle, 0x20, CAM_MEM_FLAG_HW_READ_WRITE | CAM_MEM_FLAG_KMD_ACCESS | CAM_MEM_FLAG_UMD_ACCESS | CAM_MEM_FLAG_CMD_BUF_TYPE, s->multi_cam_state->device_iommu, s->multi_cam_state->cdm_iommu);
|
||||
config_isp(s, 0, 0, 1, s->buf0_handle, 0);
|
||||
alloc_w_mmu_hdl(multi_cam_state->video0_fd, 984480, (uint32_t*)&buf0_handle, 0x20, CAM_MEM_FLAG_HW_READ_WRITE | CAM_MEM_FLAG_KMD_ACCESS | CAM_MEM_FLAG_UMD_ACCESS | CAM_MEM_FLAG_CMD_BUF_TYPE, multi_cam_state->device_iommu, multi_cam_state->cdm_iommu);
|
||||
config_isp(0, 0, 1, buf0_handle, 0);
|
||||
|
||||
LOG("-- Configuring sensor");
|
||||
sensors_i2c(s, init_array_ar0231, std::size(init_array_ar0231), CAM_SENSOR_PACKET_OPCODE_SENSOR_CONFIG);
|
||||
//sensors_i2c(s, start_reg_array, std::size(start_reg_array), CAM_SENSOR_PACKET_OPCODE_SENSOR_STREAMON);
|
||||
//sensors_i2c(s, stop_reg_array, std::size(stop_reg_array), CAM_SENSOR_PACKET_OPCODE_SENSOR_STREAMOFF);
|
||||
sensors_i2c(init_array_ar0231, std::size(init_array_ar0231), CAM_SENSOR_PACKET_OPCODE_SENSOR_CONFIG);
|
||||
//sensors_i2c(start_reg_array, std::size(start_reg_array), CAM_SENSOR_PACKET_OPCODE_SENSOR_STREAMON);
|
||||
//sensors_i2c(stop_reg_array, std::size(stop_reg_array), CAM_SENSOR_PACKET_OPCODE_SENSOR_STREAMOFF);
|
||||
|
||||
|
||||
// config csiphy
|
||||
@@ -679,7 +686,7 @@ static void camera_open(CameraState *s) {
|
||||
{
|
||||
uint32_t cam_packet_handle = 0;
|
||||
int size = sizeof(struct cam_packet)+sizeof(struct cam_cmd_buf_desc)*1;
|
||||
struct cam_packet *pkt = (struct cam_packet *)alloc_w_mmu_hdl(s->multi_cam_state->video0_fd, size, &cam_packet_handle);
|
||||
struct cam_packet *pkt = (struct cam_packet *)alloc_w_mmu_hdl(multi_cam_state->video0_fd, size, &cam_packet_handle);
|
||||
pkt->num_cmd_buf = 1;
|
||||
pkt->kmd_cmd_buf_index = -1;
|
||||
pkt->header.size = size;
|
||||
@@ -688,7 +695,7 @@ static void camera_open(CameraState *s) {
|
||||
buf_desc[0].size = buf_desc[0].length = sizeof(struct cam_csiphy_info);
|
||||
buf_desc[0].type = CAM_CMD_BUF_GENERIC;
|
||||
|
||||
struct cam_csiphy_info *csiphy_info = (struct cam_csiphy_info *)alloc_w_mmu_hdl(s->multi_cam_state->video0_fd, buf_desc[0].size, (uint32_t*)&buf_desc[0].mem_handle);
|
||||
struct cam_csiphy_info *csiphy_info = (struct cam_csiphy_info *)alloc_w_mmu_hdl(multi_cam_state->video0_fd, buf_desc[0].size, (uint32_t*)&buf_desc[0].mem_handle);
|
||||
csiphy_info->lane_mask = 0x1f;
|
||||
csiphy_info->lane_assign = 0x3210;// skip clk. How is this 16 bit for 5 channels??
|
||||
csiphy_info->csiphy_3phase = 0x0; // no 3 phase, only 2 conductors per lane
|
||||
@@ -698,54 +705,51 @@ static void camera_open(CameraState *s) {
|
||||
csiphy_info->settle_time = MIPI_SETTLE_CNT * 200000000ULL;
|
||||
csiphy_info->data_rate = 48000000; // Calculated by camera_freqs.py
|
||||
|
||||
int ret_ = device_config(s->csiphy_fd, s->session_handle, s->csiphy_dev_handle, cam_packet_handle);
|
||||
int ret_ = device_config(csiphy_fd, session_handle, csiphy_dev_handle, cam_packet_handle);
|
||||
assert(ret_ == 0);
|
||||
|
||||
munmap(csiphy_info, buf_desc[0].size);
|
||||
release_fd(s->multi_cam_state->video0_fd, buf_desc[0].mem_handle);
|
||||
release_fd(multi_cam_state->video0_fd, buf_desc[0].mem_handle);
|
||||
munmap(pkt, size);
|
||||
release_fd(s->multi_cam_state->video0_fd, cam_packet_handle);
|
||||
release_fd(multi_cam_state->video0_fd, cam_packet_handle);
|
||||
}
|
||||
|
||||
// link devices
|
||||
LOG("-- Link devices");
|
||||
struct cam_req_mgr_link_info req_mgr_link_info = {0};
|
||||
req_mgr_link_info.session_hdl = s->session_handle;
|
||||
req_mgr_link_info.session_hdl = session_handle;
|
||||
req_mgr_link_info.num_devices = 2;
|
||||
req_mgr_link_info.dev_hdls[0] = s->isp_dev_handle;
|
||||
req_mgr_link_info.dev_hdls[1] = s->sensor_dev_handle;
|
||||
ret = cam_control(s->multi_cam_state->video0_fd, CAM_REQ_MGR_LINK, &req_mgr_link_info, sizeof(req_mgr_link_info));
|
||||
s->link_handle = req_mgr_link_info.link_hdl;
|
||||
LOGD("link: %d hdl: 0x%X", ret, s->link_handle);
|
||||
req_mgr_link_info.dev_hdls[0] = isp_dev_handle;
|
||||
req_mgr_link_info.dev_hdls[1] = sensor_dev_handle;
|
||||
ret = do_cam_control(multi_cam_state->video0_fd, CAM_REQ_MGR_LINK, &req_mgr_link_info, sizeof(req_mgr_link_info));
|
||||
link_handle = req_mgr_link_info.link_hdl;
|
||||
LOGD("link: %d hdl: 0x%X", ret, link_handle);
|
||||
|
||||
struct cam_req_mgr_link_control req_mgr_link_control = {0};
|
||||
req_mgr_link_control.ops = CAM_REQ_MGR_LINK_ACTIVATE;
|
||||
req_mgr_link_control.session_hdl = s->session_handle;
|
||||
req_mgr_link_control.session_hdl = session_handle;
|
||||
req_mgr_link_control.num_links = 1;
|
||||
req_mgr_link_control.link_hdls[0] = s->link_handle;
|
||||
ret = cam_control(s->multi_cam_state->video0_fd, CAM_REQ_MGR_LINK_CONTROL, &req_mgr_link_control, sizeof(req_mgr_link_control));
|
||||
req_mgr_link_control.link_hdls[0] = link_handle;
|
||||
ret = do_cam_control(multi_cam_state->video0_fd, CAM_REQ_MGR_LINK_CONTROL, &req_mgr_link_control, sizeof(req_mgr_link_control));
|
||||
LOGD("link control: %d", ret);
|
||||
|
||||
ret = device_control(s->csiphy_fd, CAM_START_DEV, s->session_handle, s->csiphy_dev_handle);
|
||||
ret = device_control(csiphy_fd, CAM_START_DEV, session_handle, csiphy_dev_handle);
|
||||
LOGD("start csiphy: %d", ret);
|
||||
ret = device_control(s->multi_cam_state->isp_fd, CAM_START_DEV, s->session_handle, s->isp_dev_handle);
|
||||
ret = device_control(multi_cam_state->isp_fd, CAM_START_DEV, session_handle, isp_dev_handle);
|
||||
LOGD("start isp: %d", ret);
|
||||
ret = device_control(s->sensor_fd, CAM_START_DEV, s->session_handle, s->sensor_dev_handle);
|
||||
ret = device_control(sensor_fd, CAM_START_DEV, session_handle, sensor_dev_handle);
|
||||
LOGD("start sensor: %d", ret);
|
||||
|
||||
enqueue_req_multi(s, 1, FRAME_BUF_COUNT, 0);
|
||||
enqueue_req_multi(1, FRAME_BUF_COUNT, 0);
|
||||
}
|
||||
|
||||
void cameras_init(VisionIpcServer *v, MultiCameraState *s, cl_device_id device_id, cl_context ctx) {
|
||||
camera_init(s, v, &s->driver_cam, CAMERA_ID_AR0231, 2, 20, device_id, ctx,
|
||||
VISION_STREAM_RGB_FRONT, VISION_STREAM_DRIVER);
|
||||
s->driver_cam.camera_init(s, v, CAMERA_ID_AR0231, 2, 20, device_id, ctx, VISION_STREAM_RGB_FRONT, VISION_STREAM_DRIVER);
|
||||
printf("driver camera initted \n");
|
||||
if (!env_only_driver) {
|
||||
camera_init(s, v, &s->road_cam, CAMERA_ID_AR0231, 1, 20, device_id, ctx,
|
||||
VISION_STREAM_RGB_BACK, VISION_STREAM_ROAD); // swap left/right
|
||||
s->road_cam.camera_init(s, v, CAMERA_ID_AR0231, 1, 20, device_id, ctx, VISION_STREAM_RGB_BACK, VISION_STREAM_ROAD); // swap left/right
|
||||
printf("road camera initted \n");
|
||||
camera_init(s, v, &s->wide_road_cam, CAMERA_ID_AR0231, 0, 20, device_id, ctx,
|
||||
VISION_STREAM_RGB_WIDE, VISION_STREAM_WIDE_ROAD);
|
||||
s->wide_road_cam.camera_init(s, v, CAMERA_ID_AR0231, 0, 20, device_id, ctx, VISION_STREAM_RGB_WIDE, VISION_STREAM_WIDE_ROAD);
|
||||
printf("wide road camera initted \n");
|
||||
}
|
||||
|
||||
@@ -768,7 +772,7 @@ void cameras_open(MultiCameraState *s) {
|
||||
LOGD("opened video1");
|
||||
|
||||
// looks like there's only one of these
|
||||
s->isp_fd = HANDLE_EINTR(open("/dev/v4l-subdev1", O_RDWR | O_NONBLOCK));
|
||||
s->isp_fd = open_v4l_by_name_and_index("cam-isp");
|
||||
assert(s->isp_fd >= 0);
|
||||
LOGD("opened isp");
|
||||
|
||||
@@ -779,7 +783,7 @@ void cameras_open(MultiCameraState *s) {
|
||||
query_cap_cmd.handle_type = 1;
|
||||
query_cap_cmd.caps_handle = (uint64_t)&isp_query_cap_cmd;
|
||||
query_cap_cmd.size = sizeof(isp_query_cap_cmd);
|
||||
ret = cam_control(s->isp_fd, CAM_QUERY_CAP, &query_cap_cmd, sizeof(query_cap_cmd));
|
||||
ret = do_cam_control(s->isp_fd, CAM_QUERY_CAP, &query_cap_cmd, sizeof(query_cap_cmd));
|
||||
assert(ret == 0);
|
||||
LOGD("using MMU handle: %x", isp_query_cap_cmd.device_iommu.non_secure);
|
||||
LOGD("using MMU handle: %x", isp_query_cap_cmd.cdm_iommu.non_secure);
|
||||
@@ -794,74 +798,72 @@ void cameras_open(MultiCameraState *s) {
|
||||
ret = HANDLE_EINTR(ioctl(s->video0_fd, VIDIOC_SUBSCRIBE_EVENT, &sub));
|
||||
printf("req mgr subscribe: %d\n", ret);
|
||||
|
||||
camera_open(&s->driver_cam);
|
||||
s->driver_cam.camera_open();
|
||||
printf("driver camera opened \n");
|
||||
if (!env_only_driver) {
|
||||
camera_open(&s->road_cam);
|
||||
s->road_cam.camera_open();
|
||||
printf("road camera opened \n");
|
||||
camera_open(&s->wide_road_cam);
|
||||
s->wide_road_cam.camera_open();
|
||||
printf("wide road camera opened \n");
|
||||
}
|
||||
}
|
||||
|
||||
static void camera_close(CameraState *s) {
|
||||
void CameraState::camera_close() {
|
||||
int ret;
|
||||
|
||||
// stop devices
|
||||
LOG("-- Stop devices");
|
||||
// ret = device_control(s->sensor_fd, CAM_STOP_DEV, s->session_handle, s->sensor_dev_handle);
|
||||
// ret = device_control(sensor_fd, CAM_STOP_DEV, session_handle, sensor_dev_handle);
|
||||
// LOGD("stop sensor: %d", ret);
|
||||
ret = device_control(s->multi_cam_state->isp_fd, CAM_STOP_DEV, s->session_handle, s->isp_dev_handle);
|
||||
ret = device_control(multi_cam_state->isp_fd, CAM_STOP_DEV, session_handle, isp_dev_handle);
|
||||
LOGD("stop isp: %d", ret);
|
||||
ret = device_control(s->csiphy_fd, CAM_STOP_DEV, s->session_handle, s->csiphy_dev_handle);
|
||||
ret = device_control(csiphy_fd, CAM_STOP_DEV, session_handle, csiphy_dev_handle);
|
||||
LOGD("stop csiphy: %d", ret);
|
||||
// link control stop
|
||||
LOG("-- Stop link control");
|
||||
static struct cam_req_mgr_link_control req_mgr_link_control = {0};
|
||||
req_mgr_link_control.ops = CAM_REQ_MGR_LINK_DEACTIVATE;
|
||||
req_mgr_link_control.session_hdl = s->session_handle;
|
||||
req_mgr_link_control.session_hdl = session_handle;
|
||||
req_mgr_link_control.num_links = 1;
|
||||
req_mgr_link_control.link_hdls[0] = s->link_handle;
|
||||
ret = cam_control(s->multi_cam_state->video0_fd, CAM_REQ_MGR_LINK_CONTROL, &req_mgr_link_control, sizeof(req_mgr_link_control));
|
||||
req_mgr_link_control.link_hdls[0] = link_handle;
|
||||
ret = do_cam_control(multi_cam_state->video0_fd, CAM_REQ_MGR_LINK_CONTROL, &req_mgr_link_control, sizeof(req_mgr_link_control));
|
||||
LOGD("link control stop: %d", ret);
|
||||
|
||||
// unlink
|
||||
LOG("-- Unlink");
|
||||
static struct cam_req_mgr_unlink_info req_mgr_unlink_info = {0};
|
||||
req_mgr_unlink_info.session_hdl = s->session_handle;
|
||||
req_mgr_unlink_info.link_hdl = s->link_handle;
|
||||
ret = cam_control(s->multi_cam_state->video0_fd, CAM_REQ_MGR_UNLINK, &req_mgr_unlink_info, sizeof(req_mgr_unlink_info));
|
||||
req_mgr_unlink_info.session_hdl = session_handle;
|
||||
req_mgr_unlink_info.link_hdl = link_handle;
|
||||
ret = do_cam_control(multi_cam_state->video0_fd, CAM_REQ_MGR_UNLINK, &req_mgr_unlink_info, sizeof(req_mgr_unlink_info));
|
||||
LOGD("unlink: %d", ret);
|
||||
|
||||
// release devices
|
||||
LOGD("-- Release devices");
|
||||
ret = device_control(s->sensor_fd, CAM_RELEASE_DEV, s->session_handle, s->sensor_dev_handle);
|
||||
ret = device_control(sensor_fd, CAM_RELEASE_DEV, session_handle, sensor_dev_handle);
|
||||
LOGD("release sensor: %d", ret);
|
||||
ret = device_control(s->multi_cam_state->isp_fd, CAM_RELEASE_DEV, s->session_handle, s->isp_dev_handle);
|
||||
ret = device_control(multi_cam_state->isp_fd, CAM_RELEASE_DEV, session_handle, isp_dev_handle);
|
||||
LOGD("release isp: %d", ret);
|
||||
ret = device_control(s->csiphy_fd, CAM_RELEASE_DEV, s->session_handle, s->csiphy_dev_handle);
|
||||
ret = device_control(csiphy_fd, CAM_RELEASE_DEV, session_handle, csiphy_dev_handle);
|
||||
LOGD("release csiphy: %d", ret);
|
||||
|
||||
// destroyed session
|
||||
struct cam_req_mgr_session_info session_info = {.session_hdl = s->session_handle};
|
||||
ret = cam_control(s->multi_cam_state->video0_fd, CAM_REQ_MGR_DESTROY_SESSION, &session_info, sizeof(session_info));
|
||||
struct cam_req_mgr_session_info session_info = {.session_hdl = session_handle};
|
||||
ret = do_cam_control(multi_cam_state->video0_fd, CAM_REQ_MGR_DESTROY_SESSION, &session_info, sizeof(session_info));
|
||||
LOGD("destroyed session: %d", ret);
|
||||
}
|
||||
|
||||
void cameras_close(MultiCameraState *s) {
|
||||
camera_close(&s->driver_cam);
|
||||
s->driver_cam.camera_close();
|
||||
if (!env_only_driver) {
|
||||
camera_close(&s->road_cam);
|
||||
camera_close(&s->wide_road_cam);
|
||||
s->road_cam.camera_close();
|
||||
s->wide_road_cam.camera_close();
|
||||
}
|
||||
|
||||
delete s->sm;
|
||||
delete s->pm;
|
||||
}
|
||||
|
||||
// ******************* just a helper *******************
|
||||
|
||||
void handle_camera_event(CameraState *s, void *evdat) {
|
||||
void CameraState::handle_camera_event(void *evdat) {
|
||||
struct cam_req_mgr_message *event_data = (struct cam_req_mgr_message *)evdat;
|
||||
|
||||
uint64_t timestamp = event_data->u.frame_msg.timestamp;
|
||||
@@ -869,53 +871,53 @@ void handle_camera_event(CameraState *s, void *evdat) {
|
||||
int real_id = event_data->u.frame_msg.request_id;
|
||||
|
||||
if (real_id != 0) { // next ready
|
||||
if (real_id == 1) {s->idx_offset = main_id;}
|
||||
if (real_id == 1) {idx_offset = main_id;}
|
||||
int buf_idx = (real_id - 1) % FRAME_BUF_COUNT;
|
||||
|
||||
// check for skipped frames
|
||||
if (main_id > s->frame_id_last + 1 && !s->skipped) {
|
||||
if (main_id > frame_id_last + 1 && !skipped) {
|
||||
// realign
|
||||
clear_req_queue(s->multi_cam_state->video0_fd, event_data->session_hdl, event_data->u.frame_msg.link_hdl);
|
||||
enqueue_req_multi(s, real_id + 1, FRAME_BUF_COUNT - 1, 0);
|
||||
s->skipped = true;
|
||||
} else if (main_id == s->frame_id_last + 1) {
|
||||
s->skipped = false;
|
||||
clear_req_queue(multi_cam_state->video0_fd, event_data->session_hdl, event_data->u.frame_msg.link_hdl);
|
||||
enqueue_req_multi(real_id + 1, FRAME_BUF_COUNT - 1, 0);
|
||||
skipped = true;
|
||||
} else if (main_id == frame_id_last + 1) {
|
||||
skipped = false;
|
||||
}
|
||||
|
||||
// check for dropped requests
|
||||
if (real_id > s->request_id_last + 1) {
|
||||
enqueue_req_multi(s, s->request_id_last + 1 + FRAME_BUF_COUNT, real_id - (s->request_id_last + 1), 0);
|
||||
if (real_id > request_id_last + 1) {
|
||||
enqueue_req_multi(request_id_last + 1 + FRAME_BUF_COUNT, real_id - (request_id_last + 1), 0);
|
||||
}
|
||||
|
||||
// metas
|
||||
s->frame_id_last = main_id;
|
||||
s->request_id_last = real_id;
|
||||
frame_id_last = main_id;
|
||||
request_id_last = real_id;
|
||||
|
||||
auto &meta_data = s->buf.camera_bufs_metadata[buf_idx];
|
||||
meta_data.frame_id = main_id - s->idx_offset;
|
||||
auto &meta_data = buf.camera_bufs_metadata[buf_idx];
|
||||
meta_data.frame_id = main_id - idx_offset;
|
||||
meta_data.timestamp_sof = timestamp;
|
||||
s->exp_lock.lock();
|
||||
meta_data.gain = s->dc_gain_enabled ? s->analog_gain_frac * DC_GAIN : s->analog_gain_frac;
|
||||
meta_data.high_conversion_gain = s->dc_gain_enabled;
|
||||
meta_data.integ_lines = s->exposure_time;
|
||||
meta_data.measured_grey_fraction = s->measured_grey_fraction;
|
||||
meta_data.target_grey_fraction = s->target_grey_fraction;
|
||||
s->exp_lock.unlock();
|
||||
exp_lock.lock();
|
||||
meta_data.gain = dc_gain_enabled ? analog_gain_frac * DC_GAIN : analog_gain_frac;
|
||||
meta_data.high_conversion_gain = dc_gain_enabled;
|
||||
meta_data.integ_lines = exposure_time;
|
||||
meta_data.measured_grey_fraction = measured_grey_fraction;
|
||||
meta_data.target_grey_fraction = target_grey_fraction;
|
||||
exp_lock.unlock();
|
||||
|
||||
// dispatch
|
||||
enqueue_req_multi(s, real_id + FRAME_BUF_COUNT, 1, 1);
|
||||
enqueue_req_multi(real_id + FRAME_BUF_COUNT, 1, 1);
|
||||
} else { // not ready
|
||||
// reset after half second of no response
|
||||
if (main_id > s->frame_id_last + 10) {
|
||||
clear_req_queue(s->multi_cam_state->video0_fd, event_data->session_hdl, event_data->u.frame_msg.link_hdl);
|
||||
enqueue_req_multi(s, s->request_id_last + 1, FRAME_BUF_COUNT, 0);
|
||||
s->frame_id_last = main_id;
|
||||
s->skipped = true;
|
||||
if (main_id > frame_id_last + 10) {
|
||||
clear_req_queue(multi_cam_state->video0_fd, event_data->session_hdl, event_data->u.frame_msg.link_hdl);
|
||||
enqueue_req_multi(request_id_last + 1, FRAME_BUF_COUNT, 0);
|
||||
frame_id_last = main_id;
|
||||
skipped = true;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
static void set_camera_exposure(CameraState *s, float grey_frac) {
|
||||
void CameraState::set_camera_exposure(float grey_frac) {
|
||||
const float dt = 0.05;
|
||||
|
||||
const float ts_grey = 10.0;
|
||||
@@ -929,15 +931,15 @@ static void set_camera_exposure(CameraState *s, float grey_frac) {
|
||||
// Therefore we use the target EV from 3 frames ago, the grey fraction that was just measured was the result of that control action.
|
||||
// TODO: Lower latency to 2 frames, by using the histogram outputed by the sensor we can do AE before the debayering is complete
|
||||
|
||||
const float cur_ev = s->cur_ev[s->buf.cur_frame_data.frame_id % 3];
|
||||
const float cur_ev_ = cur_ev[buf.cur_frame_data.frame_id % 3];
|
||||
|
||||
// Scale target grey between 0.1 and 0.4 depending on lighting conditions
|
||||
float new_target_grey = std::clamp(0.4 - 0.3 * log2(1.0 + cur_ev) / log2(6000.0), 0.1, 0.4);
|
||||
float target_grey = (1.0 - k_grey) * s->target_grey_fraction + k_grey * new_target_grey;
|
||||
float new_target_grey = std::clamp(0.4 - 0.3 * log2(1.0 + cur_ev_) / log2(6000.0), 0.1, 0.4);
|
||||
float target_grey = (1.0 - k_grey) * target_grey_fraction + k_grey * new_target_grey;
|
||||
|
||||
float desired_ev = std::clamp(cur_ev * target_grey / grey_frac, s->min_ev, s->max_ev);
|
||||
float desired_ev = std::clamp(cur_ev_ * target_grey / grey_frac, min_ev, max_ev);
|
||||
float k = (1.0 - k_ev) / 3.0;
|
||||
desired_ev = (k * s->cur_ev[0]) + (k * s->cur_ev[1]) + (k * s->cur_ev[2]) + (k_ev * desired_ev);
|
||||
desired_ev = (k * cur_ev[0]) + (k * cur_ev[1]) + (k * cur_ev[2]) + (k_ev * desired_ev);
|
||||
|
||||
float best_ev_score = 1e6;
|
||||
int new_g = 0;
|
||||
@@ -945,7 +947,7 @@ static void set_camera_exposure(CameraState *s, float grey_frac) {
|
||||
|
||||
// Hysteresis around high conversion gain
|
||||
// We usually want this on since it results in lower noise, but turn off in very bright day scenes
|
||||
bool enable_dc_gain = s->dc_gain_enabled;
|
||||
bool enable_dc_gain = dc_gain_enabled;
|
||||
if (!enable_dc_gain && target_grey < 0.2) {
|
||||
enable_dc_gain = true;
|
||||
} else if (enable_dc_gain && target_grey > 0.3) {
|
||||
@@ -954,14 +956,14 @@ static void set_camera_exposure(CameraState *s, float grey_frac) {
|
||||
|
||||
// Simple brute force optimizer to choose sensor parameters
|
||||
// to reach desired EV
|
||||
for (int g = std::max((int)ANALOG_GAIN_MIN_IDX, s->gain_idx - 1); g <= std::min((int)ANALOG_GAIN_MAX_IDX, s->gain_idx + 1); g++) {
|
||||
for (int g = std::max((int)ANALOG_GAIN_MIN_IDX, gain_idx - 1); g <= std::min((int)ANALOG_GAIN_MAX_IDX, gain_idx + 1); g++) {
|
||||
float gain = sensor_analog_gains[g] * (enable_dc_gain ? DC_GAIN : 1);
|
||||
|
||||
// Compute optimal time for given gain
|
||||
int t = std::clamp(int(std::round(desired_ev / gain)), EXPOSURE_TIME_MIN, EXPOSURE_TIME_MAX);
|
||||
|
||||
// Only go below recomended gain when absolutely necessary to not overexpose
|
||||
if (g < ANALOG_GAIN_REC_IDX && t > 20 && g < s->gain_idx) {
|
||||
if (g < ANALOG_GAIN_REC_IDX && t > 20 && g < gain_idx) {
|
||||
continue;
|
||||
}
|
||||
|
||||
@@ -972,10 +974,10 @@ static void set_camera_exposure(CameraState *s, float grey_frac) {
|
||||
float m = g > ANALOG_GAIN_REC_IDX ? 5.0 : 0.1;
|
||||
score += std::abs(g - (int)ANALOG_GAIN_REC_IDX) * m;
|
||||
|
||||
// LOGE("cam: %d - gain: %d, t: %d (%.2f), score %.2f, score + gain %.2f, %.3f, %.3f", s->camera_num, g, t, desired_ev / gain, score, score + std::abs(g - s->gain_idx) * (score + 1.0) / 10.0, desired_ev, s->min_ev);
|
||||
// LOGE("cam: %d - gain: %d, t: %d (%.2f), score %.2f, score + gain %.2f, %.3f, %.3f", camera_num, g, t, desired_ev / gain, score, score + std::abs(g - gain_idx) * (score + 1.0) / 10.0, desired_ev, min_ev);
|
||||
|
||||
// Small penalty on changing gain
|
||||
score += std::abs(g - s->gain_idx) * (score + 1.0) / 10.0;
|
||||
score += std::abs(g - gain_idx) * (score + 1.0) / 10.0;
|
||||
|
||||
if (score < best_ev_score) {
|
||||
new_t = t;
|
||||
@@ -984,42 +986,41 @@ static void set_camera_exposure(CameraState *s, float grey_frac) {
|
||||
}
|
||||
}
|
||||
|
||||
s->exp_lock.lock();
|
||||
exp_lock.lock();
|
||||
|
||||
s->measured_grey_fraction = grey_frac;
|
||||
s->target_grey_fraction = target_grey;
|
||||
measured_grey_fraction = grey_frac;
|
||||
target_grey_fraction = target_grey;
|
||||
|
||||
s->analog_gain_frac = sensor_analog_gains[new_g];
|
||||
s->gain_idx = new_g;
|
||||
s->exposure_time = new_t;
|
||||
s->dc_gain_enabled = enable_dc_gain;
|
||||
analog_gain_frac = sensor_analog_gains[new_g];
|
||||
gain_idx = new_g;
|
||||
exposure_time = new_t;
|
||||
dc_gain_enabled = enable_dc_gain;
|
||||
|
||||
float gain = s->analog_gain_frac * (s->dc_gain_enabled ? DC_GAIN : 1.0);
|
||||
s->cur_ev[s->buf.cur_frame_data.frame_id % 3] = s->exposure_time * gain;
|
||||
float gain = analog_gain_frac * (dc_gain_enabled ? DC_GAIN : 1.0);
|
||||
cur_ev[buf.cur_frame_data.frame_id % 3] = exposure_time * gain;
|
||||
|
||||
s->exp_lock.unlock();
|
||||
exp_lock.unlock();
|
||||
|
||||
// Processing a frame takes right about 50ms, so we need to wait a few ms
|
||||
// so we don't send i2c commands around the frame start.
|
||||
int ms = (nanos_since_boot() - s->buf.cur_frame_data.timestamp_sof) / 1000000;
|
||||
int ms = (nanos_since_boot() - buf.cur_frame_data.timestamp_sof) / 1000000;
|
||||
if (ms < 60) {
|
||||
util::sleep_for(60 - ms);
|
||||
}
|
||||
// LOGE("ae - camera %d, cur_t %.5f, sof %.5f, dt %.5f", s->camera_num, 1e-9 * nanos_since_boot(), 1e-9 * s->buf.cur_frame_data.timestamp_sof, 1e-9 * (nanos_since_boot() - s->buf.cur_frame_data.timestamp_sof));
|
||||
// LOGE("ae - camera %d, cur_t %.5f, sof %.5f, dt %.5f", camera_num, 1e-9 * nanos_since_boot(), 1e-9 * buf.cur_frame_data.timestamp_sof, 1e-9 * (nanos_since_boot() - buf.cur_frame_data.timestamp_sof));
|
||||
|
||||
uint16_t analog_gain_reg = 0xFF00 | (new_g << 4) | new_g;
|
||||
struct i2c_random_wr_payload exp_reg_array[] = {
|
||||
{0x3366, analog_gain_reg},
|
||||
{0x3362, (uint16_t)(s->dc_gain_enabled ? 0x1 : 0x0)},
|
||||
{0x3012, (uint16_t)s->exposure_time},
|
||||
{0x3362, (uint16_t)(dc_gain_enabled ? 0x1 : 0x0)},
|
||||
{0x3012, (uint16_t)exposure_time},
|
||||
};
|
||||
sensors_i2c(s, exp_reg_array, sizeof(exp_reg_array)/sizeof(struct i2c_random_wr_payload),
|
||||
sensors_i2c(exp_reg_array, sizeof(exp_reg_array)/sizeof(struct i2c_random_wr_payload),
|
||||
CAM_SENSOR_PACKET_OPCODE_SENSOR_CONFIG);
|
||||
|
||||
}
|
||||
|
||||
void camera_autoexposure(CameraState *s, float grey_frac) {
|
||||
set_camera_exposure(s, grey_frac);
|
||||
s->set_camera_exposure(grey_frac);
|
||||
}
|
||||
|
||||
// called by processing_thread
|
||||
@@ -1053,11 +1054,10 @@ void cameras_run(MultiCameraState *s) {
|
||||
|
||||
// start devices
|
||||
LOG("-- Starting devices");
|
||||
int start_reg_len = sizeof(start_reg_array) / sizeof(struct i2c_random_wr_payload);
|
||||
sensors_i2c(&s->driver_cam, start_reg_array, start_reg_len, CAM_SENSOR_PACKET_OPCODE_SENSOR_CONFIG);
|
||||
s->driver_cam.sensors_start();
|
||||
if (!env_only_driver) {
|
||||
sensors_i2c(&s->road_cam, start_reg_array, start_reg_len, CAM_SENSOR_PACKET_OPCODE_SENSOR_CONFIG);
|
||||
sensors_i2c(&s->wide_road_cam, start_reg_array, start_reg_len, CAM_SENSOR_PACKET_OPCODE_SENSOR_CONFIG);
|
||||
s->road_cam.sensors_start();
|
||||
s->wide_road_cam.sensors_start();
|
||||
}
|
||||
|
||||
// poll events
|
||||
@@ -1088,11 +1088,11 @@ void cameras_run(MultiCameraState *s) {
|
||||
}
|
||||
|
||||
if (event_data->session_hdl == s->road_cam.session_handle) {
|
||||
handle_camera_event(&s->road_cam, event_data);
|
||||
s->road_cam.handle_camera_event(event_data);
|
||||
} else if (event_data->session_hdl == s->wide_road_cam.session_handle) {
|
||||
handle_camera_event(&s->wide_road_cam, event_data);
|
||||
s->wide_road_cam.handle_camera_event(event_data);
|
||||
} else if (event_data->session_hdl == s->driver_cam.session_handle) {
|
||||
handle_camera_event(&s->driver_cam, event_data);
|
||||
s->driver_cam.handle_camera_event(event_data);
|
||||
} else {
|
||||
printf("Unknown vidioc event source\n");
|
||||
assert(false);
|
||||
|
||||
@@ -9,7 +9,8 @@
|
||||
|
||||
#define FRAME_BUF_COUNT 4
|
||||
|
||||
typedef struct CameraState {
|
||||
class CameraState {
|
||||
public:
|
||||
MultiCameraState *multi_cam_state;
|
||||
CameraInfo ci;
|
||||
|
||||
@@ -31,6 +32,21 @@ typedef struct CameraState {
|
||||
|
||||
int camera_num;
|
||||
|
||||
void config_isp(int io_mem_handle, int fence, int request_id, int buf0_mem_handle, int buf0_offset);
|
||||
void enqueue_req_multi(int start, int n, bool dp);
|
||||
void enqueue_buffer(int i, bool dp);
|
||||
void handle_camera_event(void *evdat);
|
||||
void set_camera_exposure(float grey_frac);
|
||||
|
||||
void sensors_start();
|
||||
void sensors_poke(int request_id);
|
||||
void sensors_i2c(struct i2c_random_wr_payload* dat, int len, int op_code);
|
||||
void sensors_init();
|
||||
|
||||
void camera_open();
|
||||
void camera_init(MultiCameraState *multi_cam_state, VisionIpcServer * v, int camera_id, int camera_num, unsigned int fps, cl_device_id device_id, cl_context ctx, VisionStreamType rgb_type, VisionStreamType yuv_type);
|
||||
void camera_close();
|
||||
|
||||
int32_t session_handle;
|
||||
int32_t sensor_dev_handle;
|
||||
int32_t isp_dev_handle;
|
||||
@@ -48,7 +64,7 @@ typedef struct CameraState {
|
||||
bool skipped;
|
||||
|
||||
CameraBuf buf;
|
||||
} CameraState;
|
||||
};
|
||||
|
||||
typedef struct MultiCameraState {
|
||||
unique_fd video0_fd;
|
||||
|
||||
Reference in New Issue
Block a user