openpilot v0.7.1 release
parent
25d18afad5
commit
8da8b6135c
|
@ -3,11 +3,14 @@ venv/
|
||||||
.tags
|
.tags
|
||||||
.ipynb_checkpoints
|
.ipynb_checkpoints
|
||||||
.idea
|
.idea
|
||||||
|
.overlay_init
|
||||||
|
.overlay_consistent
|
||||||
.sconsign.dblite
|
.sconsign.dblite
|
||||||
.vscode
|
.vscode
|
||||||
model2.png
|
model2.png
|
||||||
a.out
|
a.out
|
||||||
|
|
||||||
|
*.dylib
|
||||||
*.DSYM
|
*.DSYM
|
||||||
*.d
|
*.d
|
||||||
*.pyc
|
*.pyc
|
||||||
|
@ -27,6 +30,7 @@ a.out
|
||||||
config.json
|
config.json
|
||||||
clcache
|
clcache
|
||||||
|
|
||||||
|
persist
|
||||||
board/obj/
|
board/obj/
|
||||||
selfdrive/boardd/boardd
|
selfdrive/boardd/boardd
|
||||||
selfdrive/logcatd/logcatd
|
selfdrive/logcatd/logcatd
|
||||||
|
@ -51,4 +55,5 @@ panda_jungle
|
||||||
|
|
||||||
.coverage*
|
.coverage*
|
||||||
htmlcov
|
htmlcov
|
||||||
|
pandaextra
|
||||||
|
|
||||||
|
|
|
@ -17,12 +17,14 @@ RUN apt-get update && apt-get install -y \
|
||||||
libffi-dev \
|
libffi-dev \
|
||||||
libglew-dev \
|
libglew-dev \
|
||||||
libgles2-mesa-dev \
|
libgles2-mesa-dev \
|
||||||
|
libglfw3-dev \
|
||||||
libglib2.0-0 \
|
libglib2.0-0 \
|
||||||
liblzma-dev \
|
liblzma-dev \
|
||||||
libmysqlclient-dev \
|
libmysqlclient-dev \
|
||||||
libomp-dev \
|
libomp-dev \
|
||||||
libopencv-dev \
|
libopencv-dev \
|
||||||
libssl-dev \
|
libssl-dev \
|
||||||
|
libsqlite3-dev \
|
||||||
libtool \
|
libtool \
|
||||||
libusb-1.0-0-dev \
|
libusb-1.0-0-dev \
|
||||||
libzmq5-dev \
|
libzmq5-dev \
|
||||||
|
|
27
Pipfile
27
Pipfile
|
@ -8,71 +8,54 @@ opencv-python= "==3.4.2.17"
|
||||||
PyQt5 = "*"
|
PyQt5 = "*"
|
||||||
ipython = "*"
|
ipython = "*"
|
||||||
networkx = "==2.3"
|
networkx = "==2.3"
|
||||||
azure-common = "==1.1.23"
|
azure-core = "==1.1.1"
|
||||||
|
azure-common = "==1.1.24"
|
||||||
azure-nspkg = "==3.0.2"
|
azure-nspkg = "==3.0.2"
|
||||||
azure-storage-blob = "==2.1.0"
|
azure-storage-blob = "==2.1.0"
|
||||||
azure-storage-common = "==2.1.0"
|
azure-storage-common = "==2.1.0"
|
||||||
azure-storage-nspkg = "==3.1.0"
|
azure-storage-nspkg = "==3.1.0"
|
||||||
bincopy = "*"
|
|
||||||
bleach = "*"
|
|
||||||
boto = "*"
|
boto = "*"
|
||||||
"boto3" = "*"
|
"boto3" = "*"
|
||||||
celery = "*"
|
|
||||||
control = "*"
|
control = "*"
|
||||||
datadog = "*"
|
datadog = "*"
|
||||||
decorator = "*"
|
|
||||||
dlib = "*"
|
dlib = "*"
|
||||||
dominate = "*"
|
|
||||||
elasticsearch = "*"
|
elasticsearch = "*"
|
||||||
fasteners = "*"
|
|
||||||
future = "*"
|
future = "*"
|
||||||
futures = "*"
|
futures = "*"
|
||||||
gevent = "*"
|
|
||||||
pycocotools = {git = "https://github.com/cocodataset/cocoapi.git",subdirectory = "PythonAPI"}
|
pycocotools = {git = "https://github.com/cocodataset/cocoapi.git",subdirectory = "PythonAPI"}
|
||||||
gunicorn = "*"
|
gunicorn = "*"
|
||||||
"h5py" = "*"
|
"h5py" = "*"
|
||||||
hexdump = "*"
|
hexdump = "*"
|
||||||
"html5lib" = "*"
|
"html5lib" = "*"
|
||||||
imageio = "*"
|
imageio = "*"
|
||||||
intervaltree = "*"
|
|
||||||
ipykernel = "*"
|
ipykernel = "*"
|
||||||
joblib = "*"
|
joblib = "*"
|
||||||
json-logging-py = "*"
|
json-logging-py = "*"
|
||||||
jupyter = "*"
|
jupyter = "*"
|
||||||
libarchive = "*"
|
libarchive = "*"
|
||||||
lru-dict = "*"
|
lru-dict = "*"
|
||||||
lxml = "*"
|
|
||||||
"mpld3" = "*"
|
"mpld3" = "*"
|
||||||
msgpack-python = "*"
|
msgpack-python = "*"
|
||||||
nbstripout = "*"
|
nbstripout = "*"
|
||||||
nose-parameterized = "*"
|
|
||||||
numpy = "*"
|
numpy = "*"
|
||||||
osmium = "*"
|
osmium = "*"
|
||||||
pbr = "*"
|
pbr = "*"
|
||||||
percache = "*"
|
percache = "*"
|
||||||
pprofile = "*"
|
pprofile = "*"
|
||||||
psutil = "*"
|
|
||||||
pycurl = "*"
|
pycurl = "*"
|
||||||
git-pylint-commit-hook = "*"
|
git-pylint-commit-hook = "*"
|
||||||
pymongo = "*"
|
pymongo = "*"
|
||||||
"pynmea2" = "*"
|
"pynmea2" = "*"
|
||||||
pypolyline = "*"
|
pypolyline = "*"
|
||||||
pysendfile = "*"
|
|
||||||
python-logstash = "*"
|
python-logstash = "*"
|
||||||
pyvcd = "*"
|
|
||||||
redis = "*"
|
redis = "*"
|
||||||
redlock = "*"
|
|
||||||
"s2sphere" = "*"
|
"s2sphere" = "*"
|
||||||
scikit-image = "*"
|
scikit-image = "*"
|
||||||
"subprocess32" = "*"
|
"subprocess32" = "*"
|
||||||
supervisor = "*"
|
|
||||||
tenacity = "*"
|
tenacity = "*"
|
||||||
tensorflow-gpu = ""
|
tensorflow-gpu = ""
|
||||||
utm = "*"
|
|
||||||
"v4l2" = "*"
|
|
||||||
PyJWT = "==1.4.1"
|
PyJWT = "==1.4.1"
|
||||||
PyMySQL = "==0.9.2"
|
PyMySQL = "==0.9.2"
|
||||||
Theano = "*"
|
|
||||||
Werkzeug = "*"
|
Werkzeug = "*"
|
||||||
"backports.lzma" = "*"
|
"backports.lzma" = "*"
|
||||||
Flask-Cors = "*"
|
Flask-Cors = "*"
|
||||||
|
@ -84,7 +67,6 @@ PyNaCl = "*"
|
||||||
reverse_geocoder = "*"
|
reverse_geocoder = "*"
|
||||||
Shapely = "*"
|
Shapely = "*"
|
||||||
SQLAlchemy = "*"
|
SQLAlchemy = "*"
|
||||||
uWSGI = "*"
|
|
||||||
scipy = "*"
|
scipy = "*"
|
||||||
fastcluster = "*"
|
fastcluster = "*"
|
||||||
backports-abc = "*"
|
backports-abc = "*"
|
||||||
|
@ -92,15 +74,13 @@ pygame = "*"
|
||||||
simplejson = "*"
|
simplejson = "*"
|
||||||
python-logstash-async = "*"
|
python-logstash-async = "*"
|
||||||
seaborn = "*"
|
seaborn = "*"
|
||||||
tensorflow-estimator = "*"
|
|
||||||
pyproj = "*"
|
pyproj = "*"
|
||||||
mock = "*"
|
mock = "*"
|
||||||
blinker = "*"
|
|
||||||
gast = "==0.2.2"
|
|
||||||
matplotlib = "*"
|
matplotlib = "*"
|
||||||
dictdiffer = "*"
|
dictdiffer = "*"
|
||||||
aenum = "*"
|
aenum = "*"
|
||||||
coverage = "*"
|
coverage = "*"
|
||||||
|
azure-cli-core = "*"
|
||||||
|
|
||||||
[packages]
|
[packages]
|
||||||
overpy = {git = "https://github.com/commaai/python-overpy.git",ref = "f86529af402d4642e1faeb146671c40284007323"}
|
overpy = {git = "https://github.com/commaai/python-overpy.git",ref = "f86529af402d4642e1faeb146671c40284007323"}
|
||||||
|
@ -144,6 +124,5 @@ pillow = "*"
|
||||||
scons = "*"
|
scons = "*"
|
||||||
cysignals = "*"
|
cysignals = "*"
|
||||||
|
|
||||||
|
|
||||||
[requires]
|
[requires]
|
||||||
python_version = "3.7.3"
|
python_version = "3.7.3"
|
||||||
|
|
File diff suppressed because it is too large
Load Diff
|
@ -57,7 +57,7 @@ openpilot should preserve all other vehicle's stock features, including, but are
|
||||||
Supported Hardware
|
Supported Hardware
|
||||||
------
|
------
|
||||||
|
|
||||||
At the moment, openpilot supports the [EON DevKit](https://comma.ai/shop/products/eon-dashcam-devkit). A [car harness](https://comma.ai/shop/products/car-harness) is recommended to connect the EON to the car. In the future, we'd like to support other platforms as well.
|
At the moment, openpilot supports the [EON DevKit](https://comma.ai/shop/products/eon-dashcam-devkit) and the [comma two](https://comma.ai/shop/products/comma-two-devkit). A [car harness](https://comma.ai/shop/products/car-harness) is recommended to connect the EON or comma two to the car. In the future, we'd like to support other platforms as well, like gaming PCs.
|
||||||
|
|
||||||
Supported Cars
|
Supported Cars
|
||||||
------
|
------
|
||||||
|
@ -159,6 +159,8 @@ Limitations of openpilot ALC and LDW
|
||||||
|
|
||||||
openpilot ALC and openpilot LDW do not automatically drive the vehicle or reduce the amount of attention that must be paid to operate your vehicle. The driver must always keep control of the steering wheel and be ready to correct the openpilot ALC action at all times.
|
openpilot ALC and openpilot LDW do not automatically drive the vehicle or reduce the amount of attention that must be paid to operate your vehicle. The driver must always keep control of the steering wheel and be ready to correct the openpilot ALC action at all times.
|
||||||
|
|
||||||
|
While changing lanes, openpilot is not capable of looking next to you or checking your blind spot. Only nudge the wheel to initiate a lane change after you have confirmed it's safe to do so.
|
||||||
|
|
||||||
Many factors can impact the performance of openpilot ALC and openpilot LDW, causing them to be unable to function as intended. These include, but are not limited to:
|
Many factors can impact the performance of openpilot ALC and openpilot LDW, causing them to be unable to function as intended. These include, but are not limited to:
|
||||||
|
|
||||||
* Poor visibility (heavy rain, snow, fog, etc.) or weather conditions that may interfere with sensor operation.
|
* Poor visibility (heavy rain, snow, fog, etc.) or weather conditions that may interfere with sensor operation.
|
||||||
|
|
|
@ -1,3 +1,12 @@
|
||||||
|
Version 0.7.1 (2020-01-20)
|
||||||
|
========================
|
||||||
|
* comma two support!
|
||||||
|
* Lane Change Assist above 45 mph!
|
||||||
|
* Replace zmq with custom messaging library, msgq!
|
||||||
|
* Supercombo model: calibration and driving models are combined for better lead estimate
|
||||||
|
* More robust updater thanks to jyoung8607! Requires NEOS update
|
||||||
|
* Improve low speed ACC tuning
|
||||||
|
|
||||||
Version 0.7 (2019-12-13)
|
Version 0.7 (2019-12-13)
|
||||||
========================
|
========================
|
||||||
* Move to SCons build system!
|
* Move to SCons build system!
|
||||||
|
|
|
@ -46,6 +46,7 @@ else:
|
||||||
"#phonelibs/capnp-cpp/include",
|
"#phonelibs/capnp-cpp/include",
|
||||||
"#phonelibs/capnp-c/include",
|
"#phonelibs/capnp-c/include",
|
||||||
"#phonelibs/zmq/x64/include",
|
"#phonelibs/zmq/x64/include",
|
||||||
|
"#external/tensorflow/include",
|
||||||
]
|
]
|
||||||
libpath = [
|
libpath = [
|
||||||
"#phonelibs/capnp-cpp/x64/lib",
|
"#phonelibs/capnp-cpp/x64/lib",
|
||||||
|
@ -55,6 +56,7 @@ else:
|
||||||
"#phonelibs/zmq/x64/lib",
|
"#phonelibs/zmq/x64/lib",
|
||||||
"#phonelibs/libyuv/x64/lib",
|
"#phonelibs/libyuv/x64/lib",
|
||||||
"#external/zmq/lib",
|
"#external/zmq/lib",
|
||||||
|
"#external/tensorflow/lib",
|
||||||
"#cereal",
|
"#cereal",
|
||||||
"#selfdrive/common",
|
"#selfdrive/common",
|
||||||
"/usr/lib",
|
"/usr/lib",
|
||||||
|
@ -62,6 +64,7 @@ else:
|
||||||
]
|
]
|
||||||
|
|
||||||
rpath = ["phonelibs/capnp-cpp/x64/lib",
|
rpath = ["phonelibs/capnp-cpp/x64/lib",
|
||||||
|
"external/tensorflow/lib",
|
||||||
"cereal",
|
"cereal",
|
||||||
"selfdrive/common"]
|
"selfdrive/common"]
|
||||||
|
|
||||||
|
@ -201,11 +204,13 @@ SConscript(['selfdrive/controls/lib/longitudinal_mpc/SConscript'])
|
||||||
SConscript(['selfdrive/boardd/SConscript'])
|
SConscript(['selfdrive/boardd/SConscript'])
|
||||||
SConscript(['selfdrive/proclogd/SConscript'])
|
SConscript(['selfdrive/proclogd/SConscript'])
|
||||||
|
|
||||||
|
SConscript(['selfdrive/ui/SConscript'])
|
||||||
|
SConscript(['selfdrive/loggerd/SConscript'])
|
||||||
|
|
||||||
if arch == "aarch64":
|
if arch == "aarch64":
|
||||||
SConscript(['selfdrive/logcatd/SConscript'])
|
SConscript(['selfdrive/logcatd/SConscript'])
|
||||||
SConscript(['selfdrive/ui/SConscript'])
|
|
||||||
SConscript(['selfdrive/sensord/SConscript'])
|
SConscript(['selfdrive/sensord/SConscript'])
|
||||||
SConscript(['selfdrive/loggerd/SConscript'])
|
SConscript(['selfdrive/clocksd/SConscript'])
|
||||||
|
|
||||||
SConscript(['selfdrive/locationd/SConscript'])
|
SConscript(['selfdrive/locationd/SConscript'])
|
||||||
|
|
||||||
|
|
Binary file not shown.
Binary file not shown.
|
@ -1,10 +1,15 @@
|
||||||
|
import os
|
||||||
import binascii
|
import binascii
|
||||||
import itertools
|
import itertools
|
||||||
import re
|
import re
|
||||||
import struct
|
import struct
|
||||||
import subprocess
|
import subprocess
|
||||||
|
|
||||||
|
ANDROID = os.path.isfile('/EON')
|
||||||
|
|
||||||
def getprop(key):
|
def getprop(key):
|
||||||
|
if not ANDROID:
|
||||||
|
return ""
|
||||||
return subprocess.check_output(["getprop", key], encoding='utf8').strip()
|
return subprocess.check_output(["getprop", key], encoding='utf8').strip()
|
||||||
|
|
||||||
def get_imei():
|
def get_imei():
|
||||||
|
@ -14,7 +19,10 @@ def get_imei():
|
||||||
return ret
|
return ret
|
||||||
|
|
||||||
def get_serial():
|
def get_serial():
|
||||||
return getprop("ro.serialno")
|
ret = getprop("ro.serialno")
|
||||||
|
if ret == "":
|
||||||
|
ret = "cccccccc"
|
||||||
|
return ret
|
||||||
|
|
||||||
def get_subscriber_info():
|
def get_subscriber_info():
|
||||||
ret = parse_service_call_string(["iphonesubinfo", "7"])
|
ret = parse_service_call_string(["iphonesubinfo", "7"])
|
||||||
|
@ -60,6 +68,8 @@ def parse_service_call_string(call):
|
||||||
return None
|
return None
|
||||||
|
|
||||||
def parse_service_call_bytes(call):
|
def parse_service_call_bytes(call):
|
||||||
|
if not ANDROID:
|
||||||
|
return None
|
||||||
ret = subprocess.check_output(["service", "call", *call], encoding='utf8').strip()
|
ret = subprocess.check_output(["service", "call", *call], encoding='utf8').strip()
|
||||||
if 'Parcel' not in ret:
|
if 'Parcel' not in ret:
|
||||||
return None
|
return None
|
||||||
|
|
|
@ -78,28 +78,6 @@ class SwagLogger(logging.Logger):
|
||||||
self.log_local = local()
|
self.log_local = local()
|
||||||
self.log_local.ctx = {}
|
self.log_local.ctx = {}
|
||||||
|
|
||||||
def findCaller(self, stack_info=None):
|
|
||||||
"""
|
|
||||||
Find the stack frame of the caller so that we can note the source
|
|
||||||
file name, line number and function name.
|
|
||||||
"""
|
|
||||||
# f = currentframe()
|
|
||||||
f = sys._getframe(3)
|
|
||||||
#On some versions of IronPython, currentframe() returns None if
|
|
||||||
#IronPython isn't run with -X:Frames.
|
|
||||||
if f is not None:
|
|
||||||
f = f.f_back
|
|
||||||
rv = "(unknown file)", 0, "(unknown function)"
|
|
||||||
while hasattr(f, "f_code"):
|
|
||||||
co = f.f_code
|
|
||||||
filename = os.path.normcase(co.co_filename)
|
|
||||||
if filename in (logging._srcfile, _srcfile):
|
|
||||||
f = f.f_back
|
|
||||||
continue
|
|
||||||
rv = (co.co_filename, f.f_lineno, co.co_name)
|
|
||||||
break
|
|
||||||
return rv
|
|
||||||
|
|
||||||
def local_ctx(self):
|
def local_ctx(self):
|
||||||
try:
|
try:
|
||||||
return self.log_local.ctx
|
return self.log_local.ctx
|
||||||
|
|
|
@ -69,9 +69,10 @@ keys = {
|
||||||
"IsLdwEnabled": [TxType.PERSISTENT],
|
"IsLdwEnabled": [TxType.PERSISTENT],
|
||||||
"IsGeofenceEnabled": [TxType.PERSISTENT],
|
"IsGeofenceEnabled": [TxType.PERSISTENT],
|
||||||
"IsMetric": [TxType.PERSISTENT],
|
"IsMetric": [TxType.PERSISTENT],
|
||||||
|
"IsOffroad": [TxType.CLEAR_ON_MANAGER_START],
|
||||||
"IsRHD": [TxType.PERSISTENT],
|
"IsRHD": [TxType.PERSISTENT],
|
||||||
"IsTakingSnapshot": [TxType.CLEAR_ON_MANAGER_START],
|
"IsTakingSnapshot": [TxType.CLEAR_ON_MANAGER_START],
|
||||||
"IsUpdateAvailable": [TxType.PERSISTENT],
|
"IsUpdateAvailable": [TxType.CLEAR_ON_MANAGER_START],
|
||||||
"IsUploadRawEnabled": [TxType.PERSISTENT],
|
"IsUploadRawEnabled": [TxType.PERSISTENT],
|
||||||
"LastUpdateTime": [TxType.PERSISTENT],
|
"LastUpdateTime": [TxType.PERSISTENT],
|
||||||
"LimitSetSpeed": [TxType.PERSISTENT],
|
"LimitSetSpeed": [TxType.PERSISTENT],
|
||||||
|
@ -80,6 +81,7 @@ keys = {
|
||||||
"LongitudinalControl": [TxType.PERSISTENT],
|
"LongitudinalControl": [TxType.PERSISTENT],
|
||||||
"OpenpilotEnabledToggle": [TxType.PERSISTENT],
|
"OpenpilotEnabledToggle": [TxType.PERSISTENT],
|
||||||
"PandaFirmware": [TxType.CLEAR_ON_MANAGER_START, TxType.CLEAR_ON_PANDA_DISCONNECT],
|
"PandaFirmware": [TxType.CLEAR_ON_MANAGER_START, TxType.CLEAR_ON_PANDA_DISCONNECT],
|
||||||
|
"PandaFirmwareHex": [TxType.CLEAR_ON_MANAGER_START, TxType.CLEAR_ON_PANDA_DISCONNECT],
|
||||||
"PandaDongleId": [TxType.CLEAR_ON_MANAGER_START, TxType.CLEAR_ON_PANDA_DISCONNECT],
|
"PandaDongleId": [TxType.CLEAR_ON_MANAGER_START, TxType.CLEAR_ON_PANDA_DISCONNECT],
|
||||||
"Passive": [TxType.PERSISTENT],
|
"Passive": [TxType.PERSISTENT],
|
||||||
"RecordFront": [TxType.PERSISTENT],
|
"RecordFront": [TxType.PERSISTENT],
|
||||||
|
|
|
@ -5,21 +5,31 @@ from common.basedir import BASEDIR
|
||||||
|
|
||||||
class Spinner():
|
class Spinner():
|
||||||
def __init__(self):
|
def __init__(self):
|
||||||
self.spinner_proc = subprocess.Popen(["./spinner"],
|
try:
|
||||||
stdin=subprocess.PIPE,
|
self.spinner_proc = subprocess.Popen(["./spinner"],
|
||||||
cwd=os.path.join(BASEDIR, "selfdrive", "ui", "spinner"),
|
stdin=subprocess.PIPE,
|
||||||
close_fds=True)
|
cwd=os.path.join(BASEDIR, "selfdrive", "ui", "spinner"),
|
||||||
|
close_fds=True)
|
||||||
|
except OSError:
|
||||||
|
self.spinner_proc = None
|
||||||
|
|
||||||
def __enter__(self):
|
def __enter__(self):
|
||||||
return self
|
return self
|
||||||
|
|
||||||
def update(self, spinner_text):
|
def update(self, spinner_text):
|
||||||
self.spinner_proc.stdin.write(spinner_text.encode('utf8') + b"\n")
|
if self.spinner_proc is not None:
|
||||||
self.spinner_proc.stdin.flush()
|
self.spinner_proc.stdin.write(spinner_text.encode('utf8') + b"\n")
|
||||||
|
try:
|
||||||
|
self.spinner_proc.stdin.flush()
|
||||||
|
except BrokenPipeError:
|
||||||
|
pass
|
||||||
|
|
||||||
def close(self):
|
def close(self):
|
||||||
if self.spinner_proc is not None:
|
if self.spinner_proc is not None:
|
||||||
self.spinner_proc.stdin.close()
|
try:
|
||||||
|
self.spinner_proc.stdin.close()
|
||||||
|
except BrokenPipeError:
|
||||||
|
pass
|
||||||
self.spinner_proc.terminate()
|
self.spinner_proc.terminate()
|
||||||
self.spinner_proc = None
|
self.spinner_proc = None
|
||||||
|
|
||||||
|
|
|
@ -44,6 +44,7 @@ def get_calib_from_vp(vp):
|
||||||
roll_calib = 0
|
roll_calib = 0
|
||||||
return roll_calib, pitch_calib, yaw_calib
|
return roll_calib, pitch_calib, yaw_calib
|
||||||
|
|
||||||
|
|
||||||
# aka 'extrinsic_matrix'
|
# aka 'extrinsic_matrix'
|
||||||
# road : x->forward, y -> left, z->up
|
# road : x->forward, y -> left, z->up
|
||||||
def get_view_frame_from_road_frame(roll, pitch, yaw, height):
|
def get_view_frame_from_road_frame(roll, pitch, yaw, height):
|
||||||
|
@ -61,6 +62,13 @@ def vp_from_ke(m):
|
||||||
"""
|
"""
|
||||||
return (m[0, 0]/m[2,0], m[1,0]/m[2,0])
|
return (m[0, 0]/m[2,0], m[1,0]/m[2,0])
|
||||||
|
|
||||||
|
|
||||||
|
def vp_from_rpy(rpy):
|
||||||
|
e = get_view_frame_from_road_frame(rpy[0], rpy[1], rpy[2], 1.22)
|
||||||
|
ke = np.dot(eon_intrinsics, e)
|
||||||
|
return vp_from_ke(ke)
|
||||||
|
|
||||||
|
|
||||||
def roll_from_ke(m):
|
def roll_from_ke(m):
|
||||||
# note: different from calibration.h/RollAnglefromKE: i think that one's just wrong
|
# note: different from calibration.h/RollAnglefromKE: i think that one's just wrong
|
||||||
return np.arctan2(-(m[1, 0] - m[1, 1] * m[2, 0] / m[2, 1]),
|
return np.arctan2(-(m[1, 0] - m[1, 1] * m[2, 0] / m[2, 1]),
|
||||||
|
|
|
@ -1,7 +1,7 @@
|
||||||
{
|
{
|
||||||
"ota_url": "https://commadist.azureedge.net/neosupdate/ota-signed-07df505453684371b6c22583ffbb74ee414fcd389a46ff369ffd1b6bac75414e.zip",
|
"ota_url": "https://commadist.azureedge.net/neosupdate/ota-signed-efdf7de63b1aef63d68301e6175930991bf9a5927d16ec6fcc69287e2ee7ca4a.zip",
|
||||||
"ota_hash": "07df505453684371b6c22583ffbb74ee414fcd389a46ff369ffd1b6bac75414e",
|
"ota_hash": "efdf7de63b1aef63d68301e6175930991bf9a5927d16ec6fcc69287e2ee7ca4a",
|
||||||
"recovery_url": "https://commadist.azureedge.net/neosupdate/recovery-3a6f973295ded6e4ff5cfff3b12e19c80d3bf45e2e8dd8699da3fc25b23ed7c6.img",
|
"recovery_url": "https://commadist.azureedge.net/neosupdate/recovery-97c27e6ed04ed6bb0608b845a2d4100912093f9380c3f2ba6b56bccd608e5f6e.img",
|
||||||
"recovery_len": 15848748,
|
"recovery_len": 15861036,
|
||||||
"recovery_hash": "3a6f973295ded6e4ff5cfff3b12e19c80d3bf45e2e8dd8699da3fc25b23ed7c6"
|
"recovery_hash": "97c27e6ed04ed6bb0608b845a2d4100912093f9380c3f2ba6b56bccd608e5f6e"
|
||||||
}
|
}
|
||||||
|
|
|
@ -6,20 +6,53 @@ export NUMEXPR_NUM_THREADS=1
|
||||||
export OPENBLAS_NUM_THREADS=1
|
export OPENBLAS_NUM_THREADS=1
|
||||||
export VECLIB_MAXIMUM_THREADS=1
|
export VECLIB_MAXIMUM_THREADS=1
|
||||||
|
|
||||||
|
if [ -z "$BASEDIR" ]; then
|
||||||
|
BASEDIR="/data/openpilot"
|
||||||
|
fi
|
||||||
|
|
||||||
if [ -z "$PASSIVE" ]; then
|
if [ -z "$PASSIVE" ]; then
|
||||||
export PASSIVE="1"
|
export PASSIVE="1"
|
||||||
fi
|
fi
|
||||||
|
|
||||||
|
STAGING_ROOT="/data/safe_staging"
|
||||||
|
|
||||||
function launch {
|
function launch {
|
||||||
# Wifi scan
|
# Wifi scan
|
||||||
wpa_cli IFNAME=wlan0 SCAN
|
wpa_cli IFNAME=wlan0 SCAN
|
||||||
|
|
||||||
# apply update
|
# Check to see if there's a valid overlay-based update available. Conditions
|
||||||
if [ "$(git rev-parse HEAD)" != "$(git rev-parse @{u})" ]; then
|
# are as follows:
|
||||||
git reset --hard @{u} &&
|
#
|
||||||
git clean -xdf &&
|
# 1. The BASEDIR init file has to exist, with a newer modtime than anything in
|
||||||
|
# the BASEDIR Git repo. This checks for local development work or the user
|
||||||
|
# switching branches/forks, which should not be overwritten.
|
||||||
|
# 2. The FINALIZED consistent file has to exist, indicating there's an update
|
||||||
|
# that completed successfully and synced to disk.
|
||||||
|
|
||||||
exec "${BASH_SOURCE[0]}"
|
if [ -f "${BASEDIR}/.overlay_init" ]; then
|
||||||
|
find ${BASEDIR}/.git -newer ${BASEDIR}/.overlay_init | grep -q '.' 2> /dev/null
|
||||||
|
if [ $? -eq 0 ]; then
|
||||||
|
echo "${BASEDIR} has been modified, skipping overlay update installation"
|
||||||
|
else
|
||||||
|
if [ -f "${STAGING_ROOT}/finalized/.overlay_consistent" ]; then
|
||||||
|
if [ ! -d /data/safe_staging/old_openpilot ]; then
|
||||||
|
echo "Valid overlay update found, installing"
|
||||||
|
LAUNCHER_LOCATION="${BASH_SOURCE[0]}"
|
||||||
|
|
||||||
|
mv $BASEDIR /data/safe_staging/old_openpilot
|
||||||
|
mv "${STAGING_ROOT}/finalized" $BASEDIR
|
||||||
|
|
||||||
|
# The mv changed our working directory to /data/safe_staging/old_openpilot
|
||||||
|
cd "${BASEDIR}"
|
||||||
|
|
||||||
|
echo "Restarting launch script ${LAUNCHER_LOCATION}"
|
||||||
|
exec "${LAUNCHER_LOCATION}"
|
||||||
|
else
|
||||||
|
echo "openpilot backup found, not updating"
|
||||||
|
# TODO: restore backup? This means the updater didn't start after swapping
|
||||||
|
fi
|
||||||
|
fi
|
||||||
|
fi
|
||||||
fi
|
fi
|
||||||
|
|
||||||
# no cpu rationing for now
|
# no cpu rationing for now
|
||||||
|
@ -32,17 +65,17 @@ function launch {
|
||||||
DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" >/dev/null && pwd )"
|
DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" >/dev/null && pwd )"
|
||||||
|
|
||||||
# Remove old NEOS update file
|
# Remove old NEOS update file
|
||||||
|
# TODO: move this code to the updater
|
||||||
if [ -d /data/neoupdate ]; then
|
if [ -d /data/neoupdate ]; then
|
||||||
rm -rf /data/neoupdate
|
rm -rf /data/neoupdate
|
||||||
fi
|
fi
|
||||||
|
|
||||||
# Check for NEOS update
|
# Check for NEOS update
|
||||||
if [ $(< /VERSION) != "13" ]; then
|
if [ $(< /VERSION) != "14" ]; then
|
||||||
if [ -f "$DIR/scripts/continue.sh" ]; then
|
if [ -f "$DIR/scripts/continue.sh" ]; then
|
||||||
cp "$DIR/scripts/continue.sh" "/data/data/com.termux/files/continue.sh"
|
cp "$DIR/scripts/continue.sh" "/data/data/com.termux/files/continue.sh"
|
||||||
fi
|
fi
|
||||||
|
|
||||||
git clean -xdf
|
|
||||||
"$DIR/installer/updater/updater" "file://$DIR/installer/updater/update.json"
|
"$DIR/installer/updater/updater" "file://$DIR/installer/updater/update.json"
|
||||||
fi
|
fi
|
||||||
|
|
||||||
|
|
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
|
@ -0,0 +1,7 @@
|
||||||
|
#!/usr/bin/env sh
|
||||||
|
|
||||||
|
# Stop updater
|
||||||
|
pkill -2 -f selfdrive.updated
|
||||||
|
|
||||||
|
# Remove pending update
|
||||||
|
rm -f /data/safe_staging/finalized/.overlay_consistent
|
|
@ -0,0 +1,4 @@
|
||||||
|
#!/usr/bin/env sh
|
||||||
|
|
||||||
|
# Send SIGHUP to updater
|
||||||
|
pkill -1 -f selfdrive.updated
|
|
@ -32,10 +32,8 @@ class TestAthenadMethods(unittest.TestCase):
|
||||||
assert dispatcher["echo"]("bob") == "bob"
|
assert dispatcher["echo"]("bob") == "bob"
|
||||||
|
|
||||||
def test_getMessage(self):
|
def test_getMessage(self):
|
||||||
try:
|
with self.assertRaises(TimeoutError) as _:
|
||||||
dispatcher["getMessage"]("controlsState")
|
dispatcher["getMessage"]("controlsState")
|
||||||
except TimeoutError:
|
|
||||||
pass
|
|
||||||
|
|
||||||
def send_thermal():
|
def send_thermal():
|
||||||
messaging.context = messaging.Context()
|
messaging.context = messaging.Context()
|
||||||
|
@ -50,6 +48,7 @@ class TestAthenadMethods(unittest.TestCase):
|
||||||
|
|
||||||
p = Process(target=send_thermal)
|
p = Process(target=send_thermal)
|
||||||
p.start()
|
p.start()
|
||||||
|
time.sleep(0.1)
|
||||||
try:
|
try:
|
||||||
thermal = dispatcher["getMessage"]("thermal")
|
thermal = dispatcher["getMessage"]("thermal")
|
||||||
assert thermal['thermal']
|
assert thermal['thermal']
|
||||||
|
@ -60,7 +59,7 @@ class TestAthenadMethods(unittest.TestCase):
|
||||||
print(dispatcher["listDataDirectory"]())
|
print(dispatcher["listDataDirectory"]())
|
||||||
|
|
||||||
@with_http_server
|
@with_http_server
|
||||||
def test_do_upload(self):
|
def test_do_upload(self, host):
|
||||||
fn = os.path.join(athenad.ROOT, 'qlog.bz2')
|
fn = os.path.join(athenad.ROOT, 'qlog.bz2')
|
||||||
Path(fn).touch()
|
Path(fn).touch()
|
||||||
|
|
||||||
|
@ -71,14 +70,14 @@ class TestAthenadMethods(unittest.TestCase):
|
||||||
except requests.exceptions.ConnectionError:
|
except requests.exceptions.ConnectionError:
|
||||||
pass
|
pass
|
||||||
|
|
||||||
item = athenad.UploadItem(path=fn, url="http://localhost:44444/qlog.bz2", headers={}, created_at=int(time.time()*1000), id='')
|
item = athenad.UploadItem(path=fn, url=f"{host}/qlog.bz2", headers={}, created_at=int(time.time()*1000), id='')
|
||||||
resp = athenad._do_upload(item)
|
resp = athenad._do_upload(item)
|
||||||
self.assertEqual(resp.status_code, 201)
|
self.assertEqual(resp.status_code, 201)
|
||||||
finally:
|
finally:
|
||||||
os.unlink(fn)
|
os.unlink(fn)
|
||||||
|
|
||||||
@with_http_server
|
@with_http_server
|
||||||
def test_uploadFileToUrl(self):
|
def test_uploadFileToUrl(self, host):
|
||||||
not_exists_resp = dispatcher["uploadFileToUrl"]("does_not_exist.bz2", "http://localhost:1238", {})
|
not_exists_resp = dispatcher["uploadFileToUrl"]("does_not_exist.bz2", "http://localhost:1238", {})
|
||||||
self.assertEqual(not_exists_resp, 404)
|
self.assertEqual(not_exists_resp, 404)
|
||||||
|
|
||||||
|
@ -86,9 +85,9 @@ class TestAthenadMethods(unittest.TestCase):
|
||||||
Path(fn).touch()
|
Path(fn).touch()
|
||||||
|
|
||||||
try:
|
try:
|
||||||
resp = dispatcher["uploadFileToUrl"]("qlog.bz2", "http://localhost:44444/qlog.bz2", {})
|
resp = dispatcher["uploadFileToUrl"]("qlog.bz2", f"{host}/qlog.bz2", {})
|
||||||
self.assertEqual(resp['enqueued'], 1)
|
self.assertEqual(resp['enqueued'], 1)
|
||||||
self.assertDictContainsSubset({"path": fn, "url": "http://localhost:44444/qlog.bz2", "headers": {}}, resp['item'])
|
self.assertDictContainsSubset({"path": fn, "url": f"{host}/qlog.bz2", "headers": {}}, resp['item'])
|
||||||
self.assertIsNotNone(resp['item'].get('id'))
|
self.assertIsNotNone(resp['item'].get('id'))
|
||||||
self.assertEqual(athenad.upload_queue.qsize(), 1)
|
self.assertEqual(athenad.upload_queue.qsize(), 1)
|
||||||
finally:
|
finally:
|
||||||
|
@ -96,10 +95,10 @@ class TestAthenadMethods(unittest.TestCase):
|
||||||
os.unlink(fn)
|
os.unlink(fn)
|
||||||
|
|
||||||
@with_http_server
|
@with_http_server
|
||||||
def test_upload_handler(self):
|
def test_upload_handler(self, host):
|
||||||
fn = os.path.join(athenad.ROOT, 'qlog.bz2')
|
fn = os.path.join(athenad.ROOT, 'qlog.bz2')
|
||||||
Path(fn).touch()
|
Path(fn).touch()
|
||||||
item = athenad.UploadItem(path=fn, url="http://localhost:44444/qlog.bz2", headers={}, created_at=int(time.time()*1000), id='')
|
item = athenad.UploadItem(path=fn, url=f"{host}/qlog.bz2", headers={}, created_at=int(time.time()*1000), id='')
|
||||||
|
|
||||||
end_event = threading.Event()
|
end_event = threading.Event()
|
||||||
thread = threading.Thread(target=athenad.upload_handler, args=(end_event,))
|
thread = threading.Thread(target=athenad.upload_handler, args=(end_event,))
|
||||||
|
|
|
@ -1,4 +1,7 @@
|
||||||
import http.server
|
import http.server
|
||||||
|
import multiprocessing
|
||||||
|
import queue
|
||||||
|
import random
|
||||||
import requests
|
import requests
|
||||||
import socket
|
import socket
|
||||||
import time
|
import time
|
||||||
|
@ -70,27 +73,41 @@ class HTTPRequestHandler(http.server.SimpleHTTPRequestHandler):
|
||||||
self.send_response(201, "Created")
|
self.send_response(201, "Created")
|
||||||
self.end_headers()
|
self.end_headers()
|
||||||
|
|
||||||
|
def http_server(port_queue, **kwargs):
|
||||||
|
while 1:
|
||||||
|
try:
|
||||||
|
port = random.randrange(40000, 50000)
|
||||||
|
port_queue.put(port)
|
||||||
|
http.server.test(**kwargs, port=port)
|
||||||
|
except OSError as e:
|
||||||
|
if e.errno == 98:
|
||||||
|
continue
|
||||||
|
|
||||||
def with_http_server(func):
|
def with_http_server(func):
|
||||||
@wraps(func)
|
@wraps(func)
|
||||||
def inner(*args, **kwargs):
|
def inner(*args, **kwargs):
|
||||||
p = Process(target=http.server.test,
|
port_queue = multiprocessing.Queue()
|
||||||
|
host = '127.0.0.1'
|
||||||
|
p = Process(target=http_server,
|
||||||
|
args=(port_queue,),
|
||||||
kwargs={
|
kwargs={
|
||||||
'HandlerClass': HTTPRequestHandler,
|
'HandlerClass': HTTPRequestHandler,
|
||||||
'port': 44444,
|
'bind': host})
|
||||||
'bind': '127.0.0.1'})
|
|
||||||
p.start()
|
p.start()
|
||||||
now = time.time()
|
now = time.time()
|
||||||
|
port = None
|
||||||
while 1:
|
while 1:
|
||||||
if time.time() - now > 5:
|
if time.time() - now > 5:
|
||||||
raise Exception('HTTP Server did not start')
|
raise Exception('HTTP Server did not start')
|
||||||
try:
|
try:
|
||||||
requests.put('http://localhost:44444/qlog.bz2', data='')
|
port = port_queue.get(timeout=0.1)
|
||||||
|
requests.put(f'http://{host}:{port}/qlog.bz2', data='')
|
||||||
break
|
break
|
||||||
except requests.exceptions.ConnectionError:
|
except (requests.exceptions.ConnectionError, queue.Empty):
|
||||||
time.sleep(0.1)
|
time.sleep(0.1)
|
||||||
|
|
||||||
try:
|
try:
|
||||||
return func(*args, **kwargs)
|
return func(*args, f'http://{host}:{port}', **kwargs)
|
||||||
finally:
|
finally:
|
||||||
p.terminate()
|
p.terminate()
|
||||||
|
|
||||||
|
|
|
@ -21,6 +21,7 @@
|
||||||
#include "cereal/gen/cpp/log.capnp.h"
|
#include "cereal/gen/cpp/log.capnp.h"
|
||||||
#include "cereal/gen/cpp/car.capnp.h"
|
#include "cereal/gen/cpp/car.capnp.h"
|
||||||
|
|
||||||
|
#include "common/util.h"
|
||||||
#include "common/messaging.h"
|
#include "common/messaging.h"
|
||||||
#include "common/params.h"
|
#include "common/params.h"
|
||||||
#include "common/swaglog.h"
|
#include "common/swaglog.h"
|
||||||
|
@ -35,8 +36,10 @@
|
||||||
|
|
||||||
#define MAX_IR_POWER 0.5f
|
#define MAX_IR_POWER 0.5f
|
||||||
#define MIN_IR_POWER 0.0f
|
#define MIN_IR_POWER 0.0f
|
||||||
#define CUTOFF_GAIN 0.015625f // iso400
|
#define CUTOFF_GAIN 0.015625f // iso400
|
||||||
#define SATURATE_GAIN 0.0625f // iso1600
|
#define SATURATE_GAIN 0.0625f // iso1600
|
||||||
|
#define NIBBLE_TO_HEX(n) ((n) < 10 ? (n) + '0' : ((n) - 10) + 'a')
|
||||||
|
#define VOLTAGE_K 0.091 // LPF gain for 5s tau (dt/tau / (dt/tau + 1))
|
||||||
|
|
||||||
namespace {
|
namespace {
|
||||||
|
|
||||||
|
@ -62,14 +65,19 @@ bool loopback_can = false;
|
||||||
cereal::HealthData::HwType hw_type = cereal::HealthData::HwType::UNKNOWN;
|
cereal::HealthData::HwType hw_type = cereal::HealthData::HwType::UNKNOWN;
|
||||||
bool is_pigeon = false;
|
bool is_pigeon = false;
|
||||||
const uint32_t NO_IGNITION_CNT_MAX = 2 * 60 * 60 * 30; // turn off charge after 30 hrs
|
const uint32_t NO_IGNITION_CNT_MAX = 2 * 60 * 60 * 30; // turn off charge after 30 hrs
|
||||||
const uint32_t VBATT_START_CHARGING = 11500;
|
const float VBATT_START_CHARGING = 11.5;
|
||||||
const uint32_t VBATT_PAUSE_CHARGING = 10500;
|
const float VBATT_PAUSE_CHARGING = 11.0;
|
||||||
|
float voltage_f = 12.5; // filtered voltage
|
||||||
uint32_t no_ignition_cnt = 0;
|
uint32_t no_ignition_cnt = 0;
|
||||||
bool connected_once = false;
|
bool connected_once = false;
|
||||||
bool ignition_last = false;
|
bool ignition_last = false;
|
||||||
|
|
||||||
pthread_t safety_setter_thread_handle = -1;
|
bool safety_setter_thread_initialized = false;
|
||||||
pthread_t pigeon_thread_handle = -1;
|
pthread_t safety_setter_thread_handle;
|
||||||
|
|
||||||
|
bool pigeon_thread_initialized = false;
|
||||||
|
pthread_t pigeon_thread_handle;
|
||||||
|
|
||||||
bool pigeon_needs_init;
|
bool pigeon_needs_init;
|
||||||
|
|
||||||
void pigeon_init();
|
void pigeon_init();
|
||||||
|
@ -130,10 +138,7 @@ void *safety_setter_thread(void *s) {
|
||||||
pthread_mutex_lock(&usb_lock);
|
pthread_mutex_lock(&usb_lock);
|
||||||
|
|
||||||
// set in the mutex to avoid race
|
// set in the mutex to avoid race
|
||||||
safety_setter_thread_handle = -1;
|
safety_setter_thread_initialized = false;
|
||||||
|
|
||||||
// set if long_control is allowed by openpilot. Hardcoded to True for now
|
|
||||||
libusb_control_transfer(dev_handle, 0x40, 0xdf, 1, 0, NULL, 0, TIMEOUT);
|
|
||||||
|
|
||||||
libusb_control_transfer(dev_handle, 0x40, 0xdc, safety_model, safety_param, NULL, 0, TIMEOUT);
|
libusb_control_transfer(dev_handle, 0x40, 0xdc, safety_model, safety_param, NULL, 0, TIMEOUT);
|
||||||
|
|
||||||
|
@ -144,13 +149,12 @@ void *safety_setter_thread(void *s) {
|
||||||
|
|
||||||
// must be called before threads or with mutex
|
// must be called before threads or with mutex
|
||||||
bool usb_connect() {
|
bool usb_connect() {
|
||||||
int err;
|
int err, err2;
|
||||||
unsigned char hw_query[1] = {0};
|
unsigned char hw_query[1] = {0};
|
||||||
unsigned char fw_ver_buf[64];
|
unsigned char fw_sig_buf[128];
|
||||||
|
unsigned char fw_sig_hex_buf[16];
|
||||||
unsigned char serial_buf[16];
|
unsigned char serial_buf[16];
|
||||||
const char *fw_ver;
|
|
||||||
const char *serial;
|
const char *serial;
|
||||||
int fw_ver_sz = 0;
|
|
||||||
int serial_sz = 0;
|
int serial_sz = 0;
|
||||||
|
|
||||||
ignition_last = false;
|
ignition_last = false;
|
||||||
|
@ -169,12 +173,17 @@ bool usb_connect() {
|
||||||
}
|
}
|
||||||
|
|
||||||
// get panda fw
|
// get panda fw
|
||||||
err = libusb_control_transfer(dev_handle, 0xc0, 0xd6, 0, 0, fw_ver_buf, 64, TIMEOUT);
|
err = libusb_control_transfer(dev_handle, 0xc0, 0xd3, 0, 0, fw_sig_buf, 64, TIMEOUT);
|
||||||
if (err > 0) {
|
err2 = libusb_control_transfer(dev_handle, 0xc0, 0xd4, 0, 0, fw_sig_buf + 64, 64, TIMEOUT);
|
||||||
fw_ver = (const char *)fw_ver_buf;
|
if ((err == 64) && (err2 == 64)) {
|
||||||
fw_ver_sz = err;
|
printf("FW signature read\n");
|
||||||
write_db_value(NULL, "PandaFirmware", fw_ver, fw_ver_sz);
|
write_db_value(NULL, "PandaFirmware", (const char *)fw_sig_buf, 128);
|
||||||
printf("panda fw: %.*s\n", fw_ver_sz, fw_ver);
|
|
||||||
|
for (size_t i = 0; i < 8; i++){
|
||||||
|
fw_sig_hex_buf[2*i] = NIBBLE_TO_HEX(fw_sig_buf[i] >> 4);
|
||||||
|
fw_sig_hex_buf[2*i+1] = NIBBLE_TO_HEX(fw_sig_buf[i] & 0xF);
|
||||||
|
}
|
||||||
|
write_db_value(NULL, "PandaFirmwareHex", (const char *)fw_sig_hex_buf, 16);
|
||||||
}
|
}
|
||||||
else { goto fail; }
|
else { goto fail; }
|
||||||
|
|
||||||
|
@ -206,9 +215,10 @@ bool usb_connect() {
|
||||||
if (is_pigeon) {
|
if (is_pigeon) {
|
||||||
LOGW("panda with gps detected");
|
LOGW("panda with gps detected");
|
||||||
pigeon_needs_init = true;
|
pigeon_needs_init = true;
|
||||||
if (pigeon_thread_handle == -1) {
|
if (!pigeon_thread_initialized) {
|
||||||
err = pthread_create(&pigeon_thread_handle, NULL, pigeon_thread, NULL);
|
err = pthread_create(&pigeon_thread_handle, NULL, pigeon_thread, NULL);
|
||||||
assert(err == 0);
|
assert(err == 0);
|
||||||
|
pigeon_thread_initialized = true;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -289,6 +299,8 @@ void can_recv(PubSocket *publisher) {
|
||||||
// return if length is 0
|
// return if length is 0
|
||||||
if (recv <= 0) {
|
if (recv <= 0) {
|
||||||
return;
|
return;
|
||||||
|
} else if (recv == RECV_SIZE) {
|
||||||
|
LOGW("Receive buffer full");
|
||||||
}
|
}
|
||||||
|
|
||||||
// create message
|
// create message
|
||||||
|
@ -296,7 +308,6 @@ void can_recv(PubSocket *publisher) {
|
||||||
cereal::Event::Builder event = msg.initRoot<cereal::Event>();
|
cereal::Event::Builder event = msg.initRoot<cereal::Event>();
|
||||||
event.setLogMonoTime(start_time);
|
event.setLogMonoTime(start_time);
|
||||||
size_t num_msg = recv / 0x10;
|
size_t num_msg = recv / 0x10;
|
||||||
|
|
||||||
auto canData = event.initCan(num_msg);
|
auto canData = event.initCan(num_msg);
|
||||||
|
|
||||||
// populate message
|
// populate message
|
||||||
|
@ -330,6 +341,7 @@ void can_health(PubSocket *publisher) {
|
||||||
uint32_t uptime;
|
uint32_t uptime;
|
||||||
uint32_t voltage;
|
uint32_t voltage;
|
||||||
uint32_t current;
|
uint32_t current;
|
||||||
|
uint32_t can_rx_errs;
|
||||||
uint32_t can_send_errs;
|
uint32_t can_send_errs;
|
||||||
uint32_t can_fwd_errs;
|
uint32_t can_fwd_errs;
|
||||||
uint32_t gmlan_send_errs;
|
uint32_t gmlan_send_errs;
|
||||||
|
@ -355,6 +367,12 @@ void can_health(PubSocket *publisher) {
|
||||||
} while(cnt != sizeof(health));
|
} while(cnt != sizeof(health));
|
||||||
pthread_mutex_unlock(&usb_lock);
|
pthread_mutex_unlock(&usb_lock);
|
||||||
|
|
||||||
|
if (spoofing_started) {
|
||||||
|
health.ignition_line = 1;
|
||||||
|
}
|
||||||
|
|
||||||
|
voltage_f = VOLTAGE_K * (health.voltage / 1000.0) + (1.0 - VOLTAGE_K) * voltage_f; // LPF
|
||||||
|
|
||||||
// Make sure CAN buses are live: safety_setter_thread does not work if Panda CAN are silent and there is only one other CAN node
|
// Make sure CAN buses are live: safety_setter_thread does not work if Panda CAN are silent and there is only one other CAN node
|
||||||
if (health.safety_model == (uint8_t)(cereal::CarParams::SafetyModel::SILENT)) {
|
if (health.safety_model == (uint8_t)(cereal::CarParams::SafetyModel::SILENT)) {
|
||||||
pthread_mutex_lock(&usb_lock);
|
pthread_mutex_lock(&usb_lock);
|
||||||
|
@ -373,13 +391,15 @@ void can_health(PubSocket *publisher) {
|
||||||
#ifndef __x86_64__
|
#ifndef __x86_64__
|
||||||
bool cdp_mode = health.usb_power_mode == (uint8_t)(cereal::HealthData::UsbPowerMode::CDP);
|
bool cdp_mode = health.usb_power_mode == (uint8_t)(cereal::HealthData::UsbPowerMode::CDP);
|
||||||
bool no_ignition_exp = no_ignition_cnt > NO_IGNITION_CNT_MAX;
|
bool no_ignition_exp = no_ignition_cnt > NO_IGNITION_CNT_MAX;
|
||||||
if ((no_ignition_exp || (health.voltage < VBATT_PAUSE_CHARGING)) && cdp_mode && !ignition) {
|
if ((no_ignition_exp || (voltage_f < VBATT_PAUSE_CHARGING)) && cdp_mode && !ignition) {
|
||||||
printf("TURN OFF CHARGING!\n");
|
printf("TURN OFF CHARGING!\n");
|
||||||
pthread_mutex_lock(&usb_lock);
|
pthread_mutex_lock(&usb_lock);
|
||||||
libusb_control_transfer(dev_handle, 0xc0, 0xe6, (uint16_t)(cereal::HealthData::UsbPowerMode::CLIENT), 0, NULL, 0, TIMEOUT);
|
libusb_control_transfer(dev_handle, 0xc0, 0xe6, (uint16_t)(cereal::HealthData::UsbPowerMode::CLIENT), 0, NULL, 0, TIMEOUT);
|
||||||
pthread_mutex_unlock(&usb_lock);
|
pthread_mutex_unlock(&usb_lock);
|
||||||
|
printf("POWER DOWN DEVICE\n");
|
||||||
|
system("service call power 17 i32 0 i32 1");
|
||||||
}
|
}
|
||||||
if (!no_ignition_exp && (health.voltage > VBATT_START_CHARGING) && !cdp_mode) {
|
if (!no_ignition_exp && (voltage_f > VBATT_START_CHARGING) && !cdp_mode) {
|
||||||
printf("TURN ON CHARGING!\n");
|
printf("TURN ON CHARGING!\n");
|
||||||
pthread_mutex_lock(&usb_lock);
|
pthread_mutex_lock(&usb_lock);
|
||||||
libusb_control_transfer(dev_handle, 0xc0, 0xe6, (uint16_t)(cereal::HealthData::UsbPowerMode::CDP), 0, NULL, 0, TIMEOUT);
|
libusb_control_transfer(dev_handle, 0xc0, 0xe6, (uint16_t)(cereal::HealthData::UsbPowerMode::CDP), 0, NULL, 0, TIMEOUT);
|
||||||
|
@ -406,15 +426,15 @@ void can_health(PubSocket *publisher) {
|
||||||
|
|
||||||
// clear VIN, CarParams, and set new safety on car start
|
// clear VIN, CarParams, and set new safety on car start
|
||||||
if (ignition && !ignition_last) {
|
if (ignition && !ignition_last) {
|
||||||
|
|
||||||
int result = delete_db_value(NULL, "CarVin");
|
int result = delete_db_value(NULL, "CarVin");
|
||||||
assert((result == 0) || (result == ERR_NO_VALUE));
|
assert((result == 0) || (result == ERR_NO_VALUE));
|
||||||
result = delete_db_value(NULL, "CarParams");
|
result = delete_db_value(NULL, "CarParams");
|
||||||
assert((result == 0) || (result == ERR_NO_VALUE));
|
assert((result == 0) || (result == ERR_NO_VALUE));
|
||||||
|
|
||||||
if (safety_setter_thread_handle == -1) {
|
if (!safety_setter_thread_initialized) {
|
||||||
err = pthread_create(&safety_setter_thread_handle, NULL, safety_setter_thread, NULL);
|
err = pthread_create(&safety_setter_thread_handle, NULL, safety_setter_thread, NULL);
|
||||||
assert(err == 0);
|
assert(err == 0);
|
||||||
|
safety_setter_thread_initialized = true;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -459,15 +479,12 @@ void can_health(PubSocket *publisher) {
|
||||||
healthData.setUptime(health.uptime);
|
healthData.setUptime(health.uptime);
|
||||||
healthData.setVoltage(health.voltage);
|
healthData.setVoltage(health.voltage);
|
||||||
healthData.setCurrent(health.current);
|
healthData.setCurrent(health.current);
|
||||||
if (spoofing_started) {
|
healthData.setIgnitionLine(health.ignition_line);
|
||||||
healthData.setIgnitionLine(true);
|
|
||||||
} else {
|
|
||||||
healthData.setIgnitionLine(health.ignition_line);
|
|
||||||
}
|
|
||||||
healthData.setIgnitionCan(health.ignition_can);
|
healthData.setIgnitionCan(health.ignition_can);
|
||||||
healthData.setControlsAllowed(health.controls_allowed);
|
healthData.setControlsAllowed(health.controls_allowed);
|
||||||
healthData.setGasInterceptorDetected(health.gas_interceptor_detected);
|
healthData.setGasInterceptorDetected(health.gas_interceptor_detected);
|
||||||
healthData.setHasGps(is_pigeon);
|
healthData.setHasGps(is_pigeon);
|
||||||
|
healthData.setCanRxErrs(health.can_rx_errs);
|
||||||
healthData.setCanSendErrs(health.can_send_errs);
|
healthData.setCanSendErrs(health.can_send_errs);
|
||||||
healthData.setCanFwdErrs(health.can_fwd_errs);
|
healthData.setCanFwdErrs(health.can_fwd_errs);
|
||||||
healthData.setGmlanSendErrs(health.gmlan_send_errs);
|
healthData.setGmlanSendErrs(health.gmlan_send_errs);
|
||||||
|
@ -842,14 +859,6 @@ void *pigeon_thread(void *crap) {
|
||||||
return NULL;
|
return NULL;
|
||||||
}
|
}
|
||||||
|
|
||||||
int set_realtime_priority(int level) {
|
|
||||||
// should match python using chrt
|
|
||||||
struct sched_param sa;
|
|
||||||
memset(&sa, 0, sizeof(sa));
|
|
||||||
sa.sched_priority = level;
|
|
||||||
return sched_setscheduler(getpid(), SCHED_FIFO, &sa);
|
|
||||||
}
|
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
int main() {
|
int main() {
|
||||||
|
|
|
@ -680,14 +680,6 @@ void* visionserver_client_thread(void* arg) {
|
||||||
} else {
|
} else {
|
||||||
assert(false);
|
assert(false);
|
||||||
}
|
}
|
||||||
|
|
||||||
if (stream_type == VISION_STREAM_RGB_BACK ||
|
|
||||||
stream_type == VISION_STREAM_RGB_FRONT) {
|
|
||||||
/*stream_bufs->buf_info.ui_info = (VisionUIInfo){
|
|
||||||
.transformed_width = s->model.in.transformed_width,
|
|
||||||
.transformed_height = s->model.in.transformed_height,
|
|
||||||
};*/
|
|
||||||
}
|
|
||||||
vipc_send(fd, &rep);
|
vipc_send(fd, &rep);
|
||||||
streams[stream_type].subscribed = true;
|
streams[stream_type].subscribed = true;
|
||||||
} else if (p.type == VIPC_STREAM_RELEASE) {
|
} else if (p.type == VIPC_STREAM_RELEASE) {
|
||||||
|
|
|
@ -3,6 +3,7 @@ from common.params import Params
|
||||||
from common.basedir import BASEDIR
|
from common.basedir import BASEDIR
|
||||||
from selfdrive.car.fingerprints import eliminate_incompatible_cars, all_known_cars
|
from selfdrive.car.fingerprints import eliminate_incompatible_cars, all_known_cars
|
||||||
from selfdrive.car.vin import get_vin, VIN_UNKNOWN
|
from selfdrive.car.vin import get_vin, VIN_UNKNOWN
|
||||||
|
from selfdrive.car.fw_versions import get_fw_versions
|
||||||
from selfdrive.swaglog import cloudlog
|
from selfdrive.swaglog import cloudlog
|
||||||
import cereal.messaging as messaging
|
import cereal.messaging as messaging
|
||||||
from selfdrive.car import gen_empty_fingerprint
|
from selfdrive.car import gen_empty_fingerprint
|
||||||
|
@ -56,12 +57,14 @@ def only_toyota_left(candidate_cars):
|
||||||
# BOUNTY: every added fingerprint in selfdrive/car/*/values.py is a $100 coupon code on shop.comma.ai
|
# BOUNTY: every added fingerprint in selfdrive/car/*/values.py is a $100 coupon code on shop.comma.ai
|
||||||
# **** for use live only ****
|
# **** for use live only ****
|
||||||
def fingerprint(logcan, sendcan, has_relay):
|
def fingerprint(logcan, sendcan, has_relay):
|
||||||
|
|
||||||
if has_relay:
|
if has_relay:
|
||||||
# Vin query only reliably works thorugh OBDII
|
# Vin query only reliably works thorugh OBDII
|
||||||
vin = get_vin(logcan, sendcan, 1)
|
bus = 1
|
||||||
|
addr, vin = get_vin(logcan, sendcan, bus)
|
||||||
|
_, car_fw = get_fw_versions(logcan, sendcan, bus)
|
||||||
else:
|
else:
|
||||||
vin = VIN_UNKNOWN
|
vin = VIN_UNKNOWN
|
||||||
|
_, car_fw = set(), []
|
||||||
|
|
||||||
cloudlog.warning("VIN %s", vin)
|
cloudlog.warning("VIN %s", vin)
|
||||||
Params().put("CarVin", vin)
|
Params().put("CarVin", vin)
|
||||||
|
@ -108,18 +111,19 @@ def fingerprint(logcan, sendcan, has_relay):
|
||||||
frame += 1
|
frame += 1
|
||||||
|
|
||||||
cloudlog.warning("fingerprinted %s", car_fingerprint)
|
cloudlog.warning("fingerprinted %s", car_fingerprint)
|
||||||
return car_fingerprint, finger, vin
|
return car_fingerprint, finger, vin, car_fw
|
||||||
|
|
||||||
|
|
||||||
def get_car(logcan, sendcan, has_relay=False):
|
def get_car(logcan, sendcan, has_relay=False):
|
||||||
|
candidate, fingerprints, vin, car_fw = fingerprint(logcan, sendcan, has_relay)
|
||||||
candidate, fingerprints, vin = fingerprint(logcan, sendcan, has_relay)
|
|
||||||
|
|
||||||
if candidate is None:
|
if candidate is None:
|
||||||
cloudlog.warning("car doesn't match any fingerprints: %r", fingerprints)
|
cloudlog.warning("car doesn't match any fingerprints: %r", fingerprints)
|
||||||
candidate = "mock"
|
candidate = "mock"
|
||||||
|
|
||||||
CarInterface, CarController = interfaces[candidate]
|
CarInterface, CarController = interfaces[candidate]
|
||||||
car_params = CarInterface.get_params(candidate, fingerprints, vin, has_relay)
|
car_params = CarInterface.get_params(candidate, fingerprints, has_relay, car_fw)
|
||||||
|
car_params.carVin = vin
|
||||||
|
car_params.carFw = car_fw
|
||||||
|
|
||||||
return CarInterface(car_params, CarController), car_params
|
return CarInterface(car_params, CarController), car_params
|
||||||
|
|
|
@ -35,13 +35,12 @@ class CarInterface(CarInterfaceBase):
|
||||||
return float(accel) / 3.0
|
return float(accel) / 3.0
|
||||||
|
|
||||||
@staticmethod
|
@staticmethod
|
||||||
def get_params(candidate, fingerprint=gen_empty_fingerprint(), vin="", has_relay=False):
|
def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False, car_fw=[]):
|
||||||
|
|
||||||
ret = car.CarParams.new_message()
|
ret = car.CarParams.new_message()
|
||||||
|
|
||||||
ret.carName = "chrysler"
|
ret.carName = "chrysler"
|
||||||
ret.carFingerprint = candidate
|
ret.carFingerprint = candidate
|
||||||
ret.carVin = vin
|
|
||||||
ret.isPandaBlack = has_relay
|
ret.isPandaBlack = has_relay
|
||||||
|
|
||||||
ret.safetyModel = car.CarParams.SafetyModel.chrysler
|
ret.safetyModel = car.CarParams.SafetyModel.chrysler
|
||||||
|
|
|
@ -1,27 +1,43 @@
|
||||||
import os
|
import os
|
||||||
from common.basedir import BASEDIR
|
from common.basedir import BASEDIR
|
||||||
|
|
||||||
|
def get_attr_from_cars(attr):
|
||||||
|
# read all the folders in selfdrive/car and return a dict where:
|
||||||
|
# - keys are all the car models
|
||||||
|
# - values are attr values from all car folders
|
||||||
|
result = {}
|
||||||
|
|
||||||
|
for car_folder in [x[0] for x in os.walk(BASEDIR + '/selfdrive/car')]:
|
||||||
|
try:
|
||||||
|
car_name = car_folder.split('/')[-1]
|
||||||
|
values = __import__('selfdrive.car.%s.values' % car_name, fromlist=[attr])
|
||||||
|
if hasattr(values, attr):
|
||||||
|
attr_values = getattr(values, attr)
|
||||||
|
else:
|
||||||
|
continue
|
||||||
|
|
||||||
|
for f, v in attr_values.items():
|
||||||
|
result[f] = v
|
||||||
|
|
||||||
|
except (ImportError, IOError):
|
||||||
|
pass
|
||||||
|
|
||||||
|
return result
|
||||||
|
|
||||||
|
|
||||||
|
def get_fw_versions_list():
|
||||||
|
return get_attr_from_cars('FW_VERSIONS')
|
||||||
|
|
||||||
|
|
||||||
def get_fingerprint_list():
|
def get_fingerprint_list():
|
||||||
# read all the folders in selfdrive/car and return a dict where:
|
# read all the folders in selfdrive/car and return a dict where:
|
||||||
# - keys are all the car models for which we have a fingerprint
|
# - keys are all the car models for which we have a fingerprint
|
||||||
# - values are lists dicts of messages that constitute the unique
|
# - values are lists dicts of messages that constitute the unique
|
||||||
# CAN fingerprint of each car model and all its variants
|
# CAN fingerprint of each car model and all its variants
|
||||||
fingerprints = {}
|
return get_attr_from_cars('FINGERPRINTS')
|
||||||
for car_folder in [x[0] for x in os.walk(BASEDIR + '/selfdrive/car')]:
|
|
||||||
try:
|
|
||||||
car_name = car_folder.split('/')[-1]
|
|
||||||
values = __import__('selfdrive.car.%s.values' % car_name, fromlist=['FINGERPRINTS'])
|
|
||||||
if hasattr(values, 'FINGERPRINTS'):
|
|
||||||
car_fingerprints = values.FINGERPRINTS
|
|
||||||
else:
|
|
||||||
continue
|
|
||||||
for f, v in car_fingerprints.items():
|
|
||||||
fingerprints[f] = v
|
|
||||||
except (ImportError, IOError):
|
|
||||||
pass
|
|
||||||
return fingerprints
|
|
||||||
|
|
||||||
|
|
||||||
|
FW_VERSIONS = get_fw_versions_list()
|
||||||
_FINGERPRINTS = get_fingerprint_list()
|
_FINGERPRINTS = get_fingerprint_list()
|
||||||
|
|
||||||
_DEBUG_ADDRESS = {1880: 8} # reserved for debug purposes
|
_DEBUG_ADDRESS = {1880: 8} # reserved for debug purposes
|
||||||
|
|
|
@ -34,13 +34,12 @@ class CarInterface(CarInterfaceBase):
|
||||||
return float(accel) / 3.0
|
return float(accel) / 3.0
|
||||||
|
|
||||||
@staticmethod
|
@staticmethod
|
||||||
def get_params(candidate, fingerprint=gen_empty_fingerprint(), vin="", has_relay=False):
|
def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False, car_fw=[]):
|
||||||
|
|
||||||
ret = car.CarParams.new_message()
|
ret = car.CarParams.new_message()
|
||||||
|
|
||||||
ret.carName = "ford"
|
ret.carName = "ford"
|
||||||
ret.carFingerprint = candidate
|
ret.carFingerprint = candidate
|
||||||
ret.carVin = vin
|
|
||||||
ret.isPandaBlack = has_relay
|
ret.isPandaBlack = has_relay
|
||||||
|
|
||||||
ret.safetyModel = car.CarParams.SafetyModel.ford
|
ret.safetyModel = car.CarParams.SafetyModel.ford
|
||||||
|
|
|
@ -0,0 +1,192 @@
|
||||||
|
#!/usr/bin/env python3
|
||||||
|
import traceback
|
||||||
|
import struct
|
||||||
|
from tqdm import tqdm
|
||||||
|
|
||||||
|
from selfdrive.car.isotp_parallel_query import IsoTpParallelQuery
|
||||||
|
from selfdrive.swaglog import cloudlog
|
||||||
|
from selfdrive.car.fingerprints import FW_VERSIONS
|
||||||
|
import panda.python.uds as uds
|
||||||
|
|
||||||
|
from cereal import car
|
||||||
|
Ecu = car.CarParams.Ecu
|
||||||
|
|
||||||
|
def p16(val):
|
||||||
|
return struct.pack("!H", val)
|
||||||
|
|
||||||
|
TESTER_PRESENT_REQUEST = bytes([uds.SERVICE_TYPE.TESTER_PRESENT, 0x0])
|
||||||
|
TESTER_PRESENT_RESPONSE = bytes([uds.SERVICE_TYPE.TESTER_PRESENT + 0x40, 0x0])
|
||||||
|
|
||||||
|
SHORT_TESTER_PRESENT_REQUEST = bytes([uds.SERVICE_TYPE.TESTER_PRESENT])
|
||||||
|
SHORT_TESTER_PRESENT_RESPONSE = bytes([uds.SERVICE_TYPE.TESTER_PRESENT + 0x40])
|
||||||
|
|
||||||
|
DEFAULT_DIAGNOSTIC_REQUEST = bytes([uds.SERVICE_TYPE.DIAGNOSTIC_SESSION_CONTROL,
|
||||||
|
uds.SESSION_TYPE.DEFAULT])
|
||||||
|
DEFAULT_DIAGNOSTIC_RESPONSE = bytes([uds.SERVICE_TYPE.DIAGNOSTIC_SESSION_CONTROL + 0x40,
|
||||||
|
uds.SESSION_TYPE.DEFAULT, 0x0, 0x32, 0x1, 0xf4])
|
||||||
|
|
||||||
|
EXTENDED_DIAGNOSTIC_REQUEST = bytes([uds.SERVICE_TYPE.DIAGNOSTIC_SESSION_CONTROL,
|
||||||
|
uds.SESSION_TYPE.EXTENDED_DIAGNOSTIC])
|
||||||
|
EXTENDED_DIAGNOSTIC_RESPONSE = bytes([uds.SERVICE_TYPE.DIAGNOSTIC_SESSION_CONTROL + 0x40,
|
||||||
|
uds.SESSION_TYPE.EXTENDED_DIAGNOSTIC, 0x0, 0x32, 0x1, 0xf4])
|
||||||
|
|
||||||
|
UDS_VERSION_REQUEST = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER]) + \
|
||||||
|
p16(uds.DATA_IDENTIFIER_TYPE.APPLICATION_SOFTWARE_IDENTIFICATION)
|
||||||
|
UDS_VERSION_RESPONSE = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER + 0x40]) + \
|
||||||
|
p16(uds.DATA_IDENTIFIER_TYPE.APPLICATION_SOFTWARE_IDENTIFICATION)
|
||||||
|
|
||||||
|
TOYOTA_VERSION_REQUEST = b'\x1a\x88\x01'
|
||||||
|
TOYOTA_VERSION_RESPONSE = b'\x5a\x88\x01'
|
||||||
|
|
||||||
|
OBD_VERSION_REQUEST = b'\x09\x04'
|
||||||
|
OBD_VERSION_RESPONSE = b'\x49\x04'
|
||||||
|
|
||||||
|
|
||||||
|
REQUESTS = [
|
||||||
|
# Honda
|
||||||
|
(
|
||||||
|
[UDS_VERSION_REQUEST],
|
||||||
|
[UDS_VERSION_RESPONSE]
|
||||||
|
),
|
||||||
|
# Toyota
|
||||||
|
(
|
||||||
|
[SHORT_TESTER_PRESENT_REQUEST, TOYOTA_VERSION_REQUEST],
|
||||||
|
[SHORT_TESTER_PRESENT_RESPONSE, TOYOTA_VERSION_RESPONSE]
|
||||||
|
),
|
||||||
|
(
|
||||||
|
[SHORT_TESTER_PRESENT_REQUEST, OBD_VERSION_REQUEST],
|
||||||
|
[SHORT_TESTER_PRESENT_RESPONSE, OBD_VERSION_RESPONSE]
|
||||||
|
),
|
||||||
|
(
|
||||||
|
[TESTER_PRESENT_REQUEST, DEFAULT_DIAGNOSTIC_REQUEST, EXTENDED_DIAGNOSTIC_REQUEST, UDS_VERSION_REQUEST],
|
||||||
|
[TESTER_PRESENT_RESPONSE, DEFAULT_DIAGNOSTIC_RESPONSE, EXTENDED_DIAGNOSTIC_RESPONSE, UDS_VERSION_RESPONSE]
|
||||||
|
)
|
||||||
|
]
|
||||||
|
|
||||||
|
def chunks(l, n=128):
|
||||||
|
for i in range(0, len(l), n):
|
||||||
|
yield l[i:i + n]
|
||||||
|
|
||||||
|
def match_fw_to_car(fw_versions):
|
||||||
|
candidates = FW_VERSIONS
|
||||||
|
invalid = []
|
||||||
|
|
||||||
|
for candidate, fws in candidates.items():
|
||||||
|
for ecu, expected_versions in fws.items():
|
||||||
|
ecu_type = ecu[0]
|
||||||
|
addr = ecu[1:]
|
||||||
|
|
||||||
|
found_version = fw_versions.get(addr, None)
|
||||||
|
|
||||||
|
# Allow DSU not being present
|
||||||
|
if ecu_type in [Ecu.unknown, Ecu.dsu] and found_version is None:
|
||||||
|
continue
|
||||||
|
|
||||||
|
if found_version not in expected_versions:
|
||||||
|
invalid.append(candidate)
|
||||||
|
break
|
||||||
|
|
||||||
|
return set(candidates.keys()) - set(invalid)
|
||||||
|
|
||||||
|
|
||||||
|
def get_fw_versions(logcan, sendcan, bus, extra=None, timeout=0.1, debug=False, progress=False):
|
||||||
|
ecu_types = {}
|
||||||
|
|
||||||
|
# Extract ECU adresses to query from fingerprints
|
||||||
|
# ECUs using a subadress need be queried one by one, the rest can be done in parallel
|
||||||
|
addrs = []
|
||||||
|
parallel_addrs = []
|
||||||
|
|
||||||
|
versions = FW_VERSIONS
|
||||||
|
if extra is not None:
|
||||||
|
versions.update(extra)
|
||||||
|
|
||||||
|
for c in versions.values():
|
||||||
|
for ecu_type, addr, sub_addr in c.keys():
|
||||||
|
a = (addr, sub_addr)
|
||||||
|
if a not in ecu_types:
|
||||||
|
ecu_types[a] = ecu_type
|
||||||
|
|
||||||
|
if sub_addr is None:
|
||||||
|
parallel_addrs.append(a)
|
||||||
|
else:
|
||||||
|
addrs.append([a])
|
||||||
|
addrs.insert(0, parallel_addrs)
|
||||||
|
|
||||||
|
fw_versions = {}
|
||||||
|
for i, addr in enumerate(tqdm(addrs, disable=not progress)):
|
||||||
|
for addr_chunk in chunks(addr):
|
||||||
|
for request, response in REQUESTS:
|
||||||
|
try:
|
||||||
|
query = IsoTpParallelQuery(sendcan, logcan, bus, addr_chunk, request, response, debug=debug)
|
||||||
|
t = 2 * timeout if i == 0 else timeout
|
||||||
|
fw_versions.update(query.get_data(t))
|
||||||
|
except Exception:
|
||||||
|
cloudlog.warning(f"FW query exception: {traceback.format_exc()}")
|
||||||
|
|
||||||
|
# Build capnp list to put into CarParams
|
||||||
|
car_fw = []
|
||||||
|
for addr, version in fw_versions.items():
|
||||||
|
f = car.CarParams.CarFw.new_message()
|
||||||
|
|
||||||
|
f.ecu = ecu_types[addr]
|
||||||
|
f.fwVersion = version
|
||||||
|
f.address = addr[0]
|
||||||
|
|
||||||
|
if addr[1] is not None:
|
||||||
|
f.subAddress = addr[1]
|
||||||
|
|
||||||
|
car_fw.append(f)
|
||||||
|
|
||||||
|
candidates = match_fw_to_car(fw_versions)
|
||||||
|
return candidates, car_fw
|
||||||
|
|
||||||
|
|
||||||
|
if __name__ == "__main__":
|
||||||
|
import time
|
||||||
|
import argparse
|
||||||
|
import cereal.messaging as messaging
|
||||||
|
from selfdrive.car.vin import get_vin
|
||||||
|
|
||||||
|
|
||||||
|
parser = argparse.ArgumentParser(description='Get firmware version of ECUs')
|
||||||
|
parser.add_argument('--scan', action='store_true')
|
||||||
|
parser.add_argument('--debug', action='store_true')
|
||||||
|
args = parser.parse_args()
|
||||||
|
|
||||||
|
logcan = messaging.sub_sock('can')
|
||||||
|
sendcan = messaging.pub_sock('sendcan')
|
||||||
|
|
||||||
|
extra = None
|
||||||
|
if args.scan:
|
||||||
|
extra = {"DEBUG": {}}
|
||||||
|
# Honda
|
||||||
|
for i in range(256):
|
||||||
|
extra["DEBUG"][(Ecu.unknown, 0x18da00f1 + (i << 8), None)] = []
|
||||||
|
extra["DEBUG"][(Ecu.unknown, 0x700 + i, None)] = []
|
||||||
|
extra["DEBUG"][(Ecu.unknown, 0x750, i)] = []
|
||||||
|
|
||||||
|
time.sleep(1.)
|
||||||
|
|
||||||
|
t = time.time()
|
||||||
|
print("Getting vin...")
|
||||||
|
addr, vin = get_vin(logcan, sendcan, 1, retry=10, debug=args.debug)
|
||||||
|
print(f"VIN: {vin}")
|
||||||
|
print("Getting VIN took %.3f s" % (time.time() - t))
|
||||||
|
print()
|
||||||
|
|
||||||
|
t = time.time()
|
||||||
|
candidates, fw_vers = get_fw_versions(logcan, sendcan, 1, extra=extra, debug=args.debug, progress=True)
|
||||||
|
|
||||||
|
print()
|
||||||
|
print("Found FW versions")
|
||||||
|
print("{")
|
||||||
|
for version in fw_vers:
|
||||||
|
subaddr = None if version.subAddress == 0 else hex(version.subAddress)
|
||||||
|
print(f" (Ecu.{version.ecu}, {hex(version.address)}, {subaddr}): [{version.fwVersion}]")
|
||||||
|
print("}")
|
||||||
|
|
||||||
|
|
||||||
|
print()
|
||||||
|
print("Possible matches:", candidates)
|
||||||
|
print("Getting fw took %.3f s" % (time.time() - t))
|
|
@ -43,12 +43,11 @@ class CarInterface(CarInterfaceBase):
|
||||||
return float(accel) / 4.0
|
return float(accel) / 4.0
|
||||||
|
|
||||||
@staticmethod
|
@staticmethod
|
||||||
def get_params(candidate, fingerprint=gen_empty_fingerprint(), vin="", has_relay=False):
|
def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False, car_fw=[]):
|
||||||
ret = car.CarParams.new_message()
|
ret = car.CarParams.new_message()
|
||||||
|
|
||||||
ret.carName = "gm"
|
ret.carName = "gm"
|
||||||
ret.carFingerprint = candidate
|
ret.carFingerprint = candidate
|
||||||
ret.carVin = vin
|
|
||||||
ret.isPandaBlack = has_relay
|
ret.isPandaBlack = has_relay
|
||||||
|
|
||||||
ret.enableCruise = False
|
ret.enableCruise = False
|
||||||
|
|
|
@ -1,12 +1,14 @@
|
||||||
from collections import namedtuple
|
from collections import namedtuple
|
||||||
|
from cereal import car
|
||||||
from common.realtime import DT_CTRL
|
from common.realtime import DT_CTRL
|
||||||
from selfdrive.controls.lib.drive_helpers import rate_limit
|
from selfdrive.controls.lib.drive_helpers import rate_limit
|
||||||
from common.numpy_fast import clip
|
from common.numpy_fast import clip
|
||||||
from selfdrive.car import create_gas_command
|
from selfdrive.car import create_gas_command
|
||||||
from selfdrive.car.honda import hondacan
|
from selfdrive.car.honda import hondacan
|
||||||
from selfdrive.car.honda.values import AH, CruiseButtons, CAR
|
from selfdrive.car.honda.values import CruiseButtons, CAR, VISUAL_HUD
|
||||||
from opendbc.can.packer import CANPacker
|
from opendbc.can.packer import CANPacker
|
||||||
|
|
||||||
|
VisualAlert = car.CarControl.HUDControl.VisualAlert
|
||||||
|
|
||||||
def actuator_hystereses(brake, braking, brake_steady, v_ego, car_fingerprint):
|
def actuator_hystereses(brake, braking, brake_steady, v_ego, car_fingerprint):
|
||||||
# hyst params
|
# hyst params
|
||||||
|
@ -56,25 +58,25 @@ def process_hud_alert(hud_alert):
|
||||||
fcw_display = 0
|
fcw_display = 0
|
||||||
steer_required = 0
|
steer_required = 0
|
||||||
acc_alert = 0
|
acc_alert = 0
|
||||||
if hud_alert == AH.NONE: # no alert
|
|
||||||
pass
|
# priority is: FCW, steer required, all others
|
||||||
elif hud_alert == AH.FCW: # FCW
|
if hud_alert == VisualAlert.fcw:
|
||||||
fcw_display = hud_alert[1]
|
fcw_display = VISUAL_HUD[hud_alert.raw]
|
||||||
elif hud_alert == AH.STEER: # STEER
|
elif hud_alert == VisualAlert.steerRequired:
|
||||||
steer_required = hud_alert[1]
|
steer_required = VISUAL_HUD[hud_alert.raw]
|
||||||
else: # any other ACC alert
|
else:
|
||||||
acc_alert = hud_alert[1]
|
acc_alert = VISUAL_HUD[hud_alert.raw]
|
||||||
|
|
||||||
return fcw_display, steer_required, acc_alert
|
return fcw_display, steer_required, acc_alert
|
||||||
|
|
||||||
|
|
||||||
HUDData = namedtuple("HUDData",
|
HUDData = namedtuple("HUDData",
|
||||||
["pcm_accel", "v_cruise", "mini_car", "car", "X4",
|
["pcm_accel", "v_cruise", "car",
|
||||||
"lanes", "fcw", "acc_alert", "steer_required"])
|
"lanes", "fcw", "acc_alert", "steer_required"])
|
||||||
|
|
||||||
|
|
||||||
class CarController():
|
class CarController():
|
||||||
def __init__(self, dbc_name):
|
def __init__(self, dbc_name, CP):
|
||||||
self.braking = False
|
self.braking = False
|
||||||
self.brake_steady = 0.
|
self.brake_steady = 0.
|
||||||
self.brake_last = 0.
|
self.brake_last = 0.
|
||||||
|
@ -82,6 +84,11 @@ class CarController():
|
||||||
self.last_pump_ts = 0.
|
self.last_pump_ts = 0.
|
||||||
self.packer = CANPacker(dbc_name)
|
self.packer = CANPacker(dbc_name)
|
||||||
self.new_radar_config = False
|
self.new_radar_config = False
|
||||||
|
self.eps_modified = False
|
||||||
|
for fw in CP.carFw:
|
||||||
|
if fw.ecu == "eps" and b"," in fw.fwVersion:
|
||||||
|
print("EPS FW MODIFIED!")
|
||||||
|
self.eps_modified = True
|
||||||
|
|
||||||
def update(self, enabled, CS, frame, actuators, \
|
def update(self, enabled, CS, frame, actuators, \
|
||||||
pcm_speed, pcm_override, pcm_cancel_cmd, pcm_accel, \
|
pcm_speed, pcm_override, pcm_cancel_cmd, pcm_accel, \
|
||||||
|
@ -96,7 +103,7 @@ class CarController():
|
||||||
pcm_cancel_cmd = True
|
pcm_cancel_cmd = True
|
||||||
|
|
||||||
# *** rate limit after the enable check ***
|
# *** rate limit after the enable check ***
|
||||||
self.brake_last = rate_limit(brake, self.brake_last, -2., 1./100)
|
self.brake_last = rate_limit(brake, self.brake_last, -2., DT_CTRL)
|
||||||
|
|
||||||
# vehicle hud display, wait for one update from 10Hz 0x304 msg
|
# vehicle hud display, wait for one update from 10Hz 0x304 msg
|
||||||
if hud_show_lanes:
|
if hud_show_lanes:
|
||||||
|
@ -114,8 +121,8 @@ class CarController():
|
||||||
|
|
||||||
fcw_display, steer_required, acc_alert = process_hud_alert(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,
|
hud = HUDData(int(pcm_accel), int(round(hud_v_cruise)), hud_car,
|
||||||
0xc1, hud_lanes, fcw_display, acc_alert, steer_required)
|
hud_lanes, fcw_display, acc_alert, steer_required)
|
||||||
|
|
||||||
# **** process the car messages ****
|
# **** process the car messages ****
|
||||||
|
|
||||||
|
@ -124,9 +131,11 @@ class CarController():
|
||||||
if CS.CP.carFingerprint in (CAR.ACURA_ILX):
|
if CS.CP.carFingerprint in (CAR.ACURA_ILX):
|
||||||
STEER_MAX = 0xF00
|
STEER_MAX = 0xF00
|
||||||
elif CS.CP.carFingerprint in (CAR.CRV, CAR.ACURA_RDX):
|
elif CS.CP.carFingerprint in (CAR.CRV, CAR.ACURA_RDX):
|
||||||
STEER_MAX = 0x3e8 # CR-V only uses 12-bits and requires a lower value (max value from energee)
|
STEER_MAX = 0x3e8 # CR-V only uses 12-bits and requires a lower value
|
||||||
elif CS.CP.carFingerprint in (CAR.ODYSSEY_CHN):
|
elif CS.CP.carFingerprint in (CAR.ODYSSEY_CHN):
|
||||||
STEER_MAX = 0x7FFF
|
STEER_MAX = 0x7FFF
|
||||||
|
elif CS.CP.carFingerprint in (CAR.CIVIC) and self.eps_modified:
|
||||||
|
STEER_MAX = 0x1400
|
||||||
else:
|
else:
|
||||||
STEER_MAX = 0x1000
|
STEER_MAX = 0x1000
|
||||||
|
|
||||||
|
@ -135,6 +144,12 @@ class CarController():
|
||||||
apply_brake = int(clip(self.brake_last * BRAKE_MAX, 0, BRAKE_MAX - 1))
|
apply_brake = int(clip(self.brake_last * BRAKE_MAX, 0, BRAKE_MAX - 1))
|
||||||
apply_steer = int(clip(-actuators.steer * STEER_MAX, -STEER_MAX, STEER_MAX))
|
apply_steer = int(clip(-actuators.steer * STEER_MAX, -STEER_MAX, STEER_MAX))
|
||||||
|
|
||||||
|
if CS.CP.carFingerprint in (CAR.CIVIC) and self.eps_modified:
|
||||||
|
if apply_steer > 0xA00:
|
||||||
|
apply_steer = (apply_steer - 0xA00) / 2 + 0xA00
|
||||||
|
elif apply_steer < -0xA00:
|
||||||
|
apply_steer = (apply_steer + 0xA00) / 2 - 0xA00
|
||||||
|
|
||||||
lkas_active = enabled and not CS.steer_not_allowed
|
lkas_active = enabled and not CS.steer_not_allowed
|
||||||
|
|
||||||
# Send CAN commands.
|
# Send CAN commands.
|
||||||
|
|
|
@ -41,8 +41,8 @@ def get_can_signals(CP):
|
||||||
("WHEEL_SPEED_RR", "WHEEL_SPEEDS", 0),
|
("WHEEL_SPEED_RR", "WHEEL_SPEEDS", 0),
|
||||||
("STEER_ANGLE", "STEERING_SENSORS", 0),
|
("STEER_ANGLE", "STEERING_SENSORS", 0),
|
||||||
("STEER_ANGLE_RATE", "STEERING_SENSORS", 0),
|
("STEER_ANGLE_RATE", "STEERING_SENSORS", 0),
|
||||||
|
("MOTOR_TORQUE", "STEER_MOTOR_TORQUE", 0),
|
||||||
("STEER_TORQUE_SENSOR", "STEER_STATUS", 0),
|
("STEER_TORQUE_SENSOR", "STEER_STATUS", 0),
|
||||||
("STEER_TORQUE_MOTOR", "STEER_STATUS", 0),
|
|
||||||
("LEFT_BLINKER", "SCM_FEEDBACK", 0),
|
("LEFT_BLINKER", "SCM_FEEDBACK", 0),
|
||||||
("RIGHT_BLINKER", "SCM_FEEDBACK", 0),
|
("RIGHT_BLINKER", "SCM_FEEDBACK", 0),
|
||||||
("GEAR", "GEARBOX", 0),
|
("GEAR", "GEARBOX", 0),
|
||||||
|
@ -325,7 +325,7 @@ class CarState():
|
||||||
self.car_gas = cp.vl["GAS_PEDAL_2"]['CAR_GAS']
|
self.car_gas = cp.vl["GAS_PEDAL_2"]['CAR_GAS']
|
||||||
|
|
||||||
self.steer_torque_driver = cp.vl["STEER_STATUS"]['STEER_TORQUE_SENSOR']
|
self.steer_torque_driver = cp.vl["STEER_STATUS"]['STEER_TORQUE_SENSOR']
|
||||||
self.steer_torque_motor = cp.vl["STEER_STATUS"]['STEER_TORQUE_MOTOR']
|
self.steer_torque_motor = cp.vl["STEER_MOTOR_TORQUE"]['MOTOR_TORQUE']
|
||||||
self.steer_override = abs(self.steer_torque_driver) > STEER_THRESHOLD[self.CP.carFingerprint]
|
self.steer_override = abs(self.steer_torque_driver) > STEER_THRESHOLD[self.CP.carFingerprint]
|
||||||
|
|
||||||
self.brake_switch = cp.vl["POWERTRAIN_DATA"]['BRAKE_SWITCH']
|
self.brake_switch = cp.vl["POWERTRAIN_DATA"]['BRAKE_SWITCH']
|
||||||
|
|
|
@ -54,7 +54,7 @@ def create_ui_commands(packer, pcm_speed, hud, car_fingerprint, is_metric, idx,
|
||||||
'PCM_SPEED': pcm_speed * CV.MS_TO_KPH,
|
'PCM_SPEED': pcm_speed * CV.MS_TO_KPH,
|
||||||
'PCM_GAS': hud.pcm_accel,
|
'PCM_GAS': hud.pcm_accel,
|
||||||
'CRUISE_SPEED': hud.v_cruise,
|
'CRUISE_SPEED': hud.v_cruise,
|
||||||
'ENABLE_MINI_CAR': hud.mini_car,
|
'ENABLE_MINI_CAR': 1,
|
||||||
'HUD_LEAD': hud.car,
|
'HUD_LEAD': hud.car,
|
||||||
'HUD_DISTANCE': 3, # max distance setting on display
|
'HUD_DISTANCE': 3, # max distance setting on display
|
||||||
'IMPERIAL_UNIT': int(not is_metric),
|
'IMPERIAL_UNIT': int(not is_metric),
|
||||||
|
|
|
@ -8,12 +8,12 @@ from selfdrive.config import Conversions as CV
|
||||||
from selfdrive.controls.lib.drive_helpers import create_event, EventTypes as ET, get_events
|
from selfdrive.controls.lib.drive_helpers import create_event, EventTypes as ET, get_events
|
||||||
from selfdrive.controls.lib.vehicle_model import VehicleModel
|
from selfdrive.controls.lib.vehicle_model import VehicleModel
|
||||||
from selfdrive.car.honda.carstate import CarState, get_can_parser, get_cam_can_parser
|
from selfdrive.car.honda.carstate import CarState, get_can_parser, get_cam_can_parser
|
||||||
from selfdrive.car.honda.values import CruiseButtons, CAR, HONDA_BOSCH, VISUAL_HUD, ECU, ECU_FINGERPRINT, FINGERPRINTS
|
from selfdrive.car.honda.values import CruiseButtons, CAR, HONDA_BOSCH, ECU, ECU_FINGERPRINT, FINGERPRINTS
|
||||||
from selfdrive.car import STD_CARGO_KG, CivicParams, scale_rot_inertia, scale_tire_stiffness, is_ecu_disconnected, gen_empty_fingerprint
|
from selfdrive.car import STD_CARGO_KG, CivicParams, scale_rot_inertia, scale_tire_stiffness, is_ecu_disconnected, gen_empty_fingerprint
|
||||||
from selfdrive.controls.lib.planner import _A_CRUISE_MAX_V
|
from selfdrive.controls.lib.planner import _A_CRUISE_MAX_V_FOLLOWING
|
||||||
from selfdrive.car.interfaces import CarInterfaceBase
|
from selfdrive.car.interfaces import CarInterfaceBase
|
||||||
|
|
||||||
A_ACC_MAX = max(_A_CRUISE_MAX_V)
|
A_ACC_MAX = max(_A_CRUISE_MAX_V_FOLLOWING)
|
||||||
|
|
||||||
ButtonType = car.CarState.ButtonEvent.Type
|
ButtonType = car.CarState.ButtonEvent.Type
|
||||||
GearShifter = car.CarState.GearShifter
|
GearShifter = car.CarState.GearShifter
|
||||||
|
@ -91,7 +91,7 @@ class CarInterface(CarInterfaceBase):
|
||||||
|
|
||||||
self.CC = None
|
self.CC = None
|
||||||
if CarController is not None:
|
if CarController is not None:
|
||||||
self.CC = CarController(self.cp.dbc_name)
|
self.CC = CarController(self.cp.dbc_name, CP)
|
||||||
|
|
||||||
if self.CS.CP.carFingerprint == CAR.ACURA_ILX:
|
if self.CS.CP.carFingerprint == CAR.ACURA_ILX:
|
||||||
self.compute_gb = get_compute_gb_acura()
|
self.compute_gb = get_compute_gb_acura()
|
||||||
|
@ -131,22 +131,21 @@ class CarInterface(CarInterfaceBase):
|
||||||
return float(max(max_accel, a_target / A_ACC_MAX)) * min(speedLimiter, accelLimiter)
|
return float(max(max_accel, a_target / A_ACC_MAX)) * min(speedLimiter, accelLimiter)
|
||||||
|
|
||||||
@staticmethod
|
@staticmethod
|
||||||
def get_params(candidate, fingerprint=gen_empty_fingerprint(), vin="", has_relay=False):
|
def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False, car_fw=[]):
|
||||||
|
|
||||||
ret = car.CarParams.new_message()
|
ret = car.CarParams.new_message()
|
||||||
ret.carName = "honda"
|
ret.carName = "honda"
|
||||||
ret.carFingerprint = candidate
|
ret.carFingerprint = candidate
|
||||||
ret.carVin = vin
|
|
||||||
ret.isPandaBlack = has_relay
|
ret.isPandaBlack = has_relay
|
||||||
|
|
||||||
if candidate in HONDA_BOSCH:
|
if candidate in HONDA_BOSCH:
|
||||||
ret.safetyModel = car.CarParams.SafetyModel.hondaBosch
|
ret.safetyModel = car.CarParams.SafetyModel.hondaBoschHarness if has_relay else car.CarParams.SafetyModel.hondaBoschGiraffe
|
||||||
rdr_bus = 0 if has_relay else 2
|
rdr_bus = 0 if has_relay else 2
|
||||||
ret.enableCamera = is_ecu_disconnected(fingerprint[rdr_bus], FINGERPRINTS, ECU_FINGERPRINT, candidate, ECU.CAM) or has_relay
|
ret.enableCamera = is_ecu_disconnected(fingerprint[rdr_bus], FINGERPRINTS, ECU_FINGERPRINT, candidate, ECU.CAM) or has_relay
|
||||||
ret.radarOffCan = True
|
ret.radarOffCan = True
|
||||||
ret.openpilotLongitudinalControl = False
|
ret.openpilotLongitudinalControl = False
|
||||||
else:
|
else:
|
||||||
ret.safetyModel = car.CarParams.SafetyModel.honda
|
ret.safetyModel = car.CarParams.SafetyModel.hondaNidec
|
||||||
ret.enableCamera = is_ecu_disconnected(fingerprint[0], FINGERPRINTS, ECU_FINGERPRINT, candidate, ECU.CAM) or has_relay
|
ret.enableCamera = is_ecu_disconnected(fingerprint[0], FINGERPRINTS, ECU_FINGERPRINT, candidate, ECU.CAM) or has_relay
|
||||||
ret.enableGasInterceptor = 0x201 in fingerprint[0]
|
ret.enableGasInterceptor = 0x201 in fingerprint[0]
|
||||||
ret.openpilotLongitudinalControl = ret.enableCamera
|
ret.openpilotLongitudinalControl = ret.enableCamera
|
||||||
|
@ -165,6 +164,11 @@ class CarInterface(CarInterfaceBase):
|
||||||
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]]
|
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]]
|
||||||
ret.lateralTuning.pid.kf = 0.00006 # conservative feed-forward
|
ret.lateralTuning.pid.kf = 0.00006 # conservative feed-forward
|
||||||
|
|
||||||
|
eps_modified = False
|
||||||
|
for fw in car_fw:
|
||||||
|
if fw.ecu == "eps" and b"," in fw.fwVersion:
|
||||||
|
eps_modified = True
|
||||||
|
|
||||||
if candidate in [CAR.CIVIC, CAR.CIVIC_BOSCH]:
|
if candidate in [CAR.CIVIC, CAR.CIVIC_BOSCH]:
|
||||||
stop_and_go = True
|
stop_and_go = True
|
||||||
ret.mass = CivicParams.MASS
|
ret.mass = CivicParams.MASS
|
||||||
|
@ -173,7 +177,8 @@ class CarInterface(CarInterfaceBase):
|
||||||
ret.steerRatio = 15.38 # 10.93 is end-to-end spec
|
ret.steerRatio = 15.38 # 10.93 is end-to-end spec
|
||||||
tire_stiffness_factor = 1.
|
tire_stiffness_factor = 1.
|
||||||
|
|
||||||
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.8], [0.24]]
|
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.4], [0.12]] if eps_modified else [[0.8], [0.24]]
|
||||||
|
ret.lateralTuning.pid.kf = 0.00006
|
||||||
ret.longitudinalTuning.kpBP = [0., 5., 35.]
|
ret.longitudinalTuning.kpBP = [0., 5., 35.]
|
||||||
ret.longitudinalTuning.kpV = [3.6, 2.4, 1.5]
|
ret.longitudinalTuning.kpV = [3.6, 2.4, 1.5]
|
||||||
ret.longitudinalTuning.kiBP = [0., 35.]
|
ret.longitudinalTuning.kiBP = [0., 35.]
|
||||||
|
@ -575,8 +580,6 @@ class CarInterface(CarInterfaceBase):
|
||||||
else:
|
else:
|
||||||
hud_v_cruise = 255
|
hud_v_cruise = 255
|
||||||
|
|
||||||
hud_alert = VISUAL_HUD[c.hudControl.visualAlert.raw]
|
|
||||||
|
|
||||||
pcm_accel = int(clip(c.cruiseControl.accelOverride, 0, 1) * 0xc6)
|
pcm_accel = int(clip(c.cruiseControl.accelOverride, 0, 1) * 0xc6)
|
||||||
|
|
||||||
can_sends = self.CC.update(c.enabled, self.CS, self.frame,
|
can_sends = self.CC.update(c.enabled, self.CS, self.frame,
|
||||||
|
@ -588,7 +591,7 @@ class CarInterface(CarInterfaceBase):
|
||||||
hud_v_cruise,
|
hud_v_cruise,
|
||||||
c.hudControl.lanesVisible,
|
c.hudControl.lanesVisible,
|
||||||
hud_show_car=c.hudControl.leadVisible,
|
hud_show_car=c.hudControl.leadVisible,
|
||||||
hud_alert=hud_alert)
|
hud_alert=c.hudControl.visualAlert)
|
||||||
|
|
||||||
self.frame += 1
|
self.frame += 1
|
||||||
return can_sends
|
return can_sends
|
||||||
|
|
|
@ -1,6 +1,7 @@
|
||||||
from cereal import car
|
from cereal import car
|
||||||
from selfdrive.car import dbc_dict
|
from selfdrive.car import dbc_dict
|
||||||
|
|
||||||
|
Ecu = car.CarParams.Ecu
|
||||||
VisualAlert = car.CarControl.HUDControl.VisualAlert
|
VisualAlert = car.CarControl.HUDControl.VisualAlert
|
||||||
|
|
||||||
# Car button codes
|
# Car button codes
|
||||||
|
@ -10,28 +11,18 @@ class CruiseButtons:
|
||||||
CANCEL = 2
|
CANCEL = 2
|
||||||
MAIN = 1
|
MAIN = 1
|
||||||
|
|
||||||
class AH:
|
# See dbc files for info on values"
|
||||||
#[alert_idx, value]
|
|
||||||
# See dbc files for info on values"
|
|
||||||
NONE = [0, 0]
|
|
||||||
FCW = [1, 1]
|
|
||||||
STEER = [2, 1]
|
|
||||||
BRAKE_PRESSED = [3, 10]
|
|
||||||
GEAR_NOT_D = [4, 6]
|
|
||||||
SEATBELT = [5, 5]
|
|
||||||
SPEED_TOO_HIGH = [6, 8]
|
|
||||||
|
|
||||||
VISUAL_HUD = {
|
VISUAL_HUD = {
|
||||||
VisualAlert.none: AH.NONE,
|
VisualAlert.none: 0,
|
||||||
VisualAlert.fcw: AH.FCW,
|
VisualAlert.fcw: 1,
|
||||||
VisualAlert.steerRequired: AH.STEER,
|
VisualAlert.steerRequired: 1,
|
||||||
VisualAlert.brakePressed: AH.BRAKE_PRESSED,
|
VisualAlert.brakePressed: 10,
|
||||||
VisualAlert.wrongGear: AH.GEAR_NOT_D,
|
VisualAlert.wrongGear: 6,
|
||||||
VisualAlert.seatbeltUnbuckled: AH.SEATBELT,
|
VisualAlert.seatbeltUnbuckled: 5,
|
||||||
VisualAlert.speedTooHigh: AH.SPEED_TOO_HIGH}
|
VisualAlert.speedTooHigh: 8}
|
||||||
|
|
||||||
class ECU:
|
class ECU:
|
||||||
CAM = 0
|
CAM = Ecu.fwdCamera
|
||||||
|
|
||||||
class CAR:
|
class CAR:
|
||||||
ACCORD = "HONDA ACCORD 2018 SPORT 2T"
|
ACCORD = "HONDA ACCORD 2018 SPORT 2T"
|
||||||
|
@ -81,9 +72,8 @@ FINGERPRINTS = {
|
||||||
CAR.CRV: [{
|
CAR.CRV: [{
|
||||||
57: 3, 145: 8, 316: 8, 340: 8, 342: 6, 344: 8, 380: 8, 398: 3, 399: 6, 401: 8, 404: 4, 420: 8, 422: 8, 426: 8, 432: 7, 464: 8, 474: 5, 476: 4, 487: 4, 490: 8, 493: 3, 506: 8, 507: 1, 512: 6, 513: 6, 542: 7, 545: 4, 597: 8, 660: 8, 661: 4, 773: 7, 777: 8, 780: 8, 800: 8, 804: 8, 808: 8, 829: 5, 882: 2, 884: 7, 888: 8, 891: 8, 892: 8, 923: 2, 929: 8, 983: 8, 985: 3, 1024: 5, 1027: 5, 1029: 8, 1033: 5, 1036: 8, 1039: 8, 1057: 5, 1064: 7, 1108: 8, 1125: 8, 1296: 8, 1365: 5, 1424: 5, 1600: 5, 1601: 8,
|
57: 3, 145: 8, 316: 8, 340: 8, 342: 6, 344: 8, 380: 8, 398: 3, 399: 6, 401: 8, 404: 4, 420: 8, 422: 8, 426: 8, 432: 7, 464: 8, 474: 5, 476: 4, 487: 4, 490: 8, 493: 3, 506: 8, 507: 1, 512: 6, 513: 6, 542: 7, 545: 4, 597: 8, 660: 8, 661: 4, 773: 7, 777: 8, 780: 8, 800: 8, 804: 8, 808: 8, 829: 5, 882: 2, 884: 7, 888: 8, 891: 8, 892: 8, 923: 2, 929: 8, 983: 8, 985: 3, 1024: 5, 1027: 5, 1029: 8, 1033: 5, 1036: 8, 1039: 8, 1057: 5, 1064: 7, 1108: 8, 1125: 8, 1296: 8, 1365: 5, 1424: 5, 1600: 5, 1601: 8,
|
||||||
}],
|
}],
|
||||||
# msg 1115 has seen with len 2 and 4, so ignore it
|
|
||||||
CAR.CRV_5G: [{
|
CAR.CRV_5G: [{
|
||||||
57: 3, 148: 8, 199: 4, 228: 5, 231: 5, 232: 7, 304: 8, 330: 8, 340: 8, 344: 8, 380: 8, 399: 7, 401: 8, 420: 8, 423: 2, 427: 3, 428: 8, 432: 7, 441: 5, 446: 3, 450: 8, 464: 8, 467: 2, 469: 3, 470: 2, 474: 8, 476: 7, 477: 8, 479: 8, 490: 8, 493: 5, 495: 8, 507: 1, 545: 6, 597: 8, 661: 4, 662: 4, 773: 7, 777: 8, 780: 8, 795: 8, 800: 8, 804: 8, 806: 8, 808: 8, 814: 4, 815: 8, 817: 4, 825: 4, 829: 5, 862: 8, 881: 8, 882: 4, 884: 8, 888: 8, 891: 8, 927: 8, 918: 7, 929: 8, 983: 8, 985: 3, 1024: 5, 1027: 5, 1029: 8, 1036: 8, 1039: 8, 1064: 7, 1108: 8, 1092: 1, 1125: 8, 1127: 2, 1296: 8, 1302: 8, 1322: 5, 1361: 5, 1365: 5, 1424: 5, 1600: 5, 1601: 8, 1618: 5, 1633: 8, 1670: 5
|
57: 3, 148: 8, 199: 4, 228: 5, 231: 5, 232: 7, 304: 8, 330: 8, 340: 8, 344: 8, 380: 8, 399: 7, 401: 8, 420: 8, 423: 2, 427: 3, 428: 8, 432: 7, 441: 5, 446: 3, 450: 8, 464: 8, 467: 2, 469: 3, 470: 2, 474: 8, 476: 7, 477: 8, 479: 8, 490: 8, 493: 5, 495: 8, 507: 1, 545: 6, 597: 8, 661: 4, 662: 4, 773: 7, 777: 8, 780: 8, 795: 8, 800: 8, 804: 8, 806: 8, 808: 8, 814: 4, 815: 8, 817: 4, 825: 4, 829: 5, 862: 8, 881: 8, 882: 4, 884: 8, 888: 8, 891: 8, 927: 8, 918: 7, 929: 8, 983: 8, 985: 3, 1024: 5, 1027: 5, 1029: 8, 1036: 8, 1039: 8, 1064: 7, 1108: 8, 1092: 1, 1115: 2, 1125: 8, 1127: 2, 1296: 8, 1302: 8, 1322: 5, 1361: 5, 1365: 5, 1424: 5, 1600: 5, 1601: 8, 1618: 5, 1633: 8, 1670: 5
|
||||||
}],
|
}],
|
||||||
CAR.CRV_HYBRID: [{
|
CAR.CRV_HYBRID: [{
|
||||||
57: 3, 148: 8, 228: 5, 304: 8, 330: 8, 344: 8, 380: 8, 387: 8, 388: 8, 399: 7, 408: 6, 415: 6, 419: 8, 420: 8, 427: 3, 428: 8, 432: 7, 441: 5, 450: 8, 464: 8, 477: 8, 479: 8, 490: 8, 495: 8, 525: 8, 531: 8, 545: 6, 662: 4, 773: 7, 777: 8, 780: 8, 804: 8, 806: 8, 808: 8, 814: 4, 829: 5, 833: 6, 862: 8, 884: 8, 891: 8, 927: 8, 929: 8, 930: 8, 931: 8, 1302: 8, 1361: 5, 1365: 5, 1600: 5, 1601: 8, 1626: 5, 1627: 5
|
57: 3, 148: 8, 228: 5, 304: 8, 330: 8, 344: 8, 380: 8, 387: 8, 388: 8, 399: 7, 408: 6, 415: 6, 419: 8, 420: 8, 427: 3, 428: 8, 432: 7, 441: 5, 450: 8, 464: 8, 477: 8, 479: 8, 490: 8, 495: 8, 525: 8, 531: 8, 545: 6, 662: 4, 773: 7, 777: 8, 780: 8, 804: 8, 806: 8, 808: 8, 814: 4, 829: 5, 833: 6, 862: 8, 884: 8, 891: 8, 927: 8, 929: 8, 930: 8, 931: 8, 1302: 8, 1361: 5, 1365: 5, 1600: 5, 1601: 8, 1626: 5, 1627: 5
|
||||||
|
@ -130,6 +120,36 @@ for c in FINGERPRINTS:
|
||||||
for d in DIAG_MSGS:
|
for d in DIAG_MSGS:
|
||||||
FINGERPRINTS[c][f][d] = DIAG_MSGS[d]
|
FINGERPRINTS[c][f][d] = DIAG_MSGS[d]
|
||||||
|
|
||||||
|
# TODO: Figure out what is relevant
|
||||||
|
FW_VERSIONS = {
|
||||||
|
CAR.CIVIC: {
|
||||||
|
(Ecu.unknown, 0x18da10f1, None): [b'37805-5AA-L660\x00\x00'],
|
||||||
|
(Ecu.unknown, 0x18da1ef1, None): [b'28101-5CG-A050\x00\x00'],
|
||||||
|
(Ecu.unknown, 0x18da28f1, None): [b'57114-TBA-A550\x00\x00'],
|
||||||
|
(Ecu.eps, 0x18da30f1, None): [b'39990-TBA-A030\x00\x00', b'39990-TBA,A030\x00\x00'],
|
||||||
|
(Ecu.unknown, 0x18da53f1, None): [b'77959-TBA-A030\x00\x00'],
|
||||||
|
(Ecu.unknown, 0x18da60f1, None): [b'78109-TBC-A310\x00\x00'],
|
||||||
|
(Ecu.unknown, 0x18dab0f1, None): [b'36161-TBC-A030\x00\x00'],
|
||||||
|
(Ecu.unknown, 0x18daeff1, None): [b'38897-TBA-A020\x00\x00'],
|
||||||
|
|
||||||
|
},
|
||||||
|
CAR.ACCORD: {
|
||||||
|
(Ecu.unknown, 0x18da10f1, None): [b'37805-6B2-A650\x00\x00'],
|
||||||
|
(Ecu.unknown, 0x18da0bf1, None): [b'54008-TVC-A910\x00\x00'],
|
||||||
|
(Ecu.unknown, 0x18da1ef1, None): [b'28102-6B8-A560\x00\x00'],
|
||||||
|
(Ecu.unknown, 0x18da2bf1, None): [b'46114-TVA-A060\x00\x00'],
|
||||||
|
(Ecu.unknown, 0x18da28f1, None): [b'57114-TVA-C050\x00\x00'],
|
||||||
|
(Ecu.eps, 0x18da30f1, None): [b'39990-TVA-A150\x00\x00'],
|
||||||
|
(Ecu.unknown, 0x18da3af1, None): [b'39390-TVA-A020\x00\x00'],
|
||||||
|
(Ecu.unknown, 0x18da53f1, None): [b'77959-TVA-A460\x00\x00'],
|
||||||
|
(Ecu.unknown, 0x18da60f1, None): [b'78109-TVC-A210\x00\x00'],
|
||||||
|
(Ecu.unknown, 0x18da61f1, None): [b'78209-TVA-A010\x00\x00'],
|
||||||
|
(Ecu.unknown, 0x18dab0f1, None): [b'36802-TVA-A160\x00\x00'],
|
||||||
|
(Ecu.unknown, 0x18dab5f1, None): [b'36161-TVA-A060\x00\x00'],
|
||||||
|
(Ecu.unknown, 0x18daeff1, None): [b'38897-TVA-A010\x00\x00'],
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
DBC = {
|
DBC = {
|
||||||
CAR.ACCORD: dbc_dict('honda_accord_s2t_2018_can_generated', None),
|
CAR.ACCORD: dbc_dict('honda_accord_s2t_2018_can_generated', None),
|
||||||
CAR.ACCORD_15: dbc_dict('honda_accord_lx15t_2018_can_generated', None),
|
CAR.ACCORD_15: dbc_dict('honda_accord_lx15t_2018_can_generated', None),
|
||||||
|
|
|
@ -38,13 +38,12 @@ class CarInterface(CarInterfaceBase):
|
||||||
return float(accel) / 3.0
|
return float(accel) / 3.0
|
||||||
|
|
||||||
@staticmethod
|
@staticmethod
|
||||||
def get_params(candidate, fingerprint=gen_empty_fingerprint(), vin="", has_relay=False):
|
def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False, car_fw=[]):
|
||||||
|
|
||||||
ret = car.CarParams.new_message()
|
ret = car.CarParams.new_message()
|
||||||
|
|
||||||
ret.carName = "hyundai"
|
ret.carName = "hyundai"
|
||||||
ret.carFingerprint = candidate
|
ret.carFingerprint = candidate
|
||||||
ret.carVin = vin
|
|
||||||
ret.isPandaBlack = has_relay
|
ret.isPandaBlack = has_relay
|
||||||
ret.radarOffCan = True
|
ret.radarOffCan = True
|
||||||
ret.safetyModel = car.CarParams.SafetyModel.hyundai
|
ret.safetyModel = car.CarParams.SafetyModel.hyundai
|
||||||
|
|
|
@ -18,7 +18,7 @@ class CarInterfaceBase():
|
||||||
raise NotImplementedError
|
raise NotImplementedError
|
||||||
|
|
||||||
@staticmethod
|
@staticmethod
|
||||||
def get_params(candidate, fingerprint=gen_empty_fingerprint(), vin="", has_relay=False):
|
def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False, car_fw=[]):
|
||||||
raise NotImplementedError
|
raise NotImplementedError
|
||||||
|
|
||||||
# returns a car.CarState, pass in car.CarControl
|
# returns a car.CarState, pass in car.CarControl
|
||||||
|
|
|
@ -0,0 +1,128 @@
|
||||||
|
import time
|
||||||
|
from collections import defaultdict
|
||||||
|
from functools import partial
|
||||||
|
|
||||||
|
import cereal.messaging as messaging
|
||||||
|
from selfdrive.swaglog import cloudlog
|
||||||
|
from selfdrive.boardd.boardd import can_list_to_can_capnp
|
||||||
|
from panda.python.uds import CanClient, IsoTpMessage, FUNCTIONAL_ADDRS, get_rx_addr_for_tx_addr
|
||||||
|
|
||||||
|
|
||||||
|
class IsoTpParallelQuery():
|
||||||
|
def __init__(self, sendcan, logcan, bus, addrs, request, response, functional_addr=False, debug=False):
|
||||||
|
self.sendcan = sendcan
|
||||||
|
self.logcan = logcan
|
||||||
|
self.bus = bus
|
||||||
|
self.request = request
|
||||||
|
self.response = response
|
||||||
|
self.debug = debug
|
||||||
|
self.functional_addr = functional_addr
|
||||||
|
|
||||||
|
self.real_addrs = []
|
||||||
|
for a in addrs:
|
||||||
|
if isinstance(a, tuple):
|
||||||
|
self.real_addrs.append(a)
|
||||||
|
else:
|
||||||
|
self.real_addrs.append((a, None))
|
||||||
|
|
||||||
|
self.msg_addrs = {tx_addr: get_rx_addr_for_tx_addr(tx_addr[0]) for tx_addr in self.real_addrs}
|
||||||
|
self.msg_buffer = defaultdict(list)
|
||||||
|
|
||||||
|
def rx(self):
|
||||||
|
"""Drain can socket and sort messages into buffers based on address"""
|
||||||
|
can_packets = messaging.drain_sock(self.logcan, wait_for_one=True)
|
||||||
|
|
||||||
|
for packet in can_packets:
|
||||||
|
for msg in packet.can:
|
||||||
|
if msg.src == self.bus:
|
||||||
|
if self.functional_addr:
|
||||||
|
if (0x7E8 <= msg.address <= 0x7EF) or (0x18DAF100 <= msg.address <= 0x18DAF1FF):
|
||||||
|
fn_addr = next(a for a in FUNCTIONAL_ADDRS if msg.address - a <= 32)
|
||||||
|
self.msg_buffer[fn_addr].append((msg.address, msg.busTime, msg.dat, msg.src))
|
||||||
|
elif msg.address in self.msg_addrs.values():
|
||||||
|
self.msg_buffer[msg.address].append((msg.address, msg.busTime, msg.dat, msg.src))
|
||||||
|
|
||||||
|
def _can_tx(self, tx_addr, dat, bus):
|
||||||
|
"""Helper function to send single message"""
|
||||||
|
msg = [tx_addr, 0, dat, bus]
|
||||||
|
self.sendcan.send(can_list_to_can_capnp([msg], msgtype='sendcan'))
|
||||||
|
|
||||||
|
def _can_rx(self, addr, sub_addr=None):
|
||||||
|
"""Helper function to retrieve message with specified address and subadress from buffer"""
|
||||||
|
keep_msgs = []
|
||||||
|
|
||||||
|
if sub_addr is None:
|
||||||
|
msgs = self.msg_buffer[addr]
|
||||||
|
else:
|
||||||
|
# Filter based on subadress
|
||||||
|
msgs = []
|
||||||
|
for m in self.msg_buffer[addr]:
|
||||||
|
first_byte = m[2][0]
|
||||||
|
if first_byte == sub_addr:
|
||||||
|
msgs.append(m)
|
||||||
|
else:
|
||||||
|
keep_msgs.append(m)
|
||||||
|
|
||||||
|
self.msg_buffer[addr] = keep_msgs
|
||||||
|
return msgs
|
||||||
|
|
||||||
|
def _drain_rx(self):
|
||||||
|
messaging.drain_sock(self.logcan)
|
||||||
|
self.msg_buffer = defaultdict(list)
|
||||||
|
|
||||||
|
def get_data(self, timeout):
|
||||||
|
self._drain_rx()
|
||||||
|
|
||||||
|
# Create message objects
|
||||||
|
msgs = {}
|
||||||
|
request_counter = {}
|
||||||
|
request_done = {}
|
||||||
|
for tx_addr, rx_addr in self.msg_addrs.items():
|
||||||
|
# rx_addr not set when using functional tx addr
|
||||||
|
id_addr = rx_addr or tx_addr[0]
|
||||||
|
sub_addr = tx_addr[1]
|
||||||
|
|
||||||
|
can_client = CanClient(self._can_tx, partial(self._can_rx, id_addr, sub_addr=sub_addr), tx_addr[0], rx_addr, self.bus, sub_addr=sub_addr, debug=self.debug)
|
||||||
|
|
||||||
|
max_len = 8 if sub_addr is None else 7
|
||||||
|
|
||||||
|
msg = IsoTpMessage(can_client, timeout=0, max_len=max_len, debug=self.debug)
|
||||||
|
msg.send(self.request[0])
|
||||||
|
|
||||||
|
msgs[tx_addr] = msg
|
||||||
|
request_counter[tx_addr] = 0
|
||||||
|
request_done[tx_addr] = False
|
||||||
|
|
||||||
|
results = {}
|
||||||
|
start_time = time.time()
|
||||||
|
while True:
|
||||||
|
self.rx()
|
||||||
|
|
||||||
|
if all(request_done.values()):
|
||||||
|
break
|
||||||
|
|
||||||
|
for tx_addr, msg in msgs.items():
|
||||||
|
dat = msg.recv()
|
||||||
|
|
||||||
|
if not dat:
|
||||||
|
continue
|
||||||
|
|
||||||
|
counter = request_counter[tx_addr]
|
||||||
|
expected_response = self.response[counter]
|
||||||
|
response_valid = dat[:len(expected_response)] == expected_response
|
||||||
|
|
||||||
|
if response_valid:
|
||||||
|
if counter + 1 < len(self.request):
|
||||||
|
msg.send(self.request[counter + 1])
|
||||||
|
request_counter[tx_addr] += 1
|
||||||
|
else:
|
||||||
|
results[tx_addr] = dat[len(expected_response):]
|
||||||
|
request_done[tx_addr] = True
|
||||||
|
else:
|
||||||
|
request_done[tx_addr] = True
|
||||||
|
cloudlog.warning(f"iso-tp query bad response: 0x{bytes.hex(dat)}")
|
||||||
|
|
||||||
|
if time.time() - start_time > timeout:
|
||||||
|
break
|
||||||
|
|
||||||
|
return results
|
|
@ -34,7 +34,7 @@ class CarInterface(CarInterfaceBase):
|
||||||
return accel
|
return accel
|
||||||
|
|
||||||
@staticmethod
|
@staticmethod
|
||||||
def get_params(candidate, fingerprint=gen_empty_fingerprint(), vin="", has_relay=False):
|
def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False, car_fw=[]):
|
||||||
|
|
||||||
ret = car.CarParams.new_message()
|
ret = car.CarParams.new_message()
|
||||||
|
|
||||||
|
|
|
@ -35,13 +35,12 @@ class CarInterface(CarInterfaceBase):
|
||||||
return float(accel) / 4.0
|
return float(accel) / 4.0
|
||||||
|
|
||||||
@staticmethod
|
@staticmethod
|
||||||
def get_params(candidate, fingerprint=gen_empty_fingerprint(), vin="", has_relay=False):
|
def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False, car_fw=[]):
|
||||||
ret = car.CarParams.new_message()
|
ret = car.CarParams.new_message()
|
||||||
|
|
||||||
ret.carName = "subaru"
|
ret.carName = "subaru"
|
||||||
ret.radarOffCan = True
|
ret.radarOffCan = True
|
||||||
ret.carFingerprint = candidate
|
ret.carFingerprint = candidate
|
||||||
ret.carVin = vin
|
|
||||||
ret.isPandaBlack = has_relay
|
ret.isPandaBlack = has_relay
|
||||||
ret.safetyModel = car.CarParams.SafetyModel.subaru
|
ret.safetyModel = car.CarParams.SafetyModel.subaru
|
||||||
|
|
||||||
|
|
|
@ -37,13 +37,12 @@ class CarInterface(CarInterfaceBase):
|
||||||
return float(accel) / 3.0
|
return float(accel) / 3.0
|
||||||
|
|
||||||
@staticmethod
|
@staticmethod
|
||||||
def get_params(candidate, fingerprint=gen_empty_fingerprint(), vin="", has_relay=False):
|
def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False, car_fw=[]):
|
||||||
|
|
||||||
ret = car.CarParams.new_message()
|
ret = car.CarParams.new_message()
|
||||||
|
|
||||||
ret.carName = "toyota"
|
ret.carName = "toyota"
|
||||||
ret.carFingerprint = candidate
|
ret.carFingerprint = candidate
|
||||||
ret.carVin = vin
|
|
||||||
ret.isPandaBlack = has_relay
|
ret.isPandaBlack = has_relay
|
||||||
|
|
||||||
ret.safetyModel = car.CarParams.SafetyModel.toyota
|
ret.safetyModel = car.CarParams.SafetyModel.toyota
|
||||||
|
|
|
@ -1,4 +1,6 @@
|
||||||
from selfdrive.car import dbc_dict
|
from selfdrive.car import dbc_dict
|
||||||
|
from cereal import car
|
||||||
|
Ecu = car.CarParams.Ecu
|
||||||
|
|
||||||
# Steer torque limits
|
# Steer torque limits
|
||||||
class SteerLimitParams:
|
class SteerLimitParams:
|
||||||
|
@ -31,9 +33,9 @@ class CAR:
|
||||||
|
|
||||||
|
|
||||||
class ECU:
|
class ECU:
|
||||||
CAM = 0 # camera
|
CAM = Ecu.fwdCamera # camera
|
||||||
DSU = 1 # driving support unit
|
DSU = Ecu.dsu # driving support unit
|
||||||
APGS = 2 # advanced parking guidance system
|
APGS = Ecu.apgs # advanced parking guidance system
|
||||||
|
|
||||||
|
|
||||||
# addr: (ecu, cars, bus, 1/freq*100, vl)
|
# addr: (ecu, cars, bus, 1/freq*100, vl)
|
||||||
|
@ -215,6 +217,32 @@ FINGERPRINTS = {
|
||||||
}]
|
}]
|
||||||
}
|
}
|
||||||
|
|
||||||
|
FW_VERSIONS = {
|
||||||
|
CAR.COROLLA_TSS2: {
|
||||||
|
(Ecu.engine, 0x700, None): [b'\x01896630ZG5000\x00\x00\x00\x00'],
|
||||||
|
(Ecu.eps, 0x7a1, None): [b'\x018965B12350\x00\x00\x00\x00\x00\x00'],
|
||||||
|
(Ecu.esp, 0x7b0, None): [b'\x01F152602280\x00\x00\x00\x00\x00\x00'],
|
||||||
|
(Ecu.fwdRadar, 0x750, 0xf): [b'\x018821F3301100\x00\x00\x00\x00'],
|
||||||
|
(Ecu.fwdCamera, 0x750, 0x6d): [b'\x028646F1201200\x00\x00\x00\x008646G26011A0\x00\x00\x00\x00'],
|
||||||
|
},
|
||||||
|
CAR.PRIUS: {
|
||||||
|
(Ecu.engine, 0x700, None): [b'\x03896634759200\x00\x00\x00\x008966A4703000\x00\x00\x00\x00897CF4701003\x00\x00\x00\x00'],
|
||||||
|
(Ecu.eps, 0x7a1, None): [b'8965B47023\x00\x00\x00\x00\x00\x00'],
|
||||||
|
(Ecu.esp, 0x7b0, None): [b'F152647416\x00\x00\x00\x00\x00\x00'],
|
||||||
|
(Ecu.dsu, 0x791, None): [b'881514703100\x00\x00\x00\x00'],
|
||||||
|
(Ecu.fwdRadar, 0x750, 0xf): [b'8821F4702100\x00\x00\x00\x00'],
|
||||||
|
(Ecu.fwdCamera, 0x750, 0x6d): [b'8646F4702100\x00\x00\x00\x00'],
|
||||||
|
},
|
||||||
|
CAR.RAV4: {
|
||||||
|
(Ecu.engine, 0x7e0, None): [b'\x02342Q2100\x00\x00\x00\x00\x00\x00\x00\x0054213000\x00\x00\x00\x00\x00\x00\x00\x00'],
|
||||||
|
(Ecu.eps, 0x7a1, None): [b'8965B42083\x00\x00\x00\x00\x00\x00'],
|
||||||
|
(Ecu.esp, 0x7b0, None): [b'F15260R103\x00\x00\x00\x00\x00\x00'],
|
||||||
|
(Ecu.dsu, 0x791, None): [b'881514201400\x00\x00\x00\x00'],
|
||||||
|
(Ecu.fwdRadar, 0x750, 0xf): [b'8821F4702100\x00\x00\x00\x00'],
|
||||||
|
(Ecu.fwdCamera, 0x750, 0x6d): [b'8646F4202100\x00\x00\x00\x00'],
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
STEER_THRESHOLD = 100
|
STEER_THRESHOLD = 100
|
||||||
|
|
||||||
DBC = {
|
DBC = {
|
||||||
|
|
|
@ -1,104 +1,33 @@
|
||||||
#!/usr/bin/env python3
|
#!/usr/bin/env python3
|
||||||
import cereal.messaging as messaging
|
import traceback
|
||||||
from selfdrive.boardd.boardd import can_list_to_can_capnp
|
|
||||||
|
|
||||||
|
import cereal.messaging as messaging
|
||||||
|
from panda.python.uds import FUNCTIONAL_ADDRS
|
||||||
|
from selfdrive.car.isotp_parallel_query import IsoTpParallelQuery
|
||||||
|
from selfdrive.swaglog import cloudlog
|
||||||
|
|
||||||
|
VIN_REQUEST = b'\x09\x02'
|
||||||
|
VIN_RESPONSE = b'\x49\x02\x01'
|
||||||
VIN_UNKNOWN = "0" * 17
|
VIN_UNKNOWN = "0" * 17
|
||||||
|
|
||||||
# sanity checks on response messages from vin query
|
|
||||||
def is_vin_response_valid(can_dat, step, cnt):
|
|
||||||
if len(can_dat) != 8:
|
|
||||||
# ISO-TP meesages are all 8 bytes
|
|
||||||
return False
|
|
||||||
|
|
||||||
if step == 0:
|
def get_vin(logcan, sendcan, bus, timeout=0.1, retry=5, debug=False):
|
||||||
# VIN does not fit in a single message and it's 20 bytes of data
|
for i in range(retry):
|
||||||
if can_dat[0] != 0x10 or can_dat[1] != 0x14:
|
try:
|
||||||
return False
|
query = IsoTpParallelQuery(sendcan, logcan, bus, FUNCTIONAL_ADDRS, [VIN_REQUEST], [VIN_RESPONSE], functional_addr=True, debug=debug)
|
||||||
|
for addr, vin in query.get_data(timeout).items():
|
||||||
|
return addr[0], vin.decode()
|
||||||
|
print(f"vin query retry ({i+1}) ...")
|
||||||
|
except Exception:
|
||||||
|
cloudlog.warning(f"VIN query exception: {traceback.format_exc()}")
|
||||||
|
|
||||||
if step == 1 and cnt == 0:
|
return 0, VIN_UNKNOWN
|
||||||
# first response after a CONTINUE query is sent
|
|
||||||
if can_dat[0] != 0x21:
|
|
||||||
return False
|
|
||||||
|
|
||||||
if step == 1 and cnt == 1:
|
|
||||||
# second response after a CONTINUE query is sent
|
|
||||||
if can_dat[0] != 0x22:
|
|
||||||
return False
|
|
||||||
|
|
||||||
return True
|
|
||||||
|
|
||||||
|
|
||||||
class VinQuery():
|
|
||||||
def __init__(self, bus):
|
|
||||||
self.bus = bus
|
|
||||||
# works on standard 11-bit addresses for diagnostic. Tested on Toyota and Subaru;
|
|
||||||
# Honda uses the extended 29-bit addresses, and unfortunately only works from OBDII
|
|
||||||
self.query_ext_msgs = [[0x18DB33F1, 0, b'\x02\x09\x02'.ljust(8, b"\x00"), bus],
|
|
||||||
[0x18DA10f1, 0, b'\x30'.ljust(8, b"\x00"), bus]]
|
|
||||||
self.query_nor_msgs = [[0x7df, 0, b'\x02\x09\x02'.ljust(8, b"\x00"), bus],
|
|
||||||
[0x7e0, 0, b'\x30'.ljust(8, b"\x00"), bus]]
|
|
||||||
|
|
||||||
self.cnts = [1, 2] # number of messages to wait for at each iteration
|
|
||||||
self.step = 0
|
|
||||||
self.cnt = 0
|
|
||||||
self.responded = False
|
|
||||||
self.never_responded = True
|
|
||||||
self.dat = b""
|
|
||||||
self.got_vin = False
|
|
||||||
self.vin = VIN_UNKNOWN
|
|
||||||
|
|
||||||
def check_response(self, msg):
|
|
||||||
# have we got a VIN query response?
|
|
||||||
if msg.src == self.bus and msg.address in [0x18daf110, 0x7e8]:
|
|
||||||
self.never_responded = False
|
|
||||||
# basic sanity checks on ISO-TP response
|
|
||||||
if is_vin_response_valid(msg.dat, self.step, self.cnt):
|
|
||||||
self.dat += bytes(msg.dat[2:]) if self.step == 0 else bytes(msg.dat[1:])
|
|
||||||
self.cnt += 1
|
|
||||||
if self.cnt == self.cnts[self.step]:
|
|
||||||
self.responded = True
|
|
||||||
self.step += 1
|
|
||||||
if self.step == len(self.cnts):
|
|
||||||
self.got_vin = True
|
|
||||||
|
|
||||||
def send_query(self, sendcan):
|
|
||||||
# keep sending VIN query if ECU isn't responsing.
|
|
||||||
# sendcan is probably not ready due to the zmq slow joiner syndrome
|
|
||||||
if self.never_responded or (self.responded and not self.got_vin):
|
|
||||||
sendcan.send(can_list_to_can_capnp([self.query_ext_msgs[self.step]], msgtype='sendcan'))
|
|
||||||
sendcan.send(can_list_to_can_capnp([self.query_nor_msgs[self.step]], msgtype='sendcan'))
|
|
||||||
self.responded = False
|
|
||||||
self.cnt = 0
|
|
||||||
|
|
||||||
def get_vin(self):
|
|
||||||
if self.got_vin:
|
|
||||||
try:
|
|
||||||
self.vin = self.dat[3:].decode('utf8')
|
|
||||||
except UnicodeDecodeError:
|
|
||||||
pass # have seen unexpected non-unicode characters
|
|
||||||
return self.vin
|
|
||||||
|
|
||||||
|
|
||||||
def get_vin(logcan, sendcan, bus, query_time=1.):
|
|
||||||
vin_query = VinQuery(bus)
|
|
||||||
frame = 0
|
|
||||||
|
|
||||||
# 1s max of VIN query time
|
|
||||||
while frame < query_time * 100 and not vin_query.got_vin:
|
|
||||||
a = messaging.get_one_can(logcan)
|
|
||||||
|
|
||||||
for can in a.can:
|
|
||||||
vin_query.check_response(can)
|
|
||||||
if vin_query.got_vin:
|
|
||||||
break
|
|
||||||
|
|
||||||
vin_query.send_query(sendcan)
|
|
||||||
frame += 1
|
|
||||||
|
|
||||||
return vin_query.get_vin()
|
|
||||||
|
|
||||||
|
|
||||||
if __name__ == "__main__":
|
if __name__ == "__main__":
|
||||||
logcan = messaging.sub_sock('can')
|
import time
|
||||||
sendcan = messaging.pub_sock('sendcan')
|
sendcan = messaging.pub_sock('sendcan')
|
||||||
print(get_vin(logcan, sendcan, 0))
|
logcan = messaging.sub_sock('can')
|
||||||
|
time.sleep(1)
|
||||||
|
addr, vin = get_vin(logcan, sendcan, 1, debug=False)
|
||||||
|
print(hex(addr), vin)
|
||||||
|
|
|
@ -42,12 +42,11 @@ class CarInterface(CarInterfaceBase):
|
||||||
return float(accel) / 4.0
|
return float(accel) / 4.0
|
||||||
|
|
||||||
@staticmethod
|
@staticmethod
|
||||||
def get_params(candidate, fingerprint=gen_empty_fingerprint(), vin="", has_relay=False):
|
def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False, car_fw=[]):
|
||||||
ret = car.CarParams.new_message()
|
ret = car.CarParams.new_message()
|
||||||
|
|
||||||
ret.carFingerprint = candidate
|
ret.carFingerprint = candidate
|
||||||
ret.isPandaBlack = has_relay
|
ret.isPandaBlack = has_relay
|
||||||
ret.carVin = vin
|
|
||||||
|
|
||||||
if candidate == CAR.GOLF:
|
if candidate == CAR.GOLF:
|
||||||
# Set common MQB parameters that will apply globally
|
# Set common MQB parameters that will apply globally
|
||||||
|
|
|
@ -0,0 +1 @@
|
||||||
|
clocksd
|
|
@ -0,0 +1,2 @@
|
||||||
|
Import('env', 'common', 'messaging')
|
||||||
|
env.Program('clocksd.cc', LIBS=['diag', 'time_genoff', common, messaging, 'capnp', 'zmq', 'kj'])
|
|
@ -0,0 +1,72 @@
|
||||||
|
#include <stdio.h>
|
||||||
|
#include <stdint.h>
|
||||||
|
#include <sys/resource.h>
|
||||||
|
#include <sys/timerfd.h>
|
||||||
|
#include <sys/time.h>
|
||||||
|
#include <utils/Timers.h>
|
||||||
|
#include <capnp/serialize.h>
|
||||||
|
#include "messaging.hpp"
|
||||||
|
#include "common/timing.h"
|
||||||
|
#include "cereal/gen/cpp/log.capnp.h"
|
||||||
|
|
||||||
|
namespace {
|
||||||
|
int64_t arm_cntpct() {
|
||||||
|
int64_t v;
|
||||||
|
asm volatile("mrs %0, cntpct_el0" : "=r"(v));
|
||||||
|
return v;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
int main() {
|
||||||
|
setpriority(PRIO_PROCESS, 0, -13);
|
||||||
|
|
||||||
|
int err = 0;
|
||||||
|
Context *context = Context::create();
|
||||||
|
|
||||||
|
PubSocket* clock_publisher = PubSocket::create(context, "clocks");
|
||||||
|
assert(clock_publisher != NULL);
|
||||||
|
|
||||||
|
int timerfd = timerfd_create(CLOCK_BOOTTIME, 0);
|
||||||
|
assert(timerfd >= 0);
|
||||||
|
|
||||||
|
struct itimerspec spec = {0};
|
||||||
|
spec.it_interval.tv_sec = 1;
|
||||||
|
spec.it_interval.tv_nsec = 0;
|
||||||
|
spec.it_value.tv_sec = 1;
|
||||||
|
spec.it_value.tv_nsec = 0;
|
||||||
|
|
||||||
|
err = timerfd_settime(timerfd, 0, &spec, 0);
|
||||||
|
assert(err == 0);
|
||||||
|
|
||||||
|
uint64_t expirations = 0;
|
||||||
|
while ((err = read(timerfd, &expirations, sizeof(expirations)))) {
|
||||||
|
if (err < 0) break;
|
||||||
|
|
||||||
|
uint64_t boottime = nanos_since_boot();
|
||||||
|
uint64_t monotonic = nanos_monotonic();
|
||||||
|
uint64_t monotonic_raw = nanos_monotonic_raw();
|
||||||
|
uint64_t wall_time = nanos_since_epoch();
|
||||||
|
|
||||||
|
uint64_t modem_uptime_v = arm_cntpct() / 19200ULL; // 19.2 mhz clock
|
||||||
|
|
||||||
|
capnp::MallocMessageBuilder msg;
|
||||||
|
cereal::Event::Builder event = msg.initRoot<cereal::Event>();
|
||||||
|
event.setLogMonoTime(boottime);
|
||||||
|
auto clocks = event.initClocks();
|
||||||
|
|
||||||
|
clocks.setBootTimeNanos(boottime);
|
||||||
|
clocks.setMonotonicNanos(monotonic);
|
||||||
|
clocks.setMonotonicRawNanos(monotonic_raw);
|
||||||
|
clocks.setWallTimeNanos(wall_time);
|
||||||
|
clocks.setModemUptimeMillis(modem_uptime_v);
|
||||||
|
|
||||||
|
auto words = capnp::messageToFlatArray(msg);
|
||||||
|
auto bytes = words.asBytes();
|
||||||
|
clock_publisher->send((char*)bytes.begin(), bytes.size());
|
||||||
|
}
|
||||||
|
|
||||||
|
close(timerfd);
|
||||||
|
delete clock_publisher;
|
||||||
|
|
||||||
|
return 0;
|
||||||
|
}
|
|
@ -1,4 +1,3 @@
|
||||||
|
|
||||||
#include <cstdio>
|
#include <cstdio>
|
||||||
#include <cstdlib>
|
#include <cstdlib>
|
||||||
#include <cassert>
|
#include <cassert>
|
||||||
|
@ -39,7 +38,6 @@ extern "C" void framebuffer_set_power(FramebufferState *s, int mode) {
|
||||||
|
|
||||||
extern "C" FramebufferState* framebuffer_init(
|
extern "C" FramebufferState* framebuffer_init(
|
||||||
const char* name, int32_t layer, int alpha,
|
const char* name, int32_t layer, int alpha,
|
||||||
EGLDisplay *out_display, EGLSurface *out_surface,
|
|
||||||
int *out_w, int *out_h) {
|
int *out_w, int *out_h) {
|
||||||
status_t status;
|
status_t status;
|
||||||
int success;
|
int success;
|
||||||
|
@ -131,11 +129,14 @@ extern "C" FramebufferState* framebuffer_init(
|
||||||
const char brightness_level[] = BACKLIGHT_LEVEL;
|
const char brightness_level[] = BACKLIGHT_LEVEL;
|
||||||
write(brightness_fd, brightness_level, strlen(brightness_level));
|
write(brightness_fd, brightness_level, strlen(brightness_level));
|
||||||
|
|
||||||
|
|
||||||
if (out_display) *out_display = s->display;
|
|
||||||
if (out_surface) *out_surface = s->surface;
|
|
||||||
if (out_w) *out_w = w;
|
if (out_w) *out_w = w;
|
||||||
if (out_h) *out_h = h;
|
if (out_h) *out_h = h;
|
||||||
|
|
||||||
return s;
|
return s;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
extern "C" void framebuffer_swap(FramebufferState *s) {
|
||||||
|
eglSwapBuffers(s->display, s->surface);
|
||||||
|
assert(glGetError() == GL_NO_ERROR);
|
||||||
|
}
|
||||||
|
|
||||||
|
|
|
@ -11,10 +11,10 @@ typedef struct FramebufferState FramebufferState;
|
||||||
|
|
||||||
FramebufferState* framebuffer_init(
|
FramebufferState* framebuffer_init(
|
||||||
const char* name, int32_t layer, int alpha,
|
const char* name, int32_t layer, int alpha,
|
||||||
EGLDisplay *out_display, EGLSurface *out_surface,
|
|
||||||
int *out_w, int *out_h);
|
int *out_w, int *out_h);
|
||||||
|
|
||||||
void framebuffer_set_power(FramebufferState *s, int mode);
|
void framebuffer_set_power(FramebufferState *s, int mode);
|
||||||
|
void framebuffer_swap(FramebufferState *s);
|
||||||
|
|
||||||
/* Display power modes */
|
/* Display power modes */
|
||||||
enum {
|
enum {
|
||||||
|
|
|
@ -243,10 +243,6 @@ int read_db_value(const char* params_path, const char* key, char** value,
|
||||||
goto cleanup;
|
goto cleanup;
|
||||||
}
|
}
|
||||||
|
|
||||||
// Remove one for null byte.
|
|
||||||
if (value_sz != NULL) {
|
|
||||||
*value_sz -= 1;
|
|
||||||
}
|
|
||||||
result = 0;
|
result = 0;
|
||||||
|
|
||||||
cleanup:
|
cleanup:
|
||||||
|
|
|
@ -19,7 +19,7 @@ void* read_file(const char* path, size_t* out_len) {
|
||||||
long f_len = ftell(f);
|
long f_len = ftell(f);
|
||||||
rewind(f);
|
rewind(f);
|
||||||
|
|
||||||
char* buf = calloc(f_len + 1, 1);
|
char* buf = (char*)calloc(f_len, 1);
|
||||||
assert(buf);
|
assert(buf);
|
||||||
|
|
||||||
size_t num_read = fread(buf, f_len, 1, f);
|
size_t num_read = fread(buf, f_len, 1, f);
|
||||||
|
@ -31,7 +31,7 @@ void* read_file(const char* path, size_t* out_len) {
|
||||||
}
|
}
|
||||||
|
|
||||||
if (out_len) {
|
if (out_len) {
|
||||||
*out_len = f_len + 1;
|
*out_len = f_len;
|
||||||
}
|
}
|
||||||
|
|
||||||
return buf;
|
return buf;
|
||||||
|
@ -54,6 +54,8 @@ int set_realtime_priority(int level) {
|
||||||
memset(&sa, 0, sizeof(sa));
|
memset(&sa, 0, sizeof(sa));
|
||||||
sa.sched_priority = level;
|
sa.sched_priority = level;
|
||||||
return sched_setscheduler(tid, SCHED_FIFO, &sa);
|
return sched_setscheduler(tid, SCHED_FIFO, &sa);
|
||||||
|
#else
|
||||||
|
return -1;
|
||||||
#endif
|
#endif
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -1 +1 @@
|
||||||
#define COMMA_VERSION "0.7-release"
|
#define COMMA_VERSION "0.7.1-release"
|
||||||
|
|
|
@ -1,13 +1,12 @@
|
||||||
#ifndef VISIONIMG_H
|
#ifndef VISIONIMG_H
|
||||||
#define VISIONIMG_H
|
#define VISIONIMG_H
|
||||||
|
|
||||||
#ifdef QCOM
|
#include "common/visionbuf.h"
|
||||||
|
|
||||||
#include <GLES3/gl3.h>
|
#include <GLES3/gl3.h>
|
||||||
#include <EGL/egl.h>
|
#include <EGL/egl.h>
|
||||||
#include <EGL/eglext.h>
|
#include <EGL/eglext.h>
|
||||||
#endif
|
#undef Status
|
||||||
|
|
||||||
#include "common/visionbuf.h"
|
|
||||||
|
|
||||||
#ifdef __cplusplus
|
#ifdef __cplusplus
|
||||||
extern "C" {
|
extern "C" {
|
||||||
|
@ -26,11 +25,9 @@ typedef struct VisionImg {
|
||||||
void visionimg_compute_aligned_width_and_height(int width, int height, int *aligned_w, int *aligned_h);
|
void visionimg_compute_aligned_width_and_height(int width, int height, int *aligned_w, int *aligned_h);
|
||||||
VisionImg visionimg_alloc_rgb24(int width, int height, VisionBuf *out_buf);
|
VisionImg visionimg_alloc_rgb24(int width, int height, VisionBuf *out_buf);
|
||||||
|
|
||||||
#ifdef QCOM
|
|
||||||
EGLClientBuffer visionimg_to_egl(const VisionImg *img, void **pph);
|
EGLClientBuffer visionimg_to_egl(const VisionImg *img, void **pph);
|
||||||
GLuint visionimg_to_gl(const VisionImg *img, EGLImageKHR *pkhr, void **pph);
|
GLuint visionimg_to_gl(const VisionImg *img, EGLImageKHR *pkhr, void **pph);
|
||||||
void visionimg_destroy_gl(EGLImageKHR khr, void *ph);
|
void visionimg_destroy_gl(EGLImageKHR khr, void *ph);
|
||||||
#endif
|
|
||||||
|
|
||||||
#ifdef __cplusplus
|
#ifdef __cplusplus
|
||||||
} // extern "C"
|
} // extern "C"
|
||||||
|
|
|
@ -67,7 +67,7 @@ def events_to_bytes(events):
|
||||||
return ret
|
return ret
|
||||||
|
|
||||||
|
|
||||||
def data_sample(CI, CC, sm, can_sock, driver_status, state, mismatch_counter, params):
|
def data_sample(CI, CC, sm, can_sock, driver_status, state, mismatch_counter, can_error_counter, params):
|
||||||
"""Receive data from sockets and create events for battery, temperature and disk space"""
|
"""Receive data from sockets and create events for battery, temperature and disk space"""
|
||||||
|
|
||||||
# Update carstate from CAN and create events
|
# Update carstate from CAN and create events
|
||||||
|
@ -82,6 +82,7 @@ def data_sample(CI, CC, sm, can_sock, driver_status, state, mismatch_counter, pa
|
||||||
|
|
||||||
# Check for CAN timeout
|
# Check for CAN timeout
|
||||||
if not can_strs:
|
if not can_strs:
|
||||||
|
can_error_counter += 1
|
||||||
events.append(create_event('canError', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
|
events.append(create_event('canError', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
|
||||||
|
|
||||||
overtemp = sm['thermal'].thermalStatus >= ThermalStatus.red
|
overtemp = sm['thermal'].thermalStatus >= ThermalStatus.red
|
||||||
|
@ -147,7 +148,7 @@ def data_sample(CI, CC, sm, can_sock, driver_status, state, mismatch_counter, pa
|
||||||
if driver_status.terminal_alert_cnt >= MAX_TERMINAL_ALERTS or driver_status.terminal_time >= MAX_TERMINAL_DURATION:
|
if driver_status.terminal_alert_cnt >= MAX_TERMINAL_ALERTS or driver_status.terminal_time >= MAX_TERMINAL_DURATION:
|
||||||
events.append(create_event("tooDistracted", [ET.NO_ENTRY]))
|
events.append(create_event("tooDistracted", [ET.NO_ENTRY]))
|
||||||
|
|
||||||
return CS, events, cal_perc, mismatch_counter
|
return CS, events, cal_perc, mismatch_counter, can_error_counter
|
||||||
|
|
||||||
|
|
||||||
def state_transition(frame, CS, CP, state, events, soft_disable_timer, v_cruise_kph, AM):
|
def state_transition(frame, CS, CP, state, events, soft_disable_timer, v_cruise_kph, AM):
|
||||||
|
@ -319,7 +320,7 @@ def state_control(frame, rcv_frame, plan, path_plan, CS, CP, state, events, v_cr
|
||||||
|
|
||||||
def data_send(sm, pm, CS, CI, CP, VM, state, events, actuators, v_cruise_kph, rk, AM,
|
def data_send(sm, pm, CS, CI, CP, VM, state, events, actuators, v_cruise_kph, rk, AM,
|
||||||
driver_status, LaC, LoC, read_only, start_time, v_acc, a_acc, lac_log, events_prev,
|
driver_status, LaC, LoC, read_only, start_time, v_acc, a_acc, lac_log, events_prev,
|
||||||
last_blinker_frame, is_ldw_enabled):
|
last_blinker_frame, is_ldw_enabled, can_error_counter):
|
||||||
"""Send actuators and hud commands to the car, send controlsstate and MPC logging"""
|
"""Send actuators and hud commands to the car, send controlsstate and MPC logging"""
|
||||||
|
|
||||||
CC = car.CarControl.new_message()
|
CC = car.CarControl.new_message()
|
||||||
|
@ -416,6 +417,7 @@ def data_send(sm, pm, CS, CI, CP, VM, state, events, actuators, v_cruise_kph, rk
|
||||||
"startMonoTime": int(start_time * 1e9),
|
"startMonoTime": int(start_time * 1e9),
|
||||||
"mapValid": sm['plan'].mapValid,
|
"mapValid": sm['plan'].mapValid,
|
||||||
"forceDecel": bool(force_decel),
|
"forceDecel": bool(force_decel),
|
||||||
|
"canErrorCounter": can_error_counter,
|
||||||
}
|
}
|
||||||
|
|
||||||
if CP.lateralTuning.which() == 'pid':
|
if CP.lateralTuning.which() == 'pid':
|
||||||
|
@ -534,6 +536,7 @@ def controlsd_thread(sm=None, pm=None, can_sock=None):
|
||||||
v_cruise_kph = 255
|
v_cruise_kph = 255
|
||||||
v_cruise_kph_last = 0
|
v_cruise_kph_last = 0
|
||||||
mismatch_counter = 0
|
mismatch_counter = 0
|
||||||
|
can_error_counter = 0
|
||||||
last_blinker_frame = 0
|
last_blinker_frame = 0
|
||||||
events_prev = []
|
events_prev = []
|
||||||
|
|
||||||
|
@ -557,11 +560,13 @@ def controlsd_thread(sm=None, pm=None, can_sock=None):
|
||||||
prof.checkpoint("Ratekeeper", ignore=True)
|
prof.checkpoint("Ratekeeper", ignore=True)
|
||||||
|
|
||||||
# Sample data and compute car events
|
# Sample data and compute car events
|
||||||
CS, events, cal_perc, mismatch_counter = data_sample(CI, CC, sm, can_sock, driver_status, state, mismatch_counter, params)
|
CS, events, cal_perc, mismatch_counter, can_error_counter = data_sample(CI, CC, sm, can_sock, driver_status, state, mismatch_counter, can_error_counter, params)
|
||||||
prof.checkpoint("Sample")
|
prof.checkpoint("Sample")
|
||||||
|
|
||||||
# Create alerts
|
# Create alerts
|
||||||
if not sm.all_alive_and_valid():
|
if not sm.alive['plan'] and sm.alive['pathPlan']: # only plan not being received: radar not communicating
|
||||||
|
events.append(create_event('radarCommIssue', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
|
||||||
|
elif not sm.all_alive_and_valid():
|
||||||
events.append(create_event('commIssue', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
|
events.append(create_event('commIssue', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
|
||||||
if not sm['pathPlan'].mpcSolutionValid:
|
if not sm['pathPlan'].mpcSolutionValid:
|
||||||
events.append(create_event('plannerError', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
|
events.append(create_event('plannerError', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
|
||||||
|
@ -583,6 +588,8 @@ def controlsd_thread(sm=None, pm=None, can_sock=None):
|
||||||
events.append(create_event('internetConnectivityNeeded', [ET.NO_ENTRY, ET.PERMANENT]))
|
events.append(create_event('internetConnectivityNeeded', [ET.NO_ENTRY, ET.PERMANENT]))
|
||||||
if community_feature_disallowed:
|
if community_feature_disallowed:
|
||||||
events.append(create_event('communityFeatureDisallowed', [ET.PERMANENT]))
|
events.append(create_event('communityFeatureDisallowed', [ET.PERMANENT]))
|
||||||
|
if read_only and not passive:
|
||||||
|
events.append(create_event('carUnrecognized', [ET.PERMANENT]))
|
||||||
|
|
||||||
# Only allow engagement with brake pressed when stopped behind another stopped car
|
# Only allow engagement with brake pressed when stopped behind another stopped car
|
||||||
if CS.brakePressed and sm['plan'].vTargetFuture >= STARTING_TARGET_SPEED and not CP.radarOffCan and CS.vEgo < 0.3:
|
if CS.brakePressed and sm['plan'].vTargetFuture >= STARTING_TARGET_SPEED and not CP.radarOffCan and CS.vEgo < 0.3:
|
||||||
|
@ -603,7 +610,8 @@ def controlsd_thread(sm=None, pm=None, can_sock=None):
|
||||||
|
|
||||||
# Publish data
|
# Publish data
|
||||||
CC, events_prev = data_send(sm, pm, CS, CI, CP, VM, state, events, actuators, v_cruise_kph, rk, AM, driver_status, LaC,
|
CC, events_prev = data_send(sm, pm, CS, CI, CP, VM, state, events, actuators, v_cruise_kph, rk, AM, driver_status, LaC,
|
||||||
LoC, read_only, start_time, v_acc, a_acc, lac_log, events_prev, last_blinker_frame, is_ldw_enabled)
|
LoC, read_only, start_time, v_acc, a_acc, lac_log, events_prev, last_blinker_frame,
|
||||||
|
is_ldw_enabled, can_error_counter)
|
||||||
prof.checkpoint("Sent")
|
prof.checkpoint("Sent")
|
||||||
|
|
||||||
rk.monitor_time()
|
rk.monitor_time()
|
||||||
|
|
|
@ -410,6 +410,13 @@ ALERTS = [
|
||||||
AlertStatus.critical, AlertSize.full,
|
AlertStatus.critical, AlertSize.full,
|
||||||
Priority.MID, VisualAlert.steerRequired, AudibleAlert.chimeWarningRepeat, .1, 2., 2.),
|
Priority.MID, VisualAlert.steerRequired, AudibleAlert.chimeWarningRepeat, .1, 2., 2.),
|
||||||
|
|
||||||
|
Alert(
|
||||||
|
"radarCommIssue",
|
||||||
|
"TAKE CONTROL IMMEDIATELY",
|
||||||
|
"Radar Communication Issue",
|
||||||
|
AlertStatus.critical, AlertSize.full,
|
||||||
|
Priority.MID, VisualAlert.steerRequired, AudibleAlert.chimeWarningRepeat, .1, 2., 2.),
|
||||||
|
|
||||||
Alert(
|
Alert(
|
||||||
"radarCanError",
|
"radarCanError",
|
||||||
"TAKE CONTROL IMMEDIATELY",
|
"TAKE CONTROL IMMEDIATELY",
|
||||||
|
@ -659,6 +666,13 @@ ALERTS = [
|
||||||
AlertStatus.normal, AlertSize.mid,
|
AlertStatus.normal, AlertSize.mid,
|
||||||
Priority.LOW, VisualAlert.none, AudibleAlert.chimeDisengage, .4, 2., 3.),
|
Priority.LOW, VisualAlert.none, AudibleAlert.chimeDisengage, .4, 2., 3.),
|
||||||
|
|
||||||
|
Alert(
|
||||||
|
"radarCommIssueNoEntry",
|
||||||
|
"openpilot Unavailable",
|
||||||
|
"Radar Communication Issue",
|
||||||
|
AlertStatus.normal, AlertSize.mid,
|
||||||
|
Priority.LOW, VisualAlert.none, AudibleAlert.chimeDisengage, .4, 2., 3.),
|
||||||
|
|
||||||
Alert(
|
Alert(
|
||||||
"internetConnectivityNeededNoEntry",
|
"internetConnectivityNeededNoEntry",
|
||||||
"openpilot Unavailable",
|
"openpilot Unavailable",
|
||||||
|
@ -739,11 +753,18 @@ ALERTS = [
|
||||||
|
|
||||||
Alert(
|
Alert(
|
||||||
"lowMemoryPermanent",
|
"lowMemoryPermanent",
|
||||||
"RAM Memory Critically Low",
|
"RAM Critically Low",
|
||||||
"Reboot your EON",
|
"Reboot your EON",
|
||||||
AlertStatus.normal, AlertSize.mid,
|
AlertStatus.normal, AlertSize.mid,
|
||||||
Priority.LOW_LOWEST, VisualAlert.none, AudibleAlert.none, 0., 0., .2),
|
Priority.LOW_LOWEST, VisualAlert.none, AudibleAlert.none, 0., 0., .2),
|
||||||
|
|
||||||
|
Alert(
|
||||||
|
"carUnrecognizedPermanent",
|
||||||
|
"Dashcam Mode",
|
||||||
|
"Car Unrecognized",
|
||||||
|
AlertStatus.normal, AlertSize.mid,
|
||||||
|
Priority.LOW_LOWEST, VisualAlert.none, AudibleAlert.none, 0., 0., .2),
|
||||||
|
|
||||||
Alert(
|
Alert(
|
||||||
"vehicleModelInvalid",
|
"vehicleModelInvalid",
|
||||||
"Vehicle Parameter Identification Failed",
|
"Vehicle Parameter Identification Failed",
|
||||||
|
|
|
@ -20,9 +20,9 @@ _PITCH_WEIGHT = 1.35 # 1.5 # pitch matters a lot more
|
||||||
_METRIC_THRESHOLD = 0.4
|
_METRIC_THRESHOLD = 0.4
|
||||||
_METRIC_THRESHOLD_SLACK = 0.55
|
_METRIC_THRESHOLD_SLACK = 0.55
|
||||||
_METRIC_THRESHOLD_STRICT = 0.4
|
_METRIC_THRESHOLD_STRICT = 0.4
|
||||||
_PITCH_POS_ALLOWANCE = 0.04 # 0.08 # rad, to not be too sensitive on positive pitch
|
_PITCH_POS_ALLOWANCE = 0.12 # rad, to not be too sensitive on positive pitch
|
||||||
_PITCH_NATURAL_OFFSET = 0.12 # 0.1 # people don't seem to look straight when they drive relaxed, rather a bit up
|
_PITCH_NATURAL_OFFSET = 0.02 # people don't seem to look straight when they drive relaxed, rather a bit up
|
||||||
_YAW_NATURAL_OFFSET = 0.08 # people don't seem to look straight when they drive relaxed, rather a bit to the right (center of car)
|
_YAW_NATURAL_OFFSET = 0.08 # people don't seem to look straight when they drive relaxed, rather a bit to the right (center of car)
|
||||||
|
|
||||||
_DISTRACTED_FILTER_TS = 0.25 # 0.6Hz
|
_DISTRACTED_FILTER_TS = 0.25 # 0.6Hz
|
||||||
|
|
||||||
|
@ -138,13 +138,13 @@ class DriverStatus():
|
||||||
if not self.pose_calibrated:
|
if not self.pose_calibrated:
|
||||||
pitch_error = pose.pitch - _PITCH_NATURAL_OFFSET
|
pitch_error = pose.pitch - _PITCH_NATURAL_OFFSET
|
||||||
yaw_error = pose.yaw - _YAW_NATURAL_OFFSET
|
yaw_error = pose.yaw - _YAW_NATURAL_OFFSET
|
||||||
# add positive pitch allowance
|
|
||||||
if pitch_error > 0.:
|
|
||||||
pitch_error = max(pitch_error - _PITCH_POS_ALLOWANCE, 0.)
|
|
||||||
else:
|
else:
|
||||||
pitch_error = pose.pitch - self.pose.pitch_offseter.filtered_stat.mean()
|
pitch_error = pose.pitch - self.pose.pitch_offseter.filtered_stat.mean()
|
||||||
yaw_error = pose.yaw - self.pose.yaw_offseter.filtered_stat.mean()
|
yaw_error = pose.yaw - self.pose.yaw_offseter.filtered_stat.mean()
|
||||||
|
|
||||||
|
# positive pitch allowance
|
||||||
|
if pitch_error > 0.:
|
||||||
|
pitch_error = max(pitch_error - _PITCH_POS_ALLOWANCE, 0.)
|
||||||
pitch_error *= _PITCH_WEIGHT
|
pitch_error *= _PITCH_WEIGHT
|
||||||
pose_metric = np.sqrt(yaw_error**2 + pitch_error**2)
|
pose_metric = np.sqrt(yaw_error**2 + pitch_error**2)
|
||||||
|
|
||||||
|
|
|
@ -14,6 +14,9 @@ LaneChangeDirection = log.PathPlan.LaneChangeDirection
|
||||||
|
|
||||||
LOG_MPC = os.environ.get('LOG_MPC', False)
|
LOG_MPC = os.environ.get('LOG_MPC', False)
|
||||||
|
|
||||||
|
LANE_CHANGE_SPEED_MIN = 45 * CV.MPH_TO_MS
|
||||||
|
LANE_CHANGE_TIME_MAX = 10.
|
||||||
|
|
||||||
DESIRES = {
|
DESIRES = {
|
||||||
LaneChangeDirection.none: {
|
LaneChangeDirection.none: {
|
||||||
LaneChangeState.off: log.PathPlan.Desire.none,
|
LaneChangeState.off: log.PathPlan.Desire.none,
|
||||||
|
@ -88,8 +91,9 @@ class PathPlanner():
|
||||||
# Lane change logic
|
# Lane change logic
|
||||||
lane_change_direction = LaneChangeDirection.none
|
lane_change_direction = LaneChangeDirection.none
|
||||||
one_blinker = sm['carState'].leftBlinker != sm['carState'].rightBlinker
|
one_blinker = sm['carState'].leftBlinker != sm['carState'].rightBlinker
|
||||||
|
below_lane_change_speed = v_ego < LANE_CHANGE_SPEED_MIN
|
||||||
|
|
||||||
if not active or self.lane_change_timer > 10.0:
|
if not active or self.lane_change_timer > LANE_CHANGE_TIME_MAX:
|
||||||
self.lane_change_state = LaneChangeState.off
|
self.lane_change_state = LaneChangeState.off
|
||||||
else:
|
else:
|
||||||
if sm['carState'].leftBlinker:
|
if sm['carState'].leftBlinker:
|
||||||
|
@ -97,23 +101,23 @@ class PathPlanner():
|
||||||
elif sm['carState'].rightBlinker:
|
elif sm['carState'].rightBlinker:
|
||||||
lane_change_direction = LaneChangeDirection.right
|
lane_change_direction = LaneChangeDirection.right
|
||||||
|
|
||||||
if lane_change_direction == LaneChangeDirection.left:
|
torque_applied = sm['carState'].steeringPressed and \
|
||||||
torque_applied = sm['carState'].steeringTorque > 0 and sm['carState'].steeringPressed
|
((sm['carState'].steeringTorque > 0 and lane_change_direction == LaneChangeDirection.left) or \
|
||||||
else:
|
(sm['carState'].steeringTorque < 0 and lane_change_direction == LaneChangeDirection.right))
|
||||||
torque_applied = sm['carState'].steeringTorque < 0 and sm['carState'].steeringPressed
|
|
||||||
|
|
||||||
lane_change_prob = self.LP.l_lane_change_prob + self.LP.r_lane_change_prob
|
lane_change_prob = self.LP.l_lane_change_prob + self.LP.r_lane_change_prob
|
||||||
|
|
||||||
# State transitions
|
# State transitions
|
||||||
# off
|
# off
|
||||||
if False: # self.lane_change_state == LaneChangeState.off and one_blinker and not self.prev_one_blinker:
|
if self.lane_change_state == LaneChangeState.off and one_blinker and not self.prev_one_blinker and not below_lane_change_speed:
|
||||||
self.lane_change_state = LaneChangeState.preLaneChange
|
self.lane_change_state = LaneChangeState.preLaneChange
|
||||||
|
|
||||||
# pre
|
# pre
|
||||||
elif self.lane_change_state == LaneChangeState.preLaneChange and not one_blinker:
|
elif self.lane_change_state == LaneChangeState.preLaneChange:
|
||||||
self.lane_change_state = LaneChangeState.off
|
if not one_blinker or below_lane_change_speed:
|
||||||
elif self.lane_change_state == LaneChangeState.preLaneChange and torque_applied:
|
self.lane_change_state = LaneChangeState.off
|
||||||
self.lane_change_state = LaneChangeState.laneChangeStarting
|
elif torque_applied:
|
||||||
|
self.lane_change_state = LaneChangeState.laneChangeStarting
|
||||||
|
|
||||||
# starting
|
# starting
|
||||||
elif self.lane_change_state == LaneChangeState.laneChangeStarting and lane_change_prob > 0.5:
|
elif self.lane_change_state == LaneChangeState.laneChangeStarting and lane_change_prob > 0.5:
|
||||||
|
@ -121,11 +125,10 @@ class PathPlanner():
|
||||||
|
|
||||||
# finishing
|
# finishing
|
||||||
elif self.lane_change_state == LaneChangeState.laneChangeFinishing and lane_change_prob < 0.2:
|
elif self.lane_change_state == LaneChangeState.laneChangeFinishing and lane_change_prob < 0.2:
|
||||||
self.lane_change_state = LaneChangeState.preLaneChange
|
if one_blinker:
|
||||||
|
self.lane_change_state = LaneChangeState.preLaneChange
|
||||||
# Don't allow starting lane change below 45 mph
|
else:
|
||||||
if (v_ego < 45 * CV.MPH_TO_MS) and (self.lane_change_state == LaneChangeState.preLaneChange):
|
self.lane_change_state = LaneChangeState.off
|
||||||
self.lane_change_state = LaneChangeState.off
|
|
||||||
|
|
||||||
if self.lane_change_state in [LaneChangeState.off, LaneChangeState.preLaneChange]:
|
if self.lane_change_state in [LaneChangeState.off, LaneChangeState.preLaneChange]:
|
||||||
self.lane_change_timer = 0.0
|
self.lane_change_timer = 0.0
|
||||||
|
|
|
@ -27,7 +27,8 @@ _A_CRUISE_MIN_BP = [ 0., 5., 10., 20., 40.]
|
||||||
|
|
||||||
# need fast accel at very low speed for stop and go
|
# need fast accel at very low speed for stop and go
|
||||||
# make sure these accelerations are smaller than mpc limits
|
# make sure these accelerations are smaller than mpc limits
|
||||||
_A_CRUISE_MAX_V = [1.6, 1.6, 0.65, .4]
|
_A_CRUISE_MAX_V = [1.2, 1.2, 0.65, .4]
|
||||||
|
_A_CRUISE_MAX_V_FOLLOWING = [1.6, 1.6, 0.65, .4]
|
||||||
_A_CRUISE_MAX_BP = [0., 6.4, 22.5, 40.]
|
_A_CRUISE_MAX_BP = [0., 6.4, 22.5, 40.]
|
||||||
|
|
||||||
# Lookup table for turns
|
# Lookup table for turns
|
||||||
|
@ -38,9 +39,13 @@ _A_TOTAL_MAX_BP = [20., 40.]
|
||||||
SPEED_PERCENTILE_IDX = 7
|
SPEED_PERCENTILE_IDX = 7
|
||||||
|
|
||||||
|
|
||||||
def calc_cruise_accel_limits(v_ego):
|
def calc_cruise_accel_limits(v_ego, following):
|
||||||
a_cruise_min = interp(v_ego, _A_CRUISE_MIN_BP, _A_CRUISE_MIN_V)
|
a_cruise_min = interp(v_ego, _A_CRUISE_MIN_BP, _A_CRUISE_MIN_V)
|
||||||
a_cruise_max = interp(v_ego, _A_CRUISE_MAX_BP, _A_CRUISE_MAX_V)
|
|
||||||
|
if following:
|
||||||
|
a_cruise_max = interp(v_ego, _A_CRUISE_MAX_BP, _A_CRUISE_MAX_V_FOLLOWING)
|
||||||
|
else:
|
||||||
|
a_cruise_max = interp(v_ego, _A_CRUISE_MAX_BP, _A_CRUISE_MAX_V)
|
||||||
return np.vstack([a_cruise_min, a_cruise_max])
|
return np.vstack([a_cruise_min, a_cruise_max])
|
||||||
|
|
||||||
|
|
||||||
|
@ -80,6 +85,7 @@ class Planner():
|
||||||
self.path_x = np.arange(192)
|
self.path_x = np.arange(192)
|
||||||
|
|
||||||
self.params = Params()
|
self.params = Params()
|
||||||
|
self.first_loop = True
|
||||||
|
|
||||||
def choose_solution(self, v_cruise_setpoint, enabled):
|
def choose_solution(self, v_cruise_setpoint, enabled):
|
||||||
if enabled:
|
if enabled:
|
||||||
|
@ -122,6 +128,7 @@ class Planner():
|
||||||
lead_2 = sm['radarState'].leadTwo
|
lead_2 = sm['radarState'].leadTwo
|
||||||
|
|
||||||
enabled = (long_control_state == LongCtrlState.pid) or (long_control_state == LongCtrlState.stopping)
|
enabled = (long_control_state == LongCtrlState.pid) or (long_control_state == LongCtrlState.stopping)
|
||||||
|
following = lead_1.status and lead_1.dRel < 45.0 and lead_1.vLeadK > v_ego and lead_1.aLeadK > 0.0
|
||||||
|
|
||||||
if len(sm['model'].path.poly):
|
if len(sm['model'].path.poly):
|
||||||
path = list(sm['model'].path.poly)
|
path = list(sm['model'].path.poly)
|
||||||
|
@ -142,8 +149,8 @@ class Planner():
|
||||||
model_speed = MAX_SPEED
|
model_speed = MAX_SPEED
|
||||||
|
|
||||||
# Calculate speed for normal cruise control
|
# Calculate speed for normal cruise control
|
||||||
if enabled:
|
if enabled and not self.first_loop:
|
||||||
accel_limits = [float(x) for x in calc_cruise_accel_limits(v_ego)]
|
accel_limits = [float(x) for x in calc_cruise_accel_limits(v_ego, following)]
|
||||||
jerk_limits = [min(-0.1, accel_limits[0]), max(0.1, accel_limits[1])] # TODO: make a separate lookup for jerk tuning
|
jerk_limits = [min(-0.1, accel_limits[0]), max(0.1, accel_limits[1])] # TODO: make a separate lookup for jerk tuning
|
||||||
accel_limits_turns = limit_accel_in_turns(v_ego, sm['carState'].steeringAngle, accel_limits, self.CP)
|
accel_limits_turns = limit_accel_in_turns(v_ego, sm['carState'].steeringAngle, accel_limits, self.CP)
|
||||||
|
|
||||||
|
@ -242,3 +249,5 @@ class Planner():
|
||||||
v_acc_sol = self.v_acc_start + CP.radarTimeStep * (a_acc_sol + self.a_acc_start) / 2.0
|
v_acc_sol = self.v_acc_start + CP.radarTimeStep * (a_acc_sol + self.a_acc_start) / 2.0
|
||||||
self.v_acc_start = v_acc_sol
|
self.v_acc_start = v_acc_sol
|
||||||
self.a_acc_start = a_acc_sol
|
self.a_acc_start = a_acc_sol
|
||||||
|
|
||||||
|
self.first_loop = False
|
||||||
|
|
|
@ -38,7 +38,7 @@ def run_following_distance_simulation(v_lead, t_end=200.0):
|
||||||
first = True
|
first = True
|
||||||
while t < t_end:
|
while t < t_end:
|
||||||
# Run cruise control
|
# Run cruise control
|
||||||
accel_limits = [float(x) for x in calc_cruise_accel_limits(v_ego)]
|
accel_limits = [float(x) for x in calc_cruise_accel_limits(v_ego, False)]
|
||||||
jerk_limits = [min(-0.1, accel_limits[0]), max(0.1, accel_limits[1])]
|
jerk_limits = [min(-0.1, accel_limits[0]), max(0.1, accel_limits[1])]
|
||||||
v_cruise, a_cruise = speed_smoother(v_ego, a_ego, v_cruise_setpoint,
|
v_cruise, a_cruise = speed_smoother(v_ego, a_ego, v_cruise_setpoint,
|
||||||
accel_limits[1], accel_limits[0],
|
accel_limits[1], accel_limits[0],
|
||||||
|
|
|
@ -10,19 +10,23 @@ from selfdrive.swaglog import cloudlog
|
||||||
from common.params import Params, put_nonblocking
|
from common.params import Params, put_nonblocking
|
||||||
from common.transformations.model import model_height
|
from common.transformations.model import model_height
|
||||||
from common.transformations.camera import view_frame_from_device_frame, get_view_frame_from_road_frame, \
|
from common.transformations.camera import view_frame_from_device_frame, get_view_frame_from_road_frame, \
|
||||||
eon_intrinsics, get_calib_from_vp, H, W
|
get_calib_from_vp, H, W, FOCAL
|
||||||
|
|
||||||
MPH_TO_MS = 0.44704
|
MPH_TO_MS = 0.44704
|
||||||
MIN_SPEED_FILTER = 15 * MPH_TO_MS
|
MIN_SPEED_FILTER = 15 * MPH_TO_MS
|
||||||
|
MAX_SPEED_STD = 1.5
|
||||||
MAX_YAW_RATE_FILTER = np.radians(2) # per second
|
MAX_YAW_RATE_FILTER = np.radians(2) # per second
|
||||||
INPUTS_NEEDED = 300 # allow to update VP every so many frames
|
|
||||||
INPUTS_WANTED = 600 # We want a little bit more than we need for stability
|
# This is all 20Hz, blocks needed for efficiency
|
||||||
WRITE_CYCLES = 400 # write every 400 cycles
|
BLOCK_SIZE = 100
|
||||||
|
INPUTS_NEEDED = 5 # allow to update VP every so many frames
|
||||||
|
INPUTS_WANTED = 20 # We want a little bit more than we need for stability
|
||||||
|
WRITE_CYCLES = 10 # write every 1000 cycles
|
||||||
VP_INIT = np.array([W/2., H/2.])
|
VP_INIT = np.array([W/2., H/2.])
|
||||||
|
|
||||||
# These validity corners were chosen by looking at 1000
|
# These validity corners were chosen by looking at 1000
|
||||||
# and taking most extreme cases with some margin.
|
# and taking most extreme cases with some margin.
|
||||||
VP_VALIDITY_CORNERS = np.array([[W//2 - 150, 280], [W//2 + 150, 540]])
|
VP_VALIDITY_CORNERS = np.array([[W//2 - 120, 300], [W//2 + 120, 520]])
|
||||||
DEBUG = os.getenv("DEBUG") is not None
|
DEBUG = os.getenv("DEBUG") is not None
|
||||||
|
|
||||||
|
|
||||||
|
@ -31,13 +35,29 @@ def is_calibration_valid(vp):
|
||||||
vp[1] > VP_VALIDITY_CORNERS[0,1] and vp[1] < VP_VALIDITY_CORNERS[1,1]
|
vp[1] > VP_VALIDITY_CORNERS[0,1] and vp[1] < VP_VALIDITY_CORNERS[1,1]
|
||||||
|
|
||||||
|
|
||||||
|
def sanity_clip(vp):
|
||||||
|
if np.isnan(vp).any():
|
||||||
|
vp = VP_INIT
|
||||||
|
return [np.clip(vp[0], VP_VALIDITY_CORNERS[0,0] - 20, VP_VALIDITY_CORNERS[1,0] + 20),
|
||||||
|
np.clip(vp[1], VP_VALIDITY_CORNERS[0,1] - 20, VP_VALIDITY_CORNERS[1,1] + 20)]
|
||||||
|
|
||||||
|
|
||||||
|
def intrinsics_from_vp(vp):
|
||||||
|
return np.array([
|
||||||
|
[FOCAL, 0., vp[0]],
|
||||||
|
[ 0., FOCAL, vp[1]],
|
||||||
|
[ 0., 0., 1.]])
|
||||||
|
|
||||||
|
|
||||||
class Calibrator():
|
class Calibrator():
|
||||||
def __init__(self, param_put=False):
|
def __init__(self, param_put=False):
|
||||||
self.param_put = param_put
|
self.param_put = param_put
|
||||||
self.vp = copy.copy(VP_INIT)
|
self.vp = copy.copy(VP_INIT)
|
||||||
self.vps = []
|
self.vps = np.zeros((INPUTS_WANTED, 2))
|
||||||
|
self.idx = 0
|
||||||
|
self.block_idx = 0
|
||||||
|
self.valid_blocks = 0
|
||||||
self.cal_status = Calibration.UNCALIBRATED
|
self.cal_status = Calibration.UNCALIBRATED
|
||||||
self.write_counter = 0
|
|
||||||
self.just_calibrated = False
|
self.just_calibrated = False
|
||||||
|
|
||||||
# Read calibration
|
# Read calibration
|
||||||
|
@ -46,14 +66,19 @@ class Calibrator():
|
||||||
try:
|
try:
|
||||||
calibration_params = json.loads(calibration_params)
|
calibration_params = json.loads(calibration_params)
|
||||||
self.vp = np.array(calibration_params["vanishing_point"])
|
self.vp = np.array(calibration_params["vanishing_point"])
|
||||||
self.vps = np.tile(self.vp, (calibration_params['valid_points'], 1)).tolist()
|
if not np.isfinite(self.vp).all():
|
||||||
|
self.vp = copy.copy(VP_INIT)
|
||||||
|
self.vps = np.tile(self.vp, (INPUTS_WANTED, 1))
|
||||||
|
self.valid_blocks = calibration_params['valid_blocks']
|
||||||
|
if not np.isfinite(self.valid_blocks) or self.valid_blocks < 0:
|
||||||
|
self.valid_blocks = 0
|
||||||
self.update_status()
|
self.update_status()
|
||||||
except Exception:
|
except Exception:
|
||||||
cloudlog.exception("CalibrationParams file found but error encountered")
|
cloudlog.exception("CalibrationParams file found but error encountered")
|
||||||
|
|
||||||
def update_status(self):
|
def update_status(self):
|
||||||
start_status = self.cal_status
|
start_status = self.cal_status
|
||||||
if len(self.vps) < INPUTS_NEEDED:
|
if self.valid_blocks < INPUTS_NEEDED:
|
||||||
self.cal_status = Calibration.UNCALIBRATED
|
self.cal_status = Calibration.UNCALIBRATED
|
||||||
else:
|
else:
|
||||||
self.cal_status = Calibration.CALIBRATED if is_calibration_valid(self.vp) else Calibration.INVALID
|
self.cal_status = Calibration.CALIBRATED if is_calibration_valid(self.vp) else Calibration.INVALID
|
||||||
|
@ -63,19 +88,28 @@ class Calibrator():
|
||||||
if start_status == Calibration.UNCALIBRATED and end_status == Calibration.CALIBRATED:
|
if start_status == Calibration.UNCALIBRATED and end_status == Calibration.CALIBRATED:
|
||||||
self.just_calibrated = True
|
self.just_calibrated = True
|
||||||
|
|
||||||
def handle_cam_odom(self, log):
|
def handle_cam_odom(self, trans, rot, trans_std, rot_std):
|
||||||
trans, rot = log.trans, log.rot
|
if ((trans[0] > MIN_SPEED_FILTER) and
|
||||||
if np.linalg.norm(trans) > MIN_SPEED_FILTER and abs(rot[2]) < MAX_YAW_RATE_FILTER:
|
(trans_std[0] < MAX_SPEED_STD) and
|
||||||
new_vp = eon_intrinsics.dot(view_frame_from_device_frame.dot(trans))
|
(abs(rot[2]) < MAX_YAW_RATE_FILTER)):
|
||||||
|
# intrinsics are not eon intrinsics, since this is calibrated frame
|
||||||
|
intrinsics = intrinsics_from_vp(self.vp)
|
||||||
|
new_vp = intrinsics.dot(view_frame_from_device_frame.dot(trans))
|
||||||
new_vp = new_vp[:2]/new_vp[2]
|
new_vp = new_vp[:2]/new_vp[2]
|
||||||
self.vps.append(new_vp)
|
|
||||||
self.vps = self.vps[-INPUTS_WANTED:]
|
self.vps[self.block_idx] = (self.idx*self.vps[self.block_idx] + (BLOCK_SIZE - self.idx) * new_vp) / float(BLOCK_SIZE)
|
||||||
self.vp = np.mean(self.vps, axis=0)
|
self.idx = (self.idx + 1) % BLOCK_SIZE
|
||||||
|
if self.idx == 0:
|
||||||
|
self.block_idx += 1
|
||||||
|
self.valid_blocks = max(self.block_idx, self.valid_blocks)
|
||||||
|
self.block_idx = self.block_idx % INPUTS_WANTED
|
||||||
|
raw_vp = np.mean(self.vps[:max(1, self.valid_blocks)], axis=0)
|
||||||
|
self.vp = sanity_clip(raw_vp)
|
||||||
self.update_status()
|
self.update_status()
|
||||||
self.write_counter += 1
|
|
||||||
if self.param_put and (self.write_counter % WRITE_CYCLES == 0 or self.just_calibrated):
|
if self.param_put and ((self.idx == 0 and self.block_idx == 0) or self.just_calibrated):
|
||||||
cal_params = {"vanishing_point": list(self.vp),
|
cal_params = {"vanishing_point": list(self.vp),
|
||||||
"valid_points": len(self.vps)}
|
"valid_blocks": self.valid_blocks}
|
||||||
put_nonblocking("CalibrationParams", json.dumps(cal_params).encode('utf8'))
|
put_nonblocking("CalibrationParams", json.dumps(cal_params).encode('utf8'))
|
||||||
return new_vp
|
return new_vp
|
||||||
else:
|
else:
|
||||||
|
@ -88,7 +122,7 @@ class Calibrator():
|
||||||
cal_send = messaging.new_message()
|
cal_send = messaging.new_message()
|
||||||
cal_send.init('liveCalibration')
|
cal_send.init('liveCalibration')
|
||||||
cal_send.liveCalibration.calStatus = self.cal_status
|
cal_send.liveCalibration.calStatus = self.cal_status
|
||||||
cal_send.liveCalibration.calPerc = min(len(self.vps) * 100 // INPUTS_NEEDED, 100)
|
cal_send.liveCalibration.calPerc = min(100 * (self.valid_blocks * BLOCK_SIZE + self.idx) // (INPUTS_NEEDED * BLOCK_SIZE), 100)
|
||||||
cal_send.liveCalibration.extrinsicMatrix = [float(x) for x in extrinsic_matrix.flatten()]
|
cal_send.liveCalibration.extrinsicMatrix = [float(x) for x in extrinsic_matrix.flatten()]
|
||||||
cal_send.liveCalibration.rpyCalib = [float(x) for x in calib]
|
cal_send.liveCalibration.rpyCalib = [float(x) for x in calib]
|
||||||
|
|
||||||
|
@ -104,15 +138,22 @@ def calibrationd_thread(sm=None, pm=None):
|
||||||
|
|
||||||
calibrator = Calibrator(param_put=True)
|
calibrator = Calibrator(param_put=True)
|
||||||
|
|
||||||
# buffer with all the messages that still need to be input into the kalman
|
send_counter = 0
|
||||||
while 1:
|
while 1:
|
||||||
sm.update()
|
sm.update()
|
||||||
|
|
||||||
new_vp = calibrator.handle_cam_odom(sm['cameraOdometry'])
|
if sm.updated['cameraOdometry']:
|
||||||
|
new_vp = calibrator.handle_cam_odom(sm['cameraOdometry'].trans,
|
||||||
|
sm['cameraOdometry'].rot,
|
||||||
|
sm['cameraOdometry'].transStd,
|
||||||
|
sm['cameraOdometry'].rotStd)
|
||||||
if DEBUG and new_vp is not None:
|
if DEBUG and new_vp is not None:
|
||||||
print('got new vp', new_vp)
|
print('got new vp', new_vp)
|
||||||
|
|
||||||
calibrator.send_data(pm)
|
# decimate outputs for efficiency
|
||||||
|
if (send_counter % 5) == 0:
|
||||||
|
calibrator.send_data(pm)
|
||||||
|
send_counter += 1
|
||||||
|
|
||||||
|
|
||||||
def main(sm=None, pm=None):
|
def main(sm=None, pm=None):
|
||||||
|
|
|
@ -40,7 +40,7 @@ void Localizer::handle_sensor_events(capnp::List<cereal::SensorEventData>::Reade
|
||||||
}
|
}
|
||||||
|
|
||||||
void Localizer::handle_camera_odometry(cereal::CameraOdometry::Reader camera_odometry, double current_time) {
|
void Localizer::handle_camera_odometry(cereal::CameraOdometry::Reader camera_odometry, double current_time) {
|
||||||
double R = 100.0 * pow(camera_odometry.getRotStd()[2], 2);
|
double R = pow(30.0 *camera_odometry.getRotStd()[2], 2);
|
||||||
double meas = camera_odometry.getRot()[2];
|
double meas = camera_odometry.getRot()[2];
|
||||||
update_state(C_posenet, R, current_time, meas);
|
update_state(C_posenet, R, current_time, meas);
|
||||||
|
|
||||||
|
@ -73,7 +73,7 @@ Localizer::Localizer() {
|
||||||
0, 0, 0, 0,
|
0, 0, 0, 0,
|
||||||
0, pow(0.1, 2.0), 0, 0,
|
0, pow(0.1, 2.0), 0, 0,
|
||||||
0, 0, 0, 0,
|
0, 0, 0, 0,
|
||||||
0, 0, pow(0.0005 / 100.0, 2.0), 0;
|
0, 0, pow(0.005 / 100.0, 2.0), 0;
|
||||||
P <<
|
P <<
|
||||||
pow(100.0, 2.0), 0, 0, 0,
|
pow(100.0, 2.0), 0, 0, 0,
|
||||||
0, pow(100.0, 2.0), 0, 0,
|
0, pow(100.0, 2.0), 0, 0,
|
||||||
|
|
|
@ -1,6 +1,12 @@
|
||||||
Import('env', 'messaging', 'common', 'visionipc')
|
Import('env', 'arch', 'messaging', 'common', 'visionipc')
|
||||||
env.Program(['loggerd.cc', 'logger.c', 'raw_logger.cc', 'encoder.c'], LIBS=[
|
|
||||||
'zmq', 'czmq', 'capnp', 'kj', 'yaml-cpp', 'z',
|
src = ['loggerd.cc', 'logger.c']
|
||||||
|
libs = ['zmq', 'czmq', 'capnp', 'kj', 'yaml-cpp', 'z',
|
||||||
'avformat', 'avcodec', 'swscale', 'avutil',
|
'avformat', 'avcodec', 'swscale', 'avutil',
|
||||||
'OmxVenc', 'OmxCore', 'yuv',
|
'yuv', 'bz2', common, 'json', messaging, visionipc]
|
||||||
'bz2', 'cutils', common, 'json', messaging, visionipc])
|
|
||||||
|
if arch == "aarch64":
|
||||||
|
src += ['encoder.c', 'raw_logger.cc']
|
||||||
|
libs += ['OmxVenc', 'OmxCore', 'cutils']
|
||||||
|
|
||||||
|
env.Program(src, LIBS=libs)
|
||||||
|
|
|
@ -9,11 +9,21 @@ else:
|
||||||
SEGMENT_LENGTH = 60
|
SEGMENT_LENGTH = 60
|
||||||
|
|
||||||
|
|
||||||
def get_available_percent():
|
def get_available_percent(default=None):
|
||||||
try:
|
try:
|
||||||
statvfs = os.statvfs(ROOT)
|
statvfs = os.statvfs(ROOT)
|
||||||
available_percent = 100.0 * statvfs.f_bavail / statvfs.f_blocks
|
available_percent = 100.0 * statvfs.f_bavail / statvfs.f_blocks
|
||||||
except OSError:
|
except OSError:
|
||||||
available_percent = 100.0
|
available_percent = default
|
||||||
|
|
||||||
return available_percent
|
return available_percent
|
||||||
|
|
||||||
|
|
||||||
|
def get_available_bytes(default=None):
|
||||||
|
try:
|
||||||
|
statvfs = os.statvfs(ROOT)
|
||||||
|
available_bytes = statvfs.f_bavail * statvfs.f_frsize
|
||||||
|
except OSError:
|
||||||
|
available_bytes = default
|
||||||
|
|
||||||
|
return available_bytes
|
||||||
|
|
|
@ -3,15 +3,15 @@ import os
|
||||||
import shutil
|
import shutil
|
||||||
import threading
|
import threading
|
||||||
from selfdrive.swaglog import cloudlog
|
from selfdrive.swaglog import cloudlog
|
||||||
from selfdrive.loggerd.config import ROOT, get_available_percent
|
from selfdrive.loggerd.config import ROOT, get_available_bytes
|
||||||
from selfdrive.loggerd.uploader import listdir_by_creation
|
from selfdrive.loggerd.uploader import listdir_by_creation
|
||||||
|
|
||||||
|
|
||||||
def deleter_thread(exit_event):
|
def deleter_thread(exit_event):
|
||||||
while not exit_event.is_set():
|
while not exit_event.is_set():
|
||||||
available_percent = get_available_percent()
|
available_bytes = get_available_bytes()
|
||||||
|
|
||||||
if available_percent < 10.0:
|
if available_bytes is not None and available_bytes < (5 * 1024 * 1024 * 1024):
|
||||||
# remove the earliest directory we can
|
# remove the earliest directory we can
|
||||||
dirs = listdir_by_creation(ROOT)
|
dirs = listdir_by_creation(ROOT)
|
||||||
for delete_dir in dirs:
|
for delete_dir in dirs:
|
||||||
|
|
|
@ -25,7 +25,10 @@
|
||||||
#include <zmq.h>
|
#include <zmq.h>
|
||||||
#include <yaml-cpp/yaml.h>
|
#include <yaml-cpp/yaml.h>
|
||||||
#include <capnp/serialize.h>
|
#include <capnp/serialize.h>
|
||||||
|
|
||||||
|
#ifdef QCOM
|
||||||
#include <cutils/properties.h>
|
#include <cutils/properties.h>
|
||||||
|
#endif
|
||||||
|
|
||||||
#include "common/version.h"
|
#include "common/version.h"
|
||||||
#include "common/timing.h"
|
#include "common/timing.h"
|
||||||
|
@ -38,6 +41,11 @@
|
||||||
#include "logger.h"
|
#include "logger.h"
|
||||||
#include "messaging.hpp"
|
#include "messaging.hpp"
|
||||||
|
|
||||||
|
#ifndef QCOM
|
||||||
|
// no encoder on PC
|
||||||
|
#define DISABLE_ENCODER
|
||||||
|
#endif
|
||||||
|
|
||||||
|
|
||||||
#ifndef DISABLE_ENCODER
|
#ifndef DISABLE_ENCODER
|
||||||
#include "encoder.h"
|
#include "encoder.h"
|
||||||
|
@ -420,6 +428,7 @@ kj::Array<capnp::word> gen_init_data() {
|
||||||
|
|
||||||
init.setKernelVersion(util::read_file("/proc/version"));
|
init.setKernelVersion(util::read_file("/proc/version"));
|
||||||
|
|
||||||
|
#ifdef QCOM
|
||||||
{
|
{
|
||||||
std::vector<std::pair<std::string, std::string> > properties;
|
std::vector<std::pair<std::string, std::string> > properties;
|
||||||
property_list(append_property, (void*)&properties);
|
property_list(append_property, (void*)&properties);
|
||||||
|
@ -431,6 +440,7 @@ kj::Array<capnp::word> gen_init_data() {
|
||||||
lentry.setValue(properties[i].second);
|
lentry.setValue(properties[i].second);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
#endif
|
||||||
|
|
||||||
const char* dongle_id = getenv("DONGLE_ID");
|
const char* dongle_id = getenv("DONGLE_ID");
|
||||||
if (dongle_id) {
|
if (dongle_id) {
|
||||||
|
|
|
@ -1,6 +1,7 @@
|
||||||
import os
|
import os
|
||||||
import time
|
import time
|
||||||
import threading
|
import threading
|
||||||
|
import unittest
|
||||||
from collections import namedtuple
|
from collections import namedtuple
|
||||||
|
|
||||||
import selfdrive.loggerd.deleter as deleter
|
import selfdrive.loggerd.deleter as deleter
|
||||||
|
@ -8,7 +9,8 @@ from common.timeout import Timeout, TimeoutException
|
||||||
|
|
||||||
from selfdrive.loggerd.tests.loggerd_tests_common import UploaderTestCase
|
from selfdrive.loggerd.tests.loggerd_tests_common import UploaderTestCase
|
||||||
|
|
||||||
Stats = namedtuple("Stats", ['f_bavail', 'f_blocks'])
|
Stats = namedtuple("Stats", ['f_bavail', 'f_blocks', 'f_frsize'])
|
||||||
|
|
||||||
|
|
||||||
class TestDeleter(UploaderTestCase):
|
class TestDeleter(UploaderTestCase):
|
||||||
def fake_statvfs(self, d):
|
def fake_statvfs(self, d):
|
||||||
|
@ -17,7 +19,7 @@ class TestDeleter(UploaderTestCase):
|
||||||
def setUp(self):
|
def setUp(self):
|
||||||
self.f_type = "fcamera.hevc"
|
self.f_type = "fcamera.hevc"
|
||||||
super(TestDeleter, self).setUp()
|
super(TestDeleter, self).setUp()
|
||||||
self.fake_stats = Stats(f_bavail=0, f_blocks=10)
|
self.fake_stats = Stats(f_bavail=0, f_blocks=10, f_frsize=4096)
|
||||||
deleter.os.statvfs = self.fake_statvfs
|
deleter.os.statvfs = self.fake_statvfs
|
||||||
deleter.ROOT = self.root
|
deleter.ROOT = self.root
|
||||||
|
|
||||||
|
@ -68,7 +70,9 @@ class TestDeleter(UploaderTestCase):
|
||||||
def test_no_delete_when_available_space(self):
|
def test_no_delete_when_available_space(self):
|
||||||
f_path = self.make_file_with_data(self.seg_dir, self.f_type)
|
f_path = self.make_file_with_data(self.seg_dir, self.f_type)
|
||||||
|
|
||||||
self.fake_stats = Stats(f_bavail=10, f_blocks=10)
|
block_size = 4096
|
||||||
|
available = (10 * 1024 * 1024 * 1024) / block_size # 10GB free
|
||||||
|
self.fake_stats = Stats(f_bavail=available, f_blocks=10, f_frsize=block_size)
|
||||||
|
|
||||||
self.start_thread()
|
self.start_thread()
|
||||||
|
|
||||||
|
@ -98,3 +102,7 @@ class TestDeleter(UploaderTestCase):
|
||||||
self.join_thread()
|
self.join_thread()
|
||||||
|
|
||||||
self.assertTrue(os.path.exists(f_path), "File deleted when locked")
|
self.assertTrue(os.path.exists(f_path), "File deleted when locked")
|
||||||
|
|
||||||
|
|
||||||
|
if __name__ == "__main__":
|
||||||
|
unittest.main()
|
||||||
|
|
|
@ -68,13 +68,15 @@ def is_on_wifi():
|
||||||
# ConnectivityManager.getActiveNetworkInfo()
|
# ConnectivityManager.getActiveNetworkInfo()
|
||||||
try:
|
try:
|
||||||
result = android.parse_service_call_string(["connectivity", "2"])
|
result = android.parse_service_call_string(["connectivity", "2"])
|
||||||
|
if result is None:
|
||||||
|
return True
|
||||||
return 'WIFI' in result
|
return 'WIFI' in result
|
||||||
except AttributeError:
|
except AttributeError:
|
||||||
return False
|
return False
|
||||||
|
|
||||||
def is_on_hotspot():
|
def is_on_hotspot():
|
||||||
try:
|
try:
|
||||||
result = subprocess.check_output(["ifconfig", "wlan0"], encoding='utf8')
|
result = subprocess.check_output(["ifconfig", "wlan0"], stderr=subprocess.STDOUT, encoding='utf8')
|
||||||
result = re.findall(r"inet addr:((\d+\.){3}\d+)", result)[0][0]
|
result = re.findall(r"inet addr:((\d+\.){3}\d+)", result)[0][0]
|
||||||
|
|
||||||
is_android = result.startswith('192.168.43.')
|
is_android = result.startswith('192.168.43.')
|
||||||
|
|
|
@ -5,10 +5,12 @@ import sys
|
||||||
import fcntl
|
import fcntl
|
||||||
import errno
|
import errno
|
||||||
import signal
|
import signal
|
||||||
|
import shutil
|
||||||
import subprocess
|
import subprocess
|
||||||
import datetime
|
import datetime
|
||||||
|
|
||||||
from common.basedir import BASEDIR
|
from common.basedir import BASEDIR
|
||||||
|
from common.android import ANDROID
|
||||||
sys.path.append(os.path.join(BASEDIR, "pyextra"))
|
sys.path.append(os.path.join(BASEDIR, "pyextra"))
|
||||||
os.environ['BASEDIR'] = BASEDIR
|
os.environ['BASEDIR'] = BASEDIR
|
||||||
|
|
||||||
|
@ -21,7 +23,7 @@ try:
|
||||||
except FileExistsError:
|
except FileExistsError:
|
||||||
pass
|
pass
|
||||||
|
|
||||||
if os.path.isfile('/EON'):
|
if ANDROID:
|
||||||
os.chmod("/dev/shm", 0o777)
|
os.chmod("/dev/shm", 0o777)
|
||||||
|
|
||||||
def unblock_stdout():
|
def unblock_stdout():
|
||||||
|
@ -74,7 +76,11 @@ if not prebuilt:
|
||||||
# run scons
|
# run scons
|
||||||
env = os.environ.copy()
|
env = os.environ.copy()
|
||||||
env['SCONS_PROGRESS'] = "1"
|
env['SCONS_PROGRESS'] = "1"
|
||||||
scons = subprocess.Popen(["scons", "-j4"], cwd=BASEDIR, env=env, stderr=subprocess.PIPE)
|
env['SCONS_CACHE'] = "1"
|
||||||
|
|
||||||
|
nproc = os.cpu_count()
|
||||||
|
j_flag = "" if nproc is None else "-j%d" % (nproc - 1)
|
||||||
|
scons = subprocess.Popen(["scons", j_flag], cwd=BASEDIR, env=env, stderr=subprocess.PIPE)
|
||||||
|
|
||||||
# Read progress from stderr and update spinner
|
# Read progress from stderr and update spinner
|
||||||
while scons.poll() is None:
|
while scons.poll() is None:
|
||||||
|
@ -96,8 +102,12 @@ if not prebuilt:
|
||||||
|
|
||||||
if scons.returncode != 0:
|
if scons.returncode != 0:
|
||||||
if retry:
|
if retry:
|
||||||
print("scons build failed, make clean")
|
print("scons build failed, cleaning in")
|
||||||
|
for i in range(3,-1,-1):
|
||||||
|
print("....%d" % i)
|
||||||
|
time.sleep(1)
|
||||||
subprocess.check_call(["scons", "-c"], cwd=BASEDIR, env=env)
|
subprocess.check_call(["scons", "-c"], cwd=BASEDIR, env=env)
|
||||||
|
shutil.rmtree("/tmp/scons_cache")
|
||||||
else:
|
else:
|
||||||
raise RuntimeError("scons build failed")
|
raise RuntimeError("scons build failed")
|
||||||
else:
|
else:
|
||||||
|
@ -139,11 +149,13 @@ managed_processes = {
|
||||||
"paramsd": ("selfdrive/locationd", ["./paramsd"]),
|
"paramsd": ("selfdrive/locationd", ["./paramsd"]),
|
||||||
"camerad": ("selfdrive/camerad", ["./camerad"]),
|
"camerad": ("selfdrive/camerad", ["./camerad"]),
|
||||||
"sensord": ("selfdrive/sensord", ["./sensord"]),
|
"sensord": ("selfdrive/sensord", ["./sensord"]),
|
||||||
|
"clocksd": ("selfdrive/clocksd", ["./clocksd"]),
|
||||||
"gpsd": ("selfdrive/sensord", ["./gpsd"]),
|
"gpsd": ("selfdrive/sensord", ["./gpsd"]),
|
||||||
"updated": "selfdrive.updated",
|
"updated": "selfdrive.updated",
|
||||||
"monitoringd": ("selfdrive/modeld", ["./monitoringd"]),
|
"monitoringd": ("selfdrive/modeld", ["./monitoringd"]),
|
||||||
"modeld": ("selfdrive/modeld", ["./modeld"]),
|
"modeld": ("selfdrive/modeld", ["./modeld"]),
|
||||||
}
|
}
|
||||||
|
|
||||||
daemon_processes = {
|
daemon_processes = {
|
||||||
"manage_athenad": ("selfdrive.athena.manage_athenad", "AthenadPid"),
|
"manage_athenad": ("selfdrive.athena.manage_athenad", "AthenadPid"),
|
||||||
}
|
}
|
||||||
|
@ -161,32 +173,42 @@ interrupt_processes = []
|
||||||
# processes to end with SIGKILL instead of SIGTERM
|
# processes to end with SIGKILL instead of SIGTERM
|
||||||
kill_processes = ['sensord', 'paramsd']
|
kill_processes = ['sensord', 'paramsd']
|
||||||
|
|
||||||
|
# processes to end if thermal conditions exceed Green parameters
|
||||||
|
green_temp_processes = ['uploader']
|
||||||
|
|
||||||
persistent_processes = [
|
persistent_processes = [
|
||||||
'thermald',
|
'thermald',
|
||||||
'logmessaged',
|
'logmessaged',
|
||||||
'logcatd',
|
|
||||||
'tombstoned',
|
|
||||||
'uploader',
|
|
||||||
'ui',
|
'ui',
|
||||||
'updated',
|
'uploader',
|
||||||
]
|
]
|
||||||
|
if ANDROID:
|
||||||
|
persistent_processes += [
|
||||||
|
'logcatd',
|
||||||
|
'tombstoned',
|
||||||
|
'updated',
|
||||||
|
]
|
||||||
|
|
||||||
car_started_processes = [
|
car_started_processes = [
|
||||||
'controlsd',
|
'controlsd',
|
||||||
'plannerd',
|
'plannerd',
|
||||||
'loggerd',
|
'loggerd',
|
||||||
'sensord',
|
|
||||||
'radard',
|
'radard',
|
||||||
'calibrationd',
|
'calibrationd',
|
||||||
'paramsd',
|
'paramsd',
|
||||||
'camerad',
|
'camerad',
|
||||||
'modeld',
|
'modeld',
|
||||||
'monitoringd',
|
|
||||||
'proclogd',
|
'proclogd',
|
||||||
'ubloxd',
|
'ubloxd',
|
||||||
'gpsd',
|
|
||||||
'deleter',
|
|
||||||
]
|
]
|
||||||
|
if ANDROID:
|
||||||
|
car_started_processes += [
|
||||||
|
'sensord',
|
||||||
|
'clocksd',
|
||||||
|
'gpsd',
|
||||||
|
'monitoringd',
|
||||||
|
'deleter',
|
||||||
|
]
|
||||||
|
|
||||||
def register_managed_process(name, desc, car_started=False):
|
def register_managed_process(name, desc, car_started=False):
|
||||||
global managed_processes, car_started_processes, persistent_processes
|
global managed_processes, car_started_processes, persistent_processes
|
||||||
|
@ -303,7 +325,8 @@ def kill_managed_process(name):
|
||||||
def cleanup_all_processes(signal, frame):
|
def cleanup_all_processes(signal, frame):
|
||||||
cloudlog.info("caught ctrl-c %s %s" % (signal, frame))
|
cloudlog.info("caught ctrl-c %s %s" % (signal, frame))
|
||||||
|
|
||||||
pm_apply_packages('disable')
|
if ANDROID:
|
||||||
|
pm_apply_packages('disable')
|
||||||
|
|
||||||
for name in list(running.keys()):
|
for name in list(running.keys()):
|
||||||
kill_managed_process(name)
|
kill_managed_process(name)
|
||||||
|
@ -365,8 +388,9 @@ def manager_thread():
|
||||||
start_managed_process(p)
|
start_managed_process(p)
|
||||||
|
|
||||||
# start frame
|
# start frame
|
||||||
pm_apply_packages('enable')
|
if ANDROID:
|
||||||
start_frame()
|
pm_apply_packages('enable')
|
||||||
|
start_frame()
|
||||||
|
|
||||||
if os.getenv("NOBOARD") is None:
|
if os.getenv("NOBOARD") is None:
|
||||||
start_managed_process("pandad")
|
start_managed_process("pandad")
|
||||||
|
@ -376,11 +400,15 @@ def manager_thread():
|
||||||
while 1:
|
while 1:
|
||||||
msg = messaging.recv_sock(thermal_sock, wait=True)
|
msg = messaging.recv_sock(thermal_sock, wait=True)
|
||||||
|
|
||||||
# uploader is gated based on the phone temperature
|
# heavyweight batch processes are gated on favorable thermal conditions
|
||||||
if msg.thermal.thermalStatus >= ThermalStatus.yellow:
|
if msg.thermal.thermalStatus >= ThermalStatus.yellow:
|
||||||
kill_managed_process("uploader")
|
for p in green_temp_processes:
|
||||||
|
if p in persistent_processes:
|
||||||
|
kill_managed_process(p)
|
||||||
else:
|
else:
|
||||||
start_managed_process("uploader")
|
for p in green_temp_processes:
|
||||||
|
if p in persistent_processes:
|
||||||
|
start_managed_process(p)
|
||||||
|
|
||||||
if msg.thermal.freeSpace < 0.05:
|
if msg.thermal.freeSpace < 0.05:
|
||||||
logger_dead = True
|
logger_dead = True
|
||||||
|
@ -430,13 +458,6 @@ def main():
|
||||||
# disable bluetooth
|
# disable bluetooth
|
||||||
os.system('service call bluetooth_manager 8')
|
os.system('service call bluetooth_manager 8')
|
||||||
|
|
||||||
# support additional internal only extensions
|
|
||||||
try:
|
|
||||||
import selfdrive.manager_extensions
|
|
||||||
selfdrive.manager_extensions.register(register_managed_process) # pylint: disable=no-member
|
|
||||||
except ImportError:
|
|
||||||
pass
|
|
||||||
|
|
||||||
params = Params()
|
params = Params()
|
||||||
params.manager_start()
|
params.manager_start()
|
||||||
|
|
||||||
|
@ -480,7 +501,8 @@ def main():
|
||||||
if params.get("Passive") is None:
|
if params.get("Passive") is None:
|
||||||
raise Exception("Passive must be set to continue")
|
raise Exception("Passive must be set to continue")
|
||||||
|
|
||||||
update_apks()
|
if ANDROID:
|
||||||
|
update_apks()
|
||||||
manager_init()
|
manager_init()
|
||||||
manager_prepare(spinner)
|
manager_prepare(spinner)
|
||||||
spinner.close()
|
spinner.close()
|
||||||
|
|
|
@ -1,25 +1,36 @@
|
||||||
Import('env', 'arch', 'messaging', 'common', 'gpucommon', 'visionipc')
|
Import('env', 'arch', 'messaging', 'common', 'gpucommon', 'visionipc')
|
||||||
|
lenv = env.Clone()
|
||||||
|
|
||||||
libs = [messaging, common, 'OpenCL', 'SNPE', 'capnp', 'zmq', 'kj', 'yuv', gpucommon, visionipc]
|
libs = [messaging, common, 'OpenCL', 'SNPE', 'capnp', 'zmq', 'kj', 'yuv', gpucommon, visionipc]
|
||||||
|
|
||||||
|
common_src = [
|
||||||
|
"models/commonmodel.c",
|
||||||
|
"runners/snpemodel.cc",
|
||||||
|
"transforms/loadyuv.c",
|
||||||
|
"transforms/transform.c"]
|
||||||
|
|
||||||
if arch == "aarch64":
|
if arch == "aarch64":
|
||||||
libs += ['gsl', 'CB', 'gnustl_shared']
|
libs += ['gsl', 'CB', 'gnustl_shared']
|
||||||
else:
|
else:
|
||||||
libs += ['symphony-cpu', 'pthread']
|
libs += ['symphony-cpu', 'pthread']
|
||||||
|
|
||||||
common = env.Object([
|
if FindFile('libtensorflow.so', env['LIBPATH']):
|
||||||
"models/commonmodel.c",
|
# for tensorflow support
|
||||||
"runners/snpemodel.cc",
|
common_src += ['runners/tfmodel.cc']
|
||||||
"transforms/loadyuv.c",
|
libs += ['tensorflow']
|
||||||
"transforms/transform.c"])
|
# tell runners to use it
|
||||||
|
lenv['CFLAGS'].append("-DUSE_TF_MODEL")
|
||||||
|
lenv['CXXFLAGS'].append("-DUSE_TF_MODEL")
|
||||||
|
|
||||||
env.Program('_monitoringd', [
|
common = lenv.Object(common_src)
|
||||||
|
|
||||||
|
lenv.Program('_monitoringd', [
|
||||||
"monitoringd.cc",
|
"monitoringd.cc",
|
||||||
"models/monitoring.cc",
|
"models/monitoring.cc",
|
||||||
]+common, LIBS=libs)
|
]+common, LIBS=libs)
|
||||||
|
|
||||||
env.Program('_modeld', [
|
lenv.Program('_modeld', [
|
||||||
"modeld.cc",
|
"modeld.cc",
|
||||||
"models/driving.cc",
|
"models/driving.cc",
|
||||||
"models/posenet.cc",
|
|
||||||
]+common, LIBS=libs)
|
]+common, LIBS=libs)
|
||||||
|
|
||||||
|
|
|
@ -1,4 +1,4 @@
|
||||||
#!/bin/sh
|
#!/bin/sh
|
||||||
export LD_LIBRARY_PATH="/data/pythonpath/phonelibs/snpe/aarch64-android-clang3.8/:$LD_LIBRARY_PATH"
|
export LD_LIBRARY_PATH="/data/pythonpath/phonelibs/snpe/aarch64-android-clang3.8/:/home/batman/one/phonelibs/snpe/x86_64-linux-clang:$LD_LIBRARY_PATH"
|
||||||
exec ./_modeld
|
exec ./_modeld
|
||||||
|
|
||||||
|
|
|
@ -1,18 +1,11 @@
|
||||||
#include <stdio.h>
|
#include <stdio.h>
|
||||||
#include <stdlib.h>
|
#include <stdlib.h>
|
||||||
|
|
||||||
#ifdef QCOM
|
|
||||||
#include <eigen3/Eigen/Dense>
|
|
||||||
#else
|
|
||||||
#include <Eigen/Dense>
|
|
||||||
#endif
|
|
||||||
|
|
||||||
#include "common/visionbuf.h"
|
#include "common/visionbuf.h"
|
||||||
#include "common/visionipc.h"
|
#include "common/visionipc.h"
|
||||||
#include "common/swaglog.h"
|
#include "common/swaglog.h"
|
||||||
|
|
||||||
#include "models/driving.h"
|
#include "models/driving.h"
|
||||||
#include "models/posenet.h"
|
|
||||||
|
|
||||||
volatile sig_atomic_t do_exit = 0;
|
volatile sig_atomic_t do_exit = 0;
|
||||||
|
|
||||||
|
@ -122,13 +115,26 @@ int main(int argc, char **argv) {
|
||||||
cl_command_queue q;
|
cl_command_queue q;
|
||||||
{
|
{
|
||||||
// TODO: refactor this
|
// TODO: refactor this
|
||||||
cl_platform_id platform_id = NULL;
|
cl_platform_id platform_id[2];
|
||||||
cl_uint num_devices;
|
cl_uint num_devices;
|
||||||
cl_uint num_platforms;
|
cl_uint num_platforms;
|
||||||
|
|
||||||
err = clGetPlatformIDs(1, &platform_id, &num_platforms);
|
err = clGetPlatformIDs(sizeof(platform_id)/sizeof(cl_platform_id), platform_id, &num_platforms);
|
||||||
assert(err == 0);
|
assert(err == 0);
|
||||||
err = clGetDeviceIDs(platform_id, CL_DEVICE_TYPE_DEFAULT, 1,
|
|
||||||
|
#ifdef QCOM
|
||||||
|
int clPlatform = 0;
|
||||||
|
#else
|
||||||
|
// don't use nvidia on pc, it's broken
|
||||||
|
// TODO: write this nicely
|
||||||
|
int clPlatform = num_platforms-1;
|
||||||
|
#endif
|
||||||
|
|
||||||
|
char cBuffer[1024];
|
||||||
|
clGetPlatformInfo(platform_id[clPlatform], CL_PLATFORM_NAME, sizeof(cBuffer), &cBuffer, NULL);
|
||||||
|
LOGD("got %d opencl platform(s), using %s", num_platforms, cBuffer);
|
||||||
|
|
||||||
|
err = clGetDeviceIDs(platform_id[clPlatform], CL_DEVICE_TYPE_DEFAULT, 1,
|
||||||
&device_id, &num_devices);
|
&device_id, &num_devices);
|
||||||
assert(err == 0);
|
assert(err == 0);
|
||||||
|
|
||||||
|
@ -141,9 +147,7 @@ int main(int argc, char **argv) {
|
||||||
|
|
||||||
// init the models
|
// init the models
|
||||||
ModelState model;
|
ModelState model;
|
||||||
PosenetState posenet;
|
|
||||||
model_init(&model, device_id, context, true);
|
model_init(&model, device_id, context, true);
|
||||||
posenet_init(&posenet);
|
|
||||||
LOGW("models loaded, modeld starting");
|
LOGW("models loaded, modeld starting");
|
||||||
|
|
||||||
// debayering does a 2x downscale
|
// debayering does a 2x downscale
|
||||||
|
@ -213,54 +217,18 @@ int main(int argc, char **argv) {
|
||||||
// TODO: don't make copies!
|
// TODO: don't make copies!
|
||||||
memcpy(yuv_ion.addr, buf->addr, buf_info.buf_len);
|
memcpy(yuv_ion.addr, buf->addr, buf_info.buf_len);
|
||||||
|
|
||||||
ModelData model_buf =
|
ModelDataRaw model_buf =
|
||||||
model_eval_frame(&model, q, yuv_cl, buf_info.width, buf_info.height,
|
model_eval_frame(&model, q, yuv_cl, buf_info.width, buf_info.height,
|
||||||
model_transform, NULL, vec_desire);
|
model_transform, NULL, vec_desire);
|
||||||
mt2 = millis_since_boot();
|
mt2 = millis_since_boot();
|
||||||
|
|
||||||
model_publish(model_sock, extra.frame_id, model_buf, extra.timestamp_eof);
|
model_publish(model_sock, extra.frame_id, model_buf, extra.timestamp_eof);
|
||||||
|
posenet_publish(posenet_sock, extra.frame_id, model_buf, extra.timestamp_eof);
|
||||||
|
|
||||||
LOGD("model process: %.2fms, from last %.2fms", mt2-mt1, mt1-last);
|
LOGD("model process: %.2fms, from last %.2fms", mt2-mt1, mt1-last);
|
||||||
last = mt1;
|
last = mt1;
|
||||||
}
|
}
|
||||||
|
|
||||||
// push the frame to the posenet
|
|
||||||
// TODO: This doesn't always have to run
|
|
||||||
double pt1 = 0, pt2 = 0, pt3 = 0;
|
|
||||||
pt1 = millis_since_boot();
|
|
||||||
posenet_push(&posenet, (uint8_t*)buf->addr, buf_info.width);
|
|
||||||
pt2 = millis_since_boot();
|
|
||||||
|
|
||||||
// posenet runs every 5
|
|
||||||
if (extra.frame_id % 5 == 0) {
|
|
||||||
posenet_eval(&posenet);
|
|
||||||
|
|
||||||
// send posenet event
|
|
||||||
{
|
|
||||||
capnp::MallocMessageBuilder msg;
|
|
||||||
cereal::Event::Builder event = msg.initRoot<cereal::Event>();
|
|
||||||
event.setLogMonoTime(nanos_since_boot());
|
|
||||||
|
|
||||||
auto posenetd = event.initCameraOdometry();
|
|
||||||
kj::ArrayPtr<const float> trans_vs(&posenet.output[0], 3);
|
|
||||||
posenetd.setTrans(trans_vs);
|
|
||||||
kj::ArrayPtr<const float> rot_vs(&posenet.output[3], 3);
|
|
||||||
posenetd.setRot(rot_vs);
|
|
||||||
kj::ArrayPtr<const float> trans_std_vs(&posenet.output[6], 3);
|
|
||||||
posenetd.setTransStd(trans_std_vs);
|
|
||||||
kj::ArrayPtr<const float> rot_std_vs(&posenet.output[9], 3);
|
|
||||||
posenetd.setRotStd(rot_std_vs);
|
|
||||||
posenetd.setTimestampEof(extra.timestamp_eof);
|
|
||||||
posenetd.setFrameId(extra.frame_id);
|
|
||||||
|
|
||||||
auto words = capnp::messageToFlatArray(msg);
|
|
||||||
auto bytes = words.asBytes();
|
|
||||||
posenet_sock->send((char*)bytes.begin(), bytes.size());
|
|
||||||
}
|
|
||||||
pt3 = millis_since_boot();
|
|
||||||
LOGD("pre: %.2fms | posenet: %.2fms", (pt2-pt1), (pt3-pt1));
|
|
||||||
}
|
|
||||||
|
|
||||||
}
|
}
|
||||||
visionbuf_free(&yuv_ion);
|
visionbuf_free(&yuv_ion);
|
||||||
}
|
}
|
||||||
|
@ -268,9 +236,7 @@ int main(int argc, char **argv) {
|
||||||
visionstream_destroy(&stream);
|
visionstream_destroy(&stream);
|
||||||
|
|
||||||
delete model_sock;
|
delete model_sock;
|
||||||
delete posenet_sock;
|
|
||||||
|
|
||||||
posenet_free(&posenet);
|
|
||||||
model_free(&model);
|
model_free(&model);
|
||||||
|
|
||||||
LOG("joining live_thread");
|
LOG("joining live_thread");
|
||||||
|
|
|
@ -5,57 +5,57 @@
|
||||||
#include "common/mat.h"
|
#include "common/mat.h"
|
||||||
#include "common/timing.h"
|
#include "common/timing.h"
|
||||||
|
|
||||||
void model_input_init(ModelInput* s, int width, int height,
|
void frame_init(ModelFrame* frame, int width, int height,
|
||||||
cl_device_id device_id, cl_context context) {
|
cl_device_id device_id, cl_context context) {
|
||||||
int err;
|
int err;
|
||||||
s->device_id = device_id;
|
frame->device_id = device_id;
|
||||||
s->context = context;
|
frame->context = context;
|
||||||
|
|
||||||
transform_init(&s->transform, context, device_id);
|
transform_init(&frame->transform, context, device_id);
|
||||||
s->transformed_width = width;
|
frame->transformed_width = width;
|
||||||
s->transformed_height = height;
|
frame->transformed_height = height;
|
||||||
|
|
||||||
s->transformed_y_cl = clCreateBuffer(s->context, CL_MEM_READ_WRITE,
|
frame->transformed_y_cl = clCreateBuffer(frame->context, CL_MEM_READ_WRITE,
|
||||||
s->transformed_width*s->transformed_height, NULL, &err);
|
frame->transformed_width*frame->transformed_height, NULL, &err);
|
||||||
assert(err == 0);
|
assert(err == 0);
|
||||||
s->transformed_u_cl = clCreateBuffer(s->context, CL_MEM_READ_WRITE,
|
frame->transformed_u_cl = clCreateBuffer(frame->context, CL_MEM_READ_WRITE,
|
||||||
(s->transformed_width/2)*(s->transformed_height/2), NULL, &err);
|
(frame->transformed_width/2)*(frame->transformed_height/2), NULL, &err);
|
||||||
assert(err == 0);
|
assert(err == 0);
|
||||||
s->transformed_v_cl = clCreateBuffer(s->context, CL_MEM_READ_WRITE,
|
frame->transformed_v_cl = clCreateBuffer(frame->context, CL_MEM_READ_WRITE,
|
||||||
(s->transformed_width/2)*(s->transformed_height/2), NULL, &err);
|
(frame->transformed_width/2)*(frame->transformed_height/2), NULL, &err);
|
||||||
assert(err == 0);
|
assert(err == 0);
|
||||||
|
|
||||||
s->net_input_size = ((width*height*3)/2)*sizeof(float);
|
frame->net_input_size = ((width*height*3)/2)*sizeof(float);
|
||||||
s->net_input = clCreateBuffer(s->context, CL_MEM_READ_WRITE,
|
frame->net_input = clCreateBuffer(frame->context, CL_MEM_READ_WRITE,
|
||||||
s->net_input_size, (void*)NULL, &err);
|
frame->net_input_size, (void*)NULL, &err);
|
||||||
assert(err == 0);
|
assert(err == 0);
|
||||||
|
|
||||||
loadyuv_init(&s->loadyuv, context, device_id, s->transformed_width, s->transformed_height);
|
loadyuv_init(&frame->loadyuv, context, device_id, frame->transformed_width, frame->transformed_height);
|
||||||
}
|
}
|
||||||
|
|
||||||
float *model_input_prepare(ModelInput* s, cl_command_queue q,
|
float *frame_prepare(ModelFrame* frame, cl_command_queue q,
|
||||||
cl_mem yuv_cl, int width, int height,
|
cl_mem yuv_cl, int width, int height,
|
||||||
mat3 transform) {
|
mat3 transform) {
|
||||||
int err;
|
int err;
|
||||||
int i = 0;
|
int i = 0;
|
||||||
transform_queue(&s->transform, q,
|
transform_queue(&frame->transform, q,
|
||||||
yuv_cl, width, height,
|
yuv_cl, width, height,
|
||||||
s->transformed_y_cl, s->transformed_u_cl, s->transformed_v_cl,
|
frame->transformed_y_cl, frame->transformed_u_cl, frame->transformed_v_cl,
|
||||||
s->transformed_width, s->transformed_height,
|
frame->transformed_width, frame->transformed_height,
|
||||||
transform);
|
transform);
|
||||||
loadyuv_queue(&s->loadyuv, q,
|
loadyuv_queue(&frame->loadyuv, q,
|
||||||
s->transformed_y_cl, s->transformed_u_cl, s->transformed_v_cl,
|
frame->transformed_y_cl, frame->transformed_u_cl, frame->transformed_v_cl,
|
||||||
s->net_input);
|
frame->net_input);
|
||||||
float *net_input_buf = (float *)clEnqueueMapBuffer(q, s->net_input, CL_TRUE,
|
float *net_input_buf = (float *)clEnqueueMapBuffer(q, frame->net_input, CL_TRUE,
|
||||||
CL_MAP_READ, 0, s->net_input_size,
|
CL_MAP_READ, 0, frame->net_input_size,
|
||||||
0, NULL, NULL, &err);
|
0, NULL, NULL, &err);
|
||||||
clFinish(q);
|
clFinish(q);
|
||||||
return net_input_buf;
|
return net_input_buf;
|
||||||
}
|
}
|
||||||
|
|
||||||
void model_input_free(ModelInput* s) {
|
void frame_free(ModelFrame* frame) {
|
||||||
transform_destroy(&s->transform);
|
transform_destroy(&frame->transform);
|
||||||
loadyuv_destroy(&s->loadyuv);
|
loadyuv_destroy(&frame->loadyuv);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
|
@ -4,7 +4,6 @@
|
||||||
#include <CL/cl.h>
|
#include <CL/cl.h>
|
||||||
|
|
||||||
#include "common/mat.h"
|
#include "common/mat.h"
|
||||||
#include "common/modeldata.h"
|
|
||||||
#include "transforms/transform.h"
|
#include "transforms/transform.h"
|
||||||
#include "transforms/loadyuv.h"
|
#include "transforms/loadyuv.h"
|
||||||
|
|
||||||
|
@ -15,7 +14,7 @@ extern "C" {
|
||||||
float softplus(float input);
|
float softplus(float input);
|
||||||
float sigmoid(float input);
|
float sigmoid(float input);
|
||||||
|
|
||||||
typedef struct ModelInput {
|
typedef struct ModelFrame {
|
||||||
cl_device_id device_id;
|
cl_device_id device_id;
|
||||||
cl_context context;
|
cl_context context;
|
||||||
|
|
||||||
|
@ -26,14 +25,14 @@ typedef struct ModelInput {
|
||||||
LoadYUVState loadyuv;
|
LoadYUVState loadyuv;
|
||||||
cl_mem net_input;
|
cl_mem net_input;
|
||||||
size_t net_input_size;
|
size_t net_input_size;
|
||||||
} ModelInput;
|
} ModelFrame;
|
||||||
|
|
||||||
void model_input_init(ModelInput* s, int width, int height,
|
void frame_init(ModelFrame* frame, int width, int height,
|
||||||
cl_device_id device_id, cl_context context);
|
cl_device_id device_id, cl_context context);
|
||||||
float *model_input_prepare(ModelInput* s, cl_command_queue q,
|
float *frame_prepare(ModelFrame* frame, cl_command_queue q,
|
||||||
cl_mem yuv_cl, int width, int height,
|
cl_mem yuv_cl, int width, int height,
|
||||||
mat3 transform);
|
mat3 transform);
|
||||||
void model_input_free(ModelInput* s);
|
void frame_free(ModelFrame* frame);
|
||||||
|
|
||||||
#ifdef __cplusplus
|
#ifdef __cplusplus
|
||||||
}
|
}
|
||||||
|
|
|
@ -2,26 +2,20 @@
|
||||||
#include <assert.h>
|
#include <assert.h>
|
||||||
#include <fcntl.h>
|
#include <fcntl.h>
|
||||||
#include <unistd.h>
|
#include <unistd.h>
|
||||||
|
|
||||||
#ifdef QCOM
|
|
||||||
#include <eigen3/Eigen/Dense>
|
|
||||||
#else
|
|
||||||
#include <Eigen/Dense>
|
|
||||||
#endif
|
|
||||||
|
|
||||||
#include "common/timing.h"
|
#include "common/timing.h"
|
||||||
#include "driving.h"
|
#include "driving.h"
|
||||||
|
|
||||||
#define MODEL_WIDTH 512
|
|
||||||
#define MODEL_HEIGHT 256
|
|
||||||
#define MODEL_NAME "driving_model_dlc"
|
|
||||||
|
|
||||||
#define LEAD_MDN_N 5 // probs for 5 groups
|
#define PATH_IDX 0
|
||||||
#define MDN_VALS 4 // output xyva for each lead group
|
#define LL_IDX PATH_IDX + MODEL_PATH_DISTANCE*2
|
||||||
#define SELECTION 3 //output 3 group (lead now, in 2s and 6s)
|
#define RL_IDX LL_IDX + MODEL_PATH_DISTANCE*2 + 1
|
||||||
#define MDN_GROUP_SIZE 11
|
#define LEAD_IDX RL_IDX + MODEL_PATH_DISTANCE*2 + 1
|
||||||
#define SPEED_BUCKETS 100
|
#define LONG_X_IDX LEAD_IDX + MDN_GROUP_SIZE*LEAD_MDN_N + SELECTION
|
||||||
#define OUTPUT_SIZE ((MODEL_PATH_DISTANCE*2) + (2*(MODEL_PATH_DISTANCE*2 + 1)) + MDN_GROUP_SIZE*LEAD_MDN_N + SELECTION + OTHER_META_SIZE + DESIRE_PRED_SIZE)
|
#define LONG_V_IDX LONG_X_IDX + TIME_DISTANCE*2
|
||||||
|
#define LONG_A_IDX LONG_V_IDX + TIME_DISTANCE*2
|
||||||
|
#define META_IDX LONG_A_IDX + TIME_DISTANCE*2
|
||||||
|
#define POSE_IDX META_IDX + OTHER_META_SIZE + DESIRE_PRED_SIZE
|
||||||
|
#define OUTPUT_SIZE POSE_IDX + POSE_SIZE
|
||||||
#ifdef TEMPORAL
|
#ifdef TEMPORAL
|
||||||
#define TEMPORAL_SIZE 512
|
#define TEMPORAL_SIZE 512
|
||||||
#else
|
#else
|
||||||
|
@ -33,15 +27,19 @@
|
||||||
Eigen::Matrix<float, MODEL_PATH_DISTANCE, POLYFIT_DEGREE - 1> vander;
|
Eigen::Matrix<float, MODEL_PATH_DISTANCE, POLYFIT_DEGREE - 1> vander;
|
||||||
|
|
||||||
void model_init(ModelState* s, cl_device_id device_id, cl_context context, int temporal) {
|
void model_init(ModelState* s, cl_device_id device_id, cl_context context, int temporal) {
|
||||||
model_input_init(&s->in, MODEL_WIDTH, MODEL_HEIGHT, device_id, context);
|
frame_init(&s->frame, MODEL_WIDTH, MODEL_HEIGHT, device_id, context);
|
||||||
|
s->input_frames = (float*)calloc(MODEL_FRAME_SIZE * 2, sizeof(float));
|
||||||
|
|
||||||
const int output_size = OUTPUT_SIZE + TEMPORAL_SIZE;
|
const int output_size = OUTPUT_SIZE + TEMPORAL_SIZE;
|
||||||
s->output = (float*)malloc(output_size * sizeof(float));
|
s->output = (float*)calloc(output_size, sizeof(float));
|
||||||
memset(s->output, 0, output_size * sizeof(float));
|
|
||||||
s->m = new DefaultRunModel("../../models/driving_model.dlc", s->output, output_size, USE_GPU_RUNTIME);
|
s->m = new DefaultRunModel("../../models/supercombo.dlc", s->output, output_size, USE_GPU_RUNTIME);
|
||||||
|
|
||||||
#ifdef TEMPORAL
|
#ifdef TEMPORAL
|
||||||
assert(temporal);
|
assert(temporal);
|
||||||
s->m->addRecurrent(&s->output[OUTPUT_SIZE], TEMPORAL_SIZE);
|
s->m->addRecurrent(&s->output[OUTPUT_SIZE], TEMPORAL_SIZE);
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
#ifdef DESIRE
|
#ifdef DESIRE
|
||||||
s->desire = (float*)malloc(DESIRE_SIZE * sizeof(float));
|
s->desire = (float*)malloc(DESIRE_SIZE * sizeof(float));
|
||||||
for (int i = 0; i < DESIRE_SIZE; i++) s->desire[i] = 0.0;
|
for (int i = 0; i < DESIRE_SIZE; i++) s->desire[i] = 0.0;
|
||||||
|
@ -56,18 +54,11 @@ void model_init(ModelState* s, cl_device_id device_id, cl_context context, int t
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
ModelData model_eval_frame(ModelState* s, cl_command_queue q,
|
|
||||||
|
|
||||||
|
ModelDataRaw model_eval_frame(ModelState* s, cl_command_queue q,
|
||||||
cl_mem yuv_cl, int width, int height,
|
cl_mem yuv_cl, int width, int height,
|
||||||
mat3 transform, void* sock, float *desire_in) {
|
mat3 transform, void* sock, float *desire_in) {
|
||||||
struct {
|
|
||||||
float *path;
|
|
||||||
float *left_lane;
|
|
||||||
float *right_lane;
|
|
||||||
float *lead;
|
|
||||||
float *speed;
|
|
||||||
float *meta;
|
|
||||||
} net_outputs = {NULL};
|
|
||||||
|
|
||||||
#ifdef DESIRE
|
#ifdef DESIRE
|
||||||
if (desire_in != NULL) {
|
if (desire_in != NULL) {
|
||||||
for (int i = 0; i < DESIRE_SIZE; i++) s->desire[i] = desire_in[i];
|
for (int i = 0; i < DESIRE_SIZE; i++) s->desire[i] = desire_in[i];
|
||||||
|
@ -76,124 +67,36 @@ ModelData model_eval_frame(ModelState* s, cl_command_queue q,
|
||||||
|
|
||||||
//for (int i = 0; i < OUTPUT_SIZE + TEMPORAL_SIZE; i++) { printf("%f ", s->output[i]); } printf("\n");
|
//for (int i = 0; i < OUTPUT_SIZE + TEMPORAL_SIZE; i++) { printf("%f ", s->output[i]); } printf("\n");
|
||||||
|
|
||||||
float *net_input_buf = model_input_prepare(&s->in, q, yuv_cl, width, height, transform);
|
float *new_frame_buf = frame_prepare(&s->frame, q, yuv_cl, width, height, transform);
|
||||||
|
memmove(&s->input_frames[0], &s->input_frames[MODEL_FRAME_SIZE], sizeof(float)*MODEL_FRAME_SIZE);
|
||||||
|
memmove(&s->input_frames[MODEL_FRAME_SIZE], new_frame_buf, sizeof(float)*MODEL_FRAME_SIZE);
|
||||||
|
s->m->execute(s->input_frames);
|
||||||
|
|
||||||
#ifdef DUMP_YUV
|
#ifdef DUMP_YUV
|
||||||
FILE *dump_yuv_file = fopen("/sdcard/dump.yuv", "wb");
|
FILE *dump_yuv_file = fopen("/sdcard/dump.yuv", "wb");
|
||||||
fwrite(net_input_buf, MODEL_HEIGHT*MODEL_WIDTH*3/2, sizeof(float), dump_yuv_file);
|
fwrite(new_frame_buf, MODEL_HEIGHT*MODEL_WIDTH*3/2, sizeof(float), dump_yuv_file);
|
||||||
fclose(dump_yuv_file);
|
fclose(dump_yuv_file);
|
||||||
assert(1==2);
|
assert(1==2);
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
//printf("readinggggg \n");
|
|
||||||
//FILE *f = fopen("goof_frame", "r");
|
|
||||||
//fread(net_input_buf, sizeof(float), MODEL_HEIGHT*MODEL_WIDTH*3/2, f);
|
|
||||||
//fclose(f);
|
|
||||||
//sleep(1);
|
|
||||||
//printf("%i \n",OUTPUT_SIZE);
|
|
||||||
//printf("%i \n",MDN_GROUP_SIZE);
|
|
||||||
s->m->execute(net_input_buf);
|
|
||||||
|
|
||||||
// net outputs
|
// net outputs
|
||||||
net_outputs.path = &s->output[0];
|
ModelDataRaw net_outputs;
|
||||||
net_outputs.left_lane = &s->output[MODEL_PATH_DISTANCE*2];
|
net_outputs.path = &s->output[PATH_IDX];
|
||||||
net_outputs.right_lane = &s->output[MODEL_PATH_DISTANCE*2 + MODEL_PATH_DISTANCE*2 + 1];
|
net_outputs.left_lane = &s->output[LL_IDX];
|
||||||
net_outputs.lead = &s->output[MODEL_PATH_DISTANCE*2 + (MODEL_PATH_DISTANCE*2 + 1)*2];
|
net_outputs.right_lane = &s->output[RL_IDX];
|
||||||
//net_outputs.speed = &s->output[OUTPUT_SIZE - SPEED_BUCKETS];
|
net_outputs.lead = &s->output[LEAD_IDX];
|
||||||
net_outputs.meta = &s->output[OUTPUT_SIZE - OTHER_META_SIZE - DESIRE_PRED_SIZE];
|
net_outputs.long_x = &s->output[LONG_X_IDX];
|
||||||
|
net_outputs.long_v = &s->output[LONG_V_IDX];
|
||||||
ModelData model = {0};
|
net_outputs.long_a = &s->output[LONG_A_IDX];
|
||||||
|
net_outputs.meta = &s->output[META_IDX];
|
||||||
for (int i=0; i<MODEL_PATH_DISTANCE; i++) {
|
net_outputs.pose = &s->output[POSE_IDX];
|
||||||
model.path.points[i] = net_outputs.path[i];
|
return net_outputs;
|
||||||
model.left_lane.points[i] = net_outputs.left_lane[i] + 1.8;
|
|
||||||
model.right_lane.points[i] = net_outputs.right_lane[i] - 1.8;
|
|
||||||
model.path.stds[i] = softplus(net_outputs.path[MODEL_PATH_DISTANCE + i]);
|
|
||||||
model.left_lane.stds[i] = softplus(net_outputs.left_lane[MODEL_PATH_DISTANCE + i]);
|
|
||||||
model.right_lane.stds[i] = softplus(net_outputs.right_lane[MODEL_PATH_DISTANCE + i]);
|
|
||||||
}
|
|
||||||
|
|
||||||
model.path.std = softplus(net_outputs.path[MODEL_PATH_DISTANCE]);
|
|
||||||
model.left_lane.std = softplus(net_outputs.left_lane[MODEL_PATH_DISTANCE]);
|
|
||||||
model.right_lane.std = softplus(net_outputs.right_lane[MODEL_PATH_DISTANCE]);
|
|
||||||
|
|
||||||
model.path.prob = 1.;
|
|
||||||
model.left_lane.prob = sigmoid(net_outputs.left_lane[MODEL_PATH_DISTANCE*2]);
|
|
||||||
model.right_lane.prob = sigmoid(net_outputs.right_lane[MODEL_PATH_DISTANCE*2]);
|
|
||||||
|
|
||||||
poly_fit(model.path.points, model.path.stds, model.path.poly);
|
|
||||||
poly_fit(model.left_lane.points, model.left_lane.stds, model.left_lane.poly);
|
|
||||||
poly_fit(model.right_lane.points, model.right_lane.stds, model.right_lane.poly);
|
|
||||||
|
|
||||||
const double max_dist = 140.0;
|
|
||||||
const double max_rel_vel = 10.0;
|
|
||||||
// Every output distribution from the MDN includes the probabilties
|
|
||||||
// of it representing a current lead car, a lead car in 2s
|
|
||||||
// or a lead car in 4s
|
|
||||||
|
|
||||||
// Find the distribution that corresponds to the current lead
|
|
||||||
int mdn_max_idx = 0;
|
|
||||||
for (int i=1; i<LEAD_MDN_N; i++) {
|
|
||||||
if (net_outputs.lead[i*MDN_GROUP_SIZE + 8] > net_outputs.lead[mdn_max_idx*MDN_GROUP_SIZE + 8]) {
|
|
||||||
mdn_max_idx = i;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
model.lead.prob = sigmoid(net_outputs.lead[LEAD_MDN_N*MDN_GROUP_SIZE]);
|
|
||||||
model.lead.dist = net_outputs.lead[mdn_max_idx*MDN_GROUP_SIZE] * max_dist;
|
|
||||||
model.lead.std = softplus(net_outputs.lead[mdn_max_idx*MDN_GROUP_SIZE + MDN_VALS]) * max_dist;
|
|
||||||
model.lead.rel_y = net_outputs.lead[mdn_max_idx*MDN_GROUP_SIZE + 1];
|
|
||||||
model.lead.rel_y_std = softplus(net_outputs.lead[mdn_max_idx*MDN_GROUP_SIZE + MDN_VALS + 1]);
|
|
||||||
model.lead.rel_v = net_outputs.lead[mdn_max_idx*MDN_GROUP_SIZE + 2] * max_rel_vel;
|
|
||||||
model.lead.rel_v_std = softplus(net_outputs.lead[mdn_max_idx*MDN_GROUP_SIZE + MDN_VALS + 2]) * max_rel_vel;
|
|
||||||
model.lead.rel_a = net_outputs.lead[mdn_max_idx*MDN_GROUP_SIZE + 3];
|
|
||||||
model.lead.rel_a_std = softplus(net_outputs.lead[mdn_max_idx*MDN_GROUP_SIZE + MDN_VALS + 3]);
|
|
||||||
|
|
||||||
// Find the distribution that corresponds to the lead in 2s
|
|
||||||
mdn_max_idx = 0;
|
|
||||||
for (int i=1; i<LEAD_MDN_N; i++) {
|
|
||||||
if (net_outputs.lead[i*MDN_GROUP_SIZE + 9] > net_outputs.lead[mdn_max_idx*MDN_GROUP_SIZE + 9]) {
|
|
||||||
mdn_max_idx = i;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
model.lead_future.prob = sigmoid(net_outputs.lead[LEAD_MDN_N*MDN_GROUP_SIZE + 1]);
|
|
||||||
model.lead_future.dist = net_outputs.lead[mdn_max_idx*MDN_GROUP_SIZE] * max_dist;
|
|
||||||
model.lead_future.std = softplus(net_outputs.lead[mdn_max_idx*MDN_GROUP_SIZE + MDN_VALS]) * max_dist;
|
|
||||||
model.lead_future.rel_y = net_outputs.lead[mdn_max_idx*MDN_GROUP_SIZE + 1];
|
|
||||||
model.lead_future.rel_y_std = softplus(net_outputs.lead[mdn_max_idx*MDN_GROUP_SIZE + MDN_VALS + 1]);
|
|
||||||
model.lead_future.rel_v = net_outputs.lead[mdn_max_idx*MDN_GROUP_SIZE + 2] * max_rel_vel;
|
|
||||||
model.lead_future.rel_v_std = softplus(net_outputs.lead[mdn_max_idx*MDN_GROUP_SIZE + MDN_VALS + 2]) * max_rel_vel;
|
|
||||||
model.lead_future.rel_a = net_outputs.lead[mdn_max_idx*MDN_GROUP_SIZE + 3];
|
|
||||||
model.lead_future.rel_a_std = softplus(net_outputs.lead[mdn_max_idx*MDN_GROUP_SIZE + MDN_VALS + 3]);
|
|
||||||
|
|
||||||
|
|
||||||
// get speed percentiles numbers represent 5th, 15th, ... 95th percentile
|
|
||||||
for (int i=0; i < SPEED_PERCENTILES; i++) {
|
|
||||||
model.speed[i] = ((float) SPEED_BUCKETS)/2.0;
|
|
||||||
}
|
|
||||||
//float sum = 0;
|
|
||||||
//for (int idx = 0; idx < SPEED_BUCKETS; idx++) {
|
|
||||||
// sum += net_outputs.speed[idx];
|
|
||||||
// int idx_percentile = (sum + .05) * SPEED_PERCENTILES;
|
|
||||||
// if (idx_percentile < SPEED_PERCENTILES ){
|
|
||||||
// model.speed[idx_percentile] = ((float)idx)/2.0;
|
|
||||||
// }
|
|
||||||
//}
|
|
||||||
// make sure no percentiles are skipped
|
|
||||||
//for (int i=SPEED_PERCENTILES-1; i > 0; i--){
|
|
||||||
// if (model.speed[i-1] > model.speed[i]){
|
|
||||||
// model.speed[i-1] = model.speed[i];
|
|
||||||
// }
|
|
||||||
//}
|
|
||||||
for (int i=0; i<OTHER_META_SIZE + DESIRE_PRED_SIZE; i++) {
|
|
||||||
model.meta[i] = net_outputs.meta[i];
|
|
||||||
}
|
|
||||||
|
|
||||||
return model;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
void model_free(ModelState* s) {
|
void model_free(ModelState* s) {
|
||||||
free(s->output);
|
free(s->output);
|
||||||
model_input_free(&s->in);
|
free(s->input_frames);
|
||||||
|
frame_free(&s->frame);
|
||||||
delete s->m;
|
delete s->m;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -224,31 +127,52 @@ void poly_fit(float *in_pts, float *in_stds, float *out) {
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
void fill_path(cereal::ModelData::PathData::Builder path, const PathData path_data) {
|
void fill_path(cereal::ModelData::PathData::Builder path, const float * data, bool has_prob, const float offset) {
|
||||||
|
float points_arr[MODEL_PATH_DISTANCE];
|
||||||
|
float stds_arr[MODEL_PATH_DISTANCE];
|
||||||
|
float poly_arr[POLYFIT_DEGREE];
|
||||||
|
float std;
|
||||||
|
float prob;
|
||||||
|
|
||||||
|
for (int i=0; i<MODEL_PATH_DISTANCE; i++) {
|
||||||
|
points_arr[i] = data[i] + offset;
|
||||||
|
stds_arr[i] = softplus(data[MODEL_PATH_DISTANCE + i]);
|
||||||
|
}
|
||||||
|
if (has_prob) {
|
||||||
|
prob = sigmoid(data[MODEL_PATH_DISTANCE*2]);
|
||||||
|
} else {
|
||||||
|
prob = 1.0;
|
||||||
|
}
|
||||||
|
std = softplus(data[MODEL_PATH_DISTANCE]);
|
||||||
|
poly_fit(points_arr, stds_arr, poly_arr);
|
||||||
|
|
||||||
if (std::getenv("DEBUG")){
|
if (std::getenv("DEBUG")){
|
||||||
kj::ArrayPtr<const float> stds(&path_data.stds[0], ARRAYSIZE(path_data.stds));
|
kj::ArrayPtr<const float> stds(&stds_arr[0], ARRAYSIZE(stds_arr));
|
||||||
path.setStds(stds);
|
path.setStds(stds);
|
||||||
|
|
||||||
kj::ArrayPtr<const float> points(&path_data.points[0], ARRAYSIZE(path_data.points));
|
kj::ArrayPtr<const float> points(&points_arr[0], ARRAYSIZE(points_arr));
|
||||||
path.setPoints(points);
|
path.setPoints(points);
|
||||||
}
|
}
|
||||||
|
|
||||||
kj::ArrayPtr<const float> poly(&path_data.poly[0], ARRAYSIZE(path_data.poly));
|
kj::ArrayPtr<const float> poly(&poly_arr[0], ARRAYSIZE(poly_arr));
|
||||||
path.setPoly(poly);
|
path.setPoly(poly);
|
||||||
path.setProb(path_data.prob);
|
path.setProb(prob);
|
||||||
path.setStd(path_data.std);
|
path.setStd(std);
|
||||||
}
|
}
|
||||||
|
|
||||||
void fill_lead(cereal::ModelData::LeadData::Builder lead, const LeadData lead_data) {
|
void fill_lead(cereal::ModelData::LeadData::Builder lead, const float * data, int mdn_max_idx) {
|
||||||
lead.setDist(lead_data.dist);
|
const double x_scale = 10.0;
|
||||||
lead.setProb(lead_data.prob);
|
const double y_scale = 10.0;
|
||||||
lead.setStd(lead_data.std);
|
|
||||||
lead.setRelY(lead_data.rel_y);
|
lead.setProb(sigmoid(data[LEAD_MDN_N*MDN_GROUP_SIZE]));
|
||||||
lead.setRelYStd(lead_data.rel_y_std);
|
lead.setDist(x_scale * data[mdn_max_idx*MDN_GROUP_SIZE]);
|
||||||
lead.setRelVel(lead_data.rel_v);
|
lead.setStd(x_scale * softplus(data[mdn_max_idx*MDN_GROUP_SIZE + MDN_VALS]));
|
||||||
lead.setRelVelStd(lead_data.rel_v_std);
|
lead.setRelY(y_scale * data[mdn_max_idx*MDN_GROUP_SIZE + 1]);
|
||||||
lead.setRelA(lead_data.rel_a);
|
lead.setRelYStd(y_scale * softplus(data[mdn_max_idx*MDN_GROUP_SIZE + MDN_VALS + 1]));
|
||||||
lead.setRelAStd(lead_data.rel_a_std);
|
lead.setRelVel(data[mdn_max_idx*MDN_GROUP_SIZE + 2]);
|
||||||
|
lead.setRelVelStd(softplus(data[mdn_max_idx*MDN_GROUP_SIZE + MDN_VALS + 2]));
|
||||||
|
lead.setRelA(data[mdn_max_idx*MDN_GROUP_SIZE + 3]);
|
||||||
|
lead.setRelAStd(softplus(data[mdn_max_idx*MDN_GROUP_SIZE + MDN_VALS + 3]));
|
||||||
}
|
}
|
||||||
|
|
||||||
void fill_meta(cereal::ModelData::MetaData::Builder meta, const float * meta_data) {
|
void fill_meta(cereal::ModelData::MetaData::Builder meta, const float * meta_data) {
|
||||||
|
@ -260,39 +184,104 @@ void fill_meta(cereal::ModelData::MetaData::Builder meta, const float * meta_dat
|
||||||
meta.setDesirePrediction(desire_pred);
|
meta.setDesirePrediction(desire_pred);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
void fill_longi(cereal::ModelData::LongitudinalData::Builder longi, const float * long_v_data, const float * long_a_data) {
|
||||||
|
// just doing 10 vals, 1 every sec for now
|
||||||
|
float speed_arr[TIME_DISTANCE/10];
|
||||||
|
float accel_arr[TIME_DISTANCE/10];
|
||||||
|
for (int i=0; i<TIME_DISTANCE/10; i++) {
|
||||||
|
speed_arr[i] = long_v_data[i*10];
|
||||||
|
accel_arr[i] = long_a_data[i*10];
|
||||||
|
}
|
||||||
|
kj::ArrayPtr<const float> speed(&speed_arr[0], ARRAYSIZE(speed_arr));
|
||||||
|
longi.setSpeeds(speed);
|
||||||
|
kj::ArrayPtr<const float> accel(&accel_arr[0], ARRAYSIZE(accel_arr));
|
||||||
|
longi.setAccelerations(accel);
|
||||||
|
}
|
||||||
|
|
||||||
void model_publish(PubSocket *sock, uint32_t frame_id,
|
void model_publish(PubSocket *sock, uint32_t frame_id,
|
||||||
const ModelData data, uint64_t timestamp_eof) {
|
const ModelDataRaw net_outputs, uint64_t timestamp_eof) {
|
||||||
// make msg
|
// make msg
|
||||||
capnp::MallocMessageBuilder msg;
|
capnp::MallocMessageBuilder msg;
|
||||||
cereal::Event::Builder event = msg.initRoot<cereal::Event>();
|
cereal::Event::Builder event = msg.initRoot<cereal::Event>();
|
||||||
event.setLogMonoTime(nanos_since_boot());
|
event.setLogMonoTime(nanos_since_boot());
|
||||||
|
|
||||||
|
auto framed = event.initModel();
|
||||||
|
framed.setFrameId(frame_id);
|
||||||
|
framed.setTimestampEof(timestamp_eof);
|
||||||
|
|
||||||
auto framed = event.initModel();
|
auto lpath = framed.initPath();
|
||||||
framed.setFrameId(frame_id);
|
fill_path(lpath, net_outputs.path, false, 0);
|
||||||
framed.setTimestampEof(timestamp_eof);
|
auto left_lane = framed.initLeftLane();
|
||||||
|
fill_path(left_lane, net_outputs.left_lane, true, 1.8);
|
||||||
kj::ArrayPtr<const float> speed(&data.speed[0], ARRAYSIZE(data.speed));
|
auto right_lane = framed.initRightLane();
|
||||||
framed.setSpeed(speed);
|
fill_path(right_lane, net_outputs.right_lane, true, -1.8);
|
||||||
|
auto longi = framed.initLongitudinal();
|
||||||
|
fill_longi(longi, net_outputs.long_v, net_outputs.long_a);
|
||||||
|
|
||||||
|
|
||||||
auto lpath = framed.initPath();
|
// Find the distribution that corresponds to the current lead
|
||||||
fill_path(lpath, data.path);
|
int mdn_max_idx = 0;
|
||||||
auto left_lane = framed.initLeftLane();
|
for (int i=1; i<LEAD_MDN_N; i++) {
|
||||||
fill_path(left_lane, data.left_lane);
|
if (net_outputs.lead[i*MDN_GROUP_SIZE + 8] > net_outputs.lead[mdn_max_idx*MDN_GROUP_SIZE + 8]) {
|
||||||
auto right_lane = framed.initRightLane();
|
mdn_max_idx = i;
|
||||||
fill_path(right_lane, data.right_lane);
|
|
||||||
|
|
||||||
auto lead = framed.initLead();
|
|
||||||
fill_lead(lead, data.lead);
|
|
||||||
auto lead_future = framed.initLeadFuture();
|
|
||||||
fill_lead(lead_future, data.lead_future);
|
|
||||||
|
|
||||||
auto meta = framed.initMeta();
|
|
||||||
fill_meta(meta, data.meta);
|
|
||||||
|
|
||||||
|
|
||||||
// send message
|
|
||||||
auto words = capnp::messageToFlatArray(msg);
|
|
||||||
auto bytes = words.asBytes();
|
|
||||||
sock->send((char*)bytes.begin(), bytes.size());
|
|
||||||
}
|
}
|
||||||
|
}
|
||||||
|
auto lead = framed.initLead();
|
||||||
|
fill_lead(lead, net_outputs.lead, mdn_max_idx);
|
||||||
|
// Find the distribution that corresponds to the lead in 2s
|
||||||
|
mdn_max_idx = 0;
|
||||||
|
for (int i=1; i<LEAD_MDN_N; i++) {
|
||||||
|
if (net_outputs.lead[i*MDN_GROUP_SIZE + 9] > net_outputs.lead[mdn_max_idx*MDN_GROUP_SIZE + 9]) {
|
||||||
|
mdn_max_idx = i;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
auto lead_future = framed.initLeadFuture();
|
||||||
|
fill_lead(lead_future, net_outputs.lead, mdn_max_idx);
|
||||||
|
|
||||||
|
|
||||||
|
auto meta = framed.initMeta();
|
||||||
|
fill_meta(meta, net_outputs.meta);
|
||||||
|
|
||||||
|
|
||||||
|
// send message
|
||||||
|
auto words = capnp::messageToFlatArray(msg);
|
||||||
|
auto bytes = words.asBytes();
|
||||||
|
sock->send((char*)bytes.begin(), bytes.size());
|
||||||
|
}
|
||||||
|
|
||||||
|
void posenet_publish(PubSocket *sock, uint32_t frame_id,
|
||||||
|
const ModelDataRaw net_outputs, uint64_t timestamp_eof) {
|
||||||
|
capnp::MallocMessageBuilder msg;
|
||||||
|
cereal::Event::Builder event = msg.initRoot<cereal::Event>();
|
||||||
|
event.setLogMonoTime(nanos_since_boot());
|
||||||
|
|
||||||
|
float trans_arr[3];
|
||||||
|
float trans_std_arr[3];
|
||||||
|
float rot_arr[3];
|
||||||
|
float rot_std_arr[3];
|
||||||
|
|
||||||
|
for (int i =0; i < 3; i++) {
|
||||||
|
trans_arr[i] = net_outputs.pose[i];
|
||||||
|
trans_std_arr[i] = softplus(net_outputs.pose[6 + i]) + 1e-6;
|
||||||
|
|
||||||
|
rot_arr[i] = M_PI * net_outputs.pose[3 + i] / 180.0;
|
||||||
|
rot_std_arr[i] = M_PI * (softplus(net_outputs.pose[9 + i]) + 1e-6) / 180.0;
|
||||||
|
}
|
||||||
|
|
||||||
|
auto posenetd = event.initCameraOdometry();
|
||||||
|
kj::ArrayPtr<const float> trans_vs(&trans_arr[0], 3);
|
||||||
|
posenetd.setTrans(trans_vs);
|
||||||
|
kj::ArrayPtr<const float> rot_vs(&rot_arr[0], 3);
|
||||||
|
posenetd.setRot(rot_vs);
|
||||||
|
kj::ArrayPtr<const float> trans_std_vs(&trans_std_arr[0], 3);
|
||||||
|
posenetd.setTransStd(trans_std_vs);
|
||||||
|
kj::ArrayPtr<const float> rot_std_vs(&rot_std_arr[0], 3);
|
||||||
|
posenetd.setRotStd(rot_std_vs);
|
||||||
|
|
||||||
|
posenetd.setTimestampEof(timestamp_eof);
|
||||||
|
posenetd.setFrameId(frame_id);
|
||||||
|
|
||||||
|
auto words = capnp::messageToFlatArray(msg);
|
||||||
|
auto bytes = words.asBytes();
|
||||||
|
sock->send((char*)bytes.begin(), bytes.size());
|
||||||
|
}
|
||||||
|
|
|
@ -9,8 +9,13 @@
|
||||||
#define DESIRE_SIZE 8
|
#define DESIRE_SIZE 8
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
#ifdef QCOM
|
||||||
|
#include <eigen3/Eigen/Dense>
|
||||||
|
#else
|
||||||
|
#include <Eigen/Dense>
|
||||||
|
#endif
|
||||||
|
|
||||||
#include "common/mat.h"
|
#include "common/mat.h"
|
||||||
#include "common/modeldata.h"
|
|
||||||
#include "common/util.h"
|
#include "common/util.h"
|
||||||
|
|
||||||
#include "commonmodel.h"
|
#include "commonmodel.h"
|
||||||
|
@ -21,10 +26,40 @@
|
||||||
#include <capnp/serialize.h>
|
#include <capnp/serialize.h>
|
||||||
#include "messaging.hpp"
|
#include "messaging.hpp"
|
||||||
|
|
||||||
|
#define MODEL_WIDTH 512
|
||||||
|
#define MODEL_HEIGHT 256
|
||||||
|
#define MODEL_FRAME_SIZE MODEL_WIDTH * MODEL_HEIGHT * 3 / 2
|
||||||
|
#define MODEL_NAME "supercombo_dlc"
|
||||||
|
|
||||||
|
#define MODEL_PATH_DISTANCE 192
|
||||||
|
#define POLYFIT_DEGREE 4
|
||||||
|
#define SPEED_PERCENTILES 10
|
||||||
|
#define DESIRE_PRED_SIZE 32
|
||||||
|
#define OTHER_META_SIZE 4
|
||||||
|
#define LEAD_MDN_N 5 // probs for 5 groups
|
||||||
|
#define MDN_VALS 4 // output xyva for each lead group
|
||||||
|
#define SELECTION 3 //output 3 group (lead now, in 2s and 6s)
|
||||||
|
#define MDN_GROUP_SIZE 11
|
||||||
|
#define TIME_DISTANCE 100
|
||||||
|
#define POSE_SIZE 12
|
||||||
|
|
||||||
|
struct ModelDataRaw {
|
||||||
|
float *path;
|
||||||
|
float *left_lane;
|
||||||
|
float *right_lane;
|
||||||
|
float *lead;
|
||||||
|
float *long_x;
|
||||||
|
float *long_v;
|
||||||
|
float *long_a;
|
||||||
|
float *meta;
|
||||||
|
float *pose;
|
||||||
|
};
|
||||||
|
|
||||||
|
|
||||||
typedef struct ModelState {
|
typedef struct ModelState {
|
||||||
ModelInput in;
|
ModelFrame frame;
|
||||||
float *output;
|
float *output;
|
||||||
|
float *input_frames;
|
||||||
RunModel *m;
|
RunModel *m;
|
||||||
#ifdef DESIRE
|
#ifdef DESIRE
|
||||||
float *desire;
|
float *desire;
|
||||||
|
@ -33,12 +68,14 @@ typedef struct ModelState {
|
||||||
|
|
||||||
void model_init(ModelState* s, cl_device_id device_id,
|
void model_init(ModelState* s, cl_device_id device_id,
|
||||||
cl_context context, int temporal);
|
cl_context context, int temporal);
|
||||||
ModelData model_eval_frame(ModelState* s, cl_command_queue q,
|
ModelDataRaw model_eval_frame(ModelState* s, cl_command_queue q,
|
||||||
cl_mem yuv_cl, int width, int height,
|
cl_mem yuv_cl, int width, int height,
|
||||||
mat3 transform, void* sock, float *desire_in);
|
mat3 transform, void* sock, float *desire_in);
|
||||||
void model_free(ModelState* s);
|
void model_free(ModelState* s);
|
||||||
void poly_fit(float *in_pts, float *in_stds, float *out);
|
void poly_fit(float *in_pts, float *in_stds, float *out);
|
||||||
|
|
||||||
void model_publish(PubSocket* sock, uint32_t frame_id,
|
void model_publish(PubSocket* sock, uint32_t frame_id,
|
||||||
const ModelData data, uint64_t timestamp_eof);
|
const ModelDataRaw data, uint64_t timestamp_eof);
|
||||||
|
void posenet_publish(PubSocket* sock, uint32_t frame_id,
|
||||||
|
const ModelDataRaw data, uint64_t timestamp_eof);
|
||||||
#endif
|
#endif
|
||||||
|
|
|
@ -1,58 +0,0 @@
|
||||||
#include <string.h>
|
|
||||||
#include <math.h>
|
|
||||||
#include "posenet.h"
|
|
||||||
|
|
||||||
void posenet_init(PosenetState *s) {
|
|
||||||
s->input = (float*)malloc(2*200*532*sizeof(float));
|
|
||||||
s->m = new DefaultRunModel("../../models/posenet.dlc", s->output, sizeof(s->output)/sizeof(float), USE_GPU_RUNTIME);
|
|
||||||
}
|
|
||||||
|
|
||||||
void posenet_push(PosenetState *s, uint8_t *yuv_ptr_y, int yuv_width) {
|
|
||||||
// move second frame to first frame
|
|
||||||
memmove(&s->input[0], &s->input[1], sizeof(float)*(200*532*2 - 1));
|
|
||||||
|
|
||||||
// fill posenet input
|
|
||||||
float a;
|
|
||||||
// posenet uses a half resolution cropped frame
|
|
||||||
// with upper left corner: [50, 237] and
|
|
||||||
// bottom right corner: [1114, 637]
|
|
||||||
// So the resulting crop is 532 X 200
|
|
||||||
for (int y=237; y<637; y+=2) {
|
|
||||||
int yy = (y-237)/2;
|
|
||||||
for (int x = 50; x < 1114; x+=2) {
|
|
||||||
int xx = (x-50)/2;
|
|
||||||
a = 0;
|
|
||||||
a += yuv_ptr_y[yuv_width*(y+0) + (x+1)];
|
|
||||||
a += yuv_ptr_y[yuv_width*(y+1) + (x+1)];
|
|
||||||
a += yuv_ptr_y[yuv_width*(y+0) + (x+0)];
|
|
||||||
a += yuv_ptr_y[yuv_width*(y+1) + (x+0)];
|
|
||||||
// The posenet takes a normalized image input
|
|
||||||
// like the driving model so [0,255] is remapped
|
|
||||||
// to [-1,1]
|
|
||||||
s->input[(yy*532+xx)*2 + 1] = (a/512.0 - 1.0);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
void posenet_eval(PosenetState *s) {
|
|
||||||
s->m->execute(s->input);
|
|
||||||
|
|
||||||
// fix stddevs
|
|
||||||
for (int i = 6; i < 12; i++) {
|
|
||||||
s->output[i] = log1p(exp(s->output[i])) + 1e-6;
|
|
||||||
}
|
|
||||||
// to radians
|
|
||||||
for (int i = 3; i < 6; i++) {
|
|
||||||
s->output[i] = M_PI * s->output[i] / 180.0;
|
|
||||||
}
|
|
||||||
// to radians
|
|
||||||
for (int i = 9; i < 12; i++) {
|
|
||||||
s->output[i] = M_PI * s->output[i] / 180.0;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
void posenet_free(PosenetState *s) {
|
|
||||||
delete s->m;
|
|
||||||
free(s->input);
|
|
||||||
}
|
|
||||||
|
|
|
@ -1,27 +0,0 @@
|
||||||
#ifndef POSENET_H
|
|
||||||
#define POSENET_H
|
|
||||||
|
|
||||||
#include <stdint.h>
|
|
||||||
#include "runners/run.h"
|
|
||||||
|
|
||||||
#ifdef __cplusplus
|
|
||||||
extern "C" {
|
|
||||||
#endif
|
|
||||||
|
|
||||||
typedef struct PosenetState {
|
|
||||||
float output[12];
|
|
||||||
float *input;
|
|
||||||
RunModel *m;
|
|
||||||
} PosenetState;
|
|
||||||
|
|
||||||
void posenet_init(PosenetState *s);
|
|
||||||
void posenet_push(PosenetState *s, uint8_t *yuv_ptr_y, int yuv_width);
|
|
||||||
void posenet_eval(PosenetState *s);
|
|
||||||
void posenet_free(PosenetState *s);
|
|
||||||
|
|
||||||
#ifdef __cplusplus
|
|
||||||
}
|
|
||||||
#endif
|
|
||||||
|
|
||||||
#endif
|
|
||||||
|
|
|
@ -1,5 +1,5 @@
|
||||||
#!/bin/sh
|
#!/bin/sh
|
||||||
export LD_LIBRARY_PATH="/data/pythonpath/phonelibs/snpe/aarch64-android-clang3.8:$LD_LIBRARY_PATH"
|
export LD_LIBRARY_PATH="/data/pythonpath/phonelibs/snpe/aarch64-android-clang3.8:/home/batman/one/phonelibs/snpe/x86_64-linux-clang:$LD_LIBRARY_PATH"
|
||||||
export ADSP_LIBRARY_PATH="/data/pythonpath/phonelibs/snpe/aarch64-android-clang3.8/"
|
export ADSP_LIBRARY_PATH="/data/pythonpath/phonelibs/snpe/aarch64-android-clang3.8/"
|
||||||
exec ./_monitoringd
|
exec ./_monitoringd
|
||||||
|
|
||||||
|
|
|
@ -7,9 +7,12 @@
|
||||||
#ifdef QCOM
|
#ifdef QCOM
|
||||||
#define DefaultRunModel SNPEModel
|
#define DefaultRunModel SNPEModel
|
||||||
#else
|
#else
|
||||||
#define DefaultRunModel SNPEModel
|
#ifdef USE_TF_MODEL
|
||||||
/* #include "tfmodel.h" */
|
#include "tfmodel.h"
|
||||||
/* #define DefaultRunModel TFModel */
|
#define DefaultRunModel TFModel
|
||||||
|
#else
|
||||||
|
#define DefaultRunModel SNPEModel
|
||||||
|
#endif
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
#endif
|
#endif
|
||||||
|
|
|
@ -4,19 +4,29 @@ import os
|
||||||
import time
|
import time
|
||||||
|
|
||||||
from selfdrive.swaglog import cloudlog
|
from selfdrive.swaglog import cloudlog
|
||||||
from panda import Panda, PandaDFU, BASEDIR
|
from panda import Panda, PandaDFU, BASEDIR, build_st
|
||||||
|
|
||||||
|
|
||||||
def get_expected_version():
|
def get_firmware_fn():
|
||||||
with open(os.path.join(BASEDIR, "VERSION")) as f:
|
signed_fn = os.path.join(BASEDIR, "board", "obj", "panda.bin.signed")
|
||||||
repo_version = f.read()
|
if os.path.exists(signed_fn):
|
||||||
repo_version += "-EON" if os.path.isfile('/EON') else "-DEV"
|
cloudlog.info("Using prebuilt signed firmware")
|
||||||
return repo_version
|
return signed_fn
|
||||||
|
else:
|
||||||
|
cloudlog.info("Building panda firmware")
|
||||||
|
fn = "obj/panda.bin"
|
||||||
|
build_st(fn, clean=False)
|
||||||
|
return os.path.join(BASEDIR, "board", fn)
|
||||||
|
|
||||||
|
|
||||||
|
def get_expected_signature(fw_fn=None):
|
||||||
|
if fw_fn is None:
|
||||||
|
fw_fn = get_firmware_fn()
|
||||||
|
|
||||||
|
return Panda.get_signature_from_firmware(fw_fn)
|
||||||
|
|
||||||
|
|
||||||
def update_panda():
|
def update_panda():
|
||||||
repo_version = get_expected_version()
|
|
||||||
|
|
||||||
panda = None
|
panda = None
|
||||||
panda_dfu = None
|
panda_dfu = None
|
||||||
|
|
||||||
|
@ -37,27 +47,28 @@ def update_panda():
|
||||||
panda_dfu = PandaDFU(panda_dfu[0])
|
panda_dfu = PandaDFU(panda_dfu[0])
|
||||||
panda_dfu.recover()
|
panda_dfu.recover()
|
||||||
|
|
||||||
print("waiting for board...")
|
|
||||||
time.sleep(1)
|
time.sleep(1)
|
||||||
|
|
||||||
|
fw_fn = get_firmware_fn()
|
||||||
|
fw_signature = get_expected_signature(fw_fn)
|
||||||
|
|
||||||
try:
|
try:
|
||||||
serial = panda.get_serial()[0].decode("utf-8")
|
serial = panda.get_serial()[0].decode("utf-8")
|
||||||
except Exception:
|
except Exception:
|
||||||
serial = None
|
serial = None
|
||||||
current_version = "bootstub" if panda.bootstub else panda.get_version()
|
|
||||||
cloudlog.warning("Panda %s connected, version: %s, expected %s" % (serial, current_version, repo_version))
|
|
||||||
|
|
||||||
if panda.bootstub or not current_version.startswith(repo_version):
|
panda_version = "bootstub" if panda.bootstub else panda.get_version()
|
||||||
|
panda_signature = "bootstub" if panda.bootstub else panda.get_signature()
|
||||||
|
cloudlog.warning("Panda %s connected, version: %s, signature %s, expected %s" % (
|
||||||
|
serial,
|
||||||
|
panda_version,
|
||||||
|
panda_signature.hex(),
|
||||||
|
fw_signature.hex(),
|
||||||
|
))
|
||||||
|
|
||||||
|
if panda.bootstub or panda_signature != fw_signature:
|
||||||
cloudlog.info("Panda firmware out of date, update required")
|
cloudlog.info("Panda firmware out of date, update required")
|
||||||
|
panda.flash(fw_fn)
|
||||||
signed_fn = os.path.join(BASEDIR, "board", "obj", "panda.bin.signed")
|
|
||||||
if os.path.exists(signed_fn):
|
|
||||||
cloudlog.info("Flashing signed firmware")
|
|
||||||
panda.flash(fn=signed_fn)
|
|
||||||
else:
|
|
||||||
cloudlog.info("Building and flashing unsigned firmware")
|
|
||||||
panda.flash()
|
|
||||||
|
|
||||||
cloudlog.info("Done flashing")
|
cloudlog.info("Done flashing")
|
||||||
|
|
||||||
if panda.bootstub:
|
if panda.bootstub:
|
||||||
|
@ -69,8 +80,8 @@ def update_panda():
|
||||||
cloudlog.info("Panda still not booting, exiting")
|
cloudlog.info("Panda still not booting, exiting")
|
||||||
raise AssertionError
|
raise AssertionError
|
||||||
|
|
||||||
version = panda.get_version()
|
panda_signature = panda.get_signature()
|
||||||
if not version.startswith(repo_version):
|
if panda_signature != fw_signature:
|
||||||
cloudlog.info("Version mismatch after flashing, exiting")
|
cloudlog.info("Version mismatch after flashing, exiting")
|
||||||
raise AssertionError
|
raise AssertionError
|
||||||
|
|
||||||
|
|
|
@ -7,6 +7,7 @@ from selfdrive.version import version, terms_version, training_version, get_git_
|
||||||
from common.android import get_imei, get_serial, get_subscriber_info
|
from common.android import get_imei, get_serial, get_subscriber_info
|
||||||
from common.api import api_get
|
from common.api import api_get
|
||||||
from common.params import Params
|
from common.params import Params
|
||||||
|
from common.file_helpers import mkdirs_exists_ok
|
||||||
|
|
||||||
def register():
|
def register():
|
||||||
params = Params()
|
params = Params()
|
||||||
|
@ -18,6 +19,17 @@ def register():
|
||||||
params.put("GitRemote", get_git_remote())
|
params.put("GitRemote", get_git_remote())
|
||||||
params.put("SubscriberInfo", get_subscriber_info())
|
params.put("SubscriberInfo", get_subscriber_info())
|
||||||
|
|
||||||
|
# create a key for auth
|
||||||
|
# your private key is kept on your device persist partition and never sent to our servers
|
||||||
|
# do not erase your persist partition
|
||||||
|
if not os.path.isfile("/persist/comma/id_rsa.pub"):
|
||||||
|
cloudlog.warning("generating your personal RSA key")
|
||||||
|
mkdirs_exists_ok("/persist/comma")
|
||||||
|
assert os.system("openssl genrsa -out /persist/comma/id_rsa.tmp 2048") == 0
|
||||||
|
assert os.system("openssl rsa -in /persist/comma/id_rsa.tmp -pubout -out /persist/comma/id_rsa.tmp.pub") == 0
|
||||||
|
os.rename("/persist/comma/id_rsa.tmp", "/persist/comma/id_rsa")
|
||||||
|
os.rename("/persist/comma/id_rsa.tmp.pub", "/persist/comma/id_rsa.pub")
|
||||||
|
|
||||||
# make key readable by app users (ai.comma.plus.offroad)
|
# make key readable by app users (ai.comma.plus.offroad)
|
||||||
os.chmod('/persist/comma/', 0o755)
|
os.chmod('/persist/comma/', 0o755)
|
||||||
os.chmod('/persist/comma/id_rsa', 0o744)
|
os.chmod('/persist/comma/id_rsa', 0o744)
|
||||||
|
@ -51,5 +63,4 @@ def register():
|
||||||
return None
|
return None
|
||||||
|
|
||||||
if __name__ == "__main__":
|
if __name__ == "__main__":
|
||||||
print(api_get("").text)
|
|
||||||
print(register())
|
print(register())
|
||||||
|
|
|
@ -7,7 +7,6 @@
|
||||||
#include <sys/time.h>
|
#include <sys/time.h>
|
||||||
#include <sys/cdefs.h>
|
#include <sys/cdefs.h>
|
||||||
#include <sys/types.h>
|
#include <sys/types.h>
|
||||||
#include <sys/timerfd.h>
|
|
||||||
#include <sys/resource.h>
|
#include <sys/resource.h>
|
||||||
|
|
||||||
#include <pthread.h>
|
#include <pthread.h>
|
||||||
|
@ -31,8 +30,6 @@ volatile sig_atomic_t do_exit = 0;
|
||||||
|
|
||||||
namespace {
|
namespace {
|
||||||
|
|
||||||
pthread_t clock_thread_handle;
|
|
||||||
|
|
||||||
Context *gps_context;
|
Context *gps_context;
|
||||||
PubSocket *gps_publisher;
|
PubSocket *gps_publisher;
|
||||||
PubSocket *gps_location_publisher;
|
PubSocket *gps_location_publisher;
|
||||||
|
@ -181,61 +178,6 @@ int64_t arm_cntpct() {
|
||||||
return v;
|
return v;
|
||||||
}
|
}
|
||||||
|
|
||||||
// TODO: move this out of here
|
|
||||||
void* clock_thread(void* args) {
|
|
||||||
int err = 0;
|
|
||||||
|
|
||||||
PubSocket* clock_publisher = PubSocket::create(gps_context, "clocks");
|
|
||||||
assert(clock_publisher != NULL);
|
|
||||||
|
|
||||||
int timerfd = timerfd_create(CLOCK_BOOTTIME, 0);
|
|
||||||
assert(timerfd >= 0);
|
|
||||||
|
|
||||||
struct itimerspec spec = {0};
|
|
||||||
spec.it_interval.tv_sec = 1;
|
|
||||||
spec.it_interval.tv_nsec = 0;
|
|
||||||
spec.it_value.tv_sec = 1;
|
|
||||||
spec.it_value.tv_nsec = 0;
|
|
||||||
|
|
||||||
err = timerfd_settime(timerfd, 0, &spec, 0);
|
|
||||||
assert(err == 0);
|
|
||||||
|
|
||||||
uint64_t expirations = 0;
|
|
||||||
while ((err = read(timerfd, &expirations, sizeof(expirations)))) {
|
|
||||||
if (err < 0) break;
|
|
||||||
|
|
||||||
if (do_exit) break;
|
|
||||||
|
|
||||||
uint64_t boottime = nanos_since_boot();
|
|
||||||
uint64_t monotonic = nanos_monotonic();
|
|
||||||
uint64_t monotonic_raw = nanos_monotonic_raw();
|
|
||||||
uint64_t wall_time = nanos_since_epoch();
|
|
||||||
|
|
||||||
uint64_t modem_uptime_v = arm_cntpct() / 19200ULL; // 19.2 mhz clock
|
|
||||||
|
|
||||||
capnp::MallocMessageBuilder msg;
|
|
||||||
cereal::Event::Builder event = msg.initRoot<cereal::Event>();
|
|
||||||
event.setLogMonoTime(boottime);
|
|
||||||
auto clocks = event.initClocks();
|
|
||||||
|
|
||||||
clocks.setBootTimeNanos(boottime);
|
|
||||||
clocks.setMonotonicNanos(monotonic);
|
|
||||||
clocks.setMonotonicRawNanos(monotonic_raw);
|
|
||||||
clocks.setWallTimeNanos(wall_time);
|
|
||||||
clocks.setModemUptimeMillis(modem_uptime_v);
|
|
||||||
|
|
||||||
auto words = capnp::messageToFlatArray(msg);
|
|
||||||
auto bytes = words.asBytes();
|
|
||||||
clock_publisher->send((char*)bytes.begin(), bytes.size());
|
|
||||||
}
|
|
||||||
|
|
||||||
close(timerfd);
|
|
||||||
delete clock_publisher;
|
|
||||||
|
|
||||||
return NULL;
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
int main() {
|
int main() {
|
||||||
|
@ -249,15 +191,8 @@ int main() {
|
||||||
|
|
||||||
rawgps_init();
|
rawgps_init();
|
||||||
|
|
||||||
err = pthread_create(&clock_thread_handle, NULL,
|
|
||||||
clock_thread, NULL);
|
|
||||||
assert(err == 0);
|
|
||||||
|
|
||||||
while(!do_exit) pause();
|
while(!do_exit) pause();
|
||||||
|
|
||||||
err = pthread_join(clock_thread_handle, NULL);
|
|
||||||
assert(err == 0);
|
|
||||||
|
|
||||||
rawgps_destroy();
|
rawgps_destroy();
|
||||||
|
|
||||||
gps_destroy();
|
gps_destroy();
|
||||||
|
|
|
@ -38,6 +38,7 @@
|
||||||
#define SENSOR_LIGHT 7
|
#define SENSOR_LIGHT 7
|
||||||
|
|
||||||
volatile sig_atomic_t do_exit = 0;
|
volatile sig_atomic_t do_exit = 0;
|
||||||
|
volatile sig_atomic_t re_init_sensors = 0;
|
||||||
|
|
||||||
namespace {
|
namespace {
|
||||||
|
|
||||||
|
@ -47,179 +48,190 @@ void set_do_exit(int sig) {
|
||||||
|
|
||||||
void sigpipe_handler(int sig) {
|
void sigpipe_handler(int sig) {
|
||||||
LOGE("SIGPIPE received");
|
LOGE("SIGPIPE received");
|
||||||
|
re_init_sensors = true;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
void sensor_loop() {
|
void sensor_loop() {
|
||||||
LOG("*** sensor loop");
|
LOG("*** sensor loop");
|
||||||
|
|
||||||
Context * c = Context::create();
|
|
||||||
PubSocket * sensor_events_sock = PubSocket::create(c, "sensorEvents");
|
|
||||||
assert(sensor_events_sock != NULL);
|
|
||||||
|
|
||||||
struct sensors_poll_device_t* device;
|
|
||||||
struct sensors_module_t* module;
|
|
||||||
|
|
||||||
hw_get_module(SENSORS_HARDWARE_MODULE_ID, (hw_module_t const**)&module);
|
|
||||||
sensors_open(&module->common, &device);
|
|
||||||
|
|
||||||
// required
|
|
||||||
struct sensor_t const* list;
|
|
||||||
int count = module->get_sensors_list(module, &list);
|
|
||||||
LOG("%d sensors found", count);
|
|
||||||
|
|
||||||
if (getenv("SENSOR_TEST")) {
|
|
||||||
exit(count);
|
|
||||||
}
|
|
||||||
|
|
||||||
for (int i = 0; i < count; i++) {
|
|
||||||
LOGD("sensor %4d: %4d %60s %d-%ld us", i, list[i].handle, list[i].name, list[i].minDelay, list[i].maxDelay);
|
|
||||||
}
|
|
||||||
|
|
||||||
device->activate(device, SENSOR_MAGNETOMETER_UNCALIBRATED, 0);
|
|
||||||
device->activate(device, SENSOR_GYRO_UNCALIBRATED, 0);
|
|
||||||
device->activate(device, SENSOR_ACCELEROMETER, 0);
|
|
||||||
device->activate(device, SENSOR_MAGNETOMETER, 0);
|
|
||||||
device->activate(device, SENSOR_GYRO, 0);
|
|
||||||
device->activate(device, SENSOR_PROXIMITY, 0);
|
|
||||||
device->activate(device, SENSOR_LIGHT, 0);
|
|
||||||
|
|
||||||
device->activate(device, SENSOR_MAGNETOMETER_UNCALIBRATED, 1);
|
|
||||||
device->activate(device, SENSOR_GYRO_UNCALIBRATED, 1);
|
|
||||||
device->activate(device, SENSOR_ACCELEROMETER, 1);
|
|
||||||
device->activate(device, SENSOR_MAGNETOMETER, 1);
|
|
||||||
device->activate(device, SENSOR_GYRO, 1);
|
|
||||||
device->activate(device, SENSOR_PROXIMITY, 1);
|
|
||||||
device->activate(device, SENSOR_LIGHT, 1);
|
|
||||||
|
|
||||||
device->setDelay(device, SENSOR_GYRO_UNCALIBRATED, ms2ns(10));
|
|
||||||
device->setDelay(device, SENSOR_MAGNETOMETER_UNCALIBRATED, ms2ns(100));
|
|
||||||
device->setDelay(device, SENSOR_ACCELEROMETER, ms2ns(10));
|
|
||||||
device->setDelay(device, SENSOR_GYRO, ms2ns(10));
|
|
||||||
device->setDelay(device, SENSOR_MAGNETOMETER, ms2ns(100));
|
|
||||||
device->setDelay(device, SENSOR_PROXIMITY, ms2ns(100));
|
|
||||||
device->setDelay(device, SENSOR_LIGHT, ms2ns(100));
|
|
||||||
|
|
||||||
static const size_t numEvents = 16;
|
|
||||||
sensors_event_t buffer[numEvents];
|
|
||||||
|
|
||||||
|
|
||||||
while (!do_exit) {
|
while (!do_exit) {
|
||||||
int n = device->poll(device, buffer, numEvents);
|
Context * c = Context::create();
|
||||||
if (n == 0) continue;
|
PubSocket * sensor_events_sock = PubSocket::create(c, "sensorEvents");
|
||||||
if (n < 0) {
|
assert(sensor_events_sock != NULL);
|
||||||
LOG("sensor_loop poll failed: %d", n);
|
|
||||||
continue;
|
struct sensors_poll_device_t* device;
|
||||||
|
struct sensors_module_t* module;
|
||||||
|
|
||||||
|
hw_get_module(SENSORS_HARDWARE_MODULE_ID, (hw_module_t const**)&module);
|
||||||
|
sensors_open(&module->common, &device);
|
||||||
|
|
||||||
|
// required
|
||||||
|
struct sensor_t const* list;
|
||||||
|
int count = module->get_sensors_list(module, &list);
|
||||||
|
LOG("%d sensors found", count);
|
||||||
|
|
||||||
|
if (getenv("SENSOR_TEST")) {
|
||||||
|
exit(count);
|
||||||
}
|
}
|
||||||
|
|
||||||
int log_events = 0;
|
for (int i = 0; i < count; i++) {
|
||||||
for (int i=0; i < n; i++) {
|
LOGD("sensor %4d: %4d %60s %d-%ld us", i, list[i].handle, list[i].name, list[i].minDelay, list[i].maxDelay);
|
||||||
switch (buffer[i].type) {
|
|
||||||
case SENSOR_TYPE_ACCELEROMETER:
|
|
||||||
case SENSOR_TYPE_MAGNETIC_FIELD_UNCALIBRATED:
|
|
||||||
case SENSOR_TYPE_MAGNETIC_FIELD:
|
|
||||||
case SENSOR_TYPE_GYROSCOPE_UNCALIBRATED:
|
|
||||||
case SENSOR_TYPE_GYROSCOPE:
|
|
||||||
case SENSOR_TYPE_PROXIMITY:
|
|
||||||
case SENSOR_TYPE_LIGHT:
|
|
||||||
log_events++;
|
|
||||||
break;
|
|
||||||
default:
|
|
||||||
continue;
|
|
||||||
}
|
|
||||||
}
|
}
|
||||||
|
|
||||||
uint64_t log_time = nanos_since_boot();
|
device->activate(device, SENSOR_MAGNETOMETER_UNCALIBRATED, 0);
|
||||||
|
device->activate(device, SENSOR_GYRO_UNCALIBRATED, 0);
|
||||||
|
device->activate(device, SENSOR_ACCELEROMETER, 0);
|
||||||
|
device->activate(device, SENSOR_MAGNETOMETER, 0);
|
||||||
|
device->activate(device, SENSOR_GYRO, 0);
|
||||||
|
device->activate(device, SENSOR_PROXIMITY, 0);
|
||||||
|
device->activate(device, SENSOR_LIGHT, 0);
|
||||||
|
|
||||||
capnp::MallocMessageBuilder msg;
|
device->activate(device, SENSOR_MAGNETOMETER_UNCALIBRATED, 1);
|
||||||
cereal::Event::Builder event = msg.initRoot<cereal::Event>();
|
device->activate(device, SENSOR_GYRO_UNCALIBRATED, 1);
|
||||||
event.setLogMonoTime(log_time);
|
device->activate(device, SENSOR_ACCELEROMETER, 1);
|
||||||
|
device->activate(device, SENSOR_MAGNETOMETER, 1);
|
||||||
|
device->activate(device, SENSOR_GYRO, 1);
|
||||||
|
device->activate(device, SENSOR_PROXIMITY, 1);
|
||||||
|
device->activate(device, SENSOR_LIGHT, 1);
|
||||||
|
|
||||||
auto sensor_events = event.initSensorEvents(log_events);
|
device->setDelay(device, SENSOR_GYRO_UNCALIBRATED, ms2ns(10));
|
||||||
|
device->setDelay(device, SENSOR_MAGNETOMETER_UNCALIBRATED, ms2ns(100));
|
||||||
|
device->setDelay(device, SENSOR_ACCELEROMETER, ms2ns(10));
|
||||||
|
device->setDelay(device, SENSOR_GYRO, ms2ns(10));
|
||||||
|
device->setDelay(device, SENSOR_MAGNETOMETER, ms2ns(100));
|
||||||
|
device->setDelay(device, SENSOR_PROXIMITY, ms2ns(100));
|
||||||
|
device->setDelay(device, SENSOR_LIGHT, ms2ns(100));
|
||||||
|
|
||||||
int log_i = 0;
|
static const size_t numEvents = 16;
|
||||||
for (int i = 0; i < n; i++) {
|
sensors_event_t buffer[numEvents];
|
||||||
|
|
||||||
const sensors_event_t& data = buffer[i];
|
|
||||||
|
|
||||||
switch (data.type) {
|
while (!do_exit) {
|
||||||
case SENSOR_TYPE_ACCELEROMETER:
|
int n = device->poll(device, buffer, numEvents);
|
||||||
case SENSOR_TYPE_MAGNETIC_FIELD_UNCALIBRATED:
|
if (n == 0) continue;
|
||||||
case SENSOR_TYPE_MAGNETIC_FIELD:
|
if (n < 0) {
|
||||||
case SENSOR_TYPE_GYROSCOPE_UNCALIBRATED:
|
LOG("sensor_loop poll failed: %d", n);
|
||||||
case SENSOR_TYPE_GYROSCOPE:
|
|
||||||
case SENSOR_TYPE_PROXIMITY:
|
|
||||||
case SENSOR_TYPE_LIGHT:
|
|
||||||
break;
|
|
||||||
default:
|
|
||||||
continue;
|
continue;
|
||||||
}
|
}
|
||||||
|
|
||||||
auto log_event = sensor_events[log_i];
|
int log_events = 0;
|
||||||
|
for (int i=0; i < n; i++) {
|
||||||
log_event.setSource(cereal::SensorEventData::SensorSource::ANDROID);
|
switch (buffer[i].type) {
|
||||||
log_event.setVersion(data.version);
|
case SENSOR_TYPE_ACCELEROMETER:
|
||||||
log_event.setSensor(data.sensor);
|
case SENSOR_TYPE_MAGNETIC_FIELD_UNCALIBRATED:
|
||||||
log_event.setType(data.type);
|
case SENSOR_TYPE_MAGNETIC_FIELD:
|
||||||
log_event.setTimestamp(data.timestamp);
|
case SENSOR_TYPE_GYROSCOPE_UNCALIBRATED:
|
||||||
|
case SENSOR_TYPE_GYROSCOPE:
|
||||||
switch (data.type) {
|
case SENSOR_TYPE_PROXIMITY:
|
||||||
case SENSOR_TYPE_ACCELEROMETER: {
|
case SENSOR_TYPE_LIGHT:
|
||||||
auto svec = log_event.initAcceleration();
|
log_events++;
|
||||||
kj::ArrayPtr<const float> vs(&data.acceleration.v[0], 3);
|
break;
|
||||||
svec.setV(vs);
|
default:
|
||||||
svec.setStatus(data.acceleration.status);
|
continue;
|
||||||
break;
|
}
|
||||||
}
|
|
||||||
case SENSOR_TYPE_MAGNETIC_FIELD_UNCALIBRATED: {
|
|
||||||
auto svec = log_event.initMagneticUncalibrated();
|
|
||||||
// assuming the uncalib and bias floats are contiguous in memory
|
|
||||||
kj::ArrayPtr<const float> vs(&data.uncalibrated_magnetic.uncalib[0], 6);
|
|
||||||
svec.setV(vs);
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
case SENSOR_TYPE_MAGNETIC_FIELD: {
|
|
||||||
auto svec = log_event.initMagnetic();
|
|
||||||
kj::ArrayPtr<const float> vs(&data.magnetic.v[0], 3);
|
|
||||||
svec.setV(vs);
|
|
||||||
svec.setStatus(data.magnetic.status);
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
case SENSOR_TYPE_GYROSCOPE_UNCALIBRATED: {
|
|
||||||
auto svec = log_event.initGyroUncalibrated();
|
|
||||||
// assuming the uncalib and bias floats are contiguous in memory
|
|
||||||
kj::ArrayPtr<const float> vs(&data.uncalibrated_gyro.uncalib[0], 6);
|
|
||||||
svec.setV(vs);
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
case SENSOR_TYPE_GYROSCOPE: {
|
|
||||||
auto svec = log_event.initGyro();
|
|
||||||
kj::ArrayPtr<const float> vs(&data.gyro.v[0], 3);
|
|
||||||
svec.setV(vs);
|
|
||||||
svec.setStatus(data.gyro.status);
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
case SENSOR_TYPE_PROXIMITY: {
|
|
||||||
log_event.setProximity(data.distance);
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
case SENSOR_TYPE_LIGHT:
|
|
||||||
log_event.setLight(data.light);
|
|
||||||
break;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
log_i++;
|
uint64_t log_time = nanos_since_boot();
|
||||||
|
|
||||||
|
capnp::MallocMessageBuilder msg;
|
||||||
|
cereal::Event::Builder event = msg.initRoot<cereal::Event>();
|
||||||
|
event.setLogMonoTime(log_time);
|
||||||
|
|
||||||
|
auto sensor_events = event.initSensorEvents(log_events);
|
||||||
|
|
||||||
|
int log_i = 0;
|
||||||
|
for (int i = 0; i < n; i++) {
|
||||||
|
|
||||||
|
const sensors_event_t& data = buffer[i];
|
||||||
|
|
||||||
|
switch (data.type) {
|
||||||
|
case SENSOR_TYPE_ACCELEROMETER:
|
||||||
|
case SENSOR_TYPE_MAGNETIC_FIELD_UNCALIBRATED:
|
||||||
|
case SENSOR_TYPE_MAGNETIC_FIELD:
|
||||||
|
case SENSOR_TYPE_GYROSCOPE_UNCALIBRATED:
|
||||||
|
case SENSOR_TYPE_GYROSCOPE:
|
||||||
|
case SENSOR_TYPE_PROXIMITY:
|
||||||
|
case SENSOR_TYPE_LIGHT:
|
||||||
|
break;
|
||||||
|
default:
|
||||||
|
continue;
|
||||||
|
}
|
||||||
|
|
||||||
|
auto log_event = sensor_events[log_i];
|
||||||
|
|
||||||
|
log_event.setSource(cereal::SensorEventData::SensorSource::ANDROID);
|
||||||
|
log_event.setVersion(data.version);
|
||||||
|
log_event.setSensor(data.sensor);
|
||||||
|
log_event.setType(data.type);
|
||||||
|
log_event.setTimestamp(data.timestamp);
|
||||||
|
|
||||||
|
switch (data.type) {
|
||||||
|
case SENSOR_TYPE_ACCELEROMETER: {
|
||||||
|
auto svec = log_event.initAcceleration();
|
||||||
|
kj::ArrayPtr<const float> vs(&data.acceleration.v[0], 3);
|
||||||
|
svec.setV(vs);
|
||||||
|
svec.setStatus(data.acceleration.status);
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
case SENSOR_TYPE_MAGNETIC_FIELD_UNCALIBRATED: {
|
||||||
|
auto svec = log_event.initMagneticUncalibrated();
|
||||||
|
// assuming the uncalib and bias floats are contiguous in memory
|
||||||
|
kj::ArrayPtr<const float> vs(&data.uncalibrated_magnetic.uncalib[0], 6);
|
||||||
|
svec.setV(vs);
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
case SENSOR_TYPE_MAGNETIC_FIELD: {
|
||||||
|
auto svec = log_event.initMagnetic();
|
||||||
|
kj::ArrayPtr<const float> vs(&data.magnetic.v[0], 3);
|
||||||
|
svec.setV(vs);
|
||||||
|
svec.setStatus(data.magnetic.status);
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
case SENSOR_TYPE_GYROSCOPE_UNCALIBRATED: {
|
||||||
|
auto svec = log_event.initGyroUncalibrated();
|
||||||
|
// assuming the uncalib and bias floats are contiguous in memory
|
||||||
|
kj::ArrayPtr<const float> vs(&data.uncalibrated_gyro.uncalib[0], 6);
|
||||||
|
svec.setV(vs);
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
case SENSOR_TYPE_GYROSCOPE: {
|
||||||
|
auto svec = log_event.initGyro();
|
||||||
|
kj::ArrayPtr<const float> vs(&data.gyro.v[0], 3);
|
||||||
|
svec.setV(vs);
|
||||||
|
svec.setStatus(data.gyro.status);
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
case SENSOR_TYPE_PROXIMITY: {
|
||||||
|
log_event.setProximity(data.distance);
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
case SENSOR_TYPE_LIGHT:
|
||||||
|
log_event.setLight(data.light);
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
|
||||||
|
log_i++;
|
||||||
|
}
|
||||||
|
|
||||||
|
auto words = capnp::messageToFlatArray(msg);
|
||||||
|
auto bytes = words.asBytes();
|
||||||
|
sensor_events_sock->send((char*)bytes.begin(), bytes.size());
|
||||||
|
|
||||||
|
if (re_init_sensors){
|
||||||
|
LOGE("Resetting sensors");
|
||||||
|
re_init_sensors = false;
|
||||||
|
break;
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
auto words = capnp::messageToFlatArray(msg);
|
delete sensor_events_sock;
|
||||||
auto bytes = words.asBytes();
|
delete c;
|
||||||
sensor_events_sock->send((char*)bytes.begin(), bytes.size());
|
|
||||||
}
|
}
|
||||||
|
}
|
||||||
|
|
||||||
delete sensor_events_sock;
|
}// Namespace end
|
||||||
delete c;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
int main(int argc, char *argv[]) {
|
int main(int argc, char *argv[]) {
|
||||||
setpriority(PRIO_PROCESS, 0, -13);
|
setpriority(PRIO_PROCESS, 0, -13);
|
||||||
|
|
|
@ -272,6 +272,7 @@ class Plant():
|
||||||
'INTERCEPTOR_GAS',
|
'INTERCEPTOR_GAS',
|
||||||
'INTERCEPTOR_GAS2',
|
'INTERCEPTOR_GAS2',
|
||||||
'IMPERIAL_UNIT',
|
'IMPERIAL_UNIT',
|
||||||
|
'MOTOR_TORQUE',
|
||||||
])
|
])
|
||||||
vls = vls_tuple(
|
vls = vls_tuple(
|
||||||
self.speed_sensor(speed),
|
self.speed_sensor(speed),
|
||||||
|
@ -304,7 +305,8 @@ class Plant():
|
||||||
0, # Brake hold
|
0, # Brake hold
|
||||||
0, # Interceptor feedback
|
0, # Interceptor feedback
|
||||||
0, # Interceptor 2 feedback
|
0, # Interceptor 2 feedback
|
||||||
False
|
False,
|
||||||
|
0,
|
||||||
)
|
)
|
||||||
|
|
||||||
# TODO: publish each message at proper frequency
|
# TODO: publish each message at proper frequency
|
||||||
|
|
|
@ -159,11 +159,11 @@ def get_car_params(msgs, fsm, can_sock):
|
||||||
_, CP = get_car(can, sendcan)
|
_, CP = get_car(can, sendcan)
|
||||||
Params().put("CarParams", CP.to_bytes())
|
Params().put("CarParams", CP.to_bytes())
|
||||||
|
|
||||||
def radar_rcv_callback(msg, CP):
|
def radar_rcv_callback(msg, CP, cfg, fsm):
|
||||||
if msg.which() != "can":
|
if msg.which() != "can":
|
||||||
return []
|
return [], False
|
||||||
elif CP.radarOffCan:
|
elif CP.radarOffCan:
|
||||||
return ["radarState", "liveTracks"]
|
return ["radarState", "liveTracks"], True
|
||||||
|
|
||||||
radar_msgs = {"honda": [0x445], "toyota": [0x19f, 0x22f], "gm": [0x474],
|
radar_msgs = {"honda": [0x445], "toyota": [0x19f, 0x22f], "gm": [0x474],
|
||||||
"chrysler": [0x2d4]}.get(CP.carName, None)
|
"chrysler": [0x2d4]}.get(CP.carName, None)
|
||||||
|
@ -173,8 +173,15 @@ def radar_rcv_callback(msg, CP):
|
||||||
|
|
||||||
for m in msg.can:
|
for m in msg.can:
|
||||||
if m.src == 1 and m.address in radar_msgs:
|
if m.src == 1 and m.address in radar_msgs:
|
||||||
return ["radarState", "liveTracks"]
|
return ["radarState", "liveTracks"], True
|
||||||
return []
|
return [], False
|
||||||
|
|
||||||
|
def calibration_rcv_callback(msg, CP, cfg, fsm):
|
||||||
|
# calibrationd publishes 1 calibrationData every 5 cameraOdometry packets.
|
||||||
|
# should_recv always true to increment frame
|
||||||
|
recv_socks = ["liveCalibration"] if (fsm.frame + 1) % 5 == 0 else []
|
||||||
|
return recv_socks, True
|
||||||
|
|
||||||
|
|
||||||
CONFIGS = [
|
CONFIGS = [
|
||||||
ProcessConfig(
|
ProcessConfig(
|
||||||
|
@ -215,7 +222,7 @@ CONFIGS = [
|
||||||
},
|
},
|
||||||
ignore=[("logMonoTime", 0), ("valid", True)],
|
ignore=[("logMonoTime", 0), ("valid", True)],
|
||||||
init_callback=get_car_params,
|
init_callback=get_car_params,
|
||||||
should_recv_callback=None,
|
should_recv_callback=calibration_rcv_callback,
|
||||||
),
|
),
|
||||||
]
|
]
|
||||||
|
|
||||||
|
@ -263,12 +270,11 @@ def replay_process(cfg, lr):
|
||||||
log_msgs, msg_queue = [], []
|
log_msgs, msg_queue = [], []
|
||||||
for msg in tqdm(pub_msgs):
|
for msg in tqdm(pub_msgs):
|
||||||
if cfg.should_recv_callback is not None:
|
if cfg.should_recv_callback is not None:
|
||||||
recv_socks = cfg.should_recv_callback(msg, CP)
|
recv_socks, should_recv = cfg.should_recv_callback(msg, CP, cfg, fsm)
|
||||||
else:
|
else:
|
||||||
recv_socks = [s for s in cfg.pub_sub[msg.which()] if
|
recv_socks = [s for s in cfg.pub_sub[msg.which()] if
|
||||||
(fsm.frame + 1) % int(service_list[msg.which()].frequency / service_list[s].frequency) == 0]
|
(fsm.frame + 1) % int(service_list[msg.which()].frequency / service_list[s].frequency) == 0]
|
||||||
|
should_recv = bool(len(recv_socks))
|
||||||
should_recv = bool(len(recv_socks))
|
|
||||||
|
|
||||||
if msg.which() == 'can':
|
if msg.which() == 'can':
|
||||||
can_sock.send(msg.as_builder().to_bytes())
|
can_sock.send(msg.as_builder().to_bytes())
|
||||||
|
|
|
@ -1 +1 @@
|
||||||
89304bdcab73fa43a8dd39cab93bc4ea4c9cbbdb
|
b60841eb6cf09037200bc2daacf0c9cf69b358fe
|
|
@ -39,8 +39,6 @@ def wait_for_socket(name, timeout=10.0):
|
||||||
break
|
break
|
||||||
time.sleep(0.5)
|
time.sleep(0.5)
|
||||||
|
|
||||||
if r is None:
|
|
||||||
sys.exit(-1)
|
|
||||||
return r
|
return r
|
||||||
|
|
||||||
def get_route_logs(route_name):
|
def get_route_logs(route_name):
|
||||||
|
@ -405,42 +403,48 @@ passive_routes = [
|
||||||
#"bfa17080b080f3ec|2018-06-28--23-27-47",
|
#"bfa17080b080f3ec|2018-06-28--23-27-47",
|
||||||
]
|
]
|
||||||
|
|
||||||
|
forced_dashcam_routes = [
|
||||||
|
# Ford fusion
|
||||||
|
"f1b4c567731f4a1b|2018-04-18--11-29-37",
|
||||||
|
"f1b4c567731f4a1b|2018-04-30--10-15-35",
|
||||||
|
]
|
||||||
|
|
||||||
# TODO: replace all these with public routes
|
# TODO: replace all these with public routes
|
||||||
# TODO: add routes for untested cars: HONDA ACCORD 2018 HYBRID TOURING and CHRYSLER PACIFICA 2018
|
# TODO: add routes for untested cars: HONDA ACCORD 2018 HYBRID TOURING and CHRYSLER PACIFICA 2018
|
||||||
non_public_routes = [
|
non_public_routes = [
|
||||||
"0607d2516fc2148f|2019-02-13--23-03-16", # CHRYSLER PACIFICA HYBRID 2019
|
"0607d2516fc2148f|2019-02-13--23-03-16", # CHRYSLER PACIFICA HYBRID 2019
|
||||||
"3e9592a1c78a3d63|2018-02-08--20-28-24", # HONDA PILOT 2017 TOURING
|
"3e9592a1c78a3d63|2018-02-08--20-28-24", # HONDA PILOT 2017 TOURING
|
||||||
"aa20e335f61ba898|2019-02-05--16-59-04", # BUICK REGAL ESSENCE 2018
|
"aa20e335f61ba898|2019-02-05--16-59-04", # BUICK REGAL ESSENCE 2018
|
||||||
"1851183c395ef471|2018-05-31--18-07-21", # HONDA CR-V 2017 EX
|
"1851183c395ef471|2018-05-31--18-07-21", # HONDA CR-V 2017 EX
|
||||||
"9d5fb4f0baa1b3e1|2019-01-14--17-45-59", # KIA SORENTO GT LINE 2018
|
"9d5fb4f0baa1b3e1|2019-01-14--17-45-59", # KIA SORENTO GT LINE 2018
|
||||||
"b4c18bf13d5955da|2018-07-29--13-39-46", # TOYOTA C-HR HYBRID 2018
|
"b4c18bf13d5955da|2018-07-29--13-39-46", # TOYOTA C-HR HYBRID 2018
|
||||||
"5a2cfe4bb362af9e|2018-02-02--23-41-07", # ACURA RDX 2018 ACURAWATCH PLUS
|
"5a2cfe4bb362af9e|2018-02-02--23-41-07", # ACURA RDX 2018 ACURAWATCH PLUS
|
||||||
"362d23d4d5bea2fa|2018-08-10--13-31-40", # TOYOTA HIGHLANDER HYBRID 2018
|
"362d23d4d5bea2fa|2018-08-10--13-31-40", # TOYOTA HIGHLANDER HYBRID 2018
|
||||||
"aa20e335f61ba898|2018-12-17--21-10-37", # BUICK REGAL ESSENCE 2018
|
"aa20e335f61ba898|2018-12-17--21-10-37", # BUICK REGAL ESSENCE 2018
|
||||||
"215cd70e9c349266|2018-11-25--22-22-12", # KIA STINGER GT2 2018
|
"215cd70e9c349266|2018-11-25--22-22-12", # KIA STINGER GT2 2018
|
||||||
"192a598e34926b1e|2019-04-04--13-27-39", # JEEP GRAND CHEROKEE 2019
|
"192a598e34926b1e|2019-04-04--13-27-39", # JEEP GRAND CHEROKEE 2019
|
||||||
"34a84d2b765df688|2018-08-28--12-41-00", # HONDA PILOT 2019 ELITE
|
"34a84d2b765df688|2018-08-28--12-41-00", # HONDA PILOT 2019 ELITE
|
||||||
"b0c9d2329ad1606b|2019-01-06--10-11-23", # CHRYSLER PACIFICA HYBRID 2017
|
"b0c9d2329ad1606b|2019-01-06--10-11-23", # CHRYSLER PACIFICA HYBRID 2017
|
||||||
"31390e3eb6f7c29a|2019-01-23--08-56-00", # KIA OPTIMA SX 2019
|
"31390e3eb6f7c29a|2019-01-23--08-56-00", # KIA OPTIMA SX 2019
|
||||||
"fd10b9a107bb2e49|2018-07-24--16-32-42", # TOYOTA C-HR 2018
|
"fd10b9a107bb2e49|2018-07-24--16-32-42", # TOYOTA C-HR 2018
|
||||||
"9f7a7e50a51fb9db|2019-01-17--18-34-21", # JEEP GRAND CHEROKEE V6 2018
|
"9f7a7e50a51fb9db|2019-01-17--18-34-21", # JEEP GRAND CHEROKEE V6 2018
|
||||||
"aadda448b49c99ad|2018-10-25--17-16-22", # CHEVROLET MALIBU PREMIER 2017
|
"aadda448b49c99ad|2018-10-25--17-16-22", # CHEVROLET MALIBU PREMIER 2017
|
||||||
"362d23d4d5bea2fa|2018-09-02--17-03-55", # TOYOTA HIGHLANDER HYBRID 2018
|
"362d23d4d5bea2fa|2018-09-02--17-03-55", # TOYOTA HIGHLANDER HYBRID 2018
|
||||||
"1582e1dc57175194|2018-08-15--07-46-07", # HONDA ACCORD 2018 LX 1.5T
|
"1582e1dc57175194|2018-08-15--07-46-07", # HONDA ACCORD 2018 LX 1.5T
|
||||||
"fd10b9a107bb2e49|2018-07-24--20-32-08", # TOYOTA C-HR 2018
|
"fd10b9a107bb2e49|2018-07-24--20-32-08", # TOYOTA C-HR 2018
|
||||||
"265007255e794bce|2018-11-24--22-08-31", # CADILLAC ATS Premium Performance 2018
|
"265007255e794bce|2018-11-24--22-08-31", # CADILLAC ATS Premium Performance 2018
|
||||||
"53ac3251e03f95d7|2019-01-10--13-43-32", # HYUNDAI ELANTRA LIMITED ULTIMATE 2017
|
"53ac3251e03f95d7|2019-01-10--13-43-32", # HYUNDAI ELANTRA LIMITED ULTIMATE 2017
|
||||||
"21aa231dee2a68bd|2018-01-30--04-54-41", # HONDA ODYSSEY 2018 EX-L
|
"21aa231dee2a68bd|2018-01-30--04-54-41", # HONDA ODYSSEY 2018 EX-L
|
||||||
"900ad17e536c3dc7|2018-04-12--22-02-36", # HONDA RIDGELINE 2017 BLACK EDITION
|
"900ad17e536c3dc7|2018-04-12--22-02-36", # HONDA RIDGELINE 2017 BLACK EDITION
|
||||||
"975b26878285314d|2018-12-25--14-42-13", # CHRYSLER PACIFICA HYBRID 2018
|
"975b26878285314d|2018-12-25--14-42-13", # CHRYSLER PACIFICA HYBRID 2018
|
||||||
"8ae193ceb56a0efe|2018-06-18--20-07-32", # TOYOTA RAV4 HYBRID 2017
|
"8ae193ceb56a0efe|2018-06-18--20-07-32", # TOYOTA RAV4 HYBRID 2017
|
||||||
"a893a80e5c5f72c8|2019-01-14--20-02-59", # HYUNDAI GENESIS 2018
|
"a893a80e5c5f72c8|2019-01-14--20-02-59", # HYUNDAI GENESIS 2018
|
||||||
"49c73650e65ff465|2018-11-19--16-58-04", # HOLDEN ASTRA RS-V BK 2017
|
"49c73650e65ff465|2018-11-19--16-58-04", # HOLDEN ASTRA RS-V BK 2017
|
||||||
"d2d8152227f7cb82|2018-07-25--13-40-56", # TOYOTA CAMRY 2018
|
"d2d8152227f7cb82|2018-07-25--13-40-56", # TOYOTA CAMRY 2018
|
||||||
"07cb8a788c31f645|2018-06-17--18-50-29", # mock
|
"07cb8a788c31f645|2018-06-17--18-50-29", # mock
|
||||||
"c9d60e5e02c04c5c|2018-01-08--16-01-49", # HONDA CR-V 2016 TOURING
|
"c9d60e5e02c04c5c|2018-01-08--16-01-49", # HONDA CR-V 2016 TOURING
|
||||||
"1632088eda5e6c4d|2018-06-07--08-03-18", # HONDA CIVIC HATCHBACK 2017 SEDAN/COUPE 2019
|
"1632088eda5e6c4d|2018-06-07--08-03-18", # HONDA CIVIC HATCHBACK 2017 SEDAN/COUPE 2019
|
||||||
"fbd011384db5e669|2018-07-26--20-51-48", # TOYOTA CAMRY HYBRID 2018
|
"fbd011384db5e669|2018-07-26--20-51-48", # TOYOTA CAMRY HYBRID 2018
|
||||||
]
|
]
|
||||||
|
|
||||||
if __name__ == "__main__":
|
if __name__ == "__main__":
|
||||||
|
@ -458,96 +462,103 @@ if __name__ == "__main__":
|
||||||
elif "UNLOGGER_PATH" not in os.environ:
|
elif "UNLOGGER_PATH" not in os.environ:
|
||||||
continue
|
continue
|
||||||
|
|
||||||
for _ in range(3):
|
shutil.rmtree('/data/params')
|
||||||
shutil.rmtree('/data/params')
|
manager.gctx = {}
|
||||||
manager.gctx = {}
|
params = Params()
|
||||||
params = Params()
|
params.manager_start()
|
||||||
params.manager_start()
|
params.put("OpenpilotEnabledToggle", "1")
|
||||||
if route in passive_routes:
|
params.put("CommunityFeaturesToggle", "1")
|
||||||
params.put("Passive", "1")
|
|
||||||
else:
|
|
||||||
params.put("Passive", "0")
|
|
||||||
|
|
||||||
print("testing ", route, " ", checks['carFingerprint'])
|
if route in passive_routes:
|
||||||
print("Preparing processes")
|
params.put("Passive", "1")
|
||||||
manager.prepare_managed_process("radard")
|
else:
|
||||||
manager.prepare_managed_process("controlsd")
|
params.put("Passive", "0")
|
||||||
manager.prepare_managed_process("plannerd")
|
|
||||||
print("Starting processes")
|
|
||||||
manager.start_managed_process("radard")
|
|
||||||
manager.start_managed_process("controlsd")
|
|
||||||
manager.start_managed_process("plannerd")
|
|
||||||
time.sleep(2)
|
|
||||||
|
|
||||||
# Start unlogger
|
print("testing ", route, " ", checks['carFingerprint'])
|
||||||
print("Start unlogger")
|
print("Preparing processes")
|
||||||
if route in non_public_routes:
|
manager.prepare_managed_process("radard")
|
||||||
unlogger_cmd = [os.path.join(BASEDIR, os.environ['UNLOGGER_PATH']), '%s' % route, '--disable', 'frame,plan,pathPlan,liveLongitudinalMpc,radarState,controlsState,liveTracks,liveMpc,sendcan,carState,carControl,carEvents,carParams', '--no-interactive']
|
manager.prepare_managed_process("controlsd")
|
||||||
else:
|
manager.prepare_managed_process("plannerd")
|
||||||
unlogger_cmd = [os.path.join(BASEDIR, 'tools/replay/unlogger.py'), '%s' % route, '/tmp', '--disable', 'frame,plan,pathPlan,liveLongitudinalMpc,radarState,controlsState,liveTracks,liveMpc,sendcan,carState,carControl,carEvents,carParams', '--no-interactive']
|
print("Starting processes")
|
||||||
unlogger = subprocess.Popen(unlogger_cmd, preexec_fn=os.setsid)
|
manager.start_managed_process("radard")
|
||||||
|
manager.start_managed_process("controlsd")
|
||||||
|
manager.start_managed_process("plannerd")
|
||||||
|
time.sleep(2)
|
||||||
|
|
||||||
print("Check sockets")
|
# Start unlogger
|
||||||
controls_state_result = wait_for_socket('controlsState', timeout=30)
|
print("Start unlogger")
|
||||||
radarstate_result = wait_for_socket('radarState', timeout=30)
|
if route in non_public_routes:
|
||||||
plan_result = wait_for_socket('plan', timeout=30)
|
unlogger_cmd = [os.path.join(BASEDIR, os.environ['UNLOGGER_PATH']), '%s' % route, '--disable', 'frame,plan,pathPlan,liveLongitudinalMpc,radarState,controlsState,liveTracks,liveMpc,sendcan,carState,carControl,carEvents,carParams', '--no-interactive']
|
||||||
|
else:
|
||||||
|
unlogger_cmd = [os.path.join(BASEDIR, 'tools/replay/unlogger.py'), '%s' % route, '/tmp', '--disable', 'frame,plan,pathPlan,liveLongitudinalMpc,radarState,controlsState,liveTracks,liveMpc,sendcan,carState,carControl,carEvents,carParams', '--no-interactive']
|
||||||
|
unlogger = subprocess.Popen(unlogger_cmd, preexec_fn=os.setsid)
|
||||||
|
|
||||||
if route not in passive_routes: # TODO The passive routes have very flaky models
|
print("Check sockets")
|
||||||
path_plan_result = wait_for_socket('pathPlan', timeout=30)
|
controls_state_result = wait_for_socket('controlsState', timeout=30)
|
||||||
else:
|
|
||||||
path_plan_result = True
|
|
||||||
|
|
||||||
carstate_result = wait_for_socket('carState', timeout=30)
|
has_camera = checks.get('enableCamera', False)
|
||||||
|
if (route not in passive_routes) and (route not in forced_dashcam_routes) and has_camera:
|
||||||
|
controls_state_result = controls_state_result and wait_for_socket('sendcan', timeout=30)
|
||||||
|
|
||||||
print("Check if everything is running")
|
radarstate_result = wait_for_socket('radarState', timeout=30)
|
||||||
running = manager.get_running()
|
plan_result = wait_for_socket('plan', timeout=30)
|
||||||
controlsd_running = running['controlsd'].is_alive()
|
|
||||||
radard_running = running['radard'].is_alive()
|
|
||||||
plannerd_running = running['plannerd'].is_alive()
|
|
||||||
|
|
||||||
manager.kill_managed_process("controlsd")
|
if route not in passive_routes: # TODO The passive routes have very flaky models
|
||||||
manager.kill_managed_process("radard")
|
path_plan_result = wait_for_socket('pathPlan', timeout=30)
|
||||||
manager.kill_managed_process("plannerd")
|
else:
|
||||||
os.killpg(os.getpgid(unlogger.pid), signal.SIGTERM)
|
path_plan_result = True
|
||||||
|
|
||||||
sockets_ok = all([
|
carstate_result = wait_for_socket('carState', timeout=30)
|
||||||
controls_state_result, radarstate_result, plan_result, path_plan_result, carstate_result,
|
|
||||||
controlsd_running, radard_running, plannerd_running
|
|
||||||
])
|
|
||||||
params_ok = True
|
|
||||||
failures = []
|
|
||||||
|
|
||||||
if not controlsd_running:
|
print("Check if everything is running")
|
||||||
failures.append('controlsd')
|
running = manager.get_running()
|
||||||
if not radard_running:
|
controlsd_running = running['controlsd'].is_alive()
|
||||||
failures.append('radard')
|
radard_running = running['radard'].is_alive()
|
||||||
if not radarstate_result:
|
plannerd_running = running['plannerd'].is_alive()
|
||||||
failures.append('radarState')
|
|
||||||
if not controls_state_result:
|
|
||||||
failures.append('controlsState')
|
|
||||||
if not plan_result:
|
|
||||||
failures.append('plan')
|
|
||||||
if not path_plan_result:
|
|
||||||
failures.append('pathPlan')
|
|
||||||
|
|
||||||
try:
|
manager.kill_managed_process("controlsd")
|
||||||
car_params = car.CarParams.from_bytes(params.get("CarParams"))
|
manager.kill_managed_process("radard")
|
||||||
for k, v in checks.items():
|
manager.kill_managed_process("plannerd")
|
||||||
if not v == getattr(car_params, k):
|
os.killpg(os.getpgid(unlogger.pid), signal.SIGTERM)
|
||||||
params_ok = False
|
|
||||||
failures.append(k)
|
|
||||||
except:
|
|
||||||
params_ok = False
|
|
||||||
|
|
||||||
if sockets_ok and params_ok:
|
sockets_ok = all([
|
||||||
print("Success")
|
controls_state_result, radarstate_result, plan_result, path_plan_result, carstate_result,
|
||||||
results[route] = True, failures
|
controlsd_running, radard_running, plannerd_running
|
||||||
break
|
])
|
||||||
else:
|
params_ok = True
|
||||||
print("Failure")
|
failures = []
|
||||||
results[route] = False, failures
|
|
||||||
|
|
||||||
time.sleep(2)
|
if not controlsd_running:
|
||||||
|
failures.append('controlsd')
|
||||||
|
if not radard_running:
|
||||||
|
failures.append('radard')
|
||||||
|
if not radarstate_result:
|
||||||
|
failures.append('radarState')
|
||||||
|
if not controls_state_result:
|
||||||
|
failures.append('controlsState')
|
||||||
|
if not plan_result:
|
||||||
|
failures.append('plan')
|
||||||
|
if not path_plan_result:
|
||||||
|
failures.append('pathPlan')
|
||||||
|
|
||||||
|
try:
|
||||||
|
car_params = car.CarParams.from_bytes(params.get("CarParams"))
|
||||||
|
for k, v in checks.items():
|
||||||
|
if not v == getattr(car_params, k):
|
||||||
|
params_ok = False
|
||||||
|
failures.append(k)
|
||||||
|
except Exception:
|
||||||
|
params_ok = False
|
||||||
|
|
||||||
|
if sockets_ok and params_ok:
|
||||||
|
print("Success")
|
||||||
|
results[route] = True, failures
|
||||||
|
else:
|
||||||
|
print("Failure")
|
||||||
|
results[route] = False, failures
|
||||||
|
break
|
||||||
|
|
||||||
|
time.sleep(2)
|
||||||
|
|
||||||
for route in results:
|
for route in results:
|
||||||
print(results[route])
|
print(results[route])
|
||||||
|
|
|
@ -149,11 +149,11 @@ def test_athena():
|
||||||
assert False, f"Athena did not start within {timeout} seconds"
|
assert False, f"Athena did not start within {timeout} seconds"
|
||||||
time.sleep(0.5)
|
time.sleep(0.5)
|
||||||
|
|
||||||
def athena_post(payload, max_retries=5):
|
def athena_post(payload, max_retries=5, wait=5):
|
||||||
tries = 0
|
tries = 0
|
||||||
while 1:
|
while 1:
|
||||||
try:
|
try:
|
||||||
return requests.post(
|
resp = requests.post(
|
||||||
"https://athena.comma.ai/" + params.get("DongleId", encoding="utf-8"),
|
"https://athena.comma.ai/" + params.get("DongleId", encoding="utf-8"),
|
||||||
headers={
|
headers={
|
||||||
"Authorization": "JWT " + os.getenv("COMMA_JWT"),
|
"Authorization": "JWT " + os.getenv("COMMA_JWT"),
|
||||||
|
@ -162,29 +162,26 @@ def test_athena():
|
||||||
data=json.dumps(payload),
|
data=json.dumps(payload),
|
||||||
timeout=30
|
timeout=30
|
||||||
)
|
)
|
||||||
|
resp_json = resp.json()
|
||||||
|
if resp_json.get('error'):
|
||||||
|
raise Exception(resp_json['error'])
|
||||||
|
return resp_json
|
||||||
except Exception as e:
|
except Exception as e:
|
||||||
print(e)
|
time.sleep(wait)
|
||||||
time.sleep(5.0)
|
|
||||||
tries += 1
|
tries += 1
|
||||||
if tries == max_retries:
|
if tries == max_retries:
|
||||||
raise
|
raise
|
||||||
|
else:
|
||||||
|
print(f'athena_post failed {e}. retrying...')
|
||||||
|
|
||||||
def expect_athena_registers(timeout=60):
|
def expect_athena_registers():
|
||||||
now = time.time()
|
resp = athena_post({
|
||||||
while 1:
|
"method": "echo",
|
||||||
resp = athena_post({
|
"params": ["hello"],
|
||||||
"method": "echo",
|
"id": 0,
|
||||||
"params": ["hello"],
|
"jsonrpc": "2.0"
|
||||||
"id": 0,
|
}, max_retries=12, wait=5)
|
||||||
"jsonrpc": "2.0"
|
assert resp.get('result') == "hello", f'Athena failed to register ({resp})'
|
||||||
})
|
|
||||||
resp_json = resp.json()
|
|
||||||
if resp_json.get('result') == "hello":
|
|
||||||
break
|
|
||||||
elif time.time() - now > timeout:
|
|
||||||
assert False, f"Athena did not become available within {timeout} seconds."
|
|
||||||
else:
|
|
||||||
time.sleep(5.0)
|
|
||||||
|
|
||||||
try:
|
try:
|
||||||
athenad_pid = expect_athena_starts()
|
athenad_pid = expect_athena_starts()
|
||||||
|
@ -204,9 +201,8 @@ def test_athena():
|
||||||
"id": 0,
|
"id": 0,
|
||||||
"jsonrpc": "2.0"
|
"jsonrpc": "2.0"
|
||||||
})
|
})
|
||||||
resp_json = resp.json()
|
assert resp.get('result'), resp
|
||||||
assert resp_json.get('result'), resp_json
|
assert 'sim_id' in resp['result'], resp['result']
|
||||||
assert 'sim_id' in resp_json['result'], resp_json['result']
|
|
||||||
|
|
||||||
print("ATHENA: takeSnapshot")
|
print("ATHENA: takeSnapshot")
|
||||||
resp = athena_post({
|
resp = athena_post({
|
||||||
|
@ -214,9 +210,8 @@ def test_athena():
|
||||||
"id": 0,
|
"id": 0,
|
||||||
"jsonrpc": "2.0"
|
"jsonrpc": "2.0"
|
||||||
})
|
})
|
||||||
resp_json = resp.json()
|
assert resp.get('result'), resp
|
||||||
assert resp_json.get('result'), resp_json
|
assert resp['result']['jpegBack'], resp['result']
|
||||||
assert resp_json['result']['jpegBack'], resp_json['result']
|
|
||||||
|
|
||||||
@with_processes(["thermald"])
|
@with_processes(["thermald"])
|
||||||
def test_athena_thermal():
|
def test_athena_thermal():
|
||||||
|
@ -227,9 +222,8 @@ def test_athena():
|
||||||
"id": 0,
|
"id": 0,
|
||||||
"jsonrpc": "2.0"
|
"jsonrpc": "2.0"
|
||||||
})
|
})
|
||||||
resp_json = resp.json()
|
assert resp.get('result'), resp
|
||||||
assert resp_json.get('result'), resp_json
|
assert resp['result']['thermal'], resp['result']
|
||||||
assert resp_json['result']['thermal'], resp_json['result']
|
|
||||||
test_athena_thermal()
|
test_athena_thermal()
|
||||||
finally:
|
finally:
|
||||||
try:
|
try:
|
||||||
|
|
|
@ -6,6 +6,7 @@ import datetime
|
||||||
import psutil
|
import psutil
|
||||||
from smbus2 import SMBus
|
from smbus2 import SMBus
|
||||||
from cereal import log
|
from cereal import log
|
||||||
|
from common.android import ANDROID
|
||||||
from common.basedir import BASEDIR
|
from common.basedir import BASEDIR
|
||||||
from common.params import Params
|
from common.params import Params
|
||||||
from common.realtime import sec_since_boot, DT_TRML
|
from common.realtime import sec_since_boot, DT_TRML
|
||||||
|
@ -15,9 +16,9 @@ from selfdrive.version import terms_version, training_version
|
||||||
from selfdrive.swaglog import cloudlog
|
from selfdrive.swaglog import cloudlog
|
||||||
import cereal.messaging as messaging
|
import cereal.messaging as messaging
|
||||||
from selfdrive.loggerd.config import get_available_percent
|
from selfdrive.loggerd.config import get_available_percent
|
||||||
from selfdrive.pandad import get_expected_version
|
from selfdrive.pandad import get_expected_signature
|
||||||
|
|
||||||
FW_VERSION = get_expected_version()
|
FW_SIGNATURE = get_expected_signature()
|
||||||
|
|
||||||
ThermalStatus = log.ThermalData.ThermalStatus
|
ThermalStatus = log.ThermalData.ThermalStatus
|
||||||
CURRENT_TAU = 15. # 15s time constant
|
CURRENT_TAU = 15. # 15s time constant
|
||||||
|
@ -29,10 +30,13 @@ with open(BASEDIR + "/selfdrive/controls/lib/alerts_offroad.json") as json_file:
|
||||||
OFFROAD_ALERTS = json.load(json_file)
|
OFFROAD_ALERTS = json.load(json_file)
|
||||||
|
|
||||||
def read_tz(x, clip=True):
|
def read_tz(x, clip=True):
|
||||||
with open("/sys/devices/virtual/thermal/thermal_zone%d/temp" % x) as f:
|
try:
|
||||||
ret = int(f.read())
|
with open("/sys/devices/virtual/thermal/thermal_zone%d/temp" % x) as f:
|
||||||
if clip:
|
ret = int(f.read())
|
||||||
ret = max(0, ret)
|
if clip:
|
||||||
|
ret = max(0, ret)
|
||||||
|
except FileNotFoundError:
|
||||||
|
return 0
|
||||||
|
|
||||||
return ret
|
return ret
|
||||||
|
|
||||||
|
@ -156,7 +160,7 @@ def thermald_thread():
|
||||||
should_start_prev = False
|
should_start_prev = False
|
||||||
|
|
||||||
is_uno = (read_tz(29, clip=False) < -1000)
|
is_uno = (read_tz(29, clip=False) < -1000)
|
||||||
if is_uno:
|
if is_uno or not ANDROID:
|
||||||
handle_fan = handle_fan_uno
|
handle_fan = handle_fan_uno
|
||||||
else:
|
else:
|
||||||
setup_eon_fan()
|
setup_eon_fan()
|
||||||
|
@ -178,20 +182,28 @@ def thermald_thread():
|
||||||
if health is not None:
|
if health is not None:
|
||||||
usb_power = health.health.usbPowerMode != log.HealthData.UsbPowerMode.client
|
usb_power = health.health.usbPowerMode != log.HealthData.UsbPowerMode.client
|
||||||
|
|
||||||
msg.thermal.freeSpace = get_available_percent() / 100.0 # disk space
|
msg.thermal.freeSpace = get_available_percent(default=100.0) / 100.0
|
||||||
msg.thermal.memUsedPercent = int(round(psutil.virtual_memory().percent))
|
msg.thermal.memUsedPercent = int(round(psutil.virtual_memory().percent))
|
||||||
msg.thermal.cpuPerc = int(round(psutil.cpu_percent()))
|
msg.thermal.cpuPerc = int(round(psutil.cpu_percent()))
|
||||||
|
|
||||||
with open("/sys/class/power_supply/battery/capacity") as f:
|
try:
|
||||||
msg.thermal.batteryPercent = int(f.read())
|
with open("/sys/class/power_supply/battery/capacity") as f:
|
||||||
with open("/sys/class/power_supply/battery/status") as f:
|
msg.thermal.batteryPercent = int(f.read())
|
||||||
msg.thermal.batteryStatus = f.read().strip()
|
with open("/sys/class/power_supply/battery/status") as f:
|
||||||
with open("/sys/class/power_supply/battery/current_now") as f:
|
msg.thermal.batteryStatus = f.read().strip()
|
||||||
msg.thermal.batteryCurrent = int(f.read())
|
with open("/sys/class/power_supply/battery/current_now") as f:
|
||||||
with open("/sys/class/power_supply/battery/voltage_now") as f:
|
msg.thermal.batteryCurrent = int(f.read())
|
||||||
msg.thermal.batteryVoltage = int(f.read())
|
with open("/sys/class/power_supply/battery/voltage_now") as f:
|
||||||
with open("/sys/class/power_supply/usb/present") as f:
|
msg.thermal.batteryVoltage = int(f.read())
|
||||||
msg.thermal.usbOnline = bool(int(f.read()))
|
with open("/sys/class/power_supply/usb/present") as f:
|
||||||
|
msg.thermal.usbOnline = bool(int(f.read()))
|
||||||
|
except FileNotFoundError:
|
||||||
|
pass
|
||||||
|
|
||||||
|
# Fake battery levels on uno for frame
|
||||||
|
if is_uno:
|
||||||
|
msg.thermal.batteryPercent = 100
|
||||||
|
msg.thermal.batteryStatus = "Charging"
|
||||||
|
|
||||||
current_filter.update(msg.thermal.batteryCurrent / 1e6)
|
current_filter.update(msg.thermal.batteryCurrent / 1e6)
|
||||||
|
|
||||||
|
@ -268,8 +280,9 @@ def thermald_thread():
|
||||||
do_uninstall = params.get("DoUninstall") == b"1"
|
do_uninstall = params.get("DoUninstall") == b"1"
|
||||||
accepted_terms = params.get("HasAcceptedTerms") == terms_version
|
accepted_terms = params.get("HasAcceptedTerms") == terms_version
|
||||||
completed_training = params.get("CompletedTrainingVersion") == training_version
|
completed_training = params.get("CompletedTrainingVersion") == training_version
|
||||||
fw_version = params.get("PandaFirmware", encoding="utf8")
|
|
||||||
fw_version_match = fw_version is None or fw_version.startswith(FW_VERSION) # don't show alert is no panda is connected (None)
|
panda_signature = params.get("PandaFirmware")
|
||||||
|
fw_version_match = (panda_signature is None) or (panda_signature == FW_SIGNATURE) # don't show alert is no panda is connected (None)
|
||||||
|
|
||||||
should_start = ignition
|
should_start = ignition
|
||||||
|
|
||||||
|
@ -306,12 +319,18 @@ def thermald_thread():
|
||||||
params.delete("Offroad_TemperatureTooHigh")
|
params.delete("Offroad_TemperatureTooHigh")
|
||||||
|
|
||||||
if should_start:
|
if should_start:
|
||||||
|
if not should_start_prev:
|
||||||
|
params.delete("IsOffroad")
|
||||||
|
|
||||||
off_ts = None
|
off_ts = None
|
||||||
if started_ts is None:
|
if started_ts is None:
|
||||||
started_ts = sec_since_boot()
|
started_ts = sec_since_boot()
|
||||||
started_seen = True
|
started_seen = True
|
||||||
os.system('echo performance > /sys/class/devfreq/soc:qcom,cpubw/governor')
|
os.system('echo performance > /sys/class/devfreq/soc:qcom,cpubw/governor')
|
||||||
else:
|
else:
|
||||||
|
if should_start_prev or (count == 0):
|
||||||
|
params.put("IsOffroad", "1")
|
||||||
|
|
||||||
started_ts = None
|
started_ts = None
|
||||||
if off_ts is None:
|
if off_ts is None:
|
||||||
off_ts = sec_since_boot()
|
off_ts = sec_since_boot()
|
||||||
|
|
|
@ -1,5 +1,15 @@
|
||||||
Import('env', 'common', 'messaging', 'gpucommon', 'visionipc', 'cereal')
|
Import('env', 'arch', 'common', 'messaging', 'gpucommon', 'visionipc', 'cereal')
|
||||||
|
|
||||||
env.Program('_ui', ['ui.cc', 'slplay.c', '#phonelibs/nanovg/nanovg.c'],
|
src = ['ui.cc', 'paint.cc', '#phonelibs/nanovg/nanovg.c']
|
||||||
|
libs = [common, 'zmq', 'czmq', 'capnp', 'capnp_c', 'm', cereal, 'json', messaging, 'OpenCL', gpucommon, visionipc]
|
||||||
|
|
||||||
|
if arch == "aarch64":
|
||||||
|
src += ['sound.cc', 'slplay.c']
|
||||||
|
libs += ['EGL', 'GLESv3', 'gnustl_shared', 'log', 'utils', 'gui', 'hardware', 'ui', 'CB', 'gsl', 'adreno_utils', 'OpenSLES', 'cutils', 'uuid']
|
||||||
|
else:
|
||||||
|
src += ['linux.cc']
|
||||||
|
libs += ['EGL', 'pthread', 'X11-xcb', 'xcb', 'X11', 'glfw']
|
||||||
|
|
||||||
|
env.Program('_ui', src,
|
||||||
LINKFLAGS=['-Wl,-rpath=/system/lib64,-rpath=/system/comma/usr/lib'],
|
LINKFLAGS=['-Wl,-rpath=/system/lib64,-rpath=/system/comma/usr/lib'],
|
||||||
LIBS=[common, 'zmq', 'czmq', 'capnp', 'capnp_c', 'm', 'GLESv3', 'EGL', cereal, 'gnustl_shared', 'log', 'utils', 'gui', 'hardware', 'ui', 'json', messaging, 'CB', 'OpenCL', 'gsl', 'adreno_utils', 'OpenSLES', 'cutils', 'uuid', gpucommon, visionipc])
|
LIBS=libs)
|
||||||
|
|
|
@ -0,0 +1,100 @@
|
||||||
|
#include <stdio.h>
|
||||||
|
#include <stdint.h>
|
||||||
|
#include <stdlib.h>
|
||||||
|
#include <assert.h>
|
||||||
|
#include <string.h>
|
||||||
|
|
||||||
|
#include "ui.hpp"
|
||||||
|
|
||||||
|
#define GLFW_INCLUDE_ES2
|
||||||
|
#define GLFW_INCLUDE_GLEXT
|
||||||
|
#include <GLFW/glfw3.h>
|
||||||
|
|
||||||
|
typedef struct FramebufferState FramebufferState;
|
||||||
|
typedef struct TouchState TouchState;
|
||||||
|
|
||||||
|
#define FALSE 0
|
||||||
|
#define TRUE 1
|
||||||
|
|
||||||
|
#include <xcb/xcb.h>
|
||||||
|
#include <X11/Xlib-xcb.h>
|
||||||
|
|
||||||
|
extern "C" {
|
||||||
|
|
||||||
|
FramebufferState* framebuffer_init(
|
||||||
|
const char* name, int32_t layer, int alpha,
|
||||||
|
int *out_w, int *out_h) {
|
||||||
|
glfwInit();
|
||||||
|
|
||||||
|
glfwWindowHint(GLFW_CLIENT_API, GLFW_OPENGL_ES_API);
|
||||||
|
glfwWindowHint(GLFW_CONTEXT_VERSION_MAJOR, 3);
|
||||||
|
glfwWindowHint(GLFW_CONTEXT_VERSION_MINOR, 0);
|
||||||
|
glfwWindowHint(GLFW_RESIZABLE, 0);
|
||||||
|
GLFWwindow* window;
|
||||||
|
window = glfwCreateWindow(1920, 1080, "ui", NULL, NULL);
|
||||||
|
if (!window) {
|
||||||
|
printf("glfwCreateWindow failed\n");
|
||||||
|
}
|
||||||
|
|
||||||
|
glfwMakeContextCurrent(window);
|
||||||
|
glfwSwapInterval(0);
|
||||||
|
|
||||||
|
// clear screen
|
||||||
|
glClearColor(0.2f, 0.2f, 0.2f, 1.0f );
|
||||||
|
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
|
||||||
|
framebuffer_swap((FramebufferState*)window);
|
||||||
|
|
||||||
|
if (out_w) *out_w = 1920;
|
||||||
|
if (out_h) *out_h = 1080;
|
||||||
|
|
||||||
|
return (FramebufferState*)window;
|
||||||
|
}
|
||||||
|
|
||||||
|
void framebuffer_set_power(FramebufferState *s, int mode) {
|
||||||
|
}
|
||||||
|
|
||||||
|
void framebuffer_swap(FramebufferState *s) {
|
||||||
|
glfwSwapBuffers((GLFWwindow*)s);
|
||||||
|
}
|
||||||
|
|
||||||
|
void touch_init(TouchState *s) {
|
||||||
|
printf("touch_init\n");
|
||||||
|
}
|
||||||
|
|
||||||
|
int touch_poll(TouchState *s, int* out_x, int* out_y, int timeout) {
|
||||||
|
return -1;
|
||||||
|
}
|
||||||
|
|
||||||
|
int touch_read(TouchState *s, int* out_x, int* out_y) {
|
||||||
|
return -1;
|
||||||
|
}
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
#include "sound.hpp"
|
||||||
|
|
||||||
|
void ui_sound_init() {}
|
||||||
|
void ui_sound_destroy() {}
|
||||||
|
|
||||||
|
void set_volume(int volume) {}
|
||||||
|
|
||||||
|
void play_alert_sound(AudibleAlert alert) {}
|
||||||
|
void stop_alert_sound(AudibleAlert alert) {}
|
||||||
|
|
||||||
|
#include "common/visionimg.h"
|
||||||
|
#include <sys/mman.h>
|
||||||
|
|
||||||
|
GLuint visionimg_to_gl(const VisionImg *img, EGLImageKHR *pkhr, void **pph) {
|
||||||
|
unsigned int texture;
|
||||||
|
glGenTextures(1, &texture);
|
||||||
|
glBindTexture(GL_TEXTURE_2D, texture);
|
||||||
|
glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB, img->width, img->height, 0, GL_RGB, GL_UNSIGNED_BYTE, *pph);
|
||||||
|
glGenerateMipmap(GL_TEXTURE_2D);
|
||||||
|
*pkhr = (EGLImageKHR *)1; // not NULL
|
||||||
|
return texture;
|
||||||
|
}
|
||||||
|
|
||||||
|
void visionimg_destroy_gl(EGLImageKHR khr, void *ph) {
|
||||||
|
// empty
|
||||||
|
}
|
||||||
|
|
File diff suppressed because it is too large
Load Diff
|
@ -0,0 +1,85 @@
|
||||||
|
#include <stdlib.h>
|
||||||
|
#include "sound.hpp"
|
||||||
|
|
||||||
|
#include "common/swaglog.h"
|
||||||
|
|
||||||
|
typedef struct {
|
||||||
|
AudibleAlert alert;
|
||||||
|
const char* uri;
|
||||||
|
bool loop;
|
||||||
|
} sound_file;
|
||||||
|
|
||||||
|
extern "C"{
|
||||||
|
#include "slplay.h"
|
||||||
|
}
|
||||||
|
|
||||||
|
void set_volume(int volume) {
|
||||||
|
char volume_change_cmd[64];
|
||||||
|
sprintf(volume_change_cmd, "service call audio 3 i32 3 i32 %d i32 1 &", volume);
|
||||||
|
|
||||||
|
// 5 second timeout at 60fps
|
||||||
|
int volume_changed = system(volume_change_cmd);
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
sound_file sound_table[] = {
|
||||||
|
{ cereal_CarControl_HUDControl_AudibleAlert_chimeDisengage, "../assets/sounds/disengaged.wav", false },
|
||||||
|
{ cereal_CarControl_HUDControl_AudibleAlert_chimeEngage, "../assets/sounds/engaged.wav", false },
|
||||||
|
{ cereal_CarControl_HUDControl_AudibleAlert_chimeWarning1, "../assets/sounds/warning_1.wav", false },
|
||||||
|
{ cereal_CarControl_HUDControl_AudibleAlert_chimeWarning2, "../assets/sounds/warning_2.wav", false },
|
||||||
|
{ cereal_CarControl_HUDControl_AudibleAlert_chimeWarningRepeat, "../assets/sounds/warning_repeat.wav", true },
|
||||||
|
{ cereal_CarControl_HUDControl_AudibleAlert_chimeError, "../assets/sounds/error.wav", false },
|
||||||
|
{ cereal_CarControl_HUDControl_AudibleAlert_chimePrompt, "../assets/sounds/error.wav", false },
|
||||||
|
{ cereal_CarControl_HUDControl_AudibleAlert_none, NULL, false },
|
||||||
|
};
|
||||||
|
|
||||||
|
sound_file* get_sound_file(AudibleAlert alert) {
|
||||||
|
for (sound_file *s = sound_table; s->alert != cereal_CarControl_HUDControl_AudibleAlert_none; s++) {
|
||||||
|
if (s->alert == alert) {
|
||||||
|
return s;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
return NULL;
|
||||||
|
}
|
||||||
|
|
||||||
|
void play_alert_sound(AudibleAlert alert) {
|
||||||
|
sound_file* sound = get_sound_file(alert);
|
||||||
|
char* error = NULL;
|
||||||
|
|
||||||
|
slplay_play(sound->uri, sound->loop, &error);
|
||||||
|
if(error) {
|
||||||
|
LOGW("error playing sound: %s", error);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void stop_alert_sound(AudibleAlert alert) {
|
||||||
|
sound_file* sound = get_sound_file(alert);
|
||||||
|
char* error = NULL;
|
||||||
|
|
||||||
|
slplay_stop_uri(sound->uri, &error);
|
||||||
|
if(error) {
|
||||||
|
LOGW("error stopping sound: %s", error);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void ui_sound_init() {
|
||||||
|
char *error = NULL;
|
||||||
|
slplay_setup(&error);
|
||||||
|
if (error) goto fail;
|
||||||
|
|
||||||
|
for (sound_file *s = sound_table; s->alert != cereal_CarControl_HUDControl_AudibleAlert_none; s++) {
|
||||||
|
slplay_create_player_for_uri(s->uri, &error);
|
||||||
|
if (error) goto fail;
|
||||||
|
}
|
||||||
|
return;
|
||||||
|
|
||||||
|
fail:
|
||||||
|
LOGW(error);
|
||||||
|
exit(1);
|
||||||
|
}
|
||||||
|
|
||||||
|
void ui_sound_destroy() {
|
||||||
|
slplay_destroy();
|
||||||
|
}
|
||||||
|
|
|
@ -0,0 +1,17 @@
|
||||||
|
#ifndef __SOUND_HPP
|
||||||
|
#define __SOUND_HPP
|
||||||
|
|
||||||
|
#include "cereal/gen/c/log.capnp.h"
|
||||||
|
|
||||||
|
typedef enum cereal_CarControl_HUDControl_AudibleAlert AudibleAlert;
|
||||||
|
|
||||||
|
void ui_sound_init();
|
||||||
|
void ui_sound_destroy();
|
||||||
|
|
||||||
|
void set_volume(int volume);
|
||||||
|
|
||||||
|
void play_alert_sound(AudibleAlert alert);
|
||||||
|
void stop_alert_sound(AudibleAlert alert);
|
||||||
|
|
||||||
|
#endif
|
||||||
|
|
1424
selfdrive/ui/ui.cc
1424
selfdrive/ui/ui.cc
File diff suppressed because it is too large
Load Diff
|
@ -0,0 +1,253 @@
|
||||||
|
#ifndef _UI_H
|
||||||
|
#define _UI_H
|
||||||
|
|
||||||
|
#include <GLES3/gl3.h>
|
||||||
|
#include <EGL/egl.h>
|
||||||
|
|
||||||
|
#include "nanovg.h"
|
||||||
|
|
||||||
|
#include "common/mat.h"
|
||||||
|
#include "common/visionipc.h"
|
||||||
|
#include "common/framebuffer.h"
|
||||||
|
#include "common/modeldata.h"
|
||||||
|
#include "messaging.hpp"
|
||||||
|
|
||||||
|
#include "cereal/gen/c/log.capnp.h"
|
||||||
|
|
||||||
|
#include "sound.hpp"
|
||||||
|
|
||||||
|
#define STATUS_STOPPED 0
|
||||||
|
#define STATUS_DISENGAGED 1
|
||||||
|
#define STATUS_ENGAGED 2
|
||||||
|
#define STATUS_WARNING 3
|
||||||
|
#define STATUS_ALERT 4
|
||||||
|
|
||||||
|
#define ALERTSIZE_NONE 0
|
||||||
|
#define ALERTSIZE_SMALL 1
|
||||||
|
#define ALERTSIZE_MID 2
|
||||||
|
#define ALERTSIZE_FULL 3
|
||||||
|
|
||||||
|
#ifndef QCOM
|
||||||
|
#define UI_60FPS
|
||||||
|
#endif
|
||||||
|
|
||||||
|
#define UI_BUF_COUNT 4
|
||||||
|
//#define SHOW_SPEEDLIMIT 1
|
||||||
|
//#define DEBUG_TURN
|
||||||
|
|
||||||
|
const int vwp_w = 1920;
|
||||||
|
const int vwp_h = 1080;
|
||||||
|
const int nav_w = 640;
|
||||||
|
const int nav_ww= 760;
|
||||||
|
const int sbr_w = 300;
|
||||||
|
const int bdr_s = 30;
|
||||||
|
const int box_x = sbr_w+bdr_s;
|
||||||
|
const int box_y = bdr_s;
|
||||||
|
const int box_w = vwp_w-sbr_w-(bdr_s*2);
|
||||||
|
const int box_h = vwp_h-(bdr_s*2);
|
||||||
|
const int viz_w = vwp_w-(bdr_s*2);
|
||||||
|
const int header_h = 420;
|
||||||
|
const int footer_h = 280;
|
||||||
|
const int footer_y = vwp_h-bdr_s-footer_h;
|
||||||
|
|
||||||
|
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 int SET_SPEED_NA = 255;
|
||||||
|
|
||||||
|
const uint8_t bg_colors[][4] = {
|
||||||
|
[STATUS_STOPPED] = {0x07, 0x23, 0x39, 0xff},
|
||||||
|
[STATUS_DISENGAGED] = {0x17, 0x33, 0x49, 0xff},
|
||||||
|
[STATUS_ENGAGED] = {0x17, 0x86, 0x44, 0xff},
|
||||||
|
[STATUS_WARNING] = {0xDA, 0x6F, 0x25, 0xff},
|
||||||
|
[STATUS_ALERT] = {0xC9, 0x22, 0x31, 0xff},
|
||||||
|
};
|
||||||
|
|
||||||
|
|
||||||
|
typedef struct UIScene {
|
||||||
|
int frontview;
|
||||||
|
int fullview;
|
||||||
|
|
||||||
|
int transformed_width, transformed_height;
|
||||||
|
|
||||||
|
ModelData model;
|
||||||
|
|
||||||
|
float mpc_x[50];
|
||||||
|
float mpc_y[50];
|
||||||
|
|
||||||
|
bool world_objects_visible;
|
||||||
|
mat4 extrinsic_matrix; // Last row is 0 so we can use mat4.
|
||||||
|
|
||||||
|
float v_cruise;
|
||||||
|
uint64_t v_cruise_update_ts;
|
||||||
|
float v_ego;
|
||||||
|
bool decel_for_model;
|
||||||
|
|
||||||
|
float speedlimit;
|
||||||
|
bool speedlimit_valid;
|
||||||
|
bool map_valid;
|
||||||
|
|
||||||
|
float curvature;
|
||||||
|
int engaged;
|
||||||
|
bool engageable;
|
||||||
|
bool monitoring_active;
|
||||||
|
|
||||||
|
bool uilayout_sidebarcollapsed;
|
||||||
|
bool uilayout_mapenabled;
|
||||||
|
// responsive layout
|
||||||
|
int ui_viz_rx;
|
||||||
|
int ui_viz_rw;
|
||||||
|
int ui_viz_ro;
|
||||||
|
|
||||||
|
int lead_status;
|
||||||
|
float lead_d_rel, lead_y_rel, lead_v_rel;
|
||||||
|
|
||||||
|
int front_box_x, front_box_y, front_box_width, front_box_height;
|
||||||
|
|
||||||
|
uint64_t alert_ts;
|
||||||
|
char alert_text1[1024];
|
||||||
|
char alert_text2[1024];
|
||||||
|
uint8_t alert_size;
|
||||||
|
float alert_blinkingrate;
|
||||||
|
|
||||||
|
float awareness_status;
|
||||||
|
|
||||||
|
// Used to show gps planner status
|
||||||
|
bool gps_planner_active;
|
||||||
|
} 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;
|
||||||
|
|
||||||
|
// framebuffer
|
||||||
|
FramebufferState *fb;
|
||||||
|
int fb_w, fb_h;
|
||||||
|
EGLDisplay display;
|
||||||
|
EGLSurface surface;
|
||||||
|
|
||||||
|
// NVG
|
||||||
|
NVGcontext *vg;
|
||||||
|
|
||||||
|
// fonts and images
|
||||||
|
int font_courbd;
|
||||||
|
int font_sans_regular;
|
||||||
|
int font_sans_semibold;
|
||||||
|
int font_sans_bold;
|
||||||
|
int img_wheel;
|
||||||
|
int img_turn;
|
||||||
|
int img_face;
|
||||||
|
int img_map;
|
||||||
|
|
||||||
|
// sockets
|
||||||
|
Context *ctx;
|
||||||
|
SubSocket *model_sock;
|
||||||
|
SubSocket *controlsstate_sock;
|
||||||
|
SubSocket *livecalibration_sock;
|
||||||
|
SubSocket *radarstate_sock;
|
||||||
|
SubSocket *map_data_sock;
|
||||||
|
SubSocket *uilayout_sock;
|
||||||
|
Poller * poller;
|
||||||
|
|
||||||
|
int active_app;
|
||||||
|
|
||||||
|
// vision state
|
||||||
|
bool vision_connected;
|
||||||
|
bool vision_connect_firstrun;
|
||||||
|
int ipc_fd;
|
||||||
|
|
||||||
|
VIPCBuf bufs[UI_BUF_COUNT];
|
||||||
|
VIPCBuf front_bufs[UI_BUF_COUNT];
|
||||||
|
int cur_vision_idx;
|
||||||
|
int cur_vision_front_idx;
|
||||||
|
|
||||||
|
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;
|
||||||
|
|
||||||
|
GLuint line_program;
|
||||||
|
GLint line_pos_loc, line_color_loc;
|
||||||
|
GLint line_transform_loc;
|
||||||
|
|
||||||
|
int rgb_width, rgb_height, rgb_stride;
|
||||||
|
size_t rgb_buf_len;
|
||||||
|
mat4 rgb_transform;
|
||||||
|
|
||||||
|
int rgb_front_width, rgb_front_height, rgb_front_stride;
|
||||||
|
size_t rgb_front_buf_len;
|
||||||
|
|
||||||
|
UIScene scene;
|
||||||
|
bool awake;
|
||||||
|
|
||||||
|
// timeouts
|
||||||
|
int awake_timeout;
|
||||||
|
int volume_timeout;
|
||||||
|
int controls_timeout;
|
||||||
|
int alert_sound_timeout;
|
||||||
|
int speed_lim_off_timeout;
|
||||||
|
int is_metric_timeout;
|
||||||
|
int longitudinal_control_timeout;
|
||||||
|
int limit_set_speed_timeout;
|
||||||
|
|
||||||
|
bool controls_seen;
|
||||||
|
|
||||||
|
int status;
|
||||||
|
bool is_metric;
|
||||||
|
bool longitudinal_control;
|
||||||
|
bool limit_set_speed;
|
||||||
|
float speed_lim_off;
|
||||||
|
bool is_ego_over_limit;
|
||||||
|
char alert_type[64];
|
||||||
|
AudibleAlert alert_sound;
|
||||||
|
int alert_size;
|
||||||
|
float alert_blinking_alpha;
|
||||||
|
bool alert_blinked;
|
||||||
|
|
||||||
|
float light_sensor;
|
||||||
|
|
||||||
|
int touch_fd;
|
||||||
|
|
||||||
|
// Hints for re-calculations and redrawing
|
||||||
|
bool model_changed;
|
||||||
|
bool livempc_or_radarstate_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;
|
||||||
|
|
||||||
|
// API
|
||||||
|
void ui_draw_vision_alert(UIState *s, int va_size, int va_color,
|
||||||
|
const char* va_text1, const char* va_text2);
|
||||||
|
void ui_draw(UIState *s);
|
||||||
|
void ui_nvg_init(UIState *s);
|
||||||
|
|
||||||
|
#endif
|
|
@ -1,57 +1,361 @@
|
||||||
#!/usr/bin/env python3
|
#!/usr/bin/env python3
|
||||||
|
|
||||||
# simple service that waits for network access and tries to update every hour
|
# Safe Update: A simple service that waits for network access and tries to
|
||||||
|
# update every 10 minutes. It's intended to make the OP update process more
|
||||||
|
# robust against Git repository corruption. This service DOES NOT try to fix
|
||||||
|
# an already-corrupt BASEDIR Git repo, only prevent it from happening.
|
||||||
|
#
|
||||||
|
# During normal operation, both onroad and offroad, the update process makes
|
||||||
|
# no changes to the BASEDIR install of OP. All update attempts are performed
|
||||||
|
# in a disposable staging area provided by OverlayFS. It assumes the deleter
|
||||||
|
# process provides enough disk space to carry out the process.
|
||||||
|
#
|
||||||
|
# If an update succeeds, a flag is set, and the update is swapped in at the
|
||||||
|
# next reboot. If an update is interrupted or otherwise fails, the OverlayFS
|
||||||
|
# upper layer and metadata can be discarded before trying again.
|
||||||
|
#
|
||||||
|
# The swap on boot is triggered by launch_chffrplus.sh
|
||||||
|
# gated on the existence of $FINALIZED/.overlay_consistent and also the
|
||||||
|
# existence and mtime of $BASEDIR/.overlay_init.
|
||||||
|
#
|
||||||
|
# Other than build byproducts, BASEDIR should not be modified while this
|
||||||
|
# service is running. Developers modifying code directly in BASEDIR should
|
||||||
|
# disable this service.
|
||||||
|
|
||||||
|
import os
|
||||||
import datetime
|
import datetime
|
||||||
import subprocess
|
import subprocess
|
||||||
import time
|
import psutil
|
||||||
|
from stat import S_ISREG, S_ISDIR, S_ISLNK, S_IMODE, ST_MODE, ST_INO, ST_UID, ST_GID, ST_ATIME, ST_MTIME
|
||||||
|
import shutil
|
||||||
|
import signal
|
||||||
|
from pathlib import Path
|
||||||
|
import fcntl
|
||||||
|
import threading
|
||||||
|
from cffi import FFI
|
||||||
|
|
||||||
|
from common.basedir import BASEDIR
|
||||||
from common.params import Params
|
from common.params import Params
|
||||||
from selfdrive.swaglog import cloudlog
|
from selfdrive.swaglog import cloudlog
|
||||||
|
|
||||||
|
STAGING_ROOT = "/data/safe_staging"
|
||||||
|
|
||||||
|
OVERLAY_UPPER = os.path.join(STAGING_ROOT, "upper")
|
||||||
|
OVERLAY_METADATA = os.path.join(STAGING_ROOT, "metadata")
|
||||||
|
OVERLAY_MERGED = os.path.join(STAGING_ROOT, "merged")
|
||||||
|
FINALIZED = os.path.join(STAGING_ROOT, "finalized")
|
||||||
|
|
||||||
NICE_LOW_PRIORITY = ["nice", "-n", "19"]
|
NICE_LOW_PRIORITY = ["nice", "-n", "19"]
|
||||||
def main(gctx=None):
|
SHORT = os.getenv("SHORT") is not None
|
||||||
|
|
||||||
|
# Workaround for the EON/termux build of Python having os.link removed.
|
||||||
|
ffi = FFI()
|
||||||
|
ffi.cdef("int link(const char *oldpath, const char *newpath);")
|
||||||
|
libc = ffi.dlopen(None)
|
||||||
|
|
||||||
|
|
||||||
|
class WaitTimeHelper:
|
||||||
|
ready_event = threading.Event()
|
||||||
|
shutdown = False
|
||||||
|
|
||||||
|
def __init__(self):
|
||||||
|
signal.signal(signal.SIGTERM, self.graceful_shutdown)
|
||||||
|
signal.signal(signal.SIGINT, self.graceful_shutdown)
|
||||||
|
signal.signal(signal.SIGHUP, self.update_now)
|
||||||
|
|
||||||
|
def graceful_shutdown(self, signum, frame):
|
||||||
|
# umount -f doesn't appear effective in avoiding "device busy" on EON,
|
||||||
|
# so don't actually die until the next convenient opportunity in main().
|
||||||
|
cloudlog.info("caught SIGINT/SIGTERM, dismounting overlay at next opportunity")
|
||||||
|
self.shutdown = True
|
||||||
|
self.ready_event.set()
|
||||||
|
|
||||||
|
def update_now(self, signum, frame):
|
||||||
|
cloudlog.info("caught SIGHUP, running update check immediately")
|
||||||
|
self.ready_event.set()
|
||||||
|
|
||||||
|
|
||||||
|
def wait_between_updates(ready_event):
|
||||||
|
ready_event.clear()
|
||||||
|
if SHORT:
|
||||||
|
ready_event.wait(timeout=10)
|
||||||
|
else:
|
||||||
|
ready_event.wait(timeout=60 * 10)
|
||||||
|
|
||||||
|
|
||||||
|
def link(src, dest):
|
||||||
|
# Workaround for the EON/termux build of Python having os.link removed.
|
||||||
|
return libc.link(src.encode(), dest.encode())
|
||||||
|
|
||||||
|
|
||||||
|
def run(cmd, cwd=None):
|
||||||
|
return subprocess.check_output(cmd, cwd=cwd, stderr=subprocess.STDOUT, encoding='utf8')
|
||||||
|
|
||||||
|
|
||||||
|
def remove_consistent_flag():
|
||||||
|
os.system("sync")
|
||||||
|
consistent_file = Path(os.path.join(FINALIZED, ".overlay_consistent"))
|
||||||
|
try:
|
||||||
|
consistent_file.unlink()
|
||||||
|
except FileNotFoundError:
|
||||||
|
pass
|
||||||
|
os.system("sync")
|
||||||
|
|
||||||
|
|
||||||
|
def set_consistent_flag():
|
||||||
|
consistent_file = Path(os.path.join(FINALIZED, ".overlay_consistent"))
|
||||||
|
os.system("sync")
|
||||||
|
consistent_file.touch()
|
||||||
|
os.system("sync")
|
||||||
|
|
||||||
|
|
||||||
|
def set_update_available_params(new_version=False):
|
||||||
params = Params()
|
params = Params()
|
||||||
|
|
||||||
|
t = datetime.datetime.now().isoformat()
|
||||||
|
params.put("LastUpdateTime", t.encode('utf8'))
|
||||||
|
|
||||||
|
if new_version:
|
||||||
|
try:
|
||||||
|
with open(os.path.join(FINALIZED, "RELEASES.md"), "rb") as f:
|
||||||
|
r = f.read()
|
||||||
|
r = r[:r.find(b'\n\n')] # Slice latest release notes
|
||||||
|
params.put("ReleaseNotes", r + b"\n")
|
||||||
|
except Exception:
|
||||||
|
params.put("ReleaseNotes", "")
|
||||||
|
params.put("UpdateAvailable", "1")
|
||||||
|
|
||||||
|
|
||||||
|
def dismount_ovfs():
|
||||||
|
if os.path.ismount(OVERLAY_MERGED):
|
||||||
|
cloudlog.error("unmounting existing overlay")
|
||||||
|
run(["umount", "-l", OVERLAY_MERGED])
|
||||||
|
|
||||||
|
|
||||||
|
def init_ovfs():
|
||||||
|
cloudlog.info("preparing new safe staging area")
|
||||||
|
Params().put("UpdateAvailable", "0")
|
||||||
|
|
||||||
|
remove_consistent_flag()
|
||||||
|
|
||||||
|
dismount_ovfs()
|
||||||
|
if os.path.isdir(STAGING_ROOT):
|
||||||
|
shutil.rmtree(STAGING_ROOT)
|
||||||
|
|
||||||
|
for dirname in [STAGING_ROOT, OVERLAY_UPPER, OVERLAY_METADATA, OVERLAY_MERGED, FINALIZED]:
|
||||||
|
os.mkdir(dirname, 0o755)
|
||||||
|
if not os.lstat(BASEDIR).st_dev == os.lstat(OVERLAY_MERGED).st_dev:
|
||||||
|
raise RuntimeError("base and overlay merge directories are on different filesystems; not valid for overlay FS!")
|
||||||
|
|
||||||
|
# Remove consistent flag from current BASEDIR so it's not copied over
|
||||||
|
if os.path.isfile(os.path.join(BASEDIR, ".overlay_consistent")):
|
||||||
|
os.remove(os.path.join(BASEDIR, ".overlay_consistent"))
|
||||||
|
|
||||||
|
# We sync FS object atimes (which EON doesn't use) and mtimes, but ctimes
|
||||||
|
# are outside user control. Make sure Git is set up to ignore system ctimes,
|
||||||
|
# because they change when we make hard links during finalize. Otherwise,
|
||||||
|
# there is a lot of unnecessary churn. This appears to be a common need on
|
||||||
|
# OSX as well: https://www.git-tower.com/blog/make-git-rebase-safe-on-osx/
|
||||||
|
run(["git", "config", "core.trustctime", "false"], BASEDIR)
|
||||||
|
|
||||||
|
# We are temporarily using copytree to copy the directory, which also changes
|
||||||
|
# inode numbers. Ignore those changes too.
|
||||||
|
run(["git", "config", "core.checkStat", "minimal"], BASEDIR)
|
||||||
|
|
||||||
|
# Leave a timestamped canary in BASEDIR to check at startup. The EON clock
|
||||||
|
# should be correct by the time we get here. If the init file disappears, or
|
||||||
|
# critical mtimes in BASEDIR are newer than .overlay_init, continue.sh can
|
||||||
|
# assume that BASEDIR has used for local development or otherwise modified,
|
||||||
|
# and skips the update activation attempt.
|
||||||
|
Path(os.path.join(BASEDIR, ".overlay_init")).touch()
|
||||||
|
|
||||||
|
overlay_opts = f"lowerdir={BASEDIR},upperdir={OVERLAY_UPPER},workdir={OVERLAY_METADATA}"
|
||||||
|
run(["mount", "-t", "overlay", "-o", overlay_opts, "none", OVERLAY_MERGED])
|
||||||
|
|
||||||
|
|
||||||
|
def inodes_in_tree(search_dir):
|
||||||
|
"""Given a search root, produce a dictionary mapping of inodes to relative
|
||||||
|
pathnames of regular files (no directories, symlinks, or special files)."""
|
||||||
|
inode_map = {}
|
||||||
|
for root, dirs, files in os.walk(search_dir, topdown=True):
|
||||||
|
for file_name in files:
|
||||||
|
full_path_name = os.path.join(root, file_name)
|
||||||
|
st = os.lstat(full_path_name)
|
||||||
|
if S_ISREG(st[ST_MODE]):
|
||||||
|
inode_map[st[ST_INO]] = full_path_name
|
||||||
|
return inode_map
|
||||||
|
|
||||||
|
|
||||||
|
def dup_ovfs_object(inode_map, source_obj, target_dir):
|
||||||
|
"""Given a relative pathname to copy, and a new target root, duplicate the
|
||||||
|
source object in the target root, using hardlinks for regular files."""
|
||||||
|
|
||||||
|
source_full_path = os.path.join(OVERLAY_MERGED, source_obj)
|
||||||
|
st = os.lstat(source_full_path)
|
||||||
|
target_full_path = os.path.join(target_dir, source_obj)
|
||||||
|
|
||||||
|
if S_ISREG(st[ST_MODE]):
|
||||||
|
# Hardlink all regular files; ownership and permissions are shared.
|
||||||
|
link(inode_map[st[ST_INO]], target_full_path)
|
||||||
|
else:
|
||||||
|
# Recreate all directories and symlinks; copy ownership and permissions.
|
||||||
|
if S_ISDIR(st[ST_MODE]):
|
||||||
|
os.mkdir(os.path.join(FINALIZED, source_obj), S_IMODE(st[ST_MODE]))
|
||||||
|
elif S_ISLNK(st[ST_MODE]):
|
||||||
|
os.symlink(os.readlink(source_full_path), target_full_path)
|
||||||
|
os.chmod(target_full_path, S_IMODE(st[ST_MODE]), follow_symlinks=False)
|
||||||
|
else:
|
||||||
|
# Ran into a FIFO, socket, etc. Should not happen in OP install dir.
|
||||||
|
# Ignore without copying for the time being; revisit later if needed.
|
||||||
|
cloudlog.error("can't copy this file type: %s" % source_full_path)
|
||||||
|
os.chown(target_full_path, st[ST_UID], st[ST_GID], follow_symlinks=False)
|
||||||
|
|
||||||
|
# Sync target mtimes to the cached lstat() value from each source object.
|
||||||
|
# Restores shared inode mtimes after linking, fixes symlinks and dirs.
|
||||||
|
os.utime(target_full_path, (st[ST_ATIME], st[ST_MTIME]), follow_symlinks=False)
|
||||||
|
|
||||||
|
|
||||||
|
def finalize_from_ovfs_hardlink():
|
||||||
|
"""Take the current OverlayFS merged view and finalize a copy outside of
|
||||||
|
OverlayFS, ready to be swapped-in at BASEDIR. Copy using hardlinks"""
|
||||||
|
|
||||||
|
cloudlog.info("creating finalized version of the overlay")
|
||||||
|
|
||||||
|
# The "copy" is done with hardlinks, but since the OverlayFS merge looks
|
||||||
|
# like a different filesystem, and hardlinks can't cross filesystems, we
|
||||||
|
# have to borrow a source pathname from the upper or lower layer.
|
||||||
|
inode_map = inodes_in_tree(BASEDIR)
|
||||||
|
inode_map.update(inodes_in_tree(OVERLAY_UPPER))
|
||||||
|
|
||||||
|
shutil.rmtree(FINALIZED)
|
||||||
|
os.umask(0o077)
|
||||||
|
os.mkdir(FINALIZED)
|
||||||
|
for root, dirs, files in os.walk(OVERLAY_MERGED, topdown=True):
|
||||||
|
for obj_name in dirs:
|
||||||
|
relative_path_name = os.path.relpath(os.path.join(root, obj_name), OVERLAY_MERGED)
|
||||||
|
dup_ovfs_object(inode_map, relative_path_name, FINALIZED)
|
||||||
|
for obj_name in files:
|
||||||
|
relative_path_name = os.path.relpath(os.path.join(root, obj_name), OVERLAY_MERGED)
|
||||||
|
dup_ovfs_object(inode_map, relative_path_name, FINALIZED)
|
||||||
|
cloudlog.info("done finalizing overlay")
|
||||||
|
|
||||||
|
|
||||||
|
def finalize_from_ovfs_copy():
|
||||||
|
"""Take the current OverlayFS merged view and finalize a copy outside of
|
||||||
|
OverlayFS, ready to be swapped-in at BASEDIR. Copy using shutil.copytree"""
|
||||||
|
|
||||||
|
cloudlog.info("creating finalized version of the overlay")
|
||||||
|
shutil.rmtree(FINALIZED)
|
||||||
|
shutil.copytree(OVERLAY_MERGED, FINALIZED, symlinks=True)
|
||||||
|
cloudlog.info("done finalizing overlay")
|
||||||
|
|
||||||
|
|
||||||
|
def attempt_update():
|
||||||
|
cloudlog.info("attempting git update inside staging overlay")
|
||||||
|
|
||||||
|
git_fetch_output = run(NICE_LOW_PRIORITY + ["git", "fetch"], OVERLAY_MERGED)
|
||||||
|
cloudlog.info("git fetch success: %s", git_fetch_output)
|
||||||
|
|
||||||
|
cur_hash = run(["git", "rev-parse", "HEAD"], OVERLAY_MERGED).rstrip()
|
||||||
|
upstream_hash = run(["git", "rev-parse", "@{u}"], OVERLAY_MERGED).rstrip()
|
||||||
|
new_version = cur_hash != upstream_hash
|
||||||
|
|
||||||
|
git_fetch_result = len(git_fetch_output) > 0 and (git_fetch_output != "Failed to add the host to the list of known hosts (/data/data/com.termux/files/home/.ssh/known_hosts).\n")
|
||||||
|
|
||||||
|
cloudlog.info("comparing %s to %s" % (cur_hash, upstream_hash))
|
||||||
|
if new_version or git_fetch_result:
|
||||||
|
cloudlog.info("Running update")
|
||||||
|
if new_version:
|
||||||
|
cloudlog.info("git reset in progress")
|
||||||
|
r = [
|
||||||
|
run(NICE_LOW_PRIORITY + ["git", "reset", "--hard", "@{u}"], OVERLAY_MERGED),
|
||||||
|
run(NICE_LOW_PRIORITY + ["git", "clean", "-xdf"], OVERLAY_MERGED),
|
||||||
|
run(NICE_LOW_PRIORITY + ["git", "submodule", "init"], OVERLAY_MERGED),
|
||||||
|
run(NICE_LOW_PRIORITY + ["git", "submodule", "update"], OVERLAY_MERGED),
|
||||||
|
]
|
||||||
|
cloudlog.info("git reset success: %s", '\n'.join(r))
|
||||||
|
|
||||||
|
# Un-set the validity flag to prevent the finalized tree from being
|
||||||
|
# activated later if the finalize step is interrupted
|
||||||
|
remove_consistent_flag()
|
||||||
|
|
||||||
|
finalize_from_ovfs_copy()
|
||||||
|
|
||||||
|
# Make sure the validity flag lands on disk LAST, only when the local git
|
||||||
|
# repo and OP install are in a consistent state.
|
||||||
|
set_consistent_flag()
|
||||||
|
|
||||||
|
cloudlog.info("update successful!")
|
||||||
|
else:
|
||||||
|
cloudlog.info("nothing new from git at this time")
|
||||||
|
|
||||||
|
set_update_available_params(new_version=new_version)
|
||||||
|
|
||||||
|
|
||||||
|
def main(gctx=None):
|
||||||
|
overlay_init_done = False
|
||||||
|
wait_helper = WaitTimeHelper()
|
||||||
|
params = Params()
|
||||||
|
|
||||||
|
if not os.geteuid() == 0:
|
||||||
|
raise RuntimeError("updated must be launched as root!")
|
||||||
|
|
||||||
|
# Set low io priority
|
||||||
|
p = psutil.Process()
|
||||||
|
if psutil.LINUX:
|
||||||
|
p.ionice(psutil.IOPRIO_CLASS_BE, value=7)
|
||||||
|
|
||||||
|
ov_lock_fd = open('/tmp/safe_staging_overlay.lock', 'w')
|
||||||
|
try:
|
||||||
|
fcntl.flock(ov_lock_fd, fcntl.LOCK_EX | fcntl.LOCK_NB)
|
||||||
|
except IOError:
|
||||||
|
raise RuntimeError("couldn't get overlay lock; is another updated running?")
|
||||||
|
|
||||||
while True:
|
while True:
|
||||||
time_wrong = datetime.datetime.now().year < 2019
|
time_wrong = datetime.datetime.now().year < 2019
|
||||||
ping_failed = subprocess.call(["ping", "-W", "4", "-c", "1", "8.8.8.8"])
|
ping_failed = subprocess.call(["ping", "-W", "4", "-c", "1", "8.8.8.8"])
|
||||||
if ping_failed or time_wrong:
|
|
||||||
time.sleep(60)
|
|
||||||
continue
|
|
||||||
|
|
||||||
# download application update
|
# Wait until we have a valid datetime to initialize the overlay
|
||||||
try:
|
if not (ping_failed or time_wrong):
|
||||||
r = subprocess.check_output(NICE_LOW_PRIORITY + ["git", "fetch"], stderr=subprocess.STDOUT).decode('utf8')
|
try:
|
||||||
except subprocess.CalledProcessError as e:
|
# If the git directory has modifcations after we created the overlay
|
||||||
cloudlog.event("git fetch failed",
|
# we need to recreate the overlay
|
||||||
cmd=e.cmd,
|
if overlay_init_done:
|
||||||
output=e.output,
|
overlay_init_fn = os.path.join(BASEDIR, ".overlay_init")
|
||||||
returncode=e.returncode)
|
git_dir_path = os.path.join(BASEDIR, ".git")
|
||||||
time.sleep(60)
|
new_files = run(["find", git_dir_path, "-newer", overlay_init_fn])
|
||||||
continue
|
|
||||||
cloudlog.info("git fetch success: %s", r)
|
|
||||||
|
|
||||||
# Write update available param
|
if len(new_files.splitlines()):
|
||||||
try:
|
cloudlog.info(".git directory changed, recreating overlay")
|
||||||
cur_hash = subprocess.check_output(["git", "rev-parse", "HEAD"]).rstrip()
|
overlay_init_done = False
|
||||||
upstream_hash = subprocess.check_output(["git", "rev-parse", "@{u}"]).rstrip()
|
|
||||||
params.put("UpdateAvailable", str(int(cur_hash != upstream_hash)))
|
|
||||||
except:
|
|
||||||
params.put("UpdateAvailable", "0")
|
|
||||||
|
|
||||||
# Write latest release notes to param
|
if not overlay_init_done:
|
||||||
try:
|
init_ovfs()
|
||||||
r = subprocess.check_output(["git", "--no-pager", "show", "@{u}:RELEASES.md"])
|
overlay_init_done = True
|
||||||
r = r[:r.find(b'\n\n')] # Slice latest release notes
|
|
||||||
params.put("ReleaseNotes", r + b"\n")
|
|
||||||
except:
|
|
||||||
params.put("ReleaseNotes", "")
|
|
||||||
|
|
||||||
t = datetime.datetime.now().isoformat()
|
if params.get("IsOffroad") == b"1":
|
||||||
params.put("LastUpdateTime", t.encode('utf8'))
|
attempt_update()
|
||||||
|
else:
|
||||||
|
cloudlog.info("not running updater, openpilot running")
|
||||||
|
|
||||||
time.sleep(60*60)
|
except subprocess.CalledProcessError as e:
|
||||||
|
cloudlog.event(
|
||||||
|
"update process failed",
|
||||||
|
cmd=e.cmd,
|
||||||
|
output=e.output,
|
||||||
|
returncode=e.returncode
|
||||||
|
)
|
||||||
|
overlay_init_done = False
|
||||||
|
except Exception:
|
||||||
|
cloudlog.exception("uncaught updated exception, shouldn't happen")
|
||||||
|
overlay_init_done = False
|
||||||
|
|
||||||
|
wait_between_updates(wait_helper.ready_event)
|
||||||
|
if wait_helper.shutdown:
|
||||||
|
break
|
||||||
|
|
||||||
|
# We've been signaled to shut down
|
||||||
|
dismount_ovfs()
|
||||||
|
|
||||||
if __name__ == "__main__":
|
if __name__ == "__main__":
|
||||||
main()
|
main()
|
||||||
|
|
Some files were not shown because too many files have changed in this diff Show More
Loading…
Reference in New Issue