Skip to content

Commit

Permalink
camerad: c++ sensorInfo (commaai#30650)
Browse files Browse the repository at this point in the history 
* move remaining sensor parameters to CameraInfo

* same order

* member functions

* fix segfault
  • Loading branch information
@deanlee
deanlee authored Dec 8, 2023
1 parent 011b1a6 commit fb2f2d9
Show file tree
Hide file tree
Showing 5 changed files with 88 additions and 87 deletions.
94 changes: 16 additions & 78 deletions system/camerad/cameras/camera_qcom2.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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) {
Expand Down Expand Up @@ -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<struct cam_packet>(size, &cam_packet_handle);
Expand All @@ -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;
Expand All @@ -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;
Expand All @@ -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
Expand Down Expand Up @@ -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<AR0231>();
} else if (camera_id == CAMERA_ID_OX03C10) {
Expand Down Expand Up @@ -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));
Expand All @@ -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!)
Expand All @@ -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?
Expand Down Expand Up @@ -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;
Expand Down Expand Up @@ -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) {
Expand All @@ -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);
}

Expand All @@ -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);
Expand Down
2 changes: 1 addition & 1 deletion system/camerad/cameras/camera_qcom2.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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();
Expand Down
28 changes: 26 additions & 2 deletions system/camerad/sensors/ar0231.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand All @@ -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;
Expand All @@ -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;

Expand All @@ -140,11 +148,27 @@ void ar0231_process_registers(MultiCameraState *s, CameraState *c, cereal::Frame
}


std::vector<struct i2c_random_wr_payload> ar0231_get_exp_registers(const SensorInfo *ci, int exposure_time, int new_exp_g, bool dc_gain_enabled) {
std::vector<i2c_random_wr_payload> 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;
}
27 changes: 25 additions & 2 deletions system/camerad/sensors/ox03c10.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand All @@ -49,11 +57,11 @@ OX03C10::OX03C10() {
target_grey_factor = 0.01;
}

std::vector<struct i2c_random_wr_payload> ox03c10_get_exp_registers(const SensorInfo *ci, int exposure_time, int new_exp_g, bool dc_gain_enabled) {
std::vector<i2c_random_wr_payload> 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];
Expand All @@ -67,3 +75,18 @@ std::vector<struct i2c_random_wr_payload> 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;
}
24 changes: 20 additions & 4 deletions system/camerad/sensors/sensor.h
View file
Original file line number Diff line number Diff line change
@@ -1,5 +1,6 @@
#pragma once

#include <cassert>
#include <cstdint>
#include <vector>
#include "media/cam_sensor.h"
Expand All @@ -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<i2c_random_wr_payload> 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;
Expand Down Expand Up @@ -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<i2c_random_wr_payload> start_reg_array;
std::vector<i2c_random_wr_payload> init_reg_array;
uint32_t in_port_info_dt;
uint32_t power_config_val_low;
};

class AR0231 : public SensorInfo {
public:
AR0231();
std::vector<i2c_random_wr_payload> 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<i2c_random_wr_payload> 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<struct i2c_random_wr_payload> ox03c10_get_exp_registers(const SensorInfo *ci, int exposure_time, int new_exp_g, bool dc_gain_enabled);
std::vector<struct i2c_random_wr_payload> ar0231_get_exp_registers(const SensorInfo *ci, int exposure_time, int new_exp_g, bool dc_gain_enabled);

0 comments on commit fb2f2d9

Please sign in to comment.