spacecruft
/
nopenpilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

openpilot v0.5.11 release

pull/617/head
Vehicle Researcher 2019-04-23 01:41:19 +00:00
parent 790732bea3
commit 2f92d577f9
99 changed files with 2744 additions and 1607 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
README.md
View File

@ -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 | 25mph<sup>1</sup>| 0mph | Inverted Nidec |
| Honda | Passport 2019 | All | Yes | Yes | 25mph<sup>1</sup>| 12mph | Inverted Nidec |
| Honda | Pilot 2016-18 | Honda Sensing | Yes | Yes | 25mph<sup>1</sup>| 12mph | Nidec |
| Honda | Pilot 2019 | All | Yes | Yes | 25mph<sup>1</sup>| 12mph | Inverted Nidec |
| Honda | Ridgeline 2017-19 | Honda Sensing | Yes | Yes | 25mph<sup>1</sup>| 12mph | Nidec |
@ -90,7 +91,8 @@ Supported Cars
| Kia | Optima 2019 | SCC + LKAS | Yes | Stock | 0mph | 0mph | Custom<sup>6</sup>|
| Kia | Sorento 2018 | All | Yes | Stock | 0mph | 0mph | Custom<sup>6</sup>|
| Kia | Stinger 2018 | SCC + LKAS | Yes | Stock | 0mph | 0mph | Custom<sup>6</sup>|
| Lexus | RX Hybrid 2016-18 | All | Yes | Yes<sup>2</sup>| 0mph | 0mph | Toyota |
| Lexus | RX Hybrid 2016-19 | All | Yes | Yes<sup>2</sup>| 0mph | 0mph | Toyota |
| Subaru | Impreza 2019 | EyeSight | Yes | Stock | 0mph | 0mph | Subaru |
| Toyota | Camry 2018<sup>4</sup> | All | Yes | Stock | 0mph<sup>5</sup> | 0mph | Toyota |
| Toyota | C-HR 2017-18<sup>4</sup> | All | Yes | Stock | 0mph | 0mph | Toyota |
| Toyota | Corolla 2017-18 | All | Yes | Yes<sup>2</sup>| 20mph<sup>1</sup>| 0mph | Toyota |

18
RELEASES.md
View File

@ -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

13
SAFETY.md
View File

@ -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.

BIN
apk/ai.comma.plus.offroad.apk
View File

Binary file not shown.

21
common/dbc.py
View File

@ -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

6
common/ffi_wrapper.py
View File

@ -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:

68
common/transformations/camera.py
View File

@ -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

2
common/transformations/model.py
View File

@ -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

2
launch_chffrplus.sh
View File

@ -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

BIN
models/monitoring_model.dlc
View File

Binary file not shown.

3
requirements_openpilot.txt
View File

@ -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

4
selfdrive/boardd/boardd.py
View File

@ -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()

56
selfdrive/can/Makefile
View File

@ -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)
-I. -I../.. \
$(CXXFLAGS) \
$(LDFLAGS) \
$(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 '$<' '$@'
$(OBJDIR)/%.o: %.cc
@echo "[ CXX ] $@"
$(CXX) -fPIC -c -o '$@' $^ \
-I. -I../.. \
$(CXXFLAGS) \
$(ZMQ_FLAGS) \
$(CEREAL_CXXFLAGS) \
.PHONY: clean
$(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)/*

2
selfdrive/can/packer.py
View File

@ -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"))

10
selfdrive/can/parser.py
View File

@ -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)

8
selfdrive/can/plant_can_parser.py
View File

@ -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

117
selfdrive/can/process_dbc.py
View File

@ -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)
def main():
if len(sys.argv) != 3:
print "usage: %s dbc_directory output_directory" % (sys.argv[0],)
sys.exit(0)
dbc_fn = sys.argv[1]
out_fn = sys.argv[2]
dbc_dir = sys.argv[1]
out_dir = sys.argv[2]
template_fn = os.path.join(os.path.dirname(__file__), "dbc_template.cc")
template_fn = os.path.join(os.path.dirname(__file__), "dbc_template.cc")
template_mtime = os.path.getmtime(template_fn)
can_dbc = dbc(dbc_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)
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]
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
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 dbc_mtime < out_mtime and template_mtime < out_mtime and this_file_mtime < out_mtime:
continue #skip output is newer than template and dbc
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
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]
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)
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())]
# 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)
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
parser_code = template.render(dbc=can_dbc, checksum_type=checksum_type, msgs=msgs, def_vals=def_vals, len=len)
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)
with open(out_fn, "w") as out_f:
out_f.write(parser_code)
# 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()

6
selfdrive/car/chrysler/chryslercan.py
View File

@ -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.

6
selfdrive/car/chrysler/chryslercan_test.py
View File

@ -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(

2
selfdrive/car/chrysler/interface.py
View File

@ -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

2
selfdrive/car/chrysler/radar_interface.py
View File

@ -104,4 +104,4 @@ if __name__ == "__main__":
while 1:
ret = RI.update()
print(chr(27) + "[2J") # clear screen
print ret
print(ret)

2
selfdrive/car/ford/radar_interface.py
View File

@ -90,4 +90,4 @@ if __name__ == "__main__":
while 1:
ret = RI.update()
print(chr(27) + "[2J")
print ret
print(ret)

2
selfdrive/car/gm/radar_interface.py
View File

@ -124,4 +124,4 @@ if __name__ == "__main__":
while 1:
ret = RI.update()
print(chr(27) + "[2J")
print ret
print(ret)

2
selfdrive/car/honda/carcontroller.py
View File

@ -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,

4
selfdrive/car/honda/interface.py
View File

@ -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.]

2
selfdrive/car/honda/radar_interface.py
View File

@ -101,4 +101,4 @@ if __name__ == "__main__":
while 1:
ret = RI.update()
print(chr(27) + "[2J")
print ret
print(ret)

5
selfdrive/car/honda/values.py
View File

@ -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: [{

4
selfdrive/car/hyundai/interface.py
View File

@ -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

2
selfdrive/car/hyundai/radar_interface.py
View File

@ -21,4 +21,4 @@ if __name__ == "__main__":
while 1:
ret = RI.update()
print(chr(27) + "[2J")
print ret
print(ret)

3
selfdrive/car/mock/radar_interface.py
View File

@ -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)

76
selfdrive/car/subaru/carcontroller.py 100644
View File

@ -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())

51
selfdrive/car/subaru/carstate.py
View File

@ -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"])

42
selfdrive/car/subaru/interface.py
View File

@ -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

2
selfdrive/car/subaru/radar_interface.py
View File

@ -21,4 +21,4 @@ if __name__ == "__main__":
while 1:
ret = RI.update()
print(chr(27) + "[2J")
print ret
print(ret)

54
selfdrive/car/subaru/subarucan.py 100644
View File

@ -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)

4
selfdrive/car/toyota/carcontroller.py
View File

@ -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

10
selfdrive/car/toyota/values.py
View File

@ -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

28
selfdrive/common/touch.c
View File

@ -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;
}

1
selfdrive/common/touch.h
View File

@ -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
}

2
selfdrive/common/version.h
View File

@ -1 +1 @@
#define COMMA_VERSION "0.5.10-release"
#define COMMA_VERSION "0.5.11-release"

18
selfdrive/common/visionimg.cc
View File

@ -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

5
selfdrive/common/visionimg.h
View File

@ -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

11
selfdrive/controls/controlsd.py
View File

@ -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

68
selfdrive/controls/lib/driver_monitor.py
View File

@ -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)

8
selfdrive/controls/lib/pathplanner.py
View File

@ -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)

2
selfdrive/debug/can_printer.py
View File

@ -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__":

99
selfdrive/debug/cpu_usage_stat.py 100644
View File

@ -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()])
))

14
selfdrive/debug/dump.py
View File

@ -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)

4
selfdrive/debug/get_fingerprint.py
View File

@ -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))

2
selfdrive/debug/getframes/getframes.py
View File

@ -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]))

86
selfdrive/locationd/Makefile 100644
View File

@ -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)

9
selfdrive/locationd/calibrationd.py
View File

@ -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))

4
selfdrive/locationd/kalman/ekf_sym.py
View File

@ -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

25
selfdrive/locationd/locationd_local.py
View File

@ -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"

0
selfdrive/locationd/test/__init__.py 100644
View File

82
selfdrive/locationd/test/ci_test.py 100755
View File

@ -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)

0
selfdrive/locationd/ephemeris.py → selfdrive/locationd/test/ephemeris.py
View File

0
selfdrive/locationd/ublox.py → selfdrive/locationd/test/ublox.py
View File

0
selfdrive/locationd/ubloxd.py → selfdrive/locationd/test/ubloxd.py
View File

55
selfdrive/locationd/test/ubloxd_easy.py 100755
View File

@ -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()

77
selfdrive/locationd/test/ubloxd_py_test.py 100755
View File

@ -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)

96
selfdrive/locationd/test/ubloxd_regression_test.py 100644
View File

@ -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)

375
selfdrive/locationd/ublox_msg.cc 100644
View File

@ -0,0 +1,375 @@
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <signal.h>
#include <unistd.h>
#include <sched.h>
#include <sys/time.h>
#include <sys/cdefs.h>
#include <sys/types.h>
#include <sys/time.h>
#include <assert.h>
#include <math.h>
#include <ctime>
#include <chrono>
#include <map>
#include <vector>
#include <algorithm>
#include <zmq.h>
#include <capnp/serialize.h>
#include "cereal/gen/cpp/log.capnp.h"
#include "common/params.h"
#include "common/swaglog.h"
#include "common/timing.h"
#include "ublox_msg.h"
#define UBLOX_MSG_SIZE(hdr) (*(uint16_t *)&hdr[4])
#define GET_FIELD_U(w, nb, pos) (((w) >> (pos)) & ((1<<(nb))-1))
namespace ublox {
inline int twos_complement(uint32_t v, uint32_t nb) {
int sign = v >> (nb - 1);
int value = v;
if(sign != 0)
value = value - (1 << nb);
return value;
}
inline int GET_FIELD_S(uint32_t w, uint32_t nb, uint32_t pos) {
int v = GET_FIELD_U(w, nb, pos);
return twos_complement(v, nb);
}
class EphemerisData {
public:
EphemerisData(uint8_t svId, subframes_map subframes) {
this->svId = svId;
int week_no = GET_FIELD_U(subframes[1][2+0], 10, 20);
int t_gd = GET_FIELD_S(subframes[1][2+4], 8, 6);
int iodc = (GET_FIELD_U(subframes[1][2+0], 2, 6) << 8) | GET_FIELD_U(
subframes[1][2+5], 8, 22);
int t_oc = GET_FIELD_U(subframes[1][2+5], 16, 6);
int a_f2 = GET_FIELD_S(subframes[1][2+6], 8, 22);
int a_f1 = GET_FIELD_S(subframes[1][2+6], 16, 6);
int a_f0 = GET_FIELD_S(subframes[1][2+7], 22, 8);
int c_rs = GET_FIELD_S(subframes[2][2+0], 16, 6);
int delta_n = GET_FIELD_S(subframes[2][2+1], 16, 14);
int m_0 = (GET_FIELD_S(subframes[2][2+1], 8, 6) << 24) | GET_FIELD_U(
subframes[2][2+2], 24, 6);
int c_uc = GET_FIELD_S(subframes[2][2+3], 16, 14);
int e = (GET_FIELD_U(subframes[2][2+3], 8, 6) << 24) | GET_FIELD_U(subframes[2][2+4], 24, 6);
int c_us = GET_FIELD_S(subframes[2][2+5], 16, 14);
uint32_t a_powhalf = (GET_FIELD_U(subframes[2][2+5], 8, 6) << 24) | GET_FIELD_U(
subframes[2][2+6], 24, 6);
int t_oe = GET_FIELD_U(subframes[2][2+7], 16, 14);
int c_ic = GET_FIELD_S(subframes[3][2+0], 16, 14);
int omega_0 = (GET_FIELD_S(subframes[3][2+0], 8, 6) << 24) | GET_FIELD_U(
subframes[3][2+1], 24, 6);
int c_is = GET_FIELD_S(subframes[3][2+2], 16, 14);
int i_0 = (GET_FIELD_S(subframes[3][2+2], 8, 6) << 24) | GET_FIELD_U(
subframes[3][2+3], 24, 6);
int c_rc = GET_FIELD_S(subframes[3][2+4], 16, 14);
int w = (GET_FIELD_S(subframes[3][2+4], 8, 6) << 24) | GET_FIELD_U(subframes[3][5], 24, 6);
int omega_dot = GET_FIELD_S(subframes[3][2+6], 24, 6);
int idot = GET_FIELD_S(subframes[3][2+7], 14, 8);
this->_rsvd1 = GET_FIELD_U(subframes[1][2+1], 23, 6);
this->_rsvd2 = GET_FIELD_U(subframes[1][2+2], 24, 6);
this->_rsvd3 = GET_FIELD_U(subframes[1][2+3], 24, 6);
this->_rsvd4 = GET_FIELD_U(subframes[1][2+4], 16, 14);
this->aodo = GET_FIELD_U(subframes[2][2+7], 5, 8);
double gpsPi = 3.1415926535898;
// now form variables in radians, meters and seconds etc
this->Tgd = t_gd * pow(2, -31);
this->A = pow(a_powhalf * pow(2, -19), 2.0);
this->cic = c_ic * pow(2, -29);
this->cis = c_is * pow(2, -29);
this->crc = c_rc * pow(2, -5);
this->crs = c_rs * pow(2, -5);
this->cuc = c_uc * pow(2, -29);
this->cus = c_us * pow(2, -29);
this->deltaN = delta_n * pow(2, -43) * gpsPi;
this->ecc = e * pow(2, -33);
this->i0 = i_0 * pow(2, -31) * gpsPi;
this->idot = idot * pow(2, -43) * gpsPi;
this->M0 = m_0 * pow(2, -31) * gpsPi;
this->omega = w * pow(2, -31) * gpsPi;
this->omega_dot = omega_dot * pow(2, -43) * gpsPi;
this->omega0 = omega_0 * pow(2, -31) * gpsPi;
this->toe = t_oe * pow(2, 4);
this->toc = t_oc * pow(2, 4);
this->gpsWeek = week_no;
this->af0 = a_f0 * pow(2, -31);
this->af1 = a_f1 * pow(2, -43);
this->af2 = a_f2 * pow(2, -55);
uint32_t iode1 = GET_FIELD_U(subframes[2][2+0], 8, 22);
uint32_t iode2 = GET_FIELD_U(subframes[3][2+7], 8, 22);
this->valid = (iode1 == iode2) && (iode1 == (iodc & 0xff));
this->iode = iode1;
if (GET_FIELD_U(subframes[4][2+0], 6, 22) == 56 &&
GET_FIELD_U(subframes[4][2+0], 2, 28) == 1 &&
GET_FIELD_U(subframes[5][2+0], 2, 28) == 1) {
double a0 = GET_FIELD_S(subframes[4][2], 8, 14) * pow(2, -30);
double a1 = GET_FIELD_S(subframes[4][2], 8, 6) * pow(2, -27);
double a2 = GET_FIELD_S(subframes[4][3], 8, 22) * pow(2, -24);
double a3 = GET_FIELD_S(subframes[4][3], 8, 14) * pow(2, -24);
double b0 = GET_FIELD_S(subframes[4][3], 8, 6) * pow(2, 11);
double b1 = GET_FIELD_S(subframes[4][4], 8, 22) * pow(2, 14);
double b2 = GET_FIELD_S(subframes[4][4], 8, 14) * pow(2, 16);
double b3 = GET_FIELD_S(subframes[4][4], 8, 6) * pow(2, 16);
this->ionoAlpha[0] = a0;this->ionoAlpha[1] = a1;this->ionoAlpha[2] = a2;this->ionoAlpha[3] = a3;
this->ionoBeta[0] = b0;this->ionoBeta[1] = b1;this->ionoBeta[2] = b2;this->ionoBeta[3] = b3;
this->ionoCoeffsValid = true;
} else {
this->ionoCoeffsValid = false;
}
}
uint16_t svId;
double Tgd, A, cic, cis, crc, crs, cuc, cus, deltaN, ecc, i0, idot, M0, omega, omega_dot, omega0, toe, toc;
uint32_t gpsWeek, iode, _rsvd1, _rsvd2, _rsvd3, _rsvd4, aodo;
double af0, af1, af2;
bool valid;
double ionoAlpha[4], ionoBeta[4];
bool ionoCoeffsValid;
};
UbloxMsgParser::UbloxMsgParser() :bytes_in_parse_buf(0) {
nav_frame_buffer[0U] = std::map<uint8_t, subframes_map>();
for(int i = 1;i < 33;i++)
nav_frame_buffer[0U][i] = subframes_map();
}
inline int UbloxMsgParser::needed_bytes() {
// Msg header incomplete?
if(bytes_in_parse_buf < UBLOX_HEADER_SIZE)
return UBLOX_HEADER_SIZE + UBLOX_CHECKSUM_SIZE - bytes_in_parse_buf;
uint16_t needed = UBLOX_MSG_SIZE(msg_parse_buf) + UBLOX_HEADER_SIZE + UBLOX_CHECKSUM_SIZE;
// too much data
if(needed < (uint16_t)bytes_in_parse_buf)
return -1;
return needed - (uint16_t)bytes_in_parse_buf;
}
inline bool UbloxMsgParser::valid_cheksum() {
uint8_t ck_a = 0, ck_b = 0;
for(int i = 2; i < bytes_in_parse_buf - UBLOX_CHECKSUM_SIZE;i++) {
ck_a = (ck_a + msg_parse_buf[i]) & 0xFF;
ck_b = (ck_b + ck_a) & 0xFF;
}
if(ck_a != msg_parse_buf[bytes_in_parse_buf - 2]) {
LOGD("Checksum a mismtach: %02X, %02X", ck_a, msg_parse_buf[6]);
return false;
}
if(ck_b != msg_parse_buf[bytes_in_parse_buf - 1]) {
LOGD("Checksum b mismtach: %02X, %02X", ck_b, msg_parse_buf[7]);
return false;
}
return true;
}
inline bool UbloxMsgParser::valid() {
return bytes_in_parse_buf >= UBLOX_HEADER_SIZE + UBLOX_CHECKSUM_SIZE &&
needed_bytes() == 0 &&
valid_cheksum();
}
inline bool UbloxMsgParser::valid_so_far() {
if(bytes_in_parse_buf > 0 && msg_parse_buf[0] != PREAMBLE1) {
//LOGD("PREAMBLE1 invalid, %02X.", msg_parse_buf[0]);
return false;
}
if(bytes_in_parse_buf > 1 && msg_parse_buf[1] != PREAMBLE2) {
//LOGD("PREAMBLE2 invalid, %02X.", msg_parse_buf[1]);
return false;
}
if(needed_bytes() == 0 && !valid())
return false;
return true;
}
kj::Array<capnp::word> UbloxMsgParser::gen_solution() {
nav_pvt_msg *msg = (nav_pvt_msg *)&msg_parse_buf[UBLOX_HEADER_SIZE];
capnp::MallocMessageBuilder msg_builder;
cereal::Event::Builder event = msg_builder.initRoot<cereal::Event>();
event.setLogMonoTime(nanos_since_boot());
auto gpsLoc = event.initGpsLocationExternal();
gpsLoc.setSource(cereal::GpsLocationData::SensorSource::UBLOX);
gpsLoc.setFlags(msg->flags);
gpsLoc.setLatitude(msg->lat * 1e-07);
gpsLoc.setLongitude(msg->lon * 1e-07);
gpsLoc.setAltitude(msg->height * 1e-03);
gpsLoc.setSpeed(msg->gSpeed * 1e-03);
gpsLoc.setBearing(msg->headMot * 1e-5);
gpsLoc.setAccuracy(msg->hAcc * 1e-03);
std::tm timeinfo = std::tm();
timeinfo.tm_year = msg->year - 1900;
timeinfo.tm_mon = msg->month - 1;
timeinfo.tm_mday = msg->day;
timeinfo.tm_hour = msg->hour;
timeinfo.tm_min = msg->min;
timeinfo.tm_sec = msg->sec;
std::time_t utc_tt = timegm(&timeinfo);
gpsLoc.setTimestamp(utc_tt * 1e+03 + msg->nano * 1e-06);
float f[] = { msg->velN * 1e-03f, msg->velE * 1e-03f, msg->velD * 1e-03f };
kj::ArrayPtr<const float> ap(&f[0], sizeof(f) / sizeof(f[0]));
gpsLoc.setVNED(ap);
gpsLoc.setVerticalAccuracy(msg->vAcc * 1e-03);
gpsLoc.setSpeedAccuracy(msg->sAcc * 1e-03);
gpsLoc.setBearingAccuracy(msg->headAcc * 1e-05);
return capnp::messageToFlatArray(msg_builder);
}
inline bool bit_to_bool(uint8_t val, int shifts) {
return (val & (1 << shifts)) ? true : false;
}
kj::Array<capnp::word> UbloxMsgParser::gen_raw() {
rxm_raw_msg *msg = (rxm_raw_msg *)&msg_parse_buf[UBLOX_HEADER_SIZE];
if(bytes_in_parse_buf != (
UBLOX_HEADER_SIZE + sizeof(rxm_raw_msg) + msg->numMeas * sizeof(rxm_raw_msg_extra) + UBLOX_CHECKSUM_SIZE
)) {
LOGD("Invalid measurement size %u, %u, %u, %u", msg->numMeas, bytes_in_parse_buf, sizeof(rxm_raw_msg_extra), sizeof(rxm_raw_msg));
return kj::Array<capnp::word>();
}
rxm_raw_msg_extra *measurements = (rxm_raw_msg_extra *)&msg_parse_buf[UBLOX_HEADER_SIZE + sizeof(rxm_raw_msg)];
capnp::MallocMessageBuilder msg_builder;
cereal::Event::Builder event = msg_builder.initRoot<cereal::Event>();
event.setLogMonoTime(nanos_since_boot());
auto gnss = event.initUbloxGnss();
auto mr = gnss.initMeasurementReport();
mr.setRcvTow(msg->rcvTow);
mr.setGpsWeek(msg->week);
mr.setLeapSeconds(msg->leapS);
mr.setGpsWeek(msg->week);
auto mb = mr.initMeasurements(msg->numMeas);
for(int8_t i = 0; i < msg->numMeas; i++) {
mb[i].setSvId(measurements[i].svId);
mb[i].setSigId(measurements[i].sigId);
mb[i].setPseudorange(measurements[i].prMes);
mb[i].setCarrierCycles(measurements[i].cpMes);
mb[i].setDoppler(measurements[i].doMes);
mb[i].setGnssId(measurements[i].gnssId);
mb[i].setGlonassFrequencyIndex(measurements[i].freqId);
mb[i].setLocktime(measurements[i].locktime);
mb[i].setCno(measurements[i].cno);
mb[i].setPseudorangeStdev(0.01*(pow(2, (measurements[i].prStdev & 15)))); // weird scaling, might be wrong
mb[i].setCarrierPhaseStdev(0.004*(measurements[i].cpStdev & 15));
mb[i].setDopplerStdev(0.002*(pow(2, (measurements[i].doStdev & 15)))); // weird scaling, might be wrong
auto ts = mb[i].initTrackingStatus();
ts.setPseudorangeValid(bit_to_bool(measurements[i].trkStat, 0));
ts.setCarrierPhaseValid(bit_to_bool(measurements[i].trkStat, 1));
ts.setHalfCycleValid(bit_to_bool(measurements[i].trkStat, 2));
ts.setHalfCycleSubtracted(bit_to_bool(measurements[i].trkStat, 3));
}
mr.setNumMeas(msg->numMeas);
auto rs = mr.initReceiverStatus();
rs.setLeapSecValid(bit_to_bool(msg->recStat, 0));
rs.setClkReset(bit_to_bool(msg->recStat, 2));
return capnp::messageToFlatArray(msg_builder);
}
kj::Array<capnp::word> UbloxMsgParser::gen_nav_data() {
rxm_sfrbx_msg *msg = (rxm_sfrbx_msg *)&msg_parse_buf[UBLOX_HEADER_SIZE];
if(bytes_in_parse_buf != (
UBLOX_HEADER_SIZE + sizeof(rxm_sfrbx_msg) + msg->numWords * sizeof(rxm_sfrbx_msg_extra) + UBLOX_CHECKSUM_SIZE
)) {
LOGD("Invalid sfrbx words size %u, %u, %u, %u", msg->numWords, bytes_in_parse_buf, sizeof(rxm_raw_msg_extra), sizeof(rxm_raw_msg));
return kj::Array<capnp::word>();
}
rxm_sfrbx_msg_extra *measurements = (rxm_sfrbx_msg_extra *)&msg_parse_buf[UBLOX_HEADER_SIZE + sizeof(rxm_sfrbx_msg)];
if(msg->gnssId == 0) {
uint8_t subframeId = GET_FIELD_U(measurements[1].dwrd, 3, 8);
std::vector<uint32_t> words;
for(int i = 0; i < msg->numWords;i++)
words.push_back(measurements[i].dwrd);
if(subframeId == 1) {
nav_frame_buffer[msg->gnssId][msg->svid] = subframes_map();
nav_frame_buffer[msg->gnssId][msg->svid][subframeId] = words;
} else if(nav_frame_buffer[msg->gnssId][msg->svid].find(subframeId-1) != nav_frame_buffer[msg->gnssId][msg->svid].end())
nav_frame_buffer[msg->gnssId][msg->svid][subframeId] = words;
if(nav_frame_buffer[msg->gnssId][msg->svid].size() == 5) {
EphemerisData ephem_data(msg->svid, nav_frame_buffer[msg->gnssId][msg->svid]);
capnp::MallocMessageBuilder msg_builder;
cereal::Event::Builder event = msg_builder.initRoot<cereal::Event>();
event.setLogMonoTime(nanos_since_boot());
auto gnss = event.initUbloxGnss();
auto eph = gnss.initEphemeris();
eph.setSvId(ephem_data.svId);
eph.setToc(ephem_data.toc);
eph.setGpsWeek(ephem_data.gpsWeek);
eph.setAf0(ephem_data.af0);
eph.setAf1(ephem_data.af1);
eph.setAf2(ephem_data.af2);
eph.setIode(ephem_data.iode);
eph.setCrs(ephem_data.crs);
eph.setDeltaN(ephem_data.deltaN);
eph.setM0(ephem_data.M0);
eph.setCuc(ephem_data.cuc);
eph.setEcc(ephem_data.ecc);
eph.setCus(ephem_data.cus);
eph.setA(ephem_data.A);
eph.setToe(ephem_data.toe);
eph.setCic(ephem_data.cic);
eph.setOmega0(ephem_data.omega0);
eph.setCis(ephem_data.cis);
eph.setI0(ephem_data.i0);
eph.setCrc(ephem_data.crc);
eph.setOmega(ephem_data.omega);
eph.setOmegaDot(ephem_data.omega_dot);
eph.setIDot(ephem_data.idot);
eph.setTgd(ephem_data.Tgd);
eph.setIonoCoeffsValid(ephem_data.ionoCoeffsValid);
if(ephem_data.ionoCoeffsValid) {
kj::ArrayPtr<const double> apa(&ephem_data.ionoAlpha[0], sizeof(ephem_data.ionoAlpha) / sizeof(ephem_data.ionoAlpha[0]));
eph.setIonoAlpha(apa);
kj::ArrayPtr<const double> apb(&ephem_data.ionoBeta[0], sizeof(ephem_data.ionoBeta) / sizeof(ephem_data.ionoBeta[0]));
eph.setIonoBeta(apb);
} else {
eph.setIonoAlpha(kj::ArrayPtr<const double>());
eph.setIonoBeta(kj::ArrayPtr<const double>());
}
return capnp::messageToFlatArray(msg_builder);
}
}
return kj::Array<capnp::word>();
}
bool UbloxMsgParser::add_data(const uint8_t *incoming_data, uint32_t incoming_data_len, size_t &bytes_consumed) {
int needed = needed_bytes();
if(needed > 0) {
bytes_consumed = min((size_t)needed, incoming_data_len );
// Add data to buffer
memcpy(msg_parse_buf + bytes_in_parse_buf, incoming_data, bytes_consumed);
bytes_in_parse_buf += bytes_consumed;
} else {
bytes_consumed = incoming_data_len;
}
// Validate msg format, detect invalid header and invalid checksum.
while(!valid_so_far() && bytes_in_parse_buf != 0) {
//LOGD("Drop corrupt data, remained in buf: %u", bytes_in_parse_buf);
// Corrupted msg, drop a byte.
bytes_in_parse_buf -= 1;
if(bytes_in_parse_buf > 0)
memmove(&msg_parse_buf[0], &msg_parse_buf[1], bytes_in_parse_buf);
}
// There is redundant data at the end of buffer, reset the buffer.
if(needed_bytes() == -1)
bytes_in_parse_buf = 0;
return valid();
}
}

149
selfdrive/locationd/ublox_msg.h 100644
View File

@ -0,0 +1,149 @@
#pragma once
#include <stdint.h>
#define min(x, y) ((x) <= (y) ? (x) : (y))
// NAV_PVT
typedef struct __attribute__((packed)) {
uint32_t iTOW;
uint16_t year;
int8_t month;
int8_t day;
int8_t hour;
int8_t min;
int8_t sec;
int8_t valid;
uint32_t tAcc;
int32_t nano;
int8_t fixType;
int8_t flags;
int8_t flags2;
int8_t numSV;
int32_t lon;
int32_t lat;
int32_t height;
int32_t hMSL;
uint32_t hAcc;
uint32_t vAcc;
int32_t velN;
int32_t velE;
int32_t velD;
int32_t gSpeed;
int32_t headMot;
uint32_t sAcc;
uint32_t headAcc;
uint16_t pDOP;
int8_t reserverd1[6];
int32_t headVeh;
int16_t magDec;
uint16_t magAcc;
} nav_pvt_msg;
// RXM_RAW
typedef struct __attribute__((packed)) {
double rcvTow;
uint16_t week;
int8_t leapS;
int8_t numMeas;
int8_t recStat;
int8_t reserved1[3];
} rxm_raw_msg;
// Extra data count is in numMeas
typedef struct __attribute__((packed)) {
double prMes;
double cpMes;
float doMes;
int8_t gnssId;
int8_t svId;
int8_t sigId;
int8_t freqId;
uint16_t locktime;
int8_t cno;
int8_t prStdev;
int8_t cpStdev;
int8_t doStdev;
int8_t trkStat;
int8_t reserved3;
} rxm_raw_msg_extra;
// RXM_SFRBX
typedef struct __attribute__((packed)) {
int8_t gnssId;
int8_t svid;
int8_t reserved1;
int8_t freqId;
int8_t numWords;
int8_t reserved2;
int8_t version;
int8_t reserved3;
} rxm_sfrbx_msg;
// Extra data count is in numWords
typedef struct __attribute__((packed)) {
uint32_t dwrd;
} rxm_sfrbx_msg_extra;
namespace ublox {
// protocol constants
const uint8_t PREAMBLE1 = 0xb5;
const uint8_t PREAMBLE2 = 0x62;
// message classes
const uint8_t CLASS_NAV = 0x01;
const uint8_t CLASS_RXM = 0x02;
// NAV messages
const uint8_t MSG_NAV_PVT = 0x7;
// RXM messages
const uint8_t MSG_RXM_RAW = 0x15;
const uint8_t MSG_RXM_SFRBX = 0x13;
const int UBLOX_HEADER_SIZE = 6;
const int UBLOX_CHECKSUM_SIZE = 2;
const int UBLOX_MAX_MSG_SIZE = 65536;
typedef std::map<uint8_t, std::vector<uint32_t>> subframes_map;
class UbloxMsgParser {
public:
UbloxMsgParser();
kj::Array<capnp::word> gen_solution();
kj::Array<capnp::word> gen_raw();
kj::Array<capnp::word> gen_nav_data();
bool add_data(const uint8_t *incoming_data, uint32_t incoming_data_len, size_t &bytes_consumed);
inline void reset() {bytes_in_parse_buf = 0;}
inline uint8_t msg_class() {
return msg_parse_buf[2];
}
inline uint8_t msg_id() {
return msg_parse_buf[3];
}
inline int needed_bytes();
void hexdump(uint8_t *d, int l) {
for (int i = 0; i < l; i++) {
if (i%0x10 == 0 && i != 0) printf("\n");
printf("%02X ", d[i]);
}
printf("\n");
}
private:
inline bool valid_cheksum();
inline bool valid();
inline bool valid_so_far();
uint8_t msg_parse_buf[UBLOX_HEADER_SIZE + UBLOX_MAX_MSG_SIZE];
int bytes_in_parse_buf;
std::map<uint8_t, std::map<uint8_t, subframes_map>> nav_frame_buffer;
};
}
typedef int (*poll_ubloxraw_msg_func)(void *gpsLocationExternal, void *ubloxGnss, void *subscriber, zmq_msg_t *msg);
typedef int (*send_gps_event_func)(uint8_t msg_cls, uint8_t msg_id, void *s, const void *buf, size_t len, int flags);
int ubloxd_main(poll_ubloxraw_msg_func poll_func, send_gps_event_func send_func);

45
selfdrive/locationd/ubloxd.cc 100644
View File

@ -0,0 +1,45 @@
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <signal.h>
#include <unistd.h>
#include <sched.h>
#include <sys/time.h>
#include <sys/cdefs.h>
#include <sys/types.h>
#include <sys/time.h>
#include <assert.h>
#include <math.h>
#include <ctime>
#include <chrono>
#include <map>
#include <vector>
#include <zmq.h>
#include <capnp/serialize.h>
#include "cereal/gen/cpp/log.capnp.h"
#include "common/params.h"
#include "common/swaglog.h"
#include "common/timing.h"
#include "ublox_msg.h"
const long ZMQ_POLL_TIMEOUT = 1000; // In miliseconds
int poll_ubloxraw_msg(void *gpsLocationExternal, void *ubloxGnss, void *subscriber, zmq_msg_t *msg) {
int err;
zmq_pollitem_t item = {.socket = subscriber, .events = ZMQ_POLLIN};
err = zmq_poll (&item, 1, ZMQ_POLL_TIMEOUT);
if(err <= 0)
return err;
return zmq_msg_recv(msg, subscriber, 0);
}
int send_gps_event(uint8_t msg_cls, uint8_t msg_id, void *s, const void *buf, size_t len, int flags) {
return zmq_send(s, buf, len, flags);
}
int main() {
return ubloxd_main(poll_ubloxraw_msg, send_gps_event);
}

113
selfdrive/locationd/ubloxd_main.cc 100644
View File

@ -0,0 +1,113 @@
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <signal.h>
#include <unistd.h>
#include <sched.h>
#include <sys/time.h>
#include <sys/cdefs.h>
#include <sys/types.h>
#include <sys/time.h>
#include <assert.h>
#include <math.h>
#include <ctime>
#include <chrono>
#include <map>
#include <vector>
#include <zmq.h>
#include <capnp/serialize.h>
#include "cereal/gen/cpp/log.capnp.h"
#include "common/params.h"
#include "common/swaglog.h"
#include "common/timing.h"
#include "ublox_msg.h"
volatile int do_exit = 0; // Flag for process exit on signal
void set_do_exit(int sig) {
do_exit = 1;
}
using namespace ublox;
int ubloxd_main(poll_ubloxraw_msg_func poll_func, send_gps_event_func send_func) {
LOGW("starting ubloxd");
signal(SIGINT, (sighandler_t) set_do_exit);
signal(SIGTERM, (sighandler_t) set_do_exit);
UbloxMsgParser parser;
void *context = zmq_ctx_new();
void *gpsLocationExternal = zmq_socket(context, ZMQ_PUB);
zmq_bind(gpsLocationExternal, "tcp://*:8032");
void *ubloxGnss = zmq_socket(context, ZMQ_PUB);
zmq_bind(ubloxGnss, "tcp://*:8033");
// ubloxRaw = 8042
void *subscriber = zmq_socket(context, ZMQ_SUB);
zmq_setsockopt(subscriber, ZMQ_SUBSCRIBE, "", 0);
zmq_connect(subscriber, "tcp://127.0.0.1:8042");
while (!do_exit) {
zmq_msg_t msg;
zmq_msg_init(&msg);
int err = poll_func(gpsLocationExternal, ubloxGnss, subscriber, &msg);
if(err < 0) {
LOGE_100("zmq_poll error %s in %s", strerror(errno ), __FUNCTION__);
break;
} else if(err == 0) {
continue;
}
// format for board, make copy due to alignment issues, will be freed on out of scope
auto amsg = kj::heapArray<capnp::word>((zmq_msg_size(&msg) / sizeof(capnp::word)) + 1);
memcpy(amsg.begin(), zmq_msg_data(&msg), zmq_msg_size(&msg));
capnp::FlatArrayMessageReader cmsg(amsg);
cereal::Event::Reader event = cmsg.getRoot<cereal::Event>();
const uint8_t *data = event.getUbloxRaw().begin();
size_t len = event.getUbloxRaw().size();
size_t bytes_consumed = 0;
while(bytes_consumed < len && !do_exit) {
size_t bytes_consumed_this_time = 0U;
if(parser.add_data(data + bytes_consumed, (uint32_t)(len - bytes_consumed), bytes_consumed_this_time)) {
// New message available
if(parser.msg_class() == CLASS_NAV) {
if(parser.msg_id() == MSG_NAV_PVT) {
LOGD("MSG_NAV_PVT");
auto words = parser.gen_solution();
if(words.size() > 0) {
auto bytes = words.asBytes();
send_func(parser.msg_class(), parser.msg_id(), gpsLocationExternal, bytes.begin(), bytes.size(), 0);
}
} else
LOGW("Unknown nav msg id: 0x%02X", parser.msg_id());
} else if(parser.msg_class() == CLASS_RXM) {
if(parser.msg_id() == MSG_RXM_RAW) {
LOGD("MSG_RXM_RAW");
auto words = parser.gen_raw();
if(words.size() > 0) {
auto bytes = words.asBytes();
send_func(parser.msg_class(), parser.msg_id(), ubloxGnss, bytes.begin(), bytes.size(), 0);
}
} else if(parser.msg_id() == MSG_RXM_SFRBX) {
LOGD("MSG_RXM_SFRBX");
auto words = parser.gen_nav_data();
if(words.size() > 0) {
auto bytes = words.asBytes();
send_func(parser.msg_class(), parser.msg_id(), ubloxGnss, bytes.begin(), bytes.size(), 0);
}
} else
LOGW("Unknown rxm msg id: 0x%02X", parser.msg_id());
} else
LOGW("Unknown msg class: 0x%02X", parser.msg_class());
parser.reset();
}
bytes_consumed += bytes_consumed_this_time;
}
zmq_msg_close(&msg);
}
zmq_close(subscriber);
zmq_close(gpsLocationExternal);
zmq_close(ubloxGnss);
zmq_ctx_destroy(context);
return 0;
}

103
selfdrive/locationd/ubloxd_test.cc 100644
View File

@ -0,0 +1,103 @@
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <signal.h>
#include <unistd.h>
#include <sched.h>
#include <sys/time.h>
#include <sys/cdefs.h>
#include <sys/types.h>
#include <sys/time.h>
#include <assert.h>
#include <math.h>
#include <ctime>
#include <chrono>
#include <map>
#include <vector>
#include <iostream>
#include <zmq.h>
#include <capnp/serialize.h>
#include "cereal/gen/cpp/log.capnp.h"
#include "common/params.h"
#include "common/swaglog.h"
#include "common/timing.h"
#include "common/util.h"
#include "ublox_msg.h"
using namespace ublox;
void write_file(std::string fpath, uint8_t *data, int len) {
FILE* f = fopen(fpath.c_str(), "wb");
if (!f) {
std::cout << "Open " << fpath << " failed" << std::endl;
return;
}
fwrite(data, len, 1, f);
fclose(f);
}
static size_t len = 0U;
static size_t consumed = 0U;
static uint8_t *data = NULL;
static int save_idx = 0;
static std::string prefix;
static void *gps_sock, *ublox_gnss_sock;
int poll_ubloxraw_msg(void *gpsLocationExternal, void *ubloxGnss, void *subscriber, zmq_msg_t *msg) {
gps_sock = gpsLocationExternal;
ublox_gnss_sock = ubloxGnss;
size_t consuming = min(len - consumed, 128);
if(consumed < len) {
// create message
capnp::MallocMessageBuilder msg_builder;
cereal::Event::Builder event = msg_builder.initRoot<cereal::Event>();
event.setLogMonoTime(nanos_since_boot());
auto ublox_raw = event.initUbloxRaw(consuming);
memcpy(ublox_raw.begin(), (void *)(data + consumed), consuming);
auto words = capnp::messageToFlatArray(msg_builder);
auto bytes = words.asBytes();
zmq_msg_init_size (msg, bytes.size());
memcpy (zmq_msg_data(msg), (void *)bytes.begin(), bytes.size());
consumed += consuming;
return 1;
} else
return -1;
}
int send_gps_event(uint8_t msg_cls, uint8_t msg_id, void *s, const void *buf, size_t len, int flags) {
if(msg_cls == CLASS_NAV && msg_id == MSG_NAV_PVT)
assert(s == gps_sock);
else if(msg_cls == CLASS_RXM && msg_id == MSG_RXM_RAW)
assert(s == ublox_gnss_sock);
else if(msg_cls == CLASS_RXM && msg_id == MSG_RXM_SFRBX)
assert(s == ublox_gnss_sock);
else
assert(0);
write_file(prefix + "/" + std::to_string(save_idx), (uint8_t *)buf, len);
save_idx ++;
return len;
}
int main(int argc, char** argv) {
if(argc < 3) {
printf("Format: ubloxd_test stream_file_path save_prefix\n");
return 0;
}
// Parse 11360 msgs, generate 9452 events
data = (uint8_t *)read_file(argv[1], &len);
if(data == NULL) {
LOGE("Read file %s failed\n", argv[1]);
return -1;
}
prefix = argv[2];
ubloxd_main(poll_ubloxraw_msg, send_gps_event);
free(data);
printf("Generated %d cereal events\n", save_idx);
if(save_idx != 9452) {
printf("Event count error: %d\n", save_idx);
return -1;
}
return 0;
}

BIN
selfdrive/loggerd/loggerd
View File

Binary file not shown.

2
selfdrive/manager.py
View File

@ -91,7 +91,7 @@ managed_processes = {
"controlsd": "selfdrive.controls.controlsd",
"plannerd": "selfdrive.controls.plannerd",
"radard": "selfdrive.controls.radard",
"ubloxd": "selfdrive.locationd.ubloxd",
"ubloxd": ("selfdrive/locationd", ["./ubloxd"]),
"mapd": "selfdrive.mapd.mapd",
"loggerd": ("selfdrive/loggerd", ["./loggerd"]),
"logmessaged": "selfdrive.logmessaged",

2
selfdrive/mapd/mapd.py
View File

@ -121,7 +121,7 @@ def query_thread():
query_lock.release()
except Exception as e:
print e
print(e)
query_lock.acquire()
last_query_result = None
query_lock.release()

8
selfdrive/orbd/.gitignore vendored
View File

@ -1,8 +0,0 @@
orbd
orbd_cpu
test/turbocv_profile
test/turbocv_test
dspout/*
dumb_test
bilinear_lut.h
orb_lut.h

105
selfdrive/orbd/Makefile
View File

@ -1,105 +0,0 @@
# CPU
CC = clang
CXX = clang++
WARN_FLAGS = -Werror=implicit-function-declaration \
-Werror=incompatible-pointer-types \
-Werror=int-conversion \
-Werror=return-type \
-Werror=format-extra-args
JSON_FLAGS = -I$(PHONELIBS)/json/src
CFLAGS = -std=gnu11 -g -O2 -fPIC $(WARN_FLAGS) -Iinclude $(JSON_FLAGS) -I.
CXXFLAGS = -std=c++11 -g -O2 -fPIC $(WARN_FLAGS) -Iinclude $(JSON_FLAGS) -I.
LDFLAGS =
# profile
# CXXFLAGS += -DTURBOCV_PROFILE=1
PHONELIBS = ../../phonelibs
BASEDIR = ../..
EXTERNAL = ../../external
PYTHONLIBS =
UNAME_M := $(shell uname -m)
ifeq ($(UNAME_M),x86_64)
# computer
ZMQ_FLAGS = -I$(PHONELIBS)/zmq/aarch64/include
ZMQ_LIBS = -L$(BASEDIR)/external/zmq/lib/ \
-l:libczmq.a -l:libzmq.a -lpthread
OPENCV_LIBS = -lopencv_core -lopencv_highgui -lopencv_features2d -lopencv_imgproc
CXXFLAGS += -fopenmp
LDFLAGS += -lomp
else
# phone
ZMQ_FLAGS = -I$(PHONELIBS)/zmq/aarch64/include
ZMQ_LIBS = -L$(PHONELIBS)/zmq/aarch64/lib \
-l:libczmq.a -l:libzmq.a \
-lgnustl_shared
OPENCV_FLAGS = -I$(PHONELIBS)/opencv/include
OPENCV_LIBS = -Wl,--enable-new-dtags -Wl,-rpath,/usr/local/lib/python2.7/site-packages -L/usr/local/lib/python2.7/site-packages -l:cv2.so
endif
.PHONY: all
all: orbd
include ../common/cereal.mk
DEP_OBJS = ../common/visionipc.o ../common/ipc.o ../common/swaglog.o $(PHONELIBS)/json/src/json.o
orbd: orbd_dsp.o $(DEP_OBJS) calculator_stub.o freethedsp.o
@echo "[ LINK ] $@"
$(CXX) -fPIC -o '$@' $^ \
$(LDFLAGS) \
$(ZMQ_LIBS) \
$(CEREAL_LIBS) \
-L/usr/lib \
-L/system/vendor/lib64 \
-ladsprpc \
-lm -lz -llog
%.o: %.c
@echo "[ CC ] $@"
$(CC) $(CFLAGS) \
$(ZMQ_FLAGS) \
-I../ \
-I../../ \
-c -o '$@' '$<'
orbd_dsp.o: orbd.cc
@echo "[ CXX ] $@"
$(CXX) $(CXXFLAGS) \
$(CEREAL_CXXFLAGS) \
$(ZMQ_FLAGS) \
$(OPENCV_FLAGS) \
-DDSP \
-I../ \
-I../../ \
-I../../../ \
-I./include \
-c -o '$@' '$<'
freethedsp.o: dsp/freethedsp.c
@echo "[ CC ] $@"
$(CC) $(CFLAGS) \
-c -o '$@' '$<'
calculator_stub.o: dsp/gen/calculator_stub.c
@echo "[ CC ] $@"
$(CC) $(CFLAGS) -I./include -c -o '$@' '$<'
-include internal.mk
.PHONY: clean
clean:
rm -f *.o turbocv.so orbd test/turbocv_profile test/turbocv_test test/*.o *_lut.h

119
selfdrive/orbd/dsp/freethedsp.c
View File

@ -1,119 +0,0 @@
// freethedsp by geohot
// (because the DSP should be free)
// released under MIT License
// usage instructions:
// 1. Compile an example from the Qualcomm Hexagon SDK
// 2. Try to run it on your phone
// 3. Be very sad when "adsprpc ... dlopen error: ... signature verify start failed for ..." appears in logcat
// ...here is where people would give up before freethedsp
// 4. Compile freethedsp with 'clang -shared freethedsp.c -o freethedsp.so' (or statically link it to your program)
// 5. Run your program with 'LD_PRELOAD=./freethedsp.so ./<your_prog>'
// 6. OMG THE DSP WORKS
// 7. Be happy.
// *** patch may have to change for your phone ***
// this is patching /dsp/fastrpc_shell_0
// correct if sha hash of fastrpc_shell_0 is "fbadc96848aefad99a95aa4edb560929dcdf78f8"
// patch to return 0xFFFFFFFF from is_test_enabled instead of 0
// your fastrpc_shell_0 may vary
#define PATCH_ADDR 0x5200c
#define PATCH_OLD "\x40\x3f\x20\x50"
#define PATCH_NEW "\x40\x3f\x00\x5a"
#define PATCH_LEN (sizeof(PATCH_OLD)-1)
#define _BITS_IOCTL_H_
// under 100 lines of code begins now
#include <stdio.h>
#include <dlfcn.h>
#include <assert.h>
#include <stdlib.h>
#include <unistd.h>
// ioctl stuff
#define IOC_OUT 0x40000000 /* copy out parameters */
#define IOC_IN 0x80000000 /* copy in parameters */
#define IOC_INOUT (IOC_IN|IOC_OUT)
#define IOCPARM_MASK 0x1fff /* parameter length, at most 13 bits */
#define _IOC(inout,group,num,len) \
(inout | ((len & IOCPARM_MASK) << 16) | ((group) << 8) | (num))
#define _IOWR(g,n,t) _IOC(IOC_INOUT, (g), (n), sizeof(t))
// ion ioctls
#include <linux/ion.h>
#define ION_IOC_MSM_MAGIC 'M'
#define ION_IOC_CLEAN_INV_CACHES _IOWR(ION_IOC_MSM_MAGIC, 2, \
struct ion_flush_data)
struct ion_flush_data {
ion_user_handle_t handle;
int fd;
void *vaddr;
unsigned int offset;
unsigned int length;
};
// fastrpc ioctls
#define FASTRPC_IOCTL_INIT _IOWR('R', 6, struct fastrpc_ioctl_init)
struct fastrpc_ioctl_init {
uint32_t flags; /* one of FASTRPC_INIT_* macros */
uintptr_t __user file; /* pointer to elf file */
int32_t filelen; /* elf file length */
int32_t filefd; /* ION fd for the file */
uintptr_t __user mem; /* mem for the PD */
int32_t memlen; /* mem length */
int32_t memfd; /* ION fd for the mem */
};
int ioctl(int fd, unsigned long request, void *arg) {
static void *handle = NULL;
static int (*orig_ioctl)(int, int, void*);
if (handle == NULL) {
handle = dlopen("/system/lib64/libc.so", RTLD_LAZY);
assert(handle != NULL);
orig_ioctl = dlsym(handle, "ioctl");
}
int ret = orig_ioctl(fd, request, arg);
// carefully modify this one
if (request == FASTRPC_IOCTL_INIT) {
struct fastrpc_ioctl_init *init = (struct fastrpc_ioctl_init *)arg;
// confirm patch is correct and do the patch
assert(memcmp((void*)(init->mem+PATCH_ADDR), PATCH_OLD, PATCH_LEN) == 0);
memcpy((void*)(init->mem+PATCH_ADDR), PATCH_NEW, PATCH_LEN);
// flush cache
int ionfd = open("/dev/ion", O_RDONLY);
assert(ionfd > 0);
struct ion_fd_data fd_data;
fd_data.fd = init->memfd;
int ret = ioctl(ionfd, ION_IOC_IMPORT, &fd_data);
assert(ret == 0);
struct ion_flush_data flush_data;
flush_data.handle = fd_data.handle;
flush_data.vaddr = (void*)init->mem;
flush_data.offset = 0;
flush_data.length = init->memlen;
ret = ioctl(ionfd, ION_IOC_CLEAN_INV_CACHES, &flush_data);
assert(ret == 0);
struct ion_handle_data handle_data;
handle_data.handle = fd_data.handle;
ret = ioctl(ionfd, ION_IOC_FREE, &handle_data);
assert(ret == 0);
// cleanup
close(ionfd);
}
return ret;
}

39
selfdrive/orbd/dsp/gen/calculator.h
View File

@ -1,39 +0,0 @@
#ifndef _CALCULATOR_H
#define _CALCULATOR_H
#include <stdint.h>
typedef uint8_t uint8;
typedef uint32_t uint32;
#ifndef __QAIC_HEADER
#define __QAIC_HEADER(ff) ff
#endif //__QAIC_HEADER
#ifndef __QAIC_HEADER_EXPORT
#define __QAIC_HEADER_EXPORT
#endif // __QAIC_HEADER_EXPORT
#ifndef __QAIC_HEADER_ATTRIBUTE
#define __QAIC_HEADER_ATTRIBUTE
#endif // __QAIC_HEADER_ATTRIBUTE
#ifndef __QAIC_IMPL
#define __QAIC_IMPL(ff) ff
#endif //__QAIC_IMPL
#ifndef __QAIC_IMPL_EXPORT
#define __QAIC_IMPL_EXPORT
#endif // __QAIC_IMPL_EXPORT
#ifndef __QAIC_IMPL_ATTRIBUTE
#define __QAIC_IMPL_ATTRIBUTE
#endif // __QAIC_IMPL_ATTRIBUTE
#ifdef __cplusplus
extern "C" {
#endif
__QAIC_HEADER_EXPORT int __QAIC_HEADER(calculator_init)(uint32* leet) __QAIC_HEADER_ATTRIBUTE;
__QAIC_HEADER_EXPORT int __QAIC_HEADER(calculator_extract_and_match)(const uint8* img, int imgLen, uint8* features, int featuresLen) __QAIC_HEADER_ATTRIBUTE;
#ifdef __cplusplus
}
#endif
#endif //_CALCULATOR_H

613
selfdrive/orbd/dsp/gen/calculator_stub.c
View File

@ -1,613 +0,0 @@
#ifndef _CALCULATOR_STUB_H
#define _CALCULATOR_STUB_H
#include "calculator.h"
// remote.h
#include <stdint.h>
#include <sys/types.h>
typedef uint32_t remote_handle;
typedef uint64_t remote_handle64;
typedef struct {
void *pv;
size_t nLen;
} remote_buf;
typedef struct {
int32_t fd;
uint32_t offset;
} remote_dma_handle;
typedef union {
remote_buf buf;
remote_handle h;
remote_handle64 h64;
remote_dma_handle dma;
} remote_arg;
int remote_handle_open(const char* name, remote_handle *ph);
int remote_handle_invoke(remote_handle h, uint32_t dwScalars, remote_arg *pra);
int remote_handle_close(remote_handle h);
#define REMOTE_SCALARS_MAKEX(nAttr,nMethod,nIn,nOut,noIn,noOut) \
((((uint32_t) (nAttr) & 0x7) << 29) | \
(((uint32_t) (nMethod) & 0x1f) << 24) | \
(((uint32_t) (nIn) & 0xff) << 16) | \
(((uint32_t) (nOut) & 0xff) << 8) | \
(((uint32_t) (noIn) & 0x0f) << 4) | \
((uint32_t) (noOut) & 0x0f))
#ifndef _QAIC_ENV_H
#define _QAIC_ENV_H
#ifdef __GNUC__
#ifdef __clang__
#pragma GCC diagnostic ignored "-Wunknown-pragmas"
#else
#pragma GCC diagnostic ignored "-Wpragmas"
#endif
#pragma GCC diagnostic ignored "-Wuninitialized"
#pragma GCC diagnostic ignored "-Wunused-parameter"
#pragma GCC diagnostic ignored "-Wunused-function"
#endif
#ifndef _ATTRIBUTE_UNUSED
#ifdef _WIN32
#define _ATTRIBUTE_UNUSED
#else
#define _ATTRIBUTE_UNUSED __attribute__ ((unused))
#endif
#endif // _ATTRIBUTE_UNUSED
#ifndef __QAIC_REMOTE
#define __QAIC_REMOTE(ff) ff
#endif //__QAIC_REMOTE
#ifndef __QAIC_HEADER
#define __QAIC_HEADER(ff) ff
#endif //__QAIC_HEADER
#ifndef __QAIC_HEADER_EXPORT
#define __QAIC_HEADER_EXPORT
#endif // __QAIC_HEADER_EXPORT
#ifndef __QAIC_HEADER_ATTRIBUTE
#define __QAIC_HEADER_ATTRIBUTE
#endif // __QAIC_HEADER_ATTRIBUTE
#ifndef __QAIC_IMPL
#define __QAIC_IMPL(ff) ff
#endif //__QAIC_IMPL
#ifndef __QAIC_IMPL_EXPORT
#define __QAIC_IMPL_EXPORT
#endif // __QAIC_IMPL_EXPORT
#ifndef __QAIC_IMPL_ATTRIBUTE
#define __QAIC_IMPL_ATTRIBUTE
#endif // __QAIC_IMPL_ATTRIBUTE
#ifndef __QAIC_STUB
#define __QAIC_STUB(ff) ff
#endif //__QAIC_STUB
#ifndef __QAIC_STUB_EXPORT
#define __QAIC_STUB_EXPORT
#endif // __QAIC_STUB_EXPORT
#ifndef __QAIC_STUB_ATTRIBUTE
#define __QAIC_STUB_ATTRIBUTE
#endif // __QAIC_STUB_ATTRIBUTE
#ifndef __QAIC_SKEL
#define __QAIC_SKEL(ff) ff
#endif //__QAIC_SKEL__
#ifndef __QAIC_SKEL_EXPORT
#define __QAIC_SKEL_EXPORT
#endif // __QAIC_SKEL_EXPORT
#ifndef __QAIC_SKEL_ATTRIBUTE
#define __QAIC_SKEL_ATTRIBUTE
#endif // __QAIC_SKEL_ATTRIBUTE
#ifdef __QAIC_DEBUG__
#ifndef __QAIC_DBG_PRINTF__
#include <stdio.h>
#define __QAIC_DBG_PRINTF__( ee ) do { printf ee ; } while(0)
#endif
#else
#define __QAIC_DBG_PRINTF__( ee ) (void)0
#endif
#define _OFFSET(src, sof) ((void*)(((char*)(src)) + (sof)))
#define _COPY(dst, dof, src, sof, sz) \
do {\
struct __copy { \
char ar[sz]; \
};\
*(struct __copy*)_OFFSET(dst, dof) = *(struct __copy*)_OFFSET(src, sof);\
} while (0)
#define _COPYIF(dst, dof, src, sof, sz) \
do {\
if(_OFFSET(dst, dof) != _OFFSET(src, sof)) {\
_COPY(dst, dof, src, sof, sz); \
} \
} while (0)
_ATTRIBUTE_UNUSED
static __inline void _qaic_memmove(void* dst, void* src, int size) {
int i;
for(i = 0; i < size; ++i) {
((char*)dst)[i] = ((char*)src)[i];
}
}
#define _MEMMOVEIF(dst, src, sz) \
do {\
if(dst != src) {\
_qaic_memmove(dst, src, sz);\
} \
} while (0)
#define _ASSIGN(dst, src, sof) \
do {\
dst = OFFSET(src, sof); \
} while (0)
#define _STD_STRLEN_IF(str) (str == 0 ? 0 : strlen(str))
#define AEE_SUCCESS 0
#define AEE_EOFFSET 0x80000400
#define AEE_EBADPARM (AEE_EOFFSET + 0x00E)
#define _TRY(ee, func) \
do { \
if (AEE_SUCCESS != ((ee) = func)) {\
__QAIC_DBG_PRINTF__((__FILE__ ":%d:error:%d:%s\n", __LINE__, (int)(ee),#func));\
goto ee##bail;\
} \
} while (0)
#define _CATCH(exception) exception##bail: if (exception != AEE_SUCCESS)
#define _ASSERT(nErr, ff) _TRY(nErr, 0 == (ff) ? AEE_EBADPARM : AEE_SUCCESS)
#ifdef __QAIC_DEBUG__
#define _ALLOCATE(nErr, pal, size, alignment, pv) _TRY(nErr, _allocator_alloc(pal, __FILE_LINE__, size, alignment, (void**)&pv))
#else
#define _ALLOCATE(nErr, pal, size, alignment, pv) _TRY(nErr, _allocator_alloc(pal, 0, size, alignment, (void**)&pv))
#endif
#endif // _QAIC_ENV_H
#ifndef _ALLOCATOR_H
#define _ALLOCATOR_H
#include <stdlib.h>
#include <stdint.h>
typedef struct _heap _heap;
struct _heap {
_heap* pPrev;
const char* loc;
uint64_t buf;
};
typedef struct _allocator {
_heap* pheap;
uint8_t* stack;
uint8_t* stackEnd;
int nSize;
} _allocator;
_ATTRIBUTE_UNUSED
static __inline int _heap_alloc(_heap** ppa, const char* loc, int size, void** ppbuf) {
_heap* pn = 0;
pn = malloc(size + sizeof(_heap) - sizeof(uint64_t));
if(pn != 0) {
pn->pPrev = *ppa;
pn->loc = loc;
*ppa = pn;
*ppbuf = (void*)&(pn->buf);
return 0;
} else {
return -1;
}
}
#define _ALIGN_SIZE(x, y) (((x) + (y-1)) & ~(y-1))
_ATTRIBUTE_UNUSED
static __inline int _allocator_alloc(_allocator* me,
const char* loc,
int size,
unsigned int al,
void** ppbuf) {
if(size < 0) {
return -1;
} else if (size == 0) {
*ppbuf = 0;
return 0;
}
if((_ALIGN_SIZE((uintptr_t)me->stackEnd, al) + size) < (uintptr_t)me->stack + me->nSize) {
*ppbuf = (uint8_t*)_ALIGN_SIZE((uintptr_t)me->stackEnd, al);
me->stackEnd = (uint8_t*)_ALIGN_SIZE((uintptr_t)me->stackEnd, al) + size;
return 0;
} else {
return _heap_alloc(&me->pheap, loc, size, ppbuf);
}
}
_ATTRIBUTE_UNUSED
static __inline void _allocator_deinit(_allocator* me) {
_heap* pa = me->pheap;
while(pa != 0) {
_heap* pn = pa;
const char* loc = pn->loc;
(void)loc;
pa = pn->pPrev;
free(pn);
}
}
_ATTRIBUTE_UNUSED
static __inline void _allocator_init(_allocator* me, uint8_t* stack, int stackSize) {
me->stack = stack;
me->stackEnd = stack + stackSize;
me->nSize = stackSize;
me->pheap = 0;
}
#endif // _ALLOCATOR_H
#ifndef SLIM_H
#define SLIM_H
#include <stdint.h>
//a C data structure for the idl types that can be used to implement
//static and dynamic language bindings fairly efficiently.
//
//the goal is to have a minimal ROM and RAM footprint and without
//doing too many allocations. A good way to package these things seemed
//like the module boundary, so all the idls within one module can share
//all the type references.
#define PARAMETER_IN 0x0
#define PARAMETER_OUT 0x1
#define PARAMETER_INOUT 0x2
#define PARAMETER_ROUT 0x3
#define PARAMETER_INROUT 0x4
//the types that we get from idl
#define TYPE_OBJECT 0x0
#define TYPE_INTERFACE 0x1
#define TYPE_PRIMITIVE 0x2
#define TYPE_ENUM 0x3
#define TYPE_STRING 0x4
#define TYPE_WSTRING 0x5
#define TYPE_STRUCTURE 0x6
#define TYPE_UNION 0x7
#define TYPE_ARRAY 0x8
#define TYPE_SEQUENCE 0x9
//these require the pack/unpack to recurse
//so it's a hint to those languages that can optimize in cases where
//recursion isn't necessary.
#define TYPE_COMPLEX_STRUCTURE (0x10 | TYPE_STRUCTURE)
#define TYPE_COMPLEX_UNION (0x10 | TYPE_UNION)
#define TYPE_COMPLEX_ARRAY (0x10 | TYPE_ARRAY)
#define TYPE_COMPLEX_SEQUENCE (0x10 | TYPE_SEQUENCE)
typedef struct Type Type;
#define INHERIT_TYPE\
int32_t nativeSize; /*in the simple case its the same as wire size and alignment*/\
union {\
struct {\
const uintptr_t p1;\
const uintptr_t p2;\
} _cast;\
struct {\
uint32_t iid;\
uint32_t bNotNil;\
} object;\
struct {\
const Type *arrayType;\
int32_t nItems;\
} array;\
struct {\
const Type *seqType;\
int32_t nMaxLen;\
} seqSimple; \
struct {\
uint32_t bFloating;\
uint32_t bSigned;\
} prim; \
const SequenceType* seqComplex;\
const UnionType *unionType;\
const StructType *structType;\
int32_t stringMaxLen;\
uint8_t bInterfaceNotNil;\
} param;\
uint8_t type;\
uint8_t nativeAlignment\
typedef struct UnionType UnionType;
typedef struct StructType StructType;
typedef struct SequenceType SequenceType;
struct Type {
INHERIT_TYPE;
};
struct SequenceType {
const Type * seqType;
uint32_t nMaxLen;
uint32_t inSize;
uint32_t routSizePrimIn;
uint32_t routSizePrimROut;
};
//byte offset from the start of the case values for
//this unions case value array. it MUST be aligned
//at the alignment requrements for the descriptor
//
//if negative it means that the unions cases are
//simple enumerators, so the value read from the descriptor
//can be used directly to find the correct case
typedef union CaseValuePtr CaseValuePtr;
union CaseValuePtr {
const uint8_t* value8s;
const uint16_t* value16s;
const uint32_t* value32s;
const uint64_t* value64s;
};
//these are only used in complex cases
//so I pulled them out of the type definition as references to make
//the type smaller
struct UnionType {
const Type *descriptor;
uint32_t nCases;
const CaseValuePtr caseValues;
const Type * const *cases;
int32_t inSize;
int32_t routSizePrimIn;
int32_t routSizePrimROut;
uint8_t inAlignment;
uint8_t routAlignmentPrimIn;
uint8_t routAlignmentPrimROut;
uint8_t inCaseAlignment;
uint8_t routCaseAlignmentPrimIn;
uint8_t routCaseAlignmentPrimROut;
uint8_t nativeCaseAlignment;
uint8_t bDefaultCase;
};
struct StructType {
uint32_t nMembers;
const Type * const *members;
int32_t inSize;
int32_t routSizePrimIn;
int32_t routSizePrimROut;
uint8_t inAlignment;
uint8_t routAlignmentPrimIn;
uint8_t routAlignmentPrimROut;
};
typedef struct Parameter Parameter;
struct Parameter {
INHERIT_TYPE;
uint8_t mode;
uint8_t bNotNil;
};
#define SLIM_IFPTR32(is32,is64) (sizeof(uintptr_t) == 4 ? (is32) : (is64))
#define SLIM_SCALARS_IS_DYNAMIC(u) (((u) & 0x00ffffff) == 0x00ffffff)
typedef struct Method Method;
struct Method {
uint32_t uScalars; //no method index
int32_t primInSize;
int32_t primROutSize;
int maxArgs;
int numParams;
const Parameter * const *params;
uint8_t primInAlignment;
uint8_t primROutAlignment;
};
typedef struct Interface Interface;
struct Interface {
int nMethods;
const Method * const *methodArray;
int nIIds;
const uint32_t *iids;
const uint16_t* methodStringArray;
const uint16_t* methodStrings;
const char* strings;
};
#endif //SLIM_H
#ifndef _CALCULATOR_SLIM_H
#define _CALCULATOR_SLIM_H
// remote.h
#include <stdint.h>
#ifndef __QAIC_SLIM
#define __QAIC_SLIM(ff) ff
#endif
#ifndef __QAIC_SLIM_EXPORT
#define __QAIC_SLIM_EXPORT
#endif
static const Type types[1];
static const Type types[1] = {{0x1,{{(const uintptr_t)0,(const uintptr_t)0}}, 2,0x1}};
static const Parameter parameters[3] = {{0x4,{{(const uintptr_t)0,(const uintptr_t)0}}, 2,0x4,3,0},{SLIM_IFPTR32(0x8,0x10),{{(const uintptr_t)&(types[0]),(const uintptr_t)0x0}}, 9,SLIM_IFPTR32(0x4,0x8),0,0},{SLIM_IFPTR32(0x8,0x10),{{(const uintptr_t)&(types[0]),(const uintptr_t)0x0}}, 9,SLIM_IFPTR32(0x4,0x8),3,0}};
static const Parameter* const parameterArrays[3] = {(&(parameters[1])),(&(parameters[2])),(&(parameters[0]))};
static const Method methods[2] = {{REMOTE_SCALARS_MAKEX(0,0,0x0,0x1,0x0,0x0),0x0,0x4,1,1,(&(parameterArrays[2])),0x1,0x4},{REMOTE_SCALARS_MAKEX(0,0,0x2,0x1,0x0,0x0),0x8,0x0,5,2,(&(parameterArrays[0])),0x4,0x1}};
static const Method* const methodArrays[2] = {&(methods[0]),&(methods[1])};
static const char strings[41] = "extract_and_match\0features\0leet\0init\0img\0";
static const uint16_t methodStrings[5] = {0,37,18,32,27};
static const uint16_t methodStringsArrays[2] = {3,0};
__QAIC_SLIM_EXPORT const Interface __QAIC_SLIM(calculator_slim) = {2,&(methodArrays[0]),0,0,&(methodStringsArrays [0]),methodStrings,strings};
#endif //_CALCULATOR_SLIM_H
#ifdef __cplusplus
extern "C" {
#endif
#ifndef _const_calculator_handle
#define _const_calculator_handle ((remote_handle)-1)
#endif //_const_calculator_handle
static void _calculator_pls_dtor(void* data) {
remote_handle* ph = (remote_handle*)data;
if(_const_calculator_handle != *ph) {
(void)__QAIC_REMOTE(remote_handle_close)(*ph);
*ph = _const_calculator_handle;
}
}
static int _calculator_pls_ctor(void* ctx, void* data) {
remote_handle* ph = (remote_handle*)data;
*ph = _const_calculator_handle;
if(*ph == (remote_handle)-1) {
return __QAIC_REMOTE(remote_handle_open)((const char*)ctx, ph);
}
return 0;
}
#if (defined __qdsp6__) || (defined __hexagon__)
#pragma weak adsp_pls_add_lookup
extern int adsp_pls_add_lookup(uint32_t type, uint32_t key, int size, int (*ctor)(void* ctx, void* data), void* ctx, void (*dtor)(void* ctx), void** ppo);
#pragma weak HAP_pls_add_lookup
extern int HAP_pls_add_lookup(uint32_t type, uint32_t key, int size, int (*ctor)(void* ctx, void* data), void* ctx, void (*dtor)(void* ctx), void** ppo);
__QAIC_STUB_EXPORT remote_handle _calculator_handle(void) {
remote_handle* ph;
if(adsp_pls_add_lookup) {
if(0 == adsp_pls_add_lookup((uint32_t)_calculator_handle, 0, sizeof(*ph), _calculator_pls_ctor, "calculator", _calculator_pls_dtor, (void**)&ph)) {
return *ph;
}
return (remote_handle)-1;
} else if(HAP_pls_add_lookup) {
if(0 == HAP_pls_add_lookup((uint32_t)_calculator_handle, 0, sizeof(*ph), _calculator_pls_ctor, "calculator", _calculator_pls_dtor, (void**)&ph)) {
return *ph;
}
return (remote_handle)-1;
}
return(remote_handle)-1;
}
#else //__qdsp6__ || __hexagon__
uint32_t _calculator_atomic_CompareAndExchange(uint32_t * volatile puDest, uint32_t uExchange, uint32_t uCompare);
#ifdef _WIN32
#include "Windows.h"
uint32_t _calculator_atomic_CompareAndExchange(uint32_t * volatile puDest, uint32_t uExchange, uint32_t uCompare) {
return (uint32_t)InterlockedCompareExchange((volatile LONG*)puDest, (LONG)uExchange, (LONG)uCompare);
}
#elif __GNUC__
uint32_t _calculator_atomic_CompareAndExchange(uint32_t * volatile puDest, uint32_t uExchange, uint32_t uCompare) {
return __sync_val_compare_and_swap(puDest, uCompare, uExchange);
}
#endif //_WIN32
__QAIC_STUB_EXPORT remote_handle _calculator_handle(void) {
static remote_handle handle = _const_calculator_handle;
if((remote_handle)-1 != handle) {
return handle;
} else {
remote_handle tmp;
int nErr = _calculator_pls_ctor("calculator", (void*)&tmp);
if(nErr) {
return (remote_handle)-1;
}
if(((remote_handle)-1 != handle) || ((remote_handle)-1 != (remote_handle)_calculator_atomic_CompareAndExchange((uint32_t*)&handle, (uint32_t)tmp, (uint32_t)-1))) {
_calculator_pls_dtor(&tmp);
}
return handle;
}
}
#endif //__qdsp6__
__QAIC_STUB_EXPORT int __QAIC_STUB(calculator_skel_invoke)(uint32_t _sc, remote_arg* _pra) __QAIC_STUB_ATTRIBUTE {
return __QAIC_REMOTE(remote_handle_invoke)(_calculator_handle(), _sc, _pra);
}
#ifdef __cplusplus
}
#endif
#ifdef __cplusplus
extern "C" {
#endif
extern int remote_register_dma_handle(int, uint32_t);
static __inline int _stub_method(remote_handle _handle, uint32_t _mid, uint32_t _rout0[1]) {
int _numIn[1];
remote_arg _pra[1];
uint32_t _primROut[1];
int _nErr = 0;
_numIn[0] = 0;
_pra[(_numIn[0] + 0)].buf.pv = (void*)_primROut;
_pra[(_numIn[0] + 0)].buf.nLen = sizeof(_primROut);
_TRY(_nErr, __QAIC_REMOTE(remote_handle_invoke)(_handle, REMOTE_SCALARS_MAKEX(0, _mid, 0, 1, 0, 0), _pra));
_COPY(_rout0, 0, _primROut, 0, 4);
_CATCH(_nErr) {}
return _nErr;
}
__QAIC_STUB_EXPORT int __QAIC_STUB(calculator_init)(uint32* leet) __QAIC_STUB_ATTRIBUTE {
uint32_t _mid = 0;
return _stub_method(_calculator_handle(), _mid, (uint32_t*)leet);
}
static __inline int _stub_method_1(remote_handle _handle, uint32_t _mid, char* _in0[1], uint32_t _in0Len[1], char* _rout1[1], uint32_t _rout1Len[1]) {
int _numIn[1];
remote_arg _pra[3];
uint32_t _primIn[2];
remote_arg* _praIn;
remote_arg* _praROut;
int _nErr = 0;
_numIn[0] = 1;
_pra[0].buf.pv = (void*)_primIn;
_pra[0].buf.nLen = sizeof(_primIn);
_COPY(_primIn, 0, _in0Len, 0, 4);
_praIn = (_pra + 1);
_praIn[0].buf.pv = _in0[0];
_praIn[0].buf.nLen = (1 * _in0Len[0]);
_COPY(_primIn, 4, _rout1Len, 0, 4);
_praROut = (_praIn + _numIn[0] + 0);
_praROut[0].buf.pv = _rout1[0];
_praROut[0].buf.nLen = (1 * _rout1Len[0]);
_TRY(_nErr, __QAIC_REMOTE(remote_handle_invoke)(_handle, REMOTE_SCALARS_MAKEX(0, _mid, 2, 1, 0, 0), _pra));
_CATCH(_nErr) {}
return _nErr;
}
__QAIC_STUB_EXPORT int __QAIC_STUB(calculator_extract_and_match)(const uint8* img, int imgLen, uint8* features, int featuresLen) __QAIC_STUB_ATTRIBUTE {
uint32_t _mid = 1;
return _stub_method_1(_calculator_handle(), _mid, (char**)&img, (uint32_t*)&imgLen, (char**)&features, (uint32_t*)&featuresLen);
}
#ifdef __cplusplus
}
#endif
#endif //_CALCULATOR_STUB_H

BIN
selfdrive/orbd/dsp/gen/libcalculator_skel.so
View File

Binary file not shown.

38
selfdrive/orbd/extractor.h
View File

@ -1,38 +0,0 @@
#ifndef EXTRACTOR_H
#define EXTRACTOR_H
#ifdef __cplusplus
extern "C" {
#endif
#include <stdint.h>
#define ORBD_KEYPOINTS 3000
#define ORBD_DESCRIPTOR_LENGTH 32
#define ORBD_HEIGHT 874
#define ORBD_WIDTH 1164
#define ORBD_FOCAL 910
// matches OrbFeatures from log.capnp
struct orb_features {
// align this
uint16_t n_corners;
uint16_t xy[ORBD_KEYPOINTS][2];
uint8_t octave[ORBD_KEYPOINTS];
uint8_t des[ORBD_KEYPOINTS][ORBD_DESCRIPTOR_LENGTH];
int16_t matches[ORBD_KEYPOINTS];
};
// forward declare this
struct pyramid;
// manage the pyramids in extractor.c
void init_gpyrs();
int extract_and_match_gpyrs(const uint8_t *img, struct orb_features *);
int extract_and_match(const uint8_t *img, struct pyramid *pyrs, struct pyramid *prev_pyrs, struct orb_features *);
#ifdef __cplusplus
}
#endif
#endif // EXTRACTOR_H

191
selfdrive/orbd/orbd.cc
View File

@ -1,191 +0,0 @@
#include <stdio.h>
#include <stdlib.h>
#include <signal.h>
#include <unistd.h>
#include <stdint.h>
#include <assert.h>
#include <sys/resource.h>
#include "common/visionipc.h"
#include "common/swaglog.h"
#include "extractor.h"
#ifdef DSP
#include "dsp/gen/calculator.h"
#else
#include "turbocv.h"
#endif
#include <zmq.h>
#include <capnp/serialize.h>
#include "cereal/gen/cpp/log.capnp.h"
#ifndef PATH_MAX
#include <linux/limits.h>
#endif
volatile int do_exit = 0;
static void set_do_exit(int sig) {
do_exit = 1;
}
int main(int argc, char *argv[]) {
int err;
setpriority(PRIO_PROCESS, 0, -13);
printf("starting orbd\n");
#ifdef DSP
uint32_t test_leet = 0;
char my_path[PATH_MAX+1];
memset(my_path, 0, sizeof(my_path));
ssize_t len = readlink("/proc/self/exe", my_path, sizeof(my_path));
assert(len > 5);
my_path[len-5] = '\0';
LOGW("running from %s with PATH_MAX %d", my_path, PATH_MAX);
char adsp_path[PATH_MAX+1];
snprintf(adsp_path, PATH_MAX, "ADSP_LIBRARY_PATH=%s/dsp/gen", my_path);
assert(putenv(adsp_path) == 0);
assert(calculator_init(&test_leet) == 0);
assert(test_leet == 0x1337);
LOGW("orbd init complete");
#else
init_gpyrs();
#endif
signal(SIGINT, (sighandler_t) set_do_exit);
signal(SIGTERM, (sighandler_t) set_do_exit);
void *ctx = zmq_ctx_new();
void *orb_features_sock = zmq_socket(ctx, ZMQ_PUB);
assert(orb_features_sock);
zmq_bind(orb_features_sock, "tcp://*:8058");
void *orb_features_summary_sock = zmq_socket(ctx, ZMQ_PUB);
assert(orb_features_summary_sock);
zmq_bind(orb_features_summary_sock, "tcp://*:8062");
struct orb_features *features = (struct orb_features *)malloc(sizeof(struct orb_features));
int last_frame_id = 0;
uint64_t frame_count = 0;
// every other frame
const int RATE = 2;
VisionStream stream;
while (!do_exit) {
VisionStreamBufs buf_info;
err = visionstream_init(&stream, VISION_STREAM_YUV, true, &buf_info);
if (err) {
printf("visionstream connect fail\n");
usleep(100000);
continue;
}
uint64_t timestamp_last_eof = 0;
while (!do_exit) {
VIPCBuf *buf;
VIPCBufExtra extra;
buf = visionstream_get(&stream, &extra);
if (buf == NULL) {
printf("visionstream get failed\n");
break;
}
// every other frame
frame_count++;
if ((frame_count%RATE) != 0) {
continue;
}
uint64_t start = nanos_since_boot();
#ifdef DSP
int ret = calculator_extract_and_match((uint8_t *)buf->addr, ORBD_HEIGHT*ORBD_WIDTH, (uint8_t *)features, sizeof(struct orb_features));
#else
int ret = extract_and_match_gpyrs((uint8_t *) buf->addr, features);
#endif
uint64_t end = nanos_since_boot();
LOGD("total(%d): %6.2f ms to get %4d features on %d", ret, (end-start)/1000000.0, features->n_corners, extra.frame_id);
assert(ret == 0);
if (last_frame_id+RATE != extra.frame_id) {
LOGW("dropped frame!");
}
last_frame_id = extra.frame_id;
if (timestamp_last_eof == 0) {
timestamp_last_eof = extra.timestamp_eof;
continue;
}
int match_count = 0;
// *** send OrbFeatures ***
{
// create capnp message
capnp::MallocMessageBuilder msg;
cereal::Event::Builder event = msg.initRoot<cereal::Event>();
event.setLogMonoTime(nanos_since_boot());
auto orb_features = event.initOrbFeatures();
// set timestamps
orb_features.setTimestampEof(extra.timestamp_eof);
orb_features.setTimestampLastEof(timestamp_last_eof);
// init descriptors for send
kj::ArrayPtr<capnp::byte> descriptorsPtr = kj::arrayPtr((uint8_t *)features->des, ORBD_DESCRIPTOR_LENGTH * features->n_corners);
orb_features.setDescriptors(descriptorsPtr);
auto xs = orb_features.initXs(features->n_corners);
auto ys = orb_features.initYs(features->n_corners);
auto octaves = orb_features.initOctaves(features->n_corners);
auto matches = orb_features.initMatches(features->n_corners);
// copy out normalized keypoints
for (int i = 0; i < features->n_corners; i++) {
xs.set(i, (features->xy[i][0] * 1.0f - ORBD_WIDTH / 2) / ORBD_FOCAL);
ys.set(i, (features->xy[i][1] * 1.0f - ORBD_HEIGHT / 2) / ORBD_FOCAL);
octaves.set(i, features->octave[i]);
matches.set(i, features->matches[i]);
match_count += features->matches[i] != -1;
}
auto words = capnp::messageToFlatArray(msg);
auto bytes = words.asBytes();
zmq_send(orb_features_sock, bytes.begin(), bytes.size(), 0);
}
// *** send OrbFeaturesSummary ***
{
// create capnp message
capnp::MallocMessageBuilder msg;
cereal::Event::Builder event = msg.initRoot<cereal::Event>();
event.setLogMonoTime(nanos_since_boot());
auto orb_features_summary = event.initOrbFeaturesSummary();
orb_features_summary.setTimestampEof(extra.timestamp_eof);
orb_features_summary.setTimestampLastEof(timestamp_last_eof);
orb_features_summary.setFeatureCount(features->n_corners);
orb_features_summary.setMatchCount(match_count);
orb_features_summary.setComputeNs(end-start);
auto words = capnp::messageToFlatArray(msg);
auto bytes = words.asBytes();
zmq_send(orb_features_summary_sock, bytes.begin(), bytes.size(), 0);
}
timestamp_last_eof = extra.timestamp_eof;
}
}
visionstream_destroy(&stream);
return 0;
}

13
selfdrive/orbd/orbd_wrapper.sh
View File

@ -1,13 +0,0 @@
#!/bin/sh
finish() {
echo "exiting orbd"
pkill -SIGINT -P $$
}
trap finish EXIT
while true; do
./orbd &
wait $!
done

BIN
selfdrive/sensord/gpsd
View File

Binary file not shown.

BIN
selfdrive/sensord/sensord
View File

Binary file not shown.

2
selfdrive/swaglog.py
View File

@ -22,7 +22,7 @@ class LogMessageHandler(logging.Handler):
self.connect()
msg = self.format(record).rstrip('\n')
# print "SEND", repr(msg)
# print("SEND".format(repr(msg)))
try:
self.sock.send(chr(record.levelno)+msg, zmq.NOBLOCK)
except zmq.error.Again:

16
selfdrive/test/plant/maneuver.py
View File

@ -8,7 +8,7 @@ class Maneuver(object):
# Was tempted to make a builder class
self.distance_lead = kwargs.get("initial_distance_lead", 200.0)
self.speed = kwargs.get("initial_speed", 0.0)
self.lead_relevancy = kwargs.get("lead_relevancy", 0)
self.lead_relevancy = kwargs.get("lead_relevancy", 0)
self.grade_values = kwargs.get("grade_values", [0.0, 0.0])
self.grade_breakpoints = kwargs.get("grade_breakpoints", [0.0, duration])
@ -37,8 +37,8 @@ class Maneuver(object):
while buttons_sorted and plant.current_time() >= buttons_sorted[0][1]:
current_button = buttons_sorted[0][0]
buttons_sorted = buttons_sorted[1:]
print "current button changed to", current_button
print("current button changed to {0}".format(current_button))
grade = np.interp(plant.current_time(), self.grade_breakpoints, self.grade_values)
speed_lead = np.interp(plant.current_time(), self.speed_lead_breakpoints, self.speed_lead_values)
@ -46,11 +46,11 @@ class Maneuver(object):
if live100:
last_live100 = live100[-1]
d_rel = distance_lead - distance if self.lead_relevancy else 200.
v_rel = speed_lead - speed if self.lead_relevancy else 0.
d_rel = distance_lead - distance if self.lead_relevancy else 200.
v_rel = speed_lead - speed if self.lead_relevancy else 0.
if last_live100:
# print last_live100
# print(last_live100)
#develop plots
plot.add_data(
time=plant.current_time(),
@ -64,8 +64,8 @@ class Maneuver(object):
jerk_factor=last_live100.jerkFactor,
a_target=last_live100.aTarget,
fcw=fcw)
print "maneuver end"
print("maneuver end")
return (None, plot)

2
selfdrive/test/plant/plant.py
View File

@ -208,7 +208,7 @@ class Plant(object):
# print at 5hz
if (self.rk.frame%(self.rate/5)) == 0:
print "%6.2f m %6.2f m/s %6.2f m/s2 %.2f ang gas: %.2f brake: %.2f steer: %5.2f lead_rel: %6.2f m %6.2f m/s" % (distance, speed, acceleration, self.angle_steer, gas, brake, steer_torque, d_rel, v_rel)
print("%6.2f m %6.2f m/s %6.2f m/s2 %.2f ang gas: %.2f brake: %.2f steer: %5.2f lead_rel: %6.2f m %6.2f m/s" % (distance, speed, acceleration, self.angle_steer, gas, brake, steer_torque, d_rel, v_rel))
# ******** publish the car ********
vls_tuple = namedtuple('vls', [

6
selfdrive/test/plant/plant_ui.py
View File

@ -75,7 +75,7 @@ if __name__ == "__main__":
car_pts = map(pt_to_car, control_pts)
print car_pts
print(car_pts)
car_poly = np.polyfit([x[0] for x in car_pts], [x[1] for x in car_pts], 3)
md.model.path.points = np.polyval(car_poly, np.arange(0, 50)).tolist()
@ -90,9 +90,9 @@ if __name__ == "__main__":
cary += plant.speed * plant.ts * math.sin(heading)
# positive steering angle = steering right
print plant.angle_steer
print(plant.angle_steer)
heading += plant.angle_steer * plant.ts
print heading
print(heading)
# draw my car
display.blit(pygame.transform.rotate(car, 90-math.degrees(heading)), (carx*METER, cary*METER))

18
selfdrive/test/test_fingerprints.py
View File

@ -53,16 +53,16 @@ for idx1, f1 in enumerate(fingerprints_flat):
for idx2, f2 in enumerate(fingerprints_flat):
if idx1 < idx2 and not check_fingerprint_consistency(f1, f2):
valid = False
print "Those two fingerprints are inconsistent", car_names[idx1], car_names[idx2]
print ""
print ', '.join("%d: %d" % v for v in sorted(f1.items()))
print ""
print ', '.join("%d: %d" % v for v in sorted(f2.items()))
print ""
print("Those two fingerprints are inconsistent {0} {1}".format(car_names[idx1], car_names[idx2]))
print("")
print(', '.join("%d: %d" % v for v in sorted(f1.items())))
print("")
print(', '.join("%d: %d" % v for v in sorted(f2.items())))
print("")
print "Found ", len(fingerprints_flat), " individual fingerprints"
print("Found {0} individual fingerprints".format(len(fingerprints_flat)))
if not valid or len(fingerprints_flat) == 0:
print "TEST FAILED"
print("TEST FAILED")
sys.exit(1)
else:
print "TEST SUCESSFUL"
print("TEST SUCESSFUL")

10
selfdrive/test/test_openpilot.py
View File

@ -63,25 +63,25 @@ def with_processes(processes):
@phone_only
@with_processes(['loggerd', 'logmessaged', 'tombstoned', 'proclogd', 'logcatd'])
def test_logging():
print "LOGGING IS SET UP"
print("LOGGING IS SET UP")
time.sleep(1.0)
@phone_only
@with_processes(['visiond'])
def test_visiond():
print "VISIOND IS SET UP"
print("VISIOND IS SET UP")
time.sleep(5.0)
@phone_only
@with_processes(['sensord'])
def test_sensord():
print "SENSORS ARE SET UP"
print("SENSORS ARE SET UP")
time.sleep(1.0)
@phone_only
@with_processes(['ui'])
def test_ui():
print "RUNNING UI"
print("RUNNING UI")
time.sleep(1.0)
# will have one thing to upload if loggerd ran
@ -89,5 +89,5 @@ def test_ui():
@phone_only
@with_processes(['uploader'])
def test_uploader():
print "UPLOADER"
print("UPLOADER")
time.sleep(10.0)

10
selfdrive/thermald.py
View File

@ -14,7 +14,7 @@ from common.numpy_fast import clip
from common.filter_simple import FirstOrderFilter
ThermalStatus = log.ThermalData.ThermalStatus
CURRENT_TAU = 2. # 2s time constant
CURRENT_TAU = 15. # 15s time constant
def read_tz(x):
with open("/sys/devices/virtual/thermal/thermal_zone%d/temp" % x) as f:
@ -46,7 +46,7 @@ def setup_eon_fan():
bus.write_byte_data(0x21, 0x02, 0x2) # needed?
bus.write_byte_data(0x21, 0x04, 0x4) # manual override source
except IOError:
print "LEON detected"
print("LEON detected")
#os.system("echo 1 > /sys/devices/soc/6a00000.ssusb/power_supply/usb/usb_otg")
LEON = True
bus.close()
@ -290,16 +290,16 @@ def thermald_thread():
started_seen and (sec_since_boot() - off_ts) > 60:
os.system('LD_LIBRARY_PATH="" svc power shutdown')
charging_disabled = check_car_battery_voltage(should_start, health, charging_disabled)
#charging_disabled = check_car_battery_voltage(should_start, health, charging_disabled)
msg.thermal.chargingDisabled = charging_disabled
msg.thermal.chargingError = current_filter.x > 1.0 # if current is > 1A out, then charger might be off
msg.thermal.chargingError = current_filter.x > 0. # if current is positive, then battery is being discharged
msg.thermal.started = started_ts is not None
msg.thermal.startedTs = int(1e9*(started_ts or 0))
msg.thermal.thermalStatus = thermal_status
thermal_sock.send(msg.to_bytes())
print msg
print(msg)
# report to server once per minute
if (count%60) == 0:

66
selfdrive/tombstoned.py
View File

@ -1,7 +1,6 @@
import os
import re
import time
import uuid
import datetime
from raven import Client
@ -38,32 +37,68 @@ def report_tombstone(fn, client):
if parsed:
parsedict = parsed.groupdict()
message = parsedict.get('thread') or ''
message += parsedict.get('signal') or ''
message += parsedict.get('abort') or ''
else:
parsedict = {}
message = fn+"\n"+dat[:1024]
client.send(
event_id=uuid.uuid4().hex,
timestamp=datetime.datetime.utcfromtimestamp(mtime),
logger='tombstoned',
platform='other',
thread_line = parsedict.get('thread', '')
thread_parsed = re.match(r'pid: (?P<pid>\d+), tid: (?P<tid>\d+), name: (?P<name>.*) >>> (?P<cmd>.*) <<<', thread_line)
if thread_parsed:
thread_parseddict = thread_parsed.groupdict()
else:
thread_parseddict = {}
pid = thread_parseddict.get('pid', '')
tid = thread_parseddict.get('tid', '')
name = thread_parseddict.get('name', 'unknown')
cmd = thread_parseddict.get('cmd', 'unknown')
signal_line = parsedict.get('signal', '')
signal_parsed = re.match(r'signal (?P<signal>.*?), code (?P<code>.*?), fault addr (?P<fault_addr>.*)\n', signal_line)
if signal_parsed:
signal_parseddict = signal_parsed.groupdict()
else:
signal_parseddict = {}
signal = signal_parseddict.get('signal', 'unknown')
code = signal_parseddict.get('code', 'unknown')
fault_addr = signal_parseddict.get('fault_addr', '')
abort_line = parsedict.get('abort', '')
if parsed:
message = 'Process {} ({}) got signal {} code {}'.format(name, cmd, signal, code)
if abort_line:
message += '\n'+abort_line
else:
message = fn+'\n'+dat[:1024]
client.captureMessage(
message=message,
date=datetime.datetime.utcfromtimestamp(mtime),
data={
'logger':'tombstoned',
'platform':'other',
},
sdk={'name': 'tombstoned', 'version': '0'},
extra={
'fault_addr': fault_addr,
'abort_msg': abort_line,
'pid': pid,
'tid': tid,
'name':'{} ({})'.format(name, cmd),
'tombstone_fn': fn,
'header': parsedict.get('header'),
'registers': parsedict.get('registers'),
'backtrace': parsedict.get('backtrace'),
'logtail': logtail,
'version': version,
'dirty': not bool(os.environ.get('CLEAN')),
},
user={'id': os.environ.get('DONGLE_ID')},
message=message,
tags={
'name':'{} ({})'.format(name, cmd),
'signal':signal,
'code':code,
'fault_addr':fault_addr,
},
)
cloudlog.error({"tombstone": message})
cloudlog.error({'tombstone': message})
def main(gctx):
@ -72,6 +107,7 @@ def main(gctx):
client = Client('https://d3b175702f62402c91ade04d1c547e68:b20d68c813c74f63a7cdf9c4039d8f56@sentry.io/157615',
install_sys_hook=False, transport=HTTPTransport, release=version, tags={'dirty': dirty})
client.user_context({'id': os.environ.get('DONGLE_ID')})
while True:
now_tombstones = set(get_tombstones())

4
selfdrive/ui/Makefile
View File

@ -10,8 +10,8 @@ WARN_FLAGS = -Werror=implicit-function-declaration \
-Werror=return-type \
-Werror=format-extra-args
CFLAGS = -std=gnu11 -g -fPIC -O2 $(WARN_FLAGS)
CXXFLAGS = -std=c++11 -g -fPIC -O2 $(WARN_FLAGS)
CFLAGS = -std=gnu11 -fPIC -O2 $(WARN_FLAGS)
CXXFLAGS = -std=c++11 -fPIC -O2 $(WARN_FLAGS)
ZMQ_FLAGS = -I$(PHONELIBS)/zmq/aarch64/include
ZMQ_LIBS = -L$(PHONELIBS)/zmq/aarch64/lib \

525
selfdrive/ui/ui.c
View File

@ -50,6 +50,8 @@
#define UI_BUF_COUNT 4
//#define DEBUG_TURN
//#define DEBUG_FPS
const int vwp_w = 1920;
const int vwp_h = 1080;
const int nav_w = 640;
@ -65,7 +67,11 @@ const int header_h = 420;
const int footer_h = 280;
const int footer_y = vwp_h-bdr_s-footer_h;
const int UI_FREQ = 60; // Hz
const int UI_FREQ = 30; // Hz
const int MODEL_PATH_MAX_VERTICES_CNT = 98;
const int MODEL_LANE_PATH_CNT = 3;
const int TRACK_POINTS_MAX_CNT = 50 * 2;
const uint8_t bg_colors[][4] = {
[STATUS_STOPPED] = {0x07, 0x23, 0x39, 0xff},
@ -158,6 +164,21 @@ typedef struct UIScene {
bool is_playing_alert;
} UIScene;
typedef struct {
float x, y;
}vertex_data;
typedef struct {
vertex_data v[MODEL_PATH_MAX_VERTICES_CNT];
int cnt;
} model_path_vertices_data;
typedef struct {
vertex_data v[TRACK_POINTS_MAX_CNT];
int cnt;
} track_vertices_data;
typedef struct UIState {
pthread_mutex_t lock;
pthread_cond_t bg_cond;
@ -212,7 +233,11 @@ typedef struct UIState {
GLuint frame_program;
GLuint frame_texs[UI_BUF_COUNT];
EGLImageKHR khr[UI_BUF_COUNT];
void *priv_hnds[UI_BUF_COUNT];
GLuint frame_front_texs[UI_BUF_COUNT];
EGLImageKHR khr_front[UI_BUF_COUNT];
void *priv_hnds_front[UI_BUF_COUNT];
GLint frame_pos_loc, frame_texcoord_loc;
GLint frame_texture_loc, frame_transform_loc;
@ -251,6 +276,20 @@ typedef struct UIState {
bool alert_blinked;
float light_sensor;
int touch_fd;
// Hints for re-calculations and redrawing
bool model_changed;
bool livempc_or_live20_changed;
GLuint frame_vao[2], frame_vbo[2], frame_ibo[2];
mat4 rear_frame_mat, front_frame_mat;
model_path_vertices_data model_path_vertices[MODEL_LANE_PATH_CNT * 2];
track_vertices_data track_vertices[2];
} UIState;
static int last_brightness = -1;
@ -534,6 +573,58 @@ static void ui_init(UIState *s) {
free(value);
}
}
for(int i = 0; i < 2; i++) {
float x1, x2, y1, y2;
if (i == 1) {
// flip horizontally so it looks like a mirror
x1 = 0.0;
x2 = 1.0;
y1 = 1.0;
y2 = 0.0;
} else {
x1 = 1.0;
x2 = 0.0;
y1 = 1.0;
y2 = 0.0;
}
const uint8_t frame_indicies[] = {0, 1, 2, 0, 2, 3};
const float frame_coords[4][4] = {
{-1.0, -1.0, x2, y1}, //bl
{-1.0, 1.0, x2, y2}, //tl
{ 1.0, 1.0, x1, y2}, //tr
{ 1.0, -1.0, x1, y1}, //br
};
glGenVertexArrays(1,&s->frame_vao[i]);
glBindVertexArray(s->frame_vao[i]);
glGenBuffers(1, &s->frame_vbo[i]);
glBindBuffer(GL_ARRAY_BUFFER, s->frame_vbo[i]);
glBufferData(GL_ARRAY_BUFFER, sizeof(frame_coords), frame_coords, GL_STATIC_DRAW);
glEnableVertexAttribArray(s->frame_pos_loc);
glVertexAttribPointer(s->frame_pos_loc, 2, GL_FLOAT, GL_FALSE,
sizeof(frame_coords[0]), (const void *)0);
glEnableVertexAttribArray(s->frame_texcoord_loc);
glVertexAttribPointer(s->frame_texcoord_loc, 2, GL_FLOAT, GL_FALSE,
sizeof(frame_coords[0]), (const void *)(sizeof(float) * 2));
glGenBuffers(1, &s->frame_ibo[i]);
glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, s->frame_ibo[i]);
glBufferData(GL_ELEMENT_ARRAY_BUFFER, sizeof(frame_indicies), frame_indicies, GL_STATIC_DRAW);
glBindBuffer(GL_ARRAY_BUFFER,0);
glBindVertexArray(0);
}
s->model_changed = false;
s->livempc_or_live20_changed = false;
s->front_frame_mat = matmul(device_transform, full_to_wide_frame_transform);
s->rear_frame_mat = matmul(device_transform, frame_transform);
for(int i = 0;i < UI_BUF_COUNT; i++) {
s->khr[i] = NULL;
s->priv_hnds[i] = NULL;
s->khr_front[i] = NULL;
s->priv_hnds_front[i] = NULL;
}
}
static void ui_init_vision(UIState *s, const VisionStreamBufs back_bufs,
@ -699,8 +790,7 @@ static void draw_chevron(UIState *s, float x_in, float y_in, float sz,
nvgRestore(s->vg);
}
static void ui_draw_lane_line(UIState *s, const float *points, float off,
NVGcolor color, bool is_ghost) {
static void ui_draw_lane_line(UIState *s, const model_path_vertices_data *pvd, NVGcolor color) {
const UIScene *scene = &s->scene;
nvgSave(s->vg);
@ -711,70 +801,36 @@ static void ui_draw_lane_line(UIState *s, const float *points, float off,
nvgBeginPath(s->vg);
bool started = false;
for (int i=0; i<49; i++) {
float px = (float)i;
float py = points[i] - off;
vec4 p_car_space = (vec4){{px, py, 0., 1.}};
vec3 p_full_frame = car_space_to_full_frame(s, p_car_space);
float x = p_full_frame.v[0];
float y = p_full_frame.v[1];
if (x < 0 || y < 0.) {
for (int i=0; i<pvd->cnt; i++) {
if (pvd->v[i].x < 0 || pvd->v[i].y < 0.) {
continue;
}
if (!started) {
nvgMoveTo(s->vg, x, y);
nvgMoveTo(s->vg, pvd->v[i].x, pvd->v[i].y);
started = true;
} else {
nvgLineTo(s->vg, x, y);
nvgLineTo(s->vg, pvd->v[i].x, pvd->v[i].y);
}
}
for (int i=49; i>0; i--) {
float px = (float)i;
float py = is_ghost?(points[i]-off):(points[i]+off);
vec4 p_car_space = (vec4){{px, py, 0., 1.}};
vec3 p_full_frame = car_space_to_full_frame(s, p_car_space);
float x = p_full_frame.v[0];
float y = p_full_frame.v[1];
if (x < 0 || y < 0.) {
continue;
}
nvgLineTo(s->vg, x, y);
}
nvgClosePath(s->vg);
nvgFillColor(s->vg, color);
nvgFill(s->vg);
nvgRestore(s->vg);
}
static void ui_draw_lane(UIState *s, const PathData path, NVGcolor color) {
ui_draw_lane_line(s, path.points, 0.025*path.prob, color, false);
float var = min(path.std, 0.7);
color.a /= 4;
ui_draw_lane_line(s, path.points, -var, color, true);
ui_draw_lane_line(s, path.points, var, color, true);
}
static void ui_draw_track(UIState *s, bool is_mpc) {
static void update_track_data(UIState *s, bool is_mpc, track_vertices_data *pvd) {
const UIScene *scene = &s->scene;
const PathData path = scene->model.path;
const float *mpc_x_coords = &scene->mpc_x[0];
const float *mpc_y_coords = &scene->mpc_y[0];
nvgSave(s->vg);
nvgTranslate(s->vg, 240.0f, 0.0); // rgb-box space
nvgTranslate(s->vg, -1440.0f / 2, -1080.0f / 2); // zoom 2x
nvgScale(s->vg, 2.0, 2.0);
nvgScale(s->vg, 1440.0f / s->rgb_width, 1080.0f / s->rgb_height);
nvgBeginPath(s->vg);
bool started = false;
float off = is_mpc?0.3:0.5;
float lead_d = scene->lead_d_rel*2.;
float path_height = is_mpc?(lead_d>5.)?min(lead_d, 25.)-min(lead_d*0.35, 10.):20.
:(lead_d>0.)?min(lead_d, 50.)-min(lead_d*0.35, 10.):49.;
pvd->cnt = 0;
// left side up
for (int i=0; i<=path_height; i++) {
float px, py, mpx;
@ -789,18 +845,12 @@ static void ui_draw_track(UIState *s, bool is_mpc) {
vec4 p_car_space = (vec4){{px, py, 0., 1.}};
vec3 p_full_frame = car_space_to_full_frame(s, p_car_space);
float x = p_full_frame.v[0];
float y = p_full_frame.v[1];
if (x < 0 || y < 0) {
if (p_full_frame.v[0] < 0. || p_full_frame.v[1] < 0.) {
continue;
}
if (!started) {
nvgMoveTo(s->vg, x, y);
started = true;
} else {
nvgLineTo(s->vg, x, y);
}
pvd->v[pvd->cnt].x = p_full_frame.v[0];
pvd->v[pvd->cnt].y = p_full_frame.v[1];
pvd->cnt += 1;
}
// right side down
@ -817,13 +867,54 @@ static void ui_draw_track(UIState *s, bool is_mpc) {
vec4 p_car_space = (vec4){{px, py, 0., 1.}};
vec3 p_full_frame = car_space_to_full_frame(s, p_car_space);
float x = p_full_frame.v[0];
float y = p_full_frame.v[1];
if (x < 0 || y < 0.) {
pvd->v[pvd->cnt].x = p_full_frame.v[0];
pvd->v[pvd->cnt].y = p_full_frame.v[1];
pvd->cnt += 1;
}
}
static void update_all_track_data(UIState *s) {
const UIScene *scene = &s->scene;
// Draw vision path
update_track_data(s, false, &s->track_vertices[0]);
if (scene->engaged) {
// Draw MPC path when engaged
update_track_data(s, true, &s->track_vertices[1]);
}
}
static void ui_draw_track(UIState *s, bool is_mpc, track_vertices_data *pvd) {
const UIScene *scene = &s->scene;
const PathData path = scene->model.path;
const float *mpc_x_coords = &scene->mpc_x[0];
const float *mpc_y_coords = &scene->mpc_y[0];
nvgSave(s->vg);
nvgTranslate(s->vg, 240.0f, 0.0); // rgb-box space
nvgTranslate(s->vg, -1440.0f / 2, -1080.0f / 2); // zoom 2x
nvgScale(s->vg, 2.0, 2.0);
nvgScale(s->vg, 1440.0f / s->rgb_width, 1080.0f / s->rgb_height);
nvgBeginPath(s->vg);
bool started = false;
float off = is_mpc?0.3:0.5;
float lead_d = scene->lead_d_rel*2.;
float path_height = is_mpc?(lead_d>5.)?min(lead_d, 25.)-min(lead_d*0.35, 10.):20.
:(lead_d>0.)?min(lead_d, 50.)-min(lead_d*0.35, 10.):49.;
int vi = 0;
for(int i = 0;i < pvd->cnt;i++) {
if (pvd->v[i].x < 0 || pvd->v[i].y < 0) {
continue;
}
nvgLineTo(s->vg, x, y);
if (!started) {
nvgMoveTo(s->vg, pvd->v[i].x, pvd->v[i].y);
started = true;
} else {
nvgLineTo(s->vg, pvd->v[i].x, pvd->v[i].y);
}
}
nvgClosePath(s->vg);
@ -860,33 +951,17 @@ static void draw_frame(UIState *s) {
float x1, x2, y1, y2;
if (s->scene.frontview) {
// flip horizontally so it looks like a mirror
x1 = 0.0;
x2 = 1.0;
y1 = 1.0;
y2 = 0.0;
glBindVertexArray(s->frame_vao[1]);
} else {
x1 = 1.0;
x2 = 0.0;
y1 = 1.0;
y2 = 0.0;
glBindVertexArray(s->frame_vao[0]);
}
mat4 out_mat;
mat4 *out_mat;
if (s->scene.frontview || s->scene.fullview) {
out_mat = matmul(device_transform, full_to_wide_frame_transform);
out_mat = &s->front_frame_mat;
} else {
out_mat = matmul(device_transform, frame_transform);
out_mat = &s->rear_frame_mat;
}
const uint8_t frame_indicies[] = {0, 1, 2, 0, 2, 3};
const float frame_coords[4][4] = {
{-1.0, -1.0, x2, y1}, //bl
{-1.0, 1.0, x2, y2}, //tl
{ 1.0, 1.0, x1, y2}, //tr
{ 1.0, -1.0, x1, y1}, //br
};
glActiveTexture(GL_TEXTURE0);
if (s->scene.frontview && s->cur_vision_front_idx >= 0) {
glBindTexture(GL_TEXTURE_2D, s->frame_front_texs[s->cur_vision_front_idx]);
@ -895,40 +970,90 @@ static void draw_frame(UIState *s) {
}
glUseProgram(s->frame_program);
glUniform1i(s->frame_texture_loc, 0);
glUniformMatrix4fv(s->frame_transform_loc, 1, GL_TRUE, out_mat.v);
glEnableVertexAttribArray(s->frame_pos_loc);
glVertexAttribPointer(s->frame_pos_loc, 2, GL_FLOAT, GL_FALSE,
sizeof(frame_coords[0]), frame_coords);
glEnableVertexAttribArray(s->frame_texcoord_loc);
glVertexAttribPointer(s->frame_texcoord_loc, 2, GL_FLOAT, GL_FALSE,
sizeof(frame_coords[0]), &frame_coords[0][2]);
glUniformMatrix4fv(s->frame_transform_loc, 1, GL_TRUE, out_mat->v);
assert(glGetError() == GL_NO_ERROR);
glDrawElements(GL_TRIANGLES, 6, GL_UNSIGNED_BYTE, &frame_indicies[0]);
glEnableVertexAttribArray(0);
glDrawElements(GL_TRIANGLES, 6, GL_UNSIGNED_BYTE, (const void*)0);
glDisableVertexAttribArray(0);
glBindVertexArray(0);
}
static inline bool valid_frame_pt(UIState *s, float x, float y) {
return x >= 0 && x <= s->rgb_width && y >= 0 && y <= s->rgb_height;
}
static void update_lane_line_data(UIState *s, const float *points, float off, bool is_ghost, model_path_vertices_data *pvd) {
pvd->cnt = 0;
for (int i = 0; i < MODEL_PATH_MAX_VERTICES_CNT / 2; i++) {
float px = (float)i;
float py = points[i] - off;
const vec4 p_car_space = (vec4){{px, py, 0., 1.}};
const vec3 p_full_frame = car_space_to_full_frame(s, p_car_space);
if(!valid_frame_pt(s, p_full_frame.v[0], p_full_frame.v[1]))
continue;
pvd->v[pvd->cnt].x = p_full_frame.v[0];
pvd->v[pvd->cnt].y = p_full_frame.v[1];
pvd->cnt += 1;
}
for (int i = MODEL_PATH_MAX_VERTICES_CNT / 2; i > 0; i--) {
float px = (float)i;
float py = is_ghost?(points[i]-off):(points[i]+off);
const vec4 p_car_space = (vec4){{px, py, 0., 1.}};
const vec3 p_full_frame = car_space_to_full_frame(s, p_car_space);
if(!valid_frame_pt(s, p_full_frame.v[0], p_full_frame.v[1]))
continue;
pvd->v[pvd->cnt].x = p_full_frame.v[0];
pvd->v[pvd->cnt].y = p_full_frame.v[1];
pvd->cnt += 1;
}
}
static void update_all_lane_lines_data(UIState *s, const PathData path, model_path_vertices_data *pstart) {
update_lane_line_data(s, path.points, 0.025*path.prob, false, pstart);
float var = min(path.std, 0.7);
update_lane_line_data(s, path.points, -var, true, pstart + 1);
update_lane_line_data(s, path.points, var, true, pstart + 2);
}
static void ui_draw_lane(UIState *s, const PathData *path, model_path_vertices_data *pstart, NVGcolor color) {
ui_draw_lane_line(s, pstart, color);
float var = min(path->std, 0.7);
color.a /= 4;
ui_draw_lane_line(s, pstart + 1, color);
ui_draw_lane_line(s, pstart + 2, color);
}
static void ui_draw_vision_lanes(UIState *s) {
const UIScene *scene = &s->scene;
model_path_vertices_data *pvd = &s->model_path_vertices[0];
if(s->model_changed) {
update_all_lane_lines_data(s, scene->model.left_lane, pvd);
update_all_lane_lines_data(s, scene->model.right_lane, pvd + MODEL_LANE_PATH_CNT);
s->model_changed = false;
}
// Draw left lane edge
ui_draw_lane(
s, scene->model.left_lane,
s, &scene->model.left_lane,
pvd,
nvgRGBAf(1.0, 1.0, 1.0, scene->model.left_lane.prob));
// Draw right lane edge
ui_draw_lane(
s, scene->model.right_lane,
s, &scene->model.right_lane,
pvd + MODEL_LANE_PATH_CNT,
nvgRGBAf(1.0, 1.0, 1.0, scene->model.right_lane.prob));
if(s->livempc_or_live20_changed) {
update_all_track_data(s);
s->livempc_or_live20_changed = false;
}
// Draw vision path
ui_draw_track(s, false);
ui_draw_track(s, false, &s->track_vertices[0]);
if (scene->engaged) {
// Draw MPC path when engaged
ui_draw_track(s, true);
ui_draw_track(s, true, &s->track_vertices[1]);
}
}
@ -1379,14 +1504,14 @@ static void ui_draw_vision(UIState *s) {
glEnable(GL_SCISSOR_TEST);
glViewport(ui_viz_rx+ui_viz_ro, s->fb_h-(box_y+box_h), viz_w, box_h);
glScissor(ui_viz_rx, s->fb_h-(box_y+box_h), ui_viz_rw, box_h);
glEnable(GL_BLEND);
glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
draw_frame(s);
glViewport(0, 0, s->fb_w, s->fb_h);
glDisable(GL_SCISSOR_TEST);
glEnable(GL_BLEND);
glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
glClear(GL_STENCIL_BUFFER_BIT);
nvgBeginFrame(s->vg, s->fb_w, s->fb_h, 1.0f);
nvgSave(s->vg);
@ -1439,8 +1564,6 @@ static void ui_draw(UIState *s) {
nvgEndFrame(s->vg);
glDisable(GL_BLEND);
}
assert(glGetError() == GL_NO_ERROR);
}
static PathData read_path(cereal_ModelData_PathData_ptr pathp) {
@ -1496,7 +1619,10 @@ static void ui_update(UIState *s) {
// do this here for now in lieu of a run_on_main_thread event
for (int i=0; i<UI_BUF_COUNT; i++) {
glDeleteTextures(1, &s->frame_texs[i]);
if(s->khr[i] != NULL) {
visionimg_destroy_gl(s->khr[i], s->priv_hnds[i]);
glDeleteTextures(1, &s->frame_texs[i]);
}
VisionImg img = {
.fd = s->bufs[i].fd,
@ -1507,7 +1633,7 @@ static void ui_update(UIState *s) {
.bpp = 3,
.size = s->rgb_buf_len,
};
s->frame_texs[i] = visionimg_to_gl(&img);
s->frame_texs[i] = visionimg_to_gl(&img, &s->khr[i], &s->priv_hnds[i]);
glBindTexture(GL_TEXTURE_2D, s->frame_texs[i]);
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST);
@ -1520,7 +1646,10 @@ static void ui_update(UIState *s) {
}
for (int i=0; i<UI_BUF_COUNT; i++) {
glDeleteTextures(1, &s->frame_front_texs[i]);
if(s->khr_front[i] != NULL) {
visionimg_destroy_gl(s->khr_front[i], s->priv_hnds_front[i]);
glDeleteTextures(1, &s->frame_front_texs[i]);
}
VisionImg img = {
.fd = s->front_bufs[i].fd,
@ -1531,7 +1660,7 @@ static void ui_update(UIState *s) {
.bpp = 3,
.size = s->rgb_front_buf_len,
};
s->frame_front_texs[i] = visionimg_to_gl(&img);
s->frame_front_texs[i] = visionimg_to_gl(&img, &s->khr_front[i], &s->priv_hnds_front[i]);
glBindTexture(GL_TEXTURE_2D, s->frame_front_texs[i]);
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST);
@ -1556,9 +1685,67 @@ static void ui_update(UIState *s) {
s->alert_blinked = false;
}
// poll for events
while (true) {
zmq_pollitem_t polls[10] = {{0}};
zmq_pollitem_t polls[9] = {{0}};
// Wait for next rgb image from visiond
while(true) {
assert(s->ipc_fd >= 0);
polls[0].fd = s->ipc_fd;
polls[0].events = ZMQ_POLLIN;
int ret = zmq_poll(polls, 1, 1000);
if (ret < 0) {
LOGW("poll failed (%d)", ret);
close(s->ipc_fd);
s->ipc_fd = -1;
s->vision_connected = false;
return;
} else if (ret == 0)
continue;
// vision ipc event
VisionPacket rp;
err = vipc_recv(s->ipc_fd, &rp);
if (err <= 0) {
LOGW("vision disconnected");
close(s->ipc_fd);
s->ipc_fd = -1;
s->vision_connected = false;
return;
}
if (rp.type == VIPC_STREAM_ACQUIRE) {
bool front = rp.d.stream_acq.type == VISION_STREAM_RGB_FRONT;
int idx = rp.d.stream_acq.idx;
int release_idx;
if (front) {
release_idx = s->cur_vision_front_idx;
} else {
release_idx = s->cur_vision_idx;
}
if (release_idx >= 0) {
VisionPacket rep = {
.type = VIPC_STREAM_RELEASE,
.d = { .stream_rel = {
.type = rp.d.stream_acq.type,
.idx = release_idx,
}},
};
vipc_send(s->ipc_fd, &rep);
}
if (front) {
assert(idx < UI_BUF_COUNT);
s->cur_vision_front_idx = idx;
} else {
assert(idx < UI_BUF_COUNT);
s->cur_vision_idx = idx;
// printf("v %d\n", ((uint8_t*)s->bufs[idx].addr)[0]);
}
} else {
assert(false);
}
break;
}
// peek and consume all events in the zmq queue, then return.
while(true) {
polls[0].socket = s->live100_sock_raw;
polls[0].events = ZMQ_POLLIN;
polls[1].socket = s->livecalibration_sock_raw;
@ -1577,22 +1764,14 @@ static void ui_update(UIState *s) {
polls[7].events = ZMQ_POLLIN;
polls[8].socket = s->plus_sock_raw; // plus_sock should be last
polls[8].events = ZMQ_POLLIN;
int num_polls = 9;
if (s->vision_connected) {
assert(s->ipc_fd >= 0);
polls[9].fd = s->ipc_fd;
polls[9].events = ZMQ_POLLIN;
num_polls++;
}
int ret = zmq_poll(polls, num_polls, 0);
if (ret < 0) {
LOGW("poll failed (%d)", ret);
break;
return;
}
if (ret == 0) {
break;
return;
}
if (polls[0].revents || polls[1].revents || polls[2].revents ||
@ -1602,52 +1781,8 @@ static void ui_update(UIState *s) {
set_awake(s, true);
}
if (s->vision_connected && polls[9].revents) {
// vision ipc event
VisionPacket rp;
err = vipc_recv(s->ipc_fd, &rp);
if (err <= 0) {
LOGW("vision disconnected");
close(s->ipc_fd);
s->ipc_fd = -1;
s->vision_connected = false;
continue;
}
if (rp.type == VIPC_STREAM_ACQUIRE) {
bool front = rp.d.stream_acq.type == VISION_STREAM_RGB_FRONT;
int idx = rp.d.stream_acq.idx;
int release_idx;
if (front) {
release_idx = s->cur_vision_front_idx;
} else {
release_idx = s->cur_vision_idx;
}
if (release_idx >= 0) {
VisionPacket rep = {
.type = VIPC_STREAM_RELEASE,
.d = { .stream_rel = {
.type = rp.d.stream_acq.type,
.idx = release_idx,
}},
};
vipc_send(s->ipc_fd, &rep);
}
if (front) {
assert(idx < UI_BUF_COUNT);
s->cur_vision_front_idx = idx;
} else {
assert(idx < UI_BUF_COUNT);
s->cur_vision_idx = idx;
// printf("v %d\n", ((uint8_t*)s->bufs[idx].addr)[0]);
}
} else {
assert(false);
}
} else if (polls[8].revents) {
if (polls[8].revents) {
// plus socket
zmq_msg_t msg;
err = zmq_msg_init(&msg);
assert(err == 0);
@ -1670,7 +1805,7 @@ static void ui_update(UIState *s) {
}
}
if (which == NULL) {
continue;
return;
}
zmq_msg_t msg;
@ -1686,7 +1821,7 @@ static void ui_update(UIState *s) {
eventp.p = capn_getp(capn_root(&ctx), 0, 1);
struct cereal_Event eventd;
cereal_read_Event(&eventd, eventp);
double t = millis_since_boot();
if (eventd.which == cereal_Event_live100) {
struct cereal_Live100Data datad;
cereal_read_Live100Data(&datad, eventd.live100);
@ -1799,6 +1934,7 @@ static void ui_update(UIState *s) {
s->scene.lead_d_rel = leaddatad.dRel;
s->scene.lead_y_rel = leaddatad.yRel;
s->scene.lead_v_rel = leaddatad.vRel;
s->livempc_or_live20_changed = true;
} else if (eventd.which == cereal_Event_liveCalibration) {
s->scene.world_objects_visible = true;
struct cereal_LiveCalibrationData datad;
@ -1820,6 +1956,7 @@ static void ui_update(UIState *s) {
} else if (eventd.which == cereal_Event_model) {
s->scene.model_ts = eventd.logMonoTime;
s->scene.model = read_model(eventd.model);
s->model_changed = true;
} else if (eventd.which == cereal_Event_liveMpc) {
struct cereal_LiveMpcData datad;
cereal_read_LiveMpcData(&datad, eventd.liveMpc);
@ -1837,6 +1974,7 @@ static void ui_update(UIState *s) {
for (int i = 0; i < 50; i++){
s->scene.mpc_y[i] = capn_to_f32(capn_get32(y_list, i));
}
s->livempc_or_live20_changed = true;
} else if (eventd.which == cereal_Event_thermal) {
struct cereal_ThermalData datad;
cereal_read_ThermalData(&datad, eventd.thermal);
@ -1877,7 +2015,6 @@ static void ui_update(UIState *s) {
zmq_msg_close(&msg);
}
}
}
static int vision_subscribe(int fd, VisionPacket *rp, int type) {
@ -2070,6 +2207,7 @@ int main() {
TouchState touch = {0};
touch_init(&touch);
s->touch_fd = touch.fd;
char* error = NULL;
ui_sound_init(&error);
@ -2089,9 +2227,18 @@ int main() {
float smooth_brightness = BRIGHTNESS_B;
set_volume(s, 0);
#ifdef DEBUG_FPS
vipc_t1 = millis_since_boot();
double t1 = millis_since_boot();
int draws = 0, old_draws = 0;
#endif //DEBUG_FPS
while (!do_exit) {
bool should_swap = false;
if (!s->vision_connected) {
// Delay a while to avoid 9% cpu usage while car is not started and user is keeping touching on the screen.
// Don't hold the lock while sleeping, so that vision_connect_thread have chances to get the lock.
usleep(30 * 1000);
}
pthread_mutex_lock(&s->lock);
if (EON) {
@ -2106,27 +2253,53 @@ int main() {
set_brightness(s, NEO_BRIGHTNESS);
}
ui_update(s);
// awake on any touch
int touch_x = -1, touch_y = -1;
int touched = touch_poll(&touch, &touch_x, &touch_y, s->awake ? 0 : 100);
if (touched == 1) {
// touch event will still happen :(
set_awake(s, true);
if (!s->vision_connected) {
// Car is not started, keep in idle state and awake on touch events
zmq_pollitem_t polls[1] = {{0}};
polls[0].fd = s->touch_fd;
polls[0].events = ZMQ_POLLIN;
int ret = zmq_poll(polls, 1, 0);
if (ret < 0)
LOGW("poll failed (%d)", ret);
else if (ret > 0) {
// awake on any touch
int touch_x = -1, touch_y = -1;
int touched = touch_read(&touch, &touch_x, &touch_y);
if (touched == 1) {
set_awake(s, true);
}
}
} else {
// Car started, fetch a new rgb image from ipc and peek for zmq events.
ui_update(s);
if(!s->vision_connected) {
// Visiond process is just stopped, force a redraw to make screen blank again.
ui_draw(s);
glFinish();
should_swap = true;
}
}
// manage wakefulness
if (s->awake_timeout > 0) {
s->awake_timeout--;
} else {
set_awake(s, false);
}
if (s->awake) {
// Don't waste resources on drawing in case screen is off or car is not started.
if (s->awake && s->vision_connected) {
ui_draw(s);
glFinish();
should_swap = true;
#ifdef DEBUG_FPS
draws++;
double t2 = millis_since_boot();
const double interval = 30.;
if(t2 - t1 >= interval * 1000.) {
printf("ui draw fps: %.2f\n",((double)(draws - old_draws)) / interval) ;
t1 = t2;
old_draws = draws;
}
#endif
}
if (s->volume_timeout > 0) {

16
selfdrive/visiond/build_from_src.mk
View File

@ -58,7 +58,8 @@ endif
OPENCV_FLAGS =
OPENCV_LIBS = -lopencv_video \
-lopencv_imgproc \
-lopencv_core
-lopencv_core \
-lopencv_highgui
OTHER_LIBS = -lz -lm -lpthread
PLATFORM_OBJS = camera_fake.o \
@ -127,6 +128,7 @@ OBJS += $(PLATFORM_OBJS) \
../common/buffering.o \
transform.o \
loadyuv.o \
rgb_to_yuv.o \
commonmodel.o \
snpemodel.o \
monitoring.o \
@ -170,6 +172,16 @@ endif
DEPS := $(OBJS:.o=.d)
rgb_to_yuv_test: rgb_to_yuv_test.o clutil.o rgb_to_yuv.o ../common/util.o
@echo "[ LINK ] $@"
$(CXX) -fPIC -o '$@' $^ \
$(LIBYUV_LIBS) \
$(LDFLAGS) \
-L/usr/lib \
-L/system/vendor/lib64 \
$(OPENCL_LIBS) \
$(OUTPUT): $(OBJS)
@echo "[ LINK ] $@"
$(CXX) -fPIC -o '$@' $^ \
@ -222,6 +234,6 @@ $(MODEL_OBJS): %.o: %.dlc
.PHONY: clean
clean:
rm -f visiond $(OBJS) $(DEPS)
rm -f visiond rgb_to_yuv_test rgb_to_yuv_test.o $(OBJS) $(DEPS)
-include $(DEPS)

2
selfdrive/visiond/monitoring.cc
View File

@ -41,7 +41,7 @@ MonitoringResult monitoring_eval_frame(MonitoringState* s, cl_command_queue q,
MonitoringResult ret = {0};
memcpy(ret.vs, s->output, sizeof(ret.vs));
ret.std = sqrtf(2.f) / s->output[6];
ret.std = sqrtf(2.f) / s->output[OUTPUT_SIZE - 1];
return ret;
}

4
selfdrive/visiond/monitoring.h
View File

@ -8,10 +8,10 @@
extern "C" {
#endif
#define OUTPUT_SIZE 7
#define OUTPUT_SIZE 8
typedef struct MonitoringResult {
float vs[6];
float vs[OUTPUT_SIZE - 1];
float std;
} MonitoringResult;

53
selfdrive/visiond/rgb_to_yuv.c 100644
View File

@ -0,0 +1,53 @@
#include <string.h>
#include <assert.h>
#include "clutil.h"
#include "rgb_to_yuv.h"
void rgb_to_yuv_init(RGBToYUVState* s, cl_context ctx, cl_device_id device_id, int width, int height, int rgb_stride) {
int err = 0;
memset(s, 0, sizeof(*s));
assert(width % 2 == 0);
assert(height % 2 == 0);
s->width = width;
s->height = height;
char args[1024];
snprintf(args, sizeof(args),
"-cl-fast-relaxed-math -cl-denorms-are-zero "
#ifdef CL_DEBUG
"-DCL_DEBUG "
#endif
"-DWIDTH=%d -DHEIGHT=%d -DUV_WIDTH=%d -DUV_HEIGHT=%d -DRGB_STRIDE=%d -DRGB_SIZE=%d",
width, height, width/ 2, height / 2, rgb_stride, width * height);
cl_program prg = CLU_LOAD_FROM_FILE(ctx, device_id, "rgb_to_yuv.cl", args);
s->rgb_to_yuv_krnl = clCreateKernel(prg, "rgb_to_yuv", &err);
assert(err == 0);
// done with this
err = clReleaseProgram(prg);
assert(err == 0);
}
void rgb_to_yuv_destroy(RGBToYUVState* s) {
int err = 0;
err = clReleaseKernel(s->rgb_to_yuv_krnl);
assert(err == 0);
}
void rgb_to_yuv_queue(RGBToYUVState* s, cl_command_queue q, cl_mem rgb_cl, cl_mem yuv_cl) {
int err = 0;
err = clSetKernelArg(s->rgb_to_yuv_krnl, 0, sizeof(cl_mem), &rgb_cl);
assert(err == 0);
err = clSetKernelArg(s->rgb_to_yuv_krnl, 1, sizeof(cl_mem), &yuv_cl);
assert(err == 0);
const size_t work_size[2] = {
(size_t)(s->width + (s->width % 4 == 0 ? 0 : (4 - s->width % 4))) / 4,
(size_t)(s->height + (s->height % 4 == 0 ? 0 : (4 - s->height % 4))) / 4
};
cl_event event;
err = clEnqueueNDRangeKernel(q, s->rgb_to_yuv_krnl, 2, NULL, &work_size[0], NULL, 0, 0, &event);
assert(err == 0);
clWaitForEvents(1, &event);
clReleaseEvent(event);
}

127
selfdrive/visiond/rgb_to_yuv.cl 100644
View File

@ -0,0 +1,127 @@
#define RGB_TO_Y(r, g, b) ((((mul24(b, 13) + mul24(g, 65) + mul24(r, 33)) + 64) >> 7) + 16)
#define RGB_TO_U(r, g, b) ((mul24(b, 56) - mul24(g, 37) - mul24(r, 19) + 0x8080) >> 8)
#define RGB_TO_V(r, g, b) ((mul24(r, 56) - mul24(g, 47) - mul24(b, 9) + 0x8080) >> 8)
#define AVERAGE(x, y, z, w) ((convert_ushort(x) + convert_ushort(y) + convert_ushort(z) + convert_ushort(w) + 1) >> 1)
inline void convert_2_ys(__global uchar * out_yuv, int yi, const uchar8 rgbs1) {
uchar2 yy = (uchar2)(
RGB_TO_Y(rgbs1.s2, rgbs1.s1, rgbs1.s0),
RGB_TO_Y(rgbs1.s5, rgbs1.s4, rgbs1.s3)
);
#ifdef CL_DEBUG
if(yi >= RGB_SIZE)
printf("Y vector2 overflow, %d > %d\n", yi, RGB_SIZE);
#endif
vstore2(yy, 0, out_yuv + yi);
}
inline void convert_4_ys(__global uchar * out_yuv, int yi, const uchar8 rgbs1, const uchar8 rgbs3) {
const uchar4 yy = (uchar4)(
RGB_TO_Y(rgbs1.s2, rgbs1.s1, rgbs1.s0),
RGB_TO_Y(rgbs1.s5, rgbs1.s4, rgbs1.s3),
RGB_TO_Y(rgbs3.s0, rgbs1.s7, rgbs1.s6),
RGB_TO_Y(rgbs3.s3, rgbs3.s2, rgbs3.s1)
);
#ifdef CL_DEBUG
if(yi > RGB_SIZE - 4)
printf("Y vector4 overflow, %d > %d\n", yi, RGB_SIZE - 4);
#endif
vstore4(yy, 0, out_yuv + yi);
}
inline void convert_uv(__global uchar * out_yuv, int ui, int vi,
const uchar8 rgbs1, const uchar8 rgbs2) {
// U & V: average of 2x2 pixels square
const short ab = AVERAGE(rgbs1.s0, rgbs1.s3, rgbs2.s0, rgbs2.s3);
const short ag = AVERAGE(rgbs1.s1, rgbs1.s4, rgbs2.s1, rgbs2.s4);
const short ar = AVERAGE(rgbs1.s2, rgbs1.s5, rgbs2.s2, rgbs2.s5);
#ifdef CL_DEBUG
if(ui >= RGB_SIZE + RGB_SIZE / 4)
printf("U overflow, %d >= %d\n", ui, RGB_SIZE + RGB_SIZE / 4);
if(vi >= RGB_SIZE + RGB_SIZE / 2)
printf("V overflow, %d >= %d\n", vi, RGB_SIZE + RGB_SIZE / 2);
#endif
out_yuv[ui] = RGB_TO_U(ar, ag, ab);
out_yuv[vi] = RGB_TO_V(ar, ag, ab);
}
inline void convert_2_uvs(__global uchar * out_yuv, int ui, int vi,
const uchar8 rgbs1, const uchar8 rgbs2, const uchar8 rgbs3, const uchar8 rgbs4) {
// U & V: average of 2x2 pixels square
const short ab1 = AVERAGE(rgbs1.s0, rgbs1.s3, rgbs2.s0, rgbs2.s3);
const short ag1 = AVERAGE(rgbs1.s1, rgbs1.s4, rgbs2.s1, rgbs2.s4);
const short ar1 = AVERAGE(rgbs1.s2, rgbs1.s5, rgbs2.s2, rgbs2.s5);
const short ab2 = AVERAGE(rgbs1.s6, rgbs3.s1, rgbs2.s6, rgbs4.s1);
const short ag2 = AVERAGE(rgbs1.s7, rgbs3.s2, rgbs2.s7, rgbs4.s2);
const short ar2 = AVERAGE(rgbs3.s0, rgbs3.s3, rgbs4.s0, rgbs4.s3);
uchar2 u2 = (uchar2)(
RGB_TO_U(ar1, ag1, ab1),
RGB_TO_U(ar2, ag2, ab2)
);
uchar2 v2 = (uchar2)(
RGB_TO_V(ar1, ag1, ab1),
RGB_TO_V(ar2, ag2, ab2)
);
#ifdef CL_DEBUG1
if(ui > RGB_SIZE + RGB_SIZE / 4 - 2)
printf("U 2 overflow, %d >= %d\n", ui, RGB_SIZE + RGB_SIZE / 4 - 2);
if(vi > RGB_SIZE + RGB_SIZE / 2 - 2)
printf("V 2 overflow, %d >= %d\n", vi, RGB_SIZE + RGB_SIZE / 2 - 2);
#endif
vstore2(u2, 0, out_yuv + ui);
vstore2(v2, 0, out_yuv + vi);
}
__kernel void rgb_to_yuv(__global uchar const * const rgb,
__global uchar * out_yuv)
{
const int dx = get_global_id(0);
const int dy = get_global_id(1);
const int col = mul24(dx, 4); // Current column in rgb image
const int row = mul24(dy, 4); // Current row in rgb image
const int bgri_start = mad24(row, RGB_STRIDE, mul24(col, 3)); // Start offset of rgb data being converted
const int yi_start = mad24(row, WIDTH, col); // Start offset in the target yuv buffer
int ui = mad24(row / 2, UV_WIDTH, RGB_SIZE + col / 2);
int vi = mad24(row / 2 , UV_WIDTH, RGB_SIZE + UV_WIDTH * UV_HEIGHT + col / 2);
int num_col = min(WIDTH - col, 4);
int num_row = min(HEIGHT - row, 4);
if(num_row == 4) {
const uchar8 rgbs0_0 = vload8(0, rgb + bgri_start);
const uchar8 rgbs0_1 = vload8(0, rgb + bgri_start + 8);
const uchar8 rgbs1_0 = vload8(0, rgb + bgri_start + RGB_STRIDE);
const uchar8 rgbs1_1 = vload8(0, rgb + bgri_start + RGB_STRIDE + 8);
const uchar8 rgbs2_0 = vload8(0, rgb + bgri_start + RGB_STRIDE * 2);
const uchar8 rgbs2_1 = vload8(0, rgb + bgri_start + RGB_STRIDE * 2 + 8);
const uchar8 rgbs3_0 = vload8(0, rgb + bgri_start + RGB_STRIDE * 3);
const uchar8 rgbs3_1 = vload8(0, rgb + bgri_start + RGB_STRIDE * 3 + 8);
if(num_col == 4) {
convert_4_ys(out_yuv, yi_start, rgbs0_0, rgbs0_1);
convert_4_ys(out_yuv, yi_start + WIDTH, rgbs1_0, rgbs1_1);
convert_4_ys(out_yuv, yi_start + WIDTH * 2, rgbs2_0, rgbs2_1);
convert_4_ys(out_yuv, yi_start + WIDTH * 3, rgbs3_0, rgbs3_1);
convert_2_uvs(out_yuv, ui, vi, rgbs0_0, rgbs1_0, rgbs0_1, rgbs1_1);
convert_2_uvs(out_yuv, ui + UV_WIDTH, vi + UV_WIDTH, rgbs2_0, rgbs3_0, rgbs2_1, rgbs3_1);
} else if(num_col == 2) {
convert_2_ys(out_yuv, yi_start, rgbs0_0);
convert_2_ys(out_yuv, yi_start + WIDTH, rgbs1_0);
convert_2_ys(out_yuv, yi_start + WIDTH * 2, rgbs2_0);
convert_2_ys(out_yuv, yi_start + WIDTH * 3, rgbs3_0);
convert_uv(out_yuv, ui, vi, rgbs0_0, rgbs1_0);
convert_uv(out_yuv, ui + UV_WIDTH, vi + UV_WIDTH, rgbs2_0, rgbs3_0);
}
} else {
const uchar8 rgbs0_0 = vload8(0, rgb + bgri_start);
const uchar8 rgbs0_1 = vload8(0, rgb + bgri_start + 8);
const uchar8 rgbs1_0 = vload8(0, rgb + bgri_start + RGB_STRIDE);
const uchar8 rgbs1_1 = vload8(0, rgb + bgri_start + RGB_STRIDE + 8);
if(num_col == 4) {
convert_4_ys(out_yuv, yi_start, rgbs0_0, rgbs0_1);
convert_4_ys(out_yuv, yi_start + WIDTH, rgbs1_0, rgbs1_1);
convert_2_uvs(out_yuv, ui, vi, rgbs0_0, rgbs1_0, rgbs0_1, rgbs1_1);
} else if(num_col == 2) {
convert_2_ys(out_yuv, yi_start, rgbs0_0);
convert_2_ys(out_yuv, yi_start + WIDTH, rgbs1_0);
convert_uv(out_yuv, ui, vi, rgbs0_0, rgbs1_0);
}
}
}

32
selfdrive/visiond/rgb_to_yuv.h 100644
View File

@ -0,0 +1,32 @@
#ifndef RGB_TO_YUV_H
#define RGB_TO_YUV_H
#include <inttypes.h>
#include <stdbool.h>
#ifdef __APPLE__
#include <OpenCL/cl.h>
#else
#include <CL/cl.h>
#endif
#ifdef __cplusplus
extern "C" {
#endif
typedef struct {
int width, height;
cl_kernel rgb_to_yuv_krnl;
} RGBToYUVState;
void rgb_to_yuv_init(RGBToYUVState* s, cl_context ctx, cl_device_id device_id, int width, int height, int rgb_stride);
void rgb_to_yuv_destroy(RGBToYUVState* s);
void rgb_to_yuv_queue(RGBToYUVState* s, cl_command_queue q, cl_mem rgb_cl, cl_mem yuv_cl);
#ifdef __cplusplus
}
#endif
#endif // RGB_TO_YUV_H

201
selfdrive/visiond/rgb_to_yuv_test.cc 100644
View File

@ -0,0 +1,201 @@
#include <memory.h>
#include <iostream>
#include <getopt.h>
#include <math.h>
#include <fstream>
#include <cstdlib>
#include <vector>
#include <string>
#include <iomanip>
#include <thread>
#include <fcntl.h>
#include <signal.h>
#include <unistd.h>
#include <cassert>
#include <cstdint>
#ifdef ANDROID
#define MAXE 0
#include <unistd.h>
#else
// The libyuv implementation on ARM is slightly different than on x86
// Our implementation matches the ARM version, so accept errors of 1
#define MAXE 1
#endif
#include <libyuv.h>
#include <CL/cl.h>
#include "clutil.h"
#include "rgb_to_yuv.h"
static inline double millis_since_boot() {
struct timespec t;
clock_gettime(CLOCK_BOOTTIME, &t);
return t.tv_sec * 1000.0 + t.tv_nsec * 1e-6;
}
void cl_init(cl_device_id &device_id, cl_context &context) {
int err;
cl_platform_id platform_id = NULL;
cl_uint num_devices;
cl_uint num_platforms;
err = clGetPlatformIDs(1, &platform_id, &num_platforms);
err = clGetDeviceIDs(platform_id, CL_DEVICE_TYPE_DEFAULT, 1,
&device_id, &num_devices);
cl_print_info(platform_id, device_id);
context = clCreateContext(NULL, 1, &device_id, NULL, NULL, &err);
}
bool compare_results(uint8_t *a, uint8_t *b, int len, int stride, int width, int height, uint8_t *rgb) {
int min_diff = 0., max_diff = 0., max_e = 0.;
int e1 = 0, e0 = 0;
int e0y = 0, e0u = 0, e0v = 0, e1y = 0, e1u = 0, e1v = 0;
int max_e_i = 0;
for (int i = 0;i < len;i++) {
int e = ((int)a[i]) - ((int)b[i]);
if(e < min_diff) {
min_diff = e;
}
if(e > max_diff) {
max_diff = e;
}
int e_abs = std::abs(e);
if(e_abs > max_e) {
max_e = e_abs;
max_e_i = i;
}
if(e_abs < 1) {
e0++;
if(i < stride * height)
e0y++;
else if(i < stride * height + stride * height / 4)
e0u++;
else
e0v++;
} else {
e1++;
if(i < stride * height)
e1y++;
else if(i < stride * height + stride * height / 4)
e1u++;
else
e1v++;
}
}
//printf("max diff : %d, min diff : %d, e < 1: %d, e >= 1: %d\n", max_diff, min_diff, e0, e1);
//printf("Y: e < 1: %d, e >= 1: %d, U: e < 1: %d, e >= 1: %d, V: e < 1: %d, e >= 1: %d\n", e0y, e1y, e0u, e1u, e0v, e1v);
if(max_e <= MAXE) {
return true;
}
int row = max_e_i / stride;
if(row < height) {
printf("max error is Y: %d = (libyuv: %u - cl: %u), row: %d, col: %d\n", max_e, a[max_e_i], b[max_e_i], row, max_e_i % stride);
} else if(row >= height && row < (height + height / 4)) {
printf("max error is U: %d = %u - %u, row: %d, col: %d\n", max_e, a[max_e_i], b[max_e_i], (row - height) / 2, max_e_i % stride / 2);
} else {
printf("max error is V: %d = %u - %u, row: %d, col: %d\n", max_e, a[max_e_i], b[max_e_i], (row - height - height / 4) / 2, max_e_i % stride / 2);
}
return false;
}
int main(int argc, char** argv) {
srand(1337);
clu_init();
cl_device_id device_id;
cl_context context;
cl_init(device_id, context) ;
int err;
const cl_queue_properties props[] = {0}; //CL_QUEUE_PRIORITY_KHR, CL_QUEUE_PRIORITY_HIGH_KHR, 0};
cl_command_queue q = clCreateCommandQueueWithProperties(context, device_id, props, &err);
if(err != 0) {
std::cout << "clCreateCommandQueueWithProperties error: " << err << std::endl;
}
int width = 1164;
int height = 874;
int opt = 0;
while ((opt = getopt(argc, argv, "f")) != -1)
{
switch (opt)
{
case 'f':
std::cout << "Using front camera dimensions" << std::endl;
int width = 1152;
int height = 846;
}
}
std::cout << "Width: " << width << " Height: " << height << std::endl;
uint8_t *rgb_frame = new uint8_t[width * height * 3];
RGBToYUVState rgb_to_yuv_state;
rgb_to_yuv_init(&rgb_to_yuv_state, context, device_id, width, height, width * 3);
int frame_yuv_buf_size = width * height * 3 / 2;
cl_mem yuv_cl = clCreateBuffer(context, CL_MEM_READ_WRITE, frame_yuv_buf_size, (void*)NULL, &err);
uint8_t *frame_yuv_buf = new uint8_t[frame_yuv_buf_size];
uint8_t *frame_yuv_ptr_y = frame_yuv_buf;
uint8_t *frame_yuv_ptr_u = frame_yuv_buf + (width * height);
uint8_t *frame_yuv_ptr_v = frame_yuv_ptr_u + ((width/2) * (height/2));
cl_mem rgb_cl = clCreateBuffer(context, CL_MEM_READ_WRITE, width * height * 3, (void*)NULL, &err);
int mismatched = 0;
int counter = 0;
srand (time(NULL));
for (int i = 0; i < 100; i++){
for (int i = 0; i < width * height * 3; i++){
rgb_frame[i] = (uint8_t)rand();
}
double t1 = millis_since_boot();
libyuv::RGB24ToI420((uint8_t*)rgb_frame, width * 3,
frame_yuv_ptr_y, width,
frame_yuv_ptr_u, width/2,
frame_yuv_ptr_v, width/2,
width, height);
double t2 = millis_since_boot();
//printf("Libyuv: rgb to yuv: %.2fms\n", t2-t1);
clEnqueueWriteBuffer(q, rgb_cl, CL_TRUE, 0, width * height * 3, (void *)rgb_frame, 0, NULL, NULL);
t1 = millis_since_boot();
rgb_to_yuv_queue(&rgb_to_yuv_state, q, rgb_cl, yuv_cl);
t2 = millis_since_boot();
//printf("OpenCL: rgb to yuv: %.2fms\n", t2-t1);
uint8_t *yyy = (uint8_t *)clEnqueueMapBuffer(q, yuv_cl, CL_TRUE,
CL_MAP_READ, 0, frame_yuv_buf_size,
0, NULL, NULL, &err);
if(!compare_results(frame_yuv_ptr_y, yyy, frame_yuv_buf_size, width, width, height, (uint8_t*)rgb_frame))
mismatched++;
clEnqueueUnmapMemObject(q, yuv_cl, yyy, 0, NULL, NULL);
// std::this_thread::sleep_for(std::chrono::milliseconds(20));
if(counter++ % 100 == 0)
printf("Matched: %d, Mismatched: %d\n", counter - mismatched, mismatched);
}
printf("Matched: %d, Mismatched: %d\n", counter - mismatched, mismatched);
delete[] frame_yuv_buf;
rgb_to_yuv_destroy(&rgb_to_yuv_state);
clReleaseContext(context);
delete[] rgb_frame;
if (mismatched == 0)
return 0;
else
return -1;
}

31
selfdrive/visiond/visiond.cc
View File

@ -46,6 +46,7 @@
#include "model.h"
#include "monitoring.h"
#include "rgb_to_yuv.h"
#include "cereal/gen/cpp/log.capnp.h"
@ -122,6 +123,7 @@ struct VisionState {
FrameMetadata yuv_metas[YUV_COUNT];
size_t yuv_buf_size;
int yuv_width, yuv_height;
RGBToYUVState rgb_to_yuv_state;
// for front camera recording
Pool yuv_front_pool;
@ -131,6 +133,7 @@ struct VisionState {
FrameMetadata yuv_front_metas[YUV_COUNT];
size_t yuv_front_buf_size;
int yuv_front_width, yuv_front_height;
RGBToYUVState front_rgb_to_yuv_state;
size_t rgb_buf_size;
int rgb_width, rgb_height, rgb_stride;
@ -398,6 +401,10 @@ void init_buffers(VisionState *s) {
assert(err == 0);
s->krnl_debayer_front = clCreateKernel(s->prg_debayer_front, "debayer10", &err);
assert(err == 0);
rgb_to_yuv_init(&s->rgb_to_yuv_state, s->context, s->device_id, s->yuv_width, s->yuv_height, s->rgb_stride);
rgb_to_yuv_init(&s->front_rgb_to_yuv_state, s->context, s->device_id, s->yuv_front_width, s->yuv_front_height, s->rgb_front_stride);
}
void free_buffers(VisionState *s) {
@ -831,7 +838,6 @@ void* frontview_thread(void *arg) {
if (cnt % 3 == 0)
#endif
{
// for driver autoexposure, use bottom right corner
const int y_start = s->rgb_front_height / 3;
const int y_end = s->rgb_front_height;
@ -869,15 +875,9 @@ void* frontview_thread(void *arg) {
int yuv_idx = pool_select(&s->yuv_front_pool);
s->yuv_front_metas[yuv_idx] = frame_data;
uint8_t *bgr_ptr = (uint8_t*)s->rgb_front_bufs[ui_idx].addr;
libyuv::RGB24ToI420(bgr_ptr, s->rgb_front_stride,
s->yuv_front_bufs[yuv_idx].y, s->yuv_front_width,
s->yuv_front_bufs[yuv_idx].u, s->yuv_front_width/2,
s->yuv_front_bufs[yuv_idx].v, s->yuv_front_width/2,
s->rgb_front_width, s->rgb_front_height);
rgb_to_yuv_queue(&s->front_rgb_to_yuv_state, q, s->rgb_front_bufs_cl[ui_idx], s->yuv_front_cl[yuv_idx]);
visionbuf_sync(&s->yuv_front_ion[yuv_idx], VISIONBUF_SYNC_FROM_DEVICE);
s->yuv_front_metas[yuv_idx] = frame_data;
visionbuf_sync(&s->yuv_front_ion[yuv_idx], VISIONBUF_SYNC_TO_DEVICE);
// no reference required cause we don't use this in visiond
//pool_acquire(&s->yuv_front_pool, yuv_idx);
@ -1020,17 +1020,10 @@ void* processing_thread(void *arg) {
uint8_t* yuv_ptr_u = s->yuv_bufs[yuv_idx].u;
uint8_t* yuv_ptr_v = s->yuv_bufs[yuv_idx].v;
cl_mem yuv_cl = s->yuv_cl[yuv_idx];
libyuv::RGB24ToI420(bgr_ptr, s->rgb_stride,
yuv_ptr_y, s->yuv_width,
yuv_ptr_u, s->yuv_width/2,
yuv_ptr_v, s->yuv_width/2,
s->rgb_width, s->rgb_height);
rgb_to_yuv_queue(&s->rgb_to_yuv_state, q, s->rgb_bufs_cl[rgb_idx], yuv_cl);
visionbuf_sync(&s->yuv_ion[yuv_idx], VISIONBUF_SYNC_FROM_DEVICE);
double yt2 = millis_since_boot();
visionbuf_sync(&s->yuv_ion[yuv_idx], VISIONBUF_SYNC_TO_DEVICE);
// keep another reference around till were done processing
pool_acquire(&s->yuv_pool, yuv_idx);
@ -1129,7 +1122,7 @@ void* processing_thread(void *arg) {
//printf("avg %f\n", pose_output[0]);
posenet->execute(posenet_input);
// fix stddevs
for (int i = 6; i < 12; i++) {
pose_output[i] = log1p(exp(pose_output[i])) + 1e-6;