sadmen
/
jyoung
mirror of https://github.com/jyoung8607/openpilot.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

camerad: imx390 support (#23966)

Browse Source 
* 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>
faw-hongqi-hs5
George Hotz 3 years ago 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
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 9
      selfdrive/camerad/cameras/camera_common.cc
  2. 3
      selfdrive/camerad/cameras/camera_common.h
  3. 93
      selfdrive/camerad/cameras/camera_qcom2.cc
  4. 6
      selfdrive/camerad/cameras/camera_qcom2.h
  5. 23
      selfdrive/camerad/cameras/real_debayer.cl
  6. 48
      selfdrive/camerad/cameras/sensor2_i2c.h

9
selfdrive/camerad/cameras/camera_common.cc
Unescape 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));

3
selfdrive/camerad/cameras/camera_common.h
Unescape 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;

93
selfdrive/camerad/cameras/camera_qcom2.cc
Unescape 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));
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);
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) {

6
selfdrive/camerad/cameras/camera_qcom2.h
Unescape 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;

23
selfdrive/camerad/cameras/real_debayer.cl
Unescape 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);
}
}

48
selfdrive/camerad/cameras/sensor2_i2c.h
Unescape 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

Write Preview
Loading…
Cancel
Save