From fb2f2d9cb21d8109d14a9d9790fda7de07202910 Mon Sep 17 00:00:00 2001 From: Dean Lee Date: Sat, 9 Dec 2023 06:27:28 +0800 Subject: [PATCH] camerad: c++ sensorInfo (#30650) * move remaining sensor parameters to CameraInfo * same order * member functions * fix segfault --- system/camerad/cameras/camera_qcom2.cc | 94 +++++--------------------- system/camerad/cameras/camera_qcom2.h | 2 +- system/camerad/sensors/ar0231.cc | 28 +++++++- system/camerad/sensors/ox03c10.cc | 27 +++++++- system/camerad/sensors/sensor.h | 24 +++++-- 5 files changed, 88 insertions(+), 87 deletions(-) diff --git a/system/camerad/cameras/camera_qcom2.cc b/system/camerad/cameras/camera_qcom2.cc index f2ce058f4fe4c9..2592721886cdb4 100644 --- a/system/camerad/cameras/camera_qcom2.cc +++ b/system/camerad/cameras/camera_qcom2.cc @@ -46,13 +46,7 @@ int CameraState::clear_req_queue() { void CameraState::sensors_start() { if (!enabled) return; LOGD("starting sensor %d", camera_num); - if (camera_id == CAMERA_ID_AR0231) { - sensors_i2c(start_reg_array_ar0231, std::size(start_reg_array_ar0231), CAM_SENSOR_PACKET_OPCODE_SENSOR_CONFIG, true); - } else if (camera_id == CAMERA_ID_OX03C10) { - sensors_i2c(start_reg_array_ox03c10, std::size(start_reg_array_ox03c10), CAM_SENSOR_PACKET_OPCODE_SENSOR_CONFIG, false); - } else { - assert(false); - } + 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) { @@ -112,6 +106,8 @@ static cam_cmd_power *power_set_wait(cam_cmd_power *power, int16_t delay_ms) { } int CameraState::sensors_init() { + create_sensor(); + 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); @@ -127,21 +123,7 @@ int CameraState::sensors_init() { auto probe = (struct cam_cmd_probe *)(i2c_info.get() + 1); probe->camera_id = camera_num; - switch (camera_num) { - case 0: - // port 0 - i2c_info->slave_addr = (camera_id == CAMERA_ID_AR0231) ? 0x20 : 0x6C; // 6C = 0x36*2 - break; - case 1: - // port 1 - i2c_info->slave_addr = (camera_id == CAMERA_ID_AR0231) ? 0x30 : 0x20; - break; - case 2: - // port 2 - i2c_info->slave_addr = (camera_id == CAMERA_ID_AR0231) ? 0x20 : 0x6C; - break; - } - + 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; @@ -151,15 +133,8 @@ int CameraState::sensors_init() { probe->addr_type = CAMERA_SENSOR_I2C_TYPE_WORD; probe->op_code = 3; // don't care? probe->cmd_type = CAMERA_SENSOR_CMD_TYPE_PROBE; - if (camera_id == CAMERA_ID_AR0231) { - probe->reg_addr = 0x3000; - probe->expected_data = 0x354; - } else if (camera_id == CAMERA_ID_OX03C10) { - probe->reg_addr = 0x300a; - probe->expected_data = 0x5803; - } else { - assert(false); - } + 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; @@ -182,7 +157,7 @@ int CameraState::sensors_init() { power->count = 1; power->cmd_type = CAMERA_SENSOR_CMD_TYPE_PWR_UP; power->power_settings[0].power_seq_type = 0; - power->power_settings[0].config_val_low = (camera_id == CAMERA_ID_AR0231) ? 19200000 : 24000000; //Hz + power->power_settings[0].config_val_low = ci->power_config_val_low; power = power_set_wait(power, 1); // reset high @@ -428,7 +403,7 @@ void CameraState::enqueue_req_multi(int start, int n, bool dp) { // ******************* camera ******************* -void CameraState::camera_set_parameters() { +void CameraState::create_sensor() { if (camera_id == CAMERA_ID_AR0231) { ci = std::make_unique(); } else if (camera_id == CAMERA_ID_OX03C10) { @@ -504,8 +479,6 @@ void CameraState::camera_open(MultiCameraState *multi_cam_state_, int camera_num return; } - camera_set_parameters(); - // 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)); @@ -520,18 +493,8 @@ void CameraState::camera_open(MultiCameraState *multi_cam_state_, int camera_num LOGD("acquire sensor dev"); LOG("-- Configuring sensor"); - uint32_t dt; - if (camera_id == CAMERA_ID_AR0231) { - sensors_i2c(init_array_ar0231, std::size(init_array_ar0231), CAM_SENSOR_PACKET_OPCODE_SENSOR_CONFIG, true); - dt = 0x12; // Changing stats to 0x2C doesn't work, so change pixels to 0x12 instead - } else if (camera_id == CAMERA_ID_OX03C10) { - sensors_i2c(init_array_ox03c10, std::size(init_array_ox03c10), CAM_SENSOR_PACKET_OPCODE_SENSOR_CONFIG, false); - // one is 0x2a, two are 0x2b - dt = 0x2c; - } else { - assert(false); - } - printf("dt is %x\n", dt); + sensors_i2c(ci->init_reg_array.data(), ci->init_reg_array.size(), CAM_SENSOR_PACKET_OPCODE_SENSOR_CONFIG, ci->data_word); + printf("dt is %x\n", ci->in_port_info_dt); // 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!) @@ -545,7 +508,7 @@ void CameraState::camera_open(MultiCameraState *multi_cam_state_, int camera_num .lane_cfg = 0x3210, .vc = 0x0, - .dt = dt, + .dt = ci->in_port_info_dt, .format = CAM_FORMAT_MIPI_RAW_12, .test_pattern = 0x2, // 0x3? @@ -839,22 +802,7 @@ void CameraState::handle_camera_event(void *evdat) { } void CameraState::update_exposure_score(float desired_ev, int exp_t, int exp_g_idx, float exp_gain) { - float score = 1e6; - if (camera_id == CAMERA_ID_AR0231) { - // Cost of ev diff - score = std::abs(desired_ev - (exp_t * exp_gain)) * 10; - // Cost of absolute gain - float m = exp_g_idx > ci->analog_gain_rec_idx ? ci->analog_gain_cost_high : ci->analog_gain_cost_low; - score += std::abs(exp_g_idx - (int)ci->analog_gain_rec_idx) * m; - // Cost of changing gain - score += std::abs(exp_g_idx - gain_idx) * (score + 1.0) / 10.0; - } else if (camera_id == CAMERA_ID_OX03C10) { - score = std::abs(desired_ev - (exp_t * exp_gain)); - float m = exp_g_idx > ci->analog_gain_rec_idx ? ci->analog_gain_cost_high : ci->analog_gain_cost_low; - score += std::abs(exp_g_idx - (int)ci->analog_gain_rec_idx) * m; - score += ((1 - ci->analog_gain_cost_delta) + ci->analog_gain_cost_delta * (exp_g_idx - ci->analog_gain_min_idx) / (ci->analog_gain_max_idx - ci->analog_gain_min_idx)) * std::abs(exp_g_idx - gain_idx) * 5.0; - } - + 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; @@ -960,13 +908,8 @@ void CameraState::set_camera_exposure(float grey_frac) { } // LOGE("ae - camera %d, cur_t %.5f, sof %.5f, dt %.5f", camera_num, 1e-9 * nanos_since_boot(), 1e-9 * buf.cur_frame_data.timestamp_sof, 1e-9 * (nanos_since_boot() - buf.cur_frame_data.timestamp_sof)); - if (camera_id == CAMERA_ID_AR0231) { - auto exp_reg_array = ar0231_get_exp_registers(ci.get(), exposure_time, new_exp_g, dc_gain_enabled); - sensors_i2c(exp_reg_array.data(), exp_reg_array.size(), CAM_SENSOR_PACKET_OPCODE_SENSOR_CONFIG, true); - } else if (camera_id == CAMERA_ID_OX03C10) { - auto exp_reg_array = ox03c10_get_exp_registers(ci.get(), exposure_time, new_exp_g, dc_gain_enabled); - sensors_i2c(exp_reg_array.data(), exp_reg_array.size(), CAM_SENSOR_PACKET_OPCODE_SENSOR_CONFIG, false); - } + 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) { @@ -977,9 +920,7 @@ static void process_driver_camera(MultiCameraState *s, CameraState *c, int cnt) framed.setFrameType(cereal::FrameData::FrameType::FRONT); fill_frame_data(framed, c->buf.cur_frame_data, c); - if (c->camera_id == CAMERA_ID_AR0231) { - ar0231_process_registers(s, c, framed); - } + c->ci->processRegisters(s, c, framed); s->pm->send("driverCameraState", msg); } @@ -994,10 +935,7 @@ void process_road_camera(MultiCameraState *s, CameraState *c, int cnt) { } LOGT(c->buf.cur_frame_data.frame_id, "%s: Image set", c == &s->road_cam ? "RoadCamera" : "WideRoadCamera"); - if (c->camera_id == CAMERA_ID_AR0231) { - ar0231_process_registers(s, c, framed); - } - + c->ci->processRegisters(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); diff --git a/system/camerad/cameras/camera_qcom2.h b/system/camerad/cameras/camera_qcom2.h index 65c7abfb36e51b..03c766a2eed87d 100644 --- a/system/camerad/cameras/camera_qcom2.h +++ b/system/camerad/cameras/camera_qcom2.h @@ -49,7 +49,7 @@ class CameraState { void sensors_start(); void camera_open(MultiCameraState *multi_cam_state, int camera_num, bool enabled); - void camera_set_parameters(); + void create_sensor(); void camera_map_bufs(MultiCameraState *s); void camera_init(MultiCameraState *s, VisionIpcServer *v, int camera_id, unsigned int fps, cl_device_id device_id, cl_context ctx, VisionStreamType yuv_type); void camera_close(); diff --git a/system/camerad/sensors/ar0231.cc b/system/camerad/sensors/ar0231.cc index 01a95252f4ad75..c750533165c660 100644 --- a/system/camerad/sensors/ar0231.cc +++ b/system/camerad/sensors/ar0231.cc @@ -90,6 +90,7 @@ float ar0231_parse_temp_sensor(uint16_t calib1, uint16_t calib2, uint16_t data_r } // namespace AR0231::AR0231() { + data_word = true; frame_width = FRAME_WIDTH; frame_height = FRAME_HEIGHT; frame_stride = FRAME_STRIDE; @@ -99,6 +100,13 @@ AR0231::AR0231() { 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; + in_port_info_dt = 0x12; // Changing stats to 0x2C doesn't work, so change pixels to 0x12 instead + power_config_val_low = 19200000; //Hz + dc_gain_factor = 2.5; dc_gain_min_weight = 0; dc_gain_max_weight = 1; @@ -120,7 +128,7 @@ AR0231::AR0231() { target_grey_factor = 1.0; } -void ar0231_process_registers(MultiCameraState *s, CameraState *c, cereal::FrameData::Builder &framed) { +void AR0231::processRegisters(MultiCameraState *s, 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; @@ -140,7 +148,7 @@ void ar0231_process_registers(MultiCameraState *s, CameraState *c, cereal::Frame } -std::vector ar0231_get_exp_registers(const SensorInfo *ci, int exposure_time, int new_exp_g, bool dc_gain_enabled) { +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}, @@ -148,3 +156,19 @@ std::vector ar0231_get_exp_registers(const SensorI {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/ox03c10.cc b/system/camerad/sensors/ox03c10.cc index d55c73a436d434..c30838a7fe55f9 100644 --- a/system/camerad/sensors/ox03c10.cc +++ b/system/camerad/sensors/ox03c10.cc @@ -22,12 +22,20 @@ const uint32_t VS_TIME_MAX_OX03C10 = 34; // vs < 35 } // namespace OX03C10::OX03C10() { + data_word = false; frame_width = FRAME_WIDTH; frame_height = FRAME_HEIGHT; frame_stride = FRAME_STRIDE; // (0xa80*12//8) 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; + in_port_info_dt = 0x2c; // one is 0x2a, two are 0x2b + power_config_val_low = 24000000; //Hz + dc_gain_factor = 7.32; dc_gain_min_weight = 1; // always on is fine dc_gain_max_weight = 1; @@ -49,11 +57,11 @@ OX03C10::OX03C10() { target_grey_factor = 0.01; } -std::vector ox03c10_get_exp_registers(const SensorInfo *ci, int exposure_time, int new_exp_g, bool dc_gain_enabled) { +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, (ci->exposure_time_max + VS_TIME_MAX_OX03C10) / 3), ci->exposure_time_max + VS_TIME_MAX_OX03C10); + 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]; @@ -67,3 +75,18 @@ std::vector ox03c10_get_exp_registers(const Sensor {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/sensor.h b/system/camerad/sensors/sensor.h index d038d445451b57..62cb31c52c3255 100644 --- a/system/camerad/sensors/sensor.h +++ b/system/camerad/sensors/sensor.h @@ -1,5 +1,6 @@ #pragma once +#include #include #include #include "media/cam_sensor.h" @@ -15,6 +16,10 @@ const size_t FRAME_STRIDE = 2896; // for 12 bit output. 1928 * 12 / 8 + 4 (alig 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(MultiCameraState *s, CameraState *c, cereal::FrameData::Builder &framed) const {} uint32_t frame_width, frame_height; uint32_t frame_stride; @@ -42,18 +47,29 @@ class SensorInfo { 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 in_port_info_dt; + uint32_t power_config_val_low; }; 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(MultiCameraState *s, CameraState *c, cereal::FrameData::Builder &framed) const override; }; 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; }; - -void ar0231_process_registers(MultiCameraState *s, CameraState *c, cereal::FrameData::Builder &framed); -std::vector ox03c10_get_exp_registers(const SensorInfo *ci, int exposure_time, int new_exp_g, bool dc_gain_enabled); -std::vector ar0231_get_exp_registers(const SensorInfo *ci, int exposure_time, int new_exp_g, bool dc_gain_enabled);