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

camera_qcom2 refactor (#23946)

Browse Source 
* 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: 0adfef0b34
taco
George Hotz 4 years ago committed by GitHub
parent 3bd71500bd
commit c0145ae709
2 changed files with 221 additions and 205 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. 406
      selfdrive/camerad/cameras/camera_qcom2.cc
  2. 20
      selfdrive/camerad/cameras/camera_qcom2.h

406
selfdrive/camerad/cameras/camera_qcom2.cc
Unescape View File

@ -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);

20
selfdrive/camerad/cameras/camera_qcom2.h
Unescape View File

@ -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;

Write Preview
Loading…
Cancel
Save