mirror of
https://github.com/dragonpilot/dragonpilot.git
synced 2026-09-30 11:23:44 +08:00
Merge branch 'devel' of https://github.com/commaai/openpilot into devel-unsupported-cars
This commit is contained in:
Binary file not shown.
+30
-13
@@ -1,6 +1,7 @@
|
||||
#!/usr/bin/env python2.7
|
||||
#!/usr/bin/env python3.7
|
||||
import json
|
||||
import os
|
||||
import io
|
||||
import random
|
||||
import re
|
||||
import select
|
||||
@@ -10,6 +11,7 @@ import time
|
||||
import threading
|
||||
import traceback
|
||||
import zmq
|
||||
import base64
|
||||
import requests
|
||||
import six.moves.queue
|
||||
from functools import partial
|
||||
@@ -24,6 +26,7 @@ from common.params import Params
|
||||
from selfdrive.services import service_list
|
||||
from selfdrive.swaglog import cloudlog
|
||||
from selfdrive.version import version, dirty
|
||||
from functools import reduce
|
||||
|
||||
ATHENA_HOST = os.getenv('ATHENA_HOST', 'wss://athena.comma.ai')
|
||||
HANDLER_THREADS = os.getenv('HANDLER_THREADS', 4)
|
||||
@@ -41,10 +44,11 @@ def handle_long_poll(ws):
|
||||
threading.Thread(target=ws_send, args=(ws, end_event))
|
||||
] + [
|
||||
threading.Thread(target=jsonrpc_handler, args=(end_event,))
|
||||
for x in xrange(HANDLER_THREADS)
|
||||
for x in range(HANDLER_THREADS)
|
||||
]
|
||||
|
||||
map(lambda thread: thread.start(), threads)
|
||||
for thread in threads:
|
||||
thread.start()
|
||||
try:
|
||||
while not end_event.is_set():
|
||||
time.sleep(0.1)
|
||||
@@ -101,7 +105,7 @@ def startLocalProxy(global_end_event, remote_ws_uri, local_port):
|
||||
raise Exception("Requested local port not whitelisted")
|
||||
|
||||
params = Params()
|
||||
dongle_id = params.get("DongleId")
|
||||
dongle_id = params.get("DongleId").decode('utf8')
|
||||
identity_token = Api(dongle_id).get_token()
|
||||
ws = create_connection(remote_ws_uri,
|
||||
cookie="jwt=" + identity_token,
|
||||
@@ -117,8 +121,8 @@ def startLocalProxy(global_end_event, remote_ws_uri, local_port):
|
||||
threading.Thread(target=ws_proxy_recv, args=(ws, local_sock, ssock, proxy_end_event, global_end_event)),
|
||||
threading.Thread(target=ws_proxy_send, args=(ws, local_sock, csock, proxy_end_event))
|
||||
]
|
||||
|
||||
map(lambda thread: thread.start(), threads)
|
||||
for thread in threads:
|
||||
thread.start()
|
||||
|
||||
return {"success": 1}
|
||||
except Exception as e:
|
||||
@@ -135,16 +139,15 @@ def getPublicKey():
|
||||
|
||||
@dispatcher.add_method
|
||||
def getSshAuthorizedKeys():
|
||||
with open('/system/comma/home/.ssh/authorized_keys', 'r') as f:
|
||||
return f.read()
|
||||
return Params().get("GithubSshKeys", encoding='utf8') or ''
|
||||
|
||||
@dispatcher.add_method
|
||||
def getSimInfo():
|
||||
sim_state = subprocess.check_output(['getprop', 'gsm.sim.state']).strip().split(',')
|
||||
network_type = subprocess.check_output(['getprop', 'gsm.network.type']).strip().split(',')
|
||||
mcc_mnc = subprocess.check_output(['getprop', 'gsm.sim.operator.numeric']).strip() or None
|
||||
sim_state = subprocess.check_output(['getprop', 'gsm.sim.state'], encoding='utf8').strip().split(',') # pylint: disable=unexpected-keyword-arg
|
||||
network_type = subprocess.check_output(['getprop', 'gsm.network.type'], encoding='utf8').strip().split(',') # pylint: disable=unexpected-keyword-arg
|
||||
mcc_mnc = subprocess.check_output(['getprop', 'gsm.sim.operator.numeric'], encoding='utf8').strip() or None # pylint: disable=unexpected-keyword-arg
|
||||
|
||||
sim_id_aidl_out = subprocess.check_output(['service', 'call', 'iphonesubinfo', '11'])
|
||||
sim_id_aidl_out = subprocess.check_output(['service', 'call', 'iphonesubinfo', '11'], encoding='utf8') # pylint: disable=unexpected-keyword-arg
|
||||
sim_id_aidl_lines = sim_id_aidl_out.split('\n')
|
||||
if len(sim_id_aidl_lines) > 3:
|
||||
sim_id_lines = sim_id_aidl_lines[1:4]
|
||||
@@ -160,6 +163,20 @@ def getSimInfo():
|
||||
'sim_state': sim_state
|
||||
}
|
||||
|
||||
@dispatcher.add_method
|
||||
def takeSnapshot():
|
||||
from selfdrive.visiond.snapshot.snapshot import snapshot, jpeg_write
|
||||
ret = snapshot()
|
||||
if ret is not None:
|
||||
def b64jpeg(x):
|
||||
f = io.BytesIO()
|
||||
jpeg_write(f, x)
|
||||
return base64.b64encode(f.getvalue()).decode("utf-8")
|
||||
return {'jpegBack': b64jpeg(ret[0]),
|
||||
'jpegFront': b64jpeg(ret[1])}
|
||||
else:
|
||||
raise Exception("not available while visiond is started")
|
||||
|
||||
def ws_proxy_recv(ws, local_sock, ssock, end_event, global_end_event):
|
||||
while not (end_event.is_set() or global_end_event.is_set()):
|
||||
try:
|
||||
@@ -221,7 +238,7 @@ def backoff(retries):
|
||||
|
||||
def main(gctx=None):
|
||||
params = Params()
|
||||
dongle_id = params.get("DongleId")
|
||||
dongle_id = params.get("DongleId").decode('utf-8')
|
||||
ws_uri = ATHENA_HOST + "/ws/v2/" + dongle_id
|
||||
|
||||
crash.bind_user(id=dongle_id)
|
||||
|
||||
@@ -78,7 +78,7 @@ boardd.o: boardd.cc
|
||||
|
||||
|
||||
boardd_api_impl.so: libcan_list_to_can_capnp.a boardd_api_impl.pyx boardd_setup.py
|
||||
python2 boardd_setup.py build_ext --inplace
|
||||
python3 boardd_setup.py build_ext --inplace
|
||||
rm -rf build
|
||||
rm -f boardd_api_impl.cpp
|
||||
|
||||
|
||||
+60
-81
@@ -32,26 +32,9 @@
|
||||
#define RECV_SIZE (0x1000)
|
||||
#define TIMEOUT 0
|
||||
|
||||
#define SAFETY_NOOUTPUT 0
|
||||
#define SAFETY_HONDA 1
|
||||
#define SAFETY_TOYOTA 2
|
||||
#define SAFETY_GM 3
|
||||
#define SAFETY_HONDA_BOSCH 4
|
||||
#define SAFETY_FORD 5
|
||||
#define SAFETY_CADILLAC 6
|
||||
#define SAFETY_HYUNDAI 7
|
||||
#define SAFETY_TESLA 8
|
||||
#define SAFETY_CHRYSLER 9
|
||||
#define SAFETY_SUBARU 10
|
||||
#define SAFETY_GM_PASSIVE 11
|
||||
#define SAFETY_TOYOTA_IPAS 0x1335
|
||||
#define SAFETY_TOYOTA_NOLIMITS 0x1336
|
||||
#define SAFETY_ALLOUTPUT 0x1337
|
||||
#define SAFETY_ELM327 0xE327 // diagnostic only
|
||||
|
||||
namespace {
|
||||
|
||||
volatile int do_exit = 0;
|
||||
volatile sig_atomic_t do_exit = 0;
|
||||
|
||||
libusb_context *ctx = NULL;
|
||||
libusb_device_handle *dev_handle;
|
||||
@@ -62,6 +45,10 @@ bool fake_send = false;
|
||||
bool loopback_can = false;
|
||||
cereal::HealthData::HwType hw_type = cereal::HealthData::HwType::UNKNOWN;
|
||||
bool is_pigeon = false;
|
||||
const uint32_t NO_IGNITION_CNT_MAX = 2 * 60 * 60 * 24 * 3; // turn off charge after 3 days
|
||||
uint32_t no_ignition_cnt = 0;
|
||||
bool connected_once = false;
|
||||
uint8_t ignition_last = 0;
|
||||
|
||||
pthread_t safety_setter_thread_handle = -1;
|
||||
pthread_t pigeon_thread_handle = -1;
|
||||
@@ -89,8 +76,8 @@ void *safety_setter_thread(void *s) {
|
||||
|
||||
pthread_mutex_lock(&usb_lock);
|
||||
|
||||
// VIN qury done, stop listening to OBDII
|
||||
libusb_control_transfer(dev_handle, 0x40, 0xdc, SAFETY_NOOUTPUT, 0, NULL, 0, TIMEOUT);
|
||||
// VIN query done, stop listening to OBDII
|
||||
libusb_control_transfer(dev_handle, 0x40, 0xdc, (uint16_t)(cereal::CarParams::SafetyModel::NO_OUTPUT), 0, NULL, 0, TIMEOUT);
|
||||
|
||||
pthread_mutex_unlock(&usb_lock);
|
||||
|
||||
@@ -119,48 +106,6 @@ void *safety_setter_thread(void *s) {
|
||||
auto safety_param = car_params.getSafetyParam();
|
||||
LOGW("setting safety model: %d with param %d", safety_model, safety_param);
|
||||
|
||||
int safety_setting = 0;
|
||||
switch (safety_model) {
|
||||
case cereal::CarParams::SafetyModel::NO_OUTPUT:
|
||||
safety_setting = SAFETY_NOOUTPUT;
|
||||
break;
|
||||
case cereal::CarParams::SafetyModel::HONDA:
|
||||
safety_setting = SAFETY_HONDA;
|
||||
break;
|
||||
case cereal::CarParams::SafetyModel::TOYOTA:
|
||||
safety_setting = SAFETY_TOYOTA;
|
||||
break;
|
||||
case cereal::CarParams::SafetyModel::ELM327:
|
||||
safety_setting = SAFETY_ELM327;
|
||||
break;
|
||||
case cereal::CarParams::SafetyModel::GM:
|
||||
safety_setting = SAFETY_GM;
|
||||
break;
|
||||
case cereal::CarParams::SafetyModel::GM_PASSIVE:
|
||||
safety_setting = SAFETY_GM_PASSIVE;
|
||||
break;
|
||||
case cereal::CarParams::SafetyModel::HONDA_BOSCH:
|
||||
safety_setting = SAFETY_HONDA_BOSCH;
|
||||
break;
|
||||
case cereal::CarParams::SafetyModel::FORD:
|
||||
safety_setting = SAFETY_FORD;
|
||||
break;
|
||||
case cereal::CarParams::SafetyModel::CADILLAC:
|
||||
safety_setting = SAFETY_CADILLAC;
|
||||
break;
|
||||
case cereal::CarParams::SafetyModel::HYUNDAI:
|
||||
safety_setting = SAFETY_HYUNDAI;
|
||||
break;
|
||||
case cereal::CarParams::SafetyModel::CHRYSLER:
|
||||
safety_setting = SAFETY_CHRYSLER;
|
||||
break;
|
||||
case cereal::CarParams::SafetyModel::SUBARU:
|
||||
safety_setting = SAFETY_SUBARU;
|
||||
break;
|
||||
default:
|
||||
LOGE("unknown safety model: %d", safety_model);
|
||||
}
|
||||
|
||||
pthread_mutex_lock(&usb_lock);
|
||||
|
||||
// set in the mutex to avoid race
|
||||
@@ -169,7 +114,7 @@ void *safety_setter_thread(void *s) {
|
||||
// set if long_control is allowed by openpilot. Hardcoded to True for now
|
||||
libusb_control_transfer(dev_handle, 0x40, 0xdf, 1, 0, NULL, 0, TIMEOUT);
|
||||
|
||||
libusb_control_transfer(dev_handle, 0x40, 0xdc, safety_setting, safety_param, NULL, 0, TIMEOUT);
|
||||
libusb_control_transfer(dev_handle, 0x40, 0xdc, (uint16_t)(cereal::CarParams::SafetyModel(safety_model)), safety_param, NULL, 0, TIMEOUT);
|
||||
|
||||
pthread_mutex_unlock(&usb_lock);
|
||||
|
||||
@@ -180,6 +125,7 @@ void *safety_setter_thread(void *s) {
|
||||
bool usb_connect() {
|
||||
int err;
|
||||
unsigned char hw_query[1] = {0};
|
||||
ignition_last = 0;
|
||||
|
||||
dev_handle = libusb_open_device_with_vid_pid(ctx, 0xbbaa, 0xddcc);
|
||||
if (dev_handle == NULL) { goto fail; }
|
||||
@@ -197,25 +143,20 @@ bool usb_connect() {
|
||||
// power off ESP
|
||||
libusb_control_transfer(dev_handle, 0xc0, 0xd9, 0, 0, NULL, 0, TIMEOUT);
|
||||
|
||||
// power on charging (may trigger a reconnection, should be okay)
|
||||
#ifndef __x86_64__
|
||||
libusb_control_transfer(dev_handle, 0xc0, 0xe6, 1, 0, NULL, 0, TIMEOUT);
|
||||
#else
|
||||
LOGW("not enabling charging on x86_64");
|
||||
#endif
|
||||
|
||||
// diagnostic only is the default, needed for VIN query
|
||||
libusb_control_transfer(dev_handle, 0x40, 0xdc, SAFETY_ELM327, 0, NULL, 0, TIMEOUT);
|
||||
|
||||
if (safety_setter_thread_handle == -1) {
|
||||
err = pthread_create(&safety_setter_thread_handle, NULL, safety_setter_thread, NULL);
|
||||
assert(err == 0);
|
||||
// power on charging, only the first time. Panda can also change mode and it causes a brief disconneciton
|
||||
#ifndef __x86_64__
|
||||
if (!connected_once) {
|
||||
libusb_control_transfer(dev_handle, 0xc0, 0xe6, (uint16_t)(cereal::HealthData::UsbPowerMode::CDP), 0, NULL, 0, TIMEOUT);
|
||||
}
|
||||
#endif
|
||||
connected_once = true;
|
||||
|
||||
libusb_control_transfer(dev_handle, 0xc0, 0xc1, 0, 0, hw_query, 1, TIMEOUT);
|
||||
|
||||
hw_type = (cereal::HealthData::HwType)(hw_query[0]);
|
||||
is_pigeon = (hw_type == cereal::HealthData::HwType::GREY_PANDA) || (hw_type == cereal::HealthData::HwType::BLACK_PANDA);
|
||||
is_pigeon = (hw_type == cereal::HealthData::HwType::GREY_PANDA) ||
|
||||
(hw_type == cereal::HealthData::HwType::BLACK_PANDA) ||
|
||||
(hw_type == cereal::HealthData::HwType::UNO);
|
||||
if (is_pigeon) {
|
||||
LOGW("panda with gps detected");
|
||||
pigeon_needs_init = true;
|
||||
@@ -304,8 +245,9 @@ void can_recv(void *s) {
|
||||
|
||||
void can_health(void *s) {
|
||||
int cnt;
|
||||
int err;
|
||||
|
||||
// copied from board/main.c
|
||||
// copied from panda/board/main.c
|
||||
struct __attribute__((packed)) health {
|
||||
uint32_t voltage;
|
||||
uint32_t current;
|
||||
@@ -315,7 +257,8 @@ void can_health(void *s) {
|
||||
uint8_t started;
|
||||
uint8_t controls_allowed;
|
||||
uint8_t gas_interceptor_detected;
|
||||
uint8_t car_harness_status_pkt;
|
||||
uint8_t car_harness_status;
|
||||
uint8_t usb_power_mode;
|
||||
} health;
|
||||
|
||||
// recv from board
|
||||
@@ -330,6 +273,42 @@ void can_health(void *s) {
|
||||
|
||||
pthread_mutex_unlock(&usb_lock);
|
||||
|
||||
if (health.started == 0) {
|
||||
no_ignition_cnt += 1;
|
||||
} else {
|
||||
no_ignition_cnt = 0;
|
||||
}
|
||||
|
||||
#ifndef __x86_64__
|
||||
if ((no_ignition_cnt > NO_IGNITION_CNT_MAX) && (health.usb_power_mode == (uint8_t)(cereal::HealthData::UsbPowerMode::CDP))) {
|
||||
printf("TURN OFF CHARGING!\n");
|
||||
pthread_mutex_lock(&usb_lock);
|
||||
libusb_control_transfer(dev_handle, 0xc0, 0xe6, (uint16_t)(cereal::HealthData::UsbPowerMode::CLIENT), 0, NULL, 0, TIMEOUT);
|
||||
pthread_mutex_unlock(&usb_lock);
|
||||
}
|
||||
#endif
|
||||
|
||||
// clear VIN, CarParams, and set new safety on car start
|
||||
if ((health.started != 0) && (ignition_last == 0)) {
|
||||
|
||||
int result = delete_db_value(NULL, "CarVin");
|
||||
assert((result == 0) || (result == ERR_NO_VALUE));
|
||||
result = delete_db_value(NULL, "CarParams");
|
||||
assert((result == 0) || (result == ERR_NO_VALUE));
|
||||
|
||||
// diagnostic only is the default, needed for VIN query
|
||||
pthread_mutex_lock(&usb_lock);
|
||||
libusb_control_transfer(dev_handle, 0x40, 0xdc, (uint16_t)(cereal::CarParams::SafetyModel::ELM327), 0, NULL, 0, TIMEOUT);
|
||||
pthread_mutex_unlock(&usb_lock);
|
||||
|
||||
if (safety_setter_thread_handle == -1) {
|
||||
err = pthread_create(&safety_setter_thread_handle, NULL, safety_setter_thread, NULL);
|
||||
assert(err == 0);
|
||||
}
|
||||
}
|
||||
|
||||
ignition_last = health.started;
|
||||
|
||||
// create message
|
||||
capnp::MallocMessageBuilder msg;
|
||||
cereal::Event::Builder event = msg.initRoot<cereal::Event>();
|
||||
@@ -351,6 +330,7 @@ void can_health(void *s) {
|
||||
healthData.setCanFwdErrs(health.can_fwd_errs);
|
||||
healthData.setGmlanSendErrs(health.gmlan_send_errs);
|
||||
healthData.setHwType(hw_type);
|
||||
healthData.setUsbPowerMode(cereal::HealthData::UsbPowerMode(health.usb_power_mode));
|
||||
|
||||
// send to health
|
||||
auto words = capnp::messageToFlatArray(msg);
|
||||
@@ -481,7 +461,6 @@ void *can_recv_thread(void *crap) {
|
||||
|
||||
void *can_health_thread(void *crap) {
|
||||
LOGD("start health thread");
|
||||
|
||||
// health = 8011
|
||||
void *context = zmq_ctx_new();
|
||||
void *publisher = zmq_socket(context, ZMQ_PUB);
|
||||
@@ -628,7 +607,7 @@ void *pigeon_thread(void *crap) {
|
||||
}
|
||||
if (alen > 0) {
|
||||
if (dat[0] == (char)0x00){
|
||||
LOGW("received invalid ublox message, resetting pigeon");
|
||||
LOGW("received invalid ublox message, resetting panda GPS");
|
||||
pigeon_init();
|
||||
} else {
|
||||
pigeon_publish_raw(publisher, dat, alen);
|
||||
|
||||
@@ -1,4 +1,5 @@
|
||||
# distutils: language = c++
|
||||
# cython: language_level=3
|
||||
from libcpp.vector cimport vector
|
||||
from libcpp.string cimport string
|
||||
from libcpp cimport bool
|
||||
|
||||
@@ -1,25 +1,29 @@
|
||||
import subprocess
|
||||
from distutils.core import setup, Extension
|
||||
from distutils.core import Extension, setup # pylint: disable=import-error,no-name-in-module
|
||||
|
||||
from Cython.Build import cythonize
|
||||
|
||||
from common.cython_hacks import BuildExtWithoutPlatformSuffix
|
||||
|
||||
PHONELIBS = '../../phonelibs'
|
||||
|
||||
ARCH = subprocess.check_output(["uname", "-m"]).rstrip()
|
||||
ARCH = subprocess.check_output(["uname", "-m"], encoding='utf8').rstrip() # pylint: disable=unexpected-keyword-arg
|
||||
ARCH_DIR = 'x64' if ARCH == "x86_64" else 'aarch64'
|
||||
|
||||
setup(name='Boardd API Implementation',
|
||||
ext_modules=cythonize(
|
||||
Extension(
|
||||
"boardd_api_impl",
|
||||
libraries=[':libcan_list_to_can_capnp.a', ':libcapnp.a', ':libcapnp.a', ':libkj.a'],
|
||||
library_dirs=[
|
||||
'./',
|
||||
PHONELIBS + '/capnp-cpp/' + ARCH_DIR + '/lib/',
|
||||
PHONELIBS + '/capnp-c/' + ARCH_DIR + '/lib/'
|
||||
],
|
||||
sources=['boardd_api_impl.pyx'],
|
||||
language="c++",
|
||||
extra_compile_args=["-std=c++11"],
|
||||
)
|
||||
)
|
||||
cmdclass={'build_ext': BuildExtWithoutPlatformSuffix},
|
||||
ext_modules=cythonize(
|
||||
Extension(
|
||||
"boardd_api_impl",
|
||||
libraries=[':libcan_list_to_can_capnp.a', ':libcapnp.a', ':libcapnp.a', ':libkj.a'],
|
||||
library_dirs=[
|
||||
'./',
|
||||
PHONELIBS + '/capnp-cpp/' + ARCH_DIR + '/lib/',
|
||||
PHONELIBS + '/capnp-c/' + ARCH_DIR + '/lib/'
|
||||
],
|
||||
sources=['boardd_api_impl.pyx'],
|
||||
language="c++",
|
||||
extra_compile_args=["-std=c++11"],
|
||||
)
|
||||
)
|
||||
)
|
||||
|
||||
@@ -1,4 +1,4 @@
|
||||
#!/usr/bin/env python
|
||||
#!/usr/bin/env python3
|
||||
|
||||
# This file is not used by OpenPilot. Only boardd.cc is used.
|
||||
# The python version is slower, but has more options for development.
|
||||
@@ -15,6 +15,9 @@ from common.realtime import Ratekeeper
|
||||
from selfdrive.services import service_list
|
||||
from selfdrive.swaglog import cloudlog
|
||||
from selfdrive.boardd.boardd import can_capnp_to_can_list
|
||||
from cereal import car
|
||||
|
||||
SafetyModel = car.CarParams.SafetyModel
|
||||
|
||||
# USB is optional
|
||||
try:
|
||||
@@ -23,13 +26,6 @@ try:
|
||||
except Exception:
|
||||
pass
|
||||
|
||||
SAFETY_NOOUTPUT = 0
|
||||
SAFETY_HONDA = 1
|
||||
SAFETY_TOYOTA = 2
|
||||
SAFETY_CHRYSLER = 9
|
||||
SAFETY_TOYOTA_NOLIMITS = 0x1336
|
||||
SAFETY_ALLOUTPUT = 0x1337
|
||||
|
||||
# *** serialization functions ***
|
||||
def can_list_to_can_capnp(can_msgs, msgtype='can'):
|
||||
dat = messaging.new_message()
|
||||
@@ -41,7 +37,7 @@ def can_list_to_can_capnp(can_msgs, msgtype='can'):
|
||||
cc = dat.can[i]
|
||||
cc.address = can_msg[0]
|
||||
cc.busTime = can_msg[1]
|
||||
cc.dat = str(can_msg[2])
|
||||
cc.dat = bytes(can_msg[2])
|
||||
cc.src = can_msg[3]
|
||||
return dat
|
||||
|
||||
@@ -71,17 +67,17 @@ def can_send_many(arr):
|
||||
for addr, _, dat, alt in arr:
|
||||
if addr < 0x800: # only support 11 bit addr
|
||||
snd = struct.pack("II", ((addr << 21) | 1), len(dat) | (alt << 4)) + dat
|
||||
snd = snd.ljust(0x10, '\x00')
|
||||
snd = snd.ljust(0x10, b'\x00')
|
||||
snds.append(snd)
|
||||
while 1:
|
||||
try:
|
||||
handle.bulkWrite(3, ''.join(snds))
|
||||
handle.bulkWrite(3, b''.join(snds))
|
||||
break
|
||||
except (USBErrorIO, USBErrorOverflow):
|
||||
cloudlog.exception("CAN: BAD SEND MANY, RETRYING")
|
||||
|
||||
def can_recv():
|
||||
dat = ""
|
||||
dat = b""
|
||||
while 1:
|
||||
try:
|
||||
dat = handle.bulkRead(1, 0x10*256)
|
||||
@@ -102,10 +98,10 @@ def can_init():
|
||||
if device.getVendorID() == 0xbbaa and device.getProductID() == 0xddcc:
|
||||
handle = device.open()
|
||||
handle.claimInterface(0)
|
||||
handle.controlWrite(0x40, 0xdc, SAFETY_ALLOUTPUT, 0, b'')
|
||||
handle.controlWrite(0x40, 0xdc, SafetyModel.allOutput, 0, b'')
|
||||
|
||||
if handle is None:
|
||||
cloudlog.warn("CAN NOT FOUND")
|
||||
cloudlog.warning("CAN NOT FOUND")
|
||||
exit(-1)
|
||||
|
||||
cloudlog.info("got handle")
|
||||
@@ -113,7 +109,7 @@ def can_init():
|
||||
|
||||
def boardd_mock_loop():
|
||||
can_init()
|
||||
handle.controlWrite(0x40, 0xdc, SAFETY_ALLOUTPUT, 0, b'')
|
||||
handle.controlWrite(0x40, 0xdc, SafetyModel.allOutput, 0, b'')
|
||||
|
||||
logcan = messaging.sub_sock(service_list['can'].port)
|
||||
sendcan = messaging.pub_sock(service_list['sendcan'].port)
|
||||
@@ -124,17 +120,17 @@ def boardd_mock_loop():
|
||||
snd = []
|
||||
for s in snds:
|
||||
snd += s
|
||||
snd = filter(lambda x: x[-1] <= 2, snd)
|
||||
snd_0 = len(filter(lambda x: x[-1] == 0, snd))
|
||||
snd_1 = len(filter(lambda x: x[-1] == 1, snd))
|
||||
snd_2 = len(filter(lambda x: x[-1] == 2, snd))
|
||||
snd = list(filter(lambda x: x[-1] <= 2, snd))
|
||||
snd_0 = len(list(filter(lambda x: x[-1] == 0, snd)))
|
||||
snd_1 = len(list(filter(lambda x: x[-1] == 1, snd)))
|
||||
snd_2 = len(list(filter(lambda x: x[-1] == 2, snd)))
|
||||
can_send_many(snd)
|
||||
|
||||
# recv @ 100hz
|
||||
can_msgs = can_recv()
|
||||
got_0 = len(filter(lambda x: x[-1] == 0+0x80, can_msgs))
|
||||
got_1 = len(filter(lambda x: x[-1] == 1+0x80, can_msgs))
|
||||
got_2 = len(filter(lambda x: x[-1] == 2+0x80, can_msgs))
|
||||
got_0 = len(list(filter(lambda x: x[-1] == 0+0x80, can_msgs)))
|
||||
got_1 = len(list(filter(lambda x: x[-1] == 1+0x80, can_msgs)))
|
||||
got_2 = len(list(filter(lambda x: x[-1] == 2+0x80, can_msgs)))
|
||||
print("sent %3d (%3d/%3d/%3d) got %3d (%3d/%3d/%3d)" %
|
||||
(len(snd), snd_0, snd_1, snd_2, len(can_msgs), got_0, got_1, got_2))
|
||||
m = can_list_to_can_capnp(can_msgs, msgtype='sendcan')
|
||||
|
||||
@@ -1,4 +1,5 @@
|
||||
#!/usr/bin/env python
|
||||
#!/usr/bin/env python3
|
||||
from __future__ import print_function
|
||||
import time
|
||||
import random
|
||||
|
||||
@@ -9,9 +10,9 @@ if __name__ == "__main__":
|
||||
while 1:
|
||||
c = random.randint(0, 3)
|
||||
if c == 0:
|
||||
print can_recv()
|
||||
print(can_recv())
|
||||
elif c == 1:
|
||||
print can_health()
|
||||
print(can_health())
|
||||
elif c == 2:
|
||||
many = [[0x123, 0, "abcdef", 0]] * random.randint(1, 10)
|
||||
can_send_many(many)
|
||||
|
||||
@@ -1,4 +1,4 @@
|
||||
#!/usr/bin/env python
|
||||
#!/usr/bin/env python3
|
||||
import sys
|
||||
import time
|
||||
import signal
|
||||
|
||||
@@ -1,7 +1,7 @@
|
||||
import random
|
||||
import numpy as np
|
||||
|
||||
import boardd_old
|
||||
import selfdrive.boardd.tests.boardd_old as boardd_old
|
||||
import selfdrive.boardd.boardd as boardd
|
||||
|
||||
from common.realtime import sec_since_boot
|
||||
@@ -12,7 +12,7 @@ import unittest
|
||||
def generate_random_can_data_list():
|
||||
can_list = []
|
||||
cnt = random.randint(1, 64)
|
||||
for j in xrange(cnt):
|
||||
for j in range(cnt):
|
||||
can_data = np.random.bytes(random.randint(1, 8))
|
||||
can_list.append([random.randint(0, 128), random.randint(0, 128), can_data, random.randint(0, 128)])
|
||||
return can_list, cnt
|
||||
@@ -20,7 +20,7 @@ def generate_random_can_data_list():
|
||||
|
||||
class TestBoarddApiMethods(unittest.TestCase):
|
||||
def test_correctness(self):
|
||||
for i in xrange(1000):
|
||||
for i in range(1000):
|
||||
can_list, _ = generate_random_can_data_list()
|
||||
|
||||
# Sendcan
|
||||
@@ -31,9 +31,10 @@ class TestBoarddApiMethods(unittest.TestCase):
|
||||
|
||||
ev_old = log.Event.from_bytes(m_old)
|
||||
ev = log.Event.from_bytes(m)
|
||||
|
||||
self.assertEqual(ev_old.which(), ev.which())
|
||||
self.assertEqual(len(ev.sendcan), len(ev_old.sendcan))
|
||||
for i in xrange(len(ev.sendcan)):
|
||||
for i in range(len(ev.sendcan)):
|
||||
attrs = ['address', 'busTime', 'dat', 'src']
|
||||
for attr in attrs:
|
||||
self.assertEqual(getattr(ev.sendcan[i], attr, 'new'), getattr(ev_old.sendcan[i], attr, 'old'))
|
||||
@@ -47,7 +48,7 @@ class TestBoarddApiMethods(unittest.TestCase):
|
||||
ev = log.Event.from_bytes(m)
|
||||
self.assertEqual(ev_old.which(), ev.which())
|
||||
self.assertEqual(len(ev.can), len(ev_old.can))
|
||||
for i in xrange(len(ev.can)):
|
||||
for i in range(len(ev.can)):
|
||||
attrs = ['address', 'busTime', 'dat', 'src']
|
||||
for attr in attrs:
|
||||
self.assertEqual(getattr(ev.can[i], attr, 'new'), getattr(ev_old.can[i], attr, 'old'))
|
||||
@@ -57,14 +58,14 @@ class TestBoarddApiMethods(unittest.TestCase):
|
||||
recursions = 1000
|
||||
|
||||
n1 = sec_since_boot()
|
||||
for i in xrange(recursions):
|
||||
for i in range(recursions):
|
||||
boardd_old.can_list_to_can_capnp(can_list, 'sendcan').to_bytes()
|
||||
n2 = sec_since_boot()
|
||||
elapsed_old = n2 - n1
|
||||
|
||||
# print('Old API, elapsed time: {} secs'.format(elapsed_old))
|
||||
n1 = sec_since_boot()
|
||||
for i in xrange(recursions):
|
||||
for i in range(recursions):
|
||||
boardd.can_list_to_can_capnp(can_list)
|
||||
n2 = sec_since_boot()
|
||||
elapsed_new = n2 - n1
|
||||
|
||||
@@ -1,4 +1,4 @@
|
||||
#!/usr/bin/env python
|
||||
#!/usr/bin/env python3
|
||||
"""Run boardd with the BOARDD_LOOPBACK envvar before running this test."""
|
||||
|
||||
import os
|
||||
|
||||
@@ -36,7 +36,7 @@ endif
|
||||
|
||||
OBJDIR = obj
|
||||
|
||||
OPENDBC_PATH := $(shell python2 -c 'import opendbc; print opendbc.DBC_PATH')
|
||||
OPENDBC_PATH := $(shell python3 -c 'import opendbc; print(opendbc.DBC_PATH)')
|
||||
|
||||
DBC_SOURCES := $(sort $(wildcard $(OPENDBC_PATH)/*.dbc))
|
||||
DBC_OBJS := $(patsubst $(OPENDBC_PATH)/%.dbc,$(OBJDIR)/%.o,$(DBC_SOURCES))
|
||||
@@ -70,12 +70,12 @@ libdbc.so:: $(LIBDBC_OBJS) $(DBC_OBJS)
|
||||
$(CEREAL_LIBS)
|
||||
|
||||
packer_impl.so: packer_impl.pyx packer_setup.py
|
||||
python2 packer_setup.py build_ext --inplace
|
||||
python3 packer_setup.py build_ext --inplace
|
||||
rm -rf build
|
||||
rm -f packer_impl.cpp
|
||||
|
||||
parser_pyx.so: parser_pyx_setup.py parser_pyx.pyx parser_pyx.pxd
|
||||
python $< build_ext --inplace
|
||||
python3 $< build_ext --inplace
|
||||
rm -rf build
|
||||
rm -f parser_pyx.cpp
|
||||
|
||||
|
||||
@@ -1,11 +1,11 @@
|
||||
from collections import defaultdict
|
||||
from selfdrive.can.libdbc_py import libdbc, ffi
|
||||
|
||||
class CANDefine(object):
|
||||
class CANDefine():
|
||||
def __init__(self, dbc_name):
|
||||
self.dv = defaultdict(dict)
|
||||
self.dbc_name = dbc_name
|
||||
self.dbc = libdbc.dbc_lookup(dbc_name)
|
||||
self.dbc = libdbc.dbc_lookup(dbc_name.encode('utf8'))
|
||||
|
||||
num_vals = self.dbc[0].num_vals
|
||||
|
||||
@@ -13,16 +13,16 @@ class CANDefine(object):
|
||||
num_msgs = self.dbc[0].num_msgs
|
||||
for i in range(num_msgs):
|
||||
msg = self.dbc[0].msgs[i]
|
||||
name = ffi.string(msg.name)
|
||||
name = ffi.string(msg.name).decode('utf8')
|
||||
address = msg.address
|
||||
self.address_to_msg_name[address] = name
|
||||
|
||||
for i in range(num_vals):
|
||||
val = self.dbc[0].vals[i]
|
||||
|
||||
sgname = ffi.string(val.name)
|
||||
sgname = ffi.string(val.name).decode('utf8')
|
||||
address = val.address
|
||||
def_val = ffi.string(val.def_val)
|
||||
def_val = ffi.string(val.def_val).decode('utf8')
|
||||
|
||||
#separate definition/value pairs
|
||||
def_val = def_val.split()
|
||||
|
||||
@@ -1,4 +1,6 @@
|
||||
# distutils: language = c++
|
||||
# cython: c_string_encoding=ascii, language_level=3
|
||||
|
||||
from libc.stdint cimport uint32_t, uint64_t
|
||||
from libcpp.vector cimport vector
|
||||
from libcpp.map cimport map
|
||||
@@ -54,7 +56,7 @@ ctypedef uint64_t (*canpack_pack_vector_func)(void* inst, uint32_t address, cons
|
||||
ctypedef const DBC * (*dbc_lookup_func)(const char* dbc_name)
|
||||
|
||||
|
||||
cdef class CANPacker(object):
|
||||
cdef class CANPacker():
|
||||
cdef void *packer
|
||||
cdef const DBC *dbc
|
||||
cdef map[string, (int, int)] name_to_address_and_size
|
||||
@@ -66,11 +68,14 @@ cdef class CANPacker(object):
|
||||
def __init__(self, dbc_name):
|
||||
can_dir = os.path.dirname(os.path.abspath(__file__))
|
||||
libdbc_fn = os.path.join(can_dir, "libdbc.so")
|
||||
libdbc_fn = str(libdbc_fn).encode('utf8')
|
||||
subprocess.check_call(["make"], cwd=can_dir)
|
||||
|
||||
cdef void *libdbc = dlopen(libdbc_fn, RTLD_LAZY)
|
||||
self.canpack_init = <canpack_init_func>dlsym(libdbc, 'canpack_init')
|
||||
self.canpack_pack_vector = <canpack_pack_vector_func>dlsym(libdbc, 'canpack_pack_vector')
|
||||
self.dbc_lookup = <dbc_lookup_func>dlsym(libdbc, 'dbc_lookup')
|
||||
|
||||
self.packer = self.canpack_init(dbc_name)
|
||||
self.dbc = self.dbc_lookup(dbc_name)
|
||||
num_msgs = self.dbc[0].num_msgs
|
||||
@@ -82,8 +87,14 @@ cdef class CANPacker(object):
|
||||
cdef uint64_t pack(self, addr, values, counter):
|
||||
cdef vector[SignalPackValue] values_thing
|
||||
cdef SignalPackValue spv
|
||||
|
||||
names = []
|
||||
|
||||
for name, value in values.iteritems():
|
||||
spv.name = name
|
||||
n = name.encode('utf8')
|
||||
names.append(n) # TODO: find better way to keep reference to temp string arround
|
||||
|
||||
spv.name = n
|
||||
spv.value = value
|
||||
values_thing.push_back(spv)
|
||||
|
||||
@@ -105,7 +116,7 @@ cdef class CANPacker(object):
|
||||
addr = name_or_addr
|
||||
size = self.address_to_size[name_or_addr]
|
||||
else:
|
||||
addr, size = self.name_to_address_and_size[name_or_addr]
|
||||
addr, size = self.name_to_address_and_size[name_or_addr.encode('utf8')]
|
||||
cdef uint64_t val = self.pack(addr, values, counter)
|
||||
val = self.ReverseBytes(val)
|
||||
return [addr, 0, (<char *>&val)[:size], bus]
|
||||
|
||||
@@ -1,5 +1,9 @@
|
||||
from distutils.core import setup, Extension
|
||||
from distutils.core import Extension, setup # pylint: disable=import-error,no-name-in-module
|
||||
|
||||
from Cython.Build import cythonize
|
||||
|
||||
from common.cython_hacks import BuildExtWithoutPlatformSuffix
|
||||
|
||||
setup(name='CAN Packer API Implementation',
|
||||
cmdclass={'build_ext': BuildExtWithoutPlatformSuffix},
|
||||
ext_modules=cythonize(Extension("packer_impl", ["packer_impl.pyx"], language="c++", extra_compile_args=["-std=c++11"])))
|
||||
|
||||
@@ -1,4 +1,6 @@
|
||||
# distutils: language = c++
|
||||
#cython: language_level=3
|
||||
|
||||
from libc.stdint cimport uint32_t, uint64_t, uint16_t
|
||||
from libcpp.vector cimport vector
|
||||
from libcpp.map cimport map
|
||||
|
||||
@@ -1,4 +1,6 @@
|
||||
# distutils: language = c++
|
||||
# cython: c_string_encoding=ascii, language_level=3
|
||||
|
||||
from posix.dlfcn cimport dlopen, dlsym, RTLD_LAZY
|
||||
|
||||
from libcpp cimport bool
|
||||
@@ -8,10 +10,11 @@ import numbers
|
||||
cdef int CAN_INVALID_CNT = 5
|
||||
|
||||
cdef class CANParser:
|
||||
def __init__(self, dbc_name, signals, checks=None, bus=0, sendcan=False, tcp_addr="", timeout=-1):
|
||||
def __init__(self, dbc_name, signals, checks=None, bus=0, sendcan=False, tcp_addr=b"", timeout=-1):
|
||||
self.test_mode_enabled = False
|
||||
can_dir = os.path.dirname(os.path.abspath(__file__))
|
||||
libdbc_fn = os.path.join(can_dir, "libdbc.so")
|
||||
libdbc_fn = str(libdbc_fn).encode('utf8')
|
||||
|
||||
cdef void *libdbc = dlopen(libdbc_fn, RTLD_LAZY)
|
||||
self.can_init_with_vectors = <can_init_with_vectors_func>dlsym(libdbc, 'can_init_with_vectors')
|
||||
@@ -33,24 +36,28 @@ cdef class CANParser:
|
||||
num_msgs = self.dbc[0].num_msgs
|
||||
for i in range(num_msgs):
|
||||
msg = self.dbc[0].msgs[i]
|
||||
self.msg_name_to_address[string(msg.name)] = msg.address
|
||||
self.address_to_msg_name[msg.address] = string(msg.name)
|
||||
name = msg.name.decode('utf8')
|
||||
|
||||
self.msg_name_to_address[name] = msg.address
|
||||
self.address_to_msg_name[msg.address] = name
|
||||
self.vl[msg.address] = {}
|
||||
self.vl[str(msg.name)] = {}
|
||||
self.vl[name] = {}
|
||||
self.ts[msg.address] = {}
|
||||
self.ts[str(msg.name)] = {}
|
||||
self.ts[name] = {}
|
||||
|
||||
# Convert message names into addresses
|
||||
for i in range(len(signals)):
|
||||
s = signals[i]
|
||||
if not isinstance(s[1], numbers.Number):
|
||||
s = (s[0], self.msg_name_to_address[s[1]], s[2])
|
||||
name = s[1].encode('utf8')
|
||||
s = (s[0], self.msg_name_to_address[name], s[2])
|
||||
signals[i] = s
|
||||
|
||||
for i in range(len(checks)):
|
||||
c = checks[i]
|
||||
if not isinstance(c[0], numbers.Number):
|
||||
c = (self.msg_name_to_address[c[0]], c[1])
|
||||
name = c[0].encode('utf8')
|
||||
c = (self.msg_name_to_address[name], c[1])
|
||||
checks[i] = c
|
||||
|
||||
cdef vector[SignalParseOptions] signal_options_v
|
||||
@@ -89,12 +96,15 @@ cdef class CANParser:
|
||||
|
||||
|
||||
for cv in self.can_values:
|
||||
self.vl[cv.address][string(cv.name)] = cv.value
|
||||
self.ts[cv.address][string(cv.name)] = cv.ts
|
||||
# Cast char * directly to unicde
|
||||
name = <unicode>self.address_to_msg_name[cv.address].c_str()
|
||||
cv_name = <unicode>cv.name
|
||||
|
||||
sig_name = self.address_to_msg_name[cv.address]
|
||||
self.vl[sig_name][string(cv.name)] = cv.value
|
||||
self.ts[sig_name][string(cv.name)] = cv.ts
|
||||
self.vl[cv.address][cv_name] = cv.value
|
||||
self.ts[cv.address][cv_name] = cv.ts
|
||||
|
||||
self.vl[name][cv_name] = cv.value
|
||||
self.ts[name][cv_name] = cv.ts
|
||||
|
||||
updated_val.insert(cv.address)
|
||||
|
||||
|
||||
@@ -1,14 +1,19 @@
|
||||
from distutils.core import setup, Extension
|
||||
from Cython.Build import cythonize
|
||||
import subprocess
|
||||
from distutils.core import Extension, setup # pylint: disable=import-error,no-name-in-module
|
||||
|
||||
from Cython.Build import cythonize
|
||||
|
||||
from common.cython_hacks import BuildExtWithoutPlatformSuffix
|
||||
|
||||
sourcefiles = ['parser_pyx.pyx']
|
||||
extra_compile_args = ["-std=c++11"]
|
||||
ARCH = subprocess.check_output(["uname", "-m"]).rstrip()
|
||||
ARCH = subprocess.check_output(["uname", "-m"], encoding='utf8').rstrip() # pylint: disable=unexpected-keyword-arg
|
||||
|
||||
if ARCH == "aarch64":
|
||||
extra_compile_args += ["-Wno-deprecated-register"]
|
||||
|
||||
setup(name='Radard Thread',
|
||||
cmdclass={'build_ext': BuildExtWithoutPlatformSuffix},
|
||||
ext_modules=cythonize(
|
||||
Extension(
|
||||
"parser_pyx",
|
||||
|
||||
@@ -7,7 +7,7 @@ from selfdrive.car.honda.hondacan import fix
|
||||
from common.realtime import sec_since_boot
|
||||
from common.dbc import dbc
|
||||
|
||||
class CANParser(object):
|
||||
class CANParser():
|
||||
def __init__(self, dbc_f, signals, checks=None):
|
||||
### input:
|
||||
# dbc_f : dbc file
|
||||
@@ -73,7 +73,7 @@ class CANParser(object):
|
||||
self.ck[msg] = True
|
||||
if "CHECKSUM" in out.keys() and msg in self.msgs_ck:
|
||||
# remove checksum (half byte)
|
||||
ck_portion = cdat[:-1] + chr(ord(cdat[-1]) & 0xF0)
|
||||
ck_portion = cdat[:-1] + (cdat[-1] & 0xF0).to_bytes(1, 'little')
|
||||
# recalculate checksum
|
||||
msg_vl = fix(ck_portion, msg)
|
||||
# compare recalculated vs received checksum
|
||||
|
||||
@@ -1,4 +1,5 @@
|
||||
#!/usr/bin/env python2
|
||||
#!/usr/bin/env python3
|
||||
from __future__ import print_function
|
||||
import os
|
||||
import glob
|
||||
import sys
|
||||
@@ -10,7 +11,7 @@ from common.dbc import dbc
|
||||
|
||||
def main():
|
||||
if len(sys.argv) != 3:
|
||||
print "usage: %s dbc_directory output_directory" % (sys.argv[0],)
|
||||
print("usage: %s dbc_directory output_directory" % (sys.argv[0],))
|
||||
sys.exit(0)
|
||||
|
||||
dbc_dir = sys.argv[1]
|
||||
@@ -38,7 +39,7 @@ def main():
|
||||
if dbc_mtime < out_mtime and template_mtime < out_mtime and this_file_mtime < out_mtime:
|
||||
continue #skip output is newer than template and dbc
|
||||
|
||||
msgs = [(address, msg_name, msg_size, sorted(msg_sigs, key=lambda s: s.name not in ("COUNTER", "CHECKSUM"))) # process counter and checksums first
|
||||
msgs = [(address, msg_name, msg_size, sorted(msg_sigs, key=lambda s: s.name not in (b"COUNTER", b"CHECKSUM"))) # process counter and checksums first
|
||||
for address, ((msg_name, msg_size), msg_sigs) in sorted(can_dbc.msgs.items()) if msg_sigs]
|
||||
|
||||
def_vals = {a: set(b) for a,b in can_dbc.def_vals.items()} #remove duplicates
|
||||
@@ -55,22 +56,22 @@ def main():
|
||||
|
||||
for address, msg_name, msg_size, sigs in msgs:
|
||||
for sig in sigs:
|
||||
if checksum_type is not None and sig.name == "CHECKSUM":
|
||||
if checksum_type is not None and sig.name == b"CHECKSUM":
|
||||
if sig.size != checksum_size:
|
||||
sys.exit("CHECKSUM is not %d bits longs %s" % (checksum_size, msg_name))
|
||||
if checksum_type == "honda" and sig.start_bit % 8 != 3:
|
||||
sys.exit("CHECKSUM starts at wrong bit %s" % msg_name)
|
||||
if checksum_type == "toyota" and sig.start_bit % 8 != 7:
|
||||
sys.exit("CHECKSUM starts at wrong bit %s" % msg_name)
|
||||
if checksum_type == "honda" and sig.name == "COUNTER":
|
||||
if checksum_type == "honda" and sig.name == b"COUNTER":
|
||||
if sig.size != 2:
|
||||
sys.exit("COUNTER is not 2 bits longs %s" % msg_name)
|
||||
if sig.start_bit % 8 != 5:
|
||||
sys.exit("COUNTER starts at wrong bit %s" % msg_name)
|
||||
if address in [0x200, 0x201]:
|
||||
if sig.name == "COUNTER_PEDAL" and sig.size != 4:
|
||||
if sig.name == b"COUNTER_PEDAL" and sig.size != 4:
|
||||
sys.exit("PEDAL COUNTER is not 4 bits longs %s" % msg_name)
|
||||
if sig.name == "CHECKSUM_PEDAL" and sig.size != 8:
|
||||
if sig.name == b"CHECKSUM_PEDAL" and sig.size != 8:
|
||||
sys.exit("PEDAL CHECKSUM is not 8 bits longs %s" % msg_name)
|
||||
|
||||
# Fail on duplicate message names
|
||||
|
||||
@@ -2,8 +2,9 @@ import struct
|
||||
from selfdrive.can.libdbc_py import libdbc, ffi
|
||||
|
||||
|
||||
class CANPacker(object):
|
||||
class CANPacker():
|
||||
def __init__(self, dbc_name):
|
||||
dbc_name = dbc_name.encode('utf8')
|
||||
self.packer = libdbc.canpack_init(dbc_name)
|
||||
self.dbc = libdbc.dbc_lookup(dbc_name)
|
||||
self.sig_names = {}
|
||||
@@ -13,16 +14,16 @@ class CANPacker(object):
|
||||
for i in range(num_msgs):
|
||||
msg = self.dbc[0].msgs[i]
|
||||
|
||||
name = ffi.string(msg.name)
|
||||
name = ffi.string(msg.name).decode('utf8')
|
||||
address = msg.address
|
||||
self.name_to_address_and_size[name] = (address, msg.size)
|
||||
self.name_to_address_and_size[address] = (address, msg.size)
|
||||
|
||||
def pack(self, addr, values, counter):
|
||||
values_thing = []
|
||||
for name, value in values.iteritems():
|
||||
for name, value in values.items():
|
||||
if name not in self.sig_names:
|
||||
self.sig_names[name] = ffi.new("char[]", name)
|
||||
self.sig_names[name] = ffi.new("char[]", name.encode('utf8'))
|
||||
|
||||
values_thing.append({
|
||||
'name': self.sig_names[name],
|
||||
|
||||
@@ -5,7 +5,7 @@ from selfdrive.can.libdbc_py import libdbc, ffi
|
||||
|
||||
CAN_INVALID_CNT = 5 # after so many consecutive CAN data with wrong checksum, counter or frequency, flag CAN invalidity
|
||||
|
||||
class CANParser(object):
|
||||
class CANParser():
|
||||
def __init__(self, dbc_name, signals, checks=None, bus=0, sendcan=False, tcp_addr="127.0.0.1", timeout=-1):
|
||||
if checks is None:
|
||||
checks = []
|
||||
@@ -16,7 +16,7 @@ class CANParser(object):
|
||||
self.ts = {}
|
||||
|
||||
self.dbc_name = dbc_name
|
||||
self.dbc = libdbc.dbc_lookup(dbc_name)
|
||||
self.dbc = libdbc.dbc_lookup(dbc_name.encode('utf8'))
|
||||
self.msg_name_to_addres = {}
|
||||
self.address_to_msg_name = {}
|
||||
|
||||
@@ -24,7 +24,7 @@ class CANParser(object):
|
||||
for i in range(num_msgs):
|
||||
msg = self.dbc[0].msgs[i]
|
||||
|
||||
name = ffi.string(msg.name)
|
||||
name = ffi.string(msg.name).decode('utf8')
|
||||
address = msg.address
|
||||
|
||||
self.msg_name_to_addres[name] = address
|
||||
@@ -48,7 +48,7 @@ class CANParser(object):
|
||||
c = (self.msg_name_to_addres[c[0]], c[1])
|
||||
checks[i] = c
|
||||
|
||||
sig_names = dict((name, ffi.new("char[]", name)) for name, _, _ in signals)
|
||||
sig_names = dict((name, ffi.new("char[]", name.encode('utf8'))) for name, _, _ in signals)
|
||||
|
||||
signal_options_c = ffi.new("SignalParseOptions[]", [
|
||||
{
|
||||
@@ -66,8 +66,8 @@ class CANParser(object):
|
||||
'check_frequency': freq,
|
||||
} for msg_address, freq in message_options.items()])
|
||||
|
||||
self.can = libdbc.can_init(bus, dbc_name, len(message_options_c), message_options_c,
|
||||
len(signal_options_c), signal_options_c, sendcan, tcp_addr, timeout)
|
||||
self.can = libdbc.can_init(bus, dbc_name.encode('utf8'), len(message_options_c), message_options_c,
|
||||
len(signal_options_c), signal_options_c, sendcan, tcp_addr.encode('utf8'), timeout)
|
||||
|
||||
self.p_can_valid = ffi.new("bool*")
|
||||
|
||||
@@ -85,11 +85,11 @@ class CANParser(object):
|
||||
self.can_valid = self.can_invalid_cnt < CAN_INVALID_CNT
|
||||
|
||||
ret = set()
|
||||
for i in xrange(can_values_len):
|
||||
for i in range(can_values_len):
|
||||
cv = self.can_values[i]
|
||||
address = cv.address
|
||||
# print("{0} {1}".format(hex(cv.address), ffi.string(cv.name)))
|
||||
name = ffi.string(cv.name)
|
||||
name = ffi.string(cv.name).decode('utf8')
|
||||
self.vl[address][name] = cv.value
|
||||
self.ts[address][name] = cv.ts
|
||||
|
||||
|
||||
@@ -13,7 +13,7 @@ class TestPackerMethods(unittest.TestCase):
|
||||
|
||||
def test_correctness(self):
|
||||
# Test all commands, randomize the params.
|
||||
for _ in xrange(1000):
|
||||
for _ in range(1000):
|
||||
gear = ('drive', 'reverse', 'low')[random.randint(0, 3) % 3]
|
||||
lkas_active = (random.randint(0, 2) % 2 == 0)
|
||||
hud_alert = random.randint(0, 6)
|
||||
|
||||
@@ -17,7 +17,7 @@ class TestPackerMethods(unittest.TestCase):
|
||||
|
||||
def test_correctness(self):
|
||||
# Test all cars' commands, randomize the params.
|
||||
for _ in xrange(1000):
|
||||
for _ in range(1000):
|
||||
bus = random.randint(0, 65536)
|
||||
apply_steer = (random.randint(0, 2) % 2 == 0)
|
||||
idx = random.randint(0, 65536)
|
||||
|
||||
@@ -15,8 +15,8 @@ class TestPackerMethods(unittest.TestCase):
|
||||
|
||||
def test_correctness(self):
|
||||
# Test all commands, randomize the params.
|
||||
for _ in xrange(1000):
|
||||
is_panda_black = False
|
||||
for _ in range(1000):
|
||||
has_relay = False
|
||||
car_fingerprint = HONDA_BOSCH[0]
|
||||
|
||||
apply_brake = (random.randint(0, 2) % 2 == 0)
|
||||
@@ -25,15 +25,15 @@ class TestPackerMethods(unittest.TestCase):
|
||||
pcm_cancel_cmd = (random.randint(0, 2) % 2 == 0)
|
||||
fcw = random.randint(0, 65536)
|
||||
idx = random.randint(0, 65536)
|
||||
m_old = hondacan.create_brake_command(self.honda_cp_old, apply_brake, pump_on, pcm_override, pcm_cancel_cmd, fcw, idx, car_fingerprint, is_panda_black)
|
||||
m = hondacan.create_brake_command(self.honda_cp, apply_brake, pump_on, pcm_override, pcm_cancel_cmd, fcw, idx, car_fingerprint, is_panda_black)
|
||||
m_old = hondacan.create_brake_command(self.honda_cp_old, apply_brake, pump_on, pcm_override, pcm_cancel_cmd, fcw, idx, car_fingerprint, has_relay)
|
||||
m = hondacan.create_brake_command(self.honda_cp, apply_brake, pump_on, pcm_override, pcm_cancel_cmd, fcw, idx, car_fingerprint, has_relay)
|
||||
self.assertEqual(m_old, m)
|
||||
|
||||
apply_steer = (random.randint(0, 2) % 2 == 0)
|
||||
lkas_active = (random.randint(0, 2) % 2 == 0)
|
||||
idx = random.randint(0, 65536)
|
||||
m_old = hondacan.create_steering_control(self.honda_cp_old, apply_steer, lkas_active, car_fingerprint, idx, is_panda_black)
|
||||
m = hondacan.create_steering_control(self.honda_cp, apply_steer, lkas_active, car_fingerprint, idx, is_panda_black)
|
||||
m_old = hondacan.create_steering_control(self.honda_cp_old, apply_steer, lkas_active, car_fingerprint, idx, has_relay)
|
||||
m = hondacan.create_steering_control(self.honda_cp, apply_steer, lkas_active, car_fingerprint, idx, has_relay)
|
||||
self.assertEqual(m_old, m)
|
||||
|
||||
pcm_speed = random.randint(0, 65536)
|
||||
@@ -41,14 +41,14 @@ class TestPackerMethods(unittest.TestCase):
|
||||
0xc1, random.randint(0, 65536), random.randint(0, 65536), random.randint(0, 65536), random.randint(0, 65536))
|
||||
idx = random.randint(0, 65536)
|
||||
is_metric = (random.randint(0, 2) % 2 == 0)
|
||||
m_old = hondacan.create_ui_commands(self.honda_cp_old, pcm_speed, hud, car_fingerprint, is_metric, idx, is_panda_black)
|
||||
m = hondacan.create_ui_commands(self.honda_cp, pcm_speed, hud, car_fingerprint, is_metric, idx, is_panda_black)
|
||||
m_old = hondacan.create_ui_commands(self.honda_cp_old, pcm_speed, hud, car_fingerprint, is_metric, idx, has_relay)
|
||||
m = hondacan.create_ui_commands(self.honda_cp, pcm_speed, hud, car_fingerprint, is_metric, idx, has_relay)
|
||||
self.assertEqual(m_old, m)
|
||||
|
||||
button_val = random.randint(0, 65536)
|
||||
idx = random.randint(0, 65536)
|
||||
m_old = hondacan.spam_buttons_command(self.honda_cp_old, button_val, idx, car_fingerprint, is_panda_black)
|
||||
m = hondacan.spam_buttons_command(self.honda_cp, button_val, idx, car_fingerprint, is_panda_black)
|
||||
m_old = hondacan.spam_buttons_command(self.honda_cp_old, button_val, idx, car_fingerprint, has_relay)
|
||||
m = hondacan.spam_buttons_command(self.honda_cp, button_val, idx, car_fingerprint, has_relay)
|
||||
self.assertEqual(m_old, m)
|
||||
|
||||
|
||||
|
||||
@@ -14,7 +14,7 @@ class TestPackerMethods(unittest.TestCase):
|
||||
|
||||
def test_correctness(self):
|
||||
# Test all commands, randomize the params.
|
||||
for _ in xrange(1000):
|
||||
for _ in range(1000):
|
||||
# Hyundai
|
||||
car_fingerprint = hyundai_checksum["crc8"][0]
|
||||
apply_steer = (random.randint(0, 2) % 2 == 0)
|
||||
|
||||
@@ -14,7 +14,7 @@ class TestPackerMethods(unittest.TestCase):
|
||||
|
||||
def test_correctness(self):
|
||||
# Test all cars' commands, randomize the params.
|
||||
for _ in xrange(1000):
|
||||
for _ in range(1000):
|
||||
apply_steer = (random.randint(0, 2) % 2 == 0)
|
||||
frame = random.randint(1, 65536)
|
||||
steer_step = random.randint(1, 65536)
|
||||
|
||||
@@ -17,7 +17,7 @@ class TestPackerMethods(unittest.TestCase):
|
||||
|
||||
def test_correctness(self):
|
||||
# Test all commands, randomize the params.
|
||||
for _ in xrange(1000):
|
||||
for _ in range(1000):
|
||||
# Toyota
|
||||
steer = random.randint(-1, 1)
|
||||
enabled = (random.randint(0, 2) % 2 == 0)
|
||||
@@ -66,14 +66,14 @@ class TestPackerMethods(unittest.TestCase):
|
||||
left_lane_depart = (random.randint(0, 2) % 2 == 0)
|
||||
right_lane_depart = (random.randint(0, 2) % 2 == 0)
|
||||
|
||||
for _ in xrange(recursions):
|
||||
for _ in range(recursions):
|
||||
create_ui_command(self.cp_old, steer, chime, left_line, right_line, left_lane_depart, right_lane_depart)
|
||||
n2 = sec_since_boot()
|
||||
elapsed_old = n2 - n1
|
||||
|
||||
# print('Old API, elapsed time: {} secs'.format(elapsed_old))
|
||||
n1 = sec_since_boot()
|
||||
for _ in xrange(recursions):
|
||||
for _ in range(recursions):
|
||||
create_ui_command(self.cp, steer, chime, left_line, right_line, left_lane_depart, right_lane_depart)
|
||||
n2 = sec_since_boot()
|
||||
elapsed_new = n2 - n1
|
||||
|
||||
@@ -1,4 +1,4 @@
|
||||
#!/usr/bin/env python2
|
||||
#!/usr/bin/env python3
|
||||
|
||||
import os
|
||||
import unittest
|
||||
@@ -45,7 +45,7 @@ def dicts_vals_differ(dict1, dict2):
|
||||
def run_route(route):
|
||||
can = messaging.pub_sock(service_list['can'].port)
|
||||
|
||||
CP = CarInterface.get_params(CAR.CIVIC, {})
|
||||
CP = CarInterface.get_params(CAR.CIVIC)
|
||||
signals, checks = get_can_signals(CP)
|
||||
parser_old = CANParserOld(DBC[CP.carFingerprint]['pt'], signals, checks, 0, timeout=-1, tcp_addr="127.0.0.1")
|
||||
parser_new = CANParserNew(DBC[CP.carFingerprint]['pt'], signals, checks, 0, timeout=-1, tcp_addr="127.0.0.1")
|
||||
@@ -95,7 +95,7 @@ class TestCanParser(unittest.TestCase):
|
||||
for route in self.routes.values():
|
||||
route_filename = route + ".bz2"
|
||||
if not os.path.isfile(route_filename):
|
||||
with open(route + ".bz2", "w") as f:
|
||||
with open(route + ".bz2", "wb") as f:
|
||||
f.write(requests.get(BASE_URL + route_filename).content)
|
||||
|
||||
def test_parser_civic(self):
|
||||
|
||||
@@ -4,6 +4,9 @@ from common.numpy_fast import clip
|
||||
# kg of standard extra cargo to count for drive, gas, etc...
|
||||
STD_CARGO_KG = 136.
|
||||
|
||||
def gen_empty_fingerprint():
|
||||
return {i: {} for i in range(0, 4)}
|
||||
|
||||
# FIXME: hardcoding honda civic 2016 touring params so they can be used to
|
||||
# scale unknown params for other cars
|
||||
class CivicParams:
|
||||
@@ -31,7 +34,7 @@ def scale_tire_stiffness(mass, wheelbase, center_to_front, tire_stiffness_factor
|
||||
(center_to_front / wheelbase) / (CivicParams.CENTER_TO_FRONT / CivicParams.WHEELBASE)
|
||||
|
||||
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}
|
||||
|
||||
@@ -53,11 +56,10 @@ def apply_std_steer_torque_limits(apply_torque, apply_torque_last, driver_torque
|
||||
apply_torque = clip(apply_torque, apply_torque_last - LIMITS.STEER_DELTA_UP,
|
||||
min(apply_torque_last + LIMITS.STEER_DELTA_DOWN, LIMITS.STEER_DELTA_UP))
|
||||
|
||||
return int(round(apply_torque))
|
||||
return int(round(float(apply_torque)))
|
||||
|
||||
|
||||
def apply_toyota_steer_torque_limits(apply_torque, apply_torque_last, motor_torque, LIMITS):
|
||||
|
||||
# limits due to comparison of commanded torque VS motor reported torque
|
||||
max_lim = min(max(motor_torque + LIMITS.STEER_ERROR_MAX, LIMITS.STEER_ERROR_MAX), LIMITS.STEER_MAX)
|
||||
min_lim = max(min(motor_torque - LIMITS.STEER_ERROR_MAX, -LIMITS.STEER_ERROR_MAX), -LIMITS.STEER_MAX)
|
||||
@@ -74,7 +76,7 @@ def apply_toyota_steer_torque_limits(apply_torque, apply_torque_last, motor_torq
|
||||
apply_torque_last - LIMITS.STEER_DELTA_UP,
|
||||
min(apply_torque_last + LIMITS.STEER_DELTA_DOWN, LIMITS.STEER_DELTA_UP))
|
||||
|
||||
return int(round(apply_torque))
|
||||
return int(round(float(apply_torque)))
|
||||
|
||||
|
||||
def crc8_pedal(data):
|
||||
@@ -106,8 +108,19 @@ def create_gas_command(packer, gas_amount, idx):
|
||||
|
||||
dat = packer.make_can_msg("GAS_COMMAND", 0, values)[2]
|
||||
|
||||
dat = [ord(i) for i in dat]
|
||||
checksum = crc8_pedal(dat[:-1])
|
||||
values["CHECKSUM_PEDAL"] = checksum
|
||||
|
||||
return packer.make_can_msg("GAS_COMMAND", 0, values)
|
||||
|
||||
|
||||
def is_ecu_disconnected(fingerprint, fingerprint_list, ecu_fingerprint, car, ecu):
|
||||
# check if a stock ecu is disconnected by looking for specific CAN msgs in the fingerprint
|
||||
# return True if the reference car fingerprint contains the ecu fingerprint msg and
|
||||
# fingerprint does not contains messages normally sent by a given ecu
|
||||
ecu_in_car = False
|
||||
for car_finger in fingerprint_list[car]:
|
||||
if any(msg in car_finger for msg in ecu_fingerprint[ecu]):
|
||||
ecu_in_car = True
|
||||
|
||||
return ecu_in_car and not any(msg in fingerprint for msg in ecu_fingerprint[ecu])
|
||||
|
||||
@@ -2,11 +2,12 @@ import os
|
||||
import zmq
|
||||
from cereal import car
|
||||
from common.params import Params
|
||||
from common.vin import get_vin, VIN_UNKNOWN
|
||||
from common.basedir import BASEDIR
|
||||
from common.fingerprints import eliminate_incompatible_cars, all_known_cars
|
||||
from selfdrive.car.fingerprints import eliminate_incompatible_cars, all_known_cars
|
||||
from selfdrive.car.vin import get_vin, VIN_UNKNOWN
|
||||
from selfdrive.swaglog import cloudlog
|
||||
import selfdrive.messaging as messaging
|
||||
from selfdrive.car import gen_empty_fingerprint
|
||||
|
||||
|
||||
def get_one_can(logcan):
|
||||
@@ -67,12 +68,7 @@ def only_toyota_left(candidate_cars):
|
||||
|
||||
# BOUNTY: every added fingerprint in selfdrive/car/*/values.py is a $100 coupon code on shop.comma.ai
|
||||
# **** for use live only ****
|
||||
def fingerprint(logcan, sendcan, is_panda_black):
|
||||
if os.getenv("SIMULATOR2") is not None:
|
||||
return ("simulator2", None, "")
|
||||
elif os.getenv("SIMULATOR") is not None:
|
||||
return ("simulator", None, "")
|
||||
|
||||
def fingerprint(logcan, sendcan, has_relay):
|
||||
params = Params()
|
||||
car_params = params.get("CarParams")
|
||||
|
||||
@@ -80,7 +76,7 @@ def fingerprint(logcan, sendcan, is_panda_black):
|
||||
# use already stored VIN: a new VIN query cannot be done, since panda isn't in ELM327 mode
|
||||
car_params = car.CarParams.from_bytes(car_params)
|
||||
vin = VIN_UNKNOWN if car_params.carVin == "" else car_params.carVin
|
||||
elif is_panda_black:
|
||||
elif has_relay:
|
||||
# Vin query only reliably works thorugh OBDII
|
||||
vin = get_vin(logcan, sendcan, 1)
|
||||
else:
|
||||
@@ -89,7 +85,7 @@ def fingerprint(logcan, sendcan, is_panda_black):
|
||||
cloudlog.warning("VIN %s", vin)
|
||||
Params().put("CarVin", vin)
|
||||
|
||||
finger = {i: {} for i in range(0, 4)} # collect on all buses
|
||||
finger = gen_empty_fingerprint()
|
||||
candidate_cars = {i: all_known_cars() for i in [0, 1]} # attempt fingerprint on both bus 0 and 1
|
||||
frame = 0
|
||||
frame_fingerprint = 10 # 0.1s
|
||||
@@ -105,10 +101,11 @@ def fingerprint(logcan, sendcan, is_panda_black):
|
||||
# and VIN query response.
|
||||
# Include bus 2 for toyotas to disambiguate cars using camera messages
|
||||
# (ideally should be done for all cars but we can't for Honda Bosch)
|
||||
if can.src in range(0, 4):
|
||||
finger[can.src][can.address] = len(can.dat)
|
||||
for b in candidate_cars:
|
||||
if (can.src == b or (only_toyota_left(candidate_cars[b]) and can.src == 2)) and \
|
||||
can.address < 0x800 and can.address not in [0x7df, 0x7e0, 0x7e8]:
|
||||
finger[can.src][can.address] = len(can.dat)
|
||||
candidate_cars[b] = eliminate_incompatible_cars(can, candidate_cars[b])
|
||||
|
||||
# if we only have one car choice and the time since we got our first
|
||||
@@ -123,7 +120,7 @@ def fingerprint(logcan, sendcan, is_panda_black):
|
||||
car_fingerprint = candidate_cars[b][0]
|
||||
|
||||
# bail if no cars left or we've been waiting for more than 2s
|
||||
failed = all(len(cc) == 0 for cc in candidate_cars.itervalues()) or frame > 200
|
||||
failed = all(len(cc) == 0 for cc in candidate_cars.values()) or frame > 200
|
||||
succeeded = car_fingerprint is not None
|
||||
done = failed or succeeded
|
||||
|
||||
@@ -133,15 +130,15 @@ def fingerprint(logcan, sendcan, is_panda_black):
|
||||
return car_fingerprint, finger, vin
|
||||
|
||||
|
||||
def get_car(logcan, sendcan, is_panda_black=False):
|
||||
def get_car(logcan, sendcan, has_relay=False):
|
||||
|
||||
candidate, fingerprints, vin = fingerprint(logcan, sendcan, is_panda_black)
|
||||
candidate, fingerprints, vin = fingerprint(logcan, sendcan, has_relay)
|
||||
|
||||
if candidate is None:
|
||||
cloudlog.warning("car doesn't match any fingerprints: %r", fingerprints)
|
||||
candidate = "mock"
|
||||
|
||||
CarInterface, CarController = interfaces[candidate]
|
||||
car_params = CarInterface.get_params(candidate, fingerprints[0], vin, is_panda_black)
|
||||
car_params = CarInterface.get_params(candidate, fingerprints, vin, has_relay)
|
||||
|
||||
return CarInterface(car_params, CarController), car_params
|
||||
|
||||
@@ -11,7 +11,7 @@ class SteerLimitParams:
|
||||
STEER_ERROR_MAX = 80
|
||||
|
||||
|
||||
class CarController(object):
|
||||
class CarController():
|
||||
def __init__(self, dbc_name, car_fingerprint, enable_camera):
|
||||
self.braking = False
|
||||
# redundant safety check with the board
|
||||
|
||||
@@ -77,7 +77,7 @@ def get_camera_parser(CP):
|
||||
return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 2)
|
||||
|
||||
|
||||
class CarState(object):
|
||||
class CarState():
|
||||
def __init__(self, CP):
|
||||
|
||||
self.CP = CP
|
||||
|
||||
@@ -1,6 +1,7 @@
|
||||
from cereal import car
|
||||
|
||||
|
||||
GearShifter = car.CarState.GearShifter
|
||||
VisualAlert = car.CarControl.HUDControl.VisualAlert
|
||||
|
||||
def calc_checksum(data):
|
||||
@@ -48,7 +49,7 @@ def create_lkas_hud(packer, gear, lkas_active, hud_alert, hud_count, lkas_car_mo
|
||||
# LKAS_HUD 0x2a6 (678) Controls what lane-keeping icon is displayed.
|
||||
|
||||
if hud_alert == VisualAlert.steerRequired:
|
||||
msg = '0000000300000000'.decode('hex')
|
||||
msg = b'\x00\x00\x00\x03\x00\x00\x00\x00'
|
||||
return make_can_msg(0x2a6, msg)
|
||||
|
||||
color = 1 # default values are for park or neutral in 2017 are 0 0, but trying 1 1 for 2019
|
||||
@@ -59,7 +60,7 @@ def create_lkas_hud(packer, gear, lkas_active, hud_alert, hud_count, lkas_car_mo
|
||||
alerts = 1
|
||||
# CAR.PACIFICA_2018_HYBRID and CAR.PACIFICA_2019_HYBRID
|
||||
# had color = 1 and lines = 1 but trying 2017 hybrid style for now.
|
||||
if gear in ('drive', 'reverse', 'low'):
|
||||
if gear in (GearShifter.drive, GearShifter.reverse, GearShifter.low):
|
||||
if lkas_active:
|
||||
color = 2 # control active, display green.
|
||||
lines = 6
|
||||
@@ -86,7 +87,7 @@ def create_lkas_command(packer, apply_steer, moving_fast, frame):
|
||||
}
|
||||
|
||||
dat = packer.make_can_msg("LKAS_COMMAND", 0, values)[2]
|
||||
dat = [ord(i) for i in dat][:-1]
|
||||
dat = dat[:-1]
|
||||
checksum = calc_checksum(dat)
|
||||
|
||||
values["CHECKSUM"] = checksum
|
||||
@@ -95,8 +96,8 @@ def create_lkas_command(packer, apply_steer, moving_fast, frame):
|
||||
|
||||
def create_wheel_buttons(frame):
|
||||
# WHEEL_BUTTONS (571) Message sent to cancel ACC.
|
||||
start = [0x01] # acc cancel set
|
||||
start = b"\x01" # acc cancel set
|
||||
counter = (frame % 10) << 4
|
||||
dat = start + [counter]
|
||||
dat = dat + [calc_checksum(dat)]
|
||||
return make_can_msg(0x23b, str(bytearray(dat)))
|
||||
dat = start + counter.to_bytes(1, 'little')
|
||||
dat = dat + calc_checksum(dat).to_bytes(1, 'little')
|
||||
return make_can_msg(0x23b, dat)
|
||||
|
||||
@@ -1,16 +1,17 @@
|
||||
#!/usr/bin/env python
|
||||
#!/usr/bin/env python3
|
||||
from cereal import car
|
||||
from selfdrive.config import Conversions as CV
|
||||
from selfdrive.controls.lib.drive_helpers import EventTypes as ET, create_event
|
||||
from selfdrive.controls.lib.vehicle_model import VehicleModel
|
||||
from selfdrive.car.chrysler.carstate import CarState, get_can_parser, get_camera_parser
|
||||
from selfdrive.car.chrysler.values import ECU, check_ecu_msgs, CAR
|
||||
from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness
|
||||
from selfdrive.car.chrysler.values import ECU, ECU_FINGERPRINT, CAR, FINGERPRINTS
|
||||
from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, is_ecu_disconnected, gen_empty_fingerprint
|
||||
from selfdrive.car.interfaces import CarInterfaceBase
|
||||
|
||||
GearShifter = car.CarState.GearShifter
|
||||
ButtonType = car.CarState.ButtonEvent.Type
|
||||
|
||||
class CarInterface(object):
|
||||
class CarInterface(CarInterfaceBase):
|
||||
def __init__(self, CP, CarController):
|
||||
self.CP = CP
|
||||
self.VM = VehicleModel(CP)
|
||||
@@ -34,18 +35,14 @@ class CarInterface(object):
|
||||
return float(accel) / 3.0
|
||||
|
||||
@staticmethod
|
||||
def calc_accel_override(a_ego, a_target, v_ego, v_target):
|
||||
return 1.0
|
||||
|
||||
@staticmethod
|
||||
def get_params(candidate, fingerprint, vin="", is_panda_black=False):
|
||||
def get_params(candidate, fingerprint=gen_empty_fingerprint(), vin="", has_relay=False):
|
||||
|
||||
ret = car.CarParams.new_message()
|
||||
|
||||
ret.carName = "chrysler"
|
||||
ret.carFingerprint = candidate
|
||||
ret.carVin = vin
|
||||
ret.isPandaBlack = is_panda_black
|
||||
ret.isPandaBlack = has_relay
|
||||
|
||||
ret.safetyModel = car.CarParams.SafetyModel.chrysler
|
||||
|
||||
@@ -95,7 +92,7 @@ class CarInterface(object):
|
||||
ret.brakeMaxBP = [5., 20.]
|
||||
ret.brakeMaxV = [1., 0.8]
|
||||
|
||||
ret.enableCamera = not check_ecu_msgs(fingerprint, ECU.CAM) or is_panda_black
|
||||
ret.enableCamera = is_ecu_disconnected(fingerprint[0], FINGERPRINTS, ECU_FINGERPRINT, candidate, ECU.CAM) or has_relay
|
||||
print("ECU Camera Simulated: {0}".format(ret.enableCamera))
|
||||
ret.openpilotLongitudinalControl = False
|
||||
|
||||
|
||||
@@ -1,10 +1,11 @@
|
||||
#!/usr/bin/env python
|
||||
#!/usr/bin/env python3
|
||||
import os
|
||||
from selfdrive.can.parser import CANParser
|
||||
from cereal import car
|
||||
from selfdrive.car.interfaces import RadarInterfaceBase
|
||||
|
||||
RADAR_MSGS_C = range(0x2c2, 0x2d4+2, 2) # c_ messages 706,...,724
|
||||
RADAR_MSGS_D = range(0x2a2, 0x2b4+2, 2) # d_ messages
|
||||
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)
|
||||
|
||||
@@ -45,10 +46,10 @@ def _address_to_track(address):
|
||||
return (address - RADAR_MSGS_D[0]) // 2
|
||||
raise ValueError("radar received unexpected address %d" % address)
|
||||
|
||||
class RadarInterface(object):
|
||||
class RadarInterface(RadarInterfaceBase):
|
||||
def __init__(self, CP):
|
||||
self.pts = {}
|
||||
self.delay = 0.0 # Delay of radar #TUNE
|
||||
self.delay = 0 # Delay of radar #TUNE
|
||||
self.rcp = _create_radar_can_parser()
|
||||
self.updated_messages = set()
|
||||
self.trigger_msg = LAST_MSG
|
||||
|
||||
@@ -1 +0,0 @@
|
||||
PYTHONPATH=`realpath ../../../` python chryslercan_test.py
|
||||
+19
-17
@@ -1,55 +1,57 @@
|
||||
from selfdrive.car.chrysler import chryslercan
|
||||
from selfdrive.can.packer import CANPacker
|
||||
import unittest
|
||||
|
||||
from cereal import car
|
||||
from selfdrive.can.packer import CANPacker
|
||||
from selfdrive.car.chrysler import chryslercan
|
||||
|
||||
VisualAlert = car.CarControl.HUDControl.VisualAlert
|
||||
GearShifter = car.CarState.GearShifter
|
||||
|
||||
import unittest
|
||||
|
||||
|
||||
class TestChryslerCan(unittest.TestCase):
|
||||
|
||||
def test_checksum(self):
|
||||
self.assertEqual(0x75, chryslercan.calc_checksum([0x01, 0x20]))
|
||||
self.assertEqual(0xcc, chryslercan.calc_checksum([0x14, 0, 0, 0, 0x20]))
|
||||
self.assertEqual(0x75, chryslercan.calc_checksum(b"\x01\x20"))
|
||||
self.assertEqual(0xcc, chryslercan.calc_checksum(b"\x14\x00\x00\x00\x20"))
|
||||
|
||||
def test_hud(self):
|
||||
packer = CANPacker('chrysler_pacifica_2017_hybrid')
|
||||
self.assertEqual(
|
||||
[0x2a6, 0, '0100010100000000'.decode('hex'), 0],
|
||||
[0x2a6, 0, b'\x01\x00\x01\x01\x00\x00\x00\x00', 0],
|
||||
chryslercan.create_lkas_hud(
|
||||
packer,
|
||||
'park', False, False, 1, 0))
|
||||
GearShifter.park, False, False, 1, 0))
|
||||
self.assertEqual(
|
||||
[0x2a6, 0, '0100010000000000'.decode('hex'), 0],
|
||||
[0x2a6, 0, b'\x01\x00\x01\x00\x00\x00\x00\x00', 0],
|
||||
chryslercan.create_lkas_hud(
|
||||
packer,
|
||||
'park', False, False, 5*4, 0))
|
||||
GearShifter.park, False, False, 5*4, 0))
|
||||
self.assertEqual(
|
||||
[0x2a6, 0, '0100010000000000'.decode('hex'), 0],
|
||||
[0x2a6, 0, b'\x01\x00\x01\x00\x00\x00\x00\x00', 0],
|
||||
chryslercan.create_lkas_hud(
|
||||
packer,
|
||||
'park', False, False, 99999, 0))
|
||||
GearShifter.park, False, False, 99999, 0))
|
||||
self.assertEqual(
|
||||
[0x2a6, 0, '0200060000000000'.decode('hex'), 0],
|
||||
[0x2a6, 0, b'\x02\x00\x06\x00\x00\x00\x00\x00', 0],
|
||||
chryslercan.create_lkas_hud(
|
||||
packer,
|
||||
'drive', True, False, 99999, 0))
|
||||
GearShifter.drive, True, False, 99999, 0))
|
||||
self.assertEqual(
|
||||
[0x2a6, 0, '0264060000000000'.decode('hex'), 0],
|
||||
[0x2a6, 0, b'\x02\x64\x06\x00\x00\x00\x00\x00', 0],
|
||||
chryslercan.create_lkas_hud(
|
||||
packer,
|
||||
'drive', True, False, 99999, 0x64))
|
||||
GearShifter.drive, True, False, 99999, 0x64))
|
||||
|
||||
def test_command(self):
|
||||
packer = CANPacker('chrysler_pacifica_2017_hybrid')
|
||||
self.assertEqual(
|
||||
[0x292, 0, '140000001086'.decode('hex'), 0],
|
||||
[0x292, 0, b'\x14\x00\x00\x00\x10\x86', 0],
|
||||
chryslercan.create_lkas_command(
|
||||
packer,
|
||||
0, True, 1))
|
||||
self.assertEqual(
|
||||
[0x292, 0, '040000008083'.decode('hex'), 0],
|
||||
[0x292, 0, b'\x04\x00\x00\x00\x80\x83', 0],
|
||||
chryslercan.create_lkas_command(
|
||||
packer,
|
||||
0, False, 8))
|
||||
@@ -4,7 +4,7 @@ class CAR:
|
||||
PACIFICA_2017_HYBRID = "CHRYSLER PACIFICA HYBRID 2017"
|
||||
PACIFICA_2018_HYBRID = "CHRYSLER PACIFICA HYBRID 2018"
|
||||
PACIFICA_2019_HYBRID = "CHRYSLER PACIFICA HYBRID 2019"
|
||||
PACIFICA_2018 = "CHRYSLER PACIFICA 2018"
|
||||
PACIFICA_2018 = "CHRYSLER PACIFICA 2018" # Also covers Pacifica 2017.
|
||||
JEEP_CHEROKEE = "JEEP GRAND CHEROKEE V6 2018" # Also covers Tailhawk 2017.
|
||||
JEEP_CHEROKEE_2019 = "JEEP GRAND CHEROKEE 2019"
|
||||
|
||||
@@ -25,14 +25,15 @@ FINGERPRINTS = {
|
||||
],
|
||||
CAR.PACIFICA_2018: [
|
||||
{55: 8, 257: 5, 258: 8, 264: 8, 268: 8, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 292: 8, 294: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 344: 8, 368: 8, 376: 3, 384: 8, 388: 4, 416: 7, 448: 6, 456: 4, 464: 8, 469: 8, 480: 8, 500: 8, 501: 8, 512: 8, 514: 8, 516: 7, 517: 7, 520: 8, 524: 8, 526: 6, 528: 8, 532: 8, 542: 8, 544: 8, 557: 8, 559: 8, 560: 4, 564: 8, 571: 3, 579: 8, 584: 8, 608: 8, 624: 8, 625: 8, 632: 8, 639: 8, 656: 4, 658: 6, 660: 8, 669: 3, 671: 8, 672: 8, 678: 8, 680: 8, 705: 8, 706: 8, 709: 8, 710: 8, 719: 8, 720: 6, 729: 5, 736: 8, 746: 5, 752: 2, 760: 8, 764: 8, 766: 8, 770: 8, 773: 8, 779: 8, 784: 8, 792: 8, 799: 8, 800: 8, 804: 8, 808: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 832: 8, 838: 2, 848: 8, 853: 8, 856: 4, 860: 6, 863: 8, 882: 8, 897: 8, 924: 8, 926: 3, 937: 8, 947: 8, 948: 8, 969: 4, 974: 5, 979: 8, 980: 8, 981: 8, 982: 8, 983: 8, 984: 8, 992: 8, 993: 7, 995: 8, 996: 8, 1000: 8, 1001: 8, 1002: 8, 1003: 8, 1008: 8, 1009: 8, 1010: 8, 1011: 8, 1012: 8, 1013: 8, 1014: 8, 1015: 8, 1024: 8, 1025: 8, 1026: 8, 1031: 8, 1033: 8, 1050: 8, 1059: 8, 1098: 8, 1100: 8, 1537: 8, 1538: 8, 1562: 8},
|
||||
{55: 8, 257: 5, 258: 8, 264: 8, 268: 8, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 292: 8, 294: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 344: 8, 368: 8, 376: 3, 384: 8, 388: 4, 416: 7, 448: 6, 456: 4, 464: 8, 469: 8, 480: 8, 500: 8, 501: 8, 512: 8, 514: 8, 516: 7, 517: 7, 520: 8, 524: 8, 526: 6, 528: 8, 532: 8, 542: 8, 544: 8, 557: 8, 559: 8, 560: 4, 564: 4, 571: 3, 584: 8, 608: 8, 624: 8, 625: 8, 632: 8, 639: 8, 656: 4, 658: 6, 660: 8, 669: 3, 671: 8, 672: 8, 678: 8, 680: 8, 705: 8, 706: 8, 709: 8, 710: 8, 719: 8, 720: 6, 729: 5, 736: 8, 746: 5, 752: 2, 760: 8, 764: 8, 766: 8, 770: 8, 773: 8, 779: 8, 784: 8, 792: 8, 799: 8, 800: 8, 804: 8, 808: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 832: 8, 838: 2, 848: 8, 853: 8, 856: 4, 860: 6, 863: 8, 882: 8, 897: 8, 924: 3, 926: 3, 937: 8, 947: 8, 948: 8, 969: 4, 974: 5, 979: 8, 980: 8, 981: 8, 982: 8, 983: 8, 984: 8, 992: 8, 993: 7, 995: 8, 996: 8, 1000: 8, 1001: 8, 1002: 8, 1003: 8, 1008: 8, 1009: 8, 1010: 8, 1011: 8, 1012: 8, 1013: 8, 1014: 8, 1015: 8, 1024: 8, 1025: 8, 1026: 8, 1031: 8, 1033: 8, 1050: 8, 1059: 8, 1098: 8, 1100: 8, 1537: 8, 1538: 8, 1562: 8},
|
||||
],
|
||||
CAR.PACIFICA_2018_HYBRID: [
|
||||
{68: 8, 168: 8, 257: 5, 258: 8, 264: 8, 268: 8, 270: 8, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 291: 8, 292: 8, 294: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 344: 8, 368: 8, 376: 3, 384: 8, 388: 4, 448: 6, 456: 4, 464: 8, 469: 8, 480: 8, 500: 8, 501: 8, 512: 8, 514: 8, 520: 8, 528: 8, 532: 8, 544: 8, 557: 8, 559: 8, 560: 4, 564: 8, 571: 3, 579: 8, 584: 8, 608: 8, 624: 8, 625: 8, 632: 8, 639: 8, 653: 8, 654: 8, 655: 8, 660: 8, 669: 3, 671: 8, 672: 8, 680: 8, 701: 8, 704: 8, 705: 8, 706: 8, 709: 8, 710: 8, 719: 8, 720: 6, 736: 8, 737: 8, 746: 5, 760: 8, 764: 8, 766: 8, 770: 8, 773: 8, 779: 8, 782: 8, 784: 8, 792: 8, 799: 8, 800: 8, 804: 8, 808: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 832: 8, 838: 2, 848: 8, 853: 8, 856: 4, 860: 6, 863: 8, 878: 8, 882: 8, 897: 8, 908: 8, 924: 8, 926: 3, 929: 8, 937: 8, 938: 8, 939: 8, 940: 8, 941: 8, 942: 8, 943: 8, 947: 8, 948: 8, 958: 8, 959: 8, 969: 4, 974: 5, 979: 8, 980: 8, 981: 8, 982: 8, 983: 8, 984: 8, 992: 8, 993: 7, 995: 8, 996: 8, 1000: 8, 1001: 8, 1002: 8, 1003: 8, 1008: 8, 1009: 8, 1010: 8, 1011: 8, 1012: 8, 1013: 8, 1014: 8, 1015: 8, 1024: 8, 1025: 8, 1026: 8, 1031: 8, 1033: 8, 1050: 8, 1059: 8, 1082: 8, 1083: 8, 1098: 8, 1100: 8},
|
||||
{68: 8, 168: 8, 257: 5, 258: 8, 264: 8, 268: 8, 270: 8, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 291: 8, 292: 8, 294: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 344: 8, 368: 8, 376: 3, 384: 8, 388: 4, 448: 6, 456: 4, 464: 8, 469: 8, 480: 8, 500: 8, 501: 8, 512: 8, 514: 8, 520: 8, 528: 8, 532: 8, 544: 8, 557: 8, 559: 8, 560: 4, 564: 8, 571: 3, 579: 8, 584: 8, 608: 8, 624: 8, 625: 8, 632: 8, 639: 8, 653: 8, 654: 8, 655: 8, 658: 6, 660: 8, 669: 3, 671: 8, 672: 8, 680: 8, 701: 8, 704: 8, 705: 8, 706: 8, 709: 8, 710: 8, 719: 8, 720: 6, 736: 8, 737: 8, 746: 5, 760: 8, 764: 8, 766: 8, 770: 8, 773: 8, 779: 8, 782: 8, 784: 8, 792: 8, 799: 8, 800: 8, 804: 8, 808: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 832: 8, 838: 2, 848: 8, 853: 8, 856: 4, 860: 6, 863: 8, 878: 8, 882: 8, 897: 8, 908: 8, 924: 8, 926: 3, 929: 8, 937: 8, 938: 8, 939: 8, 940: 8, 941: 8, 942: 8, 943: 8, 947: 8, 948: 8, 958: 8, 959: 8, 969: 4, 974: 5, 979: 8, 980: 8, 981: 8, 982: 8, 983: 8, 984: 8, 992: 8, 993: 7, 995: 8, 996: 8, 1000: 8, 1001: 8, 1002: 8, 1003: 8, 1008: 8, 1009: 8, 1010: 8, 1011: 8, 1012: 8, 1013: 8, 1014: 8, 1015: 8, 1024: 8, 1025: 8, 1026: 8, 1031: 8, 1033: 8, 1050: 8, 1059: 8, 1082: 8, 1083: 8, 1098: 8, 1100: 8},
|
||||
# based on 9ae7821dc4e92455|2019-07-01--16-42-55
|
||||
{168: 8, 257: 5, 258: 8, 264: 8, 268: 8, 270: 8, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 291: 8, 292: 8, 294: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 344: 8, 368: 8, 376: 3, 384: 8, 388: 4, 448: 6, 456: 4, 464: 8, 469: 8, 480: 8, 500: 8, 501: 8, 512: 8, 514: 8, 515: 7, 516: 7, 517: 7, 518: 7, 520: 8, 528: 8, 532: 8, 542: 8, 544: 8, 557: 8, 559: 8, 560: 4, 564: 8, 571: 3, 579: 8, 584: 8, 608: 8, 624: 8, 625: 8, 632: 8, 639: 8, 653: 8, 654: 8, 655: 8, 658: 6, 660: 8, 669: 3, 671: 8, 672: 8, 678: 8, 680: 8, 701: 8, 704: 8, 705: 8, 706: 8, 709: 8, 710: 8, 719: 8, 720: 6, 729: 5, 736: 8, 737: 8, 746: 5, 760: 8, 764: 8, 766: 8, 770: 8, 773: 8, 779: 8, 782: 8, 784: 8, 792: 8, 799: 8, 800: 8, 804: 8, 808: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 832: 8, 838: 2, 848: 8, 853: 8, 856: 4, 860: 6, 863: 8, 878: 8, 882: 8, 897: 8, 908: 8, 924: 8, 926: 3, 929: 8, 937: 8, 938: 8, 939: 8, 940: 8, 941: 8, 942: 8, 943: 8, 947: 8, 948: 8, 958: 8, 959: 8, 969: 4, 974: 5, 979: 8, 980: 8, 981: 8, 982: 8, 983: 8, 984: 8, 992: 8, 993: 7, 995: 8, 996: 8, 1000: 8, 1001: 8, 1002: 8, 1003: 8, 1008: 8, 1009: 8, 1010: 8, 1011: 8, 1012: 8, 1013: 8, 1014: 8, 1015: 8, 1024: 8, 1025: 8, 1026: 8, 1031: 8, 1033: 8, 1050: 8, 1059: 8, 1082: 8, 1083: 8, 1098: 8, 1100: 8, 1216: 8, 1218: 8, 1220: 8, 1225: 8, 1235: 8, 1242: 8, 1246: 8, 1250: 8, 1251: 8, 1252: 8, 1258: 8, 1259: 8, 1260: 8, 1262: 8, 1284: 8, 1537: 8, 1538: 8, 1562: 8, 1568: 8, 1856: 8, 1858: 8, 1860: 8, 1865: 8, 1875: 8, 1882: 8, 1886: 8, 1890: 8, 1891: 8, 1892: 8, 1898: 8, 1899: 8, 1900: 8, 1902: 8, 2016: 8, 2018: 8, 2019: 8, 2020: 8, 2023: 8, 2024: 8, 2026: 8, 2027: 8, 2028: 8, 2031: 8},
|
||||
],
|
||||
CAR.PACIFICA_2019_HYBRID: [
|
||||
{168: 8, 257: 5, 258: 8, 264: 8, 268: 8, 270: 8, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 291: 8, 292: 8, 294: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 344: 8, 368: 8, 376: 3, 384: 8, 388: 4, 448: 6, 456: 4, 464: 8, 469: 8, 480: 8, 500: 8, 501: 8, 512: 8, 514: 8, 515: 7, 516: 7, 517: 7, 518: 7, 520: 8, 528: 8, 532: 8, 542: 8, 544: 8, 557: 8, 559: 8, 560: 8, 564: 8, 571: 3, 579: 8, 584: 8, 608: 8, 624: 8, 625: 8, 632: 8, 639: 8, 653: 8, 654: 8, 655: 8, 660: 8, 669: 3, 671: 8, 672: 8, 680: 8, 701: 8, 703: 8, 704: 8, 705: 8, 706: 8, 709: 8, 710: 8, 719: 8, 720: 6, 736: 8, 737: 8, 746: 5, 752: 2, 754: 8, 760: 8, 764: 8, 766: 8, 770:8, 773: 8, 779: 8, 782: 8, 784: 8, 792: 8, 799: 8, 800: 8, 804: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 832: 8, 838: 2, 848: 8, 853: 8, 856: 4, 860: 6, 863: 8, 878: 8, 882: 8, 897: 8, 906: 8, 908: 8, 924: 8, 926: 3, 929: 8, 937: 8, 938: 8, 939: 8, 940: 8, 941: 8, 942: 8, 943: 8, 947: 8, 948: 8, 958: 8, 959: 8, 962: 8, 969: 4, 973: 8, 974: 5, 979: 8, 980: 8, 981: 8, 982: 8, 983: 8, 984: 8, 992: 8, 993: 7, 995: 8, 996: 8, 1000: 8, 1001: 8, 1002: 8, 1003: 8, 1008: 8, 1009: 8, 1010: 8, 1011: 8, 1012: 8, 1013: 8, 1014: 8, 1015: 8, 1024: 8, 1025: 8, 1026: 8, 1031: 8, 1033: 8, 1050: 8, 1059: 8, 1082: 8, 1083: 8, 1098: 8, 1100: 8, 1538: 8},
|
||||
{168: 8, 257: 5, 258: 8, 264: 8, 268: 8, 270: 8, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 291: 8, 292: 8, 294: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 344: 8, 368: 8, 376: 3, 384: 8, 388: 4, 448: 6, 456: 4, 464: 8, 469: 8, 480: 8, 500: 8, 501: 8, 512: 8, 514: 8, 515: 7, 516: 7, 517: 7, 518: 7, 520: 8, 528: 8, 532: 8, 542: 8, 544: 8, 557: 8, 559: 8, 560: 8, 564: 8, 571: 3, 579: 8, 584: 8, 608: 8, 624: 8, 625: 8, 632: 8, 639: 8, 653: 8, 654: 8, 655: 8, 658: 6, 660: 8, 669: 3, 671: 8, 672: 8, 680: 8, 701: 8, 703: 8, 704: 8, 705: 8, 706: 8, 709: 8, 710: 8, 719: 8, 720: 6, 736: 8, 737: 8, 746: 5, 752: 2, 754: 8, 760: 8, 764: 8, 766: 8, 770:8, 773: 8, 779: 8, 782: 8, 784: 8, 792: 8, 799: 8, 800: 8, 804: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 832: 8, 838: 2, 848: 8, 853: 8, 856: 4, 860: 6, 863: 8, 878: 8, 882: 8, 897: 8, 906: 8, 908: 8, 924: 8, 926: 3, 929: 8, 937: 8, 938: 8, 939: 8, 940: 8, 941: 8, 942: 8, 943: 8, 947: 8, 948: 8, 958: 8, 959: 8, 962: 8, 969: 4, 973: 8, 974: 5, 979: 8, 980: 8, 981: 8, 982: 8, 983: 8, 984: 8, 992: 8, 993: 7, 995: 8, 996: 8, 1000: 8, 1001: 8, 1002: 8, 1003: 8, 1008: 8, 1009: 8, 1010: 8, 1011: 8, 1012: 8, 1013: 8, 1014: 8, 1015: 8, 1024: 8, 1025: 8, 1026: 8, 1031: 8, 1033: 8, 1050: 8, 1059: 8, 1082: 8, 1083: 8, 1098: 8, 1100: 8, 1538: 8},
|
||||
# Based on 0607d2516fc2148f|2019-02-13--23-03-16
|
||||
{
|
||||
168: 8, 257: 5, 258: 8, 264: 8, 268: 8, 270: 8, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 291: 8, 292: 8, 294: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 344: 8, 368: 8, 376: 3, 384: 8, 388: 4, 448: 6, 456: 4, 464: 8, 469: 8, 480: 8, 500: 8, 501: 8, 512: 8, 514: 8, 520: 8, 528: 8, 532: 8, 544: 8, 557: 8, 559: 8, 560: 8, 564: 8, 571: 3, 579: 8, 584: 8, 608: 8, 624: 8, 625: 8, 632: 8, 639: 8, 653: 8, 654: 8, 655: 8, 658: 6, 660: 8, 669: 3, 671: 8, 672: 8, 678: 8, 680: 8, 701: 8, 703: 8, 704: 8, 705: 8, 706: 8, 709: 8, 710: 8, 719: 8, 720: 6, 729: 5, 736: 8, 737: 8, 746: 5, 752: 2, 754: 8, 760: 8, 764: 8, 766: 8, 770: 8, 773: 8, 779: 8, 782: 8, 784: 8, 792: 8, 799: 8, 800: 8, 804: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 832: 8, 838: 2, 848: 8, 853: 8, 856: 4, 860: 6, 863: 8, 878: 8, 882: 8, 897: 8, 906: 8, 908: 8, 924: 8, 926: 3, 929: 8, 937: 8, 938: 8, 939: 8, 940: 8, 941: 8, 942: 8, 943: 8, 947: 8, 948: 8, 958: 8, 959: 8, 962: 8, 969: 4, 973: 8, 974: 5, 979: 8, 980: 8, 981: 8, 982: 8, 983: 8, 984: 8, 992: 8, 993: 7, 995: 8, 996: 8, 1000: 8, 1001: 8, 1002: 8, 1003: 8, 1008: 8, 1009: 8, 1010: 8, 1011: 8, 1012: 8, 1013: 8, 1014: 8, 1015: 8, 1024: 8, 1025: 8, 1026: 8, 1031: 8, 1033: 8, 1050: 8, 1059: 8, 1082: 8, 1083: 8, 1098: 8, 1100: 8, 1537: 8
|
||||
@@ -46,12 +47,12 @@ FINGERPRINTS = {
|
||||
# JEEP GRAND CHEROKEE V6 2018
|
||||
{55: 8, 168: 8, 181: 8, 256: 4, 257: 5, 258: 8, 264: 8, 268: 8, 272: 6, 273: 6, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 292: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 344: 8, 352: 8, 362: 8, 368: 8, 376: 3, 384: 8, 388: 4, 416: 7, 448: 6, 456: 4, 464: 8, 500: 8, 501: 8, 512: 8, 514: 8, 520: 8, 532: 8, 544: 8, 557: 8, 559: 8, 560: 4, 564: 4, 571: 3, 579: 8, 584: 8, 608: 8, 618: 8, 624: 8, 625: 8, 632: 8, 639: 8, 656: 4, 658: 6, 660: 8, 671: 8, 672: 8, 676: 8, 678: 8, 680: 8, 683: 8, 684: 8, 703: 8, 705: 8, 706: 8, 709: 8, 710: 8, 719: 8, 720: 6, 729: 5, 736: 8, 737: 8, 738: 8, 746: 5, 752: 2, 754: 8, 760: 8, 761: 8, 764: 8, 766: 8, 773: 8, 776: 8, 779: 8, 782: 8, 783: 8, 784: 8, 785: 8, 788: 3, 792: 8, 799: 8, 800: 8, 804: 8, 806: 2, 808: 8, 810: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 831: 6, 832: 8, 838: 2, 844: 5, 848: 8, 853: 8, 856: 4, 860: 6, 863: 8, 882: 8, 897: 8, 906: 8, 924: 8, 937: 8, 938: 8, 939: 8, 940: 8, 941: 8, 942: 8, 943: 8, 947: 8, 948: 8, 956: 8, 968: 8, 969: 4, 970: 8, 973: 8, 974: 5, 976: 8, 977: 4, 979: 8, 980: 8, 981: 8, 982: 8, 983: 8, 984: 8, 992: 8, 993: 7, 995: 8, 996: 8, 1000: 8, 1001: 8, 1002: 8, 1003: 8, 1008: 8, 1009: 8, 1010: 8, 1011: 8, 1012: 8, 1013: 8, 1014: 8, 1015: 8, 1024: 8, 1025: 8, 1026: 8, 1031: 8, 1033: 8, 1050: 8, 1059: 8, 1062: 8, 1098: 8, 1100: 8},
|
||||
# Jeep Grand Cherokee 2017 Trailhawk
|
||||
{257: 5, 258: 8, 264: 8, 268: 8, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 292: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 344: 8, 352: 8, 362: 8, 368: 8, 376: 3, 384: 8, 388: 4, 416: 7, 448: 6, 456: 4, 464: 8, 500: 8, 501: 8, 512: 8, 514: 8, 520: 8, 532: 8, 544: 8, 557: 8, 559: 8, 560: 4, 564: 4, 571: 3, 584: 8, 608: 8, 618: 8, 624: 8, 625: 8, 632: 8, 639: 8, 660: 8, 671: 8, 672: 8, 680: 8, 684: 8, 703: 8, 705: 8, 706: 8, 709: 8, 710: 8, 719: 8, 720: 6, 736: 8, 737: 8, 746: 5, 752: 2, 760: 8, 761: 8, 764: 8, 766: 8, 773: 8, 776: 8, 779: 8, 783: 8, 784: 8, 792: 8, 799: 8, 800: 8, 804: 8, 806: 2, 808: 8, 810: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 831: 6, 832: 8, 838: 2, 844: 5, 848: 8, 853: 8, 856: 4, 860: 6, 863: 8, 882: 8, 897: 8, 924: 3, 937: 8, 947: 8, 948: 8, 969: 4, 974: 5, 977: 4, 979: 8, 980: 8, 981: 8, 982: 8, 983: 8, 984: 8, 992: 8, 993: 7, 995: 8, 996: 8, 1000: 8, 1001: 8, 1002: 8, 1003: 8, 1008: 8, 1009: 8, 1010: 8, 1011: 8, 1012: 8, 1013: 8, 1014: 8, 1015: 8, 1024: 8, 1025: 8, 1026: 8, 1031: 8, 1033: 8, 1050: 8, 1059: 8, 1062: 8, 1098: 8, 1100: 8},
|
||||
{257: 5, 258: 8, 264: 8, 268: 8, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 292: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 344: 8, 352: 8, 362: 8, 368: 8, 376: 3, 384: 8, 388: 4, 416: 7, 448: 6, 456: 4, 464: 8, 500: 8, 501: 8, 512: 8, 514: 8, 520: 8, 532: 8, 544: 8, 557: 8, 559: 8, 560: 4, 564: 4, 571: 3, 584: 8, 608: 8, 618: 8, 624: 8, 625: 8, 632: 8, 639: 8, 658: 6, 660: 8, 671: 8, 672: 8, 680: 8, 684: 8, 703: 8, 705: 8, 706: 8, 709: 8, 710: 8, 719: 8, 720: 6, 736: 8, 737: 8, 746: 5, 752: 2, 760: 8, 761: 8, 764: 8, 766: 8, 773: 8, 776: 8, 779: 8, 783: 8, 784: 8, 792: 8, 799: 8, 800: 8, 804: 8, 806: 2, 808: 8, 810: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 831: 6, 832: 8, 838: 2, 844: 5, 848: 8, 853: 8, 856: 4, 860: 6, 863: 8, 882: 8, 897: 8, 924: 3, 937: 8, 947: 8, 948: 8, 969: 4, 974: 5, 977: 4, 979: 8, 980: 8, 981: 8, 982: 8, 983: 8, 984: 8, 992: 8, 993: 7, 995: 8, 996: 8, 1000: 8, 1001: 8, 1002: 8, 1003: 8, 1008: 8, 1009: 8, 1010: 8, 1011: 8, 1012: 8, 1013: 8, 1014: 8, 1015: 8, 1024: 8, 1025: 8, 1026: 8, 1031: 8, 1033: 8, 1050: 8, 1059: 8, 1062: 8, 1098: 8, 1100: 8},
|
||||
],
|
||||
CAR.JEEP_CHEROKEE_2019: [
|
||||
# Jeep Grand Cherokee 2019 from Switzerland
|
||||
# 530: 8 is so far only in this Jeep.
|
||||
{55: 8, 181: 8, 256: 4, 257: 5, 258: 8, 264: 8, 268: 8, 272: 6, 273: 6, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 292: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 344: 8, 352: 8, 362: 8, 368: 8, 376: 3, 384: 8, 388: 4, 416: 7, 448: 6, 456: 4, 464: 8, 500: 8, 501: 8, 512: 8, 514: 8, 520: 8, 530: 8, 532: 8, 544: 8, 557: 8, 559: 8, 560: 8, 564: 8, 571: 3, 579: 8, 584: 8, 608: 8, 618: 8, 624: 8, 625: 8, 632: 8, 639: 8, 660: 8, 671: 8, 672: 8, 676: 8, 680: 8, 683: 8, 684: 8, 703: 8, 705: 8, 706: 8, 709: 8, 710: 8, 719: 8, 720: 6, 736: 8, 737: 8, 738: 8, 746: 5, 752: 2, 754: 8, 760: 8, 761: 8, 764: 8, 773: 8, 776: 8, 779: 8, 782: 8, 783: 8, 784: 8, 792: 8, 799: 8, 804: 8, 806: 2, 808: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 831: 6, 832: 8, 838: 2, 844: 5, 848: 8, 853: 8, 856: 4, 860: 6, 882: 8, 897: 8, 906: 8, 924: 8, 937: 8, 938: 8, 939: 8, 940: 8, 941: 8, 942: 8, 943: 8, 947: 8, 948: 8, 968: 8, 969: 4, 970: 8, 973: 8, 974: 5, 977: 4, 979: 8, 980: 8, 981: 8, 982: 8, 983: 8, 984: 8, 992: 8, 993: 7, 995: 8, 996: 8, 1000: 8, 1001: 8, 1002: 8, 1003: 8, 1008: 8, 1009: 8, 1010: 8, 1011: 8, 1012: 8, 1013: 8, 1014: 8, 1015: 8, 1024: 8, 1025: 8, 1026: 8, 1031: 8, 1033: 8, 1050: 8, 1059: 8, 1062: 8, 1098: 8, 1100: 8},
|
||||
{55: 8, 181: 8, 256: 4, 257: 5, 258: 8, 264: 8, 268: 8, 272: 6, 273: 6, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 292: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 344: 8, 352: 8, 362: 8, 368: 8, 376: 3, 384: 8, 388: 4, 416: 7, 448: 6, 456: 4, 464: 8, 500: 8, 501: 8, 512: 8, 514: 8, 520: 8, 530: 8, 532: 8, 544: 8, 557: 8, 559: 8, 560: 8, 564: 8, 571: 3, 579: 8, 584: 8, 608: 8, 618: 8, 624: 8, 625: 8, 632: 8, 639: 8, 658: 6, 660: 8, 671: 8, 672: 8, 676: 8, 680: 8, 683: 8, 684: 8, 703: 8, 705: 8, 706: 8, 709: 8, 710: 8, 719: 8, 720: 6, 736: 8, 737: 8, 738: 8, 746: 5, 752: 2, 754: 8, 760: 8, 761: 8, 764: 8, 773: 8, 776: 8, 779: 8, 782: 8, 783: 8, 784: 8, 792: 8, 799: 8, 804: 8, 806: 2, 808: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 831: 6, 832: 8, 838: 2, 844: 5, 848: 8, 853: 8, 856: 4, 860: 6, 882: 8, 897: 8, 906: 8, 924: 8, 937: 8, 938: 8, 939: 8, 940: 8, 941: 8, 942: 8, 943: 8, 947: 8, 948: 8, 968: 8, 969: 4, 970: 8, 973: 8, 974: 5, 977: 4, 979: 8, 980: 8, 981: 8, 982: 8, 983: 8, 984: 8, 992: 8, 993: 7, 995: 8, 996: 8, 1000: 8, 1001: 8, 1002: 8, 1003: 8, 1008: 8, 1009: 8, 1010: 8, 1011: 8, 1012: 8, 1013: 8, 1014: 8, 1015: 8, 1024: 8, 1025: 8, 1026: 8, 1031: 8, 1033: 8, 1050: 8, 1059: 8, 1062: 8, 1098: 8, 1100: 8},
|
||||
],
|
||||
}
|
||||
|
||||
@@ -85,10 +86,5 @@ class ECU:
|
||||
|
||||
|
||||
ECU_FINGERPRINT = {
|
||||
ECU.CAM: 0x2d9, # steer torque cmd
|
||||
ECU.CAM: [0x292], # lkas cmd
|
||||
}
|
||||
|
||||
|
||||
def check_ecu_msgs(fingerprint, ecu):
|
||||
# return True if fingerprint contains messages normally sent by a given ecu
|
||||
return ECU_FINGERPRINT[ecu] in fingerprint
|
||||
|
||||
@@ -0,0 +1,62 @@
|
||||
import os
|
||||
from common.basedir import BASEDIR
|
||||
|
||||
def get_fingerprint_list():
|
||||
# read all the folders in selfdrive/car and return a dict where:
|
||||
# - keys are all the car models for which we have a fingerprint
|
||||
# - values are lists dicts of messages that constitute the unique
|
||||
# CAN fingerprint of each car model and all its variants
|
||||
fingerprints = {}
|
||||
for car_folder in [x[0] for x in os.walk(BASEDIR + '/selfdrive/car')]:
|
||||
try:
|
||||
car_name = car_folder.split('/')[-1]
|
||||
values = __import__('selfdrive.car.%s.values' % car_name, fromlist=['FINGERPRINTS'])
|
||||
if hasattr(values, 'FINGERPRINTS'):
|
||||
car_fingerprints = values.FINGERPRINTS
|
||||
else:
|
||||
continue
|
||||
for f, v in car_fingerprints.items():
|
||||
fingerprints[f] = v
|
||||
except (ImportError, IOError):
|
||||
pass
|
||||
return fingerprints
|
||||
|
||||
|
||||
_FINGERPRINTS = get_fingerprint_list()
|
||||
|
||||
_DEBUG_ADDRESS = {1880: 8} # reserved for debug purposes
|
||||
|
||||
def is_valid_for_fingerprint(msg, car_fingerprint):
|
||||
adr = msg.address
|
||||
# ignore addresses that are more than 11 bits
|
||||
return (adr in car_fingerprint and car_fingerprint[adr] == len(msg.dat)) or adr >= 0x800
|
||||
|
||||
|
||||
def eliminate_incompatible_cars(msg, candidate_cars):
|
||||
"""Removes cars that could not have sent msg.
|
||||
|
||||
Inputs:
|
||||
msg: A cereal/log CanData message from the car.
|
||||
candidate_cars: A list of cars to consider.
|
||||
|
||||
Returns:
|
||||
A list containing the subset of candidate_cars that could have sent msg.
|
||||
"""
|
||||
compatible_cars = []
|
||||
|
||||
for car_name in candidate_cars:
|
||||
car_fingerprints = _FINGERPRINTS[car_name]
|
||||
|
||||
for fingerprint in car_fingerprints:
|
||||
fingerprint.update(_DEBUG_ADDRESS) # add alien debug address
|
||||
|
||||
if is_valid_for_fingerprint(msg, fingerprint):
|
||||
compatible_cars.append(car_name)
|
||||
break
|
||||
|
||||
return compatible_cars
|
||||
|
||||
|
||||
def all_known_cars():
|
||||
"""Returns a list of all known car strings."""
|
||||
return list(_FINGERPRINTS.keys())
|
||||
@@ -7,7 +7,7 @@ from selfdrive.can.packer import CANPacker
|
||||
MAX_STEER_DELTA = 1
|
||||
TOGGLE_DEBUG = False
|
||||
|
||||
class CarController(object):
|
||||
class CarController():
|
||||
def __init__(self, dbc_name, enable_camera, vehicle_model):
|
||||
self.packer = CANPacker(dbc_name)
|
||||
self.enable_camera = enable_camera
|
||||
@@ -48,7 +48,7 @@ class CarController(object):
|
||||
|
||||
if (frame % 100) == 0:
|
||||
|
||||
can_sends.append(make_can_msg(973, '\x00\x00\x00\x00\x00\x00\x00\x00', 0, False))
|
||||
can_sends.append(make_can_msg(973, b'\x00\x00\x00\x00\x00\x00\x00\x00', 0, False))
|
||||
#can_sends.append(make_can_msg(984, '\x00\x00\x00\x00\x80\x45\x60\x30', 0, False))
|
||||
|
||||
if (frame % 100) == 0 or (self.enabled_last != enabled) or (self.main_on_last != CS.main_on) or \
|
||||
@@ -56,29 +56,29 @@ class CarController(object):
|
||||
can_sends.append(create_lkas_ui(self.packer, CS.main_on, enabled, steer_alert))
|
||||
|
||||
if (frame % 200) == 0:
|
||||
can_sends.append(make_can_msg(1875, '\x80\xb0\x55\x55\x78\x90\x00\x00', 1, False))
|
||||
can_sends.append(make_can_msg(1875, b'\x80\xb0\x55\x55\x78\x90\x00\x00', 1, False))
|
||||
|
||||
if (frame % 10) == 0:
|
||||
|
||||
can_sends.append(make_can_msg(1648, '\x00\x00\x00\x40\x00\x00\x50\x00', 1, False))
|
||||
can_sends.append(make_can_msg(1649, '\x10\x10\xf1\x70\x04\x00\x00\x00', 1, False))
|
||||
can_sends.append(make_can_msg(1648, b'\x00\x00\x00\x40\x00\x00\x50\x00', 1, False))
|
||||
can_sends.append(make_can_msg(1649, b'\x10\x10\xf1\x70\x04\x00\x00\x00', 1, False))
|
||||
|
||||
can_sends.append(make_can_msg(1664, '\x00\x00\x03\xe8\x00\x01\xa9\xb2', 1, False))
|
||||
can_sends.append(make_can_msg(1674, '\x08\x00\x00\xff\x0c\xfb\x6a\x08', 1, False))
|
||||
can_sends.append(make_can_msg(1675, '\x00\x00\x3b\x60\x37\x00\x00\x00', 1, False))
|
||||
can_sends.append(make_can_msg(1690, '\x70\x00\x00\x55\x86\x1c\xe0\x00', 1, False))
|
||||
can_sends.append(make_can_msg(1664, b'\x00\x00\x03\xe8\x00\x01\xa9\xb2', 1, False))
|
||||
can_sends.append(make_can_msg(1674, b'\x08\x00\x00\xff\x0c\xfb\x6a\x08', 1, False))
|
||||
can_sends.append(make_can_msg(1675, b'\x00\x00\x3b\x60\x37\x00\x00\x00', 1, False))
|
||||
can_sends.append(make_can_msg(1690, b'\x70\x00\x00\x55\x86\x1c\xe0\x00', 1, False))
|
||||
|
||||
can_sends.append(make_can_msg(1910, '\x06\x4b\x06\x4b\x42\xd3\x11\x30', 1, False))
|
||||
can_sends.append(make_can_msg(1911, '\x48\x53\x37\x54\x48\x53\x37\x54', 1, False))
|
||||
can_sends.append(make_can_msg(1912, '\x31\x34\x47\x30\x38\x31\x43\x42', 1, False))
|
||||
can_sends.append(make_can_msg(1913, '\x31\x34\x47\x30\x38\x32\x43\x42', 1, False))
|
||||
can_sends.append(make_can_msg(1969, '\xf4\x40\x00\x00\x00\x00\x00\x00', 1, False))
|
||||
can_sends.append(make_can_msg(1971, '\x0b\xc0\x00\x00\x00\x00\x00\x00', 1, False))
|
||||
can_sends.append(make_can_msg(1910, b'\x06\x4b\x06\x4b\x42\xd3\x11\x30', 1, False))
|
||||
can_sends.append(make_can_msg(1911, b'\x48\x53\x37\x54\x48\x53\x37\x54', 1, False))
|
||||
can_sends.append(make_can_msg(1912, b'\x31\x34\x47\x30\x38\x31\x43\x42', 1, False))
|
||||
can_sends.append(make_can_msg(1913, b'\x31\x34\x47\x30\x38\x32\x43\x42', 1, False))
|
||||
can_sends.append(make_can_msg(1969, b'\xf4\x40\x00\x00\x00\x00\x00\x00', 1, False))
|
||||
can_sends.append(make_can_msg(1971, b'\x0b\xc0\x00\x00\x00\x00\x00\x00', 1, False))
|
||||
|
||||
static_msgs = range(1653, 1658)
|
||||
for addr in static_msgs:
|
||||
cnt = (frame % 10) + 1
|
||||
can_sends.append(make_can_msg(addr, chr(cnt<<4) + '\x00\x00\x00\x00\x00\x00\x00', 1, False))
|
||||
can_sends.append(make_can_msg(addr, (cnt<<4).to_bytes(1, 'little') + b'\x00\x00\x00\x00\x00\x00\x00', 1, False))
|
||||
|
||||
self.enabled_last = enabled
|
||||
self.main_on_last = CS.main_on
|
||||
|
||||
@@ -1,8 +1,8 @@
|
||||
from selfdrive.can.parser import CANParser
|
||||
from common.numpy_fast import mean
|
||||
from selfdrive.config import Conversions as CV
|
||||
from selfdrive.car.ford.values import DBC
|
||||
from common.kalman.simple_kalman import KF1D
|
||||
import numpy as np
|
||||
|
||||
WHEEL_RADIUS = 0.33
|
||||
|
||||
@@ -32,7 +32,7 @@ def get_can_parser(CP):
|
||||
return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 0)
|
||||
|
||||
|
||||
class CarState(object):
|
||||
class CarState():
|
||||
def __init__(self, CP):
|
||||
|
||||
self.CP = CP
|
||||
@@ -62,7 +62,7 @@ class CarState(object):
|
||||
self.v_wheel_fr = cp.vl["WheelSpeed_CG1"]['WhlRl_W_Meas'] * WHEEL_RADIUS
|
||||
self.v_wheel_rl = cp.vl["WheelSpeed_CG1"]['WhlFr_W_Meas'] * WHEEL_RADIUS
|
||||
self.v_wheel_rr = cp.vl["WheelSpeed_CG1"]['WhlFl_W_Meas'] * WHEEL_RADIUS
|
||||
v_wheel = float(np.mean([self.v_wheel_fl, self.v_wheel_fr, self.v_wheel_rl, self.v_wheel_rr]))
|
||||
v_wheel = mean([self.v_wheel_fl, self.v_wheel_fr, self.v_wheel_rl, self.v_wheel_rr])
|
||||
|
||||
# Kalman filter
|
||||
if abs(v_wheel - self.v_ego) > 2.0: # Prevent large accelerations when car starts at non zero speed
|
||||
|
||||
@@ -1,15 +1,16 @@
|
||||
#!/usr/bin/env python
|
||||
#!/usr/bin/env python3
|
||||
from cereal import car
|
||||
from selfdrive.swaglog import cloudlog
|
||||
from selfdrive.config import Conversions as CV
|
||||
from selfdrive.controls.lib.drive_helpers import EventTypes as ET, create_event
|
||||
from selfdrive.controls.lib.vehicle_model import VehicleModel
|
||||
from selfdrive.car.ford.carstate import CarState, get_can_parser
|
||||
from selfdrive.car.ford.values import MAX_ANGLE
|
||||
from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness
|
||||
from selfdrive.car.ford.values import MAX_ANGLE, ECU, ECU_FINGERPRINT, FINGERPRINTS
|
||||
from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, is_ecu_disconnected, gen_empty_fingerprint
|
||||
from selfdrive.car.interfaces import CarInterfaceBase
|
||||
|
||||
|
||||
class CarInterface(object):
|
||||
class CarInterface(CarInterfaceBase):
|
||||
def __init__(self, CP, CarController):
|
||||
self.CP = CP
|
||||
self.VM = VehicleModel(CP)
|
||||
@@ -33,18 +34,14 @@ class CarInterface(object):
|
||||
return float(accel) / 3.0
|
||||
|
||||
@staticmethod
|
||||
def calc_accel_override(a_ego, a_target, v_ego, v_target):
|
||||
return 1.0
|
||||
|
||||
@staticmethod
|
||||
def get_params(candidate, fingerprint, vin="", is_panda_black=False):
|
||||
def get_params(candidate, fingerprint=gen_empty_fingerprint(), vin="", has_relay=False):
|
||||
|
||||
ret = car.CarParams.new_message()
|
||||
|
||||
ret.carName = "ford"
|
||||
ret.carFingerprint = candidate
|
||||
ret.carVin = vin
|
||||
ret.isPandaBlack = is_panda_black
|
||||
ret.isPandaBlack = has_relay
|
||||
|
||||
ret.safetyModel = car.CarParams.SafetyModel.ford
|
||||
ret.dashcamOnly = True
|
||||
@@ -88,9 +85,9 @@ class CarInterface(object):
|
||||
ret.brakeMaxBP = [5., 20.]
|
||||
ret.brakeMaxV = [1., 0.8]
|
||||
|
||||
ret.enableCamera = not any(x for x in [970, 973, 984] if x in fingerprint) or is_panda_black
|
||||
ret.enableCamera = is_ecu_disconnected(fingerprint[0], FINGERPRINTS, ECU_FINGERPRINT, candidate, ECU.CAM) or has_relay
|
||||
ret.openpilotLongitudinalControl = False
|
||||
cloudlog.warn("ECU Camera Simulated: %r", ret.enableCamera)
|
||||
cloudlog.warning("ECU Camera Simulated: %r", ret.enableCamera)
|
||||
|
||||
ret.steerLimitAlert = False
|
||||
ret.stoppingControl = False
|
||||
|
||||
@@ -1,32 +1,32 @@
|
||||
#!/usr/bin/env python
|
||||
import os
|
||||
import numpy as np
|
||||
from selfdrive.can.parser import CANParser
|
||||
#!/usr/bin/env python3
|
||||
from cereal import car
|
||||
from selfdrive.can.parser import CANParser
|
||||
from selfdrive.car.ford.values import DBC
|
||||
from selfdrive.config import Conversions as CV
|
||||
from selfdrive.car.interfaces import RadarInterfaceBase
|
||||
|
||||
RADAR_MSGS = range(0x500, 0x540)
|
||||
RADAR_MSGS = list(range(0x500, 0x540))
|
||||
|
||||
def _create_radar_can_parser():
|
||||
dbc_f = 'ford_fusion_2018_adas.dbc'
|
||||
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(os.path.splitext(dbc_f)[0], signals, checks, 1)
|
||||
return CANParser(dbc_f, signals, checks, 1)
|
||||
|
||||
class RadarInterface(object):
|
||||
class RadarInterface(RadarInterfaceBase):
|
||||
def __init__(self, CP):
|
||||
# radar
|
||||
self.pts = {}
|
||||
self.validCnt = {key: 0 for key in RADAR_MSGS}
|
||||
self.track_id = 0
|
||||
|
||||
self.delay = 0.0 # Delay of radar
|
||||
self.delay = 0 # Delay of radar
|
||||
|
||||
# Nidec
|
||||
self.rcp = _create_radar_can_parser()
|
||||
self.rcp = _create_radar_can_parser(CP.carFingerprint)
|
||||
self.trigger_msg = 0x53f
|
||||
self.updated_messages = set()
|
||||
|
||||
@@ -44,7 +44,7 @@ class RadarInterface(object):
|
||||
errors.append("canError")
|
||||
ret.errors = errors
|
||||
|
||||
for ii in self.updated_messages:
|
||||
for ii in sorted(self.updated_messages):
|
||||
cpt = self.rcp.vl[ii]
|
||||
|
||||
if cpt['X_Rel'] > 0.00001:
|
||||
@@ -63,7 +63,7 @@ class RadarInterface(object):
|
||||
self.pts[ii].trackId = self.track_id
|
||||
self.track_id += 1
|
||||
self.pts[ii].dRel = cpt['X_Rel'] # from front of car
|
||||
self.pts[ii].yRel = cpt['X_Rel'] * cpt['Angle'] * np.pi / 180. # in car frame's y axis, left is positive
|
||||
self.pts[ii].yRel = cpt['X_Rel'] * cpt['Angle'] * CV.DEG_TO_RAD # in car frame's y axis, left is positive
|
||||
self.pts[ii].vRel = cpt['V_Rel']
|
||||
self.pts[ii].aRel = float('nan')
|
||||
self.pts[ii].yvRel = float('nan')
|
||||
@@ -72,6 +72,6 @@ class RadarInterface(object):
|
||||
if ii in self.pts:
|
||||
del self.pts[ii]
|
||||
|
||||
ret.points = self.pts.values()
|
||||
ret.points = list(self.pts.values())
|
||||
self.updated_messages.clear()
|
||||
return ret
|
||||
|
||||
@@ -11,6 +11,13 @@ FINGERPRINTS = {
|
||||
}],
|
||||
}
|
||||
|
||||
class ECU:
|
||||
CAM = 0
|
||||
|
||||
ECU_FINGERPRINT = {
|
||||
ECU.CAM: [970, 973, 984]
|
||||
}
|
||||
|
||||
DBC = {
|
||||
CAR.FUSION: dbc_dict('ford_fusion_2018_pt', 'ford_fusion_2018_adas'),
|
||||
}
|
||||
|
||||
@@ -68,7 +68,7 @@ def process_hud_alert(hud_alert):
|
||||
steer = 1
|
||||
return steer
|
||||
|
||||
class CarController(object):
|
||||
class CarController():
|
||||
def __init__(self, canbus, car_fingerprint):
|
||||
self.pedal_steady = 0.
|
||||
self.start_time = 0.
|
||||
|
||||
@@ -1,5 +1,5 @@
|
||||
import numpy as np
|
||||
from cereal import car
|
||||
from common.numpy_fast import mean
|
||||
from common.kalman.simple_kalman import KF1D
|
||||
from selfdrive.config import Conversions as CV
|
||||
from selfdrive.can.parser import CANParser
|
||||
@@ -50,7 +50,7 @@ def get_powertrain_can_parser(CP, canbus):
|
||||
return CANParser(DBC[CP.carFingerprint]['pt'], signals, [], canbus.powertrain)
|
||||
|
||||
|
||||
class CarState(object):
|
||||
class CarState():
|
||||
def __init__(self, CP, canbus):
|
||||
self.CP = CP
|
||||
# initialize can parser
|
||||
@@ -78,7 +78,7 @@ class CarState(object):
|
||||
self.v_wheel_fr = pt_cp.vl["EBCMWheelSpdFront"]['FRWheelSpd'] * CV.KPH_TO_MS
|
||||
self.v_wheel_rl = pt_cp.vl["EBCMWheelSpdRear"]['RLWheelSpd'] * CV.KPH_TO_MS
|
||||
self.v_wheel_rr = pt_cp.vl["EBCMWheelSpdRear"]['RRWheelSpd'] * CV.KPH_TO_MS
|
||||
v_wheel = float(np.mean([self.v_wheel_fl, self.v_wheel_fr, self.v_wheel_rl, self.v_wheel_rr]))
|
||||
v_wheel = mean([self.v_wheel_fl, self.v_wheel_fr, self.v_wheel_rl, self.v_wheel_rr])
|
||||
|
||||
if abs(v_wheel - self.v_ego) > 2.0: # Prevent large accelerations when car starts at non zero speed
|
||||
self.v_ego_kf.x = [[v_wheel], [0.0]]
|
||||
|
||||
+14
-14
@@ -24,7 +24,7 @@ def create_steering_control_ct6(packer, canbus, apply_steer, v_ego, idx, enabled
|
||||
dat = packer.make_can_msg("ASCMLKASteeringCmd", 0, values)[2]
|
||||
# the checksum logic is weird
|
||||
values['LKASteeringCmdChecksum'] = (0x2a +
|
||||
sum([ord(i) for i in dat][:4]) +
|
||||
sum(dat[:4]) +
|
||||
values['LKASMode']) & 0x3ff
|
||||
# pack again with checksum
|
||||
dat = packer.make_can_msg("ASCMLKASteeringCmd", 0, values)[2]
|
||||
@@ -36,7 +36,7 @@ def create_steering_control_ct6(packer, canbus, apply_steer, v_ego, idx, enabled
|
||||
|
||||
|
||||
def create_adas_keepalive(bus):
|
||||
dat = "\x00\x00\x00\x00\x00\x00\x00"
|
||||
dat = b"\x00\x00\x00\x00\x00\x00\x00"
|
||||
return [[0x409, 0, dat, bus], [0x40a, 0, dat, bus]]
|
||||
|
||||
def create_gas_regen_command(packer, bus, throttle, idx, acc_engaged, at_full_stop):
|
||||
@@ -52,9 +52,9 @@ def create_gas_regen_command(packer, bus, throttle, idx, acc_engaged, at_full_st
|
||||
}
|
||||
|
||||
dat = packer.make_can_msg("ASCMGasRegenCmd", bus, values)[2]
|
||||
values["GasRegenChecksum"] = (((0xff - ord(dat[1])) & 0xff) << 16) | \
|
||||
(((0xff - ord(dat[2])) & 0xff) << 8) | \
|
||||
((0x100 - ord(dat[3]) - idx) & 0xff)
|
||||
values["GasRegenChecksum"] = (((0xff -dat[1]) & 0xff) << 16) | \
|
||||
(((0xff - dat[2]) & 0xff) << 8) | \
|
||||
((0x100 - dat[3] - idx) & 0xff)
|
||||
|
||||
return packer.make_can_msg("ASCMGasRegenCmd", bus, values)
|
||||
|
||||
@@ -106,13 +106,13 @@ def create_adas_time_status(bus, tt, idx):
|
||||
chksum = 0x1000 - dat[0] - dat[1] - dat[2] - dat[3]
|
||||
chksum = chksum & 0xfff
|
||||
dat += [0x40 + (chksum >> 8), chksum & 0xff, 0x12]
|
||||
return [0xa1, 0, "".join(map(chr, dat)), bus]
|
||||
return [0xa1, 0, bytes(dat), bus]
|
||||
|
||||
def create_adas_steering_status(bus, idx):
|
||||
dat = [idx << 6, 0xf0, 0x20, 0, 0, 0]
|
||||
chksum = 0x60 + sum(dat)
|
||||
dat += [chksum >> 8, chksum & 0xff]
|
||||
return [0x306, 0, "".join(map(chr, dat)), bus]
|
||||
return [0x306, 0, bytes(dat), bus]
|
||||
|
||||
def create_adas_accelerometer_speed_status(bus, speed_ms, idx):
|
||||
spd = int(speed_ms * 16) & 0xfff
|
||||
@@ -125,24 +125,24 @@ def create_adas_accelerometer_speed_status(bus, speed_ms, idx):
|
||||
dat = [0x08, spd >> 4, ((spd & 0xf) << 4) | (accel >> 8), accel & 0xff, 0]
|
||||
chksum = 0x62 + far_range_mode + (idx << 2) + dat[0] + dat[1] + dat[2] + dat[3] + dat[4]
|
||||
dat += [(idx << 5) + (far_range_mode << 4) + (near_range_mode << 3) + (chksum >> 8), chksum & 0xff]
|
||||
return [0x308, 0, "".join(map(chr, dat)), bus]
|
||||
return [0x308, 0, bytes(dat), bus]
|
||||
|
||||
def create_adas_headlights_status(bus):
|
||||
return [0x310, 0, "\x42\x04", bus]
|
||||
return [0x310, 0, b"\x42\x04", bus]
|
||||
|
||||
def create_lka_icon_command(bus, active, critical, steer):
|
||||
if active and steer == 1:
|
||||
if critical:
|
||||
dat = "\x50\xc0\x14"
|
||||
dat = b"\x50\xc0\x14"
|
||||
else:
|
||||
dat = "\x50\x40\x18"
|
||||
dat = b"\x50\x40\x18"
|
||||
elif active:
|
||||
if critical:
|
||||
dat = "\x40\xc0\x14"
|
||||
dat = b"\x40\xc0\x14"
|
||||
else:
|
||||
dat = "\x40\x40\x18"
|
||||
dat = b"\x40\x40\x18"
|
||||
else:
|
||||
dat = "\x00\x00\x00"
|
||||
dat = b"\x00\x00\x00"
|
||||
return [0x104c006c, 0, dat, bus]
|
||||
|
||||
# TODO: WIP
|
||||
|
||||
@@ -1,23 +1,24 @@
|
||||
#!/usr/bin/env python
|
||||
#!/usr/bin/env python3
|
||||
from cereal import car
|
||||
from selfdrive.config import Conversions as CV
|
||||
from selfdrive.controls.lib.drive_helpers import create_event, EventTypes as ET
|
||||
from selfdrive.controls.lib.vehicle_model import VehicleModel
|
||||
from selfdrive.car.gm.values import DBC, CAR, STOCK_CONTROL_MSGS, \
|
||||
SUPERCRUISE_CARS, AccState
|
||||
from selfdrive.car.gm.values import DBC, CAR, ECU, ECU_FINGERPRINT, \
|
||||
SUPERCRUISE_CARS, AccState, FINGERPRINTS
|
||||
from selfdrive.car.gm.carstate import CarState, CruiseButtons, get_powertrain_can_parser
|
||||
from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness
|
||||
from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, is_ecu_disconnected, gen_empty_fingerprint
|
||||
from selfdrive.car.interfaces import CarInterfaceBase
|
||||
|
||||
ButtonType = car.CarState.ButtonEvent.Type
|
||||
|
||||
class CanBus(object):
|
||||
class CanBus(CarInterfaceBase):
|
||||
def __init__(self):
|
||||
self.powertrain = 0
|
||||
self.obstacle = 1
|
||||
self.chassis = 2
|
||||
self.sw_gmlan = 3
|
||||
|
||||
class CarInterface(object):
|
||||
class CarInterface(CarInterfaceBase):
|
||||
def __init__(self, CP, CarController):
|
||||
self.CP = CP
|
||||
|
||||
@@ -42,24 +43,22 @@ class CarInterface(object):
|
||||
return float(accel) / 4.0
|
||||
|
||||
@staticmethod
|
||||
def calc_accel_override(a_ego, a_target, v_ego, v_target):
|
||||
return 1.0
|
||||
|
||||
@staticmethod
|
||||
def get_params(candidate, fingerprint, vin="", is_panda_black=False):
|
||||
def get_params(candidate, fingerprint=gen_empty_fingerprint(), vin="", has_relay=False):
|
||||
ret = car.CarParams.new_message()
|
||||
|
||||
ret.carName = "gm"
|
||||
ret.carFingerprint = candidate
|
||||
ret.carVin = vin
|
||||
ret.isPandaBlack = is_panda_black
|
||||
ret.isPandaBlack = has_relay
|
||||
|
||||
ret.enableCruise = False
|
||||
|
||||
# Presence of a camera on the object bus is ok.
|
||||
# Have to go to read_only if ASCM is online (ACC-enabled cars),
|
||||
# or camera is on powertrain bus (LKA cars without ACC).
|
||||
ret.enableCamera = not any(x for x in STOCK_CONTROL_MSGS[candidate] if x in fingerprint) or is_panda_black
|
||||
ret.enableCamera = is_ecu_disconnected(fingerprint[0], FINGERPRINTS, ECU_FINGERPRINT, candidate, ECU.CAM) or \
|
||||
has_relay or \
|
||||
candidate == CAR.CADILLAC_CT6
|
||||
ret.openpilotLongitudinalControl = ret.enableCamera
|
||||
tire_stiffness_factor = 0.444 # not optimized yet
|
||||
ret.safetyModelPassive = car.CarParams.SafetyModel.gmPassive
|
||||
|
||||
@@ -1,11 +1,13 @@
|
||||
#!/usr/bin/env python
|
||||
#!/usr/bin/env python3
|
||||
from __future__ import print_function
|
||||
import math
|
||||
import time
|
||||
import numpy as np
|
||||
from cereal import car
|
||||
from selfdrive.can.parser import CANParser
|
||||
from selfdrive.car.gm.interface import CanBus
|
||||
from selfdrive.car.gm.values import DBC, CAR
|
||||
from selfdrive.config import Conversions as CV
|
||||
from selfdrive.car.interfaces import RadarInterfaceBase
|
||||
|
||||
RADAR_HEADER_MSG = 1120
|
||||
SLOT_1_MSG = RADAR_HEADER_MSG + 1
|
||||
@@ -20,7 +22,7 @@ def create_radar_can_parser(canbus, 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 = range(SLOT_1_MSG, SLOT_1_MSG + NUM_SLOTS)
|
||||
radar_targets = list(range(SLOT_1_MSG, SLOT_1_MSG + NUM_SLOTS))
|
||||
signals = list(zip(['FLRRNumValidTargets',
|
||||
'FLRRSnsrBlckd', 'FLRRYawRtPlsblityFlt',
|
||||
'FLRRHWFltPrsntInt', 'FLRRAntTngFltPrsnt',
|
||||
@@ -40,15 +42,15 @@ def create_radar_can_parser(canbus, car_fingerprint):
|
||||
else:
|
||||
return None
|
||||
|
||||
class RadarInterface(object):
|
||||
class RadarInterface(RadarInterfaceBase):
|
||||
def __init__(self, CP):
|
||||
# radar
|
||||
self.pts = {}
|
||||
|
||||
self.delay = 0.0 # Delay of radar
|
||||
self.delay = 0 # Delay of radar
|
||||
|
||||
canbus = CanBus()
|
||||
print "Using %d as obstacle CAN bus ID" % canbus.obstacle
|
||||
print("Using %d as obstacle CAN bus ID" % canbus.obstacle)
|
||||
self.rcp = create_radar_can_parser(canbus, CP.carFingerprint)
|
||||
|
||||
self.trigger_msg = LAST_RADAR_MSG
|
||||
@@ -65,7 +67,6 @@ class RadarInterface(object):
|
||||
if self.trigger_msg not in self.updated_messages:
|
||||
return None
|
||||
|
||||
|
||||
ret = car.RadarData.new_message()
|
||||
header = self.rcp.vl[RADAR_HEADER_MSG]
|
||||
fault = header['FLRRSnsrBlckd'] or header['FLRRSnstvFltPrsntInt'] or \
|
||||
@@ -101,16 +102,15 @@ class RadarInterface(object):
|
||||
distance = cpt['TrkRange']
|
||||
self.pts[targetId].dRel = distance # from front of car
|
||||
# From driver's pov, left is positive
|
||||
deg_to_rad = np.pi/180.
|
||||
self.pts[targetId].yRel = math.sin(deg_to_rad * cpt['TrkAzimuth']) * distance
|
||||
self.pts[targetId].yRel = math.sin(cpt['TrkAzimuth'] * CV.DEG_TO_RAD) * distance
|
||||
self.pts[targetId].vRel = cpt['TrkRangeRate']
|
||||
self.pts[targetId].aRel = float('nan')
|
||||
self.pts[targetId].yvRel = float('nan')
|
||||
|
||||
for oldTarget in self.pts.keys():
|
||||
for oldTarget in list(self.pts.keys()):
|
||||
if not oldTarget in currentTargets:
|
||||
del self.pts[oldTarget]
|
||||
|
||||
ret.points = self.pts.values()
|
||||
ret.points = list(self.pts.values())
|
||||
self.updated_messages.clear()
|
||||
return ret
|
||||
|
||||
@@ -48,12 +48,12 @@ def parse_gear_shifter(can_gear):
|
||||
FINGERPRINTS = {
|
||||
# Astra BK MY17, ASCM unplugged
|
||||
CAR.HOLDEN_ASTRA: [{
|
||||
190: 8, 193: 8, 197: 8, 199: 4, 201: 8, 209: 7, 211: 8, 241: 6, 249: 8, 288: 5, 298: 8, 304: 1, 309: 8, 311: 8, 313: 8, 320: 3, 328: 1, 352: 5, 381: 6, 386: 8, 388: 8, 393: 8, 398: 8, 401: 8, 413: 8, 417: 8, 419: 8, 422: 1, 426: 7, 431: 8, 442: 8, 451: 8, 452: 8, 453: 8, 455: 7, 456: 8, 458: 5, 479: 8, 481: 7, 485: 8, 489: 8, 497: 8, 499: 3, 500: 8, 501: 8, 508: 8, 528: 5, 532: 6, 554: 3, 560: 8, 562: 8, 563: 5, 564: 5, 565: 5, 567: 5, 647: 5, 707: 8, 723: 8, 753: 5, 761: 7, 806: 1, 810: 8, 840: 5, 842: 5, 844: 8, 866: 4, 961: 8, 969: 8, 977: 8, 979: 8, 985: 5, 1001: 8, 1009: 8, 1011: 6, 1017: 8, 1019: 3, 1020: 8, 1105: 6, 1217: 8, 1221: 5, 1225: 8, 1233: 8, 1249: 8, 1257: 6, 1259: 8, 1261: 7, 1263: 4, 1265: 8, 1267: 8, 1280: 4, 1300: 8, 1328: 4, 1417: 8, 1906: 7, 1907: 7, 1908: 7, 1912: 7, 1919: 7,
|
||||
190: 8, 193: 8, 197: 8, 199: 4, 201: 8, 209: 7, 211: 8, 241: 6, 249: 8, 288: 5, 298: 8, 304: 1, 309: 8, 311: 8, 313: 8, 320: 3, 328: 1, 352: 5, 381: 6, 384: 4, 386: 8, 388: 8, 393: 8, 398: 8, 401: 8, 413: 8, 417: 8, 419: 8, 422: 1, 426: 7, 431: 8, 442: 8, 451: 8, 452: 8, 453: 8, 455: 7, 456: 8, 458: 5, 479: 8, 481: 7, 485: 8, 489: 8, 497: 8, 499: 3, 500: 8, 501: 8, 508: 8, 528: 5, 532: 6, 554: 3, 560: 8, 562: 8, 563: 5, 564: 5, 565: 5, 567: 5, 647: 5, 707: 8, 715: 8, 723: 8, 753: 5, 761: 7, 806: 1, 810: 8, 840: 5, 842: 5, 844: 8, 866: 4, 961: 8, 969: 8, 977: 8, 979: 8, 985: 5, 1001: 8, 1009: 8, 1011: 6, 1017: 8, 1019: 3, 1020: 8, 1105: 6, 1217: 8, 1221: 5, 1225: 8, 1233: 8, 1249: 8, 1257: 6, 1259: 8, 1261: 7, 1263: 4, 1265: 8, 1267: 8, 1280: 4, 1300: 8, 1328: 4, 1417: 8, 1906: 7, 1907: 7, 1908: 7, 1912: 7, 1919: 7,
|
||||
}],
|
||||
CAR.VOLT: [
|
||||
# Volt Premier w/ ACC 2017
|
||||
{
|
||||
170: 8, 171: 8, 189: 7, 190: 6, 193: 8, 197: 8, 199: 4, 201: 8, 209: 7, 211: 2, 241: 6, 288: 5, 289: 8, 298: 8, 304: 1, 308: 4, 309: 8, 311: 8, 313: 8, 320: 3, 328: 1, 352: 5, 381: 6, 386: 8, 388: 8, 389: 2, 390: 7, 417: 7, 419: 1, 426: 7, 451: 8, 452: 8, 453: 6, 454: 8, 456: 8, 479: 3, 481: 7, 485: 8, 489: 8, 493: 8, 495: 4, 497: 8, 499: 3, 500: 6, 501: 8, 508: 8, 528: 4, 532: 6, 546: 7, 550: 8, 554: 3, 558: 8, 560: 8, 562: 8, 563: 5, 564: 5, 565: 5, 566: 5, 567: 3, 568: 1, 573: 1, 577: 8, 647: 3, 707: 8, 711: 6, 761: 7, 810: 8, 840: 5, 842: 5, 844: 8, 866: 4, 961: 8, 969: 8, 977: 8, 979: 7, 988: 6, 989: 8, 995: 7, 1001: 8, 1005: 6, 1009: 8, 1017: 8, 1019: 2, 1020: 8, 1105: 6, 1187: 4, 1217: 8, 1221: 5, 1223: 3, 1225: 7, 1227: 4, 1233: 8, 1249: 8, 1257: 6, 1265: 8, 1267: 1, 1273: 3, 1275: 3, 1280: 4, 1300: 8, 1322: 6, 1323: 4, 1328: 4, 1417: 8, 1601: 8, 1905: 7, 1906: 7, 1907: 7, 1910: 7, 1912: 7, 1922: 7, 1927: 7, 1928: 7, 2016: 8, 2020: 8, 2024: 8, 2028: 8
|
||||
170: 8, 171: 8, 189: 7, 190: 6, 193: 8, 197: 8, 199: 4, 201: 8, 209: 7, 211: 2, 241: 6, 288: 5, 289: 8, 298: 8, 304: 1, 308: 4, 309: 8, 311: 8, 313: 8, 320: 3, 328: 1, 352: 5, 381: 6, 384: 4, 386: 8, 388: 8, 389: 2, 390: 7, 417: 7, 419: 1, 426: 7, 451: 8, 452: 8, 453: 6, 454: 8, 456: 8, 479: 3, 481: 7, 485: 8, 489: 8, 493: 8, 495: 4, 497: 8, 499: 3, 500: 6, 501: 8, 508: 8, 528: 4, 532: 6, 546: 7, 550: 8, 554: 3, 558: 8, 560: 8, 562: 8, 563: 5, 564: 5, 565: 5, 566: 5, 567: 3, 568: 1, 573: 1, 577: 8, 647: 3, 707: 8, 711: 6, 715: 8, 761: 7, 810: 8, 840: 5, 842: 5, 844: 8, 866: 4, 961: 8, 969: 8, 977: 8, 979: 7, 988: 6, 989: 8, 995: 7, 1001: 8, 1005: 6, 1009: 8, 1017: 8, 1019: 2, 1020: 8, 1105: 6, 1187: 4, 1217: 8, 1221: 5, 1223: 3, 1225: 7, 1227: 4, 1233: 8, 1249: 8, 1257: 6, 1265: 8, 1267: 1, 1273: 3, 1275: 3, 1280: 4, 1300: 8, 1322: 6, 1323: 4, 1328: 4, 1417: 8, 1601: 8, 1905: 7, 1906: 7, 1907: 7, 1910: 7, 1912: 7, 1922: 7, 1927: 7, 1928: 7, 2016: 8, 2020: 8, 2024: 8, 2028: 8
|
||||
},
|
||||
# Volt Premier w/ ACC 2018
|
||||
{
|
||||
@@ -86,14 +86,11 @@ FINGERPRINTS = {
|
||||
|
||||
STEER_THRESHOLD = 1.0
|
||||
|
||||
STOCK_CONTROL_MSGS = {
|
||||
CAR.HOLDEN_ASTRA: [384, 715],
|
||||
CAR.VOLT: [384, 715], # 384 = "ASCMLKASteeringCmd", 715 = "ASCMGasRegenCmd"
|
||||
CAR.MALIBU: [384, 715], # 384 = "ASCMLKASteeringCmd", 715 = "ASCMGasRegenCmd"
|
||||
CAR.ACADIA: [384, 715], # 384 = "ASCMLKASteeringCmd", 715 = "ASCMGasRegenCmd"
|
||||
CAR.CADILLAC_ATS: [384, 715], # 384 = "ASCMLKASteeringCmd", 715 = "ASCMGasRegenCmd"
|
||||
CAR.BUICK_REGAL: [384, 715], # 384 = "ASCMLKASteeringCmd", 715 = "ASCMGasRegenCmd"
|
||||
CAR.CADILLAC_CT6: [], # CT6 does not require ASCMs to be disconnected
|
||||
class ECU:
|
||||
CAM = 0
|
||||
|
||||
ECU_FINGERPRINT = {
|
||||
ECU.CAM: [384, 715] # 384 = "ASCMLKASteeringCmd", 715 = "ASCMGasRegenCmd"
|
||||
}
|
||||
|
||||
DBC = {
|
||||
|
||||
@@ -73,7 +73,7 @@ HUDData = namedtuple("HUDData",
|
||||
"lanes", "fcw", "acc_alert", "steer_required"])
|
||||
|
||||
|
||||
class CarController(object):
|
||||
class CarController():
|
||||
def __init__(self, dbc_name):
|
||||
self.braking = False
|
||||
self.brake_steady = 0.
|
||||
|
||||
@@ -1,4 +1,5 @@
|
||||
from cereal import car
|
||||
from collections import defaultdict
|
||||
from common.numpy_fast import interp
|
||||
from common.kalman.simple_kalman import KF1D
|
||||
from selfdrive.can.can_define import CANDefine
|
||||
@@ -178,11 +179,12 @@ def get_cam_can_parser(CP):
|
||||
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)
|
||||
|
||||
class CarState(object):
|
||||
class CarState():
|
||||
def __init__(self, CP):
|
||||
self.CP = CP
|
||||
self.can_define = CANDefine(DBC[CP.carFingerprint]['pt'])
|
||||
self.shifter_values = self.can_define.dv["GEARBOX"]["GEAR_SHIFTER"]
|
||||
self.steer_status_values = defaultdict(lambda: "UNKNOWN", self.can_define.dv["STEER_STATUS"]["STEER_STATUS"])
|
||||
|
||||
self.user_gas, self.user_gas_pressed = 0., 0
|
||||
self.brake_switch_prev = 0
|
||||
@@ -224,7 +226,6 @@ class CarState(object):
|
||||
self.prev_right_blinker_on = self.right_blinker_on
|
||||
|
||||
# ******************* parse out can *******************
|
||||
|
||||
if self.CP.carFingerprint in (CAR.ACCORD, CAR.ACCORD_15, CAR.ACCORDH, CAR.CIVIC_BOSCH, CAR.CRV_HYBRID): # TODO: find wheels moving bit in dbc
|
||||
self.standstill = cp.vl["ENGINE_DATA"]['XMISSION_SPEED'] < 0.1
|
||||
self.door_all_closed = not cp.vl["SCM_FEEDBACK"]['DRIVERS_DOOR_OPEN']
|
||||
@@ -237,11 +238,13 @@ class CarState(object):
|
||||
cp.vl["DOORS_STATUS"]['DOOR_OPEN_RL'], cp.vl["DOORS_STATUS"]['DOOR_OPEN_RR']])
|
||||
self.seatbelt = not cp.vl["SEATBELT_STATUS"]['SEATBELT_DRIVER_LAMP'] and cp.vl["SEATBELT_STATUS"]['SEATBELT_DRIVER_LATCHED']
|
||||
|
||||
# 2 = temporary; 3 = TBD; 4 = significant steering wheel torque; 5 = (permanent); 6 = temporary; 7 = (permanent)
|
||||
# TODO: Use values from DBC to parse this field
|
||||
self.steer_error = cp.vl["STEER_STATUS"]['STEER_STATUS'] not in [0, 2, 3, 4, 6]
|
||||
self.steer_not_allowed = cp.vl["STEER_STATUS"]['STEER_STATUS'] not in [0, 4] # 4 can be caused by bump OR steering nudge from driver
|
||||
self.steer_warning = cp.vl["STEER_STATUS"]['STEER_STATUS'] not in [0, 3, 4] # 3 is low speed lockout, not worth a warning
|
||||
steer_status = self.steer_status_values[cp.vl["STEER_STATUS"]['STEER_STATUS']]
|
||||
self.steer_error = steer_status not in ['NORMAL', 'NO_TORQUE_ALERT_1', 'NO_TORQUE_ALERT_2', 'LOW_SPEED_LOCKOUT', 'TMP_FAULT']
|
||||
# NO_TORQUE_ALERT_2 can be caused by bump OR steering nudge from driver
|
||||
self.steer_not_allowed = steer_status not in ['NORMAL', 'NO_TORQUE_ALERT_2']
|
||||
# LOW_SPEED_LOCKOUT is not worth a warning
|
||||
self.steer_warning = steer_status not in ['NORMAL', 'LOW_SPEED_LOCKOUT', 'NO_TORQUE_ALERT_2']
|
||||
|
||||
if self.CP.radarOffCan:
|
||||
self.brake_error = 0
|
||||
else:
|
||||
@@ -357,7 +360,7 @@ if __name__ == '__main__':
|
||||
import zmq
|
||||
context = zmq.Context()
|
||||
|
||||
class CarParams(object):
|
||||
class CarParams():
|
||||
def __init__(self):
|
||||
self.carFingerprint = "HONDA CIVIC 2016 TOURING"
|
||||
self.enableGasInterceptor = 0
|
||||
|
||||
@@ -6,7 +6,6 @@ from selfdrive.car.honda.values import CAR, HONDA_BOSCH
|
||||
def can_cksum(mm):
|
||||
s = 0
|
||||
for c in mm:
|
||||
c = ord(c)
|
||||
s += (c>>4)
|
||||
s += c & 0xF
|
||||
s = 8-s
|
||||
@@ -15,19 +14,19 @@ def can_cksum(mm):
|
||||
|
||||
|
||||
def fix(msg, addr):
|
||||
msg2 = msg[0:-1] + chr(ord(msg[-1]) | can_cksum(struct.pack("I", addr)+msg))
|
||||
msg2 = msg[0:-1] + (msg[-1] | can_cksum(struct.pack("I", addr)+msg)).to_bytes(1, 'little')
|
||||
return msg2
|
||||
|
||||
|
||||
def get_pt_bus(car_fingerprint, is_panda_black):
|
||||
return 1 if car_fingerprint in HONDA_BOSCH and is_panda_black else 0
|
||||
def get_pt_bus(car_fingerprint, has_relay):
|
||||
return 1 if car_fingerprint in HONDA_BOSCH and has_relay else 0
|
||||
|
||||
|
||||
def get_lkas_cmd_bus(car_fingerprint, is_panda_black):
|
||||
return 2 if car_fingerprint in HONDA_BOSCH and not is_panda_black else 0
|
||||
def get_lkas_cmd_bus(car_fingerprint, has_relay):
|
||||
return 2 if car_fingerprint in HONDA_BOSCH and not has_relay else 0
|
||||
|
||||
|
||||
def create_brake_command(packer, apply_brake, pump_on, pcm_override, pcm_cancel_cmd, fcw, idx, car_fingerprint, is_panda_black):
|
||||
def create_brake_command(packer, apply_brake, pump_on, pcm_override, pcm_cancel_cmd, fcw, idx, car_fingerprint, has_relay):
|
||||
# TODO: do we loose pressure if we keep pump off for long?
|
||||
brakelights = apply_brake > 0
|
||||
brake_rq = apply_brake > 0
|
||||
@@ -49,23 +48,23 @@ def create_brake_command(packer, apply_brake, pump_on, pcm_override, pcm_cancel_
|
||||
"AEB_REQ_2": 0,
|
||||
"AEB_STATUS": 0,
|
||||
}
|
||||
bus = get_pt_bus(car_fingerprint, is_panda_black)
|
||||
bus = get_pt_bus(car_fingerprint, has_relay)
|
||||
return packer.make_can_msg("BRAKE_COMMAND", bus, values, idx)
|
||||
|
||||
|
||||
def create_steering_control(packer, apply_steer, lkas_active, car_fingerprint, idx, is_panda_black):
|
||||
def create_steering_control(packer, apply_steer, lkas_active, car_fingerprint, idx, has_relay):
|
||||
values = {
|
||||
"STEER_TORQUE": apply_steer if lkas_active else 0,
|
||||
"STEER_TORQUE_REQUEST": lkas_active,
|
||||
}
|
||||
bus = get_lkas_cmd_bus(car_fingerprint, is_panda_black)
|
||||
bus = get_lkas_cmd_bus(car_fingerprint, has_relay)
|
||||
return packer.make_can_msg("STEERING_CONTROL", bus, values, idx)
|
||||
|
||||
|
||||
def create_ui_commands(packer, pcm_speed, hud, car_fingerprint, is_metric, idx, is_panda_black):
|
||||
def create_ui_commands(packer, pcm_speed, hud, car_fingerprint, is_metric, idx, has_relay):
|
||||
commands = []
|
||||
bus_pt = get_pt_bus(car_fingerprint, is_panda_black)
|
||||
bus_lkas = get_lkas_cmd_bus(car_fingerprint, is_panda_black)
|
||||
bus_pt = get_pt_bus(car_fingerprint, has_relay)
|
||||
bus_lkas = get_lkas_cmd_bus(car_fingerprint, has_relay)
|
||||
|
||||
if car_fingerprint not in HONDA_BOSCH:
|
||||
acc_hud_values = {
|
||||
@@ -101,10 +100,10 @@ def create_ui_commands(packer, pcm_speed, hud, car_fingerprint, is_metric, idx,
|
||||
return commands
|
||||
|
||||
|
||||
def spam_buttons_command(packer, button_val, idx, car_fingerprint, is_panda_black):
|
||||
def spam_buttons_command(packer, button_val, idx, car_fingerprint, has_relay):
|
||||
values = {
|
||||
'CRUISE_BUTTONS': button_val,
|
||||
'CRUISE_SETTING': 0,
|
||||
}
|
||||
bus = get_pt_bus(car_fingerprint, is_panda_black)
|
||||
bus = get_pt_bus(car_fingerprint, has_relay)
|
||||
return packer.make_can_msg("SCM_BUTTONS", bus, values, idx)
|
||||
|
||||
@@ -1,5 +1,4 @@
|
||||
#!/usr/bin/env python
|
||||
import os
|
||||
#!/usr/bin/env python3
|
||||
import numpy as np
|
||||
from cereal import car
|
||||
from common.numpy_fast import clip, interp
|
||||
@@ -9,11 +8,12 @@ from selfdrive.config import Conversions as CV
|
||||
from selfdrive.controls.lib.drive_helpers import create_event, EventTypes as ET, get_events
|
||||
from selfdrive.controls.lib.vehicle_model import VehicleModel
|
||||
from selfdrive.car.honda.carstate import CarState, get_can_parser, get_cam_can_parser
|
||||
from selfdrive.car.honda.values import CruiseButtons, CAR, HONDA_BOSCH, VISUAL_HUD, CAMERA_MSGS
|
||||
from selfdrive.car import STD_CARGO_KG, CivicParams, scale_rot_inertia, scale_tire_stiffness
|
||||
from selfdrive.controls.lib.planner import _A_CRUISE_MAX_V_FOLLOWING
|
||||
from selfdrive.car.honda.values import CruiseButtons, CAR, HONDA_BOSCH, VISUAL_HUD, ECU, ECU_FINGERPRINT, FINGERPRINTS
|
||||
from selfdrive.car import STD_CARGO_KG, CivicParams, scale_rot_inertia, scale_tire_stiffness, is_ecu_disconnected, gen_empty_fingerprint
|
||||
from selfdrive.controls.lib.planner import _A_CRUISE_MAX_V
|
||||
from selfdrive.car.interfaces import CarInterfaceBase
|
||||
|
||||
A_ACC_MAX = max(_A_CRUISE_MAX_V_FOLLOWING)
|
||||
A_ACC_MAX = max(_A_CRUISE_MAX_V)
|
||||
|
||||
ButtonType = car.CarState.ButtonEvent.Type
|
||||
GearShifter = car.CarState.GearShifter
|
||||
@@ -72,7 +72,7 @@ def get_compute_gb_acura():
|
||||
return _compute_gb_acura
|
||||
|
||||
|
||||
class CarInterface(object):
|
||||
class CarInterface(CarInterfaceBase):
|
||||
def __init__(self, CP, CarController):
|
||||
self.CP = CP
|
||||
|
||||
@@ -131,27 +131,28 @@ class CarInterface(object):
|
||||
return float(max(max_accel, a_target / A_ACC_MAX)) * min(speedLimiter, accelLimiter)
|
||||
|
||||
@staticmethod
|
||||
def get_params(candidate, fingerprint, vin="", is_panda_black=False):
|
||||
def get_params(candidate, fingerprint=gen_empty_fingerprint(), vin="", has_relay=False):
|
||||
|
||||
ret = car.CarParams.new_message()
|
||||
ret.carName = "honda"
|
||||
ret.carFingerprint = candidate
|
||||
ret.carVin = vin
|
||||
ret.isPandaBlack = is_panda_black
|
||||
ret.isPandaBlack = has_relay
|
||||
|
||||
if candidate in HONDA_BOSCH:
|
||||
ret.safetyModel = car.CarParams.SafetyModel.hondaBosch
|
||||
ret.enableCamera = True
|
||||
rdr_bus = 0 if has_relay else 2
|
||||
ret.enableCamera = is_ecu_disconnected(fingerprint[rdr_bus], FINGERPRINTS, ECU_FINGERPRINT, candidate, ECU.CAM) or has_relay
|
||||
ret.radarOffCan = True
|
||||
ret.openpilotLongitudinalControl = False
|
||||
else:
|
||||
ret.safetyModel = car.CarParams.SafetyModel.honda
|
||||
ret.enableCamera = not any(x for x in CAMERA_MSGS if x in fingerprint) or is_panda_black
|
||||
ret.enableGasInterceptor = 0x201 in fingerprint
|
||||
ret.enableCamera = is_ecu_disconnected(fingerprint[0], FINGERPRINTS, ECU_FINGERPRINT, candidate, ECU.CAM) or has_relay
|
||||
ret.enableGasInterceptor = 0x201 in fingerprint[0]
|
||||
ret.openpilotLongitudinalControl = ret.enableCamera
|
||||
|
||||
cloudlog.warn("ECU Camera Simulated: %r", ret.enableCamera)
|
||||
cloudlog.warn("ECU Gas Interceptor: %r", ret.enableGasInterceptor)
|
||||
cloudlog.warning("ECU Camera Simulated: %r", ret.enableCamera)
|
||||
cloudlog.warning("ECU Gas Interceptor: %r", ret.enableGasInterceptor)
|
||||
|
||||
ret.enableCruise = not ret.enableGasInterceptor
|
||||
|
||||
@@ -172,12 +173,8 @@ class CarInterface(object):
|
||||
ret.centerToFront = CivicParams.CENTER_TO_FRONT
|
||||
ret.steerRatio = 15.38 # 10.93 is end-to-end spec
|
||||
tire_stiffness_factor = 1.
|
||||
# Civic at comma has modified steering FW, so different tuning for the Neo in that car
|
||||
is_fw_modified = os.getenv("DONGLE_ID") in ['5b7c365c50084530']
|
||||
if is_fw_modified:
|
||||
ret.lateralTuning.pid.kf = 0.00004
|
||||
|
||||
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.4], [0.12]] if is_fw_modified else [[0.8], [0.24]]
|
||||
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.kiBP = [0., 35.]
|
||||
|
||||
@@ -1,23 +1,25 @@
|
||||
#!/usr/bin/env python
|
||||
#!/usr/bin/env python3
|
||||
import os
|
||||
import time
|
||||
from cereal import car
|
||||
from selfdrive.can.parser import CANParser
|
||||
from common.realtime import DT_RDR
|
||||
from selfdrive.car.interfaces import RadarInterfaceBase
|
||||
|
||||
def _create_nidec_can_parser():
|
||||
dbc_f = 'acura_ilx_2016_nidec.dbc'
|
||||
radar_messages = [0x400] + range(0x430, 0x43A) + range(0x440, 0x446)
|
||||
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 +
|
||||
['REL_SPEED'] * 16,
|
||||
[0x400] + radar_messages[1:] * 4,
|
||||
[0] + [255] * 16 + [1] * 16 + [0] * 16 + [0] * 16))
|
||||
checks = list(zip([0x445], [20]))
|
||||
|
||||
return CANParser(os.path.splitext(dbc_f)[0], signals, checks, 1)
|
||||
fn = os.path.splitext(dbc_f)[0].encode('utf8')
|
||||
return CANParser(fn, signals, checks, 1)
|
||||
|
||||
|
||||
class RadarInterface(object):
|
||||
class RadarInterface(RadarInterfaceBase):
|
||||
def __init__(self, CP):
|
||||
# radar
|
||||
self.pts = {}
|
||||
@@ -26,7 +28,7 @@ class RadarInterface(object):
|
||||
self.radar_wrong_config = False
|
||||
self.radar_off_can = CP.radarOffCan
|
||||
|
||||
self.delay = 0.1 # Delay of radar
|
||||
self.delay = int(0.1 / DT_RDR) # 0.1s delay of radar
|
||||
|
||||
# Nidec
|
||||
self.rcp = _create_nidec_can_parser()
|
||||
@@ -55,7 +57,7 @@ class RadarInterface(object):
|
||||
def _update(self, updated_messages):
|
||||
ret = car.RadarData.new_message()
|
||||
|
||||
for ii in updated_messages:
|
||||
for ii in sorted(updated_messages):
|
||||
cpt = self.rcp.vl[ii]
|
||||
if ii == 0x400:
|
||||
# check for radar faults
|
||||
@@ -85,6 +87,6 @@ class RadarInterface(object):
|
||||
errors.append("wrongConfig")
|
||||
ret.errors = errors
|
||||
|
||||
ret.points = self.pts.values()
|
||||
ret.points = list(self.pts.values())
|
||||
|
||||
return ret
|
||||
|
||||
@@ -30,6 +30,9 @@ VISUAL_HUD = {
|
||||
VisualAlert.seatbeltUnbuckled: AH.SEATBELT,
|
||||
VisualAlert.speedTooHigh: AH.SPEED_TOO_HIGH}
|
||||
|
||||
class ECU:
|
||||
CAM = 0
|
||||
|
||||
class CAR:
|
||||
ACCORD = "HONDA ACCORD 2018 SPORT 2T"
|
||||
ACCORD_15 = "HONDA ACCORD 2018 LX 1.5T"
|
||||
@@ -127,7 +130,7 @@ FINGERPRINTS = {
|
||||
},
|
||||
# 2019 Ridgeline
|
||||
{
|
||||
57: 3, 145: 8, 229: 4, 308: 5, 316: 8, 339: 7, 342: 6, 344: 8, 380: 8, 392: 6, 399: 7, 419: 8, 420: 8, 422:8, 425: 8, 426: 8, 427: 3, 432: 7, 464: 8, 476: 4, 490: 8, 545: 5, 546: 3, 597: 8, 660: 8, 773: 7, 777: 8, 795: 8, 800: 8, 804: 8, 808: 8, 819: 7, 821: 5, 871: 8, 882: 2, 884: 7, 892: 8, 923: 2, 929: 8, 963: 8, 965: 8, 966: 8, 967: 8, 983: 8, 985: 3, 1027: 5, 1029: 8, 1036: 8, 1039: 8, 1064: 7, 1088: 8, 1089: 8, 1092: 1, 1108: 8, 1125: 8, 1296: 8, 1365: 5, 424: 5, 1613: 5, 1616: 5, 1618: 5, 1623: 5, 1668: 5
|
||||
57: 3, 145: 8, 228: 5, 229: 4, 308: 5, 316: 8, 339: 7, 342: 6, 344: 8, 380: 8, 392: 6, 399: 7, 419: 8, 420: 8, 422:8, 425: 8, 426: 8, 427: 3, 432: 7, 464: 8, 476: 4, 490: 8, 545: 5, 546: 3, 597: 8, 660: 8, 773: 7, 777: 8, 795: 8, 800: 8, 804: 8, 808: 8, 819: 7, 821: 5, 871: 8, 882: 2, 884: 7, 892: 8, 923: 2, 929: 8, 963: 8, 965: 8, 966: 8, 967: 8, 983: 8, 985: 3, 1027: 5, 1029: 8, 1036: 8, 1039: 8, 1064: 7, 1088: 8, 1089: 8, 1092: 1, 1108: 8, 1125: 8, 1296: 8, 1365: 5, 424: 5, 1613: 5, 1616: 5, 1618: 5, 1623: 5, 1668: 5
|
||||
}]
|
||||
}
|
||||
|
||||
@@ -196,7 +199,8 @@ SPEED_FACTOR = {
|
||||
|
||||
# msgs sent for steering controller by camera module on can 0.
|
||||
# those messages are mutually exclusive on CRV and non-CRV cars
|
||||
CAMERA_MSGS = [0xe4, 0x194]
|
||||
ECU_FINGERPRINT = {
|
||||
ECU.CAM: [0xE4, 0x194], # steer torque cmd
|
||||
}
|
||||
|
||||
# TODO: get these from dbc file
|
||||
HONDA_BOSCH = [CAR.ACCORD, CAR.ACCORD_15, CAR.ACCORDH, CAR.CIVIC_BOSCH, CAR.CRV_5G, CAR.CRV_HYBRID]
|
||||
|
||||
@@ -16,7 +16,7 @@ class SteerLimitParams:
|
||||
STEER_DRIVER_MULTIPLIER = 2
|
||||
STEER_DRIVER_FACTOR = 1
|
||||
|
||||
class CarController(object):
|
||||
class CarController():
|
||||
def __init__(self, dbc_name, car_fingerprint):
|
||||
self.apply_steer_last = 0
|
||||
self.car_fingerprint = car_fingerprint
|
||||
|
||||
@@ -1,8 +1,10 @@
|
||||
from cereal import car
|
||||
from selfdrive.car.hyundai.values import DBC, STEER_THRESHOLD
|
||||
from selfdrive.can.parser import CANParser
|
||||
from selfdrive.config import Conversions as CV
|
||||
from common.kalman.simple_kalman import KF1D
|
||||
|
||||
GearShifter = car.CarState.GearShifter
|
||||
|
||||
def get_can_parser(CP):
|
||||
|
||||
@@ -31,7 +33,7 @@ def get_can_parser(CP):
|
||||
("CYL_PRES", "ESP12", 0),
|
||||
|
||||
("CF_Clu_CruiseSwState", "CLU11", 0),
|
||||
("CF_Clu_CruiseSwMain" , "CLU11", 0),
|
||||
("CF_Clu_CruiseSwMain", "CLU11", 0),
|
||||
("CF_Clu_SldMainSW", "CLU11", 0),
|
||||
("CF_Clu_ParityBit1", "CLU11", 0),
|
||||
("CF_Clu_VanzDecimal" , "CLU11", 0),
|
||||
@@ -48,7 +50,7 @@ def get_can_parser(CP):
|
||||
("CF_Clu_InhibitN", "CLU15", 0),
|
||||
("CF_Clu_InhibitR", "CLU15", 0),
|
||||
|
||||
("CF_Lvr_Gear","LVR12",0),
|
||||
("CF_Lvr_Gear", "LVR12",0),
|
||||
("CUR_GR", "TCU12",0),
|
||||
|
||||
("ACCEnable", "TCS13", 0),
|
||||
@@ -122,7 +124,7 @@ def get_camera_parser(CP):
|
||||
return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 2)
|
||||
|
||||
|
||||
class CarState(object):
|
||||
class CarState():
|
||||
def __init__(self, CP):
|
||||
|
||||
self.CP = CP
|
||||
@@ -211,38 +213,38 @@ class CarState(object):
|
||||
# Gear Selecton - This is not compatible with all Kia/Hyundai's, But is the best way for those it is compatible with
|
||||
gear = cp.vl["LVR12"]["CF_Lvr_Gear"]
|
||||
if gear == 5:
|
||||
self.gear_shifter = "drive"
|
||||
self.gear_shifter = GearShifter.drive
|
||||
elif gear == 6:
|
||||
self.gear_shifter = "neutral"
|
||||
self.gear_shifter = GearShifter.neutral
|
||||
elif gear == 0:
|
||||
self.gear_shifter = "park"
|
||||
self.gear_shifter = GearShifter.park
|
||||
elif gear == 7:
|
||||
self.gear_shifter = "reverse"
|
||||
self.gear_shifter = GearShifter.reverse
|
||||
else:
|
||||
self.gear_shifter = "unknown"
|
||||
self.gear_shifter = GearShifter.unknown
|
||||
|
||||
# Gear Selection via Cluster - For those Kia/Hyundai which are not fully discovered, we can use the Cluster Indicator for Gear Selection, as this seems to be standard over all cars, but is not the preferred method.
|
||||
if cp.vl["CLU15"]["CF_Clu_InhibitD"] == 1:
|
||||
self.gear_shifter_cluster = "drive"
|
||||
self.gear_shifter_cluster = GearShifter.drive
|
||||
elif cp.vl["CLU15"]["CF_Clu_InhibitN"] == 1:
|
||||
self.gear_shifter_cluster = "neutral"
|
||||
self.gear_shifter_cluster = GearShifter.neutral
|
||||
elif cp.vl["CLU15"]["CF_Clu_InhibitP"] == 1:
|
||||
self.gear_shifter_cluster = "park"
|
||||
self.gear_shifter_cluster = GearShifter.park
|
||||
elif cp.vl["CLU15"]["CF_Clu_InhibitR"] == 1:
|
||||
self.gear_shifter_cluster = "reverse"
|
||||
self.gear_shifter_cluster = GearShifter.reverse
|
||||
else:
|
||||
self.gear_shifter_cluster = "unknown"
|
||||
self.gear_shifter_cluster = GearShifter.unknown
|
||||
|
||||
# Gear Selecton via TCU12
|
||||
gear2 = cp.vl["TCU12"]["CUR_GR"]
|
||||
if gear2 == 0:
|
||||
self.gear_tcu = "park"
|
||||
self.gear_tcu = GearShifter.park
|
||||
elif gear2 == 14:
|
||||
self.gear_tcu = "reverse"
|
||||
self.gear_tcu = GearShifter.reverse
|
||||
elif gear2 > 0 and gear2 < 9: # unaware of anything over 8 currently
|
||||
self.gear_tcu = "drive"
|
||||
self.gear_tcu = GearShifter.drive
|
||||
else:
|
||||
self.gear_tcu = "unknown"
|
||||
self.gear_tcu = GearShifter.unknown
|
||||
|
||||
# save the entire LKAS11 and CLU11
|
||||
self.lkas11 = cp_cam.vl["LKAS11"]
|
||||
|
||||
@@ -34,15 +34,13 @@ def create_lkas11(packer, car_fingerprint, apply_steer, steer_req, cnt, enabled,
|
||||
|
||||
if car_fingerprint in CHECKSUM["crc8"]:
|
||||
# CRC Checksum as seen on 2019 Hyundai Santa Fe
|
||||
dat = dat[:6] + dat[7]
|
||||
dat = dat[:6] + dat[7:8]
|
||||
checksum = hyundai_checksum(dat)
|
||||
elif car_fingerprint in CHECKSUM["6B"]:
|
||||
# Checksum of first 6 Bytes, as seen on 2018 Kia Sorento
|
||||
dat = [ord(i) for i in dat]
|
||||
checksum = sum(dat[:6]) % 256
|
||||
elif car_fingerprint in CHECKSUM["7B"]:
|
||||
# Checksum of first 6 Bytes and last Byte as seen on 2018 Kia Stinger
|
||||
dat = [ord(i) for i in dat]
|
||||
checksum = (sum(dat[:6]) + dat[7]) % 256
|
||||
|
||||
values["CF_Lkas_Chksum"] = checksum
|
||||
@@ -50,15 +48,15 @@ def create_lkas11(packer, car_fingerprint, apply_steer, steer_req, cnt, enabled,
|
||||
return packer.make_can_msg("LKAS11", 0, values)
|
||||
|
||||
def create_lkas12():
|
||||
return make_can_msg(1342, "\x00\x00\x00\x00\x60\x05", 0)
|
||||
return make_can_msg(1342, b"\x00\x00\x00\x00\x60\x05", 0)
|
||||
|
||||
|
||||
def create_1191():
|
||||
return make_can_msg(1191, "\x01\x00", 0)
|
||||
return make_can_msg(1191, b"\x01\x00", 0)
|
||||
|
||||
|
||||
def create_1156():
|
||||
return make_can_msg(1156, "\x08\x20\xfe\x3f\x00\xe0\xfd\x3f", 0)
|
||||
return make_can_msg(1156, b"\x08\x20\xfe\x3f\x00\xe0\xfd\x3f", 0)
|
||||
|
||||
def create_clu11(packer, clu11, button):
|
||||
values = {
|
||||
|
||||
@@ -1,16 +1,17 @@
|
||||
#!/usr/bin/env python
|
||||
#!/usr/bin/env python3
|
||||
from cereal import car
|
||||
from selfdrive.config import Conversions as CV
|
||||
from selfdrive.controls.lib.drive_helpers import EventTypes as ET, create_event
|
||||
from selfdrive.controls.lib.vehicle_model import VehicleModel
|
||||
from selfdrive.car.hyundai.carstate import CarState, get_can_parser, get_camera_parser
|
||||
from selfdrive.car.hyundai.values import CAMERA_MSGS, CAR, get_hud_alerts, FEATURES
|
||||
from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness
|
||||
from selfdrive.car.hyundai.values import ECU, ECU_FINGERPRINT, CAR, get_hud_alerts, FEATURES, FINGERPRINTS
|
||||
from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, is_ecu_disconnected, gen_empty_fingerprint
|
||||
from selfdrive.car.interfaces import CarInterfaceBase
|
||||
|
||||
GearShifter = car.CarState.GearShifter
|
||||
ButtonType = car.CarState.ButtonEvent.Type
|
||||
|
||||
class CarInterface(object):
|
||||
class CarInterface(CarInterfaceBase):
|
||||
def __init__(self, CP, CarController):
|
||||
self.CP = CP
|
||||
self.VM = VehicleModel(CP)
|
||||
@@ -37,18 +38,14 @@ class CarInterface(object):
|
||||
return float(accel) / 3.0
|
||||
|
||||
@staticmethod
|
||||
def calc_accel_override(a_ego, a_target, v_ego, v_target):
|
||||
return 1.0
|
||||
|
||||
@staticmethod
|
||||
def get_params(candidate, fingerprint, vin="", is_panda_black=False):
|
||||
def get_params(candidate, fingerprint=gen_empty_fingerprint(), vin="", has_relay=False):
|
||||
|
||||
ret = car.CarParams.new_message()
|
||||
|
||||
ret.carName = "hyundai"
|
||||
ret.carFingerprint = candidate
|
||||
ret.carVin = vin
|
||||
ret.isPandaBlack = is_panda_black
|
||||
ret.isPandaBlack = has_relay
|
||||
ret.radarOffCan = True
|
||||
ret.safetyModel = car.CarParams.SafetyModel.hyundai
|
||||
ret.enableCruise = True # stock acc
|
||||
@@ -143,7 +140,7 @@ class CarInterface(object):
|
||||
ret.brakeMaxBP = [0.]
|
||||
ret.brakeMaxV = [1.]
|
||||
|
||||
ret.enableCamera = not any(x for x in CAMERA_MSGS if x in fingerprint) or is_panda_black
|
||||
ret.enableCamera = is_ecu_disconnected(fingerprint[0], FINGERPRINTS, ECU_FINGERPRINT, candidate, ECU.CAM) or has_relay
|
||||
ret.openpilotLongitudinalControl = False
|
||||
|
||||
ret.steerLimitAlert = False
|
||||
|
||||
@@ -1,18 +1,5 @@
|
||||
#!/usr/bin/env python
|
||||
import os
|
||||
import time
|
||||
from cereal import car
|
||||
#!/usr/bin/env python3
|
||||
from selfdrive.car.interfaces import RadarInterfaceBase
|
||||
|
||||
class RadarInterface(object):
|
||||
def __init__(self, CP):
|
||||
# radar
|
||||
self.pts = {}
|
||||
self.delay = 0.1
|
||||
|
||||
def update(self, can_strings):
|
||||
ret = car.RadarData.new_message()
|
||||
|
||||
if 'NO_RADAR_SLEEP' not in os.environ:
|
||||
time.sleep(0.05) # radard runs on RI updates
|
||||
|
||||
return ret
|
||||
class RadarInterface(RadarInterfaceBase):
|
||||
pass
|
||||
|
||||
@@ -25,7 +25,7 @@ class Buttons:
|
||||
|
||||
FINGERPRINTS = {
|
||||
CAR.ELANTRA: [{
|
||||
66: 8, 67: 8, 68: 8, 127: 8, 273: 8, 274: 8, 275: 8, 339: 8, 356: 4, 399: 8, 512: 6, 544: 8, 593: 8, 608: 8, 688: 5, 790: 8, 809: 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, 1265: 4, 1280: 1, 1282: 4, 1287: 4, 1290: 8, 1292: 8, 1294: 8, 1312: 8, 1314: 8, 1322: 8, 1345: 8, 1349: 8, 1351: 8, 1353: 8, 1363: 8, 1366: 8, 1367: 8, 1369: 8, 1407: 8, 1415: 8, 1419: 8, 1425: 2, 1427: 6, 1440: 8, 1456: 4, 1472: 8, 1486: 8, 1487: 8, 1491: 8, 1530: 8, 1532: 5, 2001: 8, 2003: 8, 2004: 8, 2009: 8, 2012: 8, 2016: 8, 2017: 8, 2024: 8, 2025: 8
|
||||
66: 8, 67: 8, 68: 8, 127: 8, 273: 8, 274: 8, 275: 8, 339: 8, 356: 4, 399: 8, 512: 6, 544: 8, 593: 8, 608: 8, 688: 5, 790: 8, 809: 8, 897: 8, 832: 8, 899: 8, 902: 8, 903: 8, 905: 8, 909: 8, 916: 8, 1040: 8, 1056: 8, 1057: 8, 1078: 4, 1170: 8, 1265: 4, 1280: 1, 1282: 4, 1287: 4, 1290: 8, 1292: 8, 1294: 8, 1312: 8, 1314: 8, 1322: 8, 1345: 8, 1349: 8, 1351: 8, 1353: 8, 1363: 8, 1366: 8, 1367: 8, 1369: 8, 1407: 8, 1415: 8, 1419: 8, 1425: 2, 1427: 6, 1440: 8, 1456: 4, 1472: 8, 1486: 8, 1487: 8, 1491: 8, 1530: 8, 1532: 5, 2001: 8, 2003: 8, 2004: 8, 2009: 8, 2012: 8, 2016: 8, 2017: 8, 2024: 8, 2025: 8
|
||||
}],
|
||||
CAR.GENESIS: [{
|
||||
67: 8, 68: 8, 304: 8, 320: 8, 339: 8, 356: 4, 544: 7, 593: 8, 608: 8, 688: 5, 809: 8, 832: 8, 854: 7, 870: 7, 871: 8, 872: 5, 897: 8, 902: 8, 903: 6, 916: 8, 1024: 2, 1040: 8, 1056: 8, 1057: 8, 1078: 4, 1107: 5, 1136: 8, 1151: 6, 1168: 7, 1170: 8, 1173: 8, 1184: 8, 1265: 4, 1280: 1, 1287: 4, 1292: 8, 1312: 8, 1322: 8, 1331: 8, 1332: 8, 1333: 8, 1334: 8, 1335: 8, 1342: 6, 1345: 8, 1363: 8, 1369: 8, 1370: 8, 1371: 8, 1378: 4, 1384: 5, 1407: 8, 1419: 8, 1427: 6, 1434: 2, 1456: 4
|
||||
@@ -46,12 +46,17 @@ FINGERPRINTS = {
|
||||
67: 8, 127: 8, 304: 8, 320: 8, 339: 8, 356: 4, 544: 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, 916: 8, 1040: 8, 1042: 8, 1056: 8, 1057: 8, 1078: 4, 1107: 5, 1136: 8, 1151: 6, 1155: 8, 1156: 8, 1162: 8, 1164: 8, 1168: 7, 1170: 8, 1173: 8, 1183: 8, 1186: 2, 1191: 2, 1227: 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, 1379: 8, 1384: 8, 1407: 8, 1414: 3, 1419: 8, 1427: 6, 1456: 4, 1470: 8
|
||||
},
|
||||
{
|
||||
67: 8, 127: 8, 304: 8, 320: 8, 339: 8, 356: 4, 544: 8, 593: 8, 608: 8, 688: 6, 764: 8, 809: 8, 854: 7, 870: 7, 871: 8, 872: 8, 897: 8, 902: 8, 903: 8, 905: 8, 909: 8, 916: 8, 1040: 8, 1042: 8, 1056: 8, 1057: 8, 1064: 8, 1078: 4, 1107: 5, 1136: 8, 1151: 6, 1155: 8, 1162: 8, 1164: 8, 1168: 7, 1170: 8, 1173: 8, 1180: 8, 1183: 8, 1186: 2, 1227: 8, 1265: 4, 1280: 1, 1287: 4, 1290: 8, 1292: 8, 1294: 8, 1312: 8, 1322: 8, 1345: 8, 1348: 8, 1363: 8, 1369: 8, 1371: 8, 1378: 8, 1384: 8, 1407: 8, 1414: 3, 1419: 8, 1427: 6, 1456: 4, 1470: 8, 1988: 8, 2000: 8, 2004: 8, 2008: 8, 2012: 8
|
||||
67: 8, 127: 8, 304: 8, 320: 8, 339: 8, 356: 4, 544: 8, 593: 8, 608: 8, 688: 6, 764: 8, 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, 1042: 8, 1056: 8, 1057: 8, 1064: 8, 1078: 4, 1107: 5, 1136: 8, 1151: 6, 1155: 8, 1162: 8, 1164: 8, 1168: 7, 1170: 8, 1173: 8, 1180: 8, 1183: 8, 1186: 2, 1227: 8, 1265: 4, 1280: 1, 1287: 4, 1290: 8, 1292: 8, 1294: 8, 1312: 8, 1322: 8, 1345: 8, 1348: 8, 1363: 8, 1369: 8, 1371: 8, 1378: 8, 1384: 8, 1407: 8, 1414: 3, 1419: 8, 1427: 6, 1456: 4, 1470: 8, 1988: 8, 2000: 8, 2004: 8, 2008: 8, 2012: 8
|
||||
}
|
||||
],
|
||||
}
|
||||
|
||||
CAMERA_MSGS = [832, 1156, 1191, 1342]
|
||||
class ECU:
|
||||
CAM = 0
|
||||
|
||||
ECU_FINGERPRINT = {
|
||||
ECU.CAM: [832, 1156, 1191, 1342]
|
||||
}
|
||||
|
||||
CHECKSUM = {
|
||||
"crc8": [CAR.SANTA_FE],
|
||||
|
||||
@@ -0,0 +1,39 @@
|
||||
import os
|
||||
import time
|
||||
from cereal import car
|
||||
from selfdrive.car import gen_empty_fingerprint
|
||||
|
||||
# generic car and radar interfaces
|
||||
|
||||
class CarInterfaceBase():
|
||||
def __init__(self, CP, CarController):
|
||||
pass
|
||||
|
||||
@staticmethod
|
||||
def calc_accel_override(a_ego, a_target, v_ego, v_target):
|
||||
return 1.
|
||||
|
||||
@staticmethod
|
||||
def get_params(candidate, fingerprint=gen_empty_fingerprint(), vin="", has_relay=False):
|
||||
raise NotImplementedError
|
||||
|
||||
# returns a car.CarState, pass in car.CarControl
|
||||
def update(self, c, can_strings):
|
||||
raise NotImplementedError
|
||||
|
||||
# return sendcan, pass in a car.CarControl
|
||||
def apply(self, c):
|
||||
raise NotImplementedError
|
||||
|
||||
class RadarInterfaceBase():
|
||||
def __init__(self, CP):
|
||||
self.pts = {}
|
||||
self.delay = 0
|
||||
|
||||
def update(self, can_strings):
|
||||
ret = car.RadarData.new_message()
|
||||
|
||||
if 'NO_RADAR_SLEEP' not in os.environ:
|
||||
time.sleep(0.05) # radard runs on RI updates
|
||||
|
||||
return ret
|
||||
@@ -1,9 +1,11 @@
|
||||
#!/usr/bin/env python
|
||||
#!/usr/bin/env python3
|
||||
from cereal import car
|
||||
from selfdrive.config import Conversions as CV
|
||||
from selfdrive.services import service_list
|
||||
from selfdrive.swaglog import cloudlog
|
||||
import selfdrive.messaging as messaging
|
||||
from selfdrive.car import gen_empty_fingerprint
|
||||
from selfdrive.car.interfaces import CarInterfaceBase
|
||||
|
||||
# mocked car interface to work with chffrplus
|
||||
TS = 0.01 # 100Hz
|
||||
@@ -12,7 +14,7 @@ YAW_FR = 0.2 # ~0.8s time constant on yaw rate filter
|
||||
LPG = 2 * 3.1415 * YAW_FR * TS / (1 + 2 * 3.1415 * YAW_FR * TS)
|
||||
|
||||
|
||||
class CarInterface(object):
|
||||
class CarInterface(CarInterfaceBase):
|
||||
def __init__(self, CP, CarController):
|
||||
|
||||
self.CP = CP
|
||||
@@ -34,11 +36,7 @@ class CarInterface(object):
|
||||
return accel
|
||||
|
||||
@staticmethod
|
||||
def calc_accel_override(a_ego, a_target, v_ego, v_target):
|
||||
return 1.0
|
||||
|
||||
@staticmethod
|
||||
def get_params(candidate, fingerprint, vin="", is_panda_black=False):
|
||||
def get_params(candidate, fingerprint=gen_empty_fingerprint(), vin="", has_relay=False):
|
||||
|
||||
ret = car.CarParams.new_message()
|
||||
|
||||
|
||||
@@ -1,15 +1,5 @@
|
||||
#!/usr/bin/env python
|
||||
from cereal import car
|
||||
import time
|
||||
#!/usr/bin/env python3
|
||||
from selfdrive.car.interfaces import RadarInterfaceBase
|
||||
|
||||
|
||||
class RadarInterface(object):
|
||||
def __init__(self, CP):
|
||||
# radar
|
||||
self.pts = {}
|
||||
self.delay = 0.1
|
||||
|
||||
def update(self, can_strings):
|
||||
ret = car.RadarData.new_message()
|
||||
time.sleep(0.05) # radard runs on RI updates
|
||||
return ret
|
||||
class RadarInterface(RadarInterfaceBase):
|
||||
pass
|
||||
|
||||
@@ -18,7 +18,7 @@ class CarControllerParams():
|
||||
|
||||
|
||||
|
||||
class CarController(object):
|
||||
class CarController():
|
||||
def __init__(self, car_fingerprint):
|
||||
self.lkas_active = False
|
||||
self.steer_idx = 0
|
||||
|
||||
@@ -82,7 +82,7 @@ def get_camera_can_parser(CP):
|
||||
return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 2)
|
||||
|
||||
|
||||
class CarState(object):
|
||||
class CarState():
|
||||
def __init__(self, CP):
|
||||
# initialize can parser
|
||||
self.CP = CP
|
||||
|
||||
@@ -1,15 +1,16 @@
|
||||
#!/usr/bin/env python
|
||||
#!/usr/bin/env python3
|
||||
from cereal import car
|
||||
from selfdrive.config import Conversions as CV
|
||||
from selfdrive.controls.lib.drive_helpers import create_event, EventTypes as ET
|
||||
from selfdrive.controls.lib.vehicle_model import VehicleModel
|
||||
from selfdrive.car.subaru.values import CAR
|
||||
from selfdrive.car.subaru.values import CAR, FINGERPRINTS, ECU_FINGERPRINT, ECU
|
||||
from selfdrive.car.subaru.carstate import CarState, get_powertrain_can_parser, get_camera_can_parser
|
||||
from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness
|
||||
from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint, is_ecu_disconnected
|
||||
from selfdrive.car.interfaces import CarInterfaceBase
|
||||
|
||||
ButtonType = car.CarState.ButtonEvent.Type
|
||||
|
||||
class CarInterface(object):
|
||||
class CarInterface(CarInterfaceBase):
|
||||
def __init__(self, CP, CarController):
|
||||
self.CP = CP
|
||||
|
||||
@@ -34,24 +35,20 @@ class CarInterface(object):
|
||||
return float(accel) / 4.0
|
||||
|
||||
@staticmethod
|
||||
def calc_accel_override(a_ego, a_target, v_ego, v_target):
|
||||
return 1.0
|
||||
|
||||
@staticmethod
|
||||
def get_params(candidate, fingerprint, vin="", is_panda_black=False):
|
||||
def get_params(candidate, fingerprint=gen_empty_fingerprint(), vin="", has_relay=False):
|
||||
ret = car.CarParams.new_message()
|
||||
|
||||
ret.carName = "subaru"
|
||||
ret.radarOffCan = True
|
||||
ret.carFingerprint = candidate
|
||||
ret.carVin = vin
|
||||
ret.isPandaBlack = is_panda_black
|
||||
ret.isPandaBlack = has_relay
|
||||
ret.safetyModel = car.CarParams.SafetyModel.subaru
|
||||
|
||||
ret.enableCruise = True
|
||||
ret.steerLimitAlert = True
|
||||
|
||||
ret.enableCamera = True
|
||||
ret.enableCamera = is_ecu_disconnected(fingerprint[0], FINGERPRINTS, ECU_FINGERPRINT, candidate, ECU.CAM) or has_relay
|
||||
|
||||
ret.steerRateCost = 0.7
|
||||
|
||||
|
||||
@@ -1,18 +1,5 @@
|
||||
#!/usr/bin/env python
|
||||
import os
|
||||
import time
|
||||
from cereal import car
|
||||
#!/usr/bin/env python3
|
||||
from selfdrive.car.interfaces import RadarInterfaceBase
|
||||
|
||||
class RadarInterface(object):
|
||||
def __init__(self, CP):
|
||||
# radar
|
||||
self.pts = {}
|
||||
self.delay = 0.1
|
||||
|
||||
def update(self, can_strings):
|
||||
ret = car.RadarData.new_message()
|
||||
|
||||
if 'NO_RADAR_SLEEP' not in os.environ:
|
||||
time.sleep(0.05) # radard runs on RI updates
|
||||
|
||||
return ret
|
||||
class RadarInterface(RadarInterfaceBase):
|
||||
pass
|
||||
|
||||
@@ -6,7 +6,6 @@ VisualAlert = car.CarControl.HUDControl.VisualAlert
|
||||
|
||||
def subaru_checksum(packer, values, addr):
|
||||
dat = packer.make_can_msg(addr, 0, values)[2]
|
||||
dat = [ord(i) for i in dat]
|
||||
return (sum(dat[1:]) + (addr >> 8) + addr) & 0xff
|
||||
|
||||
def create_steering_control(packer, car_fingerprint, apply_steer, frame, steer_step):
|
||||
|
||||
@@ -17,6 +17,13 @@ STEER_THRESHOLD = {
|
||||
CAR.IMPREZA: 80,
|
||||
}
|
||||
|
||||
class ECU:
|
||||
CAM = 0
|
||||
|
||||
ECU_FINGERPRINT = {
|
||||
ECU.CAM: [290, 356], # steer torque cmd
|
||||
}
|
||||
|
||||
DBC = {
|
||||
CAR.IMPREZA: dbc_dict('subaru_global_2017', None),
|
||||
}
|
||||
|
||||
@@ -89,7 +89,7 @@ def ipas_state_transition(steer_angle_enabled, enabled, ipas_active, ipas_reset_
|
||||
return False, 0
|
||||
|
||||
|
||||
class CarController(object):
|
||||
class CarController():
|
||||
def __init__(self, dbc_name, car_fingerprint, enable_camera, enable_dsu, enable_apg):
|
||||
self.braking = False
|
||||
# redundant safety check with the board
|
||||
@@ -257,14 +257,14 @@ class CarController(object):
|
||||
# special cases
|
||||
if fr_step == 5 and ecu == ECU.CAM and bus == 1:
|
||||
cnt = (((frame // 5) % 7) + 1) << 5
|
||||
vl = chr(cnt) + vl
|
||||
vl = bytes([cnt]) + vl
|
||||
elif addr in (0x489, 0x48a) and bus == 0:
|
||||
# add counter for those 2 messages (last 4 bits)
|
||||
cnt = ((frame // 100) % 0xf) + 1
|
||||
if addr == 0x48a:
|
||||
# 0x48a has a 8 preceding the counter
|
||||
cnt += 1 << 7
|
||||
vl += chr(cnt)
|
||||
vl += bytes([cnt])
|
||||
|
||||
can_sends.append(make_can_msg(addr, vl, bus, False))
|
||||
|
||||
|
||||
@@ -1,5 +1,5 @@
|
||||
import numpy as np
|
||||
from cereal import car
|
||||
from common.numpy_fast import mean
|
||||
from common.kalman.simple_kalman import KF1D
|
||||
from selfdrive.can.can_define import CANDefine
|
||||
from selfdrive.can.parser import CANParser
|
||||
@@ -15,7 +15,7 @@ def parse_gear_shifter(gear, vals):
|
||||
try:
|
||||
return val_to_capnp[vals[gear]]
|
||||
except KeyError:
|
||||
return "unknown"
|
||||
return GearShifter.unknown
|
||||
|
||||
|
||||
def get_can_parser(CP):
|
||||
@@ -110,7 +110,7 @@ def get_cam_can_parser(CP):
|
||||
return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 2)
|
||||
|
||||
|
||||
class CarState(object):
|
||||
class CarState():
|
||||
def __init__(self, CP):
|
||||
|
||||
self.CP = CP
|
||||
@@ -158,7 +158,7 @@ class CarState(object):
|
||||
self.v_wheel_fr = cp.vl["WHEEL_SPEEDS"]['WHEEL_SPEED_FR'] * CV.KPH_TO_MS
|
||||
self.v_wheel_rl = cp.vl["WHEEL_SPEEDS"]['WHEEL_SPEED_RL'] * CV.KPH_TO_MS
|
||||
self.v_wheel_rr = cp.vl["WHEEL_SPEEDS"]['WHEEL_SPEED_RR'] * CV.KPH_TO_MS
|
||||
v_wheel = float(np.mean([self.v_wheel_fl, self.v_wheel_fr, self.v_wheel_rl, self.v_wheel_rr]))
|
||||
v_wheel = mean([self.v_wheel_fl, self.v_wheel_fr, self.v_wheel_rl, self.v_wheel_rr])
|
||||
|
||||
# Kalman filter
|
||||
if abs(v_wheel - self.v_ego) > 2.0: # Prevent large accelerations when car starts at non zero speed
|
||||
|
||||
@@ -1,17 +1,18 @@
|
||||
#!/usr/bin/env python
|
||||
#!/usr/bin/env python3
|
||||
from cereal import car
|
||||
from selfdrive.config import Conversions as CV
|
||||
from selfdrive.controls.lib.drive_helpers import EventTypes as ET, create_event
|
||||
from selfdrive.controls.lib.vehicle_model import VehicleModel
|
||||
from selfdrive.car.toyota.carstate import CarState, get_can_parser, get_cam_can_parser
|
||||
from selfdrive.car.toyota.values import ECU, check_ecu_msgs, CAR, NO_STOP_TIMER_CAR, TSS2_CAR
|
||||
from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness
|
||||
from selfdrive.car.toyota.values import ECU, ECU_FINGERPRINT, CAR, NO_STOP_TIMER_CAR, TSS2_CAR, FINGERPRINTS
|
||||
from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, is_ecu_disconnected, gen_empty_fingerprint
|
||||
from selfdrive.swaglog import cloudlog
|
||||
from selfdrive.car.interfaces import CarInterfaceBase
|
||||
|
||||
ButtonType = car.CarState.ButtonEvent.Type
|
||||
GearShifter = car.CarState.GearShifter
|
||||
|
||||
class CarInterface(object):
|
||||
class CarInterface(CarInterfaceBase):
|
||||
def __init__(self, CP, CarController):
|
||||
self.CP = CP
|
||||
self.VM = VehicleModel(CP)
|
||||
@@ -38,18 +39,14 @@ class CarInterface(object):
|
||||
return float(accel) / 3.0
|
||||
|
||||
@staticmethod
|
||||
def calc_accel_override(a_ego, a_target, v_ego, v_target):
|
||||
return 1.0
|
||||
|
||||
@staticmethod
|
||||
def get_params(candidate, fingerprint, vin="", is_panda_black=False):
|
||||
def get_params(candidate, fingerprint=gen_empty_fingerprint(), vin="", has_relay=False):
|
||||
|
||||
ret = car.CarParams.new_message()
|
||||
|
||||
ret.carName = "toyota"
|
||||
ret.carFingerprint = candidate
|
||||
ret.carVin = vin
|
||||
ret.isPandaBlack = is_panda_black
|
||||
ret.isPandaBlack = has_relay
|
||||
|
||||
ret.safetyModel = car.CarParams.SafetyModel.toyota
|
||||
|
||||
@@ -219,6 +216,16 @@ class CarInterface(object):
|
||||
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.3], [0.05]]
|
||||
ret.lateralTuning.pid.kf = 0.00006
|
||||
|
||||
elif candidate == CAR.LEXUS_CTH:
|
||||
stop_and_go = True
|
||||
ret.safetyParam = 100
|
||||
ret.wheelbase = 2.60
|
||||
ret.steerRatio = 18.6
|
||||
tire_stiffness_factor = 0.517
|
||||
ret.mass = 3108 * CV.LB_TO_KG + STD_CARGO_KG # mean between min and max
|
||||
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.3], [0.05]]
|
||||
ret.lateralTuning.pid.kf = 0.00007
|
||||
|
||||
elif candidate == CAR.LEXUS_ISH:
|
||||
stop_and_go = True # set to true because it's a hybrid
|
||||
ret.safetyParam = 66
|
||||
@@ -229,26 +236,9 @@ class CarInterface(object):
|
||||
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.3], [0.05]]
|
||||
ret.lateralTuning.pid.kf = 0.00006
|
||||
|
||||
elif candidate == CAR.LEXUS_CTH:
|
||||
stop_and_go = True
|
||||
ret.safetyParam = 100 # see conversion factor for STEER_TORQUE_EPS in dbc file
|
||||
ret.wheelbase = 2.60
|
||||
ret.steerRatio = 18.6 # 0.5.10
|
||||
tire_stiffness_factor = 0.517 # 0.5.10
|
||||
ret.mass = 3108 * CV.LB_TO_KG + STD_CARGO_KG # mean between min and max
|
||||
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.2], [0.05]]
|
||||
ret.lateralTuning.pid.kf = 0.00007 # full torque for 10 deg at 80mph means 0.00007818594
|
||||
|
||||
ret.steerRateCost = 1.
|
||||
ret.centerToFront = ret.wheelbase * 0.44
|
||||
|
||||
#detect the Pedal address
|
||||
ret.enableGasInterceptor = 0x201 in fingerprint
|
||||
|
||||
# min speed to enable ACC. if car can do stop and go, then set enabling speed
|
||||
# to a negative value, so it won't matter.
|
||||
ret.minEnableSpeed = -1. if (stop_and_go or ret.enableGasInterceptor) else 19. * CV.MPH_TO_MS
|
||||
|
||||
# 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)
|
||||
@@ -268,14 +258,19 @@ class CarInterface(object):
|
||||
ret.brakeMaxBP = [0.]
|
||||
ret.brakeMaxV = [1.]
|
||||
|
||||
ret.enableCamera = not check_ecu_msgs(fingerprint, ECU.CAM, candidate) or is_panda_black
|
||||
ret.enableDsu = not check_ecu_msgs(fingerprint, ECU.DSU, candidate) or (is_panda_black and candidate in TSS2_CAR)
|
||||
ret.enableApgs = False #not check_ecu_msgs(fingerprint, ECU.APGS)
|
||||
ret.enableCamera = is_ecu_disconnected(fingerprint[0], FINGERPRINTS, ECU_FINGERPRINT, candidate, ECU.CAM) or has_relay
|
||||
ret.enableDsu = is_ecu_disconnected(fingerprint[0], FINGERPRINTS, ECU_FINGERPRINT, candidate, ECU.DSU) or (has_relay and candidate in TSS2_CAR)
|
||||
ret.enableApgs = False # is_ecu_disconnected(fingerprint[0], FINGERPRINTS, ECU_FINGERPRINT, candidate, ECU.APGS)
|
||||
ret.enableGasInterceptor = 0x201 in fingerprint[0]
|
||||
ret.openpilotLongitudinalControl = ret.enableCamera and ret.enableDsu
|
||||
cloudlog.warn("ECU Camera Simulated: %r", ret.enableCamera)
|
||||
cloudlog.warn("ECU DSU Simulated: %r", ret.enableDsu)
|
||||
cloudlog.warn("ECU APGS Simulated: %r", ret.enableApgs)
|
||||
cloudlog.warn("ECU Gas Interceptor: %r", ret.enableGasInterceptor)
|
||||
cloudlog.warning("ECU Camera Simulated: %r", ret.enableCamera)
|
||||
cloudlog.warning("ECU DSU Simulated: %r", ret.enableDsu)
|
||||
cloudlog.warning("ECU APGS Simulated: %r", ret.enableApgs)
|
||||
cloudlog.warning("ECU Gas Interceptor: %r", ret.enableGasInterceptor)
|
||||
|
||||
# min speed to enable ACC. if car can do stop and go, then set enabling speed
|
||||
# to a negative value, so it won't matter.
|
||||
ret.minEnableSpeed = -1. if (stop_and_go or ret.enableGasInterceptor) else 19. * CV.MPH_TO_MS
|
||||
|
||||
ret.steerLimitAlert = False
|
||||
|
||||
@@ -303,10 +298,7 @@ class CarInterface(object):
|
||||
def update(self, c, can_strings):
|
||||
# ******************* do can recv *******************
|
||||
self.cp.update_strings(can_strings)
|
||||
|
||||
# run the cam can update for 10s as we just need to know if the camera is alive
|
||||
if self.frame < 1000:
|
||||
self.cp_cam.update_strings(can_strings)
|
||||
self.cp_cam.update_strings(can_strings)
|
||||
|
||||
self.CS.update(self.cp)
|
||||
|
||||
@@ -391,6 +383,8 @@ class CarInterface(object):
|
||||
if self.cp_cam.can_valid:
|
||||
self.forwarding_camera = True
|
||||
|
||||
if self.cp_cam.can_invalid_cnt >= 100 and self.CP.enableCamera:
|
||||
events.append(create_event('invalidGiraffeToyota', [ET.PERMANENT]))
|
||||
if not ret.gearShifter == GearShifter.drive and self.CP.enableDsu:
|
||||
events.append(create_event('wrongGear', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
|
||||
if ret.doorOpen:
|
||||
|
||||
@@ -1,9 +1,10 @@
|
||||
#!/usr/bin/env python
|
||||
#!/usr/bin/env python3
|
||||
import os
|
||||
import time
|
||||
from selfdrive.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']
|
||||
@@ -18,22 +19,22 @@ def _create_radar_can_parser(car_fingerprint):
|
||||
msg_a_n = len(RADAR_A_MSGS)
|
||||
msg_b_n = len(RADAR_B_MSGS)
|
||||
|
||||
signals = zip(['LONG_DIST'] * msg_a_n + ['NEW_TRACK'] * msg_a_n + ['LAT_DIST'] * msg_a_n +
|
||||
signals = list(zip(['LONG_DIST'] * msg_a_n + ['NEW_TRACK'] * msg_a_n + ['LAT_DIST'] * msg_a_n +
|
||||
['REL_SPEED'] * msg_a_n + ['VALID'] * msg_a_n + ['SCORE'] * msg_b_n,
|
||||
RADAR_A_MSGS * 5 + RADAR_B_MSGS,
|
||||
[255] * msg_a_n + [1] * msg_a_n + [0] * msg_a_n + [0] * msg_a_n + [0] * msg_a_n + [0] * msg_b_n)
|
||||
[255] * msg_a_n + [1] * msg_a_n + [0] * msg_a_n + [0] * msg_a_n + [0] * msg_a_n + [0] * msg_b_n))
|
||||
|
||||
checks = zip(RADAR_A_MSGS + RADAR_B_MSGS, [20]*(msg_a_n + msg_b_n))
|
||||
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)
|
||||
|
||||
class RadarInterface(object):
|
||||
class RadarInterface(RadarInterfaceBase):
|
||||
def __init__(self, CP):
|
||||
# radar
|
||||
self.pts = {}
|
||||
self.track_id = 0
|
||||
|
||||
self.delay = 0.0 # Delay of radar
|
||||
self.delay = 0 # Delay of radar
|
||||
|
||||
if CP.carFingerprint in TSS2_CAR:
|
||||
self.RADAR_A_MSGS = list(range(0x180, 0x190))
|
||||
@@ -75,7 +76,7 @@ class RadarInterface(object):
|
||||
errors.append("canError")
|
||||
ret.errors = errors
|
||||
|
||||
for ii in updated_messages:
|
||||
for ii in sorted(updated_messages):
|
||||
if ii in self.RADAR_A_MSGS:
|
||||
cpt = self.rcp.vl[ii]
|
||||
|
||||
@@ -105,5 +106,5 @@ class RadarInterface(object):
|
||||
if ii in self.pts:
|
||||
del self.pts[ii]
|
||||
|
||||
ret.points = self.pts.values()
|
||||
ret.points = list(self.pts.values())
|
||||
return ret
|
||||
|
||||
@@ -10,7 +10,7 @@ def fix(msg, addr):
|
||||
|
||||
checksum = idh + idl + len(msg) + 1
|
||||
for d_byte in msg:
|
||||
checksum += ord(d_byte)
|
||||
checksum += d_byte
|
||||
|
||||
#return msg + chr(checksum & 0xFF)
|
||||
return msg + struct.pack("B", checksum & 0xFF)
|
||||
|
||||
@@ -18,8 +18,8 @@ class CAR:
|
||||
LEXUS_ESH_TSS2 = "LEXUS ES 300H 2019"
|
||||
SIENNA = "TOYOTA SIENNA XLE 2018"
|
||||
LEXUS_IS = "LEXUS IS300 2018"
|
||||
LEXUS_ISH = "LEXUS IS300h 2017"
|
||||
LEXUS_CTH = "LEXUS CT 200H 2018"
|
||||
LEXUS_ISH = "LEXUS IS300h 2017"
|
||||
|
||||
|
||||
class ECU:
|
||||
@@ -30,62 +30,56 @@ class ECU:
|
||||
|
||||
# addr: (ecu, cars, bus, 1/freq*100, vl)
|
||||
STATIC_MSGS = [
|
||||
(0x130, ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.LEXUS_CTH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON), 1, 100, '\x00\x00\x00\x00\x00\x00\x38'),
|
||||
(0x240, ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.LEXUS_CTH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON), 1, 5, '\x00\x10\x01\x00\x10\x01\x00'),
|
||||
(0x241, ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.LEXUS_CTH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON), 1, 5, '\x00\x10\x01\x00\x10\x01\x00'),
|
||||
(0x244, ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.LEXUS_CTH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON), 1, 5, '\x00\x10\x01\x00\x10\x01\x00'),
|
||||
(0x245, ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.LEXUS_CTH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON), 1, 5, '\x00\x10\x01\x00\x10\x01\x00'),
|
||||
(0x248, ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.LEXUS_CTH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON), 1, 5, '\x00\x00\x00\x00\x00\x00\x01'),
|
||||
(0x367, ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.LEXUS_CTH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON), 0, 40, '\x06\x00'),
|
||||
(0x414, ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.LEXUS_CTH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON), 0, 100, '\x00\x00\x00\x00\x00\x00\x17\x00'),
|
||||
(0x466, ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.LEXUS_CTH, CAR.RAV4, CAR.HIGHLANDER, CAR.HIGHLANDERH), 1, 100, '\x20\x20\xAD'),
|
||||
(0x466, ECU.CAM, (CAR.COROLLA, CAR.AVALON), 1, 100, '\x24\x20\xB1'),
|
||||
(0x489, ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.LEXUS_CTH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON), 0, 100, '\x00\x00\x00\x00\x00\x00\x00'),
|
||||
(0x48a, ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.LEXUS_CTH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON), 0, 100, '\x00\x00\x00\x00\x00\x00\x00'),
|
||||
(0x48b, ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.LEXUS_CTH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON), 0, 100, '\x66\x06\x08\x0a\x02\x00\x00\x00'),
|
||||
(0x4d3, ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.LEXUS_CTH, CAR.RAV4, CAR.COROLLA, CAR.AVALON), 0, 100, '\x1C\x00\x00\x01\x00\x00\x00\x00'),
|
||||
(0x130, ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON), 1, 100, b'\x00\x00\x00\x00\x00\x00\x38'),
|
||||
(0x240, ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON), 1, 5, b'\x00\x10\x01\x00\x10\x01\x00'),
|
||||
(0x241, ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON), 1, 5, b'\x00\x10\x01\x00\x10\x01\x00'),
|
||||
(0x244, ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON), 1, 5, b'\x00\x10\x01\x00\x10\x01\x00'),
|
||||
(0x245, ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON), 1, 5, b'\x00\x10\x01\x00\x10\x01\x00'),
|
||||
(0x248, ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON), 1, 5, b'\x00\x00\x00\x00\x00\x00\x01'),
|
||||
(0x367, ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON), 0, 40, b'\x06\x00'),
|
||||
(0x414, ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON), 0, 100, b'\x00\x00\x00\x00\x00\x00\x17\x00'),
|
||||
(0x466, ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.HIGHLANDER, CAR.HIGHLANDERH), 1, 100, b'\x20\x20\xAD'),
|
||||
(0x466, ECU.CAM, (CAR.COROLLA, CAR.AVALON), 1, 100, b'\x24\x20\xB1'),
|
||||
(0x489, ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON), 0, 100, b'\x00\x00\x00\x00\x00\x00\x00'),
|
||||
(0x48a, ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON), 0, 100, b'\x00\x00\x00\x00\x00\x00\x00'),
|
||||
(0x48b, ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON), 0, 100, b'\x66\x06\x08\x0a\x02\x00\x00\x00'),
|
||||
(0x4d3, ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.AVALON), 0, 100, b'\x1C\x00\x00\x01\x00\x00\x00\x00'),
|
||||
|
||||
(0x128, ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.LEXUS_CTH, CAR.RAV4, CAR.COROLLA, CAR.AVALON), 1, 3, '\xf4\x01\x90\x83\x00\x37'),
|
||||
(0x128, ECU.DSU, (CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.SIENNA), 1, 3, '\x03\x00\x20\x00\x00\x52'),
|
||||
(0x141, ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.LEXUS_CTH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON, CAR.SIENNA), 1, 2, '\x00\x00\x00\x46'),
|
||||
(0x160, ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.LEXUS_CTH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON, CAR.SIENNA), 1, 7, '\x00\x00\x08\x12\x01\x31\x9c\x51'),
|
||||
(0x161, ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.LEXUS_CTH, CAR.RAV4, CAR.COROLLA, CAR.AVALON), 1, 7, '\x00\x1e\x00\x00\x00\x80\x07'),
|
||||
(0X161, ECU.DSU, (CAR.HIGHLANDERH, CAR.HIGHLANDER, CAR.SIENNA), 1, 7, '\x00\x1e\x00\xd4\x00\x00\x5b'),
|
||||
(0x283, ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.LEXUS_CTH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON, CAR.SIENNA), 0, 3, '\x00\x00\x00\x00\x00\x00\x8c'),
|
||||
(0x2E6, ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.LEXUS_CTH), 0, 3, '\xff\xf8\x00\x08\x7f\xe0\x00\x4e'),
|
||||
(0x2E7, ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.LEXUS_CTH), 0, 3, '\xa8\x9c\x31\x9c\x00\x00\x00\x02'),
|
||||
(0x33E, ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.LEXUS_CTH), 0, 20, '\x0f\xff\x26\x40\x00\x1f\x00'),
|
||||
(0x344, ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.LEXUS_CTH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON, CAR.SIENNA), 0, 5, '\x00\x00\x01\x00\x00\x00\x00\x50'),
|
||||
(0x365, ECU.DSU, (CAR.PRIUS, CAR.LEXUS_RXH, CAR.LEXUS_CTH, CAR.HIGHLANDERH), 0, 20, '\x00\x00\x00\x80\x03\x00\x08'),
|
||||
(0x365, ECU.DSU, (CAR.RAV4, CAR.RAV4H, CAR.COROLLA, CAR.HIGHLANDER, CAR.AVALON, CAR.SIENNA), 0, 20, '\x00\x00\x00\x80\xfc\x00\x08'),
|
||||
(0x366, ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.LEXUS_CTH, CAR.HIGHLANDERH), 0, 20, '\x00\x00\x4d\x82\x40\x02\x00'),
|
||||
(0x366, ECU.DSU, (CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.AVALON, CAR.SIENNA), 0, 20, '\x00\x72\x07\xff\x09\xfe\x00'),
|
||||
(0x470, ECU.DSU, (CAR.PRIUS, CAR.LEXUS_RXH, CAR.LEXUS_CTH), 1, 100, '\x00\x00\x02\x7a'),
|
||||
(0x470, ECU.DSU, (CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.RAV4H, CAR.SIENNA), 1, 100, '\x00\x00\x01\x79'),
|
||||
(0x4CB, ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.LEXUS_CTH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDERH, CAR.HIGHLANDER, CAR.AVALON, CAR.SIENNA), 0, 100, '\x0c\x00\x00\x00\x00\x00\x00\x00'),
|
||||
(0x128, ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.AVALON), 1, 3, b'\xf4\x01\x90\x83\x00\x37'),
|
||||
(0x128, ECU.DSU, (CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.SIENNA, CAR.LEXUS_CTH), 1, 3, b'\x03\x00\x20\x00\x00\x52'),
|
||||
(0x141, ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON, CAR.SIENNA, CAR.LEXUS_CTH), 1, 2, b'\x00\x00\x00\x46'),
|
||||
(0x160, ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON, CAR.SIENNA, CAR.LEXUS_CTH), 1, 7, b'\x00\x00\x08\x12\x01\x31\x9c\x51'),
|
||||
(0x161, ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.AVALON), 1, 7, b'\x00\x1e\x00\x00\x00\x80\x07'),
|
||||
(0X161, ECU.DSU, (CAR.HIGHLANDERH, CAR.HIGHLANDER, CAR.SIENNA, CAR.LEXUS_CTH), 1, 7, b'\x00\x1e\x00\xd4\x00\x00\x5b'),
|
||||
(0x283, ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON, CAR.SIENNA, CAR.LEXUS_CTH), 0, 3, b'\x00\x00\x00\x00\x00\x00\x8c'),
|
||||
(0x2E6, ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH), 0, 3, b'\xff\xf8\x00\x08\x7f\xe0\x00\x4e'),
|
||||
(0x2E7, ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH), 0, 3, b'\xa8\x9c\x31\x9c\x00\x00\x00\x02'),
|
||||
(0x33E, ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH), 0, 20, b'\x0f\xff\x26\x40\x00\x1f\x00'),
|
||||
(0x344, ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON, CAR.SIENNA, CAR.LEXUS_CTH), 0, 5, b'\x00\x00\x01\x00\x00\x00\x00\x50'),
|
||||
(0x365, ECU.DSU, (CAR.PRIUS, CAR.LEXUS_RXH, CAR.HIGHLANDERH), 0, 20, b'\x00\x00\x00\x80\x03\x00\x08'),
|
||||
(0x365, ECU.DSU, (CAR.RAV4, CAR.RAV4H, CAR.COROLLA, CAR.HIGHLANDER, CAR.AVALON, CAR.SIENNA, CAR.LEXUS_CTH), 0, 20, b'\x00\x00\x00\x80\xfc\x00\x08'),
|
||||
(0x366, ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.HIGHLANDERH), 0, 20, b'\x00\x00\x4d\x82\x40\x02\x00'),
|
||||
(0x366, ECU.DSU, (CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.AVALON, CAR.SIENNA, CAR.LEXUS_CTH), 0, 20, b'\x00\x72\x07\xff\x09\xfe\x00'),
|
||||
(0x470, ECU.DSU, (CAR.PRIUS, CAR.LEXUS_RXH), 1, 100, b'\x00\x00\x02\x7a'),
|
||||
(0x470, ECU.DSU, (CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.RAV4H, CAR.SIENNA, CAR.LEXUS_CTH), 1, 100, b'\x00\x00\x01\x79'),
|
||||
(0x4CB, ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDERH, CAR.HIGHLANDER, CAR.AVALON, CAR.SIENNA, CAR.LEXUS_CTH), 0, 100, b'\x0c\x00\x00\x00\x00\x00\x00\x00'),
|
||||
|
||||
(0x292, ECU.APGS, (CAR.PRIUS), 0, 3, '\x00\x00\x00\x00\x00\x00\x00\x9e'),
|
||||
(0x32E, ECU.APGS, (CAR.PRIUS), 0, 20, '\x00\x00\x00\x00\x00\x00\x00\x00'),
|
||||
(0x396, ECU.APGS, (CAR.PRIUS), 0, 100, '\xBD\x00\x00\x00\x60\x0F\x02\x00'),
|
||||
(0x43A, ECU.APGS, (CAR.PRIUS), 0, 100, '\x84\x00\x00\x00\x00\x00\x00\x00'),
|
||||
(0x43B, ECU.APGS, (CAR.PRIUS), 0, 100, '\x00\x00\x00\x00\x00\x00\x00\x00'),
|
||||
(0x497, ECU.APGS, (CAR.PRIUS), 0, 100, '\x00\x00\x00\x00\x00\x00\x00\x00'),
|
||||
(0x4CC, ECU.APGS, (CAR.PRIUS), 0, 100, '\x0D\x00\x00\x00\x00\x00\x00\x00'),
|
||||
(0x292, ECU.APGS, (CAR.PRIUS), 0, 3, b'\x00\x00\x00\x00\x00\x00\x00\x9e'),
|
||||
(0x32E, ECU.APGS, (CAR.PRIUS), 0, 20, b'\x00\x00\x00\x00\x00\x00\x00\x00'),
|
||||
(0x396, ECU.APGS, (CAR.PRIUS), 0, 100, b'\xBD\x00\x00\x00\x60\x0F\x02\x00'),
|
||||
(0x43A, ECU.APGS, (CAR.PRIUS), 0, 100, b'\x84\x00\x00\x00\x00\x00\x00\x00'),
|
||||
(0x43B, ECU.APGS, (CAR.PRIUS), 0, 100, b'\x00\x00\x00\x00\x00\x00\x00\x00'),
|
||||
(0x497, ECU.APGS, (CAR.PRIUS), 0, 100, b'\x00\x00\x00\x00\x00\x00\x00\x00'),
|
||||
(0x4CC, ECU.APGS, (CAR.PRIUS), 0, 100, b'\x0D\x00\x00\x00\x00\x00\x00\x00'),
|
||||
]
|
||||
|
||||
ECU_FINGERPRINT = {
|
||||
ECU.CAM: 0x2e4, # steer torque cmd
|
||||
ECU.DSU: 0x343, # accel cmd
|
||||
ECU.APGS: 0x835, # angle cmd
|
||||
ECU.CAM: [0x2e4], # steer torque cmd
|
||||
ECU.DSU: [0x343], # accel cmd
|
||||
ECU.APGS: [0x835], # angle cmd
|
||||
}
|
||||
|
||||
|
||||
def check_ecu_msgs(fingerprint, ecu, car):
|
||||
# return True if the reference car fingerprint doesn't contain the ecu fingerprint msg or
|
||||
# fingerprint contains messages normally sent by a given ecu
|
||||
return any(ECU_FINGERPRINT[ecu] not in finger for finger in FINGERPRINTS[car]) or ECU_FINGERPRINT[ecu] in fingerprint
|
||||
|
||||
|
||||
FINGERPRINTS = {
|
||||
CAR.RAV4: [{
|
||||
36: 8, 37: 8, 170: 8, 180: 8, 186: 4, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 512: 6, 513: 6, 547: 8, 548: 8, 552: 4, 562: 4, 608: 8, 610: 5, 643: 7, 705: 8, 725: 2, 740: 5, 800: 8, 835: 8, 836: 8, 849: 4, 869: 7, 870: 7, 871: 2, 896: 8, 897: 8, 900: 6, 902: 6, 905: 8, 911: 8, 916: 3, 918: 7, 921: 8, 933: 8, 944: 8, 945: 8, 951: 8, 955: 4, 956: 8, 979: 2, 998: 5, 999: 7, 1000: 8, 1001: 8, 1005: 2, 1008: 2, 1014: 8, 1017: 8, 1041: 8, 1042: 8, 1043: 8, 1044: 8, 1056: 8, 1059: 1, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1176: 8, 1177: 8, 1178: 8, 1179: 8, 1180: 8, 1181: 8, 1190: 8, 1191: 8, 1192: 8, 1196: 8, 1227: 8, 1228: 8, 1235: 8, 1237: 8, 1263: 8, 1264: 8, 1279: 8, 1408: 8, 1409: 8, 1410: 8, 1552: 8, 1553: 8, 1554: 8, 1555: 8, 1556: 8, 1557: 8, 1561: 8, 1562: 8, 1568: 8, 1569: 8, 1570: 8, 1571: 8, 1572: 8, 1584: 8, 1589: 8, 1592: 8, 1593: 8, 1595: 8, 1596: 8, 1597: 8, 1600: 8, 1656: 8, 1664: 8, 1728: 8, 1745: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8, 2015: 8, 2024: 8
|
||||
@@ -159,7 +153,7 @@ FINGERPRINTS = {
|
||||
36: 8, 37: 8, 166: 8, 170: 8, 180: 8, 295: 8, 296: 8, 426: 6, 452: 8, 466: 8, 467: 8, 550: 8, 552: 4, 560: 7, 562: 6, 581: 5, 608: 8, 610: 8, 643: 7, 658: 8, 713: 8, 728: 8, 740: 5, 761: 8, 764: 8, 800: 8, 810: 2, 812: 8, 814: 8, 818: 8, 824: 8, 829: 2, 830: 7, 835: 8, 836: 8, 869: 7, 870: 7, 871: 2, 888: 8, 889: 8, 898: 8, 900: 6, 902: 6, 905: 8, 918: 8, 921: 8, 933: 8, 934: 8, 935: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 8, 955: 8, 956: 8, 971: 7, 975: 5, 983: 8, 984: 8, 993: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1002: 8, 1011: 8, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1057: 8, 1059: 1, 1071: 8, 1076: 8, 1077: 8, 1082: 8, 1084: 8, 1085: 8, 1086: 8, 1114: 8, 1132: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1228: 8, 1235: 8, 1237: 8, 1264: 8, 1279: 8, 1541: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1595: 8, 1745: 8, 1779: 8, 1786: 8, 1787: 8, 1788: 8, 1789: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8
|
||||
}],
|
||||
CAR.HIGHLANDER: [{
|
||||
36: 8, 37: 8, 114: 5, 119: 6, 120: 4, 170: 8, 180: 8, 186: 4, 238: 4, 355: 5, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 544: 4, 545: 5, 550: 8, 552: 4, 608: 8, 610: 5, 643: 7, 705: 8, 725: 2, 740: 5, 800: 8, 835: 8, 836: 8, 849: 4, 869: 7, 870: 7, 871: 2, 896: 8, 900: 6, 902: 6, 905: 8, 911: 8, 916: 3, 921: 8, 922: 8, 933: 8, 944: 8, 945: 8, 951: 8, 955: 8, 956: 8, 979: 2, 998: 5, 999: 7, 1000: 8, 1001: 8, 1008: 2, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1043: 8, 1044: 8, 1056: 8, 1059: 1, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1176: 8, 1177: 8, 1178: 8, 1179: 8, 1180: 8, 1181: 8, 1182: 8, 1183: 8, 1189: 8, 1190: 8, 1191: 8, 1192: 8, 1196: 8, 1197: 8, 1198: 8, 1199: 8, 1206: 8, 1207: 8, 1212: 8, 1227: 8, 1235: 8, 1237: 8, 1279: 8, 1408: 8, 1409: 8, 1410: 8, 1552: 8, 1553: 8, 1554: 8, 1556: 8, 1557: 8, 1561: 8, 1562: 8, 1568: 8, 1569: 8, 1570: 8, 1571: 8, 1572: 8, 1584: 8, 1589: 8, 1592: 8, 1593: 8, 1595: 8, 1599: 8, 1656: 8, 1666: 8, 1667: 8, 1728: 8, 1745: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8
|
||||
36: 8, 37: 8, 114: 5, 119: 6, 120: 4, 170: 8, 180: 8, 186: 4, 238: 4, 355: 5, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 544: 4, 545: 5, 550: 8, 552: 4, 608: 8, 610: 5, 643: 7, 705: 8, 725: 2, 740: 5, 800: 8, 835: 8, 836: 8, 849: 4, 869: 7, 870: 7, 871: 2, 896: 8, 900: 6, 902: 6, 905: 8, 911: 8, 916: 3, 921: 8, 922: 8, 933: 8, 944: 8, 945: 8, 951: 8, 955: 8, 956: 8, 979: 2, 998: 5, 999: 7, 1000: 8, 1001: 8, 1008: 2, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1043: 8, 1044: 8, 1056: 8, 1059: 1, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1176: 8, 1177: 8, 1178: 8, 1179: 8, 1180: 8, 1181: 8, 1182: 8, 1183: 8, 1189: 8, 1190: 8, 1191: 8, 1192: 8, 1196: 8, 1197: 8, 1198: 8, 1199: 8, 1206: 8, 1207: 8, 1212: 8, 1227: 8, 1235: 8, 1237: 8, 1279: 8, 1408: 8, 1409: 8, 1410: 8, 1552: 8, 1553: 8, 1554: 8, 1556: 8, 1557: 8, 1561: 8, 1562: 8, 1568: 8, 1569: 8, 1570: 8, 1571: 8, 1572: 8, 1584: 8, 1589: 8, 1592: 8, 1593: 8, 1595: 8, 1599: 8, 1656: 8, 1666: 8, 1667: 8, 1728: 8, 1745: 8, 1779: 8, 1872: 8, 1880: 8, 1904: 8, 1912: 8, 1984: 8, 1988: 8, 1992: 8, 1996: 8, 1990: 8, 1998: 8
|
||||
},
|
||||
# 2019 Highlander XLE
|
||||
{
|
||||
@@ -171,6 +165,9 @@ FINGERPRINTS = {
|
||||
}],
|
||||
CAR.HIGHLANDERH: [{
|
||||
36: 8, 37: 8, 170: 8, 180: 8, 296: 8, 426: 6, 452: 8, 466: 8, 467: 8, 550: 8, 552: 4, 560: 7, 581: 5, 608: 8, 610: 5, 643: 7, 713: 8, 740: 5, 800: 8, 835: 8, 836: 8, 849: 4, 869: 7, 870: 7, 871: 2, 896: 8, 897: 8, 900: 6, 902: 6, 905: 8, 911: 8, 916: 3, 918: 7, 921: 8, 933: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 3, 955: 8, 956: 8, 979: 2, 998: 5, 999: 7, 1000: 8, 1001: 8, 1005: 2, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1043: 8, 1044: 8, 1056: 8, 1059: 1, 1112: 8, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1176: 8, 1177: 8, 1178: 8, 1179: 8, 1180: 8, 1181: 8, 1184: 8, 1185: 8, 1186: 8, 1189: 8, 1190: 8, 1191: 8, 1192: 8, 1196: 8, 1197: 8, 1198: 8, 1199: 8, 1206: 8, 1212: 8, 1227: 8, 1232: 8, 1235: 8, 1237: 8, 1263: 8, 1264: 8, 1279: 8, 1552: 8, 1553: 8, 1554: 8, 1556: 8, 1557: 8, 1561: 8, 1562: 8, 1568: 8, 1569: 8, 1570: 8, 1571: 8, 1572: 8, 1584: 8, 1589: 8, 1592: 8, 1593: 8, 1595: 8, 1599: 8, 1656: 8, 1728: 8, 1745: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8
|
||||
},
|
||||
{
|
||||
36: 8, 37: 8, 170: 8, 180: 8, 296: 8, 426: 6, 452: 8, 466: 8, 467: 8, 550: 8, 552: 4, 560: 7, 581: 5, 608: 8, 610: 5, 643: 7, 713: 8, 740: 5, 800: 8, 835: 8, 836: 8, 849: 4, 869: 7, 870: 7, 871: 2, 896: 8, 897: 8, 900: 6, 902: 6, 905: 8, 911: 8, 916: 3, 921: 8, 933: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 3, 955: 8, 956: 8, 979: 2, 992: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1043: 8, 1044: 8, 1056: 8, 1057: 8, 1059: 1, 1076: 8, 1077: 8, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1176: 8, 1177: 8, 1178: 8, 1179: 8, 1180: 8, 1181: 8, 1184: 8, 1185: 8, 1186: 8, 1189: 8, 1190: 8, 1191: 8, 1192: 8, 1196: 8, 1197: 8, 1198: 8, 1199: 8, 1206: 8, 1212: 8, 1227: 8, 1232: 8, 1237: 8, 1279: 8, 1552: 8, 1553: 8, 1554: 8, 1556: 8, 1557: 8, 1561: 8, 1562: 8, 1568: 8, 1569: 8, 1570: 8, 1571: 8, 1572: 8, 1584: 8, 1589: 8, 1592: 8, 1593: 8, 1595: 8, 1599: 8, 1656: 8, 1728: 8, 1745: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8
|
||||
}],
|
||||
CAR.AVALON: [{
|
||||
36: 8, 37: 8, 170: 8, 180: 8, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 547: 8, 550: 8, 552: 4, 562: 6, 608: 8, 610: 5, 643: 7, 705: 8, 740: 5, 800: 8, 835: 8, 836: 8, 849: 4, 869: 7, 870: 7, 871: 2, 896: 8, 897: 8, 905: 8, 911: 1, 916: 2, 921: 8, 933: 6, 944: 8, 945: 8, 951: 8, 955: 8, 956: 8, 979: 2, 1005: 2, 1014: 8, 1017: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1059: 1, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1176: 8, 1177: 8, 1178: 8, 1179: 8, 1180: 8, 1181: 8, 1190: 8, 1191: 8, 1192: 8, 1196: 8, 1200: 8, 1201: 8, 1202: 8, 1203: 8, 1206: 8, 1227: 8, 1235: 8, 1237: 8, 1264: 8, 1279: 8, 1552: 8, 1553: 8, 1554: 8, 1555: 8, 1556: 8, 1557: 8, 1558: 8, 1561: 8, 1562: 8, 1568: 8, 1569: 8, 1570: 8, 1571: 8, 1572: 8, 1584: 8, 1589: 8, 1592: 8, 1593: 8, 1595: 8, 1596: 8, 1597: 8, 1664: 8, 1728: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8
|
||||
@@ -223,9 +220,8 @@ FINGERPRINTS = {
|
||||
36: 8, 37: 8, 170: 8, 180: 8, 288: 8, 426: 6, 452: 8, 466: 8, 467: 8, 548: 8, 552: 4, 560: 7, 581: 5, 608: 8, 610: 5, 643: 7, 713: 8, 740: 5, 800: 8, 810: 2, 832: 8, 835: 8, 836: 8, 849: 4, 869: 7, 870: 7, 871: 2, 900: 6, 902: 6, 905: 8, 911: 8, 916: 1, 918: 7, 921: 8, 933: 8, 944: 6, 945: 8, 950: 8, 951: 8, 953: 3, 955: 4, 956: 8, 979: 2, 992: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1017: 8, 1041: 8, 1042: 8, 1043: 8, 1056: 8, 1057: 8, 1059: 1, 1076: 8, 1077: 8, 1112: 8, 1114: 8, 1116: 8, 1160: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1176: 8, 1177: 8, 1178: 8, 1179: 8, 1180: 8, 1181: 8, 1184: 8, 1185: 8, 1186: 8, 1190: 8, 1191: 8, 1192: 8, 1227: 8, 1235: 8, 1279: 8, 1552: 8, 1553: 8, 1554: 8, 1555: 8, 1556: 8, 1557: 8, 1558: 8, 1561: 8, 1562: 8, 1568: 8, 1569: 8, 1570: 8, 1571: 8, 1572: 8, 1575: 8, 1584: 8, 1589: 8, 1592: 8, 1593: 8, 1595: 8, 1664: 8, 1728: 8, 1779: 8
|
||||
},
|
||||
{
|
||||
# CT200h FP from thomaspich
|
||||
36: 8, 37: 8, 170: 8, 180: 8, 288: 8, 426: 6, 452: 8, 466: 8, 467: 8, 548: 8, 552: 4, 560: 7, 581: 5, 608: 8, 610: 5, 643: 7, 713: 8, 740: 5, 800: 8, 810: 2, 832: 8, 835: 8, 836: 8, 849: 4, 869: 7, 870: 7, 871: 2, 897: 8, 900: 6, 902: 6, 905: 8, 911: 8, 916: 1, 921: 8, 933: 8, 944: 6, 945: 8, 950: 8, 951: 8, 953: 3, 955: 4, 956: 8, 979: 2, 992: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1017: 8, 1041: 8, 1042: 8, 1043: 8, 1056: 8, 1057: 8, 1059: 1, 1076: 8, 1077: 8, 1114: 8, 1116: 8, 1160: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1176: 8, 1177: 8, 1178: 8, 1179: 8, 1180: 8, 1181: 8, 1184: 8, 1185: 8, 1186: 8, 1190: 8, 1191: 8, 1192: 8, 1227: 8, 1235: 8, 1279: 8, 1552: 8, 1553: 8, 1554: 8, 1555: 8, 1556: 8, 1557: 8, 1558: 8, 1561: 8, 1562: 8, 1568: 8, 1569: 8, 1570: 8, 1571: 8, 1572: 8, 1575: 8, 1584: 8, 1589: 8, 1592: 8, 1593: 8, 1595: 8, 1664: 8, 1728: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8
|
||||
}],
|
||||
}]
|
||||
}
|
||||
|
||||
STEER_THRESHOLD = 100
|
||||
@@ -248,8 +244,8 @@ DBC = {
|
||||
CAR.LEXUS_ESH_TSS2: dbc_dict('toyota_nodsu_hybrid_pt_generated', 'toyota_tss2_adas'),
|
||||
CAR.SIENNA: dbc_dict('toyota_sienna_xle_2018_pt_generated', 'toyota_adas'),
|
||||
CAR.LEXUS_IS: dbc_dict('lexus_is_2018_pt_generated', 'toyota_adas'),
|
||||
CAR.LEXUS_ISH: dbc_dict('lexus_is_2018_pt_generated', 'toyota_adas'),
|
||||
CAR.LEXUS_CTH: dbc_dict('lexus_ct200h_2018_pt_generated', 'toyota_adas'),
|
||||
CAR.LEXUS_ISH: dbc_dict('lexus_is_2018_pt_generated', 'toyota_adas'),
|
||||
}
|
||||
|
||||
NO_DSU_CAR = [CAR.CHR, CAR.CHRH, CAR.CAMRY, CAR.CAMRYH, CAR.RAV4_TSS2, CAR.COROLLA_TSS2, CAR.LEXUS_ESH_TSS2]
|
||||
|
||||
Executable
+98
@@ -0,0 +1,98 @@
|
||||
#!/usr/bin/env python3
|
||||
import selfdrive.messaging as messaging
|
||||
from selfdrive.boardd.boardd import can_list_to_can_capnp
|
||||
|
||||
VIN_UNKNOWN = "0" * 17
|
||||
|
||||
# sanity checks on response messages from vin query
|
||||
def is_vin_response_valid(can_dat, step, cnt):
|
||||
if len(can_dat) != 8:
|
||||
# ISO-TP meesages are all 8 bytes
|
||||
return False
|
||||
|
||||
if step == 0:
|
||||
# VIN does not fit in a single message and it's 20 bytes of data
|
||||
if can_dat[0] != 0x10 or can_dat[1] != 0x14:
|
||||
return False
|
||||
|
||||
if step == 1 and cnt == 0:
|
||||
# first response after a CONTINUE query is sent
|
||||
if can_dat[0] != 0x21:
|
||||
return False
|
||||
|
||||
if step == 1 and cnt == 1:
|
||||
# second response after a CONTINUE query is sent
|
||||
if can_dat[0] != 0x22:
|
||||
return False
|
||||
|
||||
return True
|
||||
|
||||
|
||||
class VinQuery():
|
||||
def __init__(self, bus):
|
||||
self.bus = bus
|
||||
# works on standard 11-bit addresses for diagnostic. Tested on Toyota and Subaru;
|
||||
# Honda uses the extended 29-bit addresses, and unfortunately only works from OBDII
|
||||
self.query_ext_msgs = [[0x18DB33F1, 0, b'\x02\x09\x02'.ljust(8, b"\x00"), bus],
|
||||
[0x18DA10f1, 0, b'\x30'.ljust(8, b"\x00"), bus]]
|
||||
self.query_nor_msgs = [[0x7df, 0, b'\x02\x09\x02'.ljust(8, b"\x00"), bus],
|
||||
[0x7e0, 0, b'\x30'.ljust(8, b"\x00"), bus]]
|
||||
|
||||
self.cnts = [1, 2] # number of messages to wait for at each iteration
|
||||
self.step = 0
|
||||
self.cnt = 0
|
||||
self.responded = False
|
||||
self.never_responded = True
|
||||
self.dat = b""
|
||||
self.vin = VIN_UNKNOWN
|
||||
|
||||
def check_response(self, msg):
|
||||
# have we got a VIN query response?
|
||||
if msg.src == self.bus and msg.address in [0x18daf110, 0x7e8]:
|
||||
self.never_responded = False
|
||||
# basic sanity checks on ISO-TP response
|
||||
if is_vin_response_valid(msg.dat, self.step, self.cnt):
|
||||
self.dat += bytes(msg.dat[2:]) if self.step == 0 else bytes(msg.dat[1:])
|
||||
self.cnt += 1
|
||||
if self.cnt == self.cnts[self.step]:
|
||||
self.responded = True
|
||||
self.step += 1
|
||||
|
||||
def send_query(self, sendcan):
|
||||
# keep sending VIN qury if ECU isn't responsing.
|
||||
# sendcan is probably not ready due to the zmq slow joiner syndrome
|
||||
if self.never_responded or (self.responded and self.step < len(self.cnts)):
|
||||
sendcan.send(can_list_to_can_capnp([self.query_ext_msgs[self.step]], msgtype='sendcan'))
|
||||
sendcan.send(can_list_to_can_capnp([self.query_nor_msgs[self.step]], msgtype='sendcan'))
|
||||
self.responded = False
|
||||
self.cnt = 0
|
||||
|
||||
def get_vin(self):
|
||||
# only report vin if procedure is finished
|
||||
if self.step == len(self.cnts) and self.cnt == self.cnts[-1]:
|
||||
self.vin = self.dat[3:].decode('utf8')
|
||||
return self.vin
|
||||
|
||||
|
||||
def get_vin(logcan, sendcan, bus, query_time=1.):
|
||||
vin_query = VinQuery(bus)
|
||||
frame = 0
|
||||
|
||||
# 1s max of VIN query time
|
||||
while frame < query_time * 100:
|
||||
a = messaging.recv_one(logcan)
|
||||
|
||||
for can in a.can:
|
||||
vin_query.check_response(can)
|
||||
|
||||
vin_query.send_query(sendcan)
|
||||
frame += 1
|
||||
|
||||
return vin_query.get_vin()
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
from selfdrive.services import service_list
|
||||
logcan = messaging.sub_sock(service_list['can'].port)
|
||||
sendcan = messaging.pub_sock(service_list['sendcan'].port)
|
||||
print(get_vin(logcan, sendcan, 0))
|
||||
@@ -16,6 +16,7 @@
|
||||
#include "common/util.h"
|
||||
#include "common/utilpp.h"
|
||||
|
||||
|
||||
namespace {
|
||||
|
||||
template <typename T>
|
||||
@@ -28,8 +29,42 @@ static const char* default_params_path = null_coalesce(
|
||||
|
||||
} // namespace
|
||||
|
||||
static int fsync_dir(const char* path){
|
||||
int result = 0;
|
||||
int fd = open(path, O_RDONLY);
|
||||
|
||||
if (fd < 0){
|
||||
result = -1;
|
||||
goto cleanup;
|
||||
}
|
||||
|
||||
result = fsync(fd);
|
||||
if (result < 0) {
|
||||
goto cleanup;
|
||||
}
|
||||
|
||||
cleanup:
|
||||
int result_close = 0;
|
||||
if (fd >= 0){
|
||||
result_close = close(fd);
|
||||
}
|
||||
|
||||
if (result_close < 0) {
|
||||
return result_close;
|
||||
} else {
|
||||
return result;
|
||||
}
|
||||
}
|
||||
|
||||
int write_db_value(const char* params_path, const char* key, const char* value,
|
||||
size_t value_size) {
|
||||
// Information about safely and atomically writing a file: https://lwn.net/Articles/457667/
|
||||
// 1) Create temp file
|
||||
// 2) Write data to temp file
|
||||
// 3) fsync() the temp file
|
||||
// 4) rename the temp file to the real name
|
||||
// 5) fsync() the containing directory
|
||||
|
||||
int lock_fd = -1;
|
||||
int tmp_fd = -1;
|
||||
int result;
|
||||
@@ -55,12 +90,14 @@ int write_db_value(const char* params_path, const char* key, const char* value,
|
||||
goto cleanup;
|
||||
}
|
||||
|
||||
// Build lock path
|
||||
result = snprintf(path, sizeof(path), "%s/.lock", params_path);
|
||||
if (result < 0) {
|
||||
goto cleanup;
|
||||
}
|
||||
lock_fd = open(path, 0);
|
||||
|
||||
// Build key path
|
||||
result = snprintf(path, sizeof(path), "%s/d/%s", params_path, key);
|
||||
if (result < 0) {
|
||||
goto cleanup;
|
||||
@@ -80,6 +117,20 @@ int write_db_value(const char* params_path, const char* key, const char* value,
|
||||
|
||||
// Move temp into place.
|
||||
result = rename(tmp_path, path);
|
||||
if (result < 0) {
|
||||
goto cleanup;
|
||||
}
|
||||
|
||||
// fsync parent directory
|
||||
result = snprintf(path, sizeof(path), "%s/d", params_path);
|
||||
if (result < 0) {
|
||||
goto cleanup;
|
||||
}
|
||||
|
||||
result = fsync_dir(path);
|
||||
if (result < 0) {
|
||||
goto cleanup;
|
||||
}
|
||||
|
||||
cleanup:
|
||||
// Release lock.
|
||||
@@ -95,6 +146,60 @@ cleanup:
|
||||
return result;
|
||||
}
|
||||
|
||||
int delete_db_value(const char* params_path, const char* key) {
|
||||
int lock_fd = -1;
|
||||
int result;
|
||||
char path[1024];
|
||||
|
||||
if (params_path == NULL) {
|
||||
params_path = default_params_path;
|
||||
}
|
||||
|
||||
// Build lock path, and open lockfile
|
||||
result = snprintf(path, sizeof(path), "%s/.lock", params_path);
|
||||
if (result < 0) {
|
||||
goto cleanup;
|
||||
}
|
||||
lock_fd = open(path, 0);
|
||||
|
||||
// Take lock.
|
||||
result = flock(lock_fd, LOCK_EX);
|
||||
if (result < 0) {
|
||||
goto cleanup;
|
||||
}
|
||||
|
||||
// Build key path
|
||||
result = snprintf(path, sizeof(path), "%s/d/%s", params_path, key);
|
||||
if (result < 0) {
|
||||
goto cleanup;
|
||||
}
|
||||
|
||||
// Delete value.
|
||||
result = remove(path);
|
||||
if (result != 0) {
|
||||
result = ERR_NO_VALUE;
|
||||
goto cleanup;
|
||||
}
|
||||
|
||||
// fsync parent directory
|
||||
result = snprintf(path, sizeof(path), "%s/d", params_path);
|
||||
if (result < 0) {
|
||||
goto cleanup;
|
||||
}
|
||||
|
||||
result = fsync_dir(path);
|
||||
if (result < 0) {
|
||||
goto cleanup;
|
||||
}
|
||||
|
||||
cleanup:
|
||||
// Release lock.
|
||||
if (lock_fd >= 0) {
|
||||
close(lock_fd);
|
||||
}
|
||||
return result;
|
||||
}
|
||||
|
||||
int read_db_value(const char* params_path, const char* key, char** value,
|
||||
size_t* value_sz) {
|
||||
int lock_fd = -1;
|
||||
|
||||
@@ -7,6 +7,8 @@
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
#define ERR_NO_VALUE -33
|
||||
|
||||
int write_db_value(const char* params_path, const char* key, const char* value,
|
||||
size_t value_size);
|
||||
|
||||
@@ -23,6 +25,10 @@ int write_db_value(const char* params_path, const char* key, const char* value,
|
||||
int read_db_value(const char* params_path, const char* key, char** value,
|
||||
size_t* value_sz);
|
||||
|
||||
// Delete a value from the params database.
|
||||
// Inputs are the same as read_db_value, without value and value_sz.
|
||||
int delete_db_value(const char* params_path, const char* key);
|
||||
|
||||
// Reads a value from the params database, blocking until successful.
|
||||
// Inputs are the same as read_db_value.
|
||||
void read_db_value_blocking(const char* params_path, const char* key,
|
||||
|
||||
@@ -4,6 +4,7 @@
|
||||
#include <math.h>
|
||||
#include <unistd.h>
|
||||
#include <assert.h>
|
||||
#include <ctype.h>
|
||||
|
||||
#include <GLES3/gl3.h>
|
||||
#include <EGL/egl.h>
|
||||
@@ -17,6 +18,8 @@
|
||||
#include "framebuffer.h"
|
||||
#include "spinner.h"
|
||||
|
||||
#define SPINTEXT_LENGTH 128
|
||||
|
||||
// external resources linked in
|
||||
extern const unsigned char _binary_opensans_semibold_ttf_start[];
|
||||
extern const unsigned char _binary_opensans_semibold_ttf_end[];
|
||||
@@ -27,14 +30,30 @@ extern const unsigned char _binary_img_spinner_track_png_end[];
|
||||
extern const unsigned char _binary_img_spinner_comma_png_start[];
|
||||
extern const unsigned char _binary_img_spinner_comma_png_end[];
|
||||
|
||||
bool stdin_input_available() {
|
||||
struct timeval timeout;
|
||||
timeout.tv_sec = 0;
|
||||
timeout.tv_usec = 0;
|
||||
|
||||
fd_set fds;
|
||||
FD_ZERO(&fds);
|
||||
FD_SET(STDIN_FILENO, &fds);
|
||||
select(STDIN_FILENO+1, &fds, NULL, NULL, &timeout);
|
||||
return (FD_ISSET(0, &fds));
|
||||
}
|
||||
|
||||
int spin(int argc, char** argv) {
|
||||
int err;
|
||||
|
||||
const char* spintext = NULL;
|
||||
if (argc == 1) {
|
||||
spintext = argv[0];
|
||||
} else if (argc >= 2) {
|
||||
spintext = argv[1];
|
||||
bool draw_progress = false;
|
||||
float progress_val = 0.0;
|
||||
|
||||
char spintext[SPINTEXT_LENGTH];
|
||||
spintext[0] = 0;
|
||||
|
||||
const char* spintext_arg = NULL;
|
||||
if (argc >= 2) {
|
||||
strncpy(spintext, argv[1], SPINTEXT_LENGTH);
|
||||
}
|
||||
|
||||
// spinner
|
||||
@@ -62,6 +81,30 @@ int spin(int argc, char** argv) {
|
||||
assert(spinner_comma_img >= 0);
|
||||
|
||||
for (int cnt = 0; ; cnt++) {
|
||||
// Check stdin for new text
|
||||
if (stdin_input_available()){
|
||||
fgets(spintext, SPINTEXT_LENGTH, stdin);
|
||||
spintext[strcspn(spintext, "\n")] = 0;
|
||||
|
||||
// Check if number (update progress bar)
|
||||
size_t len = strlen(spintext);
|
||||
bool is_number = len > 0;
|
||||
for (int i = 0; i < len; i++){
|
||||
if (!isdigit(spintext[i])){
|
||||
is_number = false;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
if (is_number) {
|
||||
progress_val = (float)(atoi(spintext)) / 100.0;
|
||||
progress_val = fmin(1.0, progress_val);
|
||||
progress_val = fmax(0.0, progress_val);
|
||||
}
|
||||
|
||||
draw_progress = is_number;
|
||||
}
|
||||
|
||||
glClearColor(0.1, 0.1, 0.1, 1.0);
|
||||
glClear(GL_STENCIL_BUFFER_BIT | GL_COLOR_BUFFER_BIT);
|
||||
glEnable(GL_BLEND);
|
||||
@@ -97,8 +140,36 @@ int spin(int argc, char** argv) {
|
||||
nvgRect(vg, spinner_img_x, spinner_img_y, spinner_img_s, spinner_img_s);
|
||||
nvgFill(vg);
|
||||
|
||||
// message
|
||||
if (spintext) {
|
||||
if (draw_progress){
|
||||
// draw progress bar
|
||||
int progress_width = 1000;
|
||||
int progress_x = fb_w/2-progress_width/2;
|
||||
int progress_y = 775;
|
||||
int progress_height = 25;
|
||||
|
||||
NVGpaint paint = nvgBoxGradient(
|
||||
vg, progress_x + 1, progress_y + 1,
|
||||
progress_width - 2, progress_height, 3, 4, nvgRGB(27, 27, 27), nvgRGB(27, 27, 27));
|
||||
nvgBeginPath(vg);
|
||||
nvgRoundedRect(vg, progress_x, progress_y, progress_width, progress_height, 12);
|
||||
nvgFillPaint(vg, paint);
|
||||
nvgFill(vg);
|
||||
|
||||
int bar_pos = ((progress_width - 2) * progress_val);
|
||||
|
||||
paint = nvgBoxGradient(
|
||||
vg, progress_x, progress_y,
|
||||
bar_pos+1.5f, progress_height-1, 3, 4,
|
||||
nvgRGB(245, 245, 245), nvgRGB(105, 105, 105));
|
||||
|
||||
nvgBeginPath(vg);
|
||||
nvgRoundedRect(
|
||||
vg, progress_x+1, progress_y+1,
|
||||
bar_pos, progress_height-2, 12);
|
||||
nvgFillPaint(vg, paint);
|
||||
nvgFill(vg);
|
||||
} else {
|
||||
// message
|
||||
nvgTextAlign(vg, NVG_ALIGN_CENTER | NVG_ALIGN_TOP);
|
||||
nvgFontSize(vg, 96.0f);
|
||||
nvgText(vg, fb_w/2, (fb_h*2/3)+24, spintext, NULL);
|
||||
|
||||
@@ -1 +1 @@
|
||||
#define COMMA_VERSION "0.6.4-release"
|
||||
#define COMMA_VERSION "0.6.5-release"
|
||||
|
||||
@@ -1,7 +1,9 @@
|
||||
#!/usr/bin/env python
|
||||
#!/usr/bin/env python3
|
||||
import os
|
||||
import gc
|
||||
import json
|
||||
import capnp
|
||||
import zmq
|
||||
from cereal import car, log
|
||||
from common.numpy_fast import clip
|
||||
from common.realtime import sec_since_boot, set_realtime_priority, Ratekeeper, DT_CTRL
|
||||
@@ -31,6 +33,7 @@ from selfdrive.locationd.calibration_helpers import Calibration, Filter
|
||||
|
||||
ThermalStatus = log.ThermalData.ThermalStatus
|
||||
State = log.ControlsState.OpenpilotState
|
||||
HwType = log.HealthData.HwType
|
||||
|
||||
|
||||
def isActive(state):
|
||||
@@ -52,12 +55,12 @@ def events_to_bytes(events):
|
||||
return ret
|
||||
|
||||
|
||||
def data_sample(CI, CC, sm, can_sock, cal_status, cal_perc, overtemp, free_space, low_battery,
|
||||
def data_sample(CI, CC, sm, can_poller, can_sock, cal_status, cal_perc, overtemp, free_space, low_battery,
|
||||
driver_status, state, mismatch_counter, params):
|
||||
"""Receive data from sockets and create events for battery, temperature and disk space"""
|
||||
|
||||
# Update carstate from CAN and create events
|
||||
can_strs = messaging.drain_sock_raw(can_sock, wait_for_one=True)
|
||||
can_strs = messaging.drain_sock_raw_poller(can_poller, can_sock, wait_for_one=True)
|
||||
CS = CI.update(CC, can_strs)
|
||||
|
||||
sm.update(0)
|
||||
@@ -87,7 +90,7 @@ def data_sample(CI, CC, sm, can_sock, cal_status, cal_perc, overtemp, free_space
|
||||
is_rhd = is_rhd_region(sm['gpsLocation'].latitude, sm['gpsLocation'].longitude)
|
||||
driver_status.is_rhd_region = is_rhd
|
||||
driver_status.is_rhd_region_checked = True
|
||||
put_nonblocking("IsRHD", str(int(is_rhd)))
|
||||
put_nonblocking("IsRHD", "1" if is_rhd else "0")
|
||||
|
||||
# Handle calibration
|
||||
if sm.updated['liveCalibration']:
|
||||
@@ -217,7 +220,7 @@ def state_transition(frame, CS, CP, state, events, soft_disable_timer, v_cruise_
|
||||
|
||||
|
||||
def state_control(frame, rcv_frame, plan, path_plan, CS, CP, state, events, v_cruise_kph, v_cruise_kph_last,
|
||||
AM, rk, driver_status, LaC, LoC, VM, read_only, is_metric, cal_perc):
|
||||
AM, rk, driver_status, LaC, LoC, read_only, is_metric, cal_perc):
|
||||
"""Given the state, this function returns an actuators packet"""
|
||||
|
||||
actuators = car.CarControl.Actuators.new_message()
|
||||
@@ -264,7 +267,7 @@ def state_control(frame, rcv_frame, plan, path_plan, CS, CP, state, events, v_cr
|
||||
actuators.gas, actuators.brake = LoC.update(active, CS.vEgo, CS.brakePressed, CS.standstill, CS.cruiseState.standstill,
|
||||
v_cruise_kph, v_acc_sol, plan.vTargetFuture, a_acc_sol, CP)
|
||||
# Steering PID loop and lateral MPC
|
||||
actuators.steer, actuators.steerAngle, lac_log = LaC.update(active, CS.vEgo, CS.steeringAngle, CS.steeringRate, CS.steeringTorqueEps, CS.steeringPressed, CP, VM, path_plan)
|
||||
actuators.steer, actuators.steerAngle, lac_log = LaC.update(active, CS.vEgo, CS.steeringAngle, CS.steeringRate, CS.steeringTorqueEps, CS.steeringPressed, CP, path_plan)
|
||||
|
||||
# Send a "steering required alert" if saturation count has reached the limit
|
||||
if LaC.sat_flag and CP.steerLimitAlert:
|
||||
@@ -342,7 +345,7 @@ def data_send(sm, pm, CS, CI, CP, VM, state, events, actuators, v_cruise_kph, rk
|
||||
"alertBlinkingRate": AM.alert_rate,
|
||||
"alertType": AM.alert_type,
|
||||
"alertSound": AM.audible_alert,
|
||||
"awarenessStatus": max(driver_status.awareness, 0.0) if isEnabled(state) else 0.0,
|
||||
"awarenessStatus": max(driver_status.awareness, -0.1) if isEnabled(state) else 1.0,
|
||||
"driverMonitoringOn": bool(driver_status.face_detected),
|
||||
"canMonoTimes": list(CS.canMonoTimes),
|
||||
"planMonoTime": sm.logMonoTime['plan'],
|
||||
@@ -424,8 +427,11 @@ def controlsd_thread(sm=None, pm=None, can_sock=None):
|
||||
|
||||
params = Params()
|
||||
|
||||
is_metric = params.get("IsMetric") == "1"
|
||||
passive = params.get("Passive") != "0"
|
||||
is_metric = params.get("IsMetric", encoding='utf8') == "1"
|
||||
passive = params.get("Passive", encoding='utf8') == "1"
|
||||
openpilot_enabled_toggle = params.get("OpenpilotEnabledToggle", encoding='utf8') == "1"
|
||||
|
||||
passive = passive or not openpilot_enabled_toggle
|
||||
|
||||
# Pub/Sub Sockets
|
||||
if pm is None:
|
||||
@@ -435,17 +441,20 @@ def controlsd_thread(sm=None, pm=None, can_sock=None):
|
||||
sm = messaging.SubMaster(['thermal', 'health', 'liveCalibration', 'driverMonitoring', 'plan', 'pathPlan', \
|
||||
'gpsLocation'], ignore_alive=['gpsLocation'])
|
||||
|
||||
can_poller = zmq.Poller()
|
||||
|
||||
if can_sock is None:
|
||||
can_timeout = None if os.environ.get('NO_CAN_TIMEOUT', False) else 100
|
||||
can_sock = messaging.sub_sock(service_list['can'].port, timeout=can_timeout)
|
||||
can_poller.register(can_sock)
|
||||
|
||||
# wait for health and CAN packets
|
||||
hw_type = messaging.recv_one(sm.sock['health']).health.hwType
|
||||
is_panda_black = hw_type == log.HealthData.HwType.blackPanda
|
||||
has_relay = hw_type in [HwType.blackPanda, HwType.uno]
|
||||
print("Waiting for CAN messages...")
|
||||
get_one_can(can_sock)
|
||||
|
||||
CI, CP = get_car(can_sock, pm.sock['sendcan'], is_panda_black)
|
||||
CI, CP = get_car(can_sock, pm.sock['sendcan'], has_relay)
|
||||
|
||||
car_recognized = CP.carName != 'mock'
|
||||
# If stock camera is disconnected, we loaded car controls and it's not chffrplus
|
||||
@@ -500,6 +509,10 @@ def controlsd_thread(sm=None, pm=None, can_sock=None):
|
||||
# controlsd is driven by can recv, expected at 100Hz
|
||||
rk = Ratekeeper(100, print_delay_threshold=None)
|
||||
|
||||
# FIXME: offroad alerts should not be created with negative severity
|
||||
connectivity_alert = params.get("Offroad_ConnectivityNeeded", encoding='utf8')
|
||||
internet_needed = connectivity_alert is not None and json.loads(connectivity_alert.replace("'", "\""))["severity"] >= 0
|
||||
|
||||
prof = Profiler(False) # off by default
|
||||
|
||||
while True:
|
||||
@@ -508,7 +521,7 @@ def controlsd_thread(sm=None, pm=None, can_sock=None):
|
||||
|
||||
# Sample data and compute car events
|
||||
CS, events, cal_status, cal_perc, overtemp, free_space, low_battery, mismatch_counter =\
|
||||
data_sample(CI, CC, sm, can_sock, cal_status, cal_perc, overtemp, free_space, low_battery,
|
||||
data_sample(CI, CC, sm, can_poller, can_sock, cal_status, cal_perc, overtemp, free_space, low_battery,
|
||||
driver_status, state, mismatch_counter, params)
|
||||
prof.checkpoint("Sample")
|
||||
|
||||
@@ -531,6 +544,8 @@ def controlsd_thread(sm=None, pm=None, can_sock=None):
|
||||
events.append(create_event('canError', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
|
||||
if not sounds_available:
|
||||
events.append(create_event('soundsUnavailable', [ET.NO_ENTRY, ET.PERMANENT]))
|
||||
if internet_needed:
|
||||
events.append(create_event('internetConnectivityNeeded', [ET.NO_ENTRY, ET.PERMANENT]))
|
||||
|
||||
# Only allow engagement with brake pressed when stopped behind another stopped car
|
||||
if CS.brakePressed and sm['plan'].vTargetFuture >= STARTING_TARGET_SPEED and not CP.radarOffCan and CS.vEgo < 0.3:
|
||||
@@ -545,7 +560,7 @@ def controlsd_thread(sm=None, pm=None, can_sock=None):
|
||||
# Compute actuators (runs PID loops and lateral MPC)
|
||||
actuators, v_cruise_kph, driver_status, v_acc, a_acc, lac_log = \
|
||||
state_control(sm.frame, sm.rcv_frame, sm['plan'], sm['pathPlan'], CS, CP, state, events, v_cruise_kph, v_cruise_kph_last, AM, rk,
|
||||
driver_status, LaC, LoC, VM, read_only, is_metric, cal_perc)
|
||||
driver_status, LaC, LoC, read_only, is_metric, cal_perc)
|
||||
|
||||
prof.checkpoint("State Control")
|
||||
|
||||
|
||||
@@ -10,7 +10,7 @@ AlertStatus = log.ControlsState.AlertStatus
|
||||
VisualAlert = car.CarControl.HUDControl.VisualAlert
|
||||
AudibleAlert = car.CarControl.HUDControl.AudibleAlert
|
||||
|
||||
class AlertManager(object):
|
||||
class AlertManager():
|
||||
|
||||
def __init__(self):
|
||||
self.activealerts = []
|
||||
|
||||
@@ -14,7 +14,7 @@ AlertStatus = log.ControlsState.AlertStatus
|
||||
AudibleAlert = car.CarControl.HUDControl.AudibleAlert
|
||||
VisualAlert = car.CarControl.HUDControl.VisualAlert
|
||||
|
||||
class Alert(object):
|
||||
class Alert():
|
||||
def __init__(self,
|
||||
alert_type,
|
||||
alert_text_1,
|
||||
@@ -124,7 +124,7 @@ ALERTS = [
|
||||
|
||||
Alert(
|
||||
"preDriverUnresponsive",
|
||||
"TOUCH STEERING WHEEL: No Driver Monitoring",
|
||||
"TOUCH STEERING WHEEL: No Face Detected",
|
||||
"",
|
||||
AlertStatus.normal, AlertSize.small,
|
||||
Priority.LOW, VisualAlert.steerRequired, AudibleAlert.none, .0, .1, .1, alert_rate=0.75),
|
||||
@@ -218,7 +218,7 @@ ALERTS = [
|
||||
"TAKE CONTROL",
|
||||
"Steer Unavailable Below ",
|
||||
AlertStatus.userPrompt, AlertSize.mid,
|
||||
Priority.MID, VisualAlert.steerRequired, AudibleAlert.chimeWarning1, 0., 0., .1),
|
||||
Priority.MID, VisualAlert.steerRequired, AudibleAlert.none, 0., 0.4, .3),
|
||||
|
||||
Alert(
|
||||
"debugAlert",
|
||||
@@ -633,11 +633,18 @@ ALERTS = [
|
||||
|
||||
Alert(
|
||||
"commIssueNoEntry",
|
||||
"openpilot unavailable",
|
||||
"openpilot Unavailable",
|
||||
"Communication Issue between Processes",
|
||||
AlertStatus.normal, AlertSize.mid,
|
||||
Priority.LOW, VisualAlert.none, AudibleAlert.chimeDisengage, .4, 2., 3.),
|
||||
|
||||
Alert(
|
||||
"internetConnectivityNeededNoEntry",
|
||||
"openpilot Unavailable",
|
||||
"Internet Connectivity Needed",
|
||||
AlertStatus.normal, AlertSize.mid,
|
||||
Priority.LOW, VisualAlert.none, AudibleAlert.chimeDisengage, .4, 2., 3.),
|
||||
|
||||
# permanent alerts
|
||||
Alert(
|
||||
"steerUnavailablePermanent",
|
||||
@@ -674,6 +681,20 @@ ALERTS = [
|
||||
AlertStatus.normal, AlertSize.mid,
|
||||
Priority.LOW_LOWEST, VisualAlert.none, AudibleAlert.none, 0., 0., .2),
|
||||
|
||||
Alert(
|
||||
"invalidGiraffeToyotaPermanent",
|
||||
"Unsupported Giraffe Configuration",
|
||||
"Visit comma.ai/tg",
|
||||
AlertStatus.normal, AlertSize.mid,
|
||||
Priority.LOW_LOWEST, VisualAlert.none, AudibleAlert.none, 0., 0., .2),
|
||||
|
||||
Alert(
|
||||
"internetConnectivityNeededPermanent",
|
||||
"Internet Connectivity Needed",
|
||||
"Check for Updates to Be Able to Engage",
|
||||
AlertStatus.normal, AlertSize.mid,
|
||||
Priority.LOW_LOWEST, VisualAlert.none, AudibleAlert.none, 0., 0., .2),
|
||||
|
||||
Alert(
|
||||
"sensorDataInvalidPermanent",
|
||||
"No Data from EON Sensors",
|
||||
|
||||
@@ -0,0 +1,19 @@
|
||||
{
|
||||
"Offroad_ChargeDisabled": {
|
||||
"text": "EON charging disabled after car being off for more than 3 days. Turn ignition on to start charging again.",
|
||||
"severity": 0
|
||||
},
|
||||
"Offroad_TemperatureTooHigh": {
|
||||
"text": "EON temperature too high. System won't start.",
|
||||
"severity": 1
|
||||
},
|
||||
"Offroad_ConnectivityNeededPrompt": {
|
||||
"text": "Connect to internet to check for updates. openpilot won't engage in ",
|
||||
"severity": 0,
|
||||
"_comment": "Append the number of days at the end of the text"
|
||||
},
|
||||
"Offroad_ConnectivityNeeded": {
|
||||
"text": "Connect to internet to check for updates. openpilot won't engage.",
|
||||
"severity": 1
|
||||
}
|
||||
}
|
||||
@@ -13,7 +13,7 @@ _DISTRACTED_PROMPT_TIME_TILL_TERMINAL = 6.
|
||||
|
||||
_FACE_THRESHOLD = 0.4
|
||||
_EYE_THRESHOLD = 0.4
|
||||
_BLINK_THRESHOLD = 0.2 # 0.225
|
||||
_BLINK_THRESHOLD = 0.5 # 0.225
|
||||
_PITCH_WEIGHT = 1.35 # 1.5 # pitch matters a lot more
|
||||
_METRIC_THRESHOLD = 0.4
|
||||
_PITCH_POS_ALLOWANCE = 0.04 # 0.08 # rad, to not be too sensitive on positive pitch
|
||||
@@ -29,13 +29,13 @@ _POSE_OFFSET_MAX_COUNT = 3600 # stop deweighting new data after 6 min, aka "shor
|
||||
_RECOVERY_FACTOR_MAX = 5. # relative to minus step change
|
||||
_RECOVERY_FACTOR_MIN = 1.25 # relative to minus step change
|
||||
|
||||
MAX_TERMINAL_ALERTS = 3 # not allowed to engage after 3 terminal alerts
|
||||
MAX_TERMINAL_ALERTS = 3 # not allowed to engage after 3 terminal alerts
|
||||
|
||||
# model output refers to center of cropped image, so need to apply the x displacement offset
|
||||
RESIZED_FOCAL = 320.0
|
||||
H, W, FULL_W = 320, 160, 426
|
||||
|
||||
class DistractedType(object):
|
||||
class DistractedType():
|
||||
NOT_DISTRACTED = 0
|
||||
BAD_POSE = 1
|
||||
BAD_BLINK = 2
|
||||
@@ -49,8 +49,8 @@ def head_orientation_from_descriptor(angles_desc, pos_desc, rpy_calib):
|
||||
roll_prnet = angles_desc[2]
|
||||
|
||||
face_pixel_position = ((pos_desc[0] + .5)*W - W + FULL_W, (pos_desc[1]+.5)*H)
|
||||
yaw_focal_angle = np.arctan2(face_pixel_position[0] - FULL_W/2, RESIZED_FOCAL)
|
||||
pitch_focal_angle = np.arctan2(face_pixel_position[1] - H/2, RESIZED_FOCAL)
|
||||
yaw_focal_angle = np.arctan2(face_pixel_position[0] - FULL_W//2, RESIZED_FOCAL)
|
||||
pitch_focal_angle = np.arctan2(face_pixel_position[1] - H//2, RESIZED_FOCAL)
|
||||
|
||||
roll = roll_prnet
|
||||
pitch = pitch_prnet + pitch_focal_angle
|
||||
@@ -142,7 +142,7 @@ class DriverStatus():
|
||||
|
||||
if pose_metric > _METRIC_THRESHOLD:
|
||||
return DistractedType.BAD_POSE
|
||||
elif blink.left_blink>_BLINK_THRESHOLD and blink.right_blink>_BLINK_THRESHOLD:
|
||||
elif (blink.left_blink + blink.right_blink)*0.5 > _BLINK_THRESHOLD:
|
||||
return DistractedType.BAD_BLINK
|
||||
else:
|
||||
return DistractedType.NOT_DISTRACTED
|
||||
|
||||
@@ -7,7 +7,7 @@ _FCW_A_ACT_V = [-3., -2.]
|
||||
_FCW_A_ACT_BP = [0., 30.]
|
||||
|
||||
|
||||
class FCWChecker(object):
|
||||
class FCWChecker():
|
||||
def __init__(self):
|
||||
self.reset_lead(0.0)
|
||||
|
||||
|
||||
@@ -26,13 +26,13 @@ def calc_d_poly(l_poly, r_poly, p_poly, l_prob, r_prob, lane_width):
|
||||
path_from_right_lane = r_poly.copy()
|
||||
path_from_right_lane[3] += lane_width / 2.0
|
||||
|
||||
lr_prob = l_prob + r_prob - l_prob * r_prob
|
||||
lr_prob = l_prob * r_prob
|
||||
|
||||
d_poly_lane = (l_prob * path_from_left_lane + r_prob * path_from_right_lane) / (l_prob + r_prob + 0.0001)
|
||||
return lr_prob * d_poly_lane + (1.0 - lr_prob) * p_poly
|
||||
|
||||
|
||||
class LanePlanner(object):
|
||||
class LanePlanner():
|
||||
def __init__(self):
|
||||
self.l_poly = [0., 0., 0., 0.]
|
||||
self.r_poly = [0., 0., 0., 0.]
|
||||
|
||||
@@ -9,7 +9,7 @@ from selfdrive.car import apply_toyota_steer_torque_limits
|
||||
from selfdrive.controls.lib.drive_helpers import get_steer_max
|
||||
|
||||
|
||||
class LatControlINDI(object):
|
||||
class LatControlINDI():
|
||||
def __init__(self, CP):
|
||||
self.angle_steers_des = 0.
|
||||
|
||||
@@ -47,7 +47,7 @@ class LatControlINDI(object):
|
||||
self.output_steer = 0.
|
||||
self.counter = 0
|
||||
|
||||
def update(self, active, v_ego, angle_steers, angle_steers_rate, eps_torque, steer_override, CP, VM, path_plan):
|
||||
def update(self, active, v_ego, angle_steers, angle_steers_rate, eps_torque, steer_override, CP, path_plan):
|
||||
# Update Kalman filter
|
||||
y = np.matrix([[math.radians(angle_steers)], [math.radians(angle_steers_rate)]])
|
||||
self.x = np.dot(self.A_K, self.x) + np.dot(self.K, y)
|
||||
|
||||
@@ -4,7 +4,7 @@ from common.numpy_fast import clip
|
||||
from cereal import log
|
||||
|
||||
|
||||
class LatControlLQR(object):
|
||||
class LatControlLQR():
|
||||
def __init__(self, CP, rate=100):
|
||||
self.sat_flag = False
|
||||
self.scale = CP.lateralTuning.lqr.scale
|
||||
@@ -29,7 +29,7 @@ class LatControlLQR(object):
|
||||
self.i_lqr = 0.0
|
||||
self.output_steer = 0.0
|
||||
|
||||
def update(self, active, v_ego, angle_steers, angle_steers_rate, eps_torque, steer_override, CP, VM, path_plan):
|
||||
def update(self, active, v_ego, angle_steers, angle_steers_rate, eps_torque, steer_override, CP, path_plan):
|
||||
lqr_log = log.ControlsState.LateralLQRState.new_message()
|
||||
|
||||
steers_max = get_steer_max(CP, v_ego)
|
||||
|
||||
@@ -4,7 +4,7 @@ from cereal import car
|
||||
from cereal import log
|
||||
|
||||
|
||||
class LatControlPID(object):
|
||||
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),
|
||||
@@ -14,7 +14,7 @@ class LatControlPID(object):
|
||||
def reset(self):
|
||||
self.pid.reset()
|
||||
|
||||
def update(self, active, v_ego, angle_steers, angle_steers_rate, eps_torque, steer_override, CP, VM, path_plan):
|
||||
def update(self, active, v_ego, angle_steers, angle_steers_rate, eps_torque, steer_override, CP, path_plan):
|
||||
pid_log = log.ControlsState.LateralPIDState.new_message()
|
||||
pid_log.steerAngle = float(angle_steers)
|
||||
pid_log.steerRate = float(angle_steers_rate)
|
||||
|
||||
@@ -0,0 +1,3 @@
|
||||
lateral_mpc.o: lateral_mpc.c lib_mpc_export/acado_common.h \
|
||||
lib_mpc_export/acado_qpoases_interface.hpp \
|
||||
lib_mpc_export/acado_auxiliary_functions.h
|
||||
Binary file not shown.
@@ -0,0 +1,5 @@
|
||||
lib_mpc_export/acado_auxiliary_functions.o: \
|
||||
lib_mpc_export/acado_auxiliary_functions.c \
|
||||
lib_mpc_export/acado_auxiliary_functions.h \
|
||||
lib_mpc_export/acado_common.h \
|
||||
lib_mpc_export/acado_qpoases_interface.hpp
|
||||
Binary file not shown.
@@ -0,0 +1,3 @@
|
||||
lib_mpc_export/acado_integrator.o: lib_mpc_export/acado_integrator.c \
|
||||
lib_mpc_export/acado_common.h \
|
||||
lib_mpc_export/acado_qpoases_interface.hpp
|
||||
Binary file not shown.
@@ -0,0 +1,24 @@
|
||||
lib_mpc_export/acado_qpoases_interface.o: \
|
||||
lib_mpc_export/acado_qpoases_interface.cpp \
|
||||
lib_mpc_export/acado_common.h \
|
||||
lib_mpc_export/acado_qpoases_interface.hpp \
|
||||
../../../../phonelibs/qpoases/INCLUDE/QProblem.hpp \
|
||||
../../../../phonelibs/qpoases/INCLUDE/QProblemB.hpp \
|
||||
../../../../phonelibs/qpoases/INCLUDE/Bounds.hpp \
|
||||
../../../../phonelibs/qpoases/INCLUDE/SubjectTo.hpp \
|
||||
../../../../phonelibs/qpoases/INCLUDE/Indexlist.hpp \
|
||||
../../../../phonelibs/qpoases/INCLUDE/Utils.hpp \
|
||||
../../../../phonelibs/qpoases/INCLUDE/MessageHandling.hpp \
|
||||
../../../../phonelibs/qpoases/INCLUDE/Types.hpp \
|
||||
../../../../phonelibs/qpoases/INCLUDE/Constants.hpp \
|
||||
../../../../phonelibs/qpoases/SRC/MessageHandling.ipp \
|
||||
../../../../phonelibs/qpoases/SRC/Utils.ipp \
|
||||
../../../../phonelibs/qpoases/SRC/Indexlist.ipp \
|
||||
../../../../phonelibs/qpoases/SRC/SubjectTo.ipp \
|
||||
../../../../phonelibs/qpoases/SRC/Bounds.ipp \
|
||||
../../../../phonelibs/qpoases/SRC/QProblemB.ipp \
|
||||
../../../../phonelibs/qpoases/INCLUDE/Constraints.hpp \
|
||||
../../../../phonelibs/qpoases/SRC/Constraints.ipp \
|
||||
../../../../phonelibs/qpoases/INCLUDE/CyclingManager.hpp \
|
||||
../../../../phonelibs/qpoases/SRC/CyclingManager.ipp \
|
||||
../../../../phonelibs/qpoases/SRC/QProblem.ipp
|
||||
Some files were not shown because too many files have changed in this diff Show More
Reference in New Issue
Block a user