Skip to content

Commit

Permalink
camerad: move sensor related code to /sensors (commaai#30647)
Browse files Browse the repository at this point in the history
  • Loading branch information
@deanlee
deanlee authored Dec 8, 2023
1 parent 5132860 commit ef262ff
Show file tree
Hide file tree
Showing 10 changed files with 238 additions and 200 deletions.
2 changes: 2 additions & 0 deletions release/files_common
View file
Original file line number Diff line number Diff line change
Expand Up @@ -320,7 +320,9 @@ system/camerad/cameras/camera_common.h
system/camerad/cameras/camera_common.cc
system/camerad/sensors/ar0231.cc
system/camerad/sensors/ar0231_registers.h
system/camerad/sensors/ox03c10.cc
system/camerad/sensors/ox03c10_registers.h
system/camerad/sensors/sensor.h

selfdrive/manager/__init__.py
selfdrive/manager/build.py
Expand Down
2 changes: 1 addition & 1 deletion system/camerad/SConscript
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3,7 +3,7 @@ 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/ar0231.cc', 'sensors/ox03c10.cc'])
env.Program('camerad', ['main.cc', camera_obj], LIBS=libs)

if GetOption("extras") and arch == "x86_64":
Expand Down
33 changes: 0 additions & 33 deletions system/camerad/cameras/camera_common.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -17,7 +17,6 @@
#define CAMERA_ID_AR0231 0
#define CAMERA_ID_OX03C10 1

#define ANALOG_GAIN_MAX_CNT 55
const int YUV_BUFFER_COUNT = 20;

enum CameraType {
Expand All @@ -34,35 +33,6 @@ 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 CameraInfo {
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;
} CameraInfo;

typedef struct FrameMetadata {
uint32_t frame_id;

Expand Down Expand Up @@ -123,6 +93,3 @@ 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);

void ar0231_process_registers(MultiCameraState *s, CameraState *c, cereal::FrameData::Builder &framed);

167 changes: 8 additions & 159 deletions system/camerad/cameras/camera_qcom2.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -19,85 +19,17 @@
#include "media/cam_defs.h"
#include "media/cam_isp.h"
#include "media/cam_isp_ife.h"
#include "media/cam_sensor.h"
#include "media/cam_sensor_cmn_header.h"
#include "media/cam_sync.h"
#include "common/swaglog.h"
#include "system/camerad/sensors/ar0231_registers.h"
#include "system/camerad/sensors/ox03c10_registers.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;

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;
// TODO: this extra height is universal and doesn't apply per camera
const size_t AR0231_STATS_HEIGHT = 2+8;

const int MIPI_SETTLE_CNT = 33; // Calculated by camera_freqs.py
const float DC_GAIN_AR0231 = 2.5;
const float DC_GAIN_OX03C10 = 7.32;

const float DC_GAIN_ON_GREY_AR0231 = 0.2;
const float DC_GAIN_OFF_GREY_AR0231 = 0.3;
const float DC_GAIN_ON_GREY_OX03C10 = 0.9;
const float DC_GAIN_OFF_GREY_OX03C10 = 1.0;

const int DC_GAIN_MIN_WEIGHT_AR0231 = 0;
const int DC_GAIN_MAX_WEIGHT_AR0231 = 1;
const int DC_GAIN_MIN_WEIGHT_OX03C10 = 1; // always on is fine
const int DC_GAIN_MAX_WEIGHT_OX03C10 = 1;

const float TARGET_GREY_FACTOR_AR0231 = 1.0;
const float TARGET_GREY_FACTOR_OX03C10 = 0.01;

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

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 int ANALOG_GAIN_MIN_IDX_AR0231 = 0x1; // 0.25x
const int ANALOG_GAIN_REC_IDX_AR0231 = 0x6; // 0.8x
const int ANALOG_GAIN_MAX_IDX_AR0231 = 0xD; // 4.0x
const int ANALOG_GAIN_COST_DELTA_AR0231 = 0;
const float ANALOG_GAIN_COST_LOW_AR0231 = 0.1;
const float ANALOG_GAIN_COST_HIGH_AR0231 = 5.0;

const int ANALOG_GAIN_MIN_IDX_OX03C10 = 0x0;
const int ANALOG_GAIN_REC_IDX_OX03C10 = 0x0; // 1x
const int ANALOG_GAIN_MAX_IDX_OX03C10 = 0x36;
const int ANALOG_GAIN_COST_DELTA_OX03C10 = -1;
const float ANALOG_GAIN_COST_LOW_OX03C10 = 0.4;
const float ANALOG_GAIN_COST_HIGH_OX03C10 = 6.4;

const int EXPOSURE_TIME_MIN_AR0231 = 2; // with HDR, fastest ss
const int EXPOSURE_TIME_MAX_AR0231 = 0x0855; // with HDR, slowest ss, 40ms

const int EXPOSURE_TIME_MIN_OX03C10 = 2; // 1x
const int EXPOSURE_TIME_MAX_OX03C10 = 2016;
const uint32_t VS_TIME_MIN_OX03C10 = 1;
const uint32_t VS_TIME_MAX_OX03C10 = 34; // vs < 35

int CameraState::clear_req_queue() {
struct cam_req_mgr_flush_info req_mgr_flush_request = {0};
req_mgr_flush_request.session_hdl = session_handle;
Expand Down Expand Up @@ -141,7 +73,7 @@ void CameraState::sensors_poke(int request_id) {
}
}

void CameraState::sensors_i2c(struct i2c_random_wr_payload* dat, int len, int op_code, bool data_word) {
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;
Expand Down Expand Up @@ -496,74 +428,11 @@ void CameraState::enqueue_req_multi(int start, int n, bool dp) {

// ******************* camera *******************

struct CameraAR0231 : public CameraInfo {
CameraAR0231() {
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,

dc_gain_factor = DC_GAIN_AR0231;
dc_gain_min_weight = DC_GAIN_MIN_WEIGHT_AR0231;
dc_gain_max_weight = DC_GAIN_MAX_WEIGHT_AR0231;
dc_gain_on_grey = DC_GAIN_ON_GREY_AR0231;
dc_gain_off_grey = DC_GAIN_OFF_GREY_AR0231;
exposure_time_min = EXPOSURE_TIME_MIN_AR0231;
exposure_time_max = EXPOSURE_TIME_MAX_AR0231;
analog_gain_min_idx = ANALOG_GAIN_MIN_IDX_AR0231;
analog_gain_rec_idx = ANALOG_GAIN_REC_IDX_AR0231;
analog_gain_max_idx = ANALOG_GAIN_MAX_IDX_AR0231;
analog_gain_cost_delta = ANALOG_GAIN_COST_DELTA_AR0231;
analog_gain_cost_low = ANALOG_GAIN_COST_LOW_AR0231;
analog_gain_cost_high = ANALOG_GAIN_COST_HIGH_AR0231;
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 = TARGET_GREY_FACTOR_AR0231;
}
};

struct CameraOx0310 : public CameraInfo {
CameraOx0310() {
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,

dc_gain_factor = DC_GAIN_OX03C10;
dc_gain_min_weight = DC_GAIN_MIN_WEIGHT_OX03C10;
dc_gain_max_weight = DC_GAIN_MAX_WEIGHT_OX03C10;
dc_gain_on_grey = DC_GAIN_ON_GREY_OX03C10;
dc_gain_off_grey = DC_GAIN_OFF_GREY_OX03C10;
exposure_time_min = EXPOSURE_TIME_MIN_OX03C10;
exposure_time_max = EXPOSURE_TIME_MAX_OX03C10;
analog_gain_min_idx = ANALOG_GAIN_MIN_IDX_OX03C10;
analog_gain_rec_idx = ANALOG_GAIN_REC_IDX_OX03C10;
analog_gain_max_idx = ANALOG_GAIN_MAX_IDX_OX03C10;
analog_gain_cost_delta = ANALOG_GAIN_COST_DELTA_OX03C10;
analog_gain_cost_low = ANALOG_GAIN_COST_LOW_OX03C10;
analog_gain_cost_high = ANALOG_GAIN_COST_HIGH_OX03C10;
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 = TARGET_GREY_FACTOR_OX03C10;
}
};

void CameraState::camera_set_parameters() {
if (camera_id == CAMERA_ID_AR0231) {
ci = std::make_unique<CameraAR0231>();
} else if (camera_id == CAMERA_ID_OX03C10) {
ci = std::make_unique<CameraOx0310>();
ci = std::make_unique<CameraOx03c10>();
} else {
assert(false);
}
Expand Down Expand Up @@ -1092,31 +961,11 @@ void CameraState::set_camera_exposure(float grey_frac) {
// LOGE("ae - camera %d, cur_t %.5f, sof %.5f, dt %.5f", camera_num, 1e-9 * nanos_since_boot(), 1e-9 * buf.cur_frame_data.timestamp_sof, 1e-9 * (nanos_since_boot() - buf.cur_frame_data.timestamp_sof));

if (camera_id == CAMERA_ID_AR0231) {
uint16_t analog_gain_reg = 0xFF00 | (new_exp_g << 4) | new_exp_g;
struct i2c_random_wr_payload exp_reg_array[] = {
{0x3366, analog_gain_reg},
{0x3362, (uint16_t)(dc_gain_enabled ? 0x1 : 0x0)},
{0x3012, (uint16_t)exposure_time},
};
sensors_i2c(exp_reg_array, sizeof(exp_reg_array)/sizeof(struct i2c_random_wr_payload), CAM_SENSOR_PACKET_OPCODE_SENSOR_CONFIG, true);
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) {
// 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 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];

struct i2c_random_wr_payload exp_reg_array[] = {
{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},
};
sensors_i2c(exp_reg_array, sizeof(exp_reg_array)/sizeof(struct i2c_random_wr_payload), CAM_SENSOR_PACKET_OPCODE_SENSOR_CONFIG, false);
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);
}
}

Expand Down
3 changes: 2 additions & 1 deletion system/camerad/cameras/camera_qcom2.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -9,6 +9,7 @@

#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"

Expand Down Expand Up @@ -82,7 +83,7 @@ class CameraState {

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);
void sensors_i2c(const struct i2c_random_wr_payload* dat, int len, int op_code, bool data_word);

// Register parsing
std::map<uint16_t, std::pair<int, int>> ar0231_register_lut;
Expand Down
72 changes: 72 additions & 0 deletions system/camerad/sensors/ar0231.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,9 +2,40 @@

#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 DC_GAIN_AR0231 = 2.5;

const float DC_GAIN_ON_GREY_AR0231 = 0.2;
const float DC_GAIN_OFF_GREY_AR0231 = 0.3;

const int DC_GAIN_MIN_WEIGHT_AR0231 = 0;
const int DC_GAIN_MAX_WEIGHT_AR0231 = 1;

const float TARGET_GREY_FACTOR_AR0231 = 1.0;

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

const int ANALOG_GAIN_MIN_IDX_AR0231 = 0x1; // 0.25x
const int ANALOG_GAIN_REC_IDX_AR0231 = 0x6; // 0.8x
const int ANALOG_GAIN_MAX_IDX_AR0231 = 0xD; // 4.0x
const int ANALOG_GAIN_COST_DELTA_AR0231 = 0;
const float ANALOG_GAIN_COST_LOW_AR0231 = 0.1;
const float ANALOG_GAIN_COST_HIGH_AR0231 = 5.0;

const int EXPOSURE_TIME_MIN_AR0231 = 2; // with HDR, fastest ss
const int EXPOSURE_TIME_MAX_AR0231 = 0x0855; // with HDR, slowest ss, 40ms

std::map<uint16_t, std::pair<int, int>> 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,
Expand Down Expand Up @@ -78,6 +109,37 @@ float ar0231_parse_temp_sensor(uint16_t calib1, uint16_t calib2, uint16_t data_r

} // namespace

CameraAR0231::CameraAR0231() {
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;

dc_gain_factor = DC_GAIN_AR0231;
dc_gain_min_weight = DC_GAIN_MIN_WEIGHT_AR0231;
dc_gain_max_weight = DC_GAIN_MAX_WEIGHT_AR0231;
dc_gain_on_grey = DC_GAIN_ON_GREY_AR0231;
dc_gain_off_grey = DC_GAIN_OFF_GREY_AR0231;
exposure_time_min = EXPOSURE_TIME_MIN_AR0231;
exposure_time_max = EXPOSURE_TIME_MAX_AR0231;
analog_gain_min_idx = ANALOG_GAIN_MIN_IDX_AR0231;
analog_gain_rec_idx = ANALOG_GAIN_REC_IDX_AR0231;
analog_gain_max_idx = ANALOG_GAIN_MAX_IDX_AR0231;
analog_gain_cost_delta = ANALOG_GAIN_COST_DELTA_AR0231;
analog_gain_cost_low = ANALOG_GAIN_COST_LOW_AR0231;
analog_gain_cost_high = ANALOG_GAIN_COST_HIGH_AR0231;
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 = TARGET_GREY_FACTOR_AR0231;
}

void ar0231_process_registers(MultiCameraState *s, CameraState *c, cereal::FrameData::Builder &framed) {
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 @@ -96,3 +158,13 @@ void ar0231_process_registers(MultiCameraState *s, CameraState *c, cereal::Frame
float temp_1 = ar0231_parse_temp_sensor(registers[0x30ca], registers[0x30cc], registers[0x20b2]);
framed.setTemperaturesC({temp_0, temp_1});
}


std::vector<struct i2c_random_wr_payload> ar0231_get_exp_registers(const CameraInfo *ci, int exposure_time, int new_exp_g, bool dc_gain_enabled) {
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},
};
}
Loading

0 comments on commit ef262ff

Please sign in to comment.