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

openpilot v0.8.8 release

Browse Source
This commit is contained in:
Vehicle Researcher
2021-08-22 22:13:11 -07:00
parent 444aace15f
commit baffaeee93
193 changed files with 60566 additions and 1933 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/athena/athenad.py
+62 -15
View File
@@ -22,6 +22,7 @@ from websocket import ABNF, WebSocketTimeoutException, WebSocketException, creat
import cereal.messaging as messaging
from cereal.services import service_list
from common.api import Api
from common.file_helpers import CallbackReader
from common.basedir import PERSIST
from common.params import Params
from common.realtime import sec_since_boot
@@ -41,6 +42,7 @@ RECONNECT_TIMEOUT_S = 70
RETRY_DELAY = 10 # seconds
MAX_RETRY_COUNT = 30 # Try for at most 5 minutes if upload fails immediately
WS_FRAME_SIZE = 4096
dispatcher["echo"] = lambda s: s
recv_queue: Any = queue.Queue()
@@ -49,7 +51,9 @@ upload_queue: Any = queue.Queue()
log_send_queue: Any = queue.Queue()
log_recv_queue: Any = queue.Queue()
cancelled_uploads: Any = set()
UploadItem = namedtuple('UploadItem', ['path', 'url', 'headers', 'created_at', 'id', 'retry_count'], defaults=(0,))
UploadItem = namedtuple('UploadItem', ['path', 'url', 'headers', 'created_at', 'id', 'retry_count', 'current', 'progress'], defaults=(0, False, 0))
cur_upload_items = {}
def handle_long_poll(ws):
@@ -100,35 +104,53 @@ def jsonrpc_handler(end_event):
def upload_handler(end_event):
tid = threading.get_ident()
while not end_event.is_set():
cur_upload_items[tid] = None
try:
item = upload_queue.get(timeout=1)
if item.id in cancelled_uploads:
cancelled_uploads.remove(item.id)
cur_upload_items[tid] = upload_queue.get(timeout=1)._replace(current=True)
if cur_upload_items[tid].id in cancelled_uploads:
cancelled_uploads.remove(cur_upload_items[tid].id)
continue
try:
_do_upload(item)
except (requests.exceptions.Timeout, requests.exceptions.ConnectionError, requests.exceptions.SSLError) as e:
cloudlog.warning(f"athena.upload_handler.retry {e} {item}")
def cb(sz, cur):
cur_upload_items[tid] = cur_upload_items[tid]._replace(progress=cur / sz if sz else 1)
if item.retry_count < MAX_RETRY_COUNT:
item = item._replace(retry_count=item.retry_count + 1)
_do_upload(cur_upload_items[tid], cb)
except (requests.exceptions.Timeout, requests.exceptions.ConnectionError, requests.exceptions.SSLError) as e:
cloudlog.warning(f"athena.upload_handler.retry {e} {cur_upload_items[tid]}")
if cur_upload_items[tid].retry_count < MAX_RETRY_COUNT:
item = cur_upload_items[tid]
item = item._replace(
retry_count=item.retry_count + 1,
progress=0,
current=False
)
upload_queue.put_nowait(item)
cur_upload_items[tid] = None
for _ in range(RETRY_DELAY):
time.sleep(1)
if end_event.is_set():
break
except queue.Empty:
pass
except Exception:
cloudlog.exception("athena.upload_handler.exception")
def _do_upload(upload_item):
def _do_upload(upload_item, callback=None):
with open(upload_item.path, "rb") as f:
size = os.fstat(f.fileno()).st_size
if callback:
f = CallbackReader(f, callback, size)
return requests.put(upload_item.url,
data=f,
headers={**upload_item.headers, 'Content-Length': str(size)},
@@ -171,11 +193,29 @@ def setNavDestination(latitude=0, longitude=0):
return {"success": 1}
@dispatcher.add_method
def listDataDirectory():
files = [os.path.relpath(os.path.join(dp, f), ROOT) for dp, dn, fn in os.walk(ROOT) for f in fn]
def scan_dir(path, prefix):
files = list()
# only walk directories that match the prefix
# (glob and friends traverse entire dir tree)
with os.scandir(path) as i:
for e in i:
rel_path = os.path.relpath(e.path, ROOT)
if e.is_dir(follow_symlinks=False):
# add trailing slash
rel_path = os.path.join(rel_path, '')
# if prefix is a partial dir name, current dir will start with prefix
# if prefix is a partial file name, prefix with start with dir name
if rel_path.startswith(prefix) or prefix.startswith(rel_path):
files.extend(scan_dir(e.path, prefix))
else:
if rel_path.startswith(prefix):
files.append(rel_path)
return files
@dispatcher.add_method
def listDataDirectory(prefix=''):
return scan_dir(ROOT, prefix)
@dispatcher.add_method
def reboot():
@@ -212,7 +252,8 @@ def uploadFileToUrl(fn, url, headers):
@dispatcher.add_method
def listUploadQueue():
return [item._asdict() for item in list(upload_queue.queue)]
items = list(upload_queue.queue) + list(cur_upload_items.values())
return [i._asdict() for i in items if i is not None]
@dispatcher.add_method
@@ -466,7 +507,11 @@ def ws_send(ws, end_event):
data = send_queue.get_nowait()
except queue.Empty:
data = log_send_queue.get(timeout=1)
ws.send(data)
for i in range(0, len(data), WS_FRAME_SIZE):
frame = data[i:i+WS_FRAME_SIZE]
last = i + WS_FRAME_SIZE >= len(data)
opcode = ABNF.OPCODE_TEXT if i == 0 else ABNF.OPCODE_CONT
ws.send_frame(ABNF.create_frame(frame, opcode, last))
except queue.Empty:
pass
except Exception:
@@ -514,6 +559,8 @@ def main():
manage_tokens(api)
conn_retries = 0
cur_upload_items.clear()
handle_long_poll(ws)
except (KeyboardInterrupt, SystemExit):
break
selfdrive/boardd/boardd.cc
+2 -1
View File
@@ -445,9 +445,10 @@ void hardware_control_thread() {
if (sm.updated("driverCameraState")) {
auto event = sm["driverCameraState"];
int cur_integ_lines = event.getDriverCameraState().getIntegLines();
float cur_gain = event.getDriverCameraState().getGain();
if (Hardware::TICI()) {
cur_integ_lines = integ_lines_filter.update(cur_integ_lines);
cur_integ_lines = integ_lines_filter.update(cur_integ_lines * cur_gain);
}
last_front_frame_t = event.getLogMonoTime();
selfdrive/boardd/panda.cc
+28 -3
View File
@@ -11,8 +11,10 @@
#include "selfdrive/common/swaglog.h"
#include "selfdrive/common/util.h"
Panda::Panda() {
Panda::Panda(std::string serial) {
// init libusb
ssize_t num_devices;
libusb_device **dev_list = NULL;
int err = libusb_init(&ctx);
if (err != 0) { goto fail; }
@@ -22,8 +24,28 @@ Panda::Panda() {
libusb_set_debug(ctx, 3);
#endif
dev_handle = libusb_open_device_with_vid_pid(ctx, 0xbbaa, 0xddcc);
if (dev_handle == NULL) { goto fail; }
// connect by serial
num_devices = libusb_get_device_list(ctx, &dev_list);
if (num_devices < 0) { goto fail; }
for (size_t i = 0; i < num_devices; ++i) {
libusb_device_descriptor desc;
libusb_get_device_descriptor(dev_list[i], &desc);
if (desc.idVendor == 0xbbaa && desc.idProduct == 0xddcc) {
libusb_open(dev_list[i], &dev_handle);
if (dev_handle == NULL) { goto fail; }
unsigned char desc_serial[25];
int ret = libusb_get_string_descriptor_ascii(dev_handle, desc.iSerialNumber, desc_serial, sizeof(desc_serial));
if (ret < 0) { goto fail; }
if (serial.empty() || serial.compare(reinterpret_cast<const char*>(desc_serial)) == 0) {
break;
}
libusb_close(dev_handle);
dev_handle = NULL;
}
}
libusb_free_device_list(dev_list, 1);
if (libusb_kernel_driver_active(dev_handle, 0) == 1) {
libusb_detach_kernel_driver(dev_handle, 0);
@@ -47,6 +69,9 @@ Panda::Panda() {
fail:
cleanup();
if (dev_list != NULL) {
libusb_free_device_list(dev_list, 1);
}
throw std::runtime_error("Error connecting to panda");
}
selfdrive/boardd/panda.h
+1 -1
View File
@@ -49,7 +49,7 @@ class Panda {
void cleanup();
public:
Panda();
Panda(std::string serial="");
~Panda();
std::atomic<bool> connected = true;
selfdrive/camerad/cameras/camera_common.cc
+18 -36
View File
@@ -127,8 +127,6 @@ bool CameraBuf::acquire() {
const size_t globalWorkSize[] = {size_t(camera_state->ci.frame_width), size_t(camera_state->ci.frame_height)};
const size_t localWorkSize[] = {DEBAYER_LOCAL_WORKSIZE, DEBAYER_LOCAL_WORKSIZE};
CL_CHECK(clSetKernelArg(krnl_debayer, 2, localMemSize, 0));
int ggain = camera_state->analog_gain + 4*camera_state->dc_gain_enabled;
CL_CHECK(clSetKernelArg(krnl_debayer, 3, sizeof(int), &ggain));
CL_CHECK(clEnqueueNDRangeKernel(q, krnl_debayer, 2, NULL, globalWorkSize, localWorkSize,
0, 0, &debayer_event));
#else
@@ -193,11 +191,11 @@ void fill_frame_data(cereal::FrameData::Builder &framed, const FrameMetadata &fr
}
kj::Array<uint8_t> get_frame_image(const CameraBuf *b) {
static const int x_min = getenv("XMIN") ? atoi(getenv("XMIN")) : 0;
static const int y_min = getenv("YMIN") ? atoi(getenv("YMIN")) : 0;
static const int env_xmax = getenv("XMAX") ? atoi(getenv("XMAX")) : -1;
static const int env_ymax = getenv("YMAX") ? atoi(getenv("YMAX")) : -1;
static const int scale = getenv("SCALE") ? atoi(getenv("SCALE")) : 1;
static const int x_min = util::getenv("XMIN", 0);
static const int y_min = util::getenv("YMIN", 0);
static const int env_xmax = util::getenv("XMAX", -1);
static const int env_ymax = util::getenv("YMAX", -1);
static const int scale = util::getenv("SCALE", 1);
assert(b->cur_rgb_buf);
@@ -280,39 +278,30 @@ static void publish_thumbnail(PubMaster *pm, const CameraBuf *b) {
free(thumbnail_buffer);
}
float set_exposure_target(const CameraBuf *b, int x_start, int x_end, int x_skip, int y_start, int y_end, int y_skip, int analog_gain, bool hist_ceil, bool hl_weighted) {
const uint8_t *pix_ptr = b->cur_yuv_buf->y;
float set_exposure_target(const CameraBuf *b, int x_start, int x_end, int x_skip, int y_start, int y_end, int y_skip) {
int lum_med;
uint32_t lum_binning[256] = {0};
const uint8_t *pix_ptr = b->cur_yuv_buf->y;
unsigned int lum_total = 0;
for (int y = y_start; y < y_end; y += y_skip) {
for (int x = x_start; x < x_end; x += x_skip) {
uint8_t lum = pix_ptr[(y * b->rgb_width) + x];
if (hist_ceil && lum < 80 && lum_binning[lum] > HISTO_CEIL_K * (y_end - y_start) * (x_end - x_start) / x_skip / y_skip / 256) {
continue;
}
lum_binning[lum]++;
lum_total += 1;
}
}
// Find mean lumimance value
unsigned int lum_cur = 0;
int lum_med = 0;
int lum_med_alt = 0;
for (lum_med=255; lum_med>=0; lum_med--) {
for (lum_med = 255; lum_med >= 0; lum_med--) {
lum_cur += lum_binning[lum_med];
if (hl_weighted) {
int lum_med_tmp = 0;
int hb = HLC_THRESH + (10 - analog_gain);
if (lum_cur > 0 && lum_med > hb) {
lum_med_tmp = (lum_med - hb) + 100;
}
lum_med_alt = lum_med_alt>lum_med_tmp?lum_med_alt:lum_med_tmp;
}
if (lum_cur >= lum_total / 2) {
break;
}
}
lum_med = lum_med_alt>0 ? lum_med + lum_med/32*lum_cur*abs(lum_med_alt - lum_med)/lum_total:lum_med;
return lum_med / 256.0;
}
@@ -355,18 +344,12 @@ static void driver_cam_auto_exposure(CameraState *c, SubMaster &sm) {
struct ExpRect {int x1, x2, x_skip, y1, y2, y_skip;};
const CameraBuf *b = &c->buf;
bool hist_ceil = false, hl_weighted = false;
int x_offset = 0, y_offset = 0;
int frame_width = b->rgb_width, frame_height = b->rgb_height;
#ifndef QCOM2
int analog_gain = -1;
#else
int analog_gain = c->analog_gain;
#endif
ExpRect def_rect;
if (Hardware::TICI()) {
hist_ceil = hl_weighted = true;
x_offset = 630, y_offset = 156;
frame_width = 668, frame_height = frame_width / 1.33;
def_rect = {96, 1832, 2, 242, 1148, 4};
@@ -377,7 +360,7 @@ static void driver_cam_auto_exposure(CameraState *c, SubMaster &sm) {
static ExpRect rect = def_rect;
// use driver face crop for AE
if (sm.updated("driverState")) {
if (Hardware::EON() && sm.updated("driverState")) {
if (auto state = sm["driverState"].getDriverState(); state.getFaceProb() > 0.4) {
auto face_position = state.getFacePosition();
int x = is_rhd ? 0 : frame_width - (0.5 * frame_height);
@@ -385,16 +368,15 @@ static void driver_cam_auto_exposure(CameraState *c, SubMaster &sm) {
int y = (face_position[1] + 0.5) * frame_height + y_offset;
rect = {std::max(0, x - 72), std::min(b->rgb_width - 1, x + 72), 2,
std::max(0, y - 72), std::min(b->rgb_height - 1, y + 72), 1};
} else {
rect = def_rect;
}
}
camera_autoexposure(c, set_exposure_target(b, rect.x1, rect.x2, rect.x_skip, rect.y1, rect.y2, rect.y_skip, analog_gain, hist_ceil, hl_weighted));
camera_autoexposure(c, set_exposure_target(b, rect.x1, rect.x2, rect.x_skip, rect.y1, rect.y2, rect.y_skip));
}
void common_process_driver_camera(SubMaster *sm, PubMaster *pm, CameraState *c, int cnt) {
if (cnt % 3 == 0) {
int j = Hardware::TICI() ? 1 : 3;
if (cnt % j == 0) {
sm->update(0);
driver_cam_auto_exposure(c, *sm);
}
selfdrive/camerad/cameras/camera_common.h
+3 -5
View File
@@ -27,16 +27,13 @@
#define CAMERA_ID_MAX 9
#define UI_BUF_COUNT 4
#define LOG_CAMERA_ID_FCAMERA 0
#define LOG_CAMERA_ID_DCAMERA 1
#define LOG_CAMERA_ID_ECAMERA 2
#define LOG_CAMERA_ID_QCAMERA 3
#define LOG_CAMERA_ID_MAX 4
#define HLC_THRESH 222
#define HLC_A 80
#define HISTO_CEIL_K 5
const bool env_send_driver = getenv("SEND_DRIVER") != NULL;
const bool env_send_road = getenv("SEND_ROAD") != NULL;
const bool env_send_wide_road = getenv("SEND_WIDE_ROAD") != NULL;
@@ -62,6 +59,7 @@ typedef struct LogCameraInfo {
bool is_h265;
bool downscale;
bool has_qcamera;
bool trigger_rotate;
} LogCameraInfo;
typedef struct FrameMetadata {
@@ -134,7 +132,7 @@ typedef void (*process_thread_cb)(MultiCameraState *s, CameraState *c, int cnt);
void fill_frame_data(cereal::FrameData::Builder &framed, const FrameMetadata &frame_data);
kj::Array<uint8_t> get_frame_image(const CameraBuf *b);
float set_exposure_target(const CameraBuf *b, int x_start, int x_end, int x_skip, int y_start, int y_end, int y_skip, int analog_gain, bool hist_ceil, bool hl_weighted);
float set_exposure_target(const CameraBuf *b, int x_start, int x_end, int x_skip, int y_start, int y_end, int y_skip);
std::thread start_process_thread(MultiCameraState *cameras, CameraState *cs, process_thread_cb callback);
void common_process_driver_camera(SubMaster *sm, PubMaster *pm, CameraState *c, int cnt);
selfdrive/camerad/cameras/camera_qcom.cc
+1 -1
View File
@@ -1113,7 +1113,7 @@ void process_road_camera(MultiCameraState *s, CameraState *c, int cnt) {
if (cnt % 3 == 0) {
const int x = 290, y = 322, width = 560, height = 314;
const int skip = 1;
camera_autoexposure(c, set_exposure_target(b, x, x + width, skip, y, y + height, skip, -1, false, false));
camera_autoexposure(c, set_exposure_target(b, x, x + width, skip, y, y + height, skip));
}
}
selfdrive/camerad/cameras/camera_qcom2.cc
+122 -144
View File
@@ -22,18 +22,13 @@
#include "selfdrive/common/swaglog.h"
#include "selfdrive/camerad/cameras/sensor2_i2c.h"
#define FRAME_WIDTH 1928
#define FRAME_HEIGHT 1208
//#define FRAME_STRIDE 1936 // for 8 bit output
#define FRAME_STRIDE 2416 // for 10 bit output
//#define FRAME_STRIDE 1936 // for 8 bit output
#define MIPI_SETTLE_CNT 33 // Calculated by camera_freqs.py
extern ExitHandler do_exit;
// global var for AE ops
std::atomic<CameraExpInfo> cam_exp[3] = {{{0}}};
const size_t FRAME_WIDTH = 1928;
const size_t FRAME_HEIGHT = 1208;
const size_t FRAME_STRIDE = 2416; // for 10 bit output
const int MIPI_SETTLE_CNT = 33; // Calculated by camera_freqs.py
CameraInfo cameras_supported[CAMERA_ID_MAX] = {
[CAMERA_ID_AR0231] = {
@@ -46,17 +41,27 @@ CameraInfo cameras_supported[CAMERA_ID_MAX] = {
},
};
float sensor_analog_gains[ANALOG_GAIN_MAX_IDX] = {3.0/6.0, 4.0/6.0, 4.0/5.0, 5.0/5.0,
5.0/4.0, 6.0/4.0, 6.0/3.0, 7.0/3.0,
7.0/2.0, 8.0/2.0};
const float DC_GAIN = 2.5;
const float sensor_analog_gains[] = {
1.0/8.0, 2.0/8.0, 2.0/7.0, 3.0/7.0, // 0, 1, 2, 3
3.0/6.0, 4.0/6.0, 4.0/5.0, 5.0/5.0, // 4, 5, 6, 7
5.0/4.0, 6.0/4.0, 6.0/3.0, 7.0/3.0, // 8, 9, 10, 11
7.0/2.0, 8.0/2.0, 8.0/1.0}; // 12, 13, 14, 15 = bypass
const int ANALOG_GAIN_MIN_IDX = 0x1; // 0.25x
const int ANALOG_GAIN_REC_IDX = 0x6; // 0.8x
const int ANALOG_GAIN_MAX_IDX = 0xD; // 4.0x
const int EXPOSURE_TIME_MIN = 2; // with HDR, fastest ss
const int EXPOSURE_TIME_MAX = 1904; // with HDR, slowest ss
// ************** low level camera helpers ****************
int cam_control(int fd, int op_code, void *handle, int size) {
struct cam_control camcontrol = {0};
camcontrol.op_code = op_code;
camcontrol.handle = (uint64_t)handle;
if (size == 0) { camcontrol.size = 8;
if (size == 0) {
camcontrol.size = 8;
camcontrol.handle_type = CAM_HANDLE_MEM_HANDLE;
} else {
camcontrol.size = size;
@@ -104,7 +109,7 @@ void *alloc_w_mmu_hdl(int video0_fd, int len, uint32_t *handle, int align = 8, i
assert(ptr != MAP_FAILED);
}
// LOGD("alloced: %x %d %llx mapped %p", mem_mgr_alloc_cmd.out.buf_handle, mem_mgr_alloc_cmd.out.fd, mem_mgr_alloc_cmd.out.vaddr, ptr);
// LOGD("allocated: %x %d %llx mapped %p", mem_mgr_alloc_cmd.out.buf_handle, mem_mgr_alloc_cmd.out.fd, mem_mgr_alloc_cmd.out.vaddr, ptr);
return ptr;
}
@@ -520,15 +525,17 @@ static void camera_init(MultiCameraState *multi_cam_state, VisionIpcServer * v,
s->camera_num = camera_num;
s->dc_gain_enabled = false;
s->analog_gain = 0x5;
s->analog_gain_frac = sensor_analog_gains[s->analog_gain];
s->exposure_time = 256;
s->exposure_time_max = 1.2 * EXPOSURE_TIME_MAX / 2;
s->exposure_time_min = 0.75 * EXPOSURE_TIME_MIN * 2;
s->request_id_last = 0;
s->skipped = true;
s->ef_filtered = 1.0;
s->min_ev = EXPOSURE_TIME_MIN * sensor_analog_gains[ANALOG_GAIN_MIN_IDX];
s->max_ev = EXPOSURE_TIME_MAX * sensor_analog_gains[ANALOG_GAIN_MAX_IDX] * DC_GAIN;
s->target_grey_fraction = 0.3;
s->dc_gain_enabled = false;
s->gain_idx = ANALOG_GAIN_REC_IDX;
s->exposure_time = 5;
s->cur_ev[0] = s->cur_ev[1] = s->cur_ev[2] = (s->dc_gain_enabled ? DC_GAIN : 1) * sensor_analog_gains[s->gain_idx] * s->exposure_time;
s->buf.init(device_id, ctx, s, v, FRAME_BUF_COUNT, rgb_type, yuv_type);
}
@@ -905,7 +912,7 @@ void handle_camera_event(CameraState *s, void *evdat) {
meta_data.frame_id = main_id - s->idx_offset;
meta_data.timestamp_sof = timestamp;
s->exp_lock.lock();
meta_data.gain = s->dc_gain_enabled ? s->analog_gain_frac * 2.5 : s->analog_gain_frac;
meta_data.gain = s->dc_gain_enabled ? s->analog_gain_frac * DC_GAIN : s->analog_gain_frac;
meta_data.high_conversion_gain = s->dc_gain_enabled;
meta_data.integ_lines = s->exposure_time;
meta_data.measured_grey_fraction = s->measured_grey_fraction;
@@ -925,139 +932,113 @@ void handle_camera_event(CameraState *s, void *evdat) {
}
}
// ******************* exposure control helpers *******************
void set_exposure_time_bounds(CameraState *s) {
switch (s->analog_gain) {
case 0: {
s->exposure_time_min = EXPOSURE_TIME_MIN;
s->exposure_time_max = EXPOSURE_TIME_MAX; // EXPOSURE_TIME_MIN * 4;
break;
}
case ANALOG_GAIN_MAX_IDX - 1: {
s->exposure_time_min = EXPOSURE_TIME_MIN; // EXPOSURE_TIME_MAX / 4;
s->exposure_time_max = EXPOSURE_TIME_MAX;
break;
}
default: {
// finetune margins on both ends
float k_up = sensor_analog_gains[s->analog_gain+1] / sensor_analog_gains[s->analog_gain];
float k_down = sensor_analog_gains[s->analog_gain-1] / sensor_analog_gains[s->analog_gain];
s->exposure_time_min = k_down * EXPOSURE_TIME_MIN * 2;
s->exposure_time_max = k_up * EXPOSURE_TIME_MAX / 2;
}
}
}
void switch_conversion_gain(CameraState *s) {
if (!s->dc_gain_enabled) {
s->dc_gain_enabled = true;
s->analog_gain -= 4;
} else {
s->dc_gain_enabled = false;
s->analog_gain += 4;
}
}
static void set_camera_exposure(CameraState *s, float grey_frac) {
// TODO: get stats from sensor?
float target_grey = 0.4 - ((float)(s->analog_gain + 4*s->dc_gain_enabled) / 48.0f);
float exposure_factor = 1 + 30 * pow((target_grey - grey_frac), 3);
exposure_factor = std::max(exposure_factor, 0.56f);
const float dt = 0.05;
if (s->camera_num != 1) {
s->ef_filtered = (1 - EF_LOWPASS_K) * s->ef_filtered + EF_LOWPASS_K * exposure_factor;
exposure_factor = s->ef_filtered;
const float ts_grey = 10.0;
const float ts_ev = 0.05;
const float k_grey = (dt / ts_grey) / (1.0 + dt / ts_grey);
const float k_ev = (dt / ts_ev) / (1.0 + dt / ts_ev);
// It takes 3 frames for the commanded exposure settings to take effect. The first frame is already started by the time
// we reach this function, the other 2 are due to the register buffering in the sensor.
// Therefore we use the target EV from 3 frames ago, the grey fraction that was just measured was the result of that control action.
// TODO: Lower latency to 2 frames, by using the histogram outputed by the sensor we can do AE before the debayering is complete
const float cur_ev = s->cur_ev[s->buf.cur_frame_data.frame_id % 3];
// Scale target grey between 0.1 and 0.4 depending on lighting conditions
float new_target_grey = std::clamp(0.4 - 0.3 * log2(1.0 + cur_ev) / log2(6000.0), 0.1, 0.4);
float target_grey = (1.0 - k_grey) * s->target_grey_fraction + k_grey * new_target_grey;
float desired_ev = std::clamp(cur_ev * target_grey / grey_frac, s->min_ev, s->max_ev);
float k = (1.0 - k_ev) / 3.0;
desired_ev = (k * s->cur_ev[0]) + (k * s->cur_ev[1]) + (k * s->cur_ev[2]) + (k_ev * desired_ev);
float best_ev_score = 1e6;
int new_g = 0;
int new_t = 0;
// Hysteresis around high conversion gain
// We usually want this on since it results in lower noise, but turn off in very bright day scenes
bool enable_dc_gain = s->dc_gain_enabled;
if (!enable_dc_gain && target_grey < 0.2) {
enable_dc_gain = true;
} else if (enable_dc_gain && target_grey > 0.3) {
enable_dc_gain = false;
}
// Simple brute force optimizer to choose sensor parameters
// to reach desired EV
for (int g = std::max((int)ANALOG_GAIN_MIN_IDX, s->gain_idx - 1); g <= std::min((int)ANALOG_GAIN_MAX_IDX, s->gain_idx + 1); g++) {
float gain = sensor_analog_gains[g] * (enable_dc_gain ? DC_GAIN : 1);
// Compute optimal time for given gain
int t = std::clamp(int(std::round(desired_ev / gain)), EXPOSURE_TIME_MIN, EXPOSURE_TIME_MAX);
// Only go below recomended gain when absolutely necessary to not overexpose
if (g < ANALOG_GAIN_REC_IDX && t > 20 && g < s->gain_idx) {
continue;
}
// Compute error to desired ev
float score = std::abs(desired_ev - (t * gain)) * 10;
// Going below recomended gain needs lower penalty to not overexpose
float m = g > ANALOG_GAIN_REC_IDX ? 5.0 : 0.1;
score += std::abs(g - (int)ANALOG_GAIN_REC_IDX) * m;
// LOGE("cam: %d - gain: %d, t: %d (%.2f), score %.2f, score + gain %.2f, %.3f, %.3f", s->camera_num, g, t, desired_ev / gain, score, score + std::abs(g - s->gain_idx) * (score + 1.0) / 10.0, desired_ev, s->min_ev);
// Small penalty on changing gain
score += std::abs(g - s->gain_idx) * (score + 1.0) / 10.0;
if (score < best_ev_score) {
new_t = t;
new_g = g;
best_ev_score = score;
}
}
s->exp_lock.lock();
s->measured_grey_fraction = grey_frac;
s->target_grey_fraction = target_grey;
// always prioritize exposure time adjust
s->exposure_time *= exposure_factor;
s->analog_gain_frac = sensor_analog_gains[new_g];
s->gain_idx = new_g;
s->exposure_time = new_t;
s->dc_gain_enabled = enable_dc_gain;
// switch gain if max/min exposure time is reached
// or always switch down to a lower gain when possible
bool kd = false;
if (s->analog_gain > 0) {
kd = 1.1 * s->exposure_time / (sensor_analog_gains[s->analog_gain-1] / sensor_analog_gains[s->analog_gain]) < EXPOSURE_TIME_MAX / 2;
}
if (s->exposure_time > s->exposure_time_max) {
if (s->analog_gain < ANALOG_GAIN_MAX_IDX - 1) {
s->exposure_time = EXPOSURE_TIME_MAX / 2;
s->analog_gain += 1;
if (!s->dc_gain_enabled && sensor_analog_gains[s->analog_gain] >= 4.0) { // switch to HCG
switch_conversion_gain(s);
}
set_exposure_time_bounds(s);
} else {
s->exposure_time = s->exposure_time_max;
}
} else if (s->exposure_time < s->exposure_time_min || kd) {
if (s->analog_gain > 0) {
s->exposure_time = std::max(EXPOSURE_TIME_MIN * 2, (int)(s->exposure_time / (sensor_analog_gains[s->analog_gain-1] / sensor_analog_gains[s->analog_gain])));
s->analog_gain -= 1;
if (s->dc_gain_enabled && sensor_analog_gains[s->analog_gain] <= 1.25) { // switch back to LCG
switch_conversion_gain(s);
}
set_exposure_time_bounds(s);
} else {
s->exposure_time = s->exposure_time_min;
}
}
// set up config
uint16_t AG = s->analog_gain + 4;
AG = 0xFF00 + AG * 16 + AG;
s->analog_gain_frac = sensor_analog_gains[s->analog_gain];
float gain = s->analog_gain_frac * (s->dc_gain_enabled ? DC_GAIN : 1.0);
s->cur_ev[s->buf.cur_frame_data.frame_id % 3] = s->exposure_time * gain;
s->exp_lock.unlock();
// printf("cam %d, min %d, max %d \n", s->camera_num, s->exposure_time_min, s->exposure_time_max);
// printf("cam %d, set AG to 0x%X, S to %d, dc %d \n", s->camera_num, AG, s->exposure_time, s->dc_gain_enabled);
// Processing a frame takes right about 50ms, so we need to wait a few ms
// so we don't send i2c commands around the frame start.
int ms = (nanos_since_boot() - s->buf.cur_frame_data.timestamp_sof) / 1000000;
if (ms < 60) {
util::sleep_for(60 - ms);
}
// LOGE("ae - camera %d, cur_t %.5f, sof %.5f, dt %.5f", s->camera_num, 1e-9 * nanos_since_boot(), 1e-9 * s->buf.cur_frame_data.timestamp_sof, 1e-9 * (nanos_since_boot() - s->buf.cur_frame_data.timestamp_sof));
uint16_t analog_gain_reg = 0xFF00 | (new_g << 4) | new_g;
struct i2c_random_wr_payload exp_reg_array[] = {
{0x3366, AG}, // analog gain
{0x3362, (uint16_t)(s->dc_gain_enabled?0x1:0x0)}, // DC_GAIN
{0x305A, 0x00F8}, // red gain
{0x3058, 0x0122}, // blue gain
{0x3056, 0x009A}, // g1 gain
{0x305C, 0x009A}, // g2 gain
{0x3012, (uint16_t)s->exposure_time}, // integ time
};
//{0x301A, 0x091C}}; // reset
{0x3366, analog_gain_reg},
{0x3362, (uint16_t)(s->dc_gain_enabled ? 0x1 : 0x0)},
{0x3012, (uint16_t)s->exposure_time},
};
sensors_i2c(s, exp_reg_array, sizeof(exp_reg_array)/sizeof(struct i2c_random_wr_payload),
CAM_SENSOR_PACKET_OPCODE_SENSOR_CONFIG);
CAM_SENSOR_PACKET_OPCODE_SENSOR_CONFIG);
}
void camera_autoexposure(CameraState *s, float grey_frac) {
CameraExpInfo tmp = cam_exp[s->camera_num].load();
tmp.op_id++;
tmp.grey_frac = grey_frac;
cam_exp[s->camera_num].store(tmp);
set_camera_exposure(s, grey_frac);
}
static void ae_thread(MultiCameraState *s) {
CameraState *c_handles[3] = {&s->wide_road_cam, &s->road_cam, &s->driver_cam};
int op_id_last[3] = {0};
CameraExpInfo cam_op[3];
set_thread_name("camera_settings");
while(!do_exit) {
for (int i=0;i<3;i++) {
cam_op[i] = cam_exp[i].load();
if (cam_op[i].op_id != op_id_last[i]) {
set_camera_exposure(c_handles[i], cam_op[i].grey_frac);
op_id_last[i] = cam_op[i].op_id;
}
}
util::sleep_for(50);
}
}
void process_driver_camera(MultiCameraState *s, CameraState *c, int cnt) {
common_process_driver_camera(s->sm, s->pm, c, cnt);
@@ -1078,17 +1059,14 @@ void process_road_camera(MultiCameraState *s, CameraState *c, int cnt) {
}
s->pm->send(c == &s->road_cam ? "roadCameraState" : "wideRoadCameraState", msg);
if (cnt % 3 == 0) {
const auto [x, y, w, h] = (c == &s->wide_road_cam) ? std::tuple(96, 250, 1734, 524) : std::tuple(96, 160, 1734, 986);
const int skip = 2;
camera_autoexposure(c, set_exposure_target(b, x, x + w, skip, y, y + h, skip, (int)c->analog_gain, true, true));
}
const auto [x, y, w, h] = (c == &s->wide_road_cam) ? std::tuple(96, 250, 1734, 524) : std::tuple(96, 160, 1734, 986);
const int skip = 2;
camera_autoexposure(c, set_exposure_target(b, x, x + w, skip, y, y + h, skip));
}
void cameras_run(MultiCameraState *s) {
LOG("-- Starting threads");
std::vector<std::thread> threads;
threads.push_back(std::thread(ae_thread, s));
threads.push_back(start_process_thread(s, &s->road_cam, process_road_camera));
threads.push_back(start_process_thread(s, &s->driver_cam, process_driver_camera));
threads.push_back(start_process_thread(s, &s->wide_road_cam, process_road_camera));
selfdrive/camerad/cameras/camera_qcom2.h
+7 -14
View File
@@ -10,30 +10,23 @@
#include "selfdrive/common/util.h"
#define FRAME_BUF_COUNT 4
#define ANALOG_GAIN_MAX_IDX 10 // 0xF is bypass
#define EXPOSURE_TIME_MIN 2 // with HDR, fastest ss
#define EXPOSURE_TIME_MAX 1904 // with HDR, slowest ss
#define EF_LOWPASS_K 0.35
#define DEBAYER_LOCAL_WORKSIZE 16
typedef struct CameraState {
MultiCameraState *multi_cam_state;
CameraInfo ci;
std::mutex exp_lock;
float analog_gain_frac;
uint16_t analog_gain;
bool dc_gain_enabled;
int exposure_time;
int exposure_time_min;
int exposure_time_max;
float ef_filtered;
bool dc_gain_enabled;
float analog_gain_frac;
float cur_ev[3];
float min_ev, max_ev;
float measured_grey_fraction;
float target_grey_fraction;
int gain_idx;
unique_fd sensor_fd;
unique_fd csiphy_fd;
selfdrive/camerad/cameras/real_debayer.cl
+4 -6
View File
@@ -26,9 +26,9 @@ half mf(half x, half cp) {
}
}
half3 color_correct(half3 rgb, int ggain) {
half3 color_correct(half3 rgb) {
half3 ret = (0,0,0);
half cpx = 0.01; //clamp(0.01h, 0.05h, cpxb + cpxk * min(10, ggain));
half cpx = 0.01;
ret += (half)rgb.x * color_correction[0];
ret += (half)rgb.y * color_correction[1];
ret += (half)rgb.z * color_correction[2];
@@ -89,8 +89,7 @@ half phi(half x) {
__kernel void debayer10(const __global uchar * in,
__global uchar * out,
__local half * cached,
uint ggain
__local half * cached
)
{
const int x_global = get_global_id(0);
@@ -200,10 +199,9 @@ __kernel void debayer10(const __global uchar * in,
}
rgb = clamp(0.0h, 1.0h, rgb);
rgb = color_correct(rgb, (int)ggain);
rgb = color_correct(rgb);
out[out_idx + 0] = (uchar)(rgb.z);
out[out_idx + 1] = (uchar)(rgb.y);
out[out_idx + 2] = (uchar)(rgb.x);
}
selfdrive/camerad/cameras/sensor2_i2c.h
+49 -22
View File
@@ -14,13 +14,19 @@ struct i2c_random_wr_payload init_array_ar0231[] = {
// 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_
{0x3004, 0x0000}, // X_ADDR_START_ (A)
{0x308A, 0x0000}, // X_ADDR_START_ (B)
{0x3008, 0x0787}, // X_ADDR_END_ (A)
{0x308E, 0x0787}, // X_ADDR_END_ (B)
{0x3002, 0x0000}, // Y_ADDR_START_ (A)
{0x308C, 0x0000}, // Y_ADDR_START_ (B)
{0x3006, 0x04B7}, // Y_ADDR_END_ (A)
{0x3090, 0x04B7}, // Y_ADDR_END_ (B)
{0x3032, 0x0000}, // SCALING_MODE
{0x30A2, 0x0001}, // X_ODD_INC_
{0x30A6, 0x0001}, // Y_ODD_INC_
{0x30A2, 0x0001}, // X_ODD_INC_ (A)
{0x30AE, 0x0001}, // X_ODD_INC_ (B)
{0x30A6, 0x0001}, // Y_ODD_INC_ (A)
{0x30A8, 0x0001}, // Y_ODD_INC_ (B)
{0x3402, 0x0F10}, // X_OUTPUT_CONTROL
{0x3404, 0x0970}, // Y_OUTPUT_CONTROL
{0x3064, 0x1802}, // SMIA_TEST
@@ -32,8 +38,10 @@ struct i2c_random_wr_payload init_array_ar0231[] = {
{0x340C, 0x802}, // 2 // 0000 0000 0010
// Readout timing
{0x300C, 0x07B9}, // LINE_LENGTH_PCK
{0x300A, 0x07E7}, // FRAME_LENGTH_LINES
{0x300C, 0x07B9}, // LINE_LENGTH_PCK (A)
{0x303E, 0x07B9}, // LINE_LENGTH_PCK (B)
{0x300A, 0x07E7}, // FRAME_LENGTH_LINES (A)
{0x30AA, 0x07E7}, // FRAME_LENGTH_LINES (B)
{0x3042, 0x0000}, // EXTRA_DELAY
// Readout Settings
@@ -49,7 +57,7 @@ struct i2c_random_wr_payload init_array_ar0231[] = {
{0x3350, 0x0311}, // MIPI_F4_VDT_VC
{0x31B0, 0x0053}, // FRAME_PREAMBLE
{0x31B2, 0x003B}, // LINE_PREAMBLE
{0x301A, 0x01C}, // RESET_REGISTER
{0x301A, 0x001C}, // RESET_REGISTER
// Noise Corrections
{0x3092, 0x0C24}, // ROW_NOISE_CONTROL
@@ -62,10 +70,18 @@ struct i2c_random_wr_payload init_array_ar0231[] = {
{0x31E0, 0x0003},
// HDR Settings
{0x3082, 0x0004}, // OPERATION_MODE_CTRL
{0x3238, 0x0004}, // EXPOSURE_RATIO
{0x3014, 0x098E}, // FINE_INTEGRATION_TIME_
{0x321E, 0x098E}, // FINE_INTEGRATION_TIME2
{0x3082, 0x0004}, // OPERATION_MODE_CTRL (A)
{0x3084, 0x0004}, // OPERATION_MODE_CTRL (B)
{0x3238, 0x0004}, // EXPOSURE_RATIO (A)
{0x323A, 0x0004}, // EXPOSURE_RATIO (B)
{0x3014, 0x098E}, // FINE_INTEGRATION_TIME_ (A)
{0x3018, 0x098E}, // FINE_INTEGRATION_TIME_ (B)
{0x321E, 0x098E}, // FINE_INTEGRATION_TIME2 (A)
{0x3220, 0x098E}, // FINE_INTEGRATION_TIME2 (B)
{0x31D0, 0x0000}, // COMPANDING, no good in 10 bit?
{0x33DA, 0x0000}, // COMPANDING
{0x318E, 0x0200}, // PRE_HDR_GAIN_EN
@@ -82,16 +98,27 @@ struct i2c_random_wr_payload init_array_ar0231[] = {
{0x328E, 0x0FA0}, // T2 G2
// Initial Gains
{0x3022, 0x01}, // GROUPED_PARAMETER_HOLD_
{0x3366, 0x5555}, // ANALOG_GAIN
{0x3022, 0x0001}, // GROUPED_PARAMETER_HOLD_
{0x3366, 0xFF77}, // ANALOG_GAIN (1x) (A)
{0x3368, 0xFF77}, // ANALOG_GAIN (1x) (B)
{0x3060, 0x3333}, // ANALOG_COLOR_GAIN
{0x3362, 0x0000}, // DC GAIN
{0x305A, 0x0108}, // RED_GAIN
{0x3058, 0x00FB}, // BLUE_GAIN
{0x3056, 0x009A}, // GREEN1_GAIN
{0x305C, 0x009A}, // GREEN2_GAIN
{0x3022, 0x00}, // GROUPED_PARAMETER_HOLD_
{0x3362, 0x0000}, // DC GAIN (A & B)
{0x305A, 0x00F8}, // red gain (A)
{0x3058, 0x0122}, // blue gain (A)
{0x3056, 0x009A}, // g1 gain (A)
{0x305C, 0x009A}, // g2 gain (A)
{0x30C0, 0x00F8}, // red gain (B)
{0x30BE, 0x0122}, // blue gain (B)
{0x30BC, 0x009A}, // g1 gain (B)
{0x30C2, 0x009A}, // g2 gain (B)
{0x3022, 0x0000}, // GROUPED_PARAMETER_HOLD_
// Initial Integration Time
{0x3012, 0x256},
{0x3012, 0x0005}, // (A)
{0x3016, 0x0005}, // (B)
};
selfdrive/car/fw_versions.py
+1 -1
View File
@@ -253,7 +253,7 @@ def match_fw_to_car(fw_versions, allow_fuzzy=True):
def get_fw_versions(logcan, sendcan, bus, extra=None, timeout=0.1, debug=False, progress=False):
ecu_types = {}
# Extract ECU adresses to query from fingerprints
# Extract ECU addresses to query from fingerprints
# ECUs using a subadress need be queried one by one, the rest can be done in parallel
addrs = []
parallel_addrs = []
selfdrive/car/honda/carcontroller.py
+49 -24
View File
@@ -5,17 +5,18 @@ from selfdrive.controls.lib.drive_helpers import rate_limit
from common.numpy_fast import clip, interp
from selfdrive.car import create_gas_command
from selfdrive.car.honda import hondacan
from selfdrive.car.honda.values import CruiseButtons, CAR, VISUAL_HUD, HONDA_BOSCH, CarControllerParams
from selfdrive.car.honda.values import OLD_NIDEC_LONG_CONTROL, CruiseButtons, CAR, VISUAL_HUD, HONDA_BOSCH, CarControllerParams
from opendbc.can.packer import CANPacker
VisualAlert = car.CarControl.HUDControl.VisualAlert
# TODO: not clear this does anything useful
def actuator_hystereses(brake, braking, brake_steady, v_ego, car_fingerprint):
# hyst params
brake_hyst_on = 0.02 # to activate brakes exceed this value
brake_hyst_off = 0.005 # to deactivate brakes below this value
brake_hyst_gap = 0.01 # don't change brake command for small oscillations within this value
brake_hyst_off = 0.005 # to deactivate brakes below this value
brake_hyst_gap = 0.01 # don't change brake command for small oscillations within this value
#*** hysteresis logic to avoid brake blinking. go above 0.1 to trigger
if (brake < brake_hyst_on and not braking) or brake < brake_hyst_off:
@@ -72,7 +73,7 @@ def process_hud_alert(hud_alert):
HUDData = namedtuple("HUDData",
["pcm_accel", "v_cruise", "car",
["pcm_accel", "v_cruise", "car",
"lanes", "fcw", "acc_alert", "steer_required"])
@@ -84,7 +85,6 @@ class CarController():
self.apply_brake_last = 0
self.last_pump_ts = 0.
self.packer = CANPacker(dbc_name)
self.new_radar_config = False
self.params = CarControllerParams(CP)
@@ -125,8 +125,6 @@ class CarController():
fcw_display, steer_required, acc_alert = process_hud_alert(hud_alert)
hud = HUDData(int(pcm_accel), int(round(hud_v_cruise)), hud_car,
hud_lanes, fcw_display, acc_alert, steer_required)
# **** process the car messages ****
@@ -148,10 +146,35 @@ class CarController():
can_sends.append(hondacan.create_steering_control(self.packer, apply_steer,
lkas_active, CS.CP.carFingerprint, idx, CS.CP.openpilotLongitudinalControl))
# Send dashboard UI commands.
if (frame % 10) == 0:
idx = (frame//10) % 4
can_sends.extend(hondacan.create_ui_commands(self.packer, pcm_speed, hud, CS.CP.carFingerprint, CS.is_metric, idx, CS.CP.openpilotLongitudinalControl, CS.stock_hud))
accel = actuators.gas - actuators.brake
# TODO: pass in LoC.long_control_state and use that to decide starting/stoppping
stopping = accel < 0 and CS.out.vEgo < 0.3
starting = accel > 0 and CS.out.vEgo < 0.3
# Prevent rolling backwards
accel = -1.0 if stopping else accel
if CS.CP.carFingerprint in HONDA_BOSCH:
apply_accel = interp(accel, P.BOSCH_ACCEL_LOOKUP_BP, P.BOSCH_ACCEL_LOOKUP_V)
else:
apply_accel = interp(accel, P.NIDEC_ACCEL_LOOKUP_BP, P.NIDEC_ACCEL_LOOKUP_V)
# wind brake from air resistance decel at high speed
wind_brake = interp(CS.out.vEgo, [0.0, 2.3, 35.0], [0.001, 0.002, 0.15])
if CS.CP.carFingerprint in OLD_NIDEC_LONG_CONTROL:
#pcm_speed = pcm_speed
pcm_accel = int(clip(pcm_accel, 0, 1) * 0xc6)
else:
max_accel = interp(CS.out.vEgo, P.NIDEC_MAX_ACCEL_BP, P.NIDEC_MAX_ACCEL_V)
pcm_accel = int(clip(apply_accel/max_accel, 0.0, 1.0) * 0xc6)
pcm_speed_BP = [-wind_brake,
-wind_brake*(3/4),
0.0]
pcm_speed_V = [0.0,
clip(CS.out.vEgo + apply_accel/2.0 - 2.0, 0.0, 100.0),
clip(CS.out.vEgo + apply_accel/2.0 + 2.0, 0.0, 100.0)]
pcm_speed = interp(accel, pcm_speed_BP, pcm_speed_V)
if not CS.CP.openpilotLongitudinalControl:
if (frame % 2) == 0:
@@ -168,31 +191,33 @@ class CarController():
if (frame % 2) == 0:
idx = frame // 2
ts = frame * DT_CTRL
if CS.CP.carFingerprint in HONDA_BOSCH:
accel = actuators.gas - actuators.brake
# TODO: pass in LoC.long_control_state and use that to decide starting/stoppping
stopping = accel < 0 and CS.out.vEgo < 0.3
starting = accel > 0 and CS.out.vEgo < 0.3
# Prevent rolling backwards
accel = -1.0 if stopping else accel
apply_accel = interp(accel, P.BOSCH_ACCEL_LOOKUP_BP, P.BOSCH_ACCEL_LOOKUP_V)
apply_gas = interp(accel, P.BOSCH_GAS_LOOKUP_BP, P.BOSCH_GAS_LOOKUP_V)
can_sends.extend(hondacan.create_acc_commands(self.packer, enabled, apply_accel, apply_gas, idx, stopping, starting, CS.CP.carFingerprint))
else:
apply_gas = clip(actuators.gas, 0., 1.)
apply_brake = int(clip(self.brake_last * P.BRAKE_MAX, 0, P.BRAKE_MAX - 1))
apply_brake = clip(self.brake_last - wind_brake, 0.0, 1.0)
apply_brake = int(clip(apply_brake * P.BRAKE_MAX, 0, P.BRAKE_MAX - 1))
pump_on, self.last_pump_ts = brake_pump_hysteresis(apply_brake, self.apply_brake_last, self.last_pump_ts, ts)
can_sends.append(hondacan.create_brake_command(self.packer, apply_brake, pump_on,
pcm_override, pcm_cancel_cmd, hud.fcw, idx, CS.CP.carFingerprint, CS.stock_brake))
pcm_override, pcm_cancel_cmd, fcw_display, idx, CS.CP.carFingerprint, CS.stock_brake))
self.apply_brake_last = apply_brake
if CS.CP.enableGasInterceptor:
# way too aggressive at low speed without this
gas_mult = interp(CS.out.vEgo, [0., 10.], [0.4, 1.0])
# send exactly zero if apply_gas is zero. Interceptor will send the max between read value and apply_gas.
# This prevents unexpected pedal range rescaling
apply_gas = clip(gas_mult * actuators.gas, 0., 1.)
can_sends.append(create_gas_command(self.packer, apply_gas, idx))
hud = HUDData(int(pcm_accel), int(round(hud_v_cruise)), hud_car,
hud_lanes, fcw_display, acc_alert, steer_required)
# Send dashboard UI commands.
if (frame % 10) == 0:
idx = (frame//10) % 4
can_sends.extend(hondacan.create_ui_commands(self.packer, pcm_speed, hud, CS.CP.carFingerprint, CS.is_metric, idx, CS.CP.openpilotLongitudinalControl, CS.stock_hud))
return can_sends
selfdrive/car/honda/carstate.py
+6 -6
View File
@@ -11,7 +11,7 @@ TransmissionType = car.CarParams.TransmissionType
def calc_cruise_offset(offset, speed):
# euristic formula so that speed is controlled to ~ 0.3m/s below pid_speed
# heuristic formula so that speed is controlled to ~ 0.3m/s below pid_speed
# constraints to solve for _K0, _K1, _K2 are:
# - speed = 0m/s, out = -0.3
# - speed = 34m/s, offset = 20, out = -0.25
@@ -119,7 +119,7 @@ def get_can_signals(CP, gearbox_msg="GEARBOX"):
else:
checks += [("CRUISE_PARAMS", 50)]
if CP.carFingerprint in (CAR.ACCORD, CAR.ACCORDH, CAR.CIVIC_BOSCH, CAR.CIVIC_BOSCH_DIESEL, CAR.CRV_HYBRID, CAR.INSIGHT, CAR.ACURA_RDX_3G):
if CP.carFingerprint in (CAR.ACCORD, CAR.ACCORD_2021, CAR.ACCORDH, CAR.ACCORDH_2021, CAR.CIVIC_BOSCH, CAR.CIVIC_BOSCH_DIESEL, CAR.CRV_HYBRID, CAR.INSIGHT, CAR.ACURA_RDX_3G):
signals += [("DRIVERS_DOOR_OPEN", "SCM_FEEDBACK", 1)]
elif CP.carFingerprint == CAR.ODYSSEY_CHN:
signals += [("DRIVERS_DOOR_OPEN", "SCM_BUTTONS", 1)]
@@ -230,7 +230,7 @@ class CarState(CarStateBase):
# ******************* parse out can *******************
# TODO: find wheels moving bit in dbc
if self.CP.carFingerprint in (CAR.ACCORD, CAR.ACCORDH, CAR.CIVIC_BOSCH, CAR.CIVIC_BOSCH_DIESEL, CAR.CRV_HYBRID, CAR.INSIGHT, CAR.ACURA_RDX_3G):
if self.CP.carFingerprint in (CAR.ACCORD, CAR.ACCORD_2021, CAR.ACCORDH, CAR.ACCORDH_2021, CAR.CIVIC_BOSCH, CAR.CIVIC_BOSCH_DIESEL, CAR.CRV_HYBRID, CAR.INSIGHT, CAR.ACURA_RDX_3G):
ret.standstill = cp.vl["ENGINE_DATA"]["XMISSION_SPEED"] < 0.1
ret.doorOpen = bool(cp.vl["SCM_FEEDBACK"]["DRIVERS_DOOR_OPEN"])
elif self.CP.carFingerprint == CAR.ODYSSEY_CHN:
@@ -257,7 +257,7 @@ class CarState(CarStateBase):
self.brake_error = cp.vl["STANDSTILL"]["BRAKE_ERROR_1"] or cp.vl["STANDSTILL"]["BRAKE_ERROR_2"]
ret.espDisabled = cp.vl["VSA_STATUS"]["ESP_DISABLED"] != 0
speed_factor = SPEED_FACTOR[self.CP.carFingerprint]
speed_factor = SPEED_FACTOR.get(self.CP.carFingerprint, 1.)
ret.wheelSpeeds.fl = cp.vl["WHEEL_SPEEDS"]["WHEEL_SPEED_FL"] * CV.KPH_TO_MS * speed_factor
ret.wheelSpeeds.fr = cp.vl["WHEEL_SPEEDS"]["WHEEL_SPEED_FR"] * CV.KPH_TO_MS * speed_factor
ret.wheelSpeeds.rl = cp.vl["WHEEL_SPEEDS"]["WHEEL_SPEED_RL"] * CV.KPH_TO_MS * speed_factor
@@ -279,7 +279,7 @@ class CarState(CarStateBase):
250, cp.vl["SCM_FEEDBACK"]["LEFT_BLINKER"], cp.vl["SCM_FEEDBACK"]["RIGHT_BLINKER"])
self.brake_hold = cp.vl["VSA_STATUS"]["BRAKE_HOLD_ACTIVE"]
if self.CP.carFingerprint in (CAR.CIVIC, CAR.ODYSSEY, CAR.CRV_5G, CAR.ACCORD, CAR.ACCORDH, CAR.CIVIC_BOSCH,
if self.CP.carFingerprint in (CAR.CIVIC, CAR.ODYSSEY, CAR.CRV_5G, CAR.ACCORD, CAR.ACCORD_2021, CAR.ACCORDH, CAR.ACCORDH_2021, CAR.CIVIC_BOSCH,
CAR.CIVIC_BOSCH_DIESEL, CAR.CRV_HYBRID, CAR.INSIGHT, CAR.ACURA_RDX_3G):
self.park_brake = cp.vl["EPB_STATUS"]["EPB_STATE"] != 0
main_on = cp.vl["SCM_FEEDBACK"]["MAIN_ON"]
@@ -311,7 +311,7 @@ class CarState(CarStateBase):
ret.steeringTorque = cp.vl["STEER_STATUS"]["STEER_TORQUE_SENSOR"]
ret.steeringTorqueEps = cp.vl["STEER_MOTOR_TORQUE"]["MOTOR_TORQUE"]
ret.steeringPressed = abs(ret.steeringTorque) > STEER_THRESHOLD[self.CP.carFingerprint]
ret.steeringPressed = abs(ret.steeringTorque) > STEER_THRESHOLD.get(self.CP.carFingerprint, 1200)
if self.CP.carFingerprint in HONDA_BOSCH:
if not self.CP.openpilotLongitudinalControl:
selfdrive/car/honda/hondacan.py
+13 -4
View File
@@ -1,7 +1,7 @@
from selfdrive.car.isotp_parallel_query import IsoTpParallelQuery
from selfdrive.swaglog import cloudlog
from selfdrive.car.honda.values import HONDA_BOSCH, HONDA_BOSCH_EXT, CAR
from selfdrive.config import Conversions as CV
from selfdrive.car.honda.values import HONDA_BOSCH
from selfdrive.swaglog import cloudlog
# CAN bus layout with relay
# 0 = ACC-CAN - radar side
@@ -169,12 +169,18 @@ def create_ui_commands(packer, pcm_speed, hud, car_fingerprint, is_metric, idx,
lkas_hud_values = {
'SET_ME_X41': 0x41,
'SET_ME_X48': 0x48,
'STEERING_REQUIRED': hud.steer_required,
'SOLID_LANES': hud.lanes,
'BEEP': 0,
}
commands.append(packer.make_can_msg('LKAS_HUD', bus_lkas, lkas_hud_values, idx))
if car_fingerprint not in HONDA_BOSCH_EXT:
lkas_hud_values['SET_ME_X48'] = 0x48
if car_fingerprint in HONDA_BOSCH_EXT and not openpilot_longitudinal_control:
commands.append(packer.make_can_msg('LKAS_HUD_A', bus_lkas, lkas_hud_values, idx))
commands.append(packer.make_can_msg('LKAS_HUD_B', bus_lkas, lkas_hud_values, idx))
else:
commands.append(packer.make_can_msg('LKAS_HUD', bus_lkas, lkas_hud_values, idx))
if radar_disabled and car_fingerprint in HONDA_BOSCH:
radar_hud_values = {
@@ -182,6 +188,9 @@ def create_ui_commands(packer, pcm_speed, hud, car_fingerprint, is_metric, idx,
}
commands.append(packer.make_can_msg('RADAR_HUD', bus_pt, radar_hud_values, idx))
if car_fingerprint == CAR.CIVIC_BOSCH:
commands.append(packer.make_can_msg("LEGACY_BRAKE_COMMAND", bus_pt, {}, idx))
return commands
selfdrive/car/honda/interface.py
+16 -119
View File
@@ -1,14 +1,13 @@
#!/usr/bin/env python3
import numpy as np
from cereal import car
from panda import Panda
from common.numpy_fast import clip, interp
from common.numpy_fast import interp
from common.params import Params
from selfdrive.config import Conversions as CV
from selfdrive.car.honda.values import CarControllerParams, CruiseButtons, CAR, HONDA_BOSCH, HONDA_BOSCH_ALT_BRAKE_SIGNAL
from selfdrive.car.honda.hondacan import disable_radar
from selfdrive.car import STD_CARGO_KG, CivicParams, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint
from selfdrive.car.interfaces import CarInterfaceBase
from selfdrive.config import Conversions as CV
ButtonType = car.CarState.ButtonEvent.Type
@@ -29,58 +28,17 @@ def compute_gb_honda_nidec(accel, speed):
return float(accel) / 4.8 - creep_brake
def get_compute_gb_acura():
# generate a function that takes in [desired_accel, current_speed] -> [-1.0, 1.0]
# where -1.0 is max brake and 1.0 is max gas
# see debug/dump_accel_from_fiber.py to see how those parameters were generated
w0 = np.array([[ 1.22056961, -0.39625418, 0.67952657],
[ 1.03691769, 0.78210306, -0.41343188]])
b0 = np.array([ 0.01536703, -0.14335321, -0.26932889])
w2 = np.array([[-0.59124422, 0.42899439, 0.38660881],
[ 0.79973811, 0.13178682, 0.08550351],
[-0.15651935, -0.44360259, 0.76910877]])
b2 = np.array([ 0.15624429, 0.02294923, -0.0341086 ])
w4 = np.array([[-0.31521443],
[-0.38626176],
[ 0.52667892]])
b4 = np.array([-0.02922216])
def compute_output(dat, w0, b0, w2, b2, w4, b4):
m0 = np.dot(dat, w0) + b0
m0 = leakyrelu(m0, 0.1)
m2 = np.dot(m0, w2) + b2
m2 = leakyrelu(m2, 0.1)
m4 = np.dot(m2, w4) + b4
return m4
def leakyrelu(x, alpha):
return np.maximum(x, alpha * x)
def _compute_gb_acura(accel, speed):
# linearly extrap below v1 using v1 and v2 data
v1 = 5.
v2 = 10.
dat = np.array([accel, speed])
if speed > 5.:
m4 = compute_output(dat, w0, b0, w2, b2, w4, b4)
else:
dat[1] = v1
m4v1 = compute_output(dat, w0, b0, w2, b2, w4, b4)
dat[1] = v2
m4v2 = compute_output(dat, w0, b0, w2, b2, w4, b4)
m4 = (speed - v1) * (m4v2 - m4v1) / (v2 - v1) + m4v1
return float(m4)
return _compute_gb_acura
def compute_gb_acura(accel, speed):
GB_VALUES = [-2., 0.0, 0.8]
GB_BP = [-5., 0.0, 4.0]
return interp(accel, GB_BP, GB_VALUES)
class CarInterface(CarInterfaceBase):
def __init__(self, CP, CarController, CarState):
super().__init__(CP, CarController, CarState)
if self.CS.CP.carFingerprint == CAR.ACURA_ILX:
self.compute_gb = get_compute_gb_acura()
elif self.CS.CP.carFingerprint in HONDA_BOSCH:
if self.CS.CP.carFingerprint in HONDA_BOSCH:
self.compute_gb = compute_gb_honda_bosch
else:
self.compute_gb = compute_gb_honda_nidec
@@ -159,6 +117,12 @@ class CarInterface(CarInterfaceBase):
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]]
ret.lateralTuning.pid.kf = 0.00006 # conservative feed-forward
# default longitudinal tuning for all hondas
ret.longitudinalTuning.kpBP = [0., 5., 35.]
ret.longitudinalTuning.kpV = [1.2, 0.8, 0.5]
ret.longitudinalTuning.kiBP = [0., 35.]
ret.longitudinalTuning.kiV = [0.18, 0.12]
eps_modified = False
for fw in car_fw:
if fw.ecu == "eps" and b"," in fw.fwVersion:
@@ -184,11 +148,6 @@ class CarInterface(CarInterfaceBase):
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[1.1], [0.33]]
tire_stiffness_factor = 1.
ret.longitudinalTuning.kpBP = [0., 5., 35.]
ret.longitudinalTuning.kpV = [3.6, 2.4, 1.5]
ret.longitudinalTuning.kiBP = [0., 35.]
ret.longitudinalTuning.kiV = [0.54, 0.36]
elif candidate in (CAR.CIVIC_BOSCH, CAR.CIVIC_BOSCH_DIESEL):
stop_and_go = True
ret.mass = CivicParams.MASS
@@ -198,12 +157,8 @@ class CarInterface(CarInterfaceBase):
ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 4096], [0, 4096]] # TODO: determine if there is a dead zone at the top end
tire_stiffness_factor = 1.
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.8], [0.24]]
ret.longitudinalTuning.kpBP = [0., 5., 35.]
ret.longitudinalTuning.kpV = [1.2, 0.8, 0.5]
ret.longitudinalTuning.kiBP = [0., 35.]
ret.longitudinalTuning.kiV = [0.18, 0.12]
elif candidate in (CAR.ACCORD, CAR.ACCORDH):
elif candidate in (CAR.ACCORD, CAR.ACCORD_2021, CAR.ACCORDH, CAR.ACCORDH_2021):
stop_and_go = True
ret.mass = 3279. * CV.LB_TO_KG + STD_CARGO_KG
ret.wheelbase = 2.83
@@ -211,10 +166,6 @@ class CarInterface(CarInterfaceBase):
ret.steerRatio = 16.33 # 11.82 is spec end-to-end
ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 4096], [0, 4096]] # TODO: determine if there is a dead zone at the top end
tire_stiffness_factor = 0.8467
ret.longitudinalTuning.kpBP = [0., 5., 35.]
ret.longitudinalTuning.kpV = [1.2, 0.8, 0.5]
ret.longitudinalTuning.kiBP = [0., 35.]
ret.longitudinalTuning.kiV = [0.18, 0.12]
if eps_modified:
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.3], [0.09]]
@@ -230,10 +181,6 @@ class CarInterface(CarInterfaceBase):
ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 3840], [0, 3840]] # TODO: determine if there is a dead zone at the top end
tire_stiffness_factor = 0.72
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.8], [0.24]]
ret.longitudinalTuning.kpBP = [0., 5., 35.]
ret.longitudinalTuning.kpV = [1.2, 0.8, 0.5]
ret.longitudinalTuning.kiBP = [0., 35.]
ret.longitudinalTuning.kiV = [0.18, 0.12]
elif candidate in (CAR.CRV, CAR.CRV_EU):
stop_and_go = False
@@ -244,10 +191,6 @@ class CarInterface(CarInterfaceBase):
ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 1000], [0, 1000]] # TODO: determine if there is a dead zone at the top end
tire_stiffness_factor = 0.444
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.8], [0.24]]
ret.longitudinalTuning.kpBP = [0., 5., 35.]
ret.longitudinalTuning.kpV = [1.2, 0.8, 0.5]
ret.longitudinalTuning.kiBP = [0., 35.]
ret.longitudinalTuning.kiV = [0.18, 0.12]
elif candidate == CAR.CRV_5G:
stop_and_go = True
@@ -265,10 +208,6 @@ class CarInterface(CarInterfaceBase):
ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 3840], [0, 3840]]
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.64], [0.192]]
tire_stiffness_factor = 0.677
ret.longitudinalTuning.kpBP = [0., 5., 35.]
ret.longitudinalTuning.kpV = [1.2, 0.8, 0.5]
ret.longitudinalTuning.kiBP = [0., 35.]
ret.longitudinalTuning.kiV = [0.18, 0.12]
elif candidate == CAR.CRV_HYBRID:
stop_and_go = True
@@ -279,10 +218,6 @@ class CarInterface(CarInterfaceBase):
ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 4096], [0, 4096]] # TODO: determine if there is a dead zone at the top end
tire_stiffness_factor = 0.677
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.18]]
ret.longitudinalTuning.kpBP = [0., 5., 35.]
ret.longitudinalTuning.kpV = [1.2, 0.8, 0.5]
ret.longitudinalTuning.kiBP = [0., 35.]
ret.longitudinalTuning.kiV = [0.18, 0.12]
elif candidate == CAR.FIT:
stop_and_go = False
@@ -293,10 +228,6 @@ class CarInterface(CarInterfaceBase):
ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 4096], [0, 4096]] # TODO: determine if there is a dead zone at the top end
tire_stiffness_factor = 0.75
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.2], [0.05]]
ret.longitudinalTuning.kpBP = [0., 5., 35.]
ret.longitudinalTuning.kpV = [1.2, 0.8, 0.5]
ret.longitudinalTuning.kiBP = [0., 35.]
ret.longitudinalTuning.kiV = [0.18, 0.12]
elif candidate == CAR.HRV:
stop_and_go = False
@@ -307,10 +238,6 @@ class CarInterface(CarInterfaceBase):
ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 4096], [0, 4096]]
tire_stiffness_factor = 0.5
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.16], [0.025]]
ret.longitudinalTuning.kpBP = [0., 5., 35.]
ret.longitudinalTuning.kpV = [1.2, 0.8, 0.5]
ret.longitudinalTuning.kiBP = [0., 35.]
ret.longitudinalTuning.kiV = [0.18, 0.12]
elif candidate == CAR.ACURA_RDX:
stop_and_go = False
@@ -321,10 +248,6 @@ class CarInterface(CarInterfaceBase):
ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 1000], [0, 1000]] # TODO: determine if there is a dead zone at the top end
tire_stiffness_factor = 0.444
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.8], [0.24]]
ret.longitudinalTuning.kpBP = [0., 5., 35.]
ret.longitudinalTuning.kpV = [1.2, 0.8, 0.5]
ret.longitudinalTuning.kiBP = [0., 35.]
ret.longitudinalTuning.kiV = [0.18, 0.12]
elif candidate == CAR.ACURA_RDX_3G:
stop_and_go = True
@@ -335,10 +258,6 @@ class CarInterface(CarInterfaceBase):
ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 3840], [0, 3840]]
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.18]]
tire_stiffness_factor = 0.677
ret.longitudinalTuning.kpBP = [0., 5., 35.]
ret.longitudinalTuning.kpV = [1.2, 0.8, 0.5]
ret.longitudinalTuning.kiBP = [0., 35.]
ret.longitudinalTuning.kiV = [0.18, 0.12]
elif candidate == CAR.ODYSSEY:
stop_and_go = False
@@ -349,10 +268,6 @@ class CarInterface(CarInterfaceBase):
ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 4096], [0, 4096]] # TODO: determine if there is a dead zone at the top end
tire_stiffness_factor = 0.82
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.28], [0.08]]
ret.longitudinalTuning.kpBP = [0., 5., 35.]
ret.longitudinalTuning.kpV = [1.2, 0.8, 0.5]
ret.longitudinalTuning.kiBP = [0., 35.]
ret.longitudinalTuning.kiV = [0.18, 0.12]
elif candidate == CAR.ODYSSEY_CHN:
stop_and_go = False
@@ -363,10 +278,6 @@ class CarInterface(CarInterfaceBase):
ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 32767], [0, 32767]] # TODO: determine if there is a dead zone at the top end
tire_stiffness_factor = 0.82
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.28], [0.08]]
ret.longitudinalTuning.kpBP = [0., 5., 35.]
ret.longitudinalTuning.kpV = [1.2, 0.8, 0.5]
ret.longitudinalTuning.kiBP = [0., 35.]
ret.longitudinalTuning.kiV = [0.18, 0.12]
elif candidate in (CAR.PILOT, CAR.PILOT_2019):
stop_and_go = False
@@ -377,10 +288,6 @@ class CarInterface(CarInterfaceBase):
ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 4096], [0, 4096]] # TODO: determine if there is a dead zone at the top end
tire_stiffness_factor = 0.444
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.38], [0.11]]
ret.longitudinalTuning.kpBP = [0., 5., 35.]
ret.longitudinalTuning.kpV = [1.2, 0.8, 0.5]
ret.longitudinalTuning.kiBP = [0., 35.]
ret.longitudinalTuning.kiV = [0.18, 0.12]
elif candidate == CAR.RIDGELINE:
stop_and_go = False
@@ -391,10 +298,6 @@ class CarInterface(CarInterfaceBase):
ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 4096], [0, 4096]] # TODO: determine if there is a dead zone at the top end
tire_stiffness_factor = 0.444
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.38], [0.11]]
ret.longitudinalTuning.kpBP = [0., 5., 35.]
ret.longitudinalTuning.kpV = [1.2, 0.8, 0.5]
ret.longitudinalTuning.kiBP = [0., 35.]
ret.longitudinalTuning.kiV = [0.18, 0.12]
elif candidate == CAR.INSIGHT:
stop_and_go = True
@@ -405,10 +308,6 @@ class CarInterface(CarInterfaceBase):
ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 4096], [0, 4096]] # TODO: determine if there is a dead zone at the top end
tire_stiffness_factor = 0.82
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.18]]
ret.longitudinalTuning.kpBP = [0., 5., 35.]
ret.longitudinalTuning.kpV = [1.2, 0.8, 0.5]
ret.longitudinalTuning.kiBP = [0., 35.]
ret.longitudinalTuning.kiV = [0.18, 0.12]
else:
raise ValueError("unsupported car %s" % candidate)
@@ -441,7 +340,7 @@ class CarInterface(CarInterfaceBase):
ret.brakeMaxV = [1.] # max brake allowed, 3.5m/s^2
else:
ret.gasMaxBP = [0.] # m/s
ret.gasMaxV = [0.6] if ret.enableGasInterceptor else [0.] # max gas allowed
ret.gasMaxV = [0.6] # max gas allowed
ret.brakeMaxBP = [5., 20.] # m/s
ret.brakeMaxV = [1., 0.8] # max brake allowed
@@ -559,14 +458,12 @@ class CarInterface(CarInterfaceBase):
else:
hud_v_cruise = 255
pcm_accel = int(clip(c.cruiseControl.accelOverride, 0, 1) * 0xc6)
can_sends = self.CC.update(c.enabled, self.CS, self.frame,
c.actuators,
c.cruiseControl.speedOverride,
c.cruiseControl.override,
c.cruiseControl.cancel,
pcm_accel,
c.cruiseControl.accelOverride,
hud_v_cruise,
c.hudControl.lanesVisible,
hud_show_car=c.hudControl.leadVisible,
selfdrive/car/honda/values.py
+111 -62
View File
@@ -1,5 +1,3 @@
# flake8: noqa
from cereal import car
from selfdrive.car import dbc_dict
@@ -10,18 +8,25 @@ class CarControllerParams():
ACCEL_MAX = 1.6
def __init__(self, CP):
self.BRAKE_MAX = 1024//4
self.STEER_MAX = CP.lateralParams.torqueBP[-1]
# mirror of list (assuming first item is zero) for interp of signed request values
assert(CP.lateralParams.torqueBP[0] == 0)
assert(CP.lateralParams.torqueBP[0] == 0)
self.STEER_LOOKUP_BP = [v * -1 for v in CP.lateralParams.torqueBP][1:][::-1] + list(CP.lateralParams.torqueBP)
self.STEER_LOOKUP_V = [v * -1 for v in CP.lateralParams.torqueV][1:][::-1] + list(CP.lateralParams.torqueV)
self.BRAKE_MAX = 1024//4
self.STEER_MAX = CP.lateralParams.torqueBP[-1]
# mirror of list (assuming first item is zero) for interp of signed request values
assert(CP.lateralParams.torqueBP[0] == 0)
assert(CP.lateralParams.torqueBP[0] == 0)
self.STEER_LOOKUP_BP = [v * -1 for v in CP.lateralParams.torqueBP][1:][::-1] + list(CP.lateralParams.torqueBP)
self.STEER_LOOKUP_V = [v * -1 for v in CP.lateralParams.torqueV][1:][::-1] + list(CP.lateralParams.torqueV)
self.BOSCH_ACCEL_LOOKUP_BP = [-1., 0., 0.6]
self.BOSCH_ACCEL_LOOKUP_V = [-3.5, 0., 2.]
self.BOSCH_GAS_LOOKUP_BP = [0., 0.6]
self.BOSCH_GAS_LOOKUP_V = [0, 2000]
self.NIDEC_ACCEL_LOOKUP_BP = [-1., 0., .6]
self.NIDEC_ACCEL_LOOKUP_V = [-4.8, 0., 2.0]
self.NIDEC_MAX_ACCEL_V = [0.5, 2.4, 1.4, 0.6]
self.NIDEC_MAX_ACCEL_BP = [0.0, 4.0, 10., 20.]
self.BOSCH_ACCEL_LOOKUP_BP = [-1., 0., 0.6]
self.BOSCH_ACCEL_LOOKUP_V = [-3.5, 0., 2.]
self.BOSCH_GAS_LOOKUP_BP = [0., 0.6]
self.BOSCH_GAS_LOOKUP_V = [0, 2000]
# Car button codes
@@ -31,7 +36,7 @@ class CruiseButtons:
CANCEL = 2
MAIN = 1
# See dbc files for info on values"
# See dbc files for info on values
VISUAL_HUD = {
VisualAlert.none: 0,
VisualAlert.fcw: 1,
@@ -40,11 +45,14 @@ VISUAL_HUD = {
VisualAlert.brakePressed: 10,
VisualAlert.wrongGear: 6,
VisualAlert.seatbeltUnbuckled: 5,
VisualAlert.speedTooHigh: 8}
VisualAlert.speedTooHigh: 8
}
class CAR:
ACCORD = "HONDA ACCORD 2018"
ACCORDH = "HONDA ACCORD HYBRID 2018"
ACCORD_2021 = "HONDA ACCORD 2021"
ACCORDH_2021 = "HONDA ACCORD HYBRID 2021"
CIVIC = "HONDA CIVIC 2016"
CIVIC_BOSCH = "HONDA CIVIC (BOSCH) 2019"
CIVIC_BOSCH_DIESEL = "HONDA CIVIC SEDAN 1.6 DIESEL 2019"
@@ -71,7 +79,6 @@ FINGERPRINTS = {
CAR.ACCORDH: [{
148: 8, 228: 5, 304: 8, 330: 8, 344: 8, 380: 8, 387: 8, 388: 8, 399: 7, 419: 8, 420: 8, 427: 3, 432: 7, 441: 5, 450: 8, 464: 8, 477: 8, 479: 8, 495: 8, 525: 8, 545: 6, 662: 4, 773: 7, 777: 8, 780: 8, 804: 8, 806: 8, 808: 8, 829: 5, 862: 8, 884: 8, 891: 8, 927: 8, 929: 8, 1302: 8, 1600: 5, 1601: 8, 1652: 8
}],
# Acura RDX w/ Added Comma Pedal Support (512L & 513L)
CAR.ACURA_RDX: [{
57: 3, 145: 8, 229: 4, 308: 5, 316: 8, 342: 6, 344: 8, 380: 8, 392: 6, 398: 3, 399: 6, 404: 4, 420: 8, 422: 8, 426: 8, 432: 7, 464: 8, 474: 5, 476: 4, 487: 4, 490: 8, 506: 8, 512: 6, 513: 6, 542: 7, 545: 4, 597: 8, 660: 8, 773: 7, 777: 8, 780: 8, 800: 8, 804: 8, 808: 8, 819: 7, 821: 5, 829: 5, 882: 2, 884: 7, 887: 8, 888: 8, 892: 8, 923: 2, 929: 4, 963: 8, 965: 8, 966: 8, 967: 8, 983: 8, 985: 3, 1024: 5, 1027: 5, 1029: 8, 1033: 5, 1034: 5, 1036: 8, 1039: 8, 1057: 5, 1064: 7, 1108: 8, 1365: 5, 1424: 5, 1729: 1
}],
@@ -79,32 +86,26 @@ FINGERPRINTS = {
57: 3, 148: 8, 228: 5, 304: 8, 330: 8, 344: 8, 380: 8, 399: 7, 401: 8, 420: 8, 427: 3, 428: 8, 432: 7, 450: 8, 464: 8, 470: 2, 476: 7, 487: 4, 490: 8, 493: 5, 506: 8, 512: 6, 513: 6, 545: 6, 597: 8, 662: 4, 773: 7, 777: 8, 780: 8, 795: 8, 800: 8, 804: 8, 806: 8, 808: 8, 829: 5, 862: 8, 884: 8, 891: 8, 892: 8, 927: 8, 929: 8, 985: 3, 1024: 5, 1027: 5, 1029: 8, 1036: 8, 1039: 8, 1108: 8, 1302: 8, 1322: 5, 1361: 5, 1365: 5, 1424: 5, 1633: 8,
}],
CAR.CIVIC_BOSCH: [{
# 2017 Civic Hatchback EX, 2019 Civic Sedan Touring Canadian, and 2018 Civic Hatchback Executive Premium 1.0L CVT European
57: 3, 148: 8, 228: 5, 304: 8, 330: 8, 344: 8, 380: 8, 399: 7, 401: 8, 420: 8, 427: 3, 428: 8, 432: 7, 441: 5, 450: 8, 460: 3, 464: 8, 470: 2, 476: 7, 477: 8, 479: 8, 490: 8, 493: 5, 495: 8, 506: 8, 545: 6, 597: 8, 662: 4, 773: 7, 777: 8, 780: 8, 795: 8, 800: 8, 804: 8, 806: 8, 808: 8, 829: 5, 862: 8, 884: 8, 891: 8, 892: 8, 927: 8, 929: 8, 985: 3, 1024: 5, 1027: 5, 1029: 8, 1036: 8, 1039: 8, 1108: 8, 1302: 8, 1322: 5, 1361: 5, 1365: 5, 1424: 5, 1600: 5, 1601: 8, 1625: 5, 1629: 5, 1633: 8,
},
# 2017 Civic Hatchback LX
{
57: 3, 148: 8, 228: 5, 304: 8, 330: 8, 344: 8, 380: 8, 399: 7, 401: 8, 420: 8, 423: 2, 427: 3, 428: 8, 432: 7, 441: 5, 450: 8, 464: 8, 470: 2, 476: 7, 477: 8, 479: 8, 490: 8, 493: 5, 495: 8, 506: 8, 545: 6, 597: 8, 662: 4, 773: 7, 777: 8, 780: 8, 795: 8, 800: 8, 804: 8, 806: 8, 808: 8, 815: 8, 825: 4, 829: 5, 846: 8, 862: 8, 881: 8, 882: 4, 884: 8, 888: 8, 891: 8, 892: 8, 918: 7, 927: 8, 929: 8, 983: 8, 985: 3, 1024: 5, 1027: 5, 1029: 8, 1036: 8, 1039: 8, 1064: 7, 1092: 1, 1108: 8, 1125: 8, 1127: 2, 1296: 8, 1302: 8, 1322: 5, 1361: 5, 1365: 5, 1424: 5, 1600: 5, 1601: 8, 1633: 8
}],
CAR.CRV_5G: [{
57: 3, 148: 8, 199: 4, 228: 5, 231: 5, 232: 7, 304: 8, 330: 8, 340: 8, 344: 8, 380: 8, 399: 7, 401: 8, 420: 8, 423: 2, 427: 3, 428: 8, 432: 7, 441: 5, 446: 3, 450: 8, 464: 8, 467: 2, 469: 3, 470: 2, 474: 8, 476: 7, 477: 8, 479: 8, 490: 8, 493: 5, 495: 8, 507: 1, 545: 6, 597: 8, 661: 4, 662: 4, 773: 7, 777: 8, 780: 8, 795: 8, 800: 8, 804: 8, 806: 8, 808: 8, 814: 4, 815: 8, 817: 4, 825: 4, 829: 5, 862: 8, 881: 8, 882: 4, 884: 8, 888: 8, 891: 8, 927: 8, 918: 7, 929: 8, 983: 8, 985: 3, 1024: 5, 1027: 5, 1029: 8, 1036: 8, 1039: 8, 1064: 7, 1108: 8, 1092: 1, 1115: 2, 1125: 8, 1127: 2, 1296: 8, 1302: 8, 1322: 5, 1361: 5, 1365: 5, 1424: 5, 1600: 5, 1601: 8, 1618: 5, 1633: 8, 1670: 5
}],
# 2018 Odyssey w/ Added Comma Pedal Support (512L & 513L)
CAR.ODYSSEY: [{
57: 3, 148: 8, 228: 5, 229: 4, 316: 8, 342: 6, 344: 8, 380: 8, 399: 7, 411: 5, 419: 8, 420: 8, 427: 3, 432: 7, 450: 8, 463: 8, 464: 8, 476: 4, 490: 8, 506: 8, 512: 6, 513: 6, 542: 7, 545: 6, 597: 8, 662: 4, 773: 7, 777: 8, 780: 8, 795: 8, 800: 8, 804: 8, 806: 8, 808: 8, 817: 4, 819: 7, 821: 5, 825: 4, 829: 5, 837: 5, 856: 7, 862: 8, 871: 8, 881: 8, 882: 4, 884: 8, 891: 8, 892: 8, 905: 8, 923: 2, 927: 8, 929: 8, 963: 8, 965: 8, 966: 8, 967: 8, 983: 8, 985: 3, 1029: 8, 1036: 8, 1052: 8, 1064: 7, 1088: 8, 1089: 8, 1092: 1, 1108: 8, 1110: 8, 1125: 8, 1296: 8, 1302: 8, 1600: 5, 1601: 8, 1612: 5, 1613: 5, 1614: 5, 1615: 8, 1616: 5, 1619: 5, 1623: 5, 1668: 5
},
# 2018 Odyssey Elite w/ Added Comma Pedal Support (512L & 513L)
{
57: 3, 148: 8, 228: 5, 229: 4, 304: 8, 342: 6, 344: 8, 380: 8, 399: 7, 411: 5, 419: 8, 420: 8, 427: 3, 432: 7, 440: 8, 450: 8, 463: 8, 464: 8, 476: 4, 490: 8, 506: 8, 507: 1, 542: 7, 545: 6, 597: 8, 662: 4, 773: 7, 777: 8, 780: 8, 795: 8, 800: 8, 804: 8, 806: 8, 808: 8, 817: 4, 819: 7, 821: 5, 825: 4, 829: 5, 837: 5, 856: 7, 862: 8, 871: 8, 881: 8, 882: 4, 884: 8, 891: 8, 892: 8, 905: 8, 923: 2, 927: 8, 929: 8, 963: 8, 965: 8, 966: 8, 967: 8, 983: 8, 985: 3, 1029: 8, 1036: 8, 1052: 8, 1064: 7, 1088: 8, 1089: 8, 1092: 1, 1108: 8, 1110: 8, 1125: 8, 1296: 8, 1302: 8, 1600: 5, 1601: 8, 1612: 5, 1613: 5, 1614: 5, 1616: 5, 1619: 5, 1623: 5, 1668: 5
}],
CAR.ODYSSEY_CHN: [{
57: 3, 145: 8, 316: 8, 342: 6, 344: 8, 380: 8, 398: 3, 399: 7, 401: 8, 404: 4, 411: 5, 420: 8, 422: 8, 423: 2, 426: 8, 432: 7, 450: 8, 464: 8, 490: 8, 506: 8, 507: 1, 512: 6, 513: 6, 597: 8, 610: 8, 611: 8, 612: 8, 617: 8, 660: 8, 661: 4, 773: 7, 780: 8, 804: 8, 808: 8, 829: 5, 862: 8, 884: 7, 892: 8, 923: 2, 929: 8, 1030: 5, 1137: 8, 1302: 8, 1348: 5, 1361: 5, 1365: 5, 1600: 5, 1601: 8, 1639: 8
}],
# this fingerprint also includes the Passport 2019
CAR.PILOT_2019: [{
57: 3, 145: 8, 228: 5, 308: 5, 316: 8, 334: 8, 342: 6, 344: 8, 379: 8, 380: 8, 399: 7, 411: 5, 419: 8, 420: 8, 422: 8, 425: 8, 426: 8, 427: 3, 432: 7, 463: 8, 464: 8, 476: 4, 490: 8, 506: 8, 512: 6, 513: 6, 538: 3, 542: 7, 545: 5, 546: 3, 597: 8, 660: 8, 773: 7, 777: 8, 780: 8, 795: 8, 800: 8, 804: 8, 808: 8, 817: 4, 819: 7, 821: 5, 825: 4, 829: 5, 837: 5, 856: 7, 871: 8, 881: 8, 882: 2, 884: 7, 891: 8, 892: 8, 923: 2, 927: 8, 929: 8, 983: 8, 985: 3, 1029: 8, 1052: 8, 1064: 7, 1088: 8, 1089: 8, 1092: 1, 1108: 8, 1110: 8, 1125: 8, 1296: 8, 1424: 5, 1445: 8, 1600: 5, 1601: 8, 1612: 5, 1613: 5, 1614: 5, 1615: 8, 1616: 5, 1617: 8, 1618: 5, 1623: 5, 1668: 5
},
# 2019 Pilot EX-L
{
57: 3, 145: 8, 228: 5, 229: 4, 308: 5, 316: 8, 339: 7, 342: 6, 344: 8, 380: 8, 392: 6, 399: 7, 411: 5, 419: 8, 420: 8, 422: 8, 425: 8, 426: 8, 427: 3, 432: 7, 464: 8, 476: 4, 490: 8, 506: 8, 512: 6, 513: 6, 542: 7, 545: 5, 546: 3, 597: 8, 660: 8, 773: 7, 777: 8, 780: 8, 795: 8, 800: 8, 804: 8, 808: 8, 817: 4, 819: 7, 821: 5, 829: 5, 871: 8, 881: 8, 882: 2, 884: 7, 891: 8, 892: 8, 923: 2, 927: 8, 929: 8, 963: 8, 965: 8, 966: 8, 967: 8, 983: 8, 985: 3, 1027: 5, 1029: 8, 1039: 8, 1064: 7, 1088: 8, 1089: 8, 1092: 1, 1108: 8, 1125: 8, 1296: 8, 1424: 5, 1445: 8, 1600: 5, 1601: 8, 1612: 5, 1613: 5, 1616: 5, 1617: 8, 1618: 5, 1623: 5, 1668: 5
}],
@@ -204,6 +205,7 @@ FW_VERSIONS = {
b'77959-TBX-H230\x00\x00',
],
(Ecu.combinationMeter, 0x18da60f1, None): [
b'78109-TVC-C010\x00\x00',
b'78109-TVA-A210\x00\x00',
b'78109-TVC-A010\x00\x00',
b'78109-TVC-A020\x00\x00',
@@ -287,12 +289,10 @@ FW_VERSIONS = {
],
(Ecu.fwdCamera, 0x18dab5f1, None): [
b'36161-TWA-A070\x00\x00',
b'36161-TWA-A330\x00\x00',
],
(Ecu.fwdRadar, 0x18dab0f1, None): [
b'36802-TWA-A080\x00\x00',
b'36802-TWA-A070\x00\x00',
b'36802-TWA-A330\x00\x00',
],
(Ecu.eps, 0x18da30f1, None): [
b'39990-TVA-A160\x00\x00',
@@ -300,6 +300,78 @@ FW_VERSIONS = {
b'39990-TVA-A340\x00\x00',
],
},
CAR.ACCORD_2021: {
(Ecu.programmedFuelInjection, 0x18da10f1, None): [
b'37805-6B2-C520\x00\x00',
],
(Ecu.transmission, 0x18da1ef1, None): [
b'28102-6B8-A700\x00\x00',
],
(Ecu.electricBrakeBooster, 0x18da2bf1, None): [
b'46114-TVA-A320\x00\x00',
],
(Ecu.gateway, 0x18daeff1, None): [
b'38897-TVA-A020\x00\x00',
],
(Ecu.vsa, 0x18da28f1, None): [
b'57114-TVA-E520\x00\x00',
],
(Ecu.srs, 0x18da53f1, None): [
b'77959-TVA-L420\x00\x00',
],
(Ecu.combinationMeter, 0x18da60f1, None): [
b'78109-TVC-A230\x00\x00',
],
(Ecu.hud, 0x18da61f1, None): [
b'78209-TVA-A110\x00\x00',
],
(Ecu.shiftByWire, 0x18da0bf1, None): [
b'54008-TVC-A910\x00\x00',
],
(Ecu.fwdCamera, 0x18dab5f1, None): [
b'36161-TVC-A330\x00\x00',
],
(Ecu.fwdRadar, 0x18dab0f1, None): [
b'36802-TVC-A330\x00\x00',
],
(Ecu.eps, 0x18da30f1, None): [
b'39990-TVA-A340\x00\x00',
],
(Ecu.unknown, 0x18da3af1, None): [
b'39390-TVA-A120\x00\x00',
],
},
CAR.ACCORDH_2021: {
(Ecu.gateway, 0x18daeff1, None): [
b'38897-TWD-J020\x00\x00',
],
(Ecu.vsa, 0x18da28f1, None): [
b'57114-TWA-A530\x00\x00',
b'57114-TWA-B520\x00\x00',
],
(Ecu.srs, 0x18da53f1, None): [
b'77959-TWA-L420\x00\x00',
],
(Ecu.combinationMeter, 0x18da60f1, None): [
b'78109-TWA-A030\x00\x00',
b'78109-TWA-A230\x00\x00',
],
(Ecu.shiftByWire, 0x18da0bf1, None): [
b'54008-TWA-A910\x00\x00',
],
(Ecu.fwdCamera, 0x18dab5f1, None): [
b'36161-TWA-A330\x00\x00',
],
(Ecu.fwdRadar, 0x18dab0f1, None): [
b'36802-TWA-A330\x00\x00',
],
(Ecu.eps, 0x18da30f1, None): [
b'39990-TVA-A340\x00\x00',
],
(Ecu.unknown, 0x18da3af1, None): [
b'39390-TVA-A120\x00\x00',
],
},
CAR.CIVIC: {
(Ecu.programmedFuelInjection, 0x18da10f1, None): [
b'37805-5AA-A640\x00\x00',
@@ -314,6 +386,7 @@ FW_VERSIONS = {
b'37805-5AA-L660\x00\x00',
b'37805-5AA-L680\x00\x00',
b'37805-5AA-L690\x00\x00',
b'37805-5AA-L810\000\000',
b'37805-5AG-Q710\x00\x00',
b'37805-5AJ-A610\x00\x00',
b'37805-5AJ-A620\x00\x00',
@@ -336,12 +409,12 @@ FW_VERSIONS = {
b'28101-5CG-A050\x00\x00',
b'28101-5CG-A070\x00\x00',
b'28101-5CG-A080\x00\x00',
b'28101-5CG-A320\x00\x00',
b'28101-5CG-A810\x00\x00',
b'28101-5CG-A820\x00\x00',
b'28101-5DJ-A040\x00\x00',
b'28101-5DJ-A060\x00\x00',
b'28101-5DJ-A510\x00\x00',
b'28101-5CG-A320\x00\x00',
],
(Ecu.vsa, 0x18da28f1, None): [
b'57114-TBA-A540\x00\x00',
@@ -354,8 +427,8 @@ FW_VERSIONS = {
b'39990-TBA,A030\x00\x00', # modified firmware
b'39990-TBA-A030\x00\x00',
b'39990-TBG-A030\x00\x00',
b'39990-TEG-A010\x00\x00',
b'39990-TEA-T020\x00\x00',
b'39990-TEG-A010\x00\x00',
],
(Ecu.srs, 0x18da53f1, None): [
b'77959-TBA-A030\x00\x00',
@@ -387,9 +460,9 @@ FW_VERSIONS = {
b'36161-TBA-A040\x00\x00',
b'36161-TBC-A020\x00\x00',
b'36161-TBC-A030\x00\x00',
b'36161-TED-Q320\x00\x00',
b'36161-TEG-A010\x00\x00',
b'36161-TEG-A020\x00\x00',
b'36161-TED-Q320\x00\x00',
],
(Ecu.gateway, 0x18daeff1, None): [
b'38897-TBA-A010\x00\x00',
@@ -1228,7 +1301,9 @@ FW_VERSIONS = {
DBC = {
CAR.ACCORD: dbc_dict('honda_accord_2018_can_generated', None),
CAR.ACCORD_2021: dbc_dict('honda_accord_2018_can_generated', None),
CAR.ACCORDH: dbc_dict('honda_accord_2018_can_generated', None),
CAR.ACCORDH_2021: dbc_dict('honda_accord_2018_can_generated', None),
CAR.ACURA_ILX: dbc_dict('acura_ilx_2016_can_generated', 'acura_ilx_2016_nidec'),
CAR.ACURA_RDX: dbc_dict('acura_rdx_2018_can_generated', 'acura_ilx_2016_nidec'),
CAR.ACURA_RDX_3G: dbc_dict('acura_rdx_2020_can_generated', None),
@@ -1250,50 +1325,24 @@ DBC = {
}
STEER_THRESHOLD = {
CAR.ACCORD: 1200,
CAR.ACCORDH: 1200,
CAR.ACURA_ILX: 1200,
# default is 1200, overrides go here
CAR.ACURA_RDX: 400,
CAR.ACURA_RDX_3G: 1200,
CAR.CIVIC: 1200,
CAR.CIVIC_BOSCH: 1200,
CAR.CIVIC_BOSCH_DIESEL: 1200,
CAR.CRV: 1200,
CAR.CRV_5G: 1200,
CAR.CRV_EU: 400,
CAR.CRV_HYBRID: 1200,
CAR.FIT: 1200,
CAR.HRV: 1200,
CAR.ODYSSEY: 1200,
CAR.ODYSSEY_CHN: 1200,
CAR.PILOT: 1200,
CAR.PILOT_2019: 1200,
CAR.RIDGELINE: 1200,
CAR.INSIGHT: 1200,
}
# TODO: is this real?
SPEED_FACTOR = {
CAR.ACCORD: 1.,
CAR.ACCORDH: 1.,
CAR.ACURA_ILX: 1.,
CAR.ACURA_RDX: 1.,
CAR.ACURA_RDX_3G: 1.,
CAR.CIVIC: 1.,
CAR.CIVIC_BOSCH: 1.,
CAR.CIVIC_BOSCH_DIESEL: 1.,
# default is 1, overrides go here
CAR.CRV: 1.025,
CAR.CRV_5G: 1.025,
CAR.CRV_EU: 1.025,
CAR.CRV_HYBRID: 1.025,
CAR.FIT: 1.,
CAR.HRV: 1.025,
CAR.ODYSSEY: 1.,
CAR.ODYSSEY_CHN: 1.,
CAR.PILOT: 1.,
CAR.PILOT_2019: 1.,
CAR.RIDGELINE: 1.,
CAR.INSIGHT: 1.,
}
HONDA_BOSCH = set([CAR.ACCORD, CAR.ACCORDH, CAR.CIVIC_BOSCH, CAR.CIVIC_BOSCH_DIESEL, CAR.CRV_5G, CAR.CRV_HYBRID, CAR.INSIGHT, CAR.ACURA_RDX_3G])
HONDA_BOSCH_ALT_BRAKE_SIGNAL = set([CAR.ACCORD, CAR.CRV_5G, CAR.ACURA_RDX_3G])
OLD_NIDEC_LONG_CONTROL = set([CAR.ODYSSEY, CAR.ACURA_RDX, CAR.CRV, CAR.HRV])
HONDA_BOSCH = set([CAR.ACCORD, CAR.ACCORD_2021, CAR.ACCORDH, CAR.ACCORDH_2021, CAR.CIVIC_BOSCH, CAR.CIVIC_BOSCH_DIESEL, CAR.CRV_5G, CAR.CRV_HYBRID, CAR.INSIGHT, CAR.ACURA_RDX_3G])
HONDA_BOSCH_ALT_BRAKE_SIGNAL = set([CAR.ACCORD, CAR.ACCORD_2021, CAR.CRV_5G, CAR.ACURA_RDX_3G])
# Bosch models with alternate set of LKAS_HUD messages
HONDA_BOSCH_EXT = set([CAR.ACCORD_2021, CAR.ACCORDH_2021])
selfdrive/car/hyundai/carcontroller.py
+3 -2
View File
@@ -77,8 +77,9 @@ class CarController():
self.last_resume_frame = frame
# 20 Hz LFA MFA message
if frame % 5 == 0 and self.car_fingerprint in [CAR.SONATA, CAR.PALISADE, CAR.IONIQ, CAR.KIA_NIRO_EV, CAR.KONA_EV,
CAR.IONIQ_EV_2020, CAR.IONIQ_PHEV, CAR.KIA_CEED, CAR.KIA_SELTOS, CAR.ELANTRA_2021, CAR.ELANTRA_HEV_2021]:
if frame % 5 == 0 and self.car_fingerprint in [CAR.SONATA, CAR.PALISADE, CAR.IONIQ, CAR.KIA_NIRO_EV, CAR.KIA_NIRO_HEV_2021,
CAR.IONIQ_EV_2020, CAR.IONIQ_PHEV, CAR.KIA_CEED, CAR.KIA_SELTOS, CAR.KONA_EV,
CAR.ELANTRA_2021, CAR.ELANTRA_HEV_2021, CAR.SONATA_HYBRID, CAR.KONA_HEV]:
can_sends.append(create_lfahda_mfc(self.packer, enabled))
return can_sends
selfdrive/car/hyundai/hyundaican.py
+3 -1
View File
@@ -17,7 +17,9 @@ def create_lkas11(packer, frame, car_fingerprint, apply_steer, steer_req,
values["CF_Lkas_ActToi"] = steer_req
values["CF_Lkas_MsgCount"] = frame % 0x10
if car_fingerprint in [CAR.SONATA, CAR.PALISADE, CAR.KIA_NIRO_EV, CAR.SANTA_FE, CAR.IONIQ_EV_2020, CAR.IONIQ_PHEV, CAR.KIA_SELTOS, CAR.ELANTRA_2021, CAR.ELANTRA_HEV_2021]:
if car_fingerprint in [CAR.SONATA, CAR.PALISADE, CAR.KIA_NIRO_EV, CAR.KIA_NIRO_HEV_2021, CAR.SANTA_FE,
CAR.IONIQ_EV_2020, CAR.IONIQ_PHEV, CAR.KIA_SELTOS, CAR.ELANTRA_2021,
CAR.ELANTRA_HEV_2021, CAR.SONATA_HYBRID, CAR.KONA_HEV]:
values["CF_Lkas_LdwsActivemode"] = int(left_lane) + (int(right_lane) << 1)
values["CF_Lkas_LdwsOpt_USM"] = 2
selfdrive/car/hyundai/interface.py
+6 -14
View File
@@ -38,7 +38,7 @@ class CarInterface(CarInterfaceBase):
tire_stiffness_factor = 0.82
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[9., 22.], [9., 22.]]
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.2, 0.35], [0.05, 0.09]]
elif candidate == CAR.SONATA:
elif candidate in [CAR.SONATA, CAR.SONATA_HYBRID]:
ret.lateralTuning.pid.kf = 0.00005
ret.mass = 1513. + STD_CARGO_KG
ret.wheelbase = 2.84
@@ -57,7 +57,7 @@ class CarInterface(CarInterfaceBase):
ret.lateralTuning.pid.kf = 0.00005
ret.mass = 1999. + STD_CARGO_KG
ret.wheelbase = 2.90
ret.steerRatio = 13.75 * 1.15
ret.steerRatio = 15.6 * 1.15
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]]
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.3], [0.05]]
elif candidate in [CAR.ELANTRA, CAR.ELANTRA_GT_I30]:
@@ -100,22 +100,14 @@ class CarInterface(CarInterfaceBase):
ret.lateralTuning.indi.actuatorEffectivenessBP = [0.]
ret.lateralTuning.indi.actuatorEffectivenessV = [2.3]
ret.minSteerSpeed = 60 * CV.KPH_TO_MS
elif candidate == CAR.KONA:
elif candidate in [CAR.KONA, CAR.KONA_EV, CAR.KONA_HEV]:
ret.lateralTuning.pid.kf = 0.00005
ret.mass = 1275. + STD_CARGO_KG
ret.mass = {CAR.KONA_EV: 1685., CAR.KONA_HEV: 1425.}.get(candidate, 1275.) + STD_CARGO_KG
ret.wheelbase = 2.7
ret.steerRatio = 13.73 * 1.15 # Spec
tire_stiffness_factor = 0.385
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]]
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25], [0.05]]
elif candidate == CAR.KONA_EV:
ret.lateralTuning.pid.kf = 0.00006
ret.mass = 1685. + STD_CARGO_KG
ret.wheelbase = 2.7
ret.steerRatio = 13.73 # Spec
tire_stiffness_factor = 0.385
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]]
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25], [0.05]]
elif candidate in [CAR.IONIQ, CAR.IONIQ_EV_LTD, CAR.IONIQ_EV_2020, CAR.IONIQ_PHEV]:
ret.lateralTuning.pid.kf = 0.00006
ret.mass = 1490. + STD_CARGO_KG # weight per hyundai site https://www.hyundaiusa.com/ioniq-electric/specifications.aspx
@@ -143,11 +135,11 @@ class CarInterface(CarInterfaceBase):
ret.steerRatio = 14.4 * 1.1 # 10% higher at the center seems reasonable
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]]
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25], [0.05]]
elif candidate in [CAR.KIA_NIRO_EV, CAR.KIA_NIRO_HEV]:
elif candidate in [CAR.KIA_NIRO_EV, CAR.KIA_NIRO_HEV, CAR.KIA_NIRO_HEV_2021]:
ret.lateralTuning.pid.kf = 0.00006
ret.mass = 1737. + STD_CARGO_KG
ret.wheelbase = 2.7
ret.steerRatio = 13.73 # Spec
ret.steerRatio = 13.9 if CAR.KIA_NIRO_HEV_2021 else 13.73 # Spec
tire_stiffness_factor = 0.385
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]]
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25], [0.05]]
selfdrive/car/hyundai/values.py
+61 -7
View File
@@ -7,7 +7,9 @@ Ecu = car.CarParams.Ecu
# Steer torque limits
class CarControllerParams:
def __init__(self, CP):
if CP.carFingerprint in [CAR.SONATA, CAR.PALISADE, CAR.SANTA_FE, CAR.VELOSTER, CAR.GENESIS_G70, CAR.IONIQ_EV_2020, CAR.KIA_CEED, CAR.KIA_SELTOS, CAR.ELANTRA_2021, CAR.ELANTRA_HEV_2021]:
if CP.carFingerprint in [CAR.SONATA, CAR.PALISADE, CAR.SANTA_FE, CAR.VELOSTER, CAR.GENESIS_G70,
CAR.IONIQ_EV_2020, CAR.KIA_CEED, CAR.KIA_SELTOS, CAR.ELANTRA_2021,
CAR.ELANTRA_HEV_2021, CAR.SONATA_HYBRID, CAR.KONA_HEV]:
self.STEER_MAX = 384
else:
self.STEER_MAX = 255
@@ -31,16 +33,19 @@ class CAR:
IONIQ_PHEV = "HYUNDAI IONIQ PHEV 2020"
KONA = "HYUNDAI KONA 2020"
KONA_EV = "HYUNDAI KONA ELECTRIC 2019"
KONA_HEV = "HYUNDAI KONA HYBRID 2020"
SANTA_FE = "HYUNDAI SANTA FE 2019"
SONATA = "HYUNDAI SONATA 2020"
SONATA_LF = "HYUNDAI SONATA 2019"
PALISADE = "HYUNDAI PALISADE 2020"
VELOSTER = "HYUNDAI VELOSTER 2019"
SONATA_HYBRID = "HYUNDAI SONATA HYBRID 2021"
# Kia
KIA_FORTE = "KIA FORTE E 2018 & GT 2021"
KIA_NIRO_EV = "KIA NIRO EV 2020"
KIA_NIRO_HEV = "KIA NIRO HYBRID 2019"
KIA_NIRO_HEV_2021 = "KIA NIRO HYBRID 2021"
KIA_OPTIMA = "KIA OPTIMA SX 2019 & 2016"
KIA_OPTIMA_H = "KIA OPTIMA HYBRID 2017 & SPORTS 2019"
KIA_SELTOS = "KIA SELTOS 2021"
@@ -581,6 +586,24 @@ FW_VERSIONS = {
b'\xf1\000DEhe SCC H-CUP 1.01 1.02 96400-G5100 ',
],
},
CAR.KIA_NIRO_HEV_2021: {
(Ecu.engine, 0x7e0, None): [
b'\xf1\x816H6G5051\x00\x00\x00\x00\x00\x00\x00\x00',
],
(Ecu.transmission, 0x7e1, None): [
b'\xf1\x816U3J9051\x00\x00\xf1\x006U3H1_C2\x00\x006U3J9051\x00\x00HDE0G16NL3\x00\x00\x00\x00',
b'\xf1\x816U3J9051\x00\x00\xf1\x006U3H1_C2\x00\x006U3J9051\x00\x00HDE0G16NL3\xb9\xd3\xfaW',
],
(Ecu.eps, 0x7d4, None): [
b'\xf1\x00DE MDPS C 1.00 1.01 56310G5520\x00 4DEPC101',
],
(Ecu.fwdCamera, 0x7c4, None): [
b'\xf1\x00DEH MFC AT USA LHD 1.00 1.07 99211-G5000 201221',
],
(Ecu.fwdRadar, 0x7d0, None): [
b'\xf1\x00DEhe SCC FHCUP 1.00 1.00 99110-G5600 ',
],
},
CAR.KIA_SELTOS: {
(Ecu.fwdRadar, 0x7d0, None): [b'\xf1\x8799110Q5100\xf1\000SP2_ SCC FHCUP 1.01 1.05 99110-Q5100 \xf1\xa01.05',],
(Ecu.esp, 0x7d1, None): [
@@ -647,16 +670,44 @@ FW_VERSIONS = {
b'\xf1\x8756310/BY050\xf1\000CN7 MDPS C 1.00 1.02 56310/BY050 4CNHC102\xf1\xa01.02'
],
(Ecu.transmission, 0x7e1, None) :[
b'\xf1\x816U3K3051\000\000\xf1\0006U3L0_C2\000\0006U3K3051\000\000HCN0G16NS0\xb9?A\xaa'
b'\xf1\x816U3K3051\000\000\xf1\0006U3L0_C2\000\0006U3K3051\000\000HCN0G16NS0\xb9?A\xaa',
b'\xf1\x816U3K3051\000\000\xf1\0006U3L0_C2\000\0006U3K3051\000\000HCN0G16NS0\000\000\000\000'
],
(Ecu.engine, 0x7e0, None) : [
b'\xf1\x816H6G5051\000\000\000\000\000\000\000\000'
]
}
},
CAR.KONA_HEV: {
(Ecu.esp, 0x7d1, None): [
b'\xf1\x00OS IEB \x01 104 \x11 58520-CM000\xf1\xa01.04',
],
(Ecu.fwdRadar, 0x7d0, None): [
b'\xf1\x00OShe SCC FNCUP 1.00 1.01 99110-CM000 \xf1\xa01.01',
],
(Ecu.eps, 0x7d4, None): [
b'\xf1\x00OS MDPS C 1.00 1.00 56310CM030\x00 4OHDC100',
],
(Ecu.fwdCamera, 0x7c4, None): [
b'\xf1\x00OSH LKAS AT KOR LHD 1.00 1.01 95740-CM000 l31',
],
(Ecu.transmission, 0x7e1, None): [
b'\xf1\x816U3J9051\x00\x00\xf1\x006U3H1_C2\x00\x006U3J9051\x00\x00HOS0G16DS1\x16\xc7\xb0\xd9',
],
(Ecu.engine, 0x7e0, None): [
b'\xf1\x816H6F6051\x00\x00\x00\x00\x00\x00\x00\x00',
]
},
CAR.SONATA_HYBRID: {
(Ecu.fwdRadar, 0x7d0, None): [b'\xf1\000DNhe SCC FHCUP 1.00 1.02 99110-L5000 ',],
(Ecu.eps, 0x7d4, None): [b'\xf1\x8756310-L5500\xf1\000DN8 MDPS C 1.00 1.02 56310-L5500 4DNHC102\xf1\xa01.02',],
(Ecu.fwdCamera, 0x7c4, None): [b'\xf1\000DN8HMFC AT USA LHD 1.00 1.04 99211-L1000 191016',],
(Ecu.transmission, 0x7e1, None): [b'\xf1\000PSBG2323 E09\000\000\000\000\000\000\000TDN2H20SA5\x97R\x88\x9e',],
(Ecu.engine, 0x7e0, None): [b'\xf1\x87391162J012\xf1\xa0000R',],
},
}
CHECKSUM = {
"crc8": [CAR.SANTA_FE, CAR.SONATA, CAR.PALISADE, CAR.KIA_SELTOS, CAR.ELANTRA_2021, CAR.ELANTRA_HEV_2021],
"crc8": [CAR.SANTA_FE, CAR.SONATA, CAR.PALISADE, CAR.KIA_SELTOS, CAR.ELANTRA_2021, CAR.ELANTRA_HEV_2021, CAR.SONATA_HYBRID],
"6B": [CAR.KIA_SORENTO, CAR.HYUNDAI_GENESIS],
}
@@ -664,13 +715,13 @@ FEATURES = {
# which message has the gear
"use_cluster_gears": set([CAR.ELANTRA, CAR.ELANTRA_GT_I30, CAR.KONA]),
"use_tcu_gears": set([CAR.KIA_OPTIMA, CAR.SONATA_LF, CAR.VELOSTER]),
"use_elect_gears": set([CAR.KIA_NIRO_EV, CAR.KIA_NIRO_HEV, CAR.KIA_OPTIMA_H, CAR.IONIQ_EV_LTD, CAR.KONA_EV, CAR.IONIQ, CAR.IONIQ_EV_2020, CAR.IONIQ_PHEV, CAR.ELANTRA_HEV_2021]),
"use_elect_gears": set([CAR.KIA_NIRO_EV, CAR.KIA_NIRO_HEV, CAR.KIA_NIRO_HEV_2021, CAR.KIA_OPTIMA_H, CAR.IONIQ_EV_LTD, CAR.KONA_EV, CAR.IONIQ, CAR.IONIQ_EV_2020, CAR.IONIQ_PHEV, CAR.ELANTRA_HEV_2021,CAR.SONATA_HYBRID, CAR.KONA_HEV]),
# these cars use the FCA11 message for the AEB and FCW signals, all others use SCC12
"use_fca": set([CAR.SONATA, CAR.ELANTRA, CAR.ELANTRA_2021, CAR.ELANTRA_HEV_2021, CAR.ELANTRA_GT_I30, CAR.KIA_STINGER, CAR.IONIQ, CAR.IONIQ_EV_2020, CAR.IONIQ_PHEV, CAR.KONA_EV, CAR.KIA_FORTE, CAR.KIA_NIRO_EV, CAR.PALISADE, CAR.GENESIS_G70, CAR.KONA, CAR.SANTA_FE, CAR.KIA_SELTOS]),
"use_fca": set([CAR.SONATA, CAR.SONATA_HYBRID, CAR.ELANTRA, CAR.ELANTRA_2021, CAR.ELANTRA_HEV_2021, CAR.ELANTRA_GT_I30, CAR.KIA_STINGER, CAR.IONIQ, CAR.IONIQ_EV_2020, CAR.IONIQ_PHEV, CAR.KONA_EV, CAR.KIA_FORTE, CAR.KIA_NIRO_EV, CAR.PALISADE, CAR.GENESIS_G70, CAR.KONA, CAR.SANTA_FE, CAR.KIA_SELTOS, CAR.KONA_HEV]),
}
HYBRID_CAR = set([CAR.IONIQ_PHEV, CAR.ELANTRA_HEV_2021, CAR.KIA_NIRO_HEV]) # these cars use a different gas signal
HYBRID_CAR = set([CAR.IONIQ_PHEV, CAR.ELANTRA_HEV_2021, CAR.KIA_NIRO_HEV, CAR.KIA_NIRO_HEV_2021, CAR.SONATA_HYBRID, CAR.KONA_HEV]) # these cars use a different gas signal
EV_CAR = set([CAR.IONIQ_EV_2020, CAR.IONIQ_EV_LTD, CAR.IONIQ, CAR.KONA_EV, CAR.KIA_NIRO_EV])
DBC = {
@@ -689,6 +740,7 @@ DBC = {
CAR.KIA_FORTE: dbc_dict('hyundai_kia_generic', None),
CAR.KIA_NIRO_EV: dbc_dict('hyundai_kia_generic', None),
CAR.KIA_NIRO_HEV: dbc_dict('hyundai_kia_generic', None),
CAR.KIA_NIRO_HEV_2021: dbc_dict('hyundai_kia_generic', None),
CAR.KIA_OPTIMA: dbc_dict('hyundai_kia_generic', None),
CAR.KIA_OPTIMA_H: dbc_dict('hyundai_kia_generic', None),
CAR.KIA_SELTOS: dbc_dict('hyundai_kia_generic', None),
@@ -696,12 +748,14 @@ DBC = {
CAR.KIA_STINGER: dbc_dict('hyundai_kia_generic', None),
CAR.KONA: dbc_dict('hyundai_kia_generic', None),
CAR.KONA_EV: dbc_dict('hyundai_kia_generic', None),
CAR.KONA_HEV: dbc_dict('hyundai_kia_generic', None),
CAR.SANTA_FE: dbc_dict('hyundai_kia_generic', None),
CAR.SONATA: dbc_dict('hyundai_kia_generic', None),
CAR.SONATA_LF: dbc_dict('hyundai_kia_generic', None),
CAR.PALISADE: dbc_dict('hyundai_kia_generic', None),
CAR.VELOSTER: dbc_dict('hyundai_kia_generic', None),
CAR.KIA_CEED: dbc_dict('hyundai_kia_generic', None),
CAR.SONATA_HYBRID: dbc_dict('hyundai_kia_generic', None),
}
STEER_THRESHOLD = 150
selfdrive/car/toyota/values.py
+7
View File
@@ -619,6 +619,7 @@ FW_VERSIONS = {
b'\x01896630ZP2000\x00\x00\x00\x00',
b'\x01896630ZQ5000\x00\x00\x00\x00',
b'\x018966312L8000\x00\x00\x00\x00',
b'\x018966312M0000\x00\x00\x00\x00',
b'\x018966312M9000\x00\x00\x00\x00',
b'\x018966312P9000\x00\x00\x00\x00',
b'\x018966312P9100\x00\x00\x00\x00',
@@ -630,6 +631,7 @@ FW_VERSIONS = {
b'\x018966312R3100\x00\x00\x00\x00',
b'\x018966312S5000\x00\x00\x00\x00',
b'\x018966312S7000\x00\x00\x00\x00',
b'\x018966312W3000\x00\x00\x00\x00',
],
(Ecu.engine, 0x7e0, None): [
b'\x0230ZN4000\x00\x00\x00\x00\x00\x00\x00\x00A0202000\x00\x00\x00\x00\x00\x00\x00\x00',
@@ -690,10 +692,12 @@ FW_VERSIONS = {
b'\x01896637624000\x00\x00\x00\x00',
b'\x01896637626000\x00\x00\x00\x00',
b'\x01896637648000\x00\x00\x00\x00',
b'\x02896630ZJ5000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00',
b'\x02896630ZN8000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00',
b'\x02896630ZQ3000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00',
b'\x02896630ZR2000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00',
b'\x02896630ZT8000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00',
b'\x02896630ZT9000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00',
b'\x028966312Q3000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00',
b'\x028966312Q4000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00',
b'\x038966312L7000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00897CF1205001\x00\x00\x00\x00',
@@ -716,6 +720,7 @@ FW_VERSIONS = {
b'F152612691\x00\x00\x00\x00\x00\x00',
b'F152612692\x00\x00\x00\x00\x00\x00',
b'F152612700\x00\x00\x00\x00\x00\x00',
b'F152612710\x00\x00\x00\x00\x00\x00',
b'F152612790\x00\x00\x00\x00\x00\x00',
b'F152612800\x00\x00\x00\x00\x00\x00',
b'F152612820\x00\x00\x00\x00\x00\x00',
@@ -851,6 +856,7 @@ FW_VERSIONS = {
b'\x01F152648C6300\x00\x00\x00\x00',
],
(Ecu.engine, 0x700, None): [
b'\x01896630EA1000\000\000\000\000',
b'\x01896630EA1000\x00\x00\x00\x00897CF4801001\x00\x00\x00\x00',
b'\x02896630E66000\x00\x00\x00\x00897CF4801001\x00\x00\x00\x00',
b'\x02896630EB3000\x00\x00\x00\x00897CF4801001\x00\x00\x00\x00',
@@ -1418,6 +1424,7 @@ FW_VERSIONS = {
b'\x01896630E41000\x00\x00\x00\x00',
b'\x01896630E41100\x00\x00\x00\x00',
b'\x01896630E41200\x00\x00\x00\x00',
b'\x01896630EA3100\x00\x00\x00\x00',
b'\x01896630EA4100\x00\x00\x00\x00',
b'\x01896630EA4300\x00\x00\x00\x00',
b'\x01896630EA6300\x00\x00\x00\x00',
selfdrive/car/volkswagen/values.py
+9
View File
@@ -438,6 +438,7 @@ FW_VERSIONS = {
(Ecu.engine, 0x7e0, None): [
b'\xf1\x8704L906021EL\xf1\x897542',
b'\xf1\x8704L906026BP\xf1\x891198',
b'\xf1\x8704L906026BP\xf1\x897608',
b'\xf1\x8705E906018AS\xf1\x899596',
],
(Ecu.transmission, 0x7e1, None): [
@@ -488,12 +489,14 @@ FW_VERSIONS = {
CAR.SKODA_OCTAVIA_MK3: {
(Ecu.engine, 0x7e0, None): [
b'\xf1\x8704E906027HD\xf1\x893742',
b'\xf1\x8704E906016ER\xf1\x895823',
b'\xf1\x8704L906021DT\xf1\x898127',
b'\xf1\x8704L906026BS\xf1\x891541',
b'\xf1\x875G0906259C \xf1\x890002',
],
(Ecu.transmission, 0x7e1, None): [
b'\xf1\x870CW300043B \xf1\x891601',
b'\xf1\x870CW300041N \xf1\x891605',
b'\xf1\x870D9300041C \xf1\x894936',
b'\xf1\x870D9300041J \xf1\x894902',
b'\xf1\x870D9300041P \xf1\x894507',
@@ -502,16 +505,19 @@ FW_VERSIONS = {
b'\xf1\x873Q0959655AC\xf1\x890200\xf1\x82\r11120011100010022212110200',
b'\xf1\x873Q0959655AS\xf1\x890200\xf1\x82\r11120011100010022212110200',
b'\xf1\x873Q0959655AQ\xf1\x890200\xf1\x82\r11120011100010312212113100',
b'\xf1\x873Q0959655BH\xf1\x890703\xf1\x82\0163221003221002105755331052100',
b'\xf1\x873Q0959655CN\xf1\x890720\xf1\x82\x0e3221003221002105755331052100',
],
(Ecu.eps, 0x712, None): [
b'\xf1\x873Q0909144J \xf1\x895063\xf1\x82\00566A01513A1',
b'\xf1\x875Q0909144AA\xf1\x891081\xf1\x82\00521T00403A1',
b'\xf1\x875Q0909144AB\xf1\x891082\xf1\x82\x0521T00403A1',
b'\xf1\x875Q0909144R \xf1\x891061\xf1\x82\x0516A00604A1',
],
(Ecu.fwdRadar, 0x757, None): [
b'\xf1\x875Q0907572D \xf1\x890304\xf1\x82\x0101',
b'\xf1\x875Q0907572F \xf1\x890400\xf1\x82\00101',
b'\xf1\x875Q0907572J \xf1\x890654',
b'\xf1\x875Q0907572P \xf1\x890682',
],
},
@@ -537,10 +543,12 @@ FW_VERSIONS = {
b'\xf1\x8704L906026KB\xf1\x894071',
b'\xf1\x873G0906259B \xf1\x890002',
b'\xf1\x8704L906026FP\xf1\x891196',
b'\xf1\x873G0906264A \xf1\x890002',
],
(Ecu.transmission, 0x7e1, None): [
b'\xf1\x870D9300012 \xf1\x894940',
b'\xf1\x870D9300011T \xf1\x894801',
b'\xf1\x870CW300042H \xf1\x891601',
],
(Ecu.srs, 0x715, None): [
b'\xf1\x875Q0959655AE\xf1\x890130\xf1\x82\022111200111121001121118112231292221111',
@@ -551,6 +559,7 @@ FW_VERSIONS = {
b'\xf1\x875Q0909143M \xf1\x892041\xf1\x820522UZ070303',
b'\xf1\x875Q0910143B \xf1\x892201\xf1\x82\00563UZ060700',
b'\xf1\x875Q0909143K \xf1\x892033\xf1\x820514UZ070203',
b'\xf1\x875Q0910143B \xf1\x892201\xf1\x82\x0563UZ060600',
],
(Ecu.fwdRadar, 0x757, None): [
b'\xf1\x873Q0907572B \xf1\x890194',
selfdrive/clocksd/clocksd.cc
+4 -1
View File
@@ -50,7 +50,10 @@ int main() {
uint64_t expirations = 0;
while (!do_exit && (err = read(timerfd, &expirations, sizeof(expirations)))) {
if (err < 0) break;
if (err < 0) {
if (errno == EINTR) continue;
break;
}
#else
// Just run at 1Hz on apple
while (!do_exit) {
selfdrive/common/clutil.cc
+1 -1
View File
@@ -88,7 +88,7 @@ cl_program cl_program_from_file(cl_context ctx, cl_device_id device_id, const ch
return prg;
}
// Given a cl code and return a string represenation
// 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) {
selfdrive/common/clutil.h
+1 -1
View File
@@ -11,7 +11,7 @@
#define CL_CHECK(_expr) \
do { \
assert(CL_SUCCESS == _expr); \
assert(CL_SUCCESS == (_expr)); \
} while (0)
#define CL_CHECK_ERR(_expr) \
selfdrive/common/modeldata.h
+1 -1
View File
@@ -1,7 +1,7 @@
#pragma once
const int TRAJECTORY_SIZE = 33;
const int LON_MPC_N = 32;
const int LAT_MPC_N = 16;
const int LON_MPC_N = 32;
const float MIN_DRAW_DISTANCE = 10.0;
const float MAX_DRAW_DISTANCE = 100.0;
selfdrive/common/params.cc
+10 -15
View File
@@ -33,10 +33,6 @@
namespace {
const std::string default_params_path = Hardware::PC() ? util::getenv_default("HOME", "/.comma/params", "/data/params")
: "/data/params";
const std::string persistent_params_path = Hardware::PC() ? default_params_path : "/persist/comma/params";
volatile sig_atomic_t params_do_exit = 0;
void params_sig_handler(int signal) {
params_do_exit = 1;
@@ -78,7 +74,7 @@ int mkdir_p(std::string path) {
return 0;
}
static bool create_params_path(const std::string &param_path, const std::string &key_path) {
bool create_params_path(const std::string &param_path, const std::string &key_path) {
// Make sure params path exists
if (!util::file_exists(param_path) && mkdir_p(param_path) != 0) {
return false;
@@ -117,7 +113,7 @@ static bool create_params_path(const std::string &param_path, const std::string
return chmod(key_path.c_str(), 0777) == 0;
}
static void ensure_params_path(const std::string &params_path) {
void ensure_params_path(const std::string &params_path) {
if (!create_params_path(params_path, params_path + "/d")) {
throw std::runtime_error(util::string_format("Failed to ensure params path, errno=%d", errno));
}
@@ -198,6 +194,7 @@ std::unordered_map<std::string, uint32_t> keys = {
{"PandaFirmware", CLEAR_ON_MANAGER_START | CLEAR_ON_PANDA_DISCONNECT},
{"PandaFirmwareHex", CLEAR_ON_MANAGER_START | CLEAR_ON_PANDA_DISCONNECT},
{"PandaDongleId", CLEAR_ON_MANAGER_START | CLEAR_ON_PANDA_DISCONNECT},
{"PandaHeartbeatLost", CLEAR_ON_MANAGER_START | CLEAR_ON_IGNITION_OFF},
{"Passive", PERSISTENT},
{"PrimeRedirected", PERSISTENT},
{"RecordFront", PERSISTENT},
@@ -231,15 +228,13 @@ std::unordered_map<std::string, uint32_t> keys = {
} // namespace
Params::Params(bool persistent_param) : Params(persistent_param ? persistent_params_path : default_params_path) {}
Params::Params() : params_path(Path::params()) {
static std::once_flag once_flag;
std::call_once(once_flag, ensure_params_path, params_path);
}
std::once_flag default_params_path_ensured;
Params::Params(const std::string &path) : params_path(path) {
if (path == default_params_path) {
std::call_once(default_params_path_ensured, ensure_params_path, path);
} else {
ensure_params_path(path);
}
ensure_params_path(params_path);
}
bool Params::checkKey(const std::string &key) {
@@ -331,12 +326,12 @@ std::string Params::get(const char *key, bool block) {
}
}
int Params::readAll(std::map<std::string, std::string> *params) {
std::map<std::string, std::string> Params::readAll() {
FileLock file_lock(params_path + "/.lock", LOCK_SH);
std::lock_guard<FileLock> lk(file_lock);
std::string key_path = params_path + "/d";
return util::read_files_in_dir(key_path, params);
return util::read_files_in_dir(key_path);
}
void Params::clearAll(ParamKeyType key_type) {
selfdrive/common/params.h
+6 -6
View File
@@ -13,15 +13,12 @@ enum ParamKeyType {
CLEAR_ON_IGNITION_ON = 0x10,
CLEAR_ON_IGNITION_OFF = 0x20,
DONT_LOG = 0x40,
ALL = 0x02 | 0x04 | 0x08 | 0x10 | 0x20 | 0x40
ALL = 0xFFFFFFFF
};
class Params {
private:
std::string params_path;
public:
Params(bool persistent_param = false);
Params();
Params(const std::string &path);
bool checkKey(const std::string &key);
@@ -35,7 +32,7 @@ public:
void clearAll(ParamKeyType type);
// read all values
int readAll(std::map<std::string, std::string> *params);
std::map<std::string, std::string> readAll();
// helpers for reading values
std::string get(const char *key, bool block = false);
@@ -78,4 +75,7 @@ public:
inline int putBool(const std::string &key, bool val) {
return putBool(key.c_str(), val);
}
private:
const std::string params_path;
};
selfdrive/common/swaglog.h
+1 -1
View File
@@ -31,7 +31,7 @@ void cloudlog_bind(const char* k, const char* v);
\
if (__begin + __millis*1000000ULL < __ts) { \
if (__missed) { \
cloudlog(CLOUDLOG_WARNING, "cloudlog: %d messages supressed", __missed); \
cloudlog(CLOUDLOG_WARNING, "cloudlog: %d messages suppressed", __missed); \
} \
__begin = 0; \
__printed = 0; \
selfdrive/common/util.cc
+22 -17
View File
@@ -1,5 +1,7 @@
#include "selfdrive/common/util.h"
#include <sys/stat.h>
#include <cassert>
#include <cerrno>
#include <cstring>
@@ -76,19 +78,20 @@ std::string read_file(const std::string& fn) {
return std::string();
}
int read_files_in_dir(const std::string &path, std::map<std::string, std::string> *contents) {
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 -1;
if (!d) return ret;
struct dirent *de = NULL;
while ((de = readdir(d))) {
if (isalnum(de->d_name[0])) {
(*contents)[de->d_name] = util::read_file(path + "/" + de->d_name);
ret[de->d_name] = util::read_file(path + "/" + de->d_name);
}
}
closedir(d);
return 0;
return ret;
}
int write_file(const char* path, const void* data, size_t size, int flags, mode_t mode) {
@@ -112,17 +115,23 @@ std::string readlink(const std::string &path) {
}
bool file_exists(const std::string& fn) {
std::ifstream f(fn);
return f.good();
struct stat st = {};
return stat(fn.c_str(), &st) != -1;
}
std::string getenv_default(const char* env_var, const char * suffix, const char* default_val) {
const char* env_val = getenv(env_var);
if (env_val != NULL) {
return std::string(env_val) + std::string(suffix);
} else {
return std::string(default_val);
}
std::string getenv(const char* key, const char* default_val) {
const char* val = ::getenv(key);
return val ? val : default_val;
}
int getenv(const char* key, int default_val) {
const char* val = ::getenv(key);
return val ? atoi(val) : default_val;
}
float getenv(const char* key, float default_val) {
const char* val = ::getenv(key);
return val ? atof(val) : default_val;
}
std::string tohex(const uint8_t *buf, size_t buf_size) {
@@ -155,10 +164,6 @@ std::string dir_name(std::string const &path) {
return path.substr(0, pos);
}
bool is_valid_dongle_id(std::string const& dongle_id) {
return !dongle_id.empty() && dongle_id != "UnregisteredDevice";
}
struct tm get_time() {
time_t rawtime;
time(&rawtime);
selfdrive/common/util.h
+5 -7
View File
@@ -41,10 +41,6 @@ T map_val(T x, T a1, T a2, T b1, T b2) {
// ***** string helpers *****
inline bool starts_with(const std::string& s, const std::string& prefix) {
return s.compare(0, prefix.size(), prefix) == 0;
}
template <typename... Args>
std::string string_format(const std::string& format, Args... args) {
size_t size = snprintf(nullptr, 0, format.c_str(), args...) + 1;
@@ -53,16 +49,18 @@ std::string string_format(const std::string& format, Args... args) {
return std::string(buf.get(), buf.get() + size - 1);
}
std::string getenv_default(const char* env_var, const char* suffix, const char* default_val);
std::string getenv(const char* key, const char* default_val = "");
int getenv(const char* key, int default_val);
float getenv(const char* key, float default_val);
std::string tohex(const uint8_t* buf, size_t buf_size);
std::string hexdump(const std::string& in);
std::string base_name(std::string const& path);
std::string dir_name(std::string const& path);
bool is_valid_dongle_id(std::string const& dongle_id);
// **** file fhelpers *****
std::string read_file(const std::string& fn);
int read_files_in_dir(const std::string& path, std::map<std::string, std::string>* contents);
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 = 0777);
std::string readlink(const std::string& path);
bool file_exists(const std::string& fn);
selfdrive/common/version.h
+1 -1
View File
@@ -1 +1 @@
#define COMMA_VERSION "0.8.7"
#define COMMA_VERSION "0.8.8"
selfdrive/controls/controlsd.py
+16 -2
View File
@@ -23,7 +23,8 @@ from selfdrive.controls.lib.events import Events, ET
from selfdrive.controls.lib.alertmanager import AlertManager
from selfdrive.controls.lib.vehicle_model import VehicleModel
from selfdrive.locationd.calibrationd import Calibration
from selfdrive.hardware import HARDWARE, TICI
from selfdrive.hardware import HARDWARE, TICI, EON
from selfdrive.manager.process_config import managed_processes
LDW_MIN_SPEED = 31 * CV.MPH_TO_MS
LANE_DEPARTURE_THRESHOLD = 0.1
@@ -32,7 +33,11 @@ STEER_ANGLE_SATURATION_THRESHOLD = 2.5 # Degrees
SIMULATION = "SIMULATION" in os.environ
NOSENSOR = "NOSENSOR" in os.environ
IGNORE_PROCESSES = set(["rtshield", "uploader", "deleter", "loggerd", "logmessaged", "tombstoned", "logcatd", "proclogd", "clocksd", "updated", "timezoned", "manage_athenad"])
IGNORE_PROCESSES = {"rtshield", "uploader", "deleter", "loggerd", "logmessaged", "tombstoned",
"logcatd", "proclogd", "clocksd", "updated", "timezoned", "manage_athenad"} | \
{k for k, v in managed_processes.items() if not v.enabled}
ACTUATOR_FIELDS = set(car.CarControl.Actuators.schema.fields.keys())
ThermalStatus = log.DeviceState.ThermalStatus
State = log.ControlsState.OpenpilotState
@@ -196,6 +201,9 @@ class Controls:
# TODO: make tici threshold the same
if self.sm['deviceState'].memoryUsagePercent > (90 if TICI else 65) and not SIMULATION:
self.events.add(EventName.lowMemory)
cpus = list(self.sm['deviceState'].cpuUsagePercent)[:(-1 if EON else None)]
if max(cpus, default=0) > 95:
self.events.add(EventName.highCpuUsage)
# Alert if fan isn't spinning for 5 seconds
if self.sm['pandaState'].pandaType in [PandaType.uno, PandaType.dos]:
@@ -497,6 +505,12 @@ class Controls:
if left_deviation or right_deviation:
self.events.add(EventName.steerSaturated)
# Ensure no NaNs/Infs
for p in ACTUATOR_FIELDS:
if not math.isfinite(getattr(actuators, p)):
cloudlog.error(f"actuators.{p} not finite {actuators.to_dict()}")
setattr(actuators, p, 0.0)
return actuators, lac_log
def publish_logs(self, CS, start_time, actuators, lac_log):
selfdrive/controls/lib/events.py
+26 -26
View File
@@ -240,6 +240,8 @@ def joystick_alert(CP: car.CarParams, sm: messaging.SubMaster, metric: bool) ->
EVENTS: Dict[int, Dict[str, Union[Alert, Callable[[Any, messaging.SubMaster, bool], Alert]]]] = {
# ********** events with no alerts **********
EventName.stockFcw: {},
# ********** events only containing alerts displayed in all states **********
EventName.joystickDebug: {
@@ -357,15 +359,6 @@ EVENTS: Dict[int, Dict[str, Union[Alert, Callable[[Any, messaging.SubMaster, boo
ET.NO_ENTRY: NoEntryAlert("Stock AEB: Risk of Collision"),
},
EventName.stockFcw: {
ET.PERMANENT: Alert(
"BRAKE!",
"Stock FCW: Risk of Collision",
AlertStatus.critical, AlertSize.full,
Priority.HIGHEST, VisualAlert.fcw, AudibleAlert.none, 1., 2., 2.),
ET.NO_ENTRY: NoEntryAlert("Stock FCW: Risk of Collision"),
},
EventName.fcw: {
ET.PERMANENT: Alert(
"BRAKE!",
@@ -423,7 +416,7 @@ EVENTS: Dict[int, Dict[str, Union[Alert, Callable[[Any, messaging.SubMaster, boo
"KEEP EYES ON ROAD: Driver Distracted",
"",
AlertStatus.normal, AlertSize.small,
Priority.LOW, VisualAlert.steerRequired, AudibleAlert.none, .0, .1, .1),
Priority.LOW, VisualAlert.none, AudibleAlert.none, .0, .1, .1),
},
EventName.promptDriverDistracted: {
@@ -488,34 +481,34 @@ EVENTS: Dict[int, Dict[str, Union[Alert, Callable[[Any, messaging.SubMaster, boo
EventName.preLaneChangeLeft: {
ET.WARNING: Alert(
"Steer Left to Start Lane Change",
"Monitor Other Vehicles",
AlertStatus.normal, AlertSize.mid,
Priority.LOW, VisualAlert.steerRequired, AudibleAlert.none, .0, .1, .1, alert_rate=0.75),
"Steer Left to Start Lane Change Once Safe",
"",
AlertStatus.normal, AlertSize.small,
Priority.LOW, VisualAlert.none, AudibleAlert.none, .0, .1, .1, alert_rate=0.75),
},
EventName.preLaneChangeRight: {
ET.WARNING: Alert(
"Steer Right to Start Lane Change",
"Monitor Other Vehicles",
AlertStatus.normal, AlertSize.mid,
Priority.LOW, VisualAlert.steerRequired, AudibleAlert.none, .0, .1, .1, alert_rate=0.75),
"Steer Right to Start Lane Change Once Safe",
"",
AlertStatus.normal, AlertSize.small,
Priority.LOW, VisualAlert.none, AudibleAlert.none, .0, .1, .1, alert_rate=0.75),
},
EventName.laneChangeBlocked: {
ET.WARNING: Alert(
"Car Detected in Blindspot",
"Monitor Other Vehicles",
AlertStatus.userPrompt, AlertSize.mid,
Priority.LOW, VisualAlert.steerRequired, AudibleAlert.chimePrompt, .1, .1, .1),
"",
AlertStatus.userPrompt, AlertSize.small,
Priority.LOW, VisualAlert.none, AudibleAlert.chimePrompt, .1, .1, .1),
},
EventName.laneChange: {
ET.WARNING: Alert(
"Changing Lane",
"Monitor Other Vehicles",
AlertStatus.normal, AlertSize.mid,
Priority.LOW, VisualAlert.steerRequired, AudibleAlert.none, .0, .1, .1),
"Changing Lanes",
"",
AlertStatus.normal, AlertSize.small,
Priority.LOW, VisualAlert.none, AudibleAlert.none, .0, .1, .1),
},
EventName.steerSaturated: {
@@ -736,7 +729,14 @@ EVENTS: Dict[int, Dict[str, Union[Alert, Callable[[Any, messaging.SubMaster, boo
ET.SOFT_DISABLE: SoftDisableAlert("Low Memory: Reboot Your Device"),
ET.PERMANENT: NormalPermanentAlert("Low Memory", "Reboot your Device"),
ET.NO_ENTRY: NoEntryAlert("Low Memory: Reboot Your Device",
audible_alert=AudibleAlert.chimeDisengage),
audible_alert=AudibleAlert.chimeDisengage),
},
EventName.highCpuUsage: {
#ET.SOFT_DISABLE: SoftDisableAlert("System Malfunction: Reboot Your Device"),
#ET.PERMANENT: NormalPermanentAlert("System Malfunction", "Reboot your Device"),
ET.NO_ENTRY: NoEntryAlert("System Malfunction: Reboot Your Device",
audible_alert=AudibleAlert.chimeDisengage),
},
EventName.accFaulted: {
selfdrive/controls/lib/lane_planner.py
+10 -8
View File
@@ -1,8 +1,10 @@
from common.numpy_fast import interp
import numpy as np
from cereal import log
from common.filter_simple import FirstOrderFilter
from common.numpy_fast import interp
from common.realtime import DT_MDL
from selfdrive.hardware import EON, TICI
from selfdrive.swaglog import cloudlog
from cereal import log
TRAJECTORY_SIZE = 33
@@ -24,8 +26,8 @@ class LanePlanner:
self.ll_x = np.zeros((TRAJECTORY_SIZE,))
self.lll_y = np.zeros((TRAJECTORY_SIZE,))
self.rll_y = np.zeros((TRAJECTORY_SIZE,))
self.lane_width_estimate = 3.7
self.lane_width_certainty = 1.0
self.lane_width_estimate = FirstOrderFilter(3.7, 9.95, DT_MDL)
self.lane_width_certainty = FirstOrderFilter(1.0, 0.95, DT_MDL)
self.lane_width = 3.7
self.lll_prob = 0.
@@ -79,12 +81,12 @@ class LanePlanner:
r_prob *= r_std_mod
# Find current lanewidth
self.lane_width_certainty += 0.05 * (l_prob * r_prob - self.lane_width_certainty)
self.lane_width_certainty.update(l_prob * r_prob)
current_lane_width = abs(self.rll_y[0] - self.lll_y[0])
self.lane_width_estimate += 0.005 * (current_lane_width - self.lane_width_estimate)
self.lane_width_estimate.update(current_lane_width)
speed_lane_width = interp(v_ego, [0., 31.], [2.8, 3.5])
self.lane_width = self.lane_width_certainty * self.lane_width_estimate + \
(1 - self.lane_width_certainty) * speed_lane_width
self.lane_width = self.lane_width_certainty.x * self.lane_width_estimate.x + \
(1 - self.lane_width_certainty.x) * speed_lane_width
clipped_lane_width = min(4.0, self.lane_width)
path_from_left_lane = self.lll_y + clipped_lane_width / 2.0
selfdrive/controls/lib/latcontrol_indi.py
+12 -10
View File
@@ -2,10 +2,11 @@ import math
import numpy as np
from cereal import log
from common.realtime import DT_CTRL
from common.filter_simple import FirstOrderFilter
from common.numpy_fast import clip, interp
from selfdrive.car.toyota.values import CarControllerParams
from common.realtime import DT_CTRL
from selfdrive.car import apply_toyota_steer_torque_limits
from selfdrive.car.toyota.values import CarControllerParams
from selfdrive.controls.lib.drive_helpers import get_steer_max
@@ -43,6 +44,7 @@ class LatControlINDI():
self.sat_count_rate = 1.0 * DT_CTRL
self.sat_limit = CP.steerLimitTimer
self.steer_filter = FirstOrderFilter(0., self.RC, DT_CTRL)
self.reset()
@@ -63,9 +65,9 @@ class LatControlINDI():
return interp(self.speed, self._inner_loop_gain[0], self._inner_loop_gain[1])
def reset(self):
self.delayed_output = 0.
self.steer_filter.x = 0.
self.output_steer = 0.
self.sat_count = 0.0
self.sat_count = 0.
self.speed = 0.
def _check_saturation(self, control, check_saturation, limit):
@@ -96,14 +98,14 @@ class LatControlINDI():
if CS.vEgo < 0.3 or not active:
indi_log.active = False
self.output_steer = 0.0
self.delayed_output = 0.0
self.steer_filter.x = 0.0
else:
rate_des = VM.get_steer_from_curvature(-curvature_rate, CS.vEgo)
# Expected actuator value
alpha = 1. - DT_CTRL / (self.RC + DT_CTRL)
self.delayed_output = self.delayed_output * alpha + self.output_steer * (1. - alpha)
self.steer_filter.update_alpha(self.RC)
self.steer_filter.update(self.output_steer)
# Compute acceleration error
rate_sp = self.outer_loop_gain * (steers_des - self.x[0]) + rate_des
@@ -121,12 +123,12 @@ class LatControlINDI():
# Enforce rate limit
if self.enforce_rate_limit:
steer_max = float(CarControllerParams.STEER_MAX)
new_output_steer_cmd = steer_max * (self.delayed_output + delta_u)
new_output_steer_cmd = steer_max * (self.steer_filter.x + delta_u)
prev_output_steer_cmd = steer_max * self.output_steer
new_output_steer_cmd = apply_toyota_steer_torque_limits(new_output_steer_cmd, prev_output_steer_cmd, prev_output_steer_cmd, CarControllerParams)
self.output_steer = new_output_steer_cmd / steer_max
else:
self.output_steer = self.delayed_output + delta_u
self.output_steer = self.steer_filter.x + delta_u
steers_max = get_steer_max(CP, CS.vEgo)
self.output_steer = clip(self.output_steer, -steers_max, steers_max)
@@ -135,7 +137,7 @@ class LatControlINDI():
indi_log.rateSetPoint = float(rate_sp)
indi_log.accelSetPoint = float(accel_sp)
indi_log.accelError = float(accel_error)
indi_log.delayedOutput = float(self.delayed_output)
indi_log.delayedOutput = float(self.steer_filter.x)
indi_log.delta = float(delta_u)
indi_log.output = float(self.output_steer)
selfdrive/controls/lib/lateral_planner.py
+11
View File
@@ -55,6 +55,7 @@ class LateralPlanner():
self.lane_change_direction = LaneChangeDirection.none
self.lane_change_timer = 0.0
self.lane_change_ll_prob = 1.0
self.keep_pulse_timer = 0.0
self.prev_one_blinker = False
self.desire = log.LateralPlan.Desire.none
@@ -157,6 +158,16 @@ class LateralPlanner():
self.desire = DESIRES[self.lane_change_direction][self.lane_change_state]
# Send keep pulse once per second during LaneChangeStart.preLaneChange
if self.lane_change_state in [LaneChangeState.off, LaneChangeState.laneChangeStarting]:
self.keep_pulse_timer = 0.0
elif self.lane_change_state == LaneChangeState.preLaneChange:
self.keep_pulse_timer += DT_MDL
if self.keep_pulse_timer > 1.0:
self.keep_pulse_timer = 0.0
elif self.desire in [log.LateralPlan.Desire.keepLeft, log.LateralPlan.Desire.keepRight]:
self.desire = log.LateralPlan.Desire.none
# Turn off lanes during lane change
if self.desire == log.LateralPlan.Desire.laneChangeRight or self.desire == log.LateralPlan.Desire.laneChangeLeft:
self.LP.lll_prob *= self.lane_change_ll_prob
selfdrive/controls/lib/long_mpc.py
+3 -1
View File
@@ -45,12 +45,14 @@ class LongitudinalMpc():
self.cur_state[0].v_ego = v_safe
self.cur_state[0].a_ego = a_safe
def update(self, carstate, model, v_cruise):
def update(self, carstate, radarstate, v_cruise):
v_cruise_clipped = np.clip(v_cruise, self.cur_state[0].v_ego - 10., self.cur_state[0].v_ego + 10.0)
poss = v_cruise_clipped * np.array(T_IDXS[:LON_MPC_N+1])
speeds = v_cruise_clipped * np.ones(LON_MPC_N+1)
accels = np.zeros(LON_MPC_N+1)
self.update_with_xva(poss, speeds, accels)
def update_with_xva(self, poss, speeds, accels):
# Calculate mpc
self.libmpc.run_mpc(self.cur_state, self.mpc_solution,
list(poss), list(speeds), list(accels),
selfdrive/controls/lib/longcontrol.py
+1 -1
View File
@@ -72,7 +72,7 @@ class LongControl():
def update(self, active, CS, CP, long_plan):
"""Update longitudinal control. This updates the state machine and runs a PID loop"""
# Interp control trajectory
# TODO estimate car specific lag, use .5s for now
# TODO estimate car specific lag, use .15s for now
if len(long_plan.speeds) == CONTROL_N:
v_target = interp(DEFAULT_LONG_LAG, T_IDXS[:CONTROL_N], long_plan.speeds)
v_target_future = long_plan.speeds[-1]
selfdrive/controls/lib/longitudinal_planner.py
+1 -1
View File
@@ -105,7 +105,7 @@ class Planner():
for key in self.mpcs:
self.mpcs[key].set_cur_state(self.v_desired, self.a_desired)
self.mpcs[key].update(sm['carState'], sm['radarState'], v_cruise)
if self.mpcs[key].status and self.mpcs[key].a_solution[5] < next_a:
if self.mpcs[key].status and self.mpcs[key].a_solution[5] < next_a: # picks slowest solution from accel in ~0.2 seconds
self.longitudinalPlanSource = key
self.v_desired_trajectory = self.mpcs[key].v_solution[:CONTROL_N]
self.a_desired_trajectory = self.mpcs[key].a_solution[:CONTROL_N]
selfdrive/controls/lib/radar_helpers.py
+5 -5
View File
@@ -132,11 +132,11 @@ class Cluster():
def get_RadarState_from_vision(self, lead_msg, v_ego):
return {
"dRel": float(lead_msg.xyva[0] - RADAR_TO_CAMERA),
"yRel": float(-lead_msg.xyva[1]),
"vRel": float(lead_msg.xyva[2]),
"vLead": float(v_ego + lead_msg.xyva[2]),
"vLeadK": float(v_ego + lead_msg.xyva[2]),
"dRel": float(lead_msg.x[0] - RADAR_TO_CAMERA),
"yRel": float(-lead_msg.y[0]),
"vRel": float(lead_msg.v[0] - v_ego),
"vLead": float(lead_msg.v[0]),
"vLeadK": float(lead_msg.v[0]),
"aLeadK": float(0),
"aLeadTau": _LEAD_ACCEL_TAU,
"fcw": False,
selfdrive/controls/radard.py
+8 -8
View File
@@ -38,12 +38,12 @@ def laplacian_cdf(x, mu, b):
def match_vision_to_cluster(v_ego, lead, clusters):
# match vision point to best statistical cluster match
offset_vision_dist = lead.xyva[0] - RADAR_TO_CAMERA
offset_vision_dist = lead.x[0] - RADAR_TO_CAMERA
def prob(c):
prob_d = laplacian_cdf(c.dRel, offset_vision_dist, lead.xyvaStd[0])
prob_y = laplacian_cdf(c.yRel, -lead.xyva[1], lead.xyvaStd[1])
prob_v = laplacian_cdf(c.vRel, lead.xyva[2], lead.xyvaStd[2])
prob_d = laplacian_cdf(c.dRel, offset_vision_dist, lead.xStd[0])
prob_y = laplacian_cdf(c.yRel, -lead.y[0], lead.yStd[0])
prob_v = laplacian_cdf(c.vRel + v_ego, lead.v[0], lead.vStd[0])
# This is isn't exactly right, but good heuristic
return prob_d * prob_y * prob_v
@@ -53,7 +53,7 @@ def match_vision_to_cluster(v_ego, lead, clusters):
# if no 'sane' match is found return -1
# stationary radar points can be false positives
dist_sane = abs(cluster.dRel - offset_vision_dist) < max([(offset_vision_dist)*.25, 5.0])
vel_sane = (abs(cluster.vRel - lead.xyva[2]) < 10) or (v_ego + cluster.vRel > 3)
vel_sane = (abs(cluster.vRel + v_ego - lead.v[0]) < 10) or (v_ego + cluster.vRel > 3)
if dist_sane and vel_sane:
return cluster
else:
@@ -166,9 +166,9 @@ class RadarD():
radarState.carStateMonoTime = sm.logMonoTime['carState']
if enable_lead:
if len(sm['modelV2'].leads) > 1:
radarState.leadOne = get_lead(self.v_ego, self.ready, clusters, sm['modelV2'].leads[0], low_speed_override=True)
radarState.leadTwo = get_lead(self.v_ego, self.ready, clusters, sm['modelV2'].leads[1], low_speed_override=False)
if len(sm['modelV2'].leadsV3) > 1:
radarState.leadOne = get_lead(self.v_ego, self.ready, clusters, sm['modelV2'].leadsV3[0], low_speed_override=True)
radarState.leadTwo = get_lead(self.v_ego, self.ready, clusters, sm['modelV2'].leadsV3[1], low_speed_override=False)
return dat
selfdrive/debug/cpu_usage_stat.py
+1 -1
View File
@@ -44,7 +44,7 @@ def get_arg_parser():
parser = argparse.ArgumentParser(formatter_class=argparse.ArgumentDefaultsHelpFormatter)
parser.add_argument("proc_names", nargs="?", default='',
help="Process names to be monitored, comma seperated")
help="Process names to be monitored, comma separated")
parser.add_argument("--list_all", action='store_true',
help="Show all running processes' cmdline")
parser.add_argument("--detailed_times", action='store_true',
selfdrive/debug/cycle_alerts.py
+9 -9
View File
@@ -24,16 +24,13 @@ def cycle_alerts(duration=2000, is_metric=False):
sm = messaging.SubMaster(['deviceState', 'pandaState', 'roadCameraState', 'modelV2', 'liveCalibration',
'driverMonitoringState', 'longitudinalPlan', 'lateralPlan', 'liveLocationKalman'])
controls_state = messaging.pub_sock('controlsState')
deviceState = messaging.pub_sock('deviceState')
idx, last_alert_millis = 0, 0
pm = messaging.PubMaster(['controlsState', 'pandaState', 'deviceState'])
events = Events()
AM = AlertManager()
frame = 0
idx, last_alert_millis = 0, 0
while 1:
if frame % duration == 0:
idx = (idx + 1) % len(alerts)
@@ -50,7 +47,6 @@ def cycle_alerts(duration=2000, is_metric=False):
dat = messaging.new_message()
dat.init('controlsState')
dat.controlsState.alertText1 = AM.alert_text_1
dat.controlsState.alertText2 = AM.alert_text_2
dat.controlsState.alertSize = AM.alert_size
@@ -58,14 +54,18 @@ def cycle_alerts(duration=2000, is_metric=False):
dat.controlsState.alertBlinkingRate = AM.alert_rate
dat.controlsState.alertType = AM.alert_type
dat.controlsState.alertSound = AM.audible_alert
controls_state.send(dat.to_bytes())
pm.send('controlsState', dat)
dat = messaging.new_message()
dat.init('deviceState')
dat.deviceState.started = True
deviceState.send(dat.to_bytes())
pm.send('deviceState', dat)
dat = messaging.new_message()
dat.init('pandaState')
dat.pandaState.ignitionLine = True
pm.send('pandaState', dat)
frame += 1
time.sleep(0.01)
if __name__ == '__main__':
selfdrive/debug/uiview.py
+7 -8
View File
@@ -3,9 +3,8 @@ import time
import cereal.messaging as messaging
from selfdrive.manager.process_config import managed_processes
if __name__ == "__main__":
services = ['controlsState', 'deviceState', 'radarState'] # the services needed to be spoofed to start ui offroad
services = ['controlsState', 'deviceState', 'pandaState'] # the services needed to be spoofed to start ui offroad
procs = ['camerad', 'ui', 'modeld', 'calibrationd']
for p in procs:
@@ -13,15 +12,15 @@ if __name__ == "__main__":
pm = messaging.PubMaster(services)
dat_controlsState, dat_deviceState, dat_radar = [messaging.new_message(s) for s in services]
dat_deviceState.deviceState.started = True
msgs = {s: messaging.new_message(s) for s in services}
msgs['deviceState'].deviceState.started = True
msgs['pandaState'].pandaState.ignitionLine = True
try:
while True:
pm.send('controlsState', dat_controlsState)
pm.send('deviceState', dat_deviceState)
pm.send('radarState', dat_radar)
time.sleep(1 / 100) # continually send, rate doesn't matter for deviceState
time.sleep(1 / 100) # continually send, rate doesn't matter
for s in msgs:
pm.send(s, msgs[s])
except KeyboardInterrupt:
for p in procs:
managed_processes[p].stop()
selfdrive/hardware/eon/androidd.py
Executable
+78
View File
@@ -0,0 +1,78 @@
#!/usr/bin/env python3
import os
import time
import psutil
from typing import Optional
from common.realtime import set_core_affinity, set_realtime_priority
from selfdrive.swaglog import cloudlog
MAX_MODEM_CRASHES = 3
MODEM_PATH = "/sys/devices/soc/2080000.qcom,mss/subsys5"
WATCHED_PROCS = ["zygote", "zygote64", "/system/bin/servicemanager", "/system/bin/surfaceflinger"]
def get_modem_crash_count() -> Optional[int]:
try:
with open(os.path.join(MODEM_PATH, "crash_count")) as f:
return int(f.read())
except Exception:
cloudlog.exception("Error reading modem crash count")
return None
def get_modem_state() -> str:
try:
with open(os.path.join(MODEM_PATH, "state")) as f:
return f.read().strip()
except Exception:
cloudlog.exception("Error reading modem state")
return ""
def main():
set_core_affinity(1)
set_realtime_priority(1)
procs = {}
crash_count = 0
modem_killed = False
modem_state = "ONLINE"
while True:
# check critical android services
if any(p is None or not p.is_running() for p in procs.values()) or not len(procs):
cur = {p: None for p in WATCHED_PROCS}
for p in psutil.process_iter(attrs=['cmdline']):
cmdline = None if not len(p.info['cmdline']) else p.info['cmdline'][0]
if cmdline in WATCHED_PROCS:
cur[cmdline] = p
if len(procs):
for p in WATCHED_PROCS:
if cur[p] != procs[p]:
cloudlog.event("android service pid changed", proc=p, cur=cur[p], prev=procs[p])
procs.update(cur)
# check modem state
state = get_modem_state()
if state != modem_state and not modem_killed:
cloudlog.event("modem state changed", state=state)
modem_state = state
# check modem crashes
cnt = get_modem_crash_count()
if cnt is not None:
if cnt > crash_count:
cloudlog.event("modem crash", count=cnt)
crash_count = cnt
# handle excessive modem crashes
if crash_count > MAX_MODEM_CRASHES and not modem_killed:
cloudlog.event("killing modem")
with open("/sys/kernel/debug/msm_subsys/modem", "w") as f:
f.write("put")
modem_killed = True
time.sleep(1)
if __name__ == "__main__":
main()
selfdrive/hardware/hw.h
+17
View File
@@ -1,6 +1,7 @@
#pragma once
#include "selfdrive/hardware/base.h"
#include "selfdrive/common/util.h"
#ifdef QCOM
#include "selfdrive/hardware/eon/hardware.h"
@@ -16,3 +17,19 @@ public:
};
#define Hardware HardwarePC
#endif
namespace Path {
inline static std::string HOME = util::getenv("HOME");
inline std::string log_root() {
if (const char *env = getenv("LOG_ROOT")) {
return env;
}
return Hardware::PC() ? HOME + "/.comma/media/0/realdata" : "/data/media/0/realdata";
}
inline std::string params() {
return Hardware::PC() ? HOME + "/.comma/params" : "/data/params";
}
inline std::string rsa_file() {
return Hardware::PC() ? HOME + "/.comma/persist/comma/id_rsa" : "/persist/comma/id_rsa";
}
} // namespace Path
selfdrive/hardware/tici/agnos.json
+7 -7
View File
@@ -1,10 +1,10 @@
[
{
"name": "boot",
"url": "https://commadist.azureedge.net/agnosupdate/boot-54fc7edceeabff713b51dd4d4931eb920344aa3f8f8e0f153baad1fb5a15ef19.img.xz",
"hash": "54fc7edceeabff713b51dd4d4931eb920344aa3f8f8e0f153baad1fb5a15ef19",
"hash_raw": "54fc7edceeabff713b51dd4d4931eb920344aa3f8f8e0f153baad1fb5a15ef19",
"size": 14772224,
"url": "https://commadist.azureedge.net/agnosupdate/boot-6a5ac08b5c458e24e810dd950185ee69b7c5f3556f9f43b323f1dba4e14f8d44.img.xz",
"hash": "6a5ac08b5c458e24e810dd950185ee69b7c5f3556f9f43b323f1dba4e14f8d44",
"hash_raw": "6a5ac08b5c458e24e810dd950185ee69b7c5f3556f9f43b323f1dba4e14f8d44",
"size": 14776320,
"sparse": false,
"full_check": true,
"has_ab": true
@@ -41,9 +41,9 @@
},
{
"name": "system",
"url": "https://commadist.azureedge.net/agnosupdate/system-5208f2d72b6d99bbee7dc038b3c95eccabc0bcabc5c718419e06bc7f897a675a.img.xz",
"hash": "68a3d0bb892c7b8347ce8b6d5a733a3f15d622af11febaeb3d6595308f15e738",
"hash_raw": "5208f2d72b6d99bbee7dc038b3c95eccabc0bcabc5c718419e06bc7f897a675a",
"url": "https://commadist.azureedge.net/agnosupdate/system-9056640f926b2612be503a9fec621091ae8a86ec3b2eeb15503db39490a6d2f4.img.xz",
"hash": "94445bb5db56bb2d462fa3831d3501cb8ffedafed9bf2e57128ffd71f454a4bd",
"hash_raw": "9056640f926b2612be503a9fec621091ae8a86ec3b2eeb15503db39490a6d2f4",
"size": 10737418240,
"sparse": true,
"full_check": false,
selfdrive/hardware/tici/hardware.py
+2
View File
@@ -266,6 +266,8 @@ class Tici(HardwareBase):
def set_power_save(self, powersave_enabled):
# amplifier, 100mW at idle
self.amplifier.set_global_shutdown(amp_disabled=powersave_enabled)
if not powersave_enabled:
self.amplifier.initialize_configuration()
# offline big cluster, leave core 4 online for boardd
for i in range(5, 8):
selfdrive/hardware/tici/updater
BIN
View File
Binary file not shown.
selfdrive/locationd/calibrationd.py
+2 -2
View File
@@ -31,7 +31,7 @@ INPUTS_WANTED = 50 # We want a little bit more than we need for stability
MAX_ALLOWED_SPREAD = np.radians(2)
RPY_INIT = np.array([0.0,0.0,0.0])
# These values are needed to accomodate biggest modelframe
# These values are needed to accommodate biggest modelframe
PITCH_LIMITS = np.array([-0.09074112085129739, 0.14907572052989657])
YAW_LIMITS = np.array([-0.06912048084718224, 0.06912048084718235])
DEBUG = os.getenv("DEBUG") is not None
@@ -115,7 +115,7 @@ class Calibrator():
self.cal_status = Calibration.INVALID
# If spread is too high, assume mounting was changed and reset to last block.
# Make the transition smooth. Abrupt transistion are not good foor feedback loop through supercombo model.
# Make the transition smooth. Abrupt transitions are not good foor feedback loop through supercombo model.
if max(self.calib_spread) > MAX_ALLOWED_SPREAD and self.cal_status == Calibration.CALIBRATED:
self.reset(self.rpys[self.block_idx - 1], valid_blocks=INPUTS_NEEDED, smooth_from=self.rpy)
selfdrive/locationd/locationd.cc
+5 -2
View File
@@ -82,8 +82,9 @@ void Localizer::build_live_location(cereal::LiveLocationKalman::Builder& fix) {
//fix_pos_geo_std = np.abs(coord.ecef2geodetic(fix_ecef + fix_ecef_std) - fix_pos_geo)
VectorXd orientation_ecef = quat2euler(vector2quat(predicted_state.segment<STATE_ECEF_ORIENTATION_LEN>(STATE_ECEF_ORIENTATION_START)));
VectorXd orientation_ecef_std = predicted_std.segment<STATE_ECEF_ORIENTATION_ERR_LEN>(STATE_ECEF_ORIENTATION_ERR_START);
MatrixXdr device_from_ecef = quat2rot(vector2quat(predicted_state.segment<STATE_ECEF_ORIENTATION_LEN>(STATE_ECEF_ORIENTATION_START))).transpose();
VectorXd calibrated_orientation_ecef = rot2euler(this->calib_from_device * device_from_ecef);
MatrixXdr device_from_ecef = euler2rot(orientation_ecef).transpose();
//VectorXd calibrated_orientation_ecef = rot2euler(device_from_ecef);
VectorXd calibrated_orientation_ecef = rot2euler((this->calib_from_device * device_from_ecef).transpose());
VectorXd acc_calib = this->calib_from_device * predicted_state.segment<STATE_ACCELERATION_LEN>(STATE_ACCELERATION_START);
MatrixXdr acc_calib_cov = predicted_cov.block<STATE_ACCELERATION_ERR_LEN, STATE_ACCELERATION_ERR_LEN>(STATE_ACCELERATION_ERR_START, STATE_ACCELERATION_ERR_START);
@@ -115,6 +116,7 @@ void Localizer::build_live_location(cereal::LiveLocationKalman::Builder& fix) {
VectorXd orientation_ned = ned_euler_from_ecef(fix_ecef_ecef, orientation_ecef);
//orientation_ned_std = ned_euler_from_ecef(fix_ecef, orientation_ecef + orientation_ecef_std) - orientation_ned
VectorXd calibrated_orientation_ned = ned_euler_from_ecef(fix_ecef_ecef, calibrated_orientation_ecef);
VectorXd nextfix_ecef = fix_ecef + vel_ecef;
VectorXd ned_vel = this->converter->ecef2ned((ECEF) { .x = nextfix_ecef(0), .y = nextfix_ecef(1), .z = nextfix_ecef(2) }).to_vector() - converter->ecef2ned(fix_ecef_ecef).to_vector();
//ned_vel_std = self.converter->ecef2ned(fix_ecef + vel_ecef + vel_ecef_std) - self.converter->ecef2ned(fix_ecef + vel_ecef)
@@ -137,6 +139,7 @@ void Localizer::build_live_location(cereal::LiveLocationKalman::Builder& fix) {
init_measurement(fix.initOrientationECEF(), orientation_ecef, orientation_ecef_std, true);
init_measurement(fix.initCalibratedOrientationECEF(), calibrated_orientation_ecef, nans, this->calibrated);
init_measurement(fix.initOrientationNED(), orientation_ned, nans, true);
init_measurement(fix.initCalibratedOrientationNED(), calibrated_orientation_ned, nans, true);
init_measurement(fix.initAngularVelocityDevice(), angVelocityDevice, angVelocityDeviceErr, true);
init_measurement(fix.initVelocityCalibrated(), vel_calib, vel_calib_std, this->calibrated);
init_measurement(fix.initAngularVelocityCalibrated(), ang_vel_calib, ang_vel_calib_std, this->calibrated);
selfdrive/locationd/ublox_msg.cc
+1 -1
View File
@@ -117,7 +117,7 @@ std::pair<std::string, kj::Array<capnp::word>> UbloxMsgParser::gen_msg() {
return {"ubloxGnss", gen_mon_hw2(static_cast<ubx_t::mon_hw2_t*>(body))};
break;
default:
LOGE("Unkown message type %x", ubx_message.msg_type());
LOGE("Unknown message type %x", ubx_message.msg_type());
return {"ubloxGnss", kj::Array<capnp::word>()};
break;
}
selfdrive/loggerd/bootlog.cc
+2 -4
View File
@@ -12,8 +12,7 @@ static kj::Array<capnp::word> build_boot_log() {
boot.setWallTimeNanos(nanos_since_epoch());
std::string pstore = "/sys/fs/pstore";
std::map<std::string, std::string> pstore_map;
util::read_files_in_dir(pstore, &pstore_map);
std::map<std::string, std::string> pstore_map = util::read_files_in_dir(pstore);
const std::vector<std::string> log_keywords = {"Kernel panic"};
auto lpstore = boot.initPstore().initEntries(pstore_map.size());
@@ -31,8 +30,7 @@ static kj::Array<capnp::word> build_boot_log() {
}
}
std::string launchLog = util::read_file("/tmp/launch_log");
boot.setLaunchLog(capnp::Text::Reader(launchLog.data(), launchLog.size()));
boot.setLaunchLog(util::read_file("/tmp/launch_log"));
return capnp::messageToFlatArray(msg);
}
selfdrive/loggerd/config.py
+2 -2
View File
@@ -2,8 +2,8 @@ import os
from pathlib import Path
from selfdrive.hardware import PC
if os.environ.get('LOGGERD_ROOT', False):
ROOT = os.environ['LOGGERD_ROOT']
if os.environ.get('LOG_ROOT', False):
ROOT = os.environ['LOG_ROOT']
elif PC:
ROOT = os.path.join(str(Path.home()), ".comma", "media", "0", "realdata")
else:
selfdrive/loggerd/logger.cc
+18 -16
View File
@@ -60,7 +60,7 @@ kj::Array<capnp::word> logger_build_init_data() {
init.setDeviceType(cereal::InitData::DeviceType::PC);
}
init.setVersion(capnp::Text::Reader(COMMA_VERSION));
init.setVersion(COMMA_VERSION);
std::ifstream cmdline_stream("/proc/cmdline");
std::vector<std::string> kernel_args;
@@ -94,23 +94,25 @@ kj::Array<capnp::word> logger_build_init_data() {
init.setDirty(!getenv("CLEAN"));
// log params
Params params = Params();
init.setGitCommit(params.get("GitCommit"));
init.setGitBranch(params.get("GitBranch"));
init.setGitRemote(params.get("GitRemote"));
auto params = Params();
std::map<std::string, std::string> params_map = params.readAll();
init.setGitCommit(params_map["GitCommit"]);
init.setGitBranch(params_map["GitBranch"]);
init.setGitRemote(params_map["GitRemote"]);
init.setPassive(params.getBool("Passive"));
init.setDongleId(params.get("DongleId"));
{
std::map<std::string, std::string> params_map;
params.readAll(&params_map);
auto lparams = init.initParams().initEntries(params_map.size());
int i = 0;
for (auto& kv : params_map) {
auto lentry = lparams[i];
lentry.setKey(kv.first);
lentry.setValue(capnp::Data::Reader((const kj::byte*)kv.second.data(), kv.second.size()));
i++;
init.setDongleId(params_map["DongleId"]);
auto lparams = init.initParams().initEntries(params_map.size());
int i = 0;
for (auto& [key, value] : params_map) {
auto lentry = lparams[i];
lentry.setKey(key);
if ( !(params.getKeyType(key) & DONT_LOG) ) {
lentry.setValue(capnp::Data::Reader((const kj::byte*)value.data(), value.size()));
}
i++;
}
return capnp::messageToFlatArray(msg);
}
selfdrive/loggerd/logger.h
+5 -6
View File
@@ -15,11 +15,7 @@
#include "selfdrive/common/swaglog.h"
#include "selfdrive/hardware/hw.h"
const std::string DEFAULT_LOG_ROOT =
Hardware::PC() ? util::getenv_default("HOME", "/.comma/media/0/realdata", "/data/media/0/realdata")
: "/data/media/0/realdata";
const std::string LOG_ROOT = util::getenv_default("LOG_ROOT", "", DEFAULT_LOG_ROOT.c_str());
const std::string LOG_ROOT = Path::log_root();
#define LOGGER_MAX_HANDLES 16
@@ -43,7 +39,10 @@ class BZFile {
}
inline void write(void* data, size_t size) {
int bzerror;
BZ2_bzWrite(&bzerror, bz_file, data, size);
do {
BZ2_bzWrite(&bzerror, bz_file, data, size);
} while (bzerror == BZ_IO_ERROR && errno == EINTR);
if (bzerror != BZ_OK && !error_logged) {
LOGE("BZ2_bzWrite error, bzerror=%d", bzerror);
error_logged = true;
selfdrive/loggerd/loggerd.cc
+103 -221
View File
@@ -15,6 +15,7 @@
#include <random>
#include <string>
#include <thread>
#include <unordered_map>
#include "cereal/messaging/messaging.h"
#include "cereal/services.h"
@@ -59,7 +60,8 @@ LogCameraInfo cameras_logged[LOG_CAMERA_ID_MAX] = {
.bitrate = MAIN_BITRATE,
.is_h265 = true,
.downscale = false,
.has_qcamera = true
.has_qcamera = true,
.trigger_rotate = true
},
[LOG_CAMERA_ID_DCAMERA] = {
.stream_type = VISION_STREAM_YUV_FRONT,
@@ -69,7 +71,8 @@ LogCameraInfo cameras_logged[LOG_CAMERA_ID_MAX] = {
.bitrate = DCAM_BITRATE,
.is_h265 = true,
.downscale = false,
.has_qcamera = false
.has_qcamera = false,
.trigger_rotate = Hardware::TICI(),
},
[LOG_CAMERA_ID_ECAMERA] = {
.stream_type = VISION_STREAM_YUV_WIDE,
@@ -79,7 +82,8 @@ LogCameraInfo cameras_logged[LOG_CAMERA_ID_MAX] = {
.bitrate = MAIN_BITRATE,
.is_h265 = true,
.downscale = false,
.has_qcamera = false
.has_qcamera = false,
.trigger_rotate = true
},
[LOG_CAMERA_ID_QCAMERA] = {
.filename = "qcamera.ts",
@@ -92,81 +96,27 @@ LogCameraInfo cameras_logged[LOG_CAMERA_ID_MAX] = {
},
};
class RotateState {
public:
SubSocket* fpkt_sock;
uint32_t stream_frame_id, log_frame_id, last_rotate_frame_id;
bool enabled, should_rotate, initialized;
std::atomic<bool> rotating;
std::atomic<int> cur_seg;
RotateState() : fpkt_sock(nullptr), stream_frame_id(0), log_frame_id(0),
last_rotate_frame_id(UINT32_MAX), enabled(false), should_rotate(false), initialized(false), rotating(false), cur_seg(-1) {};
void waitLogThread() {
std::unique_lock<std::mutex> lk(fid_lock);
while (stream_frame_id > log_frame_id // if the log camera is older, wait for it to catch up.
&& (stream_frame_id - log_frame_id) < 8 // but if its too old then there probably was a discontinuity (visiond restarted)
&& !do_exit) {
cv.wait(lk);
}
}
void cancelWait() {
cv.notify_one();
}
void setStreamFrameId(uint32_t frame_id) {
fid_lock.lock();
stream_frame_id = frame_id;
fid_lock.unlock();
cv.notify_one();
}
void setLogFrameId(uint32_t frame_id) {
fid_lock.lock();
log_frame_id = frame_id;
fid_lock.unlock();
cv.notify_one();
}
void rotate() {
if (enabled) {
std::unique_lock<std::mutex> lk(fid_lock);
should_rotate = true;
last_rotate_frame_id = stream_frame_id;
}
}
void finish_rotate() {
std::unique_lock<std::mutex> lk(fid_lock);
should_rotate = false;
}
private:
std::mutex fid_lock;
std::condition_variable cv;
};
struct LoggerdState {
Context *ctx;
LoggerState logger = {};
char segment_path[4096];
int rotate_segment;
pthread_mutex_t rotate_lock;
RotateState rotate_state[LOG_CAMERA_ID_MAX-1];
std::mutex rotate_lock;
std::condition_variable rotate_cv;
std::atomic<int> rotate_segment;
std::atomic<double> last_camera_seen_tms;
std::atomic<int> waiting_rotate;
int max_waiting = 0;
double last_rotate_tms = 0.;
};
LoggerdState s;
void encoder_thread(int cam_idx) {
assert(cam_idx < LOG_CAMERA_ID_MAX-1);
LogCameraInfo &cam_info = cameras_logged[cam_idx];
RotateState &rotate_state = s.rotate_state[cam_idx];
const LogCameraInfo &cam_info = cameras_logged[cam_idx];
set_thread_name(cam_info.filename);
int cnt = 0;
int cnt = 0, cur_seg = -1;
int encode_idx = 0;
LoggerHandle *lh = NULL;
std::vector<Encoder *> encoders;
VisionIpcClient vipc_client = VisionIpcClient("camerad", cam_info.stream_type, false);
@@ -198,68 +148,47 @@ void encoder_thread(int cam_idx) {
while (!do_exit) {
VisionIpcBufExtra extra;
VisionBuf* buf = vipc_client.recv(&extra);
if (buf == nullptr) {
continue;
if (buf == nullptr) continue;
if (cam_info.trigger_rotate) {
s.last_camera_seen_tms = millis_since_boot();
}
//printf("logger latency to tsEof: %f\n", (double)(nanos_since_boot() - extra.timestamp_eof) / 1000000.0);
if (cam_info.trigger_rotate && (cnt >= SEGMENT_LENGTH * MAIN_FPS)) {
// trigger rotate and wait logger rotated to new segment
++s.waiting_rotate;
std::unique_lock lk(s.rotate_lock);
s.rotate_cv.wait(lk, [&] { return s.rotate_segment > cur_seg || do_exit; });
}
if (do_exit) break;
// all the rotation stuff
{
pthread_mutex_lock(&s.rotate_lock);
pthread_mutex_unlock(&s.rotate_lock);
// rotate the encoder if the logger is on a newer segment
if (s.rotate_segment > cur_seg) {
cur_seg = s.rotate_segment;
cnt = 0;
// wait if camera pkt id is older than stream
rotate_state.waitLogThread();
if (do_exit) break;
// rotate the encoder if the logger is on a newer segment
if (rotate_state.should_rotate) {
LOGW("camera %d rotate encoder to %s", cam_idx, s.segment_path);
if (!rotate_state.initialized) {
rotate_state.last_rotate_frame_id = extra.frame_id - 1;
rotate_state.initialized = true;
}
// get new logger handle for new segment
if (lh) {
lh_close(lh);
}
lh = logger_get_handle(&s.logger);
// wait for all to start rotating
rotate_state.rotating = true;
for(auto &r : s.rotate_state) {
while(r.enabled && !r.rotating && !do_exit) util::sleep_for(5);
}
pthread_mutex_lock(&s.rotate_lock);
for (auto &e : encoders) {
e->encoder_close();
e->encoder_open(s.segment_path);
}
rotate_state.cur_seg = s.rotate_segment;
pthread_mutex_unlock(&s.rotate_lock);
// wait for all to finish rotating
for(auto &r : s.rotate_state) {
while(r.enabled && r.cur_seg != s.rotate_segment && !do_exit) util::sleep_for(5);
}
rotate_state.rotating = false;
rotate_state.finish_rotate();
LOGW("camera %d rotate encoder to %s", cam_idx, s.segment_path);
for (auto &e : encoders) {
e->encoder_close();
e->encoder_open(s.segment_path);
}
if (lh) {
lh_close(lh);
}
lh = logger_get_handle(&s.logger);
}
rotate_state.setStreamFrameId(extra.frame_id);
// encode a frame
for (int i = 0; i < encoders.size(); ++i) {
int out_id = encoders[i]->encode_frame(buf->y, buf->u, buf->v,
buf->width, buf->height, extra.timestamp_eof);
if (out_id == -1) {
LOGE("Failed to encode frame. frame_id: %d encode_id: %d", extra.frame_id, encode_idx);
}
// publish encode index
if (i == 0 && out_id != -1) {
// publish encode index
MessageBuilder msg;
// this is really ugly
auto eidx = cam_idx == LOG_CAMERA_ID_DCAMERA ? msg.initEvent().initDriverEncodeIdx() :
@@ -272,8 +201,8 @@ void encoder_thread(int cam_idx) {
} else {
eidx.setType(cam_idx == LOG_CAMERA_ID_DCAMERA ? cereal::EncodeIndex::Type::FRONT : cereal::EncodeIndex::Type::FULL_H_E_V_C);
}
eidx.setEncodeId(cnt);
eidx.setSegmentNum(rotate_state.cur_seg);
eidx.setEncodeId(encode_idx);
eidx.setSegmentNum(cur_seg);
eidx.setSegmentId(out_id);
if (lh) {
// TODO: this should read cereal/services.h for qlog decimation
@@ -284,6 +213,7 @@ void encoder_thread(int cam_idx) {
}
cnt++;
encode_idx++;
}
if (lh) {
@@ -311,6 +241,33 @@ void clear_locks() {
ftw(LOG_ROOT.c_str(), clear_locks_fn, 16);
}
void logger_rotate() {
{
std::unique_lock lk(s.rotate_lock);
int segment = -1;
int err = logger_next(&s.logger, LOG_ROOT.c_str(), s.segment_path, sizeof(s.segment_path), &segment);
assert(err == 0);
s.rotate_segment = segment;
s.waiting_rotate = 0;
s.last_rotate_tms = millis_since_boot();
}
s.rotate_cv.notify_all();
LOGW((s.logger.part == 0) ? "logging to %s" : "rotated to %s", s.segment_path);
}
void rotate_if_needed() {
if (s.waiting_rotate == s.max_waiting) {
logger_rotate();
}
double tms = millis_since_boot();
if ((tms - s.last_rotate_tms) > SEGMENT_LENGTH * 1000 &&
(tms - s.last_camera_seen_tms) > NO_CAMERA_PATIENCE) {
LOGW("no camera packet seen. auto rotating");
logger_rotate();
}
}
} // namespace
int main(int argc, char** argv) {
@@ -322,11 +279,10 @@ int main(int argc, char** argv) {
typedef struct QlogState {
int counter, freq;
} QlogState;
std::map<SubSocket*, QlogState> qlog_states;
std::unordered_map<SubSocket*, QlogState> qlog_states;
s.ctx = Context::create();
Poller * poller = Poller::create();
std::vector<SubSocket*> socks;
// subscribe to all socks
for (const auto& it : services) {
@@ -335,13 +291,6 @@ int main(int argc, char** argv) {
SubSocket * sock = SubSocket::create(s.ctx, it.name);
assert(sock != NULL);
poller->registerSocket(sock);
socks.push_back(sock);
for (int cid=0; cid<=MAX_CAM_IDX; cid++) {
if (std::string(it.name) == cameras_logged[cid].frame_packet_name) {
s.rotate_state[cid].fpkt_sock = sock;
}
}
qlog_states[sock] = {.counter = 0, .freq = it.decimation};
}
@@ -349,124 +298,57 @@ int main(int argc, char** argv) {
// init logger
logger_init(&s.logger, "rlog", true);
logger_rotate();
params.put("CurrentRoute", s.logger.route_name);
// init encoders
pthread_mutex_init(&s.rotate_lock, NULL);
s.last_camera_seen_tms = millis_since_boot();
// TODO: create these threads dynamically on frame packet presence
std::vector<std::thread> encoder_threads;
encoder_threads.push_back(std::thread(encoder_thread, LOG_CAMERA_ID_FCAMERA));
s.rotate_state[LOG_CAMERA_ID_FCAMERA].enabled = true;
if (cameras_logged[LOG_CAMERA_ID_FCAMERA].trigger_rotate) {
s.max_waiting += 1;
}
if (!Hardware::PC() && params.getBool("RecordFront")) {
encoder_threads.push_back(std::thread(encoder_thread, LOG_CAMERA_ID_DCAMERA));
s.rotate_state[LOG_CAMERA_ID_DCAMERA].enabled = true;
if (cameras_logged[LOG_CAMERA_ID_DCAMERA].trigger_rotate) {
s.max_waiting += 1;
}
}
if (Hardware::TICI()) {
encoder_threads.push_back(std::thread(encoder_thread, LOG_CAMERA_ID_ECAMERA));
s.rotate_state[LOG_CAMERA_ID_ECAMERA].enabled = true;
if (cameras_logged[LOG_CAMERA_ID_ECAMERA].trigger_rotate) {
s.max_waiting += 1;
}
}
uint64_t msg_count = 0;
uint64_t bytes_count = 0;
AlignedBuffer aligned_buf;
double start_ts = seconds_since_boot();
double last_rotate_tms = millis_since_boot();
double last_camera_seen_tms = millis_since_boot();
uint64_t msg_count = 0, bytes_count = 0;
double start_ts = millis_since_boot();
while (!do_exit) {
// TODO: fix msgs from the first poll getting dropped
// poll for new messages on all sockets
for (auto sock : poller->poll(1000)) {
// drain socket
Message * last_msg = nullptr;
while (!do_exit) {
Message * msg = sock->receive(true);
if (!msg) {
break;
}
delete last_msg;
last_msg = msg;
QlogState& qs = qlog_states[sock];
logger_log(&s.logger, (uint8_t*)msg->getData(), msg->getSize(), qs.counter == 0 && qs.freq != -1);
if (qs.freq != -1) {
qs.counter = (qs.counter + 1) % qs.freq;
}
QlogState &qs = qlog_states[sock];
Message *msg = nullptr;
while (!do_exit && (msg = sock->receive(true))) {
const bool in_qlog = qs.freq != -1 && (qs.counter++ % qs.freq == 0);
logger_log(&s.logger, (uint8_t *)msg->getData(), msg->getSize(), in_qlog);
bytes_count += msg->getSize();
delete msg;
rotate_if_needed();
if ((++msg_count % 1000) == 0) {
double ts = seconds_since_boot();
LOGD("%lu messages, %.2f msg/sec, %.2f KB/sec", msg_count, msg_count * 1.0 / (ts - start_ts), bytes_count * 0.001 / (ts - start_ts));
double seconds = (millis_since_boot() - start_ts) / 1000.0;
LOGD("%lu messages, %.2f msg/sec, %.2f KB/sec", msg_count, msg_count / seconds, bytes_count * 0.001 / seconds);
}
}
if (last_msg) {
int fpkt_id = -1;
for (int cid = 0; cid <=MAX_CAM_IDX; cid++) {
if (sock == s.rotate_state[cid].fpkt_sock) {
fpkt_id=cid;
break;
}
}
if (fpkt_id >= 0) {
// track camera frames to sync to encoder
// only process last frame
capnp::FlatArrayMessageReader cmsg(aligned_buf.align(last_msg));
cereal::Event::Reader event = cmsg.getRoot<cereal::Event>();
if (fpkt_id == LOG_CAMERA_ID_FCAMERA) {
s.rotate_state[fpkt_id].setLogFrameId(event.getRoadCameraState().getFrameId());
} else if (fpkt_id == LOG_CAMERA_ID_DCAMERA) {
s.rotate_state[fpkt_id].setLogFrameId(event.getDriverCameraState().getFrameId());
} else if (fpkt_id == LOG_CAMERA_ID_ECAMERA) {
s.rotate_state[fpkt_id].setLogFrameId(event.getWideRoadCameraState().getFrameId());
}
last_camera_seen_tms = millis_since_boot();
}
}
delete last_msg;
}
bool new_segment = s.logger.part == -1;
if (s.logger.part > -1) {
double tms = millis_since_boot();
if (tms - last_camera_seen_tms <= NO_CAMERA_PATIENCE && encoder_threads.size() > 0) {
new_segment = true;
for (auto &r : s.rotate_state) {
// this *should* be redundant on tici since all camera frames are synced
new_segment &= (((r.stream_frame_id >= r.last_rotate_frame_id + SEGMENT_LENGTH * MAIN_FPS) &&
(!r.should_rotate) && (r.initialized)) ||
(!r.enabled));
if (!Hardware::TICI()) break; // only look at fcamera frame id if not QCOM2
}
} else {
if (tms - last_rotate_tms > SEGMENT_LENGTH * 1000) {
new_segment = true;
LOGW("no camera packet seen. auto rotated");
}
}
}
// rotate to new segment
if (new_segment) {
pthread_mutex_lock(&s.rotate_lock);
last_rotate_tms = millis_since_boot();
int err = logger_next(&s.logger, LOG_ROOT.c_str(), s.segment_path, sizeof(s.segment_path), &s.rotate_segment);
assert(err == 0);
LOGW((s.logger.part == 0) ? "logging to %s" : "rotated to %s", s.segment_path);
// rotate encoders
for (auto &r : s.rotate_state) r.rotate();
pthread_mutex_unlock(&s.rotate_lock);
}
}
LOGW("closing encoders");
for (auto &r : s.rotate_state) r.cancelWait();
s.rotate_cv.notify_all();
for (auto &t : encoder_threads) t.join();
LOGW("closing logger");
@@ -479,7 +361,7 @@ int main(int argc, char** argv) {
}
// messaging cleanup
for (auto sock : socks) delete sock;
for (auto &[sock, qs] : qlog_states) delete sock;
delete poller;
delete s.ctx;
selfdrive/loggerd/omx_encoder.cc
+3 -3
View File
@@ -21,9 +21,9 @@
#include "selfdrive/loggerd/include/msm_media_info.h"
// Check the OMX error code and assert if an error occurred.
#define OMX_CHECK(_expr) \
do { \
assert(OMX_ErrorNone == _expr); \
#define OMX_CHECK(_expr) \
do { \
assert(OMX_ErrorNone == (_expr)); \
} while (0)
extern ExitHandler do_exit;
selfdrive/manager/process.py
+2 -2
View File
@@ -37,7 +37,7 @@ def launcher(proc):
except KeyboardInterrupt:
cloudlog.warning("child %s got SIGINT" % proc)
except Exception:
# can't install the crash handler becuase sys.excepthook doesn't play nice
# can't install the crash handler because sys.excepthook doesn't play nice
# with threads, so catch it here.
crash.capture_exception()
raise
@@ -228,7 +228,7 @@ class PythonProcess(ManagerProcess):
class DaemonProcess(ManagerProcess):
"""Python process that has to stay running accross manager restart.
"""Python process that has to stay running across manager restart.
This is used for athena so you don't lose SSH access when restarting manager."""
def __init__(self, name, module, param_name, enabled=True):
self.name = name
selfdrive/manager/process_config.py
+5 -1
View File
@@ -20,6 +20,7 @@ procs = [
NativeProcess("ui", "selfdrive/ui", ["./ui"], persistent=True, watchdog_max_dt=(5 if TICI else None)),
NativeProcess("soundd", "selfdrive/ui", ["./soundd"]),
NativeProcess("locationd", "selfdrive/locationd", ["./locationd"]),
NativeProcess("boardd", "selfdrive/boardd", ["./boardd"], enabled=False),
PythonProcess("calibrationd", "selfdrive.locationd.calibrationd"),
PythonProcess("controlsd", "selfdrive.controls.controlsd"),
PythonProcess("deleter", "selfdrive.loggerd.deleter", persistent=True),
@@ -29,12 +30,15 @@ procs = [
PythonProcess("paramsd", "selfdrive.locationd.paramsd"),
PythonProcess("plannerd", "selfdrive.controls.plannerd"),
PythonProcess("radard", "selfdrive.controls.radard"),
PythonProcess("rtshield", "selfdrive.rtshield", enabled=EON),
PythonProcess("thermald", "selfdrive.thermald.thermald", persistent=True),
PythonProcess("timezoned", "selfdrive.timezoned", enabled=TICI, persistent=True),
PythonProcess("tombstoned", "selfdrive.tombstoned", enabled=not PC, persistent=True),
PythonProcess("updated", "selfdrive.updated", enabled=not PC, persistent=True),
PythonProcess("uploader", "selfdrive.loggerd.uploader", persistent=True),
# EON only
PythonProcess("rtshield", "selfdrive.rtshield", enabled=EON),
PythonProcess("androidd", "selfdrive.hardware.eon.androidd", enabled=EON, persistent=True),
]
managed_processes = {p.name: p for p in procs}
selfdrive/modeld/dmonitoringmodeld
+13 -11
View File
@@ -1,15 +1,17 @@
#!/bin/sh
if [ -d /system ]; then
if [ -f /TICI ]; then # QCOM2
export LD_LIBRARY_PATH="/usr/lib/aarch64-linux-gnu:/data/pythonpath/phonelibs/snpe/larch64:$LD_LIBRARY_PATH"
else # QCOM
export LD_LIBRARY_PATH="/data/pythonpath/phonelibs/snpe/aarch64/:$LD_LIBRARY_PATH"
fi
export ADSP_LIBRARY_PATH="/data/pythonpath/phonelibs/snpe/dsp/"
else
# PC
export LD_LIBRARY_PATH="$HOME/openpilot/phonelibs/snpe/x86_64-linux-clang:/openpilot/phonelibs/snpe/x86_64:$HOME/openpilot/phonelibs/snpe/x86_64:$LD_LIBRARY_PATH"
fi
DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" >/dev/null && pwd)"
cd $DIR
if [ -d /system ]; then
if [ -f /TICI ]; then # QCOM2
export LD_LIBRARY_PATH="/usr/lib/aarch64-linux-gnu:/data/pythonpath/phonelibs/snpe/larch64:$LD_LIBRARY_PATH"
else # QCOM
export LD_LIBRARY_PATH="/data/pythonpath/phonelibs/snpe/aarch64/:$LD_LIBRARY_PATH"
fi
export ADSP_LIBRARY_PATH="/data/pythonpath/phonelibs/snpe/dsp/"
else
# PC
export LD_LIBRARY_PATH="$DIR/../../phonelibs/snpe/x86_64-linux-clang:$DIR/../../openpilot/phonelibs/snpe/x86_64:$LD_LIBRARY_PATH"
fi
exec ./_dmonitoringmodeld
selfdrive/modeld/modeld
+1
View File
@@ -1,6 +1,7 @@
#!/bin/sh
DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" >/dev/null && pwd)"
cd $DIR
if [ -d /system ]; then
if [ -f /TICI ]; then # QCOM2
selfdrive/modeld/models/driving.cc
+34 -12
View File
@@ -24,7 +24,9 @@ constexpr int PLAN_MHP_SELECTION = 1;
constexpr int PLAN_MHP_GROUP_SIZE = (2*PLAN_MHP_VALS + PLAN_MHP_SELECTION);
constexpr int LEAD_MHP_N = 5;
constexpr int LEAD_MHP_VALS = 4;
constexpr int LEAD_TRAJ_LEN = 6;
constexpr int LEAD_PRED_DIM = 4;
constexpr int LEAD_MHP_VALS = LEAD_PRED_DIM*LEAD_TRAJ_LEN;
constexpr int LEAD_MHP_SELECTION = 3;
constexpr int LEAD_MHP_GROUP_SIZE = (2*LEAD_MHP_VALS + LEAD_MHP_SELECTION);
@@ -147,18 +149,38 @@ void fill_sigmoid(const float *input, float *output, int len, int stride) {
}
}
void fill_lead_v2(cereal::ModelDataV2::LeadDataV2::Builder lead, const float *lead_data, const float *prob, int t_offset, float t) {
void fill_lead_v3(cereal::ModelDataV2::LeadDataV3::Builder lead, const float *lead_data, const float *prob, int t_offset, float prob_t) {
float t[LEAD_TRAJ_LEN] = {0.0, 2.0, 4.0, 6.0, 8.0, 10.0};
const float *data = get_lead_data(lead_data, t_offset);
lead.setProb(sigmoid(prob[t_offset]));
lead.setT(t);
float xyva_arr[LEAD_MHP_VALS];
float xyva_stds_arr[LEAD_MHP_VALS];
for (int i=0; i<LEAD_MHP_VALS; i++) {
xyva_arr[i] = data[i];
xyva_stds_arr[i] = exp(data[LEAD_MHP_VALS + i]);
lead.setProbTime(prob_t);
float x_arr[LEAD_TRAJ_LEN];
float y_arr[LEAD_TRAJ_LEN];
float v_arr[LEAD_TRAJ_LEN];
float a_arr[LEAD_TRAJ_LEN];
float x_stds_arr[LEAD_TRAJ_LEN];
float y_stds_arr[LEAD_TRAJ_LEN];
float v_stds_arr[LEAD_TRAJ_LEN];
float a_stds_arr[LEAD_TRAJ_LEN];
for (int i=0; i<LEAD_TRAJ_LEN; i++) {
x_arr[i] = data[i*LEAD_PRED_DIM+0];
y_arr[i] = data[i*LEAD_PRED_DIM+1];
v_arr[i] = data[i*LEAD_PRED_DIM+2];
a_arr[i] = data[i*LEAD_PRED_DIM+3];
x_stds_arr[i] = exp(data[LEAD_MHP_VALS + i*LEAD_PRED_DIM+0]);
y_stds_arr[i] = exp(data[LEAD_MHP_VALS + i*LEAD_PRED_DIM+1]);
v_stds_arr[i] = exp(data[LEAD_MHP_VALS + i*LEAD_PRED_DIM+2]);
a_stds_arr[i] = exp(data[LEAD_MHP_VALS + i*LEAD_PRED_DIM+3]);
}
lead.setXyva(xyva_arr);
lead.setXyvaStd(xyva_stds_arr);
lead.setT(t);
lead.setX(x_arr);
lead.setY(y_arr);
lead.setV(v_arr);
lead.setA(a_arr);
lead.setXStd(x_stds_arr);
lead.setYStd(y_stds_arr);
lead.setVStd(v_stds_arr);
lead.setAStd(a_stds_arr);
}
void fill_meta(cereal::ModelDataV2::MetaData::Builder meta, const float *meta_data) {
@@ -303,10 +325,10 @@ void fill_model(cereal::ModelDataV2::Builder &framed, const ModelDataRaw &net_ou
fill_meta(framed.initMeta(), net_outputs.meta);
// leads
auto leads = framed.initLeads(LEAD_MHP_SELECTION);
auto leads = framed.initLeadsV3(LEAD_MHP_SELECTION);
float t_offsets[LEAD_MHP_SELECTION] = {0.0, 2.0, 4.0};
for (int t_offset=0; t_offset<LEAD_MHP_SELECTION; t_offset++) {
fill_lead_v2(leads[t_offset], net_outputs.lead, net_outputs.lead_prob, t_offset, t_offsets[t_offset]);
fill_lead_v3(leads[t_offset], net_outputs.lead, net_outputs.lead_prob, t_offset, t_offsets[t_offset]);
}
}
selfdrive/modeld/thneed/serialize.cc
+1 -1
View File
@@ -238,7 +238,7 @@ void Thneed::save(const char *filename, bool save_binaries) {
if (mobj["arg_type"] == "image2d_t" || mobj["arg_type"] == "image1d_t") {
assert(false);
} else {
// buffers alloced with CL_MEM_HOST_WRITE_ONLY, hence this hack
// buffers allocated with CL_MEM_HOST_WRITE_ONLY, hence this hack
//hexdump((uint32_t*)val, 0x100);
// the worst hack in thneed, the flags are at 0x14
selfdrive/monitoring/driver_monitor.py
+10 -10
View File
@@ -26,20 +26,20 @@ class DRIVER_MONITOR_SETTINGS():
self._DISTRACTED_PROMPT_TIME_TILL_TERMINAL = 6.
self._FACE_THRESHOLD = 0.5
self._PARTIAL_FACE_THRESHOLD = 0.75 if TICI else 0.5
self._EYE_THRESHOLD = 0.5
self._SG_THRESHOLD = 0.5
self._BLINK_THRESHOLD = 0.88 if TICI else 0.5
self._BLINK_THRESHOLD_SLACK = 0.98 if TICI else 0.65
self._BLINK_THRESHOLD_STRICT = 0.88 if TICI else 0.5
self._PARTIAL_FACE_THRESHOLD = 0.765 if TICI else 0.455
self._EYE_THRESHOLD = 0.25 if TICI else 0.57
self._SG_THRESHOLD = 0.83
self._BLINK_THRESHOLD = 0.62 if TICI else 0.68
self._BLINK_THRESHOLD_SLACK = 0.82 if TICI else 0.88
self._BLINK_THRESHOLD_STRICT = 0.62 if TICI else 0.68
self._PITCH_WEIGHT = 1.175 if TICI else 1.35 # pitch matters a lot more
self._POSESTD_THRESHOLD = 0.318 if TICI else 0.14
self._POSESTD_THRESHOLD = 0.2 if TICI else 0.175
self._E2E_POSE_THRESHOLD = 0.95 if TICI else 0.9
self._E2E_EYES_THRESHOLD = 0.75
self._METRIC_THRESHOLD = 0.5 if TICI else 0.4
self._METRIC_THRESHOLD_SLACK = 0.6875 if TICI else 0.55
self._METRIC_THRESHOLD_STRICT = 0.5 if TICI else 0.4
self._METRIC_THRESHOLD = 0.55 if TICI else 0.48
self._METRIC_THRESHOLD_SLACK = 0.75 if TICI else 0.66
self._METRIC_THRESHOLD_STRICT = 0.55 if TICI else 0.48
self._PITCH_POS_ALLOWANCE = 0.12 # rad, to not be too sensitive on positive pitch
self._PITCH_NATURAL_OFFSET = 0.02 # people don't seem to look straight when they drive relaxed, rather a bit up
self._YAW_NATURAL_OFFSET = 0.08 # people don't seem to look straight when they drive relaxed, rather a bit to the right (center of car)
selfdrive/pandad.py
+2
View File
@@ -5,6 +5,7 @@ import time
from panda import BASEDIR as PANDA_BASEDIR, Panda, PandaDFU
from common.basedir import BASEDIR
from common.params import Params
from selfdrive.swaglog import cloudlog
PANDA_FW_FN = os.path.join(PANDA_BASEDIR, "board", "obj", "panda.bin.signed")
@@ -86,6 +87,7 @@ def main() -> None:
# check health for lost heartbeat
health = panda.health()
if health["heartbeat_lost"]:
Params().put_bool("PandaHeartbeatLost", True)
cloudlog.event("heartbeat lost", deviceState=health)
cloudlog.info("Resetting panda")
selfdrive/proclogd/SConscript
+5 -1
View File
@@ -1,2 +1,6 @@
Import('env', 'cereal', 'messaging', 'common')
env.Program('proclogd.cc', LIBS=[cereal, messaging, 'pthread', 'zmq', 'capnp', 'kj', 'common'])
libs = [cereal, messaging, 'pthread', 'zmq', 'capnp', 'kj', 'common', 'zmq', 'json11']
env.Program('proclogd', ['main.cc', 'proclog.cc'], LIBS=libs)
if GetOption('test'):
env.Program('tests/test_proclog', ['tests/test_proclog.cc', 'proclog.cc'], LIBS=libs)
selfdrive/proclogd/main.cc
+22
View File
@@ -0,0 +1,22 @@
#include <sys/resource.h>
#include "selfdrive/common/util.h"
#include "selfdrive/proclogd/proclog.h"
ExitHandler do_exit;
int main(int argc, char **argv) {
setpriority(PRIO_PROCESS, 0, -15);
PubMaster publisher({"procLog"});
while (!do_exit) {
MessageBuilder msg;
buildProcLogMessage(msg);
publisher.send("procLog", msg);
util::sleep_for(2000); // 2 secs
}
return 0;
}
selfdrive/proclogd/proclog.cc
+239
View File
@@ -0,0 +1,239 @@
#include "selfdrive/proclogd/proclog.h"
#include <dirent.h>
#include <cassert>
#include <fstream>
#include <iterator>
#include <sstream>
#include "selfdrive/common/swaglog.h"
#include "selfdrive/common/util.h"
namespace Parser {
// parse /proc/stat
std::vector<CPUTime> cpuTimes(std::istream &stream) {
std::vector<CPUTime> cpu_times;
std::string line;
// skip the first line for cpu total
std::getline(stream, line);
while (std::getline(stream, line)) {
if (line.compare(0, 3, "cpu") != 0) break;
CPUTime t = {};
std::istringstream iss(line);
if (iss.ignore(3) >> t.id >> t.utime >> t.ntime >> t.stime >> t.itime >> t.iowtime >> t.irqtime >> t.sirqtime)
cpu_times.push_back(t);
}
return cpu_times;
}
// parse /proc/meminfo
std::unordered_map<std::string, uint64_t> memInfo(std::istream &stream) {
std::unordered_map<std::string, uint64_t> mem_info;
std::string line, key;
while (std::getline(stream, line)) {
uint64_t val = 0;
std::istringstream iss(line);
if (iss >> key >> val) {
mem_info[key] = val * 1024;
}
}
return mem_info;
}
// field position (https://man7.org/linux/man-pages/man5/proc.5.html)
enum StatPos {
pid = 1,
state = 3,
ppid = 4,
utime = 14,
stime = 15,
cutime = 16,
cstime = 17,
priority = 18,
nice = 19,
num_threads = 20,
starttime = 22,
vsize = 23,
rss = 24,
processor = 39,
MAX_FIELD = 52,
};
// parse /proc/pid/stat
std::optional<ProcStat> procStat(std::string stat) {
// To avoid being fooled by names containing a closing paren, scan backwards.
auto open_paren = stat.find('(');
auto close_paren = stat.rfind(')');
if (open_paren == std::string::npos || close_paren == std::string::npos || open_paren > close_paren) {
return std::nullopt;
}
std::string name = stat.substr(open_paren + 1, close_paren - open_paren - 1);
// repace space in name with _
std::replace(&stat[open_paren], &stat[close_paren], ' ', '_');
std::istringstream iss(stat);
std::vector<std::string> v{std::istream_iterator<std::string>(iss),
std::istream_iterator<std::string>()};
try {
if (v.size() != StatPos::MAX_FIELD) {
throw std::invalid_argument("stat");
}
ProcStat p = {
.name = name,
.pid = stoi(v[StatPos::pid - 1]),
.state = v[StatPos::state - 1][0],
.ppid = stoi(v[StatPos::ppid - 1]),
.utime = stoul(v[StatPos::utime - 1]),
.stime = stoul(v[StatPos::stime - 1]),
.cutime = stol(v[StatPos::cutime - 1]),
.cstime = stol(v[StatPos::cstime - 1]),
.priority = stol(v[StatPos::priority - 1]),
.nice = stol(v[StatPos::nice - 1]),
.num_threads = stol(v[StatPos::num_threads - 1]),
.starttime = stoull(v[StatPos::starttime - 1]),
.vms = stoul(v[StatPos::vsize - 1]),
.rss = stoul(v[StatPos::rss - 1]),
.processor = stoi(v[StatPos::processor - 1]),
};
return p;
} catch (const std::invalid_argument &e) {
LOGE("failed to parse procStat (%s) :%s", e.what(), stat.c_str());
} catch (const std::out_of_range &e) {
LOGE("failed to parse procStat (%s) :%s", e.what(), stat.c_str());
}
return std::nullopt;
}
// return list of PIDs from /proc
std::vector<int> pids() {
std::vector<int> ids;
DIR *d = opendir("/proc");
assert(d);
char *p_end;
struct dirent *de = NULL;
while ((de = readdir(d))) {
if (de->d_type == DT_DIR) {
int pid = strtol(de->d_name, &p_end, 10);
if (p_end == (de->d_name + strlen(de->d_name))) {
ids.push_back(pid);
}
}
}
closedir(d);
return ids;
}
// null-delimited cmdline arguments to vector
std::vector<std::string> cmdline(std::istream &stream) {
std::vector<std::string> ret;
std::string line;
while (std::getline(stream, line, '\0')) {
if (!line.empty()) {
ret.push_back(line);
}
}
return ret;
}
const ProcCache &getProcExtraInfo(int pid, const std::string &name) {
static std::unordered_map<pid_t, ProcCache> proc_cache;
ProcCache &cache = proc_cache[pid];
if (cache.pid != pid || cache.name != name) {
cache.pid = pid;
cache.name = name;
std::string proc_path = "/proc/" + std::to_string(pid);
cache.exe = util::readlink(proc_path + "/exe");
std::ifstream stream(proc_path + "/cmdline");
cache.cmdline = cmdline(stream);
}
return cache;
}
} // namespace Parser
const double jiffy = sysconf(_SC_CLK_TCK);
const size_t page_size = sysconf(_SC_PAGE_SIZE);
void buildCPUTimes(cereal::ProcLog::Builder &builder) {
std::ifstream stream("/proc/stat");
std::vector<CPUTime> stats = Parser::cpuTimes(stream);
auto log_cpu_times = builder.initCpuTimes(stats.size());
for (int i = 0; i < stats.size(); ++i) {
auto l = log_cpu_times[i];
const CPUTime &r = stats[i];
l.setCpuNum(r.id);
l.setUser(r.utime / jiffy);
l.setNice(r.ntime / jiffy);
l.setSystem(r.stime / jiffy);
l.setIdle(r.itime / jiffy);
l.setIowait(r.iowtime / jiffy);
l.setIrq(r.irqtime / jiffy);
l.setSoftirq(r.sirqtime / jiffy);
}
}
void buildMemInfo(cereal::ProcLog::Builder &builder) {
std::ifstream stream("/proc/meminfo");
auto mem_info = Parser::memInfo(stream);
auto mem = builder.initMem();
mem.setTotal(mem_info["MemTotal:"]);
mem.setFree(mem_info["MemFree:"]);
mem.setAvailable(mem_info["MemAvailable:"]);
mem.setBuffers(mem_info["Buffers:"]);
mem.setCached(mem_info["Cached:"]);
mem.setActive(mem_info["Active:"]);
mem.setInactive(mem_info["Inactive:"]);
mem.setShared(mem_info["Shmem:"]);
}
void buildProcs(cereal::ProcLog::Builder &builder) {
auto pids = Parser::pids();
std::vector<ProcStat> proc_stats;
proc_stats.reserve(pids.size());
for (int pid : pids) {
std::string path = "/proc/" + std::to_string(pid) + "/stat";
if (auto stat = Parser::procStat(util::read_file(path))) {
proc_stats.push_back(*stat);
}
}
auto procs = builder.initProcs(proc_stats.size());
for (size_t i = 0; i < proc_stats.size(); i++) {
auto l = procs[i];
const ProcStat &r = proc_stats[i];
l.setPid(r.pid);
l.setState(r.state);
l.setPpid(r.ppid);
l.setCpuUser(r.utime / jiffy);
l.setCpuSystem(r.stime / jiffy);
l.setCpuChildrenUser(r.cutime / jiffy);
l.setCpuChildrenSystem(r.cstime / jiffy);
l.setPriority(r.priority);
l.setNice(r.nice);
l.setNumThreads(r.num_threads);
l.setStartTime(r.starttime / jiffy);
l.setMemVms(r.vms);
l.setMemRss((uint64_t)r.rss * page_size);
l.setProcessor(r.processor);
l.setName(r.name);
const ProcCache &extra_info = Parser::getProcExtraInfo(r.pid, r.name);
l.setExe(extra_info.exe);
auto lcmdline = l.initCmdline(extra_info.cmdline.size());
for (size_t i = 0; i < lcmdline.size(); i++) {
lcmdline.set(i, extra_info.cmdline[i]);
}
}
}
void buildProcLogMessage(MessageBuilder &msg) {
auto procLog = msg.initEvent().initProcLog();
buildProcs(procLog);
buildCPUTimes(procLog);
buildMemInfo(procLog);
}
selfdrive/proclogd/proclog.h
+40
View File
@@ -0,0 +1,40 @@
#include <optional>
#include <string>
#include <unordered_map>
#include <vector>
#include "cereal/messaging/messaging.h"
struct CPUTime {
int id;
unsigned long utime, ntime, stime, itime;
unsigned long iowtime, irqtime, sirqtime;
};
struct ProcCache {
int pid;
std::string name, exe;
std::vector<std::string> cmdline;
};
struct ProcStat {
int pid, ppid, processor;
char state;
long cutime, cstime, priority, nice, num_threads;
unsigned long utime, stime, vms, rss;
unsigned long long starttime;
std::string name;
};
namespace Parser {
std::vector<int> pids();
std::optional<ProcStat> procStat(std::string stat);
std::vector<std::string> cmdline(std::istream &stream);
std::vector<CPUTime> cpuTimes(std::istream &stream);
std::unordered_map<std::string, uint64_t> memInfo(std::istream &stream);
const ProcCache &getProcExtraInfo(int pid, const std::string &name);
}; // namespace Parser
void buildProcLogMessage(MessageBuilder &msg);
selfdrive/proclogd/proclogd.cc
-233
View File
@@ -1,233 +0,0 @@
#include <dirent.h>
#include <sys/resource.h>
#include <sys/time.h>
#include <algorithm>
#include <cassert>
#include <climits>
#include <cstdio>
#include <cstdlib>
#include <fstream>
#include <functional>
#include <memory>
#include <sstream>
#include <unordered_map>
#include <utility>
#include "cereal/messaging/messaging.h"
#include "selfdrive/common/timing.h"
#include "selfdrive/common/util.h"
ExitHandler do_exit;
namespace {
struct ProcCache {
std::string name;
std::vector<std::string> cmdline;
std::string exe;
};
}
int main() {
setpriority(PRIO_PROCESS, 0, -15);
PubMaster publisher({"procLog"});
double jiffy = sysconf(_SC_CLK_TCK);
size_t page_size = sysconf(_SC_PAGE_SIZE);
std::unordered_map<pid_t, ProcCache> proc_cache;
while (!do_exit) {
MessageBuilder msg;
auto procLog = msg.initEvent().initProcLog();
auto orphanage = msg.getOrphanage();
// stat
{
std::vector<capnp::Orphan<cereal::ProcLog::CPUTimes>> otimes;
std::ifstream sstat("/proc/stat");
std::string stat_line;
while (std::getline(sstat, stat_line)) {
if (util::starts_with(stat_line, "cpu ")) {
// cpu total
} else if (util::starts_with(stat_line, "cpu")) {
// specific cpu
int id;
unsigned long utime, ntime, stime, itime;
unsigned long iowtime, irqtime, sirqtime;
sscanf(stat_line.data(), "cpu%d %lu %lu %lu %lu %lu %lu %lu",
&id, &utime, &ntime, &stime, &itime, &iowtime, &irqtime, &sirqtime);
auto ltimeo = orphanage.newOrphan<cereal::ProcLog::CPUTimes>();
auto ltime = ltimeo.get();
ltime.setCpuNum(id);
ltime.setUser(utime / jiffy);
ltime.setNice(ntime / jiffy);
ltime.setSystem(stime / jiffy);
ltime.setIdle(itime / jiffy);
ltime.setIowait(iowtime / jiffy);
ltime.setIrq(irqtime / jiffy);
ltime.setSoftirq(irqtime / jiffy);
otimes.push_back(std::move(ltimeo));
} else {
break;
}
}
auto ltimes = procLog.initCpuTimes(otimes.size());
for (size_t i = 0; i < otimes.size(); i++) {
ltimes.adoptWithCaveats(i, std::move(otimes[i]));
}
}
// meminfo
{
auto mem = procLog.initMem();
std::ifstream smem("/proc/meminfo");
std::string mem_line;
uint64_t mem_total = 0, mem_free = 0, mem_available = 0, mem_buffers = 0;
uint64_t mem_cached = 0, mem_active = 0, mem_inactive = 0, mem_shared = 0;
while (std::getline(smem, mem_line)) {
if (util::starts_with(mem_line, "MemTotal:")) sscanf(mem_line.data(), "MemTotal: %" SCNu64 " kB", &mem_total);
else if (util::starts_with(mem_line, "MemFree:")) sscanf(mem_line.data(), "MemFree: %" SCNu64 " kB", &mem_free);
else if (util::starts_with(mem_line, "MemAvailable:")) sscanf(mem_line.data(), "MemAvailable: %" SCNu64 " kB", &mem_available);
else if (util::starts_with(mem_line, "Buffers:")) sscanf(mem_line.data(), "Buffers: %" SCNu64 " kB", &mem_buffers);
else if (util::starts_with(mem_line, "Cached:")) sscanf(mem_line.data(), "Cached: %" SCNu64 " kB", &mem_cached);
else if (util::starts_with(mem_line, "Active:")) sscanf(mem_line.data(), "Active: %" SCNu64 " kB", &mem_active);
else if (util::starts_with(mem_line, "Inactive:")) sscanf(mem_line.data(), "Inactive: %" SCNu64 " kB", &mem_inactive);
else if (util::starts_with(mem_line, "Shmem:")) sscanf(mem_line.data(), "Shmem: %" SCNu64 " kB", &mem_shared);
}
mem.setTotal(mem_total * 1024);
mem.setFree(mem_free * 1024);
mem.setAvailable(mem_available * 1024);
mem.setBuffers(mem_buffers * 1024);
mem.setCached(mem_cached * 1024);
mem.setActive(mem_active * 1024);
mem.setInactive(mem_inactive * 1024);
mem.setShared(mem_shared * 1024);
}
// processes
{
std::vector<capnp::Orphan<cereal::ProcLog::Process>> oprocs;
struct dirent *de = NULL;
DIR *d = opendir("/proc");
assert(d);
while ((de = readdir(d))) {
if (!isdigit(de->d_name[0])) continue;
pid_t pid = atoi(de->d_name);
auto lproco = orphanage.newOrphan<cereal::ProcLog::Process>();
auto lproc = lproco.get();
lproc.setPid(pid);
char tcomm[PATH_MAX] = {0};
{
std::string stat = util::read_file(util::string_format("/proc/%d/stat", pid));
char state;
int ppid;
unsigned long utime, stime;
long cutime, cstime, priority, nice, num_threads;
unsigned long long starttime;
unsigned long vms, rss;
int processor;
int count = sscanf(stat.data(),
"%*d (%1024[^)]) %c %d %*d %*d %*d %*d %*d %*d %*d %*d %*d "
"%lu %lu %ld %ld %ld %ld %ld %*d %lld "
"%lu %lu %*d %*d %*d %*d %*d %*d %*d "
"%*d %*d %*d %*d %*d %*d %*d %d",
tcomm, &state, &ppid,
&utime, &stime, &cutime, &cstime, &priority, &nice, &num_threads, &starttime,
&vms, &rss, &processor);
if (count != 14) continue;
lproc.setState(state);
lproc.setPpid(ppid);
lproc.setCpuUser(utime / jiffy);
lproc.setCpuSystem(stime / jiffy);
lproc.setCpuChildrenUser(cutime / jiffy);
lproc.setCpuChildrenSystem(cstime / jiffy);
lproc.setPriority(priority);
lproc.setNice(nice);
lproc.setNumThreads(num_threads);
lproc.setStartTime(starttime / jiffy);
lproc.setMemVms(vms);
lproc.setMemRss((uint64_t)rss * page_size);
lproc.setProcessor(processor);
}
std::string name(tcomm);
lproc.setName(name);
// populate other things from cache
auto cache_it = proc_cache.find(pid);
ProcCache cache;
if (cache_it != proc_cache.end()) {
cache = cache_it->second;
}
if (cache_it == proc_cache.end() || cache.name != name) {
cache = (ProcCache){
.name = name,
.exe = util::readlink(util::string_format("/proc/%d/exe", pid)),
};
// null-delimited cmdline arguments to vector
std::string cmdline_s = util::read_file(util::string_format("/proc/%d/cmdline", pid));
const char* cmdline_p = cmdline_s.c_str();
const char* cmdline_ep = cmdline_p + cmdline_s.size();
// strip trailing null bytes
while ((cmdline_ep-1) > cmdline_p && *(cmdline_ep-1) == 0) {
cmdline_ep--;
}
while (cmdline_p < cmdline_ep) {
std::string arg(cmdline_p);
cache.cmdline.push_back(arg);
cmdline_p += arg.size() + 1;
}
proc_cache[pid] = cache;
}
auto lcmdline = lproc.initCmdline(cache.cmdline.size());
for (size_t i = 0; i < lcmdline.size(); i++) {
lcmdline.set(i, cache.cmdline[i]);
}
lproc.setExe(cache.exe);
oprocs.push_back(std::move(lproco));
}
closedir(d);
auto lprocs = procLog.initProcs(oprocs.size());
for (size_t i = 0; i < oprocs.size(); i++) {
lprocs.adoptWithCaveats(i, std::move(oprocs[i]));
}
}
publisher.send("procLog", msg);
util::sleep_for(2000); // 2 secs
}
return 0;
}
selfdrive/sensord/sensord
+1
View File
@@ -1,3 +1,4 @@
#!/bin/sh
cd "$(dirname "$0")"
export LD_LIBRARY_PATH="/system/lib64:$LD_LIBRARY_PATH"
exec ./_sensord
selfdrive/sensord/sensors/bmx055_magn.cc
+1 -1
View File
@@ -245,6 +245,6 @@ void BMX055_Magn::get_event(cereal::SensorEventData::Builder &event) {
// The BMX055 Magnetometer has no FIFO mode. Self running mode only goes
// up to 30 Hz. Therefore we put in forced mode, and request measurements
// at a 100 Hz. When reading the registers we have to check the ready bit
// To verify the measurement was comleted this cycle.
// To verify the measurement was completed this cycle.
set_register(BMX055_MAGN_I2C_REG_MAG, BMX055_MAGN_FORCED);
}
selfdrive/sensord/sensors/mmc5603nj_magn.cc
+1 -1
View File
@@ -41,7 +41,7 @@ int MMC5603NJ_Magn::init() {
goto fail;
}
// Enable continous mode, set every 100 measurements
// Enable continuous mode, set every 100 measurements
ret = set_register(MMC5603NJ_I2C_REG_INTERNAL_2, MMC5603NJ_CMM_EN | MMC5603NJ_EN_PRD_SET | 0b11);
if (ret < 0) {
goto fail;
selfdrive/test/test_onroad.py
+13 -5
View File
@@ -13,7 +13,7 @@ from cereal.services import service_list
from common.basedir import BASEDIR
from common.timeout import Timeout
from common.params import Params
from selfdrive.hardware import TICI
from selfdrive.hardware import EON, TICI
from selfdrive.loggerd.config import ROOT
from selfdrive.test.helpers import set_params_enabled
from tools.lib.logreader import LogReader
@@ -44,11 +44,16 @@ PROCS = {
"./logcatd": 0,
}
if EON:
PROCS.update({
"selfdrive.hardware.eon.androidd": 0.4,
})
if TICI:
PROCS.update({
"./loggerd": 60.0,
"selfdrive.controls.controlsd": 26.0,
"./camerad": 25.0,
"selfdrive.controls.controlsd": 28.0,
"./camerad": 31.0,
"./_ui": 21.0,
"selfdrive.controls.plannerd": 12.0,
"selfdrive.locationd.paramsd": 5.0,
@@ -75,7 +80,10 @@ def check_cpu_usage(first_proc, last_proc):
last = [p for p in last_proc.procLog.procs if proc_name in p.cmdline][0]
cpu_time = cputime_total(last) - cputime_total(first)
cpu_usage = cpu_time / dt * 100.
if cpu_usage > max(normal_cpu_usage * 1.1, normal_cpu_usage + 5.0):
if cpu_usage > max(normal_cpu_usage * 1.15, normal_cpu_usage + 5.0):
# cpu usage is high while playing sounds
if proc_name == "./_soundd" and cpu_usage < 25.:
continue
result += f"Warning {proc_name} using more CPU than normal\n"
r = False
elif cpu_usage < min(normal_cpu_usage * 0.65, max(normal_cpu_usage - 1.0, 0.0)):
@@ -154,7 +162,7 @@ class TestOnroad(unittest.TestCase):
def test_model_timings(self):
#TODO this went up when plannerd cpu usage increased, why?
cfgs = [("modelV2", 0.035, 0.03), ("driverState", 0.025, 0.021)]
cfgs = [("modelV2", 0.038, 0.036), ("driverState", 0.028, 0.026)]
for (s, instant_max, avg_max) in cfgs:
ts = [getattr(getattr(m, s), "modelExecutionTime") for m in self.lr if m.which() == s]
self.assertLess(min(ts), instant_max, f"high '{s}' execution time: {min(ts)}")
selfdrive/thermald/thermald.py
+1 -1
View File
@@ -259,7 +259,7 @@ def thermald_thread():
msg.deviceState.freeSpacePercent = get_available_percent(default=100.0)
msg.deviceState.memoryUsagePercent = int(round(psutil.virtual_memory().percent))
msg.deviceState.cpuUsagePercent = int(round(psutil.cpu_percent()))
msg.deviceState.cpuUsagePercent = [int(round(n)) for n in psutil.cpu_percent(percpu=True)]
msg.deviceState.gpuUsagePercent = int(round(HARDWARE.get_gpu_usage_percent()))
msg.deviceState.networkType = network_type
msg.deviceState.networkStrength = network_strength
selfdrive/ui/SConscript
+11 -5
View File
@@ -63,7 +63,6 @@ if arch != 'aarch64' and GetOption('setup'):
asset_obj = qt_env.Object("assets", assets)
qt_env.Program("qt/setup/reset", ["qt/setup/reset.cc"], LIBS=qt_libs)
qt_env.Program("qt/setup/wifi", ["qt/setup/wifi.cc"], LIBS=qt_libs + ['common', 'json11'])
qt_env.Program("qt/setup/updater", ["qt/setup/updater.cc", asset_obj], LIBS=qt_libs)
qt_env.Program("qt/setup/setup", ["qt/setup/setup.cc", asset_obj],
LIBS=qt_libs + ['curl', 'common', 'json11'])
@@ -71,14 +70,21 @@ if arch != 'aarch64' and GetOption('setup'):
senv = qt_env.Clone()
senv['LINKFLAGS'].append('-Wl,-strip-debug')
installers = [
("openpilot", "release3-staging"),
("openpilot", "release3"),
("openpilot_test", "release3-staging"),
("openpilot_nightly", "master-ci"),
("openpilot_internal", "master"),
("dashcam", "dashcam3-staging"),
("dashcam", "dashcam3"),
("dashcam_test", "dashcam3-staging"),
]
cont = {}
for brand in ("openpilot", "dashcam"):
cont[brand] = senv.Command(f"qt/setup/continue_{brand}.o", f"#installer/continue_{brand}.sh",
"ld -r -b binary -o $TARGET $SOURCE")
for name, branch in installers:
d = {'BRANCH': f"'\"{branch}\"'"}
brand = "dashcam" if "dashcam" in branch else "openpilot"
d = {'BRANCH': f"'\"{branch}\"'", 'BRAND': f"'\"{brand}\"'"}
if "internal" in name:
d['INTERNAL'] = "1"
@@ -87,7 +93,7 @@ if arch != 'aarch64' and GetOption('setup'):
r.raise_for_status()
d['SSH_KEYS'] = f'\\"{r.text.strip()}\\"'
obj = senv.Object(f"qt/setup/installer_{name}.o", ["qt/setup/installer.cc"], CPPDEFINES=d)
f = senv.Program(f"qt/setup/installer_{name}", obj, LIBS=qt_libs)
f = senv.Program(f"qt/setup/installer_{name}", [obj, cont[brand]], LIBS=qt_libs)
# keep installers small
assert f[0].get_size() < 300*1e3
selfdrive/ui/paint.cc
+7 -8
View File
@@ -67,15 +67,15 @@ static void ui_draw_circle_image(const UIState *s, int center_x, int center_y, i
ui_draw_circle_image(s, center_x, center_y, radius, image, nvgRGBA(0, 0, 0, (255 * bg_alpha)), img_alpha);
}
static void draw_lead(UIState *s, const cereal::RadarState::LeadData::Reader &lead_data, const vertex_data &vd) {
static void draw_lead(UIState *s, const cereal::ModelDataV2::LeadDataV3::Reader &lead_data, const vertex_data &vd) {
// Draw lead car indicator
auto [x, y] = vd;
float fillAlpha = 0;
float speedBuff = 10.;
float leadBuff = 40.;
float d_rel = lead_data.getDRel();
float v_rel = lead_data.getVRel();
float d_rel = lead_data.getX()[0];
float v_rel = lead_data.getV()[0];
if (d_rel < leadBuff) {
fillAlpha = 255*(1.0-(d_rel/leadBuff));
if (v_rel < 0) {
@@ -167,13 +167,12 @@ static void ui_draw_world(UIState *s) {
// Draw lead indicators if openpilot is handling longitudinal
if (s->scene.longitudinal_control) {
auto radar_state = (*s->sm)["radarState"].getRadarState();
auto lead_one = radar_state.getLeadOne();
auto lead_two = radar_state.getLeadTwo();
if (lead_one.getStatus()) {
auto lead_one = (*s->sm)["modelV2"].getModelV2().getLeadsV3()[0];
auto lead_two = (*s->sm)["modelV2"].getModelV2().getLeadsV3()[1];
if (lead_one.getProb() > .5) {
draw_lead(s, lead_one, s->scene.lead_vertices[0]);
}
if (lead_two.getStatus() && (std::abs(lead_one.getDRel() - lead_two.getDRel()) > 3.0)) {
if (lead_two.getProb() > .5 && (std::abs(lead_one.getX()[0] - lead_two.getX()[0]) > 3.0)) {
draw_lead(s, lead_two, s->scene.lead_vertices[1]);
}
}
selfdrive/ui/qt/api.cc
+4 -8
View File
@@ -14,15 +14,12 @@
#include "selfdrive/common/params.h"
#include "selfdrive/common/util.h"
#include "selfdrive/hardware/hw.h"
#include "selfdrive/ui/qt/util.h"
namespace CommaApi {
const std::string private_key_path =
Hardware::PC() ? util::getenv_default("HOME", "/.comma/persist/comma/id_rsa", "/persist/comma/id_rsa")
: "/persist/comma/id_rsa";
QByteArray rsa_sign(const QByteArray &data) {
auto file = QFile(private_key_path.c_str());
auto file = QFile(Path::rsa_file().c_str());
if (!file.open(QIODevice::ReadOnly)) {
qDebug() << "No RSA private key found, please run manager.py or registration.py";
return QByteArray();
@@ -48,9 +45,8 @@ QByteArray rsa_sign(const QByteArray &data) {
QString create_jwt(const QJsonObject &payloads, int expiry) {
QJsonObject header = {{"alg", "RS256"}};
QString dongle_id = QString::fromStdString(Params().get("DongleId"));
auto t = QDateTime::currentSecsSinceEpoch();
QJsonObject payload = {{"identity", dongle_id}, {"nbf", t}, {"iat", t}, {"exp", t + expiry}};
QJsonObject payload = {{"identity", getDongleId().value_or("")}, {"nbf", t}, {"iat", t}, {"exp", t + expiry}};
for (auto it = payloads.begin(); it != payloads.end(); ++it) {
payload.insert(it.key(), it.value());
}
@@ -89,7 +85,7 @@ void HttpRequest::sendRequest(const QString &requestURL, const HttpRequest::Meth
if(create_jwt) {
token = CommaApi::create_jwt();
} else {
QString token_json = QString::fromStdString(util::read_file(util::getenv_default("HOME", "/.comma/auth.json", "/.comma/auth.json")));
QString token_json = QString::fromStdString(util::read_file(util::getenv("HOME") + "/.comma/auth.json"));
QJsonDocument json_d = QJsonDocument::fromJson(token_json.toUtf8());
token = json_d["access_token"].toString();
}
selfdrive/ui/qt/api.h
+3
View File
@@ -5,8 +5,11 @@
#include <QString>
#include <QTimer>
#include "selfdrive/common/util.h"
namespace CommaApi {
const QString BASE_URL = util::getenv("API_HOST", "https://api.commadotai.com").c_str();
QByteArray rsa_sign(const QByteArray &data);
QString create_jwt(const QJsonObject &payloads = {}, int expiry = 3600);
selfdrive/ui/qt/home.cc
+1 -1
View File
@@ -158,10 +158,10 @@ OffroadHome::OffroadHome(QWidget* parent) : QFrame(parent) {
font-size: 55px;
}
)");
refresh();
}
void OffroadHome::showEvent(QShowEvent *event) {
refresh();
timer->start(10 * 1000);
}
selfdrive/ui/qt/maps/map.cc
+2 -2
View File
@@ -129,7 +129,7 @@ void MapWindow::timerUpdate() {
if (localizer_valid) {
auto pos = location.getPositionGeodetic();
auto orientation = location.getOrientationNED();
auto orientation = location.getCalibratedOrientationNED();
float velocity = location.getVelocityCalibrated().getValue()[0];
float bearing = RAD2DEG(orientation.getValue()[2]);
@@ -198,7 +198,7 @@ void MapWindow::timerUpdate() {
}
// Transition to next route segment
if (distance_to_maneuver < -MANEUVER_TRANSITION_THRESHOLD) {
if (!shouldRecompute() && (distance_to_maneuver < -MANEUVER_TRANSITION_THRESHOLD)) {
auto next_segment = segment.nextRouteSegment();
if (next_segment.isValid()) {
segment = next_segment;
selfdrive/ui/qt/maps/map_helpers.cc
+1 -1
View File
@@ -17,7 +17,7 @@ QMapbox::CoordinatesCollections model_to_collection(
Eigen::Vector3d ecef(positionECEF.getValue()[0], positionECEF.getValue()[1], positionECEF.getValue()[2]);
Eigen::Vector3d orient(calibratedOrientationECEF.getValue()[0], calibratedOrientationECEF.getValue()[1], calibratedOrientationECEF.getValue()[2]);
Eigen::Matrix3d ecef_from_local = euler2rot(orient).transpose();
Eigen::Matrix3d ecef_from_local = euler2rot(orient);
QMapbox::Coordinates coordinates;
auto x = line.getX();
selfdrive/ui/qt/maps/map_settings.cc
+4 -5
View File
@@ -93,19 +93,18 @@ MapPanel::MapPanel(QWidget* parent) : QWidget(parent) {
clear();
std::string dongle_id = params.get("DongleId");
if (util::is_valid_dongle_id(dongle_id)) {
if (auto dongle_id = getDongleId()) {
// Fetch favorite and recent locations
{
std::string url = "https://api.commadotai.com/v1/navigation/" + dongle_id + "/locations";
RequestRepeater* repeater = new RequestRepeater(this, QString::fromStdString(url), "ApiCache_NavDestinations", 30, true);
QString url = CommaApi::BASE_URL + "/v1/navigation/" + *dongle_id + "/locations";
RequestRepeater* repeater = new RequestRepeater(this, url, "ApiCache_NavDestinations", 30, true);
QObject::connect(repeater, &RequestRepeater::receivedResponse, this, &MapPanel::parseResponse);
QObject::connect(repeater, &RequestRepeater::failedResponse, this, &MapPanel::failedResponse);
}
// Destination set while offline
{
QString url = QString::fromStdString("https://api.commadotai.com/v1/navigation/" + dongle_id + "/next");
QString url = CommaApi::BASE_URL + "/v1/navigation/" + *dongle_id + "/next";
RequestRepeater* repeater = new RequestRepeater(this, url, "", 10, true);
HttpRequest* deleter = new HttpRequest(this);
selfdrive/ui/qt/offroad/onboarding.h
+2 -2
View File
@@ -72,8 +72,8 @@ private:
QRect(303, 755, 718, 189),
};
const QString IMG_PATH = vwp_w == 2160 ? "../assets/training_wide/" : "../assets/training/";
const QVector<QRect> boundingRect = vwp_w == 2160 ? boundingRectWide : boundingRectStandard;
const QString IMG_PATH = WIDE_UI ? "../assets/training_wide/" : "../assets/training/";
const QVector<QRect> boundingRect = WIDE_UI ? boundingRectWide : boundingRectStandard;
signals:
void completedTraining();
selfdrive/ui/qt/offroad/settings.cc
+1 -3
View File
@@ -96,9 +96,7 @@ TogglesPanel::TogglesPanel(QWidget *parent) : QWidget(parent) {
DevicePanel::DevicePanel(QWidget* parent) : QWidget(parent) {
QVBoxLayout *main_layout = new QVBoxLayout(this);
Params params = Params();
QString dongle = QString::fromStdString(params.get("DongleId", false));
main_layout->addWidget(new LabelControl("Dongle ID", dongle));
main_layout->addWidget(new LabelControl("Dongle ID", getDongleId().value_or("N/A")));
main_layout->addWidget(horizontal_line());
QString serial = QString::fromStdString(params.get("HardwareSerial", false));
selfdrive/ui/qt/offroad/wifiManager.cc
+3 -3
View File
@@ -6,6 +6,7 @@
#include "selfdrive/common/params.h"
#include "selfdrive/common/swaglog.h"
#include "selfdrive/ui/qt/util.h"
template <typename T>
T get_response(QDBusMessage response) {
@@ -35,9 +36,8 @@ WifiManager::WifiManager(QWidget* parent) : QWidget(parent) {
// Set tethering ssid as "weedle" + first 4 characters of a dongle id
tethering_ssid = "weedle";
std::string bytes = Params().get("DongleId");
if (bytes.length() >= 4) {
tethering_ssid += "-" + QString::fromStdString(bytes.substr(0,4));
if (auto dongle_id = getDongleId()) {
tethering_ssid += "-" + dongle_id->left(4);
}
adapter = getAdapter();
selfdrive/ui/qt/qt_window.h
+3 -2
View File
@@ -19,11 +19,12 @@ const QString ASSET_PATH = ":/";
const QString ASSET_PATH = "../assets/";
#endif
const int vwp_w = (Hardware::TICI() || (getenv("WIDE_UI") != NULL)) ? 2160 : 1920;
const bool WIDE_UI = Hardware::TICI() || getenv("WIDE_UI") != nullptr;
const int vwp_w = WIDE_UI ? 2160 : 1920;
const int vwp_h = 1080;
inline void setMainWindow(QWidget *w) {
const float scale = getenv("SCALE") != NULL ? std::stof(getenv("SCALE")) : 1.0;
const float scale = util::getenv("SCALE", 1.0f);
w->setFixedSize(vwp_w*scale, vwp_h*scale);
w->show();
selfdrive/ui/qt/sidebar.cc
+19 -31
View File
@@ -2,9 +2,6 @@
#include <QMouseEvent>
#include "selfdrive/ui/qt/qt_window.h"
#include "selfdrive/common/util.h"
#include "selfdrive/hardware/hw.h"
#include "selfdrive/ui/qt/util.h"
void Sidebar::drawMetric(QPainter &p, const QString &label, const QString &val, QColor c, int y) {
@@ -41,9 +38,9 @@ Sidebar::Sidebar(QWidget *parent) : QFrame(parent) {
connect(this, &Sidebar::valueChanged, [=] { update(); });
setAttribute(Qt::WA_OpaquePaintEvent);
setSizePolicy(QSizePolicy::Fixed, QSizePolicy::Expanding);
setFixedWidth(300);
setMinimumHeight(vwp_h);
setStyleSheet("background-color: rgb(57, 57, 57);");
}
void Sidebar::mouseReleaseEvent(QMouseEvent *event) {
@@ -60,42 +57,31 @@ void Sidebar::updateState(const UIState &s) {
int strength = (int)deviceState.getNetworkStrength();
setProperty("netStrength", strength > 0 ? strength + 1 : 0);
ItemStatus connectstatus;
auto last_ping = deviceState.getLastAthenaPingTime();
if (last_ping == 0) {
if (params.getBool("PrimeRedirected")) {
setProperty("connectStr", "NO\nPRIME");
setProperty("connectStatus", danger_color);
} else {
setProperty("connectStr", "CONNECT\nOFFLINE");
setProperty("connectStatus", warning_color);
}
connectstatus = params.getBool("PrimeRedirected") ? ItemStatus{"NO\nPRIME", danger_color} : ItemStatus{"CONNECT\nOFFLINE", warning_color};
} else {
bool online = nanos_since_boot() - last_ping < 80e9;
setProperty("connectStr", (online ? "CONNECT\nONLINE" : "CONNECT\nERROR"));
setProperty("connectStatus", online ? good_color : danger_color);
connectstatus = nanos_since_boot() - last_ping < 80e9 ? ItemStatus{"CONNECT\nONLINE", good_color} : ItemStatus{"CONNECT\nERROR", danger_color};
}
setProperty("connectStatus", QVariant::fromValue(connectstatus));
QColor tempStatus = danger_color;
QColor tempColor = danger_color;
auto ts = deviceState.getThermalStatus();
if (ts == cereal::DeviceState::ThermalStatus::GREEN) {
tempStatus = good_color;
tempColor = good_color;
} else if (ts == cereal::DeviceState::ThermalStatus::YELLOW) {
tempStatus = warning_color;
tempColor = warning_color;
}
setProperty("tempStatus", tempStatus);
setProperty("tempVal", (int)deviceState.getAmbientTempC());
setProperty("tempStatus", QVariant::fromValue(ItemStatus{QString("%1°C").arg((int)deviceState.getAmbientTempC()), tempColor}));
QString pandaStr = "VEHICLE\nONLINE";
QColor pandaStatus = good_color;
ItemStatus pandaStatus = {"VEHICLE\nONLINE", good_color};
if (s.scene.pandaType == cereal::PandaState::PandaType::UNKNOWN) {
pandaStatus = danger_color;
pandaStr = "NO\nPANDA";
pandaStatus = {"NO\nPANDA", danger_color};
} else if (s.scene.started && !sm["liveLocationKalman"].getLiveLocationKalman().getGpsOK()) {
pandaStatus = warning_color;
pandaStr = "GPS\nSEARCHING";
pandaStatus = {"GPS\nSEARCHING", warning_color};
}
setProperty("pandaStr", pandaStr);
setProperty("pandaStatus", pandaStatus);
setProperty("pandaStatus", QVariant::fromValue(pandaStatus));
}
void Sidebar::paintEvent(QPaintEvent *event) {
@@ -103,6 +89,8 @@ void Sidebar::paintEvent(QPaintEvent *event) {
p.setPen(Qt::NoPen);
p.setRenderHint(QPainter::Antialiasing);
p.fillRect(rect(), QColor(57, 57, 57));
// static imgs
p.setOpacity(0.65);
p.drawImage(settings_btn.x(), settings_btn.y(), settings_img);
@@ -124,7 +112,7 @@ void Sidebar::paintEvent(QPaintEvent *event) {
p.drawText(r, Qt::AlignCenter, net_type);
// metrics
drawMetric(p, "TEMP", QString("%1°C").arg(temp_val), temp_status, 338);
drawMetric(p, panda_str, "", panda_status, 518);
drawMetric(p, connect_str, "", connect_status, 676);
drawMetric(p, "TEMP", temp_status.first, temp_status.second, 338);
drawMetric(p, panda_status.first, "", panda_status.second, 518);
drawMetric(p, connect_status.first, "", connect_status.second, 676);
}
selfdrive/ui/qt/sidebar.h
+7 -14
View File
@@ -6,14 +6,14 @@
#include "selfdrive/common/params.h"
#include "selfdrive/ui/ui.h"
typedef QPair<QString, QColor> ItemStatus;
Q_DECLARE_METATYPE(ItemStatus);
class Sidebar : public QFrame {
Q_OBJECT
Q_PROPERTY(QString connectStr MEMBER connect_str NOTIFY valueChanged);
Q_PROPERTY(QColor connectStatus MEMBER connect_status NOTIFY valueChanged);
Q_PROPERTY(QString pandaStr MEMBER panda_str NOTIFY valueChanged);
Q_PROPERTY(QColor pandaStatus MEMBER panda_status NOTIFY valueChanged);
Q_PROPERTY(int tempVal MEMBER temp_val NOTIFY valueChanged);
Q_PROPERTY(QColor tempStatus MEMBER temp_status NOTIFY valueChanged);
Q_PROPERTY(ItemStatus connectStatus MEMBER connect_status NOTIFY valueChanged);
Q_PROPERTY(ItemStatus pandaStatus MEMBER panda_status NOTIFY valueChanged);
Q_PROPERTY(ItemStatus tempStatus MEMBER temp_status NOTIFY valueChanged);
Q_PROPERTY(QString netType MEMBER net_type NOTIFY valueChanged);
Q_PROPERTY(int netStrength MEMBER net_strength NOTIFY valueChanged);
@@ -30,8 +30,6 @@ public slots:
protected:
void paintEvent(QPaintEvent *event) override;
void mouseReleaseEvent(QMouseEvent *event) override;
private:
void drawMetric(QPainter &p, const QString &label, const QString &val, QColor c, int y);
QImage home_img, settings_img;
@@ -51,12 +49,7 @@ private:
const QColor danger_color = QColor(201, 34, 49);
Params params;
QString connect_str = "OFFLINE";
QColor connect_status = warning_color;
QString panda_str = "NO\nPANDA";
QColor panda_status = warning_color;
int temp_val = 0;
QColor temp_status = warning_color;
ItemStatus connect_status, panda_status, temp_status;
QString net_type;
int net_strength = 0;
};
selfdrive/ui/qt/spinner.cc
+11 -7
View File
@@ -15,20 +15,24 @@
#include "selfdrive/ui/qt/util.h"
TrackWidget::TrackWidget(QWidget *parent) : QWidget(parent) {
setAttribute(Qt::WA_OpaquePaintEvent);
setFixedSize(spinner_size);
setAutoFillBackground(true);
setPalette(Qt::black);
// pre-compute all the track imgs. make this a gif instead?
QPixmap comma_img = QPixmap("../assets/img_spinner_comma.png").scaled(spinner_size, Qt::KeepAspectRatio, Qt::SmoothTransformation);
QTransform transform(1, 0, 0, 1, width() / 2, height() / 2);
QPixmap track_img = QPixmap("../assets/img_spinner_track.png").scaled(spinner_size, Qt::KeepAspectRatio, Qt::SmoothTransformation);
for (QPixmap &img : track_imgs) {
img = comma_img;
QPainter p(&img);
p.setRenderHint(QPainter::SmoothPixmapTransform);
QTransform transform(1, 0, 0, 1, width() / 2, height() / 2);
QPixmap pm(spinner_size);
QPainter p(&pm);
p.setRenderHint(QPainter::SmoothPixmapTransform);
for (int i = 0; i < track_imgs.size(); ++i) {
p.resetTransform();
p.fillRect(0, 0, spinner_size.width(), spinner_size.height(), Qt::black);
p.drawPixmap(0, 0, comma_img);
p.setTransform(transform.rotate(360 / spinner_fps));
p.drawPixmap(-width() / 2, -height() / 2, track_img);
track_imgs[i] = pm.copy();
}
m_anim.setDuration(1000);
selfdrive/ui/qt/util.cc
+16 -6
View File
@@ -16,6 +16,16 @@ QString getBrandVersion() {
return getBrand() + " v" + QString::fromStdString(Params().get("Version")).left(14).trimmed();
}
std::optional<QString> getDongleId() {
std::string id = Params().get("DongleId");
if (!id.empty() && (id != "UnregisteredDevice")) {
return QString::fromStdString(id);
} else {
return {};
}
}
void configFont(QPainter &p, const QString &family, int size, const QString &style) {
QFont f(family);
f.setPixelSize(size);
@@ -95,12 +105,12 @@ void ClickableWidget::paintEvent(QPaintEvent *) {
void swagLogMessageHandler(QtMsgType type, const QMessageLogContext &context, const QString &msg) {
static std::map<QtMsgType, int> levels = {
{QtMsgType::QtDebugMsg, 10},
{QtMsgType::QtInfoMsg, 20},
{QtMsgType::QtWarningMsg, 30},
{QtMsgType::QtCriticalMsg, 40},
{QtMsgType::QtSystemMsg, 40},
{QtMsgType::QtFatalMsg, 50},
{QtMsgType::QtDebugMsg, CLOUDLOG_DEBUG},
{QtMsgType::QtInfoMsg, CLOUDLOG_INFO},
{QtMsgType::QtWarningMsg, CLOUDLOG_WARNING},
{QtMsgType::QtCriticalMsg, CLOUDLOG_ERROR},
{QtMsgType::QtSystemMsg, CLOUDLOG_ERROR},
{QtMsgType::QtFatalMsg, CLOUDLOG_CRITICAL},
};
std::string file, function;
selfdrive/ui/qt/util.h
+3
View File
@@ -1,5 +1,7 @@
#pragma once
#include <optional>
#include <QDateTime>
#include <QLayout>
#include <QMouseEvent>
@@ -9,6 +11,7 @@
QString getBrand();
QString getBrandVersion();
std::optional<QString> getDongleId();
void configFont(QPainter &p, const QString &family, int size, const QString &style);
void clearLayout(QLayout* layout);
void setQtSurfaceFormat();
selfdrive/ui/qt/widgets/drive_stats.cc
+4 -4
View File
@@ -7,6 +7,7 @@
#include "selfdrive/common/params.h"
#include "selfdrive/ui/qt/request_repeater.h"
#include "selfdrive/ui/qt/util.h"
const double MILE_TO_KM = 1.60934;
@@ -46,10 +47,9 @@ DriveStats::DriveStats(QWidget* parent) : QFrame(parent) {
main_layout->addStretch();
add_stats_layouts("PAST WEEK", week_);
std::string dongle_id = Params().get("DongleId");
if (util::is_valid_dongle_id(dongle_id)) {
std::string url = "https://api.commadotai.com/v1.1/devices/" + dongle_id + "/stats";
RequestRepeater* repeater = new RequestRepeater(this, QString::fromStdString(url), "ApiCache_DriveStats", 30);
if (auto dongleId = getDongleId()) {
QString url = CommaApi::BASE_URL + "/v1.1/devices/" + *dongleId + "/stats";
RequestRepeater* repeater = new RequestRepeater(this, url, "ApiCache_DriveStats", 30);
QObject::connect(repeater, &RequestRepeater::receivedResponse, this, &DriveStats::parseResponse);
}
selfdrive/ui/qt/widgets/prime.cc
+7 -8
View File
@@ -11,6 +11,7 @@
#include <QrCode.hpp>
#include "selfdrive/ui/qt/request_repeater.h"
#include "selfdrive/ui/qt/util.h"
using qrcodegen::QrCode;
@@ -109,10 +110,9 @@ PrimeUserWidget::PrimeUserWidget(QWidget* parent) : QWidget(parent) {
mainLayout->addStretch();
// set up API requests
std::string dongleId = Params().get("DongleId");
if (util::is_valid_dongle_id(dongleId)) {
std::string url = "https://api.commadotai.com/v1/devices/" + dongleId + "/owner";
RequestRepeater *repeater = new RequestRepeater(this, QString::fromStdString(url), "ApiCache_Owner", 6);
if (auto dongleId = getDongleId()) {
QString url = CommaApi::BASE_URL + "/v1/devices/" + *dongleId + "/owner";
RequestRepeater *repeater = new RequestRepeater(this, url, "ApiCache_Owner", 6);
QObject::connect(repeater, &RequestRepeater::receivedResponse, this, &PrimeUserWidget::replyFinished);
}
}
@@ -255,10 +255,9 @@ SetupWidget::SetupWidget(QWidget* parent) : QFrame(parent) {
setSizePolicy(sp_retain);
// set up API requests
std::string dongleId = Params().get("DongleId");
if (util::is_valid_dongle_id(dongleId)) {
std::string url = "https://api.commadotai.com/v1.1/devices/" + dongleId + "/";
RequestRepeater* repeater = new RequestRepeater(this, QString::fromStdString(url), "ApiCache_Device", 5);
if (auto dongleId = getDongleId()) {
QString url = CommaApi::BASE_URL + "/v1.1/devices/" + *dongleId + "/";
RequestRepeater* repeater = new RequestRepeater(this, url, "ApiCache_Device", 5);
QObject::connect(repeater, &RequestRepeater::receivedResponse, this, &SetupWidget::replyFinished);
QObject::connect(repeater, &RequestRepeater::failedResponse, this, &SetupWidget::parseError);
selfdrive/ui/soundd
+1
View File
@@ -1,3 +1,4 @@
#!/bin/sh
cd "$(dirname "$0")"
export LD_LIBRARY_PATH="/system/lib64:$LD_LIBRARY_PATH"
exec ./_soundd
selfdrive/ui/ui
+1
View File
@@ -1,4 +1,5 @@
#!/bin/sh
cd "$(dirname "$0")"
export LD_LIBRARY_PATH="/system/lib64:$LD_LIBRARY_PATH"
export QT_PLUGIN_PATH="../../phonelibs/qt-plugins/$(uname -m)"
exec ./_ui
selfdrive/ui/ui.cc
+36 -29
View File
@@ -63,13 +63,13 @@ static int get_path_length_idx(const cereal::ModelDataV2::XYZTData::Reader &line
return max_idx;
}
static void update_leads(UIState *s, const cereal::RadarState::Reader &radar_state, std::optional<cereal::ModelDataV2::XYZTData::Reader> line) {
static void update_leads(UIState *s, const cereal::ModelDataV2::Reader &model) {
auto leads = model.getLeadsV3();
auto model_position = model.getPosition();
for (int i = 0; i < 2; ++i) {
auto lead_data = (i == 0) ? radar_state.getLeadOne() : radar_state.getLeadTwo();
if (lead_data.getStatus()) {
float z = line ? (*line).getZ()[get_path_length_idx(*line, lead_data.getDRel())] : 0.0;
// negative because radarState uses left positive convention
calib_frame_to_full_frame(s, lead_data.getDRel(), -lead_data.getYRel(), z + 1.22, &s->scene.lead_vertices[i]);
if (leads[i].getProb() > 0.5) {
float z = model_position.getZ()[get_path_length_idx(model_position, leads[i].getX()[0])];
calib_frame_to_full_frame(s, leads[i].getX()[0], leads[i].getY()[0], z + 1.22, &s->scene.lead_vertices[i]);
}
}
}
@@ -112,9 +112,9 @@ static void update_model(UIState *s, const cereal::ModelDataV2::Reader &model) {
}
// update path
auto lead_one = (*s->sm)["radarState"].getRadarState().getLeadOne();
if (lead_one.getStatus()) {
const float lead_d = lead_one.getDRel() * 2.;
auto lead_one = model.getLeadsV3()[0];
if (lead_one.getProb() > 0.5) {
const float lead_d = lead_one.getX()[0] * 2.;
max_distance = std::clamp((float)(lead_d - fmin(lead_d * 0.35, 10.)), 0.0f, max_distance);
}
max_idx = get_path_length_idx(model_position, max_distance);
@@ -134,12 +134,10 @@ static void update_state(UIState *s) {
scene.engageable = sm["controlsState"].getControlsState().getEngageable();
scene.dm_active = sm["driverMonitoringState"].getDriverMonitoringState().getIsActiveMode();
}
if (sm.updated("radarState") && s->vg) {
std::optional<cereal::ModelDataV2::XYZTData::Reader> line;
if (sm.rcv_frame("modelV2") > 0) {
line = sm["modelV2"].getModelV2().getPosition();
}
update_leads(s, sm["radarState"].getRadarState(), line);
if (sm.updated("modelV2") && s->vg) {
auto model = sm["modelV2"].getModelV2();
update_model(s, model);
update_leads(s, model);
}
if (sm.updated("liveCalibration")) {
scene.world_objects_visible = true;
@@ -158,9 +156,6 @@ static void update_state(UIState *s) {
}
}
}
if (sm.updated("modelV2") && s->vg) {
update_model(s, sm["modelV2"].getModelV2());
}
if (sm.updated("pandaState")) {
auto pandaState = sm["pandaState"].getPandaState();
scene.pandaType = pandaState.getPandaType();
@@ -189,15 +184,17 @@ static void update_state(UIState *s) {
if (sm.updated("roadCameraState")) {
auto camera_state = sm["roadCameraState"].getRoadCameraState();
float max_lines = Hardware::EON() ? 5408 : 1757;
float gain = camera_state.getGain();
float max_lines = Hardware::EON() ? 5408 : 1904;
float max_gain = Hardware::EON() ? 1.0: 10.0;
float max_ev = max_lines * max_gain;
if (Hardware::TICI()) {
// Max gain is 4 * 2.5 (High Conversion Gain)
gain /= 10.0;
if (Hardware::TICI) {
max_ev /= 6;
}
scene.light_sensor = std::clamp<float>((1023.0 / max_lines) * (max_lines - camera_state.getIntegLines() * gain), 0.0, 1023.0);
float ev = camera_state.getGain() * float(camera_state.getIntegLines());
scene.light_sensor = std::clamp<float>(1.0 - (ev / max_ev), 0.0, 1.0);
}
scene.started = sm["deviceState"].getDeviceState().getStarted() && scene.ignition;
}
@@ -273,7 +270,7 @@ static void update_status(UIState *s) {
QUIState::QUIState(QObject *parent) : QObject(parent) {
ui_state.sm = std::make_unique<SubMaster, const std::initializer_list<const char *>>({
"modelV2", "controlsState", "liveCalibration", "radarState", "deviceState", "roadCameraState",
"modelV2", "controlsState", "liveCalibration", "deviceState", "roadCameraState",
"pandaState", "carParams", "driverMonitoringState", "sensorEvents", "carState", "liveLocationKalman",
});
@@ -305,7 +302,7 @@ void QUIState::update() {
started_prev = ui_state.scene.started;
emit offroadTransition(!ui_state.scene.started);
// Change timeout to 0 when onroad, this will call update continously.
// Change timeout to 0 when onroad, this will call update continuously.
// This puts visionIPC in charge of update frequency, reducing video latency
timer->start(ui_state.scene.started ? 0 : 1000 / UI_FREQ);
}
@@ -339,9 +336,19 @@ void Device::setAwake(bool on, bool reset) {
}
void Device::updateBrightness(const UIState &s) {
float brightness_b = 10;
float brightness_m = 0.1;
float clipped_brightness = std::min(100.0f, (s.scene.light_sensor * brightness_m) + brightness_b);
// Scale to 0% to 100%
float clipped_brightness = 100.0 * s.scene.light_sensor;
// CIE 1931 - https://www.photonstophotos.net/GeneralTopics/Exposure/Psychometric_Lightness_and_Gamma.htm
if (clipped_brightness <= 8) {
clipped_brightness = (clipped_brightness / 903.3);
} else {
clipped_brightness = std::pow((clipped_brightness + 16.0) / 116.0, 3.0);
}
// Scale back to 10% to 100%
clipped_brightness = std::clamp(100.0f * clipped_brightness, 10.0f, 100.0f);
if (!s.scene.started) {
clipped_brightness = BACKLIGHT_OFFROAD;
}