Compile FrogPilot
This commit is contained in:
FrogAi
@@ -1,39 +0,0 @@
|
||||
Import('env', 'envCython', 'arch')
|
||||
|
||||
common_libs = [
|
||||
'params.cc',
|
||||
'swaglog.cc',
|
||||
'util.cc',
|
||||
'i2c.cc',
|
||||
'watchdog.cc',
|
||||
'ratekeeper.cc'
|
||||
]
|
||||
|
||||
if arch != "Darwin":
|
||||
common_libs.append('gpio.cc')
|
||||
|
||||
_common = env.Library('common', common_libs, LIBS="json11")
|
||||
|
||||
files = [
|
||||
'clutil.cc',
|
||||
]
|
||||
|
||||
_gpucommon = env.Library('gpucommon', files)
|
||||
Export('_common', '_gpucommon')
|
||||
|
||||
if GetOption('extras'):
|
||||
env.Program('tests/test_common',
|
||||
['tests/test_runner.cc', 'tests/test_params.cc', 'tests/test_util.cc', 'tests/test_swaglog.cc'],
|
||||
LIBS=[_common, 'json11', 'zmq', 'pthread'])
|
||||
|
||||
# Cython bindings
|
||||
params_python = envCython.Program('params_pyx.so', 'params_pyx.pyx', LIBS=envCython['LIBS'] + [_common, 'zmq', 'json11'])
|
||||
|
||||
SConscript([
|
||||
'transformations/SConscript',
|
||||
])
|
||||
|
||||
Import('transformations_python')
|
||||
common_python = [params_python, transformations_python]
|
||||
|
||||
Export('common_python')
|
||||
200
common/clutil.cc
200
common/clutil.cc
@@ -1,200 +0,0 @@
|
||||
#include "common/clutil.h"
|
||||
|
||||
#include <cassert>
|
||||
#include <iostream>
|
||||
#include <memory>
|
||||
|
||||
#include "common/util.h"
|
||||
#include "common/swaglog.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;
|
||||
}
|
||||
|
||||
LOGD("vendor: %s", get_platform_info(platform, CL_PLATFORM_VENDOR).c_str());
|
||||
LOGD("platform version: %s", get_platform_info(platform, CL_PLATFORM_VERSION).c_str());
|
||||
LOGD("profile: %s", get_platform_info(platform, CL_PLATFORM_PROFILE).c_str());
|
||||
LOGD("extensions: %s", get_platform_info(platform, CL_PLATFORM_EXTENSIONS).c_str());
|
||||
LOGD("name: %s", get_device_info(device, CL_DEVICE_NAME).c_str());
|
||||
LOGD("device version: %s", get_device_info(device, CL_DEVICE_VERSION).c_str());
|
||||
LOGD("max work group size: %zu", work_group_size);
|
||||
LOGD("type = %d, %s", (int)device_type, type_str);
|
||||
}
|
||||
|
||||
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);
|
||||
|
||||
LOGE("build failed; status=%d, log: %s", status, log.c_str());
|
||||
}
|
||||
|
||||
} // 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) {
|
||||
LOGD("platform[%zu] CL_PLATFORM_NAME: %s", i, get_platform_info(platform_ids[i], CL_PLATFORM_NAME).c_str());
|
||||
|
||||
// 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;
|
||||
}
|
||||
}
|
||||
LOGE("No valid openCL platform found");
|
||||
assert(0);
|
||||
return nullptr;
|
||||
}
|
||||
|
||||
cl_context cl_create_context(cl_device_id device_id) {
|
||||
return CL_CHECK_ERR(clCreateContext(NULL, 1, &device_id, NULL, NULL, &err));
|
||||
}
|
||||
|
||||
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) {
|
||||
const char *csrc = src.c_str();
|
||||
cl_program prg = CL_CHECK_ERR(clCreateProgramWithSource(ctx, 1, &csrc, 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, &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";
|
||||
}
|
||||
}
|
||||
@@ -1,29 +0,0 @@
|
||||
#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_context cl_create_context(cl_device_id device_id);
|
||||
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);
|
||||
@@ -1,84 +0,0 @@
|
||||
#include "common/gpio.h"
|
||||
|
||||
#include <string>
|
||||
|
||||
#ifdef __APPLE__
|
||||
int gpio_init(int pin_nr, bool output) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
int gpio_set(int pin_nr, bool high) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
int gpiochip_get_ro_value_fd(const char* consumer_label, int gpiochiop_id, int pin_nr) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
#else
|
||||
|
||||
#include <fcntl.h>
|
||||
#include <unistd.h>
|
||||
|
||||
#include <cstring>
|
||||
#include <linux/gpio.h>
|
||||
#include <sys/ioctl.h>
|
||||
|
||||
#include "common/util.h"
|
||||
#include "common/swaglog.h"
|
||||
|
||||
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);
|
||||
}
|
||||
|
||||
int gpiochip_get_ro_value_fd(const char* consumer_label, int gpiochiop_id, int pin_nr) {
|
||||
|
||||
// Assumed that all interrupt pins are unexported and rights are given to
|
||||
// read from gpiochip0.
|
||||
std::string gpiochip_path = "/dev/gpiochip" + std::to_string(gpiochiop_id);
|
||||
int fd = open(gpiochip_path.c_str(), O_RDONLY);
|
||||
if (fd < 0) {
|
||||
LOGE("Error opening gpiochip0 fd");
|
||||
return -1;
|
||||
}
|
||||
|
||||
// Setup event
|
||||
struct gpioevent_request rq;
|
||||
rq.lineoffset = pin_nr;
|
||||
rq.handleflags = GPIOHANDLE_REQUEST_INPUT;
|
||||
|
||||
/* Requesting both edges as the data ready pulse from the lsm6ds sensor is
|
||||
very short(75us) and is mostly detected as falling edge instead of rising.
|
||||
So if it is detected as rising the following falling edge is skipped. */
|
||||
rq.eventflags = GPIOEVENT_REQUEST_BOTH_EDGES;
|
||||
|
||||
strncpy(rq.consumer_label, consumer_label, std::size(rq.consumer_label) - 1);
|
||||
int ret = util::safe_ioctl(fd, GPIO_GET_LINEEVENT_IOCTL, &rq);
|
||||
if (ret == -1) {
|
||||
LOGE("Unable to get line event from ioctl : %s", strerror(errno));
|
||||
close(fd);
|
||||
return -1;
|
||||
}
|
||||
|
||||
close(fd);
|
||||
return rq.fd;
|
||||
}
|
||||
|
||||
#endif
|
||||
@@ -1,33 +0,0 @@
|
||||
#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_GNSS_PWR_EN 34 /* SCHEMATIC LABEL: GPIO_UBLOX_PWR_EN */
|
||||
#define GPIO_STM_RST_N 124
|
||||
#define GPIO_STM_BOOT0 134
|
||||
#define GPIO_BMX_ACCEL_INT 21
|
||||
#define GPIO_BMX_GYRO_INT 23
|
||||
#define GPIO_BMX_MAGN_INT 87
|
||||
#define GPIO_LSM_INT 84
|
||||
#define GPIOCHIP_INT 0
|
||||
#else
|
||||
#define GPIO_HUB_RST_N 0
|
||||
#define GPIO_UBLOX_RST_N 0
|
||||
#define GPIO_UBLOX_SAFEBOOT_N 0
|
||||
#define GPIO_GNSS_PWR_EN 0 /* SCHEMATIC LABEL: GPIO_UBLOX_PWR_EN */
|
||||
#define GPIO_STM_RST_N 0
|
||||
#define GPIO_STM_BOOT0 0
|
||||
#define GPIO_BMX_ACCEL_INT 0
|
||||
#define GPIO_BMX_GYRO_INT 0
|
||||
#define GPIO_BMX_MAGN_INT 0
|
||||
#define GPIO_LSM_INT 0
|
||||
#define GPIOCHIP_INT 0
|
||||
#endif
|
||||
|
||||
int gpio_init(int pin_nr, bool output);
|
||||
int gpio_set(int pin_nr, bool high);
|
||||
|
||||
int gpiochip_get_ro_value_fd(const char* consumer_label, int gpiochiop_id, int pin_nr);
|
||||
@@ -1,92 +0,0 @@
|
||||
#include "common/i2c.h"
|
||||
|
||||
#include <fcntl.h>
|
||||
#include <sys/ioctl.h>
|
||||
#include <unistd.h>
|
||||
|
||||
#include <cassert>
|
||||
#include <cstdio>
|
||||
#include <stdexcept>
|
||||
|
||||
#include "common/swaglog.h"
|
||||
#include "common/util.h"
|
||||
|
||||
#define UNUSED(x) (void)(x)
|
||||
|
||||
#ifdef QCOM2
|
||||
// TODO: decide if we want to install 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) {
|
||||
std::lock_guard lk(m);
|
||||
|
||||
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) {
|
||||
std::lock_guard lk(m);
|
||||
|
||||
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
|
||||
19
common/i2c.h
19
common/i2c.h
@@ -1,19 +0,0 @@
|
||||
#pragma once
|
||||
|
||||
#include <cstdint>
|
||||
#include <mutex>
|
||||
|
||||
#include <sys/types.h>
|
||||
|
||||
class I2CBus {
|
||||
private:
|
||||
int i2c_fd;
|
||||
std::mutex m;
|
||||
|
||||
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);
|
||||
};
|
||||
85
common/mat.h
85
common/mat.h
@@ -1,85 +0,0 @@
|
||||
#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));
|
||||
}
|
||||
581
common/params.cc
581
common/params.cc
@@ -1,581 +0,0 @@
|
||||
#include "common/params.h"
|
||||
|
||||
#include <dirent.h>
|
||||
#include <sys/file.h>
|
||||
|
||||
#include <algorithm>
|
||||
#include <cassert>
|
||||
#include <csignal>
|
||||
#include <unordered_map>
|
||||
|
||||
#include "common/queue.h"
|
||||
#include "common/swaglog.h"
|
||||
#include "common/util.h"
|
||||
#include "system/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 ¶m_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, path=%s, param_prefix=%s",
|
||||
errno, params_path.c_str(), prefix.c_str()));
|
||||
}
|
||||
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},
|
||||
{"ApiCache_Device", PERSISTENT},
|
||||
{"ApiCache_NavDestinations", PERSISTENT},
|
||||
{"AssistNowToken", PERSISTENT},
|
||||
{"AthenadPid", PERSISTENT},
|
||||
{"AthenadUploadQueue", PERSISTENT},
|
||||
{"AthenadRecentlyViewedRoutes", PERSISTENT},
|
||||
{"BootCount", PERSISTENT},
|
||||
{"CalibrationParams", PERSISTENT},
|
||||
{"CameraDebugExpGain", CLEAR_ON_MANAGER_START},
|
||||
{"CameraDebugExpTime", CLEAR_ON_MANAGER_START},
|
||||
{"CarBatteryCapacity", PERSISTENT},
|
||||
{"CarParams", CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION},
|
||||
{"CarParamsCache", CLEAR_ON_MANAGER_START},
|
||||
{"CarParamsPersistent", PERSISTENT},
|
||||
{"CarParamsPrevRoute", PERSISTENT},
|
||||
{"CarVin", CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION},
|
||||
{"CompletedTrainingVersion", PERSISTENT},
|
||||
{"ControlsReady", CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION},
|
||||
{"CurrentBootlog", PERSISTENT},
|
||||
{"CurrentRoute", CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION},
|
||||
{"DisableLogging", CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION},
|
||||
{"DisablePowerDown", PERSISTENT},
|
||||
{"DisableUpdates", PERSISTENT},
|
||||
{"DisengageOnAccelerator", PERSISTENT},
|
||||
{"DmModelInitialized", CLEAR_ON_ONROAD_TRANSITION},
|
||||
{"DongleId", PERSISTENT},
|
||||
{"DoReboot", CLEAR_ON_MANAGER_START},
|
||||
{"DoShutdown", CLEAR_ON_MANAGER_START},
|
||||
{"DoUninstall", CLEAR_ON_MANAGER_START},
|
||||
{"ExperimentalLongitudinalEnabled", PERSISTENT | DEVELOPMENT_ONLY},
|
||||
{"ExperimentalMode", PERSISTENT},
|
||||
{"ExperimentalModeConfirmed", PERSISTENT},
|
||||
{"FirmwareQueryDone", CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION},
|
||||
{"ForcePowerDown", PERSISTENT},
|
||||
{"GitBranch", PERSISTENT},
|
||||
{"GitCommit", PERSISTENT},
|
||||
{"GitCommitDate", PERSISTENT},
|
||||
{"GitDiff", PERSISTENT},
|
||||
{"GithubSshKeys", PERSISTENT},
|
||||
{"GithubUsername", PERSISTENT},
|
||||
{"GitRemote", PERSISTENT},
|
||||
{"GsmApn", PERSISTENT},
|
||||
{"GsmMetered", 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},
|
||||
{"IsRhdDetected", PERSISTENT},
|
||||
{"IsReleaseBranch", CLEAR_ON_MANAGER_START},
|
||||
{"IsTakingSnapshot", CLEAR_ON_MANAGER_START},
|
||||
{"IsTestedBranch", CLEAR_ON_MANAGER_START},
|
||||
{"JoystickDebugMode", CLEAR_ON_MANAGER_START | CLEAR_ON_OFFROAD_TRANSITION},
|
||||
{"LanguageSetting", PERSISTENT},
|
||||
{"LastAthenaPingTime", CLEAR_ON_MANAGER_START},
|
||||
{"LastGPSPosition", PERSISTENT},
|
||||
{"LastManagerExitReason", CLEAR_ON_MANAGER_START},
|
||||
{"LastOffroadStatusPacket", CLEAR_ON_MANAGER_START | CLEAR_ON_OFFROAD_TRANSITION},
|
||||
{"LastPowerDropDetected", CLEAR_ON_MANAGER_START},
|
||||
{"LastUpdateException", CLEAR_ON_MANAGER_START},
|
||||
{"LastUpdateTime", PERSISTENT},
|
||||
{"LiveParameters", PERSISTENT},
|
||||
{"LiveTorqueParameters", PERSISTENT | DONT_LOG},
|
||||
{"LongitudinalPersonality", PERSISTENT},
|
||||
{"NavDestination", CLEAR_ON_MANAGER_START | CLEAR_ON_OFFROAD_TRANSITION},
|
||||
{"NavDestinationWaypoints", CLEAR_ON_MANAGER_START | CLEAR_ON_OFFROAD_TRANSITION},
|
||||
{"NavPastDestinations", PERSISTENT},
|
||||
{"NavSettingLeftSide", PERSISTENT},
|
||||
{"NavSettingTime24h", PERSISTENT},
|
||||
{"NetworkMetered", PERSISTENT},
|
||||
{"ObdMultiplexingChanged", CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION},
|
||||
{"ObdMultiplexingEnabled", CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION},
|
||||
{"Offroad_BadNvme", CLEAR_ON_MANAGER_START},
|
||||
{"Offroad_CarUnrecognized", CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION},
|
||||
{"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_ONROAD_TRANSITION},
|
||||
{"Offroad_Recalibration", CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION},
|
||||
{"Offroad_StorageMissing", CLEAR_ON_MANAGER_START},
|
||||
{"Offroad_TemperatureTooHigh", CLEAR_ON_MANAGER_START},
|
||||
{"Offroad_UnofficialHardware", CLEAR_ON_MANAGER_START},
|
||||
{"Offroad_UpdateFailed", CLEAR_ON_MANAGER_START},
|
||||
{"OpenpilotEnabledToggle", PERSISTENT},
|
||||
{"PandaHeartbeatLost", CLEAR_ON_MANAGER_START | CLEAR_ON_OFFROAD_TRANSITION},
|
||||
{"PandaSomResetTriggered", CLEAR_ON_MANAGER_START | CLEAR_ON_OFFROAD_TRANSITION},
|
||||
{"PandaSignatures", CLEAR_ON_MANAGER_START},
|
||||
{"PrimeType", PERSISTENT},
|
||||
{"RecordFront", PERSISTENT},
|
||||
{"RecordFrontLock", PERSISTENT}, // for the internal fleet
|
||||
{"ReplayControlsState", CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION},
|
||||
{"SnoozeUpdate", CLEAR_ON_MANAGER_START | CLEAR_ON_OFFROAD_TRANSITION},
|
||||
{"SshEnabled", PERSISTENT},
|
||||
{"TermsVersion", PERSISTENT},
|
||||
{"Timezone", PERSISTENT},
|
||||
{"TrainingVersion", PERSISTENT},
|
||||
{"UbloxAvailable", PERSISTENT},
|
||||
{"UpdateAvailable", CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION},
|
||||
{"UpdateFailedCount", CLEAR_ON_MANAGER_START},
|
||||
{"UpdaterAvailableBranches", PERSISTENT},
|
||||
{"UpdaterCurrentDescription", CLEAR_ON_MANAGER_START},
|
||||
{"UpdaterCurrentReleaseNotes", CLEAR_ON_MANAGER_START},
|
||||
{"UpdaterFetchAvailable", CLEAR_ON_MANAGER_START},
|
||||
{"UpdaterNewDescription", CLEAR_ON_MANAGER_START},
|
||||
{"UpdaterNewReleaseNotes", CLEAR_ON_MANAGER_START},
|
||||
{"UpdaterState", CLEAR_ON_MANAGER_START},
|
||||
{"UpdaterTargetBranch", CLEAR_ON_MANAGER_START},
|
||||
{"UpdaterLastFetchTime", PERSISTENT},
|
||||
{"Version", PERSISTENT},
|
||||
{"VisionRadarToggle", PERSISTENT},
|
||||
|
||||
// FrogPilot parameters
|
||||
{"AccelerationPath", PERSISTENT},
|
||||
{"AccelerationProfile", PERSISTENT},
|
||||
{"AdjacentPath", PERSISTENT},
|
||||
{"AdjacentPathMetrics", PERSISTENT},
|
||||
{"AdjustablePersonalities", PERSISTENT},
|
||||
{"AggressiveAcceleration", PERSISTENT},
|
||||
{"AggressiveFollow", PERSISTENT},
|
||||
{"AggressiveJerk", PERSISTENT},
|
||||
{"AlertVolumeControl", PERSISTENT},
|
||||
{"AlwaysOnLateral", PERSISTENT},
|
||||
{"AlwaysOnLateralMain", PERSISTENT},
|
||||
{"AMapKey1", PERSISTENT},
|
||||
{"AMapKey2", PERSISTENT},
|
||||
{"ApiCache_DriveStats", PERSISTENT},
|
||||
{"BlindSpotPath", PERSISTENT},
|
||||
{"CameraFPS", PERSISTENT},
|
||||
{"CameraView", PERSISTENT},
|
||||
{"CarMake", PERSISTENT},
|
||||
{"CarModel", PERSISTENT},
|
||||
{"CarSpeedLimit", PERSISTENT},
|
||||
{"CECurves", PERSISTENT},
|
||||
{"CECurvesLead", PERSISTENT},
|
||||
{"CENavigation", PERSISTENT},
|
||||
{"CENavigationIntersections", PERSISTENT},
|
||||
{"CENavigationLead", PERSISTENT},
|
||||
{"CENavigationTurns", PERSISTENT},
|
||||
{"CESignal", PERSISTENT},
|
||||
{"CESlowerLead", PERSISTENT},
|
||||
{"CESpeed", PERSISTENT},
|
||||
{"CESpeedLead", PERSISTENT},
|
||||
{"CEStatus", PERSISTENT},
|
||||
{"CEStopLights", PERSISTENT},
|
||||
{"CEStopLightsLead", PERSISTENT},
|
||||
{"Compass", PERSISTENT},
|
||||
{"ConditionalExperimental", PERSISTENT},
|
||||
{"CrosstrekTorque", PERSISTENT},
|
||||
{"CurrentHolidayTheme", PERSISTENT},
|
||||
{"CurrentRandomEvent", PERSISTENT},
|
||||
{"CurveSensitivity", PERSISTENT},
|
||||
{"CustomAlerts", PERSISTENT},
|
||||
{"CustomColors", PERSISTENT},
|
||||
{"CustomIcons", PERSISTENT},
|
||||
{"CustomPersonalities", PERSISTENT},
|
||||
{"CustomUI", PERSISTENT},
|
||||
{"CustomSignals", PERSISTENT},
|
||||
{"CustomSounds", PERSISTENT},
|
||||
{"CustomTheme", PERSISTENT},
|
||||
{"CydiaTune", PERSISTENT},
|
||||
{"DecelerationProfile", PERSISTENT},
|
||||
{"DeviceShutdown", PERSISTENT},
|
||||
{"DisableMTSCSmoothing", PERSISTENT},
|
||||
{"DisableOnroadUploads", PERSISTENT},
|
||||
{"DisableOpenpilotLongitudinal", PERSISTENT},
|
||||
{"DisableVTSCSmoothing", PERSISTENT},
|
||||
{"DisengageVolume", PERSISTENT},
|
||||
{"DoSoftReboot", CLEAR_ON_MANAGER_START},
|
||||
{"DragonPilotTune", PERSISTENT},
|
||||
{"DriverCamera", PERSISTENT},
|
||||
{"DriveStats", PERSISTENT},
|
||||
{"DynamicPathWidth", PERSISTENT},
|
||||
{"EngageVolume", PERSISTENT},
|
||||
{"EVTable", PERSISTENT},
|
||||
{"ExperimentalModeActivation", PERSISTENT},
|
||||
{"ExperimentalModeViaDistance", PERSISTENT},
|
||||
{"ExperimentalModeViaLKAS", PERSISTENT},
|
||||
{"ExperimentalModeViaScreen", PERSISTENT},
|
||||
{"Fahrenheit", PERSISTENT},
|
||||
{"FireTheBabysitter", PERSISTENT},
|
||||
{"ForceAutoTune", PERSISTENT},
|
||||
{"ForceFingerprint", PERSISTENT},
|
||||
{"ForceMPHDashboard", PERSISTENT},
|
||||
{"FPSCounter", PERSISTENT},
|
||||
{"FrogPilotDrives", PERSISTENT},
|
||||
{"FrogPilotKilometers", PERSISTENT},
|
||||
{"FrogPilotMinutes", PERSISTENT},
|
||||
{"FrogPilotTogglesUpdated", PERSISTENT},
|
||||
{"FrogsGoMoo", PERSISTENT},
|
||||
{"FrogsGoMooTune", PERSISTENT},
|
||||
{"FullMap", PERSISTENT},
|
||||
{"GasRegenCmd", PERSISTENT},
|
||||
{"GMapKey", PERSISTENT},
|
||||
{"GoatScream", PERSISTENT},
|
||||
{"GreenLightAlert", PERSISTENT},
|
||||
{"HideAlerts", PERSISTENT},
|
||||
{"HideAOLStatusBar", PERSISTENT},
|
||||
{"HideCEMStatusBar", PERSISTENT},
|
||||
{"HideLeadMarker", PERSISTENT},
|
||||
{"HideMapIcon", PERSISTENT},
|
||||
{"HideMaxSpeed", PERSISTENT},
|
||||
{"HideSpeed", PERSISTENT},
|
||||
{"HideSpeedUI", PERSISTENT},
|
||||
{"HideUIElements", PERSISTENT},
|
||||
{"HigherBitrate", PERSISTENT},
|
||||
{"HolidayThemes", PERSISTENT},
|
||||
{"LaneChangeTime", PERSISTENT},
|
||||
{"LaneDetection", PERSISTENT},
|
||||
{"LaneDetectionWidth", PERSISTENT},
|
||||
{"LaneLinesWidth", PERSISTENT},
|
||||
{"LastMapsUpdate", PERSISTENT},
|
||||
{"LateralTune", PERSISTENT},
|
||||
{"LeadDepartingAlert", PERSISTENT},
|
||||
{"LeadDetectionThreshold", PERSISTENT},
|
||||
{"LeadInfo", PERSISTENT},
|
||||
{"LockDoors", PERSISTENT},
|
||||
{"LongitudinalTune", PERSISTENT},
|
||||
{"LongPitch", PERSISTENT},
|
||||
{"LoudBlindspotAlert", PERSISTENT},
|
||||
{"LowerVolt", PERSISTENT},
|
||||
{"ManualUpdateInitiated", CLEAR_ON_MANAGER_START},
|
||||
{"MapboxPublicKey", PERSISTENT},
|
||||
{"MapboxSecretKey", PERSISTENT},
|
||||
{"MapsSelected", PERSISTENT},
|
||||
{"MapSpeedLimit", PERSISTENT},
|
||||
{"MapSpeedLimitControl", PERSISTENT},
|
||||
{"MapStyle", PERSISTENT},
|
||||
{"MapTargetLatA", PERSISTENT},
|
||||
{"MapTargetVelocities", PERSISTENT},
|
||||
{"Model", PERSISTENT},
|
||||
{"ModelUI", PERSISTENT},
|
||||
{"MTSCAggressiveness", PERSISTENT},
|
||||
{"MTSCCurvatureCheck", PERSISTENT},
|
||||
{"MTSCEnabled", PERSISTENT},
|
||||
{"MTSCLimit", PERSISTENT},
|
||||
{"MuteOverheated", PERSISTENT},
|
||||
{"NavChill", PERSISTENT},
|
||||
{"NavEnable", PERSISTENT},
|
||||
{"NavSpeedLimit", PERSISTENT},
|
||||
{"NavSpeedLimitControl", PERSISTENT},
|
||||
{"NNFF", PERSISTENT},
|
||||
{"NNFFModelFuzzyMatch", PERSISTENT},
|
||||
{"NNFFModelName", PERSISTENT},
|
||||
{"NoLogging", PERSISTENT},
|
||||
{"NoUploads", PERSISTENT},
|
||||
{"NudgelessLaneChange", PERSISTENT},
|
||||
{"NumericalTemp", PERSISTENT},
|
||||
{"OfflineMode", PERSISTENT},
|
||||
{"Offset1", PERSISTENT},
|
||||
{"Offset2", PERSISTENT},
|
||||
{"Offset3", PERSISTENT},
|
||||
{"Offset4", PERSISTENT},
|
||||
{"OneLaneChange", PERSISTENT},
|
||||
{"OSMDownloadLocations", PERSISTENT},
|
||||
{"OSMDownloadProgress", CLEAR_ON_MANAGER_START},
|
||||
{"PathEdgeWidth", PERSISTENT},
|
||||
{"PathWidth", PERSISTENT},
|
||||
{"PauseLateralOnSignal", PERSISTENT},
|
||||
{"PedalsOnUI", PERSISTENT},
|
||||
{"PersonalitiesViaScreen", PERSISTENT},
|
||||
{"PersonalitiesViaWheel", PERSISTENT},
|
||||
{"PersonalityChangedViaUI", PERSISTENT},
|
||||
{"PersonalityChangedViaWheel", PERSISTENT},
|
||||
{"PreferredSchedule", PERSISTENT},
|
||||
{"PreviousSpeedLimit", PERSISTENT},
|
||||
{"PromptVolume", PERSISTENT},
|
||||
{"PromptDistractedVolume", PERSISTENT},
|
||||
{"QOLControls", PERSISTENT},
|
||||
{"QOLVisuals", PERSISTENT},
|
||||
{"RandomEvents", PERSISTENT},
|
||||
{"RefuseVolume", PERSISTENT},
|
||||
{"RelaxedFollow", PERSISTENT},
|
||||
{"RelaxedJerk", PERSISTENT},
|
||||
{"ReverseCruise", PERSISTENT},
|
||||
{"ReverseCruiseUI", PERSISTENT},
|
||||
{"RoadEdgesWidth", PERSISTENT},
|
||||
{"RoadName", PERSISTENT},
|
||||
{"RoadNameUI", PERSISTENT},
|
||||
{"RotatingWheel", PERSISTENT},
|
||||
{"SchedulePending", PERSISTENT},
|
||||
{"ScreenBrightness", PERSISTENT},
|
||||
{"ScreenBrightnessOnroad", PERSISTENT},
|
||||
{"ScreenManagement", PERSISTENT},
|
||||
{"ScreenRecorder", PERSISTENT},
|
||||
{"ScreenTimeout", PERSISTENT},
|
||||
{"ScreenTimeoutOnroad", PERSISTENT},
|
||||
{"SearchInput", PERSISTENT},
|
||||
{"SetSpeedLimit", PERSISTENT},
|
||||
{"SetSpeedOffset", PERSISTENT},
|
||||
{"SilentMode", PERSISTENT},
|
||||
{"ShowCPU", PERSISTENT},
|
||||
{"ShowGPU", PERSISTENT},
|
||||
{"ShowIP", PERSISTENT},
|
||||
{"ShowMemoryUsage", PERSISTENT},
|
||||
{"ShowSLCOffset", PERSISTENT},
|
||||
{"ShowSLCOffsetUI", PERSISTENT},
|
||||
{"ShowStorageLeft", PERSISTENT},
|
||||
{"ShowStorageUsed", PERSISTENT},
|
||||
{"Sidebar", PERSISTENT},
|
||||
{"SLCConfirmation", PERSISTENT},
|
||||
{"SLCConfirmationLower", PERSISTENT},
|
||||
{"SLCConfirmationHigher", PERSISTENT},
|
||||
{"SLCConfirmed", PERSISTENT},
|
||||
{"SLCConfirmedPressed", PERSISTENT},
|
||||
{"SLCFallback", PERSISTENT},
|
||||
{"SLCOverride", PERSISTENT},
|
||||
{"SLCPriority", PERSISTENT},
|
||||
{"SLCPriority1", PERSISTENT},
|
||||
{"SLCPriority2", PERSISTENT},
|
||||
{"SLCPriority3", PERSISTENT},
|
||||
{"SmoothBraking", PERSISTENT},
|
||||
{"SNGHack", PERSISTENT},
|
||||
{"SpeedLimitController", PERSISTENT},
|
||||
{"SpeedLimitChangedAlert", PERSISTENT},
|
||||
{"StandardFollow", PERSISTENT},
|
||||
{"StandardJerk", PERSISTENT},
|
||||
{"StandbyMode", PERSISTENT},
|
||||
{"SteerRatio", PERSISTENT},
|
||||
{"SteerRatioStock", PERSISTENT},
|
||||
{"StockTune", PERSISTENT},
|
||||
{"StoppingDistance", PERSISTENT},
|
||||
{"TetheringEnabled", PERSISTENT},
|
||||
{"TrafficMode", PERSISTENT},
|
||||
{"TrafficModeActive", PERSISTENT},
|
||||
{"TurnAggressiveness", PERSISTENT},
|
||||
{"TurnDesires", PERSISTENT},
|
||||
{"UnlimitedLength", PERSISTENT},
|
||||
{"Updated", PERSISTENT},
|
||||
{"UpdateSchedule", PERSISTENT},
|
||||
{"UpdateTime", PERSISTENT},
|
||||
{"UseLateralJerk", PERSISTENT},
|
||||
{"UseSI", PERSISTENT},
|
||||
{"UseVienna", PERSISTENT},
|
||||
{"VisionTurnControl", PERSISTENT},
|
||||
{"WarningSoftVolume", PERSISTENT},
|
||||
{"WarningImmediateVolume", PERSISTENT},
|
||||
{"WheelIcon", PERSISTENT},
|
||||
{"WheelSpeed", PERSISTENT},
|
||||
};
|
||||
|
||||
} // namespace
|
||||
|
||||
|
||||
Params::Params(const std::string &path) {
|
||||
params_prefix = "/" + util::getenv("OPENPILOT_PREFIX", "d");
|
||||
params_path = ensure_params_path(params_prefix, path);
|
||||
}
|
||||
|
||||
Params::~Params() {
|
||||
if (future.valid()) {
|
||||
future.wait();
|
||||
}
|
||||
assert(queue.empty());
|
||||
}
|
||||
|
||||
std::vector<std::string> Params::allKeys() const {
|
||||
std::vector<std::string> ret;
|
||||
for (auto &p : keys) {
|
||||
ret.push_back(p.first);
|
||||
}
|
||||
return ret;
|
||||
}
|
||||
|
||||
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");
|
||||
|
||||
// 1) delete params of key_type
|
||||
// 2) delete files that are not defined in the keys.
|
||||
if (DIR *d = opendir(getParamPath().c_str())) {
|
||||
struct dirent *de = NULL;
|
||||
while ((de = readdir(d))) {
|
||||
if (de->d_type != DT_DIR) {
|
||||
auto it = keys.find(de->d_name);
|
||||
if (it == keys.end() || (it->second & key_type)) {
|
||||
unlink(getParamPath(de->d_name).c_str());
|
||||
}
|
||||
}
|
||||
}
|
||||
closedir(d);
|
||||
}
|
||||
|
||||
fsync_dir(getParamPath());
|
||||
}
|
||||
|
||||
void Params::putNonBlocking(const std::string &key, const std::string &val) {
|
||||
queue.push(std::make_pair(key, val));
|
||||
// start thread on demand
|
||||
if (!future.valid() || future.wait_for(std::chrono::milliseconds(0)) == std::future_status::ready) {
|
||||
future = std::async(std::launch::async, &Params::asyncWriteThread, this);
|
||||
}
|
||||
}
|
||||
|
||||
void Params::asyncWriteThread() {
|
||||
// TODO: write the latest one if a key has multiple values in the queue.
|
||||
std::pair<std::string, std::string> p;
|
||||
while (queue.try_pop(p, 0)) {
|
||||
// Params::put is Thread-Safe
|
||||
put(p.first, p.second);
|
||||
}
|
||||
}
|
||||
@@ -1,92 +0,0 @@
|
||||
#pragma once
|
||||
|
||||
#include <future>
|
||||
#include <map>
|
||||
#include <string>
|
||||
#include <tuple>
|
||||
#include <utility>
|
||||
#include <vector>
|
||||
|
||||
#include "common/queue.h"
|
||||
|
||||
enum ParamKeyType {
|
||||
PERSISTENT = 0x02,
|
||||
CLEAR_ON_MANAGER_START = 0x04,
|
||||
CLEAR_ON_ONROAD_TRANSITION = 0x08,
|
||||
CLEAR_ON_OFFROAD_TRANSITION = 0x10,
|
||||
DONT_LOG = 0x20,
|
||||
DEVELOPMENT_ONLY = 0x40,
|
||||
ALL = 0xFFFFFFFF
|
||||
};
|
||||
|
||||
class Params {
|
||||
public:
|
||||
explicit Params(const std::string &path = {});
|
||||
~Params();
|
||||
// Not copyable.
|
||||
Params(const Params&) = delete;
|
||||
Params& operator=(const Params&) = delete;
|
||||
|
||||
std::vector<std::string> allKeys() const;
|
||||
bool checkKey(const std::string &key);
|
||||
ParamKeyType getKeyType(const std::string &key);
|
||||
inline std::string getParamPath(const std::string &key = {}) {
|
||||
return params_path + params_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, bool block = false) {
|
||||
return get(key, block) == "1";
|
||||
}
|
||||
inline int getInt(const std::string &key, bool block = false) {
|
||||
std::string value = get(key, block);
|
||||
return value.empty() ? 0 : std::stoi(value);
|
||||
}
|
||||
inline float getFloat(const std::string &key, bool block = false) {
|
||||
std::string value = get(key, block);
|
||||
return value.empty() ? 0.0 : std::stof(value);
|
||||
}
|
||||
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);
|
||||
}
|
||||
inline int putInt(const std::string &key, int val) {
|
||||
return put(key.c_str(), std::to_string(val).c_str(), std::to_string(val).size());
|
||||
}
|
||||
inline int putFloat(const std::string &key, float val) {
|
||||
return put(key.c_str(), std::to_string(val).c_str(), std::to_string(val).size());
|
||||
}
|
||||
void putNonBlocking(const std::string &key, const std::string &val);
|
||||
inline void putBoolNonBlocking(const std::string &key, bool val) {
|
||||
putNonBlocking(key, val ? "1" : "0");
|
||||
}
|
||||
void putIntNonBlocking(const std::string &key, const std::string &val);
|
||||
inline void putIntNonBlocking(const std::string &key, int val) {
|
||||
putNonBlocking(key, std::to_string(val));
|
||||
}
|
||||
void putFloatNonBlocking(const std::string &key, const std::string &val);
|
||||
inline void putFloatNonBlocking(const std::string &key, float val) {
|
||||
putNonBlocking(key, std::to_string(val));
|
||||
}
|
||||
|
||||
private:
|
||||
void asyncWriteThread();
|
||||
|
||||
std::string params_path;
|
||||
std::string params_prefix;
|
||||
|
||||
// for nonblocking write
|
||||
std::future<void> future;
|
||||
SafeQueue<std::pair<std::string, std::string>> queue;
|
||||
};
|
||||
19684
common/params_pyx.cpp
Normal file
19684
common/params_pyx.cpp
Normal file
File diff suppressed because it is too large
Load Diff
BIN
common/params_pyx.so
Executable file
BIN
common/params_pyx.so
Executable file
Binary file not shown.
@@ -1,34 +0,0 @@
|
||||
#pragma once
|
||||
|
||||
#include <cassert>
|
||||
#include <string>
|
||||
|
||||
#include "common/params.h"
|
||||
#include "common/util.h"
|
||||
|
||||
class OpenpilotPrefix {
|
||||
public:
|
||||
OpenpilotPrefix(std::string prefix = {}) {
|
||||
if (prefix.empty()) {
|
||||
prefix = util::random_string(15);
|
||||
}
|
||||
msgq_path = "/dev/shm/" + prefix;
|
||||
bool ret = util::create_directories(msgq_path, 0777);
|
||||
assert(ret);
|
||||
setenv("OPENPILOT_PREFIX", prefix.c_str(), 1);
|
||||
}
|
||||
|
||||
~OpenpilotPrefix() {
|
||||
auto param_path = Params().getParamPath();
|
||||
if (util::file_exists(param_path)) {
|
||||
std::string real_path = util::readlink(param_path);
|
||||
system(util::string_format("rm %s -rf", real_path.c_str()).c_str());
|
||||
unlink(param_path.c_str());
|
||||
}
|
||||
system(util::string_format("rm %s -rf", msgq_path.c_str()).c_str());
|
||||
unsetenv("OPENPILOT_PREFIX");
|
||||
}
|
||||
|
||||
private:
|
||||
std::string msgq_path;
|
||||
};
|
||||
@@ -1,52 +0,0 @@
|
||||
#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;
|
||||
};
|
||||
@@ -1,40 +0,0 @@
|
||||
#include "common/ratekeeper.h"
|
||||
|
||||
#include <algorithm>
|
||||
|
||||
#include "common/swaglog.h"
|
||||
#include "common/timing.h"
|
||||
#include "common/util.h"
|
||||
|
||||
RateKeeper::RateKeeper(const std::string &name, float rate, float print_delay_threshold)
|
||||
: name(name),
|
||||
print_delay_threshold(std::max(0.f, print_delay_threshold)) {
|
||||
interval = 1 / rate;
|
||||
last_monitor_time = seconds_since_boot();
|
||||
next_frame_time = last_monitor_time + interval;
|
||||
}
|
||||
|
||||
bool RateKeeper::keepTime() {
|
||||
bool lagged = monitorTime();
|
||||
if (remaining_ > 0) {
|
||||
util::sleep_for(remaining_ * 1000);
|
||||
}
|
||||
return lagged;
|
||||
}
|
||||
|
||||
bool RateKeeper::monitorTime() {
|
||||
++frame_;
|
||||
last_monitor_time = seconds_since_boot();
|
||||
remaining_ = next_frame_time - last_monitor_time;
|
||||
|
||||
bool lagged = remaining_ < 0;
|
||||
if (lagged) {
|
||||
if (print_delay_threshold > 0 && remaining_ < -print_delay_threshold) {
|
||||
LOGW("%s lagging by %.2f ms", name.c_str(), -remaining_ * 1000);
|
||||
}
|
||||
next_frame_time = last_monitor_time + interval;
|
||||
} else {
|
||||
next_frame_time += interval;
|
||||
}
|
||||
return lagged;
|
||||
}
|
||||
@@ -1,23 +0,0 @@
|
||||
#pragma once
|
||||
|
||||
#include <cstdint>
|
||||
#include <string>
|
||||
|
||||
class RateKeeper {
|
||||
public:
|
||||
RateKeeper(const std::string &name, float rate, float print_delay_threshold = 0);
|
||||
~RateKeeper() {}
|
||||
bool keepTime();
|
||||
bool monitorTime();
|
||||
inline double frame() const { return frame_; }
|
||||
inline double remaining() const { return remaining_; }
|
||||
|
||||
private:
|
||||
double interval;
|
||||
double next_frame_time;
|
||||
double last_monitor_time;
|
||||
double remaining_ = 0;
|
||||
float print_delay_threshold = 0;
|
||||
uint64_t frame_ = 0;
|
||||
std::string name;
|
||||
};
|
||||
@@ -1,152 +0,0 @@
|
||||
#ifndef _GNU_SOURCE
|
||||
#define _GNU_SOURCE
|
||||
#endif
|
||||
|
||||
#include "common/swaglog.h"
|
||||
|
||||
#include <cassert>
|
||||
#include <limits>
|
||||
#include <mutex>
|
||||
#include <string>
|
||||
|
||||
#include <zmq.h>
|
||||
#include <stdarg.h>
|
||||
#include "third_party/json11/json11.hpp"
|
||||
#include "common/version.h"
|
||||
#include "system/hardware/hw.h"
|
||||
|
||||
class SwaglogState {
|
||||
public:
|
||||
SwaglogState() {
|
||||
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, Path::swaglog_ipc().c_str());
|
||||
|
||||
print_level = CLOUDLOG_WARNING;
|
||||
if (const char* print_lvl = getenv("LOGPRINT")) {
|
||||
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;
|
||||
}
|
||||
}
|
||||
|
||||
ctx_j = json11::Json::object{};
|
||||
if (char* dongle_id = getenv("DONGLE_ID")) {
|
||||
ctx_j["dongle_id"] = dongle_id;
|
||||
}
|
||||
if (char* git_origin = getenv("GIT_ORIGIN")) {
|
||||
ctx_j["origin"] = git_origin;
|
||||
}
|
||||
if (char* git_branch = getenv("GIT_BRANCH")) {
|
||||
ctx_j["branch"] = git_branch;
|
||||
}
|
||||
if (char* git_commit = getenv("GIT_COMMIT")) {
|
||||
ctx_j["commit"] = git_commit;
|
||||
}
|
||||
if (char* daemon_name = getenv("MANAGER_DAEMON")) {
|
||||
ctx_j["daemon"] = daemon_name;
|
||||
}
|
||||
ctx_j["version"] = COMMA_VERSION;
|
||||
ctx_j["dirty"] = !getenv("CLEAN");
|
||||
ctx_j["device"] = Hardware::get_name();
|
||||
}
|
||||
|
||||
~SwaglogState() {
|
||||
zmq_close(sock);
|
||||
zmq_ctx_destroy(zctx);
|
||||
}
|
||||
|
||||
void log(int levelnum, const char* filename, int lineno, const char* func, const char* msg, const std::string& log_s) {
|
||||
std::lock_guard lk(lock);
|
||||
if (levelnum >= print_level) {
|
||||
printf("%s: %s\n", filename, msg);
|
||||
}
|
||||
zmq_send(sock, log_s.data(), log_s.length(), ZMQ_NOBLOCK);
|
||||
}
|
||||
|
||||
std::mutex lock;
|
||||
void* zctx = nullptr;
|
||||
void* sock = nullptr;
|
||||
int print_level;
|
||||
json11::Json::object ctx_j;
|
||||
};
|
||||
|
||||
bool LOG_TIMESTAMPS = getenv("LOG_TIMESTAMPS");
|
||||
uint32_t NO_FRAME_ID = std::numeric_limits<uint32_t>::max();
|
||||
|
||||
static void cloudlog_common(int levelnum, const char* filename, int lineno, const char* func,
|
||||
char* msg_buf, const json11::Json::object &msg_j={}) {
|
||||
static SwaglogState s;
|
||||
|
||||
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;
|
||||
log_s += (char)levelnum;
|
||||
((json11::Json)log_j).dump(log_s);
|
||||
s.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);
|
||||
}
|
||||
@@ -1,76 +0,0 @@
|
||||
#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
|
||||
|
||||
|
||||
#ifdef __GNUC__
|
||||
#define SWAG_LOG_CHECK_FMT(a, b) __attribute__ ((format (printf, a, b)))
|
||||
#else
|
||||
#define SWAG_LOG_CHECK_FMT(a, b)
|
||||
#endif
|
||||
|
||||
void cloudlog_e(int levelnum, const char* filename, int lineno, const char* func,
|
||||
const char* fmt, ...) SWAG_LOG_CHECK_FMT(5, 6);
|
||||
|
||||
void cloudlog_te(int levelnum, const char* filename, int lineno, const char* func,
|
||||
const char* fmt, ...) SWAG_LOG_CHECK_FMT(5, 6);
|
||||
|
||||
void cloudlog_te(int levelnum, const char* filename, int lineno, const char* func,
|
||||
uint32_t frame_id, const char* fmt, ...) SWAG_LOG_CHECK_FMT(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__)
|
||||
@@ -1,51 +0,0 @@
|
||||
#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;
|
||||
}
|
||||
@@ -1,5 +0,0 @@
|
||||
Import('env', 'envCython')
|
||||
|
||||
transformations = env.Library('transformations', ['orientation.cc', 'coordinates.cc'])
|
||||
transformations_python = envCython.Program('transformations.so', 'transformations.pyx')
|
||||
Export('transformations', 'transformations_python')
|
||||
@@ -1,100 +0,0 @@
|
||||
#define _USE_MATH_DEFINES
|
||||
|
||||
#include "common/transformations/coordinates.hpp"
|
||||
|
||||
#include <iostream>
|
||||
#include <cmath>
|
||||
#include <eigen3/Eigen/Dense>
|
||||
|
||||
double a = 6378137; // lgtm [cpp/short-global-name]
|
||||
double b = 6356752.3142; // lgtm [cpp/short-global-name]
|
||||
double esq = 6.69437999014 * 0.001; // lgtm [cpp/short-global-name]
|
||||
double e1sq = 6.73949674228 * 0.001;
|
||||
|
||||
|
||||
static Geodetic to_degrees(Geodetic geodetic){
|
||||
geodetic.lat = RAD2DEG(geodetic.lat);
|
||||
geodetic.lon = RAD2DEG(geodetic.lon);
|
||||
return geodetic;
|
||||
}
|
||||
|
||||
static Geodetic to_radians(Geodetic geodetic){
|
||||
geodetic.lat = DEG2RAD(geodetic.lat);
|
||||
geodetic.lon = DEG2RAD(geodetic.lon);
|
||||
return geodetic;
|
||||
}
|
||||
|
||||
|
||||
ECEF geodetic2ecef(Geodetic g){
|
||||
g = to_radians(g);
|
||||
double xi = sqrt(1.0 - esq * pow(sin(g.lat), 2));
|
||||
double x = (a / xi + g.alt) * cos(g.lat) * cos(g.lon);
|
||||
double y = (a / xi + g.alt) * cos(g.lat) * sin(g.lon);
|
||||
double z = (a / xi * (1.0 - esq) + g.alt) * sin(g.lat);
|
||||
return {x, y, z};
|
||||
}
|
||||
|
||||
Geodetic ecef2geodetic(ECEF e){
|
||||
// Convert from ECEF to geodetic using Ferrari's methods
|
||||
// https://en.wikipedia.org/wiki/Geographic_coordinate_conversion#Ferrari.27s_solution
|
||||
double x = e.x;
|
||||
double y = e.y;
|
||||
double z = e.z;
|
||||
|
||||
double r = sqrt(x * x + y * y);
|
||||
double Esq = a * a - b * b;
|
||||
double F = 54 * b * b * z * z;
|
||||
double G = r * r + (1 - esq) * z * z - esq * Esq;
|
||||
double C = (esq * esq * F * r * r) / (pow(G, 3));
|
||||
double S = cbrt(1 + C + sqrt(C * C + 2 * C));
|
||||
double P = F / (3 * pow((S + 1 / S + 1), 2) * G * G);
|
||||
double Q = sqrt(1 + 2 * esq * esq * P);
|
||||
double r_0 = -(P * esq * r) / (1 + Q) + sqrt(0.5 * a * a*(1 + 1.0 / Q) - P * (1 - esq) * z * z / (Q * (1 + Q)) - 0.5 * P * r * r);
|
||||
double U = sqrt(pow((r - esq * r_0), 2) + z * z);
|
||||
double V = sqrt(pow((r - esq * r_0), 2) + (1 - esq) * z * z);
|
||||
double Z_0 = b * b * z / (a * V);
|
||||
double h = U * (1 - b * b / (a * V));
|
||||
|
||||
double lat = atan((z + e1sq * Z_0) / r);
|
||||
double lon = atan2(y, x);
|
||||
|
||||
return to_degrees({lat, lon, h});
|
||||
}
|
||||
|
||||
LocalCoord::LocalCoord(Geodetic g, ECEF e){
|
||||
init_ecef << e.x, e.y, e.z;
|
||||
|
||||
g = to_radians(g);
|
||||
|
||||
ned2ecef_matrix <<
|
||||
-sin(g.lat)*cos(g.lon), -sin(g.lon), -cos(g.lat)*cos(g.lon),
|
||||
-sin(g.lat)*sin(g.lon), cos(g.lon), -cos(g.lat)*sin(g.lon),
|
||||
cos(g.lat), 0, -sin(g.lat);
|
||||
ecef2ned_matrix = ned2ecef_matrix.transpose();
|
||||
}
|
||||
|
||||
NED LocalCoord::ecef2ned(ECEF e) {
|
||||
Eigen::Vector3d ecef;
|
||||
ecef << e.x, e.y, e.z;
|
||||
|
||||
Eigen::Vector3d ned = (ecef2ned_matrix * (ecef - init_ecef));
|
||||
return {ned[0], ned[1], ned[2]};
|
||||
}
|
||||
|
||||
ECEF LocalCoord::ned2ecef(NED n) {
|
||||
Eigen::Vector3d ned;
|
||||
ned << n.n, n.e, n.d;
|
||||
|
||||
Eigen::Vector3d ecef = (ned2ecef_matrix * ned) + init_ecef;
|
||||
return {ecef[0], ecef[1], ecef[2]};
|
||||
}
|
||||
|
||||
NED LocalCoord::geodetic2ned(Geodetic g) {
|
||||
ECEF e = ::geodetic2ecef(g);
|
||||
return ecef2ned(e);
|
||||
}
|
||||
|
||||
Geodetic LocalCoord::ned2geodetic(NED n){
|
||||
ECEF e = ned2ecef(n);
|
||||
return ::ecef2geodetic(e);
|
||||
}
|
||||
@@ -1,144 +0,0 @@
|
||||
#define _USE_MATH_DEFINES
|
||||
|
||||
#include <iostream>
|
||||
#include <cmath>
|
||||
#include <eigen3/Eigen/Dense>
|
||||
|
||||
#include "common/transformations/orientation.hpp"
|
||||
#include "common/transformations/coordinates.hpp"
|
||||
|
||||
Eigen::Quaterniond ensure_unique(Eigen::Quaterniond quat){
|
||||
if (quat.w() > 0){
|
||||
return quat;
|
||||
} else {
|
||||
return Eigen::Quaterniond(-quat.w(), -quat.x(), -quat.y(), -quat.z());
|
||||
}
|
||||
}
|
||||
|
||||
Eigen::Quaterniond euler2quat(Eigen::Vector3d euler){
|
||||
Eigen::Quaterniond q;
|
||||
|
||||
q = Eigen::AngleAxisd(euler(2), Eigen::Vector3d::UnitZ())
|
||||
* Eigen::AngleAxisd(euler(1), Eigen::Vector3d::UnitY())
|
||||
* Eigen::AngleAxisd(euler(0), Eigen::Vector3d::UnitX());
|
||||
return ensure_unique(q);
|
||||
}
|
||||
|
||||
|
||||
Eigen::Vector3d quat2euler(Eigen::Quaterniond quat){
|
||||
// TODO: switch to eigen implementation if the range of the Euler angles doesn't matter anymore
|
||||
// Eigen::Vector3d euler = quat.toRotationMatrix().eulerAngles(2, 1, 0);
|
||||
// return {euler(2), euler(1), euler(0)};
|
||||
double gamma = atan2(2 * (quat.w() * quat.x() + quat.y() * quat.z()), 1 - 2 * (quat.x()*quat.x() + quat.y()*quat.y()));
|
||||
double asin_arg_clipped = std::clamp(2 * (quat.w() * quat.y() - quat.z() * quat.x()), -1.0, 1.0);
|
||||
double theta = asin(asin_arg_clipped);
|
||||
double psi = atan2(2 * (quat.w() * quat.z() + quat.x() * quat.y()), 1 - 2 * (quat.y()*quat.y() + quat.z()*quat.z()));
|
||||
return {gamma, theta, psi};
|
||||
}
|
||||
|
||||
Eigen::Matrix3d quat2rot(Eigen::Quaterniond quat){
|
||||
return quat.toRotationMatrix();
|
||||
}
|
||||
|
||||
Eigen::Quaterniond rot2quat(const Eigen::Matrix3d &rot){
|
||||
return ensure_unique(Eigen::Quaterniond(rot));
|
||||
}
|
||||
|
||||
Eigen::Matrix3d euler2rot(Eigen::Vector3d euler){
|
||||
return quat2rot(euler2quat(euler));
|
||||
}
|
||||
|
||||
Eigen::Vector3d rot2euler(const Eigen::Matrix3d &rot){
|
||||
return quat2euler(rot2quat(rot));
|
||||
}
|
||||
|
||||
Eigen::Matrix3d rot_matrix(double roll, double pitch, double yaw){
|
||||
return euler2rot({roll, pitch, yaw});
|
||||
}
|
||||
|
||||
Eigen::Matrix3d rot(Eigen::Vector3d axis, double angle){
|
||||
Eigen::Quaterniond q;
|
||||
q = Eigen::AngleAxisd(angle, axis);
|
||||
return q.toRotationMatrix();
|
||||
}
|
||||
|
||||
|
||||
Eigen::Vector3d ecef_euler_from_ned(ECEF ecef_init, Eigen::Vector3d ned_pose) {
|
||||
/*
|
||||
Using Rotations to Build Aerospace Coordinate Systems
|
||||
Don Koks
|
||||
https://apps.dtic.mil/dtic/tr/fulltext/u2/a484864.pdf
|
||||
*/
|
||||
LocalCoord converter = LocalCoord(ecef_init);
|
||||
Eigen::Vector3d zero = ecef_init.to_vector();
|
||||
|
||||
Eigen::Vector3d x0 = converter.ned2ecef({1, 0, 0}).to_vector() - zero;
|
||||
Eigen::Vector3d y0 = converter.ned2ecef({0, 1, 0}).to_vector() - zero;
|
||||
Eigen::Vector3d z0 = converter.ned2ecef({0, 0, 1}).to_vector() - zero;
|
||||
|
||||
Eigen::Vector3d x1 = rot(z0, ned_pose(2)) * x0;
|
||||
Eigen::Vector3d y1 = rot(z0, ned_pose(2)) * y0;
|
||||
Eigen::Vector3d z1 = rot(z0, ned_pose(2)) * z0;
|
||||
|
||||
Eigen::Vector3d x2 = rot(y1, ned_pose(1)) * x1;
|
||||
Eigen::Vector3d y2 = rot(y1, ned_pose(1)) * y1;
|
||||
Eigen::Vector3d z2 = rot(y1, ned_pose(1)) * z1;
|
||||
|
||||
Eigen::Vector3d x3 = rot(x2, ned_pose(0)) * x2;
|
||||
Eigen::Vector3d y3 = rot(x2, ned_pose(0)) * y2;
|
||||
|
||||
|
||||
x0 = Eigen::Vector3d(1, 0, 0);
|
||||
y0 = Eigen::Vector3d(0, 1, 0);
|
||||
z0 = Eigen::Vector3d(0, 0, 1);
|
||||
|
||||
double psi = atan2(x3.dot(y0), x3.dot(x0));
|
||||
double theta = atan2(-x3.dot(z0), sqrt(pow(x3.dot(x0), 2) + pow(x3.dot(y0), 2)));
|
||||
|
||||
y2 = rot(z0, psi) * y0;
|
||||
z2 = rot(y2, theta) * z0;
|
||||
|
||||
double phi = atan2(y3.dot(z2), y3.dot(y2));
|
||||
|
||||
return {phi, theta, psi};
|
||||
}
|
||||
|
||||
Eigen::Vector3d ned_euler_from_ecef(ECEF ecef_init, Eigen::Vector3d ecef_pose){
|
||||
/*
|
||||
Using Rotations to Build Aerospace Coordinate Systems
|
||||
Don Koks
|
||||
https://apps.dtic.mil/dtic/tr/fulltext/u2/a484864.pdf
|
||||
*/
|
||||
LocalCoord converter = LocalCoord(ecef_init);
|
||||
|
||||
Eigen::Vector3d x0 = Eigen::Vector3d(1, 0, 0);
|
||||
Eigen::Vector3d y0 = Eigen::Vector3d(0, 1, 0);
|
||||
Eigen::Vector3d z0 = Eigen::Vector3d(0, 0, 1);
|
||||
|
||||
Eigen::Vector3d x1 = rot(z0, ecef_pose(2)) * x0;
|
||||
Eigen::Vector3d y1 = rot(z0, ecef_pose(2)) * y0;
|
||||
Eigen::Vector3d z1 = rot(z0, ecef_pose(2)) * z0;
|
||||
|
||||
Eigen::Vector3d x2 = rot(y1, ecef_pose(1)) * x1;
|
||||
Eigen::Vector3d y2 = rot(y1, ecef_pose(1)) * y1;
|
||||
Eigen::Vector3d z2 = rot(y1, ecef_pose(1)) * z1;
|
||||
|
||||
Eigen::Vector3d x3 = rot(x2, ecef_pose(0)) * x2;
|
||||
Eigen::Vector3d y3 = rot(x2, ecef_pose(0)) * y2;
|
||||
|
||||
Eigen::Vector3d zero = ecef_init.to_vector();
|
||||
x0 = converter.ned2ecef({1, 0, 0}).to_vector() - zero;
|
||||
y0 = converter.ned2ecef({0, 1, 0}).to_vector() - zero;
|
||||
z0 = converter.ned2ecef({0, 0, 1}).to_vector() - zero;
|
||||
|
||||
double psi = atan2(x3.dot(y0), x3.dot(x0));
|
||||
double theta = atan2(-x3.dot(z0), sqrt(pow(x3.dot(x0), 2) + pow(x3.dot(y0), 2)));
|
||||
|
||||
y2 = rot(z0, psi) * y0;
|
||||
z2 = rot(y2, theta) * z0;
|
||||
|
||||
double phi = atan2(y3.dot(z2), y3.dot(y2));
|
||||
|
||||
return {phi, theta, psi};
|
||||
}
|
||||
|
||||
16660
common/transformations/transformations.cpp
Normal file
16660
common/transformations/transformations.cpp
Normal file
File diff suppressed because it is too large
Load Diff
BIN
common/transformations/transformations.so
Executable file
BIN
common/transformations/transformations.so
Executable file
Binary file not shown.
295
common/util.cc
295
common/util.cc
@@ -1,295 +0,0 @@
|
||||
#include "common/util.h"
|
||||
|
||||
#include <sys/ioctl.h>
|
||||
#include <sys/stat.h>
|
||||
#include <sys/resource.h>
|
||||
|
||||
#include <cassert>
|
||||
#include <cerrno>
|
||||
#include <cstring>
|
||||
#include <dirent.h>
|
||||
#include <fstream>
|
||||
#include <iomanip>
|
||||
#include <random>
|
||||
#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
|
||||
}
|
||||
|
||||
int set_file_descriptor_limit(uint64_t limit_val) {
|
||||
struct rlimit limit;
|
||||
int status;
|
||||
|
||||
if ((status = getrlimit(RLIMIT_NOFILE, &limit)) < 0)
|
||||
return status;
|
||||
|
||||
limit.rlim_cur = limit_val;
|
||||
if ((status = setrlimit(RLIMIT_NOFILE, &limit)) < 0)
|
||||
return status;
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
int safe_ioctl(int fd, unsigned long request, void *argp) {
|
||||
int ret;
|
||||
do {
|
||||
ret = ioctl(fd, request, argp);
|
||||
} while ((ret == -1) && (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, std::string 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();
|
||||
}
|
||||
|
||||
int random_int(int min, int max) {
|
||||
std::random_device dev;
|
||||
std::mt19937 rng(dev());
|
||||
std::uniform_int_distribution<std::mt19937::result_type> dist(min, max);
|
||||
return dist(rng);
|
||||
}
|
||||
|
||||
std::string random_string(std::string::size_type length) {
|
||||
const std::string chrs = "0123456789abcdefghijklmnopqrstuvwxyzABCDEFGHIJKLMNOPQRSTUVWXYZ";
|
||||
std::mt19937 rg{std::random_device{}()};
|
||||
std::uniform_int_distribution<std::string::size_type> pick(0, chrs.length() - 1);
|
||||
std::string s;
|
||||
s.reserve(length);
|
||||
while (length--) {
|
||||
s += chrs[pick(rg)];
|
||||
}
|
||||
return s;
|
||||
}
|
||||
|
||||
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);
|
||||
}
|
||||
|
||||
bool starts_with(const std::string &s1, const std::string &s2) {
|
||||
return strncmp(s1.c_str(), s2.c_str(), s2.size()) == 0;
|
||||
}
|
||||
|
||||
bool ends_with(const std::string &s1, const std::string &s2) {
|
||||
return strcmp(s1.c_str() + (s1.size() - s2.size()), s2.c_str()) == 0;
|
||||
}
|
||||
|
||||
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 > 2023) || (year == 2023 && month >= 6);
|
||||
}
|
||||
|
||||
} // namespace util
|
||||
184
common/util.h
184
common/util.h
@@ -1,184 +0,0 @@
|
||||
#pragma once
|
||||
|
||||
#include <fcntl.h>
|
||||
#include <sys/stat.h>
|
||||
#include <unistd.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;
|
||||
const double FOOT_TO_METER = 1 / METER_TO_FOOT;
|
||||
const double CM_TO_INCH = METER_TO_FOOT / 100.0 * 12.0;
|
||||
const double INCH_TO_CM = 1.0 / CM_TO_INCH;
|
||||
|
||||
namespace util {
|
||||
|
||||
void set_thread_name(const char* name);
|
||||
int set_realtime_priority(int level);
|
||||
int set_core_affinity(std::vector<int> cores);
|
||||
int set_file_descriptor_limit(uint64_t limit);
|
||||
|
||||
// ***** 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, std::string 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);
|
||||
bool starts_with(const std::string &s1, const std::string &s2);
|
||||
bool ends_with(const std::string &s1, const std::string &s2);
|
||||
|
||||
// ***** random helpers *****
|
||||
int random_int(int min, int max);
|
||||
std::string random_string(std::string::size_type length);
|
||||
|
||||
// **** file helpers *****
|
||||
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);
|
||||
int safe_ioctl(int fd, unsigned long request, void *argp);
|
||||
|
||||
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, bool initialized = true) {
|
||||
k_ = (dt / ts) / (1.0 + dt / ts);
|
||||
x_ = x0;
|
||||
initialized_ = initialized;
|
||||
}
|
||||
inline float update(float x) {
|
||||
if (initialized_) {
|
||||
x_ = (1. - k_) * x_ + k_ * x;
|
||||
} else {
|
||||
initialized_ = true;
|
||||
x_ = x;
|
||||
}
|
||||
return x_;
|
||||
}
|
||||
inline void reset(float x) { x_ = x; }
|
||||
inline float x(){ return x_; }
|
||||
|
||||
private:
|
||||
float x_, k_;
|
||||
bool initialized_;
|
||||
};
|
||||
|
||||
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)) {}
|
||||
}
|
||||
@@ -1,11 +0,0 @@
|
||||
#include <string>
|
||||
|
||||
#include "common/watchdog.h"
|
||||
#include "common/util.h"
|
||||
|
||||
const std::string watchdog_fn_prefix = "/dev/shm/wd_"; // + <pid>
|
||||
|
||||
bool watchdog_kick(uint64_t ts) {
|
||||
static std::string fn = watchdog_fn_prefix + std::to_string(getpid());
|
||||
return util::write_file(fn.c_str(), &ts, sizeof(ts), O_WRONLY | O_CREAT) > 0;
|
||||
}
|
||||
@@ -1,5 +0,0 @@
|
||||
#pragma once
|
||||
|
||||
#include <cstdint>
|
||||
|
||||
bool watchdog_kick(uint64_t ts);
|
||||
Reference in New Issue
Block a user