mawei/StarPilot
1
0
Fork 0
mirror of https://github.com/firestar5683/StarPilot.git synced 2026-08-21 08:14:00 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge common/ and selfdrive/common (#24556)

Browse Source 
* Merge common/ and selfdrive/common

* fix that

* fix version

* fix unit tests
old-commit-hash: cb8885cffb313bea258c012c3026461a22bf8135
This commit is contained in:
Adeeb Shihadeh
2022-05-18 14:11:57 -07:00
committed by GitHub
parent eea872ae98
commit f4675fc260
140 changed files with 288 additions and 287 deletions
Download Patch File Download Diff File Expand all files Collapse all files
common/SConscript
+37 -2
View File
@@ -1,4 +1,39 @@
Import('envCython', 'common')
Import('env', 'envCython', 'arch', 'SHARED')
if SHARED:
fxn = env.SharedLibrary
else:
fxn = env.Library
common_libs = [
'params.cc',
'statlog.cc',
'swaglog.cc',
'util.cc',
'gpio.cc',
'i2c.cc',
'watchdog.cc',
]
_common = fxn('common', common_libs, LIBS="json11")
files = [
'clutil.cc',
'visionimg.cc',
]
if arch == "larch64":
_gpu_libs = ["GLESv2"]
else:
_gpu_libs = ["GL"]
_gpucommon = fxn('gpucommon', files, LIBS=_gpu_libs)
Export('_common', '_gpucommon', '_gpu_libs')
if GetOption('test'):
env.Program('tests/test_util', ['tests/test_util.cc'], LIBS=[_common])
env.Program('tests/test_swaglog', ['tests/test_swaglog.cc'], LIBS=[_common, 'json11', 'zmq', 'pthread'])
# Cython
envCython.Program('clock.so', 'clock.pyx')
envCython.Program('params_pyx.so', 'params_pyx.pyx', LIBS=envCython['LIBS'] + [common, 'zmq'])
envCython.Program('params_pyx.so', 'params_pyx.pyx', LIBS=envCython['LIBS'] + [_common, 'zmq', 'json11'])
common/clutil.cc
+193
View File
@@ -0,0 +1,193 @@
#include "common/clutil.h"
#include <cassert>
#include <iostream>
#include <memory>
#include "common/util.h"
namespace { // helper functions
template <typename Func, typename Id, typename Name>
std::string get_info(Func get_info_func, Id id, Name param_name) {
size_t size = 0;
CL_CHECK(get_info_func(id, param_name, 0, NULL, &size));
std::string info(size, '\0');
CL_CHECK(get_info_func(id, param_name, size, info.data(), NULL));
return info;
}
inline std::string get_platform_info(cl_platform_id id, cl_platform_info name) { return get_info(&clGetPlatformInfo, id, name); }
inline std::string get_device_info(cl_device_id id, cl_device_info name) { return get_info(&clGetDeviceInfo, id, name); }
void cl_print_info(cl_platform_id platform, cl_device_id device) {
size_t work_group_size = 0;
cl_device_type device_type = 0;
clGetDeviceInfo(device, CL_DEVICE_MAX_WORK_GROUP_SIZE, sizeof(work_group_size), &work_group_size, NULL);
clGetDeviceInfo(device, CL_DEVICE_TYPE, sizeof(device_type), &device_type, NULL);
const char *type_str = "Other...";
switch (device_type) {
case CL_DEVICE_TYPE_CPU: type_str ="CL_DEVICE_TYPE_CPU"; break;
case CL_DEVICE_TYPE_GPU: type_str = "CL_DEVICE_TYPE_GPU"; break;
case CL_DEVICE_TYPE_ACCELERATOR: type_str = "CL_DEVICE_TYPE_ACCELERATOR"; break;
}
std::cout << "vendor: " << get_platform_info(platform, CL_PLATFORM_VENDOR) << std::endl
<< "platform version: " << get_platform_info(platform, CL_PLATFORM_VERSION) << std::endl
<< "profile: " << get_platform_info(platform, CL_PLATFORM_PROFILE) << std::endl
<< "extensions: " << get_platform_info(platform, CL_PLATFORM_EXTENSIONS) << std::endl
<< "name :" << get_device_info(device, CL_DEVICE_NAME) << std::endl
<< "device version :" << get_device_info(device, CL_DEVICE_VERSION) << std::endl
<< "max work group size :" << work_group_size << std::endl
<< "type = " << device_type << " = " << type_str << std::endl;
}
void cl_print_build_errors(cl_program program, cl_device_id device) {
cl_build_status status;
clGetProgramBuildInfo(program, device, CL_PROGRAM_BUILD_STATUS, sizeof(status), &status, NULL);
size_t log_size;
clGetProgramBuildInfo(program, device, CL_PROGRAM_BUILD_LOG, 0, NULL, &log_size);
std::string log(log_size, '\0');
clGetProgramBuildInfo(program, device, CL_PROGRAM_BUILD_LOG, log_size, &log[0], NULL);
std::cout << "build failed; status=" << status << ", log:" << std::endl << log << std::endl;
}
} // namespace
cl_device_id cl_get_device_id(cl_device_type device_type) {
cl_uint num_platforms = 0;
CL_CHECK(clGetPlatformIDs(0, NULL, &num_platforms));
std::unique_ptr<cl_platform_id[]> platform_ids = std::make_unique<cl_platform_id[]>(num_platforms);
CL_CHECK(clGetPlatformIDs(num_platforms, &platform_ids[0], NULL));
for (size_t i = 0; i < num_platforms; ++i) {
std::cout << "platform[" << i << "] CL_PLATFORM_NAME: " << get_platform_info(platform_ids[i], CL_PLATFORM_NAME) << std::endl;
// Get first device
if (cl_device_id device_id = NULL; clGetDeviceIDs(platform_ids[i], device_type, 1, &device_id, NULL) == 0 && device_id) {
cl_print_info(platform_ids[i], device_id);
return device_id;
}
}
std::cout << "No valid openCL platform found" << std::endl;
assert(0);
return nullptr;
}
cl_program cl_program_from_file(cl_context ctx, cl_device_id device_id, const char* path, const char* args) {
return cl_program_from_source(ctx, device_id, util::read_file(path), args);
}
cl_program cl_program_from_source(cl_context ctx, cl_device_id device_id, const std::string& src, const char* args) {
cl_program prg = CL_CHECK_ERR(clCreateProgramWithSource(ctx, 1, (const char*[]){src.c_str()}, NULL, &err));
if (int err = clBuildProgram(prg, 1, &device_id, args, NULL, NULL); err != 0) {
cl_print_build_errors(prg, device_id);
assert(0);
}
return prg;
}
cl_program cl_program_from_binary(cl_context ctx, cl_device_id device_id, const uint8_t* binary, size_t length, const char* args) {
cl_program prg = CL_CHECK_ERR(clCreateProgramWithBinary(ctx, 1, &device_id, &length, (const uint8_t*[]){binary}, NULL, &err));
if (int err = clBuildProgram(prg, 1, &device_id, args, NULL, NULL); err != 0) {
cl_print_build_errors(prg, device_id);
assert(0);
}
return prg;
}
// Given a cl code and return a string representation
#define CL_ERR_TO_STR(err) case err: return #err
const char* cl_get_error_string(int err) {
switch (err) {
CL_ERR_TO_STR(CL_SUCCESS);
CL_ERR_TO_STR(CL_DEVICE_NOT_FOUND);
CL_ERR_TO_STR(CL_DEVICE_NOT_AVAILABLE);
CL_ERR_TO_STR(CL_COMPILER_NOT_AVAILABLE);
CL_ERR_TO_STR(CL_MEM_OBJECT_ALLOCATION_FAILURE);
CL_ERR_TO_STR(CL_OUT_OF_RESOURCES);
CL_ERR_TO_STR(CL_OUT_OF_HOST_MEMORY);
CL_ERR_TO_STR(CL_PROFILING_INFO_NOT_AVAILABLE);
CL_ERR_TO_STR(CL_MEM_COPY_OVERLAP);
CL_ERR_TO_STR(CL_IMAGE_FORMAT_MISMATCH);
CL_ERR_TO_STR(CL_IMAGE_FORMAT_NOT_SUPPORTED);
CL_ERR_TO_STR(CL_MAP_FAILURE);
CL_ERR_TO_STR(CL_MISALIGNED_SUB_BUFFER_OFFSET);
CL_ERR_TO_STR(CL_EXEC_STATUS_ERROR_FOR_EVENTS_IN_WAIT_LIST);
CL_ERR_TO_STR(CL_COMPILE_PROGRAM_FAILURE);
CL_ERR_TO_STR(CL_LINKER_NOT_AVAILABLE);
CL_ERR_TO_STR(CL_LINK_PROGRAM_FAILURE);
CL_ERR_TO_STR(CL_DEVICE_PARTITION_FAILED);
CL_ERR_TO_STR(CL_KERNEL_ARG_INFO_NOT_AVAILABLE);
CL_ERR_TO_STR(CL_INVALID_VALUE);
CL_ERR_TO_STR(CL_INVALID_DEVICE_TYPE);
CL_ERR_TO_STR(CL_INVALID_PLATFORM);
CL_ERR_TO_STR(CL_INVALID_DEVICE);
CL_ERR_TO_STR(CL_INVALID_CONTEXT);
CL_ERR_TO_STR(CL_INVALID_QUEUE_PROPERTIES);
CL_ERR_TO_STR(CL_INVALID_COMMAND_QUEUE);
CL_ERR_TO_STR(CL_INVALID_HOST_PTR);
CL_ERR_TO_STR(CL_INVALID_MEM_OBJECT);
CL_ERR_TO_STR(CL_INVALID_IMAGE_FORMAT_DESCRIPTOR);
CL_ERR_TO_STR(CL_INVALID_IMAGE_SIZE);
CL_ERR_TO_STR(CL_INVALID_SAMPLER);
CL_ERR_TO_STR(CL_INVALID_BINARY);
CL_ERR_TO_STR(CL_INVALID_BUILD_OPTIONS);
CL_ERR_TO_STR(CL_INVALID_PROGRAM);
CL_ERR_TO_STR(CL_INVALID_PROGRAM_EXECUTABLE);
CL_ERR_TO_STR(CL_INVALID_KERNEL_NAME);
CL_ERR_TO_STR(CL_INVALID_KERNEL_DEFINITION);
CL_ERR_TO_STR(CL_INVALID_KERNEL);
CL_ERR_TO_STR(CL_INVALID_ARG_INDEX);
CL_ERR_TO_STR(CL_INVALID_ARG_VALUE);
CL_ERR_TO_STR(CL_INVALID_ARG_SIZE);
CL_ERR_TO_STR(CL_INVALID_KERNEL_ARGS);
CL_ERR_TO_STR(CL_INVALID_WORK_DIMENSION);
CL_ERR_TO_STR(CL_INVALID_WORK_GROUP_SIZE);
CL_ERR_TO_STR(CL_INVALID_WORK_ITEM_SIZE);
CL_ERR_TO_STR(CL_INVALID_GLOBAL_OFFSET);
CL_ERR_TO_STR(CL_INVALID_EVENT_WAIT_LIST);
CL_ERR_TO_STR(CL_INVALID_EVENT);
CL_ERR_TO_STR(CL_INVALID_OPERATION);
CL_ERR_TO_STR(CL_INVALID_GL_OBJECT);
CL_ERR_TO_STR(CL_INVALID_BUFFER_SIZE);
CL_ERR_TO_STR(CL_INVALID_MIP_LEVEL);
CL_ERR_TO_STR(CL_INVALID_GLOBAL_WORK_SIZE);
CL_ERR_TO_STR(CL_INVALID_PROPERTY);
CL_ERR_TO_STR(CL_INVALID_IMAGE_DESCRIPTOR);
CL_ERR_TO_STR(CL_INVALID_COMPILER_OPTIONS);
CL_ERR_TO_STR(CL_INVALID_LINKER_OPTIONS);
CL_ERR_TO_STR(CL_INVALID_DEVICE_PARTITION_COUNT);
case -69: return "CL_INVALID_PIPE_SIZE";
case -70: return "CL_INVALID_DEVICE_QUEUE";
case -71: return "CL_INVALID_SPEC_ID";
case -72: return "CL_MAX_SIZE_RESTRICTION_EXCEEDED";
case -1002: return "CL_INVALID_D3D10_DEVICE_KHR";
case -1003: return "CL_INVALID_D3D10_RESOURCE_KHR";
case -1004: return "CL_D3D10_RESOURCE_ALREADY_ACQUIRED_KHR";
case -1005: return "CL_D3D10_RESOURCE_NOT_ACQUIRED_KHR";
case -1006: return "CL_INVALID_D3D11_DEVICE_KHR";
case -1007: return "CL_INVALID_D3D11_RESOURCE_KHR";
case -1008: return "CL_D3D11_RESOURCE_ALREADY_ACQUIRED_KHR";
case -1009: return "CL_D3D11_RESOURCE_NOT_ACQUIRED_KHR";
case -1010: return "CL_INVALID_DX9_MEDIA_ADAPTER_KHR";
case -1011: return "CL_INVALID_DX9_MEDIA_SURFACE_KHR";
case -1012: return "CL_DX9_MEDIA_SURFACE_ALREADY_ACQUIRED_KHR";
case -1013: return "CL_DX9_MEDIA_SURFACE_NOT_ACQUIRED_KHR";
case -1093: return "CL_INVALID_EGL_OBJECT_KHR";
case -1092: return "CL_EGL_RESOURCE_NOT_ACQUIRED_KHR";
case -1001: return "CL_PLATFORM_NOT_FOUND_KHR";
case -1057: return "CL_DEVICE_PARTITION_FAILED_EXT";
case -1058: return "CL_INVALID_PARTITION_COUNT_EXT";
case -1059: return "CL_INVALID_PARTITION_NAME_EXT";
case -1094: return "CL_INVALID_ACCELERATOR_INTEL";
case -1095: return "CL_INVALID_ACCELERATOR_TYPE_INTEL";
case -1096: return "CL_INVALID_ACCELERATOR_DESCRIPTOR_INTEL";
case -1097: return "CL_ACCELERATOR_TYPE_NOT_SUPPORTED_INTEL";
case -1000: return "CL_INVALID_GL_SHAREGROUP_REFERENCE_KHR";
case -1098: return "CL_INVALID_VA_API_MEDIA_ADAPTER_INTEL";
case -1099: return "CL_INVALID_VA_API_MEDIA_SURFACE_INTEL";
case -1100: return "CL_VA_API_MEDIA_SURFACE_ALREADY_ACQUIRED_INTEL";
case -1101: return "CL_VA_API_MEDIA_SURFACE_NOT_ACQUIRED_INTEL";
default: return "CL_UNKNOWN_ERROR";
}
}
common/clutil.h
+28
View File
@@ -0,0 +1,28 @@
#pragma once
#ifdef __APPLE__
#include <OpenCL/cl.h>
#else
#include <CL/cl.h>
#endif
#include <string>
#define CL_CHECK(_expr) \
do { \
assert(CL_SUCCESS == (_expr)); \
} while (0)
#define CL_CHECK_ERR(_expr) \
({ \
cl_int err = CL_INVALID_VALUE; \
__typeof__(_expr) _ret = _expr; \
assert(_ret&& err == CL_SUCCESS); \
_ret; \
})
cl_device_id cl_get_device_id(cl_device_type device_type);
cl_program cl_program_from_source(cl_context ctx, cl_device_id device_id, const std::string& src, const char* args = nullptr);
cl_program cl_program_from_binary(cl_context ctx, cl_device_id device_id, const uint8_t* binary, size_t length, const char* args = nullptr);
cl_program cl_program_from_file(cl_context ctx, cl_device_id device_id, const char* path, const char* args);
const char* cl_get_error_string(int err);
common/gpio.cc
+32
View File
@@ -0,0 +1,32 @@
#include "common/gpio.h"
#include <fcntl.h>
#include <unistd.h>
#include <cstring>
#include "common/util.h"
// We assume that all pins have already been exported on boot,
// and that we have permission to write to them.
int gpio_init(int pin_nr, bool output) {
char pin_dir_path[50];
int pin_dir_path_len = snprintf(pin_dir_path, sizeof(pin_dir_path),
"/sys/class/gpio/gpio%d/direction", pin_nr);
if(pin_dir_path_len <= 0) {
return -1;
}
const char *value = output ? "out" : "in";
return util::write_file(pin_dir_path, (void*)value, strlen(value));
}
int gpio_set(int pin_nr, bool high) {
char pin_val_path[50];
int pin_val_path_len = snprintf(pin_val_path, sizeof(pin_val_path),
"/sys/class/gpio/gpio%d/value", pin_nr);
if(pin_val_path_len <= 0) {
return -1;
}
return util::write_file(pin_val_path, (void*)(high ? "1" : "0"), 1);
}
common/gpio.h
+21
View File
@@ -0,0 +1,21 @@
#pragma once
// Pin definitions
#ifdef QCOM2
#define GPIO_HUB_RST_N 30
#define GPIO_UBLOX_RST_N 32
#define GPIO_UBLOX_SAFEBOOT_N 33
#define GPIO_UBLOX_PWR_EN 34
#define GPIO_STM_RST_N 124
#define GPIO_STM_BOOT0 134
#else
#define GPIO_HUB_RST_N 0
#define GPIO_UBLOX_RST_N 0
#define GPIO_UBLOX_SAFEBOOT_N 0
#define GPIO_UBLOX_PWR_EN 0
#define GPIO_STM_RST_N 0
#define GPIO_STM_BOOT0 0
#endif
int gpio_init(int pin_nr, bool output);
int gpio_set(int pin_nr, bool high);
common/i2c.cc
+87
View File
@@ -0,0 +1,87 @@
#include "common/i2c.h"
#include <fcntl.h>
#include <sys/ioctl.h>
#include <unistd.h>
#include <cassert>
#include <cstdio>
#include <stdexcept>
#include "common/util.h"
#include "common/swaglog.h"
#include "common/util.h"
#define UNUSED(x) (void)(x)
#ifdef QCOM2
// TODO: decide if we want to isntall libi2c-dev everywhere
extern "C" {
#include <linux/i2c-dev.h>
#include <i2c/smbus.h>
}
I2CBus::I2CBus(uint8_t bus_id) {
char bus_name[20];
snprintf(bus_name, 20, "/dev/i2c-%d", bus_id);
i2c_fd = HANDLE_EINTR(open(bus_name, O_RDWR));
if(i2c_fd < 0) {
throw std::runtime_error("Failed to open I2C bus");
}
}
I2CBus::~I2CBus() {
if(i2c_fd >= 0) { close(i2c_fd); }
}
int I2CBus::read_register(uint8_t device_address, uint register_address, uint8_t *buffer, uint8_t len) {
int ret = 0;
ret = HANDLE_EINTR(ioctl(i2c_fd, I2C_SLAVE, device_address));
if(ret < 0) { goto fail; }
ret = i2c_smbus_read_i2c_block_data(i2c_fd, register_address, len, buffer);
if((ret < 0) || (ret != len)) { goto fail; }
fail:
return ret;
}
int I2CBus::set_register(uint8_t device_address, uint register_address, uint8_t data) {
int ret = 0;
ret = HANDLE_EINTR(ioctl(i2c_fd, I2C_SLAVE, device_address));
if(ret < 0) { goto fail; }
ret = i2c_smbus_write_byte_data(i2c_fd, register_address, data);
if(ret < 0) { goto fail; }
fail:
return ret;
}
#else
I2CBus::I2CBus(uint8_t bus_id) {
UNUSED(bus_id);
i2c_fd = -1;
}
I2CBus::~I2CBus() {}
int I2CBus::read_register(uint8_t device_address, uint register_address, uint8_t *buffer, uint8_t len) {
UNUSED(device_address);
UNUSED(register_address);
UNUSED(buffer);
UNUSED(len);
return -1;
}
int I2CBus::set_register(uint8_t device_address, uint register_address, uint8_t data) {
UNUSED(device_address);
UNUSED(register_address);
UNUSED(data);
return -1;
}
#endif
common/i2c.h
+17
View File
@@ -0,0 +1,17 @@
#pragma once
#include <cstdint>
#include <sys/types.h>
class I2CBus {
private:
int i2c_fd;
public:
I2CBus(uint8_t bus_id);
~I2CBus();
int read_register(uint8_t device_address, uint register_address, uint8_t *buffer, uint8_t len);
int set_register(uint8_t device_address, uint register_address, uint8_t data);
};
common/mat.h
+85
View File
@@ -0,0 +1,85 @@
#pragma once
typedef struct vec3 {
float v[3];
} vec3;
typedef struct vec4 {
float v[4];
} vec4;
typedef struct mat3 {
float v[3*3];
} mat3;
typedef struct mat4 {
float v[4*4];
} mat4;
static inline mat3 matmul3(const mat3 &a, const mat3 &b) {
mat3 ret = {{0.0}};
for (int r=0; r<3; r++) {
for (int c=0; c<3; c++) {
float v = 0.0;
for (int k=0; k<3; k++) {
v += a.v[r*3+k] * b.v[k*3+c];
}
ret.v[r*3+c] = v;
}
}
return ret;
}
static inline vec3 matvecmul3(const mat3 &a, const vec3 &b) {
vec3 ret = {{0.0}};
for (int r=0; r<3; r++) {
for (int c=0; c<3; c++) {
ret.v[r] += a.v[r*3+c] * b.v[c];
}
}
return ret;
}
static inline mat4 matmul(const mat4 &a, const mat4 &b) {
mat4 ret = {{0.0}};
for (int r=0; r<4; r++) {
for (int c=0; c<4; c++) {
float v = 0.0;
for (int k=0; k<4; k++) {
v += a.v[r*4+k] * b.v[k*4+c];
}
ret.v[r*4+c] = v;
}
}
return ret;
}
static inline vec4 matvecmul(const mat4 &a, const vec4 &b) {
vec4 ret = {{0.0}};
for (int r=0; r<4; r++) {
for (int c=0; c<4; c++) {
ret.v[r] += a.v[r*4+c] * b.v[c];
}
}
return ret;
}
// scales the input and output space of a transformation matrix
// that assumes pixel-center origin.
static inline mat3 transform_scale_buffer(const mat3 &in, float s) {
// in_pt = ( transform(out_pt/s + 0.5) - 0.5) * s
mat3 transform_out = {{
1.0f/s, 0.0f, 0.5f,
0.0f, 1.0f/s, 0.5f,
0.0f, 0.0f, 1.0f,
}};
mat3 transform_in = {{
s, 0.0f, -0.5f*s,
0.0f, s, -0.5f*s,
0.0f, 0.0f, 1.0f,
}};
return matmul3(transform_in, matmul3(in, transform_out));
}
common/modeldata.h
+53
View File
@@ -0,0 +1,53 @@
#pragma once
#include <array>
#include "common/mat.h"
#include "selfdrive/hardware/hw.h"
const int TRAJECTORY_SIZE = 33;
const int LAT_MPC_N = 16;
const int LON_MPC_N = 32;
const float MIN_DRAW_DISTANCE = 10.0;
const float MAX_DRAW_DISTANCE = 100.0;
template <typename T, size_t size>
constexpr std::array<T, size> build_idxs(float max_val) {
std::array<T, size> result{};
for (int i = 0; i < size; ++i) {
result[i] = max_val * ((i / (double)(size - 1)) * (i / (double)(size - 1)));
}
return result;
}
constexpr auto T_IDXS = build_idxs<double, TRAJECTORY_SIZE>(10.0);
constexpr auto T_IDXS_FLOAT = build_idxs<float, TRAJECTORY_SIZE>(10.0);
constexpr auto X_IDXS = build_idxs<double, TRAJECTORY_SIZE>(192.0);
constexpr auto X_IDXS_FLOAT = build_idxs<float, TRAJECTORY_SIZE>(192.0);
const int TICI_CAM_WIDTH = 1928;
namespace tici_dm_crop {
const int x_offset = -72;
const int y_offset = -144;
const int width = 954;
};
const mat3 fcam_intrinsic_matrix = (mat3){{2648.0, 0.0, 1928.0 / 2,
0.0, 2648.0, 1208.0 / 2,
0.0, 0.0, 1.0}};
// tici ecam focal probably wrong? magnification is not consistent across frame
// Need to retrain model before this can be changed
const mat3 ecam_intrinsic_matrix = (mat3){{567.0, 0.0, 1928.0 / 2,
0.0, 567.0, 1208.0 / 2,
0.0, 0.0, 1.0}};
static inline mat3 get_model_yuv_transform(bool bayer = true) {
float db_s = 1.0;
const mat3 transform = (mat3){{
1.0, 0.0, 0.0,
0.0, 1.0, 0.0,
0.0, 0.0, 1.0
}};
return bayer ? transform_scale_buffer(transform, db_s) : transform;
}
common/params.cc
+284
View File
@@ -0,0 +1,284 @@
#include "common/params.h"
#include <dirent.h>
#include <sys/file.h>
#include <csignal>
#include <unordered_map>
#include "common/swaglog.h"
#include "common/util.h"
#include "selfdrive/hardware/hw.h"
namespace {
volatile sig_atomic_t params_do_exit = 0;
void params_sig_handler(int signal) {
params_do_exit = 1;
}
int fsync_dir(const std::string &path) {
int result = -1;
int fd = HANDLE_EINTR(open(path.c_str(), O_RDONLY, 0755));
if (fd >= 0) {
result = fsync(fd);
close(fd);
}
return result;
}
bool create_params_path(const std::string &param_path, const std::string &key_path) {
// Make sure params path exists
if (!util::file_exists(param_path) && !util::create_directories(param_path, 0775)) {
return false;
}
// See if the symlink exists, otherwise create it
if (!util::file_exists(key_path)) {
// 1) Create temp folder
// 2) Symlink it to temp link
// 3) Move symlink to <params>/d
std::string tmp_path = param_path + "/.tmp_XXXXXX";
// this should be OK since mkdtemp just replaces characters in place
char *tmp_dir = mkdtemp((char *)tmp_path.c_str());
if (tmp_dir == NULL) {
return false;
}
std::string link_path = std::string(tmp_dir) + ".link";
if (symlink(tmp_dir, link_path.c_str()) != 0) {
return false;
}
// don't return false if it has been created by other
if (rename(link_path.c_str(), key_path.c_str()) != 0 && errno != EEXIST) {
return false;
}
}
return true;
}
std::string ensure_params_path(const std::string &prefix, const std::string &path = {}) {
std::string params_path = path.empty() ? Path::params() : path;
if (!create_params_path(params_path, params_path + prefix)) {
throw std::runtime_error(util::string_format("Failed to ensure params path, errno=%d", errno));
}
return params_path;
}
class FileLock {
public:
FileLock(const std::string &fn) {
fd_ = HANDLE_EINTR(open(fn.c_str(), O_CREAT, 0775));
if (fd_ < 0 || HANDLE_EINTR(flock(fd_, LOCK_EX)) < 0) {
LOGE("Failed to lock file %s, errno=%d", fn.c_str(), errno);
}
}
~FileLock() { close(fd_); }
private:
int fd_ = -1;
};
std::unordered_map<std::string, uint32_t> keys = {
{"AccessToken", CLEAR_ON_MANAGER_START | DONT_LOG},
{"AthenadPid", PERSISTENT},
{"AthenadUploadQueue", PERSISTENT},
{"CalibrationParams", PERSISTENT},
{"CarBatteryCapacity", PERSISTENT},
{"CarParams", CLEAR_ON_MANAGER_START | CLEAR_ON_IGNITION_ON},
{"CarParamsCache", CLEAR_ON_MANAGER_START},
{"CarVin", CLEAR_ON_MANAGER_START | CLEAR_ON_IGNITION_ON},
{"CompletedTrainingVersion", PERSISTENT},
{"ControlsReady", CLEAR_ON_MANAGER_START | CLEAR_ON_IGNITION_ON},
{"CurrentRoute", CLEAR_ON_MANAGER_START | CLEAR_ON_IGNITION_ON},
{"DisableLogging", CLEAR_ON_MANAGER_START | CLEAR_ON_IGNITION_ON},
{"DisablePowerDown", PERSISTENT},
{"DisableRadar_Allow", PERSISTENT},
{"DisableRadar", PERSISTENT}, // WARNING: THIS DISABLES AEB
{"DisableUpdates", PERSISTENT},
{"DisengageOnAccelerator", PERSISTENT},
{"DongleId", PERSISTENT},
{"DoReboot", CLEAR_ON_MANAGER_START},
{"DoShutdown", CLEAR_ON_MANAGER_START},
{"DoUninstall", CLEAR_ON_MANAGER_START},
{"EnableWideCamera", CLEAR_ON_MANAGER_START},
{"EndToEndToggle", PERSISTENT},
{"ForcePowerDown", CLEAR_ON_MANAGER_START},
{"GitBranch", PERSISTENT},
{"GitCommit", PERSISTENT},
{"GitDiff", PERSISTENT},
{"GithubSshKeys", PERSISTENT},
{"GithubUsername", PERSISTENT},
{"GitRemote", PERSISTENT},
{"GsmApn", PERSISTENT},
{"GsmRoaming", PERSISTENT},
{"HardwareSerial", PERSISTENT},
{"HasAcceptedTerms", PERSISTENT},
{"IMEI", PERSISTENT},
{"InstallDate", PERSISTENT},
{"IsDriverViewEnabled", CLEAR_ON_MANAGER_START},
{"IsEngaged", PERSISTENT},
{"IsLdwEnabled", PERSISTENT},
{"IsMetric", PERSISTENT},
{"IsOffroad", CLEAR_ON_MANAGER_START},
{"IsOnroad", PERSISTENT},
{"IsRHD", PERSISTENT},
{"IsTakingSnapshot", CLEAR_ON_MANAGER_START},
{"IsUpdateAvailable", CLEAR_ON_MANAGER_START},
{"JoystickDebugMode", CLEAR_ON_MANAGER_START | CLEAR_ON_IGNITION_OFF},
{"LastAthenaPingTime", CLEAR_ON_MANAGER_START},
{"LastGPSPosition", PERSISTENT},
{"LastManagerExitReason", CLEAR_ON_MANAGER_START},
{"LastPowerDropDetected", CLEAR_ON_MANAGER_START},
{"LastSystemShutdown", CLEAR_ON_MANAGER_START},
{"LastUpdateException", CLEAR_ON_MANAGER_START},
{"LastUpdateTime", PERSISTENT},
{"LiveParameters", PERSISTENT},
{"NavDestination", CLEAR_ON_MANAGER_START | CLEAR_ON_IGNITION_OFF},
{"NavSettingTime24h", PERSISTENT},
{"NavdRender", PERSISTENT},
{"OpenpilotEnabledToggle", PERSISTENT},
{"PandaHeartbeatLost", CLEAR_ON_MANAGER_START | CLEAR_ON_IGNITION_OFF},
{"PandaSignatures", CLEAR_ON_MANAGER_START},
{"Passive", PERSISTENT},
{"PrimeRedirected", PERSISTENT},
{"PrimeType", PERSISTENT},
{"RecordFront", PERSISTENT},
{"RecordFrontLock", PERSISTENT}, // for the internal fleet
{"ReleaseNotes", PERSISTENT},
{"ShouldDoUpdate", CLEAR_ON_MANAGER_START},
{"SnoozeUpdate", CLEAR_ON_MANAGER_START | CLEAR_ON_IGNITION_OFF},
{"SshEnabled", PERSISTENT},
{"SubscriberInfo", PERSISTENT},
{"TermsVersion", PERSISTENT},
{"Timezone", PERSISTENT},
{"TrainingVersion", PERSISTENT},
{"UpdateAvailable", CLEAR_ON_MANAGER_START},
{"UpdateFailedCount", CLEAR_ON_MANAGER_START},
{"Version", PERSISTENT},
{"VisionRadarToggle", PERSISTENT},
{"ApiCache_Device", PERSISTENT},
{"ApiCache_DriveStats", PERSISTENT},
{"ApiCache_NavDestinations", PERSISTENT},
{"ApiCache_Owner", PERSISTENT},
{"Offroad_BadNvme", CLEAR_ON_MANAGER_START},
{"Offroad_CarUnrecognized", CLEAR_ON_MANAGER_START | CLEAR_ON_IGNITION_ON},
{"Offroad_ConnectivityNeeded", CLEAR_ON_MANAGER_START},
{"Offroad_ConnectivityNeededPrompt", CLEAR_ON_MANAGER_START},
{"Offroad_InvalidTime", CLEAR_ON_MANAGER_START},
{"Offroad_IsTakingSnapshot", CLEAR_ON_MANAGER_START},
{"Offroad_NeosUpdate", CLEAR_ON_MANAGER_START},
{"Offroad_NoFirmware", CLEAR_ON_MANAGER_START | CLEAR_ON_IGNITION_ON},
{"Offroad_StorageMissing", CLEAR_ON_MANAGER_START},
{"Offroad_TemperatureTooHigh", CLEAR_ON_MANAGER_START},
{"Offroad_UnofficialHardware", CLEAR_ON_MANAGER_START},
{"Offroad_UpdateFailed", CLEAR_ON_MANAGER_START},
};
} // namespace
Params::Params(const std::string &path) {
const char* env = std::getenv("OPENPILOT_PREFIX");
prefix = env ? "/" + std::string(env) : "/d";
std::string default_param_path = ensure_params_path(prefix);
params_path = path.empty() ? default_param_path : ensure_params_path(prefix, path);
}
bool Params::checkKey(const std::string &key) {
return keys.find(key) != keys.end();
}
ParamKeyType Params::getKeyType(const std::string &key) {
return static_cast<ParamKeyType>(keys[key]);
}
int Params::put(const char* key, const char* value, size_t value_size) {
// Information about safely and atomically writing a file: https://lwn.net/Articles/457667/
// 1) Create temp file
// 2) Write data to temp file
// 3) fsync() the temp file
// 4) rename the temp file to the real name
// 5) fsync() the containing directory
std::string tmp_path = params_path + "/.tmp_value_XXXXXX";
int tmp_fd = mkstemp((char*)tmp_path.c_str());
if (tmp_fd < 0) return -1;
int result = -1;
do {
// Write value to temp.
ssize_t bytes_written = HANDLE_EINTR(write(tmp_fd, value, value_size));
if (bytes_written < 0 || (size_t)bytes_written != value_size) {
result = -20;
break;
}
// fsync to force persist the changes.
if ((result = fsync(tmp_fd)) < 0) break;
FileLock file_lock(params_path + "/.lock");
// Move temp into place.
if ((result = rename(tmp_path.c_str(), getParamPath(key).c_str())) < 0) break;
// fsync parent directory
result = fsync_dir(getParamPath());
} while (false);
close(tmp_fd);
::unlink(tmp_path.c_str());
return result;
}
int Params::remove(const std::string &key) {
FileLock file_lock(params_path + "/.lock");
int result = unlink(getParamPath(key).c_str());
if (result != 0) {
return result;
}
return fsync_dir(getParamPath());
}
std::string Params::get(const std::string &key, bool block) {
if (!block) {
return util::read_file(getParamPath(key));
} else {
// blocking read until successful
params_do_exit = 0;
void (*prev_handler_sigint)(int) = std::signal(SIGINT, params_sig_handler);
void (*prev_handler_sigterm)(int) = std::signal(SIGTERM, params_sig_handler);
std::string value;
while (!params_do_exit) {
if (value = util::read_file(getParamPath(key)); !value.empty()) {
break;
}
util::sleep_for(100); // 0.1 s
}
std::signal(SIGINT, prev_handler_sigint);
std::signal(SIGTERM, prev_handler_sigterm);
return value;
}
}
std::map<std::string, std::string> Params::readAll() {
FileLock file_lock(params_path + "/.lock");
return util::read_files_in_dir(getParamPath());
}
void Params::clearAll(ParamKeyType key_type) {
FileLock file_lock(params_path + "/.lock");
std::string path;
for (auto &[key, type] : keys) {
if (type & key_type) {
unlink(getParamPath(key).c_str());
}
}
fsync_dir(getParamPath());
}
common/params.h
+47
View File
@@ -0,0 +1,47 @@
#pragma once
#include <map>
#include <string>
enum ParamKeyType {
PERSISTENT = 0x02,
CLEAR_ON_MANAGER_START = 0x04,
CLEAR_ON_IGNITION_ON = 0x08,
CLEAR_ON_IGNITION_OFF = 0x10,
DONT_LOG = 0x20,
ALL = 0xFFFFFFFF
};
class Params {
public:
Params(const std::string &path = {});
bool checkKey(const std::string &key);
ParamKeyType getKeyType(const std::string &key);
inline std::string getParamPath(const std::string &key = {}) {
return params_path + prefix + (key.empty() ? "" : "/" + key);
}
// Delete a value
int remove(const std::string &key);
void clearAll(ParamKeyType type);
// helpers for reading values
std::string get(const std::string &key, bool block = false);
inline bool getBool(const std::string &key) {
return get(key) == "1";
}
std::map<std::string, std::string> readAll();
// helpers for writing values
int put(const char *key, const char *val, size_t value_size);
inline int put(const std::string &key, const std::string &val) {
return put(key.c_str(), val.data(), val.size());
}
inline int putBool(const std::string &key, bool val) {
return put(key.c_str(), val ? "1" : "0", 1);
}
private:
std::string params_path;
std::string prefix;
};
common/params_pyx.pyx
+1 -1
View File
@@ -4,7 +4,7 @@ from libcpp cimport bool
from libcpp.string cimport string
import threading
cdef extern from "selfdrive/common/params.h":
cdef extern from "common/params.h":
cpdef enum ParamKeyType:
PERSISTENT
CLEAR_ON_MANAGER_START
common/queue.h
+52
View File
@@ -0,0 +1,52 @@
#pragma once
#include <condition_variable>
#include <mutex>
#include <queue>
template <class T>
class SafeQueue {
public:
SafeQueue() = default;
void push(const T& v) {
{
std::unique_lock lk(m);
q.push(v);
}
cv.notify_one();
}
T pop() {
std::unique_lock lk(m);
cv.wait(lk, [this] { return !q.empty(); });
T v = q.front();
q.pop();
return v;
}
bool try_pop(T& v, int timeout_ms = 0) {
std::unique_lock lk(m);
if (!cv.wait_for(lk, std::chrono::milliseconds(timeout_ms), [this] { return !q.empty(); })) {
return false;
}
v = q.front();
q.pop();
return true;
}
bool empty() const {
std::scoped_lock lk(m);
return q.empty();
}
size_t size() const {
std::scoped_lock lk(m);
return q.size();
}
private:
mutable std::mutex m;
std::condition_variable cv;
std::queue<T> q;
};
common/statlog.cc
+46
View File
@@ -0,0 +1,46 @@
#ifndef _GNU_SOURCE
#define _GNU_SOURCE
#endif
#include "common/statlog.h"
#include "common/util.h"
#include <stdio.h>
#include <mutex>
#include <zmq.h>
class StatlogState : public LogState {
public:
StatlogState() : LogState("ipc:///tmp/stats") {}
};
static StatlogState s = {};
static void log(const char* metric_type, const char* metric, const char* fmt, ...) {
std::lock_guard lk(s.lock);
if (!s.initialized) s.initialize();
char* value_buf = nullptr;
va_list args;
va_start(args, fmt);
int ret = vasprintf(&value_buf, fmt, args);
va_end(args);
if (ret > 0 && value_buf) {
char* line_buf = nullptr;
ret = asprintf(&line_buf, "%s:%s|%s", metric, value_buf, metric_type);
if (ret > 0 && line_buf) {
zmq_send(s.sock, line_buf, ret, ZMQ_NOBLOCK);
free(line_buf);
}
free(value_buf);
}
}
void statlog_log(const char* metric_type, const char* metric, int value) {
log(metric_type, metric, "%d", value);
}
void statlog_log(const char* metric_type, const char* metric, float value) {
log(metric_type, metric, "%f", value);
}
common/statlog.h
+10
View File
@@ -0,0 +1,10 @@
#pragma once
#define STATLOG_GAUGE "g"
#define STATLOG_SAMPLE "sa"
void statlog_log(const char* metric_type, const char* metric, int value);
void statlog_log(const char* metric_type, const char* metric, float value);
#define statlog_gauge(metric, value) statlog_log(STATLOG_GAUGE, metric, value)
#define statlog_sample(metric, value) statlog_log(STATLOG_SAMPLE, metric, value)
common/swaglog.cc
+140
View File
@@ -0,0 +1,140 @@
#ifndef _GNU_SOURCE
#define _GNU_SOURCE
#endif
#include "common/swaglog.h"
#include <cassert>
#include <cstring>
#include <limits>
#include <mutex>
#include <string>
#include <zmq.h>
#include "json11.hpp"
#include "common/util.h"
#include "common/version.h"
#include "selfdrive/hardware/hw.h"
class SwaglogState : public LogState {
public:
SwaglogState() : LogState("ipc:///tmp/logmessage") {}
json11::Json::object ctx_j;
inline void initialize() {
ctx_j = json11::Json::object {};
print_level = CLOUDLOG_WARNING;
const char* print_lvl = getenv("LOGPRINT");
if (print_lvl) {
if (strcmp(print_lvl, "debug") == 0) {
print_level = CLOUDLOG_DEBUG;
} else if (strcmp(print_lvl, "info") == 0) {
print_level = CLOUDLOG_INFO;
} else if (strcmp(print_lvl, "warning") == 0) {
print_level = CLOUDLOG_WARNING;
}
}
// openpilot bindings
char* dongle_id = getenv("DONGLE_ID");
if (dongle_id) {
ctx_j["dongle_id"] = dongle_id;
}
char* daemon_name = getenv("MANAGER_DAEMON");
if (daemon_name) {
ctx_j["daemon"] = daemon_name;
}
ctx_j["version"] = COMMA_VERSION;
ctx_j["dirty"] = !getenv("CLEAN");
// device type
if (Hardware::TICI()) {
ctx_j["device"] = "tici";
} else {
ctx_j["device"] = "pc";
}
LogState::initialize();
}
};
static SwaglogState s = {};
bool LOG_TIMESTAMPS = getenv("LOG_TIMESTAMPS");
uint32_t NO_FRAME_ID = std::numeric_limits<uint32_t>::max();
static void log(int levelnum, const char* filename, int lineno, const char* func, const char* msg, const std::string& log_s) {
if (levelnum >= s.print_level) {
printf("%s: %s\n", filename, msg);
}
char levelnum_c = levelnum;
zmq_send(s.sock, (levelnum_c + log_s).c_str(), log_s.length() + 1, ZMQ_NOBLOCK);
}
static void cloudlog_common(int levelnum, const char* filename, int lineno, const char* func,
char* msg_buf, json11::Json::object msg_j={}) {
std::lock_guard lk(s.lock);
if (!s.initialized) s.initialize();
json11::Json::object log_j = json11::Json::object {
{"ctx", s.ctx_j},
{"levelnum", levelnum},
{"filename", filename},
{"lineno", lineno},
{"funcname", func},
{"created", seconds_since_epoch()}
};
if (msg_j.empty()) {
log_j["msg"] = msg_buf;
} else {
log_j["msg"] = msg_j;
}
std::string log_s = ((json11::Json)log_j).dump();
log(levelnum, filename, lineno, func, msg_buf, log_s);
free(msg_buf);
}
void cloudlog_e(int levelnum, const char* filename, int lineno, const char* func,
const char* fmt, ...) {
va_list args;
va_start(args, fmt);
char* msg_buf = nullptr;
int ret = vasprintf(&msg_buf, fmt, args);
va_end(args);
if (ret <= 0 || !msg_buf) return;
cloudlog_common(levelnum, filename, lineno, func, msg_buf);
}
void cloudlog_t_common(int levelnum, const char* filename, int lineno, const char* func,
uint32_t frame_id, const char* fmt, va_list args) {
if (!LOG_TIMESTAMPS) return;
char* msg_buf = nullptr;
int ret = vasprintf(&msg_buf, fmt, args);
if (ret <= 0 || !msg_buf) return;
json11::Json::object tspt_j = json11::Json::object{
{"event", msg_buf},
{"time", std::to_string(nanos_since_boot())}
};
if (frame_id < NO_FRAME_ID) {
tspt_j["frame_id"] = std::to_string(frame_id);
}
tspt_j = json11::Json::object{{"timestamp", tspt_j}};
cloudlog_common(levelnum, filename, lineno, func, msg_buf, tspt_j);
}
void cloudlog_te(int levelnum, const char* filename, int lineno, const char* func,
const char* fmt, ...) {
va_list args;
va_start(args, fmt);
cloudlog_t_common(levelnum, filename, lineno, func, NO_FRAME_ID, fmt, args);
va_end(args);
}
void cloudlog_te(int levelnum, const char* filename, int lineno, const char* func,
uint32_t frame_id, const char* fmt, ...) {
va_list args;
va_start(args, fmt);
cloudlog_t_common(levelnum, filename, lineno, func, frame_id, fmt, args);
va_end(args);
}
common/swaglog.h
+70
View File
@@ -0,0 +1,70 @@
#pragma once
#include "common/timing.h"
#define CLOUDLOG_DEBUG 10
#define CLOUDLOG_INFO 20
#define CLOUDLOG_WARNING 30
#define CLOUDLOG_ERROR 40
#define CLOUDLOG_CRITICAL 50
void cloudlog_e(int levelnum, const char* filename, int lineno, const char* func,
const char* fmt, ...) /*__attribute__ ((format (printf, 6, 7)))*/;
void cloudlog_te(int levelnum, const char* filename, int lineno, const char* func,
const char* fmt, ...) /*__attribute__ ((format (printf, 6, 7)))*/;
void cloudlog_te(int levelnum, const char* filename, int lineno, const char* func,
uint32_t frame_id, const char* fmt, ...) /*__attribute__ ((format (printf, 6, 7)))*/;
#define cloudlog(lvl, fmt, ...) cloudlog_e(lvl, __FILE__, __LINE__, \
__func__, \
fmt, ## __VA_ARGS__);
#define cloudlog_t(lvl, ...) cloudlog_te(lvl, __FILE__, __LINE__, \
__func__, \
__VA_ARGS__);
#define cloudlog_rl(burst, millis, lvl, fmt, ...) \
{ \
static uint64_t __begin = 0; \
static int __printed = 0; \
static int __missed = 0; \
\
int __burst = (burst); \
int __millis = (millis); \
uint64_t __ts = nanos_since_boot(); \
\
if (!__begin) __begin = __ts; \
\
if (__begin + __millis*1000000ULL < __ts) { \
if (__missed) { \
cloudlog(CLOUDLOG_WARNING, "cloudlog: %d messages suppressed", __missed); \
} \
__begin = 0; \
__printed = 0; \
__missed = 0; \
} \
\
if (__printed < __burst) { \
cloudlog(lvl, fmt, ## __VA_ARGS__); \
__printed++; \
} else { \
__missed++; \
} \
}
#define LOGT(...) cloudlog_t(CLOUDLOG_DEBUG, __VA_ARGS__)
#define LOGD(fmt, ...) cloudlog(CLOUDLOG_DEBUG, fmt, ## __VA_ARGS__)
#define LOG(fmt, ...) cloudlog(CLOUDLOG_INFO, fmt, ## __VA_ARGS__)
#define LOGW(fmt, ...) cloudlog(CLOUDLOG_WARNING, fmt, ## __VA_ARGS__)
#define LOGE(fmt, ...) cloudlog(CLOUDLOG_ERROR, fmt, ## __VA_ARGS__)
#define LOGD_100(fmt, ...) cloudlog_rl(2, 100, CLOUDLOG_DEBUG, fmt, ## __VA_ARGS__)
#define LOG_100(fmt, ...) cloudlog_rl(2, 100, CLOUDLOG_INFO, fmt, ## __VA_ARGS__)
#define LOGW_100(fmt, ...) cloudlog_rl(2, 100, CLOUDLOG_WARNING, fmt, ## __VA_ARGS__)
#define LOGE_100(fmt, ...) cloudlog_rl(2, 100, CLOUDLOG_ERROR, fmt, ## __VA_ARGS__)
common/tests/.gitignore
+2
View File
@@ -0,0 +1,2 @@
test_util
test_swaglog
common/tests/test_swaglog.cc
+92
View File
@@ -0,0 +1,92 @@
#include <zmq.h>
#include <iostream>
#define CATCH_CONFIG_MAIN
#include "catch2/catch.hpp"
#include "json11.hpp"
#include "common/swaglog.h"
#include "common/util.h"
#include "common/version.h"
#include "selfdrive/hardware/hw.h"
const char *SWAGLOG_ADDR = "ipc:///tmp/logmessage";
std::string daemon_name = "testy";
std::string dongle_id = "test_dongle_id";
int LINE_NO = 0;
void log_thread(int thread_id, int msg_cnt) {
for (int i = 0; i < msg_cnt; ++i) {
LOGD("%d", thread_id);
LINE_NO = __LINE__ - 1;
usleep(1);
}
}
void recv_log(int thread_cnt, int thread_msg_cnt) {
void *zctx = zmq_ctx_new();
void *sock = zmq_socket(zctx, ZMQ_PULL);
zmq_bind(sock, SWAGLOG_ADDR);
std::vector<int> thread_msgs(thread_cnt);
int total_count = 0;
for (auto start = std::chrono::steady_clock::now(), now = start;
now < start + std::chrono::seconds{1} && total_count < (thread_cnt * thread_msg_cnt);
now = std::chrono::steady_clock::now()) {
char buf[4096] = {};
if (zmq_recv(sock, buf, sizeof(buf), ZMQ_DONTWAIT) <= 0) {
if (errno == EAGAIN || errno == EINTR || errno == EFSM) continue;
break;
}
REQUIRE(buf[0] == CLOUDLOG_DEBUG);
std::string err;
auto msg = json11::Json::parse(buf + 1, err);
REQUIRE(!msg.is_null());
REQUIRE(msg["levelnum"].int_value() == CLOUDLOG_DEBUG);
REQUIRE_THAT(msg["filename"].string_value(), Catch::Contains("test_swaglog.cc"));
REQUIRE(msg["funcname"].string_value() == "log_thread");
REQUIRE(msg["lineno"].int_value() == LINE_NO);
auto ctx = msg["ctx"];
REQUIRE(ctx["daemon"].string_value() == daemon_name);
REQUIRE(ctx["dongle_id"].string_value() == dongle_id);
REQUIRE(ctx["dirty"].bool_value() == true);
REQUIRE(ctx["version"].string_value() == COMMA_VERSION);
std::string device = "pc";
if (Hardware::TICI()) {
device = "tici";
}
REQUIRE(ctx["device"].string_value() == device);
int thread_id = atoi(msg["msg"].string_value().c_str());
REQUIRE((thread_id >= 0 && thread_id < thread_cnt));
thread_msgs[thread_id]++;
total_count++;
}
for (int i = 0; i < thread_cnt; ++i) {
INFO("thread :" << i);
REQUIRE(thread_msgs[i] == thread_msg_cnt);
}
zmq_close(sock);
zmq_ctx_destroy(zctx);
}
TEST_CASE("swaglog") {
setenv("MANAGER_DAEMON", daemon_name.c_str(), 1);
setenv("DONGLE_ID", dongle_id.c_str(), 1);
setenv("dirty", "1", 1);
const int thread_cnt = 5;
const int thread_msg_cnt = 100;
std::vector<std::thread> log_threads;
for (int i = 0; i < thread_cnt; ++i) {
log_threads.push_back(std::thread(log_thread, i, thread_msg_cnt));
}
for (auto &t : log_threads) t.join();
recv_log(thread_cnt, thread_msg_cnt);
}
common/tests/test_util.cc
+145
View File
@@ -0,0 +1,145 @@
#include <dirent.h>
#include <sys/stat.h>
#include <sys/types.h>
#include <algorithm>
#include <climits>
#include <random>
#include <string>
#define CATCH_CONFIG_MAIN
#include "catch2/catch.hpp"
#include "common/util.h"
std::string random_bytes(int size) {
std::random_device rd;
std::independent_bits_engine<std::default_random_engine, CHAR_BIT, unsigned char> rbe(rd());
std::string bytes(size + 1, '\0');
std::generate(bytes.begin(), bytes.end(), std::ref(rbe));
return bytes;
}
TEST_CASE("util::read_file") {
SECTION("read /proc/version") {
std::string ret = util::read_file("/proc/version");
REQUIRE(ret.find("Linux version") != std::string::npos);
}
SECTION("read from sysfs") {
std::string ret = util::read_file("/sys/power/wakeup_count");
REQUIRE(!ret.empty());
}
SECTION("read file") {
char filename[] = "/tmp/test_read_XXXXXX";
int fd = mkstemp(filename);
REQUIRE(util::read_file(filename).empty());
std::string content = random_bytes(64 * 1024);
write(fd, content.c_str(), content.size());
std::string ret = util::read_file(filename);
REQUIRE(ret == content);
close(fd);
}
SECTION("read directory") {
REQUIRE(util::read_file(".").empty());
}
SECTION("read non-existent file") {
std::string ret = util::read_file("does_not_exist");
REQUIRE(ret.empty());
}
SECTION("read non-permission") {
REQUIRE(util::read_file("/proc/kmsg").empty());
}
}
TEST_CASE("util::file_exists") {
char filename[] = "/tmp/test_file_exists_XXXXXX";
int fd = mkstemp(filename);
REQUIRE(fd != -1);
close(fd);
SECTION("existent file") {
REQUIRE(util::file_exists(filename));
REQUIRE(util::file_exists("/tmp"));
}
SECTION("nonexistent file") {
std::string fn = filename;
REQUIRE(!util::file_exists(fn + "/nonexistent"));
}
SECTION("file has no access permissions") {
std::string fn = "/proc/kmsg";
std::ifstream f(fn);
REQUIRE(f.good() == false);
REQUIRE(util::file_exists(fn));
}
::remove(filename);
}
TEST_CASE("util::read_files_in_dir") {
char tmp_path[] = "/tmp/test_XXXXXX";
const std::string test_path = mkdtemp(tmp_path);
const std::string files[] = {".test1", "'test2'", "test3"};
for (auto fn : files) {
std::ofstream{test_path + "/" + fn} << fn;
}
mkdir((test_path + "/dir").c_str(), 0777);
std::map<std::string, std::string> result = util::read_files_in_dir(test_path);
REQUIRE(result.find("dir") == result.end());
REQUIRE(result.size() == std::size(files));
for (auto& [k, v] : result) {
REQUIRE(k == v);
}
}
TEST_CASE("util::safe_fwrite") {
char filename[] = "/tmp/XXXXXX";
int fd = mkstemp(filename);
close(fd);
std::string dat = random_bytes(1024 * 1024);
FILE *f = util::safe_fopen(filename, "wb");
REQUIRE(f != nullptr);
size_t size = util::safe_fwrite(dat.data(), 1, dat.size(), f);
REQUIRE(size == dat.size());
int ret = util::safe_fflush(f);
REQUIRE(ret == 0);
ret = fclose(f);
REQUIRE(ret == 0);
REQUIRE(dat == util::read_file(filename));
}
TEST_CASE("util::create_directories") {
system("rm /tmp/test_create_directories -rf");
std::string dir = "/tmp/test_create_directories/a/b/c/d/e/f";
auto check_dir_permissions = [](const std::string &dir, mode_t mode) -> bool {
struct stat st = {};
return stat(dir.c_str(), &st) == 0 && (st.st_mode & S_IFMT) == S_IFDIR && (st.st_mode & (S_IRWXU | S_IRWXG | S_IRWXO)) == mode;
};
SECTION("create_directories") {
REQUIRE(util::create_directories(dir, 0755));
REQUIRE(check_dir_permissions(dir, 0755));
}
SECTION("dir already exists") {
REQUIRE(util::create_directories(dir, 0755));
REQUIRE(util::create_directories(dir, 0755));
}
SECTION("a file exists with the same name") {
REQUIRE(util::create_directories(dir, 0755));
int f = open((dir + "/file").c_str(), O_RDWR | O_CREAT);
REQUIRE(f != -1);
close(f);
REQUIRE(util::create_directories(dir + "/file", 0755) == false);
REQUIRE(util::create_directories(dir + "/file/1/2/3", 0755) == false);
}
SECTION("end with slashs") {
REQUIRE(util::create_directories(dir + "/", 0755));
}
SECTION("empty") {
REQUIRE(util::create_directories("", 0755) == false);
}
}
common/timing.h
+51
View File
@@ -0,0 +1,51 @@
#pragma once
#include <cstdint>
#include <ctime>
#ifdef __APPLE__
#define CLOCK_BOOTTIME CLOCK_MONOTONIC
#endif
static inline uint64_t nanos_since_boot() {
struct timespec t;
clock_gettime(CLOCK_BOOTTIME, &t);
return t.tv_sec * 1000000000ULL + t.tv_nsec;
}
static inline double millis_since_boot() {
struct timespec t;
clock_gettime(CLOCK_BOOTTIME, &t);
return t.tv_sec * 1000.0 + t.tv_nsec * 1e-6;
}
static inline double seconds_since_boot() {
struct timespec t;
clock_gettime(CLOCK_BOOTTIME, &t);
return (double)t.tv_sec + t.tv_nsec * 1e-9;
}
static inline uint64_t nanos_since_epoch() {
struct timespec t;
clock_gettime(CLOCK_REALTIME, &t);
return t.tv_sec * 1000000000ULL + t.tv_nsec;
}
static inline double seconds_since_epoch() {
struct timespec t;
clock_gettime(CLOCK_REALTIME, &t);
return (double)t.tv_sec + t.tv_nsec * 1e-9;
}
// you probably should use nanos_since_boot instead
static inline uint64_t nanos_monotonic() {
struct timespec t;
clock_gettime(CLOCK_MONOTONIC, &t);
return t.tv_sec * 1000000000ULL + t.tv_nsec;
}
static inline uint64_t nanos_monotonic_raw() {
struct timespec t;
clock_gettime(CLOCK_MONOTONIC_RAW, &t);
return t.tv_sec * 1000000000ULL + t.tv_nsec;
}
common/util.cc
+244
View File
@@ -0,0 +1,244 @@
#include "common/util.h"
#include <sys/stat.h>
#include <dirent.h>
#include <cassert>
#include <cerrno>
#include <cstring>
#include <dirent.h>
#include <fstream>
#include <iomanip>
#include <sstream>
#ifdef __linux__
#include <sys/prctl.h>
#include <sys/syscall.h>
#ifndef __USE_GNU
#define __USE_GNU
#endif
#include <sched.h>
#endif // __linux__
namespace util {
void set_thread_name(const char* name) {
#ifdef __linux__
// pthread_setname_np is dumb (fails instead of truncates)
prctl(PR_SET_NAME, (unsigned long)name, 0, 0, 0);
#endif
}
int set_realtime_priority(int level) {
#ifdef __linux__
long tid = syscall(SYS_gettid);
// should match python using chrt
struct sched_param sa;
memset(&sa, 0, sizeof(sa));
sa.sched_priority = level;
return sched_setscheduler(tid, SCHED_FIFO, &sa);
#else
return -1;
#endif
}
int set_core_affinity(std::vector<int> cores) {
#ifdef __linux__
long tid = syscall(SYS_gettid);
cpu_set_t cpu;
CPU_ZERO(&cpu);
for (const int n : cores) {
CPU_SET(n, &cpu);
}
return sched_setaffinity(tid, sizeof(cpu), &cpu);
#else
return -1;
#endif
}
std::string read_file(const std::string& fn) {
std::ifstream f(fn, std::ios::binary | std::ios::in);
if (f.is_open()) {
f.seekg(0, std::ios::end);
int size = f.tellg();
if (f.good() && size > 0) {
std::string result(size, '\0');
f.seekg(0, std::ios::beg);
f.read(result.data(), size);
// return either good() or has reached end-of-file (e.g. /sys/power/wakeup_count)
if (f.good() || f.eof()) {
result.resize(f.gcount());
return result;
}
}
// fallback for files created on read, e.g. procfs
std::stringstream buffer;
buffer << f.rdbuf();
return buffer.str();
}
return std::string();
}
std::map<std::string, std::string> read_files_in_dir(const std::string &path) {
std::map<std::string, std::string> ret;
DIR *d = opendir(path.c_str());
if (!d) return ret;
struct dirent *de = NULL;
while ((de = readdir(d))) {
if (de->d_type != DT_DIR) {
ret[de->d_name] = util::read_file(path + "/" + de->d_name);
}
}
closedir(d);
return ret;
}
int write_file(const char* path, const void* data, size_t size, int flags, mode_t mode) {
int fd = HANDLE_EINTR(open(path, flags, mode));
if (fd == -1) {
return -1;
}
ssize_t n = HANDLE_EINTR(write(fd, data, size));
close(fd);
return (n >= 0 && (size_t)n == size) ? 0 : -1;
}
FILE* safe_fopen(const char* filename, const char* mode) {
FILE* fp = NULL;
do {
fp = fopen(filename, mode);
} while ((nullptr == fp) && (errno == EINTR));
return fp;
}
size_t safe_fwrite(const void* ptr, size_t size, size_t count, FILE* stream) {
size_t written = 0;
do {
size_t ret = ::fwrite((void*)((char *)ptr + written * size), size, count - written, stream);
if (ret == 0 && errno != EINTR) break;
written += ret;
} while (written != count);
return written;
}
int safe_fflush(FILE *stream) {
int ret = EOF;
do {
ret = fflush(stream);
} while ((EOF == ret) && (errno == EINTR));
return ret;
}
std::string readlink(const std::string &path) {
char buff[4096];
ssize_t len = ::readlink(path.c_str(), buff, sizeof(buff)-1);
if (len != -1) {
buff[len] = '\0';
return std::string(buff);
}
return "";
}
bool file_exists(const std::string& fn) {
struct stat st = {};
return stat(fn.c_str(), &st) != -1;
}
static bool createDirectory(std::string dir, mode_t mode) {
auto verify_dir = [](const std::string& dir) -> bool {
struct stat st = {};
return (stat(dir.c_str(), &st) == 0 && (st.st_mode & S_IFMT) == S_IFDIR);
};
// remove trailing /'s
while (dir.size() > 1 && dir.back() == '/') {
dir.pop_back();
}
// try to mkdir this directory
if (mkdir(dir.c_str(), mode) == 0) return true;
if (errno == EEXIST) return verify_dir(dir);
if (errno != ENOENT) return false;
// mkdir failed because the parent dir doesn't exist, so try to create it
size_t slash = dir.rfind('/');
if ((slash == std::string::npos || slash < 1) ||
!createDirectory(dir.substr(0, slash), mode)) {
return false;
}
// try again
if (mkdir(dir.c_str(), mode) == 0) return true;
return errno == EEXIST && verify_dir(dir);
}
bool create_directories(const std::string& dir, mode_t mode) {
if (dir.empty()) return false;
return createDirectory(dir, mode);
}
std::string getenv(const char* key, const char* default_val) {
const char* val = ::getenv(key);
return val ? val : default_val;
}
int getenv(const char* key, int default_val) {
const char* val = ::getenv(key);
return val ? atoi(val) : default_val;
}
float getenv(const char* key, float default_val) {
const char* val = ::getenv(key);
return val ? atof(val) : default_val;
}
std::string hexdump(const uint8_t* in, const size_t size) {
std::stringstream ss;
ss << std::hex << std::setfill('0');
for (size_t i = 0; i < size; i++) {
ss << std::setw(2) << static_cast<unsigned int>(in[i]);
}
return ss.str();
}
std::string dir_name(std::string const &path) {
size_t pos = path.find_last_of("/");
if (pos == std::string::npos) return "";
return path.substr(0, pos);
}
std::string check_output(const std::string& command) {
char buffer[128];
std::string result;
std::unique_ptr<FILE, decltype(&pclose)> pipe(popen(command.c_str(), "r"), pclose);
if (!pipe) {
return "";
}
while (fgets(buffer, std::size(buffer), pipe.get()) != nullptr) {
result += std::string(buffer);
}
return result;
}
struct tm get_time() {
time_t rawtime;
time(&rawtime);
struct tm sys_time;
gmtime_r(&rawtime, &sys_time);
return sys_time;
}
bool time_valid(struct tm sys_time) {
int year = 1900 + sys_time.tm_year;
int month = 1 + sys_time.tm_mon;
return (year > 2021) || (year == 2021 && month >= 6);
}
} // namespace util
common/util.h
+200
View File
@@ -0,0 +1,200 @@
#pragma once
#include <fcntl.h>
#include <sys/stat.h>
#include <unistd.h>
#include <zmq.h>
#include <algorithm>
#include <atomic>
#include <chrono>
#include <csignal>
#include <ctime>
#include <map>
#include <memory>
#include <mutex>
#include <string>
#include <thread>
#include <vector>
// keep trying if x gets interrupted by a signal
#define HANDLE_EINTR(x) \
({ \
decltype(x) ret_; \
int try_cnt = 0; \
do { \
ret_ = (x); \
} while (ret_ == -1 && errno == EINTR && try_cnt++ < 100); \
ret_; \
})
#ifndef sighandler_t
typedef void (*sighandler_t)(int sig);
#endif
const double MILE_TO_KM = 1.609344;
const double KM_TO_MILE = 1. / MILE_TO_KM;
const double MS_TO_KPH = 3.6;
const double MS_TO_MPH = MS_TO_KPH * KM_TO_MILE;
const double METER_TO_MILE = KM_TO_MILE / 1000.0;
const double METER_TO_FOOT = 3.28084;
namespace util {
void set_thread_name(const char* name);
int set_realtime_priority(int level);
int set_core_affinity(std::vector<int> cores);
// ***** Time helpers *****
struct tm get_time();
bool time_valid(struct tm sys_time);
// ***** math helpers *****
// map x from [a1, a2] to [b1, b2]
template <typename T>
T map_val(T x, T a1, T a2, T b1, T b2) {
x = std::clamp(x, a1, a2);
T ra = a2 - a1;
T rb = b2 - b1;
return (x - a1) * rb / ra + b1;
}
// ***** string helpers *****
template <typename... Args>
std::string string_format(const std::string& format, Args... args) {
size_t size = snprintf(nullptr, 0, format.c_str(), args...) + 1;
std::unique_ptr<char[]> buf(new char[size]);
snprintf(buf.get(), size, format.c_str(), args...);
return std::string(buf.get(), buf.get() + size - 1);
}
std::string getenv(const char* key, const char* default_val = "");
int getenv(const char* key, int default_val);
float getenv(const char* key, float default_val);
std::string hexdump(const uint8_t* in, const size_t size);
std::string dir_name(std::string const& path);
// **** file fhelpers *****
std::string read_file(const std::string& fn);
std::map<std::string, std::string> read_files_in_dir(const std::string& path);
int write_file(const char* path, const void* data, size_t size, int flags = O_WRONLY, mode_t mode = 0664);
FILE* safe_fopen(const char* filename, const char* mode);
size_t safe_fwrite(const void * ptr, size_t size, size_t count, FILE * stream);
int safe_fflush(FILE *stream);
std::string readlink(const std::string& path);
bool file_exists(const std::string& fn);
bool create_directories(const std::string &dir, mode_t mode);
std::string check_output(const std::string& command);
inline void sleep_for(const int milliseconds) {
if (milliseconds > 0) {
std::this_thread::sleep_for(std::chrono::milliseconds(milliseconds));
}
}
} // namespace util
class ExitHandler {
public:
ExitHandler() {
std::signal(SIGINT, (sighandler_t)set_do_exit);
std::signal(SIGTERM, (sighandler_t)set_do_exit);
#ifndef __APPLE__
std::signal(SIGPWR, (sighandler_t)set_do_exit);
#endif
};
inline static std::atomic<bool> power_failure = false;
inline static std::atomic<int> signal = 0;
inline operator bool() { return do_exit; }
inline ExitHandler& operator=(bool v) {
signal = 0;
do_exit = v;
return *this;
}
private:
static void set_do_exit(int sig) {
#ifndef __APPLE__
power_failure = (sig == SIGPWR);
#endif
signal = sig;
do_exit = true;
}
inline static std::atomic<bool> do_exit = false;
};
struct unique_fd {
unique_fd(int fd = -1) : fd_(fd) {}
unique_fd& operator=(unique_fd&& uf) {
fd_ = uf.fd_;
uf.fd_ = -1;
return *this;
}
~unique_fd() {
if (fd_ != -1) close(fd_);
}
operator int() const { return fd_; }
int fd_;
};
class FirstOrderFilter {
public:
FirstOrderFilter(float x0, float ts, float dt) {
k_ = (dt / ts) / (1.0 + dt / ts);
x_ = x0;
}
inline float update(float x) {
x_ = (1. - k_) * x_ + k_ * x;
return x_;
}
inline void reset(float x) { x_ = x; }
inline float x(){ return x_; }
private:
float x_, k_;
};
template<typename T>
void update_max_atomic(std::atomic<T>& max, T const& value) {
T prev = max;
while(prev < value && !max.compare_exchange_weak(prev, value)) {}
}
class LogState {
public:
bool initialized = false;
std::mutex lock;
void *zctx = nullptr;
void *sock = nullptr;
int print_level;
const char* endpoint;
LogState(const char* _endpoint) {
endpoint = _endpoint;
}
inline void initialize() {
zctx = zmq_ctx_new();
sock = zmq_socket(zctx, ZMQ_PUSH);
// Timeout on shutdown for messages to be received by the logging process
int timeout = 100;
zmq_setsockopt(sock, ZMQ_LINGER, &timeout, sizeof(timeout));
zmq_connect(sock, endpoint);
initialized = true;
}
~LogState() {
if (initialized) {
zmq_close(sock);
zmq_ctx_destroy(zctx);
}
}
};
common/version.h
+1
View File
@@ -0,0 +1 @@
#define COMMA_VERSION "0.8.14"
common/visionimg.cc
+14
View File
@@ -0,0 +1,14 @@
#include "common/visionimg.h"
#include <cassert>
EGLImageTexture::EGLImageTexture(const VisionBuf *buf) {
glGenTextures(1, &frame_tex);
glBindTexture(GL_TEXTURE_2D, frame_tex);
glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB, buf->width, buf->height, 0, GL_RGB, GL_UNSIGNED_BYTE, nullptr);
glGenerateMipmap(GL_TEXTURE_2D);
}
EGLImageTexture::~EGLImageTexture() {
glDeleteTextures(1, &frame_tex);
}
common/visionimg.h
+16
View File
@@ -0,0 +1,16 @@
#pragma once
#include "cereal/visionipc/visionbuf.h"
#ifdef __APPLE__
#include <OpenGL/gl3.h>
#else
#include <GLES3/gl3.h>
#endif
class EGLImageTexture {
public:
EGLImageTexture(const VisionBuf *buf);
~EGLImageTexture();
GLuint frame_tex = 0;
};
common/watchdog.cc
+12
View File
@@ -0,0 +1,12 @@
#include "common/watchdog.h"
#include "common/timing.h"
#include "common/util.h"
const std::string watchdog_fn_prefix = "/dev/shm/wd_"; // + <pid>
bool watchdog_kick() {
static std::string fn = watchdog_fn_prefix + std::to_string(getpid());
uint64_t ts = nanos_since_boot();
return util::write_file(fn.c_str(), &ts, sizeof(ts), O_WRONLY | O_CREAT) > 0;
}
common/watchdog.h
+3
View File
@@ -0,0 +1,3 @@
#pragma once
bool watchdog_kick();