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

Compile FrogPilot

Browse Source
This commit is contained in:
FrogAi
2024-03-10 20:59:55 -07:00
parent 69e7feaf01
commit f1acd339d7
1485 changed files with 426154 additions and 398339 deletions
Download Patch File Download Diff File Expand all files Collapse all files

10
system/camerad/SConscript
View File

@@ -1,10 +0,0 @@
Import('env', 'arch', 'cereal', 'messaging', 'common', 'gpucommon', 'visionipc')
libs = ['m', 'pthread', common, 'jpeg', 'OpenCL', 'yuv', cereal, messaging, 'zmq', 'capnp', 'kj', visionipc, gpucommon, 'atomic']
camera_obj = env.Object(['cameras/camera_qcom2.cc', 'cameras/camera_common.cc', 'cameras/camera_util.cc',
'sensors/ar0231.cc', 'sensors/ox03c10.cc', 'sensors/os04c10.cc'])
env.Program('camerad', ['main.cc', camera_obj], LIBS=libs)
if GetOption("extras") and arch == "x86_64":
env.Program('test/test_ae_gray', ['test/test_ae_gray.cc', camera_obj], LIBS=libs)

BIN
system/camerad/camerad Executable file
View File

Binary file not shown.

347
system/camerad/cameras/camera_common.cc
View File

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

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

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

1021
system/camerad/cameras/camera_qcom2.cc
View File

File diff suppressed because it is too large Load Diff

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

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

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

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

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

@@ -1,39 +0,0 @@
#pragma once
#include <functional>
#include <map>
#include <memory>
#include <mutex>
#include <optional>
#include <queue>
#include <media/cam_req_mgr.h>
std::optional<int32_t> device_acquire(int fd, int32_t session_handle, void *data, uint32_t num_resources=1);
int device_config(int fd, int32_t session_handle, int32_t dev_handle, uint64_t packet_handle);
int device_control(int fd, int op_code, int session_handle, int dev_handle);
int do_cam_control(int fd, int op_code, void *handle, int size);
void *alloc_w_mmu_hdl(int video0_fd, int len, uint32_t *handle, int align = 8, int flags = CAM_MEM_FLAG_KMD_ACCESS | CAM_MEM_FLAG_UMD_ACCESS | CAM_MEM_FLAG_CMD_BUF_TYPE,
int mmu_hdl = 0, int mmu_hdl2 = 0);
void release(int video0_fd, uint32_t handle);
class MemoryManager {
public:
void init(int _video0_fd) { video0_fd = _video0_fd; }
~MemoryManager();
template <class T>
auto alloc(int len, uint32_t *handle) {
return std::unique_ptr<T, std::function<void(void *)>>((T*)alloc_buf(len, handle), [this](void *ptr) { this->free(ptr); });
}
private:
void *alloc_buf(int len, uint32_t *handle);
void free(void *ptr);
std::mutex lock;
std::map<void *, uint32_t> handle_lookup;
std::map<void *, int> size_lookup;
std::map<int, std::queue<void *> > cached_allocations;
int video0_fd;
};

23
system/camerad/main.cc
View File

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

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

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

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

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

105
system/camerad/sensors/os04c10.cc
View File

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

298
system/camerad/sensors/os04c10_registers.h
View File

@@ -1,298 +0,0 @@
#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, 0x04},
{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, 0xB0},
{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},
};

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

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

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

@@ -1,761 +0,0 @@
#pragma once
const struct i2c_random_wr_payload start_reg_array_ox03c10[] = {{0x100, 1}};
const struct i2c_random_wr_payload stop_reg_array_ox03c10[] = {{0x100, 0}};
const struct i2c_random_wr_payload init_array_ox03c10[] = {
{0x103, 1},
{0x107, 1},
// X3C_1920x1280_60fps_HDR4_LFR_PWL12_mipi1200
// TPM
{0x4d5a, 0x1a}, {0x4d09, 0xff}, {0x4d09, 0xdf},
/*)
// group 4
{0x3208, 0x04},
{0x4620, 0x04},
{0x3208, 0x14},
// group 5
{0x3208, 0x05},
{0x4620, 0x04},
{0x3208, 0x15},
// group 2
{0x3208, 0x02},
{0x3507, 0x00},
{0x3208, 0x12},
// delay launch group 2
{0x3208, 0xa2},*/
// PLL setup
{0x0301, 0xc8}, // pll1_divs, pll1_predivp, pll1_divpix
{0x0303, 0x01}, // pll1_prediv
{0x0304, 0x01}, {0x0305, 0x2c}, // pll1_loopdiv = 300
{0x0306, 0x04}, // pll1_divmipi = 4
{0x0307, 0x01}, // pll1_divm = 1
{0x0316, 0x00},
{0x0317, 0x00},
{0x0318, 0x00},
{0x0323, 0x05}, // pll2_prediv
{0x0324, 0x01}, {0x0325, 0x2c}, // pll2_divp = 300
// SCLK/PCLK
{0x0400, 0xe0}, {0x0401, 0x80},
{0x0403, 0xde}, {0x0404, 0x34},
{0x0405, 0x3b}, {0x0406, 0xde},
{0x0407, 0x08},
{0x0408, 0xe0}, {0x0409, 0x7f},
{0x040a, 0xde}, {0x040b, 0x34},
{0x040c, 0x47}, {0x040d, 0xd8},
{0x040e, 0x08},
// xchk
{0x2803, 0xfe}, {0x280b, 0x00}, {0x280c, 0x79},
// SC ctrl
{0x3001, 0x03}, // io_pad_oen
{0x3002, 0xfc}, // io_pad_oen
{0x3005, 0x80}, // io_pad_out
{0x3007, 0x01}, // io_pad_sel
{0x3008, 0x80}, // io_pad_sel
// FSIN first frame
/*
{0x3009, 0x2},
{0x3015, 0x2},
{0x3822, 0x20},
{0x3823, 0x58},
{0x3826, 0x0}, {0x3827, 0x8},
{0x3881, 0x4},
{0x3882, 0x8}, {0x3883, 0x0D},
{0x3836, 0x1F}, {0x3837, 0x40},
*/
// FSIN with external pulses
{0x3009, 0x2},
{0x3015, 0x2},
{0x383E, 0x80},
{0x3881, 0x4},
{0x3882, 0x8}, {0x3883, 0x0D},
{0x3836, 0x1F}, {0x3837, 0x40},
{0x3892, 0x44},
{0x3823, 0x48},
{0x3012, 0x41}, // SC_PHY_CTRL = 4 lane MIPI
{0x3020, 0x05}, // SC_CTRL_20
// this is not in the datasheet, listed as RSVD
// but the camera doesn't work without it
{0x3700, 0x28}, {0x3701, 0x15}, {0x3702, 0x19}, {0x3703, 0x23},
{0x3704, 0x0a}, {0x3705, 0x00}, {0x3706, 0x3e}, {0x3707, 0x0d},
{0x3708, 0x50}, {0x3709, 0x5a}, {0x370a, 0x00}, {0x370b, 0x96},
{0x3711, 0x11}, {0x3712, 0x13}, {0x3717, 0x02}, {0x3718, 0x73},
{0x372c, 0x40}, {0x3733, 0x01}, {0x3738, 0x36}, {0x3739, 0x36},
{0x373a, 0x25}, {0x373b, 0x25}, {0x373f, 0x21}, {0x3740, 0x21},
{0x3741, 0x21}, {0x3742, 0x21}, {0x3747, 0x28}, {0x3748, 0x28},
{0x3749, 0x19}, {0x3755, 0x1a}, {0x3756, 0x0a}, {0x3757, 0x1c},
{0x3765, 0x19}, {0x3766, 0x05}, {0x3767, 0x05}, {0x3768, 0x13},
{0x376c, 0x07}, {0x3778, 0x20}, {0x377c, 0xc8}, {0x3781, 0x02},
{0x3783, 0x02}, {0x379c, 0x58}, {0x379e, 0x00}, {0x379f, 0x00},
{0x37a0, 0x00}, {0x37bc, 0x22}, {0x37c0, 0x01}, {0x37c4, 0x3e},
{0x37c5, 0x3e}, {0x37c6, 0x2a}, {0x37c7, 0x28}, {0x37c8, 0x02},
{0x37c9, 0x12}, {0x37cb, 0x29}, {0x37cd, 0x29}, {0x37d2, 0x00},
{0x37d3, 0x73}, {0x37d6, 0x00}, {0x37d7, 0x6b}, {0x37dc, 0x00},
{0x37df, 0x54}, {0x37e2, 0x00}, {0x37e3, 0x00}, {0x37f8, 0x00},
{0x37f9, 0x01}, {0x37fa, 0x00}, {0x37fb, 0x19},
// also RSVD
{0x3c03, 0x01}, {0x3c04, 0x01}, {0x3c06, 0x21}, {0x3c08, 0x01},
{0x3c09, 0x01}, {0x3c0a, 0x01}, {0x3c0b, 0x21}, {0x3c13, 0x21},
{0x3c14, 0x82}, {0x3c16, 0x13}, {0x3c21, 0x00}, {0x3c22, 0xf3},
{0x3c37, 0x12}, {0x3c38, 0x31}, {0x3c3c, 0x00}, {0x3c3d, 0x03},
{0x3c44, 0x16}, {0x3c5c, 0x8a}, {0x3c5f, 0x03}, {0x3c61, 0x80},
{0x3c6f, 0x2b}, {0x3c70, 0x5f}, {0x3c71, 0x2c}, {0x3c72, 0x2c},
{0x3c73, 0x2c}, {0x3c76, 0x12},
// PEC checks
{0x3182, 0x12},
{0x320e, 0x00}, {0x320f, 0x00}, // RSVD
{0x3211, 0x61},
{0x3215, 0xcd},
{0x3219, 0x08},
{0x3506, 0x20}, {0x3507, 0x00}, // hcg fine exposure
{0x350a, 0x01}, {0x350b, 0x00}, {0x350c, 0x00}, // hcg digital gain
{0x3586, 0x40}, {0x3587, 0x00}, // lcg fine exposure
{0x358a, 0x01}, {0x358b, 0x00}, {0x358c, 0x00}, // lcg digital gain
{0x3546, 0x20}, {0x3547, 0x00}, // spd fine exposure
{0x354a, 0x01}, {0x354b, 0x00}, {0x354c, 0x00}, // spd digital gain
{0x35c6, 0xb0}, {0x35c7, 0x00}, // vs fine exposure
{0x35ca, 0x01}, {0x35cb, 0x00}, {0x35cc, 0x00}, // vs digital gain
// also RSVD
{0x3600, 0x8f}, {0x3605, 0x16}, {0x3609, 0xf0}, {0x360a, 0x01},
{0x360e, 0x1d}, {0x360f, 0x10}, {0x3610, 0x70}, {0x3611, 0x3a},
{0x3612, 0x28}, {0x361a, 0x29}, {0x361b, 0x6c}, {0x361c, 0x0b},
{0x361d, 0x00}, {0x361e, 0xfc}, {0x362a, 0x00}, {0x364d, 0x0f},
{0x364e, 0x18}, {0x364f, 0x12}, {0x3653, 0x1c}, {0x3654, 0x00},
{0x3655, 0x1f}, {0x3656, 0x1f}, {0x3657, 0x0c}, {0x3658, 0x0a},
{0x3659, 0x14}, {0x365a, 0x18}, {0x365b, 0x14}, {0x365c, 0x10},
{0x365e, 0x12}, {0x3674, 0x08}, {0x3677, 0x3a}, {0x3678, 0x3a},
{0x3679, 0x19},
// Y_ADDR_START = 4
{0x3802, 0x00}, {0x3803, 0x04},
// Y_ADDR_END = 0x50b
{0x3806, 0x05}, {0x3807, 0x0b},
// X_OUTPUT_SIZE = 0x780 = 1920 (changed to 1928)
{0x3808, 0x07}, {0x3809, 0x88},
// Y_OUTPUT_SIZE = 0x500 = 1280 (changed to 1208)
{0x380a, 0x04}, {0x380b, 0xb8},
// horizontal timing 0x447
{0x380c, 0x04}, {0x380d, 0x47},
// rows per frame (was 0x2ae)
// 0x8ae = 53.65 ms
{0x380e, 0x08}, {0x380f, 0x15},
// this should be triggered by FSIN, not free running
{0x3810, 0x00}, {0x3811, 0x08}, // x cutoff
{0x3812, 0x00}, {0x3813, 0x04}, // y cutoff
{0x3816, 0x01},
{0x3817, 0x01},
{0x381c, 0x18},
{0x381e, 0x01},
{0x381f, 0x01},
// don't mirror, just flip
{0x3820, 0x04},
{0x3821, 0x19},
{0x3832, 0xF0},
{0x3834, 0xF0},
{0x384c, 0x02},
{0x384d, 0x0d},
{0x3850, 0x00},
{0x3851, 0x42},
{0x3852, 0x00},
{0x3853, 0x40},
{0x3858, 0x04},
{0x388c, 0x02},
{0x388d, 0x2b},
// APC
{0x3b40, 0x05}, {0x3b41, 0x40}, {0x3b42, 0x00}, {0x3b43, 0x90},
{0x3b44, 0x00}, {0x3b45, 0x20}, {0x3b46, 0x00}, {0x3b47, 0x20},
{0x3b48, 0x19}, {0x3b49, 0x12}, {0x3b4a, 0x16}, {0x3b4b, 0x2e},
{0x3b4c, 0x00}, {0x3b4d, 0x00},
{0x3b86, 0x00}, {0x3b87, 0x34}, {0x3b88, 0x00}, {0x3b89, 0x08},
{0x3b8a, 0x05}, {0x3b8b, 0x00}, {0x3b8c, 0x07}, {0x3b8d, 0x80},
{0x3b8e, 0x00}, {0x3b8f, 0x00}, {0x3b92, 0x05}, {0x3b93, 0x00},
{0x3b94, 0x07}, {0x3b95, 0x80}, {0x3b9e, 0x09},
// OTP
{0x3d82, 0x73},
{0x3d85, 0x05},
{0x3d8a, 0x03},
{0x3d8b, 0xff},
{0x3d99, 0x00},
{0x3d9a, 0x9f},
{0x3d9b, 0x00},
{0x3d9c, 0xa0},
{0x3da4, 0x00},
{0x3da7, 0x50},
// DTR
{0x420e, 0x6b},
{0x420f, 0x6e},
{0x4210, 0x06},
{0x4211, 0xc1},
{0x421e, 0x02},
{0x421f, 0x45},
{0x4220, 0xe1},
{0x4221, 0x01},
{0x4301, 0xff},
{0x4307, 0x03},
{0x4308, 0x13},
{0x430a, 0x13},
{0x430d, 0x93},
{0x430f, 0x57},
{0x4310, 0x95},
{0x4311, 0x16},
{0x4316, 0x00},
{0x4317, 0x38}, // both embedded rows are enabled
{0x4319, 0x03}, // spd dcg
{0x431a, 0x00}, // 8 bit mipi
{0x431b, 0x00},
{0x431d, 0x2a},
{0x431e, 0x11},
{0x431f, 0x20}, // enable PWL (pwl0_en), 12 bits
//{0x431f, 0x00}, // disable PWL
{0x4320, 0x19},
{0x4323, 0x80},
{0x4324, 0x00},
{0x4503, 0x4e},
{0x4505, 0x00},
{0x4509, 0x00},
{0x450a, 0x00},
{0x4580, 0xf8},
{0x4583, 0x07},
{0x4584, 0x6a},
{0x4585, 0x08},
{0x4586, 0x05},
{0x4587, 0x04},
{0x4588, 0x73},
{0x4589, 0x05},
{0x458a, 0x1f},
{0x458b, 0x02},
{0x458c, 0xdc},
{0x458d, 0x03},
{0x458e, 0x02},
{0x4597, 0x07},
{0x4598, 0x40},
{0x4599, 0x0e},
{0x459a, 0x0e},
{0x459b, 0xfb},
{0x459c, 0xf3},
{0x4602, 0x00},
{0x4603, 0x13},
{0x4604, 0x00},
{0x4609, 0x0a},
{0x460a, 0x30},
{0x4610, 0x00},
{0x4611, 0x70},
{0x4612, 0x01},
{0x4613, 0x00},
{0x4614, 0x00},
{0x4615, 0x70},
{0x4616, 0x01},
{0x4617, 0x00},
{0x4800, 0x04}, // invert output PCLK
{0x480a, 0x22},
{0x4813, 0xe4},
// mipi
{0x4814, 0x2a},
{0x4837, 0x0d},
{0x484b, 0x47},
{0x484f, 0x00},
{0x4887, 0x51},
{0x4d00, 0x4a},
{0x4d01, 0x18},
{0x4d05, 0xff},
{0x4d06, 0x88},
{0x4d08, 0x63},
{0x4d09, 0xdf},
{0x4d15, 0x7d},
{0x4d1a, 0x20},
{0x4d30, 0x0a},
{0x4d31, 0x00},
{0x4d34, 0x7d},
{0x4d3c, 0x7d},
{0x4f00, 0x00},
{0x4f01, 0x00},
{0x4f02, 0x00},
{0x4f03, 0x20},
{0x4f04, 0xe0},
{0x6a00, 0x00},
{0x6a01, 0x20},
{0x6a02, 0x00},
{0x6a03, 0x20},
{0x6a04, 0x02},
{0x6a05, 0x80},
{0x6a06, 0x01},
{0x6a07, 0xe0},
{0x6a08, 0xcf},
{0x6a09, 0x01},
{0x6a0a, 0x40},
{0x6a20, 0x00},
{0x6a21, 0x02},
{0x6a22, 0x00},
{0x6a23, 0x00},
{0x6a24, 0x00},
{0x6a25, 0x00},
{0x6a26, 0x00},
{0x6a27, 0x00},
{0x6a28, 0x00},
// isp
{0x5000, 0x8f},
{0x5001, 0x75},
{0x5002, 0x7f}, // PWL0
//{0x5002, 0x3f}, // PWL disable
{0x5003, 0x7a},
{0x5004, 0x3e},
{0x5005, 0x1e},
{0x5006, 0x1e},
{0x5007, 0x1e},
{0x5008, 0x00},
{0x500c, 0x00},
{0x502c, 0x00},
{0x502e, 0x00},
{0x502f, 0x00},
{0x504b, 0x00},
{0x5053, 0x00},
{0x505b, 0x00},
{0x5063, 0x00},
{0x5070, 0x00},
{0x5074, 0x04},
{0x507a, 0x04},
{0x507b, 0x09},
{0x5500, 0x02},
{0x5700, 0x02},
{0x5900, 0x02},
{0x6007, 0x04},
{0x6008, 0x05},
{0x6009, 0x02},
{0x600b, 0x08},
{0x600c, 0x07},
{0x600d, 0x88},
{0x6016, 0x00},
{0x6027, 0x04},
{0x6028, 0x05},
{0x6029, 0x02},
{0x602b, 0x08},
{0x602c, 0x07},
{0x602d, 0x88},
{0x6047, 0x04},
{0x6048, 0x05},
{0x6049, 0x02},
{0x604b, 0x08},
{0x604c, 0x07},
{0x604d, 0x88},
{0x6067, 0x04},
{0x6068, 0x05},
{0x6069, 0x02},
{0x606b, 0x08},
{0x606c, 0x07},
{0x606d, 0x88},
{0x6087, 0x04},
{0x6088, 0x05},
{0x6089, 0x02},
{0x608b, 0x08},
{0x608c, 0x07},
{0x608d, 0x88},
// 12-bit PWL0
{0x5e00, 0x00},
// m_ndX_exp[0:32]
// 9*2+0xa*3+0xb*2+0xc*2+0xd*2+0xe*2+0xf*2+0x10*2+0x11*2+0x12*4+0x13*3+0x14*3+0x15*3+0x16 = 518
{0x5e01, 0x09},
{0x5e02, 0x09},
{0x5e03, 0x0a},
{0x5e04, 0x0a},
{0x5e05, 0x0a},
{0x5e06, 0x0b},
{0x5e07, 0x0b},
{0x5e08, 0x0c},
{0x5e09, 0x0c},
{0x5e0a, 0x0d},
{0x5e0b, 0x0d},
{0x5e0c, 0x0e},
{0x5e0d, 0x0e},
{0x5e0e, 0x0f},
{0x5e0f, 0x0f},
{0x5e10, 0x10},
{0x5e11, 0x10},
{0x5e12, 0x11},
{0x5e13, 0x11},
{0x5e14, 0x12},
{0x5e15, 0x12},
{0x5e16, 0x12},
{0x5e17, 0x12},
{0x5e18, 0x13},
{0x5e19, 0x13},
{0x5e1a, 0x13},
{0x5e1b, 0x14},
{0x5e1c, 0x14},
{0x5e1d, 0x14},
{0x5e1e, 0x15},
{0x5e1f, 0x15},
{0x5e20, 0x15},
{0x5e21, 0x16},
// m_ndY_val[0:32]
// 0x200+0xff+0x100*3+0x80*12+0x40*16 = 4095
{0x5e22, 0x00}, {0x5e23, 0x02}, {0x5e24, 0x00},
{0x5e25, 0x00}, {0x5e26, 0x00}, {0x5e27, 0xff},
{0x5e28, 0x00}, {0x5e29, 0x01}, {0x5e2a, 0x00},
{0x5e2b, 0x00}, {0x5e2c, 0x01}, {0x5e2d, 0x00},
{0x5e2e, 0x00}, {0x5e2f, 0x01}, {0x5e30, 0x00},
{0x5e31, 0x00}, {0x5e32, 0x00}, {0x5e33, 0x80},
{0x5e34, 0x00}, {0x5e35, 0x00}, {0x5e36, 0x80},
{0x5e37, 0x00}, {0x5e38, 0x00}, {0x5e39, 0x80},
{0x5e3a, 0x00}, {0x5e3b, 0x00}, {0x5e3c, 0x80},
{0x5e3d, 0x00}, {0x5e3e, 0x00}, {0x5e3f, 0x80},
{0x5e40, 0x00}, {0x5e41, 0x00}, {0x5e42, 0x80},
{0x5e43, 0x00}, {0x5e44, 0x00}, {0x5e45, 0x80},
{0x5e46, 0x00}, {0x5e47, 0x00}, {0x5e48, 0x80},
{0x5e49, 0x00}, {0x5e4a, 0x00}, {0x5e4b, 0x80},
{0x5e4c, 0x00}, {0x5e4d, 0x00}, {0x5e4e, 0x80},
{0x5e4f, 0x00}, {0x5e50, 0x00}, {0x5e51, 0x80},
{0x5e52, 0x00}, {0x5e53, 0x00}, {0x5e54, 0x80},
{0x5e55, 0x00}, {0x5e56, 0x00}, {0x5e57, 0x40},
{0x5e58, 0x00}, {0x5e59, 0x00}, {0x5e5a, 0x40},
{0x5e5b, 0x00}, {0x5e5c, 0x00}, {0x5e5d, 0x40},
{0x5e5e, 0x00}, {0x5e5f, 0x00}, {0x5e60, 0x40},
{0x5e61, 0x00}, {0x5e62, 0x00}, {0x5e63, 0x40},
{0x5e64, 0x00}, {0x5e65, 0x00}, {0x5e66, 0x40},
{0x5e67, 0x00}, {0x5e68, 0x00}, {0x5e69, 0x40},
{0x5e6a, 0x00}, {0x5e6b, 0x00}, {0x5e6c, 0x40},
{0x5e6d, 0x00}, {0x5e6e, 0x00}, {0x5e6f, 0x40},
{0x5e70, 0x00}, {0x5e71, 0x00}, {0x5e72, 0x40},
{0x5e73, 0x00}, {0x5e74, 0x00}, {0x5e75, 0x40},
{0x5e76, 0x00}, {0x5e77, 0x00}, {0x5e78, 0x40},
{0x5e79, 0x00}, {0x5e7a, 0x00}, {0x5e7b, 0x40},
{0x5e7c, 0x00}, {0x5e7d, 0x00}, {0x5e7e, 0x40},
{0x5e7f, 0x00}, {0x5e80, 0x00}, {0x5e81, 0x40},
{0x5e82, 0x00}, {0x5e83, 0x00}, {0x5e84, 0x40},
// disable PWL
/*{0x5e01, 0x18}, {0x5e02, 0x00}, {0x5e03, 0x00}, {0x5e04, 0x00},
{0x5e05, 0x00}, {0x5e06, 0x00}, {0x5e07, 0x00}, {0x5e08, 0x00},
{0x5e09, 0x00}, {0x5e0a, 0x00}, {0x5e0b, 0x00}, {0x5e0c, 0x00},
{0x5e0d, 0x00}, {0x5e0e, 0x00}, {0x5e0f, 0x00}, {0x5e10, 0x00},
{0x5e11, 0x00}, {0x5e12, 0x00}, {0x5e13, 0x00}, {0x5e14, 0x00},
{0x5e15, 0x00}, {0x5e16, 0x00}, {0x5e17, 0x00}, {0x5e18, 0x00},
{0x5e19, 0x00}, {0x5e1a, 0x00}, {0x5e1b, 0x00}, {0x5e1c, 0x00},
{0x5e1d, 0x00}, {0x5e1e, 0x00}, {0x5e1f, 0x00}, {0x5e20, 0x00},
{0x5e21, 0x00},
{0x5e22, 0x00}, {0x5e23, 0x0f}, {0x5e24, 0xFF},*/
{0x4001, 0x2b}, // BLC_CTRL_1
{0x4008, 0x02}, {0x4009, 0x03},
{0x4018, 0x12},
{0x4022, 0x40},
{0x4023, 0x20},
// all black level targets are 0x40
{0x4026, 0x00}, {0x4027, 0x40},
{0x4028, 0x00}, {0x4029, 0x40},
{0x402a, 0x00}, {0x402b, 0x40},
{0x402c, 0x00}, {0x402d, 0x40},
{0x407e, 0xcc},
{0x407f, 0x18},
{0x4080, 0xff},
{0x4081, 0xff},
{0x4082, 0x01},
{0x4083, 0x53},
{0x4084, 0x01},
{0x4085, 0x2b},
{0x4086, 0x00},
{0x4087, 0xb3},
{0x4640, 0x40},
{0x4641, 0x11},
{0x4642, 0x0e},
{0x4643, 0xee},
{0x4646, 0x0f},
{0x4648, 0x00},
{0x4649, 0x03},
{0x4f00, 0x00},
{0x4f01, 0x00},
{0x4f02, 0x80},
{0x4f03, 0x2c},
{0x4f04, 0xf8},
{0x4d09, 0xff},
{0x4d09, 0xdf},
{0x5003, 0x7a},
{0x5b80, 0x08},
{0x5c00, 0x08},
{0x5c80, 0x00},
{0x5bbe, 0x12},
{0x5c3e, 0x12},
{0x5cbe, 0x12},
{0x5b8a, 0x80},
{0x5b8b, 0x80},
{0x5b8c, 0x80},
{0x5b8d, 0x80},
{0x5b8e, 0x60},
{0x5b8f, 0x80},
{0x5b90, 0x80},
{0x5b91, 0x80},
{0x5b92, 0x80},
{0x5b93, 0x20},
{0x5b94, 0x80},
{0x5b95, 0x80},
{0x5b96, 0x80},
{0x5b97, 0x20},
{0x5b98, 0x00},
{0x5b99, 0x80},
{0x5b9a, 0x40},
{0x5b9b, 0x20},
{0x5b9c, 0x00},
{0x5b9d, 0x00},
{0x5b9e, 0x80},
{0x5b9f, 0x00},
{0x5ba0, 0x00},
{0x5ba1, 0x00},
{0x5ba2, 0x00},
{0x5ba3, 0x00},
{0x5ba4, 0x00},
{0x5ba5, 0x00},
{0x5ba6, 0x00},
{0x5ba7, 0x00},
{0x5ba8, 0x02},
{0x5ba9, 0x00},
{0x5baa, 0x02},
{0x5bab, 0x76},
{0x5bac, 0x03},
{0x5bad, 0x08},
{0x5bae, 0x00},
{0x5baf, 0x80},
{0x5bb0, 0x00},
{0x5bb1, 0xc0},
{0x5bb2, 0x01},
{0x5bb3, 0x00},
// m_nNormCombineWeight
{0x5c0a, 0x80}, {0x5c0b, 0x80}, {0x5c0c, 0x80}, {0x5c0d, 0x80}, {0x5c0e, 0x60},
{0x5c0f, 0x80}, {0x5c10, 0x80}, {0x5c11, 0x80}, {0x5c12, 0x60}, {0x5c13, 0x20},
{0x5c14, 0x80}, {0x5c15, 0x80}, {0x5c16, 0x80}, {0x5c17, 0x20}, {0x5c18, 0x00},
{0x5c19, 0x80}, {0x5c1a, 0x40}, {0x5c1b, 0x20}, {0x5c1c, 0x00}, {0x5c1d, 0x00},
{0x5c1e, 0x80}, {0x5c1f, 0x00}, {0x5c20, 0x00}, {0x5c21, 0x00}, {0x5c22, 0x00},
{0x5c23, 0x00}, {0x5c24, 0x00}, {0x5c25, 0x00}, {0x5c26, 0x00}, {0x5c27, 0x00},
// m_nCombinThreL
{0x5c28, 0x02}, {0x5c29, 0x00},
{0x5c2a, 0x02}, {0x5c2b, 0x76},
{0x5c2c, 0x03}, {0x5c2d, 0x08},
// m_nCombinThreS
{0x5c2e, 0x00}, {0x5c2f, 0x80},
{0x5c30, 0x00}, {0x5c31, 0xc0},
{0x5c32, 0x01}, {0x5c33, 0x00},
// m_nNormCombineWeight
{0x5c8a, 0x80}, {0x5c8b, 0x80}, {0x5c8c, 0x80}, {0x5c8d, 0x80}, {0x5c8e, 0x80},
{0x5c8f, 0x80}, {0x5c90, 0x80}, {0x5c91, 0x80}, {0x5c92, 0x80}, {0x5c93, 0x60},
{0x5c94, 0x80}, {0x5c95, 0x80}, {0x5c96, 0x80}, {0x5c97, 0x60}, {0x5c98, 0x40},
{0x5c99, 0x80}, {0x5c9a, 0x80}, {0x5c9b, 0x80}, {0x5c9c, 0x40}, {0x5c9d, 0x00},
{0x5c9e, 0x80}, {0x5c9f, 0x80}, {0x5ca0, 0x80}, {0x5ca1, 0x20}, {0x5ca2, 0x00},
{0x5ca3, 0x80}, {0x5ca4, 0x80}, {0x5ca5, 0x00}, {0x5ca6, 0x00}, {0x5ca7, 0x00},
{0x5ca8, 0x01}, {0x5ca9, 0x00},
{0x5caa, 0x02}, {0x5cab, 0x00},
{0x5cac, 0x03}, {0x5cad, 0x08},
{0x5cae, 0x01}, {0x5caf, 0x00},
{0x5cb0, 0x02}, {0x5cb1, 0x00},
{0x5cb2, 0x03}, {0x5cb3, 0x08},
// combine ISP
{0x5be7, 0x80},
{0x5bc9, 0x80},
{0x5bca, 0x80},
{0x5bcb, 0x80},
{0x5bcc, 0x80},
{0x5bcd, 0x80},
{0x5bce, 0x80},
{0x5bcf, 0x80},
{0x5bd0, 0x80},
{0x5bd1, 0x80},
{0x5bd2, 0x20},
{0x5bd3, 0x80},
{0x5bd4, 0x40},
{0x5bd5, 0x20},
{0x5bd6, 0x00},
{0x5bd7, 0x00},
{0x5bd8, 0x00},
{0x5bd9, 0x00},
{0x5bda, 0x00},
{0x5bdb, 0x00},
{0x5bdc, 0x00},
{0x5bdd, 0x00},
{0x5bde, 0x00},
{0x5bdf, 0x00},
{0x5be0, 0x00},
{0x5be1, 0x00},
{0x5be2, 0x00},
{0x5be3, 0x00},
{0x5be4, 0x00},
{0x5be5, 0x00},
{0x5be6, 0x00},
// m_nSPDCombineWeight
{0x5c49, 0x80}, {0x5c4a, 0x80}, {0x5c4b, 0x80}, {0x5c4c, 0x80}, {0x5c4d, 0x40},
{0x5c4e, 0x80}, {0x5c4f, 0x80}, {0x5c50, 0x80}, {0x5c51, 0x60}, {0x5c52, 0x20},
{0x5c53, 0x80}, {0x5c54, 0x80}, {0x5c55, 0x80}, {0x5c56, 0x20}, {0x5c57, 0x00},
{0x5c58, 0x80}, {0x5c59, 0x40}, {0x5c5a, 0x20}, {0x5c5b, 0x00}, {0x5c5c, 0x00},
{0x5c5d, 0x80}, {0x5c5e, 0x00}, {0x5c5f, 0x00}, {0x5c60, 0x00}, {0x5c61, 0x00},
{0x5c62, 0x00}, {0x5c63, 0x00}, {0x5c64, 0x00}, {0x5c65, 0x00}, {0x5c66, 0x00},
// m_nSPDCombineWeight
{0x5cc9, 0x80}, {0x5cca, 0x80}, {0x5ccb, 0x80}, {0x5ccc, 0x80}, {0x5ccd, 0x80},
{0x5cce, 0x80}, {0x5ccf, 0x80}, {0x5cd0, 0x80}, {0x5cd1, 0x80}, {0x5cd2, 0x60},
{0x5cd3, 0x80}, {0x5cd4, 0x80}, {0x5cd5, 0x80}, {0x5cd6, 0x60}, {0x5cd7, 0x40},
{0x5cd8, 0x80}, {0x5cd9, 0x80}, {0x5cda, 0x80}, {0x5cdb, 0x40}, {0x5cdc, 0x20},
{0x5cdd, 0x80}, {0x5cde, 0x80}, {0x5cdf, 0x80}, {0x5ce0, 0x20}, {0x5ce1, 0x00},
{0x5ce2, 0x80}, {0x5ce3, 0x80}, {0x5ce4, 0x80}, {0x5ce5, 0x00}, {0x5ce6, 0x00},
{0x5d74, 0x01},
{0x5d75, 0x00},
{0x5d1f, 0x81},
{0x5d11, 0x00},
{0x5d12, 0x10},
{0x5d13, 0x10},
{0x5d15, 0x05},
{0x5d16, 0x05},
{0x5d17, 0x05},
{0x5d08, 0x03},
{0x5d09, 0xb6},
{0x5d0a, 0x03},
{0x5d0b, 0xb6},
{0x5d18, 0x03},
{0x5d19, 0xb6},
{0x5d62, 0x01},
{0x5d40, 0x02},
{0x5d41, 0x01},
{0x5d63, 0x1f},
{0x5d64, 0x00},
{0x5d65, 0x80},
{0x5d56, 0x00},
{0x5d57, 0x20},
{0x5d58, 0x00},
{0x5d59, 0x20},
{0x5d5a, 0x00},
{0x5d5b, 0x0c},
{0x5d5c, 0x02},
{0x5d5d, 0x40},
{0x5d5e, 0x02},
{0x5d5f, 0x40},
{0x5d60, 0x03},
{0x5d61, 0x40},
{0x5d4a, 0x02},
{0x5d4b, 0x40},
{0x5d4c, 0x02},
{0x5d4d, 0x40},
{0x5d4e, 0x02},
{0x5d4f, 0x40},
{0x5d50, 0x18},
{0x5d51, 0x80},
{0x5d52, 0x18},
{0x5d53, 0x80},
{0x5d54, 0x18},
{0x5d55, 0x80},
{0x5d46, 0x20},
{0x5d47, 0x00},
{0x5d48, 0x22},
{0x5d49, 0x00},
{0x5d42, 0x20},
{0x5d43, 0x00},
{0x5d44, 0x22},
{0x5d45, 0x00},
{0x5004, 0x1e},
{0x4221, 0x03}, // this is changed from 1 -> 3
// DCG exposure coarse
// {0x3501, 0x01}, {0x3502, 0xc8},
// SPD exposure coarse
// {0x3541, 0x01}, {0x3542, 0xc8},
// VS exposure coarse
// {0x35c1, 0x00}, {0x35c2, 0x01},
// crc reference
{0x420e, 0x66}, {0x420f, 0x5d}, {0x4210, 0xa8}, {0x4211, 0x55},
// crc stat check
{0x507a, 0x5f}, {0x507b, 0x46},
// watchdog control
{0x4f00, 0x00}, {0x4f01, 0x01}, {0x4f02, 0x80}, {0x4f04, 0x2c},
// color balance gains
// blue
{0x5280, 0x06}, {0x5281, 0xCB}, // hcg
{0x5480, 0x06}, {0x5481, 0xCB}, // lcg
{0x5680, 0x06}, {0x5681, 0xCB}, // spd
{0x5880, 0x06}, {0x5881, 0xCB}, // vs
// green(blue)
{0x5282, 0x04}, {0x5283, 0x00},
{0x5482, 0x04}, {0x5483, 0x00},
{0x5682, 0x04}, {0x5683, 0x00},
{0x5882, 0x04}, {0x5883, 0x00},
// green(red)
{0x5284, 0x04}, {0x5285, 0x00},
{0x5484, 0x04}, {0x5485, 0x00},
{0x5684, 0x04}, {0x5685, 0x00},
{0x5884, 0x04}, {0x5885, 0x00},
// red
{0x5286, 0x08}, {0x5287, 0xDE},
{0x5486, 0x08}, {0x5487, 0xDE},
{0x5686, 0x08}, {0x5687, 0xDE},
{0x5886, 0x08}, {0x5887, 0xDE},
// fixed gains
{0x3588, 0x01}, {0x3589, 0x00},
{0x35c8, 0x01}, {0x35c9, 0x00},
{0x3548, 0x0F}, {0x3549, 0x00},
{0x35c1, 0x00},
};

93
system/camerad/sensors/sensor.h
View File

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

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

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

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

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

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

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

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

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

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

@@ -1,83 +0,0 @@
#define CATCH_CONFIG_MAIN
#include "catch2/catch.hpp"
#include <cassert>
#include <cmath>
#include <cstring>
#include "common/util.h"
#include "system/camerad/cameras/camera_common.h"
#define W 240
#define H 160
#define TONE_SPLITS 3
float gts[TONE_SPLITS * TONE_SPLITS * TONE_SPLITS * TONE_SPLITS] = {
0.917969, 0.917969, 0.375000, 0.917969, 0.375000, 0.375000, 0.187500, 0.187500, 0.187500, 0.917969,
0.375000, 0.375000, 0.187500, 0.187500, 0.187500, 0.187500, 0.187500, 0.187500, 0.093750, 0.093750,
0.093750, 0.093750, 0.093750, 0.093750, 0.093750, 0.093750, 0.093750, 0.917969, 0.375000, 0.375000,
0.187500, 0.187500, 0.187500, 0.187500, 0.187500, 0.187500, 0.093750, 0.093750, 0.093750, 0.093750,
0.093750, 0.093750, 0.093750, 0.093750, 0.093750, 0.093750, 0.093750, 0.093750, 0.093750, 0.093750,
0.093750, 0.093750, 0.093750, 0.093750, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000,
0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000,
0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000,
0.000000};
TEST_CASE("camera.test_set_exposure_target") {
// set up fake camerabuf
CameraBuf cb = {};
VisionBuf vb = {};
uint8_t * fb_y = new uint8_t[W*H];
vb.y = fb_y;
cb.cur_yuv_buf = &vb;
cb.rgb_width = W;
cb.rgb_height = H;
printf("AE test patterns %dx%d\n", cb.rgb_width, cb.rgb_height);
// mix of 5 tones
uint8_t l[5] = {0, 24, 48, 96, 235}; // 235 is yuv max
bool passed = true;
float rtol = 0.05;
// generate pattern and calculate EV
int cnt = 0;
for (int i_0=0; i_0<TONE_SPLITS; i_0++) {
for (int i_1=0; i_1<TONE_SPLITS; i_1++) {
for (int i_2=0; i_2<TONE_SPLITS; i_2++) {
for (int i_3=0; i_3<TONE_SPLITS; i_3++) {
int h_0 = i_0 * H / TONE_SPLITS;
int h_1 = i_1 * (H - h_0) / TONE_SPLITS;
int h_2 = i_2 * (H - h_0 - h_1) / TONE_SPLITS;
int h_3 = i_3 * (H - h_0 - h_1 - h_2) / TONE_SPLITS;
int h_4 = H - h_0 - h_1 - h_2 - h_3;
memset(&fb_y[0], l[0], h_0*W);
memset(&fb_y[h_0*W], l[1], h_1*W);
memset(&fb_y[h_0*W+h_1*W], l[2], h_2*W);
memset(&fb_y[h_0*W+h_1*W+h_2*W], l[3], h_3*W);
memset(&fb_y[h_0*W+h_1*W+h_2*W+h_3*W], l[4], h_4*W);
float ev = set_exposure_target((const CameraBuf*) &cb, 0, W-1, 1, 0, H-1, 1);
// printf("%d/%d/%d/%d/%d ev is %f\n", h_0, h_1, h_2, h_3, h_4, ev);
// printf("%f\n", ev);
// compare to gt
float evgt = gts[cnt];
if (fabs(ev - evgt) > rtol*evgt) {
passed = false;
}
// report
printf("%d/%d/%d/%d/%d: ev %f, gt %f, err %f\n", h_0, h_1, h_2, h_3, h_4, ev, evgt, fabs(ev - evgt) / (evgt != 0 ? evgt : 0.00001f));
cnt++;
}
}
}
}
assert(passed);
delete[] fb_y;
}

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

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

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

@@ -1,55 +0,0 @@
#!/usr/bin/env python3
import time
import unittest
import numpy as np
from openpilot.selfdrive.test.helpers import with_processes, phone_only
from openpilot.system.camerad.snapshot.snapshot import get_snapshots
TEST_TIME = 45
REPEAT = 5
class TestCamerad(unittest.TestCase):
@classmethod
def setUpClass(cls):
pass
def _numpy_rgb2gray(self, im):
ret = np.clip(im[:,:,2] * 0.114 + im[:,:,1] * 0.587 + im[:,:,0] * 0.299, 0, 255).astype(np.uint8)
return ret
def _is_exposure_okay(self, i, med_mean=None):
if med_mean is None:
med_mean = np.array([[0.2,0.4],[0.2,0.6]])
h, w = i.shape[:2]
i = i[h//10:9*h//10,w//10:9*w//10]
med_ex, mean_ex = med_mean
i = self._numpy_rgb2gray(i)
i_median = np.median(i) / 255.
i_mean = np.mean(i) / 255.
print([i_median, i_mean])
return med_ex[0] < i_median < med_ex[1] and mean_ex[0] < i_mean < mean_ex[1]
@phone_only
@with_processes(['camerad'])
def test_camera_operation(self):
passed = 0
start = time.time()
while time.time() - start < TEST_TIME and passed < REPEAT:
rpic, dpic = get_snapshots(frame="roadCameraState", front_frame="driverCameraState")
wpic, _ = get_snapshots(frame="wideRoadCameraState")
res = self._is_exposure_okay(rpic)
res = res and self._is_exposure_okay(dpic)
res = res and self._is_exposure_okay(wpic)
if passed > 0 and not res:
passed = -passed # fails test if any failure after first sus
break
passed += int(res)
time.sleep(2)
self.assertGreaterEqual(passed, REPEAT)
if __name__ == "__main__":
unittest.main()

42
system/hardware/base.h
View File

@@ -1,42 +0,0 @@
#pragma once
#include <cstdlib>
#include <fstream>
#include <map>
#include <string>
#include "cereal/messaging/messaging.h"
// no-op base hw class
class HardwareNone {
public:
static constexpr float MAX_VOLUME = 0.7;
static constexpr float MIN_VOLUME = 0.2;
static std::string get_os_version() { return ""; }
static std::string get_name() { return ""; }
static cereal::InitData::DeviceType get_device_type() { return cereal::InitData::DeviceType::UNKNOWN; }
static int get_voltage() { return 0; }
static int get_current() { return 0; }
static std::string get_serial() { return "cccccc"; }
static std::map<std::string, std::string> get_init_logs() {
return {};
}
static void reboot() {}
static void soft_reboot() {}
static void poweroff() {}
static void set_brightness(int percent) {}
static void set_display_power(bool on) {}
static bool get_ssh_enabled() { return false; }
static void set_ssh_enabled(bool enabled) {}
static void config_cpu_rendering(bool offscreen);
static bool PC() { return false; }
static bool TICI() { return false; }
static bool AGNOS() { return false; }
};

4
system/hardware/base.py
View File

@@ -30,10 +30,6 @@ class HardwareBase(ABC):
def reboot(self, reason=None):
pass
@abstractmethod
def soft_reboot(self):
pass
@abstractmethod
def uninstall(self):
pass

50
system/hardware/hw.h
View File

@@ -1,50 +0,0 @@
#pragma once
#include <string>
#include "system/hardware/base.h"
#include "common/util.h"
#if QCOM2
#include "system/hardware/tici/hardware.h"
#define Hardware HardwareTici
#else
#include "system/hardware/pc/hardware.h"
#define Hardware HardwarePC
#endif
namespace Path {
inline std::string openpilot_prefix() {
return util::getenv("OPENPILOT_PREFIX", "");
}
inline std::string comma_home() {
return util::getenv("HOME") + "/.comma" + Path::openpilot_prefix();
}
inline std::string log_root() {
if (const char *env = getenv("LOG_ROOT")) {
return env;
}
return Hardware::PC() ? Path::comma_home() + "/media/0/realdata" : "/data/media/0/realdata";
}
inline std::string 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();
}
inline std::string download_cache_root() {
if (const char *env = getenv("COMMA_CACHE")) {
return env;
}
return "/tmp/comma_download_cache" + Path::openpilot_prefix() + "/";
}
} // namespace Path

23
system/hardware/pc/hardware.h
View File

@@ -1,23 +0,0 @@
#pragma once
#include <string>
#include "system/hardware/base.h"
class HardwarePC : public HardwareNone {
public:
static std::string get_os_version() { return "openpilot for PC"; }
static std::string get_name() { return "pc"; }
static cereal::InitData::DeviceType get_device_type() { return cereal::InitData::DeviceType::PC; }
static bool PC() { return true; }
static bool TICI() { return util::getenv("TICI", 0) == 1; }
static bool AGNOS() { return util::getenv("TICI", 0) == 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
}
};

3
system/hardware/pc/hardware.py
View File

@@ -19,9 +19,6 @@ class Pc(HardwareBase):
def reboot(self, reason=None):
print("REBOOT!")
def soft_reboot(self):
print("SOFT REBOOT!")
def uninstall(self):
print("uninstall")

113
system/hardware/tici/hardware.h
View File

@@ -1,113 +0,0 @@
#pragma once
#include <cstdlib>
#include <fstream>
#include <map>
#include <string>
#include "common/params.h"
#include "common/util.h"
#include "system/hardware/base.h"
class HardwareTici : public HardwareNone {
public:
static constexpr float MAX_VOLUME = 0.9;
static constexpr float MIN_VOLUME = 0.1;
static bool TICI() { return true; }
static bool AGNOS() { return true; }
static std::string get_os_version() {
return "AGNOS " + util::read_file("/VERSION");
}
static std::string get_name() {
std::string devicetree_model = util::read_file("/sys/firmware/devicetree/base/model");
return (devicetree_model.find("tizi") != std::string::npos) ? "tizi" : "tici";
}
static cereal::InitData::DeviceType get_device_type() {
return (get_name() == "tizi") ? cereal::InitData::DeviceType::TIZI : cereal::InitData::DeviceType::TICI;
}
static int get_voltage() { return std::atoi(util::read_file("/sys/class/hwmon/hwmon1/in1_input").c_str()); }
static int get_current() { return std::atoi(util::read_file("/sys/class/hwmon/hwmon1/curr1_input").c_str()); }
static std::string get_serial() {
static std::string serial("");
if (serial.empty()) {
std::ifstream stream("/proc/cmdline");
std::string cmdline;
std::getline(stream, cmdline);
auto start = cmdline.find("serialno=");
if (start == std::string::npos) {
serial = "cccccc";
} else {
auto end = cmdline.find(" ", start + 9);
serial = cmdline.substr(start + 9, end - start - 9);
}
}
return serial;
}
static void reboot() { std::system("sudo reboot"); }
static void soft_reboot() {
std::system("echo 894000.i2c | sudo tee /sys/bus/platform/drivers/i2c_geni/unbind");
std::this_thread::sleep_for(std::chrono::milliseconds(500));
std::system("echo 894000.i2c | sudo tee /sys/bus/platform/drivers/i2c_geni/bind");
std::this_thread::sleep_for(std::chrono::milliseconds(500));
std::system("sudo systemctl restart comma");
}
static void poweroff() { std::system("sudo poweroff"); }
static void set_brightness(int percent) {
std::string max = util::read_file("/sys/class/backlight/panel0-backlight/max_brightness");
std::ofstream brightness_control("/sys/class/backlight/panel0-backlight/brightness");
if (brightness_control.is_open()) {
brightness_control << (int)(percent * (std::stof(max)/100.)) << "\n";
brightness_control.close();
}
}
static void set_display_power(bool on) {
std::ofstream bl_power_control("/sys/class/backlight/panel0-backlight/bl_power");
if (bl_power_control.is_open()) {
bl_power_control << (on ? "0" : "4") << "\n";
bl_power_control.close();
}
}
static std::map<std::string, std::string> get_init_logs() {
std::map<std::string, std::string> ret = {
{"/BUILD", util::read_file("/BUILD")},
{"lsblk", util::check_output("lsblk -o NAME,SIZE,STATE,VENDOR,MODEL,REV,SERIAL")},
{"SOM ID", util::read_file("/sys/devices/platform/vendor/vendor:gpio-som-id/som_id")},
};
std::string bs = util::check_output("abctl --boot_slot");
ret["boot slot"] = bs.substr(0, bs.find_first_of("\n"));
std::string temp = util::read_file("/dev/disk/by-partlabel/ssd");
temp.erase(temp.find_last_not_of(std::string("\0\r\n", 3))+1);
ret["boot temp"] = temp;
// TODO: log something from system and boot
for (std::string part : {"xbl", "abl", "aop", "devcfg", "xbl_config"}) {
for (std::string slot : {"a", "b"}) {
std::string partition = part + "_" + slot;
std::string hash = util::check_output("sha256sum /dev/disk/by-partlabel/" + partition);
ret[partition] = hash.substr(0, hash.find_first_of(" "));
}
}
return ret;
}
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(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
}
};

8
system/hardware/tici/hardware.py
View File

@@ -134,14 +134,6 @@ class Tici(HardwareBase):
def reboot(self, reason=None):
subprocess.check_output(["sudo", "reboot"])
def soft_reboot(self):
# Reload the touchscreen driver to reset touch_count and avoid triggering a system reset prompt
sudo_write("894000.i2c", "/sys/bus/platform/drivers/i2c_geni/unbind")
time.sleep(0.5)
sudo_write("894000.i2c", "/sys/bus/platform/drivers/i2c_geni/bind")
time.sleep(0.5)
os.system("sudo systemctl restart comma")
def uninstall(self):
Path("/data/__system_reset__").touch()
os.sync()

3
system/logcatd/SConscript
View File

@@ -1,3 +0,0 @@
Import('env', 'cereal', 'messaging', 'common')
env.Program('logcatd', 'logcatd_systemd.cc', LIBS=[cereal, messaging, common, 'zmq', 'capnp', 'kj', 'systemd', 'json11'])

BIN
system/logcatd/logcatd Executable file
View File

Binary file not shown.

75
system/logcatd/logcatd_systemd.cc
View File

@@ -1,75 +0,0 @@
#include <systemd/sd-journal.h>
#include <cassert>
#include <csignal>
#include <map>
#include <string>
#include "third_party/json11/json11.hpp"
#include "cereal/messaging/messaging.h"
#include "common/timing.h"
#include "common/util.h"
ExitHandler do_exit;
int main(int argc, char *argv[]) {
PubMaster pm({"androidLog"});
sd_journal *journal;
int err = sd_journal_open(&journal, 0);
assert(err >= 0);
err = sd_journal_get_fd(journal); // needed so sd_journal_wait() works properly if files rotate
assert(err >= 0);
err = sd_journal_seek_tail(journal);
assert(err >= 0);
// workaround for bug https://github.com/systemd/systemd/issues/9934
// call sd_journal_previous_skip after sd_journal_seek_tail (like journalctl -f does) to makes things work.
sd_journal_previous_skip(journal, 1);
while (!do_exit) {
err = sd_journal_next(journal);
assert(err >= 0);
// Wait for new message if we didn't receive anything
if (err == 0) {
err = sd_journal_wait(journal, 1000 * 1000);
assert(err >= 0);
continue; // Try again
}
uint64_t timestamp = 0;
err = sd_journal_get_realtime_usec(journal, &timestamp);
assert(err >= 0);
const void *data;
size_t length;
std::map<std::string, std::string> kv;
SD_JOURNAL_FOREACH_DATA(journal, data, length) {
std::string str((char*)data, length);
// Split "KEY=VALUE"" on "=" and put in map
std::size_t found = str.find("=");
if (found != std::string::npos) {
kv[str.substr(0, found)] = str.substr(found + 1, std::string::npos);
}
}
MessageBuilder msg;
// Build message
auto androidEntry = msg.initEvent().initAndroidLog();
androidEntry.setTs(timestamp);
androidEntry.setMessage(json11::Json(kv).dump());
if (kv.count("_PID")) androidEntry.setPid(std::atoi(kv["_PID"].c_str()));
if (kv.count("PRIORITY")) androidEntry.setPriority(std::atoi(kv["PRIORITY"].c_str()));
if (kv.count("SYSLOG_IDENTIFIER")) androidEntry.setTag(kv["SYSLOG_IDENTIFIER"]);
pm.send("androidLog", msg);
}
sd_journal_close(journal);
return 0;
}

27
system/loggerd/SConscript
View File

@@ -1,27 +0,0 @@
Import('env', 'arch', 'cereal', 'messaging', 'common', 'visionipc')
libs = [common, cereal, messaging, visionipc,
'zmq', 'capnp', 'kj', 'z',
'avformat', 'avcodec', 'swscale', 'avutil',
'yuv', 'OpenCL', 'pthread']
src = ['logger.cc', 'video_writer.cc', 'encoder/encoder.cc', 'encoder/v4l_encoder.cc']
if arch != "larch64":
src += ['encoder/ffmpeg_encoder.cc']
if arch == "Darwin":
# fix OpenCL
del libs[libs.index('OpenCL')]
env['FRAMEWORKS'] = ['OpenCL']
# exclude v4l
del src[src.index('encoder/v4l_encoder.cc')]
logger_lib = env.Library('logger', src)
libs.insert(0, logger_lib)
env.Program('loggerd', ['loggerd.cc'], LIBS=libs)
env.Program('encoderd', ['encoderd.cc'], LIBS=libs)
env.Program('bootlog.cc', LIBS=libs)
if GetOption('extras'):
env.Program('tests/test_logger', ['tests/test_runner.cc', 'tests/test_logger.cc'], LIBS=libs + ['curl', 'crypto'])

BIN
system/loggerd/bootlog Executable file
View File

Binary file not shown.

70
system/loggerd/bootlog.cc
View File

@@ -1,70 +0,0 @@
#include <cassert>
#include <string>
#include "cereal/messaging/messaging.h"
#include "common/params.h"
#include "common/swaglog.h"
#include "system/loggerd/logger.h"
static kj::Array<capnp::word> build_boot_log() {
MessageBuilder msg;
auto boot = msg.initEvent().initBoot();
boot.setWallTimeNanos(nanos_since_epoch());
std::string pstore = "/sys/fs/pstore";
std::map<std::string, std::string> pstore_map = util::read_files_in_dir(pstore);
int i = 0;
auto lpstore = boot.initPstore().initEntries(pstore_map.size());
for (auto& kv : pstore_map) {
auto lentry = lpstore[i];
lentry.setKey(kv.first);
lentry.setValue(capnp::Data::Reader((const kj::byte*)kv.second.data(), kv.second.size()));
i++;
}
// Gather output of commands
std::vector<std::string> bootlog_commands = {
"[ -x \"$(command -v journalctl)\" ] && journalctl",
};
if (Hardware::TICI()) {
bootlog_commands.push_back("[ -e /dev/nvme0 ] && sudo nvme smart-log --output-format=json /dev/nvme0");
}
auto commands = boot.initCommands().initEntries(bootlog_commands.size());
for (int j = 0; j < bootlog_commands.size(); j++) {
auto lentry = commands[j];
lentry.setKey(bootlog_commands[j]);
const std::string result = util::check_output(bootlog_commands[j]);
lentry.setValue(capnp::Data::Reader((const kj::byte*)result.data(), result.size()));
}
boot.setLaunchLog(util::read_file("/tmp/launch_log"));
return capnp::messageToFlatArray(msg);
}
int main(int argc, char** argv) {
const std::string id = logger_get_identifier("BootCount");
const std::string path = Path::log_root() + "/boot/" + id;
LOGW("bootlog to %s", path.c_str());
// Open bootlog
bool r = util::create_directories(Path::log_root() + "/boot/", 0775);
assert(r);
RawFile file(path.c_str());
// Write initdata
file.write(logger_build_init_data().asBytes());
// Write bootlog
file.write(build_boot_log().asBytes());
// Write out bootlog param to match routes with bootlog
Params().put("CurrentBootlog", id.c_str());
return 0;
}

43
system/loggerd/encoder/encoder.cc
View File

@@ -1,43 +0,0 @@
#include "system/loggerd/encoder/encoder.h"
VideoEncoder::VideoEncoder(const EncoderInfo &encoder_info, int in_width, int in_height)
: encoder_info(encoder_info), in_width(in_width), in_height(in_height) {
out_width = encoder_info.frame_width > 0 ? encoder_info.frame_width : in_width;
out_height = encoder_info.frame_height > 0 ? encoder_info.frame_height : in_height;
pm.reset(new PubMaster({encoder_info.publish_name}));
}
void VideoEncoder::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) {
// broadcast packet
MessageBuilder msg;
auto event = msg.initEvent(true);
auto edat = (event.*(e->encoder_info.init_encode_data_func))();
auto edata = edat.initIdx();
struct timespec ts;
timespec_get(&ts, TIME_UTC);
edat.setUnixTimestampNanos((uint64_t)ts.tv_sec*1000000000 + ts.tv_nsec);
edata.setFrameId(extra.frame_id);
edata.setTimestampSof(extra.timestamp_sof);
edata.setTimestampEof(extra.timestamp_eof);
edata.setType(e->encoder_info.encode_type);
edata.setEncodeId(e->cnt++);
edata.setSegmentNum(segment_num);
edata.setSegmentId(idx);
edata.setFlags(flags);
edata.setLen(dat.size());
edat.setData(dat);
edat.setWidth(out_width);
edat.setHeight(out_height);
if (flags & V4L2_BUF_FLAG_KEYFRAME) edat.setHeader(header);
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);
}

37
system/loggerd/encoder/encoder.h
View File

@@ -1,37 +0,0 @@
#pragma once
#include <cassert>
#include <cstdint>
#include <memory>
#include <thread>
#include <vector>
#include "cereal/messaging/messaging.h"
#include "cereal/visionipc/visionipc.h"
#include "common/queue.h"
#include "system/camerad/cameras/camera_common.h"
#include "system/loggerd/loggerd.h"
#define V4L2_BUF_FLAG_KEYFRAME 8
class VideoEncoder {
public:
VideoEncoder(const EncoderInfo &encoder_info, int in_width, int in_height);
virtual ~VideoEncoder() {}
virtual int encode_frame(VisionBuf* buf, VisionIpcBufExtra *extra) = 0;
virtual void encoder_open(const char* path) = 0;
virtual void encoder_close() = 0;
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;
int out_width, out_height;
const EncoderInfo encoder_info;
private:
// total frames encoded
int cnt = 0;
std::unique_ptr<PubMaster> pm;
std::vector<capnp::byte> msg_cache;
};

150
system/loggerd/encoder/ffmpeg_encoder.cc
View File

@@ -1,150 +0,0 @@
#pragma clang diagnostic ignored "-Wdeprecated-declarations"
#include "system/loggerd/encoder/ffmpeg_encoder.h"
#include <fcntl.h>
#include <unistd.h>
#include <cassert>
#include <cstdio>
#include <cstdlib>
#define __STDC_CONSTANT_MACROS
#include "third_party/libyuv/include/libyuv.h"
extern "C" {
#include <libavcodec/avcodec.h>
#include <libavformat/avformat.h>
#include <libavutil/imgutils.h>
}
#include "common/swaglog.h"
#include "common/util.h"
const int env_debug_encoder = (getenv("DEBUG_ENCODER") != NULL) ? atoi(getenv("DEBUG_ENCODER")) : 0;
FfmpegEncoder::FfmpegEncoder(const EncoderInfo &encoder_info, int in_width, int in_height)
: VideoEncoder(encoder_info, in_width, in_height) {
frame = av_frame_alloc();
assert(frame);
frame->format = AV_PIX_FMT_YUV420P;
frame->width = out_width;
frame->height = out_height;
frame->linesize[0] = out_width;
frame->linesize[1] = out_width/2;
frame->linesize[2] = out_width/2;
convert_buf.resize(in_width * in_height * 3 / 2);
if (in_width != out_width || in_height != out_height) {
downscale_buf.resize(out_width * out_height * 3 / 2);
}
}
FfmpegEncoder::~FfmpegEncoder() {
encoder_close();
av_frame_free(&frame);
}
void FfmpegEncoder::encoder_open(const char* path) {
const AVCodec *codec = avcodec_find_encoder(AV_CODEC_ID_FFVHUFF);
this->codec_ctx = avcodec_alloc_context3(codec);
assert(this->codec_ctx);
this->codec_ctx->width = frame->width;
this->codec_ctx->height = frame->height;
this->codec_ctx->pix_fmt = AV_PIX_FMT_YUV420P;
this->codec_ctx->time_base = (AVRational){ 1, encoder_info.fps };
int err = avcodec_open2(this->codec_ctx, codec, NULL);
assert(err >= 0);
is_open = true;
segment_num++;
counter = 0;
}
void FfmpegEncoder::encoder_close() {
if (!is_open) return;
avcodec_free_context(&codec_ctx);
is_open = false;
}
int FfmpegEncoder::encode_frame(VisionBuf* buf, VisionIpcBufExtra *extra) {
assert(buf->width == this->in_width);
assert(buf->height == this->in_height);
uint8_t *cy = convert_buf.data();
uint8_t *cu = cy + in_width * in_height;
uint8_t *cv = cu + (in_width / 2) * (in_height / 2);
libyuv::NV12ToI420(buf->y, buf->stride,
buf->uv, buf->stride,
cy, in_width,
cu, in_width/2,
cv, in_width/2,
in_width, in_height);
if (downscale_buf.size() > 0) {
uint8_t *out_y = downscale_buf.data();
uint8_t *out_u = out_y + frame->width * frame->height;
uint8_t *out_v = out_u + (frame->width / 2) * (frame->height / 2);
libyuv::I420Scale(cy, in_width,
cu, in_width/2,
cv, in_width/2,
in_width, in_height,
out_y, frame->width,
out_u, frame->width/2,
out_v, frame->width/2,
frame->width, frame->height,
libyuv::kFilterNone);
frame->data[0] = out_y;
frame->data[1] = out_u;
frame->data[2] = out_v;
} else {
frame->data[0] = cy;
frame->data[1] = cu;
frame->data[2] = cv;
}
frame->pts = counter*50*1000; // 50ms per frame
int ret = counter;
int err = avcodec_send_frame(this->codec_ctx, frame);
if (err < 0) {
LOGE("avcodec_send_frame error %d", err);
ret = -1;
}
AVPacket pkt;
av_init_packet(&pkt);
pkt.data = NULL;
pkt.size = 0;
while (ret >= 0) {
err = avcodec_receive_packet(this->codec_ctx, &pkt);
if (err == AVERROR_EOF) {
break;
} else if (err == AVERROR(EAGAIN)) {
// Encoder might need a few frames on startup to get started. Keep going
ret = 0;
break;
} else if (err < 0) {
LOGE("avcodec_receive_packet error %d", err);
ret = -1;
break;
}
if (env_debug_encoder) {
printf("%20s got %8d bytes flags %8x idx %4d id %8d\n", encoder_info.publish_name, pkt.size, pkt.flags, counter, extra->frame_id);
}
publisher_publish(this, segment_num, counter, *extra,
(pkt.flags & AV_PKT_FLAG_KEY) ? V4L2_BUF_FLAG_KEYFRAME : 0,
kj::arrayPtr<capnp::byte>(pkt.data, (size_t)0), // TODO: get the header
kj::arrayPtr<capnp::byte>(pkt.data, pkt.size));
counter++;
}
av_packet_unref(&pkt);
return ret;
}

34
system/loggerd/encoder/ffmpeg_encoder.h
View File

@@ -1,34 +0,0 @@
#pragma once
#include <cstdio>
#include <cstdlib>
#include <string>
#include <vector>
extern "C" {
#include <libavcodec/avcodec.h>
#include <libavformat/avformat.h>
#include <libavutil/imgutils.h>
}
#include "system/loggerd/encoder/encoder.h"
#include "system/loggerd/loggerd.h"
class FfmpegEncoder : public VideoEncoder {
public:
FfmpegEncoder(const EncoderInfo &encoder_info, int in_width, int in_height);
~FfmpegEncoder();
int encode_frame(VisionBuf* buf, VisionIpcBufExtra *extra);
void encoder_open(const char* path);
void encoder_close();
private:
int segment_num = -1;
int counter = 0;
bool is_open = false;
AVCodecContext *codec_ctx;
AVFrame *frame = NULL;
std::vector<uint8_t> convert_buf;
std::vector<uint8_t> downscale_buf;
};

326
system/loggerd/encoder/v4l_encoder.cc
View File

@@ -1,326 +0,0 @@
#include <cassert>
#include <string>
#include <sys/ioctl.h>
#include <poll.h>
#include "system/loggerd/encoder/v4l_encoder.h"
#include "common/util.h"
#include "common/timing.h"
#include "third_party/libyuv/include/libyuv.h"
#include "third_party/linux/include/msm_media_info.h"
// has to be in this order
#include "third_party/linux/include/v4l2-controls.h"
#include <linux/videodev2.h>
#define V4L2_QCOM_BUF_FLAG_CODECCONFIG 0x00020000
#define V4L2_QCOM_BUF_FLAG_EOS 0x02000000
// echo 0x7fffffff > /sys/kernel/debug/msm_vidc/debug_level
const int env_debug_encoder = (getenv("DEBUG_ENCODER") != NULL) ? atoi(getenv("DEBUG_ENCODER")) : 0;
static void checked_ioctl(int fd, unsigned long request, void *argp) {
int ret = util::safe_ioctl(fd, request, argp);
if (ret != 0) {
LOGE("checked_ioctl failed with error %d (%d %lx %p)", errno, fd, request, argp);
assert(0);
}
}
static void dequeue_buffer(int fd, v4l2_buf_type buf_type, unsigned int *index=NULL, unsigned int *bytesused=NULL, unsigned int *flags=NULL, struct timeval *timestamp=NULL) {
v4l2_plane plane = {0};
v4l2_buffer v4l_buf = {
.type = buf_type,
.memory = V4L2_MEMORY_USERPTR,
.m = { .planes = &plane, },
.length = 1,
};
checked_ioctl(fd, VIDIOC_DQBUF, &v4l_buf);
if (index) *index = v4l_buf.index;
if (bytesused) *bytesused = v4l_buf.m.planes[0].bytesused;
if (flags) *flags = v4l_buf.flags;
if (timestamp) *timestamp = v4l_buf.timestamp;
assert(v4l_buf.m.planes[0].data_offset == 0);
}
static void queue_buffer(int fd, v4l2_buf_type buf_type, unsigned int index, VisionBuf *buf, struct timeval timestamp={}) {
v4l2_plane plane = {
.length = (unsigned int)buf->len,
.m = { .userptr = (unsigned long)buf->addr, },
.bytesused = (uint32_t)buf->len,
.reserved = {(unsigned int)buf->fd}
};
v4l2_buffer v4l_buf = {
.type = buf_type,
.index = index,
.memory = V4L2_MEMORY_USERPTR,
.m = { .planes = &plane, },
.length = 1,
.flags = V4L2_BUF_FLAG_TIMESTAMP_COPY,
.timestamp = timestamp
};
checked_ioctl(fd, VIDIOC_QBUF, &v4l_buf);
}
static void request_buffers(int fd, v4l2_buf_type buf_type, unsigned int count) {
struct v4l2_requestbuffers reqbuf = {
.type = buf_type,
.memory = V4L2_MEMORY_USERPTR,
.count = count
};
checked_ioctl(fd, VIDIOC_REQBUFS, &reqbuf);
}
void V4LEncoder::dequeue_handler(V4LEncoder *e) {
std::string dequeue_thread_name = "dq-"+std::string(e->encoder_info.publish_name);
util::set_thread_name(dequeue_thread_name.c_str());
e->segment_num++;
uint32_t idx = -1;
bool exit = false;
// POLLIN is capture, POLLOUT is frame
struct pollfd pfd;
pfd.events = POLLIN | POLLOUT;
pfd.fd = e->fd;
// save the header
kj::Array<capnp::byte> header;
while (!exit) {
int rc = poll(&pfd, 1, 1000);
if (rc < 0) {
if (errno != EINTR) {
// TODO: exit encoder?
// ignore the error and keep going
LOGE("poll failed (%d - %d)", rc, errno);
}
continue;
} else if (rc == 0) {
LOGE("encoder dequeue poll timeout");
continue;
}
if (env_debug_encoder >= 2) {
printf("%20s poll %x at %.2f ms\n", e->encoder_info.publish_name, pfd.revents, millis_since_boot());
}
int frame_id = -1;
if (pfd.revents & POLLIN) {
unsigned int bytesused, flags, index;
struct timeval timestamp;
dequeue_buffer(e->fd, V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE, &index, &bytesused, &flags, &timestamp);
e->buf_out[index].sync(VISIONBUF_SYNC_FROM_DEVICE);
uint8_t *buf = (uint8_t*)e->buf_out[index].addr;
int64_t ts = timestamp.tv_sec * 1000000 + timestamp.tv_usec;
// eof packet, we exit
if (flags & V4L2_QCOM_BUF_FLAG_EOS) {
exit = true;
} else if (flags & V4L2_QCOM_BUF_FLAG_CODECCONFIG) {
// save header
header = kj::heapArray<capnp::byte>(buf, bytesused);
} else {
VisionIpcBufExtra extra = e->extras.pop();
assert(extra.timestamp_eof/1000 == ts); // stay in sync
frame_id = extra.frame_id;
++idx;
e->publisher_publish(e, e->segment_num, idx, extra, flags, header, kj::arrayPtr<capnp::byte>(buf, bytesused));
}
if (env_debug_encoder) {
printf("%20s got(%d) %6d bytes flags %8x idx %3d/%4d id %8d ts %ld lat %.2f ms (%lu frames free)\n",
e->encoder_info.publish_name, index, bytesused, flags, e->segment_num, idx, frame_id, ts, millis_since_boot()-(ts/1000.), e->free_buf_in.size());
}
// requeue the buffer
queue_buffer(e->fd, V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE, index, &e->buf_out[index]);
}
if (pfd.revents & POLLOUT) {
unsigned int index;
dequeue_buffer(e->fd, V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE, &index);
e->free_buf_in.push(index);
}
}
}
V4LEncoder::V4LEncoder(const EncoderInfo &encoder_info, int in_width, int in_height)
: VideoEncoder(encoder_info, in_width, in_height) {
fd = open("/dev/v4l/by-path/platform-aa00000.qcom_vidc-video-index1", O_RDWR|O_NONBLOCK);
assert(fd >= 0);
struct v4l2_capability cap;
checked_ioctl(fd, VIDIOC_QUERYCAP, &cap);
LOGD("opened encoder device %s %s = %d", cap.driver, cap.card, fd);
assert(strcmp((const char *)cap.driver, "msm_vidc_driver") == 0);
assert(strcmp((const char *)cap.card, "msm_vidc_venc") == 0);
struct v4l2_format fmt_out = {
.type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE,
.fmt = {
.pix_mp = {
// downscales are free with v4l
.width = (unsigned int)(out_width),
.height = (unsigned int)(out_height),
.pixelformat = (encoder_info.encode_type == cereal::EncodeIndex::Type::FULL_H_E_V_C) ? V4L2_PIX_FMT_HEVC : V4L2_PIX_FMT_H264,
.field = V4L2_FIELD_ANY,
.colorspace = V4L2_COLORSPACE_DEFAULT,
}
}
};
checked_ioctl(fd, VIDIOC_S_FMT, &fmt_out);
v4l2_streamparm streamparm = {
.type = V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE,
.parm = {
.output = {
// TODO: more stuff here? we don't know
.timeperframe = {
.numerator = 1,
.denominator = 20
}
}
}
};
checked_ioctl(fd, VIDIOC_S_PARM, &streamparm);
struct v4l2_format fmt_in = {
.type = V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE,
.fmt = {
.pix_mp = {
.width = (unsigned int)in_width,
.height = (unsigned int)in_height,
.pixelformat = V4L2_PIX_FMT_NV12,
.field = V4L2_FIELD_ANY,
.colorspace = V4L2_COLORSPACE_470_SYSTEM_BG,
}
}
};
checked_ioctl(fd, VIDIOC_S_FMT, &fmt_in);
LOGD("in buffer size %d, out buffer size %d",
fmt_in.fmt.pix_mp.plane_fmt[0].sizeimage,
fmt_out.fmt.pix_mp.plane_fmt[0].sizeimage);
// shared ctrls
{
struct v4l2_control ctrls[] = {
{ .id = V4L2_CID_MPEG_VIDEO_HEADER_MODE, .value = V4L2_MPEG_VIDEO_HEADER_MODE_SEPARATE},
{ .id = V4L2_CID_MPEG_VIDEO_BITRATE, .value = encoder_info.bitrate},
{ .id = V4L2_CID_MPEG_VIDC_VIDEO_RATE_CONTROL, .value = V4L2_CID_MPEG_VIDC_VIDEO_RATE_CONTROL_VBR_CFR},
{ .id = V4L2_CID_MPEG_VIDC_VIDEO_PRIORITY, .value = V4L2_MPEG_VIDC_VIDEO_PRIORITY_REALTIME_DISABLE},
{ .id = V4L2_CID_MPEG_VIDC_VIDEO_IDR_PERIOD, .value = 1},
};
for (auto ctrl : ctrls) {
checked_ioctl(fd, VIDIOC_S_CTRL, &ctrl);
}
}
if (encoder_info.encode_type == cereal::EncodeIndex::Type::FULL_H_E_V_C) {
struct v4l2_control ctrls[] = {
{ .id = V4L2_CID_MPEG_VIDC_VIDEO_HEVC_PROFILE, .value = V4L2_MPEG_VIDC_VIDEO_HEVC_PROFILE_MAIN},
{ .id = V4L2_CID_MPEG_VIDC_VIDEO_HEVC_TIER_LEVEL, .value = V4L2_MPEG_VIDC_VIDEO_HEVC_LEVEL_HIGH_TIER_LEVEL_5},
{ .id = V4L2_CID_MPEG_VIDC_VIDEO_VUI_TIMING_INFO, .value = V4L2_MPEG_VIDC_VIDEO_VUI_TIMING_INFO_ENABLED},
{ .id = V4L2_CID_MPEG_VIDC_VIDEO_NUM_P_FRAMES, .value = 29},
{ .id = V4L2_CID_MPEG_VIDC_VIDEO_NUM_B_FRAMES, .value = 0},
};
for (auto ctrl : ctrls) {
checked_ioctl(fd, VIDIOC_S_CTRL, &ctrl);
}
} else {
struct v4l2_control ctrls[] = {
{ .id = V4L2_CID_MPEG_VIDEO_H264_PROFILE, .value = V4L2_MPEG_VIDEO_H264_PROFILE_HIGH},
{ .id = V4L2_CID_MPEG_VIDEO_H264_LEVEL, .value = V4L2_MPEG_VIDEO_H264_LEVEL_UNKNOWN},
{ .id = V4L2_CID_MPEG_VIDC_VIDEO_NUM_P_FRAMES, .value = 14},
{ .id = V4L2_CID_MPEG_VIDC_VIDEO_NUM_B_FRAMES, .value = 0},
{ .id = V4L2_CID_MPEG_VIDEO_H264_ENTROPY_MODE, .value = V4L2_MPEG_VIDEO_H264_ENTROPY_MODE_CABAC},
{ .id = V4L2_CID_MPEG_VIDC_VIDEO_H264_CABAC_MODEL, .value = V4L2_CID_MPEG_VIDC_VIDEO_H264_CABAC_MODEL_0},
{ .id = V4L2_CID_MPEG_VIDEO_H264_LOOP_FILTER_MODE, .value = 0},
{ .id = V4L2_CID_MPEG_VIDEO_H264_LOOP_FILTER_ALPHA, .value = 0},
{ .id = V4L2_CID_MPEG_VIDEO_H264_LOOP_FILTER_BETA, .value = 0},
{ .id = V4L2_CID_MPEG_VIDEO_MULTI_SLICE_MODE, .value = 0},
};
for (auto ctrl : ctrls) {
checked_ioctl(fd, VIDIOC_S_CTRL, &ctrl);
}
}
// allocate buffers
request_buffers(fd, V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE, BUF_OUT_COUNT);
request_buffers(fd, V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE, BUF_IN_COUNT);
// start encoder
v4l2_buf_type buf_type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
checked_ioctl(fd, VIDIOC_STREAMON, &buf_type);
buf_type = V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE;
checked_ioctl(fd, VIDIOC_STREAMON, &buf_type);
// queue up output buffers
for (unsigned int i = 0; i < BUF_OUT_COUNT; i++) {
buf_out[i].allocate(fmt_out.fmt.pix_mp.plane_fmt[0].sizeimage);
queue_buffer(fd, V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE, i, &buf_out[i]);
}
// queue up input buffers
for (unsigned int i = 0; i < BUF_IN_COUNT; i++) {
free_buf_in.push(i);
}
}
void V4LEncoder::encoder_open(const char* path) {
dequeue_handler_thread = std::thread(V4LEncoder::dequeue_handler, this);
this->is_open = true;
this->counter = 0;
}
int V4LEncoder::encode_frame(VisionBuf* buf, VisionIpcBufExtra *extra) {
struct timeval timestamp {
.tv_sec = (long)(extra->timestamp_eof/1000000000),
.tv_usec = (long)((extra->timestamp_eof/1000) % 1000000),
};
// reserve buffer
int buffer_in = free_buf_in.pop();
// push buffer
extras.push(*extra);
//buf->sync(VISIONBUF_SYNC_TO_DEVICE);
queue_buffer(fd, V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE, buffer_in, buf, timestamp);
return this->counter++;
}
void V4LEncoder::encoder_close() {
if (this->is_open) {
// pop all the frames before closing, then put the buffers back
for (int i = 0; i < BUF_IN_COUNT; i++) free_buf_in.pop();
for (int i = 0; i < BUF_IN_COUNT; i++) free_buf_in.push(i);
// no frames, stop the encoder
struct v4l2_encoder_cmd encoder_cmd = { .cmd = V4L2_ENC_CMD_STOP };
checked_ioctl(fd, VIDIOC_ENCODER_CMD, &encoder_cmd);
// join waits for V4L2_QCOM_BUF_FLAG_EOS
dequeue_handler_thread.join();
assert(extras.empty());
}
this->is_open = false;
}
V4LEncoder::~V4LEncoder() {
encoder_close();
v4l2_buf_type buf_type = V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE;
checked_ioctl(fd, VIDIOC_STREAMOFF, &buf_type);
request_buffers(fd, V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE, 0);
buf_type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
checked_ioctl(fd, VIDIOC_STREAMOFF, &buf_type);
request_buffers(fd, V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE, 0);
close(fd);
for (int i = 0; i < BUF_OUT_COUNT; i++) {
if (buf_out[i].free() != 0) {
LOGE("Failed to free buffer");
}
}
}

30
system/loggerd/encoder/v4l_encoder.h
View File

@@ -1,30 +0,0 @@
#pragma once
#include "common/queue.h"
#include "system/loggerd/encoder/encoder.h"
#define BUF_IN_COUNT 7
#define BUF_OUT_COUNT 6
class V4LEncoder : public VideoEncoder {
public:
V4LEncoder(const EncoderInfo &encoder_info, int in_width, int in_height);
~V4LEncoder();
int encode_frame(VisionBuf* buf, VisionIpcBufExtra *extra);
void encoder_open(const char* path);
void encoder_close();
private:
int fd;
bool is_open = false;
int segment_num = -1;
int counter = 0;
SafeQueue<VisionIpcBufExtra> extras;
static void dequeue_handler(V4LEncoder *e);
std::thread dequeue_handler_thread;
VisionBuf buf_out[BUF_OUT_COUNT];
SafeQueue<unsigned int> free_buf_in;
};

BIN
system/loggerd/encoderd Executable file
View File

Binary file not shown.

161
system/loggerd/encoderd.cc
View File

@@ -1,161 +0,0 @@
#include <cassert>
#include "system/loggerd/loggerd.h"
#ifdef QCOM2
#include "system/loggerd/encoder/v4l_encoder.h"
#define Encoder V4LEncoder
#else
#include "system/loggerd/encoder/ffmpeg_encoder.h"
#define Encoder FfmpegEncoder
#endif
ExitHandler do_exit;
struct EncoderdState {
int max_waiting = 0;
// Sync logic for startup
std::atomic<int> encoders_ready = 0;
std::atomic<uint32_t> start_frame_id = 0;
bool camera_ready[WideRoadCam + 1] = {};
bool camera_synced[WideRoadCam + 1] = {};
};
// Handle initial encoder syncing by waiting for all encoders to reach the same frame id
bool sync_encoders(EncoderdState *s, CameraType cam_type, uint32_t frame_id) {
if (s->camera_synced[cam_type]) return true;
if (s->max_waiting > 1 && s->encoders_ready != s->max_waiting) {
// add a small margin to the start frame id in case one of the encoders already dropped the next frame
update_max_atomic(s->start_frame_id, frame_id + 2);
if (std::exchange(s->camera_ready[cam_type], true) == false) {
++s->encoders_ready;
LOGD("camera %d encoder ready", cam_type);
}
return false;
} else {
if (s->max_waiting == 1) update_max_atomic(s->start_frame_id, frame_id);
bool synced = frame_id >= s->start_frame_id;
s->camera_synced[cam_type] = synced;
if (!synced) LOGD("camera %d waiting for frame %d, cur %d", cam_type, (int)s->start_frame_id, frame_id);
return synced;
}
}
void encoder_thread(EncoderdState *s, const LogCameraInfo &cam_info) {
util::set_thread_name(cam_info.thread_name);
std::vector<std::unique_ptr<Encoder>> encoders;
VisionIpcClient vipc_client = VisionIpcClient("camerad", cam_info.stream_type, false);
int cur_seg = 0;
while (!do_exit) {
if (!vipc_client.connect(false)) {
util::sleep_for(5);
continue;
}
// init encoders
if (encoders.empty()) {
VisionBuf buf_info = vipc_client.buffers[0];
LOGW("encoder %s init %zux%zu", cam_info.thread_name, buf_info.width, buf_info.height);
assert(buf_info.width > 0 && buf_info.height > 0);
for (const auto &encoder_info : cam_info.encoder_infos) {
auto &e = encoders.emplace_back(new Encoder(encoder_info, buf_info.width, buf_info.height));
e->encoder_open(nullptr);
}
}
bool lagging = false;
while (!do_exit) {
VisionIpcBufExtra extra;
VisionBuf* buf = vipc_client.recv(&extra);
if (buf == nullptr) continue;
// detect loop around and drop the frames
if (buf->get_frame_id() != extra.frame_id) {
if (!lagging) {
LOGE("encoder %s lag buffer id: %" PRIu64 " extra id: %d", cam_info.thread_name, buf->get_frame_id(), extra.frame_id);
lagging = true;
}
continue;
}
lagging = false;
if (!sync_encoders(s, cam_info.type, extra.frame_id)) {
continue;
}
if (do_exit) break;
// do rotation if required
const int frames_per_seg = SEGMENT_LENGTH * MAIN_FPS;
if (cur_seg >= 0 && extra.frame_id >= ((cur_seg + 1) * frames_per_seg) + s->start_frame_id) {
for (auto &e : encoders) {
e->encoder_close();
e->encoder_open(NULL);
}
++cur_seg;
}
// encode a frame
for (int i = 0; i < encoders.size(); ++i) {
int out_id = encoders[i]->encode_frame(buf, &extra);
if (out_id == -1) {
LOGE("Failed to encode frame. frame_id: %d", extra.frame_id);
}
}
}
}
}
template <size_t N>
void encoderd_thread(const LogCameraInfo (&cameras)[N]) {
EncoderdState s;
std::set<VisionStreamType> streams;
while (!do_exit) {
streams = VisionIpcClient::getAvailableStreams("camerad", false);
if (!streams.empty()) {
break;
}
util::sleep_for(100);
}
if (!streams.empty()) {
std::vector<std::thread> encoder_threads;
for (auto stream : streams) {
auto it = std::find_if(std::begin(cameras), std::end(cameras),
[stream](auto &cam) { return cam.stream_type == stream; });
assert(it != std::end(cameras));
++s.max_waiting;
encoder_threads.push_back(std::thread(encoder_thread, &s, *it));
}
for (auto &t : encoder_threads) t.join();
}
}
int main(int argc, char* argv[]) {
if (!Hardware::PC()) {
int ret;
ret = util::set_realtime_priority(52);
assert(ret == 0);
ret = util::set_core_affinity({3});
assert(ret == 0);
}
if (argc > 1) {
std::string arg1(argv[1]);
if (arg1 == "--stream") {
encoderd_thread(stream_cameras_logged);
} else {
LOGE("Argument '%s' is not supported", arg1.c_str());
}
} else {
encoderd_thread(cameras_logged);
}
return 0;
}

172
system/loggerd/logger.cc
View File

@@ -1,172 +0,0 @@
#include "system/loggerd/logger.h"
#include <fstream>
#include <map>
#include <vector>
#include <iostream>
#include <sstream>
#include <random>
#include "common/params.h"
#include "common/swaglog.h"
#include "common/version.h"
// ***** log metadata *****
kj::Array<capnp::word> logger_build_init_data() {
uint64_t wall_time = nanos_since_epoch();
MessageBuilder msg;
auto init = msg.initEvent().initInitData();
init.setWallTimeNanos(wall_time);
init.setVersion(COMMA_VERSION);
init.setDirty(!getenv("CLEAN"));
init.setDeviceType(Hardware::get_device_type());
// log kernel args
std::ifstream cmdline_stream("/proc/cmdline");
std::vector<std::string> kernel_args;
std::string buf;
while (cmdline_stream >> buf) {
kernel_args.push_back(buf);
}
auto lkernel_args = init.initKernelArgs(kernel_args.size());
for (int i=0; i<kernel_args.size(); i++) {
lkernel_args.set(i, kernel_args[i]);
}
init.setKernelVersion(util::read_file("/proc/version"));
init.setOsVersion(util::read_file("/VERSION"));
// log params
auto params = Params(util::getenv("PARAMS_COPY_PATH", ""));
std::map<std::string, std::string> params_map = params.readAll();
init.setGitCommit(params_map["GitCommit"]);
init.setGitCommitDate(params_map["GitCommitDate"]);
init.setGitBranch(params_map["GitBranch"]);
init.setGitRemote(params_map["GitRemote"]);
init.setPassive(false);
init.setDongleId(params_map["DongleId"]);
auto lparams = init.initParams().initEntries(params_map.size());
int j = 0;
for (auto& [key, value] : params_map) {
auto lentry = lparams[j];
lentry.setKey(key);
if ( !(params.getKeyType(key) & DONT_LOG) ) {
lentry.setValue(capnp::Data::Reader((const kj::byte*)value.data(), value.size()));
}
j++;
}
// log commands
std::vector<std::string> log_commands = {
"df -h", // usage for all filesystems
};
auto hw_logs = Hardware::get_init_logs();
auto commands = init.initCommands().initEntries(log_commands.size() + hw_logs.size());
for (int i = 0; i < log_commands.size(); i++) {
auto lentry = commands[i];
lentry.setKey(log_commands[i]);
const std::string result = util::check_output(log_commands[i]);
lentry.setValue(capnp::Data::Reader((const kj::byte*)result.data(), result.size()));
}
int i = log_commands.size();
for (auto &[key, value] : hw_logs) {
auto lentry = commands[i];
lentry.setKey(key);
lentry.setValue(capnp::Data::Reader((const kj::byte*)value.data(), value.size()));
i++;
}
return capnp::messageToFlatArray(msg);
}
std::string logger_get_route_name() {
char route_name[64] = {'\0'};
time_t rawtime = time(NULL);
struct tm timeinfo;
localtime_r(&rawtime, &timeinfo);
strftime(route_name, sizeof(route_name), "%Y-%m-%d--%H-%M-%S", &timeinfo);
return route_name;
}
std::string logger_get_identifier(std::string key) {
// a log identifier is a 32 bit counter, plus a 10 character unique ID.
// e.g. 000001a3--c20ba54385
Params params;
uint32_t cnt;
try {
cnt = std::stol(params.get(key));
} catch (std::exception &e) {
cnt = 0;
}
params.put(key, std::to_string(cnt + 1));
std::stringstream ss;
std::random_device rd;
std::mt19937 mt(rd());
std::uniform_int_distribution<int> dist(0, 15);
for (int i = 0; i < 10; ++i) {
ss << std::hex << dist(mt);
}
return util::string_format("%08x--%s", cnt, ss.str().c_str());
}
static void log_sentinel(LoggerState *log, SentinelType type, int eixt_signal = 0) {
MessageBuilder msg;
auto sen = msg.initEvent().initSentinel();
sen.setType(type);
sen.setSignal(eixt_signal);
log->write(msg.toBytes(), true);
}
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();
}
LoggerState::~LoggerState() {
if (rlog) {
log_sentinel(this, SentinelType::END_OF_ROUTE, exit_signal);
std::remove(lock_file.c_str());
}
}
bool LoggerState::next() {
if (rlog) {
log_sentinel(this, SentinelType::END_OF_SEGMENT);
std::remove(lock_file.c_str());
}
segment_path = route_path + "--" + std::to_string(++part);
bool ret = util::create_directories(segment_path, 0775);
assert(ret == true);
const std::string rlog_path = segment_path + "/rlog";
lock_file = rlog_path + ".lock";
std::ofstream{lock_file};
rlog.reset(new RawFile(rlog_path));
qlog.reset(new RawFile(segment_path + "/qlog"));
// 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;
}
void LoggerState::write(uint8_t* data, size_t size, bool in_qlog) {
rlog->write(data, size);
if (in_qlog) qlog->write(data, size);
}

56
system/loggerd/logger.h
View File

@@ -1,56 +0,0 @@
#pragma once
#include <cassert>
#include <memory>
#include <string>
#include "cereal/messaging/messaging.h"
#include "common/util.h"
#include "system/hardware/hw.h"
class RawFile {
public:
RawFile(const std::string &path) {
file = util::safe_fopen(path.c_str(), "wb");
assert(file != nullptr);
}
~RawFile() {
util::safe_fflush(file);
int err = fclose(file);
assert(err == 0);
}
inline void write(void* data, size_t size) {
int written = util::safe_fwrite(data, 1, size, file);
assert(written == size);
}
inline void write(kj::ArrayPtr<capnp::byte> array) { write(array.begin(), array.size()); }
private:
FILE* file = nullptr;
};
typedef cereal::Sentinel::SentinelType SentinelType;
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::unique_ptr<RawFile> rlog, qlog;
};
kj::Array<capnp::word> logger_build_init_data();
std::string logger_get_route_name();
std::string logger_get_identifier(std::string key);

BIN
system/loggerd/loggerd Executable file
View File

Binary file not shown.

313
system/loggerd/loggerd.cc
View File

@@ -1,313 +0,0 @@
#include <sys/xattr.h>
#include <map>
#include <memory>
#include <string>
#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"
ExitHandler do_exit;
struct LoggerdState {
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;
double last_rotate_tms = 0.; // last rotate time in ms
};
void logger_rotate(LoggerdState *s) {
bool ret =s->logger.next();
assert(ret);
s->ready_to_rotate = 0;
s->last_rotate_tms = millis_since_boot();
LOGW((s->logger.segment() == 0) ? "logging to %s" : "rotated to %s", s->logger.segmentPath().c_str());
}
void rotate_if_needed(LoggerdState *s) {
// all encoders ready, trigger rotation
bool all_ready = s->ready_to_rotate == s->max_waiting;
// fallback logic to prevent extremely long segments in the case of camera, encoder, etc. malfunctions
bool timed_out = false;
double tms = millis_since_boot();
double seg_length_secs = (tms - s->last_rotate_tms) / 1000.;
if ((seg_length_secs > SEGMENT_LENGTH) && !LOGGERD_TEST) {
// TODO: might be nice to put these reasons in the sentinel
if ((tms - s->last_camera_seen_tms) > NO_CAMERA_PATIENCE) {
timed_out = true;
LOGE("no camera packets seen. auto rotating");
} else if (seg_length_secs > SEGMENT_LENGTH*1.2) {
timed_out = true;
LOGE("segment too long. auto rotating");
}
}
if (all_ready || timed_out) {
logger_rotate(s);
}
}
struct RemoteEncoder {
std::unique_ptr<VideoWriter> writer;
int encoderd_segment_offset;
int current_segment = -1;
std::vector<Message *> q;
int dropped_frames = 0;
bool recording = false;
bool marked_ready_to_rotate = false;
bool seen_first_packet = false;
};
int handle_encoder_msg(LoggerdState *s, Message *msg, std::string &name, struct RemoteEncoder &re, const EncoderInfo &encoder_info) {
int bytes_count = 0;
// extract the message
capnp::FlatArrayMessageReader cmsg(kj::ArrayPtr<capnp::word>((capnp::word *)msg->getData(), msg->getSize() / sizeof(capnp::word)));
auto event = cmsg.getRoot<cereal::Event>();
auto edata = (event.*(encoder_info.get_encode_data_func))();
auto idx = edata.getIdx();
auto flags = idx.getFlags();
// encoderd can have started long before loggerd
if (!re.seen_first_packet) {
re.seen_first_packet = true;
re.encoderd_segment_offset = idx.getSegmentNum();
LOGD("%s: has encoderd offset %d", name.c_str(), re.encoderd_segment_offset);
}
int offset_segment_num = idx.getSegmentNum() - re.encoderd_segment_offset;
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->logger.segment()) {
if (re.recording) {
re.writer.reset();
re.recording = false;
}
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()) {
for (auto &qmsg : re.q) {
bytes_count += handle_encoder_msg(s, qmsg, name, re, encoder_info);
}
re.q.clear();
}
}
// if we aren't recording yet, try to start, since we are in the correct segment
if (!re.recording) {
if (flags & V4L2_BUF_FLAG_KEYFRAME) {
// only create on iframe
if (re.dropped_frames) {
// this should only happen for the first segment, maybe
LOGW("%s: dropped %d non iframe packets before init", name.c_str(), re.dropped_frames);
re.dropped_frames = 0;
}
// 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->logger.segmentPath().c_str(),
encoder_info.filename, idx.getType() != cereal::EncodeIndex::Type::FULL_H_E_V_C,
edata.getWidth(), edata.getHeight(), encoder_info.fps, idx.getType()));
// write the header
auto header = edata.getHeader();
re.writer->write((uint8_t *)header.begin(), header.size(), idx.getTimestampEof()/1000, true, false);
}
re.recording = true;
} else {
// this is a sad case when we aren't recording, but don't have an iframe
// nothing we can do but drop the frame
delete msg;
++re.dropped_frames;
return bytes_count;
}
}
// we have to be recording if we are here
assert(re.recording);
// if we are actually writing the video file, do so
if (re.writer) {
auto data = edata.getData();
re.writer->write((uint8_t *)data.begin(), data.size(), idx.getTimestampEof()/1000, false, flags & V4L2_BUF_FLAG_KEYFRAME);
}
// put it in log stream as the idx packet
MessageBuilder bmsg;
auto evt = bmsg.initEvent(event.getValid());
evt.setLogMonoTime(event.getLogMonoTime());
(evt.*(encoder_info.set_encode_idx_func))(idx);
auto new_msg = bmsg.toBytes();
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->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->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->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->logger.segment();
delete msg;
}
return bytes_count;
}
void handle_user_flag(LoggerdState *s) {
static int prev_segment = -1;
if (s->logger.segment() == prev_segment) return;
LOGW("preserving %s", s->logger.segmentPath().c_str());
#ifdef __APPLE__
int ret = setxattr(s->logger.segmentPath().c_str(), PRESERVE_ATTR_NAME, &PRESERVE_ATTR_VALUE, 1, 0, 0);
#else
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->logger.segmentPath().c_str(), strerror(errno));
}
// 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() {
// setup messaging
typedef struct ServiceState {
std::string name;
int counter, freq;
bool encoder, user_flag;
} ServiceState;
std::unordered_map<SubSocket*, ServiceState> service_state;
std::unordered_map<SubSocket*, struct RemoteEncoder> remote_encoders;
std::unique_ptr<Context> ctx(Context::create());
std::unique_ptr<Poller> poller(Poller::create());
// subscribe to all socks
for (const auto& [_, it] : services) {
const bool encoder = util::ends_with(it.name, "EncodeData");
const bool livestream_encoder = util::starts_with(it.name, "livestream");
if (!it.should_log && (!encoder || livestream_encoder)) continue;
LOGD("logging %s (on port %d)", it.name.c_str(), it.port);
SubSocket * sock = SubSocket::create(ctx.get(), it.name);
assert(sock != NULL);
poller->registerSocket(sock);
service_state[sock] = {
.name = it.name,
.counter = 0,
.freq = it.decimation,
.encoder = encoder,
.user_flag = it.name == "userFlag",
};
}
LoggerdState s;
// init logger
logger_rotate(&s);
Params().put("CurrentRoute", s.logger.routeName());
std::map<std::string, EncoderInfo> encoder_infos_dict;
for (const auto &cam : cameras_logged) {
for (const auto &encoder_info : cam.encoder_infos) {
encoder_infos_dict[encoder_info.publish_name] = encoder_info;
s.max_waiting++;
}
}
uint64_t msg_count = 0, bytes_count = 0;
double start_ts = millis_since_boot();
while (!do_exit) {
// poll for new messages on all sockets
for (auto sock : poller->poll(1000)) {
if (do_exit) break;
ServiceState &service = service_state[sock];
if (service.user_flag) {
handle_user_flag(&s);
}
// drain socket
int count = 0;
Message *msg = nullptr;
while (!do_exit && (msg = sock->receive(true))) {
const bool in_qlog = service.freq != -1 && (service.counter++ % service.freq == 0);
if (service.encoder) {
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 {
s.logger.write((uint8_t *)msg->getData(), msg->getSize(), in_qlog);
bytes_count += msg->getSize();
delete msg;
}
rotate_if_needed(&s);
if ((++msg_count % 1000) == 0) {
double seconds = (millis_since_boot() - start_ts) / 1000.0;
LOGD("%" PRIu64 " messages, %.2f msg/sec, %.2f KB/sec", msg_count, msg_count / seconds, bytes_count * 0.001 / seconds);
}
count++;
if (count >= 200) {
LOGD("large volume of '%s' messages", service.name.c_str());
break;
}
}
}
}
LOGW("closing logger");
s.logger.setExitSignal(do_exit.signal);
if (do_exit.power_failure) {
LOGE("power failure");
sync();
LOGE("sync done");
}
// messaging cleanup
for (auto &[sock, service] : service_state) delete sock;
}
int main(int argc, char** argv) {
if (!Hardware::PC()) {
int ret;
ret = util::set_core_affinity({0, 1, 2, 3});
assert(ret == 0);
// TODO: why does this impact camerad timings?
//ret = util::set_realtime_priority(1);
//assert(ret == 0);
}
loggerd_thread();
return 0;
}

154
system/loggerd/loggerd.h
View File

@@ -1,154 +0,0 @@
#pragma once
#include <vector>
#include "cereal/messaging/messaging.h"
#include "cereal/services.h"
#include "cereal/visionipc/visionipc_client.h"
#include "system/camerad/cameras/camera_common.h"
#include "system/hardware/hw.h"
#include "common/params.h"
#include "common/swaglog.h"
#include "common/util.h"
#include "system/loggerd/logger.h"
constexpr int MAIN_FPS = 20;
const int MAIN_BITRATE = Params().getBool("HigherBitrate") ? 20000000 : 1e7;
const int LIVESTREAM_BITRATE = 1e6;
const int QCAM_BITRATE = 256000;
#define NO_CAMERA_PATIENCE 500 // fall back to time-based rotation if all cameras are dead
#define INIT_ENCODE_FUNCTIONS(encode_type) \
.get_encode_data_func = &cereal::Event::Reader::get##encode_type##Data, \
.set_encode_idx_func = &cereal::Event::Builder::set##encode_type##Idx, \
.init_encode_data_func = &cereal::Event::Builder::init##encode_type##Data
const bool LOGGERD_TEST = getenv("LOGGERD_TEST");
const int SEGMENT_LENGTH = LOGGERD_TEST ? atoi(getenv("LOGGERD_SEGMENT_LENGTH")) : 60;
constexpr char PRESERVE_ATTR_NAME[] = "user.preserve";
constexpr char PRESERVE_ATTR_VALUE = '1';
class EncoderInfo {
public:
const char *publish_name;
const char *filename = NULL;
bool record = true;
int frame_width = -1;
int frame_height = -1;
int fps = MAIN_FPS;
int bitrate = MAIN_BITRATE;
cereal::EncodeIndex::Type encode_type = Hardware::PC() ? cereal::EncodeIndex::Type::BIG_BOX_LOSSLESS
: cereal::EncodeIndex::Type::FULL_H_E_V_C;
::cereal::EncodeData::Reader (cereal::Event::Reader::*get_encode_data_func)() const;
void (cereal::Event::Builder::*set_encode_idx_func)(::cereal::EncodeIndex::Reader);
cereal::EncodeData::Builder (cereal::Event::Builder::*init_encode_data_func)();
};
class LogCameraInfo {
public:
const char *thread_name;
int fps = MAIN_FPS;
CameraType type;
VisionStreamType stream_type;
std::vector<EncoderInfo> encoder_infos;
};
const EncoderInfo main_road_encoder_info = {
.publish_name = "roadEncodeData",
.filename = "fcamera.hevc",
INIT_ENCODE_FUNCTIONS(RoadEncode),
};
const EncoderInfo main_wide_road_encoder_info = {
.publish_name = "wideRoadEncodeData",
.filename = "ecamera.hevc",
INIT_ENCODE_FUNCTIONS(WideRoadEncode),
};
const EncoderInfo main_driver_encoder_info = {
.publish_name = "driverEncodeData",
.filename = "dcamera.hevc",
.record = Params().getBool("RecordFront"),
INIT_ENCODE_FUNCTIONS(DriverEncode),
};
const EncoderInfo stream_road_encoder_info = {
.publish_name = "livestreamRoadEncodeData",
.encode_type = cereal::EncodeIndex::Type::QCAMERA_H264,
.record = false,
.bitrate = LIVESTREAM_BITRATE,
INIT_ENCODE_FUNCTIONS(LivestreamRoadEncode),
};
const EncoderInfo stream_wide_road_encoder_info = {
.publish_name = "livestreamWideRoadEncodeData",
.encode_type = cereal::EncodeIndex::Type::QCAMERA_H264,
.record = false,
.bitrate = LIVESTREAM_BITRATE,
INIT_ENCODE_FUNCTIONS(LivestreamWideRoadEncode),
};
const EncoderInfo stream_driver_encoder_info = {
.publish_name = "livestreamDriverEncodeData",
.encode_type = cereal::EncodeIndex::Type::QCAMERA_H264,
.record = false,
.bitrate = LIVESTREAM_BITRATE,
INIT_ENCODE_FUNCTIONS(LivestreamDriverEncode),
};
const EncoderInfo qcam_encoder_info = {
.publish_name = "qRoadEncodeData",
.filename = "qcamera.ts",
.bitrate = QCAM_BITRATE,
.encode_type = cereal::EncodeIndex::Type::QCAMERA_H264,
.frame_width = 526,
.frame_height = 330,
INIT_ENCODE_FUNCTIONS(QRoadEncode),
};
const LogCameraInfo road_camera_info{
.thread_name = "road_cam_encoder",
.type = RoadCam,
.stream_type = VISION_STREAM_ROAD,
.encoder_infos = {main_road_encoder_info, qcam_encoder_info}
};
const LogCameraInfo wide_road_camera_info{
.thread_name = "wide_road_cam_encoder",
.type = WideRoadCam,
.stream_type = VISION_STREAM_WIDE_ROAD,
.encoder_infos = {main_wide_road_encoder_info}
};
const LogCameraInfo driver_camera_info{
.thread_name = "driver_cam_encoder",
.type = DriverCam,
.stream_type = VISION_STREAM_DRIVER,
.encoder_infos = {main_driver_encoder_info}
};
const LogCameraInfo stream_road_camera_info{
.thread_name = "road_cam_encoder",
.type = RoadCam,
.stream_type = VISION_STREAM_ROAD,
.encoder_infos = {stream_road_encoder_info}
};
const LogCameraInfo stream_wide_road_camera_info{
.thread_name = "wide_road_cam_encoder",
.type = WideRoadCam,
.stream_type = VISION_STREAM_WIDE_ROAD,
.encoder_infos = {stream_wide_road_encoder_info}
};
const LogCameraInfo stream_driver_camera_info{
.thread_name = "driver_cam_encoder",
.type = DriverCam,
.stream_type = VISION_STREAM_DRIVER,
.encoder_infos = {stream_driver_encoder_info}
};
const LogCameraInfo cameras_logged[] = {road_camera_info, wide_road_camera_info, driver_camera_info};
const LogCameraInfo stream_cameras_logged[] = {stream_road_camera_info, stream_wide_road_camera_info, stream_driver_camera_info};

112
system/loggerd/video_writer.cc
View File

@@ -1,112 +0,0 @@
#pragma clang diagnostic ignored "-Wdeprecated-declarations"
#include <cassert>
#include "system/loggerd/video_writer.h"
#include "common/swaglog.h"
#include "common/util.h"
VideoWriter::VideoWriter(const char *path, const char *filename, bool remuxing, int width, int height, int fps, cereal::EncodeIndex::Type codec)
: remuxing(remuxing) {
vid_path = util::string_format("%s/%s", path, filename);
lock_path = util::string_format("%s/%s.lock", path, filename);
int lock_fd = HANDLE_EINTR(open(lock_path.c_str(), O_RDWR | O_CREAT, 0664));
assert(lock_fd >= 0);
close(lock_fd);
LOGD("encoder_open %s remuxing:%d", this->vid_path.c_str(), this->remuxing);
if (this->remuxing) {
bool raw = (codec == cereal::EncodeIndex::Type::BIG_BOX_LOSSLESS);
avformat_alloc_output_context2(&this->ofmt_ctx, NULL, raw ? "matroska" : NULL, this->vid_path.c_str());
assert(this->ofmt_ctx);
// set codec correctly. needed?
assert(codec != cereal::EncodeIndex::Type::FULL_H_E_V_C);
const AVCodec *avcodec = avcodec_find_encoder(raw ? AV_CODEC_ID_FFVHUFF : AV_CODEC_ID_H264);
assert(avcodec);
this->codec_ctx = avcodec_alloc_context3(avcodec);
assert(this->codec_ctx);
this->codec_ctx->width = width;
this->codec_ctx->height = height;
this->codec_ctx->pix_fmt = AV_PIX_FMT_YUV420P;
this->codec_ctx->time_base = (AVRational){ 1, fps };
if (codec == cereal::EncodeIndex::Type::BIG_BOX_LOSSLESS) {
// without this, there's just noise
int err = avcodec_open2(this->codec_ctx, avcodec, NULL);
assert(err >= 0);
}
this->out_stream = avformat_new_stream(this->ofmt_ctx, raw ? avcodec : NULL);
assert(this->out_stream);
int err = avio_open(&this->ofmt_ctx->pb, this->vid_path.c_str(), AVIO_FLAG_WRITE);
assert(err >= 0);
} else {
this->of = util::safe_fopen(this->vid_path.c_str(), "wb");
assert(this->of);
}
}
void VideoWriter::write(uint8_t *data, int len, long long timestamp, bool codecconfig, bool keyframe) {
if (of && data) {
size_t written = util::safe_fwrite(data, 1, len, of);
if (written != len) {
LOGE("failed to write file.errno=%d", errno);
}
}
if (remuxing) {
if (codecconfig) {
if (len > 0) {
codec_ctx->extradata = (uint8_t*)av_mallocz(len + AV_INPUT_BUFFER_PADDING_SIZE);
codec_ctx->extradata_size = len;
memcpy(codec_ctx->extradata, data, len);
}
int err = avcodec_parameters_from_context(out_stream->codecpar, codec_ctx);
assert(err >= 0);
err = avformat_write_header(ofmt_ctx, NULL);
assert(err >= 0);
} else {
// input timestamps are in microseconds
AVRational in_timebase = {1, 1000000};
AVPacket pkt;
av_init_packet(&pkt);
pkt.data = data;
pkt.size = len;
enum AVRounding rnd = static_cast<enum AVRounding>(AV_ROUND_NEAR_INF|AV_ROUND_PASS_MINMAX);
pkt.pts = pkt.dts = av_rescale_q_rnd(timestamp, in_timebase, ofmt_ctx->streams[0]->time_base, rnd);
pkt.duration = av_rescale_q(50*1000, in_timebase, ofmt_ctx->streams[0]->time_base);
if (keyframe) {
pkt.flags |= AV_PKT_FLAG_KEY;
}
// TODO: can use av_write_frame for non raw?
int err = av_interleaved_write_frame(ofmt_ctx, &pkt);
if (err < 0) { LOGW("ts encoder write issue len: %d ts: %lld", len, timestamp); }
av_packet_unref(&pkt);
}
}
}
VideoWriter::~VideoWriter() {
if (this->remuxing) {
int err = av_write_trailer(this->ofmt_ctx);
if (err != 0) LOGE("av_write_trailer failed %d", err);
avcodec_free_context(&this->codec_ctx);
err = avio_closep(&this->ofmt_ctx->pb);
if (err != 0) LOGE("avio_closep failed %d", err);
avformat_free_context(this->ofmt_ctx);
} else {
util::safe_fflush(this->of);
fclose(this->of);
this->of = nullptr;
}
unlink(this->lock_path.c_str());
}

25
system/loggerd/video_writer.h
View File

@@ -1,25 +0,0 @@
#pragma once
#include <string>
extern "C" {
#include <libavformat/avformat.h>
#include <libavcodec/avcodec.h>
}
#include "cereal/messaging/messaging.h"
class VideoWriter {
public:
VideoWriter(const char *path, const char *filename, bool remuxing, int width, int height, int fps, cereal::EncodeIndex::Type codec);
void write(uint8_t *data, int len, long long timestamp, bool codecconfig, bool keyframe);
~VideoWriter();
private:
std::string vid_path, lock_path;
FILE *of = nullptr;
AVCodecContext *codec_ctx;
AVFormatContext *ofmt_ctx;
AVStream *out_stream;
bool remuxing;
};

6
system/proclogd/SConscript
View File

@@ -1,6 +0,0 @@
Import('env', 'cereal', 'messaging', 'common')
libs = [cereal, messaging, 'pthread', 'zmq', 'capnp', 'kj', 'common', 'zmq', 'json11']
env.Program('proclogd', ['main.cc', 'proclog.cc'], LIBS=libs)
if GetOption('extras'):
env.Program('tests/test_proclog', ['tests/test_proclog.cc', 'proclog.cc'], LIBS=libs)

25
system/proclogd/main.cc
View File

@@ -1,25 +0,0 @@
#include <sys/resource.h>
#include "common/ratekeeper.h"
#include "common/util.h"
#include "system/proclogd/proclog.h"
ExitHandler do_exit;
int main(int argc, char **argv) {
setpriority(PRIO_PROCESS, 0, -15);
RateKeeper rk("proclogd", 0.5);
PubMaster publisher({"procLog"});
while (!do_exit) {
MessageBuilder msg;
buildProcLogMessage(msg);
publisher.send("procLog", msg);
rk.keepTime();
}
return 0;
}

239
system/proclogd/proclog.cc
View File

@@ -1,239 +0,0 @@
#include "system/proclogd/proclog.h"
#include <dirent.h>
#include <cassert>
#include <fstream>
#include <iterator>
#include <sstream>
#include "common/swaglog.h"
#include "common/util.h"
namespace Parser {
// parse /proc/stat
std::vector<CPUTime> cpuTimes(std::istream &stream) {
std::vector<CPUTime> cpu_times;
std::string line;
// skip the first line for cpu total
std::getline(stream, line);
while (std::getline(stream, line)) {
if (line.compare(0, 3, "cpu") != 0) break;
CPUTime t = {};
std::istringstream iss(line);
if (iss.ignore(3) >> t.id >> t.utime >> t.ntime >> t.stime >> t.itime >> t.iowtime >> t.irqtime >> t.sirqtime)
cpu_times.push_back(t);
}
return cpu_times;
}
// parse /proc/meminfo
std::unordered_map<std::string, uint64_t> memInfo(std::istream &stream) {
std::unordered_map<std::string, uint64_t> mem_info;
std::string line, key;
while (std::getline(stream, line)) {
uint64_t val = 0;
std::istringstream iss(line);
if (iss >> key >> val) {
mem_info[key] = val * 1024;
}
}
return mem_info;
}
// field position (https://man7.org/linux/man-pages/man5/proc.5.html)
enum StatPos {
pid = 1,
state = 3,
ppid = 4,
utime = 14,
stime = 15,
cutime = 16,
cstime = 17,
priority = 18,
nice = 19,
num_threads = 20,
starttime = 22,
vsize = 23,
rss = 24,
processor = 39,
MAX_FIELD = 52,
};
// parse /proc/pid/stat
std::optional<ProcStat> procStat(std::string stat) {
// To avoid being fooled by names containing a closing paren, scan backwards.
auto open_paren = stat.find('(');
auto close_paren = stat.rfind(')');
if (open_paren == std::string::npos || close_paren == std::string::npos || open_paren > close_paren) {
return std::nullopt;
}
std::string name = stat.substr(open_paren + 1, close_paren - open_paren - 1);
// replace space in name with _
std::replace(&stat[open_paren], &stat[close_paren], ' ', '_');
std::istringstream iss(stat);
std::vector<std::string> v{std::istream_iterator<std::string>(iss),
std::istream_iterator<std::string>()};
try {
if (v.size() != StatPos::MAX_FIELD) {
throw std::invalid_argument("stat");
}
ProcStat p = {
.name = name,
.pid = stoi(v[StatPos::pid - 1]),
.state = v[StatPos::state - 1][0],
.ppid = stoi(v[StatPos::ppid - 1]),
.utime = stoul(v[StatPos::utime - 1]),
.stime = stoul(v[StatPos::stime - 1]),
.cutime = stol(v[StatPos::cutime - 1]),
.cstime = stol(v[StatPos::cstime - 1]),
.priority = stol(v[StatPos::priority - 1]),
.nice = stol(v[StatPos::nice - 1]),
.num_threads = stol(v[StatPos::num_threads - 1]),
.starttime = stoull(v[StatPos::starttime - 1]),
.vms = stoul(v[StatPos::vsize - 1]),
.rss = stol(v[StatPos::rss - 1]),
.processor = stoi(v[StatPos::processor - 1]),
};
return p;
} catch (const std::invalid_argument &e) {
LOGE("failed to parse procStat (%s) :%s", e.what(), stat.c_str());
} catch (const std::out_of_range &e) {
LOGE("failed to parse procStat (%s) :%s", e.what(), stat.c_str());
}
return std::nullopt;
}
// return list of PIDs from /proc
std::vector<int> pids() {
std::vector<int> ids;
DIR *d = opendir("/proc");
assert(d);
char *p_end;
struct dirent *de = NULL;
while ((de = readdir(d))) {
if (de->d_type == DT_DIR) {
int pid = strtol(de->d_name, &p_end, 10);
if (p_end == (de->d_name + strlen(de->d_name))) {
ids.push_back(pid);
}
}
}
closedir(d);
return ids;
}
// null-delimited cmdline arguments to vector
std::vector<std::string> cmdline(std::istream &stream) {
std::vector<std::string> ret;
std::string line;
while (std::getline(stream, line, '\0')) {
if (!line.empty()) {
ret.push_back(line);
}
}
return ret;
}
const ProcCache &getProcExtraInfo(int pid, const std::string &name) {
static std::unordered_map<pid_t, ProcCache> proc_cache;
ProcCache &cache = proc_cache[pid];
if (cache.pid != pid || cache.name != name) {
cache.pid = pid;
cache.name = name;
std::string proc_path = "/proc/" + std::to_string(pid);
cache.exe = util::readlink(proc_path + "/exe");
std::ifstream stream(proc_path + "/cmdline");
cache.cmdline = cmdline(stream);
}
return cache;
}
} // namespace Parser
const double jiffy = sysconf(_SC_CLK_TCK);
const size_t page_size = sysconf(_SC_PAGE_SIZE);
void buildCPUTimes(cereal::ProcLog::Builder &builder) {
std::ifstream stream("/proc/stat");
std::vector<CPUTime> stats = Parser::cpuTimes(stream);
auto log_cpu_times = builder.initCpuTimes(stats.size());
for (int i = 0; i < stats.size(); ++i) {
auto l = log_cpu_times[i];
const CPUTime &r = stats[i];
l.setCpuNum(r.id);
l.setUser(r.utime / jiffy);
l.setNice(r.ntime / jiffy);
l.setSystem(r.stime / jiffy);
l.setIdle(r.itime / jiffy);
l.setIowait(r.iowtime / jiffy);
l.setIrq(r.irqtime / jiffy);
l.setSoftirq(r.sirqtime / jiffy);
}
}
void buildMemInfo(cereal::ProcLog::Builder &builder) {
std::ifstream stream("/proc/meminfo");
auto mem_info = Parser::memInfo(stream);
auto mem = builder.initMem();
mem.setTotal(mem_info["MemTotal:"]);
mem.setFree(mem_info["MemFree:"]);
mem.setAvailable(mem_info["MemAvailable:"]);
mem.setBuffers(mem_info["Buffers:"]);
mem.setCached(mem_info["Cached:"]);
mem.setActive(mem_info["Active:"]);
mem.setInactive(mem_info["Inactive:"]);
mem.setShared(mem_info["Shmem:"]);
}
void buildProcs(cereal::ProcLog::Builder &builder) {
auto pids = Parser::pids();
std::vector<ProcStat> proc_stats;
proc_stats.reserve(pids.size());
for (int pid : pids) {
std::string path = "/proc/" + std::to_string(pid) + "/stat";
if (auto stat = Parser::procStat(util::read_file(path))) {
proc_stats.push_back(*stat);
}
}
auto procs = builder.initProcs(proc_stats.size());
for (size_t i = 0; i < proc_stats.size(); i++) {
auto l = procs[i];
const ProcStat &r = proc_stats[i];
l.setPid(r.pid);
l.setState(r.state);
l.setPpid(r.ppid);
l.setCpuUser(r.utime / jiffy);
l.setCpuSystem(r.stime / jiffy);
l.setCpuChildrenUser(r.cutime / jiffy);
l.setCpuChildrenSystem(r.cstime / jiffy);
l.setPriority(r.priority);
l.setNice(r.nice);
l.setNumThreads(r.num_threads);
l.setStartTime(r.starttime / jiffy);
l.setMemVms(r.vms);
l.setMemRss((uint64_t)r.rss * page_size);
l.setProcessor(r.processor);
l.setName(r.name);
const ProcCache &extra_info = Parser::getProcExtraInfo(r.pid, r.name);
l.setExe(extra_info.exe);
auto lcmdline = l.initCmdline(extra_info.cmdline.size());
for (size_t j = 0; j < lcmdline.size(); j++) {
lcmdline.set(j, extra_info.cmdline[j]);
}
}
}
void buildProcLogMessage(MessageBuilder &msg) {
auto procLog = msg.initEvent().initProcLog();
buildProcs(procLog);
buildCPUTimes(procLog);
buildMemInfo(procLog);
}

40
system/proclogd/proclog.h
View File

@@ -1,40 +0,0 @@
#include <optional>
#include <string>
#include <unordered_map>
#include <vector>
#include "cereal/messaging/messaging.h"
struct CPUTime {
int id;
unsigned long utime, ntime, stime, itime;
unsigned long iowtime, irqtime, sirqtime;
};
struct ProcCache {
int pid;
std::string name, exe;
std::vector<std::string> cmdline;
};
struct ProcStat {
int pid, ppid, processor;
char state;
long cutime, cstime, priority, nice, num_threads, rss;
unsigned long utime, stime, vms;
unsigned long long starttime;
std::string name;
};
namespace Parser {
std::vector<int> pids();
std::optional<ProcStat> procStat(std::string stat);
std::vector<std::string> cmdline(std::istream &stream);
std::vector<CPUTime> cpuTimes(std::istream &stream);
std::unordered_map<std::string, uint64_t> memInfo(std::istream &stream);
const ProcCache &getProcExtraInfo(int pid, const std::string &name);
}; // namespace Parser
void buildProcLogMessage(MessageBuilder &msg);

BIN
system/proclogd/proclogd Executable file
View File

Binary file not shown.

17
system/sensord/SConscript
View File

@@ -1,17 +0,0 @@
Import('env', 'arch', 'common', 'cereal', 'messaging')
sensors = [
'sensors/i2c_sensor.cc',
'sensors/bmx055_accel.cc',
'sensors/bmx055_gyro.cc',
'sensors/bmx055_magn.cc',
'sensors/bmx055_temp.cc',
'sensors/lsm6ds3_accel.cc',
'sensors/lsm6ds3_gyro.cc',
'sensors/lsm6ds3_temp.cc',
'sensors/mmc5603nj_magn.cc',
]
libs = [common, cereal, messaging, 'capnp', 'zmq', 'kj', 'pthread']
if arch == "larch64":
libs.append('i2c')
env.Program('sensord', ['sensors_qcom2.cc'] + sensors, LIBS=libs)

BIN
system/sensord/sensord Executable file
View File

Binary file not shown.

78
system/sensord/sensors/bmx055_accel.cc
View File

@@ -1,78 +0,0 @@
#include "system/sensord/sensors/bmx055_accel.h"
#include <cassert>
#include "common/swaglog.h"
#include "common/timing.h"
#include "common/util.h"
BMX055_Accel::BMX055_Accel(I2CBus *bus) : I2CSensor(bus) {}
int BMX055_Accel::init() {
int ret = verify_chip_id(BMX055_ACCEL_I2C_REG_ID, {BMX055_ACCEL_CHIP_ID});
if (ret == -1) return -1;
ret = set_register(BMX055_ACCEL_I2C_REG_PMU, BMX055_ACCEL_NORMAL_MODE);
if (ret < 0) {
goto fail;
}
// bmx055 accel has a 1.3ms wakeup time from deep suspend mode
util::sleep_for(10);
// High bandwidth
// ret = set_register(BMX055_ACCEL_I2C_REG_HBW, BMX055_ACCEL_HBW_ENABLE);
// if (ret < 0) {
// goto fail;
// }
// Low bandwidth
ret = set_register(BMX055_ACCEL_I2C_REG_HBW, BMX055_ACCEL_HBW_DISABLE);
if (ret < 0) {
goto fail;
}
ret = set_register(BMX055_ACCEL_I2C_REG_BW, BMX055_ACCEL_BW_125HZ);
if (ret < 0) {
goto fail;
}
fail:
return ret;
}
int BMX055_Accel::shutdown() {
// enter deep suspend mode (lowest power mode)
int ret = set_register(BMX055_ACCEL_I2C_REG_PMU, BMX055_ACCEL_DEEP_SUSPEND);
if (ret < 0) {
LOGE("Could not move BMX055 ACCEL in deep suspend mode!");
}
return ret;
}
bool BMX055_Accel::get_event(MessageBuilder &msg, uint64_t ts) {
uint64_t start_time = nanos_since_boot();
uint8_t buffer[6];
int len = read_register(BMX055_ACCEL_I2C_REG_X_LSB, buffer, sizeof(buffer));
assert(len == 6);
// 12 bit = +-2g
float scale = 9.81 * 2.0f / (1 << 11);
float x = -read_12_bit(buffer[0], buffer[1]) * scale;
float y = -read_12_bit(buffer[2], buffer[3]) * scale;
float z = read_12_bit(buffer[4], buffer[5]) * scale;
auto event = msg.initEvent().initAccelerometer2();
event.setSource(cereal::SensorEventData::SensorSource::BMX055);
event.setVersion(1);
event.setSensor(SENSOR_ACCELEROMETER);
event.setType(SENSOR_TYPE_ACCELEROMETER);
event.setTimestamp(start_time);
float xyz[] = {x, y, z};
auto svec = event.initAcceleration();
svec.setV(xyz);
svec.setStatus(true);
return true;
}

41
system/sensord/sensors/bmx055_accel.h
View File

@@ -1,41 +0,0 @@
#pragma once
#include "system/sensord/sensors/i2c_sensor.h"
// Address of the chip on the bus
#define BMX055_ACCEL_I2C_ADDR 0x18
// Registers of the chip
#define BMX055_ACCEL_I2C_REG_ID 0x00
#define BMX055_ACCEL_I2C_REG_X_LSB 0x02
#define BMX055_ACCEL_I2C_REG_TEMP 0x08
#define BMX055_ACCEL_I2C_REG_BW 0x10
#define BMX055_ACCEL_I2C_REG_PMU 0x11
#define BMX055_ACCEL_I2C_REG_HBW 0x13
#define BMX055_ACCEL_I2C_REG_FIFO 0x3F
// Constants
#define BMX055_ACCEL_CHIP_ID 0xFA
#define BMX055_ACCEL_HBW_ENABLE 0b10000000
#define BMX055_ACCEL_HBW_DISABLE 0b00000000
#define BMX055_ACCEL_DEEP_SUSPEND 0b00100000
#define BMX055_ACCEL_NORMAL_MODE 0b00000000
#define BMX055_ACCEL_BW_7_81HZ 0b01000
#define BMX055_ACCEL_BW_15_63HZ 0b01001
#define BMX055_ACCEL_BW_31_25HZ 0b01010
#define BMX055_ACCEL_BW_62_5HZ 0b01011
#define BMX055_ACCEL_BW_125HZ 0b01100
#define BMX055_ACCEL_BW_250HZ 0b01101
#define BMX055_ACCEL_BW_500HZ 0b01110
#define BMX055_ACCEL_BW_1000HZ 0b01111
class BMX055_Accel : public I2CSensor {
uint8_t get_device_address() {return BMX055_ACCEL_I2C_ADDR;}
public:
BMX055_Accel(I2CBus *bus);
int init();
bool get_event(MessageBuilder &msg, uint64_t ts = 0);
int shutdown();
};

88
system/sensord/sensors/bmx055_gyro.cc
View File

@@ -1,88 +0,0 @@
#include "system/sensord/sensors/bmx055_gyro.h"
#include <cassert>
#include <cmath>
#include "common/swaglog.h"
#include "common/util.h"
#define DEG2RAD(x) ((x) * M_PI / 180.0)
BMX055_Gyro::BMX055_Gyro(I2CBus *bus) : I2CSensor(bus) {}
int BMX055_Gyro::init() {
int ret = verify_chip_id(BMX055_GYRO_I2C_REG_ID, {BMX055_GYRO_CHIP_ID});
if (ret == -1) return -1;
ret = set_register(BMX055_GYRO_I2C_REG_LPM1, BMX055_GYRO_NORMAL_MODE);
if (ret < 0) {
goto fail;
}
// bmx055 gyro has a 30ms wakeup time from deep suspend mode
util::sleep_for(50);
// High bandwidth
// ret = set_register(BMX055_GYRO_I2C_REG_HBW, BMX055_GYRO_HBW_ENABLE);
// if (ret < 0) {
// goto fail;
// }
// Low bandwidth
ret = set_register(BMX055_GYRO_I2C_REG_HBW, BMX055_GYRO_HBW_DISABLE);
if (ret < 0) {
goto fail;
}
// 116 Hz filter
ret = set_register(BMX055_GYRO_I2C_REG_BW, BMX055_GYRO_BW_116HZ);
if (ret < 0) {
goto fail;
}
// +- 125 deg/s range
ret = set_register(BMX055_GYRO_I2C_REG_RANGE, BMX055_GYRO_RANGE_125);
if (ret < 0) {
goto fail;
}
fail:
return ret;
}
int BMX055_Gyro::shutdown() {
// enter deep suspend mode (lowest power mode)
int ret = set_register(BMX055_GYRO_I2C_REG_LPM1, BMX055_GYRO_DEEP_SUSPEND);
if (ret < 0) {
LOGE("Could not move BMX055 GYRO in deep suspend mode!");
}
return ret;
}
bool BMX055_Gyro::get_event(MessageBuilder &msg, uint64_t ts) {
uint64_t start_time = nanos_since_boot();
uint8_t buffer[6];
int len = read_register(BMX055_GYRO_I2C_REG_RATE_X_LSB, buffer, sizeof(buffer));
assert(len == 6);
// 16 bit = +- 125 deg/s
float scale = 125.0f / (1 << 15);
float x = -DEG2RAD(read_16_bit(buffer[0], buffer[1]) * scale);
float y = -DEG2RAD(read_16_bit(buffer[2], buffer[3]) * scale);
float z = DEG2RAD(read_16_bit(buffer[4], buffer[5]) * scale);
auto event = msg.initEvent().initGyroscope2();
event.setSource(cereal::SensorEventData::SensorSource::BMX055);
event.setVersion(1);
event.setSensor(SENSOR_GYRO_UNCALIBRATED);
event.setType(SENSOR_TYPE_GYROSCOPE_UNCALIBRATED);
event.setTimestamp(start_time);
float xyz[] = {x, y, z};
auto svec = event.initGyroUncalibrated();
svec.setV(xyz);
svec.setStatus(true);
return true;
}

41
system/sensord/sensors/bmx055_gyro.h
View File

@@ -1,41 +0,0 @@
#pragma once
#include "system/sensord/sensors/i2c_sensor.h"
// Address of the chip on the bus
#define BMX055_GYRO_I2C_ADDR 0x68
// Registers of the chip
#define BMX055_GYRO_I2C_REG_ID 0x00
#define BMX055_GYRO_I2C_REG_RATE_X_LSB 0x02
#define BMX055_GYRO_I2C_REG_RANGE 0x0F
#define BMX055_GYRO_I2C_REG_BW 0x10
#define BMX055_GYRO_I2C_REG_LPM1 0x11
#define BMX055_GYRO_I2C_REG_HBW 0x13
#define BMX055_GYRO_I2C_REG_FIFO 0x3F
// Constants
#define BMX055_GYRO_CHIP_ID 0x0F
#define BMX055_GYRO_HBW_ENABLE 0b10000000
#define BMX055_GYRO_HBW_DISABLE 0b00000000
#define BMX055_GYRO_DEEP_SUSPEND 0b00100000
#define BMX055_GYRO_NORMAL_MODE 0b00000000
#define BMX055_GYRO_RANGE_2000 0b000
#define BMX055_GYRO_RANGE_1000 0b001
#define BMX055_GYRO_RANGE_500 0b010
#define BMX055_GYRO_RANGE_250 0b011
#define BMX055_GYRO_RANGE_125 0b100
#define BMX055_GYRO_BW_116HZ 0b0010
class BMX055_Gyro : public I2CSensor {
uint8_t get_device_address() {return BMX055_GYRO_I2C_ADDR;}
public:
BMX055_Gyro(I2CBus *bus);
int init();
bool get_event(MessageBuilder &msg, uint64_t ts = 0);
int shutdown();
};

256
system/sensord/sensors/bmx055_magn.cc
View File

@@ -1,256 +0,0 @@
#include "system/sensord/sensors/bmx055_magn.h"
#include <unistd.h>
#include <algorithm>
#include <cassert>
#include <cstdio>
#include "common/swaglog.h"
#include "common/util.h"
static int16_t compensate_x(trim_data_t trim_data, int16_t mag_data_x, uint16_t data_rhall) {
uint16_t process_comp_x0 = data_rhall;
int32_t process_comp_x1 = ((int32_t)trim_data.dig_xyz1) * 16384;
uint16_t process_comp_x2 = ((uint16_t)(process_comp_x1 / process_comp_x0)) - ((uint16_t)0x4000);
int16_t retval = ((int16_t)process_comp_x2);
int32_t process_comp_x3 = (((int32_t)retval) * ((int32_t)retval));
int32_t process_comp_x4 = (((int32_t)trim_data.dig_xy2) * (process_comp_x3 / 128));
int32_t process_comp_x5 = (int32_t)(((int16_t)trim_data.dig_xy1) * 128);
int32_t process_comp_x6 = ((int32_t)retval) * process_comp_x5;
int32_t process_comp_x7 = (((process_comp_x4 + process_comp_x6) / 512) + ((int32_t)0x100000));
int32_t process_comp_x8 = ((int32_t)(((int16_t)trim_data.dig_x2) + ((int16_t)0xA0)));
int32_t process_comp_x9 = ((process_comp_x7 * process_comp_x8) / 4096);
int32_t process_comp_x10 = ((int32_t)mag_data_x) * process_comp_x9;
retval = ((int16_t)(process_comp_x10 / 8192));
retval = (retval + (((int16_t)trim_data.dig_x1) * 8)) / 16;
return retval;
}
static int16_t compensate_y(trim_data_t trim_data, int16_t mag_data_y, uint16_t data_rhall) {
uint16_t process_comp_y0 = trim_data.dig_xyz1;
int32_t process_comp_y1 = (((int32_t)trim_data.dig_xyz1) * 16384) / process_comp_y0;
uint16_t process_comp_y2 = ((uint16_t)process_comp_y1) - ((uint16_t)0x4000);
int16_t retval = ((int16_t)process_comp_y2);
int32_t process_comp_y3 = ((int32_t) retval) * ((int32_t)retval);
int32_t process_comp_y4 = ((int32_t)trim_data.dig_xy2) * (process_comp_y3 / 128);
int32_t process_comp_y5 = ((int32_t)(((int16_t)trim_data.dig_xy1) * 128));
int32_t process_comp_y6 = ((process_comp_y4 + (((int32_t)retval) * process_comp_y5)) / 512);
int32_t process_comp_y7 = ((int32_t)(((int16_t)trim_data.dig_y2) + ((int16_t)0xA0)));
int32_t process_comp_y8 = (((process_comp_y6 + ((int32_t)0x100000)) * process_comp_y7) / 4096);
int32_t process_comp_y9 = (((int32_t)mag_data_y) * process_comp_y8);
retval = (int16_t)(process_comp_y9 / 8192);
retval = (retval + (((int16_t)trim_data.dig_y1) * 8)) / 16;
return retval;
}
static int16_t compensate_z(trim_data_t trim_data, int16_t mag_data_z, uint16_t data_rhall) {
int16_t process_comp_z0 = ((int16_t)data_rhall) - ((int16_t) trim_data.dig_xyz1);
int32_t process_comp_z1 = (((int32_t)trim_data.dig_z3) * ((int32_t)(process_comp_z0))) / 4;
int32_t process_comp_z2 = (((int32_t)(mag_data_z - trim_data.dig_z4)) * 32768);
int32_t process_comp_z3 = ((int32_t)trim_data.dig_z1) * (((int16_t)data_rhall) * 2);
int16_t process_comp_z4 = (int16_t)((process_comp_z3 + (32768)) / 65536);
int32_t retval = ((process_comp_z2 - process_comp_z1) / (trim_data.dig_z2 + process_comp_z4));
/* saturate result to +/- 2 micro-tesla */
retval = std::clamp(retval, -32767, 32767);
/* Conversion of LSB to micro-tesla*/
retval = retval / 16;
return (int16_t)retval;
}
BMX055_Magn::BMX055_Magn(I2CBus *bus) : I2CSensor(bus) {}
int BMX055_Magn::init() {
uint8_t trim_x1y1[2] = {0};
uint8_t trim_x2y2[2] = {0};
uint8_t trim_xy1xy2[2] = {0};
uint8_t trim_z1[2] = {0};
uint8_t trim_z2[2] = {0};
uint8_t trim_z3[2] = {0};
uint8_t trim_z4[2] = {0};
uint8_t trim_xyz1[2] = {0};
// suspend -> sleep
int ret = set_register(BMX055_MAGN_I2C_REG_PWR_0, 0x01);
if (ret < 0) {
LOGE("Enabling power failed: %d", ret);
goto fail;
}
util::sleep_for(5); // wait until the chip is powered on
ret = verify_chip_id(BMX055_MAGN_I2C_REG_ID, {BMX055_MAGN_CHIP_ID});
if (ret == -1) {
goto fail;
}
// Load magnetometer trim
ret = read_register(BMX055_MAGN_I2C_REG_DIG_X1, trim_x1y1, 2);
if (ret < 0) goto fail;
ret = read_register(BMX055_MAGN_I2C_REG_DIG_X2, trim_x2y2, 2);
if (ret < 0) goto fail;
ret = read_register(BMX055_MAGN_I2C_REG_DIG_XY2, trim_xy1xy2, 2);
if (ret < 0) goto fail;
ret = read_register(BMX055_MAGN_I2C_REG_DIG_Z1_LSB, trim_z1, 2);
if (ret < 0) goto fail;
ret = read_register(BMX055_MAGN_I2C_REG_DIG_Z2_LSB, trim_z2, 2);
if (ret < 0) goto fail;
ret = read_register(BMX055_MAGN_I2C_REG_DIG_Z3_LSB, trim_z3, 2);
if (ret < 0) goto fail;
ret = read_register(BMX055_MAGN_I2C_REG_DIG_Z4_LSB, trim_z4, 2);
if (ret < 0) goto fail;
ret = read_register(BMX055_MAGN_I2C_REG_DIG_XYZ1_LSB, trim_xyz1, 2);
if (ret < 0) goto fail;
// Read trim data
trim_data.dig_x1 = trim_x1y1[0];
trim_data.dig_y1 = trim_x1y1[1];
trim_data.dig_x2 = trim_x2y2[0];
trim_data.dig_y2 = trim_x2y2[1];
trim_data.dig_xy1 = trim_xy1xy2[1]; // NB: MSB/LSB swapped
trim_data.dig_xy2 = trim_xy1xy2[0];
trim_data.dig_z1 = read_16_bit(trim_z1[0], trim_z1[1]);
trim_data.dig_z2 = read_16_bit(trim_z2[0], trim_z2[1]);
trim_data.dig_z3 = read_16_bit(trim_z3[0], trim_z3[1]);
trim_data.dig_z4 = read_16_bit(trim_z4[0], trim_z4[1]);
trim_data.dig_xyz1 = read_16_bit(trim_xyz1[0], trim_xyz1[1] & 0x7f);
assert(trim_data.dig_xyz1 != 0);
perform_self_test();
// f_max = 1 / (145us * nXY + 500us * NZ + 980us)
// Chose NXY = 7, NZ = 12, which gives 125 Hz,
// and has the same ratio as the high accuracy preset
ret = set_register(BMX055_MAGN_I2C_REG_REPXY, (7 - 1) / 2);
if (ret < 0) {
goto fail;
}
ret = set_register(BMX055_MAGN_I2C_REG_REPZ, 12 - 1);
if (ret < 0) {
goto fail;
}
return 0;
fail:
return ret;
}
int BMX055_Magn::shutdown() {
// move to suspend mode
int ret = set_register(BMX055_MAGN_I2C_REG_PWR_0, 0);
if (ret < 0) {
LOGE("Could not move BMX055 MAGN in suspend mode!");
}
return ret;
}
bool BMX055_Magn::perform_self_test() {
uint8_t buffer[8];
int16_t x, y;
int16_t neg_z, pos_z;
// Increase z reps for less false positives (~30 Hz ODR)
set_register(BMX055_MAGN_I2C_REG_REPXY, 1);
set_register(BMX055_MAGN_I2C_REG_REPZ, 64 - 1);
// Clean existing measurement
read_register(BMX055_MAGN_I2C_REG_DATAX_LSB, buffer, sizeof(buffer));
uint8_t forced = BMX055_MAGN_FORCED;
// Negative current
set_register(BMX055_MAGN_I2C_REG_MAG, forced | (uint8_t(0b10) << 6));
util::sleep_for(100);
read_register(BMX055_MAGN_I2C_REG_DATAX_LSB, buffer, sizeof(buffer));
parse_xyz(buffer, &x, &y, &neg_z);
// Positive current
set_register(BMX055_MAGN_I2C_REG_MAG, forced | (uint8_t(0b11) << 6));
util::sleep_for(100);
read_register(BMX055_MAGN_I2C_REG_DATAX_LSB, buffer, sizeof(buffer));
parse_xyz(buffer, &x, &y, &pos_z);
// Put back in normal mode
set_register(BMX055_MAGN_I2C_REG_MAG, 0);
int16_t diff = pos_z - neg_z;
bool passed = (diff > 180) && (diff < 240);
if (!passed) {
LOGE("self test failed: neg %d pos %d diff %d", neg_z, pos_z, diff);
}
return passed;
}
bool BMX055_Magn::parse_xyz(uint8_t buffer[8], int16_t *x, int16_t *y, int16_t *z) {
bool ready = buffer[6] & 0x1;
if (ready) {
int16_t mdata_x = (int16_t) (((int16_t)buffer[1] << 8) | buffer[0]) >> 3;
int16_t mdata_y = (int16_t) (((int16_t)buffer[3] << 8) | buffer[2]) >> 3;
int16_t mdata_z = (int16_t) (((int16_t)buffer[5] << 8) | buffer[4]) >> 1;
uint16_t data_r = (uint16_t) (((uint16_t)buffer[7] << 8) | buffer[6]) >> 2;
assert(data_r != 0);
*x = compensate_x(trim_data, mdata_x, data_r);
*y = compensate_y(trim_data, mdata_y, data_r);
*z = compensate_z(trim_data, mdata_z, data_r);
}
return ready;
}
bool BMX055_Magn::get_event(MessageBuilder &msg, uint64_t ts) {
uint64_t start_time = nanos_since_boot();
uint8_t buffer[8];
int16_t _x, _y, x, y, z;
int len = read_register(BMX055_MAGN_I2C_REG_DATAX_LSB, buffer, sizeof(buffer));
assert(len == sizeof(buffer));
bool parsed = parse_xyz(buffer, &_x, &_y, &z);
if (parsed) {
auto event = msg.initEvent().initMagnetometer();
event.setSource(cereal::SensorEventData::SensorSource::BMX055);
event.setVersion(2);
event.setSensor(SENSOR_MAGNETOMETER_UNCALIBRATED);
event.setType(SENSOR_TYPE_MAGNETIC_FIELD_UNCALIBRATED);
event.setTimestamp(start_time);
// Move magnetometer into same reference frame as accel/gryo
x = -_y;
y = _x;
// Axis convention
x = -x;
y = -y;
float xyz[] = {(float)x, (float)y, (float)z};
auto svec = event.initMagneticUncalibrated();
svec.setV(xyz);
svec.setStatus(true);
}
// The BMX055 Magnetometer has no FIFO mode. Self running mode only goes
// up to 30 Hz. Therefore we put in forced mode, and request measurements
// at a 100 Hz. When reading the registers we have to check the ready bit
// To verify the measurement was completed this cycle.
set_register(BMX055_MAGN_I2C_REG_MAG, BMX055_MAGN_FORCED);
return parsed;
}

64
system/sensord/sensors/bmx055_magn.h
View File

@@ -1,64 +0,0 @@
#pragma once
#include <tuple>
#include "system/sensord/sensors/i2c_sensor.h"
// Address of the chip on the bus
#define BMX055_MAGN_I2C_ADDR 0x10
// Registers of the chip
#define BMX055_MAGN_I2C_REG_ID 0x40
#define BMX055_MAGN_I2C_REG_PWR_0 0x4B
#define BMX055_MAGN_I2C_REG_MAG 0x4C
#define BMX055_MAGN_I2C_REG_DATAX_LSB 0x42
#define BMX055_MAGN_I2C_REG_RHALL_LSB 0x48
#define BMX055_MAGN_I2C_REG_REPXY 0x51
#define BMX055_MAGN_I2C_REG_REPZ 0x52
#define BMX055_MAGN_I2C_REG_DIG_X1 0x5D
#define BMX055_MAGN_I2C_REG_DIG_Y1 0x5E
#define BMX055_MAGN_I2C_REG_DIG_Z4_LSB 0x62
#define BMX055_MAGN_I2C_REG_DIG_Z4_MSB 0x63
#define BMX055_MAGN_I2C_REG_DIG_X2 0x64
#define BMX055_MAGN_I2C_REG_DIG_Y2 0x65
#define BMX055_MAGN_I2C_REG_DIG_Z2_LSB 0x68
#define BMX055_MAGN_I2C_REG_DIG_Z2_MSB 0x69
#define BMX055_MAGN_I2C_REG_DIG_Z1_LSB 0x6A
#define BMX055_MAGN_I2C_REG_DIG_Z1_MSB 0x6B
#define BMX055_MAGN_I2C_REG_DIG_XYZ1_LSB 0x6C
#define BMX055_MAGN_I2C_REG_DIG_XYZ1_MSB 0x6D
#define BMX055_MAGN_I2C_REG_DIG_Z3_LSB 0x6E
#define BMX055_MAGN_I2C_REG_DIG_Z3_MSB 0x6F
#define BMX055_MAGN_I2C_REG_DIG_XY2 0x70
#define BMX055_MAGN_I2C_REG_DIG_XY1 0x71
// Constants
#define BMX055_MAGN_CHIP_ID 0x32
#define BMX055_MAGN_FORCED (0b01 << 1)
struct trim_data_t {
int8_t dig_x1;
int8_t dig_y1;
int8_t dig_x2;
int8_t dig_y2;
uint16_t dig_z1;
int16_t dig_z2;
int16_t dig_z3;
int16_t dig_z4;
uint8_t dig_xy1;
int8_t dig_xy2;
uint16_t dig_xyz1;
};
class BMX055_Magn : public I2CSensor{
uint8_t get_device_address() {return BMX055_MAGN_I2C_ADDR;}
trim_data_t trim_data = {0};
bool perform_self_test();
bool parse_xyz(uint8_t buffer[8], int16_t *x, int16_t *y, int16_t *z);
public:
BMX055_Magn(I2CBus *bus);
int init();
bool get_event(MessageBuilder &msg, uint64_t ts = 0);
int shutdown();
};

31
system/sensord/sensors/bmx055_temp.cc
View File

@@ -1,31 +0,0 @@
#include "system/sensord/sensors/bmx055_temp.h"
#include <cassert>
#include "system/sensord/sensors/bmx055_accel.h"
#include "common/swaglog.h"
#include "common/timing.h"
BMX055_Temp::BMX055_Temp(I2CBus *bus) : I2CSensor(bus) {}
int BMX055_Temp::init() {
return verify_chip_id(BMX055_ACCEL_I2C_REG_ID, {BMX055_ACCEL_CHIP_ID}) == -1 ? -1 : 0;
}
bool BMX055_Temp::get_event(MessageBuilder &msg, uint64_t ts) {
uint64_t start_time = nanos_since_boot();
uint8_t buffer[1];
int len = read_register(BMX055_ACCEL_I2C_REG_TEMP, buffer, sizeof(buffer));
assert(len == sizeof(buffer));
float temp = 23.0f + int8_t(buffer[0]) / 2.0f;
auto event = msg.initEvent().initTemperatureSensor();
event.setSource(cereal::SensorEventData::SensorSource::BMX055);
event.setVersion(1);
event.setType(SENSOR_TYPE_AMBIENT_TEMPERATURE);
event.setTimestamp(start_time);
event.setTemperature(temp);
return true;
}

13
system/sensord/sensors/bmx055_temp.h
View File

@@ -1,13 +0,0 @@
#pragma once
#include "system/sensord/sensors/bmx055_accel.h"
#include "system/sensord/sensors/i2c_sensor.h"
class BMX055_Temp : public I2CSensor {
uint8_t get_device_address() {return BMX055_ACCEL_I2C_ADDR;}
public:
BMX055_Temp(I2CBus *bus);
int init();
bool get_event(MessageBuilder &msg, uint64_t ts = 0);
int shutdown() { return 0; }
};

18
system/sensord/sensors/constants.h
View File

@@ -1,18 +0,0 @@
#pragma once
#define SENSOR_ACCELEROMETER 1
#define SENSOR_MAGNETOMETER 2
#define SENSOR_MAGNETOMETER_UNCALIBRATED 3
#define SENSOR_GYRO 4
#define SENSOR_GYRO_UNCALIBRATED 5
#define SENSOR_LIGHT 7
#define SENSOR_TYPE_ACCELEROMETER 1
#define SENSOR_TYPE_GEOMAGNETIC_FIELD 2
#define SENSOR_TYPE_GYROSCOPE 4
#define SENSOR_TYPE_LIGHT 5
#define SENSOR_TYPE_AMBIENT_TEMPERATURE 13
#define SENSOR_TYPE_MAGNETIC_FIELD_UNCALIBRATED 14
#define SENSOR_TYPE_MAGNETIC_FIELD SENSOR_TYPE_GEOMAGNETIC_FIELD
#define SENSOR_TYPE_GYROSCOPE_UNCALIBRATED 16

50
system/sensord/sensors/i2c_sensor.cc
View File

@@ -1,50 +0,0 @@
#include "system/sensord/sensors/i2c_sensor.h"
int16_t read_12_bit(uint8_t lsb, uint8_t msb) {
uint16_t combined = (uint16_t(msb) << 8) | uint16_t(lsb & 0xF0);
return int16_t(combined) / (1 << 4);
}
int16_t read_16_bit(uint8_t lsb, uint8_t msb) {
uint16_t combined = (uint16_t(msb) << 8) | uint16_t(lsb);
return int16_t(combined);
}
int32_t read_20_bit(uint8_t b2, uint8_t b1, uint8_t b0) {
uint32_t combined = (uint32_t(b0) << 16) | (uint32_t(b1) << 8) | uint32_t(b2);
return int32_t(combined) / (1 << 4);
}
I2CSensor::I2CSensor(I2CBus *bus, int gpio_nr, bool shared_gpio) :
bus(bus), gpio_nr(gpio_nr), shared_gpio(shared_gpio) {}
I2CSensor::~I2CSensor() {
if (gpio_fd != -1) {
close(gpio_fd);
}
}
int I2CSensor::read_register(uint register_address, uint8_t *buffer, uint8_t len) {
return bus->read_register(get_device_address(), register_address, buffer, len);
}
int I2CSensor::set_register(uint register_address, uint8_t data) {
return bus->set_register(get_device_address(), register_address, data);
}
int I2CSensor::init_gpio() {
if (shared_gpio || gpio_nr == 0) {
return 0;
}
gpio_fd = gpiochip_get_ro_value_fd("sensord", GPIOCHIP_INT, gpio_nr);
if (gpio_fd < 0) {
return -1;
}
return 0;
}
bool I2CSensor::has_interrupt_enabled() {
return gpio_nr != 0;
}

51
system/sensord/sensors/i2c_sensor.h
View File

@@ -1,51 +0,0 @@
#pragma once
#include <cstdint>
#include <unistd.h>
#include <vector>
#include "cereal/gen/cpp/log.capnp.h"
#include "common/i2c.h"
#include "common/gpio.h"
#include "common/swaglog.h"
#include "system/sensord/sensors/constants.h"
#include "system/sensord/sensors/sensor.h"
int16_t read_12_bit(uint8_t lsb, uint8_t msb);
int16_t read_16_bit(uint8_t lsb, uint8_t msb);
int32_t read_20_bit(uint8_t b2, uint8_t b1, uint8_t b0);
class I2CSensor : public Sensor {
private:
I2CBus *bus;
int gpio_nr;
bool shared_gpio;
virtual uint8_t get_device_address() = 0;
public:
I2CSensor(I2CBus *bus, int gpio_nr = 0, bool shared_gpio = false);
~I2CSensor();
int read_register(uint register_address, uint8_t *buffer, uint8_t len);
int set_register(uint register_address, uint8_t data);
int init_gpio();
bool has_interrupt_enabled();
virtual int init() = 0;
virtual bool get_event(MessageBuilder &msg, uint64_t ts = 0) = 0;
virtual int shutdown() = 0;
int verify_chip_id(uint8_t address, const std::vector<uint8_t> &expected_ids) {
uint8_t chip_id = 0;
int ret = read_register(address, &chip_id, 1);
if (ret < 0) {
LOGE("Reading chip ID failed: %d", ret);
return -1;
}
for (int i = 0; i < expected_ids.size(); ++i) {
if (chip_id == expected_ids[i]) return chip_id;
}
LOGE("Chip ID wrong. Got: %d, Expected %d", chip_id, expected_ids[0]);
return -1;
}
};

250
system/sensord/sensors/lsm6ds3_accel.cc
View File

@@ -1,250 +0,0 @@
#include "system/sensord/sensors/lsm6ds3_accel.h"
#include <cassert>
#include <cmath>
#include <cstring>
#include "common/swaglog.h"
#include "common/timing.h"
#include "common/util.h"
LSM6DS3_Accel::LSM6DS3_Accel(I2CBus *bus, int gpio_nr, bool shared_gpio) :
I2CSensor(bus, gpio_nr, shared_gpio) {}
void LSM6DS3_Accel::wait_for_data_ready() {
uint8_t drdy = 0;
uint8_t buffer[6];
do {
read_register(LSM6DS3_ACCEL_I2C_REG_STAT_REG, &drdy, sizeof(drdy));
drdy &= LSM6DS3_ACCEL_DRDY_XLDA;
} while (drdy == 0);
read_register(LSM6DS3_ACCEL_I2C_REG_OUTX_L_XL, buffer, sizeof(buffer));
}
void LSM6DS3_Accel::read_and_avg_data(float* out_buf) {
uint8_t drdy = 0;
uint8_t buffer[6];
float scaling = 0.061f;
if (source == cereal::SensorEventData::SensorSource::LSM6DS3TRC) {
scaling = 0.122f;
}
for (int i = 0; i < 5; i++) {
do {
read_register(LSM6DS3_ACCEL_I2C_REG_STAT_REG, &drdy, sizeof(drdy));
drdy &= LSM6DS3_ACCEL_DRDY_XLDA;
} while (drdy == 0);
int len = read_register(LSM6DS3_ACCEL_I2C_REG_OUTX_L_XL, buffer, sizeof(buffer));
assert(len == sizeof(buffer));
for (int j = 0; j < 3; j++) {
out_buf[j] += (float)read_16_bit(buffer[j*2], buffer[j*2+1]) * scaling;
}
}
for (int i = 0; i < 3; i++) {
out_buf[i] /= 5.0f;
}
}
int LSM6DS3_Accel::self_test(int test_type) {
float val_st_off[3] = {0};
float val_st_on[3] = {0};
float test_val[3] = {0};
uint8_t ODR_FS_MO = LSM6DS3_ACCEL_ODR_52HZ; // full scale: +-2g, ODR: 52Hz
// prepare sensor for self-test
// enable block data update and automatic increment
int ret = set_register(LSM6DS3_ACCEL_I2C_REG_CTRL3_C, LSM6DS3_ACCEL_IF_INC_BDU);
if (ret < 0) {
return ret;
}
if (source == cereal::SensorEventData::SensorSource::LSM6DS3TRC) {
ODR_FS_MO = LSM6DS3_ACCEL_FS_4G | LSM6DS3_ACCEL_ODR_52HZ;
}
ret = set_register(LSM6DS3_ACCEL_I2C_REG_CTRL1_XL, ODR_FS_MO);
if (ret < 0) {
return ret;
}
// wait for stable output, and discard first values
util::sleep_for(100);
wait_for_data_ready();
read_and_avg_data(val_st_off);
// enable Self Test positive (or negative)
ret = set_register(LSM6DS3_ACCEL_I2C_REG_CTRL5_C, test_type);
if (ret < 0) {
return ret;
}
// wait for stable output, and discard first values
util::sleep_for(100);
wait_for_data_ready();
read_and_avg_data(val_st_on);
// disable sensor
ret = set_register(LSM6DS3_ACCEL_I2C_REG_CTRL1_XL, 0);
if (ret < 0) {
return ret;
}
// disable self test
ret = set_register(LSM6DS3_ACCEL_I2C_REG_CTRL5_C, 0);
if (ret < 0) {
return ret;
}
// calculate the mg values for self test
for (int i = 0; i < 3; i++) {
test_val[i] = fabs(val_st_on[i] - val_st_off[i]);
}
// verify test result
for (int i = 0; i < 3; i++) {
if ((LSM6DS3_ACCEL_MIN_ST_LIMIT_mg > test_val[i]) ||
(test_val[i] > LSM6DS3_ACCEL_MAX_ST_LIMIT_mg)) {
return -1;
}
}
return ret;
}
int LSM6DS3_Accel::init() {
uint8_t value = 0;
bool do_self_test = false;
const char* env_lsm_selftest = std::getenv("LSM_SELF_TEST");
if (env_lsm_selftest != nullptr && strncmp(env_lsm_selftest, "1", 1) == 0) {
do_self_test = true;
}
int ret = verify_chip_id(LSM6DS3_ACCEL_I2C_REG_ID, {LSM6DS3_ACCEL_CHIP_ID, LSM6DS3TRC_ACCEL_CHIP_ID});
if (ret == -1) return -1;
if (ret == LSM6DS3TRC_ACCEL_CHIP_ID) {
source = cereal::SensorEventData::SensorSource::LSM6DS3TRC;
}
ret = self_test(LSM6DS3_ACCEL_POSITIVE_TEST);
if (ret < 0) {
LOGE("LSM6DS3 accel positive self-test failed!");
if (do_self_test) goto fail;
}
ret = self_test(LSM6DS3_ACCEL_NEGATIVE_TEST);
if (ret < 0) {
LOGE("LSM6DS3 accel negative self-test failed!");
if (do_self_test) goto fail;
}
ret = init_gpio();
if (ret < 0) {
goto fail;
}
// enable continuous update, and automatic increase
ret = set_register(LSM6DS3_ACCEL_I2C_REG_CTRL3_C, LSM6DS3_ACCEL_IF_INC);
if (ret < 0) {
goto fail;
}
// TODO: set scale and bandwidth. Default is +- 2G, 50 Hz
ret = set_register(LSM6DS3_ACCEL_I2C_REG_CTRL1_XL, LSM6DS3_ACCEL_ODR_104HZ);
if (ret < 0) {
goto fail;
}
ret = set_register(LSM6DS3_ACCEL_I2C_REG_DRDY_CFG, LSM6DS3_ACCEL_DRDY_PULSE_MODE);
if (ret < 0) {
goto fail;
}
// enable data ready interrupt for accel on INT1
// (without resetting existing interrupts)
ret = read_register(LSM6DS3_ACCEL_I2C_REG_INT1_CTRL, &value, 1);
if (ret < 0) {
goto fail;
}
value |= LSM6DS3_ACCEL_INT1_DRDY_XL;
ret = set_register(LSM6DS3_ACCEL_I2C_REG_INT1_CTRL, value);
fail:
return ret;
}
int LSM6DS3_Accel::shutdown() {
int ret = 0;
// disable data ready interrupt for accel on INT1
uint8_t value = 0;
ret = read_register(LSM6DS3_ACCEL_I2C_REG_INT1_CTRL, &value, 1);
if (ret < 0) {
goto fail;
}
value &= ~(LSM6DS3_ACCEL_INT1_DRDY_XL);
ret = set_register(LSM6DS3_ACCEL_I2C_REG_INT1_CTRL, value);
if (ret < 0) {
LOGE("Could not disable lsm6ds3 acceleration interrupt!");
goto fail;
}
// enable power-down mode
value = 0;
ret = read_register(LSM6DS3_ACCEL_I2C_REG_CTRL1_XL, &value, 1);
if (ret < 0) {
goto fail;
}
value &= 0x0F;
ret = set_register(LSM6DS3_ACCEL_I2C_REG_CTRL1_XL, value);
if (ret < 0) {
LOGE("Could not power-down lsm6ds3 accelerometer!");
goto fail;
}
fail:
return ret;
}
bool LSM6DS3_Accel::get_event(MessageBuilder &msg, uint64_t ts) {
// INT1 shared with gyro, check STATUS_REG who triggered
uint8_t status_reg = 0;
read_register(LSM6DS3_ACCEL_I2C_REG_STAT_REG, &status_reg, sizeof(status_reg));
if ((status_reg & LSM6DS3_ACCEL_DRDY_XLDA) == 0) {
return false;
}
uint8_t buffer[6];
int len = read_register(LSM6DS3_ACCEL_I2C_REG_OUTX_L_XL, buffer, sizeof(buffer));
assert(len == sizeof(buffer));
float scale = 9.81 * 2.0f / (1 << 15);
float x = read_16_bit(buffer[0], buffer[1]) * scale;
float y = read_16_bit(buffer[2], buffer[3]) * scale;
float z = read_16_bit(buffer[4], buffer[5]) * scale;
auto event = msg.initEvent().initAccelerometer();
event.setSource(source);
event.setVersion(1);
event.setSensor(SENSOR_ACCELEROMETER);
event.setType(SENSOR_TYPE_ACCELEROMETER);
event.setTimestamp(ts);
float xyz[] = {y, -x, z};
auto svec = event.initAcceleration();
svec.setV(xyz);
svec.setStatus(true);
return true;
}

49
system/sensord/sensors/lsm6ds3_accel.h
View File

@@ -1,49 +0,0 @@
#pragma once
#include "system/sensord/sensors/i2c_sensor.h"
// Address of the chip on the bus
#define LSM6DS3_ACCEL_I2C_ADDR 0x6A
// Registers of the chip
#define LSM6DS3_ACCEL_I2C_REG_DRDY_CFG 0x0B
#define LSM6DS3_ACCEL_I2C_REG_ID 0x0F
#define LSM6DS3_ACCEL_I2C_REG_INT1_CTRL 0x0D
#define LSM6DS3_ACCEL_I2C_REG_CTRL1_XL 0x10
#define LSM6DS3_ACCEL_I2C_REG_CTRL3_C 0x12
#define LSM6DS3_ACCEL_I2C_REG_CTRL5_C 0x14
#define LSM6DS3_ACCEL_I2C_REG_CTR9_XL 0x18
#define LSM6DS3_ACCEL_I2C_REG_STAT_REG 0x1E
#define LSM6DS3_ACCEL_I2C_REG_OUTX_L_XL 0x28
// Constants
#define LSM6DS3_ACCEL_CHIP_ID 0x69
#define LSM6DS3TRC_ACCEL_CHIP_ID 0x6A
#define LSM6DS3_ACCEL_FS_4G (0b10 << 2)
#define LSM6DS3_ACCEL_ODR_52HZ (0b0011 << 4)
#define LSM6DS3_ACCEL_ODR_104HZ (0b0100 << 4)
#define LSM6DS3_ACCEL_INT1_DRDY_XL 0b1
#define LSM6DS3_ACCEL_DRDY_XLDA 0b1
#define LSM6DS3_ACCEL_DRDY_PULSE_MODE (1 << 7)
#define LSM6DS3_ACCEL_IF_INC 0b00000100
#define LSM6DS3_ACCEL_IF_INC_BDU 0b01000100
#define LSM6DS3_ACCEL_XYZ_DEN 0b11100000
#define LSM6DS3_ACCEL_POSITIVE_TEST 0b01
#define LSM6DS3_ACCEL_NEGATIVE_TEST 0b10
#define LSM6DS3_ACCEL_MIN_ST_LIMIT_mg 90.0f
#define LSM6DS3_ACCEL_MAX_ST_LIMIT_mg 1700.0f
class LSM6DS3_Accel : public I2CSensor {
uint8_t get_device_address() {return LSM6DS3_ACCEL_I2C_ADDR;}
cereal::SensorEventData::SensorSource source = cereal::SensorEventData::SensorSource::LSM6DS3;
// self test functions
int self_test(int test_type);
void wait_for_data_ready();
void read_and_avg_data(float* val_st_off);
public:
LSM6DS3_Accel(I2CBus *bus, int gpio_nr = 0, bool shared_gpio = false);
int init();
bool get_event(MessageBuilder &msg, uint64_t ts = 0);
int shutdown();
};

233
system/sensord/sensors/lsm6ds3_gyro.cc
View File

@@ -1,233 +0,0 @@
#include "system/sensord/sensors/lsm6ds3_gyro.h"
#include <cassert>
#include <cmath>
#include <cstring>
#include "common/swaglog.h"
#include "common/timing.h"
#include "common/util.h"
#define DEG2RAD(x) ((x) * M_PI / 180.0)
LSM6DS3_Gyro::LSM6DS3_Gyro(I2CBus *bus, int gpio_nr, bool shared_gpio) :
I2CSensor(bus, gpio_nr, shared_gpio) {}
void LSM6DS3_Gyro::wait_for_data_ready() {
uint8_t drdy = 0;
uint8_t buffer[6];
do {
read_register(LSM6DS3_GYRO_I2C_REG_STAT_REG, &drdy, sizeof(drdy));
drdy &= LSM6DS3_GYRO_DRDY_GDA;
} while (drdy == 0);
read_register(LSM6DS3_GYRO_I2C_REG_OUTX_L_G, buffer, sizeof(buffer));
}
void LSM6DS3_Gyro::read_and_avg_data(float* out_buf) {
uint8_t drdy = 0;
uint8_t buffer[6];
for (int i = 0; i < 5; i++) {
do {
read_register(LSM6DS3_GYRO_I2C_REG_STAT_REG, &drdy, sizeof(drdy));
drdy &= LSM6DS3_GYRO_DRDY_GDA;
} while (drdy == 0);
int len = read_register(LSM6DS3_GYRO_I2C_REG_OUTX_L_G, buffer, sizeof(buffer));
assert(len == sizeof(buffer));
for (int j = 0; j < 3; j++) {
out_buf[j] += (float)read_16_bit(buffer[j*2], buffer[j*2+1]) * 70.0f;
}
}
// calculate the mg average values
for (int i = 0; i < 3; i++) {
out_buf[i] /= 5.0f;
}
}
int LSM6DS3_Gyro::self_test(int test_type) {
float val_st_off[3] = {0};
float val_st_on[3] = {0};
float test_val[3] = {0};
// prepare sensor for self-test
// full scale: 2000dps, ODR: 208Hz
int ret = set_register(LSM6DS3_GYRO_I2C_REG_CTRL2_G, LSM6DS3_GYRO_ODR_208HZ | LSM6DS3_GYRO_FS_2000dps);
if (ret < 0) {
return ret;
}
// wait for stable output, and discard first values
util::sleep_for(150);
wait_for_data_ready();
read_and_avg_data(val_st_off);
// enable Self Test positive (or negative)
ret = set_register(LSM6DS3_GYRO_I2C_REG_CTRL5_C, test_type);
if (ret < 0) {
return ret;
}
// wait for stable output, and discard first values
util::sleep_for(50);
wait_for_data_ready();
read_and_avg_data(val_st_on);
// disable sensor
ret = set_register(LSM6DS3_GYRO_I2C_REG_CTRL2_G, 0);
if (ret < 0) {
return ret;
}
// disable self test
ret = set_register(LSM6DS3_GYRO_I2C_REG_CTRL5_C, 0);
if (ret < 0) {
return ret;
}
// calculate the mg values for self test
for (int i = 0; i < 3; i++) {
test_val[i] = fabs(val_st_on[i] - val_st_off[i]);
}
// verify test result
for (int i = 0; i < 3; i++) {
if ((LSM6DS3_GYRO_MIN_ST_LIMIT_mdps > test_val[i]) ||
(test_val[i] > LSM6DS3_GYRO_MAX_ST_LIMIT_mdps)) {
return -1;
}
}
return ret;
}
int LSM6DS3_Gyro::init() {
uint8_t value = 0;
bool do_self_test = false;
const char* env_lsm_selftest =env_lsm_selftest = std::getenv("LSM_SELF_TEST");
if (env_lsm_selftest != nullptr && strncmp(env_lsm_selftest, "1", 1) == 0) {
do_self_test = true;
}
int ret = verify_chip_id(LSM6DS3_GYRO_I2C_REG_ID, {LSM6DS3_GYRO_CHIP_ID, LSM6DS3TRC_GYRO_CHIP_ID});
if (ret == -1) return -1;
if (ret == LSM6DS3TRC_GYRO_CHIP_ID) {
source = cereal::SensorEventData::SensorSource::LSM6DS3TRC;
}
ret = init_gpio();
if (ret < 0) {
goto fail;
}
ret = self_test(LSM6DS3_GYRO_POSITIVE_TEST);
if (ret < 0) {
LOGE("LSM6DS3 gyro positive self-test failed!");
if (do_self_test) goto fail;
}
ret = self_test(LSM6DS3_GYRO_NEGATIVE_TEST);
if (ret < 0) {
LOGE("LSM6DS3 gyro negative self-test failed!");
if (do_self_test) goto fail;
}
// TODO: set scale. Default is +- 250 deg/s
ret = set_register(LSM6DS3_GYRO_I2C_REG_CTRL2_G, LSM6DS3_GYRO_ODR_104HZ);
if (ret < 0) {
goto fail;
}
ret = set_register(LSM6DS3_GYRO_I2C_REG_DRDY_CFG, LSM6DS3_GYRO_DRDY_PULSE_MODE);
if (ret < 0) {
goto fail;
}
// enable data ready interrupt for gyro on INT1
// (without resetting existing interrupts)
ret = read_register(LSM6DS3_GYRO_I2C_REG_INT1_CTRL, &value, 1);
if (ret < 0) {
goto fail;
}
value |= LSM6DS3_GYRO_INT1_DRDY_G;
ret = set_register(LSM6DS3_GYRO_I2C_REG_INT1_CTRL, value);
fail:
return ret;
}
int LSM6DS3_Gyro::shutdown() {
int ret = 0;
// disable data ready interrupt for gyro on INT1
uint8_t value = 0;
ret = read_register(LSM6DS3_GYRO_I2C_REG_INT1_CTRL, &value, 1);
if (ret < 0) {
goto fail;
}
value &= ~(LSM6DS3_GYRO_INT1_DRDY_G);
ret = set_register(LSM6DS3_GYRO_I2C_REG_INT1_CTRL, value);
if (ret < 0) {
LOGE("Could not disable lsm6ds3 gyroscope interrupt!");
goto fail;
}
// enable power-down mode
value = 0;
ret = read_register(LSM6DS3_GYRO_I2C_REG_CTRL2_G, &value, 1);
if (ret < 0) {
goto fail;
}
value &= 0x0F;
ret = set_register(LSM6DS3_GYRO_I2C_REG_CTRL2_G, value);
if (ret < 0) {
LOGE("Could not power-down lsm6ds3 gyroscope!");
goto fail;
}
fail:
return ret;
}
bool LSM6DS3_Gyro::get_event(MessageBuilder &msg, uint64_t ts) {
// INT1 shared with accel, check STATUS_REG who triggered
uint8_t status_reg = 0;
read_register(LSM6DS3_GYRO_I2C_REG_STAT_REG, &status_reg, sizeof(status_reg));
if ((status_reg & LSM6DS3_GYRO_DRDY_GDA) == 0) {
return false;
}
uint8_t buffer[6];
int len = read_register(LSM6DS3_GYRO_I2C_REG_OUTX_L_G, buffer, sizeof(buffer));
assert(len == sizeof(buffer));
float scale = 8.75 / 1000.0;
float x = DEG2RAD(read_16_bit(buffer[0], buffer[1]) * scale);
float y = DEG2RAD(read_16_bit(buffer[2], buffer[3]) * scale);
float z = DEG2RAD(read_16_bit(buffer[4], buffer[5]) * scale);
auto event = msg.initEvent().initGyroscope();
event.setSource(source);
event.setVersion(2);
event.setSensor(SENSOR_GYRO_UNCALIBRATED);
event.setType(SENSOR_TYPE_GYROSCOPE_UNCALIBRATED);
event.setTimestamp(ts);
float xyz[] = {y, -x, z};
auto svec = event.initGyroUncalibrated();
svec.setV(xyz);
svec.setStatus(true);
return true;
}

45
system/sensord/sensors/lsm6ds3_gyro.h
View File

@@ -1,45 +0,0 @@
#pragma once
#include "system/sensord/sensors/i2c_sensor.h"
// Address of the chip on the bus
#define LSM6DS3_GYRO_I2C_ADDR 0x6A
// Registers of the chip
#define LSM6DS3_GYRO_I2C_REG_DRDY_CFG 0x0B
#define LSM6DS3_GYRO_I2C_REG_ID 0x0F
#define LSM6DS3_GYRO_I2C_REG_INT1_CTRL 0x0D
#define LSM6DS3_GYRO_I2C_REG_CTRL2_G 0x11
#define LSM6DS3_GYRO_I2C_REG_CTRL5_C 0x14
#define LSM6DS3_GYRO_I2C_REG_STAT_REG 0x1E
#define LSM6DS3_GYRO_I2C_REG_OUTX_L_G 0x22
#define LSM6DS3_GYRO_POSITIVE_TEST (0b01 << 2)
#define LSM6DS3_GYRO_NEGATIVE_TEST (0b11 << 2)
// Constants
#define LSM6DS3_GYRO_CHIP_ID 0x69
#define LSM6DS3TRC_GYRO_CHIP_ID 0x6A
#define LSM6DS3_GYRO_FS_2000dps (0b11 << 2)
#define LSM6DS3_GYRO_ODR_104HZ (0b0100 << 4)
#define LSM6DS3_GYRO_ODR_208HZ (0b0101 << 4)
#define LSM6DS3_GYRO_INT1_DRDY_G 0b10
#define LSM6DS3_GYRO_DRDY_GDA 0b10
#define LSM6DS3_GYRO_DRDY_PULSE_MODE (1 << 7)
#define LSM6DS3_GYRO_MIN_ST_LIMIT_mdps 150000.0f
#define LSM6DS3_GYRO_MAX_ST_LIMIT_mdps 700000.0f
class LSM6DS3_Gyro : public I2CSensor {
uint8_t get_device_address() {return LSM6DS3_GYRO_I2C_ADDR;}
cereal::SensorEventData::SensorSource source = cereal::SensorEventData::SensorSource::LSM6DS3;
// self test functions
int self_test(int test_type);
void wait_for_data_ready();
void read_and_avg_data(float* val_st_off);
public:
LSM6DS3_Gyro(I2CBus *bus, int gpio_nr = 0, bool shared_gpio = false);
int init();
bool get_event(MessageBuilder &msg, uint64_t ts = 0);
int shutdown();
};

37
system/sensord/sensors/lsm6ds3_temp.cc
View File

@@ -1,37 +0,0 @@
#include "system/sensord/sensors/lsm6ds3_temp.h"
#include <cassert>
#include "common/swaglog.h"
#include "common/timing.h"
LSM6DS3_Temp::LSM6DS3_Temp(I2CBus *bus) : I2CSensor(bus) {}
int LSM6DS3_Temp::init() {
int ret = verify_chip_id(LSM6DS3_TEMP_I2C_REG_ID, {LSM6DS3_TEMP_CHIP_ID, LSM6DS3TRC_TEMP_CHIP_ID});
if (ret == -1) return -1;
if (ret == LSM6DS3TRC_TEMP_CHIP_ID) {
source = cereal::SensorEventData::SensorSource::LSM6DS3TRC;
}
return 0;
}
bool LSM6DS3_Temp::get_event(MessageBuilder &msg, uint64_t ts) {
uint64_t start_time = nanos_since_boot();
uint8_t buffer[2];
int len = read_register(LSM6DS3_TEMP_I2C_REG_OUT_TEMP_L, buffer, sizeof(buffer));
assert(len == sizeof(buffer));
float scale = (source == cereal::SensorEventData::SensorSource::LSM6DS3TRC) ? 256.0f : 16.0f;
float temp = 25.0f + read_16_bit(buffer[0], buffer[1]) / scale;
auto event = msg.initEvent().initTemperatureSensor();
event.setSource(source);
event.setVersion(1);
event.setType(SENSOR_TYPE_AMBIENT_TEMPERATURE);
event.setTimestamp(start_time);
event.setTemperature(temp);
return true;
}

26
system/sensord/sensors/lsm6ds3_temp.h
View File

@@ -1,26 +0,0 @@
#pragma once
#include "system/sensord/sensors/i2c_sensor.h"
// Address of the chip on the bus
#define LSM6DS3_TEMP_I2C_ADDR 0x6A
// Registers of the chip
#define LSM6DS3_TEMP_I2C_REG_ID 0x0F
#define LSM6DS3_TEMP_I2C_REG_OUT_TEMP_L 0x20
// Constants
#define LSM6DS3_TEMP_CHIP_ID 0x69
#define LSM6DS3TRC_TEMP_CHIP_ID 0x6A
class LSM6DS3_Temp : public I2CSensor {
uint8_t get_device_address() {return LSM6DS3_TEMP_I2C_ADDR;}
cereal::SensorEventData::SensorSource source = cereal::SensorEventData::SensorSource::LSM6DS3;
public:
LSM6DS3_Temp(I2CBus *bus);
int init();
bool get_event(MessageBuilder &msg, uint64_t ts = 0);
int shutdown() { return 0; }
};

108
system/sensord/sensors/mmc5603nj_magn.cc
View File

@@ -1,108 +0,0 @@
#include "system/sensord/sensors/mmc5603nj_magn.h"
#include <algorithm>
#include <cassert>
#include <vector>
#include "common/swaglog.h"
#include "common/timing.h"
#include "common/util.h"
MMC5603NJ_Magn::MMC5603NJ_Magn(I2CBus *bus) : I2CSensor(bus) {}
int MMC5603NJ_Magn::init() {
int ret = verify_chip_id(MMC5603NJ_I2C_REG_ID, {MMC5603NJ_CHIP_ID});
if (ret == -1) return -1;
// Set ODR to 0
ret = set_register(MMC5603NJ_I2C_REG_ODR, 0);
if (ret < 0) {
goto fail;
}
// Set BW to 0b01 for 1-150 Hz operation
ret = set_register(MMC5603NJ_I2C_REG_INTERNAL_1, 0b01);
if (ret < 0) {
goto fail;
}
fail:
return ret;
}
int MMC5603NJ_Magn::shutdown() {
int ret = 0;
// disable auto reset of measurements
uint8_t value = 0;
ret = read_register(MMC5603NJ_I2C_REG_INTERNAL_0, &value, 1);
if (ret < 0) {
goto fail;
}
value &= ~(MMC5603NJ_CMM_FREQ_EN | MMC5603NJ_AUTO_SR_EN);
ret = set_register(MMC5603NJ_I2C_REG_INTERNAL_0, value);
if (ret < 0) {
goto fail;
}
// set ODR to 0 to leave continuous mode
ret = set_register(MMC5603NJ_I2C_REG_ODR, 0);
if (ret < 0) {
goto fail;
}
return ret;
fail:
LOGE("Could not disable mmc5603nj auto set reset");
return ret;
}
void MMC5603NJ_Magn::start_measurement() {
set_register(MMC5603NJ_I2C_REG_INTERNAL_0, 0b01);
util::sleep_for(5);
}
std::vector<float> MMC5603NJ_Magn::read_measurement() {
int len;
uint8_t buffer[9];
len = read_register(MMC5603NJ_I2C_REG_XOUT0, buffer, sizeof(buffer));
assert(len == sizeof(buffer));
float scale = 1.0 / 16384.0;
float x = (read_20_bit(buffer[6], buffer[1], buffer[0]) * scale) - 32.0;
float y = (read_20_bit(buffer[7], buffer[3], buffer[2]) * scale) - 32.0;
float z = (read_20_bit(buffer[8], buffer[5], buffer[4]) * scale) - 32.0;
std::vector<float> xyz = {x, y, z};
return xyz;
}
bool MMC5603NJ_Magn::get_event(MessageBuilder &msg, uint64_t ts) {
uint64_t start_time = nanos_since_boot();
// SET - RESET cycle
set_register(MMC5603NJ_I2C_REG_INTERNAL_0, MMC5603NJ_SET);
util::sleep_for(5);
MMC5603NJ_Magn::start_measurement();
std::vector<float> xyz = MMC5603NJ_Magn::read_measurement();
set_register(MMC5603NJ_I2C_REG_INTERNAL_0, MMC5603NJ_RESET);
util::sleep_for(5);
MMC5603NJ_Magn::start_measurement();
std::vector<float> reset_xyz = MMC5603NJ_Magn::read_measurement();
auto event = msg.initEvent().initMagnetometer();
event.setSource(cereal::SensorEventData::SensorSource::MMC5603NJ);
event.setVersion(1);
event.setSensor(SENSOR_MAGNETOMETER_UNCALIBRATED);
event.setType(SENSOR_TYPE_MAGNETIC_FIELD_UNCALIBRATED);
event.setTimestamp(start_time);
float vals[] = {xyz[0], xyz[1], xyz[2], reset_xyz[0], reset_xyz[1], reset_xyz[2]};
bool valid = true;
if (std::any_of(std::begin(vals), std::end(vals), [](float val) { return val == -32.0; })) {
valid = false;
}
auto svec = event.initMagneticUncalibrated();
svec.setV(vals);
svec.setStatus(valid);
return true;
}

37
system/sensord/sensors/mmc5603nj_magn.h
View File

@@ -1,37 +0,0 @@
#pragma once
#include <vector>
#include "system/sensord/sensors/i2c_sensor.h"
// Address of the chip on the bus
#define MMC5603NJ_I2C_ADDR 0x30
// Registers of the chip
#define MMC5603NJ_I2C_REG_XOUT0 0x00
#define MMC5603NJ_I2C_REG_ODR 0x1A
#define MMC5603NJ_I2C_REG_INTERNAL_0 0x1B
#define MMC5603NJ_I2C_REG_INTERNAL_1 0x1C
#define MMC5603NJ_I2C_REG_INTERNAL_2 0x1D
#define MMC5603NJ_I2C_REG_ID 0x39
// Constants
#define MMC5603NJ_CHIP_ID 0x10
#define MMC5603NJ_CMM_FREQ_EN (1 << 7)
#define MMC5603NJ_AUTO_SR_EN (1 << 5)
#define MMC5603NJ_CMM_EN (1 << 4)
#define MMC5603NJ_EN_PRD_SET (1 << 3)
#define MMC5603NJ_SET (1 << 3)
#define MMC5603NJ_RESET (1 << 4)
class MMC5603NJ_Magn : public I2CSensor {
private:
uint8_t get_device_address() {return MMC5603NJ_I2C_ADDR;}
void start_measurement();
std::vector<float> read_measurement();
public:
MMC5603NJ_Magn(I2CBus *bus);
int init();
bool get_event(MessageBuilder &msg, uint64_t ts = 0);
int shutdown();
};

22
system/sensord/sensors/sensor.h
View File

@@ -1,22 +0,0 @@
#pragma once
#include "cereal/messaging/messaging.h"
class Sensor {
public:
int gpio_fd = -1;
uint64_t start_ts = 0;
uint64_t init_delay = 500e6; // default dealy 500ms
virtual ~Sensor() {}
virtual int init() = 0;
virtual bool get_event(MessageBuilder &msg, uint64_t ts = 0) = 0;
virtual bool has_interrupt_enabled() = 0;
virtual int shutdown() = 0;
virtual bool is_data_valid(uint64_t current_ts) {
if (start_ts == 0) {
start_ts = current_ts;
}
return (current_ts - start_ts) > init_delay;
}
};

168
system/sensord/sensors_qcom2.cc
View File

@@ -1,168 +0,0 @@
#include <sys/resource.h>
#include <chrono>
#include <thread>
#include <vector>
#include <map>
#include <poll.h>
#include <linux/gpio.h>
#include "cereal/services.h"
#include "cereal/messaging/messaging.h"
#include "common/i2c.h"
#include "common/ratekeeper.h"
#include "common/swaglog.h"
#include "common/timing.h"
#include "common/util.h"
#include "system/sensord/sensors/bmx055_accel.h"
#include "system/sensord/sensors/bmx055_gyro.h"
#include "system/sensord/sensors/bmx055_magn.h"
#include "system/sensord/sensors/bmx055_temp.h"
#include "system/sensord/sensors/constants.h"
#include "system/sensord/sensors/lsm6ds3_accel.h"
#include "system/sensord/sensors/lsm6ds3_gyro.h"
#include "system/sensord/sensors/lsm6ds3_temp.h"
#include "system/sensord/sensors/mmc5603nj_magn.h"
#define I2C_BUS_IMU 1
ExitHandler do_exit;
void interrupt_loop(std::vector<std::tuple<Sensor *, std::string>> sensors) {
PubMaster pm({"gyroscope", "accelerometer"});
int fd = -1;
for (auto &[sensor, msg_name] : sensors) {
if (sensor->has_interrupt_enabled()) {
fd = sensor->gpio_fd;
break;
}
}
struct pollfd fd_list[1] = {0};
fd_list[0].fd = fd;
fd_list[0].events = POLLIN | POLLPRI;
while (!do_exit) {
int err = poll(fd_list, 1, 100);
if (err == -1) {
if (errno == EINTR) {
continue;
}
return;
} else if (err == 0) {
LOGE("poll timed out");
continue;
}
if ((fd_list[0].revents & (POLLIN | POLLPRI)) == 0) {
LOGE("no poll events set");
continue;
}
// Read all events
struct gpioevent_data evdata[16];
err = read(fd, evdata, sizeof(evdata));
if (err < 0 || err % sizeof(*evdata) != 0) {
LOGE("error reading event data %d", err);
continue;
}
int num_events = err / sizeof(*evdata);
uint64_t offset = nanos_since_epoch() - nanos_since_boot();
uint64_t ts = evdata[num_events - 1].timestamp - offset;
for (auto &[sensor, msg_name] : sensors) {
if (!sensor->has_interrupt_enabled()) {
continue;
}
MessageBuilder msg;
if (!sensor->get_event(msg, ts)) {
continue;
}
if (!sensor->is_data_valid(ts)) {
continue;
}
pm.send(msg_name.c_str(), msg);
}
}
}
void polling_loop(Sensor *sensor, std::string msg_name) {
PubMaster pm({msg_name.c_str()});
RateKeeper rk(msg_name, services.at(msg_name).frequency);
while (!do_exit) {
MessageBuilder msg;
if (sensor->get_event(msg) && sensor->is_data_valid(nanos_since_boot())) {
pm.send(msg_name.c_str(), msg);
}
rk.keepTime();
}
}
int sensor_loop(I2CBus *i2c_bus_imu) {
// Sensor init
std::vector<std::tuple<Sensor *, std::string>> sensors_init = {
{new BMX055_Accel(i2c_bus_imu), "accelerometer2"},
{new BMX055_Gyro(i2c_bus_imu), "gyroscope2"},
{new BMX055_Magn(i2c_bus_imu), "magnetometer"},
{new BMX055_Temp(i2c_bus_imu), "temperatureSensor2"},
{new LSM6DS3_Accel(i2c_bus_imu, GPIO_LSM_INT), "accelerometer"},
{new LSM6DS3_Gyro(i2c_bus_imu, GPIO_LSM_INT, true), "gyroscope"},
{new LSM6DS3_Temp(i2c_bus_imu), "temperatureSensor"},
{new MMC5603NJ_Magn(i2c_bus_imu), "magnetometer"},
};
// Initialize sensors
std::vector<std::thread> threads;
for (auto &[sensor, msg_name] : sensors_init) {
int err = sensor->init();
if (err < 0) {
continue;
}
if (!sensor->has_interrupt_enabled()) {
threads.emplace_back(polling_loop, sensor, msg_name);
}
}
// increase interrupt quality by pinning interrupt and process to core 1
setpriority(PRIO_PROCESS, 0, -18);
util::set_core_affinity({1});
// TODO: get the IRQ number from gpiochip
std::string irq_path = "/proc/irq/336/smp_affinity_list";
if (!util::file_exists(irq_path)) {
irq_path = "/proc/irq/335/smp_affinity_list";
}
std::system(util::string_format("sudo su -c 'echo 1 > %s'", irq_path.c_str()).c_str());
// thread for reading events via interrupts
threads.emplace_back(&interrupt_loop, std::ref(sensors_init));
// wait for all threads to finish
for (auto &t : threads) {
t.join();
}
for (auto &[sensor, msg_name] : sensors_init) {
sensor->shutdown();
delete sensor;
}
return 0;
}
int main(int argc, char *argv[]) {
try {
auto i2c_bus_imu = std::make_unique<I2CBus>(I2C_BUS_IMU);
return sensor_loop(i2c_bus_imu.get());
} catch (std::exception &e) {
LOGE("I2CBus init failed");
return -1;
}
}

20
system/ubloxd/SConscript
View File

@@ -1,20 +0,0 @@
Import('env', 'common', 'cereal', 'messaging')
loc_libs = [cereal, messaging, 'zmq', common, 'capnp', 'kj', 'kaitai', 'pthread']
if GetOption('kaitai'):
generated = Dir('generated').srcnode().abspath
cmd = f"kaitai-struct-compiler --target cpp_stl --outdir {generated} $SOURCES"
env.Command(['generated/ubx.cpp', 'generated/ubx.h'], 'ubx.ksy', cmd)
env.Command(['generated/gps.cpp', 'generated/gps.h'], 'gps.ksy', cmd)
glonass = env.Command(['generated/glonass.cpp', 'generated/glonass.h'], 'glonass.ksy', cmd)
# kaitai issue: https://github.com/kaitai-io/kaitai_struct/issues/910
patch = env.Command(None, 'glonass_fix.patch', 'git apply $SOURCES')
env.Depends(patch, glonass)
glonass_obj = env.Object('generated/glonass.cpp')
env.Program("ubloxd", ["ubloxd.cc", "ublox_msg.cc", "generated/ubx.cpp", "generated/gps.cpp", glonass_obj], LIBS=loc_libs)
if GetOption('extras'):
env.Program("tests/test_glonass_runner", ['tests/test_glonass_runner.cc', 'tests/test_glonass_kaitai.cc', glonass_obj], LIBS=[loc_libs])

375
system/ubloxd/generated/glonass.h
View File

@@ -1,375 +0,0 @@
#ifndef GLONASS_H_
#define GLONASS_H_
// This is a generated file! Please edit source .ksy file and use kaitai-struct-compiler to rebuild
#include "kaitai/kaitaistruct.h"
#include <stdint.h>
#if KAITAI_STRUCT_VERSION < 9000L
#error "Incompatible Kaitai Struct C++/STL API: version 0.9 or later is required"
#endif
class glonass_t : public kaitai::kstruct {
public:
class string_4_t;
class string_non_immediate_t;
class string_5_t;
class string_1_t;
class string_2_t;
class string_3_t;
glonass_t(kaitai::kstream* p__io, kaitai::kstruct* p__parent = 0, glonass_t* p__root = 0);
private:
void _read();
void _clean_up();
public:
~glonass_t();
class string_4_t : public kaitai::kstruct {
public:
string_4_t(kaitai::kstream* p__io, glonass_t* p__parent = 0, glonass_t* p__root = 0);
private:
void _read();
void _clean_up();
public:
~string_4_t();
private:
bool f_tau_n;
int32_t m_tau_n;
public:
int32_t tau_n();
private:
bool f_delta_tau_n;
int32_t m_delta_tau_n;
public:
int32_t delta_tau_n();
private:
bool m_tau_n_sign;
uint64_t m_tau_n_value;
bool m_delta_tau_n_sign;
uint64_t m_delta_tau_n_value;
uint64_t m_e_n;
uint64_t m_not_used_1;
bool m_p4;
uint64_t m_f_t;
uint64_t m_not_used_2;
uint64_t m_n_t;
uint64_t m_n;
uint64_t m_m;
glonass_t* m__root;
glonass_t* m__parent;
public:
bool tau_n_sign() const { return m_tau_n_sign; }
uint64_t tau_n_value() const { return m_tau_n_value; }
bool delta_tau_n_sign() const { return m_delta_tau_n_sign; }
uint64_t delta_tau_n_value() const { return m_delta_tau_n_value; }
uint64_t e_n() const { return m_e_n; }
uint64_t not_used_1() const { return m_not_used_1; }
bool p4() const { return m_p4; }
uint64_t f_t() const { return m_f_t; }
uint64_t not_used_2() const { return m_not_used_2; }
uint64_t n_t() const { return m_n_t; }
uint64_t n() const { return m_n; }
uint64_t m() const { return m_m; }
glonass_t* _root() const { return m__root; }
glonass_t* _parent() const { return m__parent; }
};
class string_non_immediate_t : public kaitai::kstruct {
public:
string_non_immediate_t(kaitai::kstream* p__io, glonass_t* p__parent = 0, glonass_t* p__root = 0);
private:
void _read();
void _clean_up();
public:
~string_non_immediate_t();
private:
uint64_t m_data_1;
uint64_t m_data_2;
glonass_t* m__root;
glonass_t* m__parent;
public:
uint64_t data_1() const { return m_data_1; }
uint64_t data_2() const { return m_data_2; }
glonass_t* _root() const { return m__root; }
glonass_t* _parent() const { return m__parent; }
};
class string_5_t : public kaitai::kstruct {
public:
string_5_t(kaitai::kstream* p__io, glonass_t* p__parent = 0, glonass_t* p__root = 0);
private:
void _read();
void _clean_up();
public:
~string_5_t();
private:
uint64_t m_n_a;
uint64_t m_tau_c;
bool m_not_used;
uint64_t m_n_4;
uint64_t m_tau_gps;
bool m_l_n;
glonass_t* m__root;
glonass_t* m__parent;
public:
uint64_t n_a() const { return m_n_a; }
uint64_t tau_c() const { return m_tau_c; }
bool not_used() const { return m_not_used; }
uint64_t n_4() const { return m_n_4; }
uint64_t tau_gps() const { return m_tau_gps; }
bool l_n() const { return m_l_n; }
glonass_t* _root() const { return m__root; }
glonass_t* _parent() const { return m__parent; }
};
class string_1_t : public kaitai::kstruct {
public:
string_1_t(kaitai::kstream* p__io, glonass_t* p__parent = 0, glonass_t* p__root = 0);
private:
void _read();
void _clean_up();
public:
~string_1_t();
private:
bool f_x_vel;
int32_t m_x_vel;
public:
int32_t x_vel();
private:
bool f_x_accel;
int32_t m_x_accel;
public:
int32_t x_accel();
private:
bool f_x;
int32_t m_x;
public:
int32_t x();
private:
uint64_t m_not_used;
uint64_t m_p1;
uint64_t m_t_k;
bool m_x_vel_sign;
uint64_t m_x_vel_value;
bool m_x_accel_sign;
uint64_t m_x_accel_value;
bool m_x_sign;
uint64_t m_x_value;
glonass_t* m__root;
glonass_t* m__parent;
public:
uint64_t not_used() const { return m_not_used; }
uint64_t p1() const { return m_p1; }
uint64_t t_k() const { return m_t_k; }
bool x_vel_sign() const { return m_x_vel_sign; }
uint64_t x_vel_value() const { return m_x_vel_value; }
bool x_accel_sign() const { return m_x_accel_sign; }
uint64_t x_accel_value() const { return m_x_accel_value; }
bool x_sign() const { return m_x_sign; }
uint64_t x_value() const { return m_x_value; }
glonass_t* _root() const { return m__root; }
glonass_t* _parent() const { return m__parent; }
};
class string_2_t : public kaitai::kstruct {
public:
string_2_t(kaitai::kstream* p__io, glonass_t* p__parent = 0, glonass_t* p__root = 0);
private:
void _read();
void _clean_up();
public:
~string_2_t();
private:
bool f_y_vel;
int32_t m_y_vel;
public:
int32_t y_vel();
private:
bool f_y_accel;
int32_t m_y_accel;
public:
int32_t y_accel();
private:
bool f_y;
int32_t m_y;
public:
int32_t y();
private:
uint64_t m_b_n;
bool m_p2;
uint64_t m_t_b;
uint64_t m_not_used;
bool m_y_vel_sign;
uint64_t m_y_vel_value;
bool m_y_accel_sign;
uint64_t m_y_accel_value;
bool m_y_sign;
uint64_t m_y_value;
glonass_t* m__root;
glonass_t* m__parent;
public:
uint64_t b_n() const { return m_b_n; }
bool p2() const { return m_p2; }
uint64_t t_b() const { return m_t_b; }
uint64_t not_used() const { return m_not_used; }
bool y_vel_sign() const { return m_y_vel_sign; }
uint64_t y_vel_value() const { return m_y_vel_value; }
bool y_accel_sign() const { return m_y_accel_sign; }
uint64_t y_accel_value() const { return m_y_accel_value; }
bool y_sign() const { return m_y_sign; }
uint64_t y_value() const { return m_y_value; }
glonass_t* _root() const { return m__root; }
glonass_t* _parent() const { return m__parent; }
};
class string_3_t : public kaitai::kstruct {
public:
string_3_t(kaitai::kstream* p__io, glonass_t* p__parent = 0, glonass_t* p__root = 0);
private:
void _read();
void _clean_up();
public:
~string_3_t();
private:
bool f_gamma_n;
int32_t m_gamma_n;
public:
int32_t gamma_n();
private:
bool f_z_vel;
int32_t m_z_vel;
public:
int32_t z_vel();
private:
bool f_z_accel;
int32_t m_z_accel;
public:
int32_t z_accel();
private:
bool f_z;
int32_t m_z;
public:
int32_t z();
private:
bool m_p3;
bool m_gamma_n_sign;
uint64_t m_gamma_n_value;
bool m_not_used;
uint64_t m_p;
bool m_l_n;
bool m_z_vel_sign;
uint64_t m_z_vel_value;
bool m_z_accel_sign;
uint64_t m_z_accel_value;
bool m_z_sign;
uint64_t m_z_value;
glonass_t* m__root;
glonass_t* m__parent;
public:
bool p3() const { return m_p3; }
bool gamma_n_sign() const { return m_gamma_n_sign; }
uint64_t gamma_n_value() const { return m_gamma_n_value; }
bool not_used() const { return m_not_used; }
uint64_t p() const { return m_p; }
bool l_n() const { return m_l_n; }
bool z_vel_sign() const { return m_z_vel_sign; }
uint64_t z_vel_value() const { return m_z_vel_value; }
bool z_accel_sign() const { return m_z_accel_sign; }
uint64_t z_accel_value() const { return m_z_accel_value; }
bool z_sign() const { return m_z_sign; }
uint64_t z_value() const { return m_z_value; }
glonass_t* _root() const { return m__root; }
glonass_t* _parent() const { return m__parent; }
};
private:
bool m_idle_chip;
uint64_t m_string_number;
kaitai::kstruct* m_data;
uint64_t m_hamming_code;
uint64_t m_pad_1;
uint64_t m_superframe_number;
uint64_t m_pad_2;
uint64_t m_frame_number;
glonass_t* m__root;
kaitai::kstruct* m__parent;
public:
bool idle_chip() const { return m_idle_chip; }
uint64_t string_number() const { return m_string_number; }
kaitai::kstruct* data() const { return m_data; }
uint64_t hamming_code() const { return m_hamming_code; }
uint64_t pad_1() const { return m_pad_1; }
uint64_t superframe_number() const { return m_superframe_number; }
uint64_t pad_2() const { return m_pad_2; }
uint64_t frame_number() const { return m_frame_number; }
glonass_t* _root() const { return m__root; }
kaitai::kstruct* _parent() const { return m__parent; }
};
#endif // GLONASS_H_

359
system/ubloxd/generated/gps.h
View File

@@ -1,359 +0,0 @@
#ifndef GPS_H_
#define GPS_H_
// This is a generated file! Please edit source .ksy file and use kaitai-struct-compiler to rebuild
#include "kaitai/kaitaistruct.h"
#include <stdint.h>
#if KAITAI_STRUCT_VERSION < 9000L
#error "Incompatible Kaitai Struct C++/STL API: version 0.9 or later is required"
#endif
class gps_t : public kaitai::kstruct {
public:
class subframe_1_t;
class subframe_3_t;
class subframe_4_t;
class how_t;
class tlm_t;
class subframe_2_t;
gps_t(kaitai::kstream* p__io, kaitai::kstruct* p__parent = 0, gps_t* p__root = 0);
private:
void _read();
void _clean_up();
public:
~gps_t();
class subframe_1_t : public kaitai::kstruct {
public:
subframe_1_t(kaitai::kstream* p__io, gps_t* p__parent = 0, gps_t* p__root = 0);
private:
void _read();
void _clean_up();
public:
~subframe_1_t();
private:
bool f_af_0;
int32_t m_af_0;
public:
int32_t af_0();
private:
uint64_t m_week_no;
uint64_t m_code;
uint64_t m_sv_accuracy;
uint64_t m_sv_health;
uint64_t m_iodc_msb;
bool m_l2_p_data_flag;
uint64_t m_reserved1;
uint64_t m_reserved2;
uint64_t m_reserved3;
uint64_t m_reserved4;
int8_t m_t_gd;
uint8_t m_iodc_lsb;
uint16_t m_t_oc;
int8_t m_af_2;
int16_t m_af_1;
bool m_af_0_sign;
uint64_t m_af_0_value;
uint64_t m_reserved5;
gps_t* m__root;
gps_t* m__parent;
public:
uint64_t week_no() const { return m_week_no; }
uint64_t code() const { return m_code; }
uint64_t sv_accuracy() const { return m_sv_accuracy; }
uint64_t sv_health() const { return m_sv_health; }
uint64_t iodc_msb() const { return m_iodc_msb; }
bool l2_p_data_flag() const { return m_l2_p_data_flag; }
uint64_t reserved1() const { return m_reserved1; }
uint64_t reserved2() const { return m_reserved2; }
uint64_t reserved3() const { return m_reserved3; }
uint64_t reserved4() const { return m_reserved4; }
int8_t t_gd() const { return m_t_gd; }
uint8_t iodc_lsb() const { return m_iodc_lsb; }
uint16_t t_oc() const { return m_t_oc; }
int8_t af_2() const { return m_af_2; }
int16_t af_1() const { return m_af_1; }
bool af_0_sign() const { return m_af_0_sign; }
uint64_t af_0_value() const { return m_af_0_value; }
uint64_t reserved5() const { return m_reserved5; }
gps_t* _root() const { return m__root; }
gps_t* _parent() const { return m__parent; }
};
class subframe_3_t : public kaitai::kstruct {
public:
subframe_3_t(kaitai::kstream* p__io, gps_t* p__parent = 0, gps_t* p__root = 0);
private:
void _read();
void _clean_up();
public:
~subframe_3_t();
private:
bool f_omega_dot;
int32_t m_omega_dot;
public:
int32_t omega_dot();
private:
bool f_idot;
int32_t m_idot;
public:
int32_t idot();
private:
int16_t m_c_ic;
int32_t m_omega_0;
int16_t m_c_is;
int32_t m_i_0;
int16_t m_c_rc;
int32_t m_omega;
bool m_omega_dot_sign;
uint64_t m_omega_dot_value;
uint8_t m_iode;
bool m_idot_sign;
uint64_t m_idot_value;
uint64_t m_reserved;
gps_t* m__root;
gps_t* m__parent;
public:
int16_t c_ic() const { return m_c_ic; }
int32_t omega_0() const { return m_omega_0; }
int16_t c_is() const { return m_c_is; }
int32_t i_0() const { return m_i_0; }
int16_t c_rc() const { return m_c_rc; }
int32_t omega() const { return m_omega; }
bool omega_dot_sign() const { return m_omega_dot_sign; }
uint64_t omega_dot_value() const { return m_omega_dot_value; }
uint8_t iode() const { return m_iode; }
bool idot_sign() const { return m_idot_sign; }
uint64_t idot_value() const { return m_idot_value; }
uint64_t reserved() const { return m_reserved; }
gps_t* _root() const { return m__root; }
gps_t* _parent() const { return m__parent; }
};
class subframe_4_t : public kaitai::kstruct {
public:
class ionosphere_data_t;
subframe_4_t(kaitai::kstream* p__io, gps_t* p__parent = 0, gps_t* p__root = 0);
private:
void _read();
void _clean_up();
public:
~subframe_4_t();
class ionosphere_data_t : public kaitai::kstruct {
public:
ionosphere_data_t(kaitai::kstream* p__io, gps_t::subframe_4_t* p__parent = 0, gps_t* p__root = 0);
private:
void _read();
void _clean_up();
public:
~ionosphere_data_t();
private:
int8_t m_a0;
int8_t m_a1;
int8_t m_a2;
int8_t m_a3;
int8_t m_b0;
int8_t m_b1;
int8_t m_b2;
int8_t m_b3;
gps_t* m__root;
gps_t::subframe_4_t* m__parent;
public:
int8_t a0() const { return m_a0; }
int8_t a1() const { return m_a1; }
int8_t a2() const { return m_a2; }
int8_t a3() const { return m_a3; }
int8_t b0() const { return m_b0; }
int8_t b1() const { return m_b1; }
int8_t b2() const { return m_b2; }
int8_t b3() const { return m_b3; }
gps_t* _root() const { return m__root; }
gps_t::subframe_4_t* _parent() const { return m__parent; }
};
private:
uint64_t m_data_id;
uint64_t m_page_id;
ionosphere_data_t* m_body;
bool n_body;
public:
bool _is_null_body() { body(); return n_body; };
private:
gps_t* m__root;
gps_t* m__parent;
public:
uint64_t data_id() const { return m_data_id; }
uint64_t page_id() const { return m_page_id; }
ionosphere_data_t* body() const { return m_body; }
gps_t* _root() const { return m__root; }
gps_t* _parent() const { return m__parent; }
};
class how_t : public kaitai::kstruct {
public:
how_t(kaitai::kstream* p__io, gps_t* p__parent = 0, gps_t* p__root = 0);
private:
void _read();
void _clean_up();
public:
~how_t();
private:
uint64_t m_tow_count;
bool m_alert;
bool m_anti_spoof;
uint64_t m_subframe_id;
uint64_t m_reserved;
gps_t* m__root;
gps_t* m__parent;
public:
uint64_t tow_count() const { return m_tow_count; }
bool alert() const { return m_alert; }
bool anti_spoof() const { return m_anti_spoof; }
uint64_t subframe_id() const { return m_subframe_id; }
uint64_t reserved() const { return m_reserved; }
gps_t* _root() const { return m__root; }
gps_t* _parent() const { return m__parent; }
};
class tlm_t : public kaitai::kstruct {
public:
tlm_t(kaitai::kstream* p__io, gps_t* p__parent = 0, gps_t* p__root = 0);
private:
void _read();
void _clean_up();
public:
~tlm_t();
private:
std::string m_preamble;
uint64_t m_tlm;
bool m_integrity_status;
bool m_reserved;
gps_t* m__root;
gps_t* m__parent;
public:
std::string preamble() const { return m_preamble; }
uint64_t tlm() const { return m_tlm; }
bool integrity_status() const { return m_integrity_status; }
bool reserved() const { return m_reserved; }
gps_t* _root() const { return m__root; }
gps_t* _parent() const { return m__parent; }
};
class subframe_2_t : public kaitai::kstruct {
public:
subframe_2_t(kaitai::kstream* p__io, gps_t* p__parent = 0, gps_t* p__root = 0);
private:
void _read();
void _clean_up();
public:
~subframe_2_t();
private:
uint8_t m_iode;
int16_t m_c_rs;
int16_t m_delta_n;
int32_t m_m_0;
int16_t m_c_uc;
int32_t m_e;
int16_t m_c_us;
uint32_t m_sqrt_a;
uint16_t m_t_oe;
bool m_fit_interval_flag;
uint64_t m_aoda;
uint64_t m_reserved;
gps_t* m__root;
gps_t* m__parent;
public:
uint8_t iode() const { return m_iode; }
int16_t c_rs() const { return m_c_rs; }
int16_t delta_n() const { return m_delta_n; }
int32_t m_0() const { return m_m_0; }
int16_t c_uc() const { return m_c_uc; }
int32_t e() const { return m_e; }
int16_t c_us() const { return m_c_us; }
uint32_t sqrt_a() const { return m_sqrt_a; }
uint16_t t_oe() const { return m_t_oe; }
bool fit_interval_flag() const { return m_fit_interval_flag; }
uint64_t aoda() const { return m_aoda; }
uint64_t reserved() const { return m_reserved; }
gps_t* _root() const { return m__root; }
gps_t* _parent() const { return m__parent; }
};
private:
tlm_t* m_tlm;
how_t* m_how;
kaitai::kstruct* m_body;
bool n_body;
public:
bool _is_null_body() { body(); return n_body; };
private:
gps_t* m__root;
kaitai::kstruct* m__parent;
public:
tlm_t* tlm() const { return m_tlm; }
how_t* how() const { return m_how; }
kaitai::kstruct* body() const { return m_body; }
gps_t* _root() const { return m__root; }
kaitai::kstruct* _parent() const { return m__parent; }
};
#endif // GPS_H_

484
system/ubloxd/generated/ubx.h
View File

@@ -1,484 +0,0 @@
#ifndef UBX_H_
#define UBX_H_
// This is a generated file! Please edit source .ksy file and use kaitai-struct-compiler to rebuild
#include "kaitai/kaitaistruct.h"
#include <stdint.h>
#include <vector>
#if KAITAI_STRUCT_VERSION < 9000L
#error "Incompatible Kaitai Struct C++/STL API: version 0.9 or later is required"
#endif
class ubx_t : public kaitai::kstruct {
public:
class rxm_rawx_t;
class rxm_sfrbx_t;
class nav_sat_t;
class nav_pvt_t;
class mon_hw2_t;
class mon_hw_t;
enum gnss_type_t {
GNSS_TYPE_GPS = 0,
GNSS_TYPE_SBAS = 1,
GNSS_TYPE_GALILEO = 2,
GNSS_TYPE_BEIDOU = 3,
GNSS_TYPE_IMES = 4,
GNSS_TYPE_QZSS = 5,
GNSS_TYPE_GLONASS = 6
};
ubx_t(kaitai::kstream* p__io, kaitai::kstruct* p__parent = 0, ubx_t* p__root = 0);
private:
void _read();
void _clean_up();
public:
~ubx_t();
class rxm_rawx_t : public kaitai::kstruct {
public:
class measurement_t;
rxm_rawx_t(kaitai::kstream* p__io, ubx_t* p__parent = 0, ubx_t* p__root = 0);
private:
void _read();
void _clean_up();
public:
~rxm_rawx_t();
class measurement_t : public kaitai::kstruct {
public:
measurement_t(kaitai::kstream* p__io, ubx_t::rxm_rawx_t* p__parent = 0, ubx_t* p__root = 0);
private:
void _read();
void _clean_up();
public:
~measurement_t();
private:
double m_pr_mes;
double m_cp_mes;
float m_do_mes;
gnss_type_t m_gnss_id;
uint8_t m_sv_id;
std::string m_reserved2;
uint8_t m_freq_id;
uint16_t m_lock_time;
uint8_t m_cno;
uint8_t m_pr_stdev;
uint8_t m_cp_stdev;
uint8_t m_do_stdev;
uint8_t m_trk_stat;
std::string m_reserved3;
ubx_t* m__root;
ubx_t::rxm_rawx_t* m__parent;
public:
double pr_mes() const { return m_pr_mes; }
double cp_mes() const { return m_cp_mes; }
float do_mes() const { return m_do_mes; }
gnss_type_t gnss_id() const { return m_gnss_id; }
uint8_t sv_id() const { return m_sv_id; }
std::string reserved2() const { return m_reserved2; }
uint8_t freq_id() const { return m_freq_id; }
uint16_t lock_time() const { return m_lock_time; }
uint8_t cno() const { return m_cno; }
uint8_t pr_stdev() const { return m_pr_stdev; }
uint8_t cp_stdev() const { return m_cp_stdev; }
uint8_t do_stdev() const { return m_do_stdev; }
uint8_t trk_stat() const { return m_trk_stat; }
std::string reserved3() const { return m_reserved3; }
ubx_t* _root() const { return m__root; }
ubx_t::rxm_rawx_t* _parent() const { return m__parent; }
};
private:
double m_rcv_tow;
uint16_t m_week;
int8_t m_leap_s;
uint8_t m_num_meas;
uint8_t m_rec_stat;
std::string m_reserved1;
std::vector<measurement_t*>* m_meas;
ubx_t* m__root;
ubx_t* m__parent;
std::vector<std::string>* m__raw_meas;
std::vector<kaitai::kstream*>* m__io__raw_meas;
public:
double rcv_tow() const { return m_rcv_tow; }
uint16_t week() const { return m_week; }
int8_t leap_s() const { return m_leap_s; }
uint8_t num_meas() const { return m_num_meas; }
uint8_t rec_stat() const { return m_rec_stat; }
std::string reserved1() const { return m_reserved1; }
std::vector<measurement_t*>* meas() const { return m_meas; }
ubx_t* _root() const { return m__root; }
ubx_t* _parent() const { return m__parent; }
std::vector<std::string>* _raw_meas() const { return m__raw_meas; }
std::vector<kaitai::kstream*>* _io__raw_meas() const { return m__io__raw_meas; }
};
class rxm_sfrbx_t : public kaitai::kstruct {
public:
rxm_sfrbx_t(kaitai::kstream* p__io, ubx_t* p__parent = 0, ubx_t* p__root = 0);
private:
void _read();
void _clean_up();
public:
~rxm_sfrbx_t();
private:
gnss_type_t m_gnss_id;
uint8_t m_sv_id;
std::string m_reserved1;
uint8_t m_freq_id;
uint8_t m_num_words;
std::string m_reserved2;
uint8_t m_version;
std::string m_reserved3;
std::vector<uint32_t>* m_body;
ubx_t* m__root;
ubx_t* m__parent;
public:
gnss_type_t gnss_id() const { return m_gnss_id; }
uint8_t sv_id() const { return m_sv_id; }
std::string reserved1() const { return m_reserved1; }
uint8_t freq_id() const { return m_freq_id; }
uint8_t num_words() const { return m_num_words; }
std::string reserved2() const { return m_reserved2; }
uint8_t version() const { return m_version; }
std::string reserved3() const { return m_reserved3; }
std::vector<uint32_t>* body() const { return m_body; }
ubx_t* _root() const { return m__root; }
ubx_t* _parent() const { return m__parent; }
};
class nav_sat_t : public kaitai::kstruct {
public:
class nav_t;
nav_sat_t(kaitai::kstream* p__io, ubx_t* p__parent = 0, ubx_t* p__root = 0);
private:
void _read();
void _clean_up();
public:
~nav_sat_t();
class nav_t : public kaitai::kstruct {
public:
nav_t(kaitai::kstream* p__io, ubx_t::nav_sat_t* p__parent = 0, ubx_t* p__root = 0);
private:
void _read();
void _clean_up();
public:
~nav_t();
private:
gnss_type_t m_gnss_id;
uint8_t m_sv_id;
uint8_t m_cno;
int8_t m_elev;
int16_t m_azim;
int16_t m_pr_res;
uint32_t m_flags;
ubx_t* m__root;
ubx_t::nav_sat_t* m__parent;
public:
gnss_type_t gnss_id() const { return m_gnss_id; }
uint8_t sv_id() const { return m_sv_id; }
uint8_t cno() const { return m_cno; }
int8_t elev() const { return m_elev; }
int16_t azim() const { return m_azim; }
int16_t pr_res() const { return m_pr_res; }
uint32_t flags() const { return m_flags; }
ubx_t* _root() const { return m__root; }
ubx_t::nav_sat_t* _parent() const { return m__parent; }
};
private:
uint32_t m_itow;
uint8_t m_version;
uint8_t m_num_svs;
std::string m_reserved;
std::vector<nav_t*>* m_svs;
ubx_t* m__root;
ubx_t* m__parent;
std::vector<std::string>* m__raw_svs;
std::vector<kaitai::kstream*>* m__io__raw_svs;
public:
uint32_t itow() const { return m_itow; }
uint8_t version() const { return m_version; }
uint8_t num_svs() const { return m_num_svs; }
std::string reserved() const { return m_reserved; }
std::vector<nav_t*>* svs() const { return m_svs; }
ubx_t* _root() const { return m__root; }
ubx_t* _parent() const { return m__parent; }
std::vector<std::string>* _raw_svs() const { return m__raw_svs; }
std::vector<kaitai::kstream*>* _io__raw_svs() const { return m__io__raw_svs; }
};
class nav_pvt_t : public kaitai::kstruct {
public:
nav_pvt_t(kaitai::kstream* p__io, ubx_t* p__parent = 0, ubx_t* p__root = 0);
private:
void _read();
void _clean_up();
public:
~nav_pvt_t();
private:
uint32_t m_i_tow;
uint16_t m_year;
uint8_t m_month;
uint8_t m_day;
uint8_t m_hour;
uint8_t m_min;
uint8_t m_sec;
uint8_t m_valid;
uint32_t m_t_acc;
int32_t m_nano;
uint8_t m_fix_type;
uint8_t m_flags;
uint8_t m_flags2;
uint8_t m_num_sv;
int32_t m_lon;
int32_t m_lat;
int32_t m_height;
int32_t m_h_msl;
uint32_t m_h_acc;
uint32_t m_v_acc;
int32_t m_vel_n;
int32_t m_vel_e;
int32_t m_vel_d;
int32_t m_g_speed;
int32_t m_head_mot;
int32_t m_s_acc;
uint32_t m_head_acc;
uint16_t m_p_dop;
uint8_t m_flags3;
std::string m_reserved1;
int32_t m_head_veh;
int16_t m_mag_dec;
uint16_t m_mag_acc;
ubx_t* m__root;
ubx_t* m__parent;
public:
uint32_t i_tow() const { return m_i_tow; }
uint16_t year() const { return m_year; }
uint8_t month() const { return m_month; }
uint8_t day() const { return m_day; }
uint8_t hour() const { return m_hour; }
uint8_t min() const { return m_min; }
uint8_t sec() const { return m_sec; }
uint8_t valid() const { return m_valid; }
uint32_t t_acc() const { return m_t_acc; }
int32_t nano() const { return m_nano; }
uint8_t fix_type() const { return m_fix_type; }
uint8_t flags() const { return m_flags; }
uint8_t flags2() const { return m_flags2; }
uint8_t num_sv() const { return m_num_sv; }
int32_t lon() const { return m_lon; }
int32_t lat() const { return m_lat; }
int32_t height() const { return m_height; }
int32_t h_msl() const { return m_h_msl; }
uint32_t h_acc() const { return m_h_acc; }
uint32_t v_acc() const { return m_v_acc; }
int32_t vel_n() const { return m_vel_n; }
int32_t vel_e() const { return m_vel_e; }
int32_t vel_d() const { return m_vel_d; }
int32_t g_speed() const { return m_g_speed; }
int32_t head_mot() const { return m_head_mot; }
int32_t s_acc() const { return m_s_acc; }
uint32_t head_acc() const { return m_head_acc; }
uint16_t p_dop() const { return m_p_dop; }
uint8_t flags3() const { return m_flags3; }
std::string reserved1() const { return m_reserved1; }
int32_t head_veh() const { return m_head_veh; }
int16_t mag_dec() const { return m_mag_dec; }
uint16_t mag_acc() const { return m_mag_acc; }
ubx_t* _root() const { return m__root; }
ubx_t* _parent() const { return m__parent; }
};
class mon_hw2_t : public kaitai::kstruct {
public:
enum config_source_t {
CONFIG_SOURCE_FLASH = 102,
CONFIG_SOURCE_OTP = 111,
CONFIG_SOURCE_CONFIG_PINS = 112,
CONFIG_SOURCE_ROM = 113
};
mon_hw2_t(kaitai::kstream* p__io, ubx_t* p__parent = 0, ubx_t* p__root = 0);
private:
void _read();
void _clean_up();
public:
~mon_hw2_t();
private:
int8_t m_ofs_i;
uint8_t m_mag_i;
int8_t m_ofs_q;
uint8_t m_mag_q;
config_source_t m_cfg_source;
std::string m_reserved1;
uint32_t m_low_lev_cfg;
std::string m_reserved2;
uint32_t m_post_status;
std::string m_reserved3;
ubx_t* m__root;
ubx_t* m__parent;
public:
int8_t ofs_i() const { return m_ofs_i; }
uint8_t mag_i() const { return m_mag_i; }
int8_t ofs_q() const { return m_ofs_q; }
uint8_t mag_q() const { return m_mag_q; }
config_source_t cfg_source() const { return m_cfg_source; }
std::string reserved1() const { return m_reserved1; }
uint32_t low_lev_cfg() const { return m_low_lev_cfg; }
std::string reserved2() const { return m_reserved2; }
uint32_t post_status() const { return m_post_status; }
std::string reserved3() const { return m_reserved3; }
ubx_t* _root() const { return m__root; }
ubx_t* _parent() const { return m__parent; }
};
class mon_hw_t : public kaitai::kstruct {
public:
enum antenna_status_t {
ANTENNA_STATUS_INIT = 0,
ANTENNA_STATUS_DONTKNOW = 1,
ANTENNA_STATUS_OK = 2,
ANTENNA_STATUS_SHORT = 3,
ANTENNA_STATUS_OPEN = 4
};
enum antenna_power_t {
ANTENNA_POWER_FALSE = 0,
ANTENNA_POWER_TRUE = 1,
ANTENNA_POWER_DONTKNOW = 2
};
mon_hw_t(kaitai::kstream* p__io, ubx_t* p__parent = 0, ubx_t* p__root = 0);
private:
void _read();
void _clean_up();
public:
~mon_hw_t();
private:
uint32_t m_pin_sel;
uint32_t m_pin_bank;
uint32_t m_pin_dir;
uint32_t m_pin_val;
uint16_t m_noise_per_ms;
uint16_t m_agc_cnt;
antenna_status_t m_a_status;
antenna_power_t m_a_power;
uint8_t m_flags;
std::string m_reserved1;
uint32_t m_used_mask;
std::string m_vp;
uint8_t m_jam_ind;
std::string m_reserved2;
uint32_t m_pin_irq;
uint32_t m_pull_h;
uint32_t m_pull_l;
ubx_t* m__root;
ubx_t* m__parent;
public:
uint32_t pin_sel() const { return m_pin_sel; }
uint32_t pin_bank() const { return m_pin_bank; }
uint32_t pin_dir() const { return m_pin_dir; }
uint32_t pin_val() const { return m_pin_val; }
uint16_t noise_per_ms() const { return m_noise_per_ms; }
uint16_t agc_cnt() const { return m_agc_cnt; }
antenna_status_t a_status() const { return m_a_status; }
antenna_power_t a_power() const { return m_a_power; }
uint8_t flags() const { return m_flags; }
std::string reserved1() const { return m_reserved1; }
uint32_t used_mask() const { return m_used_mask; }
std::string vp() const { return m_vp; }
uint8_t jam_ind() const { return m_jam_ind; }
std::string reserved2() const { return m_reserved2; }
uint32_t pin_irq() const { return m_pin_irq; }
uint32_t pull_h() const { return m_pull_h; }
uint32_t pull_l() const { return m_pull_l; }
ubx_t* _root() const { return m__root; }
ubx_t* _parent() const { return m__parent; }
};
private:
bool f_checksum;
uint16_t m_checksum;
public:
uint16_t checksum();
private:
std::string m_magic;
uint16_t m_msg_type;
uint16_t m_length;
kaitai::kstruct* m_body;
bool n_body;
public:
bool _is_null_body() { body(); return n_body; };
private:
ubx_t* m__root;
kaitai::kstruct* m__parent;
public:
std::string magic() const { return m_magic; }
uint16_t msg_type() const { return m_msg_type; }
uint16_t length() const { return m_length; }
kaitai::kstruct* body() const { return m_body; }
ubx_t* _root() const { return m__root; }
kaitai::kstruct* _parent() const { return m__parent; }
};
#endif // UBX_H_

524
system/ubloxd/ublox_msg.cc
View File

@@ -1,524 +0,0 @@
#include "system/ubloxd/ublox_msg.h"
#include <unistd.h>
#include <algorithm>
#include <cassert>
#include <chrono>
#include <cmath>
#include <cstdio>
#include <cstdlib>
#include <ctime>
#include <unordered_map>
#include <utility>
#include "common/swaglog.h"
const double gpsPi = 3.1415926535898;
#define UBLOX_MSG_SIZE(hdr) (*(uint16_t *)&hdr[4])
inline static bool bit_to_bool(uint8_t val, int shifts) {
return (bool)(val & (1 << shifts));
}
inline int UbloxMsgParser::needed_bytes() {
// Msg header incomplete?
if (bytes_in_parse_buf < ublox::UBLOX_HEADER_SIZE)
return ublox::UBLOX_HEADER_SIZE + ublox::UBLOX_CHECKSUM_SIZE - bytes_in_parse_buf;
uint16_t needed = UBLOX_MSG_SIZE(msg_parse_buf) + ublox::UBLOX_HEADER_SIZE + ublox::UBLOX_CHECKSUM_SIZE;
// too much data
if (needed < (uint16_t)bytes_in_parse_buf)
return -1;
return needed - (uint16_t)bytes_in_parse_buf;
}
inline bool UbloxMsgParser::valid_cheksum() {
uint8_t ck_a = 0, ck_b = 0;
for (int i = 2; i < bytes_in_parse_buf - ublox::UBLOX_CHECKSUM_SIZE; i++) {
ck_a = (ck_a + msg_parse_buf[i]) & 0xFF;
ck_b = (ck_b + ck_a) & 0xFF;
}
if (ck_a != msg_parse_buf[bytes_in_parse_buf - 2]) {
LOGD("Checksum a mismatch: %02X, %02X", ck_a, msg_parse_buf[6]);
return false;
}
if (ck_b != msg_parse_buf[bytes_in_parse_buf - 1]) {
LOGD("Checksum b mismatch: %02X, %02X", ck_b, msg_parse_buf[7]);
return false;
}
return true;
}
inline bool UbloxMsgParser::valid() {
return bytes_in_parse_buf >= ublox::UBLOX_HEADER_SIZE + ublox::UBLOX_CHECKSUM_SIZE &&
needed_bytes() == 0 && valid_cheksum();
}
inline bool UbloxMsgParser::valid_so_far() {
if (bytes_in_parse_buf > 0 && msg_parse_buf[0] != ublox::PREAMBLE1) {
return false;
}
if (bytes_in_parse_buf > 1 && msg_parse_buf[1] != ublox::PREAMBLE2) {
return false;
}
if (needed_bytes() == 0 && !valid()) {
return false;
}
return true;
}
bool UbloxMsgParser::add_data(float log_time, const uint8_t *incoming_data, uint32_t incoming_data_len, size_t &bytes_consumed) {
last_log_time = log_time;
int needed = needed_bytes();
if (needed > 0) {
bytes_consumed = std::min((uint32_t)needed, incoming_data_len);
// Add data to buffer
memcpy(msg_parse_buf + bytes_in_parse_buf, incoming_data, bytes_consumed);
bytes_in_parse_buf += bytes_consumed;
} else {
bytes_consumed = incoming_data_len;
}
// Validate msg format, detect invalid header and invalid checksum.
while (!valid_so_far() && bytes_in_parse_buf != 0) {
// Corrupted msg, drop a byte.
bytes_in_parse_buf -= 1;
if (bytes_in_parse_buf > 0)
memmove(&msg_parse_buf[0], &msg_parse_buf[1], bytes_in_parse_buf);
}
// There is redundant data at the end of buffer, reset the buffer.
if (needed_bytes() == -1) {
bytes_in_parse_buf = 0;
}
return valid();
}
std::pair<std::string, kj::Array<capnp::word>> UbloxMsgParser::gen_msg() {
std::string dat = data();
kaitai::kstream stream(dat);
ubx_t ubx_message(&stream);
auto body = ubx_message.body();
switch (ubx_message.msg_type()) {
case 0x0107:
return {"gpsLocationExternal", gen_nav_pvt(static_cast<ubx_t::nav_pvt_t*>(body))};
case 0x0213: // UBX-RXM-SFRB (Broadcast Navigation Data Subframe)
return {"ubloxGnss", gen_rxm_sfrbx(static_cast<ubx_t::rxm_sfrbx_t*>(body))};
case 0x0215: // UBX-RXM-RAW (Multi-GNSS Raw Measurement Data)
return {"ubloxGnss", gen_rxm_rawx(static_cast<ubx_t::rxm_rawx_t*>(body))};
case 0x0a09:
return {"ubloxGnss", gen_mon_hw(static_cast<ubx_t::mon_hw_t*>(body))};
case 0x0a0b:
return {"ubloxGnss", gen_mon_hw2(static_cast<ubx_t::mon_hw2_t*>(body))};
case 0x0135:
return {"ubloxGnss", gen_nav_sat(static_cast<ubx_t::nav_sat_t*>(body))};
default:
LOGE("Unknown message type %x", ubx_message.msg_type());
return {"ubloxGnss", kj::Array<capnp::word>()};
}
}
kj::Array<capnp::word> UbloxMsgParser::gen_nav_pvt(ubx_t::nav_pvt_t *msg) {
MessageBuilder msg_builder;
auto gpsLoc = msg_builder.initEvent().initGpsLocationExternal();
gpsLoc.setSource(cereal::GpsLocationData::SensorSource::UBLOX);
gpsLoc.setFlags(msg->flags());
gpsLoc.setLatitude(msg->lat() * 1e-07);
gpsLoc.setLongitude(msg->lon() * 1e-07);
gpsLoc.setAltitude(msg->height() * 1e-03);
gpsLoc.setSpeed(msg->g_speed() * 1e-03);
gpsLoc.setBearingDeg(msg->head_mot() * 1e-5);
gpsLoc.setAccuracy(msg->h_acc() * 1e-03);
std::tm timeinfo = std::tm();
timeinfo.tm_year = msg->year() - 1900;
timeinfo.tm_mon = msg->month() - 1;
timeinfo.tm_mday = msg->day();
timeinfo.tm_hour = msg->hour();
timeinfo.tm_min = msg->min();
timeinfo.tm_sec = msg->sec();
std::time_t utc_tt = timegm(&timeinfo);
gpsLoc.setUnixTimestampMillis(utc_tt * 1e+03 + msg->nano() * 1e-06);
float f[] = { msg->vel_n() * 1e-03f, msg->vel_e() * 1e-03f, msg->vel_d() * 1e-03f };
gpsLoc.setVNED(f);
gpsLoc.setVerticalAccuracy(msg->v_acc() * 1e-03);
gpsLoc.setSpeedAccuracy(msg->s_acc() * 1e-03);
gpsLoc.setBearingAccuracyDeg(msg->head_acc() * 1e-05);
return capnp::messageToFlatArray(msg_builder);
}
kj::Array<capnp::word> UbloxMsgParser::parse_gps_ephemeris(ubx_t::rxm_sfrbx_t *msg) {
// GPS subframes are packed into 10x 4 bytes, each containing 3 actual bytes
// We will first need to separate the data from the padding and parity
auto body = *msg->body();
assert(body.size() == 10);
std::string subframe_data;
subframe_data.reserve(30);
for (uint32_t word : body) {
word = word >> 6; // TODO: Verify parity
subframe_data.push_back(word >> 16);
subframe_data.push_back(word >> 8);
subframe_data.push_back(word >> 0);
}
// Collect subframes in map and parse when we have all the parts
{
kaitai::kstream stream(subframe_data);
gps_t subframe(&stream);
int subframe_id = subframe.how()->subframe_id();
if (subframe_id > 3 || subframe_id < 1) {
// dont parse almanac subframes
return kj::Array<capnp::word>();
}
gps_subframes[msg->sv_id()][subframe_id] = subframe_data;
}
// publish if subframes 1-3 have been collected
if (gps_subframes[msg->sv_id()].size() == 3) {
MessageBuilder msg_builder;
auto eph = msg_builder.initEvent().initUbloxGnss().initEphemeris();
eph.setSvId(msg->sv_id());
int iode_s2 = 0;
int iode_s3 = 0;
int iodc_lsb = 0;
int week;
// Subframe 1
{
kaitai::kstream stream(gps_subframes[msg->sv_id()][1]);
gps_t subframe(&stream);
gps_t::subframe_1_t* subframe_1 = static_cast<gps_t::subframe_1_t*>(subframe.body());
// Each message is incremented to be greater or equal than week 1877 (2015-12-27).
// To skip this use the current_time argument
week = subframe_1->week_no();
week += 1024;
if (week < 1877) {
week += 1024;
}
//eph.setGpsWeek(subframe_1->week_no());
eph.setTgd(subframe_1->t_gd() * pow(2, -31));
eph.setToc(subframe_1->t_oc() * pow(2, 4));
eph.setAf2(subframe_1->af_2() * pow(2, -55));
eph.setAf1(subframe_1->af_1() * pow(2, -43));
eph.setAf0(subframe_1->af_0() * pow(2, -31));
eph.setSvHealth(subframe_1->sv_health());
eph.setTowCount(subframe.how()->tow_count());
iodc_lsb = subframe_1->iodc_lsb();
}
// Subframe 2
{
kaitai::kstream stream(gps_subframes[msg->sv_id()][2]);
gps_t subframe(&stream);
gps_t::subframe_2_t* subframe_2 = static_cast<gps_t::subframe_2_t*>(subframe.body());
// GPS week refers to current week, the ephemeris can be valid for the next
// if toe equals 0, this can be verified by the TOW count if it is within the
// last 2 hours of the week (gps ephemeris valid for 4hours)
if (subframe_2->t_oe() == 0 and subframe.how()->tow_count()*6 >= (SECS_IN_WEEK - 2*SECS_IN_HR)){
week += 1;
}
eph.setCrs(subframe_2->c_rs() * pow(2, -5));
eph.setDeltaN(subframe_2->delta_n() * pow(2, -43) * gpsPi);
eph.setM0(subframe_2->m_0() * pow(2, -31) * gpsPi);
eph.setCuc(subframe_2->c_uc() * pow(2, -29));
eph.setEcc(subframe_2->e() * pow(2, -33));
eph.setCus(subframe_2->c_us() * pow(2, -29));
eph.setA(pow(subframe_2->sqrt_a() * pow(2, -19), 2.0));
eph.setToe(subframe_2->t_oe() * pow(2, 4));
iode_s2 = subframe_2->iode();
}
// Subframe 3
{
kaitai::kstream stream(gps_subframes[msg->sv_id()][3]);
gps_t subframe(&stream);
gps_t::subframe_3_t* subframe_3 = static_cast<gps_t::subframe_3_t*>(subframe.body());
eph.setCic(subframe_3->c_ic() * pow(2, -29));
eph.setOmega0(subframe_3->omega_0() * pow(2, -31) * gpsPi);
eph.setCis(subframe_3->c_is() * pow(2, -29));
eph.setI0(subframe_3->i_0() * pow(2, -31) * gpsPi);
eph.setCrc(subframe_3->c_rc() * pow(2, -5));
eph.setOmega(subframe_3->omega() * pow(2, -31) * gpsPi);
eph.setOmegaDot(subframe_3->omega_dot() * pow(2, -43) * gpsPi);
eph.setIode(subframe_3->iode());
eph.setIDot(subframe_3->idot() * pow(2, -43) * gpsPi);
iode_s3 = subframe_3->iode();
}
eph.setToeWeek(week);
eph.setTocWeek(week);
gps_subframes[msg->sv_id()].clear();
if (iodc_lsb != iode_s2 || iodc_lsb != iode_s3) {
// data set cutover, reject ephemeris
return kj::Array<capnp::word>();
}
return capnp::messageToFlatArray(msg_builder);
}
return kj::Array<capnp::word>();
}
kj::Array<capnp::word> UbloxMsgParser::parse_glonass_ephemeris(ubx_t::rxm_sfrbx_t *msg) {
// This parser assumes that no 2 satellites of the same frequency
// can be in view at the same time
auto body = *msg->body();
assert(body.size() == 4);
{
std::string string_data;
string_data.reserve(16);
for (uint32_t word : body) {
for (int i = 3; i >= 0; i--)
string_data.push_back(word >> 8*i);
}
kaitai::kstream stream(string_data);
glonass_t gl_string(&stream);
int string_number = gl_string.string_number();
if (string_number < 1 || string_number > 5 || gl_string.idle_chip()) {
// dont parse non immediate data, idle_chip == 0
return kj::Array<capnp::word>();
}
// Check if new string either has same superframe_id or log transmission times make sense
bool superframe_unknown = false;
bool needs_clear = false;
for (int i = 1; i <= 5; i++) {
if (glonass_strings[msg->freq_id()].find(i) == glonass_strings[msg->freq_id()].end())
continue;
if (glonass_string_superframes[msg->freq_id()][i] == 0 || gl_string.superframe_number() == 0) {
superframe_unknown = true;
} else if (glonass_string_superframes[msg->freq_id()][i] != gl_string.superframe_number()) {
needs_clear = true;
}
// Check if string times add up to being from the same frame
// If superframe is known this is redundant
// Strings are sent 2s apart and frames are 30s apart
if (superframe_unknown &&
std::abs((glonass_string_times[msg->freq_id()][i] - 2.0 * i) - (last_log_time - 2.0 * string_number)) > 10)
needs_clear = true;
}
if (needs_clear) {
glonass_strings[msg->freq_id()].clear();
glonass_string_superframes[msg->freq_id()].clear();
glonass_string_times[msg->freq_id()].clear();
}
glonass_strings[msg->freq_id()][string_number] = string_data;
glonass_string_superframes[msg->freq_id()][string_number] = gl_string.superframe_number();
glonass_string_times[msg->freq_id()][string_number] = last_log_time;
}
if (msg->sv_id() == 255) {
// data can be decoded before identifying the SV number, in this case 255
// is returned, which means "unknown" (ublox p32)
return kj::Array<capnp::word>();
}
// publish if strings 1-5 have been collected
if (glonass_strings[msg->freq_id()].size() != 5) {
return kj::Array<capnp::word>();
}
MessageBuilder msg_builder;
auto eph = msg_builder.initEvent().initUbloxGnss().initGlonassEphemeris();
eph.setSvId(msg->sv_id());
eph.setFreqNum(msg->freq_id() - 7);
uint16_t current_day = 0;
uint16_t tk = 0;
// string number 1
{
kaitai::kstream stream(glonass_strings[msg->freq_id()][1]);
glonass_t gl_stream(&stream);
glonass_t::string_1_t* data = static_cast<glonass_t::string_1_t*>(gl_stream.data());
eph.setP1(data->p1());
tk = data->t_k();
eph.setTkDEPRECATED(tk);
eph.setXVel(data->x_vel() * pow(2, -20));
eph.setXAccel(data->x_accel() * pow(2, -30));
eph.setX(data->x() * pow(2, -11));
}
// string number 2
{
kaitai::kstream stream(glonass_strings[msg->freq_id()][2]);
glonass_t gl_stream(&stream);
glonass_t::string_2_t* data = static_cast<glonass_t::string_2_t*>(gl_stream.data());
eph.setSvHealth(data->b_n()>>2); // MSB indicates health
eph.setP2(data->p2());
eph.setTb(data->t_b());
eph.setYVel(data->y_vel() * pow(2, -20));
eph.setYAccel(data->y_accel() * pow(2, -30));
eph.setY(data->y() * pow(2, -11));
}
// string number 3
{
kaitai::kstream stream(glonass_strings[msg->freq_id()][3]);
glonass_t gl_stream(&stream);
glonass_t::string_3_t* data = static_cast<glonass_t::string_3_t*>(gl_stream.data());
eph.setP3(data->p3());
eph.setGammaN(data->gamma_n() * pow(2, -40));
eph.setSvHealth(eph.getSvHealth() | data->l_n());
eph.setZVel(data->z_vel() * pow(2, -20));
eph.setZAccel(data->z_accel() * pow(2, -30));
eph.setZ(data->z() * pow(2, -11));
}
// string number 4
{
kaitai::kstream stream(glonass_strings[msg->freq_id()][4]);
glonass_t gl_stream(&stream);
glonass_t::string_4_t* data = static_cast<glonass_t::string_4_t*>(gl_stream.data());
current_day = data->n_t();
eph.setNt(current_day);
eph.setTauN(data->tau_n() * pow(2, -30));
eph.setDeltaTauN(data->delta_tau_n() * pow(2, -30));
eph.setAge(data->e_n());
eph.setP4(data->p4());
eph.setSvURA(glonass_URA_lookup.at(data->f_t()));
if (msg->sv_id() != data->n()) {
LOGE("SV_ID != SLOT_NUMBER: %d %" PRIu64, msg->sv_id(), data->n());
}
eph.setSvType(data->m());
}
// string number 5
{
kaitai::kstream stream(glonass_strings[msg->freq_id()][5]);
glonass_t gl_stream(&stream);
glonass_t::string_5_t* data = static_cast<glonass_t::string_5_t*>(gl_stream.data());
// string5 parsing is only needed to get the year, this can be removed and
// the year can be fetched later in laika (note rollovers and leap year)
eph.setN4(data->n_4());
int tk_seconds = SECS_IN_HR * ((tk>>7) & 0x1F) + SECS_IN_MIN * ((tk>>1) & 0x3F) + (tk & 0x1) * 30;
eph.setTkSeconds(tk_seconds);
}
glonass_strings[msg->freq_id()].clear();
return capnp::messageToFlatArray(msg_builder);
}
kj::Array<capnp::word> UbloxMsgParser::gen_rxm_sfrbx(ubx_t::rxm_sfrbx_t *msg) {
switch (msg->gnss_id()) {
case ubx_t::gnss_type_t::GNSS_TYPE_GPS:
return parse_gps_ephemeris(msg);
case ubx_t::gnss_type_t::GNSS_TYPE_GLONASS:
return parse_glonass_ephemeris(msg);
default:
return kj::Array<capnp::word>();
}
}
kj::Array<capnp::word> UbloxMsgParser::gen_rxm_rawx(ubx_t::rxm_rawx_t *msg) {
MessageBuilder msg_builder;
auto mr = msg_builder.initEvent().initUbloxGnss().initMeasurementReport();
mr.setRcvTow(msg->rcv_tow());
mr.setGpsWeek(msg->week());
mr.setLeapSeconds(msg->leap_s());
mr.setGpsWeek(msg->week());
auto mb = mr.initMeasurements(msg->num_meas());
auto measurements = *msg->meas();
for (int8_t i = 0; i < msg->num_meas(); i++) {
mb[i].setSvId(measurements[i]->sv_id());
mb[i].setPseudorange(measurements[i]->pr_mes());
mb[i].setCarrierCycles(measurements[i]->cp_mes());
mb[i].setDoppler(measurements[i]->do_mes());
mb[i].setGnssId(measurements[i]->gnss_id());
mb[i].setGlonassFrequencyIndex(measurements[i]->freq_id());
mb[i].setLocktime(measurements[i]->lock_time());
mb[i].setCno(measurements[i]->cno());
mb[i].setPseudorangeStdev(0.01 * (pow(2, (measurements[i]->pr_stdev() & 15)))); // weird scaling, might be wrong
mb[i].setCarrierPhaseStdev(0.004 * (measurements[i]->cp_stdev() & 15));
mb[i].setDopplerStdev(0.002 * (pow(2, (measurements[i]->do_stdev() & 15)))); // weird scaling, might be wrong
auto ts = mb[i].initTrackingStatus();
auto trk_stat = measurements[i]->trk_stat();
ts.setPseudorangeValid(bit_to_bool(trk_stat, 0));
ts.setCarrierPhaseValid(bit_to_bool(trk_stat, 1));
ts.setHalfCycleValid(bit_to_bool(trk_stat, 2));
ts.setHalfCycleSubtracted(bit_to_bool(trk_stat, 3));
}
mr.setNumMeas(msg->num_meas());
auto rs = mr.initReceiverStatus();
rs.setLeapSecValid(bit_to_bool(msg->rec_stat(), 0));
rs.setClkReset(bit_to_bool(msg->rec_stat(), 2));
return capnp::messageToFlatArray(msg_builder);
}
kj::Array<capnp::word> UbloxMsgParser::gen_nav_sat(ubx_t::nav_sat_t *msg) {
MessageBuilder msg_builder;
auto sr = msg_builder.initEvent().initUbloxGnss().initSatReport();
sr.setITow(msg->itow());
auto svs = sr.initSvs(msg->num_svs());
auto svs_data = *msg->svs();
for (int8_t i = 0; i < msg->num_svs(); i++) {
svs[i].setSvId(svs_data[i]->sv_id());
svs[i].setGnssId(svs_data[i]->gnss_id());
svs[i].setFlagsBitfield(svs_data[i]->flags());
}
return capnp::messageToFlatArray(msg_builder);
}
kj::Array<capnp::word> UbloxMsgParser::gen_mon_hw(ubx_t::mon_hw_t *msg) {
MessageBuilder msg_builder;
auto hwStatus = msg_builder.initEvent().initUbloxGnss().initHwStatus();
hwStatus.setNoisePerMS(msg->noise_per_ms());
hwStatus.setFlags(msg->flags());
hwStatus.setAgcCnt(msg->agc_cnt());
hwStatus.setAStatus((cereal::UbloxGnss::HwStatus::AntennaSupervisorState) msg->a_status());
hwStatus.setAPower((cereal::UbloxGnss::HwStatus::AntennaPowerStatus) msg->a_power());
hwStatus.setJamInd(msg->jam_ind());
return capnp::messageToFlatArray(msg_builder);
}
kj::Array<capnp::word> UbloxMsgParser::gen_mon_hw2(ubx_t::mon_hw2_t *msg) {
MessageBuilder msg_builder;
auto hwStatus = msg_builder.initEvent().initUbloxGnss().initHwStatus2();
hwStatus.setOfsI(msg->ofs_i());
hwStatus.setMagI(msg->mag_i());
hwStatus.setOfsQ(msg->ofs_q());
hwStatus.setMagQ(msg->mag_q());
switch (msg->cfg_source()) {
case ubx_t::mon_hw2_t::config_source_t::CONFIG_SOURCE_ROM:
hwStatus.setCfgSource(cereal::UbloxGnss::HwStatus2::ConfigSource::ROM);
break;
case ubx_t::mon_hw2_t::config_source_t::CONFIG_SOURCE_OTP:
hwStatus.setCfgSource(cereal::UbloxGnss::HwStatus2::ConfigSource::OTP);
break;
case ubx_t::mon_hw2_t::config_source_t::CONFIG_SOURCE_CONFIG_PINS:
hwStatus.setCfgSource(cereal::UbloxGnss::HwStatus2::ConfigSource::CONFIGPINS);
break;
case ubx_t::mon_hw2_t::config_source_t::CONFIG_SOURCE_FLASH:
hwStatus.setCfgSource(cereal::UbloxGnss::HwStatus2::ConfigSource::FLASH);
break;
default:
hwStatus.setCfgSource(cereal::UbloxGnss::HwStatus2::ConfigSource::UNDEFINED);
break;
}
hwStatus.setLowLevCfg(msg->low_lev_cfg());
hwStatus.setPostStatus(msg->post_status());
return capnp::messageToFlatArray(msg_builder);
}

131
system/ubloxd/ublox_msg.h
View File

@@ -1,131 +0,0 @@
#pragma once
#include <cassert>
#include <cstdint>
#include <ctime>
#include <memory>
#include <string>
#include <unordered_map>
#include <utility>
#include "cereal/messaging/messaging.h"
#include "common/util.h"
#include "system/ubloxd/generated/gps.h"
#include "system/ubloxd/generated/glonass.h"
#include "system/ubloxd/generated/ubx.h"
using namespace std::string_literals;
const int SECS_IN_MIN = 60;
const int SECS_IN_HR = 60 * SECS_IN_MIN;
const int SECS_IN_DAY = 24 * SECS_IN_HR;
const int SECS_IN_WEEK = 7 * SECS_IN_DAY;
// protocol constants
namespace ublox {
const uint8_t PREAMBLE1 = 0xb5;
const uint8_t PREAMBLE2 = 0x62;
const int UBLOX_HEADER_SIZE = 6;
const int UBLOX_CHECKSUM_SIZE = 2;
const int UBLOX_MAX_MSG_SIZE = 65536;
struct ubx_mga_ini_time_utc_t {
uint8_t type;
uint8_t version;
uint8_t ref;
int8_t leapSecs;
uint16_t year;
uint8_t month;
uint8_t day;
uint8_t hour;
uint8_t minute;
uint8_t second;
uint8_t reserved1;
uint32_t ns;
uint16_t tAccS;
uint16_t reserved2;
uint32_t tAccNs;
} __attribute__((packed));
inline std::string ubx_add_checksum(const std::string &msg) {
assert(msg.size() > 2);
uint8_t ck_a = 0, ck_b = 0;
for (int i = 2; i < msg.size(); i++) {
ck_a = (ck_a + msg[i]) & 0xFF;
ck_b = (ck_b + ck_a) & 0xFF;
}
std::string r = msg;
r.push_back(ck_a);
r.push_back(ck_b);
return r;
}
inline std::string build_ubx_mga_ini_time_utc(struct tm time) {
ublox::ubx_mga_ini_time_utc_t payload = {
.type = 0x10,
.version = 0x0,
.ref = 0x0,
.leapSecs = -128, // Unknown
.year = (uint16_t)(1900 + time.tm_year),
.month = (uint8_t)(1 + time.tm_mon),
.day = (uint8_t)time.tm_mday,
.hour = (uint8_t)time.tm_hour,
.minute = (uint8_t)time.tm_min,
.second = (uint8_t)time.tm_sec,
.reserved1 = 0x0,
.ns = 0,
.tAccS = 30,
.reserved2 = 0x0,
.tAccNs = 0,
};
assert(sizeof(payload) == 24);
std::string msg = "\xb5\x62\x13\x40\x18\x00"s;
msg += std::string((char*)&payload, sizeof(payload));
return ubx_add_checksum(msg);
}
}
class UbloxMsgParser {
public:
bool add_data(float log_time, const uint8_t *incoming_data, uint32_t incoming_data_len, size_t &bytes_consumed);
inline void reset() {bytes_in_parse_buf = 0;}
inline int needed_bytes();
inline std::string data() {return std::string((const char*)msg_parse_buf, bytes_in_parse_buf);}
std::pair<std::string, kj::Array<capnp::word>> gen_msg();
kj::Array<capnp::word> gen_nav_pvt(ubx_t::nav_pvt_t *msg);
kj::Array<capnp::word> gen_rxm_sfrbx(ubx_t::rxm_sfrbx_t *msg);
kj::Array<capnp::word> gen_rxm_rawx(ubx_t::rxm_rawx_t *msg);
kj::Array<capnp::word> gen_mon_hw(ubx_t::mon_hw_t *msg);
kj::Array<capnp::word> gen_mon_hw2(ubx_t::mon_hw2_t *msg);
kj::Array<capnp::word> gen_nav_sat(ubx_t::nav_sat_t *msg);
private:
inline bool valid_cheksum();
inline bool valid();
inline bool valid_so_far();
kj::Array<capnp::word> parse_gps_ephemeris(ubx_t::rxm_sfrbx_t *msg);
kj::Array<capnp::word> parse_glonass_ephemeris(ubx_t::rxm_sfrbx_t *msg);
std::unordered_map<int, std::unordered_map<int, std::string>> gps_subframes;
float last_log_time = 0.0;
size_t bytes_in_parse_buf = 0;
uint8_t msg_parse_buf[ublox::UBLOX_HEADER_SIZE + ublox::UBLOX_MAX_MSG_SIZE];
// user range accuracy in meters
const std::unordered_map<uint8_t, float> glonass_URA_lookup =
{{ 0, 1}, { 1, 2}, { 2, 2.5}, { 3, 4}, { 4, 5}, {5, 7},
{ 6, 10}, { 7, 12}, { 8, 14}, { 9, 16}, {10, 32},
{11, 64}, {12, 128}, {13, 256}, {14, 512}, {15, 1024}};
std::unordered_map<int, std::unordered_map<int, std::string>> glonass_strings;
std::unordered_map<int, std::unordered_map<int, long>> glonass_string_times;
std::unordered_map<int, std::unordered_map<int, int>> glonass_string_superframes;
};

BIN
system/ubloxd/ubloxd Executable file
View File

Binary file not shown.

65
system/ubloxd/ubloxd.cc
View File

@@ -1,65 +0,0 @@
#include <cassert>
#include <kaitai/kaitaistream.h>
#include "cereal/messaging/messaging.h"
#include "common/swaglog.h"
#include "common/util.h"
#include "system/ubloxd/ublox_msg.h"
ExitHandler do_exit;
using namespace ublox;
int main() {
LOGW("starting ubloxd");
AlignedBuffer aligned_buf;
UbloxMsgParser parser;
PubMaster pm({"ubloxGnss", "gpsLocationExternal"});
std::unique_ptr<Context> context(Context::create());
std::unique_ptr<SubSocket> subscriber(SubSocket::create(context.get(), "ubloxRaw"));
assert(subscriber != NULL);
subscriber->setTimeout(100);
while (!do_exit) {
std::unique_ptr<Message> msg(subscriber->receive());
if (!msg) {
if (errno == EINTR) {
do_exit = true;
}
continue;
}
capnp::FlatArrayMessageReader cmsg(aligned_buf.align(msg.get()));
cereal::Event::Reader event = cmsg.getRoot<cereal::Event>();
auto ubloxRaw = event.getUbloxRaw();
float log_time = 1e-9 * event.getLogMonoTime();
const uint8_t *data = ubloxRaw.begin();
size_t len = ubloxRaw.size();
size_t bytes_consumed = 0;
while (bytes_consumed < len && !do_exit) {
size_t bytes_consumed_this_time = 0U;
if (parser.add_data(log_time, data + bytes_consumed, (uint32_t)(len - bytes_consumed), bytes_consumed_this_time)) {
try {
auto ublox_msg = parser.gen_msg();
if (ublox_msg.second.size() > 0) {
auto bytes = ublox_msg.second.asBytes();
pm.send(ublox_msg.first.c_str(), bytes.begin(), bytes.size());
}
} catch (const std::exception& e) {
LOGE("Error parsing ublox message %s", e.what());
}
parser.reset();
}
bytes_consumed += bytes_consumed_this_time;
}
}
return 0;
}