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.7.8 release

Browse Source
This commit is contained in:
Vehicle Researcher
2020-08-13 00:37:05 +00:00
parent f370bf5ba6
commit 0aa4867be4
147 changed files with 3679 additions and 2713 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/boardd/SConscript
+5 -6
View File
@@ -1,9 +1,8 @@
Import('env', 'common', 'cereal', 'messaging')
Import('env', 'common', 'cereal', 'messaging', 'cython_dependencies')
env.Program('boardd.cc', LIBS=['usb-1.0', common, cereal, messaging, 'pthread', 'zmq', 'capnp', 'kj'])
env.Program('boardd', ['boardd.cc', 'panda.cc'], LIBS=['usb-1.0', common, cereal, messaging, 'pthread', 'zmq', 'capnp', 'kj'])
env.Library('libcan_list_to_can_capnp', ['can_list_to_can_capnp.cc'])
env.Command(['boardd_api_impl.so'],
['libcan_list_to_can_capnp.a', 'boardd_api_impl.pyx', 'boardd_setup.py'],
"cd selfdrive/boardd && python3 boardd_setup.py build_ext --inplace")
env.Command(['boardd_api_impl.so', 'boardd_api_impl.cpp'],
cython_dependencies + ['libcan_list_to_can_capnp.a', 'boardd_api_impl.pyx', 'boardd_setup.py'],
"cd selfdrive/boardd && python3 boardd_setup.py build_ext --inplace")
selfdrive/boardd/boardd.cc
+269 -593
View File
File diff suppressed because it is too large Load Diff
selfdrive/boardd/boardd_setup.py
-7
View File
@@ -1,15 +1,8 @@
import subprocess
from distutils.core import Extension, setup
from Cython.Build import cythonize
from common.cython_hacks import BuildExtWithoutPlatformSuffix
from common.basedir import BASEDIR
import os
PHONELIBS = os.path.join(BASEDIR, 'phonelibs')
ARCH = subprocess.check_output(["uname", "-m"], encoding='utf8').rstrip()
libraries = ['can_list_to_can_capnp', 'capnp', 'kj']
setup(name='Boardd API Implementation',
selfdrive/boardd/panda.cc
+318
View File
@@ -0,0 +1,318 @@
#include <stdexcept>
#include <cassert>
#include <iostream>
#include "common/swaglog.h"
#include "panda.h"
Panda::Panda(){
int err;
err = pthread_mutex_init(&usb_lock, NULL);
if (err != 0) { goto fail; }
// init libusb
err = libusb_init(&ctx);
if (err != 0) { goto fail; }
#if LIBUSB_API_VERSION >= 0x01000106
libusb_set_option(ctx, LIBUSB_OPTION_LOG_LEVEL, LIBUSB_LOG_LEVEL_INFO);
#else
libusb_set_debug(ctx, 3);
#endif
dev_handle = libusb_open_device_with_vid_pid(ctx, 0xbbaa, 0xddcc);
if (dev_handle == NULL) { goto fail; }
if (libusb_kernel_driver_active(dev_handle, 0) == 1) {
libusb_detach_kernel_driver(dev_handle, 0);
}
err = libusb_set_configuration(dev_handle, 1);
if (err != 0) { goto fail; }
err = libusb_claim_interface(dev_handle, 0);
if (err != 0) { goto fail; }
hw_type = get_hw_type();
is_pigeon =
(hw_type == cereal::HealthData::HwType::GREY_PANDA) ||
(hw_type == cereal::HealthData::HwType::BLACK_PANDA) ||
(hw_type == cereal::HealthData::HwType::UNO);
has_rtc = (hw_type == cereal::HealthData::HwType::UNO);
return;
fail:
cleanup();
throw std::runtime_error("Error connecting to panda");
}
Panda::~Panda(){
pthread_mutex_lock(&usb_lock);
cleanup();
connected = false;
pthread_mutex_unlock(&usb_lock);
}
void Panda::cleanup(){
if (dev_handle){
libusb_release_interface(dev_handle, 0);
libusb_close(dev_handle);
}
if (ctx) {
libusb_exit(ctx);
}
}
void Panda::handle_usb_issue(int err, const char func[]) {
LOGE_100("usb error %d \"%s\" in %s", err, libusb_strerror((enum libusb_error)err), func);
if (err == LIBUSB_ERROR_NO_DEVICE) {
LOGE("lost connection");
connected = false;
}
// TODO: check other errors, is simply retrying okay?
}
int Panda::usb_write(uint8_t bRequest, uint16_t wValue, uint16_t wIndex, unsigned int timeout) {
int err;
const uint8_t bmRequestType = LIBUSB_ENDPOINT_OUT | LIBUSB_REQUEST_TYPE_VENDOR | LIBUSB_RECIPIENT_DEVICE;
pthread_mutex_lock(&usb_lock);
do {
err = libusb_control_transfer(dev_handle, bmRequestType, bRequest, wValue, wIndex, NULL, 0, timeout);
if (err < 0) handle_usb_issue(err, __func__);
} while (err < 0 && connected);
pthread_mutex_unlock(&usb_lock);
return err;
}
int Panda::usb_read(uint8_t bRequest, uint16_t wValue, uint16_t wIndex, unsigned char *data, uint16_t wLength, unsigned int timeout) {
int err;
const uint8_t bmRequestType = LIBUSB_ENDPOINT_IN | LIBUSB_REQUEST_TYPE_VENDOR | LIBUSB_RECIPIENT_DEVICE;
pthread_mutex_lock(&usb_lock);
do {
err = libusb_control_transfer(dev_handle, bmRequestType, bRequest, wValue, wIndex, data, wLength, timeout);
if (err < 0) handle_usb_issue(err, __func__);
} while (err < 0 && connected);
pthread_mutex_unlock(&usb_lock);
return err;
}
int Panda::usb_bulk_write(unsigned char endpoint, unsigned char* data, int length, unsigned int timeout) {
int err;
int transferred = 0;
pthread_mutex_lock(&usb_lock);
do {
// Try sending can messages. If the receive buffer on the panda is full it will NAK
// and libusb will try again. After 5ms, it will time out. We will drop the messages.
err = libusb_bulk_transfer(dev_handle, endpoint, data, length, &transferred, timeout);
if (err == LIBUSB_ERROR_TIMEOUT) {
LOGW("Transmit buffer full");
break;
} else if (err != 0 || length != transferred) {
handle_usb_issue(err, __func__);
}
} while(err != 0 && connected);
pthread_mutex_unlock(&usb_lock);
return transferred;
}
int Panda::usb_bulk_read(unsigned char endpoint, unsigned char* data, int length, unsigned int timeout) {
int err;
int transferred = 0;
pthread_mutex_lock(&usb_lock);
do {
err = libusb_bulk_transfer(dev_handle, endpoint, data, length, &transferred, timeout);
if (err == LIBUSB_ERROR_TIMEOUT) {
break; // timeout is okay to exit, recv still happened
} else if (err == LIBUSB_ERROR_OVERFLOW) {
LOGE_100("overflow got 0x%x", transferred);
} else if (err != 0) {
handle_usb_issue(err, __func__);
}
} while(err != 0 && connected);
pthread_mutex_unlock(&usb_lock);
return transferred;
}
void Panda::set_safety_model(cereal::CarParams::SafetyModel safety_model, int safety_param){
usb_write(0xdc, (uint16_t)safety_model, safety_param);
}
cereal::HealthData::HwType Panda::get_hw_type() {
unsigned char hw_query[1] = {0};
usb_read(0xc1, 0, 0, hw_query, 1);
return (cereal::HealthData::HwType)(hw_query[0]);
}
void Panda::set_rtc(struct tm sys_time){
// tm struct has year defined as years since 1900
usb_write(0xa1, (uint16_t)(1900 + sys_time.tm_year), 0);
usb_write(0xa2, (uint16_t)(1 + sys_time.tm_mon), 0);
usb_write(0xa3, (uint16_t)sys_time.tm_mday, 0);
// usb_write(0xa4, (uint16_t)(1 + sys_time.tm_wday), 0);
usb_write(0xa5, (uint16_t)sys_time.tm_hour, 0);
usb_write(0xa6, (uint16_t)sys_time.tm_min, 0);
usb_write(0xa7, (uint16_t)sys_time.tm_sec, 0);
}
struct tm Panda::get_rtc(){
struct __attribute__((packed)) timestamp_t {
uint16_t year; // Starts at 0
uint8_t month;
uint8_t day;
uint8_t weekday;
uint8_t hour;
uint8_t minute;
uint8_t second;
} rtc_time = {0};
usb_read(0xa0, 0, 0, (unsigned char*)&rtc_time, sizeof(rtc_time));
struct tm new_time = { 0 };
new_time.tm_year = rtc_time.year - 1900; // tm struct has year defined as years since 1900
new_time.tm_mon = rtc_time.month - 1;
new_time.tm_mday = rtc_time.day;
new_time.tm_hour = rtc_time.hour;
new_time.tm_min = rtc_time.minute;
new_time.tm_sec = rtc_time.second;
return new_time;
}
void Panda::set_fan_speed(uint16_t fan_speed){
usb_write(0xb1, fan_speed, 0);
}
uint16_t Panda::get_fan_speed(){
uint16_t fan_speed_rpm = 0;
usb_read(0xb2, 0, 0, (unsigned char*)&fan_speed_rpm, sizeof(fan_speed_rpm));
return fan_speed_rpm;
}
void Panda::set_ir_pwr(uint16_t ir_pwr) {
usb_write(0xb0, ir_pwr, 0);
}
health_t Panda::get_health(){
health_t health {0};
usb_read(0xd2, 0, 0, (unsigned char*)&health, sizeof(health));
return health;
}
void Panda::set_loopback(bool loopback){
usb_write(0xe5, loopback, 0);
}
const char* Panda::get_firmware_version(){
const char* fw_sig_buf = new char[128]();
int read_1 = usb_read(0xd3, 0, 0, (unsigned char*)fw_sig_buf, 64);
int read_2 = usb_read(0xd4, 0, 0, (unsigned char*)fw_sig_buf + 64, 64);
if ((read_1 == 64) && (read_2 == 64)) {
return fw_sig_buf;
}
delete[] fw_sig_buf;
return NULL;
}
const char* Panda::get_serial(){
const char* serial_buf = new char[16]();
int err = usb_read(0xd0, 0, 0, (unsigned char*)serial_buf, 16);
if (err >= 0) {
return serial_buf;
}
delete[] serial_buf;
return NULL;
}
void Panda::set_power_saving(bool power_saving){
usb_write(0xe7, power_saving, 0);
}
void Panda::set_usb_power_mode(cereal::HealthData::UsbPowerMode power_mode){
usb_write(0xe6, (uint16_t)power_mode, 0);
}
void Panda::send_heartbeat(){
usb_write(0xf3, 1, 0);
}
void Panda::can_send(capnp::List<cereal::CanData>::Reader can_data_list){
int msg_count = can_data_list.size();
uint32_t *send = new uint32_t[msg_count*0x10]();
for (int i = 0; i < msg_count; i++) {
auto cmsg = can_data_list[i];
if (cmsg.getAddress() >= 0x800) { // extended
send[i*4] = (cmsg.getAddress() << 3) | 5;
} else { // normal
send[i*4] = (cmsg.getAddress() << 21) | 1;
}
auto can_data = cmsg.getDat();
assert(can_data.size() <= 8);
send[i*4+1] = can_data.size() | (cmsg.getSrc() << 4);
memcpy(&send[i*4+2], can_data.begin(), can_data.size());
}
usb_bulk_write(3, (unsigned char*)send, msg_count*0x10, 5);
delete[] send;
}
int Panda::can_receive(cereal::Event::Builder &event){
uint32_t data[RECV_SIZE/4];
int recv = usb_bulk_read(0x81, (unsigned char*)data, RECV_SIZE);
// return if length is 0
if (recv <= 0) {
return 0;
} else if (recv == RECV_SIZE) {
LOGW("Receive buffer full");
}
size_t num_msg = recv / 0x10;
auto canData = event.initCan(num_msg);
// populate message
for (int i = 0; i < num_msg; i++) {
if (data[i*4] & 4) {
// extended
canData[i].setAddress(data[i*4] >> 3);
//printf("got extended: %x\n", data[i*4] >> 3);
} else {
// normal
canData[i].setAddress(data[i*4] >> 21);
}
canData[i].setBusTime(data[i*4+1] >> 16);
int len = data[i*4+1]&0xF;
canData[i].setDat(kj::arrayPtr((uint8_t*)&data[i*4+2], len));
canData[i].setSrc((data[i*4+1] >> 4) & 0xff);
}
return recv;
}
selfdrive/boardd/panda.h
+78
View File
@@ -0,0 +1,78 @@
#pragma once
#include <ctime>
#include <cstdint>
#include <pthread.h>
#include <libusb-1.0/libusb.h>
#include "cereal/gen/cpp/car.capnp.h"
#include "cereal/gen/cpp/log.capnp.h"
// double the FIFO size
#define RECV_SIZE (0x1000)
#define TIMEOUT 0
// copied from panda/board/main.c
struct __attribute__((packed)) health_t {
uint32_t uptime;
uint32_t voltage;
uint32_t current;
uint32_t can_rx_errs;
uint32_t can_send_errs;
uint32_t can_fwd_errs;
uint32_t gmlan_send_errs;
uint32_t faults;
uint8_t ignition_line;
uint8_t ignition_can;
uint8_t controls_allowed;
uint8_t gas_interceptor_detected;
uint8_t car_harness_status;
uint8_t usb_power_mode;
uint8_t safety_model;
uint8_t fault_status;
uint8_t power_save_enabled;
};
class Panda {
private:
libusb_context *ctx = NULL;
libusb_device_handle *dev_handle = NULL;
pthread_mutex_t usb_lock;
void handle_usb_issue(int err, const char func[]);
void cleanup();
public:
Panda();
~Panda();
bool connected = true;
cereal::HealthData::HwType hw_type = cereal::HealthData::HwType::UNKNOWN;
bool is_pigeon = false;
bool has_rtc = false;
// HW communication
int usb_write(uint8_t bRequest, uint16_t wValue, uint16_t wIndex, unsigned int timeout=TIMEOUT);
int usb_read(uint8_t bRequest, uint16_t wValue, uint16_t wIndex, unsigned char *data, uint16_t wLength, unsigned int timeout=TIMEOUT);
int usb_bulk_write(unsigned char endpoint, unsigned char* data, int length, unsigned int timeout=TIMEOUT);
int usb_bulk_read(unsigned char endpoint, unsigned char* data, int length, unsigned int timeout=TIMEOUT);
// Panda functionality
cereal::HealthData::HwType get_hw_type();
void set_safety_model(cereal::CarParams::SafetyModel safety_model, int safety_param=0);
void set_rtc(struct tm sys_time);
struct tm get_rtc();
void set_fan_speed(uint16_t fan_speed);
uint16_t get_fan_speed();
void set_ir_pwr(uint16_t ir_pwr);
health_t get_health();
void set_loopback(bool loopback);
const char* get_firmware_version();
const char* get_serial();
void set_power_saving(bool power_saving);
void set_usb_power_mode(cereal::HealthData::UsbPowerMode power_mode);
void send_heartbeat();
void can_send(capnp::List<cereal::CanData>::Reader can_data_list);
int can_receive(cereal::Event::Builder &event);
};
selfdrive/camerad/cameras/camera_frame_stream.cc
-2
View File
@@ -94,8 +94,6 @@ CameraInfo cameras_supported[CAMERA_ID_MAX] = {
};
void cameras_init(DualCameraState *s) {
memset(s, 0, sizeof(*s));
camera_init(&s->rear, CAMERA_ID_IMX298, 20);
s->rear.transform = (mat3){{
1.0, 0.0, 0.0,
selfdrive/camerad/cameras/camera_qcom.cc
+22 -18
View File
@@ -12,7 +12,6 @@
#include <cutils/properties.h>
#include <pthread.h>
#include <czmq.h>
#include <capnp/serialize.h>
#include "msmb_isp.h"
#include "msmb_ispif.h"
@@ -108,8 +107,6 @@ static void camera_release_buffer(void* cookie, int buf_idx) {
static void camera_init(CameraState *s, int camera_id, int camera_num,
uint32_t pixel_clock, uint32_t line_length_pclk,
unsigned int max_gain, unsigned int fps) {
memset(s, 0, sizeof(*s));
s->camera_num = camera_num;
s->camera_id = camera_id;
@@ -125,9 +122,9 @@ static void camera_init(CameraState *s, int camera_id, int camera_num,
s->self_recover = 0;
zsock_t *ops_sock = zsock_new_push(">inproc://cameraops");
assert(ops_sock);
s->ops_sock = zsock_resolve(ops_sock);
s->ops_sock = zsock_new_push(">inproc://cameraops");
assert(s->ops_sock);
s->ops_sock_handle = zsock_resolve(s->ops_sock);
tbuffer_init2(&s->camera_tb, FRAME_BUF_COUNT, "frame",
camera_release_buffer, s);
@@ -262,8 +259,6 @@ static int imx179_s5k3p8sp_apply_exposure(CameraState *s, int gain, int integ_li
}
void cameras_init(DualCameraState *s) {
memset(s, 0, sizeof(*s));
char project_name[1024] = {0};
property_get("ro.boot.project_name", project_name, "");
@@ -397,7 +392,9 @@ static void set_exposure(CameraState *s, float exposure_frac, float gain_frac) {
if (err == 0) {
s->cur_exposure_frac = exposure_frac;
pthread_mutex_lock(&s->frame_info_lock);
s->cur_gain_frac = gain_frac;
pthread_mutex_unlock(&s->frame_info_lock);
}
//LOGD("set exposure: %f %f - %d", exposure_frac, gain_frac, err);
@@ -414,16 +411,20 @@ static void do_autoexposure(CameraState *s, float grey_frac) {
const unsigned int exposure_time_min = 16;
const unsigned int exposure_time_max = frame_length - 11; // copied from set_exposure()
float cur_gain_frac = s->cur_gain_frac;
float exposure_factor = pow(1.05, (target_grey - grey_frac) / 0.05);
if (s->cur_gain_frac > 0.125 && exposure_factor < 1) {
s->cur_gain_frac *= exposure_factor;
if (cur_gain_frac > 0.125 && exposure_factor < 1) {
cur_gain_frac *= exposure_factor;
} else if (s->cur_integ_lines * exposure_factor <= exposure_time_max && s->cur_integ_lines * exposure_factor >= exposure_time_min) { // adjust exposure time first
s->cur_exposure_frac *= exposure_factor;
} else if (s->cur_gain_frac * exposure_factor <= gain_frac_max && s->cur_gain_frac * exposure_factor >= gain_frac_min) {
s->cur_gain_frac *= exposure_factor;
} else if (cur_gain_frac * exposure_factor <= gain_frac_max && cur_gain_frac * exposure_factor >= gain_frac_min) {
cur_gain_frac *= exposure_factor;
}
pthread_mutex_lock(&s->frame_info_lock);
s->cur_gain_frac = cur_gain_frac;
pthread_mutex_unlock(&s->frame_info_lock);
set_exposure(s, s->cur_exposure_frac, s->cur_gain_frac);
set_exposure(s, s->cur_exposure_frac, cur_gain_frac);
} else { // keep the old for others
float new_exposure = s->cur_exposure_frac;
@@ -448,7 +449,7 @@ void camera_autoexposure(CameraState *s, float grey_frac) {
.grey_frac = grey_frac,
};
zmq_send(s->ops_sock, &msg, sizeof(msg), ZMQ_DONTWAIT);
zmq_send(s->ops_sock_handle, &msg, sizeof(msg), ZMQ_DONTWAIT);
}
static uint8_t* get_eeprom(int eeprom_fd, size_t *out_len) {
@@ -1795,15 +1796,16 @@ static void do_autofocus(CameraState *s) {
const int dac_up = s->device == DEVICE_LP3? LP3_AF_DAC_UP:OP3T_AF_DAC_UP;
const int dac_down = s->device == DEVICE_LP3? LP3_AF_DAC_DOWN:OP3T_AF_DAC_DOWN;
float lens_true_pos = s->lens_true_pos;
if (!isnan(err)) {
// learn lens_true_pos
s->lens_true_pos -= err*focus_kp;
lens_true_pos -= err*focus_kp;
}
// stay off the walls
s->lens_true_pos = clamp(s->lens_true_pos, dac_down, dac_up);
int target = clamp(s->lens_true_pos - sag, dac_down, dac_up);
lens_true_pos = clamp(lens_true_pos, dac_down, dac_up);
int target = clamp(lens_true_pos - sag, dac_down, dac_up);
s->lens_true_pos = lens_true_pos;
/*char debug[4096];
char *pdebug = debug;
@@ -1956,6 +1958,8 @@ static void camera_close(CameraState *s) {
}
free(s->eeprom);
zsock_destroy(&s->ops_sock);
}
selfdrive/camerad/cameras/camera_qcom.h
+8 -8
View File
@@ -1,9 +1,10 @@
#ifndef CAMERA_H
#define CAMERA_H
#pragma once
#include <stdint.h>
#include <stdbool.h>
#include <pthread.h>
#include <czmq.h>
#include <atomic>
#include "messaging.hpp"
#include "msmb_isp.h"
@@ -61,7 +62,8 @@ typedef struct CameraState {
int device;
void* ops_sock;
void* ops_sock_handle;
zsock_t * ops_sock;
uint32_t pixel_clock;
uint32_t line_length_pclk;
@@ -94,7 +96,7 @@ typedef struct CameraState {
int cur_frame_length;
int cur_integ_lines;
float digital_gain;
std::atomic<float> digital_gain;
StreamState ss[3];
@@ -111,9 +113,9 @@ typedef struct CameraState {
uint16_t cur_lens_pos;
uint64_t last_sag_ts;
float last_sag_acc_z;
float lens_true_pos;
std::atomic<float> lens_true_pos;
int self_recover; // af recovery counter, neg is patience, pos is active
std::atomic<int> self_recover; // af recovery counter, neg is patience, pos is active
int fps;
@@ -142,5 +144,3 @@ int sensor_write_regs(CameraState *s, struct msm_camera_i2c_reg_array* arr, size
#ifdef __cplusplus
} // extern "C"
#endif
#endif
selfdrive/camerad/main.cc
+38 -25
View File
@@ -332,6 +332,7 @@ void* frontview_thread(void *arg) {
//double t2 = millis_since_boot();
//LOGD("front process: %.2fms", t2-t1);
}
clReleaseCommandQueue(q);
return NULL;
}
@@ -345,6 +346,11 @@ void* processing_thread(void *arg) {
err = set_realtime_priority(51);
LOG("setpriority returns %d", err);
#if defined(QCOM) && !defined(QCOM_REPLAY)
std::unique_ptr<uint8_t[]> rgb_roi_buf = std::make_unique<uint8_t[]>((s->rgb_width/NUM_SEGMENTS_X)*(s->rgb_height/NUM_SEGMENTS_Y)*3);
std::unique_ptr<int16_t[]> conv_result = std::make_unique<int16_t[]>((s->rgb_width/NUM_SEGMENTS_X)*(s->rgb_height/NUM_SEGMENTS_Y));
#endif
// init cl stuff
#ifdef __APPLE__
cl_command_queue q = clCreateCommandQueue(s->context, s->device_id, 0, &err);
@@ -416,13 +422,12 @@ void* processing_thread(void *arg) {
/*double t10 = millis_since_boot();*/
// cache rgb roi and write to cl
uint8_t *rgb_roi_buf = new uint8_t[(s->rgb_width/NUM_SEGMENTS_X)*(s->rgb_height/NUM_SEGMENTS_Y)*3];
int roi_id = cnt % ((ROI_X_MAX-ROI_X_MIN+1)*(ROI_Y_MAX-ROI_Y_MIN+1)); // rolling roi
int roi_x_offset = roi_id % (ROI_X_MAX-ROI_X_MIN+1);
int roi_y_offset = roi_id / (ROI_X_MAX-ROI_X_MIN+1);
for (int r=0;r<(s->rgb_height/NUM_SEGMENTS_Y);r++) {
memcpy(rgb_roi_buf + r * (s->rgb_width/NUM_SEGMENTS_X) * 3,
memcpy(rgb_roi_buf.get() + r * (s->rgb_width/NUM_SEGMENTS_X) * 3,
(uint8_t *) s->rgb_bufs[rgb_idx].addr + \
(ROI_Y_MIN + roi_y_offset) * s->rgb_height/NUM_SEGMENTS_Y * FULL_STRIDE_X * 3 + \
(ROI_X_MIN + roi_x_offset) * s->rgb_width/NUM_SEGMENTS_X * 3 + r * FULL_STRIDE_X * 3,
@@ -430,7 +435,7 @@ void* processing_thread(void *arg) {
}
err = clEnqueueWriteBuffer (q, s->rgb_conv_roi_cl, true, 0,
s->rgb_width/NUM_SEGMENTS_X * s->rgb_height/NUM_SEGMENTS_Y * 3 * sizeof(uint8_t), rgb_roi_buf, 0, 0, 0);
s->rgb_width/NUM_SEGMENTS_X * s->rgb_height/NUM_SEGMENTS_Y * 3 * sizeof(uint8_t), rgb_roi_buf.get(), 0, 0, 0);
assert(err == 0);
/*double t11 = millis_since_boot();
@@ -453,48 +458,45 @@ void* processing_thread(void *arg) {
clWaitForEvents(1, &conv_event);
clReleaseEvent(conv_event);
int16_t *conv_result = new int16_t[(s->rgb_width/NUM_SEGMENTS_X)*(s->rgb_height/NUM_SEGMENTS_Y)];
err = clEnqueueReadBuffer(q, s->rgb_conv_result_cl, true, 0,
s->rgb_width/NUM_SEGMENTS_X * s->rgb_height/NUM_SEGMENTS_Y * sizeof(int16_t), conv_result, 0, 0, 0);
s->rgb_width/NUM_SEGMENTS_X * s->rgb_height/NUM_SEGMENTS_Y * sizeof(int16_t), conv_result.get(), 0, 0, 0);
assert(err == 0);
/*t11 = millis_since_boot();
printf("conv time: %f ms\n", t11 - t10);
t10 = millis_since_boot();*/
get_lapmap_one(conv_result, &s->lapres[roi_id], s->rgb_width/NUM_SEGMENTS_X, s->rgb_height/NUM_SEGMENTS_Y);
get_lapmap_one(conv_result.get(), &s->lapres[roi_id], s->rgb_width/NUM_SEGMENTS_X, s->rgb_height/NUM_SEGMENTS_Y);
/*t11 = millis_since_boot();
printf("pool time: %f ms\n", t11 - t10);
t10 = millis_since_boot();*/
delete [] rgb_roi_buf;
delete [] conv_result;
/*t11 = millis_since_boot();
printf("process time: %f ms\n ----- \n", t11 - t10);
t10 = millis_since_boot();*/
// setup self recover
const float lens_true_pos = s->cameras.rear.lens_true_pos;
if (is_blur(&s->lapres[0]) &&
(s->cameras.rear.lens_true_pos < (s->cameras.device == DEVICE_LP3? LP3_AF_DAC_DOWN:OP3T_AF_DAC_DOWN)+1 ||
s->cameras.rear.lens_true_pos > (s->cameras.device == DEVICE_LP3? LP3_AF_DAC_UP:OP3T_AF_DAC_UP)-1) &&
(lens_true_pos < (s->cameras.device == DEVICE_LP3? LP3_AF_DAC_DOWN:OP3T_AF_DAC_DOWN)+1 ||
lens_true_pos > (s->cameras.device == DEVICE_LP3? LP3_AF_DAC_UP:OP3T_AF_DAC_UP)-1) &&
s->cameras.rear.self_recover < 2) {
// truly stuck, needs help
s->cameras.rear.self_recover -= 1;
if (s->cameras.rear.self_recover < -FOCUS_RECOVER_PATIENCE) {
LOGW("rear camera bad state detected. attempting recovery from %.1f, recover state is %d",
s->cameras.rear.lens_true_pos, s->cameras.rear.self_recover);
s->cameras.rear.self_recover = FOCUS_RECOVER_STEPS + ((s->cameras.rear.lens_true_pos < (s->cameras.device == DEVICE_LP3? LP3_AF_DAC_M:OP3T_AF_DAC_M))?1:0); // parity determined by which end is stuck at
lens_true_pos, s->cameras.rear.self_recover.load());
s->cameras.rear.self_recover = FOCUS_RECOVER_STEPS + ((lens_true_pos < (s->cameras.device == DEVICE_LP3? LP3_AF_DAC_M:OP3T_AF_DAC_M))?1:0); // parity determined by which end is stuck at
}
} else if ((s->cameras.rear.lens_true_pos < (s->cameras.device == DEVICE_LP3? LP3_AF_DAC_M - LP3_AF_DAC_3SIG:OP3T_AF_DAC_M - OP3T_AF_DAC_3SIG) ||
s->cameras.rear.lens_true_pos > (s->cameras.device == DEVICE_LP3? LP3_AF_DAC_M + LP3_AF_DAC_3SIG:OP3T_AF_DAC_M + OP3T_AF_DAC_3SIG)) &&
} else if ((lens_true_pos < (s->cameras.device == DEVICE_LP3? LP3_AF_DAC_M - LP3_AF_DAC_3SIG:OP3T_AF_DAC_M - OP3T_AF_DAC_3SIG) ||
lens_true_pos > (s->cameras.device == DEVICE_LP3? LP3_AF_DAC_M + LP3_AF_DAC_3SIG:OP3T_AF_DAC_M + OP3T_AF_DAC_3SIG)) &&
s->cameras.rear.self_recover < 2) {
// in suboptimal position with high prob, but may still recover by itself
s->cameras.rear.self_recover -= 1;
if (s->cameras.rear.self_recover < -(FOCUS_RECOVER_PATIENCE*3)) {
LOGW("rear camera bad state detected. attempting recovery from %.1f, recover state is %d", s->cameras.rear.lens_true_pos, s->cameras.rear.self_recover);
s->cameras.rear.self_recover = FOCUS_RECOVER_STEPS/2 + ((s->cameras.rear.lens_true_pos < (s->cameras.device == DEVICE_LP3? LP3_AF_DAC_M:OP3T_AF_DAC_M))?1:0);
LOGW("rear camera bad state detected. attempting recovery from %.1f, recover state is %d", lens_true_pos, s->cameras.rear.self_recover.load());
s->cameras.rear.self_recover = FOCUS_RECOVER_STEPS/2 + ((lens_true_pos < (s->cameras.device == DEVICE_LP3? LP3_AF_DAC_M:OP3T_AF_DAC_M))?1:0);
}
} else if (s->cameras.rear.self_recover < 0) {
s->cameras.rear.self_recover += 1; // reset if fine
@@ -504,7 +506,9 @@ void* processing_thread(void *arg) {
double t2 = millis_since_boot();
#ifndef QCOM2
uint8_t *bgr_ptr = (uint8_t*)s->rgb_bufs[rgb_idx].addr;
#endif
double yt1 = millis_since_boot();
@@ -668,6 +672,7 @@ void* processing_thread(void *arg) {
LOGD("queued: %.2fms, yuv: %.2f, | processing: %.3fms", (t2-t1), (yt2-yt1), (t5-t1));
}
clReleaseCommandQueue(q);
return NULL;
}
@@ -1173,25 +1178,33 @@ void free_buffers(VisionState *s) {
// free bufs
for (int i=0; i<FRAME_BUF_COUNT; i++) {
visionbuf_free(&s->camera_bufs[i]);
visionbuf_free(&s->front_camera_bufs[i]);
visionbuf_free(&s->focus_bufs[i]);
visionbuf_free(&s->stats_bufs[i]);
}
for (int i=0; i<FRAME_BUF_COUNT; i++) {
visionbuf_free(&s->front_camera_bufs[i]);
}
for (int i=0; i<UI_BUF_COUNT; i++) {
visionbuf_free(&s->rgb_bufs[i]);
}
for (int i=0; i<UI_BUF_COUNT; i++) {
visionbuf_free(&s->rgb_front_bufs[i]);
}
for (int i=0; i<YUV_COUNT; i++) {
visionbuf_free(&s->yuv_ion[i]);
visionbuf_free(&s->yuv_front_ion[i]);
}
clReleaseMemObject(s->rgb_conv_roi_cl);
clReleaseMemObject(s->rgb_conv_result_cl);
clReleaseMemObject(s->rgb_conv_filter_cl);
clReleaseProgram(s->prg_debayer_rear);
clReleaseProgram(s->prg_debayer_front);
clReleaseKernel(s->krnl_debayer_rear);
clReleaseKernel(s->krnl_debayer_front);
clReleaseProgram(s->prg_rgb_laplacian);
clReleaseKernel(s->krnl_rgb_laplacian);
}
void party(VisionState *s) {
@@ -1255,7 +1268,7 @@ int main(int argc, char *argv[]) {
signal(SIGINT, (sighandler_t)set_do_exit);
signal(SIGTERM, (sighandler_t)set_do_exit);
VisionState state = {0};
VisionState state = {};
VisionState *s = &state;
clu_init();
selfdrive/camerad/snapshot/snapshot.py
+3 -6
View File
@@ -1,6 +1,5 @@
#!/usr/bin/env python3
import os
import json
import signal
import subprocess
import time
@@ -8,9 +7,7 @@ from PIL import Image
from common.basedir import BASEDIR
from common.params import Params
from selfdrive.camerad.snapshot.visionipc import VisionIPC
with open(BASEDIR + "/selfdrive/controls/lib/alerts_offroad.json") as json_file:
OFFROAD_ALERTS = json.load(json_file)
from selfdrive.controls.lib.alertmanager import set_offroad_alert
def jpeg_write(fn, dat):
@@ -26,7 +23,7 @@ def snapshot():
return None
params.put("IsTakingSnapshot", "1")
params.put("Offroad_IsTakingSnapshot", json.dumps(OFFROAD_ALERTS["Offroad_IsTakingSnapshot"]))
set_offroad_alert("Offroad_IsTakingSnapshot", True)
time.sleep(2.0) # Give thermald time to read the param, or if just started give camerad time to start
# Check if camerad is already started
@@ -64,7 +61,7 @@ def snapshot():
proc.communicate()
params.put("IsTakingSnapshot", "0")
params.delete("Offroad_IsTakingSnapshot")
set_offroad_alert("Offroad_IsTakingSnapshot", False)
return ret
selfdrive/car/__init__.py
+2 -2
View File
@@ -40,8 +40,8 @@ def scale_tire_stiffness(mass, wheelbase, center_to_front, tire_stiffness_factor
return tire_stiffness_front, tire_stiffness_rear
def dbc_dict(pt_dbc, radar_dbc, chassis_dbc=None):
return {'pt': pt_dbc, 'radar': radar_dbc, 'chassis': chassis_dbc}
def dbc_dict(pt_dbc, radar_dbc, chassis_dbc=None, body_dbc=None):
return {'pt': pt_dbc, 'radar': radar_dbc, 'chassis': chassis_dbc, 'body': body_dbc}
def apply_std_steer_torque_limits(apply_torque, apply_torque_last, driver_torque, LIMITS):
selfdrive/car/car_helpers.py
+8 -1
View File
@@ -27,6 +27,13 @@ def get_startup_event(car_recognized, controller_available):
return event
def get_one_can(logcan):
while True:
can = messaging.recv_one_retry(logcan)
if len(can.can) > 0:
return can
def load_interfaces(brand_names):
ret = {}
for brand_name in brand_names:
@@ -114,7 +121,7 @@ def fingerprint(logcan, sendcan, has_relay):
done = False
while not done:
a = messaging.get_one_can(logcan)
a = get_one_can(logcan)
for can in a.can:
# need to independently try to fingerprint both bus 0 and 1 to work
selfdrive/car/chrysler/carcontroller.py
-1
View File
@@ -11,7 +11,6 @@ class CarController():
self.prev_frame = -1
self.hud_count = 0
self.car_fingerprint = CP.carFingerprint
self.alert_active = False
self.gone_fast_yet = False
self.steer_rate_limited = False
selfdrive/car/chrysler/carstate.py
+12 -1
View File
@@ -103,6 +103,13 @@ class CarState(CarStateBase):
("WHEEL_SPEEDS", 50),
("STEERING", 100),
("ACC_2", 50),
("GEAR", 50),
("ACCEL_GAS_134", 50),
("DASHBOARD", 15),
("STEERING_LEVERS", 10),
("SEATBELT_STATUS", 2),
("DOORS", 1),
("TRACTION_BUTTON", 1),
]
return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 0)
@@ -115,6 +122,10 @@ class CarState(CarStateBase):
("CAR_MODEL", "LKAS_HUD", -1),
("LKAS_STATUS_OK", "LKAS_HEARTBIT", -1)
]
checks = []
checks = [
("LKAS_COMMAND", 100),
("LKAS_HEARTBIT", 10),
("LKAS_HUD", 4),
]
return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 2)
selfdrive/car/chrysler/interface.py
-4
View File
@@ -14,8 +14,6 @@ class CarInterface(CarInterfaceBase):
def get_params(candidate, fingerprint=None, has_relay=False, car_fw=None):
if fingerprint is None:
fingerprint = gen_empty_fingerprint()
if car_fw is None:
car_fw = []
ret = CarInterfaceBase.get_std_params(candidate, fingerprint, has_relay)
ret.carName = "chrysler"
@@ -72,8 +70,6 @@ class CarInterface(CarInterfaceBase):
# speeds
ret.steeringRateLimited = self.CC.steer_rate_limited if self.CC is not None else False
ret.buttonEvents = []
# events
events = self.create_common_events(ret, extra_gears=[car.CarState.GearShifter.low],
gas_resume_speed=2.)
selfdrive/car/chrysler/radar_interface.py
+4 -5
View File
@@ -1,16 +1,15 @@
#!/usr/bin/env python3
import os
from opendbc.can.parser import CANParser
from cereal import car
from selfdrive.car.interfaces import RadarInterfaceBase
from selfdrive.car.chrysler.values import DBC
RADAR_MSGS_C = list(range(0x2c2, 0x2d4+2, 2)) # c_ messages 706,...,724
RADAR_MSGS_D = list(range(0x2a2, 0x2b4+2, 2)) # d_ messages
LAST_MSG = max(RADAR_MSGS_C + RADAR_MSGS_D)
NUMBER_MSGS = len(RADAR_MSGS_C) + len(RADAR_MSGS_D)
def _create_radar_can_parser():
dbc_f = 'chrysler_pacifica_2017_hybrid_private_fusion.dbc'
def _create_radar_can_parser(car_fingerprint):
msg_n = len(RADAR_MSGS_C)
# list of [(signal name, message name or number, initial values), (...)]
# [('RADAR_STATE', 1024, 0),
@@ -37,7 +36,7 @@ def _create_radar_can_parser():
[20]*msg_n + # 20Hz (0.05s)
[20]*msg_n)) # 20Hz (0.05s)
return CANParser(os.path.splitext(dbc_f)[0], signals, checks, 1)
return CANParser(DBC[car_fingerprint]['radar'], signals, checks, 1)
def _address_to_track(address):
if address in RADAR_MSGS_C:
@@ -49,7 +48,7 @@ def _address_to_track(address):
class RadarInterface(RadarInterfaceBase):
def __init__(self, CP):
super().__init__(CP)
self.rcp = _create_radar_can_parser()
self.rcp = _create_radar_can_parser(CP.carFingerprint)
self.updated_messages = set()
self.trigger_msg = LAST_MSG
selfdrive/car/chrysler/values.py
+8 -23
View File
@@ -4,7 +4,6 @@ from selfdrive.car import dbc_dict
from cereal import car
Ecu = car.CarParams.Ecu
class SteerLimitParams:
STEER_MAX = 261 # 262 faults
STEER_DELTA_UP = 3 # 3 is stock. 100 is fine. 200 is too much it seems
@@ -82,32 +81,18 @@ FINGERPRINTS = {
DBC = {
CAR.PACIFICA_2017_HYBRID: dbc_dict(
'chrysler_pacifica_2017_hybrid', # 'pt'
'chrysler_pacifica_2017_hybrid_private_fusion'), # 'radar'
CAR.PACIFICA_2018: dbc_dict( # Same DBC file works.
'chrysler_pacifica_2017_hybrid', # 'pt'
'chrysler_pacifica_2017_hybrid_private_fusion'), # 'radar'
CAR.PACIFICA_2020: dbc_dict( # Same DBC file works.
'chrysler_pacifica_2017_hybrid', # 'pt'
'chrysler_pacifica_2017_hybrid_private_fusion'), # 'radar'
CAR.PACIFICA_2018_HYBRID: dbc_dict( # Same DBC file works.
'chrysler_pacifica_2017_hybrid', # 'pt'
'chrysler_pacifica_2017_hybrid_private_fusion'), # 'radar'
CAR.PACIFICA_2019_HYBRID: dbc_dict( # Same DBC file works.
'chrysler_pacifica_2017_hybrid', # 'pt'
'chrysler_pacifica_2017_hybrid_private_fusion'), # 'radar'
CAR.JEEP_CHEROKEE: dbc_dict( # Same DBC file works.
'chrysler_pacifica_2017_hybrid', # 'pt'
'chrysler_pacifica_2017_hybrid_private_fusion'), # 'radar'
CAR.JEEP_CHEROKEE_2019: dbc_dict( # Same DBC file works.
'chrysler_pacifica_2017_hybrid', # 'pt'
'chrysler_pacifica_2017_hybrid_private_fusion'), # 'radar'
CAR.PACIFICA_2017_HYBRID: dbc_dict('chrysler_pacifica_2017_hybrid', 'chrysler_pacifica_2017_hybrid_private_fusion'),
CAR.PACIFICA_2018: dbc_dict('chrysler_pacifica_2017_hybrid', 'chrysler_pacifica_2017_hybrid_private_fusion'),
CAR.PACIFICA_2020: dbc_dict('chrysler_pacifica_2017_hybrid', 'chrysler_pacifica_2017_hybrid_private_fusion'),
CAR.PACIFICA_2018_HYBRID: dbc_dict('chrysler_pacifica_2017_hybrid', 'chrysler_pacifica_2017_hybrid_private_fusion'),
CAR.PACIFICA_2019_HYBRID: dbc_dict('chrysler_pacifica_2017_hybrid', 'chrysler_pacifica_2017_hybrid_private_fusion'),
CAR.JEEP_CHEROKEE: dbc_dict('chrysler_pacifica_2017_hybrid', 'chrysler_pacifica_2017_hybrid_private_fusion'),
CAR.JEEP_CHEROKEE_2019: dbc_dict('chrysler_pacifica_2017_hybrid', 'chrysler_pacifica_2017_hybrid_private_fusion'),
}
STEER_THRESHOLD = 120
ECU_FINGERPRINT = {
Ecu.fwdCamera: [0x292], # lkas cmd
Ecu.fwdCamera: [0x292], # lkas cmd
}
selfdrive/car/ford/interface.py
+1 -1
View File
@@ -14,7 +14,7 @@ class CarInterface(CarInterfaceBase):
return float(accel) / 3.0
@staticmethod
def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False, car_fw=[]): # pylint: disable=dangerous-default-value
def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False, car_fw=None):
ret = CarInterfaceBase.get_std_params(candidate, fingerprint, has_relay)
ret.carName = "ford"
ret.safetyModel = car.CarParams.SafetyModel.ford
selfdrive/car/ford/radar_interface.py
+1 -2
View File
@@ -8,14 +8,13 @@ from selfdrive.car.interfaces import RadarInterfaceBase
RADAR_MSGS = list(range(0x500, 0x540))
def _create_radar_can_parser(car_fingerprint):
dbc_f = DBC[car_fingerprint]['radar']
msg_n = len(RADAR_MSGS)
signals = list(zip(['X_Rel'] * msg_n + ['Angle'] * msg_n + ['V_Rel'] * msg_n,
RADAR_MSGS * 3,
[0] * msg_n + [0] * msg_n + [0] * msg_n))
checks = list(zip(RADAR_MSGS, [20]*msg_n))
return CANParser(dbc_f, signals, checks, 1)
return CANParser(DBC[car_fingerprint]['radar'], signals, checks, 1)
class RadarInterface(RadarInterfaceBase):
def __init__(self, CP):
selfdrive/car/gm/interface.py
+3 -3
View File
@@ -16,13 +16,13 @@ class CarInterface(CarInterfaceBase):
return float(accel) / 4.0
@staticmethod
def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False, car_fw=[]): # pylint: disable=dangerous-default-value
def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False, car_fw=None):
ret = CarInterfaceBase.get_std_params(candidate, fingerprint, has_relay)
ret.carName = "gm"
ret.safetyModel = car.CarParams.SafetyModel.gm # default to gm
ret.safetyModel = car.CarParams.SafetyModel.gm
ret.enableCruise = False # stock cruise control is kept off
# GM port is considered a community feature, since it disables AEB;
# GM port is a community feature
# TODO: make a port that uses a car harness and it only intercepts the camera
ret.communityFeature = True
selfdrive/car/gm/radar_interface.py
+21 -25
View File
@@ -1,7 +1,6 @@
#!/usr/bin/env python3
from __future__ import print_function
import math
import time
from cereal import car
from opendbc.can.parser import CANParser
from selfdrive.car.gm.values import DBC, CAR, CanBus
@@ -17,30 +16,28 @@ NUM_SLOTS = 20
LAST_RADAR_MSG = RADAR_HEADER_MSG + NUM_SLOTS
def create_radar_can_parser(car_fingerprint):
dbc_f = DBC[car_fingerprint]['radar']
if car_fingerprint in (CAR.VOLT, CAR.MALIBU, CAR.HOLDEN_ASTRA, CAR.ACADIA, CAR.CADILLAC_ATS):
# C1A-ARS3-A by Continental
radar_targets = list(range(SLOT_1_MSG, SLOT_1_MSG + NUM_SLOTS))
signals = list(zip(['FLRRNumValidTargets',
'FLRRSnsrBlckd', 'FLRRYawRtPlsblityFlt',
'FLRRHWFltPrsntInt', 'FLRRAntTngFltPrsnt',
'FLRRAlgnFltPrsnt', 'FLRRSnstvFltPrsntInt'] +
['TrkRange'] * NUM_SLOTS + ['TrkRangeRate'] * NUM_SLOTS +
['TrkRangeAccel'] * NUM_SLOTS + ['TrkAzimuth'] * NUM_SLOTS +
['TrkWidth'] * NUM_SLOTS + ['TrkObjectID'] * NUM_SLOTS,
[RADAR_HEADER_MSG] * 7 + radar_targets * 6,
[0] * 7 +
[0.0] * NUM_SLOTS + [0.0] * NUM_SLOTS +
[0.0] * NUM_SLOTS + [0.0] * NUM_SLOTS +
[0.0] * NUM_SLOTS + [0] * NUM_SLOTS))
checks = []
return CANParser(dbc_f, signals, checks, CanBus.OBSTACLE)
else:
if car_fingerprint not in (CAR.VOLT, CAR.MALIBU, CAR.HOLDEN_ASTRA, CAR.ACADIA, CAR.CADILLAC_ATS):
return None
# C1A-ARS3-A by Continental
radar_targets = list(range(SLOT_1_MSG, SLOT_1_MSG + NUM_SLOTS))
signals = list(zip(['FLRRNumValidTargets',
'FLRRSnsrBlckd', 'FLRRYawRtPlsblityFlt',
'FLRRHWFltPrsntInt', 'FLRRAntTngFltPrsnt',
'FLRRAlgnFltPrsnt', 'FLRRSnstvFltPrsntInt'] +
['TrkRange'] * NUM_SLOTS + ['TrkRangeRate'] * NUM_SLOTS +
['TrkRangeAccel'] * NUM_SLOTS + ['TrkAzimuth'] * NUM_SLOTS +
['TrkWidth'] * NUM_SLOTS + ['TrkObjectID'] * NUM_SLOTS,
[RADAR_HEADER_MSG] * 7 + radar_targets * 6,
[0] * 7 +
[0.0] * NUM_SLOTS + [0.0] * NUM_SLOTS +
[0.0] * NUM_SLOTS + [0.0] * NUM_SLOTS +
[0.0] * NUM_SLOTS + [0] * NUM_SLOTS))
checks = []
return CANParser(DBC[car_fingerprint]['radar'], signals, checks, CanBus.OBSTACLE)
class RadarInterface(RadarInterfaceBase):
def __init__(self, CP):
super().__init__(CP)
@@ -53,8 +50,7 @@ class RadarInterface(RadarInterfaceBase):
def update(self, can_strings):
if self.rcp is None:
time.sleep(self.radar_ts) # nothing to do
return car.RadarData.new_message()
return super().update(None)
vls = self.rcp.update_strings(can_strings)
self.updated_messages.update(vls)
selfdrive/car/honda/carstate.py
+21 -1
View File
@@ -172,7 +172,7 @@ class CarState(CarStateBase):
self.v_cruise_pcm_prev = 0
self.cruise_mode = 0
def update(self, cp, cp_cam):
def update(self, cp, cp_cam, cp_body):
ret = car.CarState.new_message()
# car params
@@ -322,6 +322,12 @@ class CarState(CarStateBase):
self.stock_hud = cp_cam.vl["ACC_HUD"]
self.stock_brake = cp_cam.vl["BRAKE_COMMAND"]
if self.CP.carFingerprint in (CAR.CRV_5G, ):
# BSM messages are on B-CAN, requires a panda forwarding B-CAN messages to CAN 0
# more info here: https://github.com/commaai/openpilot/pull/1867
ret.leftBlindspot = cp_body.vl["BSM_STATUS_LEFT"]['BSM_ALERT'] == 1
ret.rightBlindspot = cp_body.vl["BSM_STATUS_RIGHT"]['BSM_ALERT'] == 1
return ret
@staticmethod
@@ -354,3 +360,17 @@ class CarState(CarStateBase):
bus_cam = 1 if CP.carFingerprint in HONDA_BOSCH and not CP.isPandaBlack else 2
return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, bus_cam)
@staticmethod
def get_body_can_parser(CP):
signals = []
checks = []
if CP.carFingerprint == CAR.CRV_5G:
signals += [("BSM_ALERT", "BSM_STATUS_RIGHT", 0),
("BSM_ALERT", "BSM_STATUS_LEFT", 0)]
bus_body = 0 # B-CAN is forwarded to ACC-CAN radar side (CAN 0 on fake ethernet port)
return CANParser(DBC[CP.carFingerprint]['body'], signals, checks, bus_body)
return None
selfdrive/car/honda/interface.py
+7 -5
View File
@@ -83,7 +83,7 @@ class CarInterface(CarInterfaceBase):
self.compute_gb = compute_gb_honda
@staticmethod
def compute_gb(accel, speed):
def compute_gb(accel, speed): # pylint: disable=method-hidden
raise NotImplementedError
@staticmethod
@@ -189,9 +189,9 @@ class CarInterface(CarInterfaceBase):
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 = [3.6, 2.4, 1.5]
ret.longitudinalTuning.kpV = [1.2, 0.8, 0.5]
ret.longitudinalTuning.kiBP = [0., 35.]
ret.longitudinalTuning.kiV = [0.54, 0.36]
ret.longitudinalTuning.kiV = [0.18, 0.12]
elif candidate in (CAR.ACCORD, CAR.ACCORD_15, CAR.ACCORDH):
stop_and_go = True
@@ -426,10 +426,12 @@ class CarInterface(CarInterfaceBase):
# ******************* do can recv *******************
self.cp.update_strings(can_strings)
self.cp_cam.update_strings(can_strings)
if self.cp_body:
self.cp_body.update_strings(can_strings)
ret = self.CS.update(self.cp, self.cp_cam)
ret = self.CS.update(self.cp, self.cp_cam, self.cp_body)
ret.canValid = self.cp.can_valid and self.cp_cam.can_valid
ret.canValid = self.cp.can_valid and self.cp_cam.can_valid and (self.cp_body is None or self.cp_body.can_valid)
ret.yawRate = self.VM.yaw_rate(ret.steeringAngle * CV.DEG_TO_RAD, ret.vEgo)
# FIXME: read sendcan for brakelights
brakelights_threshold = 0.02 if self.CS.CP.carFingerprint == CAR.CIVIC else 0.1
selfdrive/car/honda/radar_interface.py
+8 -10
View File
@@ -1,12 +1,10 @@
#!/usr/bin/env python3
import os
import time
from cereal import car
from opendbc.can.parser import CANParser
from selfdrive.car.interfaces import RadarInterfaceBase
from selfdrive.car.honda.values import DBC
def _create_nidec_can_parser():
dbc_f = 'acura_ilx_2016_nidec.dbc'
def _create_nidec_can_parser(car_fingerprint):
radar_messages = [0x400] + list(range(0x430, 0x43A)) + list(range(0x440, 0x446))
signals = list(zip(['RADAR_STATE'] +
['LONG_DIST'] * 16 + ['NEW_TRACK'] * 16 + ['LAT_DIST'] * 16 +
@@ -14,8 +12,7 @@ def _create_nidec_can_parser():
[0x400] + radar_messages[1:] * 4,
[0] + [255] * 16 + [1] * 16 + [0] * 16 + [0] * 16))
checks = list(zip([0x445], [20]))
fn = os.path.splitext(dbc_f)[0].encode('utf8')
return CANParser(fn, signals, checks, 1)
return CANParser(DBC[car_fingerprint]['radar'], signals, checks, 1)
class RadarInterface(RadarInterfaceBase):
@@ -30,7 +27,10 @@ class RadarInterface(RadarInterfaceBase):
self.delay = int(round(0.1 / CP.radarTimeStep)) # 0.1s delay of radar
# Nidec
self.rcp = _create_nidec_can_parser()
if self.radar_off_can:
self.rcp = None
else:
self.rcp = _create_nidec_can_parser(CP.carFingerprint)
self.trigger_msg = 0x445
self.updated_messages = set()
@@ -38,9 +38,7 @@ class RadarInterface(RadarInterfaceBase):
# in Bosch radar and we are only steering for now, so sleep 0.05s to keep
# radard at 20Hz and return no points
if self.radar_off_can:
if 'NO_RADAR_SLEEP' not in os.environ:
time.sleep(self.radar_ts)
return car.RadarData.new_message()
return super().update(None)
vls = self.rcp.update_strings(can_strings)
self.updated_messages.update(vls)
selfdrive/car/honda/values.py
+15 -2
View File
@@ -885,10 +885,23 @@ FW_VERSIONS = {
],
(Ecu.fwdCamera, 0x18dab5f1, None): [
b'36161-TXM-A050\x00\x00',
b'36161-TXM-A060\x00\x00',
],
(Ecu.srs, 0x18da53f1, None): [
b'77959-TXM-A230\x00\x00',
],
(Ecu.vsa, 0x18da28f1, None): [
b'57114-TXM-A040\x00\x00',
],
(Ecu.shiftByWire, 0x18da0bf1, None): [
b'54008-TWA-A910\x00\x00',
],
(Ecu.gateway, 0x18daeff1, None): [
b'38897-TXM-A020\x00\x00',
],
(Ecu.combinationMeter, 0x18da60f1, None): [
b'78109-TXM-A020\x00\x00',
],
},
CAR.HRV: {
(Ecu.gateway, 0x18daeff1, None): [b'38897-T7A-A010\x00\x00'],
@@ -909,7 +922,7 @@ DBC = {
CAR.CIVIC_BOSCH: dbc_dict('honda_civic_hatchback_ex_2017_can_generated', None),
CAR.CIVIC_BOSCH_DIESEL: dbc_dict('honda_civic_sedan_16_diesel_2019_can_generated', None),
CAR.CRV: dbc_dict('honda_crv_touring_2016_can_generated', 'acura_ilx_2016_nidec'),
CAR.CRV_5G: dbc_dict('honda_crv_ex_2017_can_generated', None),
CAR.CRV_5G: dbc_dict('honda_crv_ex_2017_can_generated', None, body_dbc='honda_crv_ex_2017_body_generated'),
CAR.CRV_EU: dbc_dict('honda_crv_executive_2016_can_generated', 'acura_ilx_2016_nidec'),
CAR.CRV_HYBRID: dbc_dict('honda_crv_hybrid_2019_can_generated', None),
CAR.FIT: dbc_dict('honda_fit_ex_2018_can_generated', 'acura_ilx_2016_nidec'),
@@ -974,4 +987,4 @@ ECU_FINGERPRINT = {
Ecu.fwdCamera: [0xE4, 0x194], # steer torque cmd
}
HONDA_BOSCH = [CAR.ACCORD, CAR.ACCORD_15, CAR.ACCORDH, CAR.CIVIC_BOSCH, CAR.CIVIC_BOSCH_DIESEL, CAR.CRV_5G, CAR.CRV_HYBRID, CAR.INSIGHT]
HONDA_BOSCH = set([CAR.ACCORD, CAR.ACCORD_15, CAR.ACCORDH, CAR.CIVIC_BOSCH, CAR.CIVIC_BOSCH_DIESEL, CAR.CRV_5G, CAR.CRV_HYBRID, CAR.INSIGHT])
selfdrive/car/hyundai/carstate.py
+5 -4
View File
@@ -41,8 +41,7 @@ class CarState(CarStateBase):
ret.cruiseState.standstill = cp.vl["SCC11"]['SCCInfoDisplay'] == 4.
if ret.cruiseState.enabled:
is_set_speed_in_mph = int(cp.vl["CLU11"]["CF_Clu_SPEED_UNIT"])
speed_conv = CV.MPH_TO_MS if is_set_speed_in_mph else CV.KPH_TO_MS
speed_conv = CV.MPH_TO_MS if cp.vl["CLU11"]["CF_Clu_SPEED_UNIT"] else CV.KPH_TO_MS
ret.cruiseState.speed = cp.vl["SCC11"]['VSetDis'] * speed_conv
else:
ret.cruiseState.speed = 0
@@ -282,7 +281,7 @@ class CarState(CarStateBase):
signals = [
# sig_name, sig_address, default
("CF_Lkas_Bca_R", "LKAS11", 0),
("CF_Lkas_LdwsActivemode", "LKAS11", 0),
("CF_Lkas_LdwsSysState", "LKAS11", 0),
("CF_Lkas_SysWarning", "LKAS11", 0),
("CF_Lkas_LdwsLHWarning", "LKAS11", 0),
@@ -299,6 +298,8 @@ class CarState(CarStateBase):
("CF_Lkas_LdwsOpt_USM", "LKAS11", 0)
]
checks = []
checks = [
("LKAS11", 100)
]
return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 2)
selfdrive/car/hyundai/hyundaican.py
+3 -3
View File
@@ -20,7 +20,7 @@ def create_lkas11(packer, frame, car_fingerprint, apply_steer, steer_req,
values["CF_Lkas_Chksum"] = 0
if car_fingerprint in [CAR.SONATA, CAR.PALISADE]:
values["CF_Lkas_Bca_R"] = int(left_lane) + (int(right_lane) << 1)
values["CF_Lkas_LdwsActivemode"] = int(left_lane) + (int(right_lane) << 1)
values["CF_Lkas_LdwsOpt_USM"] = 2
# FcwOpt_USM 5 = Orange blinking car + lanes
@@ -40,9 +40,9 @@ def create_lkas11(packer, frame, car_fingerprint, apply_steer, steer_req,
elif car_fingerprint == CAR.HYUNDAI_GENESIS:
# This field is actually LdwsActivemode
# Genesis and Optima fault when forwarding while engaged
values["CF_Lkas_Bca_R"] = 2
values["CF_Lkas_LdwsActivemode"] = 2
elif car_fingerprint == CAR.KIA_OPTIMA:
values["CF_Lkas_Bca_R"] = 0
values["CF_Lkas_LdwsActivemode"] = 0
dat = packer.make_can_msg("LKAS11", 0, values)[2]
selfdrive/car/hyundai/interface.py
+19 -6
View File
@@ -82,6 +82,13 @@ class CarInterface(CarInterfaceBase):
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]]
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.16], [0.01]]
ret.minSteerSpeed = 60 * CV.KPH_TO_MS
elif candidate == CAR.GENESIS_G70:
ret.lateralTuning.pid.kf = 0.00005
ret.mass = 1640. + STD_CARGO_KG
ret.wheelbase = 2.84
ret.steerRatio = 16.5
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]]
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.16], [0.01]]
elif candidate == CAR.GENESIS_G80:
ret.lateralTuning.pid.kf = 0.00005
ret.mass = 2060. + STD_CARGO_KG
@@ -111,10 +118,10 @@ class CarInterface(CarInterfaceBase):
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:
ret.lateralTuning.pid.kf = 0.00006
ret.lateralTuning.pid.kf = 0.00005
ret.mass = 1275. + STD_CARGO_KG
ret.wheelbase = 2.7
ret.steerRatio = 13.73 # Spec
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]]
@@ -143,9 +150,18 @@ class CarInterface(CarInterfaceBase):
tire_stiffness_factor = 0.5
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.VELOSTER:
ret.lateralTuning.pid.kf = 0.00005
ret.mass = 3558. * CV.LB_TO_KG
ret.wheelbase = 2.80
ret.steerRatio = 13.75 * 1.15
tire_stiffness_factor = 0.5
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]]
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25], [0.05]]
# these cars require a special panda safety mode due to missing counters and checksums in the messages
if candidate in [CAR.HYUNDAI_GENESIS, CAR.IONIQ_EV_LTD, CAR.IONIQ, CAR.KONA_EV]:
if candidate in [CAR.HYUNDAI_GENESIS, CAR.IONIQ_EV_LTD, CAR.IONIQ, CAR.KONA_EV, CAR.KIA_SORENTO, CAR.SONATA_2019,
CAR.KIA_OPTIMA, CAR.VELOSTER, CAR.KIA_STINGER, CAR.GENESIS_G70]:
ret.safetyModel = car.CarParams.SafetyModel.hyundaiLegacy
ret.centerToFront = ret.wheelbase * 0.4
@@ -170,9 +186,6 @@ class CarInterface(CarInterfaceBase):
ret = self.CS.update(self.cp, self.cp_cam)
ret.canValid = self.cp.can_valid and self.cp_cam.can_valid
# TODO: button presses
ret.buttonEvents = []
events = self.create_common_events(ret)
#TODO: addd abs(self.CS.angle_steers) > 90 to 'steerTempUnavailable' event
selfdrive/car/hyundai/radar_interface.py
+1 -6
View File
@@ -1,6 +1,4 @@
#!/usr/bin/env python3
import os
import time
from cereal import car
from opendbc.can.parser import CANParser
from selfdrive.car.interfaces import RadarInterfaceBase
@@ -33,10 +31,7 @@ class RadarInterface(RadarInterfaceBase):
def update(self, can_strings):
if self.radar_off_can:
if 'NO_RADAR_SLEEP' not in os.environ:
time.sleep(0.05) # radard runs on RI updates
return car.RadarData.new_message()
return super().update(None)
vls = self.rcp.update_strings(can_strings)
self.updated_messages.update(vls)
selfdrive/car/hyundai/values.py
+76 -24
View File
@@ -17,6 +17,7 @@ class SteerLimitParams:
class CAR:
ELANTRA = "HYUNDAI ELANTRA LIMITED ULTIMATE 2017"
ELANTRA_GT_I30 = "HYUNDAI I30 N LINE 2019 & GT 2018 DCT"
GENESIS_G70 = "GENESIS G70 2018"
GENESIS_G80 = "GENESIS G80 2017"
GENESIS_G90 = "GENESIS G90 2017"
HYUNDAI_GENESIS = "HYUNDAI GENESIS 2015-2016"
@@ -27,13 +28,13 @@ class CAR:
KIA_OPTIMA_H = "KIA OPTIMA HYBRID 2017 & SPORTS 2019"
KIA_SORENTO = "KIA SORENTO GT LINE 2018"
KIA_STINGER = "KIA STINGER GT2 2018"
KONA = "HYUNDAI KONA 2019"
KONA = "HYUNDAI KONA 2020"
KONA_EV = "HYUNDAI KONA ELECTRIC 2019"
SANTA_FE = "HYUNDAI SANTA FE LIMITED 2019"
SANTA_FE_1 = "HYUNDAI SANTA FE has no scc"
SONATA = "HYUNDAI SONATA 2020"
SONATA_2019 = "HYUNDAI SONATA 2019"
PALISADE = "HYUNDAI PALISADE 2020"
VELOSTER = "HYUNDAI VELOSTER 2019"
class Buttons:
@@ -85,20 +86,18 @@ FINGERPRINTS = {
CAR.SONATA_2019: [
{66: 8, 67: 8, 68: 8, 127: 8, 273: 8, 274: 8, 275: 8, 339: 8, 356: 4, 399: 8, 447: 8, 512: 6, 544: 8, 593: 8, 608: 8, 688: 5, 790: 8, 809: 8, 832: 8, 884: 8, 897: 8, 899: 8, 902: 8, 903: 6, 916: 8, 1040: 8, 1056: 8, 1057: 8, 1078: 4, 1151: 6, 1168: 7, 1170: 8, 1253: 8, 1254: 8, 1255: 8, 1265: 4, 1280: 1, 1287: 4, 1290: 8, 1292: 8, 1294: 8, 1312: 8, 1314: 8, 1322: 8, 1331: 8, 1332: 8, 1333: 8, 1342: 6, 1345: 8, 1348: 8, 1349: 8, 1351: 8, 1353: 8, 1363: 8, 1365: 8, 1366: 8, 1367: 8, 1369: 8, 1397: 8, 1407: 8, 1415: 8, 1419: 8, 1425: 2, 1427: 6, 1440: 8, 1456: 4, 1470: 8, 1472: 8, 1486: 8, 1487: 8, 1491: 8, 1530: 8, 1532: 5, 2000: 8, 2001: 8, 2004: 8, 2005: 8, 2008: 8, 2009: 8, 2012: 8, 2013: 8, 2014: 8, 2016: 8, 2017: 8, 2024: 8, 2025: 8},
],
CAR.KIA_OPTIMA: [
{
64: 8, 66: 8, 67: 8, 68: 8, 127: 8, 273: 8, 274: 8, 275: 8, 339: 8, 356: 4, 399: 8, 447: 8, 512: 6, 544: 8, 593: 8, 608: 8, 688: 5, 790: 8, 809: 8, 832: 8, 884: 8, 897: 8, 899: 8, 902: 8, 903: 6, 909: 8, 916: 8, 1040: 8, 1056: 8, 1057: 8, 1078: 4, 1151: 6, 1168: 7, 1170: 8, 1186: 2, 1191: 2, 1253: 8, 1254: 8, 1255: 8, 1265: 4, 1280: 1, 1282: 4, 1287: 4, 1290: 8, 1292: 8, 1294: 8, 1312: 8, 1322: 8, 1331: 8, 1332: 8, 1333: 8, 1342: 6, 1345: 8, 1348: 8, 1349: 8, 1351: 8, 1353: 8, 1363: 8, 1365: 8, 1366: 8, 1367: 8, 1369: 8, 1407: 8, 1414: 3, 1415: 8, 1419: 8, 1425: 2, 1427: 6, 1440: 8, 1456: 4, 1470: 8, 1472: 8, 1486: 8, 1487: 8, 1491: 8, 1530: 8, 1532: 5, 1952: 8, 1960: 8, 1988: 8, 1996: 8, 2001: 8, 2004: 8, 2008: 8, 2009: 8, 2012: 8, 2016: 8, 2017: 8, 2024: 8, 2025: 8
},
{
64: 8, 66: 8, 67: 8, 68: 8, 127: 8, 128: 8, 129: 8, 273: 8, 274: 8, 275: 8, 339: 8, 354: 3, 356: 4, 399: 8, 512: 6, 544: 8, 593: 8, 608: 8, 688: 5, 790: 8, 809: 8, 832: 8, 897: 8, 899: 8, 902: 8, 903: 6, 912: 7, 916: 8, 1040: 8, 1056: 8, 1057: 8, 1078: 4, 1151: 6, 1168: 7, 1170: 8, 1265: 4, 1268: 8, 1280: 1, 1282: 4, 1287: 4, 1290: 8, 1292: 8, 1294: 8, 1312: 8, 1322: 8, 1331: 8, 1332: 8, 1333: 8, 1342: 6, 1345: 8, 1348: 8, 1349: 8, 1351: 8, 1353: 8, 1356: 8, 1363: 8, 1365: 8, 1366: 8, 1367: 8, 1369: 8, 1407: 8, 1419: 8, 1425: 2, 1427: 6, 1440: 8, 1456: 4, 1470: 8, 1472: 8, 1491: 8, 1492: 8
},
],
CAR.KIA_OPTIMA: [{
64: 8, 66: 8, 67: 8, 68: 8, 127: 8, 128: 8, 129: 8, 273: 8, 274: 8, 275: 8, 339: 8, 354: 3, 356: 4, 399: 8, 447: 8, 512: 6, 544: 8, 558: 8, 593: 8, 608: 8, 640: 8, 688: 5, 790: 8, 809: 8, 832: 8, 884: 8, 897: 8, 899: 8, 902: 8, 903: 6, 909: 8, 912: 7, 916: 8, 1040: 8, 1056: 8, 1057: 8, 1078: 4, 1151: 6, 1168: 7, 1170: 8, 1186: 2, 1191: 2, 1253: 8, 1254: 8, 1255: 8, 1265: 4, 1268: 8, 1280: 1, 1282: 4, 1287: 4, 1290: 8, 1292: 8, 1294: 8, 1312: 8, 1322: 8, 1331: 8, 1332: 8, 1333: 8, 1342: 6, 1345: 8, 1348: 8, 1349: 8, 1351: 8, 1353: 8, 1356: 8, 1363: 8, 1365: 8, 1366: 8, 1367: 8, 1369: 8, 1407: 8, 1414: 3, 1415: 8, 1419: 8, 1425: 2, 1427: 6, 1440: 8, 1456: 4, 1470: 8, 1472: 8, 1486: 8, 1487: 8, 1491: 8, 1492: 8, 1530: 8, 1532: 5, 1792: 8, 1872: 8, 1937: 8, 1953: 8, 1968: 8, 1988: 8, 1996: 8, 2000: 8, 2001: 8, 2004: 8, 2008: 8, 2009: 8, 2012: 8, 2015: 8, 2016: 8, 2017: 8, 2024: 8, 2025: 8
}],
CAR.KIA_SORENTO: [{
67: 8, 68: 8, 127: 8, 304: 8, 320: 8, 339: 8, 356: 4, 544: 8, 593: 8, 608: 8, 688: 5, 809: 8, 832: 8, 854: 7, 870: 7, 871: 8, 872: 8, 897: 8, 902: 8, 903: 8, 916: 8, 1040: 8, 1042: 8, 1056: 8, 1057: 8, 1064: 8, 1078: 4, 1107: 5, 1136: 8, 1151: 6, 1168: 7, 1170: 8, 1173: 8, 1265: 4, 1280: 1, 1287: 4, 1290: 8, 1292: 8, 1294: 8, 1312: 8, 1322: 8, 1331: 8, 1332: 8, 1333: 8, 1342: 6, 1345: 8, 1348: 8, 1363: 8, 1369: 8, 1370: 8, 1371: 8, 1384: 8, 1407: 8, 1411: 8, 1419: 8, 1425: 2, 1427: 6, 1444: 8, 1456: 4, 1470: 8, 1489: 1
}],
CAR.KIA_STINGER: [{
67: 8, 127: 8, 304: 8, 320: 8, 339: 8, 356: 4, 358: 6, 359: 8, 544: 8, 576: 8, 593: 8, 608: 8, 688: 5, 809: 8, 832: 8, 854: 7, 870: 7, 871: 8, 872: 8, 897: 8, 902: 8, 909: 8, 916: 8, 1040: 8, 1042: 8, 1056: 8, 1057: 8, 1064: 8, 1078: 4, 1107: 5, 1136: 8, 1151: 6, 1168: 7, 1170: 8, 1173: 8, 1184: 8, 1265: 4, 1280: 1, 1281: 4, 1287: 4, 1290: 8, 1292: 8, 1294: 8, 1312: 8, 1322: 8, 1342: 6, 1345: 8, 1348: 8, 1363: 8, 1369: 8, 1371: 8, 1378: 4, 1379: 8, 1384: 8, 1407: 8, 1419: 8, 1425: 2, 1427: 6, 1456: 4, 1470: 8
}],
CAR.GENESIS_G70: [{
67: 8, 127: 8, 304: 8, 320: 8, 339: 8, 356: 4, 358: 6, 544: 8, 576: 8, 593: 8, 608: 8, 688: 5, 809: 8, 832:8, 854: 7, 870: 7, 871: 8, 872: 8, 897: 8, 902: 8, 909: 8, 916: 8, 1040: 8, 1042: 8, 1056: 8, 1057: 8, 1064: 8, 1078: 4, 1107: 5, 1136: 8, 1151: 6, 1156: 8, 1168: 7, 1170: 8, 1173:8, 1184: 8, 1186: 2, 1191: 2, 1265: 4, 1280: 1, 1287: 4, 1290: 8, 1292: 8, 1294: 8, 1312: 8, 1322: 8, 1342: 6, 1345: 8, 1348: 8, 1363: 8, 1369: 8, 1379: 8, 1384: 8, 1407: 8, 1419:8, 1427: 6, 1456: 4, 1470: 8, 1988: 8, 1996: 8, 2000: 8, 2004: 8, 2008: 8, 2012: 8, 2015: 8
}],
CAR.GENESIS_G80: [{
67: 8, 68: 8, 127: 8, 304: 8, 320: 8, 339: 8, 356: 4, 358: 6, 544: 8, 593: 8, 608: 8, 688: 5, 809: 8, 832: 8, 854: 7, 870: 7, 871: 8, 872: 8, 897: 8, 902: 8, 903: 8, 916: 8, 1024: 2, 1040: 8, 1042: 8, 1056: 8, 1057: 8, 1078: 4, 1107: 5, 1136: 8, 1151: 6, 1156: 8, 1168: 7, 1170: 8, 1173: 8, 1184: 8, 1191: 2, 1265: 4, 1280: 1, 1287: 4, 1290: 8, 1292: 8, 1294: 8, 1312: 8, 1322: 8, 1342: 6, 1345: 8, 1348: 8, 1363: 8, 1369: 8, 1370: 8, 1371: 8, 1378: 4, 1384: 8, 1407: 8, 1419: 8, 1425: 2, 1427: 6, 1434: 2, 1456: 4, 1470: 8
},
@@ -119,9 +118,9 @@ FINGERPRINTS = {
},
{
68:8, 127: 8, 304: 8, 320: 8, 339: 8, 352: 8, 356: 4, 524: 8, 544: 8, 576:8, 593: 8, 688: 5, 832: 8, 881: 8, 882: 8, 897: 8, 902: 8, 903: 8, 905: 8, 909: 8, 916: 8, 1040: 8, 1042: 8, 1056: 8, 1057: 8, 1078: 4, 1136: 6, 1151: 6, 1155: 8, 1156: 8, 1157: 4, 1164: 8, 1168: 7, 1173: 8, 1183: 8, 1186: 2, 1191: 2, 1225: 8, 1265: 4, 1280: 1, 1287: 4, 1290: 8, 1291: 8, 1292: 8, 1294: 8, 1312: 8, 1322: 8, 1342: 6, 1345: 8, 1348: 8, 1355: 8, 1363: 8, 1369: 8, 1379: 8, 1407: 8, 1419: 8, 1426: 8, 1427: 6, 1429: 8, 1430: 8, 1448: 8, 1456: 4, 1470: 8, 1473: 8, 1476: 8, 1507: 8, 1535: 8, 1988: 8, 1996: 8, 2000: 8, 2004: 8, 2005: 8, 2008: 8, 2012: 8, 2013: 8
}],
}],
CAR.KONA: [{
67: 8, 127: 8, 304: 8, 320: 8, 339: 8, 354: 3, 356: 4, 544: 8, 593: 8, 608: 8, 688: 5, 809: 8, 832: 8, 854: 7, 870: 7, 871: 8, 872: 8, 897: 8, 902: 8, 903: 8, 909: 8, 916: 8, 1040: 8, 1078: 4, 1107: 5, 1136: 8, 1156: 8, 1170: 8, 1173: 8, 1191: 2, 1265: 4, 1280: 1, 1287: 4, 1292: 8, 1294: 8, 1312: 8, 1322: 8, 1342: 6, 1345: 8, 1348: 8, 1363: 8, 1369: 8, 1384: 8, 1394: 8, 1407: 8, 1414: 3, 1419: 8, 1427: 6, 1456: 4, 1470: 8, 2004: 8, 2009: 8, 2012: 8
67: 8, 127: 8, 304: 8, 320: 8, 339: 8, 354: 3, 356: 4, 544: 8, 593: 8, 608: 8, 688: 5, 809: 8, 832 : 8, 854: 7, 870: 7, 871: 8, 872: 8, 897: 8, 902: 8, 903: 8, 905: 8, 909: 8, 916: 8, 1040: 8, 1056: 8, 1057: 8, 1064: 8, 1078: 4, 1107: 5, 1136: 8, 1151: 6, 1156: 8, 1170: 8, 1173: 8, 1186: 2, 1191: 2, 1193: 8, 1265: 4,1280: 1, 1287: 4, 1290: 8, 1292: 8, 1294: 8, 1312: 8, 1322: 8, 1342: 6, 1345: 8, 1348: 8, 1363: 8, 1369: 8, 1378: 8, 1384: 8, 1394: 8, 1407: 8, 1414: 3, 1419: 8, 1427: 6, 1456: 4, 1470: 8, 1988: 8, 1996: 8, 2000: 8, 2001: 8, 2004: 8, 2008: 8, 2009: 8, 2012: 8
}],
CAR.KONA_EV: [{
127: 8, 304: 8, 320: 8, 339: 8, 352: 8, 356: 4, 544: 8, 549: 8, 593: 8, 688: 5, 832: 8, 881: 8, 882: 8, 897: 8, 902: 8, 903: 8, 905: 8, 909: 8, 916: 8, 1040: 8, 1042: 8, 1056: 8, 1057: 8, 1078: 4, 1136: 8, 1151: 6, 1168: 7, 1173: 8, 1183: 8, 1186: 2, 1191: 2, 1225: 8, 1265: 4, 1280: 1, 1287: 4, 1290: 8, 1291: 8, 1292: 8, 1294: 8, 1307: 8, 1312: 8, 1322: 8, 1342: 6, 1345: 8, 1348: 8, 1355: 8, 1363: 8, 1369: 8, 1378: 4, 1407: 8, 1419: 8, 1426: 8, 1427: 6, 1429: 8, 1430: 8, 1456: 4, 1470: 8, 1473: 8, 1507: 8, 1535: 8, 2000: 8, 2004: 8, 2008: 8, 2012: 8
@@ -136,14 +135,20 @@ FINGERPRINTS = {
68: 8, 127: 8, 304: 8, 320: 8, 339: 8, 352: 8, 356: 4, 544: 8, 576: 8, 593: 8, 688: 5, 881: 8, 882: 8, 897: 8, 902: 8, 903: 8, 909: 8, 912: 7, 916: 8, 1040: 8, 1056: 8, 1057: 8, 1078: 4, 1136: 6, 1151: 6, 1168: 7, 1173: 8, 1180: 8, 1186: 2, 1191: 2, 1265: 4, 1268: 8, 1280: 1, 1287: 4, 1290: 8, 1291: 8, 1292: 8, 1294: 8, 1312: 8, 1322: 8, 1342: 6, 1345: 8, 1348: 8, 1355: 8, 1363: 8, 1369: 8, 1371: 8, 1407: 8, 1419: 8, 1420: 8, 1425: 2, 1427: 6, 1429: 8, 1430: 8, 1448: 8, 1456: 4, 1470: 8, 1476: 8, 1535: 8
}],
CAR.PALISADE: [{
67: 8, 127: 8, 304: 8, 320: 8, 339: 8, 356: 4, 544: 8, 549: 8, 576: 8, 593: 8, 608: 8, 688: 6, 809: 8, 832: 8, 854: 7, 870: 7, 871: 8, 872: 8, 897: 8, 902: 8, 903: 8, 905: 8, 909: 8, 913: 8, 916: 8, 1040: 8, 1042: 8, 1056: 8, 1057: 8, 1064: 8, 1078: 4, 1107: 5, 1123: 8, 1136: 8, 1151: 6, 1155: 8, 1156: 8, 1157: 4, 1162: 8, 1164: 8, 1168: 7, 1170: 8, 1173: 8, 1180: 8, 1186: 2, 1191: 2, 1193: 8, 1210: 8, 1225: 8, 1227: 8, 1265: 4, 1280: 8, 1287: 4, 1290: 8, 1292: 8, 1294: 8, 1312: 8, 1322: 8, 1342: 6, 1345: 8, 1348: 8, 1363: 8, 1369: 8, 1371: 8, 1378: 8, 1384: 8, 1407: 8, 1419: 8, 1427: 6, 1456: 4, 1470: 8, 2000: 8, 2005: 8, 2008: 8
67: 8, 127: 8, 304: 8, 320: 8, 339: 8, 356: 4, 544: 8, 549: 8, 576: 8, 593: 8, 608: 8, 688: 6, 809: 8, 832: 8, 854: 7, 870: 7, 871: 8, 872: 8, 897: 8, 902: 8, 903: 8, 905: 8, 909: 8, 913: 8, 916: 8, 1040: 8, 1042: 8, 1056: 8, 1057: 8, 1064: 8, 1078: 4, 1107: 5, 1123: 8, 1136: 8, 1151: 6, 1155: 8, 1156: 8, 1157: 4, 1162: 8, 1164: 8, 1168: 7, 1170: 8, 1173: 8, 1180: 8, 1186: 2, 1191: 2, 1193: 8, 1210: 8, 1225: 8, 1227: 8, 1265: 4, 1280: 8, 1287: 4, 1290: 8, 1292: 8, 1294: 8, 1312: 8, 1322: 8, 1342: 6, 1345: 8, 1348: 8, 1363: 8, 1369: 8, 1371: 8, 1378: 8, 1384: 8, 1407: 8, 1419: 8, 1427: 6, 1456: 4, 1470: 8, 2000: 8, 2004: 8, 2005: 8, 2008: 8, 2012: 8
}],
CAR.VELOSTER: [{
64: 8, 66: 8, 67: 8, 68: 8, 127: 8, 128: 8, 129: 8, 273: 8, 274: 8, 275: 8, 339: 8, 354: 3, 356: 4, 399: 8, 512: 6, 544: 8, 558: 8, 593: 8, 608: 8, 688: 5, 790: 8, 809: 8, 832: 8, 884: 8, 897: 8, 899: 8, 902: 8, 903: 8, 905: 8, 909: 8, 916: 8, 1040: 8, 1056: 8, 1057: 8, 1078: 4, 1170: 8, 1181: 5, 1186: 2, 1191: 2, 1265: 4, 1280: 1, 1282: 4, 1287: 4, 1290: 8, 1292: 8, 1294: 8, 1312: 8, 1322: 8, 1342: 6, 1345: 8, 1348: 8, 1349: 8, 1351: 8, 1353: 8, 1356: 8, 1363: 8, 1365: 8, 1366: 8, 1367: 8, 1369: 8, 1378: 4, 1407: 8, 1414: 3, 1415: 8, 1419: 8, 1427: 6, 1440: 8, 1456: 4, 1470: 8, 1486: 8, 1487: 8, 1491: 8, 1530: 8, 1532: 5, 1872: 8, 1988: 8, 1996: 8, 2000: 8, 2001: 8, 2004: 8, 2008: 8, 2009: 8, 2012: 8, 2015: 8, 2016: 8, 2017: 8, 2024: 8, 2025: 8
}]
}
ECU_FINGERPRINT = {
Ecu.fwdCamera: [832, 1156, 1191, 1342]
}
# Don't use these fingerprints for fingerprinting, they are still used for ECU detection
IGNORED_FINGERPRINTS = [CAR.VELOSTER, CAR.GENESIS_G70, CAR.KONA]
FW_VERSIONS = {
CAR.SONATA: {
(Ecu.fwdRadar, 0x7d0, None): [
@@ -181,7 +186,10 @@ FW_VERSIONS = {
(Ecu.engine, 0x7e0, None): [ b'\xf1\x81640E0051\x00\x00\x00\x00\x00\x00\x00\x00',],
(Ecu.eps, 0x7d4, None): [b'\xf1\x00CK MDPS R 1.00 1.04 57700-J5420 4C4VL104'],
(Ecu.fwdCamera, 0x7c4, None): [b'\xf1\x00CK MFC AT USA LHD 1.00 1.03 95740-J5000 170822'],
(Ecu.transmission, 0x7e1, None): [b'\xf1\x87VDHLG17118862DK2\x8awWwgu\x96wVfUVwv\x97xWvfvUTGTx\x87o\xff\xc9\xed\xf1\x81E21\x00\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 E21\x00\x00\x00\x00\x00\x00\x00SCK0T33NB0\x88\xa2\xe6\xf0'],
(Ecu.transmission, 0x7e1, None): [
b'\xf1\x87VDHLG17118862DK2\x8awWwgu\x96wVfUVwv\x97xWvfvUTGTx\x87o\xff\xc9\xed\xf1\x81E21\x00\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 E21\x00\x00\x00\x00\x00\x00\x00SCK0T33NB0\x88\xa2\xe6\xf0',
b'\xf1\x87VDHLG17000192DK2xdFffT\xa5VUD$DwT\x86wveVeeD&T\x99\xba\x8f\xff\xcc\x99\xf1\x81E21\x00\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 E21\x00\x00\x00\x00\x00\x00\x00SCK0T33NB0\x88\xa2\xe6\xf0',
],
},
CAR.KIA_OPTIMA_H: {
(Ecu.fwdRadar, 0x7d0, None): [b'\xf1\x00DEhe SCC H-CUP 1.01 1.02 96400-G5100 ',],
@@ -190,16 +198,58 @@ FW_VERSIONS = {
(Ecu.fwdCamera, 0x7c4, None): [b'\xf1\x00DEP MFC AT USA LHD 1.00 1.01 95740-G5010 170424',],
(Ecu.transmission, 0x7e1, None): [b"\xf1\x816U3J2051\x00\x00\xf1\x006U3H0_C2\x00\x006U3J2051\x00\x00PDE0G16NS2\xf4'\\\x91",],
},
CAR.PALISADE: {
(Ecu.fwdRadar, 0x7d0, None): [b'\xf1\x00LX2_ SCC FHCUP 1.00 1.04 99110-S8100 \xf1\xa01.04',],
CAR.PALISADE: {
(Ecu.fwdRadar, 0x7d0, None): [
b'\xf1\x00LX2_ SCC FHCUP 1.00 1.04 99110-S8100 \xf1\xa01.04',
b'\xf1\x00LX2 SCC FHCUP 1.00 1.04 99110-S8100 \xf1\xa01.04',
],
(Ecu.esp, 0x7d1, None): [
b'\xf1\x00LX ESC \x0b 102\x19\x05\x07 58910-S8330',
b'\xf1\x00LX ESC \v 102\x19\x05\a 58910-S8330\xf1\xa01.02',
b'\xf1\x00LX ESC \v 103\x19\t\x10 58910-S8360\xf1\xa01.03',
],
(Ecu.engine, 0x7e0, None): [b'\xf1\x81640J0051\x00\x00\x00\x00\x00\x00\x00\x00',],
(Ecu.eps, 0x7d4, None): [b'\xf1\x00LX2 MDPS C 1.00 1.03 56310-S8020 4LXDC103',],
(Ecu.fwdCamera, 0x7c4, None): [b'\xf1\x00LX2 MFC AT USA LHD 1.00 1.03 99211-S8100 190125',],
(Ecu.transmission, 0x7e1, None): [b'\xf1\x87LDKVBN424201KF26\xba\xaa\x9a\xa9\x99\x99\x89\x98\x89\x99\xa8\x99\x88\x99\x98\x89\x88\x99\xa8\x89v\x7f\xf7\xffwf_\xffq\xa6\xf1\x81U891\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U891\x00\x00\x00\x00\x00\x00SLX4G38NB2\xafL]\xe7',],
(Ecu.fwdCamera, 0x7c4, None): [
b'\xf1\x00LX2 MFC AT USA LHD 1.00 1.03 99211-S8100 190125',
b'\xf1\x00LX2 MFC AT USA LHD 1.00 1.05 99211-S8100 190909',
],
(Ecu.transmission, 0x7e1, None): [
b'\xf1\x87LDKVBN424201KF26\xba\xaa\x9a\xa9\x99\x99\x89\x98\x89\x99\xa8\x99\x88\x99\x98\x89\x88\x99\xa8\x89v\x7f\xf7\xffwf_\xffq\xa6\xf1\x81U891\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U891\x00\x00\x00\x00\x00\x00SLX4G38NB2\xafL]\xe7',
b'\xf1\x87LDLVBN560098KF26\x86fff\x87vgfg\x88\x96xfw\x86gfw\x86g\x95\xf6\xffeU_\xff\x92c\xf1\x81U891\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U891\x00\x00\x00\x00\x00\x00SLX4G38NB2\xafL]\xe7',
],
},
CAR.VELOSTER: {
(Ecu.fwdRadar, 0x7d0, None): [b'\xf1\x00JS__ SCC H-CUP 1.00 1.02 95650-J3200 ', ],
(Ecu.esp, 0x7d1, None): [b'\xf1\x00\x00\x00\x00\x00\x00\x00', ],
(Ecu.engine, 0x7e0, None): [b'\x01TJS-JNU06F200H0A', ],
(Ecu.eps, 0x7d4, None): [b'\xf1\x00JSL MDPS C 1.00 1.03 56340-J3000 8308', ],
(Ecu.fwdCamera, 0x7c4, None): [b'\xf1\x00JS LKAS AT USA LHD 1.00 1.02 95740-J3000 K32', ],
(Ecu.transmission, 0x7e1, None): [b'\xf1\x816U2V8051\x00\x00\xf1\x006U2V0_C2\x00\x006U2V8051\x00\x00DJS0T16NS1\xba\x02\xb8\x80', ],
},
CAR.GENESIS_G70: {
(Ecu.fwdRadar, 0x7d0, None): [b'\xf1\x00IK__ SCC F-CUP 1.00 1.02 96400-G9100 \xf1\xa01.02', ],
(Ecu.esp, 0x7d1, None): [b'\xf1\x00\x00\x00\x00\x00\x00\x00', ],
(Ecu.engine, 0x7e0, None): [b'\xf1\x81640F0051\x00\x00\x00\x00\x00\x00\x00\x00', ],
(Ecu.eps, 0x7d4, None): [b'\xf1\x00IK MDPS R 1.00 1.06 57700-G9420 4I4VL106', ],
(Ecu.fwdCamera, 0x7c4, None): [b'\xf1\x00IK MFC AT USA LHD 1.00 1.01 95740-G9000 170920', ],
(Ecu.transmission, 0x7e1, None): [b'\xf1\x87VDJLT17895112DN4\x88fVf\x99\x88\x88\x88\x87fVe\x88vhwwUFU\x97eFex\x99\xff\xb7\x82\xf1\x81E25\x00\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 E25\x00\x00\x00\x00\x00\x00\x00SIK0T33NB2\x11\x1am\xda', ],
},
CAR.KONA: {
(Ecu.fwdRadar, 0x7d0, None): [b'\xf1\x00OS__ SCC F-CUP 1.00 1.00 95655-J9200 \xf1\xa01.00', ],
(Ecu.esp, 0x7d1, None): [b'\xf1\x816V5RAK00018.ELF\xf1\x00\x00\x00\x00\x00\x00\x00\xf1\xa01.05', ],
(Ecu.engine, 0x7e0, None): [b'"\x01TOS-0NU06F301J02', ],
(Ecu.eps, 0x7d4, None): [b'\xf1\x00OS MDPS C 1.00 1.05 56310J9030\x00 4OSDC105', ],
(Ecu.fwdCamera, 0x7c4, None): [b'\xf1\x00OS9 LKAS AT USA LHD 1.00 1.00 95740-J9300 g21', ],
(Ecu.transmission, 0x7e1, None): [b'\xf1\x816U2VE051\x00\x00\xf1\x006U2V0_C2\x00\x006U2VE051\x00\x00DOS4T16NS3\x00\x00\x00\x00', ],
},
CAR.KIA_OPTIMA: {
(Ecu.fwdRadar, 0x7d0, None): [b'\xf1\x00JF__ SCC F-CUP 1.00 1.00 96400-D4110 '],
(Ecu.esp, 0x7d1, None): [b'\xf1\x00JF ESC \v 11 \x18\x030 58920-D5180',],
(Ecu.engine, 0x7e0, None): [b'\x01TJFAJNU06F201H03'],
(Ecu.eps, 0x7d4, None): [b'\xf1\x00TM MDPS C 1.00 1.00 56340-S2000 8409'],
(Ecu.fwdCamera, 0x7c4, None): [b'\xf1\x00JFA LKAS AT USA LHD 1.00 1.02 95895-D5000 h31'],
(Ecu.transmission, 0x7e1, None): [b'\xf1\x816U2V8051\x00\x00\xf1\x006U2V0_C2\x00\x006U2V8051\x00\x00DJF0T16NL0\t\xd2GW'],
}
}
@@ -210,21 +260,22 @@ CHECKSUM = {
FEATURES = {
# which message has the gear
"use_cluster_gears": [CAR.ELANTRA, CAR.KONA, CAR.ELANTRA_GT_I30],
"use_tcu_gears": [CAR.KIA_OPTIMA, CAR.SONATA_2019],
"use_elect_gears": [CAR.KIA_OPTIMA_H, CAR.IONIQ_EV_LTD, CAR.KONA_EV, CAR.IONIQ],
"use_cluster_gears": set([CAR.ELANTRA, CAR.ELANTRA_GT_I30, CAR.KONA]),
"use_tcu_gears": set([CAR.KIA_OPTIMA, CAR.SONATA_2019, CAR.VELOSTER]),
"use_elect_gears": set([CAR.KIA_OPTIMA_H, CAR.IONIQ_EV_LTD, CAR.KONA_EV, CAR.IONIQ]),
# these cars use the FCA11 message for the AEB and FCW signals, all others use SCC12
"use_fca": [CAR.SONATA, CAR.ELANTRA, CAR.ELANTRA_GT_I30, CAR.KIA_STINGER, CAR.IONIQ, CAR.KONA, CAR.KONA_EV, CAR.KIA_FORTE, CAR.PALISADE],
"use_fca": set([CAR.SONATA, CAR.ELANTRA, CAR.ELANTRA_GT_I30, CAR.KIA_STINGER, CAR.IONIQ, CAR.KONA_EV, CAR.KIA_FORTE, CAR.PALISADE, CAR.GENESIS_G70, CAR.KONA]),
"use_bsm": [CAR.SONATA, CAR.PALISADE, CAR.HYUNDAI_GENESIS, CAR.GENESIS_G80, CAR.GENESIS_G90],
"use_bsm": set([CAR.SONATA, CAR.PALISADE, CAR.HYUNDAI_GENESIS, CAR.GENESIS_G70, CAR.GENESIS_G80, CAR.GENESIS_G90, CAR.KONA]),
}
EV_HYBRID = [CAR.IONIQ_EV_LTD, CAR.IONIQ, CAR.KONA_EV]
EV_HYBRID = set([CAR.IONIQ_EV_LTD, CAR.IONIQ, CAR.KONA_EV])
DBC = {
CAR.ELANTRA: dbc_dict('hyundai_kia_generic', None),
CAR.ELANTRA_GT_I30: dbc_dict('hyundai_kia_generic', None),
CAR.GENESIS_G70: dbc_dict('hyundai_kia_generic', None),
CAR.GENESIS_G80: dbc_dict('hyundai_kia_generic', None),
CAR.GENESIS_G90: dbc_dict('hyundai_kia_generic', None),
CAR.HYUNDAI_GENESIS: dbc_dict('hyundai_kia_generic', None),
@@ -241,6 +292,7 @@ DBC = {
CAR.SONATA: dbc_dict('hyundai_kia_generic', None),
CAR.SONATA_2019: dbc_dict('hyundai_kia_generic', None),
CAR.PALISADE: dbc_dict('hyundai_kia_generic', None),
CAR.VELOSTER: dbc_dict('hyundai_kia_generic', None),
}
STEER_THRESHOLD = 150
selfdrive/car/interfaces.py
+8 -4
View File
@@ -27,6 +27,7 @@ class CarInterfaceBase():
self.CS = CarState(CP)
self.cp = self.CS.get_can_parser(CP)
self.cp_cam = self.CS.get_cam_can_parser(CP)
self.cp_body = self.CS.get_body_can_parser(CP)
self.CC = None
if CarController is not None:
@@ -41,7 +42,7 @@ class CarInterfaceBase():
raise NotImplementedError
@staticmethod
def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False, car_fw=[]): # pylint: disable=dangerous-default-value
def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False, car_fw=None):
raise NotImplementedError
# returns a set of default params to avoid repetition in car specific params
@@ -136,13 +137,12 @@ class RadarInterfaceBase():
self.pts = {}
self.delay = 0
self.radar_ts = CP.radarTimeStep
self.no_radar_sleep = 'NO_RADAR_SLEEP' in os.environ
def update(self, can_strings):
ret = car.RadarData.new_message()
if 'NO_RADAR_SLEEP' not in os.environ:
if not self.no_radar_sleep:
time.sleep(self.radar_ts) # radard runs on RI updates
return ret
class CarStateBase:
@@ -175,3 +175,7 @@ class CarStateBase:
@staticmethod
def get_cam_can_parser(CP):
return None
@staticmethod
def get_body_can_parser(CP):
return None
selfdrive/car/mazda/carstate.py
+2 -2
View File
@@ -162,8 +162,8 @@ class CarState(CarStateBase):
@staticmethod
def get_cam_can_parser(CP):
signals = [ ]
checks = [ ]
signals = []
checks = []
if CP.carFingerprint == CAR.CX5:
signals += [
selfdrive/car/mazda/interface.py
+1 -4
View File
@@ -15,7 +15,7 @@ class CarInterface(CarInterfaceBase):
return float(accel) / 4.0
@staticmethod
def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False, car_fw=[]): # pylint: disable=dangerous-default-value
def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False, car_fw=None):
ret = CarInterfaceBase.get_std_params(candidate, fingerprint, has_relay)
ret.carName = "mazda"
@@ -70,9 +70,6 @@ class CarInterface(CarInterfaceBase):
ret = self.CS.update(self.cp, self.cp_cam)
ret.canValid = self.cp.can_valid and self.cp_cam.can_valid
# TODO: button presses
ret.buttonEvents = []
# events
events = self.create_common_events(ret)
selfdrive/car/mock/interface.py
+1 -1
View File
@@ -33,7 +33,7 @@ class CarInterface(CarInterfaceBase):
return accel
@staticmethod
def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False, car_fw=[]): # pylint: disable=dangerous-default-value
def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False, car_fw=None):
ret = CarInterfaceBase.get_std_params(candidate, fingerprint, has_relay)
ret.carName = "mock"
ret.safetyModel = car.CarParams.SafetyModel.noOutput
selfdrive/car/nissan/carcontroller.py
+6 -2
View File
@@ -2,7 +2,7 @@ from cereal import car
from common.numpy_fast import clip, interp
from selfdrive.car.nissan import nissancan
from opendbc.can.packer import CANPacker
from selfdrive.car.nissan.values import CAR
from selfdrive.car.nissan.values import CAR, STEER_THRESHOLD
# Steer angle limits
ANGLE_DELTA_BP = [0., 5., 15.]
@@ -53,7 +53,11 @@ class CarController():
else:
# Scale max torque based on how much torque the driver is applying to the wheel
self.lkas_max_torque = max(
0, LKAS_MAX_TORQUE - 0.4 * abs(CS.out.steeringTorque))
# Scale max torque down to half LKAX_MAX_TORQUE as a minimum
LKAS_MAX_TORQUE * 0.5,
# Start scaling torque at STEER_THRESHOLD
LKAS_MAX_TORQUE - 0.6 * max(0, abs(CS.out.steeringTorque) - STEER_THRESHOLD)
)
else:
apply_angle = CS.out.steeringAngle
selfdrive/car/nissan/carstate.py
+6 -1
View File
@@ -1,4 +1,5 @@
import copy
from collections import deque
from cereal import car
from opendbc.can.can_define import CANDefine
from selfdrive.car.interfaces import CarStateBase
@@ -6,12 +7,14 @@ from selfdrive.config import Conversions as CV
from opendbc.can.parser import CANParser
from selfdrive.car.nissan.values import CAR, DBC, STEER_THRESHOLD
TORQUE_SAMPLES = 12
class CarState(CarStateBase):
def __init__(self, CP):
super().__init__(CP)
can_define = CANDefine(DBC[CP.carFingerprint]['pt'])
self.steeringTorqueSamples = deque(TORQUE_SAMPLES*[0], TORQUE_SAMPLES)
self.shifter_values = can_define.dv["GEARBOX"]["GEAR_SHIFTER"]
def update(self, cp, cp_adas, cp_cam):
@@ -60,7 +63,9 @@ class CarState(CarStateBase):
ret.cruiseState.speed = speed * conversion
ret.steeringTorque = cp.vl["STEER_TORQUE_SENSOR"]["STEER_TORQUE_DRIVER"]
ret.steeringPressed = abs(ret.steeringTorque) > STEER_THRESHOLD
self.steeringTorqueSamples.append(ret.steeringTorque)
# Filtering driver torque to prevent steeringPressed false positives
ret.steeringPressed = bool(abs(sum(self.steeringTorqueSamples) / TORQUE_SAMPLES) > STEER_THRESHOLD)
ret.steeringAngle = cp.vl["STEER_ANGLE_SENSOR"]["STEER_ANGLE"]
selfdrive/car/nissan/interface.py
+1 -1
View File
@@ -14,7 +14,7 @@ class CarInterface(CarInterfaceBase):
return float(accel) / 4.0
@staticmethod
def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False, car_fw=[]): # pylint: disable=dangerous-default-value
def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False, car_fw=None):
ret = CarInterfaceBase.get_std_params(candidate, fingerprint, has_relay)
ret.carName = "nissan"
selfdrive/car/nissan/values.py
+1 -1
View File
@@ -2,7 +2,7 @@
from selfdrive.car import dbc_dict
STEER_THRESHOLD = 1.75
STEER_THRESHOLD = 1.0
class CAR:
XTRAIL = "NISSAN X-TRAIL 2017"
selfdrive/car/subaru/carcontroller.py
+41 -20
View File
@@ -1,7 +1,6 @@
#from common.numpy_fast import clip
from selfdrive.car import apply_std_steer_torque_limits
from selfdrive.car.subaru import subarucan
from selfdrive.car.subaru.values import DBC
from selfdrive.car.subaru.values import DBC, PREGLOBAL_CARS
from opendbc.can.packer import CANPacker
@@ -18,49 +17,71 @@ class CarControllerParams():
class CarController():
def __init__(self, dbc_name, CP, VM):
self.lkas_active = False
self.apply_steer_last = 0
self.es_distance_cnt = -1
self.es_accel_cnt = -1
self.es_lkas_cnt = -1
self.fake_button_prev = 0
self.steer_rate_limited = False
self.params = CarControllerParams()
self.packer = CANPacker(DBC[CP.carFingerprint]['pt'])
def update(self, enabled, CS, frame, actuators, pcm_cancel_cmd, visual_alert, left_line, right_line):
""" Controls thread """
P = self.params
# Send CAN commands.
can_sends = []
### STEER ###
# *** steering ***
if (frame % self.params.STEER_STEP) == 0:
if (frame % P.STEER_STEP) == 0:
final_steer = actuators.steer if enabled else 0.
apply_steer = int(round(final_steer * P.STEER_MAX))
apply_steer = int(round(actuators.steer * self.params.STEER_MAX))
# limits due to driver torque
new_steer = int(round(apply_steer))
apply_steer = apply_std_steer_torque_limits(new_steer, self.apply_steer_last, CS.out.steeringTorque, P)
apply_steer = apply_std_steer_torque_limits(new_steer, self.apply_steer_last, CS.out.steeringTorque, self.params)
self.steer_rate_limited = new_steer != apply_steer
if not enabled:
apply_steer = 0
can_sends.append(subarucan.create_steering_control(self.packer, apply_steer, frame, P.STEER_STEP))
if CS.CP.carFingerprint in PREGLOBAL_CARS:
can_sends.append(subarucan.create_preglobal_steering_control(self.packer, apply_steer, frame, self.params.STEER_STEP))
else:
can_sends.append(subarucan.create_steering_control(self.packer, apply_steer, frame, self.params.STEER_STEP))
self.apply_steer_last = apply_steer
if self.es_distance_cnt != CS.es_distance_msg["Counter"]:
can_sends.append(subarucan.create_es_distance(self.packer, CS.es_distance_msg, pcm_cancel_cmd))
self.es_distance_cnt = CS.es_distance_msg["Counter"]
if self.es_lkas_cnt != CS.es_lkas_msg["Counter"]:
can_sends.append(subarucan.create_es_lkas(self.packer, CS.es_lkas_msg, visual_alert, left_line, right_line))
self.es_lkas_cnt = CS.es_lkas_msg["Counter"]
# *** alerts and pcm cancel ***
if CS.CP.carFingerprint in PREGLOBAL_CARS:
if self.es_accel_cnt != CS.es_accel_msg["Counter"]:
# 1 = main, 2 = set shallow, 3 = set deep, 4 = resume shallow, 5 = resume deep
# disengage ACC when OP is disengaged
if pcm_cancel_cmd:
fake_button = 1
# turn main on if off and past start-up state
elif not CS.out.cruiseState.available and CS.ready:
fake_button = 1
else:
fake_button = CS.button
# unstick previous mocked button press
if fake_button == 1 and self.fake_button_prev == 1:
fake_button = 0
self.fake_button_prev = fake_button
can_sends.append(subarucan.create_es_throttle_control(self.packer, fake_button, CS.es_accel_msg))
self.es_accel_cnt = CS.es_accel_msg["Counter"]
else:
if self.es_distance_cnt != CS.es_distance_msg["Counter"]:
can_sends.append(subarucan.create_es_distance(self.packer, CS.es_distance_msg, pcm_cancel_cmd))
self.es_distance_cnt = CS.es_distance_msg["Counter"]
if self.es_lkas_cnt != CS.es_lkas_msg["Counter"]:
can_sends.append(subarucan.create_es_lkas(self.packer, CS.es_lkas_msg, visual_alert, left_line, right_line))
self.es_lkas_cnt = CS.es_lkas_msg["Counter"]
return can_sends
selfdrive/car/subaru/carstate.py
+126 -40
View File
@@ -4,7 +4,7 @@ from opendbc.can.can_define import CANDefine
from selfdrive.config import Conversions as CV
from selfdrive.car.interfaces import CarStateBase
from opendbc.can.parser import CANParser
from selfdrive.car.subaru.values import DBC, STEER_THRESHOLD
from selfdrive.car.subaru.values import DBC, STEER_THRESHOLD, CAR, PREGLOBAL_CARS
class CarState(CarStateBase):
@@ -20,7 +20,10 @@ class CarState(CarStateBase):
ret.gas = cp.vl["Throttle"]['Throttle_Pedal'] / 255.
ret.gasPressed = ret.gas > 1e-5
ret.brakePressed = cp.vl["Brake_Pedal"]['Brake_Pedal'] > 1e-5
if self.car_fingerprint in PREGLOBAL_CARS:
ret.brakePressed = cp.vl["Brake_Pedal"]['Brake_Pedal'] > 2
else:
ret.brakePressed = cp.vl["Brake_Pedal"]['Brake_Pedal'] > 1e-5
ret.brakeLights = ret.brakePressed
ret.wheelSpeeds.fl = cp.vl["Wheel_Speeds"]['FL'] * CV.KPH_TO_MS
@@ -51,8 +54,12 @@ class CarState(CarStateBase):
ret.cruiseState.enabled = cp.vl["CruiseControl"]['Cruise_Activated'] != 0
ret.cruiseState.available = cp.vl["CruiseControl"]['Cruise_On'] != 0
ret.cruiseState.speed = cp_cam.vl["ES_DashStatus"]['Cruise_Set_Speed'] * CV.KPH_TO_MS
# EDM Impreza: 1 = mph, UDM Forester: 7 = mph
if cp.vl["Dash_State"]['Units'] in [1, 7]:
# UDM Forester, Legacy: mph = 0
if self.car_fingerprint in [CAR.FORESTER_PREGLOBAL, CAR.LEGACY_PREGLOBAL] and cp.vl["Dash_State"]['Units'] == 0:
ret.cruiseState.speed *= CV.MPH_TO_KPH
# EDM Global: mph = 1, 2; All Outback: mph = 1, UDM Forester: mph = 7
elif self.car_fingerprint not in [CAR.FORESTER_PREGLOBAL, CAR.LEGACY_PREGLOBAL] and cp.vl["Dash_State"]['Units'] in [1, 2, 7]:
ret.cruiseState.speed *= CV.MPH_TO_KPH
ret.seatbeltUnlatched = cp.vl["Dashlights"]['SEATBELT_FL'] == 1
@@ -61,8 +68,17 @@ class CarState(CarStateBase):
cp.vl["BodyInfo"]['DOOR_OPEN_FR'],
cp.vl["BodyInfo"]['DOOR_OPEN_FL']])
self.es_distance_msg = copy.copy(cp_cam.vl["ES_Distance"])
self.es_lkas_msg = copy.copy(cp_cam.vl["ES_LKAS_State"])
if self.car_fingerprint in PREGLOBAL_CARS:
ret.steerError = cp.vl["Steering_Torque"]["LKA_Lockout"] == 1
self.button = cp_cam.vl["ES_CruiseThrottle"]["Button"]
self.ready = not cp_cam.vl["ES_DashStatus"]["Not_Ready_Startup"]
self.es_accel_msg = copy.copy(cp_cam.vl["ES_CruiseThrottle"])
else:
ret.steerError = cp.vl["Steering_Torque"]['Steer_Error_1'] == 1
ret.steerWarning = cp.vl["Steering_Torque"]['Steer_Warning'] == 1
ret.cruiseState.nonAdaptive = cp_cam.vl["ES_DashStatus"]['Conventional_Cruise'] == 1
self.es_distance_msg = copy.copy(cp_cam.vl["ES_Distance"])
self.es_lkas_msg = copy.copy(cp_cam.vl["ES_LKAS_State"])
return ret
@@ -98,51 +114,121 @@ class CarState(CarStateBase):
checks = [
# sig_address, frequency
("Throttle", 100),
("Dashlights", 10),
("CruiseControl", 20),
("Brake_Pedal", 50),
("Wheel_Speeds", 50),
("Transmission", 100),
("Steering_Torque", 50),
("BodyInfo", 10),
]
if CP.carFingerprint in PREGLOBAL_CARS:
signals += [
("LKA_Lockout", "Steering_Torque", 0),
]
else:
signals += [
("Steer_Error_1", "Steering_Torque", 0),
("Steer_Warning", "Steering_Torque", 0),
]
checks += [
("Dashlights", 10),
("BodyInfo", 10),
("CruiseControl", 20),
]
if CP.carFingerprint == CAR.FORESTER_PREGLOBAL:
checks += [
("Dashlights", 20),
("BodyInfo", 1),
("CruiseControl", 50),
]
if CP.carFingerprint in [CAR.LEGACY_PREGLOBAL, CAR.OUTBACK_PREGLOBAL, CAR.OUTBACK_PREGLOBAL_2018]:
checks += [
("Dashlights", 10),
("CruiseControl", 50),
]
return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 0)
@staticmethod
def get_cam_can_parser(CP):
signals = [
("Cruise_Set_Speed", "ES_DashStatus", 0),
if CP.carFingerprint in PREGLOBAL_CARS:
signals = [
("Cruise_Set_Speed", "ES_DashStatus", 0),
("Not_Ready_Startup", "ES_DashStatus", 0),
("Counter", "ES_Distance", 0),
("Signal1", "ES_Distance", 0),
("Signal2", "ES_Distance", 0),
("Main", "ES_Distance", 0),
("Signal3", "ES_Distance", 0),
("Throttle_Cruise", "ES_CruiseThrottle", 0),
("Signal1", "ES_CruiseThrottle", 0),
("Cruise_Activated", "ES_CruiseThrottle", 0),
("Signal2", "ES_CruiseThrottle", 0),
("Brake_On", "ES_CruiseThrottle", 0),
("DistanceSwap", "ES_CruiseThrottle", 0),
("Standstill", "ES_CruiseThrottle", 0),
("Signal3", "ES_CruiseThrottle", 0),
("CloseDistance", "ES_CruiseThrottle", 0),
("Signal4", "ES_CruiseThrottle", 0),
("Standstill_2", "ES_CruiseThrottle", 0),
("ES_Error", "ES_CruiseThrottle", 0),
("Signal5", "ES_CruiseThrottle", 0),
("Counter", "ES_CruiseThrottle", 0),
("Signal6", "ES_CruiseThrottle", 0),
("Button", "ES_CruiseThrottle", 0),
("Signal7", "ES_CruiseThrottle", 0),
]
("Counter", "ES_LKAS_State", 0),
("Keep_Hands_On_Wheel", "ES_LKAS_State", 0),
("Empty_Box", "ES_LKAS_State", 0),
("Signal1", "ES_LKAS_State", 0),
("LKAS_ACTIVE", "ES_LKAS_State", 0),
("Signal2", "ES_LKAS_State", 0),
("Backward_Speed_Limit_Menu", "ES_LKAS_State", 0),
("LKAS_ENABLE_3", "ES_LKAS_State", 0),
("Signal3", "ES_LKAS_State", 0),
("LKAS_ENABLE_2", "ES_LKAS_State", 0),
("Signal4", "ES_LKAS_State", 0),
("LKAS_Left_Line_Visible", "ES_LKAS_State", 0),
("Signal6", "ES_LKAS_State", 0),
("LKAS_Right_Line_Visible", "ES_LKAS_State", 0),
("Signal7", "ES_LKAS_State", 0),
("FCW_Cont_Beep", "ES_LKAS_State", 0),
("FCW_Repeated_Beep", "ES_LKAS_State", 0),
("Throttle_Management_Activated", "ES_LKAS_State", 0),
("Traffic_light_Ahead", "ES_LKAS_State", 0),
("Right_Depart", "ES_LKAS_State", 0),
("Signal5", "ES_LKAS_State", 0),
]
checks = [
("ES_DashStatus", 20),
("ES_CruiseThrottle", 20),
]
else:
signals = [
("Cruise_Set_Speed", "ES_DashStatus", 0),
("Conventional_Cruise", "ES_DashStatus", 0),
checks = [
("ES_DashStatus", 10),
]
("Counter", "ES_Distance", 0),
("Signal1", "ES_Distance", 0),
("Cruise_Fault", "ES_Distance", 0),
("Cruise_Throttle", "ES_Distance", 0),
("Signal2", "ES_Distance", 0),
("Car_Follow", "ES_Distance", 0),
("Signal3", "ES_Distance", 0),
("Cruise_Brake_Active", "ES_Distance", 0),
("Distance_Swap", "ES_Distance", 0),
("Cruise_EPB", "ES_Distance", 0),
("Signal4", "ES_Distance", 0),
("Close_Distance", "ES_Distance", 0),
("Signal5", "ES_Distance", 0),
("Cruise_Cancel", "ES_Distance", 0),
("Cruise_Set", "ES_Distance", 0),
("Cruise_Resume", "ES_Distance", 0),
("Signal6", "ES_Distance", 0),
("Counter", "ES_LKAS_State", 0),
("Keep_Hands_On_Wheel", "ES_LKAS_State", 0),
("Empty_Box", "ES_LKAS_State", 0),
("Signal1", "ES_LKAS_State", 0),
("LKAS_ACTIVE", "ES_LKAS_State", 0),
("Signal2", "ES_LKAS_State", 0),
("Backward_Speed_Limit_Menu", "ES_LKAS_State", 0),
("LKAS_ENABLE_3", "ES_LKAS_State", 0),
("LKAS_Left_Line_Light_Blink", "ES_LKAS_State", 0),
("LKAS_ENABLE_2", "ES_LKAS_State", 0),
("LKAS_Right_Line_Light_Blink", "ES_LKAS_State", 0),
("LKAS_Left_Line_Visible", "ES_LKAS_State", 0),
("LKAS_Left_Line_Green", "ES_LKAS_State", 0),
("LKAS_Right_Line_Visible", "ES_LKAS_State", 0),
("LKAS_Right_Line_Green", "ES_LKAS_State", 0),
("LKAS_Alert", "ES_LKAS_State", 0),
("Signal3", "ES_LKAS_State", 0),
]
checks = [
("ES_DashStatus", 10),
("ES_Distance", 20),
("ES_LKAS_State", 10),
]
return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 2)
selfdrive/car/subaru/interface.py
+40 -3
View File
@@ -1,6 +1,6 @@
#!/usr/bin/env python3
from cereal import car
from selfdrive.car.subaru.values import CAR
from selfdrive.car.subaru.values import CAR, PREGLOBAL_CARS
from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint
from selfdrive.car.interfaces import CarInterfaceBase
@@ -11,16 +11,22 @@ class CarInterface(CarInterfaceBase):
return float(accel) / 4.0
@staticmethod
def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False, car_fw=[]): # pylint: disable=dangerous-default-value
def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False, car_fw=None):
ret = CarInterfaceBase.get_std_params(candidate, fingerprint, has_relay)
ret.carName = "subaru"
ret.radarOffCan = True
ret.safetyModel = car.CarParams.SafetyModel.subaru
if candidate in PREGLOBAL_CARS:
ret.safetyModel = car.CarParams.SafetyModel.subaruLegacy
else:
ret.safetyModel = car.CarParams.SafetyModel.subaru
# Subaru port is a community feature, since we don't own one to test
ret.communityFeature = True
ret.dashcamOnly = candidate in PREGLOBAL_CARS
# force openpilot to fake the stock camera, since car harness is not supported yet and old style giraffe (with switches)
# was never released
ret.enableCamera = True
@@ -58,6 +64,37 @@ class CarInterface(CarInterfaceBase):
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0., 14., 23.], [0., 14., 23.]]
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.01, 0.065, 0.2], [0.001, 0.015, 0.025]]
if candidate in [CAR.FORESTER_PREGLOBAL, CAR.OUTBACK_PREGLOBAL_2018]:
ret.safetyParam = 1 # Outback 2018-2019 and Forester have reversed driver torque signal
ret.mass = 1568 + STD_CARGO_KG
ret.wheelbase = 2.67
ret.centerToFront = ret.wheelbase * 0.5
ret.steerRatio = 20 # learned, 14 stock
ret.steerActuatorDelay = 0.1
ret.lateralTuning.pid.kf = 0.000039
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0., 10., 20.], [0., 10., 20.]]
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.01, 0.05, 0.2], [0.003, 0.018, 0.025]]
if candidate == CAR.LEGACY_PREGLOBAL:
ret.mass = 1568 + STD_CARGO_KG
ret.wheelbase = 2.67
ret.centerToFront = ret.wheelbase * 0.5
ret.steerRatio = 12.5 # 14.5 stock
ret.steerActuatorDelay = 0.15
ret.lateralTuning.pid.kf = 0.00005
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0., 20.], [0., 20.]]
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.1, 0.2], [0.01, 0.02]]
if candidate == CAR.OUTBACK_PREGLOBAL:
ret.mass = 1568 + STD_CARGO_KG
ret.wheelbase = 2.67
ret.centerToFront = ret.wheelbase * 0.5
ret.steerRatio = 20 # learned, 14 stock
ret.steerActuatorDelay = 0.1
ret.lateralTuning.pid.kf = 0.000039
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0., 10., 20.], [0., 10., 20.]]
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.01, 0.05, 0.2], [0.003, 0.018, 0.025]]
# TODO: get actual value, for now starting with reasonable value for
# civic and scaling by mass and wheelbase
ret.rotationalInertia = scale_rot_inertia(ret.mass, ret.wheelbase)
selfdrive/car/subaru/subarucan.py
+30 -3
View File
@@ -5,8 +5,7 @@ VisualAlert = car.CarControl.HUDControl.VisualAlert
def create_steering_control(packer, apply_steer, frame, steer_step):
# counts from 0 to 15 then back to 0 + 16 for enable bit
idx = ((frame // steer_step) % 16)
idx = (frame / steer_step) % 16
values = {
"Counter": idx,
@@ -24,7 +23,7 @@ def create_es_distance(packer, es_distance_msg, pcm_cancel_cmd):
values = copy.copy(es_distance_msg)
if pcm_cancel_cmd:
values["Main"] = 1
values["Cruise_Cancel"] = 1
return packer.make_can_msg("ES_Distance", 0, values)
@@ -38,3 +37,31 @@ def create_es_lkas(packer, es_lkas_msg, visual_alert, left_line, right_line):
values["LKAS_Right_Line_Visible"] = int(right_line)
return packer.make_can_msg("ES_LKAS_State", 0, values)
# *** Subaru Pre-global ***
def subaru_preglobal_checksum(packer, values, addr):
dat = packer.make_can_msg(addr, 0, values)[2]
return (sum(dat[:7])) % 256
def create_preglobal_steering_control(packer, apply_steer, frame, steer_step):
idx = (frame / steer_step) % 8
values = {
"Counter": idx,
"LKAS_Command": apply_steer,
"LKAS_Active": 1 if apply_steer != 0 else 0
}
values["Checksum"] = subaru_preglobal_checksum(packer, values, "ES_LKAS")
return packer.make_can_msg("ES_LKAS", 0, values)
def create_es_throttle_control(packer, fake_button, es_accel_msg):
values = copy.copy(es_accel_msg)
values["Button"] = fake_button
values["Checksum"] = subaru_preglobal_checksum(packer, values, "ES_CruiseThrottle")
return packer.make_can_msg("ES_CruiseThrottle", 0, values)
selfdrive/car/subaru/values.py
+53 -7
View File
@@ -8,19 +8,23 @@ class CAR:
ASCENT = "SUBARU ASCENT LIMITED 2019"
IMPREZA = "SUBARU IMPREZA LIMITED 2019"
FORESTER = "SUBARU FORESTER 2019"
FORESTER_PREGLOBAL = "SUBARU FORESTER 2017 - 2018"
LEGACY_PREGLOBAL = "SUBARU LEGACY 2015 - 2018"
OUTBACK_PREGLOBAL = "SUBARU OUTBACK 2015 - 2017"
OUTBACK_PREGLOBAL_2018 = "SUBARU OUTBACK 2018 - 2019"
FINGERPRINTS = {
CAR.ASCENT: [
CAR.ASCENT: [{
# SUBARU ASCENT LIMITED 2019
{
2: 8, 64: 8, 65: 8, 72: 8, 73: 8, 280: 8, 281: 8, 290: 8, 312: 8, 313: 8, 314: 8, 315: 8, 316: 8, 326: 8, 544: 8, 545: 8, 546: 8, 552: 8, 554: 8, 557: 8, 576: 8, 577: 8, 722: 8, 801: 8, 802: 8, 805: 8, 808: 8, 811: 8, 816: 8, 826: 8, 837: 8, 838: 8, 839: 8, 842: 8, 912: 8, 915: 8, 940: 8, 1614: 8, 1617: 8, 1632: 8, 1650: 8, 1657: 8, 1658: 8, 1677: 8, 1722: 8, 1743: 8, 1759: 8, 1785: 5, 1786: 5, 1787: 5, 1788: 8
}],
CAR.IMPREZA: [{
# SUBARU IMPREZA LIMITED 2019
2: 8, 64: 8, 65: 8, 72: 8, 73: 8, 280: 8, 281: 8, 290: 8, 312: 8, 313: 8, 314: 8, 315: 8, 316: 8, 326: 8, 544: 8, 545: 8, 546: 8, 552: 8, 554: 8, 557: 8, 576: 8, 577: 8, 722: 8, 801: 8, 802: 8, 805: 8, 808: 8, 816: 8, 826: 8, 837: 8, 838: 8, 839: 8, 842: 8, 912: 8, 915: 8, 940: 8, 1614: 8, 1617: 8, 1632: 8, 1650: 8, 1657: 8, 1658: 8, 1677: 8, 1697: 8, 1722: 8, 1743: 8, 1759: 8, 1786: 5, 1787: 5, 1788: 8, 1809: 8, 1813: 8, 1817: 8, 1821: 8, 1840: 8, 1848: 8, 1924: 8, 1932: 8, 1952: 8, 1960: 8
},
# Crosstrek 2018 (same platform as Impreza)
# SUBARU CROSSTREK 2018
{
2: 8, 64: 8, 65: 8, 72: 8, 73: 8, 256: 8, 280: 8, 281: 8, 290: 8, 312: 8, 313: 8, 314: 8, 315: 8, 316: 8, 326: 8, 372: 8, 544: 8, 545: 8, 546: 8, 554: 8, 557: 8, 576: 8, 577: 8, 722: 8, 801: 8, 802: 8, 805: 8, 808: 8, 811: 8, 826: 8, 837: 8, 838: 8, 839: 8, 842: 8, 912: 8, 915: 8, 940: 8, 1614: 8, 1617: 8, 1632: 8, 1650: 8, 1657: 8, 1658: 8, 1677: 8, 1697: 8, 1759: 8, 1786: 5, 1787: 5, 1788: 8
2: 8, 64: 8, 65: 8, 72: 8, 73: 8, 280: 8, 281: 8, 290: 8, 312: 8, 313: 8, 314: 8, 315: 8, 316: 8, 326: 8, 372: 8, 544: 8, 545: 8, 546: 8, 554: 8, 557: 8, 576: 8, 577: 8, 722: 8, 801: 8, 802: 8, 805: 8, 808: 8, 811: 8, 826: 8, 837: 8, 838: 8, 839: 8, 842: 8, 912: 8, 915: 8, 940: 8, 1614: 8, 1617: 8, 1632: 8, 1650: 8, 1657: 8, 1658: 8, 1677: 8, 1697: 8, 1759: 8, 1786: 5, 1787: 5, 1788: 8
}],
CAR.FORESTER: [{
# Forester Sport 2019
@@ -30,12 +34,48 @@ FINGERPRINTS = {
{
2: 8, 64: 8, 65: 8, 72: 8, 73: 8, 280: 8, 281: 8, 282: 8, 290: 8, 312: 8, 313: 8, 314: 8, 315: 8, 316: 8, 326: 8, 372: 8, 544: 8, 545: 8, 546: 8, 554: 8, 557: 8, 576: 8, 577: 8, 722: 8, 801: 8, 802: 8, 803: 8, 805: 8, 808: 8, 811: 8, 826: 8, 837: 8, 838: 8, 839: 8, 842: 8, 912: 8, 915: 8, 940: 8, 1614: 8, 1617: 8, 1632: 8, 1650: 8, 1651: 8, 1657: 8, 1658: 8, 1677: 8, 1722: 8, 1759: 8, 1787: 5, 1788: 8
}],
CAR.OUTBACK_PREGLOBAL: [{
# OUTBACK PREMIUM 2.5i 2015
2: 8, 208: 8, 209: 4, 210: 8, 211: 7, 212: 8, 320: 8, 321: 8, 324: 8, 328: 8, 329: 8, 336: 2, 338: 8, 342: 8, 346: 8, 352: 8, 353: 8, 354: 8, 356: 8, 358: 8, 359: 8, 392: 8, 640: 8, 642: 8, 644: 8, 864: 8, 865: 8, 866: 8, 872: 8, 880: 8, 881: 8, 882: 8, 884: 8, 977: 8, 1632: 8, 1745: 8, 1786: 5, 1882: 8, 2015: 8, 2016: 8, 2024: 8, 604: 8, 885: 8, 1788: 8, 316: 8, 1614: 8, 1640: 8, 1657: 8, 1658: 8, 1672: 8, 1743: 8, 1785: 5, 1787: 5
},
# OUTBACK PREMIUM 3.6i 2015
{
2: 8, 208: 8, 209: 4, 210: 8, 211: 7, 212: 8, 320: 8, 321: 8, 324: 8, 328: 8, 329: 8, 336: 2, 338: 8, 342: 8, 392: 8, 604: 8, 640: 8, 642: 8, 644: 8, 864: 8, 865: 8, 866: 8, 872: 8, 880: 8, 881: 8, 882: 8, 884: 8, 977: 8, 1632: 8, 1745: 8, 1779: 8, 1786: 5
},
# OUTBACK LIMITED 2.5i 2018
{
2: 8, 208: 8, 209: 4, 210: 8, 211: 7, 212: 8, 316: 8, 320: 8, 321: 8, 324: 8, 328: 8, 329: 8, 336: 2, 338: 8, 342: 8, 352: 8, 353: 8, 354: 8, 356: 8, 358: 8, 359: 8, 392: 8, 554: 8, 604: 8, 640: 8, 642: 8, 644: 8, 805: 8, 864: 8, 865: 8, 866: 8, 872: 8, 880: 8, 881: 8, 882: 8, 884: 8, 885: 8, 977: 8, 1614: 8, 1632: 8, 1657: 8, 1658: 8, 1672: 8, 1722: 8, 1736: 8, 1743: 8, 1745: 8, 1785: 5, 1786: 5, 1787: 5, 1788: 8
}],
CAR.OUTBACK_PREGLOBAL_2018: [{
# OUTBACK LIMITED 3.6R 2019
2: 8, 208: 8, 209: 4, 210: 8, 211: 7, 212: 8, 316: 8, 320: 8, 321: 8, 324: 8, 328: 8, 329: 8, 336: 2, 338: 8, 342: 8, 352: 8, 353: 8, 354: 8, 356: 8, 358: 8, 359: 8, 392: 8, 554: 8, 604: 8, 640: 8, 642: 8, 644: 8, 805: 8, 864: 8, 865: 8, 866: 8, 872: 8, 880: 8, 881: 8, 882: 8, 884: 8, 885: 8, 886: 2, 977: 8, 1614: 8, 1632: 8, 1657: 8, 1658: 8, 1672: 8, 1736: 8, 1743: 8, 1745: 8, 1785: 5, 1786: 5, 1787: 5, 1788: 8, 1862: 8, 1870: 8, 1920: 8, 1927: 8, 1928: 8, 1935: 8, 1968: 8, 1976: 8, 2016: 8, 2017: 8, 2024: 8, 2025: 8
}],
CAR.FORESTER_PREGLOBAL: [{
# FORESTER PREMIUM 2.5i 2017
2: 8, 112: 8, 117: 8, 128: 8, 208: 8, 209: 4, 210: 8, 211: 7, 212: 8, 320: 8, 321: 8, 324: 8, 328: 8, 329: 8, 336: 2, 338: 8, 340: 7, 342: 8, 352: 8, 353: 8, 354: 8, 355: 8, 356: 8, 554: 8, 604: 8, 640: 8, 641: 8, 642: 8, 805: 8, 864: 8, 865: 8, 866: 8, 872: 8, 880: 8, 881: 8, 882: 8, 884: 8, 885: 8, 886: 1, 888: 8, 977: 8, 1398: 8, 1632: 8, 1743: 8, 1744: 8, 1745: 8, 1785: 5, 1786: 5, 1787: 5, 1788: 8, 1882: 8, 1895: 8, 1903: 8, 1986: 8, 1994: 8, 2015: 8, 2016: 8, 2024: 8, 644:8, 890:8, 1736:8
}],
CAR.LEGACY_PREGLOBAL: [{
# LEGACY 2.5i 2017
2: 8, 208: 8, 209: 4, 210: 8, 211: 7, 212: 8, 320: 8, 321: 8, 324: 8, 328: 8, 329: 8, 336: 2, 338: 8, 342: 8, 392: 8, 604: 8, 640: 8, 642: 8, 864: 8, 865: 8, 866: 8, 872: 8, 880: 8, 881: 8, 882: 8, 884: 8, 885: 8, 977: 8, 1632: 8, 1640: 8, 1736: 8, 1745: 8, 1785: 5, 1786: 5, 1787: 5, 1788: 8, 352: 8, 353: 8, 354: 8, 356: 8, 358: 8, 359: 8, 644: 8
},
# LEGACY 2018
{
2: 8, 208: 8, 209: 4, 210: 8, 211: 7, 212: 8, 316: 8, 320: 8, 321: 8, 324: 8, 328: 8, 329: 8, 336: 2, 338: 8, 342: 8, 392: 8, 604: 8, 640: 8, 642: 8, 864: 8, 865: 8, 866: 8, 872: 8, 880: 8, 881: 8, 882: 8, 884: 8, 885: 8, 977: 8, 1614: 8, 1632: 8, 1640: 8, 1657: 8, 1658: 8, 1672: 8, 1722: 8, 1743: 8, 1745: 8, 1778: 8, 1785: 5, 1786: 5, 1787: 5, 1788: 8, 2015: 8, 2016: 8, 2024: 8
},
# LEGACY 2018
{
2: 8, 208: 8, 209: 4, 210: 8, 211: 7, 212: 8, 316: 8, 320: 8, 321: 8, 324: 8, 328: 8, 329: 8, 336: 2, 338: 8, 342: 8, 352: 8, 353: 8, 354: 8, 356: 8, 358: 8, 359: 8, 392: 8, 554: 8, 604: 8, 640: 8, 642: 8, 805: 8, 864: 8, 865: 8, 866: 8, 872: 8, 880: 8, 881: 8, 882: 8, 884: 8, 885: 8, 977: 8, 1614: 8, 1632: 8, 1640: 8, 1657: 8, 1658: 8, 1672: 8, 1722: 8, 1743: 8, 1745: 8, 1785: 5, 1786: 5, 1787: 5, 1788: 8, 2015: 8, 2016: 8, 2024: 8
}],
}
STEER_THRESHOLD = {
CAR.ASCENT: 80,
CAR.IMPREZA: 80,
CAR.FORESTER: 80,
CAR.FORESTER_PREGLOBAL: 75,
CAR.LEGACY_PREGLOBAL: 75,
CAR.OUTBACK_PREGLOBAL: 75,
CAR.OUTBACK_PREGLOBAL_2018: 75,
}
ECU_FINGERPRINT = {
@@ -43,7 +83,13 @@ ECU_FINGERPRINT = {
}
DBC = {
CAR.ASCENT: dbc_dict('subaru_global_2017', None),
CAR.IMPREZA: dbc_dict('subaru_global_2017', None),
CAR.FORESTER: dbc_dict('subaru_global_2017', None),
CAR.ASCENT: dbc_dict('subaru_global_2017_generated', None),
CAR.IMPREZA: dbc_dict('subaru_global_2017_generated', None),
CAR.FORESTER: dbc_dict('subaru_global_2017_generated', None),
CAR.FORESTER_PREGLOBAL: dbc_dict('subaru_forester_2017_generated', None),
CAR.LEGACY_PREGLOBAL: dbc_dict('subaru_outback_2015_generated', None),
CAR.OUTBACK_PREGLOBAL: dbc_dict('subaru_outback_2015_generated', None),
CAR.OUTBACK_PREGLOBAL_2018: dbc_dict('subaru_outback_2019_generated', None),
}
PREGLOBAL_CARS = [CAR.FORESTER_PREGLOBAL, CAR.LEGACY_PREGLOBAL, CAR.OUTBACK_PREGLOBAL, CAR.OUTBACK_PREGLOBAL_2018]
selfdrive/car/tests/test_car_interfaces.py
+1 -1
View File
@@ -63,7 +63,7 @@ class TestCarInterfaces(unittest.TestCase):
# Run radar interface once
radar_interface.update([])
if hasattr(radar_interface, '_update') and hasattr(radar_interface, 'trigger_msg'):
if not car_params.radarOffCan and hasattr(radar_interface, '_update') and hasattr(radar_interface, 'trigger_msg'):
radar_interface._update([radar_interface.trigger_msg])
if __name__ == "__main__":
selfdrive/car/toyota/carstate.py
+7 -2
View File
@@ -182,9 +182,14 @@ class CarState(CarStateBase):
@staticmethod
def get_cam_can_parser(CP):
signals = [("FORCE", "PRE_COLLISION", 0), ("PRECOLLISION_ACTIVE", "PRE_COLLISION", 0)]
signals = [
("FORCE", "PRE_COLLISION", 0),
("PRECOLLISION_ACTIVE", "PRE_COLLISION", 0)
]
# use steering message to check if panda is connected to frc
checks = [("STEERING_LKA", 42)]
checks = [
("STEERING_LKA", 42)
]
return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 2)
selfdrive/car/toyota/interface.py
-1
View File
@@ -313,7 +313,6 @@ class CarInterface(CarInterfaceBase):
ret.canValid = self.cp.can_valid and self.cp_cam.can_valid
ret.steeringRateLimited = self.CC.steer_rate_limited if self.CC is not None else False
ret.buttonEvents = []
# events
events = self.create_common_events(ret)
selfdrive/car/toyota/radar_interface.py
+2 -7
View File
@@ -1,14 +1,10 @@
#!/usr/bin/env python3
import os
import time
from opendbc.can.parser import CANParser
from cereal import car
from selfdrive.car.toyota.values import NO_DSU_CAR, DBC, TSS2_CAR
from selfdrive.car.interfaces import RadarInterfaceBase
def _create_radar_can_parser(car_fingerprint):
dbc_f = DBC[car_fingerprint]['radar']
if car_fingerprint in TSS2_CAR:
RADAR_A_MSGS = list(range(0x180, 0x190))
RADAR_B_MSGS = list(range(0x190, 0x1a0))
@@ -26,7 +22,7 @@ def _create_radar_can_parser(car_fingerprint):
checks = list(zip(RADAR_A_MSGS + RADAR_B_MSGS, [20]*(msg_a_n + msg_b_n)))
return CANParser(os.path.splitext(dbc_f)[0], signals, checks, 1)
return CANParser(DBC[car_fingerprint]['radar'], signals, checks, 1)
class RadarInterface(RadarInterfaceBase):
def __init__(self, CP):
@@ -53,8 +49,7 @@ class RadarInterface(RadarInterfaceBase):
def update(self, can_strings):
if self.no_radar:
time.sleep(self.radar_ts)
return car.RadarData.new_message()
return super().update(None)
vls = self.rcp.update_strings(can_strings)
self.updated_messages.update(vls)
selfdrive/car/toyota/values.py
+28 -8
View File
@@ -341,6 +341,7 @@ FW_VERSIONS = {
b'8646F0603400 ',
b'8646F0605000 ',
b'8646F0606000 ',
b'8646F0606100 ',
],
},
CAR.CAMRYH: {
@@ -614,6 +615,7 @@ FW_VERSIONS = {
b'\x028646F1201100\x00\x00\x00\x008646G26011A0\x00\x00\x00\x00',
b'\x028646F1201300\x00\x00\x00\x008646G2601400\x00\x00\x00\x00',
b'\x028646F1202000\x00\x00\x00\x008646G2601200\x00\x00\x00\x00',
b'\x028646F1202100\x00\x00\x00\x008646G2601400\x00\x00\x00\x00',
b'\x028646F4203400\x00\x00\x00\x008646G2601200\x00\x00\x00\x00',
],
},
@@ -711,15 +713,24 @@ FW_VERSIONS = {
(Ecu.engine, 0x700, None): [
b'\x018966353M7100\x00\x00\x00\x00',
b'\x018966353Q2300\x00\x00\x00\x00',
b'\x018966353R8100\x00\x00\x00\x00',
],
(Ecu.esp, 0x7b0, None): [
b'F152653330\x00\x00\x00\x00\x00\x00',
],
(Ecu.esp, 0x7b0, None): [b'F152653330\x00\x00\x00\x00\x00\x00'],
(Ecu.dsu, 0x791, None): [
b'881515306400\x00\x00\x00\x00',
b'881515306500\x00\x00\x00\x00',
],
(Ecu.eps, 0x7a1, None): [b'8965B53271\x00\x00\x00\x00\x00\x00'],
(Ecu.fwdRadar, 0x750, 0xf): [b'8821F4702300\x00\x00\x00\x00'],
(Ecu.fwdCamera, 0x750, 0x6d): [b'8646F5301400\x00\x00\x00\x00'],
(Ecu.eps, 0x7a1, None): [
b'8965B53271\x00\x00\x00\x00\x00\x00',
],
(Ecu.fwdRadar, 0x750, 0xf): [
b'8821F4702300\x00\x00\x00\x00',
],
(Ecu.fwdCamera, 0x750, 0x6d): [
b'8646F5301400\x00\x00\x00\x00',
],
},
CAR.PRIUS: {
(Ecu.engine, 0x700, None): [
@@ -932,6 +943,7 @@ FW_VERSIONS = {
(Ecu.engine, 0x700, None): [
b'\x018966342M5000\x00\x00\x00\x00',
b'\x018966342X6000\x00\x00\x00\x00',
b'\x018966342W8000\x00\x00\x00\x00',
b'\x028966342W4001\x00\x00\x00\x00897CF1203001\x00\x00\x00\x00',
b'\x02896634A23001\x00\x00\x00\x00897CF1203001\x00\x00\x00\x00',
],
@@ -951,15 +963,17 @@ FW_VERSIONS = {
b'\x028965B0R01300\x00\x00\x00\x008965B0R02300\x00\x00\x00\x00',
],
(Ecu.fwdRadar, 0x750, 0xf): [
b'\x018821F3301100\x00\x00\x00\x00',
b'\x018821F3301200\x00\x00\x00\x00',
b'\x018821F3301300\x00\x00\x00\x00',
b'\x018821F3301400\x00\x00\x00\x00',
],
(Ecu.fwdCamera, 0x750, 0x6d): [
b'\x028646F4203700\x00\x00\x00\x008646G2601400\x00\x00\x00\x00',
b'\x028646F4203200\x00\x00\x00\x008646G26011A0\x00\x00\x00\x00',
b'\x028646F4203300\x00\x00\x00\x008646G26011A0\x00\x00\x00\x00',
b'\x028646F4203400\x00\x00\x00\x008646G2601200\x00\x00\x00\x00',
b'\x028646F4203500\x00\x00\x00\x008646G2601200\x00\x00\x00\x00',
b'\x028646F4203700\x00\x00\x00\x008646G2601400\x00\x00\x00\x00',
],
},
CAR.LEXUS_ES_TSS2: {
@@ -1063,30 +1077,36 @@ FW_VERSIONS = {
b'\x01896630E41000\x00\x00\x00\x00',
b'\x01896630E41200\x00\x00\x00\x00',
b'\x01896630E37300\x00\x00\x00\x00',
b'\x018966348R8500\x00\x00\x00\x00',
],
(Ecu.esp, 0x7b0, None): [
b'F152648472\x00\x00\x00\x00\x00\x00',
b'F152648473\x00\x00\x00\x00\x00\x00',
b'F152648492\x00\x00\x00\x00\x00\x00',
b'F152648493\x00\x00\x00\x00\x00\x00',
b'F152648474\x00\x00\x00\x00\x00\x00',
],
(Ecu.dsu, 0x791, None): [
b'881514810300\x00\x00\x00\x00',
b'881514810500\x00\x00\x00\x00',
b'881514810700\x00\x00\x00\x00',
],
(Ecu.eps, 0x7a1, None): [
b'8965B0E011\x00\x00\x00\x00\x00\x00',
b'8965B0E012\x00\x00\x00\x00\x00\x00',
b'8965B48102\x00\x00\x00\x00\x00\x00',
],
(Ecu.fwdRadar, 0x750, 0xf): [
b'8821F4701000\x00\x00\x00\x00',
b'8821F4701100\x00\x00\x00\x00',
b'8821F4701300\x00\x00\x00\x00',
],
(Ecu.fwdCamera, 0x750, 0x6d): [
b'8646F4801100\x00\x00\x00\x00',
b'8646F4801200\x00\x00\x00\x00',
b'8646F4802001\x00\x00\x00\x00',
b'8646F4802100\x00\x00\x00\x00',
b'8646F4809000\x00\x00\x00\x00',
],
},
CAR.LEXUS_RXH: {
@@ -1198,6 +1218,6 @@ DBC = {
CAR.LEXUS_NXH: dbc_dict('lexus_nx300h_2018_pt_generated', 'toyota_adas'),
}
NO_DSU_CAR = [CAR.CHR, CAR.CHRH, CAR.CAMRY, CAR.CAMRYH, CAR.RAV4_TSS2, CAR.COROLLA_TSS2, CAR.COROLLAH_TSS2, CAR.LEXUS_ES_TSS2, CAR.LEXUS_ESH_TSS2, CAR.RAV4H_TSS2, CAR.LEXUS_RX_TSS2, CAR.LEXUS_RXH_TSS2, CAR.HIGHLANDER_TSS2, CAR.HIGHLANDERH_TSS2]
TSS2_CAR = [CAR.RAV4_TSS2, CAR.COROLLA_TSS2, CAR.COROLLAH_TSS2, CAR.LEXUS_ES_TSS2, CAR.LEXUS_ESH_TSS2, CAR.RAV4H_TSS2, CAR.LEXUS_RX_TSS2, CAR.LEXUS_RXH_TSS2, CAR.HIGHLANDER_TSS2, CAR.HIGHLANDERH_TSS2]
NO_STOP_TIMER_CAR = [CAR.RAV4H, CAR.HIGHLANDERH, CAR.HIGHLANDER, CAR.RAV4_TSS2, CAR.COROLLA_TSS2, CAR.COROLLAH_TSS2, CAR.LEXUS_ES_TSS2, CAR.LEXUS_ESH_TSS2, CAR.SIENNA, CAR.RAV4H_TSS2, CAR.LEXUS_RX_TSS2, CAR.LEXUS_RXH_TSS2, CAR.HIGHLANDER_TSS2, CAR.HIGHLANDERH_TSS2] # no resume button press required
NO_DSU_CAR = set([CAR.CHR, CAR.CHRH, CAR.CAMRY, CAR.CAMRYH, CAR.RAV4_TSS2, CAR.COROLLA_TSS2, CAR.COROLLAH_TSS2, CAR.LEXUS_ES_TSS2, CAR.LEXUS_ESH_TSS2, CAR.RAV4H_TSS2, CAR.LEXUS_RX_TSS2, CAR.LEXUS_RXH_TSS2, CAR.HIGHLANDER_TSS2, CAR.HIGHLANDERH_TSS2])
TSS2_CAR = set([CAR.RAV4_TSS2, CAR.COROLLA_TSS2, CAR.COROLLAH_TSS2, CAR.LEXUS_ES_TSS2, CAR.LEXUS_ESH_TSS2, CAR.RAV4H_TSS2, CAR.LEXUS_RX_TSS2, CAR.LEXUS_RXH_TSS2, CAR.HIGHLANDER_TSS2, CAR.HIGHLANDERH_TSS2])
NO_STOP_TIMER_CAR = set([CAR.RAV4H, CAR.HIGHLANDERH, CAR.HIGHLANDER, CAR.RAV4_TSS2, CAR.COROLLA_TSS2, CAR.COROLLAH_TSS2, CAR.LEXUS_ES_TSS2, CAR.LEXUS_ESH_TSS2, CAR.SIENNA, CAR.RAV4H_TSS2, CAR.LEXUS_RX_TSS2, CAR.LEXUS_RXH_TSS2, CAR.HIGHLANDER_TSS2, CAR.HIGHLANDERH_TSS2]) # no resume button press required
selfdrive/car/volkswagen/carcontroller.py
+1 -3
View File
@@ -4,8 +4,6 @@ from selfdrive.car.volkswagen import volkswagencan
from selfdrive.car.volkswagen.values import DBC, CANBUS, MQB_LDW_MESSAGES, BUTTON_STATES, CarControllerParams
from opendbc.can.packer import CANPacker
VisualAlert = car.CarControl.HUDControl.VisualAlert
class CarController():
def __init__(self, dbc_name, CP, VM):
@@ -114,7 +112,7 @@ class CarController():
if frame % P.LDW_STEP == 0:
hcaEnabled = True if enabled and not CS.out.standstill else False
if visual_alert == VisualAlert.steerRequired:
if visual_alert == car.CarControl.HUDControl.VisualAlert.steerRequired:
hud_alert = MQB_LDW_MESSAGES["laneAssistTakeOverSilent"]
else:
hud_alert = MQB_LDW_MESSAGES["none"]
selfdrive/car/volkswagen/carstate.py
+1 -1
View File
@@ -53,7 +53,7 @@ class CarState(CarStateBase):
pt_cp.vl["Gateway_72"]['ZV_HD_offen']])
# Update seatbelt fastened status.
ret.seatbeltUnlatched = False if pt_cp.vl["Airbag_02"]["AB_Gurtschloss_FA"] == 3 else True
ret.seatbeltUnlatched = pt_cp.vl["Airbag_02"]["AB_Gurtschloss_FA"] != 3
# Update driver preference for metric. VW stores many different unit
# preferences, including separate units for for distance vs. speed.
selfdrive/car/volkswagen/interface.py
+5 -7
View File
@@ -1,6 +1,5 @@
from cereal import car
from selfdrive.car.volkswagen.values import CAR, BUTTON_STATES
from common.params import put_nonblocking
from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint
from selfdrive.car.interfaces import CarInterfaceBase
@@ -19,7 +18,7 @@ class CarInterface(CarInterfaceBase):
return float(accel) / 4.0
@staticmethod
def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False, car_fw=[]): # pylint: disable=dangerous-default-value
def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False, car_fw=None):
ret = CarInterfaceBase.get_std_params(candidate, fingerprint, has_relay)
# VW port is a community feature, since we don't own one to test
@@ -64,7 +63,6 @@ class CarInterface(CarInterfaceBase):
# returns a car.CarState
def update(self, c, can_strings):
canMonoTimes = []
buttonEvents = []
# Process the most recent CAN message traffic, and check for validity
@@ -77,10 +75,11 @@ class CarInterface(CarInterfaceBase):
ret.canValid = self.cp.can_valid and self.cp_cam.can_valid
ret.steeringRateLimited = self.CC.steer_rate_limited if self.CC is not None else False
# Update the EON metric configuration to match the car at first startup,
# TODO: add a field for this to carState, car interface code shouldn't write params
# Update the device metric configuration to match the car at first startup,
# or if there's been a change.
if self.CS.displayMetricUnits != self.displayMetricUnitsPrev:
put_nonblocking("IsMetric", "1" if self.CS.displayMetricUnits else "0")
#if self.CS.displayMetricUnits != self.displayMetricUnitsPrev:
# put_nonblocking("IsMetric", "1" if self.CS.displayMetricUnits else "0")
# Check for and process state-change events (button press or release) from
# the turn stalk switch or ACC steering wheel/control stalk buttons.
@@ -101,7 +100,6 @@ class CarInterface(CarInterfaceBase):
ret.events = events.to_msg()
ret.buttonEvents = buttonEvents
ret.canMonoTimes = canMonoTimes
# update previous car states
self.displayMetricUnitsPrev = self.CS.displayMetricUnits
selfdrive/car/volkswagen/values.py
+1 -1
View File
@@ -46,7 +46,7 @@ MQB_LDW_MESSAGES = {
}
class CAR:
GOLF = "Volkswagen Golf"
GOLF = "VOLKSWAGEN GOLF"
FINGERPRINTS = {
CAR.GOLF: [
selfdrive/car/volkswagen/volkswagencan.py
-1
View File
@@ -48,5 +48,4 @@ def create_mqb_acc_buttons_control(packer, bus, buttonStatesToSend, CS, idx):
"GRA_Tip_Stufe_2": CS.graTipStufe2,
"GRA_ButtonTypeInfo": CS.graButtonTypeInfo
}
return packer.make_can_msg("GRA_ACC_01", bus, values, idx)
selfdrive/common/version.h
+1 -1
View File
@@ -1 +1 @@
#define COMMA_VERSION "0.7.7-release"
#define COMMA_VERSION "0.7.8-release"
selfdrive/controls/controlsd.py
+4 -2
View File
@@ -10,7 +10,7 @@ from common.params import Params, put_nonblocking
import cereal.messaging as messaging
from selfdrive.config import Conversions as CV
from selfdrive.boardd.boardd import can_list_to_can_capnp
from selfdrive.car.car_helpers import get_car, get_startup_event
from selfdrive.car.car_helpers import get_car, get_startup_event, get_one_can
from selfdrive.controls.lib.lane_planner import CAMERA_OFFSET
from selfdrive.controls.lib.drive_helpers import update_v_cruise, initialize_v_cruise
from selfdrive.controls.lib.longcontrol import LongControl, STARTING_TARGET_SPEED
@@ -64,7 +64,7 @@ class Controls:
hw_type = messaging.recv_one(self.sm.sock['health']).health.hwType
has_relay = hw_type in [HwType.blackPanda, HwType.uno, HwType.dos]
print("Waiting for CAN messages...")
messaging.get_one_can(self.can_sock)
get_one_can(self.can_sock)
self.CI, self.CP = get_car(self.can_sock, self.pm.sock['sendcan'], has_relay)
@@ -216,6 +216,8 @@ class Controls:
self.events.add(EventName.vehicleModelInvalid)
if not self.sm['liveLocationKalman'].posenetOK:
self.events.add(EventName.posenetInvalid)
if not self.sm['liveLocationKalman'].deviceStable:
self.events.add(EventName.deviceFalling)
if not self.sm['frame'].recoverState < 2:
# counter>=2 is active
self.events.add(EventName.focusRecoverActive)
selfdrive/controls/lib/alertmanager.py
+22 -2
View File
@@ -1,14 +1,34 @@
import os
import copy
import json
from cereal import car, log
from common.basedir import BASEDIR
from common.params import Params
from common.realtime import DT_CTRL
from selfdrive.swaglog import cloudlog
import copy
AlertSize = log.ControlsState.AlertSize
AlertStatus = log.ControlsState.AlertStatus
VisualAlert = car.CarControl.HUDControl.VisualAlert
AudibleAlert = car.CarControl.HUDControl.AudibleAlert
with open(os.path.join(BASEDIR, "selfdrive/controls/lib/alerts_offroad.json")) as f:
OFFROAD_ALERTS = json.load(f)
def set_offroad_alert(alert, show_alert, extra_text=None):
if show_alert:
a = OFFROAD_ALERTS[alert]
if extra_text is not None:
a = copy.copy(OFFROAD_ALERTS[alert])
a['text'] += extra_text
Params().put(alert, json.dumps(a))
else:
Params().delete(alert)
class AlertManager():
def __init__(self):
selfdrive/controls/lib/alerts_offroad.json
+9
View File
@@ -16,6 +16,11 @@
"text": "Connect to internet to check for updates. openpilot won't engage until it connects to internet to check for updates.",
"severity": 1
},
"Offroad_UpdateFailed": {
"text": "Unable to download updates\n",
"severity": 1,
"_comment": "Append the command and error to the text."
},
"Offroad_PandaFirmwareMismatch": {
"text": "Unexpected panda firmware version. System won't start. Reboot your device to reflash panda.",
"severity": 1
@@ -27,5 +32,9 @@
"Offroad_IsTakingSnapshot": {
"text": "Taking camera snapshots. System won't start until finished.",
"severity": 0
},
"Offroad_NeosUpdate": {
"text": "An update to your device's operating system is downloading in the background. You will be prompted to update when it's ready to install.",
"severity": 0
}
}
selfdrive/controls/lib/events.py
+16 -10
View File
@@ -1,5 +1,6 @@
from cereal import log, car
from functools import total_ordering
from cereal import log, car
from common.realtime import DT_CTRL
from selfdrive.config import Conversions as CV
from selfdrive.locationd.calibration_helpers import Filter
@@ -93,6 +94,7 @@ class Events:
ret.append(event)
return ret
@total_ordering
class Alert:
def __init__(self,
alert_text_1,
@@ -136,6 +138,9 @@ class Alert:
def __gt__(self, alert2):
return self.alert_priority > alert2.alert_priority
def __eq__(self, alert2):
return self.alert_priority == alert2.alert_priority
class NoEntryAlert(Alert):
def __init__(self, alert_text_2, audible_alert=AudibleAlert.chimeError,
visual_alert=VisualAlert.none, duration_hud_alert=2.):
@@ -525,15 +530,6 @@ EVENTS = {
duration_hud_alert=0.),
},
EventName.posenetInvalid: {
ET.WARNING: Alert(
"TAKE CONTROL",
"Vision Model Output Uncertain",
AlertStatus.userPrompt, AlertSize.mid,
Priority.LOW, VisualAlert.steerRequired, AudibleAlert.chimeWarning1, .4, 2., 3.),
ET.NO_ENTRY: NoEntryAlert("Vision Model Output Uncertain"),
},
EventName.focusRecoverActive: {
ET.WARNING: Alert(
"TAKE CONTROL",
@@ -654,6 +650,16 @@ EVENTS = {
ET.NO_ENTRY : NoEntryAlert("Driving model lagging"),
},
EventName.posenetInvalid: {
ET.SOFT_DISABLE: SoftDisableAlert("Vision Model Output Uncertain"),
ET.NO_ENTRY: NoEntryAlert("Vision Model Output Uncertain"),
},
EventName.deviceFalling: {
ET.SOFT_DISABLE: SoftDisableAlert("Device Fell Off Mount"),
ET.NO_ENTRY: NoEntryAlert("Device Fell Off Mount"),
},
EventName.lowMemory: {
ET.SOFT_DISABLE: SoftDisableAlert("Low Memory: Reboot Your Device"),
ET.PERMANENT: Alert(
selfdrive/controls/lib/latcontrol_pid.py
+2 -1
View File
@@ -8,7 +8,8 @@ class LatControlPID():
def __init__(self, CP):
self.pid = PIController((CP.lateralTuning.pid.kpBP, CP.lateralTuning.pid.kpV),
(CP.lateralTuning.pid.kiBP, CP.lateralTuning.pid.kiV),
k_f=CP.lateralTuning.pid.kf, pos_limit=1.0, sat_limit=CP.steerLimitTimer)
k_f=CP.lateralTuning.pid.kf, pos_limit=1.0, neg_limit=-1.0,
sat_limit=CP.steerLimitTimer)
self.angle_steers_des = 0.
def reset(self):
selfdrive/controls/lib/longcontrol.py
+1 -1
View File
@@ -85,7 +85,7 @@ class LongControl():
v_ego_pid = max(CS.vEgo, MIN_CAN_SPEED) # Without this we get jumps, CAN bus reports 0 when speed < 0.3
if self.long_control_state == LongCtrlState.off:
if self.long_control_state == LongCtrlState.off or CS.gasPressed:
self.v_pid = v_ego_pid
self.pid.reset()
output_gb = 0.
selfdrive/controls/lib/planner.py
+1 -1
View File
@@ -129,7 +129,7 @@ class Planner():
following = lead_1.status and lead_1.dRel < 45.0 and lead_1.vLeadK > v_ego and lead_1.aLeadK > 0.0
# Calculate speed for normal cruise control
if enabled and not self.first_loop:
if enabled and not self.first_loop and not sm['carState'].gasPressed:
accel_limits = [float(x) for x in calc_cruise_accel_limits(v_ego, following)]
jerk_limits = [min(-0.1, accel_limits[0]), max(0.1, accel_limits[1])] # TODO: make a separate lookup for jerk tuning
accel_limits_turns = limit_accel_in_turns(v_ego, sm['carState'].steeringAngle, accel_limits, self.CP)
selfdrive/debug/cpu_usage_stat.py
+1 -1
View File
@@ -31,7 +31,7 @@ SLEEP_INTERVAL = 0.2
monitored_proc_names = [
'ubloxd', 'thermald', 'uploader', 'deleter', 'controlsd', 'plannerd', 'radard', 'mapd', 'loggerd', 'logmessaged', 'tombstoned',
'logcatd', 'proclogd', 'boardd', 'pandad', './ui', 'ui', 'calibrationd', 'params_learner', 'modeld', 'dmonitoringmodeld', 'camerad', 'sensord', 'updated', 'gpsd', 'athena']
'logcatd', 'proclogd', 'boardd', 'pandad', './ui', 'ui', 'calibrationd', 'params_learner', 'modeld', 'dmonitoringd', 'dmonitoringmodeld', 'camerad', 'sensord', 'updated', 'gpsd', 'athena', 'locationd', 'paramsd']
cpu_time_names = ['user', 'system', 'children_user', 'children_system']
timer = getattr(time, 'monotonic', time.time)
selfdrive/debug/uiview.py
+21
View File
@@ -0,0 +1,21 @@
#!/usr/bin/env python3
import time
import cereal.messaging as messaging
from selfdrive.manager import start_managed_process, kill_managed_process
services = ['controlsState', 'thermal'] # the services needed to be spoofed to start ui offroad
procs = ['camerad', 'ui']
[start_managed_process(p) for p in procs] # start needed processes
pm = messaging.PubMaster(services)
dat_cs, dat_thermal = [messaging.new_message(s) for s in services]
dat_cs.controlsState.rearViewCam = False # ui checks for these two messages
dat_thermal.thermal.started = True
try:
while True:
pm.send('controlsState', dat_cs)
pm.send('thermal', dat_thermal)
time.sleep(1 / 100) # continually send, rate doesn't matter for thermal
except KeyboardInterrupt:
[kill_managed_process(p) for p in procs]
selfdrive/locationd/SConscript
+1 -7
View File
@@ -1,12 +1,6 @@
Import('env', 'common', 'cereal', 'messaging')
loc_objs = [
"locationd_yawrate.cc",
"params_learner.cc",
"paramsd.cc"]
loc_libs = [cereal, messaging, 'zmq', common, 'capnp', 'kj', 'json11', 'pthread']
env.Program("paramsd", loc_objs, LIBS=loc_libs)
env.SharedLibrary("locationd", loc_objs, LIBS=loc_libs)
loc_libs = [cereal, messaging, 'zmq', common, 'capnp', 'kj', 'pthread']
env.Program("ubloxd", [
"ubloxd.cc",
selfdrive/locationd/calibrationd.py
+8 -2
View File
@@ -126,13 +126,21 @@ class Calibrator():
def send_data(self, pm):
calib = get_calib_from_vp(self.vp)
if self.valid_blocks > 0:
max_vp_calib = np.array(get_calib_from_vp(np.max(self.vps[:self.valid_blocks], axis=0)))
min_vp_calib = np.array(get_calib_from_vp(np.min(self.vps[:self.valid_blocks], axis=0)))
calib_spread = np.abs(max_vp_calib - min_vp_calib)
else:
calib_spread = np.zeros(3)
extrinsic_matrix = get_view_frame_from_road_frame(0, calib[1], calib[2], model_height)
cal_send = messaging.new_message('liveCalibration')
cal_send.liveCalibration.validBlocks = self.valid_blocks
cal_send.liveCalibration.calStatus = self.cal_status
cal_send.liveCalibration.calPerc = min(100 * (self.valid_blocks * BLOCK_SIZE + self.idx) // (INPUTS_NEEDED * BLOCK_SIZE), 100)
cal_send.liveCalibration.extrinsicMatrix = [float(x) for x in extrinsic_matrix.flatten()]
cal_send.liveCalibration.rpyCalib = [float(x) for x in calib]
cal_send.liveCalibration.rpyCalibSpread = [float(x) for x in calib_spread]
pm.send('liveCalibration', cal_send)
@@ -170,8 +178,6 @@ def calibrationd_thread(sm=None, pm=None):
if DEBUG and new_vp is not None:
print('got new vp', new_vp)
# decimate outputs for efficiency
def main(sm=None, pm=None):
calibrationd_thread(sm, pm)
selfdrive/locationd/locationd.py
+42 -54
View File
@@ -1,7 +1,6 @@
#!/usr/bin/env python3
import numpy as np
import sympy as sp
import cereal.messaging as messaging
import common.transformations.coordinates as coord
from common.transformations.orientation import ecef_euler_from_ned, \
@@ -23,6 +22,7 @@ from rednose.helpers.sympy_helpers import euler_rotate
VISION_DECIMATION = 2
SENSOR_DECIMATION = 10
POSENET_STD_HIST = 40
def to_float(arr):
@@ -52,7 +52,7 @@ class Localizer():
self.kf = LiveKalman(GENERATED_DIR)
self.reset_kalman()
self.max_age = .2 # seconds
self.max_age = .1 # seconds
self.disabled_logs = disabled_logs
self.calib = np.zeros(3)
self.device_from_calib = np.eye(3)
@@ -63,11 +63,13 @@ class Localizer():
self.posenet_invalid_count = 0
self.posenet_speed = 0
self.car_speed = 0
self.posenet_stds = 10*np.ones((POSENET_STD_HIST))
self.converter = coord.LocalCoord.from_ecef(self.kf.x[States.ECEF_POS])
self.unix_timestamp_millis = 0
self.last_gps_fix = 0
self.device_fell = False
@staticmethod
def msg_from_state(converter, calib_from_device, H, predicted_state, predicted_cov):
@@ -112,57 +114,42 @@ class Localizer():
#ned_vel_std = self.converter.ecef2ned(fix_ecef + vel_ecef + vel_ecef_std) - self.converter.ecef2ned(fix_ecef + vel_ecef)
fix = messaging.log.LiveLocationKalman.new_message()
fix.positionGeodetic.value = to_float(fix_pos_geo)
#fix.positionGeodetic.std = to_float(fix_pos_geo_std)
#fix.positionGeodetic.valid = True
fix.positionECEF.value = to_float(fix_ecef)
fix.positionECEF.std = to_float(fix_ecef_std)
fix.positionECEF.valid = True
fix.velocityECEF.value = to_float(vel_ecef)
fix.velocityECEF.std = to_float(vel_ecef_std)
fix.velocityECEF.valid = True
fix.velocityNED.value = to_float(ned_vel)
#fix.velocityNED.std = to_float(ned_vel_std)
#fix.velocityNED.valid = True
fix.velocityDevice.value = to_float(vel_device)
fix.velocityDevice.std = to_float(vel_device_std)
fix.velocityDevice.valid = True
fix.accelerationDevice.value = to_float(predicted_state[States.ACCELERATION])
fix.accelerationDevice.std = to_float(predicted_std[States.ACCELERATION_ERR])
fix.accelerationDevice.valid = True
fix.orientationECEF.value = to_float(orientation_ecef)
fix.orientationECEF.std = to_float(orientation_ecef_std)
fix.orientationECEF.valid = True
fix.calibratedOrientationECEF.value = to_float(calibrated_orientation_ecef)
#fix.calibratedOrientationECEF.std = to_float(calibrated_orientation_ecef_std)
#fix.calibratedOrientationECEF.valid = True
fix.orientationNED.value = to_float(orientation_ned)
#fix.orientationNED.std = to_float(orientation_ned_std)
#fix.orientationNED.valid = True
fix.angularVelocityDevice.value = to_float(predicted_state[States.ANGULAR_VELOCITY])
fix.angularVelocityDevice.std = to_float(predicted_std[States.ANGULAR_VELOCITY_ERR])
fix.angularVelocityDevice.valid = True
# write measurements to msg
measurements = [
# measurement field, value, std, valid
(fix.positionGeodetic, fix_pos_geo, np.nan*np.zeros(3), True),
(fix.positionECEF, fix_ecef, fix_ecef_std, True),
(fix.velocityECEF, vel_ecef, vel_ecef_std, True),
(fix.velocityNED, ned_vel, np.nan*np.zeros(3), True),
(fix.velocityDevice, vel_device, vel_device_std, True),
(fix.accelerationDevice, predicted_state[States.ACCELERATION], predicted_std[States.ACCELERATION_ERR], True),
(fix.orientationECEF, orientation_ecef, orientation_ecef_std, True),
(fix.calibratedOrientationECEF, calibrated_orientation_ecef, np.nan*np.zeros(3), True),
(fix.orientationNED, orientation_ned, np.nan*np.zeros(3), True),
(fix.angularVelocityDevice, predicted_state[States.ANGULAR_VELOCITY], predicted_std[States.ANGULAR_VELOCITY_ERR], True),
(fix.velocityCalibrated, vel_calib, vel_calib_std, True),
(fix.angularVelocityCalibrated, ang_vel_calib, ang_vel_calib_std, True),
(fix.accelerationCalibrated, acc_calib, acc_calib_std, True),
]
for field, value, std, valid in measurements:
# TODO: can we write the lists faster?
field.value = to_float(value)
field.std = to_float(std)
field.valid = valid
fix.velocityCalibrated.value = to_float(vel_calib)
fix.velocityCalibrated.std = to_float(vel_calib_std)
fix.velocityCalibrated.valid = True
fix.angularVelocityCalibrated.value = to_float(ang_vel_calib)
fix.angularVelocityCalibrated.std = to_float(ang_vel_calib_std)
fix.angularVelocityCalibrated.valid = True
fix.accelerationCalibrated.value = to_float(acc_calib)
fix.accelerationCalibrated.std = to_float(acc_calib_std)
fix.accelerationCalibrated.valid = True
return fix
def liveLocationMsg(self, time):
def liveLocationMsg(self):
fix = self.msg_from_state(self.converter, self.calib_from_device, self.H, self.kf.x, self.kf.P)
# experimentally found these values, no false positives in 20k minutes of driving
old_mean, new_mean = np.mean(self.posenet_stds[:POSENET_STD_HIST//2]), np.mean(self.posenet_stds[POSENET_STD_HIST//2:])
std_spike = new_mean/old_mean > 4 and new_mean > 7
if abs(self.posenet_speed - self.car_speed) > max(0.4 * self.car_speed, 5.0):
self.posenet_invalid_count += 1
else:
self.posenet_invalid_count = 0
fix.posenetOK = self.posenet_invalid_count < 4
fix.posenetOK = not (std_spike and self.car_speed > 5)
fix.deviceStable = not self.device_fell
self.device_fell = False
#fix.gpsWeek = self.time.week
#fix.gpsTimeOfWeek = self.time.tow
@@ -178,15 +165,10 @@ class Localizer():
def update_kalman(self, time, kind, meas, R=None):
try:
self.kf.predict_and_observe(time, kind, meas, R=R)
self.kf.predict_and_observe(time, kind, meas, R)
except KalmanError:
cloudlog.error("Error in predict and observe, kalman reset")
self.reset_kalman()
#idx = bisect_right([x[0] for x in self.observation_buffer], time)
#self.observation_buffer.insert(idx, (time, kind, meas))
#while len(self.observation_buffer) > 0 and self.observation_buffer[-1][0] - self.observation_buffer[0][0] > self.max_age:
# else:
# self.observation_buffer.pop(0)
def handle_gps(self, current_time, log):
# ignore the message if the fix is invalid
@@ -244,6 +226,8 @@ class Localizer():
trans_device = self.device_from_calib.dot(log.trans)
trans_device_std = self.device_from_calib.dot(log.transStd)
self.posenet_speed = np.linalg.norm(trans_device)
self.posenet_stds[:-1] = self.posenet_stds[1:]
self.posenet_stds[-1] = trans_device_std[0]
self.update_kalman(current_time,
ObservationKind.CAMERA_ODO_TRANSLATION,
np.concatenate([trans_device, 10*trans_device_std]))
@@ -260,6 +244,10 @@ class Localizer():
# Accelerometer
if sensor_reading.sensor == 1 and sensor_reading.type == 1:
# check if device fell, estimate 10 for g
# 40m/s**2 is a good filter for falling detection, no false positives in 20k minutes of driving
self.device_fell = self.device_fell or (np.linalg.norm(np.array(sensor_reading.acceleration.v) - np.array([10, 0, 0])) > 40)
self.acc_counter += 1
if self.acc_counter % SENSOR_DECIMATION == 0:
v = sensor_reading.acceleration.v
@@ -322,7 +310,7 @@ def locationd_thread(sm, pm, disabled_logs=None):
msg = messaging.new_message('liveLocationKalman')
msg.logMonoTime = t
msg.liveLocationKalman = localizer.liveLocationMsg(t * 1e-9)
msg.liveLocationKalman = localizer.liveLocationMsg()
msg.liveLocationKalman.inputsOK = sm.all_alive_and_valid()
msg.liveLocationKalman.sensorsOK = sm.alive['sensorEvents'] and sm.valid['sensorEvents']
selfdrive/locationd/locationd_yawrate.cc
-150
View File
@@ -1,150 +0,0 @@
#include <iostream>
#include <cmath>
#include <capnp/message.h>
#include <capnp/serialize-packed.h>
#include <eigen3/Eigen/Dense>
#include "cereal/gen/cpp/log.capnp.h"
#include "locationd_yawrate.h"
void Localizer::update_state(const Eigen::Matrix<double, 1, 2> &C, const double R, double current_time, double meas) {
double dt = current_time - prev_update_time;
if (dt < 0) {
dt = 0;
} else {
prev_update_time = current_time;
}
x = A * x;
P = A * P * A.transpose() + dt * Q;
double y = meas - C * x;
double S = R + C * P * C.transpose();
Eigen::Vector2d K = P * C.transpose() * (1.0 / S);
x = x + K * y;
P = (I - K * C) * P;
}
void Localizer::handle_sensor_events(capnp::List<cereal::SensorEventData>::Reader sensor_events, double current_time) {
for (cereal::SensorEventData::Reader sensor_event : sensor_events){
if (sensor_event.getSensor() == 5 && sensor_event.getType() == 16) {
double meas = -sensor_event.getGyroUncalibrated().getV()[0];
update_state(C_gyro, R_gyro, current_time, meas);
}
}
}
void Localizer::handle_camera_odometry(cereal::CameraOdometry::Reader camera_odometry, double current_time) {
double R = pow(5 * camera_odometry.getRotStd()[2], 2);
double meas = camera_odometry.getRot()[2];
update_state(C_posenet, R, current_time, meas);
}
void Localizer::handle_controls_state(cereal::ControlsState::Reader controls_state, double current_time) {
steering_angle = controls_state.getAngleSteers() * DEGREES_TO_RADIANS;
car_speed = controls_state.getVEgo();
}
Localizer::Localizer() {
// States: [yaw rate, gyro bias]
A <<
1, 0,
0, 1;
Q <<
pow(.1, 2.0), 0,
0, pow(0.05/ 100.0, 2.0),
P <<
pow(10000.0, 2.0), 0,
0, pow(10000.0, 2.0);
I <<
1, 0,
0, 1;
C_posenet << 1, 0;
C_gyro << 1, 1;
x << 0, 0;
R_gyro = pow(0.025, 2.0);
}
void Localizer::handle_log(cereal::Event::Reader event) {
double current_time = event.getLogMonoTime() / 1.0e9;
// Initialize update_time on first update
if (prev_update_time < 0) {
prev_update_time = current_time;
}
auto type = event.which();
switch(type) {
case cereal::Event::CONTROLS_STATE:
handle_controls_state(event.getControlsState(), current_time);
break;
case cereal::Event::CAMERA_ODOMETRY:
handle_camera_odometry(event.getCameraOdometry(), current_time);
break;
case cereal::Event::SENSOR_EVENTS:
handle_sensor_events(event.getSensorEvents(), current_time);
break;
default:
break;
}
}
extern "C" {
void *localizer_init(void) {
Localizer * localizer = new Localizer;
return (void*)localizer;
}
void localizer_handle_log(void * localizer, const unsigned char * data, size_t len) {
const kj::ArrayPtr<const capnp::word> view((const capnp::word*)data, len);
capnp::FlatArrayMessageReader msg(view);
cereal::Event::Reader event = msg.getRoot<cereal::Event>();
Localizer * loc = (Localizer*) localizer;
loc->handle_log(event);
}
double localizer_get_yaw(void * localizer) {
Localizer * loc = (Localizer*) localizer;
return loc->x[0];
}
double localizer_get_bias(void * localizer) {
Localizer * loc = (Localizer*) localizer;
return loc->x[1];
}
double * localizer_get_state(void * localizer) {
Localizer * loc = (Localizer*) localizer;
return loc->x.data();
}
void localizer_set_state(void * localizer, double * state) {
Localizer * loc = (Localizer*) localizer;
memcpy(loc->x.data(), state, 4 * sizeof(double));
}
double localizer_get_t(void * localizer) {
Localizer * loc = (Localizer*) localizer;
return loc->prev_update_time;
}
double * localizer_get_P(void * localizer) {
Localizer * loc = (Localizer*) localizer;
return loc->P.data();
}
void localizer_set_P(void * localizer, double * P) {
Localizer * loc = (Localizer*) localizer;
memcpy(loc->P.data(), P, 16 * sizeof(double));
}
}
selfdrive/locationd/locationd_yawrate.h
-33
View File
@@ -1,33 +0,0 @@
#pragma once
#include <eigen3/Eigen/Dense>
#include "cereal/gen/cpp/log.capnp.h"
#define DEGREES_TO_RADIANS 0.017453292519943295
class Localizer
{
Eigen::Matrix2d A;
Eigen::Matrix2d I;
Eigen::Matrix2d Q;
Eigen::Matrix<double, 1, 2> C_posenet;
Eigen::Matrix<double, 1, 2> C_gyro;
double R_gyro;
void update_state(const Eigen::Matrix<double, 1, 2> &C, const double R, double current_time, double meas);
void handle_sensor_events(capnp::List<cereal::SensorEventData>::Reader sensor_events, double current_time);
void handle_camera_odometry(cereal::CameraOdometry::Reader camera_odometry, double current_time);
void handle_controls_state(cereal::ControlsState::Reader controls_state, double current_time);
public:
Eigen::Vector2d x;
Eigen::Matrix2d P;
double steering_angle = 0;
double car_speed = 0;
double prev_update_time = -1;
Localizer();
void handle_log(cereal::Event::Reader event);
};
selfdrive/locationd/models/live_kf.py
+1 -1
View File
@@ -206,7 +206,7 @@ class LiveKalman():
ObservationKind.ECEF_ORIENTATION_FROM_GPS: np.diag([.2**2, .2**2, .2**2, .2**2])}
# init filter
self.filter = EKF_sym(generated_dir, self.name, self.Q, self.initial_x, np.diag(self.initial_P_diag), self.dim_state, self.dim_state_err)
self.filter = EKF_sym(generated_dir, self.name, self.Q, self.initial_x, np.diag(self.initial_P_diag), self.dim_state, self.dim_state_err, max_rewind_age=0.2)
@property
def x(self):
selfdrive/locationd/params_learner.cc
-118
View File
@@ -1,118 +0,0 @@
#include <algorithm>
#include <cmath>
#include <iostream>
#include <capnp/message.h>
#include <capnp/serialize-packed.h>
#include "cereal/gen/cpp/log.capnp.h"
#include "cereal/gen/cpp/car.capnp.h"
#include "params_learner.h"
// #define DEBUG
template <typename T>
T clip(const T& n, const T& lower, const T& upper) {
return std::max(lower, std::min(n, upper));
}
ParamsLearner::ParamsLearner(cereal::CarParams::Reader car_params,
double angle_offset,
double stiffness_factor,
double steer_ratio,
double learning_rate) :
ao(angle_offset * DEGREES_TO_RADIANS),
slow_ao(angle_offset * DEGREES_TO_RADIANS),
x(stiffness_factor),
sR(steer_ratio) {
cF0 = car_params.getTireStiffnessFront();
cR0 = car_params.getTireStiffnessRear();
l = car_params.getWheelbase();
m = car_params.getMass();
aF = car_params.getCenterToFront();
aR = l - aF;
min_sr = MIN_SR * car_params.getSteerRatio();
max_sr = MAX_SR * car_params.getSteerRatio();
min_sr_th = MIN_SR_TH * car_params.getSteerRatio();
max_sr_th = MAX_SR_TH * car_params.getSteerRatio();
alpha1 = 0.01 * learning_rate;
alpha2 = 0.0005 * learning_rate;
alpha3 = 0.1 * learning_rate;
alpha4 = 1.0 * learning_rate;
}
bool ParamsLearner::update(double psi, double u, double sa) {
if (u > 10.0 && fabs(sa) < (DEGREES_TO_RADIANS * 90.)) {
double ao_diff = 2.0*cF0*cR0*l*u*x*(1.0*cF0*cR0*l*u*x*(ao - sa) + psi*sR*(cF0*cR0*pow(l, 2)*x - m*pow(u, 2)*(aF*cF0 - aR*cR0)))/(pow(sR, 2)*pow(cF0*cR0*pow(l, 2)*x - m*pow(u, 2)*(aF*cF0 - aR*cR0), 2));
double new_ao = ao - alpha1 * ao_diff;
double slow_ao_diff = 2.0*cF0*cR0*l*u*x*(1.0*cF0*cR0*l*u*x*(slow_ao - sa) + psi*sR*(cF0*cR0*pow(l, 2)*x - m*pow(u, 2)*(aF*cF0 - aR*cR0)))/(pow(sR, 2)*pow(cF0*cR0*pow(l, 2)*x - m*pow(u, 2)*(aF*cF0 - aR*cR0), 2));
double new_slow_ao = slow_ao - alpha2 * slow_ao_diff;
double new_x = x - alpha3 * (-2.0*cF0*cR0*l*m*pow(u, 3)*(slow_ao - sa)*(aF*cF0 - aR*cR0)*(1.0*cF0*cR0*l*u*x*(slow_ao - sa) + psi*sR*(cF0*cR0*pow(l, 2)*x - m*pow(u, 2)*(aF*cF0 - aR*cR0)))/(pow(sR, 2)*pow(cF0*cR0*pow(l, 2)*x - m*pow(u, 2)*(aF*cF0 - aR*cR0), 3)));
double new_sR = sR - alpha4 * (-2.0*cF0*cR0*l*u*x*(slow_ao - sa)*(1.0*cF0*cR0*l*u*x*(slow_ao - sa) + psi*sR*(cF0*cR0*pow(l, 2)*x - m*pow(u, 2)*(aF*cF0 - aR*cR0)))/(pow(sR, 3)*pow(cF0*cR0*pow(l, 2)*x - m*pow(u, 2)*(aF*cF0 - aR*cR0), 2)));
ao = new_ao;
slow_ao = new_slow_ao;
x = new_x;
sR = new_sR;
}
#ifdef DEBUG
std::cout << "Instant AO: " << (RADIANS_TO_DEGREES * ao) << "\tAverage AO: " << (RADIANS_TO_DEGREES * slow_ao);
std::cout << "\tStiffness: " << x << "\t sR: " << sR << std::endl;
#endif
ao = clip(ao, -MAX_ANGLE_OFFSET, MAX_ANGLE_OFFSET);
slow_ao = clip(slow_ao, -MAX_ANGLE_OFFSET, MAX_ANGLE_OFFSET);
x = clip(x, MIN_STIFFNESS, MAX_STIFFNESS);
sR = clip(sR, min_sr, max_sr);
bool valid = fabs(slow_ao) < MAX_ANGLE_OFFSET_TH;
valid = valid && sR > min_sr_th;
valid = valid && sR < max_sr_th;
return valid;
}
extern "C" {
void *params_learner_init(size_t len, char * params, double angle_offset, double stiffness_factor, double steer_ratio, double learning_rate) {
auto amsg = kj::heapArray<capnp::word>((len / sizeof(capnp::word)) + 1);
memcpy(amsg.begin(), params, len);
capnp::FlatArrayMessageReader cmsg(amsg);
cereal::CarParams::Reader car_params = cmsg.getRoot<cereal::CarParams>();
ParamsLearner * p = new ParamsLearner(car_params, angle_offset, stiffness_factor, steer_ratio, learning_rate);
return (void*)p;
}
bool params_learner_update(void * params_learner, double psi, double u, double sa) {
ParamsLearner * p = (ParamsLearner*) params_learner;
return p->update(psi, u, sa);
}
double params_learner_get_ao(void * params_learner){
ParamsLearner * p = (ParamsLearner*) params_learner;
return p->ao;
}
double params_learner_get_x(void * params_learner){
ParamsLearner * p = (ParamsLearner*) params_learner;
return p->x;
}
double params_learner_get_slow_ao(void * params_learner){
ParamsLearner * p = (ParamsLearner*) params_learner;
return p->slow_ao;
}
double params_learner_get_sR(void * params_learner){
ParamsLearner * p = (ParamsLearner*) params_learner;
return p->sR;
}
}
selfdrive/locationd/params_learner.h
-35
View File
@@ -1,35 +0,0 @@
#pragma once
#define DEGREES_TO_RADIANS 0.017453292519943295
#define RADIANS_TO_DEGREES (1.0 / DEGREES_TO_RADIANS)
#define MAX_ANGLE_OFFSET (10.0 * DEGREES_TO_RADIANS)
#define MAX_ANGLE_OFFSET_TH (9.0 * DEGREES_TO_RADIANS)
#define MIN_STIFFNESS 0.5
#define MAX_STIFFNESS 2.0
#define MIN_SR 0.5
#define MAX_SR 2.0
#define MIN_SR_TH 0.55
#define MAX_SR_TH 1.9
class ParamsLearner {
double cF0, cR0;
double aR, aF;
double l, m;
double min_sr, max_sr, min_sr_th, max_sr_th;
double alpha1, alpha2, alpha3, alpha4;
public:
double ao;
double slow_ao;
double x, sR;
ParamsLearner(cereal::CarParams::Reader car_params,
double angle_offset,
double stiffness_factor,
double steer_ratio,
double learning_rate);
bool update(double psi, double u, double sa);
};
selfdrive/locationd/paramsd.cc
-135
View File
@@ -1,135 +0,0 @@
#include <future>
#include <iostream>
#include <cassert>
#include <csignal>
#include <unistd.h>
#include <capnp/serialize-packed.h>
#include "json11.hpp"
#include "common/swaglog.h"
#include "common/params.h"
#include "common/timing.h"
#include "messaging.hpp"
#include "locationd_yawrate.h"
#include "params_learner.h"
#include "common/util.h"
void sigpipe_handler(int sig) {
LOGE("SIGPIPE received");
}
int main(int argc, char *argv[]) {
signal(SIGPIPE, (sighandler_t)sigpipe_handler);
SubMaster sm({"controlsState", "sensorEvents", "cameraOdometry"});
PubMaster pm({"liveParameters"});
Localizer localizer;
// Read car params
std::vector<char> params;
LOGW("waiting for params to set vehicle model");
while (true) {
params = read_db_bytes("CarParams");
if (params.size() > 0) break;
usleep(100*1000);
}
LOGW("got %d bytes CarParams", params.size());
// make copy due to alignment issues
auto amsg = kj::heapArray<capnp::word>((params.size() / sizeof(capnp::word)) + 1);
memcpy(amsg.begin(), params.data(), params.size());
capnp::FlatArrayMessageReader cmsg(amsg);
cereal::CarParams::Reader car_params = cmsg.getRoot<cereal::CarParams>();
// Read params from previous run
std::string fingerprint = car_params.getCarFingerprint();
std::string vin = car_params.getCarVin();
double sR = car_params.getSteerRatio();
double x = 1.0;
double ao = 0.0;
std::vector<char> live_params = read_db_bytes("LiveParameters");
if (live_params.size() > 0){
std::string err;
std::string str(live_params.begin(), live_params.end());
auto json = json11::Json::parse(str, err);
if (json.is_null() || !err.empty()) {
std::string log = "Error parsing json: " + err;
LOGW(log.c_str());
} else {
std::string new_fingerprint = json["carFingerprint"].string_value();
std::string new_vin = json["carVin"].string_value();
if (fingerprint == new_fingerprint && vin == new_vin) {
std::string log = "Parameter starting with: " + str;
LOGW(log.c_str());
sR = json["steerRatio"].number_value();
x = json["stiffnessFactor"].number_value();
ao = json["angleOffsetAverage"].number_value();
}
}
}
ParamsLearner learner(car_params, ao, x, sR, 1.0);
// Main loop
int save_counter = 0;
while (true){
if (sm.update(100) == 0) continue;
if (sm.updated("controlsState")){
localizer.handle_log(sm["controlsState"]);
save_counter++;
double yaw_rate = -localizer.x[0];
bool valid = learner.update(yaw_rate, localizer.car_speed, localizer.steering_angle);
double angle_offset_degrees = RADIANS_TO_DEGREES * learner.ao;
double angle_offset_average_degrees = RADIANS_TO_DEGREES * learner.slow_ao;
capnp::MallocMessageBuilder msg;
cereal::Event::Builder event = msg.initRoot<cereal::Event>();
event.setLogMonoTime(nanos_since_boot());
auto live_params = event.initLiveParameters();
live_params.setValid(valid);
live_params.setYawRate(localizer.x[0]);
live_params.setGyroBias(localizer.x[1]);
live_params.setAngleOffset(angle_offset_degrees);
live_params.setAngleOffsetAverage(angle_offset_average_degrees);
live_params.setStiffnessFactor(learner.x);
live_params.setSteerRatio(learner.sR);
pm.send("liveParameters", msg);
// Save parameters every minute
if (save_counter % 6000 == 0) {
json11::Json json = json11::Json::object {
{"carVin", vin},
{"carFingerprint", fingerprint},
{"steerRatio", learner.sR},
{"stiffnessFactor", learner.x},
{"angleOffsetAverage", angle_offset_average_degrees},
};
std::string out = json.dump();
std::async(std::launch::async,
[out]{
write_db_value("LiveParameters", out.c_str(), out.length());
});
}
}
if (sm.updated("sensorEvents")){
localizer.handle_log(sm["sensorEvents"]);
}
if (sm.updated("cameraOdometry")){
localizer.handle_log(sm["cameraOdometry"]);
}
}
return 0;
}
selfdrive/loggerd/uploader.py
+2 -2
View File
@@ -247,7 +247,8 @@ def uploader_fn(exit_event):
backoff = 0.1
while True:
allow_raw_upload = (params.get("IsUploadRawEnabled") != b"0")
offroad = params.get("IsOffroad") == b'1'
allow_raw_upload = (params.get("IsUploadRawEnabled") != b"0") and offroad
on_hotspot = is_on_hotspot()
on_wifi = is_on_wifi()
should_upload = on_wifi and not on_hotspot
@@ -257,7 +258,6 @@ def uploader_fn(exit_event):
d = uploader.next_file_to_upload(with_raw=allow_raw_upload and should_upload)
if d is None: # Nothing to upload
offroad = params.get("IsOffroad") == b'1'
time.sleep(60 if offroad else 5)
continue
selfdrive/manager.py
+39 -17
View File
@@ -19,7 +19,7 @@ WEBCAM = os.getenv("WEBCAM") is not None
sys.path.append(os.path.join(BASEDIR, "pyextra"))
os.environ['BASEDIR'] = BASEDIR
TOTAL_SCONS_NODES = 1140
TOTAL_SCONS_NODES = 1005
prebuilt = os.path.exists(os.path.join(BASEDIR, 'prebuilt'))
# Create folders needed for msgq
@@ -70,19 +70,15 @@ def unblock_stdout():
if __name__ == "__main__":
unblock_stdout()
if __name__ == "__main__" and ANDROID:
from common.spinner import Spinner
from common.text_window import TextWindow
else:
from common.spinner import FakeSpinner as Spinner
from common.text_window import FakeTextWindow as TextWindow
from common.spinner import Spinner
from common.text_window import TextWindow
import importlib
import traceback
from multiprocessing import Process
# Run scons
spinner = Spinner()
spinner = Spinner(noop=(__name__ != "__main__" or not ANDROID))
spinner.update("0")
if not prebuilt:
@@ -142,8 +138,9 @@ if not prebuilt:
cloudlog.error("scons build failed\n" + error_s)
# Show TextWindow
no_ui = __name__ != "__main__" or not ANDROID
error_s = "\n \n".join(["\n".join(textwrap.wrap(e, 65)) for e in errors])
with TextWindow("openpilot failed to build\n \n" + error_s) as t:
with TextWindow("openpilot failed to build\n \n" + error_s, noop=no_ui) as t:
t.wait_for_exit()
exit(1)
@@ -192,7 +189,6 @@ managed_processes = {
"updated": "selfdrive.updated",
"dmonitoringmodeld": ("selfdrive/modeld", ["./dmonitoringmodeld"]),
"modeld": ("selfdrive/modeld", ["./modeld"]),
"driverview": "selfdrive.monitoring.driverview",
}
daemon_processes = {
@@ -245,6 +241,12 @@ car_started_processes = [
'locationd',
]
driver_view_processes = [
'camerad',
'dmonitoringd',
'dmonitoringmodeld'
]
if WEBCAM:
car_started_processes += [
'dmonitoringmodeld',
@@ -387,6 +389,14 @@ def cleanup_all_processes(signal, frame):
kill_managed_process(name)
cloudlog.info("everything is dead")
def send_managed_process_signal(name, sig):
if name not in running or name not in managed_processes:
return
cloudlog.info(f"sending signal {sig} to {name}")
os.kill(running[name].pid, sig)
# ****************** run loop ******************
def manager_init(should_register=True):
@@ -455,6 +465,7 @@ def manager_thread():
for k in os.getenv("BLOCK").split(","):
del managed_processes[k]
started_prev = False
logger_dead = False
while 1:
@@ -473,7 +484,7 @@ def manager_thread():
if msg.thermal.freeSpace < 0.05:
logger_dead = True
if msg.thermal.started and "driverview" not in running:
if msg.thermal.started:
for p in car_started_processes:
if p == "loggerd" and logger_dead:
kill_managed_process(p)
@@ -481,13 +492,24 @@ def manager_thread():
start_managed_process(p)
else:
logger_dead = False
driver_view = params.get("IsDriverViewEnabled") == b"1"
# TODO: refactor how manager manages processes
for p in reversed(car_started_processes):
kill_managed_process(p)
# this is ugly
if "driverview" not in running and params.get("IsDriverViewEnabled") == b"1":
start_managed_process("driverview")
elif "driverview" in running and params.get("IsDriverViewEnabled") == b"0":
kill_managed_process("driverview")
if p not in driver_view_processes or not driver_view:
kill_managed_process(p)
for p in driver_view_processes:
if driver_view:
start_managed_process(p)
else:
kill_managed_process(p)
# trigger an update after going offroad
if started_prev:
send_managed_process_signal("updated", signal.SIGHUP)
started_prev = msg.thermal.started
# check the status of all processes, did any of them die?
running_list = ["%s%s\u001b[0m" % ("\u001b[32m" if running[p].is_alive() else "\u001b[31m", p) for p in running]
selfdrive/modeld/dmonitoringmodeld.cc
+1 -4
View File
@@ -56,7 +56,6 @@ int main(int argc, char **argv) {
buf = visionstream_get(&stream, &extra);
if (buf == NULL) {
printf("visionstream get failed\n");
visionstream_destroy(&stream);
break;
}
//printf("frame_id: %d %dx%d\n", extra.frame_id, buf_info.width, buf_info.height);
@@ -84,11 +83,9 @@ int main(int argc, char **argv) {
LOGD("dmonitoring process: %.2fms, from last %.2fms", t2-t1, t1-last);
last = t1;
}
visionstream_destroy(&stream);
}
visionstream_destroy(&stream);
dmonitoring_free(&dmonitoringmodel);
return 0;
selfdrive/modeld/modeld.cc
+5 -7
View File
@@ -181,7 +181,7 @@ int main(int argc, char **argv) {
cl_mem yuv_cl;
VisionBuf yuv_ion = visionbuf_allocate_cl(buf_info.buf_len, device_id, context, &yuv_cl);
uint32_t last_vipc_frame_id = 0;
uint32_t frame_id = 0, last_vipc_frame_id = 0;
double last = 0;
int desire = -1;
while (!do_exit) {
@@ -190,7 +190,6 @@ int main(int argc, char **argv) {
buf = visionstream_get(&stream, &extra);
if (buf == NULL) {
LOGW("visionstream get failed");
visionstream_destroy(&stream);
break;
}
@@ -202,6 +201,7 @@ int main(int argc, char **argv) {
if (sm.update(0) > 0){
// TODO: path planner timeout?
desire = ((int)sm["pathPlan"].getPathPlan().getDesire()) - 1;
frame_id = sm["frame"].getFrame().getFrameId();
}
double mt1 = 0, mt2 = 0;
@@ -212,8 +212,7 @@ int main(int argc, char **argv) {
}
mat3 model_transform = matmul3(yuv_transform, transform);
uint32_t frame_id = sm["frame"].getFrame().getFrameId();
mt1 = millis_since_boot();
// TODO: don't make copies!
@@ -232,17 +231,16 @@ int main(int argc, char **argv) {
model_publish(pm, extra.frame_id, frame_id, vipc_dropped_frames, frame_drop_perc, model_buf, extra.timestamp_eof);
posenet_publish(pm, extra.frame_id, frame_id, vipc_dropped_frames, frame_drop_perc, model_buf, extra.timestamp_eof);
LOGD("model process: %.2fms, from last %.2fms, vipc_frame_id %zu, frame_id, %zu, frame_drop %.3f%", mt2-mt1, mt1-last, extra.frame_id, frame_id, frame_drop_perc);
LOGD("model process: %.2fms, from last %.2fms, vipc_frame_id %zu, frame_id, %zu, frame_drop %.3f", mt2-mt1, mt1-last, extra.frame_id, frame_id, frame_drop_perc);
last = mt1;
last_vipc_frame_id = extra.frame_id;
}
}
visionbuf_free(&yuv_ion);
visionstream_destroy(&stream);
}
visionstream_destroy(&stream);
model_free(&model);
LOG("joining live_thread");
selfdrive/modeld/models/commonmodel.c
+3 -3
View File
@@ -15,13 +15,13 @@ void frame_init(ModelFrame* frame, int width, int height,
frame->transformed_height = height;
frame->transformed_y_cl = clCreateBuffer(frame->context, CL_MEM_READ_WRITE,
frame->transformed_width*frame->transformed_height, NULL, &err);
(size_t)frame->transformed_width*frame->transformed_height, NULL, &err);
assert(err == 0);
frame->transformed_u_cl = clCreateBuffer(frame->context, CL_MEM_READ_WRITE,
(frame->transformed_width/2)*(frame->transformed_height/2), NULL, &err);
(size_t)(frame->transformed_width/2)*(frame->transformed_height/2), NULL, &err);
assert(err == 0);
frame->transformed_v_cl = clCreateBuffer(frame->context, CL_MEM_READ_WRITE,
(frame->transformed_width/2)*(frame->transformed_height/2), NULL, &err);
(size_t)(frame->transformed_width/2)*(frame->transformed_height/2), NULL, &err);
assert(err == 0);
frame->net_input_size = ((width*height*3)/2)*sizeof(float);
selfdrive/modeld/models/dmonitoring.cc
+5 -3
View File
@@ -5,9 +5,9 @@
#include <libyuv.h>
#define MODEL_WIDTH 160
#define MODEL_HEIGHT 320
#define FULL_W 426
#define MODEL_WIDTH 320
#define MODEL_HEIGHT 640
#define FULL_W 852
#if defined(QCOM) || defined(QCOM2)
#define input_lambda(x) (x - 128.f) * 0.0078125f
@@ -136,6 +136,7 @@ DMonitoringResult dmonitoring_eval_frame(DMonitoringModelState* s, void* stream_
memcpy(&ret.right_eye_prob, &s->output[30], sizeof ret.right_eye_prob);
memcpy(&ret.left_blink_prob, &s->output[31], sizeof ret.right_eye_prob);
memcpy(&ret.right_blink_prob, &s->output[32], sizeof ret.right_eye_prob);
memcpy(&ret.sg_prob, &s->output[33], sizeof ret.sg_prob);
ret.face_orientation_meta[0] = softplus(ret.face_orientation_meta[0]);
ret.face_orientation_meta[1] = softplus(ret.face_orientation_meta[1]);
ret.face_orientation_meta[2] = softplus(ret.face_orientation_meta[2]);
@@ -166,6 +167,7 @@ void dmonitoring_publish(PubMaster &pm, uint32_t frame_id, const DMonitoringResu
framed.setRightEyeProb(res.right_eye_prob);
framed.setLeftBlinkProb(res.left_blink_prob);
framed.setRightBlinkProb(res.right_blink_prob);
framed.setSgProb(res.sg_prob);
pm.send("driverState", msg);
}
selfdrive/modeld/models/dmonitoring.h
+2 -1
View File
@@ -9,7 +9,7 @@
extern "C" {
#endif
#define OUTPUT_SIZE 33
#define OUTPUT_SIZE 34
#define RHD_CHECK_INTERVAL 10
typedef struct DMonitoringResult {
@@ -22,6 +22,7 @@ typedef struct DMonitoringResult {
float right_eye_prob;
float left_blink_prob;
float right_blink_prob;
float sg_prob;
} DMonitoringResult;
typedef struct DMonitoringModelState {
selfdrive/modeld/models/driving.cc
+5 -15
View File
@@ -9,9 +9,6 @@
#include "common/params.h"
#include "driving.h"
#define PATH_IDX 0
#define LL_IDX PATH_IDX + MODEL_PATH_DISTANCE*2 + 1
#define RL_IDX LL_IDX + MODEL_PATH_DISTANCE*2 + 2
@@ -48,17 +45,14 @@ void model_init(ModelState* s, cl_device_id device_id, cl_context context, int t
#endif
#ifdef DESIRE
s->prev_desire = (float*)malloc(DESIRE_LEN * sizeof(float));
for (int i = 0; i < DESIRE_LEN; i++) s->prev_desire[i] = 0.0;
s->pulse_desire = (float*)malloc(DESIRE_LEN * sizeof(float));
for (int i = 0; i < DESIRE_LEN; i++) s->pulse_desire[i] = 0.0;
s->m->addDesire(s->pulse_desire, DESIRE_LEN);
s->prev_desire = std::make_unique<float[]>(DESIRE_LEN);
s->pulse_desire = std::make_unique<float[]>(DESIRE_LEN);
s->m->addDesire(s->pulse_desire.get(), DESIRE_LEN);
#endif
#ifdef TRAFFIC_CONVENTION
s->traffic_convention = (float*)malloc(TRAFFIC_CONVENTION_LEN * sizeof(float));
for (int i = 0; i < TRAFFIC_CONVENTION_LEN; i++) s->traffic_convention[i] = 0.0;
s->m->addTrafficConvention(s->traffic_convention, TRAFFIC_CONVENTION_LEN);
s->traffic_convention = std::make_unique<float[]>(TRAFFIC_CONVENTION_LEN);
s->m->addTrafficConvention(s->traffic_convention.get(), TRAFFIC_CONVENTION_LEN);
std::vector<char> result = read_db_bytes("IsRHD");
if (result.size() > 0) {
@@ -79,8 +73,6 @@ void model_init(ModelState* s, cl_device_id device_id, cl_context context, int t
}
}
ModelDataRaw model_eval_frame(ModelState* s, cl_command_queue q,
cl_mem yuv_cl, int width, int height,
mat3 transform, void* sock,
@@ -100,7 +92,6 @@ ModelDataRaw model_eval_frame(ModelState* s, cl_command_queue q,
}
#endif
//for (int i = 0; i < OUTPUT_SIZE + TEMPORAL_SIZE; i++) { printf("%f ", s->output[i]); } printf("\n");
float *new_frame_buf = frame_prepare(&s->frame, q, yuv_cl, width, height, transform);
@@ -163,7 +154,6 @@ void poly_fit(float *in_pts, float *in_stds, float *out, int valid_len) {
out[3] = y0;
}
void fill_path(cereal::ModelData::PathData::Builder path, const float * data, bool has_prob, const float offset) {
float points_arr[MODEL_PATH_DISTANCE];
float stds_arr[MODEL_PATH_DISTANCE];
selfdrive/modeld/models/driving.h
+4 -3
View File
@@ -14,6 +14,7 @@
#include "runners/run.h"
#include <czmq.h>
#include <memory>
#include "messaging.hpp"
#define MODEL_WIDTH 512
@@ -58,11 +59,11 @@ typedef struct ModelState {
float *input_frames;
RunModel *m;
#ifdef DESIRE
float *prev_desire;
float *pulse_desire;
std::unique_ptr<float[]> prev_desire;
std::unique_ptr<float[]> pulse_desire;
#endif
#ifdef TRAFFIC_CONVENTION
float *traffic_convention;
std::unique_ptr<float[]> traffic_convention;
#endif
} ModelState;
selfdrive/monitoring/dmonitoringd.py
+16 -19
View File
@@ -8,14 +8,11 @@ from selfdrive.controls.lib.events import Events
from selfdrive.monitoring.driver_monitor import DriverStatus, MAX_TERMINAL_ALERTS, MAX_TERMINAL_DURATION
from selfdrive.locationd.calibration_helpers import Calibration
def dmonitoringd_thread(sm=None, pm=None):
gc.disable()
# start the loop
set_realtime_priority(53)
params = Params()
# Pub/Sub Sockets
if pm is None:
pm = messaging.PubMaster(['dMonitoringState'])
@@ -23,40 +20,38 @@ def dmonitoringd_thread(sm=None, pm=None):
if sm is None:
sm = messaging.SubMaster(['driverState', 'liveCalibration', 'carState', 'model'])
params = Params()
driver_status = DriverStatus()
is_rhd = params.get("IsRHD")
if is_rhd is not None:
driver_status.is_rhd_region = bool(int(is_rhd))
driver_status.is_rhd_region_checked = True
driver_status.is_rhd_region = is_rhd == b"1"
driver_status.is_rhd_region_checked = is_rhd is not None
sm['liveCalibration'].calStatus = Calibration.INVALID
sm['liveCalibration'].rpyCalib = [0, 0, 0]
sm['carState'].vEgo = 0.
sm['carState'].cruiseState.enabled = False
sm['carState'].cruiseState.speed = 0.
sm['carState'].buttonEvents = []
sm['carState'].steeringPressed = False
sm['carState'].gasPressed = False
sm['carState'].standstill = True
cal_rpy = [0, 0, 0]
v_cruise_last = 0
driver_engaged = False
offroad = params.get("IsOffroad") == b"1"
# 10Hz <- dmonitoringmodeld
while True:
sm.update()
# Handle calibration
if sm.updated['liveCalibration']:
if sm['liveCalibration'].calStatus == Calibration.CALIBRATED:
if len(sm['liveCalibration'].rpyCalib) == 3:
cal_rpy = sm['liveCalibration'].rpyCalib
# Get interaction
if sm.updated['carState']:
v_cruise = sm['carState'].cruiseState.speed
driver_engaged = len(sm['carState'].buttonEvents) > 0 or \
v_cruise != v_cruise_last or \
sm['carState'].steeringPressed
sm['carState'].steeringPressed or \
sm['carState'].gasPressed
if driver_engaged:
driver_status.update(Events(), True, sm['carState'].cruiseState.enabled, sm['carState'].standstill)
v_cruise_last = v_cruise
@@ -68,14 +63,16 @@ def dmonitoringd_thread(sm=None, pm=None):
# Get data from dmonitoringmodeld
if sm.updated['driverState']:
events = Events()
driver_status.get_pose(sm['driverState'], cal_rpy, sm['carState'].vEgo, sm['carState'].cruiseState.enabled)
# Block any engage after certain distrations
driver_status.get_pose(sm['driverState'], sm['liveCalibration'].rpyCalib, sm['carState'].vEgo, sm['carState'].cruiseState.enabled)
# Block engaging after max number of distrations
if driver_status.terminal_alert_cnt >= MAX_TERMINAL_ALERTS or driver_status.terminal_time >= MAX_TERMINAL_DURATION:
events.add(car.CarEvent.EventName.tooDistracted)
# Update events from driver state
driver_status.update(events, driver_engaged, sm['carState'].cruiseState.enabled, sm['carState'].standstill)
# dMonitoringState packet
# build dMonitoringState packet
dat = messaging.new_message('dMonitoringState')
dat.dMonitoringState = {
"events": events.to_msg(),
@@ -93,7 +90,7 @@ def dmonitoringd_thread(sm=None, pm=None):
"awarenessPassive": driver_status.awareness_passive,
"isLowStd": driver_status.pose.low_std,
"hiStdCount": driver_status.hi_stds,
"isPreview": False,
"isPreview": offroad,
}
pm.send('dMonitoringState', dat)
selfdrive/monitoring/driver_monitor.py
+4 -3
View File
@@ -21,8 +21,9 @@ _DISTRACTED_TIME = 11.
_DISTRACTED_PRE_TIME_TILL_TERMINAL = 8.
_DISTRACTED_PROMPT_TIME_TILL_TERMINAL = 6.
_FACE_THRESHOLD = 0.4
_FACE_THRESHOLD = 0.6
_EYE_THRESHOLD = 0.6
_SG_THRESHOLD = 0.5
_BLINK_THRESHOLD = 0.5 # 0.225
_BLINK_THRESHOLD_SLACK = 0.65
_BLINK_THRESHOLD_STRICT = 0.5
@@ -189,8 +190,8 @@ class DriverStatus():
# self.pose.roll_std = driver_state.faceOrientationStd[2]
model_std_max = max(self.pose.pitch_std, self.pose.yaw_std)
self.pose.low_std = model_std_max < _POSESTD_THRESHOLD
self.blink.left_blink = driver_state.leftBlinkProb * (driver_state.leftEyeProb > _EYE_THRESHOLD)
self.blink.right_blink = driver_state.rightBlinkProb * (driver_state.rightEyeProb > _EYE_THRESHOLD)
self.blink.left_blink = driver_state.leftBlinkProb * (driver_state.leftEyeProb > _EYE_THRESHOLD) * (driver_state.sgProb < _SG_THRESHOLD)
self.blink.right_blink = driver_state.rightBlinkProb * (driver_state.rightEyeProb > _EYE_THRESHOLD) * (driver_state.sgProb < _SG_THRESHOLD)
self.face_detected = driver_state.faceProb > _FACE_THRESHOLD and \
abs(driver_state.facePosition[0]) <= 0.4 and abs(driver_state.facePosition[1]) <= 0.45
selfdrive/monitoring/driverview.py
-81
View File
@@ -1,81 +0,0 @@
#!/usr/bin/env python3
import os
import subprocess
import multiprocessing
import signal
import time
import cereal.messaging as messaging
from common.params import Params
from common.basedir import BASEDIR
KILL_TIMEOUT = 15
def send_controls_packet(pm):
while True:
dat = messaging.new_message('controlsState')
dat.controlsState = {
"rearViewCam": True,
}
pm.send('controlsState', dat)
time.sleep(0.01)
def send_dmon_packet(pm, d):
dat = messaging.new_message('dMonitoringState')
dat.dMonitoringState = {
"isRHD": d[0],
"rhdChecked": d[1],
"isPreview": d[2],
}
pm.send('dMonitoringState', dat)
def main():
pm = messaging.PubMaster(['controlsState', 'dMonitoringState'])
controls_sender = multiprocessing.Process(target=send_controls_packet, args=[pm])
controls_sender.start()
# TODO: refactor with manager start/kill
proc_cam = subprocess.Popen(os.path.join(BASEDIR, "selfdrive/camerad/camerad"), cwd=os.path.join(BASEDIR, "selfdrive/camerad"))
proc_mon = subprocess.Popen(os.path.join(BASEDIR, "selfdrive/modeld/dmonitoringmodeld"), cwd=os.path.join(BASEDIR, "selfdrive/modeld"))
params = Params()
is_rhd = False
is_rhd_checked = False
should_exit = False
def terminate(signalNumber, frame):
print('got SIGTERM, exiting..')
should_exit = True
send_dmon_packet(pm, [is_rhd, is_rhd_checked, not should_exit])
proc_cam.send_signal(signal.SIGINT)
proc_mon.send_signal(signal.SIGINT)
kill_start = time.time()
while proc_cam.poll() is None:
if time.time() - kill_start > KILL_TIMEOUT:
from selfdrive.swaglog import cloudlog
cloudlog.critical("FORCE REBOOTING PHONE!")
os.system("date >> /sdcard/unkillable_reboot")
os.system("reboot")
raise RuntimeError
continue
controls_sender.terminate()
exit()
signal.signal(signal.SIGTERM, terminate)
while True:
send_dmon_packet(pm, [is_rhd, is_rhd_checked, not should_exit])
if not is_rhd_checked:
is_rhd = params.get("IsRHD") == b"1"
is_rhd_checked = True
time.sleep(0.01)
if __name__ == '__main__':
main()
selfdrive/test/helpers.py
+11
View File
@@ -4,8 +4,19 @@ from nose.tools import nottest
from common.android import ANDROID
from common.apk import update_apks, start_offroad, pm_apply_packages, android_packages
from common.params import Params
from selfdrive.version import training_version, terms_version
from selfdrive.manager import start_managed_process, kill_managed_process, get_running
def set_params_enabled():
params = Params()
params.put("HasAcceptedTerms", terms_version)
params.put("HasCompletedSetup", "1")
params.put("OpenpilotEnabledToggle", "1")
params.put("CommunityFeaturesToggle", "1")
params.put("Passive", "0")
params.put("CompletedTrainingVersion", training_version)
def phone_only(x):
if ANDROID:
return x
selfdrive/test/id_rsa
-28
View File
@@ -1,28 +0,0 @@
-----BEGIN RSA PRIVATE KEY-----
MIIEvAIBADANBgkqhkiG9w0BAQEFAASCBKYwggSiAgEAAoIBAQC+iXXq30Tq+J5N
Kat3KWHCzcmwZ55nGh6WggAqECa5CasBlM9VeROpVu3beA+5h0MibRgbD4DMtVXB
t6gEvZ8nd04E7eLA9LTZyFDZ7SkSOVj4oXOQsT0GnJmKrASW5KslTWqVzTfo2XCt
Z+004ikLxmyFeBO8NOcErW1pa8gFdQDToH9FrA7kgysic/XVESTOoe7XlzRoe/eZ
acEQ+jtnmFd21A4aEADkk00Ahjr0uKaJiLUAPatxs2icIXWpgYtfqqtaKF23wSt6
1OTu6cAwXbOWr3m+IUSRUO0IRzEIQS3z1jfd1svgzSgSSwZ1Lhj4AoKxIEAIc8qJ
rO4uymCJAgMBAAECggEBAISFevxHGdoL3Z5xkw6oO5SQKO2GxEeVhRzNgmu/HA+q
x8OryqD6O1CWY4037kft6iWxlwiLOdwna2P25ueVM3LxqdQH2KS4DmlCx+kq6FwC
gv063fQPMhC9LpWimvaQSPEC7VUPjQlo4tPY6sTTYBUOh0A1ihRm/x7juKuQCWix
Cq8C/DVnB1X4mGj+W3nJc5TwVJtgJbbiBrq6PWrhvB/3qmkxHRL7dU2SBb2iNRF1
LLY30dJx/cD73UDKNHrlrsjk3UJc29Mp4/MladKvUkRqNwlYxSuAtJV0nZ3+iFkL
s3adSTHdJpClQer45R51rFDlVsDz2ZBpb/hRNRoGDuECgYEA6A1EixLq7QYOh3cb
Xhyh3W4kpVvA/FPfKH1OMy3ONOD/Y9Oa+M/wthW1wSoRL2n+uuIW5OAhTIvIEivj
6bAZsTT3twrvOrvYu9rx9aln4p8BhyvdjeW4kS7T8FP5ol6LoOt2sTP3T1LOuJPO
uQvOjlKPKIMh3c3RFNWTnGzMPa0CgYEA0jNiPLxP3A2nrX0keKDI+VHuvOY88gdh
0W5BuLMLovOIDk9aQFIbBbMuW1OTjHKv9NK+Lrw+YbCFqOGf1dU/UN5gSyE8lX/Q
FsUGUqUZx574nJZnOIcy3ONOnQLcvHAQToLFAGUd7PWgP3CtHkt9hEv2koUwL4vo
ikTP1u9Gkc0CgYEA2apoWxPZrY963XLKBxNQecYxNbLFaWq67t3rFnKm9E8BAICi
4zUaE5J1tMVi7Vi9iks9Ml9SnNyZRQJKfQ+kaebHXbkyAaPmfv+26rqHKboA0uxA
nDOZVwXX45zBkp6g1sdHxJx8JLoGEnkC9eyvSi0C//tRLx86OhLErXwYcNkCf1it
VMRKrWYoXJTUNo6tRhvodM88UnnIo3u3CALjhgU4uC1RTMHV4ZCGBwiAOb8GozSl
s5YD1E1iKwEULloHnK6BIh6P5v8q7J6uf/xdqoKMjlWBHgq6/roxKvkSPA1DOZ3l
jTadcgKFnRUmc+JT9p/ZbCxkA/ALFg8++G+0ghECgYA8vG3M/utweLvq4RI7l7U7
b+i2BajfK2OmzNi/xugfeLjY6k2tfQGRuv6ppTjehtji2uvgDWkgjJUgPfZpir3I
RsVMUiFgloWGHETOy0Qvc5AwtqTJFLTD1Wza2uBilSVIEsg6Y83Gickh+ejOmEsY
6co17RFaAZHwGfCFFjO76Q==
-----END RSA PRIVATE KEY-----
selfdrive/test/phone_ci.py
-98
View File
@@ -1,98 +0,0 @@
#!/usr/bin/env python3
import paramiko # pylint: disable=import-error
import os
import sys
import re
import time
import socket
SOURCE_DIR = "/data/openpilot_source/"
TEST_DIR = "/data/openpilot/"
def run_on_phone(test_cmd):
eon_ip = os.environ.get('eon_ip', None)
if eon_ip is None:
raise Exception("'eon_ip' not set")
ssh = paramiko.SSHClient()
ssh.set_missing_host_key_policy(paramiko.AutoAddPolicy())
key_file = open(os.path.join(os.path.dirname(__file__), "id_rsa"))
key = paramiko.RSAKey.from_private_key(key_file)
print("SSH to phone at {}".format(eon_ip))
# try connecting for one minute
t_start = time.time()
while True:
try:
ssh.connect(hostname=eon_ip, port=8022, pkey=key, timeout=10)
except (paramiko.ssh_exception.SSHException, socket.timeout, paramiko.ssh_exception.NoValidConnectionsError):
print("Connection failed")
if time.time() - t_start > 60:
raise
else:
break
time.sleep(1)
branch = os.environ['GIT_BRANCH']
commit = os.environ.get('GIT_COMMIT', branch)
conn = ssh.invoke_shell()
# pass in all environment variables prefixed with 'CI_'
for k, v in os.environ.items():
if k.startswith("CI_") or k in ["GIT_BRANCH", "GIT_COMMIT"]:
conn.send(f"export {k}='{v}'\n")
conn.send("export CI=1\n")
# clear scons cache dirs that haven't been written to in one day
conn.send("cd /tmp && find -name 'scons_cache_*' -type d -maxdepth 1 -mtime 1 -exec rm -rf '{}' \\;\n")
# set up environment
conn.send(f"cd {SOURCE_DIR}\n")
conn.send("git reset --hard\n")
conn.send("git fetch origin\n")
conn.send("find . -maxdepth 1 -not -path './.git' -not -name '.' -not -name '..' -exec rm -rf '{}' \\;\n")
conn.send(f"git reset --hard {commit}\n")
conn.send(f"git checkout {commit}\n")
conn.send("git clean -xdf\n")
conn.send("git submodule update --init\n")
conn.send("git submodule foreach --recursive git reset --hard\n")
conn.send("git submodule foreach --recursive git clean -xdf\n")
conn.send('echo "git took $SECONDS seconds"\n')
conn.send(f"rsync -a --delete {SOURCE_DIR} {TEST_DIR}\n")
# run the test
conn.send(test_cmd + "\n")
# get the result and print it back out
conn.send('echo "RESULT:" $?\n')
conn.send("exit\n")
dat = b""
conn.settimeout(240)
while True:
try:
recvd = conn.recv(4096)
except socket.timeout:
print("connection to phone timed out")
sys.exit(1)
if len(recvd) == 0:
break
dat += recvd
sys.stdout.buffer.write(recvd)
sys.stdout.flush()
return_code = int(re.findall(rb'^RESULT: (\d+)', dat[-1024:], flags=re.MULTILINE)[0])
sys.exit(return_code)
if __name__ == "__main__":
run_on_phone(sys.argv[1])
selfdrive/test/setup_device_ci.sh
Executable
+35
View File
@@ -0,0 +1,35 @@
#!/usr/bin/bash -e
export SOURCE_DIR="/data/openpilot_source/"
if [ -z "$GIT_COMMIT" ]; then
echo "GIT_COMMIT must be set"
exit 1
fi
if [ -z "$TEST_DIR" ]; then
echo "TEST_DIR must be set"
exit 1
fi
# TODO: never clear qcom_replay cache
# clear scons cache dirs that haven't been written to in one day
cd /tmp && find -name 'scons_cache_*' -type d -maxdepth 1 -mtime 1 -exec rm -rf '{}' \;
# set up environment
cd $SOURCE_DIR
git reset --hard
git fetch origin
find . -maxdepth 1 -not -path './.git' -not -name '.' -not -name '..' -exec rm -rf '{}' \;
git reset --hard $GIT_COMMIT
git checkout $GIT_COMMIT
git clean -xdf
git submodule update --init
git submodule foreach --recursive git reset --hard
git submodule foreach --recursive git clean -xdf
echo "git checkout took $SECONDS seconds"
rsync -a --delete $SOURCE_DIR $TEST_DIR
echo "$TEST_DIR synced with $GIT_COMMIT, took $SECONDS seconds"
selfdrive/test/test_cpu_usage.py
+43 -36
View File
@@ -1,13 +1,13 @@
#!/usr/bin/env python3
import os
import time
import threading
import _thread
import signal
import sys
import subprocess
import cereal.messaging as messaging
import selfdrive.manager as manager
from common.basedir import BASEDIR
from common.params import Params
from selfdrive.test.helpers import set_params_enabled
def cputime_total(ct):
return ct.cpuUser + ct.cpuSystem + ct.cpuChildrenUser + ct.cpuChildrenSystem
@@ -15,7 +15,7 @@ def cputime_total(ct):
def print_cpu_usage(first_proc, last_proc):
procs = [
("selfdrive.controls.controlsd", 59.46),
("selfdrive.controls.controlsd", 66.15),
("selfdrive.locationd.locationd", 34.38),
("./loggerd", 33.90),
("selfdrive.controls.plannerd", 19.77),
@@ -39,7 +39,7 @@ def print_cpu_usage(first_proc, last_proc):
("./logcatd", 0),
]
r = 0
r = True
dt = (last_proc.logMonoTime - first_proc.logMonoTime) / 1e9
result = "------------------------------------------------\n"
for proc_name, normal_cpu_usage in procs:
@@ -50,47 +50,54 @@ def print_cpu_usage(first_proc, last_proc):
cpu_usage = cpu_time / dt * 100.
if cpu_usage > max(normal_cpu_usage * 1.1, normal_cpu_usage + 5.0):
result += f"Warning {proc_name} using more CPU than normal\n"
r = 1
r = False
elif cpu_usage < min(normal_cpu_usage * 0.3, max(normal_cpu_usage - 1.0, 0.0)):
result += f"Warning {proc_name} using less CPU than normal\n"
r = 1
r = False
result += f"{proc_name.ljust(35)} {cpu_usage:.2f}%\n"
except IndexError:
result += f"{proc_name.ljust(35)} NO METRICS FOUND\n"
r = 1
r = False
result += "------------------------------------------------\n"
print(result)
return r
return_code = 1
def test_thread():
global return_code
proc_sock = messaging.sub_sock('procLog', conflate=True)
def test_cpu_usage():
cpu_ok = False
# wait until everything's started and get first sample
time.sleep(30)
first_proc = messaging.recv_sock(proc_sock, wait=True)
# start manager
manager_path = os.path.join(BASEDIR, "selfdrive/manager.py")
manager_proc = subprocess.Popen(["python", manager_path])
try:
proc_sock = messaging.sub_sock('procLog', conflate=True, timeout=2000)
# run for a minute and get last sample
time.sleep(60)
last_proc = messaging.recv_sock(proc_sock, wait=True)
# wait until everything's started and get first sample
start_time = time.monotonic()
while time.monotonic() - start_time < 120:
if Params().get("CarParams") is not None:
break
time.sleep(2)
first_proc = messaging.recv_sock(proc_sock, wait=True)
if first_proc is None:
raise Exception("\n\nTEST FAILED: progLog recv timed out\n\n")
running = manager.get_running()
all_running = all(p in running and running[p].is_alive() for p in manager.car_started_processes)
return_code = print_cpu_usage(first_proc, last_proc)
if not all_running:
return_code = 1
_thread.interrupt_main()
# run for a minute and get last sample
time.sleep(60)
last_proc = messaging.recv_sock(proc_sock, wait=True)
cpu_ok = print_cpu_usage(first_proc, last_proc)
finally:
manager_proc.terminate()
ret = manager_proc.wait(20)
if ret is None:
manager_proc.kill()
return cpu_ok
if __name__ == "__main__":
set_params_enabled()
Params().delete("CarParams")
# setup signal handler to exit with test status
def handle_exit(sig, frame):
sys.exit(return_code)
signal.signal(signal.SIGINT, handle_exit)
# start manager and test thread
t = threading.Thread(target=test_thread)
t.daemon = True
t.start()
manager.main()
passed = False
try:
passed = test_cpu_usage()
finally:
sys.exit(int(not passed))
selfdrive/test/test_openpilot.py
+2 -1
View File
@@ -5,7 +5,7 @@ os.environ['FAKEUPLOAD'] = "1"
from common.params import Params
from common.realtime import sec_since_boot
from selfdrive.manager import manager_init, manager_prepare, start_daemon_process
from selfdrive.test.helpers import phone_only, with_processes
from selfdrive.test.helpers import phone_only, with_processes, set_params_enabled
import json
import requests
import signal
@@ -16,6 +16,7 @@ import time
# must run first
@phone_only
def test_manager_prepare():
set_params_enabled()
manager_init()
manager_prepare()
selfdrive/thermald/thermald.py
+41 -32
View File
@@ -1,20 +1,18 @@
#!/usr/bin/env python3
import os
import json
import copy
import datetime
import psutil
from smbus2 import SMBus
from cereal import log
from common.android import ANDROID, get_network_type, get_network_strength
from common.basedir import BASEDIR
from common.params import Params, put_nonblocking
from common.realtime import sec_since_boot, DT_TRML
from common.numpy_fast import clip, interp
from common.filter_simple import FirstOrderFilter
from selfdrive.version import terms_version, training_version
from selfdrive.version import terms_version, training_version, get_git_branch
from selfdrive.swaglog import cloudlog
import cereal.messaging as messaging
from selfdrive.controls.lib.alertmanager import set_offroad_alert
from selfdrive.loggerd.config import get_available_percent
from selfdrive.pandad import get_expected_signature
from selfdrive.thermald.power_monitoring import PowerMonitoring, get_battery_capacity, get_battery_status, \
@@ -35,10 +33,6 @@ LEON = False
last_eon_fan_val = None
with open(BASEDIR + "/selfdrive/controls/lib/alerts_offroad.json") as json_file:
OFFROAD_ALERTS = json.load(json_file)
def read_tz(x, clip=True):
if not ANDROID:
# we don't monitor thermal on PC
@@ -171,6 +165,7 @@ def thermald_thread():
thermal_status_prev = ThermalStatus.green
usb_power = True
usb_power_prev = True
current_branch = get_git_branch()
network_type = NetworkType.none
network_strength = NetworkStrength.unknown
@@ -179,7 +174,7 @@ def thermald_thread():
cpu_temp_filter = FirstOrderFilter(0., CPU_TEMP_TAU, DT_TRML)
health_prev = None
fw_version_match_prev = True
current_connectivity_alert = None
current_update_alert = None
time_valid_prev = True
should_start_prev = False
handle_fan = None
@@ -253,6 +248,7 @@ def thermald_thread():
if is_uno:
msg.thermal.batteryPercent = 100
msg.thermal.batteryStatus = "Charging"
msg.thermal.bat = 0
current_filter.update(msg.thermal.batteryCurrent / 1e6)
@@ -300,9 +296,9 @@ def thermald_thread():
# show invalid date/time alert
time_valid = now.year >= 2019
if time_valid and not time_valid_prev:
params.delete("Offroad_InvalidTime")
set_offroad_alert("Offroad_InvalidTime", False)
if not time_valid and time_valid_prev:
put_nonblocking("Offroad_InvalidTime", json.dumps(OFFROAD_ALERTS["Offroad_InvalidTime"]))
set_offroad_alert("Offroad_InvalidTime", True)
time_valid_prev = time_valid
# Show update prompt
@@ -314,24 +310,37 @@ def thermald_thread():
update_failed_count = params.get("UpdateFailedCount")
update_failed_count = 0 if update_failed_count is None else int(update_failed_count)
last_update_exception = params.get("LastUpdateException", encoding='utf8')
if dt.days > DAYS_NO_CONNECTIVITY_MAX and update_failed_count > 1:
if current_connectivity_alert != "expired":
current_connectivity_alert = "expired"
params.delete("Offroad_ConnectivityNeededPrompt")
put_nonblocking("Offroad_ConnectivityNeeded", json.dumps(OFFROAD_ALERTS["Offroad_ConnectivityNeeded"]))
if update_failed_count > 15 and last_update_exception is not None:
if current_branch in ["release2", "dashcam"]:
extra_text = "Ensure the software is correctly installed"
else:
extra_text = last_update_exception
if current_update_alert != "update" + extra_text:
current_update_alert = "update" + extra_text
set_offroad_alert("Offroad_ConnectivityNeeded", False)
set_offroad_alert("Offroad_ConnectivityNeededPrompt", False)
set_offroad_alert("Offroad_UpdateFailed", True, extra_text=extra_text)
elif dt.days > DAYS_NO_CONNECTIVITY_MAX and update_failed_count > 1:
if current_update_alert != "expired":
current_update_alert = "expired"
set_offroad_alert("Offroad_UpdateFailed", False)
set_offroad_alert("Offroad_ConnectivityNeededPrompt", False)
set_offroad_alert("Offroad_ConnectivityNeeded", True)
elif dt.days > DAYS_NO_CONNECTIVITY_PROMPT:
remaining_time = str(max(DAYS_NO_CONNECTIVITY_MAX - dt.days, 0))
if current_connectivity_alert != "prompt" + remaining_time:
current_connectivity_alert = "prompt" + remaining_time
alert_connectivity_prompt = copy.copy(OFFROAD_ALERTS["Offroad_ConnectivityNeededPrompt"])
alert_connectivity_prompt["text"] += remaining_time + " days."
params.delete("Offroad_ConnectivityNeeded")
put_nonblocking("Offroad_ConnectivityNeededPrompt", json.dumps(alert_connectivity_prompt))
elif current_connectivity_alert is not None:
current_connectivity_alert = None
params.delete("Offroad_ConnectivityNeeded")
params.delete("Offroad_ConnectivityNeededPrompt")
if current_update_alert != "prompt" + remaining_time:
current_update_alert = "prompt" + remaining_time
set_offroad_alert("Offroad_UpdateFailed", False)
set_offroad_alert("Offroad_ConnectivityNeeded", False)
set_offroad_alert("Offroad_ConnectivityNeededPrompt", True, extra_text=f"{remaining_time} days.")
elif current_update_alert is not None:
current_update_alert = None
set_offroad_alert("Offroad_UpdateFailed", False)
set_offroad_alert("Offroad_ConnectivityNeeded", False)
set_offroad_alert("Offroad_ConnectivityNeededPrompt", False)
do_uninstall = params.get("DoUninstall") == b"1"
accepted_terms = params.get("HasAcceptedTerms") == terms_version
@@ -361,19 +370,19 @@ def thermald_thread():
should_start = should_start and (not is_taking_snapshot) and (not is_viewing_driver)
if fw_version_match and not fw_version_match_prev:
params.delete("Offroad_PandaFirmwareMismatch")
set_offroad_alert("Offroad_PandaFirmwareMismatch", False)
if not fw_version_match and fw_version_match_prev:
put_nonblocking("Offroad_PandaFirmwareMismatch", json.dumps(OFFROAD_ALERTS["Offroad_PandaFirmwareMismatch"]))
set_offroad_alert("Offroad_PandaFirmwareMismatch", True)
# if any CPU gets above 107 or the battery gets above 63, kill all processes
# controls will warn with CPU above 95 or battery above 60
if thermal_status >= ThermalStatus.danger:
should_start = False
if thermal_status_prev < ThermalStatus.danger:
put_nonblocking("Offroad_TemperatureTooHigh", json.dumps(OFFROAD_ALERTS["Offroad_TemperatureTooHigh"]))
set_offroad_alert("Offroad_TemperatureTooHigh", True)
else:
if thermal_status_prev >= ThermalStatus.danger:
params.delete("Offroad_TemperatureTooHigh")
set_offroad_alert("Offroad_TemperatureTooHigh", False)
if should_start:
if not should_start_prev:
@@ -411,9 +420,9 @@ def thermald_thread():
thermal_sock.send(msg.to_bytes())
if usb_power_prev and not usb_power:
put_nonblocking("Offroad_ChargeDisabled", json.dumps(OFFROAD_ALERTS["Offroad_ChargeDisabled"]))
set_offroad_alert("Offroad_ChargeDisabled", True)
elif usb_power and not usb_power_prev:
params.delete("Offroad_ChargeDisabled")
set_offroad_alert("Offroad_ChargeDisabled", False)
thermal_status_prev = thermal_status
usb_power_prev = usb_power
selfdrive/ui/linux.cc
+1 -1
View File
@@ -83,7 +83,7 @@ int touch_read(TouchState *s, int* out_x, int* out_y) {
#include "sound.hpp"
bool Sound::init(int volume) { return true; }
bool Sound::play(AudibleAlert alert) { return true; }
bool Sound::play(AudibleAlert alert) { printf("play sound: %d\n", (int)alert); return true; }
void Sound::stop() {}
void Sound::setVolume(int volume) {}
Sound::~Sound() {}
selfdrive/ui/paint.cc
+17 -23
View File
@@ -368,11 +368,14 @@ static void ui_draw_world(UIState *s) {
// Draw lane edges and vision/mpc tracks
ui_draw_vision_lanes(s);
if (scene->lead_data[0].getStatus()) {
draw_lead(s, scene->lead_data[0]);
}
if (scene->lead_data[1].getStatus() && (std::abs(scene->lead_data[0].getDRel() - scene->lead_data[1].getDRel()) > 3.0)) {
draw_lead(s, scene->lead_data[1]);
// Draw lead indicators if openpilot is handling longitudinal
if (s->longitudinal_control) {
if (scene->lead_data[0].getStatus()) {
draw_lead(s, scene->lead_data[0]);
}
if (scene->lead_data[1].getStatus() && (std::abs(scene->lead_data[0].getDRel() - scene->lead_data[1].getDRel()) > 3.0)) {
draw_lead(s, scene->lead_data[1]);
}
}
nvgRestore(s->vg);
}
@@ -557,7 +560,7 @@ static void ui_draw_vision_face(UIState *s) {
const int face_size = 96;
const int face_x = (s->scene.ui_viz_rx + face_size + (bdr_s * 2));
const int face_y = (footer_y + ((footer_h - face_size) / 2));
ui_draw_circle_image(s->vg, face_x, face_y, face_size, s->img_face, s->scene.controls_state.getDriverMonitoringOn());
ui_draw_circle_image(s->vg, face_x, face_y, face_size, s->img_face, s->scene.dmonitoring_state.getFaceDetected());
}
static void ui_draw_driver_view(UIState *s) {
@@ -569,19 +572,11 @@ static void ui_draw_driver_view(UIState *s) {
const int valid_frame_x = frame_x + (frame_w - valid_frame_w) / 2 + ff_xoffset;
// blackout
if (!scene->is_rhd) {
NVGpaint gradient = nvgLinearGradient(s->vg, valid_frame_x + valid_frame_w,
box_y,
valid_frame_x + box_h / 2, box_y,
nvgRGBAf(0,0,0,1), nvgRGBAf(0,0,0,0));
ui_draw_rect(s->vg, valid_frame_x + box_h / 2, box_y, valid_frame_w - box_h / 2, box_h, gradient);
} else {
NVGpaint gradient = nvgLinearGradient(s->vg, valid_frame_x,
box_y,
valid_frame_w - box_h / 2, box_y,
nvgRGBAf(0,0,0,1), nvgRGBAf(0,0,0,0));
ui_draw_rect(s->vg, valid_frame_x, box_y, valid_frame_w - box_h / 2, box_h, gradient);
}
NVGpaint gradient = nvgLinearGradient(s->vg, scene->is_rhd ? valid_frame_x : (valid_frame_x + valid_frame_w),
box_y,
scene->is_rhd ? (valid_frame_w - box_h / 2) : (valid_frame_x + box_h / 2), box_y,
COLOR_BLACK, COLOR_BLACK_ALPHA(0));
ui_draw_rect(s->vg, scene->is_rhd ? valid_frame_x : (valid_frame_x + box_h / 2), box_y, valid_frame_w - box_h / 2, box_h, gradient);
ui_draw_rect(s->vg, scene->is_rhd ? valid_frame_x : valid_frame_x + box_h / 2, box_y, valid_frame_w - box_h / 2, box_h, COLOR_BLACK_ALPHA(144));
// borders
@@ -589,7 +584,7 @@ static void ui_draw_driver_view(UIState *s) {
ui_draw_rect(s->vg, valid_frame_x + valid_frame_w, box_y, frame_w - valid_frame_w - (valid_frame_x - frame_x), box_h, nvgRGBA(23, 51, 73, 255));
// draw face box
if (scene->driver_state.getFaceProb() > 0.4) {
if (scene->dmonitoring_state.getFaceDetected()) {
auto fxy_list = scene->driver_state.getFacePosition();
const float face_x = fxy_list[0];
const float face_y = fxy_list[1];
@@ -600,6 +595,7 @@ static void ui_draw_driver_view(UIState *s) {
} else {
fbox_x = valid_frame_x + valid_frame_w - box_h / 2 + (face_x + 0.5) * (box_h / 2) - 0.5 * 0.6 * box_h / 2;
}
if (std::abs(face_x) <= 0.35 && std::abs(face_y) <= 0.4) {
ui_draw_rect(s->vg, fbox_x, fbox_y, 0.6 * box_h / 2, 0.6 * box_h / 2,
nvgRGBAf(1.0, 1.0, 1.0, 0.8 - ((std::abs(face_x) > std::abs(face_y) ? std::abs(face_x) : std::abs(face_y))) * 0.6 / 0.375),
@@ -613,7 +609,7 @@ static void ui_draw_driver_view(UIState *s) {
const int face_size = 85;
const int x = (valid_frame_x + face_size + (bdr_s * 2)) + (scene->is_rhd ? valid_frame_w - box_h / 2:0);
const int y = (box_y + box_h - face_size - bdr_s - (bdr_s * 1.5));
ui_draw_circle_image(s->vg, x, y, face_size, s->img_face, scene->driver_state.getFaceProb() > 0.4);
ui_draw_circle_image(s->vg, x, y, face_size, s->img_face, scene->dmonitoring_state.getFaceDetected());
}
static void ui_draw_vision_header(UIState *s) {
@@ -853,8 +849,6 @@ void ui_nvg_init(UIState *s) {
assert(s->vg);
s->font_courbd = nvgCreateFont(s->vg, "courbd", "../assets/fonts/courbd.ttf");
assert(s->font_courbd >= 0);
s->font_sans_regular = nvgCreateFont(s->vg, "sans-regular", "../assets/fonts/opensans_regular.ttf");
assert(s->font_sans_regular >= 0);
s->font_sans_semibold = nvgCreateFont(s->vg, "sans-semibold", "../assets/fonts/opensans_semibold.ttf");
selfdrive/ui/sidebar.cc
+8 -12
View File
@@ -10,17 +10,13 @@ static void ui_draw_sidebar_background(UIState *s) {
}
static void ui_draw_sidebar_settings_button(UIState *s) {
bool settingsActive = s->active_app == cereal::UiLayoutState::App::SETTINGS;
const int settings_btn_xr = !s->scene.uilayout_sidebarcollapsed ? settings_btn_x : -(sbr_w);
ui_draw_image(s->vg, settings_btn_xr, settings_btn_y, settings_btn_w, settings_btn_h, s->img_button_settings, settingsActive ? 1.0f : 0.65f);
const float alpha = s->active_app == cereal::UiLayoutState::App::SETTINGS ? 1.0f : 0.65f;
ui_draw_image(s->vg, settings_btn_x, settings_btn_y, settings_btn_w, settings_btn_h, s->img_button_settings, alpha);
}
static void ui_draw_sidebar_home_button(UIState *s) {
bool homeActive = s->active_app == cereal::UiLayoutState::App::HOME;
const int home_btn_xr = !s->scene.uilayout_sidebarcollapsed ? home_btn_x : -(sbr_w);
ui_draw_image(s->vg, home_btn_xr, home_btn_y, home_btn_w, home_btn_h, s->img_button_home, homeActive ? 1.0f : 0.65f);
const float alpha = s->active_app == cereal::UiLayoutState::App::HOME ? 1.0f : 0.65f;;
ui_draw_image(s->vg, home_btn_x, home_btn_y, home_btn_w, home_btn_h, s->img_button_home, alpha);
}
static void ui_draw_sidebar_network_strength(UIState *s) {
@@ -32,7 +28,7 @@ static void ui_draw_sidebar_network_strength(UIState *s) {
{cereal::ThermalData::NetworkStrength::GREAT, 5}};
const int network_img_h = 27;
const int network_img_w = 176;
const int network_img_x = !s->scene.uilayout_sidebarcollapsed ? 58 : -(sbr_w);
const int network_img_x = 58;
const int network_img_y = 196;
const int img_idx = s->scene.thermal.getNetworkType() == cereal::ThermalData::NetworkType::NONE ? 0 : network_strength_map[s->scene.thermal.getNetworkStrength()];
ui_draw_image(s->vg, network_img_x, network_img_y, network_img_w, network_img_h, s->img_network[img_idx], 1.0f);
@@ -41,7 +37,7 @@ static void ui_draw_sidebar_network_strength(UIState *s) {
static void ui_draw_sidebar_battery_icon(UIState *s) {
const int battery_img_h = 36;
const int battery_img_w = 76;
const int battery_img_x = !s->scene.uilayout_sidebarcollapsed ? 160 : -(sbr_w);
const int battery_img_x = 160;
const int battery_img_y = 255;
int battery_img = s->scene.thermal.getBatteryStatus() == "Charging" ? s->img_battery_charging : s->img_battery;
@@ -60,7 +56,7 @@ static void ui_draw_sidebar_network_type(UIState *s) {
{cereal::ThermalData::NetworkType::CELL3_G, "3G"},
{cereal::ThermalData::NetworkType::CELL4_G, "4G"},
{cereal::ThermalData::NetworkType::CELL5_G, "5G"}};
const int network_x = !s->scene.uilayout_sidebarcollapsed ? 50 : -(sbr_w);
const int network_x = 50;
const int network_y = 273;
const int network_w = 100;
const char *network_type = network_type_map[s->scene.thermal.getNetworkType()];
@@ -72,7 +68,7 @@ static void ui_draw_sidebar_network_type(UIState *s) {
}
static void ui_draw_sidebar_metric(UIState *s, const char* label_str, const char* value_str, const int severity, const int y_offset, const char* message_str) {
const int metric_x = !s->scene.uilayout_sidebarcollapsed ? 30 : -(sbr_w);
const int metric_x = 30;
const int metric_y = 338 + y_offset;
const int metric_w = 240;
const int metric_h = message_str ? strchr(message_str, '\n') ? 124 : 100 : 148;
selfdrive/ui/sound.hpp
+2 -2
View File
@@ -2,7 +2,7 @@
#include <map>
#include "cereal/gen/cpp/log.capnp.h"
#if defined(QCOM) || defined(QCOM2)
#if defined(QCOM)
#include <SLES/OpenSLES.h>
#include <SLES/OpenSLES_Android.h>
#endif
@@ -18,7 +18,7 @@ class Sound {
void setVolume(int volume);
~Sound();
#if defined(QCOM) || defined(QCOM2)
#if defined(QCOM)
private:
SLObjectItf engine_ = nullptr;
SLObjectItf outputMix_ = nullptr;
selfdrive/ui/ui.cc
+11 -23
View File
@@ -193,8 +193,6 @@ static void ui_init_vision(UIState *s, const VisionStreamBufs back_bufs,
int num_back_fds, const int *back_fds,
const VisionStreamBufs front_bufs, int num_front_fds,
const int *front_fds) {
const VisionUIInfo ui_info = back_bufs.buf_info.ui_info;
assert(num_back_fds == UI_BUF_COUNT);
assert(num_front_fds == UI_BUF_COUNT);
@@ -206,14 +204,7 @@ static void ui_init_vision(UIState *s, const VisionStreamBufs back_bufs,
s->scene.frontview = getenv("FRONTVIEW") != NULL;
s->scene.fullview = getenv("FULLVIEW") != NULL;
s->scene.transformed_width = ui_info.transformed_width;
s->scene.transformed_height = ui_info.transformed_height;
s->scene.front_box_x = ui_info.front_box_x;
s->scene.front_box_y = ui_info.front_box_y;
s->scene.front_box_width = ui_info.front_box_width;
s->scene.front_box_height = ui_info.front_box_height;
s->scene.world_objects_visible = false; // Invisible until we receive a calibration message.
s->scene.gps_planner_active = false;
s->rgb_width = back_bufs.width;
s->rgb_height = back_bufs.height;
@@ -225,13 +216,6 @@ static void ui_init_vision(UIState *s, const VisionStreamBufs back_bufs,
s->rgb_front_stride = front_bufs.stride;
s->rgb_front_buf_len = front_bufs.buf_len;
s->rgb_transform = (mat4){{
2.0f/s->rgb_width, 0.0f, 0.0f, -1.0f,
0.0f, 2.0f/s->rgb_height, 0.0f, -1.0f,
0.0f, 0.0f, 1.0f, 0.0f,
0.0f, 0.0f, 0.0f, 1.0f,
}};
read_param(&s->speed_lim_off, "SpeedLimitOffset");
read_param(&s->is_metric, "IsMetric");
read_param(&s->longitudinal_control, "LongitudinalControl");
@@ -285,8 +269,7 @@ void handle_message(UIState *s, SubMaster &sm) {
auto event = sm["controlsState"];
scene.controls_state = event.getControlsState();
s->controls_timeout = 1 * UI_FREQ;
scene.frontview = scene.controls_state.getRearViewCam();
if (!scene.frontview){ s->controls_seen = true; }
s->controls_seen = true;
auto alert_sound = scene.controls_state.getAlertSound();
if (scene.alert_type.compare(scene.controls_state.getAlertType()) != 0) {
@@ -379,12 +362,17 @@ void handle_message(UIState *s, SubMaster &sm) {
scene.driver_state = sm["driverState"].getDriverState();
}
if (sm.updated("dMonitoringState")) {
auto data = sm["dMonitoringState"].getDMonitoringState();
scene.is_rhd = data.getIsRHD();
s->preview_started = data.getIsPreview();
scene.dmonitoring_state = sm["dMonitoringState"].getDMonitoringState();
scene.is_rhd = scene.dmonitoring_state.getIsRHD();
scene.frontview = scene.dmonitoring_state.getIsPreview();
}
s->started = scene.thermal.getStarted() || s->preview_started;
// timeout on frontview
if ((sm.frame - sm.rcv_frame("dMonitoringState")) > 1*UI_FREQ) {
scene.frontview = false;
}
s->started = scene.thermal.getStarted() || scene.frontview;
// Handle onroad/offroad transition
if (!s->started) {
if (s->status != STATUS_STOPPED) {
@@ -831,7 +819,7 @@ int main(int argc, char* argv[]) {
if (s->controls_timeout > 0) {
s->controls_timeout--;
} else if (s->started) {
} else if (s->started && !s->scene.frontview) {
if (!s->controls_seen) {
// car is started, but controlsState hasn't been seen at all
s->scene.alert_text1 = "openpilot Unavailable";
selfdrive/ui/ui.hpp
+1 -10
View File
@@ -92,8 +92,6 @@ typedef struct UIScene {
int frontview;
int fullview;
int transformed_width, transformed_height;
ModelData model;
float mpc_x[50];
@@ -114,16 +112,11 @@ typedef struct UIScene {
int ui_viz_rw;
int ui_viz_ro;
int front_box_x, front_box_y, front_box_width, front_box_height;
std::string alert_text1;
std::string alert_text2;
std::string alert_type;
cereal::ControlsState::AlertSize alert_size;
// Used to show gps planner status
bool gps_planner_active;
cereal::HealthData::HwType hwType;
int satelliteCount;
uint8_t athenaStatus;
@@ -132,6 +125,7 @@ typedef struct UIScene {
cereal::RadarState::LeadData::Reader lead_data[2];
cereal::ControlsState::Reader controls_state;
cereal::DriverState::Reader driver_state;
cereal::DMonitoringState::Reader dmonitoring_state;
} UIScene;
typedef struct {
@@ -160,7 +154,6 @@ typedef struct UIState {
NVGcontext *vg;
// fonts and images
int font_courbd;
int font_sans_regular;
int font_sans_semibold;
int font_sans_bold;
@@ -203,7 +196,6 @@ typedef struct UIState {
int rgb_width, rgb_height, rgb_stride;
size_t rgb_buf_len;
mat4 rgb_transform;
int rgb_front_width, rgb_front_height, rgb_front_stride;
size_t rgb_front_buf_len;
@@ -233,7 +225,6 @@ typedef struct UIState {
float alert_blinking_alpha;
bool alert_blinked;
bool started;
bool preview_started;
bool vision_seen;
std::atomic<float> light_sensor;
selfdrive/updated.py
+161 -189
View File
@@ -26,48 +26,59 @@ import os
import datetime
import subprocess
import psutil
from stat import S_ISREG, S_ISDIR, S_ISLNK, S_IMODE, ST_MODE, ST_INO, ST_UID, ST_GID, ST_ATIME, ST_MTIME
import shutil
import signal
from pathlib import Path
import fcntl
import threading
import time
import threading
from cffi import FFI
from pathlib import Path
from common.basedir import BASEDIR
from common.params import Params
from selfdrive.swaglog import cloudlog
from selfdrive.controls.lib.alertmanager import set_offroad_alert
STAGING_ROOT = "/data/safe_staging"
TEST_IP = os.getenv("UPDATER_TEST_IP", "8.8.8.8")
LOCK_FILE = os.getenv("UPDATER_LOCK_FILE", "/tmp/safe_staging_overlay.lock")
STAGING_ROOT = os.getenv("UPDATER_STAGING_ROOT", "/data/safe_staging")
NEOS_VERSION = os.getenv("UPDATER_NEOS_VERSION", "/VERSION")
NEOSUPDATE_DIR = os.getenv("UPDATER_NEOSUPDATE_DIR", "/data/neoupdate")
OVERLAY_UPPER = os.path.join(STAGING_ROOT, "upper")
OVERLAY_METADATA = os.path.join(STAGING_ROOT, "metadata")
OVERLAY_MERGED = os.path.join(STAGING_ROOT, "merged")
FINALIZED = os.path.join(STAGING_ROOT, "finalized")
NICE_LOW_PRIORITY = ["nice", "-n", "19"]
SHORT = os.getenv("SHORT") is not None
# Workaround for the EON/termux build of Python having os.link removed.
# Workaround for lack of os.link in the NEOS/termux python
ffi = FFI()
ffi.cdef("int link(const char *oldpath, const char *newpath);")
libc = ffi.dlopen(None)
def link(src, dest):
return libc.link(src.encode(), dest.encode())
class WaitTimeHelper:
ready_event = threading.Event()
shutdown = False
def __init__(self):
def __init__(self, proc):
self.proc = proc
self.ready_event = threading.Event()
self.shutdown = False
signal.signal(signal.SIGTERM, self.graceful_shutdown)
signal.signal(signal.SIGINT, self.graceful_shutdown)
signal.signal(signal.SIGHUP, self.update_now)
def graceful_shutdown(self, signum, frame):
# umount -f doesn't appear effective in avoiding "device busy" on EON,
# umount -f doesn't appear effective in avoiding "device busy" on NEOS,
# so don't actually die until the next convenient opportunity in main().
cloudlog.info("caught SIGINT/SIGTERM, dismounting overlay at next opportunity")
# forward the signal to all our child processes
child_procs = self.proc.children(recursive=True)
for p in child_procs:
p.send_signal(signum)
self.shutdown = True
self.ready_event.set()
@@ -75,42 +86,27 @@ class WaitTimeHelper:
cloudlog.info("caught SIGHUP, running update check immediately")
self.ready_event.set()
def wait_between_updates(ready_event):
ready_event.clear()
if SHORT:
ready_event.wait(timeout=10)
else:
ready_event.wait(timeout=60 * 10)
def sleep(self, t):
self.ready_event.wait(timeout=t)
def link(src, dest):
# Workaround for the EON/termux build of Python having os.link removed.
return libc.link(src.encode(), dest.encode())
def run(cmd, cwd=None):
def run(cmd, cwd=None, low_priority=False):
if low_priority:
cmd = ["nice", "-n", "19"] + cmd
return subprocess.check_output(cmd, cwd=cwd, stderr=subprocess.STDOUT, encoding='utf8')
def remove_consistent_flag():
def set_consistent_flag(consistent):
os.system("sync")
consistent_file = Path(os.path.join(FINALIZED, ".overlay_consistent"))
try:
if consistent:
consistent_file.touch()
elif not consistent and consistent_file.exists():
consistent_file.unlink()
except FileNotFoundError:
pass
os.system("sync")
def set_consistent_flag():
consistent_file = Path(os.path.join(FINALIZED, ".overlay_consistent"))
os.system("sync")
consistent_file.touch()
os.system("sync")
def set_update_available_params(new_version=False):
def set_update_available_params(new_version):
params = Params()
t = datetime.datetime.utcnow().isoformat()
@@ -134,39 +130,35 @@ def dismount_ovfs():
def setup_git_options(cwd):
# We sync FS object atimes (which EON doesn't use) and mtimes, but ctimes
# We sync FS object atimes (which NEOS doesn't use) and mtimes, but ctimes
# are outside user control. Make sure Git is set up to ignore system ctimes,
# because they change when we make hard links during finalize. Otherwise,
# there is a lot of unnecessary churn. This appears to be a common need on
# OSX as well: https://www.git-tower.com/blog/make-git-rebase-safe-on-osx/
try:
trustctime = run(["git", "config", "--get", "core.trustctime"], cwd)
trustctime_set = (trustctime.strip() == "false")
except subprocess.CalledProcessError:
trustctime_set = False
if not trustctime_set:
cloudlog.info("Setting core.trustctime false")
run(["git", "config", "core.trustctime", "false"], cwd)
# We are temporarily using copytree to copy the directory, which also changes
# We are using copytree to copy the directory, which also changes
# inode numbers. Ignore those changes too.
try:
checkstat = run(["git", "config", "--get", "core.checkStat"], cwd)
checkstat_set = (checkstat.strip() == "minimal")
except subprocess.CalledProcessError:
checkstat_set = False
git_cfg = [
("core.trustctime", "false"),
("core.checkStat", "minimal"),
]
for option, value in git_cfg:
try:
ret = run(["git", "config", "--get", option], cwd)
config_ok = ret.strip() == value
except subprocess.CalledProcessError:
config_ok = False
if not checkstat_set:
cloudlog.info("Setting core.checkState minimal")
run(["git", "config", "core.checkStat", "minimal"], cwd)
if not config_ok:
cloudlog.info(f"Setting git '{option}' to '{value}'")
run(["git", "config", option, value], cwd)
def init_ovfs():
cloudlog.info("preparing new safe staging area")
Params().put("UpdateAvailable", "0")
remove_consistent_flag()
set_consistent_flag(False)
dismount_ovfs()
if os.path.isdir(STAGING_ROOT):
@@ -174,6 +166,7 @@ def init_ovfs():
for dirname in [STAGING_ROOT, OVERLAY_UPPER, OVERLAY_METADATA, OVERLAY_MERGED, FINALIZED]:
os.mkdir(dirname, 0o755)
if not os.lstat(BASEDIR).st_dev == os.lstat(OVERLAY_MERGED).st_dev:
raise RuntimeError("base and overlay merge directories are on different filesystems; not valid for overlay FS!")
@@ -181,100 +174,39 @@ def init_ovfs():
if os.path.isfile(os.path.join(BASEDIR, ".overlay_consistent")):
os.remove(os.path.join(BASEDIR, ".overlay_consistent"))
# Leave a timestamped canary in BASEDIR to check at startup. The EON clock
# Leave a timestamped canary in BASEDIR to check at startup. The device clock
# should be correct by the time we get here. If the init file disappears, or
# critical mtimes in BASEDIR are newer than .overlay_init, continue.sh can
# assume that BASEDIR has used for local development or otherwise modified,
# and skips the update activation attempt.
Path(os.path.join(BASEDIR, ".overlay_init")).touch()
os.system("sync")
overlay_opts = f"lowerdir={BASEDIR},upperdir={OVERLAY_UPPER},workdir={OVERLAY_METADATA}"
run(["mount", "-t", "overlay", "-o", overlay_opts, "none", OVERLAY_MERGED])
def inodes_in_tree(search_dir):
"""Given a search root, produce a dictionary mapping of inodes to relative
pathnames of regular files (no directories, symlinks, or special files)."""
inode_map = {}
for root, _, files in os.walk(search_dir, topdown=True):
for file_name in files:
full_path_name = os.path.join(root, file_name)
st = os.lstat(full_path_name)
if S_ISREG(st[ST_MODE]):
inode_map[st[ST_INO]] = full_path_name
return inode_map
def dup_ovfs_object(inode_map, source_obj, target_dir):
"""Given a relative pathname to copy, and a new target root, duplicate the
source object in the target root, using hardlinks for regular files."""
source_full_path = os.path.join(OVERLAY_MERGED, source_obj)
st = os.lstat(source_full_path)
target_full_path = os.path.join(target_dir, source_obj)
if S_ISREG(st[ST_MODE]):
# Hardlink all regular files; ownership and permissions are shared.
link(inode_map[st[ST_INO]], target_full_path)
else:
# Recreate all directories and symlinks; copy ownership and permissions.
if S_ISDIR(st[ST_MODE]):
os.mkdir(os.path.join(FINALIZED, source_obj), S_IMODE(st[ST_MODE]))
elif S_ISLNK(st[ST_MODE]):
os.symlink(os.readlink(source_full_path), target_full_path)
os.chmod(target_full_path, S_IMODE(st[ST_MODE]), follow_symlinks=False)
else:
# Ran into a FIFO, socket, etc. Should not happen in OP install dir.
# Ignore without copying for the time being; revisit later if needed.
cloudlog.error("can't copy this file type: %s" % source_full_path)
os.chown(target_full_path, st[ST_UID], st[ST_GID], follow_symlinks=False)
# Sync target mtimes to the cached lstat() value from each source object.
# Restores shared inode mtimes after linking, fixes symlinks and dirs.
os.utime(target_full_path, (st[ST_ATIME], st[ST_MTIME]), follow_symlinks=False)
def finalize_from_ovfs_hardlink():
"""Take the current OverlayFS merged view and finalize a copy outside of
OverlayFS, ready to be swapped-in at BASEDIR. Copy using hardlinks"""
cloudlog.info("creating finalized version of the overlay")
# The "copy" is done with hardlinks, but since the OverlayFS merge looks
# like a different filesystem, and hardlinks can't cross filesystems, we
# have to borrow a source pathname from the upper or lower layer.
inode_map = inodes_in_tree(BASEDIR)
inode_map.update(inodes_in_tree(OVERLAY_UPPER))
shutil.rmtree(FINALIZED)
os.umask(0o077)
os.mkdir(FINALIZED)
for root, dirs, files in os.walk(OVERLAY_MERGED, topdown=True):
for obj_name in dirs:
relative_path_name = os.path.relpath(os.path.join(root, obj_name), OVERLAY_MERGED)
dup_ovfs_object(inode_map, relative_path_name, FINALIZED)
for obj_name in files:
relative_path_name = os.path.relpath(os.path.join(root, obj_name), OVERLAY_MERGED)
dup_ovfs_object(inode_map, relative_path_name, FINALIZED)
cloudlog.info("done finalizing overlay")
def finalize_from_ovfs_copy():
def finalize_from_ovfs():
"""Take the current OverlayFS merged view and finalize a copy outside of
OverlayFS, ready to be swapped-in at BASEDIR. Copy using shutil.copytree"""
# Remove the update ready flag and any old updates
cloudlog.info("creating finalized version of the overlay")
set_consistent_flag(False)
shutil.rmtree(FINALIZED)
# Copy the merged overlay view and set the update ready flag
shutil.copytree(OVERLAY_MERGED, FINALIZED, symlinks=True)
set_consistent_flag(True)
cloudlog.info("done finalizing overlay")
def attempt_update():
def attempt_update(wait_helper):
cloudlog.info("attempting git update inside staging overlay")
setup_git_options(OVERLAY_MERGED)
git_fetch_output = run(NICE_LOW_PRIORITY + ["git", "fetch"], OVERLAY_MERGED)
git_fetch_output = run(["git", "fetch"], OVERLAY_MERGED, low_priority=True)
cloudlog.info("git fetch success: %s", git_fetch_output)
cur_hash = run(["git", "rev-parse", "HEAD"], OVERLAY_MERGED).rstrip()
@@ -287,106 +219,146 @@ def attempt_update():
cloudlog.info("comparing %s to %s" % (cur_hash, upstream_hash))
if new_version or git_fetch_result:
cloudlog.info("Running update")
if new_version:
cloudlog.info("git reset in progress")
r = [
run(NICE_LOW_PRIORITY + ["git", "reset", "--hard", "@{u}"], OVERLAY_MERGED),
run(NICE_LOW_PRIORITY + ["git", "clean", "-xdf"], OVERLAY_MERGED),
run(NICE_LOW_PRIORITY + ["git", "submodule", "init"], OVERLAY_MERGED),
run(NICE_LOW_PRIORITY + ["git", "submodule", "update"], OVERLAY_MERGED),
run(["git", "reset", "--hard", "@{u}"], OVERLAY_MERGED, low_priority=True),
run(["git", "clean", "-xdf"], OVERLAY_MERGED, low_priority=True ),
run(["git", "submodule", "init"], OVERLAY_MERGED, low_priority=True),
run(["git", "submodule", "update"], OVERLAY_MERGED, low_priority=True),
]
cloudlog.info("git reset success: %s", '\n'.join(r))
# Un-set the validity flag to prevent the finalized tree from being
# activated later if the finalize step is interrupted
remove_consistent_flag()
# Download the accompanying NEOS version if it doesn't match the current version
with open(NEOS_VERSION, "r") as f:
cur_neos = f.read().strip()
finalize_from_ovfs_copy()
updated_neos = run(["bash", "-c", r"unset REQUIRED_NEOS_VERSION && source launch_env.sh && \
echo -n $REQUIRED_NEOS_VERSION"], OVERLAY_MERGED).strip()
# Make sure the validity flag lands on disk LAST, only when the local git
# repo and OP install are in a consistent state.
set_consistent_flag()
cloudlog.info(f"NEOS version check: {cur_neos} vs {updated_neos}")
if cur_neos != updated_neos:
cloudlog.info(f"Beginning background download for NEOS {updated_neos}")
cloudlog.info("update successful!")
set_offroad_alert("Offroad_NeosUpdate", True)
updater_path = os.path.join(OVERLAY_MERGED, "installer/updater/updater")
update_manifest = f"file://{OVERLAY_MERGED}/installer/updater/update.json"
neos_downloaded = False
start_time = time.monotonic()
# Try to download for one day
while (time.monotonic() - start_time < 60*60*24) and not wait_helper.shutdown:
wait_helper.ready_event.clear()
try:
run([updater_path, "bgcache", update_manifest], OVERLAY_MERGED, low_priority=True)
neos_downloaded = True
break
except subprocess.CalledProcessError:
cloudlog.info("NEOS background download failed, retrying")
wait_helper.sleep(120)
# If the download failed, we'll show the alert again when we retry
set_offroad_alert("Offroad_NeosUpdate", False)
if not neos_downloaded:
raise Exception("Failed to download NEOS update")
cloudlog.info(f"NEOS background download successful, took {time.monotonic() - start_time} seconds")
# Create the finalized, ready-to-swap update
finalize_from_ovfs()
cloudlog.info("openpilot update successful!")
else:
cloudlog.info("nothing new from git at this time")
set_update_available_params(new_version=new_version)
set_update_available_params(new_version)
return new_version
def main():
update_failed_count = 0
overlay_init_done = False
params = Params()
if params.get("DisableUpdates") == b"1":
raise RuntimeError("updates are disabled by param")
raise RuntimeError("updates are disabled by the DisableUpdates param")
if not os.geteuid() == 0:
if os.geteuid() != 0:
raise RuntimeError("updated must be launched as root!")
# Set low io priority
p = psutil.Process()
proc = psutil.Process()
if psutil.LINUX:
p.ionice(psutil.IOPRIO_CLASS_BE, value=7)
proc.ionice(psutil.IOPRIO_CLASS_BE, value=7)
ov_lock_fd = open('/tmp/safe_staging_overlay.lock', 'w')
ov_lock_fd = open(LOCK_FILE, 'w')
try:
fcntl.flock(ov_lock_fd, fcntl.LOCK_EX | fcntl.LOCK_NB)
except IOError:
raise RuntimeError("couldn't get overlay lock; is another updated running?")
# Wait a short time before our first update attempt
# Avoids race with IsOffroad not being set, reduces manager startup load
time.sleep(30)
wait_helper = WaitTimeHelper()
# Wait for IsOffroad to be set before our first update attempt
wait_helper = WaitTimeHelper(proc)
wait_helper.sleep(30)
while True:
update_failed_count += 1
update_failed_count = 0
update_available = False
overlay_initialized = False
while not wait_helper.shutdown:
wait_helper.ready_event.clear()
# Check for internet every 30s
time_wrong = datetime.datetime.utcnow().year < 2019
ping_failed = subprocess.call(["ping", "-W", "4", "-c", "1", "8.8.8.8"])
ping_failed = os.system(f"ping -W 4 -c 1 {TEST_IP}") != 0
if ping_failed or time_wrong:
wait_helper.sleep(30)
continue
# Wait until we have a valid datetime to initialize the overlay
if not (ping_failed or time_wrong):
try:
# If the git directory has modifcations after we created the overlay
# we need to recreate the overlay
if overlay_init_done:
overlay_init_fn = os.path.join(BASEDIR, ".overlay_init")
git_dir_path = os.path.join(BASEDIR, ".git")
new_files = run(["find", git_dir_path, "-newer", overlay_init_fn])
# Attempt an update
exception = None
update_failed_count += 1
try:
# Re-create the overlay if BASEDIR/.git has changed since we created the overlay
if overlay_initialized:
overlay_init_fn = os.path.join(BASEDIR, ".overlay_init")
git_dir_path = os.path.join(BASEDIR, ".git")
new_files = run(["find", git_dir_path, "-newer", overlay_init_fn])
if len(new_files.splitlines()):
cloudlog.info(".git directory changed, recreating overlay")
overlay_init_done = False
if len(new_files.splitlines()):
cloudlog.info(".git directory changed, recreating overlay")
overlay_initialized = False
if not overlay_init_done:
init_ovfs()
overlay_init_done = True
if not overlay_initialized:
init_ovfs()
overlay_initialized = True
if params.get("IsOffroad") == b"1":
attempt_update()
update_failed_count = 0
else:
cloudlog.info("not running updater, openpilot running")
if params.get("IsOffroad") == b"1":
update_available = attempt_update(wait_helper) or update_available
update_failed_count = 0
if not update_available and os.path.isdir(NEOSUPDATE_DIR):
shutil.rmtree(NEOSUPDATE_DIR)
else:
cloudlog.info("not running updater, openpilot running")
except subprocess.CalledProcessError as e:
cloudlog.event(
"update process failed",
cmd=e.cmd,
output=e.output,
returncode=e.returncode
)
overlay_init_done = False
except Exception:
cloudlog.exception("uncaught updated exception, shouldn't happen")
except subprocess.CalledProcessError as e:
cloudlog.event(
"update process failed",
cmd=e.cmd,
output=e.output,
returncode=e.returncode
)
exception = e
overlay_initialized = False
except Exception:
cloudlog.exception("uncaught updated exception, shouldn't happen")
params.put("UpdateFailedCount", str(update_failed_count))
wait_between_updates(wait_helper.ready_event)
if wait_helper.shutdown:
break
if exception is None:
params.delete("LastUpdateException")
else:
params.put("LastUpdateException", f"command failed: {exception.cmd}\n{exception.output}")
# Wait 10 minutes between update attempts
wait_helper.sleep(60*10)
# We've been signaled to shut down
dismount_ovfs()
if __name__ == "__main__":
selfdrive/version.py
+47 -40
View File
@@ -1,47 +1,47 @@
#!/usr/bin/env python3
import os
import subprocess
from common.basedir import BASEDIR
from selfdrive.swaglog import cloudlog
def get_git_commit(default=None):
def run_cmd(cmd):
return subprocess.check_output(cmd, encoding='utf8').strip()
def run_cmd_default(cmd, default=None):
try:
return subprocess.check_output(["git", "rev-parse", "HEAD"], encoding='utf8').strip()
return run_cmd(cmd)
except subprocess.CalledProcessError:
return default
def get_git_commit(branch="HEAD", default=None):
return run_cmd_default(["git", "rev-parse", branch], default=default)
def get_git_branch(default=None):
try:
return subprocess.check_output(["git", "rev-parse", "--abbrev-ref", "HEAD"], encoding='utf8').strip()
except subprocess.CalledProcessError:
return default
return run_cmd_default(["git", "rev-parse", "--abbrev-ref", "HEAD"], default=default)
def get_git_full_branchname(default=None):
try:
return subprocess.check_output(["git", "rev-parse", "--abbrev-ref", "--symbolic-full-name", "@{u}"], encoding='utf8').strip()
except subprocess.CalledProcessError:
return default
return run_cmd_default(["git", "rev-parse", "--abbrev-ref", "--symbolic-full-name", "@{u}"], default=default)
def get_git_remote(default=None):
try:
local_branch = subprocess.check_output(["git", "name-rev", "--name-only", "HEAD"], encoding='utf8').strip()
tracking_remote = subprocess.check_output(["git", "config", "branch." + local_branch + ".remote"], encoding='utf8').strip()
return subprocess.check_output(["git", "config", "remote." + tracking_remote + ".url"], encoding='utf8').strip()
except subprocess.CalledProcessError:
try:
# Not on a branch, fallback
return subprocess.check_output(["git", "config", "--get", "remote.origin.url"], encoding='utf8').strip()
except subprocess.CalledProcessError:
return default
local_branch = run_cmd(["git", "name-rev", "--name-only", "HEAD"])
tracking_remote = run_cmd(["git", "config", "branch." + local_branch + ".remote"])
return run_cmd(["git", "config", "remote." + tracking_remote + ".url"])
except subprocess.CalledProcessError: # Not on a branch, fallback
return run_cmd_default(["git", "config", "--get", "remote.origin.url"], default=default)
with open(os.path.join(os.path.dirname(os.path.abspath(__file__)), "common", "version.h")) as _versionf:
version = _versionf.read().split('"')[1]
prebuilt = os.path.exists(os.path.join(BASEDIR, 'prebuilt'))
training_version = b"0.2.0"
terms_version = b"2"
@@ -51,35 +51,42 @@ tested_branch = False
origin = get_git_remote()
branch = get_git_full_branchname()
try:
# This is needed otherwise touched files might show up as modified
if (origin is not None) and (branch is not None):
try:
subprocess.check_call(["git", "update-index", "--refresh"])
except subprocess.CalledProcessError:
pass
if (origin is not None) and (branch is not None):
comma_remote = origin.startswith('git@github.com:commaai') or origin.startswith('https://github.com/commaai')
tested_branch = get_git_branch() in ['devel', 'release2-staging', 'dashcam-staging', 'release2', 'dashcam']
dirty = not comma_remote
dirty = False
# Actually check dirty files
if not prebuilt:
# This is needed otherwise touched files might show up as modified
try:
subprocess.check_call(["git", "update-index", "--refresh"])
except subprocess.CalledProcessError:
pass
dirty = (subprocess.call(["git", "diff-index", "--quiet", branch, "--"]) != 0)
# Log dirty files
if dirty and comma_remote:
try:
dirty_files = run_cmd(["git", "diff-index", branch, "--"])
cloudlog.event("dirty comma branch", version=version, dirty=dirty, origin=origin, branch=branch,
dirty_files=dirty_files, commit=get_git_commit(), origin_commit=get_git_commit(branch))
except subprocess.CalledProcessError:
pass
dirty = dirty or (not comma_remote)
dirty = dirty or ('master' in branch)
dirty = dirty or (subprocess.call(["git", "diff-index", "--quiet", branch, "--"]) != 0)
if dirty:
dirty_files = subprocess.check_output(["git", "diff-index", branch, "--"], encoding='utf8')
commit = subprocess.check_output(["git", "rev-parse", "--verify", "HEAD"], encoding='utf8').rstrip()
origin_commit = subprocess.check_output(["git", "rev-parse", "--verify", branch], encoding='utf8').rstrip()
cloudlog.event("dirty comma branch", version=version, dirty=dirty, origin=origin, branch=branch,
dirty_files=dirty_files, commit=commit, origin_commit=origin_commit)
except subprocess.CalledProcessError:
dirty = True
cloudlog.exception("git subprocess failed while checking dirty")
except subprocess.CalledProcessError:
dirty = True
cloudlog.exception("git subprocess failed while checking dirty")
if __name__ == "__main__":
print("Dirty: %s" % dirty)
print("Version: %s" % version)
print("Remote: %s" % origin)
print("Branch %s" % branch)
print("Branch: %s" % branch)
print("Prebuilt: %s" % prebuilt)