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

openpilot v0.5.12 release

Browse Source
This commit is contained in:
Vehicle Researcher
2019-05-16 13:20:29 -07:00
parent 52e55eb244
commit 3f9059fea8
177 changed files with 10381 additions and 835 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/athena/athenad.py
+15
View File
@@ -6,9 +6,11 @@ import time
import threading
import traceback
import zmq
import requests
import six.moves.queue
from jsonrpc import JSONRPCResponseManager, dispatcher
from websocket import create_connection, WebSocketTimeoutException
from selfdrive.loggerd.config import ROOT
import selfdrive.crash as crash
import selfdrive.messaging as messaging
@@ -71,6 +73,19 @@ def getMessage(service=None, timeout=1000):
ret = messaging.recv_one(socket)
return ret.to_dict()
@dispatcher.add_method
def listDataDirectory():
files = [os.path.relpath(os.path.join(dp, f), ROOT) for dp, dn, fn in os.walk(ROOT) for f in fn]
return files
@dispatcher.add_method
def uploadFileToUrl(fn, url, headers):
if len(fn) == 0 or fn[0] == '/' or '..' in fn:
return 500
with open(os.path.join(ROOT, fn), "rb") as f:
ret = requests.put(url, data=f, headers=headers, timeout=10)
return ret.status_code
def ws_recv(ws, end_event):
while not end_event.is_set():
try:
selfdrive/boardd/Makefile
+19 -1
View File
@@ -46,6 +46,7 @@ all: boardd
include ../common/cereal.mk
OBJS = boardd.o \
can_list_to_can_capnp.o \
../common/swaglog.o \
../common/params.o \
../common/util.o \
@@ -72,6 +73,15 @@ boardd.o: boardd.cc
-I../../ \
-c -o '$@' '$<'
boardd_api_impl.so: libcan_list_to_can_capnp.a boardd_api_impl.pyx boardd_setup.py
python boardd_setup.py build_ext --inplace
rm -rf build
rm -f boardd_api_impl.cpp
libcan_list_to_can_capnp.a: can_list_to_can_capnp.o $(CEREAL_OBJS)
ar rcs '$@' $^
%.o: %.c
@echo "[ CC ] $@"
$(CC) $(CFLAGS) -MMD \
@@ -81,8 +91,16 @@ boardd.o: boardd.cc
$(JSON_FLAGS) \
-c -o '$@' '$<'
%.o: %.cc
@echo "[ CC ] $@"
$(CXX) $(CXXFLAGS) -MMD \
-Iinclude -I.. -I../.. \
$(CEREAL_CXXFLAGS) \
$(ZMQ_FLAGS) \
-c -o '$@' '$<'
.PHONY: clean
clean:
rm -f boardd $(OBJS) $(DEPS)
rm -f boardd libcan_list_to_can_capnp.a boardd_api_impl.so $(OBJS) $(DEPS)
-include $(DEPS)
selfdrive/boardd/boardd.cc
+1
View File
@@ -85,6 +85,7 @@ void *safety_setter_thread(void *s) {
// format for board, make copy due to alignment issues, will be freed on out of scope
auto amsg = kj::heapArray<capnp::word>((value_sz / sizeof(capnp::word)) + 1);
memcpy(amsg.begin(), value, value_sz);
free(value);
capnp::FlatArrayMessageReader cmsg(amsg);
cereal::CarParams::Reader car_params = cmsg.getRoot<cereal::CarParams>();
selfdrive/boardd/boardd.py
Executable → Regular
+7 -239
View File
@@ -1,49 +1,13 @@
#!/usr/bin/env python
# This file is not used by OpenPilot. Only boardd.cc is used.
# The python version is slower, but has more options for development.
# TODO: merge the extra functionalities of this file (like MOCK) in boardd.c and
# delete this python version of boardd
# pylint: skip-file
import os
import struct
import zmq
import time
import subprocess
import selfdrive.messaging as messaging
from common.realtime import Ratekeeper
from selfdrive.services import service_list
from selfdrive.swaglog import cloudlog
# Cython
boardd_api_dir = os.path.dirname(os.path.abspath(__file__))
subprocess.check_call(["make", "boardd_api_impl.so"], cwd=boardd_api_dir)
from selfdrive.boardd.boardd_api_impl import can_list_to_can_capnp
assert can_list_to_can_capnp
# USB is optional
try:
import usb1
from usb1 import USBErrorIO, USBErrorOverflow #pylint: disable=no-name-in-module
except Exception:
pass
SAFETY_NOOUTPUT = 0
SAFETY_HONDA = 1
SAFETY_TOYOTA = 2
SAFETY_CHRYSLER = 9
SAFETY_TOYOTA_NOLIMITS = 0x1336
SAFETY_ALLOUTPUT = 0x1337
# *** serialization functions ***
def can_list_to_can_capnp(can_msgs, msgtype='can'):
dat = messaging.new_message()
dat.init(msgtype, len(can_msgs))
for i, can_msg in enumerate(can_msgs):
if msgtype == 'sendcan':
cc = dat.sendcan[i]
else:
cc = dat.can[i]
cc.address = can_msg[0]
cc.busTime = can_msg[1]
cc.dat = str(can_msg[2])
cc.src = can_msg[3]
return dat
def can_capnp_to_can_list(can, src_filter=None):
ret = []
@@ -51,199 +15,3 @@ def can_capnp_to_can_list(can, src_filter=None):
if src_filter is None or msg.src in src_filter:
ret.append((msg.address, msg.busTime, msg.dat, msg.src))
return ret
# *** can driver ***
def can_health():
while 1:
try:
dat = handle.controlRead(usb1.TYPE_VENDOR | usb1.RECIPIENT_DEVICE, 0xd2, 0, 0, 0x10)
break
except (USBErrorIO, USBErrorOverflow):
cloudlog.exception("CAN: BAD HEALTH, RETRYING")
v, i, started = struct.unpack("IIB", dat[0:9])
# TODO: units
return {"voltage": v, "current": i, "started": bool(started)}
def __parse_can_buffer(dat):
ret = []
for j in range(0, len(dat), 0x10):
ddat = dat[j:j+0x10]
f1, f2 = struct.unpack("II", ddat[0:8])
ret.append((f1 >> 21, f2>>16, ddat[8:8+(f2&0xF)], (f2>>4)&0xF))
return ret
def can_send_many(arr):
snds = []
for addr, _, dat, alt in arr:
if addr < 0x800: # only support 11 bit addr
snd = struct.pack("II", ((addr << 21) | 1), len(dat) | (alt << 4)) + dat
snd = snd.ljust(0x10, '\x00')
snds.append(snd)
while 1:
try:
handle.bulkWrite(3, ''.join(snds))
break
except (USBErrorIO, USBErrorOverflow):
cloudlog.exception("CAN: BAD SEND MANY, RETRYING")
def can_recv():
dat = ""
while 1:
try:
dat = handle.bulkRead(1, 0x10*256)
break
except (USBErrorIO, USBErrorOverflow):
cloudlog.exception("CAN: BAD RECV, RETRYING")
return __parse_can_buffer(dat)
def can_init():
global handle, context
handle = None
cloudlog.info("attempting can init")
context = usb1.USBContext()
#context.setDebug(9)
for device in context.getDeviceList(skip_on_error=True):
if device.getVendorID() == 0xbbaa and device.getProductID() == 0xddcc:
handle = device.open()
handle.claimInterface(0)
handle.controlWrite(0x40, 0xdc, SAFETY_ALLOUTPUT, 0, b'')
if handle is None:
cloudlog.warn("CAN NOT FOUND")
exit(-1)
cloudlog.info("got handle")
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)
while 1:
tsc = messaging.drain_sock(logcan, wait_for_one=True)
snds = [can_capnp_to_can_list(x.can) for x in tsc]
snds = [x for x in snds if x[-1] <= 1]
can_send_many(snds)
# recv @ 100hz
can_msgs = can_recv()
print("sent %d got %d" % (len(snds), len(can_msgs)))
m = can_list_to_can_capnp(can_msgs)
sendcan.send(m.to_bytes())
def boardd_test_loop():
can_init()
cnt = 0
while 1:
can_send_many([[0xbb,0,"\xaa\xaa\xaa\xaa",0], [0xaa,0,"\xaa\xaa\xaa\xaa"+struct.pack("!I", cnt),1]])
#can_send_many([[0xaa,0,"\xaa\xaa\xaa\xaa",0]])
#can_send_many([[0xaa,0,"\xaa\xaa\xaa\xaa",1]])
# recv @ 100hz
can_msgs = can_recv()
print("got %d" % (len(can_msgs)))
time.sleep(0.01)
cnt += 1
# *** 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)
# *** subscribes to can send
sendcan = messaging.sub_sock(context, service_list['sendcan'].port)
# drain sendcan to delete any stale messages from previous runs
messaging.drain_sock(sendcan)
while 1:
# health packet @ 1hz
if (rk.frame%rate) == 0:
health = can_health()
msg = messaging.new_message()
msg.init('health')
# store the health to be logged
msg.health.voltage = health['voltage']
msg.health.current = health['current']
msg.health.started = health['started']
msg.health.controlsAllowed = True
health_sock.send(msg.to_bytes())
# recv @ 100hz
can_msgs = can_recv()
# publish to logger
# TODO: refactor for speed
if len(can_msgs) > 0:
dat = can_list_to_can_capnp(can_msgs)
logcan.send(dat.to_bytes())
# send can if we have a packet
tsc = messaging.recv_sock(sendcan)
if tsc is not None:
can_send_many(can_capnp_to_can_list(tsc.sendcan))
rk.keep_time()
# *** 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)
# *** publishes to can send
sendcan = messaging.pub_sock(context, service_list['sendcan'].port)
# drain sendcan to delete any stale messages from previous runs
messaging.drain_sock(sendcan)
while 1:
# recv @ 100hz
can_msgs = can_recv()
#for m in can_msgs:
# print("R: {0} {1}".format(hex(m[0]), str(m[2]).encode("hex")))
# publish to logger
# TODO: refactor for speed
if len(can_msgs) > 0:
dat = can_list_to_can_capnp(can_msgs, "sendcan")
sendcan.send(dat.to_bytes())
# send can if we have a packet
tsc = messaging.recv_sock(logcan)
if tsc is not None:
cl = can_capnp_to_can_list(tsc.can)
#for m in cl:
# print("S: {0} {1}".format(hex(m[0]), str(m[2]).encode("hex")))
can_send_many(cl)
rk.keep_time()
def main(gctx=None):
if os.getenv("MOCK") is not None:
boardd_mock_loop()
elif os.getenv("PROXY") is not None:
boardd_proxy_loop()
elif os.getenv("BOARDTEST") is not None:
boardd_test_loop()
else:
boardd_loop()
if __name__ == "__main__":
main()
selfdrive/boardd/boardd_api_impl.pyx
+25
View File
@@ -0,0 +1,25 @@
# distutils: language = c++
from libcpp.vector cimport vector
from libcpp.string cimport string
from libcpp cimport bool
cdef struct can_frame:
long address
string dat
long busTime
long src
cdef extern void can_list_to_can_capnp_cpp(const vector[can_frame] &can_list, string &out, bool sendCan)
def can_list_to_can_capnp(can_msgs, msgtype='can'):
cdef vector[can_frame] can_list
cdef can_frame f
for can_msg in can_msgs:
f.address = can_msg[0]
f.busTime = can_msg[1]
f.dat = can_msg[2]
f.src = can_msg[3]
can_list.push_back(f)
cdef string out
can_list_to_can_capnp_cpp(can_list, out, msgtype == 'sendcan')
return out
selfdrive/boardd/boardd_setup.py
+25
View File
@@ -0,0 +1,25 @@
import subprocess
from distutils.core import setup, Extension
from Cython.Build import cythonize
PHONELIBS = '../../phonelibs'
ARCH = subprocess.check_output(["uname", "-m"]).rstrip()
ARCH_DIR = 'x64' if ARCH == "x86_64" else 'aarch64'
setup(name='Boardd API Implementation',
ext_modules=cythonize(
Extension(
"boardd_api_impl",
libraries=[':libcan_list_to_can_capnp.a', ':libcapnp.a', ':libcapnp.a', ':libkj.a'],
library_dirs=[
'./',
PHONELIBS + '/capnp-cpp/' + ARCH_DIR + '/lib/',
PHONELIBS + '/capnp-c/' + ARCH_DIR + '/lib/'
],
sources=['boardd_api_impl.pyx'],
language="c++",
extra_compile_args=["-std=c++11"],
)
)
)
selfdrive/boardd/can_list_to_can_capnp.cc
+36
View File
@@ -0,0 +1,36 @@
#include <vector>
#include <tuple>
#include <string>
#include "common/timing.h"
#include <capnp/serialize.h>
#include "cereal/gen/cpp/log.capnp.h"
#include "cereal/gen/cpp/car.capnp.h"
typedef struct {
long address;
std::string dat;
long busTime;
long src;
} can_frame;
extern "C" {
void can_list_to_can_capnp_cpp(const std::vector<can_frame> &can_list, std::string &out, bool sendCan) {
capnp::MallocMessageBuilder msg;
cereal::Event::Builder event = msg.initRoot<cereal::Event>();
event.setLogMonoTime(nanos_since_boot());
auto canData = sendCan ? event.initSendcan(can_list.size()) : event.initCan(can_list.size());
int i = 0;
for (auto it = can_list.begin(); it != can_list.end(); it++, i++) {
canData[i].setAddress(it->address);
canData[i].setBusTime(it->busTime);
canData[i].setDat(kj::arrayPtr((uint8_t*)it->dat.data(), it->dat.size()));
canData[i].setSrc(it->src);
}
auto words = capnp::messageToFlatArray(msg);
auto bytes = words.asBytes();
out.append((const char *)bytes.begin(), bytes.size());
}
}
selfdrive/boardd/tests/__init__.py
View File
selfdrive/boardd/tests/boardd_old.py
Executable
+247
View File
@@ -0,0 +1,247 @@
#!/usr/bin/env python
# This file is not used by OpenPilot. Only boardd.cc is used.
# The python version is slower, but has more options for development.
# TODO: merge the extra functionalities of this file (like MOCK) in boardd.c and
# delete this python version of boardd
import os
import struct
import zmq
import time
import selfdrive.messaging as messaging
from common.realtime import Ratekeeper
from selfdrive.services import service_list
from selfdrive.swaglog import cloudlog
from selfdrive.boardd.boardd import can_capnp_to_can_list
# USB is optional
try:
import usb1
from usb1 import USBErrorIO, USBErrorOverflow #pylint: disable=no-name-in-module
except Exception:
pass
SAFETY_NOOUTPUT = 0
SAFETY_HONDA = 1
SAFETY_TOYOTA = 2
SAFETY_CHRYSLER = 9
SAFETY_TOYOTA_NOLIMITS = 0x1336
SAFETY_ALLOUTPUT = 0x1337
# *** serialization functions ***
def can_list_to_can_capnp(can_msgs, msgtype='can'):
dat = messaging.new_message()
dat.init(msgtype, len(can_msgs))
for i, can_msg in enumerate(can_msgs):
if msgtype == 'sendcan':
cc = dat.sendcan[i]
else:
cc = dat.can[i]
cc.address = can_msg[0]
cc.busTime = can_msg[1]
cc.dat = str(can_msg[2])
cc.src = can_msg[3]
return dat
# *** can driver ***
def can_health():
while 1:
try:
dat = handle.controlRead(usb1.TYPE_VENDOR | usb1.RECIPIENT_DEVICE, 0xd2, 0, 0, 0x10)
break
except (USBErrorIO, USBErrorOverflow):
cloudlog.exception("CAN: BAD HEALTH, RETRYING")
v, i, started = struct.unpack("IIB", dat[0:9])
# TODO: units
return {"voltage": v, "current": i, "started": bool(started)}
def __parse_can_buffer(dat):
ret = []
for j in range(0, len(dat), 0x10):
ddat = dat[j:j+0x10]
f1, f2 = struct.unpack("II", ddat[0:8])
ret.append((f1 >> 21, f2>>16, ddat[8:8+(f2&0xF)], (f2>>4)&0xFF))
return ret
def can_send_many(arr):
snds = []
for addr, _, dat, alt in arr:
if addr < 0x800: # only support 11 bit addr
snd = struct.pack("II", ((addr << 21) | 1), len(dat) | (alt << 4)) + dat
snd = snd.ljust(0x10, '\x00')
snds.append(snd)
while 1:
try:
handle.bulkWrite(3, ''.join(snds))
break
except (USBErrorIO, USBErrorOverflow):
cloudlog.exception("CAN: BAD SEND MANY, RETRYING")
def can_recv():
dat = ""
while 1:
try:
dat = handle.bulkRead(1, 0x10*256)
break
except (USBErrorIO, USBErrorOverflow):
cloudlog.exception("CAN: BAD RECV, RETRYING")
return __parse_can_buffer(dat)
def can_init():
global handle, context
handle = None
cloudlog.info("attempting can init")
context = usb1.USBContext()
#context.setDebug(9)
for device in context.getDeviceList(skip_on_error=True):
if device.getVendorID() == 0xbbaa and device.getProductID() == 0xddcc:
handle = device.open()
handle.claimInterface(0)
handle.controlWrite(0x40, 0xdc, SAFETY_ALLOUTPUT, 0, b'')
if handle is None:
cloudlog.warn("CAN NOT FOUND")
exit(-1)
cloudlog.info("got handle")
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)
while 1:
tsc = messaging.drain_sock(logcan, wait_for_one=True)
snds = map(lambda x: can_capnp_to_can_list(x.can), tsc)
snd = []
for s in snds:
snd += s
snd = filter(lambda x: x[-1] <= 1, snd)
can_send_many(snd)
# recv @ 100hz
can_msgs = can_recv()
print("sent %d got %d" % (len(snd), len(can_msgs)))
m = can_list_to_can_capnp(can_msgs, msgtype='sendcan')
sendcan.send(m.to_bytes())
def boardd_test_loop():
can_init()
cnt = 0
while 1:
can_send_many([[0xbb,0,"\xaa\xaa\xaa\xaa",0], [0xaa,0,"\xaa\xaa\xaa\xaa"+struct.pack("!I", cnt),1]])
#can_send_many([[0xaa,0,"\xaa\xaa\xaa\xaa",0]])
#can_send_many([[0xaa,0,"\xaa\xaa\xaa\xaa",1]])
# recv @ 100hz
can_msgs = can_recv()
print("got %d" % (len(can_msgs)))
time.sleep(0.01)
cnt += 1
# *** 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)
# *** subscribes to can send
sendcan = messaging.sub_sock(context, service_list['sendcan'].port)
# drain sendcan to delete any stale messages from previous runs
messaging.drain_sock(sendcan)
while 1:
# health packet @ 1hz
if (rk.frame%rate) == 0:
health = can_health()
msg = messaging.new_message()
msg.init('health')
# store the health to be logged
msg.health.voltage = health['voltage']
msg.health.current = health['current']
msg.health.started = health['started']
msg.health.controlsAllowed = True
health_sock.send(msg.to_bytes())
# recv @ 100hz
can_msgs = can_recv()
# publish to logger
# TODO: refactor for speed
if len(can_msgs) > 0:
dat = can_list_to_can_capnp(can_msgs).to_bytes()
logcan.send(dat)
# send can if we have a packet
tsc = messaging.recv_sock(sendcan)
if tsc is not None:
can_send_many(can_capnp_to_can_list(tsc.sendcan))
rk.keep_time()
# *** 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)
# *** publishes to can send
sendcan = messaging.pub_sock(context, service_list['sendcan'].port)
# drain sendcan to delete any stale messages from previous runs
messaging.drain_sock(sendcan)
while 1:
# recv @ 100hz
can_msgs = can_recv()
#for m in can_msgs:
# print("R: {0} {1}".format(hex(m[0]), str(m[2]).encode("hex")))
# publish to logger
# TODO: refactor for speed
if len(can_msgs) > 0:
dat = can_list_to_can_capnp(can_msgs, "sendcan")
sendcan.send(dat)
# send can if we have a packet
tsc = messaging.recv_sock(logcan)
if tsc is not None:
cl = can_capnp_to_can_list(tsc.can)
#for m in cl:
# print("S: {0} {1}".format(hex(m[0]), str(m[2]).encode("hex")))
can_send_many(cl)
rk.keep_time()
def main(gctx=None):
if os.getenv("MOCK") is not None:
boardd_mock_loop()
elif os.getenv("PROXY") is not None:
boardd_proxy_loop()
elif os.getenv("BOARDTEST") is not None:
boardd_test_loop()
else:
boardd_loop()
if __name__ == "__main__":
main()
selfdrive/boardd/tests/fuzzer.py
Executable
+19
View File
@@ -0,0 +1,19 @@
#!/usr/bin/env python
import time
import random
from boardd_old import can_init, can_recv, can_send_many, can_health
if __name__ == "__main__":
can_init()
while 1:
c = random.randint(0, 3)
if c == 0:
print can_recv()
elif c == 1:
print can_health()
elif c == 2:
many = [[0x123, 0, "abcdef", 0]] * random.randint(1, 10)
can_send_many(many)
elif c == 3:
time.sleep(random.randint(0, 100) / 1000.0)
selfdrive/boardd/tests/test_boardd_api.py
+76
View File
@@ -0,0 +1,76 @@
import random
import numpy as np
import boardd_old
import selfdrive.boardd.boardd as boardd
from common.realtime import sec_since_boot
from cereal import log
import unittest
def generate_random_can_data_list():
can_list = []
cnt = random.randint(1, 64)
for j in xrange(cnt):
can_data = np.random.bytes(random.randint(1, 8))
can_list.append([random.randint(0, 128), random.randint(0, 128), can_data, random.randint(0, 128)])
return can_list, cnt
class TestBoarddApiMethods(unittest.TestCase):
def test_correctness(self):
for i in xrange(1000):
can_list, _ = generate_random_can_data_list()
# Sendcan
# Old API
m_old = boardd_old.can_list_to_can_capnp(can_list, 'sendcan').to_bytes()
# new API
m = boardd.can_list_to_can_capnp(can_list, 'sendcan')
ev_old = log.Event.from_bytes(m_old)
ev = log.Event.from_bytes(m)
self.assertEqual(ev_old.which(), ev.which())
self.assertEqual(len(ev.sendcan), len(ev_old.sendcan))
for i in xrange(len(ev.sendcan)):
attrs = ['address', 'busTime', 'dat', 'src']
for attr in attrs:
self.assertEqual(getattr(ev.sendcan[i], attr, 'new'), getattr(ev_old.sendcan[i], attr, 'old'))
# Can
m_old = boardd_old.can_list_to_can_capnp(can_list, 'can').to_bytes()
# new API
m = boardd.can_list_to_can_capnp(can_list, 'can')
ev_old = log.Event.from_bytes(m_old)
ev = log.Event.from_bytes(m)
self.assertEqual(ev_old.which(), ev.which())
self.assertEqual(len(ev.can), len(ev_old.can))
for i in xrange(len(ev.can)):
attrs = ['address', 'busTime', 'dat', 'src']
for attr in attrs:
self.assertEqual(getattr(ev.can[i], attr, 'new'), getattr(ev_old.can[i], attr, 'old'))
def test_performance(self):
can_list, cnt = generate_random_can_data_list()
recursions = 1000
n1 = sec_since_boot()
for i in xrange(recursions):
boardd_old.can_list_to_can_capnp(can_list, 'sendcan').to_bytes()
n2 = sec_since_boot()
elapsed_old = n2 - n1
# print('Old API, elapsed time: {} secs'.format(elapsed_old))
n1 = sec_since_boot()
for i in xrange(recursions):
boardd.can_list_to_can_capnp(can_list)
n2 = sec_since_boot()
elapsed_new = n2 - n1
# print('New API, elapsed time: {} secs'.format(elapsed_new))
self.assertTrue(elapsed_new < elapsed_old / 2)
if __name__ == '__main__':
unittest.main()
selfdrive/boardd/tests/test_boardd_loopback.py
Executable
+51
View File
@@ -0,0 +1,51 @@
#!/usr/bin/env python
"""Run boardd with the BOARDD_LOOPBACK envvar before running this test."""
import os
import random
import zmq
import time
from selfdrive.boardd.boardd import can_list_to_can_capnp
from selfdrive.messaging import drain_sock, pub_sock, sub_sock
from selfdrive.services import service_list
def get_test_string():
return b"test"+os.urandom(10)
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
time.sleep(0.3) # wait to bind before send/recv
for i in range(10):
print("Loop %d" % i)
at = random.randint(1024, 2000)
st = get_test_string()[0:8]
snd.send(can_list_to_can_capnp([[at, 0, st, 0]], msgtype='sendcan').to_bytes())
time.sleep(0.1)
res = drain_sock(rcv, True)
assert len(res) == 1
res = res[0].can
assert len(res) == 2
msg0, msg1 = res
assert msg0.dat == st
assert msg1.dat == st
assert msg0.address == at
assert msg1.address == at
assert msg0.src == 0x80 | BUS
assert msg1.src == BUS
print("Success")
if __name__ == "__main__":
main()
selfdrive/can/Makefile
+7 -3
View File
@@ -23,9 +23,8 @@ ifeq ($(UNAME_S),Darwin)
else ifeq ($(OPTEST),1)
ZMQ_LIBS = -lzmq
else ifeq ($(UNAME_M),x86_64)
EXTERNAL := ../../external
ZMQ_FLAGS = -I$(EXTERNAL)/zmq/include
ZMQ_LIBS = -L$(EXTERNAL)/zmq/lib -l:libzmq.a
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
@@ -67,6 +66,11 @@ libdbc.so:: $(LIBDBC_OBJS) $(DBC_OBJS)
$(CEREAL_CXXFLAGS) \
$(CEREAL_LIBS)
packer_impl.so: packer_impl.pyx packer_setup.py
python packer_setup.py build_ext --inplace
rm -rf build
rm -f packer_impl.cpp
$(OBJDIR)/%.o: %.cc
@echo "[ CXX ] $@"
$(CXX) -fPIC -c -o '$@' $^ \
selfdrive/can/packer.cc
+4
View File
@@ -129,4 +129,8 @@ extern "C" {
return cp->pack(address, std::vector<SignalPackValue>(vals, vals+num_vals), counter);
}
uint64_t canpack_pack_vector(void* inst, uint32_t address, const std::vector<SignalPackValue> &signals, int counter) {
CANPacker *cp = (CANPacker*)inst;
return cp->pack(address, signals, counter);
}
}
selfdrive/can/packer.py
+7 -66
View File
@@ -1,68 +1,9 @@
import six
import struct
from selfdrive.can.libdbc_py import libdbc, ffi
# pylint: skip-file
import os
import subprocess
can_dir = os.path.dirname(os.path.abspath(__file__))
subprocess.check_call(["make", "packer_impl.so"], cwd=can_dir)
class CANPacker(object):
def __init__(self, dbc_name):
self.packer = libdbc.canpack_init(dbc_name)
self.dbc = libdbc.dbc_lookup(dbc_name)
self.sig_names = {}
self.name_to_address_and_size = {}
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.name_to_address_and_size[name] = (address, msg.size)
self.name_to_address_and_size[address] = (address, msg.size)
def pack(self, addr, values, counter):
values_thing = []
for name, value in six.iteritems(values):
if name not in self.sig_names:
self.sig_names[name] = ffi.new("char[]", name)
values_thing.append({
'name': self.sig_names[name],
'value': value
})
values_c = ffi.new("SignalPackValue[]", values_thing)
return libdbc.canpack_pack(self.packer, addr, len(values_thing), values_c, counter)
def pack_bytes(self, addr, values, counter=-1):
addr, size = self.name_to_address_and_size[addr]
val = self.pack(addr, values, counter)
r = struct.pack(">Q", val)
return addr, r[:size]
def make_can_msg(self, addr, bus, values, counter=-1):
addr, msg = self.pack_bytes(addr, values, counter)
return [addr, 0, msg, bus]
if __name__ == "__main__":
## little endian test
cp = CANPacker("hyundai_santa_fe_2019_ccan")
s = cp.pack_bytes(0x340, {
"CR_Lkas_StrToqReq": -0.06,
#"CF_Lkas_FcwBasReq": 1,
"CF_Lkas_MsgCount": 7,
"CF_Lkas_HbaSysState": 0,
#"CF_Lkas_Chksum": 3,
})
s = cp.pack_bytes(0x340, {
"CF_Lkas_MsgCount": 1,
})
# big endian test
#cp = CANPacker("honda_civic_touring_2016_can_generated")
#s = cp.pack_bytes(0xe4, {
# "STEER_TORQUE": -2,
#})
print([hex(ord(v)) for v in s[1]])
print(s[1].encode("hex"))
from selfdrive.can.packer_impl import CANPacker
assert CANPacker
selfdrive/can/packer_impl.pyx
+111
View File
@@ -0,0 +1,111 @@
# distutils: language = c++
from libc.stdint cimport uint32_t, uint64_t
from libcpp.vector cimport vector
from libcpp.map cimport map
from libcpp.string cimport string
from libcpp cimport bool
from posix.dlfcn cimport dlopen, dlsym, RTLD_LAZY
import os
import subprocess
cdef struct SignalPackValue:
const char* name
double value
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
ctypedef void * (*canpack_init_func)(const char* dbc_name)
ctypedef uint64_t (*canpack_pack_vector_func)(void* inst, uint32_t address, const vector[SignalPackValue] &signals, int counter)
ctypedef const DBC * (*dbc_lookup_func)(const char* dbc_name)
cdef class CANPacker(object):
cdef void *packer
cdef const DBC *dbc
cdef map[string, (int, int)] name_to_address_and_size
cdef map[int, int] address_to_size
cdef canpack_init_func canpack_init
cdef canpack_pack_vector_func canpack_pack_vector
cdef dbc_lookup_func dbc_lookup
def __init__(self, dbc_name):
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)
cdef void *libdbc = dlopen(libdbc_fn, RTLD_LAZY)
self.canpack_init = <canpack_init_func>dlsym(libdbc, 'canpack_init')
self.canpack_pack_vector = <canpack_pack_vector_func>dlsym(libdbc, 'canpack_pack_vector')
self.dbc_lookup = <dbc_lookup_func>dlsym(libdbc, 'dbc_lookup')
self.packer = self.canpack_init(dbc_name)
self.dbc = self.dbc_lookup(dbc_name)
num_msgs = self.dbc[0].num_msgs
for i in range(num_msgs):
msg = self.dbc[0].msgs[i]
self.name_to_address_and_size[string(msg.name)] = (msg.address, msg.size)
self.address_to_size[msg.address] = msg.size
cdef uint64_t pack(self, addr, values, counter):
cdef vector[SignalPackValue] values_thing
cdef SignalPackValue spv
for name, value in values.iteritems():
spv.name = name
spv.value = value
values_thing.push_back(spv)
return self.canpack_pack_vector(self.packer, addr, values_thing, counter)
cdef inline uint64_t ReverseBytes(self, uint64_t x):
return (((x & 0xff00000000000000ull) >> 56) |
((x & 0x00ff000000000000ull) >> 40) |
((x & 0x0000ff0000000000ull) >> 24) |
((x & 0x000000ff00000000ull) >> 8) |
((x & 0x00000000ff000000ull) << 8) |
((x & 0x0000000000ff0000ull) << 24) |
((x & 0x000000000000ff00ull) << 40) |
((x & 0x00000000000000ffull) << 56))
cpdef make_can_msg(self, name_or_addr, bus, values, counter=-1):
cdef int addr, size
if type(name_or_addr) == int:
addr = name_or_addr
size = self.address_to_size[name_or_addr]
else:
addr, size = self.name_to_address_and_size[name_or_addr]
cdef uint64_t val = self.pack(addr, values, counter)
val = self.ReverseBytes(val)
return [addr, 0, (<char *>&val)[:size], bus]
selfdrive/can/packer_setup.py
+5
View File
@@ -0,0 +1,5 @@
from distutils.core import setup, Extension
from Cython.Build import cythonize
setup(name='CAN Packer API Implementation',
ext_modules=cythonize(Extension("packer_impl", ["packer_impl.pyx"], language="c++", extra_compile_args=["-std=c++11"])))
selfdrive/can/parser.py
+1 -2
View File
@@ -1,4 +1,3 @@
import six
import time
from collections import defaultdict
import numbers
@@ -59,7 +58,7 @@ class CANParser(object):
{
'address': msg_address,
'check_frequency': freq,
} for msg_address, freq in six.iteritems(message_options)])
} 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)
selfdrive/can/process_dbc.py
+3 -3
View File
@@ -2,7 +2,7 @@
import os
import glob
import sys
import six
import jinja2
from collections import Counter
@@ -39,10 +39,10 @@ def main():
continue #skip output is newer than template and dbc
msgs = [(address, msg_name, msg_size, sorted(msg_sigs, key=lambda s: s.name not in ("COUNTER", "CHECKSUM"))) # process counter and checksums first
for address, ((msg_name, msg_size), msg_sigs) in sorted(six.iteritems(can_dbc.msgs)) if msg_sigs]
for address, ((msg_name, msg_size), msg_sigs) in sorted(can_dbc.msgs.items()) if msg_sigs]
def_vals = {a: set(b) for a,b in can_dbc.def_vals.items()} #remove duplicates
def_vals = [(address, sig) for address, sig in sorted(six.iteritems(def_vals))]
def_vals = [(address, sig) for address, sig in sorted(def_vals.items())]
if can_dbc.name.startswith("honda") or can_dbc.name.startswith("acura"):
checksum_type = "honda"
selfdrive/can/tests/__init__.py
View File
selfdrive/can/tests/packer_old.py
+67
View File
@@ -0,0 +1,67 @@
import struct
from selfdrive.can.libdbc_py import libdbc, ffi
class CANPacker(object):
def __init__(self, dbc_name):
self.packer = libdbc.canpack_init(dbc_name)
self.dbc = libdbc.dbc_lookup(dbc_name)
self.sig_names = {}
self.name_to_address_and_size = {}
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.name_to_address_and_size[name] = (address, msg.size)
self.name_to_address_and_size[address] = (address, msg.size)
def pack(self, addr, values, counter):
values_thing = []
for name, value in values.iteritems():
if name not in self.sig_names:
self.sig_names[name] = ffi.new("char[]", name)
values_thing.append({
'name': self.sig_names[name],
'value': value
})
values_c = ffi.new("SignalPackValue[]", values_thing)
return libdbc.canpack_pack(self.packer, addr, len(values_thing), values_c, counter)
def pack_bytes(self, addr, values, counter=-1):
addr, size = self.name_to_address_and_size[addr]
val = self.pack(addr, values, counter)
r = struct.pack(">Q", val)
return addr, r[:size]
def make_can_msg(self, addr, bus, values, counter=-1):
addr, msg = self.pack_bytes(addr, values, counter)
return [addr, 0, msg, bus]
if __name__ == "__main__":
## little endian test
cp = CANPacker("hyundai_santa_fe_2019_ccan")
s = cp.pack_bytes(0x340, {
"CR_Lkas_StrToqReq": -0.06,
#"CF_Lkas_FcwBasReq": 1,
"CF_Lkas_MsgCount": 7,
"CF_Lkas_HbaSysState": 0,
#"CF_Lkas_Chksum": 3,
})
s = cp.pack_bytes(0x340, {
"CF_Lkas_MsgCount": 1,
})
# big endian test
#cp = CANPacker("honda_civic_touring_2016_can_generated")
#s = cp.pack_bytes(0xe4, {
# "STEER_TORQUE": -2,
#})
print([hex(ord(v)) for v in s[1]])
print(s[1].encode("hex"))
selfdrive/can/tests/test_packer_chrysler.py
+35
View File
@@ -0,0 +1,35 @@
import unittest
import random
from selfdrive.can.tests.packer_old import CANPacker as CANPackerOld
from selfdrive.can.packer import CANPacker
import selfdrive.car.chrysler.chryslercan as chryslercan
class TestPackerMethods(unittest.TestCase):
def setUp(self):
self.chrysler_cp_old = CANPackerOld("chrysler_pacifica_2017_hybrid")
self.chrysler_cp = CANPacker("chrysler_pacifica_2017_hybrid")
def test_correctness(self):
# Test all commands, randomize the params.
for _ in xrange(1000):
gear = ('drive', 'reverse', 'low')[random.randint(0, 3) % 3]
lkas_active = (random.randint(0, 2) % 2 == 0)
hud_alert = random.randint(0, 6)
hud_count = random.randint(0, 65536)
lkas_car_model = random.randint(0, 65536)
m_old = chryslercan.create_lkas_hud(self.chrysler_cp_old, gear, lkas_active, hud_alert, hud_count, lkas_car_model)
m = chryslercan.create_lkas_hud(self.chrysler_cp, gear, lkas_active, hud_alert, hud_count, lkas_car_model)
self.assertEqual(m_old, m)
apply_steer = (random.randint(0, 2) % 2 == 0)
moving_fast = (random.randint(0, 2) % 2 == 0)
frame = random.randint(0, 65536)
m_old = chryslercan.create_lkas_command(self.chrysler_cp_old, apply_steer, moving_fast, frame)
m = chryslercan.create_lkas_command(self.chrysler_cp, apply_steer, moving_fast, frame)
self.assertEqual(m_old, m)
if __name__ == "__main__":
unittest.main()
selfdrive/can/tests/test_packer_gm.py
+66
View File
@@ -0,0 +1,66 @@
import unittest
import random
from selfdrive.can.tests.packer_old import CANPacker as CANPackerOld
from selfdrive.can.packer import CANPacker
import selfdrive.car.gm.gmcan as gmcan
from selfdrive.car.gm.interface import CanBus as GMCanBus
class TestPackerMethods(unittest.TestCase):
def setUp(self):
self.gm_cp_old = CANPackerOld("gm_global_a_powertrain")
self.gm_cp = CANPacker("gm_global_a_powertrain")
self.ct6_cp_old = CANPackerOld("cadillac_ct6_chassis")
self.ct6_cp = CANPacker("cadillac_ct6_chassis")
def test_correctness(self):
# Test all cars' commands, randomize the params.
for _ in xrange(1000):
bus = random.randint(0, 65536)
apply_steer = (random.randint(0, 2) % 2 == 0)
idx = random.randint(0, 65536)
lkas_active = (random.randint(0, 2) % 2 == 0)
m_old = gmcan.create_steering_control(self.gm_cp_old, bus, apply_steer, idx, lkas_active)
m = gmcan.create_steering_control(self.gm_cp, bus, apply_steer, idx, lkas_active)
self.assertEqual(m_old, m)
canbus = GMCanBus()
apply_steer = (random.randint(0, 2) % 2 == 0)
v_ego = random.randint(0, 65536)
idx = random.randint(0, 65536)
enabled = (random.randint(0, 2) % 2 == 0)
m_old = gmcan.create_steering_control_ct6(self.ct6_cp_old, canbus, apply_steer, v_ego, idx, enabled)
m = gmcan.create_steering_control_ct6(self.ct6_cp, canbus, apply_steer, v_ego, idx, enabled)
self.assertEqual(m_old, m)
bus = random.randint(0, 65536)
throttle = random.randint(0, 65536)
idx = random.randint(0, 65536)
acc_engaged = (random.randint(0, 2) % 2 == 0)
at_full_stop = (random.randint(0, 2) % 2 == 0)
m_old = gmcan.create_gas_regen_command(self.gm_cp_old, bus, throttle, idx, acc_engaged, at_full_stop)
m = gmcan.create_gas_regen_command(self.gm_cp, bus, throttle, idx, acc_engaged, at_full_stop)
self.assertEqual(m_old, m)
bus = random.randint(0, 65536)
apply_brake = (random.randint(0, 2) % 2 == 0)
idx = random.randint(0, 65536)
near_stop = (random.randint(0, 2) % 2 == 0)
at_full_stop = (random.randint(0, 2) % 2 == 0)
m_old = gmcan.create_friction_brake_command(self.ct6_cp_old, bus, apply_brake, idx, near_stop, at_full_stop)
m = gmcan.create_friction_brake_command(self.ct6_cp, bus, apply_brake, idx, near_stop, at_full_stop)
self.assertEqual(m_old, m)
bus = random.randint(0, 65536)
acc_engaged = (random.randint(0, 2) % 2 == 0)
target_speed_kph = random.randint(0, 65536)
lead_car_in_sight = (random.randint(0, 2) % 2 == 0)
m_old = gmcan.create_acc_dashboard_command(self.gm_cp_old, bus, acc_engaged, target_speed_kph, lead_car_in_sight)
m = gmcan.create_acc_dashboard_command(self.gm_cp, bus, acc_engaged, target_speed_kph, lead_car_in_sight)
self.assertEqual(m_old, m)
if __name__ == "__main__":
unittest.main()
selfdrive/can/tests/test_packer_honda.py
+56
View File
@@ -0,0 +1,56 @@
import unittest
import random
from selfdrive.can.tests.packer_old import CANPacker as CANPackerOld
from selfdrive.can.packer import CANPacker
import selfdrive.car.honda.hondacan as hondacan
from selfdrive.car.honda.values import HONDA_BOSCH
from selfdrive.car.honda.carcontroller import HUDData
class TestPackerMethods(unittest.TestCase):
def setUp(self):
self.honda_cp_old = CANPackerOld("honda_pilot_touring_2017_can_generated")
self.honda_cp = CANPacker("honda_pilot_touring_2017_can_generated")
def test_correctness(self):
# Test all commands, randomize the params.
for _ in xrange(1000):
apply_brake = (random.randint(0, 2) % 2 == 0)
pump_on = (random.randint(0, 2) % 2 == 0)
pcm_override = (random.randint(0, 2) % 2 == 0)
pcm_cancel_cmd = (random.randint(0, 2) % 2 == 0)
chime = random.randint(0, 65536)
fcw = random.randint(0, 65536)
idx = random.randint(0, 65536)
m_old = hondacan.create_brake_command(self.honda_cp_old, apply_brake, pump_on, pcm_override, pcm_cancel_cmd, chime, fcw, idx)
m = hondacan.create_brake_command(self.honda_cp, apply_brake, pump_on, pcm_override, pcm_cancel_cmd, chime, fcw, idx)
self.assertEqual(m_old, m)
apply_steer = (random.randint(0, 2) % 2 == 0)
lkas_active = (random.randint(0, 2) % 2 == 0)
car_fingerprint = HONDA_BOSCH[0]
idx = random.randint(0, 65536)
m_old = hondacan.create_steering_control(self.honda_cp_old, apply_steer, lkas_active, car_fingerprint, idx)
m = hondacan.create_steering_control(self.honda_cp, apply_steer, lkas_active, car_fingerprint, idx)
self.assertEqual(m_old, m)
pcm_speed = random.randint(0, 65536)
hud = HUDData(random.randint(0, 65536), random.randint(0, 65536), 1, random.randint(0, 65536),
0xc1, random.randint(0, 65536), random.randint(0, 65536), random.randint(0, 65536),
random.randint(0, 65536), random.randint(0, 65536), random.randint(0, 65536))
car_fingerprint = HONDA_BOSCH[0]
idx = random.randint(0, 65536)
m_old = hondacan.create_ui_commands(self.honda_cp_old, pcm_speed, hud, car_fingerprint, idx)
m = hondacan.create_ui_commands(self.honda_cp, pcm_speed, hud, car_fingerprint, idx)
self.assertEqual(m_old, m)
button_val = random.randint(0, 65536)
idx = random.randint(0, 65536)
m_old = hondacan.spam_buttons_command(self.honda_cp_old, button_val, idx)
m = hondacan.spam_buttons_command(self.honda_cp, button_val, idx)
self.assertEqual(m_old, m)
if __name__ == "__main__":
unittest.main()
selfdrive/can/tests/test_packer_hyundai.py
+70
View File
@@ -0,0 +1,70 @@
import unittest
import random
from selfdrive.can.tests.packer_old import CANPacker as CANPackerOld
from selfdrive.can.packer import CANPacker
import selfdrive.car.hyundai.hyundaican as hyundaican
from selfdrive.car.hyundai.values import CHECKSUM as hyundai_checksum
class TestPackerMethods(unittest.TestCase):
def setUp(self):
self.hyundai_cp_old = CANPackerOld("hyundai_kia_generic")
self.hyundai_cp = CANPacker("hyundai_kia_generic")
def test_correctness(self):
# Test all commands, randomize the params.
for _ in xrange(1000):
# Hyundai
car_fingerprint = hyundai_checksum["crc8"][0]
apply_steer = (random.randint(0, 2) % 2 == 0)
steer_req = (random.randint(0, 2) % 2 == 0)
cnt = random.randint(0, 65536)
enabled = (random.randint(0, 2) % 2 == 0)
lkas11 = {
"CF_Lkas_LdwsSysState": random.randint(0,65536),
"CF_Lkas_SysWarning": random.randint(0,65536),
"CF_Lkas_LdwsLHWarning": random.randint(0,65536),
"CF_Lkas_LdwsRHWarning": random.randint(0,65536),
"CF_Lkas_HbaLamp": random.randint(0,65536),
"CF_Lkas_FcwBasReq": random.randint(0,65536),
"CF_Lkas_ToiFlt": random.randint(0,65536),
"CF_Lkas_HbaSysState": random.randint(0,65536),
"CF_Lkas_FcwOpt": random.randint(0,65536),
"CF_Lkas_HbaOpt": random.randint(0,65536),
"CF_Lkas_FcwSysState": random.randint(0,65536),
"CF_Lkas_FcwCollisionWarning": random.randint(0,65536),
"CF_Lkas_FusionState": random.randint(0,65536),
"CF_Lkas_FcwOpt_USM": random.randint(0,65536),
"CF_Lkas_LdwsOpt_USM": random.randint(0,65536)
}
hud_alert = random.randint(0, 65536)
keep_stock = (random.randint(0, 2) % 2 == 0)
m_old = hyundaican.create_lkas11(self.hyundai_cp_old, car_fingerprint, apply_steer, steer_req, cnt, enabled,
lkas11, hud_alert, keep_stock)
m = hyundaican.create_lkas11(self.hyundai_cp, car_fingerprint, apply_steer, steer_req, cnt, enabled,
lkas11, hud_alert, keep_stock)
self.assertEqual(m_old, m)
clu11 = {
"CF_Clu_CruiseSwState": random.randint(0,65536),
"CF_Clu_CruiseSwMain": random.randint(0,65536),
"CF_Clu_SldMainSW": random.randint(0,65536),
"CF_Clu_ParityBit1": random.randint(0,65536),
"CF_Clu_VanzDecimal": random.randint(0,65536),
"CF_Clu_Vanz": random.randint(0,65536),
"CF_Clu_SPEED_UNIT": random.randint(0,65536),
"CF_Clu_DetentOut": random.randint(0,65536),
"CF_Clu_RheostatLevel": random.randint(0,65536),
"CF_Clu_CluInfo": random.randint(0,65536),
"CF_Clu_AmpInfo": random.randint(0,65536),
"CF_Clu_AliveCnt1": random.randint(0,65536),
}
button = random.randint(0, 65536)
m_old = hyundaican.create_clu11(self.hyundai_cp_old, clu11, button)
m = hyundaican.create_clu11(self.hyundai_cp, clu11, button)
self.assertEqual(m_old, m)
if __name__ == "__main__":
unittest.main()
selfdrive/can/tests/test_packer_subaru.py
+37
View File
@@ -0,0 +1,37 @@
import unittest
import random
from selfdrive.can.tests.packer_old import CANPacker as CANPackerOld
from selfdrive.can.packer import CANPacker
import selfdrive.car.subaru.subarucan as subarucan
from selfdrive.car.subaru.values import CAR as subaru_car
class TestPackerMethods(unittest.TestCase):
def setUp(self):
self.subaru_cp_old = CANPackerOld("subaru_global_2017")
self.subaru_cp = CANPacker("subaru_global_2017")
def test_correctness(self):
# Test all cars' commands, randomize the params.
for _ in xrange(1000):
apply_steer = (random.randint(0, 2) % 2 == 0)
frame = random.randint(1, 65536)
steer_step = random.randint(1, 65536)
m_old = subarucan.create_steering_control(self.subaru_cp_old, subaru_car.IMPREZA, apply_steer, frame, steer_step)
m = subarucan.create_steering_control(self.subaru_cp, subaru_car.IMPREZA, apply_steer, frame, steer_step)
self.assertEqual(m_old, m)
m_old = subarucan.create_steering_status(self.subaru_cp_old, subaru_car.IMPREZA, apply_steer, frame, steer_step)
m = subarucan.create_steering_status(self.subaru_cp, subaru_car.IMPREZA, apply_steer, frame, steer_step)
self.assertEqual(m_old, m)
es_distance_msg = {}
pcm_cancel_cmd = (random.randint(0, 2) % 2 == 0)
m_old = subarucan.create_es_distance(self.subaru_cp_old, es_distance_msg, pcm_cancel_cmd)
m = subarucan.create_es_distance(self.subaru_cp, es_distance_msg, pcm_cancel_cmd)
self.assertEqual(m_old, m)
if __name__ == "__main__":
unittest.main()
selfdrive/can/tests/test_packer_toyota.py
+88
View File
@@ -0,0 +1,88 @@
import unittest
import random
from selfdrive.can.tests.packer_old import CANPacker as CANPackerOld
from selfdrive.can.packer import CANPacker
from selfdrive.car.toyota.toyotacan import (
create_ipas_steer_command, create_steer_command, create_accel_command,
create_fcw_command, create_ui_command
)
from common.realtime import sec_since_boot
class TestPackerMethods(unittest.TestCase):
def setUp(self):
self.cp_old = CANPackerOld("toyota_rav4_hybrid_2017_pt_generated")
self.cp = CANPacker("toyota_rav4_hybrid_2017_pt_generated")
def test_correctness(self):
# Test all commands, randomize the params.
for _ in xrange(1000):
# Toyota
steer = random.randint(-1, 1)
enabled = (random.randint(0, 2) % 2 == 0)
apgs_enabled = (random.randint(0, 2) % 2 == 0)
m_old = create_ipas_steer_command(self.cp_old, steer, enabled, apgs_enabled)
m = create_ipas_steer_command(self.cp, steer, enabled, apgs_enabled)
self.assertEqual(m_old, m)
steer = (random.randint(0, 2) % 2 == 0)
steer_req = (random.randint(0, 2) % 2 == 0)
raw_cnt = random.randint(1, 65536)
m_old = create_steer_command(self.cp_old, steer, steer_req, raw_cnt)
m = create_steer_command(self.cp, steer, steer_req, raw_cnt)
self.assertEqual(m_old, m)
accel = (random.randint(0, 2) % 2 == 0)
pcm_cancel = (random.randint(0, 2) % 2 == 0)
standstill_req = (random.randint(0, 2) % 2 == 0)
lead = (random.randint(0, 2) % 2 == 0)
m_old = create_accel_command(self.cp_old, accel, pcm_cancel, standstill_req, lead)
m = create_accel_command(self.cp, accel, pcm_cancel, standstill_req, lead)
self.assertEqual(m_old, m)
fcw = random.randint(1, 65536)
m_old = create_fcw_command(self.cp_old, fcw)
m = create_fcw_command(self.cp, fcw)
self.assertEqual(m_old, m)
steer = (random.randint(0, 2) % 2 == 0)
sound1 = random.randint(1, 65536)
sound2 = random.randint(1, 65536)
left_line = (random.randint(0, 2) % 2 == 0)
right_line = (random.randint(0, 2) % 2 == 0)
left_lane_depart = (random.randint(0, 2) % 2 == 0)
right_lane_depart = (random.randint(0, 2) % 2 == 0)
m_old = create_ui_command(self.cp_old, steer, sound1, sound2, left_line, right_line, left_lane_depart, right_lane_depart)
m = create_ui_command(self.cp, steer, sound1, sound2, left_line, right_line, left_lane_depart, right_lane_depart)
self.assertEqual(m_old, m)
def test_performance(self):
n1 = sec_since_boot()
recursions = 100000
steer = (random.randint(0, 2) % 2 == 0)
sound1 = random.randint(1, 65536)
sound2 = random.randint(1, 65536)
left_line = (random.randint(0, 2) % 2 == 0)
right_line = (random.randint(0, 2) % 2 == 0)
left_lane_depart = (random.randint(0, 2) % 2 == 0)
right_lane_depart = (random.randint(0, 2) % 2 == 0)
for _ in xrange(recursions):
create_ui_command(self.cp_old, steer, sound1, sound2, left_line, right_line, left_lane_depart, right_lane_depart)
n2 = sec_since_boot()
elapsed_old = n2 - n1
# print('Old API, elapsed time: {} secs'.format(elapsed_old))
n1 = sec_since_boot()
for _ in xrange(recursions):
create_ui_command(self.cp, steer, sound1, sound2, left_line, right_line, left_lane_depart, right_lane_depart)
n2 = sec_since_boot()
elapsed_new = n2 - n1
# print('New API, elapsed time: {} secs'.format(elapsed_new))
self.assertTrue(elapsed_new < elapsed_old / 2)
if __name__ == "__main__":
unittest.main()
selfdrive/car/chrysler/carcontroller.py
+1 -1
View File
@@ -97,4 +97,4 @@ class CarController(object):
self.ccframe += 1
self.prev_frame = frame
sendcan.send(can_list_to_can_capnp(can_sends, msgtype='sendcan').to_bytes())
sendcan.send(can_list_to_can_capnp(can_sends, msgtype='sendcan'))
selfdrive/car/chrysler/carstate.py
+5 -6
View File
@@ -1,7 +1,6 @@
from selfdrive.can.parser import CANParser
from selfdrive.car.chrysler.values import DBC, STEER_THRESHOLD
from common.kalman.simple_kalman import KF1D
import numpy as np
def parse_gear_shifter(can_gear):
@@ -90,10 +89,10 @@ class CarState(object):
dt = 0.01
# Q = np.matrix([[10.0, 0.0], [0.0, 100.0]])
# R = 1e3
self.v_ego_kf = KF1D(x0=np.matrix([[0.0], [0.0]]),
A=np.matrix([[1.0, dt], [0.0, 1.0]]),
C=np.matrix([1.0, 0.0]),
K=np.matrix([[0.12287673], [0.29666309]]))
self.v_ego_kf = KF1D(x0=[[0.0], [0.0]],
A=[[1.0, dt], [0.0, 1.0]],
C=[1.0, 0.0],
K=[[0.12287673], [0.29666309]])
self.v_ego = 0.0
@@ -127,7 +126,7 @@ class CarState(object):
# Kalman filter
if abs(v_wheel - self.v_ego) > 2.0: # Prevent large accelerations when car starts at non zero speed
self.v_ego_kf.x = np.matrix([[v_wheel], [0.0]])
self.v_ego_kf.x = [[v_wheel], [0.0]]
self.v_ego_raw = v_wheel
v_ego_x = self.v_ego_kf.update(v_wheel)
selfdrive/car/chrysler/interface.py
+9 -9
View File
@@ -70,12 +70,12 @@ class CarInterface(object):
tireStiffnessRear_civic = 90000 * 2.0
# Speed conversion: 20, 45 mph
ret.steerKpBP, ret.steerKiBP = [[9., 20.], [9., 20.]]
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.steerKpV, ret.steerKiV = [[0.15,0.30], [0.03,0.05]]
ret.steerKf = 0.00006 # full torque for 10 deg at 80mph means 0.00007818594
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
ret.steerActuatorDelay = 0.1
ret.steerRateCost = 0.7
@@ -86,8 +86,6 @@ class CarInterface(object):
ret.centerToFront = ret.wheelbase * 0.44
ret.longPidDeadzoneBP = [0., 9.]
ret.longPidDeadzoneV = [0., .15]
ret.minSteerSpeed = 3.8 # m/s
ret.minEnableSpeed = -1. # enable is done by stock ACC, so ignore this
@@ -129,10 +127,12 @@ class CarInterface(object):
ret.stoppingControl = False
ret.startAccel = 0.0
ret.longitudinalKpBP = [0., 5., 35.]
ret.longitudinalKpV = [3.6, 2.4, 1.5]
ret.longitudinalKiBP = [0., 35.]
ret.longitudinalKiV = [0.54, 0.36]
ret.longitudinalTuning.deadzoneBP = [0., 9.]
ret.longitudinalTuning.deadzoneV = [0., .15]
ret.longitudinalTuning.kpBP = [0., 5., 35.]
ret.longitudinalTuning.kpV = [3.6, 2.4, 1.5]
ret.longitudinalTuning.kiBP = [0., 35.]
ret.longitudinalTuning.kiV = [0.54, 0.36]
return ret
selfdrive/car/chrysler/values.py
+4
View File
@@ -34,6 +34,10 @@ FINGERPRINTS = {
# Based on 0607d2516fc2148f|2019-02-13--23-03-16
{
168: 8, 257: 5, 258: 8, 264: 8, 268: 8, 270: 8, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 291: 8, 292: 8, 294: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 344: 8, 368: 8, 376: 3, 384: 8, 388: 4, 448: 6, 456: 4, 464: 8, 469: 8, 480: 8, 500: 8, 501: 8, 512: 8, 514: 8, 520: 8, 528: 8, 532: 8, 544: 8, 557: 8, 559: 8, 560: 8, 564: 8, 571: 3, 579: 8, 584: 8, 608: 8, 624: 8, 625: 8, 632: 8, 639: 8, 653: 8, 654: 8, 655: 8, 658: 6, 660: 8, 669: 3, 671: 8, 672: 8, 678: 8, 680: 8, 701: 8, 703: 8, 704: 8, 705: 8, 706: 8, 709: 8, 710: 8, 719: 8, 720: 6, 729: 5, 736: 8, 737: 8, 746: 5, 752: 2, 754: 8, 760: 8, 764: 8, 766: 8, 770: 8, 773: 8, 779: 8, 782: 8, 784: 8, 792: 8, 799: 8, 800: 8, 804: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 832: 8, 838: 2, 848: 8, 853: 8, 856: 4, 860: 6, 863: 8, 878: 8, 882: 8, 897: 8, 906: 8, 908: 8, 924: 8, 926: 3, 929: 8, 937: 8, 938: 8, 939: 8, 940: 8, 941: 8, 942: 8, 943: 8, 947: 8, 948: 8, 958: 8, 959: 8, 962: 8, 969: 4, 973: 8, 974: 5, 979: 8, 980: 8, 981: 8, 982: 8, 983: 8, 984: 8, 992: 8, 993: 7, 995: 8, 996: 8, 1000: 8, 1001: 8, 1002: 8, 1003: 8, 1008: 8, 1009: 8, 1010: 8, 1011: 8, 1012: 8, 1013: 8, 1014: 8, 1015: 8, 1024: 8, 1025: 8, 1026: 8, 1031: 8, 1033: 8, 1050: 8, 1059: 8, 1082: 8, 1083: 8, 1098: 8, 1100: 8, 1537: 8
},
# Based on 3c7ce223e3571b54|2019-05-11--20-16-14
{
168: 8, 257: 5, 258: 8, 264: 8, 268: 8, 270: 8, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 291: 8, 292: 8, 294: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 344: 8, 368: 8, 376: 3, 384: 8, 388: 4, 448: 6, 456: 4, 464: 8, 469: 8, 480: 8, 500: 8, 501: 8, 512: 8, 514: 8, 520: 8, 528: 8, 532: 8, 544: 8, 557: 8, 559: 8, 560: 8, 564: 8, 571: 3, 579: 8, 584: 8, 608: 8, 624: 8, 625: 8, 632: 8, 639: 8, 653: 8, 654: 8, 655: 8, 658: 6, 660: 8, 669: 3, 671: 8, 672: 8, 678: 8, 680: 8, 701: 8, 703: 8, 704: 8, 705: 8, 706: 8, 709: 8, 710: 8, 719: 8, 720: 6, 729: 5, 736: 8, 737: 8, 746: 5, 752: 2, 754: 8, 760: 8, 764: 8, 766: 8, 770: 8, 773: 8, 779: 8, 782: 8, 784: 8, 792: 8, 799: 8, 800: 8, 804: 8, 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, 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, 1562: 8, 1570: 8
}
],
CAR.JEEP_CHEROKEE: [
selfdrive/car/ford/carstate.py
+5 -5
View File
@@ -46,10 +46,10 @@ class CarState(object):
dt = 0.01
# Q = np.matrix([[10.0, 0.0], [0.0, 100.0]])
# R = 1e3
self.v_ego_kf = KF1D(x0=np.matrix([[0.0], [0.0]]),
A=np.matrix([[1.0, dt], [0.0, 1.0]]),
C=np.matrix([1.0, 0.0]),
K=np.matrix([[0.12287673], [0.29666309]]))
self.v_ego_kf = KF1D(x0=[[0.0], [0.0]],
A=[[1.0, dt], [0.0, 1.0]],
C=[1.0, 0.0],
K=[[0.12287673], [0.29666309]])
self.v_ego = 0.0
def update(self, cp):
@@ -69,7 +69,7 @@ class CarState(object):
# Kalman filter
if abs(v_wheel - self.v_ego) > 2.0: # Prevent large accelerations when car starts at non zero speed
self.v_ego_kf.x = np.matrix([[v_wheel], [0.0]])
self.v_ego_kf.x = [[v_wheel], [0.0]]
self.v_ego_raw = v_wheel
v_ego_x = self.v_ego_kf.update(v_wheel)
selfdrive/car/ford/interface.py
+9 -9
View File
@@ -69,12 +69,12 @@ class CarInterface(object):
tireStiffnessFront_civic = 85400
tireStiffnessRear_civic = 90000
ret.steerKiBP, ret.steerKpBP = [[0.], [0.]]
ret.wheelbase = 2.85
ret.steerRatio = 14.8
ret.mass = 3045. * CV.LB_TO_KG + std_cargo
ret.steerKpV, ret.steerKiV = [[0.01], [0.005]] # TODO: tune this
ret.steerKf = 1. / MAX_ANGLE # MAX Steer angle to normalize FF
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
@@ -84,8 +84,6 @@ class CarInterface(object):
ret.centerToFront = ret.wheelbase * 0.44
ret.longPidDeadzoneBP = [0., 9.]
ret.longPidDeadzoneV = [0., .15]
# 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.
@@ -126,10 +124,12 @@ class CarInterface(object):
ret.stoppingControl = False
ret.startAccel = 0.0
ret.longitudinalKpBP = [0., 5., 35.]
ret.longitudinalKpV = [3.6, 2.4, 1.5]
ret.longitudinalKiBP = [0., 35.]
ret.longitudinalKiV = [0.54, 0.36]
ret.longitudinalTuning.deadzoneBP = [0., 9.]
ret.longitudinalTuning.deadzoneV = [0., .15]
ret.longitudinalTuning.kpBP = [0., 5., 35.]
ret.longitudinalTuning.kpV = [3.6, 2.4, 1.5]
ret.longitudinalTuning.kiBP = [0., 35.]
ret.longitudinalTuning.kiV = [0.54, 0.36]
return ret
selfdrive/car/gm/carcontroller.py
+1 -1
View File
@@ -195,4 +195,4 @@ class CarController(object):
# issued for the same chime type and duration
self.chime = chime
sendcan.send(can_list_to_can_capnp(can_sends, msgtype='sendcan').to_bytes())
sendcan.send(can_list_to_can_capnp(can_sends, msgtype='sendcan'))
selfdrive/car/gm/carstate.py
+4 -4
View File
@@ -63,10 +63,10 @@ class CarState(object):
# vEgo kalman filter
dt = 0.01
self.v_ego_kf = KF1D(x0=np.matrix([[0.], [0.]]),
A=np.matrix([[1., dt], [0., 1.]]),
C=np.matrix([1., 0.]),
K=np.matrix([[0.12287673], [0.29666309]]))
self.v_ego_kf = KF1D(x0=[[0.], [0.]],
A=[[1., dt], [0., 1.]],
C=[1., 0.],
K=[[0.12287673], [0.29666309]])
self.v_ego = 0.
def update(self, pt_cp):
selfdrive/car/gm/interface.py
+15 -13
View File
@@ -4,7 +4,8 @@ from common.realtime import sec_since_boot
from selfdrive.config import Conversions as CV
from selfdrive.controls.lib.drive_helpers import create_event, EventTypes as ET
from selfdrive.controls.lib.vehicle_model import VehicleModel
from selfdrive.car.gm.values import DBC, CAR, STOCK_CONTROL_MSGS, AUDIO_HUD, SUPERCRUISE_CARS
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
try:
@@ -162,25 +163,24 @@ class CarInterface(object):
ret.mass / mass_civic * \
(ret.centerToFront / ret.wheelbase) / (centerToFront_civic / wheelbase_civic)
# same tuning for Volt and CT6 for now
ret.steerKiBP, ret.steerKpBP = [[0.], [0.]]
ret.steerKpV, ret.steerKiV = [[0.2], [0.00]]
ret.steerKf = 0.00004 # full torque for 20 deg at 80mph means 0.00007818594
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]]
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.2], [0.00]]
ret.lateralTuning.pid.kf = 0.00004 # full torque for 20 deg at 80mph means 0.00007818594
ret.steerMaxBP = [0.] # m/s
ret.steerMaxBP = [0.] # m/s
ret.steerMaxV = [1.]
ret.gasMaxBP = [0.]
ret.gasMaxV = [.5]
ret.brakeMaxBP = [0.]
ret.brakeMaxV = [1.]
ret.longPidDeadzoneBP = [0.]
ret.longPidDeadzoneV = [0.]
ret.longitudinalKpBP = [5., 35.]
ret.longitudinalKpV = [2.4, 1.5]
ret.longitudinalKiBP = [0.]
ret.longitudinalKiV = [0.36]
ret.longitudinalTuning.kpBP = [5., 35.]
ret.longitudinalTuning.kpV = [2.4, 1.5]
ret.longitudinalTuning.kiBP = [0.]
ret.longitudinalTuning.kiV = [0.36]
ret.longitudinalTuning.deadzoneBP = [0.]
ret.longitudinalTuning.deadzoneV = [0.]
ret.steerLimitAlert = True
@@ -231,7 +231,7 @@ class CarInterface(object):
# cruise state
ret.cruiseState.available = bool(self.CS.main_on)
cruiseEnabled = self.CS.pcm_acc_status != 0
cruiseEnabled = self.CS.pcm_acc_status != AccState.OFF
ret.cruiseState.enabled = cruiseEnabled
ret.cruiseState.standstill = self.CS.pcm_acc_status == 4
@@ -323,6 +323,8 @@ class CarInterface(object):
events.append(create_event('pedalPressed', [ET.PRE_ENABLE]))
if ret.cruiseState.standstill:
events.append(create_event('resumeRequired', [ET.WARNING]))
if self.CS.pcm_acc_status == AccState.FAULTED:
events.append(create_event('controlsFailed', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
# handle button presses
for b in ret.buttonEvents:
selfdrive/car/gm/values.py
+6
View File
@@ -30,6 +30,12 @@ class CM:
LOW_CHIME = 0x86
HIGH_CHIME = 0x87
class AccState:
OFF = 0
ACTIVE = 1
FAULTED = 3
STANDSTILL = 4
AUDIO_HUD = {
AudibleAlert.none: (0, 0),
AudibleAlert.chimeEngage: (CM.HIGH_CHIME, 1),
selfdrive/car/honda/carcontroller.py
+1 -1
View File
@@ -184,4 +184,4 @@ class CarController(object):
# This prevents unexpected pedal range rescaling
can_sends.append(create_gas_command(self.packer, apply_gas, idx))
sendcan.send(can_list_to_can_capnp(can_sends, msgtype='sendcan').to_bytes())
sendcan.send(can_list_to_can_capnp(can_sends, msgtype='sendcan'))
selfdrive/car/honda/carstate.py
+1 -1
View File
@@ -192,7 +192,7 @@ class CarState(object):
# R = 1e3
self.v_ego_kf = KF1D(x0=[[0.0], [0.0]],
A=[[1.0, dt], [0.0, 1.0]],
C=[[1.0, 0.0]],
C=[1.0, 0.0],
K=[[0.12287673], [0.29666309]])
self.v_ego = 0.0
selfdrive/car/honda/interface.py
+60 -61
View File
@@ -183,9 +183,8 @@ class CarInterface(object):
# Tire stiffness factor fictitiously lower if it includes the steering column torsion effect.
# For modeling details, see p.198-200 in "The Science of Vehicle Dynamics (2014), M. Guiggiani"
ret.steerKiBP, ret.steerKpBP = [[0.], [0.]]
ret.steerKf = 0.00006 # conservative feed-forward
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]]
ret.lateralTuning.pid.kf = 0.00006 # conservative feed-forward
if candidate in [CAR.CIVIC, CAR.CIVIC_BOSCH]:
stop_and_go = True
@@ -197,13 +196,13 @@ class CarInterface(object):
# Civic at comma has modified steering FW, so different tuning for the Neo in that car
is_fw_modified = os.getenv("DONGLE_ID") in ['99c94dc769b5d96e']
if is_fw_modified:
ret.steerKf = 0.00004
ret.lateralTuning.pid.kf = 0.00004
ret.steerKpV, ret.steerKiV = [[0.4], [0.12]] if is_fw_modified else [[0.8], [0.24]]
ret.longitudinalKpBP = [0., 5., 35.]
ret.longitudinalKpV = [3.6, 2.4, 1.5]
ret.longitudinalKiBP = [0., 35.]
ret.longitudinalKiV = [0.54, 0.36]
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.4], [0.12]] if is_fw_modified else [[0.8], [0.24]]
ret.longitudinalTuning.kpBP = [0., 5., 35.]
ret.longitudinalTuning.kpV = [3.6, 2.4, 1.5]
ret.longitudinalTuning.kiBP = [0., 35.]
ret.longitudinalTuning.kiV = [0.54, 0.36]
elif candidate in (CAR.ACCORD, CAR.ACCORD_15, CAR.ACCORDH):
stop_and_go = True
@@ -214,11 +213,11 @@ class CarInterface(object):
ret.centerToFront = ret.wheelbase * 0.39
ret.steerRatio = 15.96 # 11.82 is spec end-to-end
tire_stiffness_factor = 0.8467
ret.steerKpV, ret.steerKiV = [[0.6], [0.18]]
ret.longitudinalKpBP = [0., 5., 35.]
ret.longitudinalKpV = [1.2, 0.8, 0.5]
ret.longitudinalKiBP = [0., 35.]
ret.longitudinalKiV = [0.18, 0.12]
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.18]]
ret.longitudinalTuning.kpBP = [0., 5., 35.]
ret.longitudinalTuning.kpV = [1.2, 0.8, 0.5]
ret.longitudinalTuning.kiBP = [0., 35.]
ret.longitudinalTuning.kiV = [0.18, 0.12]
elif candidate == CAR.ACURA_ILX:
stop_and_go = False
@@ -227,11 +226,11 @@ class CarInterface(object):
ret.centerToFront = ret.wheelbase * 0.37
ret.steerRatio = 18.61 # 15.3 is spec end-to-end
tire_stiffness_factor = 0.72
ret.steerKpV, ret.steerKiV = [[0.8], [0.24]]
ret.longitudinalKpBP = [0., 5., 35.]
ret.longitudinalKpV = [1.2, 0.8, 0.5]
ret.longitudinalKiBP = [0., 35.]
ret.longitudinalKiV = [0.18, 0.12]
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.8], [0.24]]
ret.longitudinalTuning.kpBP = [0., 5., 35.]
ret.longitudinalTuning.kpV = [1.2, 0.8, 0.5]
ret.longitudinalTuning.kiBP = [0., 35.]
ret.longitudinalTuning.kiV = [0.18, 0.12]
elif candidate == CAR.CRV:
stop_and_go = False
@@ -240,11 +239,11 @@ class CarInterface(object):
ret.centerToFront = ret.wheelbase * 0.41
ret.steerRatio = 15.3 # as spec
tire_stiffness_factor = 0.444 # not optimized yet
ret.steerKpV, ret.steerKiV = [[0.8], [0.24]]
ret.longitudinalKpBP = [0., 5., 35.]
ret.longitudinalKpV = [1.2, 0.8, 0.5]
ret.longitudinalKiBP = [0., 35.]
ret.longitudinalKiV = [0.18, 0.12]
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.8], [0.24]]
ret.longitudinalTuning.kpBP = [0., 5., 35.]
ret.longitudinalTuning.kpV = [1.2, 0.8, 0.5]
ret.longitudinalTuning.kiBP = [0., 35.]
ret.longitudinalTuning.kiV = [0.18, 0.12]
elif candidate == CAR.CRV_5G:
stop_and_go = True
@@ -254,11 +253,11 @@ class CarInterface(object):
ret.centerToFront = ret.wheelbase * 0.41
ret.steerRatio = 16.0 # 12.3 is spec end-to-end
tire_stiffness_factor = 0.677
ret.steerKpV, ret.steerKiV = [[0.6], [0.18]]
ret.longitudinalKpBP = [0., 5., 35.]
ret.longitudinalKpV = [1.2, 0.8, 0.5]
ret.longitudinalKiBP = [0., 35.]
ret.longitudinalKiV = [0.18, 0.12]
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.18]]
ret.longitudinalTuning.kpBP = [0., 5., 35.]
ret.longitudinalTuning.kpV = [1.2, 0.8, 0.5]
ret.longitudinalTuning.kiBP = [0., 35.]
ret.longitudinalTuning.kiV = [0.18, 0.12]
elif candidate == CAR.CRV_HYBRID:
stop_and_go = True
@@ -268,11 +267,11 @@ class CarInterface(object):
ret.centerToFront = ret.wheelbase * 0.41
ret.steerRatio = 16.0 # 12.3 is spec end-to-end
tire_stiffness_factor = 0.677
ret.steerKpV, ret.steerKiV = [[0.6], [0.18]]
ret.longitudinalKpBP = [0., 5., 35.]
ret.longitudinalKpV = [1.2, 0.8, 0.5]
ret.longitudinalKiBP = [0., 35.]
ret.longitudinalKiV = [0.18, 0.12]
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.18]]
ret.longitudinalTuning.kpBP = [0., 5., 35.]
ret.longitudinalTuning.kpV = [1.2, 0.8, 0.5]
ret.longitudinalTuning.kiBP = [0., 35.]
ret.longitudinalTuning.kiV = [0.18, 0.12]
elif candidate == CAR.ACURA_RDX:
stop_and_go = False
@@ -281,11 +280,11 @@ class CarInterface(object):
ret.centerToFront = ret.wheelbase * 0.38
ret.steerRatio = 15.0 # as spec
tire_stiffness_factor = 0.444 # not optimized yet
ret.steerKpV, ret.steerKiV = [[0.8], [0.24]]
ret.longitudinalKpBP = [0., 5., 35.]
ret.longitudinalKpV = [1.2, 0.8, 0.5]
ret.longitudinalKiBP = [0., 35.]
ret.longitudinalKiV = [0.18, 0.12]
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.8], [0.24]]
ret.longitudinalTuning.kpBP = [0., 5., 35.]
ret.longitudinalTuning.kpV = [1.2, 0.8, 0.5]
ret.longitudinalTuning.kiBP = [0., 35.]
ret.longitudinalTuning.kiV = [0.18, 0.12]
elif candidate == CAR.ODYSSEY:
stop_and_go = False
@@ -294,11 +293,11 @@ class CarInterface(object):
ret.centerToFront = ret.wheelbase * 0.41
ret.steerRatio = 14.35 # as spec
tire_stiffness_factor = 0.82
ret.steerKpV, ret.steerKiV = [[0.45], [0.135]]
ret.longitudinalKpBP = [0., 5., 35.]
ret.longitudinalKpV = [1.2, 0.8, 0.5]
ret.longitudinalKiBP = [0., 35.]
ret.longitudinalKiV = [0.18, 0.12]
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.45], [0.135]]
ret.longitudinalTuning.kpBP = [0., 5., 35.]
ret.longitudinalTuning.kpV = [1.2, 0.8, 0.5]
ret.longitudinalTuning.kiBP = [0., 35.]
ret.longitudinalTuning.kiV = [0.18, 0.12]
elif candidate == CAR.ODYSSEY_CHN:
stop_and_go = False
@@ -307,11 +306,11 @@ class CarInterface(object):
ret.centerToFront = ret.wheelbase * 0.41 # from CAR.ODYSSEY
ret.steerRatio = 14.35 # from CAR.ODYSSEY
tire_stiffness_factor = 0.82 # from CAR.ODYSSEY
ret.steerKpV, ret.steerKiV = [[0.45], [0.135]]
ret.longitudinalKpBP = [0., 5., 35.]
ret.longitudinalKpV = [1.2, 0.8, 0.5]
ret.longitudinalKiBP = [0., 35.]
ret.longitudinalKiV = [0.18, 0.12]
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.45], [0.135]]
ret.longitudinalTuning.kpBP = [0., 5., 35.]
ret.longitudinalTuning.kpV = [1.2, 0.8, 0.5]
ret.longitudinalTuning.kiBP = [0., 35.]
ret.longitudinalTuning.kiV = [0.18, 0.12]
elif candidate in (CAR.PILOT, CAR.PILOT_2019):
stop_and_go = False
@@ -320,11 +319,11 @@ class CarInterface(object):
ret.centerToFront = ret.wheelbase * 0.41
ret.steerRatio = 16.0 # as spec
tire_stiffness_factor = 0.444 # not optimized yet
ret.steerKpV, ret.steerKiV = [[0.38], [0.11]]
ret.longitudinalKpBP = [0., 5., 35.]
ret.longitudinalKpV = [1.2, 0.8, 0.5]
ret.longitudinalKiBP = [0., 35.]
ret.longitudinalKiV = [0.18, 0.12]
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.38], [0.11]]
ret.longitudinalTuning.kpBP = [0., 5., 35.]
ret.longitudinalTuning.kpV = [1.2, 0.8, 0.5]
ret.longitudinalTuning.kiBP = [0., 35.]
ret.longitudinalTuning.kiV = [0.18, 0.12]
elif candidate == CAR.RIDGELINE:
stop_and_go = False
@@ -333,11 +332,11 @@ class CarInterface(object):
ret.centerToFront = ret.wheelbase * 0.41
ret.steerRatio = 15.59 # as spec
tire_stiffness_factor = 0.444 # not optimized yet
ret.steerKpV, ret.steerKiV = [[0.38], [0.11]]
ret.longitudinalKpBP = [0., 5., 35.]
ret.longitudinalKpV = [1.2, 0.8, 0.5]
ret.longitudinalKiBP = [0., 35.]
ret.longitudinalKiV = [0.18, 0.12]
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.38], [0.11]]
ret.longitudinalTuning.kpBP = [0., 5., 35.]
ret.longitudinalTuning.kpV = [1.2, 0.8, 0.5]
ret.longitudinalTuning.kiBP = [0., 35.]
ret.longitudinalTuning.kiV = [0.18, 0.12]
else:
raise ValueError("unsupported car %s" % candidate)
@@ -376,8 +375,8 @@ class CarInterface(object):
ret.brakeMaxBP = [5., 20.] # m/s
ret.brakeMaxV = [1., 0.8] # max brake allowed
ret.longPidDeadzoneBP = [0.]
ret.longPidDeadzoneV = [0.]
ret.longitudinalTuning.deadzoneBP = [0.]
ret.longitudinalTuning.deadzoneV = [0.]
ret.stoppingControl = True
ret.steerLimitAlert = True
selfdrive/car/hyundai/carcontroller.py
+1 -1
View File
@@ -71,6 +71,6 @@ class CarController(object):
can_sends.append(create_clu11(self.packer, CS.clu11, Buttons.RES_ACCEL))
### Send messages to canbus
sendcan.send(can_list_to_can_capnp(can_sends, msgtype='sendcan').to_bytes())
sendcan.send(can_list_to_can_capnp(can_sends, msgtype='sendcan'))
self.cnt += 1
selfdrive/car/hyundai/carstate.py
+5 -6
View File
@@ -2,7 +2,6 @@ from selfdrive.car.hyundai.values import DBC, STEER_THRESHOLD
from selfdrive.can.parser import CANParser
from selfdrive.config import Conversions as CV
from common.kalman.simple_kalman import KF1D
import numpy as np
def get_can_parser(CP):
@@ -133,10 +132,10 @@ class CarState(object):
dt = 0.01
# Q = np.matrix([[10.0, 0.0], [0.0, 100.0]])
# R = 1e3
self.v_ego_kf = KF1D(x0=np.matrix([[0.0], [0.0]]),
A=np.matrix([[1.0, dt], [0.0, 1.0]]),
C=np.matrix([1.0, 0.0]),
K=np.matrix([[0.12287673], [0.29666309]]))
self.v_ego_kf = KF1D(x0=[[0.0], [0.0]],
A=[[1.0, dt], [0.0, 1.0]],
C=[1.0, 0.0],
K=[[0.12287673], [0.29666309]])
self.v_ego = 0.0
self.left_blinker_on = 0
self.left_blinker_flash = 0
@@ -173,7 +172,7 @@ class CarState(object):
# Kalman filter, even though Hyundai raw wheel speed is heaviliy filtered by default
if abs(v_wheel - self.v_ego) > 2.0: # Prevent large accelerations when car starts at non zero speed
self.v_ego_kf.x = np.matrix([[v_wheel], [0.0]])
self.v_ego_kf.x = [[v_wheel], [0.0]]
self.v_ego_raw = v_wheel
v_ego_x = self.v_ego_kf.update(v_wheel)
selfdrive/car/hyundai/interface.py
+24 -24
View File
@@ -74,7 +74,7 @@ class CarInterface(object):
ret.steerRateCost = 0.5
if candidate == CAR.SANTA_FE:
ret.steerKf = 0.00005
ret.lateralTuning.pid.kf = 0.00005
ret.mass = 3982 * CV.LB_TO_KG + std_cargo
ret.wheelbase = 2.766
@@ -82,56 +82,58 @@ class CarInterface(object):
ret.steerRatio = 16.55 # 13.8 is spec end-to-end
tire_stiffness_factor = 0.82
ret.steerKiBP, ret.steerKpBP = [[9., 22.], [9., 22.]]
ret.steerKpV, ret.steerKiV = [[0.2, 0.35], [0.05, 0.09]]
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[9., 22.], [9., 22.]]
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.2, 0.35], [0.05, 0.09]]
ret.minSteerSpeed = 0.
elif candidate == CAR.KIA_SORENTO:
ret.steerKf = 0.00005
ret.lateralTuning.pid.kf = 0.00005
ret.mass = 1985 + std_cargo
ret.wheelbase = 2.78
ret.steerRatio = 14.4 * 1.1 # 10% higher at the center seems reasonable
ret.steerKiBP, ret.steerKpBP = [[0.], [0.]]
ret.steerKpV, ret.steerKiV = [[0.25], [0.05]]
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]]
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25], [0.05]]
ret.minSteerSpeed = 0.
elif candidate == CAR.ELANTRA:
ret.steerKf = 0.00006
ret.lateralTuning.pid.kf = 0.00006
ret.mass = 1275 + std_cargo
ret.wheelbase = 2.7
ret.steerRatio = 13.73 #Spec
tire_stiffness_factor = 0.385
ret.steerKiBP, ret.steerKpBP = [[0.], [0.]]
ret.steerKpV, ret.steerKiV = [[0.25], [0.05]]
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]]
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25], [0.05]]
ret.minSteerSpeed = 32 * CV.MPH_TO_MS
elif candidate == CAR.GENESIS:
ret.steerKf = 0.00005
ret.lateralTuning.pid.kf = 0.00005
ret.mass = 2060 + std_cargo
ret.wheelbase = 3.01
ret.steerRatio = 16.5
ret.steerKiBP, ret.steerKpBP = [[0.], [0.]]
ret.steerKpV, ret.steerKiV = [[0.16], [0.01]]
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]]
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.16], [0.01]]
ret.minSteerSpeed = 35 * CV.MPH_TO_MS
elif candidate == CAR.KIA_OPTIMA:
ret.steerKf = 0.00005
ret.lateralTuning.pid.kf = 0.00005
ret.mass = 3558 * CV.LB_TO_KG
ret.wheelbase = 2.80
ret.steerRatio = 13.75
tire_stiffness_factor = 0.5
ret.steerKiBP, ret.steerKpBP = [[0.], [0.]]
ret.steerKpV, ret.steerKiV = [[0.25], [0.05]]
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]]
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25], [0.05]]
elif candidate == CAR.KIA_STINGER:
ret.steerKf = 0.00005
ret.lateralTuning.pid.kf = 0.00005
ret.mass = 1825 + std_cargo
ret.wheelbase = 2.78
ret.steerRatio = 14.4 * 1.15 # 15% higher at the center seems reasonable
ret.steerKiBP, ret.steerKpBP = [[0.], [0.]]
ret.steerKpV, ret.steerKiV = [[0.25], [0.05]]
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]]
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25], [0.05]]
ret.minSteerSpeed = 0.
ret.minEnableSpeed = -1. # enable is done by stock ACC, so ignore this
ret.longitudinalKpBP = [0.]
ret.longitudinalKpV = [0.]
ret.longitudinalKiBP = [0.]
ret.longitudinalKiV = [0.]
ret.longitudinalTuning.kpBP = [0.]
ret.longitudinalTuning.kpV = [0.]
ret.longitudinalTuning.kiBP = [0.]
ret.longitudinalTuning.kiV = [0.]
ret.longitudinalTuning.deadzoneBP = [0.]
ret.longitudinalTuning.deadzoneV = [0.]
ret.centerToFront = ret.wheelbase * 0.4
@@ -163,8 +165,6 @@ class CarInterface(object):
ret.gasMaxV = [1.]
ret.brakeMaxBP = [0.]
ret.brakeMaxV = [1.]
ret.longPidDeadzoneBP = [0.]
ret.longPidDeadzoneV = [0.]
ret.enableCamera = not any(x for x in CAMERA_MSGS if x in fingerprint)
ret.openpilotLongitudinalControl = False
selfdrive/car/mock/interface.py
+6 -6
View File
@@ -56,8 +56,6 @@ class CarInterface(object):
ret.wheelbase = 2.70
ret.centerToFront = ret.wheelbase * 0.5
ret.steerRatio = 13. # reasonable
ret.longPidDeadzoneBP = [0.]
ret.longPidDeadzoneV = [0.]
ret.tireStiffnessFront = 1e6 # very stiff to neglect slip
ret.tireStiffnessRear = 1e6 # very stiff to neglect slip
ret.steerRatioRear = 0.
@@ -69,10 +67,12 @@ class CarInterface(object):
ret.brakeMaxBP = [0.]
ret.brakeMaxV = [0.]
ret.longitudinalKpBP = [0.]
ret.longitudinalKpV = [0.]
ret.longitudinalKiBP = [0.]
ret.longitudinalKiV = [0.]
ret.longitudinalTuning.kpBP = [0.]
ret.longitudinalTuning.kpV = [0.]
ret.longitudinalTuning.kiBP = [0.]
ret.longitudinalTuning.kiV = [0.]
ret.longitudinalTuning.deadzoneBP = [0.]
ret.longitudinalTuning.deadzoneV = [0.]
ret.steerActuatorDelay = 0.
return ret
selfdrive/car/subaru/carcontroller.py
+1 -1
View File
@@ -73,4 +73,4 @@ class CarController(object):
can_sends.append(subarucan.create_es_lkas(self.packer, CS.es_lkas_msg, visual_alert))
self.es_lkas_cnt = CS.es_lkas_msg["Counter"]
sendcan.send(can_list_to_can_capnp(can_sends, msgtype='sendcan').to_bytes())
sendcan.send(can_list_to_can_capnp(can_sends, msgtype='sendcan'))
selfdrive/car/subaru/carstate.py
+14 -7
View File
@@ -1,5 +1,4 @@
import copy
import numpy as np
from common.kalman.simple_kalman import KF1D
from selfdrive.config import Conversions as CV
from selfdrive.can.parser import CANParser
@@ -26,6 +25,7 @@ def get_powertrain_can_parser(CP):
("DOOR_OPEN_FL", "BodyInfo", 1),
("DOOR_OPEN_RR", "BodyInfo", 1),
("DOOR_OPEN_RL", "BodyInfo", 1),
("Units", "Dash_State", 1),
]
checks = [
@@ -61,6 +61,10 @@ def get_camera_can_parser(CP):
("Signal3", "ES_LKAS_State", 0),
("LKAS_ENABLE_2", "ES_LKAS_State", 0),
("Signal4", "ES_LKAS_State", 0),
("LKAS_Left_Line_Visible", "ES_LKAS_State", 0),
("Signal6", "ES_LKAS_State", 0),
("LKAS_Right_Line_Visible", "ES_LKAS_State", 0),
("Signal7", "ES_LKAS_State", 0),
("FCW_Cont_Beep", "ES_LKAS_State", 0),
("FCW_Repeated_Beep", "ES_LKAS_State", 0),
("Throttle_Management_Activated", "ES_LKAS_State", 0),
@@ -92,10 +96,10 @@ class CarState(object):
# vEgo kalman filter
dt = 0.01
self.v_ego_kf = KF1D(x0=np.matrix([[0.], [0.]]),
A=np.matrix([[1., dt], [0., 1.]]),
C=np.matrix([1., 0.]),
K=np.matrix([[0.12287673], [0.29666309]]))
self.v_ego_kf = KF1D(x0=[[0.], [0.]],
A=[[1., dt], [0., 1.]],
C=[1., 0.],
K=[[0.12287673], [0.29666309]])
self.v_ego = 0.
def update(self, cp, cp_cam):
@@ -114,12 +118,15 @@ class CarState(object):
self.v_wheel_rl = cp.vl["Wheel_Speeds"]['RL'] * CV.KPH_TO_MS
self.v_wheel_rr = cp.vl["Wheel_Speeds"]['RR'] * CV.KPH_TO_MS
self.v_cruise_pcm = cp_cam.vl["ES_DashStatus"]["Cruise_Set_Speed"] * CV.MPH_TO_KPH
self.v_cruise_pcm = cp_cam.vl["ES_DashStatus"]['Cruise_Set_Speed']
# 1 = imperial, 6 = metric
if cp.vl["Dash_State"]['Units'] == 1:
self.v_cruise_pcm *= CV.MPH_TO_KPH
v_wheel = (self.v_wheel_fl + self.v_wheel_fr + self.v_wheel_rl + self.v_wheel_rr) / 4.
# Kalman filter, even though Hyundai raw wheel speed is heaviliy filtered by default
if abs(v_wheel - self.v_ego) > 2.0: # Prevent large accelerations when car starts at non zero speed
self.v_ego_kf.x = np.matrix([[v_wheel], [0.0]])
self.v_ego_kf.x = [[v_wheel], [0.0]]
self.v_ego_raw = v_wheel
v_ego_x = self.v_ego_kf.update(v_wheel)
selfdrive/car/subaru/interface.py
+9 -9
View File
@@ -66,9 +66,9 @@ class CarInterface(object):
ret.steerRatio = 15
tire_stiffness_factor = 1.0
ret.steerActuatorDelay = 0.4 # end-to-end angle controller
ret.steerKf = 0.00005
ret.steerKiBP, ret.steerKpBP = [[0., 20.], [0., 20.]]
ret.steerKpV, ret.steerKiV = [[0.2, 0.3], [0.02, 0.03]]
ret.lateralTuning.pid.kf = 0.00005
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0., 20.], [0., 20.]]
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.2, 0.3], [0.02, 0.03]]
ret.steerMaxBP = [0.] # m/s
ret.steerMaxV = [1.]
@@ -81,12 +81,12 @@ class CarInterface(object):
ret.gasMaxV = [0.]
ret.brakeMaxBP = [0.]
ret.brakeMaxV = [0.]
ret.longPidDeadzoneBP = [0.]
ret.longPidDeadzoneV = [0.]
ret.longitudinalKpBP = [0.]
ret.longitudinalKpV = [0.]
ret.longitudinalKiBP = [0.]
ret.longitudinalKiV = [0.]
ret.longitudinalTuning.deadzoneBP = [0.]
ret.longitudinalTuning.deadzoneV = [0.]
ret.longitudinalTuning.kpBP = [0.]
ret.longitudinalTuning.kpV = [0.]
ret.longitudinalTuning.kiBP = [0.]
ret.longitudinalTuning.kiV = [0.]
# end from gm
selfdrive/car/subaru/values.py
+4
View File
@@ -6,6 +6,10 @@ class CAR:
FINGERPRINTS = {
CAR.IMPREZA: [{
2: 8, 64: 8, 65: 8, 72: 8, 73: 8, 280: 8, 281: 8, 290: 8, 312: 8, 313: 8, 314: 8, 315: 8, 316: 8, 326: 8, 544: 8, 545: 8, 546: 8, 552: 8, 554: 8, 557: 8, 576: 8, 577: 8, 722: 8, 801: 8, 802: 8, 805: 8, 808: 8, 816: 8, 826: 8, 837: 8, 838: 8, 839: 8, 842: 8, 912: 8, 915: 8, 940: 8, 1614: 8, 1617: 8, 1632: 8, 1650: 8, 1657: 8, 1658: 8, 1677: 8, 1697: 8, 1722: 8, 1743: 8, 1759: 8, 1786: 5, 1787: 5, 1788: 8, 1809: 8, 1813: 8, 1817: 8, 1821: 8, 1840: 8, 1848: 8, 1924: 8, 1932: 8, 1952: 8, 1960: 8
},
# Crosstrek 2018 (same platform as Subaru)
{
2: 8, 64: 8, 65: 8, 72: 8, 73: 8, 256: 8, 280: 8, 281: 8, 290: 8, 312: 8, 313: 8, 314: 8, 315: 8, 316: 8, 326: 8, 372: 8, 544: 8, 545: 8, 546: 8, 554: 8, 557: 8, 576: 8, 577: 8, 722: 8, 801: 8, 802: 8, 805: 8, 808: 8, 811: 8, 826: 8, 837: 8, 838: 8, 839: 8, 842: 8, 912: 8, 915: 8, 940: 8, 1614: 8, 1617: 8, 1632: 8, 1650: 8, 1657: 8, 1658: 8, 1677: 8, 1697: 8, 1759: 8, 1786: 5, 1787: 5, 1788: 8
}],
}
selfdrive/car/toyota/carcontroller.py
+3 -3
View File
@@ -7,7 +7,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
from selfdrive.car.toyota.values import ECU, STATIC_MSGS, TSSP2_CAR
from selfdrive.can.packer import CANPacker
VisualAlert = car.CarControl.HUDControl.VisualAlert
@@ -249,7 +249,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:
if frame % 100 == 0 and ECU.DSU in self.fake_ecus and self.car_fingerprint not in TSSP2_CAR:
can_sends.append(create_fcw_command(self.packer, fcw))
#*** static msgs ***
@@ -271,4 +271,4 @@ class CarController(object):
can_sends.append(make_can_msg(addr, vl, bus, False))
sendcan.send(can_list_to_can_capnp(can_sends, msgtype='sendcan').to_bytes())
sendcan.send(can_list_to_can_capnp(can_sends, msgtype='sendcan'))
selfdrive/car/toyota/carstate.py
+5 -5
View File
@@ -97,10 +97,10 @@ class CarState(object):
dt = 0.01
# Q = np.matrix([[10.0, 0.0], [0.0, 100.0]])
# R = 1e3
self.v_ego_kf = KF1D(x0=np.matrix([[0.0], [0.0]]),
A=np.matrix([[1.0, dt], [0.0, 1.0]]),
C=np.matrix([1.0, 0.0]),
K=np.matrix([[0.12287673], [0.29666309]]))
self.v_ego_kf = KF1D(x0=[[0.0], [0.0]],
A=[[1.0, dt], [0.0, 1.0]],
C=[1.0, 0.0],
K=[[0.12287673], [0.29666309]])
self.v_ego = 0.0
def update(self, cp, cp_cam):
@@ -133,7 +133,7 @@ class CarState(object):
# Kalman filter
if abs(v_wheel - self.v_ego) > 2.0: # Prevent large accelerations when car starts at non zero speed
self.v_ego_kf.x = np.matrix([[v_wheel], [0.0]])
self.v_ego_kf.x = [[v_wheel], [0.0]]
self.v_ego_raw = v_wheel
v_ego_x = self.v_ego_kf.update(v_wheel)
selfdrive/car/toyota/interface.py
+66 -29
View File
@@ -5,7 +5,7 @@ from selfdrive.config import Conversions as CV
from selfdrive.controls.lib.drive_helpers import EventTypes as ET, create_event
from selfdrive.controls.lib.vehicle_model import VehicleModel
from selfdrive.car.toyota.carstate import CarState, get_can_parser, get_cam_can_parser
from selfdrive.car.toyota.values import ECU, check_ecu_msgs, CAR
from selfdrive.car.toyota.values import ECU, check_ecu_msgs, CAR, NO_STOP_TIMER_CAR
from selfdrive.swaglog import cloudlog
try:
@@ -73,8 +73,10 @@ class CarInterface(object):
tireStiffnessFront_civic = 192150
tireStiffnessRear_civic = 202500
ret.steerKiBP, ret.steerKpBP = [[0.], [0.]]
ret.steerActuatorDelay = 0.12 # Default delay, Prius has larger delay
if candidate != CAR.PRIUS:
ret.lateralTuning.init('pid')
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]]
if candidate == CAR.PRIUS:
stop_and_go = True
@@ -83,10 +85,15 @@ class CarInterface(object):
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.steerKpV, ret.steerKiV = [[0.4], [0.01]]
ret.steerKf = 0.00006 # full torque for 10 deg at 80mph means 0.00007818594
# TODO: Prius seem to have very laggy actuators. Understand if it is lag or hysteresis
ret.steerActuatorDelay = 0.25
ret.lateralTuning.init('indi')
ret.lateralTuning.indi.innerLoopGain = 4.0
ret.lateralTuning.indi.outerLoopGain = 3.0
ret.lateralTuning.indi.timeConstant = 1.0
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
@@ -95,8 +102,8 @@ class CarInterface(object):
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.steerKpV, ret.steerKiV = [[0.6], [0.05]]
ret.steerKf = 0.00006 # full torque for 10 deg at 80mph means 0.00007818594
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
@@ -105,8 +112,8 @@ class CarInterface(object):
ret.steerRatio = 17.8
tire_stiffness_factor = 0.444
ret.mass = 2860 * CV.LB_TO_KG + std_cargo # mean between normal and hybrid
ret.steerKpV, ret.steerKiV = [[0.2], [0.05]]
ret.steerKf = 0.00003 # full torque for 20 deg at 80mph means 0.00007818594
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
@@ -115,8 +122,8 @@ class CarInterface(object):
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.steerKpV, ret.steerKiV = [[0.6], [0.1]]
ret.steerKf = 0.00006 # full torque for 10 deg at 80mph means 0.00007818594
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
@@ -125,8 +132,8 @@ class CarInterface(object):
ret.steerRatio = 13.6
tire_stiffness_factor = 0.7933
ret.mass = 3300. * CV.LB_TO_KG + std_cargo
ret.steerKpV, ret.steerKiV = [[0.723], [0.0428]]
ret.steerKf = 0.00006
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
@@ -135,8 +142,8 @@ class CarInterface(object):
ret.steerRatio = 13.7
tire_stiffness_factor = 0.7933
ret.mass = 3400 * CV.LB_TO_KG + std_cargo #mean between normal and hybrid
ret.steerKpV, ret.steerKiV = [[0.6], [0.1]]
ret.steerKf = 0.00006
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
@@ -145,15 +152,42 @@ class CarInterface(object):
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
ret.steerKpV, ret.steerKiV = [[0.6], [0.05]]
ret.steerKf = 0.00006
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.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.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.17], [0.03]]
ret.lateralTuning.pid.kf = 0.00006
elif candidate == CAR.RAV4_2019:
stop_and_go = True
ret.safetyParam = 100
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.kf = 0.00007818594
elif candidate == CAR.COROLLA_HATCH:
stop_and_go = True
ret.safetyParam = 100
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]]
ret.lateralTuning.pid.kf = 0.00007818594
ret.steerRateCost = 1.
ret.centerToFront = ret.wheelbase * 0.44
ret.longPidDeadzoneBP = [0., 9.]
ret.longPidDeadzoneV = [0., .15]
#detect the Pedal address
ret.enableGasInterceptor = 0x201 in fingerprint
@@ -196,21 +230,24 @@ class CarInterface(object):
cloudlog.warn("ECU Gas Interceptor: %r", ret.enableGasInterceptor)
ret.steerLimitAlert = False
ret.longitudinalKpBP = [0., 5., 35.]
ret.longitudinalKiBP = [0., 35.]
ret.longitudinalTuning.deadzoneBP = [0., 9.]
ret.longitudinalTuning.deadzoneV = [0., .15]
ret.longitudinalTuning.kpBP = [0., 5., 35.]
ret.longitudinalTuning.kiBP = [0., 35.]
ret.stoppingControl = False
ret.startAccel = 0.0
if ret.enableGasInterceptor:
ret.gasMaxBP = [0., 9., 35]
ret.gasMaxV = [0.2, 0.5, 0.7]
ret.longitudinalKpV = [1.2, 0.8, 0.5]
ret.longitudinalKiV = [0.18, 0.12]
ret.longitudinalTuning.kpV = [1.2, 0.8, 0.5]
ret.longitudinalTuning.kiV = [0.18, 0.12]
else:
ret.gasMaxBP = [0.]
ret.gasMaxV = [0.5]
ret.longitudinalKpV = [3.6, 2.4, 1.5]
ret.longitudinalKiV = [0.54, 0.36]
ret.longitudinalTuning.kpV = [3.6, 2.4, 1.5]
ret.longitudinalTuning.kiV = [0.54, 0.36]
return ret
@@ -270,7 +307,7 @@ class CarInterface(object):
ret.cruiseState.available = bool(self.CS.main_on)
ret.cruiseState.speedOffset = 0.
if self.CP.carFingerprint in [CAR.RAV4H, CAR.HIGHLANDERH, CAR.HIGHLANDER] or self.CP.enableGasInterceptor:
if self.CP.carFingerprint in NO_STOP_TIMER_CAR or self.CP.enableGasInterceptor:
# ignore standstill in hybrid vehicles, since pcm allows to restart without
# receiving any special command
# also if interceptor is detected
@@ -369,7 +406,7 @@ class CarInterface(object):
self.CC.update(self.sendcan, c.enabled, self.CS, self.frame,
c.actuators, c.cruiseControl.cancel, c.hudControl.visualAlert,
c.hudControl.audibleAlert, self.forwarding_camera,
c.hudControl.leftLaneVisible, c.hudControl.rightLaneVisible, c.hudControl.leadVisible,
c.hudControl.leftLaneVisible, c.hudControl.rightLaneVisible, c.hudControl.leadVisible,
c.hudControl.leftLaneDepart, c.hudControl.rightLaneDepart)
self.frame += 1
selfdrive/car/toyota/radar_interface.py
+27 -17
View File
@@ -7,39 +7,48 @@ 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
from selfdrive.car.toyota.values import NO_DSU_CAR, DBC, TSSP2_CAR
def _create_radard_can_parser(car_fingerprint):
dbc_f = DBC[car_fingerprint]['radar']
RADAR_A_MSGS = list(range(0x210, 0x220))
RADAR_B_MSGS = list(range(0x220, 0x230))
def _create_radard_can_parser():
dbc_f = 'toyota_prius_2017_adas.dbc'
if car_fingerprint in TSSP2_CAR:
RADAR_A_MSGS = list(range(0x180, 0x190))
RADAR_B_MSGS = list(range(0x190, 0x1a0))
else:
RADAR_A_MSGS = list(range(0x210, 0x220))
RADAR_B_MSGS = list(range(0x220, 0x230))
msg_a_n = len(RADAR_A_MSGS)
msg_b_n = len(RADAR_B_MSGS)
signals = list(zip(
['LONG_DIST'] * msg_a_n + ['NEW_TRACK'] * msg_a_n + ['LAT_DIST'] * msg_a_n +
['REL_SPEED'] * msg_a_n + ['VALID'] * msg_a_n + ['SCORE'] * msg_b_n,
RADAR_A_MSGS * 5 + RADAR_B_MSGS,
[255] * msg_a_n + [1] * msg_a_n + [0] * msg_a_n + [0] * msg_a_n + [0] * msg_a_n + [0] * msg_b_n))
signals = zip(['LONG_DIST'] * msg_a_n + ['NEW_TRACK'] * msg_a_n + ['LAT_DIST'] * msg_a_n +
['REL_SPEED'] * msg_a_n + ['VALID'] * msg_a_n + ['SCORE'] * msg_b_n,
RADAR_A_MSGS * 5 + RADAR_B_MSGS,
[255] * msg_a_n + [1] * msg_a_n + [0] * msg_a_n + [0] * msg_a_n + [0] * msg_a_n + [0] * msg_b_n)
checks = list(zip(RADAR_A_MSGS + RADAR_B_MSGS, [20]*(msg_a_n + msg_b_n)))
checks = zip(RADAR_A_MSGS + RADAR_B_MSGS, [20]*(msg_a_n + msg_b_n))
return CANParser(os.path.splitext(dbc_f)[0], signals, checks, 1)
class RadarInterface(object):
def __init__(self, CP):
# radar
self.pts = {}
self.valid_cnt = {key: 0 for key in RADAR_A_MSGS}
self.track_id = 0
self.delay = 0.0 # Delay of radar
self.rcp = _create_radard_can_parser()
if CP.carFingerprint in TSSP2_CAR:
self.RADAR_A_MSGS = list(range(0x180, 0x190))
self.RADAR_B_MSGS = list(range(0x190, 0x1a0))
else:
self.RADAR_A_MSGS = list(range(0x210, 0x220))
self.RADAR_B_MSGS = list(range(0x220, 0x230))
self.valid_cnt = {key: 0 for key in self.RADAR_A_MSGS}
self.rcp = _create_radard_can_parser(CP.carFingerprint)
self.no_dsu_car = CP.carFingerprint in NO_DSU_CAR
context = zmq.Context()
@@ -48,6 +57,7 @@ class RadarInterface(object):
def update(self):
ret = car.RadarState.new_message()
if self.no_dsu_car:
# TODO: make a adas dbc file for dsu-less models
time.sleep(0.05)
@@ -58,7 +68,7 @@ class RadarInterface(object):
while 1:
tm = int(sec_since_boot() * 1e9)
updated_messages.update(self.rcp.update(tm, True))
if RADAR_B_MSGS[-1] in updated_messages:
if self.RADAR_B_MSGS[-1] in updated_messages:
break
errors = []
@@ -68,7 +78,7 @@ class RadarInterface(object):
ret.canMonoTimes = canMonoTimes
for ii in updated_messages:
if ii in RADAR_A_MSGS:
if ii in self.RADAR_A_MSGS:
cpt = self.rcp.vl[ii]
if cpt['LONG_DIST'] >=255 or cpt['NEW_TRACK']:
selfdrive/car/toyota/values.py
+41 -24
View File
@@ -12,6 +12,9 @@ class CAR:
CAMRYH = "TOYOTA CAMRY HYBRID 2018"
HIGHLANDER = "TOYOTA HIGHLANDER 2017"
HIGHLANDERH = "TOYOTA HIGHLANDER HYBRID 2018"
AVALON = "TOYOTA AVALON 2016"
RAV4_2019 = "TOYOTA RAV4 2019"
COROLLA_HATCH = "TOYOTA COROLLA HATCH 2019"
class ECU:
@@ -22,39 +25,39 @@ class ECU:
# addr: (ecu, cars, bus, 1/freq*100, vl)
STATIC_MSGS = [
(0x130, ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH), 1, 100, '\x00\x00\x00\x00\x00\x00\x38'),
(0x240, ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH), 1, 5, '\x00\x10\x01\x00\x10\x01\x00'),
(0x241, ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH), 1, 5, '\x00\x10\x01\x00\x10\x01\x00'),
(0x244, ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH), 1, 5, '\x00\x10\x01\x00\x10\x01\x00'),
(0x245, ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH), 1, 5, '\x00\x10\x01\x00\x10\x01\x00'),
(0x248, ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH), 1, 5, '\x00\x00\x00\x00\x00\x00\x01'),
(0x367, ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH), 0, 40, '\x06\x00'),
(0x414, ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH), 0, 100, '\x00\x00\x00\x00\x00\x00\x17\x00'),
(0x130, ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON), 1, 100, '\x00\x00\x00\x00\x00\x00\x38'),
(0x240, ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON), 1, 5, '\x00\x10\x01\x00\x10\x01\x00'),
(0x241, ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON), 1, 5, '\x00\x10\x01\x00\x10\x01\x00'),
(0x244, ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON), 1, 5, '\x00\x10\x01\x00\x10\x01\x00'),
(0x245, ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON), 1, 5, '\x00\x10\x01\x00\x10\x01\x00'),
(0x248, ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON), 1, 5, '\x00\x00\x00\x00\x00\x00\x01'),
(0x367, ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON), 0, 40, '\x06\x00'),
(0x414, ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON), 0, 100, '\x00\x00\x00\x00\x00\x00\x17\x00'),
(0x466, ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.HIGHLANDER, CAR.HIGHLANDERH), 1, 100, '\x20\x20\xAD'),
(0x466, ECU.CAM, (CAR.COROLLA), 1, 100, '\x24\x20\xB1'),
(0x489, ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH), 0, 100, '\x00\x00\x00\x00\x00\x00\x00'),
(0x48a, ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH), 0, 100, '\x00\x00\x00\x00\x00\x00\x00'),
(0x48b, ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH), 0, 100, '\x66\x06\x08\x0a\x02\x00\x00\x00'),
(0x4d3, ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA), 0, 100, '\x1C\x00\x00\x01\x00\x00\x00\x00'),
(0x466, ECU.CAM, (CAR.COROLLA, CAR.AVALON), 1, 100, '\x24\x20\xB1'),
(0x489, ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON), 0, 100, '\x00\x00\x00\x00\x00\x00\x00'),
(0x48a, ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON), 0, 100, '\x00\x00\x00\x00\x00\x00\x00'),
(0x48b, ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON), 0, 100, '\x66\x06\x08\x0a\x02\x00\x00\x00'),
(0x4d3, ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.AVALON), 0, 100, '\x1C\x00\x00\x01\x00\x00\x00\x00'),
(0x128, ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA), 1, 3, '\xf4\x01\x90\x83\x00\x37'),
(0x128, ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.AVALON), 1, 3, '\xf4\x01\x90\x83\x00\x37'),
(0x128, ECU.DSU, (CAR.HIGHLANDER, CAR.HIGHLANDERH), 1, 3, '\x03\x00\x20\x00\x00\x52'),
(0x141, ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH), 1, 2, '\x00\x00\x00\x46'),
(0x160, ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH), 1, 7, '\x00\x00\x08\x12\x01\x31\x9c\x51'),
(0x161, ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA), 1, 7, '\x00\x1e\x00\x00\x00\x80\x07'),
(0x141, ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON), 1, 2, '\x00\x00\x00\x46'),
(0x160, ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON), 1, 7, '\x00\x00\x08\x12\x01\x31\x9c\x51'),
(0x161, ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.AVALON), 1, 7, '\x00\x1e\x00\x00\x00\x80\x07'),
(0X161, ECU.DSU, (CAR.HIGHLANDERH, CAR.HIGHLANDER), 1, 7, '\x00\x1e\x00\xd4\x00\x00\x5b'),
(0x283, ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH), 0, 3, '\x00\x00\x00\x00\x00\x00\x8c'),
(0x283, ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON), 0, 3, '\x00\x00\x00\x00\x00\x00\x8c'),
(0x2E6, ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH), 0, 3, '\xff\xf8\x00\x08\x7f\xe0\x00\x4e'),
(0x2E7, ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH), 0, 3, '\xa8\x9c\x31\x9c\x00\x00\x00\x02'),
(0x33E, ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH), 0, 20, '\x0f\xff\x26\x40\x00\x1f\x00'),
(0x344, ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH), 0, 5, '\x00\x00\x01\x00\x00\x00\x00\x50'),
(0x344, ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON), 0, 5, '\x00\x00\x01\x00\x00\x00\x00\x50'),
(0x365, ECU.DSU, (CAR.PRIUS, CAR.LEXUS_RXH, CAR.HIGHLANDERH), 0, 20, '\x00\x00\x00\x80\x03\x00\x08'),
(0x365, ECU.DSU, (CAR.RAV4, CAR.RAV4H, CAR.COROLLA, CAR.HIGHLANDER), 0, 20, '\x00\x00\x00\x80\xfc\x00\x08'),
(0x365, ECU.DSU, (CAR.RAV4, CAR.RAV4H, CAR.COROLLA, CAR.HIGHLANDER, CAR.AVALON), 0, 20, '\x00\x00\x00\x80\xfc\x00\x08'),
(0x366, ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.HIGHLANDERH), 0, 20, '\x00\x00\x4d\x82\x40\x02\x00'),
(0x366, ECU.DSU, (CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER), 0, 20, '\x00\x72\x07\xff\x09\xfe\x00'),
(0x366, ECU.DSU, (CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.AVALON), 0, 20, '\x00\x72\x07\xff\x09\xfe\x00'),
(0x470, ECU.DSU, (CAR.PRIUS, CAR.LEXUS_RXH), 1, 100, '\x00\x00\x02\x7a'),
(0x470, ECU.DSU, (CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.RAV4H), 1, 100, '\x00\x00\x01\x79'),
(0x4CB, ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDERH, CAR.HIGHLANDER), 0, 100, '\x0c\x00\x00\x00\x00\x00\x00\x00'),
(0x4CB, ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDERH, CAR.HIGHLANDER, CAR.AVALON), 0, 100, '\x0c\x00\x00\x00\x00\x00\x00\x00'),
(0x292, ECU.APGS, (CAR.PRIUS), 0, 3, '\x00\x00\x00\x00\x00\x00\x00\x9e'),
(0x32E, ECU.APGS, (CAR.PRIUS), 0, 20, '\x00\x00\x00\x00\x00\x00\x00\x00'),
@@ -123,9 +126,9 @@ FINGERPRINTS = {
36: 8, 37: 8, 114: 5, 119: 6, 120: 4, 170: 8, 180: 8, 186: 4, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 544: 4, 550: 8, 552: 4, 562: 6, 608: 8, 610: 8, 643: 7, 658: 8, 705: 8, 728: 8, 761: 8, 764: 8, 800: 8, 810: 2, 812: 8, 814: 8, 818: 8, 822: 8, 824: 8, 830: 7, 835: 8, 836: 8, 869: 7, 870: 7, 888: 8, 889: 8, 891: 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, 983: 8, 984: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1002: 8, 1011: 8, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1056: 8, 1059: 1, 1076: 8, 1077: 8, 1082: 8, 1114: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1228: 8, 1237: 8, 1263: 8, 1264: 8, 1279: 8, 1412: 8, 1541: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1595: 8, 1745: 8, 1779: 8, 1786: 8, 1787: 8, 1788: 8, 1789: 8, 1808: 8, 1816: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8
}],
CAR.CAMRYH: [
#LE and LE with Blindspot Monitor
#SE, LE and LE with Blindspot Monitor
{
36: 8, 37: 8, 166: 8, 170: 8, 180: 8, 295: 8, 296: 8, 426: 6, 452: 8, 466: 8, 467: 8, 550: 8, 552: 4, 560: 7, 562: 6, 581: 5, 608: 8, 610: 8, 643: 7, 713: 8, 728: 8, 740: 5, 761: 8, 764: 8, 800: 8, 810: 2, 812: 8, 818: 8, 824: 8, 829: 2, 830: 7, 835: 8, 836: 8, 869: 7, 870: 7, 871: 2, 889: 8, 898: 8, 900: 6, 902: 6, 905: 8, 921: 8, 933: 8, 934: 8, 935: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 8, 955: 8, 956: 8, 971: 7, 975: 5, 983: 8, 984: 8, 993: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1002: 8, 1011: 8, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1057: 8, 1059: 1, 1071: 8, 1076: 8, 1077: 8, 1084: 8, 1085: 8, 1086: 8, 1114: 8, 1132: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1235: 8, 1237: 8, 1264: 8, 1279: 8, 1541: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1595: 8, 1745: 8, 1779: 8, 1786: 8, 1787: 8, 1788: 8, 1789: 8, 1808: 8, 1810: 8, 1816: 8, 1818: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8
36: 8, 37: 8, 166: 8, 170: 8, 180: 8, 295: 8, 296: 8, 426: 6, 452: 8, 466: 8, 467: 8, 550: 8, 552: 4, 560: 7, 562: 6, 581: 5, 608: 8, 610: 8, 643: 7, 713: 8, 728: 8, 740: 5, 761: 8, 764: 8, 800: 8, 810: 2, 812: 8, 818: 8, 824: 8, 829: 2, 830: 7, 835: 8, 836: 8, 865: 8, 869: 7, 870: 7, 871: 2, 889: 8, 896: 8, 898: 8, 900: 6, 902: 6, 905: 8, 921: 8, 933: 8, 934: 8, 935: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 8, 955: 8, 956: 8, 971: 7, 975: 5, 983: 8, 984: 8, 993: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1002: 8, 1011: 8, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1057: 8, 1059: 1, 1071: 8, 1076: 8, 1077: 8, 1084: 8, 1085: 8, 1086: 8, 1114: 8, 1132: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1235: 8, 1237: 8, 1264: 8, 1279: 8, 1541: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1592: 8, 1594: 8, 1595: 8, 1649: 8, 1745: 8, 1779: 8, 1786: 8, 1787: 8, 1788: 8, 1789: 8, 1808: 8, 1810: 8, 1816: 8, 1818: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8
},
#SL
{
@@ -141,6 +144,15 @@ FINGERPRINTS = {
CAR.HIGHLANDERH: [{
36: 8, 37: 8, 170: 8, 180: 8, 296: 8, 426: 6, 452: 8, 466: 8, 467: 8, 550: 8, 552: 4, 560: 7, 581: 5, 608: 8, 610: 5, 643: 7, 713: 8, 740: 5, 800: 8, 835: 8, 836: 8, 849: 4, 869: 7, 870: 7, 871: 2, 896: 8, 897: 8, 900: 6, 902: 6, 905: 8, 911: 8, 916: 3, 918: 7, 921: 8, 933: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 3, 955: 8, 956: 8, 979: 2, 998: 5, 999: 7, 1000: 8, 1001: 8, 1005: 2, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1043: 8, 1044: 8, 1056: 8, 1059: 1, 1112: 8, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1176: 8, 1177: 8, 1178: 8, 1179: 8, 1180: 8, 1181: 8, 1184: 8, 1185: 8, 1186: 8, 1189: 8, 1190: 8, 1191: 8, 1192: 8, 1196: 8, 1197: 8, 1198: 8, 1199: 8, 1206: 8, 1212: 8, 1227: 8, 1232: 8, 1235: 8, 1237: 8, 1263: 8, 1264: 8, 1279: 8, 1552: 8, 1553: 8, 1554: 8, 1556: 8, 1557: 8, 1561: 8, 1562: 8, 1568: 8, 1569: 8, 1570: 8, 1571: 8, 1572: 8, 1584: 8, 1589: 8, 1592: 8, 1593: 8, 1595: 8, 1599: 8, 1656: 8, 1728: 8, 1745: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8
}],
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: [{
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
}],
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, 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
@@ -157,6 +169,11 @@ DBC = {
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'),
}
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] # no resume button press required
selfdrive/common/cereal.mk
+2 -6
View File
@@ -5,11 +5,7 @@ UNAME_S ?= $(shell uname -s)
CEREAL_CFLAGS = -I$(PHONELIBS)/capnp-c/include
ifeq ($(OPTEST),1)
CEREAL_LIBS = -lcapnp -lkj
else ifeq ($(UNAME_S),Darwin)
ifeq ($(UNAME_S),Darwin)
CEREAL_CXXFLAGS = -I$(PHONELIBS)/capnp-cpp/mac/include
CEREAL_LIBS = $(PHONELIBS)/capnp-cpp/mac/lib/libcapnp.a \
@@ -33,9 +29,9 @@ ifeq ($(CEREAL_LIBS),)
-L$(PHONELIBS)/capnp-c/aarch64/lib/ \
-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/util.c
+1
View File
@@ -26,6 +26,7 @@ void* read_file(const char* path, size_t* out_len) {
fclose(f);
if (num_read != 1) {
free(buf);
return NULL;
}
selfdrive/common/version.h
+1 -1
View File
@@ -1 +1 @@
#define COMMA_VERSION "0.5.11-release"
#define COMMA_VERSION "0.5.12-release"
selfdrive/controls/controlsd.py
+51 -29
View File
@@ -2,6 +2,8 @@
import gc
import zmq
import json
from collections import defaultdict
from cereal import car, log
from common.numpy_fast import clip
from common.realtime import sec_since_boot, set_realtime_priority, Ratekeeper
@@ -19,7 +21,8 @@ from selfdrive.controls.lib.drive_helpers import learn_angle_model_bias, \
update_v_cruise, \
initialize_v_cruise
from selfdrive.controls.lib.longcontrol import LongControl, STARTING_TARGET_SPEED
from selfdrive.controls.lib.latcontrol import LatControl
from selfdrive.controls.lib.latcontrol_pid import LatControlPID
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
@@ -40,7 +43,7 @@ def isEnabled(state):
return (isActive(state) or state == State.preEnabled)
def data_sample(CI, CC, plan_sock, path_plan_sock, thermal, calibration, health, driver_monitor,
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):
"""Receive data from sockets and create events for battery, temperature and disk space"""
@@ -57,18 +60,21 @@ def data_sample(CI, CC, plan_sock, path_plan_sock, thermal, calibration, health,
dm = None
for socket, event in poller.poll(0):
msg = messaging.recv_one(socket)
rcv_times[msg.which()] = sec_since_boot()
if socket is thermal:
td = messaging.recv_one(socket)
td = msg
elif socket is calibration:
cal = messaging.recv_one(socket)
cal = msg
elif socket is health:
hh = messaging.recv_one(socket)
hh = msg
elif socket is driver_monitor:
dm = messaging.recv_one(socket)
dm = msg
elif socket is plan_sock:
plan = messaging.recv_one(socket)
plan = msg
elif socket is path_plan_sock:
path_plan = messaging.recv_one(socket)
path_plan = msg
if td is not None:
overtemp = td.thermal.thermalStatus >= ThermalStatus.red
@@ -202,7 +208,7 @@ 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(plan, path_plan, CS, CP, state, events, v_cruise_kph, v_cruise_kph_last, AM, rk,
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, passive, is_metric, cal_perc):
"""Given the state, this function returns an actuators packet"""
@@ -246,11 +252,11 @@ def state_control(plan, path_plan, CS, CP, state, events, v_cruise_kph, v_cruise
path_plan.cPoly, path_plan.cProb, CS.steeringAngle,
CS.steeringPressed)
cur_time = sec_since_boot() # TODO: This won't work in replay
mpc_time = plan.l20MonoTime / 1e9
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 - mpc_time, _DT_MPC + _DT) + _DT # no greater than dt mpc + dt, to prevent too high extraps
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)
v_acc_sol = plan.vStart + dt * (a_acc_sol + plan.aStart) / 2.0
@@ -258,8 +264,8 @@ def state_control(plan, path_plan, CS, CP, state, events, v_cruise_kph, v_cruise
actuators.gas, actuators.brake = LoC.update(active, CS.vEgo, CS.brakePressed, CS.standstill, CS.cruiseState.standstill,
v_cruise_kph, v_acc_sol, plan.vTargetFuture, a_acc_sol, CP)
# Steering PID loop and lateral MPC
actuators.steer, actuators.steerAngle = LaC.update(active, CS.vEgo, CS.steeringAngle,
CS.steeringPressed, CP, VM, path_plan)
actuators.steer, actuators.steerAngle, lac_log = LaC.update(active, CS.vEgo, CS.steeringAngle, CS.steeringRate,
CS.steeringPressed, CP, VM, path_plan)
# Send a "steering required alert" if saturation count has reached the limit
if LaC.sat_flag and CP.steerLimitAlert:
@@ -278,12 +284,12 @@ def state_control(plan, path_plan, CS, CP, state, events, v_cruise_kph, v_cruise
AM.process_alerts(sec_since_boot())
return actuators, v_cruise_kph, driver_status, angle_model_bias, v_acc_sol, a_acc_sol
return actuators, v_cruise_kph, driver_status, angle_model_bias, 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, live100, AM, driver_status,
LaC, LoC, angle_model_bias, passive, start_time, v_acc, a_acc):
LaC, LoC, angle_model_bias, passive, start_time, v_acc, a_acc, lac_log):
"""Send actuators and hud commands to the car, send live100 and MPC logging"""
plan_ts = plan.logMonoTime
plan = plan.plan
@@ -361,9 +367,6 @@ def data_send(plan, path_plan, CS, CI, CP, VM, state, events, actuators, v_cruis
"uiAccelCmd": float(LoC.pid.i),
"ufAccelCmd": float(LoC.pid.f),
"angleSteersDes": float(LaC.angle_steers_des),
"upSteer": float(LaC.pid.p),
"uiSteer": float(LaC.pid.i),
"ufSteer": float(LaC.pid.f),
"vTargetLead": float(v_acc),
"aTarget": float(a_acc),
"jerkFactor": float(plan.jerkFactor),
@@ -372,10 +375,15 @@ def data_send(plan, path_plan, CS, CI, CP, VM, state, events, actuators, v_cruis
"vCurvature": plan.vCurvature,
"decelForTurn": plan.decelForTurn,
"cumLagMs": -rk.remaining * 1000.,
"startMonoTime": start_time,
"startMonoTime": int(start_time * 1e9),
"mapValid": plan.mapValid,
"forceDecel": bool(force_decel),
}
if CP.lateralTuning.which() == 'pid':
dat.live100.lateralControlState.pidState = lac_log
else:
dat.live100.lateralControlState.indiState = lac_log
live100.send(dat.to_bytes())
# carState
@@ -443,7 +451,12 @@ def controlsd_thread(gctx=None, rate=100):
LoC = LongControl(CP, CI.compute_gb)
VM = VehicleModel(CP)
LaC = LatControl(CP)
if CP.lateralTuning.which() == 'pid':
LaC = LatControlPID(CP)
else:
LaC = LatControlINDI(CP)
AM = AlertManager()
driver_status = DriverStatus()
@@ -465,6 +478,8 @@ def controlsd_thread(gctx=None, rate=100):
mismatch_counter = 0
low_battery = False
rcv_times = defaultdict(int)
plan = messaging.new_message()
plan.init('plan')
path_plan = messaging.new_message()
@@ -485,23 +500,30 @@ def controlsd_thread(gctx=None, rate=100):
prof = Profiler(False) # off by default
while True:
start_time = int(sec_since_boot() * 1e9)
start_time = sec_since_boot()
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(CI, CC, plan_sock, path_plan_sock, thermal, cal, health, driver_monitor,
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)
prof.checkpoint("Sample")
path_plan_age = (start_time - path_plan.logMonoTime) / 1e9
plan_age = (start_time - plan.logMonoTime) / 1e9
# 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.paramsValid:
events.append(create_event('vehicleModelInvalid', [ET.WARNING]))
events += list(plan.plan.events)
if not path_plan.pathPlan.modelValid:
events.append(create_event('modelCommIssue', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
if not plan.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]))
# 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:
@@ -514,8 +536,8 @@ def controlsd_thread(gctx=None, rate=100):
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 = \
state_control(plan.plan, path_plan.pathPlan, CS, CP, state, events, v_cruise_kph,
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, passive, is_metric, cal_perc)
@@ -523,7 +545,7 @@ def controlsd_thread(gctx=None, rate=100):
# Publish data
CC = data_send(plan, path_plan, CS, CI, CP, VM, state, events, actuators, v_cruise_kph, rk, carstate, carcontrol,
live100, AM, driver_status, LaC, LoC, angle_model_bias, passive, start_time, v_acc, a_acc)
live100, AM, driver_status, LaC, LoC, angle_model_bias, passive, start_time, v_acc, a_acc, lac_log)
prof.checkpoint("Sent")
rk.keep_time() # Run at 100Hz
selfdrive/controls/lib/drive_helpers.py
+7 -1
View File
@@ -1,7 +1,9 @@
from cereal import car
from common.numpy_fast import clip
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
@@ -55,6 +57,10 @@ def rate_limit(new_value, last_value, dw_step, up_step):
return clip(new_value, last_value + dw_step, last_value + up_step)
def get_steer_max(CP, v_ego):
return interp(v_ego, CP.steerMaxBP, CP.steerMaxV)
def learn_angle_model_bias(lateral_control, v_ego, angle_model_bias, c_poly, c_prob, angle_steers, steer_override):
# simple integral controller that learns how much steering offset to put to have the car going straight
# while being in the middle of the lane
selfdrive/controls/lib/driver_monitor.py
+2 -1
View File
@@ -16,6 +16,7 @@ _PITCH_WEIGHT = 1.5 # pitch matters a lot more
_METRIC_THRESHOLD = 0.4
_PITCH_POS_ALLOWANCE = 0.08 # rad, to not be too sensitive on positive pitch
_PITCH_NATURAL_OFFSET = 0.1 # people don't seem to look straight when they drive relaxed, rather a bit up
_YAW_NATURAL_OFFSET = 0.08 # people don't seem to look straight when they drive relaxed, rather a bit to the right (center of car)
_STD_THRESHOLD = 0.1 # above this standard deviation consider the measurement invalid
_DISTRACTED_FILTER_TS = 0.25 # 0.6Hz
_VARIANCE_FILTER_TS = 20. # 0.008Hz
@@ -89,7 +90,7 @@ class DriverStatus():
def _is_driver_distracted(self, pose):
# to be tuned and to learn the driver's normal pose
pitch_error = pose.pitch - _PITCH_NATURAL_OFFSET
yaw_error = pose.yaw
yaw_error = pose.yaw - _YAW_NATURAL_OFFSET
# add positive pitch allowance
if pitch_error > 0.:
pitch_error = max(pitch_error - _PITCH_POS_ALLOWANCE, 0.)
selfdrive/controls/lib/latcontrol_indi.py
+104
View File
@@ -0,0 +1,104 @@
import math
import numpy as np
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
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],
[0.0, 0.0, 1.0]])
C = np.matrix([[1.0, 0.0, 0.0],
[0.0, 1.0, 0.0]])
# Q = np.matrix([[1e-2, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 10.0]])
# R = np.matrix([[1e-2, 0.0], [0.0, 1e3]])
# (x, l, K) = control.dare(np.transpose(A), np.transpose(C), Q, R)
# K = np.transpose(K)
K = np.matrix([[7.30262179e-01, 2.07003658e-04],
[7.29394177e+00, 1.39159419e-02],
[1.71022442e+01, 3.38495381e-02]])
self.K = K
self.A_K = A - np.dot(K, C)
self.x = np.matrix([[0.], [0.], [0.]])
self.enfore_rate_limit = CP.carName == "toyota"
self.RC = CP.lateralTuning.indi.timeConstant
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.reset()
def reset(self):
self.delayed_output = 0.
self.output_steer = 0.
self.counter = 0
def update(self, active, v_ego, angle_steers, angle_steers_rate, steer_override, CP, VM, path_plan):
# Update Kalman filter
y = np.matrix([[math.radians(angle_steers)], [math.radians(angle_steers_rate)]])
self.x = np.dot(self.A_K, self.x) + np.dot(self.K, y)
indi_log = log.Live100Data.LateralINDIState.new_message()
indi_log.steerAngle = math.degrees(self.x[0])
indi_log.steerRate = math.degrees(self.x[1])
indi_log.steerAccel = math.degrees(self.x[2])
if v_ego < 0.3 or not active:
indi_log.active = False
self.output_steer = 0.0
self.delayed_output = 0.0
else:
self.angle_steers_des = path_plan.angleSteers
self.rate_steers_des = path_plan.rateSteers
steers_des = math.radians(self.angle_steers_des)
rate_des = math.radians(self.rate_steers_des)
# Expected actuator value
self.delayed_output = self.delayed_output * self.alpha + self.output_steer * (1. - self.alpha)
# Compute acceleration error
rate_sp = self.outer_loop_gain * (steers_des - self.x[0]) + rate_des
accel_sp = self.inner_loop_gain * (rate_sp - self.x[1])
accel_error = accel_sp - self.x[2]
# Compute change in actuator
g_inv = 1. / self.G
delta_u = g_inv * accel_error
# Enforce rate limit
if self.enfore_rate_limit:
steer_max = float(SteerLimitParams.STEER_MAX)
new_output_steer_cmd = steer_max * (self.delayed_output + delta_u)
prev_output_steer_cmd = steer_max * self.output_steer
new_output_steer_cmd = apply_toyota_steer_torque_limits(new_output_steer_cmd, prev_output_steer_cmd, prev_output_steer_cmd, SteerLimitParams)
self.output_steer = new_output_steer_cmd / steer_max
else:
self.output_steer = self.delayed_output + delta_u
steers_max = get_steer_max(CP, v_ego)
self.output_steer = clip(self.output_steer, -steers_max, steers_max)
indi_log.active = True
indi_log.rateSetPoint = float(rate_sp)
indi_log.accelSetPoint = float(accel_sp)
indi_log.accelError = float(accel_error)
indi_log.delayedOutput = float(self.delayed_output)
indi_log.delta = float(delta_u)
indi_log.output = float(self.output_steer)
self.sat_flag = False
return float(self.output_steer), float(self.angle_steers_des), indi_log
selfdrive/controls/lib/latcontrol.py → selfdrive/controls/lib/latcontrol_pid.py
+19 -15
View File
@@ -1,29 +1,27 @@
from selfdrive.controls.lib.pid import PIController
from common.numpy_fast import interp
from selfdrive.controls.lib.drive_helpers import get_steer_max
from cereal import car
_DT = 0.01 # 100Hz
_DT_MPC = 0.05 # 20Hz
from cereal import log
def get_steer_max(CP, v_ego):
return interp(v_ego, CP.steerMaxBP, CP.steerMaxV)
class LatControl(object):
class LatControlPID(object):
def __init__(self, CP):
self.pid = PIController((CP.steerKpBP, CP.steerKpV),
(CP.steerKiBP, CP.steerKiV),
k_f=CP.steerKf, pos_limit=1.0)
self.last_cloudlog_t = 0.0
self.pid = PIController((CP.lateralTuning.pid.kpBP, CP.lateralTuning.pid.kpV),
(CP.lateralTuning.pid.kiBP, CP.lateralTuning.pid.kiV),
k_f=CP.lateralTuning.pid.kf, pos_limit=1.0)
self.angle_steers_des = 0.
def reset(self):
self.pid.reset()
def update(self, active, v_ego, angle_steers, steer_override, CP, VM, path_plan):
def update(self, active, v_ego, angle_steers, angle_steers_rate, steer_override, CP, VM, path_plan):
pid_log = log.Live100Data.LateralPIDState.new_message()
pid_log.steerAngle = float(angle_steers)
pid_log.steerRate = float(angle_steers_rate)
if v_ego < 0.3 or not active:
output_steer = 0.0
pid_log.active = False
self.pid.reset()
else:
# TODO: ideally we should interp, but for tuning reasons we keep the mpc solution
@@ -43,6 +41,12 @@ class LatControl(object):
deadzone = 0.0
output_steer = self.pid.update(self.angle_steers_des, angle_steers, check_saturation=(v_ego > 10), override=steer_override,
feedforward=steer_feedforward, speed=v_ego, deadzone=deadzone)
pid_log.active = True
pid_log.p = self.pid.p
pid_log.i = self.pid.i
pid_log.f = self.pid.f
pid_log.output = output_steer
pid_log.saturated = bool(self.pid.saturated)
self.sat_flag = self.pid.saturated
return output_steer, float(self.angle_steers_des)
return output_steer, float(self.angle_steers_des), pid_log
selfdrive/controls/lib/longcontrol.py
+3 -3
View File
@@ -58,8 +58,8 @@ def long_control_state_trans(active, long_control_state, v_ego, v_target, v_pid,
class LongControl(object):
def __init__(self, CP, compute_gb):
self.long_control_state = LongCtrlState.off # initialized to off
self.pid = PIController((CP.longitudinalKpBP, CP.longitudinalKpV),
(CP.longitudinalKiBP, CP.longitudinalKiV),
self.pid = PIController((CP.longitudinalTuning.kpBP, CP.longitudinalTuning.kpV),
(CP.longitudinalTuning.kiBP, CP.longitudinalTuning.kiV),
rate=RATE,
sat_limit=0.8,
convert=compute_gb)
@@ -99,7 +99,7 @@ class LongControl(object):
# Toyota starts braking more when it thinks you want to stop
# Freeze the integrator so we don't accelerate to compensate, and don't allow positive acceleration
prevent_overshoot = not CP.stoppingControl and v_ego < 1.5 and v_target_future < 0.7
deadzone = interp(v_ego_pid, CP.longPidDeadzoneBP, CP.longPidDeadzoneV)
deadzone = interp(v_ego_pid, CP.longitudinalTuning.deadzoneBP, CP.longitudinalTuning.deadzoneV)
output_gb = self.pid.update(self.v_pid, v_ego_pid, speed=v_ego_pid, deadzone=deadzone, feedforward=a_target, freeze_integrator=prevent_overshoot)
selfdrive/controls/lib/longitudinal_mpc/generator.cpp
+2 -2
View File
@@ -32,7 +32,7 @@ int main( )
// Running cost
Function h;
h << exp(0.3 * NORM_RW_ERROR(v_ego, v_l, d_l));
h << exp(0.3 * NORM_RW_ERROR(v_ego, v_l, d_l)) - 1;
h << (d_l - desired) / (0.05 * v_ego + 0.5);
h << a_ego * (0.1 * v_ego + 1.0);
h << j_ego * (0.1 * v_ego + 1.0);
@@ -42,7 +42,7 @@ int main( )
// Terminal cost
Function hN;
hN << exp(0.3 * NORM_RW_ERROR(v_ego, v_l, d_l));
hN << exp(0.3 * NORM_RW_ERROR(v_ego, v_l, d_l)) - 1;
hN << (d_l - desired) / (0.05 * v_ego + 0.5);
hN << a_ego * (0.1 * v_ego + 1.0);
selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_common.h
+2 -2
View File
@@ -181,8 +181,8 @@ real_t evGx[ 180 ];
/** Column vector of size: 60 */
real_t evGu[ 60 ];
/** Column vector of size: 15 */
real_t objAuxVar[ 15 ];
/** Column vector of size: 13 */
real_t objAuxVar[ 13 ];
/** Row vector of size: 6 */
real_t objValueIn[ 6 ];
selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_qpoases_interface.o
BIN
View File
Binary file not shown.
selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_solver.c
+17 -20
View File
@@ -73,7 +73,7 @@ void acado_evaluateLSQ(const real_t* in, real_t* out)
const real_t* xd = in;
const real_t* u = in + 3;
const real_t* od = in + 4;
/* Vector of auxiliary variables; number of elements: 15. */
/* Vector of auxiliary variables; number of elements: 13. */
real_t* a = acadoWorkspace.objAuxVar;
/* Compute intermediate quantities: */
@@ -87,22 +87,20 @@ a[6] = (1.0/sqrt((xd[1]+(real_t)(5.0000000000000000e-01))));
a[7] = (a[6]*(real_t)(5.0000000000000000e-01));
a[8] = (a[2]*a[2]);
a[9] = (((real_t)(2.9999999999999999e-01)*(((((real_t)(1.8000000000000000e+00)-((real_t)(-1.8000000000000000e+00)))+((xd[1]+xd[1])*a[5]))*a[2])-((((((((xd[1]*(real_t)(1.8000000000000000e+00))-((od[1]-xd[1])*(real_t)(1.8000000000000000e+00)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((od[1]*od[1])/(real_t)(1.9620000000000001e+01)))+(real_t)(4.0000000000000000e+00))-(od[0]-xd[0]))*a[7])*a[8])))*a[3]);
a[10] = (real_t)(0.0000000000000000e+00);
a[11] = ((real_t)(1.0000000000000000e+00)/(((real_t)(5.0000000000000003e-02)*xd[1])+(real_t)(5.0000000000000000e-01)));
a[12] = ((real_t)(1.0000000000000000e+00)/(real_t)(1.9620000000000001e+01));
a[13] = (a[11]*a[11]);
a[14] = (real_t)(0.0000000000000000e+00);
a[10] = ((real_t)(1.0000000000000000e+00)/(((real_t)(5.0000000000000003e-02)*xd[1])+(real_t)(5.0000000000000000e-01)));
a[11] = ((real_t)(1.0000000000000000e+00)/(real_t)(1.9620000000000001e+01));
a[12] = (a[10]*a[10]);
/* Compute outputs: */
out[0] = a[1];
out[0] = (a[1]-(real_t)(1.0000000000000000e+00));
out[1] = (((od[0]-xd[0])-((real_t)(4.0000000000000000e+00)+((((xd[1]*(real_t)(1.8000000000000000e+00))-((od[1]-xd[1])*(real_t)(1.8000000000000000e+00)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((od[1]*od[1])/(real_t)(1.9620000000000001e+01)))))/(((real_t)(5.0000000000000003e-02)*xd[1])+(real_t)(5.0000000000000000e-01)));
out[2] = (xd[2]*(((real_t)(1.0000000000000001e-01)*xd[1])+(real_t)(1.0000000000000000e+00)));
out[3] = (u[0]*(((real_t)(1.0000000000000001e-01)*xd[1])+(real_t)(1.0000000000000000e+00)));
out[4] = a[4];
out[5] = a[9];
out[6] = a[10];
out[7] = (((real_t)(0.0000000000000000e+00)-(real_t)(1.0000000000000000e+00))*a[11]);
out[8] = ((((real_t)(0.0000000000000000e+00)-(((real_t)(1.8000000000000000e+00)-((real_t)(-1.8000000000000000e+00)))+((xd[1]+xd[1])*a[12])))*a[11])-((((od[0]-xd[0])-((real_t)(4.0000000000000000e+00)+((((xd[1]*(real_t)(1.8000000000000000e+00))-((od[1]-xd[1])*(real_t)(1.8000000000000000e+00)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((od[1]*od[1])/(real_t)(1.9620000000000001e+01)))))*(real_t)(5.0000000000000003e-02))*a[13]));
out[6] = (real_t)(0.0000000000000000e+00);
out[7] = (((real_t)(0.0000000000000000e+00)-(real_t)(1.0000000000000000e+00))*a[10]);
out[8] = ((((real_t)(0.0000000000000000e+00)-(((real_t)(1.8000000000000000e+00)-((real_t)(-1.8000000000000000e+00)))+((xd[1]+xd[1])*a[11])))*a[10])-((((od[0]-xd[0])-((real_t)(4.0000000000000000e+00)+((((xd[1]*(real_t)(1.8000000000000000e+00))-((od[1]-xd[1])*(real_t)(1.8000000000000000e+00)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((od[1]*od[1])/(real_t)(1.9620000000000001e+01)))))*(real_t)(5.0000000000000003e-02))*a[12]));
out[9] = (real_t)(0.0000000000000000e+00);
out[10] = (real_t)(0.0000000000000000e+00);
out[11] = (xd[2]*(real_t)(1.0000000000000001e-01));
@@ -110,7 +108,7 @@ out[12] = (((real_t)(1.0000000000000001e-01)*xd[1])+(real_t)(1.0000000000000000e
out[13] = (real_t)(0.0000000000000000e+00);
out[14] = (u[0]*(real_t)(1.0000000000000001e-01));
out[15] = (real_t)(0.0000000000000000e+00);
out[16] = a[14];
out[16] = (real_t)(0.0000000000000000e+00);
out[17] = (real_t)(0.0000000000000000e+00);
out[18] = (real_t)(0.0000000000000000e+00);
out[19] = (((real_t)(1.0000000000000001e-01)*xd[1])+(real_t)(1.0000000000000000e+00));
@@ -120,7 +118,7 @@ void acado_evaluateLSQEndTerm(const real_t* in, real_t* out)
{
const real_t* xd = in;
const real_t* od = in + 3;
/* Vector of auxiliary variables; number of elements: 14. */
/* Vector of auxiliary variables; number of elements: 13. */
real_t* a = acadoWorkspace.objAuxVar;
/* Compute intermediate quantities: */
@@ -134,20 +132,19 @@ a[6] = (1.0/sqrt((xd[1]+(real_t)(5.0000000000000000e-01))));
a[7] = (a[6]*(real_t)(5.0000000000000000e-01));
a[8] = (a[2]*a[2]);
a[9] = (((real_t)(2.9999999999999999e-01)*(((((real_t)(1.8000000000000000e+00)-((real_t)(-1.8000000000000000e+00)))+((xd[1]+xd[1])*a[5]))*a[2])-((((((((xd[1]*(real_t)(1.8000000000000000e+00))-((od[1]-xd[1])*(real_t)(1.8000000000000000e+00)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((od[1]*od[1])/(real_t)(1.9620000000000001e+01)))+(real_t)(4.0000000000000000e+00))-(od[0]-xd[0]))*a[7])*a[8])))*a[3]);
a[10] = (real_t)(0.0000000000000000e+00);
a[11] = ((real_t)(1.0000000000000000e+00)/(((real_t)(5.0000000000000003e-02)*xd[1])+(real_t)(5.0000000000000000e-01)));
a[12] = ((real_t)(1.0000000000000000e+00)/(real_t)(1.9620000000000001e+01));
a[13] = (a[11]*a[11]);
a[10] = ((real_t)(1.0000000000000000e+00)/(((real_t)(5.0000000000000003e-02)*xd[1])+(real_t)(5.0000000000000000e-01)));
a[11] = ((real_t)(1.0000000000000000e+00)/(real_t)(1.9620000000000001e+01));
a[12] = (a[10]*a[10]);
/* Compute outputs: */
out[0] = a[1];
out[0] = (a[1]-(real_t)(1.0000000000000000e+00));
out[1] = (((od[0]-xd[0])-((real_t)(4.0000000000000000e+00)+((((xd[1]*(real_t)(1.8000000000000000e+00))-((od[1]-xd[1])*(real_t)(1.8000000000000000e+00)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((od[1]*od[1])/(real_t)(1.9620000000000001e+01)))))/(((real_t)(5.0000000000000003e-02)*xd[1])+(real_t)(5.0000000000000000e-01)));
out[2] = (xd[2]*(((real_t)(1.0000000000000001e-01)*xd[1])+(real_t)(1.0000000000000000e+00)));
out[3] = a[4];
out[4] = a[9];
out[5] = a[10];
out[6] = (((real_t)(0.0000000000000000e+00)-(real_t)(1.0000000000000000e+00))*a[11]);
out[7] = ((((real_t)(0.0000000000000000e+00)-(((real_t)(1.8000000000000000e+00)-((real_t)(-1.8000000000000000e+00)))+((xd[1]+xd[1])*a[12])))*a[11])-((((od[0]-xd[0])-((real_t)(4.0000000000000000e+00)+((((xd[1]*(real_t)(1.8000000000000000e+00))-((od[1]-xd[1])*(real_t)(1.8000000000000000e+00)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((od[1]*od[1])/(real_t)(1.9620000000000001e+01)))))*(real_t)(5.0000000000000003e-02))*a[13]));
out[5] = (real_t)(0.0000000000000000e+00);
out[6] = (((real_t)(0.0000000000000000e+00)-(real_t)(1.0000000000000000e+00))*a[10]);
out[7] = ((((real_t)(0.0000000000000000e+00)-(((real_t)(1.8000000000000000e+00)-((real_t)(-1.8000000000000000e+00)))+((xd[1]+xd[1])*a[11])))*a[10])-((((od[0]-xd[0])-((real_t)(4.0000000000000000e+00)+((((xd[1]*(real_t)(1.8000000000000000e+00))-((od[1]-xd[1])*(real_t)(1.8000000000000000e+00)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((od[1]*od[1])/(real_t)(1.9620000000000001e+01)))))*(real_t)(5.0000000000000003e-02))*a[12]));
out[8] = (real_t)(0.0000000000000000e+00);
out[9] = (real_t)(0.0000000000000000e+00);
out[10] = (xd[2]*(real_t)(1.0000000000000001e-01));
selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_solver.o
BIN
View File
Binary file not shown.
selfdrive/controls/lib/longitudinal_mpc/libmpc1.so
BIN
View File
Binary file not shown.
selfdrive/controls/lib/longitudinal_mpc/longitudinal_mpc.o
BIN
View File
Binary file not shown.
selfdrive/controls/lib/pathplanner.py
+4 -1
View File
@@ -49,7 +49,7 @@ class PathPlanner(object):
self.angle_steers_des_prev = 0.0
self.angle_steers_des_time = 0.0
def update(self, CP, VM, CS, md, live100, live_parameters):
def update(self, rcv_times, CP, VM, CS, md, live100, live_parameters):
v_ego = CS.carState.vEgo
angle_steers = CS.carState.steeringAngle
active = live100.live100.active
@@ -104,6 +104,8 @@ class PathPlanner(object):
else:
self.invalid_counter = 0
cur_time = sec_since_boot()
model_dead = cur_time - rcv_times['model'] > 0.5
plan_valid = self.invalid_counter < 2
plan_send = messaging.new_message()
@@ -121,6 +123,7 @@ class PathPlanner(object):
plan_send.pathPlan.angleOffset = float(angle_offset_average)
plan_send.pathPlan.valid = bool(plan_valid)
plan_send.pathPlan.paramsValid = bool(live_parameters.liveParameters.valid)
plan_send.pathPlan.modelValid = bool(not model_dead)
self.plan.send(plan_send.to_bytes())
selfdrive/controls/lib/planner.py
+19 -18
View File
@@ -6,10 +6,11 @@ from common.params import Params
from common.numpy_fast import interp
import selfdrive.messaging as messaging
from cereal import car
from common.realtime import sec_since_boot
from selfdrive.swaglog import cloudlog
from selfdrive.config import Conversions as CV
from selfdrive.services import service_list
from selfdrive.controls.lib.drive_helpers import create_event, EventTypes as ET
from selfdrive.controls.lib.speed_smoother import speed_smoother
from selfdrive.controls.lib.longcontrol import LongCtrlState, MIN_CAN_SPEED
from selfdrive.controls.lib.fcw import FCWChecker
@@ -113,9 +114,9 @@ class Planner(object):
self.v_acc_future = min([self.mpc1.v_mpc_future, self.mpc2.v_mpc_future, v_cruise_setpoint])
def update(self, CS, CP, VM, PP, live20, live100, md, live_map_data):
def update(self, rcv_times, CS, CP, VM, PP, live20, live100, md, live_map_data):
"""Gets called when new live20 is available"""
cur_time = live20.logMonoTime / 1e9
cur_time = sec_since_boot()
v_ego = CS.carState.vEgo
long_control_state = live100.live100.longControlState
@@ -131,11 +132,13 @@ class Planner(object):
v_speedlimit = NO_CURVATURE_SPEED
v_curvature = NO_CURVATURE_SPEED
map_valid = live_map_data.liveMapData.mapValid
map_age = cur_time - rcv_times['liveMapData']
map_valid = live_map_data.liveMapData.mapValid and map_age < 10.0
# Speed limit and curvature
set_speed_limit_active = self.params.get("LimitSetSpeed") == "1" and self.params.get("SpeedLimitOffset") is not None
if set_speed_limit_active:
if set_speed_limit_active and map_valid:
if live_map_data.liveMapData.speedLimitValid:
speed_limit = live_map_data.liveMapData.speedLimit
offset = float(self.params.get("SpeedLimitOffset"))
@@ -206,24 +209,16 @@ class Planner(object):
if fcw:
cloudlog.info("FCW triggered %s", self.fcw_checker.counters)
model_dead = cur_time - (md.logMonoTime / 1e9) > 0.5
radar_dead = cur_time - rcv_times['live20'] > 0.5
radar_errors = list(live20.live20.radarErrors)
radar_fault = car.RadarState.Error.fault in radar_errors
radar_comm_issue = car.RadarState.Error.commIssue in radar_errors
# **** send the plan ****
plan_send = messaging.new_message()
plan_send.init('plan')
# TODO: Move all these events to controlsd. This has nothing to do with planning
events = []
if model_dead:
events.append(create_event('modelCommIssue', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
radar_errors = list(live20.live20.radarErrors)
if 'commIssue' in radar_errors:
events.append(create_event('radarCommIssue', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
if 'fault' in radar_errors:
events.append(create_event('radarFault', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
plan_send.plan.events = events
plan_send.plan.mdMonoTime = md.logMonoTime
plan_send.plan.l20MonoTime = live20.logMonoTime
@@ -242,6 +237,12 @@ class Planner(object):
plan_send.plan.decelForTurn = decel_for_turn
plan_send.plan.mapValid = map_valid
radar_valid = not (radar_dead or radar_fault)
plan_send.plan.radarValid = bool(radar_valid)
plan_send.plan.radarCommIssue = bool(radar_comm_issue)
plan_send.plan.processingDelay = (plan_send.logMonoTime / 1e9) - rcv_times['live20']
# Send out fcw
fcw = fcw and (self.fcw_enabled or long_control_state != LongCtrlState.off)
plan_send.plan.fcw = fcw
selfdrive/controls/lib/radar_helpers.py
+1 -1
View File
@@ -27,7 +27,7 @@ v_ego_stationary = 4. # no stationary object flag below this speed
# Lead Kalman Filter params
_VLEAD_A = [[1.0, ts], [0.0, 1.0]]
_VLEAD_C = [[1.0, 0.0]]
_VLEAD_C = [1.0, 0.0]
#_VLEAD_Q = np.matrix([[10., 0.0], [0.0, 100.]])
#_VLEAD_R = 1e3
#_VLEAD_K = np.matrix([[ 0.05705578], [ 0.03073241]])
selfdrive/controls/plannerd.py
+15 -8
View File
@@ -1,8 +1,10 @@
#!/usr/bin/env python
import zmq
from collections import defaultdict
from cereal import car
from common.params import Params
from common.realtime import sec_since_boot
from selfdrive.swaglog import cloudlog
from selfdrive.services import service_list
from selfdrive.controls.lib.planner import Planner
@@ -52,22 +54,27 @@ def plannerd_thread():
live_parameters.liveParameters.steerRatio = CP.steerRatio
live_parameters.liveParameters.stiffnessFactor = 1.0
rcv_times = defaultdict(int)
while True:
for socket, event in poller.poll():
msg = messaging.recv_one(socket)
rcv_times[msg.which()] = sec_since_boot()
if socket is live100_sock:
live100 = messaging.recv_one(socket)
live100 = msg
elif socket is car_state_sock:
car_state = messaging.recv_one(socket)
car_state = msg
elif socket is live_parameters_sock:
live_parameters = messaging.recv_one(socket)
live_parameters = msg
elif socket is model_sock:
model = messaging.recv_one(socket)
PP.update(CP, VM, car_state, model, live100, live_parameters)
model = msg
PP.update(rcv_times, CP, VM, car_state, model, live100, live_parameters)
elif socket is live_map_data_sock:
live_map_data = messaging.recv_one(socket)
live_map_data = msg
elif socket is live20_sock:
live20 = messaging.recv_one(socket)
PL.update(car_state, CP, VM, PP, live20, live100, model, live_map_data)
live20 = msg
PL.update(rcv_times, car_state, CP, VM, PP, live20, live100, model, live_map_data)
def main(gctx=None):
selfdrive/controls/tests/__init__.py
View File
selfdrive/controls/tests/test_following_distance.py
+90
View File
@@ -0,0 +1,90 @@
import unittest
import numpy as np
from cereal import log
import selfdrive.messaging as messaging
from selfdrive.config import Conversions as CV
from selfdrive.controls.lib.planner import calc_cruise_accel_limits
from selfdrive.controls.lib.speed_smoother import speed_smoother
from selfdrive.controls.lib.long_mpc import LongitudinalMpc
def RW(v_ego, v_l):
TR = 1.8
G = 9.81
return (v_ego * TR - (v_l - v_ego) * TR + v_ego * v_ego / (2 * G) - v_l * v_l / (2 * G))
class FakeSocket(object):
def send(self, data):
assert data
def run_following_distance_simulation(v_lead, t_end=200.0):
dt = 0.2
t = 0.
x_lead = 200.0
x_ego = 0.0
v_ego = v_lead
a_ego = 0.0
v_cruise_setpoint = v_lead + 10.
mpc = LongitudinalMpc(1, FakeSocket())
first = True
while t < t_end:
# Run cruise control
accel_limits = [float(x) for x in calc_cruise_accel_limits(v_ego, False)]
jerk_limits = [min(-0.1, accel_limits[0]), max(0.1, accel_limits[1])]
v_cruise, a_cruise = speed_smoother(v_ego, a_ego, v_cruise_setpoint,
accel_limits[1], accel_limits[0],
jerk_limits[1], jerk_limits[0],
dt)
# Setup CarState
CS = messaging.new_message()
CS.init('carState')
CS.carState.vEgo = v_ego
CS.carState.aEgo = a_ego
# Setup lead packet
lead = log.Live20Data.LeadData.new_message()
lead.status = True
lead.dRel = x_lead - x_ego
lead.vLead = v_lead
lead.aLeadK = 0.0
# Run MPC
mpc.set_cur_state(v_ego, a_ego)
if first: # Make sure MPC is converged on first timestep
for _ in range(20):
mpc.update(CS, lead, v_cruise_setpoint)
mpc.update(CS, lead, v_cruise_setpoint)
# Choose slowest of two solutions
if v_cruise < mpc.v_mpc:
v_ego, a_ego = v_cruise, a_cruise
else:
v_ego, a_ego = mpc.v_mpc, mpc.a_mpc
# Update state
x_lead += v_lead * dt
x_ego += v_ego * dt
t += dt
first = False
return x_lead - x_ego
class TestFollowingDistance(unittest.TestCase):
def test_following_distanc(self):
for speed_mph in np.linspace(10, 100, num=10):
v_lead = float(speed_mph * CV.MPH_TO_MS)
simulation_steady_state = run_following_distance_simulation(v_lead)
correct_steady_state = RW(v_lead, v_lead) + 4.0
self.assertAlmostEqual(simulation_steady_state, correct_steady_state, delta=0.1)
selfdrive/debug/cpu_usage_stat.py
+41 -21
View File
@@ -5,6 +5,7 @@ import sys
import numpy as np
import argparse
import re
from collections import defaultdict
'''
System tools like top/htop can only show current cpu usage values, so I write this script to do statistics jobs.
@@ -27,9 +28,11 @@ PRINT_INTERVAL = 5
SLEEP_INTERVAL = 0.2
monitored_proc_names = [
'ubloxd', 'thermald', 'uploader', 'controlsd', 'plannerd', 'radard', 'mapd', 'loggerd' , 'logmessaged', 'tombstoned',
'ubloxd', 'thermald', 'uploader', 'deleter', 'controlsd', 'plannerd', 'radard', 'mapd', 'loggerd' , 'logmessaged', 'tombstoned',
'logcatd', 'proclogd', 'boardd', 'pandad', './ui', 'calibrationd', 'locationd', 'visiond', 'sensord', 'updated', 'gpsd', 'athena']
cpu_time_names = ['user', 'system', 'children_user', 'children_system']
timer = getattr(time, 'monotonic', time.time)
def get_arg_parser():
parser = argparse.ArgumentParser(
@@ -38,8 +41,10 @@ def get_arg_parser():
parser.add_argument("proc_names", nargs="?", default='',
help="Process names to be monitored, comma seperated")
parser.add_argument("--list_all", nargs="?", type=bool, default=False,
parser.add_argument("--list_all", action='store_true',
help="Show all running processes' cmdline")
parser.add_argument("--detailed_times", action='store_true',
help="show cpu time details (split by user, system, child user, child system)")
return parser
@@ -61,39 +66,54 @@ if __name__ == "__main__":
if matched:
k = ' '.join(p.cmdline())
print('Add monitored proc:', k)
stats[k] = {'cpu_samples': [], 'avg_cpu': None, 'min': None, 'max': None}
stats[k] = {'cpu_samples': defaultdict(list), 'min': defaultdict(lambda: None), 'max': defaultdict(lambda: None),
'avg': defaultdict(lambda: 0.0), 'last_cpu_times': None, 'last_sys_time':None}
stats[k]['last_sys_time'] = timer()
stats[k]['last_cpu_times'] = p.cpu_times()
monitored_procs.append(p)
i = 0
interval_int = int(PRINT_INTERVAL / SLEEP_INTERVAL)
while True:
for p in monitored_procs:
k = ' '.join(p.cmdline())
stats[k]['cpu_samples'].append(p.cpu_percent())
cur_sys_time = timer()
cur_cpu_times = p.cpu_times()
cpu_times = np.subtract(cur_cpu_times, stats[k]['last_cpu_times']) / (cur_sys_time - stats[k]['last_sys_time'])
stats[k]['last_sys_time'] = cur_sys_time
stats[k]['last_cpu_times'] = cur_cpu_times
cpu_percent = 0
for num, name in enumerate(cpu_time_names):
stats[k]['cpu_samples'][name].append(cpu_times[num])
cpu_percent += cpu_times[num]
stats[k]['cpu_samples']['total'].append(cpu_percent)
time.sleep(SLEEP_INTERVAL)
i += 1
if i % interval_int == 0:
l = []
avg_cpus = []
for k, stat in stats.items():
if len(stat['cpu_samples']) <= 0:
continue
avg_cpu = np.array(stat['cpu_samples']).mean()
c = len(stat['cpu_samples'])
stat['cpu_samples'] = []
if not stat['avg_cpu']:
stat['avg_cpu'] = avg_cpu
else:
stat['avg_cpu'] = (stat['avg_cpu'] * (c + i) + avg_cpu * c) / (c + i + c)
if not stat['min'] or avg_cpu < stat['min']:
stat['min'] = avg_cpu
if not stat['max'] or avg_cpu > stat['max']:
stat['max'] = avg_cpu
msg = 'avg: {1:.2f}%, min: {2:.2f}%, max: {3:.2f}% {0}'.format(os.path.basename(k), stat['avg_cpu'], stat['min'], stat['max'])
l.append((os.path.basename(k), avg_cpu, msg))
avg_cpus.append(avg_cpu)
for name, samples in stat['cpu_samples'].iteritems():
samples = np.array(samples)
avg = samples.mean()
c = samples.size
min_cpu = np.amin(samples)
max_cpu = np.amax(samples)
if stat['min'][name] is None or min_cpu < stat['min'][name]:
stat['min'][name] = min_cpu
if stat['max'][name] is None or max_cpu > stat['max'][name]:
stat['max'][name] = max_cpu
stat['avg'][name] = (stat['avg'][name] * (i - c) + avg * c) / (i)
stat['cpu_samples'][name] = []
msg = 'avg: {1:.2%}, min: {2:.2%}, max: {3:.2%} {0}'.format(os.path.basename(k), stat['avg']['total'], stat['min']['total'], stat['max']['total'])
if args.detailed_times:
for stat_type in ['avg', 'min', 'max']:
msg += '\n {}: {}'.format(stat_type, [name + ':' + str(round(stat[stat_type][name]*100, 2)) for name in cpu_time_names])
l.append((os.path.basename(k), stat['avg']['total'], msg))
l.sort(key= lambda x: -x[1])
for x in l:
print(x[2])
print('avg sum: {0:.2f}%\n'.format(
sum([stat['avg_cpu'] for k, stat in stats.items()])
print('avg sum: {0:.2%} over {1} samples {2} seconds\n'.format(
sum([stat['avg']['total'] for k, stat in stats.items()]), i, i * SLEEP_INTERVAL
))
selfdrive/debug/tuner.py
Executable
+67
View File
@@ -0,0 +1,67 @@
#!/usr/bin/env python
"""
This tool can be used to quickly changes the values in a JSON file used for tuning
Keys like in vim:
- h: decrease by 0.05
- l: increase by 0.05
- k: move pointer up
- j: move pointer down
"""
import tty
import sys
import json
import termios
from collections import OrderedDict
FILENAME = '/data/tuning.json'
def read_tuning():
while True:
try:
return json.loads(open(FILENAME).read())
except:
pass
def main():
dat = json.loads(open(FILENAME, 'r').read())
dat = OrderedDict(sorted(dat.items(), key=lambda i: i[0]))
cur = 0
while True:
sys.stdout.write("\x1Bc")
for i, (k, v) in enumerate(dat.items()):
prefix = "> " if cur == i else " "
print((prefix + k).ljust(20) + "%.2f" % v)
key = sys.stdin.read(1)[0]
write = False
if key == "k":
cur = max(0, cur - 1)
elif key == "j":
cur = min(len(dat.keys()) - 1, cur + 1)
elif key == "l":
dat[dat.keys()[cur]] += 0.05
write = True
elif key == "h":
dat[dat.keys()[cur]] -= 0.05
write = True
elif key == "q":
break
if write:
open(FILENAME, 'w').write(json.dumps(dat))
if __name__ == "__main__":
orig_settings = termios.tcgetattr(sys.stdin)
tty.setcbreak(sys.stdin)
try:
main()
termios.tcsetattr(sys.stdin, termios.TCSADRAIN, orig_settings)
except:
termios.tcsetattr(sys.stdin, termios.TCSADRAIN, orig_settings)
raise
selfdrive/locationd/.gitignore
+2
View File
@@ -0,0 +1,2 @@
ubloxd
ubloxd_test
selfdrive/locationd/kalman/.gitignore
+4
View File
@@ -0,0 +1,4 @@
lane.cpp
gnss.cpp
loc*.cpp
pos_computer*.cpp
selfdrive/loggerd/deleter.py
+39
View File
@@ -0,0 +1,39 @@
#!/usr/bin/env python
import os
import shutil
import threading
from selfdrive.swaglog import cloudlog
from selfdrive.loggerd.config import ROOT
from selfdrive.loggerd.uploader import listdir_by_creation_date
def deleter_thread(exit_event):
while not exit_event.is_set():
statvfs = os.statvfs(ROOT)
available_percent = 100.0 * statvfs.f_bavail / statvfs.f_blocks
if available_percent < 10.0:
# remove the earliest directory we can
dirs = listdir_by_creation_date(ROOT)
for delete_dir in dirs:
delete_path = os.path.join(ROOT, delete_dir)
if any(name.endswith(".lock") for name in os.listdir(delete_path)):
continue
try:
cloudlog.info("deleting %s" % delete_path)
shutil.rmtree(delete_path)
break
except OSError:
cloudlog.exception("issue deleting %s" % delete_path)
exit_event.wait(30)
def main(gctx=None):
deleter_thread(threading.Event())
if __name__ == "__main__":
main()
selfdrive/loggerd/loggerd
BIN
View File
Binary file not shown.
selfdrive/loggerd/tests/Makefile
+35
View File
@@ -0,0 +1,35 @@
CC = clang
CXX = clang++
PHONELIBS = ../../../phonelibs
WARN_FLAGS = -Werror=implicit-function-declaration \
-Werror=incompatible-pointer-types \
-Werror=int-conversion \
-Werror=return-type \
-Werror=format-extra-args \
-Wno-deprecated-declarations
CFLAGS = -std=gnu11 -g -fPIC -O2 $(WARN_FLAGS)
CXXFLAGS = -std=c++11 -g -fPIC -O2 $(WARN_FLAGS)
FFMPEG_LIBS = -lavformat \
-lavcodec \
-lswscale \
-lavutil \
-lz
OBJS = testraw.o \
../RawLogger.o \
../../common/visionipc.o
testraw: $(OBJS)
$(CXX) -fPIC -o '$@' $^ -L/usr/lib $(FFMPEG_LIBS)
%.o: %.cc
@echo "[ CXX ] $@"
$(CXX) $(CXXFLAGS) \
-I../ \
-I../../ \
-I../../../ \
-c -o '$@' '$<'
selfdrive/loggerd/tests/__init__.py
View File
selfdrive/loggerd/tests/fill_eon.py
Executable
+27
View File
@@ -0,0 +1,27 @@
#!/usr/bin/env python
"""Script to fill up EON with fake data"""
import os
from selfdrive.loggerd.config import ROOT
from selfdrive.loggerd.tests.loggerd_tests_common import create_random_file
if __name__ == "__main__":
segment_idx = 0
while True:
seg_name = "1970-01-01--00-00-00--%d" % segment_idx
seg_path = os.path.join(ROOT, seg_name)
print(seg_path)
create_random_file(os.path.join(seg_path, 'fcamera.hevc'), 36)
create_random_file(os.path.join(seg_path, 'rlog.bz2'), 2)
segment_idx += 1
# Fill up to 99 percent
statvfs = os.statvfs(ROOT)
available_percent = 100.0 * statvfs.f_bavail / statvfs.f_blocks
if available_percent < 1.0:
break
selfdrive/loggerd/tests/loggerd_tests_common.py
+53
View File
@@ -0,0 +1,53 @@
import os
import errno
import shutil
import random
import tempfile
import unittest
import selfdrive.loggerd.uploader as uploader
def create_random_file(file_path, size_mb, lock=False):
try:
os.mkdir(os.path.dirname(file_path))
except OSError:
pass
lock_path = file_path + ".lock"
os.close(os.open(lock_path, os.O_CREAT | os.O_EXCL))
chunks = 128
chunk_bytes = int(size_mb * 1024 * 1024 / chunks)
data = os.urandom(chunk_bytes)
with open(file_path, 'wb') as f:
for _ in range(chunks):
f.write(data)
if not lock:
os.remove(lock_path)
class UploaderTestCase(unittest.TestCase):
f_type = "UNKNOWN"
def setUp(self):
self.root = tempfile.mkdtemp()
uploader.ROOT = self.root # Monkey patch root dir
self.seg_num = random.randint(1, 300)
self.seg_format = "2019-04-18--12-52-54--{}"
self.seg_dir = self.seg_format.format(self.seg_num)
def tearDown(self):
try:
shutil.rmtree(self.root)
except OSError as e:
if e.errno != errno.ENOENT:
raise
def make_file_with_data(self, f_dir, fn, size_mb=.1, lock=False):
file_path = os.path.join(self.root, f_dir, fn)
create_random_file(file_path, size_mb)
return file_path
selfdrive/loggerd/tests/test_deleter.py
+104
View File
@@ -0,0 +1,104 @@
import os
import time
import threading
from collections import namedtuple
import selfdrive.loggerd.deleter as deleter
from common.timeout import Timeout, TimeoutException
from loggerd_tests_common import UploaderTestCase
Stats = namedtuple("Stats", ['f_bavail', 'f_blocks'])
fake_stats = Stats(f_bavail=0, f_blocks=10)
def fake_statvfs(d):
return fake_stats
class TestDeleter(UploaderTestCase):
def setUp(self):
self.f_type = "fcamera.hevc"
super(TestDeleter, self).setUp()
deleter.os.statvfs = fake_statvfs
deleter.ROOT = self.root
def tearDown(self):
super(TestDeleter, self).tearDown()
def start_thread(self):
self.end_event = threading.Event()
self.del_thread = threading.Thread(target=deleter.deleter_thread, args=[self.end_event])
self.del_thread.daemon = True
self.del_thread.start()
def join_thread(self):
self.end_event.set()
self.del_thread.join()
def test_delete(self):
f_path = self.make_file_with_data(self.seg_dir, self.f_type, 1)
self.start_thread()
with Timeout(5, "Timeout waiting for file to be deleted"):
while os.path.exists(f_path):
time.sleep(0.5)
self.join_thread()
self.assertFalse(os.path.exists(f_path), "File not deleted")
def test_delete_files_in_create_order(self):
f_path_1 = self.make_file_with_data(self.seg_dir, self.f_type)
time.sleep(1)
self.seg_num += 1
self.seg_dir = self.seg_format.format(self.seg_num)
f_path_2 = self.make_file_with_data(self.seg_dir, self.f_type)
self.start_thread()
with Timeout(5, "Timeout waiting for file to be deleted"):
while os.path.exists(f_path_1) and os.path.exists(f_path_2):
time.sleep(0.5)
self.join_thread()
self.assertFalse(os.path.exists(f_path_1), "Older file not deleted")
self.assertTrue(os.path.exists(f_path_2), "Newer file deleted before older file")
def test_no_delete_when_available_space(self):
global fake_stats
f_path = self.make_file_with_data(self.seg_dir, self.f_type)
fake_stats = Stats(f_bavail=10, f_blocks=10)
self.start_thread()
try:
with Timeout(2, "Timeout waiting for file to be deleted"):
while os.path.exists(f_path):
time.sleep(0.5)
except TimeoutException:
pass
finally:
self.join_thread()
self.assertTrue(os.path.exists(f_path), "File deleted with available space")
def test_no_delete_with_lock_file(self):
f_path = self.make_file_with_data(self.seg_dir, self.f_type, lock=True)
self.start_thread()
try:
with Timeout(2, "Timeout waiting for file to be deleted"):
while os.path.exists(f_path):
time.sleep(0.5)
except TimeoutException:
pass
finally:
self.join_thread()
self.assertTrue(os.path.exists(f_path), "File deleted when locked")
selfdrive/loggerd/tests/testraw.cc
+57
View File
@@ -0,0 +1,57 @@
#include <cstdio>
#include <cstdlib>
#include <unistd.h>
#include <zmq.h>
#include "common/visionipc.h"
#include "common/timing.h"
#include "RawLogger.h"
int main() {
int err;
VisionStream stream;
VisionStreamBufs buf_info;
while (true) {
err = visionstream_init(&stream, VISION_STREAM_YUV, false, &buf_info);
if (err != 0) {
printf("visionstream fail\n");
usleep(100000);
}
break;
}
RawLogger vidlogger("prcamera", buf_info.width, buf_info.height, 20);
vidlogger.Open("o1");
for (int cnt=0; cnt<200; cnt++) {
VIPCBufExtra extra;
VIPSBuf* buf = visionstream_get(&stream, &extra);
if (buf == NULL) {
printf("visionstream get failed\n");
break;
}
if (cnt == 100) {
vidlogger.Rotate("o2", 2);
}
uint8_t *y = (uint8_t*)buf->addr;
uint8_t *u = y + (buf_info.width*buf_info.height);
uint8_t *v = u + (buf_info.width/2)*(buf_info.height/2);
double t1 = millis_since_boot();
vidlogger.LogFrame(cnt, y, u, v, NULL);
double t2 = millis_since_boot();
printf("%d %.2f\n", cnt, (t2-t1));
}
vidlogger.Close();
visionstream_destroy(&stream);
return 0;
}
selfdrive/loggerd/uploader.py
+4 -22
View File
@@ -116,7 +116,10 @@ class Uploader(object):
return
for logname in listdir_by_creation_date(self.root):
path = os.path.join(self.root, logname)
names = os.listdir(path)
try:
names = os.listdir(path)
except OSError:
continue
if any(name.endswith(".lock") for name in names):
continue
@@ -194,26 +197,6 @@ class Uploader(object):
return self.last_resp
def killable_upload(self, key, fn):
self.last_resp = None
self.last_exc = None
self.upload_thread = threading.Thread(target=lambda: self.do_upload(key, fn))
self.upload_thread.start()
self.upload_thread.join()
self.upload_thread = None
return self.last_resp
def abort_upload(self):
thread = self.upload_thread
if thread is None:
return
if not thread.is_alive():
return
raise_on_thread(thread, SystemExit)
thread.join()
def compress(self, key, fn):
# write out the bz2 compress
if fn.endswith("log"):
@@ -246,7 +229,6 @@ class Uploader(object):
success = True
else:
cloudlog.info("uploading %r", fn)
# stat = self.killable_upload(key, fn)
stat = self.normal_upload(key, fn)
if stat is not None and stat.status_code in (200, 201):
cloudlog.event("upload_success", key=key, fn=fn, sz=sz)
selfdrive/manager.py
+3 -1
View File
@@ -42,7 +42,7 @@ def unblock_stdout():
if __name__ == "__main__":
neos_update_required = os.path.isfile("/init.qcom.rc") \
and (not os.path.isfile("/VERSION") or int(open("/VERSION").read()) < 8)
and (not os.path.isfile("/VERSION") or int(open("/VERSION").read()) < 9)
if neos_update_required:
# update continue.sh before updating NEOS
if os.path.isfile(os.path.join(BASEDIR, "scripts", "continue.sh")):
@@ -88,6 +88,7 @@ from selfdrive.loggerd.config import ROOT
managed_processes = {
"thermald": "selfdrive.thermald",
"uploader": "selfdrive.loggerd.uploader",
"deleter": "selfdrive.loggerd.deleter",
"controlsd": "selfdrive.controls.controlsd",
"plannerd": "selfdrive.controls.plannerd",
"radard": "selfdrive.controls.radard",
@@ -127,6 +128,7 @@ persistent_processes = [
'logcatd',
'tombstoned',
'uploader',
'deleter',
'ui',
'gpsd',
'updated',
selfdrive/mapd/default_speeds_generator.py
+26 -2
View File
@@ -1,6 +1,5 @@
#!/usr/bin/env python
import json
import six
DEFAULT_OUTPUT_FILENAME = "default_speeds_by_region.json"
@@ -146,6 +145,31 @@ def main(filename = DEFAULT_OUTPUT_FILENAME):
DE.add_rule({"zone:maxspeed": "DE:rural"}, "100")
DE.add_rule({"zone:maxspeed": "DE:motorway"}, "none")
DE.add_rule({"bicycle_road": "yes"}, "30")
"""
--------------------------------------------------
EE - Estonia
--------------------------------------------------
"""
EE = Country("EE")
countries.append(EE)
""" Default rules """
EE.add_rule({"highway": "motorway"}, "90")
EE.add_rule({"highway": "trunk"}, "90")
EE.add_rule({"highway": "primary"}, "90")
EE.add_rule({"highway": "secondary"}, "50")
EE.add_rule({"highway": "tertiary"}, "50")
EE.add_rule({"highway": "unclassified"}, "90")
EE.add_rule({"highway": "residential"}, "40")
EE.add_rule({"highway": "service"}, "40")
EE.add_rule({"highway": "motorway_link"}, "90")
EE.add_rule({"highway": "trunk_link"}, "70")
EE.add_rule({"highway": "primary_link"}, "70")
EE.add_rule({"highway": "secondary_link"}, "50")
EE.add_rule({"highway": "tertiary_link"}, "50")
EE.add_rule({"highway": "living_street"}, "20")
""" --- DO NOT MODIFY CODE BELOW THIS LINE --- """
@@ -206,7 +230,7 @@ class Country(Region):
def jsonify(self):
ret_dict = {}
ret_dict[self.name] = {}
for r_name, region in six.iteritems(self.regions):
for r_name, region in self.regions.items():
ret_dict[self.name].update(region.jsonify())
ret_dict[self.name]['Default'] = self.rules
return ret_dict
selfdrive/mapd/mapd_helpers.py
+2 -3
View File
@@ -1,4 +1,3 @@
import six
import math
import json
import numpy as np
@@ -92,7 +91,7 @@ def geocode_maxspeed(tags, location_info):
rule_valid = all(
tag_name in tags
and tags[tag_name] == value
for tag_name, value in six.iteritems(rule['tags'])
for tag_name, value in rule['tags'].items()
)
if rule_valid:
max_speed = rule['speed']
@@ -103,7 +102,7 @@ def geocode_maxspeed(tags, location_info):
rule_valid = all(
tag_name in tags
and tags[tag_name] == value
for tag_name, value in six.iteritems(rule['tags'])
for tag_name, value in rule['tags'].items()
)
if rule_valid:
max_speed = rule['speed']
selfdrive/sensord/gpsd
BIN
View File
Binary file not shown.
selfdrive/sensord/sensord
BIN
View File
Binary file not shown.
selfdrive/test/plant/plant.py
+1 -1
View File
@@ -318,7 +318,7 @@ class Plant(object):
msg_data = fix(msg_data, 0xe4)
can_msgs.append([0xe4, 0, msg_data, 2])
Plant.logcan.send(can_list_to_can_capnp(can_msgs).to_bytes())
Plant.logcan.send(can_list_to_can_capnp(can_msgs))
# ******** publish a fake model going straight and fake calibration ********
# note that this is worst case for MPC, since model will delay long mpc by one time step
selfdrive/thermald.py
+2
View File
@@ -279,10 +279,12 @@ def thermald_thread():
params.car_start()
started_ts = sec_since_boot()
started_seen = True
os.system('echo performance > /sys/class/devfreq/soc:qcom,cpubw/governor')
else:
started_ts = None
if off_ts is None:
off_ts = sec_since_boot()
os.system('echo powersave > /sys/class/devfreq/soc:qcom,cpubw/governor')
# shutdown if the battery gets lower than 3%, it's discharging, we aren't running for
# more than a minute but we were running
selfdrive/tombstoned.py
+1 -1
View File
@@ -105,7 +105,7 @@ def main(gctx):
initial_tombstones = set(get_tombstones())
client = Client('https://d3b175702f62402c91ade04d1c547e68:b20d68c813c74f63a7cdf9c4039d8f56@sentry.io/157615',
install_sys_hook=False, transport=HTTPTransport, release=version, tags={'dirty': dirty})
install_sys_hook=False, transport=HTTPTransport, release=version, tags={'dirty': dirty}, string_max_length=10000)
client.user_context({'id': os.environ.get('DONGLE_ID')})
while True:
selfdrive/ui/slplay.c
+13 -13
View File
@@ -38,25 +38,25 @@ uri_player* slplay_create_player_for_uri(const char* uri, char **error) {
result = (*engineInterface)->CreateAudioPlayer(engineInterface, &player.player, &audioSrc, &audioSnk, 0, NULL, NULL);
if (result != SL_RESULT_SUCCESS) {
*error = strdup("Failed to create audio player");
*error = "Failed to create audio player";
return NULL;
}
result = (*(player.player))->Realize(player.player, SL_BOOLEAN_FALSE);
if (result != SL_RESULT_SUCCESS) {
*error = strdup("Failed to realize audio player");
*error = "Failed to realize audio player";
return NULL;
}
result = (*(player.player))->GetInterface(player.player, SL_IID_PLAY, &(player.playInterface));
if (result != SL_RESULT_SUCCESS) {
*error = strdup("Failed to get player interface");
*error = "Failed to get player interface";
return NULL;
}
result = (*(player.playInterface))->SetPlayState(player.playInterface, SL_PLAYSTATE_PAUSED);
if (result != SL_RESULT_SUCCESS) {
*error = strdup("Failed to initialize playstate to SL_PLAYSTATE_PAUSED");
*error = "Failed to initialize playstate to SL_PLAYSTATE_PAUSED";
return NULL;
}
@@ -74,29 +74,29 @@ void slplay_setup(char **error) {
SLEngineOption engineOptions[] = {{SL_ENGINEOPTION_THREADSAFE, SL_BOOLEAN_TRUE}};
result = slCreateEngine(&engine, 1, engineOptions, 0, NULL, NULL);
if (result != SL_RESULT_SUCCESS) {
*error = strdup("Failed to create OpenSL engine");
*error = "Failed to create OpenSL engine";
}
result = (*engine)->Realize(engine, SL_BOOLEAN_FALSE);
if (result != SL_RESULT_SUCCESS) {
*error = strdup("Failed to realize OpenSL engine");
*error = "Failed to realize OpenSL engine";
}
result = (*engine)->GetInterface(engine, SL_IID_ENGINE, &engineInterface);
if (result != SL_RESULT_SUCCESS) {
*error = strdup("Failed to realize OpenSL engine");
*error = "Failed to realize OpenSL engine";
}
const SLInterfaceID ids[1] = {SL_IID_VOLUME};
const SLboolean req[1] = {SL_BOOLEAN_FALSE};
result = (*engineInterface)->CreateOutputMix(engineInterface, &outputMix, 1, ids, req);
if (result != SL_RESULT_SUCCESS) {
*error = strdup("Failed to create output mix");
*error = "Failed to create output mix";
}
result = (*outputMix)->Realize(outputMix, SL_BOOLEAN_FALSE);
if (result != SL_RESULT_SUCCESS) {
*error = strdup("Failed to realize output mix");
*error = "Failed to realize output mix";
}
}
@@ -120,7 +120,7 @@ void slplay_stop (uri_player* player, char **error) {
result = (*playInterface)->SetPlayState(playInterface, SL_PLAYSTATE_PAUSED);
if (result != SL_RESULT_SUCCESS) {
*error = strdup("Failed to set SL_PLAYSTATE_STOPPED");
*error = "Failed to set SL_PLAYSTATE_STOPPED";
return;
}
}
@@ -164,7 +164,7 @@ void slplay_play (const char *uri, bool loop, char **error) {
result = (*playInterface)->SetCallbackEventsMask(playInterface, SL_PLAYEVENT_HEADATEND);
if (result != SL_RESULT_SUCCESS) {
*error = strdup("Failed to set callback event mask");
*error = "Failed to set callback event mask";
return;
}
}
@@ -172,13 +172,13 @@ void slplay_play (const char *uri, bool loop, char **error) {
// Reset the audio player
result = (*playInterface)->ClearMarkerPosition(playInterface);
if (result != SL_RESULT_SUCCESS) {
*error = strdup("Failed to clear marker position");
*error = "Failed to clear marker position";
return;
}
result = (*playInterface)->SetPlayState(playInterface, SL_PLAYSTATE_PAUSED);
result = (*playInterface)->SetPlayState(playInterface, SL_PLAYSTATE_STOPPED);
result = (*playInterface)->SetPlayState(playInterface, SL_PLAYSTATE_PLAYING);
if (result != SL_RESULT_SUCCESS) {
*error = strdup("Failed to set SL_PLAYSTATE_PLAYING");
*error = "Failed to set SL_PLAYSTATE_PLAYING";
}
}
selfdrive/visiond/camera_qcom.c
+2
View File
@@ -1925,6 +1925,8 @@ static void camera_close(CameraState *s) {
LOG("isp release stream: %d", err);
}
}
free(s->eeprom);
}
selfdrive/visiond/model.cc
+1
View File
@@ -90,6 +90,7 @@ ModelData model_eval_frame(ModelState* s, cl_command_queue q,
}
void model_free(ModelState* s) {
free(s->output);
model_input_free(&s->in);
delete s->m;
}