spacecruft
/
nopenpilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

Tici camerad (#2048) 

* fix camera config

* typos

* oops

* more typo

* lambless

* forget to send

* visualize hist

* more typo

* 0xC000

* simple

* data loss prevention, clean up later

* loggerd

* back up code

* backup

* fixed memory leak

* fix vsync

* upload ecam

* WB

* 3stream

* fix OMX crash on loggerd rotation

* rewritten debayer kernel

* update viewer

* improved AE

* no artifact lines/grids

* standard trigger

* cleanups

* CCM

* cleanups

* slight tweak

* upd push sock

* build all these

* update tele fl

* update cereal

* upd viewer

* DualCameraState -> MultiCameraState

* cameras_open

* disable frame zmq push by default

* more cleanup

* no apks

* fix submodule error

* wat

* clean up trash

* remove junk

* only build on qcom2

* no need to check these

* update cereal

* some more minor cleanup

* bump panda

* add todo

* minor typo

* Revert "minor typo"

This reverts commit 9233a1df7c.

* not care

* use consistent hdr

* some cleanup

* Update selfdrive/camerad/main.cc

Co-authored-by: Adeeb Shihadeh <adeebshihadeh@gmail.com>

* more cleanups

* remove irrelevant stuff

* this too

* cleanup

* rerun ci

Co-authored-by: ZwX1616 <zwx1616@gmail.com>
Co-authored-by: Tici <robbe@comma.ai>
Co-authored-by: Adeeb Shihadeh <adeebshihadeh@gmail.com>
pull/2092/head
robbederks 2020-08-27 04:59:56 +02:00 committed by GitHub
parent 3014280d1a
commit 37d6472bfa
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
21 changed files with 1243 additions and 466 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
selfdrive/camerad/SConscript
View File

@ -11,6 +11,10 @@ if arch == "aarch64":
elif arch == "larch64":
libs += []
cameras = ['cameras/camera_qcom2.c']
# no screen
# env = env.Clone()
# env.Append(CXXFLAGS = '-DNOSCREEN')
# env.Append(CFLAGS = '-DNOSCREEN')
else:
if webcam:
libs += ['opencv_core', 'opencv_highgui', 'opencv_imgproc', 'opencv_videoio']

10
selfdrive/camerad/cameras/camera_frame_stream.cc
View File

@ -45,7 +45,7 @@ void camera_init(CameraState *s, int camera_id, unsigned int fps) {
tbuffer_init2(&s->camera_tb, FRAME_BUF_COUNT, "frame", camera_release_buffer, s);
}
void run_frame_stream(DualCameraState *s) {
void run_frame_stream(MultiCameraState *s) {
SubMaster sm({"frame"});
CameraState *const rear_camera = &s->rear;
@ -93,7 +93,7 @@ CameraInfo cameras_supported[CAMERA_ID_MAX] = {
},
};
void cameras_init(DualCameraState *s) {
void cameras_init(MultiCameraState *s) {
camera_init(&s->rear, CAMERA_ID_IMX298, 20);
s->rear.transform = (mat3){{
1.0, 0.0, 0.0,
@ -111,7 +111,7 @@ void cameras_init(DualCameraState *s) {
void camera_autoexposure(CameraState *s, float grey_frac) {}
void cameras_open(DualCameraState *s, VisionBuf *camera_bufs_rear,
void cameras_open(MultiCameraState *s, VisionBuf *camera_bufs_rear,
VisionBuf *camera_bufs_focus, VisionBuf *camera_bufs_stats,
VisionBuf *camera_bufs_front) {
assert(camera_bufs_rear);
@ -124,11 +124,11 @@ void cameras_open(DualCameraState *s, VisionBuf *camera_bufs_rear,
camera_open(&s->rear, camera_bufs_rear, true);
}
void cameras_close(DualCameraState *s) {
void cameras_close(MultiCameraState *s) {
camera_close(&s->rear);
}
void cameras_run(DualCameraState *s) {
void cameras_run(MultiCameraState *s) {
set_thread_name("frame_streaming");
run_frame_stream(s);
cameras_close(s);

12
selfdrive/camerad/cameras/camera_frame_stream.h
View File

@ -40,17 +40,17 @@ typedef struct CameraState {
} CameraState;
typedef struct DualCameraState {
typedef struct MultiCameraState {
int ispif_fd;
CameraState rear;
CameraState front;
} DualCameraState;
} MultiCameraState;
void cameras_init(DualCameraState *s);
void cameras_open(DualCameraState *s, VisionBuf *camera_bufs_rear, VisionBuf *camera_bufs_focus, VisionBuf *camera_bufs_stats, VisionBuf *camera_bufs_front);
void cameras_run(DualCameraState *s);
void cameras_close(DualCameraState *s);
void cameras_init(MultiCameraState *s);
void cameras_open(MultiCameraState *s, VisionBuf *camera_bufs_rear, VisionBuf *camera_bufs_focus, VisionBuf *camera_bufs_stats, VisionBuf *camera_bufs_front);
void cameras_run(MultiCameraState *s);
void cameras_close(MultiCameraState *s);
void camera_autoexposure(CameraState *s, float grey_frac);
#ifdef __cplusplus
} // extern "C"

12
selfdrive/camerad/cameras/camera_qcom.cc
View File

@ -258,7 +258,7 @@ static int imx179_s5k3p8sp_apply_exposure(CameraState *s, int gain, int integ_li
return err;
}
void cameras_init(DualCameraState *s) {
void cameras_init(MultiCameraState *s) {
char project_name[1024] = {0};
property_get("ro.boot.project_name", project_name, "");
@ -545,7 +545,7 @@ static void imx298_ois_calibration(int ois_fd, uint8_t* eeprom) {
static void sensors_init(DualCameraState *s) {
static void sensors_init(MultiCameraState *s) {
int err;
int sensorinit_fd = -1;
@ -1829,7 +1829,7 @@ static void front_start(CameraState *s) {
void cameras_open(DualCameraState *s, VisionBuf *camera_bufs_rear, VisionBuf *camera_bufs_focus, VisionBuf *camera_bufs_stats, VisionBuf *camera_bufs_front) {
void cameras_open(MultiCameraState *s, VisionBuf *camera_bufs_rear, VisionBuf *camera_bufs_focus, VisionBuf *camera_bufs_stats, VisionBuf *camera_bufs_front) {
int err;
struct ispif_cfg_data ispif_cfg_data;
@ -2018,7 +2018,7 @@ static void ops_term() {
static void* ops_thread(void* arg) {
int err;
DualCameraState *s = (DualCameraState*)arg;
MultiCameraState *s = (MultiCameraState*)arg;
set_thread_name("camera_settings");
@ -2109,7 +2109,7 @@ static void* ops_thread(void* arg) {
return NULL;
}
void cameras_run(DualCameraState *s) {
void cameras_run(MultiCameraState *s) {
int err;
pthread_t ops_thread_handle;
@ -2221,7 +2221,7 @@ void cameras_run(DualCameraState *s) {
cameras_close(s);
}
void cameras_close(DualCameraState *s) {
void cameras_close(MultiCameraState *s) {
camera_close(&s->rear);
camera_close(&s->front);
}

12
selfdrive/camerad/cameras/camera_qcom.h
View File

@ -123,19 +123,19 @@ typedef struct CameraState {
} CameraState;
typedef struct DualCameraState {
typedef struct MultiCameraState {
int device;
int ispif_fd;
CameraState rear;
CameraState front;
} DualCameraState;
} MultiCameraState;
void cameras_init(DualCameraState *s);
void cameras_open(DualCameraState *s, VisionBuf *camera_bufs_rear, VisionBuf *camera_bufs_focus, VisionBuf *camera_bufs_stats, VisionBuf *camera_bufs_front);
void cameras_run(DualCameraState *s);
void cameras_close(DualCameraState *s);
void cameras_init(MultiCameraState *s);
void cameras_open(MultiCameraState *s, VisionBuf *camera_bufs_rear, VisionBuf *camera_bufs_focus, VisionBuf *camera_bufs_stats, VisionBuf *camera_bufs_front);
void cameras_run(MultiCameraState *s);
void cameras_close(MultiCameraState *s);
void camera_autoexposure(CameraState *s, float grey_frac);
void actuator_move(CameraState *s, uint16_t target);

299
selfdrive/camerad/cameras/camera_qcom2.c
View File

@ -1,4 +1,4 @@
#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include <errno.h>
@ -47,7 +47,7 @@ CameraInfo cameras_supported[CAMERA_ID_MAX] = {
.frame_height = FRAME_HEIGHT,
.frame_stride = FRAME_STRIDE,
.bayer = true,
.bayer_flip = 0,
.bayer_flip = 1,
.hdr = false
},
};
@ -67,6 +67,7 @@ int cam_control(int fd, int op_code, void *handle, int size) {
int ret = ioctl(fd, VIDIOC_CAM_CONTROL, &camcontrol);
if (ret == -1) {
printf("OP CODE ERR - %d \n", op_code);
perror("wat");
}
return ret;
@ -81,7 +82,7 @@ int device_control(int fd, int op_code, int session_handle, int dev_handle) {
}
void *alloc_w_mmu_hdl(int video0_fd, int len, int align, int flags, uint32_t *handle, int mmu_hdl, int mmu_hdl2) {
static struct cam_mem_mgr_alloc_cmd mem_mgr_alloc_cmd = {0};
struct cam_mem_mgr_alloc_cmd mem_mgr_alloc_cmd = {0};
mem_mgr_alloc_cmd.len = len;
mem_mgr_alloc_cmd.align = align;
mem_mgr_alloc_cmd.flags = flags;
@ -115,14 +116,13 @@ void *alloc(int video0_fd, int len, int align, int flags, uint32_t *handle) {
void release(int video0_fd, uint32_t handle) {
int ret;
static struct cam_mem_mgr_release_cmd mem_mgr_release_cmd = {0};
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));
assert(ret == 0);
}
void release_fd(int video0_fd, uint32_t handle) {
// handle to fd
close(handle>>16);
@ -143,7 +143,7 @@ void sensors_poke(struct CameraState *s, int request_id) {
pkt->header.request_id = request_id;
//struct cam_cmd_buf_desc *buf_desc = (struct cam_cmd_buf_desc *)&pkt->payload;
static struct cam_config_dev_cmd config_dev_cmd = {};
struct cam_config_dev_cmd config_dev_cmd = {};
config_dev_cmd.session_handle = s->session_handle;
config_dev_cmd.dev_handle = s->sensor_dev_handle;
config_dev_cmd.offset = 0;
@ -152,6 +152,7 @@ void sensors_poke(struct CameraState *s, int request_id) {
int ret = cam_control(s->sensor_fd, CAM_CONFIG_DEV, &config_dev_cmd, sizeof(config_dev_cmd));
assert(ret == 0);
munmap(pkt, size);
release_fd(s->video0_fd, cam_packet_handle);
}
@ -178,7 +179,7 @@ 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));
static struct cam_config_dev_cmd config_dev_cmd = {};
struct cam_config_dev_cmd config_dev_cmd = {};
config_dev_cmd.session_handle = s->session_handle;
config_dev_cmd.dev_handle = s->sensor_dev_handle;
config_dev_cmd.offset = 0;
@ -187,7 +188,9 @@ void sensors_i2c(struct CameraState *s, struct i2c_random_wr_payload* dat, int l
int ret = cam_control(s->sensor_fd, CAM_CONFIG_DEV, &config_dev_cmd, sizeof(config_dev_cmd));
assert(ret == 0);
munmap(power, buf_desc[0].size);
release_fd(s->video0_fd, buf_desc[0].mem_handle);
munmap(pkt, size);
release_fd(s->video0_fd, cam_packet_handle);
}
@ -353,8 +356,11 @@ void sensors_init(int video0_fd, int sensor_fd, int camera_num) {
int ret = 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);
release_fd(video0_fd, buf_desc[0].mem_handle);
munmap(power, buf_desc[1].size);
release_fd(video0_fd, buf_desc[1].mem_handle);
munmap(pkt, size);
release_fd(video0_fd, cam_packet_handle);
}
@ -443,17 +449,17 @@ void config_isp(struct CameraState *s, int io_mem_handle, int fence, int request
.h_init = 0x0,
.v_init = 0x0,
};
io_cfg[0].format = 0x3;
io_cfg[0].format = CAM_FORMAT_MIPI_RAW_10;
io_cfg[0].color_pattern = 0x5;
io_cfg[0].bpp = 0xc;
io_cfg[0].resource_type = CAM_ISP_IFE_OUT_RES_RDI_0;
io_cfg[0].fence = fence;
io_cfg[0].direction = 0x2;
io_cfg[0].direction = CAM_BUF_OUTPUT;
io_cfg[0].subsample_pattern = 0x1;
io_cfg[0].framedrop_pattern = 0x1;
}
static struct cam_config_dev_cmd config_dev_cmd = {};
struct cam_config_dev_cmd config_dev_cmd = {};
config_dev_cmd.session_handle = s->session_handle;
config_dev_cmd.dev_handle = s->isp_dev_handle;
config_dev_cmd.offset = 0;
@ -464,33 +470,37 @@ void config_isp(struct CameraState *s, int io_mem_handle, int fence, int request
printf("ISP CONFIG FAILED\n");
}
munmap(buf2, buf_desc[1].size);
release_fd(s->video0_fd, buf_desc[1].mem_handle);
//release(s->video0_fd, buf_desc[0].mem_handle);
// release_fd(s->video0_fd, buf_desc[0].mem_handle);
munmap(pkt, size);
release_fd(s->video0_fd, cam_packet_handle);
}
void enqueue_buffer(struct CameraState *s, int i) {
int ret;
int request_id = (++s->sched_request_id);
bool first = true;
int request_id = s->frame_id;
if (s->buf_handle[i]) {
first = false;
release(s->video0_fd, s->buf_handle[i]);
// wait
//struct cam_sync_wait sync_wait = {0};
//sync_wait.sync_obj = s->sync_objs[i];
//sync_wait.timeout_ms = 175;
//ret = cam_control(s->video1_fd, CAM_SYNC_WAIT, &sync_wait, sizeof(sync_wait));
//LOGD("fence wait: %d %d", ret, sync_wait.sync_obj);
// destroy old output fence
static struct cam_sync_info sync_destroy = {0};
struct cam_sync_info sync_destroy = {0};
strcpy(sync_destroy.name, "NodeOutputPortFence");
sync_destroy.sync_obj = s->sync_objs[i];
ret = cam_control(s->video1_fd, CAM_SYNC_DESTROY, &sync_destroy, sizeof(sync_destroy));
LOGD("fence destroy: %d %d", ret, sync_destroy.sync_obj);
}
// new request_ids
s->request_ids[i] = request_id;
// do stuff
static struct cam_req_mgr_sched_request req_mgr_sched_request = {0};
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.req_id = request_id;
@ -498,14 +508,14 @@ void enqueue_buffer(struct CameraState *s, int i) {
LOGD("sched req: %d %d", ret, request_id);
// create output fence
static struct cam_sync_info sync_create = {0};
struct cam_sync_info sync_create = {0};
strcpy(sync_create.name, "NodeOutputPortFence");
ret = cam_control(s->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;
// configure ISP to put the image in place
static struct cam_mem_mgr_map_cmd mem_mgr_map_cmd = {0};
struct cam_mem_mgr_map_cmd mem_mgr_map_cmd = {0};
mem_mgr_map_cmd.mmu_hdls[0] = s->device_iommu;
mem_mgr_map_cmd.num_hdl = 1;
mem_mgr_map_cmd.flags = 1;
@ -513,11 +523,11 @@ void enqueue_buffer(struct CameraState *s, int i) {
ret = cam_control(s->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;
// poke sensor
sensors_poke(s, 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));
}
@ -533,7 +543,6 @@ static void camera_release_buffer(void* cookie, int i) {
static void camera_init(CameraState *s, int camera_id, int camera_num, unsigned int fps) {
LOGD("camera init %d", camera_num);
// TODO: this is copied code from camera_webcam
assert(camera_id < ARRAYSIZE(cameras_supported));
s->ci = cameras_supported[camera_id];
assert(s->ci.frame_width != 0);
@ -548,7 +557,16 @@ static void camera_init(CameraState *s, int camera_id, int camera_num, unsigned
0.0, 1.0, 0.0,
0.0, 0.0, 1.0,
}};
s->digital_gain = 1.0;
// s->digital_gain = 1.0;
// s->digital_gain_pre = 4; // for WB
s->dc_opstate = 0;
s->dc_gain_enabled = false;
s->analog_gain_frac = 1.0;
s->analog_gain = 0x8;
s->exposure_time = 598;
s->frame_id = -1;
s->first = true;
//s->missed_frame = 0;
}
static void camera_open(CameraState *s, VisionBuf* b) {
@ -593,7 +611,6 @@ static void camera_open(CameraState *s, VisionBuf* b) {
ret = cam_control(s->video0_fd, CAM_REQ_MGR_CREATE_SESSION, &s->req_mgr_session_info, sizeof(s->req_mgr_session_info));
LOGD("get session: %d 0x%X", ret, s->req_mgr_session_info.session_hdl);
s->session_handle = s->req_mgr_session_info.session_hdl;
// access the sensor
LOGD("-- Accessing sensor");
static struct cam_acquire_dev_cmd acquire_dev_cmd = {0};
@ -685,6 +702,7 @@ static void camera_open(CameraState *s, VisionBuf* b) {
acquire_dev_cmd.resource_hdl = (uint64_t)&csiphy_acquire_dev_info;
ret = cam_control(s->csiphy_fd, CAM_ACQUIRE_DEV, &acquire_dev_cmd, sizeof(acquire_dev_cmd));
LOGD("acquire csiphy dev: %d", ret);
s->csiphy_dev_handle = acquire_dev_cmd.dev_handle;
@ -697,10 +715,10 @@ static void camera_open(CameraState *s, VisionBuf* b) {
LOG("-- Configuring sensor");
sensors_i2c(s, init_array_ar0231, sizeof(init_array_ar0231)/sizeof(struct i2c_random_wr_payload),
CAM_SENSOR_PACKET_OPCODE_SENSOR_CONFIG);
sensors_i2c(s, start_reg_array, sizeof(start_reg_array)/sizeof(struct i2c_random_wr_payload),
CAM_SENSOR_PACKET_OPCODE_SENSOR_STREAMON);
sensors_i2c(s, stop_reg_array, sizeof(stop_reg_array)/sizeof(struct i2c_random_wr_payload),
CAM_SENSOR_PACKET_OPCODE_SENSOR_STREAMOFF);
//sensors_i2c(s, start_reg_array, sizeof(start_reg_array)/sizeof(struct i2c_random_wr_payload),
//CAM_SENSOR_PACKET_OPCODE_SENSOR_STREAMON);
//sensors_i2c(s, stop_reg_array, sizeof(stop_reg_array)/sizeof(struct i2c_random_wr_payload),
//CAM_SENSOR_PACKET_OPCODE_SENSOR_STREAMOFF);
// config csiphy
LOG("-- Config CSI PHY");
@ -757,38 +775,40 @@ static void camera_open(CameraState *s, VisionBuf* b) {
ret = cam_control(s->video0_fd, CAM_REQ_MGR_LINK_CONTROL, &req_mgr_link_control, sizeof(req_mgr_link_control));
LOGD("link control: %d", ret);
// start devices
LOG("-- Start devices");
ret = device_control(s->isp_fd, CAM_START_DEV, s->session_handle, s->isp_dev_handle);
LOGD("start isp: %d", ret);
ret = device_control(s->csiphy_fd, CAM_START_DEV, s->session_handle, s->csiphy_dev_handle);
LOGD("start csiphy: %d", ret);
ret = device_control(s->sensor_fd, CAM_START_DEV, s->session_handle, s->sensor_dev_handle);
ret = device_control(s->csiphy_fd, CAM_START_DEV, s->session_handle, s->csiphy_dev_handle);
LOGD("start isp: %d", ret);
ret = device_control(s->isp_fd, CAM_START_DEV, s->session_handle, s->isp_dev_handle);
LOGD("start sensor: %d", ret);
ret = device_control(s->sensor_fd, CAM_START_DEV, s->session_handle, s->sensor_dev_handle);
for (int i = 0; i < FRAME_BUF_COUNT; i++) {
LOG("-- Initting buffer %d", i);
enqueue_buffer(s, i);
}
}
void cameras_init(DualCameraState *s) {
camera_init(&s->rear, CAMERA_ID_AR0231, 0, 20);
camera_init(&s->wide, CAMERA_ID_AR0231, 1, 20);
void cameras_init(MultiCameraState *s) {
camera_init(&s->rear, CAMERA_ID_AR0231, 1, 20); // swap left/right
printf("rear initted \n");
camera_init(&s->wide, CAMERA_ID_AR0231, 0, 20);
printf("wide initted \n");
camera_init(&s->front, CAMERA_ID_AR0231, 2, 20);
printf("front initted \n");
#ifdef NOSCREEN
zsock_t *rgb_sock = zsock_new_push("tcp://192.168.3.4:7768");
assert(rgb_sock);
s->rgb_sock = rgb_sock;
#endif
}
void cameras_open(DualCameraState *s, VisionBuf *camera_bufs_rear, VisionBuf *camera_bufs_focus, VisionBuf *camera_bufs_stats, VisionBuf *camera_bufs_front) {
void cameras_open(MultiCameraState *s, VisionBuf *camera_bufs_rear, VisionBuf *camera_bufs_focus, VisionBuf *camera_bufs_stats, VisionBuf *camera_bufs_front, VisionBuf *camera_bufs_wide) {
int ret;
LOG("-- Opening devices");
// video0 is the target of many ioctls
// video0 is req_mgr, the target of many ioctls
s->video0_fd = open("/dev/video0", O_RDWR | O_NONBLOCK);
assert(s->video0_fd >= 0);
LOGD("opened video0");
s->rear.video0_fd = s->front.video0_fd = s->wide.video0_fd = s->video0_fd;
// video1 is the target of some ioctls
// video1 is cam_sync, the target of some ioctls
s->video1_fd = open("/dev/video1", O_RDWR | O_NONBLOCK);
assert(s->video1_fd >= 0);
LOGD("opened video1");
@ -820,16 +840,17 @@ void cameras_open(DualCameraState *s, VisionBuf *camera_bufs_rear, VisionBuf *ca
LOG("-- Subscribing");
static struct v4l2_event_subscription sub = {0};
sub.type = 0x8000000;
sub.id = 0;
sub.id = 2; // should use boot time for sof
ret = ioctl(s->video0_fd, VIDIOC_SUBSCRIBE_EVENT, &sub);
LOGD("isp subscribe: %d", ret);
sub.id = 1;
ret = ioctl(s->video0_fd, VIDIOC_SUBSCRIBE_EVENT, &sub);
LOGD("isp subscribe: %d", ret);
printf("req mgr subscribe: %d\n", ret);
camera_open(&s->rear, camera_bufs_rear);
//camera_open(&s->front, camera_bufs_front);
// TODO: add bufs for camera wide
printf("rear opened \n");
camera_open(&s->wide, camera_bufs_wide);
printf("wide opened \n");
camera_open(&s->front, camera_bufs_front);
printf("front opened \n");
// TODO: refactor this api for compat
}
static void camera_close(CameraState *s) {
@ -837,13 +858,12 @@ static void camera_close(CameraState *s) {
// stop devices
LOG("-- Stop devices");
ret = device_control(s->sensor_fd, CAM_STOP_DEV, s->session_handle, s->sensor_dev_handle);
LOGD("stop sensor: %d", ret);
// ret = device_control(s->sensor_fd, CAM_STOP_DEV, s->session_handle, s->sensor_dev_handle);
// LOGD("stop sensor: %d", ret);
ret = device_control(s->isp_fd, CAM_STOP_DEV, s->session_handle, s->isp_dev_handle);
LOGD("stop isp: %d", ret);
ret = device_control(s->csiphy_fd, CAM_STOP_DEV, s->session_handle, s->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};
@ -873,14 +893,16 @@ static void camera_close(CameraState *s) {
ret = cam_control(s->video0_fd, CAM_REQ_MGR_DESTROY_SESSION, &s->req_mgr_session_info, sizeof(s->req_mgr_session_info));
LOGD("destroyed session: %d", ret);
tbuffer_stop(&s->camera_tb);
}
static void cameras_close(DualCameraState *s) {
static void cameras_close(MultiCameraState *s) {
camera_close(&s->rear);
//camera_close(&s->front);
//camera_close(&s->wide);
camera_close(&s->wide);
camera_close(&s->front);
#ifdef NOSCREEN
zsock_destroy(&s->rgb_sock);
#endif
}
struct video_event_data {
@ -888,23 +910,30 @@ struct video_event_data {
int32_t link_hdl;
int32_t frame_id;
int32_t reserved;
int64_t req_id;
uint64_t tv_sec;
uint64_t tv_usec;
};
void cameras_run(DualCameraState *s) {
LOG("-- Dequeueing Video events");
int frame_id = 1;
void cameras_run(MultiCameraState *s) {
// start devices
LOG("-- Start devices");
sensors_i2c(&s->rear, start_reg_array, sizeof(start_reg_array)/sizeof(struct i2c_random_wr_payload),
CAM_SENSOR_PACKET_OPCODE_SENSOR_CONFIG);
sensors_i2c(&s->wide, start_reg_array, sizeof(start_reg_array)/sizeof(struct i2c_random_wr_payload),
CAM_SENSOR_PACKET_OPCODE_SENSOR_CONFIG);
sensors_i2c(&s->front, start_reg_array, sizeof(start_reg_array)/sizeof(struct i2c_random_wr_payload),
CAM_SENSOR_PACKET_OPCODE_SENSOR_CONFIG);
// poll events
LOG("-- Dequeueing Video events");
while (!do_exit) {
struct pollfd fds[2] = {{0}};
struct pollfd fds[1] = {{0}};
fds[0].fd = s->video0_fd;
fds[0].events = POLLPRI;
fds[1].fd = s->video1_fd;
fds[1].events = POLLPRI;
int ret = poll(fds, ARRAYSIZE(fds), 1000);
if (ret <= 0) {
if (errno == EINTR || errno == EAGAIN) continue;
@ -912,26 +941,48 @@ void cameras_run(DualCameraState *s) {
break;
}
for (int i=0; i<2; i++) {
if (!fds[i].revents) continue;
static struct v4l2_event ev = {0};
ret = ioctl(fds[i].fd, VIDIOC_DQEVENT, &ev);
if (ev.type == 0x8000000) {
struct video_event_data *event_data = (struct video_event_data *)ev.u.data;
uint64_t timestamp = (event_data->tv_sec*1000000000ULL
+ event_data->tv_usec*1000);
LOGD("video%d dqevent: %d type:0x%x frame_id:%d timestamp: %llu", i, ret, ev.type, event_data->frame_id, timestamp);
if (!fds[0].revents) continue;
if (event_data->frame_id != 0) {
for (int j = 0; j < FRAME_BUF_COUNT; j++) {
if (s->rear.request_ids[j] == event_data->frame_id) {
// TODO: support more than rear camera
tbuffer_dispatch(&s->rear.camera_tb, j);
s->rear.camera_bufs_metadata[j].frame_id = frame_id++;
break;
}
}
}
struct v4l2_event ev = {0};
ret = ioctl(fds[0].fd, VIDIOC_DQEVENT, &ev);
if (ev.type == 0x8000000) {
struct cam_req_mgr_message *event_data = (struct cam_req_mgr_message *)ev.u.data;
uint64_t timestamp = event_data->u.frame_msg.timestamp;
LOGD("v4l2 event: sess_hdl %d, link_hdl %d, frame_id %d, req_id %lld, timestamp 0x%llx, sof_status %d\n", event_data->session_hdl, event_data->u.frame_msg.link_hdl, event_data->u.frame_msg.frame_id, event_data->u.frame_msg.request_id, event_data->u.frame_msg.timestamp, event_data->u.frame_msg.sof_status);
// printf("sess_hdl %d, link_hdl %d, frame_id %d, req_id %lld, timestamp 0x%llx, sof_status %d\n", event_data->session_hdl, event_data->u.frame_msg.link_hdl, event_data->u.frame_msg.frame_id, event_data->u.frame_msg.request_id, event_data->u.frame_msg.timestamp, event_data->u.frame_msg.sof_status);
if (event_data->u.frame_msg.request_id != 0 || (event_data->u.frame_msg.request_id == 0 &&
((s->rear.first && event_data->session_hdl == s->rear.req_mgr_session_info.session_hdl) ||
(s->wide.first && event_data->session_hdl == s->wide.req_mgr_session_info.session_hdl) ||
(s->front.first && event_data->session_hdl == s->front.req_mgr_session_info.session_hdl)))) {
if (event_data->session_hdl == s->rear.req_mgr_session_info.session_hdl) {
s->rear.frame_id++;
//printf("rear %d\n", s->rear.frame_id);
if (event_data->u.frame_msg.request_id > 0) {s->rear.first = false;}
int buf_idx = s->rear.frame_id % FRAME_BUF_COUNT;
s->rear.camera_bufs_metadata[buf_idx].frame_id = s->rear.frame_id;
s->rear.camera_bufs_metadata[buf_idx].timestamp_eof = timestamp; // only has sof?
tbuffer_dispatch(&s->rear.camera_tb, buf_idx);
} else if (event_data->session_hdl == s->wide.req_mgr_session_info.session_hdl) {
s->wide.frame_id++;
//printf("wide %d\n", s->wide.frame_id);
if (event_data->u.frame_msg.request_id > 0) {s->wide.first = false;}
int buf_idx = s->wide.frame_id % FRAME_BUF_COUNT;
s->wide.camera_bufs_metadata[buf_idx].frame_id = s->wide.frame_id;
s->wide.camera_bufs_metadata[buf_idx].timestamp_eof = timestamp;
tbuffer_dispatch(&s->wide.camera_tb, buf_idx);
} else if (event_data->session_hdl == s->front.req_mgr_session_info.session_hdl) {
s->front.frame_id++;
//printf("front %d\n", s->front.frame_id);
if (event_data->u.frame_msg.request_id > 0) {s->front.first = false;}
int buf_idx = s->front.frame_id % FRAME_BUF_COUNT;
s->front.camera_bufs_metadata[buf_idx].frame_id = s->front.frame_id;
s->front.camera_bufs_metadata[buf_idx].timestamp_eof = timestamp;
tbuffer_dispatch(&s->front.camera_tb, buf_idx);
} else {
printf("Unknown vidioc event source\n");
assert(false);
}
}
}
}
@ -941,5 +992,79 @@ void cameras_run(DualCameraState *s) {
}
void camera_autoexposure(CameraState *s, float grey_frac) {
// TODO: get stats from sensor
const float target_grey = 0.3;
const float analog_gain_frac_min = 0.25;
float analog_gain_frac_max = s->dc_gain_enabled?8.0:2.0;
// const float digital_gain_min = 1.0;
// const float digital_gain_max = 3.99; // is the correct?
const int exposure_time_min = 64;
const int exposure_time_max = 1066; //1416; // no slower than 1/25 sec. calculated from 0x300C and clock freq
float exposure_factor = pow(1.05, (target_grey - grey_frac) / 0.16 );
if (s->analog_gain_frac > 1 && exposure_factor > 1 && !s->dc_gain_enabled && s->dc_opstate != 1) { // iso 800
s->dc_gain_enabled = true;
s->analog_gain_frac *= 0.5;
s->dc_opstate = 1;
} else if (s->analog_gain_frac < 0.5 && exposure_factor < 1 && s->dc_gain_enabled && s->dc_opstate != 1) { // iso 400
s->dc_gain_enabled = false;
s->analog_gain_frac *= 2;
s->dc_opstate = 1;
} else if (s->analog_gain_frac > 0.5 && exposure_factor < 0.9) {
s->analog_gain_frac *= sqrt(exposure_factor);
s->exposure_time = max(min(s->exposure_time * sqrt(exposure_factor), exposure_time_max),exposure_time_min);
s->dc_opstate = 0;
} else if ((s->exposure_time < exposure_time_max || exposure_factor < 1) && (s->exposure_time > exposure_time_min || exposure_factor > 1)) {
s->exposure_time = max(min(s->exposure_time * exposure_factor, exposure_time_max),exposure_time_min);
s->dc_opstate = 0;
} else {
s->analog_gain_frac = max(min(s->analog_gain_frac * exposure_factor, analog_gain_frac_max),analog_gain_frac_min);
s->dc_opstate = 0;
}
// set up config
// gain mapping: [1/8, 2/8, 2/7, 3/7, 3/6, 4/6, 4/5, 5/5, 5/4, 6/4, 6/3, 7/3, 7/2, 8/2, 8/1, N/A] -> 0 to 15
uint16_t AG;
if (s->analog_gain_frac > 4) {
s->analog_gain_frac = 8.0;
AG = 0xEEEE;
printf("cam %d gain_frac is %f, set AG to 0x%X, S to %d, dc %d \n", s->camera_num, s->analog_gain_frac, AG, s->exposure_time, s->dc_gain_enabled);
} else {
AG = -(1.147 * s->analog_gain_frac * s->analog_gain_frac) + (7.67 * s->analog_gain_frac) - 0.1;
if (AG - s->analog_gain == -1) {AG = s->analog_gain;}
s->analog_gain = AG;
AG = AG * 4096 + AG * 256 + AG * 16 + AG;
printf("cam %d gain_frac is %f, set AG to 0x%X, S to %d, dc %d \n", s->camera_num, s->analog_gain_frac, AG, s->exposure_time, s->dc_gain_enabled);
}
struct i2c_random_wr_payload exp_reg_array[] = {{0x3366, AG}, // analog gain
{0x3362, s->dc_gain_enabled?0x1:0x0}, // DC_GAIN
{0x305A, 0x00C4}, // red gain
{0x3058, 0x00B1}, // blue gain
{0x3056, 0x009A}, // g1 gain
{0x305C, 0x009A}, // g2 gain
{0x3012, s->exposure_time}, // integ time
{0x301A, 0x091C}}; // reset
sensors_i2c(s, exp_reg_array, sizeof(exp_reg_array)/sizeof(struct i2c_random_wr_payload),
CAM_SENSOR_PACKET_OPCODE_SENSOR_CONFIG);
}
#ifdef NOSCREEN
void sendrgb(MultiCameraState *s, uint8_t* dat, int len, uint8_t cam_id) {
int err, err2;
int scale = 6;
int old_width = FRAME_WIDTH;
int new_width = FRAME_WIDTH / scale;
int new_height = FRAME_HEIGHT / scale;
uint8_t resized_dat[new_width*new_height*3];
memset(&resized_dat, cam_id, 3);
for (uint32_t r=1;r<new_height;r++) {
for (uint32_t c=1;c<new_width;c++) {
resized_dat[(r*new_width+c)*3] = dat[(r*old_width + c)*3*scale];
resized_dat[(r*new_width+c)*3+1] = dat[(r*old_width + c)*3*scale+1];
resized_dat[(r*new_width+c)*3+2] = dat[(r*old_width + c)*3*scale+2];
}
}
err = zmq_send(zsock_resolve(s->rgb_sock), &resized_dat, new_width*new_height*3, 0);
err2 = zmq_errno();
//printf("zmq errcode %d, %d\n",err,err2);
}
#endif

32
selfdrive/camerad/cameras/camera_qcom2.h
View File

@ -4,6 +4,7 @@
#include <stdint.h>
#include <stdbool.h>
#include <pthread.h>
#include <czmq.h>
#include "common/mat.h"
#include "common/visionbuf.h"
@ -27,7 +28,14 @@ typedef struct CameraState {
TBuffer camera_tb;
int frame_size;
float digital_gain;
//float digital_gain;
//int digital_gain_pre;
float analog_gain_frac;
uint16_t analog_gain;
uint8_t dc_opstate;
bool dc_gain_enabled;
int exposure_time;
mat3 transform;
int device_iommu;
@ -54,14 +62,16 @@ typedef struct CameraState {
int buf0_handle;
int buf_handle[FRAME_BUF_COUNT];
int sched_request_id;
int request_ids[FRAME_BUF_COUNT];
int sync_objs[FRAME_BUF_COUNT];
int frame_id;
bool first;
struct cam_req_mgr_session_info req_mgr_session_info;
} CameraState;
typedef struct DualCameraState {
typedef struct MultiCameraState {
int device;
int video0_fd;
@ -71,12 +81,20 @@ typedef struct DualCameraState {
CameraState rear;
CameraState front;
CameraState wide;
} DualCameraState;
#ifdef NOSCREEN
zsock_t *rgb_sock;
#endif
void cameras_init(DualCameraState *s);
void cameras_open(DualCameraState *s, VisionBuf *camera_bufs_rear, VisionBuf *camera_bufs_focus, VisionBuf *camera_bufs_stats, VisionBuf *camera_bufs_front);
void cameras_run(DualCameraState *s);
pthread_mutex_t isp_lock;
} MultiCameraState;
void cameras_init(MultiCameraState *s);
void cameras_open(MultiCameraState *s, VisionBuf *camera_bufs_rear, VisionBuf *camera_bufs_focus, VisionBuf *camera_bufs_stats, VisionBuf *camera_bufs_front, VisionBuf *camera_bufs_wide);
void cameras_run(MultiCameraState *s);
void camera_autoexposure(CameraState *s, float grey_frac);
#ifdef NOSCREEN
void sendrgb(MultiCameraState *s, uint8_t* dat, int len, uint8_t cam_id);
#endif
#ifdef __cplusplus
} // extern "C"

8
selfdrive/camerad/cameras/camera_webcam.cc
View File

@ -221,7 +221,7 @@ CameraInfo cameras_supported[CAMERA_ID_MAX] = {
},
};
void cameras_init(DualCameraState *s) {
void cameras_init(MultiCameraState *s) {
camera_init(&s->rear, CAMERA_ID_LGC920, 20);
s->rear.transform = (mat3){{
@ -240,7 +240,7 @@ void cameras_init(DualCameraState *s) {
void camera_autoexposure(CameraState *s, float grey_frac) {}
void cameras_open(DualCameraState *s, VisionBuf *camera_bufs_rear,
void cameras_open(MultiCameraState *s, VisionBuf *camera_bufs_rear,
VisionBuf *camera_bufs_focus, VisionBuf *camera_bufs_stats,
VisionBuf *camera_bufs_front) {
assert(camera_bufs_rear);
@ -254,12 +254,12 @@ void cameras_open(DualCameraState *s, VisionBuf *camera_bufs_rear,
camera_open(&s->rear, camera_bufs_rear, true);
}
void cameras_close(DualCameraState *s) {
void cameras_close(MultiCameraState *s) {
camera_close(&s->rear);
camera_close(&s->front);
}
void cameras_run(DualCameraState *s) {
void cameras_run(MultiCameraState *s) {
set_thread_name("webcam_thread");
int err;

12
selfdrive/camerad/cameras/camera_webcam.h
View File

@ -39,17 +39,17 @@ typedef struct CameraState {
} CameraState;
typedef struct DualCameraState {
typedef struct MultiCameraState {
int ispif_fd;
CameraState rear;
CameraState front;
} DualCameraState;
} MultiCameraState;
void cameras_init(DualCameraState *s);
void cameras_open(DualCameraState *s, VisionBuf *camera_bufs_rear, VisionBuf *camera_bufs_focus, VisionBuf *camera_bufs_stats, VisionBuf *camera_bufs_front);
void cameras_run(DualCameraState *s);
void cameras_close(DualCameraState *s);
void cameras_init(MultiCameraState *s);
void cameras_open(MultiCameraState *s, VisionBuf *camera_bufs_rear, VisionBuf *camera_bufs_focus, VisionBuf *camera_bufs_stats, VisionBuf *camera_bufs_front);
void cameras_run(MultiCameraState *s);
void cameras_close(MultiCameraState *s);
void camera_autoexposure(CameraState *s, float grey_frac);
#ifdef __cplusplus
} // extern "C"

14
selfdrive/camerad/cameras/debayer.cl
View File

@ -81,28 +81,18 @@ __kernel void debayer10(__global uchar const * const in,
float4 p = convert_float4(pint);
#if NEW == 1
// now it's 0x2a
const float black_level = 42.0f;
// max on black level
p = max(0.0, p - black_level);
#else
// 64 is the black level of the sensor, remove
// (changed to 56 for HDR)
const float black_level = 56.0f;
// TODO: switch to max here?
p = (p - black_level);
#endif
#if NEW == 0
// correct vignetting (no pow function?)
// see https://www.eecis.udel.edu/~jye/lab_research/09/JiUp.pdf the A (4th order)
const float r = ((oy - RGB_HEIGHT/2)*(oy - RGB_HEIGHT/2) + (ox - RGB_WIDTH/2)*(ox - RGB_WIDTH/2));
const float fake_f = 700.0f; // should be 910, but this fits...
const float lil_a = (1.0f + r/(fake_f*fake_f));
p = p * lil_a * lil_a;
#endif
// rescale to 1.0
#if HDR
@ -125,12 +115,8 @@ __kernel void debayer10(__global uchar const * const in,
float3 c1 = (float3)(p.s0, (p.s1+p.s2)/2.0f, p.s3);
#endif
#if NEW
// TODO: new color correction
#else
// color correction
c1 = color_correct(c1);
#endif
#if HDR
// srgb gamma isn't right for YUV, so it's disabled for now

128
selfdrive/camerad/cameras/real_debayer.cl 100644
View File

@ -0,0 +1,128 @@
const __constant float3 color_correction[3] = {
// post wb CCM
(float3)(1.17898387, -0.19583185, -0.19881648),
(float3)(-0.03367879, 1.33692858, -0.02475203),
(float3)(-0.14530508, -0.14109673, 1.22356851),
};
float3 color_correct(float r, float g, float b) {
float3 ret = (0,0,0);
ret += r * color_correction[0];
ret += g * color_correction[1];
ret += b * color_correction[2];
ret = max(0.0, min(1.0, ret));
return ret;
}
uint int_from_10(const uchar * source, uint start, uint offset) {
// source: source
// start: starting address of 0
// offset: 0 - 3
uint major = (uint)source[start + offset] << 2;
uint minor = (source[start + 4] >> (2 * offset)) & 3;
return major + minor;
}
float to_normal(uint x) {
float pv = (float)(x);
const float black_level = 0;
pv = max(0.0, pv - black_level);
pv /= (1024.0f - black_level);
pv = 1.6*pv / (1.0f + 1.6*pv); // reinhard
return pv;
}
__kernel void debayer10(const __global uchar * in,
__global uchar * out,
__local float * cached
)
{
const int x_global = get_global_id(0);
const int y_global = get_global_id(1);
// const int globalOffset = ;
const int localRowLen = 2 + get_local_size(0); // 2 padding
const int x_local = get_local_id(0);
const int y_local = get_local_id(1);
const int localOffset = (y_local + 1) * localRowLen + x_local + 1;
// cache local pixels first
// saves memory access and avoids repeated normalization
uint globalStart_10 = y_global * FRAME_STRIDE + (5 * (x_global / 4));
uint offset_10 = x_global % 4;
uint raw_val = int_from_10(in, globalStart_10, offset_10);
cached[localOffset] = to_normal(raw_val);
// edges
if (x_global < 1 || x_global > RGB_WIDTH - 2 || y_global < 1 || y_global > RGB_WIDTH - 2) {
barrier(CLK_LOCAL_MEM_FENCE);
return;
} else {
int localColOffset = -1;
int globalColOffset = -1;
// cache padding
if (x_local < 1) {
localColOffset = x_local;
globalColOffset = -1;
cached[(y_local + 1) * localRowLen + x_local] = to_normal(int_from_10(in, y_global * FRAME_STRIDE + (5 * ((x_global-1) / 4)), (offset_10 + 3) % 4));
} else if (x_local >= get_local_size(0) - 1) {
localColOffset = x_local + 2;
globalColOffset = 1;
cached[localOffset + 1] = to_normal(int_from_10(in, y_global * FRAME_STRIDE + (5 * ((x_global+1) / 4)), (offset_10 + 1) % 4));
}
if (y_local < 1) {
cached[y_local * localRowLen + x_local + 1] = to_normal(int_from_10(in, globalStart_10 - FRAME_STRIDE, offset_10));
if (localColOffset != -1) {
cached[y_local * localRowLen + localColOffset] = to_normal(int_from_10(in, (y_global-1) * FRAME_STRIDE + (5 * ((x_global+globalColOffset) / 4)), (offset_10+4+globalColOffset) % 4));
}
} else if (y_local >= get_local_size(1) - 1) {
cached[(y_local + 2) * localRowLen + x_local + 1] = to_normal(int_from_10(in, globalStart_10 + FRAME_STRIDE, offset_10));
if (localColOffset != -1) {
cached[(y_local + 2) * localRowLen + localColOffset] = to_normal(int_from_10(in, (y_global+1) * FRAME_STRIDE + (5 * ((x_global+globalColOffset) / 4)), (offset_10+4+globalColOffset) % 4));
}
}
// sync
barrier(CLK_LOCAL_MEM_FENCE);
// perform debayer
float r;
float g;
float b;
if (x_global % 2 == 0) {
if (y_global % 2 == 0) { // G1
r = (cached[localOffset - 1] + cached[localOffset + 1]) / 2.0f;
g = (cached[localOffset] + cached[localOffset + localRowLen + 1]) / 2.0f;
b = (cached[localOffset - localRowLen] + cached[localOffset + localRowLen]) / 2.0f;
} else { // B
r = (cached[localOffset - localRowLen - 1] + cached[localOffset - localRowLen + 1] + cached[localOffset + localRowLen - 1] + cached[localOffset + localRowLen + 1]) / 4.0f;
g = (cached[localOffset - localRowLen] + cached[localOffset + localRowLen] + cached[localOffset - 1] + cached[localOffset + 1]) / 4.0f;
b = cached[localOffset];
}
} else {
if (y_global % 2 == 0) { // R
r = cached[localOffset];
g = (cached[localOffset - localRowLen] + cached[localOffset + localRowLen] + cached[localOffset - 1] + cached[localOffset + 1]) / 4.0f;
b = (cached[localOffset - localRowLen - 1] + cached[localOffset - localRowLen + 1] + cached[localOffset + localRowLen - 1] + cached[localOffset + localRowLen + 1]) / 4.0f;
} else { // G2
r = (cached[localOffset - localRowLen] + cached[localOffset + localRowLen]) / 2.0f;
g = (cached[localOffset] + cached[localOffset - localRowLen - 1]) / 2.0f;
b = (cached[localOffset - 1] + cached[localOffset + 1]) / 2.0f;
}
}
float3 rgb = color_correct(r, g, b);
// rgb = srgb_gamma(rgb);
// BGR output
out[3 * x_global + 3 * y_global * RGB_WIDTH + 0] = (uchar)(255.0f * rgb.z);
out[3 * x_global + 3 * y_global * RGB_WIDTH + 1] = (uchar)(255.0f * rgb.y);
out[3 * x_global + 3 * y_global * RGB_WIDTH + 2] = (uchar)(255.0f * rgb.x);
}
}

649
selfdrive/camerad/cameras/sensor2_i2c.h
View File

@ -1,304 +1,373 @@
struct i2c_random_wr_payload start_reg_array[] = {{0x301a, 0x1c}};
struct i2c_random_wr_payload start_reg_array[] = {{0x301a, 0x91c}};
//struct i2c_random_wr_payload stop_reg_array[] = {{0x301a, 0x10d8}};
struct i2c_random_wr_payload stop_reg_array[] = {{0x301a, 0x18}};;
struct i2c_random_wr_payload stop_reg_array[] = {{0x301a, 0x918}};;
struct i2c_random_wr_payload init_array_ar0231[] = {
{0x3092, 0x0C24},
{0x337A, 0x0C80},
{0x3520, 0x1288},
{0x3522, 0x880C},
{0x3524, 0x0C12},
{0x352C, 0x1212},
{0x354A, 0x007F},
{0x350C, 28},
{0x3506, 0x3333},
{0x3508, 0x3333},
{0x3100, 0x4000},
{0x3280, 0x0FA0},
{0x3282, 0x0FA0},
{0x3284, 0x0FA0},
{0x3286, 0x0FA0},
{0x3288, 0x0FA0},
{0x328A, 0x0FA0},
{0x328C, 0x0FA0},
{0x328E, 0x0FA0},
{0x3290, 0x0FA0},
{0x3292, 0x0FA0},
{0x3294, 0x0FA0},
{0x3296, 0x0FA0},
{0x3298, 0x0FA0},
{0x329A, 0x0FA0},
{0x329C, 0x0FA0},
{0x329E, 0x0FA0},
{0x301A, 0x0018}, // RESET_REGISTER
//
{0x3092, 0x0C24}, // ROW_NOISE_CONTROL
{0x337A, 0x0C80}, // DBLC_SCALE0
{0x3520, 0x1288}, // RESERVED_MFR_3520
{0x3522, 0x880C}, // RESERVED_MFR_3522
{0x3524, 0x0C12}, // RESERVED_MFR_3524
{0x352C, 0x1212}, // RESERVED_MFR_352C
{0x354A, 0x007F}, // RESERVED_MFR_354A
{0x350C, 0x055C}, // RESERVED_MFR_350C
{0x3506, 0x3333}, // RESERVED_MFR_3506
{0x3508, 0x3333}, // RESERVED_MFR_3508
{0x3100, 0x4000}, // DLO_CONTROL0
{0x3280, 0x0CCC}, // RESERVED_MFR_3280
{0x3282, 0x0CCC}, // RESERVED_MFR_3282
{0x3284, 0x0CCC}, // RESERVED_MFR_3284
{0x3286, 0x0CCC}, // RESERVED_MFR_3286
{0x3288, 0x0FA0}, // RESERVED_MFR_3288
{0x328A, 0x0FA0}, // RESERVED_MFR_328A
{0x328C, 0x0FA0}, // RESERVED_MFR_328C
{0x328E, 0x0FA0}, // RESERVED_MFR_328E
{0x3290, 0x0FA0}, // RESERVED_MFR_3290
{0x3292, 0x0FA0}, // RESERVED_MFR_3292
{0x3294, 0x0FA0}, // RESERVED_MFR_3294
{0x3296, 0x0FA0}, // RESERVED_MFR_3296
{0x3298, 0x0FA0}, // RESERVED_MFR_3298
{0x329A, 0x0FA0}, // RESERVED_MFR_329A
{0x329C, 0x0FA0}, // RESERVED_MFR_329C
{0x329E, 0x0FA0}, // RESERVED_MFR_329E
{0x2512, 0x8000}, // SEQ_CTRL_PORT
{0x2510, 0x0905}, // SEQ_DATA_PORT
{0x2510, 0x3350}, // SEQ_DATA_PORT
{0x2510, 0x2004}, // SEQ_DATA_PORT
{0x2510, 0x1460}, // SEQ_DATA_PORT
{0x2510, 0x1578}, // SEQ_DATA_PORT
{0x2510, 0x0901}, // SEQ_DATA_PORT
{0x2510, 0x7B24}, // SEQ_DATA_PORT
{0x2510, 0xFF24}, // SEQ_DATA_PORT
{0x2510, 0xFF24}, // SEQ_DATA_PORT
{0x2510, 0xEA24}, // SEQ_DATA_PORT
{0x2510, 0x1022}, // SEQ_DATA_PORT
{0x2510, 0x2410}, // SEQ_DATA_PORT
{0x2510, 0x155A}, // SEQ_DATA_PORT
{0x2510, 0x0901}, // SEQ_DATA_PORT
{0x2510, 0x1400}, // SEQ_DATA_PORT
{0x2510, 0x24FF}, // SEQ_DATA_PORT
{0x2510, 0x24FF}, // SEQ_DATA_PORT
{0x2510, 0x24EA}, // SEQ_DATA_PORT
{0x2510, 0x2324}, // SEQ_DATA_PORT
{0x2510, 0x647A}, // SEQ_DATA_PORT
{0x2510, 0x2404}, // SEQ_DATA_PORT
{0x2510, 0x052C}, // SEQ_DATA_PORT
{0x2510, 0x400A}, // SEQ_DATA_PORT
{0x2510, 0xFF0A}, // SEQ_DATA_PORT
{0x2510, 0xFF0A}, // SEQ_DATA_PORT
{0x2510, 0x1008}, // SEQ_DATA_PORT
{0x2510, 0x3851}, // SEQ_DATA_PORT
{0x2510, 0x1440}, // SEQ_DATA_PORT
{0x2510, 0x0004}, // SEQ_DATA_PORT
{0x2510, 0x0801}, // SEQ_DATA_PORT
{0x2510, 0x0408}, // SEQ_DATA_PORT
{0x2510, 0x1180}, // SEQ_DATA_PORT
{0x2510, 0x2652}, // SEQ_DATA_PORT
{0x2510, 0x1518}, // SEQ_DATA_PORT
{0x2510, 0x0906}, // SEQ_DATA_PORT
{0x2510, 0x1348}, // SEQ_DATA_PORT
{0x2510, 0x1002}, // SEQ_DATA_PORT
{0x2510, 0x1016}, // SEQ_DATA_PORT
{0x2510, 0x1181}, // SEQ_DATA_PORT
{0x2510, 0x1189}, // SEQ_DATA_PORT
{0x2510, 0x1056}, // SEQ_DATA_PORT
{0x2510, 0x1210}, // SEQ_DATA_PORT
{0x2510, 0x0901}, // SEQ_DATA_PORT
{0x2510, 0x0D09}, // SEQ_DATA_PORT
{0x2510, 0x1413}, // SEQ_DATA_PORT
{0x2510, 0x8809}, // SEQ_DATA_PORT
{0x2510, 0x2B15}, // SEQ_DATA_PORT
{0x2510, 0x8809}, // SEQ_DATA_PORT
{0x2510, 0x0311}, // SEQ_DATA_PORT
{0x2510, 0xD909}, // SEQ_DATA_PORT
{0x2510, 0x1214}, // SEQ_DATA_PORT
{0x2510, 0x4109}, // SEQ_DATA_PORT
{0x2510, 0x0312}, // SEQ_DATA_PORT
{0x2510, 0x1409}, // SEQ_DATA_PORT
{0x2510, 0x0110}, // SEQ_DATA_PORT
{0x2510, 0xD612}, // SEQ_DATA_PORT
{0x2510, 0x1012}, // SEQ_DATA_PORT
{0x2510, 0x1212}, // SEQ_DATA_PORT
{0x2510, 0x1011}, // SEQ_DATA_PORT
{0x2510, 0xDD11}, // SEQ_DATA_PORT
{0x2510, 0xD910}, // SEQ_DATA_PORT
{0x2510, 0x5609}, // SEQ_DATA_PORT
{0x2510, 0x1511}, // SEQ_DATA_PORT
{0x2510, 0xDB09}, // SEQ_DATA_PORT
{0x2510, 0x1511}, // SEQ_DATA_PORT
{0x2510, 0x9B09}, // SEQ_DATA_PORT
{0x2510, 0x0F11}, // SEQ_DATA_PORT
{0x2510, 0xBB12}, // SEQ_DATA_PORT
{0x2510, 0x1A12}, // SEQ_DATA_PORT
{0x2510, 0x1014}, // SEQ_DATA_PORT
{0x2510, 0x6012}, // SEQ_DATA_PORT
{0x2510, 0x5010}, // SEQ_DATA_PORT
{0x2510, 0x7610}, // SEQ_DATA_PORT
{0x2510, 0xE609}, // SEQ_DATA_PORT
{0x2510, 0x0812}, // SEQ_DATA_PORT
{0x2510, 0x4012}, // SEQ_DATA_PORT
{0x2510, 0x6009}, // SEQ_DATA_PORT
{0x2510, 0x290B}, // SEQ_DATA_PORT
{0x2510, 0x0904}, // SEQ_DATA_PORT
{0x2510, 0x1440}, // SEQ_DATA_PORT
{0x2510, 0x0923}, // SEQ_DATA_PORT
{0x2510, 0x15C8}, // SEQ_DATA_PORT
{0x2510, 0x13C8}, // SEQ_DATA_PORT
{0x2510, 0x092C}, // SEQ_DATA_PORT
{0x2510, 0x1588}, // SEQ_DATA_PORT
{0x2510, 0x1388}, // SEQ_DATA_PORT
{0x2510, 0x0C09}, // SEQ_DATA_PORT
{0x2510, 0x0C14}, // SEQ_DATA_PORT
{0x2510, 0x4109}, // SEQ_DATA_PORT
{0x2510, 0x1112}, // SEQ_DATA_PORT
{0x2510, 0x6212}, // SEQ_DATA_PORT
{0x2510, 0x6011}, // SEQ_DATA_PORT
{0x2510, 0xBF11}, // SEQ_DATA_PORT
{0x2510, 0xBB10}, // SEQ_DATA_PORT
{0x2510, 0x6611}, // SEQ_DATA_PORT
{0x2510, 0xFB09}, // SEQ_DATA_PORT
{0x2510, 0x3511}, // SEQ_DATA_PORT
{0x2510, 0xBB12}, // SEQ_DATA_PORT
{0x2510, 0x6312}, // SEQ_DATA_PORT
{0x2510, 0x6014}, // SEQ_DATA_PORT
{0x2510, 0x0015}, // SEQ_DATA_PORT
{0x2510, 0x0011}, // SEQ_DATA_PORT
{0x2510, 0xB812}, // SEQ_DATA_PORT
{0x2510, 0xA012}, // SEQ_DATA_PORT
{0x2510, 0x0010}, // SEQ_DATA_PORT
{0x2510, 0x2610}, // SEQ_DATA_PORT
{0x2510, 0x0013}, // SEQ_DATA_PORT
{0x2510, 0x0011}, // SEQ_DATA_PORT
{0x2510, 0x0008}, // SEQ_DATA_PORT
{0x2510, 0x3053}, // SEQ_DATA_PORT
{0x2510, 0x4215}, // SEQ_DATA_PORT
{0x2510, 0x4013}, // SEQ_DATA_PORT
{0x2510, 0x4010}, // SEQ_DATA_PORT
{0x2510, 0x0210}, // SEQ_DATA_PORT
{0x2510, 0x1611}, // SEQ_DATA_PORT
{0x2510, 0x8111}, // SEQ_DATA_PORT
{0x2510, 0x8910}, // SEQ_DATA_PORT
{0x2510, 0x5612}, // SEQ_DATA_PORT
{0x2510, 0x1009}, // SEQ_DATA_PORT
{0x2510, 0x010D}, // SEQ_DATA_PORT
{0x2510, 0x0815}, // SEQ_DATA_PORT
{0x2510, 0xC015}, // SEQ_DATA_PORT
{0x2510, 0xD013}, // SEQ_DATA_PORT
{0x2510, 0x5009}, // SEQ_DATA_PORT
{0x2510, 0x1313}, // SEQ_DATA_PORT
{0x2510, 0xD009}, // SEQ_DATA_PORT
{0x2510, 0x0215}, // SEQ_DATA_PORT
{0x2510, 0xC015}, // SEQ_DATA_PORT
{0x2510, 0xC813}, // SEQ_DATA_PORT
{0x2510, 0xC009}, // SEQ_DATA_PORT
{0x2510, 0x0515}, // SEQ_DATA_PORT
{0x2510, 0x8813}, // SEQ_DATA_PORT
{0x2510, 0x8009}, // SEQ_DATA_PORT
{0x2510, 0x0213}, // SEQ_DATA_PORT
{0x2510, 0x8809}, // SEQ_DATA_PORT
{0x2510, 0x0411}, // SEQ_DATA_PORT
{0x2510, 0xC909}, // SEQ_DATA_PORT
{0x2510, 0x0814}, // SEQ_DATA_PORT
{0x2510, 0x0109}, // SEQ_DATA_PORT
{0x2510, 0x0B11}, // SEQ_DATA_PORT
{0x2510, 0xD908}, // SEQ_DATA_PORT
{0x2510, 0x1400}, // SEQ_DATA_PORT
{0x2510, 0x091A}, // SEQ_DATA_PORT
{0x2510, 0x1440}, // SEQ_DATA_PORT
{0x2510, 0x0903}, // SEQ_DATA_PORT
{0x2510, 0x1214}, // SEQ_DATA_PORT
{0x2510, 0x0901}, // SEQ_DATA_PORT
{0x2510, 0x10D6}, // SEQ_DATA_PORT
{0x2510, 0x1210}, // SEQ_DATA_PORT
{0x2510, 0x1212}, // SEQ_DATA_PORT
{0x2510, 0x1210}, // SEQ_DATA_PORT
{0x2510, 0x11DD}, // SEQ_DATA_PORT
{0x2510, 0x11D9}, // SEQ_DATA_PORT
{0x2510, 0x1056}, // SEQ_DATA_PORT
{0x2510, 0x0917}, // SEQ_DATA_PORT
{0x2510, 0x11DB}, // SEQ_DATA_PORT
{0x2510, 0x0913}, // SEQ_DATA_PORT
{0x2510, 0x11FB}, // SEQ_DATA_PORT
{0x2510, 0x0905}, // SEQ_DATA_PORT
{0x2510, 0x11BB}, // SEQ_DATA_PORT
{0x2510, 0x121A}, // SEQ_DATA_PORT
{0x2510, 0x1210}, // SEQ_DATA_PORT
{0x2510, 0x1460}, // SEQ_DATA_PORT
{0x2510, 0x1250}, // SEQ_DATA_PORT
{0x2510, 0x1076}, // SEQ_DATA_PORT
{0x2510, 0x10E6}, // SEQ_DATA_PORT
{0x2510, 0x0901}, // SEQ_DATA_PORT
{0x2510, 0x15A8}, // SEQ_DATA_PORT
{0x2510, 0x0901}, // SEQ_DATA_PORT
{0x2510, 0x13A8}, // SEQ_DATA_PORT
{0x2510, 0x1240}, // SEQ_DATA_PORT
{0x2510, 0x1260}, // SEQ_DATA_PORT
{0x2510, 0x0925}, // SEQ_DATA_PORT
{0x2510, 0x13AD}, // SEQ_DATA_PORT
{0x2510, 0x0902}, // SEQ_DATA_PORT
{0x2510, 0x0907}, // SEQ_DATA_PORT
{0x2510, 0x1588}, // SEQ_DATA_PORT
{0x2510, 0x0901}, // SEQ_DATA_PORT
{0x2510, 0x138D}, // SEQ_DATA_PORT
{0x2510, 0x0B09}, // SEQ_DATA_PORT
{0x2510, 0x0914}, // SEQ_DATA_PORT
{0x2510, 0x4009}, // SEQ_DATA_PORT
{0x2510, 0x0B13}, // SEQ_DATA_PORT
{0x2510, 0x8809}, // SEQ_DATA_PORT
{0x2510, 0x1C0C}, // SEQ_DATA_PORT
{0x2510, 0x0920}, // SEQ_DATA_PORT
{0x2510, 0x1262}, // SEQ_DATA_PORT
{0x2510, 0x1260}, // SEQ_DATA_PORT
{0x2510, 0x11BF}, // SEQ_DATA_PORT
{0x2510, 0x11BB}, // SEQ_DATA_PORT
{0x2510, 0x1066}, // SEQ_DATA_PORT
{0x2510, 0x090A}, // SEQ_DATA_PORT
{0x2510, 0x11FB}, // SEQ_DATA_PORT
{0x2510, 0x093B}, // SEQ_DATA_PORT
{0x2510, 0x11BB}, // SEQ_DATA_PORT
{0x2510, 0x1263}, // SEQ_DATA_PORT
{0x2510, 0x1260}, // SEQ_DATA_PORT
{0x2510, 0x1400}, // SEQ_DATA_PORT
{0x2510, 0x1508}, // SEQ_DATA_PORT
{0x2510, 0x11B8}, // SEQ_DATA_PORT
{0x2510, 0x12A0}, // SEQ_DATA_PORT
{0x2510, 0x1200}, // SEQ_DATA_PORT
{0x2510, 0x1026}, // SEQ_DATA_PORT
{0x2510, 0x1000}, // SEQ_DATA_PORT
{0x2510, 0x1300}, // SEQ_DATA_PORT
{0x2510, 0x1100}, // SEQ_DATA_PORT
{0x2510, 0x437A}, // SEQ_DATA_PORT
{0x2510, 0x0609}, // SEQ_DATA_PORT
{0x2510, 0x0B05}, // SEQ_DATA_PORT
{0x2510, 0x0708}, // SEQ_DATA_PORT
{0x2510, 0x4137}, // SEQ_DATA_PORT
{0x2510, 0x502C}, // SEQ_DATA_PORT
{0x2510, 0x2CFE}, // SEQ_DATA_PORT
{0x2510, 0x15FE}, // SEQ_DATA_PORT
{0x2510, 0x0C2C}, // SEQ_DATA_PORT
{0x32E6, 0x00E0}, // RESERVED_MFR_32E6
{0x1008, 0x036F}, // RESERVED_PARAM_1008
{0x100C, 0x058F}, // RESERVED_PARAM_100C
{0x100E, 0x07AF}, // RESERVED_PARAM_100E
{0x1010, 0x014F}, // RESERVED_PARAM_1010
{0x3230, 0x0312}, // FINE_CORRECTION
{0x3232, 0x0532}, // FINE_CORRECTION2
{0x3234, 0x0752}, // FINE_CORRECTION3
{0x3236, 0x00F2}, // FINE_CORRECTION4
{0x3566, 0x3328}, // RESERVED_MFR_3566
{0x32D0, 0x3A02}, // RESERVED_MFR_32D0
{0x32D2, 0x3508}, // RESERVED_MFR_32D2
{0x32D4, 0x3702}, // RESERVED_MFR_32D4
{0x32D6, 0x3C04}, // RESERVED_MFR_32D6
{0x32DC, 0x370A}, // RESERVED_MFR_32DC
{0x30B0, 0x0800}, // DIGITAL_TEST
{0x302A, 0x0006}, // VT_PIX_CLK_DIV
{0x302C, 0x0001}, // VT_SYS_CLK_DIV
{0x302E, 0x0002}, // PRE_PLL_CLK_DIV
{0x3030, 0x002C}, // PLL_MULTIPLIER
{0x3036, 0x000A}, // OP_WORD_CLK_DIV
{0x3038, 0x0001}, // OP_SYS_CLK_DIV
{0x30B0, 0x0800}, // DIGITAL_TEST
{0x30A2, 0x0001}, // X_ODD_INC_
{0x30A6, 0x0001}, // Y_ODD_INC_
{0x3040, 0xC000}, // READ_MODE C000
{0x30BA, 0x11F2}, // DIGITAL_CTRL
{0x3044, 0x0400}, // DARK_CONTROL
{0x3064, 0x1802}, // SMIA_TEST
/*{0x3064, 0xCC2}, // STATS_EN
{0x3270, 0x10}, //
{0x3272, 0x30}, //
{0x3274, 0x50}, //
{0x3276, 0x10}, //
{0x3278, 0x30}, //
{0x327A, 0x50}, //
// SEQ_DATA_PORT?
{0x2512, 0x8000},
{0x2510, 0x0905},
{0x2510, 0x3350},
{0x2510, 0x2004},
{0x2510, 0x1460},
{0x2510, 0x1578},
{0x2510, 0x0901},
{0x2510, 0x7B24},
{0x2510, 0xFF24},
{0x2510, 0xFF24},
{0x2510, 0xEA24},
{0x2510, 0x1022},
{0x2510, 0x2410},
{0x2510, 0x155A},
{0x2510, 0x0901},
{0x2510, 0x1400},
{0x2510, 0x24FF},
{0x2510, 0x24FF},
{0x2510, 0x24EA},
{0x2510, 0x2324},
{0x2510, 0x647A},
{0x2510, 0x2404},
{0x2510, 0x052C},
{0x2510, 0x400A},
{0x2510, 0xFF0A},
{0x2510, 0xFF0A},
{0x2510, 0x1008},
{0x2510, 0x3851},
{0x2510, 0x1440},
{0x2510, 0x0004},
{0x2510, 0x0801},
{0x2510, 0x0408},
{0x2510, 0x1180},
{0x2510, 0x2652},
{0x2510, 0x1518},
{0x2510, 0x0906},
{0x2510, 0x1348},
{0x2510, 0x1002},
{0x2510, 0x1016},
{0x2510, 0x1181},
{0x2510, 0x1189},
{0x2510, 0x1056},
{0x2510, 0x1210},
{0x2510, 0x0901},
{0x2510, 0x0D09},
{0x2510, 0x1413},
{0x2510, 0x8809},
{0x2510, 0x2B15},
{0x2510, 0x8809},
{0x2510, 0x0311},
{0x2510, 0xD909},
{0x2510, 0x1214},
{0x2510, 0x4109},
{0x2510, 0x0312},
{0x2510, 0x1409},
{0x2510, 0x0110},
{0x2510, 0xD612},
{0x2510, 0x1012},
{0x2510, 0x1212},
{0x2510, 0x1011},
{0x2510, 0xDD11},
{0x2510, 0xD910},
{0x2510, 0x5609},
{0x2510, 0x1511},
{0x2510, 0xDB09},
{0x2510, 0x1511},
{0x2510, 0x9B09},
{0x2510, 0x0F11},
{0x2510, 0xBB12},
{0x2510, 0x1A12},
{0x2510, 0x1014},
{0x2510, 0x6012},
{0x2510, 0x5010},
{0x2510, 0x7610},
{0x2510, 0xE609},
{0x2510, 0x0812},
{0x2510, 0x4012},
{0x2510, 0x6009},
{0x2510, 0x290B},
{0x2510, 0x0904},
{0x2510, 0x1440},
{0x2510, 0x0923},
{0x2510, 0x15C8},
{0x2510, 0x13C8},
{0x2510, 0x092C},
{0x2510, 0x1588},
{0x2510, 0x1388},
{0x2510, 0x0C09},
{0x2510, 0x0C14},
{0x2510, 0x4109},
{0x2510, 0x1112},
{0x2510, 0x6212},
{0x2510, 0x6011},
{0x2510, 0xBF11},
{0x2510, 0xBB10},
{0x2510, 0x6611},
{0x2510, 0xFB09},
{0x2510, 0x3511},
{0x2510, 0xBB12},
{0x2510, 0x6312},
{0x2510, 0x6014},
{0x2510, 0x0015},
{0x2510, 0x0011},
{0x2510, 0xB812},
{0x2510, 0xA012},
{0x2510, 0x0010},
{0x2510, 0x2610},
{0x2510, 0x0013},
{0x2510, 0x0011},
{0x2510, 0x0008},
{0x2510, 0x3053},
{0x2510, 0x4215},
{0x2510, 0x4013},
{0x2510, 0x4010},
{0x2510, 0x0210},
{0x2510, 0x1611},
{0x2510, 0x8111},
{0x2510, 0x8910},
{0x2510, 0x5612},
{0x2510, 0x1009},
{0x2510, 0x010D},
{0x2510, 0x0815},
{0x2510, 0xC015},
{0x2510, 0xD013},
{0x2510, 0x5009},
{0x2510, 0x1313},
{0x2510, 0xD009},
{0x2510, 0x0215},
{0x2510, 0xC015},
{0x2510, 0xC813},
{0x2510, 0xC009},
{0x2510, 0x0515},
{0x2510, 0x8813},
{0x2510, 0x8009},
{0x2510, 0x0213},
{0x2510, 0x8809},
{0x2510, 0x0411},
{0x2510, 0xC909},
{0x2510, 0x0814},
{0x2510, 0x0109},
{0x2510, 0x0B11},
{0x2510, 0xD908},
{0x2510, 0x1400},
{0x2510, 0x091A},
{0x2510, 0x1440},
{0x2510, 0x0903},
{0x2510, 0x1214},
{0x2510, 0x0901},
{0x2510, 0x10D6},
{0x2510, 0x1210},
{0x2510, 0x1212},
{0x2510, 0x1210},
{0x2510, 0x11DD},
{0x2510, 0x11D9},
{0x2510, 0x1056},
{0x2510, 0x0917},
{0x2510, 0x11DB},
{0x2510, 0x0913},
{0x2510, 0x11FB},
{0x2510, 0x0905},
{0x2510, 0x11BB},
{0x2510, 0x121A},
{0x2510, 0x1210},
{0x2510, 0x1460},
{0x2510, 0x1250},
{0x2510, 0x1076},
{0x2510, 0x10E6},
{0x2510, 0x0901},
{0x2510, 0x15A8},
{0x2510, 0x0901},
{0x2510, 0x13A8},
{0x2510, 0x1240},
{0x2510, 0x1260},
{0x2510, 0x0925},
{0x2510, 0x13AD},
{0x2510, 0x0902},
{0x2510, 0x0907},
{0x2510, 0x1588},
{0x2510, 0x0901},
{0x2510, 0x138D},
{0x2510, 0x0B09},
{0x2510, 0x0914},
{0x2510, 0x4009},
{0x2510, 0x0B13},
{0x2510, 0x8809},
{0x2510, 0x1C0C},
{0x2510, 0x0920},
{0x2510, 0x1262},
{0x2510, 0x1260},
{0x2510, 0x11BF},
{0x2510, 0x11BB},
{0x2510, 0x1066},
{0x2510, 0x090A},
{0x2510, 0x11FB},
{0x2510, 0x093B},
{0x2510, 0x11BB},
{0x2510, 0x1263},
{0x2510, 0x1260},
{0x2510, 0x1400},
{0x2510, 0x1508},
{0x2510, 0x11B8},
{0x2510, 0x12A0},
{0x2510, 0x1200},
{0x2510, 0x1026},
{0x2510, 0x1000},
{0x2510, 0x1300},
{0x2510, 0x1100},
{0x2510, 0x437A},
{0x2510, 0x0609},
{0x2510, 0x0B05},
{0x2510, 0x0708},
{0x2510, 0x4137},
{0x2510, 0x502C},
{0x2510, 0x2CFE},
{0x2510, 0x15FE},
{0x2510, 0x0C2C},
{0x3144, 0x0}, //
{0x3146, 0x0}, //
{0x3244, 0x0}, //
{0x3246, 0x0}, //
{0x3268, 0x0}, //
{0x326A, 0x0}, //
*/
{0x33E0, 0x0C80}, // TEST_ASIL_ROWS
{0x3180, 0x0080}, // RESERVED_MFR_3180
{0x33E4, 0x0080}, // RESERVED_MFR_33E4
{0x33E0, 0x0C80}, // TEST_ASIL_ROWS
{0x33E0, 0x0C80}, // TEST_ASIL_ROWS
{0x3004, 0x0000}, // X_ADDR_START_
{0x3008, 0x0787}, // X_ADDR_END_ 787
{0x3002, 0x0000}, // Y_ADDR_START_
{0x3006, 0x04B7}, // Y_ADDR_END_ 4B7
{0x3032, 0x0000}, // SCALING_MODE
{0x3400, 0x0010}, // RESERVED_MFR_3400
{0x3402, 0x0788}, // X_OUTPUT_CONTROL
{0x3402, 0x0F10}, // X_OUTPUT_CONTROL
{0x3404, 0x04B8}, // Y_OUTPUT_CONTROL
{0x3404, 0x0970}, // Y_OUTPUT_CONTROL
{0x30BA, 0x11F3}, // DIGITAL_CTRL
// end SEQ_DATA_PORT
{0x32e6,0xe0},
// SLAV* MODE
{0x30CE, 0x0120},
{0x340A, 0xE6}, // E6 // 0000 1110 0110
{0x340C, 0x802}, // 2 // 0000 0000 0010
// exposure time
{0x1008,0x36f},
{0x100c,0x58f},
{0x100e,0x7af},
{0x1010,0x14f},
{0x3230,0x312},
{0x3232,0x532},
{0x3234,0x752},
{0x3236,0xf2},
// FPS = 88e6 / 0x09C4 / 0x06E0 = 20
{0x300C, 0x09B4}, // LINE_LENGTH_PCK_ 9B4
{0x300A, 0x06EB}, // FRAME_LENGTH_LINES_ 6EB
{0x3042, 0x0000}, // EXTRA_DELAY
{0x3566, 0x0},
{0x32D0, 0x3A02},
{0x32D2, 0x3508},
{0x32D4, 0x3702},
{0x32D6, 0x3C04},
{0x32DC, 0x370A},
{0x30b0, 0x200},
{0x3082, 0x0},
{0x33E0, 0x0080},
{0x3180, 0x0080},
{0x33E4, 0x0080},
{0x33E0, 0x0C00},
{0x33E0, 0x0000},
// Readout Settings
{0x31AE, 0x0204}, // SERIAL_FORMAT, 4-lane MIPI
{0x31AC, 0x0C0A}, // DATA_FORMAT_BITS, 12 -> 10
{0x3342, 0x122B}, // MIPI_F1_PDT_EDT
{0x3346, 0x122B}, // MIPI_F2_PDT_EDT
{0x334A, 0x122B}, // MIPI_F3_PDT_EDT
{0x334E, 0x122B}, // MIPI_F4_PDT_EDT
{0x3344, 0x0011}, // MIPI_F1_VDT_VC
{0x3348, 0x0111}, // MIPI_F2_VDT_VC
{0x334C, 0x0211}, // MIPI_F3_VDT_VC
{0x3350, 0x0311}, // MIPI_F4_VDT_VC
{0x31B0, 0x0049}, // FRAME_PREAMBLE
{0x31B2, 0x0033}, // LINE_PREAMBLE
{0x31B4, 0x2185}, // RESERVED_MFR_31B4
{0x31B6, 0x1146}, // RESERVED_MFR_31B6
{0x31B8, 0x3047}, // RESERVED_MFR_31B8
{0x31BA, 0x0186}, // RESERVED_MFR_31BA
{0x31BC, 0x0805}, // RESERVED_MFR_31BC
{0x301A, 0x01C}, // RESET_REGISTER
{0x31B4, 0x2185},
{0x31B6, 0x1146},
{0x31B8, 0x3047},
// HDR Settings
{0x3082, 0x0004}, // OPERATION_MODE_CTRL
{0x3238, 0x0444}, // EXPOSURE_RATIO
{0x3014, 0x098E}, // FINE_INTEGRATION_TIME_
{0x321E, 0x098E}, // FINE_INTEGRATION_TIME2
{0x3222, 0x098E}, // FINE_INTEGRATION_TIME3
{0x3226, 0x098E}, // FINE_INTEGRATION_TIME4, 098E?
{0x30B0, 0x0800}, // DIGITAL_TEST
{0x32EA, 0x3C0E}, // RESERVED_MFR_32EA
{0x32EC, 0x72A1}, // RESERVED_MFR_32EC
{0x31D0, 0x0000}, // COMPANDING, no good in 10 bit?
{0x33DA, 0x0000}, // COMPANDING
{0x3362, 0x0000}, // DC GAIN
{0x3370, 0x0231}, // DBLC
{0x318E, 0x0200}, // PRE_HDR_GAIN_EN
{0x31BA, 0x186},
{0x31BC, 0x805},
// Initial Gains
{0x3022, 0x01}, // GROUPED_PARAMETER_HOLD_
{0x3366, 0x7777}, // ANALOG_GAIN
{0x3060, 0xBBBB}, // ANALOG_COLOR_GAIN
{0x305A, 0x00C4}, // RED_GAIN
{0x3058, 0x00B1}, // BLUE_GAIN
{0x3056, 0x009A}, // GREEN1_GAIN
{0x305C, 0x009A}, // GREEN2_GAIN
{0x3022, 0x00}, // GROUPED_PARAMETER_HOLD_
// additions
// mipi + 4 lanes
{0x31AE, 0x0204},
// DATA_FORMAT_RAW = 12
// DATA_FORMAT_OUTPUT = 0?
//{0x31AC, 0x0C08},
{0x31AC, 0x0C0A},
// Initial Integration Time
{0x3012, 0x256},
// 0x2B = CSI_RAW10
{0x3342, 0x122B},
{0x3346, 0x122B},
{0x334a, 0x122B},
{0x334e, 0x122B},
// 10-bit
{0x3036, 0xA},
};
struct i2c_random_wr_payload poke_array_ov7750[] = {

413
selfdrive/camerad/main.cc
View File

@ -33,6 +33,7 @@
#include <jpeglib.h>
#define UI_BUF_COUNT 4
#define DEBAYER_LOCAL_WORKSIZE 16
#define YUV_COUNT 40
#define MAX_CLIENTS 5
@ -44,6 +45,10 @@ void set_do_exit(int sig) {
do_exit = 1;
}
/*
TODO: refactor out camera specific things from here
*/
struct VisionState;
struct VisionClientState {
@ -74,8 +79,13 @@ struct VisionState {
cl_program prg_debayer_rear;
cl_program prg_debayer_front;
cl_program prg_debayer_wide;
cl_kernel krnl_debayer_rear;
cl_kernel krnl_debayer_front;
cl_kernel krnl_debayer_wide;
int debayer_cl_localMemSize;
size_t debayer_cl_globalWorkSize[2];
size_t debayer_cl_localWorkSize[2];
cl_program prg_rgb_laplacian;
cl_kernel krnl_rgb_laplacian;
@ -88,10 +98,12 @@ struct VisionState {
// processing
TBuffer ui_tb;
TBuffer ui_front_tb;
TBuffer ui_wide_tb;
mat3 yuv_transform;
TBuffer *yuv_tb;
TBuffer *yuv_front_tb;
TBuffer *yuv_wide_tb;
// TODO: refactor for both cameras?
Pool yuv_pool;
@ -113,6 +125,15 @@ struct VisionState {
int yuv_front_width, yuv_front_height;
RGBToYUVState front_rgb_to_yuv_state;
Pool yuv_wide_pool;
VisionBuf yuv_wide_ion[YUV_COUNT];
cl_mem yuv_wide_cl[YUV_COUNT];
YUVBuf yuv_wide_bufs[YUV_COUNT];
FrameMetadata yuv_wide_metas[YUV_COUNT];
size_t yuv_wide_buf_size;
int yuv_wide_width, yuv_wide_height;
RGBToYUVState wide_rgb_to_yuv_state;
size_t rgb_buf_size;
int rgb_width, rgb_height, rgb_stride;
VisionBuf rgb_bufs[UI_BUF_COUNT];
@ -129,6 +150,10 @@ struct VisionState {
int front_meteringbox_xmin, front_meteringbox_xmax;
int front_meteringbox_ymin, front_meteringbox_ymax;
size_t rgb_wide_buf_size;
int rgb_wide_width, rgb_wide_height, rgb_wide_stride;
VisionBuf rgb_wide_bufs[UI_BUF_COUNT];
cl_mem rgb_wide_bufs_cl[UI_BUF_COUNT];
cl_mem camera_bufs_cl[FRAME_BUF_COUNT];
VisionBuf camera_bufs[FRAME_BUF_COUNT];
@ -138,7 +163,11 @@ struct VisionState {
cl_mem front_camera_bufs_cl[FRAME_BUF_COUNT];
VisionBuf front_camera_bufs[FRAME_BUF_COUNT];
DualCameraState cameras;
cl_mem wide_camera_bufs_cl[FRAME_BUF_COUNT];
VisionBuf wide_camera_bufs[FRAME_BUF_COUNT];
MultiCameraState cameras;
zsock_t *terminate_pub;
@ -179,14 +208,20 @@ void* frontview_thread(void *arg) {
assert(err == 0);
err = clSetKernelArg(s->krnl_debayer_front, 1, sizeof(cl_mem), &s->rgb_front_bufs_cl[rgb_idx]);
assert(err == 0);
#ifdef QCOM2
err = clSetKernelArg(s->krnl_debayer_front, 2, s->debayer_cl_localMemSize, 0);
assert(err == 0);
err = clEnqueueNDRangeKernel(q, s->krnl_debayer_front, 2, NULL,
s->debayer_cl_globalWorkSize, s->debayer_cl_localWorkSize, 0, 0, &debayer_event);
#else
float digital_gain = 1.0;
err = clSetKernelArg(s->krnl_debayer_front, 2, sizeof(float), &digital_gain);
assert(err == 0);
const size_t debayer_work_size = s->rgb_front_height; // doesn't divide evenly, is this okay?
const size_t debayer_local_work_size = 128;
err = clEnqueueNDRangeKernel(q, s->krnl_debayer_front, 1, NULL,
&debayer_work_size, &debayer_local_work_size, 0, 0, &debayer_event);
#endif
assert(err == 0);
} else {
assert(s->rgb_front_buf_size >= s->cameras.front.frame_size);
@ -208,6 +243,12 @@ void* frontview_thread(void *arg) {
s->rhd_front_checked = state.getRhdChecked();
}
#ifdef NOSCREEN
if (frame_data.frame_id % 4 == 2) {
sendrgb(&s->cameras, (uint8_t*) s->rgb_front_bufs[rgb_idx].addr, s->rgb_front_bufs[rgb_idx].len, 2);
}
#endif
if (sm.updated("driverState")) {
auto state = sm["driverState"].getDriverState();
float face_prob = state.getFaceProb();
@ -256,7 +297,12 @@ void* frontview_thread(void *arg) {
x_start = s->rhd_front ? 0:s->rgb_front_width * 3 / 5;
x_end = s->rhd_front ? s->rgb_front_width * 2 / 5:s->rgb_front_width;
}
#ifdef QCOM2
x_start = 0.15*s->rgb_front_width;
x_end = 0.85*s->rgb_front_width;
y_start = 0.15*s->rgb_front_height;
y_end = 0.85*s->rgb_front_height;
#endif
uint32_t lum_binning[256] = {0,};
for (int y = y_start; y < y_end; ++y) {
for (int x = x_start; x < x_end; x += 2) { // every 2nd col
@ -336,6 +382,162 @@ void* frontview_thread(void *arg) {
return NULL;
}
#ifdef QCOM2
// wide
void* wideview_thread(void *arg) {
int err;
VisionState *s = (VisionState*)arg;
set_thread_name("wideview");
err = set_realtime_priority(1);
LOG("setpriority returns %d", err);
// init cl stuff
const cl_queue_properties props[] = {0}; //CL_QUEUE_PRIORITY_KHR, CL_QUEUE_PRIORITY_HIGH_KHR, 0};
cl_command_queue q = clCreateCommandQueueWithProperties(s->context, s->device_id, props, &err);
assert(err == 0);
// init the net
LOG("wideview start!");
for (int cnt = 0; !do_exit; cnt++) {
int buf_idx = tbuffer_acquire(&s->cameras.wide.camera_tb);
// int buf_idx = camera_acquire_buffer(s);
if (buf_idx < 0) {
break;
}
double t1 = millis_since_boot();
FrameMetadata frame_data = s->cameras.wide.camera_bufs_metadata[buf_idx];
uint32_t frame_id = frame_data.frame_id;
if (frame_id == -1) {
LOGE("no frame data? wtf");
tbuffer_release(&s->cameras.wide.camera_tb, buf_idx);
continue;
}
int ui_idx = tbuffer_select(&s->ui_wide_tb);
int rgb_idx = ui_idx;
cl_event debayer_event;
if (s->cameras.wide.ci.bayer) {
err = clSetKernelArg(s->krnl_debayer_wide, 0, sizeof(cl_mem), &s->wide_camera_bufs_cl[buf_idx]);
assert(err == 0);
err = clSetKernelArg(s->krnl_debayer_wide, 1, sizeof(cl_mem), &s->rgb_wide_bufs_cl[rgb_idx]);
assert(err == 0);
err = clSetKernelArg(s->krnl_debayer_wide, 2, s->debayer_cl_localMemSize, 0);
assert(err == 0);
err = clEnqueueNDRangeKernel(q, s->krnl_debayer_wide, 2, NULL,
s->debayer_cl_globalWorkSize, s->debayer_cl_localWorkSize, 0, 0, &debayer_event);
assert(err == 0);
} else {
assert(s->rgb_wide_buf_size >= s->frame_size);
assert(s->rgb_stride == s->frame_stride);
err = clEnqueueCopyBuffer(q, s->wide_camera_bufs_cl[buf_idx], s->rgb_wide_bufs_cl[rgb_idx],
0, 0, s->rgb_wide_buf_size, 0, 0, &debayer_event);
assert(err == 0);
}
clWaitForEvents(1, &debayer_event);
clReleaseEvent(debayer_event);
tbuffer_release(&s->cameras.wide.camera_tb, buf_idx);
visionbuf_sync(&s->rgb_wide_bufs[rgb_idx], VISIONBUF_SYNC_FROM_DEVICE);
#ifdef NOSCREEN
if (frame_data.frame_id % 4 == 0) {
sendrgb(&s->cameras, (uint8_t*) s->rgb_wide_bufs[rgb_idx].addr, s->rgb_wide_bufs[rgb_idx].len, 1);
}
#endif
double t2 = millis_since_boot();
double yt1 = millis_since_boot();
int yuv_idx = pool_select(&s->yuv_wide_pool);
s->yuv_wide_metas[yuv_idx] = frame_data;
uint8_t* yuv_ptr_y = s->yuv_wide_bufs[yuv_idx].y;
cl_mem yuv_wide_cl = s->yuv_wide_cl[yuv_idx];
rgb_to_yuv_queue(&s->wide_rgb_to_yuv_state, q, s->rgb_wide_bufs_cl[rgb_idx], yuv_wide_cl);
visionbuf_sync(&s->yuv_wide_ion[yuv_idx], VISIONBUF_SYNC_FROM_DEVICE);
double yt2 = millis_since_boot();
// keep another reference around till were done processing
pool_acquire(&s->yuv_wide_pool, yuv_idx);
pool_push(&s->yuv_wide_pool, yuv_idx);
// send frame event
{
if (s->pm != NULL) {
capnp::MallocMessageBuilder msg;
cereal::Event::Builder event = msg.initRoot<cereal::Event>();
event.setLogMonoTime(nanos_since_boot());
auto framed = event.initWideFrame();
framed.setFrameId(frame_data.frame_id);
framed.setEncodeId(cnt);
framed.setTimestampEof(frame_data.timestamp_eof);
framed.setFrameLength(frame_data.frame_length);
framed.setIntegLines(frame_data.integ_lines);
framed.setGlobalGain(frame_data.global_gain);
framed.setLensPos(frame_data.lens_pos);
framed.setLensSag(frame_data.lens_sag);
framed.setLensErr(frame_data.lens_err);
framed.setLensTruePos(frame_data.lens_true_pos);
framed.setGainFrac(frame_data.gain_frac);
s->pm->send("wideFrame", msg);
}
}
tbuffer_dispatch(&s->ui_wide_tb, ui_idx);
// auto exposure over big box
// TODO: fix this? should not use med imo
const int exposure_x = 240;
const int exposure_y = 300;
const int exposure_height = 600;
const int exposure_width = 1440;
if (cnt % 3 == 0) {
// find median box luminance for AE
uint32_t lum_binning[256] = {0,};
for (int y=0; y<exposure_height; y++) {
for (int x=0; x<exposure_width; x++) {
uint8_t lum = yuv_ptr_y[((exposure_y+y)*s->yuv_wide_width) + exposure_x + x];
lum_binning[lum]++;
}
}
const unsigned int lum_total = exposure_height * exposure_width;
unsigned int lum_cur = 0;
int lum_med = 0;
for (lum_med=0; lum_med<256; lum_med++) {
// shouldn't be any values less than 16 - yuv footroom
lum_cur += lum_binning[lum_med];
if (lum_cur >= lum_total / 2) {
break;
}
}
camera_autoexposure(&s->cameras.wide, lum_med / 256.0);
}
pool_release(&s->yuv_wide_pool, yuv_idx);
double t5 = millis_since_boot();
LOGD("queued: %.2fms, yuv: %.2f, | processing: %.3fms", (t2-t1), (yt2-yt1), (t5-t1));
}
return NULL;
}
#endif
// processing
void* processing_thread(void *arg) {
int err;
@ -390,13 +592,19 @@ void* processing_thread(void *arg) {
assert(err == 0);
err = clSetKernelArg(s->krnl_debayer_rear, 1, sizeof(cl_mem), &s->rgb_bufs_cl[rgb_idx]);
assert(err == 0);
#ifdef QCOM2
err = clSetKernelArg(s->krnl_debayer_rear, 2, s->debayer_cl_localMemSize, 0);
assert(err == 0);
err = clEnqueueNDRangeKernel(q, s->krnl_debayer_rear, 2, NULL,
s->debayer_cl_globalWorkSize, s->debayer_cl_localWorkSize, 0, 0, &debayer_event);
#else
err = clSetKernelArg(s->krnl_debayer_rear, 2, sizeof(float), &s->cameras.rear.digital_gain);
assert(err == 0);
const size_t debayer_work_size = s->rgb_height; // doesn't divide evenly, is this okay?
const size_t debayer_local_work_size = 128;
err = clEnqueueNDRangeKernel(q, s->krnl_debayer_rear, 1, NULL,
&debayer_work_size, &debayer_local_work_size, 0, 0, &debayer_event);
#endif
assert(err == 0);
} else {
assert(s->rgb_buf_size >= s->frame_size);
@ -405,7 +613,6 @@ void* processing_thread(void *arg) {
0, 0, s->rgb_buf_size, 0, 0, &debayer_event);
assert(err == 0);
}
clWaitForEvents(1, &debayer_event);
clReleaseEvent(debayer_event);
@ -413,6 +620,12 @@ void* processing_thread(void *arg) {
visionbuf_sync(&s->rgb_bufs[rgb_idx], VISIONBUF_SYNC_FROM_DEVICE);
#ifdef NOSCREEN
if (frame_data.frame_id % 4 == 1) {
sendrgb(&s->cameras, (uint8_t*) s->rgb_bufs[rgb_idx].addr, s->rgb_bufs[rgb_idx].len, 0);
}
#endif
#if defined(QCOM) && !defined(QCOM_REPLAY)
/*FILE *dump_rgb_file = fopen("/tmp/process_dump.rgb", "wb");
fwrite(s->rgb_bufs[rgb_idx].addr, s->rgb_bufs[rgb_idx].len, sizeof(uint8_t), dump_rgb_file);
@ -638,10 +851,17 @@ void* processing_thread(void *arg) {
tbuffer_dispatch(&s->ui_tb, ui_idx);
// auto exposure over big box
#ifdef QCOM2
const int exposure_x = 240;
const int exposure_y = 300;
const int exposure_height = 600;
const int exposure_width = 1440;
#else
const int exposure_x = 290;
const int exposure_y = 282 + 40;
const int exposure_y = 322;
const int exposure_height = 314;
const int exposure_width = 560;
#endif
if (cnt % 3 == 0) {
// find median box luminance for AE
uint32_t lum_binning[256] = {0,};
@ -661,8 +881,6 @@ void* processing_thread(void *arg) {
break;
}
}
// double avg = (double)acc / (big_box_width * big_box_height) - 16;
// printf("avg %d\n", lum_med);
camera_autoexposure(&s->cameras.rear, lum_med / 256.0);
}
@ -769,6 +987,20 @@ void* visionserver_client_thread(void* arg) {
} else {
assert(false);
}
} else if (stream_type == VISION_STREAM_RGB_WIDE) {
stream_bufs->width = s->rgb_wide_width;
stream_bufs->height = s->rgb_wide_height;
stream_bufs->stride = s->rgb_wide_stride;
stream_bufs->buf_len = s->rgb_wide_bufs[0].len;
rep.num_fds = UI_BUF_COUNT;
for (int i=0; i<rep.num_fds; i++) {
rep.fds[i] = s->rgb_wide_bufs[i].fd;
}
if (stream->tb) {
stream->tbuffer = &s->ui_wide_tb;
} else {
assert(false);
}
} else if (stream_type == VISION_STREAM_YUV) {
stream_bufs->width = s->yuv_width;
stream_bufs->height = s->yuv_height;
@ -797,6 +1029,21 @@ void* visionserver_client_thread(void* arg) {
} else {
stream->queue = pool_get_queue(&s->yuv_front_pool);
}
} else if (stream_type == VISION_STREAM_YUV_WIDE) {
stream_bufs->width = s->yuv_wide_width;
stream_bufs->height = s->yuv_wide_height;
stream_bufs->stride = s->yuv_wide_width;
stream_bufs->buf_len = s->yuv_wide_buf_size;
rep.num_fds = YUV_COUNT;
for (int i=0; i<rep.num_fds; i++) {
rep.fds[i] = s->yuv_wide_ion[i].fd;
}
if (stream->tb) {
stream->tbuffer = s->yuv_wide_tb;
} else {
stream->queue = pool_get_queue(&s->yuv_wide_pool);
}
} else {
assert(false);
}
@ -849,6 +1096,9 @@ void* visionserver_client_thread(void* arg) {
} else if (stream_i == VISION_STREAM_YUV_FRONT) {
rep.d.stream_acq.extra.frame_id = s->yuv_front_metas[idx].frame_id;
rep.d.stream_acq.extra.timestamp_eof = s->yuv_front_metas[idx].timestamp_eof;
} else if (stream_i == VISION_STREAM_YUV_WIDE) {
rep.d.stream_acq.extra.frame_id = s->yuv_wide_metas[idx].frame_id;
rep.d.stream_acq.extra.timestamp_eof = s->yuv_wide_metas[idx].timestamp_eof;
}
vipc_send(fd, &rep);
}
@ -959,25 +1209,28 @@ cl_program build_debayer_program(VisionState *s,
int frame_width, int frame_height, int frame_stride,
int rgb_width, int rgb_height, int rgb_stride,
int bayer_flip, int hdr) {
#ifdef QCOM2
assert(rgb_width == frame_width);
assert(rgb_height == frame_height);
#else
assert(rgb_width == frame_width/2);
assert(rgb_height == frame_height/2);
#ifdef QCOM2
int dnew = 1;
#else
int dnew = 0;
#endif
#endif
char args[4096];
snprintf(args, sizeof(args),
"-cl-fast-relaxed-math -cl-denorms-are-zero "
"-DFRAME_WIDTH=%d -DFRAME_HEIGHT=%d -DFRAME_STRIDE=%d "
"-DRGB_WIDTH=%d -DRGB_HEIGHT=%d -DRGB_STRIDE=%d "
"-DBAYER_FLIP=%d -DHDR=%d -DNEW=%d",
"-DBAYER_FLIP=%d -DHDR=%d",
frame_width, frame_height, frame_stride,
rgb_width, rgb_height, rgb_stride,
bayer_flip, hdr, dnew);
bayer_flip, hdr);
#ifdef QCOM2
return CLU_LOAD_FROM_FILE(s->context, s->device_id, "cameras/real_debayer.cl", args);
#else
return CLU_LOAD_FROM_FILE(s->context, s->device_id, "cameras/debayer.cl", args);
#endif
}
cl_program build_conv_program(VisionState *s,
@ -1030,7 +1283,7 @@ void init_buffers(VisionState *s) {
for (int i=0; i<FRAME_BUF_COUNT; i++) {
s->camera_bufs[i] = visionbuf_allocate_cl(s->frame_size, s->device_id, s->context,
&s->camera_bufs_cl[i]);
#ifndef QCOM2
#ifdef QCOM
// TODO: make lengths correct
s->focus_bufs[i] = visionbuf_allocate(0xb80);
s->stats_bufs[i] = visionbuf_allocate(0xb80);
@ -1042,11 +1295,22 @@ void init_buffers(VisionState *s) {
s->device_id, s->context,
&s->front_camera_bufs_cl[i]);
}
#ifdef QCOM2
for (int i=0; i<FRAME_BUF_COUNT; i++) {
s->wide_camera_bufs[i] = visionbuf_allocate_cl(s->cameras.wide.frame_size,
s->device_id, s->context,
&s->wide_camera_bufs_cl[i]);
}
#endif
// processing buffers
if (s->cameras.rear.ci.bayer) {
s->rgb_width = s->frame_width/2;
s->rgb_height = s->frame_height/2;
#ifdef QCOM2
s->rgb_width = s->frame_width;
s->rgb_height = s->frame_height;
#else
s->rgb_width = s->frame_width / 2;
s->rgb_height = s->frame_height / 2;
#endif
} else {
s->rgb_width = s->frame_width;
s->rgb_height = s->frame_height;
@ -1062,15 +1326,22 @@ void init_buffers(VisionState *s) {
}
tbuffer_init(&s->ui_tb, UI_BUF_COUNT, "rgb");
//assert(s->cameras.front.ci.bayer);
if (s->cameras.front.ci.bayer) {
s->rgb_front_width = s->cameras.front.ci.frame_width/2;
s->rgb_front_height = s->cameras.front.ci.frame_height/2;
#ifdef QCOM2
s->rgb_front_width = s->cameras.front.ci.frame_width;
s->rgb_front_height = s->cameras.front.ci.frame_height;
#else
s->rgb_front_width = s->cameras.front.ci.frame_width / 2;
s->rgb_front_height = s->cameras.front.ci.frame_height / 2;
#endif
} else {
s->rgb_front_width = s->cameras.front.ci.frame_width;
s->rgb_front_height = s->cameras.front.ci.frame_height;
}
#ifdef QCOM2
s->rgb_wide_width = s->cameras.wide.ci.frame_width;
s->rgb_wide_height = s->cameras.wide.ci.frame_height;
#endif
for (int i=0; i<UI_BUF_COUNT; i++) {
VisionImg img = visionimg_alloc_rgb24(s->rgb_front_width, s->rgb_front_height, &s->rgb_front_bufs[i]);
@ -1081,6 +1352,17 @@ void init_buffers(VisionState *s) {
}
}
tbuffer_init(&s->ui_front_tb, UI_BUF_COUNT, "frontrgb");
#ifdef QCOM2
for (int i=0; i<UI_BUF_COUNT; i++) {
VisionImg img = visionimg_alloc_rgb24(s->rgb_wide_width, s->rgb_wide_height, &s->rgb_wide_bufs[i]);
s->rgb_wide_bufs_cl[i] = visionbuf_to_cl(&s->rgb_wide_bufs[i], s->device_id, s->context);
if (i == 0){
s->rgb_wide_stride = img.stride;
s->rgb_wide_buf_size = img.size;
}
}
tbuffer_init(&s->ui_wide_tb, UI_BUF_COUNT, "widergb");
#endif
// yuv back for recording and orbd
pool_init(&s->yuv_pool, YUV_COUNT);
@ -1112,6 +1394,23 @@ void init_buffers(VisionState *s) {
s->yuv_front_bufs[i].v = s->yuv_front_bufs[i].u + (s->yuv_front_width/2 * s->yuv_front_height/2);
}
// yuv wide for recording
#ifdef QCOM2
pool_init(&s->yuv_wide_pool, YUV_COUNT);
s->yuv_wide_tb = pool_get_tbuffer(&s->yuv_wide_pool);
s->yuv_wide_width = s->rgb_wide_width;
s->yuv_wide_height = s->rgb_wide_height;
s->yuv_wide_buf_size = s->rgb_wide_width * s->rgb_wide_height * 3 / 2;
for (int i=0; i<YUV_COUNT; i++) {
s->yuv_wide_ion[i] = visionbuf_allocate_cl(s->yuv_wide_buf_size, s->device_id, s->context, &s->yuv_wide_cl[i]);
s->yuv_wide_bufs[i].y = (uint8_t*)s->yuv_wide_ion[i].addr;
s->yuv_wide_bufs[i].u = s->yuv_wide_bufs[i].y + (s->yuv_wide_width * s->yuv_wide_height);
s->yuv_wide_bufs[i].v = s->yuv_wide_bufs[i].u + (s->yuv_wide_width/2 * s->yuv_wide_height/2);
}
#endif
if (s->cameras.rear.ci.bayer) {
// debayering does a 2x downscale
s->yuv_transform = transform_scale_buffer(s->cameras.rear.transform, 0.5);
@ -1137,7 +1436,24 @@ void init_buffers(VisionState *s) {
s->krnl_debayer_front = clCreateKernel(s->prg_debayer_front, "debayer10", &err);
assert(err == 0);
}
#ifdef QCOM2
if (s->cameras.wide.ci.bayer) {
s->prg_debayer_wide = build_debayer_program(s, s->cameras.wide.ci.frame_width, s->cameras.wide.ci.frame_height,
s->cameras.wide.ci.frame_stride,
s->rgb_wide_width, s->rgb_wide_height, s->rgb_wide_stride,
s->cameras.wide.ci.bayer_flip, s->cameras.wide.ci.hdr);
s->krnl_debayer_wide = clCreateKernel(s->prg_debayer_wide, "debayer10", &err);
assert(err == 0);
}
#endif
s->debayer_cl_localMemSize = (DEBAYER_LOCAL_WORKSIZE + 2 * (3 / 2)) * (DEBAYER_LOCAL_WORKSIZE + 2 * (3 / 2)) * sizeof(float);
s->debayer_cl_globalWorkSize[0] = s->rgb_width;
s->debayer_cl_globalWorkSize[1] = s->rgb_height;
s->debayer_cl_localWorkSize[0] = DEBAYER_LOCAL_WORKSIZE;
s->debayer_cl_localWorkSize[1] = DEBAYER_LOCAL_WORKSIZE;
#ifdef QCOM
s->prg_rgb_laplacian = build_conv_program(s, s->rgb_width/NUM_SEGMENTS_X, s->rgb_height/NUM_SEGMENTS_Y,
3);
s->krnl_rgb_laplacian = clCreateKernel(s->prg_rgb_laplacian, "rgb2gray_conv2d", &err);
@ -1157,9 +1473,13 @@ void init_buffers(VisionState *s) {
s->conv_cl_localWorkSize[1] = CONV_LOCAL_WORKSIZE;
for (int i=0; i<(ROI_X_MAX-ROI_X_MIN+1)*(ROI_Y_MAX-ROI_Y_MIN+1); i++) {s->lapres[i] = 16160;}
#endif
rgb_to_yuv_init(&s->rgb_to_yuv_state, s->context, s->device_id, s->yuv_width, s->yuv_height, s->rgb_stride);
rgb_to_yuv_init(&s->front_rgb_to_yuv_state, s->context, s->device_id, s->yuv_front_width, s->yuv_front_height, s->rgb_front_stride);
#ifdef QCOM2
rgb_to_yuv_init(&s->wide_rgb_to_yuv_state, s->context, s->device_id, s->yuv_wide_width, s->yuv_wide_height, s->rgb_wide_stride);
#endif
}
void free_buffers(VisionState *s) {
@ -1167,18 +1487,28 @@ void free_buffers(VisionState *s) {
for (int i=0; i<FRAME_BUF_COUNT; i++) {
visionbuf_free(&s->camera_bufs[i]);
visionbuf_free(&s->front_camera_bufs[i]);
#ifdef QCOM
visionbuf_free(&s->focus_bufs[i]);
visionbuf_free(&s->stats_bufs[i]);
#elif defined(QCOM2)
visionbuf_free(&s->wide_camera_bufs[i]);
#endif
}
for (int i=0; i<UI_BUF_COUNT; i++) {
visionbuf_free(&s->rgb_bufs[i]);
visionbuf_free(&s->rgb_front_bufs[i]);
#ifdef QCOM2
visionbuf_free(&s->rgb_wide_bufs[i]);
#endif
}
for (int i=0; i<YUV_COUNT; i++) {
visionbuf_free(&s->yuv_ion[i]);
visionbuf_free(&s->yuv_front_ion[i]);
#ifdef QCOM2
visionbuf_free(&s->yuv_wide_ion[i]);
#endif
}
clReleaseMemObject(s->rgb_conv_roi_cl);
@ -1189,7 +1519,11 @@ void free_buffers(VisionState *s) {
clReleaseProgram(s->prg_debayer_front);
clReleaseKernel(s->krnl_debayer_rear);
clReleaseKernel(s->krnl_debayer_front);
#ifdef QCOM2
clReleaseProgram(s->prg_debayer_wide);
clReleaseKernel(s->krnl_debayer_wide);
#endif
clReleaseProgram(s->prg_rgb_laplacian);
clReleaseKernel(s->krnl_rgb_laplacian);
@ -1211,13 +1545,18 @@ void party(VisionState *s) {
processing_thread, s);
assert(err == 0);
#if !defined(QCOM2) && !defined(__APPLE__)
// TODO: fix front camera on qcom2
#ifndef __APPLE__
pthread_t frontview_thread_handle;
err = pthread_create(&frontview_thread_handle, NULL,
frontview_thread, s);
assert(err == 0);
#endif
#ifdef QCOM2
pthread_t wideview_thread_handle;
err = pthread_create(&wideview_thread_handle, NULL,
wideview_thread, s);
assert(err == 0);
#endif
// priority for cameras
err = set_realtime_priority(51);
@ -1229,15 +1568,23 @@ void party(VisionState *s) {
tbuffer_stop(&s->ui_front_tb);
pool_stop(&s->yuv_pool);
pool_stop(&s->yuv_front_pool);
#ifdef QCOM2
tbuffer_stop(&s->ui_wide_tb);
pool_stop(&s->yuv_wide_pool);
#endif
zsock_signal(s->terminate_pub, 0);
#if (defined(QCOM) && !defined(QCOM_REPLAY)) || defined(WEBCAM)
#if (defined(QCOM) && !defined(QCOM_REPLAY)) || defined(WEBCAM) || defined(QCOM2)
LOG("joining frontview_thread");
err = pthread_join(frontview_thread_handle, NULL);
assert(err == 0);
#endif
#ifdef QCOM2
LOG("joining wideview_thread");
err = pthread_join(wideview_thread_handle, NULL);
assert(err == 0);
#endif
LOG("joining visionserver_thread");
err = pthread_join(visionserver_thread_handle, NULL);
assert(err == 0);
@ -1271,11 +1618,17 @@ int main(int argc, char *argv[]) {
init_buffers(s);
#if (defined(QCOM) && !defined(QCOM_REPLAY)) || defined(QCOM2)
#if defined(QCOM) && !defined(QCOM_REPLAY)
s->pm = new PubMaster({"frame", "frontFrame", "thumbnail"});
#elif defined(QCOM2)
s->pm = new PubMaster({"frame", "frontFrame", "wideFrame", "thumbnail"});
#endif
#ifndef QCOM2
cameras_open(&s->cameras, &s->camera_bufs[0], &s->focus_bufs[0], &s->stats_bufs[0], &s->front_camera_bufs[0]);
#else
cameras_open(&s->cameras, &s->camera_bufs[0], &s->focus_bufs[0], &s->stats_bufs[0], &s->front_camera_bufs[0], &s->wide_camera_bufs[0]);
#endif
party(s);

3
selfdrive/camerad/test/camera/test.c
View File

@ -3,6 +3,7 @@
#include <stdbool.h>
#include "camera_qcom.h"
// TODO: add qcom2 test
bool do_exit = false;
@ -34,7 +35,7 @@ void tbuffer_stop(TBuffer *tb) {
}
int main() {
DualCameraState s={};
MultiCameraState s={};
cameras_init(&s);
VisionBuf camera_bufs_rear[0x10] = {0};
VisionBuf camera_bufs_focus[0x10] = {0};

47
selfdrive/camerad/test/tici_zclient/livergb.py 100755
View File

@ -0,0 +1,47 @@
# flake8: noqa
# pylint: disable=W
#!/usr/bin/env python
import numpy as np
import cv2
from time import time, sleep
H, W = (604*2//6, 964*2//6)
# H, W = (604, 964)
cam_bufs = np.zeros((3,H,W,3), dtype=np.uint8)
hist_bufs = np.zeros((3,H,200,3), dtype=np.uint8)
if __name__ == '__main__':
import zmq
context = zmq.Context()
socket = context.socket(zmq.PULL)
socket.bind("tcp://192.168.3.4:7768")
while True:
try:
message = socket.recv()
except Exception as ex:
print(ex)
message = b"123"
dat = np.frombuffer(message, dtype=np.uint8)
cam_id = (dat[0] + 1) % 3
# import pdb; pdb.set_trace()
b = dat[::3].reshape(H, W)
g = dat[1::3].reshape(H, W)
r = dat[2::3].reshape(H, W)
cam_bufs[cam_id] = cv2.merge((r, g, b))
cam_bufs[cam_id]= cv2.cvtColor(cam_bufs[cam_id], cv2.COLOR_RGB2BGR)
hist = cv2.calcHist([cv2.cvtColor(cam_bufs[cam_id], cv2.COLOR_BGR2GRAY)],[0],None,[32],[0,256])
hist = (H*hist/hist.max()).astype(np.uint8)
for i,bb in enumerate(hist):
hist_bufs[cam_id, H-bb[0]:,i*(200//32):(i+1)*(200//32), :] = (222,222,222)
out = cam_bufs.reshape((3*H,W,3))
hist_bufs_out = hist_bufs.reshape((3*H,200,3))
out = np.hstack([out, hist_bufs_out])
cv2.imshow('RGB', out)
cv2.waitKey(55)
#dat.tofile('/tmp/c3rgb.img')
#cv2.imwrite('/tmp/c3rgb.png', out)

34
selfdrive/camerad/test/tici_zclient/liveyuv.py 100755
View File

@ -0,0 +1,34 @@
# flake8: noqa
# pylint: disable=W
#!/usr/bin/env python
import numpy as np
import cv2
from time import time, sleep
H, W = (256, 512)
if __name__ == '__main__':
import zmq
context = zmq.Context()
socket = context.socket(zmq.PULL)
socket.bind("tcp://192.168.3.4:7769")
while True:
try:
message = socket.recv()
except Exception as ex:
print(ex)
message = b"123"
dat = np.frombuffer(message, dtype=np.float32)
mc = (dat.reshape(H//2, W//2)).astype(np.uint8)
hist = cv2.calcHist([mc],[0],None,[32],[0,256])
hist = (H*hist/hist.max()).astype(np.uint8)
himg = np.zeros((H//2, W//2), dtype=np.uint8)
for i,b in enumerate(hist):
himg[H//2-b[0]:,i*(W//2//32):(i+1)*(W//2//32)] = 222
cv2.imshow('model fov', np.hstack([mc, himg]))
cv2.waitKey(20)
dat.tofile('/tmp/c3yuv.img')

4
selfdrive/common/visionipc.h
View File

@ -37,6 +37,10 @@ typedef struct VisionUIInfo {
int front_box_x, front_box_y;
int front_box_width, front_box_height;
int wide_box_x, wide_box_y;
int wide_box_width, wide_box_height;
} VisionUIInfo;
typedef struct VisionStreamBufs {

2
selfdrive/loggerd/uploader.py
View File

@ -102,7 +102,7 @@ class Uploader():
self.last_exc = None
self.immediate_priority = {"qlog.bz2": 0, "qcamera.ts": 1}
self.high_priority = {"rlog.bz2": 0, "fcamera.hevc": 1, "dcamera.hevc": 2}
self.high_priority = {"rlog.bz2": 0, "fcamera.hevc": 1, "dcamera.hevc": 2, "ecamera.hevc": 3}
def get_upload_sort(self, name):
if name in self.immediate_priority:

8
selfdrive/modeld/modeld.cc
View File

@ -37,11 +37,19 @@ void* live_thread(void *arg) {
-1.09890110e-03, 0.00000000e+00, 2.81318681e-01,
-1.84808520e-20, 9.00738606e-04,-4.28751576e-02;
#ifndef QCOM2
Eigen::Matrix<float, 3, 3> eon_intrinsics;
eon_intrinsics <<
910.0, 0.0, 582.0,
0.0, 910.0, 437.0,
0.0, 0.0, 1.0;
#else
Eigen::Matrix<float, 3, 3> eon_intrinsics;
eon_intrinsics <<
2648.0/2, 0.0, 1928.0/2,
0.0, 2648.0/2, 1208.0/2,
0.0, 0.0, 1.0;
#endif
// debayering does a 2x downscale
mat3 yuv_transform = transform_scale_buffer((mat3){{

4
selfdrive/modeld/runners/snpemodel.cc
View File

@ -13,7 +13,7 @@ void PrintErrorStringAndExit() {
SNPEModel::SNPEModel(const char *path, float *loutput, size_t loutput_size, int runtime) {
output = loutput;
output_size = loutput_size;
#ifdef QCOM
#if defined(QCOM) || defined(QCOM2)
if (runtime==USE_GPU_RUNTIME) {
Runtime = zdl::DlSystem::Runtime_t::GPU;
} else if (runtime==USE_DSP_RUNTIME) {
@ -35,7 +35,7 @@ SNPEModel::SNPEModel(const char *path, float *loutput, size_t loutput_size, int
// create model runner
zdl::SNPE::SNPEBuilder snpeBuilder(container.get());
while (!snpe) {
#ifdef QCOM
#if defined(QCOM) || defined(QCOM2)
snpe = snpeBuilder.setOutputLayers({})
.setRuntimeProcessor(Runtime)
.setUseUserSuppliedBuffers(true)

2
selfdrive/modeld/runners/snpemodel.h
View File

@ -38,7 +38,7 @@ private:
Thneed *thneed = NULL;
#endif
#ifdef QCOM
#if defined(QCOM) || defined(QCOM2)
zdl::DlSystem::Runtime_t Runtime;
#endif