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 <device@comma.ai>
pull/24004/head
George Hotz 2022-03-18 15:21:51 -07:00 committed by GitHub
parent 6c7d178d96
commit 5a11101bcf
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
6 changed files with 146 additions and 48 deletions

View File

@ -52,7 +52,7 @@ public:
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 @@ public:
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));

View File

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

View File

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

View File

@ -40,8 +40,8 @@ public:
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 @@ public:
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;

View File

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

View File

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