From 37d6472bfa1c0ef9f1f8353162b97444ca66e45c Mon Sep 17 00:00:00 2001 From: robbederks Date: Thu, 27 Aug 2020 04:59:56 +0200 Subject: [PATCH] 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 9233a1df7cac214fae6827cdae3a10cb3bd060d9. * not care * use consistent hdr * some cleanup * Update selfdrive/camerad/main.cc Co-authored-by: Adeeb Shihadeh * more cleanups * remove irrelevant stuff * this too * cleanup * rerun ci Co-authored-by: ZwX1616 Co-authored-by: Tici Co-authored-by: Adeeb Shihadeh --- selfdrive/camerad/SConscript | 4 + .../camerad/cameras/camera_frame_stream.cc | 10 +- .../camerad/cameras/camera_frame_stream.h | 12 +- selfdrive/camerad/cameras/camera_qcom.cc | 12 +- selfdrive/camerad/cameras/camera_qcom.h | 12 +- selfdrive/camerad/cameras/camera_qcom2.c | 301 +++++--- selfdrive/camerad/cameras/camera_qcom2.h | 32 +- selfdrive/camerad/cameras/camera_webcam.cc | 8 +- selfdrive/camerad/cameras/camera_webcam.h | 12 +- selfdrive/camerad/cameras/debayer.cl | 14 - selfdrive/camerad/cameras/real_debayer.cl | 128 ++++ selfdrive/camerad/cameras/sensor2_i2c.h | 649 ++++++++++-------- selfdrive/camerad/main.cc | 413 ++++++++++- selfdrive/camerad/test/camera/test.c | 3 +- .../camerad/test/tici_zclient/livergb.py | 47 ++ .../camerad/test/tici_zclient/liveyuv.py | 34 + selfdrive/common/visionipc.h | 4 + selfdrive/loggerd/uploader.py | 2 +- selfdrive/modeld/modeld.cc | 8 + selfdrive/modeld/runners/snpemodel.cc | 4 +- selfdrive/modeld/runners/snpemodel.h | 2 +- 21 files changed, 1244 insertions(+), 467 deletions(-) create mode 100644 selfdrive/camerad/cameras/real_debayer.cl create mode 100755 selfdrive/camerad/test/tici_zclient/livergb.py create mode 100755 selfdrive/camerad/test/tici_zclient/liveyuv.py diff --git a/selfdrive/camerad/SConscript b/selfdrive/camerad/SConscript index 1b205198c6..18754ca811 100644 --- a/selfdrive/camerad/SConscript +++ b/selfdrive/camerad/SConscript @@ -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'] diff --git a/selfdrive/camerad/cameras/camera_frame_stream.cc b/selfdrive/camerad/cameras/camera_frame_stream.cc index aecca564a2..9a4263103d 100644 --- a/selfdrive/camerad/cameras/camera_frame_stream.cc +++ b/selfdrive/camerad/cameras/camera_frame_stream.cc @@ -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); diff --git a/selfdrive/camerad/cameras/camera_frame_stream.h b/selfdrive/camerad/cameras/camera_frame_stream.h index 80ff54b5e3..a29e403040 100644 --- a/selfdrive/camerad/cameras/camera_frame_stream.h +++ b/selfdrive/camerad/cameras/camera_frame_stream.h @@ -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" diff --git a/selfdrive/camerad/cameras/camera_qcom.cc b/selfdrive/camerad/cameras/camera_qcom.cc index dfed65238c..4e8958818c 100644 --- a/selfdrive/camerad/cameras/camera_qcom.cc +++ b/selfdrive/camerad/cameras/camera_qcom.cc @@ -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); } diff --git a/selfdrive/camerad/cameras/camera_qcom.h b/selfdrive/camerad/cameras/camera_qcom.h index 812f1a5968..e2456c253a 100644 --- a/selfdrive/camerad/cameras/camera_qcom.h +++ b/selfdrive/camerad/cameras/camera_qcom.h @@ -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); diff --git a/selfdrive/camerad/cameras/camera_qcom2.c b/selfdrive/camerad/cameras/camera_qcom2.c index 259521ed98..a417cf75bf 100644 --- a/selfdrive/camerad/cameras/camera_qcom2.c +++ b/selfdrive/camerad/cameras/camera_qcom2.c @@ -1,4 +1,4 @@ -#include + #include #include #include @@ -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; - ret = ioctl(s->video0_fd, VIDIOC_SUBSCRIBE_EVENT, &sub); - LOGD("isp subscribe: %d", ret); - sub.id = 1; + sub.id = 2; // should use boot time for sof 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 (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; - } - } - } + if (!fds[0].revents) continue; + + 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;rrgb_sock), &resized_dat, new_width*new_height*3, 0); + err2 = zmq_errno(); + //printf("zmq errcode %d, %d\n",err,err2); +} +#endif diff --git a/selfdrive/camerad/cameras/camera_qcom2.h b/selfdrive/camerad/cameras/camera_qcom2.h index b15ea8b128..b7e81e4943 100644 --- a/selfdrive/camerad/cameras/camera_qcom2.h +++ b/selfdrive/camerad/cameras/camera_qcom2.h @@ -4,6 +4,7 @@ #include #include #include +#include #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" diff --git a/selfdrive/camerad/cameras/camera_webcam.cc b/selfdrive/camerad/cameras/camera_webcam.cc index 4a7c6626a6..972d9643f4 100644 --- a/selfdrive/camerad/cameras/camera_webcam.cc +++ b/selfdrive/camerad/cameras/camera_webcam.cc @@ -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; diff --git a/selfdrive/camerad/cameras/camera_webcam.h b/selfdrive/camerad/cameras/camera_webcam.h index 28c3e65209..1a18fc9376 100644 --- a/selfdrive/camerad/cameras/camera_webcam.h +++ b/selfdrive/camerad/cameras/camera_webcam.h @@ -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" diff --git a/selfdrive/camerad/cameras/debayer.cl b/selfdrive/camerad/cameras/debayer.cl index c9e3716dfd..5188dc88c1 100644 --- a/selfdrive/camerad/cameras/debayer.cl +++ b/selfdrive/camerad/cameras/debayer.cl @@ -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 diff --git a/selfdrive/camerad/cameras/real_debayer.cl b/selfdrive/camerad/cameras/real_debayer.cl new file mode 100644 index 0000000000..eaea1ed12f --- /dev/null +++ b/selfdrive/camerad/cameras/real_debayer.cl @@ -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); + } +} diff --git a/selfdrive/camerad/cameras/sensor2_i2c.h b/selfdrive/camerad/cameras/sensor2_i2c.h index 7174bb897a..070ed43677 100644 --- a/selfdrive/camerad/cameras/sensor2_i2c.h +++ b/selfdrive/camerad/cameras/sensor2_i2c.h @@ -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[] = { diff --git a/selfdrive/camerad/main.cc b/selfdrive/camerad/main.cc index 8038b82322..658204a751 100644 --- a/selfdrive/camerad/main.cc +++ b/selfdrive/camerad/main.cc @@ -33,6 +33,7 @@ #include #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(); + 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; yyuv_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; irgb_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; iyuv_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; icamera_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; iwide_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; irgb_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; irgb_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; iyuv_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; icamera_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; irgb_bufs[i]); visionbuf_free(&s->rgb_front_bufs[i]); +#ifdef QCOM2 + visionbuf_free(&s->rgb_wide_bufs[i]); +#endif } for (int i=0; iyuv_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); diff --git a/selfdrive/camerad/test/camera/test.c b/selfdrive/camerad/test/camera/test.c index 2a7272cb9f..4ff6a07526 100644 --- a/selfdrive/camerad/test/camera/test.c +++ b/selfdrive/camerad/test/camera/test.c @@ -3,6 +3,7 @@ #include #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}; diff --git a/selfdrive/camerad/test/tici_zclient/livergb.py b/selfdrive/camerad/test/tici_zclient/livergb.py new file mode 100755 index 0000000000..13ed0b5e94 --- /dev/null +++ b/selfdrive/camerad/test/tici_zclient/livergb.py @@ -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) diff --git a/selfdrive/camerad/test/tici_zclient/liveyuv.py b/selfdrive/camerad/test/tici_zclient/liveyuv.py new file mode 100755 index 0000000000..4f24275643 --- /dev/null +++ b/selfdrive/camerad/test/tici_zclient/liveyuv.py @@ -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') diff --git a/selfdrive/common/visionipc.h b/selfdrive/common/visionipc.h index 63bedc38fe..1b216f5255 100644 --- a/selfdrive/common/visionipc.h +++ b/selfdrive/common/visionipc.h @@ -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 { diff --git a/selfdrive/loggerd/uploader.py b/selfdrive/loggerd/uploader.py index 453fe90ca1..7e8e94a4c0 100644 --- a/selfdrive/loggerd/uploader.py +++ b/selfdrive/loggerd/uploader.py @@ -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: diff --git a/selfdrive/modeld/modeld.cc b/selfdrive/modeld/modeld.cc index be47d1811a..747388a796 100644 --- a/selfdrive/modeld/modeld.cc +++ b/selfdrive/modeld/modeld.cc @@ -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 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 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){{ diff --git a/selfdrive/modeld/runners/snpemodel.cc b/selfdrive/modeld/runners/snpemodel.cc index 5a92d94bf4..38da13453a 100644 --- a/selfdrive/modeld/runners/snpemodel.cc +++ b/selfdrive/modeld/runners/snpemodel.cc @@ -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) diff --git a/selfdrive/modeld/runners/snpemodel.h b/selfdrive/modeld/runners/snpemodel.h index f7f217cdc6..90c26664f6 100644 --- a/selfdrive/modeld/runners/snpemodel.h +++ b/selfdrive/modeld/runners/snpemodel.h @@ -38,7 +38,7 @@ private: Thneed *thneed = NULL; #endif -#ifdef QCOM +#if defined(QCOM) || defined(QCOM2) zdl::DlSystem::Runtime_t Runtime; #endif