brianhansonxyz/clearpilot
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

wip

Browse Source
This commit is contained in:
Your Name
2024-04-27 03:22:06 -05:00
parent 4ffbd8982f
commit c95d41c62e
25 changed files with 3801 additions and 53 deletions
Download Patch File Download Diff File Expand all files Collapse all files

10
system/camerad/SConscript Normal file
View File

@@ -0,0 +1,10 @@
Import('env', 'arch', 'cereal', 'messaging', 'common', 'gpucommon', 'visionipc')
libs = ['m', 'pthread', common, 'jpeg', 'OpenCL', 'yuv', cereal, messaging, 'zmq', 'capnp', 'kj', visionipc, gpucommon, 'atomic']
camera_obj = env.Object(['cameras/camera_qcom2.cc', 'cameras/camera_common.cc', 'cameras/camera_util.cc',
'sensors/ar0231.cc', 'sensors/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
system/camerad/__init__.py Normal file
View File

BIN
system/camerad/camerad
View File

Binary file not shown.

355
system/camerad/cameras/camera_common.cc Normal file
View File

@@ -0,0 +1,355 @@
#include "system/camerad/cameras/camera_common.h"
#include <cassert>
#include <string>
#include "third_party/libyuv/include/libyuv.h"
#include <jpeglib.h>
#include "common/clutil.h"
#include "common/swaglog.h"
#include "common/util.h"
#include "third_party/linux/include/msm_media_info.h"
#include "system/camerad/cameras/camera_qcom2.h"
#ifdef QCOM2
#include "CL/cl_ext_qcom.h"
#endif
ExitHandler do_exit;
class Debayer {
public:
Debayer(cl_device_id device_id, cl_context context, const CameraBuf *b, const CameraState *s, int buf_width, int uv_offset) {
char args[4096];
const SensorInfo *ci = s->ci.get();
snprintf(args, sizeof(args),
"-cl-fast-relaxed-math -cl-denorms-are-zero "
"-DFRAME_WIDTH=%d -DFRAME_HEIGHT=%d -DFRAME_STRIDE=%d -DFRAME_OFFSET=%d "
"-DRGB_WIDTH=%d -DRGB_HEIGHT=%d -DYUV_STRIDE=%d -DUV_OFFSET=%d "
"-DIS_OX=%d -DIS_OS=%d -DIS_BGGR=%d -DCAM_NUM=%d%s",
ci->frame_width, ci->frame_height, ci->frame_stride, ci->frame_offset,
b->rgb_width, b->rgb_height, buf_width, uv_offset,
ci->image_sensor == cereal::FrameData::ImageSensor::OX03C10,
ci->image_sensor == cereal::FrameData::ImageSensor::OS04C10,
ci->image_sensor == cereal::FrameData::ImageSensor::OS04C10,
s->camera_num, s->camera_num==1 ? " -DVIGNETTING" : "");
const char *cl_file = "cameras/real_debayer.cl";
cl_program prg_debayer = cl_program_from_file(context, device_id, cl_file, args);
krnl_ = CL_CHECK_ERR(clCreateKernel(prg_debayer, "debayer10", &err));
CL_CHECK(clReleaseProgram(prg_debayer));
}
void queue(cl_command_queue q, cl_mem cam_buf_cl, cl_mem buf_cl, int width, int height, cl_event *debayer_event) {
CL_CHECK(clSetKernelArg(krnl_, 0, sizeof(cl_mem), &cam_buf_cl));
CL_CHECK(clSetKernelArg(krnl_, 1, sizeof(cl_mem), &buf_cl));
const size_t globalWorkSize[] = {size_t(width / 2), size_t(height / 2)};
const int debayer_local_worksize = 16;
const size_t localWorkSize[] = {debayer_local_worksize, debayer_local_worksize};
CL_CHECK(clEnqueueNDRangeKernel(q, krnl_, 2, NULL, globalWorkSize, localWorkSize, 0, 0, debayer_event));
}
~Debayer() {
CL_CHECK(clReleaseKernel(krnl_));
}
private:
cl_kernel krnl_;
};
void CameraBuf::init(cl_device_id device_id, cl_context context, CameraState *s, VisionIpcServer * v, int frame_cnt, VisionStreamType type) {
vipc_server = v;
stream_type = type;
frame_buf_count = frame_cnt;
const SensorInfo *ci = s->ci.get();
// RAW frame
const int frame_size = (ci->frame_height + ci->extra_height) * ci->frame_stride;
camera_bufs = std::make_unique<VisionBuf[]>(frame_buf_count);
camera_bufs_metadata = std::make_unique<FrameMetadata[]>(frame_buf_count);
for (int i = 0; i < frame_buf_count; i++) {
camera_bufs[i].allocate(frame_size);
camera_bufs[i].init_cl(device_id, context);
}
LOGD("allocated %d CL buffers", frame_buf_count);
rgb_width = ci->frame_width;
rgb_height = ci->frame_height;
int nv12_width = VENUS_Y_STRIDE(COLOR_FMT_NV12, rgb_width);
int nv12_height = VENUS_Y_SCANLINES(COLOR_FMT_NV12, rgb_height);
assert(nv12_width == VENUS_UV_STRIDE(COLOR_FMT_NV12, rgb_width));
assert(nv12_height/2 == VENUS_UV_SCANLINES(COLOR_FMT_NV12, rgb_height));
size_t nv12_uv_offset = nv12_width * nv12_height;
// the encoder HW tells us the size it wants after setting it up.
// TODO: VENUS_BUFFER_SIZE should give the size, but it's too small. dependent on encoder settings?
size_t nv12_size = (rgb_width >= 2688 ? 2900 : 2346)*nv12_width;
vipc_server->create_buffers_with_sizes(stream_type, YUV_BUFFER_COUNT, false, rgb_width, rgb_height, nv12_size, nv12_width, nv12_uv_offset);
LOGD("created %d YUV vipc buffers with size %dx%d", YUV_BUFFER_COUNT, nv12_width, nv12_height);
debayer = new Debayer(device_id, context, this, s, nv12_width, nv12_uv_offset);
const cl_queue_properties props[] = {0}; //CL_QUEUE_PRIORITY_KHR, CL_QUEUE_PRIORITY_HIGH_KHR, 0};
q = CL_CHECK_ERR(clCreateCommandQueueWithProperties(context, device_id, props, &err));
}
CameraBuf::~CameraBuf() {
for (int i = 0; i < frame_buf_count; i++) {
camera_bufs[i].free();
}
if (debayer) delete debayer;
if (q) CL_CHECK(clReleaseCommandQueue(q));
}
bool CameraBuf::acquire() {
if (!safe_queue.try_pop(cur_buf_idx, 50)) return false;
if (camera_bufs_metadata[cur_buf_idx].frame_id == -1) {
LOGE("no frame data? wtf");
return false;
}
cur_frame_data = camera_bufs_metadata[cur_buf_idx];
cur_yuv_buf = vipc_server->get_buffer(stream_type);
cur_camera_buf = &camera_bufs[cur_buf_idx];
double start_time = millis_since_boot();
cl_event event;
debayer->queue(q, camera_bufs[cur_buf_idx].buf_cl, cur_yuv_buf->buf_cl, rgb_width, rgb_height, &event);
clWaitForEvents(1, &event);
CL_CHECK(clReleaseEvent(event));
cur_frame_data.processing_time = (millis_since_boot() - start_time) / 1000.0;
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);
return true;
}
void CameraBuf::queue(size_t buf_idx) {
safe_queue.push(buf_idx);
}
// common functions
void fill_frame_data(cereal::FrameData::Builder &framed, const FrameMetadata &frame_data, CameraState *c) {
framed.setFrameId(frame_data.frame_id);
framed.setRequestId(frame_data.request_id);
framed.setTimestampEof(frame_data.timestamp_eof);
framed.setTimestampSof(frame_data.timestamp_sof);
framed.setIntegLines(frame_data.integ_lines);
framed.setGain(frame_data.gain);
framed.setHighConversionGain(frame_data.high_conversion_gain);
framed.setMeasuredGreyFraction(frame_data.measured_grey_fraction);
framed.setTargetGreyFraction(frame_data.target_grey_fraction);
framed.setProcessingTime(frame_data.processing_time);
const float ev = c->cur_ev[frame_data.frame_id % 3];
const float perc = util::map_val(ev, c->ci->min_ev, c->ci->max_ev, 0.0f, 100.0f);
framed.setExposureValPercent(perc);
framed.setSensor(c->ci->image_sensor);
}
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);
}
static kj::Array<capnp::byte> yuv420_to_jpeg(const CameraBuf *b, int thumbnail_width, int thumbnail_height) {
int downscale = b->cur_yuv_buf->width / thumbnail_width;
assert(downscale * thumbnail_height == b->cur_yuv_buf->height);
int in_stride = b->cur_yuv_buf->stride;
// make the buffer big enough. jpeg_write_raw_data requires 16-pixels aligned height to be used.
std::unique_ptr<uint8[]> buf(new uint8_t[(thumbnail_width * ((thumbnail_height + 15) & ~15) * 3) / 2]);
uint8_t *y_plane = buf.get();
uint8_t *u_plane = y_plane + thumbnail_width * thumbnail_height;
uint8_t *v_plane = u_plane + (thumbnail_width * thumbnail_height) / 4;
{
// subsampled conversion from nv12 to yuv
for (int hy = 0; hy < thumbnail_height/2; hy++) {
for (int hx = 0; hx < thumbnail_width/2; hx++) {
int ix = hx * downscale + (downscale-1)/2;
int iy = hy * downscale + (downscale-1)/2;
y_plane[(hy*2 + 0)*thumbnail_width + (hx*2 + 0)] = b->cur_yuv_buf->y[(iy*2 + 0) * in_stride + ix*2 + 0];
y_plane[(hy*2 + 0)*thumbnail_width + (hx*2 + 1)] = b->cur_yuv_buf->y[(iy*2 + 0) * in_stride + ix*2 + 1];
y_plane[(hy*2 + 1)*thumbnail_width + (hx*2 + 0)] = b->cur_yuv_buf->y[(iy*2 + 1) * in_stride + ix*2 + 0];
y_plane[(hy*2 + 1)*thumbnail_width + (hx*2 + 1)] = b->cur_yuv_buf->y[(iy*2 + 1) * in_stride + ix*2 + 1];
u_plane[hy*thumbnail_width/2 + hx] = b->cur_yuv_buf->uv[iy*in_stride + ix*2 + 0];
v_plane[hy*thumbnail_width/2 + hx] = b->cur_yuv_buf->uv[iy*in_stride + ix*2 + 1];
}
}
}
struct jpeg_compress_struct cinfo;
struct jpeg_error_mgr jerr;
cinfo.err = jpeg_std_error(&jerr);
jpeg_create_compress(&cinfo);
uint8_t *thumbnail_buffer = nullptr;
size_t thumbnail_len = 0;
jpeg_mem_dest(&cinfo, &thumbnail_buffer, &thumbnail_len);
cinfo.image_width = thumbnail_width;
cinfo.image_height = thumbnail_height;
cinfo.input_components = 3;
jpeg_set_defaults(&cinfo);
jpeg_set_colorspace(&cinfo, JCS_YCbCr);
// configure sampling factors for yuv420.
cinfo.comp_info[0].h_samp_factor = 2; // Y
cinfo.comp_info[0].v_samp_factor = 2;
cinfo.comp_info[1].h_samp_factor = 1; // U
cinfo.comp_info[1].v_samp_factor = 1;
cinfo.comp_info[2].h_samp_factor = 1; // V
cinfo.comp_info[2].v_samp_factor = 1;
cinfo.raw_data_in = TRUE;
jpeg_set_quality(&cinfo, 50, TRUE);
jpeg_start_compress(&cinfo, TRUE);
JSAMPROW y[16], u[8], v[8];
JSAMPARRAY planes[3]{y, u, v};
for (int line = 0; line < cinfo.image_height; line += 16) {
for (int i = 0; i < 16; ++i) {
y[i] = y_plane + (line + i) * cinfo.image_width;
if (i % 2 == 0) {
int offset = (cinfo.image_width / 2) * ((i + line) / 2);
u[i / 2] = u_plane + offset;
v[i / 2] = v_plane + offset;
}
}
jpeg_write_raw_data(&cinfo, planes, 16);
}
jpeg_finish_compress(&cinfo);
jpeg_destroy_compress(&cinfo);
kj::Array<capnp::byte> dat = kj::heapArray<capnp::byte>(thumbnail_buffer, thumbnail_len);
free(thumbnail_buffer);
return dat;
}
static void publish_thumbnail(PubMaster *pm, const CameraBuf *b) {
auto thumbnail = yuv420_to_jpeg(b, b->rgb_width / 4, b->rgb_height / 4);
if (thumbnail.size() == 0) return;
MessageBuilder msg;
auto thumbnaild = msg.initEvent().initThumbnail();
thumbnaild.setFrameId(b->cur_frame_data.frame_id);
thumbnaild.setTimestampEof(b->cur_frame_data.timestamp_eof);
thumbnaild.setThumbnail(thumbnail);
pm->send("thumbnail", msg);
}
float set_exposure_target(const CameraBuf *b, int x_start, int x_end, int x_skip, int y_start, int y_end, 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 = y_start; y < y_end; y += y_skip) {
for (int x = x_start; x < x_end; x += x_skip) {
uint8_t lum = pix_ptr[(y * b->rgb_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;
}
void *processing_thread(MultiCameraState *cameras, CameraState *cs, process_thread_cb callback) {
const char *thread_name = nullptr;
if (cs == &cameras->road_cam) {
thread_name = "RoadCamera";
} else if (cs == &cameras->driver_cam) {
thread_name = "DriverCamera";
} else {
thread_name = "WideRoadCamera";
}
util::set_thread_name(thread_name);
uint32_t cnt = 0;
while (!do_exit) {
if (!cs->buf.acquire()) continue;
callback(cameras, cs, cnt);
if (cs == &(cameras->road_cam) && cameras->pm && cnt % 100 == 3) {
// this takes 10ms???
publish_thumbnail(cameras->pm, &(cs->buf));
}
++cnt;
}
return NULL;
}
std::thread start_process_thread(MultiCameraState *cameras, CameraState *cs, process_thread_cb callback) {
return std::thread(processing_thread, cameras, cs, callback);
}
void camerad_thread() {
cl_device_id device_id = cl_get_device_id(CL_DEVICE_TYPE_DEFAULT);
#ifdef QCOM2
const cl_context_properties props[] = {CL_CONTEXT_PRIORITY_HINT_QCOM, CL_PRIORITY_HINT_HIGH_QCOM, 0};
cl_context context = CL_CHECK_ERR(clCreateContext(props, 1, &device_id, NULL, NULL, &err));
#else
cl_context context = CL_CHECK_ERR(clCreateContext(NULL, 1, &device_id, NULL, NULL, &err));
#endif
{
MultiCameraState cameras = {};
VisionIpcServer vipc_server("camerad", device_id, context);
cameras_open(&cameras);
cameras_init(&vipc_server, &cameras, device_id, context);
vipc_server.start_listener();
cameras_run(&cameras);
}
CL_CHECK(clReleaseContext(context));
}
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--;
}
}
}

87
system/camerad/cameras/camera_common.h Normal file
View File

@@ -0,0 +1,87 @@
#pragma once
#include <fcntl.h>
#include <memory>
#include <thread>
#include "cereal/messaging/messaging.h"
#include "cereal/visionipc/visionipc_server.h"
#include "common/queue.h"
const int YUV_BUFFER_COUNT = 20;
enum CameraType {
RoadCam = 0,
DriverCam,
WideRoadCam
};
// for debugging
const bool env_disable_road = getenv("DISABLE_ROAD") != NULL;
const bool env_disable_wide_road = getenv("DISABLE_WIDE_ROAD") != NULL;
const bool env_disable_driver = getenv("DISABLE_DRIVER") != NULL;
const bool env_debug_frames = getenv("DEBUG_FRAMES") != NULL;
const bool env_log_raw_frames = getenv("LOG_RAW_FRAMES") != NULL;
const bool env_ctrl_exp_from_params = getenv("CTRL_EXP_FROM_PARAMS") != NULL;
typedef struct FrameMetadata {
uint32_t frame_id;
uint32_t request_id;
// Timestamps
uint64_t timestamp_sof;
uint64_t timestamp_eof;
// Exposure
unsigned int integ_lines;
bool high_conversion_gain;
float gain;
float measured_grey_fraction;
float target_grey_fraction;
float processing_time;
} FrameMetadata;
struct MultiCameraState;
class CameraState;
class Debayer;
class CameraBuf {
private:
VisionIpcServer *vipc_server;
Debayer *debayer = nullptr;
VisionStreamType stream_type;
int cur_buf_idx;
SafeQueue<int> safe_queue;
int frame_buf_count;
public:
cl_command_queue q;
FrameMetadata cur_frame_data;
VisionBuf *cur_yuv_buf;
VisionBuf *cur_camera_buf;
std::unique_ptr<VisionBuf[]> camera_bufs;
std::unique_ptr<FrameMetadata[]> camera_bufs_metadata;
int rgb_width, rgb_height;
CameraBuf() = default;
~CameraBuf();
void init(cl_device_id device_id, cl_context context, CameraState *s, VisionIpcServer * v, int frame_cnt, VisionStreamType type);
bool acquire();
void queue(size_t buf_idx);
};
typedef void (*process_thread_cb)(MultiCameraState *s, CameraState *c, int cnt);
void fill_frame_data(cereal::FrameData::Builder &framed, const FrameMetadata &frame_data, CameraState *c);
kj::Array<uint8_t> get_raw_frame_image(const CameraBuf *b);
float set_exposure_target(const CameraBuf *b, int x_start, int x_end, int x_skip, int y_start, int y_end, int y_skip);
std::thread start_process_thread(MultiCameraState *cameras, CameraState *cs, process_thread_cb callback);
void cameras_init(VisionIpcServer *v, MultiCameraState *s, cl_device_id device_id, cl_context ctx);
void cameras_open(MultiCameraState *s);
void cameras_run(MultiCameraState *s);
void cameras_close(MultiCameraState *s);
void camerad_thread();
int open_v4l_by_name_and_index(const char name[], int index = 0, int flags = O_RDWR | O_NONBLOCK);

1017
system/camerad/cameras/camera_qcom2.cc Normal file
View File

File diff suppressed because it is too large Load Diff

98
system/camerad/cameras/camera_qcom2.h Normal file
View File

@@ -0,0 +1,98 @@
#pragma once
#include <memory>
#include <utility>
#include "system/camerad/cameras/camera_common.h"
#include "system/camerad/cameras/camera_util.h"
#include "system/camerad/sensors/sensor.h"
#include "common/params.h"
#include "common/util.h"
#define FRAME_BUF_COUNT 4
class CameraState {
public:
MultiCameraState *multi_cam_state;
std::unique_ptr<const SensorInfo> ci;
bool enabled;
std::mutex exp_lock;
int exposure_time;
bool dc_gain_enabled;
int dc_gain_weight;
int gain_idx;
float analog_gain_frac;
float cur_ev[3];
float best_ev_score;
int new_exp_g;
int new_exp_t;
float measured_grey_fraction;
float target_grey_fraction;
unique_fd sensor_fd;
unique_fd csiphy_fd;
int camera_num;
void handle_camera_event(void *evdat);
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 sensors_start();
void camera_open(MultiCameraState *multi_cam_state, int camera_num, bool enabled);
void sensor_set_parameters();
void camera_map_bufs(MultiCameraState *s);
void camera_init(MultiCameraState *s, VisionIpcServer *v, cl_device_id device_id, cl_context ctx, VisionStreamType yuv_type);
void camera_close();
int32_t session_handle;
int32_t sensor_dev_handle;
int32_t isp_dev_handle;
int32_t csiphy_dev_handle;
int32_t link_handle;
int buf0_handle;
int buf_handle[FRAME_BUF_COUNT];
int sync_objs[FRAME_BUF_COUNT];
int request_ids[FRAME_BUF_COUNT];
int request_id_last;
int frame_id_last;
int idx_offset;
bool skipped;
CameraBuf buf;
MemoryManager mm;
void config_isp(int io_mem_handle, int fence, int request_id, int buf0_mem_handle, int buf0_offset);
void enqueue_req_multi(int start, int n, bool dp);
void enqueue_buffer(int i, bool dp);
int clear_req_queue();
int sensors_init();
void sensors_poke(int request_id);
void sensors_i2c(const struct i2c_random_wr_payload* dat, int len, int op_code, bool data_word);
private:
// for debugging
Params params;
};
typedef struct MultiCameraState {
unique_fd video0_fd;
unique_fd cam_sync_fd;
unique_fd isp_fd;
int device_iommu;
int cdm_iommu;
CameraState road_cam;
CameraState wide_road_cam;
CameraState driver_cam;
PubMaster *pm;
} MultiCameraState;

138
system/camerad/cameras/camera_util.cc Normal file
View File

@@ -0,0 +1,138 @@
#include "system/camerad/cameras/camera_util.h"
#include <string.h>
#include <cassert>
#include <sys/ioctl.h>
#include <sys/mman.h>
#include "common/swaglog.h"
#include "common/util.h"
// ************** low level camera helpers ****************
int do_cam_control(int fd, int op_code, void *handle, int size) {
struct cam_control camcontrol = {0};
camcontrol.op_code = op_code;
camcontrol.handle = (uint64_t)handle;
if (size == 0) {
camcontrol.size = 8;
camcontrol.handle_type = CAM_HANDLE_MEM_HANDLE;
} else {
camcontrol.size = size;
camcontrol.handle_type = CAM_HANDLE_USER_POINTER;
}
int ret = HANDLE_EINTR(ioctl(fd, VIDIOC_CAM_CONTROL, &camcontrol));
if (ret == -1) {
LOGE("VIDIOC_CAM_CONTROL error: op_code %d - errno %d", op_code, errno);
}
return ret;
}
std::optional<int32_t> device_acquire(int fd, int32_t session_handle, void *data, uint32_t num_resources) {
struct cam_acquire_dev_cmd cmd = {
.session_handle = session_handle,
.handle_type = CAM_HANDLE_USER_POINTER,
.num_resources = (uint32_t)(data ? num_resources : 0),
.resource_hdl = (uint64_t)data,
};
int err = do_cam_control(fd, CAM_ACQUIRE_DEV, &cmd, sizeof(cmd));
return err == 0 ? std::make_optional(cmd.dev_handle) : std::nullopt;
}
int device_config(int fd, int32_t session_handle, int32_t dev_handle, uint64_t packet_handle) {
struct cam_config_dev_cmd cmd = {
.session_handle = session_handle,
.dev_handle = dev_handle,
.packet_handle = packet_handle,
};
return do_cam_control(fd, CAM_CONFIG_DEV, &cmd, sizeof(cmd));
}
int device_control(int fd, int op_code, int session_handle, int dev_handle) {
// start stop and release are all the same
struct cam_start_stop_dev_cmd cmd { .session_handle = session_handle, .dev_handle = dev_handle };
return do_cam_control(fd, op_code, &cmd, sizeof(cmd));
}
void *alloc_w_mmu_hdl(int video0_fd, int len, uint32_t *handle, int align, int flags, int mmu_hdl, int mmu_hdl2) {
struct cam_mem_mgr_alloc_cmd mem_mgr_alloc_cmd = {0};
mem_mgr_alloc_cmd.len = len;
mem_mgr_alloc_cmd.align = align;
mem_mgr_alloc_cmd.flags = flags;
mem_mgr_alloc_cmd.num_hdl = 0;
if (mmu_hdl != 0) {
mem_mgr_alloc_cmd.mmu_hdls[0] = mmu_hdl;
mem_mgr_alloc_cmd.num_hdl++;
}
if (mmu_hdl2 != 0) {
mem_mgr_alloc_cmd.mmu_hdls[1] = mmu_hdl2;
mem_mgr_alloc_cmd.num_hdl++;
}
do_cam_control(video0_fd, CAM_REQ_MGR_ALLOC_BUF, &mem_mgr_alloc_cmd, sizeof(mem_mgr_alloc_cmd));
*handle = mem_mgr_alloc_cmd.out.buf_handle;
void *ptr = NULL;
if (mem_mgr_alloc_cmd.out.fd > 0) {
ptr = mmap(NULL, len, PROT_READ | PROT_WRITE, MAP_SHARED, mem_mgr_alloc_cmd.out.fd, 0);
assert(ptr != MAP_FAILED);
}
// LOGD("allocated: %x %d %llx mapped %p", mem_mgr_alloc_cmd.out.buf_handle, mem_mgr_alloc_cmd.out.fd, mem_mgr_alloc_cmd.out.vaddr, ptr);
return ptr;
}
void release(int video0_fd, uint32_t handle) {
int ret;
struct cam_mem_mgr_release_cmd mem_mgr_release_cmd = {0};
mem_mgr_release_cmd.buf_handle = handle;
ret = do_cam_control(video0_fd, CAM_REQ_MGR_RELEASE_BUF, &mem_mgr_release_cmd, sizeof(mem_mgr_release_cmd));
assert(ret == 0);
}
void release_fd(int video0_fd, uint32_t handle) {
// handle to fd
close(handle>>16);
release(video0_fd, handle);
}
void *MemoryManager::alloc_buf(int size, uint32_t *handle) {
lock.lock();
void *ptr;
if (!cached_allocations[size].empty()) {
ptr = cached_allocations[size].front();
cached_allocations[size].pop();
*handle = handle_lookup[ptr];
} else {
ptr = alloc_w_mmu_hdl(video0_fd, size, handle);
handle_lookup[ptr] = *handle;
size_lookup[ptr] = size;
}
lock.unlock();
memset(ptr, 0, size);
return ptr;
}
void MemoryManager::free(void *ptr) {
lock.lock();
cached_allocations[size_lookup[ptr]].push(ptr);
lock.unlock();
}
MemoryManager::~MemoryManager() {
for (auto& x : cached_allocations) {
while (!x.second.empty()) {
void *ptr = x.second.front();
x.second.pop();
LOGD("freeing cached allocation %p with size %d", ptr, size_lookup[ptr]);
munmap(ptr, size_lookup[ptr]);
release_fd(video0_fd, handle_lookup[ptr]);
handle_lookup.erase(ptr);
size_lookup.erase(ptr);
}
}
}

39
system/camerad/cameras/camera_util.h Normal file
View File

@@ -0,0 +1,39 @@
#pragma once
#include <functional>
#include <map>
#include <memory>
#include <mutex>
#include <optional>
#include <queue>
#include <media/cam_req_mgr.h>
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::mutex lock;
std::map<void *, uint32_t> handle_lookup;
std::map<void *, int> size_lookup;
std::map<int, std::queue<void *> > cached_allocations;
int video0_fd;
};

169
system/camerad/cameras/real_debayer.cl
View File

@@ -8,7 +8,7 @@
float3 color_correct(float3 rgb) { float3 color_correct(float3 rgb) {
// color correction // color correction
#if IS_OX #if IS_OX | IS_OS
float3 x = rgb.x * (float3)(1.5664815 , -0.29808738, -0.03973474); float3 x = rgb.x * (float3)(1.5664815 , -0.29808738, -0.03973474);
x += rgb.y * (float3)(-0.48672447, 1.41914433, -0.40295248); x += rgb.y * (float3)(-0.48672447, 1.41914433, -0.40295248);
x += rgb.z * (float3)(-0.07975703, -0.12105695, 1.44268722); x += rgb.z * (float3)(-0.07975703, -0.12105695, 1.44268722);
@@ -20,6 +20,8 @@ float3 color_correct(float3 rgb) {
#if IS_OX #if IS_OX
return -0.507089*exp(-12.54124638*x)+0.9655*powr(x,0.5)-0.472597*x+0.507089; return -0.507089*exp(-12.54124638*x)+0.9655*powr(x,0.5)-0.472597*x+0.507089;
#elif IS_OS
return powr(x,0.7);
#else #else
// tone mapping params // tone mapping params
const float gamma_k = 0.75; const float gamma_k = 0.75;
@@ -35,6 +37,9 @@ float3 color_correct(float3 rgb) {
} }
float get_vignetting_s(float r) { float get_vignetting_s(float r) {
#if IS_OS
r = r / 2.2545f;
#endif
if (r < 62500) { if (r < 62500) {
return (1.0f + 0.0000008f*r); return (1.0f + 0.0000008f*r);
} else if (r < 490000) { } else if (r < 490000) {
@@ -85,6 +90,24 @@ float4 val4_from_12(uchar8 pvs, float gain) {
} }
float4 val4_from_10(uchar8 pvs, uchar ext, bool aligned, float gain) {
uint4 parsed;
if (aligned) {
parsed = (uint4)(((uint)pvs.s0 << 2) + (pvs.s1 & 0b00000011),
((uint)pvs.s2 << 2) + ((pvs.s6 & 0b11000000) / 64),
((uint)pvs.s3 << 2) + ((pvs.s6 & 0b00110000) / 16),
((uint)pvs.s4 << 2) + ((pvs.s6 & 0b00001100) / 4));
} else {
parsed = (uint4)(((uint)pvs.s0 << 2) + ((pvs.s3 & 0b00110000) / 16),
((uint)pvs.s1 << 2) + ((pvs.s3 & 0b00001100) / 4),
((uint)pvs.s2 << 2) + ((pvs.s3 & 0b00000011)),
((uint)pvs.s4 << 2) + ((ext & 0b11000000) / 64));
}
float4 pv = convert_float4(parsed) / 1024.0;
return clamp(pv*gain, 0.0, 1.0);
}
float get_k(float a, float b, float c, float d) { float get_k(float a, float b, float c, float d) {
return 2.0 - (fabs(a - b) + fabs(c - d)); return 2.0 - (fabs(a - b) + fabs(c - d));
} }
@@ -94,20 +117,51 @@ __kernel void debayer10(const __global uchar * in, __global uchar * out)
const int gid_x = get_global_id(0); const int gid_x = get_global_id(0);
const int gid_y = get_global_id(1); const int gid_y = get_global_id(1);
const int y_top_mod = (gid_y == 0) ? 2: 0; const int row_before_offset = (gid_y == 0) ? 2 : 0;
const int y_bot_mod = (gid_y == (RGB_HEIGHT/2 - 1)) ? 1: 3; const int row_after_offset = (gid_y == (RGB_HEIGHT/2 - 1)) ? 1 : 3;
float3 rgb; float3 rgb;
uchar3 rgb_out[4]; uchar3 rgb_out[4];
int start = (2 * gid_y - 1) * FRAME_STRIDE + (3 * gid_x - 2) + (FRAME_STRIDE * FRAME_OFFSET); #if IS_BGGR
constant int row_read_order[] = {3, 2, 1, 0};
constant int rgb_write_order[] = {2, 3, 0, 1};
#else
constant int row_read_order[] = {0, 1, 2, 3};
constant int rgb_write_order[] = {0, 1, 2, 3};
#endif
int start_idx;
#if IS_10BIT
bool aligned10;
if (gid_x % 2 == 0) {
aligned10 = true;
start_idx = (2 * gid_y - 1) * FRAME_STRIDE + (5 * gid_x / 2 - 2) + (FRAME_STRIDE * FRAME_OFFSET);
} else {
aligned10 = false;
start_idx = (2 * gid_y - 1) * FRAME_STRIDE + (5 * (gid_x - 1) / 2 + 1) + (FRAME_STRIDE * FRAME_OFFSET);
}
#else
start_idx = (2 * gid_y - 1) * FRAME_STRIDE + (3 * gid_x - 2) + (FRAME_STRIDE * FRAME_OFFSET);
#endif
// read in 8x4 chars // read in 8x4 chars
uchar8 dat[4]; uchar8 dat[4];
dat[0] = vload8(0, in + start + FRAME_STRIDE*y_top_mod); dat[0] = vload8(0, in + start_idx + FRAME_STRIDE*row_before_offset);
dat[1] = vload8(0, in + start + FRAME_STRIDE*1); dat[1] = vload8(0, in + start_idx + FRAME_STRIDE*1);
dat[2] = vload8(0, in + start + FRAME_STRIDE*2); dat[2] = vload8(0, in + start_idx + FRAME_STRIDE*2);
dat[3] = vload8(0, in + start + FRAME_STRIDE*y_bot_mod); dat[3] = vload8(0, in + start_idx + FRAME_STRIDE*row_after_offset);
// need extra bit for 10-bit
#if IS_10BIT
uchar extra[4];
if (!aligned10) {
extra[0] = in[start_idx + FRAME_STRIDE*row_before_offset + 8];
extra[1] = in[start_idx + FRAME_STRIDE*1 + 8];
extra[2] = in[start_idx + FRAME_STRIDE*2 + 8];
extra[3] = in[start_idx + FRAME_STRIDE*row_after_offset + 8];
}
#endif
// correct vignetting // correct vignetting
#if VIGNETTING #if VIGNETTING
@@ -118,60 +172,69 @@ __kernel void debayer10(const __global uchar * in, __global uchar * out)
const float gain = 1.0; const float gain = 1.0;
#endif #endif
// process them to floats float4 v_rows[4];
float4 va = val4_from_12(dat[0], gain); // parse into floats
float4 vb = val4_from_12(dat[1], gain); #if IS_10BIT
float4 vc = val4_from_12(dat[2], gain); v_rows[row_read_order[0]] = val4_from_10(dat[0], extra[0], aligned10, 1.0);
float4 vd = val4_from_12(dat[3], gain); v_rows[row_read_order[1]] = val4_from_10(dat[1], extra[1], aligned10, 1.0);
v_rows[row_read_order[2]] = val4_from_10(dat[2], extra[2], aligned10, 1.0);
v_rows[row_read_order[3]] = val4_from_10(dat[3], extra[3], aligned10, 1.0);
#else
v_rows[row_read_order[0]] = val4_from_12(dat[0], gain);
v_rows[row_read_order[1]] = val4_from_12(dat[1], gain);
v_rows[row_read_order[2]] = val4_from_12(dat[2], gain);
v_rows[row_read_order[3]] = val4_from_12(dat[3], gain);
#endif
// mirror padding
if (gid_x == 0) { if (gid_x == 0) {
va.s0 = va.s2; v_rows[0].s0 = v_rows[0].s2;
vb.s0 = vb.s2; v_rows[1].s0 = v_rows[1].s2;
vc.s0 = vc.s2; v_rows[2].s0 = v_rows[2].s2;
vd.s0 = vd.s2; v_rows[3].s0 = v_rows[3].s2;
} else if (gid_x == RGB_WIDTH/2 - 1) { } else if (gid_x == RGB_WIDTH/2 - 1) {
va.s3 = va.s1; v_rows[0].s3 = v_rows[0].s1;
vb.s3 = vb.s1; v_rows[1].s3 = v_rows[1].s1;
vc.s3 = vc.s1; v_rows[2].s3 = v_rows[2].s1;
vd.s3 = vd.s1; v_rows[3].s3 = v_rows[3].s1;
} }
// a simplified version of https://opensignalprocessingjournal.com/contents/volumes/V6/TOSIGPJ-6-1/TOSIGPJ-6-1.pdf // a simplified version of https://opensignalprocessingjournal.com/contents/volumes/V6/TOSIGPJ-6-1/TOSIGPJ-6-1.pdf
const float k01 = get_k(va.s0, vb.s1, va.s2, vb.s1); const float k01 = get_k(v_rows[0].s0, v_rows[1].s1, v_rows[0].s2, v_rows[1].s1);
const float k02 = get_k(va.s2, vb.s1, vc.s2, vb.s1); const float k02 = get_k(v_rows[0].s2, v_rows[1].s1, v_rows[2].s2, v_rows[1].s1);
const float k03 = get_k(vc.s0, vb.s1, vc.s2, vb.s1); const float k03 = get_k(v_rows[2].s0, v_rows[1].s1, v_rows[2].s2, v_rows[1].s1);
const float k04 = get_k(va.s0, vb.s1, vc.s0, vb.s1); const float k04 = get_k(v_rows[0].s0, v_rows[1].s1, v_rows[2].s0, v_rows[1].s1);
rgb.x = (k02*vb.s2+k04*vb.s0)/(k02+k04); // R_G1 rgb.x = (k02*v_rows[1].s2+k04*v_rows[1].s0)/(k02+k04); // R_G1
rgb.y = vb.s1; // G1(R) rgb.y = v_rows[1].s1; // G1(R)
rgb.z = (k01*va.s1+k03*vc.s1)/(k01+k03); // B_G1 rgb.z = (k01*v_rows[0].s1+k03*v_rows[2].s1)/(k01+k03); // B_G1
rgb_out[0] = convert_uchar3_sat(color_correct(clamp(rgb, 0.0, 1.0)) * 255.0); rgb_out[rgb_write_order[0]] = convert_uchar3_sat(color_correct(clamp(rgb, 0.0, 1.0)) * 255.0);
const float k11 = get_k(va.s1, vc.s1, va.s3, vc.s3); const float k11 = get_k(v_rows[0].s1, v_rows[2].s1, v_rows[0].s3, v_rows[2].s3);
const float k12 = get_k(va.s2, vb.s1, vb.s3, vc.s2); const float k12 = get_k(v_rows[0].s2, v_rows[1].s1, v_rows[1].s3, v_rows[2].s2);
const float k13 = get_k(va.s1, va.s3, vc.s1, vc.s3); const float k13 = get_k(v_rows[0].s1, v_rows[0].s3, v_rows[2].s1, v_rows[2].s3);
const float k14 = get_k(va.s2, vb.s3, vc.s2, vb.s1); const float k14 = get_k(v_rows[0].s2, v_rows[1].s3, v_rows[2].s2, v_rows[1].s1);
rgb.x = vb.s2; // R rgb.x = v_rows[1].s2; // R
rgb.y = (k11*(va.s2+vc.s2)*0.5+k13*(vb.s3+vb.s1)*0.5)/(k11+k13); // G_R rgb.y = (k11*(v_rows[0].s2+v_rows[2].s2)*0.5+k13*(v_rows[1].s3+v_rows[1].s1)*0.5)/(k11+k13); // G_R
rgb.z = (k12*(va.s3+vc.s1)*0.5+k14*(va.s1+vc.s3)*0.5)/(k12+k14); // B_R rgb.z = (k12*(v_rows[0].s3+v_rows[2].s1)*0.5+k14*(v_rows[0].s1+v_rows[2].s3)*0.5)/(k12+k14); // B_R
rgb_out[1] = convert_uchar3_sat(color_correct(clamp(rgb, 0.0, 1.0)) * 255.0); rgb_out[rgb_write_order[1]] = convert_uchar3_sat(color_correct(clamp(rgb, 0.0, 1.0)) * 255.0);
const float k21 = get_k(vb.s0, vd.s0, vb.s2, vd.s2); const float k21 = get_k(v_rows[1].s0, v_rows[3].s0, v_rows[1].s2, v_rows[3].s2);
const float k22 = get_k(vb.s1, vc.s0, vc.s2, vd.s1); const float k22 = get_k(v_rows[1].s1, v_rows[2].s0, v_rows[2].s2, v_rows[3].s1);
const float k23 = get_k(vb.s0, vb.s2, vd.s0, vd.s2); const float k23 = get_k(v_rows[1].s0, v_rows[1].s2, v_rows[3].s0, v_rows[3].s2);
const float k24 = get_k(vb.s1, vc.s2, vd.s1, vc.s0); const float k24 = get_k(v_rows[1].s1, v_rows[2].s2, v_rows[3].s1, v_rows[2].s0);
rgb.x = (k22*(vb.s2+vd.s0)*0.5+k24*(vb.s0+vd.s2)*0.5)/(k22+k24); // R_B rgb.x = (k22*(v_rows[1].s2+v_rows[3].s0)*0.5+k24*(v_rows[1].s0+v_rows[3].s2)*0.5)/(k22+k24); // R_B
rgb.y = (k21*(vb.s1+vd.s1)*0.5+k23*(vc.s2+vc.s0)*0.5)/(k21+k23); // G_B rgb.y = (k21*(v_rows[1].s1+v_rows[3].s1)*0.5+k23*(v_rows[2].s2+v_rows[2].s0)*0.5)/(k21+k23); // G_B
rgb.z = vc.s1; // B rgb.z = v_rows[2].s1; // B
rgb_out[2] = convert_uchar3_sat(color_correct(clamp(rgb, 0.0, 1.0)) * 255.0); rgb_out[rgb_write_order[2]] = convert_uchar3_sat(color_correct(clamp(rgb, 0.0, 1.0)) * 255.0);
const float k31 = get_k(vb.s1, vc.s2, vb.s3, vc.s2); const float k31 = get_k(v_rows[1].s1, v_rows[2].s2, v_rows[1].s3, v_rows[2].s2);
const float k32 = get_k(vb.s3, vc.s2, vd.s3, vc.s2); const float k32 = get_k(v_rows[1].s3, v_rows[2].s2, v_rows[3].s3, v_rows[2].s2);
const float k33 = get_k(vd.s1, vc.s2, vd.s3, vc.s2); const float k33 = get_k(v_rows[3].s1, v_rows[2].s2, v_rows[3].s3, v_rows[2].s2);
const float k34 = get_k(vb.s1, vc.s2, vd.s1, vc.s2); const float k34 = get_k(v_rows[1].s1, v_rows[2].s2, v_rows[3].s1, v_rows[2].s2);
rgb.x = (k31*vb.s2+k33*vd.s2)/(k31+k33); // R_G2 rgb.x = (k31*v_rows[1].s2+k33*v_rows[3].s2)/(k31+k33); // R_G2
rgb.y = vc.s2; // G2(B) rgb.y = v_rows[2].s2; // G2(B)
rgb.z = (k32*vc.s3+k34*vc.s1)/(k32+k34); // B_G2 rgb.z = (k32*v_rows[2].s3+k34*v_rows[2].s1)/(k32+k34); // B_G2
rgb_out[3] = convert_uchar3_sat(color_correct(clamp(rgb, 0.0, 1.0)) * 255.0); rgb_out[rgb_write_order[3]] = convert_uchar3_sat(color_correct(clamp(rgb, 0.0, 1.0)) * 255.0);
// write ys // write ys
uchar2 yy = (uchar2)( uchar2 yy = (uchar2)(

23
system/camerad/main.cc Normal file
View File

@@ -0,0 +1,23 @@
#include "system/camerad/cameras/camera_common.h"
#include <cassert>
#include "common/params.h"
#include "common/util.h"
#include "system/hardware/hw.h"
int main(int argc, char *argv[]) {
if (Hardware::PC()) {
printf("exiting, camerad is not meant to run on PC\n");
return 0;
}
int ret;
ret = util::set_realtime_priority(53);
assert(ret == 0);
ret = util::set_core_affinity({6});
assert(ret == 0 || Params().getBool("IsOffroad")); // failure ok while offroad due to offlining cores
camerad_thread();
return 0;
}

171
system/camerad/sensors/ar0231.cc Normal file
View File

@@ -0,0 +1,171 @@
#include <cassert>
#include "common/swaglog.h"
#include "system/camerad/cameras/camera_common.h"
#include "system/camerad/cameras/camera_qcom2.h"
#include "system/camerad/sensors/sensor.h"
namespace {
const size_t AR0231_REGISTERS_HEIGHT = 2;
// TODO: this extra height is universal and doesn't apply per camera
const size_t AR0231_STATS_HEIGHT = 2 + 8;
const float 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
std::map<uint16_t, std::pair<int, int>> ar0231_build_register_lut(CameraState *c, uint8_t *data) {
// This function builds a lookup table from register address, to a pair of indices in the
// buffer where to read this address. The buffer contains padding bytes,
// as well as markers to indicate the type of the next byte.
//
// 0xAA is used to indicate the MSB of the address, 0xA5 for the LSB of the address.
// Every byte of data (MSB and LSB) is preceded by 0x5A. Specifying an address is optional
// for contiguous ranges. See page 27-29 of the AR0231 Developer guide for more information.
int max_i[] = {1828 / 2 * 3, 1500 / 2 * 3};
auto get_next_idx = [](int cur_idx) {
return (cur_idx % 3 == 1) ? cur_idx + 2 : cur_idx + 1; // Every third byte is padding
};
std::map<uint16_t, std::pair<int, int>> registers;
for (int register_row = 0; register_row < 2; register_row++) {
uint8_t *registers_raw = data + c->ci->frame_stride * register_row;
assert(registers_raw[0] == 0x0a); // Start of line
int value_tag_count = 0;
int first_val_idx = 0;
uint16_t cur_addr = 0;
for (int i = 1; i <= max_i[register_row]; i = get_next_idx(get_next_idx(i))) {
int val_idx = get_next_idx(i);
uint8_t tag = registers_raw[i];
uint16_t val = registers_raw[val_idx];
if (tag == 0xAA) { // Register MSB tag
cur_addr = val << 8;
} else if (tag == 0xA5) { // Register LSB tag
cur_addr |= val;
cur_addr -= 2; // Next value tag will increment address again
} else if (tag == 0x5A) { // Value tag
// First tag
if (value_tag_count % 2 == 0) {
cur_addr += 2;
first_val_idx = val_idx;
} else {
registers[cur_addr] = std::make_pair(first_val_idx + c->ci->frame_stride * register_row, val_idx + c->ci->frame_stride * register_row);
}
value_tag_count++;
}
}
}
return registers;
}
float ar0231_parse_temp_sensor(uint16_t calib1, uint16_t calib2, uint16_t data_reg) {
// See AR0231 Developer Guide - page 36
float slope = (125.0 - 55.0) / ((float)calib1 - (float)calib2);
float t0 = 55.0 - slope * (float)calib2;
return t0 + slope * (float)data_reg;
}
} // namespace
AR0231::AR0231() {
image_sensor = cereal::FrameData::ImageSensor::AR0231;
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;
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
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;
}
void AR0231::processRegisters(CameraState *c, cereal::FrameData::Builder &framed) const {
const uint8_t expected_preamble[] = {0x0a, 0xaa, 0x55, 0x20, 0xa5, 0x55};
uint8_t *data = (uint8_t *)c->buf.cur_camera_buf->addr + c->ci->registers_offset;
if (memcmp(data, expected_preamble, std::size(expected_preamble)) != 0) {
LOGE("unexpected register data found");
return;
}
if (ar0231_register_lut.empty()) {
ar0231_register_lut = ar0231_build_register_lut(c, data);
}
std::map<uint16_t, uint16_t> registers;
for (uint16_t addr : {0x2000, 0x2002, 0x20b0, 0x20b2, 0x30c6, 0x30c8, 0x30ca, 0x30cc}) {
auto offset = ar0231_register_lut[addr];
registers[addr] = ((uint16_t)data[offset.first] << 8) | data[offset.second];
}
uint32_t frame_id = ((uint32_t)registers[0x2000] << 16) | registers[0x2002];
framed.setFrameIdSensor(frame_id);
float temp_0 = ar0231_parse_temp_sensor(registers[0x30c6], registers[0x30c8], registers[0x20b0]);
float temp_1 = ar0231_parse_temp_sensor(registers[0x30ca], registers[0x30cc], registers[0x20b2]);
framed.setTemperaturesC({temp_0, temp_1});
}
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;
}

119
system/camerad/sensors/ar0231_registers.h Normal file
View File

@@ -0,0 +1,119 @@
#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
// 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},
};

88
system/camerad/sensors/os04c10.cc Normal file
View File

@@ -0,0 +1,88 @@
#include "system/camerad/sensors/sensor.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;
data_word = false;
frame_width = 2688;
frame_height = 1520;
frame_stride = (frame_width * 12 / 8); // no alignment
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));
probe_reg_addr = 0x300a;
probe_expected_data = 0x5304;
mipi_format = CAM_FORMAT_MIPI_RAW_12;
frame_data_type = 0x2c;
mclk_frequency = 24000000; // Hz
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; // 1x
exposure_time_max = 2200;
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_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;
}
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];
// uint32_t short_time = long_time > exposure_time_min*8 ? long_time / 8 : exposure_time_min;
return {
{0x3501, long_time>>8}, {0x3502, long_time&0xFF},
// {0x3511, short_time>>8}, {0x3512, short_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) * 5.0;
return score;
}

313
system/camerad/sensors/os04c10_registers.h Normal file
View File

@@ -0,0 +1,313 @@
#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},
// PLL
{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},
// ?
{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, 0x40},
{0x3699, 0x80},
{0x369a, 0x18},
{0x369b, 0x1f},
{0x369c, 0x14},
{0x369d, 0x80},
{0x369e, 0x40},
{0x369f, 0x21},
{0x36a0, 0x12},
{0x36a1, 0x5d},
{0x36a2, 0x66},
{0x370a, 0x02},
{0x370e, 0x0c},
{0x3710, 0x00},
{0x3713, 0x00},
{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, 0x08},
{0x37c4, 0x11},
{0x37c5, 0x80},
{0x37c6, 0x14},
{0x37c7, 0x08},
{0x37da, 0x11},
{0x381f, 0x08},
{0x3829, 0x03},
{0x3881, 0x00},
{0x3888, 0x04},
{0x388b, 0x00},
{0x3c80, 0x10},
{0x3c86, 0x00},
{0x3c8c, 0x20},
{0x3c9f, 0x01},
{0x3d85, 0x1b},
{0x3d8c, 0x71},
{0x3d8d, 0xe2},
{0x3f00, 0x0b},
{0x3f06, 0x04},
// BLC
{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
{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, 0xa6},
{0x3682, 0x53},
{0x3683, 0x2a},
{0x3684, 0x15},
{0x3700, 0x2a},
{0x3701, 0x12},
{0x3703, 0x28},
{0x3704, 0x0e},
{0x3706, 0x9d},
{0x3709, 0x4a},
{0x370b, 0x48},
{0x370c, 0x01},
{0x370f, 0x04},
{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},
// 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
{0x380e, 0x09}, {0x380f, 0x38}, // VTS
{0x3820, 0xb0},
{0x3821, 0x00},
{0x3880, 0x25},
{0x3882, 0x20},
{0x3c91, 0x0b},
{0x3c94, 0x45},
{0x3cad, 0x00},
{0x3cae, 0x00},
{0x4000, 0xf3},
{0x4001, 0x60},
{0x4003, 0x80},
{0x4300, 0xff},
{0x4302, 0x0f},
{0x4305, 0x83},
{0x4505, 0x84},
{0x4809, 0x0e},
{0x480a, 0x04},
{0x4837, 0x15},
{0x4c00, 0x08},
{0x4c01, 0x08},
{0x4c04, 0x00},
{0x4c05, 0x00},
{0x5000, 0xf9},
{0x3822, 0x14},
// initialize exposure
{0x3503, 0x88},
// long
{0x3500, 0x00}, {0x3501, 0x00}, {0x3502, 0x80},
{0x3508, 0x00}, {0x3509, 0x80},
{0x350a, 0x04}, {0x350b, 0x00},
// short
// {0x3510, 0x00}, {0x3511, 0x00}, {0x3512, 0x10},
// {0x350c, 0x00}, {0x350d, 0x80},
// {0x350e, 0x04}, {0x350f, 0x00},
// wb
{0x5100, 0x06}, {0x5101, 0xcb},
{0x5102, 0x04}, {0x5103, 0x00},
{0x5104, 0x08}, {0x5105, 0xde},
{0x5106, 0x02}, {0x5107, 0x00},
};

94
system/camerad/sensors/ox03c10.cc Normal file
View File

@@ -0,0 +1,94 @@
#include "system/camerad/sensors/sensor.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;
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));
probe_reg_addr = 0x300a;
probe_expected_data = 0x5803;
mipi_format = CAM_FORMAT_MIPI_RAW_12;
frame_data_type = 0x2c; // one is 0x2a, two are 0x2b
mclk_frequency = 24000000; //Hz
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;
}
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;
}

761
system/camerad/sensors/ox03c10_registers.h Normal file
View File

@@ -0,0 +1,761 @@
#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},*/
// 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 first frame
/*
{0x3009, 0x2},
{0x3015, 0x2},
{0x3822, 0x20},
{0x3823, 0x58},
{0x3826, 0x0}, {0x3827, 0x8},
{0x3881, 0x4},
{0x3882, 0x8}, {0x3883, 0x0D},
{0x3836, 0x1F}, {0x3837, 0x40},
*/
// FSIN with external pulses
{0x3009, 0x2},
{0x3015, 0x2},
{0x383E, 0x80},
{0x3881, 0x4},
{0x3882, 0x8}, {0x3883, 0x0D},
{0x3836, 0x1F}, {0x3837, 0x40},
{0x3892, 0x44},
{0x3823, 0x48},
{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},
};

89
system/camerad/sensors/sensor.h Normal file
View File

@@ -0,0 +1,89 @@
#pragma once
#include <cassert>
#include <cstdint>
#include <map>
#include <utility>
#include <vector>
#include "media/cam_sensor.h"
#include "system/camerad/cameras/camera_common.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); }
virtual void processRegisters(CameraState *c, cereal::FrameData::Builder &framed) const {}
cereal::FrameData::ImageSensor image_sensor = cereal::FrameData::ImageSensor::UNKNOWN;
uint32_t frame_width, frame_height;
uint32_t frame_stride;
uint32_t frame_offset = 0;
uint32_t extra_height = 0;
int registers_offset = -1;
int stats_offset = -1;
int exposure_time_min;
int exposure_time_max;
float dc_gain_factor;
int dc_gain_min_weight;
int dc_gain_max_weight;
float dc_gain_on_grey;
float dc_gain_off_grey;
float sensor_analog_gains[ANALOG_GAIN_MAX_CNT];
int analog_gain_min_idx;
int analog_gain_max_idx;
int analog_gain_rec_idx;
int analog_gain_cost_delta;
float analog_gain_cost_low;
float analog_gain_cost_high;
float target_grey_factor;
float min_ev;
float max_ev;
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 mipi_format;
uint32_t mclk_frequency;
uint32_t frame_data_type;
};
class AR0231 : public SensorInfo {
public:
AR0231();
std::vector<i2c_random_wr_payload> getExposureRegisters(int exposure_time, int new_exp_g, bool dc_gain_enabled) const override;
float getExposureScore(float desired_ev, int exp_t, int exp_g_idx, float exp_gain, int gain_idx) const override;
int getSlaveAddress(int port) const override;
void processRegisters(CameraState *c, cereal::FrameData::Builder &framed) 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;
};

2
system/camerad/test/.gitignore vendored Normal file
View File

@@ -0,0 +1,2 @@
jpegs/
test_ae_gray

27
system/camerad/test/check_skips.py Normal file
View File

@@ -0,0 +1,27 @@
#!/usr/bin/env python3
# type: ignore
import cereal.messaging as messaging
all_sockets = ['roadCameraState', 'driverCameraState', 'wideRoadCameraState']
prev_id = [None,None,None]
this_id = [None,None,None]
dt = [None,None,None]
num_skipped = [0,0,0]
if __name__ == "__main__":
sm = messaging.SubMaster(all_sockets)
while True:
sm.update()
for i in range(len(all_sockets)):
if not sm.updated[all_sockets[i]]:
continue
this_id[i] = sm[all_sockets[i]].frameId
if prev_id[i] is None:
prev_id[i] = this_id[i]
continue
dt[i] = this_id[i] - prev_id[i]
if dt[i] != 1:
num_skipped[i] += dt[i] - 1
print(all_sockets[i] ,dt[i] - 1, num_skipped[i])
prev_id[i] = this_id[i]

24
system/camerad/test/get_thumbnails_for_segment.py Normal file
View File

@@ -0,0 +1,24 @@
#!/usr/bin/env python3
import argparse
import os
from tqdm import tqdm
from openpilot.tools.lib.logreader import LogReader
if __name__ == "__main__":
parser = argparse.ArgumentParser()
parser.add_argument("route", help="The route name")
args = parser.parse_args()
out_path = os.path.join("jpegs", f"{args.route.replace('|', '_').replace('/', '_')}")
os.makedirs(out_path, exist_ok=True)
lr = LogReader(args.route)
for msg in tqdm(lr):
if msg.which() == 'thumbnail':
with open(os.path.join(out_path, f"{msg.thumbnail.frameId}.jpg"), 'wb') as f:
f.write(msg.thumbnail.thumbnail)
elif msg.which() == 'navThumbnail':
with open(os.path.join(out_path, f"nav_{msg.navThumbnail.frameId}.jpg"), 'wb') as f:
f.write(msg.navThumbnail.thumbnail)

9
system/camerad/test/stress_restart.sh Normal file
View File

@@ -0,0 +1,9 @@
#!/bin/sh
cd ..
while :; do
./camerad &
pid="$!"
sleep 2
kill -2 $pid
wait $pid
done

83
system/camerad/test/test_ae_gray.cc Normal file
View File

@@ -0,0 +1,83 @@
#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_set_exposure_target") {
// 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.rgb_width = W;
cb.rgb_height = H;
printf("AE test patterns %dx%d\n", cb.rgb_width, cb.rgb_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 = set_exposure_target((const CameraBuf*) &cb, 0, W-1, 1, 0, H-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;
}

83
system/camerad/test/test_camerad.py Normal file
View File

@@ -0,0 +1,83 @@
#!/usr/bin/env python3
import pytest
import time
import numpy as np
from flaky import flaky
from collections import defaultdict
import cereal.messaging as messaging
from cereal import log
from cereal.services import SERVICE_LIST
from openpilot.selfdrive.manager.process_config import managed_processes
TEST_TIMESPAN = 30
LAG_FRAME_TOLERANCE = {log.FrameData.ImageSensor.ar0231: 0.5, # ARs use synced pulses for frame starts
log.FrameData.ImageSensor.ox03c10: 1.1} # OXs react to out-of-sync at next frame
FRAME_DELTA_TOLERANCE = {log.FrameData.ImageSensor.ar0231: 1.0,
log.FrameData.ImageSensor.ox03c10: 1.0}
CAMERAS = ('roadCameraState', 'driverCameraState', 'wideRoadCameraState')
# TODO: this shouldn't be needed
@flaky(max_runs=3)
@pytest.mark.tici
class TestCamerad:
def setup_method(self):
# run camerad and record logs
managed_processes['camerad'].start()
time.sleep(3)
socks = {c: messaging.sub_sock(c, conflate=False, timeout=100) for c in CAMERAS}
self.logs = defaultdict(list)
start_time = time.monotonic()
while time.monotonic()- start_time < TEST_TIMESPAN:
for cam, s in socks.items():
self.logs[cam] += messaging.drain_sock(s)
time.sleep(0.2)
managed_processes['camerad'].stop()
self.log_by_frame_id = defaultdict(list)
self.sensor_type = None
for cam, msgs in self.logs.items():
if self.sensor_type is None:
self.sensor_type = getattr(msgs[0], msgs[0].which()).sensor.raw
expected_frames = SERVICE_LIST[cam].frequency * TEST_TIMESPAN
assert expected_frames*0.95 < len(msgs) < expected_frames*1.05, f"unexpected frame count {cam}: {expected_frames=}, got {len(msgs)}"
dts = np.abs(np.diff([getattr(m, m.which()).timestampSof/1e6 for m in msgs]) - 1000/SERVICE_LIST[cam].frequency)
assert (dts < FRAME_DELTA_TOLERANCE[self.sensor_type]).all(), f"{cam} dts(ms) out of spec: max diff {dts.max()}, 99 percentile {np.percentile(dts, 99)}"
for m in msgs:
self.log_by_frame_id[getattr(m, m.which()).frameId].append(m)
# strip beginning and end
for _ in range(3):
mn, mx = min(self.log_by_frame_id.keys()), max(self.log_by_frame_id.keys())
del self.log_by_frame_id[mn]
del self.log_by_frame_id[mx]
def test_frame_skips(self):
skips = {}
frame_ids = self.log_by_frame_id.keys()
for frame_id in range(min(frame_ids), max(frame_ids)):
seen_cams = [msg.which() for msg in self.log_by_frame_id[frame_id]]
skip_cams = set(CAMERAS) - set(seen_cams)
if len(skip_cams):
skips[frame_id] = skip_cams
assert len(skips) == 0, f"Found frame skips, missing cameras for the following frames: {skips}"
def test_frame_sync(self):
frame_times = {frame_id: [getattr(m, m.which()).timestampSof for m in msgs] for frame_id, msgs in self.log_by_frame_id.items()}
diffs = {frame_id: (max(ts) - min(ts))/1e6 for frame_id, ts in frame_times.items()}
def get_desc(fid, diff):
cam_times = [(m.which(), getattr(m, m.which()).timestampSof/1e6) for m in self.log_by_frame_id[fid]]
return (diff, cam_times)
laggy_frames = {k: get_desc(k, v) for k, v in diffs.items() if v > LAG_FRAME_TOLERANCE[self.sensor_type]}
def in_tol(diff):
return 50 - LAG_FRAME_TOLERANCE[self.sensor_type] < diff and diff < 50 + LAG_FRAME_TOLERANCE[self.sensor_type]
if len(laggy_frames) != 0 and all( in_tol(laggy_frames[lf][0]) for lf in laggy_frames):
print("TODO: handle camera out of sync")
else:
assert len(laggy_frames) == 0, f"Frames not synced properly: {laggy_frames=}"

55
system/camerad/test/test_exposure.py Normal file
View File

@@ -0,0 +1,55 @@
#!/usr/bin/env python3
import time
import unittest
import numpy as np
from openpilot.selfdrive.test.helpers import with_processes, phone_only
from openpilot.system.camerad.snapshot.snapshot import get_snapshots
TEST_TIME = 45
REPEAT = 5
class TestCamerad(unittest.TestCase):
@classmethod
def setUpClass(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.2,0.4],[0.2,0.6]])
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]
@phone_only
@with_processes(['camerad'])
def test_camera_operation(self):
passed = 0
start = time.time()
while time.time() - 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)
self.assertGreaterEqual(passed, REPEAT)
if __name__ == "__main__":
unittest.main()