mirror of
https://github.com/dragonpilot/dragonpilot.git
synced 2026-09-18 05:23:44 +08:00
openpilot v0.9.4 release
date: 2023-07-27T18:38:32
master commit: fa310d9e25
This commit is contained in:
Vehicle Researcher
@@ -0,0 +1,14 @@
|
||||
Import('env', 'arch', 'cereal', 'messaging', 'common', 'gpucommon', 'visionipc')
|
||||
|
||||
libs = ['m', 'pthread', common, 'jpeg', 'OpenCL', 'yuv', cereal, messaging, 'zmq', 'capnp', 'kj', visionipc, gpucommon, 'atomic']
|
||||
|
||||
camera_obj = env.Object(['cameras/camera_qcom2.cc', 'cameras/camera_common.cc', 'cameras/camera_util.cc'])
|
||||
env.Program('camerad', [
|
||||
'main.cc',
|
||||
camera_obj,
|
||||
], LIBS=libs)
|
||||
|
||||
if GetOption("test") and arch == "x86_64":
|
||||
env.Program('test/ae_gray_test',
|
||||
['test/ae_gray_test.cc', camera_obj],
|
||||
LIBS=libs)
|
||||
@@ -0,0 +1,363 @@
|
||||
#include "system/camerad/cameras/camera_common.h"
|
||||
|
||||
#include <unistd.h>
|
||||
|
||||
#include <cassert>
|
||||
#include <cstdio>
|
||||
#include <chrono>
|
||||
#include <thread>
|
||||
|
||||
#include "libyuv.h"
|
||||
#include <jpeglib.h>
|
||||
|
||||
#include "system/camerad/imgproc/utils.h"
|
||||
#include "common/clutil.h"
|
||||
#include "common/modeldata.h"
|
||||
#include "common/swaglog.h"
|
||||
#include "common/util.h"
|
||||
#include "system/hardware/hw.h"
|
||||
#include "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 CameraInfo *ci = &s->ci;
|
||||
snprintf(args, sizeof(args),
|
||||
"-cl-fast-relaxed-math -cl-denorms-are-zero "
|
||||
"-DFRAME_WIDTH=%d -DFRAME_HEIGHT=%d -DFRAME_STRIDE=%d -DFRAME_OFFSET=%d "
|
||||
"-DRGB_WIDTH=%d -DRGB_HEIGHT=%d -DRGB_STRIDE=%d -DYUV_STRIDE=%d -DUV_OFFSET=%d "
|
||||
"-DIS_OX=%d -DCAM_NUM=%d%s",
|
||||
ci->frame_width, ci->frame_height, ci->frame_stride, ci->frame_offset,
|
||||
b->rgb_width, b->rgb_height, b->rgb_stride, buf_width, uv_offset,
|
||||
s->camera_id==CAMERA_ID_OX03C10 ? 1 : 0, s->camera_num, s->camera_num==1 ? " -DVIGNETTING" : "");
|
||||
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 init_yuv_type) {
|
||||
vipc_server = v;
|
||||
this->yuv_type = init_yuv_type;
|
||||
frame_buf_count = frame_cnt;
|
||||
|
||||
const CameraInfo *ci = &s->ci;
|
||||
// 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(yuv_type, YUV_BUFFER_COUNT, false, rgb_width, rgb_height, nv12_size, nv12_width, nv12_uv_offset);
|
||||
LOGD("created %d YUV vipc buffers with size %dx%d", YUV_BUFFER_COUNT, nv12_width, nv12_height);
|
||||
|
||||
debayer = new Debayer(device_id, context, this, s, nv12_width, nv12_uv_offset);
|
||||
|
||||
#ifdef __APPLE__
|
||||
q = CL_CHECK_ERR(clCreateCommandQueue(context, device_id, 0, &err));
|
||||
#else
|
||||
const cl_queue_properties props[] = {0}; //CL_QUEUE_PRIORITY_KHR, CL_QUEUE_PRIORITY_HIGH_KHR, 0};
|
||||
q = CL_CHECK_ERR(clCreateCommandQueueWithProperties(context, device_id, props, &err));
|
||||
#endif
|
||||
}
|
||||
|
||||
CameraBuf::~CameraBuf() {
|
||||
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(yuv_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.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->min_ev, c->max_ev, 0.0f, 100.0f);
|
||||
framed.setExposureValPercent(perc);
|
||||
|
||||
if (c->camera_id == CAMERA_ID_AR0231) {
|
||||
framed.setSensor(cereal::FrameData::ImageSensor::AR0231);
|
||||
} else if (c->camera_id == CAMERA_ID_OX03C10) {
|
||||
framed.setSensor(cereal::FrameData::ImageSensor::OX03C10);
|
||||
}
|
||||
}
|
||||
|
||||
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--;
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,113 @@
|
||||
#pragma once
|
||||
|
||||
#include <cstdint>
|
||||
#include <cstdlib>
|
||||
#include <memory>
|
||||
#include <thread>
|
||||
|
||||
#include "cereal/messaging/messaging.h"
|
||||
#include "cereal/visionipc/visionbuf.h"
|
||||
#include "cereal/visionipc/visionipc.h"
|
||||
#include "cereal/visionipc/visionipc_server.h"
|
||||
#include "common/mat.h"
|
||||
#include "common/queue.h"
|
||||
#include "common/swaglog.h"
|
||||
#include "system/hardware/hw.h"
|
||||
|
||||
#define CAMERA_ID_IMX298 0
|
||||
#define CAMERA_ID_IMX179 1
|
||||
#define CAMERA_ID_S5K3P8SP 2
|
||||
#define CAMERA_ID_OV8865 3
|
||||
#define CAMERA_ID_IMX298_FLIPPED 4
|
||||
#define CAMERA_ID_OV10640 5
|
||||
#define CAMERA_ID_LGC920 6
|
||||
#define CAMERA_ID_LGC615 7
|
||||
#define CAMERA_ID_AR0231 8
|
||||
#define CAMERA_ID_OX03C10 9
|
||||
#define CAMERA_ID_MAX 10
|
||||
|
||||
const int YUV_BUFFER_COUNT = 40;
|
||||
|
||||
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 CameraInfo {
|
||||
uint32_t frame_width, frame_height;
|
||||
uint32_t frame_stride;
|
||||
uint32_t frame_offset = 0;
|
||||
uint32_t extra_height = 0;
|
||||
int registers_offset = -1;
|
||||
int stats_offset = -1;
|
||||
} CameraInfo;
|
||||
|
||||
typedef struct FrameMetadata {
|
||||
uint32_t frame_id;
|
||||
|
||||
// Timestamps
|
||||
uint64_t timestamp_sof; // only set on tici
|
||||
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 yuv_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, rgb_stride;
|
||||
|
||||
CameraBuf() = default;
|
||||
~CameraBuf();
|
||||
void init(cl_device_id device_id, cl_context context, CameraState *s, VisionIpcServer * v, int frame_cnt, VisionStreamType yuv_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);
|
||||
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,124 @@
|
||||
#pragma once
|
||||
|
||||
#include <cstdint>
|
||||
#include <map>
|
||||
#include <utility>
|
||||
|
||||
#include <media/cam_req_mgr.h>
|
||||
|
||||
#include "system/camerad/cameras/camera_common.h"
|
||||
#include "system/camerad/cameras/camera_util.h"
|
||||
#include "common/params.h"
|
||||
#include "common/util.h"
|
||||
|
||||
#define FRAME_BUF_COUNT 4
|
||||
#define ANALOG_GAIN_MAX_CNT 55
|
||||
|
||||
class CameraState {
|
||||
public:
|
||||
MultiCameraState *multi_cam_state;
|
||||
CameraInfo ci;
|
||||
bool enabled;
|
||||
|
||||
std::mutex exp_lock;
|
||||
|
||||
int exposure_time;
|
||||
bool dc_gain_enabled;
|
||||
int dc_gain_weight;
|
||||
int gain_idx;
|
||||
float analog_gain_frac;
|
||||
|
||||
int exposure_time_min;
|
||||
int exposure_time_max;
|
||||
|
||||
float dc_gain_factor;
|
||||
int dc_gain_min_weight;
|
||||
int dc_gain_max_weight;
|
||||
float dc_gain_on_grey;
|
||||
float dc_gain_off_grey;
|
||||
|
||||
float sensor_analog_gains[ANALOG_GAIN_MAX_CNT];
|
||||
int analog_gain_min_idx;
|
||||
int analog_gain_max_idx;
|
||||
int analog_gain_rec_idx;
|
||||
int analog_gain_cost_delta;
|
||||
float analog_gain_cost_low;
|
||||
float analog_gain_cost_high;
|
||||
|
||||
float cur_ev[3];
|
||||
float min_ev, max_ev;
|
||||
float best_ev_score;
|
||||
int new_exp_g;
|
||||
int new_exp_t;
|
||||
|
||||
float measured_grey_fraction;
|
||||
float target_grey_fraction;
|
||||
float target_grey_factor;
|
||||
|
||||
unique_fd sensor_fd;
|
||||
unique_fd csiphy_fd;
|
||||
|
||||
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 camera_set_parameters();
|
||||
void camera_map_bufs(MultiCameraState *s);
|
||||
void camera_init(MultiCameraState *s, VisionIpcServer *v, int camera_id, unsigned int fps, cl_device_id device_id, cl_context ctx, VisionStreamType yuv_type);
|
||||
void camera_close();
|
||||
|
||||
std::map<uint16_t, uint16_t> ar0231_parse_registers(uint8_t *data, std::initializer_list<uint16_t> addrs);
|
||||
|
||||
int32_t session_handle;
|
||||
int32_t sensor_dev_handle;
|
||||
int32_t isp_dev_handle;
|
||||
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;
|
||||
int camera_id;
|
||||
|
||||
CameraBuf buf;
|
||||
MemoryManager mm;
|
||||
|
||||
private:
|
||||
void config_isp(int io_mem_handle, int fence, int request_id, int buf0_mem_handle, int buf0_offset);
|
||||
void enqueue_req_multi(int start, int n, bool dp);
|
||||
void enqueue_buffer(int i, bool dp);
|
||||
int clear_req_queue();
|
||||
|
||||
int sensors_init();
|
||||
void sensors_poke(int request_id);
|
||||
void sensors_i2c(struct i2c_random_wr_payload* dat, int len, int op_code, bool data_word);
|
||||
|
||||
// Register parsing
|
||||
std::map<uint16_t, std::pair<int, int>> ar0231_register_lut;
|
||||
std::map<uint16_t, std::pair<int, int>> ar0231_build_register_lut(uint8_t *data);
|
||||
};
|
||||
|
||||
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;
|
||||
@@ -0,0 +1,135 @@
|
||||
#include "system/camerad/cameras/camera_util.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(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();
|
||||
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);
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,30 @@
|
||||
#pragma once
|
||||
|
||||
#include <map>
|
||||
#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; }
|
||||
void *alloc(int len, uint32_t *handle);
|
||||
void free(void *ptr);
|
||||
~MemoryManager();
|
||||
private:
|
||||
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;
|
||||
};
|
||||
@@ -0,0 +1,197 @@
|
||||
#define UV_WIDTH RGB_WIDTH / 2
|
||||
#define UV_HEIGHT RGB_HEIGHT / 2
|
||||
|
||||
#define RGB_TO_Y(r, g, b) ((((mul24(b, 13) + mul24(g, 65) + mul24(r, 33)) + 64) >> 7) + 16)
|
||||
#define RGB_TO_U(r, g, b) ((mul24(b, 56) - mul24(g, 37) - mul24(r, 19) + 0x8080) >> 8)
|
||||
#define RGB_TO_V(r, g, b) ((mul24(r, 56) - mul24(g, 47) - mul24(b, 9) + 0x8080) >> 8)
|
||||
#define AVERAGE(x, y, z, w) ((convert_ushort(x) + convert_ushort(y) + convert_ushort(z) + convert_ushort(w) + 1) >> 1)
|
||||
|
||||
float3 color_correct(float3 rgb) {
|
||||
// color correction
|
||||
#if IS_OX
|
||||
float3 x = rgb.x * (float3)(1.5664815 , -0.29808738, -0.03973474);
|
||||
x += rgb.y * (float3)(-0.48672447, 1.41914433, -0.40295248);
|
||||
x += rgb.z * (float3)(-0.07975703, -0.12105695, 1.44268722);
|
||||
#else
|
||||
float3 x = rgb.x * (float3)(1.82717181, -0.31231438, 0.07307673);
|
||||
x += rgb.y * (float3)(-0.5743977, 1.36858544, -0.53183455);
|
||||
x += rgb.z * (float3)(-0.25277411, -0.05627105, 1.45875782);
|
||||
#endif
|
||||
|
||||
#if IS_OX
|
||||
return -0.507089*exp(-12.54124638*x)+0.9655*powr(x,0.5)-0.472597*x+0.507089;
|
||||
#else
|
||||
// tone mapping params
|
||||
const float gamma_k = 0.75;
|
||||
const float gamma_b = 0.125;
|
||||
const float mp = 0.01; // ideally midpoint should be adaptive
|
||||
const float rk = 9 - 100*mp;
|
||||
|
||||
// poly approximation for s curve
|
||||
return (x > mp) ?
|
||||
((rk * (x-mp) * (1-(gamma_k*mp+gamma_b)) * (1+1/(rk*(1-mp))) / (1+rk*(x-mp))) + gamma_k*mp + gamma_b) :
|
||||
((rk * (x-mp) * (gamma_k*mp+gamma_b) * (1+1/(rk*mp)) / (1-rk*(x-mp))) + gamma_k*mp + gamma_b);
|
||||
#endif
|
||||
}
|
||||
|
||||
float get_vignetting_s(float r) {
|
||||
if (r < 62500) {
|
||||
return (1.0f + 0.0000008f*r);
|
||||
} else if (r < 490000) {
|
||||
return (0.9625f + 0.0000014f*r);
|
||||
} else if (r < 1102500) {
|
||||
return (1.26434f + 0.0000000000016f*r*r);
|
||||
} else {
|
||||
return (0.53503625f + 0.0000000000022f*r*r);
|
||||
}
|
||||
}
|
||||
|
||||
constant float ox03c10_lut[] = {
|
||||
0.0000e+00, 5.9488e-08, 1.1898e-07, 1.7846e-07, 2.3795e-07, 2.9744e-07, 3.5693e-07, 4.1642e-07, 4.7591e-07, 5.3539e-07, 5.9488e-07, 6.5437e-07, 7.1386e-07, 7.7335e-07, 8.3284e-07, 8.9232e-07, 9.5181e-07, 1.0113e-06, 1.0708e-06, 1.1303e-06, 1.1898e-06, 1.2493e-06, 1.3087e-06, 1.3682e-06, 1.4277e-06, 1.4872e-06, 1.5467e-06, 1.6062e-06, 1.6657e-06, 1.7252e-06, 1.7846e-06, 1.8441e-06, 1.9036e-06, 1.9631e-06, 2.0226e-06, 2.0821e-06, 2.1416e-06, 2.2011e-06, 2.2606e-06, 2.3200e-06, 2.3795e-06, 2.4390e-06, 2.4985e-06, 2.5580e-06, 2.6175e-06, 2.6770e-06, 2.7365e-06, 2.7959e-06, 2.8554e-06, 2.9149e-06, 2.9744e-06, 3.0339e-06, 3.0934e-06, 3.1529e-06, 3.2124e-06, 3.2719e-06, 3.3313e-06, 3.3908e-06, 3.4503e-06, 3.5098e-06, 3.5693e-06, 3.6288e-06, 3.6883e-06, 3.7478e-06, 3.8072e-06, 3.8667e-06, 3.9262e-06, 3.9857e-06, 4.0452e-06, 4.1047e-06, 4.1642e-06, 4.2237e-06, 4.2832e-06, 4.3426e-06, 4.4021e-06, 4.4616e-06, 4.5211e-06, 4.5806e-06, 4.6401e-06, 4.6996e-06, 4.7591e-06, 4.8185e-06, 4.8780e-06, 4.9375e-06, 4.9970e-06, 5.0565e-06, 5.1160e-06, 5.1755e-06, 5.2350e-06, 5.2945e-06, 5.3539e-06, 5.4134e-06, 5.4729e-06, 5.5324e-06, 5.5919e-06, 5.6514e-06, 5.7109e-06, 5.7704e-06, 5.8298e-06, 5.8893e-06, 5.9488e-06, 6.0083e-06, 6.0678e-06, 6.1273e-06, 6.1868e-06, 6.2463e-06, 6.3058e-06, 6.3652e-06, 6.4247e-06, 6.4842e-06, 6.5437e-06, 6.6032e-06, 6.6627e-06, 6.7222e-06, 6.7817e-06, 6.8411e-06, 6.9006e-06, 6.9601e-06, 7.0196e-06, 7.0791e-06, 7.1386e-06, 7.1981e-06, 7.2576e-06, 7.3171e-06, 7.3765e-06, 7.4360e-06, 7.4955e-06, 7.5550e-06, 7.6145e-06, 7.6740e-06, 7.7335e-06, 7.7930e-06, 7.8524e-06, 7.9119e-06, 7.9714e-06, 8.0309e-06, 8.0904e-06, 8.1499e-06, 8.2094e-06, 8.2689e-06, 8.3284e-06, 8.3878e-06, 8.4473e-06, 8.5068e-06, 8.5663e-06, 8.6258e-06, 8.6853e-06, 8.7448e-06, 8.8043e-06, 8.8637e-06, 8.9232e-06, 8.9827e-06, 9.0422e-06, 9.1017e-06, 9.1612e-06, 9.2207e-06, 9.2802e-06, 9.3397e-06, 9.3991e-06, 9.4586e-06, 9.5181e-06, 9.5776e-06, 9.6371e-06, 9.6966e-06, 9.7561e-06, 9.8156e-06, 9.8750e-06, 9.9345e-06, 9.9940e-06, 1.0054e-05, 1.0113e-05, 1.0172e-05, 1.0232e-05, 1.0291e-05, 1.0351e-05, 1.0410e-05, 1.0470e-05, 1.0529e-05, 1.0589e-05, 1.0648e-05, 1.0708e-05, 1.0767e-05, 1.0827e-05, 1.0886e-05, 1.0946e-05, 1.1005e-05, 1.1065e-05, 1.1124e-05, 1.1184e-05, 1.1243e-05, 1.1303e-05, 1.1362e-05, 1.1422e-05, 1.1481e-05, 1.1541e-05, 1.1600e-05, 1.1660e-05, 1.1719e-05, 1.1779e-05, 1.1838e-05, 1.1898e-05, 1.1957e-05, 1.2017e-05, 1.2076e-05, 1.2136e-05, 1.2195e-05, 1.2255e-05, 1.2314e-05, 1.2374e-05, 1.2433e-05, 1.2493e-05, 1.2552e-05, 1.2612e-05, 1.2671e-05, 1.2730e-05, 1.2790e-05, 1.2849e-05, 1.2909e-05, 1.2968e-05, 1.3028e-05, 1.3087e-05, 1.3147e-05, 1.3206e-05, 1.3266e-05, 1.3325e-05, 1.3385e-05, 1.3444e-05, 1.3504e-05, 1.3563e-05, 1.3623e-05, 1.3682e-05, 1.3742e-05, 1.3801e-05, 1.3861e-05, 1.3920e-05, 1.3980e-05, 1.4039e-05, 1.4099e-05, 1.4158e-05, 1.4218e-05, 1.4277e-05, 1.4337e-05, 1.4396e-05, 1.4456e-05, 1.4515e-05, 1.4575e-05, 1.4634e-05, 1.4694e-05, 1.4753e-05, 1.4813e-05, 1.4872e-05, 1.4932e-05, 1.4991e-05, 1.5051e-05, 1.5110e-05, 1.5169e-05,
|
||||
1.5229e-05, 1.5288e-05, 1.5348e-05, 1.5407e-05, 1.5467e-05, 1.5526e-05, 1.5586e-05, 1.5645e-05, 1.5705e-05, 1.5764e-05, 1.5824e-05, 1.5883e-05, 1.5943e-05, 1.6002e-05, 1.6062e-05, 1.6121e-05, 1.6181e-05, 1.6240e-05, 1.6300e-05, 1.6359e-05, 1.6419e-05, 1.6478e-05, 1.6538e-05, 1.6597e-05, 1.6657e-05, 1.6716e-05, 1.6776e-05, 1.6835e-05, 1.6895e-05, 1.6954e-05, 1.7014e-05, 1.7073e-05, 1.7133e-05, 1.7192e-05, 1.7252e-05, 1.7311e-05, 1.7371e-05, 1.7430e-05, 1.7490e-05, 1.7549e-05, 1.7609e-05, 1.7668e-05, 1.7727e-05, 1.7787e-05, 1.7846e-05, 1.7906e-05, 1.7965e-05, 1.8025e-05, 1.8084e-05, 1.8144e-05, 1.8203e-05, 1.8263e-05, 1.8322e-05, 1.8382e-05, 1.8441e-05, 1.8501e-05, 1.8560e-05, 1.8620e-05, 1.8679e-05, 1.8739e-05, 1.8798e-05, 1.8858e-05, 1.8917e-05, 1.8977e-05, 1.9036e-05, 1.9096e-05, 1.9155e-05, 1.9215e-05, 1.9274e-05, 1.9334e-05, 1.9393e-05, 1.9453e-05, 1.9512e-05, 1.9572e-05, 1.9631e-05, 1.9691e-05, 1.9750e-05, 1.9810e-05, 1.9869e-05, 1.9929e-05, 1.9988e-05, 2.0048e-05, 2.0107e-05, 2.0167e-05, 2.0226e-05, 2.0285e-05, 2.0345e-05, 2.0404e-05, 2.0464e-05, 2.0523e-05, 2.0583e-05, 2.0642e-05, 2.0702e-05, 2.0761e-05, 2.0821e-05, 2.0880e-05, 2.0940e-05, 2.0999e-05, 2.1059e-05, 2.1118e-05, 2.1178e-05, 2.1237e-05, 2.1297e-05, 2.1356e-05, 2.1416e-05, 2.1475e-05, 2.1535e-05, 2.1594e-05, 2.1654e-05, 2.1713e-05, 2.1773e-05, 2.1832e-05, 2.1892e-05, 2.1951e-05, 2.2011e-05, 2.2070e-05, 2.2130e-05, 2.2189e-05, 2.2249e-05, 2.2308e-05, 2.2368e-05, 2.2427e-05, 2.2487e-05, 2.2546e-05, 2.2606e-05, 2.2665e-05, 2.2725e-05, 2.2784e-05, 2.2843e-05, 2.2903e-05, 2.2962e-05, 2.3022e-05, 2.3081e-05, 2.3141e-05, 2.3200e-05, 2.3260e-05, 2.3319e-05, 2.3379e-05, 2.3438e-05, 2.3498e-05, 2.3557e-05, 2.3617e-05, 2.3676e-05, 2.3736e-05, 2.3795e-05, 2.3855e-05, 2.3914e-05, 2.3974e-05, 2.4033e-05, 2.4093e-05, 2.4152e-05, 2.4212e-05, 2.4271e-05, 2.4331e-05, 2.4390e-05, 2.4450e-05, 2.4509e-05, 2.4569e-05, 2.4628e-05, 2.4688e-05, 2.4747e-05, 2.4807e-05, 2.4866e-05, 2.4926e-05, 2.4985e-05, 2.5045e-05, 2.5104e-05, 2.5164e-05, 2.5223e-05, 2.5282e-05, 2.5342e-05, 2.5401e-05, 2.5461e-05, 2.5520e-05, 2.5580e-05, 2.5639e-05, 2.5699e-05, 2.5758e-05, 2.5818e-05, 2.5877e-05, 2.5937e-05, 2.5996e-05, 2.6056e-05, 2.6115e-05, 2.6175e-05, 2.6234e-05, 2.6294e-05, 2.6353e-05, 2.6413e-05, 2.6472e-05, 2.6532e-05, 2.6591e-05, 2.6651e-05, 2.6710e-05, 2.6770e-05, 2.6829e-05, 2.6889e-05, 2.6948e-05, 2.7008e-05, 2.7067e-05, 2.7127e-05, 2.7186e-05, 2.7246e-05, 2.7305e-05, 2.7365e-05, 2.7424e-05, 2.7484e-05, 2.7543e-05, 2.7603e-05, 2.7662e-05, 2.7722e-05, 2.7781e-05, 2.7840e-05, 2.7900e-05, 2.7959e-05, 2.8019e-05, 2.8078e-05, 2.8138e-05, 2.8197e-05, 2.8257e-05, 2.8316e-05, 2.8376e-05, 2.8435e-05, 2.8495e-05, 2.8554e-05, 2.8614e-05, 2.8673e-05, 2.8733e-05, 2.8792e-05, 2.8852e-05, 2.8911e-05, 2.8971e-05, 2.9030e-05, 2.9090e-05, 2.9149e-05, 2.9209e-05, 2.9268e-05, 2.9328e-05, 2.9387e-05, 2.9447e-05, 2.9506e-05, 2.9566e-05, 2.9625e-05, 2.9685e-05, 2.9744e-05, 2.9804e-05, 2.9863e-05, 2.9923e-05, 2.9982e-05, 3.0042e-05, 3.0101e-05, 3.0161e-05, 3.0220e-05, 3.0280e-05, 3.0339e-05, 3.0398e-05,
|
||||
3.0458e-05, 3.0577e-05, 3.0697e-05, 3.0816e-05, 3.0936e-05, 3.1055e-05, 3.1175e-05, 3.1294e-05, 3.1414e-05, 3.1533e-05, 3.1652e-05, 3.1772e-05, 3.1891e-05, 3.2011e-05, 3.2130e-05, 3.2250e-05, 3.2369e-05, 3.2489e-05, 3.2608e-05, 3.2727e-05, 3.2847e-05, 3.2966e-05, 3.3086e-05, 3.3205e-05, 3.3325e-05, 3.3444e-05, 3.3563e-05, 3.3683e-05, 3.3802e-05, 3.3922e-05, 3.4041e-05, 3.4161e-05, 3.4280e-05, 3.4400e-05, 3.4519e-05, 3.4638e-05, 3.4758e-05, 3.4877e-05, 3.4997e-05, 3.5116e-05, 3.5236e-05, 3.5355e-05, 3.5475e-05, 3.5594e-05, 3.5713e-05, 3.5833e-05, 3.5952e-05, 3.6072e-05, 3.6191e-05, 3.6311e-05, 3.6430e-05, 3.6550e-05, 3.6669e-05, 3.6788e-05, 3.6908e-05, 3.7027e-05, 3.7147e-05, 3.7266e-05, 3.7386e-05, 3.7505e-05, 3.7625e-05, 3.7744e-05, 3.7863e-05, 3.7983e-05, 3.8102e-05, 3.8222e-05, 3.8341e-05, 3.8461e-05, 3.8580e-05, 3.8700e-05, 3.8819e-05, 3.8938e-05, 3.9058e-05, 3.9177e-05, 3.9297e-05, 3.9416e-05, 3.9536e-05, 3.9655e-05, 3.9775e-05, 3.9894e-05, 4.0013e-05, 4.0133e-05, 4.0252e-05, 4.0372e-05, 4.0491e-05, 4.0611e-05, 4.0730e-05, 4.0850e-05, 4.0969e-05, 4.1088e-05, 4.1208e-05, 4.1327e-05, 4.1447e-05, 4.1566e-05, 4.1686e-05, 4.1805e-05, 4.1925e-05, 4.2044e-05, 4.2163e-05, 4.2283e-05, 4.2402e-05, 4.2522e-05, 4.2641e-05, 4.2761e-05, 4.2880e-05, 4.2999e-05, 4.3119e-05, 4.3238e-05, 4.3358e-05, 4.3477e-05, 4.3597e-05, 4.3716e-05, 4.3836e-05, 4.3955e-05, 4.4074e-05, 4.4194e-05, 4.4313e-05, 4.4433e-05, 4.4552e-05, 4.4672e-05, 4.4791e-05, 4.4911e-05, 4.5030e-05, 4.5149e-05, 4.5269e-05, 4.5388e-05, 4.5508e-05, 4.5627e-05, 4.5747e-05, 4.5866e-05, 4.5986e-05, 4.6105e-05, 4.6224e-05, 4.6344e-05, 4.6463e-05, 4.6583e-05, 4.6702e-05, 4.6822e-05, 4.6941e-05, 4.7061e-05, 4.7180e-05, 4.7299e-05, 4.7419e-05, 4.7538e-05, 4.7658e-05, 4.7777e-05, 4.7897e-05, 4.8016e-05, 4.8136e-05, 4.8255e-05, 4.8374e-05, 4.8494e-05, 4.8613e-05, 4.8733e-05, 4.8852e-05, 4.8972e-05, 4.9091e-05, 4.9211e-05, 4.9330e-05, 4.9449e-05, 4.9569e-05, 4.9688e-05, 4.9808e-05, 4.9927e-05, 5.0047e-05, 5.0166e-05, 5.0286e-05, 5.0405e-05, 5.0524e-05, 5.0644e-05, 5.0763e-05, 5.0883e-05, 5.1002e-05, 5.1122e-05, 5.1241e-05, 5.1361e-05, 5.1480e-05, 5.1599e-05, 5.1719e-05, 5.1838e-05, 5.1958e-05, 5.2077e-05, 5.2197e-05, 5.2316e-05, 5.2435e-05, 5.2555e-05, 5.2674e-05, 5.2794e-05, 5.2913e-05, 5.3033e-05, 5.3152e-05, 5.3272e-05, 5.3391e-05, 5.3510e-05, 5.3630e-05, 5.3749e-05, 5.3869e-05, 5.3988e-05, 5.4108e-05, 5.4227e-05, 5.4347e-05, 5.4466e-05, 5.4585e-05, 5.4705e-05, 5.4824e-05, 5.4944e-05, 5.5063e-05, 5.5183e-05, 5.5302e-05, 5.5422e-05, 5.5541e-05, 5.5660e-05, 5.5780e-05, 5.5899e-05, 5.6019e-05, 5.6138e-05, 5.6258e-05, 5.6377e-05, 5.6497e-05, 5.6616e-05, 5.6735e-05, 5.6855e-05, 5.6974e-05, 5.7094e-05, 5.7213e-05, 5.7333e-05, 5.7452e-05, 5.7572e-05, 5.7691e-05, 5.7810e-05, 5.7930e-05, 5.8049e-05, 5.8169e-05, 5.8288e-05, 5.8408e-05, 5.8527e-05, 5.8647e-05, 5.8766e-05, 5.8885e-05, 5.9005e-05, 5.9124e-05, 5.9244e-05, 5.9363e-05, 5.9483e-05, 5.9602e-05, 5.9722e-05, 5.9841e-05, 5.9960e-05, 6.0080e-05, 6.0199e-05, 6.0319e-05, 6.0438e-05, 6.0558e-05, 6.0677e-05, 6.0797e-05, 6.0916e-05,
|
||||
6.1154e-05, 6.1392e-05, 6.1631e-05, 6.1869e-05, 6.2107e-05, 6.2345e-05, 6.2583e-05, 6.2821e-05, 6.3060e-05, 6.3298e-05, 6.3536e-05, 6.3774e-05, 6.4012e-05, 6.4251e-05, 6.4489e-05, 6.4727e-05, 6.4965e-05, 6.5203e-05, 6.5441e-05, 6.5680e-05, 6.5918e-05, 6.6156e-05, 6.6394e-05, 6.6632e-05, 6.6871e-05, 6.7109e-05, 6.7347e-05, 6.7585e-05, 6.7823e-05, 6.8062e-05, 6.8300e-05, 6.8538e-05, 6.8776e-05, 6.9014e-05, 6.9252e-05, 6.9491e-05, 6.9729e-05, 6.9967e-05, 7.0205e-05, 7.0443e-05, 7.0682e-05, 7.0920e-05, 7.1158e-05, 7.1396e-05, 7.1634e-05, 7.1872e-05, 7.2111e-05, 7.2349e-05, 7.2587e-05, 7.2825e-05, 7.3063e-05, 7.3302e-05, 7.3540e-05, 7.3778e-05, 7.4016e-05, 7.4254e-05, 7.4493e-05, 7.4731e-05, 7.4969e-05, 7.5207e-05, 7.5445e-05, 7.5683e-05, 7.5922e-05, 7.6160e-05, 7.6398e-05, 7.6636e-05, 7.6874e-05, 7.7113e-05, 7.7351e-05, 7.7589e-05, 7.7827e-05, 7.8065e-05, 7.8304e-05, 7.8542e-05, 7.8780e-05, 7.9018e-05, 7.9256e-05, 7.9494e-05, 7.9733e-05, 7.9971e-05, 8.0209e-05, 8.0447e-05, 8.0685e-05, 8.0924e-05, 8.1162e-05, 8.1400e-05, 8.1638e-05, 8.1876e-05, 8.2114e-05, 8.2353e-05, 8.2591e-05, 8.2829e-05, 8.3067e-05, 8.3305e-05, 8.3544e-05, 8.3782e-05, 8.4020e-05, 8.4258e-05, 8.4496e-05, 8.4735e-05, 8.4973e-05, 8.5211e-05, 8.5449e-05, 8.5687e-05, 8.5925e-05, 8.6164e-05, 8.6402e-05, 8.6640e-05, 8.6878e-05, 8.7116e-05, 8.7355e-05, 8.7593e-05, 8.7831e-05, 8.8069e-05, 8.8307e-05, 8.8545e-05, 8.8784e-05, 8.9022e-05, 8.9260e-05, 8.9498e-05, 8.9736e-05, 8.9975e-05, 9.0213e-05, 9.0451e-05, 9.0689e-05, 9.0927e-05, 9.1166e-05, 9.1404e-05, 9.1642e-05, 9.1880e-05, 9.2118e-05, 9.2356e-05, 9.2595e-05, 9.2833e-05, 9.3071e-05, 9.3309e-05, 9.3547e-05, 9.3786e-05, 9.4024e-05, 9.4262e-05, 9.4500e-05, 9.4738e-05, 9.4977e-05, 9.5215e-05, 9.5453e-05, 9.5691e-05, 9.5929e-05, 9.6167e-05, 9.6406e-05, 9.6644e-05, 9.6882e-05, 9.7120e-05, 9.7358e-05, 9.7597e-05, 9.7835e-05, 9.8073e-05, 9.8311e-05, 9.8549e-05, 9.8787e-05, 9.9026e-05, 9.9264e-05, 9.9502e-05, 9.9740e-05, 9.9978e-05, 1.0022e-04, 1.0045e-04, 1.0069e-04, 1.0093e-04, 1.0117e-04, 1.0141e-04, 1.0165e-04, 1.0188e-04, 1.0212e-04, 1.0236e-04, 1.0260e-04, 1.0284e-04, 1.0307e-04, 1.0331e-04, 1.0355e-04, 1.0379e-04, 1.0403e-04, 1.0427e-04, 1.0450e-04, 1.0474e-04, 1.0498e-04, 1.0522e-04, 1.0546e-04, 1.0569e-04, 1.0593e-04, 1.0617e-04, 1.0641e-04, 1.0665e-04, 1.0689e-04, 1.0712e-04, 1.0736e-04, 1.0760e-04, 1.0784e-04, 1.0808e-04, 1.0831e-04, 1.0855e-04, 1.0879e-04, 1.0903e-04, 1.0927e-04, 1.0951e-04, 1.0974e-04, 1.0998e-04, 1.1022e-04, 1.1046e-04, 1.1070e-04, 1.1093e-04, 1.1117e-04, 1.1141e-04, 1.1165e-04, 1.1189e-04, 1.1213e-04, 1.1236e-04, 1.1260e-04, 1.1284e-04, 1.1308e-04, 1.1332e-04, 1.1355e-04, 1.1379e-04, 1.1403e-04, 1.1427e-04, 1.1451e-04, 1.1475e-04, 1.1498e-04, 1.1522e-04, 1.1546e-04, 1.1570e-04, 1.1594e-04, 1.1618e-04, 1.1641e-04, 1.1665e-04, 1.1689e-04, 1.1713e-04, 1.1737e-04, 1.1760e-04, 1.1784e-04, 1.1808e-04, 1.1832e-04, 1.1856e-04, 1.1880e-04, 1.1903e-04, 1.1927e-04, 1.1951e-04, 1.1975e-04, 1.1999e-04, 1.2022e-04, 1.2046e-04, 1.2070e-04, 1.2094e-04, 1.2118e-04, 1.2142e-04, 1.2165e-04, 1.2189e-04,
|
||||
1.2213e-04, 1.2237e-04, 1.2261e-04, 1.2284e-04, 1.2308e-04, 1.2332e-04, 1.2356e-04, 1.2380e-04, 1.2404e-04, 1.2427e-04, 1.2451e-04, 1.2475e-04, 1.2499e-04, 1.2523e-04, 1.2546e-04, 1.2570e-04, 1.2594e-04, 1.2618e-04, 1.2642e-04, 1.2666e-04, 1.2689e-04, 1.2713e-04, 1.2737e-04, 1.2761e-04, 1.2785e-04, 1.2808e-04, 1.2832e-04, 1.2856e-04, 1.2880e-04, 1.2904e-04, 1.2928e-04, 1.2951e-04, 1.2975e-04, 1.2999e-04, 1.3023e-04, 1.3047e-04, 1.3070e-04, 1.3094e-04, 1.3118e-04, 1.3142e-04, 1.3166e-04, 1.3190e-04, 1.3213e-04, 1.3237e-04, 1.3261e-04, 1.3285e-04, 1.3309e-04, 1.3332e-04, 1.3356e-04, 1.3380e-04, 1.3404e-04, 1.3428e-04, 1.3452e-04, 1.3475e-04, 1.3499e-04, 1.3523e-04, 1.3547e-04, 1.3571e-04, 1.3594e-04, 1.3618e-04, 1.3642e-04, 1.3666e-04, 1.3690e-04, 1.3714e-04, 1.3737e-04, 1.3761e-04, 1.3785e-04, 1.3809e-04, 1.3833e-04, 1.3856e-04, 1.3880e-04, 1.3904e-04, 1.3928e-04, 1.3952e-04, 1.3976e-04, 1.3999e-04, 1.4023e-04, 1.4047e-04, 1.4071e-04, 1.4095e-04, 1.4118e-04, 1.4142e-04, 1.4166e-04, 1.4190e-04, 1.4214e-04, 1.4238e-04, 1.4261e-04, 1.4285e-04, 1.4309e-04, 1.4333e-04, 1.4357e-04, 1.4380e-04, 1.4404e-04, 1.4428e-04, 1.4452e-04, 1.4476e-04, 1.4500e-04, 1.4523e-04, 1.4547e-04, 1.4571e-04, 1.4595e-04, 1.4619e-04, 1.4642e-04, 1.4666e-04, 1.4690e-04, 1.4714e-04, 1.4738e-04, 1.4762e-04, 1.4785e-04, 1.4809e-04, 1.4833e-04, 1.4857e-04, 1.4881e-04, 1.4904e-04, 1.4928e-04, 1.4952e-04, 1.4976e-04, 1.5000e-04, 1.5024e-04, 1.5047e-04, 1.5071e-04, 1.5095e-04, 1.5119e-04, 1.5143e-04, 1.5166e-04, 1.5190e-04, 1.5214e-04, 1.5238e-04, 1.5262e-04, 1.5286e-04, 1.5309e-04, 1.5333e-04, 1.5357e-04, 1.5381e-04, 1.5405e-04, 1.5428e-04, 1.5452e-04, 1.5476e-04, 1.5500e-04, 1.5524e-04, 1.5548e-04, 1.5571e-04, 1.5595e-04, 1.5619e-04, 1.5643e-04, 1.5667e-04, 1.5690e-04, 1.5714e-04, 1.5738e-04, 1.5762e-04, 1.5786e-04, 1.5810e-04, 1.5833e-04, 1.5857e-04, 1.5881e-04, 1.5905e-04, 1.5929e-04, 1.5952e-04, 1.5976e-04, 1.6000e-04, 1.6024e-04, 1.6048e-04, 1.6072e-04, 1.6095e-04, 1.6119e-04, 1.6143e-04, 1.6167e-04, 1.6191e-04, 1.6214e-04, 1.6238e-04, 1.6262e-04, 1.6286e-04, 1.6310e-04, 1.6334e-04, 1.6357e-04, 1.6381e-04, 1.6405e-04, 1.6429e-04, 1.6453e-04, 1.6476e-04, 1.6500e-04, 1.6524e-04, 1.6548e-04, 1.6572e-04, 1.6596e-04, 1.6619e-04, 1.6643e-04, 1.6667e-04, 1.6691e-04, 1.6715e-04, 1.6738e-04, 1.6762e-04, 1.6786e-04, 1.6810e-04, 1.6834e-04, 1.6858e-04, 1.6881e-04, 1.6905e-04, 1.6929e-04, 1.6953e-04, 1.6977e-04, 1.7001e-04, 1.7024e-04, 1.7048e-04, 1.7072e-04, 1.7096e-04, 1.7120e-04, 1.7143e-04, 1.7167e-04, 1.7191e-04, 1.7215e-04, 1.7239e-04, 1.7263e-04, 1.7286e-04, 1.7310e-04, 1.7334e-04, 1.7358e-04, 1.7382e-04, 1.7405e-04, 1.7429e-04, 1.7453e-04, 1.7477e-04, 1.7501e-04, 1.7525e-04, 1.7548e-04, 1.7572e-04, 1.7596e-04, 1.7620e-04, 1.7644e-04, 1.7667e-04, 1.7691e-04, 1.7715e-04, 1.7739e-04, 1.7763e-04, 1.7787e-04, 1.7810e-04, 1.7834e-04, 1.7858e-04, 1.7882e-04, 1.7906e-04, 1.7929e-04, 1.7953e-04, 1.7977e-04, 1.8001e-04, 1.8025e-04, 1.8049e-04, 1.8072e-04, 1.8096e-04, 1.8120e-04, 1.8144e-04, 1.8168e-04, 1.8191e-04, 1.8215e-04, 1.8239e-04, 1.8263e-04, 1.8287e-04,
|
||||
1.8311e-04, 1.8334e-04, 1.8358e-04, 1.8382e-04, 1.8406e-04, 1.8430e-04, 1.8453e-04, 1.8477e-04, 1.8501e-04, 1.8525e-04, 1.8549e-04, 1.8573e-04, 1.8596e-04, 1.8620e-04, 1.8644e-04, 1.8668e-04, 1.8692e-04, 1.8715e-04, 1.8739e-04, 1.8763e-04, 1.8787e-04, 1.8811e-04, 1.8835e-04, 1.8858e-04, 1.8882e-04, 1.8906e-04, 1.8930e-04, 1.8954e-04, 1.8977e-04, 1.9001e-04, 1.9025e-04, 1.9049e-04, 1.9073e-04, 1.9097e-04, 1.9120e-04, 1.9144e-04, 1.9168e-04, 1.9192e-04, 1.9216e-04, 1.9239e-04, 1.9263e-04, 1.9287e-04, 1.9311e-04, 1.9335e-04, 1.9359e-04, 1.9382e-04, 1.9406e-04, 1.9430e-04, 1.9454e-04, 1.9478e-04, 1.9501e-04, 1.9525e-04, 1.9549e-04, 1.9573e-04, 1.9597e-04, 1.9621e-04, 1.9644e-04, 1.9668e-04, 1.9692e-04, 1.9716e-04, 1.9740e-04, 1.9763e-04, 1.9787e-04, 1.9811e-04, 1.9835e-04, 1.9859e-04, 1.9883e-04, 1.9906e-04, 1.9930e-04, 1.9954e-04, 1.9978e-04, 2.0002e-04, 2.0025e-04, 2.0049e-04, 2.0073e-04, 2.0097e-04, 2.0121e-04, 2.0145e-04, 2.0168e-04, 2.0192e-04, 2.0216e-04, 2.0240e-04, 2.0264e-04, 2.0287e-04, 2.0311e-04, 2.0335e-04, 2.0359e-04, 2.0383e-04, 2.0407e-04, 2.0430e-04, 2.0454e-04, 2.0478e-04, 2.0502e-04, 2.0526e-04, 2.0549e-04, 2.0573e-04, 2.0597e-04, 2.0621e-04, 2.0645e-04, 2.0669e-04, 2.0692e-04, 2.0716e-04, 2.0740e-04, 2.0764e-04, 2.0788e-04, 2.0811e-04, 2.0835e-04, 2.0859e-04, 2.0883e-04, 2.0907e-04, 2.0931e-04, 2.0954e-04, 2.0978e-04, 2.1002e-04, 2.1026e-04, 2.1050e-04, 2.1073e-04, 2.1097e-04, 2.1121e-04, 2.1145e-04, 2.1169e-04, 2.1193e-04, 2.1216e-04, 2.1240e-04, 2.1264e-04, 2.1288e-04, 2.1312e-04, 2.1335e-04, 2.1359e-04, 2.1383e-04, 2.1407e-04, 2.1431e-04, 2.1455e-04, 2.1478e-04, 2.1502e-04, 2.1526e-04, 2.1550e-04, 2.1574e-04, 2.1597e-04, 2.1621e-04, 2.1645e-04, 2.1669e-04, 2.1693e-04, 2.1717e-04, 2.1740e-04, 2.1764e-04, 2.1788e-04, 2.1812e-04, 2.1836e-04, 2.1859e-04, 2.1883e-04, 2.1907e-04, 2.1931e-04, 2.1955e-04, 2.1979e-04, 2.2002e-04, 2.2026e-04, 2.2050e-04, 2.2074e-04, 2.2098e-04, 2.2121e-04, 2.2145e-04, 2.2169e-04, 2.2193e-04, 2.2217e-04, 2.2241e-04, 2.2264e-04, 2.2288e-04, 2.2312e-04, 2.2336e-04, 2.2360e-04, 2.2383e-04, 2.2407e-04, 2.2431e-04, 2.2455e-04, 2.2479e-04, 2.2503e-04, 2.2526e-04, 2.2550e-04, 2.2574e-04, 2.2598e-04, 2.2622e-04, 2.2646e-04, 2.2669e-04, 2.2693e-04, 2.2717e-04, 2.2741e-04, 2.2765e-04, 2.2788e-04, 2.2812e-04, 2.2836e-04, 2.2860e-04, 2.2884e-04, 2.2908e-04, 2.2931e-04, 2.2955e-04, 2.2979e-04, 2.3003e-04, 2.3027e-04, 2.3050e-04, 2.3074e-04, 2.3098e-04, 2.3122e-04, 2.3146e-04, 2.3170e-04, 2.3193e-04, 2.3217e-04, 2.3241e-04, 2.3265e-04, 2.3289e-04, 2.3312e-04, 2.3336e-04, 2.3360e-04, 2.3384e-04, 2.3408e-04, 2.3432e-04, 2.3455e-04, 2.3479e-04, 2.3503e-04, 2.3527e-04, 2.3551e-04, 2.3574e-04, 2.3598e-04, 2.3622e-04, 2.3646e-04, 2.3670e-04, 2.3694e-04, 2.3717e-04, 2.3741e-04, 2.3765e-04, 2.3789e-04, 2.3813e-04, 2.3836e-04, 2.3860e-04, 2.3884e-04, 2.3908e-04, 2.3932e-04, 2.3956e-04, 2.3979e-04, 2.4003e-04, 2.4027e-04, 2.4051e-04, 2.4075e-04, 2.4098e-04, 2.4122e-04, 2.4146e-04, 2.4170e-04, 2.4194e-04, 2.4218e-04, 2.4241e-04, 2.4265e-04, 2.4289e-04, 2.4313e-04, 2.4337e-04, 2.4360e-04, 2.4384e-04,
|
||||
2.4480e-04, 2.4575e-04, 2.4670e-04, 2.4766e-04, 2.4861e-04, 2.4956e-04, 2.5052e-04, 2.5147e-04, 2.5242e-04, 2.5337e-04, 2.5433e-04, 2.5528e-04, 2.5623e-04, 2.5719e-04, 2.5814e-04, 2.5909e-04, 2.6005e-04, 2.6100e-04, 2.6195e-04, 2.6291e-04, 2.6386e-04, 2.6481e-04, 2.6577e-04, 2.6672e-04, 2.6767e-04, 2.6863e-04, 2.6958e-04, 2.7053e-04, 2.7149e-04, 2.7244e-04, 2.7339e-04, 2.7435e-04, 2.7530e-04, 2.7625e-04, 2.7720e-04, 2.7816e-04, 2.7911e-04, 2.8006e-04, 2.8102e-04, 2.8197e-04, 2.8292e-04, 2.8388e-04, 2.8483e-04, 2.8578e-04, 2.8674e-04, 2.8769e-04, 2.8864e-04, 2.8960e-04, 2.9055e-04, 2.9150e-04, 2.9246e-04, 2.9341e-04, 2.9436e-04, 2.9532e-04, 2.9627e-04, 2.9722e-04, 2.9818e-04, 2.9913e-04, 3.0008e-04, 3.0104e-04, 3.0199e-04, 3.0294e-04, 3.0389e-04, 3.0485e-04, 3.0580e-04, 3.0675e-04, 3.0771e-04, 3.0866e-04, 3.0961e-04, 3.1057e-04, 3.1152e-04, 3.1247e-04, 3.1343e-04, 3.1438e-04, 3.1533e-04, 3.1629e-04, 3.1724e-04, 3.1819e-04, 3.1915e-04, 3.2010e-04, 3.2105e-04, 3.2201e-04, 3.2296e-04, 3.2391e-04, 3.2487e-04, 3.2582e-04, 3.2677e-04, 3.2772e-04, 3.2868e-04, 3.2963e-04, 3.3058e-04, 3.3154e-04, 3.3249e-04, 3.3344e-04, 3.3440e-04, 3.3535e-04, 3.3630e-04, 3.3726e-04, 3.3821e-04, 3.3916e-04, 3.4012e-04, 3.4107e-04, 3.4202e-04, 3.4298e-04, 3.4393e-04, 3.4488e-04, 3.4584e-04, 3.4679e-04, 3.4774e-04, 3.4870e-04, 3.4965e-04, 3.5060e-04, 3.5156e-04, 3.5251e-04, 3.5346e-04, 3.5441e-04, 3.5537e-04, 3.5632e-04, 3.5727e-04, 3.5823e-04, 3.5918e-04, 3.6013e-04, 3.6109e-04, 3.6204e-04, 3.6299e-04, 3.6395e-04, 3.6490e-04, 3.6585e-04, 3.6681e-04, 3.6776e-04, 3.6871e-04, 3.6967e-04, 3.7062e-04, 3.7157e-04, 3.7253e-04, 3.7348e-04, 3.7443e-04, 3.7539e-04, 3.7634e-04, 3.7729e-04, 3.7825e-04, 3.7920e-04, 3.8015e-04, 3.8110e-04, 3.8206e-04, 3.8301e-04, 3.8396e-04, 3.8492e-04, 3.8587e-04, 3.8682e-04, 3.8778e-04, 3.8873e-04, 3.8968e-04, 3.9064e-04, 3.9159e-04, 3.9254e-04, 3.9350e-04, 3.9445e-04, 3.9540e-04, 3.9636e-04, 3.9731e-04, 3.9826e-04, 3.9922e-04, 4.0017e-04, 4.0112e-04, 4.0208e-04, 4.0303e-04, 4.0398e-04, 4.0493e-04, 4.0589e-04, 4.0684e-04, 4.0779e-04, 4.0875e-04, 4.0970e-04, 4.1065e-04, 4.1161e-04, 4.1256e-04, 4.1351e-04, 4.1447e-04, 4.1542e-04, 4.1637e-04, 4.1733e-04, 4.1828e-04, 4.1923e-04, 4.2019e-04, 4.2114e-04, 4.2209e-04, 4.2305e-04, 4.2400e-04, 4.2495e-04, 4.2591e-04, 4.2686e-04, 4.2781e-04, 4.2877e-04, 4.2972e-04, 4.3067e-04, 4.3162e-04, 4.3258e-04, 4.3353e-04, 4.3448e-04, 4.3544e-04, 4.3639e-04, 4.3734e-04, 4.3830e-04, 4.3925e-04, 4.4020e-04, 4.4116e-04, 4.4211e-04, 4.4306e-04, 4.4402e-04, 4.4497e-04, 4.4592e-04, 4.4688e-04, 4.4783e-04, 4.4878e-04, 4.4974e-04, 4.5069e-04, 4.5164e-04, 4.5260e-04, 4.5355e-04, 4.5450e-04, 4.5545e-04, 4.5641e-04, 4.5736e-04, 4.5831e-04, 4.5927e-04, 4.6022e-04, 4.6117e-04, 4.6213e-04, 4.6308e-04, 4.6403e-04, 4.6499e-04, 4.6594e-04, 4.6689e-04, 4.6785e-04, 4.6880e-04, 4.6975e-04, 4.7071e-04, 4.7166e-04, 4.7261e-04, 4.7357e-04, 4.7452e-04, 4.7547e-04, 4.7643e-04, 4.7738e-04, 4.7833e-04, 4.7929e-04, 4.8024e-04, 4.8119e-04, 4.8214e-04, 4.8310e-04, 4.8405e-04, 4.8500e-04, 4.8596e-04, 4.8691e-04, 4.8786e-04,
|
||||
4.8977e-04, 4.9168e-04, 4.9358e-04, 4.9549e-04, 4.9740e-04, 4.9931e-04, 5.0121e-04, 5.0312e-04, 5.0503e-04, 5.0693e-04, 5.0884e-04, 5.1075e-04, 5.1265e-04, 5.1456e-04, 5.1647e-04, 5.1837e-04, 5.2028e-04, 5.2219e-04, 5.2409e-04, 5.2600e-04, 5.2791e-04, 5.2982e-04, 5.3172e-04, 5.3363e-04, 5.3554e-04, 5.3744e-04, 5.3935e-04, 5.4126e-04, 5.4316e-04, 5.4507e-04, 5.4698e-04, 5.4888e-04, 5.5079e-04, 5.5270e-04, 5.5460e-04, 5.5651e-04, 5.5842e-04, 5.6033e-04, 5.6223e-04, 5.6414e-04, 5.6605e-04, 5.6795e-04, 5.6986e-04, 5.7177e-04, 5.7367e-04, 5.7558e-04, 5.7749e-04, 5.7939e-04, 5.8130e-04, 5.8321e-04, 5.8512e-04, 5.8702e-04, 5.8893e-04, 5.9084e-04, 5.9274e-04, 5.9465e-04, 5.9656e-04, 5.9846e-04, 6.0037e-04, 6.0228e-04, 6.0418e-04, 6.0609e-04, 6.0800e-04, 6.0990e-04, 6.1181e-04, 6.1372e-04, 6.1563e-04, 6.1753e-04, 6.1944e-04, 6.2135e-04, 6.2325e-04, 6.2516e-04, 6.2707e-04, 6.2897e-04, 6.3088e-04, 6.3279e-04, 6.3469e-04, 6.3660e-04, 6.3851e-04, 6.4041e-04, 6.4232e-04, 6.4423e-04, 6.4614e-04, 6.4804e-04, 6.4995e-04, 6.5186e-04, 6.5376e-04, 6.5567e-04, 6.5758e-04, 6.5948e-04, 6.6139e-04, 6.6330e-04, 6.6520e-04, 6.6711e-04, 6.6902e-04, 6.7092e-04, 6.7283e-04, 6.7474e-04, 6.7665e-04, 6.7855e-04, 6.8046e-04, 6.8237e-04, 6.8427e-04, 6.8618e-04, 6.8809e-04, 6.8999e-04, 6.9190e-04, 6.9381e-04, 6.9571e-04, 6.9762e-04, 6.9953e-04, 7.0143e-04, 7.0334e-04, 7.0525e-04, 7.0716e-04, 7.0906e-04, 7.1097e-04, 7.1288e-04, 7.1478e-04, 7.1669e-04, 7.1860e-04, 7.2050e-04, 7.2241e-04, 7.2432e-04, 7.2622e-04, 7.2813e-04, 7.3004e-04, 7.3195e-04, 7.3385e-04, 7.3576e-04, 7.3767e-04, 7.3957e-04, 7.4148e-04, 7.4339e-04, 7.4529e-04, 7.4720e-04, 7.4911e-04, 7.5101e-04, 7.5292e-04, 7.5483e-04, 7.5673e-04, 7.5864e-04, 7.6055e-04, 7.6246e-04, 7.6436e-04, 7.6627e-04, 7.6818e-04, 7.7008e-04, 7.7199e-04, 7.7390e-04, 7.7580e-04, 7.7771e-04, 7.7962e-04, 7.8152e-04, 7.8343e-04, 7.8534e-04, 7.8724e-04, 7.8915e-04, 7.9106e-04, 7.9297e-04, 7.9487e-04, 7.9678e-04, 7.9869e-04, 8.0059e-04, 8.0250e-04, 8.0441e-04, 8.0631e-04, 8.0822e-04, 8.1013e-04, 8.1203e-04, 8.1394e-04, 8.1585e-04, 8.1775e-04, 8.1966e-04, 8.2157e-04, 8.2348e-04, 8.2538e-04, 8.2729e-04, 8.2920e-04, 8.3110e-04, 8.3301e-04, 8.3492e-04, 8.3682e-04, 8.3873e-04, 8.4064e-04, 8.4254e-04, 8.4445e-04, 8.4636e-04, 8.4826e-04, 8.5017e-04, 8.5208e-04, 8.5399e-04, 8.5589e-04, 8.5780e-04, 8.5971e-04, 8.6161e-04, 8.6352e-04, 8.6543e-04, 8.6733e-04, 8.6924e-04, 8.7115e-04, 8.7305e-04, 8.7496e-04, 8.7687e-04, 8.7878e-04, 8.8068e-04, 8.8259e-04, 8.8450e-04, 8.8640e-04, 8.8831e-04, 8.9022e-04, 8.9212e-04, 8.9403e-04, 8.9594e-04, 8.9784e-04, 8.9975e-04, 9.0166e-04, 9.0356e-04, 9.0547e-04, 9.0738e-04, 9.0929e-04, 9.1119e-04, 9.1310e-04, 9.1501e-04, 9.1691e-04, 9.1882e-04, 9.2073e-04, 9.2263e-04, 9.2454e-04, 9.2645e-04, 9.2835e-04, 9.3026e-04, 9.3217e-04, 9.3407e-04, 9.3598e-04, 9.3789e-04, 9.3980e-04, 9.4170e-04, 9.4361e-04, 9.4552e-04, 9.4742e-04, 9.4933e-04, 9.5124e-04, 9.5314e-04, 9.5505e-04, 9.5696e-04, 9.5886e-04, 9.6077e-04, 9.6268e-04, 9.6458e-04, 9.6649e-04, 9.6840e-04, 9.7031e-04, 9.7221e-04, 9.7412e-04, 9.7603e-04,
|
||||
9.7984e-04, 9.8365e-04, 9.8747e-04, 9.9128e-04, 9.9510e-04, 9.9891e-04, 1.0027e-03, 1.0065e-03, 1.0104e-03, 1.0142e-03, 1.0180e-03, 1.0218e-03, 1.0256e-03, 1.0294e-03, 1.0332e-03, 1.0371e-03, 1.0409e-03, 1.0447e-03, 1.0485e-03, 1.0523e-03, 1.0561e-03, 1.0599e-03, 1.0638e-03, 1.0676e-03, 1.0714e-03, 1.0752e-03, 1.0790e-03, 1.0828e-03, 1.0866e-03, 1.0905e-03, 1.0943e-03, 1.0981e-03, 1.1019e-03, 1.1057e-03, 1.1095e-03, 1.1133e-03, 1.1172e-03, 1.1210e-03, 1.1248e-03, 1.1286e-03, 1.1324e-03, 1.1362e-03, 1.1400e-03, 1.1439e-03, 1.1477e-03, 1.1515e-03, 1.1553e-03, 1.1591e-03, 1.1629e-03, 1.1667e-03, 1.1706e-03, 1.1744e-03, 1.1782e-03, 1.1820e-03, 1.1858e-03, 1.1896e-03, 1.1934e-03, 1.1973e-03, 1.2011e-03, 1.2049e-03, 1.2087e-03, 1.2125e-03, 1.2163e-03, 1.2201e-03, 1.2240e-03, 1.2278e-03, 1.2316e-03, 1.2354e-03, 1.2392e-03, 1.2430e-03, 1.2468e-03, 1.2507e-03, 1.2545e-03, 1.2583e-03, 1.2621e-03, 1.2659e-03, 1.2697e-03, 1.2735e-03, 1.2774e-03, 1.2812e-03, 1.2850e-03, 1.2888e-03, 1.2926e-03, 1.2964e-03, 1.3002e-03, 1.3040e-03, 1.3079e-03, 1.3117e-03, 1.3155e-03, 1.3193e-03, 1.3231e-03, 1.3269e-03, 1.3307e-03, 1.3346e-03, 1.3384e-03, 1.3422e-03, 1.3460e-03, 1.3498e-03, 1.3536e-03, 1.3574e-03, 1.3613e-03, 1.3651e-03, 1.3689e-03, 1.3727e-03, 1.3765e-03, 1.3803e-03, 1.3841e-03, 1.3880e-03, 1.3918e-03, 1.3956e-03, 1.3994e-03, 1.4032e-03, 1.4070e-03, 1.4108e-03, 1.4147e-03, 1.4185e-03, 1.4223e-03, 1.4261e-03, 1.4299e-03, 1.4337e-03, 1.4375e-03, 1.4414e-03, 1.4452e-03, 1.4490e-03, 1.4528e-03, 1.4566e-03, 1.4604e-03, 1.4642e-03, 1.4681e-03, 1.4719e-03, 1.4757e-03, 1.4795e-03, 1.4833e-03, 1.4871e-03, 1.4909e-03, 1.4948e-03, 1.4986e-03, 1.5024e-03, 1.5062e-03, 1.5100e-03, 1.5138e-03, 1.5176e-03, 1.5215e-03, 1.5253e-03, 1.5291e-03, 1.5329e-03, 1.5367e-03, 1.5405e-03, 1.5443e-03, 1.5482e-03, 1.5520e-03, 1.5558e-03, 1.5596e-03, 1.5634e-03, 1.5672e-03, 1.5710e-03, 1.5749e-03, 1.5787e-03, 1.5825e-03, 1.5863e-03, 1.5901e-03, 1.5939e-03, 1.5977e-03, 1.6016e-03, 1.6054e-03, 1.6092e-03, 1.6130e-03, 1.6168e-03, 1.6206e-03, 1.6244e-03, 1.6283e-03, 1.6321e-03, 1.6359e-03, 1.6397e-03, 1.6435e-03, 1.6473e-03, 1.6511e-03, 1.6550e-03, 1.6588e-03, 1.6626e-03, 1.6664e-03, 1.6702e-03, 1.6740e-03, 1.6778e-03, 1.6817e-03, 1.6855e-03, 1.6893e-03, 1.6931e-03, 1.6969e-03, 1.7007e-03, 1.7045e-03, 1.7084e-03, 1.7122e-03, 1.7160e-03, 1.7198e-03, 1.7236e-03, 1.7274e-03, 1.7312e-03, 1.7351e-03, 1.7389e-03, 1.7427e-03, 1.7465e-03, 1.7503e-03, 1.7541e-03, 1.7579e-03, 1.7618e-03, 1.7656e-03, 1.7694e-03, 1.7732e-03, 1.7770e-03, 1.7808e-03, 1.7846e-03, 1.7885e-03, 1.7923e-03, 1.7961e-03, 1.7999e-03, 1.8037e-03, 1.8075e-03, 1.8113e-03, 1.8152e-03, 1.8190e-03, 1.8228e-03, 1.8266e-03, 1.8304e-03, 1.8342e-03, 1.8380e-03, 1.8419e-03, 1.8457e-03, 1.8495e-03, 1.8533e-03, 1.8571e-03, 1.8609e-03, 1.8647e-03, 1.8686e-03, 1.8724e-03, 1.8762e-03, 1.8800e-03, 1.8838e-03, 1.8876e-03, 1.8914e-03, 1.8953e-03, 1.8991e-03, 1.9029e-03, 1.9067e-03, 1.9105e-03, 1.9143e-03, 1.9181e-03, 1.9220e-03, 1.9258e-03, 1.9296e-03, 1.9334e-03, 1.9372e-03, 1.9410e-03, 1.9448e-03, 1.9487e-03, 1.9525e-03,
|
||||
1.9601e-03, 1.9677e-03, 1.9754e-03, 1.9830e-03, 1.9906e-03, 1.9982e-03, 2.0059e-03, 2.0135e-03, 2.0211e-03, 2.0288e-03, 2.0364e-03, 2.0440e-03, 2.0516e-03, 2.0593e-03, 2.0669e-03, 2.0745e-03, 2.0822e-03, 2.0898e-03, 2.0974e-03, 2.1050e-03, 2.1127e-03, 2.1203e-03, 2.1279e-03, 2.1356e-03, 2.1432e-03, 2.1508e-03, 2.1585e-03, 2.1661e-03, 2.1737e-03, 2.1813e-03, 2.1890e-03, 2.1966e-03, 2.2042e-03, 2.2119e-03, 2.2195e-03, 2.2271e-03, 2.2347e-03, 2.2424e-03, 2.2500e-03, 2.2576e-03, 2.2653e-03, 2.2729e-03, 2.2805e-03, 2.2881e-03, 2.2958e-03, 2.3034e-03, 2.3110e-03, 2.3187e-03, 2.3263e-03, 2.3339e-03, 2.3415e-03, 2.3492e-03, 2.3568e-03, 2.3644e-03, 2.3721e-03, 2.3797e-03, 2.3873e-03, 2.3949e-03, 2.4026e-03, 2.4102e-03, 2.4178e-03, 2.4255e-03, 2.4331e-03, 2.4407e-03, 2.4483e-03, 2.4560e-03, 2.4636e-03, 2.4712e-03, 2.4789e-03, 2.4865e-03, 2.4941e-03, 2.5018e-03, 2.5094e-03, 2.5170e-03, 2.5246e-03, 2.5323e-03, 2.5399e-03, 2.5475e-03, 2.5552e-03, 2.5628e-03, 2.5704e-03, 2.5780e-03, 2.5857e-03, 2.5933e-03, 2.6009e-03, 2.6086e-03, 2.6162e-03, 2.6238e-03, 2.6314e-03, 2.6391e-03, 2.6467e-03, 2.6543e-03, 2.6620e-03, 2.6696e-03, 2.6772e-03, 2.6848e-03, 2.6925e-03, 2.7001e-03, 2.7077e-03, 2.7154e-03, 2.7230e-03, 2.7306e-03, 2.7382e-03, 2.7459e-03, 2.7535e-03, 2.7611e-03, 2.7688e-03, 2.7764e-03, 2.7840e-03, 2.7917e-03, 2.7993e-03, 2.8069e-03, 2.8145e-03, 2.8222e-03, 2.8298e-03, 2.8374e-03, 2.8451e-03, 2.8527e-03, 2.8603e-03, 2.8679e-03, 2.8756e-03, 2.8832e-03, 2.8908e-03, 2.8985e-03, 2.9061e-03, 2.9137e-03, 2.9213e-03, 2.9290e-03, 2.9366e-03, 2.9442e-03, 2.9519e-03, 2.9595e-03, 2.9671e-03, 2.9747e-03, 2.9824e-03, 2.9900e-03, 2.9976e-03, 3.0053e-03, 3.0129e-03, 3.0205e-03, 3.0281e-03, 3.0358e-03, 3.0434e-03, 3.0510e-03, 3.0587e-03, 3.0663e-03, 3.0739e-03, 3.0816e-03, 3.0892e-03, 3.0968e-03, 3.1044e-03, 3.1121e-03, 3.1197e-03, 3.1273e-03, 3.1350e-03, 3.1426e-03, 3.1502e-03, 3.1578e-03, 3.1655e-03, 3.1731e-03, 3.1807e-03, 3.1884e-03, 3.1960e-03, 3.2036e-03, 3.2112e-03, 3.2189e-03, 3.2265e-03, 3.2341e-03, 3.2418e-03, 3.2494e-03, 3.2570e-03, 3.2646e-03, 3.2723e-03, 3.2799e-03, 3.2875e-03, 3.2952e-03, 3.3028e-03, 3.3104e-03, 3.3180e-03, 3.3257e-03, 3.3333e-03, 3.3409e-03, 3.3486e-03, 3.3562e-03, 3.3638e-03, 3.3715e-03, 3.3791e-03, 3.3867e-03, 3.3943e-03, 3.4020e-03, 3.4096e-03, 3.4172e-03, 3.4249e-03, 3.4325e-03, 3.4401e-03, 3.4477e-03, 3.4554e-03, 3.4630e-03, 3.4706e-03, 3.4783e-03, 3.4859e-03, 3.4935e-03, 3.5011e-03, 3.5088e-03, 3.5164e-03, 3.5240e-03, 3.5317e-03, 3.5393e-03, 3.5469e-03, 3.5545e-03, 3.5622e-03, 3.5698e-03, 3.5774e-03, 3.5851e-03, 3.5927e-03, 3.6003e-03, 3.6079e-03, 3.6156e-03, 3.6232e-03, 3.6308e-03, 3.6385e-03, 3.6461e-03, 3.6537e-03, 3.6613e-03, 3.6690e-03, 3.6766e-03, 3.6842e-03, 3.6919e-03, 3.6995e-03, 3.7071e-03, 3.7148e-03, 3.7224e-03, 3.7300e-03, 3.7376e-03, 3.7453e-03, 3.7529e-03, 3.7605e-03, 3.7682e-03, 3.7758e-03, 3.7834e-03, 3.7910e-03, 3.7987e-03, 3.8063e-03, 3.8139e-03, 3.8216e-03, 3.8292e-03, 3.8368e-03, 3.8444e-03, 3.8521e-03, 3.8597e-03, 3.8673e-03, 3.8750e-03, 3.8826e-03, 3.8902e-03, 3.8978e-03, 3.9055e-03,
|
||||
3.9207e-03, 3.9360e-03, 3.9513e-03, 3.9665e-03, 3.9818e-03, 3.9970e-03, 4.0123e-03, 4.0275e-03, 4.0428e-03, 4.0581e-03, 4.0733e-03, 4.0886e-03, 4.1038e-03, 4.1191e-03, 4.1343e-03, 4.1496e-03, 4.1649e-03, 4.1801e-03, 4.1954e-03, 4.2106e-03, 4.2259e-03, 4.2412e-03, 4.2564e-03, 4.2717e-03, 4.2869e-03, 4.3022e-03, 4.3174e-03, 4.3327e-03, 4.3480e-03, 4.3632e-03, 4.3785e-03, 4.3937e-03, 4.4090e-03, 4.4243e-03, 4.4395e-03, 4.4548e-03, 4.4700e-03, 4.4853e-03, 4.5005e-03, 4.5158e-03, 4.5311e-03, 4.5463e-03, 4.5616e-03, 4.5768e-03, 4.5921e-03, 4.6074e-03, 4.6226e-03, 4.6379e-03, 4.6531e-03, 4.6684e-03, 4.6836e-03, 4.6989e-03, 4.7142e-03, 4.7294e-03, 4.7447e-03, 4.7599e-03, 4.7752e-03, 4.7905e-03, 4.8057e-03, 4.8210e-03, 4.8362e-03, 4.8515e-03, 4.8667e-03, 4.8820e-03, 4.8973e-03, 4.9125e-03, 4.9278e-03, 4.9430e-03, 4.9583e-03, 4.9736e-03, 4.9888e-03, 5.0041e-03, 5.0193e-03, 5.0346e-03, 5.0498e-03, 5.0651e-03, 5.0804e-03, 5.0956e-03, 5.1109e-03, 5.1261e-03, 5.1414e-03, 5.1567e-03, 5.1719e-03, 5.1872e-03, 5.2024e-03, 5.2177e-03, 5.2329e-03, 5.2482e-03, 5.2635e-03, 5.2787e-03, 5.2940e-03, 5.3092e-03, 5.3245e-03, 5.3398e-03, 5.3550e-03, 5.3703e-03, 5.3855e-03, 5.4008e-03, 5.4160e-03, 5.4313e-03, 5.4466e-03, 5.4618e-03, 5.4771e-03, 5.4923e-03, 5.5076e-03, 5.5229e-03, 5.5381e-03, 5.5534e-03, 5.5686e-03, 5.5839e-03, 5.5991e-03, 5.6144e-03, 5.6297e-03, 5.6449e-03, 5.6602e-03, 5.6754e-03, 5.6907e-03, 5.7060e-03, 5.7212e-03, 5.7365e-03, 5.7517e-03, 5.7670e-03, 5.7822e-03, 5.7975e-03, 5.8128e-03, 5.8280e-03, 5.8433e-03, 5.8585e-03, 5.8738e-03, 5.8891e-03, 5.9043e-03, 5.9196e-03, 5.9348e-03, 5.9501e-03, 5.9653e-03, 5.9806e-03, 5.9959e-03, 6.0111e-03, 6.0264e-03, 6.0416e-03, 6.0569e-03, 6.0722e-03, 6.0874e-03, 6.1027e-03, 6.1179e-03, 6.1332e-03, 6.1484e-03, 6.1637e-03, 6.1790e-03, 6.1942e-03, 6.2095e-03, 6.2247e-03, 6.2400e-03, 6.2553e-03, 6.2705e-03, 6.2858e-03, 6.3010e-03, 6.3163e-03, 6.3315e-03, 6.3468e-03, 6.3621e-03, 6.3773e-03, 6.3926e-03, 6.4078e-03, 6.4231e-03, 6.4384e-03, 6.4536e-03, 6.4689e-03, 6.4841e-03, 6.4994e-03, 6.5146e-03, 6.5299e-03, 6.5452e-03, 6.5604e-03, 6.5757e-03, 6.5909e-03, 6.6062e-03, 6.6215e-03, 6.6367e-03, 6.6520e-03, 6.6672e-03, 6.6825e-03, 6.6977e-03, 6.7130e-03, 6.7283e-03, 6.7435e-03, 6.7588e-03, 6.7740e-03, 6.7893e-03, 6.8046e-03, 6.8198e-03, 6.8351e-03, 6.8503e-03, 6.8656e-03, 6.8808e-03, 6.8961e-03, 6.9114e-03, 6.9266e-03, 6.9419e-03, 6.9571e-03, 6.9724e-03, 6.9877e-03, 7.0029e-03, 7.0182e-03, 7.0334e-03, 7.0487e-03, 7.0639e-03, 7.0792e-03, 7.0945e-03, 7.1097e-03, 7.1250e-03, 7.1402e-03, 7.1555e-03, 7.1708e-03, 7.1860e-03, 7.2013e-03, 7.2165e-03, 7.2318e-03, 7.2470e-03, 7.2623e-03, 7.2776e-03, 7.2928e-03, 7.3081e-03, 7.3233e-03, 7.3386e-03, 7.3539e-03, 7.3691e-03, 7.3844e-03, 7.3996e-03, 7.4149e-03, 7.4301e-03, 7.4454e-03, 7.4607e-03, 7.4759e-03, 7.4912e-03, 7.5064e-03, 7.5217e-03, 7.5370e-03, 7.5522e-03, 7.5675e-03, 7.5827e-03, 7.5980e-03, 7.6132e-03, 7.6285e-03, 7.6438e-03, 7.6590e-03, 7.6743e-03, 7.6895e-03, 7.7048e-03, 7.7201e-03, 7.7353e-03, 7.7506e-03, 7.7658e-03, 7.7811e-03, 7.7963e-03, 7.8116e-03,
|
||||
7.8421e-03, 7.8726e-03, 7.9032e-03, 7.9337e-03, 7.9642e-03, 7.9947e-03, 8.0252e-03, 8.0557e-03, 8.0863e-03, 8.1168e-03, 8.1473e-03, 8.1778e-03, 8.2083e-03, 8.2388e-03, 8.2694e-03, 8.2999e-03, 8.3304e-03, 8.3609e-03, 8.3914e-03, 8.4219e-03, 8.4525e-03, 8.4830e-03, 8.5135e-03, 8.5440e-03, 8.5745e-03, 8.6051e-03, 8.6356e-03, 8.6661e-03, 8.6966e-03, 8.7271e-03, 8.7576e-03, 8.7882e-03, 8.8187e-03, 8.8492e-03, 8.8797e-03, 8.9102e-03, 8.9407e-03, 8.9713e-03, 9.0018e-03, 9.0323e-03, 9.0628e-03, 9.0933e-03, 9.1238e-03, 9.1544e-03, 9.1849e-03, 9.2154e-03, 9.2459e-03, 9.2764e-03, 9.3069e-03, 9.3375e-03, 9.3680e-03, 9.3985e-03, 9.4290e-03, 9.4595e-03, 9.4900e-03, 9.5206e-03, 9.5511e-03, 9.5816e-03, 9.6121e-03, 9.6426e-03, 9.6731e-03, 9.7037e-03, 9.7342e-03, 9.7647e-03, 9.7952e-03, 9.8257e-03, 9.8563e-03, 9.8868e-03, 9.9173e-03, 9.9478e-03, 9.9783e-03, 1.0009e-02, 1.0039e-02, 1.0070e-02, 1.0100e-02, 1.0131e-02, 1.0161e-02, 1.0192e-02, 1.0222e-02, 1.0253e-02, 1.0283e-02, 1.0314e-02, 1.0345e-02, 1.0375e-02, 1.0406e-02, 1.0436e-02, 1.0467e-02, 1.0497e-02, 1.0528e-02, 1.0558e-02, 1.0589e-02, 1.0619e-02, 1.0650e-02, 1.0680e-02, 1.0711e-02, 1.0741e-02, 1.0772e-02, 1.0802e-02, 1.0833e-02, 1.0863e-02, 1.0894e-02, 1.0924e-02, 1.0955e-02, 1.0985e-02, 1.1016e-02, 1.1046e-02, 1.1077e-02, 1.1107e-02, 1.1138e-02, 1.1168e-02, 1.1199e-02, 1.1230e-02, 1.1260e-02, 1.1291e-02, 1.1321e-02, 1.1352e-02, 1.1382e-02, 1.1413e-02, 1.1443e-02, 1.1474e-02, 1.1504e-02, 1.1535e-02, 1.1565e-02, 1.1596e-02, 1.1626e-02, 1.1657e-02, 1.1687e-02, 1.1718e-02, 1.1748e-02, 1.1779e-02, 1.1809e-02, 1.1840e-02, 1.1870e-02, 1.1901e-02, 1.1931e-02, 1.1962e-02, 1.1992e-02, 1.2023e-02, 1.2053e-02, 1.2084e-02, 1.2115e-02, 1.2145e-02, 1.2176e-02, 1.2206e-02, 1.2237e-02, 1.2267e-02, 1.2298e-02, 1.2328e-02, 1.2359e-02, 1.2389e-02, 1.2420e-02, 1.2450e-02, 1.2481e-02, 1.2511e-02, 1.2542e-02, 1.2572e-02, 1.2603e-02, 1.2633e-02, 1.2664e-02, 1.2694e-02, 1.2725e-02, 1.2755e-02, 1.2786e-02, 1.2816e-02, 1.2847e-02, 1.2877e-02, 1.2908e-02, 1.2938e-02, 1.2969e-02, 1.3000e-02, 1.3030e-02, 1.3061e-02, 1.3091e-02, 1.3122e-02, 1.3152e-02, 1.3183e-02, 1.3213e-02, 1.3244e-02, 1.3274e-02, 1.3305e-02, 1.3335e-02, 1.3366e-02, 1.3396e-02, 1.3427e-02, 1.3457e-02, 1.3488e-02, 1.3518e-02, 1.3549e-02, 1.3579e-02, 1.3610e-02, 1.3640e-02, 1.3671e-02, 1.3701e-02, 1.3732e-02, 1.3762e-02, 1.3793e-02, 1.3823e-02, 1.3854e-02, 1.3885e-02, 1.3915e-02, 1.3946e-02, 1.3976e-02, 1.4007e-02, 1.4037e-02, 1.4068e-02, 1.4098e-02, 1.4129e-02, 1.4159e-02, 1.4190e-02, 1.4220e-02, 1.4251e-02, 1.4281e-02, 1.4312e-02, 1.4342e-02, 1.4373e-02, 1.4403e-02, 1.4434e-02, 1.4464e-02, 1.4495e-02, 1.4525e-02, 1.4556e-02, 1.4586e-02, 1.4617e-02, 1.4647e-02, 1.4678e-02, 1.4708e-02, 1.4739e-02, 1.4770e-02, 1.4800e-02, 1.4831e-02, 1.4861e-02, 1.4892e-02, 1.4922e-02, 1.4953e-02, 1.4983e-02, 1.5014e-02, 1.5044e-02, 1.5075e-02, 1.5105e-02, 1.5136e-02, 1.5166e-02, 1.5197e-02, 1.5227e-02, 1.5258e-02, 1.5288e-02, 1.5319e-02, 1.5349e-02, 1.5380e-02, 1.5410e-02, 1.5441e-02, 1.5471e-02, 1.5502e-02, 1.5532e-02, 1.5563e-02, 1.5593e-02, 1.5624e-02,
|
||||
1.5746e-02, 1.5868e-02, 1.5990e-02, 1.6112e-02, 1.6234e-02, 1.6356e-02, 1.6478e-02, 1.6601e-02, 1.6723e-02, 1.6845e-02, 1.6967e-02, 1.7089e-02, 1.7211e-02, 1.7333e-02, 1.7455e-02, 1.7577e-02, 1.7699e-02, 1.7821e-02, 1.7943e-02, 1.8065e-02, 1.8187e-02, 1.8310e-02, 1.8432e-02, 1.8554e-02, 1.8676e-02, 1.8798e-02, 1.8920e-02, 1.9042e-02, 1.9164e-02, 1.9286e-02, 1.9408e-02, 1.9530e-02, 1.9652e-02, 1.9774e-02, 1.9896e-02, 2.0018e-02, 2.0141e-02, 2.0263e-02, 2.0385e-02, 2.0507e-02, 2.0629e-02, 2.0751e-02, 2.0873e-02, 2.0995e-02, 2.1117e-02, 2.1239e-02, 2.1361e-02, 2.1483e-02, 2.1605e-02, 2.1727e-02, 2.1850e-02, 2.1972e-02, 2.2094e-02, 2.2216e-02, 2.2338e-02, 2.2460e-02, 2.2582e-02, 2.2704e-02, 2.2826e-02, 2.2948e-02, 2.3070e-02, 2.3192e-02, 2.3314e-02, 2.3436e-02, 2.3558e-02, 2.3681e-02, 2.3803e-02, 2.3925e-02, 2.4047e-02, 2.4169e-02, 2.4291e-02, 2.4413e-02, 2.4535e-02, 2.4657e-02, 2.4779e-02, 2.4901e-02, 2.5023e-02, 2.5145e-02, 2.5267e-02, 2.5390e-02, 2.5512e-02, 2.5634e-02, 2.5756e-02, 2.5878e-02, 2.6000e-02, 2.6122e-02, 2.6244e-02, 2.6366e-02, 2.6488e-02, 2.6610e-02, 2.6732e-02, 2.6854e-02, 2.6976e-02, 2.7099e-02, 2.7221e-02, 2.7343e-02, 2.7465e-02, 2.7587e-02, 2.7709e-02, 2.7831e-02, 2.7953e-02, 2.8075e-02, 2.8197e-02, 2.8319e-02, 2.8441e-02, 2.8563e-02, 2.8685e-02, 2.8807e-02, 2.8930e-02, 2.9052e-02, 2.9174e-02, 2.9296e-02, 2.9418e-02, 2.9540e-02, 2.9662e-02, 2.9784e-02, 2.9906e-02, 3.0028e-02, 3.0150e-02, 3.0272e-02, 3.0394e-02, 3.0516e-02, 3.0639e-02, 3.0761e-02, 3.0883e-02, 3.1005e-02, 3.1127e-02, 3.1249e-02, 3.1493e-02, 3.1737e-02, 3.1981e-02, 3.2225e-02, 3.2470e-02, 3.2714e-02, 3.2958e-02, 3.3202e-02, 3.3446e-02, 3.3690e-02, 3.3934e-02, 3.4179e-02, 3.4423e-02, 3.4667e-02, 3.4911e-02, 3.5155e-02, 3.5399e-02, 3.5643e-02, 3.5888e-02, 3.6132e-02, 3.6376e-02, 3.6620e-02, 3.6864e-02, 3.7108e-02, 3.7352e-02, 3.7596e-02, 3.7841e-02, 3.8085e-02, 3.8329e-02, 3.8573e-02, 3.8817e-02, 3.9061e-02, 3.9305e-02, 3.9550e-02, 3.9794e-02, 4.0038e-02, 4.0282e-02, 4.0526e-02, 4.0770e-02, 4.1014e-02, 4.1259e-02, 4.1503e-02, 4.1747e-02, 4.1991e-02, 4.2235e-02, 4.2479e-02, 4.2723e-02, 4.2968e-02, 4.3212e-02, 4.3456e-02, 4.3700e-02, 4.3944e-02, 4.4188e-02, 4.4432e-02, 4.4677e-02, 4.4921e-02, 4.5165e-02, 4.5409e-02, 4.5653e-02, 4.5897e-02, 4.6141e-02, 4.6386e-02, 4.6630e-02, 4.6874e-02, 4.7118e-02, 4.7362e-02, 4.7606e-02, 4.7850e-02, 4.8095e-02, 4.8339e-02, 4.8583e-02, 4.8827e-02, 4.9071e-02, 4.9315e-02, 4.9559e-02, 4.9803e-02, 5.0048e-02, 5.0292e-02, 5.0536e-02, 5.0780e-02, 5.1024e-02, 5.1268e-02, 5.1512e-02, 5.1757e-02, 5.2001e-02, 5.2245e-02, 5.2489e-02, 5.2733e-02, 5.2977e-02, 5.3221e-02, 5.3466e-02, 5.3710e-02, 5.3954e-02, 5.4198e-02, 5.4442e-02, 5.4686e-02, 5.4930e-02, 5.5175e-02, 5.5419e-02, 5.5663e-02, 5.5907e-02, 5.6151e-02, 5.6395e-02, 5.6639e-02, 5.6884e-02, 5.7128e-02, 5.7372e-02, 5.7616e-02, 5.7860e-02, 5.8104e-02, 5.8348e-02, 5.8593e-02, 5.8837e-02, 5.9081e-02, 5.9325e-02, 5.9569e-02, 5.9813e-02, 6.0057e-02, 6.0301e-02, 6.0546e-02, 6.0790e-02, 6.1034e-02, 6.1278e-02, 6.1522e-02, 6.1766e-02, 6.2010e-02, 6.2255e-02, 6.2499e-02,
|
||||
6.2743e-02, 6.2987e-02, 6.3231e-02, 6.3475e-02, 6.3719e-02, 6.3964e-02, 6.4208e-02, 6.4452e-02, 6.4696e-02, 6.4940e-02, 6.5184e-02, 6.5428e-02, 6.5673e-02, 6.5917e-02, 6.6161e-02, 6.6405e-02, 6.6649e-02, 6.6893e-02, 6.7137e-02, 6.7382e-02, 6.7626e-02, 6.7870e-02, 6.8114e-02, 6.8358e-02, 6.8602e-02, 6.8846e-02, 6.9091e-02, 6.9335e-02, 6.9579e-02, 6.9823e-02, 7.0067e-02, 7.0311e-02, 7.0555e-02, 7.0799e-02, 7.1044e-02, 7.1288e-02, 7.1532e-02, 7.1776e-02, 7.2020e-02, 7.2264e-02, 7.2508e-02, 7.2753e-02, 7.2997e-02, 7.3241e-02, 7.3485e-02, 7.3729e-02, 7.3973e-02, 7.4217e-02, 7.4462e-02, 7.4706e-02, 7.4950e-02, 7.5194e-02, 7.5438e-02, 7.5682e-02, 7.5926e-02, 7.6171e-02, 7.6415e-02, 7.6659e-02, 7.6903e-02, 7.7147e-02, 7.7391e-02, 7.7635e-02, 7.7880e-02, 7.8124e-02, 7.8368e-02, 7.8612e-02, 7.8856e-02, 7.9100e-02, 7.9344e-02, 7.9589e-02, 7.9833e-02, 8.0077e-02, 8.0321e-02, 8.0565e-02, 8.0809e-02, 8.1053e-02, 8.1298e-02, 8.1542e-02, 8.1786e-02, 8.2030e-02, 8.2274e-02, 8.2518e-02, 8.2762e-02, 8.3006e-02, 8.3251e-02, 8.3495e-02, 8.3739e-02, 8.3983e-02, 8.4227e-02, 8.4471e-02, 8.4715e-02, 8.4960e-02, 8.5204e-02, 8.5448e-02, 8.5692e-02, 8.5936e-02, 8.6180e-02, 8.6424e-02, 8.6669e-02, 8.6913e-02, 8.7157e-02, 8.7401e-02, 8.7645e-02, 8.7889e-02, 8.8133e-02, 8.8378e-02, 8.8622e-02, 8.8866e-02, 8.9110e-02, 8.9354e-02, 8.9598e-02, 8.9842e-02, 9.0087e-02, 9.0331e-02, 9.0575e-02, 9.0819e-02, 9.1063e-02, 9.1307e-02, 9.1551e-02, 9.1796e-02, 9.2040e-02, 9.2284e-02, 9.2528e-02, 9.2772e-02, 9.3016e-02, 9.3260e-02, 9.3504e-02, 9.3749e-02, 9.4237e-02, 9.4725e-02, 9.5213e-02, 9.5702e-02, 9.6190e-02, 9.6678e-02, 9.7167e-02, 9.7655e-02, 9.8143e-02, 9.8631e-02, 9.9120e-02, 9.9608e-02, 1.0010e-01, 1.0058e-01, 1.0107e-01, 1.0156e-01, 1.0205e-01, 1.0254e-01, 1.0303e-01, 1.0351e-01, 1.0400e-01, 1.0449e-01, 1.0498e-01, 1.0547e-01, 1.0596e-01, 1.0644e-01, 1.0693e-01, 1.0742e-01, 1.0791e-01, 1.0840e-01, 1.0889e-01, 1.0937e-01, 1.0986e-01, 1.1035e-01, 1.1084e-01, 1.1133e-01, 1.1182e-01, 1.1230e-01, 1.1279e-01, 1.1328e-01, 1.1377e-01, 1.1426e-01, 1.1474e-01, 1.1523e-01, 1.1572e-01, 1.1621e-01, 1.1670e-01, 1.1719e-01, 1.1767e-01, 1.1816e-01, 1.1865e-01, 1.1914e-01, 1.1963e-01, 1.2012e-01, 1.2060e-01, 1.2109e-01, 1.2158e-01, 1.2207e-01, 1.2256e-01, 1.2305e-01, 1.2353e-01, 1.2402e-01, 1.2451e-01, 1.2500e-01, 1.2549e-01, 1.2598e-01, 1.2646e-01, 1.2695e-01, 1.2744e-01, 1.2793e-01, 1.2842e-01, 1.2890e-01, 1.2939e-01, 1.2988e-01, 1.3037e-01, 1.3086e-01, 1.3135e-01, 1.3183e-01, 1.3232e-01, 1.3281e-01, 1.3330e-01, 1.3379e-01, 1.3428e-01, 1.3476e-01, 1.3525e-01, 1.3574e-01, 1.3623e-01, 1.3672e-01, 1.3721e-01, 1.3769e-01, 1.3818e-01, 1.3867e-01, 1.3916e-01, 1.3965e-01, 1.4014e-01, 1.4062e-01, 1.4111e-01, 1.4160e-01, 1.4209e-01, 1.4258e-01, 1.4306e-01, 1.4355e-01, 1.4404e-01, 1.4453e-01, 1.4502e-01, 1.4551e-01, 1.4599e-01, 1.4648e-01, 1.4697e-01, 1.4746e-01, 1.4795e-01, 1.4844e-01, 1.4892e-01, 1.4941e-01, 1.4990e-01, 1.5039e-01, 1.5088e-01, 1.5137e-01, 1.5185e-01, 1.5234e-01, 1.5283e-01, 1.5332e-01, 1.5381e-01, 1.5430e-01, 1.5478e-01, 1.5527e-01, 1.5576e-01, 1.5625e-01,
|
||||
1.5674e-01, 1.5723e-01, 1.5771e-01, 1.5820e-01, 1.5869e-01, 1.5918e-01, 1.5967e-01, 1.6015e-01, 1.6064e-01, 1.6113e-01, 1.6162e-01, 1.6211e-01, 1.6260e-01, 1.6308e-01, 1.6357e-01, 1.6406e-01, 1.6455e-01, 1.6504e-01, 1.6553e-01, 1.6601e-01, 1.6650e-01, 1.6699e-01, 1.6748e-01, 1.6797e-01, 1.6846e-01, 1.6894e-01, 1.6943e-01, 1.6992e-01, 1.7041e-01, 1.7090e-01, 1.7139e-01, 1.7187e-01, 1.7236e-01, 1.7285e-01, 1.7334e-01, 1.7383e-01, 1.7431e-01, 1.7480e-01, 1.7529e-01, 1.7578e-01, 1.7627e-01, 1.7676e-01, 1.7724e-01, 1.7773e-01, 1.7822e-01, 1.7871e-01, 1.7920e-01, 1.7969e-01, 1.8017e-01, 1.8066e-01, 1.8115e-01, 1.8164e-01, 1.8213e-01, 1.8262e-01, 1.8310e-01, 1.8359e-01, 1.8408e-01, 1.8457e-01, 1.8506e-01, 1.8555e-01, 1.8603e-01, 1.8652e-01, 1.8701e-01, 1.8750e-01, 1.8848e-01, 1.8945e-01, 1.9043e-01, 1.9140e-01, 1.9238e-01, 1.9336e-01, 1.9433e-01, 1.9531e-01, 1.9629e-01, 1.9726e-01, 1.9824e-01, 1.9922e-01, 2.0019e-01, 2.0117e-01, 2.0215e-01, 2.0312e-01, 2.0410e-01, 2.0508e-01, 2.0605e-01, 2.0703e-01, 2.0801e-01, 2.0898e-01, 2.0996e-01, 2.1094e-01, 2.1191e-01, 2.1289e-01, 2.1387e-01, 2.1484e-01, 2.1582e-01, 2.1680e-01, 2.1777e-01, 2.1875e-01, 2.1972e-01, 2.2070e-01, 2.2168e-01, 2.2265e-01, 2.2363e-01, 2.2461e-01, 2.2558e-01, 2.2656e-01, 2.2754e-01, 2.2851e-01, 2.2949e-01, 2.3047e-01, 2.3144e-01, 2.3242e-01, 2.3340e-01, 2.3437e-01, 2.3535e-01, 2.3633e-01, 2.3730e-01, 2.3828e-01, 2.3926e-01, 2.4023e-01, 2.4121e-01, 2.4219e-01, 2.4316e-01, 2.4414e-01, 2.4512e-01, 2.4609e-01, 2.4707e-01, 2.4805e-01, 2.4902e-01, 2.5000e-01, 2.5097e-01, 2.5195e-01, 2.5293e-01, 2.5390e-01, 2.5488e-01, 2.5586e-01, 2.5683e-01, 2.5781e-01, 2.5879e-01, 2.5976e-01, 2.6074e-01, 2.6172e-01, 2.6269e-01, 2.6367e-01, 2.6465e-01, 2.6562e-01, 2.6660e-01, 2.6758e-01, 2.6855e-01, 2.6953e-01, 2.7051e-01, 2.7148e-01, 2.7246e-01, 2.7344e-01, 2.7441e-01, 2.7539e-01, 2.7637e-01, 2.7734e-01, 2.7832e-01, 2.7930e-01, 2.8027e-01, 2.8125e-01, 2.8222e-01, 2.8320e-01, 2.8418e-01, 2.8515e-01, 2.8613e-01, 2.8711e-01, 2.8808e-01, 2.8906e-01, 2.9004e-01, 2.9101e-01, 2.9199e-01, 2.9297e-01, 2.9394e-01, 2.9492e-01, 2.9590e-01, 2.9687e-01, 2.9785e-01, 2.9883e-01, 2.9980e-01, 3.0078e-01, 3.0176e-01, 3.0273e-01, 3.0371e-01, 3.0469e-01, 3.0566e-01, 3.0664e-01, 3.0762e-01, 3.0859e-01, 3.0957e-01, 3.1055e-01, 3.1152e-01, 3.1250e-01, 3.1347e-01, 3.1445e-01, 3.1543e-01, 3.1640e-01, 3.1738e-01, 3.1836e-01, 3.1933e-01, 3.2031e-01, 3.2129e-01, 3.2226e-01, 3.2324e-01, 3.2422e-01, 3.2519e-01, 3.2617e-01, 3.2715e-01, 3.2812e-01, 3.2910e-01, 3.3008e-01, 3.3105e-01, 3.3203e-01, 3.3301e-01, 3.3398e-01, 3.3496e-01, 3.3594e-01, 3.3691e-01, 3.3789e-01, 3.3887e-01, 3.3984e-01, 3.4082e-01, 3.4180e-01, 3.4277e-01, 3.4375e-01, 3.4472e-01, 3.4570e-01, 3.4668e-01, 3.4765e-01, 3.4863e-01, 3.4961e-01, 3.5058e-01, 3.5156e-01, 3.5254e-01, 3.5351e-01, 3.5449e-01, 3.5547e-01, 3.5644e-01, 3.5742e-01, 3.5840e-01, 3.5937e-01, 3.6035e-01, 3.6133e-01, 3.6230e-01, 3.6328e-01, 3.6426e-01, 3.6523e-01, 3.6621e-01, 3.6719e-01, 3.6816e-01, 3.6914e-01, 3.7012e-01, 3.7109e-01, 3.7207e-01, 3.7305e-01, 3.7402e-01, 3.7500e-01,
|
||||
3.7695e-01, 3.7890e-01, 3.8086e-01, 3.8281e-01, 3.8476e-01, 3.8672e-01, 3.8867e-01, 3.9062e-01, 3.9258e-01, 3.9453e-01, 3.9648e-01, 3.9844e-01, 4.0039e-01, 4.0234e-01, 4.0430e-01, 4.0625e-01, 4.0820e-01, 4.1015e-01, 4.1211e-01, 4.1406e-01, 4.1601e-01, 4.1797e-01, 4.1992e-01, 4.2187e-01, 4.2383e-01, 4.2578e-01, 4.2773e-01, 4.2969e-01, 4.3164e-01, 4.3359e-01, 4.3555e-01, 4.3750e-01, 4.3945e-01, 4.4140e-01, 4.4336e-01, 4.4531e-01, 4.4726e-01, 4.4922e-01, 4.5117e-01, 4.5312e-01, 4.5508e-01, 4.5703e-01, 4.5898e-01, 4.6094e-01, 4.6289e-01, 4.6484e-01, 4.6680e-01, 4.6875e-01, 4.7070e-01, 4.7265e-01, 4.7461e-01, 4.7656e-01, 4.7851e-01, 4.8047e-01, 4.8242e-01, 4.8437e-01, 4.8633e-01, 4.8828e-01, 4.9023e-01, 4.9219e-01, 4.9414e-01, 4.9609e-01, 4.9805e-01, 5.0000e-01, 5.0195e-01, 5.0390e-01, 5.0586e-01, 5.0781e-01, 5.0976e-01, 5.1172e-01, 5.1367e-01, 5.1562e-01, 5.1758e-01, 5.1953e-01, 5.2148e-01, 5.2344e-01, 5.2539e-01, 5.2734e-01, 5.2930e-01, 5.3125e-01, 5.3320e-01, 5.3515e-01, 5.3711e-01, 5.3906e-01, 5.4101e-01, 5.4297e-01, 5.4492e-01, 5.4687e-01, 5.4883e-01, 5.5078e-01, 5.5273e-01, 5.5469e-01, 5.5664e-01, 5.5859e-01, 5.6055e-01, 5.6250e-01, 5.6445e-01, 5.6640e-01, 5.6836e-01, 5.7031e-01, 5.7226e-01, 5.7422e-01, 5.7617e-01, 5.7812e-01, 5.8008e-01, 5.8203e-01, 5.8398e-01, 5.8594e-01, 5.8789e-01, 5.8984e-01, 5.9180e-01, 5.9375e-01, 5.9570e-01, 5.9765e-01, 5.9961e-01, 6.0156e-01, 6.0351e-01, 6.0547e-01, 6.0742e-01, 6.0937e-01, 6.1133e-01, 6.1328e-01, 6.1523e-01, 6.1719e-01, 6.1914e-01, 6.2109e-01, 6.2305e-01, 6.2500e-01, 6.2695e-01, 6.2890e-01, 6.3086e-01, 6.3281e-01, 6.3476e-01, 6.3672e-01, 6.3867e-01, 6.4062e-01, 6.4258e-01, 6.4453e-01, 6.4648e-01, 6.4844e-01, 6.5039e-01, 6.5234e-01, 6.5430e-01, 6.5625e-01, 6.5820e-01, 6.6015e-01, 6.6211e-01, 6.6406e-01, 6.6601e-01, 6.6797e-01, 6.6992e-01, 6.7187e-01, 6.7383e-01, 6.7578e-01, 6.7773e-01, 6.7969e-01, 6.8164e-01, 6.8359e-01, 6.8554e-01, 6.8750e-01, 6.8945e-01, 6.9140e-01, 6.9336e-01, 6.9531e-01, 6.9726e-01, 6.9922e-01, 7.0117e-01, 7.0312e-01, 7.0508e-01, 7.0703e-01, 7.0898e-01, 7.1094e-01, 7.1289e-01, 7.1484e-01, 7.1679e-01, 7.1875e-01, 7.2070e-01, 7.2265e-01, 7.2461e-01, 7.2656e-01, 7.2851e-01, 7.3047e-01, 7.3242e-01, 7.3437e-01, 7.3633e-01, 7.3828e-01, 7.4023e-01, 7.4219e-01, 7.4414e-01, 7.4609e-01, 7.4804e-01, 7.5000e-01, 7.5390e-01, 7.5781e-01, 7.6172e-01, 7.6562e-01, 7.6953e-01, 7.7344e-01, 7.7734e-01, 7.8125e-01, 7.8515e-01, 7.8906e-01, 7.9297e-01, 7.9687e-01, 8.0078e-01, 8.0469e-01, 8.0859e-01, 8.1250e-01, 8.1640e-01, 8.2031e-01, 8.2422e-01, 8.2812e-01, 8.3203e-01, 8.3594e-01, 8.3984e-01, 8.4375e-01, 8.4765e-01, 8.5156e-01, 8.5547e-01, 8.5937e-01, 8.6328e-01, 8.6719e-01, 8.7109e-01, 8.7500e-01, 8.7890e-01, 8.8281e-01, 8.8672e-01, 8.9062e-01, 8.9453e-01, 8.9844e-01, 9.0234e-01, 9.0625e-01, 9.1015e-01, 9.1406e-01, 9.1797e-01, 9.2187e-01, 9.2578e-01, 9.2969e-01, 9.3359e-01, 9.3750e-01, 9.4140e-01, 9.4531e-01, 9.4922e-01, 9.5312e-01, 9.5703e-01, 9.6094e-01, 9.6484e-01, 9.6875e-01, 9.7265e-01, 9.7656e-01, 9.8047e-01, 9.8437e-01, 9.8828e-01, 9.9219e-01, 9.9609e-01, 1.0000e+00
|
||||
};
|
||||
|
||||
float4 val4_from_12(uchar8 pvs, float gain) {
|
||||
uint4 parsed = (uint4)(((uint)pvs.s0<<4) + (pvs.s1>>4), // is from the previous 10 bit
|
||||
((uint)pvs.s2<<4) + (pvs.s4&0xF),
|
||||
((uint)pvs.s3<<4) + (pvs.s4>>4),
|
||||
((uint)pvs.s5<<4) + (pvs.s7&0xF));
|
||||
#if IS_OX
|
||||
// PWL
|
||||
//float4 pv = (convert_float4(parsed) - 64.0) / (4096.0 - 64.0);
|
||||
float4 pv = {ox03c10_lut[parsed.s0], ox03c10_lut[parsed.s1], ox03c10_lut[parsed.s2], ox03c10_lut[parsed.s3]};
|
||||
|
||||
// it's a 24 bit signal, center in the middle 8 bits
|
||||
return clamp(pv*gain*256.0, 0.0, 1.0);
|
||||
#else // AR
|
||||
// normalize and scale
|
||||
float4 pv = (convert_float4(parsed) - 168.0) / (4096.0 - 168.0);
|
||||
return clamp(pv*gain, 0.0, 1.0);
|
||||
#endif
|
||||
|
||||
}
|
||||
|
||||
float get_k(float a, float b, float c, float d) {
|
||||
return 2.0 - (fabs(a - b) + fabs(c - d));
|
||||
}
|
||||
|
||||
__kernel void debayer10(const __global uchar * in, __global uchar * out)
|
||||
{
|
||||
const int gid_x = get_global_id(0);
|
||||
const int gid_y = get_global_id(1);
|
||||
|
||||
const int y_top_mod = (gid_y == 0) ? 2: 0;
|
||||
const int y_bot_mod = (gid_y == (RGB_HEIGHT/2 - 1)) ? 1: 3;
|
||||
|
||||
float3 rgb;
|
||||
uchar3 rgb_out[4];
|
||||
|
||||
int start = (2 * gid_y - 1) * FRAME_STRIDE + (3 * gid_x - 2) + (FRAME_STRIDE * FRAME_OFFSET);
|
||||
|
||||
// read in 8x4 chars
|
||||
uchar8 dat[4];
|
||||
dat[0] = vload8(0, in + start + FRAME_STRIDE*y_top_mod);
|
||||
dat[1] = vload8(0, in + start + FRAME_STRIDE*1);
|
||||
dat[2] = vload8(0, in + start + FRAME_STRIDE*2);
|
||||
dat[3] = vload8(0, in + start + FRAME_STRIDE*y_bot_mod);
|
||||
|
||||
// correct vignetting
|
||||
#if VIGNETTING
|
||||
int gx = (gid_x*2 - RGB_WIDTH/2);
|
||||
int gy = (gid_y*2 - RGB_HEIGHT/2);
|
||||
const float gain = get_vignetting_s(gx*gx + gy*gy);
|
||||
#else
|
||||
const float gain = 1.0;
|
||||
#endif
|
||||
|
||||
// process them to floats
|
||||
float4 va = val4_from_12(dat[0], gain);
|
||||
float4 vb = val4_from_12(dat[1], gain);
|
||||
float4 vc = val4_from_12(dat[2], gain);
|
||||
float4 vd = val4_from_12(dat[3], gain);
|
||||
|
||||
if (gid_x == 0) {
|
||||
va.s0 = va.s2;
|
||||
vb.s0 = vb.s2;
|
||||
vc.s0 = vc.s2;
|
||||
vd.s0 = vd.s2;
|
||||
} else if (gid_x == RGB_WIDTH/2 - 1) {
|
||||
va.s3 = va.s1;
|
||||
vb.s3 = vb.s1;
|
||||
vc.s3 = vc.s1;
|
||||
vd.s3 = vd.s1;
|
||||
}
|
||||
|
||||
// a simplified version of https://opensignalprocessingjournal.com/contents/volumes/V6/TOSIGPJ-6-1/TOSIGPJ-6-1.pdf
|
||||
const float k01 = get_k(va.s0, vb.s1, va.s2, vb.s1);
|
||||
const float k02 = get_k(va.s2, vb.s1, vc.s2, vb.s1);
|
||||
const float k03 = get_k(vc.s0, vb.s1, vc.s2, vb.s1);
|
||||
const float k04 = get_k(va.s0, vb.s1, vc.s0, vb.s1);
|
||||
rgb.x = (k02*vb.s2+k04*vb.s0)/(k02+k04); // R_G1
|
||||
rgb.y = vb.s1; // G1(R)
|
||||
rgb.z = (k01*va.s1+k03*vc.s1)/(k01+k03); // B_G1
|
||||
rgb_out[0] = convert_uchar3_sat(color_correct(clamp(rgb, 0.0, 1.0)) * 255.0);
|
||||
|
||||
const float k11 = get_k(va.s1, vc.s1, va.s3, vc.s3);
|
||||
const float k12 = get_k(va.s2, vb.s1, vb.s3, vc.s2);
|
||||
const float k13 = get_k(va.s1, va.s3, vc.s1, vc.s3);
|
||||
const float k14 = get_k(va.s2, vb.s3, vc.s2, vb.s1);
|
||||
rgb.x = vb.s2; // R
|
||||
rgb.y = (k11*(va.s2+vc.s2)*0.5+k13*(vb.s3+vb.s1)*0.5)/(k11+k13); // G_R
|
||||
rgb.z = (k12*(va.s3+vc.s1)*0.5+k14*(va.s1+vc.s3)*0.5)/(k12+k14); // B_R
|
||||
rgb_out[1] = convert_uchar3_sat(color_correct(clamp(rgb, 0.0, 1.0)) * 255.0);
|
||||
|
||||
const float k21 = get_k(vb.s0, vd.s0, vb.s2, vd.s2);
|
||||
const float k22 = get_k(vb.s1, vc.s0, vc.s2, vd.s1);
|
||||
const float k23 = get_k(vb.s0, vb.s2, vd.s0, vd.s2);
|
||||
const float k24 = get_k(vb.s1, vc.s2, vd.s1, vc.s0);
|
||||
rgb.x = (k22*(vb.s2+vd.s0)*0.5+k24*(vb.s0+vd.s2)*0.5)/(k22+k24); // R_B
|
||||
rgb.y = (k21*(vb.s1+vd.s1)*0.5+k23*(vc.s2+vc.s0)*0.5)/(k21+k23); // G_B
|
||||
rgb.z = vc.s1; // B
|
||||
rgb_out[2] = convert_uchar3_sat(color_correct(clamp(rgb, 0.0, 1.0)) * 255.0);
|
||||
|
||||
const float k31 = get_k(vb.s1, vc.s2, vb.s3, vc.s2);
|
||||
const float k32 = get_k(vb.s3, vc.s2, vd.s3, vc.s2);
|
||||
const float k33 = get_k(vd.s1, vc.s2, vd.s3, vc.s2);
|
||||
const float k34 = get_k(vb.s1, vc.s2, vd.s1, vc.s2);
|
||||
rgb.x = (k31*vb.s2+k33*vd.s2)/(k31+k33); // R_G2
|
||||
rgb.y = vc.s2; // G2(B)
|
||||
rgb.z = (k32*vc.s3+k34*vc.s1)/(k32+k34); // B_G2
|
||||
rgb_out[3] = convert_uchar3_sat(color_correct(clamp(rgb, 0.0, 1.0)) * 255.0);
|
||||
|
||||
// write ys
|
||||
uchar2 yy = (uchar2)(
|
||||
RGB_TO_Y(rgb_out[0].s0, rgb_out[0].s1, rgb_out[0].s2),
|
||||
RGB_TO_Y(rgb_out[1].s0, rgb_out[1].s1, rgb_out[1].s2)
|
||||
);
|
||||
vstore2(yy, 0, out + mad24(gid_y * 2, YUV_STRIDE, gid_x * 2));
|
||||
yy = (uchar2)(
|
||||
RGB_TO_Y(rgb_out[2].s0, rgb_out[2].s1, rgb_out[2].s2),
|
||||
RGB_TO_Y(rgb_out[3].s0, rgb_out[3].s1, rgb_out[3].s2)
|
||||
);
|
||||
vstore2(yy, 0, out + mad24(gid_y * 2 + 1, YUV_STRIDE, gid_x * 2));
|
||||
|
||||
// write uvs
|
||||
const short ar = AVERAGE(rgb_out[0].s0, rgb_out[1].s0, rgb_out[2].s0, rgb_out[3].s0);
|
||||
const short ag = AVERAGE(rgb_out[0].s1, rgb_out[1].s1, rgb_out[2].s1, rgb_out[3].s1);
|
||||
const short ab = AVERAGE(rgb_out[0].s2, rgb_out[1].s2, rgb_out[2].s2, rgb_out[3].s2);
|
||||
uchar2 uv = (uchar2)(
|
||||
RGB_TO_U(ar, ag, ab),
|
||||
RGB_TO_V(ar, ag, ab)
|
||||
);
|
||||
vstore2(uv, 0, out + UV_OFFSET + mad24(gid_y, YUV_STRIDE, gid_x * 2));
|
||||
}
|
||||
@@ -0,0 +1,876 @@
|
||||
struct i2c_random_wr_payload start_reg_array_ar0231[] = {{0x301A, 0x91C}};
|
||||
struct i2c_random_wr_payload stop_reg_array_ar0231[] = {{0x301A, 0x918}};
|
||||
struct i2c_random_wr_payload start_reg_array_ox03c10[] = {{0x100, 1}};
|
||||
struct i2c_random_wr_payload stop_reg_array_ox03c10[] = {{0x100, 0}};
|
||||
|
||||
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},
|
||||
};
|
||||
|
||||
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},
|
||||
};
|
||||
@@ -0,0 +1,110 @@
|
||||
// const __constant float3 rgb_weights = (0.299, 0.587, 0.114); // opencv rgb2gray weights
|
||||
// const __constant float3 bgr_weights = (0.114, 0.587, 0.299); // bgr2gray weights
|
||||
|
||||
// convert input rgb image to single channel then conv
|
||||
__kernel void rgb2gray_conv2d(
|
||||
const __global uchar * input,
|
||||
__global short * output,
|
||||
__constant short * filter,
|
||||
__local uchar3 * cached
|
||||
)
|
||||
{
|
||||
const int rowOffset = get_global_id(1) * IMAGE_W;
|
||||
const int my = get_global_id(0) + rowOffset;
|
||||
|
||||
const int localRowLen = TWICE_HALF_FILTER_SIZE + get_local_size(0);
|
||||
const int localRowOffset = ( get_local_id(1) + HALF_FILTER_SIZE ) * localRowLen;
|
||||
const int myLocal = localRowOffset + get_local_id(0) + HALF_FILTER_SIZE;
|
||||
|
||||
// cache local pixels
|
||||
cached[ myLocal ].x = input[ my * 3 ]; // r
|
||||
cached[ myLocal ].y = input[ my * 3 + 1]; // g
|
||||
cached[ myLocal ].z = input[ my * 3 + 2]; // b
|
||||
|
||||
// pad
|
||||
if (
|
||||
get_global_id(0) < HALF_FILTER_SIZE ||
|
||||
get_global_id(0) > IMAGE_W - HALF_FILTER_SIZE - 1 ||
|
||||
get_global_id(1) < HALF_FILTER_SIZE ||
|
||||
get_global_id(1) > IMAGE_H - HALF_FILTER_SIZE - 1
|
||||
)
|
||||
{
|
||||
barrier(CLK_LOCAL_MEM_FENCE);
|
||||
return;
|
||||
}
|
||||
else
|
||||
{
|
||||
int localColOffset = -1;
|
||||
int globalColOffset = -1;
|
||||
|
||||
// cache extra
|
||||
if ( get_local_id(0) < HALF_FILTER_SIZE )
|
||||
{
|
||||
localColOffset = get_local_id(0);
|
||||
globalColOffset = -HALF_FILTER_SIZE;
|
||||
|
||||
cached[ localRowOffset + get_local_id(0) ].x = input[ my * 3 - HALF_FILTER_SIZE * 3 ];
|
||||
cached[ localRowOffset + get_local_id(0) ].y = input[ my * 3 - HALF_FILTER_SIZE * 3 + 1];
|
||||
cached[ localRowOffset + get_local_id(0) ].z = input[ my * 3 - HALF_FILTER_SIZE * 3 + 2];
|
||||
}
|
||||
else if ( get_local_id(0) >= get_local_size(0) - HALF_FILTER_SIZE )
|
||||
{
|
||||
localColOffset = get_local_id(0) + TWICE_HALF_FILTER_SIZE;
|
||||
globalColOffset = HALF_FILTER_SIZE;
|
||||
|
||||
cached[ myLocal + HALF_FILTER_SIZE ].x = input[ my * 3 + HALF_FILTER_SIZE * 3 ];
|
||||
cached[ myLocal + HALF_FILTER_SIZE ].y = input[ my * 3 + HALF_FILTER_SIZE * 3 + 1];
|
||||
cached[ myLocal + HALF_FILTER_SIZE ].z = input[ my * 3 + HALF_FILTER_SIZE * 3 + 2];
|
||||
}
|
||||
|
||||
|
||||
if ( get_local_id(1) < HALF_FILTER_SIZE )
|
||||
{
|
||||
cached[ get_local_id(1) * localRowLen + get_local_id(0) + HALF_FILTER_SIZE ].x = input[ my * 3 - HALF_FILTER_SIZE_IMAGE_W * 3 ];
|
||||
cached[ get_local_id(1) * localRowLen + get_local_id(0) + HALF_FILTER_SIZE ].y = input[ my * 3 - HALF_FILTER_SIZE_IMAGE_W * 3 + 1];
|
||||
cached[ get_local_id(1) * localRowLen + get_local_id(0) + HALF_FILTER_SIZE ].z = input[ my * 3 - HALF_FILTER_SIZE_IMAGE_W * 3 + 2];
|
||||
if (localColOffset > 0)
|
||||
{
|
||||
cached[ get_local_id(1) * localRowLen + localColOffset ].x = input[ my * 3 - HALF_FILTER_SIZE_IMAGE_W * 3 + globalColOffset * 3];
|
||||
cached[ get_local_id(1) * localRowLen + localColOffset ].y = input[ my * 3 - HALF_FILTER_SIZE_IMAGE_W * 3 + globalColOffset * 3 + 1];
|
||||
cached[ get_local_id(1) * localRowLen + localColOffset ].z = input[ my * 3 - HALF_FILTER_SIZE_IMAGE_W * 3 + globalColOffset * 3 + 2];
|
||||
}
|
||||
}
|
||||
else if ( get_local_id(1) >= get_local_size(1) -HALF_FILTER_SIZE )
|
||||
{
|
||||
int offset = ( get_local_id(1) + TWICE_HALF_FILTER_SIZE ) * localRowLen;
|
||||
cached[ offset + get_local_id(0) + HALF_FILTER_SIZE ].x = input[ my * 3 + HALF_FILTER_SIZE_IMAGE_W * 3 ];
|
||||
cached[ offset + get_local_id(0) + HALF_FILTER_SIZE ].y = input[ my * 3 + HALF_FILTER_SIZE_IMAGE_W * 3 + 1];
|
||||
cached[ offset + get_local_id(0) + HALF_FILTER_SIZE ].z = input[ my * 3 + HALF_FILTER_SIZE_IMAGE_W * 3 + 2];
|
||||
if (localColOffset > 0)
|
||||
{
|
||||
cached[ offset + localColOffset ].x = input[ my * 3 + HALF_FILTER_SIZE_IMAGE_W * 3 + globalColOffset * 3];
|
||||
cached[ offset + localColOffset ].y = input[ my * 3 + HALF_FILTER_SIZE_IMAGE_W * 3 + globalColOffset * 3 + 1];
|
||||
cached[ offset + localColOffset ].z = input[ my * 3 + HALF_FILTER_SIZE_IMAGE_W * 3 + globalColOffset * 3 + 2];
|
||||
}
|
||||
}
|
||||
|
||||
// sync
|
||||
barrier(CLK_LOCAL_MEM_FENCE);
|
||||
|
||||
// perform convolution
|
||||
int fIndex = 0;
|
||||
short sum = 0;
|
||||
|
||||
for (int r = -HALF_FILTER_SIZE; r <= HALF_FILTER_SIZE; r++)
|
||||
{
|
||||
int curRow = r * localRowLen;
|
||||
for (int c = -HALF_FILTER_SIZE; c <= HALF_FILTER_SIZE; c++, fIndex++)
|
||||
{
|
||||
if (!FLIP_RB){
|
||||
// sum += dot(rgb_weights, cached[ myLocal + curRow + c ]) * filter[ fIndex ];
|
||||
sum += (cached[ myLocal + curRow + c ].x / 3 + cached[ myLocal + curRow + c ].y / 2 + cached[ myLocal + curRow + c ].z / 9) * filter[ fIndex ];
|
||||
} else {
|
||||
// sum += dot(bgr_weights, cached[ myLocal + curRow + c ]) * filter[ fIndex ];
|
||||
sum += (cached[ myLocal + curRow + c ].x / 9 + cached[ myLocal + curRow + c ].y / 2 + cached[ myLocal + curRow + c ].z / 3) * filter[ fIndex ];
|
||||
}
|
||||
}
|
||||
}
|
||||
output[my] = sum;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,34 @@
|
||||
// calculate variance in each subregion
|
||||
__kernel void var_pool(
|
||||
const __global char * input,
|
||||
__global ushort * output // should not be larger than 128*128 so uint16
|
||||
)
|
||||
{
|
||||
const int xidx = get_global_id(0) + ROI_X_MIN;
|
||||
const int yidx = get_global_id(1) + ROI_Y_MIN;
|
||||
|
||||
const int size = X_PITCH * Y_PITCH;
|
||||
|
||||
float fsum = 0;
|
||||
char mean, max;
|
||||
|
||||
for (int i = 0; i < size; i++) {
|
||||
int x_offset = i % X_PITCH;
|
||||
int y_offset = i / X_PITCH;
|
||||
fsum += input[xidx*X_PITCH + yidx*Y_PITCH*FULL_STRIDE_X + x_offset + y_offset*FULL_STRIDE_X];
|
||||
max = input[xidx*X_PITCH + yidx*Y_PITCH*FULL_STRIDE_X + x_offset + y_offset*FULL_STRIDE_X]>max ? input[xidx*X_PITCH + yidx*Y_PITCH*FULL_STRIDE_X + x_offset + y_offset*FULL_STRIDE_X]:max;
|
||||
}
|
||||
|
||||
mean = convert_char_rte(fsum / size);
|
||||
|
||||
float fvar = 0;
|
||||
for (int i = 0; i < size; i++) {
|
||||
int x_offset = i % X_PITCH;
|
||||
int y_offset = i / X_PITCH;
|
||||
fvar += (input[xidx*X_PITCH + yidx*Y_PITCH*FULL_STRIDE_X + x_offset + y_offset*FULL_STRIDE_X] - mean) * (input[xidx*X_PITCH + yidx*Y_PITCH*FULL_STRIDE_X + x_offset + y_offset*FULL_STRIDE_X] - mean);
|
||||
}
|
||||
|
||||
fvar = fvar / size;
|
||||
|
||||
output[(xidx-ROI_X_MIN)+(yidx-ROI_Y_MIN)*(ROI_X_MAX-ROI_X_MIN+1)] = convert_ushort_rte(5 * fvar + convert_float_rte(max));
|
||||
}
|
||||
@@ -0,0 +1,106 @@
|
||||
#include "system/camerad/imgproc/utils.h"
|
||||
|
||||
#include <algorithm>
|
||||
#include <cassert>
|
||||
#include <cstdio>
|
||||
#include <cmath>
|
||||
#include <cstring>
|
||||
|
||||
const int16_t lapl_conv_krnl[9] = {0, 1, 0,
|
||||
1, -4, 1,
|
||||
0, 1, 0};
|
||||
|
||||
// calculate score based on laplacians in one area
|
||||
uint16_t get_lapmap_one(const int16_t *lap, int x_pitch, int y_pitch) {
|
||||
const int size = x_pitch * y_pitch;
|
||||
// avg and max of roi
|
||||
int16_t max = 0;
|
||||
int sum = 0;
|
||||
for (int i = 0; i < size; ++i) {
|
||||
const int16_t v = lap[i];
|
||||
sum += v;
|
||||
if (v > max) max = v;
|
||||
}
|
||||
|
||||
const int16_t mean = sum / size;
|
||||
|
||||
// var of roi
|
||||
int var = 0;
|
||||
for (int i = 0; i < size; ++i) {
|
||||
var += std::pow(lap[i] - mean, 2);
|
||||
}
|
||||
|
||||
const float fvar = (float)var / size;
|
||||
return std::min(5 * fvar + max, (float)65535);
|
||||
}
|
||||
|
||||
bool is_blur(const uint16_t *lapmap, const size_t size) {
|
||||
float bad_sum = 0;
|
||||
for (int i = 0; i < size; i++) {
|
||||
if (lapmap[i] < LM_THRESH) {
|
||||
bad_sum += 1 / (float)size;
|
||||
}
|
||||
}
|
||||
return (bad_sum > LM_PREC_THRESH);
|
||||
}
|
||||
|
||||
static cl_program build_conv_program(cl_device_id device_id, cl_context context, int image_w, int image_h, int filter_size) {
|
||||
char args[4096];
|
||||
snprintf(args, sizeof(args),
|
||||
"-cl-fast-relaxed-math -cl-denorms-are-zero "
|
||||
"-DIMAGE_W=%d -DIMAGE_H=%d -DFLIP_RB=%d "
|
||||
"-DFILTER_SIZE=%d -DHALF_FILTER_SIZE=%d -DTWICE_HALF_FILTER_SIZE=%d -DHALF_FILTER_SIZE_IMAGE_W=%d",
|
||||
image_w, image_h, 1,
|
||||
filter_size, filter_size/2, (filter_size/2)*2, (filter_size/2)*image_w);
|
||||
return cl_program_from_file(context, device_id, "imgproc/conv.cl", args);
|
||||
}
|
||||
|
||||
LapConv::LapConv(cl_device_id device_id, cl_context ctx, int rgb_width, int rgb_height, int rgb_stride, int filter_size)
|
||||
: width(rgb_width / NUM_SEGMENTS_X), height(rgb_height / NUM_SEGMENTS_Y), rgb_stride(rgb_stride),
|
||||
roi_buf(width * height * 3), result_buf(width * height) {
|
||||
|
||||
prg = build_conv_program(device_id, ctx, width, height, filter_size);
|
||||
krnl = CL_CHECK_ERR(clCreateKernel(prg, "rgb2gray_conv2d", &err));
|
||||
// TODO: Removed CL_MEM_SVM_FINE_GRAIN_BUFFER, confirm it doesn't matter
|
||||
roi_cl = CL_CHECK_ERR(clCreateBuffer(ctx, CL_MEM_READ_WRITE, roi_buf.size() * sizeof(roi_buf[0]), NULL, &err));
|
||||
result_cl = CL_CHECK_ERR(clCreateBuffer(ctx, CL_MEM_READ_WRITE, result_buf.size() * sizeof(result_buf[0]), NULL, &err));
|
||||
filter_cl = CL_CHECK_ERR(clCreateBuffer(ctx, CL_MEM_READ_ONLY | CL_MEM_COPY_HOST_PTR,
|
||||
9 * sizeof(int16_t), (void *)&lapl_conv_krnl, &err));
|
||||
}
|
||||
|
||||
LapConv::~LapConv() {
|
||||
CL_CHECK(clReleaseMemObject(roi_cl));
|
||||
CL_CHECK(clReleaseMemObject(result_cl));
|
||||
CL_CHECK(clReleaseMemObject(filter_cl));
|
||||
CL_CHECK(clReleaseKernel(krnl));
|
||||
CL_CHECK(clReleaseProgram(prg));
|
||||
}
|
||||
|
||||
uint16_t LapConv::Update(cl_command_queue q, const uint8_t *rgb_buf, const int roi_id) {
|
||||
// sharpness scores
|
||||
const int x_offset = ROI_X_MIN + roi_id % (ROI_X_MAX - ROI_X_MIN + 1);
|
||||
const int y_offset = ROI_Y_MIN + roi_id / (ROI_X_MAX - ROI_X_MIN + 1);
|
||||
|
||||
const uint8_t *rgb_offset = rgb_buf + y_offset * height * rgb_stride + x_offset * width * 3;
|
||||
for (int i = 0; i < height; ++i) {
|
||||
memcpy(&roi_buf[i * width * 3], &rgb_offset[i * rgb_stride], width * 3);
|
||||
}
|
||||
|
||||
constexpr int local_mem_size = (CONV_LOCAL_WORKSIZE + 2 * (3 / 2)) * (CONV_LOCAL_WORKSIZE + 2 * (3 / 2)) * (3 * sizeof(uint8_t));
|
||||
const size_t global_work_size[] = {(size_t)width, (size_t)height};
|
||||
const size_t local_work_size[] = {CONV_LOCAL_WORKSIZE, CONV_LOCAL_WORKSIZE};
|
||||
|
||||
CL_CHECK(clEnqueueWriteBuffer(q, roi_cl, CL_TRUE, 0, roi_buf.size() * sizeof(roi_buf[0]), roi_buf.data(), 0, 0, 0));
|
||||
CL_CHECK(clSetKernelArg(krnl, 0, sizeof(cl_mem), (void *)&roi_cl));
|
||||
CL_CHECK(clSetKernelArg(krnl, 1, sizeof(cl_mem), (void *)&result_cl));
|
||||
CL_CHECK(clSetKernelArg(krnl, 2, sizeof(cl_mem), (void *)&filter_cl));
|
||||
CL_CHECK(clSetKernelArg(krnl, 3, local_mem_size, 0));
|
||||
cl_event conv_event;
|
||||
CL_CHECK(clEnqueueNDRangeKernel(q, krnl, 2, NULL, global_work_size, local_work_size, 0, 0, &conv_event));
|
||||
CL_CHECK(clWaitForEvents(1, &conv_event));
|
||||
CL_CHECK(clReleaseEvent(conv_event));
|
||||
CL_CHECK(clEnqueueReadBuffer(q, result_cl, CL_TRUE, 0,
|
||||
result_buf.size() * sizeof(result_buf[0]), result_buf.data(), 0, 0, 0));
|
||||
|
||||
return get_lapmap_one(result_buf.data(), width, height);
|
||||
}
|
||||
@@ -0,0 +1,37 @@
|
||||
#pragma once
|
||||
|
||||
#include <cstddef>
|
||||
#include <cstdint>
|
||||
#include <vector>
|
||||
|
||||
#include "common/clutil.h"
|
||||
|
||||
#define NUM_SEGMENTS_X 8
|
||||
#define NUM_SEGMENTS_Y 6
|
||||
|
||||
#define ROI_X_MIN 1
|
||||
#define ROI_X_MAX 6
|
||||
#define ROI_Y_MIN 2
|
||||
#define ROI_Y_MAX 3
|
||||
|
||||
#define LM_THRESH 120
|
||||
#define LM_PREC_THRESH 0.9 // 90 perc is blur
|
||||
#define CONV_LOCAL_WORKSIZE 16
|
||||
|
||||
class LapConv {
|
||||
public:
|
||||
LapConv(cl_device_id device_id, cl_context ctx, int rgb_width, int rgb_height, int rgb_stride, int filter_size);
|
||||
~LapConv();
|
||||
uint16_t Update(cl_command_queue q, const uint8_t *rgb_buf, const int roi_id);
|
||||
|
||||
private:
|
||||
cl_mem roi_cl, result_cl, filter_cl;
|
||||
cl_program prg;
|
||||
cl_kernel krnl;
|
||||
const int width, height;
|
||||
const int rgb_stride;
|
||||
std::vector<uint8_t> roi_buf;
|
||||
std::vector<int16_t> result_buf;
|
||||
};
|
||||
|
||||
bool is_blur(const uint16_t *lapmap, const size_t size);
|
||||
@@ -0,0 +1,23 @@
|
||||
#include "system/camerad/cameras/camera_common.h"
|
||||
|
||||
#include <cassert>
|
||||
|
||||
#include "common/params.h"
|
||||
#include "common/util.h"
|
||||
#include "system/hardware/hw.h"
|
||||
|
||||
int main(int argc, char *argv[]) {
|
||||
if (Hardware::PC()) {
|
||||
printf("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;
|
||||
}
|
||||
Executable
+126
@@ -0,0 +1,126 @@
|
||||
#!/usr/bin/env python3
|
||||
import subprocess
|
||||
import time
|
||||
|
||||
import numpy as np
|
||||
from PIL import Image
|
||||
|
||||
import cereal.messaging as messaging
|
||||
from cereal.visionipc import VisionIpcClient, VisionStreamType
|
||||
from common.params import Params
|
||||
from common.realtime import DT_MDL
|
||||
from system.hardware import PC
|
||||
from selfdrive.controls.lib.alertmanager import set_offroad_alert
|
||||
from selfdrive.manager.process_config import managed_processes
|
||||
|
||||
LM_THRESH = 120 # defined in system/camerad/imgproc/utils.h
|
||||
|
||||
VISION_STREAMS = {
|
||||
"roadCameraState": VisionStreamType.VISION_STREAM_ROAD,
|
||||
"driverCameraState": VisionStreamType.VISION_STREAM_DRIVER,
|
||||
"wideRoadCameraState": VisionStreamType.VISION_STREAM_WIDE_ROAD,
|
||||
}
|
||||
|
||||
|
||||
def jpeg_write(fn, dat):
|
||||
img = Image.fromarray(dat)
|
||||
img.save(fn, "JPEG")
|
||||
|
||||
|
||||
def yuv_to_rgb(y, u, v):
|
||||
ul = np.repeat(np.repeat(u, 2).reshape(u.shape[0], y.shape[1]), 2, axis=0).reshape(y.shape)
|
||||
vl = np.repeat(np.repeat(v, 2).reshape(v.shape[0], y.shape[1]), 2, axis=0).reshape(y.shape)
|
||||
|
||||
yuv = np.dstack((y, ul, vl)).astype(np.int16)
|
||||
yuv[:, :, 1:] -= 128
|
||||
|
||||
m = np.array([
|
||||
[1.00000, 1.00000, 1.00000],
|
||||
[0.00000, -0.39465, 2.03211],
|
||||
[1.13983, -0.58060, 0.00000],
|
||||
])
|
||||
rgb = np.dot(yuv, m).clip(0, 255)
|
||||
return rgb.astype(np.uint8)
|
||||
|
||||
|
||||
def extract_image(buf):
|
||||
y = np.array(buf.data[:buf.uv_offset], dtype=np.uint8).reshape((-1, buf.stride))[:buf.height, :buf.width]
|
||||
u = np.array(buf.data[buf.uv_offset::2], dtype=np.uint8).reshape((-1, buf.stride//2))[:buf.height//2, :buf.width//2]
|
||||
v = np.array(buf.data[buf.uv_offset+1::2], dtype=np.uint8).reshape((-1, buf.stride//2))[:buf.height//2, :buf.width//2]
|
||||
|
||||
return yuv_to_rgb(y, u, v)
|
||||
|
||||
|
||||
def get_snapshots(frame="roadCameraState", front_frame="driverCameraState"):
|
||||
sockets = [s for s in (frame, front_frame) if s is not None]
|
||||
sm = messaging.SubMaster(sockets)
|
||||
vipc_clients = {s: VisionIpcClient("camerad", VISION_STREAMS[s], True) for s in sockets}
|
||||
|
||||
# wait 4 sec from camerad startup for focus and exposure
|
||||
while sm[sockets[0]].frameId < int(4. / DT_MDL):
|
||||
sm.update()
|
||||
|
||||
for client in vipc_clients.values():
|
||||
client.connect(True)
|
||||
|
||||
# grab images
|
||||
rear, front = None, None
|
||||
if frame is not None:
|
||||
c = vipc_clients[frame]
|
||||
rear = extract_image(c.recv())
|
||||
if front_frame is not None:
|
||||
c = vipc_clients[front_frame]
|
||||
front = extract_image(c.recv())
|
||||
return rear, front
|
||||
|
||||
|
||||
def snapshot():
|
||||
params = Params()
|
||||
|
||||
if (not params.get_bool("IsOffroad")) or params.get_bool("IsTakingSnapshot"):
|
||||
print("Already taking snapshot")
|
||||
return None, None
|
||||
|
||||
front_camera_allowed = params.get_bool("RecordFront")
|
||||
params.put_bool("IsTakingSnapshot", True)
|
||||
set_offroad_alert("Offroad_IsTakingSnapshot", True)
|
||||
time.sleep(2.0) # Give thermald time to read the param, or if just started give camerad time to start
|
||||
|
||||
# Check if camerad is already started
|
||||
try:
|
||||
subprocess.check_call(["pgrep", "camerad"])
|
||||
print("Camerad already running")
|
||||
params.put_bool("IsTakingSnapshot", False)
|
||||
params.remove("Offroad_IsTakingSnapshot")
|
||||
return None, None
|
||||
except subprocess.CalledProcessError:
|
||||
pass
|
||||
|
||||
try:
|
||||
# Allow testing on replay on PC
|
||||
if not PC:
|
||||
managed_processes['camerad'].start()
|
||||
|
||||
frame = "wideRoadCameraState"
|
||||
front_frame = "driverCameraState" if front_camera_allowed else None
|
||||
rear, front = get_snapshots(frame, front_frame)
|
||||
finally:
|
||||
managed_processes['camerad'].stop()
|
||||
params.put_bool("IsTakingSnapshot", False)
|
||||
set_offroad_alert("Offroad_IsTakingSnapshot", False)
|
||||
|
||||
if not front_camera_allowed:
|
||||
front = None
|
||||
|
||||
return rear, front
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
pic, fpic = snapshot()
|
||||
if pic is not None:
|
||||
print(pic.shape)
|
||||
jpeg_write("/tmp/back.jpg", pic)
|
||||
if fpic is not None:
|
||||
jpeg_write("/tmp/front.jpg", fpic)
|
||||
else:
|
||||
print("Error taking snapshot")
|
||||
@@ -0,0 +1 @@
|
||||
clocksd
|
||||
@@ -0,0 +1,2 @@
|
||||
Import('env', 'common', 'cereal', 'messaging')
|
||||
env.Program('clocksd.cc', LIBS=[common, cereal, messaging, 'capnp', 'zmq', 'kj'])
|
||||
@@ -0,0 +1,73 @@
|
||||
#include <sys/resource.h>
|
||||
#include <sys/time.h>
|
||||
#include <unistd.h>
|
||||
|
||||
#include <cstdint>
|
||||
#include <cstdio>
|
||||
|
||||
// Apple doesn't have timerfd
|
||||
#ifdef __APPLE__
|
||||
#include <thread>
|
||||
#else
|
||||
#include <sys/timerfd.h>
|
||||
#endif
|
||||
|
||||
#include <cassert>
|
||||
#include <chrono>
|
||||
|
||||
#include "cereal/messaging/messaging.h"
|
||||
#include "common/timing.h"
|
||||
#include "common/util.h"
|
||||
|
||||
ExitHandler do_exit;
|
||||
|
||||
int main() {
|
||||
setpriority(PRIO_PROCESS, 0, -13);
|
||||
PubMaster pm({"clocks"});
|
||||
|
||||
#ifndef __APPLE__
|
||||
int timerfd = timerfd_create(CLOCK_BOOTTIME, 0);
|
||||
assert(timerfd >= 0);
|
||||
|
||||
struct itimerspec spec = {0};
|
||||
spec.it_interval.tv_sec = 1;
|
||||
spec.it_interval.tv_nsec = 0;
|
||||
spec.it_value.tv_sec = 1;
|
||||
spec.it_value.tv_nsec = 0;
|
||||
|
||||
int err = timerfd_settime(timerfd, 0, &spec, 0);
|
||||
assert(err == 0);
|
||||
|
||||
uint64_t expirations = 0;
|
||||
while (!do_exit && (err = read(timerfd, &expirations, sizeof(expirations)))) {
|
||||
if (err < 0) {
|
||||
if (errno == EINTR) continue;
|
||||
break;
|
||||
}
|
||||
#else
|
||||
// Just run at 1Hz on apple
|
||||
while (!do_exit) {
|
||||
util::sleep_for(1000);
|
||||
#endif
|
||||
|
||||
uint64_t boottime = nanos_since_boot();
|
||||
uint64_t monotonic = nanos_monotonic();
|
||||
uint64_t monotonic_raw = nanos_monotonic_raw();
|
||||
uint64_t wall_time = nanos_since_epoch();
|
||||
|
||||
MessageBuilder msg;
|
||||
auto clocks = msg.initEvent().initClocks();
|
||||
|
||||
clocks.setBootTimeNanos(boottime);
|
||||
clocks.setMonotonicNanos(monotonic);
|
||||
clocks.setMonotonicRawNanos(monotonic_raw);
|
||||
clocks.setWallTimeNanos(wall_time);
|
||||
|
||||
pm.send("clocks", msg);
|
||||
}
|
||||
|
||||
#ifndef __APPLE__
|
||||
close(timerfd);
|
||||
#endif
|
||||
return 0;
|
||||
}
|
||||
@@ -0,0 +1,16 @@
|
||||
import os
|
||||
from typing import cast
|
||||
|
||||
from system.hardware.base import HardwareBase
|
||||
from system.hardware.tici.hardware import Tici
|
||||
from system.hardware.pc.hardware import Pc
|
||||
|
||||
TICI = os.path.isfile('/TICI')
|
||||
AGNOS = os.path.isfile('/AGNOS')
|
||||
PC = not TICI
|
||||
|
||||
|
||||
if TICI:
|
||||
HARDWARE = cast(HardwareBase, Tici())
|
||||
else:
|
||||
HARDWARE = cast(HardwareBase, Pc())
|
||||
@@ -0,0 +1,37 @@
|
||||
#pragma once
|
||||
|
||||
#include <cstdlib>
|
||||
#include <fstream>
|
||||
#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 poweroff() {}
|
||||
static void set_brightness(int percent) {}
|
||||
static void set_display_power(bool on) {}
|
||||
static void set_volume(float volume) {}
|
||||
|
||||
static bool get_ssh_enabled() { return false; }
|
||||
static void set_ssh_enabled(bool enabled) {}
|
||||
|
||||
static bool PC() { return false; }
|
||||
static bool TICI() { return false; }
|
||||
static bool AGNOS() { return false; }
|
||||
};
|
||||
@@ -0,0 +1,148 @@
|
||||
from abc import abstractmethod, ABC
|
||||
from collections import namedtuple
|
||||
from typing import Dict
|
||||
|
||||
from cereal import log
|
||||
|
||||
ThermalConfig = namedtuple('ThermalConfig', ['cpu', 'gpu', 'mem', 'bat', 'ambient', 'pmic'])
|
||||
NetworkType = log.DeviceState.NetworkType
|
||||
|
||||
|
||||
class HardwareBase(ABC):
|
||||
@staticmethod
|
||||
def get_cmdline() -> Dict[str, str]:
|
||||
with open('/proc/cmdline') as f:
|
||||
cmdline = f.read()
|
||||
return {kv[0]: kv[1] for kv in [s.split('=') for s in cmdline.split(' ')] if len(kv) == 2}
|
||||
|
||||
@staticmethod
|
||||
def read_param_file(path, parser, default=0):
|
||||
try:
|
||||
with open(path) as f:
|
||||
return parser(f.read())
|
||||
except Exception:
|
||||
return default
|
||||
|
||||
@abstractmethod
|
||||
def reboot(self, reason=None):
|
||||
pass
|
||||
|
||||
@abstractmethod
|
||||
def uninstall(self):
|
||||
pass
|
||||
|
||||
@abstractmethod
|
||||
def get_os_version(self):
|
||||
pass
|
||||
|
||||
@abstractmethod
|
||||
def get_device_type(self):
|
||||
pass
|
||||
|
||||
@abstractmethod
|
||||
def get_sound_card_online(self):
|
||||
pass
|
||||
|
||||
@abstractmethod
|
||||
def get_imei(self, slot) -> str:
|
||||
pass
|
||||
|
||||
@abstractmethod
|
||||
def get_serial(self):
|
||||
pass
|
||||
|
||||
@abstractmethod
|
||||
def get_subscriber_info(self):
|
||||
pass
|
||||
|
||||
@abstractmethod
|
||||
def get_network_info(self):
|
||||
pass
|
||||
|
||||
@abstractmethod
|
||||
def get_network_type(self):
|
||||
pass
|
||||
|
||||
@abstractmethod
|
||||
def get_sim_info(self):
|
||||
pass
|
||||
|
||||
@abstractmethod
|
||||
def get_network_strength(self, network_type):
|
||||
pass
|
||||
|
||||
def get_network_metered(self, network_type) -> bool:
|
||||
return network_type not in (NetworkType.none, NetworkType.wifi, NetworkType.ethernet)
|
||||
|
||||
@staticmethod
|
||||
def set_bandwidth_limit(upload_speed_kbps: int, download_speed_kbps: int) -> None:
|
||||
pass
|
||||
|
||||
@abstractmethod
|
||||
def get_current_power_draw(self):
|
||||
pass
|
||||
|
||||
@abstractmethod
|
||||
def get_som_power_draw(self):
|
||||
pass
|
||||
|
||||
@abstractmethod
|
||||
def shutdown(self):
|
||||
pass
|
||||
|
||||
@abstractmethod
|
||||
def get_thermal_config(self):
|
||||
pass
|
||||
|
||||
@abstractmethod
|
||||
def set_screen_brightness(self, percentage):
|
||||
pass
|
||||
|
||||
@abstractmethod
|
||||
def get_screen_brightness(self):
|
||||
pass
|
||||
|
||||
@abstractmethod
|
||||
def set_power_save(self, powersave_enabled):
|
||||
pass
|
||||
|
||||
@abstractmethod
|
||||
def get_gpu_usage_percent(self):
|
||||
pass
|
||||
|
||||
def get_modem_version(self):
|
||||
return None
|
||||
|
||||
def get_modem_nv(self):
|
||||
return None
|
||||
|
||||
@abstractmethod
|
||||
def get_modem_temperatures(self):
|
||||
pass
|
||||
|
||||
@abstractmethod
|
||||
def get_nvme_temperatures(self):
|
||||
pass
|
||||
|
||||
@abstractmethod
|
||||
def initialize_hardware(self):
|
||||
pass
|
||||
|
||||
def configure_modem(self):
|
||||
pass
|
||||
|
||||
@abstractmethod
|
||||
def get_networks(self):
|
||||
pass
|
||||
|
||||
def has_internal_panda(self) -> bool:
|
||||
return False
|
||||
|
||||
def reset_internal_panda(self):
|
||||
pass
|
||||
|
||||
def recover_internal_panda(self):
|
||||
pass
|
||||
|
||||
def get_modem_data_usage(self):
|
||||
return -1, -1
|
||||
@@ -0,0 +1,27 @@
|
||||
#pragma once
|
||||
|
||||
#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 log_root() {
|
||||
if (const char *env = getenv("LOG_ROOT")) {
|
||||
return env;
|
||||
}
|
||||
return Hardware::PC() ? util::getenv("HOME") + "/.comma/media/0/realdata" : "/data/media/0/realdata";
|
||||
}
|
||||
inline std::string params() {
|
||||
return Hardware::PC() ? util::getenv("PARAMS_ROOT", util::getenv("HOME") + "/.comma/params") : "/data/params";
|
||||
}
|
||||
inline std::string rsa_file() {
|
||||
return Hardware::PC() ? util::getenv("HOME") + "/.comma/persist/comma/id_rsa" : "/persist/comma/id_rsa";
|
||||
}
|
||||
} // namespace Path
|
||||
@@ -0,0 +1,21 @@
|
||||
#pragma once
|
||||
|
||||
#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 set_volume(float volume) {
|
||||
volume = util::map_val(volume, 0.f, 1.f, MIN_VOLUME, MAX_VOLUME);
|
||||
|
||||
char volume_str[6];
|
||||
snprintf(volume_str, sizeof(volume_str), "%.3f", volume);
|
||||
std::system(("pactl set-sink-volume @DEFAULT_SINK@ " + std::string(volume_str)).c_str());
|
||||
}
|
||||
};
|
||||
@@ -0,0 +1,87 @@
|
||||
import random
|
||||
|
||||
from cereal import log
|
||||
from system.hardware.base import HardwareBase, ThermalConfig
|
||||
|
||||
NetworkType = log.DeviceState.NetworkType
|
||||
NetworkStrength = log.DeviceState.NetworkStrength
|
||||
|
||||
|
||||
class Pc(HardwareBase):
|
||||
def get_os_version(self):
|
||||
return None
|
||||
|
||||
def get_device_type(self):
|
||||
return "pc"
|
||||
|
||||
def get_sound_card_online(self):
|
||||
return True
|
||||
|
||||
def reboot(self, reason=None):
|
||||
print("REBOOT!")
|
||||
|
||||
def uninstall(self):
|
||||
print("uninstall")
|
||||
|
||||
def get_imei(self, slot):
|
||||
return "%015d" % random.randint(0, 1 << 32)
|
||||
|
||||
def get_serial(self):
|
||||
return "cccccccc"
|
||||
|
||||
def get_subscriber_info(self):
|
||||
return ""
|
||||
|
||||
def get_network_info(self):
|
||||
return None
|
||||
|
||||
def get_network_type(self):
|
||||
return NetworkType.wifi
|
||||
|
||||
def get_sim_info(self):
|
||||
return {
|
||||
'sim_id': '',
|
||||
'mcc_mnc': None,
|
||||
'network_type': ["Unknown"],
|
||||
'sim_state': ["ABSENT"],
|
||||
'data_connected': False
|
||||
}
|
||||
|
||||
def get_network_strength(self, network_type):
|
||||
return NetworkStrength.unknown
|
||||
|
||||
def get_current_power_draw(self):
|
||||
return 0
|
||||
|
||||
def get_som_power_draw(self):
|
||||
return 0
|
||||
|
||||
def shutdown(self):
|
||||
print("SHUTDOWN!")
|
||||
|
||||
def get_thermal_config(self):
|
||||
return ThermalConfig(cpu=((None,), 1), gpu=((None,), 1), mem=(None, 1), bat=(None, 1), ambient=(None, 1), pmic=((None,), 1))
|
||||
|
||||
def set_screen_brightness(self, percentage):
|
||||
pass
|
||||
|
||||
def get_screen_brightness(self):
|
||||
return 0
|
||||
|
||||
def set_power_save(self, powersave_enabled):
|
||||
pass
|
||||
|
||||
def get_gpu_usage_percent(self):
|
||||
return 0
|
||||
|
||||
def get_modem_temperatures(self):
|
||||
return []
|
||||
|
||||
def get_nvme_temperatures(self):
|
||||
return []
|
||||
|
||||
def initialize_hardware(self):
|
||||
pass
|
||||
|
||||
def get_networks(self):
|
||||
return None
|
||||
@@ -0,0 +1,72 @@
|
||||
[
|
||||
{
|
||||
"name": "boot",
|
||||
"url": "https://commadist.azureedge.net/agnosupdate/boot-8d8d8620de8b2687f3a8fffdb81b2abd1fe2ead5bc831361a1a212e5589ac279.img.xz",
|
||||
"hash": "8d8d8620de8b2687f3a8fffdb81b2abd1fe2ead5bc831361a1a212e5589ac279",
|
||||
"hash_raw": "8d8d8620de8b2687f3a8fffdb81b2abd1fe2ead5bc831361a1a212e5589ac279",
|
||||
"size": 15636480,
|
||||
"sparse": false,
|
||||
"full_check": true,
|
||||
"has_ab": true
|
||||
},
|
||||
{
|
||||
"name": "abl",
|
||||
"url": "https://commadist.azureedge.net/agnosupdate/abl-0084fcf79fea067632a1c2d9519b6445ad484aa8b09f49f22e6b45b4dccacd2d.img.xz",
|
||||
"hash": "0084fcf79fea067632a1c2d9519b6445ad484aa8b09f49f22e6b45b4dccacd2d",
|
||||
"hash_raw": "0084fcf79fea067632a1c2d9519b6445ad484aa8b09f49f22e6b45b4dccacd2d",
|
||||
"size": 274432,
|
||||
"sparse": false,
|
||||
"full_check": true,
|
||||
"has_ab": true
|
||||
},
|
||||
{
|
||||
"name": "xbl",
|
||||
"url": "https://commadist.azureedge.net/agnosupdate/xbl-942b9b2914d89c2a70fdf27380b59e04b549ac2fd53ecb29d6549d1a9c8daeaa.img.xz",
|
||||
"hash": "942b9b2914d89c2a70fdf27380b59e04b549ac2fd53ecb29d6549d1a9c8daeaa",
|
||||
"hash_raw": "942b9b2914d89c2a70fdf27380b59e04b549ac2fd53ecb29d6549d1a9c8daeaa",
|
||||
"size": 3282672,
|
||||
"sparse": false,
|
||||
"full_check": true,
|
||||
"has_ab": true
|
||||
},
|
||||
{
|
||||
"name": "xbl_config",
|
||||
"url": "https://commadist.azureedge.net/agnosupdate/xbl_config-6881d94599f65d94c13bcc0bd860184dfba2dfe96ec776d08fb35ac5b5f85bbf.img.xz",
|
||||
"hash": "6881d94599f65d94c13bcc0bd860184dfba2dfe96ec776d08fb35ac5b5f85bbf",
|
||||
"hash_raw": "6881d94599f65d94c13bcc0bd860184dfba2dfe96ec776d08fb35ac5b5f85bbf",
|
||||
"size": 98124,
|
||||
"sparse": false,
|
||||
"full_check": true,
|
||||
"has_ab": true
|
||||
},
|
||||
{
|
||||
"name": "devcfg",
|
||||
"url": "https://commadist.azureedge.net/agnosupdate/devcfg-9bbf168baff6101f4890c5c95c118e30813c2610cfb35b8e19e363f04a32a262.img.xz",
|
||||
"hash": "9bbf168baff6101f4890c5c95c118e30813c2610cfb35b8e19e363f04a32a262",
|
||||
"hash_raw": "9bbf168baff6101f4890c5c95c118e30813c2610cfb35b8e19e363f04a32a262",
|
||||
"size": 40336,
|
||||
"sparse": false,
|
||||
"full_check": true,
|
||||
"has_ab": true
|
||||
},
|
||||
{
|
||||
"name": "aop",
|
||||
"url": "https://commadist.azureedge.net/agnosupdate/aop-c1d9d712980f6b2a4b12196597f4d1bf3fe4fec6c59edf29ae63ef21f11b8222.img.xz",
|
||||
"hash": "c1d9d712980f6b2a4b12196597f4d1bf3fe4fec6c59edf29ae63ef21f11b8222",
|
||||
"hash_raw": "c1d9d712980f6b2a4b12196597f4d1bf3fe4fec6c59edf29ae63ef21f11b8222",
|
||||
"size": 184364,
|
||||
"sparse": false,
|
||||
"full_check": true,
|
||||
"has_ab": true
|
||||
},
|
||||
{
|
||||
"name": "system",
|
||||
"url": "https://commadist.azureedge.net/agnosupdate/system-e1fa3018bce9bad01c6967e5e21f1141cf5c8f02d2edfaed51c738f74a32a432.img.xz",
|
||||
"hash": "611011f3e3f147bc24f371105a9dd3760ec11ba424c56d4a442a66b098c784c0",
|
||||
"hash_raw": "e1fa3018bce9bad01c6967e5e21f1141cf5c8f02d2edfaed51c738f74a32a432",
|
||||
"size": 10737418240,
|
||||
"sparse": true,
|
||||
"full_check": false,
|
||||
"has_ab": true
|
||||
}
|
||||
]
|
||||
Executable
+328
@@ -0,0 +1,328 @@
|
||||
#!/usr/bin/env python3
|
||||
import hashlib
|
||||
import json
|
||||
import lzma
|
||||
import os
|
||||
import struct
|
||||
import subprocess
|
||||
import time
|
||||
from typing import Dict, Generator, List, Tuple, Union
|
||||
|
||||
import requests
|
||||
|
||||
import system.hardware.tici.casync as casync
|
||||
|
||||
SPARSE_CHUNK_FMT = struct.Struct('H2xI4x')
|
||||
CAIBX_URL = "https://commadist.azureedge.net/agnosupdate/"
|
||||
|
||||
|
||||
class StreamingDecompressor:
|
||||
def __init__(self, url: str) -> None:
|
||||
self.buf = b""
|
||||
|
||||
self.req = requests.get(url, stream=True, headers={'Accept-Encoding': None}, timeout=60) # type: ignore
|
||||
self.it = self.req.iter_content(chunk_size=1024 * 1024)
|
||||
self.decompressor = lzma.LZMADecompressor(format=lzma.FORMAT_AUTO)
|
||||
self.eof = False
|
||||
self.sha256 = hashlib.sha256()
|
||||
|
||||
def read(self, length: int) -> bytes:
|
||||
while len(self.buf) < length:
|
||||
self.req.raise_for_status()
|
||||
|
||||
try:
|
||||
compressed = next(self.it)
|
||||
except StopIteration:
|
||||
self.eof = True
|
||||
break
|
||||
out = self.decompressor.decompress(compressed)
|
||||
self.buf += out
|
||||
|
||||
result = self.buf[:length]
|
||||
self.buf = self.buf[length:]
|
||||
|
||||
self.sha256.update(result)
|
||||
return result
|
||||
|
||||
|
||||
def unsparsify(f: StreamingDecompressor) -> Generator[bytes, None, None]:
|
||||
# https://source.android.com/devices/bootloader/images#sparse-format
|
||||
magic = struct.unpack("I", f.read(4))[0]
|
||||
assert(magic == 0xed26ff3a)
|
||||
|
||||
# Version
|
||||
major = struct.unpack("H", f.read(2))[0]
|
||||
minor = struct.unpack("H", f.read(2))[0]
|
||||
assert(major == 1 and minor == 0)
|
||||
|
||||
f.read(2) # file header size
|
||||
f.read(2) # chunk header size
|
||||
|
||||
block_sz = struct.unpack("I", f.read(4))[0]
|
||||
f.read(4) # total blocks
|
||||
num_chunks = struct.unpack("I", f.read(4))[0]
|
||||
f.read(4) # crc checksum
|
||||
|
||||
for _ in range(num_chunks):
|
||||
chunk_type, out_blocks = SPARSE_CHUNK_FMT.unpack(f.read(12))
|
||||
|
||||
if chunk_type == 0xcac1: # Raw
|
||||
# TODO: yield in smaller chunks. Yielding only block_sz is too slow. Largest observed data chunk is 252 MB.
|
||||
yield f.read(out_blocks * block_sz)
|
||||
elif chunk_type == 0xcac2: # Fill
|
||||
filler = f.read(4) * (block_sz // 4)
|
||||
for _ in range(out_blocks):
|
||||
yield filler
|
||||
elif chunk_type == 0xcac3: # Don't care
|
||||
yield b""
|
||||
else:
|
||||
raise Exception("Unhandled sparse chunk type")
|
||||
|
||||
|
||||
# noop wrapper with same API as unsparsify() for non sparse images
|
||||
def noop(f: StreamingDecompressor) -> Generator[bytes, None, None]:
|
||||
while not f.eof:
|
||||
yield f.read(1024 * 1024)
|
||||
|
||||
|
||||
def get_target_slot_number() -> int:
|
||||
current_slot = subprocess.check_output(["abctl", "--boot_slot"], encoding='utf-8').strip()
|
||||
return 1 if current_slot == "_a" else 0
|
||||
|
||||
|
||||
def slot_number_to_suffix(slot_number: int) -> str:
|
||||
assert slot_number in (0, 1)
|
||||
return '_a' if slot_number == 0 else '_b'
|
||||
|
||||
|
||||
def get_partition_path(target_slot_number: int, partition: dict) -> str:
|
||||
path = f"/dev/disk/by-partlabel/{partition['name']}"
|
||||
|
||||
if partition.get('has_ab', True):
|
||||
path += slot_number_to_suffix(target_slot_number)
|
||||
|
||||
return path
|
||||
|
||||
|
||||
def get_raw_hash(path: str, partition_size: int) -> str:
|
||||
raw_hash = hashlib.sha256()
|
||||
pos, chunk_size = 0, 1024 * 1024
|
||||
|
||||
with open(path, 'rb+') as out:
|
||||
while pos < partition_size:
|
||||
n = min(chunk_size, partition_size - pos)
|
||||
raw_hash.update(out.read(n))
|
||||
pos += n
|
||||
|
||||
return raw_hash.hexdigest().lower()
|
||||
|
||||
|
||||
def verify_partition(target_slot_number: int, partition: Dict[str, Union[str, int]], force_full_check: bool = False) -> bool:
|
||||
full_check = partition['full_check'] or force_full_check
|
||||
path = get_partition_path(target_slot_number, partition)
|
||||
|
||||
if not isinstance(partition['size'], int):
|
||||
return False
|
||||
|
||||
partition_size: int = partition['size']
|
||||
|
||||
if not isinstance(partition['hash_raw'], str):
|
||||
return False
|
||||
|
||||
partition_hash: str = partition['hash_raw']
|
||||
|
||||
if full_check:
|
||||
return get_raw_hash(path, partition_size) == partition_hash.lower()
|
||||
else:
|
||||
with open(path, 'rb+') as out:
|
||||
out.seek(partition_size)
|
||||
return out.read(64) == partition_hash.lower().encode()
|
||||
|
||||
|
||||
def clear_partition_hash(target_slot_number: int, partition: dict) -> None:
|
||||
path = get_partition_path(target_slot_number, partition)
|
||||
with open(path, 'wb+') as out:
|
||||
partition_size = partition['size']
|
||||
|
||||
out.seek(partition_size)
|
||||
out.write(b"\x00" * 64)
|
||||
os.sync()
|
||||
|
||||
|
||||
def extract_compressed_image(target_slot_number: int, partition: dict, cloudlog):
|
||||
path = get_partition_path(target_slot_number, partition)
|
||||
downloader = StreamingDecompressor(partition['url'])
|
||||
|
||||
with open(path, 'wb+') as out:
|
||||
# Flash partition
|
||||
last_p = 0
|
||||
raw_hash = hashlib.sha256()
|
||||
f = unsparsify if partition['sparse'] else noop
|
||||
for chunk in f(downloader):
|
||||
raw_hash.update(chunk)
|
||||
out.write(chunk)
|
||||
p = int(out.tell() / partition['size'] * 100)
|
||||
if p != last_p:
|
||||
last_p = p
|
||||
print(f"Installing {partition['name']}: {p}", flush=True)
|
||||
|
||||
if raw_hash.hexdigest().lower() != partition['hash_raw'].lower():
|
||||
raise Exception(f"Raw hash mismatch '{raw_hash.hexdigest().lower()}'")
|
||||
|
||||
if downloader.sha256.hexdigest().lower() != partition['hash'].lower():
|
||||
raise Exception("Uncompressed hash mismatch")
|
||||
|
||||
if out.tell() != partition['size']:
|
||||
raise Exception("Uncompressed size mismatch")
|
||||
|
||||
os.sync()
|
||||
|
||||
|
||||
def extract_casync_image(target_slot_number: int, partition: dict, cloudlog):
|
||||
path = get_partition_path(target_slot_number, partition)
|
||||
seed_path = path[:-1] + ('b' if path[-1] == 'a' else 'a')
|
||||
|
||||
target = casync.parse_caibx(partition['casync_caibx'])
|
||||
|
||||
sources: List[Tuple[str, casync.ChunkReader, casync.ChunkDict]] = []
|
||||
|
||||
# First source is the current partition.
|
||||
try:
|
||||
raw_hash = get_raw_hash(seed_path, partition['size'])
|
||||
caibx_url = f"{CAIBX_URL}{partition['name']}-{raw_hash}.caibx"
|
||||
|
||||
try:
|
||||
cloudlog.info(f"casync fetching {caibx_url}")
|
||||
sources += [('seed', casync.FileChunkReader(seed_path), casync.build_chunk_dict(casync.parse_caibx(caibx_url)))]
|
||||
except requests.RequestException:
|
||||
cloudlog.error(f"casync failed to load {caibx_url}")
|
||||
except Exception:
|
||||
cloudlog.exception("casync failed to hash seed partition")
|
||||
|
||||
# Second source is the target partition, this allows for resuming
|
||||
sources += [('target', casync.FileChunkReader(path), casync.build_chunk_dict(target))]
|
||||
|
||||
# Finally we add the remote source to download any missing chunks
|
||||
sources += [('remote', casync.RemoteChunkReader(partition['casync_store']), casync.build_chunk_dict(target))]
|
||||
|
||||
last_p = 0
|
||||
|
||||
def progress(cur):
|
||||
nonlocal last_p
|
||||
p = int(cur / partition['size'] * 100)
|
||||
if p != last_p:
|
||||
last_p = p
|
||||
print(f"Installing {partition['name']}: {p}", flush=True)
|
||||
|
||||
stats = casync.extract(target, sources, path, progress)
|
||||
cloudlog.error(f'casync done {json.dumps(stats)}')
|
||||
|
||||
os.sync()
|
||||
if not verify_partition(target_slot_number, partition, force_full_check=True):
|
||||
raise Exception(f"Raw hash mismatch '{partition['hash_raw'].lower()}'")
|
||||
|
||||
|
||||
def flash_partition(target_slot_number: int, partition: dict, cloudlog, standalone=False):
|
||||
cloudlog.info(f"Downloading and writing {partition['name']}")
|
||||
|
||||
if verify_partition(target_slot_number, partition):
|
||||
cloudlog.info(f"Already flashed {partition['name']}")
|
||||
return
|
||||
|
||||
# Clear hash before flashing in case we get interrupted
|
||||
full_check = partition['full_check']
|
||||
if not full_check:
|
||||
clear_partition_hash(target_slot_number, partition)
|
||||
|
||||
path = get_partition_path(target_slot_number, partition)
|
||||
|
||||
if ('casync_caibx' in partition) and not standalone:
|
||||
extract_casync_image(target_slot_number, partition, cloudlog)
|
||||
else:
|
||||
extract_compressed_image(target_slot_number, partition, cloudlog)
|
||||
|
||||
# Write hash after successful flash
|
||||
if not full_check:
|
||||
with open(path, 'wb+') as out:
|
||||
out.seek(partition['size'])
|
||||
out.write(partition['hash_raw'].lower().encode())
|
||||
|
||||
|
||||
def swap(manifest_path: str, target_slot_number: int, cloudlog) -> None:
|
||||
update = json.load(open(manifest_path))
|
||||
for partition in update:
|
||||
if not partition.get('full_check', False):
|
||||
clear_partition_hash(target_slot_number, partition)
|
||||
|
||||
while True:
|
||||
out = subprocess.check_output(f"abctl --set_active {target_slot_number}", shell=True, stderr=subprocess.STDOUT, encoding='utf8')
|
||||
if ("No such file or directory" not in out) and ("lun as boot lun" in out):
|
||||
cloudlog.info(f"Swap successful {out}")
|
||||
break
|
||||
else:
|
||||
cloudlog.error(f"Swap failed {out}")
|
||||
|
||||
|
||||
def flash_agnos_update(manifest_path: str, target_slot_number: int, cloudlog, standalone=False) -> None:
|
||||
update = json.load(open(manifest_path))
|
||||
|
||||
cloudlog.info(f"Target slot {target_slot_number}")
|
||||
|
||||
# set target slot as unbootable
|
||||
os.system(f"abctl --set_unbootable {target_slot_number}")
|
||||
|
||||
for partition in update:
|
||||
success = False
|
||||
|
||||
for retries in range(10):
|
||||
try:
|
||||
flash_partition(target_slot_number, partition, cloudlog, standalone)
|
||||
success = True
|
||||
break
|
||||
|
||||
except requests.exceptions.RequestException:
|
||||
cloudlog.exception("Failed")
|
||||
cloudlog.info(f"Failed to download {partition['name']}, retrying ({retries})")
|
||||
time.sleep(10)
|
||||
|
||||
if not success:
|
||||
cloudlog.info(f"Failed to flash {partition['name']}, aborting")
|
||||
raise Exception("Maximum retries exceeded")
|
||||
|
||||
cloudlog.info(f"AGNOS ready on slot {target_slot_number}")
|
||||
|
||||
|
||||
def verify_agnos_update(manifest_path: str, target_slot_number: int) -> bool:
|
||||
update = json.load(open(manifest_path))
|
||||
return all(verify_partition(target_slot_number, partition) for partition in update)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
import argparse
|
||||
import logging
|
||||
|
||||
parser = argparse.ArgumentParser(description="Flash and verify AGNOS update",
|
||||
formatter_class=argparse.ArgumentDefaultsHelpFormatter)
|
||||
|
||||
parser.add_argument("--verify", action="store_true", help="Verify and perform swap if update ready")
|
||||
parser.add_argument("--swap", action="store_true", help="Verify and perform swap, downloads if necessary")
|
||||
parser.add_argument("manifest", help="Manifest json")
|
||||
args = parser.parse_args()
|
||||
|
||||
logging.basicConfig(level=logging.INFO)
|
||||
|
||||
target_slot_number = get_target_slot_number()
|
||||
if args.verify:
|
||||
if verify_agnos_update(args.manifest, target_slot_number):
|
||||
swap(args.manifest, target_slot_number, logging)
|
||||
exit(0)
|
||||
exit(1)
|
||||
elif args.swap:
|
||||
while not verify_agnos_update(args.manifest, target_slot_number):
|
||||
logging.error("Verification failed. Flashing AGNOS")
|
||||
flash_agnos_update(args.manifest, target_slot_number, logging, standalone=True)
|
||||
|
||||
logging.warning(f"Verification succeeded. Swapping to slot {target_slot_number}")
|
||||
swap(args.manifest, target_slot_number, logging)
|
||||
else:
|
||||
flash_agnos_update(args.manifest, target_slot_number, logging, standalone=True)
|
||||
Executable
+156
@@ -0,0 +1,156 @@
|
||||
import time
|
||||
from smbus2 import SMBus
|
||||
from collections import namedtuple
|
||||
from typing import List
|
||||
|
||||
# https://datasheets.maximintegrated.com/en/ds/MAX98089.pdf
|
||||
|
||||
AmpConfig = namedtuple('AmpConfig', ['name', 'value', 'register', 'offset', 'mask'])
|
||||
EQParams = namedtuple('EQParams', ['K', 'k1', 'k2', 'c1', 'c2'])
|
||||
|
||||
def configs_from_eq_params(base, eq_params):
|
||||
return [
|
||||
AmpConfig("K (high)", (eq_params.K >> 8), base, 0, 0xFF),
|
||||
AmpConfig("K (low)", (eq_params.K & 0xFF), base + 1, 0, 0xFF),
|
||||
AmpConfig("k1 (high)", (eq_params.k1 >> 8), base + 2, 0, 0xFF),
|
||||
AmpConfig("k1 (low)", (eq_params.k1 & 0xFF), base + 3, 0, 0xFF),
|
||||
AmpConfig("k2 (high)", (eq_params.k2 >> 8), base + 4, 0, 0xFF),
|
||||
AmpConfig("k2 (low)", (eq_params.k2 & 0xFF), base + 5, 0, 0xFF),
|
||||
AmpConfig("c1 (high)", (eq_params.c1 >> 8), base + 6, 0, 0xFF),
|
||||
AmpConfig("c1 (low)", (eq_params.c1 & 0xFF), base + 7, 0, 0xFF),
|
||||
AmpConfig("c2 (high)", (eq_params.c2 >> 8), base + 8, 0, 0xFF),
|
||||
AmpConfig("c2 (low)", (eq_params.c2 & 0xFF), base + 9, 0, 0xFF),
|
||||
]
|
||||
|
||||
BASE_CONFIG = [
|
||||
AmpConfig("MCLK prescaler", 0b01, 0x10, 4, 0b00110000),
|
||||
AmpConfig("PM: enable speakers", 0b11, 0x4D, 4, 0b00110000),
|
||||
AmpConfig("PM: enable DACs", 0b11, 0x4D, 0, 0b00000011),
|
||||
AmpConfig("Enable PLL1", 0b1, 0x12, 7, 0b10000000),
|
||||
AmpConfig("Enable PLL2", 0b1, 0x1A, 7, 0b10000000),
|
||||
AmpConfig("DAI1: I2S mode", 0b00100, 0x14, 2, 0b01111100),
|
||||
AmpConfig("DAI2: I2S mode", 0b00100, 0x1C, 2, 0b01111100),
|
||||
AmpConfig("DAI1 Passband filtering: music mode", 0b1, 0x18, 7, 0b10000000),
|
||||
AmpConfig("DAI1 voice mode gain (DV1G)", 0b00, 0x2F, 4, 0b00110000),
|
||||
AmpConfig("DAI1 attenuation (DV1)", 0x0, 0x2F, 0, 0b00001111),
|
||||
AmpConfig("DAI2 attenuation (DV2)", 0x0, 0x31, 0, 0b00001111),
|
||||
AmpConfig("DAI2: DC blocking", 0b1, 0x20, 0, 0b00000001),
|
||||
AmpConfig("DAI2: High sample rate", 0b0, 0x20, 3, 0b00001000),
|
||||
AmpConfig("ALC enable", 0b1, 0x43, 7, 0b10000000),
|
||||
AmpConfig("ALC/excursion limiter release time", 0b101, 0x43, 4, 0b01110000),
|
||||
AmpConfig("ALC multiband enable", 0b1, 0x43, 3, 0b00001000),
|
||||
AmpConfig("DAI1 EQ enable", 0b0, 0x49, 0, 0b00000001),
|
||||
AmpConfig("DAI2 EQ clip detection disabled", 0b1, 0x32, 4, 0b00010000),
|
||||
AmpConfig("DAI2 EQ attenuation", 0x5, 0x32, 0, 0b00001111),
|
||||
AmpConfig("Excursion limiter upper corner freq", 0b100, 0x41, 4, 0b01110000),
|
||||
AmpConfig("Excursion limiter lower corner freq", 0b00, 0x41, 0, 0b00000011),
|
||||
AmpConfig("Excursion limiter threshold", 0b000, 0x42, 0, 0b00001111),
|
||||
AmpConfig("Distortion limit (THDCLP)", 0x6, 0x46, 4, 0b11110000),
|
||||
AmpConfig("Distortion limiter release time constant", 0b0, 0x46, 0, 0b00000001),
|
||||
AmpConfig("Right DAC input mixer: DAI1 left", 0b0, 0x22, 3, 0b00001000),
|
||||
AmpConfig("Right DAC input mixer: DAI1 right", 0b0, 0x22, 2, 0b00000100),
|
||||
AmpConfig("Right DAC input mixer: DAI2 left", 0b1, 0x22, 1, 0b00000010),
|
||||
AmpConfig("Right DAC input mixer: DAI2 right", 0b0, 0x22, 0, 0b00000001),
|
||||
AmpConfig("DAI1 audio port selector", 0b10, 0x16, 6, 0b11000000),
|
||||
AmpConfig("DAI2 audio port selector", 0b01, 0x1E, 6, 0b11000000),
|
||||
AmpConfig("Enable left digital microphone", 0b1, 0x48, 5, 0b00100000),
|
||||
AmpConfig("Enable right digital microphone", 0b1, 0x48, 4, 0b00010000),
|
||||
AmpConfig("Enhanced volume smoothing disabled", 0b0, 0x49, 7, 0b10000000),
|
||||
AmpConfig("Volume adjustment smoothing disabled", 0b0, 0x49, 6, 0b01000000),
|
||||
AmpConfig("Zero-crossing detection disabled", 0b0, 0x49, 5, 0b00100000),
|
||||
]
|
||||
|
||||
CONFIGS = {
|
||||
"tici": [
|
||||
AmpConfig("Right speaker output from right DAC", 0b1, 0x2C, 0, 0b11111111),
|
||||
AmpConfig("Right Speaker Mixer Gain", 0b00, 0x2D, 2, 0b00001100),
|
||||
AmpConfig("Right speaker output volume", 0x1c, 0x3E, 0, 0b00011111),
|
||||
AmpConfig("DAI2 EQ enable", 0b1, 0x49, 1, 0b00000010),
|
||||
|
||||
*configs_from_eq_params(0x84, EQParams(0x274F, 0xC0FF, 0x3BF9, 0x0B3C, 0x1656)),
|
||||
*configs_from_eq_params(0x8E, EQParams(0x1009, 0xC6BF, 0x2952, 0x1C97, 0x30DF)),
|
||||
*configs_from_eq_params(0x98, EQParams(0x0F75, 0xCBE5, 0x0ED2, 0x2528, 0x3E42)),
|
||||
*configs_from_eq_params(0xA2, EQParams(0x091F, 0x3D4C, 0xCE11, 0x1266, 0x2807)),
|
||||
*configs_from_eq_params(0xAC, EQParams(0x0A9E, 0x3F20, 0xE573, 0x0A8B, 0x3A3B)),
|
||||
],
|
||||
"tizi": [
|
||||
AmpConfig("Left speaker output from left DAC", 0b1, 0x2B, 0, 0b11111111),
|
||||
AmpConfig("Right speaker output from right DAC", 0b1, 0x2C, 0, 0b11111111),
|
||||
AmpConfig("Left Speaker Mixer Gain", 0b00, 0x2D, 0, 0b00000011),
|
||||
AmpConfig("Right Speaker Mixer Gain", 0b00, 0x2D, 2, 0b00001100),
|
||||
AmpConfig("Left speaker output volume", 0x17, 0x3D, 0, 0b00011111),
|
||||
AmpConfig("Right speaker output volume", 0x17, 0x3E, 0, 0b00011111),
|
||||
|
||||
AmpConfig("DAI2 EQ enable", 0b0, 0x49, 1, 0b00000010),
|
||||
AmpConfig("DAI2: DC blocking", 0b0, 0x20, 0, 0b00000001),
|
||||
AmpConfig("ALC enable", 0b0, 0x43, 7, 0b10000000),
|
||||
AmpConfig("DAI2 EQ attenuation", 0x2, 0x32, 0, 0b00001111),
|
||||
AmpConfig("Excursion limiter upper corner freq", 0b001, 0x41, 4, 0b01110000),
|
||||
AmpConfig("Excursion limiter threshold", 0b100, 0x42, 0, 0b00001111),
|
||||
AmpConfig("Distortion limit (THDCLP)", 0x0, 0x46, 4, 0b11110000),
|
||||
AmpConfig("Distortion limiter release time constant", 0b1, 0x46, 0, 0b00000001),
|
||||
AmpConfig("Left DAC input mixer: DAI1 left", 0b0, 0x22, 7, 0b10000000),
|
||||
AmpConfig("Left DAC input mixer: DAI1 right", 0b0, 0x22, 6, 0b01000000),
|
||||
AmpConfig("Left DAC input mixer: DAI2 left", 0b1, 0x22, 5, 0b00100000),
|
||||
AmpConfig("Left DAC input mixer: DAI2 right", 0b0, 0x22, 4, 0b00010000),
|
||||
AmpConfig("Right DAC input mixer: DAI2 left", 0b0, 0x22, 1, 0b00000010),
|
||||
AmpConfig("Right DAC input mixer: DAI2 right", 0b1, 0x22, 0, 0b00000001),
|
||||
AmpConfig("Volume adjustment smoothing disabled", 0b1, 0x49, 6, 0b01000000),
|
||||
],
|
||||
}
|
||||
|
||||
class Amplifier:
|
||||
AMP_I2C_BUS = 0
|
||||
AMP_ADDRESS = 0x10
|
||||
|
||||
def __init__(self, debug=False):
|
||||
self.debug = debug
|
||||
|
||||
def _get_shutdown_config(self, amp_disabled: bool) -> AmpConfig:
|
||||
return AmpConfig("Global shutdown", 0b0 if amp_disabled else 0b1, 0x51, 7, 0b10000000)
|
||||
|
||||
def _set_configs(self, configs: List[AmpConfig]) -> None:
|
||||
with SMBus(self.AMP_I2C_BUS) as bus:
|
||||
for config in configs:
|
||||
if self.debug:
|
||||
print(f"Setting \"{config.name}\" to {config.value}:")
|
||||
|
||||
old_value = bus.read_byte_data(self.AMP_ADDRESS, config.register, force=True)
|
||||
new_value = (old_value & (~config.mask)) | ((config.value << config.offset) & config.mask)
|
||||
bus.write_byte_data(self.AMP_ADDRESS, config.register, new_value, force=True)
|
||||
|
||||
if self.debug:
|
||||
print(f" Changed {hex(config.register)}: {hex(old_value)} -> {hex(new_value)}")
|
||||
|
||||
def set_configs(self, configs: List[AmpConfig]) -> bool:
|
||||
# retry in case panda is using the amp
|
||||
tries = 15
|
||||
for i in range(15):
|
||||
try:
|
||||
self._set_configs(configs)
|
||||
return True
|
||||
except OSError:
|
||||
print(f"Failed to set amp config, {tries - i - 1} retries left")
|
||||
time.sleep(0.02)
|
||||
return False
|
||||
|
||||
def set_global_shutdown(self, amp_disabled: bool) -> bool:
|
||||
return self.set_configs([self._get_shutdown_config(amp_disabled), ])
|
||||
|
||||
def initialize_configuration(self, model: str) -> bool:
|
||||
cfgs = [
|
||||
self._get_shutdown_config(True),
|
||||
*BASE_CONFIG,
|
||||
*CONFIGS[model],
|
||||
self._get_shutdown_config(False),
|
||||
]
|
||||
return self.set_configs(cfgs)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
with open("/sys/firmware/devicetree/base/model") as f:
|
||||
model = f.read().strip('\x00')
|
||||
model = model.split('comma ')[-1]
|
||||
|
||||
amp = Amplifier()
|
||||
amp.initialize_configuration(model)
|
||||
Executable
+208
@@ -0,0 +1,208 @@
|
||||
#!/usr/bin/env python3
|
||||
import io
|
||||
import lzma
|
||||
import os
|
||||
import struct
|
||||
import sys
|
||||
import time
|
||||
from abc import ABC, abstractmethod
|
||||
from collections import defaultdict, namedtuple
|
||||
from typing import Callable, Dict, List, Optional, Tuple
|
||||
|
||||
import requests
|
||||
from Crypto.Hash import SHA512
|
||||
|
||||
CA_FORMAT_INDEX = 0x96824d9c7b129ff9
|
||||
CA_FORMAT_TABLE = 0xe75b9e112f17417d
|
||||
CA_FORMAT_TABLE_TAIL_MARKER = 0xe75b9e112f17417
|
||||
FLAGS = 0xb000000000000000
|
||||
|
||||
CA_HEADER_LEN = 48
|
||||
CA_TABLE_HEADER_LEN = 16
|
||||
CA_TABLE_ENTRY_LEN = 40
|
||||
CA_TABLE_MIN_LEN = CA_TABLE_HEADER_LEN + CA_TABLE_ENTRY_LEN
|
||||
|
||||
CHUNK_DOWNLOAD_TIMEOUT = 60
|
||||
CHUNK_DOWNLOAD_RETRIES = 3
|
||||
|
||||
CAIBX_DOWNLOAD_TIMEOUT = 120
|
||||
|
||||
Chunk = namedtuple('Chunk', ['sha', 'offset', 'length'])
|
||||
ChunkDict = Dict[bytes, Chunk]
|
||||
|
||||
|
||||
class ChunkReader(ABC):
|
||||
@abstractmethod
|
||||
def read(self, chunk: Chunk) -> bytes:
|
||||
...
|
||||
|
||||
|
||||
class FileChunkReader(ChunkReader):
|
||||
"""Reads chunks from a local file"""
|
||||
def __init__(self, fn: str) -> None:
|
||||
super().__init__()
|
||||
self.f = open(fn, 'rb')
|
||||
|
||||
def __del__(self):
|
||||
self.f.close()
|
||||
|
||||
def read(self, chunk: Chunk) -> bytes:
|
||||
self.f.seek(chunk.offset)
|
||||
return self.f.read(chunk.length)
|
||||
|
||||
|
||||
class RemoteChunkReader(ChunkReader):
|
||||
"""Reads lzma compressed chunks from a remote store"""
|
||||
|
||||
def __init__(self, url: str) -> None:
|
||||
super().__init__()
|
||||
self.url = url
|
||||
self.session = requests.Session()
|
||||
|
||||
def read(self, chunk: Chunk) -> bytes:
|
||||
sha_hex = chunk.sha.hex()
|
||||
url = os.path.join(self.url, sha_hex[:4], sha_hex + ".cacnk")
|
||||
|
||||
if os.path.isfile(url):
|
||||
with open(url, 'rb') as f:
|
||||
contents = f.read()
|
||||
else:
|
||||
for i in range(CHUNK_DOWNLOAD_RETRIES):
|
||||
try:
|
||||
resp = self.session.get(url, timeout=CHUNK_DOWNLOAD_TIMEOUT)
|
||||
break
|
||||
except Exception:
|
||||
if i == CHUNK_DOWNLOAD_RETRIES - 1:
|
||||
raise
|
||||
time.sleep(CHUNK_DOWNLOAD_TIMEOUT)
|
||||
|
||||
resp.raise_for_status()
|
||||
contents = resp.content
|
||||
|
||||
decompressor = lzma.LZMADecompressor(format=lzma.FORMAT_AUTO)
|
||||
return decompressor.decompress(contents)
|
||||
|
||||
|
||||
def parse_caibx(caibx_path: str) -> List[Chunk]:
|
||||
"""Parses the chunks from a caibx file. Can handle both local and remote files.
|
||||
Returns a list of chunks with hash, offset and length"""
|
||||
caibx: io.BufferedIOBase
|
||||
if os.path.isfile(caibx_path):
|
||||
caibx = open(caibx_path, 'rb')
|
||||
else:
|
||||
resp = requests.get(caibx_path, timeout=CAIBX_DOWNLOAD_TIMEOUT)
|
||||
resp.raise_for_status()
|
||||
caibx = io.BytesIO(resp.content)
|
||||
|
||||
caibx.seek(0, os.SEEK_END)
|
||||
caibx_len = caibx.tell()
|
||||
caibx.seek(0, os.SEEK_SET)
|
||||
|
||||
# Parse header
|
||||
length, magic, flags, min_size, _, max_size = struct.unpack("<QQQQQQ", caibx.read(CA_HEADER_LEN))
|
||||
assert flags == flags
|
||||
assert length == CA_HEADER_LEN
|
||||
assert magic == CA_FORMAT_INDEX
|
||||
|
||||
# Parse table header
|
||||
length, magic = struct.unpack("<QQ", caibx.read(CA_TABLE_HEADER_LEN))
|
||||
assert magic == CA_FORMAT_TABLE
|
||||
|
||||
# Parse chunks
|
||||
num_chunks = (caibx_len - CA_HEADER_LEN - CA_TABLE_MIN_LEN) // CA_TABLE_ENTRY_LEN
|
||||
chunks = []
|
||||
|
||||
offset = 0
|
||||
for i in range(num_chunks):
|
||||
new_offset = struct.unpack("<Q", caibx.read(8))[0]
|
||||
|
||||
sha = caibx.read(32)
|
||||
length = new_offset - offset
|
||||
|
||||
assert length <= max_size
|
||||
|
||||
# Last chunk can be smaller
|
||||
if i < num_chunks - 1:
|
||||
assert length >= min_size
|
||||
|
||||
chunks.append(Chunk(sha, offset, length))
|
||||
offset = new_offset
|
||||
|
||||
caibx.close()
|
||||
return chunks
|
||||
|
||||
|
||||
def build_chunk_dict(chunks: List[Chunk]) -> ChunkDict:
|
||||
"""Turn a list of chunks into a dict for faster lookups based on hash.
|
||||
Keep first chunk since it's more likely to be already downloaded."""
|
||||
r = {}
|
||||
for c in chunks:
|
||||
if c.sha not in r:
|
||||
r[c.sha] = c
|
||||
return r
|
||||
|
||||
|
||||
def extract(target: List[Chunk],
|
||||
sources: List[Tuple[str, ChunkReader, ChunkDict]],
|
||||
out_path: str,
|
||||
progress: Optional[Callable[[int], None]] = None):
|
||||
stats: Dict[str, int] = defaultdict(int)
|
||||
|
||||
mode = 'rb+' if os.path.exists(out_path) else 'wb'
|
||||
with open(out_path, mode) as out:
|
||||
for cur_chunk in target:
|
||||
|
||||
# Find source for desired chunk
|
||||
for name, chunk_reader, store_chunks in sources:
|
||||
if cur_chunk.sha in store_chunks:
|
||||
bts = chunk_reader.read(store_chunks[cur_chunk.sha])
|
||||
|
||||
# Check length
|
||||
if len(bts) != cur_chunk.length:
|
||||
continue
|
||||
|
||||
# Check hash
|
||||
if SHA512.new(bts, truncate="256").digest() != cur_chunk.sha:
|
||||
continue
|
||||
|
||||
# Write to output
|
||||
out.seek(cur_chunk.offset)
|
||||
out.write(bts)
|
||||
|
||||
stats[name] += cur_chunk.length
|
||||
|
||||
if progress is not None:
|
||||
progress(sum(stats.values()))
|
||||
|
||||
break
|
||||
else:
|
||||
raise RuntimeError("Desired chunk not found in provided stores")
|
||||
|
||||
return stats
|
||||
|
||||
|
||||
def print_stats(stats: Dict[str, int]):
|
||||
total_bytes = sum(stats.values())
|
||||
print(f"Total size: {total_bytes / 1024 / 1024:.2f} MB")
|
||||
for name, total in stats.items():
|
||||
print(f" {name}: {total / 1024 / 1024:.2f} MB ({total / total_bytes * 100:.1f}%)")
|
||||
|
||||
|
||||
def extract_simple(caibx_path, out_path, store_path):
|
||||
# (name, callback, chunks)
|
||||
target = parse_caibx(caibx_path)
|
||||
sources = [
|
||||
# (store_path, RemoteChunkReader(store_path), build_chunk_dict(target)),
|
||||
(store_path, FileChunkReader(store_path), build_chunk_dict(target)),
|
||||
]
|
||||
|
||||
return extract(target, sources, out_path)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
caibx = sys.argv[1]
|
||||
out = sys.argv[2]
|
||||
store = sys.argv[3]
|
||||
|
||||
stats = extract_simple(caibx, out, store)
|
||||
print_stats(stats)
|
||||
@@ -0,0 +1,104 @@
|
||||
#pragma once
|
||||
|
||||
#include <cstdlib>
|
||||
#include <fstream>
|
||||
|
||||
#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 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 void set_volume(float volume) {
|
||||
volume = util::map_val(volume, 0.f, 1.f, MIN_VOLUME, MAX_VOLUME);
|
||||
|
||||
char volume_str[6];
|
||||
snprintf(volume_str, sizeof(volume_str), "%.3f", volume);
|
||||
std::system(("pactl set-sink-volume @DEFAULT_SINK@ " + std::string(volume_str)).c_str());
|
||||
}
|
||||
|
||||
|
||||
static std::map<std::string, std::string> get_init_logs() {
|
||||
std::map<std::string, std::string> ret = {
|
||||
{"/BUILD", util::read_file("/BUILD")},
|
||||
{"lsblk", util::check_output("lsblk -o NAME,SIZE,STATE,VENDOR,MODEL,REV,SERIAL")},
|
||||
};
|
||||
|
||||
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); };
|
||||
};
|
||||
@@ -0,0 +1,601 @@
|
||||
import json
|
||||
import math
|
||||
import os
|
||||
import subprocess
|
||||
import time
|
||||
from enum import IntEnum
|
||||
from functools import cached_property, lru_cache
|
||||
from pathlib import Path
|
||||
|
||||
from cereal import log
|
||||
from common.gpio import gpio_set, gpio_init, get_irqs_for_action
|
||||
from system.hardware.base import HardwareBase, ThermalConfig
|
||||
from system.hardware.tici import iwlist
|
||||
from system.hardware.tici.pins import GPIO
|
||||
from system.hardware.tici.amplifier import Amplifier
|
||||
|
||||
NM = 'org.freedesktop.NetworkManager'
|
||||
NM_CON_ACT = NM + '.Connection.Active'
|
||||
NM_DEV = NM + '.Device'
|
||||
NM_DEV_WL = NM + '.Device.Wireless'
|
||||
NM_DEV_STATS = NM + '.Device.Statistics'
|
||||
NM_AP = NM + '.AccessPoint'
|
||||
DBUS_PROPS = 'org.freedesktop.DBus.Properties'
|
||||
|
||||
MM = 'org.freedesktop.ModemManager1'
|
||||
MM_MODEM = MM + ".Modem"
|
||||
MM_MODEM_SIMPLE = MM + ".Modem.Simple"
|
||||
MM_SIM = MM + ".Sim"
|
||||
|
||||
class MM_MODEM_STATE(IntEnum):
|
||||
FAILED = -1
|
||||
UNKNOWN = 0
|
||||
INITIALIZING = 1
|
||||
LOCKED = 2
|
||||
DISABLED = 3
|
||||
DISABLING = 4
|
||||
ENABLING = 5
|
||||
ENABLED = 6
|
||||
SEARCHING = 7
|
||||
REGISTERED = 8
|
||||
DISCONNECTING = 9
|
||||
CONNECTING = 10
|
||||
CONNECTED = 11
|
||||
|
||||
class NMMetered(IntEnum):
|
||||
NM_METERED_UNKNOWN = 0
|
||||
NM_METERED_YES = 1
|
||||
NM_METERED_NO = 2
|
||||
NM_METERED_GUESS_YES = 3
|
||||
NM_METERED_GUESS_NO = 4
|
||||
|
||||
TIMEOUT = 0.1
|
||||
REFRESH_RATE_MS = 1000
|
||||
|
||||
NetworkType = log.DeviceState.NetworkType
|
||||
NetworkStrength = log.DeviceState.NetworkStrength
|
||||
|
||||
# https://developer.gnome.org/ModemManager/unstable/ModemManager-Flags-and-Enumerations.html#MMModemAccessTechnology
|
||||
MM_MODEM_ACCESS_TECHNOLOGY_UMTS = 1 << 5
|
||||
MM_MODEM_ACCESS_TECHNOLOGY_LTE = 1 << 14
|
||||
|
||||
|
||||
def sudo_write(val, path):
|
||||
try:
|
||||
with open(path, 'w') as f:
|
||||
f.write(str(val))
|
||||
except PermissionError:
|
||||
os.system(f"sudo chmod a+w {path}")
|
||||
try:
|
||||
with open(path, 'w') as f:
|
||||
f.write(str(val))
|
||||
except PermissionError:
|
||||
# fallback for debugfs files
|
||||
os.system(f"sudo su -c 'echo {val} > {path}'")
|
||||
|
||||
|
||||
def affine_irq(val, action):
|
||||
irqs = get_irqs_for_action(action)
|
||||
if len(irqs) == 0:
|
||||
print(f"No IRQs found for '{action}'")
|
||||
return
|
||||
|
||||
for i in irqs:
|
||||
sudo_write(str(val), f"/proc/irq/{i}/smp_affinity_list")
|
||||
|
||||
class Tici(HardwareBase):
|
||||
@cached_property
|
||||
def bus(self):
|
||||
import dbus # pylint: disable=import-error
|
||||
return dbus.SystemBus()
|
||||
|
||||
@cached_property
|
||||
def nm(self):
|
||||
return self.bus.get_object(NM, '/org/freedesktop/NetworkManager')
|
||||
|
||||
@cached_property
|
||||
def mm(self):
|
||||
return self.bus.get_object(MM, '/org/freedesktop/ModemManager1')
|
||||
|
||||
@cached_property
|
||||
def amplifier(self):
|
||||
return Amplifier()
|
||||
|
||||
def get_os_version(self):
|
||||
with open("/VERSION") as f:
|
||||
return f.read().strip()
|
||||
|
||||
@lru_cache
|
||||
def get_device_type(self):
|
||||
with open("/sys/firmware/devicetree/base/model") as f:
|
||||
model = f.read().strip('\x00')
|
||||
model = model.split('comma ')[-1]
|
||||
# TODO: remove this with AGNOS 7+
|
||||
if model.startswith('Qualcomm'):
|
||||
model = 'tici'
|
||||
return model
|
||||
|
||||
def get_sound_card_online(self):
|
||||
if os.path.isfile('/proc/asound/card0/state'):
|
||||
with open('/proc/asound/card0/state') as f:
|
||||
return f.read().strip() == 'ONLINE'
|
||||
return False
|
||||
|
||||
def reboot(self, reason=None):
|
||||
subprocess.check_output(["sudo", "reboot"])
|
||||
|
||||
def uninstall(self):
|
||||
Path("/data/__system_reset__").touch()
|
||||
os.sync()
|
||||
self.reboot()
|
||||
|
||||
def get_serial(self):
|
||||
return self.get_cmdline()['androidboot.serialno']
|
||||
|
||||
def get_network_type(self):
|
||||
try:
|
||||
primary_connection = self.nm.Get(NM, 'PrimaryConnection', dbus_interface=DBUS_PROPS, timeout=TIMEOUT)
|
||||
primary_connection = self.bus.get_object(NM, primary_connection)
|
||||
primary_type = primary_connection.Get(NM_CON_ACT, 'Type', dbus_interface=DBUS_PROPS, timeout=TIMEOUT)
|
||||
|
||||
if primary_type == '802-3-ethernet':
|
||||
return NetworkType.ethernet
|
||||
elif primary_type == '802-11-wireless':
|
||||
return NetworkType.wifi
|
||||
else:
|
||||
active_connections = self.nm.Get(NM, 'ActiveConnections', dbus_interface=DBUS_PROPS, timeout=TIMEOUT)
|
||||
for conn in active_connections:
|
||||
c = self.bus.get_object(NM, conn)
|
||||
tp = c.Get(NM_CON_ACT, 'Type', dbus_interface=DBUS_PROPS, timeout=TIMEOUT)
|
||||
if tp == 'gsm':
|
||||
modem = self.get_modem()
|
||||
access_t = modem.Get(MM_MODEM, 'AccessTechnologies', dbus_interface=DBUS_PROPS, timeout=TIMEOUT)
|
||||
if access_t >= MM_MODEM_ACCESS_TECHNOLOGY_LTE:
|
||||
return NetworkType.cell4G
|
||||
elif access_t >= MM_MODEM_ACCESS_TECHNOLOGY_UMTS:
|
||||
return NetworkType.cell3G
|
||||
else:
|
||||
return NetworkType.cell2G
|
||||
except Exception:
|
||||
pass
|
||||
|
||||
return NetworkType.none
|
||||
|
||||
def get_modem(self):
|
||||
objects = self.mm.GetManagedObjects(dbus_interface="org.freedesktop.DBus.ObjectManager", timeout=TIMEOUT)
|
||||
modem_path = list(objects.keys())[0]
|
||||
return self.bus.get_object(MM, modem_path)
|
||||
|
||||
def get_wlan(self):
|
||||
wlan_path = self.nm.GetDeviceByIpIface('wlan0', dbus_interface=NM, timeout=TIMEOUT)
|
||||
return self.bus.get_object(NM, wlan_path)
|
||||
|
||||
def get_wwan(self):
|
||||
wwan_path = self.nm.GetDeviceByIpIface('wwan0', dbus_interface=NM, timeout=TIMEOUT)
|
||||
return self.bus.get_object(NM, wwan_path)
|
||||
|
||||
def get_sim_info(self):
|
||||
modem = self.get_modem()
|
||||
sim_path = modem.Get(MM_MODEM, 'Sim', dbus_interface=DBUS_PROPS, timeout=TIMEOUT)
|
||||
|
||||
if sim_path == "/":
|
||||
return {
|
||||
'sim_id': '',
|
||||
'mcc_mnc': None,
|
||||
'network_type': ["Unknown"],
|
||||
'sim_state': ["ABSENT"],
|
||||
'data_connected': False
|
||||
}
|
||||
else:
|
||||
sim = self.bus.get_object(MM, sim_path)
|
||||
return {
|
||||
'sim_id': str(sim.Get(MM_SIM, 'SimIdentifier', dbus_interface=DBUS_PROPS, timeout=TIMEOUT)),
|
||||
'mcc_mnc': str(sim.Get(MM_SIM, 'OperatorIdentifier', dbus_interface=DBUS_PROPS, timeout=TIMEOUT)),
|
||||
'network_type': ["Unknown"],
|
||||
'sim_state': ["READY"],
|
||||
'data_connected': modem.Get(MM_MODEM, 'State', dbus_interface=DBUS_PROPS, timeout=TIMEOUT) == MM_MODEM_STATE.CONNECTED,
|
||||
}
|
||||
|
||||
def get_subscriber_info(self):
|
||||
return ""
|
||||
|
||||
def get_imei(self, slot):
|
||||
if slot != 0:
|
||||
return ""
|
||||
|
||||
return str(self.get_modem().Get(MM_MODEM, 'EquipmentIdentifier', dbus_interface=DBUS_PROPS, timeout=TIMEOUT))
|
||||
|
||||
def get_network_info(self):
|
||||
modem = self.get_modem()
|
||||
try:
|
||||
info = modem.Command("AT+QNWINFO", math.ceil(TIMEOUT), dbus_interface=MM_MODEM, timeout=TIMEOUT)
|
||||
extra = modem.Command('AT+QENG="servingcell"', math.ceil(TIMEOUT), dbus_interface=MM_MODEM, timeout=TIMEOUT)
|
||||
state = modem.Get(MM_MODEM, 'State', dbus_interface=DBUS_PROPS, timeout=TIMEOUT)
|
||||
except Exception:
|
||||
return None
|
||||
|
||||
if info and info.startswith('+QNWINFO: '):
|
||||
info = info.replace('+QNWINFO: ', '').replace('"', '').split(',')
|
||||
extra = "" if extra is None else extra.replace('+QENG: "servingcell",', '').replace('"', '')
|
||||
state = "" if state is None else MM_MODEM_STATE(state).name
|
||||
|
||||
if len(info) != 4:
|
||||
return None
|
||||
|
||||
technology, operator, band, channel = info
|
||||
|
||||
return({
|
||||
'technology': technology,
|
||||
'operator': operator,
|
||||
'band': band,
|
||||
'channel': int(channel),
|
||||
'extra': extra,
|
||||
'state': state,
|
||||
})
|
||||
else:
|
||||
return None
|
||||
|
||||
def parse_strength(self, percentage):
|
||||
if percentage < 25:
|
||||
return NetworkStrength.poor
|
||||
elif percentage < 50:
|
||||
return NetworkStrength.moderate
|
||||
elif percentage < 75:
|
||||
return NetworkStrength.good
|
||||
else:
|
||||
return NetworkStrength.great
|
||||
|
||||
def get_network_strength(self, network_type):
|
||||
network_strength = NetworkStrength.unknown
|
||||
|
||||
try:
|
||||
if network_type == NetworkType.none:
|
||||
pass
|
||||
elif network_type == NetworkType.wifi:
|
||||
wlan = self.get_wlan()
|
||||
active_ap_path = wlan.Get(NM_DEV_WL, 'ActiveAccessPoint', dbus_interface=DBUS_PROPS, timeout=TIMEOUT)
|
||||
if active_ap_path != "/":
|
||||
active_ap = self.bus.get_object(NM, active_ap_path)
|
||||
strength = int(active_ap.Get(NM_AP, 'Strength', dbus_interface=DBUS_PROPS, timeout=TIMEOUT))
|
||||
network_strength = self.parse_strength(strength)
|
||||
else: # Cellular
|
||||
modem = self.get_modem()
|
||||
strength = int(modem.Get(MM_MODEM, 'SignalQuality', dbus_interface=DBUS_PROPS, timeout=TIMEOUT)[0])
|
||||
network_strength = self.parse_strength(strength)
|
||||
except Exception:
|
||||
pass
|
||||
|
||||
return network_strength
|
||||
|
||||
def get_network_metered(self, network_type) -> bool:
|
||||
try:
|
||||
primary_connection = self.nm.Get(NM, 'PrimaryConnection', dbus_interface=DBUS_PROPS, timeout=TIMEOUT)
|
||||
primary_connection = self.bus.get_object(NM, primary_connection)
|
||||
primary_devices = primary_connection.Get(NM_CON_ACT, 'Devices', dbus_interface=DBUS_PROPS, timeout=TIMEOUT)
|
||||
|
||||
for dev in primary_devices:
|
||||
dev_obj = self.bus.get_object(NM, str(dev))
|
||||
metered_prop = dev_obj.Get(NM_DEV, 'Metered', dbus_interface=DBUS_PROPS, timeout=TIMEOUT)
|
||||
|
||||
if network_type == NetworkType.wifi:
|
||||
if metered_prop in [NMMetered.NM_METERED_YES, NMMetered.NM_METERED_GUESS_YES]:
|
||||
return True
|
||||
elif network_type in [NetworkType.cell2G, NetworkType.cell3G, NetworkType.cell4G, NetworkType.cell5G]:
|
||||
if metered_prop == NMMetered.NM_METERED_NO:
|
||||
return False
|
||||
except Exception:
|
||||
pass
|
||||
|
||||
return super().get_network_metered(network_type)
|
||||
|
||||
@staticmethod
|
||||
def set_bandwidth_limit(upload_speed_kbps: int, download_speed_kbps: int) -> None:
|
||||
upload_speed_kbps = int(upload_speed_kbps) # Ensure integer value
|
||||
download_speed_kbps = int(download_speed_kbps) # Ensure integer value
|
||||
|
||||
adapter = "wwan0"
|
||||
ifb = "ifb0"
|
||||
|
||||
sudo = ["sudo"]
|
||||
tc = sudo + ["tc"]
|
||||
|
||||
# check, cmd
|
||||
cleanup = [
|
||||
# Clean up old rules
|
||||
(False, tc + ["qdisc", "del", "dev", adapter, "root"]),
|
||||
(False, tc + ["qdisc", "del", "dev", ifb, "root"]),
|
||||
(False, tc + ["qdisc", "del", "dev", adapter, "ingress"]),
|
||||
(False, tc + ["qdisc", "del", "dev", ifb, "ingress"]),
|
||||
|
||||
# Bring ifb0 down
|
||||
(False, sudo + ["ip", "link", "set", "dev", ifb, "down"]),
|
||||
]
|
||||
|
||||
upload = [
|
||||
# Create root Hierarchy Token Bucket that sends all traffic to 1:20
|
||||
(True, tc + ["qdisc", "add", "dev", adapter, "root", "handle", "1:", "htb", "default", "20"]),
|
||||
|
||||
# Create class 1:20 with specified rate limit
|
||||
(True, tc + ["class", "add", "dev", adapter, "parent", "1:", "classid", "1:20", "htb", "rate", f"{upload_speed_kbps}kbit"]),
|
||||
|
||||
# Create universal 32 bit filter on adapter that sends all outbound ip traffic through the class
|
||||
(True, tc + ["filter", "add", "dev", adapter, "parent", "1:", "protocol", "ip", "prio", "10", "u32", "match", "ip", "dst", "0.0.0.0/0", "flowid", "1:20"]),
|
||||
]
|
||||
|
||||
download = [
|
||||
# Bring ifb0 up
|
||||
(True, sudo + ["ip", "link", "set", "dev", ifb, "up"]),
|
||||
|
||||
# Redirect ingress (incoming) to egress ifb0
|
||||
(True, tc + ["qdisc", "add", "dev", adapter, "handle", "ffff:", "ingress"]),
|
||||
(True, tc + ["filter", "add", "dev", adapter, "parent", "ffff:", "protocol", "ip", "u32", "match", "u32", "0", "0", "action", "mirred", "egress", "redirect", "dev", ifb]),
|
||||
|
||||
# Add class and rules for virtual interface
|
||||
(True, tc + ["qdisc", "add", "dev", ifb, "root", "handle", "2:", "htb"]),
|
||||
(True, tc + ["class", "add", "dev", ifb, "parent", "2:", "classid", "2:1", "htb", "rate", f"{download_speed_kbps}kbit"]),
|
||||
|
||||
# Add filter to rule for IP address
|
||||
(True, tc + ["filter", "add", "dev", ifb, "protocol", "ip", "parent", "2:", "prio", "1", "u32", "match", "ip", "src", "0.0.0.0/0", "flowid", "2:1"]),
|
||||
]
|
||||
|
||||
commands = cleanup
|
||||
if upload_speed_kbps != -1:
|
||||
commands += upload
|
||||
if download_speed_kbps != -1:
|
||||
commands += download
|
||||
|
||||
for check, cmd in commands:
|
||||
subprocess.run(cmd, check=check)
|
||||
|
||||
def get_modem_version(self):
|
||||
try:
|
||||
modem = self.get_modem()
|
||||
return modem.Get(MM_MODEM, 'Revision', dbus_interface=DBUS_PROPS, timeout=TIMEOUT)
|
||||
except Exception:
|
||||
return None
|
||||
|
||||
def get_modem_nv(self):
|
||||
timeout = 0.2 # Default timeout is too short
|
||||
files = (
|
||||
'/nv/item_files/modem/mmode/ue_usage_setting',
|
||||
'/nv/item_files/ims/IMS_enable',
|
||||
'/nv/item_files/modem/mmode/sms_only',
|
||||
)
|
||||
try:
|
||||
modem = self.get_modem()
|
||||
return { fn: str(modem.Command(f'AT+QNVFR="{fn}"', math.ceil(timeout), dbus_interface=MM_MODEM, timeout=timeout)) for fn in files}
|
||||
except Exception:
|
||||
return None
|
||||
|
||||
def get_modem_temperatures(self):
|
||||
timeout = 0.2 # Default timeout is too short
|
||||
try:
|
||||
modem = self.get_modem()
|
||||
temps = modem.Command("AT+QTEMP", math.ceil(timeout), dbus_interface=MM_MODEM, timeout=timeout)
|
||||
return list(map(int, temps.split(' ')[1].split(',')))
|
||||
except Exception:
|
||||
return []
|
||||
|
||||
def get_nvme_temperatures(self):
|
||||
ret = []
|
||||
try:
|
||||
out = subprocess.check_output("sudo smartctl -aj /dev/nvme0", shell=True)
|
||||
dat = json.loads(out)
|
||||
ret = list(map(int, dat["nvme_smart_health_information_log"]["temperature_sensors"]))
|
||||
except Exception:
|
||||
pass
|
||||
return ret
|
||||
|
||||
def get_usb_present(self):
|
||||
# Not sure if relevant on tici, but the file exists
|
||||
return self.read_param_file("/sys/class/power_supply/usb/present", lambda x: bool(int(x)), False)
|
||||
|
||||
def get_current_power_draw(self):
|
||||
return (self.read_param_file("/sys/class/hwmon/hwmon1/power1_input", int) / 1e6)
|
||||
|
||||
def get_som_power_draw(self):
|
||||
return (self.read_param_file("/sys/class/power_supply/bms/voltage_now", int) * self.read_param_file("/sys/class/power_supply/bms/current_now", int) / 1e12)
|
||||
|
||||
def shutdown(self):
|
||||
os.system("sudo poweroff")
|
||||
|
||||
def get_thermal_config(self):
|
||||
return ThermalConfig(cpu=(["cpu%d-silver-usr" % i for i in range(4)] +
|
||||
["cpu%d-gold-usr" % i for i in range(4)], 1000),
|
||||
gpu=(("gpu0-usr", "gpu1-usr"), 1000),
|
||||
mem=("ddr-usr", 1000),
|
||||
bat=(None, 1),
|
||||
ambient=("xo-therm-adc", 1000),
|
||||
pmic=(("pm8998_tz", "pm8005_tz"), 1000))
|
||||
|
||||
def set_screen_brightness(self, percentage):
|
||||
try:
|
||||
with open("/sys/class/backlight/panel0-backlight/max_brightness") as f:
|
||||
max_brightness = float(f.read().strip())
|
||||
|
||||
val = int(percentage * (max_brightness / 100.))
|
||||
with open("/sys/class/backlight/panel0-backlight/brightness", "w") as f:
|
||||
f.write(str(val))
|
||||
except Exception:
|
||||
pass
|
||||
|
||||
def get_screen_brightness(self):
|
||||
try:
|
||||
with open("/sys/class/backlight/panel0-backlight/max_brightness") as f:
|
||||
max_brightness = float(f.read().strip())
|
||||
|
||||
with open("/sys/class/backlight/panel0-backlight/brightness") as f:
|
||||
return int(float(f.read()) / (max_brightness / 100.))
|
||||
except Exception:
|
||||
return 0
|
||||
|
||||
def set_power_save(self, powersave_enabled):
|
||||
# amplifier, 100mW at idle
|
||||
self.amplifier.set_global_shutdown(amp_disabled=powersave_enabled)
|
||||
if not powersave_enabled:
|
||||
self.amplifier.initialize_configuration(self.get_device_type())
|
||||
|
||||
# *** CPU config ***
|
||||
|
||||
# offline big cluster, leave core 4 online for boardd
|
||||
for i in range(5, 8):
|
||||
val = '0' if powersave_enabled else '1'
|
||||
sudo_write(val, f'/sys/devices/system/cpu/cpu{i}/online')
|
||||
|
||||
for n in ('0', '4'):
|
||||
gov = 'ondemand' if powersave_enabled else 'performance'
|
||||
sudo_write(gov, f'/sys/devices/system/cpu/cpufreq/policy{n}/scaling_governor')
|
||||
|
||||
# *** IRQ config ***
|
||||
|
||||
# boardd core
|
||||
affine_irq(4, "spi_geni") # SPI
|
||||
affine_irq(4, "xhci-hcd:usb3") # aux panda USB (or potentially anything else on USB)
|
||||
if "tici" in self.get_device_type():
|
||||
affine_irq(4, "xhci-hcd:usb1") # internal panda USB (also modem)
|
||||
|
||||
# GPU
|
||||
affine_irq(5, "kgsl-3d0")
|
||||
|
||||
# camerad core
|
||||
camera_irqs = ("cci", "cpas_camnoc", "cpas-cdm", "csid", "ife", "csid-lite", "ife-lite")
|
||||
for n in camera_irqs:
|
||||
affine_irq(5, n)
|
||||
|
||||
def get_gpu_usage_percent(self):
|
||||
try:
|
||||
with open('/sys/class/kgsl/kgsl-3d0/gpubusy') as f:
|
||||
used, total = f.read().strip().split()
|
||||
return 100.0 * int(used) / int(total)
|
||||
except Exception:
|
||||
return 0
|
||||
|
||||
def initialize_hardware(self):
|
||||
self.amplifier.initialize_configuration(self.get_device_type())
|
||||
|
||||
# Allow thermald to write engagement status to kmsg
|
||||
os.system("sudo chmod a+w /dev/kmsg")
|
||||
|
||||
# Ensure fan gpio is enabled so fan runs until shutdown, also turned on at boot by the ABL
|
||||
gpio_init(GPIO.SOM_ST_IO, True)
|
||||
gpio_set(GPIO.SOM_ST_IO, 1)
|
||||
|
||||
# *** IRQ config ***
|
||||
|
||||
# mask off big cluster from default affinity
|
||||
sudo_write("f", "/proc/irq/default_smp_affinity")
|
||||
|
||||
# move these off the default core
|
||||
affine_irq(1, "msm_drm") # display
|
||||
affine_irq(1, "msm_vidc") # encoders
|
||||
affine_irq(1, "i2c_geni") # sensors
|
||||
|
||||
# *** GPU config ***
|
||||
# https://github.com/commaai/agnos-kernel-sdm845/blob/master/arch/arm64/boot/dts/qcom/sdm845-gpu.dtsi#L216
|
||||
sudo_write("1", "/sys/class/kgsl/kgsl-3d0/min_pwrlevel")
|
||||
sudo_write("1", "/sys/class/kgsl/kgsl-3d0/max_pwrlevel")
|
||||
sudo_write("1", "/sys/class/kgsl/kgsl-3d0/force_bus_on")
|
||||
sudo_write("1", "/sys/class/kgsl/kgsl-3d0/force_clk_on")
|
||||
sudo_write("1", "/sys/class/kgsl/kgsl-3d0/force_rail_on")
|
||||
sudo_write("1000", "/sys/class/kgsl/kgsl-3d0/idle_timer")
|
||||
sudo_write("performance", "/sys/class/kgsl/kgsl-3d0/devfreq/governor")
|
||||
sudo_write("596", "/sys/class/kgsl/kgsl-3d0/max_clock_mhz")
|
||||
|
||||
# setup governors
|
||||
sudo_write("performance", "/sys/class/devfreq/soc:qcom,cpubw/governor")
|
||||
sudo_write("performance", "/sys/class/devfreq/soc:qcom,memlat-cpu0/governor")
|
||||
sudo_write("performance", "/sys/class/devfreq/soc:qcom,memlat-cpu4/governor")
|
||||
|
||||
# *** VIDC (encoder) config ***
|
||||
sudo_write("N", "/sys/kernel/debug/msm_vidc/clock_scaling")
|
||||
sudo_write("Y", "/sys/kernel/debug/msm_vidc/disable_thermal_mitigation")
|
||||
|
||||
def configure_modem(self):
|
||||
sim_id = self.get_sim_info().get('sim_id', '')
|
||||
|
||||
# configure modem as data-centric
|
||||
cmds = [
|
||||
'AT+QNVW=5280,0,"0102000000000000"',
|
||||
'AT+QNVFW="/nv/item_files/ims/IMS_enable",00',
|
||||
'AT+QNVFW="/nv/item_files/modem/mmode/ue_usage_setting",01',
|
||||
]
|
||||
modem = self.get_modem()
|
||||
for cmd in cmds:
|
||||
try:
|
||||
modem.Command(cmd, math.ceil(TIMEOUT), dbus_interface=MM_MODEM, timeout=TIMEOUT)
|
||||
except Exception:
|
||||
pass
|
||||
|
||||
# blue prime config
|
||||
if sim_id.startswith('8901410'):
|
||||
os.system('mmcli -m any --3gpp-set-initial-eps-bearer-settings="apn=Broadband"')
|
||||
|
||||
def get_networks(self):
|
||||
r = {}
|
||||
|
||||
wlan = iwlist.scan()
|
||||
if wlan is not None:
|
||||
r['wlan'] = wlan
|
||||
|
||||
lte_info = self.get_network_info()
|
||||
if lte_info is not None:
|
||||
extra = lte_info['extra']
|
||||
|
||||
# <state>,"LTE",<is_tdd>,<mcc>,<mnc>,<cellid>,<pcid>,<earfcn>,<freq_band_ind>,
|
||||
# <ul_bandwidth>,<dl_bandwidth>,<tac>,<rsrp>,<rsrq>,<rssi>,<sinr>,<srxlev>
|
||||
if 'LTE' in extra:
|
||||
extra = extra.split(',')
|
||||
try:
|
||||
r['lte'] = [{
|
||||
"mcc": int(extra[3]),
|
||||
"mnc": int(extra[4]),
|
||||
"cid": int(extra[5], 16),
|
||||
"nmr": [{"pci": int(extra[6]), "earfcn": int(extra[7])}],
|
||||
}]
|
||||
except (ValueError, IndexError):
|
||||
pass
|
||||
|
||||
return r
|
||||
|
||||
def get_modem_data_usage(self):
|
||||
try:
|
||||
wwan = self.get_wwan()
|
||||
|
||||
# Ensure refresh rate is set so values don't go stale
|
||||
refresh_rate = wwan.Get(NM_DEV_STATS, 'RefreshRateMs', dbus_interface=DBUS_PROPS, timeout=TIMEOUT)
|
||||
if refresh_rate != REFRESH_RATE_MS:
|
||||
u = type(refresh_rate)
|
||||
wwan.Set(NM_DEV_STATS, 'RefreshRateMs', u(REFRESH_RATE_MS), dbus_interface=DBUS_PROPS, timeout=TIMEOUT)
|
||||
|
||||
tx = wwan.Get(NM_DEV_STATS, 'TxBytes', dbus_interface=DBUS_PROPS, timeout=TIMEOUT)
|
||||
rx = wwan.Get(NM_DEV_STATS, 'RxBytes', dbus_interface=DBUS_PROPS, timeout=TIMEOUT)
|
||||
return int(tx), int(rx)
|
||||
except Exception:
|
||||
return -1, -1
|
||||
|
||||
def has_internal_panda(self):
|
||||
return True
|
||||
|
||||
def reset_internal_panda(self):
|
||||
gpio_init(GPIO.STM_RST_N, True)
|
||||
|
||||
gpio_set(GPIO.STM_RST_N, 1)
|
||||
time.sleep(2)
|
||||
gpio_set(GPIO.STM_RST_N, 0)
|
||||
|
||||
def recover_internal_panda(self):
|
||||
gpio_init(GPIO.STM_RST_N, True)
|
||||
gpio_init(GPIO.STM_BOOT0, True)
|
||||
|
||||
gpio_set(GPIO.STM_RST_N, 1)
|
||||
gpio_set(GPIO.STM_BOOT0, 1)
|
||||
time.sleep(1)
|
||||
gpio_set(GPIO.STM_RST_N, 0)
|
||||
time.sleep(1)
|
||||
gpio_set(GPIO.STM_BOOT0, 0)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
t = Tici()
|
||||
t.initialize_hardware()
|
||||
t.set_power_save(False)
|
||||
@@ -0,0 +1,35 @@
|
||||
import subprocess
|
||||
|
||||
|
||||
def scan(interface="wlan0"):
|
||||
result = []
|
||||
try:
|
||||
r = subprocess.check_output(["iwlist", interface, "scan"], encoding='utf8')
|
||||
|
||||
mac = None
|
||||
for line in r.split('\n'):
|
||||
if "Address" in line:
|
||||
# Based on the adapter eithere a percentage or dBm is returned
|
||||
# Add previous network in case no dBm signal level was seen
|
||||
if mac is not None:
|
||||
result.append({"mac": mac})
|
||||
mac = None
|
||||
|
||||
mac = line.split(' ')[-1]
|
||||
elif "dBm" in line:
|
||||
try:
|
||||
level = line.split('Signal level=')[1]
|
||||
rss = int(level.split(' ')[0])
|
||||
result.append({"mac": mac, "rss": rss})
|
||||
mac = None
|
||||
except ValueError:
|
||||
continue
|
||||
|
||||
# Add last network if no dBm was found
|
||||
if mac is not None:
|
||||
result.append({"mac": mac})
|
||||
|
||||
return result
|
||||
|
||||
except Exception:
|
||||
return None
|
||||
@@ -0,0 +1,30 @@
|
||||
# TODO: these are also defined in a header
|
||||
|
||||
# GPIO pin definitions
|
||||
class GPIO:
|
||||
# both GPIO_STM_RST_N and GPIO_LTE_RST_N are misnamed, they are high to reset
|
||||
HUB_RST_N = 30
|
||||
UBLOX_RST_N = 32
|
||||
UBLOX_SAFEBOOT_N = 33
|
||||
UBLOX_PWR_EN = 34
|
||||
STM_RST_N = 124
|
||||
STM_BOOT0 = 134
|
||||
|
||||
SIREN = 42
|
||||
SOM_ST_IO = 49
|
||||
|
||||
LTE_RST_N = 50
|
||||
LTE_PWRKEY = 116
|
||||
LTE_BOOT = 52
|
||||
|
||||
# GPIO_CAM0_DVDD_EN = /sys/kernel/debug/regulator/camera_rear_ldo
|
||||
CAM0_AVDD_EN = 8
|
||||
CAM0_RSTN = 9
|
||||
CAM1_RSTN = 7
|
||||
CAM2_RSTN = 12
|
||||
|
||||
# Sensor interrupts
|
||||
BMX055_ACCEL_INT = 21
|
||||
BMX055_GYRO_INT = 23
|
||||
BMX055_MAGN_INT = 87
|
||||
LSM_INT = 84
|
||||
Executable
BIN
Binary file not shown.
@@ -0,0 +1 @@
|
||||
logcatd
|
||||
@@ -0,0 +1,3 @@
|
||||
Import('env', 'cereal', 'messaging', 'common')
|
||||
|
||||
env.Program('logcatd', 'logcatd_systemd.cc', LIBS=[cereal, messaging, common, 'zmq', 'capnp', 'kj', 'systemd', 'json11'])
|
||||
@@ -0,0 +1,75 @@
|
||||
#include <systemd/sd-journal.h>
|
||||
|
||||
#include <cassert>
|
||||
#include <csignal>
|
||||
#include <map>
|
||||
#include <string>
|
||||
|
||||
#include "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, ×tamp);
|
||||
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;
|
||||
}
|
||||
@@ -0,0 +1,4 @@
|
||||
loggerd
|
||||
encoderd
|
||||
bootlog
|
||||
tests/test_logger
|
||||
@@ -0,0 +1,27 @@
|
||||
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('test'):
|
||||
env.Program('tests/test_logger', ['tests/test_runner.cc', 'tests/test_logger.cc'], LIBS=libs + ['curl', 'crypto'])
|
||||
@@ -0,0 +1,70 @@
|
||||
#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 timestr = logger_get_route_name();
|
||||
const std::string path = LOG_ROOT + "/boot/" + timestr;
|
||||
LOGW("bootlog to %s", path.c_str());
|
||||
|
||||
// Open bootlog
|
||||
bool r = util::create_directories(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", timestr.c_str());
|
||||
|
||||
return 0;
|
||||
}
|
||||
@@ -0,0 +1,41 @@
|
||||
import os
|
||||
from pathlib import Path
|
||||
from system.hardware import PC
|
||||
|
||||
if os.environ.get('LOG_ROOT', False):
|
||||
ROOT = os.environ['LOG_ROOT']
|
||||
elif PC:
|
||||
ROOT = str(Path.home() / ".comma" / "media" / "0" / "realdata")
|
||||
else:
|
||||
ROOT = '/data/media/0/realdata/'
|
||||
|
||||
|
||||
CAMERA_FPS = 20
|
||||
SEGMENT_LENGTH = 60
|
||||
|
||||
STATS_DIR_FILE_LIMIT = 10000
|
||||
STATS_SOCKET = "ipc:///tmp/stats"
|
||||
if PC:
|
||||
STATS_DIR = str(Path.home() / ".comma" / "stats")
|
||||
else:
|
||||
STATS_DIR = "/data/stats/"
|
||||
STATS_FLUSH_TIME_S = 60
|
||||
|
||||
def get_available_percent(default=None):
|
||||
try:
|
||||
statvfs = os.statvfs(ROOT)
|
||||
available_percent = 100.0 * statvfs.f_bavail / statvfs.f_blocks
|
||||
except OSError:
|
||||
available_percent = default
|
||||
|
||||
return available_percent
|
||||
|
||||
|
||||
def get_available_bytes(default=None):
|
||||
try:
|
||||
statvfs = os.statvfs(ROOT)
|
||||
available_bytes = statvfs.f_bavail * statvfs.f_frsize
|
||||
except OSError:
|
||||
available_bytes = default
|
||||
|
||||
return available_bytes
|
||||
@@ -0,0 +1,85 @@
|
||||
#!/usr/bin/env python3
|
||||
import os
|
||||
import shutil
|
||||
import threading
|
||||
from typing import List
|
||||
|
||||
from system.swaglog import cloudlog
|
||||
from system.loggerd.config import ROOT, get_available_bytes, get_available_percent
|
||||
from system.loggerd.uploader import listdir_by_creation
|
||||
from system.loggerd.xattr_cache import getxattr
|
||||
|
||||
MIN_BYTES = 5 * 1024 * 1024 * 1024
|
||||
MIN_PERCENT = 10
|
||||
|
||||
DELETE_LAST = ['boot', 'crash']
|
||||
|
||||
PRESERVE_ATTR_NAME = 'user.preserve'
|
||||
PRESERVE_ATTR_VALUE = b'1'
|
||||
PRESERVE_COUNT = 5
|
||||
|
||||
|
||||
def has_preserve_xattr(d: str) -> bool:
|
||||
return getxattr(os.path.join(ROOT, d), PRESERVE_ATTR_NAME) == PRESERVE_ATTR_VALUE
|
||||
|
||||
|
||||
def get_preserved_segments(dirs_by_creation: List[str]) -> List[str]:
|
||||
preserved = []
|
||||
for n, d in enumerate(filter(has_preserve_xattr, reversed(dirs_by_creation))):
|
||||
if n == PRESERVE_COUNT:
|
||||
break
|
||||
date_str, _, seg_str = d.rpartition("--")
|
||||
|
||||
# ignore non-segment directories
|
||||
if not date_str:
|
||||
continue
|
||||
try:
|
||||
seg_num = int(seg_str)
|
||||
except ValueError:
|
||||
continue
|
||||
|
||||
# preserve segment and its prior
|
||||
preserved.append(d)
|
||||
preserved.append(f"{date_str}--{seg_num - 1}")
|
||||
|
||||
return preserved
|
||||
|
||||
|
||||
def deleter_thread(exit_event):
|
||||
while not exit_event.is_set():
|
||||
out_of_bytes = get_available_bytes(default=MIN_BYTES + 1) < MIN_BYTES
|
||||
out_of_percent = get_available_percent(default=MIN_PERCENT + 1) < MIN_PERCENT
|
||||
|
||||
if out_of_percent or out_of_bytes:
|
||||
dirs = listdir_by_creation(ROOT)
|
||||
|
||||
# skip deleting most recent N preserved segments (and their prior segment)
|
||||
preserved_dirs = get_preserved_segments(dirs)
|
||||
|
||||
# remove the earliest directory we can
|
||||
for delete_dir in sorted(dirs, key=lambda d: (d in DELETE_LAST, d in preserved_dirs)):
|
||||
delete_path = os.path.join(ROOT, delete_dir)
|
||||
|
||||
if any(name.endswith(".lock") for name in os.listdir(delete_path)):
|
||||
continue
|
||||
|
||||
try:
|
||||
cloudlog.info(f"deleting {delete_path}")
|
||||
if os.path.isfile(delete_path):
|
||||
os.remove(delete_path)
|
||||
else:
|
||||
shutil.rmtree(delete_path)
|
||||
break
|
||||
except OSError:
|
||||
cloudlog.exception(f"issue deleting {delete_path}")
|
||||
exit_event.wait(.1)
|
||||
else:
|
||||
exit_event.wait(30)
|
||||
|
||||
|
||||
def main():
|
||||
deleter_thread(threading.Event())
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@@ -0,0 +1,34 @@
|
||||
#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) {
|
||||
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);
|
||||
if (flags & V4L2_BUF_FLAG_KEYFRAME) edat.setHeader(header);
|
||||
|
||||
auto words = new kj::Array<capnp::word>(capnp::messageToFlatArray(msg));
|
||||
auto bytes = words->asBytes();
|
||||
e->pm->send(e->encoder_info.publish_name, bytes.begin(), bytes.size());
|
||||
delete words;
|
||||
}
|
||||
@@ -0,0 +1,34 @@
|
||||
#pragma once
|
||||
|
||||
#include <cassert>
|
||||
#include <cstdint>
|
||||
#include <thread>
|
||||
|
||||
#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;
|
||||
|
||||
static void publisher_publish(VideoEncoder *e, int segment_num, uint32_t idx, VisionIpcBufExtra &extra, unsigned int flags, kj::ArrayPtr<capnp::byte> header, kj::ArrayPtr<capnp::byte> dat);
|
||||
|
||||
|
||||
protected:
|
||||
int in_width, in_height;
|
||||
const EncoderInfo encoder_info;
|
||||
|
||||
private:
|
||||
// total frames encoded
|
||||
int cnt = 0;
|
||||
std::unique_ptr<PubMaster> pm;
|
||||
};
|
||||
@@ -0,0 +1,150 @@
|
||||
#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 "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 = encoder_info.frame_width;
|
||||
frame->height = encoder_info.frame_height;
|
||||
frame->linesize[0] = encoder_info.frame_width;
|
||||
frame->linesize[1] = encoder_info.frame_width/2;
|
||||
frame->linesize[2] = encoder_info.frame_width/2;
|
||||
|
||||
convert_buf.resize(in_width * in_height * 3 / 2);
|
||||
|
||||
if (in_width != encoder_info.frame_width || in_height != encoder_info.frame_height) {
|
||||
downscale_buf.resize(encoder_info.frame_width * encoder_info.frame_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.filename, 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;
|
||||
}
|
||||
@@ -0,0 +1,34 @@
|
||||
#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;
|
||||
};
|
||||
@@ -0,0 +1,309 @@
|
||||
#include <cassert>
|
||||
#include <sys/ioctl.h>
|
||||
#include <poll.h>
|
||||
|
||||
#include "system/loggerd/encoder/v4l_encoder.h"
|
||||
#include "common/util.h"
|
||||
#include "common/timing.h"
|
||||
|
||||
#include "libyuv.h"
|
||||
#include "msm_media_info.h"
|
||||
|
||||
// has to be in this order
|
||||
#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;
|
||||
|
||||
#define checked_ioctl(x,y,z) { int _ret = HANDLE_EINTR(ioctl(x,y,z)); if (_ret!=0) { LOGE("checked_ioctl failed %d %lx %p", x, y, z); } assert(_ret==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.filename);
|
||||
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) { LOGE("encoder dequeue poll timeout"); continue; }
|
||||
|
||||
if (env_debug_encoder >= 2) {
|
||||
printf("%20s poll %x at %.2f ms\n", e->encoder_info.filename, 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, ×tamp);
|
||||
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.filename, 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)encoder_info.frame_width,
|
||||
.height = (unsigned int)encoder_info.frame_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");
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,30 @@
|
||||
#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;
|
||||
};
|
||||
@@ -0,0 +1,160 @@
|
||||
#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<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 %dx%d", 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) {
|
||||
encoders.push_back(new Encoder(encoder_info, buf_info.width, buf_info.height));
|
||||
}
|
||||
}
|
||||
|
||||
for (int i = 0; i < encoders.size(); ++i) {
|
||||
encoders[i]->encoder_open(NULL);
|
||||
}
|
||||
|
||||
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: %d 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);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
LOG("encoder destroy");
|
||||
for(auto &e : encoders) {
|
||||
e->encoder_close();
|
||||
delete e;
|
||||
}
|
||||
}
|
||||
|
||||
void encoderd_thread() {
|
||||
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_logged), std::end(cameras_logged),
|
||||
[stream](auto &cam) { return cam.stream_type == stream; });
|
||||
assert(it != std::end(cameras_logged));
|
||||
++s.max_waiting;
|
||||
encoder_threads.push_back(std::thread(encoder_thread, &s, *it));
|
||||
}
|
||||
|
||||
for (auto &t : encoder_threads) t.join();
|
||||
}
|
||||
}
|
||||
|
||||
int main() {
|
||||
if (!Hardware::PC()) {
|
||||
int ret;
|
||||
ret = util::set_realtime_priority(52);
|
||||
assert(ret == 0);
|
||||
ret = util::set_core_affinity({3});
|
||||
assert(ret == 0);
|
||||
}
|
||||
encoderd_thread();
|
||||
return 0;
|
||||
}
|
||||
@@ -0,0 +1,260 @@
|
||||
#include "system/loggerd/logger.h"
|
||||
|
||||
#include <sys/stat.h>
|
||||
#include <unistd.h>
|
||||
#include <ftw.h>
|
||||
|
||||
#include <cassert>
|
||||
#include <cerrno>
|
||||
#include <cstdint>
|
||||
#include <cstdio>
|
||||
#include <cstdlib>
|
||||
#include <cstring>
|
||||
#include <ctime>
|
||||
#include <fstream>
|
||||
#include <iostream>
|
||||
#include <streambuf>
|
||||
|
||||
#include "common/params.h"
|
||||
#include "common/swaglog.h"
|
||||
#include "common/version.h"
|
||||
|
||||
// ***** log metadata *****
|
||||
kj::Array<capnp::word> logger_build_init_data() {
|
||||
MessageBuilder msg;
|
||||
auto init = msg.initEvent().initInitData();
|
||||
|
||||
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();
|
||||
std::map<std::string, std::string> params_map = params.readAll();
|
||||
|
||||
init.setGitCommit(params_map["GitCommit"]);
|
||||
init.setGitBranch(params_map["GitBranch"]);
|
||||
init.setGitRemote(params_map["GitRemote"]);
|
||||
init.setPassive(params.getBool("Passive"));
|
||||
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;
|
||||
}
|
||||
|
||||
void log_init_data(LoggerState *s) {
|
||||
auto bytes = s->init_data.asBytes();
|
||||
logger_log(s, bytes.begin(), bytes.size(), s->has_qlog);
|
||||
}
|
||||
|
||||
|
||||
static void lh_log_sentinel(LoggerHandle *h, SentinelType type) {
|
||||
MessageBuilder msg;
|
||||
auto sen = msg.initEvent().initSentinel();
|
||||
sen.setType(type);
|
||||
sen.setSignal(h->exit_signal);
|
||||
auto bytes = msg.toBytes();
|
||||
|
||||
lh_log(h, bytes.begin(), bytes.size(), true);
|
||||
}
|
||||
|
||||
// ***** logging functions *****
|
||||
|
||||
void logger_init(LoggerState *s, bool has_qlog) {
|
||||
pthread_mutex_init(&s->lock, NULL);
|
||||
|
||||
s->part = -1;
|
||||
s->has_qlog = has_qlog;
|
||||
s->route_name = logger_get_route_name();
|
||||
s->init_data = logger_build_init_data();
|
||||
}
|
||||
|
||||
static LoggerHandle* logger_open(LoggerState *s, const char* root_path) {
|
||||
LoggerHandle *h = NULL;
|
||||
for (int i=0; i<LOGGER_MAX_HANDLES; i++) {
|
||||
if (s->handles[i].refcnt == 0) {
|
||||
h = &s->handles[i];
|
||||
break;
|
||||
}
|
||||
}
|
||||
assert(h);
|
||||
|
||||
snprintf(h->segment_path, sizeof(h->segment_path),
|
||||
"%s/%s--%d", root_path, s->route_name.c_str(), s->part);
|
||||
|
||||
snprintf(h->log_path, sizeof(h->log_path), "%s/rlog", h->segment_path);
|
||||
snprintf(h->qlog_path, sizeof(h->qlog_path), "%s/qlog", h->segment_path);
|
||||
snprintf(h->lock_path, sizeof(h->lock_path), "%s.lock", h->log_path);
|
||||
h->end_sentinel_type = SentinelType::END_OF_SEGMENT;
|
||||
h->exit_signal = 0;
|
||||
|
||||
if (!util::create_directories(h->segment_path, 0775)) return nullptr;
|
||||
|
||||
FILE* lock_file = fopen(h->lock_path, "wb");
|
||||
if (lock_file == NULL) return NULL;
|
||||
fclose(lock_file);
|
||||
|
||||
h->log = std::make_unique<RawFile>(h->log_path);
|
||||
if (s->has_qlog) {
|
||||
h->q_log = std::make_unique<RawFile>(h->qlog_path);
|
||||
}
|
||||
|
||||
pthread_mutex_init(&h->lock, NULL);
|
||||
h->refcnt++;
|
||||
return h;
|
||||
}
|
||||
|
||||
int logger_next(LoggerState *s, const char* root_path,
|
||||
char* out_segment_path, size_t out_segment_path_len,
|
||||
int* out_part) {
|
||||
bool is_start_of_route = !s->cur_handle;
|
||||
|
||||
pthread_mutex_lock(&s->lock);
|
||||
s->part++;
|
||||
|
||||
LoggerHandle* next_h = logger_open(s, root_path);
|
||||
if (!next_h) {
|
||||
pthread_mutex_unlock(&s->lock);
|
||||
return -1;
|
||||
}
|
||||
|
||||
if (s->cur_handle) {
|
||||
lh_close(s->cur_handle);
|
||||
}
|
||||
s->cur_handle = next_h;
|
||||
|
||||
if (out_segment_path) {
|
||||
snprintf(out_segment_path, out_segment_path_len, "%s", next_h->segment_path);
|
||||
}
|
||||
if (out_part) {
|
||||
*out_part = s->part;
|
||||
}
|
||||
|
||||
pthread_mutex_unlock(&s->lock);
|
||||
|
||||
// write beginning of log metadata
|
||||
log_init_data(s);
|
||||
lh_log_sentinel(s->cur_handle, is_start_of_route ? SentinelType::START_OF_ROUTE : SentinelType::START_OF_SEGMENT);
|
||||
return 0;
|
||||
}
|
||||
|
||||
LoggerHandle* logger_get_handle(LoggerState *s) {
|
||||
pthread_mutex_lock(&s->lock);
|
||||
LoggerHandle* h = s->cur_handle;
|
||||
if (h) {
|
||||
pthread_mutex_lock(&h->lock);
|
||||
h->refcnt++;
|
||||
pthread_mutex_unlock(&h->lock);
|
||||
}
|
||||
pthread_mutex_unlock(&s->lock);
|
||||
return h;
|
||||
}
|
||||
|
||||
void logger_log(LoggerState *s, uint8_t* data, size_t data_size, bool in_qlog) {
|
||||
pthread_mutex_lock(&s->lock);
|
||||
if (s->cur_handle) {
|
||||
lh_log(s->cur_handle, data, data_size, in_qlog);
|
||||
}
|
||||
pthread_mutex_unlock(&s->lock);
|
||||
}
|
||||
|
||||
void logger_close(LoggerState *s, ExitHandler *exit_handler) {
|
||||
pthread_mutex_lock(&s->lock);
|
||||
if (s->cur_handle) {
|
||||
s->cur_handle->exit_signal = exit_handler && exit_handler->signal.load();
|
||||
s->cur_handle->end_sentinel_type = SentinelType::END_OF_ROUTE;
|
||||
lh_close(s->cur_handle);
|
||||
}
|
||||
pthread_mutex_unlock(&s->lock);
|
||||
}
|
||||
|
||||
void lh_log(LoggerHandle* h, uint8_t* data, size_t data_size, bool in_qlog) {
|
||||
pthread_mutex_lock(&h->lock);
|
||||
assert(h->refcnt > 0);
|
||||
h->log->write(data, data_size);
|
||||
if (in_qlog && h->q_log) {
|
||||
h->q_log->write(data, data_size);
|
||||
}
|
||||
pthread_mutex_unlock(&h->lock);
|
||||
}
|
||||
|
||||
void lh_close(LoggerHandle* h) {
|
||||
pthread_mutex_lock(&h->lock);
|
||||
assert(h->refcnt > 0);
|
||||
if (h->refcnt == 1) {
|
||||
// a very ugly hack. only here can guarantee sentinel is the last msg
|
||||
pthread_mutex_unlock(&h->lock);
|
||||
lh_log_sentinel(h, h->end_sentinel_type);
|
||||
pthread_mutex_lock(&h->lock);
|
||||
}
|
||||
h->refcnt--;
|
||||
if (h->refcnt == 0) {
|
||||
h->log.reset(nullptr);
|
||||
h->q_log.reset(nullptr);
|
||||
unlink(h->lock_path);
|
||||
pthread_mutex_unlock(&h->lock);
|
||||
pthread_mutex_destroy(&h->lock);
|
||||
return;
|
||||
}
|
||||
pthread_mutex_unlock(&h->lock);
|
||||
}
|
||||
@@ -0,0 +1,80 @@
|
||||
#pragma once
|
||||
|
||||
#include <cassert>
|
||||
#include <pthread.h>
|
||||
|
||||
#include <cstdint>
|
||||
#include <cstdio>
|
||||
#include <memory>
|
||||
|
||||
#include <capnp/serialize.h>
|
||||
#include <kj/array.h>
|
||||
|
||||
#include "cereal/messaging/messaging.h"
|
||||
#include "common/util.h"
|
||||
#include "common/swaglog.h"
|
||||
#include "system/hardware/hw.h"
|
||||
|
||||
const std::string LOG_ROOT = Path::log_root();
|
||||
|
||||
#define LOGGER_MAX_HANDLES 16
|
||||
|
||||
class RawFile {
|
||||
public:
|
||||
RawFile(const char* path) {
|
||||
file = util::safe_fopen(path, "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;
|
||||
|
||||
typedef struct LoggerHandle {
|
||||
pthread_mutex_t lock;
|
||||
SentinelType end_sentinel_type;
|
||||
int exit_signal;
|
||||
int refcnt;
|
||||
char segment_path[4096];
|
||||
char log_path[4096];
|
||||
char qlog_path[4096];
|
||||
char lock_path[4096];
|
||||
std::unique_ptr<RawFile> log, q_log;
|
||||
} LoggerHandle;
|
||||
|
||||
typedef struct LoggerState {
|
||||
pthread_mutex_t lock;
|
||||
int part;
|
||||
kj::Array<capnp::word> init_data;
|
||||
std::string route_name;
|
||||
char log_name[64];
|
||||
bool has_qlog;
|
||||
|
||||
LoggerHandle handles[LOGGER_MAX_HANDLES];
|
||||
LoggerHandle* cur_handle;
|
||||
} LoggerState;
|
||||
|
||||
kj::Array<capnp::word> logger_build_init_data();
|
||||
std::string logger_get_route_name();
|
||||
void logger_init(LoggerState *s, bool has_qlog);
|
||||
int logger_next(LoggerState *s, const char* root_path,
|
||||
char* out_segment_path, size_t out_segment_path_len,
|
||||
int* out_part);
|
||||
LoggerHandle* logger_get_handle(LoggerState *s);
|
||||
void logger_close(LoggerState *s, ExitHandler *exit_handler=nullptr);
|
||||
void logger_log(LoggerState *s, uint8_t* data, size_t data_size, bool in_qlog);
|
||||
|
||||
void lh_log(LoggerHandle* h, uint8_t* data, size_t data_size, bool in_qlog);
|
||||
void lh_close(LoggerHandle* h);
|
||||
@@ -0,0 +1,305 @@
|
||||
#include <sys/xattr.h>
|
||||
|
||||
#include <unordered_map>
|
||||
|
||||
#include "system/loggerd/encoder/encoder.h"
|
||||
#include "system/loggerd/loggerd.h"
|
||||
#include "system/loggerd/video_writer.h"
|
||||
|
||||
ExitHandler do_exit;
|
||||
|
||||
struct LoggerdState {
|
||||
LoggerState logger = {};
|
||||
char segment_path[4096];
|
||||
std::atomic<int> rotate_segment;
|
||||
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) {
|
||||
int segment = -1;
|
||||
int err = logger_next(&s->logger, LOG_ROOT.c_str(), s->segment_path, sizeof(s->segment_path), &segment);
|
||||
assert(err == 0);
|
||||
s->rotate_segment = segment;
|
||||
s->ready_to_rotate = 0;
|
||||
s->last_rotate_tms = millis_since_boot();
|
||||
LOGW((s->logger.part == 0) ? "logging to %s" : "rotated to %s", s->segment_path);
|
||||
}
|
||||
|
||||
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->rotate_segment) {
|
||||
// loggerd is now on the segment that matches this packet
|
||||
|
||||
// if this is a new segment, we close any possible old segments, move to the new, and process any queued packets
|
||||
if (re.current_segment != s->rotate_segment) {
|
||||
if (re.recording) {
|
||||
re.writer.reset();
|
||||
re.recording = false;
|
||||
}
|
||||
re.current_segment = s->rotate_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) {
|
||||
re.writer.reset(new VideoWriter(s->segment_path,
|
||||
encoder_info.filename, idx.getType() != cereal::EncodeIndex::Type::FULL_H_E_V_C,
|
||||
encoder_info.frame_width, encoder_info.frame_height, encoder_info.fps, idx.getType()));
|
||||
// write the header
|
||||
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();
|
||||
logger_log(&s->logger, (uint8_t *)new_msg.begin(), new_msg.size(), true); // always in qlog?
|
||||
bytes_count += new_msg.size();
|
||||
|
||||
// free the message, we used it
|
||||
delete msg;
|
||||
} else if (offset_segment_num > s->rotate_segment) {
|
||||
// encoderd packet has a newer segment, this means encoderd has rolled over
|
||||
if (!re.marked_ready_to_rotate) {
|
||||
re.marked_ready_to_rotate = true;
|
||||
++s->ready_to_rotate;
|
||||
LOGD("rotate %d -> %d ready %d/%d for %s",
|
||||
s->rotate_segment.load(), offset_segment_num,
|
||||
s->ready_to_rotate.load(), s->max_waiting, name.c_str());
|
||||
}
|
||||
// queue up all the new segment messages, they go in after the rotate
|
||||
re.q.push_back(msg);
|
||||
} else {
|
||||
LOGE("%s: encoderd packet has a older segment!!! idx.getSegmentNum():%d s->rotate_segment:%d re.encoderd_segment_offset:%d",
|
||||
name.c_str(), idx.getSegmentNum(), s->rotate_segment.load(), re.encoderd_segment_offset);
|
||||
// free the message, it's useless. this should never happen
|
||||
// actually, this can happen if you restart encoderd
|
||||
re.encoderd_segment_offset = -s->rotate_segment.load();
|
||||
delete msg;
|
||||
}
|
||||
|
||||
return bytes_count;
|
||||
}
|
||||
|
||||
void handle_user_flag(LoggerdState *s) {
|
||||
static int prev_segment = -1;
|
||||
if (s->rotate_segment == prev_segment) return;
|
||||
|
||||
LOGW("preserving %s", s->segment_path);
|
||||
|
||||
#ifdef __APPLE__
|
||||
int ret = setxattr(s->segment_path, PRESERVE_ATTR_NAME, &PRESERVE_ATTR_VALUE, 1, 0, 0);
|
||||
#else
|
||||
int ret = setxattr(s->segment_path, PRESERVE_ATTR_NAME, &PRESERVE_ATTR_VALUE, 1, 0);
|
||||
#endif
|
||||
if (ret) {
|
||||
LOGE("setxattr %s failed for %s: %s", PRESERVE_ATTR_NAME, s->segment_path, strerror(errno));
|
||||
}
|
||||
prev_segment = s->rotate_segment.load();
|
||||
}
|
||||
|
||||
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 = strcmp(it.name+strlen(it.name)-strlen("EncodeData"), "EncodeData") == 0;
|
||||
if (!it.should_log && !encoder) continue;
|
||||
LOGD("logging %s (on port %d)", it.name, 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 = (strcmp(it.name, "userFlag") == 0),
|
||||
};
|
||||
}
|
||||
|
||||
LoggerdState s;
|
||||
// init logger
|
||||
logger_init(&s.logger, true);
|
||||
logger_rotate(&s);
|
||||
Params().put("CurrentRoute", s.logger.route_name);
|
||||
|
||||
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 {
|
||||
logger_log(&s.logger, (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("%lu 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");
|
||||
logger_close(&s.logger, &do_exit);
|
||||
|
||||
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;
|
||||
}
|
||||
@@ -0,0 +1,104 @@
|
||||
#pragma once
|
||||
|
||||
#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 = 10000000;
|
||||
|
||||
#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;
|
||||
bool record = true;
|
||||
int frame_width = 1928;
|
||||
int frame_height = 1208;
|
||||
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 qcam_encoder_info = {
|
||||
.publish_name = "qRoadEncodeData",
|
||||
.filename = "qcamera.ts",
|
||||
.bitrate = 256000,
|
||||
.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 cameras_logged[] = {road_camera_info, wide_road_camera_info, driver_camera_info};
|
||||
@@ -0,0 +1,300 @@
|
||||
#!/usr/bin/env python3
|
||||
import bz2
|
||||
import io
|
||||
import json
|
||||
import os
|
||||
import random
|
||||
import requests
|
||||
import threading
|
||||
import time
|
||||
import traceback
|
||||
from pathlib import Path
|
||||
from typing import BinaryIO, Iterator, List, Optional, Tuple, Union
|
||||
|
||||
from cereal import log
|
||||
import cereal.messaging as messaging
|
||||
from common.api import Api
|
||||
from common.params import Params
|
||||
from common.realtime import set_core_affinity
|
||||
from system.hardware import TICI
|
||||
from system.loggerd.xattr_cache import getxattr, setxattr
|
||||
from system.loggerd.config import ROOT
|
||||
from system.swaglog import cloudlog
|
||||
|
||||
NetworkType = log.DeviceState.NetworkType
|
||||
UPLOAD_ATTR_NAME = 'user.upload'
|
||||
UPLOAD_ATTR_VALUE = b'1'
|
||||
|
||||
UPLOAD_QLOG_QCAM_MAX_SIZE = 100 * 1e6 # MB
|
||||
|
||||
allow_sleep = bool(os.getenv("UPLOADER_SLEEP", "1"))
|
||||
force_wifi = os.getenv("FORCEWIFI") is not None
|
||||
fake_upload = os.getenv("FAKEUPLOAD") is not None
|
||||
|
||||
|
||||
class FakeRequest:
|
||||
def __init__(self):
|
||||
self.headers = {"Content-Length": "0"}
|
||||
|
||||
|
||||
class FakeResponse:
|
||||
def __init__(self):
|
||||
self.status_code = 200
|
||||
self.request = FakeRequest()
|
||||
|
||||
|
||||
UploadResponse = Union[requests.Response, FakeResponse]
|
||||
|
||||
def get_directory_sort(d: str) -> List[str]:
|
||||
return list(map(lambda s: s.rjust(10, '0'), d.rsplit('--', 1)))
|
||||
|
||||
def listdir_by_creation(d: str) -> List[str]:
|
||||
try:
|
||||
paths = os.listdir(d)
|
||||
paths = sorted(paths, key=get_directory_sort)
|
||||
return paths
|
||||
except OSError:
|
||||
cloudlog.exception("listdir_by_creation failed")
|
||||
return list()
|
||||
|
||||
def clear_locks(root: str) -> None:
|
||||
for logname in os.listdir(root):
|
||||
path = os.path.join(root, logname)
|
||||
try:
|
||||
for fname in os.listdir(path):
|
||||
if fname.endswith(".lock"):
|
||||
os.unlink(os.path.join(path, fname))
|
||||
except OSError:
|
||||
cloudlog.exception("clear_locks failed")
|
||||
|
||||
|
||||
class Uploader:
|
||||
def __init__(self, dongle_id: str, root: str):
|
||||
self.dongle_id = dongle_id
|
||||
self.api = Api(dongle_id)
|
||||
self.root = root
|
||||
|
||||
self.last_resp: Optional[UploadResponse] = None
|
||||
self.last_exc: Optional[Tuple[Exception, str]] = None
|
||||
|
||||
self.immediate_size = 0
|
||||
self.immediate_count = 0
|
||||
|
||||
# stats for last successfully uploaded file
|
||||
self.last_time = 0.0
|
||||
self.last_speed = 0.0
|
||||
self.last_filename = ""
|
||||
|
||||
self.immediate_folders = ["crash/", "boot/"]
|
||||
self.immediate_priority = {"qlog": 0, "qlog.bz2": 0, "qcamera.ts": 1}
|
||||
|
||||
def get_upload_sort(self, name: str) -> int:
|
||||
if name in self.immediate_priority:
|
||||
return self.immediate_priority[name]
|
||||
return 1000
|
||||
|
||||
def list_upload_files(self) -> Iterator[Tuple[str, str, str]]:
|
||||
if not os.path.isdir(self.root):
|
||||
return
|
||||
|
||||
self.immediate_size = 0
|
||||
self.immediate_count = 0
|
||||
|
||||
for logname in listdir_by_creation(self.root):
|
||||
path = os.path.join(self.root, logname)
|
||||
try:
|
||||
names = os.listdir(path)
|
||||
except OSError:
|
||||
continue
|
||||
|
||||
if any(name.endswith(".lock") for name in names):
|
||||
continue
|
||||
|
||||
for name in sorted(names, key=self.get_upload_sort):
|
||||
key = os.path.join(logname, name)
|
||||
fn = os.path.join(path, name)
|
||||
# skip files already uploaded
|
||||
try:
|
||||
is_uploaded = getxattr(fn, UPLOAD_ATTR_NAME) == UPLOAD_ATTR_VALUE
|
||||
except OSError:
|
||||
cloudlog.event("uploader_getxattr_failed", exc=self.last_exc, key=key, fn=fn)
|
||||
is_uploaded = True # deleter could have deleted
|
||||
if is_uploaded:
|
||||
continue
|
||||
|
||||
try:
|
||||
if name in self.immediate_priority:
|
||||
self.immediate_count += 1
|
||||
self.immediate_size += os.path.getsize(fn)
|
||||
except OSError:
|
||||
pass
|
||||
|
||||
yield name, key, fn
|
||||
|
||||
def next_file_to_upload(self) -> Optional[Tuple[str, str, str]]:
|
||||
upload_files = list(self.list_upload_files())
|
||||
|
||||
for name, key, fn in upload_files:
|
||||
if any(f in fn for f in self.immediate_folders):
|
||||
return name, key, fn
|
||||
|
||||
for name, key, fn in upload_files:
|
||||
if name in self.immediate_priority:
|
||||
return name, key, fn
|
||||
|
||||
return None
|
||||
|
||||
def do_upload(self, key: str, fn: str) -> None:
|
||||
try:
|
||||
url_resp = self.api.get("v1.4/" + self.dongle_id + "/upload_url/", timeout=10, path=key, access_token=self.api.get_token())
|
||||
if url_resp.status_code == 412:
|
||||
self.last_resp = url_resp
|
||||
return
|
||||
|
||||
url_resp_json = json.loads(url_resp.text)
|
||||
url = url_resp_json['url']
|
||||
headers = url_resp_json['headers']
|
||||
cloudlog.debug("upload_url v1.4 %s %s", url, str(headers))
|
||||
|
||||
if fake_upload:
|
||||
cloudlog.debug(f"*** WARNING, THIS IS A FAKE UPLOAD TO {url} ***")
|
||||
self.last_resp = FakeResponse()
|
||||
else:
|
||||
with open(fn, "rb") as f:
|
||||
data: BinaryIO
|
||||
if key.endswith('.bz2') and not fn.endswith('.bz2'):
|
||||
compressed = bz2.compress(f.read())
|
||||
data = io.BytesIO(compressed)
|
||||
else:
|
||||
data = f
|
||||
|
||||
self.last_resp = requests.put(url, data=data, headers=headers, timeout=10)
|
||||
except Exception as e:
|
||||
self.last_exc = (e, traceback.format_exc())
|
||||
raise
|
||||
|
||||
def normal_upload(self, key: str, fn: str) -> Optional[UploadResponse]:
|
||||
self.last_resp = None
|
||||
self.last_exc = None
|
||||
|
||||
try:
|
||||
self.do_upload(key, fn)
|
||||
except Exception:
|
||||
pass
|
||||
|
||||
return self.last_resp
|
||||
|
||||
def upload(self, name: str, key: str, fn: str, network_type: int, metered: bool) -> bool:
|
||||
try:
|
||||
sz = os.path.getsize(fn)
|
||||
except OSError:
|
||||
cloudlog.exception("upload: getsize failed")
|
||||
return False
|
||||
|
||||
cloudlog.event("upload_start", key=key, fn=fn, sz=sz, network_type=network_type, metered=metered)
|
||||
|
||||
if sz == 0:
|
||||
# tag files of 0 size as uploaded
|
||||
success = True
|
||||
elif name in self.immediate_priority and sz > UPLOAD_QLOG_QCAM_MAX_SIZE:
|
||||
cloudlog.event("uploader_too_large", key=key, fn=fn, sz=sz)
|
||||
success = True
|
||||
else:
|
||||
start_time = time.monotonic()
|
||||
stat = self.normal_upload(key, fn)
|
||||
if stat is not None and stat.status_code in (200, 201, 401, 403, 412):
|
||||
self.last_filename = fn
|
||||
self.last_time = time.monotonic() - start_time
|
||||
if stat.status_code == 412:
|
||||
self.last_speed = 0
|
||||
cloudlog.event("upload_ignored", key=key, fn=fn, sz=sz, network_type=network_type, metered=metered)
|
||||
else:
|
||||
content_length = int(stat.request.headers.get("Content-Length", 0))
|
||||
self.last_speed = (content_length / 1e6) / self.last_time
|
||||
cloudlog.event("upload_success", key=key, fn=fn, sz=sz, content_length=content_length, network_type=network_type, metered=metered, speed=self.last_speed)
|
||||
success = True
|
||||
else:
|
||||
success = False
|
||||
cloudlog.event("upload_failed", stat=stat, exc=self.last_exc, key=key, fn=fn, sz=sz, network_type=network_type, metered=metered)
|
||||
|
||||
if success:
|
||||
# tag file as uploaded
|
||||
try:
|
||||
setxattr(fn, UPLOAD_ATTR_NAME, UPLOAD_ATTR_VALUE)
|
||||
except OSError:
|
||||
cloudlog.event("uploader_setxattr_failed", exc=self.last_exc, key=key, fn=fn, sz=sz)
|
||||
|
||||
return success
|
||||
|
||||
def get_msg(self):
|
||||
msg = messaging.new_message("uploaderState")
|
||||
us = msg.uploaderState
|
||||
us.immediateQueueSize = int(self.immediate_size / 1e6)
|
||||
us.immediateQueueCount = self.immediate_count
|
||||
us.lastTime = self.last_time
|
||||
us.lastSpeed = self.last_speed
|
||||
us.lastFilename = self.last_filename
|
||||
return msg
|
||||
|
||||
|
||||
def uploader_fn(exit_event: threading.Event) -> None:
|
||||
try:
|
||||
set_core_affinity([0, 1, 2, 3])
|
||||
except Exception:
|
||||
cloudlog.exception("failed to set core affinity")
|
||||
|
||||
clear_locks(ROOT)
|
||||
|
||||
params = Params()
|
||||
dongle_id = params.get("DongleId", encoding='utf8')
|
||||
|
||||
if dongle_id is None:
|
||||
cloudlog.info("uploader missing dongle_id")
|
||||
raise Exception("uploader can't start without dongle id")
|
||||
|
||||
if TICI and not Path("/data/media").is_mount():
|
||||
cloudlog.warning("NVME not mounted")
|
||||
|
||||
sm = messaging.SubMaster(['deviceState'])
|
||||
pm = messaging.PubMaster(['uploaderState'])
|
||||
uploader = Uploader(dongle_id, ROOT)
|
||||
|
||||
backoff = 0.1
|
||||
while not exit_event.is_set():
|
||||
sm.update(0)
|
||||
offroad = params.get_bool("IsOffroad")
|
||||
network_type = sm['deviceState'].networkType if not force_wifi else NetworkType.wifi
|
||||
if network_type == NetworkType.none:
|
||||
if allow_sleep:
|
||||
time.sleep(60 if offroad else 5)
|
||||
continue
|
||||
|
||||
d = uploader.next_file_to_upload()
|
||||
if d is None: # Nothing to upload
|
||||
if allow_sleep:
|
||||
time.sleep(60 if offroad else 5)
|
||||
continue
|
||||
|
||||
name, key, fn = d
|
||||
|
||||
# qlogs and bootlogs need to be compressed before uploading
|
||||
if key.endswith(('qlog', 'rlog')) or (key.startswith('boot/') and not key.endswith('.bz2')):
|
||||
key += ".bz2"
|
||||
|
||||
success = uploader.upload(name, key, fn, sm['deviceState'].networkType.raw, sm['deviceState'].networkMetered)
|
||||
if success:
|
||||
backoff = 0.1
|
||||
elif allow_sleep:
|
||||
cloudlog.info("upload backoff %r", backoff)
|
||||
time.sleep(backoff + random.uniform(0, backoff))
|
||||
backoff = min(backoff*2, 120)
|
||||
|
||||
pm.send("uploaderState", uploader.get_msg())
|
||||
|
||||
|
||||
def main() -> None:
|
||||
uploader_fn(threading.Event())
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@@ -0,0 +1,114 @@
|
||||
#pragma clang diagnostic ignored "-Wdeprecated-declarations"
|
||||
#include <cassert>
|
||||
#include <cstdlib>
|
||||
|
||||
#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) {
|
||||
raw = codec == cereal::EncodeIndex::Type::BIG_BOX_LOSSLESS;
|
||||
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) {
|
||||
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: %lu", len, timestamp); }
|
||||
|
||||
av_packet_unref(&pkt);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
VideoWriter::~VideoWriter() {
|
||||
if (this->remuxing) {
|
||||
if (this->raw) { avcodec_close(this->codec_ctx); }
|
||||
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());
|
||||
}
|
||||
@@ -0,0 +1,26 @@
|
||||
#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, raw;
|
||||
};
|
||||
@@ -0,0 +1,23 @@
|
||||
import os
|
||||
import errno
|
||||
from typing import Dict, Optional, Tuple
|
||||
|
||||
_cached_attributes: Dict[Tuple, Optional[bytes]] = {}
|
||||
|
||||
def getxattr(path: str, attr_name: str) -> Optional[bytes]:
|
||||
key = (path, attr_name)
|
||||
if key not in _cached_attributes:
|
||||
try:
|
||||
response = os.getxattr(path, attr_name)
|
||||
except OSError as e:
|
||||
# ENODATA means attribute hasn't been set
|
||||
if e.errno == errno.ENODATA:
|
||||
response = None
|
||||
else:
|
||||
raise
|
||||
_cached_attributes[key] = response
|
||||
return _cached_attributes[key]
|
||||
|
||||
def setxattr(path: str, attr_name: str, attr_value: bytes) -> None:
|
||||
_cached_attributes.pop((path, attr_name), None)
|
||||
return os.setxattr(path, attr_name, attr_value)
|
||||
Executable
+56
@@ -0,0 +1,56 @@
|
||||
#!/usr/bin/env python3
|
||||
import zmq
|
||||
from typing import NoReturn
|
||||
|
||||
import cereal.messaging as messaging
|
||||
from common.logging_extra import SwagLogFileFormatter
|
||||
from system.swaglog import get_file_handler
|
||||
|
||||
|
||||
def main() -> NoReturn:
|
||||
log_handler = get_file_handler()
|
||||
log_handler.setFormatter(SwagLogFileFormatter(None))
|
||||
log_level = 20 # logging.INFO
|
||||
|
||||
ctx = zmq.Context.instance()
|
||||
sock = ctx.socket(zmq.PULL)
|
||||
sock.bind("ipc:///tmp/logmessage")
|
||||
|
||||
# and we publish them
|
||||
log_message_sock = messaging.pub_sock('logMessage')
|
||||
error_log_message_sock = messaging.pub_sock('errorLogMessage')
|
||||
|
||||
try:
|
||||
while True:
|
||||
dat = b''.join(sock.recv_multipart())
|
||||
level = dat[0]
|
||||
record = dat[1:].decode("utf-8")
|
||||
if level >= log_level:
|
||||
log_handler.emit(record)
|
||||
|
||||
if len(record) > 2*1024*1024:
|
||||
print("WARNING: log too big to publish", len(record))
|
||||
print(print(record[:100]))
|
||||
continue
|
||||
|
||||
# then we publish them
|
||||
msg = messaging.new_message()
|
||||
msg.logMessage = record
|
||||
log_message_sock.send(msg.to_bytes())
|
||||
|
||||
if level >= 40: # logging.ERROR
|
||||
msg = messaging.new_message()
|
||||
msg.errorLogMessage = record
|
||||
error_log_message_sock.send(msg.to_bytes())
|
||||
finally:
|
||||
sock.close()
|
||||
ctx.term()
|
||||
|
||||
# can hit this if interrupted during a rollover
|
||||
try:
|
||||
log_handler.close()
|
||||
except ValueError:
|
||||
pass
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
Executable
+105
@@ -0,0 +1,105 @@
|
||||
#!/usr/bin/env python3
|
||||
import numpy as np
|
||||
|
||||
from cereal import messaging
|
||||
from common.filter_simple import FirstOrderFilter
|
||||
from common.realtime import Ratekeeper
|
||||
from system.swaglog import cloudlog
|
||||
|
||||
RATE = 10
|
||||
FFT_SAMPLES = 4096
|
||||
REFERENCE_SPL = 2e-5 # newtons/m^2
|
||||
SAMPLE_RATE = 44100
|
||||
FILTER_DT = 1. / (SAMPLE_RATE / FFT_SAMPLES)
|
||||
|
||||
|
||||
def calculate_spl(measurements):
|
||||
# https://www.engineeringtoolbox.com/sound-pressure-d_711.html
|
||||
sound_pressure = np.sqrt(np.mean(measurements ** 2)) # RMS of amplitudes
|
||||
if sound_pressure > 0:
|
||||
sound_pressure_level = 20 * np.log10(sound_pressure / REFERENCE_SPL) # dB
|
||||
else:
|
||||
sound_pressure_level = 0
|
||||
return sound_pressure, sound_pressure_level
|
||||
|
||||
|
||||
def apply_a_weighting(measurements: np.ndarray) -> np.ndarray:
|
||||
# Generate a Hanning window of the same length as the audio measurements
|
||||
measurements_windowed = measurements * np.hanning(len(measurements))
|
||||
|
||||
# Calculate the frequency axis for the signal
|
||||
freqs = np.fft.fftfreq(measurements_windowed.size, d=1 / SAMPLE_RATE)
|
||||
|
||||
# Calculate the A-weighting filter
|
||||
# https://en.wikipedia.org/wiki/A-weighting
|
||||
A = 12194 ** 2 * freqs ** 4 / ((freqs ** 2 + 20.6 ** 2) * (freqs ** 2 + 12194 ** 2) * np.sqrt((freqs ** 2 + 107.7 ** 2) * (freqs ** 2 + 737.9 ** 2)))
|
||||
A /= np.max(A) # Normalize the filter
|
||||
|
||||
# Apply the A-weighting filter to the signal
|
||||
return np.abs(np.fft.ifft(np.fft.fft(measurements_windowed) * A))
|
||||
|
||||
|
||||
class Mic:
|
||||
def __init__(self, pm):
|
||||
self.pm = pm
|
||||
self.rk = Ratekeeper(RATE)
|
||||
|
||||
self.measurements = np.empty(0)
|
||||
|
||||
self.sound_pressure = 0
|
||||
self.sound_pressure_weighted = 0
|
||||
self.sound_pressure_level_weighted = 0
|
||||
|
||||
self.spl_filter_weighted = FirstOrderFilter(0, 2.5, FILTER_DT, initialized=False)
|
||||
|
||||
def update(self):
|
||||
msg = messaging.new_message('microphone')
|
||||
msg.microphone.soundPressure = float(self.sound_pressure)
|
||||
msg.microphone.soundPressureWeighted = float(self.sound_pressure_weighted)
|
||||
|
||||
msg.microphone.soundPressureWeightedDb = float(self.sound_pressure_level_weighted)
|
||||
msg.microphone.filteredSoundPressureWeightedDb = float(self.spl_filter_weighted.x)
|
||||
|
||||
self.pm.send('microphone', msg)
|
||||
self.rk.keep_time()
|
||||
|
||||
def callback(self, indata, frames, time, status):
|
||||
"""
|
||||
Using amplitude measurements, calculate an uncalibrated sound pressure and sound pressure level.
|
||||
Then apply A-weighting to the raw amplitudes and run the same calculations again.
|
||||
|
||||
Logged A-weighted equivalents are rough approximations of the human-perceived loudness.
|
||||
"""
|
||||
|
||||
self.measurements = np.concatenate((self.measurements, indata[:, 0]))
|
||||
|
||||
while self.measurements.size >= FFT_SAMPLES:
|
||||
measurements = self.measurements[:FFT_SAMPLES]
|
||||
|
||||
self.sound_pressure, _ = calculate_spl(measurements)
|
||||
measurements_weighted = apply_a_weighting(measurements)
|
||||
self.sound_pressure_weighted, self.sound_pressure_level_weighted = calculate_spl(measurements_weighted)
|
||||
self.spl_filter_weighted.update(self.sound_pressure_level_weighted)
|
||||
|
||||
self.measurements = self.measurements[FFT_SAMPLES:]
|
||||
|
||||
def micd_thread(self):
|
||||
# sounddevice must be imported after forking processes
|
||||
import sounddevice as sd # pylint: disable=import-outside-toplevel
|
||||
|
||||
with sd.InputStream(channels=1, samplerate=SAMPLE_RATE, callback=self.callback) as stream:
|
||||
cloudlog.info(f"micd stream started: {stream.samplerate=} {stream.channels=} {stream.dtype=} {stream.device=}")
|
||||
while True:
|
||||
self.update()
|
||||
|
||||
|
||||
def main(pm=None):
|
||||
if pm is None:
|
||||
pm = messaging.PubMaster(['microphone'])
|
||||
|
||||
mic = Mic(pm)
|
||||
mic.micd_thread()
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@@ -0,0 +1,6 @@
|
||||
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('test'):
|
||||
env.Program('tests/test_proclog', ['tests/test_proclog.cc', 'proclog.cc'], LIBS=libs)
|
||||
@@ -0,0 +1,22 @@
|
||||
|
||||
#include <sys/resource.h>
|
||||
|
||||
#include "common/util.h"
|
||||
#include "system/proclogd/proclog.h"
|
||||
|
||||
ExitHandler do_exit;
|
||||
|
||||
int main(int argc, char **argv) {
|
||||
setpriority(PRIO_PROCESS, 0, -15);
|
||||
|
||||
PubMaster publisher({"procLog"});
|
||||
while (!do_exit) {
|
||||
MessageBuilder msg;
|
||||
buildProcLogMessage(msg);
|
||||
publisher.send("procLog", msg);
|
||||
|
||||
util::sleep_for(2000); // 2 secs
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
@@ -0,0 +1,239 @@
|
||||
#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);
|
||||
}
|
||||
@@ -0,0 +1,40 @@
|
||||
#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);
|
||||
@@ -0,0 +1 @@
|
||||
_sensord
|
||||
@@ -0,0 +1,19 @@
|
||||
Import('env', 'arch', 'common', 'cereal', 'messaging')
|
||||
|
||||
sensors = [
|
||||
'sensors/file_sensor.cc',
|
||||
'sensors/i2c_sensor.cc',
|
||||
'sensors/light_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)
|
||||
Executable
+313
@@ -0,0 +1,313 @@
|
||||
#!/usr/bin/env python3
|
||||
import sys
|
||||
import time
|
||||
import signal
|
||||
import serial
|
||||
import struct
|
||||
import requests
|
||||
import urllib.parse
|
||||
from datetime import datetime
|
||||
from typing import List, Optional, Tuple
|
||||
|
||||
from cereal import messaging
|
||||
from common.params import Params
|
||||
from system.swaglog import cloudlog
|
||||
from system.hardware import TICI
|
||||
from common.gpio import gpio_init, gpio_set
|
||||
from system.hardware.tici.pins import GPIO
|
||||
|
||||
UBLOX_TTY = "/dev/ttyHS0"
|
||||
|
||||
UBLOX_ACK = b"\xb5\x62\x05\x01\x02\x00"
|
||||
UBLOX_NACK = b"\xb5\x62\x05\x00\x02\x00"
|
||||
UBLOX_SOS_ACK = b"\xb5\x62\x09\x14\x08\x00\x02\x00\x00\x00\x01\x00\x00\x00"
|
||||
UBLOX_SOS_NACK = b"\xb5\x62\x09\x14\x08\x00\x02\x00\x00\x00\x00\x00\x00\x00"
|
||||
UBLOX_BACKUP_RESTORE_MSG = b"\xb5\x62\x09\x14\x08\x00\x03"
|
||||
UBLOX_ASSIST_ACK = b"\xb5\x62\x13\x60\x08\x00"
|
||||
|
||||
def set_power(enabled: bool) -> None:
|
||||
gpio_init(GPIO.UBLOX_SAFEBOOT_N, True)
|
||||
gpio_init(GPIO.UBLOX_PWR_EN, True)
|
||||
gpio_init(GPIO.UBLOX_RST_N, True)
|
||||
|
||||
gpio_set(GPIO.UBLOX_SAFEBOOT_N, True)
|
||||
gpio_set(GPIO.UBLOX_PWR_EN, enabled)
|
||||
gpio_set(GPIO.UBLOX_RST_N, enabled)
|
||||
|
||||
def add_ubx_checksum(msg: bytes) -> bytes:
|
||||
A = B = 0
|
||||
for b in msg[2:]:
|
||||
A = (A + b) % 256
|
||||
B = (B + A) % 256
|
||||
return msg + bytes([A, B])
|
||||
|
||||
def get_assistnow_messages(token: bytes) -> List[bytes]:
|
||||
# make request
|
||||
# TODO: implement adding the last known location
|
||||
r = requests.get("https://online-live2.services.u-blox.com/GetOnlineData.ashx", params=urllib.parse.urlencode({
|
||||
'token': token,
|
||||
'gnss': 'gps,glo',
|
||||
'datatype': 'eph,alm,aux',
|
||||
}, safe=':,'), timeout=5)
|
||||
assert r.status_code == 200, "Got invalid status code"
|
||||
dat = r.content
|
||||
|
||||
# split up messages
|
||||
msgs = []
|
||||
while len(dat) > 0:
|
||||
assert dat[:2] == b"\xB5\x62"
|
||||
msg_len = 6 + (dat[5] << 8 | dat[4]) + 2
|
||||
msgs.append(dat[:msg_len])
|
||||
dat = dat[msg_len:]
|
||||
return msgs
|
||||
|
||||
|
||||
class TTYPigeon():
|
||||
def __init__(self):
|
||||
self.tty = serial.VTIMESerial(UBLOX_TTY, baudrate=9600, timeout=0)
|
||||
|
||||
def send(self, dat: bytes) -> None:
|
||||
self.tty.write(dat)
|
||||
|
||||
def receive(self) -> bytes:
|
||||
dat = b''
|
||||
while len(dat) < 0x1000:
|
||||
d = self.tty.read(0x40)
|
||||
dat += d
|
||||
if len(d) == 0:
|
||||
break
|
||||
return dat
|
||||
|
||||
def set_baud(self, baud: int) -> None:
|
||||
self.tty.baudrate = baud
|
||||
|
||||
def wait_for_ack(self, ack: bytes = UBLOX_ACK, nack: bytes = UBLOX_NACK, timeout: float = 0.5) -> bool:
|
||||
dat = b''
|
||||
st = time.monotonic()
|
||||
while True:
|
||||
dat += self.receive()
|
||||
if ack in dat:
|
||||
cloudlog.debug("Received ACK from ublox")
|
||||
return True
|
||||
elif nack in dat:
|
||||
cloudlog.error("Received NACK from ublox")
|
||||
return False
|
||||
elif time.monotonic() - st > timeout:
|
||||
cloudlog.error("No response from ublox")
|
||||
raise TimeoutError('No response from ublox')
|
||||
time.sleep(0.001)
|
||||
|
||||
def send_with_ack(self, dat: bytes, ack: bytes = UBLOX_ACK, nack: bytes = UBLOX_NACK) -> None:
|
||||
self.send(dat)
|
||||
self.wait_for_ack(ack, nack)
|
||||
|
||||
def wait_for_backup_restore_status(self, timeout: float = 1.) -> int:
|
||||
dat = b''
|
||||
st = time.monotonic()
|
||||
while True:
|
||||
dat += self.receive()
|
||||
position = dat.find(UBLOX_BACKUP_RESTORE_MSG)
|
||||
if position >= 0 and len(dat) >= position + 11:
|
||||
return dat[position + 10]
|
||||
elif time.monotonic() - st > timeout:
|
||||
cloudlog.error("No backup restore response from ublox")
|
||||
raise TimeoutError('No response from ublox')
|
||||
time.sleep(0.001)
|
||||
|
||||
def reset_device(self) -> bool:
|
||||
# deleting the backup does not always work on first try (mostly on second try)
|
||||
for _ in range(5):
|
||||
# device cold start
|
||||
self.send(b"\xb5\x62\x06\x04\x04\x00\xff\xff\x00\x00\x0c\x5d")
|
||||
time.sleep(1) # wait for cold start
|
||||
init_baudrate(self)
|
||||
|
||||
# clear configuration
|
||||
self.send_with_ack(b"\xb5\x62\x06\x09\x0d\x00\x1f\x1f\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x17\x71\xd7")
|
||||
|
||||
# clear flash memory (almanac backup)
|
||||
self.send_with_ack(b"\xB5\x62\x09\x14\x04\x00\x01\x00\x00\x00\x22\xf0")
|
||||
|
||||
# try restoring backup to verify it got deleted
|
||||
self.send(b"\xB5\x62\x09\x14\x00\x00\x1D\x60")
|
||||
# 1: failed to restore, 2: could restore, 3: no backup
|
||||
status = self.wait_for_backup_restore_status()
|
||||
if status == 1 or status == 3:
|
||||
return True
|
||||
return False
|
||||
|
||||
def init_baudrate(pigeon: TTYPigeon):
|
||||
# ublox default setting on startup is 9600 baudrate
|
||||
pigeon.set_baud(9600)
|
||||
|
||||
# $PUBX,41,1,0007,0003,460800,0*15\r\n
|
||||
pigeon.send(b"\x24\x50\x55\x42\x58\x2C\x34\x31\x2C\x31\x2C\x30\x30\x30\x37\x2C\x30\x30\x30\x33\x2C\x34\x36\x30\x38\x30\x30\x2C\x30\x2A\x31\x35\x0D\x0A")
|
||||
time.sleep(0.1)
|
||||
pigeon.set_baud(460800)
|
||||
|
||||
|
||||
def initialize_pigeon(pigeon: TTYPigeon) -> bool:
|
||||
# try initializing a few times
|
||||
for _ in range(10):
|
||||
try:
|
||||
|
||||
# setup port config
|
||||
pigeon.send_with_ack(b"\xb5\x62\x06\x00\x14\x00\x03\xFF\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x01\x00\x01\x00\x00\x00\x00\x00\x1E\x7F")
|
||||
pigeon.send_with_ack(b"\xb5\x62\x06\x00\x14\x00\x00\xFF\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x19\x35")
|
||||
pigeon.send_with_ack(b"\xb5\x62\x06\x00\x14\x00\x01\x00\x00\x00\xC0\x08\x00\x00\x00\x08\x07\x00\x01\x00\x01\x00\x00\x00\x00\x00\xF4\x80")
|
||||
pigeon.send_with_ack(b"\xb5\x62\x06\x00\x14\x00\x04\xFF\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x1D\x85")
|
||||
pigeon.send_with_ack(b"\xb5\x62\x06\x00\x00\x00\x06\x18")
|
||||
pigeon.send_with_ack(b"\xb5\x62\x06\x00\x01\x00\x01\x08\x22")
|
||||
pigeon.send_with_ack(b"\xb5\x62\x06\x00\x01\x00\x03\x0A\x24")
|
||||
|
||||
# UBX-CFG-RATE (0x06 0x08)
|
||||
pigeon.send_with_ack(b"\xB5\x62\x06\x08\x06\x00\x64\x00\x01\x00\x00\x00\x79\x10")
|
||||
|
||||
# UBX-CFG-NAV5 (0x06 0x24)
|
||||
pigeon.send_with_ack(b"\xB5\x62\x06\x24\x24\x00\x05\x00\x04\x03\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x5A\x63")
|
||||
|
||||
# UBX-CFG-ODO (0x06 0x1E)
|
||||
pigeon.send_with_ack(b"\xB5\x62\x06\x1E\x14\x00\x00\x00\x00\x00\x01\x03\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x3C\x37")
|
||||
pigeon.send_with_ack(b"\xB5\x62\x06\x39\x08\x00\xFF\xAD\x62\xAD\x1E\x63\x00\x00\x83\x0C")
|
||||
pigeon.send_with_ack(b"\xB5\x62\x06\x23\x28\x00\x00\x00\x00\x04\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x01\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x56\x24")
|
||||
|
||||
# UBX-CFG-NAV5 (0x06 0x24)
|
||||
pigeon.send_with_ack(b"\xB5\x62\x06\x24\x00\x00\x2A\x84")
|
||||
pigeon.send_with_ack(b"\xB5\x62\x06\x23\x00\x00\x29\x81")
|
||||
pigeon.send_with_ack(b"\xB5\x62\x06\x1E\x00\x00\x24\x72")
|
||||
pigeon.send_with_ack(b"\xB5\x62\x06\x39\x00\x00\x3F\xC3")
|
||||
|
||||
# UBX-CFG-MSG (set message rate)
|
||||
pigeon.send_with_ack(b"\xB5\x62\x06\x01\x03\x00\x01\x07\x01\x13\x51")
|
||||
pigeon.send_with_ack(b"\xB5\x62\x06\x01\x03\x00\x02\x15\x01\x22\x70")
|
||||
pigeon.send_with_ack(b"\xB5\x62\x06\x01\x03\x00\x02\x13\x01\x20\x6C")
|
||||
pigeon.send_with_ack(b"\xB5\x62\x06\x01\x03\x00\x0A\x09\x01\x1E\x70")
|
||||
pigeon.send_with_ack(b"\xB5\x62\x06\x01\x03\x00\x0A\x0B\x01\x20\x74")
|
||||
pigeon.send_with_ack(b"\xB5\x62\x06\x01\x03\x00\x01\x35\x01\x41\xAD")
|
||||
cloudlog.debug("pigeon configured")
|
||||
|
||||
# try restoring almanac backup
|
||||
pigeon.send(b"\xB5\x62\x09\x14\x00\x00\x1D\x60")
|
||||
restore_status = pigeon.wait_for_backup_restore_status()
|
||||
if restore_status == 2:
|
||||
cloudlog.warning("almanac backup restored")
|
||||
elif restore_status == 3:
|
||||
cloudlog.warning("no almanac backup found")
|
||||
else:
|
||||
cloudlog.error(f"failed to restore almanac backup, status: {restore_status}")
|
||||
|
||||
# sending time to ublox
|
||||
t_now = datetime.utcnow()
|
||||
if t_now >= datetime(2021, 6, 1):
|
||||
cloudlog.warning("Sending current time to ublox")
|
||||
|
||||
# UBX-MGA-INI-TIME_UTC
|
||||
msg = add_ubx_checksum(b"\xB5\x62\x13\x40\x18\x00" + struct.pack("<BBBBHBBBBBxIHxxI",
|
||||
0x10,
|
||||
0x00,
|
||||
0x00,
|
||||
0x80,
|
||||
t_now.year,
|
||||
t_now.month,
|
||||
t_now.day,
|
||||
t_now.hour,
|
||||
t_now.minute,
|
||||
t_now.second,
|
||||
0,
|
||||
30,
|
||||
0
|
||||
))
|
||||
pigeon.send_with_ack(msg, ack=UBLOX_ASSIST_ACK)
|
||||
|
||||
# try getting AssistNow if we have a token
|
||||
token = Params().get('AssistNowToken')
|
||||
if token is not None:
|
||||
try:
|
||||
for msg in get_assistnow_messages(token):
|
||||
pigeon.send_with_ack(msg, ack=UBLOX_ASSIST_ACK)
|
||||
cloudlog.warning("AssistNow messages sent")
|
||||
except Exception:
|
||||
cloudlog.warning("failed to get AssistNow messages")
|
||||
|
||||
cloudlog.warning("Pigeon GPS on!")
|
||||
break
|
||||
except TimeoutError:
|
||||
cloudlog.warning("Initialization failed, trying again!")
|
||||
else:
|
||||
cloudlog.warning("Failed to initialize pigeon")
|
||||
return False
|
||||
return True
|
||||
|
||||
def deinitialize_and_exit(pigeon: Optional[TTYPigeon]):
|
||||
cloudlog.warning("Storing almanac in ublox flash")
|
||||
|
||||
if pigeon is not None:
|
||||
# controlled GNSS stop
|
||||
pigeon.send(b"\xB5\x62\x06\x04\x04\x00\x00\x00\x08\x00\x16\x74")
|
||||
|
||||
# store almanac in flash
|
||||
pigeon.send(b"\xB5\x62\x09\x14\x04\x00\x00\x00\x00\x00\x21\xEC")
|
||||
try:
|
||||
if pigeon.wait_for_ack(ack=UBLOX_SOS_ACK, nack=UBLOX_SOS_NACK):
|
||||
cloudlog.warning("Done storing almanac")
|
||||
else:
|
||||
cloudlog.error("Error storing almanac")
|
||||
except TimeoutError:
|
||||
pass
|
||||
|
||||
# turn off power and exit cleanly
|
||||
set_power(False)
|
||||
sys.exit(0)
|
||||
|
||||
def create_pigeon() -> Tuple[TTYPigeon, messaging.PubMaster]:
|
||||
pigeon = None
|
||||
|
||||
# register exit handler
|
||||
signal.signal(signal.SIGINT, lambda sig, frame: deinitialize_and_exit(pigeon))
|
||||
pm = messaging.PubMaster(['ubloxRaw'])
|
||||
|
||||
# power cycle ublox
|
||||
set_power(False)
|
||||
time.sleep(0.1)
|
||||
set_power(True)
|
||||
time.sleep(0.5)
|
||||
|
||||
pigeon = TTYPigeon()
|
||||
return pigeon, pm
|
||||
|
||||
def run_receiving(pigeon: TTYPigeon, pm: messaging.PubMaster, duration: int = 0):
|
||||
|
||||
start_time = time.monotonic()
|
||||
def end_condition():
|
||||
return True if duration == 0 else time.monotonic() - start_time < duration
|
||||
|
||||
while end_condition():
|
||||
dat = pigeon.receive()
|
||||
if len(dat) > 0:
|
||||
if dat[0] == 0x00:
|
||||
cloudlog.warning("received invalid data from ublox, re-initing!")
|
||||
init_baudrate(pigeon)
|
||||
initialize_pigeon(pigeon)
|
||||
continue
|
||||
|
||||
# send out to socket
|
||||
msg = messaging.new_message('ubloxRaw', len(dat))
|
||||
msg.ubloxRaw = dat[:]
|
||||
pm.send('ubloxRaw', msg)
|
||||
else:
|
||||
# prevent locking up a CPU core if ublox disconnects
|
||||
time.sleep(0.001)
|
||||
|
||||
|
||||
def main():
|
||||
assert TICI, "unsupported hardware for pigeond"
|
||||
|
||||
pigeon, pm = create_pigeon()
|
||||
init_baudrate(pigeon)
|
||||
initialize_pigeon(pigeon)
|
||||
|
||||
# start receiving data
|
||||
run_receiving(pigeon, pm)
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
Executable
+66
@@ -0,0 +1,66 @@
|
||||
#!/usr/bin/env python3
|
||||
import cereal.messaging as messaging
|
||||
from laika import constants
|
||||
|
||||
if __name__ == "__main__":
|
||||
sm = messaging.SubMaster(['ubloxGnss', 'qcomGnss'])
|
||||
|
||||
meas = None
|
||||
while 1:
|
||||
sm.update()
|
||||
if sm['ubloxGnss'].which() == "measurementReport":
|
||||
meas = sm['ubloxGnss'].measurementReport.measurements
|
||||
if not sm.updated['qcomGnss'] or meas is None:
|
||||
continue
|
||||
report = sm['qcomGnss'].measurementReport
|
||||
if report.source not in [0, 1]:
|
||||
continue
|
||||
GLONASS = report.source == 1
|
||||
recv_time = report.milliseconds / 1000
|
||||
|
||||
car = []
|
||||
print("qcom has ", list(sorted([x.svId for x in report.sv])))
|
||||
print("ublox has", list(sorted([x.svId for x in meas if x.gnssId == (6 if GLONASS else 0)])))
|
||||
for i in report.sv:
|
||||
# match to ublox
|
||||
tm = None
|
||||
for m in meas:
|
||||
if i.svId == m.svId and m.gnssId == 0 and m.sigId == 0 and not GLONASS:
|
||||
tm = m
|
||||
if (i.svId-64) == m.svId and m.gnssId == 6 and m.sigId == 0 and GLONASS:
|
||||
tm = m
|
||||
if tm is None:
|
||||
continue
|
||||
|
||||
if not i.measurementStatus.measurementNotUsable and i.measurementStatus.satelliteTimeIsKnown:
|
||||
sat_time = (i.unfilteredMeasurementIntegral + i.unfilteredMeasurementFraction + i.latency) / 1000
|
||||
ublox_psuedorange = tm.pseudorange
|
||||
qcom_psuedorange = (recv_time - sat_time)*constants.SPEED_OF_LIGHT
|
||||
if GLONASS:
|
||||
glonass_freq = tm.glonassFrequencyIndex - 7
|
||||
ublox_speed = -(constants.SPEED_OF_LIGHT / (constants.GLONASS_L1 + glonass_freq*constants.GLONASS_L1_DELTA)) * (tm.doppler)
|
||||
else:
|
||||
ublox_speed = -(constants.SPEED_OF_LIGHT / constants.GPS_L1) * tm.doppler
|
||||
qcom_speed = i.unfilteredSpeed
|
||||
car.append((i.svId, tm.pseudorange, ublox_speed, qcom_psuedorange, qcom_speed, tm.cno))
|
||||
|
||||
if len(car) == 0:
|
||||
print("nothing to compare")
|
||||
continue
|
||||
|
||||
pr_err, speed_err = 0., 0.
|
||||
for c in car:
|
||||
ublox_psuedorange, ublox_speed, qcom_psuedorange, qcom_speed = c[1:5]
|
||||
pr_err += ublox_psuedorange - qcom_psuedorange
|
||||
speed_err += ublox_speed - qcom_speed
|
||||
pr_err /= len(car)
|
||||
speed_err /= len(car)
|
||||
print("avg psuedorange err %f avg speed err %f" % (pr_err, speed_err))
|
||||
for c in sorted(car, key=lambda x: abs(x[1] - x[3] - pr_err)):
|
||||
svid, ublox_psuedorange, ublox_speed, qcom_psuedorange, qcom_speed, cno = c
|
||||
print("svid: %3d pseudorange: %10.2f m speed: %8.2f m/s meas: %12.2f speed: %10.2f meas_err: %10.3f speed_err: %8.3f cno: %d" %
|
||||
(svid, ublox_psuedorange, ublox_speed, qcom_psuedorange, qcom_speed,
|
||||
ublox_psuedorange - qcom_psuedorange - pr_err, ublox_speed - qcom_speed - speed_err, cno))
|
||||
|
||||
|
||||
|
||||
@@ -0,0 +1,94 @@
|
||||
import select
|
||||
from serial import Serial
|
||||
from crcmod import mkCrcFun
|
||||
from struct import pack, unpack_from, calcsize
|
||||
|
||||
class ModemDiag:
|
||||
def __init__(self):
|
||||
self.serial = self.open_serial()
|
||||
self.pend = b''
|
||||
|
||||
def open_serial(self):
|
||||
serial = Serial("/dev/ttyUSB0", baudrate=115200, rtscts=True, dsrdtr=True, timeout=0, exclusive=True)
|
||||
serial.flush()
|
||||
serial.reset_input_buffer()
|
||||
serial.reset_output_buffer()
|
||||
return serial
|
||||
|
||||
ccitt_crc16 = mkCrcFun(0x11021, initCrc=0, xorOut=0xffff)
|
||||
ESCAPE_CHAR = b'\x7d'
|
||||
TRAILER_CHAR = b'\x7e'
|
||||
|
||||
def hdlc_encapsulate(self, payload):
|
||||
payload += pack('<H', ModemDiag.ccitt_crc16(payload))
|
||||
payload = payload.replace(self.ESCAPE_CHAR, bytes([self.ESCAPE_CHAR[0], self.ESCAPE_CHAR[0] ^ 0x20]))
|
||||
payload = payload.replace(self.TRAILER_CHAR, bytes([self.ESCAPE_CHAR[0], self.TRAILER_CHAR[0] ^ 0x20]))
|
||||
payload += self.TRAILER_CHAR
|
||||
return payload
|
||||
|
||||
def hdlc_decapsulate(self, payload):
|
||||
assert len(payload) >= 3
|
||||
assert payload[-1:] == self.TRAILER_CHAR
|
||||
payload = payload[:-1]
|
||||
payload = payload.replace(bytes([self.ESCAPE_CHAR[0], self.TRAILER_CHAR[0] ^ 0x20]), self.TRAILER_CHAR)
|
||||
payload = payload.replace(bytes([self.ESCAPE_CHAR[0], self.ESCAPE_CHAR[0] ^ 0x20]), self.ESCAPE_CHAR)
|
||||
assert payload[-2:] == pack('<H', ModemDiag.ccitt_crc16(payload[:-2]))
|
||||
return payload[:-2]
|
||||
|
||||
def recv(self):
|
||||
# self.serial.read_until makes tons of syscalls!
|
||||
raw_payload = [self.pend]
|
||||
while self.TRAILER_CHAR not in raw_payload[-1]:
|
||||
select.select([self.serial.fd], [], [])
|
||||
raw = self.serial.read(0x10000)
|
||||
raw_payload.append(raw)
|
||||
raw_payload = b''.join(raw_payload)
|
||||
raw_payload, self.pend = raw_payload.split(self.TRAILER_CHAR, 1)
|
||||
raw_payload += self.TRAILER_CHAR
|
||||
unframed_message = self.hdlc_decapsulate(raw_payload)
|
||||
return unframed_message[0], unframed_message[1:]
|
||||
|
||||
def send(self, packet_type, packet_payload):
|
||||
self.serial.write(self.hdlc_encapsulate(bytes([packet_type]) + packet_payload))
|
||||
|
||||
# *** end class ***
|
||||
|
||||
DIAG_LOG_F = 16
|
||||
DIAG_LOG_CONFIG_F = 115
|
||||
LOG_CONFIG_RETRIEVE_ID_RANGES_OP = 1
|
||||
LOG_CONFIG_SET_MASK_OP = 3
|
||||
LOG_CONFIG_SUCCESS_S = 0
|
||||
|
||||
def send_recv(diag, packet_type, packet_payload):
|
||||
diag.send(packet_type, packet_payload)
|
||||
while 1:
|
||||
opcode, payload = diag.recv()
|
||||
if opcode != DIAG_LOG_F:
|
||||
break
|
||||
return opcode, payload
|
||||
|
||||
def setup_logs(diag, types_to_log):
|
||||
opcode, payload = send_recv(diag, DIAG_LOG_CONFIG_F, pack('<3xI', LOG_CONFIG_RETRIEVE_ID_RANGES_OP))
|
||||
|
||||
header_spec = '<3xII'
|
||||
operation, status = unpack_from(header_spec, payload)
|
||||
assert operation == LOG_CONFIG_RETRIEVE_ID_RANGES_OP
|
||||
assert status == LOG_CONFIG_SUCCESS_S
|
||||
|
||||
log_masks = unpack_from('<16I', payload, calcsize(header_spec))
|
||||
|
||||
for log_type, log_mask_bitsize in enumerate(log_masks):
|
||||
if log_mask_bitsize:
|
||||
log_mask = [0] * ((log_mask_bitsize+7)//8)
|
||||
for i in range(log_mask_bitsize):
|
||||
if ((log_type<<12)|i) in types_to_log:
|
||||
log_mask[i//8] |= 1 << (i%8)
|
||||
opcode, payload = send_recv(diag, DIAG_LOG_CONFIG_F, pack('<3xIII',
|
||||
LOG_CONFIG_SET_MASK_OP,
|
||||
log_type,
|
||||
log_mask_bitsize
|
||||
) + bytes(log_mask))
|
||||
assert opcode == DIAG_LOG_CONFIG_F
|
||||
operation, status = unpack_from(header_spec, payload)
|
||||
assert operation == LOG_CONFIG_SET_MASK_OP
|
||||
assert status == LOG_CONFIG_SUCCESS_S
|
||||
Executable
+473
@@ -0,0 +1,473 @@
|
||||
#!/usr/bin/env python3
|
||||
import os
|
||||
import sys
|
||||
import signal
|
||||
import itertools
|
||||
import math
|
||||
import time
|
||||
import pycurl
|
||||
import subprocess
|
||||
from datetime import datetime
|
||||
from multiprocessing import Process, Event
|
||||
from typing import NoReturn, Optional
|
||||
from struct import unpack_from, calcsize, pack
|
||||
|
||||
from cereal import log
|
||||
import cereal.messaging as messaging
|
||||
from common.gpio import gpio_init, gpio_set
|
||||
from laika.gps_time import GPSTime, utc_to_gpst, get_leap_seconds
|
||||
from laika.helpers import get_prn_from_nmea_id
|
||||
from laika.constants import SECS_IN_HR, SECS_IN_DAY, SECS_IN_WEEK
|
||||
from system.hardware.tici.pins import GPIO
|
||||
from system.swaglog import cloudlog
|
||||
from system.sensord.rawgps.modemdiag import ModemDiag, DIAG_LOG_F, setup_logs, send_recv
|
||||
from system.sensord.rawgps.structs import (dict_unpacker, position_report, relist,
|
||||
gps_measurement_report, gps_measurement_report_sv,
|
||||
glonass_measurement_report, glonass_measurement_report_sv,
|
||||
oemdre_measurement_report, oemdre_measurement_report_sv, oemdre_svpoly_report,
|
||||
LOG_GNSS_GPS_MEASUREMENT_REPORT, LOG_GNSS_GLONASS_MEASUREMENT_REPORT,
|
||||
LOG_GNSS_POSITION_REPORT, LOG_GNSS_OEMDRE_MEASUREMENT_REPORT,
|
||||
LOG_GNSS_OEMDRE_SVPOLY_REPORT)
|
||||
|
||||
DEBUG = int(os.getenv("DEBUG", "0"))==1
|
||||
ASSIST_DATA_FILE = '/tmp/xtra3grc.bin'
|
||||
ASSIST_DATA_FILE_DOWNLOAD = ASSIST_DATA_FILE + '.download'
|
||||
ASSISTANCE_URL = 'http://xtrapath3.izatcloud.net/xtra3grc.bin'
|
||||
|
||||
LOG_TYPES = [
|
||||
LOG_GNSS_GPS_MEASUREMENT_REPORT,
|
||||
LOG_GNSS_GLONASS_MEASUREMENT_REPORT,
|
||||
LOG_GNSS_OEMDRE_MEASUREMENT_REPORT,
|
||||
LOG_GNSS_POSITION_REPORT,
|
||||
LOG_GNSS_OEMDRE_SVPOLY_REPORT,
|
||||
]
|
||||
|
||||
|
||||
miscStatusFields = {
|
||||
"multipathEstimateIsValid": 0,
|
||||
"directionIsValid": 1,
|
||||
}
|
||||
|
||||
measurementStatusFields = {
|
||||
"subMillisecondIsValid": 0,
|
||||
"subBitTimeIsKnown": 1,
|
||||
"satelliteTimeIsKnown": 2,
|
||||
"bitEdgeConfirmedFromSignal": 3,
|
||||
"measuredVelocity": 4,
|
||||
"fineOrCoarseVelocity": 5,
|
||||
"lockPointValid": 6,
|
||||
"lockPointPositive": 7,
|
||||
|
||||
"lastUpdateFromDifference": 9,
|
||||
"lastUpdateFromVelocityDifference": 10,
|
||||
"strongIndicationOfCrossCorelation": 11,
|
||||
"tentativeMeasurement": 12,
|
||||
"measurementNotUsable": 13,
|
||||
"sirCheckIsNeeded": 14,
|
||||
"probationMode": 15,
|
||||
|
||||
"multipathIndicator": 24,
|
||||
"imdJammingIndicator": 25,
|
||||
"lteB13TxJammingIndicator": 26,
|
||||
"freshMeasurementIndicator": 27,
|
||||
}
|
||||
|
||||
measurementStatusGPSFields = {
|
||||
"gpsRoundRobinRxDiversity": 18,
|
||||
"gpsRxDiversity": 19,
|
||||
"gpsLowBandwidthRxDiversityCombined": 20,
|
||||
"gpsHighBandwidthNu4": 21,
|
||||
"gpsHighBandwidthNu8": 22,
|
||||
"gpsHighBandwidthUniform": 23,
|
||||
}
|
||||
|
||||
measurementStatusGlonassFields = {
|
||||
"glonassMeanderBitEdgeValid": 16,
|
||||
"glonassTimeMarkValid": 17
|
||||
}
|
||||
|
||||
|
||||
def try_setup_logs(diag, log_types):
|
||||
for _ in range(10):
|
||||
try:
|
||||
setup_logs(diag, log_types)
|
||||
break
|
||||
except Exception:
|
||||
cloudlog.exception("setup logs failed, trying again")
|
||||
time.sleep(1.0)
|
||||
else:
|
||||
raise Exception(f"setup logs failed, {log_types=}")
|
||||
|
||||
def at_cmd(cmd: str) -> Optional[str]:
|
||||
for _ in range(3):
|
||||
try:
|
||||
return subprocess.check_output(f"mmcli -m any --timeout 30 --command='{cmd}'", shell=True, encoding='utf8')
|
||||
except subprocess.CalledProcessError:
|
||||
cloudlog.exception("rawgps.mmcli_command_failed")
|
||||
time.sleep(1.0)
|
||||
raise Exception(f"failed to execute mmcli command {cmd=}")
|
||||
|
||||
|
||||
def gps_enabled() -> bool:
|
||||
try:
|
||||
p = subprocess.check_output("mmcli -m any --command=\"AT+QGPS?\"", shell=True)
|
||||
return b"QGPS: 1" in p
|
||||
except subprocess.CalledProcessError as exc:
|
||||
raise Exception("failed to execute QGPS mmcli command") from exc
|
||||
|
||||
def download_assistance():
|
||||
try:
|
||||
c = pycurl.Curl()
|
||||
c.setopt(pycurl.URL, ASSISTANCE_URL)
|
||||
c.setopt(pycurl.NOBODY, 1)
|
||||
c.setopt(pycurl.CONNECTTIMEOUT, 2)
|
||||
c.perform()
|
||||
bytes_n = c.getinfo(pycurl.CONTENT_LENGTH_DOWNLOAD)
|
||||
c.close()
|
||||
if bytes_n > 1e5:
|
||||
cloudlog.error("Qcom assistance data larger than expected")
|
||||
return
|
||||
|
||||
with open(ASSIST_DATA_FILE_DOWNLOAD, 'wb') as fp:
|
||||
c = pycurl.Curl()
|
||||
c.setopt(pycurl.URL, ASSISTANCE_URL)
|
||||
c.setopt(pycurl.CONNECTTIMEOUT, 5)
|
||||
|
||||
c.setopt(pycurl.WRITEDATA, fp)
|
||||
c.perform()
|
||||
c.close()
|
||||
os.rename(ASSIST_DATA_FILE_DOWNLOAD, ASSIST_DATA_FILE)
|
||||
except pycurl.error:
|
||||
cloudlog.exception("Failed to download assistance file")
|
||||
return
|
||||
|
||||
def downloader_loop(event):
|
||||
if os.path.exists(ASSIST_DATA_FILE):
|
||||
os.remove(ASSIST_DATA_FILE)
|
||||
while not os.path.exists(ASSIST_DATA_FILE) and not event.is_set():
|
||||
download_assistance()
|
||||
time.sleep(10)
|
||||
|
||||
def inject_assistance():
|
||||
for _ in range(5):
|
||||
try:
|
||||
cmd = f"mmcli -m any --timeout 30 --location-inject-assistance-data={ASSIST_DATA_FILE}"
|
||||
subprocess.check_output(cmd, stderr=subprocess.PIPE, shell=True)
|
||||
cloudlog.info("successfully loaded assistance data")
|
||||
return
|
||||
except subprocess.CalledProcessError as e:
|
||||
cloudlog.event(
|
||||
"rawgps.assistance_loading_failed",
|
||||
error=True,
|
||||
cmd=e.cmd,
|
||||
output=e.output,
|
||||
returncode=e.returncode
|
||||
)
|
||||
time.sleep(0.2)
|
||||
cloudlog.error("failed to load assistance after retry")
|
||||
|
||||
def setup_quectel(diag: ModemDiag) -> bool:
|
||||
ret = False
|
||||
|
||||
# enable OEMDRE in the NV
|
||||
# TODO: it has to reboot for this to take effect
|
||||
DIAG_NV_READ_F = 38
|
||||
DIAG_NV_WRITE_F = 39
|
||||
NV_GNSS_OEM_FEATURE_MASK = 7165
|
||||
send_recv(diag, DIAG_NV_WRITE_F, pack('<HI', NV_GNSS_OEM_FEATURE_MASK, 1))
|
||||
send_recv(diag, DIAG_NV_READ_F, pack('<H', NV_GNSS_OEM_FEATURE_MASK))
|
||||
|
||||
try_setup_logs(diag, LOG_TYPES)
|
||||
|
||||
if gps_enabled():
|
||||
at_cmd("AT+QGPSEND")
|
||||
#at_cmd("AT+QGPSDEL=0")
|
||||
|
||||
# disable DPO power savings for more accuracy
|
||||
at_cmd("AT+QGPSCFG=\"dpoenable\",0")
|
||||
# don't automatically turn on GNSS on powerup
|
||||
at_cmd("AT+QGPSCFG=\"autogps\",0")
|
||||
|
||||
# Do internet assistance
|
||||
at_cmd("AT+QGPSXTRA=1")
|
||||
at_cmd("AT+QGPSSUPLURL=\"NULL\"")
|
||||
if os.path.exists(ASSIST_DATA_FILE):
|
||||
ret = True
|
||||
inject_assistance()
|
||||
os.remove(ASSIST_DATA_FILE)
|
||||
#at_cmd("AT+QGPSXTRADATA?")
|
||||
time_str = datetime.utcnow().strftime("%Y/%m/%d,%H:%M:%S")
|
||||
at_cmd(f"AT+QGPSXTRATIME=0,\"{time_str}\",1,1,1000")
|
||||
|
||||
at_cmd("AT+QGPSCFG=\"outport\",\"usbnmea\"")
|
||||
at_cmd("AT+QGPS=1")
|
||||
|
||||
# enable OEMDRE mode
|
||||
DIAG_SUBSYS_CMD_F = 75
|
||||
DIAG_SUBSYS_GPS = 13
|
||||
CGPS_DIAG_PDAPI_CMD = 0x64
|
||||
CGPS_OEM_CONTROL = 202
|
||||
GPSDIAG_OEMFEATURE_DRE = 1
|
||||
GPSDIAG_OEM_DRE_ON = 1
|
||||
|
||||
# gpsdiag_OemControlReqType
|
||||
send_recv(diag, DIAG_SUBSYS_CMD_F, pack('<BHBBIIII',
|
||||
DIAG_SUBSYS_GPS, # Subsystem Id
|
||||
CGPS_DIAG_PDAPI_CMD, # Subsystem Command Code
|
||||
CGPS_OEM_CONTROL, # CGPS Command Code
|
||||
0, # Version
|
||||
GPSDIAG_OEMFEATURE_DRE,
|
||||
GPSDIAG_OEM_DRE_ON,
|
||||
0,0
|
||||
))
|
||||
|
||||
return ret
|
||||
|
||||
def teardown_quectel(diag):
|
||||
at_cmd("AT+QGPSCFG=\"outport\",\"none\"")
|
||||
if gps_enabled():
|
||||
at_cmd("AT+QGPSEND")
|
||||
try_setup_logs(diag, [])
|
||||
|
||||
|
||||
def wait_for_modem():
|
||||
cloudlog.warning("waiting for modem to come up")
|
||||
while True:
|
||||
ret = subprocess.call("mmcli -m any --timeout 10 --command=\"AT+QGPS?\"", stdout=subprocess.DEVNULL, stderr=subprocess.DEVNULL, shell=True)
|
||||
if ret == 0:
|
||||
return
|
||||
time.sleep(0.1)
|
||||
|
||||
|
||||
def main() -> NoReturn:
|
||||
unpack_gps_meas, size_gps_meas = dict_unpacker(gps_measurement_report, True)
|
||||
unpack_gps_meas_sv, size_gps_meas_sv = dict_unpacker(gps_measurement_report_sv, True)
|
||||
|
||||
unpack_glonass_meas, size_glonass_meas = dict_unpacker(glonass_measurement_report, True)
|
||||
unpack_glonass_meas_sv, size_glonass_meas_sv = dict_unpacker(glonass_measurement_report_sv, True)
|
||||
|
||||
unpack_oemdre_meas, size_oemdre_meas = dict_unpacker(oemdre_measurement_report, True)
|
||||
unpack_oemdre_meas_sv, size_oemdre_meas_sv = dict_unpacker(oemdre_measurement_report_sv, True)
|
||||
|
||||
unpack_svpoly, _ = dict_unpacker(oemdre_svpoly_report, True)
|
||||
unpack_position, _ = dict_unpacker(position_report)
|
||||
|
||||
unpack_position, _ = dict_unpacker(position_report)
|
||||
|
||||
wait_for_modem()
|
||||
|
||||
stop_download_event = Event()
|
||||
assist_fetch_proc = Process(target=downloader_loop, args=(stop_download_event,))
|
||||
assist_fetch_proc.start()
|
||||
def cleanup(proc):
|
||||
cloudlog.warning("caught sig disabling quectel gps")
|
||||
gpio_set(GPIO.UBLOX_PWR_EN, False)
|
||||
teardown_quectel(diag)
|
||||
cloudlog.warning("quectel cleanup done")
|
||||
sys.exit(0)
|
||||
signal.signal(signal.SIGINT, lambda sig, frame: cleanup(assist_fetch_proc))
|
||||
signal.signal(signal.SIGTERM, lambda sig, frame: cleanup(assist_fetch_proc))
|
||||
|
||||
# connect to modem
|
||||
diag = ModemDiag()
|
||||
r = setup_quectel(diag)
|
||||
want_assistance = not r
|
||||
current_gps_time = utc_to_gpst(GPSTime.from_datetime(datetime.utcnow()))
|
||||
cloudlog.warning("quectel setup done")
|
||||
gpio_init(GPIO.UBLOX_PWR_EN, True)
|
||||
gpio_set(GPIO.UBLOX_PWR_EN, True)
|
||||
|
||||
pm = messaging.PubMaster(['qcomGnss', 'gpsLocation'])
|
||||
|
||||
while 1:
|
||||
if os.path.exists(ASSIST_DATA_FILE) and want_assistance:
|
||||
setup_quectel(diag)
|
||||
want_assistance = False
|
||||
|
||||
opcode, payload = diag.recv()
|
||||
if opcode != DIAG_LOG_F:
|
||||
cloudlog.error(f"Unhandled opcode: {opcode}")
|
||||
continue
|
||||
|
||||
(pending_msgs, log_outer_length), inner_log_packet = unpack_from('<BH', payload), payload[calcsize('<BH'):]
|
||||
if pending_msgs > 0:
|
||||
cloudlog.debug("have %d pending messages" % pending_msgs)
|
||||
assert log_outer_length == len(inner_log_packet)
|
||||
|
||||
(log_inner_length, log_type, log_time), log_payload = unpack_from('<HHQ', inner_log_packet), inner_log_packet[calcsize('<HHQ'):]
|
||||
assert log_inner_length == len(inner_log_packet)
|
||||
|
||||
if log_type not in LOG_TYPES:
|
||||
continue
|
||||
|
||||
if DEBUG:
|
||||
print("%.4f: got log: %x len %d" % (time.time(), log_type, len(log_payload)))
|
||||
|
||||
if log_type == LOG_GNSS_OEMDRE_MEASUREMENT_REPORT:
|
||||
msg = messaging.new_message('qcomGnss')
|
||||
|
||||
gnss = msg.qcomGnss
|
||||
gnss.logTs = log_time
|
||||
gnss.init('drMeasurementReport')
|
||||
report = gnss.drMeasurementReport
|
||||
|
||||
dat = unpack_oemdre_meas(log_payload)
|
||||
for k,v in dat.items():
|
||||
if k in ["gpsTimeBias", "gpsClockTimeUncertainty"]:
|
||||
k += "Ms"
|
||||
if k == "version":
|
||||
assert v == 2
|
||||
elif k == "svCount" or k.startswith("cdmaClockInfo["):
|
||||
# TODO: should we save cdmaClockInfo?
|
||||
pass
|
||||
elif k == "systemRtcValid":
|
||||
setattr(report, k, bool(v))
|
||||
else:
|
||||
setattr(report, k, v)
|
||||
|
||||
report.init('sv', dat['svCount'])
|
||||
sats = log_payload[size_oemdre_meas:]
|
||||
for i in range(dat['svCount']):
|
||||
sat = unpack_oemdre_meas_sv(sats[size_oemdre_meas_sv*i:size_oemdre_meas_sv*(i+1)])
|
||||
sv = report.sv[i]
|
||||
sv.init('measurementStatus')
|
||||
for k,v in sat.items():
|
||||
if k in ["unkn", "measurementStatus2"]:
|
||||
pass
|
||||
elif k == "multipathEstimateValid":
|
||||
sv.measurementStatus.multipathEstimateIsValid = bool(v)
|
||||
elif k == "directionValid":
|
||||
sv.measurementStatus.directionIsValid = bool(v)
|
||||
elif k == "goodParity":
|
||||
setattr(sv, k, bool(v))
|
||||
elif k == "measurementStatus":
|
||||
for kk,vv in measurementStatusFields.items():
|
||||
setattr(sv.measurementStatus, kk, bool(v & (1<<vv)))
|
||||
else:
|
||||
setattr(sv, k, v)
|
||||
if report.source == log.QcomGnss.MeasurementSource.gps:
|
||||
current_gps_time = GPSTime(report.gpsWeek, report.gpsMilliseconds / 1000.0)
|
||||
pm.send('qcomGnss', msg)
|
||||
elif log_type == LOG_GNSS_POSITION_REPORT:
|
||||
report = unpack_position(log_payload)
|
||||
if report["u_PosSource"] != 2:
|
||||
continue
|
||||
vNED = [report["q_FltVelEnuMps[1]"], report["q_FltVelEnuMps[0]"], -report["q_FltVelEnuMps[2]"]]
|
||||
vNEDsigma = [report["q_FltVelSigmaMps[1]"], report["q_FltVelSigmaMps[0]"], -report["q_FltVelSigmaMps[2]"]]
|
||||
|
||||
msg = messaging.new_message('gpsLocation')
|
||||
gps = msg.gpsLocation
|
||||
gps.latitude = report["t_DblFinalPosLatLon[0]"] * 180/math.pi
|
||||
gps.longitude = report["t_DblFinalPosLatLon[1]"] * 180/math.pi
|
||||
gps.altitude = report["q_FltFinalPosAlt"]
|
||||
gps.speed = math.sqrt(sum([x**2 for x in vNED]))
|
||||
gps.bearingDeg = report["q_FltHeadingRad"] * 180/math.pi
|
||||
gps.unixTimestampMillis = GPSTime(report['w_GpsWeekNumber'],
|
||||
1e-3*report['q_GpsFixTimeMs']).as_unix_timestamp()*1e3
|
||||
gps.source = log.GpsLocationData.SensorSource.qcomdiag
|
||||
gps.vNED = vNED
|
||||
gps.verticalAccuracy = report["q_FltVdop"]
|
||||
gps.bearingAccuracyDeg = report["q_FltHeadingUncRad"] * 180/math.pi if (report["q_FltHeadingUncRad"] != 0) else 180
|
||||
gps.speedAccuracy = math.sqrt(sum([x**2 for x in vNEDsigma]))
|
||||
# quectel gps verticalAccuracy is clipped to 500, set invalid if so
|
||||
gps.flags = 1 if gps.verticalAccuracy != 500 else 0
|
||||
if gps.flags:
|
||||
want_assistance = False
|
||||
stop_download_event.set()
|
||||
|
||||
|
||||
pm.send('gpsLocation', msg)
|
||||
|
||||
elif log_type == LOG_GNSS_OEMDRE_SVPOLY_REPORT:
|
||||
msg = messaging.new_message('qcomGnss')
|
||||
dat = unpack_svpoly(log_payload)
|
||||
dat = relist(dat)
|
||||
gnss = msg.qcomGnss
|
||||
gnss.logTs = log_time
|
||||
gnss.init('drSvPoly')
|
||||
poly = gnss.drSvPoly
|
||||
for k,v in dat.items():
|
||||
if k == "version":
|
||||
assert v == 2
|
||||
elif k == "flags":
|
||||
pass
|
||||
else:
|
||||
setattr(poly, k, v)
|
||||
|
||||
prn = get_prn_from_nmea_id(poly.svId)
|
||||
if prn[0] == 'R':
|
||||
epoch = GPSTime(current_gps_time.week, (poly.t0 - 3*SECS_IN_HR + SECS_IN_DAY) % (SECS_IN_WEEK) + get_leap_seconds(current_gps_time))
|
||||
else:
|
||||
epoch = GPSTime(current_gps_time.week, poly.t0)
|
||||
|
||||
# handle week rollover
|
||||
if epoch.tow < SECS_IN_DAY and current_gps_time.tow > 6*SECS_IN_DAY:
|
||||
epoch.week += 1
|
||||
elif epoch.tow > 6*SECS_IN_DAY and current_gps_time.tow < SECS_IN_DAY:
|
||||
epoch.week -= 1
|
||||
|
||||
poly.gpsWeek = epoch.week
|
||||
poly.gpsTow = epoch.tow
|
||||
pm.send('qcomGnss', msg)
|
||||
|
||||
elif log_type in [LOG_GNSS_GPS_MEASUREMENT_REPORT, LOG_GNSS_GLONASS_MEASUREMENT_REPORT]:
|
||||
msg = messaging.new_message('qcomGnss')
|
||||
|
||||
gnss = msg.qcomGnss
|
||||
gnss.logTs = log_time
|
||||
gnss.init('measurementReport')
|
||||
report = gnss.measurementReport
|
||||
|
||||
if log_type == LOG_GNSS_GPS_MEASUREMENT_REPORT:
|
||||
dat = unpack_gps_meas(log_payload)
|
||||
sats = log_payload[size_gps_meas:]
|
||||
unpack_meas_sv, size_meas_sv = unpack_gps_meas_sv, size_gps_meas_sv
|
||||
report.source = 0 # gps
|
||||
measurement_status_fields = (measurementStatusFields.items(), measurementStatusGPSFields.items())
|
||||
elif log_type == LOG_GNSS_GLONASS_MEASUREMENT_REPORT:
|
||||
dat = unpack_glonass_meas(log_payload)
|
||||
sats = log_payload[size_glonass_meas:]
|
||||
unpack_meas_sv, size_meas_sv = unpack_glonass_meas_sv, size_glonass_meas_sv
|
||||
report.source = 1 # glonass
|
||||
measurement_status_fields = (measurementStatusFields.items(), measurementStatusGlonassFields.items())
|
||||
else:
|
||||
assert False
|
||||
|
||||
for k,v in dat.items():
|
||||
if k == "version":
|
||||
assert v == 0
|
||||
elif k == "week":
|
||||
report.gpsWeek = v
|
||||
elif k == "svCount":
|
||||
pass
|
||||
else:
|
||||
setattr(report, k, v)
|
||||
report.init('sv', dat['svCount'])
|
||||
if dat['svCount'] > 0:
|
||||
assert len(sats)//dat['svCount'] == size_meas_sv
|
||||
for i in range(dat['svCount']):
|
||||
sv = report.sv[i]
|
||||
sv.init('measurementStatus')
|
||||
sat = unpack_meas_sv(sats[size_meas_sv*i:size_meas_sv*(i+1)])
|
||||
for k,v in sat.items():
|
||||
if k == "parityErrorCount":
|
||||
sv.gpsParityErrorCount = v
|
||||
elif k == "frequencyIndex":
|
||||
sv.glonassFrequencyIndex = v
|
||||
elif k == "hemmingErrorCount":
|
||||
sv.glonassHemmingErrorCount = v
|
||||
elif k == "measurementStatus":
|
||||
for kk,vv in itertools.chain(*measurement_status_fields):
|
||||
setattr(sv.measurementStatus, kk, bool(v & (1<<vv)))
|
||||
elif k == "miscStatus":
|
||||
for kk,vv in miscStatusFields.items():
|
||||
setattr(sv.measurementStatus, kk, bool(v & (1<<vv)))
|
||||
elif k == "pad":
|
||||
pass
|
||||
else:
|
||||
setattr(sv, k, v)
|
||||
|
||||
pm.send('qcomGnss', msg)
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@@ -0,0 +1,354 @@
|
||||
from struct import unpack_from, calcsize
|
||||
|
||||
LOG_GNSS_POSITION_REPORT = 0x1476
|
||||
LOG_GNSS_GPS_MEASUREMENT_REPORT = 0x1477
|
||||
LOG_GNSS_CLOCK_REPORT = 0x1478
|
||||
LOG_GNSS_GLONASS_MEASUREMENT_REPORT = 0x1480
|
||||
LOG_GNSS_BDS_MEASUREMENT_REPORT = 0x1756
|
||||
LOG_GNSS_GAL_MEASUREMENT_REPORT = 0x1886
|
||||
|
||||
LOG_GNSS_OEMDRE_MEASUREMENT_REPORT = 0x14DE
|
||||
LOG_GNSS_OEMDRE_SVPOLY_REPORT = 0x14E1
|
||||
|
||||
LOG_GNSS_ME_DPO_STATUS = 0x1838
|
||||
LOG_GNSS_CD_DB_REPORT = 0x147B
|
||||
LOG_GNSS_PRX_RF_HW_STATUS_REPORT = 0x147E
|
||||
LOG_CGPS_SLOW_CLOCK_CLIB_REPORT = 0x1488
|
||||
LOG_GNSS_CONFIGURATION_STATE = 0x1516
|
||||
|
||||
oemdre_measurement_report = """
|
||||
uint8_t version;
|
||||
uint8_t reason;
|
||||
uint8_t sv_count;
|
||||
uint8_t seq_num;
|
||||
uint8_t seq_max;
|
||||
uint16_t rf_loss;
|
||||
|
||||
uint8_t system_rtc_valid;
|
||||
uint32_t f_count;
|
||||
uint32_t clock_resets;
|
||||
uint64_t system_rtc_time;
|
||||
|
||||
uint8_t gps_leap_seconds;
|
||||
uint8_t gps_leap_seconds_uncertainty;
|
||||
float gps_to_glonass_time_bias_milliseconds;
|
||||
float gps_to_glonass_time_bias_milliseconds_uncertainty;
|
||||
|
||||
uint16_t gps_week;
|
||||
uint32_t gps_milliseconds;
|
||||
uint32_t gps_time_bias;
|
||||
uint32_t gps_clock_time_uncertainty;
|
||||
uint8_t gps_clock_source;
|
||||
|
||||
uint8_t glonass_clock_source;
|
||||
uint8_t glonass_year;
|
||||
uint16_t glonass_day;
|
||||
uint32_t glonass_milliseconds;
|
||||
float glonass_time_bias;
|
||||
float glonass_clock_time_uncertainty;
|
||||
|
||||
float clock_frequency_bias;
|
||||
float clock_frequency_uncertainty;
|
||||
uint8_t frequency_source;
|
||||
|
||||
uint32_t cdma_clock_info[5];
|
||||
|
||||
uint8_t source;
|
||||
"""
|
||||
|
||||
oemdre_svpoly_report = """
|
||||
uint8_t version;
|
||||
uint16_t sv_id;
|
||||
int8_t frequency_index;
|
||||
uint8_t flags;
|
||||
uint16_t iode;
|
||||
double t0;
|
||||
double xyz0[3];
|
||||
double xyzN[9];
|
||||
float other[4];
|
||||
float position_uncertainty;
|
||||
float iono_delay;
|
||||
float iono_dot;
|
||||
float sbas_iono_delay;
|
||||
float sbas_iono_dot;
|
||||
float tropo_delay;
|
||||
float elevation;
|
||||
float elevation_dot;
|
||||
float elevation_uncertainty;
|
||||
double velocity_coeff[12];
|
||||
"""
|
||||
|
||||
|
||||
oemdre_measurement_report_sv = """
|
||||
uint8_t sv_id;
|
||||
uint8_t unkn;
|
||||
int8_t glonass_frequency_index;
|
||||
uint32_t observation_state;
|
||||
uint8_t observations;
|
||||
uint8_t good_observations;
|
||||
uint8_t filter_stages;
|
||||
uint8_t predetect_interval;
|
||||
uint8_t cycle_slip_count;
|
||||
uint16_t postdetections;
|
||||
|
||||
uint32_t measurement_status;
|
||||
uint32_t measurement_status2;
|
||||
|
||||
uint16_t carrier_noise;
|
||||
uint16_t rf_loss;
|
||||
int16_t latency;
|
||||
|
||||
float filtered_measurement_fraction;
|
||||
uint32_t filtered_measurement_integral;
|
||||
float filtered_time_uncertainty;
|
||||
float filtered_speed;
|
||||
float filtered_speed_uncertainty;
|
||||
|
||||
float unfiltered_measurement_fraction;
|
||||
uint32_t unfiltered_measurement_integral;
|
||||
float unfiltered_time_uncertainty;
|
||||
float unfiltered_speed;
|
||||
float unfiltered_speed_uncertainty;
|
||||
|
||||
uint8_t multipath_estimate_valid;
|
||||
uint32_t multipath_estimate;
|
||||
uint8_t direction_valid;
|
||||
float azimuth;
|
||||
float elevation;
|
||||
float doppler_acceleration;
|
||||
float fine_speed;
|
||||
float fine_speed_uncertainty;
|
||||
|
||||
uint64_t carrier_phase;
|
||||
uint32_t f_count;
|
||||
|
||||
uint16_t parity_error_count;
|
||||
uint8_t good_parity;
|
||||
"""
|
||||
|
||||
glonass_measurement_report = """
|
||||
uint8_t version;
|
||||
uint32_t f_count;
|
||||
uint8_t glonass_cycle_number;
|
||||
uint16_t glonass_number_of_days;
|
||||
uint32_t milliseconds;
|
||||
float time_bias;
|
||||
float clock_time_uncertainty;
|
||||
float clock_frequency_bias;
|
||||
float clock_frequency_uncertainty;
|
||||
uint8_t sv_count;
|
||||
"""
|
||||
|
||||
glonass_measurement_report_sv = """
|
||||
uint8_t sv_id;
|
||||
int8_t frequency_index;
|
||||
uint8_t observation_state; // SVObservationStates
|
||||
uint8_t observations;
|
||||
uint8_t good_observations;
|
||||
uint8_t hemming_error_count;
|
||||
uint8_t filter_stages;
|
||||
uint16_t carrier_noise;
|
||||
int16_t latency;
|
||||
uint8_t predetect_interval;
|
||||
uint16_t postdetections;
|
||||
uint32_t unfiltered_measurement_integral;
|
||||
float unfiltered_measurement_fraction;
|
||||
float unfiltered_time_uncertainty;
|
||||
float unfiltered_speed;
|
||||
float unfiltered_speed_uncertainty;
|
||||
uint32_t measurement_status;
|
||||
uint8_t misc_status;
|
||||
uint32_t multipath_estimate;
|
||||
float azimuth;
|
||||
float elevation;
|
||||
int32_t carrier_phase_cycles_integral;
|
||||
uint16_t carrier_phase_cycles_fraction;
|
||||
float fine_speed;
|
||||
float fine_speed_uncertainty;
|
||||
uint8_t cycle_slip_count;
|
||||
uint32_t pad;
|
||||
"""
|
||||
|
||||
gps_measurement_report = """
|
||||
uint8_t version;
|
||||
uint32_t f_count;
|
||||
uint16_t week;
|
||||
uint32_t milliseconds;
|
||||
float time_bias;
|
||||
float clock_time_uncertainty;
|
||||
float clock_frequency_bias;
|
||||
float clock_frequency_uncertainty;
|
||||
uint8_t sv_count;
|
||||
"""
|
||||
|
||||
gps_measurement_report_sv = """
|
||||
uint8_t sv_id; // SV PRN
|
||||
uint8_t observation_state; // SV Observation state
|
||||
uint8_t observations; // Count of all observation (both success and failure)
|
||||
uint8_t good_observations; // Count of Good observations
|
||||
uint16_t parity_error_count; // Carrier to Code filtering N count
|
||||
uint8_t filter_stages; // Pre-Detection (Coherent) Interval (msecs)
|
||||
uint16_t carrier_noise; // CNo. Units of 0.1 dB
|
||||
int16_t latency; // Age of the measurement in msecs (+ve meas Meas precedes ref time)
|
||||
uint8_t predetect_interval; // Pre-Detection (Coherent) Interval (msecs)
|
||||
uint16_t postdetections; // Num Post-Detections (uints of PreInts
|
||||
uint32_t unfiltered_measurement_integral; // Range of 0 thru (WEEK_MSECS-1) [msecs]
|
||||
float unfiltered_measurement_fraction; // Range of 0 thru 0.99999 [msecs]
|
||||
float unfiltered_time_uncertainty; // Time uncertainty (msec)
|
||||
float unfiltered_speed; // Speed estimate (meters/sec)
|
||||
float unfiltered_speed_uncertainty; // Speed uncertainty estimate (meters/sec)
|
||||
uint32_t measurement_status;
|
||||
uint8_t misc_status;
|
||||
uint32_t multipath_estimate;
|
||||
float azimuth; // Azimuth (radians)
|
||||
float elevation; // Elevation (radians)
|
||||
int32_t carrier_phase_cycles_integral;
|
||||
uint16_t carrier_phase_cycles_fraction;
|
||||
float fine_speed; // Carrier phase derived speed
|
||||
float fine_speed_uncertainty; // Carrier phase derived speed UNC
|
||||
uint8_t cycle_slip_count; // Increments when a CSlip is detected
|
||||
uint32_t pad;
|
||||
"""
|
||||
|
||||
position_report = """
|
||||
uint8 u_Version; /* Version number of DM log */
|
||||
uint32 q_Fcount; /* Local millisecond counter */
|
||||
uint8 u_PosSource; /* Source of position information */ /* 0: None 1: Weighted least-squares 2: Kalman filter 3: Externally injected 4: Internal database */
|
||||
uint32 q_Reserved1; /* Reserved memory field */
|
||||
uint16 w_PosVelFlag; /* Position velocity bit field: (see DM log 0x1476 documentation) */
|
||||
uint32 q_PosVelFlag2; /* Position velocity 2 bit field: (see DM log 0x1476 documentation) */
|
||||
uint8 u_FailureCode; /* Failure code: (see DM log 0x1476 documentation) */
|
||||
uint16 w_FixEvents; /* Fix events bit field: (see DM log 0x1476 documentation) */
|
||||
uint32 _fake_align_week_number;
|
||||
uint16 w_GpsWeekNumber; /* GPS week number of position */
|
||||
uint32 q_GpsFixTimeMs; /* GPS fix time of week of in milliseconds */
|
||||
uint8 u_GloNumFourYear; /* Number of Glonass four year cycles */
|
||||
uint16 w_GloNumDaysInFourYear; /* Glonass calendar day in four year cycle */
|
||||
uint32 q_GloFixTimeMs; /* Glonass fix time of day in milliseconds */
|
||||
uint32 q_PosCount; /* Integer count of the number of unique positions reported */
|
||||
uint64 t_DblFinalPosLatLon[2]; /* Final latitude and longitude of position in radians */
|
||||
uint32 q_FltFinalPosAlt; /* Final height-above-ellipsoid altitude of position */
|
||||
uint32 q_FltHeadingRad; /* User heading in radians */
|
||||
uint32 q_FltHeadingUncRad; /* User heading uncertainty in radians */
|
||||
uint32 q_FltVelEnuMps[3]; /* User velocity in east, north, up coordinate frame. In meters per second. */
|
||||
uint32 q_FltVelSigmaMps[3]; /* Gaussian 1-sigma value for east, north, up components of user velocity */
|
||||
uint32 q_FltClockBiasMeters; /* Receiver clock bias in meters */
|
||||
uint32 q_FltClockBiasSigmaMeters; /* Gaussian 1-sigma value for receiver clock bias in meters */
|
||||
uint32 q_FltGGTBMeters; /* GPS to Glonass time bias in meters */
|
||||
uint32 q_FltGGTBSigmaMeters; /* Gaussian 1-sigma value for GPS to Glonass time bias uncertainty in meters */
|
||||
uint32 q_FltGBTBMeters; /* GPS to BeiDou time bias in meters */
|
||||
uint32 q_FltGBTBSigmaMeters; /* Gaussian 1-sigma value for GPS to BeiDou time bias uncertainty in meters */
|
||||
uint32 q_FltBGTBMeters; /* BeiDou to Glonass time bias in meters */
|
||||
uint32 q_FltBGTBSigmaMeters; /* Gaussian 1-sigma value for BeiDou to Glonass time bias uncertainty in meters */
|
||||
uint32 q_FltFiltGGTBMeters; /* Filtered GPS to Glonass time bias in meters */
|
||||
uint32 q_FltFiltGGTBSigmaMeters; /* Filtered Gaussian 1-sigma value for GPS to Glonass time bias uncertainty in meters */
|
||||
uint32 q_FltFiltGBTBMeters; /* Filtered GPS to BeiDou time bias in meters */
|
||||
uint32 q_FltFiltGBTBSigmaMeters; /* Filtered Gaussian 1-sigma value for GPS to BeiDou time bias uncertainty in meters */
|
||||
uint32 q_FltFiltBGTBMeters; /* Filtered BeiDou to Glonass time bias in meters */
|
||||
uint32 q_FltFiltBGTBSigmaMeters; /* Filtered Gaussian 1-sigma value for BeiDou to Glonass time bias uncertainty in meters */
|
||||
uint32 q_FltSftOffsetSec; /* SFT offset as computed by WLS in seconds */
|
||||
uint32 q_FltSftOffsetSigmaSec; /* Gaussian 1-sigma value for SFT offset in seconds */
|
||||
uint32 q_FltClockDriftMps; /* Clock drift (clock frequency bias) in meters per second */
|
||||
uint32 q_FltClockDriftSigmaMps; /* Gaussian 1-sigma value for clock drift in meters per second */
|
||||
uint32 q_FltFilteredAlt; /* Filtered height-above-ellipsoid altitude in meters as computed by WLS */
|
||||
uint32 q_FltFilteredAltSigma; /* Gaussian 1-sigma value for filtered height-above-ellipsoid altitude in meters */
|
||||
uint32 q_FltRawAlt; /* Raw height-above-ellipsoid altitude in meters as computed by WLS */
|
||||
uint32 q_FltRawAltSigma; /* Gaussian 1-sigma value for raw height-above-ellipsoid altitude in meters */
|
||||
uint32 align_Flt[14];
|
||||
uint32 q_FltPdop; /* 3D position dilution of precision as computed from the unweighted
|
||||
uint32 q_FltHdop; /* Horizontal position dilution of precision as computed from the unweighted least-squares covariance matrix */
|
||||
uint32 q_FltVdop; /* Vertical position dilution of precision as computed from the unweighted least-squares covariance matrix */
|
||||
uint8 u_EllipseConfidence; /* Statistical measure of the confidence (percentage) associated with the uncertainty ellipse values */
|
||||
uint32 q_FltEllipseAngle; /* Angle of semimajor axis with respect to true North, with increasing angles moving clockwise from North. In units of degrees. */
|
||||
uint32 q_FltEllipseSemimajorAxis; /* Semimajor axis of final horizontal position uncertainty error ellipse. In units of meters. */
|
||||
uint32 q_FltEllipseSemiminorAxis; /* Semiminor axis of final horizontal position uncertainty error ellipse. In units of meters. */
|
||||
uint32 q_FltPosSigmaVertical; /* Gaussian 1-sigma value for final position height-above-ellipsoid altitude in meters */
|
||||
uint8 u_HorizontalReliability; /* Horizontal position reliability 0: Not set 1: Very Low 2: Low 3: Medium 4: High */
|
||||
uint8 u_VerticalReliability; /* Vertical position reliability */
|
||||
uint16 w_Reserved2; /* Reserved memory field */
|
||||
uint32 q_FltGnssHeadingRad; /* User heading in radians derived from GNSS only solution */
|
||||
uint32 q_FltGnssHeadingUncRad; /* User heading uncertainty in radians derived from GNSS only solution */
|
||||
uint32 q_SensorDataUsageMask; /* Denotes which additional sensor data were used to compute this position fix. BIT[0] 0x00000001 <96> Accelerometer BIT[1] 0x00000002 <96> Gyro 0x0000FFFC - Reserved A bit set to 1 indicates that certain fields as defined by the SENSOR_AIDING_MASK were aided with sensor data*/
|
||||
uint32 q_SensorAidMask; /* Denotes which component of the position report was assisted with additional sensors defined in SENSOR_DATA_USAGE_MASK BIT[0] 0x00000001 <96> Heading aided with sensor data BIT[1] 0x00000002 <96> Speed aided with sensor data BIT[2] 0x00000004 <96> Position aided with sensor data BIT[3] 0x00000008 <96> Velocity aided with sensor data 0xFFFFFFF0 <96> Reserved */
|
||||
uint8 u_NumGpsSvsUsed; /* The number of GPS SVs used in the fix */
|
||||
uint8 u_TotalGpsSvs; /* Total number of GPS SVs detected by searcher, including ones not used in position calculation */
|
||||
uint8 u_NumGloSvsUsed; /* The number of Glonass SVs used in the fix */
|
||||
uint8 u_TotalGloSvs; /* Total number of Glonass SVs detected by searcher, including ones not used in position calculation */
|
||||
uint8 u_NumBdsSvsUsed; /* The number of BeiDou SVs used in the fix */
|
||||
uint8 u_TotalBdsSvs; /* Total number of BeiDou SVs detected by searcher, including ones not used in position calculation */
|
||||
"""
|
||||
|
||||
def name_to_camelcase(nam):
|
||||
ret = []
|
||||
i = 0
|
||||
while i < len(nam):
|
||||
if nam[i] == "_":
|
||||
ret.append(nam[i+1].upper())
|
||||
i += 2
|
||||
else:
|
||||
ret.append(nam[i])
|
||||
i += 1
|
||||
return ''.join(ret)
|
||||
|
||||
def parse_struct(ss):
|
||||
st = "<"
|
||||
nams = []
|
||||
for l in ss.strip().split("\n"):
|
||||
if len(l.strip()) == 0:
|
||||
continue
|
||||
typ, nam = l.split(";")[0].split()
|
||||
#print(typ, nam)
|
||||
if typ == "float" or '_Flt' in nam:
|
||||
st += "f"
|
||||
elif typ == "double" or '_Dbl' in nam:
|
||||
st += "d"
|
||||
elif typ in ["uint8", "uint8_t"]:
|
||||
st += "B"
|
||||
elif typ in ["int8", "int8_t"]:
|
||||
st += "b"
|
||||
elif typ in ["uint32", "uint32_t"]:
|
||||
st += "I"
|
||||
elif typ in ["int32", "int32_t"]:
|
||||
st += "i"
|
||||
elif typ in ["uint16", "uint16_t"]:
|
||||
st += "H"
|
||||
elif typ in ["int16", "int16_t"]:
|
||||
st += "h"
|
||||
elif typ in ["uint64", "uint64_t"]:
|
||||
st += "Q"
|
||||
else:
|
||||
print("unknown type", typ)
|
||||
assert False
|
||||
if '[' in nam:
|
||||
cnt = int(nam.split("[")[1].split("]")[0])
|
||||
st += st[-1]*(cnt-1)
|
||||
for i in range(cnt):
|
||||
nams.append("%s[%d]" % (nam.split("[")[0], i))
|
||||
else:
|
||||
nams.append(nam)
|
||||
return st, nams
|
||||
|
||||
def dict_unpacker(ss, camelcase = False):
|
||||
st, nams = parse_struct(ss)
|
||||
if camelcase:
|
||||
nams = [name_to_camelcase(x) for x in nams]
|
||||
sz = calcsize(st)
|
||||
return lambda x: dict(zip(nams, unpack_from(st, x))), sz
|
||||
|
||||
def relist(dat):
|
||||
list_keys = set()
|
||||
for key in dat.keys():
|
||||
if '[' in key:
|
||||
list_keys.add(key.split('[')[0])
|
||||
list_dict = {}
|
||||
for list_key in list_keys:
|
||||
list_dict[list_key] = []
|
||||
i = 0
|
||||
while True:
|
||||
key = list_key + f'[{i}]'
|
||||
if key not in dat:
|
||||
break
|
||||
list_dict[list_key].append(dat[key])
|
||||
del dat[key]
|
||||
i += 1
|
||||
return {**dat, **list_dict}
|
||||
Executable
+144
@@ -0,0 +1,144 @@
|
||||
#!/usr/bin/env python3
|
||||
import os
|
||||
import json
|
||||
import time
|
||||
import datetime
|
||||
import unittest
|
||||
import subprocess
|
||||
import numpy as np
|
||||
|
||||
import cereal.messaging as messaging
|
||||
from system.hardware import TICI
|
||||
from system.sensord.rawgps.rawgpsd import at_cmd, wait_for_modem
|
||||
from selfdrive.manager.process_config import managed_processes
|
||||
from common.transformations.coordinates import ecef_from_geodetic
|
||||
|
||||
GOOD_SIGNAL = bool(int(os.getenv("GOOD_SIGNAL", '0')))
|
||||
|
||||
|
||||
class TestRawgpsd(unittest.TestCase):
|
||||
@classmethod
|
||||
def setUpClass(cls):
|
||||
if not TICI:
|
||||
raise unittest.SkipTest
|
||||
|
||||
os.system("sudo systemctl start systemd-resolved")
|
||||
os.system("sudo systemctl restart ModemManager lte")
|
||||
wait_for_modem()
|
||||
|
||||
@classmethod
|
||||
def tearDownClass(cls):
|
||||
managed_processes['rawgpsd'].stop()
|
||||
os.system("sudo systemctl restart systemd-resolved")
|
||||
os.system("sudo systemctl restart ModemManager lte")
|
||||
|
||||
def setUp(self):
|
||||
at_cmd("AT+QGPSDEL=0")
|
||||
self.sm = messaging.SubMaster(['qcomGnss', 'gpsLocation', 'gnssMeasurements'])
|
||||
|
||||
def tearDown(self):
|
||||
managed_processes['rawgpsd'].stop()
|
||||
os.system("sudo systemctl restart systemd-resolved")
|
||||
|
||||
def _wait_for_output(self, t):
|
||||
dt = 0.1
|
||||
for _ in range(t*int(1/dt)):
|
||||
self.sm.update(0)
|
||||
if self.sm.updated['qcomGnss']:
|
||||
break
|
||||
time.sleep(dt)
|
||||
return self.sm.updated['qcomGnss']
|
||||
|
||||
def test_no_crash_double_command(self):
|
||||
at_cmd("AT+QGPSDEL=0")
|
||||
at_cmd("AT+QGPSDEL=0")
|
||||
|
||||
def test_wait_for_modem(self):
|
||||
os.system("sudo systemctl stop ModemManager")
|
||||
managed_processes['rawgpsd'].start()
|
||||
assert not self._wait_for_output(5)
|
||||
|
||||
os.system("sudo systemctl restart ModemManager")
|
||||
assert self._wait_for_output(30)
|
||||
|
||||
def test_startup_time(self):
|
||||
for internet in (True, False):
|
||||
if not internet:
|
||||
os.system("sudo systemctl stop systemd-resolved")
|
||||
with self.subTest(internet=internet):
|
||||
managed_processes['rawgpsd'].start()
|
||||
assert self._wait_for_output(7)
|
||||
managed_processes['rawgpsd'].stop()
|
||||
|
||||
def test_turns_off_gnss(self):
|
||||
for s in (0.1, 1, 5):
|
||||
managed_processes['rawgpsd'].start()
|
||||
time.sleep(s)
|
||||
managed_processes['rawgpsd'].stop()
|
||||
|
||||
ls = subprocess.check_output("mmcli -m any --location-status --output-json", shell=True, encoding='utf-8')
|
||||
loc_status = json.loads(ls)
|
||||
assert set(loc_status['modem']['location']['enabled']) <= {'3gpp-lac-ci'}
|
||||
|
||||
|
||||
def check_assistance(self, should_be_loaded):
|
||||
# after QGPSDEL: '+QGPSXTRADATA: 0,"1980/01/05,19:00:00"'
|
||||
# after loading: '+QGPSXTRADATA: 10080,"2023/06/24,19:00:00"'
|
||||
out = at_cmd("AT+QGPSXTRADATA?")
|
||||
out = out.split("+QGPSXTRADATA:")[1].split("'")[0].strip()
|
||||
valid_duration, injected_time_str = out.split(",", 1)
|
||||
if should_be_loaded:
|
||||
assert valid_duration == "10080" # should be max time
|
||||
injected_time = datetime.datetime.strptime(injected_time_str.replace("\"", ""), "%Y/%m/%d,%H:%M:%S")
|
||||
self.assertLess(abs((datetime.datetime.utcnow() - injected_time).total_seconds()), 60*60*12)
|
||||
else:
|
||||
valid_duration, injected_time_str = out.split(",", 1)
|
||||
injected_time_str = injected_time_str.replace('\"', '').replace('\'', '')
|
||||
assert injected_time_str[:] == '1980/01/05,19:00:00'[:]
|
||||
assert valid_duration == '0'
|
||||
|
||||
def test_assistance_loading(self):
|
||||
managed_processes['rawgpsd'].start()
|
||||
assert self._wait_for_output(10)
|
||||
managed_processes['rawgpsd'].stop()
|
||||
self.check_assistance(True)
|
||||
|
||||
def test_no_assistance_loading(self):
|
||||
os.system("sudo systemctl stop systemd-resolved")
|
||||
|
||||
managed_processes['rawgpsd'].start()
|
||||
assert self._wait_for_output(10)
|
||||
managed_processes['rawgpsd'].stop()
|
||||
self.check_assistance(False)
|
||||
|
||||
def test_late_assistance_loading(self):
|
||||
os.system("sudo systemctl stop systemd-resolved")
|
||||
|
||||
managed_processes['rawgpsd'].start()
|
||||
self._wait_for_output(17)
|
||||
assert self.sm.updated['qcomGnss']
|
||||
|
||||
os.system("sudo systemctl restart systemd-resolved")
|
||||
time.sleep(15)
|
||||
managed_processes['rawgpsd'].stop()
|
||||
self.check_assistance(True)
|
||||
|
||||
@unittest.skipIf(not GOOD_SIGNAL, "No good GPS signal")
|
||||
def test_fix(self):
|
||||
managed_processes['rawgpsd'].start()
|
||||
managed_processes['laikad'].start()
|
||||
assert self._wait_for_output(60)
|
||||
assert self.sm.updated['qcomGnss']
|
||||
assert self.sm.updated['gpsLocation']
|
||||
assert self.sm['gpsLocation'].flags == 1
|
||||
module_fix = ecef_from_geodetic([self.sm['gpsLocation'].latitude,
|
||||
self.sm['gpsLocation'].longitude,
|
||||
self.sm['gpsLocation'].altitude])
|
||||
assert self.sm['gnssMeasurements'].positionECEF.valid
|
||||
total_diff = np.array(self.sm['gnssMeasurements'].positionECEF.value) - module_fix
|
||||
self.assertLess(np.linalg.norm(total_diff), 100)
|
||||
managed_processes['laikad'].stop()
|
||||
managed_processes['rawgpsd'].stop()
|
||||
|
||||
if __name__ == "__main__":
|
||||
unittest.main(failfast=True)
|
||||
Executable
+4
@@ -0,0 +1,4 @@
|
||||
#!/bin/sh
|
||||
cd "$(dirname "$0")"
|
||||
export LD_LIBRARY_PATH="/system/lib64:$LD_LIBRARY_PATH"
|
||||
exec ./_sensord
|
||||
@@ -0,0 +1,78 @@
|
||||
#include "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;
|
||||
}
|
||||
@@ -0,0 +1,41 @@
|
||||
#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();
|
||||
};
|
||||
@@ -0,0 +1,88 @@
|
||||
#include "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;
|
||||
}
|
||||
@@ -0,0 +1,41 @@
|
||||
#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();
|
||||
};
|
||||
@@ -0,0 +1,256 @@
|
||||
#include "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;
|
||||
}
|
||||
@@ -0,0 +1,64 @@
|
||||
#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();
|
||||
};
|
||||
@@ -0,0 +1,31 @@
|
||||
#include "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;
|
||||
}
|
||||
@@ -0,0 +1,13 @@
|
||||
#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; }
|
||||
};
|
||||
@@ -0,0 +1,18 @@
|
||||
#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
|
||||
@@ -0,0 +1,17 @@
|
||||
#include "file_sensor.h"
|
||||
|
||||
#include <string>
|
||||
|
||||
FileSensor::FileSensor(std::string filename) : file(filename) {}
|
||||
|
||||
int FileSensor::init() {
|
||||
return file.is_open() ? 0 : 1;
|
||||
}
|
||||
|
||||
FileSensor::~FileSensor() {
|
||||
file.close();
|
||||
}
|
||||
|
||||
bool FileSensor::has_interrupt_enabled() {
|
||||
return false;
|
||||
}
|
||||
@@ -0,0 +1,19 @@
|
||||
#pragma once
|
||||
|
||||
#include <fstream>
|
||||
#include <string>
|
||||
|
||||
#include "cereal/gen/cpp/log.capnp.h"
|
||||
#include "system/sensord/sensors/sensor.h"
|
||||
|
||||
class FileSensor : public Sensor {
|
||||
protected:
|
||||
std::ifstream file;
|
||||
|
||||
public:
|
||||
FileSensor(std::string filename);
|
||||
~FileSensor();
|
||||
int init();
|
||||
bool has_interrupt_enabled();
|
||||
virtual bool get_event(MessageBuilder &msg, uint64_t ts = 0) = 0;
|
||||
};
|
||||
@@ -0,0 +1,50 @@
|
||||
#include "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;
|
||||
}
|
||||
@@ -0,0 +1,50 @@
|
||||
#pragma once
|
||||
|
||||
#include <cstdint>
|
||||
#include <unistd.h>
|
||||
#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;
|
||||
}
|
||||
};
|
||||
@@ -0,0 +1,27 @@
|
||||
#include "light_sensor.h"
|
||||
|
||||
#include <string>
|
||||
|
||||
#include "common/timing.h"
|
||||
#include "system/sensord/sensors/constants.h"
|
||||
|
||||
LightSensor::LightSensor(std::string filename) : FileSensor(filename) {}
|
||||
|
||||
bool LightSensor::get_event(MessageBuilder &msg, uint64_t ts) {
|
||||
uint64_t start_time = nanos_since_boot();
|
||||
file.clear();
|
||||
file.seekg(0);
|
||||
|
||||
int value;
|
||||
file >> value;
|
||||
|
||||
auto event = msg.initEvent().initLightSensor();
|
||||
event.setSource(cereal::SensorEventData::SensorSource::RPR0521);
|
||||
event.setVersion(1);
|
||||
event.setSensor(SENSOR_LIGHT);
|
||||
event.setType(SENSOR_TYPE_LIGHT);
|
||||
event.setTimestamp(start_time);
|
||||
event.setLight(value);
|
||||
|
||||
return true;
|
||||
}
|
||||
@@ -0,0 +1,9 @@
|
||||
#pragma once
|
||||
#include "file_sensor.h"
|
||||
|
||||
class LightSensor : public FileSensor {
|
||||
public:
|
||||
LightSensor(std::string filename);
|
||||
bool get_event(MessageBuilder &msg, uint64_t ts = 0);
|
||||
int shutdown() { return 0; }
|
||||
};
|
||||
@@ -0,0 +1,250 @@
|
||||
#include "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 =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;
|
||||
}
|
||||
@@ -0,0 +1,49 @@
|
||||
#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();
|
||||
};
|
||||
@@ -0,0 +1,233 @@
|
||||
#include "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;
|
||||
}
|
||||
@@ -0,0 +1,45 @@
|
||||
#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();
|
||||
};
|
||||
@@ -0,0 +1,37 @@
|
||||
#include "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;
|
||||
}
|
||||
@@ -0,0 +1,26 @@
|
||||
#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; }
|
||||
};
|
||||
@@ -0,0 +1,94 @@
|
||||
#include "mmc5603nj_magn.h"
|
||||
|
||||
#include <cassert>
|
||||
|
||||
#include "common/swaglog.h"
|
||||
#include "common/timing.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 100 Hz
|
||||
ret = set_register(MMC5603NJ_I2C_REG_ODR, 100);
|
||||
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;
|
||||
}
|
||||
|
||||
// Set compute measurement rate
|
||||
ret = set_register(MMC5603NJ_I2C_REG_INTERNAL_0, MMC5603NJ_CMM_FREQ_EN | MMC5603NJ_AUTO_SR_EN);
|
||||
if (ret < 0) {
|
||||
goto fail;
|
||||
}
|
||||
|
||||
// Enable continuous mode, set every 100 measurements
|
||||
ret = set_register(MMC5603NJ_I2C_REG_INTERNAL_2, MMC5603NJ_CMM_EN | MMC5603NJ_EN_PRD_SET | 0b11);
|
||||
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;
|
||||
}
|
||||
|
||||
bool MMC5603NJ_Magn::get_event(MessageBuilder &msg, uint64_t ts) {
|
||||
uint64_t start_time = nanos_since_boot();
|
||||
uint8_t buffer[9];
|
||||
int 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;
|
||||
float y = read_20_bit(buffer[7], buffer[3], buffer[2]) * scale;
|
||||
float z = read_20_bit(buffer[8], buffer[5], buffer[4]) * scale;
|
||||
|
||||
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 xyz[] = {x, y, z};
|
||||
auto svec = event.initMagneticUncalibrated();
|
||||
svec.setV(xyz);
|
||||
svec.setStatus(true);
|
||||
|
||||
return true;
|
||||
}
|
||||
@@ -0,0 +1,30 @@
|
||||
#pragma once
|
||||
|
||||
#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)
|
||||
|
||||
class MMC5603NJ_Magn : public I2CSensor {
|
||||
uint8_t get_device_address() {return MMC5603NJ_I2C_ADDR;}
|
||||
public:
|
||||
MMC5603NJ_Magn(I2CBus *bus);
|
||||
int init();
|
||||
bool get_event(MessageBuilder &msg, uint64_t ts = 0);
|
||||
int shutdown();
|
||||
};
|
||||
@@ -0,0 +1,18 @@
|
||||
#pragma once
|
||||
|
||||
#include "cereal/messaging/messaging.h"
|
||||
|
||||
class Sensor {
|
||||
public:
|
||||
int gpio_fd = -1;
|
||||
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 st, uint64_t ct) {
|
||||
return (ct - st) > init_delay;
|
||||
}
|
||||
};
|
||||
Some files were not shown because too many files have changed in this diff Show More
Reference in New Issue
Block a user