From e19baf2d3a798e90892628d57baadb20d7c9840c Mon Sep 17 00:00:00 2001 From: George Hotz <72895+geohot@users.noreply.github.com> Date: Fri, 18 Mar 2022 15:21:51 -0700 Subject: [PATCH] camerad: imx390 support (#23966) * something is output * min stuff * visible picture * pics look good * maybe * whole sensor * fix all cameras * support both cameras * autoexposure for imx390 * fix imx390 blacklevel * touchups * put gain in db scale * inline and fix max exposure Co-authored-by: Comma Device --- selfdrive/camerad/cameras/camera_common.cc | 9 +- selfdrive/camerad/cameras/camera_common.h | 3 +- selfdrive/camerad/cameras/camera_qcom2.cc | 105 +++++++++++++++------ selfdrive/camerad/cameras/camera_qcom2.h | 6 +- selfdrive/camerad/cameras/real_debayer.cl | 23 +++-- selfdrive/camerad/cameras/sensor2_i2c.h | 48 +++++++++- 6 files changed, 146 insertions(+), 48 deletions(-) diff --git a/selfdrive/camerad/cameras/camera_common.cc b/selfdrive/camerad/cameras/camera_common.cc index 20edab14eaf7ef..0570364ec30805 100644 --- a/selfdrive/camerad/cameras/camera_common.cc +++ b/selfdrive/camerad/cameras/camera_common.cc @@ -52,7 +52,7 @@ class Debayer { CL_CHECK(clReleaseProgram(prg_debayer)); } - void queue(cl_command_queue q, cl_mem cam_buf_cl, cl_mem buf_cl, int width, int height, float gain, cl_event *debayer_event) { + void queue(cl_command_queue q, cl_mem cam_buf_cl, cl_mem buf_cl, int width, int height, float gain, float black_level, cl_event *debayer_event) { CL_CHECK(clSetKernelArg(krnl_, 0, sizeof(cl_mem), &cam_buf_cl)); CL_CHECK(clSetKernelArg(krnl_, 1, sizeof(cl_mem), &buf_cl)); @@ -62,6 +62,7 @@ class Debayer { const size_t globalWorkSize[] = {size_t(width), size_t(height)}; const size_t localWorkSize[] = {debayer_local_worksize, debayer_local_worksize}; CL_CHECK(clSetKernelArg(krnl_, 2, localMemSize, 0)); + CL_CHECK(clSetKernelArg(krnl_, 3, sizeof(float), &black_level)); CL_CHECK(clEnqueueNDRangeKernel(q, krnl_, 2, NULL, globalWorkSize, localWorkSize, 0, 0, debayer_event)); } else { if (hdr_) { @@ -165,13 +166,15 @@ bool CameraBuf::acquire() { if (debayer) { float gain = 0.0; - + float black_level = 42.0; #ifndef QCOM2 gain = camera_state->digital_gain; if ((int)gain == 0) gain = 1.0; +#else + if (camera_state->camera_id == CAMERA_ID_IMX390) black_level = 64.0; #endif - debayer->queue(q, camrabuf_cl, cur_rgb_buf->buf_cl, rgb_width, rgb_height, gain, &event); + debayer->queue(q, camrabuf_cl, cur_rgb_buf->buf_cl, rgb_width, rgb_height, gain, black_level, &event); } else { assert(rgb_stride == camera_state->ci.frame_stride); CL_CHECK(clEnqueueCopyBuffer(q, camrabuf_cl, cur_rgb_buf->buf_cl, 0, 0, cur_rgb_buf->len, 0, 0, &event)); diff --git a/selfdrive/camerad/cameras/camera_common.h b/selfdrive/camerad/cameras/camera_common.h index d3cdc67b9165a7..7aa1891aac71d5 100644 --- a/selfdrive/camerad/cameras/camera_common.h +++ b/selfdrive/camerad/cameras/camera_common.h @@ -25,7 +25,8 @@ #define CAMERA_ID_LGC920 6 #define CAMERA_ID_LGC615 7 #define CAMERA_ID_AR0231 8 -#define CAMERA_ID_MAX 9 +#define CAMERA_ID_IMX390 9 +#define CAMERA_ID_MAX 10 const int UI_BUF_COUNT = 4; const int YUV_BUFFER_COUNT = Hardware::EON() ? 100 : 40; diff --git a/selfdrive/camerad/cameras/camera_qcom2.cc b/selfdrive/camerad/cameras/camera_qcom2.cc index 2e810e42021d36..8b27b94d0f200c 100644 --- a/selfdrive/camerad/cameras/camera_qcom2.cc +++ b/selfdrive/camerad/cameras/camera_qcom2.cc @@ -22,6 +22,9 @@ #include "selfdrive/common/swaglog.h" #include "selfdrive/camerad/cameras/sensor2_i2c.h" +// For debugging: +// echo "4294967295" > /sys/module/cam_debug_util/parameters/debug_mdl + extern ExitHandler do_exit; const size_t FRAME_WIDTH = 1928; @@ -39,6 +42,14 @@ CameraInfo cameras_supported[CAMERA_ID_MAX] = { .bayer_flip = 1, .hdr = false }, + [CAMERA_ID_IMX390] = { + .frame_width = FRAME_WIDTH, + .frame_height = FRAME_HEIGHT, + .frame_stride = FRAME_STRIDE, + .bayer = true, + .bayer_flip = 1, + .hdr = false + }, }; const float DC_GAIN = 2.5; @@ -160,8 +171,15 @@ void clear_req_queue(int fd, int32_t session_hdl, int32_t link_hdl) { // ************** high level camera helpers **************** void CameraState::sensors_start() { - int start_reg_len = sizeof(start_reg_array) / sizeof(struct i2c_random_wr_payload); - sensors_i2c(start_reg_array, start_reg_len, CAM_SENSOR_PACKET_OPCODE_SENSOR_CONFIG); + if (camera_id == CAMERA_ID_AR0231) { + int start_reg_len = sizeof(start_reg_array_ar0231) / sizeof(struct i2c_random_wr_payload); + sensors_i2c(start_reg_array_ar0231, start_reg_len, CAM_SENSOR_PACKET_OPCODE_SENSOR_CONFIG, true); + } else if (camera_id == CAMERA_ID_IMX390) { + int start_reg_len = sizeof(start_reg_array_imx390) / sizeof(struct i2c_random_wr_payload); + sensors_i2c(start_reg_array_imx390, start_reg_len, CAM_SENSOR_PACKET_OPCODE_SENSOR_CONFIG, false); + } else { + assert(false); + } } void CameraState::sensors_poke(int request_id) { @@ -181,7 +199,7 @@ void CameraState::sensors_poke(int request_id) { release_fd(multi_cam_state->video0_fd, cam_packet_handle); } -void CameraState::sensors_i2c(struct i2c_random_wr_payload* dat, int len, int op_code) { +void CameraState::sensors_i2c(struct i2c_random_wr_payload* dat, int len, int op_code, bool data_word) { // LOGD("sensors_i2c: %d", len); uint32_t cam_packet_handle = 0; int size = sizeof(struct cam_packet)+sizeof(struct cam_cmd_buf_desc)*1; @@ -199,7 +217,7 @@ void CameraState::sensors_i2c(struct i2c_random_wr_payload* dat, int len, int op i2c_random_wr->header.count = len; i2c_random_wr->header.op_code = 1; i2c_random_wr->header.cmd_type = CAMERA_SENSOR_CMD_TYPE_I2C_RNDM_WR; - i2c_random_wr->header.data_type = CAMERA_SENSOR_I2C_TYPE_WORD; + i2c_random_wr->header.data_type = data_word ? CAMERA_SENSOR_I2C_TYPE_WORD : CAMERA_SENSOR_I2C_TYPE_BYTE; 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)); @@ -220,7 +238,7 @@ static cam_cmd_power *power_set_wait(cam_cmd_power *power, int16_t delay_ms) { return (struct cam_cmd_power *)(unconditional_wait + 1); }; -void CameraState::sensors_init() { +int CameraState::sensors_init() { int video0_fd = multi_cam_state->video0_fd; uint32_t cam_packet_handle = 0; int size = sizeof(struct cam_packet)+sizeof(struct cam_cmd_buf_desc)*2; @@ -239,17 +257,17 @@ void CameraState::sensors_init() { switch (camera_num) { case 0: // port 0 - i2c_info->slave_addr = 0x20; + i2c_info->slave_addr = (camera_id == CAMERA_ID_AR0231) ? 0x20 : 0x34; probe->camera_id = 0; break; case 1: // port 1 - i2c_info->slave_addr = 0x30; + i2c_info->slave_addr = (camera_id == CAMERA_ID_AR0231) ? 0x30 : 0x36; probe->camera_id = 1; break; case 2: // port 2 - i2c_info->slave_addr = 0x20; + i2c_info->slave_addr = (camera_id == CAMERA_ID_AR0231) ? 0x20 : 0x34; probe->camera_id = 2; break; } @@ -263,8 +281,15 @@ void CameraState::sensors_init() { probe->addr_type = CAMERA_SENSOR_I2C_TYPE_WORD; probe->op_code = 3; // don't care? probe->cmd_type = CAMERA_SENSOR_CMD_TYPE_PROBE; - probe->reg_addr = 0x3000; //0x300a; //0x300b; - probe->expected_data = 0x354; //0x7750; //0x885a; + if (camera_id == CAMERA_ID_AR0231) { + probe->reg_addr = 0x3000; + probe->expected_data = 0x354; + } else if (camera_id == CAMERA_ID_IMX390) { + probe->reg_addr = 0x330; + probe->expected_data = 0x1538; + } else { + assert(false); + } probe->data_mask = 0; //buf_desc[1].size = buf_desc[1].length = 148; @@ -293,7 +318,7 @@ void CameraState::sensors_init() { power->count = 1; power->cmd_type = CAMERA_SENSOR_CMD_TYPE_PWR_UP; power->power_settings[0].power_seq_type = 0; - power->power_settings[0].config_val_low = 19200000; //Hz + power->power_settings[0].config_val_low = (camera_id == CAMERA_ID_AR0231) ? 19200000 : 24000000; //Hz power = power_set_wait(power, 10); // 8,1 is this reset? @@ -341,7 +366,6 @@ void CameraState::sensors_init() { LOGD("probing the sensor"); int ret = do_cam_control(sensor_fd, CAM_SENSOR_PROBE_CMD, (void *)(uintptr_t)cam_packet_handle, 0); - assert(ret == 0); munmap(i2c_info, buf_desc[0].size); release_fd(video0_fd, buf_desc[0].mem_handle); @@ -349,6 +373,8 @@ void CameraState::sensors_init() { release_fd(video0_fd, buf_desc[1].mem_handle); munmap(pkt, size); release_fd(video0_fd, cam_packet_handle); + + return ret; } void CameraState::config_isp(int io_mem_handle, int fence, int request_id, int buf0_mem_handle, int buf0_offset) { @@ -561,9 +587,10 @@ void CameraState::enqueue_req_multi(int start, int n, bool dp) { // ******************* camera ******************* -void CameraState::camera_init(MultiCameraState *multi_cam_state_, VisionIpcServer * v, int camera_id, int camera_num_, unsigned int fps, cl_device_id device_id, cl_context ctx, VisionStreamType rgb_type, VisionStreamType yuv_type) { +void CameraState::camera_init(MultiCameraState *multi_cam_state_, VisionIpcServer * v, int camera_id_, int camera_num_, unsigned int fps, cl_device_id device_id, cl_context ctx, VisionStreamType rgb_type, VisionStreamType yuv_type) { LOGD("camera init %d", camera_num); multi_cam_state = multi_cam_state_; + camera_id = camera_id_; assert(camera_id < std::size(cameras_supported)); ci = cameras_supported[camera_id]; assert(ci.frame_width != 0); @@ -586,17 +613,24 @@ void CameraState::camera_init(MultiCameraState *multi_cam_state_, VisionIpcServe } void CameraState::camera_open() { + int ret; sensor_fd = open_v4l_by_name_and_index("cam-sensor-driver", camera_num); assert(sensor_fd >= 0); LOGD("opened sensor for %d", camera_num); // probe the sensor LOGD("-- Probing sensor %d", camera_num); - sensors_init(); + ret = sensors_init(); + if (ret != 0) { + LOGD("AR0231 init failed, trying IMX390"); + camera_id = CAMERA_ID_IMX390; + ret = sensors_init(); + } + assert(ret == 0); // create session struct cam_req_mgr_session_info session_info = {}; - int ret = do_cam_control(multi_cam_state->video0_fd, CAM_REQ_MGR_CREATE_SESSION, &session_info, sizeof(session_info)); + ret = do_cam_control(multi_cam_state->video0_fd, CAM_REQ_MGR_CREATE_SESSION, &session_info, sizeof(session_info)); LOGD("get session: %d 0x%X", ret, session_info.session_hdl); session_handle = session_info.session_hdl; @@ -675,10 +709,13 @@ void CameraState::camera_open() { config_isp(0, 0, 1, buf0_handle, 0); LOG("-- Configuring sensor"); - sensors_i2c(init_array_ar0231, std::size(init_array_ar0231), CAM_SENSOR_PACKET_OPCODE_SENSOR_CONFIG); - //sensors_i2c(start_reg_array, std::size(start_reg_array), CAM_SENSOR_PACKET_OPCODE_SENSOR_STREAMON); - //sensors_i2c(stop_reg_array, std::size(stop_reg_array), CAM_SENSOR_PACKET_OPCODE_SENSOR_STREAMOFF); - + if (camera_id == CAMERA_ID_AR0231) { + sensors_i2c(init_array_ar0231, std::size(init_array_ar0231), CAM_SENSOR_PACKET_OPCODE_SENSOR_CONFIG, true); + } else if (camera_id == CAMERA_ID_IMX390) { + sensors_i2c(init_array_imx390, std::size(init_array_imx390), CAM_SENSOR_PACKET_OPCODE_SENSOR_CONFIG, false); + } else { + assert(false); + } // config csiphy LOG("-- Config CSI PHY"); @@ -1008,14 +1045,28 @@ void CameraState::set_camera_exposure(float grey_frac) { } // LOGE("ae - camera %d, cur_t %.5f, sof %.5f, dt %.5f", camera_num, 1e-9 * nanos_since_boot(), 1e-9 * buf.cur_frame_data.timestamp_sof, 1e-9 * (nanos_since_boot() - buf.cur_frame_data.timestamp_sof)); - uint16_t analog_gain_reg = 0xFF00 | (new_g << 4) | new_g; - struct i2c_random_wr_payload exp_reg_array[] = { - {0x3366, analog_gain_reg}, - {0x3362, (uint16_t)(dc_gain_enabled ? 0x1 : 0x0)}, - {0x3012, (uint16_t)exposure_time}, - }; - sensors_i2c(exp_reg_array, sizeof(exp_reg_array)/sizeof(struct i2c_random_wr_payload), - CAM_SENSOR_PACKET_OPCODE_SENSOR_CONFIG); + if (camera_id == CAMERA_ID_AR0231) { + uint16_t analog_gain_reg = 0xFF00 | (new_g << 4) | new_g; + struct i2c_random_wr_payload exp_reg_array[] = { + {0x3366, analog_gain_reg}, + {0x3362, (uint16_t)(dc_gain_enabled ? 0x1 : 0x0)}, + {0x3012, (uint16_t)exposure_time}, + }; + sensors_i2c(exp_reg_array, sizeof(exp_reg_array)/sizeof(struct i2c_random_wr_payload), CAM_SENSOR_PACKET_OPCODE_SENSOR_CONFIG, true); + } else if (camera_id == CAMERA_ID_IMX390) { + // if gain is sub 1, we have to use exposure to mimic sub 1 gains + uint32_t real_exposure_time = (gain < 1.0) ? (exposure_time*gain) : exposure_time; + // invert real_exposure_time, max exposure is 2 + real_exposure_time = (exposure_time >= 0x7cf) ? 2 : (0x7cf - exposure_time); + uint32_t real_gain = int((10*log10(fmax(1.0, gain)))/0.3); + //printf("%d expose: %d gain: %f = %d\n", camera_num, exposure_time, gain, real_gain); + struct i2c_random_wr_payload exp_reg_array[] = { + {0x000c, real_exposure_time&0xFF}, {0x000d, real_exposure_time>>8}, + {0x0010, real_exposure_time&0xFF}, {0x0011, real_exposure_time>>8}, + {0x0018, real_gain&0xFF}, {0x0019, real_gain>>8}, + }; + sensors_i2c(exp_reg_array, sizeof(exp_reg_array)/sizeof(struct i2c_random_wr_payload), CAM_SENSOR_PACKET_OPCODE_SENSOR_CONFIG, false); + } } void camera_autoexposure(CameraState *s, float grey_frac) { diff --git a/selfdrive/camerad/cameras/camera_qcom2.h b/selfdrive/camerad/cameras/camera_qcom2.h index d021ba256ea6cb..ef57a728a382e0 100644 --- a/selfdrive/camerad/cameras/camera_qcom2.h +++ b/selfdrive/camerad/cameras/camera_qcom2.h @@ -40,8 +40,8 @@ class CameraState { void sensors_start(); void sensors_poke(int request_id); - void sensors_i2c(struct i2c_random_wr_payload* dat, int len, int op_code); - void sensors_init(); + void sensors_i2c(struct i2c_random_wr_payload* dat, int len, int op_code, bool data_word); + int sensors_init(); void camera_open(); void camera_init(MultiCameraState *multi_cam_state, VisionIpcServer * v, int camera_id, int camera_num, unsigned int fps, cl_device_id device_id, cl_context ctx, VisionStreamType rgb_type, VisionStreamType yuv_type); @@ -62,6 +62,7 @@ class CameraState { int frame_id_last; int idx_offset; bool skipped; + int camera_id; CameraBuf buf; }; @@ -73,7 +74,6 @@ typedef struct MultiCameraState { int device_iommu; int cdm_iommu; - CameraState road_cam; CameraState wide_road_cam; CameraState driver_cam; diff --git a/selfdrive/camerad/cameras/real_debayer.cl b/selfdrive/camerad/cameras/real_debayer.cl index 4553036ee35646..6452e44ebb6b62 100644 --- a/selfdrive/camerad/cameras/real_debayer.cl +++ b/selfdrive/camerad/cameras/real_debayer.cl @@ -1,7 +1,5 @@ #pragma OPENCL EXTENSION cl_khr_fp16 : enable -const half black_level = 42.0; - const __constant half3 color_correction[3] = { // post wb CCM (half3)(1.82717181, -0.31231438, 0.07307673), @@ -39,7 +37,7 @@ half3 color_correct(half3 rgb) { return ret; } -half val_from_10(const uchar * source, int gx, int gy) { +inline half val_from_10(const uchar * source, int gx, int gy, half black_level) { // parse 12bit int start = gy * FRAME_STRIDE + (3 * (gx / 2)); int offset = gx % 2; @@ -49,7 +47,7 @@ half val_from_10(const uchar * source, int gx, int gy) { // normalize pv = max(0.0h, pv - black_level); - pv *= 0.00101833h; // /= (1024.0f - black_level); + pv /= (1024.0f - black_level); // correct vignetting if (CAM_NUM == 1) { // fcamera @@ -89,7 +87,8 @@ half phi(half x) { __kernel void debayer10(const __global uchar * in, __global uchar * out, - __local half * cached + __local half * cached, + float black_level ) { const int x_global = get_global_id(0); @@ -102,7 +101,7 @@ __kernel void debayer10(const __global uchar * in, int out_idx = 3 * x_global + 3 * y_global * RGB_WIDTH; - half pv = val_from_10(in, x_global, y_global); + half pv = val_from_10(in, x_global, y_global, black_level); cached[localOffset] = pv; // don't care @@ -118,22 +117,22 @@ __kernel void debayer10(const __global uchar * in, if (x_local < 1) { localColOffset = x_local; globalColOffset = -1; - cached[(y_local + 1) * localRowLen + x_local] = val_from_10(in, x_global-1, y_global); + cached[(y_local + 1) * localRowLen + x_local] = val_from_10(in, x_global-1, y_global, black_level); } else if (x_local >= get_local_size(0) - 1) { localColOffset = x_local + 2; globalColOffset = 1; - cached[localOffset + 1] = val_from_10(in, x_global+1, y_global); + cached[localOffset + 1] = val_from_10(in, x_global+1, y_global, black_level); } if (y_local < 1) { - cached[y_local * localRowLen + x_local + 1] = val_from_10(in, x_global, y_global-1); + cached[y_local * localRowLen + x_local + 1] = val_from_10(in, x_global, y_global-1, black_level); if (localColOffset != -1) { - cached[y_local * localRowLen + localColOffset] = val_from_10(in, x_global+globalColOffset, y_global-1); + cached[y_local * localRowLen + localColOffset] = val_from_10(in, x_global+globalColOffset, y_global-1, black_level); } } else if (y_local >= get_local_size(1) - 1) { - cached[(y_local + 2) * localRowLen + x_local + 1] = val_from_10(in, x_global, y_global+1); + cached[(y_local + 2) * localRowLen + x_local + 1] = val_from_10(in, x_global, y_global+1, black_level); if (localColOffset != -1) { - cached[(y_local + 2) * localRowLen + localColOffset] = val_from_10(in, x_global+globalColOffset, y_global+1); + cached[(y_local + 2) * localRowLen + localColOffset] = val_from_10(in, x_global+globalColOffset, y_global+1, black_level); } } diff --git a/selfdrive/camerad/cameras/sensor2_i2c.h b/selfdrive/camerad/cameras/sensor2_i2c.h index 0f91503be0f9b8..41f7e36cdd7f92 100644 --- a/selfdrive/camerad/cameras/sensor2_i2c.h +++ b/selfdrive/camerad/cameras/sensor2_i2c.h @@ -1,5 +1,49 @@ -struct i2c_random_wr_payload start_reg_array[] = {{0x301A, 0x91C}}; -struct i2c_random_wr_payload stop_reg_array[] = {{0x301A, 0x918}}; +struct i2c_random_wr_payload start_reg_array_ar0231[] = {{0x301A, 0x91C}}; +struct i2c_random_wr_payload stop_reg_array_ar0231[] = {{0x301A, 0x918}}; +struct i2c_random_wr_payload start_reg_array_imx390[] = {{0x0, 0}}; +struct i2c_random_wr_payload stop_reg_array_imx390[] = {{0x0, 1}}; + +struct i2c_random_wr_payload init_array_imx390[] = { + {0x2008, 0xd0}, {0x2009, 0x07}, {0x200a, 0x00}, // MODE_VMAX = time between frames + {0x200C, 0xe4}, {0x200D, 0x0c}, // MODE_HMAX + + // crop + {0x3410, 0x88}, {0x3411, 0x7}, // CROP_H_SIZE + {0x3418, 0xb8}, {0x3419, 0x4}, // CROP_V_SIZE + {0x0078, 1}, {0x03c0, 1}, + + // external trigger (off) + // while images still come in, they are blank with this + {0x3650, 0}, // CU_MODE + + // exposure + {0x000c, 0xc0}, {0x000d, 0x07}, + {0x0010, 0xc0}, {0x0011, 0x07}, + + // WUXGA mode + // not in datasheet, from https://github.com/bogsen/STLinux-Kernel/blob/master/drivers/media/platform/tegra/imx185.c + {0x0086, 0xc4}, {0x0087, 0xff}, // WND_SHIFT_V = -60 + {0x03c6, 0xc4}, {0x03c7, 0xff}, // SM_WND_SHIFT_V_APL = -60 + + {0x201c, 0xe1}, {0x201d, 0x12}, // image read amount + {0x21ee, 0xc4}, {0x21ef, 0x04}, // image send amount (1220 is the end) + {0x21f0, 0xc4}, {0x21f1, 0x04}, // image processing amount + + // disable a bunch of errors causing blanking + {0x0390, 0x00}, {0x0391, 0x00}, {0x0392, 0x00}, + + // flip bayer + {0x2D64, 0x64 + 2}, + + // color correction + {0x0030, 0xf8}, {0x0031, 0x00}, // red gain + {0x0032, 0x9a}, {0x0033, 0x00}, // gr gain + {0x0034, 0x9a}, {0x0035, 0x00}, // gb gain + {0x0036, 0x22}, {0x0037, 0x01}, // blue gain + + // hdr enable (noise with this on for now) + {0x00f9, 0} +}; struct i2c_random_wr_payload init_array_ar0231[] = { {0x301A, 0x0018}, // RESET_REGISTER