mawei/sunnypilot
1
0
Fork 0
mirror of https://github.com/sunnypilot/sunnypilot.git synced 2026-09-30 17:03:41 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge remote-tracking branch 'openpilot-comma/master' into master-sync-op

Browse Source 
# Conflicts:
#	.gitmodules
#	README.md
#	cereal
#	common/version.h
#	docs/CARS.md
#	launch_chffrplus.sh
#	opendbc
#	panda
#	selfdrive/car/honda/values.py
#	selfdrive/car/hyundai/interface.py
#	selfdrive/car/hyundai/values.py
#	selfdrive/car/torque_data/override.yaml
#	selfdrive/car/torque_data/params.yaml
#	selfdrive/controls/controlsd.py
#	selfdrive/controls/lib/lateral_planner.py
#	selfdrive/controls/lib/longitudinal_planner.py
#	selfdrive/controls/plannerd.py
#	selfdrive/debug/cycle_alerts.py
#	selfdrive/manager/manager.py
#	selfdrive/monitoring/dmonitoringd.py
#	selfdrive/thermald/power_monitoring.py
#	selfdrive/ui/qt/util.cc
#	system/loggerd/uploader.py
This commit is contained in:
DevTekVE
2024-01-25 17:01:21 +01:00
parent c09b651bd7 be15b31696
commit 7f86fef7c6
713 changed files with 21027 additions and 18073 deletions
Download Patch File Download Diff File Expand all files Collapse all files
system/camerad/SConscript
+4 -8
View File
@@ -2,13 +2,9 @@ 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'])
env.Program('camerad', [
'main.cc',
camera_obj,
], LIBS=libs)
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/ae_gray_test',
['test/ae_gray_test.cc', camera_obj],
LIBS=libs)
env.Program('test/test_ae_gray', ['test/test_ae_gray.cc', camera_obj], LIBS=libs)
system/camerad/cameras/camera_common.cc
+12 -29
View File
@@ -1,22 +1,14 @@
#include "system/camerad/cameras/camera_common.h"
#include <unistd.h>
#include <cassert>
#include <cstdio>
#include <chrono>
#include <string>
#include <thread>
#include "third_party/libyuv/include/libyuv.h"
#include <jpeglib.h>
#include "system/camerad/imgproc/utils.h"
#include "common/clutil.h"
#include "common/modeldata.h"
#include "common/swaglog.h"
#include "common/util.h"
#include "system/hardware/hw.h"
#include "third_party/linux/include/msm_media_info.h"
#include "system/camerad/cameras/camera_qcom2.h"
@@ -30,15 +22,15 @@ 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 CameraInfo *ci = &s->ci;
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 -DRGB_STRIDE=%d -DYUV_STRIDE=%d -DUV_OFFSET=%d "
"-DRGB_WIDTH=%d -DRGB_HEIGHT=%d -DYUV_STRIDE=%d -DUV_OFFSET=%d "
"-DIS_OX=%d -DCAM_NUM=%d%s",
ci->frame_width, ci->frame_height, ci->frame_stride, ci->frame_offset,
b->rgb_width, b->rgb_height, b->rgb_stride, buf_width, uv_offset,
s->camera_id==CAMERA_ID_OX03C10 ? 1 : 0, s->camera_num, s->camera_num==1 ? " -DVIGNETTING" : "");
b->rgb_width, b->rgb_height, buf_width, uv_offset,
ci->image_sensor == cereal::FrameData::ImageSensor::OX03C10, 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));
@@ -63,12 +55,12 @@ private:
cl_kernel krnl_;
};
void CameraBuf::init(cl_device_id device_id, cl_context context, CameraState *s, VisionIpcServer * v, int frame_cnt, VisionStreamType init_yuv_type) {
void CameraBuf::init(cl_device_id device_id, cl_context context, CameraState *s, VisionIpcServer * v, int frame_cnt, VisionStreamType type) {
vipc_server = v;
this->yuv_type = init_yuv_type;
stream_type = type;
frame_buf_count = frame_cnt;
const CameraInfo *ci = &s->ci;
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);
@@ -89,17 +81,13 @@ void CameraBuf::init(cl_device_id device_id, cl_context context, CameraState *s,
assert(nv12_height/2 == VENUS_UV_SCANLINES(COLOR_FMT_NV12, rgb_height));
size_t nv12_size = 2346 * nv12_width; // comes from v4l2_format.fmt.pix_mp.plane_fmt[0].sizeimage
size_t nv12_uv_offset = nv12_width * nv12_height;
vipc_server->create_buffers_with_sizes(yuv_type, YUV_BUFFER_COUNT, false, rgb_width, rgb_height, nv12_size, nv12_width, nv12_uv_offset);
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);
#ifdef __APPLE__
q = CL_CHECK_ERR(clCreateCommandQueue(context, device_id, 0, &err));
#else
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));
#endif
}
CameraBuf::~CameraBuf() {
@@ -119,7 +107,7 @@ bool CameraBuf::acquire() {
}
cur_frame_data = camera_bufs_metadata[cur_buf_idx];
cur_yuv_buf = vipc_server->get_buffer(yuv_type);
cur_yuv_buf = vipc_server->get_buffer(stream_type);
cur_camera_buf = &camera_bufs[cur_buf_idx];
double start_time = millis_since_boot();
@@ -148,6 +136,7 @@ void CameraBuf::queue(size_t buf_idx) {
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);
@@ -158,14 +147,9 @@ void fill_frame_data(cereal::FrameData::Builder &framed, const FrameMetadata &fr
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->min_ev, c->max_ev, 0.0f, 100.0f);
const float perc = util::map_val(ev, c->ci->min_ev, c->ci->max_ev, 0.0f, 100.0f);
framed.setExposureValPercent(perc);
if (c->camera_id == CAMERA_ID_AR0231) {
framed.setSensor(cereal::FrameData::ImageSensor::AR0231);
} else if (c->camera_id == CAMERA_ID_OX03C10) {
framed.setSensor(cereal::FrameData::ImageSensor::OX03C10);
}
framed.setSensor(c->ci->image_sensor);
}
kj::Array<uint8_t> get_raw_frame_image(const CameraBuf *b) {
@@ -282,7 +266,6 @@ float set_exposure_target(const CameraBuf *b, int x_start, int x_end, int x_skip
}
}
// Find mean lumimance value
unsigned int lum_cur = 0;
for (lum_med = 255; lum_med >= 0; lum_med--) {
system/camerad/cameras/camera_common.h
+6 -32
View File
@@ -1,30 +1,12 @@
#pragma once
#include <cstdint>
#include <cstdlib>
#include <fcntl.h>
#include <memory>
#include <thread>
#include "cereal/messaging/messaging.h"
#include "cereal/visionipc/visionbuf.h"
#include "cereal/visionipc/visionipc.h"
#include "cereal/visionipc/visionipc_server.h"
#include "common/mat.h"
#include "common/queue.h"
#include "common/swaglog.h"
#include "system/hardware/hw.h"
#define CAMERA_ID_IMX298 0
#define CAMERA_ID_IMX179 1
#define CAMERA_ID_S5K3P8SP 2
#define CAMERA_ID_OV8865 3
#define CAMERA_ID_IMX298_FLIPPED 4
#define CAMERA_ID_OV10640 5
#define CAMERA_ID_LGC920 6
#define CAMERA_ID_LGC615 7
#define CAMERA_ID_AR0231 8
#define CAMERA_ID_OX03C10 9
#define CAMERA_ID_MAX 10
const int YUV_BUFFER_COUNT = 20;
@@ -42,20 +24,12 @@ const bool env_debug_frames = getenv("DEBUG_FRAMES") != NULL;
const bool env_log_raw_frames = getenv("LOG_RAW_FRAMES") != NULL;
const bool env_ctrl_exp_from_params = getenv("CTRL_EXP_FROM_PARAMS") != NULL;
typedef struct CameraInfo {
uint32_t frame_width, frame_height;
uint32_t frame_stride;
uint32_t frame_offset = 0;
uint32_t extra_height = 0;
int registers_offset = -1;
int stats_offset = -1;
} CameraInfo;
typedef struct FrameMetadata {
uint32_t frame_id;
uint32_t request_id;
// Timestamps
uint64_t timestamp_sof; // only set on tici
uint64_t timestamp_sof;
uint64_t timestamp_eof;
// Exposure
@@ -76,7 +50,7 @@ class CameraBuf {
private:
VisionIpcServer *vipc_server;
Debayer *debayer = nullptr;
VisionStreamType yuv_type;
VisionStreamType stream_type;
int cur_buf_idx;
SafeQueue<int> safe_queue;
int frame_buf_count;
@@ -88,11 +62,11 @@ public:
VisionBuf *cur_camera_buf;
std::unique_ptr<VisionBuf[]> camera_bufs;
std::unique_ptr<FrameMetadata[]> camera_bufs_metadata;
int rgb_width, rgb_height, rgb_stride;
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 yuv_type);
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);
};
system/camerad/cameras/camera_qcom2.cc
+102 -408
View File
@@ -1,17 +1,12 @@
#include "system/camerad/cameras/camera_qcom2.h"
#include <fcntl.h>
#include <poll.h>
#include <sys/ioctl.h>
#include <sys/mman.h>
#include <unistd.h>
#include <algorithm>
#include <atomic>
#include <cassert>
#include <cerrno>
#include <cmath>
#include <cstdio>
#include <cstring>
#include <string>
#include <vector>
@@ -19,105 +14,18 @@
#include "media/cam_defs.h"
#include "media/cam_isp.h"
#include "media/cam_isp_ife.h"
#include "media/cam_sensor.h"
#include "media/cam_req_mgr.h"
#include "media/cam_sensor_cmn_header.h"
#include "media/cam_sync.h"
#include "common/swaglog.h"
#include "system/camerad/cameras/sensor2_i2c.h"
const int MIPI_SETTLE_CNT = 33; // Calculated by camera_freqs.py
// For debugging:
// echo "4294967295" > /sys/module/cam_debug_util/parameters/debug_mdl
extern ExitHandler do_exit;
const size_t FRAME_WIDTH = 1928;
const size_t FRAME_HEIGHT = 1208;
const size_t FRAME_STRIDE = 2896; // for 12 bit output. 1928 * 12 / 8 + 4 (alignment)
const size_t AR0231_REGISTERS_HEIGHT = 2;
// TODO: this extra height is universal and doesn't apply per camera
const size_t AR0231_STATS_HEIGHT = 2+8;
const int MIPI_SETTLE_CNT = 33; // Calculated by camera_freqs.py
CameraInfo cameras_supported[CAMERA_ID_MAX] = {
[CAMERA_ID_AR0231] = {
.frame_width = FRAME_WIDTH,
.frame_height = FRAME_HEIGHT,
.frame_stride = FRAME_STRIDE,
.extra_height = AR0231_REGISTERS_HEIGHT + AR0231_STATS_HEIGHT,
.registers_offset = 0,
.frame_offset = AR0231_REGISTERS_HEIGHT,
.stats_offset = AR0231_REGISTERS_HEIGHT + FRAME_HEIGHT,
},
[CAMERA_ID_OX03C10] = {
.frame_width = FRAME_WIDTH,
.frame_height = FRAME_HEIGHT,
.frame_stride = FRAME_STRIDE, // (0xa80*12//8)
.extra_height = 16, // top 2 + bot 14
.frame_offset = 2,
},
};
const float DC_GAIN_AR0231 = 2.5;
const float DC_GAIN_OX03C10 = 7.32;
const float DC_GAIN_ON_GREY_AR0231 = 0.2;
const float DC_GAIN_OFF_GREY_AR0231 = 0.3;
const float DC_GAIN_ON_GREY_OX03C10 = 0.9;
const float DC_GAIN_OFF_GREY_OX03C10 = 1.0;
const int DC_GAIN_MIN_WEIGHT_AR0231 = 0;
const int DC_GAIN_MAX_WEIGHT_AR0231 = 1;
const int DC_GAIN_MIN_WEIGHT_OX03C10 = 1; // always on is fine
const int DC_GAIN_MAX_WEIGHT_OX03C10 = 1;
const float TARGET_GREY_FACTOR_AR0231 = 1.0;
const float TARGET_GREY_FACTOR_OX03C10 = 0.01;
const float sensor_analog_gains_AR0231[] = {
1.0/8.0, 2.0/8.0, 2.0/7.0, 3.0/7.0, // 0, 1, 2, 3
3.0/6.0, 4.0/6.0, 4.0/5.0, 5.0/5.0, // 4, 5, 6, 7
5.0/4.0, 6.0/4.0, 6.0/3.0, 7.0/3.0, // 8, 9, 10, 11
7.0/2.0, 8.0/2.0, 8.0/1.0}; // 12, 13, 14, 15 = bypass
const float sensor_analog_gains_OX03C10[] = {
1.0, 1.0625, 1.125, 1.1875, 1.25, 1.3125, 1.375, 1.4375, 1.5, 1.5625, 1.6875,
1.8125, 1.9375, 2.0, 2.125, 2.25, 2.375, 2.5, 2.625, 2.75, 2.875, 3.0,
3.125, 3.375, 3.625, 3.875, 4.0, 4.25, 4.5, 4.75, 5.0, 5.25, 5.5,
5.75, 6.0, 6.25, 6.5, 7.0, 7.5, 8.0, 8.5, 9.0, 9.5, 10.0,
10.5, 11.0, 11.5, 12.0, 12.5, 13.0, 13.5, 14.0, 14.5, 15.0, 15.5};
const uint32_t ox03c10_analog_gains_reg[] = {
0x100, 0x110, 0x120, 0x130, 0x140, 0x150, 0x160, 0x170, 0x180, 0x190, 0x1B0,
0x1D0, 0x1F0, 0x200, 0x220, 0x240, 0x260, 0x280, 0x2A0, 0x2C0, 0x2E0, 0x300,
0x320, 0x360, 0x3A0, 0x3E0, 0x400, 0x440, 0x480, 0x4C0, 0x500, 0x540, 0x580,
0x5C0, 0x600, 0x640, 0x680, 0x700, 0x780, 0x800, 0x880, 0x900, 0x980, 0xA00,
0xA80, 0xB00, 0xB80, 0xC00, 0xC80, 0xD00, 0xD80, 0xE00, 0xE80, 0xF00, 0xF80};
const int ANALOG_GAIN_MIN_IDX_AR0231 = 0x1; // 0.25x
const int ANALOG_GAIN_REC_IDX_AR0231 = 0x6; // 0.8x
const int ANALOG_GAIN_MAX_IDX_AR0231 = 0xD; // 4.0x
const int ANALOG_GAIN_COST_DELTA_AR0231 = 0;
const float ANALOG_GAIN_COST_LOW_AR0231 = 0.1;
const float ANALOG_GAIN_COST_HIGH_AR0231 = 5.0;
const int ANALOG_GAIN_MIN_IDX_OX03C10 = 0x0;
const int ANALOG_GAIN_REC_IDX_OX03C10 = 0x0; // 1x
const int ANALOG_GAIN_MAX_IDX_OX03C10 = 0x36;
const int ANALOG_GAIN_COST_DELTA_OX03C10 = -1;
const float ANALOG_GAIN_COST_LOW_OX03C10 = 0.4;
const float ANALOG_GAIN_COST_HIGH_OX03C10 = 6.4;
const int EXPOSURE_TIME_MIN_AR0231 = 2; // with HDR, fastest ss
const int EXPOSURE_TIME_MAX_AR0231 = 0x0855; // with HDR, slowest ss, 40ms
const int EXPOSURE_TIME_MIN_OX03C10 = 2; // 1x
const int EXPOSURE_TIME_MAX_OX03C10 = 2016;
const uint32_t VS_TIME_MIN_OX03C10 = 1;
const uint32_t VS_TIME_MAX_OX03C10 = 34; // vs < 35
int CameraState::clear_req_queue() {
struct cam_req_mgr_flush_info req_mgr_flush_request = {0};
req_mgr_flush_request.session_hdl = session_handle;
@@ -134,13 +42,7 @@ int CameraState::clear_req_queue() {
void CameraState::sensors_start() {
if (!enabled) return;
LOGD("starting sensor %d", camera_num);
if (camera_id == CAMERA_ID_AR0231) {
sensors_i2c(start_reg_array_ar0231, std::size(start_reg_array_ar0231), CAM_SENSOR_PACKET_OPCODE_SENSOR_CONFIG, true);
} else if (camera_id == CAMERA_ID_OX03C10) {
sensors_i2c(start_reg_array_ox03c10, std::size(start_reg_array_ox03c10), CAM_SENSOR_PACKET_OPCODE_SENSOR_CONFIG, false);
} else {
assert(false);
}
sensors_i2c(ci->start_reg_array.data(), ci->start_reg_array.size(), CAM_SENSOR_PACKET_OPCODE_SENSOR_CONFIG, ci->data_word);
}
void CameraState::sensors_poke(int request_id) {
@@ -161,7 +63,7 @@ void CameraState::sensors_poke(int request_id) {
}
}
void CameraState::sensors_i2c(struct i2c_random_wr_payload* dat, int len, int op_code, bool data_word) {
void CameraState::sensors_i2c(const struct i2c_random_wr_payload* dat, int len, int op_code, bool data_word) {
// LOGD("sensors_i2c: %d", len);
uint32_t cam_packet_handle = 0;
int size = sizeof(struct cam_packet)+sizeof(struct cam_cmd_buf_desc)*1;
@@ -215,21 +117,7 @@ int CameraState::sensors_init() {
auto probe = (struct cam_cmd_probe *)(i2c_info.get() + 1);
probe->camera_id = camera_num;
switch (camera_num) {
case 0:
// port 0
i2c_info->slave_addr = (camera_id == CAMERA_ID_AR0231) ? 0x20 : 0x6C; // 6C = 0x36*2
break;
case 1:
// port 1
i2c_info->slave_addr = (camera_id == CAMERA_ID_AR0231) ? 0x30 : 0x20;
break;
case 2:
// port 2
i2c_info->slave_addr = (camera_id == CAMERA_ID_AR0231) ? 0x20 : 0x6C;
break;
}
i2c_info->slave_addr = ci->getSlaveAddress(camera_num);
// 0(I2C_STANDARD_MODE) = 100khz, 1(I2C_FAST_MODE) = 400khz
//i2c_info->i2c_freq_mode = I2C_STANDARD_MODE;
i2c_info->i2c_freq_mode = I2C_FAST_MODE;
@@ -239,22 +127,14 @@ int CameraState::sensors_init() {
probe->addr_type = CAMERA_SENSOR_I2C_TYPE_WORD;
probe->op_code = 3; // don't care?
probe->cmd_type = CAMERA_SENSOR_CMD_TYPE_PROBE;
if (camera_id == CAMERA_ID_AR0231) {
probe->reg_addr = 0x3000;
probe->expected_data = 0x354;
} else if (camera_id == CAMERA_ID_OX03C10) {
probe->reg_addr = 0x300a;
probe->expected_data = 0x5803;
} else {
assert(false);
}
probe->reg_addr = ci->probe_reg_addr;
probe->expected_data = ci->probe_expected_data;
probe->data_mask = 0;
//buf_desc[1].size = buf_desc[1].length = 148;
buf_desc[1].size = buf_desc[1].length = 196;
buf_desc[1].type = CAM_CMD_BUF_I2C;
auto power_settings = mm.alloc<struct cam_cmd_power>(buf_desc[1].size, (uint32_t*)&buf_desc[1].mem_handle);
memset(power_settings.get(), 0, buf_desc[1].size);
// power on
struct cam_cmd_power *power = power_settings.get();
@@ -270,7 +150,7 @@ int CameraState::sensors_init() {
power->count = 1;
power->cmd_type = CAMERA_SENSOR_CMD_TYPE_PWR_UP;
power->power_settings[0].power_seq_type = 0;
power->power_settings[0].config_val_low = (camera_id == CAMERA_ID_AR0231) ? 19200000 : 24000000; //Hz
power->power_settings[0].config_val_low = ci->mclk_frequency;
power = power_set_wait(power, 1);
// reset high
@@ -423,10 +303,10 @@ void CameraState::config_isp(int io_mem_handle, int fence, int request_id, int b
if (io_mem_handle != 0) {
io_cfg[0].mem_handle[0] = io_mem_handle;
io_cfg[0].planes[0] = (struct cam_plane_cfg){
.width = ci.frame_width,
.height = ci.frame_height + ci.extra_height,
.plane_stride = ci.frame_stride,
.slice_height = ci.frame_height + ci.extra_height,
.width = ci->frame_width,
.height = ci->frame_height + ci->extra_height,
.plane_stride = ci->frame_stride,
.slice_height = ci->frame_height + ci->extra_height,
.meta_stride = 0x0, // YUV has meta(stride=0x400, size=0x5000)
.meta_size = 0x0,
.meta_offset = 0x0,
@@ -436,10 +316,10 @@ void CameraState::config_isp(int io_mem_handle, int fence, int request_id, int b
.h_init = 0x0,
.v_init = 0x0,
};
io_cfg[0].format = CAM_FORMAT_MIPI_RAW_12; // CAM_FORMAT_UBWC_TP10 for YUV
io_cfg[0].format = ci->mipi_format; // CAM_FORMAT_UBWC_TP10 for YUV
io_cfg[0].color_space = CAM_COLOR_SPACE_BASE; // CAM_COLOR_SPACE_BT601_FULL for YUV
io_cfg[0].color_pattern = 0x5; // 0x0 for YUV
io_cfg[0].bpp = 0xc;
io_cfg[0].bpp = (ci->mipi_format == CAM_FORMAT_MIPI_RAW_10 ? 0xa : 0xc); // bits per pixel
io_cfg[0].resource_type = CAM_ISP_IFE_OUT_RES_RDI_0; // CAM_ISP_IFE_OUT_RES_FULL for YUV
io_cfg[0].fence = fence;
io_cfg[0].direction = CAM_BUF_OUTPUT;
@@ -516,57 +396,14 @@ void CameraState::enqueue_req_multi(int start, int n, bool dp) {
// ******************* camera *******************
void CameraState::camera_set_parameters() {
if (camera_id == CAMERA_ID_AR0231) {
dc_gain_factor = DC_GAIN_AR0231;
dc_gain_min_weight = DC_GAIN_MIN_WEIGHT_AR0231;
dc_gain_max_weight = DC_GAIN_MAX_WEIGHT_AR0231;
dc_gain_on_grey = DC_GAIN_ON_GREY_AR0231;
dc_gain_off_grey = DC_GAIN_OFF_GREY_AR0231;
exposure_time_min = EXPOSURE_TIME_MIN_AR0231;
exposure_time_max = EXPOSURE_TIME_MAX_AR0231;
analog_gain_min_idx = ANALOG_GAIN_MIN_IDX_AR0231;
analog_gain_rec_idx = ANALOG_GAIN_REC_IDX_AR0231;
analog_gain_max_idx = ANALOG_GAIN_MAX_IDX_AR0231;
analog_gain_cost_delta = ANALOG_GAIN_COST_DELTA_AR0231;
analog_gain_cost_low = ANALOG_GAIN_COST_LOW_AR0231;
analog_gain_cost_high = ANALOG_GAIN_COST_HIGH_AR0231;
for (int i=0; i<=analog_gain_max_idx; i++) {
sensor_analog_gains[i] = sensor_analog_gains_AR0231[i];
}
min_ev = exposure_time_min * sensor_analog_gains[analog_gain_min_idx];
target_grey_factor = TARGET_GREY_FACTOR_AR0231;
} else if (camera_id == CAMERA_ID_OX03C10) {
dc_gain_factor = DC_GAIN_OX03C10;
dc_gain_min_weight = DC_GAIN_MIN_WEIGHT_OX03C10;
dc_gain_max_weight = DC_GAIN_MAX_WEIGHT_OX03C10;
dc_gain_on_grey = DC_GAIN_ON_GREY_OX03C10;
dc_gain_off_grey = DC_GAIN_OFF_GREY_OX03C10;
exposure_time_min = EXPOSURE_TIME_MIN_OX03C10;
exposure_time_max = EXPOSURE_TIME_MAX_OX03C10;
analog_gain_min_idx = ANALOG_GAIN_MIN_IDX_OX03C10;
analog_gain_rec_idx = ANALOG_GAIN_REC_IDX_OX03C10;
analog_gain_max_idx = ANALOG_GAIN_MAX_IDX_OX03C10;
analog_gain_cost_delta = ANALOG_GAIN_COST_DELTA_OX03C10;
analog_gain_cost_low = ANALOG_GAIN_COST_LOW_OX03C10;
analog_gain_cost_high = ANALOG_GAIN_COST_HIGH_OX03C10;
for (int i=0; i<=analog_gain_max_idx; i++) {
sensor_analog_gains[i] = sensor_analog_gains_OX03C10[i];
}
min_ev = (exposure_time_min + VS_TIME_MIN_OX03C10) * sensor_analog_gains[analog_gain_min_idx];
target_grey_factor = TARGET_GREY_FACTOR_OX03C10;
} else {
assert(false);
}
max_ev = exposure_time_max * dc_gain_factor * sensor_analog_gains[analog_gain_max_idx];
void CameraState::sensor_set_parameters() {
target_grey_fraction = 0.3;
dc_gain_enabled = false;
dc_gain_weight = dc_gain_min_weight;
gain_idx = analog_gain_rec_idx;
dc_gain_weight = ci->dc_gain_min_weight;
gain_idx = ci->analog_gain_rec_idx;
exposure_time = 5;
cur_ev[0] = cur_ev[1] = cur_ev[2] = (1 + dc_gain_weight * (dc_gain_factor-1) / dc_gain_max_weight) * sensor_analog_gains[gain_idx] * exposure_time;
cur_ev[0] = cur_ev[1] = cur_ev[2] = (1 + dc_gain_weight * (ci->dc_gain_factor-1) / ci->dc_gain_max_weight) * ci->sensor_analog_gains[gain_idx] * exposure_time;
}
void CameraState::camera_map_bufs(MultiCameraState *s) {
@@ -584,20 +421,13 @@ void CameraState::camera_map_bufs(MultiCameraState *s) {
enqueue_req_multi(1, FRAME_BUF_COUNT, 0);
}
void CameraState::camera_init(MultiCameraState *s, VisionIpcServer * v, int camera_id_, unsigned int fps, cl_device_id device_id, cl_context ctx, VisionStreamType yuv_type) {
void CameraState::camera_init(MultiCameraState *s, VisionIpcServer * v, cl_device_id device_id, cl_context ctx, VisionStreamType yuv_type) {
if (!enabled) return;
camera_id = camera_id_;
LOGD("camera init %d", camera_num);
assert(camera_id < std::size(cameras_supported));
ci = cameras_supported[camera_id];
assert(ci.frame_width != 0);
request_id_last = 0;
skipped = true;
camera_set_parameters();
buf.init(device_id, ctx, this, v, FRAME_BUF_COUNT, yuv_type);
camera_map_bufs(s);
}
@@ -616,22 +446,26 @@ void CameraState::camera_open(MultiCameraState *multi_cam_state_, int camera_num
// init memorymanager for this camera
mm.init(multi_cam_state->video0_fd);
// probe the sensor
LOGD("-- Probing sensor %d", camera_num);
camera_id = CAMERA_ID_AR0231;
ret = sensors_init();
if (ret != 0) {
// TODO: use build flag instead?
LOGD("AR0231 init failed, trying OX03C10");
camera_id = CAMERA_ID_OX03C10;
ret = sensors_init();
}
LOGD("-- Probing sensor %d done with %d", camera_num, ret);
if (ret != 0) {
auto init_sensor_lambda = [this](SensorInfo *sensor) {
ci.reset(sensor);
int ret = sensors_init();
if (ret == 0) {
sensor_set_parameters();
}
return ret == 0;
};
// Try different sensors one by one until it success.
if (!init_sensor_lambda(new AR0231) &&
!init_sensor_lambda(new OX03C10) &&
!init_sensor_lambda(new OS04C10)) {
LOGE("** sensor %d FAILED bringup, disabling", camera_num);
enabled = false;
return;
}
LOGD("-- Probing sensor %d success", camera_num);
// create session
struct cam_req_mgr_session_info session_info = {};
@@ -647,69 +481,58 @@ void CameraState::camera_open(MultiCameraState *multi_cam_state_, int camera_num
LOGD("acquire sensor dev");
LOG("-- Configuring sensor");
uint32_t dt;
if (camera_id == CAMERA_ID_AR0231) {
sensors_i2c(init_array_ar0231, std::size(init_array_ar0231), CAM_SENSOR_PACKET_OPCODE_SENSOR_CONFIG, true);
dt = 0x12; // Changing stats to 0x2C doesn't work, so change pixels to 0x12 instead
} else if (camera_id == CAMERA_ID_OX03C10) {
sensors_i2c(init_array_ox03c10, std::size(init_array_ox03c10), CAM_SENSOR_PACKET_OPCODE_SENSOR_CONFIG, false);
// one is 0x2a, two are 0x2b
dt = 0x2c;
} else {
assert(false);
}
printf("dt is %x\n", dt);
sensors_i2c(ci->init_reg_array.data(), ci->init_reg_array.size(), CAM_SENSOR_PACKET_OPCODE_SENSOR_CONFIG, ci->data_word);
// 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!)
if (!enabled) return;
struct cam_isp_in_port_info in_port_info = {
.res_type = (uint32_t[]){CAM_ISP_IFE_IN_RES_PHY_0, CAM_ISP_IFE_IN_RES_PHY_1, CAM_ISP_IFE_IN_RES_PHY_2}[camera_num],
.res_type = (uint32_t[]){CAM_ISP_IFE_IN_RES_PHY_0, CAM_ISP_IFE_IN_RES_PHY_1, CAM_ISP_IFE_IN_RES_PHY_2}[camera_num],
.lane_type = CAM_ISP_LANE_TYPE_DPHY,
.lane_num = 4,
.lane_cfg = 0x3210,
.lane_type = CAM_ISP_LANE_TYPE_DPHY,
.lane_num = 4,
.lane_cfg = 0x3210,
.vc = 0x0,
.dt = dt,
.format = CAM_FORMAT_MIPI_RAW_12,
.vc = 0x0,
.dt = ci->frame_data_type,
.format = ci->mipi_format,
.test_pattern = 0x2, // 0x3?
.usage_type = 0x0,
.test_pattern = 0x2, // 0x3?
.usage_type = 0x0,
.left_start = 0,
.left_stop = ci.frame_width - 1,
.left_width = ci.frame_width,
.left_start = 0,
.left_stop = ci->frame_width - 1,
.left_width = ci->frame_width,
.right_start = 0,
.right_stop = ci.frame_width - 1,
.right_width = ci.frame_width,
.right_start = 0,
.right_stop = ci->frame_width - 1,
.right_width = ci->frame_width,
.line_start = 0,
.line_stop = ci.frame_height + ci.extra_height - 1,
.height = ci.frame_height + ci.extra_height,
.line_start = 0,
.line_stop = ci->frame_height + ci->extra_height - 1,
.height = ci->frame_height + ci->extra_height,
.pixel_clk = 0x0,
.batch_size = 0x0,
.dsp_mode = CAM_ISP_DSP_MODE_NONE,
.hbi_cnt = 0x0,
.custom_csid = 0x0,
.pixel_clk = 0x0,
.batch_size = 0x0,
.dsp_mode = CAM_ISP_DSP_MODE_NONE,
.hbi_cnt = 0x0,
.custom_csid = 0x0,
.num_out_res = 0x1,
.data[0] = (struct cam_isp_out_port_info){
.res_type = CAM_ISP_IFE_OUT_RES_RDI_0,
.format = CAM_FORMAT_MIPI_RAW_12,
.width = ci.frame_width,
.height = ci.frame_height + ci.extra_height,
.comp_grp_id = 0x0, .split_point = 0x0, .secure_mode = 0x0,
},
.num_out_res = 0x1,
.data[0] = (struct cam_isp_out_port_info){
.res_type = CAM_ISP_IFE_OUT_RES_RDI_0,
.format = ci->mipi_format,
.width = ci->frame_width,
.height = ci->frame_height + ci->extra_height,
.comp_grp_id = 0x0, .split_point = 0x0, .secure_mode = 0x0,
},
};
struct cam_isp_resource isp_resource = {
.resource_id = CAM_ISP_RES_ID_PORT,
.handle_type = CAM_HANDLE_USER_POINTER,
.res_hdl = (uint64_t)&in_port_info,
.length = sizeof(in_port_info),
.resource_id = CAM_ISP_RES_ID_PORT,
.handle_type = CAM_HANDLE_USER_POINTER,
.res_hdl = (uint64_t)&in_port_info,
.length = sizeof(in_port_info),
};
auto isp_dev_handle_ = device_acquire(multi_cam_state->isp_fd, session_handle, &isp_resource);
@@ -790,9 +613,9 @@ void CameraState::camera_open(MultiCameraState *multi_cam_state_, int camera_num
}
void cameras_init(VisionIpcServer *v, MultiCameraState *s, cl_device_id device_id, cl_context ctx) {
s->driver_cam.camera_init(s, v, s->driver_cam.camera_id, 20, device_id, ctx, VISION_STREAM_DRIVER);
s->road_cam.camera_init(s, v, s->road_cam.camera_id, 20, device_id, ctx, VISION_STREAM_ROAD);
s->wide_road_cam.camera_init(s, v, s->wide_road_cam.camera_id, 20, device_id, ctx, VISION_STREAM_WIDE_ROAD);
s->driver_cam.camera_init(s, v, device_id, ctx, VISION_STREAM_DRIVER);
s->road_cam.camera_init(s, v, device_id, ctx, VISION_STREAM_ROAD);
s->wide_road_cam.camera_init(s, v, device_id, ctx, VISION_STREAM_WIDE_ROAD);
s->pm = new PubMaster({"roadCameraState", "driverCameraState", "wideRoadCameraState", "thumbnail"});
}
@@ -907,70 +730,6 @@ void cameras_close(MultiCameraState *s) {
delete s->pm;
}
std::map<uint16_t, std::pair<int, int>> CameraState::ar0231_build_register_lut(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 + 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 + ci.frame_stride * register_row, val_idx + ci.frame_stride * register_row);
}
value_tag_count++;
}
}
}
return registers;
}
std::map<uint16_t, uint16_t> CameraState::ar0231_parse_registers(uint8_t *data, std::initializer_list<uint16_t> addrs) {
if (ar0231_register_lut.empty()) {
ar0231_register_lut = ar0231_build_register_lut(data);
}
std::map<uint16_t, uint16_t> registers;
for (uint16_t addr : addrs) {
auto offset = ar0231_register_lut[addr];
registers[addr] = ((uint16_t)data[offset.first] << 8) | data[offset.second];
}
return registers;
}
void CameraState::handle_camera_event(void *evdat) {
if (!enabled) return;
struct cam_req_mgr_message *event_data = (struct cam_req_mgr_message *)evdat;
@@ -1007,9 +766,10 @@ void CameraState::handle_camera_event(void *evdat) {
auto &meta_data = buf.camera_bufs_metadata[buf_idx];
meta_data.frame_id = main_id - idx_offset;
meta_data.request_id = real_id;
meta_data.timestamp_sof = timestamp;
exp_lock.lock();
meta_data.gain = analog_gain_frac * (1 + dc_gain_weight * (dc_gain_factor-1) / dc_gain_max_weight);
meta_data.gain = analog_gain_frac * (1 + dc_gain_weight * (ci->dc_gain_factor-1) / ci->dc_gain_max_weight);
meta_data.high_conversion_gain = dc_gain_enabled;
meta_data.integ_lines = exposure_time;
meta_data.measured_grey_fraction = measured_grey_fraction;
@@ -1030,22 +790,7 @@ void CameraState::handle_camera_event(void *evdat) {
}
void CameraState::update_exposure_score(float desired_ev, int exp_t, int exp_g_idx, float exp_gain) {
float score = 1e6;
if (camera_id == CAMERA_ID_AR0231) {
// Cost of ev diff
score = std::abs(desired_ev - (exp_t * exp_gain)) * 10;
// Cost of absolute gain
float m = exp_g_idx > 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;
} else if (camera_id == CAMERA_ID_OX03C10) {
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;
}
float score = ci->getExposureScore(desired_ev, exp_t, exp_g_idx, exp_gain, gain_idx);
if (score < best_ev_score) {
new_exp_t = exp_t;
new_exp_g = exp_g_idx;
@@ -1071,10 +816,10 @@ void CameraState::set_camera_exposure(float grey_frac) {
const float cur_ev_ = cur_ev[buf.cur_frame_data.frame_id % 3];
// Scale target grey between 0.1 and 0.4 depending on lighting conditions
float new_target_grey = std::clamp(0.4 - 0.3 * log2(1.0 + target_grey_factor*cur_ev_) / log2(6000.0), 0.1, 0.4);
float new_target_grey = std::clamp(0.4 - 0.3 * log2(1.0 + ci->target_grey_factor*cur_ev_) / log2(6000.0), 0.1, 0.4);
float target_grey = (1.0 - k_grey) * target_grey_fraction + k_grey * new_target_grey;
float desired_ev = std::clamp(cur_ev_ * target_grey / grey_frac, min_ev, max_ev);
float desired_ev = std::clamp(cur_ev_ * target_grey / grey_frac, ci->min_ev, ci->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);
@@ -1085,21 +830,21 @@ void CameraState::set_camera_exposure(float grey_frac) {
// 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 < dc_gain_on_grey) {
if (!enable_dc_gain && target_grey < ci->dc_gain_on_grey) {
enable_dc_gain = true;
dc_gain_weight = dc_gain_min_weight;
} else if (enable_dc_gain && target_grey > dc_gain_off_grey) {
dc_gain_weight = ci->dc_gain_min_weight;
} else if (enable_dc_gain && target_grey > ci->dc_gain_off_grey) {
enable_dc_gain = false;
dc_gain_weight = dc_gain_max_weight;
dc_gain_weight = ci->dc_gain_max_weight;
}
if (enable_dc_gain && dc_gain_weight < dc_gain_max_weight) {dc_gain_weight += 1;}
if (!enable_dc_gain && dc_gain_weight > dc_gain_min_weight) {dc_gain_weight -= 1;}
if (enable_dc_gain && dc_gain_weight < ci->dc_gain_max_weight) {dc_gain_weight += 1;}
if (!enable_dc_gain && dc_gain_weight > ci->dc_gain_min_weight) {dc_gain_weight -= 1;}
std::string gain_bytes, time_bytes;
if (env_ctrl_exp_from_params) {
gain_bytes = Params().get("CameraDebugExpGain");
time_bytes = Params().get("CameraDebugExpTime");
gain_bytes = params.get("CameraDebugExpGain");
time_bytes = params.get("CameraDebugExpTime");
}
if (gain_bytes.size() > 0 && time_bytes.size() > 0) {
@@ -1113,14 +858,14 @@ void CameraState::set_camera_exposure(float grey_frac) {
} else {
// Simple brute force optimizer to choose sensor parameters
// to reach desired EV
for (int g = std::max((int)analog_gain_min_idx, gain_idx - 1); g <= std::min((int)analog_gain_max_idx, gain_idx + 1); g++) {
float gain = sensor_analog_gains[g] * (1 + dc_gain_weight * (dc_gain_factor-1) / dc_gain_max_weight);
for (int g = std::max((int)ci->analog_gain_min_idx, gain_idx - 1); g <= std::min((int)ci->analog_gain_max_idx, gain_idx + 1); g++) {
float gain = ci->sensor_analog_gains[g] * (1 + dc_gain_weight * (ci->dc_gain_factor-1) / ci->dc_gain_max_weight);
// Compute optimal time for given gain
int t = std::clamp(int(std::round(desired_ev / gain)), exposure_time_min, exposure_time_max);
int t = std::clamp(int(std::round(desired_ev / gain)), ci->exposure_time_min, ci->exposure_time_max);
// Only go below recommended gain when absolutely necessary to not overexpose
if (g < analog_gain_rec_idx && t > 20 && g < gain_idx) {
if (g < ci->analog_gain_rec_idx && t > 20 && g < gain_idx) {
continue;
}
@@ -1133,12 +878,12 @@ void CameraState::set_camera_exposure(float grey_frac) {
measured_grey_fraction = grey_frac;
target_grey_fraction = target_grey;
analog_gain_frac = sensor_analog_gains[new_exp_g];
analog_gain_frac = ci->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 * (1 + dc_gain_weight * (dc_gain_factor-1) / dc_gain_max_weight);
float gain = analog_gain_frac * (1 + dc_gain_weight * (ci->dc_gain_factor-1) / ci->dc_gain_max_weight);
cur_ev[buf.cur_frame_data.frame_id % 3] = exposure_time * gain;
exp_lock.unlock();
@@ -1151,59 +896,8 @@ void CameraState::set_camera_exposure(float grey_frac) {
}
// LOGE("ae - camera %d, cur_t %.5f, sof %.5f, dt %.5f", camera_num, 1e-9 * nanos_since_boot(), 1e-9 * buf.cur_frame_data.timestamp_sof, 1e-9 * (nanos_since_boot() - buf.cur_frame_data.timestamp_sof));
if (camera_id == CAMERA_ID_AR0231) {
uint16_t analog_gain_reg = 0xFF00 | (new_exp_g << 4) | new_exp_g;
struct i2c_random_wr_payload exp_reg_array[] = {
{0x3366, analog_gain_reg},
{0x3362, (uint16_t)(dc_gain_enabled ? 0x1 : 0x0)},
{0x3012, (uint16_t)exposure_time},
};
sensors_i2c(exp_reg_array, sizeof(exp_reg_array)/sizeof(struct i2c_random_wr_payload), CAM_SENSOR_PACKET_OPCODE_SENSOR_CONFIG, true);
} else if (camera_id == CAMERA_ID_OX03C10) {
// t_HCG&t_LCG + t_VS on LPD, t_SPD on SPD
uint32_t hcg_time = exposure_time;
uint32_t lcg_time = hcg_time;
uint32_t spd_time = std::min(std::max((uint32_t)exposure_time, (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];
struct i2c_random_wr_payload exp_reg_array[] = {
{0x3501, hcg_time>>8}, {0x3502, hcg_time&0xFF},
{0x3581, lcg_time>>8}, {0x3582, lcg_time&0xFF},
{0x3541, spd_time>>8}, {0x3542, spd_time&0xFF},
{0x35c2, vs_time&0xFF},
{0x3508, real_gain>>8}, {0x3509, real_gain&0xFF},
};
sensors_i2c(exp_reg_array, sizeof(exp_reg_array)/sizeof(struct i2c_random_wr_payload), CAM_SENSOR_PACKET_OPCODE_SENSOR_CONFIG, false);
}
}
static 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;
}
static void ar0231_process_registers(MultiCameraState *s, CameraState *c, cereal::FrameData::Builder &framed){
const uint8_t expected_preamble[] = {0x0a, 0xaa, 0x55, 0x20, 0xa5, 0x55};
uint8_t *data = (uint8_t*)c->buf.cur_camera_buf->addr + c->ci.registers_offset;
if (memcmp(data, expected_preamble, std::size(expected_preamble)) != 0){
LOGE("unexpected register data found");
return;
}
auto registers = c->ar0231_parse_registers(data, {0x2000, 0x2002, 0x20b0, 0x20b2, 0x30c6, 0x30c8, 0x30ca, 0x30cc});
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});
auto exp_reg_array = ci->getExposureRegisters(exposure_time, new_exp_g, dc_gain_enabled);
sensors_i2c(exp_reg_array.data(), exp_reg_array.size(), CAM_SENSOR_PACKET_OPCODE_SENSOR_CONFIG, ci->data_word);
}
static void process_driver_camera(MultiCameraState *s, CameraState *c, int cnt) {
@@ -1214,9 +908,7 @@ static void process_driver_camera(MultiCameraState *s, CameraState *c, int cnt)
framed.setFrameType(cereal::FrameData::FrameType::FRONT);
fill_frame_data(framed, c->buf.cur_frame_data, c);
if (c->camera_id == CAMERA_ID_AR0231) {
ar0231_process_registers(s, c, framed);
}
c->ci->processRegisters(c, framed);
s->pm->send("driverCameraState", msg);
}
@@ -1231,10 +923,7 @@ void process_road_camera(MultiCameraState *s, CameraState *c, int cnt) {
}
LOGT(c->buf.cur_frame_data.frame_id, "%s: Image set", c == &s->road_cam ? "RoadCamera" : "WideRoadCamera");
if (c->camera_id == CAMERA_ID_AR0231) {
ar0231_process_registers(s, c, framed);
}
c->ci->processRegisters(c, framed);
s->pm->send(c == &s->road_cam ? "roadCameraState" : "wideRoadCameraState", msg);
const auto [x, y, w, h] = (c == &s->wide_road_cam) ? std::tuple(96, 250, 1734, 524) : std::tuple(96, 160, 1734, 986);
@@ -1282,6 +971,9 @@ void cameras_run(MultiCameraState *s) {
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);
}
// for debugging
//do_exit = do_exit || event_data->u.frame_msg.frame_id > (30*20);
if (event_data->session_hdl == s->road_cam.session_handle) {
s->road_cam.handle_camera_event(event_data);
} else if (event_data->session_hdl == s->wide_road_cam.session_handle) {
@@ -1292,6 +984,8 @@ void cameras_run(MultiCameraState *s) {
LOGE("Unknown vidioc event source");
assert(false);
}
} else {
LOGE("unhandled event %d\n", ev.type);
}
} else {
LOGE("VIDIOC_DQEVENT failed, errno=%d", errno);
system/camerad/cameras/camera_qcom2.h
+9 -35
View File
@@ -1,23 +1,20 @@
#pragma once
#include <cstdint>
#include <map>
#include <memory>
#include <utility>
#include <media/cam_req_mgr.h>
#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
#define ANALOG_GAIN_MAX_CNT 55
class CameraState {
public:
MultiCameraState *multi_cam_state;
CameraInfo ci;
std::unique_ptr<const SensorInfo> ci;
bool enabled;
std::mutex exp_lock;
@@ -28,32 +25,13 @@ public:
int gain_idx;
float analog_gain_frac;
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 cur_ev[3];
float min_ev, max_ev;
float best_ev_score;
int new_exp_g;
int new_exp_t;
float measured_grey_fraction;
float target_grey_fraction;
float target_grey_factor;
unique_fd sensor_fd;
unique_fd csiphy_fd;
@@ -67,13 +45,11 @@ public:
void sensors_start();
void camera_open(MultiCameraState *multi_cam_state, int camera_num, bool enabled);
void camera_set_parameters();
void sensor_set_parameters();
void camera_map_bufs(MultiCameraState *s);
void camera_init(MultiCameraState *s, VisionIpcServer *v, int camera_id, unsigned int fps, cl_device_id device_id, cl_context ctx, VisionStreamType yuv_type);
void camera_init(MultiCameraState *s, VisionIpcServer *v, cl_device_id device_id, cl_context ctx, VisionStreamType yuv_type);
void camera_close();
std::map<uint16_t, uint16_t> ar0231_parse_registers(uint8_t *data, std::initializer_list<uint16_t> addrs);
int32_t session_handle;
int32_t sensor_dev_handle;
int32_t isp_dev_handle;
@@ -89,12 +65,10 @@ public:
int frame_id_last;
int idx_offset;
bool skipped;
int camera_id;
CameraBuf buf;
MemoryManager mm;
private:
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);
@@ -102,11 +76,11 @@ private:
int sensors_init();
void sensors_poke(int request_id);
void sensors_i2c(struct i2c_random_wr_payload* dat, int len, int op_code, bool data_word);
void sensors_i2c(const struct i2c_random_wr_payload* dat, int len, int op_code, bool data_word);
// Register parsing
std::map<uint16_t, std::pair<int, int>> ar0231_register_lut;
std::map<uint16_t, std::pair<int, int>> ar0231_build_register_lut(uint8_t *data);
private:
// for debugging
Params params;
};
typedef struct MultiCameraState {
system/camerad/cameras/camera_util.cc
+10 -7
View File
@@ -1,5 +1,7 @@
#include "system/camerad/cameras/camera_util.h"
#include <string.h>
#include <cassert>
#include <sys/ioctl.h>
@@ -30,10 +32,10 @@ int do_cam_control(int fd, int op_code, void *handle, int size) {
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,
.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;
@@ -41,9 +43,9 @@ std::optional<int32_t> device_acquire(int fd, int32_t session_handle, void *data
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,
.session_handle = session_handle,
.dev_handle = dev_handle,
.packet_handle = packet_handle,
};
return do_cam_control(fd, CAM_CONFIG_DEV, &cmd, sizeof(cmd));
}
@@ -111,6 +113,7 @@ void *MemoryManager::alloc_buf(int size, uint32_t *handle) {
size_lookup[ptr] = size;
}
lock.unlock();
memset(ptr, 0, size);
return ptr;
}
system/camerad/cameras/camera_util.h
+1 -1
View File
@@ -1,6 +1,6 @@
#pragma once
#include <functional>
#include <functional>
#include <map>
#include <memory>
#include <mutex>
system/camerad/imgproc/conv.cl
-110
View File
@@ -1,110 +0,0 @@
// const __constant float3 rgb_weights = (0.299, 0.587, 0.114); // opencv rgb2gray weights
// const __constant float3 bgr_weights = (0.114, 0.587, 0.299); // bgr2gray weights
// convert input rgb image to single channel then conv
__kernel void rgb2gray_conv2d(
const __global uchar * input,
__global short * output,
__constant short * filter,
__local uchar3 * cached
)
{
const int rowOffset = get_global_id(1) * IMAGE_W;
const int my = get_global_id(0) + rowOffset;
const int localRowLen = TWICE_HALF_FILTER_SIZE + get_local_size(0);
const int localRowOffset = ( get_local_id(1) + HALF_FILTER_SIZE ) * localRowLen;
const int myLocal = localRowOffset + get_local_id(0) + HALF_FILTER_SIZE;
// cache local pixels
cached[ myLocal ].x = input[ my * 3 ]; // r
cached[ myLocal ].y = input[ my * 3 + 1]; // g
cached[ myLocal ].z = input[ my * 3 + 2]; // b
// pad
if (
get_global_id(0) < HALF_FILTER_SIZE ||
get_global_id(0) > IMAGE_W - HALF_FILTER_SIZE - 1 ||
get_global_id(1) < HALF_FILTER_SIZE ||
get_global_id(1) > IMAGE_H - HALF_FILTER_SIZE - 1
)
{
barrier(CLK_LOCAL_MEM_FENCE);
return;
}
else
{
int localColOffset = -1;
int globalColOffset = -1;
// cache extra
if ( get_local_id(0) < HALF_FILTER_SIZE )
{
localColOffset = get_local_id(0);
globalColOffset = -HALF_FILTER_SIZE;
cached[ localRowOffset + get_local_id(0) ].x = input[ my * 3 - HALF_FILTER_SIZE * 3 ];
cached[ localRowOffset + get_local_id(0) ].y = input[ my * 3 - HALF_FILTER_SIZE * 3 + 1];
cached[ localRowOffset + get_local_id(0) ].z = input[ my * 3 - HALF_FILTER_SIZE * 3 + 2];
}
else if ( get_local_id(0) >= get_local_size(0) - HALF_FILTER_SIZE )
{
localColOffset = get_local_id(0) + TWICE_HALF_FILTER_SIZE;
globalColOffset = HALF_FILTER_SIZE;
cached[ myLocal + HALF_FILTER_SIZE ].x = input[ my * 3 + HALF_FILTER_SIZE * 3 ];
cached[ myLocal + HALF_FILTER_SIZE ].y = input[ my * 3 + HALF_FILTER_SIZE * 3 + 1];
cached[ myLocal + HALF_FILTER_SIZE ].z = input[ my * 3 + HALF_FILTER_SIZE * 3 + 2];
}
if ( get_local_id(1) < HALF_FILTER_SIZE )
{
cached[ get_local_id(1) * localRowLen + get_local_id(0) + HALF_FILTER_SIZE ].x = input[ my * 3 - HALF_FILTER_SIZE_IMAGE_W * 3 ];
cached[ get_local_id(1) * localRowLen + get_local_id(0) + HALF_FILTER_SIZE ].y = input[ my * 3 - HALF_FILTER_SIZE_IMAGE_W * 3 + 1];
cached[ get_local_id(1) * localRowLen + get_local_id(0) + HALF_FILTER_SIZE ].z = input[ my * 3 - HALF_FILTER_SIZE_IMAGE_W * 3 + 2];
if (localColOffset > 0)
{
cached[ get_local_id(1) * localRowLen + localColOffset ].x = input[ my * 3 - HALF_FILTER_SIZE_IMAGE_W * 3 + globalColOffset * 3];
cached[ get_local_id(1) * localRowLen + localColOffset ].y = input[ my * 3 - HALF_FILTER_SIZE_IMAGE_W * 3 + globalColOffset * 3 + 1];
cached[ get_local_id(1) * localRowLen + localColOffset ].z = input[ my * 3 - HALF_FILTER_SIZE_IMAGE_W * 3 + globalColOffset * 3 + 2];
}
}
else if ( get_local_id(1) >= get_local_size(1) -HALF_FILTER_SIZE )
{
int offset = ( get_local_id(1) + TWICE_HALF_FILTER_SIZE ) * localRowLen;
cached[ offset + get_local_id(0) + HALF_FILTER_SIZE ].x = input[ my * 3 + HALF_FILTER_SIZE_IMAGE_W * 3 ];
cached[ offset + get_local_id(0) + HALF_FILTER_SIZE ].y = input[ my * 3 + HALF_FILTER_SIZE_IMAGE_W * 3 + 1];
cached[ offset + get_local_id(0) + HALF_FILTER_SIZE ].z = input[ my * 3 + HALF_FILTER_SIZE_IMAGE_W * 3 + 2];
if (localColOffset > 0)
{
cached[ offset + localColOffset ].x = input[ my * 3 + HALF_FILTER_SIZE_IMAGE_W * 3 + globalColOffset * 3];
cached[ offset + localColOffset ].y = input[ my * 3 + HALF_FILTER_SIZE_IMAGE_W * 3 + globalColOffset * 3 + 1];
cached[ offset + localColOffset ].z = input[ my * 3 + HALF_FILTER_SIZE_IMAGE_W * 3 + globalColOffset * 3 + 2];
}
}
// sync
barrier(CLK_LOCAL_MEM_FENCE);
// perform convolution
int fIndex = 0;
short sum = 0;
for (int r = -HALF_FILTER_SIZE; r <= HALF_FILTER_SIZE; r++)
{
int curRow = r * localRowLen;
for (int c = -HALF_FILTER_SIZE; c <= HALF_FILTER_SIZE; c++, fIndex++)
{
if (!FLIP_RB){
// sum += dot(rgb_weights, cached[ myLocal + curRow + c ]) * filter[ fIndex ];
sum += (cached[ myLocal + curRow + c ].x / 3 + cached[ myLocal + curRow + c ].y / 2 + cached[ myLocal + curRow + c ].z / 9) * filter[ fIndex ];
} else {
// sum += dot(bgr_weights, cached[ myLocal + curRow + c ]) * filter[ fIndex ];
sum += (cached[ myLocal + curRow + c ].x / 9 + cached[ myLocal + curRow + c ].y / 2 + cached[ myLocal + curRow + c ].z / 3) * filter[ fIndex ];
}
}
}
output[my] = sum;
}
}
system/camerad/imgproc/pool.cl
-34
View File
@@ -1,34 +0,0 @@
// calculate variance in each subregion
__kernel void var_pool(
const __global char * input,
__global ushort * output // should not be larger than 128*128 so uint16
)
{
const int xidx = get_global_id(0) + ROI_X_MIN;
const int yidx = get_global_id(1) + ROI_Y_MIN;
const int size = X_PITCH * Y_PITCH;
float fsum = 0;
char mean, max;
for (int i = 0; i < size; i++) {
int x_offset = i % X_PITCH;
int y_offset = i / X_PITCH;
fsum += input[xidx*X_PITCH + yidx*Y_PITCH*FULL_STRIDE_X + x_offset + y_offset*FULL_STRIDE_X];
max = input[xidx*X_PITCH + yidx*Y_PITCH*FULL_STRIDE_X + x_offset + y_offset*FULL_STRIDE_X]>max ? input[xidx*X_PITCH + yidx*Y_PITCH*FULL_STRIDE_X + x_offset + y_offset*FULL_STRIDE_X]:max;
}
mean = convert_char_rte(fsum / size);
float fvar = 0;
for (int i = 0; i < size; i++) {
int x_offset = i % X_PITCH;
int y_offset = i / X_PITCH;
fvar += (input[xidx*X_PITCH + yidx*Y_PITCH*FULL_STRIDE_X + x_offset + y_offset*FULL_STRIDE_X] - mean) * (input[xidx*X_PITCH + yidx*Y_PITCH*FULL_STRIDE_X + x_offset + y_offset*FULL_STRIDE_X] - mean);
}
fvar = fvar / size;
output[(xidx-ROI_X_MIN)+(yidx-ROI_Y_MIN)*(ROI_X_MAX-ROI_X_MIN+1)] = convert_ushort_rte(5 * fvar + convert_float_rte(max));
}
system/camerad/imgproc/utils.cc
-106
View File
@@ -1,106 +0,0 @@
#include "system/camerad/imgproc/utils.h"
#include <algorithm>
#include <cassert>
#include <cstdio>
#include <cmath>
#include <cstring>
const int16_t lapl_conv_krnl[9] = {0, 1, 0,
1, -4, 1,
0, 1, 0};
// calculate score based on laplacians in one area
uint16_t get_lapmap_one(const int16_t *lap, int x_pitch, int y_pitch) {
const int size = x_pitch * y_pitch;
// avg and max of roi
int16_t max = 0;
int sum = 0;
for (int i = 0; i < size; ++i) {
const int16_t v = lap[i];
sum += v;
if (v > max) max = v;
}
const int16_t mean = sum / size;
// var of roi
int var = 0;
for (int i = 0; i < size; ++i) {
var += std::pow(lap[i] - mean, 2);
}
const float fvar = (float)var / size;
return std::min(5 * fvar + max, (float)65535);
}
bool is_blur(const uint16_t *lapmap, const size_t size) {
float bad_sum = 0;
for (int i = 0; i < size; i++) {
if (lapmap[i] < LM_THRESH) {
bad_sum += 1 / (float)size;
}
}
return (bad_sum > LM_PREC_THRESH);
}
static cl_program build_conv_program(cl_device_id device_id, cl_context context, int image_w, int image_h, int filter_size) {
char args[4096];
snprintf(args, sizeof(args),
"-cl-fast-relaxed-math -cl-denorms-are-zero "
"-DIMAGE_W=%d -DIMAGE_H=%d -DFLIP_RB=%d "
"-DFILTER_SIZE=%d -DHALF_FILTER_SIZE=%d -DTWICE_HALF_FILTER_SIZE=%d -DHALF_FILTER_SIZE_IMAGE_W=%d",
image_w, image_h, 1,
filter_size, filter_size/2, (filter_size/2)*2, (filter_size/2)*image_w);
return cl_program_from_file(context, device_id, "imgproc/conv.cl", args);
}
LapConv::LapConv(cl_device_id device_id, cl_context ctx, int rgb_width, int rgb_height, int rgb_stride, int filter_size)
: width(rgb_width / NUM_SEGMENTS_X), height(rgb_height / NUM_SEGMENTS_Y), rgb_stride(rgb_stride),
roi_buf(width * height * 3), result_buf(width * height) {
prg = build_conv_program(device_id, ctx, width, height, filter_size);
krnl = CL_CHECK_ERR(clCreateKernel(prg, "rgb2gray_conv2d", &err));
// TODO: Removed CL_MEM_SVM_FINE_GRAIN_BUFFER, confirm it doesn't matter
roi_cl = CL_CHECK_ERR(clCreateBuffer(ctx, CL_MEM_READ_WRITE, roi_buf.size() * sizeof(roi_buf[0]), NULL, &err));
result_cl = CL_CHECK_ERR(clCreateBuffer(ctx, CL_MEM_READ_WRITE, result_buf.size() * sizeof(result_buf[0]), NULL, &err));
filter_cl = CL_CHECK_ERR(clCreateBuffer(ctx, CL_MEM_READ_ONLY | CL_MEM_COPY_HOST_PTR,
9 * sizeof(int16_t), (void *)&lapl_conv_krnl, &err));
}
LapConv::~LapConv() {
CL_CHECK(clReleaseMemObject(roi_cl));
CL_CHECK(clReleaseMemObject(result_cl));
CL_CHECK(clReleaseMemObject(filter_cl));
CL_CHECK(clReleaseKernel(krnl));
CL_CHECK(clReleaseProgram(prg));
}
uint16_t LapConv::Update(cl_command_queue q, const uint8_t *rgb_buf, const int roi_id) {
// sharpness scores
const int x_offset = ROI_X_MIN + roi_id % (ROI_X_MAX - ROI_X_MIN + 1);
const int y_offset = ROI_Y_MIN + roi_id / (ROI_X_MAX - ROI_X_MIN + 1);
const uint8_t *rgb_offset = rgb_buf + y_offset * height * rgb_stride + x_offset * width * 3;
for (int i = 0; i < height; ++i) {
memcpy(&roi_buf[i * width * 3], &rgb_offset[i * rgb_stride], width * 3);
}
constexpr int local_mem_size = (CONV_LOCAL_WORKSIZE + 2 * (3 / 2)) * (CONV_LOCAL_WORKSIZE + 2 * (3 / 2)) * (3 * sizeof(uint8_t));
const size_t global_work_size[] = {(size_t)width, (size_t)height};
const size_t local_work_size[] = {CONV_LOCAL_WORKSIZE, CONV_LOCAL_WORKSIZE};
CL_CHECK(clEnqueueWriteBuffer(q, roi_cl, CL_TRUE, 0, roi_buf.size() * sizeof(roi_buf[0]), roi_buf.data(), 0, 0, 0));
CL_CHECK(clSetKernelArg(krnl, 0, sizeof(cl_mem), (void *)&roi_cl));
CL_CHECK(clSetKernelArg(krnl, 1, sizeof(cl_mem), (void *)&result_cl));
CL_CHECK(clSetKernelArg(krnl, 2, sizeof(cl_mem), (void *)&filter_cl));
CL_CHECK(clSetKernelArg(krnl, 3, local_mem_size, 0));
cl_event conv_event;
CL_CHECK(clEnqueueNDRangeKernel(q, krnl, 2, NULL, global_work_size, local_work_size, 0, 0, &conv_event));
CL_CHECK(clWaitForEvents(1, &conv_event));
CL_CHECK(clReleaseEvent(conv_event));
CL_CHECK(clEnqueueReadBuffer(q, result_cl, CL_TRUE, 0,
result_buf.size() * sizeof(result_buf[0]), result_buf.data(), 0, 0, 0));
return get_lapmap_one(result_buf.data(), width, height);
}
system/camerad/imgproc/utils.h
-37
View File
@@ -1,37 +0,0 @@
#pragma once
#include <cstddef>
#include <cstdint>
#include <vector>
#include "common/clutil.h"
#define NUM_SEGMENTS_X 8
#define NUM_SEGMENTS_Y 6
#define ROI_X_MIN 1
#define ROI_X_MAX 6
#define ROI_Y_MIN 2
#define ROI_Y_MAX 3
#define LM_THRESH 120
#define LM_PREC_THRESH 0.9 // 90 perc is blur
#define CONV_LOCAL_WORKSIZE 16
class LapConv {
public:
LapConv(cl_device_id device_id, cl_context ctx, int rgb_width, int rgb_height, int rgb_stride, int filter_size);
~LapConv();
uint16_t Update(cl_command_queue q, const uint8_t *rgb_buf, const int roi_id);
private:
cl_mem roi_cl, result_cl, filter_cl;
cl_program prg;
cl_kernel krnl;
const int width, height;
const int rgb_stride;
std::vector<uint8_t> roi_buf;
std::vector<int16_t> result_buf;
};
bool is_blur(const uint16_t *lapmap, const size_t size);
system/camerad/main.cc
+1 -1
View File
@@ -8,7 +8,7 @@
int main(int argc, char *argv[]) {
if (Hardware::PC()) {
printf("camerad is not meant to run on PC\n");
printf("exiting, camerad is not meant to run on PC\n");
return 0;
}
system/camerad/sensors/ar0231.cc
+171
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 = FRAME_WIDTH;
frame_height = FRAME_HEIGHT;
frame_stride = FRAME_STRIDE;
extra_height = AR0231_REGISTERS_HEIGHT + AR0231_STATS_HEIGHT;
registers_offset = 0;
frame_offset = AR0231_REGISTERS_HEIGHT;
stats_offset = AR0231_REGISTERS_HEIGHT + FRAME_HEIGHT;
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;
}
system/camerad/sensors/ar0231_registers.h
+119
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},
};
system/camerad/sensors/os04c10.cc
+105
View File
@@ -0,0 +1,105 @@
#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[] = {
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_OS04C10 = 1;
//const uint32_t VS_TIME_MAX_OS04C10 = 34; // vs < 35
} // namespace
OS04C10::OS04C10() {
image_sensor = cereal::FrameData::ImageSensor::OS04C10;
data_word = false;
frame_width = 1920;
frame_height = 1080;
frame_stride = (1920*10/8);
/*
frame_width = 0xa80;
frame_height = 0x5f0;
frame_stride = 0xd20;
*/
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_10;
frame_data_type = 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_OS04C10[i];
}
min_ev = (exposure_time_min + VS_TIME_MIN_OS04C10) * 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 {
// 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_OS04C10) / 3), exposure_time_max + VS_TIME_MAX_OS04C10);
//uint32_t vs_time = std::min(std::max((uint32_t)exposure_time / 40, VS_TIME_MIN_OS04C10), VS_TIME_MAX_OS04C10);
uint32_t real_gain = os04c10_analog_gains_reg[new_exp_g];
hcg_time = 100;
real_gain = 0x320;
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 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;
}
system/camerad/sensors/os04c10_registers.h
+298
View File
@@ -0,0 +1,298 @@
#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_1920x1080_MIPI728Mbps_Linear12bit_20FPS_4Lane_MCLK24MHz
{0x0103, 0x01},
{0x0301, 0x84},
{0x0303, 0x01},
{0x0305, 0x5b},
{0x0306, 0x01},
{0x0307, 0x17},
{0x0323, 0x04},
{0x0324, 0x01},
{0x0325, 0x62},
{0x3012, 0x06},
{0x3013, 0x02},
{0x3016, 0x72},
{0x3021, 0x03},
{0x3106, 0x21},
{0x3107, 0xa1},
{0x3500, 0x00},
{0x3501, 0x00},
{0x3502, 0x40},
{0x3503, 0x88},
{0x3508, 0x07},
{0x3509, 0xc0},
{0x350a, 0x04},
{0x350b, 0x00},
{0x350c, 0x07},
{0x350d, 0xc0},
{0x350e, 0x04},
{0x350f, 0x00},
{0x3510, 0x00},
{0x3511, 0x00},
{0x3512, 0x20},
{0x3624, 0x00},
{0x3625, 0x4c},
{0x3660, 0x00},
{0x3666, 0xa5},
{0x3667, 0xa5},
{0x366a, 0x64},
{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, 0x00},
{0x370e, 0x0c},
{0x3710, 0x00},
{0x3713, 0x00},
{0x3725, 0x02},
{0x372a, 0x03},
{0x3738, 0xce},
{0x3748, 0x00},
{0x374a, 0x00},
{0x374c, 0x00},
{0x374e, 0x00},
{0x3756, 0x00},
{0x3757, 0x0e},
{0x3767, 0x00},
{0x3771, 0x00},
{0x377b, 0x20},
{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},
{0x400a, 0x01},
{0x400b, 0x50},
{0x400e, 0x08},
{0x4043, 0x7e},
{0x4045, 0x7e},
{0x4047, 0x7e},
{0x4049, 0x7e},
{0x4090, 0x14},
{0x40b0, 0x00},
{0x40b1, 0x00},
{0x40b2, 0x00},
{0x40b3, 0x00},
{0x40b4, 0x00},
{0x40b5, 0x00},
{0x40b7, 0x00},
{0x40b8, 0x00},
{0x40b9, 0x00},
{0x40ba, 0x00},
{0x4301, 0x00},
{0x4303, 0x00},
{0x4502, 0x04},
{0x4503, 0x00},
{0x4504, 0x06},
{0x4506, 0x00},
{0x4507, 0x64},
{0x4803, 0x00},
{0x480c, 0x32},
{0x480e, 0x00},
{0x4813, 0x00},
{0x4819, 0x70},
{0x481f, 0x30},
{0x4823, 0x3f},
{0x4825, 0x30},
{0x4833, 0x10},
{0x484b, 0x07},
{0x488b, 0x00},
{0x4d00, 0x04},
{0x4d01, 0xad},
{0x4d02, 0xbc},
{0x4d03, 0xa1},
{0x4d04, 0x1f},
{0x4d05, 0x4c},
{0x4d0b, 0x01},
{0x4e00, 0x2a},
{0x4e0d, 0x00},
{0x5001, 0x09},
{0x5004, 0x00},
{0x5080, 0x04},
{0x5036, 0x00},
{0x5180, 0x70},
{0x5181, 0x10},
{0x520a, 0x03},
{0x520b, 0x06},
{0x520c, 0x0c},
{0x580b, 0x0f},
{0x580d, 0x00},
{0x580f, 0x00},
{0x5820, 0x00},
{0x5821, 0x00},
{0x301c, 0xf8},
{0x301e, 0xb4},
{0x301f, 0xd0},
{0x3022, 0x01},
{0x3109, 0xe7},
{0x3600, 0x00},
{0x3610, 0x65},
{0x3611, 0x85},
{0x3613, 0x3a},
{0x3615, 0x60},
{0x3621, 0x90},
{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, 0x4a},
{0x3709, 0x4a},
{0x370b, 0xa2},
{0x370c, 0x01},
{0x370f, 0x04},
{0x3714, 0x24},
{0x3716, 0x24},
{0x3719, 0x11},
{0x371a, 0x1e},
{0x3720, 0x00},
{0x3724, 0x13},
{0x373f, 0xb0},
{0x3741, 0x4a},
{0x3743, 0x4a},
{0x3745, 0x4a},
{0x3747, 0x4a},
{0x3749, 0xa2},
{0x374b, 0xa2},
{0x374d, 0xa2},
{0x374f, 0xa2},
{0x3755, 0x10},
{0x376c, 0x00},
{0x378d, 0x30},
{0x3790, 0x4a},
{0x3791, 0xa2},
{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},
{0x3800, 0x01},
{0x3801, 0x80},
{0x3802, 0x00},
{0x3803, 0xdc},
{0x3804, 0x09},
{0x3805, 0x0f},
{0x3806, 0x05},
{0x3807, 0x23},
{0x3808, 0x07},
{0x3809, 0x80},
{0x380a, 0x04},
{0x380b, 0x38},
{0x380c, 0x04},
{0x380d, 0x2e},
{0x380e, 0x12},
{0x380f, 0x70},
{0x3811, 0x08},
{0x3813, 0x08},
{0x3814, 0x01},
{0x3815, 0x01},
{0x3816, 0x01},
{0x3817, 0x01},
{0x3820, 0x88},
{0x3821, 0x00},
{0x3880, 0x25},
{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, 0x1e},
{0x480a, 0x04},
{0x4837, 0x15},
{0x4c00, 0x08},
{0x4c01, 0x08},
{0x4c04, 0x00},
{0x4c05, 0x00},
{0x5000, 0xf9},
{0x3c8c, 0x10},
};
system/camerad/sensors/ox03c10.cc
+94
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 = FRAME_WIDTH;
frame_height = FRAME_HEIGHT;
frame_stride = FRAME_STRIDE; // (0xa80*12//8)
extra_height = 16; // top 2 + bot 14
frame_offset = 2;
start_reg_array.assign(std::begin(start_reg_array_ox03c10), std::end(start_reg_array_ox03c10));
init_reg_array.assign(std::begin(init_array_ox03c10), std::end(init_array_ox03c10));
probe_reg_addr = 0x300a;
probe_expected_data = 0x5803;
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;
}
system/camerad/cameras/sensor2_i2c.h → system/camerad/sensors/ox03c10_registers.h
+5 -120
View File
@@ -1,9 +1,9 @@
struct i2c_random_wr_payload start_reg_array_ar0231[] = {{0x301A, 0x91C}};
struct i2c_random_wr_payload stop_reg_array_ar0231[] = {{0x301A, 0x918}};
struct i2c_random_wr_payload start_reg_array_ox03c10[] = {{0x100, 1}};
struct i2c_random_wr_payload stop_reg_array_ox03c10[] = {{0x100, 0}};
#pragma once
struct i2c_random_wr_payload init_array_ox03c10[] = {
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},
@@ -759,118 +759,3 @@ struct i2c_random_wr_payload init_array_ox03c10[] = {
{0x3548, 0x0F}, {0x3549, 0x00},
{0x35c1, 0x00},
};
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},
};
system/camerad/sensors/sensor.h
+93
View File
@@ -0,0 +1,93 @@
#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
const size_t FRAME_WIDTH = 1928;
const size_t FRAME_HEIGHT = 1208;
const size_t FRAME_STRIDE = 2896; // for 12 bit output. 1928 * 12 / 8 + 4 (alignment)
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;
};
system/camerad/snapshot/snapshot.py
-1
View File
@@ -13,7 +13,6 @@ from openpilot.system.hardware import PC
from openpilot.selfdrive.controls.lib.alertmanager import set_offroad_alert
from openpilot.selfdrive.manager.process_config import managed_processes
LM_THRESH = 120 # defined in system/camerad/imgproc/utils.h
VISION_STREAMS = {
"roadCameraState": VisionStreamType.VISION_STREAM_ROAD,
system/camerad/test/.gitignore
+1
View File
@@ -1 +1,2 @@
jpegs/
test_ae_gray
system/camerad/test/ae_gray_test.h
-17
View File
@@ -1,17 +0,0 @@
#pragma once
#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};
system/camerad/test/get_thumbnails_for_segment.py
+4 -12
View File
@@ -1,27 +1,19 @@
#!/usr/bin/env python3
import argparse
import os
from tqdm import tqdm
from openpilot.common.file_helpers import mkdirs_exists_ok
from openpilot.tools.lib.logreader import LogReader
from openpilot.tools.lib.route import Route
import argparse
if __name__ == "__main__":
parser = argparse.ArgumentParser()
parser.add_argument("route", help="The route name")
parser.add_argument("segment", type=int, help="The index of the segment")
args = parser.parse_args()
out_path = os.path.join("jpegs", f"{args.route.replace('|', '_')}_{args.segment}")
mkdirs_exists_ok(out_path)
out_path = os.path.join("jpegs", f"{args.route.replace('|', '_').replace('/', '_')}")
os.makedirs(out_path, exist_ok=True)
r = Route(args.route)
path = r.log_paths()[args.segment] or r.qlog_paths()[args.segment]
lr = list(LogReader(path))
lr = LogReader(args.route)
for msg in tqdm(lr):
if msg.which() == 'thumbnail':
system/camerad/test/ae_gray_test.cc → system/camerad/test/test_ae_gray.cc
+21 -5
View File
@@ -1,6 +1,5 @@
// unittest for set_exposure_target
#include "system/camerad/test/ae_gray_test.h"
#define CATCH_CONFIG_MAIN
#include "catch2/catch.hpp"
#include <cassert>
@@ -10,7 +9,25 @@
#include "common/util.h"
#include "system/camerad/cameras/camera_common.h"
int main() {
#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 = {};
@@ -63,5 +80,4 @@ int main() {
assert(passed);
delete[] fb_y;
return 0;
}
system/hardware/base.h
+1 -2
View File
@@ -29,12 +29,11 @@ public:
static void poweroff() {}
static void set_brightness(int percent) {}
static void set_display_power(bool on) {}
static void set_volume(float volume) {}
static bool get_ssh_enabled() { return false; }
static void set_ssh_enabled(bool enabled) {}
static void config_cpu_rendering();
static void config_cpu_rendering(bool offscreen);
static bool PC() { return false; }
static bool TICI() { return false; }
system/hardware/base.py
+3 -4
View File
@@ -23,6 +23,9 @@ class HardwareBase(ABC):
except Exception:
return default
def booted(self) -> bool:
return True
@abstractmethod
def reboot(self, reason=None):
pass
@@ -51,10 +54,6 @@ class HardwareBase(ABC):
def get_serial(self):
pass
@abstractmethod
def get_subscriber_info(self):
pass
@abstractmethod
def get_network_info(self):
pass
system/hardware/hw.h
+2 -2
View File
@@ -30,13 +30,13 @@ namespace Path {
}
inline std::string params() {
return Hardware::PC() ? util::getenv("PARAMS_ROOT", Path::comma_home() + "/params") : "/data/params";
return util::getenv("PARAMS_ROOT", Hardware::PC() ? (Path::comma_home() + "/params") : "/data/params");
}
inline std::string rsa_file() {
return Hardware::PC() ? Path::comma_home() + "/persist/comma/id_rsa" : "/persist/comma/id_rsa";
}
inline std::string swaglog_ipc() {
return "ipc:///tmp/logmessage" + Path::openpilot_prefix();
}
system/hardware/hw.py
+25 -2
View File
@@ -3,6 +3,8 @@ from pathlib import Path
from openpilot.system.hardware import PC
DEFAULT_DOWNLOAD_CACHE_ROOT = "/tmp/comma_download_cache"
class Paths:
@staticmethod
def comma_home() -> str:
@@ -31,5 +33,26 @@ class Paths:
@staticmethod
def download_cache_root() -> str:
if os.environ.get('COMMA_CACHE', False):
return os.environ['COMMA_CACHE']
return "/tmp/comma_download_cache" + os.environ.get("OPENPILOT_PREFIX", "") + "/"
return os.environ['COMMA_CACHE'] + "/"
return DEFAULT_DOWNLOAD_CACHE_ROOT + os.environ.get("OPENPILOT_PREFIX", "") + "/"
@staticmethod
def persist_root() -> str:
if PC:
return os.path.join(Paths.comma_home(), "persist")
else:
return "/persist/"
@staticmethod
def stats_root() -> str:
if PC:
return str(Path(Paths.comma_home()) / "stats")
else:
return "/data/stats/"
@staticmethod
def config_root() -> str:
if PC:
return Paths.comma_home()
else:
return "/tmp/.comma"
system/hardware/pc/hardware.h
+4 -10
View File
@@ -13,16 +13,10 @@ public:
static bool TICI() { return util::getenv("TICI", 0) == 1; }
static bool AGNOS() { return util::getenv("TICI", 0) == 1; }
static void set_volume(float volume) {
volume = util::map_val(volume, 0.f, 1.f, MIN_VOLUME, MAX_VOLUME);
char volume_str[6];
snprintf(volume_str, sizeof(volume_str), "%.3f", volume);
std::system(("pactl set-sink-volume @DEFAULT_SINK@ " + std::string(volume_str)).c_str());
}
static void config_cpu_rendering() {
setenv("QT_QPA_PLATFORM", "offscreen", 1);
static void config_cpu_rendering(bool offscreen) {
if (offscreen) {
setenv("QT_QPA_PLATFORM", "offscreen", 1);
}
setenv("__GLX_VENDOR_LIBRARY_NAME", "mesa", 1);
setenv("LP_NUM_THREADS", "0", 1); // disable threading so we stay on our assigned CPU
}
system/hardware/pc/hardware.py
-3
View File
@@ -29,9 +29,6 @@ class Pc(HardwareBase):
def get_serial(self):
return "cccccccc"
def get_subscriber_info(self):
return ""
def get_network_info(self):
return None
system/hardware/tici/agnos.json
+25 -24
View File
@@ -1,9 +1,9 @@
[
{
"name": "boot",
"url": "https://commadist.azureedge.net/agnosupdate/boot-8d8d8620de8b2687f3a8fffdb81b2abd1fe2ead5bc831361a1a212e5589ac279.img.xz",
"hash": "8d8d8620de8b2687f3a8fffdb81b2abd1fe2ead5bc831361a1a212e5589ac279",
"hash_raw": "8d8d8620de8b2687f3a8fffdb81b2abd1fe2ead5bc831361a1a212e5589ac279",
"url": "https://commadist.azureedge.net/agnosupdate/boot-fd30f580375279ff4605034ec13711890a2b227205571a087cdc5226a2710275.img.xz",
"hash": "fd30f580375279ff4605034ec13711890a2b227205571a087cdc5226a2710275",
"hash_raw": "fd30f580375279ff4605034ec13711890a2b227205571a087cdc5226a2710275",
"size": 15636480,
"sparse": false,
"full_check": true,
@@ -11,9 +11,9 @@
},
{
"name": "abl",
"url": "https://commadist.azureedge.net/agnosupdate/abl-0084fcf79fea067632a1c2d9519b6445ad484aa8b09f49f22e6b45b4dccacd2d.img.xz",
"hash": "0084fcf79fea067632a1c2d9519b6445ad484aa8b09f49f22e6b45b4dccacd2d",
"hash_raw": "0084fcf79fea067632a1c2d9519b6445ad484aa8b09f49f22e6b45b4dccacd2d",
"url": "https://commadist.azureedge.net/agnosupdate/abl-bb234733816781b3d09266f91f741436e9bf17e1a7caf468cf7d09ee788cef4a.img.xz",
"hash": "bb234733816781b3d09266f91f741436e9bf17e1a7caf468cf7d09ee788cef4a",
"hash_raw": "bb234733816781b3d09266f91f741436e9bf17e1a7caf468cf7d09ee788cef4a",
"size": 274432,
"sparse": false,
"full_check": true,
@@ -21,9 +21,9 @@
},
{
"name": "xbl",
"url": "https://commadist.azureedge.net/agnosupdate/xbl-942b9b2914d89c2a70fdf27380b59e04b549ac2fd53ecb29d6549d1a9c8daeaa.img.xz",
"hash": "942b9b2914d89c2a70fdf27380b59e04b549ac2fd53ecb29d6549d1a9c8daeaa",
"hash_raw": "942b9b2914d89c2a70fdf27380b59e04b549ac2fd53ecb29d6549d1a9c8daeaa",
"url": "https://commadist.azureedge.net/agnosupdate/xbl-bcef195b00a1ab685da601f4072722569773ab161e91c8753ad99ca4217a28f5.img.xz",
"hash": "bcef195b00a1ab685da601f4072722569773ab161e91c8753ad99ca4217a28f5",
"hash_raw": "bcef195b00a1ab685da601f4072722569773ab161e91c8753ad99ca4217a28f5",
"size": 3282672,
"sparse": false,
"full_check": true,
@@ -31,9 +31,9 @@
},
{
"name": "xbl_config",
"url": "https://commadist.azureedge.net/agnosupdate/xbl_config-6881d94599f65d94c13bcc0bd860184dfba2dfe96ec776d08fb35ac5b5f85bbf.img.xz",
"hash": "6881d94599f65d94c13bcc0bd860184dfba2dfe96ec776d08fb35ac5b5f85bbf",
"hash_raw": "6881d94599f65d94c13bcc0bd860184dfba2dfe96ec776d08fb35ac5b5f85bbf",
"url": "https://commadist.azureedge.net/agnosupdate/xbl_config-19791056558c16f8dae787531b5e30b3b3db2ded9d666688df45ce1b91a72bac.img.xz",
"hash": "19791056558c16f8dae787531b5e30b3b3db2ded9d666688df45ce1b91a72bac",
"hash_raw": "19791056558c16f8dae787531b5e30b3b3db2ded9d666688df45ce1b91a72bac",
"size": 98124,
"sparse": false,
"full_check": true,
@@ -41,9 +41,9 @@
},
{
"name": "devcfg",
"url": "https://commadist.azureedge.net/agnosupdate/devcfg-9bbf168baff6101f4890c5c95c118e30813c2610cfb35b8e19e363f04a32a262.img.xz",
"hash": "9bbf168baff6101f4890c5c95c118e30813c2610cfb35b8e19e363f04a32a262",
"hash_raw": "9bbf168baff6101f4890c5c95c118e30813c2610cfb35b8e19e363f04a32a262",
"url": "https://commadist.azureedge.net/agnosupdate/devcfg-be44b73dda5be840b09d5347d536459e31098da3fea97596956c0bdad19bdf27.img.xz",
"hash": "be44b73dda5be840b09d5347d536459e31098da3fea97596956c0bdad19bdf27",
"hash_raw": "be44b73dda5be840b09d5347d536459e31098da3fea97596956c0bdad19bdf27",
"size": 40336,
"sparse": false,
"full_check": true,
@@ -51,9 +51,9 @@
},
{
"name": "aop",
"url": "https://commadist.azureedge.net/agnosupdate/aop-c1d9d712980f6b2a4b12196597f4d1bf3fe4fec6c59edf29ae63ef21f11b8222.img.xz",
"hash": "c1d9d712980f6b2a4b12196597f4d1bf3fe4fec6c59edf29ae63ef21f11b8222",
"hash_raw": "c1d9d712980f6b2a4b12196597f4d1bf3fe4fec6c59edf29ae63ef21f11b8222",
"url": "https://commadist.azureedge.net/agnosupdate/aop-5d764611a683d6a738cf06a1dcf8a926d0f47b5117ad40d3054167de6dd8bd0f.img.xz",
"hash": "5d764611a683d6a738cf06a1dcf8a926d0f47b5117ad40d3054167de6dd8bd0f",
"hash_raw": "5d764611a683d6a738cf06a1dcf8a926d0f47b5117ad40d3054167de6dd8bd0f",
"size": 184364,
"sparse": false,
"full_check": true,
@@ -61,16 +61,17 @@
},
{
"name": "system",
"url": "https://commadist.azureedge.net/agnosupdate/system-e1fa3018bce9bad01c6967e5e21f1141cf5c8f02d2edfaed51c738f74a32a432.img.xz",
"hash": "611011f3e3f147bc24f371105a9dd3760ec11ba424c56d4a442a66b098c784c0",
"hash_raw": "e1fa3018bce9bad01c6967e5e21f1141cf5c8f02d2edfaed51c738f74a32a432",
"url": "https://commadist.azureedge.net/agnosupdate/system-e1952bb363688c0f5c0646e39bcdfb45be25b5e2baed37d1ba7801aa1a3a9c98.img.xz",
"hash": "3b6cdf9bd881a5e90b21dd02c6faa923b415e32ecae9bfdc96753d4208fb82fe",
"hash_raw": "e1952bb363688c0f5c0646e39bcdfb45be25b5e2baed37d1ba7801aa1a3a9c98",
"size": 10737418240,
"sparse": true,
"full_check": false,
"has_ab": true,
"alt": {
"hash": "256442a55fcb9e8f72969f003a4db91598dee1136f8dda85b553a557d36b93d8",
"url": "https://commadist.azureedge.net/agnosupdate/system-skip-chunks-e1fa3018bce9bad01c6967e5e21f1141cf5c8f02d2edfaed51c738f74a32a432.img.xz"
"hash": "2fb81e58f4bc6c4e5e71c8e7ac7553f85082c430627d7a5cc54a6bbc82862500",
"url": "https://commadist.azureedge.net/agnosupdate/system-skip-chunks-e1952bb363688c0f5c0646e39bcdfb45be25b5e2baed37d1ba7801aa1a3a9c98.img.xz",
"size": 4543090376
}
}
]
]
system/hardware/tici/esim.nmconnection
+30
View File
@@ -0,0 +1,30 @@
[connection]
id=esim
uuid=fff6553c-3284-4707-a6b1-acc021caaafb
type=gsm
permissions=
autoconnect=true
autoconnect-retries=100
autoconnect-priority=2
metered=1
[gsm]
apn=
home-only=false
auto-config=true
sim-id=
[ipv4]
route-metric=1000
dns-priority=1000
dns-search=
method=auto
[ipv6]
ddr-gen-mode=stable-privacy
dns-search=
route-metric=1000
dns-priority=1000
method=auto
[proxy]
system/hardware/tici/hardware.h
+4 -10
View File
@@ -67,14 +67,6 @@ public:
bl_power_control.close();
}
}
static void set_volume(float volume) {
volume = util::map_val(volume, 0.f, 1.f, MIN_VOLUME, MAX_VOLUME);
char volume_str[6];
snprintf(volume_str, sizeof(volume_str), "%.3f", volume);
std::system(("pactl set-sink-volume @DEFAULT_SINK@ " + std::string(volume_str)).c_str());
}
static std::map<std::string, std::string> get_init_logs() {
std::map<std::string, std::string> ret = {
@@ -104,8 +96,10 @@ public:
static bool get_ssh_enabled() { return Params().getBool("SshEnabled"); }
static void set_ssh_enabled(bool enabled) { Params().putBool("SshEnabled", enabled); }
static void config_cpu_rendering() {
setenv("QT_QPA_PLATFORM", "eglfs", 1); // offscreen doesn't work with EGL/GLES
static void config_cpu_rendering(bool offscreen) {
if (offscreen) {
setenv("QT_QPA_PLATFORM", "eglfs", 1); // offscreen doesn't work with EGL/GLES
}
setenv("LP_NUM_THREADS", "0", 1); // disable threading so we stay on our assigned CPU
}
};
system/hardware/tici/hardware.py
+32 -73
View File
@@ -3,6 +3,7 @@ import math
import os
import subprocess
import time
import tempfile
from enum import IntEnum
from functools import cached_property, lru_cache
from pathlib import Path
@@ -73,6 +74,11 @@ def sudo_write(val, path):
# fallback for debugfs files
os.system(f"sudo su -c 'echo {val} > {path}'")
def sudo_read(path: str) -> str:
try:
return subprocess.check_output(f"sudo cat {path}", shell=True, encoding='utf8')
except Exception:
return ""
def affine_irq(val, action):
irqs = get_irqs_for_action(action)
@@ -104,7 +110,7 @@ class Tici(HardwareBase):
def nm(self):
return self.bus.get_object(NM, '/org/freedesktop/NetworkManager')
@cached_property
@property # this should not be cached, in case the modemmanager restarts
def mm(self):
return self.bus.get_object(MM, '/org/freedesktop/ModemManager1')
@@ -200,9 +206,6 @@ class Tici(HardwareBase):
'data_connected': modem.Get(MM_MODEM, 'State', dbus_interface=DBUS_PROPS, timeout=TIMEOUT) == MM_MODEM_STATE.CONNECTED,
}
def get_subscriber_info(self):
return ""
def get_imei(self, slot):
if slot != 0:
return ""
@@ -210,8 +213,8 @@ class Tici(HardwareBase):
return str(self.get_modem().Get(MM_MODEM, 'EquipmentIdentifier', dbus_interface=DBUS_PROPS, timeout=TIMEOUT))
def get_network_info(self):
modem = self.get_modem()
try:
modem = self.get_modem()
info = modem.Command("AT+QNWINFO", math.ceil(TIMEOUT), dbus_interface=MM_MODEM, timeout=TIMEOUT)
extra = modem.Command('AT+QENG="servingcell"', math.ceil(TIMEOUT), dbus_interface=MM_MODEM, timeout=TIMEOUT)
state = modem.Get(MM_MODEM, 'State', dbus_interface=DBUS_PROPS, timeout=TIMEOUT)
@@ -292,67 +295,6 @@ class Tici(HardwareBase):
return super().get_network_metered(network_type)
@staticmethod
def set_bandwidth_limit(upload_speed_kbps: int, download_speed_kbps: int) -> None:
upload_speed_kbps = int(upload_speed_kbps) # Ensure integer value
download_speed_kbps = int(download_speed_kbps) # Ensure integer value
adapter = "wwan0"
ifb = "ifb0"
sudo = ["sudo"]
tc = sudo + ["tc"]
# check, cmd
cleanup = [
# Clean up old rules
(False, tc + ["qdisc", "del", "dev", adapter, "root"]),
(False, tc + ["qdisc", "del", "dev", ifb, "root"]),
(False, tc + ["qdisc", "del", "dev", adapter, "ingress"]),
(False, tc + ["qdisc", "del", "dev", ifb, "ingress"]),
# Bring ifb0 down
(False, sudo + ["ip", "link", "set", "dev", ifb, "down"]),
]
upload = [
# Create root Hierarchy Token Bucket that sends all traffic to 1:20
(True, tc + ["qdisc", "add", "dev", adapter, "root", "handle", "1:", "htb", "default", "20"]),
# Create class 1:20 with specified rate limit
(True, tc + ["class", "add", "dev", adapter, "parent", "1:", "classid", "1:20", "htb", "rate", f"{upload_speed_kbps}kbit"]),
# Create universal 32 bit filter on adapter that sends all outbound ip traffic through the class
(True, tc + ["filter", "add", "dev", adapter, "parent", "1:", "protocol", "ip", "prio", \
"10", "u32", "match", "ip", "dst", "0.0.0.0/0", "flowid", "1:20"]),
]
download = [
# Bring ifb0 up
(True, sudo + ["ip", "link", "set", "dev", ifb, "up"]),
# Redirect ingress (incoming) to egress ifb0
(True, tc + ["qdisc", "add", "dev", adapter, "handle", "ffff:", "ingress"]),
(True, tc + ["filter", "add", "dev", adapter, "parent", "ffff:", "protocol", "ip", "u32", \
"match", "u32", "0", "0", "action", "mirred", "egress", "redirect", "dev", ifb]),
# Add class and rules for virtual interface
(True, tc + ["qdisc", "add", "dev", ifb, "root", "handle", "2:", "htb"]),
(True, tc + ["class", "add", "dev", ifb, "parent", "2:", "classid", "2:1", "htb", "rate", f"{download_speed_kbps}kbit"]),
# Add filter to rule for IP address
(True, tc + ["filter", "add", "dev", ifb, "protocol", "ip", "parent", "2:", "prio", "1", "u32", "match", "ip", "src", "0.0.0.0/0", "flowid", "2:1"]),
]
commands = cleanup
if upload_speed_kbps != -1:
commands += upload
if download_speed_kbps != -1:
commands += download
for check, cmd in commands:
subprocess.run(cmd, check=check)
def get_modem_version(self):
try:
modem = self.get_modem()
@@ -392,10 +334,6 @@ class Tici(HardwareBase):
pass
return ret
def get_usb_present(self):
# Not sure if relevant on tici, but the file exists
return self.read_param_file("/sys/class/power_supply/usb/present", lambda x: bool(int(x)), False)
def get_current_power_draw(self):
return (self.read_param_file("/sys/class/hwmon/hwmon1/power1_input", int) / 1e6)
@@ -532,9 +470,23 @@ class Tici(HardwareBase):
except Exception:
pass
# blue prime config
if sim_id.startswith('8901410'):
os.system('mmcli -m any --3gpp-set-initial-eps-bearer-settings="apn=Broadband"')
# blue prime
blue_prime = sim_id.startswith('8901410')
initial_apn = "Broadband" if blue_prime else ""
os.system(f'mmcli -m any --3gpp-set-initial-eps-bearer-settings="apn={initial_apn}"')
# eSIM prime
if sim_id.startswith('8985235'):
dest = "/etc/NetworkManager/system-connections/esim.nmconnection"
with open(Path(__file__).parent/'esim.nmconnection') as f, tempfile.NamedTemporaryFile(mode='w') as tf:
dat = f.read()
dat = dat.replace("sim-id=", f"sim-id={sim_id}")
tf.write(dat)
tf.flush()
# needs to be root
os.system(f"sudo cp {tf.name} {dest}")
os.system(f"sudo nmcli con load {dest}")
def get_networks(self):
r = {}
@@ -600,8 +552,15 @@ class Tici(HardwareBase):
time.sleep(0.5)
gpio_set(GPIO.STM_BOOT0, 0)
def booted(self):
# this normally boots within 8s, but on rare occasions takes 30+s
encoder_state = sudo_read("/sys/kernel/debug/msm_vidc/core0/info")
if "Core state: 0" in encoder_state and (time.monotonic() < 60*2):
return False
return True
if __name__ == "__main__":
t = Tici()
t.configure_modem()
t.initialize_hardware()
t.set_power_save(False)
system/hardware/tici/tests/test_power_draw.py
+3 -1
View File
@@ -10,6 +10,7 @@ from typing import List
import cereal.messaging as messaging
from cereal.services import SERVICE_LIST
from openpilot.selfdrive.car.car_helpers import write_car_param
from openpilot.system.hardware import HARDWARE
from openpilot.system.hardware.tici.power_monitor import get_power
from openpilot.selfdrive.manager.process_config import managed_processes
@@ -51,6 +52,7 @@ class TestPowerDraw(unittest.TestCase):
def setUp(self):
HARDWARE.initialize_hardware()
HARDWARE.set_power_save(False)
write_car_param()
# wait a bit for power save to disable
time.sleep(5)
@@ -91,8 +93,8 @@ class TestPowerDraw(unittest.TestCase):
msgs_expected = int(sum(SAMPLE_TIME * SERVICE_LIST[msg].frequency for msg in proc.msgs))
tab.append([proc.name, round(expected, 2), round(cur, 2), msgs_expected, msgs_received])
with self.subTest(proc=proc.name):
np.testing.assert_allclose(cur, expected, rtol=proc.rtol, atol=proc.atol)
np.testing.assert_allclose(msgs_expected, msgs_received, rtol=.02, atol=2)
np.testing.assert_allclose(cur, expected, rtol=proc.rtol, atol=proc.atol)
print(tabulate(tab))
print(f"Baseline {baseline:.2f}W\n")
system/loggerd/config.py
-6
View File
@@ -1,6 +1,4 @@
import os
from pathlib import Path
from openpilot.system.hardware import PC
from openpilot.system.hardware.hw import Paths
@@ -9,10 +7,6 @@ SEGMENT_LENGTH = 60
STATS_DIR_FILE_LIMIT = 10000
STATS_SOCKET = "ipc:///tmp/stats"
if PC:
STATS_DIR = str(Path.home() / ".comma" / "stats")
else:
STATS_DIR = "/data/stats/"
STATS_FLUSH_TIME_S = 60
def get_available_percent(default=None):
system/loggerd/deleter.py
+1 -1
View File
@@ -4,7 +4,7 @@ import shutil
import threading
from typing import List
from openpilot.system.hardware.hw import Paths
from openpilot.system.swaglog import cloudlog
from openpilot.common.swaglog import cloudlog
from openpilot.system.loggerd.config import get_available_bytes, get_available_percent
from openpilot.system.loggerd.uploader import listdir_by_creation
from openpilot.system.loggerd.xattr_cache import getxattr
system/loggerd/encoder/encoder.cc
+7 -4
View File
@@ -27,8 +27,11 @@ void VideoEncoder::publisher_publish(VideoEncoder *e, int segment_num, uint32_t
edat.setData(dat);
if (flags & V4L2_BUF_FLAG_KEYFRAME) edat.setHeader(header);
auto words = new kj::Array<capnp::word>(capnp::messageToFlatArray(msg));
auto bytes = words->asBytes();
e->pm->send(e->encoder_info.publish_name, bytes.begin(), bytes.size());
delete words;
uint32_t bytes_size = capnp::computeSerializedSizeInWords(msg) * sizeof(capnp::word);
if (e->msg_cache.size() < bytes_size) {
e->msg_cache.resize(bytes_size);
}
kj::ArrayOutputStream output_stream(kj::ArrayPtr<capnp::byte>(e->msg_cache.data(), bytes_size));
capnp::writeMessage(output_stream, msg);
e->pm->send(e->encoder_info.publish_name, e->msg_cache.data(), bytes_size);
}
system/loggerd/encoder/encoder.h
+2 -1
View File
@@ -4,6 +4,7 @@
#include <cstdint>
#include <memory>
#include <thread>
#include <vector>
#include "cereal/messaging/messaging.h"
#include "cereal/visionipc/visionipc.h"
@@ -23,7 +24,6 @@ public:
static void publisher_publish(VideoEncoder *e, int segment_num, uint32_t idx, VisionIpcBufExtra &extra, unsigned int flags, kj::ArrayPtr<capnp::byte> header, kj::ArrayPtr<capnp::byte> dat);
protected:
int in_width, in_height;
const EncoderInfo encoder_info;
@@ -32,4 +32,5 @@ private:
// total frames encoded
int cnt = 0;
std::unique_ptr<PubMaster> pm;
std::vector<capnp::byte> msg_cache;
};
system/loggerd/logger.cc
+31 -153
View File
@@ -1,21 +1,7 @@
#include "system/loggerd/logger.h"
#include <sys/stat.h>
#include <unistd.h>
#include <ftw.h>
#include <cassert>
#include <cerrno>
#include <cstdint>
#include <cstdio>
#include <cstdlib>
#include <cstring>
#include <ctime>
#include <fstream>
#include <iostream>
#include <map>
#include <streambuf>
#include <string>
#include <vector>
#include "common/params.h"
@@ -57,7 +43,7 @@ kj::Array<capnp::word> logger_build_init_data() {
init.setGitCommit(params_map["GitCommit"]);
init.setGitBranch(params_map["GitBranch"]);
init.setGitRemote(params_map["GitRemote"]);
init.setPassive(params.getBool("Passive"));
init.setPassive(false);
init.setDongleId(params_map["DongleId"]);
auto lparams = init.initParams().initEntries(params_map.size());
@@ -108,159 +94,51 @@ std::string logger_get_route_name() {
return route_name;
}
void log_init_data(LoggerState *s) {
auto bytes = s->init_data.asBytes();
logger_log(s, bytes.begin(), bytes.size(), s->has_qlog);
}
static void lh_log_sentinel(LoggerHandle *h, SentinelType type) {
static void log_sentinel(LoggerState *log, SentinelType type, int eixt_signal = 0) {
MessageBuilder msg;
auto sen = msg.initEvent().initSentinel();
sen.setType(type);
sen.setSignal(h->exit_signal);
auto bytes = msg.toBytes();
lh_log(h, bytes.begin(), bytes.size(), true);
sen.setSignal(eixt_signal);
log->write(msg.toBytes(), true);
}
// ***** logging functions *****
void logger_init(LoggerState *s, bool has_qlog) {
pthread_mutex_init(&s->lock, NULL);
s->part = -1;
s->has_qlog = has_qlog;
s->route_name = logger_get_route_name();
s->init_data = logger_build_init_data();
LoggerState::LoggerState(const std::string &log_root) {
route_name = logger_get_route_name();
route_path = log_root + "/" + route_name;
init_data = logger_build_init_data();
}
static LoggerHandle* logger_open(LoggerState *s, const char* root_path) {
LoggerHandle *h = NULL;
for (int i=0; i<LOGGER_MAX_HANDLES; i++) {
if (s->handles[i].refcnt == 0) {
h = &s->handles[i];
break;
}
LoggerState::~LoggerState() {
if (rlog) {
log_sentinel(this, SentinelType::END_OF_ROUTE, exit_signal);
std::remove(lock_file.c_str());
}
assert(h);
snprintf(h->segment_path, sizeof(h->segment_path),
"%s/%s--%d", root_path, s->route_name.c_str(), s->part);
snprintf(h->log_path, sizeof(h->log_path), "%s/rlog", h->segment_path);
snprintf(h->qlog_path, sizeof(h->qlog_path), "%s/qlog", h->segment_path);
snprintf(h->lock_path, sizeof(h->lock_path), "%s.lock", h->log_path);
h->end_sentinel_type = SentinelType::END_OF_SEGMENT;
h->exit_signal = 0;
if (!util::create_directories(h->segment_path, 0775)) return nullptr;
FILE* lock_file = fopen(h->lock_path, "wb");
if (lock_file == NULL) return NULL;
fclose(lock_file);
h->log = std::make_unique<RawFile>(h->log_path);
if (s->has_qlog) {
h->q_log = std::make_unique<RawFile>(h->qlog_path);
}
pthread_mutex_init(&h->lock, NULL);
h->refcnt++;
return h;
}
int logger_next(LoggerState *s, const char* root_path,
char* out_segment_path, size_t out_segment_path_len,
int* out_part) {
bool is_start_of_route = !s->cur_handle;
pthread_mutex_lock(&s->lock);
s->part++;
LoggerHandle* next_h = logger_open(s, root_path);
if (!next_h) {
pthread_mutex_unlock(&s->lock);
return -1;
bool LoggerState::next() {
if (rlog) {
log_sentinel(this, SentinelType::END_OF_SEGMENT);
std::remove(lock_file.c_str());
}
if (s->cur_handle) {
lh_close(s->cur_handle);
}
s->cur_handle = next_h;
segment_path = route_path + "--" + std::to_string(++part);
bool ret = util::create_directories(segment_path, 0775);
assert(ret == true);
if (out_segment_path) {
snprintf(out_segment_path, out_segment_path_len, "%s", next_h->segment_path);
}
if (out_part) {
*out_part = s->part;
}
const std::string rlog_path = segment_path + "/rlog";
lock_file = rlog_path + ".lock";
std::ofstream{lock_file};
pthread_mutex_unlock(&s->lock);
rlog.reset(new RawFile(rlog_path));
qlog.reset(new RawFile(segment_path + "/qlog"));
// write beginning of log metadata
log_init_data(s);
lh_log_sentinel(s->cur_handle, is_start_of_route ? SentinelType::START_OF_ROUTE : SentinelType::START_OF_SEGMENT);
return 0;
// log init data & sentinel type.
write(init_data.asBytes(), true);
log_sentinel(this, part > 0 ? SentinelType::START_OF_SEGMENT : SentinelType::START_OF_ROUTE);
return true;
}
LoggerHandle* logger_get_handle(LoggerState *s) {
pthread_mutex_lock(&s->lock);
LoggerHandle* h = s->cur_handle;
if (h) {
pthread_mutex_lock(&h->lock);
h->refcnt++;
pthread_mutex_unlock(&h->lock);
}
pthread_mutex_unlock(&s->lock);
return h;
}
void logger_log(LoggerState *s, uint8_t* data, size_t data_size, bool in_qlog) {
pthread_mutex_lock(&s->lock);
if (s->cur_handle) {
lh_log(s->cur_handle, data, data_size, in_qlog);
}
pthread_mutex_unlock(&s->lock);
}
void logger_close(LoggerState *s, ExitHandler *exit_handler) {
pthread_mutex_lock(&s->lock);
if (s->cur_handle) {
s->cur_handle->exit_signal = exit_handler && exit_handler->signal.load();
s->cur_handle->end_sentinel_type = SentinelType::END_OF_ROUTE;
lh_close(s->cur_handle);
}
pthread_mutex_unlock(&s->lock);
}
void lh_log(LoggerHandle* h, uint8_t* data, size_t data_size, bool in_qlog) {
pthread_mutex_lock(&h->lock);
assert(h->refcnt > 0);
h->log->write(data, data_size);
if (in_qlog && h->q_log) {
h->q_log->write(data, data_size);
}
pthread_mutex_unlock(&h->lock);
}
void lh_close(LoggerHandle* h) {
pthread_mutex_lock(&h->lock);
assert(h->refcnt > 0);
if (h->refcnt == 1) {
// a very ugly hack. only here can guarantee sentinel is the last msg
pthread_mutex_unlock(&h->lock);
lh_log_sentinel(h, h->end_sentinel_type);
pthread_mutex_lock(&h->lock);
}
h->refcnt--;
if (h->refcnt == 0) {
h->log.reset(nullptr);
h->q_log.reset(nullptr);
unlink(h->lock_path);
pthread_mutex_unlock(&h->lock);
pthread_mutex_destroy(&h->lock);
return;
}
pthread_mutex_unlock(&h->lock);
void LoggerState::write(uint8_t* data, size_t size, bool in_qlog) {
rlog->write(data, size);
if (in_qlog) qlog->write(data, size);
}
system/loggerd/logger.h
+19 -43
View File
@@ -1,27 +1,17 @@
#pragma once
#include <pthread.h>
#include <cassert>
#include <cstdint>
#include <cstdio>
#include <memory>
#include <string>
#include <capnp/serialize.h>
#include <kj/array.h>
#include "cereal/messaging/messaging.h"
#include "common/util.h"
#include "common/swaglog.h"
#include "system/hardware/hw.h"
#define LOGGER_MAX_HANDLES 16
class RawFile {
public:
RawFile(const char* path) {
file = util::safe_fopen(path, "wb");
RawFile(const std::string &path) {
file = util::safe_fopen(path.c_str(), "wb");
assert(file != nullptr);
}
~RawFile() {
@@ -41,39 +31,25 @@ class RawFile {
typedef cereal::Sentinel::SentinelType SentinelType;
typedef struct LoggerHandle {
pthread_mutex_t lock;
SentinelType end_sentinel_type;
int exit_signal;
int refcnt;
char segment_path[4096];
char log_path[4096];
char qlog_path[4096];
char lock_path[4096];
std::unique_ptr<RawFile> log, q_log;
} LoggerHandle;
typedef struct LoggerState {
pthread_mutex_t lock;
int part;
class LoggerState {
public:
LoggerState(const std::string& log_root = Path::log_root());
~LoggerState();
bool next();
void write(uint8_t* data, size_t size, bool in_qlog);
inline int segment() const { return part; }
inline const std::string& segmentPath() const { return segment_path; }
inline const std::string& routeName() const { return route_name; }
inline void write(kj::ArrayPtr<kj::byte> bytes, bool in_qlog) { write(bytes.begin(), bytes.size(), in_qlog); }
inline void setExitSignal(int signal) { exit_signal = signal; }
protected:
int part = -1, exit_signal = 0;
std::string route_path, route_name, segment_path, lock_file;
kj::Array<capnp::word> init_data;
std::string route_name;
char log_name[64];
bool has_qlog;
LoggerHandle handles[LOGGER_MAX_HANDLES];
LoggerHandle* cur_handle;
} LoggerState;
std::unique_ptr<RawFile> rlog, qlog;
};
kj::Array<capnp::word> logger_build_init_data();
std::string logger_get_route_name();
void logger_init(LoggerState *s, bool has_qlog);
int logger_next(LoggerState *s, const char* root_path,
char* out_segment_path, size_t out_segment_path_len,
int* out_part);
LoggerHandle* logger_get_handle(LoggerState *s);
void logger_close(LoggerState *s, ExitHandler *exit_handler=nullptr);
void logger_log(LoggerState *s, uint8_t* data, size_t data_size, bool in_qlog);
void lh_log(LoggerHandle* h, uint8_t* data, size_t data_size, bool in_qlog);
void lh_close(LoggerHandle* h);
system/loggerd/loggerd.cc
+30 -28
View File
@@ -6,6 +6,7 @@
#include <unordered_map>
#include <vector>
#include "common/params.h"
#include "system/loggerd/encoder/encoder.h"
#include "system/loggerd/loggerd.h"
#include "system/loggerd/video_writer.h"
@@ -13,9 +14,7 @@
ExitHandler do_exit;
struct LoggerdState {
LoggerState logger = {};
char segment_path[4096];
std::atomic<int> rotate_segment;
LoggerState logger;
std::atomic<double> last_camera_seen_tms;
std::atomic<int> ready_to_rotate; // count of encoders ready to rotate
int max_waiting = 0;
@@ -23,13 +22,11 @@ struct LoggerdState {
};
void logger_rotate(LoggerdState *s) {
int segment = -1;
int err = logger_next(&s->logger, Path::log_root().c_str(), s->segment_path, sizeof(s->segment_path), &segment);
assert(err == 0);
s->rotate_segment = segment;
bool ret =s->logger.next();
assert(ret);
s->ready_to_rotate = 0;
s->last_rotate_tms = millis_since_boot();
LOGW((s->logger.part == 0) ? "logging to %s" : "rotated to %s", s->segment_path);
LOGW((s->logger.segment() == 0) ? "logging to %s" : "rotated to %s", s->logger.segmentPath().c_str());
}
void rotate_if_needed(LoggerdState *s) {
@@ -85,16 +82,16 @@ int handle_encoder_msg(LoggerdState *s, Message *msg, std::string &name, struct
}
int offset_segment_num = idx.getSegmentNum() - re.encoderd_segment_offset;
if (offset_segment_num == s->rotate_segment) {
if (offset_segment_num == s->logger.segment()) {
// loggerd is now on the segment that matches this packet
// if this is a new segment, we close any possible old segments, move to the new, and process any queued packets
if (re.current_segment != s->rotate_segment) {
if (re.current_segment != s->logger.segment()) {
if (re.recording) {
re.writer.reset();
re.recording = false;
}
re.current_segment = s->rotate_segment;
re.current_segment = s->logger.segment();
re.marked_ready_to_rotate = false;
// we are in this segment now, process any queued messages before this one
if (!re.q.empty()) {
@@ -117,7 +114,7 @@ int handle_encoder_msg(LoggerdState *s, Message *msg, std::string &name, struct
// if we aren't actually recording, don't create the writer
if (encoder_info.record) {
assert(encoder_info.filename != NULL);
re.writer.reset(new VideoWriter(s->segment_path,
re.writer.reset(new VideoWriter(s->logger.segmentPath().c_str(),
encoder_info.filename, idx.getType() != cereal::EncodeIndex::Type::FULL_H_E_V_C,
encoder_info.frame_width, encoder_info.frame_height, encoder_info.fps, idx.getType()));
// write the header
@@ -149,28 +146,28 @@ int handle_encoder_msg(LoggerdState *s, Message *msg, std::string &name, struct
evt.setLogMonoTime(event.getLogMonoTime());
(evt.*(encoder_info.set_encode_idx_func))(idx);
auto new_msg = bmsg.toBytes();
logger_log(&s->logger, (uint8_t *)new_msg.begin(), new_msg.size(), true); // always in qlog?
s->logger.write((uint8_t *)new_msg.begin(), new_msg.size(), true); // always in qlog?
bytes_count += new_msg.size();
// free the message, we used it
delete msg;
} else if (offset_segment_num > s->rotate_segment) {
} else if (offset_segment_num > s->logger.segment()) {
// encoderd packet has a newer segment, this means encoderd has rolled over
if (!re.marked_ready_to_rotate) {
re.marked_ready_to_rotate = true;
++s->ready_to_rotate;
LOGD("rotate %d -> %d ready %d/%d for %s",
s->rotate_segment.load(), offset_segment_num,
s->logger.segment(), offset_segment_num,
s->ready_to_rotate.load(), s->max_waiting, name.c_str());
}
// queue up all the new segment messages, they go in after the rotate
re.q.push_back(msg);
} else {
LOGE("%s: encoderd packet has a older segment!!! idx.getSegmentNum():%d s->rotate_segment:%d re.encoderd_segment_offset:%d",
name.c_str(), idx.getSegmentNum(), s->rotate_segment.load(), re.encoderd_segment_offset);
LOGE("%s: encoderd packet has a older segment!!! idx.getSegmentNum():%d s->logger.segment():%d re.encoderd_segment_offset:%d",
name.c_str(), idx.getSegmentNum(), s->logger.segment(), re.encoderd_segment_offset);
// free the message, it's useless. this should never happen
// actually, this can happen if you restart encoderd
re.encoderd_segment_offset = -s->rotate_segment.load();
re.encoderd_segment_offset = -s->logger.segment();
delete msg;
}
@@ -179,19 +176,25 @@ int handle_encoder_msg(LoggerdState *s, Message *msg, std::string &name, struct
void handle_user_flag(LoggerdState *s) {
static int prev_segment = -1;
if (s->rotate_segment == prev_segment) return;
if (s->logger.segment() == prev_segment) return;
LOGW("preserving %s", s->segment_path);
LOGW("preserving %s", s->logger.segmentPath().c_str());
#ifdef __APPLE__
int ret = setxattr(s->segment_path, PRESERVE_ATTR_NAME, &PRESERVE_ATTR_VALUE, 1, 0, 0);
int ret = setxattr(s->logger.segmentPath().c_str(), PRESERVE_ATTR_NAME, &PRESERVE_ATTR_VALUE, 1, 0, 0);
#else
int ret = setxattr(s->segment_path, PRESERVE_ATTR_NAME, &PRESERVE_ATTR_VALUE, 1, 0);
int ret = setxattr(s->logger.segmentPath().c_str(), PRESERVE_ATTR_NAME, &PRESERVE_ATTR_VALUE, 1, 0);
#endif
if (ret) {
LOGE("setxattr %s failed for %s: %s", PRESERVE_ATTR_NAME, s->segment_path, strerror(errno));
LOGE("setxattr %s failed for %s: %s", PRESERVE_ATTR_NAME, s->logger.segmentPath().c_str(), strerror(errno));
}
prev_segment = s->rotate_segment.load();
// mark route for uploading
Params params;
std::string routes = Params().get("AthenadRecentlyViewedRoutes");
params.put("AthenadRecentlyViewedRoutes", routes + "," + s->logger.routeName());
prev_segment = s->logger.segment();
}
void loggerd_thread() {
@@ -228,9 +231,8 @@ void loggerd_thread() {
LoggerdState s;
// init logger
logger_init(&s.logger, true);
logger_rotate(&s);
Params().put("CurrentRoute", s.logger.route_name);
Params().put("CurrentRoute", s.logger.routeName());
std::map<std::string, EncoderInfo> encoder_infos_dict;
for (const auto &cam : cameras_logged) {
@@ -261,7 +263,7 @@ void loggerd_thread() {
s.last_camera_seen_tms = millis_since_boot();
bytes_count += handle_encoder_msg(&s, msg, service.name, remote_encoders[sock], encoder_infos_dict[service.name]);
} else {
logger_log(&s.logger, (uint8_t *)msg->getData(), msg->getSize(), in_qlog);
s.logger.write((uint8_t *)msg->getData(), msg->getSize(), in_qlog);
bytes_count += msg->getSize();
delete msg;
}
@@ -283,7 +285,7 @@ void loggerd_thread() {
}
LOGW("closing logger");
logger_close(&s.logger, &do_exit);
s.logger.setExitSignal(do_exit.signal);
if (do_exit.power_failure) {
LOGE("power failure");
system/loggerd/tests/fill.py
-27
View File
@@ -1,27 +0,0 @@
#!/usr/bin/env python3
"""Script to fill up storage with fake data"""
from pathlib import Path
from openpilot.system.hardware.hw import Paths
from openpilot.system.loggerd.config import get_available_percent
from openpilot.system.loggerd.tests.loggerd_tests_common import create_random_file
if __name__ == "__main__":
segment_idx = 0
while True:
seg_name = f"1970-01-01--00-00-00--{segment_idx}"
seg_path = Path(Paths.log_root()) / seg_name
print(seg_path)
create_random_file(seg_path / "fcamera.hevc", 36)
create_random_file(seg_path / "rlog.bz2", 2)
segment_idx += 1
# Fill up to 99 percent
available_percent = get_available_percent()
if available_percent < 1.0:
break
system/loggerd/tests/loggerd_tests_common.py
+7 -21
View File
@@ -3,10 +3,12 @@ import random
import unittest
from pathlib import Path
from typing import Optional
from openpilot.system.hardware.hw import Paths
import openpilot.system.loggerd.deleter as deleter
import openpilot.system.loggerd.uploader as uploader
from openpilot.common.params import Params
from openpilot.system.hardware.hw import Paths
from openpilot.system.loggerd.xattr_cache import setxattr
@@ -53,25 +55,6 @@ class MockApiIgnore():
def get_token(self):
return "fake-token"
class MockParams():
def __init__(self):
self.params = {
"DongleId": b"0000000000000000",
"IsOffroad": b"1",
}
def get(self, k, block=False, encoding=None):
val = self.params[k]
if encoding is not None:
return val.decode(encoding)
else:
return val
def get_bool(self, k):
val = self.params[k]
return (val == b'1')
class UploaderTestCase(unittest.TestCase):
f_type = "UNKNOWN"
@@ -86,7 +69,6 @@ class UploaderTestCase(unittest.TestCase):
def setUp(self):
uploader.Api = MockApi
uploader.Params = MockParams
uploader.fake_upload = True
uploader.force_wifi = True
uploader.allow_sleep = False
@@ -95,6 +77,10 @@ class UploaderTestCase(unittest.TestCase):
self.seg_format2 = "2019-05-18--11-22-33--{}"
self.seg_dir = self.seg_format.format(self.seg_num)
self.params = Params()
self.params.put("IsOffroad", "1")
self.params.put("DongleId", "0000000000000000")
def make_file_with_data(self, f_dir: str, fn: str, size_mb: float = .1, lock: bool = False,
upload_xattr: Optional[bytes] = None, preserve_xattr: Optional[bytes] = None) -> Path:
file_path = Path(Paths.log_root()) / f_dir / fn
system/loggerd/tests/test_logger.cc
+14 -87
View File
@@ -1,16 +1,5 @@
#include <sys/stat.h>
#include <climits>
#include <condition_variable>
#include <sstream>
#include <thread>
#include <utility>
#include "catch2/catch.hpp"
#include "cereal/messaging/messaging.h"
#include "common/util.h"
#include "system/loggerd/logger.h"
#include "tools/replay/util.h"
typedef cereal::Sentinel::SentinelType SentinelType;
@@ -57,91 +46,29 @@ void verify_segment(const std::string &route_path, int segment, int max_segment,
}
}
void write_msg(LoggerHandle *logger) {
void write_msg(LoggerState *logger) {
MessageBuilder msg;
msg.initEvent().initClocks();
auto bytes = msg.toBytes();
lh_log(logger, bytes.begin(), bytes.size(), true);
logger->write(msg.toBytes(), true);
}
TEST_CASE("logger") {
const int segment_cnt = 100;
const std::string log_root = "/tmp/test_logger";
system(("rm " + log_root + " -rf").c_str());
ExitHandler do_exit;
LoggerState logger = {};
logger_init(&logger, true);
char segment_path[PATH_MAX] = {};
int segment = -1;
SECTION("single thread logging & rotation(100 segments, one thread)") {
const int segment_cnt = 100;
std::string route_name;
{
LoggerState logger(log_root);
route_name = logger.routeName();
for (int i = 0; i < segment_cnt; ++i) {
REQUIRE(logger_next(&logger, log_root.c_str(), segment_path, sizeof(segment_path), &segment) == 0);
REQUIRE(util::file_exists(std::string(segment_path) + "/rlog.lock"));
REQUIRE(segment == i);
write_msg(logger.cur_handle);
}
do_exit = true;
do_exit.signal = 1;
logger_close(&logger, &do_exit);
for (int i = 0; i < segment_cnt; ++i) {
verify_segment(log_root + "/" + logger.route_name, i, segment_cnt, 1);
REQUIRE(logger.next());
REQUIRE(util::file_exists(logger.segmentPath() + "/rlog.lock"));
REQUIRE(logger.segment() == i);
write_msg(&logger);
}
logger.setExitSignal(1);
}
SECTION("multiple threads logging & rotation(100 segments, 10 threads") {
const int segment_cnt = 100, thread_cnt = 10;
std::atomic<int> event_cnt[segment_cnt] = {};
std::atomic<int> main_segment = -1;
auto logging_thread = [&]() -> void {
LoggerHandle *lh = logger_get_handle(&logger);
assert(lh != nullptr);
int segment = main_segment;
int delayed_cnt = 0;
while (!do_exit) {
// write 2 more messages in the current segment and then rotate to the new segment.
if (main_segment > segment && ++delayed_cnt == 2) {
lh_close(lh);
lh = logger_get_handle(&logger);
segment = main_segment;
delayed_cnt = 0;
}
write_msg(lh);
event_cnt[segment] += 1;
usleep(1);
}
lh_close(lh);
};
// start logging
std::vector<std::thread> threads;
for (int i = 0; i < segment_cnt; ++i) {
REQUIRE(logger_next(&logger, log_root.c_str(), segment_path, sizeof(segment_path), &segment) == 0);
REQUIRE(segment == i);
main_segment = segment;
if (i == 0) {
for (int j = 0; j < thread_cnt; ++j) {
threads.push_back(std::thread(logging_thread));
}
}
for (int j = 0; j < 100; ++j) {
write_msg(logger.cur_handle);
usleep(1);
}
event_cnt[segment] += 100;
}
// end logging
for (auto &t : threads) t.join();
do_exit = true;
do_exit.signal = 1;
logger_close(&logger, &do_exit);
REQUIRE(logger.cur_handle->refcnt == 0);
for (int i = 0; i < segment_cnt; ++i) {
verify_segment(log_root + "/" + logger.route_name, i, segment_cnt, event_cnt[i]);
}
for (int i = 0; i < segment_cnt; ++i) {
verify_segment(log_root + "/" + route_name, i, segment_cnt, 1);
}
}
system/loggerd/tests/test_loggerd.py
+27 -34
View File
@@ -5,7 +5,6 @@ import random
import string
import subprocess
import time
import unittest
from collections import defaultdict
from pathlib import Path
from typing import Dict, List
@@ -31,10 +30,7 @@ CEREAL_SERVICES = [f for f in log.Event.schema.union_fields if f in SERVICE_LIST
and SERVICE_LIST[f].should_log and "encode" not in f.lower()]
class TestLoggerd(unittest.TestCase):
def setUp(self):
os.environ.pop("LOG_ROOT", None)
class TestLoggerd:
def _get_latest_log_dir(self):
log_dirs = sorted(Path(Paths.log_root()).iterdir(), key=lambda f: f.stat().st_mtime)
return log_dirs[-1]
@@ -68,21 +64,21 @@ class TestLoggerd(unittest.TestCase):
def _check_init_data(self, msgs):
msg = msgs[0]
self.assertEqual(msg.which(), 'initData')
assert msg.which() == 'initData'
def _check_sentinel(self, msgs, route):
start_type = SentinelType.startOfRoute if route else SentinelType.startOfSegment
self.assertTrue(msgs[1].sentinel.type == start_type)
assert msgs[1].sentinel.type == start_type
end_type = SentinelType.endOfRoute if route else SentinelType.endOfSegment
self.assertTrue(msgs[-1].sentinel.type == end_type)
assert msgs[-1].sentinel.type == end_type
def _publish_random_messages(self, services: List[str]) -> Dict[str, list]:
pm = messaging.PubMaster(services)
managed_processes["loggerd"].start()
for s in services:
self.assertTrue(pm.wait_for_readers_to_update(s, timeout=5))
assert pm.wait_for_readers_to_update(s, timeout=5)
sent_msgs = defaultdict(list)
for _ in range(random.randint(2, 10) * 100):
@@ -93,10 +89,9 @@ class TestLoggerd(unittest.TestCase):
m = messaging.new_message(s, random.randint(2, 10))
pm.send(s, m)
sent_msgs[s].append(m)
time.sleep(0.01)
for s in services:
self.assertTrue(pm.wait_for_readers_to_update(s, timeout=5))
assert pm.wait_for_readers_to_update(s, timeout=5)
managed_processes["loggerd"].stop()
return sent_msgs
@@ -116,31 +111,31 @@ class TestLoggerd(unittest.TestCase):
params.clear_all()
for k, _, v in fake_params:
params.put(k, v)
params.put("LaikadEphemerisV3", "abc")
params.put("AccessToken", "abc")
lr = list(LogReader(str(self._gen_bootlog())))
initData = lr[0].initData
self.assertTrue(initData.dirty != bool(os.environ["CLEAN"]))
self.assertEqual(initData.version, get_version())
assert initData.dirty != bool(os.environ["CLEAN"])
assert initData.version == get_version()
if os.path.isfile("/proc/cmdline"):
with open("/proc/cmdline") as f:
self.assertEqual(list(initData.kernelArgs), f.read().strip().split(" "))
assert list(initData.kernelArgs) == f.read().strip().split(" ")
with open("/proc/version") as f:
self.assertEqual(initData.kernelVersion, f.read())
assert initData.kernelVersion == f.read()
# check params
logged_params = {entry.key: entry.value for entry in initData.params.entries}
expected_params = {k for k, _, __ in fake_params} | {'LaikadEphemerisV3'}
expected_params = {k for k, _, __ in fake_params} | {'AccessToken'}
assert set(logged_params.keys()) == expected_params, set(logged_params.keys()) ^ expected_params
assert logged_params['LaikadEphemerisV3'] == b'', f"DONT_LOG param value was logged: {repr(logged_params['LaikadEphemerisV3'])}"
assert logged_params['AccessToken'] == b'', f"DONT_LOG param value was logged: {repr(logged_params['AccessToken'])}"
for param_key, initData_key, v in fake_params:
self.assertEqual(getattr(initData, initData_key), v)
self.assertEqual(logged_params[param_key].decode(), v)
assert getattr(initData, initData_key) == v
assert logged_params[param_key].decode() == v
params.put("LaikadEphemerisV3", "")
params.put("AccessToken", "")
def test_rotation(self):
os.environ["LOGGERD_TEST"] = "1"
@@ -162,11 +157,10 @@ class TestLoggerd(unittest.TestCase):
os.environ["LOGGERD_SEGMENT_LENGTH"] = str(length)
managed_processes["loggerd"].start()
managed_processes["encoderd"].start()
time.sleep(1)
assert pm.wait_for_readers_to_update("roadCameraState", timeout=5)
fps = 20.0
for n in range(1, int(num_segs*length*fps)+1):
time_start = time.monotonic()
for stream_type, frame_spec, state in streams:
dat = np.empty(frame_spec[2], dtype=np.uint8)
vipc_server.send(stream_type, dat[:].flatten().tobytes(), n, n/fps, n/fps)
@@ -175,7 +169,9 @@ class TestLoggerd(unittest.TestCase):
frame = getattr(camera_state, state)
frame.frameId = n
pm.send(state, camera_state)
time.sleep(max((1.0/fps) - (time.monotonic() - time_start), 0))
for _, _, state in streams:
assert pm.wait_for_readers_to_update(state, timeout=5, dt=0.001)
managed_processes["loggerd"].stop()
managed_processes["encoderd"].stop()
@@ -185,7 +181,7 @@ class TestLoggerd(unittest.TestCase):
p = Path(f"{route_path}--{n}")
logged = {f.name for f in p.iterdir() if f.is_file()}
diff = logged ^ expected_files
self.assertEqual(len(diff), 0, f"didn't get all expected files. run={_} seg={n} {route_path=}, {diff=}\n{logged=} {expected_files=}")
assert len(diff) == 0, f"didn't get all expected files. run={_} seg={n} {route_path=}, {diff=}\n{logged=} {expected_files=}"
def test_bootlog(self):
# generate bootlog with fake launch log
@@ -216,7 +212,7 @@ class TestLoggerd(unittest.TestCase):
with open(path, "rb") as f:
expected_val = f.read()
bootlog_val = [e.value for e in boot.pstore.entries if e.key == fn][0]
self.assertEqual(expected_val, bootlog_val)
assert expected_val == bootlog_val
def test_qlog(self):
qlog_services = [s for s in CEREAL_SERVICES if SERVICE_LIST[s].decimation is not None]
@@ -242,11 +238,11 @@ class TestLoggerd(unittest.TestCase):
if s in no_qlog_services:
# check services with no specific decimation aren't in qlog
self.assertEqual(recv_cnt, 0, f"got {recv_cnt} {s} msgs in qlog")
assert recv_cnt == 0, f"got {recv_cnt} {s} msgs in qlog"
else:
# check logged message count matches decimation
expected_cnt = (len(msgs) - 1) // SERVICE_LIST[s].decimation + 1
self.assertEqual(recv_cnt, expected_cnt, f"expected {expected_cnt} msgs for {s}, got {recv_cnt}")
assert recv_cnt == expected_cnt, f"expected {expected_cnt} msgs for {s}, got {recv_cnt}"
def test_rlog(self):
services = random.sample(CEREAL_SERVICES, random.randint(5, 10))
@@ -263,22 +259,19 @@ class TestLoggerd(unittest.TestCase):
for m in lr:
sent = sent_msgs[m.which()].pop(0)
sent.clear_write_flag()
self.assertEqual(sent.to_bytes(), m.as_builder().to_bytes())
assert sent.to_bytes() == m.as_builder().to_bytes()
def test_preserving_flagged_segments(self):
services = set(random.sample(CEREAL_SERVICES, random.randint(5, 10))) | {"userFlag"}
self._publish_random_messages(services)
segment_dir = self._get_latest_log_dir()
self.assertEqual(getxattr(segment_dir, PRESERVE_ATTR_NAME), PRESERVE_ATTR_VALUE)
assert getxattr(segment_dir, PRESERVE_ATTR_NAME) == PRESERVE_ATTR_VALUE
def test_not_preserving_unflagged_segments(self):
services = set(random.sample(CEREAL_SERVICES, random.randint(5, 10))) - {"userFlag"}
self._publish_random_messages(services)
segment_dir = self._get_latest_log_dir()
self.assertIsNone(getxattr(segment_dir, PRESERVE_ATTR_NAME))
assert getxattr(segment_dir, PRESERVE_ATTR_NAME) is None
if __name__ == "__main__":
unittest.main()
system/loggerd/tests/test_uploader.py
+3 -3
View File
@@ -9,8 +9,8 @@ from pathlib import Path
from typing import List, Optional
from openpilot.system.hardware.hw import Paths
from openpilot.system.swaglog import cloudlog
from openpilot.system.loggerd.uploader import uploader_fn, UPLOAD_ATTR_NAME, UPLOAD_ATTR_VALUE
from openpilot.common.swaglog import cloudlog
from openpilot.system.loggerd.uploader import main, UPLOAD_ATTR_NAME, UPLOAD_ATTR_VALUE
from openpilot.system.loggerd.tests.loggerd_tests_common import UploaderTestCase
@@ -45,7 +45,7 @@ class TestUploader(UploaderTestCase):
def start_thread(self):
self.end_event = threading.Event()
self.up_thread = threading.Thread(target=uploader_fn, args=[self.end_event])
self.up_thread = threading.Thread(target=main, args=[self.end_event])
self.up_thread.daemon = True
self.up_thread.start()
system/loggerd/tools/mark_all_uploaded.py
-8
View File
@@ -1,8 +0,0 @@
import os
from openpilot.system.hardware.hw import Paths
from openpilot.system.loggerd.uploader import UPLOAD_ATTR_NAME, UPLOAD_ATTR_VALUE
for folder in os.walk(Paths.log_root()):
for file1 in folder[2]:
full_path = os.path.join(folder[0], file1)
os.setxattr(full_path, UPLOAD_ATTR_NAME, UPLOAD_ATTR_VALUE)
system/loggerd/tools/mark_unuploaded.py
-8
View File
@@ -1,8 +0,0 @@
#!/usr/bin/env python3
import os
import sys
from openpilot.system.loggerd.uploader import UPLOAD_ATTR_NAME
for fn in sys.argv[1:]:
print(f"unmarking {fn}")
os.removexattr(fn, UPLOAD_ATTR_NAME)
system/loggerd/uploader.py
+83 -119
View File
@@ -8,24 +8,23 @@ import requests
import threading
import time
import traceback
from pathlib import Path
from typing import BinaryIO, Iterator, List, Optional, Tuple, Union
import datetime
from typing import BinaryIO, Iterator, List, Optional, Tuple
from cereal import log
import cereal.messaging as messaging
from openpilot.common.api import Api
from openpilot.common.params import Params
from openpilot.common.realtime import set_core_affinity
from openpilot.system.hardware import TICI
from openpilot.system.hardware.hw import Paths
from openpilot.system.loggerd.xattr_cache import getxattr, setxattr
from openpilot.system.swaglog import cloudlog
from openpilot.common.swaglog import cloudlog
NetworkType = log.DeviceState.NetworkType
UPLOAD_ATTR_NAME = 'user.upload'
UPLOAD_ATTR_VALUE = b'1'
UPLOAD_QLOG_QCAM_MAX_SIZE = 100 * 1e6 # MB
UPLOAD_QLOG_QCAM_MAX_SIZE = 5 * 1e6 # MB
allow_sleep = bool(os.getenv("UPLOADER_SLEEP", "1"))
force_wifi = os.getenv("FORCEWIFI") is not None
@@ -43,23 +42,24 @@ class FakeResponse:
self.request = FakeRequest()
UploadResponse = Union[requests.Response, FakeResponse]
def get_directory_sort(d: str) -> List[str]:
return [s.rjust(10, '0') for s in d.rsplit('--', 1)]
def listdir_by_creation(d: str) -> List[str]:
if not os.path.isdir(d):
return []
try:
paths = os.listdir(d)
paths = [f for f in os.listdir(d) if os.path.isdir(os.path.join(d, f))]
paths = sorted(paths, key=get_directory_sort)
return paths
except OSError:
cloudlog.exception("listdir_by_creation failed")
return list()
return []
def clear_locks(root: str) -> None:
for logname in os.listdir(root):
path = os.path.join(root, logname)
for logdir in os.listdir(root):
path = os.path.join(root, logdir)
try:
for fname in os.listdir(path):
if fname.endswith(".lock"):
@@ -74,34 +74,20 @@ class Uploader:
self.api = Api(dongle_id)
self.root = root
self.last_resp: Optional[UploadResponse] = None
self.last_exc: Optional[Tuple[Exception, str]] = None
self.immediate_size = 0
self.immediate_count = 0
self.params = Params()
# stats for last successfully uploaded file
self.last_time = 0.0
self.last_speed = 0.0
self.last_filename = ""
self.immediate_folders = ["crash/", "boot/"]
self.immediate_priority = {"qlog": 0, "qlog.bz2": 0, "qcamera.ts": 1}
def get_upload_sort(self, name: str) -> int:
if name in self.immediate_priority:
return self.immediate_priority[name]
return 1000
def list_upload_files(self, metered: bool) -> Iterator[Tuple[str, str, str]]:
r = self.params.get("AthenadRecentlyViewedRoutes", encoding="utf8")
requested_routes = [] if r is None else r.split(",")
def list_upload_files(self) -> Iterator[Tuple[str, str, str]]:
if not os.path.isdir(self.root):
return
self.immediate_size = 0
self.immediate_count = 0
for logname in listdir_by_creation(self.root):
path = os.path.join(self.root, logname)
for logdir in listdir_by_creation(self.root):
path = os.path.join(self.root, logdir)
try:
names = os.listdir(path)
except OSError:
@@ -110,29 +96,33 @@ class Uploader:
if any(name.endswith(".lock") for name in names):
continue
for name in sorted(names, key=self.get_upload_sort):
key = os.path.join(logname, name)
for name in sorted(names, key=lambda n: self.immediate_priority.get(n, 1000)):
key = os.path.join(logdir, name)
fn = os.path.join(path, name)
# skip files already uploaded
try:
ctime = os.path.getctime(fn)
is_uploaded = getxattr(fn, UPLOAD_ATTR_NAME) == UPLOAD_ATTR_VALUE
except OSError:
cloudlog.event("uploader_getxattr_failed", exc=self.last_exc, key=key, fn=fn)
is_uploaded = True # deleter could have deleted
cloudlog.event("uploader_getxattr_failed", key=key, fn=fn)
# deleter could have deleted, so skip
continue
if is_uploaded:
continue
try:
if name in self.immediate_priority:
self.immediate_count += 1
self.immediate_size += os.path.getsize(fn)
except OSError:
pass
# limit uploading on metered connections
if metered:
dt = datetime.timedelta(hours=12)
if logdir in self.immediate_folders and (datetime.datetime.now() - datetime.datetime.fromtimestamp(ctime)) < dt:
continue
if name == "qcamera.ts" and not any(logdir.startswith(r.split('|')[-1]) for r in requested_routes):
continue
yield name, key, fn
def next_file_to_upload(self) -> Optional[Tuple[str, str, str]]:
upload_files = list(self.list_upload_files())
def next_file_to_upload(self, metered: bool) -> Optional[Tuple[str, str, str]]:
upload_files = list(self.list_upload_files(metered))
for name, key, fn in upload_files:
if any(f in fn for f in self.immediate_folders):
@@ -144,45 +134,28 @@ class Uploader:
return None
def do_upload(self, key: str, fn: str) -> None:
try:
url_resp = self.api.get("v1.4/" + self.dongle_id + "/upload_url/", timeout=10, path=key, access_token=self.api.get_token())
if url_resp.status_code == 412:
self.last_resp = url_resp
return
def do_upload(self, key: str, fn: str):
url_resp = self.api.get("v1.4/" + self.dongle_id + "/upload_url/", timeout=10, path=key, access_token=self.api.get_token())
if url_resp.status_code == 412:
return url_resp
url_resp_json = json.loads(url_resp.text)
url = url_resp_json['url']
headers = url_resp_json['headers']
cloudlog.debug("upload_url v1.4 %s %s", url, str(headers))
url_resp_json = json.loads(url_resp.text)
url = url_resp_json['url']
headers = url_resp_json['headers']
cloudlog.debug("upload_url v1.4 %s %s", url, str(headers))
if fake_upload:
cloudlog.debug(f"*** WARNING, THIS IS A FAKE UPLOAD TO {url} ***")
self.last_resp = FakeResponse()
if fake_upload:
return FakeResponse()
with open(fn, "rb") as f:
data: BinaryIO
if key.endswith('.bz2') and not fn.endswith('.bz2'):
compressed = bz2.compress(f.read())
data = io.BytesIO(compressed)
else:
with open(fn, "rb") as f:
data: BinaryIO
if key.endswith('.bz2') and not fn.endswith('.bz2'):
compressed = bz2.compress(f.read())
data = io.BytesIO(compressed)
else:
data = f
data = f
self.last_resp = requests.put(url, data=data, headers=headers, timeout=10)
except Exception as e:
self.last_exc = (e, traceback.format_exc())
raise
def normal_upload(self, key: str, fn: str) -> Optional[UploadResponse]:
self.last_resp = None
self.last_exc = None
try:
self.do_upload(key, fn)
except Exception:
pass
return self.last_resp
return requests.put(url, data=data, headers=headers, timeout=10)
def upload(self, name: str, key: str, fn: str, network_type: int, metered: bool) -> bool:
try:
@@ -201,44 +174,57 @@ class Uploader:
success = True
else:
start_time = time.monotonic()
stat = self.normal_upload(key, fn)
stat = None
last_exc = None
try:
stat = self.do_upload(key, fn)
except Exception as e:
last_exc = (e, traceback.format_exc())
if stat is not None and stat.status_code in (200, 201, 401, 403, 412):
self.last_filename = fn
self.last_time = time.monotonic() - start_time
dt = time.monotonic() - start_time
if stat.status_code == 412:
self.last_speed = 0
cloudlog.event("upload_ignored", key=key, fn=fn, sz=sz, network_type=network_type, metered=metered)
else:
content_length = int(stat.request.headers.get("Content-Length", 0))
self.last_speed = (content_length / 1e6) / self.last_time
speed = (content_length / 1e6) / dt
cloudlog.event("upload_success", key=key, fn=fn, sz=sz, content_length=content_length,
network_type=network_type, metered=metered, speed=self.last_speed)
network_type=network_type, metered=metered, speed=speed)
success = True
else:
success = False
cloudlog.event("upload_failed", stat=stat, exc=self.last_exc, key=key, fn=fn, sz=sz, network_type=network_type, metered=metered)
cloudlog.event("upload_failed", stat=stat, exc=last_exc, key=key, fn=fn, sz=sz, network_type=network_type, metered=metered)
if success:
# tag file as uploaded
try:
setxattr(fn, UPLOAD_ATTR_NAME, UPLOAD_ATTR_VALUE)
except OSError:
cloudlog.event("uploader_setxattr_failed", exc=self.last_exc, key=key, fn=fn, sz=sz)
cloudlog.event("uploader_setxattr_failed", exc=last_exc, key=key, fn=fn, sz=sz)
return success
def get_msg(self):
msg = messaging.new_message("uploaderState")
us = msg.uploaderState
us.immediateQueueSize = int(self.immediate_size / 1e6)
us.immediateQueueCount = self.immediate_count
us.lastTime = self.last_time
us.lastSpeed = self.last_speed
us.lastFilename = self.last_filename
return msg
def step(self, network_type: int, metered: bool) -> bool:
d = self.next_file_to_upload(metered)
if d is None:
return True
name, key, fn = d
# qlogs and bootlogs need to be compressed before uploading
if key.endswith(('qlog', 'rlog')) or (key.startswith('boot/') and not key.endswith('.bz2')):
key += ".bz2"
return self.upload(name, key, fn, network_type, metered)
def uploader_fn(exit_event: threading.Event) -> None:
def main(exit_event: Optional[threading.Event] = None) -> None:
if exit_event is None:
exit_event = threading.Event()
try:
set_core_affinity([0, 1, 2, 3])
except Exception:
@@ -257,11 +243,7 @@ def uploader_fn(exit_event: threading.Event) -> None:
cloudlog.info("uploader missing dongle_id")
raise Exception("uploader can't start without dongle id")
if TICI and not Path("/data/media").is_mount():
cloudlog.warning("NVME not mounted")
sm = messaging.SubMaster(['deviceState'])
pm = messaging.PubMaster(['uploaderState'])
uploader = Uploader(dongle_id, Paths.log_root())
backoff = 0.1
@@ -297,31 +279,13 @@ def uploader_fn(exit_event: threading.Event) -> None:
time.sleep(60)
continue
d = uploader.next_file_to_upload()
if d is None: # Nothing to upload
if allow_sleep:
time.sleep(60 if offroad else 5)
continue
name, key, fn = d
# qlogs and bootlogs need to be compressed before uploading
if key.endswith(('qlog', 'rlog')) or (key.startswith('boot/') and not key.endswith('.bz2')):
key += ".bz2"
success = uploader.upload(name, key, fn, sm['deviceState'].networkType.raw, sm['deviceState'].networkMetered)
success = uploader.step(sm['deviceState'].networkType.raw, sm['deviceState'].networkMetered)
if success:
backoff = 0.1
elif allow_sleep:
cloudlog.info("upload backoff %r", backoff)
time.sleep(backoff + random.uniform(0, backoff))
backoff = min(backoff*2, 120)
pm.send("uploaderState", uploader.get_msg())
def main() -> None:
uploader_fn(threading.Event())
time.sleep(backoff + random.uniform(0, backoff))
if __name__ == "__main__":
system/logmessaged.py
+3 -5
View File
@@ -5,7 +5,7 @@ from typing import NoReturn
import cereal.messaging as messaging
from openpilot.common.logging_extra import SwagLogFileFormatter
from openpilot.system.hardware.hw import Paths
from openpilot.system.swaglog import get_file_handler
from openpilot.common.swaglog import get_file_handler
def main() -> NoReturn:
@@ -35,13 +35,11 @@ def main() -> NoReturn:
continue
# then we publish them
msg = messaging.new_message()
msg.logMessage = record
msg = messaging.new_message(None, valid=True, logMessage=record)
log_message_sock.send(msg.to_bytes())
if level >= 40: # logging.ERROR
msg = messaging.new_message()
msg.errorLogMessage = record
msg = messaging.new_message(None, valid=True, errorLogMessage=record)
error_log_message_sock.send(msg.to_bytes())
finally:
sock.close()
system/micd.py
+13 -10
View File
@@ -2,15 +2,15 @@
import numpy as np
from cereal import messaging
from openpilot.common.filter_simple import FirstOrderFilter
from openpilot.common.realtime import Ratekeeper
from openpilot.system.swaglog import cloudlog
from openpilot.common.retry import retry
from openpilot.common.swaglog import cloudlog
RATE = 10
FFT_SAMPLES = 4096
REFERENCE_SPL = 2e-5 # newtons/m^2
SAMPLE_RATE = 44100
FILTER_DT = 1. / (SAMPLE_RATE / FFT_SAMPLES)
SAMPLE_BUFFER = 4096 # (approx 100ms)
def calculate_spl(measurements):
@@ -50,15 +50,12 @@ class Mic:
self.sound_pressure_weighted = 0
self.sound_pressure_level_weighted = 0
self.spl_filter_weighted = FirstOrderFilter(0, 2.5, FILTER_DT, initialized=False)
def update(self):
msg = messaging.new_message('microphone')
msg = messaging.new_message('microphone', valid=True)
msg.microphone.soundPressure = float(self.sound_pressure)
msg.microphone.soundPressureWeighted = float(self.sound_pressure_weighted)
msg.microphone.soundPressureWeightedDb = float(self.sound_pressure_level_weighted)
msg.microphone.filteredSoundPressureWeightedDb = float(self.spl_filter_weighted.x)
self.pm.send('microphone', msg)
self.rk.keep_time()
@@ -79,16 +76,22 @@ class Mic:
self.sound_pressure, _ = calculate_spl(measurements)
measurements_weighted = apply_a_weighting(measurements)
self.sound_pressure_weighted, self.sound_pressure_level_weighted = calculate_spl(measurements_weighted)
self.spl_filter_weighted.update(self.sound_pressure_level_weighted)
self.measurements = self.measurements[FFT_SAMPLES:]
@retry(attempts=7, delay=3)
def get_stream(self, sd):
# reload sounddevice to reinitialize portaudio
sd._terminate()
sd._initialize()
return sd.InputStream(channels=1, samplerate=SAMPLE_RATE, callback=self.callback, blocksize=SAMPLE_BUFFER)
def micd_thread(self):
# sounddevice must be imported after forking processes
import sounddevice as sd
with sd.InputStream(channels=1, samplerate=SAMPLE_RATE, callback=self.callback) as stream:
cloudlog.info(f"micd stream started: {stream.samplerate=} {stream.channels=} {stream.dtype=} {stream.device=}")
with self.get_stream(sd) as stream:
cloudlog.info(f"micd stream started: {stream.samplerate=} {stream.channels=} {stream.dtype=} {stream.device=}, {stream.blocksize=}")
while True:
self.update()
system/proclogd/tests/test_proclog.cc
+1 -14
View File
@@ -109,7 +109,7 @@ TEST_CASE("Parser::cmdline") {
test_cmdline(std::string("a\0b\0c\0\0\0", 9), {"a", "b", "c"});
}
TEST_CASE("buildProcLogerMessage") {
TEST_CASE("buildProcLoggerMessage") {
MessageBuilder msg;
buildProcLogMessage(msg);
@@ -137,19 +137,6 @@ TEST_CASE("buildProcLogerMessage") {
REQUIRE(p.getState() == 'R');
REQUIRE_THAT(p.getExe().cStr(), Catch::Matchers::Contains("test_proclog"));
REQUIRE_THAT(p.getCmdline()[0], Catch::Matchers::Contains("test_proclog"));
} else {
std::string cmd_path = "/proc/" + std::to_string(p.getPid()) + "/cmdline";
if (util::file_exists(cmd_path)) {
std::ifstream stream(cmd_path);
auto cmdline = Parser::cmdline(stream);
REQUIRE(cmdline.size() == p.getCmdline().size());
// do not check the cmdline of pytest as it will change.
if (cmdline.size() > 0 && cmdline[0].find("[pytest") != 0) {
for (int i = 0; i < p.getCmdline().size(); ++i) {
REQUIRE(cmdline[i] == p.getCmdline()[i].cStr());
}
}
}
}
}
}
system/sensord/rawgps/modemdiag.py → system/qcomgpsd/modemdiag.py
View File
system/sensord/rawgps/nmeaport.py → system/qcomgpsd/nmeaport.py
+3 -3
View File
@@ -120,11 +120,11 @@ def process_nmea_port_messages(device:str="/dev/ttyUSB1") -> NoReturn:
def main() -> NoReturn:
from openpilot.common.gpio import gpio_init, gpio_set
from openpilot.system.hardware.tici.pins import GPIO
from openpilot.system.sensord.rawgps.rawgpsd import at_cmd
from openpilot.system.qcomgpsd.qcomgpsd import at_cmd
try:
check_output(["pidof", "rawgpsd"])
print("rawgpsd is running, please kill openpilot before running this script! (aborted)")
check_output(["pidof", "qcomgpsd"])
print("qcomgpsd is running, please kill openpilot before running this script! (aborted)")
sys.exit(1)
except CalledProcessError as e:
if e.returncode != 1: # 1 == no process found (boardd not running)
system/sensord/rawgps/rawgpsd.py → system/qcomgpsd/qcomgpsd.py
+41 -73
View File
@@ -5,10 +5,10 @@ import signal
import itertools
import math
import time
import pycurl
import requests
import shutil
import subprocess
from datetime import datetime
import datetime
from multiprocessing import Process, Event
from typing import NoReturn, Optional
from struct import unpack_from, calcsize, pack
@@ -16,13 +16,11 @@ from struct import unpack_from, calcsize, pack
from cereal import log
import cereal.messaging as messaging
from openpilot.common.gpio import gpio_init, gpio_set
from laika.gps_time import GPSTime, utc_to_gpst, get_leap_seconds
from laika.helpers import get_prn_from_nmea_id
from laika.constants import SECS_IN_HR, SECS_IN_DAY, SECS_IN_WEEK
from openpilot.common.retry import retry
from openpilot.system.hardware.tici.pins import GPIO
from openpilot.system.swaglog import cloudlog
from openpilot.system.sensord.rawgps.modemdiag import ModemDiag, DIAG_LOG_F, setup_logs, send_recv
from openpilot.system.sensord.rawgps.structs import (dict_unpacker, position_report, relist,
from openpilot.common.swaglog import cloudlog
from openpilot.system.qcomgpsd.modemdiag import ModemDiag, DIAG_LOG_F, setup_logs, send_recv
from openpilot.system.qcomgpsd.structs import (dict_unpacker, position_report, relist,
gps_measurement_report, gps_measurement_report_sv,
glonass_measurement_report, glonass_measurement_report_sv,
oemdre_measurement_report, oemdre_measurement_report_sv, oemdre_svpoly_report,
@@ -87,27 +85,13 @@ measurementStatusGlonassFields = {
"glonassTimeMarkValid": 17
}
@retry(attempts=10, delay=1.0)
def try_setup_logs(diag, logs):
return setup_logs(diag, logs)
def try_setup_logs(diag, log_types):
for _ in range(10):
try:
setup_logs(diag, log_types)
break
except Exception:
cloudlog.exception("setup logs failed, trying again")
time.sleep(1.0)
else:
raise Exception(f"setup logs failed, {log_types=}")
@retry(attempts=3, delay=1.0)
def at_cmd(cmd: str) -> Optional[str]:
for _ in range(3):
try:
return subprocess.check_output(f"mmcli -m any --timeout 30 --command='{cmd}'", shell=True, encoding='utf8')
except subprocess.CalledProcessError:
cloudlog.exception("rawgps.mmcli_command_failed")
time.sleep(1.0)
raise Exception(f"failed to execute mmcli command {cmd=}")
return subprocess.check_output(f"mmcli -m any --timeout 30 --command='{cmd}'", shell=True, encoding='utf8')
def gps_enabled() -> bool:
try:
@@ -118,27 +102,18 @@ def gps_enabled() -> bool:
def download_assistance():
try:
c = pycurl.Curl()
c.setopt(pycurl.URL, ASSISTANCE_URL)
c.setopt(pycurl.NOBODY, 1)
c.setopt(pycurl.CONNECTTIMEOUT, 2)
c.perform()
bytes_n = c.getinfo(pycurl.CONTENT_LENGTH_DOWNLOAD)
c.close()
if bytes_n > 1e5:
cloudlog.error("Qcom assistance data larger than expected")
return
response = requests.get(ASSISTANCE_URL, timeout=5, stream=True)
with open(ASSIST_DATA_FILE_DOWNLOAD, 'wb') as fp:
c = pycurl.Curl()
c.setopt(pycurl.URL, ASSISTANCE_URL)
c.setopt(pycurl.CONNECTTIMEOUT, 5)
for chunk in response.iter_content(chunk_size=8192):
fp.write(chunk)
if fp.tell() > 1e5:
cloudlog.error("Qcom assistance data larger than expected")
return
c.setopt(pycurl.WRITEDATA, fp)
c.perform()
c.close()
os.rename(ASSIST_DATA_FILE_DOWNLOAD, ASSIST_DATA_FILE)
except pycurl.error:
os.rename(ASSIST_DATA_FILE_DOWNLOAD, ASSIST_DATA_FILE)
except requests.exceptions.RequestException:
cloudlog.exception("Failed to download assistance file")
return
@@ -157,24 +132,13 @@ def downloader_loop(event):
except KeyboardInterrupt:
pass
@retry(attempts=5, delay=0.2, ignore_failure=True)
def inject_assistance():
for _ in range(5):
try:
cmd = f"mmcli -m any --timeout 30 --location-inject-assistance-data={ASSIST_DATA_FILE}"
subprocess.check_output(cmd, stderr=subprocess.PIPE, shell=True)
cloudlog.info("successfully loaded assistance data")
return
except subprocess.CalledProcessError as e:
cloudlog.event(
"rawgps.assistance_loading_failed",
error=True,
cmd=e.cmd,
output=e.output,
returncode=e.returncode
)
time.sleep(0.2)
cloudlog.error("failed to load assistance after retry")
cmd = f"mmcli -m any --timeout 30 --location-inject-assistance-data={ASSIST_DATA_FILE}"
subprocess.check_output(cmd, stderr=subprocess.PIPE, shell=True)
cloudlog.info("successfully loaded assistance data")
@retry(attempts=5, delay=1.0)
def setup_quectel(diag: ModemDiag) -> bool:
ret = False
@@ -211,7 +175,7 @@ def setup_quectel(diag: ModemDiag) -> bool:
inject_assistance()
os.remove(ASSIST_DATA_FILE)
#at_cmd("AT+QGPSXTRADATA?")
time_str = datetime.utcnow().strftime("%Y/%m/%d,%H:%M:%S")
time_str = datetime.datetime.utcnow().strftime("%Y/%m/%d,%H:%M:%S")
at_cmd(f"AT+QGPSXTRATIME=0,\"{time_str}\",1,1,1000")
at_cmd("AT+QGPSCFG=\"outport\",\"usbnmea\"")
@@ -293,7 +257,6 @@ def main() -> NoReturn:
diag = ModemDiag()
r = setup_quectel(diag)
want_assistance = not r
current_gps_time = utc_to_gpst(GPSTime.from_datetime(datetime.utcnow()))
cloudlog.warning("quectel setup done")
gpio_init(GPIO.GNSS_PWR_EN, True)
gpio_set(GPIO.GNSS_PWR_EN, True)
@@ -325,7 +288,7 @@ def main() -> NoReturn:
print("%.4f: got log: %x len %d" % (time.time(), log_type, len(log_payload)))
if log_type == LOG_GNSS_OEMDRE_MEASUREMENT_REPORT:
msg = messaging.new_message('qcomGnss')
msg = messaging.new_message('qcomGnss', valid=True)
gnss = msg.qcomGnss
gnss.logTs = log_time
@@ -366,8 +329,6 @@ def main() -> NoReturn:
setattr(sv.measurementStatus, kk, bool(v & (1<<vv)))
else:
setattr(sv, k, v)
if report.source == log.QcomGnss.MeasurementSource.gps:
current_gps_time = GPSTime(report.gpsWeek, report.gpsMilliseconds / 1000.0)
pm.send('qcomGnss', msg)
elif log_type == LOG_GNSS_POSITION_REPORT:
report = unpack_position(log_payload)
@@ -376,15 +337,19 @@ def main() -> NoReturn:
vNED = [report["q_FltVelEnuMps[1]"], report["q_FltVelEnuMps[0]"], -report["q_FltVelEnuMps[2]"]]
vNEDsigma = [report["q_FltVelSigmaMps[1]"], report["q_FltVelSigmaMps[0]"], -report["q_FltVelSigmaMps[2]"]]
msg = messaging.new_message('gpsLocation')
msg = messaging.new_message('gpsLocation', valid=True)
gps = msg.gpsLocation
gps.latitude = report["t_DblFinalPosLatLon[0]"] * 180/math.pi
gps.longitude = report["t_DblFinalPosLatLon[1]"] * 180/math.pi
gps.altitude = report["q_FltFinalPosAlt"]
gps.speed = math.sqrt(sum([x**2 for x in vNED]))
gps.bearingDeg = report["q_FltHeadingRad"] * 180/math.pi
gps.unixTimestampMillis = GPSTime(report['w_GpsWeekNumber'],
1e-3*report['q_GpsFixTimeMs']).as_unix_timestamp()*1e3
# TODO needs update if there is another leap second, after june 2024?
dt_timestamp = (datetime.datetime(1980, 1, 6, 0, 0, 0, 0, datetime.timezone.utc) +
datetime.timedelta(weeks=report['w_GpsWeekNumber']) +
datetime.timedelta(seconds=(1e-3*report['q_GpsFixTimeMs'] - 18)))
gps.unixTimestampMillis = dt_timestamp.timestamp()*1e3
gps.source = log.GpsLocationData.SensorSource.qcomdiag
gps.vNED = vNED
gps.verticalAccuracy = report["q_FltVdop"]
@@ -395,12 +360,10 @@ def main() -> NoReturn:
if gps.flags:
want_assistance = False
stop_download_event.set()
pm.send('gpsLocation', msg)
elif log_type == LOG_GNSS_OEMDRE_SVPOLY_REPORT:
msg = messaging.new_message('qcomGnss')
msg = messaging.new_message('qcomGnss', valid=True)
dat = unpack_svpoly(log_payload)
dat = relist(dat)
gnss = msg.qcomGnss
@@ -415,6 +378,10 @@ def main() -> NoReturn:
else:
setattr(poly, k, v)
'''
# Timestamp glonass polys with GPSTime
from laika.gps_time import GPSTime, utc_to_gpst, get_leap_seconds
from laika.helpers import get_prn_from_nmea_id
prn = get_prn_from_nmea_id(poly.svId)
if prn[0] == 'R':
epoch = GPSTime(current_gps_time.week, (poly.t0 - 3*SECS_IN_HR + SECS_IN_DAY) % (SECS_IN_WEEK) + get_leap_seconds(current_gps_time))
@@ -429,10 +396,11 @@ def main() -> NoReturn:
poly.gpsWeek = epoch.week
poly.gpsTow = epoch.tow
'''
pm.send('qcomGnss', msg)
elif log_type in [LOG_GNSS_GPS_MEASUREMENT_REPORT, LOG_GNSS_GLONASS_MEASUREMENT_REPORT]:
msg = messaging.new_message('qcomGnss')
msg = messaging.new_message('qcomGnss', valid=True)
gnss = msg.qcomGnss
gnss.logTs = log_time
system/sensord/rawgps/structs.py → system/qcomgpsd/structs.py
View File
system/sensord/rawgps/test_rawgps.py → system/qcomgpsd/tests/test_qcomgpsd.py
+14 -14
View File
@@ -8,7 +8,7 @@ import unittest
import subprocess
import cereal.messaging as messaging
from openpilot.system.sensord.rawgps.rawgpsd import at_cmd, wait_for_modem
from openpilot.system.qcomgpsd.qcomgpsd import at_cmd, wait_for_modem
from openpilot.selfdrive.manager.process_config import managed_processes
GOOD_SIGNAL = bool(int(os.getenv("GOOD_SIGNAL", '0')))
@@ -24,7 +24,7 @@ class TestRawgpsd(unittest.TestCase):
@classmethod
def tearDownClass(cls):
managed_processes['rawgpsd'].stop()
managed_processes['qcomgpsd'].stop()
os.system("sudo systemctl restart systemd-resolved")
os.system("sudo systemctl restart ModemManager lte")
@@ -33,7 +33,7 @@ class TestRawgpsd(unittest.TestCase):
self.sm = messaging.SubMaster(['qcomGnss', 'gpsLocation', 'gnssMeasurements'])
def tearDown(self):
managed_processes['rawgpsd'].stop()
managed_processes['qcomgpsd'].stop()
os.system("sudo systemctl restart systemd-resolved")
def _wait_for_output(self, t):
@@ -51,7 +51,7 @@ class TestRawgpsd(unittest.TestCase):
def test_wait_for_modem(self):
os.system("sudo systemctl stop ModemManager")
managed_processes['rawgpsd'].start()
managed_processes['qcomgpsd'].start()
assert not self._wait_for_output(5)
os.system("sudo systemctl restart ModemManager")
@@ -62,15 +62,15 @@ class TestRawgpsd(unittest.TestCase):
if not internet:
os.system("sudo systemctl stop systemd-resolved")
with self.subTest(internet=internet):
managed_processes['rawgpsd'].start()
managed_processes['qcomgpsd'].start()
assert self._wait_for_output(7)
managed_processes['rawgpsd'].stop()
managed_processes['qcomgpsd'].stop()
def test_turns_off_gnss(self):
for s in (0.1, 1, 5):
managed_processes['rawgpsd'].start()
managed_processes['qcomgpsd'].start()
time.sleep(s)
managed_processes['rawgpsd'].stop()
managed_processes['qcomgpsd'].stop()
ls = subprocess.check_output("mmcli -m any --location-status --output-json", shell=True, encoding='utf-8')
loc_status = json.loads(ls)
@@ -94,29 +94,29 @@ class TestRawgpsd(unittest.TestCase):
assert valid_duration == '0'
def test_assistance_loading(self):
managed_processes['rawgpsd'].start()
managed_processes['qcomgpsd'].start()
assert self._wait_for_output(10)
managed_processes['rawgpsd'].stop()
managed_processes['qcomgpsd'].stop()
self.check_assistance(True)
def test_no_assistance_loading(self):
os.system("sudo systemctl stop systemd-resolved")
managed_processes['rawgpsd'].start()
managed_processes['qcomgpsd'].start()
assert self._wait_for_output(10)
managed_processes['rawgpsd'].stop()
managed_processes['qcomgpsd'].stop()
self.check_assistance(False)
def test_late_assistance_loading(self):
os.system("sudo systemctl stop systemd-resolved")
managed_processes['rawgpsd'].start()
managed_processes['qcomgpsd'].start()
self._wait_for_output(17)
assert self.sm.updated['qcomGnss']
os.system("sudo systemctl restart systemd-resolved")
time.sleep(15)
managed_processes['rawgpsd'].stop()
managed_processes['qcomgpsd'].stop()
self.check_assistance(True)
if __name__ == "__main__":
system/sensord/pigeond.py
+2 -2
View File
@@ -11,7 +11,7 @@ from typing import List, Optional, Tuple
from cereal import messaging
from openpilot.common.params import Params
from openpilot.system.swaglog import cloudlog
from openpilot.common.swaglog import cloudlog
from openpilot.system.hardware import TICI
from openpilot.common.gpio import gpio_init, gpio_set
from openpilot.system.hardware.tici.pins import GPIO
@@ -291,7 +291,7 @@ def run_receiving(pigeon: TTYPigeon, pm: messaging.PubMaster, duration: int = 0)
continue
# send out to socket
msg = messaging.new_message('ubloxRaw', len(dat))
msg = messaging.new_message('ubloxRaw', len(dat), valid=True)
msg.ubloxRaw = dat[:]
pm.send('ubloxRaw', msg)
else:
system/sensord/rawgps/compare.py
-66
View File
@@ -1,66 +0,0 @@
#!/usr/bin/env python3
import cereal.messaging as messaging
from laika import constants
if __name__ == "__main__":
sm = messaging.SubMaster(['ubloxGnss', 'qcomGnss'])
meas = None
while 1:
sm.update()
if sm['ubloxGnss'].which() == "measurementReport":
meas = sm['ubloxGnss'].measurementReport.measurements
if not sm.updated['qcomGnss'] or meas is None:
continue
report = sm['qcomGnss'].measurementReport
if report.source not in [0, 1]:
continue
GLONASS = report.source == 1
recv_time = report.milliseconds / 1000
car = []
print("qcom has ", sorted([x.svId for x in report.sv]))
print("ublox has", sorted([x.svId for x in meas if x.gnssId == (6 if GLONASS else 0)]))
for i in report.sv:
# match to ublox
tm = None
for m in meas:
if i.svId == m.svId and m.gnssId == 0 and m.sigId == 0 and not GLONASS:
tm = m
if (i.svId-64) == m.svId and m.gnssId == 6 and m.sigId == 0 and GLONASS:
tm = m
if tm is None:
continue
if not i.measurementStatus.measurementNotUsable and i.measurementStatus.satelliteTimeIsKnown:
sat_time = (i.unfilteredMeasurementIntegral + i.unfilteredMeasurementFraction + i.latency) / 1000
ublox_psuedorange = tm.pseudorange
qcom_psuedorange = (recv_time - sat_time)*constants.SPEED_OF_LIGHT
if GLONASS:
glonass_freq = tm.glonassFrequencyIndex - 7
ublox_speed = -(constants.SPEED_OF_LIGHT / (constants.GLONASS_L1 + glonass_freq*constants.GLONASS_L1_DELTA)) * (tm.doppler)
else:
ublox_speed = -(constants.SPEED_OF_LIGHT / constants.GPS_L1) * tm.doppler
qcom_speed = i.unfilteredSpeed
car.append((i.svId, tm.pseudorange, ublox_speed, qcom_psuedorange, qcom_speed, tm.cno))
if len(car) == 0:
print("nothing to compare")
continue
pr_err, speed_err = 0., 0.
for c in car:
ublox_psuedorange, ublox_speed, qcom_psuedorange, qcom_speed = c[1:5]
pr_err += ublox_psuedorange - qcom_psuedorange
speed_err += ublox_speed - qcom_speed
pr_err /= len(car)
speed_err /= len(car)
print("avg psuedorange err %f avg speed err %f" % (pr_err, speed_err))
for c in sorted(car, key=lambda x: abs(x[1] - x[3] - pr_err)):
svid, ublox_psuedorange, ublox_speed, qcom_psuedorange, qcom_speed, cno = c
print("svid: %3d pseudorange: %10.2f m speed: %8.2f m/s meas: %12.2f speed: %10.2f meas_err: %10.3f speed_err: %8.3f cno: %d" %
(svid, ublox_psuedorange, ublox_speed, qcom_psuedorange, qcom_speed,
ublox_psuedorange - qcom_psuedorange - pr_err, ublox_speed - qcom_speed - speed_err, cno))
system/sensord/tests/test_sensord.py
+1 -1
View File
@@ -106,7 +106,7 @@ class TestSensord(unittest.TestCase):
os.environ["LSM_SELF_TEST"] = "1"
# read initial sensor values every test case can use
os.system("pkill -f ./sensord")
os.system("pkill -f \\\\./sensord")
try:
managed_processes["sensord"].start()
cls.sample_secs = int(os.getenv("SAMPLE_SECS", "10"))
system/swaglog.py
-135
View File
@@ -1,135 +0,0 @@
import logging
import os
import time
import warnings
from pathlib import Path
from logging.handlers import BaseRotatingHandler
import zmq
from openpilot.common.logging_extra import SwagLogger, SwagFormatter, SwagLogFileFormatter
from openpilot.system.hardware.hw import Paths
def get_file_handler():
Path(Paths.swaglog_root()).mkdir(parents=True, exist_ok=True)
base_filename = os.path.join(Paths.swaglog_root(), "swaglog")
handler = SwaglogRotatingFileHandler(base_filename)
return handler
class SwaglogRotatingFileHandler(BaseRotatingHandler):
def __init__(self, base_filename, interval=60, max_bytes=1024*256, backup_count=2500, encoding=None):
super().__init__(base_filename, mode="a", encoding=encoding, delay=True)
self.base_filename = base_filename
self.interval = interval # seconds
self.max_bytes = max_bytes
self.backup_count = backup_count
self.log_files = self.get_existing_logfiles()
log_indexes = [f.split(".")[-1] for f in self.log_files]
self.last_file_idx = max([int(i) for i in log_indexes if i.isdigit()] or [-1])
self.last_rollover = None
self.doRollover()
def _open(self):
self.last_rollover = time.monotonic()
self.last_file_idx += 1
next_filename = f"{self.base_filename}.{self.last_file_idx:010}"
stream = open(next_filename, self.mode, encoding=self.encoding)
self.log_files.insert(0, next_filename)
return stream
def get_existing_logfiles(self):
log_files = list()
base_dir = os.path.dirname(self.base_filename)
for fn in os.listdir(base_dir):
fp = os.path.join(base_dir, fn)
if fp.startswith(self.base_filename) and os.path.isfile(fp):
log_files.append(fp)
return sorted(log_files)
def shouldRollover(self, record):
size_exceeded = self.max_bytes > 0 and self.stream.tell() >= self.max_bytes
time_exceeded = self.interval > 0 and self.last_rollover + self.interval <= time.monotonic()
return size_exceeded or time_exceeded
def doRollover(self):
if self.stream:
self.stream.close()
self.stream = self._open()
if self.backup_count > 0:
while len(self.log_files) > self.backup_count:
to_delete = self.log_files.pop()
if os.path.exists(to_delete): # just being safe, should always exist
os.remove(to_delete)
class UnixDomainSocketHandler(logging.Handler):
def __init__(self, formatter):
logging.Handler.__init__(self)
self.setFormatter(formatter)
self.pid = None
self.zctx = None
self.sock = None
def __del__(self):
self.close()
def close(self):
if self.sock is not None:
self.sock.close()
if self.zctx is not None:
self.zctx.term()
def connect(self):
self.zctx = zmq.Context()
self.sock = self.zctx.socket(zmq.PUSH)
self.sock.setsockopt(zmq.LINGER, 10)
self.sock.connect(Paths.swaglog_ipc())
self.pid = os.getpid()
def emit(self, record):
if os.getpid() != self.pid:
# TODO suppresses warning about forking proc with zmq socket, fix root cause
warnings.filterwarnings("ignore", category=ResourceWarning, message="unclosed.*<zmq.*>")
self.connect()
msg = self.format(record).rstrip('\n')
# print("SEND".format(repr(msg)))
try:
s = chr(record.levelno)+msg
self.sock.send(s.encode('utf8'), zmq.NOBLOCK)
except zmq.error.Again:
# drop :/
pass
def add_file_handler(log):
"""
Function to add the file log handler to swaglog.
This can be used to store logs when logmessaged is not running.
"""
handler = get_file_handler()
handler.setFormatter(SwagLogFileFormatter(log))
log.addHandler(handler)
cloudlog = log = SwagLogger()
log.setLevel(logging.DEBUG)
outhandler = logging.StreamHandler()
print_level = os.environ.get('LOGPRINT', 'warning')
if print_level == 'debug':
outhandler.setLevel(logging.DEBUG)
elif print_level == 'info':
outhandler.setLevel(logging.INFO)
elif print_level == 'warning':
outhandler.setLevel(logging.WARNING)
ipchandler = UnixDomainSocketHandler(SwagFormatter(log))
log.addHandler(outhandler)
# logs are sent through IPC before writing to disk to prevent disk I/O blocking
log.addHandler(ipchandler)
system/tests/test_logmessaged.py
+1 -1
View File
@@ -7,7 +7,7 @@ import unittest
import cereal.messaging as messaging
from openpilot.selfdrive.manager.process_config import managed_processes
from openpilot.system.hardware.hw import Paths
from openpilot.system.swaglog import cloudlog, ipchandler
from openpilot.common.swaglog import cloudlog, ipchandler
class TestLogmessaged(unittest.TestCase):
system/timed.py
Executable
+94
View File
@@ -0,0 +1,94 @@
#!/usr/bin/env python3
import datetime
import os
import subprocess
import time
from typing import NoReturn
from timezonefinder import TimezoneFinder
import cereal.messaging as messaging
from openpilot.common.params import Params
from openpilot.common.swaglog import cloudlog
from openpilot.system.hardware import AGNOS
def set_timezone(timezone):
valid_timezones = subprocess.check_output('timedatectl list-timezones', shell=True, encoding='utf8').strip().split('\n')
if timezone not in valid_timezones:
cloudlog.error(f"Timezone not supported {timezone}")
return
cloudlog.debug(f"Setting timezone to {timezone}")
try:
if AGNOS:
tzpath = os.path.join("/usr/share/zoneinfo/", timezone)
subprocess.check_call(f'sudo su -c "ln -snf {tzpath} /data/etc/tmptime && \
mv /data/etc/tmptime /data/etc/localtime"', shell=True)
subprocess.check_call(f'sudo su -c "echo \"{timezone}\" > /data/etc/timezone"', shell=True)
else:
subprocess.check_call(f'sudo timedatectl set-timezone {timezone}', shell=True)
except subprocess.CalledProcessError:
cloudlog.exception(f"Error setting timezone to {timezone}")
def set_time(new_time):
diff = datetime.datetime.now() - new_time
if diff < datetime.timedelta(seconds=10):
cloudlog.debug(f"Time diff too small: {diff}")
return
cloudlog.debug(f"Setting time to {new_time}")
try:
subprocess.run(f"TZ=UTC date -s '{new_time}'", shell=True, check=True)
except subprocess.CalledProcessError:
cloudlog.exception("timed.failed_setting_time")
def main() -> NoReturn:
"""
timed has two responsibilities:
- getting the current time
- getting the current timezone
GPS directly gives time, and timezone is looked up from GPS position.
AGNOS will also use NTP to update the time.
"""
params = Params()
tf = TimezoneFinder()
# Restore timezone from param
tz = params.get("Timezone", encoding='utf8')
tf = TimezoneFinder()
if tz is not None:
cloudlog.debug("Restoring timezone from param")
set_timezone(tz)
sm = messaging.SubMaster(['liveLocationKalman'])
while True:
sm.update(1000)
llk = sm['liveLocationKalman']
if not llk.gpsOK or (time.monotonic() - sm.logMonoTime['liveLocationKalman']/1e9) > 0.2:
continue
# set time
# TODO: account for unixTimesatmpMillis being a (usually short) time in the past
gps_time = datetime.datetime.fromtimestamp(llk.unixTimestampMillis / 1000.)
set_time(gps_time)
# set timezone
pos = llk.positionGeodetic.value
if len(pos) == 3:
gps_timezone = tf.timezone_at(lat=pos[0], lng=pos[1])
if gps_timezone is None:
cloudlog.critical(f"No timezone found based on {pos=}")
else:
set_timezone(gps_timezone)
params.put_nonblocking("Timezone", gps_timezone)
time.sleep(10)
if __name__ == "__main__":
main()
system/timezoned.py
-92
View File
@@ -1,92 +0,0 @@
#!/usr/bin/env python3
import json
import os
import time
import subprocess
from typing import NoReturn
import requests
from timezonefinder import TimezoneFinder
from openpilot.common.params import Params
from openpilot.system.hardware import AGNOS
from openpilot.system.swaglog import cloudlog
from openpilot.system.version import get_version
REQUEST_HEADERS = {'User-Agent': "openpilot-" + get_version()}
def set_timezone(valid_timezones, timezone):
if timezone not in valid_timezones:
cloudlog.error(f"Timezone not supported {timezone}")
return
cloudlog.info(f"Setting timezone to {timezone}")
try:
if AGNOS:
tzpath = os.path.join("/usr/share/zoneinfo/", timezone)
subprocess.check_call(f'sudo su -c "ln -snf {tzpath} /data/etc/tmptime && \
mv /data/etc/tmptime /data/etc/localtime"', shell=True)
subprocess.check_call(f'sudo su -c "echo \"{timezone}\" > /data/etc/timezone"', shell=True)
else:
subprocess.check_call(f'sudo timedatectl set-timezone {timezone}', shell=True)
except subprocess.CalledProcessError:
cloudlog.exception(f"Error setting timezone to {timezone}")
def main() -> NoReturn:
params = Params()
tf = TimezoneFinder()
# Get allowed timezones
valid_timezones = subprocess.check_output('timedatectl list-timezones', shell=True, encoding='utf8').strip().split('\n')
while True:
time.sleep(60)
is_onroad = not params.get_bool("IsOffroad")
if is_onroad:
continue
# Set based on param
timezone = params.get("Timezone", encoding='utf8')
if timezone is not None:
cloudlog.debug("Setting timezone based on param")
set_timezone(valid_timezones, timezone)
continue
location = params.get("LastGPSPosition", encoding='utf8')
# Find timezone based on IP geolocation if no gps location is available
if location is None:
cloudlog.debug("Setting timezone based on IP lookup")
try:
r = requests.get("https://ipapi.co/timezone", headers=REQUEST_HEADERS, timeout=10)
if r.status_code == 200:
set_timezone(valid_timezones, r.text)
else:
cloudlog.error(f"Unexpected status code from api {r.status_code}")
time.sleep(3600) # Don't make too many API requests
except requests.exceptions.RequestException:
cloudlog.exception("Error getting timezone based on IP")
continue
# Find timezone by reverse geocoding the last known gps location
else:
cloudlog.debug("Setting timezone based on GPS location")
try:
location = json.loads(location)
except Exception:
cloudlog.exception("Error parsing location")
continue
timezone = tf.timezone_at(lng=location['longitude'], lat=location['latitude'])
if timezone is None:
cloudlog.error(f"No timezone found based on location, {location}")
continue
set_timezone(valid_timezones, timezone)
if __name__ == "__main__":
main()
system/ubloxd/tests/test_ublox_processing.py
-117
View File
@@ -1,117 +0,0 @@
#!/usr/bin/env python3
import unittest
import time
import numpy as np
from laika import AstroDog
from laika.helpers import ConstellationId
from laika.raw_gnss import correct_measurements, process_measurements, read_raw_ublox
from laika.opt import calc_pos_fix
from openpilot.selfdrive.test.openpilotci import get_url
from openpilot.system.hardware.hw import Paths
from openpilot.tools.lib.logreader import LogReader
from openpilot.selfdrive.test.helpers import with_processes
import cereal.messaging as messaging
def get_gnss_measurements(log_reader):
gnss_measurements = []
for msg in log_reader:
if msg.which() == "ubloxGnss":
ublox_msg = msg.ubloxGnss
if ublox_msg.which == 'measurementReport':
report = ublox_msg.measurementReport
if len(report.measurements) > 0:
gnss_measurements.append(read_raw_ublox(report))
return gnss_measurements
def get_ublox_raw(log_reader):
ublox_raw = []
for msg in log_reader:
if msg.which() == "ubloxRaw":
ublox_raw.append(msg)
return ublox_raw
class TestUbloxProcessing(unittest.TestCase):
NUM_TEST_PROCESS_MEAS = 10
@classmethod
def setUpClass(cls):
lr = LogReader(get_url("4cf7a6ad03080c90|2021-09-29--13-46-36", 0))
cls.gnss_measurements = get_gnss_measurements(lr)
# test gps ephemeris continuity check (drive has ephemeris issues with cutover data)
lr = LogReader(get_url("37b6542f3211019a|2023-01-15--23-45-10", 14))
cls.ublox_raw = get_ublox_raw(lr)
def test_read_ublox_raw(self):
count_gps = 0
count_glonass = 0
for measurements in self.gnss_measurements:
for m in measurements:
if m.constellation_id == ConstellationId.GPS:
count_gps += 1
elif m.constellation_id == ConstellationId.GLONASS:
count_glonass += 1
self.assertEqual(count_gps, 5036)
self.assertEqual(count_glonass, 3651)
def test_get_fix(self):
dog = AstroDog(cache_dir=Paths.download_cache_root())
position_fix_found = 0
count_processed_measurements = 0
count_corrected_measurements = 0
position_fix_found_after_correcting = 0
pos_ests = []
for measurements in self.gnss_measurements[:self.NUM_TEST_PROCESS_MEAS]:
processed_meas = process_measurements(measurements, dog)
count_processed_measurements += len(processed_meas)
pos_fix = calc_pos_fix(processed_meas)
if len(pos_fix) > 0 and all(p != 0 for p in pos_fix[0]):
position_fix_found += 1
corrected_meas = correct_measurements(processed_meas, pos_fix[0][:3], dog)
count_corrected_measurements += len(corrected_meas)
pos_fix = calc_pos_fix(corrected_meas)
if len(pos_fix) > 0 and all(p != 0 for p in pos_fix[0]):
pos_ests.append(pos_fix[0])
position_fix_found_after_correcting += 1
mean_fix = np.mean(np.array(pos_ests)[:, :3], axis=0)
np.testing.assert_allclose(mean_fix, [-2452306.662377, -4778343.136806, 3428550.090557], rtol=0, atol=1)
# Note that can happen that there are less corrected measurements compared to processed when they are invalid.
# However, not for the current segment
self.assertEqual(position_fix_found, self.NUM_TEST_PROCESS_MEAS)
self.assertEqual(position_fix_found_after_correcting, self.NUM_TEST_PROCESS_MEAS)
self.assertEqual(count_processed_measurements, 69)
self.assertEqual(count_corrected_measurements, 69)
@with_processes(['ubloxd'])
def test_ublox_gps_cutover(self):
time.sleep(2)
ugs = messaging.sub_sock("ubloxGnss", timeout=0.1)
ur_pm = messaging.PubMaster(['ubloxRaw'])
def replay_segment():
rcv_msgs = []
for msg in self.ublox_raw:
ur_pm.send(msg.which(), msg.as_builder())
time.sleep(0.001)
rcv_msgs += messaging.drain_sock(ugs)
time.sleep(0.1)
rcv_msgs += messaging.drain_sock(ugs)
return rcv_msgs
# replay twice to enforce cutover data on rewind
rcv_msgs = replay_segment()
rcv_msgs += replay_segment()
ephems_cnt = sum(m.ubloxGnss.which() == 'ephemeris' for m in rcv_msgs)
self.assertEqual(ephems_cnt, 15)
if __name__ == "__main__":
unittest.main()
system/version.py
+2 -2
View File
@@ -5,9 +5,9 @@ from typing import List, Optional
from functools import lru_cache
from openpilot.common.basedir import BASEDIR
from openpilot.system.swaglog import cloudlog
from openpilot.common.swaglog import cloudlog
RELEASE_BRANCHES = ['release3-staging', 'dashcam3-staging', 'release3', 'dashcam3', 'nightly']
RELEASE_BRANCHES = ['release3-staging', 'release3', 'nightly']
TESTED_BRANCHES = RELEASE_BRANCHES + ['devel', 'devel-staging']
training_version: bytes = b"0.2.0"
system/webrtc/__init__.py
View File
system/webrtc/device/audio.py
+110
View File
@@ -0,0 +1,110 @@
import asyncio
import io
from typing import Optional, List, Tuple
import aiortc
import av
import numpy as np
import pyaudio
class AudioInputStreamTrack(aiortc.mediastreams.AudioStreamTrack):
PYAUDIO_TO_AV_FORMAT_MAP = {
pyaudio.paUInt8: 'u8',
pyaudio.paInt16: 's16',
pyaudio.paInt24: 's24',
pyaudio.paInt32: 's32',
pyaudio.paFloat32: 'flt',
}
def __init__(self, audio_format: int = pyaudio.paInt16, rate: int = 16000, channels: int = 1, packet_time: float = 0.020, device_index: Optional[int] = None):
super().__init__()
self.p = pyaudio.PyAudio()
chunk_size = int(packet_time * rate)
self.stream = self.p.open(format=audio_format,
channels=channels,
rate=rate,
frames_per_buffer=chunk_size,
input=True,
input_device_index=device_index)
self.format = audio_format
self.rate = rate
self.channels = channels
self.packet_time = packet_time
self.chunk_size = chunk_size
self.pts = 0
async def recv(self):
mic_data = self.stream.read(self.chunk_size)
mic_array = np.frombuffer(mic_data, dtype=np.int16)
mic_array = np.expand_dims(mic_array, axis=0)
layout = 'stereo' if self.channels > 1 else 'mono'
frame = av.AudioFrame.from_ndarray(mic_array, format=self.PYAUDIO_TO_AV_FORMAT_MAP[self.format], layout=layout)
frame.rate = self.rate
frame.pts = self.pts
self.pts += frame.samples
return frame
class AudioOutputSpeaker:
def __init__(self, audio_format: int = pyaudio.paInt16, rate: int = 48000, channels: int = 2, packet_time: float = 0.2, device_index: Optional[int] = None):
chunk_size = int(packet_time * rate)
self.p = pyaudio.PyAudio()
self.buffer = io.BytesIO()
self.channels = channels
self.stream = self.p.open(format=audio_format,
channels=channels,
rate=rate,
frames_per_buffer=chunk_size,
output=True,
output_device_index=device_index,
stream_callback=self.__pyaudio_callback)
self.tracks_and_tasks: List[Tuple[aiortc.MediaStreamTrack, Optional[asyncio.Task]]] = []
def __pyaudio_callback(self, in_data, frame_count, time_info, status):
if self.buffer.getbuffer().nbytes < frame_count * self.channels * 2:
buff = b'\x00\x00' * frame_count * self.channels
elif self.buffer.getbuffer().nbytes > 115200: # 3x the usual read size
self.buffer.seek(0)
buff = self.buffer.read(frame_count * self.channels * 4)
buff = buff[:frame_count * self.channels * 2]
self.buffer.seek(2)
else:
self.buffer.seek(0)
buff = self.buffer.read(frame_count * self.channels * 2)
self.buffer.seek(2)
return (buff, pyaudio.paContinue)
async def __consume(self, track):
while True:
try:
frame = await track.recv()
except aiortc.MediaStreamError:
return
self.buffer.write(bytes(frame.planes[0]))
def hasTrack(self, track: aiortc.MediaStreamTrack) -> bool:
return any(t == track for t, _ in self.tracks_and_tasks)
def addTrack(self, track: aiortc.MediaStreamTrack):
if not self.hasTrack(track):
self.tracks_and_tasks.append((track, None))
def start(self):
for index, (track, task) in enumerate(self.tracks_and_tasks):
if task is None:
self.tracks_and_tasks[index] = (track, asyncio.create_task(self.__consume(track)))
def stop(self):
for _, task in self.tracks_and_tasks:
if task is not None:
task.cancel()
self.tracks_and_tasks = []
self.stream.stop_stream()
self.stream.close()
self.p.terminate()
system/webrtc/device/video.py
+69
View File
@@ -0,0 +1,69 @@
import asyncio
from typing import Optional
import av
from teleoprtc.tracks import TiciVideoStreamTrack
from cereal import messaging
from openpilot.tools.lib.framereader import FrameReader
from openpilot.common.realtime import DT_MDL, DT_DMON
class LiveStreamVideoStreamTrack(TiciVideoStreamTrack):
camera_to_sock_mapping = {
"driver": "livestreamDriverEncodeData",
"wideRoad": "livestreamWideRoadEncodeData",
"road": "livestreamRoadEncodeData",
}
def __init__(self, camera_type: str):
dt = DT_DMON if camera_type == "driver" else DT_MDL
super().__init__(camera_type, dt)
self._sock = messaging.sub_sock(self.camera_to_sock_mapping[camera_type], conflate=True)
self._pts = 0
async def recv(self):
while True:
msg = messaging.recv_one_or_none(self._sock)
if msg is not None:
break
await asyncio.sleep(0.005)
evta = getattr(msg, msg.which())
packet = av.Packet(evta.header + evta.data)
packet.time_base = self._time_base
packet.pts = self._pts
self.log_debug("track sending frame %s", self._pts)
self._pts += self._dt * self._clock_rate
return packet
def codec_preference(self) -> Optional[str]:
return "H264"
class FrameReaderVideoStreamTrack(TiciVideoStreamTrack):
def __init__(self, input_file: str, dt: float = DT_MDL, camera_type: str = "driver"):
super().__init__(camera_type, dt)
frame_reader = FrameReader(input_file)
self._frames = [frame_reader.get(i, pix_fmt="rgb24") for i in range(frame_reader.frame_count)]
self._frame_count = len(self.frames)
self._frame_index = 0
self._pts = 0
async def recv(self):
self.log_debug("track sending frame %s", self._pts)
img = self._frames[self._frame_index]
new_frame = av.VideoFrame.from_ndarray(img, format="rgb24")
new_frame.pts = self._pts
new_frame.time_base = self._time_base
self._frame_index = (self._frame_index + 1) % self._frame_count
self._pts = await self.next_pts(self._pts)
return new_frame
system/webrtc/schema.py
+43
View File
@@ -0,0 +1,43 @@
import capnp
from typing import Union, List, Dict, Any
def generate_type(type_walker, schema_walker) -> Union[str, List[Any], Dict[str, Any]]:
data_type = next(type_walker)
if data_type.which() == 'struct':
return generate_struct(next(schema_walker))
elif data_type.which() == 'list':
_ = next(schema_walker)
return [generate_type(type_walker, schema_walker)]
elif data_type.which() == 'enum':
return "text"
else:
return str(data_type.which())
def generate_struct(schema: capnp.lib.capnp._StructSchema) -> Dict[str, Any]:
return {field: generate_field(schema.fields[field]) for field in schema.fields if not field.endswith("DEPRECATED")}
def generate_field(field: capnp.lib.capnp._StructSchemaField) -> Union[str, List[Any], Dict[str, Any]]:
def schema_walker(field):
yield field.schema
s = field.schema
while hasattr(s, 'elementType'):
s = s.elementType
yield s
def type_walker(field):
yield field.proto.slot.type
t = field.proto.slot.type
while hasattr(getattr(t, t.which()), 'elementType'):
t = getattr(t, t.which()).elementType
yield t
if field.proto.which() == "slot":
schema_gen, type_gen = schema_walker(field), type_walker(field)
return generate_type(type_gen, schema_gen)
else:
return generate_struct(field.schema)
system/webrtc/tests/test_stream_session.py
Executable
+108
View File
@@ -0,0 +1,108 @@
#!/usr/bin/env python3
import asyncio
import unittest
from unittest.mock import Mock, MagicMock, patch
import json
# for aiortc and its dependencies
import warnings
warnings.filterwarnings("ignore", category=DeprecationWarning)
from aiortc import RTCDataChannel
from aiortc.mediastreams import VIDEO_CLOCK_RATE, VIDEO_TIME_BASE
import capnp
import pyaudio
from cereal import messaging, log
from openpilot.system.webrtc.webrtcd import CerealOutgoingMessageProxy, CerealIncomingMessageProxy
from openpilot.system.webrtc.device.video import LiveStreamVideoStreamTrack
from openpilot.system.webrtc.device.audio import AudioInputStreamTrack
from openpilot.common.realtime import DT_DMON
class TestStreamSession(unittest.TestCase):
def setUp(self):
self.loop = asyncio.new_event_loop()
def tearDown(self):
self.loop.stop()
self.loop.close()
def test_outgoing_proxy(self):
test_msg = log.Event.new_message()
test_msg.logMonoTime = 123
test_msg.valid = True
test_msg.customReservedRawData0 = b"test"
expected_dict = {"type": "customReservedRawData0", "logMonoTime": 123, "valid": True, "data": "test"}
expected_json = json.dumps(expected_dict).encode()
channel = Mock(spec=RTCDataChannel)
mocked_submaster = messaging.SubMaster(["customReservedRawData0"])
def mocked_update(t):
mocked_submaster.update_msgs(0, [test_msg])
with patch.object(messaging.SubMaster, "update", side_effect=mocked_update):
proxy = CerealOutgoingMessageProxy(mocked_submaster)
proxy.add_channel(channel)
proxy.update()
channel.send.assert_called_once_with(expected_json)
def test_incoming_proxy(self):
tested_msgs = [
{"type": "customReservedRawData0", "data": "test"}, # primitive
{"type": "can", "data": [{"address": 0, "busTime": 0, "dat": "", "src": 0}]}, # list
{"type": "testJoystick", "data": {"axes": [0, 0], "buttons": [False]}}, # dict
]
mocked_pubmaster = MagicMock(spec=messaging.PubMaster)
proxy = CerealIncomingMessageProxy(mocked_pubmaster)
for msg in tested_msgs:
proxy.send(json.dumps(msg).encode())
mocked_pubmaster.send.assert_called_once()
mt, md = mocked_pubmaster.send.call_args.args
self.assertEqual(mt, msg["type"])
self.assertIsInstance(md, capnp._DynamicStructBuilder)
self.assertTrue(hasattr(md, msg["type"]))
mocked_pubmaster.reset_mock()
def test_livestream_track(self):
fake_msg = messaging.new_message("livestreamDriverEncodeData")
config = {"receive.return_value": fake_msg.to_bytes()}
with patch("cereal.messaging.SubSocket", spec=True, **config):
track = LiveStreamVideoStreamTrack("driver")
self.assertTrue(track.id.startswith("driver"))
self.assertEqual(track.codec_preference(), "H264")
for i in range(5):
packet = self.loop.run_until_complete(track.recv())
self.assertEqual(packet.time_base, VIDEO_TIME_BASE)
self.assertEqual(packet.pts, int(i * DT_DMON * VIDEO_CLOCK_RATE))
self.assertEqual(packet.size, 0)
def test_input_audio_track(self):
packet_time, rate = 0.02, 16000
sample_count = int(packet_time * rate)
mocked_stream = MagicMock(spec=pyaudio.Stream)
mocked_stream.read.return_value = b"\x00" * 2 * sample_count
config = {"open.side_effect": lambda *args, **kwargs: mocked_stream}
with patch("pyaudio.PyAudio", spec=True, **config):
track = AudioInputStreamTrack(audio_format=pyaudio.paInt16, packet_time=packet_time, rate=rate)
for i in range(5):
frame = self.loop.run_until_complete(track.recv())
self.assertEqual(frame.rate, rate)
self.assertEqual(frame.samples, sample_count)
self.assertEqual(frame.pts, i * sample_count)
if __name__ == "__main__":
unittest.main()
system/webrtc/tests/test_webrtcd.py
Executable
+60
View File
@@ -0,0 +1,60 @@
#!/usr/bin/env python
import asyncio
import json
import unittest
from unittest.mock import MagicMock, AsyncMock
# for aiortc and its dependencies
import warnings
warnings.filterwarnings("ignore", category=DeprecationWarning)
from openpilot.system.webrtc.webrtcd import get_stream
import aiortc
from teleoprtc import WebRTCOfferBuilder
class TestWebrtcdProc(unittest.IsolatedAsyncioTestCase):
async def assertCompletesWithTimeout(self, awaitable, timeout=1):
try:
async with asyncio.timeout(timeout):
await awaitable
except asyncio.TimeoutError:
self.fail("Timeout while waiting for awaitable to complete")
async def test_webrtcd(self):
mock_request = MagicMock()
async def connect(offer):
body = {'sdp': offer.sdp, 'cameras': offer.video, 'bridge_services_in': [], 'bridge_services_out': []}
mock_request.json.side_effect = AsyncMock(return_value=body)
response = await get_stream(mock_request)
response_json = json.loads(response.text)
return aiortc.RTCSessionDescription(**response_json)
builder = WebRTCOfferBuilder(connect)
builder.offer_to_receive_video_stream("road")
builder.offer_to_receive_audio_stream()
builder.add_messaging()
stream = builder.stream()
await self.assertCompletesWithTimeout(stream.start())
await self.assertCompletesWithTimeout(stream.wait_for_connection())
self.assertTrue(stream.has_incoming_video_track("road"))
self.assertTrue(stream.has_incoming_audio_track())
self.assertTrue(stream.has_messaging_channel())
video_track, audio_track = stream.get_incoming_video_track("road"), stream.get_incoming_audio_track()
await self.assertCompletesWithTimeout(video_track.recv())
await self.assertCompletesWithTimeout(audio_track.recv())
await self.assertCompletesWithTimeout(stream.stop())
# cleanup, very implementation specific, test may break if it changes
self.assertTrue(mock_request.app["streams"].__setitem__.called, "Implementation changed, please update this test")
_, session = mock_request.app["streams"].__setitem__.call_args.args
await self.assertCompletesWithTimeout(session.post_run_cleanup())
if __name__ == "__main__":
unittest.main()
system/webrtc/webrtcd.py
Executable
+251
View File
@@ -0,0 +1,251 @@
#!/usr/bin/env python3
import argparse
import asyncio
import json
import uuid
import logging
from dataclasses import dataclass, field
from typing import Any, List, Optional, Union
# aiortc and its dependencies have lots of internal warnings :(
import warnings
warnings.filterwarnings("ignore", category=DeprecationWarning)
import aiortc
from aiortc.mediastreams import VideoStreamTrack, AudioStreamTrack
from aiortc.contrib.media import MediaBlackhole
from aiortc.exceptions import InvalidStateError
from aiohttp import web
import capnp
from teleoprtc import WebRTCAnswerBuilder
from teleoprtc.info import parse_info_from_offer
from openpilot.system.webrtc.device.video import LiveStreamVideoStreamTrack
from openpilot.system.webrtc.device.audio import AudioInputStreamTrack, AudioOutputSpeaker
from openpilot.system.webrtc.schema import generate_field
from cereal import messaging, log
class CerealOutgoingMessageProxy:
def __init__(self, sm: messaging.SubMaster):
self.sm = sm
self.channels: List[aiortc.RTCDataChannel] = []
def add_channel(self, channel: aiortc.RTCDataChannel):
self.channels.append(channel)
def to_json(self, msg_content: Any):
if isinstance(msg_content, capnp._DynamicStructReader):
msg_dict = msg_content.to_dict()
elif isinstance(msg_content, capnp._DynamicListReader):
msg_dict = [self.to_json(msg) for msg in msg_content]
elif isinstance(msg_content, bytes):
msg_dict = msg_content.decode()
else:
msg_dict = msg_content
return msg_dict
def update(self):
# this is blocking in async context...
self.sm.update(0)
for service, updated in self.sm.updated.items():
if not updated:
continue
msg_dict = self.to_json(self.sm[service])
mono_time, valid = self.sm.logMonoTime[service], self.sm.valid[service]
outgoing_msg = {"type": service, "logMonoTime": mono_time, "valid": valid, "data": msg_dict}
encoded_msg = json.dumps(outgoing_msg).encode()
for channel in self.channels:
channel.send(encoded_msg)
class CerealIncomingMessageProxy:
def __init__(self, pm: messaging.PubMaster):
self.pm = pm
def send(self, message: bytes):
msg_json = json.loads(message)
msg_type, msg_data = msg_json["type"], msg_json["data"]
size = None
if not isinstance(msg_data, dict):
size = len(msg_data)
msg = messaging.new_message(msg_type, size=size)
setattr(msg, msg_type, msg_data)
self.pm.send(msg_type, msg)
class CerealProxyRunner:
def __init__(self, proxy: CerealOutgoingMessageProxy):
self.proxy = proxy
self.is_running = False
self.task = None
self.logger = logging.getLogger("webrtcd")
def start(self):
assert self.task is None
self.task = asyncio.create_task(self.run())
def stop(self):
if self.task is None or self.task.done():
return
self.task.cancel()
self.task = None
async def run(self):
while True:
try:
self.proxy.update()
except InvalidStateError:
self.logger.warning("Cereal outgoing proxy invalid state (connection closed)")
break
except Exception as ex:
self.logger.error("Cereal outgoing proxy failure: %s", ex)
await asyncio.sleep(0.01)
class StreamSession:
def __init__(self, sdp: str, cameras: List[str], incoming_services: List[str], outgoing_services: List[str], debug_mode: bool = False):
config = parse_info_from_offer(sdp)
builder = WebRTCAnswerBuilder(sdp)
assert len(cameras) == config.n_expected_camera_tracks, "Incoming stream has misconfigured number of video tracks"
for cam in cameras:
track = LiveStreamVideoStreamTrack(cam) if not debug_mode else VideoStreamTrack()
builder.add_video_stream(cam, track)
if config.expected_audio_track:
track = AudioInputStreamTrack() if not debug_mode else AudioStreamTrack()
builder.add_audio_stream(track)
if config.incoming_audio_track:
self.audio_output_cls = AudioOutputSpeaker if not debug_mode else MediaBlackhole
builder.offer_to_receive_audio_stream()
self.stream = builder.stream()
self.identifier = str(uuid.uuid4())
self.outgoing_bridge = CerealOutgoingMessageProxy(messaging.SubMaster(outgoing_services))
self.incoming_bridge = CerealIncomingMessageProxy(messaging.PubMaster(incoming_services))
self.outgoing_bridge_runner = CerealProxyRunner(self.outgoing_bridge)
self.audio_output: Optional[Union[AudioOutputSpeaker, MediaBlackhole]] = None
self.run_task: Optional[asyncio.Task] = None
self.logger = logging.getLogger("webrtcd")
self.logger.info("New stream session (%s), cameras %s, audio in %s out %s, incoming services %s, outgoing services %s",
self.identifier, cameras, config.incoming_audio_track, config.expected_audio_track, incoming_services, outgoing_services)
def start(self):
self.run_task = asyncio.create_task(self.run())
def stop(self):
if self.run_task.done():
return
self.run_task.cancel()
self.run_task = None
asyncio.run(self.post_run_cleanup())
async def get_answer(self):
return await self.stream.start()
async def message_handler(self, message: bytes):
try:
self.incoming_bridge.send(message)
except Exception as ex:
self.logger.error("Cereal incoming proxy failure: %s", ex)
async def run(self):
try:
await self.stream.wait_for_connection()
if self.stream.has_messaging_channel():
self.stream.set_message_handler(self.message_handler)
channel = self.stream.get_messaging_channel()
self.outgoing_bridge_runner.proxy.add_channel(channel)
self.outgoing_bridge_runner.start()
if self.stream.has_incoming_audio_track():
track = self.stream.get_incoming_audio_track(buffered=False)
self.audio_output = self.audio_output_cls()
self.audio_output.addTrack(track)
self.audio_output.start()
self.logger.info("Stream session (%s) connected", self.identifier)
await self.stream.wait_for_disconnection()
await self.post_run_cleanup()
self.logger.info("Stream session (%s) ended", self.identifier)
except Exception as ex:
self.logger.error("Stream session failure: %s", ex)
async def post_run_cleanup(self):
await self.stream.stop()
self.outgoing_bridge_runner.stop()
if self.audio_output:
self.audio_output.stop()
@dataclass
class StreamRequestBody:
sdp: str
cameras: List[str]
bridge_services_in: List[str] = field(default_factory=list)
bridge_services_out: List[str] = field(default_factory=list)
async def get_stream(request: web.Request):
stream_dict, debug_mode = request.app['streams'], request.app['debug']
raw_body = await request.json()
body = StreamRequestBody(**raw_body)
session = StreamSession(body.sdp, body.cameras, body.bridge_services_in, body.bridge_services_out, debug_mode)
answer = await session.get_answer()
session.start()
stream_dict[session.identifier] = session
return web.json_response({"sdp": answer.sdp, "type": answer.type})
async def get_schema(request: web.Request):
services = request.query["services"].split(",")
services = [s for s in services if s]
assert all(s in log.Event.schema.fields and not s.endswith("DEPRECATED") for s in services), "Invalid service name"
schema_dict = {s: generate_field(log.Event.schema.fields[s]) for s in services}
return web.json_response(schema_dict)
async def on_shutdown(app: web.Application):
for session in app['streams'].values():
session.stop()
del app['streams']
def webrtcd_thread(host: str, port: int, debug: bool):
logging.basicConfig(level=logging.CRITICAL, handlers=[logging.StreamHandler()])
logging_level = logging.DEBUG if debug else logging.INFO
logging.getLogger("WebRTCStream").setLevel(logging_level)
logging.getLogger("webrtcd").setLevel(logging_level)
app = web.Application()
app['streams'] = dict()
app['debug'] = debug
app.on_shutdown.append(on_shutdown)
app.router.add_post("/stream", get_stream)
app.router.add_get("/schema", get_schema)
web.run_app(app, host=host, port=port)
def main():
parser = argparse.ArgumentParser(description="WebRTC daemon")
parser.add_argument("--host", type=str, default="0.0.0.0", help="Host to listen on")
parser.add_argument("--port", type=int, default=5001, help="Port to listen on")
parser.add_argument("--debug", action="store_true", help="Enable debug mode")
args = parser.parse_args()
webrtcd_thread(args.host, args.port, args.debug)
if __name__=="__main__":
main()