diff --git a/selfdrive/camerad/cameras/camera_common.cc b/selfdrive/camerad/cameras/camera_common.cc index 77b054887..676e74081 100644 --- a/selfdrive/camerad/cameras/camera_common.cc +++ b/selfdrive/camerad/cameras/camera_common.cc @@ -36,10 +36,10 @@ public: hdr_ = ci->hdr; snprintf(args, sizeof(args), "-cl-fast-relaxed-math -cl-denorms-are-zero " - "-DFRAME_WIDTH=%d -DFRAME_HEIGHT=%d -DFRAME_STRIDE=%d " + "-DFRAME_WIDTH=%d -DFRAME_HEIGHT=%d -DFRAME_STRIDE=%d -DFRAME_OFFSET=%d " "-DRGB_WIDTH=%d -DRGB_HEIGHT=%d -DRGB_STRIDE=%d " "-DBAYER_FLIP=%d -DHDR=%d -DCAM_NUM=%d", - ci->frame_width, ci->frame_height, ci->frame_stride, + ci->frame_width, ci->frame_height, ci->frame_stride, ci->frame_offset, b->rgb_width, b->rgb_height, b->rgb_stride, ci->bayer_flip, ci->hdr, s->camera_num); const char *cl_file = "cameras/real_debayer.cl"; @@ -81,7 +81,7 @@ void CameraBuf::init(cl_device_id device_id, cl_context context, CameraState *s, frame_buf_count = frame_cnt; // RAW frame - const int frame_size = ci->frame_height * ci->frame_stride; + 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); diff --git a/selfdrive/camerad/cameras/camera_common.h b/selfdrive/camerad/cameras/camera_common.h index 9c7bf6b03..8c836e0bb 100644 --- a/selfdrive/camerad/cameras/camera_common.h +++ b/selfdrive/camerad/cameras/camera_common.h @@ -52,11 +52,15 @@ const bool env_log_raw_frames = getenv("LOG_RAW_FRAMES") != NULL; typedef void (*release_cb)(void *cookie, int buf_idx); typedef struct CameraInfo { - int frame_width, frame_height; - int frame_stride; + uint32_t frame_width, frame_height; + uint32_t frame_stride; bool bayer; int bayer_flip; bool hdr; + uint32_t frame_offset = 0; + uint32_t extra_height = 0; + int registers_offset = -1; + int stats_offset = -1; } CameraInfo; typedef struct FrameMetadata { diff --git a/selfdrive/camerad/cameras/camera_qcom2.cc b/selfdrive/camerad/cameras/camera_qcom2.cc index 7120bf957..ce9b5663d 100644 --- a/selfdrive/camerad/cameras/camera_qcom2.cc +++ b/selfdrive/camerad/cameras/camera_qcom2.cc @@ -31,6 +31,9 @@ const size_t FRAME_WIDTH = 1928; const size_t FRAME_HEIGHT = 1208; const size_t FRAME_STRIDE = 2896; // for 12 bit output. 1928 * 12 / 8 + 4 (alignment) +const size_t AR0231_REGISTERS_HEIGHT = 2; +const size_t AR0231_STATS_HEIGHT = 2; + const int MIPI_SETTLE_CNT = 33; // Calculated by camera_freqs.py CameraInfo cameras_supported[CAMERA_ID_MAX] = { @@ -38,17 +41,24 @@ CameraInfo cameras_supported[CAMERA_ID_MAX] = { .frame_width = FRAME_WIDTH, .frame_height = FRAME_HEIGHT, .frame_stride = FRAME_STRIDE, + .extra_height = AR0231_REGISTERS_HEIGHT + AR0231_STATS_HEIGHT, + + .registers_offset = 0, + .frame_offset = AR0231_REGISTERS_HEIGHT, + .stats_offset = AR0231_REGISTERS_HEIGHT + FRAME_HEIGHT, + .bayer = true, .bayer_flip = 1, - .hdr = false + .hdr = false, }, [CAMERA_ID_IMX390] = { .frame_width = FRAME_WIDTH, .frame_height = FRAME_HEIGHT, .frame_stride = FRAME_STRIDE, + .bayer = true, .bayer_flip = 1, - .hdr = false + .hdr = false, }, }; @@ -509,10 +519,10 @@ void CameraState::config_isp(int io_mem_handle, int fence, int request_id, int b if (io_mem_handle != 0) { io_cfg[0].mem_handle[0] = io_mem_handle; io_cfg[0].planes[0] = (struct cam_plane_cfg){ - .width = FRAME_WIDTH, - .height = FRAME_HEIGHT, - .plane_stride = FRAME_STRIDE, - .slice_height = FRAME_HEIGHT, + .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, @@ -682,23 +692,23 @@ void CameraState::camera_open() { .lane_cfg = 0x3210, .vc = 0x0, - .dt = 0x2C, // CSI_RAW12 + .dt = 0x12, // Changing stats to 0x2C doesn't work, so change pixels to 0x12 instead .format = CAM_FORMAT_MIPI_RAW_12, .test_pattern = 0x2, // 0x3? .usage_type = 0x0, .left_start = 0, - .left_stop = FRAME_WIDTH - 1, - .left_width = FRAME_WIDTH, + .left_stop = ci.frame_width - 1, + .left_width = ci.frame_width, .right_start = 0, - .right_stop = FRAME_WIDTH - 1, - .right_width = FRAME_WIDTH, + .right_stop = ci.frame_width - 1, + .right_width = ci.frame_width, .line_start = 0, - .line_stop = FRAME_HEIGHT - 1, - .height = FRAME_HEIGHT, + .line_stop = ci.frame_height + ci.extra_height - 1, + .height = ci.frame_height + ci.extra_height, .pixel_clk = 0x0, .batch_size = 0x0, @@ -710,8 +720,8 @@ void CameraState::camera_open() { .data[0] = (struct cam_isp_out_port_info){ .res_type = CAM_ISP_IFE_OUT_RES_RDI_0, .format = CAM_FORMAT_MIPI_RAW_12, - .width = FRAME_WIDTH, - .height = FRAME_HEIGHT, + .width = ci.frame_width, + .height = ci.frame_height + ci.extra_height, .comp_grp_id = 0x0, .split_point = 0x0, .secure_mode = 0x0, }, }; @@ -935,6 +945,70 @@ void cameras_close(MultiCameraState *s) { delete s->pm; } +std::map> CameraState::ar0231_build_register_lut(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 contigous 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 + 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 + ci.frame_stride * register_row, val_idx + ci.frame_stride * register_row); + } + + value_tag_count++; + } + } + } + return registers; +} + +std::map CameraState::ar0231_parse_registers(uint8_t *data, std::initializer_list addrs) { + if (ar0231_register_lut.empty()) { + ar0231_register_lut = ar0231_build_register_lut(data); + } + + std::map registers; + for (uint16_t addr : addrs) { + auto offset = ar0231_register_lut[addr]; + registers[addr] = ((uint16_t)data[offset.first] << 8) | data[offset.second]; + } + return registers; +} + void CameraState::handle_camera_event(void *evdat) { if (!enabled) return; struct cam_req_mgr_message *event_data = (struct cam_req_mgr_message *)evdat; @@ -1114,6 +1188,25 @@ void camera_autoexposure(CameraState *s, float grey_frac) { s->set_camera_exposure(grey_frac); } +static 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; +} + +static void ar0231_process_registers(MultiCameraState *s, CameraState *c, cereal::FrameData::Builder &framed){ + uint8_t *data = (uint8_t*)c->buf.cur_camera_buf->addr + c->ci.registers_offset; + auto registers = c->ar0231_parse_registers(data, {0x2000, 0x2002, 0x20b0, 0x20b2, 0x30c6, 0x30c8, 0x30ca, 0x30cc}); + + 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}); +} + static void driver_cam_auto_exposure(CameraState *c, SubMaster &sm) { struct ExpRect {int x1, x2, x_skip, y1, y2, y_skip;}; const CameraBuf *b = &c->buf; @@ -1132,10 +1225,12 @@ static void process_driver_camera(MultiCameraState *s, CameraState *c, int cnt) if (env_send_driver) { framed.setImage(get_frame_image(&c->buf)); } + if (c->camera_id == CAMERA_ID_AR0231) { + ar0231_process_registers(s, c, framed); + } s->pm->send("driverCameraState", msg); } -// called by processing_thread void process_road_camera(MultiCameraState *s, CameraState *c, int cnt) { const CameraBuf *b = &c->buf; @@ -1152,6 +1247,11 @@ void process_road_camera(MultiCameraState *s, CameraState *c, int cnt) { framed.setTransform(b->yuv_transform.v); LOGT(c->buf.cur_frame_data.frame_id, "%s: Transformed", "RoadCamera"); } + + if (c->camera_id == CAMERA_ID_AR0231) { + ar0231_process_registers(s, 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); @@ -1221,3 +1321,4 @@ void cameras_run(MultiCameraState *s) { cameras_close(s); } + diff --git a/selfdrive/camerad/cameras/camera_qcom2.h b/selfdrive/camerad/cameras/camera_qcom2.h index a7c64c066..e0553f000 100644 --- a/selfdrive/camerad/cameras/camera_qcom2.h +++ b/selfdrive/camerad/cameras/camera_qcom2.h @@ -1,6 +1,8 @@ #pragma once #include +#include +#include #include @@ -56,6 +58,8 @@ public: void camera_init(MultiCameraState *multi_cam_state, VisionIpcServer * v, int camera_id, int camera_num, unsigned int fps, cl_device_id device_id, cl_context ctx, VisionStreamType rgb_type, VisionStreamType yuv_type, bool enabled); void camera_close(); + std::map ar0231_parse_registers(uint8_t *data, std::initializer_list addrs); + int32_t session_handle; int32_t sensor_dev_handle; int32_t isp_dev_handle; @@ -75,6 +79,7 @@ public: CameraBuf buf; MemoryManager mm; + private: 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); @@ -84,6 +89,10 @@ private: int sensors_init(); void sensors_poke(int request_id); void sensors_i2c(struct i2c_random_wr_payload* dat, int len, int op_code, bool data_word); + + // Register parsing + std::map> ar0231_register_lut; + std::map> ar0231_build_register_lut(uint8_t *data); }; typedef struct MultiCameraState { diff --git a/selfdrive/camerad/cameras/camera_replay.cc b/selfdrive/camerad/cameras/camera_replay.cc index a9983fe23..05bdfb923 100644 --- a/selfdrive/camerad/cameras/camera_replay.cc +++ b/selfdrive/camerad/cameras/camera_replay.cc @@ -30,9 +30,9 @@ void camera_init(VisionIpcServer *v, CameraState *s, int camera_id, unsigned int } CameraInfo ci = { - .frame_width = s->frame->width, - .frame_height = s->frame->height, - .frame_stride = s->frame->width * 3, + .frame_width = (uint32_t)s->frame->width, + .frame_height = (uint32_t)s->frame->height, + .frame_stride = (uint32_t)s->frame->width * 3, }; s->ci = ci; s->camera_num = camera_id; diff --git a/selfdrive/camerad/cameras/real_debayer.cl b/selfdrive/camerad/cameras/real_debayer.cl index 50a784621..783abaabb 100644 --- a/selfdrive/camerad/cameras/real_debayer.cl +++ b/selfdrive/camerad/cameras/real_debayer.cl @@ -42,7 +42,7 @@ half3 color_correct(half3 rgb) { inline half val_from_10(const uchar * source, int gx, int gy, half black_level) { // parse 12bit - int start = gy * FRAME_STRIDE + (3 * (gx / 2)); + int start = gy * FRAME_STRIDE + (3 * (gx / 2)) + (FRAME_STRIDE * FRAME_OFFSET); int offset = gx % 2; uint major = (uint)source[start + offset] << 4; uint minor = (source[start + 2] >> (4 * offset)) & 0xf; diff --git a/selfdrive/camerad/cameras/sensor2_i2c.h b/selfdrive/camerad/cameras/sensor2_i2c.h index e85223243..650c17cdf 100644 --- a/selfdrive/camerad/cameras/sensor2_i2c.h +++ b/selfdrive/camerad/cameras/sensor2_i2c.h @@ -7,7 +7,7 @@ struct i2c_random_wr_payload init_array_imx390[] = { {0x2008, 0xd0}, {0x2009, 0x07}, {0x200a, 0x00}, // MODE_VMAX = time between frames {0x200C, 0xe4}, {0x200D, 0x0c}, // MODE_HMAX - // crop + // crop {0x3410, 0x88}, {0x3411, 0x7}, // CROP_H_SIZE {0x3418, 0xb8}, {0x3419, 0x4}, // CROP_V_SIZE {0x0078, 1}, {0x03c0, 1}, @@ -67,7 +67,7 @@ struct i2c_random_wr_payload init_array_ar0231[] = { {0x30A6, 0x0001}, // Y_ODD_INC_ {0x3402, 0x0788}, // X_OUTPUT_CONTROL {0x3404, 0x04B8}, // Y_OUTPUT_CONTROL - {0x3064, 0x1802}, // SMIA_TEST + {0x3064, 0x1982}, // SMIA_TEST {0x30BA, 0x11F2}, // DIGITAL_CTRL // Enable external trigger and disable GPIO outputs @@ -83,10 +83,10 @@ struct i2c_random_wr_payload init_array_ar0231[] = { // Readout Settings {0x31AE, 0x0204}, // SERIAL_FORMAT, 4-lane MIPI {0x31AC, 0x0C0C}, // DATA_FORMAT_BITS, 12 -> 12 - {0x3342, 0x122C}, // MIPI_F1_PDT_EDT - {0x3346, 0x122C}, // MIPI_F2_PDT_EDT - {0x334A, 0x122C}, // MIPI_F3_PDT_EDT - {0x334E, 0x122C}, // MIPI_F4_PDT_EDT + {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 @@ -101,6 +101,10 @@ struct i2c_random_wr_payload init_array_ar0231[] = { {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}, @@ -114,9 +118,7 @@ struct i2c_random_wr_payload init_array_ar0231[] = { {0x100E, 0x07B1}, // FINE_INTEGRATION_TIME3_MIN {0x1010, 0x0139}, // FINE_INTEGRATION_TIME4_MIN {0x3014, 0x08CB}, // FINE_INTEGRATION_TIME_ - {0x321E, 0x08CB}, // FINE_INTEGRATION_TIME2 - {0x321E, 0x08CB}, // FINE_INTEGRATION_TIME3 - {0x321E, 0x0894}, // FINE_INTEGRATION_TIME4 + {0x321E, 0x0894}, // FINE_INTEGRATION_TIME2 {0x31D0, 0x0000}, // COMPANDING, no good in 10 bit? {0x33DA, 0x0000}, // COMPANDING