diff --git a/system/camerad/SConscript b/system/camerad/SConscript new file mode 100644 index 0000000..8f19e7e --- /dev/null +++ b/system/camerad/SConscript @@ -0,0 +1,10 @@ +Import('env', 'arch', 'cereal', 'messaging', 'common', 'gpucommon', 'visionipc') + +libs = ['m', 'pthread', common, 'jpeg', 'OpenCL', 'yuv', cereal, messaging, 'zmq', 'capnp', 'kj', visionipc, gpucommon, 'atomic'] + +camera_obj = env.Object(['cameras/camera_qcom2.cc', 'cameras/camera_common.cc', 'cameras/camera_util.cc', + 'sensors/ar0231.cc', 'sensors/ox03c10.cc', 'sensors/os04c10.cc']) +env.Program('camerad', ['main.cc', camera_obj], LIBS=libs) + +if GetOption("extras") and arch == "x86_64": + env.Program('test/test_ae_gray', ['test/test_ae_gray.cc', camera_obj], LIBS=libs) diff --git a/system/camerad/__init__.py b/system/camerad/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/system/camerad/camerad b/system/camerad/camerad deleted file mode 100755 index b89994d..0000000 Binary files a/system/camerad/camerad and /dev/null differ diff --git a/system/camerad/cameras/camera_common.cc b/system/camerad/cameras/camera_common.cc new file mode 100644 index 0000000..aa815bf --- /dev/null +++ b/system/camerad/cameras/camera_common.cc @@ -0,0 +1,355 @@ +#include "system/camerad/cameras/camera_common.h" + +#include +#include + +#include "third_party/libyuv/include/libyuv.h" +#include + +#include "common/clutil.h" +#include "common/swaglog.h" +#include "common/util.h" +#include "third_party/linux/include/msm_media_info.h" + +#include "system/camerad/cameras/camera_qcom2.h" +#ifdef QCOM2 +#include "CL/cl_ext_qcom.h" +#endif + +ExitHandler do_exit; + +class Debayer { +public: + Debayer(cl_device_id device_id, cl_context context, const CameraBuf *b, const CameraState *s, int buf_width, int uv_offset) { + char args[4096]; + const SensorInfo *ci = s->ci.get(); + snprintf(args, sizeof(args), + "-cl-fast-relaxed-math -cl-denorms-are-zero " + "-DFRAME_WIDTH=%d -DFRAME_HEIGHT=%d -DFRAME_STRIDE=%d -DFRAME_OFFSET=%d " + "-DRGB_WIDTH=%d -DRGB_HEIGHT=%d -DYUV_STRIDE=%d -DUV_OFFSET=%d " + "-DIS_OX=%d -DIS_OS=%d -DIS_BGGR=%d -DCAM_NUM=%d%s", + ci->frame_width, ci->frame_height, ci->frame_stride, ci->frame_offset, + b->rgb_width, b->rgb_height, buf_width, uv_offset, + ci->image_sensor == cereal::FrameData::ImageSensor::OX03C10, + ci->image_sensor == cereal::FrameData::ImageSensor::OS04C10, + ci->image_sensor == cereal::FrameData::ImageSensor::OS04C10, + s->camera_num, s->camera_num==1 ? " -DVIGNETTING" : ""); + const char *cl_file = "cameras/real_debayer.cl"; + cl_program prg_debayer = cl_program_from_file(context, device_id, cl_file, args); + krnl_ = CL_CHECK_ERR(clCreateKernel(prg_debayer, "debayer10", &err)); + CL_CHECK(clReleaseProgram(prg_debayer)); + + } + + void queue(cl_command_queue q, cl_mem cam_buf_cl, cl_mem buf_cl, int width, int height, 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)); + + const size_t globalWorkSize[] = {size_t(width / 2), size_t(height / 2)}; + const int debayer_local_worksize = 16; + const size_t localWorkSize[] = {debayer_local_worksize, debayer_local_worksize}; + CL_CHECK(clEnqueueNDRangeKernel(q, krnl_, 2, NULL, globalWorkSize, localWorkSize, 0, 0, debayer_event)); + } + + ~Debayer() { + CL_CHECK(clReleaseKernel(krnl_)); + } + +private: + cl_kernel krnl_; +}; + +void CameraBuf::init(cl_device_id device_id, cl_context context, CameraState *s, VisionIpcServer * v, int frame_cnt, VisionStreamType type) { + vipc_server = v; + stream_type = type; + frame_buf_count = frame_cnt; + + const SensorInfo *ci = s->ci.get(); + // RAW frame + const int frame_size = (ci->frame_height + ci->extra_height) * ci->frame_stride; + camera_bufs = std::make_unique(frame_buf_count); + camera_bufs_metadata = std::make_unique(frame_buf_count); + + for (int i = 0; i < frame_buf_count; i++) { + camera_bufs[i].allocate(frame_size); + camera_bufs[i].init_cl(device_id, context); + } + LOGD("allocated %d CL buffers", frame_buf_count); + + rgb_width = ci->frame_width; + rgb_height = ci->frame_height; + + int nv12_width = VENUS_Y_STRIDE(COLOR_FMT_NV12, rgb_width); + int nv12_height = VENUS_Y_SCANLINES(COLOR_FMT_NV12, rgb_height); + assert(nv12_width == VENUS_UV_STRIDE(COLOR_FMT_NV12, rgb_width)); + assert(nv12_height/2 == VENUS_UV_SCANLINES(COLOR_FMT_NV12, rgb_height)); + size_t nv12_uv_offset = nv12_width * nv12_height; + + // the encoder HW tells us the size it wants after setting it up. + // TODO: VENUS_BUFFER_SIZE should give the size, but it's too small. dependent on encoder settings? + size_t nv12_size = (rgb_width >= 2688 ? 2900 : 2346)*nv12_width; + + vipc_server->create_buffers_with_sizes(stream_type, YUV_BUFFER_COUNT, false, rgb_width, rgb_height, nv12_size, nv12_width, nv12_uv_offset); + LOGD("created %d YUV vipc buffers with size %dx%d", YUV_BUFFER_COUNT, nv12_width, nv12_height); + + debayer = new Debayer(device_id, context, this, s, nv12_width, nv12_uv_offset); + + const cl_queue_properties props[] = {0}; //CL_QUEUE_PRIORITY_KHR, CL_QUEUE_PRIORITY_HIGH_KHR, 0}; + q = CL_CHECK_ERR(clCreateCommandQueueWithProperties(context, device_id, props, &err)); +} + +CameraBuf::~CameraBuf() { + for (int i = 0; i < frame_buf_count; i++) { + camera_bufs[i].free(); + } + if (debayer) delete debayer; + if (q) CL_CHECK(clReleaseCommandQueue(q)); +} + +bool CameraBuf::acquire() { + if (!safe_queue.try_pop(cur_buf_idx, 50)) return false; + + if (camera_bufs_metadata[cur_buf_idx].frame_id == -1) { + LOGE("no frame data? wtf"); + return false; + } + + cur_frame_data = camera_bufs_metadata[cur_buf_idx]; + cur_yuv_buf = vipc_server->get_buffer(stream_type); + cur_camera_buf = &camera_bufs[cur_buf_idx]; + + double start_time = millis_since_boot(); + cl_event event; + debayer->queue(q, camera_bufs[cur_buf_idx].buf_cl, cur_yuv_buf->buf_cl, rgb_width, rgb_height, &event); + clWaitForEvents(1, &event); + CL_CHECK(clReleaseEvent(event)); + cur_frame_data.processing_time = (millis_since_boot() - start_time) / 1000.0; + + VisionIpcBufExtra extra = { + cur_frame_data.frame_id, + cur_frame_data.timestamp_sof, + cur_frame_data.timestamp_eof, + }; + cur_yuv_buf->set_frame_id(cur_frame_data.frame_id); + vipc_server->send(cur_yuv_buf, &extra); + + return true; +} + +void CameraBuf::queue(size_t buf_idx) { + safe_queue.push(buf_idx); +} + +// common functions + +void fill_frame_data(cereal::FrameData::Builder &framed, const FrameMetadata &frame_data, CameraState *c) { + framed.setFrameId(frame_data.frame_id); + framed.setRequestId(frame_data.request_id); + framed.setTimestampEof(frame_data.timestamp_eof); + framed.setTimestampSof(frame_data.timestamp_sof); + framed.setIntegLines(frame_data.integ_lines); + framed.setGain(frame_data.gain); + framed.setHighConversionGain(frame_data.high_conversion_gain); + framed.setMeasuredGreyFraction(frame_data.measured_grey_fraction); + framed.setTargetGreyFraction(frame_data.target_grey_fraction); + framed.setProcessingTime(frame_data.processing_time); + + const float ev = c->cur_ev[frame_data.frame_id % 3]; + const float perc = util::map_val(ev, c->ci->min_ev, c->ci->max_ev, 0.0f, 100.0f); + framed.setExposureValPercent(perc); + framed.setSensor(c->ci->image_sensor); +} + +kj::Array get_raw_frame_image(const CameraBuf *b) { + const uint8_t *dat = (const uint8_t *)b->cur_camera_buf->addr; + + kj::Array frame_image = kj::heapArray(b->cur_camera_buf->len); + uint8_t *resized_dat = frame_image.begin(); + + memcpy(resized_dat, dat, b->cur_camera_buf->len); + + return kj::mv(frame_image); +} + +static kj::Array yuv420_to_jpeg(const CameraBuf *b, int thumbnail_width, int thumbnail_height) { + int downscale = b->cur_yuv_buf->width / thumbnail_width; + assert(downscale * thumbnail_height == b->cur_yuv_buf->height); + int in_stride = b->cur_yuv_buf->stride; + + // make the buffer big enough. jpeg_write_raw_data requires 16-pixels aligned height to be used. + std::unique_ptr buf(new uint8_t[(thumbnail_width * ((thumbnail_height + 15) & ~15) * 3) / 2]); + uint8_t *y_plane = buf.get(); + uint8_t *u_plane = y_plane + thumbnail_width * thumbnail_height; + uint8_t *v_plane = u_plane + (thumbnail_width * thumbnail_height) / 4; + { + // subsampled conversion from nv12 to yuv + for (int hy = 0; hy < thumbnail_height/2; hy++) { + for (int hx = 0; hx < thumbnail_width/2; hx++) { + int ix = hx * downscale + (downscale-1)/2; + int iy = hy * downscale + (downscale-1)/2; + y_plane[(hy*2 + 0)*thumbnail_width + (hx*2 + 0)] = b->cur_yuv_buf->y[(iy*2 + 0) * in_stride + ix*2 + 0]; + y_plane[(hy*2 + 0)*thumbnail_width + (hx*2 + 1)] = b->cur_yuv_buf->y[(iy*2 + 0) * in_stride + ix*2 + 1]; + y_plane[(hy*2 + 1)*thumbnail_width + (hx*2 + 0)] = b->cur_yuv_buf->y[(iy*2 + 1) * in_stride + ix*2 + 0]; + y_plane[(hy*2 + 1)*thumbnail_width + (hx*2 + 1)] = b->cur_yuv_buf->y[(iy*2 + 1) * in_stride + ix*2 + 1]; + u_plane[hy*thumbnail_width/2 + hx] = b->cur_yuv_buf->uv[iy*in_stride + ix*2 + 0]; + v_plane[hy*thumbnail_width/2 + hx] = b->cur_yuv_buf->uv[iy*in_stride + ix*2 + 1]; + } + } + } + + struct jpeg_compress_struct cinfo; + struct jpeg_error_mgr jerr; + cinfo.err = jpeg_std_error(&jerr); + jpeg_create_compress(&cinfo); + + uint8_t *thumbnail_buffer = nullptr; + size_t thumbnail_len = 0; + jpeg_mem_dest(&cinfo, &thumbnail_buffer, &thumbnail_len); + + cinfo.image_width = thumbnail_width; + cinfo.image_height = thumbnail_height; + cinfo.input_components = 3; + + jpeg_set_defaults(&cinfo); + jpeg_set_colorspace(&cinfo, JCS_YCbCr); + // configure sampling factors for yuv420. + cinfo.comp_info[0].h_samp_factor = 2; // Y + cinfo.comp_info[0].v_samp_factor = 2; + cinfo.comp_info[1].h_samp_factor = 1; // U + cinfo.comp_info[1].v_samp_factor = 1; + cinfo.comp_info[2].h_samp_factor = 1; // V + cinfo.comp_info[2].v_samp_factor = 1; + cinfo.raw_data_in = TRUE; + + jpeg_set_quality(&cinfo, 50, TRUE); + jpeg_start_compress(&cinfo, TRUE); + + JSAMPROW y[16], u[8], v[8]; + JSAMPARRAY planes[3]{y, u, v}; + + for (int line = 0; line < cinfo.image_height; line += 16) { + for (int i = 0; i < 16; ++i) { + y[i] = y_plane + (line + i) * cinfo.image_width; + if (i % 2 == 0) { + int offset = (cinfo.image_width / 2) * ((i + line) / 2); + u[i / 2] = u_plane + offset; + v[i / 2] = v_plane + offset; + } + } + jpeg_write_raw_data(&cinfo, planes, 16); + } + + jpeg_finish_compress(&cinfo); + jpeg_destroy_compress(&cinfo); + + kj::Array dat = kj::heapArray(thumbnail_buffer, thumbnail_len); + free(thumbnail_buffer); + return dat; +} + +static void publish_thumbnail(PubMaster *pm, const CameraBuf *b) { + auto thumbnail = yuv420_to_jpeg(b, b->rgb_width / 4, b->rgb_height / 4); + if (thumbnail.size() == 0) return; + + MessageBuilder msg; + auto thumbnaild = msg.initEvent().initThumbnail(); + thumbnaild.setFrameId(b->cur_frame_data.frame_id); + thumbnaild.setTimestampEof(b->cur_frame_data.timestamp_eof); + thumbnaild.setThumbnail(thumbnail); + + pm->send("thumbnail", msg); +} + +float set_exposure_target(const CameraBuf *b, int x_start, int x_end, int x_skip, int y_start, int y_end, int y_skip) { + int lum_med; + uint32_t lum_binning[256] = {0}; + const uint8_t *pix_ptr = b->cur_yuv_buf->y; + + unsigned int lum_total = 0; + for (int y = y_start; y < y_end; y += y_skip) { + for (int x = x_start; x < x_end; x += x_skip) { + uint8_t lum = pix_ptr[(y * b->rgb_width) + x]; + lum_binning[lum]++; + lum_total += 1; + } + } + + // Find mean lumimance value + unsigned int lum_cur = 0; + for (lum_med = 255; lum_med >= 0; lum_med--) { + lum_cur += lum_binning[lum_med]; + + if (lum_cur >= lum_total / 2) { + break; + } + } + + return lum_med / 256.0; +} + +void *processing_thread(MultiCameraState *cameras, CameraState *cs, process_thread_cb callback) { + const char *thread_name = nullptr; + if (cs == &cameras->road_cam) { + thread_name = "RoadCamera"; + } else if (cs == &cameras->driver_cam) { + thread_name = "DriverCamera"; + } else { + thread_name = "WideRoadCamera"; + } + util::set_thread_name(thread_name); + + uint32_t cnt = 0; + while (!do_exit) { + if (!cs->buf.acquire()) continue; + + callback(cameras, cs, cnt); + + if (cs == &(cameras->road_cam) && cameras->pm && cnt % 100 == 3) { + // this takes 10ms??? + publish_thumbnail(cameras->pm, &(cs->buf)); + } + ++cnt; + } + return NULL; +} + +std::thread start_process_thread(MultiCameraState *cameras, CameraState *cs, process_thread_cb callback) { + return std::thread(processing_thread, cameras, cs, callback); +} + +void camerad_thread() { + cl_device_id device_id = cl_get_device_id(CL_DEVICE_TYPE_DEFAULT); +#ifdef QCOM2 + const cl_context_properties props[] = {CL_CONTEXT_PRIORITY_HINT_QCOM, CL_PRIORITY_HINT_HIGH_QCOM, 0}; + cl_context context = CL_CHECK_ERR(clCreateContext(props, 1, &device_id, NULL, NULL, &err)); +#else + cl_context context = CL_CHECK_ERR(clCreateContext(NULL, 1, &device_id, NULL, NULL, &err)); +#endif + + { + MultiCameraState cameras = {}; + VisionIpcServer vipc_server("camerad", device_id, context); + + cameras_open(&cameras); + cameras_init(&vipc_server, &cameras, device_id, context); + + vipc_server.start_listener(); + + cameras_run(&cameras); + } + + CL_CHECK(clReleaseContext(context)); +} + +int open_v4l_by_name_and_index(const char name[], int index, int flags) { + for (int v4l_index = 0; /**/; ++v4l_index) { + std::string v4l_name = util::read_file(util::string_format("/sys/class/video4linux/v4l-subdev%d/name", v4l_index)); + if (v4l_name.empty()) return -1; + if (v4l_name.find(name) == 0) { + if (index == 0) { + return HANDLE_EINTR(open(util::string_format("/dev/v4l-subdev%d", v4l_index).c_str(), flags)); + } + index--; + } + } +} diff --git a/system/camerad/cameras/camera_common.h b/system/camerad/cameras/camera_common.h new file mode 100644 index 0000000..f98691e --- /dev/null +++ b/system/camerad/cameras/camera_common.h @@ -0,0 +1,87 @@ +#pragma once + +#include +#include +#include + +#include "cereal/messaging/messaging.h" +#include "cereal/visionipc/visionipc_server.h" +#include "common/queue.h" + +const int YUV_BUFFER_COUNT = 20; + +enum CameraType { + RoadCam = 0, + DriverCam, + WideRoadCam +}; + +// for debugging +const bool env_disable_road = getenv("DISABLE_ROAD") != NULL; +const bool env_disable_wide_road = getenv("DISABLE_WIDE_ROAD") != NULL; +const bool env_disable_driver = getenv("DISABLE_DRIVER") != NULL; +const bool env_debug_frames = getenv("DEBUG_FRAMES") != NULL; +const bool env_log_raw_frames = getenv("LOG_RAW_FRAMES") != NULL; +const bool env_ctrl_exp_from_params = getenv("CTRL_EXP_FROM_PARAMS") != NULL; + +typedef struct FrameMetadata { + uint32_t frame_id; + uint32_t request_id; + + // Timestamps + uint64_t timestamp_sof; + uint64_t timestamp_eof; + + // Exposure + unsigned int integ_lines; + bool high_conversion_gain; + float gain; + float measured_grey_fraction; + float target_grey_fraction; + + float processing_time; +} FrameMetadata; + +struct MultiCameraState; +class CameraState; +class Debayer; + +class CameraBuf { +private: + VisionIpcServer *vipc_server; + Debayer *debayer = nullptr; + VisionStreamType stream_type; + int cur_buf_idx; + SafeQueue safe_queue; + int frame_buf_count; + +public: + cl_command_queue q; + FrameMetadata cur_frame_data; + VisionBuf *cur_yuv_buf; + VisionBuf *cur_camera_buf; + std::unique_ptr camera_bufs; + std::unique_ptr camera_bufs_metadata; + int rgb_width, rgb_height; + + CameraBuf() = default; + ~CameraBuf(); + void init(cl_device_id device_id, cl_context context, CameraState *s, VisionIpcServer * v, int frame_cnt, VisionStreamType type); + bool acquire(); + void queue(size_t buf_idx); +}; + +typedef void (*process_thread_cb)(MultiCameraState *s, CameraState *c, int cnt); + +void fill_frame_data(cereal::FrameData::Builder &framed, const FrameMetadata &frame_data, CameraState *c); +kj::Array get_raw_frame_image(const CameraBuf *b); +float set_exposure_target(const CameraBuf *b, int x_start, int x_end, int x_skip, int y_start, int y_end, int y_skip); +std::thread start_process_thread(MultiCameraState *cameras, CameraState *cs, process_thread_cb callback); + +void cameras_init(VisionIpcServer *v, MultiCameraState *s, cl_device_id device_id, cl_context ctx); +void cameras_open(MultiCameraState *s); +void cameras_run(MultiCameraState *s); +void cameras_close(MultiCameraState *s); +void camerad_thread(); + +int open_v4l_by_name_and_index(const char name[], int index = 0, int flags = O_RDWR | O_NONBLOCK); diff --git a/system/camerad/cameras/camera_qcom2.cc b/system/camerad/cameras/camera_qcom2.cc new file mode 100644 index 0000000..f87ae14 --- /dev/null +++ b/system/camerad/cameras/camera_qcom2.cc @@ -0,0 +1,1017 @@ +#include "system/camerad/cameras/camera_qcom2.h" + +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include "media/cam_defs.h" +#include "media/cam_isp.h" +#include "media/cam_isp_ife.h" +#include "media/cam_req_mgr.h" +#include "media/cam_sensor_cmn_header.h" +#include "media/cam_sync.h" +#include "common/swaglog.h" + +const int MIPI_SETTLE_CNT = 33; // Calculated by camera_freqs.py + +// For debugging: +// echo "4294967295" > /sys/module/cam_debug_util/parameters/debug_mdl + +extern ExitHandler do_exit; + +int CameraState::clear_req_queue() { + struct cam_req_mgr_flush_info req_mgr_flush_request = {0}; + req_mgr_flush_request.session_hdl = session_handle; + req_mgr_flush_request.link_hdl = link_handle; + req_mgr_flush_request.flush_type = CAM_REQ_MGR_FLUSH_TYPE_ALL; + int ret; + ret = do_cam_control(multi_cam_state->video0_fd, CAM_REQ_MGR_FLUSH_REQ, &req_mgr_flush_request, sizeof(req_mgr_flush_request)); + // LOGD("flushed all req: %d", ret); + return ret; +} + +// ************** high level camera helpers **************** + +void CameraState::sensors_start() { + if (!enabled) return; + LOGD("starting sensor %d", camera_num); + sensors_i2c(ci->start_reg_array.data(), ci->start_reg_array.size(), CAM_SENSOR_PACKET_OPCODE_SENSOR_CONFIG, ci->data_word); +} + +void CameraState::sensors_poke(int request_id) { + uint32_t cam_packet_handle = 0; + int size = sizeof(struct cam_packet); + auto pkt = mm.alloc(size, &cam_packet_handle); + pkt->num_cmd_buf = 0; + pkt->kmd_cmd_buf_index = -1; + pkt->header.size = size; + pkt->header.op_code = CAM_SENSOR_PACKET_OPCODE_SENSOR_NOP; + pkt->header.request_id = request_id; + + int ret = device_config(sensor_fd, session_handle, sensor_dev_handle, cam_packet_handle); + if (ret != 0) { + LOGE("** sensor %d FAILED poke, disabling", camera_num); + enabled = false; + return; + } +} + +void CameraState::sensors_i2c(const 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; + auto pkt = mm.alloc(size, &cam_packet_handle); + pkt->num_cmd_buf = 1; + pkt->kmd_cmd_buf_index = -1; + pkt->header.size = size; + pkt->header.op_code = op_code; + struct cam_cmd_buf_desc *buf_desc = (struct cam_cmd_buf_desc *)&pkt->payload; + + buf_desc[0].size = buf_desc[0].length = sizeof(struct i2c_rdwr_header) + len*sizeof(struct i2c_random_wr_payload); + buf_desc[0].type = CAM_CMD_BUF_I2C; + + auto i2c_random_wr = mm.alloc(buf_desc[0].size, (uint32_t*)&buf_desc[0].mem_handle); + 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 = 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)); + + int ret = device_config(sensor_fd, session_handle, sensor_dev_handle, cam_packet_handle); + if (ret != 0) { + LOGE("** sensor %d FAILED i2c, disabling", camera_num); + enabled = false; + return; + } +} + +static cam_cmd_power *power_set_wait(cam_cmd_power *power, int16_t delay_ms) { + cam_cmd_unconditional_wait *unconditional_wait = (cam_cmd_unconditional_wait *)((char *)power + (sizeof(struct cam_cmd_power) + (power->count - 1) * sizeof(struct cam_power_settings))); + unconditional_wait->cmd_type = CAMERA_SENSOR_CMD_TYPE_WAIT; + unconditional_wait->delay = delay_ms; + unconditional_wait->op_code = CAMERA_SENSOR_WAIT_OP_SW_UCND; + return (struct cam_cmd_power *)(unconditional_wait + 1); +} + +int CameraState::sensors_init() { + uint32_t cam_packet_handle = 0; + int size = sizeof(struct cam_packet)+sizeof(struct cam_cmd_buf_desc)*2; + auto pkt = mm.alloc(size, &cam_packet_handle); + pkt->num_cmd_buf = 2; + pkt->kmd_cmd_buf_index = -1; + pkt->header.op_code = 0x1000000 | CAM_SENSOR_PACKET_OPCODE_SENSOR_PROBE; + pkt->header.size = size; + struct cam_cmd_buf_desc *buf_desc = (struct cam_cmd_buf_desc *)&pkt->payload; + + buf_desc[0].size = buf_desc[0].length = sizeof(struct cam_cmd_i2c_info) + sizeof(struct cam_cmd_probe); + buf_desc[0].type = CAM_CMD_BUF_LEGACY; + auto i2c_info = mm.alloc(buf_desc[0].size, (uint32_t*)&buf_desc[0].mem_handle); + auto probe = (struct cam_cmd_probe *)(i2c_info.get() + 1); + + probe->camera_id = camera_num; + i2c_info->slave_addr = ci->getSlaveAddress(camera_num); + // 0(I2C_STANDARD_MODE) = 100khz, 1(I2C_FAST_MODE) = 400khz + //i2c_info->i2c_freq_mode = I2C_STANDARD_MODE; + i2c_info->i2c_freq_mode = I2C_FAST_MODE; + i2c_info->cmd_type = CAMERA_SENSOR_CMD_TYPE_I2C_INFO; + + probe->data_type = CAMERA_SENSOR_I2C_TYPE_WORD; + 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 = ci->probe_reg_addr; + probe->expected_data = ci->probe_expected_data; + probe->data_mask = 0; + + //buf_desc[1].size = buf_desc[1].length = 148; + buf_desc[1].size = buf_desc[1].length = 196; + buf_desc[1].type = CAM_CMD_BUF_I2C; + auto power_settings = mm.alloc(buf_desc[1].size, (uint32_t*)&buf_desc[1].mem_handle); + + // power on + struct cam_cmd_power *power = power_settings.get(); + power->count = 4; + power->cmd_type = CAMERA_SENSOR_CMD_TYPE_PWR_UP; + power->power_settings[0].power_seq_type = 3; // clock?? + power->power_settings[1].power_seq_type = 1; // analog + power->power_settings[2].power_seq_type = 2; // digital + power->power_settings[3].power_seq_type = 8; // reset low + power = power_set_wait(power, 1); + + // set clock + 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 = ci->mclk_frequency; + power = power_set_wait(power, 1); + + // reset high + power->count = 1; + power->cmd_type = CAMERA_SENSOR_CMD_TYPE_PWR_UP; + power->power_settings[0].power_seq_type = 8; + power->power_settings[0].config_val_low = 1; + // wait 650000 cycles @ 19.2 mhz = 33.8 ms + power = power_set_wait(power, 34); + + // probe happens here + + // disable clock + power->count = 1; + power->cmd_type = CAMERA_SENSOR_CMD_TYPE_PWR_DOWN; + power->power_settings[0].power_seq_type = 0; + power->power_settings[0].config_val_low = 0; + power = power_set_wait(power, 1); + + // reset high + power->count = 1; + power->cmd_type = CAMERA_SENSOR_CMD_TYPE_PWR_DOWN; + power->power_settings[0].power_seq_type = 8; + power->power_settings[0].config_val_low = 1; + power = power_set_wait(power, 1); + + // reset low + power->count = 1; + power->cmd_type = CAMERA_SENSOR_CMD_TYPE_PWR_DOWN; + power->power_settings[0].power_seq_type = 8; + power->power_settings[0].config_val_low = 0; + power = power_set_wait(power, 1); + + // power off + power->count = 3; + power->cmd_type = CAMERA_SENSOR_CMD_TYPE_PWR_DOWN; + power->power_settings[0].power_seq_type = 2; + power->power_settings[1].power_seq_type = 1; + power->power_settings[2].power_seq_type = 3; + + int ret = do_cam_control(sensor_fd, CAM_SENSOR_PROBE_CMD, (void *)(uintptr_t)cam_packet_handle, 0); + LOGD("probing the sensor: %d", ret); + return ret; +} + +void CameraState::config_isp(int io_mem_handle, int fence, int request_id, int buf0_mem_handle, int buf0_offset) { + uint32_t cam_packet_handle = 0; + int size = sizeof(struct cam_packet)+sizeof(struct cam_cmd_buf_desc)*2; + if (io_mem_handle != 0) { + size += sizeof(struct cam_buf_io_cfg); + } + auto pkt = mm.alloc(size, &cam_packet_handle); + pkt->num_cmd_buf = 2; + pkt->kmd_cmd_buf_index = 0; + // YUV has kmd_cmd_buf_offset = 1780 + // I guess this is the ISP command + // YUV also has patch_offset = 0x1030 and num_patches = 10 + + if (io_mem_handle != 0) { + pkt->io_configs_offset = sizeof(struct cam_cmd_buf_desc)*pkt->num_cmd_buf; + pkt->num_io_configs = 1; + } + + if (io_mem_handle != 0) { + pkt->header.op_code = 0xf000001; + pkt->header.request_id = request_id; + } else { + pkt->header.op_code = 0xf000000; + } + pkt->header.size = size; + struct cam_cmd_buf_desc *buf_desc = (struct cam_cmd_buf_desc *)&pkt->payload; + struct cam_buf_io_cfg *io_cfg = (struct cam_buf_io_cfg *)((char*)&pkt->payload + pkt->io_configs_offset); + + // TODO: support MMU + buf_desc[0].size = 65624; + buf_desc[0].length = 0; + buf_desc[0].type = CAM_CMD_BUF_DIRECT; + buf_desc[0].meta_data = 3; + buf_desc[0].mem_handle = buf0_mem_handle; + buf_desc[0].offset = buf0_offset; + + // parsed by cam_isp_packet_generic_blob_handler + struct isp_packet { + uint32_t type_0; + cam_isp_resource_hfr_config resource_hfr; + + uint32_t type_1; + cam_isp_clock_config clock; + uint64_t extra_rdi_hz[3]; + + uint32_t type_2; + cam_isp_bw_config bw; + struct cam_isp_bw_vote extra_rdi_vote[6]; + } __attribute__((packed)) tmp; + memset(&tmp, 0, sizeof(tmp)); + + tmp.type_0 = CAM_ISP_GENERIC_BLOB_TYPE_HFR_CONFIG; + tmp.type_0 |= sizeof(cam_isp_resource_hfr_config) << 8; + static_assert(sizeof(cam_isp_resource_hfr_config) == 0x20); + tmp.resource_hfr = { + .num_ports = 1, // 10 for YUV (but I don't think we need them) + .port_hfr_config[0] = { + .resource_type = CAM_ISP_IFE_OUT_RES_RDI_0, // CAM_ISP_IFE_OUT_RES_FULL for YUV + .subsample_pattern = 1, + .subsample_period = 0, + .framedrop_pattern = 1, + .framedrop_period = 0, + }}; + + tmp.type_1 = CAM_ISP_GENERIC_BLOB_TYPE_CLOCK_CONFIG; + tmp.type_1 |= (sizeof(cam_isp_clock_config) + sizeof(tmp.extra_rdi_hz)) << 8; + static_assert((sizeof(cam_isp_clock_config) + sizeof(tmp.extra_rdi_hz)) == 0x38); + tmp.clock = { + .usage_type = 1, // dual mode + .num_rdi = 4, + .left_pix_hz = 404000000, + .right_pix_hz = 404000000, + .rdi_hz[0] = 404000000, + }; + + + tmp.type_2 = CAM_ISP_GENERIC_BLOB_TYPE_BW_CONFIG; + tmp.type_2 |= (sizeof(cam_isp_bw_config) + sizeof(tmp.extra_rdi_vote)) << 8; + static_assert((sizeof(cam_isp_bw_config) + sizeof(tmp.extra_rdi_vote)) == 0xe0); + tmp.bw = { + .usage_type = 1, // dual mode + .num_rdi = 4, + .left_pix_vote = { + .resource_id = 0, + .cam_bw_bps = 450000000, + .ext_bw_bps = 450000000, + }, + .rdi_vote[0] = { + .resource_id = 0, + .cam_bw_bps = 8706200000, + .ext_bw_bps = 8706200000, + }, + }; + + static_assert(offsetof(struct isp_packet, type_2) == 0x60); + + buf_desc[1].size = sizeof(tmp); + buf_desc[1].offset = io_mem_handle != 0 ? 0x60 : 0; + buf_desc[1].length = buf_desc[1].size - buf_desc[1].offset; + buf_desc[1].type = CAM_CMD_BUF_GENERIC; + buf_desc[1].meta_data = CAM_ISP_PACKET_META_GENERIC_BLOB_COMMON; + auto buf2 = mm.alloc(buf_desc[1].size, (uint32_t*)&buf_desc[1].mem_handle); + memcpy(buf2.get(), &tmp, sizeof(tmp)); + + if (io_mem_handle != 0) { + io_cfg[0].mem_handle[0] = io_mem_handle; + io_cfg[0].planes[0] = (struct cam_plane_cfg){ + .width = ci->frame_width, + .height = ci->frame_height + ci->extra_height, + .plane_stride = ci->frame_stride, + .slice_height = ci->frame_height + ci->extra_height, + .meta_stride = 0x0, // YUV has meta(stride=0x400, size=0x5000) + .meta_size = 0x0, + .meta_offset = 0x0, + .packer_config = 0x0, // 0xb for YUV + .mode_config = 0x0, // 0x9ef for YUV + .tile_config = 0x0, + .h_init = 0x0, + .v_init = 0x0, + }; + io_cfg[0].format = ci->mipi_format; // CAM_FORMAT_UBWC_TP10 for YUV + io_cfg[0].color_space = CAM_COLOR_SPACE_BASE; // CAM_COLOR_SPACE_BT601_FULL for YUV + io_cfg[0].color_pattern = 0x5; // 0x0 for YUV + io_cfg[0].bpp = (ci->mipi_format == CAM_FORMAT_MIPI_RAW_10 ? 0xa : 0xc); // bits per pixel + io_cfg[0].resource_type = CAM_ISP_IFE_OUT_RES_RDI_0; // CAM_ISP_IFE_OUT_RES_FULL for YUV + io_cfg[0].fence = fence; + io_cfg[0].direction = CAM_BUF_OUTPUT; + io_cfg[0].subsample_pattern = 0x1; + io_cfg[0].framedrop_pattern = 0x1; + } + + int ret = device_config(multi_cam_state->isp_fd, session_handle, isp_dev_handle, cam_packet_handle); + assert(ret == 0); + if (ret != 0) { + LOGE("isp config failed"); + } +} + +void CameraState::enqueue_buffer(int i, bool dp) { + int ret; + int request_id = request_ids[i]; + + if (buf_handle[i] && sync_objs[i]) { + // wait + struct cam_sync_wait sync_wait = {0}; + sync_wait.sync_obj = sync_objs[i]; + sync_wait.timeout_ms = 50; // max dt tolerance, typical should be 23 + ret = do_cam_control(multi_cam_state->cam_sync_fd, CAM_SYNC_WAIT, &sync_wait, sizeof(sync_wait)); + if (ret != 0) { + LOGE("failed to wait for sync: %d %d", ret, sync_wait.sync_obj); + // TODO: handle frame drop cleanly + } + + buf.camera_bufs_metadata[i].timestamp_eof = (uint64_t)nanos_since_boot(); // set true eof + if (dp) buf.queue(i); + + // destroy old output fence + struct cam_sync_info sync_destroy = {0}; + sync_destroy.sync_obj = sync_objs[i]; + ret = do_cam_control(multi_cam_state->cam_sync_fd, CAM_SYNC_DESTROY, &sync_destroy, sizeof(sync_destroy)); + if (ret != 0) { + LOGE("failed to destroy sync object: %d %d", ret, sync_destroy.sync_obj); + } + } + + // create output fence + struct cam_sync_info sync_create = {0}; + strcpy(sync_create.name, "NodeOutputPortFence"); + ret = do_cam_control(multi_cam_state->cam_sync_fd, CAM_SYNC_CREATE, &sync_create, sizeof(sync_create)); + if (ret != 0) { + LOGE("failed to create fence: %d %d", ret, sync_create.sync_obj); + } + sync_objs[i] = sync_create.sync_obj; + + // schedule request with camera request manager + struct cam_req_mgr_sched_request req_mgr_sched_request = {0}; + req_mgr_sched_request.session_hdl = session_handle; + req_mgr_sched_request.link_hdl = link_handle; + req_mgr_sched_request.req_id = request_id; + ret = do_cam_control(multi_cam_state->video0_fd, CAM_REQ_MGR_SCHED_REQ, &req_mgr_sched_request, sizeof(req_mgr_sched_request)); + if (ret != 0) { + LOGE("failed to schedule cam mgr request: %d %d", ret, request_id); + } + + // poke sensor, must happen after schedule + sensors_poke(request_id); + + // submit request to the ife + config_isp(buf_handle[i], sync_objs[i], request_id, buf0_handle, 65632*(i+1)); +} + +void CameraState::enqueue_req_multi(int start, int n, bool dp) { + for (int i=start; idc_gain_min_weight; + gain_idx = ci->analog_gain_rec_idx; + exposure_time = 5; + cur_ev[0] = cur_ev[1] = cur_ev[2] = (1 + dc_gain_weight * (ci->dc_gain_factor-1) / ci->dc_gain_max_weight) * ci->sensor_analog_gains[gain_idx] * exposure_time; +} + +void CameraState::camera_map_bufs(MultiCameraState *s) { + for (int i = 0; i < FRAME_BUF_COUNT; i++) { + // configure ISP to put the image in place + 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 = CAM_MEM_FLAG_HW_READ_WRITE; + mem_mgr_map_cmd.fd = buf.camera_bufs[i].fd; + int ret = do_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", buf.camera_bufs[i].fd, mem_mgr_map_cmd.out.buf_handle, ret); + buf_handle[i] = mem_mgr_map_cmd.out.buf_handle; + } + enqueue_req_multi(1, FRAME_BUF_COUNT, 0); +} + +void CameraState::camera_init(MultiCameraState *s, VisionIpcServer * v, cl_device_id device_id, cl_context ctx, VisionStreamType yuv_type) { + if (!enabled) return; + + LOGD("camera init %d", camera_num); + request_id_last = 0; + skipped = true; + + buf.init(device_id, ctx, this, v, FRAME_BUF_COUNT, yuv_type); + camera_map_bufs(s); +} + +void CameraState::camera_open(MultiCameraState *multi_cam_state_, int camera_num_, bool enabled_) { + multi_cam_state = multi_cam_state_; + camera_num = camera_num_; + enabled = enabled_; + if (!enabled) return; + + 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); + + // init memorymanager for this camera + mm.init(multi_cam_state->video0_fd); + + LOGD("-- Probing sensor %d", camera_num); + + auto init_sensor_lambda = [this](SensorInfo *sensor) { + ci.reset(sensor); + int ret = sensors_init(); + if (ret == 0) { + sensor_set_parameters(); + } + return ret == 0; + }; + + // Try different sensors one by one until it success. + if (!init_sensor_lambda(new AR0231) && + !init_sensor_lambda(new OX03C10) && + !init_sensor_lambda(new OS04C10)) { + LOGE("** sensor %d FAILED bringup, disabling", camera_num); + enabled = false; + return; + } + LOGD("-- Probing sensor %d success", camera_num); + + // create session + struct cam_req_mgr_session_info 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; + + // access the sensor + LOGD("-- Accessing sensor"); + auto sensor_dev_handle_ = device_acquire(sensor_fd, session_handle, nullptr); + assert(sensor_dev_handle_); + sensor_dev_handle = *sensor_dev_handle_; + LOGD("acquire sensor dev"); + + LOG("-- Configuring sensor"); + sensors_i2c(ci->init_reg_array.data(), ci->init_reg_array.size(), CAM_SENSOR_PACKET_OPCODE_SENSOR_CONFIG, ci->data_word); + + // NOTE: to be able to disable road and wide road, we still have to configure the sensor over i2c + // If you don't do this, the strobe GPIO is an output (even in reset it seems!) + if (!enabled) return; + + struct cam_isp_in_port_info in_port_info = { + .res_type = (uint32_t[]){CAM_ISP_IFE_IN_RES_PHY_0, CAM_ISP_IFE_IN_RES_PHY_1, CAM_ISP_IFE_IN_RES_PHY_2}[camera_num], + + .lane_type = CAM_ISP_LANE_TYPE_DPHY, + .lane_num = 4, + .lane_cfg = 0x3210, + + .vc = 0x0, + .dt = ci->frame_data_type, + .format = ci->mipi_format, + + .test_pattern = 0x2, // 0x3? + .usage_type = 0x0, + + .left_start = 0, + .left_stop = ci->frame_width - 1, + .left_width = ci->frame_width, + + .right_start = 0, + .right_stop = ci->frame_width - 1, + .right_width = ci->frame_width, + + .line_start = 0, + .line_stop = ci->frame_height + ci->extra_height - 1, + .height = ci->frame_height + ci->extra_height, + + .pixel_clk = 0x0, + .batch_size = 0x0, + .dsp_mode = CAM_ISP_DSP_MODE_NONE, + .hbi_cnt = 0x0, + .custom_csid = 0x0, + + .num_out_res = 0x1, + .data[0] = (struct cam_isp_out_port_info){ + .res_type = CAM_ISP_IFE_OUT_RES_RDI_0, + .format = ci->mipi_format, + .width = ci->frame_width, + .height = ci->frame_height + ci->extra_height, + .comp_grp_id = 0x0, .split_point = 0x0, .secure_mode = 0x0, + }, + }; + struct cam_isp_resource isp_resource = { + .resource_id = CAM_ISP_RES_ID_PORT, + .handle_type = CAM_HANDLE_USER_POINTER, + .res_hdl = (uint64_t)&in_port_info, + .length = sizeof(in_port_info), + }; + + auto isp_dev_handle_ = device_acquire(multi_cam_state->isp_fd, session_handle, &isp_resource); + assert(isp_dev_handle_); + isp_dev_handle = *isp_dev_handle_; + LOGD("acquire isp dev"); + + csiphy_fd = open_v4l_by_name_and_index("cam-csiphy-driver", camera_num); + assert(csiphy_fd >= 0); + LOGD("opened csiphy for %d", camera_num); + + struct cam_csiphy_acquire_dev_info csiphy_acquire_dev_info = {.combo_mode = 0}; + auto csiphy_dev_handle_ = device_acquire(csiphy_fd, session_handle, &csiphy_acquire_dev_info); + assert(csiphy_dev_handle_); + csiphy_dev_handle = *csiphy_dev_handle_; + LOGD("acquire csiphy dev"); + + // config ISP + alloc_w_mmu_hdl(multi_cam_state->video0_fd, 984480, (uint32_t*)&buf0_handle, 0x20, CAM_MEM_FLAG_HW_READ_WRITE | CAM_MEM_FLAG_KMD_ACCESS | + CAM_MEM_FLAG_UMD_ACCESS | CAM_MEM_FLAG_CMD_BUF_TYPE, multi_cam_state->device_iommu, multi_cam_state->cdm_iommu); + config_isp(0, 0, 1, buf0_handle, 0); + + // config csiphy + LOG("-- Config CSI PHY"); + { + uint32_t cam_packet_handle = 0; + int size = sizeof(struct cam_packet)+sizeof(struct cam_cmd_buf_desc)*1; + auto pkt = mm.alloc(size, &cam_packet_handle); + pkt->num_cmd_buf = 1; + pkt->kmd_cmd_buf_index = -1; + pkt->header.size = size; + struct cam_cmd_buf_desc *buf_desc = (struct cam_cmd_buf_desc *)&pkt->payload; + + buf_desc[0].size = buf_desc[0].length = sizeof(struct cam_csiphy_info); + buf_desc[0].type = CAM_CMD_BUF_GENERIC; + + auto csiphy_info = mm.alloc(buf_desc[0].size, (uint32_t*)&buf_desc[0].mem_handle); + csiphy_info->lane_mask = 0x1f; + csiphy_info->lane_assign = 0x3210;// skip clk. How is this 16 bit for 5 channels?? + csiphy_info->csiphy_3phase = 0x0; // no 3 phase, only 2 conductors per lane + csiphy_info->combo_mode = 0x0; + csiphy_info->lane_cnt = 0x4; + csiphy_info->secure_mode = 0x0; + csiphy_info->settle_time = MIPI_SETTLE_CNT * 200000000ULL; + csiphy_info->data_rate = 48000000; // Calculated by camera_freqs.py + + int ret_ = device_config(csiphy_fd, session_handle, csiphy_dev_handle, cam_packet_handle); + assert(ret_ == 0); + } + + // link devices + LOG("-- Link devices"); + struct cam_req_mgr_link_info req_mgr_link_info = {0}; + req_mgr_link_info.session_hdl = session_handle; + req_mgr_link_info.num_devices = 2; + req_mgr_link_info.dev_hdls[0] = isp_dev_handle; + req_mgr_link_info.dev_hdls[1] = sensor_dev_handle; + ret = do_cam_control(multi_cam_state->video0_fd, CAM_REQ_MGR_LINK, &req_mgr_link_info, sizeof(req_mgr_link_info)); + link_handle = req_mgr_link_info.link_hdl; + LOGD("link: %d session: 0x%X isp: 0x%X sensors: 0x%X link: 0x%X", ret, session_handle, isp_dev_handle, sensor_dev_handle, link_handle); + + struct cam_req_mgr_link_control req_mgr_link_control = {0}; + req_mgr_link_control.ops = CAM_REQ_MGR_LINK_ACTIVATE; + req_mgr_link_control.session_hdl = session_handle; + req_mgr_link_control.num_links = 1; + req_mgr_link_control.link_hdls[0] = link_handle; + ret = do_cam_control(multi_cam_state->video0_fd, CAM_REQ_MGR_LINK_CONTROL, &req_mgr_link_control, sizeof(req_mgr_link_control)); + LOGD("link control: %d", ret); + + ret = device_control(csiphy_fd, CAM_START_DEV, session_handle, csiphy_dev_handle); + LOGD("start csiphy: %d", ret); + ret = device_control(multi_cam_state->isp_fd, CAM_START_DEV, session_handle, isp_dev_handle); + LOGD("start isp: %d", ret); + assert(ret == 0); + + // TODO: this is unneeded, should we be doing the start i2c in a different way? + //ret = device_control(sensor_fd, CAM_START_DEV, session_handle, sensor_dev_handle); + //LOGD("start sensor: %d", ret); +} + +void cameras_init(VisionIpcServer *v, MultiCameraState *s, cl_device_id device_id, cl_context ctx) { + s->driver_cam.camera_init(s, v, device_id, ctx, VISION_STREAM_DRIVER); + s->road_cam.camera_init(s, v, device_id, ctx, VISION_STREAM_ROAD); + s->wide_road_cam.camera_init(s, v, device_id, ctx, VISION_STREAM_WIDE_ROAD); + + s->pm = new PubMaster({"roadCameraState", "driverCameraState", "wideRoadCameraState", "thumbnail"}); +} + +void cameras_open(MultiCameraState *s) { + int ret; + + LOG("-- Opening devices"); + // video0 is req_mgr, the target of many ioctls + s->video0_fd = HANDLE_EINTR(open("/dev/v4l/by-path/platform-soc:qcom_cam-req-mgr-video-index0", O_RDWR | O_NONBLOCK)); + assert(s->video0_fd >= 0); + LOGD("opened video0"); + + // video1 is cam_sync, the target of some ioctls + s->cam_sync_fd = HANDLE_EINTR(open("/dev/v4l/by-path/platform-cam_sync-video-index0", O_RDWR | O_NONBLOCK)); + assert(s->cam_sync_fd >= 0); + LOGD("opened video1 (cam_sync)"); + + // looks like there's only one of these + s->isp_fd = open_v4l_by_name_and_index("cam-isp"); + assert(s->isp_fd >= 0); + LOGD("opened isp"); + + // query icp for MMU handles + LOG("-- Query ICP for MMU handles"); + struct cam_isp_query_cap_cmd isp_query_cap_cmd = {0}; + struct cam_query_cap_cmd query_cap_cmd = {0}; + query_cap_cmd.handle_type = 1; + query_cap_cmd.caps_handle = (uint64_t)&isp_query_cap_cmd; + query_cap_cmd.size = sizeof(isp_query_cap_cmd); + ret = do_cam_control(s->isp_fd, CAM_QUERY_CAP, &query_cap_cmd, sizeof(query_cap_cmd)); + assert(ret == 0); + LOGD("using MMU handle: %x", isp_query_cap_cmd.device_iommu.non_secure); + LOGD("using MMU handle: %x", isp_query_cap_cmd.cdm_iommu.non_secure); + s->device_iommu = isp_query_cap_cmd.device_iommu.non_secure; + s->cdm_iommu = isp_query_cap_cmd.cdm_iommu.non_secure; + + // subscribe + LOG("-- Subscribing"); + struct v4l2_event_subscription sub = {0}; + sub.type = V4L_EVENT_CAM_REQ_MGR_EVENT; + sub.id = V4L_EVENT_CAM_REQ_MGR_SOF_BOOT_TS; + ret = HANDLE_EINTR(ioctl(s->video0_fd, VIDIOC_SUBSCRIBE_EVENT, &sub)); + LOGD("req mgr subscribe: %d", ret); + + s->driver_cam.camera_open(s, 2, !env_disable_driver); + LOGD("driver camera opened"); + s->road_cam.camera_open(s, 1, !env_disable_road); + LOGD("road camera opened"); + s->wide_road_cam.camera_open(s, 0, !env_disable_wide_road); + LOGD("wide road camera opened"); +} + +void CameraState::camera_close() { + int ret; + + // stop devices + LOG("-- Stop devices %d", camera_num); + + if (enabled) { + // ret = device_control(sensor_fd, CAM_STOP_DEV, session_handle, sensor_dev_handle); + // LOGD("stop sensor: %d", ret); + ret = device_control(multi_cam_state->isp_fd, CAM_STOP_DEV, session_handle, isp_dev_handle); + LOGD("stop isp: %d", ret); + ret = device_control(csiphy_fd, CAM_STOP_DEV, session_handle, csiphy_dev_handle); + LOGD("stop csiphy: %d", ret); + // link control stop + LOG("-- Stop link control"); + struct cam_req_mgr_link_control req_mgr_link_control = {0}; + req_mgr_link_control.ops = CAM_REQ_MGR_LINK_DEACTIVATE; + req_mgr_link_control.session_hdl = session_handle; + req_mgr_link_control.num_links = 1; + req_mgr_link_control.link_hdls[0] = link_handle; + ret = do_cam_control(multi_cam_state->video0_fd, CAM_REQ_MGR_LINK_CONTROL, &req_mgr_link_control, sizeof(req_mgr_link_control)); + LOGD("link control stop: %d", ret); + + // unlink + LOG("-- Unlink"); + struct cam_req_mgr_unlink_info req_mgr_unlink_info = {0}; + req_mgr_unlink_info.session_hdl = session_handle; + req_mgr_unlink_info.link_hdl = link_handle; + ret = do_cam_control(multi_cam_state->video0_fd, CAM_REQ_MGR_UNLINK, &req_mgr_unlink_info, sizeof(req_mgr_unlink_info)); + LOGD("unlink: %d", ret); + + // release devices + LOGD("-- Release devices"); + ret = device_control(multi_cam_state->isp_fd, CAM_RELEASE_DEV, session_handle, isp_dev_handle); + LOGD("release isp: %d", ret); + ret = device_control(csiphy_fd, CAM_RELEASE_DEV, session_handle, csiphy_dev_handle); + LOGD("release csiphy: %d", ret); + + for (int i = 0; i < FRAME_BUF_COUNT; i++) { + release(multi_cam_state->video0_fd, buf_handle[i]); + } + LOGD("released buffers"); + } + + ret = device_control(sensor_fd, CAM_RELEASE_DEV, session_handle, sensor_dev_handle); + LOGD("release sensor: %d", ret); + + // destroyed session + struct cam_req_mgr_session_info session_info = {.session_hdl = session_handle}; + ret = do_cam_control(multi_cam_state->video0_fd, CAM_REQ_MGR_DESTROY_SESSION, &session_info, sizeof(session_info)); + LOGD("destroyed session %d: %d", camera_num, ret); +} + +void cameras_close(MultiCameraState *s) { + s->driver_cam.camera_close(); + s->road_cam.camera_close(); + s->wide_road_cam.camera_close(); + + delete s->pm; +} + +void CameraState::handle_camera_event(void *evdat) { + if (!enabled) return; + struct cam_req_mgr_message *event_data = (struct cam_req_mgr_message *)evdat; + assert(event_data->session_hdl == session_handle); + assert(event_data->u.frame_msg.link_hdl == link_handle); + + uint64_t timestamp = event_data->u.frame_msg.timestamp; + int main_id = event_data->u.frame_msg.frame_id; + int real_id = event_data->u.frame_msg.request_id; + + if (real_id != 0) { // next ready + if (real_id == 1) {idx_offset = main_id;} + int buf_idx = (real_id - 1) % FRAME_BUF_COUNT; + + // check for skipped frames + if (main_id > frame_id_last + 1 && !skipped) { + LOGE("camera %d realign", camera_num); + clear_req_queue(); + enqueue_req_multi(real_id + 1, FRAME_BUF_COUNT - 1, 0); + skipped = true; + } else if (main_id == frame_id_last + 1) { + skipped = false; + } + + // check for dropped requests + if (real_id > request_id_last + 1) { + LOGE("camera %d dropped requests %d %d", camera_num, real_id, request_id_last); + enqueue_req_multi(request_id_last + 1 + FRAME_BUF_COUNT, real_id - (request_id_last + 1), 0); + } + + // metas + frame_id_last = main_id; + request_id_last = real_id; + + auto &meta_data = buf.camera_bufs_metadata[buf_idx]; + meta_data.frame_id = main_id - idx_offset; + meta_data.request_id = real_id; + meta_data.timestamp_sof = timestamp; + exp_lock.lock(); + meta_data.gain = analog_gain_frac * (1 + dc_gain_weight * (ci->dc_gain_factor-1) / ci->dc_gain_max_weight); + meta_data.high_conversion_gain = dc_gain_enabled; + meta_data.integ_lines = exposure_time; + meta_data.measured_grey_fraction = measured_grey_fraction; + meta_data.target_grey_fraction = target_grey_fraction; + exp_lock.unlock(); + + // dispatch + enqueue_req_multi(real_id + FRAME_BUF_COUNT, 1, 1); + } else { // not ready + if (main_id > frame_id_last + 10) { + LOGE("camera %d reset after half second of no response", camera_num); + clear_req_queue(); + enqueue_req_multi(request_id_last + 1, FRAME_BUF_COUNT, 0); + frame_id_last = main_id; + skipped = true; + } + } +} + +void CameraState::update_exposure_score(float desired_ev, int exp_t, int exp_g_idx, float exp_gain) { + float score = ci->getExposureScore(desired_ev, exp_t, exp_g_idx, exp_gain, gain_idx); + if (score < best_ev_score) { + new_exp_t = exp_t; + new_exp_g = exp_g_idx; + best_ev_score = score; + } +} + +void CameraState::set_camera_exposure(float grey_frac) { + if (!enabled) return; + const float dt = 0.05; + + const float ts_grey = 10.0; + const float ts_ev = 0.05; + + const float k_grey = (dt / ts_grey) / (1.0 + dt / ts_grey); + const float k_ev = (dt / ts_ev) / (1.0 + dt / ts_ev); + + // It takes 3 frames for the commanded exposure settings to take effect. The first frame is already started by the time + // we reach this function, the other 2 are due to the register buffering in the sensor. + // Therefore we use the target EV from 3 frames ago, the grey fraction that was just measured was the result of that control action. + // TODO: Lower latency to 2 frames, by using the histogram outputted by the sensor we can do AE before the debayering is complete + + const float cur_ev_ = cur_ev[buf.cur_frame_data.frame_id % 3]; + + // Scale target grey between 0.1 and 0.4 depending on lighting conditions + float new_target_grey = std::clamp(0.4 - 0.3 * log2(1.0 + ci->target_grey_factor*cur_ev_) / log2(6000.0), 0.1, 0.4); + float target_grey = (1.0 - k_grey) * target_grey_fraction + k_grey * new_target_grey; + + float desired_ev = std::clamp(cur_ev_ * target_grey / grey_frac, ci->min_ev, ci->max_ev); + float k = (1.0 - k_ev) / 3.0; + desired_ev = (k * cur_ev[0]) + (k * cur_ev[1]) + (k * cur_ev[2]) + (k_ev * desired_ev); + + best_ev_score = 1e6; + new_exp_g = 0; + new_exp_t = 0; + + // Hysteresis around high conversion gain + // We usually want this on since it results in lower noise, but turn off in very bright day scenes + bool enable_dc_gain = dc_gain_enabled; + if (!enable_dc_gain && target_grey < ci->dc_gain_on_grey) { + enable_dc_gain = true; + dc_gain_weight = ci->dc_gain_min_weight; + } else if (enable_dc_gain && target_grey > ci->dc_gain_off_grey) { + enable_dc_gain = false; + dc_gain_weight = ci->dc_gain_max_weight; + } + + if (enable_dc_gain && dc_gain_weight < ci->dc_gain_max_weight) {dc_gain_weight += 1;} + if (!enable_dc_gain && dc_gain_weight > ci->dc_gain_min_weight) {dc_gain_weight -= 1;} + + std::string gain_bytes, time_bytes; + if (env_ctrl_exp_from_params) { + gain_bytes = params.get("CameraDebugExpGain"); + time_bytes = params.get("CameraDebugExpTime"); + } + + if (gain_bytes.size() > 0 && time_bytes.size() > 0) { + // Override gain and exposure time + gain_idx = std::stoi(gain_bytes); + exposure_time = std::stoi(time_bytes); + + new_exp_g = gain_idx; + new_exp_t = exposure_time; + enable_dc_gain = false; + } else { + // Simple brute force optimizer to choose sensor parameters + // to reach desired EV + for (int g = std::max((int)ci->analog_gain_min_idx, gain_idx - 1); g <= std::min((int)ci->analog_gain_max_idx, gain_idx + 1); g++) { + float gain = ci->sensor_analog_gains[g] * (1 + dc_gain_weight * (ci->dc_gain_factor-1) / ci->dc_gain_max_weight); + + // Compute optimal time for given gain + int t = std::clamp(int(std::round(desired_ev / gain)), ci->exposure_time_min, ci->exposure_time_max); + + // Only go below recommended gain when absolutely necessary to not overexpose + if (g < ci->analog_gain_rec_idx && t > 20 && g < gain_idx) { + continue; + } + + update_exposure_score(desired_ev, t, g, gain); + } + } + + exp_lock.lock(); + + measured_grey_fraction = grey_frac; + target_grey_fraction = target_grey; + + analog_gain_frac = ci->sensor_analog_gains[new_exp_g]; + gain_idx = new_exp_g; + exposure_time = new_exp_t; + dc_gain_enabled = enable_dc_gain; + + float gain = analog_gain_frac * (1 + dc_gain_weight * (ci->dc_gain_factor-1) / ci->dc_gain_max_weight); + cur_ev[buf.cur_frame_data.frame_id % 3] = exposure_time * gain; + + exp_lock.unlock(); + + // Processing a frame takes right about 50ms, so we need to wait a few ms + // so we don't send i2c commands around the frame start. + int ms = (nanos_since_boot() - buf.cur_frame_data.timestamp_sof) / 1000000; + if (ms < 60) { + util::sleep_for(60 - ms); + } + // 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)); + + auto exp_reg_array = ci->getExposureRegisters(exposure_time, new_exp_g, dc_gain_enabled); + sensors_i2c(exp_reg_array.data(), exp_reg_array.size(), CAM_SENSOR_PACKET_OPCODE_SENSOR_CONFIG, ci->data_word); +} + +static void process_driver_camera(MultiCameraState *s, CameraState *c, int cnt) { + c->set_camera_exposure(set_exposure_target(&c->buf, 96, 1832, 2, 242, 1148, 4)); + + MessageBuilder msg; + auto framed = msg.initEvent().initDriverCameraState(); + framed.setFrameType(cereal::FrameData::FrameType::FRONT); + fill_frame_data(framed, c->buf.cur_frame_data, c); + + c->ci->processRegisters(c, framed); + s->pm->send("driverCameraState", msg); +} + +void process_road_camera(MultiCameraState *s, CameraState *c, int cnt) { + const CameraBuf *b = &c->buf; + + MessageBuilder msg; + auto framed = c == &s->road_cam ? msg.initEvent().initRoadCameraState() : msg.initEvent().initWideRoadCameraState(); + fill_frame_data(framed, b->cur_frame_data, c); + if (env_log_raw_frames && c == &s->road_cam && cnt % 100 == 5) { // no overlap with qlog decimation + framed.setImage(get_raw_frame_image(b)); + } + LOGT(c->buf.cur_frame_data.frame_id, "%s: Image set", c == &s->road_cam ? "RoadCamera" : "WideRoadCamera"); + + c->ci->processRegisters(c, framed); + s->pm->send(c == &s->road_cam ? "roadCameraState" : "wideRoadCameraState", msg); + + const auto [x, y, w, h] = (c == &s->wide_road_cam) ? std::tuple(96, 250, 1734, 524) : std::tuple(96, 160, 1734, 986); + const int skip = 2; + c->set_camera_exposure(set_exposure_target(b, x, x + w, skip, y, y + h, skip)); +} + +void cameras_run(MultiCameraState *s) { + // FrogPilot variables + Params paramsMemory{"/dev/shm/params"}; + const std::chrono::seconds fpsUpdateInterval(1); + std::chrono::steady_clock::time_point startTime = std::chrono::steady_clock::now(); + int frameCount = 0; + + LOG("-- Starting threads"); + std::vector threads; + if (s->driver_cam.enabled) threads.push_back(start_process_thread(s, &s->driver_cam, process_driver_camera)); + if (s->road_cam.enabled) threads.push_back(start_process_thread(s, &s->road_cam, process_road_camera)); + if (s->wide_road_cam.enabled) threads.push_back(start_process_thread(s, &s->wide_road_cam, process_road_camera)); + + // start devices + LOG("-- Starting devices"); + s->driver_cam.sensors_start(); + s->road_cam.sensors_start(); + s->wide_road_cam.sensors_start(); + + // poll events + LOG("-- Dequeueing Video events"); + while (!do_exit) { + struct pollfd fds[1] = {{0}}; + + fds[0].fd = s->video0_fd; + fds[0].events = POLLPRI; + + int ret = poll(fds, std::size(fds), 1000); + if (ret < 0) { + if (errno == EINTR || errno == EAGAIN) continue; + LOGE("poll failed (%d - %d)", ret, errno); + break; + } + + if (!fds[0].revents) continue; + + struct v4l2_event ev = {0}; + ret = HANDLE_EINTR(ioctl(fds[0].fd, VIDIOC_DQEVENT, &ev)); + if (ret == 0) { + if (ev.type == V4L_EVENT_CAM_REQ_MGR_EVENT) { + struct cam_req_mgr_message *event_data = (struct cam_req_mgr_message *)ev.u.data; + if (env_debug_frames) { + printf("sess_hdl 0x%6X, link_hdl 0x%6X, frame_id %lu, req_id %lu, timestamp %.2f ms, 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/1e6, event_data->u.frame_msg.sof_status); + } + + // for debugging + //do_exit = do_exit || event_data->u.frame_msg.frame_id > (30*20); + + frameCount++; + + std::chrono::steady_clock::time_point currentTime = std::chrono::steady_clock::now(); + if (currentTime - startTime >= fpsUpdateInterval) { + double fps = static_cast(frameCount) / std::chrono::duration(currentTime - startTime).count(); + paramsMemory.putIntNonBlocking("CameraFPS", fps / 3); + frameCount = 0; + startTime = currentTime; + } + + if (event_data->session_hdl == s->road_cam.session_handle) { + s->road_cam.handle_camera_event(event_data); + } else if (event_data->session_hdl == s->wide_road_cam.session_handle) { + s->wide_road_cam.handle_camera_event(event_data); + } else if (event_data->session_hdl == s->driver_cam.session_handle) { + s->driver_cam.handle_camera_event(event_data); + } else { + LOGE("Unknown vidioc event source"); + assert(false); + } + } else { + LOGE("unhandled event %d\n", ev.type); + } + } else { + LOGE("VIDIOC_DQEVENT failed, errno=%d", errno); + } + } + + LOG(" ************** STOPPING **************"); + + for (auto &t : threads) t.join(); + + cameras_close(s); +} diff --git a/system/camerad/cameras/camera_qcom2.h b/system/camerad/cameras/camera_qcom2.h new file mode 100644 index 0000000..47ca578 --- /dev/null +++ b/system/camerad/cameras/camera_qcom2.h @@ -0,0 +1,98 @@ +#pragma once + +#include +#include + +#include "system/camerad/cameras/camera_common.h" +#include "system/camerad/cameras/camera_util.h" +#include "system/camerad/sensors/sensor.h" +#include "common/params.h" +#include "common/util.h" + +#define FRAME_BUF_COUNT 4 + +class CameraState { +public: + MultiCameraState *multi_cam_state; + std::unique_ptr ci; + bool enabled; + + std::mutex exp_lock; + + int exposure_time; + bool dc_gain_enabled; + int dc_gain_weight; + int gain_idx; + float analog_gain_frac; + + float cur_ev[3]; + float best_ev_score; + int new_exp_g; + int new_exp_t; + + float measured_grey_fraction; + float target_grey_fraction; + + unique_fd sensor_fd; + unique_fd csiphy_fd; + + int camera_num; + + void handle_camera_event(void *evdat); + void update_exposure_score(float desired_ev, int exp_t, int exp_g_idx, float exp_gain); + void set_camera_exposure(float grey_frac); + + void sensors_start(); + + void camera_open(MultiCameraState *multi_cam_state, int camera_num, bool enabled); + void sensor_set_parameters(); + void camera_map_bufs(MultiCameraState *s); + void camera_init(MultiCameraState *s, VisionIpcServer *v, cl_device_id device_id, cl_context ctx, VisionStreamType yuv_type); + void camera_close(); + + int32_t session_handle; + int32_t sensor_dev_handle; + int32_t isp_dev_handle; + int32_t csiphy_dev_handle; + + int32_t link_handle; + + int buf0_handle; + int buf_handle[FRAME_BUF_COUNT]; + int sync_objs[FRAME_BUF_COUNT]; + int request_ids[FRAME_BUF_COUNT]; + int request_id_last; + int frame_id_last; + int idx_offset; + bool skipped; + + CameraBuf buf; + MemoryManager mm; + + void config_isp(int io_mem_handle, int fence, int request_id, int buf0_mem_handle, int buf0_offset); + void enqueue_req_multi(int start, int n, bool dp); + void enqueue_buffer(int i, bool dp); + int clear_req_queue(); + + int sensors_init(); + void sensors_poke(int request_id); + void sensors_i2c(const struct i2c_random_wr_payload* dat, int len, int op_code, bool data_word); + +private: + // for debugging + Params params; +}; + +typedef struct MultiCameraState { + unique_fd video0_fd; + unique_fd cam_sync_fd; + unique_fd isp_fd; + int device_iommu; + int cdm_iommu; + + CameraState road_cam; + CameraState wide_road_cam; + CameraState driver_cam; + + PubMaster *pm; +} MultiCameraState; diff --git a/system/camerad/cameras/camera_util.cc b/system/camerad/cameras/camera_util.cc new file mode 100644 index 0000000..4fe3899 --- /dev/null +++ b/system/camerad/cameras/camera_util.cc @@ -0,0 +1,138 @@ +#include "system/camerad/cameras/camera_util.h" + +#include + +#include + +#include +#include + +#include "common/swaglog.h" +#include "common/util.h" + +// ************** low level camera helpers **************** +int do_cam_control(int fd, int op_code, void *handle, int size) { + struct cam_control camcontrol = {0}; + camcontrol.op_code = op_code; + camcontrol.handle = (uint64_t)handle; + if (size == 0) { + camcontrol.size = 8; + camcontrol.handle_type = CAM_HANDLE_MEM_HANDLE; + } else { + camcontrol.size = size; + camcontrol.handle_type = CAM_HANDLE_USER_POINTER; + } + + int ret = HANDLE_EINTR(ioctl(fd, VIDIOC_CAM_CONTROL, &camcontrol)); + if (ret == -1) { + LOGE("VIDIOC_CAM_CONTROL error: op_code %d - errno %d", op_code, errno); + } + return ret; +} + +std::optional device_acquire(int fd, int32_t session_handle, void *data, uint32_t num_resources) { + struct cam_acquire_dev_cmd cmd = { + .session_handle = session_handle, + .handle_type = CAM_HANDLE_USER_POINTER, + .num_resources = (uint32_t)(data ? num_resources : 0), + .resource_hdl = (uint64_t)data, + }; + int err = do_cam_control(fd, CAM_ACQUIRE_DEV, &cmd, sizeof(cmd)); + return err == 0 ? std::make_optional(cmd.dev_handle) : std::nullopt; +} + +int device_config(int fd, int32_t session_handle, int32_t dev_handle, uint64_t packet_handle) { + struct cam_config_dev_cmd cmd = { + .session_handle = session_handle, + .dev_handle = dev_handle, + .packet_handle = packet_handle, + }; + return do_cam_control(fd, CAM_CONFIG_DEV, &cmd, sizeof(cmd)); +} + +int device_control(int fd, int op_code, int session_handle, int dev_handle) { + // start stop and release are all the same + struct cam_start_stop_dev_cmd cmd { .session_handle = session_handle, .dev_handle = dev_handle }; + return do_cam_control(fd, op_code, &cmd, sizeof(cmd)); +} + +void *alloc_w_mmu_hdl(int video0_fd, int len, uint32_t *handle, int align, int flags, int mmu_hdl, int mmu_hdl2) { + 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; + mem_mgr_alloc_cmd.num_hdl = 0; + if (mmu_hdl != 0) { + mem_mgr_alloc_cmd.mmu_hdls[0] = mmu_hdl; + mem_mgr_alloc_cmd.num_hdl++; + } + if (mmu_hdl2 != 0) { + mem_mgr_alloc_cmd.mmu_hdls[1] = mmu_hdl2; + mem_mgr_alloc_cmd.num_hdl++; + } + + do_cam_control(video0_fd, CAM_REQ_MGR_ALLOC_BUF, &mem_mgr_alloc_cmd, sizeof(mem_mgr_alloc_cmd)); + *handle = mem_mgr_alloc_cmd.out.buf_handle; + + void *ptr = NULL; + if (mem_mgr_alloc_cmd.out.fd > 0) { + ptr = mmap(NULL, len, PROT_READ | PROT_WRITE, MAP_SHARED, mem_mgr_alloc_cmd.out.fd, 0); + assert(ptr != MAP_FAILED); + } + + // LOGD("allocated: %x %d %llx mapped %p", mem_mgr_alloc_cmd.out.buf_handle, mem_mgr_alloc_cmd.out.fd, mem_mgr_alloc_cmd.out.vaddr, ptr); + + return ptr; +} + +void release(int video0_fd, uint32_t handle) { + int ret; + struct cam_mem_mgr_release_cmd mem_mgr_release_cmd = {0}; + mem_mgr_release_cmd.buf_handle = handle; + + ret = do_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); + release(video0_fd, handle); +} + +void *MemoryManager::alloc_buf(int size, uint32_t *handle) { + lock.lock(); + void *ptr; + if (!cached_allocations[size].empty()) { + ptr = cached_allocations[size].front(); + cached_allocations[size].pop(); + *handle = handle_lookup[ptr]; + } else { + ptr = alloc_w_mmu_hdl(video0_fd, size, handle); + handle_lookup[ptr] = *handle; + size_lookup[ptr] = size; + } + lock.unlock(); + memset(ptr, 0, size); + return ptr; +} + +void MemoryManager::free(void *ptr) { + lock.lock(); + cached_allocations[size_lookup[ptr]].push(ptr); + lock.unlock(); +} + +MemoryManager::~MemoryManager() { + for (auto& x : cached_allocations) { + while (!x.second.empty()) { + void *ptr = x.second.front(); + x.second.pop(); + LOGD("freeing cached allocation %p with size %d", ptr, size_lookup[ptr]); + munmap(ptr, size_lookup[ptr]); + release_fd(video0_fd, handle_lookup[ptr]); + handle_lookup.erase(ptr); + size_lookup.erase(ptr); + } + } +} diff --git a/system/camerad/cameras/camera_util.h b/system/camerad/cameras/camera_util.h new file mode 100644 index 0000000..891ce3e --- /dev/null +++ b/system/camerad/cameras/camera_util.h @@ -0,0 +1,39 @@ +#pragma once + +#include +#include +#include +#include +#include +#include + +#include + +std::optional device_acquire(int fd, int32_t session_handle, void *data, uint32_t num_resources=1); +int device_config(int fd, int32_t session_handle, int32_t dev_handle, uint64_t packet_handle); +int device_control(int fd, int op_code, int session_handle, int dev_handle); +int do_cam_control(int fd, int op_code, void *handle, int size); +void *alloc_w_mmu_hdl(int video0_fd, int len, uint32_t *handle, int align = 8, int flags = CAM_MEM_FLAG_KMD_ACCESS | CAM_MEM_FLAG_UMD_ACCESS | CAM_MEM_FLAG_CMD_BUF_TYPE, + int mmu_hdl = 0, int mmu_hdl2 = 0); +void release(int video0_fd, uint32_t handle); + +class MemoryManager { + public: + void init(int _video0_fd) { video0_fd = _video0_fd; } + ~MemoryManager(); + + template + auto alloc(int len, uint32_t *handle) { + return std::unique_ptr>((T*)alloc_buf(len, handle), [this](void *ptr) { this->free(ptr); }); + } + + private: + void *alloc_buf(int len, uint32_t *handle); + void free(void *ptr); + + std::mutex lock; + std::map handle_lookup; + std::map size_lookup; + std::map > cached_allocations; + int video0_fd; +}; diff --git a/system/camerad/cameras/real_debayer.cl b/system/camerad/cameras/real_debayer.cl index e15a873..5f8d046 100644 --- a/system/camerad/cameras/real_debayer.cl +++ b/system/camerad/cameras/real_debayer.cl @@ -8,7 +8,7 @@ float3 color_correct(float3 rgb) { // color correction - #if IS_OX + #if IS_OX | IS_OS float3 x = rgb.x * (float3)(1.5664815 , -0.29808738, -0.03973474); x += rgb.y * (float3)(-0.48672447, 1.41914433, -0.40295248); x += rgb.z * (float3)(-0.07975703, -0.12105695, 1.44268722); @@ -20,6 +20,8 @@ float3 color_correct(float3 rgb) { #if IS_OX return -0.507089*exp(-12.54124638*x)+0.9655*powr(x,0.5)-0.472597*x+0.507089; + #elif IS_OS + return powr(x,0.7); #else // tone mapping params const float gamma_k = 0.75; @@ -35,6 +37,9 @@ float3 color_correct(float3 rgb) { } float get_vignetting_s(float r) { + #if IS_OS + r = r / 2.2545f; + #endif if (r < 62500) { return (1.0f + 0.0000008f*r); } else if (r < 490000) { @@ -85,6 +90,24 @@ float4 val4_from_12(uchar8 pvs, float gain) { } +float4 val4_from_10(uchar8 pvs, uchar ext, bool aligned, float gain) { + uint4 parsed; + if (aligned) { + parsed = (uint4)(((uint)pvs.s0 << 2) + (pvs.s1 & 0b00000011), + ((uint)pvs.s2 << 2) + ((pvs.s6 & 0b11000000) / 64), + ((uint)pvs.s3 << 2) + ((pvs.s6 & 0b00110000) / 16), + ((uint)pvs.s4 << 2) + ((pvs.s6 & 0b00001100) / 4)); + } else { + parsed = (uint4)(((uint)pvs.s0 << 2) + ((pvs.s3 & 0b00110000) / 16), + ((uint)pvs.s1 << 2) + ((pvs.s3 & 0b00001100) / 4), + ((uint)pvs.s2 << 2) + ((pvs.s3 & 0b00000011)), + ((uint)pvs.s4 << 2) + ((ext & 0b11000000) / 64)); + } + + float4 pv = convert_float4(parsed) / 1024.0; + return clamp(pv*gain, 0.0, 1.0); +} + float get_k(float a, float b, float c, float d) { return 2.0 - (fabs(a - b) + fabs(c - d)); } @@ -94,20 +117,51 @@ __kernel void debayer10(const __global uchar * in, __global uchar * out) const int gid_x = get_global_id(0); const int gid_y = get_global_id(1); - const int y_top_mod = (gid_y == 0) ? 2: 0; - const int y_bot_mod = (gid_y == (RGB_HEIGHT/2 - 1)) ? 1: 3; + const int row_before_offset = (gid_y == 0) ? 2 : 0; + const int row_after_offset = (gid_y == (RGB_HEIGHT/2 - 1)) ? 1 : 3; float3 rgb; uchar3 rgb_out[4]; - int start = (2 * gid_y - 1) * FRAME_STRIDE + (3 * gid_x - 2) + (FRAME_STRIDE * FRAME_OFFSET); + #if IS_BGGR + constant int row_read_order[] = {3, 2, 1, 0}; + constant int rgb_write_order[] = {2, 3, 0, 1}; + #else + constant int row_read_order[] = {0, 1, 2, 3}; + constant int rgb_write_order[] = {0, 1, 2, 3}; + #endif + + int start_idx; + #if IS_10BIT + bool aligned10; + if (gid_x % 2 == 0) { + aligned10 = true; + start_idx = (2 * gid_y - 1) * FRAME_STRIDE + (5 * gid_x / 2 - 2) + (FRAME_STRIDE * FRAME_OFFSET); + } else { + aligned10 = false; + start_idx = (2 * gid_y - 1) * FRAME_STRIDE + (5 * (gid_x - 1) / 2 + 1) + (FRAME_STRIDE * FRAME_OFFSET); + } + #else + start_idx = (2 * gid_y - 1) * FRAME_STRIDE + (3 * gid_x - 2) + (FRAME_STRIDE * FRAME_OFFSET); + #endif // read in 8x4 chars uchar8 dat[4]; - dat[0] = vload8(0, in + start + FRAME_STRIDE*y_top_mod); - dat[1] = vload8(0, in + start + FRAME_STRIDE*1); - dat[2] = vload8(0, in + start + FRAME_STRIDE*2); - dat[3] = vload8(0, in + start + FRAME_STRIDE*y_bot_mod); + dat[0] = vload8(0, in + start_idx + FRAME_STRIDE*row_before_offset); + dat[1] = vload8(0, in + start_idx + FRAME_STRIDE*1); + dat[2] = vload8(0, in + start_idx + FRAME_STRIDE*2); + dat[3] = vload8(0, in + start_idx + FRAME_STRIDE*row_after_offset); + + // need extra bit for 10-bit + #if IS_10BIT + uchar extra[4]; + if (!aligned10) { + extra[0] = in[start_idx + FRAME_STRIDE*row_before_offset + 8]; + extra[1] = in[start_idx + FRAME_STRIDE*1 + 8]; + extra[2] = in[start_idx + FRAME_STRIDE*2 + 8]; + extra[3] = in[start_idx + FRAME_STRIDE*row_after_offset + 8]; + } + #endif // correct vignetting #if VIGNETTING @@ -118,60 +172,69 @@ __kernel void debayer10(const __global uchar * in, __global uchar * out) const float gain = 1.0; #endif - // process them to floats - float4 va = val4_from_12(dat[0], gain); - float4 vb = val4_from_12(dat[1], gain); - float4 vc = val4_from_12(dat[2], gain); - float4 vd = val4_from_12(dat[3], gain); + float4 v_rows[4]; + // parse into floats + #if IS_10BIT + v_rows[row_read_order[0]] = val4_from_10(dat[0], extra[0], aligned10, 1.0); + v_rows[row_read_order[1]] = val4_from_10(dat[1], extra[1], aligned10, 1.0); + v_rows[row_read_order[2]] = val4_from_10(dat[2], extra[2], aligned10, 1.0); + v_rows[row_read_order[3]] = val4_from_10(dat[3], extra[3], aligned10, 1.0); + #else + v_rows[row_read_order[0]] = val4_from_12(dat[0], gain); + v_rows[row_read_order[1]] = val4_from_12(dat[1], gain); + v_rows[row_read_order[2]] = val4_from_12(dat[2], gain); + v_rows[row_read_order[3]] = val4_from_12(dat[3], gain); + #endif + // mirror padding if (gid_x == 0) { - va.s0 = va.s2; - vb.s0 = vb.s2; - vc.s0 = vc.s2; - vd.s0 = vd.s2; + v_rows[0].s0 = v_rows[0].s2; + v_rows[1].s0 = v_rows[1].s2; + v_rows[2].s0 = v_rows[2].s2; + v_rows[3].s0 = v_rows[3].s2; } else if (gid_x == RGB_WIDTH/2 - 1) { - va.s3 = va.s1; - vb.s3 = vb.s1; - vc.s3 = vc.s1; - vd.s3 = vd.s1; + v_rows[0].s3 = v_rows[0].s1; + v_rows[1].s3 = v_rows[1].s1; + v_rows[2].s3 = v_rows[2].s1; + v_rows[3].s3 = v_rows[3].s1; } // a simplified version of https://opensignalprocessingjournal.com/contents/volumes/V6/TOSIGPJ-6-1/TOSIGPJ-6-1.pdf - const float k01 = get_k(va.s0, vb.s1, va.s2, vb.s1); - const float k02 = get_k(va.s2, vb.s1, vc.s2, vb.s1); - const float k03 = get_k(vc.s0, vb.s1, vc.s2, vb.s1); - const float k04 = get_k(va.s0, vb.s1, vc.s0, vb.s1); - rgb.x = (k02*vb.s2+k04*vb.s0)/(k02+k04); // R_G1 - rgb.y = vb.s1; // G1(R) - rgb.z = (k01*va.s1+k03*vc.s1)/(k01+k03); // B_G1 - rgb_out[0] = convert_uchar3_sat(color_correct(clamp(rgb, 0.0, 1.0)) * 255.0); + const float k01 = get_k(v_rows[0].s0, v_rows[1].s1, v_rows[0].s2, v_rows[1].s1); + const float k02 = get_k(v_rows[0].s2, v_rows[1].s1, v_rows[2].s2, v_rows[1].s1); + const float k03 = get_k(v_rows[2].s0, v_rows[1].s1, v_rows[2].s2, v_rows[1].s1); + const float k04 = get_k(v_rows[0].s0, v_rows[1].s1, v_rows[2].s0, v_rows[1].s1); + rgb.x = (k02*v_rows[1].s2+k04*v_rows[1].s0)/(k02+k04); // R_G1 + rgb.y = v_rows[1].s1; // G1(R) + rgb.z = (k01*v_rows[0].s1+k03*v_rows[2].s1)/(k01+k03); // B_G1 + rgb_out[rgb_write_order[0]] = convert_uchar3_sat(color_correct(clamp(rgb, 0.0, 1.0)) * 255.0); - const float k11 = get_k(va.s1, vc.s1, va.s3, vc.s3); - const float k12 = get_k(va.s2, vb.s1, vb.s3, vc.s2); - const float k13 = get_k(va.s1, va.s3, vc.s1, vc.s3); - const float k14 = get_k(va.s2, vb.s3, vc.s2, vb.s1); - rgb.x = vb.s2; // R - rgb.y = (k11*(va.s2+vc.s2)*0.5+k13*(vb.s3+vb.s1)*0.5)/(k11+k13); // G_R - rgb.z = (k12*(va.s3+vc.s1)*0.5+k14*(va.s1+vc.s3)*0.5)/(k12+k14); // B_R - rgb_out[1] = convert_uchar3_sat(color_correct(clamp(rgb, 0.0, 1.0)) * 255.0); + const float k11 = get_k(v_rows[0].s1, v_rows[2].s1, v_rows[0].s3, v_rows[2].s3); + const float k12 = get_k(v_rows[0].s2, v_rows[1].s1, v_rows[1].s3, v_rows[2].s2); + const float k13 = get_k(v_rows[0].s1, v_rows[0].s3, v_rows[2].s1, v_rows[2].s3); + const float k14 = get_k(v_rows[0].s2, v_rows[1].s3, v_rows[2].s2, v_rows[1].s1); + rgb.x = v_rows[1].s2; // R + rgb.y = (k11*(v_rows[0].s2+v_rows[2].s2)*0.5+k13*(v_rows[1].s3+v_rows[1].s1)*0.5)/(k11+k13); // G_R + rgb.z = (k12*(v_rows[0].s3+v_rows[2].s1)*0.5+k14*(v_rows[0].s1+v_rows[2].s3)*0.5)/(k12+k14); // B_R + rgb_out[rgb_write_order[1]] = convert_uchar3_sat(color_correct(clamp(rgb, 0.0, 1.0)) * 255.0); - const float k21 = get_k(vb.s0, vd.s0, vb.s2, vd.s2); - const float k22 = get_k(vb.s1, vc.s0, vc.s2, vd.s1); - const float k23 = get_k(vb.s0, vb.s2, vd.s0, vd.s2); - const float k24 = get_k(vb.s1, vc.s2, vd.s1, vc.s0); - rgb.x = (k22*(vb.s2+vd.s0)*0.5+k24*(vb.s0+vd.s2)*0.5)/(k22+k24); // R_B - rgb.y = (k21*(vb.s1+vd.s1)*0.5+k23*(vc.s2+vc.s0)*0.5)/(k21+k23); // G_B - rgb.z = vc.s1; // B - rgb_out[2] = convert_uchar3_sat(color_correct(clamp(rgb, 0.0, 1.0)) * 255.0); + const float k21 = get_k(v_rows[1].s0, v_rows[3].s0, v_rows[1].s2, v_rows[3].s2); + const float k22 = get_k(v_rows[1].s1, v_rows[2].s0, v_rows[2].s2, v_rows[3].s1); + const float k23 = get_k(v_rows[1].s0, v_rows[1].s2, v_rows[3].s0, v_rows[3].s2); + const float k24 = get_k(v_rows[1].s1, v_rows[2].s2, v_rows[3].s1, v_rows[2].s0); + rgb.x = (k22*(v_rows[1].s2+v_rows[3].s0)*0.5+k24*(v_rows[1].s0+v_rows[3].s2)*0.5)/(k22+k24); // R_B + rgb.y = (k21*(v_rows[1].s1+v_rows[3].s1)*0.5+k23*(v_rows[2].s2+v_rows[2].s0)*0.5)/(k21+k23); // G_B + rgb.z = v_rows[2].s1; // B + rgb_out[rgb_write_order[2]] = convert_uchar3_sat(color_correct(clamp(rgb, 0.0, 1.0)) * 255.0); - const float k31 = get_k(vb.s1, vc.s2, vb.s3, vc.s2); - const float k32 = get_k(vb.s3, vc.s2, vd.s3, vc.s2); - const float k33 = get_k(vd.s1, vc.s2, vd.s3, vc.s2); - const float k34 = get_k(vb.s1, vc.s2, vd.s1, vc.s2); - rgb.x = (k31*vb.s2+k33*vd.s2)/(k31+k33); // R_G2 - rgb.y = vc.s2; // G2(B) - rgb.z = (k32*vc.s3+k34*vc.s1)/(k32+k34); // B_G2 - rgb_out[3] = convert_uchar3_sat(color_correct(clamp(rgb, 0.0, 1.0)) * 255.0); + const float k31 = get_k(v_rows[1].s1, v_rows[2].s2, v_rows[1].s3, v_rows[2].s2); + const float k32 = get_k(v_rows[1].s3, v_rows[2].s2, v_rows[3].s3, v_rows[2].s2); + const float k33 = get_k(v_rows[3].s1, v_rows[2].s2, v_rows[3].s3, v_rows[2].s2); + const float k34 = get_k(v_rows[1].s1, v_rows[2].s2, v_rows[3].s1, v_rows[2].s2); + rgb.x = (k31*v_rows[1].s2+k33*v_rows[3].s2)/(k31+k33); // R_G2 + rgb.y = v_rows[2].s2; // G2(B) + rgb.z = (k32*v_rows[2].s3+k34*v_rows[2].s1)/(k32+k34); // B_G2 + rgb_out[rgb_write_order[3]] = convert_uchar3_sat(color_correct(clamp(rgb, 0.0, 1.0)) * 255.0); // write ys uchar2 yy = (uchar2)( diff --git a/system/camerad/main.cc b/system/camerad/main.cc new file mode 100644 index 0000000..19de21c --- /dev/null +++ b/system/camerad/main.cc @@ -0,0 +1,23 @@ +#include "system/camerad/cameras/camera_common.h" + +#include + +#include "common/params.h" +#include "common/util.h" +#include "system/hardware/hw.h" + +int main(int argc, char *argv[]) { + if (Hardware::PC()) { + printf("exiting, camerad is not meant to run on PC\n"); + return 0; + } + + int ret; + ret = util::set_realtime_priority(53); + assert(ret == 0); + ret = util::set_core_affinity({6}); + assert(ret == 0 || Params().getBool("IsOffroad")); // failure ok while offroad due to offlining cores + + camerad_thread(); + return 0; +} diff --git a/system/camerad/sensors/ar0231.cc b/system/camerad/sensors/ar0231.cc new file mode 100644 index 0000000..5c4934f --- /dev/null +++ b/system/camerad/sensors/ar0231.cc @@ -0,0 +1,171 @@ +#include + +#include "common/swaglog.h" +#include "system/camerad/cameras/camera_common.h" +#include "system/camerad/cameras/camera_qcom2.h" +#include "system/camerad/sensors/sensor.h" + +namespace { + +const size_t AR0231_REGISTERS_HEIGHT = 2; +// TODO: this extra height is universal and doesn't apply per camera +const size_t AR0231_STATS_HEIGHT = 2 + 8; + +const float sensor_analog_gains_AR0231[] = { + 1.0 / 8.0, 2.0 / 8.0, 2.0 / 7.0, 3.0 / 7.0, // 0, 1, 2, 3 + 3.0 / 6.0, 4.0 / 6.0, 4.0 / 5.0, 5.0 / 5.0, // 4, 5, 6, 7 + 5.0 / 4.0, 6.0 / 4.0, 6.0 / 3.0, 7.0 / 3.0, // 8, 9, 10, 11 + 7.0 / 2.0, 8.0 / 2.0, 8.0 / 1.0}; // 12, 13, 14, 15 = bypass + +std::map> ar0231_build_register_lut(CameraState *c, uint8_t *data) { + // This function builds a lookup table from register address, to a pair of indices in the + // buffer where to read this address. The buffer contains padding bytes, + // as well as markers to indicate the type of the next byte. + // + // 0xAA is used to indicate the MSB of the address, 0xA5 for the LSB of the address. + // Every byte of data (MSB and LSB) is preceded by 0x5A. Specifying an address is optional + // for contiguous ranges. See page 27-29 of the AR0231 Developer guide for more information. + + int max_i[] = {1828 / 2 * 3, 1500 / 2 * 3}; + auto get_next_idx = [](int cur_idx) { + return (cur_idx % 3 == 1) ? cur_idx + 2 : cur_idx + 1; // Every third byte is padding + }; + + std::map> registers; + for (int register_row = 0; register_row < 2; register_row++) { + uint8_t *registers_raw = data + c->ci->frame_stride * register_row; + assert(registers_raw[0] == 0x0a); // Start of line + + int value_tag_count = 0; + int first_val_idx = 0; + uint16_t cur_addr = 0; + + for (int i = 1; i <= max_i[register_row]; i = get_next_idx(get_next_idx(i))) { + int val_idx = get_next_idx(i); + + uint8_t tag = registers_raw[i]; + uint16_t val = registers_raw[val_idx]; + + if (tag == 0xAA) { // Register MSB tag + cur_addr = val << 8; + } else if (tag == 0xA5) { // Register LSB tag + cur_addr |= val; + cur_addr -= 2; // Next value tag will increment address again + } else if (tag == 0x5A) { // Value tag + + // First tag + if (value_tag_count % 2 == 0) { + cur_addr += 2; + first_val_idx = val_idx; + } else { + registers[cur_addr] = std::make_pair(first_val_idx + c->ci->frame_stride * register_row, val_idx + c->ci->frame_stride * register_row); + } + + value_tag_count++; + } + } + } + return registers; +} + +float ar0231_parse_temp_sensor(uint16_t calib1, uint16_t calib2, uint16_t data_reg) { + // See AR0231 Developer Guide - page 36 + float slope = (125.0 - 55.0) / ((float)calib1 - (float)calib2); + float t0 = 55.0 - slope * (float)calib2; + return t0 + slope * (float)data_reg; +} + +} // namespace + +AR0231::AR0231() { + image_sensor = cereal::FrameData::ImageSensor::AR0231; + data_word = true; + frame_width = 1928; + frame_height = 1208; + frame_stride = (frame_width * 12 / 8) + 4; + extra_height = AR0231_REGISTERS_HEIGHT + AR0231_STATS_HEIGHT; + + registers_offset = 0; + frame_offset = AR0231_REGISTERS_HEIGHT; + stats_offset = AR0231_REGISTERS_HEIGHT + frame_height; + + start_reg_array.assign(std::begin(start_reg_array_ar0231), std::end(start_reg_array_ar0231)); + init_reg_array.assign(std::begin(init_array_ar0231), std::end(init_array_ar0231)); + probe_reg_addr = 0x3000; + probe_expected_data = 0x354; + mipi_format = CAM_FORMAT_MIPI_RAW_12; + frame_data_type = 0x12; // Changing stats to 0x2C doesn't work, so change pixels to 0x12 instead + mclk_frequency = 19200000; //Hz + + dc_gain_factor = 2.5; + dc_gain_min_weight = 0; + dc_gain_max_weight = 1; + dc_gain_on_grey = 0.2; + dc_gain_off_grey = 0.3; + exposure_time_min = 2; // with HDR, fastest ss + exposure_time_max = 0x0855; // with HDR, slowest ss, 40ms + analog_gain_min_idx = 0x1; // 0.25x + analog_gain_rec_idx = 0x6; // 0.8x + analog_gain_max_idx = 0xD; // 4.0x + analog_gain_cost_delta = 0; + analog_gain_cost_low = 0.1; + analog_gain_cost_high = 5.0; + for (int i = 0; i <= analog_gain_max_idx; i++) { + sensor_analog_gains[i] = sensor_analog_gains_AR0231[i]; + } + min_ev = exposure_time_min * sensor_analog_gains[analog_gain_min_idx]; + max_ev = exposure_time_max * dc_gain_factor * sensor_analog_gains[analog_gain_max_idx]; + target_grey_factor = 1.0; +} + +void AR0231::processRegisters(CameraState *c, cereal::FrameData::Builder &framed) const { + const uint8_t expected_preamble[] = {0x0a, 0xaa, 0x55, 0x20, 0xa5, 0x55}; + uint8_t *data = (uint8_t *)c->buf.cur_camera_buf->addr + c->ci->registers_offset; + + if (memcmp(data, expected_preamble, std::size(expected_preamble)) != 0) { + LOGE("unexpected register data found"); + return; + } + + if (ar0231_register_lut.empty()) { + ar0231_register_lut = ar0231_build_register_lut(c, data); + } + std::map registers; + for (uint16_t addr : {0x2000, 0x2002, 0x20b0, 0x20b2, 0x30c6, 0x30c8, 0x30ca, 0x30cc}) { + auto offset = ar0231_register_lut[addr]; + registers[addr] = ((uint16_t)data[offset.first] << 8) | data[offset.second]; + } + + uint32_t frame_id = ((uint32_t)registers[0x2000] << 16) | registers[0x2002]; + framed.setFrameIdSensor(frame_id); + + float temp_0 = ar0231_parse_temp_sensor(registers[0x30c6], registers[0x30c8], registers[0x20b0]); + float temp_1 = ar0231_parse_temp_sensor(registers[0x30ca], registers[0x30cc], registers[0x20b2]); + framed.setTemperaturesC({temp_0, temp_1}); +} + + +std::vector AR0231::getExposureRegisters(int exposure_time, int new_exp_g, bool dc_gain_enabled) const { + uint16_t analog_gain_reg = 0xFF00 | (new_exp_g << 4) | new_exp_g; + return { + {0x3366, analog_gain_reg}, + {0x3362, (uint16_t)(dc_gain_enabled ? 0x1 : 0x0)}, + {0x3012, (uint16_t)exposure_time}, + }; +} + +int AR0231::getSlaveAddress(int port) const { + assert(port >= 0 && port <= 2); + return (int[]){0x20, 0x30, 0x20}[port]; +} + +float AR0231::getExposureScore(float desired_ev, int exp_t, int exp_g_idx, float exp_gain, int gain_idx) const { + // Cost of ev diff + float score = std::abs(desired_ev - (exp_t * exp_gain)) * 10; + // Cost of absolute gain + float m = exp_g_idx > analog_gain_rec_idx ? analog_gain_cost_high : analog_gain_cost_low; + score += std::abs(exp_g_idx - (int)analog_gain_rec_idx) * m; + // Cost of changing gain + score += std::abs(exp_g_idx - gain_idx) * (score + 1.0) / 10.0; + return score; +} diff --git a/system/camerad/sensors/ar0231_registers.h b/system/camerad/sensors/ar0231_registers.h new file mode 100644 index 0000000..6c4c251 --- /dev/null +++ b/system/camerad/sensors/ar0231_registers.h @@ -0,0 +1,119 @@ +#pragma once + +const struct i2c_random_wr_payload start_reg_array_ar0231[] = {{0x301A, 0x91C}}; +const struct i2c_random_wr_payload stop_reg_array_ar0231[] = {{0x301A, 0x918}}; + +const struct i2c_random_wr_payload init_array_ar0231[] = { + {0x301A, 0x0018}, // RESET_REGISTER + + // CLOCK Settings + // input clock is 19.2 / 2 * 0x37 = 528 MHz + // pixclk is 528 / 6 = 88 MHz + // full roll time is 1000/(PIXCLK/(LINE_LENGTH_PCK*FRAME_LENGTH_LINES)) = 39.99 ms + // img roll time is 1000/(PIXCLK/(LINE_LENGTH_PCK*Y_OUTPUT_CONTROL)) = 22.85 ms + {0x302A, 0x0006}, // VT_PIX_CLK_DIV + {0x302C, 0x0001}, // VT_SYS_CLK_DIV + {0x302E, 0x0002}, // PRE_PLL_CLK_DIV + {0x3030, 0x0037}, // PLL_MULTIPLIER + {0x3036, 0x000C}, // OP_PIX_CLK_DIV + {0x3038, 0x0001}, // OP_SYS_CLK_DIV + + // FORMAT + {0x3040, 0xC000}, // READ_MODE + {0x3004, 0x0000}, // X_ADDR_START_ + {0x3008, 0x0787}, // X_ADDR_END_ + {0x3002, 0x0000}, // Y_ADDR_START_ + {0x3006, 0x04B7}, // Y_ADDR_END_ + {0x3032, 0x0000}, // SCALING_MODE + {0x30A2, 0x0001}, // X_ODD_INC_ + {0x30A6, 0x0001}, // Y_ODD_INC_ + {0x3402, 0x0788}, // X_OUTPUT_CONTROL + {0x3404, 0x04B8}, // Y_OUTPUT_CONTROL + {0x3064, 0x1982}, // SMIA_TEST + {0x30BA, 0x11F2}, // DIGITAL_CTRL + + // Enable external trigger and disable GPIO outputs + {0x30CE, 0x0120}, // SLAVE_SH_SYNC_MODE | FRAME_START_MODE + {0x340A, 0xE0}, // GPIO3_INPUT_DISABLE | GPIO2_INPUT_DISABLE | GPIO1_INPUT_DISABLE + {0x340C, 0x802}, // GPIO_HIDRV_EN | GPIO0_ISEL=2 + + // Readout timing + {0x300C, 0x0672}, // LINE_LENGTH_PCK (valid for 3-exposure HDR) + {0x300A, 0x0855}, // FRAME_LENGTH_LINES + {0x3042, 0x0000}, // EXTRA_DELAY + + // Readout Settings + {0x31AE, 0x0204}, // SERIAL_FORMAT, 4-lane MIPI + {0x31AC, 0x0C0C}, // DATA_FORMAT_BITS, 12 -> 12 + {0x3342, 0x1212}, // MIPI_F1_PDT_EDT + {0x3346, 0x1212}, // MIPI_F2_PDT_EDT + {0x334A, 0x1212}, // MIPI_F3_PDT_EDT + {0x334E, 0x1212}, // 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, 0x0053}, // FRAME_PREAMBLE + {0x31B2, 0x003B}, // LINE_PREAMBLE + {0x301A, 0x001C}, // RESET_REGISTER + + // Noise Corrections + {0x3092, 0x0C24}, // ROW_NOISE_CONTROL + {0x337A, 0x0C80}, // DBLC_SCALE0 + {0x3370, 0x03B1}, // DBLC + {0x3044, 0x0400}, // DARK_CONTROL + + // Enable temperature sensor + {0x30B4, 0x0007}, // TEMPSENS0_CTRL_REG + {0x30B8, 0x0007}, // TEMPSENS1_CTRL_REG + + // Enable dead pixel correction using + // the 1D line correction scheme + {0x31E0, 0x0003}, + + // HDR Settings + {0x3082, 0x0004}, // OPERATION_MODE_CTRL + {0x3238, 0x0444}, // EXPOSURE_RATIO + + {0x1008, 0x0361}, // FINE_INTEGRATION_TIME_MIN + {0x100C, 0x0589}, // FINE_INTEGRATION_TIME2_MIN + {0x100E, 0x07B1}, // FINE_INTEGRATION_TIME3_MIN + {0x1010, 0x0139}, // FINE_INTEGRATION_TIME4_MIN + + // TODO: do these have to be lower than LINE_LENGTH_PCK? + {0x3014, 0x08CB}, // FINE_INTEGRATION_TIME_ + {0x321E, 0x0894}, // FINE_INTEGRATION_TIME2 + + {0x31D0, 0x0000}, // COMPANDING, no good in 10 bit? + {0x33DA, 0x0000}, // COMPANDING + {0x318E, 0x0200}, // PRE_HDR_GAIN_EN + + // DLO Settings + {0x3100, 0x4000}, // DLO_CONTROL0 + {0x3280, 0x0CCC}, // T1 G1 + {0x3282, 0x0CCC}, // T1 R + {0x3284, 0x0CCC}, // T1 B + {0x3286, 0x0CCC}, // T1 G2 + {0x3288, 0x0FA0}, // T2 G1 + {0x328A, 0x0FA0}, // T2 R + {0x328C, 0x0FA0}, // T2 B + {0x328E, 0x0FA0}, // T2 G2 + + // Initial Gains + {0x3022, 0x0001}, // GROUPED_PARAMETER_HOLD_ + {0x3366, 0xFF77}, // ANALOG_GAIN (1x) + + {0x3060, 0x3333}, // ANALOG_COLOR_GAIN + + {0x3362, 0x0000}, // DC GAIN + + {0x305A, 0x00F8}, // red gain + {0x3058, 0x0122}, // blue gain + {0x3056, 0x009A}, // g1 gain + {0x305C, 0x009A}, // g2 gain + + {0x3022, 0x0000}, // GROUPED_PARAMETER_HOLD_ + + // Initial Integration Time + {0x3012, 0x0005}, +}; diff --git a/system/camerad/sensors/os04c10.cc b/system/camerad/sensors/os04c10.cc new file mode 100644 index 0000000..aaef998 --- /dev/null +++ b/system/camerad/sensors/os04c10.cc @@ -0,0 +1,88 @@ +#include "system/camerad/sensors/sensor.h" + +namespace { + +const float sensor_analog_gains_OS04C10[] = { + 1.0, 1.0625, 1.125, 1.1875, 1.25, 1.3125, 1.375, 1.4375, 1.5, 1.5625, 1.6875, + 1.8125, 1.9375, 2.0, 2.125, 2.25, 2.375, 2.5, 2.625, 2.75, 2.875, 3.0, + 3.125, 3.375, 3.625, 3.875, 4.0, 4.25, 4.5, 4.75, 5.0, 5.25, 5.5, + 5.75, 6.0, 6.25, 6.5, 7.0, 7.5, 8.0, 8.5, 9.0, 9.5, 10.0, + 10.5, 11.0, 11.5, 12.0, 12.5, 13.0, 13.5, 14.0, 14.5, 15.0, 15.5}; + +const uint32_t os04c10_analog_gains_reg[] = { + 0x080, 0x088, 0x090, 0x098, 0x0A0, 0x0A8, 0x0B0, 0x0B8, 0x0C0, 0x0C8, 0x0D8, + 0x0E8, 0x0F8, 0x100, 0x110, 0x120, 0x130, 0x140, 0x150, 0x160, 0x170, 0x180, + 0x190, 0x1B0, 0x1D0, 0x1F0, 0x200, 0x220, 0x240, 0x260, 0x280, 0x2A0, 0x2C0, + 0x2E0, 0x300, 0x320, 0x340, 0x380, 0x3C0, 0x400, 0x440, 0x480, 0x4C0, 0x500, + 0x540, 0x580, 0x5C0, 0x600, 0x640, 0x680, 0x6C0, 0x700, 0x740, 0x780, 0x7C0}; + +} // namespace + +OS04C10::OS04C10() { + image_sensor = cereal::FrameData::ImageSensor::OS04C10; + data_word = false; + + frame_width = 2688; + frame_height = 1520; + frame_stride = (frame_width * 12 / 8); // no alignment + + extra_height = 0; + frame_offset = 0; + + start_reg_array.assign(std::begin(start_reg_array_os04c10), std::end(start_reg_array_os04c10)); + init_reg_array.assign(std::begin(init_array_os04c10), std::end(init_array_os04c10)); + probe_reg_addr = 0x300a; + probe_expected_data = 0x5304; + mipi_format = CAM_FORMAT_MIPI_RAW_12; + frame_data_type = 0x2c; + mclk_frequency = 24000000; // Hz + + dc_gain_factor = 1; + dc_gain_min_weight = 1; // always on is fine + dc_gain_max_weight = 1; + dc_gain_on_grey = 0.9; + dc_gain_off_grey = 1.0; + exposure_time_min = 2; // 1x + exposure_time_max = 2200; + analog_gain_min_idx = 0x0; + analog_gain_rec_idx = 0x0; // 1x + analog_gain_max_idx = 0x36; + analog_gain_cost_delta = -1; + analog_gain_cost_low = 0.4; + analog_gain_cost_high = 6.4; + for (int i = 0; i <= analog_gain_max_idx; i++) { + sensor_analog_gains[i] = sensor_analog_gains_OS04C10[i]; + } + min_ev = (exposure_time_min) * sensor_analog_gains[analog_gain_min_idx]; + max_ev = exposure_time_max * dc_gain_factor * sensor_analog_gains[analog_gain_max_idx]; + target_grey_factor = 0.01; +} + +std::vector OS04C10::getExposureRegisters(int exposure_time, int new_exp_g, bool dc_gain_enabled) const { + uint32_t long_time = exposure_time; + uint32_t real_gain = os04c10_analog_gains_reg[new_exp_g]; + + // uint32_t short_time = long_time > exposure_time_min*8 ? long_time / 8 : exposure_time_min; + + return { + {0x3501, long_time>>8}, {0x3502, long_time&0xFF}, + // {0x3511, short_time>>8}, {0x3512, short_time&0xFF}, + {0x3508, real_gain>>8}, {0x3509, real_gain&0xFF}, + // {0x350c, real_gain>>8}, {0x350d, real_gain&0xFF}, + }; +} + +int OS04C10::getSlaveAddress(int port) const { + assert(port >= 0 && port <= 2); + return (int[]){0x6C, 0x20, 0x6C}[port]; +} + +float OS04C10::getExposureScore(float desired_ev, int exp_t, int exp_g_idx, float exp_gain, int gain_idx) const { + float score = std::abs(desired_ev - (exp_t * exp_gain)); + float m = exp_g_idx > analog_gain_rec_idx ? analog_gain_cost_high : analog_gain_cost_low; + score += std::abs(exp_g_idx - (int)analog_gain_rec_idx) * m; + score += ((1 - analog_gain_cost_delta) + + analog_gain_cost_delta * (exp_g_idx - analog_gain_min_idx) / (analog_gain_max_idx - analog_gain_min_idx)) * + std::abs(exp_g_idx - gain_idx) * 5.0; + return score; +} diff --git a/system/camerad/sensors/os04c10_registers.h b/system/camerad/sensors/os04c10_registers.h new file mode 100644 index 0000000..f2388d9 --- /dev/null +++ b/system/camerad/sensors/os04c10_registers.h @@ -0,0 +1,313 @@ +#pragma once + +const struct i2c_random_wr_payload start_reg_array_os04c10[] = {{0x100, 1}}; +const struct i2c_random_wr_payload stop_reg_array_os04c10[] = {{0x100, 0}}; + +const struct i2c_random_wr_payload init_array_os04c10[] = { + // OS04C10_AA_00_02_17_wAO_2688x1524_MIPI728Mbps_Linear12bit_20FPS_4Lane_MCLK24MHz + {0x0103, 0x01}, + + // PLL + {0x0301, 0xe4}, + {0x0303, 0x01}, + {0x0305, 0xb6}, + {0x0306, 0x01}, + {0x0307, 0x17}, + {0x0323, 0x04}, + {0x0324, 0x01}, + {0x0325, 0x62}, + + {0x3012, 0x06}, + {0x3013, 0x02}, + {0x3016, 0x72}, + {0x3021, 0x03}, + {0x3106, 0x21}, + {0x3107, 0xa1}, + + // ? + {0x3624, 0x00}, + {0x3625, 0x4c}, + {0x3660, 0x04}, + {0x3666, 0xa5}, + {0x3667, 0xa5}, + {0x366a, 0x50}, + {0x3673, 0x0d}, + {0x3672, 0x0d}, + {0x3671, 0x0d}, + {0x3670, 0x0d}, + {0x3685, 0x00}, + {0x3694, 0x0d}, + {0x3693, 0x0d}, + {0x3692, 0x0d}, + {0x3691, 0x0d}, + {0x3696, 0x4c}, + {0x3697, 0x4c}, + {0x3698, 0x40}, + {0x3699, 0x80}, + {0x369a, 0x18}, + {0x369b, 0x1f}, + {0x369c, 0x14}, + {0x369d, 0x80}, + {0x369e, 0x40}, + {0x369f, 0x21}, + {0x36a0, 0x12}, + {0x36a1, 0x5d}, + {0x36a2, 0x66}, + {0x370a, 0x02}, + {0x370e, 0x0c}, + {0x3710, 0x00}, + {0x3713, 0x00}, + {0x3725, 0x02}, + {0x372a, 0x03}, + {0x3738, 0xce}, + {0x3748, 0x02}, + {0x374a, 0x02}, + {0x374c, 0x02}, + {0x374e, 0x02}, + {0x3756, 0x00}, + {0x3757, 0x00}, + {0x3767, 0x00}, + {0x3771, 0x00}, + {0x377b, 0x28}, + {0x377c, 0x00}, + {0x377d, 0x0c}, + {0x3781, 0x03}, + {0x3782, 0x00}, + {0x3789, 0x14}, + {0x3795, 0x02}, + {0x379c, 0x00}, + {0x379d, 0x00}, + {0x37b8, 0x04}, + {0x37ba, 0x03}, + {0x37bb, 0x00}, + {0x37bc, 0x04}, + {0x37be, 0x08}, + {0x37c4, 0x11}, + {0x37c5, 0x80}, + {0x37c6, 0x14}, + {0x37c7, 0x08}, + {0x37da, 0x11}, + {0x381f, 0x08}, + {0x3829, 0x03}, + {0x3881, 0x00}, + {0x3888, 0x04}, + {0x388b, 0x00}, + {0x3c80, 0x10}, + {0x3c86, 0x00}, + {0x3c8c, 0x20}, + {0x3c9f, 0x01}, + {0x3d85, 0x1b}, + {0x3d8c, 0x71}, + {0x3d8d, 0xe2}, + {0x3f00, 0x0b}, + {0x3f06, 0x04}, + + // BLC + {0x400a, 0x01}, + {0x400b, 0x50}, + {0x400e, 0x08}, + {0x4043, 0x7e}, + {0x4045, 0x7e}, + {0x4047, 0x7e}, + {0x4049, 0x7e}, + {0x4090, 0x04}, + {0x40b0, 0x00}, + {0x40b1, 0x00}, + {0x40b2, 0x00}, + {0x40b3, 0x00}, + {0x40b4, 0x00}, + {0x40b5, 0x00}, + {0x40b7, 0x00}, + {0x40b8, 0x00}, + {0x40b9, 0x00}, + {0x40ba, 0x01}, + + {0x4301, 0x00}, + {0x4303, 0x00}, + {0x4502, 0x04}, + {0x4503, 0x00}, + {0x4504, 0x06}, + {0x4506, 0x00}, + {0x4507, 0x47}, + {0x4803, 0x00}, + {0x480c, 0x32}, + {0x480e, 0x04}, + {0x4813, 0xe4}, + {0x4819, 0x70}, + {0x481f, 0x30}, + {0x4823, 0x3f}, + {0x4825, 0x30}, + {0x4833, 0x10}, + {0x484b, 0x27}, + {0x488b, 0x00}, + {0x4d00, 0x04}, + {0x4d01, 0xad}, + {0x4d02, 0xbc}, + {0x4d03, 0xa1}, + {0x4d04, 0x1f}, + {0x4d05, 0x4c}, + {0x4d0b, 0x01}, + {0x4e00, 0x2a}, + {0x4e0d, 0x00}, + + // ISP + {0x5001, 0x09}, + {0x5004, 0x00}, + {0x5080, 0x04}, + {0x5036, 0x80}, + {0x5180, 0x70}, + {0x5181, 0x10}, + + // DPC + {0x520a, 0x03}, + {0x520b, 0x06}, + {0x520c, 0x0c}, + + {0x580b, 0x0f}, + {0x580d, 0x00}, + {0x580f, 0x00}, + {0x5820, 0x00}, + {0x5821, 0x00}, + + {0x301c, 0xf8}, + {0x301e, 0xb4}, + {0x301f, 0xf0}, + {0x3022, 0x61}, + {0x3109, 0xe7}, + {0x3600, 0x00}, + {0x3610, 0x65}, + {0x3611, 0x85}, + {0x3613, 0x3a}, + {0x3615, 0x60}, + {0x3621, 0xb0}, + {0x3620, 0x0c}, + {0x3629, 0x00}, + {0x3661, 0x04}, + {0x3664, 0x70}, + {0x3665, 0x00}, + {0x3681, 0xa6}, + {0x3682, 0x53}, + {0x3683, 0x2a}, + {0x3684, 0x15}, + {0x3700, 0x2a}, + {0x3701, 0x12}, + {0x3703, 0x28}, + {0x3704, 0x0e}, + {0x3706, 0x9d}, + {0x3709, 0x4a}, + {0x370b, 0x48}, + {0x370c, 0x01}, + {0x370f, 0x04}, + {0x3714, 0x24}, + {0x3716, 0x04}, + {0x3719, 0x11}, + {0x371a, 0x1e}, + {0x3720, 0x00}, + {0x3724, 0x13}, + {0x373f, 0xb0}, + {0x3741, 0x9d}, + {0x3743, 0x9d}, + {0x3745, 0x9d}, + {0x3747, 0x9d}, + {0x3749, 0x48}, + {0x374b, 0x48}, + {0x374d, 0x48}, + {0x374f, 0x48}, + {0x3755, 0x10}, + {0x376c, 0x00}, + {0x378d, 0x3c}, + {0x3790, 0x01}, + {0x3791, 0x01}, + {0x3798, 0x40}, + {0x379e, 0x00}, + {0x379f, 0x04}, + {0x37a1, 0x10}, + {0x37a2, 0x1e}, + {0x37a8, 0x10}, + {0x37a9, 0x1e}, + {0x37ac, 0xa0}, + {0x37b9, 0x01}, + {0x37bd, 0x01}, + {0x37bf, 0x26}, + {0x37c0, 0x11}, + {0x37c2, 0x04}, + {0x37cd, 0x19}, + {0x37e0, 0x08}, + {0x37e6, 0x04}, + {0x37e5, 0x02}, + {0x37e1, 0x0c}, + {0x3737, 0x04}, + {0x37d8, 0x02}, + {0x37e2, 0x10}, + {0x3739, 0x10}, + {0x3662, 0x10}, + {0x37e4, 0x20}, + {0x37e3, 0x08}, + {0x37d9, 0x08}, + {0x4040, 0x00}, + {0x4041, 0x07}, + {0x4008, 0x02}, + {0x4009, 0x0d}, + + // 2704x1536 -> 2688x1520 out + {0x3800, 0x00}, {0x3801, 0x00}, + {0x3802, 0x00}, {0x3803, 0x00}, + {0x3804, 0x0a}, {0x3805, 0x8f}, + {0x3806, 0x05}, {0x3807, 0xff}, + {0x3808, 0x0a}, {0x3809, 0x80}, + {0x380a, 0x05}, {0x380b, 0xf0}, + {0x3811, 0x08}, + {0x3813, 0x08}, + {0x3814, 0x01}, + {0x3815, 0x01}, + {0x3816, 0x01}, + {0x3817, 0x01}, + + {0x380c, 0x08}, {0x380d, 0x5c}, // HTS + {0x380e, 0x09}, {0x380f, 0x38}, // VTS + + {0x3820, 0xb0}, + {0x3821, 0x00}, + {0x3880, 0x25}, + {0x3882, 0x20}, + {0x3c91, 0x0b}, + {0x3c94, 0x45}, + {0x3cad, 0x00}, + {0x3cae, 0x00}, + {0x4000, 0xf3}, + {0x4001, 0x60}, + {0x4003, 0x80}, + {0x4300, 0xff}, + {0x4302, 0x0f}, + {0x4305, 0x83}, + {0x4505, 0x84}, + {0x4809, 0x0e}, + {0x480a, 0x04}, + {0x4837, 0x15}, + {0x4c00, 0x08}, + {0x4c01, 0x08}, + {0x4c04, 0x00}, + {0x4c05, 0x00}, + {0x5000, 0xf9}, + {0x3822, 0x14}, + + // initialize exposure + {0x3503, 0x88}, + + // long + {0x3500, 0x00}, {0x3501, 0x00}, {0x3502, 0x80}, + {0x3508, 0x00}, {0x3509, 0x80}, + {0x350a, 0x04}, {0x350b, 0x00}, + + // short + // {0x3510, 0x00}, {0x3511, 0x00}, {0x3512, 0x10}, + // {0x350c, 0x00}, {0x350d, 0x80}, + // {0x350e, 0x04}, {0x350f, 0x00}, + + // wb + {0x5100, 0x06}, {0x5101, 0xcb}, + {0x5102, 0x04}, {0x5103, 0x00}, + {0x5104, 0x08}, {0x5105, 0xde}, + + {0x5106, 0x02}, {0x5107, 0x00}, +}; diff --git a/system/camerad/sensors/ox03c10.cc b/system/camerad/sensors/ox03c10.cc new file mode 100644 index 0000000..c742748 --- /dev/null +++ b/system/camerad/sensors/ox03c10.cc @@ -0,0 +1,94 @@ +#include "system/camerad/sensors/sensor.h" + +namespace { + +const float sensor_analog_gains_OX03C10[] = { + 1.0, 1.0625, 1.125, 1.1875, 1.25, 1.3125, 1.375, 1.4375, 1.5, 1.5625, 1.6875, + 1.8125, 1.9375, 2.0, 2.125, 2.25, 2.375, 2.5, 2.625, 2.75, 2.875, 3.0, + 3.125, 3.375, 3.625, 3.875, 4.0, 4.25, 4.5, 4.75, 5.0, 5.25, 5.5, + 5.75, 6.0, 6.25, 6.5, 7.0, 7.5, 8.0, 8.5, 9.0, 9.5, 10.0, + 10.5, 11.0, 11.5, 12.0, 12.5, 13.0, 13.5, 14.0, 14.5, 15.0, 15.5}; + +const uint32_t ox03c10_analog_gains_reg[] = { + 0x100, 0x110, 0x120, 0x130, 0x140, 0x150, 0x160, 0x170, 0x180, 0x190, 0x1B0, + 0x1D0, 0x1F0, 0x200, 0x220, 0x240, 0x260, 0x280, 0x2A0, 0x2C0, 0x2E0, 0x300, + 0x320, 0x360, 0x3A0, 0x3E0, 0x400, 0x440, 0x480, 0x4C0, 0x500, 0x540, 0x580, + 0x5C0, 0x600, 0x640, 0x680, 0x700, 0x780, 0x800, 0x880, 0x900, 0x980, 0xA00, + 0xA80, 0xB00, 0xB80, 0xC00, 0xC80, 0xD00, 0xD80, 0xE00, 0xE80, 0xF00, 0xF80}; + +const uint32_t VS_TIME_MIN_OX03C10 = 1; +const uint32_t VS_TIME_MAX_OX03C10 = 34; // vs < 35 + +} // namespace + +OX03C10::OX03C10() { + image_sensor = cereal::FrameData::ImageSensor::OX03C10; + data_word = false; + frame_width = 1928; + frame_height = 1208; + frame_stride = (frame_width * 12 / 8) + 4; + extra_height = 16; // top 2 + bot 14 + frame_offset = 2; + + start_reg_array.assign(std::begin(start_reg_array_ox03c10), std::end(start_reg_array_ox03c10)); + init_reg_array.assign(std::begin(init_array_ox03c10), std::end(init_array_ox03c10)); + probe_reg_addr = 0x300a; + probe_expected_data = 0x5803; + mipi_format = CAM_FORMAT_MIPI_RAW_12; + frame_data_type = 0x2c; // one is 0x2a, two are 0x2b + mclk_frequency = 24000000; //Hz + + dc_gain_factor = 7.32; + dc_gain_min_weight = 1; // always on is fine + dc_gain_max_weight = 1; + dc_gain_on_grey = 0.9; + dc_gain_off_grey = 1.0; + exposure_time_min = 2; // 1x + exposure_time_max = 2016; + analog_gain_min_idx = 0x0; + analog_gain_rec_idx = 0x0; // 1x + analog_gain_max_idx = 0x36; + analog_gain_cost_delta = -1; + analog_gain_cost_low = 0.4; + analog_gain_cost_high = 6.4; + for (int i = 0; i <= analog_gain_max_idx; i++) { + sensor_analog_gains[i] = sensor_analog_gains_OX03C10[i]; + } + min_ev = (exposure_time_min + VS_TIME_MIN_OX03C10) * sensor_analog_gains[analog_gain_min_idx]; + max_ev = exposure_time_max * dc_gain_factor * sensor_analog_gains[analog_gain_max_idx]; + target_grey_factor = 0.01; +} + +std::vector OX03C10::getExposureRegisters(int exposure_time, int new_exp_g, bool dc_gain_enabled) const { + // t_HCG&t_LCG + t_VS on LPD, t_SPD on SPD + uint32_t hcg_time = exposure_time; + uint32_t lcg_time = hcg_time; + uint32_t spd_time = std::min(std::max((uint32_t)exposure_time, (exposure_time_max + VS_TIME_MAX_OX03C10) / 3), exposure_time_max + VS_TIME_MAX_OX03C10); + uint32_t vs_time = std::min(std::max((uint32_t)exposure_time / 40, VS_TIME_MIN_OX03C10), VS_TIME_MAX_OX03C10); + + uint32_t real_gain = ox03c10_analog_gains_reg[new_exp_g]; + + return { + {0x3501, hcg_time>>8}, {0x3502, hcg_time&0xFF}, + {0x3581, lcg_time>>8}, {0x3582, lcg_time&0xFF}, + {0x3541, spd_time>>8}, {0x3542, spd_time&0xFF}, + {0x35c2, vs_time&0xFF}, + + {0x3508, real_gain>>8}, {0x3509, real_gain&0xFF}, + }; +} + +int OX03C10::getSlaveAddress(int port) const { + assert(port >= 0 && port <= 2); + return (int[]){0x6C, 0x20, 0x6C}[port]; +} + +float OX03C10::getExposureScore(float desired_ev, int exp_t, int exp_g_idx, float exp_gain, int gain_idx) const { + float score = std::abs(desired_ev - (exp_t * exp_gain)); + float m = exp_g_idx > analog_gain_rec_idx ? analog_gain_cost_high : analog_gain_cost_low; + score += std::abs(exp_g_idx - (int)analog_gain_rec_idx) * m; + score += ((1 - analog_gain_cost_delta) + + analog_gain_cost_delta * (exp_g_idx - analog_gain_min_idx) / (analog_gain_max_idx - analog_gain_min_idx)) * + std::abs(exp_g_idx - gain_idx) * 5.0; + return score; +} diff --git a/system/camerad/sensors/ox03c10_registers.h b/system/camerad/sensors/ox03c10_registers.h new file mode 100644 index 0000000..575a2cb --- /dev/null +++ b/system/camerad/sensors/ox03c10_registers.h @@ -0,0 +1,761 @@ +#pragma once + +const struct i2c_random_wr_payload start_reg_array_ox03c10[] = {{0x100, 1}}; +const struct i2c_random_wr_payload stop_reg_array_ox03c10[] = {{0x100, 0}}; + +const struct i2c_random_wr_payload init_array_ox03c10[] = { + {0x103, 1}, + {0x107, 1}, + + // X3C_1920x1280_60fps_HDR4_LFR_PWL12_mipi1200 + + // TPM + {0x4d5a, 0x1a}, {0x4d09, 0xff}, {0x4d09, 0xdf}, + + /*) + // group 4 + {0x3208, 0x04}, + {0x4620, 0x04}, + {0x3208, 0x14}, + + // group 5 + {0x3208, 0x05}, + {0x4620, 0x04}, + {0x3208, 0x15}, + + // group 2 + {0x3208, 0x02}, + {0x3507, 0x00}, + {0x3208, 0x12}, + + // delay launch group 2 + {0x3208, 0xa2},*/ + + // PLL setup + {0x0301, 0xc8}, // pll1_divs, pll1_predivp, pll1_divpix + {0x0303, 0x01}, // pll1_prediv + {0x0304, 0x01}, {0x0305, 0x2c}, // pll1_loopdiv = 300 + {0x0306, 0x04}, // pll1_divmipi = 4 + {0x0307, 0x01}, // pll1_divm = 1 + {0x0316, 0x00}, + {0x0317, 0x00}, + {0x0318, 0x00}, + {0x0323, 0x05}, // pll2_prediv + {0x0324, 0x01}, {0x0325, 0x2c}, // pll2_divp = 300 + + // SCLK/PCLK + {0x0400, 0xe0}, {0x0401, 0x80}, + {0x0403, 0xde}, {0x0404, 0x34}, + {0x0405, 0x3b}, {0x0406, 0xde}, + {0x0407, 0x08}, + {0x0408, 0xe0}, {0x0409, 0x7f}, + {0x040a, 0xde}, {0x040b, 0x34}, + {0x040c, 0x47}, {0x040d, 0xd8}, + {0x040e, 0x08}, + + // xchk + {0x2803, 0xfe}, {0x280b, 0x00}, {0x280c, 0x79}, + + // SC ctrl + {0x3001, 0x03}, // io_pad_oen + {0x3002, 0xfc}, // io_pad_oen + {0x3005, 0x80}, // io_pad_out + {0x3007, 0x01}, // io_pad_sel + {0x3008, 0x80}, // io_pad_sel + + // FSIN first frame + /* + {0x3009, 0x2}, + {0x3015, 0x2}, + {0x3822, 0x20}, + {0x3823, 0x58}, + + {0x3826, 0x0}, {0x3827, 0x8}, + {0x3881, 0x4}, + + {0x3882, 0x8}, {0x3883, 0x0D}, + {0x3836, 0x1F}, {0x3837, 0x40}, + */ + + // FSIN with external pulses + {0x3009, 0x2}, + {0x3015, 0x2}, + {0x383E, 0x80}, + {0x3881, 0x4}, + {0x3882, 0x8}, {0x3883, 0x0D}, + {0x3836, 0x1F}, {0x3837, 0x40}, + + {0x3892, 0x44}, + {0x3823, 0x48}, + + {0x3012, 0x41}, // SC_PHY_CTRL = 4 lane MIPI + {0x3020, 0x05}, // SC_CTRL_20 + + // this is not in the datasheet, listed as RSVD + // but the camera doesn't work without it + {0x3700, 0x28}, {0x3701, 0x15}, {0x3702, 0x19}, {0x3703, 0x23}, + {0x3704, 0x0a}, {0x3705, 0x00}, {0x3706, 0x3e}, {0x3707, 0x0d}, + {0x3708, 0x50}, {0x3709, 0x5a}, {0x370a, 0x00}, {0x370b, 0x96}, + {0x3711, 0x11}, {0x3712, 0x13}, {0x3717, 0x02}, {0x3718, 0x73}, + {0x372c, 0x40}, {0x3733, 0x01}, {0x3738, 0x36}, {0x3739, 0x36}, + {0x373a, 0x25}, {0x373b, 0x25}, {0x373f, 0x21}, {0x3740, 0x21}, + {0x3741, 0x21}, {0x3742, 0x21}, {0x3747, 0x28}, {0x3748, 0x28}, + {0x3749, 0x19}, {0x3755, 0x1a}, {0x3756, 0x0a}, {0x3757, 0x1c}, + {0x3765, 0x19}, {0x3766, 0x05}, {0x3767, 0x05}, {0x3768, 0x13}, + {0x376c, 0x07}, {0x3778, 0x20}, {0x377c, 0xc8}, {0x3781, 0x02}, + {0x3783, 0x02}, {0x379c, 0x58}, {0x379e, 0x00}, {0x379f, 0x00}, + {0x37a0, 0x00}, {0x37bc, 0x22}, {0x37c0, 0x01}, {0x37c4, 0x3e}, + {0x37c5, 0x3e}, {0x37c6, 0x2a}, {0x37c7, 0x28}, {0x37c8, 0x02}, + {0x37c9, 0x12}, {0x37cb, 0x29}, {0x37cd, 0x29}, {0x37d2, 0x00}, + {0x37d3, 0x73}, {0x37d6, 0x00}, {0x37d7, 0x6b}, {0x37dc, 0x00}, + {0x37df, 0x54}, {0x37e2, 0x00}, {0x37e3, 0x00}, {0x37f8, 0x00}, + {0x37f9, 0x01}, {0x37fa, 0x00}, {0x37fb, 0x19}, + + // also RSVD + {0x3c03, 0x01}, {0x3c04, 0x01}, {0x3c06, 0x21}, {0x3c08, 0x01}, + {0x3c09, 0x01}, {0x3c0a, 0x01}, {0x3c0b, 0x21}, {0x3c13, 0x21}, + {0x3c14, 0x82}, {0x3c16, 0x13}, {0x3c21, 0x00}, {0x3c22, 0xf3}, + {0x3c37, 0x12}, {0x3c38, 0x31}, {0x3c3c, 0x00}, {0x3c3d, 0x03}, + {0x3c44, 0x16}, {0x3c5c, 0x8a}, {0x3c5f, 0x03}, {0x3c61, 0x80}, + {0x3c6f, 0x2b}, {0x3c70, 0x5f}, {0x3c71, 0x2c}, {0x3c72, 0x2c}, + {0x3c73, 0x2c}, {0x3c76, 0x12}, + + // PEC checks + {0x3182, 0x12}, + + {0x320e, 0x00}, {0x320f, 0x00}, // RSVD + {0x3211, 0x61}, + {0x3215, 0xcd}, + {0x3219, 0x08}, + + {0x3506, 0x20}, {0x3507, 0x00}, // hcg fine exposure + {0x350a, 0x01}, {0x350b, 0x00}, {0x350c, 0x00}, // hcg digital gain + + {0x3586, 0x40}, {0x3587, 0x00}, // lcg fine exposure + {0x358a, 0x01}, {0x358b, 0x00}, {0x358c, 0x00}, // lcg digital gain + + {0x3546, 0x20}, {0x3547, 0x00}, // spd fine exposure + {0x354a, 0x01}, {0x354b, 0x00}, {0x354c, 0x00}, // spd digital gain + + {0x35c6, 0xb0}, {0x35c7, 0x00}, // vs fine exposure + {0x35ca, 0x01}, {0x35cb, 0x00}, {0x35cc, 0x00}, // vs digital gain + + // also RSVD + {0x3600, 0x8f}, {0x3605, 0x16}, {0x3609, 0xf0}, {0x360a, 0x01}, + {0x360e, 0x1d}, {0x360f, 0x10}, {0x3610, 0x70}, {0x3611, 0x3a}, + {0x3612, 0x28}, {0x361a, 0x29}, {0x361b, 0x6c}, {0x361c, 0x0b}, + {0x361d, 0x00}, {0x361e, 0xfc}, {0x362a, 0x00}, {0x364d, 0x0f}, + {0x364e, 0x18}, {0x364f, 0x12}, {0x3653, 0x1c}, {0x3654, 0x00}, + {0x3655, 0x1f}, {0x3656, 0x1f}, {0x3657, 0x0c}, {0x3658, 0x0a}, + {0x3659, 0x14}, {0x365a, 0x18}, {0x365b, 0x14}, {0x365c, 0x10}, + {0x365e, 0x12}, {0x3674, 0x08}, {0x3677, 0x3a}, {0x3678, 0x3a}, + {0x3679, 0x19}, + + // Y_ADDR_START = 4 + {0x3802, 0x00}, {0x3803, 0x04}, + // Y_ADDR_END = 0x50b + {0x3806, 0x05}, {0x3807, 0x0b}, + + // X_OUTPUT_SIZE = 0x780 = 1920 (changed to 1928) + {0x3808, 0x07}, {0x3809, 0x88}, + + // Y_OUTPUT_SIZE = 0x500 = 1280 (changed to 1208) + {0x380a, 0x04}, {0x380b, 0xb8}, + + // horizontal timing 0x447 + {0x380c, 0x04}, {0x380d, 0x47}, + + // rows per frame (was 0x2ae) + // 0x8ae = 53.65 ms + {0x380e, 0x08}, {0x380f, 0x15}, + // this should be triggered by FSIN, not free running + + {0x3810, 0x00}, {0x3811, 0x08}, // x cutoff + {0x3812, 0x00}, {0x3813, 0x04}, // y cutoff + {0x3816, 0x01}, + {0x3817, 0x01}, + {0x381c, 0x18}, + {0x381e, 0x01}, + {0x381f, 0x01}, + + // don't mirror, just flip + {0x3820, 0x04}, + + {0x3821, 0x19}, + {0x3832, 0xF0}, + {0x3834, 0xF0}, + {0x384c, 0x02}, + {0x384d, 0x0d}, + {0x3850, 0x00}, + {0x3851, 0x42}, + {0x3852, 0x00}, + {0x3853, 0x40}, + {0x3858, 0x04}, + {0x388c, 0x02}, + {0x388d, 0x2b}, + + // APC + {0x3b40, 0x05}, {0x3b41, 0x40}, {0x3b42, 0x00}, {0x3b43, 0x90}, + {0x3b44, 0x00}, {0x3b45, 0x20}, {0x3b46, 0x00}, {0x3b47, 0x20}, + {0x3b48, 0x19}, {0x3b49, 0x12}, {0x3b4a, 0x16}, {0x3b4b, 0x2e}, + {0x3b4c, 0x00}, {0x3b4d, 0x00}, + {0x3b86, 0x00}, {0x3b87, 0x34}, {0x3b88, 0x00}, {0x3b89, 0x08}, + {0x3b8a, 0x05}, {0x3b8b, 0x00}, {0x3b8c, 0x07}, {0x3b8d, 0x80}, + {0x3b8e, 0x00}, {0x3b8f, 0x00}, {0x3b92, 0x05}, {0x3b93, 0x00}, + {0x3b94, 0x07}, {0x3b95, 0x80}, {0x3b9e, 0x09}, + + // OTP + {0x3d82, 0x73}, + {0x3d85, 0x05}, + {0x3d8a, 0x03}, + {0x3d8b, 0xff}, + {0x3d99, 0x00}, + {0x3d9a, 0x9f}, + {0x3d9b, 0x00}, + {0x3d9c, 0xa0}, + {0x3da4, 0x00}, + {0x3da7, 0x50}, + + // DTR + {0x420e, 0x6b}, + {0x420f, 0x6e}, + {0x4210, 0x06}, + {0x4211, 0xc1}, + {0x421e, 0x02}, + {0x421f, 0x45}, + {0x4220, 0xe1}, + {0x4221, 0x01}, + {0x4301, 0xff}, + {0x4307, 0x03}, + {0x4308, 0x13}, + {0x430a, 0x13}, + {0x430d, 0x93}, + {0x430f, 0x57}, + {0x4310, 0x95}, + {0x4311, 0x16}, + {0x4316, 0x00}, + + {0x4317, 0x38}, // both embedded rows are enabled + + {0x4319, 0x03}, // spd dcg + {0x431a, 0x00}, // 8 bit mipi + {0x431b, 0x00}, + {0x431d, 0x2a}, + {0x431e, 0x11}, + + {0x431f, 0x20}, // enable PWL (pwl0_en), 12 bits + //{0x431f, 0x00}, // disable PWL + + {0x4320, 0x19}, + {0x4323, 0x80}, + {0x4324, 0x00}, + {0x4503, 0x4e}, + {0x4505, 0x00}, + {0x4509, 0x00}, + {0x450a, 0x00}, + {0x4580, 0xf8}, + {0x4583, 0x07}, + {0x4584, 0x6a}, + {0x4585, 0x08}, + {0x4586, 0x05}, + {0x4587, 0x04}, + {0x4588, 0x73}, + {0x4589, 0x05}, + {0x458a, 0x1f}, + {0x458b, 0x02}, + {0x458c, 0xdc}, + {0x458d, 0x03}, + {0x458e, 0x02}, + {0x4597, 0x07}, + {0x4598, 0x40}, + {0x4599, 0x0e}, + {0x459a, 0x0e}, + {0x459b, 0xfb}, + {0x459c, 0xf3}, + {0x4602, 0x00}, + {0x4603, 0x13}, + {0x4604, 0x00}, + {0x4609, 0x0a}, + {0x460a, 0x30}, + {0x4610, 0x00}, + {0x4611, 0x70}, + {0x4612, 0x01}, + {0x4613, 0x00}, + {0x4614, 0x00}, + {0x4615, 0x70}, + {0x4616, 0x01}, + {0x4617, 0x00}, + + {0x4800, 0x04}, // invert output PCLK + {0x480a, 0x22}, + {0x4813, 0xe4}, + + // mipi + {0x4814, 0x2a}, + {0x4837, 0x0d}, + {0x484b, 0x47}, + {0x484f, 0x00}, + {0x4887, 0x51}, + {0x4d00, 0x4a}, + {0x4d01, 0x18}, + {0x4d05, 0xff}, + {0x4d06, 0x88}, + {0x4d08, 0x63}, + {0x4d09, 0xdf}, + {0x4d15, 0x7d}, + {0x4d1a, 0x20}, + {0x4d30, 0x0a}, + {0x4d31, 0x00}, + {0x4d34, 0x7d}, + {0x4d3c, 0x7d}, + {0x4f00, 0x00}, + {0x4f01, 0x00}, + {0x4f02, 0x00}, + {0x4f03, 0x20}, + {0x4f04, 0xe0}, + {0x6a00, 0x00}, + {0x6a01, 0x20}, + {0x6a02, 0x00}, + {0x6a03, 0x20}, + {0x6a04, 0x02}, + {0x6a05, 0x80}, + {0x6a06, 0x01}, + {0x6a07, 0xe0}, + {0x6a08, 0xcf}, + {0x6a09, 0x01}, + {0x6a0a, 0x40}, + {0x6a20, 0x00}, + {0x6a21, 0x02}, + {0x6a22, 0x00}, + {0x6a23, 0x00}, + {0x6a24, 0x00}, + {0x6a25, 0x00}, + {0x6a26, 0x00}, + {0x6a27, 0x00}, + {0x6a28, 0x00}, + + // isp + {0x5000, 0x8f}, + {0x5001, 0x75}, + {0x5002, 0x7f}, // PWL0 + //{0x5002, 0x3f}, // PWL disable + {0x5003, 0x7a}, + + {0x5004, 0x3e}, + {0x5005, 0x1e}, + {0x5006, 0x1e}, + {0x5007, 0x1e}, + + {0x5008, 0x00}, + {0x500c, 0x00}, + {0x502c, 0x00}, + {0x502e, 0x00}, + {0x502f, 0x00}, + {0x504b, 0x00}, + {0x5053, 0x00}, + {0x505b, 0x00}, + {0x5063, 0x00}, + {0x5070, 0x00}, + {0x5074, 0x04}, + {0x507a, 0x04}, + {0x507b, 0x09}, + {0x5500, 0x02}, + {0x5700, 0x02}, + {0x5900, 0x02}, + {0x6007, 0x04}, + {0x6008, 0x05}, + {0x6009, 0x02}, + {0x600b, 0x08}, + {0x600c, 0x07}, + {0x600d, 0x88}, + {0x6016, 0x00}, + {0x6027, 0x04}, + {0x6028, 0x05}, + {0x6029, 0x02}, + {0x602b, 0x08}, + {0x602c, 0x07}, + {0x602d, 0x88}, + {0x6047, 0x04}, + {0x6048, 0x05}, + {0x6049, 0x02}, + {0x604b, 0x08}, + {0x604c, 0x07}, + {0x604d, 0x88}, + {0x6067, 0x04}, + {0x6068, 0x05}, + {0x6069, 0x02}, + {0x606b, 0x08}, + {0x606c, 0x07}, + {0x606d, 0x88}, + {0x6087, 0x04}, + {0x6088, 0x05}, + {0x6089, 0x02}, + {0x608b, 0x08}, + {0x608c, 0x07}, + {0x608d, 0x88}, + + // 12-bit PWL0 + {0x5e00, 0x00}, + + // m_ndX_exp[0:32] + // 9*2+0xa*3+0xb*2+0xc*2+0xd*2+0xe*2+0xf*2+0x10*2+0x11*2+0x12*4+0x13*3+0x14*3+0x15*3+0x16 = 518 + {0x5e01, 0x09}, + {0x5e02, 0x09}, + {0x5e03, 0x0a}, + {0x5e04, 0x0a}, + {0x5e05, 0x0a}, + {0x5e06, 0x0b}, + {0x5e07, 0x0b}, + {0x5e08, 0x0c}, + {0x5e09, 0x0c}, + {0x5e0a, 0x0d}, + {0x5e0b, 0x0d}, + {0x5e0c, 0x0e}, + {0x5e0d, 0x0e}, + {0x5e0e, 0x0f}, + {0x5e0f, 0x0f}, + {0x5e10, 0x10}, + {0x5e11, 0x10}, + {0x5e12, 0x11}, + {0x5e13, 0x11}, + {0x5e14, 0x12}, + {0x5e15, 0x12}, + {0x5e16, 0x12}, + {0x5e17, 0x12}, + {0x5e18, 0x13}, + {0x5e19, 0x13}, + {0x5e1a, 0x13}, + {0x5e1b, 0x14}, + {0x5e1c, 0x14}, + {0x5e1d, 0x14}, + {0x5e1e, 0x15}, + {0x5e1f, 0x15}, + {0x5e20, 0x15}, + {0x5e21, 0x16}, + + // m_ndY_val[0:32] + // 0x200+0xff+0x100*3+0x80*12+0x40*16 = 4095 + {0x5e22, 0x00}, {0x5e23, 0x02}, {0x5e24, 0x00}, + {0x5e25, 0x00}, {0x5e26, 0x00}, {0x5e27, 0xff}, + {0x5e28, 0x00}, {0x5e29, 0x01}, {0x5e2a, 0x00}, + {0x5e2b, 0x00}, {0x5e2c, 0x01}, {0x5e2d, 0x00}, + {0x5e2e, 0x00}, {0x5e2f, 0x01}, {0x5e30, 0x00}, + {0x5e31, 0x00}, {0x5e32, 0x00}, {0x5e33, 0x80}, + {0x5e34, 0x00}, {0x5e35, 0x00}, {0x5e36, 0x80}, + {0x5e37, 0x00}, {0x5e38, 0x00}, {0x5e39, 0x80}, + {0x5e3a, 0x00}, {0x5e3b, 0x00}, {0x5e3c, 0x80}, + {0x5e3d, 0x00}, {0x5e3e, 0x00}, {0x5e3f, 0x80}, + {0x5e40, 0x00}, {0x5e41, 0x00}, {0x5e42, 0x80}, + {0x5e43, 0x00}, {0x5e44, 0x00}, {0x5e45, 0x80}, + {0x5e46, 0x00}, {0x5e47, 0x00}, {0x5e48, 0x80}, + {0x5e49, 0x00}, {0x5e4a, 0x00}, {0x5e4b, 0x80}, + {0x5e4c, 0x00}, {0x5e4d, 0x00}, {0x5e4e, 0x80}, + {0x5e4f, 0x00}, {0x5e50, 0x00}, {0x5e51, 0x80}, + {0x5e52, 0x00}, {0x5e53, 0x00}, {0x5e54, 0x80}, + {0x5e55, 0x00}, {0x5e56, 0x00}, {0x5e57, 0x40}, + {0x5e58, 0x00}, {0x5e59, 0x00}, {0x5e5a, 0x40}, + {0x5e5b, 0x00}, {0x5e5c, 0x00}, {0x5e5d, 0x40}, + {0x5e5e, 0x00}, {0x5e5f, 0x00}, {0x5e60, 0x40}, + {0x5e61, 0x00}, {0x5e62, 0x00}, {0x5e63, 0x40}, + {0x5e64, 0x00}, {0x5e65, 0x00}, {0x5e66, 0x40}, + {0x5e67, 0x00}, {0x5e68, 0x00}, {0x5e69, 0x40}, + {0x5e6a, 0x00}, {0x5e6b, 0x00}, {0x5e6c, 0x40}, + {0x5e6d, 0x00}, {0x5e6e, 0x00}, {0x5e6f, 0x40}, + {0x5e70, 0x00}, {0x5e71, 0x00}, {0x5e72, 0x40}, + {0x5e73, 0x00}, {0x5e74, 0x00}, {0x5e75, 0x40}, + {0x5e76, 0x00}, {0x5e77, 0x00}, {0x5e78, 0x40}, + {0x5e79, 0x00}, {0x5e7a, 0x00}, {0x5e7b, 0x40}, + {0x5e7c, 0x00}, {0x5e7d, 0x00}, {0x5e7e, 0x40}, + {0x5e7f, 0x00}, {0x5e80, 0x00}, {0x5e81, 0x40}, + {0x5e82, 0x00}, {0x5e83, 0x00}, {0x5e84, 0x40}, + + // disable PWL + /*{0x5e01, 0x18}, {0x5e02, 0x00}, {0x5e03, 0x00}, {0x5e04, 0x00}, + {0x5e05, 0x00}, {0x5e06, 0x00}, {0x5e07, 0x00}, {0x5e08, 0x00}, + {0x5e09, 0x00}, {0x5e0a, 0x00}, {0x5e0b, 0x00}, {0x5e0c, 0x00}, + {0x5e0d, 0x00}, {0x5e0e, 0x00}, {0x5e0f, 0x00}, {0x5e10, 0x00}, + {0x5e11, 0x00}, {0x5e12, 0x00}, {0x5e13, 0x00}, {0x5e14, 0x00}, + {0x5e15, 0x00}, {0x5e16, 0x00}, {0x5e17, 0x00}, {0x5e18, 0x00}, + {0x5e19, 0x00}, {0x5e1a, 0x00}, {0x5e1b, 0x00}, {0x5e1c, 0x00}, + {0x5e1d, 0x00}, {0x5e1e, 0x00}, {0x5e1f, 0x00}, {0x5e20, 0x00}, + {0x5e21, 0x00}, + + {0x5e22, 0x00}, {0x5e23, 0x0f}, {0x5e24, 0xFF},*/ + + {0x4001, 0x2b}, // BLC_CTRL_1 + {0x4008, 0x02}, {0x4009, 0x03}, + {0x4018, 0x12}, + {0x4022, 0x40}, + {0x4023, 0x20}, + + // all black level targets are 0x40 + {0x4026, 0x00}, {0x4027, 0x40}, + {0x4028, 0x00}, {0x4029, 0x40}, + {0x402a, 0x00}, {0x402b, 0x40}, + {0x402c, 0x00}, {0x402d, 0x40}, + + {0x407e, 0xcc}, + {0x407f, 0x18}, + {0x4080, 0xff}, + {0x4081, 0xff}, + {0x4082, 0x01}, + {0x4083, 0x53}, + {0x4084, 0x01}, + {0x4085, 0x2b}, + {0x4086, 0x00}, + {0x4087, 0xb3}, + + {0x4640, 0x40}, + {0x4641, 0x11}, + {0x4642, 0x0e}, + {0x4643, 0xee}, + {0x4646, 0x0f}, + {0x4648, 0x00}, + {0x4649, 0x03}, + + {0x4f00, 0x00}, + {0x4f01, 0x00}, + {0x4f02, 0x80}, + {0x4f03, 0x2c}, + {0x4f04, 0xf8}, + + {0x4d09, 0xff}, + {0x4d09, 0xdf}, + + {0x5003, 0x7a}, + {0x5b80, 0x08}, + {0x5c00, 0x08}, + {0x5c80, 0x00}, + {0x5bbe, 0x12}, + {0x5c3e, 0x12}, + {0x5cbe, 0x12}, + {0x5b8a, 0x80}, + {0x5b8b, 0x80}, + {0x5b8c, 0x80}, + {0x5b8d, 0x80}, + {0x5b8e, 0x60}, + {0x5b8f, 0x80}, + {0x5b90, 0x80}, + {0x5b91, 0x80}, + {0x5b92, 0x80}, + {0x5b93, 0x20}, + {0x5b94, 0x80}, + {0x5b95, 0x80}, + {0x5b96, 0x80}, + {0x5b97, 0x20}, + {0x5b98, 0x00}, + {0x5b99, 0x80}, + {0x5b9a, 0x40}, + {0x5b9b, 0x20}, + {0x5b9c, 0x00}, + {0x5b9d, 0x00}, + {0x5b9e, 0x80}, + {0x5b9f, 0x00}, + {0x5ba0, 0x00}, + {0x5ba1, 0x00}, + {0x5ba2, 0x00}, + {0x5ba3, 0x00}, + {0x5ba4, 0x00}, + {0x5ba5, 0x00}, + {0x5ba6, 0x00}, + {0x5ba7, 0x00}, + {0x5ba8, 0x02}, + {0x5ba9, 0x00}, + {0x5baa, 0x02}, + {0x5bab, 0x76}, + {0x5bac, 0x03}, + {0x5bad, 0x08}, + {0x5bae, 0x00}, + {0x5baf, 0x80}, + {0x5bb0, 0x00}, + {0x5bb1, 0xc0}, + {0x5bb2, 0x01}, + {0x5bb3, 0x00}, + + // m_nNormCombineWeight + {0x5c0a, 0x80}, {0x5c0b, 0x80}, {0x5c0c, 0x80}, {0x5c0d, 0x80}, {0x5c0e, 0x60}, + {0x5c0f, 0x80}, {0x5c10, 0x80}, {0x5c11, 0x80}, {0x5c12, 0x60}, {0x5c13, 0x20}, + {0x5c14, 0x80}, {0x5c15, 0x80}, {0x5c16, 0x80}, {0x5c17, 0x20}, {0x5c18, 0x00}, + {0x5c19, 0x80}, {0x5c1a, 0x40}, {0x5c1b, 0x20}, {0x5c1c, 0x00}, {0x5c1d, 0x00}, + {0x5c1e, 0x80}, {0x5c1f, 0x00}, {0x5c20, 0x00}, {0x5c21, 0x00}, {0x5c22, 0x00}, + {0x5c23, 0x00}, {0x5c24, 0x00}, {0x5c25, 0x00}, {0x5c26, 0x00}, {0x5c27, 0x00}, + + // m_nCombinThreL + {0x5c28, 0x02}, {0x5c29, 0x00}, + {0x5c2a, 0x02}, {0x5c2b, 0x76}, + {0x5c2c, 0x03}, {0x5c2d, 0x08}, + + // m_nCombinThreS + {0x5c2e, 0x00}, {0x5c2f, 0x80}, + {0x5c30, 0x00}, {0x5c31, 0xc0}, + {0x5c32, 0x01}, {0x5c33, 0x00}, + + // m_nNormCombineWeight + {0x5c8a, 0x80}, {0x5c8b, 0x80}, {0x5c8c, 0x80}, {0x5c8d, 0x80}, {0x5c8e, 0x80}, + {0x5c8f, 0x80}, {0x5c90, 0x80}, {0x5c91, 0x80}, {0x5c92, 0x80}, {0x5c93, 0x60}, + {0x5c94, 0x80}, {0x5c95, 0x80}, {0x5c96, 0x80}, {0x5c97, 0x60}, {0x5c98, 0x40}, + {0x5c99, 0x80}, {0x5c9a, 0x80}, {0x5c9b, 0x80}, {0x5c9c, 0x40}, {0x5c9d, 0x00}, + {0x5c9e, 0x80}, {0x5c9f, 0x80}, {0x5ca0, 0x80}, {0x5ca1, 0x20}, {0x5ca2, 0x00}, + {0x5ca3, 0x80}, {0x5ca4, 0x80}, {0x5ca5, 0x00}, {0x5ca6, 0x00}, {0x5ca7, 0x00}, + + {0x5ca8, 0x01}, {0x5ca9, 0x00}, + {0x5caa, 0x02}, {0x5cab, 0x00}, + {0x5cac, 0x03}, {0x5cad, 0x08}, + + {0x5cae, 0x01}, {0x5caf, 0x00}, + {0x5cb0, 0x02}, {0x5cb1, 0x00}, + {0x5cb2, 0x03}, {0x5cb3, 0x08}, + + // combine ISP + {0x5be7, 0x80}, + {0x5bc9, 0x80}, + {0x5bca, 0x80}, + {0x5bcb, 0x80}, + {0x5bcc, 0x80}, + {0x5bcd, 0x80}, + {0x5bce, 0x80}, + {0x5bcf, 0x80}, + {0x5bd0, 0x80}, + {0x5bd1, 0x80}, + {0x5bd2, 0x20}, + {0x5bd3, 0x80}, + {0x5bd4, 0x40}, + {0x5bd5, 0x20}, + {0x5bd6, 0x00}, + {0x5bd7, 0x00}, + {0x5bd8, 0x00}, + {0x5bd9, 0x00}, + {0x5bda, 0x00}, + {0x5bdb, 0x00}, + {0x5bdc, 0x00}, + {0x5bdd, 0x00}, + {0x5bde, 0x00}, + {0x5bdf, 0x00}, + {0x5be0, 0x00}, + {0x5be1, 0x00}, + {0x5be2, 0x00}, + {0x5be3, 0x00}, + {0x5be4, 0x00}, + {0x5be5, 0x00}, + {0x5be6, 0x00}, + + // m_nSPDCombineWeight + {0x5c49, 0x80}, {0x5c4a, 0x80}, {0x5c4b, 0x80}, {0x5c4c, 0x80}, {0x5c4d, 0x40}, + {0x5c4e, 0x80}, {0x5c4f, 0x80}, {0x5c50, 0x80}, {0x5c51, 0x60}, {0x5c52, 0x20}, + {0x5c53, 0x80}, {0x5c54, 0x80}, {0x5c55, 0x80}, {0x5c56, 0x20}, {0x5c57, 0x00}, + {0x5c58, 0x80}, {0x5c59, 0x40}, {0x5c5a, 0x20}, {0x5c5b, 0x00}, {0x5c5c, 0x00}, + {0x5c5d, 0x80}, {0x5c5e, 0x00}, {0x5c5f, 0x00}, {0x5c60, 0x00}, {0x5c61, 0x00}, + {0x5c62, 0x00}, {0x5c63, 0x00}, {0x5c64, 0x00}, {0x5c65, 0x00}, {0x5c66, 0x00}, + + // m_nSPDCombineWeight + {0x5cc9, 0x80}, {0x5cca, 0x80}, {0x5ccb, 0x80}, {0x5ccc, 0x80}, {0x5ccd, 0x80}, + {0x5cce, 0x80}, {0x5ccf, 0x80}, {0x5cd0, 0x80}, {0x5cd1, 0x80}, {0x5cd2, 0x60}, + {0x5cd3, 0x80}, {0x5cd4, 0x80}, {0x5cd5, 0x80}, {0x5cd6, 0x60}, {0x5cd7, 0x40}, + {0x5cd8, 0x80}, {0x5cd9, 0x80}, {0x5cda, 0x80}, {0x5cdb, 0x40}, {0x5cdc, 0x20}, + {0x5cdd, 0x80}, {0x5cde, 0x80}, {0x5cdf, 0x80}, {0x5ce0, 0x20}, {0x5ce1, 0x00}, + {0x5ce2, 0x80}, {0x5ce3, 0x80}, {0x5ce4, 0x80}, {0x5ce5, 0x00}, {0x5ce6, 0x00}, + + {0x5d74, 0x01}, + {0x5d75, 0x00}, + + {0x5d1f, 0x81}, + {0x5d11, 0x00}, + {0x5d12, 0x10}, + {0x5d13, 0x10}, + {0x5d15, 0x05}, + {0x5d16, 0x05}, + {0x5d17, 0x05}, + {0x5d08, 0x03}, + {0x5d09, 0xb6}, + {0x5d0a, 0x03}, + {0x5d0b, 0xb6}, + {0x5d18, 0x03}, + {0x5d19, 0xb6}, + {0x5d62, 0x01}, + {0x5d40, 0x02}, + {0x5d41, 0x01}, + {0x5d63, 0x1f}, + {0x5d64, 0x00}, + {0x5d65, 0x80}, + {0x5d56, 0x00}, + {0x5d57, 0x20}, + {0x5d58, 0x00}, + {0x5d59, 0x20}, + {0x5d5a, 0x00}, + {0x5d5b, 0x0c}, + {0x5d5c, 0x02}, + {0x5d5d, 0x40}, + {0x5d5e, 0x02}, + {0x5d5f, 0x40}, + {0x5d60, 0x03}, + {0x5d61, 0x40}, + {0x5d4a, 0x02}, + {0x5d4b, 0x40}, + {0x5d4c, 0x02}, + {0x5d4d, 0x40}, + {0x5d4e, 0x02}, + {0x5d4f, 0x40}, + {0x5d50, 0x18}, + {0x5d51, 0x80}, + {0x5d52, 0x18}, + {0x5d53, 0x80}, + {0x5d54, 0x18}, + {0x5d55, 0x80}, + {0x5d46, 0x20}, + {0x5d47, 0x00}, + {0x5d48, 0x22}, + {0x5d49, 0x00}, + {0x5d42, 0x20}, + {0x5d43, 0x00}, + {0x5d44, 0x22}, + {0x5d45, 0x00}, + + {0x5004, 0x1e}, + {0x4221, 0x03}, // this is changed from 1 -> 3 + + // DCG exposure coarse + // {0x3501, 0x01}, {0x3502, 0xc8}, + // SPD exposure coarse + // {0x3541, 0x01}, {0x3542, 0xc8}, + // VS exposure coarse + // {0x35c1, 0x00}, {0x35c2, 0x01}, + + // crc reference + {0x420e, 0x66}, {0x420f, 0x5d}, {0x4210, 0xa8}, {0x4211, 0x55}, + // crc stat check + {0x507a, 0x5f}, {0x507b, 0x46}, + + // watchdog control + {0x4f00, 0x00}, {0x4f01, 0x01}, {0x4f02, 0x80}, {0x4f04, 0x2c}, + + // color balance gains + // blue + {0x5280, 0x06}, {0x5281, 0xCB}, // hcg + {0x5480, 0x06}, {0x5481, 0xCB}, // lcg + {0x5680, 0x06}, {0x5681, 0xCB}, // spd + {0x5880, 0x06}, {0x5881, 0xCB}, // vs + + // green(blue) + {0x5282, 0x04}, {0x5283, 0x00}, + {0x5482, 0x04}, {0x5483, 0x00}, + {0x5682, 0x04}, {0x5683, 0x00}, + {0x5882, 0x04}, {0x5883, 0x00}, + + // green(red) + {0x5284, 0x04}, {0x5285, 0x00}, + {0x5484, 0x04}, {0x5485, 0x00}, + {0x5684, 0x04}, {0x5685, 0x00}, + {0x5884, 0x04}, {0x5885, 0x00}, + + // red + {0x5286, 0x08}, {0x5287, 0xDE}, + {0x5486, 0x08}, {0x5487, 0xDE}, + {0x5686, 0x08}, {0x5687, 0xDE}, + {0x5886, 0x08}, {0x5887, 0xDE}, + + // fixed gains + {0x3588, 0x01}, {0x3589, 0x00}, + {0x35c8, 0x01}, {0x35c9, 0x00}, + {0x3548, 0x0F}, {0x3549, 0x00}, + {0x35c1, 0x00}, +}; diff --git a/system/camerad/sensors/sensor.h b/system/camerad/sensors/sensor.h new file mode 100644 index 0000000..d97fd32 --- /dev/null +++ b/system/camerad/sensors/sensor.h @@ -0,0 +1,89 @@ +#pragma once + +#include +#include +#include +#include +#include +#include "media/cam_sensor.h" +#include "system/camerad/cameras/camera_common.h" +#include "system/camerad/sensors/ar0231_registers.h" +#include "system/camerad/sensors/ox03c10_registers.h" +#include "system/camerad/sensors/os04c10_registers.h" + +#define ANALOG_GAIN_MAX_CNT 55 + +class SensorInfo { +public: + SensorInfo() = default; + virtual std::vector getExposureRegisters(int exposure_time, int new_exp_g, bool dc_gain_enabled) const { return {}; } + virtual float getExposureScore(float desired_ev, int exp_t, int exp_g_idx, float exp_gain, int gain_idx) const {return 0; } + virtual int getSlaveAddress(int port) const { assert(0); } + virtual void processRegisters(CameraState *c, cereal::FrameData::Builder &framed) const {} + + cereal::FrameData::ImageSensor image_sensor = cereal::FrameData::ImageSensor::UNKNOWN; + uint32_t frame_width, frame_height; + uint32_t frame_stride; + uint32_t frame_offset = 0; + uint32_t extra_height = 0; + int registers_offset = -1; + int stats_offset = -1; + + int exposure_time_min; + int exposure_time_max; + + float dc_gain_factor; + int dc_gain_min_weight; + int dc_gain_max_weight; + float dc_gain_on_grey; + float dc_gain_off_grey; + + float sensor_analog_gains[ANALOG_GAIN_MAX_CNT]; + int analog_gain_min_idx; + int analog_gain_max_idx; + int analog_gain_rec_idx; + int analog_gain_cost_delta; + float analog_gain_cost_low; + float analog_gain_cost_high; + float target_grey_factor; + float min_ev; + float max_ev; + + bool data_word; + uint32_t probe_reg_addr; + uint32_t probe_expected_data; + std::vector start_reg_array; + std::vector init_reg_array; + + uint32_t mipi_format; + uint32_t mclk_frequency; + uint32_t frame_data_type; +}; + +class AR0231 : public SensorInfo { +public: + AR0231(); + std::vector getExposureRegisters(int exposure_time, int new_exp_g, bool dc_gain_enabled) const override; + float getExposureScore(float desired_ev, int exp_t, int exp_g_idx, float exp_gain, int gain_idx) const override; + int getSlaveAddress(int port) const override; + void processRegisters(CameraState *c, cereal::FrameData::Builder &framed) const override; + +private: + mutable std::map> ar0231_register_lut; +}; + +class OX03C10 : public SensorInfo { +public: + OX03C10(); + std::vector getExposureRegisters(int exposure_time, int new_exp_g, bool dc_gain_enabled) const override; + float getExposureScore(float desired_ev, int exp_t, int exp_g_idx, float exp_gain, int gain_idx) const override; + int getSlaveAddress(int port) const override; +}; + +class OS04C10 : public SensorInfo { +public: + OS04C10(); + std::vector getExposureRegisters(int exposure_time, int new_exp_g, bool dc_gain_enabled) const override; + float getExposureScore(float desired_ev, int exp_t, int exp_g_idx, float exp_gain, int gain_idx) const override; + int getSlaveAddress(int port) const override; +}; diff --git a/system/camerad/test/.gitignore b/system/camerad/test/.gitignore new file mode 100644 index 0000000..d67473e --- /dev/null +++ b/system/camerad/test/.gitignore @@ -0,0 +1,2 @@ +jpegs/ +test_ae_gray diff --git a/system/camerad/test/check_skips.py b/system/camerad/test/check_skips.py new file mode 100644 index 0000000..0814ce4 --- /dev/null +++ b/system/camerad/test/check_skips.py @@ -0,0 +1,27 @@ +#!/usr/bin/env python3 +# type: ignore +import cereal.messaging as messaging + +all_sockets = ['roadCameraState', 'driverCameraState', 'wideRoadCameraState'] +prev_id = [None,None,None] +this_id = [None,None,None] +dt = [None,None,None] +num_skipped = [0,0,0] + +if __name__ == "__main__": + sm = messaging.SubMaster(all_sockets) + while True: + sm.update() + + for i in range(len(all_sockets)): + if not sm.updated[all_sockets[i]]: + continue + this_id[i] = sm[all_sockets[i]].frameId + if prev_id[i] is None: + prev_id[i] = this_id[i] + continue + dt[i] = this_id[i] - prev_id[i] + if dt[i] != 1: + num_skipped[i] += dt[i] - 1 + print(all_sockets[i] ,dt[i] - 1, num_skipped[i]) + prev_id[i] = this_id[i] diff --git a/system/camerad/test/get_thumbnails_for_segment.py b/system/camerad/test/get_thumbnails_for_segment.py new file mode 100644 index 0000000..21409f3 --- /dev/null +++ b/system/camerad/test/get_thumbnails_for_segment.py @@ -0,0 +1,24 @@ +#!/usr/bin/env python3 +import argparse +import os +from tqdm import tqdm + +from openpilot.tools.lib.logreader import LogReader + +if __name__ == "__main__": + parser = argparse.ArgumentParser() + parser.add_argument("route", help="The route name") + args = parser.parse_args() + + out_path = os.path.join("jpegs", f"{args.route.replace('|', '_').replace('/', '_')}") + os.makedirs(out_path, exist_ok=True) + + lr = LogReader(args.route) + + for msg in tqdm(lr): + if msg.which() == 'thumbnail': + with open(os.path.join(out_path, f"{msg.thumbnail.frameId}.jpg"), 'wb') as f: + f.write(msg.thumbnail.thumbnail) + elif msg.which() == 'navThumbnail': + with open(os.path.join(out_path, f"nav_{msg.navThumbnail.frameId}.jpg"), 'wb') as f: + f.write(msg.navThumbnail.thumbnail) diff --git a/system/camerad/test/stress_restart.sh b/system/camerad/test/stress_restart.sh new file mode 100644 index 0000000..0445dcb --- /dev/null +++ b/system/camerad/test/stress_restart.sh @@ -0,0 +1,9 @@ +#!/bin/sh +cd .. +while :; do + ./camerad & + pid="$!" + sleep 2 + kill -2 $pid + wait $pid +done diff --git a/system/camerad/test/test_ae_gray.cc b/system/camerad/test/test_ae_gray.cc new file mode 100644 index 0000000..06d7849 --- /dev/null +++ b/system/camerad/test/test_ae_gray.cc @@ -0,0 +1,83 @@ +#define CATCH_CONFIG_MAIN +#include "catch2/catch.hpp" + +#include + +#include +#include + +#include "common/util.h" +#include "system/camerad/cameras/camera_common.h" + +#define W 240 +#define H 160 + + +#define TONE_SPLITS 3 + +float gts[TONE_SPLITS * TONE_SPLITS * TONE_SPLITS * TONE_SPLITS] = { + 0.917969, 0.917969, 0.375000, 0.917969, 0.375000, 0.375000, 0.187500, 0.187500, 0.187500, 0.917969, + 0.375000, 0.375000, 0.187500, 0.187500, 0.187500, 0.187500, 0.187500, 0.187500, 0.093750, 0.093750, + 0.093750, 0.093750, 0.093750, 0.093750, 0.093750, 0.093750, 0.093750, 0.917969, 0.375000, 0.375000, + 0.187500, 0.187500, 0.187500, 0.187500, 0.187500, 0.187500, 0.093750, 0.093750, 0.093750, 0.093750, + 0.093750, 0.093750, 0.093750, 0.093750, 0.093750, 0.093750, 0.093750, 0.093750, 0.093750, 0.093750, + 0.093750, 0.093750, 0.093750, 0.093750, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, + 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, + 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, + 0.000000}; + + +TEST_CASE("camera.test_set_exposure_target") { + // set up fake camerabuf + CameraBuf cb = {}; + VisionBuf vb = {}; + uint8_t * fb_y = new uint8_t[W*H]; + vb.y = fb_y; + cb.cur_yuv_buf = &vb; + cb.rgb_width = W; + cb.rgb_height = H; + + printf("AE test patterns %dx%d\n", cb.rgb_width, cb.rgb_height); + + // mix of 5 tones + uint8_t l[5] = {0, 24, 48, 96, 235}; // 235 is yuv max + + bool passed = true; + float rtol = 0.05; + // generate pattern and calculate EV + int cnt = 0; + for (int i_0=0; i_0 rtol*evgt) { + passed = false; + } + + // report + printf("%d/%d/%d/%d/%d: ev %f, gt %f, err %f\n", h_0, h_1, h_2, h_3, h_4, ev, evgt, fabs(ev - evgt) / (evgt != 0 ? evgt : 0.00001f)); + cnt++; + } + } + } + } + assert(passed); + + delete[] fb_y; +} diff --git a/system/camerad/test/test_camerad.py b/system/camerad/test/test_camerad.py new file mode 100644 index 0000000..4081156 --- /dev/null +++ b/system/camerad/test/test_camerad.py @@ -0,0 +1,83 @@ +#!/usr/bin/env python3 +import pytest +import time +import numpy as np +from flaky import flaky +from collections import defaultdict + +import cereal.messaging as messaging +from cereal import log +from cereal.services import SERVICE_LIST +from openpilot.selfdrive.manager.process_config import managed_processes + +TEST_TIMESPAN = 30 +LAG_FRAME_TOLERANCE = {log.FrameData.ImageSensor.ar0231: 0.5, # ARs use synced pulses for frame starts + log.FrameData.ImageSensor.ox03c10: 1.1} # OXs react to out-of-sync at next frame +FRAME_DELTA_TOLERANCE = {log.FrameData.ImageSensor.ar0231: 1.0, + log.FrameData.ImageSensor.ox03c10: 1.0} + +CAMERAS = ('roadCameraState', 'driverCameraState', 'wideRoadCameraState') + +# TODO: this shouldn't be needed +@flaky(max_runs=3) +@pytest.mark.tici +class TestCamerad: + def setup_method(self): + # run camerad and record logs + managed_processes['camerad'].start() + time.sleep(3) + socks = {c: messaging.sub_sock(c, conflate=False, timeout=100) for c in CAMERAS} + + self.logs = defaultdict(list) + start_time = time.monotonic() + while time.monotonic()- start_time < TEST_TIMESPAN: + for cam, s in socks.items(): + self.logs[cam] += messaging.drain_sock(s) + time.sleep(0.2) + managed_processes['camerad'].stop() + + self.log_by_frame_id = defaultdict(list) + self.sensor_type = None + for cam, msgs in self.logs.items(): + if self.sensor_type is None: + self.sensor_type = getattr(msgs[0], msgs[0].which()).sensor.raw + expected_frames = SERVICE_LIST[cam].frequency * TEST_TIMESPAN + assert expected_frames*0.95 < len(msgs) < expected_frames*1.05, f"unexpected frame count {cam}: {expected_frames=}, got {len(msgs)}" + + dts = np.abs(np.diff([getattr(m, m.which()).timestampSof/1e6 for m in msgs]) - 1000/SERVICE_LIST[cam].frequency) + assert (dts < FRAME_DELTA_TOLERANCE[self.sensor_type]).all(), f"{cam} dts(ms) out of spec: max diff {dts.max()}, 99 percentile {np.percentile(dts, 99)}" + + for m in msgs: + self.log_by_frame_id[getattr(m, m.which()).frameId].append(m) + + # strip beginning and end + for _ in range(3): + mn, mx = min(self.log_by_frame_id.keys()), max(self.log_by_frame_id.keys()) + del self.log_by_frame_id[mn] + del self.log_by_frame_id[mx] + + def test_frame_skips(self): + skips = {} + frame_ids = self.log_by_frame_id.keys() + for frame_id in range(min(frame_ids), max(frame_ids)): + seen_cams = [msg.which() for msg in self.log_by_frame_id[frame_id]] + skip_cams = set(CAMERAS) - set(seen_cams) + if len(skip_cams): + skips[frame_id] = skip_cams + assert len(skips) == 0, f"Found frame skips, missing cameras for the following frames: {skips}" + + def test_frame_sync(self): + frame_times = {frame_id: [getattr(m, m.which()).timestampSof for m in msgs] for frame_id, msgs in self.log_by_frame_id.items()} + diffs = {frame_id: (max(ts) - min(ts))/1e6 for frame_id, ts in frame_times.items()} + + def get_desc(fid, diff): + cam_times = [(m.which(), getattr(m, m.which()).timestampSof/1e6) for m in self.log_by_frame_id[fid]] + return (diff, cam_times) + laggy_frames = {k: get_desc(k, v) for k, v in diffs.items() if v > LAG_FRAME_TOLERANCE[self.sensor_type]} + + def in_tol(diff): + return 50 - LAG_FRAME_TOLERANCE[self.sensor_type] < diff and diff < 50 + LAG_FRAME_TOLERANCE[self.sensor_type] + if len(laggy_frames) != 0 and all( in_tol(laggy_frames[lf][0]) for lf in laggy_frames): + print("TODO: handle camera out of sync") + else: + assert len(laggy_frames) == 0, f"Frames not synced properly: {laggy_frames=}" diff --git a/system/camerad/test/test_exposure.py b/system/camerad/test/test_exposure.py new file mode 100644 index 0000000..50467f9 --- /dev/null +++ b/system/camerad/test/test_exposure.py @@ -0,0 +1,55 @@ +#!/usr/bin/env python3 +import time +import unittest +import numpy as np + +from openpilot.selfdrive.test.helpers import with_processes, phone_only +from openpilot.system.camerad.snapshot.snapshot import get_snapshots + +TEST_TIME = 45 +REPEAT = 5 + +class TestCamerad(unittest.TestCase): + @classmethod + def setUpClass(cls): + pass + + def _numpy_rgb2gray(self, im): + ret = np.clip(im[:,:,2] * 0.114 + im[:,:,1] * 0.587 + im[:,:,0] * 0.299, 0, 255).astype(np.uint8) + return ret + + def _is_exposure_okay(self, i, med_mean=None): + if med_mean is None: + med_mean = np.array([[0.2,0.4],[0.2,0.6]]) + h, w = i.shape[:2] + i = i[h//10:9*h//10,w//10:9*w//10] + med_ex, mean_ex = med_mean + i = self._numpy_rgb2gray(i) + i_median = np.median(i) / 255. + i_mean = np.mean(i) / 255. + print([i_median, i_mean]) + return med_ex[0] < i_median < med_ex[1] and mean_ex[0] < i_mean < mean_ex[1] + + @phone_only + @with_processes(['camerad']) + def test_camera_operation(self): + passed = 0 + start = time.time() + while time.time() - start < TEST_TIME and passed < REPEAT: + rpic, dpic = get_snapshots(frame="roadCameraState", front_frame="driverCameraState") + wpic, _ = get_snapshots(frame="wideRoadCameraState") + + res = self._is_exposure_okay(rpic) + res = res and self._is_exposure_okay(dpic) + res = res and self._is_exposure_okay(wpic) + + if passed > 0 and not res: + passed = -passed # fails test if any failure after first sus + break + + passed += int(res) + time.sleep(2) + self.assertGreaterEqual(passed, REPEAT) + +if __name__ == "__main__": + unittest.main()