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

openpilot v0.7.1 release

pull/950/head
Vehicle Researcher 2020-01-15 14:05:04 -08:00
parent 25d18afad5
commit 8da8b6135c
101 changed files with 4314 additions and 3352 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
.gitignore vendored
View File

@ -3,11 +3,14 @@ venv/
.tags
.ipynb_checkpoints
.idea
.overlay_init
.overlay_consistent
.sconsign.dblite
.vscode
model2.png
a.out
*.dylib
*.DSYM
*.d
*.pyc
@ -27,6 +30,7 @@ a.out
config.json
clcache
persist
board/obj/
selfdrive/boardd/boardd
selfdrive/logcatd/logcatd
@ -51,4 +55,5 @@ panda_jungle
.coverage*
htmlcov
pandaextra

2
Dockerfile.openpilot
View File

@ -17,12 +17,14 @@ RUN apt-get update && apt-get install -y \
libffi-dev \
libglew-dev \
libgles2-mesa-dev \
libglfw3-dev \
libglib2.0-0 \
liblzma-dev \
libmysqlclient-dev \
libomp-dev \
libopencv-dev \
libssl-dev \
libsqlite3-dev \
libtool \
libusb-1.0-0-dev \
libzmq5-dev \

27
Pipfile
View File

@ -8,71 +8,54 @@ opencv-python= "==3.4.2.17"
PyQt5 = "*"
ipython = "*"
networkx = "==2.3"
azure-common = "==1.1.23"
azure-core = "==1.1.1"
azure-common = "==1.1.24"
azure-nspkg = "==3.0.2"
azure-storage-blob = "==2.1.0"
azure-storage-common = "==2.1.0"
azure-storage-nspkg = "==3.1.0"
bincopy = "*"
bleach = "*"
boto = "*"
"boto3" = "*"
celery = "*"
control = "*"
datadog = "*"
decorator = "*"
dlib = "*"
dominate = "*"
elasticsearch = "*"
fasteners = "*"
future = "*"
futures = "*"
gevent = "*"
pycocotools = {git = "https://github.com/cocodataset/cocoapi.git",subdirectory = "PythonAPI"}
gunicorn = "*"
"h5py" = "*"
hexdump = "*"
"html5lib" = "*"
imageio = "*"
intervaltree = "*"
ipykernel = "*"
joblib = "*"
json-logging-py = "*"
jupyter = "*"
libarchive = "*"
lru-dict = "*"
lxml = "*"
"mpld3" = "*"
msgpack-python = "*"
nbstripout = "*"
nose-parameterized = "*"
numpy = "*"
osmium = "*"
pbr = "*"
percache = "*"
pprofile = "*"
psutil = "*"
pycurl = "*"
git-pylint-commit-hook = "*"
pymongo = "*"
"pynmea2" = "*"
pypolyline = "*"
pysendfile = "*"
python-logstash = "*"
pyvcd = "*"
redis = "*"
redlock = "*"
"s2sphere" = "*"
scikit-image = "*"
"subprocess32" = "*"
supervisor = "*"
tenacity = "*"
tensorflow-gpu = ""
utm = "*"
"v4l2" = "*"
PyJWT = "==1.4.1"
PyMySQL = "==0.9.2"
Theano = "*"
Werkzeug = "*"
"backports.lzma" = "*"
Flask-Cors = "*"
@ -84,7 +67,6 @@ PyNaCl = "*"
reverse_geocoder = "*"
Shapely = "*"
SQLAlchemy = "*"
uWSGI = "*"
scipy = "*"
fastcluster = "*"
backports-abc = "*"
@ -92,15 +74,13 @@ pygame = "*"
simplejson = "*"
python-logstash-async = "*"
seaborn = "*"
tensorflow-estimator = "*"
pyproj = "*"
mock = "*"
blinker = "*"
gast = "==0.2.2"
matplotlib = "*"
dictdiffer = "*"
aenum = "*"
coverage = "*"
azure-cli-core = "*"
[packages]
overpy = {git = "https://github.com/commaai/python-overpy.git",ref = "f86529af402d4642e1faeb146671c40284007323"}
@ -144,6 +124,5 @@ pillow = "*"
scons = "*"
cysignals = "*"
[requires]
python_version = "3.7.3"

1382
Pipfile.lock generated
View File

File diff suppressed because it is too large Load Diff

4
README.md
View File

@ -57,7 +57,7 @@ openpilot should preserve all other vehicle's stock features, including, but are
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
------
@ -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.
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:
* Poor visibility (heavy rain, snow, fog, etc.) or weather conditions that may interfere with sensor operation.

9
RELEASES.md
View File

@ -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)
========================
* Move to SCons build system!

9
SConstruct
View File

@ -46,6 +46,7 @@ else:
"#phonelibs/capnp-cpp/include",
"#phonelibs/capnp-c/include",
"#phonelibs/zmq/x64/include",
"#external/tensorflow/include",
]
libpath = [
"#phonelibs/capnp-cpp/x64/lib",
@ -55,6 +56,7 @@ else:
"#phonelibs/zmq/x64/lib",
"#phonelibs/libyuv/x64/lib",
"#external/zmq/lib",
"#external/tensorflow/lib",
"#cereal",
"#selfdrive/common",
"/usr/lib",
@ -62,6 +64,7 @@ else:
]
rpath = ["phonelibs/capnp-cpp/x64/lib",
"external/tensorflow/lib",
"cereal",
"selfdrive/common"]
@ -201,11 +204,13 @@ SConscript(['selfdrive/controls/lib/longitudinal_mpc/SConscript'])
SConscript(['selfdrive/boardd/SConscript'])
SConscript(['selfdrive/proclogd/SConscript'])
SConscript(['selfdrive/ui/SConscript'])
SConscript(['selfdrive/loggerd/SConscript'])
if arch == "aarch64":
SConscript(['selfdrive/logcatd/SConscript'])
SConscript(['selfdrive/ui/SConscript'])
SConscript(['selfdrive/sensord/SConscript'])
SConscript(['selfdrive/loggerd/SConscript'])
SConscript(['selfdrive/clocksd/SConscript'])
SConscript(['selfdrive/locationd/SConscript'])

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

Binary file not shown.

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

Binary file not shown.

12
common/android.py
View File

@ -1,10 +1,15 @@
import os
import binascii
import itertools
import re
import struct
import subprocess
ANDROID = os.path.isfile('/EON')
def getprop(key):
if not ANDROID:
return ""
return subprocess.check_output(["getprop", key], encoding='utf8').strip()
def get_imei():
@ -14,7 +19,10 @@ def get_imei():
return ret
def get_serial():
return getprop("ro.serialno")
ret = getprop("ro.serialno")
if ret == "":
ret = "cccccccc"
return ret
def get_subscriber_info():
ret = parse_service_call_string(["iphonesubinfo", "7"])
@ -60,6 +68,8 @@ def parse_service_call_string(call):
return None
def parse_service_call_bytes(call):
if not ANDROID:
return None
ret = subprocess.check_output(["service", "call", *call], encoding='utf8').strip()
if 'Parcel' not in ret:
return None

22
common/logging_extra.py
View File

@ -78,28 +78,6 @@ class SwagLogger(logging.Logger):
self.log_local = local()
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):
try:
return self.log_local.ctx

4
common/params.py
View File

@ -69,9 +69,10 @@ keys = {
"IsLdwEnabled": [TxType.PERSISTENT],
"IsGeofenceEnabled": [TxType.PERSISTENT],
"IsMetric": [TxType.PERSISTENT],
"IsOffroad": [TxType.CLEAR_ON_MANAGER_START],
"IsRHD": [TxType.PERSISTENT],
"IsTakingSnapshot": [TxType.CLEAR_ON_MANAGER_START],
"IsUpdateAvailable": [TxType.PERSISTENT],
"IsUpdateAvailable": [TxType.CLEAR_ON_MANAGER_START],
"IsUploadRawEnabled": [TxType.PERSISTENT],
"LastUpdateTime": [TxType.PERSISTENT],
"LimitSetSpeed": [TxType.PERSISTENT],
@ -80,6 +81,7 @@ keys = {
"LongitudinalControl": [TxType.PERSISTENT],
"OpenpilotEnabledToggle": [TxType.PERSISTENT],
"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],
"Passive": [TxType.PERSISTENT],
"RecordFront": [TxType.PERSISTENT],

24
common/spinner.py
View File

@ -5,21 +5,31 @@ from common.basedir import BASEDIR
class Spinner():
def __init__(self):
self.spinner_proc = subprocess.Popen(["./spinner"],
stdin=subprocess.PIPE,
cwd=os.path.join(BASEDIR, "selfdrive", "ui", "spinner"),
close_fds=True)
try:
self.spinner_proc = subprocess.Popen(["./spinner"],
stdin=subprocess.PIPE,
cwd=os.path.join(BASEDIR, "selfdrive", "ui", "spinner"),
close_fds=True)
except OSError:
self.spinner_proc = None
def __enter__(self):
return self
def update(self, spinner_text):
self.spinner_proc.stdin.write(spinner_text.encode('utf8') + b"\n")
self.spinner_proc.stdin.flush()
if self.spinner_proc is not None:
self.spinner_proc.stdin.write(spinner_text.encode('utf8') + b"\n")
try:
self.spinner_proc.stdin.flush()
except BrokenPipeError:
pass
def close(self):
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 = None

8
common/transformations/camera.py
View File

@ -44,6 +44,7 @@ def get_calib_from_vp(vp):
roll_calib = 0
return roll_calib, pitch_calib, yaw_calib
# aka 'extrinsic_matrix'
# road : x->forward, y -> left, z->up
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])
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):
# 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]),

10
installer/updater/update.json
View File

@ -1,7 +1,7 @@
{
"ota_url": "https://commadist.azureedge.net/neosupdate/ota-signed-07df505453684371b6c22583ffbb74ee414fcd389a46ff369ffd1b6bac75414e.zip",
"ota_hash": "07df505453684371b6c22583ffbb74ee414fcd389a46ff369ffd1b6bac75414e",
"recovery_url": "https://commadist.azureedge.net/neosupdate/recovery-3a6f973295ded6e4ff5cfff3b12e19c80d3bf45e2e8dd8699da3fc25b23ed7c6.img",
"recovery_len": 15848748,
"recovery_hash": "3a6f973295ded6e4ff5cfff3b12e19c80d3bf45e2e8dd8699da3fc25b23ed7c6"
"ota_url": "https://commadist.azureedge.net/neosupdate/ota-signed-efdf7de63b1aef63d68301e6175930991bf9a5927d16ec6fcc69287e2ee7ca4a.zip",
"ota_hash": "efdf7de63b1aef63d68301e6175930991bf9a5927d16ec6fcc69287e2ee7ca4a",
"recovery_url": "https://commadist.azureedge.net/neosupdate/recovery-97c27e6ed04ed6bb0608b845a2d4100912093f9380c3f2ba6b56bccd608e5f6e.img",
"recovery_len": 15861036,
"recovery_hash": "97c27e6ed04ed6bb0608b845a2d4100912093f9380c3f2ba6b56bccd608e5f6e"
}

47
launch_chffrplus.sh
View File

@ -6,20 +6,53 @@ export NUMEXPR_NUM_THREADS=1
export OPENBLAS_NUM_THREADS=1
export VECLIB_MAXIMUM_THREADS=1
if [ -z "$BASEDIR" ]; then
BASEDIR="/data/openpilot"
fi
if [ -z "$PASSIVE" ]; then
export PASSIVE="1"
fi
STAGING_ROOT="/data/safe_staging"
function launch {
# Wifi scan
wpa_cli IFNAME=wlan0 SCAN
# apply update
if [ "$(git rev-parse HEAD)" != "$(git rev-parse @{u})" ]; then
git reset --hard @{u} &&
git clean -xdf &&
# Check to see if there's a valid overlay-based update available. Conditions
# are as follows:
#
# 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
# no cpu rationing for now
@ -32,17 +65,17 @@ function launch {
DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" >/dev/null && pwd )"
# Remove old NEOS update file
# TODO: move this code to the updater
if [ -d /data/neoupdate ]; then
rm -rf /data/neoupdate
fi
# Check for NEOS update
if [ $(< /VERSION) != "13" ]; then
if [ $(< /VERSION) != "14" ]; then
if [ -f "$DIR/scripts/continue.sh" ]; then
cp "$DIR/scripts/continue.sh" "/data/data/com.termux/files/continue.sh"
fi
git clean -xdf
"$DIR/installer/updater/updater" "file://$DIR/installer/updater/update.json"
fi

BIN
models/driving_model.dlc
View File

Binary file not shown.

BIN
models/monitoring_model_q.dlc
View File

Binary file not shown.

BIN
models/posenet.dlc
View File

Binary file not shown.

BIN
models/supercombo.dlc 100644
View File

Binary file not shown.

BIN
panda/board/obj/panda.bin.signed 100644
View File

Binary file not shown.

7
scripts/stop_updater.sh 100755
View File

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

4
scripts/update_now.sh 100755
View File

@ -0,0 +1,4 @@
#!/usr/bin/env sh
# Send SIGHUP to updater
pkill -1 -f selfdrive.updated

19
selfdrive/athena/test.py
View File

@ -32,10 +32,8 @@ class TestAthenadMethods(unittest.TestCase):
assert dispatcher["echo"]("bob") == "bob"
def test_getMessage(self):
try:
with self.assertRaises(TimeoutError) as _:
dispatcher["getMessage"]("controlsState")
except TimeoutError:
pass
def send_thermal():
messaging.context = messaging.Context()
@ -50,6 +48,7 @@ class TestAthenadMethods(unittest.TestCase):
p = Process(target=send_thermal)
p.start()
time.sleep(0.1)
try:
thermal = dispatcher["getMessage"]("thermal")
assert thermal['thermal']
@ -60,7 +59,7 @@ class TestAthenadMethods(unittest.TestCase):
print(dispatcher["listDataDirectory"]())
@with_http_server
def test_do_upload(self):
def test_do_upload(self, host):
fn = os.path.join(athenad.ROOT, 'qlog.bz2')
Path(fn).touch()
@ -71,14 +70,14 @@ class TestAthenadMethods(unittest.TestCase):
except requests.exceptions.ConnectionError:
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)
self.assertEqual(resp.status_code, 201)
finally:
os.unlink(fn)
@with_http_server
def test_uploadFileToUrl(self):
def test_uploadFileToUrl(self, host):
not_exists_resp = dispatcher["uploadFileToUrl"]("does_not_exist.bz2", "http://localhost:1238", {})
self.assertEqual(not_exists_resp, 404)
@ -86,9 +85,9 @@ class TestAthenadMethods(unittest.TestCase):
Path(fn).touch()
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.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.assertEqual(athenad.upload_queue.qsize(), 1)
finally:
@ -96,10 +95,10 @@ class TestAthenadMethods(unittest.TestCase):
os.unlink(fn)
@with_http_server
def test_upload_handler(self):
def test_upload_handler(self, host):
fn = os.path.join(athenad.ROOT, 'qlog.bz2')
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()
thread = threading.Thread(target=athenad.upload_handler, args=(end_event,))

29
selfdrive/athena/test_helpers.py
View File

@ -1,4 +1,7 @@
import http.server
import multiprocessing
import queue
import random
import requests
import socket
import time
@ -70,27 +73,41 @@ class HTTPRequestHandler(http.server.SimpleHTTPRequestHandler):
self.send_response(201, "Created")
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):
@wraps(func)
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={
'HandlerClass': HTTPRequestHandler,
'port': 44444,
'bind': '127.0.0.1'})
'bind': host})
p.start()
now = time.time()
port = None
while 1:
if time.time() - now > 5:
raise Exception('HTTP Server did not start')
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
except requests.exceptions.ConnectionError:
except (requests.exceptions.ConnectionError, queue.Empty):
time.sleep(0.1)
try:
return func(*args, **kwargs)
return func(*args, f'http://{host}:{port}', **kwargs)
finally:
p.terminate()

87
selfdrive/boardd/boardd.cc
View File

@ -21,6 +21,7 @@
#include "cereal/gen/cpp/log.capnp.h"
#include "cereal/gen/cpp/car.capnp.h"
#include "common/util.h"
#include "common/messaging.h"
#include "common/params.h"
#include "common/swaglog.h"
@ -35,8 +36,10 @@
#define MAX_IR_POWER 0.5f
#define MIN_IR_POWER 0.0f
#define CUTOFF_GAIN 0.015625f // iso400
#define SATURATE_GAIN 0.0625f // iso1600
#define CUTOFF_GAIN 0.015625f // iso400
#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 {
@ -62,14 +65,19 @@ bool loopback_can = false;
cereal::HealthData::HwType hw_type = cereal::HealthData::HwType::UNKNOWN;
bool is_pigeon = false;
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 uint32_t VBATT_PAUSE_CHARGING = 10500;
const float VBATT_START_CHARGING = 11.5;
const float VBATT_PAUSE_CHARGING = 11.0;
float voltage_f = 12.5; // filtered voltage
uint32_t no_ignition_cnt = 0;
bool connected_once = false;
bool ignition_last = false;
pthread_t safety_setter_thread_handle = -1;
pthread_t pigeon_thread_handle = -1;
bool safety_setter_thread_initialized = false;
pthread_t safety_setter_thread_handle;
bool pigeon_thread_initialized = false;
pthread_t pigeon_thread_handle;
bool pigeon_needs_init;
void pigeon_init();
@ -130,10 +138,7 @@ void *safety_setter_thread(void *s) {
pthread_mutex_lock(&usb_lock);
// set in the mutex to avoid race
safety_setter_thread_handle = -1;
// 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);
safety_setter_thread_initialized = false;
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
bool usb_connect() {
int err;
int err, err2;
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];
const char *fw_ver;
const char *serial;
int fw_ver_sz = 0;
int serial_sz = 0;
ignition_last = false;
@ -169,12 +173,17 @@ bool usb_connect() {
}
// get panda fw
err = libusb_control_transfer(dev_handle, 0xc0, 0xd6, 0, 0, fw_ver_buf, 64, TIMEOUT);
if (err > 0) {
fw_ver = (const char *)fw_ver_buf;
fw_ver_sz = err;
write_db_value(NULL, "PandaFirmware", fw_ver, fw_ver_sz);
printf("panda fw: %.*s\n", fw_ver_sz, fw_ver);
err = libusb_control_transfer(dev_handle, 0xc0, 0xd3, 0, 0, fw_sig_buf, 64, TIMEOUT);
err2 = libusb_control_transfer(dev_handle, 0xc0, 0xd4, 0, 0, fw_sig_buf + 64, 64, TIMEOUT);
if ((err == 64) && (err2 == 64)) {
printf("FW signature read\n");
write_db_value(NULL, "PandaFirmware", (const char *)fw_sig_buf, 128);
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; }
@ -206,9 +215,10 @@ bool usb_connect() {
if (is_pigeon) {
LOGW("panda with gps detected");
pigeon_needs_init = true;
if (pigeon_thread_handle == -1) {
if (!pigeon_thread_initialized) {
err = pthread_create(&pigeon_thread_handle, NULL, pigeon_thread, NULL);
assert(err == 0);
pigeon_thread_initialized = true;
}
}
@ -289,6 +299,8 @@ void can_recv(PubSocket *publisher) {
// return if length is 0
if (recv <= 0) {
return;
} else if (recv == RECV_SIZE) {
LOGW("Receive buffer full");
}
// create message
@ -296,7 +308,6 @@ void can_recv(PubSocket *publisher) {
cereal::Event::Builder event = msg.initRoot<cereal::Event>();
event.setLogMonoTime(start_time);
size_t num_msg = recv / 0x10;
auto canData = event.initCan(num_msg);
// populate message
@ -330,6 +341,7 @@ void can_health(PubSocket *publisher) {
uint32_t uptime;
uint32_t voltage;
uint32_t current;
uint32_t can_rx_errs;
uint32_t can_send_errs;
uint32_t can_fwd_errs;
uint32_t gmlan_send_errs;
@ -355,6 +367,12 @@ void can_health(PubSocket *publisher) {
} while(cnt != sizeof(health));
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
if (health.safety_model == (uint8_t)(cereal::CarParams::SafetyModel::SILENT)) {
pthread_mutex_lock(&usb_lock);
@ -373,13 +391,15 @@ void can_health(PubSocket *publisher) {
#ifndef __x86_64__
bool cdp_mode = health.usb_power_mode == (uint8_t)(cereal::HealthData::UsbPowerMode::CDP);
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");
pthread_mutex_lock(&usb_lock);
libusb_control_transfer(dev_handle, 0xc0, 0xe6, (uint16_t)(cereal::HealthData::UsbPowerMode::CLIENT), 0, NULL, 0, TIMEOUT);
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");
pthread_mutex_lock(&usb_lock);
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
if (ignition && !ignition_last) {
int result = delete_db_value(NULL, "CarVin");
assert((result == 0) || (result == ERR_NO_VALUE));
result = delete_db_value(NULL, "CarParams");
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);
assert(err == 0);
safety_setter_thread_initialized = true;
}
}
@ -459,15 +479,12 @@ void can_health(PubSocket *publisher) {
healthData.setUptime(health.uptime);
healthData.setVoltage(health.voltage);
healthData.setCurrent(health.current);
if (spoofing_started) {
healthData.setIgnitionLine(true);
} else {
healthData.setIgnitionLine(health.ignition_line);
}
healthData.setIgnitionLine(health.ignition_line);
healthData.setIgnitionCan(health.ignition_can);
healthData.setControlsAllowed(health.controls_allowed);
healthData.setGasInterceptorDetected(health.gas_interceptor_detected);
healthData.setHasGps(is_pigeon);
healthData.setCanRxErrs(health.can_rx_errs);
healthData.setCanSendErrs(health.can_send_errs);
healthData.setCanFwdErrs(health.can_fwd_errs);
healthData.setGmlanSendErrs(health.gmlan_send_errs);
@ -842,14 +859,6 @@ void *pigeon_thread(void *crap) {
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() {

8
selfdrive/camerad/main.cc
View File

@ -680,14 +680,6 @@ void* visionserver_client_thread(void* arg) {
} else {
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);
streams[stream_type].subscribed = true;
} else if (p.type == VIPC_STREAM_RELEASE) {

16
selfdrive/car/car_helpers.py
View File

@ -3,6 +3,7 @@ from common.params import Params
from common.basedir import BASEDIR
from selfdrive.car.fingerprints import eliminate_incompatible_cars, all_known_cars
from selfdrive.car.vin import get_vin, VIN_UNKNOWN
from selfdrive.car.fw_versions import get_fw_versions
from selfdrive.swaglog import cloudlog
import cereal.messaging as messaging
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
# **** for use live only ****
def fingerprint(logcan, sendcan, has_relay):
if has_relay:
# 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:
vin = VIN_UNKNOWN
_, car_fw = set(), []
cloudlog.warning("VIN %s", vin)
Params().put("CarVin", vin)
@ -108,18 +111,19 @@ def fingerprint(logcan, sendcan, has_relay):
frame += 1
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):
candidate, fingerprints, vin = fingerprint(logcan, sendcan, has_relay)
candidate, fingerprints, vin, car_fw = fingerprint(logcan, sendcan, has_relay)
if candidate is None:
cloudlog.warning("car doesn't match any fingerprints: %r", fingerprints)
candidate = "mock"
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

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

@ -35,13 +35,12 @@ class CarInterface(CarInterfaceBase):
return float(accel) / 3.0
@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.carName = "chrysler"
ret.carFingerprint = candidate
ret.carVin = vin
ret.isPandaBlack = has_relay
ret.safetyModel = car.CarParams.SafetyModel.chrysler

44
selfdrive/car/fingerprints.py
View File

@ -1,27 +1,43 @@
import os
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():
# read all the folders in selfdrive/car and return a dict where:
# - keys are all the car models for which we have a fingerprint
# - values are lists dicts of messages that constitute the unique
# CAN fingerprint of each car model and all its variants
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
return get_attr_from_cars('FINGERPRINTS')
FW_VERSIONS = get_fw_versions_list()
_FINGERPRINTS = get_fingerprint_list()
_DEBUG_ADDRESS = {1880: 8} # reserved for debug purposes

3
selfdrive/car/ford/interface.py
View File

@ -34,13 +34,12 @@ class CarInterface(CarInterfaceBase):
return float(accel) / 3.0
@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.carName = "ford"
ret.carFingerprint = candidate
ret.carVin = vin
ret.isPandaBlack = has_relay
ret.safetyModel = car.CarParams.SafetyModel.ford

192
selfdrive/car/fw_versions.py 100755
View File

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

3
selfdrive/car/gm/interface.py
View File

@ -43,12 +43,11 @@ class CarInterface(CarInterfaceBase):
return float(accel) / 4.0
@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.carName = "gm"
ret.carFingerprint = candidate
ret.carVin = vin
ret.isPandaBlack = has_relay
ret.enableCruise = False

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

@ -1,12 +1,14 @@
from collections import namedtuple
from cereal import car
from common.realtime import DT_CTRL
from selfdrive.controls.lib.drive_helpers import rate_limit
from common.numpy_fast import clip
from selfdrive.car import create_gas_command
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
VisualAlert = car.CarControl.HUDControl.VisualAlert
def actuator_hystereses(brake, braking, brake_steady, v_ego, car_fingerprint):
# hyst params
@ -56,25 +58,25 @@ def process_hud_alert(hud_alert):
fcw_display = 0
steer_required = 0
acc_alert = 0
if hud_alert == AH.NONE: # no alert
pass
elif hud_alert == AH.FCW: # FCW
fcw_display = hud_alert[1]
elif hud_alert == AH.STEER: # STEER
steer_required = hud_alert[1]
else: # any other ACC alert
acc_alert = hud_alert[1]
# priority is: FCW, steer required, all others
if hud_alert == VisualAlert.fcw:
fcw_display = VISUAL_HUD[hud_alert.raw]
elif hud_alert == VisualAlert.steerRequired:
steer_required = VISUAL_HUD[hud_alert.raw]
else:
acc_alert = VISUAL_HUD[hud_alert.raw]
return fcw_display, steer_required, acc_alert
HUDData = namedtuple("HUDData",
["pcm_accel", "v_cruise", "mini_car", "car", "X4",
"lanes", "fcw", "acc_alert", "steer_required"])
["pcm_accel", "v_cruise", "car",
"lanes", "fcw", "acc_alert", "steer_required"])
class CarController():
def __init__(self, dbc_name):
def __init__(self, dbc_name, CP):
self.braking = False
self.brake_steady = 0.
self.brake_last = 0.
@ -82,6 +84,11 @@ class CarController():
self.last_pump_ts = 0.
self.packer = CANPacker(dbc_name)
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, \
pcm_speed, pcm_override, pcm_cancel_cmd, pcm_accel, \
@ -96,7 +103,7 @@ class CarController():
pcm_cancel_cmd = True
# *** 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
if hud_show_lanes:
@ -114,8 +121,8 @@ class CarController():
fcw_display, steer_required, acc_alert = process_hud_alert(hud_alert)
hud = HUDData(int(pcm_accel), int(round(hud_v_cruise)), 1, hud_car,
0xc1, hud_lanes, fcw_display, acc_alert, steer_required)
hud = HUDData(int(pcm_accel), int(round(hud_v_cruise)), hud_car,
hud_lanes, fcw_display, acc_alert, steer_required)
# **** process the car messages ****
@ -124,9 +131,11 @@ class CarController():
if CS.CP.carFingerprint in (CAR.ACURA_ILX):
STEER_MAX = 0xF00
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):
STEER_MAX = 0x7FFF
elif CS.CP.carFingerprint in (CAR.CIVIC) and self.eps_modified:
STEER_MAX = 0x1400
else:
STEER_MAX = 0x1000
@ -135,6 +144,12 @@ class CarController():
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))
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
# Send CAN commands.

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

@ -41,8 +41,8 @@ def get_can_signals(CP):
("WHEEL_SPEED_RR", "WHEEL_SPEEDS", 0),
("STEER_ANGLE", "STEERING_SENSORS", 0),
("STEER_ANGLE_RATE", "STEERING_SENSORS", 0),
("MOTOR_TORQUE", "STEER_MOTOR_TORQUE", 0),
("STEER_TORQUE_SENSOR", "STEER_STATUS", 0),
("STEER_TORQUE_MOTOR", "STEER_STATUS", 0),
("LEFT_BLINKER", "SCM_FEEDBACK", 0),
("RIGHT_BLINKER", "SCM_FEEDBACK", 0),
("GEAR", "GEARBOX", 0),
@ -325,7 +325,7 @@ class CarState():
self.car_gas = cp.vl["GAS_PEDAL_2"]['CAR_GAS']
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.brake_switch = cp.vl["POWERTRAIN_DATA"]['BRAKE_SWITCH']

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

@ -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_GAS': hud.pcm_accel,
'CRUISE_SPEED': hud.v_cruise,
'ENABLE_MINI_CAR': hud.mini_car,
'ENABLE_MINI_CAR': 1,
'HUD_LEAD': hud.car,
'HUD_DISTANCE': 3, # max distance setting on display
'IMPERIAL_UNIT': int(not is_metric),

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

@ -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.vehicle_model import VehicleModel
from selfdrive.car.honda.carstate import CarState, get_can_parser, get_cam_can_parser
from selfdrive.car.honda.values import CruiseButtons, CAR, HONDA_BOSCH, 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.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
A_ACC_MAX = max(_A_CRUISE_MAX_V)
A_ACC_MAX = max(_A_CRUISE_MAX_V_FOLLOWING)
ButtonType = car.CarState.ButtonEvent.Type
GearShifter = car.CarState.GearShifter
@ -91,7 +91,7 @@ class CarInterface(CarInterfaceBase):
self.CC = 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:
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)
@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.carName = "honda"
ret.carFingerprint = candidate
ret.carVin = vin
ret.isPandaBlack = has_relay
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
ret.enableCamera = is_ecu_disconnected(fingerprint[rdr_bus], FINGERPRINTS, ECU_FINGERPRINT, candidate, ECU.CAM) or has_relay
ret.radarOffCan = True
ret.openpilotLongitudinalControl = False
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.enableGasInterceptor = 0x201 in fingerprint[0]
ret.openpilotLongitudinalControl = ret.enableCamera
@ -165,6 +164,11 @@ class CarInterface(CarInterfaceBase):
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]]
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]:
stop_and_go = True
ret.mass = CivicParams.MASS
@ -173,7 +177,8 @@ class CarInterface(CarInterfaceBase):
ret.steerRatio = 15.38 # 10.93 is end-to-end spec
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.kpV = [3.6, 2.4, 1.5]
ret.longitudinalTuning.kiBP = [0., 35.]
@ -575,8 +580,6 @@ class CarInterface(CarInterfaceBase):
else:
hud_v_cruise = 255
hud_alert = VISUAL_HUD[c.hudControl.visualAlert.raw]
pcm_accel = int(clip(c.cruiseControl.accelOverride, 0, 1) * 0xc6)
can_sends = self.CC.update(c.enabled, self.CS, self.frame,
@ -588,7 +591,7 @@ class CarInterface(CarInterfaceBase):
hud_v_cruise,
c.hudControl.lanesVisible,
hud_show_car=c.hudControl.leadVisible,
hud_alert=hud_alert)
hud_alert=c.hudControl.visualAlert)
self.frame += 1
return can_sends

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

@ -1,6 +1,7 @@
from cereal import car
from selfdrive.car import dbc_dict
Ecu = car.CarParams.Ecu
VisualAlert = car.CarControl.HUDControl.VisualAlert
# Car button codes
@ -10,28 +11,18 @@ class CruiseButtons:
CANCEL = 2
MAIN = 1
class AH:
#[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]
# See dbc files for info on values"
VISUAL_HUD = {
VisualAlert.none: AH.NONE,
VisualAlert.fcw: AH.FCW,
VisualAlert.steerRequired: AH.STEER,
VisualAlert.brakePressed: AH.BRAKE_PRESSED,
VisualAlert.wrongGear: AH.GEAR_NOT_D,
VisualAlert.seatbeltUnbuckled: AH.SEATBELT,
VisualAlert.speedTooHigh: AH.SPEED_TOO_HIGH}
VisualAlert.none: 0,
VisualAlert.fcw: 1,
VisualAlert.steerRequired: 1,
VisualAlert.brakePressed: 10,
VisualAlert.wrongGear: 6,
VisualAlert.seatbeltUnbuckled: 5,
VisualAlert.speedTooHigh: 8}
class ECU:
CAM = 0
CAM = Ecu.fwdCamera
class CAR:
ACCORD = "HONDA ACCORD 2018 SPORT 2T"
@ -81,9 +72,8 @@ FINGERPRINTS = {
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,
}],
# msg 1115 has seen with len 2 and 4, so ignore it
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: [{
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:
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 = {
CAR.ACCORD: dbc_dict('honda_accord_s2t_2018_can_generated', None),
CAR.ACCORD_15: dbc_dict('honda_accord_lx15t_2018_can_generated', None),

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

@ -38,13 +38,12 @@ class CarInterface(CarInterfaceBase):
return float(accel) / 3.0
@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.carName = "hyundai"
ret.carFingerprint = candidate
ret.carVin = vin
ret.isPandaBlack = has_relay
ret.radarOffCan = True
ret.safetyModel = car.CarParams.SafetyModel.hyundai

2
selfdrive/car/interfaces.py
View File

@ -18,7 +18,7 @@ class CarInterfaceBase():
raise NotImplementedError
@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
# returns a car.CarState, pass in car.CarControl

128
selfdrive/car/isotp_parallel_query.py 100644
View File

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

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

@ -34,7 +34,7 @@ class CarInterface(CarInterfaceBase):
return accel
@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()

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

@ -35,13 +35,12 @@ class CarInterface(CarInterfaceBase):
return float(accel) / 4.0
@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.carName = "subaru"
ret.radarOffCan = True
ret.carFingerprint = candidate
ret.carVin = vin
ret.isPandaBlack = has_relay
ret.safetyModel = car.CarParams.SafetyModel.subaru

3
selfdrive/car/toyota/interface.py
View File

@ -37,13 +37,12 @@ class CarInterface(CarInterfaceBase):
return float(accel) / 3.0
@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.carName = "toyota"
ret.carFingerprint = candidate
ret.carVin = vin
ret.isPandaBlack = has_relay
ret.safetyModel = car.CarParams.SafetyModel.toyota

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

@ -1,4 +1,6 @@
from selfdrive.car import dbc_dict
from cereal import car
Ecu = car.CarParams.Ecu
# Steer torque limits
class SteerLimitParams:
@ -31,9 +33,9 @@ class CAR:
class ECU:
CAM = 0 # camera
DSU = 1 # driving support unit
APGS = 2 # advanced parking guidance system
CAM = Ecu.fwdCamera # camera
DSU = Ecu.dsu # driving support unit
APGS = Ecu.apgs # advanced parking guidance system
# 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
DBC = {

117
selfdrive/car/vin.py
View File

@ -1,104 +1,33 @@
#!/usr/bin/env python3
import cereal.messaging as messaging
from selfdrive.boardd.boardd import can_list_to_can_capnp
import traceback
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
# 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:
# VIN does not fit in a single message and it's 20 bytes of data
if can_dat[0] != 0x10 or can_dat[1] != 0x14:
return False
def get_vin(logcan, sendcan, bus, timeout=0.1, retry=5, debug=False):
for i in range(retry):
try:
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:
# 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()
return 0, VIN_UNKNOWN
if __name__ == "__main__":
logcan = messaging.sub_sock('can')
import time
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)

3
selfdrive/car/volkswagen/interface.py
View File

@ -42,12 +42,11 @@ class CarInterface(CarInterfaceBase):
return float(accel) / 4.0
@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.carFingerprint = candidate
ret.isPandaBlack = has_relay
ret.carVin = vin
if candidate == CAR.GOLF:
# Set common MQB parameters that will apply globally

1
selfdrive/clocksd/.gitignore vendored 100644
View File

@ -0,0 +1 @@
clocksd

2
selfdrive/clocksd/SConscript 100644
View File

@ -0,0 +1,2 @@
Import('env', 'common', 'messaging')
env.Program('clocksd.cc', LIBS=['diag', 'time_genoff', common, messaging, 'capnp', 'zmq', 'kj'])

72
selfdrive/clocksd/clocksd.cc 100644
View File

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

11
selfdrive/common/framebuffer.cc
View File

@ -1,4 +1,3 @@
#include <cstdio>
#include <cstdlib>
#include <cassert>
@ -39,7 +38,6 @@ extern "C" void framebuffer_set_power(FramebufferState *s, int mode) {
extern "C" FramebufferState* framebuffer_init(
const char* name, int32_t layer, int alpha,
EGLDisplay *out_display, EGLSurface *out_surface,
int *out_w, int *out_h) {
status_t status;
int success;
@ -131,11 +129,14 @@ extern "C" FramebufferState* framebuffer_init(
const char brightness_level[] = BACKLIGHT_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_h) *out_h = h;
return s;
}
extern "C" void framebuffer_swap(FramebufferState *s) {
eglSwapBuffers(s->display, s->surface);
assert(glGetError() == GL_NO_ERROR);
}

2
selfdrive/common/framebuffer.h
View File

@ -11,10 +11,10 @@ typedef struct FramebufferState FramebufferState;
FramebufferState* framebuffer_init(
const char* name, int32_t layer, int alpha,
EGLDisplay *out_display, EGLSurface *out_surface,
int *out_w, int *out_h);
void framebuffer_set_power(FramebufferState *s, int mode);
void framebuffer_swap(FramebufferState *s);
/* Display power modes */
enum {

4
selfdrive/common/params.cc
View File

@ -243,10 +243,6 @@ int read_db_value(const char* params_path, const char* key, char** value,
goto cleanup;
}
// Remove one for null byte.
if (value_sz != NULL) {
*value_sz -= 1;
}
result = 0;
cleanup:

6
selfdrive/common/util.c
View File

@ -19,7 +19,7 @@ void* read_file(const char* path, size_t* out_len) {
long f_len = ftell(f);
rewind(f);
char* buf = calloc(f_len + 1, 1);
char* buf = (char*)calloc(f_len, 1);
assert(buf);
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) {
*out_len = f_len + 1;
*out_len = f_len;
}
return buf;
@ -54,6 +54,8 @@ int set_realtime_priority(int level) {
memset(&sa, 0, sizeof(sa));
sa.sched_priority = level;
return sched_setscheduler(tid, SCHED_FIFO, &sa);
#else
return -1;
#endif
}

2
selfdrive/common/version.h
View File

@ -1 +1 @@
#define COMMA_VERSION "0.7-release"
#define COMMA_VERSION "0.7.1-release"

9
selfdrive/common/visionimg.h
View File

@ -1,13 +1,12 @@
#ifndef VISIONIMG_H
#define VISIONIMG_H
#ifdef QCOM
#include "common/visionbuf.h"
#include <GLES3/gl3.h>
#include <EGL/egl.h>
#include <EGL/eglext.h>
#endif
#include "common/visionbuf.h"
#undef Status
#ifdef __cplusplus
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);
VisionImg visionimg_alloc_rgb24(int width, int height, VisionBuf *out_buf);
#ifdef QCOM
EGLClientBuffer visionimg_to_egl(const VisionImg *img, void **pph);
GLuint visionimg_to_gl(const VisionImg *img, EGLImageKHR *pkhr, void **pph);
void visionimg_destroy_gl(EGLImageKHR khr, void *ph);
#endif
#ifdef __cplusplus
} // extern "C"

20
selfdrive/controls/controlsd.py
View File

@ -67,7 +67,7 @@ def events_to_bytes(events):
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"""
# 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
if not can_strs:
can_error_counter += 1
events.append(create_event('canError', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
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:
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):
@ -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,
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"""
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),
"mapValid": sm['plan'].mapValid,
"forceDecel": bool(force_decel),
"canErrorCounter": can_error_counter,
}
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_last = 0
mismatch_counter = 0
can_error_counter = 0
last_blinker_frame = 0
events_prev = []
@ -557,11 +560,13 @@ def controlsd_thread(sm=None, pm=None, can_sock=None):
prof.checkpoint("Ratekeeper", ignore=True)
# 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")
# 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]))
if not sm['pathPlan'].mpcSolutionValid:
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]))
if community_feature_disallowed:
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
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
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")
rk.monitor_time()

23
selfdrive/controls/lib/alerts.py
View File

@ -410,6 +410,13 @@ ALERTS = [
AlertStatus.critical, AlertSize.full,
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(
"radarCanError",
"TAKE CONTROL IMMEDIATELY",
@ -659,6 +666,13 @@ ALERTS = [
AlertStatus.normal, AlertSize.mid,
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(
"internetConnectivityNeededNoEntry",
"openpilot Unavailable",
@ -739,11 +753,18 @@ ALERTS = [
Alert(
"lowMemoryPermanent",
"RAM Memory Critically Low",
"RAM Critically Low",
"Reboot your EON",
AlertStatus.normal, AlertSize.mid,
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(
"vehicleModelInvalid",
"Vehicle Parameter Identification Failed",

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

@ -20,9 +20,9 @@ _PITCH_WEIGHT = 1.35 # 1.5 # pitch matters a lot more
_METRIC_THRESHOLD = 0.4
_METRIC_THRESHOLD_SLACK = 0.55
_METRIC_THRESHOLD_STRICT = 0.4
_PITCH_POS_ALLOWANCE = 0.04 # 0.08 # 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
_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)
_PITCH_POS_ALLOWANCE = 0.12 # rad, to not be too sensitive on positive pitch
_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)
_DISTRACTED_FILTER_TS = 0.25 # 0.6Hz
@ -138,13 +138,13 @@ class DriverStatus():
if not self.pose_calibrated:
pitch_error = pose.pitch - _PITCH_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:
pitch_error = pose.pitch - self.pose.pitch_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
pose_metric = np.sqrt(yaw_error**2 + pitch_error**2)

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

@ -14,6 +14,9 @@ LaneChangeDirection = log.PathPlan.LaneChangeDirection
LOG_MPC = os.environ.get('LOG_MPC', False)
LANE_CHANGE_SPEED_MIN = 45 * CV.MPH_TO_MS
LANE_CHANGE_TIME_MAX = 10.
DESIRES = {
LaneChangeDirection.none: {
LaneChangeState.off: log.PathPlan.Desire.none,
@ -88,8 +91,9 @@ class PathPlanner():
# Lane change logic
lane_change_direction = LaneChangeDirection.none
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
else:
if sm['carState'].leftBlinker:
@ -97,23 +101,23 @@ class PathPlanner():
elif sm['carState'].rightBlinker:
lane_change_direction = LaneChangeDirection.right
if lane_change_direction == LaneChangeDirection.left:
torque_applied = sm['carState'].steeringTorque > 0 and sm['carState'].steeringPressed
else:
torque_applied = sm['carState'].steeringTorque < 0 and sm['carState'].steeringPressed
torque_applied = sm['carState'].steeringPressed and \
((sm['carState'].steeringTorque > 0 and lane_change_direction == LaneChangeDirection.left) or \
(sm['carState'].steeringTorque < 0 and lane_change_direction == LaneChangeDirection.right))
lane_change_prob = self.LP.l_lane_change_prob + self.LP.r_lane_change_prob
# State transitions
# 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
# pre
elif self.lane_change_state == LaneChangeState.preLaneChange and not one_blinker:
self.lane_change_state = LaneChangeState.off
elif self.lane_change_state == LaneChangeState.preLaneChange and torque_applied:
self.lane_change_state = LaneChangeState.laneChangeStarting
elif self.lane_change_state == LaneChangeState.preLaneChange:
if not one_blinker or below_lane_change_speed:
self.lane_change_state = LaneChangeState.off
elif torque_applied:
self.lane_change_state = LaneChangeState.laneChangeStarting
# starting
elif self.lane_change_state == LaneChangeState.laneChangeStarting and lane_change_prob > 0.5:
@ -121,11 +125,10 @@ class PathPlanner():
# finishing
elif self.lane_change_state == LaneChangeState.laneChangeFinishing and lane_change_prob < 0.2:
self.lane_change_state = LaneChangeState.preLaneChange
# Don't allow starting lane change below 45 mph
if (v_ego < 45 * CV.MPH_TO_MS) and (self.lane_change_state == LaneChangeState.preLaneChange):
self.lane_change_state = LaneChangeState.off
if one_blinker:
self.lane_change_state = LaneChangeState.preLaneChange
else:
self.lane_change_state = LaneChangeState.off
if self.lane_change_state in [LaneChangeState.off, LaneChangeState.preLaneChange]:
self.lane_change_timer = 0.0

19
selfdrive/controls/lib/planner.py
View File

@ -27,7 +27,8 @@ _A_CRUISE_MIN_BP = [ 0., 5., 10., 20., 40.]
# need fast accel at very low speed for stop and go
# 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.]
# Lookup table for turns
@ -38,9 +39,13 @@ _A_TOTAL_MAX_BP = [20., 40.]
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_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])
@ -80,6 +85,7 @@ class Planner():
self.path_x = np.arange(192)
self.params = Params()
self.first_loop = True
def choose_solution(self, v_cruise_setpoint, enabled):
if enabled:
@ -122,6 +128,7 @@ class Planner():
lead_2 = sm['radarState'].leadTwo
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):
path = list(sm['model'].path.poly)
@ -142,8 +149,8 @@ class Planner():
model_speed = MAX_SPEED
# Calculate speed for normal cruise control
if enabled:
accel_limits = [float(x) for x in calc_cruise_accel_limits(v_ego)]
if enabled and not self.first_loop:
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
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
self.v_acc_start = v_acc_sol
self.a_acc_start = a_acc_sol
self.first_loop = False

2
selfdrive/controls/tests/test_following_distance.py
View File

@ -38,7 +38,7 @@ def run_following_distance_simulation(v_lead, t_end=200.0):
first = True
while t < t_end:
# 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])]
v_cruise, a_cruise = speed_smoother(v_ego, a_ego, v_cruise_setpoint,
accel_limits[1], accel_limits[0],

87
selfdrive/locationd/calibrationd.py
View File

@ -10,19 +10,23 @@ from selfdrive.swaglog import cloudlog
from common.params import Params, put_nonblocking
from common.transformations.model import model_height
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
MIN_SPEED_FILTER = 15 * MPH_TO_MS
MAX_SPEED_STD = 1.5
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
WRITE_CYCLES = 400 # write every 400 cycles
# This is all 20Hz, blocks needed for efficiency
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.])
# These validity corners were chosen by looking at 1000
# 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
@ -31,13 +35,29 @@ def is_calibration_valid(vp):
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():
def __init__(self, param_put=False):
self.param_put = param_put
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.write_counter = 0
self.just_calibrated = False
# Read calibration
@ -46,14 +66,19 @@ class Calibrator():
try:
calibration_params = json.loads(calibration_params)
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()
except Exception:
cloudlog.exception("CalibrationParams file found but error encountered")
def update_status(self):
start_status = self.cal_status
if len(self.vps) < INPUTS_NEEDED:
if self.valid_blocks < INPUTS_NEEDED:
self.cal_status = Calibration.UNCALIBRATED
else:
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:
self.just_calibrated = True
def handle_cam_odom(self, log):
trans, rot = log.trans, log.rot
if np.linalg.norm(trans) > MIN_SPEED_FILTER and abs(rot[2]) < MAX_YAW_RATE_FILTER:
new_vp = eon_intrinsics.dot(view_frame_from_device_frame.dot(trans))
def handle_cam_odom(self, trans, rot, trans_std, rot_std):
if ((trans[0] > MIN_SPEED_FILTER) and
(trans_std[0] < MAX_SPEED_STD) and
(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]
self.vps.append(new_vp)
self.vps = self.vps[-INPUTS_WANTED:]
self.vp = np.mean(self.vps, axis=0)
self.vps[self.block_idx] = (self.idx*self.vps[self.block_idx] + (BLOCK_SIZE - self.idx) * new_vp) / float(BLOCK_SIZE)
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.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),
"valid_points": len(self.vps)}
"valid_blocks": self.valid_blocks}
put_nonblocking("CalibrationParams", json.dumps(cal_params).encode('utf8'))
return new_vp
else:
@ -88,7 +122,7 @@ class Calibrator():
cal_send = messaging.new_message()
cal_send.init('liveCalibration')
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.rpyCalib = [float(x) for x in calib]
@ -104,15 +138,22 @@ def calibrationd_thread(sm=None, pm=None):
calibrator = Calibrator(param_put=True)
# buffer with all the messages that still need to be input into the kalman
send_counter = 0
while 1:
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:
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):

4
selfdrive/locationd/locationd_yawrate.cc
View File

@ -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) {
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];
update_state(C_posenet, R, current_time, meas);
@ -73,7 +73,7 @@ Localizer::Localizer() {
0, 0, 0, 0,
0, pow(0.1, 2.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 <<
pow(100.0, 2.0), 0, 0, 0,
0, pow(100.0, 2.0), 0, 0,

16
selfdrive/loggerd/SConscript
View File

@ -1,6 +1,12 @@
Import('env', 'messaging', 'common', 'visionipc')
env.Program(['loggerd.cc', 'logger.c', 'raw_logger.cc', 'encoder.c'], LIBS=[
'zmq', 'czmq', 'capnp', 'kj', 'yaml-cpp', 'z',
Import('env', 'arch', 'messaging', 'common', 'visionipc')
src = ['loggerd.cc', 'logger.c']
libs = ['zmq', 'czmq', 'capnp', 'kj', 'yaml-cpp', 'z',
'avformat', 'avcodec', 'swscale', 'avutil',
'OmxVenc', 'OmxCore', 'yuv',
'bz2', 'cutils', common, 'json', messaging, visionipc])
'yuv', 'bz2', common, 'json', messaging, visionipc]
if arch == "aarch64":
src += ['encoder.c', 'raw_logger.cc']
libs += ['OmxVenc', 'OmxCore', 'cutils']
env.Program(src, LIBS=libs)

14
selfdrive/loggerd/config.py
View File

@ -9,11 +9,21 @@ else:
SEGMENT_LENGTH = 60
def get_available_percent():
def get_available_percent(default=None):
try:
statvfs = os.statvfs(ROOT)
available_percent = 100.0 * statvfs.f_bavail / statvfs.f_blocks
except OSError:
available_percent = 100.0
available_percent = default
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

6
selfdrive/loggerd/deleter.py
View File

@ -3,15 +3,15 @@ import os
import shutil
import threading
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
def deleter_thread(exit_event):
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
dirs = listdir_by_creation(ROOT)
for delete_dir in dirs:

10
selfdrive/loggerd/loggerd.cc
View File

@ -25,7 +25,10 @@
#include <zmq.h>
#include <yaml-cpp/yaml.h>
#include <capnp/serialize.h>
#ifdef QCOM
#include <cutils/properties.h>
#endif
#include "common/version.h"
#include "common/timing.h"
@ -38,6 +41,11 @@
#include "logger.h"
#include "messaging.hpp"
#ifndef QCOM
// no encoder on PC
#define DISABLE_ENCODER
#endif
#ifndef DISABLE_ENCODER
#include "encoder.h"
@ -420,6 +428,7 @@ kj::Array<capnp::word> gen_init_data() {
init.setKernelVersion(util::read_file("/proc/version"));
#ifdef QCOM
{
std::vector<std::pair<std::string, std::string> > properties;
property_list(append_property, (void*)&properties);
@ -431,6 +440,7 @@ kj::Array<capnp::word> gen_init_data() {
lentry.setValue(properties[i].second);
}
}
#endif
const char* dongle_id = getenv("DONGLE_ID");
if (dongle_id) {

14
selfdrive/loggerd/tests/test_deleter.py
View File

@ -1,6 +1,7 @@
import os
import time
import threading
import unittest
from collections import namedtuple
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
Stats = namedtuple("Stats", ['f_bavail', 'f_blocks'])
Stats = namedtuple("Stats", ['f_bavail', 'f_blocks', 'f_frsize'])
class TestDeleter(UploaderTestCase):
def fake_statvfs(self, d):
@ -17,7 +19,7 @@ class TestDeleter(UploaderTestCase):
def setUp(self):
self.f_type = "fcamera.hevc"
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.ROOT = self.root
@ -68,7 +70,9 @@ class TestDeleter(UploaderTestCase):
def test_no_delete_when_available_space(self):
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()
@ -98,3 +102,7 @@ class TestDeleter(UploaderTestCase):
self.join_thread()
self.assertTrue(os.path.exists(f_path), "File deleted when locked")
if __name__ == "__main__":
unittest.main()

4
selfdrive/loggerd/uploader.py
View File

@ -68,13 +68,15 @@ def is_on_wifi():
# ConnectivityManager.getActiveNetworkInfo()
try:
result = android.parse_service_call_string(["connectivity", "2"])
if result is None:
return True
return 'WIFI' in result
except AttributeError:
return False
def is_on_hotspot():
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]
is_android = result.startswith('192.168.43.')

72
selfdrive/manager.py
View File

@ -5,10 +5,12 @@ import sys
import fcntl
import errno
import signal
import shutil
import subprocess
import datetime
from common.basedir import BASEDIR
from common.android import ANDROID
sys.path.append(os.path.join(BASEDIR, "pyextra"))
os.environ['BASEDIR'] = BASEDIR
@ -21,7 +23,7 @@ try:
except FileExistsError:
pass
if os.path.isfile('/EON'):
if ANDROID:
os.chmod("/dev/shm", 0o777)
def unblock_stdout():
@ -74,7 +76,11 @@ if not prebuilt:
# run scons
env = os.environ.copy()
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
while scons.poll() is None:
@ -96,8 +102,12 @@ if not prebuilt:
if scons.returncode != 0:
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)
shutil.rmtree("/tmp/scons_cache")
else:
raise RuntimeError("scons build failed")
else:
@ -139,11 +149,13 @@ managed_processes = {
"paramsd": ("selfdrive/locationd", ["./paramsd"]),
"camerad": ("selfdrive/camerad", ["./camerad"]),
"sensord": ("selfdrive/sensord", ["./sensord"]),
"clocksd": ("selfdrive/clocksd", ["./clocksd"]),
"gpsd": ("selfdrive/sensord", ["./gpsd"]),
"updated": "selfdrive.updated",
"monitoringd": ("selfdrive/modeld", ["./monitoringd"]),
"modeld": ("selfdrive/modeld", ["./modeld"]),
}
daemon_processes = {
"manage_athenad": ("selfdrive.athena.manage_athenad", "AthenadPid"),
}
@ -161,32 +173,42 @@ interrupt_processes = []
# processes to end with SIGKILL instead of SIGTERM
kill_processes = ['sensord', 'paramsd']
# processes to end if thermal conditions exceed Green parameters
green_temp_processes = ['uploader']
persistent_processes = [
'thermald',
'logmessaged',
'logcatd',
'tombstoned',
'uploader',
'ui',
'updated',
'uploader',
]
if ANDROID:
persistent_processes += [
'logcatd',
'tombstoned',
'updated',
]
car_started_processes = [
'controlsd',
'plannerd',
'loggerd',
'sensord',
'radard',
'calibrationd',
'paramsd',
'camerad',
'modeld',
'monitoringd',
'proclogd',
'ubloxd',
'gpsd',
'deleter',
]
if ANDROID:
car_started_processes += [
'sensord',
'clocksd',
'gpsd',
'monitoringd',
'deleter',
]
def register_managed_process(name, desc, car_started=False):
global managed_processes, car_started_processes, persistent_processes
@ -303,7 +325,8 @@ def kill_managed_process(name):
def cleanup_all_processes(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()):
kill_managed_process(name)
@ -365,8 +388,9 @@ def manager_thread():
start_managed_process(p)
# start frame
pm_apply_packages('enable')
start_frame()
if ANDROID:
pm_apply_packages('enable')
start_frame()
if os.getenv("NOBOARD") is None:
start_managed_process("pandad")
@ -376,11 +400,15 @@ def manager_thread():
while 1:
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:
kill_managed_process("uploader")
for p in green_temp_processes:
if p in persistent_processes:
kill_managed_process(p)
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:
logger_dead = True
@ -430,13 +458,6 @@ def main():
# disable bluetooth
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.manager_start()
@ -480,7 +501,8 @@ def main():
if params.get("Passive") is None:
raise Exception("Passive must be set to continue")
update_apks()
if ANDROID:
update_apks()
manager_init()
manager_prepare(spinner)
spinner.close()

27
selfdrive/modeld/SConscript
View File

@ -1,25 +1,36 @@
Import('env', 'arch', 'messaging', 'common', 'gpucommon', 'visionipc')
lenv = env.Clone()
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":
libs += ['gsl', 'CB', 'gnustl_shared']
else:
libs += ['symphony-cpu', 'pthread']
common = env.Object([
"models/commonmodel.c",
"runners/snpemodel.cc",
"transforms/loadyuv.c",
"transforms/transform.c"])
if FindFile('libtensorflow.so', env['LIBPATH']):
# for tensorflow support
common_src += ['runners/tfmodel.cc']
libs += ['tensorflow']
# 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",
"models/monitoring.cc",
]+common, LIBS=libs)
env.Program('_modeld', [
lenv.Program('_modeld', [
"modeld.cc",
"models/driving.cc",
"models/posenet.cc",
]+common, LIBS=libs)

2
selfdrive/modeld/modeld
View File

@ -1,4 +1,4 @@
#!/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

72
selfdrive/modeld/modeld.cc
View File

@ -1,18 +1,11 @@
#include <stdio.h>
#include <stdlib.h>
#ifdef QCOM
#include <eigen3/Eigen/Dense>
#else
#include <Eigen/Dense>
#endif
#include "common/visionbuf.h"
#include "common/visionipc.h"
#include "common/swaglog.h"
#include "models/driving.h"
#include "models/posenet.h"
volatile sig_atomic_t do_exit = 0;
@ -122,13 +115,26 @@ int main(int argc, char **argv) {
cl_command_queue q;
{
// TODO: refactor this
cl_platform_id platform_id = NULL;
cl_platform_id platform_id[2];
cl_uint num_devices;
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);
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);
assert(err == 0);
@ -141,9 +147,7 @@ int main(int argc, char **argv) {
// init the models
ModelState model;
PosenetState posenet;
model_init(&model, device_id, context, true);
posenet_init(&posenet);
LOGW("models loaded, modeld starting");
// debayering does a 2x downscale
@ -213,54 +217,18 @@ int main(int argc, char **argv) {
// TODO: don't make copies!
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_transform, NULL, vec_desire);
mt2 = millis_since_boot();
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);
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);
}
@ -268,9 +236,7 @@ int main(int argc, char **argv) {
visionstream_destroy(&stream);
delete model_sock;
delete posenet_sock;
posenet_free(&posenet);
model_free(&model);
LOG("joining live_thread");

56
selfdrive/modeld/models/commonmodel.c
View File

@ -5,57 +5,57 @@
#include "common/mat.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) {
int err;
s->device_id = device_id;
s->context = context;
frame->device_id = device_id;
frame->context = context;
transform_init(&s->transform, context, device_id);
s->transformed_width = width;
s->transformed_height = height;
transform_init(&frame->transform, context, device_id);
frame->transformed_width = width;
frame->transformed_height = height;
s->transformed_y_cl = clCreateBuffer(s->context, CL_MEM_READ_WRITE,
s->transformed_width*s->transformed_height, NULL, &err);
frame->transformed_y_cl = clCreateBuffer(frame->context, CL_MEM_READ_WRITE,
frame->transformed_width*frame->transformed_height, NULL, &err);
assert(err == 0);
s->transformed_u_cl = clCreateBuffer(s->context, CL_MEM_READ_WRITE,
(s->transformed_width/2)*(s->transformed_height/2), NULL, &err);
frame->transformed_u_cl = clCreateBuffer(frame->context, CL_MEM_READ_WRITE,
(frame->transformed_width/2)*(frame->transformed_height/2), NULL, &err);
assert(err == 0);
s->transformed_v_cl = clCreateBuffer(s->context, CL_MEM_READ_WRITE,
(s->transformed_width/2)*(s->transformed_height/2), NULL, &err);
frame->transformed_v_cl = clCreateBuffer(frame->context, CL_MEM_READ_WRITE,
(frame->transformed_width/2)*(frame->transformed_height/2), NULL, &err);
assert(err == 0);
s->net_input_size = ((width*height*3)/2)*sizeof(float);
s->net_input = clCreateBuffer(s->context, CL_MEM_READ_WRITE,
s->net_input_size, (void*)NULL, &err);
frame->net_input_size = ((width*height*3)/2)*sizeof(float);
frame->net_input = clCreateBuffer(frame->context, CL_MEM_READ_WRITE,
frame->net_input_size, (void*)NULL, &err);
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,
mat3 transform) {
int err;
int i = 0;
transform_queue(&s->transform, q,
transform_queue(&frame->transform, q,
yuv_cl, width, height,
s->transformed_y_cl, s->transformed_u_cl, s->transformed_v_cl,
s->transformed_width, s->transformed_height,
frame->transformed_y_cl, frame->transformed_u_cl, frame->transformed_v_cl,
frame->transformed_width, frame->transformed_height,
transform);
loadyuv_queue(&s->loadyuv, q,
s->transformed_y_cl, s->transformed_u_cl, s->transformed_v_cl,
s->net_input);
float *net_input_buf = (float *)clEnqueueMapBuffer(q, s->net_input, CL_TRUE,
CL_MAP_READ, 0, s->net_input_size,
loadyuv_queue(&frame->loadyuv, q,
frame->transformed_y_cl, frame->transformed_u_cl, frame->transformed_v_cl,
frame->net_input);
float *net_input_buf = (float *)clEnqueueMapBuffer(q, frame->net_input, CL_TRUE,
CL_MAP_READ, 0, frame->net_input_size,
0, NULL, NULL, &err);
clFinish(q);
return net_input_buf;
}
void model_input_free(ModelInput* s) {
transform_destroy(&s->transform);
loadyuv_destroy(&s->loadyuv);
void frame_free(ModelFrame* frame) {
transform_destroy(&frame->transform);
loadyuv_destroy(&frame->loadyuv);
}

11
selfdrive/modeld/models/commonmodel.h
View File

@ -4,7 +4,6 @@
#include <CL/cl.h>
#include "common/mat.h"
#include "common/modeldata.h"
#include "transforms/transform.h"
#include "transforms/loadyuv.h"
@ -15,7 +14,7 @@ extern "C" {
float softplus(float input);
float sigmoid(float input);
typedef struct ModelInput {
typedef struct ModelFrame {
cl_device_id device_id;
cl_context context;
@ -26,14 +25,14 @@ typedef struct ModelInput {
LoadYUVState loadyuv;
cl_mem net_input;
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);
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,
mat3 transform);
void model_input_free(ModelInput* s);
void frame_free(ModelFrame* frame);
#ifdef __cplusplus
}

355
selfdrive/modeld/models/driving.cc
View File

@ -2,26 +2,20 @@
#include <assert.h>
#include <fcntl.h>
#include <unistd.h>
#ifdef QCOM
#include <eigen3/Eigen/Dense>
#else
#include <Eigen/Dense>
#endif
#include "common/timing.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 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 SPEED_BUCKETS 100
#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 PATH_IDX 0
#define LL_IDX PATH_IDX + MODEL_PATH_DISTANCE*2
#define RL_IDX LL_IDX + MODEL_PATH_DISTANCE*2 + 1
#define LEAD_IDX RL_IDX + MODEL_PATH_DISTANCE*2 + 1
#define LONG_X_IDX LEAD_IDX + MDN_GROUP_SIZE*LEAD_MDN_N + SELECTION
#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
#define TEMPORAL_SIZE 512
#else
@ -33,15 +27,19 @@
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) {
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;
s->output = (float*)malloc(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->output = (float*)calloc(output_size, sizeof(float));
s->m = new DefaultRunModel("../../models/supercombo.dlc", s->output, output_size, USE_GPU_RUNTIME);
#ifdef TEMPORAL
assert(temporal);
s->m->addRecurrent(&s->output[OUTPUT_SIZE], TEMPORAL_SIZE);
#endif
#ifdef DESIRE
s->desire = (float*)malloc(DESIRE_SIZE * sizeof(float));
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,
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
if (desire_in != NULL) {
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");
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
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);
assert(1==2);
#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.path = &s->output[0];
net_outputs.left_lane = &s->output[MODEL_PATH_DISTANCE*2];
net_outputs.right_lane = &s->output[MODEL_PATH_DISTANCE*2 + MODEL_PATH_DISTANCE*2 + 1];
net_outputs.lead = &s->output[MODEL_PATH_DISTANCE*2 + (MODEL_PATH_DISTANCE*2 + 1)*2];
//net_outputs.speed = &s->output[OUTPUT_SIZE - SPEED_BUCKETS];
net_outputs.meta = &s->output[OUTPUT_SIZE - OTHER_META_SIZE - DESIRE_PRED_SIZE];
ModelData model = {0};
for (int i=0; i<MODEL_PATH_DISTANCE; i++) {
model.path.points[i] = net_outputs.path[i];
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;
ModelDataRaw net_outputs;
net_outputs.path = &s->output[PATH_IDX];
net_outputs.left_lane = &s->output[LL_IDX];
net_outputs.right_lane = &s->output[RL_IDX];
net_outputs.lead = &s->output[LEAD_IDX];
net_outputs.long_x = &s->output[LONG_X_IDX];
net_outputs.long_v = &s->output[LONG_V_IDX];
net_outputs.long_a = &s->output[LONG_A_IDX];
net_outputs.meta = &s->output[META_IDX];
net_outputs.pose = &s->output[POSE_IDX];
return net_outputs;
}
void model_free(ModelState* s) {
free(s->output);
model_input_free(&s->in);
free(s->input_frames);
frame_free(&s->frame);
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")){
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);
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);
}
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.setProb(path_data.prob);
path.setStd(path_data.std);
path.setProb(prob);
path.setStd(std);
}
void fill_lead(cereal::ModelData::LeadData::Builder lead, const LeadData lead_data) {
lead.setDist(lead_data.dist);
lead.setProb(lead_data.prob);
lead.setStd(lead_data.std);
lead.setRelY(lead_data.rel_y);
lead.setRelYStd(lead_data.rel_y_std);
lead.setRelVel(lead_data.rel_v);
lead.setRelVelStd(lead_data.rel_v_std);
lead.setRelA(lead_data.rel_a);
lead.setRelAStd(lead_data.rel_a_std);
void fill_lead(cereal::ModelData::LeadData::Builder lead, const float * data, int mdn_max_idx) {
const double x_scale = 10.0;
const double y_scale = 10.0;
lead.setProb(sigmoid(data[LEAD_MDN_N*MDN_GROUP_SIZE]));
lead.setDist(x_scale * data[mdn_max_idx*MDN_GROUP_SIZE]);
lead.setStd(x_scale * softplus(data[mdn_max_idx*MDN_GROUP_SIZE + MDN_VALS]));
lead.setRelY(y_scale * data[mdn_max_idx*MDN_GROUP_SIZE + 1]);
lead.setRelYStd(y_scale * softplus(data[mdn_max_idx*MDN_GROUP_SIZE + MDN_VALS + 1]));
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) {
@ -260,39 +184,104 @@ void fill_meta(cereal::ModelData::MetaData::Builder meta, const float * meta_dat
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,
const ModelData data, uint64_t timestamp_eof) {
// make msg
capnp::MallocMessageBuilder msg;
cereal::Event::Builder event = msg.initRoot<cereal::Event>();
event.setLogMonoTime(nanos_since_boot());
const ModelDataRaw net_outputs, uint64_t timestamp_eof) {
// make msg
capnp::MallocMessageBuilder msg;
cereal::Event::Builder event = msg.initRoot<cereal::Event>();
event.setLogMonoTime(nanos_since_boot());
auto framed = event.initModel();
framed.setFrameId(frame_id);
framed.setTimestampEof(timestamp_eof);
auto framed = event.initModel();
framed.setFrameId(frame_id);
framed.setTimestampEof(timestamp_eof);
kj::ArrayPtr<const float> speed(&data.speed[0], ARRAYSIZE(data.speed));
framed.setSpeed(speed);
auto lpath = framed.initPath();
fill_path(lpath, net_outputs.path, false, 0);
auto left_lane = framed.initLeftLane();
fill_path(left_lane, net_outputs.left_lane, true, 1.8);
auto right_lane = framed.initRightLane();
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();
fill_path(lpath, data.path);
auto left_lane = framed.initLeftLane();
fill_path(left_lane, data.left_lane);
auto right_lane = framed.initRightLane();
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());
// 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;
}
}
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());
}

45
selfdrive/modeld/models/driving.h
View File

@ -9,8 +9,13 @@
#define DESIRE_SIZE 8
#endif
#ifdef QCOM
#include <eigen3/Eigen/Dense>
#else
#include <Eigen/Dense>
#endif
#include "common/mat.h"
#include "common/modeldata.h"
#include "common/util.h"
#include "commonmodel.h"
@ -21,10 +26,40 @@
#include <capnp/serialize.h>
#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 {
ModelInput in;
ModelFrame frame;
float *output;
float *input_frames;
RunModel *m;
#ifdef DESIRE
float *desire;
@ -33,12 +68,14 @@ typedef struct ModelState {
void model_init(ModelState* s, cl_device_id device_id,
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,
mat3 transform, void* sock, float *desire_in);
void model_free(ModelState* s);
void poly_fit(float *in_pts, float *in_stds, float *out);
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

58
selfdrive/modeld/models/posenet.cc
View File

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

27
selfdrive/modeld/models/posenet.h
View File

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

2
selfdrive/modeld/monitoringd
View File

@ -1,5 +1,5 @@
#!/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/"
exec ./_monitoringd

9
selfdrive/modeld/runners/run.h
View File

@ -7,9 +7,12 @@
#ifdef QCOM
#define DefaultRunModel SNPEModel
#else
#define DefaultRunModel SNPEModel
/* #include "tfmodel.h" */
/* #define DefaultRunModel TFModel */
#ifdef USE_TF_MODEL
#include "tfmodel.h"
#define DefaultRunModel TFModel
#else
#define DefaultRunModel SNPEModel
#endif
#endif
#endif

57
selfdrive/pandad.py
View File

@ -4,19 +4,29 @@ import os
import time
from selfdrive.swaglog import cloudlog
from panda import Panda, PandaDFU, BASEDIR
from panda import Panda, PandaDFU, BASEDIR, build_st
def get_expected_version():
with open(os.path.join(BASEDIR, "VERSION")) as f:
repo_version = f.read()
repo_version += "-EON" if os.path.isfile('/EON') else "-DEV"
return repo_version
def get_firmware_fn():
signed_fn = os.path.join(BASEDIR, "board", "obj", "panda.bin.signed")
if os.path.exists(signed_fn):
cloudlog.info("Using prebuilt signed firmware")
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():
repo_version = get_expected_version()
panda = None
panda_dfu = None
@ -37,27 +47,28 @@ def update_panda():
panda_dfu = PandaDFU(panda_dfu[0])
panda_dfu.recover()
print("waiting for board...")
time.sleep(1)
fw_fn = get_firmware_fn()
fw_signature = get_expected_signature(fw_fn)
try:
serial = panda.get_serial()[0].decode("utf-8")
except Exception:
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")
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()
panda.flash(fw_fn)
cloudlog.info("Done flashing")
if panda.bootstub:
@ -69,8 +80,8 @@ def update_panda():
cloudlog.info("Panda still not booting, exiting")
raise AssertionError
version = panda.get_version()
if not version.startswith(repo_version):
panda_signature = panda.get_signature()
if panda_signature != fw_signature:
cloudlog.info("Version mismatch after flashing, exiting")
raise AssertionError

13
selfdrive/registration.py
View File

@ -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.api import api_get
from common.params import Params
from common.file_helpers import mkdirs_exists_ok
def register():
params = Params()
@ -18,6 +19,17 @@ def register():
params.put("GitRemote", get_git_remote())
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)
os.chmod('/persist/comma/', 0o755)
os.chmod('/persist/comma/id_rsa', 0o744)
@ -51,5 +63,4 @@ def register():
return None
if __name__ == "__main__":
print(api_get("").text)
print(register())

65
selfdrive/sensord/gpsd.cc
View File

@ -7,7 +7,6 @@
#include <sys/time.h>
#include <sys/cdefs.h>
#include <sys/types.h>
#include <sys/timerfd.h>
#include <sys/resource.h>
#include <pthread.h>
@ -31,8 +30,6 @@ volatile sig_atomic_t do_exit = 0;
namespace {
pthread_t clock_thread_handle;
Context *gps_context;
PubSocket *gps_publisher;
PubSocket *gps_location_publisher;
@ -181,61 +178,6 @@ int64_t arm_cntpct() {
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() {
@ -249,15 +191,8 @@ int main() {
rawgps_init();
err = pthread_create(&clock_thread_handle, NULL,
clock_thread, NULL);
assert(err == 0);
while(!do_exit) pause();
err = pthread_join(clock_thread_handle, NULL);
assert(err == 0);
rawgps_destroy();
gps_destroy();

306
selfdrive/sensord/sensors.cc
View File

@ -38,6 +38,7 @@
#define SENSOR_LIGHT 7
volatile sig_atomic_t do_exit = 0;
volatile sig_atomic_t re_init_sensors = 0;
namespace {
@ -47,179 +48,190 @@ void set_do_exit(int sig) {
void sigpipe_handler(int sig) {
LOGE("SIGPIPE received");
re_init_sensors = true;
}
void 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) {
int n = device->poll(device, buffer, numEvents);
if (n == 0) continue;
if (n < 0) {
LOG("sensor_loop poll failed: %d", n);
continue;
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);
}
int log_events = 0;
for (int i=0; i < n; i++) {
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;
}
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);
}
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;
cereal::Event::Builder event = msg.initRoot<cereal::Event>();
event.setLogMonoTime(log_time);
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);
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;
for (int i = 0; i < n; i++) {
static const size_t numEvents = 16;
sensors_event_t buffer[numEvents];
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:
while (!do_exit) {
int n = device->poll(device, buffer, numEvents);
if (n == 0) continue;
if (n < 0) {
LOG("sensor_loop poll failed: %d", n);
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;
int log_events = 0;
for (int i=0; i < n; i++) {
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;
}
}
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);
auto bytes = words.asBytes();
sensor_events_sock->send((char*)bytes.begin(), bytes.size());
delete sensor_events_sock;
delete c;
}
}
delete sensor_events_sock;
delete c;
}
}
}// Namespace end
int main(int argc, char *argv[]) {
setpriority(PRIO_PROCESS, 0, -13);

4
selfdrive/test/longitudinal_maneuvers/plant.py
View File

@ -272,6 +272,7 @@ class Plant():
'INTERCEPTOR_GAS',
'INTERCEPTOR_GAS2',
'IMPERIAL_UNIT',
'MOTOR_TORQUE',
])
vls = vls_tuple(
self.speed_sensor(speed),
@ -304,7 +305,8 @@ class Plant():
0, # Brake hold
0, # Interceptor feedback
0, # Interceptor 2 feedback
False
False,
0,
)
# TODO: publish each message at proper frequency

24
selfdrive/test/process_replay/process_replay.py
View File

@ -159,11 +159,11 @@ def get_car_params(msgs, fsm, can_sock):
_, CP = get_car(can, sendcan)
Params().put("CarParams", CP.to_bytes())
def radar_rcv_callback(msg, CP):
def radar_rcv_callback(msg, CP, cfg, fsm):
if msg.which() != "can":
return []
return [], False
elif CP.radarOffCan:
return ["radarState", "liveTracks"]
return ["radarState", "liveTracks"], True
radar_msgs = {"honda": [0x445], "toyota": [0x19f, 0x22f], "gm": [0x474],
"chrysler": [0x2d4]}.get(CP.carName, None)
@ -173,8 +173,15 @@ def radar_rcv_callback(msg, CP):
for m in msg.can:
if m.src == 1 and m.address in radar_msgs:
return ["radarState", "liveTracks"]
return []
return ["radarState", "liveTracks"], True
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 = [
ProcessConfig(
@ -215,7 +222,7 @@ CONFIGS = [
},
ignore=[("logMonoTime", 0), ("valid", True)],
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 = [], []
for msg in tqdm(pub_msgs):
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:
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]
should_recv = bool(len(recv_socks))
should_recv = bool(len(recv_socks))
if msg.which() == 'can':
can_sock.send(msg.as_builder().to_bytes())

2
selfdrive/test/process_replay/ref_commit
View File

@ -1 +1 @@
89304bdcab73fa43a8dd39cab93bc4ea4c9cbbdb
b60841eb6cf09037200bc2daacf0c9cf69b358fe

237
selfdrive/test/test_car_models.py
View File

@ -39,8 +39,6 @@ def wait_for_socket(name, timeout=10.0):
break
time.sleep(0.5)
if r is None:
sys.exit(-1)
return r
def get_route_logs(route_name):
@ -405,42 +403,48 @@ passive_routes = [
#"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: add routes for untested cars: HONDA ACCORD 2018 HYBRID TOURING and CHRYSLER PACIFICA 2018
non_public_routes = [
"0607d2516fc2148f|2019-02-13--23-03-16", # CHRYSLER PACIFICA HYBRID 2019
"3e9592a1c78a3d63|2018-02-08--20-28-24", # HONDA PILOT 2017 TOURING
"aa20e335f61ba898|2019-02-05--16-59-04", # BUICK REGAL ESSENCE 2018
"1851183c395ef471|2018-05-31--18-07-21", # HONDA CR-V 2017 EX
"9d5fb4f0baa1b3e1|2019-01-14--17-45-59", # KIA SORENTO GT LINE 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
"362d23d4d5bea2fa|2018-08-10--13-31-40", # TOYOTA HIGHLANDER HYBRID 2018
"aa20e335f61ba898|2018-12-17--21-10-37", # BUICK REGAL ESSENCE 2018
"215cd70e9c349266|2018-11-25--22-22-12", # KIA STINGER GT2 2018
"192a598e34926b1e|2019-04-04--13-27-39", # JEEP GRAND CHEROKEE 2019
"34a84d2b765df688|2018-08-28--12-41-00", # HONDA PILOT 2019 ELITE
"b0c9d2329ad1606b|2019-01-06--10-11-23", # CHRYSLER PACIFICA HYBRID 2017
"31390e3eb6f7c29a|2019-01-23--08-56-00", # KIA OPTIMA SX 2019
"fd10b9a107bb2e49|2018-07-24--16-32-42", # TOYOTA C-HR 2018
"9f7a7e50a51fb9db|2019-01-17--18-34-21", # JEEP GRAND CHEROKEE V6 2018
"aadda448b49c99ad|2018-10-25--17-16-22", # CHEVROLET MALIBU PREMIER 2017
"362d23d4d5bea2fa|2018-09-02--17-03-55", # TOYOTA HIGHLANDER HYBRID 2018
"1582e1dc57175194|2018-08-15--07-46-07", # HONDA ACCORD 2018 LX 1.5T
"fd10b9a107bb2e49|2018-07-24--20-32-08", # TOYOTA C-HR 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
"21aa231dee2a68bd|2018-01-30--04-54-41", # HONDA ODYSSEY 2018 EX-L
"900ad17e536c3dc7|2018-04-12--22-02-36", # HONDA RIDGELINE 2017 BLACK EDITION
"975b26878285314d|2018-12-25--14-42-13", # CHRYSLER PACIFICA HYBRID 2018
"8ae193ceb56a0efe|2018-06-18--20-07-32", # TOYOTA RAV4 HYBRID 2017
"a893a80e5c5f72c8|2019-01-14--20-02-59", # HYUNDAI GENESIS 2018
"49c73650e65ff465|2018-11-19--16-58-04", # HOLDEN ASTRA RS-V BK 2017
"d2d8152227f7cb82|2018-07-25--13-40-56", # TOYOTA CAMRY 2018
"07cb8a788c31f645|2018-06-17--18-50-29", # mock
"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
"fbd011384db5e669|2018-07-26--20-51-48", # TOYOTA CAMRY HYBRID 2018
"0607d2516fc2148f|2019-02-13--23-03-16", # CHRYSLER PACIFICA HYBRID 2019
"3e9592a1c78a3d63|2018-02-08--20-28-24", # HONDA PILOT 2017 TOURING
"aa20e335f61ba898|2019-02-05--16-59-04", # BUICK REGAL ESSENCE 2018
"1851183c395ef471|2018-05-31--18-07-21", # HONDA CR-V 2017 EX
"9d5fb4f0baa1b3e1|2019-01-14--17-45-59", # KIA SORENTO GT LINE 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
"362d23d4d5bea2fa|2018-08-10--13-31-40", # TOYOTA HIGHLANDER HYBRID 2018
"aa20e335f61ba898|2018-12-17--21-10-37", # BUICK REGAL ESSENCE 2018
"215cd70e9c349266|2018-11-25--22-22-12", # KIA STINGER GT2 2018
"192a598e34926b1e|2019-04-04--13-27-39", # JEEP GRAND CHEROKEE 2019
"34a84d2b765df688|2018-08-28--12-41-00", # HONDA PILOT 2019 ELITE
"b0c9d2329ad1606b|2019-01-06--10-11-23", # CHRYSLER PACIFICA HYBRID 2017
"31390e3eb6f7c29a|2019-01-23--08-56-00", # KIA OPTIMA SX 2019
"fd10b9a107bb2e49|2018-07-24--16-32-42", # TOYOTA C-HR 2018
"9f7a7e50a51fb9db|2019-01-17--18-34-21", # JEEP GRAND CHEROKEE V6 2018
"aadda448b49c99ad|2018-10-25--17-16-22", # CHEVROLET MALIBU PREMIER 2017
"362d23d4d5bea2fa|2018-09-02--17-03-55", # TOYOTA HIGHLANDER HYBRID 2018
"1582e1dc57175194|2018-08-15--07-46-07", # HONDA ACCORD 2018 LX 1.5T
"fd10b9a107bb2e49|2018-07-24--20-32-08", # TOYOTA C-HR 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
"21aa231dee2a68bd|2018-01-30--04-54-41", # HONDA ODYSSEY 2018 EX-L
"900ad17e536c3dc7|2018-04-12--22-02-36", # HONDA RIDGELINE 2017 BLACK EDITION
"975b26878285314d|2018-12-25--14-42-13", # CHRYSLER PACIFICA HYBRID 2018
"8ae193ceb56a0efe|2018-06-18--20-07-32", # TOYOTA RAV4 HYBRID 2017
"a893a80e5c5f72c8|2019-01-14--20-02-59", # HYUNDAI GENESIS 2018
"49c73650e65ff465|2018-11-19--16-58-04", # HOLDEN ASTRA RS-V BK 2017
"d2d8152227f7cb82|2018-07-25--13-40-56", # TOYOTA CAMRY 2018
"07cb8a788c31f645|2018-06-17--18-50-29", # mock
"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
"fbd011384db5e669|2018-07-26--20-51-48", # TOYOTA CAMRY HYBRID 2018
]
if __name__ == "__main__":
@ -458,96 +462,103 @@ if __name__ == "__main__":
elif "UNLOGGER_PATH" not in os.environ:
continue
for _ in range(3):
shutil.rmtree('/data/params')
manager.gctx = {}
params = Params()
params.manager_start()
if route in passive_routes:
params.put("Passive", "1")
else:
params.put("Passive", "0")
shutil.rmtree('/data/params')
manager.gctx = {}
params = Params()
params.manager_start()
params.put("OpenpilotEnabledToggle", "1")
params.put("CommunityFeaturesToggle", "1")
print("testing ", route, " ", checks['carFingerprint'])
print("Preparing processes")
manager.prepare_managed_process("radard")
manager.prepare_managed_process("controlsd")
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)
if route in passive_routes:
params.put("Passive", "1")
else:
params.put("Passive", "0")
# Start unlogger
print("Start unlogger")
if route in non_public_routes:
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)
print("testing ", route, " ", checks['carFingerprint'])
print("Preparing processes")
manager.prepare_managed_process("radard")
manager.prepare_managed_process("controlsd")
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)
print("Check sockets")
controls_state_result = wait_for_socket('controlsState', timeout=30)
radarstate_result = wait_for_socket('radarState', timeout=30)
plan_result = wait_for_socket('plan', timeout=30)
# Start unlogger
print("Start unlogger")
if route in non_public_routes:
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
path_plan_result = wait_for_socket('pathPlan', timeout=30)
else:
path_plan_result = True
print("Check sockets")
controls_state_result = wait_for_socket('controlsState', timeout=30)
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")
running = manager.get_running()
controlsd_running = running['controlsd'].is_alive()
radard_running = running['radard'].is_alive()
plannerd_running = running['plannerd'].is_alive()
radarstate_result = wait_for_socket('radarState', timeout=30)
plan_result = wait_for_socket('plan', timeout=30)
manager.kill_managed_process("controlsd")
manager.kill_managed_process("radard")
manager.kill_managed_process("plannerd")
os.killpg(os.getpgid(unlogger.pid), signal.SIGTERM)
if route not in passive_routes: # TODO The passive routes have very flaky models
path_plan_result = wait_for_socket('pathPlan', timeout=30)
else:
path_plan_result = True
sockets_ok = all([
controls_state_result, radarstate_result, plan_result, path_plan_result, carstate_result,
controlsd_running, radard_running, plannerd_running
])
params_ok = True
failures = []
carstate_result = wait_for_socket('carState', timeout=30)
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')
print("Check if everything is running")
running = manager.get_running()
controlsd_running = running['controlsd'].is_alive()
radard_running = running['radard'].is_alive()
plannerd_running = running['plannerd'].is_alive()
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:
params_ok = False
manager.kill_managed_process("controlsd")
manager.kill_managed_process("radard")
manager.kill_managed_process("plannerd")
os.killpg(os.getpgid(unlogger.pid), signal.SIGTERM)
if sockets_ok and params_ok:
print("Success")
results[route] = True, failures
break
else:
print("Failure")
results[route] = False, failures
sockets_ok = all([
controls_state_result, radarstate_result, plan_result, path_plan_result, carstate_result,
controlsd_running, radard_running, plannerd_running
])
params_ok = True
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:
print(results[route])

52
selfdrive/test/test_openpilot.py
View File

@ -149,11 +149,11 @@ def test_athena():
assert False, f"Athena did not start within {timeout} seconds"
time.sleep(0.5)
def athena_post(payload, max_retries=5):
def athena_post(payload, max_retries=5, wait=5):
tries = 0
while 1:
try:
return requests.post(
resp = requests.post(
"https://athena.comma.ai/" + params.get("DongleId", encoding="utf-8"),
headers={
"Authorization": "JWT " + os.getenv("COMMA_JWT"),
@ -162,29 +162,26 @@ def test_athena():
data=json.dumps(payload),
timeout=30
)
resp_json = resp.json()
if resp_json.get('error'):
raise Exception(resp_json['error'])
return resp_json
except Exception as e:
print(e)
time.sleep(5.0)
time.sleep(wait)
tries += 1
if tries == max_retries:
raise
else:
print(f'athena_post failed {e}. retrying...')
def expect_athena_registers(timeout=60):
now = time.time()
while 1:
resp = athena_post({
"method": "echo",
"params": ["hello"],
"id": 0,
"jsonrpc": "2.0"
})
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)
def expect_athena_registers():
resp = athena_post({
"method": "echo",
"params": ["hello"],
"id": 0,
"jsonrpc": "2.0"
}, max_retries=12, wait=5)
assert resp.get('result') == "hello", f'Athena failed to register ({resp})'
try:
athenad_pid = expect_athena_starts()
@ -204,9 +201,8 @@ def test_athena():
"id": 0,
"jsonrpc": "2.0"
})
resp_json = resp.json()
assert resp_json.get('result'), resp_json
assert 'sim_id' in resp_json['result'], resp_json['result']
assert resp.get('result'), resp
assert 'sim_id' in resp['result'], resp['result']
print("ATHENA: takeSnapshot")
resp = athena_post({
@ -214,9 +210,8 @@ def test_athena():
"id": 0,
"jsonrpc": "2.0"
})
resp_json = resp.json()
assert resp_json.get('result'), resp_json
assert resp_json['result']['jpegBack'], resp_json['result']
assert resp.get('result'), resp
assert resp['result']['jpegBack'], resp['result']
@with_processes(["thermald"])
def test_athena_thermal():
@ -227,9 +222,8 @@ def test_athena():
"id": 0,
"jsonrpc": "2.0"
})
resp_json = resp.json()
assert resp_json.get('result'), resp_json
assert resp_json['result']['thermal'], resp_json['result']
assert resp.get('result'), resp
assert resp['result']['thermal'], resp['result']
test_athena_thermal()
finally:
try:

59
selfdrive/thermald.py
View File

@ -6,6 +6,7 @@ import datetime
import psutil
from smbus2 import SMBus
from cereal import log
from common.android import ANDROID
from common.basedir import BASEDIR
from common.params import Params
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
import cereal.messaging as messaging
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
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)
def read_tz(x, clip=True):
with open("/sys/devices/virtual/thermal/thermal_zone%d/temp" % x) as f:
ret = int(f.read())
if clip:
ret = max(0, ret)
try:
with open("/sys/devices/virtual/thermal/thermal_zone%d/temp" % x) as f:
ret = int(f.read())
if clip:
ret = max(0, ret)
except FileNotFoundError:
return 0
return ret
@ -156,7 +160,7 @@ def thermald_thread():
should_start_prev = False
is_uno = (read_tz(29, clip=False) < -1000)
if is_uno:
if is_uno or not ANDROID:
handle_fan = handle_fan_uno
else:
setup_eon_fan()
@ -178,20 +182,28 @@ def thermald_thread():
if health is not None:
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.cpuPerc = int(round(psutil.cpu_percent()))
with open("/sys/class/power_supply/battery/capacity") as f:
msg.thermal.batteryPercent = int(f.read())
with open("/sys/class/power_supply/battery/status") as f:
msg.thermal.batteryStatus = f.read().strip()
with open("/sys/class/power_supply/battery/current_now") as f:
msg.thermal.batteryCurrent = int(f.read())
with open("/sys/class/power_supply/battery/voltage_now") as f:
msg.thermal.batteryVoltage = int(f.read())
with open("/sys/class/power_supply/usb/present") as f:
msg.thermal.usbOnline = bool(int(f.read()))
try:
with open("/sys/class/power_supply/battery/capacity") as f:
msg.thermal.batteryPercent = int(f.read())
with open("/sys/class/power_supply/battery/status") as f:
msg.thermal.batteryStatus = f.read().strip()
with open("/sys/class/power_supply/battery/current_now") as f:
msg.thermal.batteryCurrent = int(f.read())
with open("/sys/class/power_supply/battery/voltage_now") as f:
msg.thermal.batteryVoltage = 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)
@ -268,8 +280,9 @@ def thermald_thread():
do_uninstall = params.get("DoUninstall") == b"1"
accepted_terms = params.get("HasAcceptedTerms") == terms_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
@ -306,12 +319,18 @@ def thermald_thread():
params.delete("Offroad_TemperatureTooHigh")
if should_start:
if not should_start_prev:
params.delete("IsOffroad")
off_ts = None
if started_ts is None:
started_ts = sec_since_boot()
started_seen = True
os.system('echo performance > /sys/class/devfreq/soc:qcom,cpubw/governor')
else:
if should_start_prev or (count == 0):
params.put("IsOffroad", "1")
started_ts = None
if off_ts is None:
off_ts = sec_since_boot()

16
selfdrive/ui/SConscript
View File

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

100
selfdrive/ui/linux.cc 100644
View File

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

1050
selfdrive/ui/paint.cc 100644
View File

File diff suppressed because it is too large Load Diff

85
selfdrive/ui/sound.cc 100644
View File

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

17
selfdrive/ui/sound.hpp 100644
View File

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

File diff suppressed because it is too large Load Diff

253
selfdrive/ui/ui.hpp 100644
View File

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

372
selfdrive/updated.py
View File

@ -1,57 +1,361 @@
#!/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 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 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"]
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()
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:
time_wrong = datetime.datetime.now().year < 2019
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
try:
r = subprocess.check_output(NICE_LOW_PRIORITY + ["git", "fetch"], stderr=subprocess.STDOUT).decode('utf8')
except subprocess.CalledProcessError as e:
cloudlog.event("git fetch failed",
cmd=e.cmd,
output=e.output,
returncode=e.returncode)
time.sleep(60)
continue
cloudlog.info("git fetch success: %s", r)
# Wait until we have a valid datetime to initialize the overlay
if not (ping_failed or time_wrong):
try:
# If the git directory has modifcations after we created the overlay
# we need to recreate the overlay
if overlay_init_done:
overlay_init_fn = os.path.join(BASEDIR, ".overlay_init")
git_dir_path = os.path.join(BASEDIR, ".git")
new_files = run(["find", git_dir_path, "-newer", overlay_init_fn])
# Write update available param
try:
cur_hash = subprocess.check_output(["git", "rev-parse", "HEAD"]).rstrip()
upstream_hash = subprocess.check_output(["git", "rev-parse", "@{u}"]).rstrip()
params.put("UpdateAvailable", str(int(cur_hash != upstream_hash)))
except:
params.put("UpdateAvailable", "0")
if len(new_files.splitlines()):
cloudlog.info(".git directory changed, recreating overlay")
overlay_init_done = False
# Write latest release notes to param
try:
r = subprocess.check_output(["git", "--no-pager", "show", "@{u}:RELEASES.md"])
r = r[:r.find(b'\n\n')] # Slice latest release notes
params.put("ReleaseNotes", r + b"\n")
except:
params.put("ReleaseNotes", "")
if not overlay_init_done:
init_ovfs()
overlay_init_done = True
t = datetime.datetime.now().isoformat()
params.put("LastUpdateTime", t.encode('utf8'))
if params.get("IsOffroad") == b"1":
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__":
main()

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