diff --git a/README.md b/README.md
index a82b98dbc2..902781b892 100644
--- a/README.md
+++ b/README.md
@@ -79,6 +79,7 @@ Supported Cars
| Honda | CR-V 2017-18 | Honda Sensing | Yes | Stock | 0mph | 12mph | Bosch |
| Honda | CR-V Hybrid 2019 | All | Yes | Stock | 0mph | 12mph | Bosch |
| Honda | Odyssey 2017-19 | Honda Sensing | Yes | Yes | 25mph1| 0mph | Inverted Nidec |
+| Honda | Passport 2019 | All | Yes | Yes | 25mph1| 12mph | Inverted Nidec |
| Honda | Pilot 2016-18 | Honda Sensing | Yes | Yes | 25mph1| 12mph | Nidec |
| Honda | Pilot 2019 | All | Yes | Yes | 25mph1| 12mph | Inverted Nidec |
| Honda | Ridgeline 2017-19 | Honda Sensing | Yes | Yes | 25mph1| 12mph | Nidec |
@@ -90,7 +91,8 @@ Supported Cars
| Kia | Optima 2019 | SCC + LKAS | Yes | Stock | 0mph | 0mph | Custom6|
| Kia | Sorento 2018 | All | Yes | Stock | 0mph | 0mph | Custom6|
| Kia | Stinger 2018 | SCC + LKAS | Yes | Stock | 0mph | 0mph | Custom6|
-| Lexus | RX Hybrid 2016-18 | All | Yes | Yes2| 0mph | 0mph | Toyota |
+| Lexus | RX Hybrid 2016-19 | All | Yes | Yes2| 0mph | 0mph | Toyota |
+| Subaru | Impreza 2019 | EyeSight | Yes | Stock | 0mph | 0mph | Subaru |
| Toyota | Camry 20184 | All | Yes | Stock | 0mph5 | 0mph | Toyota |
| Toyota | C-HR 2017-184 | All | Yes | Stock | 0mph | 0mph | Toyota |
| Toyota | Corolla 2017-18 | All | Yes | Yes2| 20mph1| 0mph | Toyota |
diff --git a/RELEASES.md b/RELEASES.md
index 48e3b647d5..61d905ce23 100644
--- a/RELEASES.md
+++ b/RELEASES.md
@@ -1,13 +1,25 @@
+Version 0.5.11 (2019-04-17)
+========================
+ * Add support for Subaru
+ * Reduce panda power consumption by 60% when car is off
+ * Fix controlsd lag every 6 minutes. This would sometimes cause disengagements
+ * Fix bug in controls with new angle-offset learner in MPC
+ * Reduce cpu consumption of ubloxd by rewriting it in C++
+ * Improve driver monitoring model and face detection
+ * Improve performance of visiond and ui
+ * Honda Passport 2019 support
+ * Lexus RX Hybrid 2019 support thanks to schomems!
+
Version 0.5.10 (2019-03-19)
========================
- * Self-tuning vehicle parameters: steering offset, tires stiffness and steering ratio
+ * Self-tuning vehicle parameters: steering offset, tire stiffness and steering ratio
* Improve longitudinal control at low speed when lead vehicle harshly decelerates
* Fix panda bug going unexpectedly in DCP mode when EON is connected
* Reduce white panda power consumption by 500mW when EON is disconnected by turning off WIFI
* New Driver Monitoring Model
* Support QR codes for login using comma connect
- * Refactor comma pedal FW and use CRC-8 checksum algorithm for safety. Reflashing pedal is required.
- Please see `#hw-pedal` on [discord](discord.comma.ai) for assistance updating comma pedal.
+ * Refactor comma pedal FW and use CRC-8 checksum algorithm for safety. Reflashing pedal is required.
+ Please see `#hw-pedal` on [discord](discord.comma.ai) for assistance updating comma pedal.
* Additional speed limit rules for Germany thanks to arne182
* Allow negative speed limit offsets
diff --git a/SAFETY.md b/SAFETY.md
index b5800a0d50..3cda811e0c 100644
--- a/SAFETY.md
+++ b/SAFETY.md
@@ -133,6 +133,19 @@ Chrysler/Jeep/Fiat (Lateral only)
units above the actual EPS generated motor torque to ensure limited differences between
commanded and actual torques.
+Subaru (Lateral only)
+------
+
+ - While the system is engaged, steer commands are subject to the same limits used by
+ the stock system.
+
+ - Steering torque is controlled through the 0x122 CAN message and it's limited by the panda firmware and by
+ openpilot to a value between -255 and 255. In addition, the vehicle EPS unit will fault for
+ commands outside the values of -2047 and 2047. A steering torque rate limit is enforced by the panda firmware and by
+ openpilot, so that the commanded steering torque must rise from 0 to max value no faster than
+ 0.41s. Commanded steering torque is gradually limited by the panda firmware and by openpilot if the driver's
+ torque exceeds 60 units in the opposite dicrection to ensure limited applied torque against the
+ driver's will.
**Extra note**: comma.ai strongly discourages the use of openpilot forks with safety code either missing or
not fully meeting the above requirements.
diff --git a/apk/ai.comma.plus.offroad.apk b/apk/ai.comma.plus.offroad.apk
index 342f832c2a..b572a93fab 100644
Binary files a/apk/ai.comma.plus.offroad.apk and b/apk/ai.comma.plus.offroad.apk differ
diff --git a/common/dbc.py b/common/dbc.py
index 6accad43f8..4acfc84adb 100755
--- a/common/dbc.py
+++ b/common/dbc.py
@@ -7,9 +7,9 @@ from collections import namedtuple, defaultdict
def int_or_float(s):
# return number, trying to maintain int format
- try:
- return int(s)
- except ValueError:
+ if s.isdigit():
+ return int(s, 10)
+ else:
return float(s)
DBCSignal = namedtuple(
@@ -21,7 +21,7 @@ class dbc(object):
def __init__(self, fn):
self.name, _ = os.path.splitext(os.path.basename(fn))
with open(fn) as f:
- self.txt = f.read().split("\n")
+ self.txt = f.readlines()
self._warned_addresses = set()
# regexps from https://github.com/ebroecker/canmatrix/blob/master/canmatrix/importdbc.py
@@ -51,7 +51,8 @@ class dbc(object):
dat = bo_regexp.match(l)
if dat is None:
- print "bad BO", l
+ print("bad BO {0}".format(l))
+
name = dat.group(2)
size = int(dat.group(3))
ids = int(dat.group(1), 0) # could be hex
@@ -67,8 +68,9 @@ class dbc(object):
if dat is None:
dat = sgm_regexp.match(l)
go = 1
+
if dat is None:
- print "bad SG", l
+ print("bad SG {0}".format(l))
sgname = dat.group(1)
start_bit = int(dat.group(go+2))
@@ -90,7 +92,8 @@ class dbc(object):
dat = val_regexp.match(l)
if dat is None:
- print "bad VAL", l
+ print("bad VAL {0}".format(l))
+
ids = int(dat.group(1), 0) # could be hex
sgname = dat.group(2)
defvals = dat.group(3)
@@ -208,7 +211,7 @@ class dbc(object):
name = msg[0][0]
if debug:
- print name
+ print(name)
st = x[2].ljust(8, '\x00')
le, be = None, None
@@ -252,7 +255,7 @@ class dbc(object):
tmp = tmp * factor + offset
# if debug:
- # print "%40s %2d %2d %7.2f %s" % (s[0], s[1], s[2], tmp, s[-1])
+ # print("%40s %2d %2d %7.2f %s" % (s[0], s[1], s[2], tmp, s[-1]))
if arr is None:
out[s[0]] = tmp
diff --git a/common/ffi_wrapper.py b/common/ffi_wrapper.py
index fd6b2bf7b3..f41115d1ce 100644
--- a/common/ffi_wrapper.py
+++ b/common/ffi_wrapper.py
@@ -4,10 +4,8 @@ import fcntl
import hashlib
from cffi import FFI
-TMPDIR = "/tmp/ccache"
-
-def ffi_wrap(name, c_code, c_header, tmpdir=TMPDIR, cflags="", libraries=None):
+def ffi_wrap(name, c_code, c_header, tmpdir="/tmp/ccache", cflags="", libraries=None):
if libraries is None:
libraries = []
@@ -24,7 +22,7 @@ def ffi_wrap(name, c_code, c_header, tmpdir=TMPDIR, cflags="", libraries=None):
try:
mod = __import__(cache)
except Exception:
- print "cache miss", cache
+ print("cache miss {0}".format(cache))
compile_code(cache, c_code, c_header, tmpdir, cflags, libraries)
mod = __import__(cache)
finally:
diff --git a/common/transformations/camera.py b/common/transformations/camera.py
index 367c0879af..eafd71c989 100644
--- a/common/transformations/camera.py
+++ b/common/transformations/camera.py
@@ -1,6 +1,7 @@
import numpy as np
import common.transformations.orientation as orient
import cv2
+import math
FULL_FRAME_SIZE = (1164, 874)
W, H = FULL_FRAME_SIZE[0], FULL_FRAME_SIZE[1]
@@ -12,6 +13,17 @@ eon_intrinsics = np.array([
[ 0., FOCAL, H/2.],
[ 0., 0., 1.]])
+
+leon_dcam_intrinsics = np.array([
+ [650, 0, 816/2],
+ [ 0, 650, 612/2],
+ [ 0, 0, 1]])
+
+eon_dcam_intrinsics = np.array([
+ [860, 0, 1152/2],
+ [ 0, 860, 864/2],
+ [ 0, 0, 1]])
+
# aka 'K_inv' aka view_frame_from_camera_frame
eon_intrinsics_inv = np.linalg.inv(eon_intrinsics)
@@ -147,28 +159,44 @@ def transform_img(base_img,
from_intr=eon_intrinsics,
to_intr=eon_intrinsics,
calib_rot_view=None,
- output_size=None):
- cy = from_intr[1,2]
+ output_size=None,
+ pretransform=None,
+ top_hacks=True):
size = base_img.shape[:2]
if not output_size:
output_size = size[::-1]
- h = 1.22
- quadrangle = np.array([[0, cy + 20],
- [size[1]-1, cy + 20],
- [0, size[0]-1],
- [size[1]-1, size[0]-1]], dtype=np.float32)
- quadrangle_norm = np.hstack((normalize(quadrangle, intrinsics=from_intr), np.ones((4,1))))
- quadrangle_world = np.column_stack((h*quadrangle_norm[:,0]/quadrangle_norm[:,1],
- h*np.ones(4),
- h/quadrangle_norm[:,1]))
- rot = orient.rot_from_euler(augment_eulers)
- if calib_rot_view is not None:
- rot = calib_rot_view.dot(rot)
- to_extrinsics = np.hstack((rot.T, -augment_trans[:,None]))
- to_KE = to_intr.dot(to_extrinsics)
- warped_quadrangle_full = np.einsum('jk,ik->ij', to_KE, np.hstack((quadrangle_world, np.ones((4,1)))))
- warped_quadrangle = np.column_stack((warped_quadrangle_full[:,0]/warped_quadrangle_full[:,2],
- warped_quadrangle_full[:,1]/warped_quadrangle_full[:,2])).astype(np.float32)
- M = cv2.getPerspectiveTransform(quadrangle, warped_quadrangle.astype(np.float32))
+
+ cy = from_intr[1,2]
+ def get_M(h=1.22):
+ quadrangle = np.array([[0, cy + 20],
+ [size[1]-1, cy + 20],
+ [0, size[0]-1],
+ [size[1]-1, size[0]-1]], dtype=np.float32)
+ quadrangle_norm = np.hstack((normalize(quadrangle, intrinsics=from_intr), np.ones((4,1))))
+ quadrangle_world = np.column_stack((h*quadrangle_norm[:,0]/quadrangle_norm[:,1],
+ h*np.ones(4),
+ h/quadrangle_norm[:,1]))
+ rot = orient.rot_from_euler(augment_eulers)
+ if calib_rot_view is not None:
+ rot = calib_rot_view.dot(rot)
+ to_extrinsics = np.hstack((rot.T, -augment_trans[:,None]))
+ to_KE = to_intr.dot(to_extrinsics)
+ warped_quadrangle_full = np.einsum('jk,ik->ij', to_KE, np.hstack((quadrangle_world, np.ones((4,1)))))
+ warped_quadrangle = np.column_stack((warped_quadrangle_full[:,0]/warped_quadrangle_full[:,2],
+ warped_quadrangle_full[:,1]/warped_quadrangle_full[:,2])).astype(np.float32)
+ M = cv2.getPerspectiveTransform(quadrangle, warped_quadrangle.astype(np.float32))
+ return M
+
+ M = get_M()
+ if pretransform is not None:
+ M = M.dot(pretransform)
augmented_rgb = cv2.warpPerspective(base_img, M, output_size, borderMode=cv2.BORDER_REPLICATE)
+
+ if top_hacks:
+ cyy = int(math.ceil(to_intr[1,2]))
+ M = get_M(1000)
+ if pretransform is not None:
+ M = M.dot(pretransform)
+ augmented_rgb[:cyy] = cv2.warpPerspective(base_img, M, (output_size[0], cyy), borderMode=cv2.BORDER_REPLICATE)
+
return augmented_rgb
diff --git a/common/transformations/model.py b/common/transformations/model.py
index 2d41d9cebe..0e1d5845a8 100644
--- a/common/transformations/model.py
+++ b/common/transformations/model.py
@@ -32,7 +32,7 @@ model_intrinsics = np.array(
# MED model
-MEDMODEL_INPUT_SIZE = (640, 240)
+MEDMODEL_INPUT_SIZE = (512, 256)
MEDMODEL_YUV_SIZE = (MEDMODEL_INPUT_SIZE[0], MEDMODEL_INPUT_SIZE[1] * 3 // 2)
MEDMODEL_CY = 47.6
diff --git a/launch_chffrplus.sh b/launch_chffrplus.sh
index 41f8a607cf..4b7c4c0369 100755
--- a/launch_chffrplus.sh
+++ b/launch_chffrplus.sh
@@ -19,6 +19,8 @@ function launch {
echo 0-3 > /dev/cpuset/foreground/cpus
echo 0-3 > /dev/cpuset/android/cpus
+ # handle pythonpath
+ ln -s /data/openpilot /data/pythonpath
export PYTHONPATH="$PWD"
# start manager
diff --git a/models/monitoring_model.dlc b/models/monitoring_model.dlc
index 0b9f8060d9..b860a1185a 100644
Binary files a/models/monitoring_model.dlc and b/models/monitoring_model.dlc differ
diff --git a/requirements_openpilot.txt b/requirements_openpilot.txt
index ec525a07e0..880073ee79 100644
--- a/requirements_openpilot.txt
+++ b/requirements_openpilot.txt
@@ -23,7 +23,7 @@ enum34==1.1.6
evdev==0.6.1
fastcluster==1.1.20
filterpy==1.2.4
-hexdump
+hexdump==3.3
ipaddress==1.0.16
json-rpc==1.12.1
libusb1==1.5.0
@@ -33,6 +33,7 @@ nose==1.3.7
numpy==1.11.1
opencv-python==3.4.0.12
pause==0.1.2
+psutil==3.4.2
py==1.4.31
pyOpenSSL==16.0.0
pyasn1-modules==0.0.8
diff --git a/selfdrive/boardd/boardd.py b/selfdrive/boardd/boardd.py
index fb045bb3aa..6863386854 100755
--- a/selfdrive/boardd/boardd.py
+++ b/selfdrive/boardd/boardd.py
@@ -220,7 +220,7 @@ def boardd_proxy_loop(rate=200, address="192.168.2.251"):
# recv @ 100hz
can_msgs = can_recv()
#for m in can_msgs:
- # print "R:",hex(m[0]), str(m[2]).encode("hex")
+ # print("R: {0} {1}".format(hex(m[0]), str(m[2]).encode("hex")))
# publish to logger
# TODO: refactor for speed
@@ -233,7 +233,7 @@ def boardd_proxy_loop(rate=200, address="192.168.2.251"):
if tsc is not None:
cl = can_capnp_to_can_list(tsc.can)
#for m in cl:
- # print "S:",hex(m[0]), str(m[2]).encode("hex")
+ # print("S: {0} {1}".format(hex(m[0]), str(m[2]).encode("hex")))
can_send_many(cl)
rk.keep_time()
diff --git a/selfdrive/can/Makefile b/selfdrive/can/Makefile
index b140aa5f87..1909a7bb93 100644
--- a/selfdrive/can/Makefile
+++ b/selfdrive/can/Makefile
@@ -16,6 +16,7 @@ WARN_FLAGS = -Werror=implicit-function-declaration \
CFLAGS = -std=gnu11 -g -fPIC -O2 $(WARN_FLAGS)
CXXFLAGS = -std=c++11 -g -fPIC -O2 $(WARN_FLAGS)
+LDFLAGS =
ifeq ($(UNAME_S),Darwin)
ZMQ_LIBS = -L/usr/local/lib -lzmq
@@ -28,18 +29,24 @@ else ifeq ($(UNAME_M),x86_64)
else ifeq ($(UNAME_M),aarch64)
ZMQ_FLAGS = -I$(PHONELIBS)/zmq/aarch64/include
ZMQ_LIBS = -L$(PHONELIBS)/zmq/aarch64/lib -l:libzmq.a
- CXXFLAGS += -lgnustl_shared
+ LDFLAGS += -lgnustl_shared
endif
+OBJDIR = obj
+
OPENDBC_PATH := $(shell python -c 'import opendbc; print opendbc.DBC_PATH')
DBC_SOURCES := $(wildcard $(OPENDBC_PATH)/*.dbc)
+DBC_OBJS := $(patsubst $(OPENDBC_PATH)/%.dbc,$(OBJDIR)/%.o,$(DBC_SOURCES))
DBC_CCS := $(patsubst $(OPENDBC_PATH)/%.dbc,dbc_out/%.cc,$(DBC_SOURCES))
+.SECONDARY: $(DBC_CCS)
+
+LIBDBC_OBJS := $(OBJDIR)/dbc.o $(OBJDIR)/parser.o $(OBJDIR)/packer.o
CWD := $(shell pwd)
.PHONY: all
-all: libdbc.so
+all: $(OBJDIR) libdbc.so
include ../common/cereal.mk
@@ -49,22 +56,45 @@ libdbc.so:: ../../cereal/gen/cpp/log.capnp.h
../../cereal/gen/cpp/log.capnp.h:
cd ../../cereal && make
-libdbc.so:: dbc.cc parser.cc packer.cc $(DBC_CCS)
+libdbc.so:: $(LIBDBC_OBJS) $(DBC_OBJS)
+ @echo "[ LINK ] $@"
$(CXX) -fPIC -shared -o '$@' $^ \
- -I. \
- -I../.. \
- $(CXXFLAGS) \
- $(ZMQ_FLAGS) \
- $(ZMQ_LIBS) \
- $(CEREAL_CXXFLAGS) \
- $(CEREAL_LIBS)
-
-dbc_out/%.cc: $(OPENDBC_PATH)/%.dbc process_dbc.py dbc_template.cc
- PYTHONPATH=$(PYTHONPATH):$(CWD)/../../pyextra ./process_dbc.py '$<' '$@'
-
-.PHONY: clean
+ -I. -I../.. \
+ $(CXXFLAGS) \
+ $(LDFLAGS) \
+ $(ZMQ_FLAGS) \
+ $(ZMQ_LIBS) \
+ $(CEREAL_CXXFLAGS) \
+ $(CEREAL_LIBS)
+
+$(OBJDIR)/%.o: %.cc
+ @echo "[ CXX ] $@"
+ $(CXX) -fPIC -c -o '$@' $^ \
+ -I. -I../.. \
+ $(CXXFLAGS) \
+ $(ZMQ_FLAGS) \
+ $(CEREAL_CXXFLAGS) \
+
+$(OBJDIR)/%.o: dbc_out/%.cc
+ @echo "[ CXX ] $@"
+ $(CXX) -fPIC -c -o '$@' $^ \
+ -I. -I../.. \
+ $(CXXFLAGS) \
+ $(ZMQ_FLAGS) \
+ $(CEREAL_CXXFLAGS) \
+
+dbc_out/%.cc: process_dbc.py dbc_template.cc $(OPENDBC_PATH)/%.dbc
+ @echo "[ DBC GEN ] $@"
+ @echo "Missing prereq $?"
+ PYTHONPATH=$(PYTHONPATH):$(CWD)/../../pyextra ./process_dbc.py $(OPENDBC_PATH) dbc_out
+
+$(OBJDIR):
+ mkdir -p $@
+
+.PHONY: clean $(OBJDIR)
clean:
rm -rf libdbc.so*
rm -f dbc_out/*.cc
rm -f dbcs.txt
rm -f dbcs.csv
+ rm -rf $(OBJDIR)/*
diff --git a/selfdrive/can/packer.py b/selfdrive/can/packer.py
index cc669e60c6..0c17c2e4d9 100644
--- a/selfdrive/can/packer.py
+++ b/selfdrive/can/packer.py
@@ -63,5 +63,5 @@ if __name__ == "__main__":
#s = cp.pack_bytes(0xe4, {
# "STEER_TORQUE": -2,
#})
- print [hex(ord(v)) for v in s[1]]
+ print([hex(ord(v)) for v in s[1]])
print(s[1].encode("hex"))
diff --git a/selfdrive/can/parser.py b/selfdrive/can/parser.py
index 8960953eda..3b02531f2e 100644
--- a/selfdrive/can/parser.py
+++ b/selfdrive/can/parser.py
@@ -68,7 +68,7 @@ class CANParser(object):
value_count = libdbc.can_query(self.can, 0, self.p_can_valid, 0, ffi.NULL)
self.can_values = ffi.new("SignalValue[%d]" % value_count)
self.update_vl(0)
- # print "==="
+ # print("===")
def update_vl(self, sec):
@@ -77,12 +77,12 @@ class CANParser(object):
self.can_valid = self.p_can_valid[0]
- # print can_values_len
+ # print(can_values_len)
ret = set()
for i in xrange(can_values_len):
cv = self.can_values[i]
address = cv.address
- # print hex(cv.address), ffi.string(cv.name)
+ # print("{0} {1}".format(hex(cv.address), ffi.string(cv.name)))
name = ffi.string(cv.name)
self.vl[address][name] = cv.value
self.ts[address][name] = cv.ts
@@ -240,11 +240,11 @@ if __name__ == "__main__":
cp = CANParser("toyota_rav4_2017_pt_generated", signals, checks, 0)
- # print cp.vl
+ # print(cp.vl)
while True:
cp.update(int(sec_since_boot()*1e9), True)
- # print cp.vl
+ # print(cp.vl)
print(cp.ts)
print(cp.can_valid)
time.sleep(0.01)
diff --git a/selfdrive/can/plant_can_parser.py b/selfdrive/can/plant_can_parser.py
index 44939747ce..e58906d7a0 100644
--- a/selfdrive/can/plant_can_parser.py
+++ b/selfdrive/can/plant_can_parser.py
@@ -78,7 +78,7 @@ class CANParser(object):
msg_vl = fix(ck_portion, msg)
# compare recalculated vs received checksum
if msg_vl != cdat:
- print "CHECKSUM FAIL: " + hex(msg)
+ print("CHECKSUM FAIL: {0}".format(hex(msg)))
self.ck[msg] = False
self.ok[msg] = False
# counter check
@@ -87,13 +87,13 @@ class CANParser(object):
cn = out["COUNTER"]
# check counter validity if it's a relevant message
if cn != ((self.cn[msg] + 1) % 4) and msg in self.msgs_ck and "COUNTER" in out.keys():
- #print hex(msg), "FAILED COUNTER!"
+ #print("FAILED COUNTER: {0}".format(hex(msg)()
self.cn_vl[msg] += 1 # counter check failed
else:
self.cn_vl[msg] -= 1 # counter check passed
# message status is invalid if we received too many wrong counter values
if self.cn_vl[msg] >= cn_vl_max:
- print "COUNTER WRONG: " + hex(msg)
+ print("COUNTER WRONG: {0}".format(hex(msg)))
self.ok[msg] = False
# update msg time stamps and counter value
@@ -118,7 +118,7 @@ class CANParser(object):
self.can_valid = True
if False in self.ok.values():
- #print "CAN INVALID!"
+ #print("CAN INVALID!")
self.can_valid = False
return msgs_upd
diff --git a/selfdrive/can/process_dbc.py b/selfdrive/can/process_dbc.py
index 810947c8ef..61cad32bc9 100755
--- a/selfdrive/can/process_dbc.py
+++ b/selfdrive/can/process_dbc.py
@@ -1,5 +1,6 @@
#!/usr/bin/env python
import os
+import glob
import sys
import jinja2
@@ -7,62 +8,82 @@ import jinja2
from collections import Counter
from common.dbc import dbc
-if len(sys.argv) != 3:
- print "usage: %s dbc_path struct_path" % (sys.argv[0],)
- sys.exit(0)
-
-dbc_fn = sys.argv[1]
-out_fn = sys.argv[2]
-
-template_fn = os.path.join(os.path.dirname(__file__), "dbc_template.cc")
-
-can_dbc = dbc(dbc_fn)
-
-with open(template_fn, "r") as template_f:
- template = jinja2.Template(template_f.read(), trim_blocks=True, lstrip_blocks=True)
-
-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(can_dbc.msgs.iteritems()) 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(def_vals.iteritems())]
-
-if can_dbc.name.startswith("honda") or can_dbc.name.startswith("acura"):
- checksum_type = "honda"
- checksum_size = 4
-elif can_dbc.name.startswith("toyota") or can_dbc.name.startswith("lexus"):
- checksum_type = "toyota"
- checksum_size = 8
-else:
- checksum_type = None
-
-for address, msg_name, msg_size, sigs in msgs:
- for sig in sigs:
- if checksum_type is not None and sig.name == "CHECKSUM":
- if sig.size != checksum_size:
- sys.exit("CHECKSUM is not %d bits longs %s" % (checksum_size, msg_name))
- if checksum_type == "honda" and sig.start_bit % 8 != 3:
- sys.exit("CHECKSUM starts at wrong bit %s" % msg_name)
- if checksum_type == "toyota" and sig.start_bit % 8 != 7:
- sys.exit("CHECKSUM starts at wrong bit %s" % msg_name)
- if checksum_type == "honda" and sig.name == "COUNTER":
- if sig.size != 2:
- sys.exit("COUNTER is not 2 bits longs %s" % msg_name)
- if sig.start_bit % 8 != 5:
- sys.exit("COUNTER starts at wrong bit %s" % msg_name)
- if address in [0x200, 0x201]:
- if sig.name == "COUNTER_PEDAL" and sig.size != 4:
- sys.exit("PEDAL COUNTER is not 4 bits longs %s" % msg_name)
- if sig.name == "CHECKSUM_PEDAL" and sig.size != 8:
- sys.exit("PEDAL CHECKSUM is not 8 bits longs %s" % msg_name)
-
-# Fail on duplicate message names
-c = Counter([msg_name for address, msg_name, msg_size, sigs in msgs])
-for name, count in c.items():
- if count > 1:
- sys.exit("Duplicate message name in DBC file %s" % name)
-
-parser_code = template.render(dbc=can_dbc, checksum_type=checksum_type, msgs=msgs, def_vals=def_vals, len=len)
-
-with open(out_fn, "w") as out_f:
- out_f.write(parser_code)
+def main():
+ if len(sys.argv) != 3:
+ print "usage: %s dbc_directory output_directory" % (sys.argv[0],)
+ sys.exit(0)
+
+ dbc_dir = sys.argv[1]
+ out_dir = sys.argv[2]
+
+ template_fn = os.path.join(os.path.dirname(__file__), "dbc_template.cc")
+ template_mtime = os.path.getmtime(template_fn)
+
+ this_file_mtime = os.path.getmtime(__file__)
+
+ with open(template_fn, "r") as template_f:
+ template = jinja2.Template(template_f.read(), trim_blocks=True, lstrip_blocks=True)
+
+ for dbc_path in glob.iglob(os.path.join(dbc_dir, "*.dbc")):
+ dbc_mtime = os.path.getmtime(dbc_path)
+ dbc_fn = os.path.split(dbc_path)[1]
+ dbc_name = os.path.splitext(dbc_fn)[0]
+ can_dbc = dbc(dbc_path)
+ out_fn = os.path.join(os.path.dirname(__file__), out_dir, dbc_name + ".cc")
+ if os.path.exists(out_fn):
+ out_mtime = os.path.getmtime(out_fn)
+ else:
+ out_mtime = 0
+
+ if dbc_mtime < out_mtime and template_mtime < out_mtime and this_file_mtime < out_mtime:
+ continue #skip output is newer than template and dbc
+
+ msgs = [(address, msg_name, msg_size, sorted(msg_sigs, key=lambda s: s.name not in ("COUNTER", "CHECKSUM"))) # process counter and checksums first
+ for address, ((msg_name, msg_size), msg_sigs) in sorted(can_dbc.msgs.iteritems()) 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(def_vals.iteritems())]
+
+ if can_dbc.name.startswith("honda") or can_dbc.name.startswith("acura"):
+ checksum_type = "honda"
+ checksum_size = 4
+ elif can_dbc.name.startswith("toyota") or can_dbc.name.startswith("lexus"):
+ checksum_type = "toyota"
+ checksum_size = 8
+ else:
+ checksum_type = None
+
+ for address, msg_name, msg_size, sigs in msgs:
+ for sig in sigs:
+ if checksum_type is not None and sig.name == "CHECKSUM":
+ if sig.size != checksum_size:
+ sys.exit("CHECKSUM is not %d bits longs %s" % (checksum_size, msg_name))
+ if checksum_type == "honda" and sig.start_bit % 8 != 3:
+ sys.exit("CHECKSUM starts at wrong bit %s" % msg_name)
+ if checksum_type == "toyota" and sig.start_bit % 8 != 7:
+ sys.exit("CHECKSUM starts at wrong bit %s" % msg_name)
+ if checksum_type == "honda" and sig.name == "COUNTER":
+ if sig.size != 2:
+ sys.exit("COUNTER is not 2 bits longs %s" % msg_name)
+ if sig.start_bit % 8 != 5:
+ sys.exit("COUNTER starts at wrong bit %s" % msg_name)
+ if address in [0x200, 0x201]:
+ if sig.name == "COUNTER_PEDAL" and sig.size != 4:
+ sys.exit("PEDAL COUNTER is not 4 bits longs %s" % msg_name)
+ if sig.name == "CHECKSUM_PEDAL" and sig.size != 8:
+ sys.exit("PEDAL CHECKSUM is not 8 bits longs %s" % msg_name)
+
+ # Fail on duplicate message names
+ c = Counter([msg_name for address, msg_name, msg_size, sigs in msgs])
+ for name, count in c.items():
+ if count > 1:
+ sys.exit("Duplicate message name in DBC file %s" % name)
+
+ parser_code = template.render(dbc=can_dbc, checksum_type=checksum_type, msgs=msgs, def_vals=def_vals, len=len)
+
+
+ with open(out_fn, "w") as out_f:
+ out_f.write(parser_code)
+
+if __name__ == '__main__':
+ main()
diff --git a/selfdrive/car/chrysler/chryslercan.py b/selfdrive/car/chrysler/chryslercan.py
index d225420afb..761b44b15e 100644
--- a/selfdrive/car/chrysler/chryslercan.py
+++ b/selfdrive/car/chrysler/chryslercan.py
@@ -44,12 +44,6 @@ def calc_checksum(data):
def make_can_msg(addr, dat):
return [addr, 0, dat, 0]
-def create_lkas_heartbit(packer, lkas_status_ok):
- # LKAS_HEARTBIT 0x2d9 (729) Lane-keeping heartbeat.
- values = {
- "LKAS_STATUS_OK": lkas_status_ok
- }
- return packer.make_can_msg("LKAS_HEARTBIT", 0, values)
def create_lkas_hud(packer, gear, lkas_active, hud_alert, hud_count, lkas_car_model):
# LKAS_HUD 0x2a6 (678) Controls what lane-keeping icon is displayed.
diff --git a/selfdrive/car/chrysler/chryslercan_test.py b/selfdrive/car/chrysler/chryslercan_test.py
index f3c2a54f04..524a79f7ae 100644
--- a/selfdrive/car/chrysler/chryslercan_test.py
+++ b/selfdrive/car/chrysler/chryslercan_test.py
@@ -14,12 +14,6 @@ class TestChryslerCan(unittest.TestCase):
self.assertEqual(0x75, chryslercan.calc_checksum([0x01, 0x20]))
self.assertEqual(0xcc, chryslercan.calc_checksum([0x14, 0, 0, 0, 0x20]))
- def test_heartbit(self):
- packer = CANPacker('chrysler_pacifica_2017_hybrid')
- self.assertEqual(
- [0x2d9, 0, '0000000820'.decode('hex'), 0],
- chryslercan.create_lkas_heartbit(packer, 0x820))
-
def test_hud(self):
packer = CANPacker('chrysler_pacifica_2017_hybrid')
self.assertEqual(
diff --git a/selfdrive/car/chrysler/interface.py b/selfdrive/car/chrysler/interface.py
index 7e96ebc2b5..e0551ef0cf 100755
--- a/selfdrive/car/chrysler/interface.py
+++ b/selfdrive/car/chrysler/interface.py
@@ -122,7 +122,7 @@ class CarInterface(object):
ret.brakeMaxV = [1., 0.8]
ret.enableCamera = not check_ecu_msgs(fingerprint, ECU.CAM)
- print "ECU Camera Simulated: ", ret.enableCamera
+ print("ECU Camera Simulated: {0}".format(ret.enableCamera))
ret.openpilotLongitudinalControl = False
ret.steerLimitAlert = True
diff --git a/selfdrive/car/chrysler/radar_interface.py b/selfdrive/car/chrysler/radar_interface.py
index 60eaa4566a..e5fe7437f7 100755
--- a/selfdrive/car/chrysler/radar_interface.py
+++ b/selfdrive/car/chrysler/radar_interface.py
@@ -104,4 +104,4 @@ if __name__ == "__main__":
while 1:
ret = RI.update()
print(chr(27) + "[2J") # clear screen
- print ret
+ print(ret)
diff --git a/selfdrive/car/ford/radar_interface.py b/selfdrive/car/ford/radar_interface.py
index 2acea9d708..970b411ff2 100755
--- a/selfdrive/car/ford/radar_interface.py
+++ b/selfdrive/car/ford/radar_interface.py
@@ -90,4 +90,4 @@ if __name__ == "__main__":
while 1:
ret = RI.update()
print(chr(27) + "[2J")
- print ret
+ print(ret)
diff --git a/selfdrive/car/gm/radar_interface.py b/selfdrive/car/gm/radar_interface.py
index 6af0d3f8fa..3d72ff7ab2 100755
--- a/selfdrive/car/gm/radar_interface.py
+++ b/selfdrive/car/gm/radar_interface.py
@@ -124,4 +124,4 @@ if __name__ == "__main__":
while 1:
ret = RI.update()
print(chr(27) + "[2J")
- print ret
+ print(ret)
diff --git a/selfdrive/car/honda/carcontroller.py b/selfdrive/car/honda/carcontroller.py
index 9325e89646..ea69daba76 100644
--- a/selfdrive/car/honda/carcontroller.py
+++ b/selfdrive/car/honda/carcontroller.py
@@ -124,7 +124,7 @@ class CarController(object):
if CS.CP.radarOffCan:
snd_beep = snd_beep if snd_beep != 0 else snd_chime
- #print chime, alert_id, hud_alert
+ #print("{0} {1} {2}".format(chime, alert_id, hud_alert))
fcw_display, steer_required, acc_alert = process_hud_alert(hud_alert)
hud = HUDData(int(pcm_accel), int(round(hud_v_cruise)), 1, hud_car,
diff --git a/selfdrive/car/honda/interface.py b/selfdrive/car/honda/interface.py
index 4072b03548..6c4b4a7672 100755
--- a/selfdrive/car/honda/interface.py
+++ b/selfdrive/car/honda/interface.py
@@ -196,10 +196,10 @@ class CarInterface(object):
tire_stiffness_factor = 1.
# Civic at comma has modified steering FW, so different tuning for the Neo in that car
is_fw_modified = os.getenv("DONGLE_ID") in ['99c94dc769b5d96e']
- ret.steerKpV, ret.steerKiV = [[0.4], [0.12]] if is_fw_modified else [[0.8], [0.24]]
if is_fw_modified:
- tire_stiffness_factor = 0.9
ret.steerKf = 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.]
diff --git a/selfdrive/car/honda/radar_interface.py b/selfdrive/car/honda/radar_interface.py
index 7d30265c2f..aa963df9a8 100755
--- a/selfdrive/car/honda/radar_interface.py
+++ b/selfdrive/car/honda/radar_interface.py
@@ -101,4 +101,4 @@ if __name__ == "__main__":
while 1:
ret = RI.update()
print(chr(27) + "[2J")
- print ret
+ print(ret)
diff --git a/selfdrive/car/honda/values.py b/selfdrive/car/honda/values.py
index f106ccab34..22fb53d7c0 100644
--- a/selfdrive/car/honda/values.py
+++ b/selfdrive/car/honda/values.py
@@ -121,12 +121,13 @@ FINGERPRINTS = {
CAR.PILOT: [{
57: 3, 145: 8, 228: 5, 229: 4, 308: 5, 316: 8, 334: 8, 339: 7, 342: 6, 344: 8, 379: 8, 380: 8, 392: 6, 399: 7, 419: 8, 420: 8, 422: 8, 425: 8, 426: 8, 427: 3, 432: 7, 463: 8, 464: 8, 476: 4, 490: 8, 506: 8, 507: 1, 512: 6, 513: 6, 538: 3, 542: 7, 545: 5, 546: 3, 597: 8, 660: 8, 773: 7, 777: 8, 780: 8, 795: 8, 800: 8, 804: 8, 808: 8, 819: 7, 821: 5, 829: 5, 837: 5, 856: 7, 871: 8, 882: 2, 884: 7, 891: 8, 892: 8, 923: 2, 929: 8, 963: 8, 965: 8, 966: 8, 967: 8, 983: 8, 985: 3, 1027: 5, 1029: 8, 1036: 8, 1039: 8, 1064: 7, 1088: 8, 1089: 8, 1108: 8, 1125: 8, 1296: 8, 1424: 5, 1600: 5, 1601: 8, 1612: 5, 1613: 5, 1616: 5, 1618: 5, 1668: 5
}],
+ # this fingerprint also includes the Passport 2019
CAR.PILOT_2019: [{
- 57: 3, 145: 8, 228: 5, 308: 5, 316: 8, 334: 8, 342: 6, 344: 8, 379: 8, 380: 8, 399: 7, 411: 5, 419: 8, 420: 8, 422: 8, 425: 8, 426: 8, 427: 3, 432: 7, 463: 8, 464: 8, 476: 4, 490: 8, 506: 8, 538: 3, 542: 7, 545: 5, 546: 3, 597: 8, 660: 8, 773: 7, 777: 8, 780: 8, 795: 8, 800: 8, 804: 8, 808: 8, 817: 4, 819: 7, 821: 5, 825: 4, 829: 5, 837: 5, 856: 7, 871: 8, 881: 8, 882: 2, 884: 7, 891: 8, 892: 8, 923: 2, 927: 8, 929: 8, 983: 8, 985: 3, 1029: 8, 1052: 8, 1064: 7, 1088: 8, 1089: 8, 1092: 1, 1108: 8, 1110: 8, 1125: 8, 1296: 8, 1424: 5, 1445: 8, 1600: 5, 1601: 8, 1612: 5, 1613: 5, 1614: 5, 1615: 8, 1616: 5, 1617: 8, 1618: 5, 1623: 5, 1668: 5
+ 57: 3, 145: 8, 228: 5, 308: 5, 316: 8, 334: 8, 342: 6, 344: 8, 379: 8, 380: 8, 399: 7, 411: 5, 419: 8, 420: 8, 422: 8, 425: 8, 426: 8, 427: 3, 432: 7, 463: 8, 464: 8, 476: 4, 490: 8, 506: 8, 512: 6, 513: 6, 538: 3, 542: 7, 545: 5, 546: 3, 597: 8, 660: 8, 773: 7, 777: 8, 780: 8, 795: 8, 800: 8, 804: 8, 808: 8, 817: 4, 819: 7, 821: 5, 825: 4, 829: 5, 837: 5, 856: 7, 871: 8, 881: 8, 882: 2, 884: 7, 891: 8, 892: 8, 923: 2, 927: 8, 929: 8, 983: 8, 985: 3, 1029: 8, 1052: 8, 1064: 7, 1088: 8, 1089: 8, 1092: 1, 1108: 8, 1110: 8, 1125: 8, 1296: 8, 1424: 5, 1445: 8, 1600: 5, 1601: 8, 1612: 5, 1613: 5, 1614: 5, 1615: 8, 1616: 5, 1617: 8, 1618: 5, 1623: 5, 1668: 5
},
# 2019 Pilot EX-L
{
- 57: 3, 145: 8, 228: 5, 229: 4, 308: 5, 316: 8, 339: 7, 342: 6, 344: 8, 380: 8, 392: 6, 399: 7, 411: 5, 419: 8, 420: 8, 422: 8, 425: 8, 426: 8, 427: 3, 432: 7, 464: 8, 476: 4, 490: 8, 506: 8, 542: 7, 545: 5, 546: 3, 597: 8, 660: 8, 773: 7, 777: 8, 780: 8, 795: 8, 800: 8, 804: 8, 808: 8, 817: 4, 819: 7, 821: 5, 829: 5, 871: 8, 881: 8, 882: 2, 884: 7, 891: 8, 892: 8, 923: 2, 927: 8, 929: 8, 963: 8, 965: 8, 966: 8, 967: 8, 983: 8, 985: 3, 1027: 5, 1029: 8, 1039: 8, 1064: 7, 1088: 8, 1089: 8, 1092: 1, 1108: 8, 1125: 8, 1296: 8, 1424: 5, 1445: 8, 1600: 5, 1601: 8, 1612: 5, 1613: 5, 1616: 5, 1617: 8, 1618: 5, 1623: 5, 1668: 5
+ 57: 3, 145: 8, 228: 5, 229: 4, 308: 5, 316: 8, 339: 7, 342: 6, 344: 8, 380: 8, 392: 6, 399: 7, 411: 5, 419: 8, 420: 8, 422: 8, 425: 8, 426: 8, 427: 3, 432: 7, 464: 8, 476: 4, 490: 8, 506: 8, 512: 6, 513: 6, 542: 7, 545: 5, 546: 3, 597: 8, 660: 8, 773: 7, 777: 8, 780: 8, 795: 8, 800: 8, 804: 8, 808: 8, 817: 4, 819: 7, 821: 5, 829: 5, 871: 8, 881: 8, 882: 2, 884: 7, 891: 8, 892: 8, 923: 2, 927: 8, 929: 8, 963: 8, 965: 8, 966: 8, 967: 8, 983: 8, 985: 3, 1027: 5, 1029: 8, 1039: 8, 1064: 7, 1088: 8, 1089: 8, 1092: 1, 1108: 8, 1125: 8, 1296: 8, 1424: 5, 1445: 8, 1600: 5, 1601: 8, 1612: 5, 1613: 5, 1616: 5, 1617: 8, 1618: 5, 1623: 5, 1668: 5
}],
# Ridgeline w/ Added Comma Pedal Support (512L & 513L)
CAR.RIDGELINE: [{
diff --git a/selfdrive/car/hyundai/interface.py b/selfdrive/car/hyundai/interface.py
index d7fc2bd394..6186565584 100644
--- a/selfdrive/car/hyundai/interface.py
+++ b/selfdrive/car/hyundai/interface.py
@@ -68,13 +68,13 @@ class CarInterface(object):
rotationalInertia_civic = 2500
tireStiffnessFront_civic = 192150
tireStiffnessRear_civic = 202500
+ tire_stiffness_factor = 1.
ret.steerActuatorDelay = 0.1 # Default delay
- tire_stiffness_factor = 1.
+ ret.steerRateCost = 0.5
if candidate == CAR.SANTA_FE:
ret.steerKf = 0.00005
- ret.steerRateCost = 0.5
ret.mass = 3982 * CV.LB_TO_KG + std_cargo
ret.wheelbase = 2.766
diff --git a/selfdrive/car/hyundai/radar_interface.py b/selfdrive/car/hyundai/radar_interface.py
index 96159fd87d..3e458df576 100644
--- a/selfdrive/car/hyundai/radar_interface.py
+++ b/selfdrive/car/hyundai/radar_interface.py
@@ -21,4 +21,4 @@ if __name__ == "__main__":
while 1:
ret = RI.update()
print(chr(27) + "[2J")
- print ret
+ print(ret)
diff --git a/selfdrive/car/mock/radar_interface.py b/selfdrive/car/mock/radar_interface.py
index ec042d1794..a99494b1ad 100755
--- a/selfdrive/car/mock/radar_interface.py
+++ b/selfdrive/car/mock/radar_interface.py
@@ -13,7 +13,6 @@ class RadarInterface(object):
ret = car.RadarState.new_message()
time.sleep(0.05) # radard runs on RI updates
-
return ret
if __name__ == "__main__":
@@ -21,4 +20,4 @@ if __name__ == "__main__":
while 1:
ret = RI.update()
print(chr(27) + "[2J")
- print ret
+ print(ret)
diff --git a/selfdrive/car/subaru/carcontroller.py b/selfdrive/car/subaru/carcontroller.py
new file mode 100644
index 0000000000..33a0fde868
--- /dev/null
+++ b/selfdrive/car/subaru/carcontroller.py
@@ -0,0 +1,76 @@
+#from common.numpy_fast import clip
+from common.realtime import sec_since_boot
+from selfdrive.boardd.boardd import can_list_to_can_capnp
+from selfdrive.car import apply_std_steer_torque_limits
+from selfdrive.car.subaru import subarucan
+from selfdrive.car.subaru.values import CAR, DBC
+from selfdrive.can.packer import CANPacker
+
+
+class CarControllerParams():
+ def __init__(self, car_fingerprint):
+ self.STEER_MAX = 2047 # max_steer 4095
+ self.STEER_STEP = 2 # how often we update the steer cmd
+ self.STEER_DELTA_UP = 50 # torque increase per refresh, 0.8s to max
+ self.STEER_DELTA_DOWN = 70 # torque decrease per refresh
+ if car_fingerprint == CAR.IMPREZA:
+ self.STEER_DRIVER_ALLOWANCE = 60 # allowed driver torque before start limiting
+ self.STEER_DRIVER_MULTIPLIER = 10 # weight driver torque heavily
+ self.STEER_DRIVER_FACTOR = 1 # from dbc
+
+
+
+class CarController(object):
+ def __init__(self, car_fingerprint):
+ self.start_time = sec_since_boot()
+ self.lkas_active = False
+ self.steer_idx = 0
+ self.apply_steer_last = 0
+ self.car_fingerprint = car_fingerprint
+ self.es_distance_cnt = -1
+ self.es_lkas_cnt = -1
+
+ # Setup detection helper. Routes commands to
+ # an appropriate CAN bus number.
+ self.params = CarControllerParams(car_fingerprint)
+ print(DBC)
+ self.packer = CANPacker(DBC[car_fingerprint]['pt'])
+
+ def update(self, sendcan, enabled, CS, frame, actuators, pcm_cancel_cmd, visual_alert):
+ """ Controls thread """
+
+ P = self.params
+
+ # Send CAN commands.
+ can_sends = []
+
+ ### STEER ###
+
+ if (frame % P.STEER_STEP) == 0:
+
+ final_steer = actuators.steer if enabled else 0.
+ apply_steer = int(round(final_steer * P.STEER_MAX))
+
+ # limits due to driver torque
+
+ apply_steer = int(round(apply_steer))
+ apply_steer = apply_std_steer_torque_limits(apply_steer, self.apply_steer_last, CS.steer_torque_driver, P)
+
+ lkas_enabled = enabled and not CS.steer_not_allowed
+
+ if not lkas_enabled:
+ apply_steer = 0
+
+ can_sends.append(subarucan.create_steering_control(self.packer, CS.CP.carFingerprint, apply_steer, frame, P.STEER_STEP))
+
+ self.apply_steer_last = apply_steer
+
+ if self.es_distance_cnt != CS.es_distance_msg["Counter"]:
+ can_sends.append(subarucan.create_es_distance(self.packer, CS.es_distance_msg, pcm_cancel_cmd))
+ self.es_distance_cnt = CS.es_distance_msg["Counter"]
+
+ if self.es_lkas_cnt != CS.es_lkas_msg["Counter"]:
+ can_sends.append(subarucan.create_es_lkas(self.packer, CS.es_lkas_msg, visual_alert))
+ self.es_lkas_cnt = CS.es_lkas_msg["Counter"]
+
+ sendcan.send(can_list_to_can_capnp(can_sends, msgtype='sendcan').to_bytes())
diff --git a/selfdrive/car/subaru/carstate.py b/selfdrive/car/subaru/carstate.py
index e12f5f957f..38d2fe4794 100644
--- a/selfdrive/car/subaru/carstate.py
+++ b/selfdrive/car/subaru/carstate.py
@@ -1,3 +1,4 @@
+import copy
import numpy as np
from common.kalman.simple_kalman import KF1D
from selfdrive.config import Conversions as CV
@@ -32,12 +33,49 @@ def get_powertrain_can_parser(CP):
("Dashlights", 10),
("CruiseControl", 20),
("Wheel_Speeds", 50),
- ("Steering_Torque", 100),
+ ("Steering_Torque", 50),
("BodyInfo", 10),
]
return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 0)
+def get_camera_can_parser(CP):
+ signals = [
+ ("Cruise_Set_Speed", "ES_DashStatus", 0),
+
+ ("Counter", "ES_Distance", 0),
+ ("Signal1", "ES_Distance", 0),
+ ("Signal2", "ES_Distance", 0),
+ ("Main", "ES_Distance", 0),
+ ("Signal3", "ES_Distance", 0),
+
+ ("Checksum", "ES_LKAS_State", 0),
+ ("Counter", "ES_LKAS_State", 0),
+ ("Keep_Hands_On_Wheel", "ES_LKAS_State", 0),
+ ("Empty_Box", "ES_LKAS_State", 0),
+ ("Signal1", "ES_LKAS_State", 0),
+ ("LKAS_ACTIVE", "ES_LKAS_State", 0),
+ ("Signal2", "ES_LKAS_State", 0),
+ ("Backward_Speed_Limit_Menu", "ES_LKAS_State", 0),
+ ("LKAS_ENABLE_3", "ES_LKAS_State", 0),
+ ("Signal3", "ES_LKAS_State", 0),
+ ("LKAS_ENABLE_2", "ES_LKAS_State", 0),
+ ("Signal4", "ES_LKAS_State", 0),
+ ("FCW_Cont_Beep", "ES_LKAS_State", 0),
+ ("FCW_Repeated_Beep", "ES_LKAS_State", 0),
+ ("Throttle_Management_Activated", "ES_LKAS_State", 0),
+ ("Traffic_light_Ahead", "ES_LKAS_State", 0),
+ ("Right_Depart", "ES_LKAS_State", 0),
+ ("Signal5", "ES_LKAS_State", 0),
+
+ ]
+
+ checks = [
+ ("ES_DashStatus", 10),
+ ]
+
+ return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 2)
+
class CarState(object):
def __init__(self, CP):
# initialize can parser
@@ -60,9 +98,11 @@ class CarState(object):
K=np.matrix([[0.12287673], [0.29666309]]))
self.v_ego = 0.
- def update(self, cp):
+ def update(self, cp, cp_cam):
+
+ self.can_valid = cp.can_valid
+ self.cam_can_valid = cp_cam.can_valid
- self.can_valid = True
self.pedal_gas = cp.vl["Throttle"]['Throttle_Pedal']
self.brake_pressure = cp.vl["Brake_Pedal"]['Brake_Pedal']
self.user_gas_pressed = self.pedal_gas > 0
@@ -74,6 +114,8 @@ 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
+
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
@@ -101,4 +143,5 @@ class CarState(object):
cp.vl["BodyInfo"]['DOOR_OPEN_FR'],
cp.vl["BodyInfo"]['DOOR_OPEN_FL']])
-
+ self.es_distance_msg = copy.copy(cp_cam.vl["ES_Distance"])
+ self.es_lkas_msg = copy.copy(cp_cam.vl["ES_LKAS_State"])
diff --git a/selfdrive/car/subaru/interface.py b/selfdrive/car/subaru/interface.py
index 93a1ac62b2..ef3da2f398 100644
--- a/selfdrive/car/subaru/interface.py
+++ b/selfdrive/car/subaru/interface.py
@@ -5,7 +5,7 @@ from selfdrive.config import Conversions as CV
from selfdrive.controls.lib.drive_helpers import create_event, EventTypes as ET
from selfdrive.controls.lib.vehicle_model import VehicleModel
from selfdrive.car.subaru.values import CAR
-from selfdrive.car.subaru.carstate import CarState, get_powertrain_can_parser
+from selfdrive.car.subaru.carstate import CarState, get_powertrain_can_parser, get_camera_can_parser
try:
from selfdrive.car.subaru.carcontroller import CarController
@@ -20,11 +20,15 @@ class CarInterface(object):
self.frame = 0
self.can_invalid_count = 0
self.acc_active_prev = 0
+ self.gas_pressed_prev = False
# *** init the major players ***
self.CS = CarState(CP)
self.VM = VehicleModel(CP)
self.pt_cp = get_powertrain_can_parser(CP)
+ self.cam_cp = get_camera_can_parser(CP)
+
+ self.gas_pressed_prev = False
# sending if read only is False
if sendcan is not None:
@@ -47,8 +51,9 @@ class CarInterface(object):
ret.carFingerprint = candidate
ret.safetyModel = car.CarParams.SafetyModels.subaru
- ret.enableCruise = False
+ ret.enableCruise = True
ret.steerLimitAlert = True
+
ret.enableCamera = True
std_cargo = 136
@@ -116,7 +121,8 @@ class CarInterface(object):
def update(self, c):
self.pt_cp.update(int(sec_since_boot() * 1e9), False)
- self.CS.update(self.pt_cp)
+ self.cam_cp.update(int(sec_since_boot() * 1e9), False)
+ self.CS.update(self.pt_cp, self.cam_cp)
# create message
ret = car.CarState.new_message()
@@ -140,11 +146,19 @@ class CarInterface(object):
ret.steeringPressed = self.CS.steer_override
ret.steeringTorque = self.CS.steer_torque_driver
+ ret.gas = self.CS.pedal_gas / 255.
+ ret.gasPressed = self.CS.user_gas_pressed
+
# cruise state
+ ret.cruiseState.enabled = bool(self.CS.acc_active)
+ ret.cruiseState.speed = self.CS.v_cruise_pcm * CV.KPH_TO_MS
ret.cruiseState.available = bool(self.CS.main_on)
+ ret.cruiseState.speedOffset = 0.
+
ret.leftBlinker = self.CS.left_blinker_on
ret.rightBlinker = self.CS.right_blinker_on
ret.seatbeltUnlatched = self.CS.seatbelt_unlatched
+ ret.doorOpen = self.CS.door_open
buttonEvents = []
@@ -177,29 +191,31 @@ class CarInterface(object):
if ret.seatbeltUnlatched:
events.append(create_event('seatbeltNotLatched', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
+ if ret.doorOpen:
+ events.append(create_event('doorOpen', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
+
if self.CS.acc_active and not self.acc_active_prev:
events.append(create_event('pcmEnable', [ET.ENABLE]))
if not self.CS.acc_active:
events.append(create_event('pcmDisable', [ET.USER_DISABLE]))
- ## handle button presses
- #for b in ret.buttonEvents:
- # # do enable on both accel and decel buttons
- # if b.type in ["accelCruise", "decelCruise"] and not b.pressed:
- # events.append(create_event('buttonEnable', [ET.ENABLE]))
- # # do disable on button down
- # if b.type == "cancel" and b.pressed:
- # events.append(create_event('buttonCancel', [ET.USER_DISABLE]))
+ # disable on gas pedal rising edge
+ if (ret.gasPressed and not self.gas_pressed_prev):
+ events.append(create_event('pedalPressed', [ET.NO_ENTRY, ET.USER_DISABLE]))
+
+ if ret.gasPressed:
+ events.append(create_event('pedalPressed', [ET.PRE_ENABLE]))
ret.events = events
# update previous brake/gas pressed
+ self.gas_pressed_prev = ret.gasPressed
self.acc_active_prev = self.CS.acc_active
-
# cast to reader so it can't be modified
return ret.as_reader()
def apply(self, c):
- self.CC.update(self.sendcan, c.enabled, self.CS, self.frame, c.actuators)
+ self.CC.update(self.sendcan, c.enabled, self.CS, self.frame, c.actuators,
+ c.cruiseControl.cancel, c.hudControl.visualAlert)
self.frame += 1
diff --git a/selfdrive/car/subaru/radar_interface.py b/selfdrive/car/subaru/radar_interface.py
index 96159fd87d..3e458df576 100644
--- a/selfdrive/car/subaru/radar_interface.py
+++ b/selfdrive/car/subaru/radar_interface.py
@@ -21,4 +21,4 @@ if __name__ == "__main__":
while 1:
ret = RI.update()
print(chr(27) + "[2J")
- print ret
+ print(ret)
diff --git a/selfdrive/car/subaru/subarucan.py b/selfdrive/car/subaru/subarucan.py
new file mode 100644
index 0000000000..5c9cbde794
--- /dev/null
+++ b/selfdrive/car/subaru/subarucan.py
@@ -0,0 +1,54 @@
+import copy
+from cereal import car
+from selfdrive.car.subaru.values import CAR
+
+VisualAlert = car.CarControl.HUDControl.VisualAlert
+
+def subaru_checksum(packer, values, addr):
+ dat = packer.make_can_msg(addr, 0, values)[2]
+ dat = [ord(i) for i in dat]
+ return (sum(dat[1:]) + (addr >> 8) + addr) & 0xff
+
+def create_steering_control(packer, car_fingerprint, apply_steer, frame, steer_step):
+
+ if car_fingerprint == CAR.IMPREZA:
+ #counts from 0 to 15 then back to 0 + 16 for enable bit
+ idx = ((frame / steer_step) % 16)
+
+ values = {
+ "Counter": idx,
+ "LKAS_Output": apply_steer,
+ "LKAS_Request": 1 if apply_steer != 0 else 0,
+ "SET_1": 1
+ }
+ values["Checksum"] = subaru_checksum(packer, values, 0x122)
+
+ return packer.make_can_msg("ES_LKAS", 0, values)
+
+def create_steering_status(packer, car_fingerprint, apply_steer, frame, steer_step):
+
+ if car_fingerprint == CAR.IMPREZA:
+ values = {}
+ values["Checksum"] = subaru_checksum(packer, {}, 0x322)
+
+ return packer.make_can_msg("ES_LKAS_State", 0, values)
+
+def create_es_distance(packer, es_distance_msg, pcm_cancel_cmd):
+
+ values = copy.copy(es_distance_msg)
+ if pcm_cancel_cmd:
+ values["Main"] = 1
+
+ values["Checksum"] = subaru_checksum(packer, values, 545)
+
+ return packer.make_can_msg("ES_Distance", 0, values)
+
+def create_es_lkas(packer, es_lkas_msg, visual_alert):
+
+ values = copy.copy(es_lkas_msg)
+ if visual_alert == VisualAlert.steerRequired:
+ values["Keep_Hands_On_Wheel"] = 1
+
+ values["Checksum"] = subaru_checksum(packer, values, 802)
+
+ return packer.make_can_msg("ES_LKAS_State", 0, values)
diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py
index cdd1bde51b..0885efcb74 100644
--- a/selfdrive/car/toyota/carcontroller.py
+++ b/selfdrive/car/toyota/carcontroller.py
@@ -161,7 +161,7 @@ class CarController(object):
self.steer_angle_enabled, self.ipas_reset_counter = \
ipas_state_transition(self.steer_angle_enabled, enabled, CS.ipas_active, self.ipas_reset_counter)
- #print self.steer_angle_enabled, self.ipas_reset_counter, CS.ipas_active
+ #print("{0} {1} {2}".format(self.steer_angle_enabled, self.ipas_reset_counter, CS.ipas_active))
# steer angle
if self.steer_angle_enabled and CS.ipas_active:
@@ -198,7 +198,7 @@ class CarController(object):
can_sends = []
#*** control msgs ***
- #print "steer", apply_steer, min_lim, max_lim, CS.steer_torque_motor
+ #print("steer {0} {1} {2} {3}".format(apply_steer, min_lim, max_lim, CS.steer_torque_motor)
# toyota can trace shows this message at 42Hz, with counter adding alternatively 1 and 2;
# sending it at 100Hz seem to allow a higher rate limit, as the rate limit seems imposed
diff --git a/selfdrive/car/toyota/values.py b/selfdrive/car/toyota/values.py
index 5a8abd83c1..ce291ef3a6 100644
--- a/selfdrive/car/toyota/values.py
+++ b/selfdrive/car/toyota/values.py
@@ -99,9 +99,13 @@ FINGERPRINTS = {
CAR.LEXUS_RXH: [{
36: 8, 37: 8, 166: 8, 170: 8, 180: 8, 295: 8, 296: 8, 426: 6, 452: 8, 466: 8, 467: 8, 512: 6, 513:6, 550: 8, 552: 4, 560: 7, 562: 6, 581: 5, 608: 8, 610: 5, 643: 7, 658: 8, 713: 8, 740: 5, 742: 8, 743: 8, 800: 8, 810: 2, 812: 3, 814: 8, 830: 7, 835: 8, 836: 8, 845: 5, 863: 8, 869: 7, 870: 7, 871: 2, 898: 8, 900: 6, 902: 6, 905: 8, 913: 8, 918: 8, 921: 8, 933: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 8, 955: 8, 956: 8, 971: 7, 975: 6, 993: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1005: 2, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1059: 1, 1063: 8, 1071: 8, 1077: 8, 1082: 8, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1227: 8, 1228: 8, 1235: 8, 1237: 8, 1264: 8, 1279: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1575: 8, 1595: 8, 1777: 8, 1779: 8, 1808: 8, 1810: 8, 1816: 8, 1818: 8, 1840: 8, 1848: 8, 1904: 8, 1912: 8, 1940: 8, 1941: 8, 1948: 8, 1949: 8, 1952: 8, 1956: 8, 1960: 8, 1964: 8, 1986: 8, 1990: 8, 1994: 8, 1998: 8, 2004: 8, 2012: 8
},
- # RX450HL
+ # RX450HL
{
- 36: 8, 37: 8, 166: 8, 170: 8, 180: 8, 295: 8, 296: 8, 426: 6, 452: 8, 466: 8, 467: 8, 512:6, 513:6, 550: 8, 552: 4, 560: 7, 562: 6, 581: 5, 608: 8, 610: 5, 643: 7, 658: 8, 713: 8, 742: 8, 743: 8, 800: 8, 810: 2, 812: 3, 814: 8, 830: 7, 835: 8, 836: 8, 863: 8, 865: 8, 869: 7, 870: 7, 898: 8, 900: 6, 902: 6, 905: 8, 918: 8, 921: 8, 933: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 8, 955: 8, 956: 8, 971: 7, 975: 6, 993: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1005: 2, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1056: 8, 1057: 8, 1059: 1, 1063: 8, 1071: 8, 1076: 8, 1077: 8, 1082: 8, 1114: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1227: 8, 1228: 8, 1237: 8, 1264: 8, 1279: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1575: 8, 1592: 8, 1594: 8, 1595: 8, 1649: 8, 1777: 8, 1779: 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, 512: 6, 513: 6, 550: 8, 552: 4, 560: 7, 562: 6, 581: 5, 608: 8, 610: 5, 643: 7, 658: 8, 713: 8, 742: 8, 743: 8, 800: 8, 810: 2, 812: 3, 814: 8, 830: 7, 835: 8, 836: 8, 863: 8, 865: 8, 869: 7, 870: 7, 898: 8, 900: 6, 902: 6, 905: 8, 918: 8, 921: 8, 933: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 8, 955: 8, 956: 8, 971: 7, 975: 6, 993: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1005: 2, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1056: 8, 1057: 8, 1059: 1, 1063: 8, 1071: 8, 1076: 8, 1077: 8, 1082: 8, 1114: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1227: 8, 1228: 8, 1237: 8, 1264: 8, 1279: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1575: 8, 1592: 8, 1594: 8, 1595: 8, 1649: 8, 1777: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8
+ },
+ # RX540H 2019 with color hud
+ {
+ 36: 8, 37: 8, 166: 8, 170: 8, 180: 8, 295: 8, 296: 8, 426: 6, 452: 8, 466: 8, 467: 8, 512: 6, 513: 6, 550: 8, 552: 4, 560: 7, 562: 6, 581: 5, 608: 8, 610: 5, 643: 7, 658: 8, 713: 8, 740: 5, 742: 8, 743: 8, 800: 8, 810: 2, 812: 3, 814: 8, 818: 8, 819: 8, 820: 8, 821: 8, 822: 8, 830: 7, 835: 8, 836: 8, 845: 5, 863: 8, 865: 8, 869: 7, 870: 7, 871: 2, 898: 8, 900: 6, 902: 6, 905: 8, 913: 8, 918: 8, 921: 8, 933: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 8, 955: 8, 956: 8, 971: 7, 975: 6, 993: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1057: 8, 1059: 1, 1063: 8, 1071: 8, 1076: 8, 1077: 8, 1082: 8, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1227: 8, 1228: 8, 1235: 8, 1237: 8, 1263: 8, 1264: 8, 1279: 8, 1349: 8, 1350: 8, 1351: 8, 1413: 8, 1414: 8, 1415: 8, 1416: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1575: 8, 1592: 8, 1594: 8, 1595: 8, 1649: 8, 1777: 8, 1779: 8, 1808: 8, 1810: 8, 1816: 8, 1818: 8, 1904: 8, 1912: 8, 1952: 8, 1960: 8, 1990: 8, 1998: 8
}],
CAR.CHR: [{
36: 8, 37: 8, 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, 614: 8, 643: 7, 658: 8, 705: 8, 740: 5, 800: 8, 810: 2, 812: 8, 814: 8, 830: 7, 835: 8, 836: 8, 845: 5, 869: 7, 870: 7, 871: 2, 898: 8, 913: 8, 918: 8, 921: 8, 944: 8, 945: 8, 951: 8, 955: 8, 956: 8, 976: 1, 1014: 8, 1017: 8, 1020: 8, 1021: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1059: 1, 1082: 8, 1083: 8, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1175: 8, 1228: 8, 1235: 8, 1237: 8, 1279: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1595: 8, 1745: 8, 1779: 8
@@ -122,7 +126,7 @@ FINGERPRINTS = {
#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
- },
+ },
#SL
{
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, 888: 8, 889: 8, 898: 8, 900: 6, 902: 6, 905: 8, 913: 8, 918: 8, 921: 8, 933: 8, 934: 8, 935: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 8, 955: 8, 956: 8, 971: 7, 975: 5, 993: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1002: 8, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1057: 8, 1059: 1, 1071: 8, 1076: 8, 1077: 8, 1084: 8, 1085: 8, 1086: 8, 1114: 8, 1132: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1228: 8, 1235: 8, 1237: 8, 1264: 8, 1279: 8, 1541: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1595: 8, 1745: 8, 1779: 8, 1786: 8, 1787: 8, 1788: 8, 1789: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8
diff --git a/selfdrive/common/touch.c b/selfdrive/common/touch.c
index f21fac8f07..9117154b0c 100644
--- a/selfdrive/common/touch.c
+++ b/selfdrive/common/touch.c
@@ -90,3 +90,31 @@ int touch_poll(TouchState *s, int* out_x, int* out_y, int timeout) {
return up;
}
+int touch_read(TouchState *s, int* out_x, int* out_y) {
+ assert(out_x && out_y);
+ struct input_event event;
+ int err = read(s->fd, &event, sizeof(event));
+ if (err < sizeof(event)) {
+ return -1;
+ }
+ bool up = false;
+ switch (event.type) {
+ case EV_ABS:
+ if (event.code == ABS_MT_POSITION_X) {
+ s->last_x = event.value;
+ } else if (event.code == ABS_MT_POSITION_Y) {
+ s->last_y = event.value;
+ }
+ up = true;
+ break;
+ default:
+ break;
+ }
+ if (up) {
+ // adjust for flippening
+ *out_x = s->last_y;
+ *out_y = 1080 - s->last_x;
+ }
+ return up;
+}
+
diff --git a/selfdrive/common/touch.h b/selfdrive/common/touch.h
index c2bb6dfece..c48f66b982 100644
--- a/selfdrive/common/touch.h
+++ b/selfdrive/common/touch.h
@@ -12,6 +12,7 @@ typedef struct TouchState {
void touch_init(TouchState *s);
int touch_poll(TouchState *s, int *out_x, int *out_y, int timeout);
+int touch_read(TouchState *s, int* out_x, int* out_y);
#ifdef __cplusplus
}
diff --git a/selfdrive/common/version.h b/selfdrive/common/version.h
index b1cb55d122..fd8219990d 100644
--- a/selfdrive/common/version.h
+++ b/selfdrive/common/version.h
@@ -1 +1 @@
-#define COMMA_VERSION "0.5.10-release"
+#define COMMA_VERSION "0.5.11-release"
diff --git a/selfdrive/common/visionimg.cc b/selfdrive/common/visionimg.cc
index 8179166e1c..a533acb597 100644
--- a/selfdrive/common/visionimg.cc
+++ b/selfdrive/common/visionimg.cc
@@ -68,7 +68,7 @@ VisionImg visionimg_alloc_rgb24(int width, int height, VisionBuf *out_buf) {
#ifdef QCOM
-EGLClientBuffer visionimg_to_egl(const VisionImg *img) {
+EGLClientBuffer visionimg_to_egl(const VisionImg *img, void **pph) {
assert((img->size % img->stride) == 0);
assert((img->stride % img->bpp) == 0);
@@ -87,13 +87,14 @@ EGLClientBuffer visionimg_to_egl(const VisionImg *img) {
GraphicBuffer* gb = new GraphicBuffer(img->width, img->height, (PixelFormat)format,
GraphicBuffer::USAGE_HW_TEXTURE, img->stride/img->bpp, hnd, false);
-
+ // GraphicBuffer is ref counted by EGLClientBuffer(ANativeWindowBuffer), no need and not possible to release.
+ *pph = hnd;
return (EGLClientBuffer) gb->getNativeBuffer();
}
-GLuint visionimg_to_gl(const VisionImg *img) {
+GLuint visionimg_to_gl(const VisionImg *img, EGLImageKHR *pkhr, void **pph) {
- EGLClientBuffer buf = visionimg_to_egl(img);
+ EGLClientBuffer buf = visionimg_to_egl(img, pph);
EGLDisplay display = eglGetDisplay(EGL_DEFAULT_DISPLAY);
assert(display != EGL_NO_DISPLAY);
@@ -107,8 +108,15 @@ GLuint visionimg_to_gl(const VisionImg *img) {
glGenTextures(1, &tex);
glBindTexture(GL_TEXTURE_2D, tex);
glEGLImageTargetTexture2DOES(GL_TEXTURE_2D, image);
-
+ *pkhr = image;
return tex;
}
+void visionimg_destroy_gl(EGLImageKHR khr, void *ph) {
+ EGLDisplay display = eglGetDisplay(EGL_DEFAULT_DISPLAY);
+ assert(display != EGL_NO_DISPLAY);
+ eglDestroyImageKHR(display, khr);
+ delete (private_handle_t*)ph;
+}
+
#endif
diff --git a/selfdrive/common/visionimg.h b/selfdrive/common/visionimg.h
index 9fabb6054f..74b0f3137d 100644
--- a/selfdrive/common/visionimg.h
+++ b/selfdrive/common/visionimg.h
@@ -27,8 +27,9 @@ void visionimg_compute_aligned_width_and_height(int width, int height, int *alig
VisionImg visionimg_alloc_rgb24(int width, int height, VisionBuf *out_buf);
#ifdef QCOM
-EGLClientBuffer visionimg_to_egl(const VisionImg *img);
-GLuint visionimg_to_gl(const VisionImg *img);
+EGLClientBuffer visionimg_to_egl(const VisionImg *img, void **pph);
+GLuint visionimg_to_gl(const VisionImg *img, EGLImageKHR *pkhr, void **pph);
+void visionimg_destroy_gl(EGLImageKHR khr, void *ph);
#endif
#ifdef __cplusplus
diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py
index b5d699bdaa..60d3536ffe 100755
--- a/selfdrive/controls/controlsd.py
+++ b/selfdrive/controls/controlsd.py
@@ -72,7 +72,7 @@ def data_sample(CI, CC, plan_sock, path_plan_sock, thermal, calibration, health,
if td is not None:
overtemp = td.thermal.thermalStatus >= ThermalStatus.red
free_space = td.thermal.freeSpace < 0.07 # under 7% of space free no enable allowed
- low_battery = td.thermal.batteryPercent < 1 # at zero percent battery, OP should not be allowed
+ low_battery = td.thermal.batteryPercent < 1 and td.thermal.chargingError # at zero percent battery, while discharging, OP should not be allowed
# Create events for battery, temperature and disk space
if low_battery:
@@ -282,7 +282,7 @@ def state_control(plan, path_plan, CS, CP, state, events, v_cruise_kph, v_cruise
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, params, v_acc, a_acc):
+ LaC, LoC, angle_model_bias, passive, start_time, v_acc, a_acc):
"""Send actuators and hud commands to the car, send live100 and MPC logging"""
plan_ts = plan.logMonoTime
plan = plan.plan
@@ -327,7 +327,7 @@ def data_send(plan, path_plan, CS, CI, CP, VM, state, events, actuators, v_cruis
"alertType": AM.alert_type,
"alertSound": "", # no EON sounds yet
"awarenessStatus": max(driver_status.awareness, 0.0) if isEnabled(state) else 0.0,
- "driverMonitoringOn": bool(driver_status.monitor_on),
+ "driverMonitoringOn": bool(driver_status.monitor_on and driver_status.face_detected),
"canMonoTimes": list(CS.canMonoTimes),
"planMonoTime": plan_ts,
"pathPlanMonoTime": path_plan.logMonoTime,
@@ -377,9 +377,6 @@ def data_send(plan, path_plan, CS, CI, CP, VM, state, events, actuators, v_cruis
cc_send.carControl = CC
carcontrol.send(cc_send.to_bytes())
- if (rk.frame % 36000) == 0: # update angle offset every 6 minutes
- params.put("ControlsParams", json.dumps({'angle_model_bias': angle_model_bias}))
-
return CC
@@ -512,7 +509,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, params, v_acc, a_acc)
+ live100, AM, driver_status, LaC, LoC, angle_model_bias, passive, start_time, v_acc, a_acc)
prof.checkpoint("Sent")
rk.keep_time() # Run at 100Hz
diff --git a/selfdrive/controls/lib/driver_monitor.py b/selfdrive/controls/lib/driver_monitor.py
index 10e0e0be66..767b1c1a1b 100644
--- a/selfdrive/controls/lib/driver_monitor.py
+++ b/selfdrive/controls/lib/driver_monitor.py
@@ -4,26 +4,42 @@ from selfdrive.controls.lib.drive_helpers import create_event, EventTypes as ET
from common.filter_simple import FirstOrderFilter
_DT = 0.01 # update runs at 100Hz
-_DTM = 0.1 # DM runs at 10Hz
-_AWARENESS_TIME = 180 # 3 minutes limit without user touching steering wheels make the car enter a terminal status
-_AWARENESS_PRE_TIME = 20. # a first alert is issued 20s before expiration
-_AWARENESS_PROMPT_TIME = 5. # a second alert is issued 5s before start decelerating the car
+_DTM = 0.1 # DM runs at 10Hz
+_AWARENESS_TIME = 180 # 3 minutes limit without user touching steering wheels make the car enter a terminal status
+_AWARENESS_PRE_TIME = 20. # a first alert is issued 20s before expiration
+_AWARENESS_PROMPT_TIME = 5. # a second alert is issued 5s before start decelerating the car
_DISTRACTED_TIME = 7.
_DISTRACTED_PRE_TIME = 4.
_DISTRACTED_PROMPT_TIME = 2.
-# measured 1 rad in x FOV. 1152x864 is original image, 160x320 is a right crop for model
-_CAMERA_FOV_X = 1. # rad
-_CAMERA_FOV_Y = 0.75 # 4/3 aspect ratio
# model output refers to center of cropped image, so need to apply the x displacement offset
-_CAMERA_OFFSET_X = 0.3125 #(1152/2 - 0.5*(160*864/320))/1152
-_CAMERA_X_CONV = 0.375 # 160*864/320/1152
_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
-_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
+_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
+_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
+
+RESIZED_FOCAL = 320.0
+H, W, FULL_W = 320, 160, 426
+
+
+def head_orientation_from_descriptor(desc):
+ # the output of these angles are in device frame
+ # so from driver's perspective, pitch is up and yaw is right
+ # TODO this should be calibrated
+ pitch_prnet = desc[0]
+ yaw_prnet = desc[1]
+ roll_prnet = desc[2]
+
+ face_pixel_position = ((desc[3] + .5)*W - W + FULL_W, (desc[4]+.5)*H)
+ yaw_focal_angle = np.arctan2(face_pixel_position[0] - FULL_W/2, RESIZED_FOCAL)
+ pitch_focal_angle = np.arctan2(face_pixel_position[1] - H/2, RESIZED_FOCAL)
+
+ roll = roll_prnet
+ pitch = pitch_prnet + pitch_focal_angle
+ yaw = -yaw_prnet + yaw_focal_angle
+ return np.array([roll, pitch, yaw])
class _DriverPose():
@@ -34,10 +50,12 @@ class _DriverPose():
self.yaw_offset = 0.
self.pitch_offset = 0.
+
def _monitor_hysteresys(variance_level, monitor_valid_prev):
var_thr = 0.63 if monitor_valid_prev else 0.37
return variance_level < var_thr
+
class DriverStatus():
def __init__(self, monitor_on=False):
self.pose = _DriverPose()
@@ -50,6 +68,7 @@ class DriverStatus():
self.variance_high = False
self.variance_filter = FirstOrderFilter(0., _VARIANCE_FILTER_TS, _DTM)
self.ts_last_check = 0.
+ self.face_detected = False
self._set_timers()
def _reset_filters(self):
@@ -69,8 +88,8 @@ class DriverStatus():
def _is_driver_distracted(self, pose):
# to be tuned and to learn the driver's normal pose
- yaw_error = pose.yaw - pose.yaw_offset
- pitch_error = pose.pitch - pose.pitch_offset - _PITCH_NATURAL_OFFSET
+ pitch_error = pose.pitch - _PITCH_NATURAL_OFFSET
+ yaw_error = pose.yaw
# add positive pitch allowance
if pitch_error > 0.:
pitch_error = max(pitch_error - _PITCH_POS_ALLOWANCE, 0.)
@@ -82,11 +101,14 @@ class DriverStatus():
def get_pose(self, driver_monitoring, params):
- self.pose.pitch = driver_monitoring.descriptor[0]
- self.pose.yaw = driver_monitoring.descriptor[1]
- self.pose.roll = driver_monitoring.descriptor[2]
- self.pose.yaw_offset = (driver_monitoring.descriptor[3] * _CAMERA_X_CONV + _CAMERA_OFFSET_X) * _CAMERA_FOV_X
- self.pose.pitch_offset = -driver_monitoring.descriptor[4] * _CAMERA_FOV_Y # positive y is down
+ self.pose.roll, self.pose.pitch, self.pose.yaw = head_orientation_from_descriptor(driver_monitoring.descriptor)
+
+ # TODO: DM data should not be in a list if they are not homogeneous
+ if len(driver_monitoring.descriptor) > 6:
+ self.face_detected = driver_monitoring.descriptor[6] > 0.
+ else:
+ self.face_detected = True
+
self.driver_distracted = self._is_driver_distracted(self.pose)
# first order filters
self.driver_distraction_filter.update(self.driver_distracted)
@@ -117,7 +139,8 @@ class DriverStatus():
# always reset if driver is in control (unless we are in red alert state) or op isn't active
self.awareness = 1.
- if (not self.monitor_on or (self.driver_distraction_filter.x > 0.63 and self.driver_distracted)) and \
+ # only update if face is detected, driver is distracted and distraction filter is high
+ if (not self.monitor_on or (self.driver_distraction_filter.x > 0.63 and self.driver_distracted and self.face_detected)) and \
not (standstill and self.awareness - self.step_change <= self.threshold_prompt):
self.awareness = max(self.awareness - self.step_change, -0.1)
@@ -146,4 +169,3 @@ if __name__ == "__main__":
print(ds.awareness, ds.driver_distracted, ds.driver_distraction_filter.x)
ds.update([], True, True, False)
print(ds.awareness, ds.driver_distracted, ds.driver_distraction_filter.x)
-
diff --git a/selfdrive/controls/lib/pathplanner.py b/selfdrive/controls/lib/pathplanner.py
index 0161d2f946..9c0a62a0ba 100644
--- a/selfdrive/controls/lib/pathplanner.py
+++ b/selfdrive/controls/lib/pathplanner.py
@@ -51,7 +51,8 @@ class PathPlanner(object):
angle_steers = CS.carState.steeringAngle
active = live100.live100.active
- angle_offset_bias = live100.live100.angleModelBias + live_parameters.liveParameters.angleOffsetAverage
+ angle_offset_average = live_parameters.liveParameters.angleOffsetAverage
+ angle_offset_bias = live100.live100.angleModelBias + angle_offset_average
self.MP.update(v_ego, md)
@@ -65,7 +66,7 @@ class PathPlanner(object):
p_poly = libmpc_py.ffi.new("double[4]", list(self.MP.p_poly))
# account for actuation delay
- self.cur_state = calc_states_after_delay(self.cur_state, v_ego, angle_steers, curvature_factor, VM.sR, CP.steerActuatorDelay)
+ self.cur_state = calc_states_after_delay(self.cur_state, v_ego, angle_steers - angle_offset_average, curvature_factor, VM.sR, CP.steerActuatorDelay)
v_ego_mpc = max(v_ego, 5.0) # avoid mpc roughness due to low speed
self.libmpc.run_mpc(self.cur_state, self.mpc_solution,
@@ -84,7 +85,6 @@ class PathPlanner(object):
self.angle_steers_des_mpc = float(math.degrees(delta_desired * VM.sR) + angle_offset_bias)
-
# Check for infeasable MPC solution
mpc_nans = np.any(np.isnan(list(self.mpc_solution[0].delta)))
t = sec_since_boot()
@@ -115,7 +115,7 @@ class PathPlanner(object):
plan_send.pathPlan.rProb = float(self.MP.r_prob)
plan_send.pathPlan.angleSteers = float(self.angle_steers_des_mpc)
plan_send.pathPlan.rateSteers = float(rate_desired)
- plan_send.pathPlan.angleOffset = float(live_parameters.liveParameters.angleOffsetAverage)
+ plan_send.pathPlan.angleOffset = float(angle_offset_average)
plan_send.pathPlan.valid = bool(plan_valid)
plan_send.pathPlan.paramsValid = bool(live_parameters.liveParameters.valid)
diff --git a/selfdrive/debug/can_printer.py b/selfdrive/debug/can_printer.py
index d7cbabe63a..876b2ab808 100755
--- a/selfdrive/debug/can_printer.py
+++ b/selfdrive/debug/can_printer.py
@@ -29,7 +29,7 @@ def can_printer(bus=0, max_msg=None, addr="127.0.0.1"):
for k,v in sorted(zip(msgs.keys(), map(lambda x: x[-1].encode("hex"), msgs.values()))):
if max_msg is None or k < max_msg:
dd += "%s(%6d) %s\n" % ("%04X(%4d)" % (k,k),len(msgs[k]), v)
- print dd
+ print(dd)
lp = sec_since_boot()
if __name__ == "__main__":
diff --git a/selfdrive/debug/cpu_usage_stat.py b/selfdrive/debug/cpu_usage_stat.py
new file mode 100644
index 0000000000..48cca99f91
--- /dev/null
+++ b/selfdrive/debug/cpu_usage_stat.py
@@ -0,0 +1,99 @@
+import psutil
+import time
+import os
+import sys
+import numpy as np
+import argparse
+import re
+
+'''
+System tools like top/htop can only show current cpu usage values, so I write this script to do statistics jobs.
+ Features:
+ Use psutil library to sample cpu usage(avergage for all cores) of OpenPilot processes, at a rate of 5 samples/sec.
+ Do cpu usage statistics periodically, 5 seconds as a cycle.
+ Caculate the average cpu usage within this cycle.
+ Caculate minumium/maximium/accumulated_average cpu usage as long term inspections.
+ Monitor multiple processes simuteneously.
+ Sample usage:
+ root@localhost:/data/openpilot$ python selfdrive/debug/cpu_usage_stat.py boardd,ubloxd
+ ('Add monitored proc:', './boardd')
+ ('Add monitored proc:', 'python locationd/ubloxd.py')
+ boardd: 1.96%, min: 1.96%, max: 1.96%, acc: 1.96%
+ ubloxd.py: 0.39%, min: 0.39%, max: 0.39%, acc: 0.39%
+'''
+
+# Do statistics every 5 seconds
+PRINT_INTERVAL = 5
+SLEEP_INTERVAL = 0.2
+
+monitored_proc_names = [
+ 'ubloxd', 'thermald', 'uploader', 'controlsd', 'plannerd', 'radard', 'mapd', 'loggerd' , 'logmessaged', 'tombstoned',
+ 'logcatd', 'proclogd', 'boardd', 'pandad', './ui', 'calibrationd', 'locationd', 'visiond', 'sensord', 'updated', 'gpsd', 'athena']
+
+
+def get_arg_parser():
+ parser = argparse.ArgumentParser(
+ description="Unlogger and UI",
+ formatter_class=argparse.ArgumentDefaultsHelpFormatter)
+
+ 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,
+ help="Show all running processes' cmdline")
+ return parser
+
+
+if __name__ == "__main__":
+ args = get_arg_parser().parse_args(sys.argv[1:])
+ if args.list_all:
+ for p in psutil.process_iter():
+ print('cmdline', p.cmdline(), 'name', p.name())
+ sys.exit(0)
+
+ if len(args.proc_names) > 0:
+ monitored_proc_names = args.proc_names.split(',')
+ monitored_procs = []
+ stats = {}
+ for p in psutil.process_iter():
+ if p == psutil.Process():
+ continue
+ matched = any([l for l in p.cmdline() if any([pn for pn in monitored_proc_names if re.match(r'.*{}.*'.format(pn), l, re.M | re.I)])])
+ if matched:
+ k = ' '.join(p.cmdline())
+ print('Add monitored proc:', k)
+ stats[k] = {'cpu_samples': [], 'avg_cpu': None, 'min': None, 'max': None}
+ 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())
+ 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)
+ 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()])
+ ))
diff --git a/selfdrive/debug/dump.py b/selfdrive/debug/dump.py
index 45b737953f..0434023f71 100755
--- a/selfdrive/debug/dump.py
+++ b/selfdrive/debug/dump.py
@@ -52,7 +52,7 @@ if __name__ == "__main__":
server_thread = Thread(target=run_server, args=(socketio,))
server_thread.daemon = True
server_thread.start()
- print 'server running'
+ print('server running')
values = None
if args.values:
@@ -68,7 +68,7 @@ if __name__ == "__main__":
if sock in republish_socks:
republish_socks[sock].send(msg)
if args.map and evt.which() == 'liveLocation':
- print 'send loc'
+ print('send loc')
socketio.emit('location', {
'lat': evt.liveLocation.lat,
'lon': evt.liveLocation.lon,
@@ -83,15 +83,15 @@ if __name__ == "__main__":
elif args.json:
print(json.loads(msg))
elif args.dump_json:
- print json.dumps(evt.to_dict())
+ print(json.dumps(evt.to_dict()))
elif values:
- print "logMonotime = {}".format(evt.logMonoTime)
+ print("logMonotime = {}".format(evt.logMonoTime))
for value in values:
if hasattr(evt, value[0]):
item = evt
for key in value:
item = getattr(item, key)
- print "{} = {}".format(".".join(value), item)
- print ""
+ print("{} = {}".format(".".join(value), item))
+ print("")
else:
- print evt
+ print(evt)
diff --git a/selfdrive/debug/get_fingerprint.py b/selfdrive/debug/get_fingerprint.py
index dd8eb4d808..c46e84970d 100755
--- a/selfdrive/debug/get_fingerprint.py
+++ b/selfdrive/debug/get_fingerprint.py
@@ -26,5 +26,5 @@ while True:
fingerprint = ', '.join("%d: %d" % v for v in sorted(msgs.items()))
- print "number of messages:", len(msgs)
- print "fingerprint", fingerprint
+ print("number of messages {0}:".format(len(msgs)))
+ print("fingerprint {0}".format(fingerprint))
diff --git a/selfdrive/debug/getframes/getframes.py b/selfdrive/debug/getframes/getframes.py
index 292368d6a9..4964841750 100755
--- a/selfdrive/debug/getframes/getframes.py
+++ b/selfdrive/debug/getframes/getframes.py
@@ -98,4 +98,4 @@ def getframes(front=False):
if __name__ == "__main__":
for buf in getframes():
- print buf.shape, buf[101, 101]
+ print("{0} {1}".format(buf.shape, buf[101, 101]))
diff --git a/selfdrive/locationd/Makefile b/selfdrive/locationd/Makefile
new file mode 100644
index 0000000000..110c1457ee
--- /dev/null
+++ b/selfdrive/locationd/Makefile
@@ -0,0 +1,86 @@
+CC = clang
+CXX = clang++
+
+ARCH := $(shell uname -m)
+OS := $(shell uname -o)
+
+BASEDIR = ../..
+PHONELIBS = ../../phonelibs
+
+WARN_FLAGS = -Werror=implicit-function-declaration \
+ -Werror=incompatible-pointer-types \
+ -Werror=int-conversion \
+ -Werror=return-type \
+ -Werror=format-extra-args
+
+CFLAGS = -std=gnu11 -g -fPIC -I../ -I../../ -O2 $(WARN_FLAGS)
+CXXFLAGS = -std=c++11 -g -fPIC -I../ -I../../ -O2 $(WARN_FLAGS)
+
+ZMQ_FLAGS = -I$(PHONELIBS)/zmq/aarch64/include
+ZMQ_LIBS = -L$(PHONELIBS)/zmq/aarch64/lib \
+ -l:libczmq.a -l:libzmq.a \
+ -lgnustl_shared
+
+JSON_FLAGS = -I$(PHONELIBS)/json/src
+
+EXTRA_LIBS = -lpthread
+
+ifeq ($(ARCH),x86_64)
+ZMQ_LIBS = -L$(BASEDIR)/external/zmq/lib \
+ -l:libczmq.a -l:libzmq.a
+endif
+
+.PHONY: all
+all: ubloxd
+
+include ../common/cereal.mk
+
+OBJS = ublox_msg.o \
+ ubloxd_main.o \
+ ../common/swaglog.o \
+ ../common/params.o \
+ ../common/util.o \
+ $(PHONELIBS)/json/src/json.o \
+ $(CEREAL_OBJS)
+
+DEPS := $(OBJS:.o=.d) ubloxd.d ubloxd_test.d
+
+ubloxd: ubloxd.o $(OBJS)
+ @echo "[ LINK ] $@"
+ $(CXX) -fPIC -o '$@' $^ \
+ $(CEREAL_LIBS) \
+ $(ZMQ_LIBS) \
+ $(EXTRA_LIBS)
+
+ubloxd_test: ubloxd_test.o $(OBJS)
+ @echo "[ LINK ] $@"
+ $(CXX) -fPIC -o '$@' $^ \
+ $(CEREAL_LIBS) \
+ $(ZMQ_LIBS) \
+ $(EXTRA_LIBS)
+
+%.o: %.cc
+ @echo "[ CXX ] $@"
+ $(CXX) $(CXXFLAGS) -MMD \
+ -Iinclude -I.. -I../.. \
+ $(CEREAL_CXXFLAGS) \
+ $(ZMQ_FLAGS) \
+ $(JSON_FLAGS) \
+ -I../ \
+ -I../../ \
+ -c -o '$@' '$<'
+
+%.o: %.c
+ @echo "[ CC ] $@"
+ $(CC) $(CFLAGS) -MMD \
+ -Iinclude -I.. -I../.. \
+ $(CEREAL_CFLAGS) \
+ $(ZMQ_FLAGS) \
+ $(JSON_FLAGS) \
+ -c -o '$@' '$<'
+
+.PHONY: clean
+clean:
+ rm -f ubloxd ubloxd.d ubloxd.o ubloxd_test ubloxd_test.o ubloxd_test.d $(OBJS) $(DEPS)
+
+-include $(DEPS)
diff --git a/selfdrive/locationd/calibrationd.py b/selfdrive/locationd/calibrationd.py
index 41911a6b74..1c0ce8ef7d 100755
--- a/selfdrive/locationd/calibrationd.py
+++ b/selfdrive/locationd/calibrationd.py
@@ -39,6 +39,7 @@ class Calibrator(object):
self.vps = []
self.cal_status = Calibration.UNCALIBRATED
self.write_counter = 0
+ self.just_calibrated = False
self.params = Params()
calibration_params = self.params.get("CalibrationParams")
if calibration_params:
@@ -52,10 +53,16 @@ class Calibrator(object):
def update_status(self):
+ start_status = self.cal_status
if len(self.vps) < INPUTS_NEEDED:
self.cal_status = Calibration.UNCALIBRATED
else:
self.cal_status = Calibration.CALIBRATED if is_calibration_valid(self.vp) else Calibration.INVALID
+ end_status = self.cal_status
+
+ self.just_calibrated = False
+ if start_status == Calibration.UNCALIBRATED and end_status == Calibration.CALIBRATED:
+ self.just_calibrated = True
def handle_cam_odom(self, log):
trans, rot = log.cameraOdometry.trans, log.cameraOdometry.rot
@@ -67,7 +74,7 @@ class Calibrator(object):
self.vp = np.mean(self.vps, axis=0)
self.update_status()
self.write_counter += 1
- if self.param_put and self.write_counter % WRITE_CYCLES == 0:
+ if self.param_put and (self.write_counter % WRITE_CYCLES == 0 or self.just_calibrated):
cal_params = {"vanishing_point": list(self.vp),
"valid_points": len(self.vps)}
self.params.put("CalibrationParams", json.dumps(cal_params))
diff --git a/selfdrive/locationd/kalman/ekf_sym.py b/selfdrive/locationd/kalman/ekf_sym.py
index 2dfefe92d3..8b537c6995 100644
--- a/selfdrive/locationd/kalman/ekf_sym.py
+++ b/selfdrive/locationd/kalman/ekf_sym.py
@@ -340,7 +340,7 @@ class EKF_sym(object):
# rewind
if t < self.filter_time:
if len(self.rewind_t) == 0 or t < self.rewind_t[0] or t < self.rewind_t[-1] -1.0:
- print "observation too old at %.3f with filter at %.3f, ignoring" % (t, self.filter_time)
+ print("observation too old at %.3f with filter at %.3f, ignoring" % (t, self.filter_time))
return None
rewound = self.rewind(t)
else:
@@ -457,7 +457,7 @@ class EKF_sym(object):
# TODO If nullspace isn't the dimension we want
if A.shape[1] + He.shape[1] != A.shape[0]:
- print 'Warning: null space projection failed, measurement ignored'
+ print('Warning: null space projection failed, measurement ignored')
return x, P, np.zeros(A.shape[0] - He.shape[1])
# if using eskf
diff --git a/selfdrive/locationd/locationd_local.py b/selfdrive/locationd/locationd_local.py
index 0b1af2f4c1..cc5498cf3a 100755
--- a/selfdrive/locationd/locationd_local.py
+++ b/selfdrive/locationd/locationd_local.py
@@ -73,10 +73,10 @@ class Localizer(object):
self.update_kalman(current_time, ObservationKind.CAMERA_ODO_TRANSLATION, np.concatenate([log.cameraOdometry.trans,
log.cameraOdometry.transStd]))
- def handle_car_state(self, log, current_time):
+ def handle_live100(self, log, current_time):
self.speed_counter += 1
if self.speed_counter % 5 == 0:
- self.update_kalman(current_time, ObservationKind.ODOMETRIC_SPEED, np.array([log.carState.vEgo]))
+ self.update_kalman(current_time, ObservationKind.ODOMETRIC_SPEED, np.array([log.live100.vEgo]))
def handle_sensors(self, log, current_time):
for sensor_reading in log.sensorEvents:
@@ -93,8 +93,8 @@ class Localizer(object):
return
if typ == "sensorEvents":
self.handle_sensors(log, current_time)
- elif typ == "carState":
- self.handle_car_state(log, current_time)
+ elif typ == "live100":
+ self.handle_live100(log, current_time)
elif typ == "cameraOdometry":
self.handle_cam_odo(log, current_time)
@@ -113,7 +113,7 @@ class ParamsLearner(object):
self.MAX_SR_TH = MAX_SR_TH * self.VM.sR
self.alpha1 = 0.01 * learning_rate
- self.alpha2 = 0.00025 * learning_rate
+ self.alpha2 = 0.0005 * learning_rate
self.alpha3 = 0.1 * learning_rate
self.alpha4 = 1.0 * learning_rate
@@ -154,7 +154,7 @@ class ParamsLearner(object):
# instant_ao = aF*m*psi*sR*u/(cR0*l*x) - aR*m*psi*sR*u/(cF0*l*x) - l*psi*sR/u + sa
s4 = "Instant AO: % .2f Avg. AO % .2f" % (math.degrees(self.ao), math.degrees(self.slow_ao))
s5 = "Stiffnes: % .3f x" % self.x
- print s4, s5
+ print("{0} {1}".format(s4, s5))
self.ao = clip(self.ao, -MAX_ANGLE_OFFSET, MAX_ANGLE_OFFSET)
@@ -173,7 +173,7 @@ def locationd_thread(gctx, addr, disabled_logs):
ctx = zmq.Context()
poller = zmq.Poller()
- car_state_socket = messaging.sub_sock(ctx, service_list['carState'].port, poller, addr=addr, conflate=True)
+ live100_socket = messaging.sub_sock(ctx, service_list['live100'].port, poller, addr=addr, conflate=True)
sensor_events_socket = messaging.sub_sock(ctx, service_list['sensorEvents'].port, poller, addr=addr, conflate=True)
camera_odometry_socket = messaging.sub_sock(ctx, service_list['cameraOdometry'].port, poller, addr=addr, conflate=True)
@@ -219,19 +219,19 @@ def locationd_thread(gctx, addr, disabled_logs):
log = messaging.recv_one(socket)
localizer.handle_log(log)
- if socket is car_state_socket:
+ if socket is live100_socket:
if not localizer.kf.t:
continue
if i % LEARNING_RATE == 0:
- # carState is not updating the Kalman Filter, so update KF manually
+ # live100 is not updating the Kalman Filter, so update KF manually
localizer.kf.predict(1e-9 * log.logMonoTime)
predicted_state = localizer.kf.x
yaw_rate = -float(predicted_state[5])
- steering_angle = math.radians(log.carState.steeringAngle)
- params_valid = learner.update(yaw_rate, log.carState.vEgo, steering_angle)
+ steering_angle = math.radians(log.live100.angleSteers)
+ params_valid = learner.update(yaw_rate, log.live100.vEgo, steering_angle)
params = messaging.new_message()
params.init('liveParameters')
@@ -246,6 +246,7 @@ def locationd_thread(gctx, addr, disabled_logs):
params = learner.get_values()
params['carFingerprint'] = CP.carFingerprint
params_reader.put("LiveParameters", json.dumps(params))
+ params_reader.put("ControlsParams", json.dumps({'angle_model_bias': log.live100.angleModelBias}))
i += 1
elif socket is camera_odometry_socket:
@@ -263,7 +264,7 @@ def main(gctx=None, addr="127.0.0.1"):
disabled_logs = os.getenv("DISABLED_LOGS", "").split(",")
# No speed for now
- disabled_logs.append('carState')
+ disabled_logs.append('live100')
if IN_CAR:
addr = "192.168.5.11"
diff --git a/selfdrive/locationd/test/__init__.py b/selfdrive/locationd/test/__init__.py
new file mode 100644
index 0000000000..e69de29bb2
diff --git a/selfdrive/locationd/test/ci_test.py b/selfdrive/locationd/test/ci_test.py
new file mode 100755
index 0000000000..36dc4d9a7d
--- /dev/null
+++ b/selfdrive/locationd/test/ci_test.py
@@ -0,0 +1,82 @@
+#!/usr/bin/env python2
+import subprocess
+import os
+import sys
+import argparse
+import tempfile
+
+from ubloxd_py_test import parser_test
+from ubloxd_regression_test import compare_results
+
+
+def mkdirs_exists_ok(path):
+ try:
+ os.makedirs(path)
+ except OSError:
+ if not os.path.isdir(path):
+ raise
+
+
+def main(args):
+ cur_dir = os.path.dirname(os.path.realpath(__file__))
+ ubloxd_dir = os.path.join(cur_dir, '../')
+
+ cc_output_dir = os.path.join(args.output_dir, 'cc')
+ mkdirs_exists_ok(cc_output_dir)
+
+ py_output_dir = os.path.join(args.output_dir, 'py')
+ mkdirs_exists_ok(py_output_dir)
+
+ archive_file = os.path.join(cur_dir, args.stream_gz_file)
+
+ try:
+ print('Extracting stream file')
+ subprocess.check_call(['tar', 'zxf', archive_file], cwd=tempfile.gettempdir())
+ stream_file_path = os.path.join(tempfile.gettempdir(), 'ubloxRaw.stream')
+
+ if not os.path.isfile(stream_file_path):
+ print('Extract file failed')
+ sys.exit(-3)
+
+ print('Compiling test app...')
+ subprocess.check_call(["make", "ubloxd_test"], cwd=ubloxd_dir)
+
+ print('Run regression test - CC parser...')
+ if args.valgrind:
+ subprocess.check_call(["valgrind", "--leak-check=full", os.path.join(ubloxd_dir, 'ubloxd_test'), stream_file_path, cc_output_dir])
+ else:
+ subprocess.check_call([os.path.join(ubloxd_dir, 'ubloxd_test'), stream_file_path, cc_output_dir])
+
+ print('Running regression test - py parser...')
+ parser_test(stream_file_path, py_output_dir)
+
+ print('Running regression test - compare result...')
+ r = compare_results(cc_output_dir, py_output_dir)
+
+ print('All done!')
+
+ subprocess.check_call(["rm", stream_file_path])
+ subprocess.check_call(["rm", '-rf', cc_output_dir])
+ subprocess.check_call(["rm", '-rf', py_output_dir])
+ sys.exit(r)
+
+ except subprocess.CalledProcessError as e:
+ print('CI test failed with {}'.format(e.returncode))
+ sys.exit(e.returncode)
+
+
+if __name__ == "__main__":
+ parser = argparse.ArgumentParser(description="Ubloxd CI test",
+ formatter_class=argparse.ArgumentDefaultsHelpFormatter)
+
+ parser.add_argument("stream_gz_file", nargs='?', default='ubloxRaw.tar.gz',
+ help="UbloxRaw data stream zip file")
+
+ parser.add_argument("output_dir", nargs='?', default='out',
+ help="Output events temp directory")
+
+ parser.add_argument("--valgrind", default=False, action='store_true',
+ help="Run in valgrind")
+
+ args = parser.parse_args()
+ main(args)
diff --git a/selfdrive/locationd/ephemeris.py b/selfdrive/locationd/test/ephemeris.py
similarity index 100%
rename from selfdrive/locationd/ephemeris.py
rename to selfdrive/locationd/test/ephemeris.py
diff --git a/selfdrive/locationd/ublox.py b/selfdrive/locationd/test/ublox.py
similarity index 100%
rename from selfdrive/locationd/ublox.py
rename to selfdrive/locationd/test/ublox.py
diff --git a/selfdrive/locationd/ubloxd.py b/selfdrive/locationd/test/ubloxd.py
similarity index 100%
rename from selfdrive/locationd/ubloxd.py
rename to selfdrive/locationd/test/ubloxd.py
diff --git a/selfdrive/locationd/test/ubloxd_easy.py b/selfdrive/locationd/test/ubloxd_easy.py
new file mode 100755
index 0000000000..32f7f3d248
--- /dev/null
+++ b/selfdrive/locationd/test/ubloxd_easy.py
@@ -0,0 +1,55 @@
+#!/usr/bin/env python
+import os
+import ublox
+from common import realtime
+from ubloxd import gen_raw, gen_solution
+import zmq
+import selfdrive.messaging as messaging
+from selfdrive.services import service_list
+
+
+unlogger = os.getenv("UNLOGGER") is not None # debug prints
+
+def main(gctx=None):
+ context = zmq.Context()
+ poller = zmq.Poller()
+
+ context = zmq.Context()
+ gpsLocationExternal = messaging.pub_sock(context, service_list['gpsLocationExternal'].port)
+ ubloxGnss = messaging.pub_sock(context, service_list['ubloxGnss'].port)
+
+ # ubloxRaw = messaging.sub_sock(context, service_list['ubloxRaw'].port, poller)
+
+ # buffer with all the messages that still need to be input into the kalman
+ while 1:
+ polld = poller.poll(timeout=1000)
+ for sock, mode in polld:
+ if mode != zmq.POLLIN:
+ continue
+ logs = messaging.drain_sock(sock)
+ for log in logs:
+ buff = log.ubloxRaw
+ time = log.logMonoTime
+ msg = ublox.UBloxMessage()
+ msg.add(buff)
+ if msg.valid():
+ if msg.name() == 'NAV_PVT':
+ sol = gen_solution(msg)
+ if unlogger:
+ sol.logMonoTime = time
+ else:
+ sol.logMonoTime = int(realtime.sec_since_boot() * 1e9)
+ gpsLocationExternal.send(sol.to_bytes())
+ elif msg.name() == 'RXM_RAW':
+ raw = gen_raw(msg)
+ if unlogger:
+ raw.logMonoTime = time
+ else:
+ raw.logMonoTime = int(realtime.sec_since_boot() * 1e9)
+ ubloxGnss.send(raw.to_bytes())
+ else:
+ print "INVALID MESSAGE"
+
+
+if __name__ == "__main__":
+ main()
diff --git a/selfdrive/locationd/test/ubloxd_py_test.py b/selfdrive/locationd/test/ubloxd_py_test.py
new file mode 100755
index 0000000000..ab56ecb8c5
--- /dev/null
+++ b/selfdrive/locationd/test/ubloxd_py_test.py
@@ -0,0 +1,77 @@
+import sys
+import os
+
+from ublox import UBloxMessage
+from ubloxd import gen_solution, gen_raw, gen_nav_data
+from common import realtime
+
+
+def mkdirs_exists_ok(path):
+ try:
+ os.makedirs(path)
+ except OSError:
+ if not os.path.isdir(path):
+ raise
+
+
+def parser_test(fn, prefix):
+ nav_frame_buffer = {}
+ nav_frame_buffer[0] = {}
+ for i in xrange(1, 33):
+ nav_frame_buffer[0][i] = {}
+
+ if not os.path.exists(prefix):
+ print('Prefix invalid')
+ sys.exit(-1)
+
+ with open(fn, 'rb') as f:
+ i = 0
+ saved_i = 0
+ msg = UBloxMessage()
+ while True:
+ n = msg.needed_bytes()
+ b = f.read(n)
+ if not b:
+ break
+ msg.add(b)
+ if msg.valid():
+ i += 1
+ if msg.name() == 'NAV_PVT':
+ sol = gen_solution(msg)
+ sol.logMonoTime = int(realtime.sec_since_boot() * 1e9)
+ with open(os.path.join(prefix, str(saved_i)), 'wb') as f1:
+ f1.write(sol.to_bytes())
+ saved_i += 1
+ elif msg.name() == 'RXM_RAW':
+ raw = gen_raw(msg)
+ raw.logMonoTime = int(realtime.sec_since_boot() * 1e9)
+ with open(os.path.join(prefix, str(saved_i)), 'wb') as f1:
+ f1.write(raw.to_bytes())
+ saved_i += 1
+ elif msg.name() == 'RXM_SFRBX':
+ nav = gen_nav_data(msg, nav_frame_buffer)
+ if nav is not None:
+ nav.logMonoTime = int(realtime.sec_since_boot() * 1e9)
+ with open(os.path.join(prefix, str(saved_i)), 'wb') as f1:
+ f1.write(nav.to_bytes())
+ saved_i += 1
+
+ msg = UBloxMessage()
+ msg.debug_level = 0
+ print('Parsed {} msgs'.format(i))
+ print('Generated {} cereal events'.format(saved_i))
+
+
+if __name__ == "__main__":
+ if len(sys.argv) < 3:
+ print('Format: ubloxd_py_test.py file_path prefix')
+ sys.exit(0)
+
+ fn = sys.argv[1]
+ if not os.path.isfile(fn):
+ print('File path invalid')
+ sys.exit(0)
+
+ prefix = sys.argv[2]
+ mkdirs_exists_ok(prefix)
+ parser_test(fn, prefix)
diff --git a/selfdrive/locationd/test/ubloxd_regression_test.py b/selfdrive/locationd/test/ubloxd_regression_test.py
new file mode 100644
index 0000000000..94d6b4fe20
--- /dev/null
+++ b/selfdrive/locationd/test/ubloxd_regression_test.py
@@ -0,0 +1,96 @@
+#!/usr/bin/env python
+import os
+import sys
+import argparse
+
+from cereal import log
+from common.basedir import BASEDIR
+os.environ['BASEDIR'] = BASEDIR
+
+
+def get_arg_parser():
+ parser = argparse.ArgumentParser(
+ description="Compare two result files",
+ formatter_class=argparse.ArgumentDefaultsHelpFormatter)
+
+ parser.add_argument("dir1", nargs='?', default='/data/ubloxdc',
+ help="Directory path 1 from which events are loaded")
+
+ parser.add_argument("dir2", nargs='?', default='/data/ubloxdpy',
+ help="Directory path 2 from which msgs are loaded")
+
+ return parser
+
+
+def read_file(fn):
+ with open(fn, 'rb') as f:
+ return f.read()
+
+
+def compare_results(dir1, dir2):
+ onlyfiles1 = [f for f in os.listdir(dir1) if os.path.isfile(os.path.join(dir1, f))]
+ onlyfiles1.sort()
+
+ onlyfiles2 = [f for f in os.listdir(dir2) if os.path.isfile(os.path.join(dir2, f))]
+ onlyfiles2.sort()
+
+ if len(onlyfiles1) != len(onlyfiles2):
+ print('len mismatch: {} != {}'.format(len(onlyfiles1), len(onlyfiles2)))
+ return -1
+ events1 = [log.Event.from_bytes(read_file(os.path.join(dir1, f))) for f in onlyfiles1]
+ events2 = [log.Event.from_bytes(read_file(os.path.join(dir2, f))) for f in onlyfiles2]
+
+ for i in range(len(events1)):
+ if events1[i].which() != events2[i].which():
+ print('event {} type mismatch: {} != {}'.format(i, events1[i].which(), events2[i].which()))
+ return -2
+ if events1[i].which() == 'gpsLocationExternal':
+ old_gps = events1[i].gpsLocationExternal
+ gps = events2[i].gpsLocationExternal
+ # print(gps, old_gps)
+ attrs = ['flags', 'latitude', 'longitude', 'altitude', 'speed', 'bearing',
+ 'accuracy', 'timestamp', 'source', 'vNED', 'verticalAccuracy', 'bearingAccuracy', 'speedAccuracy']
+ for attr in attrs:
+ o = getattr(old_gps, attr)
+ n = getattr(gps, attr)
+ if attr == 'vNED':
+ if len(o) != len(n):
+ print('Gps vNED len mismatch', o, n)
+ return -3
+ else:
+ for i in range(len(o)):
+ if abs(o[i] - n[i]) > 1e-3:
+ print('Gps vNED mismatch', o, n)
+ return
+ elif o != n:
+ print('Gps mismatch', attr, o, n)
+ return -4
+ elif events1[i].which() == 'ubloxGnss':
+ old_gnss = events1[i].ubloxGnss
+ gnss = events2[i].ubloxGnss
+ if old_gnss.which() == 'measurementReport' and gnss.which() == 'measurementReport':
+ attrs = ['gpsWeek', 'leapSeconds', 'measurements', 'numMeas', 'rcvTow', 'receiverStatus', 'schema']
+ for attr in attrs:
+ o = getattr(old_gnss.measurementReport, attr)
+ n = getattr(gnss.measurementReport, attr)
+ if str(o) != str(n):
+ print('measurementReport {} mismatched'.format(attr))
+ return -5
+ if not (str(old_gnss.measurementReport) == str(gnss.measurementReport)):
+ print('Gnss measurementReport mismatched!')
+ print('gnss measurementReport old', old_gnss.measurementReport.measurements)
+ print('gnss measurementReport new', gnss.measurementReport.measurements)
+ return -6
+ elif old_gnss.which() == 'ephemeris' and gnss.which() == 'ephemeris':
+ if not (str(old_gnss.ephemeris) == str(gnss.ephemeris)):
+ print('Gnss ephemeris mismatched!')
+ print('gnss ephemeris old', old_gnss.ephemeris)
+ print('gnss ephemeris new', gnss.ephemeris)
+ return -7
+ print('All {} events matched!'.format(len(events1)))
+ return 0
+
+
+if __name__ == "__main__":
+ args = get_arg_parser().parse_args(sys.argv[1:])
+ compare_results(args.dir1, args.dir2)
diff --git a/selfdrive/locationd/ublox_msg.cc b/selfdrive/locationd/ublox_msg.cc
new file mode 100644
index 0000000000..ef523b8870
--- /dev/null
+++ b/selfdrive/locationd/ublox_msg.cc
@@ -0,0 +1,375 @@
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include