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

update to 0.6

Browse Source
This commit is contained in:
Rick Lan
2019-07-10 09:51:52 +10:00
parent 979d1e9fb4 99a96a99bc
commit 74cbb6fde3
1372 changed files with 92887 additions and 177092 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/athena/athenad.py
+1 -2
View File
@@ -65,10 +65,9 @@ def jsonrpc_handler(end_event):
# TODO: add service to, for example, start visiond and take a picture
@dispatcher.add_method
def getMessage(service=None, timeout=1000):
context = zmq.Context()
if service is None or service not in service_list:
raise Exception("invalid service")
socket = messaging.sub_sock(context, service_list[service].port)
socket = messaging.sub_sock(service_list[service].port)
socket.setsockopt(zmq.RCVTIMEO, timeout)
ret = messaging.recv_one(socket)
return ret.to_dict()
selfdrive/boardd/Makefile
+9 -6
View File
@@ -16,14 +16,11 @@ WARN_FLAGS = -Werror=implicit-function-declaration \
CFLAGS = -std=gnu11 -g -fPIC -I../ -I../../ -O2 $(WARN_FLAGS)
CXXFLAGS = -std=c++11 -g -fPIC -I../ -I../../ -O2 $(WARN_FLAGS)
ZMQ_FLAGS = -I$(PHONELIBS)/zmq/aarch64/include
ZMQ_LIBS = -L$(PHONELIBS)/zmq/aarch64/lib \
-l:libczmq.a -l:libzmq.a \
-lgnustl_shared
ZMQ_LIBS = -l:libczmq.a -l:libzmq.a -lgnustl_shared
JSON_FLAGS = -I$(PHONELIBS)/json/src
EXTRA_LIBS = -lusb
EXTRA_LIBS = -lusb-1.0
# ifeq ($(OS),GNU/Linux)
# # for Drive PX2
@@ -31,8 +28,14 @@ EXTRA_LIBS = -lusb
# CEREAL_LIBS = -lcapnp -lkj -lcapnp_c
# EXTRA_LIBS = -lusb-1.0 -lpthread
# endif
ifeq ($(ARCH),aarch64)
CFLAGS += -mcpu=cortex-a57
CXXFLAGS += -mcpu=cortex-a57
endif
ifeq ($(ARCH),x86_64)
ZMQ_FLAGS = -I$(PHONELIBS)/zmq/aarch64/include
ZMQ_LIBS = -L$(BASEDIR)/external/zmq/lib/ \
-l:libczmq.a -l:libzmq.a
EXTRA_LIBS = -lusb-1.0 -lpthread
@@ -75,7 +78,7 @@ boardd.o: boardd.cc
boardd_api_impl.so: libcan_list_to_can_capnp.a boardd_api_impl.pyx boardd_setup.py
python boardd_setup.py build_ext --inplace
python2 boardd_setup.py build_ext --inplace
rm -rf build
rm -f boardd_api_impl.cpp
selfdrive/boardd/boardd.cc
+12 -12
View File
@@ -15,7 +15,7 @@
#include <pthread.h>
#include <zmq.h>
#include <libusb.h>
#include <libusb-1.0/libusb.h>
#include <capnp/serialize.h>
#include "cereal/gen/cpp/log.capnp.h"
@@ -96,37 +96,37 @@ void *safety_setter_thread(void *s) {
int safety_setting = 0;
switch (safety_model) {
case (int)cereal::CarParams::SafetyModels::NO_OUTPUT:
case cereal::CarParams::SafetyModel::NO_OUTPUT:
safety_setting = SAFETY_NOOUTPUT;
break;
case (int)cereal::CarParams::SafetyModels::HONDA:
case cereal::CarParams::SafetyModel::HONDA:
safety_setting = SAFETY_HONDA;
break;
case (int)cereal::CarParams::SafetyModels::TOYOTA:
case cereal::CarParams::SafetyModel::TOYOTA:
safety_setting = SAFETY_TOYOTA;
break;
case (int)cereal::CarParams::SafetyModels::ELM327:
case cereal::CarParams::SafetyModel::ELM327:
safety_setting = SAFETY_ELM327;
break;
case (int)cereal::CarParams::SafetyModels::GM:
case cereal::CarParams::SafetyModel::GM:
safety_setting = SAFETY_GM;
break;
case (int)cereal::CarParams::SafetyModels::HONDA_BOSCH:
case cereal::CarParams::SafetyModel::HONDA_BOSCH:
safety_setting = SAFETY_HONDA_BOSCH;
break;
case (int)cereal::CarParams::SafetyModels::FORD:
case cereal::CarParams::SafetyModel::FORD:
safety_setting = SAFETY_FORD;
break;
case (int)cereal::CarParams::SafetyModels::CADILLAC:
case cereal::CarParams::SafetyModel::CADILLAC:
safety_setting = SAFETY_CADILLAC;
break;
case (int)cereal::CarParams::SafetyModels::HYUNDAI:
case cereal::CarParams::SafetyModel::HYUNDAI:
safety_setting = SAFETY_HYUNDAI;
break;
case (int)cereal::CarParams::SafetyModels::CHRYSLER:
case cereal::CarParams::SafetyModel::CHRYSLER:
safety_setting = SAFETY_CHRYSLER;
break;
case (int)cereal::CarParams::SafetyModels::SUBARU:
case cereal::CarParams::SafetyModel::SUBARU:
safety_setting = SAFETY_SUBARU;
break;
default:
selfdrive/boardd/boardd_api_impl.pyx
+3 -3
View File
@@ -9,9 +9,9 @@ cdef struct can_frame:
long busTime
long src
cdef extern void can_list_to_can_capnp_cpp(const vector[can_frame] &can_list, string &out, bool sendCan)
cdef extern void can_list_to_can_capnp_cpp(const vector[can_frame] &can_list, string &out, bool sendCan, bool valid)
def can_list_to_can_capnp(can_msgs, msgtype='can'):
def can_list_to_can_capnp(can_msgs, msgtype='can', valid=True):
cdef vector[can_frame] can_list
cdef can_frame f
for can_msg in can_msgs:
@@ -21,5 +21,5 @@ def can_list_to_can_capnp(can_msgs, msgtype='can'):
f.src = can_msg[3]
can_list.push_back(f)
cdef string out
can_list_to_can_capnp_cpp(can_list, out, msgtype == 'sendcan')
can_list_to_can_capnp_cpp(can_list, out, msgtype == 'sendcan', valid)
return out
selfdrive/boardd/can_list_to_can_capnp.cc
+2 -1
View File
@@ -15,10 +15,11 @@ typedef struct {
extern "C" {
void can_list_to_can_capnp_cpp(const std::vector<can_frame> &can_list, std::string &out, bool sendCan) {
void can_list_to_can_capnp_cpp(const std::vector<can_frame> &can_list, std::string &out, bool sendCan, bool valid) {
capnp::MallocMessageBuilder msg;
cereal::Event::Builder event = msg.initRoot<cereal::Event>();
event.setLogMonoTime(nanos_since_boot());
event.setValid(valid);
auto canData = sendCan ? event.initSendcan(can_list.size()) : event.initCan(can_list.size());
int j = 0;
selfdrive/boardd/tests/boardd_old.py
+16 -13
View File
@@ -8,7 +8,6 @@
import os
import struct
import zmq
import time
import selfdrive.messaging as messaging
@@ -113,12 +112,11 @@ def can_init():
cloudlog.info("can init done")
def boardd_mock_loop():
context = zmq.Context()
can_init()
handle.controlWrite(0x40, 0xdc, SAFETY_ALLOUTPUT, 0, b'')
logcan = messaging.sub_sock(context, service_list['can'].port)
sendcan = messaging.pub_sock(context, service_list['sendcan'].port)
logcan = messaging.sub_sock(service_list['can'].port)
sendcan = messaging.pub_sock(service_list['sendcan'].port)
while 1:
tsc = messaging.drain_sock(logcan, wait_for_one=True)
@@ -126,12 +124,19 @@ def boardd_mock_loop():
snd = []
for s in snds:
snd += s
snd = filter(lambda x: x[-1] <= 1, snd)
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))
can_send_many(snd)
# recv @ 100hz
can_msgs = can_recv()
print("sent %d got %d" % (len(snd), len(can_msgs)))
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))
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')
sendcan.send(m.to_bytes())
@@ -151,16 +156,15 @@ def boardd_test_loop():
# *** main loop ***
def boardd_loop(rate=200):
rk = Ratekeeper(rate)
context = zmq.Context()
can_init()
# *** publishes can and health
logcan = messaging.pub_sock(context, service_list['can'].port)
health_sock = messaging.pub_sock(context, service_list['health'].port)
logcan = messaging.pub_sock(service_list['can'].port)
health_sock = messaging.pub_sock(service_list['health'].port)
# *** subscribes to can send
sendcan = messaging.sub_sock(context, service_list['sendcan'].port)
sendcan = messaging.sub_sock(service_list['sendcan'].port)
# drain sendcan to delete any stale messages from previous runs
messaging.drain_sock(sendcan)
@@ -199,14 +203,13 @@ def boardd_loop(rate=200):
# *** main loop ***
def boardd_proxy_loop(rate=200, address="192.168.2.251"):
rk = Ratekeeper(rate)
context = zmq.Context()
can_init()
# *** subscribes can
logcan = messaging.sub_sock(context, service_list['can'].port, addr=address)
logcan = messaging.sub_sock(service_list['can'].port, addr=address)
# *** publishes to can send
sendcan = messaging.pub_sock(context, service_list['sendcan'].port)
sendcan = messaging.pub_sock(service_list['sendcan'].port)
# drain sendcan to delete any stale messages from previous runs
messaging.drain_sock(sendcan)
selfdrive/boardd/tests/replay_many.py
Executable
+54
View File
@@ -0,0 +1,54 @@
#!/usr/bin/env python
import sys
import time
import signal
from panda import Panda
from multiprocessing import Pool
import selfdrive.messaging as messaging
from selfdrive.services import service_list
from selfdrive.boardd.boardd import can_capnp_to_can_list
def initializer():
"""Ignore CTRL+C in the worker process.
source: https://stackoverflow.com/a/44869451 """
signal.signal(signal.SIGINT, signal.SIG_IGN)
def send_thread(serial):
panda = Panda(serial)
panda.set_safety_mode(Panda.SAFETY_ALLOUTPUT)
panda.set_can_loopback(False)
can_sock = messaging.sub_sock(service_list['can'].port)
while True:
# Send messages one bus 0 and 1
tsc = messaging.recv_one(can_sock)
snd = can_capnp_to_can_list(tsc.can)
snd = filter(lambda x: x[-1] <= 2, snd)
panda.can_send_many(snd)
# Drain panda message buffer
panda.can_recv()
if __name__ == "__main__":
serials = Panda.list()
num_pandas = len(serials)
if num_pandas == 0:
print("No pandas found. Exiting")
sys.exit(1)
else:
print("%d pandas found. Starting broadcast" % num_pandas)
pool = Pool(num_pandas, initializer=initializer)
pool.map_async(send_thread, serials)
while True:
try:
time.sleep(10)
except KeyboardInterrupt:
pool.terminate()
pool.join()
raise
selfdrive/boardd/tests/test_boardd_loopback.py
+2 -5
View File
@@ -3,7 +3,6 @@
import os
import random
import zmq
import time
from selfdrive.boardd.boardd import can_list_to_can_capnp
@@ -16,10 +15,8 @@ def get_test_string():
BUS = 0
def main():
context = zmq.Context()
rcv = sub_sock(context, service_list['can'].port) # port 8006
snd = pub_sock(context, service_list['sendcan'].port) # port 8017
rcv = sub_sock(service_list['can'].port) # port 8006
snd = pub_sock(service_list['sendcan'].port) # port 8017
time.sleep(0.3) # wait to bind before send/recv
for i in range(10):
selfdrive/can/Makefile
+16 -8
View File
@@ -18,6 +18,11 @@ CFLAGS = -std=gnu11 -g -fPIC -O2 $(WARN_FLAGS)
CXXFLAGS = -std=c++11 -g -fPIC -O2 $(WARN_FLAGS)
LDFLAGS =
ifeq ($(ARCH),aarch64)
CFLAGS += -mcpu=cortex-a57
CXXFLAGS += -mcpu=cortex-a57
endif
ifeq ($(UNAME_S),Darwin)
ZMQ_LIBS = -L/usr/local/lib -lzmq
else ifeq ($(OPTEST),1)
@@ -26,16 +31,14 @@ else ifeq ($(UNAME_M),x86_64)
ZMQ_FLAGS = -I$(PHONELIBS)/zmq/x64/include
ZMQ_LIBS = -L$(PHONELIBS)/zmq/x64/lib -l:libzmq.a
else ifeq ($(UNAME_M),aarch64)
ZMQ_FLAGS = -I$(PHONELIBS)/zmq/aarch64/include
ZMQ_LIBS = -L$(PHONELIBS)/zmq/aarch64/lib -l:libzmq.a
LDFLAGS += -lgnustl_shared
ZMQ_LIBS = -l:libzmq.a -lgnustl_shared
endif
OBJDIR = obj
OPENDBC_PATH := $(shell python -c 'import opendbc; print opendbc.DBC_PATH')
OPENDBC_PATH := $(shell python2 -c 'import opendbc; print opendbc.DBC_PATH')
DBC_SOURCES := $(wildcard $(OPENDBC_PATH)/*.dbc)
DBC_SOURCES := $(sort $(wildcard $(OPENDBC_PATH)/*.dbc))
DBC_OBJS := $(patsubst $(OPENDBC_PATH)/%.dbc,$(OBJDIR)/%.o,$(DBC_SOURCES))
DBC_CCS := $(patsubst $(OPENDBC_PATH)/%.dbc,dbc_out/%.cc,$(DBC_SOURCES))
.SECONDARY: $(DBC_CCS)
@@ -45,7 +48,7 @@ LIBDBC_OBJS := $(OBJDIR)/dbc.o $(OBJDIR)/parser.o $(OBJDIR)/packer.o
CWD := $(shell pwd)
.PHONY: all
all: $(OBJDIR) libdbc.so
all: $(OBJDIR) libdbc.so parser_pyx.so
include ../common/cereal.mk
@@ -67,10 +70,15 @@ libdbc.so:: $(LIBDBC_OBJS) $(DBC_OBJS)
$(CEREAL_LIBS)
packer_impl.so: packer_impl.pyx packer_setup.py
python packer_setup.py build_ext --inplace
python2 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
rm -rf build
rm -f parser_pyx.cpp
$(OBJDIR)/%.o: %.cc
@echo "[ CXX ] $@"
$(CXX) -fPIC -c -o '$@' $^ \
@@ -90,7 +98,7 @@ $(OBJDIR)/%.o: dbc_out/%.cc
dbc_out/%.cc: process_dbc.py dbc_template.cc $(OPENDBC_PATH)/%.dbc
@echo "[ DBC GEN ] $@"
@echo "Missing prereq $?"
PYTHONPATH=$(PYTHONPATH):$(CWD)/../../pyextra ./process_dbc.py $(OPENDBC_PATH) dbc_out
./process_dbc.py $(OPENDBC_PATH) dbc_out
$(OBJDIR):
mkdir -p $@
selfdrive/can/can_define.py
+39
View File
@@ -0,0 +1,39 @@
from collections import defaultdict
from selfdrive.can.libdbc_py import libdbc, ffi
class CANDefine(object):
def __init__(self, dbc_name):
self.dv = defaultdict(dict)
self.dbc_name = dbc_name
self.dbc = libdbc.dbc_lookup(dbc_name)
num_vals = self.dbc[0].num_vals
self.address_to_msg_name = {}
num_msgs = self.dbc[0].num_msgs
for i in range(num_msgs):
msg = self.dbc[0].msgs[i]
name = ffi.string(msg.name)
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)
address = val.address
def_val = ffi.string(val.def_val)
#separate definition/value pairs
def_val = def_val.split()
values = [int(v) for v in def_val[::2]]
defs = def_val[1::2]
if address not in self.dv:
self.dv[address] = {}
msgname = self.address_to_msg_name[address]
self.dv[msgname] = {}
# two ways to lookup: address or msg name
self.dv[address][sgname] = {v: d for v, d in zip(values, defs)} #build dict
self.dv[msgname][sgname] = self.dv[address][sgname]
selfdrive/can/parser.cc
+19
View File
@@ -416,6 +416,17 @@ void* can_init(int bus, const char* dbc_name,
return (void*)ret;
}
void* can_init_with_vectors(int bus, const char* dbc_name,
std::vector<MessageParseOptions> message_options,
std::vector<SignalParseOptions> signal_options,
bool sendcan, const char* tcp_addr, int timeout) {
CANParser* ret = new CANParser(bus, std::string(dbc_name),
message_options,
signal_options,
sendcan, std::string(tcp_addr), timeout);
return (void*)ret;
}
int can_update(void* can, uint64_t sec, bool wait) {
CANParser* cp = (CANParser*)can;
return cp->update(sec, wait);
@@ -435,6 +446,14 @@ size_t can_query(void* can, uint64_t sec, bool *out_can_valid, size_t out_values
return values.size();
};
void can_query_vector(void* can, uint64_t sec, bool *out_can_valid, std::vector<SignalValue> &values) {
CANParser* cp = (CANParser*)can;
if (out_can_valid) {
*out_can_valid = cp->can_valid;
}
values = cp->query(sec);
};
}
#ifdef TEST
selfdrive/can/parser.py
+7 -250
View File
@@ -1,252 +1,9 @@
import time
from collections import defaultdict
import numbers
import os
import subprocess
from selfdrive.can.libdbc_py import libdbc, ffi
can_dir = os.path.dirname(os.path.abspath(__file__))
libdbc_fn = os.path.join(can_dir, "libdbc.so")
subprocess.check_call(["make"], cwd=can_dir)
class CANParser(object):
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 = []
self.can_valid = True
self.vl = defaultdict(dict)
self.ts = defaultdict(dict)
self.dbc_name = dbc_name
self.dbc = libdbc.dbc_lookup(dbc_name)
self.msg_name_to_addres = {}
self.address_to_msg_name = {}
num_msgs = self.dbc[0].num_msgs
for i in range(num_msgs):
msg = self.dbc[0].msgs[i]
name = ffi.string(msg.name)
address = msg.address
self.msg_name_to_addres[name] = address
self.address_to_msg_name[address] = 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_addres[s[1]], 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_addres[c[0]], c[1])
checks[i] = c
sig_names = dict((name, ffi.new("char[]", name)) for name, _, _ in signals)
signal_options_c = ffi.new("SignalParseOptions[]", [
{
'address': sig_address,
'name': sig_names[sig_name],
'default_value': sig_default,
} for sig_name, sig_address, sig_default in signals])
message_options = dict((address, 0) for _, address, _ in signals)
message_options.update(dict(checks))
message_options_c = ffi.new("MessageParseOptions[]", [
{
'address': msg_address,
'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.p_can_valid = ffi.new("bool*")
value_count = libdbc.can_query(self.can, 0, self.p_can_valid, 0, ffi.NULL)
self.can_values = ffi.new("SignalValue[%d]" % value_count)
self.update_vl(0)
# print("===")
def update_vl(self, sec):
can_values_len = libdbc.can_query(self.can, sec, self.p_can_valid, len(self.can_values), self.can_values)
assert can_values_len <= len(self.can_values)
self.can_valid = self.p_can_valid[0]
# print(can_values_len)
ret = set()
for i in xrange(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)
self.vl[address][name] = cv.value
self.ts[address][name] = cv.ts
sig_name = self.address_to_msg_name[address]
self.vl[sig_name][name] = cv.value
self.ts[sig_name][name] = cv.ts
ret.add(address)
return ret
def update(self, sec, wait):
"""Returns if the update was successfull (e.g. no rcv timeout happened)"""
r = libdbc.can_update(self.can, sec, wait)
return bool(r >= 0), self.update_vl(sec)
class CANDefine(object):
def __init__(self, dbc_name):
self.dv = defaultdict(dict)
self.dbc_name = dbc_name
self.dbc = libdbc.dbc_lookup(dbc_name)
num_vals = self.dbc[0].num_vals
self.address_to_msg_name = {}
num_msgs = self.dbc[0].num_msgs
for i in range(num_msgs):
msg = self.dbc[0].msgs[i]
name = ffi.string(msg.name)
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)
address = val.address
def_val = ffi.string(val.def_val)
#separate definition/value pairs
def_val = def_val.split()
values = [int(v) for v in def_val[::2]]
defs = def_val[1::2]
if address not in self.dv:
self.dv[address] = {}
msgname = self.address_to_msg_name[address]
self.dv[msgname] = {}
# two ways to lookup: address or msg name
self.dv[address][sgname] = {v: d for v, d in zip(values, defs)} #build dict
self.dv[msgname][sgname] = self.dv[address][sgname]
if __name__ == "__main__":
from common.realtime import sec_since_boot
radar_messages = range(0x430, 0x43A) + range(0x440, 0x446)
# signals = zip(['LONG_DIST'] * 16 + ['NEW_TRACK'] * 16 + ['LAT_DIST'] * 16 +
# ['REL_SPEED'] * 16, radar_messages * 4,
# [255] * 16 + [1] * 16 + [0] * 16 + [0] * 16)
# checks = zip(radar_messages, [20]*16)
# cp = CANParser("acura_ilx_2016_nidec", signals, checks, 1)
# signals = [
# ("XMISSION_SPEED", 0x158, 0), #sig_name, sig_address, default
# ("WHEEL_SPEED_FL", 0x1d0, 0),
# ("WHEEL_SPEED_FR", 0x1d0, 0),
# ("WHEEL_SPEED_RL", 0x1d0, 0),
# ("STEER_ANGLE", 0x14a, 0),
# ("STEER_TORQUE_SENSOR", 0x18f, 0),
# ("GEAR", 0x191, 0),
# ("WHEELS_MOVING", 0x1b0, 1),
# ("DOOR_OPEN_FL", 0x405, 1),
# ("DOOR_OPEN_FR", 0x405, 1),
# ("DOOR_OPEN_RL", 0x405, 1),
# ("DOOR_OPEN_RR", 0x405, 1),
# ("CRUISE_SPEED_PCM", 0x324, 0),
# ("SEATBELT_DRIVER_LAMP", 0x305, 1),
# ("SEATBELT_DRIVER_LATCHED", 0x305, 0),
# ("BRAKE_PRESSED", 0x17c, 0),
# ("CAR_GAS", 0x130, 0),
# ("CRUISE_BUTTONS", 0x296, 0),
# ("ESP_DISABLED", 0x1a4, 1),
# ("HUD_LEAD", 0x30c, 0),
# ("USER_BRAKE", 0x1a4, 0),
# ("STEER_STATUS", 0x18f, 5),
# ("WHEEL_SPEED_RR", 0x1d0, 0),
# ("BRAKE_ERROR_1", 0x1b0, 1),
# ("BRAKE_ERROR_2", 0x1b0, 1),
# ("GEAR_SHIFTER", 0x191, 0),
# ("MAIN_ON", 0x326, 0),
# ("ACC_STATUS", 0x17c, 0),
# ("PEDAL_GAS", 0x17c, 0),
# ("CRUISE_SETTING", 0x296, 0),
# ("LEFT_BLINKER", 0x326, 0),
# ("RIGHT_BLINKER", 0x326, 0),
# ("COUNTER", 0x324, 0),
# ("ENGINE_RPM", 0x17C, 0)
# ]
# checks = [
# (0x14a, 100), # address, frequency
# (0x158, 100),
# (0x17c, 100),
# (0x191, 100),
# (0x1a4, 50),
# (0x326, 10),
# (0x1b0, 50),
# (0x1d0, 50),
# (0x305, 10),
# (0x324, 10),
# (0x405, 3),
# ]
# cp = CANParser("honda_civic_touring_2016_can_generated", signals, checks, 0)
signals = [
# sig_name, sig_address, default
("GEAR", 956, 0x20),
("BRAKE_PRESSED", 548, 0),
("GAS_PEDAL", 705, 0),
("WHEEL_SPEED_FL", 170, 0),
("WHEEL_SPEED_FR", 170, 0),
("WHEEL_SPEED_RL", 170, 0),
("WHEEL_SPEED_RR", 170, 0),
("DOOR_OPEN_FL", 1568, 1),
("DOOR_OPEN_FR", 1568, 1),
("DOOR_OPEN_RL", 1568, 1),
("DOOR_OPEN_RR", 1568, 1),
("SEATBELT_DRIVER_UNLATCHED", 1568, 1),
("TC_DISABLED", 951, 1),
("STEER_ANGLE", 37, 0),
("STEER_FRACTION", 37, 0),
("STEER_RATE", 37, 0),
("GAS_RELEASED", 466, 0),
("CRUISE_STATE", 466, 0),
("MAIN_ON", 467, 0),
("SET_SPEED", 467, 0),
("STEER_TORQUE_DRIVER", 608, 0),
("STEER_TORQUE_EPS", 608, 0),
("TURN_SIGNALS", 1556, 3), # 3 is no blinkers
("LKA_STATE", 610, 0),
]
checks = [
(548, 40),
(705, 33),
(170, 80),
(37, 80),
(466, 33),
(608, 50),
]
cp = CANParser("toyota_rav4_2017_pt_generated", signals, checks, 0)
# print(cp.vl)
while True:
cp.update(int(sec_since_boot()*1e9), True)
# print(cp.vl)
print(cp.ts)
print(cp.can_valid)
time.sleep(0.01)
from selfdrive.can.parser_pyx import CANParser # pylint: disable=no-name-in-module, import-error
assert CANParser
selfdrive/can/parser_pyx.pxd
+92
View File
@@ -0,0 +1,92 @@
# distutils: language = c++
from libc.stdint cimport uint32_t, uint64_t, uint16_t
from libcpp.vector cimport vector
from libcpp.map cimport map
from libcpp.string cimport string
from libcpp.unordered_set cimport unordered_set
from libcpp cimport bool
ctypedef enum SignalType:
DEFAULT,
HONDA_CHECKSUM,
HONDA_COUNTER,
TOYOTA_CHECKSUM,
PEDAL_CHECKSUM,
PEDAL_COUNTER
cdef struct Signal:
const char* name
int b1, b2, bo
bool is_signed
double factor, offset
SignalType type
cdef struct Msg:
const char* name
uint32_t address
unsigned int size
size_t num_sigs
const Signal *sigs
cdef struct Val:
const char* name
uint32_t address
const char* def_val
const Signal *sigs
cdef struct DBC:
const char* name
size_t num_msgs
const Msg *msgs
const Val *vals
size_t num_vals
cdef struct SignalParseOptions:
uint32_t address
const char* name
double default_value
cdef struct MessageParseOptions:
uint32_t address
int check_frequency
cdef struct SignalValue:
uint32_t address
uint16_t ts
const char* name
double value
ctypedef const DBC * (*dbc_lookup_func)(const char* dbc_name)
ctypedef void* (*can_init_with_vectors_func)(int bus, const char* dbc_name,
vector[MessageParseOptions] message_options,
vector[SignalParseOptions] signal_options,
bool sendcan,
const char* tcp_addr,
int timeout)
ctypedef int (*can_update_func)(void* can, uint64_t sec, bool wait);
ctypedef size_t (*can_query_func)(void* can, uint64_t sec, bool *out_can_valid, size_t out_values_size, SignalValue* out_values);
ctypedef void (*can_query_vector_func)(void* can, uint64_t sec, bool *out_can_valid, vector[SignalValue] &values)
cdef class CANParser:
cdef:
void *can
const DBC *dbc
dbc_lookup_func dbc_lookup
can_init_with_vectors_func can_init_with_vectors
can_update_func can_update
can_query_vector_func can_query_vector
map[string, uint32_t] msg_name_to_address
map[uint32_t, string] address_to_msg_name
vector[SignalValue] can_values
bool test_mode_enabled
cdef public:
string dbc_name
dict vl
dict ts
bool can_valid
int can_invalid_cnt
cdef unordered_set[uint32_t] update_vl(self, uint64_t sec)
selfdrive/can/parser_pyx.pyx
+105
View File
@@ -0,0 +1,105 @@
# distutils: language = c++
from posix.dlfcn cimport dlopen, dlsym, RTLD_LAZY
from libcpp cimport bool
import os
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="127.0.0.1", 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")
cdef void *libdbc = dlopen(libdbc_fn, RTLD_LAZY)
self.can_init_with_vectors = <can_init_with_vectors_func>dlsym(libdbc, 'can_init_with_vectors')
self.dbc_lookup = <dbc_lookup_func>dlsym(libdbc, 'dbc_lookup')
self.can_update = <can_update_func>dlsym(libdbc, 'can_update')
self.can_query_vector = <can_query_vector_func>dlsym(libdbc, 'can_query_vector')
if checks is None:
checks = []
self.can_valid = True
self.dbc_name = dbc_name
self.dbc = self.dbc_lookup(dbc_name)
self.vl = {}
self.ts = {}
self.can_invalid_cnt = CAN_INVALID_CNT
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)
self.vl[msg.address] = {}
self.vl[str(msg.name)] = {}
self.ts[msg.address] = {}
self.ts[str(msg.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])
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])
checks[i] = c
cdef vector[SignalParseOptions] signal_options_v
cdef SignalParseOptions spo
for sig_name, sig_address, sig_default in signals:
spo.address = sig_address
spo.name = sig_name
spo.default_value = sig_default
signal_options_v.push_back(spo)
message_options = dict((address, 0) for _, address, _ in signals)
message_options.update(dict(checks))
cdef vector[MessageParseOptions] message_options_v
cdef MessageParseOptions mpo
for msg_address, freq in message_options.items():
mpo.address = msg_address
mpo.check_frequency = freq
message_options_v.push_back(mpo)
self.can = self.can_init_with_vectors(bus, dbc_name, message_options_v, signal_options_v, sendcan, tcp_addr, timeout)
self.update_vl(0)
cdef unordered_set[uint32_t] update_vl(self, uint64_t sec):
cdef string sig_name
cdef unordered_set[uint32_t] updated_val
cdef bool valid = False
self.can_query_vector(self.can, sec, &valid, self.can_values)
# Update invalid flag
self.can_invalid_cnt += 1
if valid:
self.can_invalid_cnt = 0
self.can_valid = self.can_invalid_cnt < CAN_INVALID_CNT
for cv in self.can_values:
self.vl[cv.address][string(cv.name)] = cv.value
self.ts[cv.address][string(cv.name)] = cv.ts
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
updated_val.insert(cv.address)
return updated_val
def update(self, uint64_t sec, bool wait):
r = (self.can_update(self.can, sec, wait) >= 0)
updated_val = self.update_vl(sec)
return r, updated_val
selfdrive/can/parser_pyx_setup.py
+18
View File
@@ -0,0 +1,18 @@
from distutils.core import setup, Extension
from Cython.Build import cythonize
import subprocess
sourcefiles = ['parser_pyx.pyx']
extra_compile_args = ["-std=c++11"]
ARCH = subprocess.check_output(["uname", "-m"]).rstrip()
if ARCH == "aarch64":
extra_compile_args += ["-Wno-deprecated-register"]
setup(name='Radard Thread',
ext_modules=cythonize(
Extension(
"parser_pyx",
sources=sourcefiles,
extra_compile_args=extra_compile_args
),
nthreads=4))
selfdrive/can/process_dbc.py
+1 -1
View File
@@ -1,4 +1,4 @@
#!/usr/bin/env python
#!/usr/bin/env python2
import os
import glob
import sys
selfdrive/can/tests/parser_old.py
+220
View File
@@ -0,0 +1,220 @@
import time
import numbers
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):
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 = []
self.can_valid = True
self.can_invalid_cnt = CAN_INVALID_CNT
self.vl = {}
self.ts = {}
self.dbc_name = dbc_name
self.dbc = libdbc.dbc_lookup(dbc_name)
self.msg_name_to_addres = {}
self.address_to_msg_name = {}
num_msgs = self.dbc[0].num_msgs
for i in range(num_msgs):
msg = self.dbc[0].msgs[i]
name = ffi.string(msg.name)
address = msg.address
self.msg_name_to_addres[name] = address
self.address_to_msg_name[address] = name
self.vl[address] = {}
self.vl[name] = {}
self.ts[address] = {}
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_addres[s[1]], 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_addres[c[0]], c[1])
checks[i] = c
sig_names = dict((name, ffi.new("char[]", name)) for name, _, _ in signals)
signal_options_c = ffi.new("SignalParseOptions[]", [
{
'address': sig_address,
'name': sig_names[sig_name],
'default_value': sig_default,
} for sig_name, sig_address, sig_default in signals])
message_options = dict((address, 0) for _, address, _ in signals)
message_options.update(dict(checks))
message_options_c = ffi.new("MessageParseOptions[]", [
{
'address': msg_address,
'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.p_can_valid = ffi.new("bool*")
value_count = libdbc.can_query(self.can, 0, self.p_can_valid, 0, ffi.NULL)
self.can_values = ffi.new("SignalValue[%d]" % value_count)
self.update_vl(0)
def update_vl(self, sec):
can_values_len = libdbc.can_query(self.can, sec, self.p_can_valid, len(self.can_values), self.can_values)
assert can_values_len <= len(self.can_values)
self.can_invalid_cnt += 1
if self.p_can_valid[0]:
self.can_invalid_cnt = 0
self.can_valid = self.can_invalid_cnt < CAN_INVALID_CNT
ret = set()
for i in xrange(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)
self.vl[address][name] = cv.value
self.ts[address][name] = cv.ts
sig_name = self.address_to_msg_name[address]
self.vl[sig_name][name] = cv.value
self.ts[sig_name][name] = cv.ts
ret.add(address)
return ret
def update(self, sec, wait):
"""Returns if the update was successfull (e.g. no rcv timeout happened)"""
r = libdbc.can_update(self.can, sec, wait)
return bool(r >= 0), self.update_vl(sec)
if __name__ == "__main__":
from common.realtime import sec_since_boot
radar_messages = range(0x430, 0x43A) + range(0x440, 0x446)
# signals = zip(['LONG_DIST'] * 16 + ['NEW_TRACK'] * 16 + ['LAT_DIST'] * 16 +
# ['REL_SPEED'] * 16, radar_messages * 4,
# [255] * 16 + [1] * 16 + [0] * 16 + [0] * 16)
# checks = zip(radar_messages, [20]*16)
# cp = CANParser("acura_ilx_2016_nidec", signals, checks, 1)
# signals = [
# ("XMISSION_SPEED", 0x158, 0), #sig_name, sig_address, default
# ("WHEEL_SPEED_FL", 0x1d0, 0),
# ("WHEEL_SPEED_FR", 0x1d0, 0),
# ("WHEEL_SPEED_RL", 0x1d0, 0),
# ("STEER_ANGLE", 0x14a, 0),
# ("STEER_TORQUE_SENSOR", 0x18f, 0),
# ("GEAR", 0x191, 0),
# ("WHEELS_MOVING", 0x1b0, 1),
# ("DOOR_OPEN_FL", 0x405, 1),
# ("DOOR_OPEN_FR", 0x405, 1),
# ("DOOR_OPEN_RL", 0x405, 1),
# ("DOOR_OPEN_RR", 0x405, 1),
# ("CRUISE_SPEED_PCM", 0x324, 0),
# ("SEATBELT_DRIVER_LAMP", 0x305, 1),
# ("SEATBELT_DRIVER_LATCHED", 0x305, 0),
# ("BRAKE_PRESSED", 0x17c, 0),
# ("CAR_GAS", 0x130, 0),
# ("CRUISE_BUTTONS", 0x296, 0),
# ("ESP_DISABLED", 0x1a4, 1),
# ("HUD_LEAD", 0x30c, 0),
# ("USER_BRAKE", 0x1a4, 0),
# ("STEER_STATUS", 0x18f, 5),
# ("WHEEL_SPEED_RR", 0x1d0, 0),
# ("BRAKE_ERROR_1", 0x1b0, 1),
# ("BRAKE_ERROR_2", 0x1b0, 1),
# ("GEAR_SHIFTER", 0x191, 0),
# ("MAIN_ON", 0x326, 0),
# ("ACC_STATUS", 0x17c, 0),
# ("PEDAL_GAS", 0x17c, 0),
# ("CRUISE_SETTING", 0x296, 0),
# ("LEFT_BLINKER", 0x326, 0),
# ("RIGHT_BLINKER", 0x326, 0),
# ("COUNTER", 0x324, 0),
# ("ENGINE_RPM", 0x17C, 0)
# ]
# checks = [
# (0x14a, 100), # address, frequency
# (0x158, 100),
# (0x17c, 100),
# (0x191, 100),
# (0x1a4, 50),
# (0x326, 10),
# (0x1b0, 50),
# (0x1d0, 50),
# (0x305, 10),
# (0x324, 10),
# (0x405, 3),
# ]
# cp = CANParser("honda_civic_touring_2016_can_generated", signals, checks, 0)
signals = [
# sig_name, sig_address, default
("GEAR", 956, 0x20),
("BRAKE_PRESSED", 548, 0),
("GAS_PEDAL", 705, 0),
("WHEEL_SPEED_FL", 170, 0),
("WHEEL_SPEED_FR", 170, 0),
("WHEEL_SPEED_RL", 170, 0),
("WHEEL_SPEED_RR", 170, 0),
("DOOR_OPEN_FL", 1568, 1),
("DOOR_OPEN_FR", 1568, 1),
("DOOR_OPEN_RL", 1568, 1),
("DOOR_OPEN_RR", 1568, 1),
("SEATBELT_DRIVER_UNLATCHED", 1568, 1),
("TC_DISABLED", 951, 1),
("STEER_ANGLE", 37, 0),
("STEER_FRACTION", 37, 0),
("STEER_RATE", 37, 0),
("GAS_RELEASED", 466, 0),
("CRUISE_STATE", 466, 0),
("MAIN_ON", 467, 0),
("SET_SPEED", 467, 0),
("STEER_TORQUE_DRIVER", 608, 0),
("STEER_TORQUE_EPS", 608, 0),
("TURN_SIGNALS", 1556, 3), # 3 is no blinkers
("LKA_STATE", 610, 0),
]
checks = [
(548, 40),
(705, 33),
(170, 80),
(37, 80),
(466, 33),
(608, 50),
]
cp = CANParser("toyota_rav4_2017_pt_generated", signals, checks, 0)
# print(cp.vl)
while True:
cp.update(int(sec_since_boot()*1e9), True)
# print(cp.vl)
print(cp.ts)
print(cp.can_valid)
time.sleep(0.01)
selfdrive/can/tests/test_parser.py
Executable
+96
View File
@@ -0,0 +1,96 @@
#!/usr/bin/env python2
import os
import unittest
import requests
import selfdrive.messaging as messaging
from selfdrive.can.parser import CANParser as CANParserNew
from selfdrive.can.tests.parser_old import CANParser as CANParserOld
from selfdrive.car.honda.carstate import get_can_signals
from selfdrive.car.honda.interface import CarInterface
from selfdrive.car.honda.values import CAR, DBC
from selfdrive.services import service_list
from tools.lib.logreader import LogReader
BASE_URL = "https://commadataci.blob.core.windows.net/openpilotci/"
DT = int(0.01 * 1e9) # ns
def dict_keys_differ(dict1, dict2):
keys1 = set(dict1.keys())
keys2 = set(dict2.keys())
if keys1 != keys2:
return True
for k in keys1:
keys1 = set(dict1[k].keys())
keys2 = set(dict2[k].keys())
if keys1 != keys2:
return True
return False
def dicts_vals_differ(dict1, dict2):
for k_outer in dict1.keys():
for k_inner in dict1[k_outer].keys():
if dict1[k_outer][k_inner] != dict2[k_outer][k_inner]:
return True
return False
def run_route(route):
can = messaging.pub_sock(service_list['can'].port)
CP = CarInterface.get_params(CAR.CIVIC, {})
signals, checks = get_can_signals(CP)
parser_old = CANParserOld(DBC[CP.carFingerprint]['pt'], signals, checks, 0, timeout=-1)
parser_new = CANParserNew(DBC[CP.carFingerprint]['pt'], signals, checks, 0, timeout=-1)
if dict_keys_differ(parser_old.vl, parser_new.vl):
return False
lr = LogReader(route + ".bz2")
route_ok = True
t = 0
for msg in lr:
if msg.which() == 'can':
t += DT
can.send(msg.as_builder().to_bytes())
_, updated_old = parser_old.update(t, True)
_, updated_new = parser_new.update(t, True)
if updated_old != updated_new:
route_ok = False
print(t, "Diff in seen")
if dicts_vals_differ(parser_old.vl, parser_new.vl):
print(t, "Diff in dict")
route_ok = False
return route_ok
class TestCanParser(unittest.TestCase):
def setUp(self):
self.routes = {
CAR.CIVIC: "b0c9d2329ad1606b|2019-05-30--20-23-57"
}
for route in self.routes.values():
route_filename = route + ".bz2"
if not os.path.isfile(route_filename):
with open(route + ".bz2", "w") as f:
f.write(requests.get(BASE_URL + route_filename).content)
def test_parser_civic(self):
self.assertTrue(run_route(self.routes[CAR.CIVIC]))
if __name__ == "__main__":
unittest.main()
selfdrive/car/__init__.py
+30
View File
@@ -1,7 +1,37 @@
# functions common among cars
from common.numpy_fast import clip
# kg of standard extra cargo to count for drive, gas, etc...
STD_CARGO_KG = 136.
# FIXME: hardcoding honda civic 2016 touring params so they can be used to
# scale unknown params for other cars
class CivicParams:
MASS = 1326. + STD_CARGO_KG
WHEELBASE = 2.70
CENTER_TO_FRONT = WHEELBASE * 0.4
CENTER_TO_REAR = WHEELBASE - CENTER_TO_FRONT
ROTATIONAL_INERTIA = 2500
TIRE_STIFFNESS_FRONT = 192150
TIRE_STIFFNESS_REAR = 202500
# TODO: get actual value, for now starting with reasonable value for
# civic and scaling by mass and wheelbase
def scale_rot_inertia(mass, wheelbase):
return CivicParams.ROTATIONAL_INERTIA * mass * wheelbase ** 2 / (CivicParams.MASS * CivicParams.WHEELBASE ** 2)
# TODO: start from empirically derived lateral slip stiffness for the civic and scale by
# mass and CG position, so all cars will have approximately similar dyn behaviors
def scale_tire_stiffness(mass, wheelbase, center_to_front, tire_stiffness_factor=1.0):
center_to_rear = wheelbase - center_to_front
tire_stiffness_front = (CivicParams.TIRE_STIFFNESS_FRONT * tire_stiffness_factor) * mass / CivicParams.MASS * \
(center_to_rear / wheelbase) / (CivicParams.CENTER_TO_REAR / CivicParams.WHEELBASE)
tire_stiffness_rear = (CivicParams.TIRE_STIFFNESS_REAR * tire_stiffness_factor) * mass / CivicParams.MASS * \
(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}
selfdrive/car/car_helpers.py
+32 -31
View File
@@ -1,8 +1,6 @@
import os
import time
from common.vin import is_vin_response_valid
from common.basedir import BASEDIR
from common.realtime import sec_since_boot
from common.fingerprints import eliminate_incompatible_cars, all_known_cars
from selfdrive.boardd.boardd import can_list_to_can_capnp
from selfdrive.swaglog import cloudlog
@@ -64,7 +62,7 @@ def fingerprint(logcan, sendcan):
finger = {}
cloudlog.warning("waiting for fingerprint...")
candidate_cars = all_known_cars()
can_seen_ts = None
can_seen_frame = None
can_seen = False
# works on standard 11-bit addresses for diagnostic. Tested on Toyota and Subaru;
@@ -80,52 +78,55 @@ def fingerprint(logcan, sendcan):
vin_dat = []
vin = ""
while 1:
for a in messaging.drain_sock(logcan):
for can in a.can:
can_seen = True
frame = 0
while True:
a = messaging.recv_one(logcan)
# have we got a VIN query response?
if can.src == 0 and can.address == 0x7e8:
vin_never_responded = False
# basic sanity checks on ISO-TP response
if is_vin_response_valid(can.dat, vin_step, vin_cnt):
vin_dat += can.dat[2:] if vin_step == 0 else can.dat[1:]
vin_cnt += 1
if vin_cnt == vin_cnts[vin_step]:
vin_responded = True
vin_step += 1
for can in a.can:
can_seen = True
# ignore everything not on bus 0 and with more than 11 bits,
# which are ussually sporadic and hard to include in fingerprints.
# also exclude VIN query response on 0x7e8
if can.src == 0 and can.address < 0x800 and can.address != 0x7e8:
finger[can.address] = len(can.dat)
candidate_cars = eliminate_incompatible_cars(can, candidate_cars)
# have we got a VIN query response?
if can.src == 0 and can.address == 0x7e8:
vin_never_responded = False
# basic sanity checks on ISO-TP response
if is_vin_response_valid(can.dat, vin_step, vin_cnt):
vin_dat += can.dat[2:] if vin_step == 0 else can.dat[1:]
vin_cnt += 1
if vin_cnt == vin_cnts[vin_step]:
vin_responded = True
vin_step += 1
# ignore everything not on bus 0 and with more than 11 bits,
# which are ussually sporadic and hard to include in fingerprints.
# also exclude VIN query response on 0x7e8
if can.src == 0 and can.address < 0x800 and can.address != 0x7e8:
finger[can.address] = len(can.dat)
candidate_cars = eliminate_incompatible_cars(can, candidate_cars)
if can_seen_frame is None and can_seen:
can_seen_frame = frame
if can_seen_ts is None and can_seen:
can_seen_ts = sec_since_boot() # start time
ts = sec_since_boot()
# if we only have one car choice and the time_fingerprint since we got our first
# message has elapsed, exit. Toyota needs higher time_fingerprint, since DSU does not
# broadcast immediately
if len(candidate_cars) == 1 and can_seen_ts is not None:
if len(candidate_cars) == 1 and can_seen_frame is not None:
time_fingerprint = 1.0 if ("TOYOTA" in candidate_cars[0] or "LEXUS" in candidate_cars[0]) else 0.1
if (ts - can_seen_ts) > time_fingerprint:
if (frame - can_seen_frame) > (time_fingerprint * 100):
break
# bail if no cars left or we've been waiting for more than 2s since can_seen
elif len(candidate_cars) == 0 or (can_seen_ts is not None and (ts - can_seen_ts) > 2.):
elif len(candidate_cars) == 0 or (can_seen_frame is not None and (frame - can_seen_frame) > 200):
return None, finger, ""
# keep sending VIN qury if ECU isn't responsing.
# sendcan is probably not ready due to the zmq slow joiner syndrome
if can_seen and (vin_never_responded or (vin_responded and vin_step < len(vin_cnts))):
# TODO: VIN query temporarily disabled until we have the harness
if False and can_seen and (vin_never_responded or (vin_responded and vin_step < len(vin_cnts))):
sendcan.send(can_list_to_can_capnp([vin_query_msg[vin_step]], msgtype='sendcan'))
vin_responded = False
vin_cnt = 0
time.sleep(0.01)
frame += 1
# only report vin if procedure is finished
if vin_step == len(vin_cnts) and vin_cnt == vin_cnts[-1]:
selfdrive/car/chrysler/carstate.py
-2
View File
@@ -97,8 +97,6 @@ class CarState(object):
def update(self, cp, cp_cam):
# copy can_valid
self.can_valid = cp.can_valid
# update prevs, update must run once per loop
self.prev_left_blinker_on = self.left_blinker_on
selfdrive/car/chrysler/interface.py
+9 -36
View File
@@ -6,6 +6,7 @@ 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
class CarInterface(object):
@@ -14,7 +15,6 @@ class CarInterface(object):
self.VM = VehicleModel(CP)
self.frame = 0
self.can_invalid_count = 0
self.gas_pressed_prev = False
self.brake_pressed_prev = False
self.cruise_enabled_prev = False
@@ -40,34 +40,21 @@ class CarInterface(object):
@staticmethod
def get_params(candidate, fingerprint, vin=""):
# kg of standard extra cargo to count for drive, gas, etc...
std_cargo = 136
ret = car.CarParams.new_message()
ret.carName = "chrysler"
ret.carFingerprint = candidate
ret.carVin = vin
ret.safetyModel = car.CarParams.SafetyModels.chrysler
ret.safetyModel = car.CarParams.SafetyModel.chrysler
# pedal
ret.enableCruise = True
# FIXME: hardcoding honda civic 2016 touring params so they can be used to
# scale unknown params for other cars
mass_civic = 2923./2.205 + std_cargo
wheelbase_civic = 2.70
centerToFront_civic = wheelbase_civic * 0.4
centerToRear_civic = wheelbase_civic - centerToFront_civic
rotationalInertia_civic = 2500
tireStiffnessFront_civic = 85400 * 2.0
tireStiffnessRear_civic = 90000 * 2.0
# Speed conversion: 20, 45 mph
ret.wheelbase = 3.089 # in meters for Pacifica Hybrid 2017
ret.steerRatio = 16.2 # Pacifica Hybrid 2017
ret.mass = 2858 + std_cargo # kg curb weight Pacifica Hybrid 2017
ret.mass = 2858. + STD_CARGO_KG # kg curb weight Pacifica Hybrid 2017
ret.lateralTuning.pid.kpBP, ret.lateralTuning.pid.kiBP = [[9., 20.], [9., 20.]]
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.15,0.30], [0.03,0.05]]
ret.lateralTuning.pid.kf = 0.00006 # full torque for 10 deg at 80mph means 0.00007818594
@@ -88,20 +75,13 @@ class CarInterface(object):
ret.minSteerSpeed = 17.5 # m/s 17 on the way up, 13 on the way down once engaged.
# TODO allow 2019 cars to steer down to 13 m/s if already engaged.
centerToRear = ret.wheelbase - ret.centerToFront
# TODO: get actual value, for now starting with reasonable value for
# civic and scaling by mass and wheelbase
ret.rotationalInertia = rotationalInertia_civic * \
ret.mass * ret.wheelbase**2 / (mass_civic * wheelbase_civic**2)
ret.rotationalInertia = scale_rot_inertia(ret.mass, ret.wheelbase)
# TODO: start from empirically derived lateral slip stiffness for the civic and scale by
# mass and CG position, so all cars will have approximately similar dyn behaviors
ret.tireStiffnessFront = tireStiffnessFront_civic * \
ret.mass / mass_civic * \
(centerToRear / ret.wheelbase) / (centerToRear_civic / wheelbase_civic)
ret.tireStiffnessRear = tireStiffnessRear_civic * \
ret.mass / mass_civic * \
(ret.centerToFront / ret.wheelbase) / (centerToFront_civic / wheelbase_civic)
ret.tireStiffnessFront, ret.tireStiffnessRear = scale_tire_stiffness(ret.mass, ret.wheelbase, ret.centerToFront)
# no rear steering, at least on the listed cars above
ret.steerRatioRear = 0.
@@ -135,15 +115,16 @@ class CarInterface(object):
def update(self, c):
# ******************* do can recv *******************
canMonoTimes = []
can_valid, _ = self.cp.update(int(sec_since_boot() * 1e9), True)
cam_valid, _ = self.cp_cam.update(int(sec_since_boot() * 1e9), False)
can_rcv_error = not can_valid or not cam_valid
can_rcv_valid, _ = self.cp.update(int(sec_since_boot() * 1e9), True)
cam_rcv_valid, _ = self.cp_cam.update(int(sec_since_boot() * 1e9), False)
self.CS.update(self.cp, self.cp_cam)
# create message
ret = car.CarState.new_message()
ret.canValid = can_rcv_valid and cam_rcv_valid and self.cp.can_valid and self.cp_cam.can_valid
# speeds
ret.vEgo = self.CS.v_ego
ret.vEgoRaw = self.CS.v_ego_raw
@@ -212,14 +193,6 @@ class CarInterface(object):
# events
events = []
if not self.CS.can_valid:
self.can_invalid_count += 1
else:
self.can_invalid_count = 0
if can_rcv_error or self.can_invalid_count >= 5:
events.append(create_event('commIssue', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
if not (ret.gearShifter in ('drive', 'low')):
events.append(create_event('wrongGear', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
if ret.doorOpen:
selfdrive/car/chrysler/radar_interface.py
+1 -7
View File
@@ -3,10 +3,6 @@ import os
from selfdrive.can.parser import CANParser
from cereal import car
from common.realtime import sec_since_boot
import zmq
from selfdrive.services import service_list
import selfdrive.messaging as messaging
RADAR_MSGS_C = range(0x2c2, 0x2d4+2, 2) # c_ messages 706,...,724
RADAR_MSGS_D = range(0x2a2, 0x2b4+2, 2) # d_ messages
@@ -55,8 +51,6 @@ class RadarInterface(object):
self.pts = {}
self.delay = 0.0 # Delay of radar #TUNE
self.rcp = _create_radar_can_parser()
context = zmq.Context()
self.logcan = messaging.sub_sock(context, service_list['can'].port)
def update(self):
canMonoTimes = []
@@ -73,7 +67,7 @@ class RadarInterface(object):
ret = car.RadarData.new_message()
errors = []
if not self.rcp.can_valid:
errors.append("commIssue")
errors.append("canError")
ret.errors = errors
ret.canMonoTimes = canMonoTimes
selfdrive/car/chrysler/values.py
+2
View File
@@ -28,6 +28,8 @@ FINGERPRINTS = {
],
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},
# 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},
selfdrive/car/ford/carstate.py
-3
View File
@@ -53,9 +53,6 @@ class CarState(object):
self.v_ego = 0.0
def update(self, cp):
# copy can_valid
self.can_valid = cp.can_valid
# update prevs, update must run once per loop
self.prev_left_blinker_on = self.left_blinker_on
self.prev_right_blinker_on = self.right_blinker_on
selfdrive/car/ford/interface.py
+10 -40
View File
@@ -7,6 +7,7 @@ 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
class CarInterface(object):
@@ -15,7 +16,6 @@ class CarInterface(object):
self.VM = VehicleModel(CP)
self.frame = 0
self.can_invalid_count = 0
self.gas_pressed_prev = False
self.brake_pressed_prev = False
self.cruise_enabled_prev = False
@@ -40,64 +40,40 @@ class CarInterface(object):
@staticmethod
def get_params(candidate, fingerprint, vin=""):
# kg of standard extra cargo to count for drive, gas, etc...
std_cargo = 136
ret = car.CarParams.new_message()
ret.carName = "ford"
ret.carFingerprint = candidate
ret.carVin = vin
ret.safetyModel = car.CarParams.SafetyModels.ford
ret.safetyModel = car.CarParams.SafetyModel.ford
# pedal
ret.enableCruise = True
# FIXME: hardcoding honda civic 2016 touring params so they can be used to
# scale unknown params for other cars
mass_civic = 2923. * CV.LB_TO_KG + std_cargo
wheelbase_civic = 2.70
centerToFront_civic = wheelbase_civic * 0.4
centerToRear_civic = wheelbase_civic - centerToFront_civic
rotationalInertia_civic = 2500
tireStiffnessFront_civic = 85400
tireStiffnessRear_civic = 90000
ret.wheelbase = 2.85
ret.steerRatio = 14.8
ret.mass = 3045. * CV.LB_TO_KG + std_cargo
ret.mass = 3045. * CV.LB_TO_KG + STD_CARGO_KG
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]]
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.01], [0.005]] # TODO: tune this
ret.lateralTuning.pid.kf = 1. / MAX_ANGLE # MAX Steer angle to normalize FF
ret.steerActuatorDelay = 0.1 # Default delay, not measured yet
ret.steerRateCost = 1.0
f = 1.2
tireStiffnessFront_civic *= f
tireStiffnessRear_civic *= f
ret.centerToFront = ret.wheelbase * 0.44
tire_stiffness_factor = 0.5328
# 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.
centerToRear = ret.wheelbase - ret.centerToFront
# TODO: get actual value, for now starting with reasonable value for
# civic and scaling by mass and wheelbase
ret.rotationalInertia = rotationalInertia_civic * \
ret.mass * ret.wheelbase**2 / (mass_civic * wheelbase_civic**2)
ret.rotationalInertia = scale_rot_inertia(ret.mass, ret.wheelbase)
# TODO: start from empirically derived lateral slip stiffness for the civic and scale by
# mass and CG position, so all cars will have approximately similar dyn behaviors
ret.tireStiffnessFront = tireStiffnessFront_civic * \
ret.mass / mass_civic * \
(centerToRear / ret.wheelbase) / (centerToRear_civic / wheelbase_civic)
ret.tireStiffnessRear = tireStiffnessRear_civic * \
ret.mass / mass_civic * \
(ret.centerToFront / ret.wheelbase) / (centerToFront_civic / wheelbase_civic)
ret.tireStiffnessFront, ret.tireStiffnessRear = scale_tire_stiffness(ret.mass, ret.wheelbase, ret.centerToFront,
tire_stiffness_factor=tire_stiffness_factor)
# no rear steering, at least on the listed cars above
ret.steerRatioRear = 0.
@@ -133,14 +109,15 @@ class CarInterface(object):
# ******************* do can recv *******************
canMonoTimes = []
can_valid, _ = self.cp.update(int(sec_since_boot() * 1e9), True)
can_rcv_error = not can_valid
can_rcv_valid, _ = self.cp.update(int(sec_since_boot() * 1e9), True)
self.CS.update(self.cp)
# create message
ret = car.CarState.new_message()
ret.canValid = can_rcv_valid and self.cp.can_valid
# speeds
ret.vEgo = self.CS.v_ego
ret.vEgoRaw = self.CS.v_ego_raw
@@ -168,13 +145,6 @@ class CarInterface(object):
# events
events = []
if not self.CS.can_valid:
self.can_invalid_count += 1
else:
self.can_invalid_count = 0
if can_rcv_error or self.can_invalid_count >= 5:
events.append(create_event('commIssue', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
if self.CS.steer_error:
events.append(create_event('steerUnavailable', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE, ET.PERMANENT]))
selfdrive/car/ford/radar_interface.py
+1 -8
View File
@@ -4,10 +4,6 @@ import numpy as np
from selfdrive.can.parser import CANParser
from cereal import car
from common.realtime import sec_since_boot
import zmq
from selfdrive.services import service_list
import selfdrive.messaging as messaging
RADAR_MSGS = range(0x500, 0x540)
@@ -33,9 +29,6 @@ class RadarInterface(object):
# Nidec
self.rcp = _create_radar_can_parser()
context = zmq.Context()
self.logcan = messaging.sub_sock(context, service_list['can'].port)
def update(self):
canMonoTimes = []
@@ -52,7 +45,7 @@ class RadarInterface(object):
ret = car.RadarData.new_message()
errors = []
if not self.rcp.can_valid:
errors.append("commIssue")
errors.append("canError")
ret.errors = errors
ret.canMonoTimes = canMonoTimes
selfdrive/car/gm/carcontroller.py
+5 -4
View File
@@ -1,6 +1,6 @@
from cereal import car
from common.realtime import DT_CTRL
from common.numpy_fast import interp
from common.realtime import sec_since_boot
from selfdrive.config import Conversions as CV
from selfdrive.car import apply_std_steer_torque_limits
from selfdrive.car.gm import gmcan
@@ -9,6 +9,7 @@ from selfdrive.can.packer import CANPacker
VisualAlert = car.CarControl.HUDControl.VisualAlert
class CarControllerParams():
def __init__(self, car_fingerprint):
if car_fingerprint in SUPERCRUISE_CARS:
@@ -47,7 +48,7 @@ class CarControllerParams():
def actuator_hystereses(final_pedal, pedal_steady):
# hyst params... TODO: move these to VehicleParams
pedal_hyst_gap = 0.01 # don't change pedal command for small oscilalitons within this value
pedal_hyst_gap = 0.01 # don't change pedal command for small oscillations within this value
# for small pedal oscillations within pedal_hyst_gap, don't change the pedal command
if final_pedal == 0.:
@@ -70,7 +71,7 @@ def process_hud_alert(hud_alert):
class CarController(object):
def __init__(self, canbus, car_fingerprint):
self.pedal_steady = 0.
self.start_time = sec_since_boot()
self.start_time = 0.
self.chime = 0
self.steer_idx = 0
self.apply_steer_last = 0
@@ -154,7 +155,7 @@ class CarController(object):
# Radar needs to know current speed and yaw rate (50hz),
# and that ADAS is alive (10hz)
time_and_headlights_step = 10
tt = sec_since_boot()
tt = frame * DT_CTRL
if frame % time_and_headlights_step == 0:
idx = (frame // time_and_headlights_step) % 4
selfdrive/car/gm/carstate.py
-3
View File
@@ -71,8 +71,6 @@ class CarState(object):
self.v_ego = 0.
def update(self, pt_cp):
self.can_valid = pt_cp.can_valid
self.prev_cruise_buttons = self.cruise_buttons
self.cruise_buttons = pt_cp.vl["ASCMSteeringButton"]['ACCButtons']
@@ -118,7 +116,6 @@ class CarState(object):
self.steer_error = False
self.brake_error = False
self.can_valid = True
self.prev_left_blinker_on = self.left_blinker_on
self.prev_right_blinker_on = self.right_blinker_on
selfdrive/car/gm/interface.py
+23 -49
View File
@@ -7,6 +7,7 @@ from selfdrive.controls.lib.vehicle_model import VehicleModel
from selfdrive.car.gm.values import DBC, CAR, STOCK_CONTROL_MSGS, AUDIO_HUD, \
SUPERCRUISE_CARS, AccState
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
class CanBus(object):
@@ -23,7 +24,6 @@ class CarInterface(object):
self.frame = 0
self.gas_pressed_prev = False
self.brake_pressed_prev = False
self.can_invalid_count = 0
self.acc_active_prev = 0
# *** init the major players ***
@@ -60,15 +60,13 @@ class CarInterface(object):
# 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)
ret.openpilotLongitudinalControl = ret.enableCamera
std_cargo = 136
tire_stiffness_factor = 0.444 # not optimized yet
if candidate == CAR.VOLT:
# supports stop and go, but initial engage must be above 18mph (which include conservatism)
ret.minEnableSpeed = 18 * CV.MPH_TO_MS
# kg of standard extra cargo to count for driver, gas, etc...
ret.mass = 1607. + std_cargo
ret.safetyModel = car.CarParams.SafetyModels.gm
ret.mass = 1607. + STD_CARGO_KG
ret.safetyModel = car.CarParams.SafetyModel.gm
ret.wheelbase = 2.69
ret.steerRatio = 15.7
ret.steerRatioRear = 0.
@@ -77,28 +75,27 @@ class CarInterface(object):
elif candidate == CAR.MALIBU:
# supports stop and go, but initial engage must be above 18mph (which include conservatism)
ret.minEnableSpeed = 18 * CV.MPH_TO_MS
ret.mass = 1496. + std_cargo
ret.safetyModel = car.CarParams.SafetyModels.gm
ret.mass = 1496. + STD_CARGO_KG
ret.safetyModel = car.CarParams.SafetyModel.gm
ret.wheelbase = 2.83
ret.steerRatio = 15.8
ret.steerRatioRear = 0.
ret.centerToFront = ret.wheelbase * 0.4 # wild guess
elif candidate == CAR.HOLDEN_ASTRA:
# kg of standard extra cargo to count for driver, gas, etc...
ret.mass = 1363. + std_cargo
ret.mass = 1363. + STD_CARGO_KG
ret.wheelbase = 2.662
# Remaining parameters copied from Volt for now
ret.centerToFront = ret.wheelbase * 0.4
ret.minEnableSpeed = 18 * CV.MPH_TO_MS
ret.safetyModel = car.CarParams.SafetyModels.gm
ret.safetyModel = car.CarParams.SafetyModel.gm
ret.steerRatio = 15.7
ret.steerRatioRear = 0.
elif candidate == CAR.ACADIA:
ret.minEnableSpeed = -1. # engage speed is decided by pcm
ret.mass = 4353. * CV.LB_TO_KG + std_cargo
ret.safetyModel = car.CarParams.SafetyModels.gm
ret.mass = 4353. * CV.LB_TO_KG + STD_CARGO_KG
ret.safetyModel = car.CarParams.SafetyModel.gm
ret.wheelbase = 2.86
ret.steerRatio = 14.4 #end to end is 13.46
ret.steerRatioRear = 0.
@@ -106,8 +103,8 @@ class CarInterface(object):
elif candidate == CAR.BUICK_REGAL:
ret.minEnableSpeed = 18 * CV.MPH_TO_MS
ret.mass = 3779. * CV.LB_TO_KG + std_cargo # (3849+3708)/2
ret.safetyModel = car.CarParams.SafetyModels.gm
ret.mass = 3779. * CV.LB_TO_KG + STD_CARGO_KG # (3849+3708)/2
ret.safetyModel = car.CarParams.SafetyModel.gm
ret.wheelbase = 2.83 #111.4 inches in meters
ret.steerRatio = 14.4 # guess for tourx
ret.steerRatioRear = 0.
@@ -115,8 +112,8 @@ class CarInterface(object):
elif candidate == CAR.CADILLAC_ATS:
ret.minEnableSpeed = 18 * CV.MPH_TO_MS
ret.mass = 1601. + std_cargo
ret.safetyModel = car.CarParams.SafetyModels.gm
ret.mass = 1601. + STD_CARGO_KG
ret.safetyModel = car.CarParams.SafetyModel.gm
ret.wheelbase = 2.78
ret.steerRatio = 15.3
ret.steerRatioRear = 0.
@@ -125,39 +122,22 @@ class CarInterface(object):
elif candidate == CAR.CADILLAC_CT6:
# engage speed is decided by pcm
ret.minEnableSpeed = -1.
# kg of standard extra cargo to count for driver, gas, etc...
ret.mass = 4016. * CV.LB_TO_KG + std_cargo
ret.safetyModel = car.CarParams.SafetyModels.cadillac
ret.mass = 4016. * CV.LB_TO_KG + STD_CARGO_KG
ret.safetyModel = car.CarParams.SafetyModel.cadillac
ret.wheelbase = 3.11
ret.steerRatio = 14.6 # it's 16.3 without rear active steering
ret.steerRatioRear = 0. # TODO: there is RAS on this car!
ret.centerToFront = ret.wheelbase * 0.465
# hardcoding honda civic 2016 touring params so they can be used to
# scale unknown params for other cars
mass_civic = 2923. * CV.LB_TO_KG + std_cargo
wheelbase_civic = 2.70
centerToFront_civic = wheelbase_civic * 0.4
centerToRear_civic = wheelbase_civic - centerToFront_civic
rotationalInertia_civic = 2500
tireStiffnessFront_civic = 85400
tireStiffnessRear_civic = 90000
centerToRear = ret.wheelbase - ret.centerToFront
# TODO: get actual value, for now starting with reasonable value for
# civic and scaling by mass and wheelbase
ret.rotationalInertia = rotationalInertia_civic * \
ret.mass * ret.wheelbase**2 / (mass_civic * wheelbase_civic**2)
ret.rotationalInertia = scale_rot_inertia(ret.mass, ret.wheelbase)
# TODO: start from empirically derived lateral slip stiffness for the civic and scale by
# mass and CG position, so all cars will have approximately similar dyn behaviors
ret.tireStiffnessFront = tireStiffnessFront_civic * \
ret.mass / mass_civic * \
(centerToRear / ret.wheelbase) / (centerToRear_civic / wheelbase_civic)
ret.tireStiffnessRear = tireStiffnessRear_civic * \
ret.mass / mass_civic * \
(ret.centerToFront / ret.wheelbase) / (centerToFront_civic / wheelbase_civic)
ret.tireStiffnessFront, ret.tireStiffnessRear = scale_tire_stiffness(ret.mass, ret.wheelbase, ret.centerToFront,
tire_stiffness_factor=tire_stiffness_factor)
# same tuning for Volt and CT6 for now
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]]
@@ -191,13 +171,15 @@ class CarInterface(object):
# returns a car.CarState
def update(self, c):
can_valid, _ = self.pt_cp.update(int(sec_since_boot() * 1e9), True)
can_rcv_error = not can_valid
can_rcv_valid, _ = self.pt_cp.update(int(sec_since_boot() * 1e9), True)
self.CS.update(self.pt_cp)
# create message
ret = car.CarState.new_message()
ret.canValid = can_rcv_valid and self.pt_cp.can_valid
# speeds
ret.vEgo = self.CS.v_ego
ret.aEgo = self.CS.a_ego
@@ -275,14 +257,6 @@ class CarInterface(object):
ret.buttonEvents = buttonEvents
events = []
if not self.CS.can_valid:
self.can_invalid_count += 1
else:
self.can_invalid_count = 0
if can_rcv_error or self.can_invalid_count >= 5:
events.append(create_event('commIssue', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
if self.CS.steer_error:
events.append(create_event('steerUnavailable', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE, ET.PERMANENT]))
if self.CS.steer_not_allowed:
selfdrive/car/gm/radar_interface.py
+1 -7
View File
@@ -1,5 +1,4 @@
#!/usr/bin/env python
import zmq
import math
import time
import numpy as np
@@ -8,8 +7,6 @@ from selfdrive.can.parser import CANParser
from selfdrive.car.gm.interface import CanBus
from selfdrive.car.gm.values import DBC, CAR
from common.realtime import sec_since_boot
from selfdrive.services import service_list
import selfdrive.messaging as messaging
RADAR_HEADER_MSG = 1120
SLOT_1_MSG = RADAR_HEADER_MSG + 1
@@ -55,9 +52,6 @@ class RadarInterface(object):
print "Using %d as obstacle CAN bus ID" % canbus.obstacle
self.rcp = create_radar_can_parser(canbus, CP.carFingerprint)
context = zmq.Context()
self.logcan = messaging.sub_sock(context, service_list['can'].port)
def update(self):
updated_messages = set()
ret = car.RadarData.new_message()
@@ -79,7 +73,7 @@ class RadarInterface(object):
header['FLRRAntTngFltPrsnt'] or header['FLRRAlgnFltPrsnt']
errors = []
if not self.rcp.can_valid:
errors.append("commIssue")
errors.append("canError")
if fault:
errors.append("fault")
ret.errors = errors
selfdrive/car/honda/carcontroller.py
+7 -6
View File
@@ -1,5 +1,5 @@
from collections import namedtuple
from common.realtime import sec_since_boot
from common.realtime import DT_CTRL
from selfdrive.controls.lib.drive_helpers import rate_limit
from common.numpy_fast import clip
from selfdrive.car import create_gas_command
@@ -7,6 +7,7 @@ from selfdrive.car.honda import hondacan
from selfdrive.car.honda.values import AH, CruiseButtons, CAR
from selfdrive.can.packer import CANPacker
def actuator_hystereses(brake, braking, brake_steady, v_ego, car_fingerprint):
# hyst params
brake_hyst_on = 0.02 # to activate brakes exceed this value
@@ -33,15 +34,14 @@ def actuator_hystereses(brake, braking, brake_steady, v_ego, car_fingerprint):
return brake, braking, brake_steady
def brake_pump_hysteresis(apply_brake, apply_brake_last, last_pump_ts):
ts = sec_since_boot()
def brake_pump_hysteresis(apply_brake, apply_brake_last, last_pump_ts, ts):
pump_on = False
# reset pump timer if:
# - there is an increment in brake request
# - we are applying steady state brakes and we haven't been running the pump
# for more than 20s (to prevent pressure bleeding)
if apply_brake > apply_brake_last or (ts - last_pump_ts > 20 and apply_brake > 0):
if apply_brake > apply_brake_last or (ts - last_pump_ts > 20. and apply_brake > 0):
last_pump_ts = ts
# once the pump is on, run it for at least 0.2s
@@ -79,7 +79,7 @@ class CarController(object):
self.brake_steady = 0.
self.brake_last = 0.
self.apply_brake_last = 0
self.last_pump_ts = 0
self.last_pump_ts = 0.
self.packer = CANPacker(dbc_name)
self.new_radar_config = False
@@ -167,7 +167,8 @@ class CarController(object):
# Send gas and brake commands.
if (frame % 2) == 0:
idx = frame // 2
pump_on, self.last_pump_ts = brake_pump_hysteresis(apply_brake, self.apply_brake_last, self.last_pump_ts)
ts = frame * DT_CTRL
pump_on, self.last_pump_ts = brake_pump_hysteresis(apply_brake, self.apply_brake_last, self.last_pump_ts, ts)
can_sends.append(hondacan.create_brake_command(self.packer, apply_brake, pump_on,
pcm_override, pcm_cancel_cmd, hud.chime, hud.fcw, idx))
self.apply_brake_last = apply_brake
selfdrive/car/honda/carstate.py
+2 -5
View File
@@ -1,6 +1,7 @@
from common.numpy_fast import interp
from common.kalman.simple_kalman import KF1D
from selfdrive.can.parser import CANParser, CANDefine
from selfdrive.can.can_define import CANDefine
from selfdrive.can.parser import CANParser
from selfdrive.config import Conversions as CV
from selfdrive.car.honda.values import CAR, DBC, STEER_THRESHOLD, SPEED_FACTOR, HONDA_BOSCH
@@ -199,10 +200,6 @@ class CarState(object):
def update(self, cp, cp_cam):
# copy can_valid on buses 0 and 2
self.can_valid = cp.can_valid
self.cam_can_valid = cp_cam.can_valid
# car params
v_weight_v = [0., 1.] # don't trust smooth speed at low values to avoid premature zero snapping
v_weight_bp = [1., 6.] # smooth blending, below ~0.6m/s the smooth speed snaps to zero
selfdrive/car/honda/interface.py
+29 -68
View File
@@ -3,20 +3,16 @@ import os
import numpy as np
from cereal import car
from common.numpy_fast import clip, interp
from common.realtime import sec_since_boot
from common.realtime import sec_since_boot, DT_CTRL
from selfdrive.swaglog import cloudlog
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, AUDIO_HUD, VISUAL_HUD
from selfdrive.car.honda.values import CruiseButtons, CAR, HONDA_BOSCH, AUDIO_HUD, 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
# 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]
A_ACC_MAX = max(_A_CRUISE_MAX_V_FOLLOWING)
@@ -83,8 +79,6 @@ class CarInterface(object):
self.last_enable_sent = 0
self.gas_pressed_prev = False
self.brake_pressed_prev = False
self.can_invalid_count = 0
self.cam_can_invalid_count = 0
self.cp = get_can_parser(CP)
self.cp_cam = get_cam_can_parser(CP)
@@ -143,12 +137,12 @@ class CarInterface(object):
ret.carVin = vin
if candidate in HONDA_BOSCH:
ret.safetyModel = car.CarParams.SafetyModels.hondaBosch
ret.safetyModel = car.CarParams.SafetyModel.hondaBosch
ret.enableCamera = True
ret.radarOffCan = True
ret.openpilotLongitudinalControl = False
else:
ret.safetyModel = car.CarParams.SafetyModels.honda
ret.safetyModel = car.CarParams.SafetyModel.honda
ret.enableCamera = not any(x for x in CAMERA_MSGS if x in fingerprint)
ret.enableGasInterceptor = 0x201 in fingerprint
ret.openpilotLongitudinalControl = ret.enableCamera
@@ -158,19 +152,6 @@ class CarInterface(object):
ret.enableCruise = not ret.enableGasInterceptor
# kg of standard extra cargo to count for drive, gas, etc...
std_cargo = 136
# FIXME: hardcoding honda civic 2016 touring params so they can be used to
# scale unknown params for other cars
mass_civic = 2923 * CV.LB_TO_KG + std_cargo
wheelbase_civic = 2.70
centerToFront_civic = wheelbase_civic * 0.4
centerToRear_civic = wheelbase_civic - centerToFront_civic
rotationalInertia_civic = 2500
tireStiffnessFront_civic = 192150
tireStiffnessRear_civic = 202500
# Optimized car params: tire_stiffness_factor and steerRatio are a result of a vehicle
# model optimization process. Certain Hondas have an extra steering sensor at the bottom
# of the steering rack, which improves controls quality as it removes the steering column
@@ -183,9 +164,9 @@ class CarInterface(object):
if candidate in [CAR.CIVIC, CAR.CIVIC_BOSCH]:
stop_and_go = True
ret.mass = mass_civic
ret.wheelbase = wheelbase_civic
ret.centerToFront = centerToFront_civic
ret.mass = CivicParams.MASS
ret.wheelbase = CivicParams.WHEELBASE
ret.centerToFront = CivicParams.CENTER_TO_FRONT
ret.steerRatio = 14.63 # 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
@@ -203,7 +184,7 @@ class CarInterface(object):
stop_and_go = True
if not candidate == CAR.ACCORDH: # Hybrid uses same brake msg as hatch
ret.safetyParam = 1 # Accord and CRV 5G use an alternate user brake msg
ret.mass = 3279. * CV.LB_TO_KG + std_cargo
ret.mass = 3279. * CV.LB_TO_KG + STD_CARGO_KG
ret.wheelbase = 2.83
ret.centerToFront = ret.wheelbase * 0.39
ret.steerRatio = 15.96 # 11.82 is spec end-to-end
@@ -216,7 +197,7 @@ class CarInterface(object):
elif candidate == CAR.ACURA_ILX:
stop_and_go = False
ret.mass = 3095 * CV.LB_TO_KG + std_cargo
ret.mass = 3095. * CV.LB_TO_KG + STD_CARGO_KG
ret.wheelbase = 2.67
ret.centerToFront = ret.wheelbase * 0.37
ret.steerRatio = 18.61 # 15.3 is spec end-to-end
@@ -229,7 +210,7 @@ class CarInterface(object):
elif candidate == CAR.CRV:
stop_and_go = False
ret.mass = 3572 * CV.LB_TO_KG + std_cargo
ret.mass = 3572. * CV.LB_TO_KG + STD_CARGO_KG
ret.wheelbase = 2.62
ret.centerToFront = ret.wheelbase * 0.41
ret.steerRatio = 15.3 # as spec
@@ -243,7 +224,7 @@ class CarInterface(object):
elif candidate == CAR.CRV_5G:
stop_and_go = True
ret.safetyParam = 1 # Accord and CRV 5G use an alternate user brake msg
ret.mass = 3410. * CV.LB_TO_KG + std_cargo
ret.mass = 3410. * CV.LB_TO_KG + STD_CARGO_KG
ret.wheelbase = 2.66
ret.centerToFront = ret.wheelbase * 0.41
ret.steerRatio = 16.0 # 12.3 is spec end-to-end
@@ -257,7 +238,7 @@ class CarInterface(object):
elif candidate == CAR.CRV_HYBRID:
stop_and_go = True
ret.safetyParam = 1 # Accord and CRV 5G use an alternate user brake msg
ret.mass = 1667. + std_cargo # mean of 4 models in kg
ret.mass = 1667. + STD_CARGO_KG # mean of 4 models in kg
ret.wheelbase = 2.66
ret.centerToFront = ret.wheelbase * 0.41
ret.steerRatio = 16.0 # 12.3 is spec end-to-end
@@ -270,7 +251,7 @@ class CarInterface(object):
elif candidate == CAR.ACURA_RDX:
stop_and_go = False
ret.mass = 3935 * CV.LB_TO_KG + std_cargo
ret.mass = 3935. * CV.LB_TO_KG + STD_CARGO_KG
ret.wheelbase = 2.68
ret.centerToFront = ret.wheelbase * 0.38
ret.steerRatio = 15.0 # as spec
@@ -283,7 +264,7 @@ class CarInterface(object):
elif candidate == CAR.ODYSSEY:
stop_and_go = False
ret.mass = 4471 * CV.LB_TO_KG + std_cargo
ret.mass = 4471. * CV.LB_TO_KG + STD_CARGO_KG
ret.wheelbase = 3.00
ret.centerToFront = ret.wheelbase * 0.41
ret.steerRatio = 14.35 # as spec
@@ -296,7 +277,7 @@ class CarInterface(object):
elif candidate == CAR.ODYSSEY_CHN:
stop_and_go = False
ret.mass = 1849.2 + std_cargo # mean of 4 models in kg
ret.mass = 1849.2 + STD_CARGO_KG # mean of 4 models in kg
ret.wheelbase = 2.90 # spec
ret.centerToFront = ret.wheelbase * 0.41 # from CAR.ODYSSEY
ret.steerRatio = 14.35 # from CAR.ODYSSEY
@@ -309,7 +290,7 @@ class CarInterface(object):
elif candidate in (CAR.PILOT, CAR.PILOT_2019):
stop_and_go = False
ret.mass = 4204 * CV.LB_TO_KG + std_cargo # average weight
ret.mass = 4204. * CV.LB_TO_KG + STD_CARGO_KG # average weight
ret.wheelbase = 2.82
ret.centerToFront = ret.wheelbase * 0.428 # average weight distribution
ret.steerRatio = 16.0 # as spec
@@ -322,7 +303,7 @@ class CarInterface(object):
elif candidate == CAR.RIDGELINE:
stop_and_go = False
ret.mass = 4515 * CV.LB_TO_KG + std_cargo
ret.mass = 4515. * CV.LB_TO_KG + STD_CARGO_KG
ret.wheelbase = 3.18
ret.centerToFront = ret.wheelbase * 0.41
ret.steerRatio = 15.59 # as spec
@@ -343,20 +324,14 @@ class CarInterface(object):
# conflict with PCM acc
ret.minEnableSpeed = -1. if (stop_and_go or ret.enableGasInterceptor) else 25.5 * CV.MPH_TO_MS
centerToRear = ret.wheelbase - ret.centerToFront
# TODO: get actual value, for now starting with reasonable value for
# civic and scaling by mass and wheelbase
ret.rotationalInertia = rotationalInertia_civic * \
ret.mass * ret.wheelbase**2 / (mass_civic * wheelbase_civic**2)
ret.rotationalInertia = scale_rot_inertia(ret.mass, ret.wheelbase)
# TODO: start from empirically derived lateral slip stiffness for the civic and scale by
# mass and CG position, so all cars will have approximately similar dyn behaviors
ret.tireStiffnessFront = (tireStiffnessFront_civic * tire_stiffness_factor) * \
ret.mass / mass_civic * \
(centerToRear / ret.wheelbase) / (centerToRear_civic / wheelbase_civic)
ret.tireStiffnessRear = (tireStiffnessRear_civic * tire_stiffness_factor) * \
ret.mass / mass_civic * \
(ret.centerToFront / ret.wheelbase) / (centerToFront_civic / wheelbase_civic)
ret.tireStiffnessFront, ret.tireStiffnessRear = scale_tire_stiffness(ret.mass, ret.wheelbase, ret.centerToFront,
tire_stiffness_factor=tire_stiffness_factor)
# no rear steering, at least on the listed cars above
ret.steerRatioRear = 0.
@@ -386,15 +361,16 @@ class CarInterface(object):
def update(self, c):
# ******************* do can recv *******************
canMonoTimes = []
can_valid, _ = self.cp.update(int(sec_since_boot() * 1e9), True)
cam_valid, _ = self.cp_cam.update(int(sec_since_boot() * 1e9), False)
can_rcv_error = not can_valid or not cam_valid
can_rcv_valid, _ = self.cp.update(int(sec_since_boot() * 1e9), True)
cam_rcv_valid, _ = self.cp_cam.update(int(sec_since_boot() * 1e9), False)
self.CS.update(self.cp, self.cp_cam)
# create message
ret = car.CarState.new_message()
ret.canValid = can_rcv_valid and cam_rcv_valid and self.cp.can_valid
# speeds
ret.vEgo = self.CS.v_ego
ret.aEgo = self.CS.a_ego
@@ -493,25 +469,10 @@ class CarInterface(object):
ret.buttonEvents = buttonEvents
# events
# TODO: event names aren't checked at compile time.
# Maybe there is a way to use capnp enums directly
events = []
if not self.CS.can_valid:
self.can_invalid_count += 1
else:
self.can_invalid_count = 0
if can_rcv_error or self.can_invalid_count >= 5:
events.append(create_event('commIssue', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
if not self.CS.cam_can_valid and self.CP.enableCamera:
self.cam_can_invalid_count += 1
# wait 1.0s before throwing the alert to avoid it popping when you turn off the car
if self.cam_can_invalid_count >= 100 and self.CS.CP.carFingerprint not in HONDA_BOSCH:
events.append(create_event('invalidGiraffeHonda', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE, ET.PERMANENT]))
else:
self.cam_can_invalid_count = 0
# wait 1.0s before throwing the alert to avoid it popping when you turn off the car
if self.cp_cam.can_invalid_cnt >= 100 and self.CS.CP.carFingerprint not in HONDA_BOSCH and self.CP.enableCamera:
events.append(create_event('invalidGiraffeHonda', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE, ET.PERMANENT]))
if self.CS.steer_error:
events.append(create_event('steerUnavailable', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE, ET.PERMANENT]))
elif self.CS.steer_warning:
@@ -557,7 +518,7 @@ class CarInterface(object):
if self.CS.CP.minEnableSpeed > 0 and ret.vEgo < 0.001:
events.append(create_event('manualRestart', [ET.WARNING]))
cur_time = sec_since_boot()
cur_time = self.frame * DT_CTRL
enable_pressed = False
# handle button presses
for b in ret.buttonEvents:
selfdrive/car/honda/radar_interface.py
+1 -8
View File
@@ -1,13 +1,9 @@
#!/usr/bin/env python
import os
import zmq
import time
from cereal import car
from selfdrive.can.parser import CANParser
from common.realtime import sec_since_boot
from selfdrive.services import service_list
import selfdrive.messaging as messaging
def _create_nidec_can_parser():
dbc_f = 'acura_ilx_2016_nidec.dbc'
@@ -36,9 +32,6 @@ class RadarInterface(object):
# Nidec
self.rcp = _create_nidec_can_parser()
context = zmq.Context()
self.logcan = messaging.sub_sock(context, service_list['can'].port)
def update(self):
canMonoTimes = []
@@ -81,7 +74,7 @@ class RadarInterface(object):
errors = []
if not self.rcp.can_valid:
errors.append("commIssue")
errors.append("canError")
if self.radar_fault:
errors.append("fault")
if self.radar_wrong_config:
selfdrive/car/honda/values.py
+5 -1
View File
@@ -104,7 +104,7 @@ FINGERPRINTS = {
57: 3, 148: 8, 199: 4, 228: 5, 231: 5, 232: 7, 304: 8, 330: 8, 340: 8, 344: 8, 380: 8, 399: 7, 401: 8, 420: 8, 423: 2, 427: 3, 428: 8, 432: 7, 441: 5, 446: 3, 450: 8, 464: 8, 467: 2, 469: 3, 470: 2, 474: 8, 476: 7, 477: 8, 479: 8, 490: 8, 493: 5, 495: 8, 507: 1, 545: 6, 597: 8, 661: 4, 662: 4, 773: 7, 777: 8, 780: 8, 795: 8, 800: 8, 804: 8, 806: 8, 808: 8, 814: 4, 815: 8, 817: 4, 825: 4, 829: 5, 862: 8, 881: 8, 882: 4, 884: 8, 888: 8, 891: 8, 927: 8, 918: 7, 929: 8, 983: 8, 985: 3, 1024: 5, 1027: 5, 1029: 8, 1036: 8, 1039: 8, 1064: 7, 1108: 8, 1092: 1, 1115: 4, 1125: 8, 1127: 2, 1296: 8, 1302: 8, 1322: 5, 1361: 5, 1365: 5, 1424: 5, 1600: 5, 1601: 8, 1618: 5, 1633: 8, 1670: 5
}],
CAR.CRV_HYBRID: [{
57: 3, 148: 8, 228: 5, 304: 8, 330: 8, 344: 8, 380: 8, 387: 8, 388: 8, 399: 7, 419: 8, 420: 8, 427: 3, 432: 7, 441: 5, 450: 8, 464: 8, 477: 8, 479: 8, 490: 8, 495: 8, 525: 8, 531: 8, 545: 6, 662: 4, 773: 7, 777: 8, 780: 8, 804: 8, 806: 8, 808: 8, 814: 4, 829: 5, 833: 6, 862: 8, 884: 8, 891: 8, 927: 8, 929: 8, 1302: 8, 1361: 5, 1365: 5, 1600: 5, 1601: 8
57: 3, 148: 8, 228: 5, 304: 8, 330: 8, 344: 8, 380: 8, 387: 8, 388: 8, 399: 7, 408: 6, 415: 6, 419: 8, 420: 8, 427: 3, 428: 8, 432: 7, 441: 5, 450: 8, 464: 8, 477: 8, 479: 8, 490: 8, 495: 8, 525: 8, 531: 8, 545: 6, 662: 4, 773: 7, 777: 8, 780: 8, 804: 8, 806: 8, 808: 8, 814: 4, 829: 5, 833: 6, 862: 8, 884: 8, 891: 8, 927: 8, 929: 8, 930: 8, 931: 8, 1302: 8, 1361: 5, 1365: 5, 1600: 5, 1601: 8, 1626: 5, 1627: 5
}],
# 2018 Odyssey w/ Added Comma Pedal Support (512L & 513L)
CAR.ODYSSEY: [{
@@ -193,5 +193,9 @@ SPEED_FACTOR = {
CAR.RIDGELINE: 1.,
}
# 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]
# TODO: get these from dbc file
HONDA_BOSCH = [CAR.ACCORD, CAR.ACCORD_15, CAR.ACCORDH, CAR.CIVIC_BOSCH, CAR.CRV_5G, CAR.CRV_HYBRID]
selfdrive/car/hyundai/carstate.py
-3
View File
@@ -145,9 +145,6 @@ class CarState(object):
self.right_blinker_flash = 0
def update(self, cp, cp_cam):
# copy can_valid
self.can_valid = cp.can_valid
# update prevs, update must run once per Loop
self.prev_left_blinker_on = self.left_blinker_on
self.prev_right_blinker_on = self.right_blinker_on
selfdrive/car/hyundai/interface.py
+17 -45
View File
@@ -6,6 +6,7 @@ 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
class CarInterface(object):
@@ -18,7 +19,6 @@ class CarInterface(object):
self.gas_pressed_prev = False
self.brake_pressed_prev = False
self.can_invalid_count = 0
self.cruise_enabled_prev = False
self.low_speed_alert = False
@@ -42,35 +42,22 @@ class CarInterface(object):
@staticmethod
def get_params(candidate, fingerprint, vin=""):
# kg of standard extra cargo to count for drive, gas, etc...
std_cargo = 136
ret = car.CarParams.new_message()
ret.carName = "hyundai"
ret.carFingerprint = candidate
ret.carVin = vin
ret.radarOffCan = True
ret.safetyModel = car.CarParams.SafetyModels.hyundai
ret.safetyModel = car.CarParams.SafetyModel.hyundai
ret.enableCruise = True # stock acc
# FIXME: hardcoding honda civic 2016 touring params so they can be used to
# scale unknown params for other cars
mass_civic = 2923 * CV.LB_TO_KG + std_cargo
wheelbase_civic = 2.70
centerToFront_civic = wheelbase_civic * 0.4
centerToRear_civic = wheelbase_civic - centerToFront_civic
rotationalInertia_civic = 2500
tireStiffnessFront_civic = 192150
tireStiffnessRear_civic = 202500
tire_stiffness_factor = 1.
ret.steerActuatorDelay = 0.1 # Default delay
ret.steerRateCost = 0.5
tire_stiffness_factor = 1.
if candidate == CAR.SANTA_FE:
ret.lateralTuning.pid.kf = 0.00005
ret.mass = 3982 * CV.LB_TO_KG + std_cargo
ret.mass = 3982. * CV.LB_TO_KG + STD_CARGO_KG
ret.wheelbase = 2.766
# Values from optimizer
@@ -82,7 +69,7 @@ class CarInterface(object):
ret.minSteerSpeed = 0.
elif candidate == CAR.KIA_SORENTO:
ret.lateralTuning.pid.kf = 0.00005
ret.mass = 1985 + std_cargo
ret.mass = 1985. + STD_CARGO_KG
ret.wheelbase = 2.78
ret.steerRatio = 14.4 * 1.1 # 10% higher at the center seems reasonable
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]]
@@ -90,7 +77,7 @@ class CarInterface(object):
ret.minSteerSpeed = 0.
elif candidate == CAR.ELANTRA:
ret.lateralTuning.pid.kf = 0.00006
ret.mass = 1275 + std_cargo
ret.mass = 1275. + STD_CARGO_KG
ret.wheelbase = 2.7
ret.steerRatio = 13.73 #Spec
tire_stiffness_factor = 0.385
@@ -99,7 +86,7 @@ class CarInterface(object):
ret.minSteerSpeed = 32 * CV.MPH_TO_MS
elif candidate == CAR.GENESIS:
ret.lateralTuning.pid.kf = 0.00005
ret.mass = 2060 + std_cargo
ret.mass = 2060. + STD_CARGO_KG
ret.wheelbase = 3.01
ret.steerRatio = 16.5
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]]
@@ -107,7 +94,7 @@ class CarInterface(object):
ret.minSteerSpeed = 35 * CV.MPH_TO_MS
elif candidate == CAR.KIA_OPTIMA:
ret.lateralTuning.pid.kf = 0.00005
ret.mass = 3558 * CV.LB_TO_KG
ret.mass = 3558. * CV.LB_TO_KG
ret.wheelbase = 2.80
ret.steerRatio = 13.75
tire_stiffness_factor = 0.5
@@ -115,7 +102,7 @@ class CarInterface(object):
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25], [0.05]]
elif candidate == CAR.KIA_STINGER:
ret.lateralTuning.pid.kf = 0.00005
ret.mass = 1825 + std_cargo
ret.mass = 1825. + STD_CARGO_KG
ret.wheelbase = 2.78
ret.steerRatio = 14.4 * 1.15 # 15% higher at the center seems reasonable
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]]
@@ -132,21 +119,14 @@ class CarInterface(object):
ret.centerToFront = ret.wheelbase * 0.4
centerToRear = ret.wheelbase - ret.centerToFront
# TODO: get actual value, for now starting with reasonable value for
# civic and scaling by mass and wheelbase
ret.rotationalInertia = rotationalInertia_civic * \
ret.mass * ret.wheelbase**2 / (mass_civic * wheelbase_civic**2)
ret.rotationalInertia = scale_rot_inertia(ret.mass, ret.wheelbase)
# TODO: start from empirically derived lateral slip stiffness for the civic and scale by
# mass and CG position, so all cars will have approximately similar dyn behaviors
ret.tireStiffnessFront = (tireStiffnessFront_civic * tire_stiffness_factor) * \
ret.mass / mass_civic * \
(centerToRear / ret.wheelbase) / (centerToRear_civic / wheelbase_civic)
ret.tireStiffnessRear = (tireStiffnessRear_civic * tire_stiffness_factor) * \
ret.mass / mass_civic * \
(ret.centerToFront / ret.wheelbase) / (centerToFront_civic / wheelbase_civic)
ret.tireStiffnessFront, ret.tireStiffnessRear = scale_tire_stiffness(ret.mass, ret.wheelbase, ret.centerToFront,
tire_stiffness_factor=tire_stiffness_factor)
# no rear steering, at least on the listed cars above
@@ -174,13 +154,15 @@ class CarInterface(object):
def update(self, c):
# ******************* do can recv *******************
canMonoTimes = []
can_rcv_error = not self.cp.update(int(sec_since_boot() * 1e9), True)
cam_rcv_error = not self.cp_cam.update(int(sec_since_boot() * 1e9), False)
can_rcv_error = can_rcv_error or cam_rcv_error
can_rcv_valid, _ = self.cp.update(int(sec_since_boot() * 1e9), True)
cam_rcv_valid, _ = self.cp_cam.update(int(sec_since_boot() * 1e9), False)
self.CS.update(self.cp, self.cp_cam)
# create message
ret = car.CarState.new_message()
ret.canValid = can_rcv_valid and cam_rcv_valid and self.cp.can_valid # TODO: check cp_cam validity
# speeds
ret.vEgo = self.CS.v_ego
ret.vEgoRaw = self.CS.v_ego_raw
@@ -247,23 +229,13 @@ class CarInterface(object):
ret.doorOpen = not self.CS.door_all_closed
ret.seatbeltUnlatched = not self.CS.seatbelt
# low speed steer alert hysteresis logic (only for cars with steer cut off above 10 m/s)
if ret.vEgo < (self.CP.minSteerSpeed + 2.) and self.CP.minSteerSpeed > 10.:
self.low_speed_alert = True
if ret.vEgo > (self.CP.minSteerSpeed + 4.):
self.low_speed_alert = False
# events
events = []
if not self.CS.can_valid:
self.can_invalid_count += 1
else:
self.can_invalid_count = 0
if can_rcv_error or self.can_invalid_count >= 5:
events.append(create_event('commIssue', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
if not ret.gearShifter == 'drive':
events.append(create_event('wrongGear', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
if ret.doorOpen:
selfdrive/car/mock/interface.py
+3 -5
View File
@@ -1,5 +1,4 @@
#!/usr/bin/env python
import zmq
from cereal import car
from selfdrive.config import Conversions as CV
from selfdrive.services import service_list
@@ -21,11 +20,10 @@ class CarInterface(object):
self.CC = CarController
cloudlog.debug("Using Mock Car Interface")
context = zmq.Context()
# TODO: subscribe to phone sensor
self.sensor = messaging.sub_sock(context, service_list['sensorEvents'].port)
self.gps = messaging.sub_sock(context, service_list['gpsLocation'].port)
self.sensor = messaging.sub_sock(service_list['sensorEvents'].port)
self.gps = messaging.sub_sock(service_list['gpsLocation'].port)
self.speed = 0.
self.prev_speed = 0.
@@ -50,7 +48,7 @@ class CarInterface(object):
ret.carName = "mock"
ret.carFingerprint = candidate
ret.safetyModel = car.CarParams.SafetyModels.noOutput
ret.safetyModel = car.CarParams.SafetyModel.noOutput
ret.openpilotLongitudinalControl = False
# FIXME: hardcoding honda civic 2016 touring params so they can be used to
selfdrive/car/subaru/carcontroller.py
-2
View File
@@ -1,5 +1,4 @@
#from common.numpy_fast import clip
from common.realtime import sec_since_boot
from selfdrive.car import apply_std_steer_torque_limits
from selfdrive.car.subaru import subarucan
from selfdrive.car.subaru.values import CAR, DBC
@@ -21,7 +20,6 @@ class CarControllerParams():
class CarController(object):
def __init__(self, car_fingerprint):
self.start_time = sec_since_boot()
self.lkas_active = False
self.steer_idx = 0
self.apply_steer_last = 0
selfdrive/car/subaru/carstate.py
-3
View File
@@ -106,9 +106,6 @@ class CarState(object):
def update(self, cp, cp_cam):
self.can_valid = cp.can_valid
self.cam_can_valid = cp_cam.can_valid
self.pedal_gas = cp.vl["Throttle"]['Throttle_Pedal']
self.brake_pressure = cp.vl["Brake_Pedal"]['Brake_Pedal']
self.user_gas_pressed = self.pedal_gas > 0
selfdrive/car/subaru/interface.py
+9 -35
View File
@@ -6,6 +6,7 @@ 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.carstate import CarState, get_powertrain_can_parser, get_camera_can_parser
from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness
class CarInterface(object):
@@ -13,7 +14,6 @@ class CarInterface(object):
self.CP = CP
self.frame = 0
self.can_invalid_count = 0
self.acc_active_prev = 0
self.gas_pressed_prev = False
@@ -44,22 +44,20 @@ class CarInterface(object):
ret.carName = "subaru"
ret.carFingerprint = candidate
ret.carVin = vin
ret.safetyModel = car.CarParams.SafetyModels.subaru
ret.safetyModel = car.CarParams.SafetyModel.subaru
ret.enableCruise = True
ret.steerLimitAlert = True
ret.enableCamera = True
std_cargo = 136
ret.steerRateCost = 0.7
if candidate in [CAR.IMPREZA]:
ret.mass = 1568 + std_cargo
ret.mass = 1568. + STD_CARGO_KG
ret.wheelbase = 2.67
ret.centerToFront = ret.wheelbase * 0.5
ret.steerRatio = 15
tire_stiffness_factor = 1.0
ret.steerActuatorDelay = 0.4 # end-to-end angle controller
ret.lateralTuning.pid.kf = 0.00005
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0., 20.], [0., 20.]]
@@ -85,44 +83,28 @@ class CarInterface(object):
# end from gm
# hardcoding honda civic 2016 touring params so they can be used to
# scale unknown params for other cars
mass_civic = 2923./2.205 + std_cargo
wheelbase_civic = 2.70
centerToFront_civic = wheelbase_civic * 0.4
centerToRear_civic = wheelbase_civic - centerToFront_civic
rotationalInertia_civic = 2500
tireStiffnessFront_civic = 192150
tireStiffnessRear_civic = 202500
centerToRear = ret.wheelbase - ret.centerToFront
# TODO: get actual value, for now starting with reasonable value for
# civic and scaling by mass and wheelbase
ret.rotationalInertia = rotationalInertia_civic * \
ret.mass * ret.wheelbase**2 / (mass_civic * wheelbase_civic**2)
ret.rotationalInertia = scale_rot_inertia(ret.mass, ret.wheelbase)
# TODO: start from empirically derived lateral slip stiffness for the civic and scale by
# mass and CG position, so all cars will have approximately similar dyn behaviors
ret.tireStiffnessFront = (tireStiffnessFront_civic * tire_stiffness_factor) * \
ret.mass / mass_civic * \
(centerToRear / ret.wheelbase) / (centerToRear_civic / wheelbase_civic)
ret.tireStiffnessRear = (tireStiffnessRear_civic * tire_stiffness_factor) * \
ret.mass / mass_civic * \
(ret.centerToFront / ret.wheelbase) / (centerToFront_civic / wheelbase_civic)
ret.tireStiffnessFront, ret.tireStiffnessRear = scale_tire_stiffness(ret.mass, ret.wheelbase, ret.centerToFront)
return ret
# returns a car.CarState
def update(self, c):
can_rcv_error = not self.pt_cp.update(int(sec_since_boot() * 1e9), True)
cam_rcv_error = not self.cam_cp.update(int(sec_since_boot() * 1e9), False)
can_rcv_error = can_rcv_error or cam_rcv_error
can_rcv_valid, _ = self.pt_cp.update(int(sec_since_boot() * 1e9), True)
cam_rcv_valid, _ = self.cam_cp.update(int(sec_since_boot() * 1e9), False)
self.CS.update(self.pt_cp, self.cam_cp)
# create message
ret = car.CarState.new_message()
ret.canValid = can_rcv_valid and cam_rcv_valid and self.pt_cp.can_valid and self.cam_cp.can_valid
# speeds
ret.vEgo = self.CS.v_ego
ret.aEgo = self.CS.a_ego
@@ -177,14 +159,6 @@ class CarInterface(object):
events = []
if not self.CS.can_valid:
self.can_invalid_count += 1
else:
self.can_invalid_count = 0
if can_rcv_error or self.can_invalid_count >= 5:
events.append(create_event('commIssue', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
if ret.seatbeltUnlatched:
events.append(create_event('seatbeltNotLatched', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
selfdrive/car/toyota/carcontroller.py
+2 -2
View File
@@ -6,7 +6,7 @@ from selfdrive.car.toyota.toyotacan import make_can_msg, create_video_target,\
create_steer_command, create_ui_command, \
create_ipas_steer_command, create_accel_command, \
create_fcw_command
from selfdrive.car.toyota.values import ECU, STATIC_MSGS, TSSP2_CAR
from selfdrive.car.toyota.values import ECU, STATIC_MSGS, TSS2_CAR
from selfdrive.can.packer import CANPacker
VisualAlert = car.CarControl.HUDControl.VisualAlert
@@ -248,7 +248,7 @@ class CarController(object):
if (frame % 100 == 0 or send_ui) and ECU.CAM in self.fake_ecus:
can_sends.append(create_ui_command(self.packer, steer, sound1, sound2, left_line, right_line, left_lane_depart, right_lane_depart))
if frame % 100 == 0 and ECU.DSU in self.fake_ecus and self.car_fingerprint not in TSSP2_CAR:
if frame % 100 == 0 and ECU.DSU in self.fake_ecus and self.car_fingerprint not in TSS2_CAR:
can_sends.append(create_fcw_command(self.packer, fcw))
#*** static msgs ***
selfdrive/car/toyota/carstate.py
+14 -11
View File
@@ -1,8 +1,9 @@
import numpy as np
from common.kalman.simple_kalman import KF1D
from selfdrive.can.parser import CANParser, CANDefine
from selfdrive.can.parser import CANParser
from selfdrive.can.can_define import CANDefine
from selfdrive.config import Conversions as CV
from selfdrive.car.toyota.values import CAR, DBC, STEER_THRESHOLD
from selfdrive.car.toyota.values import CAR, DBC, STEER_THRESHOLD, NO_DSU_CAR
def parse_gear_shifter(gear, vals):
@@ -31,10 +32,8 @@ def get_can_parser(CP):
("DOOR_OPEN_RR", "SEATS_DOORS", 1),
("SEATBELT_DRIVER_UNLATCHED", "SEATS_DOORS", 1),
("TC_DISABLED", "ESP_CONTROL", 1),
("STEER_ANGLE", "STEER_ANGLE_SENSOR", 0),
("STEER_FRACTION", "STEER_ANGLE_SENSOR", 0),
("STEER_RATE", "STEER_ANGLE_SENSOR", 0),
("GAS_RELEASED", "PCM_CRUISE", 0),
("CRUISE_ACTIVE", "PCM_CRUISE", 0),
("CRUISE_STATE", "PCM_CRUISE", 0),
("STEER_TORQUE_DRIVER", "STEER_TORQUE_SENSOR", 0),
@@ -52,6 +51,11 @@ def get_can_parser(CP):
("EPS_STATUS", 25),
]
if CP.carFingerprint in NO_DSU_CAR:
signals += [("STEER_ANGLE", "STEER_TORQUE_SENSOR", 0)]
else:
signals += [("STEER_ANGLE", "STEER_ANGLE_SENSOR", 0)]
if CP.carFingerprint == CAR.LEXUS_ISH:
checks += [
("BRAKE_MODULE", 50),
@@ -121,11 +125,7 @@ class CarState(object):
K=[[0.12287673], [0.29666309]])
self.v_ego = 0.0
def update(self, cp, cp_cam):
# copy can_valid
self.can_valid = cp.can_valid
self.cam_can_valid = cp_cam.can_valid
def update(self, cp):
# update prevs, update must run once per loop
self.prev_left_blinker_on = self.left_blinker_on
self.prev_right_blinker_on = self.right_blinker_on
@@ -159,7 +159,10 @@ class CarState(object):
self.a_ego = float(v_ego_x[1])
self.standstill = not v_wheel > 0.001
self.angle_steers = cp.vl["STEER_ANGLE_SENSOR"]['STEER_ANGLE'] + cp.vl["STEER_ANGLE_SENSOR"]['STEER_FRACTION']
if self.CP.carFingerprint in NO_DSU_CAR:
self.angle_steers = cp.vl["STEER_TORQUE_SENSOR"]['STEER_ANGLE']
else:
self.angle_steers = cp.vl["STEER_ANGLE_SENSOR"]['STEER_ANGLE'] + cp.vl["STEER_ANGLE_SENSOR"]['STEER_FRACTION']
self.angle_steers_rate = cp.vl["STEER_ANGLE_SENSOR"]['STEER_RATE']
can_gear = int(cp.vl["GEAR_PACKET"]['GEAR'])
self.gear_shifter = parse_gear_shifter(can_gear, self.shifter_values)
@@ -194,7 +197,7 @@ class CarState(object):
self.pcm_acc_status = cp.vl["PCM_CRUISE"]['CRUISE_STATE']
self.low_speed_lockout = cp.vl["PCM_CRUISE_2"]['LOW_SPEED_LOCKOUT'] == 2
self.pcm_acc_active = bool(cp.vl["PCM_CRUISE"]['CRUISE_ACTIVE'])
self.gas_pressed = not cp.vl["PCM_CRUISE"]['GAS_RELEASED']
self.low_speed_lockout = cp.vl["PCM_CRUISE_2"]['LOW_SPEED_LOCKOUT'] == 2
self.brake_lights = bool(cp.vl["ESP_CONTROL"]['BRAKE_LIGHTS_ACC'] or self.brake_pressed)
if self.CP.carFingerprint == CAR.PRIUS:
self.generic_toggle = cp.vl["AUTOPARK_STATUS"]['STATE'] != 0
selfdrive/car/toyota/interface.py
+48 -68
View File
@@ -6,6 +6,7 @@ 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
from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness
from selfdrive.swaglog import cloudlog
@@ -17,8 +18,6 @@ class CarInterface(object):
self.frame = 0
self.gas_pressed_prev = False
self.brake_pressed_prev = False
self.can_invalid_count = 0
self.cam_can_valid_count = 0
self.cruise_enabled_prev = False
# *** init the major players ***
@@ -44,30 +43,17 @@ class CarInterface(object):
@staticmethod
def get_params(candidate, fingerprint, vin=""):
# kg of standard extra cargo to count for drive, gas, etc...
std_cargo = 136
ret = car.CarParams.new_message()
ret.carName = "toyota"
ret.carFingerprint = candidate
ret.carVin = vin
ret.safetyModel = car.CarParams.SafetyModels.toyota
ret.safetyModel = car.CarParams.SafetyModel.toyota
# pedal
ret.enableCruise = not ret.enableGasInterceptor
# FIXME: hardcoding honda civic 2016 touring params so they can be used to
# scale unknown params for other cars
mass_civic = 2923 * CV.LB_TO_KG + std_cargo
wheelbase_civic = 2.70
centerToFront_civic = wheelbase_civic * 0.4
centerToRear_civic = wheelbase_civic - centerToFront_civic
rotationalInertia_civic = 2500
tireStiffnessFront_civic = 192150
tireStiffnessRear_civic = 202500
ret.steerActuatorDelay = 0.12 # Default delay, Prius has larger delay
if candidate != CAR.PRIUS:
ret.lateralTuning.init('pid')
@@ -79,7 +65,7 @@ class CarInterface(object):
ret.wheelbase = 2.70
ret.steerRatio = 15.00 # unknown end-to-end spec
tire_stiffness_factor = 0.6371 # hand-tune
ret.mass = 3045 * CV.LB_TO_KG + std_cargo
ret.mass = 3045. * CV.LB_TO_KG + STD_CARGO_KG
ret.lateralTuning.init('indi')
ret.lateralTuning.indi.innerLoopGain = 4.0
@@ -88,96 +74,105 @@ class CarInterface(object):
ret.lateralTuning.indi.actuatorEffectiveness = 1.0
ret.steerActuatorDelay = 0.5
ret.steerRateCost = 0.5
elif candidate in [CAR.RAV4, CAR.RAV4H]:
stop_and_go = True if (candidate in CAR.RAV4H) else False
ret.safetyParam = 73 # see conversion factor for STEER_TORQUE_EPS in dbc file
ret.safetyParam = 73
ret.wheelbase = 2.65
ret.steerRatio = 16.30 # 14.5 is spec end-to-end
tire_stiffness_factor = 0.5533
ret.mass = 3650 * CV.LB_TO_KG + std_cargo # mean between normal and hybrid
ret.mass = 3650. * CV.LB_TO_KG + STD_CARGO_KG # mean between normal and hybrid
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.05]]
ret.lateralTuning.pid.kf = 0.00006 # full torque for 10 deg at 80mph means 0.00007818594
elif candidate == CAR.COROLLA:
stop_and_go = False
ret.safetyParam = 100 # see conversion factor for STEER_TORQUE_EPS in dbc file
ret.safetyParam = 100
ret.wheelbase = 2.70
ret.steerRatio = 17.8
tire_stiffness_factor = 0.444
ret.mass = 2860 * CV.LB_TO_KG + std_cargo # mean between normal and hybrid
tire_stiffness_factor = 0.444 # not optimized yet
ret.mass = 2860. * CV.LB_TO_KG + STD_CARGO_KG # mean between normal and hybrid
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.2], [0.05]]
ret.lateralTuning.pid.kf = 0.00003 # full torque for 20 deg at 80mph means 0.00007818594
elif candidate == CAR.LEXUS_RXH:
stop_and_go = True
ret.safetyParam = 100 # see conversion factor for STEER_TORQUE_EPS in dbc file
ret.safetyParam = 73
ret.wheelbase = 2.79
ret.steerRatio = 16. # 14.8 is spec end-to-end
tire_stiffness_factor = 0.444 # not optimized yet
ret.mass = 4481 * CV.LB_TO_KG + std_cargo # mean between min and max
ret.mass = 4481. * CV.LB_TO_KG + STD_CARGO_KG # mean between min and max
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.1]]
ret.lateralTuning.pid.kf = 0.00006 # full torque for 10 deg at 80mph means 0.00007818594
elif candidate in [CAR.CHR, CAR.CHRH]:
stop_and_go = True
ret.safetyParam = 100
ret.safetyParam = 73
ret.wheelbase = 2.63906
ret.steerRatio = 13.6
tire_stiffness_factor = 0.7933
ret.mass = 3300. * CV.LB_TO_KG + std_cargo
ret.mass = 3300. * CV.LB_TO_KG + STD_CARGO_KG
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.723], [0.0428]]
ret.lateralTuning.pid.kf = 0.00006
elif candidate in [CAR.CAMRY, CAR.CAMRYH]:
stop_and_go = True
ret.safetyParam = 100
ret.safetyParam = 73
ret.wheelbase = 2.82448
ret.steerRatio = 13.7
tire_stiffness_factor = 0.7933
ret.mass = 3400 * CV.LB_TO_KG + std_cargo #mean between normal and hybrid
ret.mass = 3400. * CV.LB_TO_KG + STD_CARGO_KG #mean between normal and hybrid
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.1]]
ret.lateralTuning.pid.kf = 0.00006
elif candidate in [CAR.HIGHLANDER, CAR.HIGHLANDERH]:
stop_and_go = True
ret.safetyParam = 100
ret.safetyParam = 73
ret.wheelbase = 2.78
ret.steerRatio = 16.0
tire_stiffness_factor = 0.444 # not optimized yet
ret.mass = 4607 * CV.LB_TO_KG + std_cargo #mean between normal and hybrid limited
tire_stiffness_factor = 0.444 # not optimized yet
ret.mass = 4607. * CV.LB_TO_KG + STD_CARGO_KG #mean between normal and hybrid limited
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.05]]
ret.lateralTuning.pid.kf = 0.00006
elif candidate == CAR.AVALON:
stop_and_go = False
ret.safetyParam = 73 # see conversion factor for STEER_TORQUE_EPS in dbc file
ret.safetyParam = 73
ret.wheelbase = 2.82
ret.steerRatio = 14.8 #Found at https://pressroom.toyota.com/releases/2016+avalon+product+specs.download
tire_stiffness_factor = 0.7983
ret.mass = 3505 * CV.LB_TO_KG + std_cargo # mean between normal and hybrid
ret.mass = 3505. * CV.LB_TO_KG + STD_CARGO_KG # mean between normal and hybrid
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.17], [0.03]]
ret.lateralTuning.pid.kf = 0.00006
elif candidate == CAR.RAV4_2019:
elif candidate == CAR.RAV4_TSS2:
stop_and_go = True
ret.safetyParam = 100
ret.safetyParam = 73
ret.wheelbase = 2.68986
ret.steerRatio = 14.3
tire_stiffness_factor = 0.7933
ret.mass = 3370. * CV.LB_TO_KG + std_cargo
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.3], [0.05]]
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.1]]
ret.mass = 3370. * CV.LB_TO_KG + STD_CARGO_KG
ret.lateralTuning.pid.kf = 0.00007818594
elif candidate == CAR.COROLLA_HATCH:
elif candidate == CAR.COROLLA_TSS2:
stop_and_go = True
ret.safetyParam = 100
ret.safetyParam = 73
ret.wheelbase = 2.63906
ret.steerRatio = 13.9
tire_stiffness_factor = 0.444
ret.mass = 3060. * CV.LB_TO_KG + std_cargo
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.3], [0.05]]
tire_stiffness_factor = 0.444 # not optimized yet
ret.mass = 3060. * CV.LB_TO_KG + STD_CARGO_KG
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.1]]
ret.lateralTuning.pid.kf = 0.00007818594
elif candidate == CAR.LEXUS_ESH_TSS2:
stop_and_go = True
ret.safetyParam = 73
ret.wheelbase = 2.8702
ret.steerRatio = 16.0 # not optimized
tire_stiffness_factor = 0.444 # not optimized yet
ret.mass = 3704. * CV.LB_TO_KG + STD_CARGO_KG
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.1]]
ret.lateralTuning.pid.kf = 0.00007818594
elif candidate == CAR.LEXUS_ISH:
@@ -186,7 +181,7 @@ class CarInterface(object):
ret.wheelbase = 2.80 # in spec
ret.steerRatio = 13.3 # in spec
tire_stiffness_factor = 0.444 # from camry
ret.mass = 3736.8 * CV.LB_TO_KG + std_cargo # in spec, mean of is300 (1680 kg) / is300h (1720 kg) / is350 (1685 kg)
ret.mass = 3736.8 * CV.LB_TO_KG + STD_CARGO_KG # in spec, mean of is300 (1680 kg) / is300h (1720 kg) / is350 (1685 kg)
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.19], [0.04]]
ret.lateralTuning.pid.kf = 0.00006 # from camry
@@ -200,20 +195,14 @@ class CarInterface(object):
# 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
centerToRear = ret.wheelbase - ret.centerToFront
# TODO: get actual value, for now starting with reasonable value for
# civic and scaling by mass and wheelbase
ret.rotationalInertia = rotationalInertia_civic * \
ret.mass * ret.wheelbase**2 / (mass_civic * wheelbase_civic**2)
ret.rotationalInertia = scale_rot_inertia(ret.mass, ret.wheelbase)
# TODO: start from empirically derived lateral slip stiffness for the civic and scale by
# mass and CG position, so all cars will have approximately similar dyn behaviors
ret.tireStiffnessFront = (tireStiffnessFront_civic * tire_stiffness_factor) * \
ret.mass / mass_civic * \
(centerToRear / ret.wheelbase) / (centerToRear_civic / wheelbase_civic)
ret.tireStiffnessRear = (tireStiffnessRear_civic * tire_stiffness_factor) * \
ret.mass / mass_civic * \
(ret.centerToFront / ret.wheelbase) / (centerToFront_civic / wheelbase_civic)
ret.tireStiffnessFront, ret.tireStiffnessRear = scale_tire_stiffness(ret.mass, ret.wheelbase, ret.centerToFront,
tire_stiffness_factor=tire_stiffness_factor)
# no rear steering, at least on the listed cars above
ret.steerRatioRear = 0.
@@ -261,18 +250,19 @@ class CarInterface(object):
# ******************* do can recv *******************
canMonoTimes = []
can_valid, _ = self.cp.update(int(sec_since_boot() * 1e9), True)
can_rcv_error = not can_valid
can_rcv_valid, _ = self.cp.update(int(sec_since_boot() * 1e9), True)
# 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(int(sec_since_boot() * 1e9), False)
self.CS.update(self.cp, self.cp_cam)
self.CS.update(self.cp)
# create message
ret = car.CarState.new_message()
ret.canValid = can_rcv_valid and self.cp.can_valid
# speeds
ret.vEgo = self.CS.v_ego
ret.vEgoRaw = self.CS.v_ego_raw
@@ -345,18 +335,8 @@ class CarInterface(object):
# events
events = []
if not self.CS.can_valid:
self.can_invalid_count += 1
else:
self.can_invalid_count = 0
if can_rcv_error or self.can_invalid_count >= 5:
events.append(create_event('commIssue', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
if self.CS.cam_can_valid:
self.cam_can_valid_count += 1
if self.cam_can_valid_count >= 5:
self.forwarding_camera = True
if self.cp_cam.can_valid:
self.forwarding_camera = True
if not ret.gearShifter == 'drive' and self.CP.enableDsu:
events.append(create_event('wrongGear', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
selfdrive/car/toyota/radar_interface.py
+8 -13
View File
@@ -1,18 +1,15 @@
#!/usr/bin/env python
import os
import zmq
import time
from selfdrive.can.parser import CANParser
from cereal import car
from common.realtime import sec_since_boot
from selfdrive.services import service_list
import selfdrive.messaging as messaging
from selfdrive.car.toyota.values import NO_DSU_CAR, DBC, TSSP2_CAR
from selfdrive.car.toyota.values import NO_DSU_CAR, DBC, TSS2_CAR
def _create_radar_can_parser(car_fingerprint):
dbc_f = DBC[car_fingerprint]['radar']
if car_fingerprint in TSSP2_CAR:
if car_fingerprint in TSS2_CAR:
RADAR_A_MSGS = list(range(0x180, 0x190))
RADAR_B_MSGS = list(range(0x190, 0x1a0))
else:
@@ -39,7 +36,7 @@ class RadarInterface(object):
self.delay = 0.0 # Delay of radar
if CP.carFingerprint in TSSP2_CAR:
if CP.carFingerprint in TSS2_CAR:
self.RADAR_A_MSGS = list(range(0x180, 0x190))
self.RADAR_B_MSGS = list(range(0x190, 0x1a0))
else:
@@ -49,17 +46,15 @@ class RadarInterface(object):
self.valid_cnt = {key: 0 for key in self.RADAR_A_MSGS}
self.rcp = _create_radar_can_parser(CP.carFingerprint)
self.no_dsu_car = CP.carFingerprint in NO_DSU_CAR
context = zmq.Context()
self.logcan = messaging.sub_sock(context, service_list['can'].port)
# No radar dbc for cars without DSU which are not TSS 2.0
# TODO: make a adas dbc file for dsu-less models
self.no_radar = CP.carFingerprint in NO_DSU_CAR and CP.carFingerprint not in TSS2_CAR
def update(self):
ret = car.RadarData.new_message()
if self.no_dsu_car:
# TODO: make a adas dbc file for dsu-less models
if self.no_radar:
time.sleep(0.05)
return ret
@@ -74,7 +69,7 @@ class RadarInterface(object):
errors = []
if not self.rcp.can_valid:
errors.append("commIssue")
errors.append("canError")
ret.errors = errors
ret.canMonoTimes = canMonoTimes
selfdrive/car/toyota/toyotacan.py
+16
View File
@@ -64,6 +64,22 @@ def create_steer_command(packer, steer, steer_req, raw_cnt):
return packer.make_can_msg("STEERING_LKA", 0, values)
def create_lta_steer_command(packer, steer, steer_req, raw_cnt, angle):
"""Creates a CAN message for the Toyota LTA Steer Command."""
values = {
"COUNTER": raw_cnt,
"SETME_X3": 3,
"PERCENTAGE" : 100,
"SETME_X64": 0x64,
"ANGLE": angle,
"STEER_ANGLE_CMD": steer,
"STEER_REQUEST": steer_req,
"BIT": 0,
}
return packer.make_can_msg("STEERING_LTA", 0, values)
def create_accel_command(packer, accel, pcm_cancel, standstill_req, lead):
# TODO: find the exact canceling bit that does not create a chime
values = {
selfdrive/car/toyota/values.py
+38 -26
View File
@@ -13,8 +13,9 @@ class CAR:
HIGHLANDER = "TOYOTA HIGHLANDER 2017"
HIGHLANDERH = "TOYOTA HIGHLANDER HYBRID 2018"
AVALON = "TOYOTA AVALON 2016"
RAV4_2019 = "TOYOTA RAV4 2019"
COROLLA_HATCH = "TOYOTA COROLLA HATCH 2019"
RAV4_TSS2 = "TOYOTA RAV4 2019"
COROLLA_TSS2 = "TOYOTA COROLLA TSS2 2019"
LEXUS_ESH_TSS2 = "LEXUS ES 300H 2019"
LEXUS_ISH = "LEXUS IS HYBRID 2017"
class ECU:
@@ -103,6 +104,10 @@ FINGERPRINTS = {
CAR.PRIUS: [{
# with ipas
36: 8, 37: 8, 166: 8, 170: 8, 180: 8, 295: 8, 296: 8, 426: 6, 452: 8, 466: 8, 467: 8, 512: 6, 513: 6, 550: 8, 552: 4, 560: 7, 562: 6, 581: 5, 608: 8, 610: 8, 614: 8, 643: 7, 658: 8, 713: 8, 740: 5, 742: 8, 743: 8, 800: 8, 810: 2, 814: 8, 824: 2, 829: 2, 830: 7, 835: 8, 836: 8, 845: 5, 863: 8, 869: 7, 870: 7, 871: 2,898: 8, 900: 6, 902: 6, 905: 8, 913: 8, 918: 8, 921: 8, 933: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 8, 955: 8, 956: 8, 971: 7, 974: 8, 975: 5, 993: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1005: 2, 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, 1083: 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, 1175: 8, 1227: 8, 1228: 8, 1235: 8, 1237: 8, 1264: 8, 1279: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1595: 8, 1777: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8
},
#2019 LE
{
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, 614: 8, 643: 7, 658: 8, 713: 8, 740: 5, 742: 8, 743: 8, 800: 8, 810: 2, 814: 8, 829: 2, 830: 7, 835: 8, 836: 8, 863: 8, 865: 8, 869: 7, 870: 7, 871: 2, 896: 8, 898: 8, 900: 6, 902: 6, 905: 8, 918: 8, 921: 8, 933: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 8, 955: 8, 956: 8, 971: 7, 975: 5, 993: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1002: 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, 1083: 8, 1084: 8, 1085: 8, 1086: 8, 1114: 8, 1132: 8, 1161: 8, 1162: 8, 1163: 8, 1175: 8, 1227: 8, 1228: 8, 1235: 8, 1237: 8, 1279: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1592: 8, 1595: 8, 1777: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8
}],
#Corolla w/ added Pedal Support (512L and 513L)
CAR.COROLLA: [{
@@ -164,40 +169,47 @@ FINGERPRINTS = {
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
}],
CAR.RAV4_2019: [
CAR.RAV4_TSS2: [
# LE
{
36: 8, 37: 8, 170: 8, 180: 8, 186: 4, 355: 5, 401: 8, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 544: 4, 550: 8, 552: 4, 562: 6, 565: 8, 608: 8, 610: 8, 643: 7, 705: 8, 728: 8, 740: 5, 742: 8, 743: 8, 761: 8, 764: 8, 765: 8, 800: 8, 810: 2, 812: 8, 824: 8, 829: 2, 830: 7, 835: 8, 836: 8, 865: 8, 869: 7, 870: 7, 871: 2, 877: 8, 881: 8, 882: 8, 885: 8, 896: 8, 898: 8, 900: 6, 902: 6, 905: 8, 921: 8, 933: 8, 934: 8, 935: 8, 944: 8, 945: 8, 951: 8, 955: 8, 956: 8, 976: 1, 998: 5, 999: 7, 1000: 8, 1001: 8, 1002: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1059: 1, 1063: 8, 1076: 8, 1077: 8,1114: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1172: 8, 1235: 8, 1279: 8, 1541: 8, 1552: 8, 1553:8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1592: 8, 1594: 8, 1595: 8, 1649: 8, 1745: 8, 1775: 8, 1779: 8, 1786: 8, 1787: 8, 1788: 8, 1789: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8
},
# XLE and AWD
# XLE, Limited, and AWD
{
36: 8, 37: 8, 170: 8, 180: 8, 186: 4, 401: 8, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 544: 4, 550: 8, 552: 4, 562: 6, 565: 8, 608: 8, 610: 8, 643: 7, 658: 8, 705: 8, 728: 8, 740: 5, 742: 8, 743: 8, 761: 8, 764: 8, 765: 8, 800: 8, 810: 2, 812: 8, 814: 8, 818: 8, 822: 8, 824: 8, 829: 2, 830: 7, 835: 8, 836: 8, 865: 8, 869: 7, 870: 7, 871: 2, 877: 8, 881: 8, 882: 8, 885: 8, 889: 8, 891: 8, 896: 8, 898: 8, 900: 6, 902: 6, 905: 8, 918: 8, 921: 8, 933: 8, 934: 8, 935: 8, 944: 8, 945: 8, 951: 8, 955: 8, 956: 8, 976: 1, 987: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1002: 8, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1059: 1, 1063: 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, 1172: 8, 1228: 8, 1235: 8, 1237: 8, 1263: 8, 1264: 8, 1279: 8, 1541: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1592: 8, 1594: 8, 1595: 8, 1649: 8, 1696: 8, 1745: 8, 1775: 8, 1779: 8, 1786: 8, 1787: 8, 1788: 8, 1789: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8
36: 8, 37: 8, 170: 8, 180: 8, 186: 4, 401: 8, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 544: 4, 550: 8, 552: 4, 562: 6, 565: 8, 608: 8, 610: 8, 643: 7, 658: 8, 705: 8, 728: 8, 740: 5, 742: 8, 743: 8, 761: 8, 764: 8, 765: 8, 800: 8, 810: 2, 812: 8, 814: 8, 818: 8, 822: 8, 824: 8, 829: 2, 830: 7, 835: 8, 836: 8, 865: 8, 869: 7, 870: 7, 871: 2, 877: 8, 881: 8, 882: 8, 885: 8, 889: 8, 891: 8, 896: 8, 898: 8, 900: 6, 902: 6, 905: 8, 918: 8, 921: 8, 933: 8, 934: 8, 935: 8, 944: 8, 945: 8, 951: 8, 955: 8, 956: 8, 976: 1, 987: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1002: 8, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1059: 1, 1063: 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, 1172: 8, 1228: 8, 1235: 8, 1237: 8, 1263: 8, 1264: 8, 1279: 8, 1541: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1592: 8, 1594: 8, 1595: 8, 1649: 8, 1696: 8, 1745: 8, 1775: 8, 1779: 8, 1786: 8, 1787: 8, 1788: 8, 1789: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8, 2015: 8, 2016: 8, 2024: 8
}],
CAR.COROLLA_TSS2: [
# hatch 2019+ and sedan 2020+
{
36: 8, 37: 8, 114: 5, 170: 8, 180: 8, 186: 4, 401: 8, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 544: 4, 550: 8, 552: 4, 562: 6, 608: 8, 610: 8, 643: 7, 705: 8, 728: 8, 740: 5, 742: 8, 743: 8, 761: 8, 764: 8, 765: 8, 800: 8, 810: 2, 812: 8, 824: 8, 829: 2, 830: 7, 835: 8, 836: 8, 865: 8, 869: 7, 870: 7, 871: 2, 877: 8, 881: 8, 896: 8, 898: 8, 900: 6, 902: 6, 905: 8, 921: 8, 933: 8, 934: 8, 935: 8, 944: 8, 945: 8, 951: 8, 955: 8, 956: 8, 976: 1, 998: 5, 999: 7, 1000: 8, 1001: 8, 1002: 8, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1059: 1, 1076: 8, 1077: 8, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1172: 8, 1235: 8, 1237: 8, 1279: 8, 1541: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1592: 8, 1595: 8, 1649: 8, 1745: 8, 1775: 8, 1779: 8, 1786: 8, 1787: 8, 1788: 8, 1789: 8, 1808: 8, 1809: 8, 1816: 8, 1817: 8, 1840: 8, 1848: 8, 1904: 8, 1912: 8, 1940: 8, 1941: 8, 1948: 8, 1949: 8, 1952: 8, 1960: 8, 1981: 8, 1986: 8, 1990: 8, 1994: 8, 1998: 8, 2004: 8
}],
CAR.LEXUS_ESH_TSS2: [
{
36: 8, 37: 8, 166: 8, 170: 8, 180: 8, 295: 8, 296: 8, 401: 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, 742: 8, 743: 8, 744: 8, 761: 8, 764: 8, 765: 8, 800: 8, 810: 2, 812: 8, 814: 8, 818: 8, 824: 8, 829: 2, 830: 7, 835: 8, 836: 8, 863: 8, 865: 8, 869: 7, 870: 7, 871: 2, 877: 8, 881: 8, 882: 8, 885: 8, 889: 8, 896: 8, 898: 8, 900: 6, 902: 6, 905: 8, 913: 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, 987: 8, 993: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1002: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 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, 1172: 8, 1228: 8, 1235: 8, 1264: 8, 1279: 8, 1541: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1575: 8, 1592: 8, 1594: 8, 1595: 8, 1649: 8, 1696: 8, 1775: 8, 1777: 8, 1779: 8, 1786: 8, 1787: 8, 1788: 8, 1789: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8
}],
CAR.COROLLA_HATCH: [{
36: 8, 37: 8, 114: 5, 170: 8, 180: 8, 186: 4, 401: 8, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 544: 4, 550: 8, 552: 4, 562: 6, 608: 8, 610: 8, 643: 7, 705: 8, 728: 8, 740: 5, 742: 8, 743: 8, 761: 8, 764: 8, 765: 8, 800: 8, 810: 2, 812: 8, 824: 8, 829: 2, 830: 7, 835: 8, 836: 8, 865: 8, 869: 7, 870: 7, 871: 2, 877: 8, 881: 8, 896: 8, 898: 8, 900: 6, 902: 6, 905: 8, 921: 8, 933: 8, 934: 8, 935: 8, 944: 8, 945: 8, 951: 8, 955: 8, 956: 8, 976: 1, 998: 5, 999: 7, 1000: 8, 1001: 8, 1002: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1059: 1, 1076: 8, 1077: 8, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1172: 8, 1235: 8, 1279: 8, 1541: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1592: 8, 1595: 8, 1649: 8, 1745: 8, 1775: 8, 1779: 8, 1786: 8, 1787: 8, 1788: 8, 1789: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8
}]
}
STEER_THRESHOLD = 100
DBC = {
CAR.RAV4H: dbc_dict('toyota_rav4_hybrid_2017_pt_generated', 'toyota_prius_2017_adas'),
CAR.RAV4: dbc_dict('toyota_rav4_2017_pt_generated', 'toyota_prius_2017_adas'),
CAR.PRIUS: dbc_dict('toyota_prius_2017_pt_generated', 'toyota_prius_2017_adas'),
CAR.COROLLA: dbc_dict('toyota_corolla_2017_pt_generated', 'toyota_prius_2017_adas'),
CAR.LEXUS_RXH: dbc_dict('lexus_rx_hybrid_2017_pt_generated', 'toyota_prius_2017_adas'),
CAR.CHR: dbc_dict('toyota_chr_2018_pt_generated', 'toyota_prius_2017_adas'),
CAR.CHRH: dbc_dict('toyota_chr_hybrid_2018_pt_generated', 'toyota_prius_2017_adas'),
CAR.CAMRY: dbc_dict('toyota_chr_2018_pt_generated', 'toyota_prius_2017_adas'),
CAR.CAMRYH: dbc_dict('toyota_camry_hybrid_2018_pt_generated', 'toyota_prius_2017_adas'),
CAR.HIGHLANDER: dbc_dict('toyota_highlander_2017_pt_generated', 'toyota_prius_2017_adas'),
CAR.HIGHLANDERH: dbc_dict('toyota_highlander_hybrid_2018_pt_generated', 'toyota_prius_2017_adas'),
CAR.AVALON: dbc_dict('toyota_avalon_2017_pt_generated', 'toyota_prius_2017_adas'),
CAR.RAV4_2019: dbc_dict('toyota_chr_2018_pt_generated', 'toyota_rav4_2019_adas'),
CAR.COROLLA_HATCH: dbc_dict('toyota_chr_2018_pt_generated', 'toyota_rav4_2019_adas'),
CAR.LEXUS_ISH: dbc_dict('lexus_is_hybrid_2017_pt_generated', 'toyota_prius_2017_adas'),
CAR.RAV4H: dbc_dict('toyota_rav4_hybrid_2017_pt_generated', 'toyota_adas'),
CAR.RAV4: dbc_dict('toyota_rav4_2017_pt_generated', 'toyota_adas'),
CAR.PRIUS: dbc_dict('toyota_prius_2017_pt_generated', 'toyota_adas'),
CAR.COROLLA: dbc_dict('toyota_corolla_2017_pt_generated', 'toyota_adas'),
CAR.LEXUS_RXH: dbc_dict('lexus_rx_hybrid_2017_pt_generated', 'toyota_adas'),
CAR.CHR: dbc_dict('toyota_nodsu_pt_generated', 'toyota_adas'),
CAR.CHRH: dbc_dict('toyota_nodsu_hybrid_pt_generated', 'toyota_adas'),
CAR.CAMRY: dbc_dict('toyota_nodsu_pt_generated', 'toyota_adas'),
CAR.CAMRYH: dbc_dict('toyota_camry_hybrid_2018_pt_generated', 'toyota_adas'),
CAR.HIGHLANDER: dbc_dict('toyota_highlander_2017_pt_generated', 'toyota_adas'),
CAR.HIGHLANDERH: dbc_dict('toyota_highlander_hybrid_2018_pt_generated', 'toyota_adas'),
CAR.AVALON: dbc_dict('toyota_avalon_2017_pt_generated', 'toyota_adas'),
CAR.RAV4_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'),
CAR.COROLLA_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'),
CAR.LEXUS_ESH_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'),
CAR.LEXUS_ISH: dbc_dict('lexus_is_hybrid_2017_pt_generated', 'toyota_adas'),
}
NO_DSU_CAR = [CAR.CHR, CAR.CHRH, CAR.CAMRY, CAR.CAMRYH]
TSSP2_CAR = [CAR.RAV4_2019, CAR.COROLLA_HATCH]
NO_STOP_TIMER_CAR = [CAR.RAV4H, CAR.HIGHLANDERH, CAR.HIGHLANDER, CAR.RAV4_2019, CAR.COROLLA_HATCH, CAR.LEXUS_ISH] # no resume button press required
NO_DSU_CAR = [CAR.CHR, CAR.CHRH, CAR.CAMRY, CAR.CAMRYH, CAR.RAV4_TSS2, CAR.COROLLA_TSS2, CAR.LEXUS_ESH_TSS2]
TSS2_CAR = [CAR.RAV4_TSS2, CAR.COROLLA_TSS2, CAR.LEXUS_ESH_TSS2]
NO_STOP_TIMER_CAR = [CAR.RAV4H, CAR.HIGHLANDERH, CAR.HIGHLANDER, CAR.RAV4_TSS2, CAR.COROLLA_TSS2, CAR.LEXUS_ESH_TSS2, CAR.LEXUS_ISH] # no resume button press required
selfdrive/common/cereal.mk
+4 -11
View File
@@ -1,19 +1,15 @@
UNAME_M ?= $(shell uname -m)
UNAME_S ?= $(shell uname -s)
CEREAL_CFLAGS = -I$(PHONELIBS)/capnp-c/include
ifeq ($(UNAME_S),Darwin)
CEREAL_CFLAGS = -I$(PHONELIBS)/capnp-c/include
CEREAL_CXXFLAGS = -I$(PHONELIBS)/capnp-cpp/mac/include
CEREAL_LIBS = $(PHONELIBS)/capnp-cpp/mac/lib/libcapnp.a \
$(PHONELIBS)/capnp-cpp/mac/lib/libkj.a \
$(PHONELIBS)/capnp-c/mac/lib/libcapnp_c.a
else ifeq ($(UNAME_M),x86_64)
CEREAL_CFLAGS = -I$(PHONELIBS)/capnp-c/include
CEREAL_CXXFLAGS = -I$(PHONELIBS)/capnp-cpp/include
ifeq ($(CEREAL_LIBS),)
CEREAL_LIBS = -L$(PHONELIBS)/capnp-cpp/x64/lib/ \
@@ -23,15 +19,12 @@ endif
else
CEREAL_CXXFLAGS = -I$(PHONELIBS)/capnp-cpp/include
#CEREAL_CXXFLAGS = -I$(PHONELIBS)/capnp-cpp/include
ifeq ($(CEREAL_LIBS),)
CEREAL_LIBS = -L$(PHONELIBS)/capnp-cpp/aarch64/lib/ \
-L$(PHONELIBS)/capnp-c/aarch64/lib/ \
-l:libcapn.a -l:libcapnp.a -l:libkj.a
CEREAL_LIBS = -l:libcapn.a -l:libcapnp.a -l:libkj.a
endif
endif
CEREAL_OBJS = ../../cereal/gen/c/log.capnp.o ../../cereal/gen/c/car.capnp.o
log.capnp.o: ../../cereal/gen/cpp/log.capnp.c++
selfdrive/common/cqueue.c
+52
View File
@@ -0,0 +1,52 @@
#include <stdlib.h>
#include <string.h>
#include <assert.h>
#include "cqueue.h"
void queue_init(Queue *q) {
memset(q, 0, sizeof(*q));
TAILQ_INIT(&q->q);
pthread_mutex_init(&q->lock, NULL);
pthread_cond_init(&q->cv, NULL);
}
void* queue_pop(Queue *q) {
pthread_mutex_lock(&q->lock);
while (TAILQ_EMPTY(&q->q)) {
pthread_cond_wait(&q->cv, &q->lock);
}
QueueEntry *entry = TAILQ_FIRST(&q->q);
TAILQ_REMOVE(&q->q, entry, entries);
pthread_mutex_unlock(&q->lock);
void* r = entry->data;
free(entry);
return r;
}
void* queue_try_pop(Queue *q) {
pthread_mutex_lock(&q->lock);
void* r = NULL;
if (!TAILQ_EMPTY(&q->q)) {
QueueEntry *entry = TAILQ_FIRST(&q->q);
TAILQ_REMOVE(&q->q, entry, entries);
r = entry->data;
free(entry);
}
pthread_mutex_unlock(&q->lock);
return r;
}
void queue_push(Queue *q, void *data) {
QueueEntry *entry = calloc(1, sizeof(QueueEntry));
assert(entry);
entry->data = data;
pthread_mutex_lock(&q->lock);
TAILQ_INSERT_TAIL(&q->q, entry, entries);
pthread_cond_signal(&q->cv);
pthread_mutex_unlock(&q->lock);
}
selfdrive/common/cqueue.h
+33
View File
@@ -0,0 +1,33 @@
#ifndef COMMON_CQUEUE_H
#define COMMON_CQUEUE_H
#include <sys/queue.h>
#include <pthread.h>
#ifdef __cplusplus
extern "C" {
#endif
// a blocking queue
typedef struct QueueEntry {
TAILQ_ENTRY(QueueEntry) entries;
void *data;
} QueueEntry;
typedef struct Queue {
TAILQ_HEAD(queue, QueueEntry) q;
pthread_mutex_t lock;
pthread_cond_t cv;
} Queue;
void queue_init(Queue *q);
void* queue_pop(Queue *q);
void* queue_try_pop(Queue *q);
void queue_push(Queue *q, void *data);
#ifdef __cplusplus
} // extern "C"
#endif
#endif
selfdrive/common/modeldata.h
+6 -1
View File
@@ -1,18 +1,23 @@
#ifndef MODELDATA_H
#define MODELDATA_H
#define MODEL_PATH_DISTANCE 50
#define MODEL_PATH_DISTANCE 100
#define POLYFIT_DEGREE 4
typedef struct PathData {
float points[MODEL_PATH_DISTANCE];
float prob;
float std;
float stds[MODEL_PATH_DISTANCE];
float poly[POLYFIT_DEGREE];
} PathData;
typedef struct LeadData {
float dist;
float prob;
float std;
float rel_v;
float rel_v_std;
} LeadData;
typedef struct ModelData {
selfdrive/common/mutex.h
+13
View File
@@ -0,0 +1,13 @@
#ifndef COMMON_MUTEX_H
#define COMMON_MUTEX_H
#include <pthread.h>
static inline void mutex_init_reentrant(pthread_mutex_t *mutex) {
pthread_mutexattr_t attr;
pthread_mutexattr_init(&attr);
pthread_mutexattr_settype(&attr, PTHREAD_MUTEX_RECURSIVE);
pthread_mutex_init(mutex, &attr);
}
#endif
selfdrive/common/util.h
+1
View File
@@ -1,6 +1,7 @@
#ifndef COMMON_UTIL_H
#define COMMON_UTIL_H
#include <stdio.h>
#ifndef __cplusplus
selfdrive/common/version.h
+1 -1
View File
@@ -1 +1 @@
#define COMMA_VERSION "0.5.13-release"
#define COMMA_VERSION "0.6-release"
selfdrive/controls/controlsd.py
+147 -182
View File
@@ -1,12 +1,9 @@
#!/usr/bin/env python
import gc
import zmq
import json
from collections import defaultdict
import capnp
from cereal import car, log
from common.numpy_fast import clip
from common.realtime import sec_since_boot, set_realtime_priority, Ratekeeper
from common.realtime import sec_since_boot, set_realtime_priority, Ratekeeper, DT_CTRL
from common.profiler import Profiler
from common.params import Params
import selfdrive.messaging as messaging
@@ -15,8 +12,7 @@ from selfdrive.services import service_list
from selfdrive.boardd.boardd import can_list_to_can_capnp
from selfdrive.car.car_helpers import get_car, get_startup_alert
from selfdrive.controls.lib.model_parser import CAMERA_OFFSET
from selfdrive.controls.lib.drive_helpers import learn_angle_model_bias, \
get_events, \
from selfdrive.controls.lib.drive_helpers import get_events, \
create_event, \
EventTypes as ET, \
update_v_cruise, \
@@ -27,7 +23,7 @@ from selfdrive.controls.lib.latcontrol_indi import LatControlINDI
from selfdrive.controls.lib.alertmanager import AlertManager
from selfdrive.controls.lib.vehicle_model import VehicleModel
from selfdrive.controls.lib.driver_monitor import DriverStatus
from selfdrive.controls.lib.planner import _DT_MPC
from selfdrive.controls.lib.planner import LON_MPC_STEP
from selfdrive.locationd.calibration_helpers import Calibration, Filter
ThermalStatus = log.ThermalData.ThermalStatus
@@ -43,10 +39,18 @@ def isEnabled(state):
"""Check if openpilot is engaged"""
return (isActive(state) or state == State.preEnabled)
def events_to_bytes(events):
# optimization when comparing capnp structs: str() or tree traverse are much slower
ret = []
for e in events:
if isinstance(e, capnp.lib.capnp._DynamicStructReader):
e = e.as_builder()
ret.append(e.to_bytes())
return ret
def data_sample(rcv_times, CI, CC, plan_sock, path_plan_sock, thermal, calibration, health, driver_monitor,
poller, cal_status, cal_perc, overtemp, free_space, low_battery,
driver_status, state, mismatch_counter, params, plan, path_plan):
def data_sample(CI, CC, sm, 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
@@ -54,33 +58,12 @@ def data_sample(rcv_times, CI, CC, plan_sock, path_plan_sock, thermal, calibrati
events = list(CS.events)
enabled = isEnabled(state)
# Receive from sockets
td = None
cal = None
hh = None
dm = None
sm.update(0)
for socket, event in poller.poll(0):
msg = messaging.recv_one(socket)
rcv_times[msg.which()] = sec_since_boot()
if socket is thermal:
td = msg
elif socket is calibration:
cal = msg
elif socket is health:
hh = msg
elif socket is driver_monitor:
dm = msg
elif socket is plan_sock:
plan = msg
elif socket is path_plan_sock:
path_plan = msg
if td is not None:
overtemp = td.thermal.thermalStatus >= ThermalStatus.red
free_space = td.thermal.freeSpace < 0.07 # under 7% of space free no enable allowed
low_battery = td.thermal.batteryPercent < 1 and td.thermal.chargingError # at zero percent battery, while discharging, OP should not be allowed
if sm.updated['thermal']:
overtemp = sm['thermal'].thermalStatus >= ThermalStatus.red
free_space = sm['thermal'].freeSpace < 0.07 # under 7% of space free no enable allowed
low_battery = sm['thermal'].batteryPercent < 1 and sm['thermal'].chargingError # at zero percent battery, while discharging, OP should not allowed
# Create events for battery, temperature and disk space
if low_battery:
@@ -91,9 +74,9 @@ def data_sample(rcv_times, CI, CC, plan_sock, path_plan_sock, thermal, calibrati
events.append(create_event('outOfSpace', [ET.NO_ENTRY]))
# Handle calibration
if cal is not None:
cal_status = cal.liveCalibration.calStatus
cal_perc = cal.liveCalibration.calPerc
if sm.updated['liveCalibration']:
cal_status = sm['liveCalibration'].calStatus
cal_perc = sm['liveCalibration'].calPerc
if cal_status != Calibration.CALIBRATED:
if cal_status == Calibration.UNCALIBRATED:
@@ -103,26 +86,26 @@ def data_sample(rcv_times, CI, CC, plan_sock, path_plan_sock, thermal, calibrati
# When the panda and controlsd do not agree on controls_allowed
# we want to disengage openpilot. However the status from the panda goes through
# another socket than the CAN messages, therefore one can arrive earlier than the other.
# another socket other than the CAN messages and one can arrive earlier than the other.
# Therefore we allow a mismatch for two samples, then we trigger the disengagement.
if not enabled:
mismatch_counter = 0
if hh is not None:
controls_allowed = hh.health.controlsAllowed
if sm.updated['health']:
controls_allowed = sm['health'].controlsAllowed
if not controls_allowed and enabled:
mismatch_counter += 1
if mismatch_counter >= 2:
events.append(create_event('controlsMismatch', [ET.IMMEDIATE_DISABLE]))
# Driver monitoring
if dm is not None:
driver_status.get_pose(dm.driverMonitoring, params)
if sm.updated['driverMonitoring']:
driver_status.get_pose(sm['driverMonitoring'], params)
return CS, events, cal_status, cal_perc, overtemp, free_space, low_battery, mismatch_counter, plan, path_plan
return CS, events, cal_status, cal_perc, overtemp, free_space, low_battery, mismatch_counter
def state_transition(CS, CP, state, events, soft_disable_timer, v_cruise_kph, AM):
def state_transition(frame, CS, CP, state, events, soft_disable_timer, v_cruise_kph, AM):
"""Compute conditional state transitions and execute actions on state transitions"""
enabled = isEnabled(state)
@@ -143,43 +126,43 @@ def state_transition(CS, CP, state, events, soft_disable_timer, v_cruise_kph, AM
if get_events(events, [ET.ENABLE]):
if get_events(events, [ET.NO_ENTRY]):
for e in get_events(events, [ET.NO_ENTRY]):
AM.add(str(e) + "NoEntry", enabled)
AM.add(frame, str(e) + "NoEntry", enabled)
else:
if get_events(events, [ET.PRE_ENABLE]):
state = State.preEnabled
else:
state = State.enabled
AM.add("enable", enabled)
AM.add(frame, "enable", enabled)
v_cruise_kph = initialize_v_cruise(CS.vEgo, CS.buttonEvents, v_cruise_kph_last)
# ENABLED
elif state == State.enabled:
if get_events(events, [ET.USER_DISABLE]):
state = State.disabled
AM.add("disable", enabled)
AM.add(frame, "disable", enabled)
elif get_events(events, [ET.IMMEDIATE_DISABLE]):
state = State.disabled
for e in get_events(events, [ET.IMMEDIATE_DISABLE]):
AM.add(e, enabled)
AM.add(frame, e, enabled)
elif get_events(events, [ET.SOFT_DISABLE]):
state = State.softDisabling
soft_disable_timer = 300 # 3s
for e in get_events(events, [ET.SOFT_DISABLE]):
AM.add(e, enabled)
AM.add(frame, e, enabled)
# SOFT DISABLING
elif state == State.softDisabling:
if get_events(events, [ET.USER_DISABLE]):
state = State.disabled
AM.add("disable", enabled)
AM.add(frame, "disable", enabled)
elif get_events(events, [ET.IMMEDIATE_DISABLE]):
state = State.disabled
for e in get_events(events, [ET.IMMEDIATE_DISABLE]):
AM.add(e, enabled)
AM.add(frame, e, enabled)
elif not get_events(events, [ET.SOFT_DISABLE]):
# no more soft disabling condition, so go back to ENABLED
@@ -187,7 +170,7 @@ def state_transition(CS, CP, state, events, soft_disable_timer, v_cruise_kph, AM
elif get_events(events, [ET.SOFT_DISABLE]) and soft_disable_timer > 0:
for e in get_events(events, [ET.SOFT_DISABLE]):
AM.add(e, enabled)
AM.add(frame, e, enabled)
elif soft_disable_timer <= 0:
state = State.disabled
@@ -196,12 +179,12 @@ def state_transition(CS, CP, state, events, soft_disable_timer, v_cruise_kph, AM
elif state == State.preEnabled:
if get_events(events, [ET.USER_DISABLE]):
state = State.disabled
AM.add("disable", enabled)
AM.add(frame, "disable", enabled)
elif get_events(events, [ET.IMMEDIATE_DISABLE, ET.SOFT_DISABLE]):
state = State.disabled
for e in get_events(events, [ET.IMMEDIATE_DISABLE, ET.SOFT_DISABLE]):
AM.add(e, enabled)
AM.add(frame, e, enabled)
elif not get_events(events, [ET.PRE_ENABLE]):
state = State.enabled
@@ -209,8 +192,8 @@ def state_transition(CS, CP, state, events, soft_disable_timer, v_cruise_kph, AM
return state, soft_disable_timer, v_cruise_kph, v_cruise_kph_last
def state_control(rcv_times, plan, path_plan, CS, CP, state, events, v_cruise_kph, v_cruise_kph_last, AM, rk,
driver_status, LaC, LoC, VM, angle_model_bias, read_only, is_metric, cal_perc):
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):
"""Given the state, this function returns an actuators packet"""
actuators = car.CarControl.Actuators.new_message()
@@ -228,7 +211,7 @@ def state_control(rcv_times, plan, path_plan, CS, CP, state, events, v_cruise_kp
# send FCW alert if triggered by planner
if plan.fcw:
AM.add("fcw", enabled)
AM.add(frame, "fcw", enabled)
# State specific actions
@@ -245,20 +228,12 @@ def state_control(rcv_times, plan, path_plan, CS, CP, state, events, v_cruise_kp
extra_text = str(int(round(CP.minSteerSpeed * CV.MS_TO_KPH))) + " kph"
else:
extra_text = str(int(round(CP.minSteerSpeed * CV.MS_TO_MPH))) + " mph"
AM.add(e, enabled, extra_text_2=extra_text)
AM.add(frame, e, enabled, extra_text_2=extra_text)
# Run angle offset learner at 20 Hz
if rk.frame % 5 == 2:
angle_model_bias = learn_angle_model_bias(active, CS.vEgo, angle_model_bias,
path_plan.cPoly, path_plan.cProb, CS.steeringAngle,
CS.steeringPressed)
plan_age = DT_CTRL * (frame - rcv_frame['plan'])
dt = min(plan_age, LON_MPC_STEP + DT_CTRL) + DT_CTRL # no greater than dt mpc + dt, to prevent too high extraps
cur_time = sec_since_boot()
radar_time = rcv_times['plan'] - plan.processingDelay # Subtract processing delay to get the original measurement time
_DT = 0.01 # 100Hz
dt = min(cur_time - radar_time, _DT_MPC + _DT) + _DT # no greater than dt mpc + dt, to prevent too high extraps
a_acc_sol = plan.aStart + (dt / _DT_MPC) * (plan.aTarget - plan.aStart)
a_acc_sol = plan.aStart + (dt / LON_MPC_STEP) * (plan.aTarget - plan.aStart)
v_acc_sol = plan.vStart + dt * (a_acc_sol + plan.aStart) / 2.0
# Gas/Brake PID loop
@@ -270,7 +245,7 @@ def state_control(rcv_times, plan, path_plan, CS, CP, state, events, v_cruise_kp
# Send a "steering required alert" if saturation count has reached the limit
if LaC.sat_flag and CP.steerLimitAlert:
AM.add("steerSaturated", enabled)
AM.add(frame, "steerSaturated", enabled)
# Parse permanent warnings to display constantly
for e in get_events(events, [ET.PERMANENT]):
@@ -281,65 +256,63 @@ def state_control(rcv_times, plan, path_plan, CS, CP, state, events, v_cruise_kp
extra_text_2 = str(int(round(Filter.MIN_SPEED * CV.MS_TO_KPH))) + " kph"
else:
extra_text_2 = str(int(round(Filter.MIN_SPEED * CV.MS_TO_MPH))) + " mph"
AM.add(str(e) + "Permanent", enabled, extra_text_1=extra_text_1, extra_text_2=extra_text_2)
AM.add(frame, str(e) + "Permanent", enabled, extra_text_1=extra_text_1, extra_text_2=extra_text_2)
AM.process_alerts(sec_since_boot())
AM.process_alerts(frame)
return actuators, v_cruise_kph, driver_status, angle_model_bias, v_acc_sol, a_acc_sol, lac_log
return actuators, v_cruise_kph, driver_status, v_acc_sol, a_acc_sol, lac_log
def data_send(plan, path_plan, CS, CI, CP, VM, state, events, actuators, v_cruise_kph, rk, carstate,
carcontrol, controlsstate, sendcan, AM, driver_status,
LaC, LoC, angle_model_bias, read_only, start_time, v_acc, a_acc, lac_log):
def data_send(sm, CS, CI, CP, VM, state, events, actuators, v_cruise_kph, rk, carstate,
carcontrol, carevents, carparams, controlsstate, sendcan, AM, driver_status,
LaC, LoC, read_only, start_time, v_acc, a_acc, lac_log, events_prev):
"""Send actuators and hud commands to the car, send controlsstate and MPC logging"""
plan_ts = plan.logMonoTime
plan = plan.plan
CC = car.CarControl.new_message()
CC.enabled = isEnabled(state)
CC.actuators = actuators
CC.cruiseControl.override = True
CC.cruiseControl.cancel = not CP.enableCruise or (not isEnabled(state) and CS.cruiseState.enabled)
# Some override values for Honda
brake_discount = (1.0 - clip(actuators.brake * 3., 0.0, 1.0)) # brake discount removes a sharp nonlinearity
CC.cruiseControl.speedOverride = float(max(0.0, (LoC.v_pid + CS.cruiseState.speedOffset) * brake_discount) if CP.enableCruise else 0.0)
CC.cruiseControl.accelOverride = CI.calc_accel_override(CS.aEgo, sm['plan'].aTarget, CS.vEgo, sm['plan'].vTarget)
CC.hudControl.setSpeed = float(v_cruise_kph * CV.KPH_TO_MS)
CC.hudControl.speedVisible = isEnabled(state)
CC.hudControl.lanesVisible = isEnabled(state)
CC.hudControl.leadVisible = sm['plan'].hasLead
right_lane_visible = sm['pathPlan'].rProb > 0.5
left_lane_visible = sm['pathPlan'].lProb > 0.5
CC.hudControl.rightLaneVisible = bool(right_lane_visible)
CC.hudControl.leftLaneVisible = bool(left_lane_visible)
blinker = CS.leftBlinker or CS.rightBlinker
ldw_allowed = CS.vEgo > 12.5 and not blinker
if len(list(sm['pathPlan'].rPoly)) == 4:
CC.hudControl.rightLaneDepart = bool(ldw_allowed and sm['pathPlan'].rPoly[3] > -(1 + CAMERA_OFFSET) and right_lane_visible)
if len(list(sm['pathPlan'].lPoly)) == 4:
CC.hudControl.leftLaneDepart = bool(ldw_allowed and sm['pathPlan'].lPoly[3] < (1 - CAMERA_OFFSET) and left_lane_visible)
CC.hudControl.visualAlert = AM.visual_alert
CC.hudControl.audibleAlert = AM.audible_alert
if not read_only:
CC.enabled = isEnabled(state)
CC.actuators = actuators
CC.cruiseControl.override = True
CC.cruiseControl.cancel = not CP.enableCruise or (not isEnabled(state) and CS.cruiseState.enabled)
# Some override values for Honda
brake_discount = (1.0 - clip(actuators.brake * 3., 0.0, 1.0)) # brake discount removes a sharp nonlinearity
CC.cruiseControl.speedOverride = float(max(0.0, (LoC.v_pid + CS.cruiseState.speedOffset) * brake_discount) if CP.enableCruise else 0.0)
CC.cruiseControl.accelOverride = CI.calc_accel_override(CS.aEgo, plan.aTarget, CS.vEgo, plan.vTarget)
CC.hudControl.setSpeed = float(v_cruise_kph * CV.KPH_TO_MS)
CC.hudControl.speedVisible = isEnabled(state)
CC.hudControl.lanesVisible = isEnabled(state)
CC.hudControl.leadVisible = plan.hasLead
right_lane_visible = path_plan.pathPlan.rProb > 0.5
left_lane_visible = path_plan.pathPlan.lProb > 0.5
CC.hudControl.rightLaneVisible = bool(right_lane_visible)
CC.hudControl.leftLaneVisible = bool(left_lane_visible)
blinker = CS.leftBlinker or CS.rightBlinker
ldw_allowed = CS.vEgo > 12.5 and not blinker
if len(list(path_plan.pathPlan.rPoly)) == 4:
CC.hudControl.rightLaneDepart = bool(ldw_allowed and path_plan.pathPlan.rPoly[3] > -(1 + CAMERA_OFFSET) and right_lane_visible)
if len(list(path_plan.pathPlan.lPoly)) == 4:
CC.hudControl.leftLaneDepart = bool(ldw_allowed and path_plan.pathPlan.lPoly[3] < (1 - CAMERA_OFFSET) and left_lane_visible)
CC.hudControl.visualAlert = AM.visual_alert
CC.hudControl.audibleAlert = AM.audible_alert
# send car controls over can
can_sends = CI.apply(CC)
sendcan.send(can_list_to_can_capnp(can_sends, msgtype='sendcan'))
sendcan.send(can_list_to_can_capnp(can_sends, msgtype='sendcan', valid=CS.canValid))
force_decel = driver_status.awareness < 0.
# controlsState
dat = messaging.new_message()
dat.init('controlsState')
dat.valid = CS.canValid
dat.controlsState = {
"alertText1": AM.alert_text_1,
"alertText2": AM.alert_text_2,
@@ -351,8 +324,8 @@ def data_send(plan, path_plan, CS, CI, CP, VM, state, events, actuators, v_cruis
"awarenessStatus": max(driver_status.awareness, 0.0) if isEnabled(state) else 0.0,
"driverMonitoringOn": bool(driver_status.monitor_on and driver_status.face_detected),
"canMonoTimes": list(CS.canMonoTimes),
"planMonoTime": plan_ts,
"pathPlanMonoTime": path_plan.logMonoTime,
"planMonoTime": sm.logMonoTime['plan'],
"pathPlanMonoTime": sm.logMonoTime['pathPlan'],
"enabled": isEnabled(state),
"active": isActive(state),
"vEgo": CS.vEgo,
@@ -371,14 +344,14 @@ def data_send(plan, path_plan, CS, CI, CP, VM, state, events, actuators, v_cruis
"angleSteersDes": float(LaC.angle_steers_des),
"vTargetLead": float(v_acc),
"aTarget": float(a_acc),
"jerkFactor": float(plan.jerkFactor),
"angleModelBias": float(angle_model_bias),
"gpsPlannerActive": plan.gpsPlannerActive,
"vCurvature": plan.vCurvature,
"decelForTurn": plan.decelForTurn,
"jerkFactor": float(sm['plan'].jerkFactor),
"angleModelBias": 0.,
"gpsPlannerActive": sm['plan'].gpsPlannerActive,
"vCurvature": sm['plan'].vCurvature,
"decelForTurn": sm['plan'].decelForTurn,
"cumLagMs": -rk.remaining * 1000.,
"startMonoTime": int(start_time * 1e9),
"mapValid": plan.mapValid,
"mapValid": sm['plan'].mapValid,
"forceDecel": bool(force_decel),
}
@@ -391,17 +364,34 @@ def data_send(plan, path_plan, CS, CI, CP, VM, state, events, actuators, v_cruis
# carState
cs_send = messaging.new_message()
cs_send.init('carState')
cs_send.valid = CS.canValid
cs_send.carState = CS
cs_send.carState.events = events
carstate.send(cs_send.to_bytes())
# carEvents - logged every second or on change
events_bytes = events_to_bytes(events)
if (sm.frame % int(1. / DT_CTRL) == 0) or (events_bytes != events_prev):
ce_send = messaging.new_message()
ce_send.init('carEvents', len(events))
ce_send.carEvents = events
carevents.send(ce_send.to_bytes())
# carParams - logged every 50 seconds (> 1 per segment)
if (sm.frame % int(50. / DT_CTRL) == 0):
cp_send = messaging.new_message()
cp_send.init('carParams')
cp_send.carParams = CP
carparams.send(cp_send.to_bytes())
# carControl
cc_send = messaging.new_message()
cc_send.init('carControl')
cc_send.valid = CS.canValid
cc_send.carControl = CC
carcontrol.send(cc_send.to_bytes())
return CC
return CC, events_bytes
def controlsd_thread(gctx=None):
@@ -410,28 +400,21 @@ def controlsd_thread(gctx=None):
# start the loop
set_realtime_priority(3)
context = zmq.Context()
params = Params()
# Pub Sockets
controlsstate = messaging.pub_sock(context, service_list['controlsState'].port)
carstate = messaging.pub_sock(context, service_list['carState'].port)
carcontrol = messaging.pub_sock(context, service_list['carControl'].port)
sendcan = messaging.pub_sock(service_list['sendcan'].port)
controlsstate = messaging.pub_sock(service_list['controlsState'].port)
carstate = messaging.pub_sock(service_list['carState'].port)
carcontrol = messaging.pub_sock(service_list['carControl'].port)
carevents = messaging.pub_sock(service_list['carEvents'].port)
carparams = messaging.pub_sock(service_list['carParams'].port)
is_metric = params.get("IsMetric") == "1"
passive = params.get("Passive") != "0"
sendcan = messaging.pub_sock(context, service_list['sendcan'].port)
# Sub sockets
poller = zmq.Poller()
thermal = messaging.sub_sock(context, service_list['thermal'].port, conflate=True, poller=poller)
health = messaging.sub_sock(context, service_list['health'].port, conflate=True, poller=poller)
cal = messaging.sub_sock(context, service_list['liveCalibration'].port, conflate=True, poller=poller)
driver_monitor = messaging.sub_sock(context, service_list['driverMonitoring'].port, conflate=True, poller=poller)
plan_sock = messaging.sub_sock(context, service_list['plan'].port, conflate=True, poller=poller)
path_plan_sock = messaging.sub_sock(context, service_list['pathPlan'].port, conflate=True, poller=poller)
logcan = messaging.sub_sock(context, service_list['can'].port)
sm = messaging.SubMaster(['thermal', 'health', 'liveCalibration', 'driverMonitoring', 'plan', 'pathPlan'])
logcan = messaging.sub_sock(service_list['can'].port)
CC = car.CarControl.new_message()
CI, CP = get_car(logcan, sendcan)
@@ -442,10 +425,10 @@ def controlsd_thread(gctx=None):
controller_available = CP.enableCamera and CI.CC is not None and not passive
read_only = not car_recognized or not controller_available
if read_only:
CP.safetyModel = car.CarParams.SafetyModels.elm327 # diagnostic only
CP.safetyModel = car.CarParams.SafetyModel.elm327 # diagnostic only
startup_alert = get_startup_alert(car_recognized, controller_available)
AM.add(startup_alert, False)
AM.add(sm.frame, startup_alert, False)
LoC = LongControl(CP, CI.compute_gb)
VM = VehicleModel(CP)
@@ -471,27 +454,12 @@ def controlsd_thread(gctx=None):
cal_perc = 0
mismatch_counter = 0
low_battery = False
events_prev = []
rcv_times = defaultdict(int)
plan = messaging.new_message()
plan.init('plan')
path_plan = messaging.new_message()
path_plan.init('pathPlan')
path_plan.pathPlan.sensorValid = True
sm['pathPlan'].sensorValid = True
# controlsd is driven by can recv, expected at 100Hz
rk = Ratekeeper(100, print_delay_threshold=None)
controls_params = params.get("ControlsParams")
# Read angle offset from previous drive
angle_model_bias = 0.
if controls_params is not None:
try:
controls_params = json.loads(controls_params)
angle_model_bias = controls_params['angle_model_bias']
except (ValueError, KeyError):
pass
prof = Profiler(False) # off by default
@@ -500,50 +468,47 @@ def controlsd_thread(gctx=None):
prof.checkpoint("Ratekeeper", ignore=True)
# Sample data and compute car events
CS, events, cal_status, cal_perc, overtemp, free_space, low_battery, mismatch_counter, plan, path_plan =\
data_sample(rcv_times, CI, CC, plan_sock, path_plan_sock, thermal, cal, health, driver_monitor,
poller, cal_status, cal_perc, overtemp, free_space, low_battery, driver_status,
state, mismatch_counter, params, plan, path_plan)
CS, events, cal_status, cal_perc, overtemp, free_space, low_battery, mismatch_counter =\
data_sample(CI, CC, sm, cal_status, cal_perc, overtemp, free_space, low_battery,
driver_status, state, mismatch_counter, params)
prof.checkpoint("Sample")
# Create alerts
path_plan_age = start_time - rcv_times['pathPlan']
plan_age = start_time - rcv_times['plan']
if not path_plan.pathPlan.valid or plan_age > 0.5 or path_plan_age > 0.5:
events.append(create_event('plannerError', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
if not path_plan.pathPlan.sensorValid:
if not sm.all_alive_and_valid():
events.append(create_event('commIssue', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
if not sm['pathPlan'].mpcSolutionValid:
events.append(create_event('plannerError', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
if not sm['pathPlan'].sensorValid:
events.append(create_event('sensorDataInvalid', [ET.NO_ENTRY, ET.PERMANENT]))
if not path_plan.pathPlan.paramsValid:
if not sm['pathPlan'].paramsValid:
events.append(create_event('vehicleModelInvalid', [ET.WARNING]))
if not path_plan.pathPlan.modelValid:
events.append(create_event('modelCommIssue', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
if not plan.plan.radarValid:
if not sm['plan'].radarValid:
events.append(create_event('radarFault', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
if plan.plan.radarCommIssue:
events.append(create_event('radarCommIssue', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
if sm['plan'].radarCanError:
events.append(create_event('radarCanError', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
if not CS.canValid:
events.append(create_event('canError', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
# Only allow engagement with brake pressed when stopped behind another stopped car
if CS.brakePressed and plan.plan.vTargetFuture >= STARTING_TARGET_SPEED and not CP.radarOffCan and CS.vEgo < 0.3:
if CS.brakePressed and sm['plan'].vTargetFuture >= STARTING_TARGET_SPEED and not CP.radarOffCan and CS.vEgo < 0.3:
events.append(create_event('noTarget', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
if not read_only:
# update control state
state, soft_disable_timer, v_cruise_kph, v_cruise_kph_last = \
state_transition(CS, CP, state, events, soft_disable_timer, v_cruise_kph, AM)
state_transition(sm.frame, CS, CP, state, events, soft_disable_timer, v_cruise_kph, AM)
prof.checkpoint("State transition")
# Compute actuators (runs PID loops and lateral MPC)
actuators, v_cruise_kph, driver_status, angle_model_bias, v_acc, a_acc, lac_log = \
state_control(rcv_times, plan.plan, path_plan.pathPlan, CS, CP, state, events, v_cruise_kph,
v_cruise_kph_last, AM, rk, driver_status,
LaC, LoC, VM, angle_model_bias, read_only, is_metric, cal_perc)
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)
prof.checkpoint("State Control")
# Publish data
CC = data_send(plan, path_plan, CS, CI, CP, VM, state, events, actuators, v_cruise_kph, rk, carstate, carcontrol,
controlsstate, sendcan, AM, driver_status, LaC, LoC, angle_model_bias, read_only, start_time, v_acc, a_acc, lac_log)
CC, events_prev = data_send(sm, CS, CI, CP, VM, state, events, actuators, v_cruise_kph, rk, carstate, carcontrol, carevents, carparams,
controlsstate, sendcan, AM, driver_status, LaC, LoC, read_only, start_time, v_acc, a_acc, lac_log, events_prev)
prof.checkpoint("Sent")
rk.monitor_time()
selfdrive/controls/lib/alertmanager.py
+5 -4
View File
@@ -1,7 +1,7 @@
from cereal import log
from common.realtime import DT_CTRL
from selfdrive.swaglog import cloudlog
from selfdrive.controls.lib.alerts import ALERTS
from common.realtime import sec_since_boot
import copy
@@ -18,12 +18,12 @@ class AlertManager(object):
def alertPresent(self):
return len(self.activealerts) > 0
def add(self, alert_type, enabled=True, extra_text_1='', extra_text_2=''):
def add(self, frame, alert_type, enabled=True, extra_text_1='', extra_text_2=''):
alert_type = str(alert_type)
added_alert = copy.copy(self.alerts[alert_type])
added_alert.alert_text_1 += extra_text_1
added_alert.alert_text_2 += extra_text_2
added_alert.start_time = sec_since_boot()
added_alert.start_time = frame * DT_CTRL
# if new alert is higher priority, log it
if not self.alertPresent() or added_alert.alert_priority > self.activealerts[0].alert_priority:
@@ -34,7 +34,8 @@ class AlertManager(object):
# sort by priority first and then by start_time
self.activealerts.sort(key=lambda k: (k.alert_priority, k.start_time), reverse=True)
def process_alerts(self, cur_time):
def process_alerts(self, frame):
cur_time = frame * DT_CTRL
# first get rid of all the expired alerts
self.activealerts = [a for a in self.activealerts if a.start_time +
selfdrive/controls/lib/alerts.py
+21 -21
View File
@@ -355,28 +355,28 @@ ALERTS = [
AlertStatus.critical, AlertSize.full,
Priority.MID, VisualAlert.steerRequired, AudibleAlert.chimeWarning2, .1, 2., 2.),
# Cancellation alerts causing immediate disabling
Alert(
"radarCommIssue",
"commIssue",
"TAKE CONTROL IMMEDIATELY",
"Communication Issue between Processes",
AlertStatus.critical, AlertSize.full,
Priority.MID, VisualAlert.steerRequired, AudibleAlert.chimeWarning2, .1, 2., 2.),
Alert(
"radarCanError",
"TAKE CONTROL IMMEDIATELY",
"Radar Error: Restart the Car",
AlertStatus.critical, AlertSize.full,
Priority.HIGHEST, VisualAlert.steerRequired, AudibleAlert.chimeWarningRepeat, 1., 3., 4.),
Priority.MID, VisualAlert.steerRequired, AudibleAlert.chimeWarning2, .1, 2., 2.),
Alert(
"radarFault",
"TAKE CONTROL IMMEDIATELY",
"Radar Error: Restart the Car",
AlertStatus.critical, AlertSize.full,
Priority.HIGHEST, VisualAlert.steerRequired, AudibleAlert.chimeWarningRepeat, 1., 3., 4.),
Alert(
"modelCommIssue",
"TAKE CONTROL IMMEDIATELY",
"Model Error: Check Internet Connection",
AlertStatus.critical, AlertSize.full,
Priority.HIGHEST, VisualAlert.steerRequired, AudibleAlert.chimeWarningRepeat, 1., 3., 4.),
Priority.MID, VisualAlert.steerRequired, AudibleAlert.chimeWarning2, .1, 2., 2.),
# Cancellation alerts causing immediate disabling
Alert(
"controlsFailed",
"TAKE CONTROL IMMEDIATELY",
@@ -392,7 +392,7 @@ ALERTS = [
Priority.HIGHEST, VisualAlert.steerRequired, AudibleAlert.chimeWarningRepeat, 1., 3., 4.),
Alert(
"commIssue",
"canError",
"TAKE CONTROL IMMEDIATELY",
"CAN Error: Check Connections",
AlertStatus.critical, AlertSize.full,
@@ -520,7 +520,7 @@ ALERTS = [
Priority.MID, VisualAlert.none, AudibleAlert.chimeError, .4, 2., 3.),
Alert(
"radarCommIssueNoEntry",
"radarCanErrorNoEntry",
"openpilot Unavailable",
"Radar Error: Restart the Car",
AlertStatus.normal, AlertSize.mid,
@@ -533,13 +533,6 @@ ALERTS = [
AlertStatus.normal, AlertSize.mid,
Priority.LOW, VisualAlert.none, AudibleAlert.chimeError, .4, 2., 3.),
Alert(
"modelCommIssueNoEntry",
"openpilot Unavailable",
"Model Error: Check Internet Connection",
AlertStatus.normal, AlertSize.mid,
Priority.LOW, VisualAlert.none, AudibleAlert.chimeError, .4, 2., 3.),
Alert(
"controlsFailedNoEntry",
"openpilot Unavailable",
@@ -548,7 +541,7 @@ ALERTS = [
Priority.LOW, VisualAlert.none, AudibleAlert.chimeError, .4, 2., 3.),
Alert(
"commIssueNoEntry",
"canErrorNoEntry",
"openpilot Unavailable",
"CAN Error: Check Connections",
AlertStatus.normal, AlertSize.mid,
@@ -610,6 +603,13 @@ ALERTS = [
AlertStatus.normal, AlertSize.mid,
Priority.LOW, VisualAlert.none, AudibleAlert.chimeDisengage, .4, 2., 3.),
Alert(
"commIssueNoEntry",
"openpilot unavailable",
"Communication Issue between Processes",
AlertStatus.normal, AlertSize.mid,
Priority.LOW, VisualAlert.none, AudibleAlert.chimeDisengage, .4, 2., 3.),
# permanent alerts
Alert(
"steerUnavailablePermanent",
selfdrive/controls/lib/cluster/Makefile
+6 -2
View File
@@ -1,7 +1,11 @@
CC = clang
CXX = clang++
CPPFLAGS = -Wall -g -fPIC -std=c++11 -O2
CXXFLAGS = -Wall -g -fPIC -std=c++11 -O2
ifeq ($(ARCH),aarch64)
CXXFLAGS += -mcpu=cortex-a57
endif
OBJS = fastcluster.o test.o
DEPS := $(OBJS:.o=.d)
@@ -19,7 +23,7 @@ libfastcluster.so: fastcluster.o
$(CXX) -g -shared -o $@ $+
%.o: %.cpp
$(CXX) $(CPPFLAGS) -MMD -c $*.cpp
$(CXX) $(CXXFLAGS) -MMD -c $*.cpp
clean:
rm -f $(OBJS) $(DEPS) libfastcluster.so test
selfdrive/controls/lib/drive_helpers.py
-2
View File
@@ -2,8 +2,6 @@ from cereal import car
from common.numpy_fast import clip, interp
from selfdrive.config import Conversions as CV
DT = 0.01 # Controlsd runs at 100Hz
# kph
V_CRUISE_MAX = 144
V_CRUISE_MIN = 8
selfdrive/controls/lib/driver_monitor.py
+5 -7
View File
@@ -1,10 +1,8 @@
import numpy as np
from common.realtime import sec_since_boot
from common.realtime import sec_since_boot, DT_CTRL, DT_DMON
from selfdrive.controls.lib.drive_helpers import create_event, EventTypes as ET
from common.filter_simple import FirstOrderFilter
_DT = 0.01 # update runs at 100Hz
_DTM = 0.1 # DM runs at 10Hz
_AWARENESS_TIME = 180 # 3 minutes limit without user touching steering wheels make the car enter a terminal status
_AWARENESS_PRE_TIME = 20. # a first alert is issued 20s before expiration
_AWARENESS_PROMPT_TIME = 5. # a second alert is issued 5s before start decelerating the car
@@ -65,9 +63,9 @@ class DriverStatus():
self.monitor_valid = True # variance needs to be low
self.awareness = 1.
self.driver_distracted = False
self.driver_distraction_filter = FirstOrderFilter(0., _DISTRACTED_FILTER_TS, _DTM)
self.driver_distraction_filter = FirstOrderFilter(0., _DISTRACTED_FILTER_TS, DT_DMON)
self.variance_high = False
self.variance_filter = FirstOrderFilter(0., _VARIANCE_FILTER_TS, _DTM)
self.variance_filter = FirstOrderFilter(0., _VARIANCE_FILTER_TS, DT_DMON)
self.ts_last_check = 0.
self.face_detected = False
self._set_timers()
@@ -81,11 +79,11 @@ class DriverStatus():
if self.monitor_on:
self.threshold_pre = _DISTRACTED_PRE_TIME / _DISTRACTED_TIME
self.threshold_prompt = _DISTRACTED_PROMPT_TIME / _DISTRACTED_TIME
self.step_change = _DT / _DISTRACTED_TIME
self.step_change = DT_CTRL / _DISTRACTED_TIME
else:
self.threshold_pre = _AWARENESS_PRE_TIME / _AWARENESS_TIME
self.threshold_prompt = _AWARENESS_PROMPT_TIME / _AWARENESS_TIME
self.step_change = _DT / _AWARENESS_TIME
self.step_change = DT_CTRL / _AWARENESS_TIME
def _is_driver_distracted(self, pose):
# to be tuned and to learn the driver's normal pose
selfdrive/controls/lib/latcontrol_indi.py
+7 -6
View File
@@ -1,19 +1,20 @@
import math
import numpy as np
from cereal import log
from common.realtime import DT_CTRL
from common.numpy_fast import clip
from selfdrive.car.toyota.carcontroller import SteerLimitParams
from selfdrive.car import apply_toyota_steer_torque_limits
from selfdrive.controls.lib.drive_helpers import get_steer_max, DT
from common.numpy_fast import clip
from cereal import log
from selfdrive.controls.lib.drive_helpers import get_steer_max
class LatControlINDI(object):
def __init__(self, CP):
self.angle_steers_des = 0.
A = np.matrix([[1.0, DT, 0.0],
[0.0, 1.0, DT],
A = np.matrix([[1.0, DT_CTRL, 0.0],
[0.0, 1.0, DT_CTRL],
[0.0, 0.0, 1.0]])
C = np.matrix([[1.0, 0.0, 0.0],
[0.0, 1.0, 0.0]])
@@ -37,7 +38,7 @@ class LatControlINDI(object):
self.G = CP.lateralTuning.indi.actuatorEffectiveness
self.outer_loop_gain = CP.lateralTuning.indi.outerLoopGain
self.inner_loop_gain = CP.lateralTuning.indi.innerLoopGain
self.alpha = 1. - DT / (self.RC + DT)
self.alpha = 1. - DT_CTRL / (self.RC + DT_CTRL)
self.reset()
selfdrive/controls/lib/latcontrol_pid.py
-4
View File
@@ -24,10 +24,6 @@ class LatControlPID(object):
pid_log.active = False
self.pid.reset()
else:
# TODO: ideally we should interp, but for tuning reasons we keep the mpc solution
# constant for 0.05s.
#dt = min(cur_time - self.angle_steers_des_time, _DT_MPC + _DT) + _DT # no greater than dt mpc + dt, to prevent too high extraps
#self.angle_steers_des = self.angle_steers_des_prev + (dt / _DT_MPC) * (self.angle_steers_des_mpc - self.angle_steers_des_prev)
self.angle_steers_des = path_plan.angleSteers # get from MPC/PathPlanner
steers_max = get_steer_max(CP, v_ego)
selfdrive/controls/lib/lateral_mpc/Makefile
+5 -3
View File
@@ -15,11 +15,13 @@ QPOASES_FLAGS = -I$(PHONELIBS)/qpoases -I$(PHONELIBS)/qpoases/INCLUDE -I$(PHONEL
ACADO_FLAGS = -I$(PHONELIBS)/acado/include -I$(PHONELIBS)/acado/include/acado
ifeq ($(UNAME_S),Darwin)
ACADO_LIBS := -lacado_toolkit_s
ACADO_LIBS := -lacado_toolkit_s
else ifeq ($(UNAME_M),aarch64)
ACADO_LIBS := -L $(PHONELIBS)/acado/aarch64/lib -l:libacado_toolkit.a -l:libacado_casadi.a -l:libacado_csparse.a
ACADO_LIBS := -L $(PHONELIBS)/acado/aarch64/lib -l:libacado_toolkit.a -l:libacado_casadi.a -l:libacado_csparse.a
CFLAGS += -mcpu=cortex-a57
CXXFLAGS += -mcpu=cortex-a57
else
ACADO_LIBS := -L $(PHONELIBS)/acado/x64/lib -l:libacado_toolkit.a -l:libacado_casadi.a -l:libacado_csparse.a
ACADO_LIBS := -L $(PHONELIBS)/acado/x64/lib -l:libacado_toolkit.a -l:libacado_casadi.a -l:libacado_csparse.a
endif
OBJS = \
selfdrive/controls/lib/lateral_mpc/generator.cpp
+13 -11
View File
@@ -46,20 +46,22 @@ int main( )
auto angle_p = atan(3*p_poly_r0*xx*xx + 2*p_poly_r1*xx + p_poly_r2);
// given the lane width estimate, this is where we estimate the path given lane lines
auto l_phantom = poly_l - lane_width/2.0;
auto r_phantom = poly_r + lane_width/2.0;
auto path_from_left_lane = poly_l - lane_width/2.0;
auto path_from_right_lane = poly_r + lane_width/2.0;
// best path estimate path is a linear combination of poly_p and the path estimate
// given the lane lines
auto path = lr_prob * (l_prob * l_phantom + r_prob * r_phantom) / (l_prob + r_prob + 0.0001)
+ (1-lr_prob) * poly_p;
// if the lanes are visible, drive in the center, otherwise follow the path
auto path = lr_prob * (l_prob * path_from_left_lane + r_prob * path_from_right_lane) / (l_prob + r_prob + 0.0001)
+ (1-lr_prob) * poly_p;
auto angle = lr_prob * (l_prob * angle_l + r_prob * angle_r) / (l_prob + r_prob + 0.0001)
+ (1-lr_prob) * angle_p;
auto angle = lr_prob * (l_prob * angle_l + r_prob * angle_r) / (l_prob + r_prob + 0.0001)
+ (1-lr_prob) * angle_p;
// instead of using actual lane lines, use their estimated distance from path given lane_width
auto c_left_lane = exp(-(path + lane_width/2.0 - yy));
auto c_right_lane = exp(path - lane_width/2.0 - yy);
// When the lane is not visible, use an estimate of its position
auto weighted_left_lane = l_prob * poly_l + (1 - l_prob) * (path + lane_width/2.0);
auto weighted_right_lane = r_prob * poly_r + (1 - r_prob) * (path - lane_width/2.0);
auto c_left_lane = exp(-(weighted_left_lane - yy));
auto c_right_lane = exp(weighted_right_lane - yy);
// Running cost
Function h;
selfdrive/controls/lib/lateral_mpc/lateral_mpc.c
+14 -11
View File
@@ -26,7 +26,7 @@ typedef struct {
double y[N+1];
double psi[N+1];
double delta[N+1];
double rate[N+1];
double rate[N];
double cost;
} log_t;
@@ -51,16 +51,17 @@ void init(double pathCost, double laneCost, double headingCost, double steerRate
if (i > 4){
f = STEP_MULTIPLIER;
}
acadoVariables.W[25 * i + 0] = pathCost * f;
acadoVariables.W[25 * i + 6] = laneCost * f;
acadoVariables.W[25 * i + 12] = laneCost * f;
acadoVariables.W[25 * i + 18] = headingCost * f;
acadoVariables.W[25 * i + 24] = steerRateCost * f;
// Setup diagonal entries
acadoVariables.W[NY*NY*i + (NY+1)*0] = pathCost * f;
acadoVariables.W[NY*NY*i + (NY+1)*1] = laneCost * f;
acadoVariables.W[NY*NY*i + (NY+1)*2] = laneCost * f;
acadoVariables.W[NY*NY*i + (NY+1)*3] = headingCost * f;
acadoVariables.W[NY*NY*i + (NY+1)*4] = steerRateCost * f;
}
acadoVariables.WN[0] = pathCost * STEP_MULTIPLIER;
acadoVariables.WN[5] = laneCost * STEP_MULTIPLIER;
acadoVariables.WN[10] = laneCost * STEP_MULTIPLIER;
acadoVariables.WN[15] = headingCost * STEP_MULTIPLIER;
acadoVariables.WN[(NYN+1)*0] = pathCost * STEP_MULTIPLIER;
acadoVariables.WN[(NYN+1)*1] = laneCost * STEP_MULTIPLIER;
acadoVariables.WN[(NYN+1)*2] = laneCost * STEP_MULTIPLIER;
acadoVariables.WN[(NYN+1)*3] = headingCost * STEP_MULTIPLIER;
}
int run_mpc(state_t * x0, log_t * solution,
@@ -113,7 +114,9 @@ int run_mpc(state_t * x0, log_t * solution,
solution->y[i] = acadoVariables.x[i*NX+1];
solution->psi[i] = acadoVariables.x[i*NX+2];
solution->delta[i] = acadoVariables.x[i*NX+3];
solution->rate[i] = acadoVariables.u[i];
if (i < N){
solution->rate[i] = acadoVariables.u[i];
}
}
solution->cost = acado_getObjective();
selfdrive/controls/lib/lateral_mpc/lateral_mpc.d
-3
View File
@@ -1,3 +0,0 @@
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
selfdrive/controls/lib/lateral_mpc/lateral_mpc.o
BIN
View File
Binary file not shown.
selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_auxiliary_functions.d
-5
View File
@@ -1,5 +0,0 @@
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
selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_auxiliary_functions.o
BIN
View File
Binary file not shown.
selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_integrator.d
-3
View File
@@ -1,3 +0,0 @@
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
selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_integrator.o
BIN
View File
Binary file not shown.
selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_qpoases_interface.d
-24
View File
@@ -1,24 +0,0 @@
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
selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_qpoases_interface.o
BIN
View File
Binary file not shown.
selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_solver.c
+12 -12
View File
@@ -103,19 +103,19 @@ const real_t* od = in + 5;
real_t* a = acadoWorkspace.objAuxVar;
/* Compute intermediate quantities: */
a[0] = (exp(((real_t)(0.0000000000000000e+00)-(((((((od[14]+od[15])-(od[14]*od[15]))*((od[14]*(((((od[2]*((xd[0]*xd[0])*xd[0]))+(od[3]*(xd[0]*xd[0])))+(od[4]*xd[0]))+od[5])-(od[17]/(real_t)(2.0000000000000000e+00))))+(od[15]*(((((od[6]*((xd[0]*xd[0])*xd[0]))+(od[7]*(xd[0]*xd[0])))+(od[8]*xd[0]))+od[9])+(od[17]/(real_t)(2.0000000000000000e+00))))))/((od[14]+od[15])+(real_t)(1.0000000000000000e-04)))+(((real_t)(1.0000000000000000e+00)-((od[14]+od[15])-(od[14]*od[15])))*((((od[10]*((xd[0]*xd[0])*xd[0]))+(od[11]*(xd[0]*xd[0])))+(od[12]*xd[0]))+od[13])))+(od[17]/(real_t)(2.0000000000000000e+00)))-xd[1]))));
a[1] = (exp((((((((od[14]+od[15])-(od[14]*od[15]))*((od[14]*(((((od[2]*((xd[0]*xd[0])*xd[0]))+(od[3]*(xd[0]*xd[0])))+(od[4]*xd[0]))+od[5])-(od[17]/(real_t)(2.0000000000000000e+00))))+(od[15]*(((((od[6]*((xd[0]*xd[0])*xd[0]))+(od[7]*(xd[0]*xd[0])))+(od[8]*xd[0]))+od[9])+(od[17]/(real_t)(2.0000000000000000e+00))))))/((od[14]+od[15])+(real_t)(1.0000000000000000e-04)))+(((real_t)(1.0000000000000000e+00)-((od[14]+od[15])-(od[14]*od[15])))*((((od[10]*((xd[0]*xd[0])*xd[0]))+(od[11]*(xd[0]*xd[0])))+(od[12]*xd[0]))+od[13])))-(od[17]/(real_t)(2.0000000000000000e+00)))-xd[1])));
a[0] = (exp(((real_t)(0.0000000000000000e+00)-(((od[14]*((((od[2]*((xd[0]*xd[0])*xd[0]))+(od[3]*(xd[0]*xd[0])))+(od[4]*xd[0]))+od[5]))+(((real_t)(1.0000000000000000e+00)-od[14])*((((((od[14]+od[15])-(od[14]*od[15]))*((od[14]*(((((od[2]*((xd[0]*xd[0])*xd[0]))+(od[3]*(xd[0]*xd[0])))+(od[4]*xd[0]))+od[5])-(od[17]/(real_t)(2.0000000000000000e+00))))+(od[15]*(((((od[6]*((xd[0]*xd[0])*xd[0]))+(od[7]*(xd[0]*xd[0])))+(od[8]*xd[0]))+od[9])+(od[17]/(real_t)(2.0000000000000000e+00))))))/((od[14]+od[15])+(real_t)(1.0000000000000000e-04)))+(((real_t)(1.0000000000000000e+00)-((od[14]+od[15])-(od[14]*od[15])))*((((od[10]*((xd[0]*xd[0])*xd[0]))+(od[11]*(xd[0]*xd[0])))+(od[12]*xd[0]))+od[13])))+(od[17]/(real_t)(2.0000000000000000e+00)))))-xd[1]))));
a[1] = (exp((((od[15]*((((od[6]*((xd[0]*xd[0])*xd[0]))+(od[7]*(xd[0]*xd[0])))+(od[8]*xd[0]))+od[9]))+(((real_t)(1.0000000000000000e+00)-od[15])*((((((od[14]+od[15])-(od[14]*od[15]))*((od[14]*(((((od[2]*((xd[0]*xd[0])*xd[0]))+(od[3]*(xd[0]*xd[0])))+(od[4]*xd[0]))+od[5])-(od[17]/(real_t)(2.0000000000000000e+00))))+(od[15]*(((((od[6]*((xd[0]*xd[0])*xd[0]))+(od[7]*(xd[0]*xd[0])))+(od[8]*xd[0]))+od[9])+(od[17]/(real_t)(2.0000000000000000e+00))))))/((od[14]+od[15])+(real_t)(1.0000000000000000e-04)))+(((real_t)(1.0000000000000000e+00)-((od[14]+od[15])-(od[14]*od[15])))*((((od[10]*((xd[0]*xd[0])*xd[0]))+(od[11]*(xd[0]*xd[0])))+(od[12]*xd[0]))+od[13])))-(od[17]/(real_t)(2.0000000000000000e+00)))))-xd[1])));
a[2] = (atan(((((((real_t)(3.0000000000000000e+00)*od[2])*xd[0])*xd[0])+(((real_t)(2.0000000000000000e+00)*od[3])*xd[0]))+od[4])));
a[3] = (atan(((((((real_t)(3.0000000000000000e+00)*od[6])*xd[0])*xd[0])+(((real_t)(2.0000000000000000e+00)*od[7])*xd[0]))+od[8])));
a[4] = (atan(((((((real_t)(3.0000000000000000e+00)*od[10])*xd[0])*xd[0])+(((real_t)(2.0000000000000000e+00)*od[11])*xd[0]))+od[12])));
a[5] = ((real_t)(1.0000000000000000e+00)/((od[14]+od[15])+(real_t)(1.0000000000000000e-04)));
a[6] = ((real_t)(1.0000000000000000e+00)/((od[14]+od[15])+(real_t)(1.0000000000000000e-04)));
a[7] = (exp(((real_t)(0.0000000000000000e+00)-(((((((od[14]+od[15])-(od[14]*od[15]))*((od[14]*(((((od[2]*((xd[0]*xd[0])*xd[0]))+(od[3]*(xd[0]*xd[0])))+(od[4]*xd[0]))+od[5])-(od[17]/(real_t)(2.0000000000000000e+00))))+(od[15]*(((((od[6]*((xd[0]*xd[0])*xd[0]))+(od[7]*(xd[0]*xd[0])))+(od[8]*xd[0]))+od[9])+(od[17]/(real_t)(2.0000000000000000e+00))))))/((od[14]+od[15])+(real_t)(1.0000000000000000e-04)))+(((real_t)(1.0000000000000000e+00)-((od[14]+od[15])-(od[14]*od[15])))*((((od[10]*((xd[0]*xd[0])*xd[0]))+(od[11]*(xd[0]*xd[0])))+(od[12]*xd[0]))+od[13])))+(od[17]/(real_t)(2.0000000000000000e+00)))-xd[1]))));
a[8] = (((real_t)(0.0000000000000000e+00)-(((((od[14]+od[15])-(od[14]*od[15]))*((od[14]*(((od[2]*(((xd[0]+xd[0])*xd[0])+(xd[0]*xd[0])))+(od[3]*(xd[0]+xd[0])))+od[4]))+(od[15]*(((od[6]*(((xd[0]+xd[0])*xd[0])+(xd[0]*xd[0])))+(od[7]*(xd[0]+xd[0])))+od[8]))))*a[6])+(((real_t)(1.0000000000000000e+00)-((od[14]+od[15])-(od[14]*od[15])))*(((od[10]*(((xd[0]+xd[0])*xd[0])+(xd[0]*xd[0])))+(od[11]*(xd[0]+xd[0])))+od[12]))))*a[7]);
a[7] = (exp(((real_t)(0.0000000000000000e+00)-(((od[14]*((((od[2]*((xd[0]*xd[0])*xd[0]))+(od[3]*(xd[0]*xd[0])))+(od[4]*xd[0]))+od[5]))+(((real_t)(1.0000000000000000e+00)-od[14])*((((((od[14]+od[15])-(od[14]*od[15]))*((od[14]*(((((od[2]*((xd[0]*xd[0])*xd[0]))+(od[3]*(xd[0]*xd[0])))+(od[4]*xd[0]))+od[5])-(od[17]/(real_t)(2.0000000000000000e+00))))+(od[15]*(((((od[6]*((xd[0]*xd[0])*xd[0]))+(od[7]*(xd[0]*xd[0])))+(od[8]*xd[0]))+od[9])+(od[17]/(real_t)(2.0000000000000000e+00))))))/((od[14]+od[15])+(real_t)(1.0000000000000000e-04)))+(((real_t)(1.0000000000000000e+00)-((od[14]+od[15])-(od[14]*od[15])))*((((od[10]*((xd[0]*xd[0])*xd[0]))+(od[11]*(xd[0]*xd[0])))+(od[12]*xd[0]))+od[13])))+(od[17]/(real_t)(2.0000000000000000e+00)))))-xd[1]))));
a[8] = (((real_t)(0.0000000000000000e+00)-((od[14]*(((od[2]*(((xd[0]+xd[0])*xd[0])+(xd[0]*xd[0])))+(od[3]*(xd[0]+xd[0])))+od[4]))+(((real_t)(1.0000000000000000e+00)-od[14])*(((((od[14]+od[15])-(od[14]*od[15]))*((od[14]*(((od[2]*(((xd[0]+xd[0])*xd[0])+(xd[0]*xd[0])))+(od[3]*(xd[0]+xd[0])))+od[4]))+(od[15]*(((od[6]*(((xd[0]+xd[0])*xd[0])+(xd[0]*xd[0])))+(od[7]*(xd[0]+xd[0])))+od[8]))))*a[6])+(((real_t)(1.0000000000000000e+00)-((od[14]+od[15])-(od[14]*od[15])))*(((od[10]*(((xd[0]+xd[0])*xd[0])+(xd[0]*xd[0])))+(od[11]*(xd[0]+xd[0])))+od[12]))))))*a[7]);
a[9] = (((real_t)(0.0000000000000000e+00)-((real_t)(0.0000000000000000e+00)-(real_t)(1.0000000000000000e+00)))*a[7]);
a[10] = ((real_t)(1.0000000000000000e+00)/((od[14]+od[15])+(real_t)(1.0000000000000000e-04)));
a[11] = (exp((((((((od[14]+od[15])-(od[14]*od[15]))*((od[14]*(((((od[2]*((xd[0]*xd[0])*xd[0]))+(od[3]*(xd[0]*xd[0])))+(od[4]*xd[0]))+od[5])-(od[17]/(real_t)(2.0000000000000000e+00))))+(od[15]*(((((od[6]*((xd[0]*xd[0])*xd[0]))+(od[7]*(xd[0]*xd[0])))+(od[8]*xd[0]))+od[9])+(od[17]/(real_t)(2.0000000000000000e+00))))))/((od[14]+od[15])+(real_t)(1.0000000000000000e-04)))+(((real_t)(1.0000000000000000e+00)-((od[14]+od[15])-(od[14]*od[15])))*((((od[10]*((xd[0]*xd[0])*xd[0]))+(od[11]*(xd[0]*xd[0])))+(od[12]*xd[0]))+od[13])))-(od[17]/(real_t)(2.0000000000000000e+00)))-xd[1])));
a[12] = ((((((od[14]+od[15])-(od[14]*od[15]))*((od[14]*(((od[2]*(((xd[0]+xd[0])*xd[0])+(xd[0]*xd[0])))+(od[3]*(xd[0]+xd[0])))+od[4]))+(od[15]*(((od[6]*(((xd[0]+xd[0])*xd[0])+(xd[0]*xd[0])))+(od[7]*(xd[0]+xd[0])))+od[8]))))*a[10])+(((real_t)(1.0000000000000000e+00)-((od[14]+od[15])-(od[14]*od[15])))*(((od[10]*(((xd[0]+xd[0])*xd[0])+(xd[0]*xd[0])))+(od[11]*(xd[0]+xd[0])))+od[12])))*a[11]);
a[11] = (exp((((od[15]*((((od[6]*((xd[0]*xd[0])*xd[0]))+(od[7]*(xd[0]*xd[0])))+(od[8]*xd[0]))+od[9]))+(((real_t)(1.0000000000000000e+00)-od[15])*((((((od[14]+od[15])-(od[14]*od[15]))*((od[14]*(((((od[2]*((xd[0]*xd[0])*xd[0]))+(od[3]*(xd[0]*xd[0])))+(od[4]*xd[0]))+od[5])-(od[17]/(real_t)(2.0000000000000000e+00))))+(od[15]*(((((od[6]*((xd[0]*xd[0])*xd[0]))+(od[7]*(xd[0]*xd[0])))+(od[8]*xd[0]))+od[9])+(od[17]/(real_t)(2.0000000000000000e+00))))))/((od[14]+od[15])+(real_t)(1.0000000000000000e-04)))+(((real_t)(1.0000000000000000e+00)-((od[14]+od[15])-(od[14]*od[15])))*((((od[10]*((xd[0]*xd[0])*xd[0]))+(od[11]*(xd[0]*xd[0])))+(od[12]*xd[0]))+od[13])))-(od[17]/(real_t)(2.0000000000000000e+00)))))-xd[1])));
a[12] = (((od[15]*(((od[6]*(((xd[0]+xd[0])*xd[0])+(xd[0]*xd[0])))+(od[7]*(xd[0]+xd[0])))+od[8]))+(((real_t)(1.0000000000000000e+00)-od[15])*(((((od[14]+od[15])-(od[14]*od[15]))*((od[14]*(((od[2]*(((xd[0]+xd[0])*xd[0])+(xd[0]*xd[0])))+(od[3]*(xd[0]+xd[0])))+od[4]))+(od[15]*(((od[6]*(((xd[0]+xd[0])*xd[0])+(xd[0]*xd[0])))+(od[7]*(xd[0]+xd[0])))+od[8]))))*a[10])+(((real_t)(1.0000000000000000e+00)-((od[14]+od[15])-(od[14]*od[15])))*(((od[10]*(((xd[0]+xd[0])*xd[0])+(xd[0]*xd[0])))+(od[11]*(xd[0]+xd[0])))+od[12])))))*a[11]);
a[13] = (((real_t)(0.0000000000000000e+00)-(real_t)(1.0000000000000000e+00))*a[11]);
a[14] = ((real_t)(1.0000000000000000e+00)/((real_t)(1.0000000000000000e+00)+(pow(((((((real_t)(3.0000000000000000e+00)*od[2])*xd[0])*xd[0])+(((real_t)(2.0000000000000000e+00)*od[3])*xd[0]))+od[4]),2))));
a[15] = ((((((real_t)(3.0000000000000000e+00)*od[2])*xd[0])+(((real_t)(3.0000000000000000e+00)*od[2])*xd[0]))+((real_t)(2.0000000000000000e+00)*od[3]))*a[14]);
@@ -166,19 +166,19 @@ const real_t* od = in + 4;
real_t* a = acadoWorkspace.objAuxVar;
/* Compute intermediate quantities: */
a[0] = (exp(((real_t)(0.0000000000000000e+00)-(((((((od[14]+od[15])-(od[14]*od[15]))*((od[14]*(((((od[2]*((xd[0]*xd[0])*xd[0]))+(od[3]*(xd[0]*xd[0])))+(od[4]*xd[0]))+od[5])-(od[17]/(real_t)(2.0000000000000000e+00))))+(od[15]*(((((od[6]*((xd[0]*xd[0])*xd[0]))+(od[7]*(xd[0]*xd[0])))+(od[8]*xd[0]))+od[9])+(od[17]/(real_t)(2.0000000000000000e+00))))))/((od[14]+od[15])+(real_t)(1.0000000000000000e-04)))+(((real_t)(1.0000000000000000e+00)-((od[14]+od[15])-(od[14]*od[15])))*((((od[10]*((xd[0]*xd[0])*xd[0]))+(od[11]*(xd[0]*xd[0])))+(od[12]*xd[0]))+od[13])))+(od[17]/(real_t)(2.0000000000000000e+00)))-xd[1]))));
a[1] = (exp((((((((od[14]+od[15])-(od[14]*od[15]))*((od[14]*(((((od[2]*((xd[0]*xd[0])*xd[0]))+(od[3]*(xd[0]*xd[0])))+(od[4]*xd[0]))+od[5])-(od[17]/(real_t)(2.0000000000000000e+00))))+(od[15]*(((((od[6]*((xd[0]*xd[0])*xd[0]))+(od[7]*(xd[0]*xd[0])))+(od[8]*xd[0]))+od[9])+(od[17]/(real_t)(2.0000000000000000e+00))))))/((od[14]+od[15])+(real_t)(1.0000000000000000e-04)))+(((real_t)(1.0000000000000000e+00)-((od[14]+od[15])-(od[14]*od[15])))*((((od[10]*((xd[0]*xd[0])*xd[0]))+(od[11]*(xd[0]*xd[0])))+(od[12]*xd[0]))+od[13])))-(od[17]/(real_t)(2.0000000000000000e+00)))-xd[1])));
a[0] = (exp(((real_t)(0.0000000000000000e+00)-(((od[14]*((((od[2]*((xd[0]*xd[0])*xd[0]))+(od[3]*(xd[0]*xd[0])))+(od[4]*xd[0]))+od[5]))+(((real_t)(1.0000000000000000e+00)-od[14])*((((((od[14]+od[15])-(od[14]*od[15]))*((od[14]*(((((od[2]*((xd[0]*xd[0])*xd[0]))+(od[3]*(xd[0]*xd[0])))+(od[4]*xd[0]))+od[5])-(od[17]/(real_t)(2.0000000000000000e+00))))+(od[15]*(((((od[6]*((xd[0]*xd[0])*xd[0]))+(od[7]*(xd[0]*xd[0])))+(od[8]*xd[0]))+od[9])+(od[17]/(real_t)(2.0000000000000000e+00))))))/((od[14]+od[15])+(real_t)(1.0000000000000000e-04)))+(((real_t)(1.0000000000000000e+00)-((od[14]+od[15])-(od[14]*od[15])))*((((od[10]*((xd[0]*xd[0])*xd[0]))+(od[11]*(xd[0]*xd[0])))+(od[12]*xd[0]))+od[13])))+(od[17]/(real_t)(2.0000000000000000e+00)))))-xd[1]))));
a[1] = (exp((((od[15]*((((od[6]*((xd[0]*xd[0])*xd[0]))+(od[7]*(xd[0]*xd[0])))+(od[8]*xd[0]))+od[9]))+(((real_t)(1.0000000000000000e+00)-od[15])*((((((od[14]+od[15])-(od[14]*od[15]))*((od[14]*(((((od[2]*((xd[0]*xd[0])*xd[0]))+(od[3]*(xd[0]*xd[0])))+(od[4]*xd[0]))+od[5])-(od[17]/(real_t)(2.0000000000000000e+00))))+(od[15]*(((((od[6]*((xd[0]*xd[0])*xd[0]))+(od[7]*(xd[0]*xd[0])))+(od[8]*xd[0]))+od[9])+(od[17]/(real_t)(2.0000000000000000e+00))))))/((od[14]+od[15])+(real_t)(1.0000000000000000e-04)))+(((real_t)(1.0000000000000000e+00)-((od[14]+od[15])-(od[14]*od[15])))*((((od[10]*((xd[0]*xd[0])*xd[0]))+(od[11]*(xd[0]*xd[0])))+(od[12]*xd[0]))+od[13])))-(od[17]/(real_t)(2.0000000000000000e+00)))))-xd[1])));
a[2] = (atan(((((((real_t)(3.0000000000000000e+00)*od[2])*xd[0])*xd[0])+(((real_t)(2.0000000000000000e+00)*od[3])*xd[0]))+od[4])));
a[3] = (atan(((((((real_t)(3.0000000000000000e+00)*od[6])*xd[0])*xd[0])+(((real_t)(2.0000000000000000e+00)*od[7])*xd[0]))+od[8])));
a[4] = (atan(((((((real_t)(3.0000000000000000e+00)*od[10])*xd[0])*xd[0])+(((real_t)(2.0000000000000000e+00)*od[11])*xd[0]))+od[12])));
a[5] = ((real_t)(1.0000000000000000e+00)/((od[14]+od[15])+(real_t)(1.0000000000000000e-04)));
a[6] = ((real_t)(1.0000000000000000e+00)/((od[14]+od[15])+(real_t)(1.0000000000000000e-04)));
a[7] = (exp(((real_t)(0.0000000000000000e+00)-(((((((od[14]+od[15])-(od[14]*od[15]))*((od[14]*(((((od[2]*((xd[0]*xd[0])*xd[0]))+(od[3]*(xd[0]*xd[0])))+(od[4]*xd[0]))+od[5])-(od[17]/(real_t)(2.0000000000000000e+00))))+(od[15]*(((((od[6]*((xd[0]*xd[0])*xd[0]))+(od[7]*(xd[0]*xd[0])))+(od[8]*xd[0]))+od[9])+(od[17]/(real_t)(2.0000000000000000e+00))))))/((od[14]+od[15])+(real_t)(1.0000000000000000e-04)))+(((real_t)(1.0000000000000000e+00)-((od[14]+od[15])-(od[14]*od[15])))*((((od[10]*((xd[0]*xd[0])*xd[0]))+(od[11]*(xd[0]*xd[0])))+(od[12]*xd[0]))+od[13])))+(od[17]/(real_t)(2.0000000000000000e+00)))-xd[1]))));
a[8] = (((real_t)(0.0000000000000000e+00)-(((((od[14]+od[15])-(od[14]*od[15]))*((od[14]*(((od[2]*(((xd[0]+xd[0])*xd[0])+(xd[0]*xd[0])))+(od[3]*(xd[0]+xd[0])))+od[4]))+(od[15]*(((od[6]*(((xd[0]+xd[0])*xd[0])+(xd[0]*xd[0])))+(od[7]*(xd[0]+xd[0])))+od[8]))))*a[6])+(((real_t)(1.0000000000000000e+00)-((od[14]+od[15])-(od[14]*od[15])))*(((od[10]*(((xd[0]+xd[0])*xd[0])+(xd[0]*xd[0])))+(od[11]*(xd[0]+xd[0])))+od[12]))))*a[7]);
a[7] = (exp(((real_t)(0.0000000000000000e+00)-(((od[14]*((((od[2]*((xd[0]*xd[0])*xd[0]))+(od[3]*(xd[0]*xd[0])))+(od[4]*xd[0]))+od[5]))+(((real_t)(1.0000000000000000e+00)-od[14])*((((((od[14]+od[15])-(od[14]*od[15]))*((od[14]*(((((od[2]*((xd[0]*xd[0])*xd[0]))+(od[3]*(xd[0]*xd[0])))+(od[4]*xd[0]))+od[5])-(od[17]/(real_t)(2.0000000000000000e+00))))+(od[15]*(((((od[6]*((xd[0]*xd[0])*xd[0]))+(od[7]*(xd[0]*xd[0])))+(od[8]*xd[0]))+od[9])+(od[17]/(real_t)(2.0000000000000000e+00))))))/((od[14]+od[15])+(real_t)(1.0000000000000000e-04)))+(((real_t)(1.0000000000000000e+00)-((od[14]+od[15])-(od[14]*od[15])))*((((od[10]*((xd[0]*xd[0])*xd[0]))+(od[11]*(xd[0]*xd[0])))+(od[12]*xd[0]))+od[13])))+(od[17]/(real_t)(2.0000000000000000e+00)))))-xd[1]))));
a[8] = (((real_t)(0.0000000000000000e+00)-((od[14]*(((od[2]*(((xd[0]+xd[0])*xd[0])+(xd[0]*xd[0])))+(od[3]*(xd[0]+xd[0])))+od[4]))+(((real_t)(1.0000000000000000e+00)-od[14])*(((((od[14]+od[15])-(od[14]*od[15]))*((od[14]*(((od[2]*(((xd[0]+xd[0])*xd[0])+(xd[0]*xd[0])))+(od[3]*(xd[0]+xd[0])))+od[4]))+(od[15]*(((od[6]*(((xd[0]+xd[0])*xd[0])+(xd[0]*xd[0])))+(od[7]*(xd[0]+xd[0])))+od[8]))))*a[6])+(((real_t)(1.0000000000000000e+00)-((od[14]+od[15])-(od[14]*od[15])))*(((od[10]*(((xd[0]+xd[0])*xd[0])+(xd[0]*xd[0])))+(od[11]*(xd[0]+xd[0])))+od[12]))))))*a[7]);
a[9] = (((real_t)(0.0000000000000000e+00)-((real_t)(0.0000000000000000e+00)-(real_t)(1.0000000000000000e+00)))*a[7]);
a[10] = ((real_t)(1.0000000000000000e+00)/((od[14]+od[15])+(real_t)(1.0000000000000000e-04)));
a[11] = (exp((((((((od[14]+od[15])-(od[14]*od[15]))*((od[14]*(((((od[2]*((xd[0]*xd[0])*xd[0]))+(od[3]*(xd[0]*xd[0])))+(od[4]*xd[0]))+od[5])-(od[17]/(real_t)(2.0000000000000000e+00))))+(od[15]*(((((od[6]*((xd[0]*xd[0])*xd[0]))+(od[7]*(xd[0]*xd[0])))+(od[8]*xd[0]))+od[9])+(od[17]/(real_t)(2.0000000000000000e+00))))))/((od[14]+od[15])+(real_t)(1.0000000000000000e-04)))+(((real_t)(1.0000000000000000e+00)-((od[14]+od[15])-(od[14]*od[15])))*((((od[10]*((xd[0]*xd[0])*xd[0]))+(od[11]*(xd[0]*xd[0])))+(od[12]*xd[0]))+od[13])))-(od[17]/(real_t)(2.0000000000000000e+00)))-xd[1])));
a[12] = ((((((od[14]+od[15])-(od[14]*od[15]))*((od[14]*(((od[2]*(((xd[0]+xd[0])*xd[0])+(xd[0]*xd[0])))+(od[3]*(xd[0]+xd[0])))+od[4]))+(od[15]*(((od[6]*(((xd[0]+xd[0])*xd[0])+(xd[0]*xd[0])))+(od[7]*(xd[0]+xd[0])))+od[8]))))*a[10])+(((real_t)(1.0000000000000000e+00)-((od[14]+od[15])-(od[14]*od[15])))*(((od[10]*(((xd[0]+xd[0])*xd[0])+(xd[0]*xd[0])))+(od[11]*(xd[0]+xd[0])))+od[12])))*a[11]);
a[11] = (exp((((od[15]*((((od[6]*((xd[0]*xd[0])*xd[0]))+(od[7]*(xd[0]*xd[0])))+(od[8]*xd[0]))+od[9]))+(((real_t)(1.0000000000000000e+00)-od[15])*((((((od[14]+od[15])-(od[14]*od[15]))*((od[14]*(((((od[2]*((xd[0]*xd[0])*xd[0]))+(od[3]*(xd[0]*xd[0])))+(od[4]*xd[0]))+od[5])-(od[17]/(real_t)(2.0000000000000000e+00))))+(od[15]*(((((od[6]*((xd[0]*xd[0])*xd[0]))+(od[7]*(xd[0]*xd[0])))+(od[8]*xd[0]))+od[9])+(od[17]/(real_t)(2.0000000000000000e+00))))))/((od[14]+od[15])+(real_t)(1.0000000000000000e-04)))+(((real_t)(1.0000000000000000e+00)-((od[14]+od[15])-(od[14]*od[15])))*((((od[10]*((xd[0]*xd[0])*xd[0]))+(od[11]*(xd[0]*xd[0])))+(od[12]*xd[0]))+od[13])))-(od[17]/(real_t)(2.0000000000000000e+00)))))-xd[1])));
a[12] = (((od[15]*(((od[6]*(((xd[0]+xd[0])*xd[0])+(xd[0]*xd[0])))+(od[7]*(xd[0]+xd[0])))+od[8]))+(((real_t)(1.0000000000000000e+00)-od[15])*(((((od[14]+od[15])-(od[14]*od[15]))*((od[14]*(((od[2]*(((xd[0]+xd[0])*xd[0])+(xd[0]*xd[0])))+(od[3]*(xd[0]+xd[0])))+od[4]))+(od[15]*(((od[6]*(((xd[0]+xd[0])*xd[0])+(xd[0]*xd[0])))+(od[7]*(xd[0]+xd[0])))+od[8]))))*a[10])+(((real_t)(1.0000000000000000e+00)-((od[14]+od[15])-(od[14]*od[15])))*(((od[10]*(((xd[0]+xd[0])*xd[0])+(xd[0]*xd[0])))+(od[11]*(xd[0]+xd[0])))+od[12])))))*a[11]);
a[13] = (((real_t)(0.0000000000000000e+00)-(real_t)(1.0000000000000000e+00))*a[11]);
a[14] = ((real_t)(1.0000000000000000e+00)/((real_t)(1.0000000000000000e+00)+(pow(((((((real_t)(3.0000000000000000e+00)*od[2])*xd[0])*xd[0])+(((real_t)(2.0000000000000000e+00)*od[3])*xd[0]))+od[4]),2))));
a[15] = ((((((real_t)(3.0000000000000000e+00)*od[2])*xd[0])+(((real_t)(3.0000000000000000e+00)*od[2])*xd[0]))+((real_t)(2.0000000000000000e+00)*od[3]))*a[14]);
selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_solver.d
-3
View File
@@ -1,3 +0,0 @@
lib_mpc_export/acado_solver.o: lib_mpc_export/acado_solver.c \
lib_mpc_export/acado_common.h \
lib_mpc_export/acado_qpoases_interface.hpp
selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_solver.o
BIN
View File
Binary file not shown.
selfdrive/controls/lib/lateral_mpc/lib_qp/Bounds.d
-14
View File
@@ -1,14 +0,0 @@
lib_qp/Bounds.o: ../../../../phonelibs/qpoases/SRC/Bounds.cpp \
../../../../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 \
lib_mpc_export/acado_qpoases_interface.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
selfdrive/controls/lib/lateral_mpc/lib_qp/Bounds.o
BIN
View File
Binary file not shown.
selfdrive/controls/lib/lateral_mpc/lib_qp/Constraints.d
-14
View File
@@ -1,14 +0,0 @@
lib_qp/Constraints.o: ../../../../phonelibs/qpoases/SRC/Constraints.cpp \
../../../../phonelibs/qpoases/INCLUDE/Constraints.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 \
lib_mpc_export/acado_qpoases_interface.hpp \
../../../../phonelibs/qpoases/SRC/MessageHandling.ipp \
../../../../phonelibs/qpoases/SRC/Utils.ipp \
../../../../phonelibs/qpoases/SRC/Indexlist.ipp \
../../../../phonelibs/qpoases/SRC/SubjectTo.ipp \
../../../../phonelibs/qpoases/SRC/Constraints.ipp
selfdrive/controls/lib/lateral_mpc/lib_qp/Constraints.o
BIN
View File
Binary file not shown.
selfdrive/controls/lib/lateral_mpc/lib_qp/CyclingManager.d
-11
View File
@@ -1,11 +0,0 @@
lib_qp/CyclingManager.o: \
../../../../phonelibs/qpoases/SRC/CyclingManager.cpp \
../../../../phonelibs/qpoases/INCLUDE/CyclingManager.hpp \
../../../../phonelibs/qpoases/INCLUDE/Utils.hpp \
../../../../phonelibs/qpoases/INCLUDE/MessageHandling.hpp \
../../../../phonelibs/qpoases/INCLUDE/Types.hpp \
../../../../phonelibs/qpoases/INCLUDE/Constants.hpp \
lib_mpc_export/acado_qpoases_interface.hpp \
../../../../phonelibs/qpoases/SRC/MessageHandling.ipp \
../../../../phonelibs/qpoases/SRC/Utils.ipp \
../../../../phonelibs/qpoases/SRC/CyclingManager.ipp
selfdrive/controls/lib/lateral_mpc/lib_qp/CyclingManager.o
BIN
View File
Binary file not shown.
selfdrive/controls/lib/lateral_mpc/lib_qp/EXTRAS/SolutionAnalysis.d
-24
View File
@@ -1,24 +0,0 @@
lib_qp/EXTRAS/SolutionAnalysis.o: \
../../../../phonelibs/qpoases/SRC/EXTRAS/SolutionAnalysis.cpp \
../../../../phonelibs/qpoases/INCLUDE/EXTRAS/SolutionAnalysis.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 \
lib_mpc_export/acado_qpoases_interface.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
selfdrive/controls/lib/lateral_mpc/lib_qp/EXTRAS/SolutionAnalysis.o
BIN
View File
Binary file not shown.
selfdrive/controls/lib/lateral_mpc/lib_qp/Indexlist.d
-10
View File
@@ -1,10 +0,0 @@
lib_qp/Indexlist.o: ../../../../phonelibs/qpoases/SRC/Indexlist.cpp \
../../../../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 \
lib_mpc_export/acado_qpoases_interface.hpp \
../../../../phonelibs/qpoases/SRC/MessageHandling.ipp \
../../../../phonelibs/qpoases/SRC/Utils.ipp \
../../../../phonelibs/qpoases/SRC/Indexlist.ipp
selfdrive/controls/lib/lateral_mpc/lib_qp/Indexlist.o
BIN
View File
Binary file not shown.
selfdrive/controls/lib/lateral_mpc/lib_qp/MessageHandling.d
-9
View File
@@ -1,9 +0,0 @@
lib_qp/MessageHandling.o: \
../../../../phonelibs/qpoases/SRC/MessageHandling.cpp \
../../../../phonelibs/qpoases/INCLUDE/MessageHandling.hpp \
../../../../phonelibs/qpoases/INCLUDE/Types.hpp \
../../../../phonelibs/qpoases/INCLUDE/Constants.hpp \
lib_mpc_export/acado_qpoases_interface.hpp \
../../../../phonelibs/qpoases/SRC/MessageHandling.ipp \
../../../../phonelibs/qpoases/INCLUDE/Utils.hpp \
../../../../phonelibs/qpoases/SRC/Utils.ipp
selfdrive/controls/lib/lateral_mpc/lib_qp/MessageHandling.o
BIN
View File
Binary file not shown.
selfdrive/controls/lib/lateral_mpc/lib_qp/QProblem.d
-22
View File
@@ -1,22 +0,0 @@
lib_qp/QProblem.o: ../../../../phonelibs/qpoases/SRC/QProblem.cpp \
../../../../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 \
lib_mpc_export/acado_qpoases_interface.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
selfdrive/controls/lib/lateral_mpc/lib_qp/QProblem.o
BIN
View File
Binary file not shown.
selfdrive/controls/lib/lateral_mpc/lib_qp/QProblemB.d
-16
View File
@@ -1,16 +0,0 @@
lib_qp/QProblemB.o: ../../../../phonelibs/qpoases/SRC/QProblemB.cpp \
../../../../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 \
lib_mpc_export/acado_qpoases_interface.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
selfdrive/controls/lib/lateral_mpc/lib_qp/QProblemB.o
BIN
View File
Binary file not shown.
selfdrive/controls/lib/lateral_mpc/lib_qp/SubjectTo.d
-12
View File
@@ -1,12 +0,0 @@
lib_qp/SubjectTo.o: ../../../../phonelibs/qpoases/SRC/SubjectTo.cpp \
../../../../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 \
lib_mpc_export/acado_qpoases_interface.hpp \
../../../../phonelibs/qpoases/SRC/MessageHandling.ipp \
../../../../phonelibs/qpoases/SRC/Utils.ipp \
../../../../phonelibs/qpoases/SRC/Indexlist.ipp \
../../../../phonelibs/qpoases/SRC/SubjectTo.ipp
selfdrive/controls/lib/lateral_mpc/lib_qp/SubjectTo.o
BIN
View File
Binary file not shown.
selfdrive/controls/lib/lateral_mpc/lib_qp/Utils.d
-8
View File
@@ -1,8 +0,0 @@
lib_qp/Utils.o: ../../../../phonelibs/qpoases/SRC/Utils.cpp \
../../../../phonelibs/qpoases/INCLUDE/Utils.hpp \
../../../../phonelibs/qpoases/INCLUDE/MessageHandling.hpp \
../../../../phonelibs/qpoases/INCLUDE/Types.hpp \
../../../../phonelibs/qpoases/INCLUDE/Constants.hpp \
lib_mpc_export/acado_qpoases_interface.hpp \
../../../../phonelibs/qpoases/SRC/MessageHandling.ipp \
../../../../phonelibs/qpoases/SRC/Utils.ipp
selfdrive/controls/lib/lateral_mpc/lib_qp/Utils.o
BIN
View File
Binary file not shown.
selfdrive/controls/lib/lateral_mpc/libmpc.so
BIN
View File
Binary file not shown.
selfdrive/controls/lib/lateral_mpc/libmpc_py.py
+1 -1
View File
@@ -18,7 +18,7 @@ typedef struct {
double y[21];
double psi[21];
double delta[21];
double rate[21];
double rate[20];
double cost;
} log_t;
selfdrive/controls/lib/long_mpc.py
+8 -3
View File
@@ -1,3 +1,4 @@
import os
import numpy as np
import selfdrive.messaging as messaging
@@ -7,6 +8,8 @@ from selfdrive.controls.lib.radar_helpers import _LEAD_ACCEL_TAU
from selfdrive.controls.lib.longitudinal_mpc import libmpc_py
from selfdrive.controls.lib.drive_helpers import MPC_COST_LONG
LOG_MPC = os.environ.get('LOG_MPC', False)
class LongitudinalMpc(object):
def __init__(self, mpc_id, live_longitudinal_mpc):
@@ -56,7 +59,7 @@ class LongitudinalMpc(object):
self.cur_state[0].a_ego = a
def update(self, CS, lead, v_cruise_setpoint):
v_ego = CS.carState.vEgo
v_ego = CS.vEgo
# Setup current mpc state
self.cur_state[0].x_ego = 0.0
@@ -92,7 +95,9 @@ class LongitudinalMpc(object):
t = sec_since_boot()
n_its = self.libmpc.run_mpc(self.cur_state, self.mpc_solution, self.a_lead_tau, a_lead)
duration = int((sec_since_boot() - t) * 1e9)
self.send_mpc_solution(n_its, duration)
if LOG_MPC:
self.send_mpc_solution(n_its, duration)
# Get solution. MPC timestep is 0.2 s, so interpolation to 0.05 s is needed
self.v_mpc = self.mpc_solution[0].v_ego[1]
@@ -116,5 +121,5 @@ class LongitudinalMpc(object):
self.cur_state[0].v_ego = v_ego
self.cur_state[0].a_ego = 0.0
self.v_mpc = v_ego
self.a_mpc = CS.carState.aEgo
self.a_mpc = CS.aEgo
self.prev_lead_status = False

Some files were not shown because too many files have changed in this diff Show More