IQ.Pilot Release Commit @ b6534c0
This commit is contained in:
IQ.Lvbs CI [bot]
11
iqpilot/system/camerad/SConscript
Normal file
11
iqpilot/system/camerad/SConscript
Normal file
@@ -0,0 +1,11 @@
|
||||
Import('env', 'arch', 'messaging', 'common', 'visionipc')
|
||||
|
||||
libs = [common, 'OpenCL', messaging, visionipc]
|
||||
|
||||
if arch != "Darwin":
|
||||
camera_obj = env.Object(['cameras/camera_qcom2.cc', 'cameras/camera_common.cc', 'cameras/spectra.cc',
|
||||
'cameras/cdm.cc', 'sensors/ar0231.cc', 'sensors/ox03c10.cc', 'sensors/os04c10.cc'])
|
||||
env.Program('camerad', ['main.cc', camera_obj], LIBS=libs)
|
||||
|
||||
if GetOption("extras") and arch == "x86_64":
|
||||
env.Program('test/test_ae_gray', ['test/test_ae_gray.cc', camera_obj], LIBS=libs)
|
||||
0
iqpilot/system/camerad/__init__.py
Normal file
0
iqpilot/system/camerad/__init__.py
Normal file
30
iqpilot/system/camerad/cameras/bps_blobs.h
Normal file
30
iqpilot/system/camerad/cameras/bps_blobs.h
Normal file
File diff suppressed because one or more lines are too long
110
iqpilot/system/camerad/cameras/camera_common.cc
Normal file
110
iqpilot/system/camerad/cameras/camera_common.cc
Normal file
@@ -0,0 +1,110 @@
|
||||
#include "system/camerad/cameras/camera_common.h"
|
||||
|
||||
#include <cassert>
|
||||
#include <string>
|
||||
|
||||
#include "common/swaglog.h"
|
||||
#include "system/camerad/cameras/spectra.h"
|
||||
|
||||
|
||||
void CameraBuf::init(cl_device_id device_id, cl_context context, SpectraCamera *cam, VisionIpcServer * v, int frame_cnt, VisionStreamType type) {
|
||||
vipc_server = v;
|
||||
stream_type = type;
|
||||
frame_buf_count = frame_cnt;
|
||||
|
||||
const SensorInfo *sensor = cam->sensor.get();
|
||||
|
||||
// RAW frames from ISP
|
||||
if (cam->cc.output_type != ISP_IFE_PROCESSED) {
|
||||
camera_bufs_raw = std::make_unique<VisionBuf[]>(frame_buf_count);
|
||||
|
||||
const int raw_frame_size = (sensor->frame_height + sensor->extra_height) * sensor->frame_stride;
|
||||
for (int i = 0; i < frame_buf_count; i++) {
|
||||
camera_bufs_raw[i].allocate(raw_frame_size);
|
||||
camera_bufs_raw[i].init_cl(device_id, context);
|
||||
}
|
||||
LOGD("allocated %d CL buffers", frame_buf_count);
|
||||
}
|
||||
|
||||
vipc_server->create_buffers_with_sizes(stream_type, VIPC_BUFFER_COUNT, out_img_width, out_img_height, cam->yuv_size, cam->stride, cam->uv_offset);
|
||||
LOGD("created %d YUV vipc buffers with size %dx%d", VIPC_BUFFER_COUNT, cam->stride, cam->y_height);
|
||||
}
|
||||
|
||||
CameraBuf::~CameraBuf() {
|
||||
if (camera_bufs_raw != nullptr) {
|
||||
for (int i = 0; i < frame_buf_count; i++) {
|
||||
camera_bufs_raw[i].free();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void CameraBuf::sendFrameToVipc() {
|
||||
assert(cur_buf_idx >=0 && cur_buf_idx < frame_buf_count);
|
||||
|
||||
if (camera_bufs_raw) {
|
||||
cur_camera_buf = &camera_bufs_raw[cur_buf_idx];
|
||||
}
|
||||
|
||||
cur_yuv_buf = vipc_server->get_buffer(stream_type, cur_buf_idx);
|
||||
|
||||
VisionIpcBufExtra extra = {
|
||||
cur_frame_data.frame_id,
|
||||
cur_frame_data.timestamp_sof,
|
||||
cur_frame_data.timestamp_eof,
|
||||
};
|
||||
cur_yuv_buf->set_frame_id(cur_frame_data.frame_id);
|
||||
vipc_server->send(cur_yuv_buf, &extra);
|
||||
}
|
||||
|
||||
// common functions
|
||||
|
||||
kj::Array<uint8_t> get_raw_frame_image(const CameraBuf *b) {
|
||||
const uint8_t *dat = (const uint8_t *)b->cur_camera_buf->addr;
|
||||
|
||||
kj::Array<uint8_t> frame_image = kj::heapArray<uint8_t>(b->cur_camera_buf->len);
|
||||
uint8_t *resized_dat = frame_image.begin();
|
||||
|
||||
memcpy(resized_dat, dat, b->cur_camera_buf->len);
|
||||
|
||||
return kj::mv(frame_image);
|
||||
}
|
||||
|
||||
float calculate_exposure_value(const CameraBuf *b, Rect ae_xywh, int x_skip, int y_skip) {
|
||||
int lum_med;
|
||||
uint32_t lum_binning[256] = {0};
|
||||
const uint8_t *pix_ptr = b->cur_yuv_buf->y;
|
||||
|
||||
unsigned int lum_total = 0;
|
||||
for (int y = ae_xywh.y; y < ae_xywh.y + ae_xywh.h; y += y_skip) {
|
||||
for (int x = ae_xywh.x; x < ae_xywh.x + ae_xywh.w; x += x_skip) {
|
||||
uint8_t lum = pix_ptr[(y * b->out_img_width) + x];
|
||||
lum_binning[lum]++;
|
||||
lum_total += 1;
|
||||
}
|
||||
}
|
||||
|
||||
// Find mean lumimance value
|
||||
unsigned int lum_cur = 0;
|
||||
for (lum_med = 255; lum_med >= 0; lum_med--) {
|
||||
lum_cur += lum_binning[lum_med];
|
||||
|
||||
if (lum_cur >= lum_total / 2) {
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
return lum_med / 256.0;
|
||||
}
|
||||
|
||||
int open_v4l_by_name_and_index(const char name[], int index, int flags) {
|
||||
for (int v4l_index = 0; /**/; ++v4l_index) {
|
||||
std::string v4l_name = util::read_file(util::string_format("/sys/class/video4linux/v4l-subdev%d/name", v4l_index));
|
||||
if (v4l_name.empty()) return -1;
|
||||
if (v4l_name.find(name) == 0) {
|
||||
if (index == 0) {
|
||||
return HANDLE_EINTR(open(util::string_format("/dev/v4l-subdev%d", v4l_index).c_str(), flags));
|
||||
}
|
||||
index--;
|
||||
}
|
||||
}
|
||||
}
|
||||
47
iqpilot/system/camerad/cameras/camera_common.h
Normal file
47
iqpilot/system/camerad/cameras/camera_common.h
Normal file
@@ -0,0 +1,47 @@
|
||||
#pragma once
|
||||
|
||||
#include <memory>
|
||||
|
||||
#include "cereal/messaging/messaging.h"
|
||||
#include "cereal/visionstream.h"
|
||||
#include "msgq/visionipc/visionipc_server.h"
|
||||
#include "common/util.h"
|
||||
|
||||
|
||||
const int VIPC_BUFFER_COUNT = 18;
|
||||
|
||||
typedef struct FrameMetadata {
|
||||
uint32_t frame_id;
|
||||
uint32_t request_id;
|
||||
uint64_t timestamp_sof;
|
||||
uint64_t timestamp_eof;
|
||||
float processing_time;
|
||||
} FrameMetadata;
|
||||
|
||||
class SpectraCamera;
|
||||
|
||||
class CameraBuf {
|
||||
private:
|
||||
int frame_buf_count;
|
||||
|
||||
public:
|
||||
VisionIpcServer *vipc_server;
|
||||
VisionStreamType stream_type;
|
||||
|
||||
int cur_buf_idx;
|
||||
FrameMetadata cur_frame_data;
|
||||
VisionBuf *cur_yuv_buf;
|
||||
VisionBuf *cur_camera_buf;
|
||||
std::unique_ptr<VisionBuf[]> camera_bufs_raw;
|
||||
uint32_t out_img_width, out_img_height;
|
||||
|
||||
CameraBuf() = default;
|
||||
~CameraBuf();
|
||||
void init(cl_device_id device_id, cl_context context, SpectraCamera *cam, VisionIpcServer * v, int frame_cnt, VisionStreamType type);
|
||||
void sendFrameToVipc();
|
||||
};
|
||||
|
||||
void camerad_thread();
|
||||
kj::Array<uint8_t> get_raw_frame_image(const CameraBuf *b);
|
||||
float calculate_exposure_value(const CameraBuf *b, Rect ae_xywh, int x_skip, int y_skip);
|
||||
int open_v4l_by_name_and_index(const char name[], int index = 0, int flags = O_RDWR | O_NONBLOCK);
|
||||
351
iqpilot/system/camerad/cameras/camera_qcom2.cc
Normal file
351
iqpilot/system/camerad/cameras/camera_qcom2.cc
Normal file
@@ -0,0 +1,351 @@
|
||||
#include "system/camerad/cameras/camera_common.h"
|
||||
#include "system/camerad/cameras/spectra.h"
|
||||
|
||||
#include <poll.h>
|
||||
#include <sys/ioctl.h>
|
||||
|
||||
#include <algorithm>
|
||||
#include <cassert>
|
||||
#include <cerrno>
|
||||
#include <cmath>
|
||||
#include <cstdlib>
|
||||
#include <cstring>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
#ifdef __TICI__
|
||||
#include "CL/cl_ext_qcom.h"
|
||||
#else
|
||||
#define CL_PRIORITY_HINT_HIGH_QCOM NULL
|
||||
#define CL_CONTEXT_PRIORITY_HINT_QCOM NULL
|
||||
#endif
|
||||
|
||||
#include "media/cam_sensor_cmn_header.h"
|
||||
|
||||
#include "common/clutil.h"
|
||||
#include "common/params.h"
|
||||
#include "common/swaglog.h"
|
||||
#include "common/timing.h"
|
||||
|
||||
|
||||
ExitHandler do_exit;
|
||||
|
||||
// for debugging
|
||||
const bool env_debug_frames = getenv("DEBUG_FRAMES") != nullptr;
|
||||
const bool env_log_raw_frames = getenv("LOG_RAW_FRAMES") != nullptr;
|
||||
const bool env_ctrl_exp_from_params = getenv("CTRL_EXP_FROM_PARAMS") != nullptr;
|
||||
|
||||
|
||||
class CameraState {
|
||||
public:
|
||||
SpectraCamera camera;
|
||||
int exposure_time = 5;
|
||||
bool dc_gain_enabled = false;
|
||||
int dc_gain_weight = 0;
|
||||
int gain_idx = 0;
|
||||
float analog_gain_frac = 0;
|
||||
|
||||
float cur_ev[3] = {};
|
||||
float best_ev_score = 0;
|
||||
int new_exp_g = 0;
|
||||
int new_exp_t = 0;
|
||||
|
||||
Rect ae_xywh = {};
|
||||
float measured_grey_fraction = 0;
|
||||
float target_grey_fraction = 0.125;
|
||||
|
||||
float fl_pix = 0;
|
||||
std::unique_ptr<PubMaster> pm;
|
||||
|
||||
CameraState(SpectraMaster *master, const CameraConfig &config) : camera(master, config) {};
|
||||
~CameraState();
|
||||
void init(VisionIpcServer *v, cl_device_id device_id, cl_context ctx);
|
||||
void update_exposure_score(float desired_ev, int exp_t, int exp_g_idx, float exp_gain);
|
||||
void set_camera_exposure(float grey_frac);
|
||||
void set_exposure_rect();
|
||||
void sendState();
|
||||
|
||||
float get_gain_factor() const {
|
||||
return (1 + dc_gain_weight * (camera.sensor->dc_gain_factor-1) / camera.sensor->dc_gain_max_weight);
|
||||
}
|
||||
};
|
||||
|
||||
void CameraState::init(VisionIpcServer *v, cl_device_id device_id, cl_context ctx) {
|
||||
camera.camera_open(v, device_id, ctx);
|
||||
|
||||
if (!camera.enabled) return;
|
||||
|
||||
fl_pix = camera.cc.focal_len / camera.sensor->pixel_size_mm / camera.sensor->out_scale;
|
||||
set_exposure_rect();
|
||||
|
||||
dc_gain_weight = camera.sensor->dc_gain_min_weight;
|
||||
gain_idx = camera.sensor->analog_gain_rec_idx;
|
||||
cur_ev[0] = cur_ev[1] = cur_ev[2] = get_gain_factor() * camera.sensor->sensor_analog_gains[gain_idx] * exposure_time;
|
||||
|
||||
pm = std::make_unique<PubMaster>(std::vector{camera.cc.publish_name});
|
||||
}
|
||||
|
||||
CameraState::~CameraState() {}
|
||||
|
||||
void CameraState::set_exposure_rect() {
|
||||
// set areas for each camera, shouldn't be changed
|
||||
std::vector<std::pair<Rect, float>> ae_targets = {
|
||||
// (Rect, F)
|
||||
std::make_pair((Rect){96, 400, 1734, 524}, 567.0), // wide
|
||||
std::make_pair((Rect){96, 160, 1734, 986}, 2648.0), // road
|
||||
std::make_pair((Rect){96, 242, 1736, 906}, 567.0) // driver
|
||||
};
|
||||
int h_ref = 1208;
|
||||
/*
|
||||
exposure target intrinsics is
|
||||
[
|
||||
[F, 0, 0.5*ae_xywh[2]]
|
||||
[0, F, 0.5*H-ae_xywh[1]]
|
||||
[0, 0, 1]
|
||||
]
|
||||
*/
|
||||
auto ae_target = ae_targets[camera.cc.camera_num];
|
||||
Rect xywh_ref = ae_target.first;
|
||||
float fl_ref = ae_target.second;
|
||||
|
||||
ae_xywh = (Rect){
|
||||
std::max(0, (int)camera.buf.out_img_width / 2 - (int)(fl_pix / fl_ref * xywh_ref.w / 2)),
|
||||
std::max(0, (int)camera.buf.out_img_height / 2 - (int)(fl_pix / fl_ref * (h_ref / 2 - xywh_ref.y))),
|
||||
std::min((int)(fl_pix / fl_ref * xywh_ref.w), (int)camera.buf.out_img_width / 2 + (int)(fl_pix / fl_ref * xywh_ref.w / 2)),
|
||||
std::min((int)(fl_pix / fl_ref * xywh_ref.h), (int)camera.buf.out_img_height / 2 + (int)(fl_pix / fl_ref * (h_ref / 2 - xywh_ref.y)))
|
||||
};
|
||||
}
|
||||
|
||||
void CameraState::update_exposure_score(float desired_ev, int exp_t, int exp_g_idx, float exp_gain) {
|
||||
float score = camera.sensor->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;
|
||||
best_ev_score = score;
|
||||
}
|
||||
}
|
||||
|
||||
void CameraState::set_camera_exposure(float grey_frac) {
|
||||
if (!camera.enabled) return;
|
||||
std::vector<double> target_grey_minimums = {0.1, 0.1, 0.125}; // wide, road, driver
|
||||
|
||||
const float dt = 0.05;
|
||||
|
||||
const float ts_grey = 10.0;
|
||||
const float ts_ev = 0.05;
|
||||
|
||||
const float k_grey = (dt / ts_grey) / (1.0 + dt / ts_grey);
|
||||
const float k_ev = (dt / ts_ev) / (1.0 + dt / ts_ev);
|
||||
|
||||
// It takes 3 frames for the commanded exposure settings to take effect. The first frame is already started by the time
|
||||
// we reach this function, the other 2 are due to the register buffering in the sensor.
|
||||
// Therefore we use the target EV from 3 frames ago, the grey fraction that was just measured was the result of that control action.
|
||||
// TODO: Lower latency to 2 frames, by using the histogram outputted by the sensor we can do AE before the debayering is complete
|
||||
|
||||
const auto &sensor = camera.sensor;
|
||||
// Offset idx by one to not get stuck in self loop
|
||||
const float cur_ev_ = cur_ev[(camera.buf.cur_frame_data.frame_id - 1) % 3] * sensor->ev_scale;
|
||||
|
||||
// Scale target grey between min and 0.4 depending on lighting conditions
|
||||
float new_target_grey = std::clamp(0.4 - 0.3 * log2(1.0 + sensor->target_grey_factor*cur_ev_) / log2(6000.0), target_grey_minimums[camera.cc.camera_num], 0.4);
|
||||
float target_grey = (1.0 - k_grey) * target_grey_fraction + k_grey * new_target_grey;
|
||||
|
||||
float desired_ev = std::clamp(cur_ev_ / sensor->ev_scale * target_grey / grey_frac, sensor->min_ev, sensor->max_ev);
|
||||
float k = (1.0 - k_ev) / 3.0;
|
||||
desired_ev = (k * cur_ev[0]) + (k * cur_ev[1]) + (k * cur_ev[2]) + (k_ev * desired_ev);
|
||||
|
||||
best_ev_score = 1e6;
|
||||
new_exp_g = 0;
|
||||
new_exp_t = 0;
|
||||
|
||||
// Hysteresis around high conversion gain
|
||||
// We usually want this on since it results in lower noise, but turn off in very bright day scenes
|
||||
bool enable_dc_gain = dc_gain_enabled;
|
||||
if (!enable_dc_gain && target_grey < sensor->dc_gain_on_grey) {
|
||||
enable_dc_gain = true;
|
||||
dc_gain_weight = sensor->dc_gain_min_weight;
|
||||
} else if (enable_dc_gain && target_grey > sensor->dc_gain_off_grey) {
|
||||
enable_dc_gain = false;
|
||||
dc_gain_weight = sensor->dc_gain_max_weight;
|
||||
}
|
||||
|
||||
if (enable_dc_gain && dc_gain_weight < sensor->dc_gain_max_weight) {dc_gain_weight += 1;}
|
||||
if (!enable_dc_gain && dc_gain_weight > sensor->dc_gain_min_weight) {dc_gain_weight -= 1;}
|
||||
|
||||
std::string gain_bytes, time_bytes;
|
||||
if (env_ctrl_exp_from_params) {
|
||||
static Params params;
|
||||
gain_bytes = params.get("CameraDebugExpGain");
|
||||
time_bytes = params.get("CameraDebugExpTime");
|
||||
}
|
||||
|
||||
if (gain_bytes.size() > 0 && time_bytes.size() > 0) {
|
||||
// Override gain and exposure time
|
||||
gain_idx = std::stoi(gain_bytes);
|
||||
exposure_time = std::stoi(time_bytes);
|
||||
|
||||
new_exp_g = gain_idx;
|
||||
new_exp_t = exposure_time;
|
||||
enable_dc_gain = false;
|
||||
} else {
|
||||
// Simple brute force optimizer to choose sensor parameters to reach desired EV
|
||||
int min_g = std::max(gain_idx - 1, sensor->analog_gain_min_idx);
|
||||
int max_g = std::min(gain_idx + 1, sensor->analog_gain_max_idx);
|
||||
for (int g = min_g; g <= max_g; g++) {
|
||||
float gain = sensor->sensor_analog_gains[g] * get_gain_factor();
|
||||
|
||||
// Compute optimal time for given gain
|
||||
int t = std::clamp(int(std::round(desired_ev / gain)), sensor->exposure_time_min, sensor->exposure_time_max);
|
||||
|
||||
// Only go below recommended gain when absolutely necessary to not overexpose
|
||||
if (g < sensor->analog_gain_rec_idx && t > 20 && g < gain_idx) {
|
||||
continue;
|
||||
}
|
||||
|
||||
update_exposure_score(desired_ev, t, g, gain);
|
||||
}
|
||||
}
|
||||
|
||||
measured_grey_fraction = grey_frac;
|
||||
target_grey_fraction = target_grey;
|
||||
|
||||
analog_gain_frac = sensor->sensor_analog_gains[new_exp_g];
|
||||
gain_idx = new_exp_g;
|
||||
exposure_time = new_exp_t;
|
||||
dc_gain_enabled = enable_dc_gain;
|
||||
|
||||
float gain = analog_gain_frac * get_gain_factor();
|
||||
cur_ev[camera.buf.cur_frame_data.frame_id % 3] = exposure_time * gain;
|
||||
|
||||
// LOGE("ae - camera %d, cur_t %.5f, sof %.5f, dt %.5f", camera.cc.camera_num, 1e-9 * nanos_since_boot(), 1e-9 * camera.buf.cur_frame_data.timestamp_sof, 1e-9 * (nanos_since_boot() - camera.buf.cur_frame_data.timestamp_sof));
|
||||
|
||||
auto exp_reg_array = sensor->getExposureRegisters(exposure_time, new_exp_g, dc_gain_enabled);
|
||||
camera.sensors_i2c(exp_reg_array.data(), exp_reg_array.size(), CAM_SENSOR_PACKET_OPCODE_SENSOR_CONFIG, camera.sensor->data_word);
|
||||
}
|
||||
|
||||
void CameraState::sendState() {
|
||||
camera.buf.sendFrameToVipc();
|
||||
|
||||
MessageBuilder msg;
|
||||
auto framed = (msg.initEvent().*camera.cc.init_camera_state)();
|
||||
const FrameMetadata &meta = camera.buf.cur_frame_data;
|
||||
framed.setFrameId(meta.frame_id);
|
||||
framed.setRequestId(meta.request_id);
|
||||
framed.setTimestampEof(meta.timestamp_eof);
|
||||
framed.setTimestampSof(meta.timestamp_sof);
|
||||
framed.setIntegLines(exposure_time);
|
||||
framed.setGain(analog_gain_frac * get_gain_factor());
|
||||
framed.setHighConversionGain(dc_gain_enabled);
|
||||
framed.setMeasuredGreyFraction(measured_grey_fraction);
|
||||
framed.setTargetGreyFraction(target_grey_fraction);
|
||||
framed.setProcessingTime(meta.processing_time);
|
||||
|
||||
const float ev = cur_ev[meta.frame_id % 3];
|
||||
const float perc = util::map_val(ev, camera.sensor->min_ev, camera.sensor->max_ev, 0.0f, 100.0f);
|
||||
framed.setExposureValPercent(perc);
|
||||
framed.setSensor(camera.sensor->image_sensor);
|
||||
|
||||
// Log raw frames for road camera
|
||||
if (env_log_raw_frames && camera.cc.stream_type == VISION_STREAM_ROAD && meta.frame_id % 100 == 5) { // no overlap with qlog decimation
|
||||
framed.setImage(get_raw_frame_image(&camera.buf));
|
||||
}
|
||||
|
||||
set_camera_exposure(calculate_exposure_value(&camera.buf, ae_xywh, 2, camera.cc.stream_type != VISION_STREAM_DRIVER ? 2 : 4));
|
||||
|
||||
// Send the message
|
||||
pm->send(camera.cc.publish_name, msg);
|
||||
}
|
||||
|
||||
void camerad_thread() {
|
||||
// TODO: centralize enabled handling
|
||||
|
||||
cl_device_id device_id = cl_get_device_id(CL_DEVICE_TYPE_DEFAULT);
|
||||
const cl_context_properties props[] = {CL_CONTEXT_PRIORITY_HINT_QCOM, CL_PRIORITY_HINT_HIGH_QCOM, 0};
|
||||
cl_context ctx = CL_CHECK_ERR(clCreateContext(props, 1, &device_id, NULL, NULL, &err));
|
||||
|
||||
VisionIpcServer v("camerad", device_id, ctx);
|
||||
|
||||
// *** initial ISP init ***
|
||||
SpectraMaster m;
|
||||
m.init();
|
||||
|
||||
// *** per-cam init ***
|
||||
Params params;
|
||||
const bool wide_cam_faulty = params.getBool("WideCamFaulty");
|
||||
bool wide_fault_latched = wide_cam_faulty;
|
||||
std::vector<std::unique_ptr<CameraState>> cams;
|
||||
for (CameraConfig config : ALL_CAMERA_CONFIGS) {
|
||||
if (config.stream_type == VISION_STREAM_WIDE_ROAD && wide_cam_faulty) {
|
||||
config.enabled = false;
|
||||
}
|
||||
auto cam = std::make_unique<CameraState>(&m, config);
|
||||
cam->init(&v, device_id, ctx);
|
||||
cams.emplace_back(std::move(cam));
|
||||
}
|
||||
|
||||
// start devices
|
||||
LOG("-- Starting devices");
|
||||
for (auto &cam : cams) cam->camera.sensors_start();
|
||||
|
||||
bool listening = false;
|
||||
const double listener_deadline = millis_since_boot() + 10000;
|
||||
|
||||
// poll events
|
||||
LOG("-- Dequeueing Video events");
|
||||
while (!do_exit) {
|
||||
if (!wide_fault_latched && SpectraCamera::wideCamFault() != SpectraCamera::WideCamFault::none) {
|
||||
wide_fault_latched = true;
|
||||
params.putBool("WideCamFaulty", true);
|
||||
if (SpectraCamera::wideCamFault() == SpectraCamera::WideCamFault::noFrames) {
|
||||
LOGE("wide road camera advertised a stream it never fills, restarting without it");
|
||||
cams.clear();
|
||||
exit(0);
|
||||
}
|
||||
}
|
||||
|
||||
if (!listening && (SpectraCamera::firstFrameSynced() || millis_since_boot() > listener_deadline)) {
|
||||
v.start_listener();
|
||||
listening = true;
|
||||
}
|
||||
|
||||
struct pollfd fds[1] = {{.fd = m.video0_fd, .events = POLLPRI}};
|
||||
int ret = poll(fds, std::size(fds), 1000);
|
||||
if (ret < 0) {
|
||||
if (errno == EINTR || errno == EAGAIN) continue;
|
||||
LOGE("poll failed (%d - %d)", ret, errno);
|
||||
break;
|
||||
}
|
||||
|
||||
if (!(fds[0].revents & POLLPRI)) continue;
|
||||
|
||||
struct v4l2_event ev = {0};
|
||||
ret = HANDLE_EINTR(ioctl(fds[0].fd, VIDIOC_DQEVENT, &ev));
|
||||
if (ret == 0) {
|
||||
if (ev.type == V4L_EVENT_CAM_REQ_MGR_EVENT) {
|
||||
struct cam_req_mgr_message *event_data = (struct cam_req_mgr_message *)ev.u.data;
|
||||
if (env_debug_frames) {
|
||||
printf("sess_hdl 0x%6X, link_hdl 0x%6X, frame_id %lu, req_id %lu, timestamp %.2f ms, sof_status %d\n", event_data->session_hdl, event_data->u.frame_msg.link_hdl,
|
||||
event_data->u.frame_msg.frame_id, event_data->u.frame_msg.request_id, event_data->u.frame_msg.timestamp/1e6, event_data->u.frame_msg.sof_status);
|
||||
do_exit = do_exit || event_data->u.frame_msg.frame_id > (1*20);
|
||||
}
|
||||
|
||||
for (auto &cam : cams) {
|
||||
if (event_data->session_hdl == cam->camera.session_handle) {
|
||||
if (cam->camera.handle_camera_event(event_data)) {
|
||||
cam->sendState();
|
||||
}
|
||||
break;
|
||||
}
|
||||
}
|
||||
} else {
|
||||
LOGE("unhandled event %d\n", ev.type);
|
||||
}
|
||||
} else {
|
||||
LOGE("VIDIOC_DQEVENT failed, errno=%d", errno);
|
||||
}
|
||||
}
|
||||
|
||||
if (!listening) {
|
||||
v.start_listener();
|
||||
}
|
||||
}
|
||||
47
iqpilot/system/camerad/cameras/cdm.cc
Normal file
47
iqpilot/system/camerad/cameras/cdm.cc
Normal file
@@ -0,0 +1,47 @@
|
||||
#include "cdm.h"
|
||||
#include "stddef.h"
|
||||
|
||||
int write_dmi(uint8_t *dst, uint64_t *addr, uint32_t length, uint32_t dmi_addr, uint8_t sel, uint8_t opcode) {
|
||||
struct cdm_dmi_cmd *cmd = (struct cdm_dmi_cmd*)dst;
|
||||
cmd->cmd = opcode;
|
||||
cmd->length = length - 1;
|
||||
cmd->reserved = 0;
|
||||
cmd->addr = 0; // gets patched in
|
||||
cmd->DMIAddr = dmi_addr;
|
||||
cmd->DMISel = sel;
|
||||
|
||||
*addr = (uint64_t)(dst + offsetof(struct cdm_dmi_cmd, addr));
|
||||
return sizeof(struct cdm_dmi_cmd);
|
||||
}
|
||||
|
||||
int write_cont(uint8_t *dst, uint32_t reg, const std::vector<uint32_t> &vals) {
|
||||
struct cdm_regcontinuous_cmd *cmd = (struct cdm_regcontinuous_cmd*)dst;
|
||||
cmd->cmd = CAM_CDM_CMD_REG_CONT;
|
||||
cmd->count = vals.size();
|
||||
cmd->offset = reg;
|
||||
cmd->reserved0 = 0;
|
||||
cmd->reserved1 = 0;
|
||||
|
||||
uint32_t *vd = (uint32_t*)(dst + sizeof(struct cdm_regcontinuous_cmd));
|
||||
for (int i = 0; i < vals.size(); i++) {
|
||||
*vd = vals[i];
|
||||
vd++;
|
||||
}
|
||||
|
||||
return sizeof(struct cdm_regcontinuous_cmd) + vals.size()*sizeof(uint32_t);
|
||||
}
|
||||
|
||||
int write_random(uint8_t *dst, const std::vector<uint32_t> &vals) {
|
||||
struct cdm_regrandom_cmd *cmd = (struct cdm_regrandom_cmd*)dst;
|
||||
cmd->cmd = CAM_CDM_CMD_REG_RANDOM;
|
||||
cmd->count = vals.size() / 2;
|
||||
cmd->reserved = 0;
|
||||
|
||||
uint32_t *vd = (uint32_t*)(dst + sizeof(struct cdm_regrandom_cmd));
|
||||
for (int i = 0; i < vals.size(); i++) {
|
||||
*vd = vals[i];
|
||||
vd++;
|
||||
}
|
||||
|
||||
return sizeof(struct cdm_regrandom_cmd) + vals.size()*sizeof(uint32_t);
|
||||
}
|
||||
79
iqpilot/system/camerad/cameras/cdm.h
Normal file
79
iqpilot/system/camerad/cameras/cdm.h
Normal file
@@ -0,0 +1,79 @@
|
||||
#pragma once
|
||||
|
||||
#include <stdint.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
#include <vector>
|
||||
#include <memory>
|
||||
|
||||
// from drivers/media/platform/msm/camera/cam_cdm/cam_cdm_util.{c,h}
|
||||
|
||||
enum cam_cdm_command {
|
||||
CAM_CDM_CMD_UNUSED = 0x0,
|
||||
CAM_CDM_CMD_DMI = 0x1,
|
||||
CAM_CDM_CMD_NOT_DEFINED = 0x2,
|
||||
CAM_CDM_CMD_REG_CONT = 0x3,
|
||||
CAM_CDM_CMD_REG_RANDOM = 0x4,
|
||||
CAM_CDM_CMD_BUFF_INDIRECT = 0x5,
|
||||
CAM_CDM_CMD_GEN_IRQ = 0x6,
|
||||
CAM_CDM_CMD_WAIT_EVENT = 0x7,
|
||||
CAM_CDM_CMD_CHANGE_BASE = 0x8,
|
||||
CAM_CDM_CMD_PERF_CTRL = 0x9,
|
||||
CAM_CDM_CMD_DMI_32 = 0xa,
|
||||
CAM_CDM_CMD_DMI_64 = 0xb,
|
||||
CAM_CDM_CMD_PRIVATE_BASE = 0xc,
|
||||
CAM_CDM_CMD_SWD_DMI_32 = (CAM_CDM_CMD_PRIVATE_BASE + 0x64),
|
||||
CAM_CDM_CMD_SWD_DMI_64 = (CAM_CDM_CMD_PRIVATE_BASE + 0x65),
|
||||
CAM_CDM_CMD_PRIVATE_BASE_MAX = 0x7F
|
||||
};
|
||||
|
||||
// our helpers
|
||||
int write_random(uint8_t *dst, const std::vector<uint32_t> &vals);
|
||||
int write_cont(uint8_t *dst, uint32_t reg, const std::vector<uint32_t> &vals);
|
||||
int write_dmi(uint8_t *dst, uint64_t *addr, uint32_t length, uint32_t dmi_addr, uint8_t sel, uint8_t opcode = CAM_CDM_CMD_DMI_32);
|
||||
|
||||
/**
|
||||
* struct cdm_regrandom_cmd - Definition for CDM random register command.
|
||||
* @count: Number of register writes
|
||||
* @reserved: reserved bits
|
||||
* @cmd: Command ID (CDMCmd)
|
||||
*/
|
||||
struct cdm_regrandom_cmd {
|
||||
unsigned int count : 16;
|
||||
unsigned int reserved : 8;
|
||||
unsigned int cmd : 8;
|
||||
} __attribute__((__packed__));
|
||||
|
||||
/**
|
||||
* struct cdm_regcontinuous_cmd - Definition for a CDM register range command.
|
||||
* @count: Number of register writes
|
||||
* @reserved0: reserved bits
|
||||
* @cmd: Command ID (CDMCmd)
|
||||
* @offset: Start address of the range of registers
|
||||
* @reserved1: reserved bits
|
||||
*/
|
||||
struct cdm_regcontinuous_cmd {
|
||||
unsigned int count : 16;
|
||||
unsigned int reserved0 : 8;
|
||||
unsigned int cmd : 8;
|
||||
unsigned int offset : 24;
|
||||
unsigned int reserved1 : 8;
|
||||
} __attribute__((__packed__));
|
||||
|
||||
/**
|
||||
* struct cdm_dmi_cmd - Definition for a CDM DMI command.
|
||||
* @length: Number of bytes in LUT - 1
|
||||
* @reserved: reserved bits
|
||||
* @cmd: Command ID (CDMCmd)
|
||||
* @addr: Address of the LUT in memory
|
||||
* @DMIAddr: Address of the target DMI config register
|
||||
* @DMISel: DMI identifier
|
||||
*/
|
||||
struct cdm_dmi_cmd {
|
||||
unsigned int length : 16;
|
||||
unsigned int reserved : 8;
|
||||
unsigned int cmd : 8;
|
||||
unsigned int addr;
|
||||
unsigned int DMIAddr : 24;
|
||||
unsigned int DMISel : 8;
|
||||
} __attribute__((__packed__));
|
||||
69
iqpilot/system/camerad/cameras/hw.h
Normal file
69
iqpilot/system/camerad/cameras/hw.h
Normal file
@@ -0,0 +1,69 @@
|
||||
#pragma once
|
||||
|
||||
#include "common/util.h"
|
||||
#include "cereal/gen/cpp/log.capnp.h"
|
||||
#include "cereal/visionstream.h"
|
||||
#include "msgq/visionipc/visionipc_server.h"
|
||||
|
||||
#include "media/cam_isp_ife.h"
|
||||
|
||||
|
||||
typedef enum {
|
||||
ISP_RAW_OUTPUT, // raw frame from sensor
|
||||
ISP_IFE_PROCESSED, // fully processed image through the IFE
|
||||
ISP_BPS_PROCESSED, // fully processed image through the BPS
|
||||
} SpectraOutputType;
|
||||
|
||||
// For the comma 3X three camera platform
|
||||
|
||||
struct CameraConfig {
|
||||
int camera_num;
|
||||
VisionStreamType stream_type;
|
||||
float focal_len; // millimeters
|
||||
const char *publish_name;
|
||||
cereal::FrameData::Builder (cereal::Event::Builder::*init_camera_state)();
|
||||
bool enabled;
|
||||
uint32_t phy;
|
||||
bool vignetting_correction;
|
||||
SpectraOutputType output_type;
|
||||
};
|
||||
|
||||
// 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!)
|
||||
const CameraConfig WIDE_ROAD_CAMERA_CONFIG = {
|
||||
.camera_num = 0,
|
||||
.stream_type = VISION_STREAM_WIDE_ROAD,
|
||||
.focal_len = 1.71,
|
||||
.publish_name = "wideRoadCameraState",
|
||||
.init_camera_state = &cereal::Event::Builder::initWideRoadCameraState,
|
||||
.enabled = !getenv("DISABLE_WIDE_ROAD"),
|
||||
.phy = CAM_ISP_IFE_IN_RES_PHY_0,
|
||||
.vignetting_correction = false,
|
||||
.output_type = ISP_IFE_PROCESSED,
|
||||
};
|
||||
|
||||
const CameraConfig ROAD_CAMERA_CONFIG = {
|
||||
.camera_num = 1,
|
||||
.stream_type = VISION_STREAM_ROAD,
|
||||
.focal_len = 8.0,
|
||||
.publish_name = "roadCameraState",
|
||||
.init_camera_state = &cereal::Event::Builder::initRoadCameraState,
|
||||
.enabled = !getenv("DISABLE_ROAD"),
|
||||
.phy = CAM_ISP_IFE_IN_RES_PHY_1,
|
||||
.vignetting_correction = true,
|
||||
.output_type = ISP_IFE_PROCESSED,
|
||||
};
|
||||
|
||||
const CameraConfig DRIVER_CAMERA_CONFIG = {
|
||||
.camera_num = 2,
|
||||
.stream_type = VISION_STREAM_DRIVER,
|
||||
.focal_len = 1.71,
|
||||
.publish_name = "driverCameraState",
|
||||
.init_camera_state = &cereal::Event::Builder::initDriverCameraState,
|
||||
.enabled = !getenv("DISABLE_DRIVER"),
|
||||
.phy = CAM_ISP_IFE_IN_RES_PHY_2,
|
||||
.vignetting_correction = false,
|
||||
.output_type = ISP_BPS_PROCESSED,
|
||||
};
|
||||
|
||||
const CameraConfig ALL_CAMERA_CONFIGS[] = {WIDE_ROAD_CAMERA_CONFIG, ROAD_CAMERA_CONFIG, DRIVER_CAMERA_CONFIG};
|
||||
236
iqpilot/system/camerad/cameras/ife.h
Normal file
236
iqpilot/system/camerad/cameras/ife.h
Normal file
@@ -0,0 +1,236 @@
|
||||
#pragma once
|
||||
|
||||
#include "cdm.h"
|
||||
|
||||
#include "system/camerad/cameras/hw.h"
|
||||
#include "system/camerad/sensors/sensor.h"
|
||||
|
||||
int build_common_ife_bps(uint8_t *dst, const CameraConfig cam, const SensorInfo *s, std::vector<uint32_t> &patches, bool ife) {
|
||||
uint8_t *start = dst;
|
||||
|
||||
/*
|
||||
Common between IFE and BPS.
|
||||
*/
|
||||
|
||||
// IFE -> BPS addresses
|
||||
/*
|
||||
std::map<uint32_t, uint32_t> addrs = {
|
||||
{0xf30, 0x3468},
|
||||
};
|
||||
*/
|
||||
|
||||
// YUV
|
||||
dst += write_cont(dst, ife ? 0xf30 : 0x3468, {
|
||||
0x00680208,
|
||||
0x00000108,
|
||||
0x00400000,
|
||||
0x03ff0000,
|
||||
0x01c01ed8,
|
||||
0x00001f68,
|
||||
0x02000000,
|
||||
0x03ff0000,
|
||||
0x1fb81e88,
|
||||
0x000001c0,
|
||||
0x02000000,
|
||||
0x03ff0000,
|
||||
});
|
||||
|
||||
return dst - start;
|
||||
}
|
||||
|
||||
int build_update(uint8_t *dst, const CameraConfig cam, const SensorInfo *s, std::vector<uint32_t> &patches) {
|
||||
uint8_t *start = dst;
|
||||
|
||||
// init sequence
|
||||
dst += write_random(dst, {
|
||||
0x2c, 0xffffffff,
|
||||
0x30, 0xffffffff,
|
||||
0x34, 0xffffffff,
|
||||
0x38, 0xffffffff,
|
||||
0x3c, 0xffffffff,
|
||||
});
|
||||
|
||||
// demux cfg
|
||||
dst += write_cont(dst, 0x560, {
|
||||
0x00000001,
|
||||
0x04440444,
|
||||
0x04450445,
|
||||
0x04440444,
|
||||
0x04450445,
|
||||
0x000000ca,
|
||||
0x0000009c,
|
||||
});
|
||||
|
||||
// white balance
|
||||
dst += write_cont(dst, 0x6fc, {
|
||||
0x00800080,
|
||||
0x00000080,
|
||||
0x00000000,
|
||||
0x00000000,
|
||||
});
|
||||
|
||||
// module config/enables (e.g. enable debayer, white balance, etc.)
|
||||
dst += write_cont(dst, 0x40, {
|
||||
0x00000c06 | ((uint32_t)(cam.vignetting_correction) << 8),
|
||||
});
|
||||
dst += write_cont(dst, 0x44, {
|
||||
0x00000000,
|
||||
});
|
||||
dst += write_cont(dst, 0x48, {
|
||||
(1 << 3) | (1 << 1),
|
||||
});
|
||||
dst += write_cont(dst, 0x4c, {
|
||||
0x00000019,
|
||||
});
|
||||
dst += write_cont(dst, 0xf00, {
|
||||
0x00000000,
|
||||
});
|
||||
|
||||
// cropping
|
||||
dst += write_cont(dst, 0xe0c, {
|
||||
0x00000e00,
|
||||
});
|
||||
dst += write_cont(dst, 0xe2c, {
|
||||
0x00000e00,
|
||||
});
|
||||
|
||||
// black level scale + offset
|
||||
dst += write_cont(dst, 0x6b0, {
|
||||
((uint32_t)(1 << 11) << 0xf) | (s->black_level << (14 - s->bits_per_pixel)),
|
||||
0x0,
|
||||
0x0,
|
||||
});
|
||||
|
||||
return dst - start;
|
||||
}
|
||||
|
||||
|
||||
int build_initial_config(uint8_t *dst, const CameraConfig cam, const SensorInfo *s, std::vector<uint32_t> &patches, uint32_t out_width, uint32_t out_height) {
|
||||
uint8_t *start = dst;
|
||||
|
||||
// start with the every frame config
|
||||
dst += build_update(dst, cam, s, patches);
|
||||
|
||||
uint64_t addr;
|
||||
|
||||
// setup
|
||||
dst += write_cont(dst, 0x478, {
|
||||
0x00000004,
|
||||
0x004000c0,
|
||||
});
|
||||
dst += write_cont(dst, 0x488, {
|
||||
0x00000000,
|
||||
0x00000000,
|
||||
0x00000f0f,
|
||||
});
|
||||
dst += write_cont(dst, 0x49c, {
|
||||
0x00000001,
|
||||
});
|
||||
dst += write_cont(dst, 0xce4, {
|
||||
0x00000000,
|
||||
0x00000000,
|
||||
});
|
||||
|
||||
// linearization
|
||||
dst += write_cont(dst, 0x4dc, {
|
||||
0x00000000,
|
||||
});
|
||||
dst += write_cont(dst, 0x4e0, s->linearization_pts);
|
||||
dst += write_cont(dst, 0x4f0, s->linearization_pts);
|
||||
dst += write_cont(dst, 0x500, s->linearization_pts);
|
||||
dst += write_cont(dst, 0x510, s->linearization_pts);
|
||||
// TODO: this is DMI64 in the dump, does that matter?
|
||||
dst += write_dmi(dst, &addr, s->linearization_lut.size()*sizeof(uint32_t), 0xc24, 9);
|
||||
patches.push_back(addr - (uint64_t)start);
|
||||
|
||||
// vignetting correction
|
||||
dst += write_cont(dst, 0x6bc, {
|
||||
0x0b3c0000,
|
||||
0x00670067,
|
||||
0xd3b1300c,
|
||||
0x13b1300c,
|
||||
});
|
||||
dst += write_cont(dst, 0x6d8, {
|
||||
0xec4e4000,
|
||||
0x0100c003,
|
||||
});
|
||||
dst += write_dmi(dst, &addr, s->vignetting_lut.size()*sizeof(uint32_t), 0xc24, 14); // GRR
|
||||
patches.push_back(addr - (uint64_t)start);
|
||||
dst += write_dmi(dst, &addr, s->vignetting_lut.size()*sizeof(uint32_t), 0xc24, 15); // GBB
|
||||
patches.push_back(addr - (uint64_t)start);
|
||||
|
||||
// debayer
|
||||
dst += write_cont(dst, 0x6f8, {
|
||||
0x00000100,
|
||||
});
|
||||
dst += write_cont(dst, 0x71c, {
|
||||
0x00008000,
|
||||
0x08000066,
|
||||
});
|
||||
|
||||
// color correction
|
||||
dst += write_cont(dst, 0x760, s->color_correct_matrix);
|
||||
|
||||
// gamma
|
||||
dst += write_cont(dst, 0x798, {
|
||||
0x00000000,
|
||||
});
|
||||
dst += write_dmi(dst, &addr, s->gamma_lut_rgb.size()*sizeof(uint32_t), 0xc24, 26); // G
|
||||
patches.push_back(addr - (uint64_t)start);
|
||||
dst += write_dmi(dst, &addr, s->gamma_lut_rgb.size()*sizeof(uint32_t), 0xc24, 28); // B
|
||||
patches.push_back(addr - (uint64_t)start);
|
||||
dst += write_dmi(dst, &addr, s->gamma_lut_rgb.size()*sizeof(uint32_t), 0xc24, 30); // R
|
||||
patches.push_back(addr - (uint64_t)start);
|
||||
|
||||
// output size/scaling
|
||||
dst += write_cont(dst, 0xa3c, {
|
||||
0x00000003,
|
||||
((out_width - 1) << 16) | (s->frame_width - 1),
|
||||
0x30036666,
|
||||
0x00000000,
|
||||
0x00000000,
|
||||
s->frame_width - 1,
|
||||
((out_height - 1) << 16) | (s->frame_height - 1),
|
||||
0x30036666,
|
||||
0x00000000,
|
||||
0x00000000,
|
||||
s->frame_height - 1,
|
||||
});
|
||||
dst += write_cont(dst, 0xa68, {
|
||||
0x00000003,
|
||||
((out_width / 2 - 1) << 16) | (s->frame_width - 1),
|
||||
0x3006cccc,
|
||||
0x00000000,
|
||||
0x00000000,
|
||||
s->frame_width - 1,
|
||||
((out_height / 2 - 1) << 16) | (s->frame_height - 1),
|
||||
0x3006cccc,
|
||||
0x00000000,
|
||||
0x00000000,
|
||||
s->frame_height - 1,
|
||||
});
|
||||
|
||||
// cropping
|
||||
dst += write_cont(dst, 0xe10, {
|
||||
out_height - 1,
|
||||
out_width - 1,
|
||||
});
|
||||
dst += write_cont(dst, 0xe30, {
|
||||
out_height / 2 - 1,
|
||||
out_width - 1,
|
||||
});
|
||||
dst += write_cont(dst, 0xe18, {
|
||||
0x0ff00000,
|
||||
0x00000016,
|
||||
});
|
||||
dst += write_cont(dst, 0xe38, {
|
||||
0x0ff00000,
|
||||
0x00000017,
|
||||
});
|
||||
|
||||
dst += build_common_ife_bps(dst, cam, s, patches, true);
|
||||
|
||||
return dst - start;
|
||||
}
|
||||
|
||||
|
||||
22
iqpilot/system/camerad/cameras/nv12_info.h
Normal file
22
iqpilot/system/camerad/cameras/nv12_info.h
Normal file
@@ -0,0 +1,22 @@
|
||||
#pragma once
|
||||
|
||||
#include <cassert>
|
||||
#include <cstdint>
|
||||
#include <tuple>
|
||||
|
||||
#include "third_party/linux/include/msm_media_info.h"
|
||||
|
||||
// Returns NV12 aligned (stride, y_height, uv_height, buffer_size) for the given frame dimensions.
|
||||
inline std::tuple<uint32_t, uint32_t, uint32_t, uint32_t> get_nv12_info(int width, int height) {
|
||||
const uint32_t stride = VENUS_Y_STRIDE(COLOR_FMT_NV12, width);
|
||||
const uint32_t y_height = VENUS_Y_SCANLINES(COLOR_FMT_NV12, height);
|
||||
const uint32_t uv_height = VENUS_UV_SCANLINES(COLOR_FMT_NV12, height);
|
||||
const uint32_t size = VENUS_BUFFER_SIZE(COLOR_FMT_NV12, width, height);
|
||||
|
||||
// Sanity checks for NV12 format assumptions
|
||||
assert(stride == VENUS_UV_STRIDE(COLOR_FMT_NV12, width));
|
||||
assert(y_height / 2 == uv_height);
|
||||
assert((stride * y_height) % 0x1000 == 0); // uv_offset must be page-aligned
|
||||
|
||||
return {stride, y_height, uv_height, size};
|
||||
}
|
||||
21
iqpilot/system/camerad/cameras/nv12_info.py
Normal file
21
iqpilot/system/camerad/cameras/nv12_info.py
Normal file
@@ -0,0 +1,21 @@
|
||||
# Python version of system/camerad/cameras/nv12_info.h
|
||||
# Calculations from third_party/linux/include/msm_media_info.h (VENUS_BUFFER_SIZE)
|
||||
|
||||
def align(val: int, alignment: int) -> int:
|
||||
return ((val + alignment - 1) // alignment) * alignment
|
||||
|
||||
def get_nv12_info(width: int, height: int) -> tuple[int, int, int, int]:
|
||||
"""Returns (stride, y_height, uv_height, buffer_size) for NV12 frame dimensions."""
|
||||
stride = align(width, 128)
|
||||
y_height = align(height, 32)
|
||||
uv_height = align(height // 2, 16)
|
||||
|
||||
# VENUS_BUFFER_SIZE for NV12
|
||||
y_plane = stride * y_height
|
||||
uv_plane = stride * uv_height + 4096
|
||||
size = y_plane + uv_plane + max(16 * 1024, 8 * stride)
|
||||
size = align(size, 4096)
|
||||
size += align(width, 512) * 512 # kernel padding for non-aligned frames
|
||||
size = align(size, 4096)
|
||||
|
||||
return stride, y_height, uv_height, size
|
||||
1834
iqpilot/system/camerad/cameras/spectra.cc
Normal file
1834
iqpilot/system/camerad/cameras/spectra.cc
Normal file
File diff suppressed because it is too large
Load Diff
235
iqpilot/system/camerad/cameras/spectra.h
Normal file
235
iqpilot/system/camerad/cameras/spectra.h
Normal file
@@ -0,0 +1,235 @@
|
||||
#pragma once
|
||||
|
||||
#include <sys/mman.h>
|
||||
#include <functional>
|
||||
#include <memory>
|
||||
#include <queue>
|
||||
#include <optional>
|
||||
#include <set>
|
||||
#include <utility>
|
||||
|
||||
#include "media/cam_req_mgr.h"
|
||||
|
||||
#include "common/util.h"
|
||||
#include "common/swaglog.h"
|
||||
#include "system/camerad/cameras/hw.h"
|
||||
#include "system/camerad/cameras/camera_common.h"
|
||||
#include "system/camerad/sensors/sensor.h"
|
||||
|
||||
#define MAX_IFE_BUFS 20
|
||||
|
||||
const int MIPI_SETTLE_CNT = 33; // Calculated by camera_freqs.py
|
||||
|
||||
// For use with the Titan 170 ISP in the SDM845
|
||||
// https://github.com/commaai/agnos-kernel-sdm845
|
||||
|
||||
// CSLDeviceType/CSLPacketOpcodesIFE from camx
|
||||
// cam_packet_header.op_code = (device << 24) | (opcode);
|
||||
#define CSLDeviceTypeImageSensor (0x01 << 24)
|
||||
#define CSLDeviceTypeIFE (0x0F << 24)
|
||||
#define CSLDeviceTypeBPS (0x10 << 24)
|
||||
#define OpcodesIFEInitialConfig 0x0
|
||||
#define OpcodesIFEUpdate 0x1
|
||||
|
||||
std::optional<int32_t> device_acquire(int fd, int32_t session_handle, void *data, uint32_t num_resources=1);
|
||||
int device_config(int fd, int32_t session_handle, int32_t dev_handle, uint64_t packet_handle);
|
||||
int device_control(int fd, int op_code, int session_handle, int dev_handle);
|
||||
int do_cam_control(int fd, int op_code, void *handle, int size);
|
||||
void *alloc_w_mmu_hdl(int video0_fd, int len, uint32_t *handle, int align = 8, int flags = CAM_MEM_FLAG_KMD_ACCESS | CAM_MEM_FLAG_UMD_ACCESS | CAM_MEM_FLAG_CMD_BUF_TYPE,
|
||||
int mmu_hdl = 0, int mmu_hdl2 = 0);
|
||||
void release(int video0_fd, uint32_t handle);
|
||||
|
||||
class MemoryManager {
|
||||
public:
|
||||
void init(int _video0_fd) { video0_fd = _video0_fd; }
|
||||
~MemoryManager();
|
||||
|
||||
template <class T>
|
||||
auto alloc(int len, uint32_t *handle) {
|
||||
return std::unique_ptr<T, std::function<void(void *)>>((T*)alloc_buf(len, handle), [this](void *ptr) { this->free(ptr); });
|
||||
}
|
||||
|
||||
private:
|
||||
void *alloc_buf(int len, uint32_t *handle);
|
||||
void free(void *ptr);
|
||||
|
||||
std::map<void *, uint32_t> handle_lookup;
|
||||
std::map<void *, int> size_lookup;
|
||||
std::map<int, std::queue<void *> > cached_allocations;
|
||||
int video0_fd;
|
||||
};
|
||||
|
||||
class SpectraMaster {
|
||||
public:
|
||||
void init();
|
||||
|
||||
unique_fd video0_fd;
|
||||
unique_fd cam_sync_fd;
|
||||
unique_fd isp_fd;
|
||||
unique_fd icp_fd;
|
||||
int device_iommu = -1;
|
||||
int cdm_iommu = -1;
|
||||
int icp_device_iommu = -1;
|
||||
MemoryManager mem_mgr;
|
||||
};
|
||||
|
||||
class SpectraBuf {
|
||||
public:
|
||||
SpectraBuf() = default;
|
||||
|
||||
~SpectraBuf() {
|
||||
if (video_fd >= 0 && ptr) {
|
||||
munmap(ptr, mmap_size);
|
||||
release(video_fd, handle);
|
||||
}
|
||||
}
|
||||
|
||||
void init(SpectraMaster *m, int s, int a, bool shared_access, int mmu_hdl = 0, int mmu_hdl2 = 0, int count = 1) {
|
||||
video_fd = m->video0_fd;
|
||||
size = s;
|
||||
alignment = a;
|
||||
mmap_size = aligned_size() * count;
|
||||
|
||||
uint32_t flags = CAM_MEM_FLAG_HW_READ_WRITE | CAM_MEM_FLAG_KMD_ACCESS | CAM_MEM_FLAG_UMD_ACCESS | CAM_MEM_FLAG_CMD_BUF_TYPE;
|
||||
if (shared_access) {
|
||||
flags |= CAM_MEM_FLAG_HW_SHARED_ACCESS;
|
||||
}
|
||||
|
||||
void *p = alloc_w_mmu_hdl(video_fd, mmap_size, (uint32_t*)&handle, alignment, flags, mmu_hdl, mmu_hdl2);
|
||||
ptr = (unsigned char*)p;
|
||||
assert(ptr != NULL);
|
||||
};
|
||||
|
||||
uint32_t aligned_size() {
|
||||
return ALIGNED_SIZE(size, alignment);
|
||||
};
|
||||
|
||||
int video_fd = -1;
|
||||
unsigned char *ptr = nullptr;
|
||||
int size = 0, alignment = 0, handle = 0, mmap_size = 0;
|
||||
};
|
||||
|
||||
class SpectraCamera {
|
||||
public:
|
||||
SpectraCamera(SpectraMaster *master, const CameraConfig &config);
|
||||
~SpectraCamera();
|
||||
|
||||
void camera_open(VisionIpcServer *v, cl_device_id device_id, cl_context ctx);
|
||||
bool handle_camera_event(const cam_req_mgr_message *event_data);
|
||||
void camera_close();
|
||||
void camera_map_bufs();
|
||||
void config_bps(int idx, int request_id);
|
||||
void config_bps_downscale(int idx, int request_id); // mici driver cam: full-res + 2x BPS downscale (#37876)
|
||||
void config_ife(int idx, int request_id, bool init=false);
|
||||
|
||||
int clear_req_queue();
|
||||
void enqueue_frame();
|
||||
|
||||
int sensors_init();
|
||||
void sensors_start();
|
||||
void sensors_poke(int request_id);
|
||||
void sensors_i2c(const struct i2c_random_wr_payload* dat, int len, int op_code, bool data_word);
|
||||
|
||||
bool openSensor();
|
||||
void configISP();
|
||||
void configICP();
|
||||
void configCSIPHY();
|
||||
void linkDevices();
|
||||
void destroySyncObjectAt(int index);
|
||||
|
||||
// *** state ***
|
||||
|
||||
int ife_buf_depth = -1;
|
||||
bool open = false;
|
||||
bool enabled = true;
|
||||
CameraConfig cc;
|
||||
std::unique_ptr<const SensorInfo> sensor;
|
||||
|
||||
// YUV image size
|
||||
uint32_t stride;
|
||||
uint32_t y_height;
|
||||
uint32_t uv_height;
|
||||
uint32_t uv_offset;
|
||||
uint32_t yuv_size;
|
||||
|
||||
unique_fd sensor_fd;
|
||||
unique_fd csiphy_fd;
|
||||
|
||||
int32_t session_handle = -1;
|
||||
int32_t sensor_dev_handle = -1;
|
||||
int32_t isp_dev_handle = -1;
|
||||
int32_t icp_dev_handle = -1;
|
||||
int32_t csiphy_dev_handle = -1;
|
||||
|
||||
int32_t link_handle = -1;
|
||||
|
||||
SpectraBuf ife_cmd;
|
||||
SpectraBuf ife_gamma_lut;
|
||||
SpectraBuf ife_linearization_lut;
|
||||
SpectraBuf ife_vignetting_lut;
|
||||
|
||||
SpectraBuf bps_cmd;
|
||||
SpectraBuf bps_cdm_buffer;
|
||||
SpectraBuf bps_cdm_program_array;
|
||||
SpectraBuf bps_cdm_striping_bl;
|
||||
SpectraBuf bps_iq;
|
||||
SpectraBuf bps_striping;
|
||||
SpectraBuf bps_linearization_lut;
|
||||
SpectraBuf bps_gamma_lut; // only allocated for out_scale>1 (mici driver cam downscale)
|
||||
SpectraBuf bps_fullres_dummy; // only allocated for out_scale>1 (mici driver cam downscale)
|
||||
std::vector<uint32_t> bps_lin_reg;
|
||||
std::vector<uint32_t> bps_ccm_reg;
|
||||
|
||||
int buf_handle_yuv[MAX_IFE_BUFS] = {};
|
||||
int buf_handle_raw[MAX_IFE_BUFS] = {};
|
||||
int sync_objs_ife[MAX_IFE_BUFS] = {};
|
||||
int sync_objs_bps[MAX_IFE_BUFS] = {};
|
||||
uint64_t next_request_id = 1;
|
||||
uint64_t request_id_last = 0;
|
||||
uint64_t last_requeue_ts = 0;
|
||||
uint64_t frame_id_raw_last = 0;
|
||||
int invalid_request_count = 0;
|
||||
bool skip_expected = true;
|
||||
|
||||
CameraBuf buf;
|
||||
SpectraMaster *m;
|
||||
|
||||
enum class WideCamFault {
|
||||
none,
|
||||
noBringup,
|
||||
noFrames,
|
||||
};
|
||||
static WideCamFault wideCamFault() { return wide_cam_fault; }
|
||||
static bool firstFrameSynced() { return first_frame_synced; }
|
||||
|
||||
private:
|
||||
void clearAndRequeue();
|
||||
bool validateEvent(uint64_t request_id, uint64_t frame_id_raw);
|
||||
bool waitForFrameReady(uint64_t request_id);
|
||||
bool processFrame(int buf_idx, uint64_t request_id, uint64_t frame_id_raw, uint64_t timestamp);
|
||||
static bool syncFirstFrame(int camera_id, uint64_t request_id, uint64_t raw_id, uint64_t timestamp);
|
||||
struct SyncData {
|
||||
uint64_t timestamp;
|
||||
uint64_t frame_id_offset = 0;
|
||||
};
|
||||
inline static std::map<int, SyncData> camera_sync_data;
|
||||
inline static bool first_frame_synced = false;
|
||||
inline static std::set<int> running_cameras;
|
||||
inline static WideCamFault wide_cam_fault = WideCamFault::none;
|
||||
static void classifyWideCamFault();
|
||||
|
||||
// a mode for stressing edge cases: realignment, sync failures, etc.
|
||||
inline bool stress_test(std::string log) {
|
||||
static double last_trigger = 0;
|
||||
static double prob = std::stod(util::getenv("SPECTRA_ERROR_PROB", "-1"));
|
||||
static double dt = std::stod(util::getenv("SPECTRA_ERROR_DT", "1"));
|
||||
bool triggered = (prob > 0) && \
|
||||
((static_cast<double>(rand()) / RAND_MAX) < prob) && \
|
||||
(millis_since_boot() - last_trigger) > dt;
|
||||
if (triggered) {
|
||||
last_trigger = millis_since_boot();
|
||||
LOGE("stress test (cam %d): %s", cc.camera_num, log.c_str());
|
||||
}
|
||||
return triggered;
|
||||
}
|
||||
};
|
||||
15
iqpilot/system/camerad/main.cc
Normal file
15
iqpilot/system/camerad/main.cc
Normal file
@@ -0,0 +1,15 @@
|
||||
#include "system/camerad/cameras/camera_common.h"
|
||||
|
||||
#include <cassert>
|
||||
|
||||
#include "common/params.h"
|
||||
#include "common/util.h"
|
||||
|
||||
int main(int argc, char *argv[]) {
|
||||
// doesn't need RT priority since we're using isolcpus
|
||||
int ret = util::set_core_affinity({6});
|
||||
assert(ret == 0 || Params().getBool("IsOffroad")); // failure ok while offroad due to offlining cores
|
||||
|
||||
camerad_thread();
|
||||
return 0;
|
||||
}
|
||||
136
iqpilot/system/camerad/sensors/ar0231.cc
Normal file
136
iqpilot/system/camerad/sensors/ar0231.cc
Normal file
@@ -0,0 +1,136 @@
|
||||
#include <cassert>
|
||||
#include <cmath>
|
||||
|
||||
#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 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
|
||||
|
||||
} // namespace
|
||||
|
||||
AR0231::AR0231() {
|
||||
image_sensor = cereal::FrameData::ImageSensor::AR0231;
|
||||
bayer_pattern = CAM_ISP_PATTERN_BAYER_GRGRGR;
|
||||
pixel_size_mm = 0.003;
|
||||
data_word = true;
|
||||
frame_width = 1928;
|
||||
frame_height = 1208;
|
||||
frame_stride = (frame_width * 12 / 8) + 4;
|
||||
extra_height = AR0231_REGISTERS_HEIGHT + AR0231_STATS_HEIGHT;
|
||||
|
||||
registers_offset = 0;
|
||||
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;
|
||||
bits_per_pixel = 12;
|
||||
mipi_format = CAM_FORMAT_MIPI_RAW_12;
|
||||
frame_data_type = 0x12; // Changing stats to 0x2C doesn't work, so change pixels to 0x12 instead
|
||||
mclk_frequency = 19200000; //Hz
|
||||
|
||||
readout_time_ns = 22850000;
|
||||
|
||||
dc_gain_factor = 2.5;
|
||||
dc_gain_min_weight = 0;
|
||||
dc_gain_max_weight = 1;
|
||||
dc_gain_on_grey = 0.2;
|
||||
dc_gain_off_grey = 0.3;
|
||||
exposure_time_min = 2; // with HDR, fastest ss
|
||||
exposure_time_max = 0x0855; // with HDR, slowest ss, 40ms
|
||||
analog_gain_min_idx = 0x1; // 0.25x
|
||||
analog_gain_rec_idx = 0x6; // 0.8x
|
||||
analog_gain_max_idx = 0xD; // 4.0x
|
||||
analog_gain_cost_delta = 0;
|
||||
analog_gain_cost_low = 0.1;
|
||||
analog_gain_cost_high = 5.0;
|
||||
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 = 1.0;
|
||||
|
||||
black_level = 168;
|
||||
color_correct_matrix = {
|
||||
0x000000af, 0x00000ff9, 0x00000fd8,
|
||||
0x00000fbc, 0x000000bb, 0x00000009,
|
||||
0x00000fb6, 0x00000fe0, 0x000000ea,
|
||||
};
|
||||
for (int i = 0; i < 65; i++) {
|
||||
float fx = i / 64.0;
|
||||
const float gamma_k = 0.75;
|
||||
const float gamma_b = 0.125;
|
||||
const float mp = 0.01; // ideally midpoint should be adaptive
|
||||
const float rk = 9 - 100*mp;
|
||||
// poly approximation for s curve
|
||||
fx = (fx > mp) ?
|
||||
((rk * (fx-mp) * (1-(gamma_k*mp+gamma_b)) * (1+1/(rk*(1-mp))) / (1+rk*(fx-mp))) + gamma_k*mp + gamma_b) :
|
||||
((rk * (fx-mp) * (gamma_k*mp+gamma_b) * (1+1/(rk*mp)) / (1-rk*(fx-mp))) + gamma_k*mp + gamma_b);
|
||||
gamma_lut_rgb.push_back((uint32_t)(fx*1023.0 + 0.5));
|
||||
}
|
||||
prepare_gamma_lut();
|
||||
linearization_lut = {
|
||||
0x02000000, 0x02000000, 0x02000000, 0x02000000,
|
||||
0x020007ff, 0x020007ff, 0x020007ff, 0x020007ff,
|
||||
0x02000bff, 0x02000bff, 0x02000bff, 0x02000bff,
|
||||
0x020017ff, 0x020017ff, 0x020017ff, 0x020017ff,
|
||||
0x02001bff, 0x02001bff, 0x02001bff, 0x02001bff,
|
||||
0x020023ff, 0x020023ff, 0x020023ff, 0x020023ff,
|
||||
0x00003fff, 0x00003fff, 0x00003fff, 0x00003fff,
|
||||
0x00003fff, 0x00003fff, 0x00003fff, 0x00003fff,
|
||||
0x00003fff, 0x00003fff, 0x00003fff, 0x00003fff,
|
||||
};
|
||||
linearization_pts = {0x07ff0bff, 0x17ff1bff, 0x23ff3fff, 0x3fff3fff};
|
||||
vignetting_lut = {
|
||||
0x00eaa755, 0x00cf2679, 0x00bc05e0, 0x00acc566, 0x00a1450a, 0x009984cc, 0x0095a4ad, 0x009584ac, 0x009944ca, 0x00a0c506, 0x00ac0560, 0x00bb25d9, 0x00ce2671, 0x00e90748, 0x01112889, 0x014a2a51, 0x01984cc2,
|
||||
0x00db06d8, 0x00c30618, 0x00afe57f, 0x00a0a505, 0x009524a9, 0x008d646b, 0x0089844c, 0x0089644b, 0x008d2469, 0x0094a4a5, 0x009fe4ff, 0x00af0578, 0x00c20610, 0x00d986cc, 0x00fda7ed, 0x01320990, 0x017aebd7,
|
||||
0x00d1868c, 0x00baa5d5, 0x00a7853c, 0x009844c2, 0x008cc466, 0x0085a42d, 0x0083641b, 0x0083641b, 0x0085842c, 0x008c4462, 0x0097a4bd, 0x00a6c536, 0x00b9a5cd, 0x00d06683, 0x00f1678b, 0x01226913, 0x0167ab3d,
|
||||
0x00cd0668, 0x00b625b1, 0x00a30518, 0x0093c49e, 0x00884442, 0x00830418, 0x0080e407, 0x0080c406, 0x0082e417, 0x0087c43e, 0x00932499, 0x00a22511, 0x00b525a9, 0x00cbe65f, 0x00eb0758, 0x011a68d3, 0x015daaed,
|
||||
0x00cc4662, 0x00b565ab, 0x00a24512, 0x00930498, 0x0087843c, 0x0082a415, 0x00806403, 0x00806403, 0x00828414, 0x00870438, 0x00926493, 0x00a1850c, 0x00b465a3, 0x00cb2659, 0x00ea2751, 0x011928c9, 0x015c2ae1,
|
||||
0x00cf667b, 0x00b885c4, 0x00a5652b, 0x009624b1, 0x008aa455, 0x00846423, 0x00822411, 0x00822411, 0x00844422, 0x008a2451, 0x009564ab, 0x00a48524, 0x00b785bc, 0x00ce4672, 0x00ee6773, 0x011e88f4, 0x0162eb17,
|
||||
0x00d6c6b6, 0x00bf65fb, 0x00ac4562, 0x009d04e8, 0x0091848c, 0x0089c44e, 0x00862431, 0x00860430, 0x0089844c, 0x00910488, 0x009c64e3, 0x00ab655b, 0x00be65f3, 0x00d566ab, 0x00f847c2, 0x012b2959, 0x01726b93,
|
||||
0x00e3e71f, 0x00ca0650, 0x00b705b8, 0x00a7a53d, 0x009c24e1, 0x009484a4, 0x00908484, 0x00908484, 0x009424a1, 0x009bc4de, 0x00a70538, 0x00b625b1, 0x00c90648, 0x00e26713, 0x0108e847, 0x013fe9ff, 0x018bcc5e,
|
||||
0x00f807c0, 0x00d966cb, 0x00c5862c, 0x00b625b1, 0x00aaa555, 0x00a30518, 0x009f04f8, 0x009f04f8, 0x00a2a515, 0x00aa2551, 0x00b585ac, 0x00c4a625, 0x00d846c2, 0x00f647b2, 0x0121a90d, 0x015e4af2, 0x01b8cdc6,
|
||||
0x011548aa, 0x00f1678b, 0x00d886c4, 0x00c86643, 0x00bce5e7, 0x00b545aa, 0x00b1658b, 0x00b1458a, 0x00b505a8, 0x00bc85e4, 0x00c7c63e, 0x00d786bc, 0x00efe77f, 0x0113489a, 0x0144ea27, 0x01888c44, 0x01fdcfee,
|
||||
0x013e49f2, 0x0113e89f, 0x00f5a7ad, 0x00e0c706, 0x00d30698, 0x00cb665b, 0x00c7663b, 0x00c7663b, 0x00cb0658, 0x00d2a695, 0x00dfe6ff, 0x00f467a3, 0x01122891, 0x013be9df, 0x01750ba8, 0x01cfae7d, 0x025912c8,
|
||||
0x01766bb3, 0x01446a23, 0x011fc8fe, 0x0105e82f, 0x00f467a3, 0x00e9874c, 0x00e46723, 0x00e44722, 0x00e92749, 0x00f3a79d, 0x0104c826, 0x011e48f2, 0x01424a12, 0x01738b9c, 0x01bf6dfb, 0x023611b0, 0x02ced676,
|
||||
0x01cf8e7c, 0x01866c33, 0x015aaad5, 0x013ae9d7, 0x01250928, 0x011768bb, 0x0110a885, 0x01108884, 0x0116e8b7, 0x01242921, 0x0139a9cd, 0x0158eac7, 0x01840c20, 0x01cb0e58, 0x0233719b, 0x02b9d5ce, 0x03645b22,
|
||||
};
|
||||
}
|
||||
|
||||
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;
|
||||
}
|
||||
121
iqpilot/system/camerad/sensors/ar0231_registers.h
Normal file
121
iqpilot/system/camerad/sensors/ar0231_registers.h
Normal file
@@ -0,0 +1,121 @@
|
||||
#pragma once
|
||||
|
||||
const struct i2c_random_wr_payload start_reg_array_ar0231[] = {{0x301A, 0x91C}};
|
||||
const struct i2c_random_wr_payload stop_reg_array_ar0231[] = {{0x301A, 0x918}};
|
||||
|
||||
const struct i2c_random_wr_payload init_array_ar0231[] = {
|
||||
{0x301A, 0x0018}, // RESET_REGISTER
|
||||
|
||||
// **NOTE**: if this is changed, readout_time_ns must be updated in the Sensor config
|
||||
|
||||
// CLOCK Settings
|
||||
// input clock is 19.2 / 2 * 0x37 = 528 MHz
|
||||
// pixclk is 528 / 6 = 88 MHz
|
||||
// full roll time is 1000/(PIXCLK/(LINE_LENGTH_PCK*FRAME_LENGTH_LINES)) = 39.99 ms
|
||||
// img roll time is 1000/(PIXCLK/(LINE_LENGTH_PCK*Y_OUTPUT_CONTROL)) = 22.85 ms
|
||||
{0x302A, 0x0006}, // VT_PIX_CLK_DIV
|
||||
{0x302C, 0x0001}, // VT_SYS_CLK_DIV
|
||||
{0x302E, 0x0002}, // PRE_PLL_CLK_DIV
|
||||
{0x3030, 0x0037}, // PLL_MULTIPLIER
|
||||
{0x3036, 0x000C}, // OP_PIX_CLK_DIV
|
||||
{0x3038, 0x0001}, // OP_SYS_CLK_DIV
|
||||
|
||||
// FORMAT
|
||||
{0x3040, 0xC000}, // READ_MODE
|
||||
{0x3004, 0x0000}, // X_ADDR_START_
|
||||
{0x3008, 0x0787}, // X_ADDR_END_
|
||||
{0x3002, 0x0000}, // Y_ADDR_START_
|
||||
{0x3006, 0x04B7}, // Y_ADDR_END_
|
||||
{0x3032, 0x0000}, // SCALING_MODE
|
||||
{0x30A2, 0x0001}, // X_ODD_INC_
|
||||
{0x30A6, 0x0001}, // Y_ODD_INC_
|
||||
{0x3402, 0x0788}, // X_OUTPUT_CONTROL
|
||||
{0x3404, 0x04B8}, // Y_OUTPUT_CONTROL
|
||||
{0x3064, 0x1982}, // SMIA_TEST
|
||||
{0x30BA, 0x11F2}, // DIGITAL_CTRL
|
||||
|
||||
// Enable external trigger and disable GPIO outputs
|
||||
{0x30CE, 0x0120}, // SLAVE_SH_SYNC_MODE | FRAME_START_MODE
|
||||
{0x340A, 0xE0}, // GPIO3_INPUT_DISABLE | GPIO2_INPUT_DISABLE | GPIO1_INPUT_DISABLE
|
||||
{0x340C, 0x802}, // GPIO_HIDRV_EN | GPIO0_ISEL=2
|
||||
|
||||
// Readout timing
|
||||
{0x300C, 0x0672}, // LINE_LENGTH_PCK (valid for 3-exposure HDR)
|
||||
{0x300A, 0x0855}, // FRAME_LENGTH_LINES
|
||||
{0x3042, 0x0000}, // EXTRA_DELAY
|
||||
|
||||
// Readout Settings
|
||||
{0x31AE, 0x0204}, // SERIAL_FORMAT, 4-lane MIPI
|
||||
{0x31AC, 0x0C0C}, // DATA_FORMAT_BITS, 12 -> 12
|
||||
{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
|
||||
{0x3350, 0x0311}, // MIPI_F4_VDT_VC
|
||||
{0x31B0, 0x0053}, // FRAME_PREAMBLE
|
||||
{0x31B2, 0x003B}, // LINE_PREAMBLE
|
||||
{0x301A, 0x001C}, // RESET_REGISTER
|
||||
|
||||
// Noise Corrections
|
||||
{0x3092, 0x0C24}, // ROW_NOISE_CONTROL
|
||||
{0x337A, 0x0C80}, // DBLC_SCALE0
|
||||
{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},
|
||||
|
||||
// HDR Settings
|
||||
{0x3082, 0x0004}, // OPERATION_MODE_CTRL
|
||||
{0x3238, 0x0444}, // EXPOSURE_RATIO
|
||||
|
||||
{0x1008, 0x0361}, // FINE_INTEGRATION_TIME_MIN
|
||||
{0x100C, 0x0589}, // FINE_INTEGRATION_TIME2_MIN
|
||||
{0x100E, 0x07B1}, // FINE_INTEGRATION_TIME3_MIN
|
||||
{0x1010, 0x0139}, // FINE_INTEGRATION_TIME4_MIN
|
||||
|
||||
// TODO: do these have to be lower than LINE_LENGTH_PCK?
|
||||
{0x3014, 0x08CB}, // FINE_INTEGRATION_TIME_
|
||||
{0x321E, 0x0894}, // FINE_INTEGRATION_TIME2
|
||||
|
||||
{0x31D0, 0x0000}, // COMPANDING, no good in 10 bit?
|
||||
{0x33DA, 0x0000}, // COMPANDING
|
||||
{0x318E, 0x0200}, // PRE_HDR_GAIN_EN
|
||||
|
||||
// DLO Settings
|
||||
{0x3100, 0x4000}, // DLO_CONTROL0
|
||||
{0x3280, 0x0CCC}, // T1 G1
|
||||
{0x3282, 0x0CCC}, // T1 R
|
||||
{0x3284, 0x0CCC}, // T1 B
|
||||
{0x3286, 0x0CCC}, // T1 G2
|
||||
{0x3288, 0x0FA0}, // T2 G1
|
||||
{0x328A, 0x0FA0}, // T2 R
|
||||
{0x328C, 0x0FA0}, // T2 B
|
||||
{0x328E, 0x0FA0}, // T2 G2
|
||||
|
||||
// Initial Gains
|
||||
{0x3022, 0x0001}, // GROUPED_PARAMETER_HOLD_
|
||||
{0x3366, 0xFF77}, // ANALOG_GAIN (1x)
|
||||
|
||||
{0x3060, 0x3333}, // ANALOG_COLOR_GAIN
|
||||
|
||||
{0x3362, 0x0000}, // DC GAIN
|
||||
|
||||
{0x305A, 0x00F8}, // red gain
|
||||
{0x3058, 0x0122}, // blue gain
|
||||
{0x3056, 0x009A}, // g1 gain
|
||||
{0x305C, 0x009A}, // g2 gain
|
||||
|
||||
{0x3022, 0x0000}, // GROUPED_PARAMETER_HOLD_
|
||||
|
||||
// Initial Integration Time
|
||||
{0x3012, 0x0005},
|
||||
};
|
||||
140
iqpilot/system/camerad/sensors/os04c10.cc
Normal file
140
iqpilot/system/camerad/sensors/os04c10.cc
Normal file
@@ -0,0 +1,140 @@
|
||||
#include <cmath>
|
||||
#include <cstdlib>
|
||||
|
||||
#include "system/camerad/sensors/sensor.h"
|
||||
#include "third_party/linux/include/msm_camsensor_sdk.h"
|
||||
|
||||
namespace {
|
||||
|
||||
const float sensor_analog_gains_OS04C10[] = {
|
||||
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 os04c10_analog_gains_reg[] = {
|
||||
0x080, 0x088, 0x090, 0x098, 0x0A0, 0x0A8, 0x0B0, 0x0B8, 0x0C0, 0x0C8, 0x0D8,
|
||||
0x0E8, 0x0F8, 0x100, 0x110, 0x120, 0x130, 0x140, 0x150, 0x160, 0x170, 0x180,
|
||||
0x190, 0x1B0, 0x1D0, 0x1F0, 0x200, 0x220, 0x240, 0x260, 0x280, 0x2A0, 0x2C0,
|
||||
0x2E0, 0x300, 0x320, 0x340, 0x380, 0x3C0, 0x400, 0x440, 0x480, 0x4C0, 0x500,
|
||||
0x540, 0x580, 0x5C0, 0x600, 0x640, 0x680, 0x6C0, 0x700, 0x740, 0x780, 0x7C0};
|
||||
|
||||
} // namespace
|
||||
|
||||
OS04C10::OS04C10() {
|
||||
image_sensor = cereal::FrameData::ImageSensor::OS04C10;
|
||||
bayer_pattern = CAM_ISP_PATTERN_BAYER_BGBGBG;
|
||||
pixel_size_mm = 0.002;
|
||||
data_word = false;
|
||||
|
||||
// mici driver cam: read full-res and downscale 2x in the BPS (HDR), upstream #37876
|
||||
out_scale = 2;
|
||||
frame_width = 2688;
|
||||
frame_height = 1520;
|
||||
frame_stride = frame_width * 12 / 8;
|
||||
|
||||
extra_height = 0;
|
||||
frame_offset = 0;
|
||||
|
||||
start_reg_array.assign(std::begin(start_reg_array_os04c10), std::end(start_reg_array_os04c10));
|
||||
init_reg_array.assign(std::begin(init_array_os04c10), std::end(init_array_os04c10));
|
||||
if (std::getenv("SPECTRA_TEST_PATTERN")) {
|
||||
init_reg_array.push_back({0x5080, 0xc4});
|
||||
}
|
||||
probe_reg_addr = 0x300a;
|
||||
probe_expected_data = 0x5304;
|
||||
bits_per_pixel = 12;
|
||||
mipi_format = CAM_FORMAT_MIPI_RAW_12;
|
||||
frame_data_type = CSI_RAW12;
|
||||
mclk_frequency = 24000000; // Hz
|
||||
|
||||
// TODO: this was set from logs. actually calculate it out
|
||||
readout_time_ns = 11000000;
|
||||
|
||||
ev_scale = 150.0;
|
||||
dc_gain_factor = 1;
|
||||
dc_gain_min_weight = 1; // always on is fine
|
||||
dc_gain_max_weight = 1;
|
||||
dc_gain_on_grey = 0.9;
|
||||
dc_gain_off_grey = 1.0;
|
||||
exposure_time_min = 2;
|
||||
exposure_time_max = 2352;
|
||||
analog_gain_min_idx = 0x0;
|
||||
analog_gain_rec_idx = 0x0; // 1x
|
||||
analog_gain_max_idx = 0x28;
|
||||
analog_gain_cost_delta = -1;
|
||||
analog_gain_cost_low = 0.4;
|
||||
analog_gain_cost_high = 6.4;
|
||||
for (int i = 0; i <= analog_gain_max_idx; i++) {
|
||||
sensor_analog_gains[i] = sensor_analog_gains_OS04C10[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 = 0.01;
|
||||
|
||||
black_level = 48;
|
||||
color_correct_matrix = {
|
||||
0x000000c2, 0x00000fe0, 0x00000fde,
|
||||
0x00000fa7, 0x000000d9, 0x00001000,
|
||||
0x00000fca, 0x00000fef, 0x000000c7,
|
||||
};
|
||||
for (int i = 0; i < 65; i++) {
|
||||
float fx = i / 64.0;
|
||||
gamma_lut_rgb.push_back((uint32_t)((10*fx)/(1+9*fx)*1023.0 + 0.5));
|
||||
}
|
||||
prepare_gamma_lut();
|
||||
linearization_lut = {
|
||||
0x02000000, 0x02000000, 0x02000000, 0x02000000,
|
||||
0x020007ff, 0x020007ff, 0x020007ff, 0x020007ff,
|
||||
0x02000bff, 0x02000bff, 0x02000bff, 0x02000bff,
|
||||
0x020017ff, 0x020017ff, 0x020017ff, 0x020017ff,
|
||||
0x02001bff, 0x02001bff, 0x02001bff, 0x02001bff,
|
||||
0x020023ff, 0x020023ff, 0x020023ff, 0x020023ff,
|
||||
0x00003fff, 0x00003fff, 0x00003fff, 0x00003fff,
|
||||
0x00003fff, 0x00003fff, 0x00003fff, 0x00003fff,
|
||||
0x00003fff, 0x00003fff, 0x00003fff, 0x00003fff,
|
||||
};
|
||||
linearization_pts = {0x07ff0bff, 0x17ff1bff, 0x23ff3fff, 0x3fff3fff};
|
||||
vignetting_lut = {
|
||||
0x01064832, 0x00da26d1, 0x00bb25d9, 0x00aac556, 0x00a06503, 0x009a64d3, 0x009744ba, 0x009744ba, 0x009a24d1, 0x00a00500, 0x00aa2551, 0x00ba45d2, 0x00d826c1, 0x01040820, 0x013729b9, 0x0171ab8d, 0x01b36d9b,
|
||||
0x00eee777, 0x00c2c616, 0x00ae2571, 0x009fe4ff, 0x0096e4b7, 0x0090e487, 0x008d446a, 0x008d2469, 0x0090a485, 0x009684b4, 0x009f64fb, 0x00ad456a, 0x00c1a60d, 0x00eca765, 0x011fc8fe, 0x015a4ad2, 0x019c0ce0,
|
||||
0x00dee6f7, 0x00b9c5ce, 0x00a5652b, 0x009964cb, 0x00904482, 0x00892449, 0x0085842c, 0x0085642b, 0x0088e447, 0x008fe47f, 0x0098e4c7, 0x00a4c526, 0x00b8a5c5, 0x00dc86e4, 0x010fc87e, 0x014a2a51, 0x018c0c60,
|
||||
0x00d626b1, 0x00b4e5a7, 0x00a1e50f, 0x0095e4af, 0x008c2461, 0x00850428, 0x0081640b, 0x0081440a, 0x0084a425, 0x008ba45d, 0x009564ab, 0x00a1450a, 0x00b3c59e, 0x00d3e69f, 0x01070838, 0x01418a0c, 0x01834c1a,
|
||||
0x00d4c6a6, 0x00b425a1, 0x00a1450a, 0x009544aa, 0x008b645b, 0x00844422, 0x0080a405, 0x0080a405, 0x00840420, 0x008b0458, 0x0094c4a6, 0x00a0a505, 0x00b30598, 0x00d26693, 0x0105a82d, 0x01402a01, 0x0181ec0f,
|
||||
0x00daa6d5, 0x00b765bb, 0x00a3c51e, 0x0097a4bd, 0x008e4472, 0x00872439, 0x0083841c, 0x0083641b, 0x0086e437, 0x008de46f, 0x009724b9, 0x00a30518, 0x00b665b3, 0x00d866c3, 0x010b885c, 0x01460a30, 0x0187ec3f,
|
||||
0x00e80740, 0x00bec5f6, 0x00aa6553, 0x009d24e9, 0x009404a0, 0x008d846c, 0x0089e44f, 0x0089e44f, 0x008d446a, 0x0093c49e, 0x009ca4e5, 0x00a9854c, 0x00bdc5ee, 0x00e5a72d, 0x0118c8c6, 0x01534a9a, 0x01952ca9,
|
||||
0x00fca7e5, 0x00d06683, 0x00b5c5ae, 0x00a5852c, 0x009c84e4, 0x009664b3, 0x0093649b, 0x0093449a, 0x009624b1, 0x009c24e1, 0x00a50528, 0x00b4e5a7, 0x00ce8674, 0x00fa47d2, 0x012d696b, 0x0167eb3f, 0x01a9cd4e,
|
||||
0x011888c4, 0x00ec6763, 0x00c7863c, 0x00b4e5a7, 0x00a8a545, 0x00a1c50e, 0x009ec4f6, 0x009ea4f5, 0x00a1a50d, 0x00a82541, 0x00b445a2, 0x00c5e62f, 0x00ea6753, 0x011648b2, 0x01496a4b, 0x0183ec1f, 0x01c5ae2d,
|
||||
0x013bc9de, 0x010fa87d, 0x00eac756, 0x00cd466a, 0x00bc25e1, 0x00b405a0, 0x00afc57e, 0x00afa57d, 0x00b3a59d, 0x00bbc5de, 0x00cc0660, 0x00e92749, 0x010da86d, 0x013989cc, 0x016cab65, 0x01a72d39, 0x01e8ef47,
|
||||
0x01666b33, 0x013a49d2, 0x011568ab, 0x00f7e7bf, 0x00e1c70e, 0x00d2e697, 0x00cb665b, 0x00cb2659, 0x00d26693, 0x00e0c706, 0x00f6a7b5, 0x0113c89e, 0x013849c2, 0x01642b21, 0x01974cba, 0x01d1ce8e, 0x0213909c,
|
||||
0x01986cc3, 0x016c2b61, 0x01476a3b, 0x0129e94f, 0x0113a89d, 0x0104c826, 0x00fd47ea, 0x00fd27e9, 0x01044822, 0x0112c896, 0x0128a945, 0x0145ca2e, 0x016a4b52, 0x01960cb0, 0x01c92e49, 0x0203b01d, 0x0245922c,
|
||||
0x01d1ae8d, 0x01a58d2c, 0x0180ac05, 0x01632b19, 0x014cea67, 0x013e29f1, 0x013689b4, 0x013669b3, 0x013d89ec, 0x014c0a60, 0x0161eb0f, 0x017f0bf8, 0x01a38d1c, 0x01cf4e7a, 0x02029014, 0x023d11e8, 0x027ed3f6,
|
||||
};
|
||||
}
|
||||
|
||||
std::vector<i2c_random_wr_payload> OS04C10::getExposureRegisters(int exposure_time, int new_exp_g, bool dc_gain_enabled) const {
|
||||
uint32_t long_time = exposure_time;
|
||||
uint32_t real_gain = os04c10_analog_gains_reg[new_exp_g];
|
||||
|
||||
return {
|
||||
{0x3501, long_time>>8}, {0x3502, long_time&0xFF},
|
||||
{0x3508, real_gain>>8}, {0x3509, real_gain&0xFF},
|
||||
{0x350c, real_gain>>8}, {0x350d, real_gain&0xFF},
|
||||
};
|
||||
}
|
||||
|
||||
int OS04C10::getSlaveAddress(int port) const {
|
||||
assert(port >= 0 && port <= 2);
|
||||
return (int[]){0x6C, 0x20, 0x6C}[port];
|
||||
}
|
||||
|
||||
float OS04C10::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) * 3.0;
|
||||
return score;
|
||||
}
|
||||
332
iqpilot/system/camerad/sensors/os04c10_registers.h
Normal file
332
iqpilot/system/camerad/sensors/os04c10_registers.h
Normal file
@@ -0,0 +1,332 @@
|
||||
#pragma once
|
||||
|
||||
const struct i2c_random_wr_payload start_reg_array_os04c10[] = {{0x100, 1}};
|
||||
const struct i2c_random_wr_payload stop_reg_array_os04c10[] = {{0x100, 0}};
|
||||
|
||||
const struct i2c_random_wr_payload init_array_os04c10[] = {
|
||||
// OS04C10_AA_00_02_17_wAO_2688x1524_MIPI728Mbps_Linear12bit_20FPS_4Lane_MCLK24MHz
|
||||
{0x0103, 0x01}, // software reset
|
||||
|
||||
// PLL + clocks
|
||||
{0x0301, 0xe4},
|
||||
{0x0303, 0x01},
|
||||
{0x0305, 0xb6},
|
||||
{0x0306, 0x01},
|
||||
{0x0307, 0x17},
|
||||
{0x0323, 0x04},
|
||||
{0x0324, 0x01},
|
||||
{0x0325, 0x62},
|
||||
|
||||
{0x3012, 0x06},
|
||||
{0x3013, 0x02},
|
||||
{0x3016, 0x72},
|
||||
{0x3021, 0x03},
|
||||
{0x3106, 0x21},
|
||||
{0x3107, 0xa1},
|
||||
|
||||
// Analog/timing fine-tuning block
|
||||
{0x3624, 0x00},
|
||||
{0x3625, 0x4c},
|
||||
{0x3660, 0x04},
|
||||
{0x3666, 0xa5},
|
||||
{0x3667, 0xa5},
|
||||
{0x366a, 0x50},
|
||||
{0x3673, 0x0d},
|
||||
{0x3672, 0x0d},
|
||||
{0x3671, 0x0d},
|
||||
{0x3670, 0x0d},
|
||||
{0x3685, 0x00},
|
||||
{0x3694, 0x0d},
|
||||
{0x3693, 0x0d},
|
||||
{0x3692, 0x0d},
|
||||
{0x3691, 0x0d},
|
||||
{0x3696, 0x4c},
|
||||
{0x3697, 0x4c},
|
||||
{0x3698, 0x00},
|
||||
{0x3699, 0x80},
|
||||
{0x369a, 0x80},
|
||||
{0x369b, 0x1f},
|
||||
{0x369c, 0x1f},
|
||||
{0x369d, 0x80},
|
||||
{0x369e, 0x40},
|
||||
{0x369f, 0x21},
|
||||
{0x36a0, 0x12},
|
||||
{0x36a1, 0xdd},
|
||||
{0x36a2, 0x66},
|
||||
{0x370a, 0x02},
|
||||
{0x370e, 0x00},
|
||||
{0x3710, 0x00},
|
||||
{0x3713, 0x04},
|
||||
{0x3725, 0x02},
|
||||
{0x372a, 0x03},
|
||||
{0x3738, 0xce},
|
||||
{0x3748, 0x02},
|
||||
{0x374a, 0x02},
|
||||
{0x374c, 0x02},
|
||||
{0x374e, 0x02},
|
||||
{0x3756, 0x00},
|
||||
{0x3757, 0x00},
|
||||
{0x3767, 0x00},
|
||||
{0x3771, 0x00},
|
||||
{0x377b, 0x28},
|
||||
{0x377c, 0x00},
|
||||
{0x377d, 0x0c},
|
||||
{0x3781, 0x03},
|
||||
{0x3782, 0x00},
|
||||
{0x3789, 0x14},
|
||||
{0x3795, 0x02},
|
||||
{0x379c, 0x00},
|
||||
{0x379d, 0x00},
|
||||
{0x37b8, 0x04},
|
||||
{0x37ba, 0x03},
|
||||
{0x37bb, 0x00},
|
||||
{0x37bc, 0x04},
|
||||
{0x37be, 0x26},
|
||||
{0x37c4, 0x11},
|
||||
{0x37c5, 0x80},
|
||||
{0x37c6, 0x14},
|
||||
{0x37c7, 0xa8},
|
||||
{0x37da, 0x11},
|
||||
{0x381f, 0x08},
|
||||
{0x3881, 0x00},
|
||||
{0x3888, 0x04},
|
||||
{0x388b, 0x00},
|
||||
{0x3c80, 0x10},
|
||||
{0x3c86, 0x00},
|
||||
{0x3c8c, 0x40},
|
||||
{0x3c9f, 0x01},
|
||||
{0x3d85, 0x1b},
|
||||
{0x3d8c, 0x71},
|
||||
{0x3d8d, 0xe2},
|
||||
{0x3f00, 0x0b},
|
||||
{0x3f06, 0x04},
|
||||
|
||||
// BLC - black level correction
|
||||
{0x400a, 0x01},
|
||||
{0x400b, 0x50},
|
||||
{0x400e, 0x08},
|
||||
{0x4043, 0x7e},
|
||||
{0x4045, 0x7e},
|
||||
{0x4047, 0x7e},
|
||||
{0x4049, 0x7e},
|
||||
{0x4090, 0x04},
|
||||
{0x40b0, 0x00},
|
||||
{0x40b1, 0x00},
|
||||
{0x40b2, 0x00},
|
||||
{0x40b3, 0x00},
|
||||
{0x40b4, 0x00},
|
||||
{0x40b5, 0x00},
|
||||
{0x40b7, 0x00},
|
||||
{0x40b8, 0x00},
|
||||
{0x40b9, 0x00},
|
||||
{0x40ba, 0x01},
|
||||
|
||||
{0x4301, 0x00},
|
||||
{0x4303, 0x00},
|
||||
{0x4502, 0x04},
|
||||
{0x4503, 0x00},
|
||||
{0x4504, 0x06},
|
||||
{0x4506, 0x00},
|
||||
{0x4507, 0x47},
|
||||
{0x4803, 0x00},
|
||||
{0x480c, 0x32},
|
||||
{0x480e, 0x04},
|
||||
{0x4813, 0xe4},
|
||||
{0x4819, 0x70},
|
||||
{0x481f, 0x30},
|
||||
{0x4823, 0x3f},
|
||||
{0x4825, 0x30},
|
||||
{0x4833, 0x10},
|
||||
{0x484b, 0x27},
|
||||
{0x488b, 0x00},
|
||||
{0x4d00, 0x04},
|
||||
{0x4d01, 0xad},
|
||||
{0x4d02, 0xbc},
|
||||
{0x4d03, 0xa1},
|
||||
{0x4d04, 0x1f},
|
||||
{0x4d05, 0x4c},
|
||||
{0x4d0b, 0x01},
|
||||
{0x4e00, 0x2a},
|
||||
{0x4e0d, 0x00},
|
||||
|
||||
// ISP
|
||||
{0x5001, 0x09},
|
||||
{0x5004, 0x00},
|
||||
{0x5080, 0x04},
|
||||
{0x5036, 0x80},
|
||||
{0x5180, 0x70},
|
||||
{0x5181, 0x10},
|
||||
|
||||
// DPC - defective pixel correction
|
||||
{0x520a, 0x03},
|
||||
{0x520b, 0x06},
|
||||
{0x520c, 0x0c},
|
||||
|
||||
{0x580b, 0x0f},
|
||||
{0x580d, 0x00},
|
||||
{0x580f, 0x00},
|
||||
{0x5820, 0x00},
|
||||
{0x5821, 0x00},
|
||||
|
||||
{0x301c, 0xf8},
|
||||
{0x301e, 0xb4},
|
||||
{0x301f, 0xf0},
|
||||
{0x3022, 0x61},
|
||||
{0x3109, 0xe7},
|
||||
{0x3600, 0x00},
|
||||
{0x3610, 0x65},
|
||||
{0x3611, 0x85},
|
||||
{0x3613, 0x3a},
|
||||
{0x3615, 0x60},
|
||||
{0x3621, 0xb0},
|
||||
{0x3620, 0x0c},
|
||||
{0x3629, 0x00},
|
||||
{0x3661, 0x04},
|
||||
{0x3664, 0x70},
|
||||
{0x3665, 0x00},
|
||||
{0x3681, 0x80},
|
||||
{0x3682, 0x40},
|
||||
{0x3683, 0x21},
|
||||
{0x3684, 0x12},
|
||||
{0x3700, 0x2a},
|
||||
{0x3701, 0x12},
|
||||
{0x3703, 0x28},
|
||||
{0x3704, 0x0e},
|
||||
{0x3706, 0x9d},
|
||||
{0x3709, 0x4a},
|
||||
{0x370b, 0x48},
|
||||
{0x370c, 0x01},
|
||||
{0x370f, 0x00},
|
||||
{0x3714, 0x24},
|
||||
{0x3716, 0x04},
|
||||
{0x3719, 0x11},
|
||||
{0x371a, 0x1e},
|
||||
{0x3720, 0x00},
|
||||
{0x3724, 0x13},
|
||||
{0x373f, 0xb0},
|
||||
{0x3741, 0x9d},
|
||||
{0x3743, 0x9d},
|
||||
{0x3745, 0x9d},
|
||||
{0x3747, 0x9d},
|
||||
{0x3749, 0x48},
|
||||
{0x374b, 0x48},
|
||||
{0x374d, 0x48},
|
||||
{0x374f, 0x48},
|
||||
{0x3755, 0x10},
|
||||
{0x376c, 0x00},
|
||||
{0x378d, 0x3c},
|
||||
{0x3790, 0x01},
|
||||
{0x3791, 0x01},
|
||||
{0x3798, 0x40},
|
||||
{0x379e, 0x00},
|
||||
{0x379f, 0x04},
|
||||
{0x37a1, 0x10},
|
||||
{0x37a2, 0x1e},
|
||||
{0x37a8, 0x10},
|
||||
{0x37a9, 0x1e},
|
||||
{0x37ac, 0xa0},
|
||||
{0x37b9, 0x01},
|
||||
{0x37bd, 0x01},
|
||||
{0x37bf, 0x26},
|
||||
{0x37c0, 0x11},
|
||||
{0x37c2, 0x04},
|
||||
{0x37cd, 0x19},
|
||||
{0x37e0, 0x08},
|
||||
{0x37e6, 0x04},
|
||||
{0x37e5, 0x02},
|
||||
{0x37e1, 0x0c},
|
||||
{0x3737, 0x04},
|
||||
{0x37d8, 0x02},
|
||||
{0x37e2, 0x10},
|
||||
{0x3739, 0x10},
|
||||
{0x3662, 0x10},
|
||||
{0x37e4, 0x20},
|
||||
{0x37e3, 0x08},
|
||||
{0x37d9, 0x08},
|
||||
{0x4040, 0x00},
|
||||
{0x4041, 0x07},
|
||||
{0x4008, 0x02},
|
||||
{0x4009, 0x0d},
|
||||
|
||||
// FSIN - frame sync
|
||||
{0x3002, 0x22},
|
||||
{0x3663, 0x22},
|
||||
{0x368a, 0x04},
|
||||
{0x3822, 0x44},
|
||||
{0x3823, 0x00},
|
||||
{0x3829, 0x03},
|
||||
{0x3832, 0xf8},
|
||||
{0x382c, 0x00},
|
||||
{0x3844, 0x06},
|
||||
{0x3843, 0x00},
|
||||
{0x382a, 0x00},
|
||||
{0x382b, 0x0c},
|
||||
|
||||
// 2704x1536 -> 2688x1520 out
|
||||
{0x3800, 0x00}, {0x3801, 0x00},
|
||||
{0x3802, 0x00}, {0x3803, 0x00},
|
||||
{0x3804, 0x0a}, {0x3805, 0x8f},
|
||||
{0x3806, 0x05}, {0x3807, 0xff},
|
||||
{0x3808, 0x0a}, {0x3809, 0x80},
|
||||
{0x380a, 0x05}, {0x380b, 0xf0},
|
||||
{0x3811, 0x08},
|
||||
{0x3813, 0x08},
|
||||
{0x3814, 0x01},
|
||||
{0x3815, 0x01},
|
||||
{0x3816, 0x01},
|
||||
{0x3817, 0x01},
|
||||
|
||||
{0x380c, 0x08}, {0x380d, 0x5c}, // HTS (line length)
|
||||
{0x380e, 0x09}, {0x380f, 0x38}, // VTS (frame length)
|
||||
|
||||
{0x3820, 0xb0},
|
||||
{0x3821, 0x00},
|
||||
{0x3880, 0x00},
|
||||
{0x3882, 0x20},
|
||||
{0x3c91, 0x0b},
|
||||
{0x3c94, 0x45},
|
||||
{0x3cad, 0x00},
|
||||
{0x3cae, 0x00},
|
||||
{0x4000, 0xf3},
|
||||
{0x4001, 0x60},
|
||||
{0x4003, 0x40},
|
||||
{0x4300, 0xff},
|
||||
{0x4302, 0x0f},
|
||||
{0x4305, 0x83},
|
||||
{0x4505, 0x84},
|
||||
{0x4809, 0x0e},
|
||||
{0x480a, 0x04},
|
||||
{0x4837, 0x15},
|
||||
{0x4c00, 0x08},
|
||||
{0x4c01, 0x08},
|
||||
{0x4c04, 0x00},
|
||||
{0x4c05, 0x00},
|
||||
{0x5000, 0xf9},
|
||||
// {0x0100, 0x01},
|
||||
// {0x320d, 0x00},
|
||||
// {0x3208, 0xa0},
|
||||
|
||||
// initialize exposure
|
||||
{0x3503, 0x88},
|
||||
|
||||
// long exposure
|
||||
{0x3500, 0x00}, {0x3501, 0x00}, {0x3502, 0x10},
|
||||
{0x3508, 0x00}, {0x3509, 0x80},
|
||||
{0x350a, 0x04}, {0x350b, 0x00},
|
||||
|
||||
// short exposure
|
||||
{0x3510, 0x00}, {0x3511, 0x00}, {0x3512, 0x40},
|
||||
{0x350c, 0x00}, {0x350d, 0x80},
|
||||
{0x350e, 0x04}, {0x350f, 0x00},
|
||||
|
||||
// white balance
|
||||
// b
|
||||
{0x5100, 0x06}, {0x5101, 0x7e},
|
||||
{0x5140, 0x06}, {0x5141, 0x7e},
|
||||
// g
|
||||
{0x5102, 0x04}, {0x5103, 0x00},
|
||||
{0x5142, 0x04}, {0x5143, 0x00},
|
||||
// r
|
||||
{0x5104, 0x08}, {0x5105, 0xd6},
|
||||
{0x5144, 0x08}, {0x5145, 0xd6},
|
||||
};
|
||||
149
iqpilot/system/camerad/sensors/ox03c10.cc
Normal file
149
iqpilot/system/camerad/sensors/ox03c10.cc
Normal file
@@ -0,0 +1,149 @@
|
||||
#include <cmath>
|
||||
#include <cstdlib>
|
||||
|
||||
#include "system/camerad/sensors/sensor.h"
|
||||
#include "third_party/linux/include/msm_camsensor_sdk.h"
|
||||
|
||||
namespace {
|
||||
|
||||
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 uint32_t VS_TIME_MIN_OX03C10 = 1;
|
||||
const uint32_t VS_TIME_MAX_OX03C10 = 34; // vs < 35
|
||||
|
||||
} // namespace
|
||||
|
||||
OX03C10::OX03C10() {
|
||||
image_sensor = cereal::FrameData::ImageSensor::OX03C10;
|
||||
bayer_pattern = CAM_ISP_PATTERN_BAYER_GRGRGR;
|
||||
pixel_size_mm = 0.003;
|
||||
data_word = false;
|
||||
frame_width = 1928;
|
||||
frame_height = 1208;
|
||||
frame_stride = (frame_width * 12 / 8) + 4;
|
||||
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));
|
||||
if (std::getenv("SPECTRA_TEST_PATTERN")) {
|
||||
init_reg_array.insert(init_reg_array.end(), {
|
||||
{0x5004, 0x1f}, {0x5005, 0x1f}, {0x5006, 0x1f}, {0x5007, 0x1f},
|
||||
{0x5240, 0x03}, {0x5440, 0x03}, {0x5640, 0x03}, {0x5840, 0x03},
|
||||
});
|
||||
}
|
||||
probe_reg_addr = 0x300a;
|
||||
probe_expected_data = 0x5803;
|
||||
bits_per_pixel = 12;
|
||||
mipi_format = CAM_FORMAT_MIPI_RAW_12;
|
||||
frame_data_type = CSI_RAW12;
|
||||
mclk_frequency = 24000000; // Hz
|
||||
|
||||
readout_time_ns = 14697000;
|
||||
|
||||
dc_gain_factor = 7.32;
|
||||
dc_gain_min_weight = 1; // always on is fine
|
||||
dc_gain_max_weight = 1;
|
||||
dc_gain_on_grey = 0.9;
|
||||
dc_gain_off_grey = 1.0;
|
||||
exposure_time_min = 2; // 1x
|
||||
exposure_time_max = 2016;
|
||||
analog_gain_min_idx = 0x0;
|
||||
analog_gain_rec_idx = 0x0; // 1x
|
||||
analog_gain_max_idx = 0x36;
|
||||
analog_gain_cost_delta = -1;
|
||||
analog_gain_cost_low = 0.4;
|
||||
analog_gain_cost_high = 6.4;
|
||||
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 = 0.01;
|
||||
|
||||
black_level = 0;
|
||||
color_correct_matrix = {
|
||||
0x000000b6, 0x00000ff1, 0x00000fda,
|
||||
0x00000fcc, 0x000000b9, 0x00000ffb,
|
||||
0x00000fc2, 0x00000ff6, 0x000000c9,
|
||||
};
|
||||
for (int i = 0; i < 65; i++) {
|
||||
float fx = i / 64.0;
|
||||
fx = -0.507089*exp(-12.54124638*fx) + 0.9655*pow(fx, 0.5) - 0.472597*fx + 0.507089;
|
||||
gamma_lut_rgb.push_back((uint32_t)(fx*1023.0 + 0.5));
|
||||
}
|
||||
prepare_gamma_lut();
|
||||
linearization_lut = {
|
||||
0x00200000, 0x00200000, 0x00200000, 0x00200000,
|
||||
0x00404080, 0x00404080, 0x00404080, 0x00404080,
|
||||
0x00804100, 0x00804100, 0x00804100, 0x00804100,
|
||||
0x02014402, 0x02014402, 0x02014402, 0x02014402,
|
||||
0x0402c804, 0x0402c804, 0x0402c804, 0x0402c804,
|
||||
0x0805d00a, 0x0805d00a, 0x0805d00a, 0x0805d00a,
|
||||
0x100ba015, 0x100ba015, 0x100ba015, 0x100ba015,
|
||||
0x00003fff, 0x00003fff, 0x00003fff, 0x00003fff,
|
||||
0x00003fff, 0x00003fff, 0x00003fff, 0x00003fff,
|
||||
};
|
||||
linearization_pts = {0x07ff0bff, 0x17ff1bff, 0x1fff23ff, 0x27ff3fff};
|
||||
vignetting_lut = {
|
||||
0x00eaa755, 0x00cf2679, 0x00bc05e0, 0x00acc566, 0x00a1450a, 0x009984cc, 0x0095a4ad, 0x009584ac, 0x009944ca, 0x00a0c506, 0x00ac0560, 0x00bb25d9, 0x00ce2671, 0x00e90748, 0x01112889, 0x014a2a51, 0x01984cc2,
|
||||
0x00db06d8, 0x00c30618, 0x00afe57f, 0x00a0a505, 0x009524a9, 0x008d646b, 0x0089844c, 0x0089644b, 0x008d2469, 0x0094a4a5, 0x009fe4ff, 0x00af0578, 0x00c20610, 0x00d986cc, 0x00fda7ed, 0x01320990, 0x017aebd7,
|
||||
0x00d1868c, 0x00baa5d5, 0x00a7853c, 0x009844c2, 0x008cc466, 0x0085a42d, 0x0083641b, 0x0083641b, 0x0085842c, 0x008c4462, 0x0097a4bd, 0x00a6c536, 0x00b9a5cd, 0x00d06683, 0x00f1678b, 0x01226913, 0x0167ab3d,
|
||||
0x00cd0668, 0x00b625b1, 0x00a30518, 0x0093c49e, 0x00884442, 0x00830418, 0x0080e407, 0x0080c406, 0x0082e417, 0x0087c43e, 0x00932499, 0x00a22511, 0x00b525a9, 0x00cbe65f, 0x00eb0758, 0x011a68d3, 0x015daaed,
|
||||
0x00cc4662, 0x00b565ab, 0x00a24512, 0x00930498, 0x0087843c, 0x0082a415, 0x00806403, 0x00806403, 0x00828414, 0x00870438, 0x00926493, 0x00a1850c, 0x00b465a3, 0x00cb2659, 0x00ea2751, 0x011928c9, 0x015c2ae1,
|
||||
0x00cf667b, 0x00b885c4, 0x00a5652b, 0x009624b1, 0x008aa455, 0x00846423, 0x00822411, 0x00822411, 0x00844422, 0x008a2451, 0x009564ab, 0x00a48524, 0x00b785bc, 0x00ce4672, 0x00ee6773, 0x011e88f4, 0x0162eb17,
|
||||
0x00d6c6b6, 0x00bf65fb, 0x00ac4562, 0x009d04e8, 0x0091848c, 0x0089c44e, 0x00862431, 0x00860430, 0x0089844c, 0x00910488, 0x009c64e3, 0x00ab655b, 0x00be65f3, 0x00d566ab, 0x00f847c2, 0x012b2959, 0x01726b93,
|
||||
0x00e3e71f, 0x00ca0650, 0x00b705b8, 0x00a7a53d, 0x009c24e1, 0x009484a4, 0x00908484, 0x00908484, 0x009424a1, 0x009bc4de, 0x00a70538, 0x00b625b1, 0x00c90648, 0x00e26713, 0x0108e847, 0x013fe9ff, 0x018bcc5e,
|
||||
0x00f807c0, 0x00d966cb, 0x00c5862c, 0x00b625b1, 0x00aaa555, 0x00a30518, 0x009f04f8, 0x009f04f8, 0x00a2a515, 0x00aa2551, 0x00b585ac, 0x00c4a625, 0x00d846c2, 0x00f647b2, 0x0121a90d, 0x015e4af2, 0x01b8cdc6,
|
||||
0x011548aa, 0x00f1678b, 0x00d886c4, 0x00c86643, 0x00bce5e7, 0x00b545aa, 0x00b1658b, 0x00b1458a, 0x00b505a8, 0x00bc85e4, 0x00c7c63e, 0x00d786bc, 0x00efe77f, 0x0113489a, 0x0144ea27, 0x01888c44, 0x01fdcfee,
|
||||
0x013e49f2, 0x0113e89f, 0x00f5a7ad, 0x00e0c706, 0x00d30698, 0x00cb665b, 0x00c7663b, 0x00c7663b, 0x00cb0658, 0x00d2a695, 0x00dfe6ff, 0x00f467a3, 0x01122891, 0x013be9df, 0x01750ba8, 0x01cfae7d, 0x025912c8,
|
||||
0x01766bb3, 0x01446a23, 0x011fc8fe, 0x0105e82f, 0x00f467a3, 0x00e9874c, 0x00e46723, 0x00e44722, 0x00e92749, 0x00f3a79d, 0x0104c826, 0x011e48f2, 0x01424a12, 0x01738b9c, 0x01bf6dfb, 0x023611b0, 0x02ced676,
|
||||
0x01cf8e7c, 0x01866c33, 0x015aaad5, 0x013ae9d7, 0x01250928, 0x011768bb, 0x0110a885, 0x01108884, 0x0116e8b7, 0x01242921, 0x0139a9cd, 0x0158eac7, 0x01840c20, 0x01cb0e58, 0x0233719b, 0x02b9d5ce, 0x03645b22,
|
||||
};
|
||||
}
|
||||
|
||||
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, (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];
|
||||
|
||||
return {
|
||||
{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},
|
||||
};
|
||||
}
|
||||
|
||||
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;
|
||||
}
|
||||
751
iqpilot/system/camerad/sensors/ox03c10_registers.h
Normal file
751
iqpilot/system/camerad/sensors/ox03c10_registers.h
Normal file
@@ -0,0 +1,751 @@
|
||||
#pragma once
|
||||
|
||||
const struct i2c_random_wr_payload start_reg_array_ox03c10[] = {{0x100, 1}};
|
||||
const struct i2c_random_wr_payload stop_reg_array_ox03c10[] = {{0x100, 0}};
|
||||
|
||||
const struct i2c_random_wr_payload init_array_ox03c10[] = {
|
||||
{0x103, 1},
|
||||
{0x107, 1},
|
||||
|
||||
// X3C_1920x1280_60fps_HDR4_LFR_PWL12_mipi1200
|
||||
|
||||
// TPM
|
||||
{0x4d5a, 0x1a}, {0x4d09, 0xff}, {0x4d09, 0xdf},
|
||||
|
||||
/*)
|
||||
// group 4
|
||||
{0x3208, 0x04},
|
||||
{0x4620, 0x04},
|
||||
{0x3208, 0x14},
|
||||
|
||||
// group 5
|
||||
{0x3208, 0x05},
|
||||
{0x4620, 0x04},
|
||||
{0x3208, 0x15},
|
||||
|
||||
// group 2
|
||||
{0x3208, 0x02},
|
||||
{0x3507, 0x00},
|
||||
{0x3208, 0x12},
|
||||
|
||||
// delay launch group 2
|
||||
{0x3208, 0xa2},*/
|
||||
|
||||
// **NOTE**: if this is changed, readout_time_ns must be updated in the Sensor config
|
||||
// PLL setup
|
||||
{0x0301, 0xc8}, // pll1_divs, pll1_predivp, pll1_divpix
|
||||
{0x0303, 0x01}, // pll1_prediv
|
||||
{0x0304, 0x01}, {0x0305, 0x2c}, // pll1_loopdiv = 300
|
||||
{0x0306, 0x04}, // pll1_divmipi = 4
|
||||
{0x0307, 0x01}, // pll1_divm = 1
|
||||
{0x0316, 0x00},
|
||||
{0x0317, 0x00},
|
||||
{0x0318, 0x00},
|
||||
{0x0323, 0x05}, // pll2_prediv
|
||||
{0x0324, 0x01}, {0x0325, 0x2c}, // pll2_divp = 300
|
||||
|
||||
// SCLK/PCLK
|
||||
{0x0400, 0xe0}, {0x0401, 0x80},
|
||||
{0x0403, 0xde}, {0x0404, 0x34},
|
||||
{0x0405, 0x3b}, {0x0406, 0xde},
|
||||
{0x0407, 0x08},
|
||||
{0x0408, 0xe0}, {0x0409, 0x7f},
|
||||
{0x040a, 0xde}, {0x040b, 0x34},
|
||||
{0x040c, 0x47}, {0x040d, 0xd8},
|
||||
{0x040e, 0x08},
|
||||
|
||||
// xchk
|
||||
{0x2803, 0xfe}, {0x280b, 0x00}, {0x280c, 0x79},
|
||||
|
||||
// SC ctrl
|
||||
{0x3001, 0x03}, // io_pad_oen
|
||||
{0x3002, 0xfc}, // io_pad_oen
|
||||
{0x3005, 0x80}, // io_pad_out
|
||||
{0x3007, 0x01}, // io_pad_sel
|
||||
{0x3008, 0x80}, // io_pad_sel
|
||||
|
||||
// FSIN (frame sync) with external pulses
|
||||
{0x3009, 0x2},
|
||||
{0x3015, 0x2},
|
||||
{0x383E, 0x80},
|
||||
{0x3881, 0x4},
|
||||
{0x3882, 0x8}, {0x3883, 0x0D},
|
||||
{0x3836, 0x1F}, {0x3837, 0x40},
|
||||
|
||||
// causes issues on some devices
|
||||
//{0x3822, 0x33}, // wait for pulse before first frame
|
||||
|
||||
{0x3892, 0x44},
|
||||
{0x3823, 0x41},
|
||||
|
||||
{0x3012, 0x41}, // SC_PHY_CTRL = 4 lane MIPI
|
||||
{0x3020, 0x05}, // SC_CTRL_20
|
||||
|
||||
// this is not in the datasheet, listed as RSVD
|
||||
// but the camera doesn't work without it
|
||||
{0x3700, 0x28}, {0x3701, 0x15}, {0x3702, 0x19}, {0x3703, 0x23},
|
||||
{0x3704, 0x0a}, {0x3705, 0x00}, {0x3706, 0x3e}, {0x3707, 0x0d},
|
||||
{0x3708, 0x50}, {0x3709, 0x5a}, {0x370a, 0x00}, {0x370b, 0x96},
|
||||
{0x3711, 0x11}, {0x3712, 0x13}, {0x3717, 0x02}, {0x3718, 0x73},
|
||||
{0x372c, 0x40}, {0x3733, 0x01}, {0x3738, 0x36}, {0x3739, 0x36},
|
||||
{0x373a, 0x25}, {0x373b, 0x25}, {0x373f, 0x21}, {0x3740, 0x21},
|
||||
{0x3741, 0x21}, {0x3742, 0x21}, {0x3747, 0x28}, {0x3748, 0x28},
|
||||
{0x3749, 0x19}, {0x3755, 0x1a}, {0x3756, 0x0a}, {0x3757, 0x1c},
|
||||
{0x3765, 0x19}, {0x3766, 0x05}, {0x3767, 0x05}, {0x3768, 0x13},
|
||||
{0x376c, 0x07}, {0x3778, 0x20}, {0x377c, 0xc8}, {0x3781, 0x02},
|
||||
{0x3783, 0x02}, {0x379c, 0x58}, {0x379e, 0x00}, {0x379f, 0x00},
|
||||
{0x37a0, 0x00}, {0x37bc, 0x22}, {0x37c0, 0x01}, {0x37c4, 0x3e},
|
||||
{0x37c5, 0x3e}, {0x37c6, 0x2a}, {0x37c7, 0x28}, {0x37c8, 0x02},
|
||||
{0x37c9, 0x12}, {0x37cb, 0x29}, {0x37cd, 0x29}, {0x37d2, 0x00},
|
||||
{0x37d3, 0x73}, {0x37d6, 0x00}, {0x37d7, 0x6b}, {0x37dc, 0x00},
|
||||
{0x37df, 0x54}, {0x37e2, 0x00}, {0x37e3, 0x00}, {0x37f8, 0x00},
|
||||
{0x37f9, 0x01}, {0x37fa, 0x00}, {0x37fb, 0x19},
|
||||
|
||||
// also RSVD
|
||||
{0x3c03, 0x01}, {0x3c04, 0x01}, {0x3c06, 0x21}, {0x3c08, 0x01},
|
||||
{0x3c09, 0x01}, {0x3c0a, 0x01}, {0x3c0b, 0x21}, {0x3c13, 0x21},
|
||||
{0x3c14, 0x82}, {0x3c16, 0x13}, {0x3c21, 0x00}, {0x3c22, 0xf3},
|
||||
{0x3c37, 0x12}, {0x3c38, 0x31}, {0x3c3c, 0x00}, {0x3c3d, 0x03},
|
||||
{0x3c44, 0x16}, {0x3c5c, 0x8a}, {0x3c5f, 0x03}, {0x3c61, 0x80},
|
||||
{0x3c6f, 0x2b}, {0x3c70, 0x5f}, {0x3c71, 0x2c}, {0x3c72, 0x2c},
|
||||
{0x3c73, 0x2c}, {0x3c76, 0x12},
|
||||
|
||||
// PEC checks
|
||||
{0x3182, 0x12},
|
||||
|
||||
{0x320e, 0x00}, {0x320f, 0x00}, // RSVD
|
||||
{0x3211, 0x61},
|
||||
{0x3215, 0xcd},
|
||||
{0x3219, 0x08},
|
||||
|
||||
{0x3506, 0x20}, {0x3507, 0x00}, // hcg fine exposure
|
||||
{0x350a, 0x01}, {0x350b, 0x00}, {0x350c, 0x00}, // hcg digital gain
|
||||
|
||||
{0x3586, 0x40}, {0x3587, 0x00}, // lcg fine exposure
|
||||
{0x358a, 0x01}, {0x358b, 0x00}, {0x358c, 0x00}, // lcg digital gain
|
||||
|
||||
{0x3546, 0x20}, {0x3547, 0x00}, // spd fine exposure
|
||||
{0x354a, 0x01}, {0x354b, 0x00}, {0x354c, 0x00}, // spd digital gain
|
||||
|
||||
{0x35c6, 0xb0}, {0x35c7, 0x00}, // vs fine exposure
|
||||
{0x35ca, 0x01}, {0x35cb, 0x00}, {0x35cc, 0x00}, // vs digital gain
|
||||
|
||||
// also RSVD
|
||||
{0x3600, 0x8f}, {0x3605, 0x16}, {0x3609, 0xf0}, {0x360a, 0x01},
|
||||
{0x360e, 0x1d}, {0x360f, 0x10}, {0x3610, 0x70}, {0x3611, 0x3a},
|
||||
{0x3612, 0x28}, {0x361a, 0x29}, {0x361b, 0x6c}, {0x361c, 0x0b},
|
||||
{0x361d, 0x00}, {0x361e, 0xfc}, {0x362a, 0x00}, {0x364d, 0x0f},
|
||||
{0x364e, 0x18}, {0x364f, 0x12}, {0x3653, 0x1c}, {0x3654, 0x00},
|
||||
{0x3655, 0x1f}, {0x3656, 0x1f}, {0x3657, 0x0c}, {0x3658, 0x0a},
|
||||
{0x3659, 0x14}, {0x365a, 0x18}, {0x365b, 0x14}, {0x365c, 0x10},
|
||||
{0x365e, 0x12}, {0x3674, 0x08}, {0x3677, 0x3a}, {0x3678, 0x3a},
|
||||
{0x3679, 0x19},
|
||||
|
||||
// Y_ADDR_START = 4
|
||||
{0x3802, 0x00}, {0x3803, 0x04},
|
||||
// Y_ADDR_END = 0x50b
|
||||
{0x3806, 0x05}, {0x3807, 0x0b},
|
||||
|
||||
// X_OUTPUT_SIZE = 0x780 = 1920 (changed to 1928)
|
||||
{0x3808, 0x07}, {0x3809, 0x88},
|
||||
|
||||
// Y_OUTPUT_SIZE = 0x500 = 1280 (changed to 1208)
|
||||
{0x380a, 0x04}, {0x380b, 0xb8},
|
||||
|
||||
// horizontal timing 0x447
|
||||
{0x380c, 0x04}, {0x380d, 0x47},
|
||||
|
||||
// rows per frame (was 0x2ae)
|
||||
// 0x8ae = 53.65 ms
|
||||
{0x380e, 0x08}, {0x380f, 0x15},
|
||||
// this should be triggered by FSIN, not free running
|
||||
|
||||
{0x3810, 0x00}, {0x3811, 0x08}, // x cutoff
|
||||
{0x3812, 0x00}, {0x3813, 0x04}, // y cutoff
|
||||
{0x3816, 0x01},
|
||||
{0x3817, 0x01},
|
||||
{0x381c, 0x18},
|
||||
{0x381e, 0x01},
|
||||
{0x381f, 0x01},
|
||||
|
||||
// don't mirror, just flip
|
||||
{0x3820, 0x04},
|
||||
|
||||
{0x3821, 0x19},
|
||||
{0x3832, 0xF0},
|
||||
{0x3834, 0xF0},
|
||||
{0x384c, 0x02},
|
||||
{0x384d, 0x0d},
|
||||
{0x3850, 0x00},
|
||||
{0x3851, 0x42},
|
||||
{0x3852, 0x00},
|
||||
{0x3853, 0x40},
|
||||
{0x3858, 0x04},
|
||||
{0x388c, 0x02},
|
||||
{0x388d, 0x2b},
|
||||
|
||||
// APC
|
||||
{0x3b40, 0x05}, {0x3b41, 0x40}, {0x3b42, 0x00}, {0x3b43, 0x90},
|
||||
{0x3b44, 0x00}, {0x3b45, 0x20}, {0x3b46, 0x00}, {0x3b47, 0x20},
|
||||
{0x3b48, 0x19}, {0x3b49, 0x12}, {0x3b4a, 0x16}, {0x3b4b, 0x2e},
|
||||
{0x3b4c, 0x00}, {0x3b4d, 0x00},
|
||||
{0x3b86, 0x00}, {0x3b87, 0x34}, {0x3b88, 0x00}, {0x3b89, 0x08},
|
||||
{0x3b8a, 0x05}, {0x3b8b, 0x00}, {0x3b8c, 0x07}, {0x3b8d, 0x80},
|
||||
{0x3b8e, 0x00}, {0x3b8f, 0x00}, {0x3b92, 0x05}, {0x3b93, 0x00},
|
||||
{0x3b94, 0x07}, {0x3b95, 0x80}, {0x3b9e, 0x09},
|
||||
|
||||
// OTP
|
||||
{0x3d82, 0x73},
|
||||
{0x3d85, 0x05},
|
||||
{0x3d8a, 0x03},
|
||||
{0x3d8b, 0xff},
|
||||
{0x3d99, 0x00},
|
||||
{0x3d9a, 0x9f},
|
||||
{0x3d9b, 0x00},
|
||||
{0x3d9c, 0xa0},
|
||||
{0x3da4, 0x00},
|
||||
{0x3da7, 0x50},
|
||||
|
||||
// DTR
|
||||
{0x420e, 0x6b},
|
||||
{0x420f, 0x6e},
|
||||
{0x4210, 0x06},
|
||||
{0x4211, 0xc1},
|
||||
{0x421e, 0x02},
|
||||
{0x421f, 0x45},
|
||||
{0x4220, 0xe1},
|
||||
{0x4221, 0x01},
|
||||
{0x4301, 0xff},
|
||||
{0x4307, 0x03},
|
||||
{0x4308, 0x13},
|
||||
{0x430a, 0x13},
|
||||
{0x430d, 0x93},
|
||||
{0x430f, 0x57},
|
||||
{0x4310, 0x95},
|
||||
{0x4311, 0x16},
|
||||
{0x4316, 0x00},
|
||||
|
||||
{0x4317, 0x38}, // both embedded rows are enabled
|
||||
|
||||
{0x4319, 0x03}, // spd dcg
|
||||
{0x431a, 0x00}, // 8 bit mipi
|
||||
{0x431b, 0x00},
|
||||
{0x431d, 0x2a},
|
||||
{0x431e, 0x11},
|
||||
|
||||
{0x431f, 0x20}, // enable PWL (pwl0_en), 12 bits
|
||||
//{0x431f, 0x00}, // disable PWL
|
||||
|
||||
{0x4320, 0x19},
|
||||
{0x4323, 0x80},
|
||||
{0x4324, 0x00},
|
||||
{0x4503, 0x4e},
|
||||
{0x4505, 0x00},
|
||||
{0x4509, 0x00},
|
||||
{0x450a, 0x00},
|
||||
{0x4580, 0xf8},
|
||||
{0x4583, 0x07},
|
||||
{0x4584, 0x6a},
|
||||
{0x4585, 0x08},
|
||||
{0x4586, 0x05},
|
||||
{0x4587, 0x04},
|
||||
{0x4588, 0x73},
|
||||
{0x4589, 0x05},
|
||||
{0x458a, 0x1f},
|
||||
{0x458b, 0x02},
|
||||
{0x458c, 0xdc},
|
||||
{0x458d, 0x03},
|
||||
{0x458e, 0x02},
|
||||
{0x4597, 0x07},
|
||||
{0x4598, 0x40},
|
||||
{0x4599, 0x0e},
|
||||
{0x459a, 0x0e},
|
||||
{0x459b, 0xfb},
|
||||
{0x459c, 0xf3},
|
||||
{0x4602, 0x00},
|
||||
{0x4603, 0x13},
|
||||
{0x4604, 0x00},
|
||||
{0x4609, 0x0a},
|
||||
{0x460a, 0x30},
|
||||
{0x4610, 0x00},
|
||||
{0x4611, 0x70},
|
||||
{0x4612, 0x01},
|
||||
{0x4613, 0x00},
|
||||
{0x4614, 0x00},
|
||||
{0x4615, 0x70},
|
||||
{0x4616, 0x01},
|
||||
{0x4617, 0x00},
|
||||
|
||||
{0x4800, 0x04}, // invert output PCLK
|
||||
{0x480a, 0x22},
|
||||
{0x4813, 0xe4},
|
||||
|
||||
// mipi
|
||||
{0x4814, 0x2a},
|
||||
{0x4837, 0x0d},
|
||||
{0x484b, 0x47},
|
||||
{0x484f, 0x00},
|
||||
{0x4887, 0x51},
|
||||
{0x4d00, 0x4a},
|
||||
{0x4d01, 0x18},
|
||||
{0x4d05, 0xff},
|
||||
{0x4d06, 0x88},
|
||||
{0x4d08, 0x63},
|
||||
{0x4d09, 0xdf},
|
||||
{0x4d15, 0x7d},
|
||||
{0x4d1a, 0x20},
|
||||
{0x4d30, 0x0a},
|
||||
{0x4d31, 0x00},
|
||||
{0x4d34, 0x7d},
|
||||
{0x4d3c, 0x7d},
|
||||
{0x4f00, 0x00},
|
||||
{0x4f01, 0x00},
|
||||
{0x4f02, 0x00},
|
||||
{0x4f03, 0x20},
|
||||
{0x4f04, 0xe0},
|
||||
{0x6a00, 0x00},
|
||||
{0x6a01, 0x20},
|
||||
{0x6a02, 0x00},
|
||||
{0x6a03, 0x20},
|
||||
{0x6a04, 0x02},
|
||||
{0x6a05, 0x80},
|
||||
{0x6a06, 0x01},
|
||||
{0x6a07, 0xe0},
|
||||
{0x6a08, 0xcf},
|
||||
{0x6a09, 0x01},
|
||||
{0x6a0a, 0x40},
|
||||
{0x6a20, 0x00},
|
||||
{0x6a21, 0x02},
|
||||
{0x6a22, 0x00},
|
||||
{0x6a23, 0x00},
|
||||
{0x6a24, 0x00},
|
||||
{0x6a25, 0x00},
|
||||
{0x6a26, 0x00},
|
||||
{0x6a27, 0x00},
|
||||
{0x6a28, 0x00},
|
||||
|
||||
// isp
|
||||
{0x5000, 0x8f},
|
||||
{0x5001, 0x75},
|
||||
{0x5002, 0x7f}, // PWL0
|
||||
//{0x5002, 0x3f}, // PWL disable
|
||||
{0x5003, 0x7a},
|
||||
|
||||
{0x5004, 0x3e},
|
||||
{0x5005, 0x1e},
|
||||
{0x5006, 0x1e},
|
||||
{0x5007, 0x1e},
|
||||
|
||||
{0x5008, 0x00},
|
||||
{0x500c, 0x00},
|
||||
{0x502c, 0x00},
|
||||
{0x502e, 0x00},
|
||||
{0x502f, 0x00},
|
||||
{0x504b, 0x00},
|
||||
{0x5053, 0x00},
|
||||
{0x505b, 0x00},
|
||||
{0x5063, 0x00},
|
||||
{0x5070, 0x00},
|
||||
{0x5074, 0x04},
|
||||
{0x507a, 0x04},
|
||||
{0x507b, 0x09},
|
||||
{0x5500, 0x02},
|
||||
{0x5700, 0x02},
|
||||
{0x5900, 0x02},
|
||||
{0x6007, 0x04},
|
||||
{0x6008, 0x05},
|
||||
{0x6009, 0x02},
|
||||
{0x600b, 0x08},
|
||||
{0x600c, 0x07},
|
||||
{0x600d, 0x88},
|
||||
{0x6016, 0x00},
|
||||
{0x6027, 0x04},
|
||||
{0x6028, 0x05},
|
||||
{0x6029, 0x02},
|
||||
{0x602b, 0x08},
|
||||
{0x602c, 0x07},
|
||||
{0x602d, 0x88},
|
||||
{0x6047, 0x04},
|
||||
{0x6048, 0x05},
|
||||
{0x6049, 0x02},
|
||||
{0x604b, 0x08},
|
||||
{0x604c, 0x07},
|
||||
{0x604d, 0x88},
|
||||
{0x6067, 0x04},
|
||||
{0x6068, 0x05},
|
||||
{0x6069, 0x02},
|
||||
{0x606b, 0x08},
|
||||
{0x606c, 0x07},
|
||||
{0x606d, 0x88},
|
||||
{0x6087, 0x04},
|
||||
{0x6088, 0x05},
|
||||
{0x6089, 0x02},
|
||||
{0x608b, 0x08},
|
||||
{0x608c, 0x07},
|
||||
{0x608d, 0x88},
|
||||
|
||||
// 12-bit PWL0
|
||||
{0x5e00, 0x00},
|
||||
|
||||
// m_ndX_exp[0:32]
|
||||
// 9*2+0xa*3+0xb*2+0xc*2+0xd*2+0xe*2+0xf*2+0x10*2+0x11*2+0x12*4+0x13*3+0x14*3+0x15*3+0x16 = 518
|
||||
{0x5e01, 0x09},
|
||||
{0x5e02, 0x09},
|
||||
{0x5e03, 0x0a},
|
||||
{0x5e04, 0x0a},
|
||||
{0x5e05, 0x0a},
|
||||
{0x5e06, 0x0b},
|
||||
{0x5e07, 0x0b},
|
||||
{0x5e08, 0x0c},
|
||||
{0x5e09, 0x0c},
|
||||
{0x5e0a, 0x0d},
|
||||
{0x5e0b, 0x0d},
|
||||
{0x5e0c, 0x0e},
|
||||
{0x5e0d, 0x0e},
|
||||
{0x5e0e, 0x0f},
|
||||
{0x5e0f, 0x0f},
|
||||
{0x5e10, 0x10},
|
||||
{0x5e11, 0x10},
|
||||
{0x5e12, 0x11},
|
||||
{0x5e13, 0x11},
|
||||
{0x5e14, 0x12},
|
||||
{0x5e15, 0x12},
|
||||
{0x5e16, 0x12},
|
||||
{0x5e17, 0x12},
|
||||
{0x5e18, 0x13},
|
||||
{0x5e19, 0x13},
|
||||
{0x5e1a, 0x13},
|
||||
{0x5e1b, 0x14},
|
||||
{0x5e1c, 0x14},
|
||||
{0x5e1d, 0x14},
|
||||
{0x5e1e, 0x15},
|
||||
{0x5e1f, 0x15},
|
||||
{0x5e20, 0x15},
|
||||
{0x5e21, 0x16},
|
||||
|
||||
// m_ndY_val[0:32]
|
||||
// 0x200+0xff+0x100*3+0x80*12+0x40*16 = 4095
|
||||
{0x5e22, 0x00}, {0x5e23, 0x02}, {0x5e24, 0x00},
|
||||
{0x5e25, 0x00}, {0x5e26, 0x00}, {0x5e27, 0xff},
|
||||
{0x5e28, 0x00}, {0x5e29, 0x01}, {0x5e2a, 0x00},
|
||||
{0x5e2b, 0x00}, {0x5e2c, 0x01}, {0x5e2d, 0x00},
|
||||
{0x5e2e, 0x00}, {0x5e2f, 0x01}, {0x5e30, 0x00},
|
||||
{0x5e31, 0x00}, {0x5e32, 0x00}, {0x5e33, 0x80},
|
||||
{0x5e34, 0x00}, {0x5e35, 0x00}, {0x5e36, 0x80},
|
||||
{0x5e37, 0x00}, {0x5e38, 0x00}, {0x5e39, 0x80},
|
||||
{0x5e3a, 0x00}, {0x5e3b, 0x00}, {0x5e3c, 0x80},
|
||||
{0x5e3d, 0x00}, {0x5e3e, 0x00}, {0x5e3f, 0x80},
|
||||
{0x5e40, 0x00}, {0x5e41, 0x00}, {0x5e42, 0x80},
|
||||
{0x5e43, 0x00}, {0x5e44, 0x00}, {0x5e45, 0x80},
|
||||
{0x5e46, 0x00}, {0x5e47, 0x00}, {0x5e48, 0x80},
|
||||
{0x5e49, 0x00}, {0x5e4a, 0x00}, {0x5e4b, 0x80},
|
||||
{0x5e4c, 0x00}, {0x5e4d, 0x00}, {0x5e4e, 0x80},
|
||||
{0x5e4f, 0x00}, {0x5e50, 0x00}, {0x5e51, 0x80},
|
||||
{0x5e52, 0x00}, {0x5e53, 0x00}, {0x5e54, 0x80},
|
||||
{0x5e55, 0x00}, {0x5e56, 0x00}, {0x5e57, 0x40},
|
||||
{0x5e58, 0x00}, {0x5e59, 0x00}, {0x5e5a, 0x40},
|
||||
{0x5e5b, 0x00}, {0x5e5c, 0x00}, {0x5e5d, 0x40},
|
||||
{0x5e5e, 0x00}, {0x5e5f, 0x00}, {0x5e60, 0x40},
|
||||
{0x5e61, 0x00}, {0x5e62, 0x00}, {0x5e63, 0x40},
|
||||
{0x5e64, 0x00}, {0x5e65, 0x00}, {0x5e66, 0x40},
|
||||
{0x5e67, 0x00}, {0x5e68, 0x00}, {0x5e69, 0x40},
|
||||
{0x5e6a, 0x00}, {0x5e6b, 0x00}, {0x5e6c, 0x40},
|
||||
{0x5e6d, 0x00}, {0x5e6e, 0x00}, {0x5e6f, 0x40},
|
||||
{0x5e70, 0x00}, {0x5e71, 0x00}, {0x5e72, 0x40},
|
||||
{0x5e73, 0x00}, {0x5e74, 0x00}, {0x5e75, 0x40},
|
||||
{0x5e76, 0x00}, {0x5e77, 0x00}, {0x5e78, 0x40},
|
||||
{0x5e79, 0x00}, {0x5e7a, 0x00}, {0x5e7b, 0x40},
|
||||
{0x5e7c, 0x00}, {0x5e7d, 0x00}, {0x5e7e, 0x40},
|
||||
{0x5e7f, 0x00}, {0x5e80, 0x00}, {0x5e81, 0x40},
|
||||
{0x5e82, 0x00}, {0x5e83, 0x00}, {0x5e84, 0x40},
|
||||
|
||||
// disable PWL
|
||||
/*{0x5e01, 0x18}, {0x5e02, 0x00}, {0x5e03, 0x00}, {0x5e04, 0x00},
|
||||
{0x5e05, 0x00}, {0x5e06, 0x00}, {0x5e07, 0x00}, {0x5e08, 0x00},
|
||||
{0x5e09, 0x00}, {0x5e0a, 0x00}, {0x5e0b, 0x00}, {0x5e0c, 0x00},
|
||||
{0x5e0d, 0x00}, {0x5e0e, 0x00}, {0x5e0f, 0x00}, {0x5e10, 0x00},
|
||||
{0x5e11, 0x00}, {0x5e12, 0x00}, {0x5e13, 0x00}, {0x5e14, 0x00},
|
||||
{0x5e15, 0x00}, {0x5e16, 0x00}, {0x5e17, 0x00}, {0x5e18, 0x00},
|
||||
{0x5e19, 0x00}, {0x5e1a, 0x00}, {0x5e1b, 0x00}, {0x5e1c, 0x00},
|
||||
{0x5e1d, 0x00}, {0x5e1e, 0x00}, {0x5e1f, 0x00}, {0x5e20, 0x00},
|
||||
{0x5e21, 0x00},
|
||||
|
||||
{0x5e22, 0x00}, {0x5e23, 0x0f}, {0x5e24, 0xFF},*/
|
||||
|
||||
{0x4001, 0x2b}, // BLC_CTRL_1
|
||||
{0x4008, 0x02}, {0x4009, 0x03},
|
||||
{0x4018, 0x12},
|
||||
{0x4022, 0x40},
|
||||
{0x4023, 0x20},
|
||||
|
||||
// all black level targets are 0x40
|
||||
{0x4026, 0x00}, {0x4027, 0x40},
|
||||
{0x4028, 0x00}, {0x4029, 0x40},
|
||||
{0x402a, 0x00}, {0x402b, 0x40},
|
||||
{0x402c, 0x00}, {0x402d, 0x40},
|
||||
|
||||
{0x407e, 0xcc},
|
||||
{0x407f, 0x18},
|
||||
{0x4080, 0xff},
|
||||
{0x4081, 0xff},
|
||||
{0x4082, 0x01},
|
||||
{0x4083, 0x53},
|
||||
{0x4084, 0x01},
|
||||
{0x4085, 0x2b},
|
||||
{0x4086, 0x00},
|
||||
{0x4087, 0xb3},
|
||||
|
||||
{0x4640, 0x40},
|
||||
{0x4641, 0x11},
|
||||
{0x4642, 0x0e},
|
||||
{0x4643, 0xee},
|
||||
{0x4646, 0x0f},
|
||||
{0x4648, 0x00},
|
||||
{0x4649, 0x03},
|
||||
|
||||
{0x4f00, 0x00},
|
||||
{0x4f01, 0x00},
|
||||
{0x4f02, 0x80},
|
||||
{0x4f03, 0x2c},
|
||||
{0x4f04, 0xf8},
|
||||
|
||||
{0x4d09, 0xff},
|
||||
{0x4d09, 0xdf},
|
||||
|
||||
{0x5003, 0x7a},
|
||||
{0x5b80, 0x08},
|
||||
{0x5c00, 0x08},
|
||||
{0x5c80, 0x00},
|
||||
{0x5bbe, 0x12},
|
||||
{0x5c3e, 0x12},
|
||||
{0x5cbe, 0x12},
|
||||
{0x5b8a, 0x80},
|
||||
{0x5b8b, 0x80},
|
||||
{0x5b8c, 0x80},
|
||||
{0x5b8d, 0x80},
|
||||
{0x5b8e, 0x60},
|
||||
{0x5b8f, 0x80},
|
||||
{0x5b90, 0x80},
|
||||
{0x5b91, 0x80},
|
||||
{0x5b92, 0x80},
|
||||
{0x5b93, 0x20},
|
||||
{0x5b94, 0x80},
|
||||
{0x5b95, 0x80},
|
||||
{0x5b96, 0x80},
|
||||
{0x5b97, 0x20},
|
||||
{0x5b98, 0x00},
|
||||
{0x5b99, 0x80},
|
||||
{0x5b9a, 0x40},
|
||||
{0x5b9b, 0x20},
|
||||
{0x5b9c, 0x00},
|
||||
{0x5b9d, 0x00},
|
||||
{0x5b9e, 0x80},
|
||||
{0x5b9f, 0x00},
|
||||
{0x5ba0, 0x00},
|
||||
{0x5ba1, 0x00},
|
||||
{0x5ba2, 0x00},
|
||||
{0x5ba3, 0x00},
|
||||
{0x5ba4, 0x00},
|
||||
{0x5ba5, 0x00},
|
||||
{0x5ba6, 0x00},
|
||||
{0x5ba7, 0x00},
|
||||
{0x5ba8, 0x02},
|
||||
{0x5ba9, 0x00},
|
||||
{0x5baa, 0x02},
|
||||
{0x5bab, 0x76},
|
||||
{0x5bac, 0x03},
|
||||
{0x5bad, 0x08},
|
||||
{0x5bae, 0x00},
|
||||
{0x5baf, 0x80},
|
||||
{0x5bb0, 0x00},
|
||||
{0x5bb1, 0xc0},
|
||||
{0x5bb2, 0x01},
|
||||
{0x5bb3, 0x00},
|
||||
|
||||
// m_nNormCombineWeight
|
||||
{0x5c0a, 0x80}, {0x5c0b, 0x80}, {0x5c0c, 0x80}, {0x5c0d, 0x80}, {0x5c0e, 0x60},
|
||||
{0x5c0f, 0x80}, {0x5c10, 0x80}, {0x5c11, 0x80}, {0x5c12, 0x60}, {0x5c13, 0x20},
|
||||
{0x5c14, 0x80}, {0x5c15, 0x80}, {0x5c16, 0x80}, {0x5c17, 0x20}, {0x5c18, 0x00},
|
||||
{0x5c19, 0x80}, {0x5c1a, 0x40}, {0x5c1b, 0x20}, {0x5c1c, 0x00}, {0x5c1d, 0x00},
|
||||
{0x5c1e, 0x80}, {0x5c1f, 0x00}, {0x5c20, 0x00}, {0x5c21, 0x00}, {0x5c22, 0x00},
|
||||
{0x5c23, 0x00}, {0x5c24, 0x00}, {0x5c25, 0x00}, {0x5c26, 0x00}, {0x5c27, 0x00},
|
||||
|
||||
// m_nCombinThreL
|
||||
{0x5c28, 0x02}, {0x5c29, 0x00},
|
||||
{0x5c2a, 0x02}, {0x5c2b, 0x76},
|
||||
{0x5c2c, 0x03}, {0x5c2d, 0x08},
|
||||
|
||||
// m_nCombinThreS
|
||||
{0x5c2e, 0x00}, {0x5c2f, 0x80},
|
||||
{0x5c30, 0x00}, {0x5c31, 0xc0},
|
||||
{0x5c32, 0x01}, {0x5c33, 0x00},
|
||||
|
||||
// m_nNormCombineWeight
|
||||
{0x5c8a, 0x80}, {0x5c8b, 0x80}, {0x5c8c, 0x80}, {0x5c8d, 0x80}, {0x5c8e, 0x80},
|
||||
{0x5c8f, 0x80}, {0x5c90, 0x80}, {0x5c91, 0x80}, {0x5c92, 0x80}, {0x5c93, 0x60},
|
||||
{0x5c94, 0x80}, {0x5c95, 0x80}, {0x5c96, 0x80}, {0x5c97, 0x60}, {0x5c98, 0x40},
|
||||
{0x5c99, 0x80}, {0x5c9a, 0x80}, {0x5c9b, 0x80}, {0x5c9c, 0x40}, {0x5c9d, 0x00},
|
||||
{0x5c9e, 0x80}, {0x5c9f, 0x80}, {0x5ca0, 0x80}, {0x5ca1, 0x20}, {0x5ca2, 0x00},
|
||||
{0x5ca3, 0x80}, {0x5ca4, 0x80}, {0x5ca5, 0x00}, {0x5ca6, 0x00}, {0x5ca7, 0x00},
|
||||
|
||||
{0x5ca8, 0x01}, {0x5ca9, 0x00},
|
||||
{0x5caa, 0x02}, {0x5cab, 0x00},
|
||||
{0x5cac, 0x03}, {0x5cad, 0x08},
|
||||
|
||||
{0x5cae, 0x01}, {0x5caf, 0x00},
|
||||
{0x5cb0, 0x02}, {0x5cb1, 0x00},
|
||||
{0x5cb2, 0x03}, {0x5cb3, 0x08},
|
||||
|
||||
// combine ISP
|
||||
{0x5be7, 0x80},
|
||||
{0x5bc9, 0x80},
|
||||
{0x5bca, 0x80},
|
||||
{0x5bcb, 0x80},
|
||||
{0x5bcc, 0x80},
|
||||
{0x5bcd, 0x80},
|
||||
{0x5bce, 0x80},
|
||||
{0x5bcf, 0x80},
|
||||
{0x5bd0, 0x80},
|
||||
{0x5bd1, 0x80},
|
||||
{0x5bd2, 0x20},
|
||||
{0x5bd3, 0x80},
|
||||
{0x5bd4, 0x40},
|
||||
{0x5bd5, 0x20},
|
||||
{0x5bd6, 0x00},
|
||||
{0x5bd7, 0x00},
|
||||
{0x5bd8, 0x00},
|
||||
{0x5bd9, 0x00},
|
||||
{0x5bda, 0x00},
|
||||
{0x5bdb, 0x00},
|
||||
{0x5bdc, 0x00},
|
||||
{0x5bdd, 0x00},
|
||||
{0x5bde, 0x00},
|
||||
{0x5bdf, 0x00},
|
||||
{0x5be0, 0x00},
|
||||
{0x5be1, 0x00},
|
||||
{0x5be2, 0x00},
|
||||
{0x5be3, 0x00},
|
||||
{0x5be4, 0x00},
|
||||
{0x5be5, 0x00},
|
||||
{0x5be6, 0x00},
|
||||
|
||||
// m_nSPDCombineWeight
|
||||
{0x5c49, 0x80}, {0x5c4a, 0x80}, {0x5c4b, 0x80}, {0x5c4c, 0x80}, {0x5c4d, 0x40},
|
||||
{0x5c4e, 0x80}, {0x5c4f, 0x80}, {0x5c50, 0x80}, {0x5c51, 0x60}, {0x5c52, 0x20},
|
||||
{0x5c53, 0x80}, {0x5c54, 0x80}, {0x5c55, 0x80}, {0x5c56, 0x20}, {0x5c57, 0x00},
|
||||
{0x5c58, 0x80}, {0x5c59, 0x40}, {0x5c5a, 0x20}, {0x5c5b, 0x00}, {0x5c5c, 0x00},
|
||||
{0x5c5d, 0x80}, {0x5c5e, 0x00}, {0x5c5f, 0x00}, {0x5c60, 0x00}, {0x5c61, 0x00},
|
||||
{0x5c62, 0x00}, {0x5c63, 0x00}, {0x5c64, 0x00}, {0x5c65, 0x00}, {0x5c66, 0x00},
|
||||
|
||||
// m_nSPDCombineWeight
|
||||
{0x5cc9, 0x80}, {0x5cca, 0x80}, {0x5ccb, 0x80}, {0x5ccc, 0x80}, {0x5ccd, 0x80},
|
||||
{0x5cce, 0x80}, {0x5ccf, 0x80}, {0x5cd0, 0x80}, {0x5cd1, 0x80}, {0x5cd2, 0x60},
|
||||
{0x5cd3, 0x80}, {0x5cd4, 0x80}, {0x5cd5, 0x80}, {0x5cd6, 0x60}, {0x5cd7, 0x40},
|
||||
{0x5cd8, 0x80}, {0x5cd9, 0x80}, {0x5cda, 0x80}, {0x5cdb, 0x40}, {0x5cdc, 0x20},
|
||||
{0x5cdd, 0x80}, {0x5cde, 0x80}, {0x5cdf, 0x80}, {0x5ce0, 0x20}, {0x5ce1, 0x00},
|
||||
{0x5ce2, 0x80}, {0x5ce3, 0x80}, {0x5ce4, 0x80}, {0x5ce5, 0x00}, {0x5ce6, 0x00},
|
||||
|
||||
{0x5d74, 0x01},
|
||||
{0x5d75, 0x00},
|
||||
|
||||
{0x5d1f, 0x81},
|
||||
{0x5d11, 0x00},
|
||||
{0x5d12, 0x10},
|
||||
{0x5d13, 0x10},
|
||||
{0x5d15, 0x05},
|
||||
{0x5d16, 0x05},
|
||||
{0x5d17, 0x05},
|
||||
{0x5d08, 0x03},
|
||||
{0x5d09, 0xb6},
|
||||
{0x5d0a, 0x03},
|
||||
{0x5d0b, 0xb6},
|
||||
{0x5d18, 0x03},
|
||||
{0x5d19, 0xb6},
|
||||
{0x5d62, 0x01},
|
||||
{0x5d40, 0x02},
|
||||
{0x5d41, 0x01},
|
||||
{0x5d63, 0x1f},
|
||||
{0x5d64, 0x00},
|
||||
{0x5d65, 0x80},
|
||||
{0x5d56, 0x00},
|
||||
{0x5d57, 0x20},
|
||||
{0x5d58, 0x00},
|
||||
{0x5d59, 0x20},
|
||||
{0x5d5a, 0x00},
|
||||
{0x5d5b, 0x0c},
|
||||
{0x5d5c, 0x02},
|
||||
{0x5d5d, 0x40},
|
||||
{0x5d5e, 0x02},
|
||||
{0x5d5f, 0x40},
|
||||
{0x5d60, 0x03},
|
||||
{0x5d61, 0x40},
|
||||
{0x5d4a, 0x02},
|
||||
{0x5d4b, 0x40},
|
||||
{0x5d4c, 0x02},
|
||||
{0x5d4d, 0x40},
|
||||
{0x5d4e, 0x02},
|
||||
{0x5d4f, 0x40},
|
||||
{0x5d50, 0x18},
|
||||
{0x5d51, 0x80},
|
||||
{0x5d52, 0x18},
|
||||
{0x5d53, 0x80},
|
||||
{0x5d54, 0x18},
|
||||
{0x5d55, 0x80},
|
||||
{0x5d46, 0x20},
|
||||
{0x5d47, 0x00},
|
||||
{0x5d48, 0x22},
|
||||
{0x5d49, 0x00},
|
||||
{0x5d42, 0x20},
|
||||
{0x5d43, 0x00},
|
||||
{0x5d44, 0x22},
|
||||
{0x5d45, 0x00},
|
||||
|
||||
{0x5004, 0x1e},
|
||||
{0x4221, 0x03}, // this is changed from 1 -> 3
|
||||
|
||||
// DCG exposure coarse
|
||||
// {0x3501, 0x01}, {0x3502, 0xc8},
|
||||
// SPD exposure coarse
|
||||
// {0x3541, 0x01}, {0x3542, 0xc8},
|
||||
// VS exposure coarse
|
||||
// {0x35c1, 0x00}, {0x35c2, 0x01},
|
||||
|
||||
// crc reference
|
||||
{0x420e, 0x66}, {0x420f, 0x5d}, {0x4210, 0xa8}, {0x4211, 0x55},
|
||||
// crc stat check
|
||||
{0x507a, 0x5f}, {0x507b, 0x46},
|
||||
|
||||
// watchdog control
|
||||
{0x4f00, 0x00}, {0x4f01, 0x01}, {0x4f02, 0x80}, {0x4f04, 0x2c},
|
||||
|
||||
// color balance gains
|
||||
// blue
|
||||
{0x5280, 0x06}, {0x5281, 0xCB}, // hcg
|
||||
{0x5480, 0x06}, {0x5481, 0xCB}, // lcg
|
||||
{0x5680, 0x06}, {0x5681, 0xCB}, // spd
|
||||
{0x5880, 0x06}, {0x5881, 0xCB}, // vs
|
||||
|
||||
// green(blue)
|
||||
{0x5282, 0x04}, {0x5283, 0x00},
|
||||
{0x5482, 0x04}, {0x5483, 0x00},
|
||||
{0x5682, 0x04}, {0x5683, 0x00},
|
||||
{0x5882, 0x04}, {0x5883, 0x00},
|
||||
|
||||
// green(red)
|
||||
{0x5284, 0x04}, {0x5285, 0x00},
|
||||
{0x5484, 0x04}, {0x5485, 0x00},
|
||||
{0x5684, 0x04}, {0x5685, 0x00},
|
||||
{0x5884, 0x04}, {0x5885, 0x00},
|
||||
|
||||
// red
|
||||
{0x5286, 0x08}, {0x5287, 0xDE},
|
||||
{0x5486, 0x08}, {0x5487, 0xDE},
|
||||
{0x5686, 0x08}, {0x5687, 0xDE},
|
||||
{0x5886, 0x08}, {0x5887, 0xDE},
|
||||
|
||||
// fixed gains
|
||||
{0x3588, 0x01}, {0x3589, 0x00},
|
||||
{0x35c8, 0x01}, {0x35c9, 0x00},
|
||||
{0x3548, 0x0F}, {0x3549, 0x00},
|
||||
{0x35c1, 0x00},
|
||||
};
|
||||
116
iqpilot/system/camerad/sensors/sensor.h
Normal file
116
iqpilot/system/camerad/sensors/sensor.h
Normal file
@@ -0,0 +1,116 @@
|
||||
#pragma once
|
||||
|
||||
#include <cassert>
|
||||
#include <cstdint>
|
||||
#include <map>
|
||||
#include <utility>
|
||||
#include <vector>
|
||||
|
||||
#include "media/cam_isp.h"
|
||||
#include "media/cam_sensor.h"
|
||||
|
||||
#include "cereal/gen/cpp/log.capnp.h"
|
||||
#include "system/camerad/sensors/ar0231_registers.h"
|
||||
#include "system/camerad/sensors/ox03c10_registers.h"
|
||||
#include "system/camerad/sensors/os04c10_registers.h"
|
||||
|
||||
#define ANALOG_GAIN_MAX_CNT 55
|
||||
|
||||
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); }
|
||||
|
||||
cereal::FrameData::ImageSensor image_sensor = cereal::FrameData::ImageSensor::UNKNOWN;
|
||||
float pixel_size_mm;
|
||||
uint32_t frame_width, frame_height;
|
||||
uint32_t frame_stride;
|
||||
uint32_t frame_offset = 0;
|
||||
uint32_t extra_height = 0;
|
||||
int out_scale = 1;
|
||||
int registers_offset = -1;
|
||||
int stats_offset = -1;
|
||||
int hdr_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 ev_scale = 1.0;
|
||||
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;
|
||||
|
||||
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 bits_per_pixel;
|
||||
uint32_t bayer_pattern;
|
||||
uint32_t mipi_format;
|
||||
uint32_t mclk_frequency;
|
||||
uint32_t frame_data_type;
|
||||
|
||||
uint32_t readout_time_ns; // used to recover EOF from SOF
|
||||
|
||||
// ISP image processing params
|
||||
uint32_t black_level;
|
||||
std::vector<uint32_t> color_correct_matrix; // 3x3
|
||||
std::vector<uint32_t> gamma_lut_rgb; // gamma LUTs are length 64 * sizeof(uint32_t); same for r/g/b here
|
||||
void prepare_gamma_lut() {
|
||||
for (int i = 0; i < 64; i++) {
|
||||
gamma_lut_rgb[i] |= ((uint32_t)(gamma_lut_rgb[i+1] - gamma_lut_rgb[i]) << 10);
|
||||
}
|
||||
gamma_lut_rgb.pop_back();
|
||||
}
|
||||
std::vector<uint32_t> linearization_lut; // length 36
|
||||
std::vector<uint32_t> linearization_pts; // length 4
|
||||
std::vector<uint32_t> vignetting_lut; // length 221
|
||||
|
||||
const int num() const {
|
||||
return static_cast<int>(image_sensor);
|
||||
};
|
||||
};
|
||||
|
||||
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;
|
||||
|
||||
private:
|
||||
mutable std::map<uint16_t, std::pair<int, int>> ar0231_register_lut;
|
||||
};
|
||||
|
||||
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;
|
||||
};
|
||||
|
||||
class OS04C10 : public SensorInfo {
|
||||
public:
|
||||
OS04C10();
|
||||
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;
|
||||
};
|
||||
132
iqpilot/system/camerad/snapshot.py
Executable file
132
iqpilot/system/camerad/snapshot.py
Executable file
@@ -0,0 +1,132 @@
|
||||
#!/usr/bin/env python3
|
||||
import subprocess
|
||||
import time
|
||||
|
||||
import numpy as np
|
||||
from PIL import Image
|
||||
|
||||
import iqpilot.cereal.messaging as messaging
|
||||
from iqpilot.cereal.visionipc import VisionStreamType
|
||||
from msgq.visionipc import VisionIpcClient
|
||||
from iqpilot.common.params import Params
|
||||
from iqpilot.common.realtime import DT_MDL
|
||||
from iqpilot.system.hardware import PC
|
||||
from iqpilot.selfdrive.selfdrived.alertmanager import set_offroad_alert
|
||||
from iqpilot.system.manager.process_config import managed_processes
|
||||
|
||||
|
||||
VISION_STREAMS = {
|
||||
"roadCameraState": VisionStreamType.VISION_STREAM_ROAD,
|
||||
"driverCameraState": VisionStreamType.VISION_STREAM_DRIVER,
|
||||
"wideRoadCameraState": VisionStreamType.VISION_STREAM_WIDE_ROAD,
|
||||
}
|
||||
|
||||
|
||||
def jpeg_write(fn, dat):
|
||||
img = Image.fromarray(dat)
|
||||
img.save(fn, "JPEG")
|
||||
|
||||
|
||||
def yuv_to_rgb(y, u, v):
|
||||
ul = np.repeat(np.repeat(u, 2).reshape(u.shape[0], y.shape[1]), 2, axis=0).reshape(y.shape)
|
||||
vl = np.repeat(np.repeat(v, 2).reshape(v.shape[0], y.shape[1]), 2, axis=0).reshape(y.shape)
|
||||
|
||||
yuv = np.dstack((y, ul, vl)).astype(np.int16)
|
||||
yuv[:, :, 1:] -= 128
|
||||
|
||||
m = np.array([
|
||||
[1.00000, 1.00000, 1.00000],
|
||||
[0.00000, -0.39465, 2.03211],
|
||||
[1.13983, -0.58060, 0.00000],
|
||||
])
|
||||
rgb = np.dot(yuv, m).clip(0, 255)
|
||||
return rgb.astype(np.uint8)
|
||||
|
||||
|
||||
def extract_image(buf):
|
||||
# NV12 format: Y plane followed by interleaved UV plane
|
||||
# UV plane size is stride * uv_height, where uv_height = align(height/2, 16)
|
||||
uv_height = ((buf.height // 2) + 15) // 16 * 16
|
||||
uv_plane_size = buf.stride * uv_height
|
||||
|
||||
y = np.array(buf.data[:buf.uv_offset], dtype=np.uint8).reshape((-1, buf.stride))[:buf.height, :buf.width]
|
||||
uv_data = buf.data[buf.uv_offset:buf.uv_offset + uv_plane_size]
|
||||
u = np.array(uv_data[::2], dtype=np.uint8).reshape((-1, buf.stride//2))[:buf.height//2, :buf.width//2]
|
||||
v = np.array(uv_data[1::2], dtype=np.uint8).reshape((-1, buf.stride//2))[:buf.height//2, :buf.width//2]
|
||||
|
||||
return yuv_to_rgb(y, u, v)
|
||||
|
||||
|
||||
def get_snapshots(frame="roadCameraState", front_frame="driverCameraState"):
|
||||
sockets = [s for s in (frame, front_frame) if s is not None]
|
||||
sm = messaging.SubMaster(sockets)
|
||||
vipc_clients = {s: VisionIpcClient("camerad", VISION_STREAMS[s], True) for s in sockets}
|
||||
|
||||
# wait 4 sec from camerad startup for focus and exposure
|
||||
while sm[sockets[0]].frameId < int(4. / DT_MDL):
|
||||
sm.update()
|
||||
|
||||
for client in vipc_clients.values():
|
||||
client.connect(True)
|
||||
|
||||
# grab images
|
||||
rear, front = None, None
|
||||
if frame is not None:
|
||||
c = vipc_clients[frame]
|
||||
rear = extract_image(c.recv())
|
||||
if front_frame is not None:
|
||||
c = vipc_clients[front_frame]
|
||||
front = extract_image(c.recv())
|
||||
return rear, front
|
||||
|
||||
|
||||
def snapshot():
|
||||
params = Params()
|
||||
|
||||
if (not params.get_bool("IsOffroad")) or params.get_bool("IsTakingSnapshot"):
|
||||
print("Already taking snapshot")
|
||||
return None, None
|
||||
|
||||
front_camera_allowed = params.get_bool("RecordFront")
|
||||
params.put_bool("IsTakingSnapshot", True)
|
||||
set_offroad_alert("Offroad_IsTakingSnapshot", True)
|
||||
time.sleep(2.0) # Give hardwared time to read the param, or if just started give camerad time to start
|
||||
|
||||
# Check if camerad is already started
|
||||
try:
|
||||
subprocess.check_call(["pgrep", "camerad"])
|
||||
print("Camerad already running")
|
||||
params.put_bool("IsTakingSnapshot", False)
|
||||
params.remove("Offroad_IsTakingSnapshot")
|
||||
return None, None
|
||||
except subprocess.CalledProcessError:
|
||||
pass
|
||||
|
||||
try:
|
||||
# Allow testing on replay on PC
|
||||
if not PC:
|
||||
managed_processes['camerad'].start()
|
||||
|
||||
frame = "wideRoadCameraState"
|
||||
front_frame = "driverCameraState" if front_camera_allowed else None
|
||||
rear, front = get_snapshots(frame, front_frame)
|
||||
finally:
|
||||
managed_processes['camerad'].stop()
|
||||
params.put_bool("IsTakingSnapshot", False)
|
||||
set_offroad_alert("Offroad_IsTakingSnapshot", False)
|
||||
|
||||
if not front_camera_allowed:
|
||||
front = None
|
||||
|
||||
return rear, front
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
pic, fpic = snapshot()
|
||||
if pic is not None:
|
||||
print(pic.shape)
|
||||
jpeg_write("/tmp/back.jpg", pic)
|
||||
if fpic is not None:
|
||||
jpeg_write("/tmp/front.jpg", fpic)
|
||||
else:
|
||||
print("Error taking snapshot")
|
||||
2
iqpilot/system/camerad/test/.gitignore
vendored
Normal file
2
iqpilot/system/camerad/test/.gitignore
vendored
Normal file
@@ -0,0 +1,2 @@
|
||||
jpegs/
|
||||
test_ae_gray
|
||||
16
iqpilot/system/camerad/test/debug.sh
Executable file
16
iqpilot/system/camerad/test/debug.sh
Executable file
@@ -0,0 +1,16 @@
|
||||
#!/usr/bin/env bash
|
||||
set -e
|
||||
|
||||
#echo 4294967295 | sudo tee /sys/module/cam_debug_util/parameters/debug_mdl
|
||||
|
||||
# no CCI and UTIL, very spammy
|
||||
echo 0xfffdbfff | sudo tee /sys/module/cam_debug_util/parameters/debug_mdl
|
||||
#echo 0 | sudo tee /sys/module/cam_debug_util/parameters/debug_mdl
|
||||
|
||||
sudo dmesg -C
|
||||
scons -u -j8 --minimal .
|
||||
export DEBUG_FRAMES=1
|
||||
export DISABLE_ROAD=1 DISABLE_WIDE_ROAD=1
|
||||
#export DISABLE_DRIVER=1
|
||||
export LOGPRINT=debug
|
||||
./camerad
|
||||
13
iqpilot/system/camerad/test/icp_debug.sh
Executable file
13
iqpilot/system/camerad/test/icp_debug.sh
Executable file
@@ -0,0 +1,13 @@
|
||||
#!/usr/bin/env bash
|
||||
set -e
|
||||
|
||||
cd /sys/kernel/debug/tracing
|
||||
echo "" > trace
|
||||
echo 1 > tracing_on
|
||||
#echo Y > /sys/kernel/debug/camera_icp/a5_debug_q
|
||||
echo 0x1 > /sys/kernel/debug/camera_icp/a5_debug_type
|
||||
echo 1 > /sys/kernel/debug/tracing/events/camera/enable
|
||||
echo 0xffffffff > /sys/kernel/debug/camera_icp/a5_debug_lvl
|
||||
echo 1 > /sys/kernel/debug/tracing/events/camera/cam_icp_fw_dbg/enable
|
||||
|
||||
cat /sys/kernel/debug/tracing/trace_pipe
|
||||
2
iqpilot/system/camerad/test/intercept.sh
Executable file
2
iqpilot/system/camerad/test/intercept.sh
Executable file
@@ -0,0 +1,2 @@
|
||||
#!/usr/bin/env bash
|
||||
DISABLE_ROAD=1 DISABLE_WIDE_ROAD=1 DEBUG_FRAMES=1 LOGPRINT=debug LD_PRELOAD=/data/tici_test_scripts/isp/interceptor/tmpioctl.so ./camerad
|
||||
9
iqpilot/system/camerad/test/stress_restart.sh
Executable file
9
iqpilot/system/camerad/test/stress_restart.sh
Executable file
@@ -0,0 +1,9 @@
|
||||
#!/bin/sh
|
||||
cd ..
|
||||
while :; do
|
||||
./camerad &
|
||||
pid="$!"
|
||||
sleep 2
|
||||
kill -2 $pid
|
||||
wait $pid
|
||||
done
|
||||
84
iqpilot/system/camerad/test/test_ae_gray.cc
Normal file
84
iqpilot/system/camerad/test/test_ae_gray.cc
Normal file
@@ -0,0 +1,84 @@
|
||||
#define CATCH_CONFIG_MAIN
|
||||
#include "catch2/catch.hpp"
|
||||
|
||||
#include <cassert>
|
||||
|
||||
#include <cmath>
|
||||
#include <cstring>
|
||||
|
||||
#include "common/util.h"
|
||||
#include "system/camerad/cameras/camera_common.h"
|
||||
|
||||
#define W 240
|
||||
#define H 160
|
||||
|
||||
|
||||
#define TONE_SPLITS 3
|
||||
|
||||
float gts[TONE_SPLITS * TONE_SPLITS * TONE_SPLITS * TONE_SPLITS] = {
|
||||
0.917969, 0.917969, 0.375000, 0.917969, 0.375000, 0.375000, 0.187500, 0.187500, 0.187500, 0.917969,
|
||||
0.375000, 0.375000, 0.187500, 0.187500, 0.187500, 0.187500, 0.187500, 0.187500, 0.093750, 0.093750,
|
||||
0.093750, 0.093750, 0.093750, 0.093750, 0.093750, 0.093750, 0.093750, 0.917969, 0.375000, 0.375000,
|
||||
0.187500, 0.187500, 0.187500, 0.187500, 0.187500, 0.187500, 0.093750, 0.093750, 0.093750, 0.093750,
|
||||
0.093750, 0.093750, 0.093750, 0.093750, 0.093750, 0.093750, 0.093750, 0.093750, 0.093750, 0.093750,
|
||||
0.093750, 0.093750, 0.093750, 0.093750, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000,
|
||||
0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000,
|
||||
0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000,
|
||||
0.000000};
|
||||
|
||||
|
||||
TEST_CASE("camera.test_calculate_exposure_value") {
|
||||
// set up fake camerabuf
|
||||
CameraBuf cb = {};
|
||||
VisionBuf vb = {};
|
||||
uint8_t * fb_y = new uint8_t[W*H];
|
||||
vb.y = fb_y;
|
||||
cb.cur_yuv_buf = &vb;
|
||||
cb.out_img_width = W;
|
||||
cb.out_img_height = H;
|
||||
Rect rect = {0, 0, W-1, H-1};
|
||||
|
||||
printf("AE test patterns %dx%d\n", cb.out_img_width, cb.out_img_height);
|
||||
|
||||
// mix of 5 tones
|
||||
uint8_t l[5] = {0, 24, 48, 96, 235}; // 235 is yuv max
|
||||
|
||||
bool passed = true;
|
||||
float rtol = 0.05;
|
||||
// generate pattern and calculate EV
|
||||
int cnt = 0;
|
||||
for (int i_0=0; i_0<TONE_SPLITS; i_0++) {
|
||||
for (int i_1=0; i_1<TONE_SPLITS; i_1++) {
|
||||
for (int i_2=0; i_2<TONE_SPLITS; i_2++) {
|
||||
for (int i_3=0; i_3<TONE_SPLITS; i_3++) {
|
||||
int h_0 = i_0 * H / TONE_SPLITS;
|
||||
int h_1 = i_1 * (H - h_0) / TONE_SPLITS;
|
||||
int h_2 = i_2 * (H - h_0 - h_1) / TONE_SPLITS;
|
||||
int h_3 = i_3 * (H - h_0 - h_1 - h_2) / TONE_SPLITS;
|
||||
int h_4 = H - h_0 - h_1 - h_2 - h_3;
|
||||
memset(&fb_y[0], l[0], h_0*W);
|
||||
memset(&fb_y[h_0*W], l[1], h_1*W);
|
||||
memset(&fb_y[h_0*W+h_1*W], l[2], h_2*W);
|
||||
memset(&fb_y[h_0*W+h_1*W+h_2*W], l[3], h_3*W);
|
||||
memset(&fb_y[h_0*W+h_1*W+h_2*W+h_3*W], l[4], h_4*W);
|
||||
float ev = calculate_exposure_value((const CameraBuf*) &cb, rect, 1, 1);
|
||||
// printf("%d/%d/%d/%d/%d ev is %f\n", h_0, h_1, h_2, h_3, h_4, ev);
|
||||
// printf("%f\n", ev);
|
||||
|
||||
// compare to gt
|
||||
float evgt = gts[cnt];
|
||||
if (fabs(ev - evgt) > rtol*evgt) {
|
||||
passed = false;
|
||||
}
|
||||
|
||||
// report
|
||||
printf("%d/%d/%d/%d/%d: ev %f, gt %f, err %f\n", h_0, h_1, h_2, h_3, h_4, ev, evgt, fabs(ev - evgt) / (evgt != 0 ? evgt : 0.00001f));
|
||||
cnt++;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
assert(passed);
|
||||
|
||||
delete[] fb_y;
|
||||
}
|
||||
217
iqpilot/system/camerad/test/test_camerad.py
Normal file
217
iqpilot/system/camerad/test/test_camerad.py
Normal file
@@ -0,0 +1,217 @@
|
||||
import os
|
||||
import time
|
||||
|
||||
import pytest
|
||||
import numpy as np
|
||||
from msgq.visionipc import VisionIpcClient
|
||||
|
||||
import iqpilot.cereal.messaging as messaging
|
||||
from iqpilot.cereal.services import SERVICE_LIST
|
||||
from iqpilot.selfdrive.test.helpers import processes_context
|
||||
from iqpilot.system.camerad.snapshot import VISION_STREAMS
|
||||
from iqpilot.system.manager.process_config import managed_processes
|
||||
from iqpilot.tools.lib.logreader import msgs_to_time_series
|
||||
|
||||
TEST_TIMESPAN = 10
|
||||
CAMERAS = ('roadCameraState', 'driverCameraState', 'wideRoadCameraState')
|
||||
TEST_PATTERN_FRAMES = 200
|
||||
TEST_PATTERN_MIN_CONFIDENCE = 10
|
||||
TEST_PATTERN_CONNECT_TIMEOUT = 15
|
||||
TEST_PATTERN_CONFIGS = {
|
||||
'ox03c10': (41, 4),
|
||||
'os04c10': (97, 4),
|
||||
}
|
||||
|
||||
|
||||
def _pattern_sample(client):
|
||||
buf = client.recv(1000)
|
||||
if buf is None:
|
||||
return None
|
||||
|
||||
y = np.asarray(buf.data[:buf.uv_offset], dtype=np.uint8).reshape((-1, buf.stride))[:buf.height, :buf.width]
|
||||
profile = y[:, ::8].mean(axis=1)
|
||||
padded = np.pad(profile, (4, 4), mode='edge')
|
||||
neighbors = [padded[i:i + len(profile)] for i in range(9) if i != 4]
|
||||
residual = profile - np.median(neighbors, axis=0)
|
||||
position = int(np.argmax(residual))
|
||||
return client.frame_id, client.timestamp_sof, position, residual[position], buf.height
|
||||
|
||||
|
||||
def _test_pattern_session():
|
||||
samples = {camera: [] for camera in CAMERAS}
|
||||
sockets = {camera: messaging.sub_sock(camera, conflate=False, timeout=100) for camera in CAMERAS}
|
||||
logs = []
|
||||
with pytest.MonkeyPatch.context() as monkeypatch:
|
||||
monkeypatch.setenv('SPECTRA_TEST_PATTERN', '1')
|
||||
monkeypatch.setenv('SPECTRA_ERROR_PROB', '-1')
|
||||
with processes_context(['camerad']) as processes:
|
||||
clients = {camera: VisionIpcClient('camerad', VISION_STREAMS[camera], False) for camera in CAMERAS}
|
||||
pending = set(clients)
|
||||
deadline = time.monotonic() + TEST_PATTERN_CONNECT_TIMEOUT
|
||||
while pending and time.monotonic() < deadline:
|
||||
assert processes[0].proc is not None and processes[0].proc.exitcode is None
|
||||
pending = {camera for camera in pending if not clients[camera].connect(False)}
|
||||
if pending:
|
||||
time.sleep(0.1)
|
||||
assert not pending, f'VisionIPC connection timeout: {sorted(pending)}'
|
||||
|
||||
for _ in range(TEST_PATTERN_FRAMES):
|
||||
for camera, client in clients.items():
|
||||
sample = _pattern_sample(client)
|
||||
if sample is not None:
|
||||
samples[camera].append(sample)
|
||||
for sock in sockets.values():
|
||||
logs.extend(messaging.drain_sock(sock))
|
||||
|
||||
return msgs_to_time_series(logs), samples
|
||||
|
||||
|
||||
def run_and_log(procs, services, duration):
|
||||
logs = []
|
||||
|
||||
try:
|
||||
for p in procs:
|
||||
managed_processes[p].start()
|
||||
socks = [messaging.sub_sock(s, conflate=False, timeout=100) for s in services]
|
||||
|
||||
start_time = time.monotonic()
|
||||
while time.monotonic() - start_time < duration:
|
||||
for s in socks:
|
||||
logs.extend(messaging.drain_sock(s))
|
||||
for p in procs:
|
||||
assert managed_processes[p].proc.is_alive()
|
||||
finally:
|
||||
for p in procs:
|
||||
managed_processes[p].stop()
|
||||
|
||||
return logs
|
||||
|
||||
@pytest.fixture(scope="module")
|
||||
def logs():
|
||||
logs = run_and_log(["camerad", ], CAMERAS, TEST_TIMESPAN)
|
||||
ts = msgs_to_time_series(logs)
|
||||
|
||||
for cam in CAMERAS:
|
||||
expected_frames = SERVICE_LIST[cam].frequency * TEST_TIMESPAN
|
||||
cnt = len(ts[cam]['t'])
|
||||
assert expected_frames*0.8 < cnt < expected_frames*1.2, f"unexpected frame count {cam}: {expected_frames=}, got {cnt}"
|
||||
|
||||
dts = np.abs(np.diff([ts[cam]['timestampSof']/1e6]) - 1000/SERVICE_LIST[cam].frequency)
|
||||
assert (dts < 1.0).all(), f"{cam} dts(ms) out of spec: max diff {dts.max()}, 99 percentile {np.percentile(dts, 99)}"
|
||||
return ts
|
||||
|
||||
@pytest.mark.tici
|
||||
class TestCamerad:
|
||||
def test_frame_skips(self, logs):
|
||||
for c in CAMERAS:
|
||||
assert set(np.diff(logs[c]['frameId'])) == {1, }, f"{c} has frame skips"
|
||||
|
||||
def test_frame_sync(self, logs):
|
||||
n = range(len(logs['roadCameraState']['t'][:-10]))
|
||||
|
||||
frame_ids = {i: [logs[cam]['frameId'][i] for cam in CAMERAS] for i in n}
|
||||
assert all(len(set(v)) == 1 for v in frame_ids.values()), "frame IDs not aligned"
|
||||
|
||||
frame_times = {i: [logs[cam]['timestampSof'][i] for cam in CAMERAS] for i in n}
|
||||
diffs = {i: (max(ts) - min(ts))/1e6 for i, ts in frame_times.items()}
|
||||
|
||||
laggy_frames = {k: v for k, v in diffs.items() if v > 1.1}
|
||||
assert len(laggy_frames) == 0, f"Frames not synced properly: {laggy_frames=}"
|
||||
|
||||
def test_sanity_checks(self, logs):
|
||||
self._sanity_checks(logs)
|
||||
|
||||
def _sanity_checks(self, ts):
|
||||
for c in CAMERAS:
|
||||
assert c in ts
|
||||
assert len(ts[c]['t']) > 20
|
||||
|
||||
# not a valid request id
|
||||
assert 0 not in ts[c]['requestId']
|
||||
|
||||
# should monotonically increase
|
||||
assert np.all(np.diff(ts[c]['frameId']) >= 1)
|
||||
assert np.all(np.diff(ts[c]['requestId']) >= 1)
|
||||
|
||||
# EOF > SOF
|
||||
assert np.all((ts[c]['timestampEof'] - ts[c]['timestampSof']) > 0)
|
||||
|
||||
# logMonoTime > SOF
|
||||
assert np.all((ts[c]['t'] - ts[c]['timestampSof']/1e9) > 1e-7)
|
||||
|
||||
# logMonoTime > EOF, needs some tolerance since EOF is (SOF + readout time) but there is noise in the SOF timestamping (done via IRQ)
|
||||
assert np.mean((ts[c]['t'] - ts[c]['timestampEof']/1e9) > 1e-7) > 0.7 # should be mostly logMonoTime > EOF
|
||||
assert np.all((ts[c]['t'] - ts[c]['timestampEof']/1e9) > -0.10) # when EOF > logMonoTime, it should never be more than two frames
|
||||
|
||||
def test_stress_test(self):
|
||||
os.environ['SPECTRA_ERROR_PROB'] = '0.008'
|
||||
logs = run_and_log(["camerad", ], CAMERAS, 10)
|
||||
ts = msgs_to_time_series(logs)
|
||||
|
||||
# we should see some jumps from introduced errors
|
||||
assert np.max([ np.max(np.diff(ts[c]['frameId'])) for c in CAMERAS ]) > 1
|
||||
assert np.max([ np.max(np.diff(ts[c]['requestId'])) for c in CAMERAS ]) > 1
|
||||
|
||||
self._sanity_checks(ts)
|
||||
|
||||
|
||||
@pytest.fixture(scope="module")
|
||||
def test_pattern_data():
|
||||
return _test_pattern_session()
|
||||
|
||||
|
||||
@pytest.mark.tici
|
||||
@pytest.mark.xdist_group("camerad_test_pattern")
|
||||
class TestCameradTestPattern:
|
||||
def test_frame_delivery(self, test_pattern_data):
|
||||
logs, samples_by_camera = test_pattern_data
|
||||
for camera in CAMERAS:
|
||||
assert camera in logs
|
||||
samples = samples_by_camera[camera]
|
||||
assert len(samples) > TEST_PATTERN_FRAMES * 0.9
|
||||
|
||||
state_frame_ids = logs[camera]['frameId']
|
||||
state_request_ids = logs[camera]['requestId']
|
||||
vipc_frame_ids = np.array([sample[0] for sample in samples])
|
||||
for source, frame_ids in (('camera state', state_frame_ids), ('VisionIPC', vipc_frame_ids)):
|
||||
frame_steps = np.diff(frame_ids)
|
||||
skipped = frame_ids[1:][frame_steps != 1]
|
||||
assert len(skipped) == 0, f'{camera} {source} skipped frames before {skipped}'
|
||||
|
||||
expected_sof_step = 1e9 / SERVICE_LIST[camera].frequency
|
||||
sof_step_errors = np.diff(logs[camera]['timestampSof']) - expected_sof_step
|
||||
assert np.all(np.abs(sof_step_errors) < 1e6), f'{camera} SOF cadence errors: {sof_step_errors[np.abs(sof_step_errors) >= 1e6]}'
|
||||
|
||||
request_steps = np.diff(state_request_ids)
|
||||
skipped_requests = state_request_ids[1:][request_steps != 1]
|
||||
assert len(skipped_requests) == 0, f'{camera} skipped requests before {skipped_requests}'
|
||||
|
||||
state_sofs = dict(zip(state_frame_ids, logs[camera]['timestampSof'], strict=True))
|
||||
matched_samples = [sample for sample in samples if sample[0] in state_sofs]
|
||||
assert len(matched_samples) > len(samples) * 0.8
|
||||
mismatched_sofs = {
|
||||
frame_id: (timestamp_sof, state_sofs[frame_id]) for frame_id, timestamp_sof, *_ in matched_samples if timestamp_sof != state_sofs[frame_id]
|
||||
}
|
||||
assert not mismatched_sofs, f'{camera} VisionIPC/camera state SOFs disagree: {mismatched_sofs}'
|
||||
|
||||
def test_pattern(self, test_pattern_data):
|
||||
logs, samples_by_camera = test_pattern_data
|
||||
for camera in CAMERAS:
|
||||
sensors = set(logs[camera]['sensor'])
|
||||
assert len(sensors) == 1
|
||||
sensor = sensors.pop()
|
||||
assert sensor in TEST_PATTERN_CONFIGS, f'unsupported test pattern sensor: {sensor}'
|
||||
cycle_frames, position_tolerance = TEST_PATTERN_CONFIGS[sensor]
|
||||
|
||||
samples = samples_by_camera[camera]
|
||||
confident = [sample for sample in samples if sample[3] > TEST_PATTERN_MIN_CONFIDENCE]
|
||||
positions = np.array([sample[2] for sample in confident])
|
||||
assert len(confident) > len(samples) * 0.7, f'{camera} test pattern confidence too low'
|
||||
assert len(np.unique(positions)) > 20, f'{camera} test pattern is not moving'
|
||||
assert np.ptp(positions) > confident[0][4] * 0.75, f'{camera} test pattern does not span the frame'
|
||||
|
||||
samples_by_frame = {sample[0]: sample for sample in confident}
|
||||
repeating_pairs = [(sample, samples_by_frame[sample[0] + cycle_frames]) for sample in confident if sample[0] + cycle_frames in samples_by_frame]
|
||||
assert len(repeating_pairs) > 20
|
||||
unexpected = [(first[0], first[2], second[2]) for first, second in repeating_pairs if abs(second[2] - first[2]) > position_tolerance]
|
||||
assert len(unexpected) < len(repeating_pairs) * 0.3, f'{camera} test pattern cycle mismatches: {unexpected}'
|
||||
51
iqpilot/system/camerad/test/test_exposure.py
Normal file
51
iqpilot/system/camerad/test/test_exposure.py
Normal file
@@ -0,0 +1,51 @@
|
||||
import time
|
||||
import numpy as np
|
||||
import pytest
|
||||
|
||||
from iqpilot.selfdrive.test.helpers import with_processes
|
||||
from iqpilot.system.camerad.snapshot import get_snapshots
|
||||
|
||||
TEST_TIME = 45
|
||||
REPEAT = 5
|
||||
|
||||
@pytest.mark.tici
|
||||
class TestCamerad:
|
||||
@classmethod
|
||||
def setup_class(cls):
|
||||
pass
|
||||
|
||||
def _numpy_rgb2gray(self, im):
|
||||
ret = np.clip(im[:,:,2] * 0.114 + im[:,:,1] * 0.587 + im[:,:,0] * 0.299, 0, 255).astype(np.uint8)
|
||||
return ret
|
||||
|
||||
def _is_exposure_okay(self, i, med_mean=None):
|
||||
if med_mean is None:
|
||||
med_mean = np.array([[0.18,0.3],[0.18,0.3]])
|
||||
h, w = i.shape[:2]
|
||||
i = i[h//10:9*h//10,w//10:9*w//10]
|
||||
med_ex, mean_ex = med_mean
|
||||
i = self._numpy_rgb2gray(i)
|
||||
i_median = np.median(i) / 255.
|
||||
i_mean = np.mean(i) / 255.
|
||||
print([i_median, i_mean])
|
||||
return med_ex[0] < i_median < med_ex[1] and mean_ex[0] < i_mean < mean_ex[1]
|
||||
|
||||
@with_processes(['camerad'])
|
||||
def test_camera_operation(self):
|
||||
passed = 0
|
||||
start = time.monotonic()
|
||||
while time.monotonic() - start < TEST_TIME and passed < REPEAT:
|
||||
rpic, dpic = get_snapshots(frame="roadCameraState", front_frame="driverCameraState")
|
||||
wpic, _ = get_snapshots(frame="wideRoadCameraState")
|
||||
|
||||
res = self._is_exposure_okay(rpic)
|
||||
res = res and self._is_exposure_okay(dpic)
|
||||
res = res and self._is_exposure_okay(wpic)
|
||||
|
||||
if passed > 0 and not res:
|
||||
passed = -passed # fails test if any failure after first sus
|
||||
break
|
||||
|
||||
passed += int(res)
|
||||
time.sleep(2)
|
||||
assert passed >= REPEAT
|
||||
Reference in New Issue
Block a user