mawei/dragonpilot
1
0
Fork 0
mirror of https://github.com/dragonpilot/dragonpilot.git synced 2026-08-21 16:23:50 +08:00
Code Issues Packages Projects Releases Wiki Activity

dragonpilot beta3

Browse Source 
date: 2023-12-23T21:19:29
commit: 38612b14f1a8aa49d1c6ef61bd67f5a095abb3f9
This commit is contained in:
dragonpilot
2023-12-23 21:18:48 -07:00
parent dd4c663a58
commit 7ee7cb59b3
389 changed files with 36391 additions and 48841 deletions
Download Patch File Download Diff File Expand all files Collapse all files
system/camerad/camerad
BIN
View File
Binary file not shown.
system/camerad/cameras/camera_common.h
+4 -25
View File
@@ -14,19 +14,7 @@
#include "common/swaglog.h"
#include "system/hardware/hw.h"
#define CAMERA_ID_IMX298 0
#define CAMERA_ID_IMX179 1
#define CAMERA_ID_S5K3P8SP 2
#define CAMERA_ID_OV8865 3
#define CAMERA_ID_IMX298_FLIPPED 4
#define CAMERA_ID_OV10640 5
#define CAMERA_ID_LGC920 6
#define CAMERA_ID_LGC615 7
#define CAMERA_ID_AR0231 8
#define CAMERA_ID_OX03C10 9
#define CAMERA_ID_MAX 10
const int YUV_BUFFER_COUNT = 40;
const int YUV_BUFFER_COUNT = 20;
enum CameraType {
RoadCam = 0,
@@ -42,20 +30,11 @@ const bool env_debug_frames = getenv("DEBUG_FRAMES") != NULL;
const bool env_log_raw_frames = getenv("LOG_RAW_FRAMES") != NULL;
const bool env_ctrl_exp_from_params = getenv("CTRL_EXP_FROM_PARAMS") != NULL;
typedef struct CameraInfo {
uint32_t frame_width, frame_height;
uint32_t frame_stride;
uint32_t frame_offset = 0;
uint32_t extra_height = 0;
int registers_offset = -1;
int stats_offset = -1;
} CameraInfo;
typedef struct FrameMetadata {
uint32_t frame_id;
// Timestamps
uint64_t timestamp_sof; // only set on tici
uint64_t timestamp_sof;
uint64_t timestamp_eof;
// Exposure
@@ -76,7 +55,7 @@ class CameraBuf {
private:
VisionIpcServer *vipc_server;
Debayer *debayer = nullptr;
VisionStreamType yuv_type;
VisionStreamType stream_type;
int cur_buf_idx;
SafeQueue<int> safe_queue;
int frame_buf_count;
@@ -92,7 +71,7 @@ public:
CameraBuf() = default;
~CameraBuf();
void init(cl_device_id device_id, cl_context context, CameraState *s, VisionIpcServer * v, int frame_cnt, VisionStreamType yuv_type);
void init(cl_device_id device_id, cl_context context, CameraState *s, VisionIpcServer * v, int frame_cnt, VisionStreamType type);
bool acquire();
void queue(size_t buf_idx);
};
system/camerad/cameras/camera_qcom2.h
+9 -32
View File
@@ -1,23 +1,23 @@
#pragma once
#include <cstdint>
#include <map>
#include <memory>
#include <utility>
#include <media/cam_req_mgr.h>
#include "system/camerad/cameras/camera_common.h"
#include "system/camerad/cameras/camera_util.h"
#include "system/camerad/sensors/sensor.h"
#include "common/params.h"
#include "common/util.h"
#define FRAME_BUF_COUNT 4
#define ANALOG_GAIN_MAX_CNT 55
class CameraState {
public:
MultiCameraState *multi_cam_state;
CameraInfo ci;
std::unique_ptr<const SensorInfo> ci;
bool enabled;
std::mutex exp_lock;
@@ -28,32 +28,13 @@ public:
int gain_idx;
float analog_gain_frac;
int exposure_time_min;
int exposure_time_max;
float dc_gain_factor;
int dc_gain_min_weight;
int dc_gain_max_weight;
float dc_gain_on_grey;
float dc_gain_off_grey;
float sensor_analog_gains[ANALOG_GAIN_MAX_CNT];
int analog_gain_min_idx;
int analog_gain_max_idx;
int analog_gain_rec_idx;
int analog_gain_cost_delta;
float analog_gain_cost_low;
float analog_gain_cost_high;
float cur_ev[3];
float min_ev, max_ev;
float best_ev_score;
int new_exp_g;
int new_exp_t;
float measured_grey_fraction;
float target_grey_fraction;
float target_grey_factor;
unique_fd sensor_fd;
unique_fd csiphy_fd;
@@ -67,13 +48,11 @@ public:
void sensors_start();
void camera_open(MultiCameraState *multi_cam_state, int camera_num, bool enabled);
void camera_set_parameters();
void sensor_set_parameters();
void camera_map_bufs(MultiCameraState *s);
void camera_init(MultiCameraState *s, VisionIpcServer *v, int camera_id, unsigned int fps, cl_device_id device_id, cl_context ctx, VisionStreamType yuv_type);
void camera_init(MultiCameraState *s, VisionIpcServer *v, cl_device_id device_id, cl_context ctx, VisionStreamType yuv_type);
void camera_close();
std::map<uint16_t, uint16_t> ar0231_parse_registers(uint8_t *data, std::initializer_list<uint16_t> addrs);
int32_t session_handle;
int32_t sensor_dev_handle;
int32_t isp_dev_handle;
@@ -89,12 +68,10 @@ public:
int frame_id_last;
int idx_offset;
bool skipped;
int camera_id;
CameraBuf buf;
MemoryManager mm;
private:
void config_isp(int io_mem_handle, int fence, int request_id, int buf0_mem_handle, int buf0_offset);
void enqueue_req_multi(int start, int n, bool dp);
void enqueue_buffer(int i, bool dp);
@@ -102,11 +79,11 @@ private:
int sensors_init();
void sensors_poke(int request_id);
void sensors_i2c(struct i2c_random_wr_payload* dat, int len, int op_code, bool data_word);
void sensors_i2c(const struct i2c_random_wr_payload* dat, int len, int op_code, bool data_word);
// Register parsing
std::map<uint16_t, std::pair<int, int>> ar0231_register_lut;
std::map<uint16_t, std::pair<int, int>> ar0231_build_register_lut(uint8_t *data);
private:
// for debugging
Params params;
};
typedef struct MultiCameraState {
system/camerad/cameras/camera_util.h
+1 -1
View File
@@ -1,6 +1,6 @@
#pragma once
#include <functional>
#include <functional>
#include <map>
#include <memory>
#include <mutex>
system/camerad/imgproc/conv.cl
-110
View File
@@ -1,110 +0,0 @@
// const __constant float3 rgb_weights = (0.299, 0.587, 0.114); // opencv rgb2gray weights
// const __constant float3 bgr_weights = (0.114, 0.587, 0.299); // bgr2gray weights
// convert input rgb image to single channel then conv
__kernel void rgb2gray_conv2d(
const __global uchar * input,
__global short * output,
__constant short * filter,
__local uchar3 * cached
)
{
const int rowOffset = get_global_id(1) * IMAGE_W;
const int my = get_global_id(0) + rowOffset;
const int localRowLen = TWICE_HALF_FILTER_SIZE + get_local_size(0);
const int localRowOffset = ( get_local_id(1) + HALF_FILTER_SIZE ) * localRowLen;
const int myLocal = localRowOffset + get_local_id(0) + HALF_FILTER_SIZE;
// cache local pixels
cached[ myLocal ].x = input[ my * 3 ]; // r
cached[ myLocal ].y = input[ my * 3 + 1]; // g
cached[ myLocal ].z = input[ my * 3 + 2]; // b
// pad
if (
get_global_id(0) < HALF_FILTER_SIZE ||
get_global_id(0) > IMAGE_W - HALF_FILTER_SIZE - 1 ||
get_global_id(1) < HALF_FILTER_SIZE ||
get_global_id(1) > IMAGE_H - HALF_FILTER_SIZE - 1
)
{
barrier(CLK_LOCAL_MEM_FENCE);
return;
}
else
{
int localColOffset = -1;
int globalColOffset = -1;
// cache extra
if ( get_local_id(0) < HALF_FILTER_SIZE )
{
localColOffset = get_local_id(0);
globalColOffset = -HALF_FILTER_SIZE;
cached[ localRowOffset + get_local_id(0) ].x = input[ my * 3 - HALF_FILTER_SIZE * 3 ];
cached[ localRowOffset + get_local_id(0) ].y = input[ my * 3 - HALF_FILTER_SIZE * 3 + 1];
cached[ localRowOffset + get_local_id(0) ].z = input[ my * 3 - HALF_FILTER_SIZE * 3 + 2];
}
else if ( get_local_id(0) >= get_local_size(0) - HALF_FILTER_SIZE )
{
localColOffset = get_local_id(0) + TWICE_HALF_FILTER_SIZE;
globalColOffset = HALF_FILTER_SIZE;
cached[ myLocal + HALF_FILTER_SIZE ].x = input[ my * 3 + HALF_FILTER_SIZE * 3 ];
cached[ myLocal + HALF_FILTER_SIZE ].y = input[ my * 3 + HALF_FILTER_SIZE * 3 + 1];
cached[ myLocal + HALF_FILTER_SIZE ].z = input[ my * 3 + HALF_FILTER_SIZE * 3 + 2];
}
if ( get_local_id(1) < HALF_FILTER_SIZE )
{
cached[ get_local_id(1) * localRowLen + get_local_id(0) + HALF_FILTER_SIZE ].x = input[ my * 3 - HALF_FILTER_SIZE_IMAGE_W * 3 ];
cached[ get_local_id(1) * localRowLen + get_local_id(0) + HALF_FILTER_SIZE ].y = input[ my * 3 - HALF_FILTER_SIZE_IMAGE_W * 3 + 1];
cached[ get_local_id(1) * localRowLen + get_local_id(0) + HALF_FILTER_SIZE ].z = input[ my * 3 - HALF_FILTER_SIZE_IMAGE_W * 3 + 2];
if (localColOffset > 0)
{
cached[ get_local_id(1) * localRowLen + localColOffset ].x = input[ my * 3 - HALF_FILTER_SIZE_IMAGE_W * 3 + globalColOffset * 3];
cached[ get_local_id(1) * localRowLen + localColOffset ].y = input[ my * 3 - HALF_FILTER_SIZE_IMAGE_W * 3 + globalColOffset * 3 + 1];
cached[ get_local_id(1) * localRowLen + localColOffset ].z = input[ my * 3 - HALF_FILTER_SIZE_IMAGE_W * 3 + globalColOffset * 3 + 2];
}
}
else if ( get_local_id(1) >= get_local_size(1) -HALF_FILTER_SIZE )
{
int offset = ( get_local_id(1) + TWICE_HALF_FILTER_SIZE ) * localRowLen;
cached[ offset + get_local_id(0) + HALF_FILTER_SIZE ].x = input[ my * 3 + HALF_FILTER_SIZE_IMAGE_W * 3 ];
cached[ offset + get_local_id(0) + HALF_FILTER_SIZE ].y = input[ my * 3 + HALF_FILTER_SIZE_IMAGE_W * 3 + 1];
cached[ offset + get_local_id(0) + HALF_FILTER_SIZE ].z = input[ my * 3 + HALF_FILTER_SIZE_IMAGE_W * 3 + 2];
if (localColOffset > 0)
{
cached[ offset + localColOffset ].x = input[ my * 3 + HALF_FILTER_SIZE_IMAGE_W * 3 + globalColOffset * 3];
cached[ offset + localColOffset ].y = input[ my * 3 + HALF_FILTER_SIZE_IMAGE_W * 3 + globalColOffset * 3 + 1];
cached[ offset + localColOffset ].z = input[ my * 3 + HALF_FILTER_SIZE_IMAGE_W * 3 + globalColOffset * 3 + 2];
}
}
// sync
barrier(CLK_LOCAL_MEM_FENCE);
// perform convolution
int fIndex = 0;
short sum = 0;
for (int r = -HALF_FILTER_SIZE; r <= HALF_FILTER_SIZE; r++)
{
int curRow = r * localRowLen;
for (int c = -HALF_FILTER_SIZE; c <= HALF_FILTER_SIZE; c++, fIndex++)
{
if (!FLIP_RB){
// sum += dot(rgb_weights, cached[ myLocal + curRow + c ]) * filter[ fIndex ];
sum += (cached[ myLocal + curRow + c ].x / 3 + cached[ myLocal + curRow + c ].y / 2 + cached[ myLocal + curRow + c ].z / 9) * filter[ fIndex ];
} else {
// sum += dot(bgr_weights, cached[ myLocal + curRow + c ]) * filter[ fIndex ];
sum += (cached[ myLocal + curRow + c ].x / 9 + cached[ myLocal + curRow + c ].y / 2 + cached[ myLocal + curRow + c ].z / 3) * filter[ fIndex ];
}
}
}
output[my] = sum;
}
}
system/camerad/imgproc/pool.cl
-34
View File
@@ -1,34 +0,0 @@
// calculate variance in each subregion
__kernel void var_pool(
const __global char * input,
__global ushort * output // should not be larger than 128*128 so uint16
)
{
const int xidx = get_global_id(0) + ROI_X_MIN;
const int yidx = get_global_id(1) + ROI_Y_MIN;
const int size = X_PITCH * Y_PITCH;
float fsum = 0;
char mean, max;
for (int i = 0; i < size; i++) {
int x_offset = i % X_PITCH;
int y_offset = i / X_PITCH;
fsum += input[xidx*X_PITCH + yidx*Y_PITCH*FULL_STRIDE_X + x_offset + y_offset*FULL_STRIDE_X];
max = input[xidx*X_PITCH + yidx*Y_PITCH*FULL_STRIDE_X + x_offset + y_offset*FULL_STRIDE_X]>max ? input[xidx*X_PITCH + yidx*Y_PITCH*FULL_STRIDE_X + x_offset + y_offset*FULL_STRIDE_X]:max;
}
mean = convert_char_rte(fsum / size);
float fvar = 0;
for (int i = 0; i < size; i++) {
int x_offset = i % X_PITCH;
int y_offset = i / X_PITCH;
fvar += (input[xidx*X_PITCH + yidx*Y_PITCH*FULL_STRIDE_X + x_offset + y_offset*FULL_STRIDE_X] - mean) * (input[xidx*X_PITCH + yidx*Y_PITCH*FULL_STRIDE_X + x_offset + y_offset*FULL_STRIDE_X] - mean);
}
fvar = fvar / size;
output[(xidx-ROI_X_MIN)+(yidx-ROI_Y_MIN)*(ROI_X_MAX-ROI_X_MIN+1)] = convert_ushort_rte(5 * fvar + convert_float_rte(max));
}
system/camerad/imgproc/utils.h
-37
View File
@@ -1,37 +0,0 @@
#pragma once
#include <cstddef>
#include <cstdint>
#include <vector>
#include "common/clutil.h"
#define NUM_SEGMENTS_X 8
#define NUM_SEGMENTS_Y 6
#define ROI_X_MIN 1
#define ROI_X_MAX 6
#define ROI_Y_MIN 2
#define ROI_Y_MAX 3
#define LM_THRESH 120
#define LM_PREC_THRESH 0.9 // 90 perc is blur
#define CONV_LOCAL_WORKSIZE 16
class LapConv {
public:
LapConv(cl_device_id device_id, cl_context ctx, int rgb_width, int rgb_height, int rgb_stride, int filter_size);
~LapConv();
uint16_t Update(cl_command_queue q, const uint8_t *rgb_buf, const int roi_id);
private:
cl_mem roi_cl, result_cl, filter_cl;
cl_program prg;
cl_kernel krnl;
const int width, height;
const int rgb_stride;
std::vector<uint8_t> roi_buf;
std::vector<int16_t> result_buf;
};
bool is_blur(const uint16_t *lapmap, const size_t size);
system/camerad/sensors/ar0231_registers.h
+119
View File
@@ -0,0 +1,119 @@
#pragma once
const struct i2c_random_wr_payload start_reg_array_ar0231[] = {{0x301A, 0x91C}};
const struct i2c_random_wr_payload stop_reg_array_ar0231[] = {{0x301A, 0x918}};
const struct i2c_random_wr_payload init_array_ar0231[] = {
{0x301A, 0x0018}, // RESET_REGISTER
// CLOCK Settings
// input clock is 19.2 / 2 * 0x37 = 528 MHz
// pixclk is 528 / 6 = 88 MHz
// full roll time is 1000/(PIXCLK/(LINE_LENGTH_PCK*FRAME_LENGTH_LINES)) = 39.99 ms
// img roll time is 1000/(PIXCLK/(LINE_LENGTH_PCK*Y_OUTPUT_CONTROL)) = 22.85 ms
{0x302A, 0x0006}, // VT_PIX_CLK_DIV
{0x302C, 0x0001}, // VT_SYS_CLK_DIV
{0x302E, 0x0002}, // PRE_PLL_CLK_DIV
{0x3030, 0x0037}, // PLL_MULTIPLIER
{0x3036, 0x000C}, // OP_PIX_CLK_DIV
{0x3038, 0x0001}, // OP_SYS_CLK_DIV
// FORMAT
{0x3040, 0xC000}, // READ_MODE
{0x3004, 0x0000}, // X_ADDR_START_
{0x3008, 0x0787}, // X_ADDR_END_
{0x3002, 0x0000}, // Y_ADDR_START_
{0x3006, 0x04B7}, // Y_ADDR_END_
{0x3032, 0x0000}, // SCALING_MODE
{0x30A2, 0x0001}, // X_ODD_INC_
{0x30A6, 0x0001}, // Y_ODD_INC_
{0x3402, 0x0788}, // X_OUTPUT_CONTROL
{0x3404, 0x04B8}, // Y_OUTPUT_CONTROL
{0x3064, 0x1982}, // SMIA_TEST
{0x30BA, 0x11F2}, // DIGITAL_CTRL
// Enable external trigger and disable GPIO outputs
{0x30CE, 0x0120}, // SLAVE_SH_SYNC_MODE | FRAME_START_MODE
{0x340A, 0xE0}, // GPIO3_INPUT_DISABLE | GPIO2_INPUT_DISABLE | GPIO1_INPUT_DISABLE
{0x340C, 0x802}, // GPIO_HIDRV_EN | GPIO0_ISEL=2
// Readout timing
{0x300C, 0x0672}, // LINE_LENGTH_PCK (valid for 3-exposure HDR)
{0x300A, 0x0855}, // FRAME_LENGTH_LINES
{0x3042, 0x0000}, // EXTRA_DELAY
// Readout Settings
{0x31AE, 0x0204}, // SERIAL_FORMAT, 4-lane MIPI
{0x31AC, 0x0C0C}, // DATA_FORMAT_BITS, 12 -> 12
{0x3342, 0x1212}, // MIPI_F1_PDT_EDT
{0x3346, 0x1212}, // MIPI_F2_PDT_EDT
{0x334A, 0x1212}, // MIPI_F3_PDT_EDT
{0x334E, 0x1212}, // MIPI_F4_PDT_EDT
{0x3344, 0x0011}, // MIPI_F1_VDT_VC
{0x3348, 0x0111}, // MIPI_F2_VDT_VC
{0x334C, 0x0211}, // MIPI_F3_VDT_VC
{0x3350, 0x0311}, // MIPI_F4_VDT_VC
{0x31B0, 0x0053}, // FRAME_PREAMBLE
{0x31B2, 0x003B}, // LINE_PREAMBLE
{0x301A, 0x001C}, // RESET_REGISTER
// Noise Corrections
{0x3092, 0x0C24}, // ROW_NOISE_CONTROL
{0x337A, 0x0C80}, // DBLC_SCALE0
{0x3370, 0x03B1}, // DBLC
{0x3044, 0x0400}, // DARK_CONTROL
// Enable temperature sensor
{0x30B4, 0x0007}, // TEMPSENS0_CTRL_REG
{0x30B8, 0x0007}, // TEMPSENS1_CTRL_REG
// Enable dead pixel correction using
// the 1D line correction scheme
{0x31E0, 0x0003},
// HDR Settings
{0x3082, 0x0004}, // OPERATION_MODE_CTRL
{0x3238, 0x0444}, // EXPOSURE_RATIO
{0x1008, 0x0361}, // FINE_INTEGRATION_TIME_MIN
{0x100C, 0x0589}, // FINE_INTEGRATION_TIME2_MIN
{0x100E, 0x07B1}, // FINE_INTEGRATION_TIME3_MIN
{0x1010, 0x0139}, // FINE_INTEGRATION_TIME4_MIN
// TODO: do these have to be lower than LINE_LENGTH_PCK?
{0x3014, 0x08CB}, // FINE_INTEGRATION_TIME_
{0x321E, 0x0894}, // FINE_INTEGRATION_TIME2
{0x31D0, 0x0000}, // COMPANDING, no good in 10 bit?
{0x33DA, 0x0000}, // COMPANDING
{0x318E, 0x0200}, // PRE_HDR_GAIN_EN
// DLO Settings
{0x3100, 0x4000}, // DLO_CONTROL0
{0x3280, 0x0CCC}, // T1 G1
{0x3282, 0x0CCC}, // T1 R
{0x3284, 0x0CCC}, // T1 B
{0x3286, 0x0CCC}, // T1 G2
{0x3288, 0x0FA0}, // T2 G1
{0x328A, 0x0FA0}, // T2 R
{0x328C, 0x0FA0}, // T2 B
{0x328E, 0x0FA0}, // T2 G2
// Initial Gains
{0x3022, 0x0001}, // GROUPED_PARAMETER_HOLD_
{0x3366, 0xFF77}, // ANALOG_GAIN (1x)
{0x3060, 0x3333}, // ANALOG_COLOR_GAIN
{0x3362, 0x0000}, // DC GAIN
{0x305A, 0x00F8}, // red gain
{0x3058, 0x0122}, // blue gain
{0x3056, 0x009A}, // g1 gain
{0x305C, 0x009A}, // g2 gain
{0x3022, 0x0000}, // GROUPED_PARAMETER_HOLD_
// Initial Integration Time
{0x3012, 0x0005},
};
system/camerad/cameras/sensor2_i2c.h → system/camerad/sensors/ox03c10_registers.h
+5 -120
View File
@@ -1,9 +1,9 @@
struct i2c_random_wr_payload start_reg_array_ar0231[] = {{0x301A, 0x91C}};
struct i2c_random_wr_payload stop_reg_array_ar0231[] = {{0x301A, 0x918}};
struct i2c_random_wr_payload start_reg_array_ox03c10[] = {{0x100, 1}};
struct i2c_random_wr_payload stop_reg_array_ox03c10[] = {{0x100, 0}};
#pragma once
struct i2c_random_wr_payload init_array_ox03c10[] = {
const struct i2c_random_wr_payload start_reg_array_ox03c10[] = {{0x100, 1}};
const struct i2c_random_wr_payload stop_reg_array_ox03c10[] = {{0x100, 0}};
const struct i2c_random_wr_payload init_array_ox03c10[] = {
{0x103, 1},
{0x107, 1},
@@ -759,118 +759,3 @@ struct i2c_random_wr_payload init_array_ox03c10[] = {
{0x3548, 0x0F}, {0x3549, 0x00},
{0x35c1, 0x00},
};
struct i2c_random_wr_payload init_array_ar0231[] = {
{0x301A, 0x0018}, // RESET_REGISTER
// CLOCK Settings
// input clock is 19.2 / 2 * 0x37 = 528 MHz
// pixclk is 528 / 6 = 88 MHz
// full roll time is 1000/(PIXCLK/(LINE_LENGTH_PCK*FRAME_LENGTH_LINES)) = 39.99 ms
// img roll time is 1000/(PIXCLK/(LINE_LENGTH_PCK*Y_OUTPUT_CONTROL)) = 22.85 ms
{0x302A, 0x0006}, // VT_PIX_CLK_DIV
{0x302C, 0x0001}, // VT_SYS_CLK_DIV
{0x302E, 0x0002}, // PRE_PLL_CLK_DIV
{0x3030, 0x0037}, // PLL_MULTIPLIER
{0x3036, 0x000C}, // OP_PIX_CLK_DIV
{0x3038, 0x0001}, // OP_SYS_CLK_DIV
// FORMAT
{0x3040, 0xC000}, // READ_MODE
{0x3004, 0x0000}, // X_ADDR_START_
{0x3008, 0x0787}, // X_ADDR_END_
{0x3002, 0x0000}, // Y_ADDR_START_
{0x3006, 0x04B7}, // Y_ADDR_END_
{0x3032, 0x0000}, // SCALING_MODE
{0x30A2, 0x0001}, // X_ODD_INC_
{0x30A6, 0x0001}, // Y_ODD_INC_
{0x3402, 0x0788}, // X_OUTPUT_CONTROL
{0x3404, 0x04B8}, // Y_OUTPUT_CONTROL
{0x3064, 0x1982}, // SMIA_TEST
{0x30BA, 0x11F2}, // DIGITAL_CTRL
// Enable external trigger and disable GPIO outputs
{0x30CE, 0x0120}, // SLAVE_SH_SYNC_MODE | FRAME_START_MODE
{0x340A, 0xE0}, // GPIO3_INPUT_DISABLE | GPIO2_INPUT_DISABLE | GPIO1_INPUT_DISABLE
{0x340C, 0x802}, // GPIO_HIDRV_EN | GPIO0_ISEL=2
// Readout timing
{0x300C, 0x0672}, // LINE_LENGTH_PCK (valid for 3-exposure HDR)
{0x300A, 0x0855}, // FRAME_LENGTH_LINES
{0x3042, 0x0000}, // EXTRA_DELAY
// Readout Settings
{0x31AE, 0x0204}, // SERIAL_FORMAT, 4-lane MIPI
{0x31AC, 0x0C0C}, // DATA_FORMAT_BITS, 12 -> 12
{0x3342, 0x1212}, // MIPI_F1_PDT_EDT
{0x3346, 0x1212}, // MIPI_F2_PDT_EDT
{0x334A, 0x1212}, // MIPI_F3_PDT_EDT
{0x334E, 0x1212}, // MIPI_F4_PDT_EDT
{0x3344, 0x0011}, // MIPI_F1_VDT_VC
{0x3348, 0x0111}, // MIPI_F2_VDT_VC
{0x334C, 0x0211}, // MIPI_F3_VDT_VC
{0x3350, 0x0311}, // MIPI_F4_VDT_VC
{0x31B0, 0x0053}, // FRAME_PREAMBLE
{0x31B2, 0x003B}, // LINE_PREAMBLE
{0x301A, 0x001C}, // RESET_REGISTER
// Noise Corrections
{0x3092, 0x0C24}, // ROW_NOISE_CONTROL
{0x337A, 0x0C80}, // DBLC_SCALE0
{0x3370, 0x03B1}, // DBLC
{0x3044, 0x0400}, // DARK_CONTROL
// Enable temperature sensor
{0x30B4, 0x0007}, // TEMPSENS0_CTRL_REG
{0x30B8, 0x0007}, // TEMPSENS1_CTRL_REG
// Enable dead pixel correction using
// the 1D line correction scheme
{0x31E0, 0x0003},
// HDR Settings
{0x3082, 0x0004}, // OPERATION_MODE_CTRL
{0x3238, 0x0444}, // EXPOSURE_RATIO
{0x1008, 0x0361}, // FINE_INTEGRATION_TIME_MIN
{0x100C, 0x0589}, // FINE_INTEGRATION_TIME2_MIN
{0x100E, 0x07B1}, // FINE_INTEGRATION_TIME3_MIN
{0x1010, 0x0139}, // FINE_INTEGRATION_TIME4_MIN
// TODO: do these have to be lower than LINE_LENGTH_PCK?
{0x3014, 0x08CB}, // FINE_INTEGRATION_TIME_
{0x321E, 0x0894}, // FINE_INTEGRATION_TIME2
{0x31D0, 0x0000}, // COMPANDING, no good in 10 bit?
{0x33DA, 0x0000}, // COMPANDING
{0x318E, 0x0200}, // PRE_HDR_GAIN_EN
// DLO Settings
{0x3100, 0x4000}, // DLO_CONTROL0
{0x3280, 0x0CCC}, // T1 G1
{0x3282, 0x0CCC}, // T1 R
{0x3284, 0x0CCC}, // T1 B
{0x3286, 0x0CCC}, // T1 G2
{0x3288, 0x0FA0}, // T2 G1
{0x328A, 0x0FA0}, // T2 R
{0x328C, 0x0FA0}, // T2 B
{0x328E, 0x0FA0}, // T2 G2
// Initial Gains
{0x3022, 0x0001}, // GROUPED_PARAMETER_HOLD_
{0x3366, 0xFF77}, // ANALOG_GAIN (1x)
{0x3060, 0x3333}, // ANALOG_COLOR_GAIN
{0x3362, 0x0000}, // DC GAIN
{0x305A, 0x00F8}, // red gain
{0x3058, 0x0122}, // blue gain
{0x3056, 0x009A}, // g1 gain
{0x305C, 0x009A}, // g2 gain
{0x3022, 0x0000}, // GROUPED_PARAMETER_HOLD_
// Initial Integration Time
{0x3012, 0x0005},
};
system/camerad/sensors/sensor.h
+81
View File
@@ -0,0 +1,81 @@
#pragma once
#include <cassert>
#include <cstdint>
#include <map>
#include <utility>
#include <vector>
#include "media/cam_sensor.h"
#include "system/camerad/cameras/camera_common.h"
#include "system/camerad/sensors/ar0231_registers.h"
#include "system/camerad/sensors/ox03c10_registers.h"
#define ANALOG_GAIN_MAX_CNT 55
const size_t FRAME_WIDTH = 1928;
const size_t FRAME_HEIGHT = 1208;
const size_t FRAME_STRIDE = 2896; // for 12 bit output. 1928 * 12 / 8 + 4 (alignment)
class SensorInfo {
public:
SensorInfo() = default;
virtual std::vector<i2c_random_wr_payload> getExposureRegisters(int exposure_time, int new_exp_g, bool dc_gain_enabled) const { return {}; }
virtual float getExposureScore(float desired_ev, int exp_t, int exp_g_idx, float exp_gain, int gain_idx) const {return 0; }
virtual int getSlaveAddress(int port) const { assert(0); }
virtual void processRegisters(CameraState *c, cereal::FrameData::Builder &framed) const {}
cereal::FrameData::ImageSensor image_sensor = cereal::FrameData::ImageSensor::UNKNOWN;
uint32_t frame_width, frame_height;
uint32_t frame_stride;
uint32_t frame_offset = 0;
uint32_t extra_height = 0;
int registers_offset = -1;
int stats_offset = -1;
int exposure_time_min;
int exposure_time_max;
float dc_gain_factor;
int dc_gain_min_weight;
int dc_gain_max_weight;
float dc_gain_on_grey;
float dc_gain_off_grey;
float sensor_analog_gains[ANALOG_GAIN_MAX_CNT];
int analog_gain_min_idx;
int analog_gain_max_idx;
int analog_gain_rec_idx;
int analog_gain_cost_delta;
float analog_gain_cost_low;
float analog_gain_cost_high;
float target_grey_factor;
float min_ev;
float max_ev;
bool data_word;
uint32_t probe_reg_addr;
uint32_t probe_expected_data;
std::vector<i2c_random_wr_payload> start_reg_array;
std::vector<i2c_random_wr_payload> init_reg_array;
uint32_t in_port_info_dt;
uint32_t power_config_val_low;
};
class AR0231 : public SensorInfo {
public:
AR0231();
std::vector<i2c_random_wr_payload> getExposureRegisters(int exposure_time, int new_exp_g, bool dc_gain_enabled) const override;
float getExposureScore(float desired_ev, int exp_t, int exp_g_idx, float exp_gain, int gain_idx) const override;
int getSlaveAddress(int port) const override;
void processRegisters(CameraState *c, cereal::FrameData::Builder &framed) const override;
private:
mutable std::map<uint16_t, std::pair<int, int>> ar0231_register_lut;
};
class OX03C10 : public SensorInfo {
public:
OX03C10();
std::vector<i2c_random_wr_payload> getExposureRegisters(int exposure_time, int new_exp_g, bool dc_gain_enabled) const override;
float getExposureScore(float desired_ev, int exp_t, int exp_g_idx, float exp_gain, int gain_idx) const override;
int getSlaveAddress(int port) const override;
};
system/camerad/snapshot/snapshot.py
-1
View File
@@ -13,7 +13,6 @@ from openpilot.system.hardware import PC
from openpilot.selfdrive.controls.lib.alertmanager import set_offroad_alert
from openpilot.selfdrive.manager.process_config import managed_processes
LM_THRESH = 120 # defined in system/camerad/imgproc/utils.h
VISION_STREAMS = {
"roadCameraState": VisionStreamType.VISION_STREAM_ROAD,
system/hardware/base.h
+1 -2
View File
@@ -29,12 +29,11 @@ public:
static void poweroff() {}
static void set_brightness(int percent) {}
static void set_display_power(bool on) {}
static void set_volume(float volume) {}
static bool get_ssh_enabled() { return false; }
static void set_ssh_enabled(bool enabled) {}
static void config_cpu_rendering();
static void config_cpu_rendering(bool offscreen);
static bool PC() { return false; }
static bool TICI() { return false; }
system/hardware/hw.py
+21
View File
@@ -33,3 +33,24 @@ class Paths:
if os.environ.get('COMMA_CACHE', False):
return os.environ['COMMA_CACHE']
return "/tmp/comma_download_cache" + os.environ.get("OPENPILOT_PREFIX", "") + "/"
@staticmethod
def persist_root() -> str:
if PC:
return os.path.join(Paths.comma_home(), "persist")
else:
return "/persist/"
@staticmethod
def stats_root() -> str:
if PC:
return str(Path(Paths.comma_home()) / "stats")
else:
return "/data/stats/"
@staticmethod
def config_root() -> str:
if PC:
return Paths.comma_home()
else:
return "/tmp/.comma"
system/hardware/pc/hardware.h
+4 -10
View File
@@ -13,16 +13,10 @@ public:
static bool TICI() { return util::getenv("TICI", 0) == 1; }
static bool AGNOS() { return util::getenv("TICI", 0) == 1; }
static void set_volume(float volume) {
volume = util::map_val(volume, 0.f, 1.f, MIN_VOLUME, MAX_VOLUME);
char volume_str[6];
snprintf(volume_str, sizeof(volume_str), "%.3f", volume);
std::system(("pactl set-sink-volume @DEFAULT_SINK@ " + std::string(volume_str)).c_str());
}
static void config_cpu_rendering() {
setenv("QT_QPA_PLATFORM", "offscreen", 1);
static void config_cpu_rendering(bool offscreen) {
if (offscreen) {
setenv("QT_QPA_PLATFORM", "offscreen", 1);
}
setenv("__GLX_VENDOR_LIBRARY_NAME", "mesa", 1);
setenv("LP_NUM_THREADS", "0", 1); // disable threading so we stay on our assigned CPU
}
system/hardware/tici/agnos.json
+24 -24
View File
@@ -1,9 +1,9 @@
[
{
"name": "boot",
"url": "https://commadist.azureedge.net/agnosupdate/boot-8d8d8620de8b2687f3a8fffdb81b2abd1fe2ead5bc831361a1a212e5589ac279.img.xz",
"hash": "8d8d8620de8b2687f3a8fffdb81b2abd1fe2ead5bc831361a1a212e5589ac279",
"hash_raw": "8d8d8620de8b2687f3a8fffdb81b2abd1fe2ead5bc831361a1a212e5589ac279",
"url": "https://commadist.azureedge.net/agnosupdate/boot-fd30f580375279ff4605034ec13711890a2b227205571a087cdc5226a2710275.img.xz",
"hash": "fd30f580375279ff4605034ec13711890a2b227205571a087cdc5226a2710275",
"hash_raw": "fd30f580375279ff4605034ec13711890a2b227205571a087cdc5226a2710275",
"size": 15636480,
"sparse": false,
"full_check": true,
@@ -11,9 +11,9 @@
},
{
"name": "abl",
"url": "https://commadist.azureedge.net/agnosupdate/abl-0084fcf79fea067632a1c2d9519b6445ad484aa8b09f49f22e6b45b4dccacd2d.img.xz",
"hash": "0084fcf79fea067632a1c2d9519b6445ad484aa8b09f49f22e6b45b4dccacd2d",
"hash_raw": "0084fcf79fea067632a1c2d9519b6445ad484aa8b09f49f22e6b45b4dccacd2d",
"url": "https://commadist.azureedge.net/agnosupdate/abl-bb234733816781b3d09266f91f741436e9bf17e1a7caf468cf7d09ee788cef4a.img.xz",
"hash": "bb234733816781b3d09266f91f741436e9bf17e1a7caf468cf7d09ee788cef4a",
"hash_raw": "bb234733816781b3d09266f91f741436e9bf17e1a7caf468cf7d09ee788cef4a",
"size": 274432,
"sparse": false,
"full_check": true,
@@ -21,9 +21,9 @@
},
{
"name": "xbl",
"url": "https://commadist.azureedge.net/agnosupdate/xbl-942b9b2914d89c2a70fdf27380b59e04b549ac2fd53ecb29d6549d1a9c8daeaa.img.xz",
"hash": "942b9b2914d89c2a70fdf27380b59e04b549ac2fd53ecb29d6549d1a9c8daeaa",
"hash_raw": "942b9b2914d89c2a70fdf27380b59e04b549ac2fd53ecb29d6549d1a9c8daeaa",
"url": "https://commadist.azureedge.net/agnosupdate/xbl-bcef195b00a1ab685da601f4072722569773ab161e91c8753ad99ca4217a28f5.img.xz",
"hash": "bcef195b00a1ab685da601f4072722569773ab161e91c8753ad99ca4217a28f5",
"hash_raw": "bcef195b00a1ab685da601f4072722569773ab161e91c8753ad99ca4217a28f5",
"size": 3282672,
"sparse": false,
"full_check": true,
@@ -31,9 +31,9 @@
},
{
"name": "xbl_config",
"url": "https://commadist.azureedge.net/agnosupdate/xbl_config-6881d94599f65d94c13bcc0bd860184dfba2dfe96ec776d08fb35ac5b5f85bbf.img.xz",
"hash": "6881d94599f65d94c13bcc0bd860184dfba2dfe96ec776d08fb35ac5b5f85bbf",
"hash_raw": "6881d94599f65d94c13bcc0bd860184dfba2dfe96ec776d08fb35ac5b5f85bbf",
"url": "https://commadist.azureedge.net/agnosupdate/xbl_config-19791056558c16f8dae787531b5e30b3b3db2ded9d666688df45ce1b91a72bac.img.xz",
"hash": "19791056558c16f8dae787531b5e30b3b3db2ded9d666688df45ce1b91a72bac",
"hash_raw": "19791056558c16f8dae787531b5e30b3b3db2ded9d666688df45ce1b91a72bac",
"size": 98124,
"sparse": false,
"full_check": true,
@@ -41,9 +41,9 @@
},
{
"name": "devcfg",
"url": "https://commadist.azureedge.net/agnosupdate/devcfg-9bbf168baff6101f4890c5c95c118e30813c2610cfb35b8e19e363f04a32a262.img.xz",
"hash": "9bbf168baff6101f4890c5c95c118e30813c2610cfb35b8e19e363f04a32a262",
"hash_raw": "9bbf168baff6101f4890c5c95c118e30813c2610cfb35b8e19e363f04a32a262",
"url": "https://commadist.azureedge.net/agnosupdate/devcfg-be44b73dda5be840b09d5347d536459e31098da3fea97596956c0bdad19bdf27.img.xz",
"hash": "be44b73dda5be840b09d5347d536459e31098da3fea97596956c0bdad19bdf27",
"hash_raw": "be44b73dda5be840b09d5347d536459e31098da3fea97596956c0bdad19bdf27",
"size": 40336,
"sparse": false,
"full_check": true,
@@ -51,9 +51,9 @@
},
{
"name": "aop",
"url": "https://commadist.azureedge.net/agnosupdate/aop-c1d9d712980f6b2a4b12196597f4d1bf3fe4fec6c59edf29ae63ef21f11b8222.img.xz",
"hash": "c1d9d712980f6b2a4b12196597f4d1bf3fe4fec6c59edf29ae63ef21f11b8222",
"hash_raw": "c1d9d712980f6b2a4b12196597f4d1bf3fe4fec6c59edf29ae63ef21f11b8222",
"url": "https://commadist.azureedge.net/agnosupdate/aop-5d764611a683d6a738cf06a1dcf8a926d0f47b5117ad40d3054167de6dd8bd0f.img.xz",
"hash": "5d764611a683d6a738cf06a1dcf8a926d0f47b5117ad40d3054167de6dd8bd0f",
"hash_raw": "5d764611a683d6a738cf06a1dcf8a926d0f47b5117ad40d3054167de6dd8bd0f",
"size": 184364,
"sparse": false,
"full_check": true,
@@ -61,16 +61,16 @@
},
{
"name": "system",
"url": "https://commadist.azureedge.net/agnosupdate/system-e1fa3018bce9bad01c6967e5e21f1141cf5c8f02d2edfaed51c738f74a32a432.img.xz",
"hash": "611011f3e3f147bc24f371105a9dd3760ec11ba424c56d4a442a66b098c784c0",
"hash_raw": "e1fa3018bce9bad01c6967e5e21f1141cf5c8f02d2edfaed51c738f74a32a432",
"url": "https://commadist.azureedge.net/agnosupdate/system-e1952bb363688c0f5c0646e39bcdfb45be25b5e2baed37d1ba7801aa1a3a9c98.img.xz",
"hash": "3b6cdf9bd881a5e90b21dd02c6faa923b415e32ecae9bfdc96753d4208fb82fe",
"hash_raw": "e1952bb363688c0f5c0646e39bcdfb45be25b5e2baed37d1ba7801aa1a3a9c98",
"size": 10737418240,
"sparse": true,
"full_check": false,
"has_ab": true,
"alt": {
"hash": "256442a55fcb9e8f72969f003a4db91598dee1136f8dda85b553a557d36b93d8",
"url": "https://commadist.azureedge.net/agnosupdate/system-skip-chunks-e1fa3018bce9bad01c6967e5e21f1141cf5c8f02d2edfaed51c738f74a32a432.img.xz"
"hash": "2fb81e58f4bc6c4e5e71c8e7ac7553f85082c430627d7a5cc54a6bbc82862500",
"url": "https://commadist.azureedge.net/agnosupdate/system-skip-chunks-e1952bb363688c0f5c0646e39bcdfb45be25b5e2baed37d1ba7801aa1a3a9c98.img.xz"
}
}
]
]
system/hardware/tici/esim.nmconnection
+30
View File
@@ -0,0 +1,30 @@
[connection]
id=esim
uuid=fff6553c-3284-4707-a6b1-acc021caaafb
type=gsm
permissions=
autoconnect=true
autoconnect-retries=100
autoconnect-priority=2
metered=1
[gsm]
apn=
home-only=false
auto-config=true
sim-id=
[ipv4]
route-metric=1000
dns-priority=1000
dns-search=
method=auto
[ipv6]
ddr-gen-mode=stable-privacy
dns-search=
route-metric=1000
dns-priority=1000
method=auto
[proxy]
system/hardware/tici/hardware.h
+4 -10
View File
@@ -67,14 +67,6 @@ public:
bl_power_control.close();
}
}
static void set_volume(float volume) {
volume = util::map_val(volume, 0.f, 1.f, MIN_VOLUME, MAX_VOLUME);
char volume_str[6];
snprintf(volume_str, sizeof(volume_str), "%.3f", volume);
std::system(("pactl set-sink-volume @DEFAULT_SINK@ " + std::string(volume_str)).c_str());
}
static std::map<std::string, std::string> get_init_logs() {
std::map<std::string, std::string> ret = {
@@ -104,8 +96,10 @@ public:
static bool get_ssh_enabled() { return Params().getBool("SshEnabled"); }
static void set_ssh_enabled(bool enabled) { Params().putBool("SshEnabled", enabled); }
static void config_cpu_rendering() {
setenv("QT_QPA_PLATFORM", "eglfs", 1); // offscreen doesn't work with EGL/GLES
static void config_cpu_rendering(bool offscreen) {
if (offscreen) {
setenv("QT_QPA_PLATFORM", "eglfs", 1); // offscreen doesn't work with EGL/GLES
}
setenv("LP_NUM_THREADS", "0", 1); // disable threading so we stay on our assigned CPU
}
};
system/hardware/tici/hardware.py
+21 -66
View File
@@ -3,6 +3,7 @@ import math
import os
import subprocess
import time
import tempfile
from enum import IntEnum
from functools import cached_property, lru_cache
from pathlib import Path
@@ -104,7 +105,7 @@ class Tici(HardwareBase):
def nm(self):
return self.bus.get_object(NM, '/org/freedesktop/NetworkManager')
@cached_property
@property # this should not be cached, in case the modemmanager restarts
def mm(self):
return self.bus.get_object(MM, '/org/freedesktop/ModemManager1')
@@ -210,8 +211,8 @@ class Tici(HardwareBase):
return str(self.get_modem().Get(MM_MODEM, 'EquipmentIdentifier', dbus_interface=DBUS_PROPS, timeout=TIMEOUT))
def get_network_info(self):
modem = self.get_modem()
try:
modem = self.get_modem()
info = modem.Command("AT+QNWINFO", math.ceil(TIMEOUT), dbus_interface=MM_MODEM, timeout=TIMEOUT)
extra = modem.Command('AT+QENG="servingcell"', math.ceil(TIMEOUT), dbus_interface=MM_MODEM, timeout=TIMEOUT)
state = modem.Get(MM_MODEM, 'State', dbus_interface=DBUS_PROPS, timeout=TIMEOUT)
@@ -292,67 +293,6 @@ class Tici(HardwareBase):
return super().get_network_metered(network_type)
@staticmethod
def set_bandwidth_limit(upload_speed_kbps: int, download_speed_kbps: int) -> None:
upload_speed_kbps = int(upload_speed_kbps) # Ensure integer value
download_speed_kbps = int(download_speed_kbps) # Ensure integer value
adapter = "wwan0"
ifb = "ifb0"
sudo = ["sudo"]
tc = sudo + ["tc"]
# check, cmd
cleanup = [
# Clean up old rules
(False, tc + ["qdisc", "del", "dev", adapter, "root"]),
(False, tc + ["qdisc", "del", "dev", ifb, "root"]),
(False, tc + ["qdisc", "del", "dev", adapter, "ingress"]),
(False, tc + ["qdisc", "del", "dev", ifb, "ingress"]),
# Bring ifb0 down
(False, sudo + ["ip", "link", "set", "dev", ifb, "down"]),
]
upload = [
# Create root Hierarchy Token Bucket that sends all traffic to 1:20
(True, tc + ["qdisc", "add", "dev", adapter, "root", "handle", "1:", "htb", "default", "20"]),
# Create class 1:20 with specified rate limit
(True, tc + ["class", "add", "dev", adapter, "parent", "1:", "classid", "1:20", "htb", "rate", f"{upload_speed_kbps}kbit"]),
# Create universal 32 bit filter on adapter that sends all outbound ip traffic through the class
(True, tc + ["filter", "add", "dev", adapter, "parent", "1:", "protocol", "ip", "prio", \
"10", "u32", "match", "ip", "dst", "0.0.0.0/0", "flowid", "1:20"]),
]
download = [
# Bring ifb0 up
(True, sudo + ["ip", "link", "set", "dev", ifb, "up"]),
# Redirect ingress (incoming) to egress ifb0
(True, tc + ["qdisc", "add", "dev", adapter, "handle", "ffff:", "ingress"]),
(True, tc + ["filter", "add", "dev", adapter, "parent", "ffff:", "protocol", "ip", "u32", \
"match", "u32", "0", "0", "action", "mirred", "egress", "redirect", "dev", ifb]),
# Add class and rules for virtual interface
(True, tc + ["qdisc", "add", "dev", ifb, "root", "handle", "2:", "htb"]),
(True, tc + ["class", "add", "dev", ifb, "parent", "2:", "classid", "2:1", "htb", "rate", f"{download_speed_kbps}kbit"]),
# Add filter to rule for IP address
(True, tc + ["filter", "add", "dev", ifb, "protocol", "ip", "parent", "2:", "prio", "1", "u32", "match", "ip", "src", "0.0.0.0/0", "flowid", "2:1"]),
]
commands = cleanup
if upload_speed_kbps != -1:
commands += upload
if download_speed_kbps != -1:
commands += download
for check, cmd in commands:
subprocess.run(cmd, check=check)
def get_modem_version(self):
try:
modem = self.get_modem()
@@ -532,9 +472,23 @@ class Tici(HardwareBase):
except Exception:
pass
# blue prime config
if sim_id.startswith('8901410'):
os.system('mmcli -m any --3gpp-set-initial-eps-bearer-settings="apn=Broadband"')
# blue prime
blue_prime = sim_id.startswith('8901410')
initial_apn = "Broadband" if blue_prime else ""
os.system(f'mmcli -m any --3gpp-set-initial-eps-bearer-settings="apn={initial_apn}"')
# eSIM prime
if sim_id.startswith('8985235'):
dest = "/etc/NetworkManager/system-connections/esim.nmconnection"
with open(Path(__file__).parent/'esim.nmconnection') as f, tempfile.NamedTemporaryFile(mode='w') as tf:
dat = f.read()
dat = dat.replace("sim-id=", f"sim-id={sim_id}")
tf.write(dat)
tf.flush()
# needs to be root
os.system(f"sudo cp {tf.name} {dest}")
os.system(f"sudo nmcli con load {dest}")
def get_networks(self):
r = {}
@@ -603,5 +557,6 @@ class Tici(HardwareBase):
if __name__ == "__main__":
t = Tici()
t.configure_modem()
t.initialize_hardware()
t.set_power_save(False)
system/logcatd/logcatd
BIN
View File
Binary file not shown.
system/loggerd/bootlog
BIN
View File
Binary file not shown.
system/loggerd/config.py
-6
View File
@@ -1,6 +1,4 @@
import os
from pathlib import Path
from openpilot.system.hardware import PC
from openpilot.system.hardware.hw import Paths
@@ -9,10 +7,6 @@ SEGMENT_LENGTH = 60
STATS_DIR_FILE_LIMIT = 10000
STATS_SOCKET = "ipc:///tmp/stats"
if PC:
STATS_DIR = str(Path.home() / ".comma" / "stats")
else:
STATS_DIR = "/data/stats/"
STATS_FLUSH_TIME_S = 60
def get_available_percent(default=None):
system/loggerd/deleter.py
+1 -1
View File
@@ -4,7 +4,7 @@ import shutil
import threading
from typing import List
from openpilot.system.hardware.hw import Paths
from openpilot.system.swaglog import cloudlog
from openpilot.common.swaglog import cloudlog
from openpilot.system.loggerd.config import get_available_bytes, get_available_percent
from openpilot.system.loggerd.uploader import listdir_by_creation
from openpilot.system.loggerd.xattr_cache import getxattr
system/loggerd/encoder/encoder.h
+2 -1
View File
@@ -4,6 +4,7 @@
#include <cstdint>
#include <memory>
#include <thread>
#include <vector>
#include "cereal/messaging/messaging.h"
#include "cereal/visionipc/visionipc.h"
@@ -23,7 +24,6 @@ public:
static void publisher_publish(VideoEncoder *e, int segment_num, uint32_t idx, VisionIpcBufExtra &extra, unsigned int flags, kj::ArrayPtr<capnp::byte> header, kj::ArrayPtr<capnp::byte> dat);
protected:
int in_width, in_height;
const EncoderInfo encoder_info;
@@ -32,4 +32,5 @@ private:
// total frames encoded
int cnt = 0;
std::unique_ptr<PubMaster> pm;
std::vector<capnp::byte> msg_cache;
};
system/loggerd/encoderd
BIN
View File
Binary file not shown.
system/loggerd/logger.h
+19 -43
View File
@@ -1,27 +1,17 @@
#pragma once
#include <pthread.h>
#include <cassert>
#include <cstdint>
#include <cstdio>
#include <memory>
#include <string>
#include <capnp/serialize.h>
#include <kj/array.h>
#include "cereal/messaging/messaging.h"
#include "common/util.h"
#include "common/swaglog.h"
#include "system/hardware/hw.h"
#define LOGGER_MAX_HANDLES 16
class RawFile {
public:
RawFile(const char* path) {
file = util::safe_fopen(path, "wb");
RawFile(const std::string &path) {
file = util::safe_fopen(path.c_str(), "wb");
assert(file != nullptr);
}
~RawFile() {
@@ -41,39 +31,25 @@ class RawFile {
typedef cereal::Sentinel::SentinelType SentinelType;
typedef struct LoggerHandle {
pthread_mutex_t lock;
SentinelType end_sentinel_type;
int exit_signal;
int refcnt;
char segment_path[4096];
char log_path[4096];
char qlog_path[4096];
char lock_path[4096];
std::unique_ptr<RawFile> log, q_log;
} LoggerHandle;
typedef struct LoggerState {
pthread_mutex_t lock;
int part;
class LoggerState {
public:
LoggerState(const std::string& log_root = Path::log_root());
~LoggerState();
bool next();
void write(uint8_t* data, size_t size, bool in_qlog);
inline int segment() const { return part; }
inline const std::string& segmentPath() const { return segment_path; }
inline const std::string& routeName() const { return route_name; }
inline void write(kj::ArrayPtr<kj::byte> bytes, bool in_qlog) { write(bytes.begin(), bytes.size(), in_qlog); }
inline void setExitSignal(int signal) { exit_signal = signal; }
protected:
int part = -1, exit_signal = 0;
std::string route_path, route_name, segment_path, lock_file;
kj::Array<capnp::word> init_data;
std::string route_name;
char log_name[64];
bool has_qlog;
LoggerHandle handles[LOGGER_MAX_HANDLES];
LoggerHandle* cur_handle;
} LoggerState;
std::unique_ptr<RawFile> rlog, qlog;
};
kj::Array<capnp::word> logger_build_init_data();
std::string logger_get_route_name();
void logger_init(LoggerState *s, bool has_qlog);
int logger_next(LoggerState *s, const char* root_path,
char* out_segment_path, size_t out_segment_path_len,
int* out_part);
LoggerHandle* logger_get_handle(LoggerState *s);
void logger_close(LoggerState *s, ExitHandler *exit_handler=nullptr);
void logger_log(LoggerState *s, uint8_t* data, size_t data_size, bool in_qlog);
void lh_log(LoggerHandle* h, uint8_t* data, size_t data_size, bool in_qlog);
void lh_close(LoggerHandle* h);
system/loggerd/loggerd
BIN
View File
Binary file not shown.
system/loggerd/uploader.py
+3 -3
View File
@@ -19,13 +19,13 @@ from openpilot.common.realtime import set_core_affinity
from openpilot.system.hardware import TICI
from openpilot.system.hardware.hw import Paths
from openpilot.system.loggerd.xattr_cache import getxattr, setxattr
from openpilot.system.swaglog import cloudlog
from openpilot.common.swaglog import cloudlog
NetworkType = log.DeviceState.NetworkType
UPLOAD_ATTR_NAME = 'user.upload'
UPLOAD_ATTR_VALUE = b'1'
UPLOAD_QLOG_QCAM_MAX_SIZE = 100 * 1e6 # MB
UPLOAD_QLOG_QCAM_MAX_SIZE = 5 * 1e6 # MB
allow_sleep = bool(os.getenv("UPLOADER_SLEEP", "1"))
force_wifi = os.getenv("FORCEWIFI") is not None
@@ -228,7 +228,7 @@ class Uploader:
return success
def get_msg(self):
msg = messaging.new_message("uploaderState")
msg = messaging.new_message("uploaderState", valid=True)
us = msg.uploaderState
us.immediateQueueSize = int(self.immediate_size / 1e6)
us.immediateQueueCount = self.immediate_count
system/logmessaged.py
+3 -5
View File
@@ -5,7 +5,7 @@ from typing import NoReturn
import cereal.messaging as messaging
from openpilot.common.logging_extra import SwagLogFileFormatter
from openpilot.system.hardware.hw import Paths
from openpilot.system.swaglog import get_file_handler
from openpilot.common.swaglog import get_file_handler
def main() -> NoReturn:
@@ -35,13 +35,11 @@ def main() -> NoReturn:
continue
# then we publish them
msg = messaging.new_message()
msg.logMessage = record
msg = messaging.new_message(None, valid=True, logMessage=record)
log_message_sock.send(msg.to_bytes())
if level >= 40: # logging.ERROR
msg = messaging.new_message()
msg.errorLogMessage = record
msg = messaging.new_message(None, valid=True, errorLogMessage=record)
error_log_message_sock.send(msg.to_bytes())
finally:
sock.close()
system/micd.py
+13 -10
View File
@@ -2,15 +2,15 @@
import numpy as np
from cereal import messaging
from openpilot.common.filter_simple import FirstOrderFilter
from openpilot.common.realtime import Ratekeeper
from openpilot.system.swaglog import cloudlog
from openpilot.common.retry import retry
from openpilot.common.swaglog import cloudlog
RATE = 10
FFT_SAMPLES = 4096
REFERENCE_SPL = 2e-5 # newtons/m^2
SAMPLE_RATE = 44100
FILTER_DT = 1. / (SAMPLE_RATE / FFT_SAMPLES)
SAMPLE_BUFFER = 4096 # (approx 100ms)
def calculate_spl(measurements):
@@ -50,15 +50,12 @@ class Mic:
self.sound_pressure_weighted = 0
self.sound_pressure_level_weighted = 0
self.spl_filter_weighted = FirstOrderFilter(0, 2.5, FILTER_DT, initialized=False)
def update(self):
msg = messaging.new_message('microphone')
msg = messaging.new_message('microphone', valid=True)
msg.microphone.soundPressure = float(self.sound_pressure)
msg.microphone.soundPressureWeighted = float(self.sound_pressure_weighted)
msg.microphone.soundPressureWeightedDb = float(self.sound_pressure_level_weighted)
msg.microphone.filteredSoundPressureWeightedDb = float(self.spl_filter_weighted.x)
self.pm.send('microphone', msg)
self.rk.keep_time()
@@ -79,16 +76,22 @@ class Mic:
self.sound_pressure, _ = calculate_spl(measurements)
measurements_weighted = apply_a_weighting(measurements)
self.sound_pressure_weighted, self.sound_pressure_level_weighted = calculate_spl(measurements_weighted)
self.spl_filter_weighted.update(self.sound_pressure_level_weighted)
self.measurements = self.measurements[FFT_SAMPLES:]
@retry(attempts=7, delay=3)
def get_stream(self, sd):
# reload sounddevice to reinitialize portaudio
sd._terminate()
sd._initialize()
return sd.InputStream(channels=1, samplerate=SAMPLE_RATE, callback=self.callback, blocksize=SAMPLE_BUFFER)
def micd_thread(self):
# sounddevice must be imported after forking processes
import sounddevice as sd
with sd.InputStream(channels=1, samplerate=SAMPLE_RATE, callback=self.callback) as stream:
cloudlog.info(f"micd stream started: {stream.samplerate=} {stream.channels=} {stream.dtype=} {stream.device=}")
with self.get_stream(sd) as stream:
cloudlog.info(f"micd stream started: {stream.samplerate=} {stream.channels=} {stream.dtype=} {stream.device=}, {stream.blocksize=}")
while True:
self.update()
system/proclogd/proclogd
BIN
View File
Binary file not shown.
system/sensord/rawgps/modemdiag.py → system/qcomgpsd/modemdiag.py
View File
system/sensord/rawgps/nmeaport.py → system/qcomgpsd/nmeaport.py
+3 -3
View File
@@ -120,11 +120,11 @@ def process_nmea_port_messages(device:str="/dev/ttyUSB1") -> NoReturn:
def main() -> NoReturn:
from openpilot.common.gpio import gpio_init, gpio_set
from openpilot.system.hardware.tici.pins import GPIO
from openpilot.system.sensord.rawgps.rawgpsd import at_cmd
from openpilot.system.qcomgpsd.qcomgpsd import at_cmd
try:
check_output(["pidof", "rawgpsd"])
print("rawgpsd is running, please kill openpilot before running this script! (aborted)")
check_output(["pidof", "qcomgpsd"])
print("qcomgpsd is running, please kill openpilot before running this script! (aborted)")
sys.exit(1)
except CalledProcessError as e:
if e.returncode != 1: # 1 == no process found (boardd not running)
system/sensord/rawgps/rawgpsd.py → system/qcomgpsd/qcomgpsd.py
+31 -54
View File
@@ -8,7 +8,7 @@ import time
import pycurl
import shutil
import subprocess
from datetime import datetime
import datetime
from multiprocessing import Process, Event
from typing import NoReturn, Optional
from struct import unpack_from, calcsize, pack
@@ -16,13 +16,11 @@ from struct import unpack_from, calcsize, pack
from cereal import log
import cereal.messaging as messaging
from openpilot.common.gpio import gpio_init, gpio_set
from laika.gps_time import GPSTime, utc_to_gpst, get_leap_seconds
from laika.helpers import get_prn_from_nmea_id
from laika.constants import SECS_IN_HR, SECS_IN_DAY, SECS_IN_WEEK
from openpilot.common.retry import retry
from openpilot.system.hardware.tici.pins import GPIO
from openpilot.system.swaglog import cloudlog
from openpilot.system.sensord.rawgps.modemdiag import ModemDiag, DIAG_LOG_F, setup_logs, send_recv
from openpilot.system.sensord.rawgps.structs import (dict_unpacker, position_report, relist,
from openpilot.common.swaglog import cloudlog
from openpilot.system.qcomgpsd.modemdiag import ModemDiag, DIAG_LOG_F, setup_logs, send_recv
from openpilot.system.qcomgpsd.structs import (dict_unpacker, position_report, relist,
gps_measurement_report, gps_measurement_report_sv,
glonass_measurement_report, glonass_measurement_report_sv,
oemdre_measurement_report, oemdre_measurement_report_sv, oemdre_svpoly_report,
@@ -87,27 +85,13 @@ measurementStatusGlonassFields = {
"glonassTimeMarkValid": 17
}
@retry(attempts=10, delay=1.0)
def try_setup_logs(diag, logs):
return setup_logs(diag, logs)
def try_setup_logs(diag, log_types):
for _ in range(10):
try:
setup_logs(diag, log_types)
break
except Exception:
cloudlog.exception("setup logs failed, trying again")
time.sleep(1.0)
else:
raise Exception(f"setup logs failed, {log_types=}")
@retry(attempts=3, delay=1.0)
def at_cmd(cmd: str) -> Optional[str]:
for _ in range(3):
try:
return subprocess.check_output(f"mmcli -m any --timeout 30 --command='{cmd}'", shell=True, encoding='utf8')
except subprocess.CalledProcessError:
cloudlog.exception("rawgps.mmcli_command_failed")
time.sleep(1.0)
raise Exception(f"failed to execute mmcli command {cmd=}")
return subprocess.check_output(f"mmcli -m any --timeout 30 --command='{cmd}'", shell=True, encoding='utf8')
def gps_enabled() -> bool:
try:
@@ -157,24 +141,13 @@ def downloader_loop(event):
except KeyboardInterrupt:
pass
@retry(attempts=5, delay=0.2, ignore_failure=True)
def inject_assistance():
for _ in range(5):
try:
cmd = f"mmcli -m any --timeout 30 --location-inject-assistance-data={ASSIST_DATA_FILE}"
subprocess.check_output(cmd, stderr=subprocess.PIPE, shell=True)
cloudlog.info("successfully loaded assistance data")
return
except subprocess.CalledProcessError as e:
cloudlog.event(
"rawgps.assistance_loading_failed",
error=True,
cmd=e.cmd,
output=e.output,
returncode=e.returncode
)
time.sleep(0.2)
cloudlog.error("failed to load assistance after retry")
cmd = f"mmcli -m any --timeout 30 --location-inject-assistance-data={ASSIST_DATA_FILE}"
subprocess.check_output(cmd, stderr=subprocess.PIPE, shell=True)
cloudlog.info("successfully loaded assistance data")
@retry(attempts=5, delay=1.0)
def setup_quectel(diag: ModemDiag) -> bool:
ret = False
@@ -211,7 +184,7 @@ def setup_quectel(diag: ModemDiag) -> bool:
inject_assistance()
os.remove(ASSIST_DATA_FILE)
#at_cmd("AT+QGPSXTRADATA?")
time_str = datetime.utcnow().strftime("%Y/%m/%d,%H:%M:%S")
time_str = datetime.datetime.utcnow().strftime("%Y/%m/%d,%H:%M:%S")
at_cmd(f"AT+QGPSXTRATIME=0,\"{time_str}\",1,1,1000")
at_cmd("AT+QGPSCFG=\"outport\",\"usbnmea\"")
@@ -293,7 +266,6 @@ def main() -> NoReturn:
diag = ModemDiag()
r = setup_quectel(diag)
want_assistance = not r
current_gps_time = utc_to_gpst(GPSTime.from_datetime(datetime.utcnow()))
cloudlog.warning("quectel setup done")
gpio_init(GPIO.GNSS_PWR_EN, True)
gpio_set(GPIO.GNSS_PWR_EN, True)
@@ -325,7 +297,7 @@ def main() -> NoReturn:
print("%.4f: got log: %x len %d" % (time.time(), log_type, len(log_payload)))
if log_type == LOG_GNSS_OEMDRE_MEASUREMENT_REPORT:
msg = messaging.new_message('qcomGnss')
msg = messaging.new_message('qcomGnss', valid=True)
gnss = msg.qcomGnss
gnss.logTs = log_time
@@ -366,8 +338,6 @@ def main() -> NoReturn:
setattr(sv.measurementStatus, kk, bool(v & (1<<vv)))
else:
setattr(sv, k, v)
if report.source == log.QcomGnss.MeasurementSource.gps:
current_gps_time = GPSTime(report.gpsWeek, report.gpsMilliseconds / 1000.0)
pm.send('qcomGnss', msg)
elif log_type == LOG_GNSS_POSITION_REPORT:
report = unpack_position(log_payload)
@@ -376,15 +346,19 @@ def main() -> NoReturn:
vNED = [report["q_FltVelEnuMps[1]"], report["q_FltVelEnuMps[0]"], -report["q_FltVelEnuMps[2]"]]
vNEDsigma = [report["q_FltVelSigmaMps[1]"], report["q_FltVelSigmaMps[0]"], -report["q_FltVelSigmaMps[2]"]]
msg = messaging.new_message('gpsLocation')
msg = messaging.new_message('gpsLocation', valid=True)
gps = msg.gpsLocation
gps.latitude = report["t_DblFinalPosLatLon[0]"] * 180/math.pi
gps.longitude = report["t_DblFinalPosLatLon[1]"] * 180/math.pi
gps.altitude = report["q_FltFinalPosAlt"]
gps.speed = math.sqrt(sum([x**2 for x in vNED]))
gps.bearingDeg = report["q_FltHeadingRad"] * 180/math.pi
gps.unixTimestampMillis = GPSTime(report['w_GpsWeekNumber'],
1e-3*report['q_GpsFixTimeMs']).as_unix_timestamp()*1e3
# TODO needs update if there is another leap second, after june 2024?
dt_timestamp = (datetime.datetime(1980, 1, 6, 0, 0, 0, 0, datetime.timezone.utc) +
datetime.timedelta(weeks=report['w_GpsWeekNumber']) +
datetime.timedelta(seconds=(1e-3*report['q_GpsFixTimeMs'] - 18)))
gps.unixTimestampMillis = dt_timestamp.timestamp()*1e3
gps.source = log.GpsLocationData.SensorSource.qcomdiag
gps.vNED = vNED
gps.verticalAccuracy = report["q_FltVdop"]
@@ -395,12 +369,10 @@ def main() -> NoReturn:
if gps.flags:
want_assistance = False
stop_download_event.set()
pm.send('gpsLocation', msg)
elif log_type == LOG_GNSS_OEMDRE_SVPOLY_REPORT:
msg = messaging.new_message('qcomGnss')
msg = messaging.new_message('qcomGnss', valid=True)
dat = unpack_svpoly(log_payload)
dat = relist(dat)
gnss = msg.qcomGnss
@@ -415,6 +387,10 @@ def main() -> NoReturn:
else:
setattr(poly, k, v)
'''
# Timestamp glonass polys with GPSTime
from laika.gps_time import GPSTime, utc_to_gpst, get_leap_seconds
from laika.helpers import get_prn_from_nmea_id
prn = get_prn_from_nmea_id(poly.svId)
if prn[0] == 'R':
epoch = GPSTime(current_gps_time.week, (poly.t0 - 3*SECS_IN_HR + SECS_IN_DAY) % (SECS_IN_WEEK) + get_leap_seconds(current_gps_time))
@@ -429,10 +405,11 @@ def main() -> NoReturn:
poly.gpsWeek = epoch.week
poly.gpsTow = epoch.tow
'''
pm.send('qcomGnss', msg)
elif log_type in [LOG_GNSS_GPS_MEASUREMENT_REPORT, LOG_GNSS_GLONASS_MEASUREMENT_REPORT]:
msg = messaging.new_message('qcomGnss')
msg = messaging.new_message('qcomGnss', valid=True)
gnss = msg.qcomGnss
gnss.logTs = log_time
system/sensord/rawgps/structs.py → system/qcomgpsd/structs.py
View File
system/sensord/rawgps/test_rawgps.py → system/qcomgpsd/tests/test_qcomgpsd.py
+16 -18
View File
@@ -1,5 +1,6 @@
#!/usr/bin/env python3
import os
import pytest
import json
import time
import datetime
@@ -7,26 +8,23 @@ import unittest
import subprocess
import cereal.messaging as messaging
from openpilot.system.hardware import TICI
from openpilot.system.sensord.rawgps.rawgpsd import at_cmd, wait_for_modem
from openpilot.system.qcomgpsd.qcomgpsd import at_cmd, wait_for_modem
from openpilot.selfdrive.manager.process_config import managed_processes
GOOD_SIGNAL = bool(int(os.getenv("GOOD_SIGNAL", '0')))
@pytest.mark.tici
class TestRawgpsd(unittest.TestCase):
@classmethod
def setUpClass(cls):
if not TICI:
raise unittest.SkipTest
os.system("sudo systemctl start systemd-resolved")
os.system("sudo systemctl restart ModemManager lte")
wait_for_modem()
@classmethod
def tearDownClass(cls):
managed_processes['rawgpsd'].stop()
managed_processes['qcomgpsd'].stop()
os.system("sudo systemctl restart systemd-resolved")
os.system("sudo systemctl restart ModemManager lte")
@@ -35,7 +33,7 @@ class TestRawgpsd(unittest.TestCase):
self.sm = messaging.SubMaster(['qcomGnss', 'gpsLocation', 'gnssMeasurements'])
def tearDown(self):
managed_processes['rawgpsd'].stop()
managed_processes['qcomgpsd'].stop()
os.system("sudo systemctl restart systemd-resolved")
def _wait_for_output(self, t):
@@ -53,7 +51,7 @@ class TestRawgpsd(unittest.TestCase):
def test_wait_for_modem(self):
os.system("sudo systemctl stop ModemManager")
managed_processes['rawgpsd'].start()
managed_processes['qcomgpsd'].start()
assert not self._wait_for_output(5)
os.system("sudo systemctl restart ModemManager")
@@ -64,15 +62,15 @@ class TestRawgpsd(unittest.TestCase):
if not internet:
os.system("sudo systemctl stop systemd-resolved")
with self.subTest(internet=internet):
managed_processes['rawgpsd'].start()
managed_processes['qcomgpsd'].start()
assert self._wait_for_output(7)
managed_processes['rawgpsd'].stop()
managed_processes['qcomgpsd'].stop()
def test_turns_off_gnss(self):
for s in (0.1, 1, 5):
managed_processes['rawgpsd'].start()
managed_processes['qcomgpsd'].start()
time.sleep(s)
managed_processes['rawgpsd'].stop()
managed_processes['qcomgpsd'].stop()
ls = subprocess.check_output("mmcli -m any --location-status --output-json", shell=True, encoding='utf-8')
loc_status = json.loads(ls)
@@ -96,29 +94,29 @@ class TestRawgpsd(unittest.TestCase):
assert valid_duration == '0'
def test_assistance_loading(self):
managed_processes['rawgpsd'].start()
managed_processes['qcomgpsd'].start()
assert self._wait_for_output(10)
managed_processes['rawgpsd'].stop()
managed_processes['qcomgpsd'].stop()
self.check_assistance(True)
def test_no_assistance_loading(self):
os.system("sudo systemctl stop systemd-resolved")
managed_processes['rawgpsd'].start()
managed_processes['qcomgpsd'].start()
assert self._wait_for_output(10)
managed_processes['rawgpsd'].stop()
managed_processes['qcomgpsd'].stop()
self.check_assistance(False)
def test_late_assistance_loading(self):
os.system("sudo systemctl stop systemd-resolved")
managed_processes['rawgpsd'].start()
managed_processes['qcomgpsd'].start()
self._wait_for_output(17)
assert self.sm.updated['qcomGnss']
os.system("sudo systemctl restart systemd-resolved")
time.sleep(15)
managed_processes['rawgpsd'].stop()
managed_processes['qcomgpsd'].stop()
self.check_assistance(True)
if __name__ == "__main__":
system/sensord/pigeond.py
+2 -2
View File
@@ -11,7 +11,7 @@ from typing import List, Optional, Tuple
from cereal import messaging
from openpilot.common.params import Params
from openpilot.system.swaglog import cloudlog
from openpilot.common.swaglog import cloudlog
from openpilot.system.hardware import TICI
from openpilot.common.gpio import gpio_init, gpio_set
from openpilot.system.hardware.tici.pins import GPIO
@@ -291,7 +291,7 @@ def run_receiving(pigeon: TTYPigeon, pm: messaging.PubMaster, duration: int = 0)
continue
# send out to socket
msg = messaging.new_message('ubloxRaw', len(dat))
msg = messaging.new_message('ubloxRaw', len(dat), valid=True)
msg.ubloxRaw = dat[:]
pm.send('ubloxRaw', msg)
else:
system/sensord/rawgps/compare.py
-66
View File
@@ -1,66 +0,0 @@
#!/usr/bin/env python3
import cereal.messaging as messaging
from laika import constants
if __name__ == "__main__":
sm = messaging.SubMaster(['ubloxGnss', 'qcomGnss'])
meas = None
while 1:
sm.update()
if sm['ubloxGnss'].which() == "measurementReport":
meas = sm['ubloxGnss'].measurementReport.measurements
if not sm.updated['qcomGnss'] or meas is None:
continue
report = sm['qcomGnss'].measurementReport
if report.source not in [0, 1]:
continue
GLONASS = report.source == 1
recv_time = report.milliseconds / 1000
car = []
print("qcom has ", sorted([x.svId for x in report.sv]))
print("ublox has", sorted([x.svId for x in meas if x.gnssId == (6 if GLONASS else 0)]))
for i in report.sv:
# match to ublox
tm = None
for m in meas:
if i.svId == m.svId and m.gnssId == 0 and m.sigId == 0 and not GLONASS:
tm = m
if (i.svId-64) == m.svId and m.gnssId == 6 and m.sigId == 0 and GLONASS:
tm = m
if tm is None:
continue
if not i.measurementStatus.measurementNotUsable and i.measurementStatus.satelliteTimeIsKnown:
sat_time = (i.unfilteredMeasurementIntegral + i.unfilteredMeasurementFraction + i.latency) / 1000
ublox_psuedorange = tm.pseudorange
qcom_psuedorange = (recv_time - sat_time)*constants.SPEED_OF_LIGHT
if GLONASS:
glonass_freq = tm.glonassFrequencyIndex - 7
ublox_speed = -(constants.SPEED_OF_LIGHT / (constants.GLONASS_L1 + glonass_freq*constants.GLONASS_L1_DELTA)) * (tm.doppler)
else:
ublox_speed = -(constants.SPEED_OF_LIGHT / constants.GPS_L1) * tm.doppler
qcom_speed = i.unfilteredSpeed
car.append((i.svId, tm.pseudorange, ublox_speed, qcom_psuedorange, qcom_speed, tm.cno))
if len(car) == 0:
print("nothing to compare")
continue
pr_err, speed_err = 0., 0.
for c in car:
ublox_psuedorange, ublox_speed, qcom_psuedorange, qcom_speed = c[1:5]
pr_err += ublox_psuedorange - qcom_psuedorange
speed_err += ublox_speed - qcom_speed
pr_err /= len(car)
speed_err /= len(car)
print("avg psuedorange err %f avg speed err %f" % (pr_err, speed_err))
for c in sorted(car, key=lambda x: abs(x[1] - x[3] - pr_err)):
svid, ublox_psuedorange, ublox_speed, qcom_psuedorange, qcom_speed, cno = c
print("svid: %3d pseudorange: %10.2f m speed: %8.2f m/s meas: %12.2f speed: %10.2f meas_err: %10.3f speed_err: %8.3f cno: %d" %
(svid, ublox_psuedorange, ublox_speed, qcom_psuedorange, qcom_speed,
ublox_psuedorange - qcom_psuedorange - pr_err, ublox_speed - qcom_speed - speed_err, cno))
system/sensord/sensord
BIN
View File
Binary file not shown.
system/swaglog.py
-135
View File
@@ -1,135 +0,0 @@
import logging
import os
import time
import warnings
from pathlib import Path
from logging.handlers import BaseRotatingHandler
import zmq
from openpilot.common.logging_extra import SwagLogger, SwagFormatter, SwagLogFileFormatter
from openpilot.system.hardware.hw import Paths
def get_file_handler():
Path(Paths.swaglog_root()).mkdir(parents=True, exist_ok=True)
base_filename = os.path.join(Paths.swaglog_root(), "swaglog")
handler = SwaglogRotatingFileHandler(base_filename)
return handler
class SwaglogRotatingFileHandler(BaseRotatingHandler):
def __init__(self, base_filename, interval=60, max_bytes=1024*256, backup_count=2500, encoding=None):
super().__init__(base_filename, mode="a", encoding=encoding, delay=True)
self.base_filename = base_filename
self.interval = interval # seconds
self.max_bytes = max_bytes
self.backup_count = backup_count
self.log_files = self.get_existing_logfiles()
log_indexes = [f.split(".")[-1] for f in self.log_files]
self.last_file_idx = max([int(i) for i in log_indexes if i.isdigit()] or [-1])
self.last_rollover = None
self.doRollover()
def _open(self):
self.last_rollover = time.monotonic()
self.last_file_idx += 1
next_filename = f"{self.base_filename}.{self.last_file_idx:010}"
stream = open(next_filename, self.mode, encoding=self.encoding)
self.log_files.insert(0, next_filename)
return stream
def get_existing_logfiles(self):
log_files = list()
base_dir = os.path.dirname(self.base_filename)
for fn in os.listdir(base_dir):
fp = os.path.join(base_dir, fn)
if fp.startswith(self.base_filename) and os.path.isfile(fp):
log_files.append(fp)
return sorted(log_files)
def shouldRollover(self, record):
size_exceeded = self.max_bytes > 0 and self.stream.tell() >= self.max_bytes
time_exceeded = self.interval > 0 and self.last_rollover + self.interval <= time.monotonic()
return size_exceeded or time_exceeded
def doRollover(self):
if self.stream:
self.stream.close()
self.stream = self._open()
if self.backup_count > 0:
while len(self.log_files) > self.backup_count:
to_delete = self.log_files.pop()
if os.path.exists(to_delete): # just being safe, should always exist
os.remove(to_delete)
class UnixDomainSocketHandler(logging.Handler):
def __init__(self, formatter):
logging.Handler.__init__(self)
self.setFormatter(formatter)
self.pid = None
self.zctx = None
self.sock = None
def __del__(self):
self.close()
def close(self):
if self.sock is not None:
self.sock.close()
if self.zctx is not None:
self.zctx.term()
def connect(self):
self.zctx = zmq.Context()
self.sock = self.zctx.socket(zmq.PUSH)
self.sock.setsockopt(zmq.LINGER, 10)
self.sock.connect(Paths.swaglog_ipc())
self.pid = os.getpid()
def emit(self, record):
if os.getpid() != self.pid:
# TODO suppresses warning about forking proc with zmq socket, fix root cause
warnings.filterwarnings("ignore", category=ResourceWarning, message="unclosed.*<zmq.*>")
self.connect()
msg = self.format(record).rstrip('\n')
# print("SEND".format(repr(msg)))
try:
s = chr(record.levelno)+msg
self.sock.send(s.encode('utf8'), zmq.NOBLOCK)
except zmq.error.Again:
# drop :/
pass
def add_file_handler(log):
"""
Function to add the file log handler to swaglog.
This can be used to store logs when logmessaged is not running.
"""
handler = get_file_handler()
handler.setFormatter(SwagLogFileFormatter(log))
log.addHandler(handler)
cloudlog = log = SwagLogger()
log.setLevel(logging.DEBUG)
outhandler = logging.StreamHandler()
print_level = os.environ.get('LOGPRINT', 'warning')
if print_level == 'debug':
outhandler.setLevel(logging.DEBUG)
elif print_level == 'info':
outhandler.setLevel(logging.INFO)
elif print_level == 'warning':
outhandler.setLevel(logging.WARNING)
ipchandler = UnixDomainSocketHandler(SwagFormatter(log))
log.addHandler(outhandler)
# logs are sent through IPC before writing to disk to prevent disk I/O blocking
log.addHandler(ipchandler)
system/timezoned.py
+1 -1
View File
@@ -10,7 +10,7 @@ from timezonefinder import TimezoneFinder
from openpilot.common.params import Params
from openpilot.system.hardware import AGNOS
from openpilot.system.swaglog import cloudlog
from openpilot.common.swaglog import cloudlog
from openpilot.system.version import get_version
REQUEST_HEADERS = {'User-Agent': "openpilot-" + get_version()}
system/ubloxd/ubloxd
BIN
View File
Binary file not shown.
system/version.py
+1 -1
View File
@@ -5,7 +5,7 @@ from typing import List, Optional
from functools import lru_cache
from openpilot.common.basedir import BASEDIR
from openpilot.system.swaglog import cloudlog
from openpilot.common.swaglog import cloudlog
RELEASE_BRANCHES = ['release3-staging', 'dashcam3-staging', 'release3', 'dashcam3', 'nightly']
TESTED_BRANCHES = RELEASE_BRANCHES + ['devel', 'devel-staging']
system/webrtc/__init__.py
View File
system/webrtc/device/audio.py
+110
View File
@@ -0,0 +1,110 @@
import asyncio
import io
from typing import Optional, List, Tuple
import aiortc
import av
import numpy as np
import pyaudio
class AudioInputStreamTrack(aiortc.mediastreams.AudioStreamTrack):
PYAUDIO_TO_AV_FORMAT_MAP = {
pyaudio.paUInt8: 'u8',
pyaudio.paInt16: 's16',
pyaudio.paInt24: 's24',
pyaudio.paInt32: 's32',
pyaudio.paFloat32: 'flt',
}
def __init__(self, audio_format: int = pyaudio.paInt16, rate: int = 16000, channels: int = 1, packet_time: float = 0.020, device_index: Optional[int] = None):
super().__init__()
self.p = pyaudio.PyAudio()
chunk_size = int(packet_time * rate)
self.stream = self.p.open(format=audio_format,
channels=channels,
rate=rate,
frames_per_buffer=chunk_size,
input=True,
input_device_index=device_index)
self.format = audio_format
self.rate = rate
self.channels = channels
self.packet_time = packet_time
self.chunk_size = chunk_size
self.pts = 0
async def recv(self):
mic_data = self.stream.read(self.chunk_size)
mic_array = np.frombuffer(mic_data, dtype=np.int16)
mic_array = np.expand_dims(mic_array, axis=0)
layout = 'stereo' if self.channels > 1 else 'mono'
frame = av.AudioFrame.from_ndarray(mic_array, format=self.PYAUDIO_TO_AV_FORMAT_MAP[self.format], layout=layout)
frame.rate = self.rate
frame.pts = self.pts
self.pts += frame.samples
return frame
class AudioOutputSpeaker:
def __init__(self, audio_format: int = pyaudio.paInt16, rate: int = 48000, channels: int = 2, packet_time: float = 0.2, device_index: Optional[int] = None):
chunk_size = int(packet_time * rate)
self.p = pyaudio.PyAudio()
self.buffer = io.BytesIO()
self.channels = channels
self.stream = self.p.open(format=audio_format,
channels=channels,
rate=rate,
frames_per_buffer=chunk_size,
output=True,
output_device_index=device_index,
stream_callback=self.__pyaudio_callback)
self.tracks_and_tasks: List[Tuple[aiortc.MediaStreamTrack, Optional[asyncio.Task]]] = []
def __pyaudio_callback(self, in_data, frame_count, time_info, status):
if self.buffer.getbuffer().nbytes < frame_count * self.channels * 2:
buff = b'\x00\x00' * frame_count * self.channels
elif self.buffer.getbuffer().nbytes > 115200: # 3x the usual read size
self.buffer.seek(0)
buff = self.buffer.read(frame_count * self.channels * 4)
buff = buff[:frame_count * self.channels * 2]
self.buffer.seek(2)
else:
self.buffer.seek(0)
buff = self.buffer.read(frame_count * self.channels * 2)
self.buffer.seek(2)
return (buff, pyaudio.paContinue)
async def __consume(self, track):
while True:
try:
frame = await track.recv()
except aiortc.MediaStreamError:
return
self.buffer.write(bytes(frame.planes[0]))
def hasTrack(self, track: aiortc.MediaStreamTrack) -> bool:
return any(t == track for t, _ in self.tracks_and_tasks)
def addTrack(self, track: aiortc.MediaStreamTrack):
if not self.hasTrack(track):
self.tracks_and_tasks.append((track, None))
def start(self):
for index, (track, task) in enumerate(self.tracks_and_tasks):
if task is None:
self.tracks_and_tasks[index] = (track, asyncio.create_task(self.__consume(track)))
def stop(self):
for _, task in self.tracks_and_tasks:
if task is not None:
task.cancel()
self.tracks_and_tasks = []
self.stream.stop_stream()
self.stream.close()
self.p.terminate()
system/webrtc/device/video.py
+69
View File
@@ -0,0 +1,69 @@
import asyncio
from typing import Optional
import av
from teleoprtc.tracks import TiciVideoStreamTrack
from cereal import messaging
from openpilot.tools.lib.framereader import FrameReader
from openpilot.common.realtime import DT_MDL, DT_DMON
class LiveStreamVideoStreamTrack(TiciVideoStreamTrack):
camera_to_sock_mapping = {
"driver": "livestreamDriverEncodeData",
"wideRoad": "livestreamWideRoadEncodeData",
"road": "livestreamRoadEncodeData",
}
def __init__(self, camera_type: str):
dt = DT_DMON if camera_type == "driver" else DT_MDL
super().__init__(camera_type, dt)
self._sock = messaging.sub_sock(self.camera_to_sock_mapping[camera_type], conflate=True)
self._pts = 0
async def recv(self):
while True:
msg = messaging.recv_one_or_none(self._sock)
if msg is not None:
break
await asyncio.sleep(0.005)
evta = getattr(msg, msg.which())
packet = av.Packet(evta.header + evta.data)
packet.time_base = self._time_base
packet.pts = self._pts
self.log_debug("track sending frame %s", self._pts)
self._pts += self._dt * self._clock_rate
return packet
def codec_preference(self) -> Optional[str]:
return "H264"
class FrameReaderVideoStreamTrack(TiciVideoStreamTrack):
def __init__(self, input_file: str, dt: float = DT_MDL, camera_type: str = "driver"):
super().__init__(camera_type, dt)
frame_reader = FrameReader(input_file)
self._frames = [frame_reader.get(i, pix_fmt="rgb24") for i in range(frame_reader.frame_count)]
self._frame_count = len(self.frames)
self._frame_index = 0
self._pts = 0
async def recv(self):
self.log_debug("track sending frame %s", self._pts)
img = self._frames[self._frame_index]
new_frame = av.VideoFrame.from_ndarray(img, format="rgb24")
new_frame.pts = self._pts
new_frame.time_base = self._time_base
self._frame_index = (self._frame_index + 1) % self._frame_count
self._pts = await self.next_pts(self._pts)
return new_frame
system/webrtc/schema.py
+43
View File
@@ -0,0 +1,43 @@
import capnp
from typing import Union, List, Dict, Any
def generate_type(type_walker, schema_walker) -> Union[str, List[Any], Dict[str, Any]]:
data_type = next(type_walker)
if data_type.which() == 'struct':
return generate_struct(next(schema_walker))
elif data_type.which() == 'list':
_ = next(schema_walker)
return [generate_type(type_walker, schema_walker)]
elif data_type.which() == 'enum':
return "text"
else:
return str(data_type.which())
def generate_struct(schema: capnp.lib.capnp._StructSchema) -> Dict[str, Any]:
return {field: generate_field(schema.fields[field]) for field in schema.fields if not field.endswith("DEPRECATED")}
def generate_field(field: capnp.lib.capnp._StructSchemaField) -> Union[str, List[Any], Dict[str, Any]]:
def schema_walker(field):
yield field.schema
s = field.schema
while hasattr(s, 'elementType'):
s = s.elementType
yield s
def type_walker(field):
yield field.proto.slot.type
t = field.proto.slot.type
while hasattr(getattr(t, t.which()), 'elementType'):
t = getattr(t, t.which()).elementType
yield t
if field.proto.which() == "slot":
schema_gen, type_gen = schema_walker(field), type_walker(field)
return generate_type(type_gen, schema_gen)
else:
return generate_struct(field.schema)
system/webrtc/webrtcd.py
Executable
+251
View File
@@ -0,0 +1,251 @@
#!/usr/bin/env python3
import argparse
import asyncio
import json
import uuid
import logging
from dataclasses import dataclass, field
from typing import Any, List, Optional, Union
# aiortc and its dependencies have lots of internal warnings :(
import warnings
warnings.filterwarnings("ignore", category=DeprecationWarning)
import aiortc
from aiortc.mediastreams import VideoStreamTrack, AudioStreamTrack
from aiortc.contrib.media import MediaBlackhole
from aiortc.exceptions import InvalidStateError
from aiohttp import web
import capnp
from teleoprtc import WebRTCAnswerBuilder
from teleoprtc.info import parse_info_from_offer
from openpilot.system.webrtc.device.video import LiveStreamVideoStreamTrack
from openpilot.system.webrtc.device.audio import AudioInputStreamTrack, AudioOutputSpeaker
from openpilot.system.webrtc.schema import generate_field
from cereal import messaging, log
class CerealOutgoingMessageProxy:
def __init__(self, sm: messaging.SubMaster):
self.sm = sm
self.channels: List[aiortc.RTCDataChannel] = []
def add_channel(self, channel: aiortc.RTCDataChannel):
self.channels.append(channel)
def to_json(self, msg_content: Any):
if isinstance(msg_content, capnp._DynamicStructReader):
msg_dict = msg_content.to_dict()
elif isinstance(msg_content, capnp._DynamicListReader):
msg_dict = [self.to_json(msg) for msg in msg_content]
elif isinstance(msg_content, bytes):
msg_dict = msg_content.decode()
else:
msg_dict = msg_content
return msg_dict
def update(self):
# this is blocking in async context...
self.sm.update(0)
for service, updated in self.sm.updated.items():
if not updated:
continue
msg_dict = self.to_json(self.sm[service])
mono_time, valid = self.sm.logMonoTime[service], self.sm.valid[service]
outgoing_msg = {"type": service, "logMonoTime": mono_time, "valid": valid, "data": msg_dict}
encoded_msg = json.dumps(outgoing_msg).encode()
for channel in self.channels:
channel.send(encoded_msg)
class CerealIncomingMessageProxy:
def __init__(self, pm: messaging.PubMaster):
self.pm = pm
def send(self, message: bytes):
msg_json = json.loads(message)
msg_type, msg_data = msg_json["type"], msg_json["data"]
size = None
if not isinstance(msg_data, dict):
size = len(msg_data)
msg = messaging.new_message(msg_type, size=size)
setattr(msg, msg_type, msg_data)
self.pm.send(msg_type, msg)
class CerealProxyRunner:
def __init__(self, proxy: CerealOutgoingMessageProxy):
self.proxy = proxy
self.is_running = False
self.task = None
self.logger = logging.getLogger("webrtcd")
def start(self):
assert self.task is None
self.task = asyncio.create_task(self.run())
def stop(self):
if self.task is None or self.task.done():
return
self.task.cancel()
self.task = None
async def run(self):
while True:
try:
self.proxy.update()
except InvalidStateError:
self.logger.warning("Cereal outgoing proxy invalid state (connection closed)")
break
except Exception as ex:
self.logger.error("Cereal outgoing proxy failure: %s", ex)
await asyncio.sleep(0.01)
class StreamSession:
def __init__(self, sdp: str, cameras: List[str], incoming_services: List[str], outgoing_services: List[str], debug_mode: bool = False):
config = parse_info_from_offer(sdp)
builder = WebRTCAnswerBuilder(sdp)
assert len(cameras) == config.n_expected_camera_tracks, "Incoming stream has misconfigured number of video tracks"
for cam in cameras:
track = LiveStreamVideoStreamTrack(cam) if not debug_mode else VideoStreamTrack()
builder.add_video_stream(cam, track)
if config.expected_audio_track:
track = AudioInputStreamTrack() if not debug_mode else AudioStreamTrack()
builder.add_audio_stream(track)
if config.incoming_audio_track:
self.audio_output_cls = AudioOutputSpeaker if not debug_mode else MediaBlackhole
builder.offer_to_receive_audio_stream()
self.stream = builder.stream()
self.identifier = str(uuid.uuid4())
self.outgoing_bridge = CerealOutgoingMessageProxy(messaging.SubMaster(outgoing_services))
self.incoming_bridge = CerealIncomingMessageProxy(messaging.PubMaster(incoming_services))
self.outgoing_bridge_runner = CerealProxyRunner(self.outgoing_bridge)
self.audio_output: Optional[Union[AudioOutputSpeaker, MediaBlackhole]] = None
self.run_task: Optional[asyncio.Task] = None
self.logger = logging.getLogger("webrtcd")
self.logger.info("New stream session (%s), cameras %s, audio in %s out %s, incoming services %s, outgoing services %s",
self.identifier, cameras, config.incoming_audio_track, config.expected_audio_track, incoming_services, outgoing_services)
def start(self):
self.run_task = asyncio.create_task(self.run())
def stop(self):
if self.run_task.done():
return
self.run_task.cancel()
self.run_task = None
asyncio.run(self.post_run_cleanup())
async def get_answer(self):
return await self.stream.start()
async def message_handler(self, message: bytes):
try:
self.incoming_bridge.send(message)
except Exception as ex:
self.logger.error("Cereal incoming proxy failure: %s", ex)
async def run(self):
try:
await self.stream.wait_for_connection()
if self.stream.has_messaging_channel():
self.stream.set_message_handler(self.message_handler)
channel = self.stream.get_messaging_channel()
self.outgoing_bridge_runner.proxy.add_channel(channel)
self.outgoing_bridge_runner.start()
if self.stream.has_incoming_audio_track():
track = self.stream.get_incoming_audio_track(buffered=False)
self.audio_output = self.audio_output_cls()
self.audio_output.addTrack(track)
self.audio_output.start()
self.logger.info("Stream session (%s) connected", self.identifier)
await self.stream.wait_for_disconnection()
await self.post_run_cleanup()
self.logger.info("Stream session (%s) ended", self.identifier)
except Exception as ex:
self.logger.error("Stream session failure: %s", ex)
async def post_run_cleanup(self):
await self.stream.stop()
self.outgoing_bridge_runner.stop()
if self.audio_output:
self.audio_output.stop()
@dataclass
class StreamRequestBody:
sdp: str
cameras: List[str]
bridge_services_in: List[str] = field(default_factory=list)
bridge_services_out: List[str] = field(default_factory=list)
async def get_stream(request: web.Request):
stream_dict, debug_mode = request.app['streams'], request.app['debug']
raw_body = await request.json()
body = StreamRequestBody(**raw_body)
session = StreamSession(body.sdp, body.cameras, body.bridge_services_in, body.bridge_services_out, debug_mode)
answer = await session.get_answer()
session.start()
stream_dict[session.identifier] = session
return web.json_response({"sdp": answer.sdp, "type": answer.type})
async def get_schema(request: web.Request):
services = request.query["services"].split(",")
services = [s for s in services if s]
assert all(s in log.Event.schema.fields and not s.endswith("DEPRECATED") for s in services), "Invalid service name"
schema_dict = {s: generate_field(log.Event.schema.fields[s]) for s in services}
return web.json_response(schema_dict)
async def on_shutdown(app: web.Application):
for session in app['streams'].values():
session.stop()
del app['streams']
def webrtcd_thread(host: str, port: int, debug: bool):
logging.basicConfig(level=logging.CRITICAL, handlers=[logging.StreamHandler()])
logging_level = logging.DEBUG if debug else logging.INFO
logging.getLogger("WebRTCStream").setLevel(logging_level)
logging.getLogger("webrtcd").setLevel(logging_level)
app = web.Application()
app['streams'] = dict()
app['debug'] = debug
app.on_shutdown.append(on_shutdown)
app.router.add_post("/stream", get_stream)
app.router.add_get("/schema", get_schema)
web.run_app(app, host=host, port=port)
def main():
parser = argparse.ArgumentParser(description="WebRTC daemon")
parser.add_argument("--host", type=str, default="0.0.0.0", help="Host to listen on")
parser.add_argument("--port", type=int, default=5001, help="Port to listen on")
parser.add_argument("--debug", action="store_true", help="Enable debug mode")
args = parser.parse_args()
webrtcd_thread(args.host, args.port, args.debug)
if __name__=="__main__":
main()