sadmen
/
openpilot_comma
mirror of https://github.com/commaai/openpilot.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

camerad: c++ sensorInfo (#30650)

Browse Source 
* move remaining sensor parameters to CameraInfo

* same order

* member functions

* fix segfault
old-commit-hash: fb2f2d9cb2
pull/32199/head
Dean Lee 2 years ago committed by GitHub
parent 2a948de1f6
commit 8d17ed5bb8
5 changed files with 88 additions and 87 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 94
      system/camerad/cameras/camera_qcom2.cc
  2. 2
      system/camerad/cameras/camera_qcom2.h
  3. 28
      system/camerad/sensors/ar0231.cc
  4. 27
      system/camerad/sensors/ox03c10.cc
  5. 24
      system/camerad/sensors/sensor.h

94
system/camerad/cameras/camera_qcom2.cc
Unescape View File

@ -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<struct cam_packet>(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<AR0231>();
} 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);

2
system/camerad/cameras/camera_qcom2.h
Unescape View File

@ -49,7 +49,7 @@ public:
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();

28
system/camerad/sensors/ar0231.cc
Unescape View File

@ -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<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},
@ -148,3 +156,19 @@ std::vector<struct i2c_random_wr_payload> 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;
}

27
system/camerad/sensors/ox03c10.cc
Unescape View File

@ -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<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];
@ -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
system/camerad/sensors/sensor.h
Unescape View File

@ -1,5 +1,6 @@
#pragma once
#include <cassert>
#include <cstdint>
#include <vector>
#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<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;
@ -42,18 +47,29 @@ public:
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);

Write Preview
Loading…
Cancel
Save