Skip to content

Commit

Permalink
camerad: imx390 support (commaai#23966)
Browse files Browse the repository at this point in the history 
* 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>
  • Loading branch information
@geohot @budney
2 people authored and budney committed Mar 27, 2022
1 parent bc582c5 commit e19baf2
Show file tree
Hide file tree
Showing 6 changed files with 146 additions and 48 deletions.
9 changes: 6 additions & 3 deletions selfdrive/camerad/cameras/camera_common.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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));

Expand All @@ -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_) {
Expand Down Expand Up @@ -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));
Expand Down
3 changes: 2 additions & 1 deletion selfdrive/camerad/cameras/camera_common.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down
105 changes: 78 additions & 27 deletions selfdrive/camerad/cameras/camera_qcom2.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand All @@ -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;
Expand Down Expand Up @@ -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) {
Expand All @@ -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;
Expand All @@ -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));

Expand All @@ -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;
Expand All @@ -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;
}
Expand All @@ -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;
Expand Down Expand Up @@ -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?
Expand Down Expand Up @@ -341,14 +366,15 @@ 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);
munmap(power_settings, buf_desc[1].size);
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) {
Expand Down Expand Up @@ -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);
Expand All @@ -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;

Expand Down Expand Up @@ -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");
Expand Down Expand Up @@ -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) {
Expand Down
6 changes: 3 additions & 3 deletions selfdrive/camerad/cameras/camera_qcom2.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand All @@ -62,6 +62,7 @@ class CameraState {
int frame_id_last;
int idx_offset;
bool skipped;
int camera_id;

CameraBuf buf;
};
Expand All @@ -73,7 +74,6 @@ typedef struct MultiCameraState {
int device_iommu;
int cdm_iommu;


CameraState road_cam;
CameraState wide_road_cam;
CameraState driver_cam;
Expand Down
23 changes: 11 additions & 12 deletions selfdrive/camerad/cameras/real_debayer.cl
View file
Original file line number Diff line number Diff line change
@@ -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),
Expand Down Expand Up @@ -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;
Expand All @@ -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
Expand Down Expand Up @@ -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);
Expand All @@ -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
Expand All @@ -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);
}
}

Expand Down
Loading

0 comments on commit e19baf2

Please sign in to comment.