diff --git a/.github/ISSUE_TEMPLATE/bug_report.md b/.github/ISSUE_TEMPLATE/bug_report.md new file mode 100644 index 0000000000..433b370d93 --- /dev/null +++ b/.github/ISSUE_TEMPLATE/bug_report.md @@ -0,0 +1,25 @@ +--- +name: Bug report +about: Create a report to help us improve openpilot +title: '' +labels: 'bug' +assignees: '' + +--- + +**Describe the bug** +A clear and concise description of what the bug is. + +**How to reproduce or log data** +Steps to reproduce the behavior, or a explorer/cabana link to the exact drive and timestamp of when the bug occurred. + +**Expected behavior** +A clear and concise description of what you expected to happen. + +** Device/Version information (please complete the following information):** + - Device: [e.g. EON/EON Gold] + - Version: [e.g. 0.6.4], or commit hash when on devel + - Car make/model [e.g. Toyota Prius 2016] + +**Additional context** +Add any other context about the problem here. diff --git a/.github/pull_request_template.md b/.github/pull_request_template.md new file mode 100644 index 0000000000..72e1845d7e --- /dev/null +++ b/.github/pull_request_template.md @@ -0,0 +1,21 @@ +Choose one of the templates below: + +# Fingerprint +This pull requests adds a fingerprint for . + +This is an explorer link to a drive with the stock system enabled: ... + +# Car support +This pull requests adds support for . + +This is an explorer link to a drive with the stock system enabled: ... +This is an explorer link to a drive with openpilot system enabled: ... + +# Feature +This pull requests adds feature X + +## Description +Explain what the feature does + +## Testing +Explain how the feature was tested. Either by the added unit tests, or what tests were performed while driving. diff --git a/.gitignore b/.gitignore index 44294dde72..5877ee1169 100644 --- a/.gitignore +++ b/.gitignore @@ -4,6 +4,7 @@ venv/ .ipynb_checkpoints .idea .sconsign.dblite +.vscode model2.png a.out @@ -30,7 +31,7 @@ selfdrive/logcatd/logcatd selfdrive/mapd/default_speeds_by_region.json selfdrive/proclogd/proclogd selfdrive/ui/ui -selfdrive/test/tests/plant/out +selfdrive/test/longitudinal_maneuvers/out selfdrive/visiond/visiond selfdrive/loggerd/loggerd selfdrive/sensord/gpsd diff --git a/Dockerfile.openpilot b/Dockerfile.openpilot index 6be64c1311..c49bd576b1 100644 --- a/Dockerfile.openpilot +++ b/Dockerfile.openpilot @@ -6,43 +6,66 @@ RUN apt-get update && apt-get install -y \ build-essential \ bzip2 \ clang \ + cmake \ + curl \ + ffmpeg \ git \ libarchive-dev \ - libavcodec-dev \ - libavdevice-dev \ - libavfilter-dev \ - libavresample-dev \ - libavutil-dev \ + libbz2-dev \ + libcurl4-openssl-dev \ + libeigen3-dev \ libffi-dev \ + libglew-dev \ libglib2.0-0 \ + liblzma-dev \ + libmysqlclient-dev \ + libomp-dev \ + libopencv-dev \ libssl-dev \ - libswscale-dev \ libtool \ libusb-1.0-0 \ libzmq5-dev \ + locales \ ocl-icd-libopencl1 \ ocl-icd-opencl-dev \ opencl-headers \ - pkg-config \ + python-dev \ python-pip \ + screen \ + sudo \ + vim \ wget -COPY phonelibs/install_capnp.sh /tmp/install_capnp.sh -RUN /tmp/install_capnp.sh -RUN pip install --upgrade pip==18.0 +RUN sed -i -e 's/# en_US.UTF-8 UTF-8/en_US.UTF-8 UTF-8/' /etc/locale.gen && locale-gen +ENV LANG en_US.UTF-8 +ENV LANGUAGE en_US:en +ENV LC_ALL en_US.UTF-8 + +RUN curl -L https://github.com/pyenv/pyenv-installer/raw/master/bin/pyenv-installer | bash + +ENV PATH="/root/.pyenv/bin:/root/.pyenv/shims:${PATH}" +RUN pyenv install 3.7.3 +RUN pyenv global 3.7.3 +RUN pyenv rehash + RUN pip install pipenv==2018.11.26 COPY Pipfile /tmp/ COPY Pipfile.lock /tmp/ -RUN cd /tmp && pipenv install --deploy --system -ENV PYTHONPATH /tmp/openpilot:$PYTHONPATH +RUN python --version +RUN cd /tmp && pipenv install --system --deploy + +# Install subset of dev dependencies needed for CI +RUN pip install matplotlib==3.1.1 dictdiffer==0.8.0 fastcluster==1.1.25 aenum==2.2.1 scipy==1.3.1 lru-dict==1.1.6 tenacity==5.1.1 azure-common==1.1.23 azure-nspkg==3.0.2 azure-storage-blob==2.1.0 azure-storage-common==2.1.0 azure-storage-nspkg==3.1.0 pycurl==7.43.0.3 + +COPY phonelibs/install_capnp.sh /tmp/install_capnp.sh +RUN /tmp/install_capnp.sh -RUN git clone --branch v0.6.2 https://github.com/commaai/openpilot-tools.git /tmp/openpilot/tools -RUN pip install -r /tmp/openpilot/tools/requirements.txt -RUN pip install fastcluster==1.1.20 scipy==0.19.1 dictdiffer==0.8.0 azure-batch==4.1.3 azure-common==1.1.16 azure-nspkg==3.0.0 azure-storage-blob==1.3.1 azure-storage-common==1.3.0 azure-storage-nspkg==3.0.0 +RUN git clone --branch v0.6.5 https://github.com/commaai/openpilot-tools.git /tmp/openpilot/tools +ENV PYTHONPATH /tmp/openpilot:${PYTHONPATH} COPY ./.pylintrc /tmp/openpilot/.pylintrc COPY ./common /tmp/openpilot/common COPY ./cereal /tmp/openpilot/cereal diff --git a/Pipfile b/Pipfile index 95b95b6bd5..c68956060a 100644 --- a/Pipfile +++ b/Pipfile @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:9c487f33783e0fc4a6aee30c18c5e5b5391dca8cf360c531a69d46baa5f5a922 -size 2668 +oid sha256:8ed482abc0c75c5606769e049088894156f68864dd2b85fc769311de781e3998 +size 2507 diff --git a/Pipfile.lock b/Pipfile.lock index ea1a76eb6a..e56ec91c98 100644 --- a/Pipfile.lock +++ b/Pipfile.lock @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:795c35481c2671b9d4555fc684eb9a2b91e5cfdbc66be7a5650a7fef92b0d736 -size 157287 +oid sha256:a9dfee10cd1c1bacb443032aecaa4e785ad28b5f407f1c2dc42c0d54f7a42b6e +size 150547 diff --git a/README.md b/README.md index 16562ca0fb..b3ff4bb29b 100644 --- a/README.md +++ b/README.md @@ -3,7 +3,7 @@ Welcome to openpilot ====== -[openpilot](http://github.com/commaai/openpilot) is an open source driving agent. Currently, it performs the functions of Adaptive Cruise Control (ACC) and Lane Keeping Assist System (LKAS) for selected Honda, Toyota, Acura, Lexus, Chevrolet, Hyundai, Kia. It's about on par with Tesla Autopilot and GM Super Cruise, and better than [all other manufacturers](http://www.thedrive.com/tech/5707/the-war-for-autonomous-driving-part-iii-us-vs-germany-vs-japan). +[openpilot](http://github.com/commaai/openpilot) is an open source driver assistance system. Currently, it performs the functions of Adaptive Cruise Control (ACC) and Lane Keeping Assist System (LKAS) for selected Honda, Toyota, Acura, Lexus, Chevrolet, Hyundai, Kia. It's about on par with Tesla Autopilot and GM Super Cruise, and better than [all other manufacturers](http://www.thedrive.com/tech/5707/the-war-for-autonomous-driving-part-iii-us-vs-germany-vs-japan). The openpilot codebase has been written to be concise and to enable rapid prototyping. We look forward to your contributions - improving real vehicle automation has never been easier. @@ -93,6 +93,7 @@ Supported Cars | Kia | Optima 2019 | SCC + LKAS | Yes | Stock | 0mph | 0mph | Custom6| | Kia | Sorento 2018 | All | Yes | Stock | 0mph | 0mph | Custom6| | Kia | Stinger 2018 | SCC + LKAS | Yes | Stock | 0mph | 0mph | Custom6| +| Lexus | CT Hybrid 2017-18 | All | Yes | Yes2| 0mph | 0mph | Toyota | | Lexus | ES Hybrid 2019 | All | Yes | Yes | 0mph | 0mph | Toyota | | Lexus | RX Hybrid 2016-19 | All | Yes | Yes2| 0mph | 0mph | Toyota | | Lexus | IS 2017-2019 | All | Yes | Stock | 22mph | 0mph | Toyota | @@ -144,7 +145,7 @@ In Progress Cars ------ - All TSS-P Toyota with Steering Assist and LSS-P Lexus with Steering Assist or Lane Keep Assist. - All Hyundai with SmartSense. -- All Kia with SCC and LKAS. +- All Kia, Genesis with SCC and LKAS. - All Chrysler, Jeep, Fiat with Adaptive Cruise Control and LaneSense. - All Subaru with EyeSight. diff --git a/RELEASES.md b/RELEASES.md index 14ddf78b6b..2f6f6729a0 100644 --- a/RELEASES.md +++ b/RELEASES.md @@ -1,3 +1,17 @@ +Version 0.6.5 (2019-10-07) +======================== + * NEOS update: upgrade to Python3 and new installer! + * comma Harness support! + * New driving model: lateral control has lower reliance on lanelines + * New driver monitoring model: more accurate face and eye detection + * Redesign offroad screen to display updates and alerts + * Increase maximum allowed acceleration + * Prevent car 12V battery drain by cutting off EON charge after 3 days of no drive + * Lexus CT Hybrid support thanks to thomaspich! + * Louder chime for critical alerts + * Add toggle to switch to dashcam mode + * Fix "invalid vehicle params" error on DSU-less Toyota + Version 0.6.4 (2019-09-08) ======================== * Forward stock AEB for Honda Nidec diff --git a/apk/ai.comma.plus.frame.apk b/apk/ai.comma.plus.frame.apk index 6eb9f17013..f0dc46f2bb 100644 --- a/apk/ai.comma.plus.frame.apk +++ b/apk/ai.comma.plus.frame.apk @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:2b53c6b8927e634d09ece8c780cf8f3d4201a6ea1c8327396a46b663da0e94a6 -size 2604116 +oid sha256:1d3e4d7e9a84cb5a507137f1d57046649ffc000f2ff0bbd047438846f07d04d5 +size 2616780 diff --git a/apk/ai.comma.plus.offroad.apk b/apk/ai.comma.plus.offroad.apk index aa81486c99..9f212ac8ce 100644 --- a/apk/ai.comma.plus.offroad.apk +++ b/apk/ai.comma.plus.offroad.apk @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:e1d77c74a7e8122ee0c0595f55b5f23b6365b454f6a369fa77ca237c3ea4b0d8 -size 17715672 +oid sha256:8cf70342d6d92801517e068eee81f10df6b52de2222efd5df0f17b3e600e98b7 +size 17786424 diff --git a/common/api/__init__.py b/common/api/__init__.py index 8eb9817399..b27520738e 100644 --- a/common/api/__init__.py +++ b/common/api/__init__.py @@ -4,7 +4,7 @@ from datetime import datetime, timedelta from selfdrive.version import version -class Api(object): +class Api(): def __init__(self, dongle_id): self.dongle_id = dongle_id with open('/persist/comma/id_rsa') as f: @@ -27,7 +27,7 @@ class Api(object): 'iat': now, 'exp': now + timedelta(hours=1) } - return jwt.encode(payload, self.private_key, algorithm='RS256') + return jwt.encode(payload, self.private_key, algorithm='RS256').decode('utf8') def api_get(endpoint, method='GET', timeout=None, access_token=None, **params): backend = "https://api.commadotai.com/" diff --git a/common/cython_hacks.py b/common/cython_hacks.py new file mode 100644 index 0000000000..d0e154746d --- /dev/null +++ b/common/cython_hacks.py @@ -0,0 +1,23 @@ +import os +import sysconfig +from Cython.Distutils import build_ext + +def get_ext_filename_without_platform_suffix(filename): + name, ext = os.path.splitext(filename) + ext_suffix = sysconfig.get_config_var('EXT_SUFFIX') + + if ext_suffix == ext: + return filename + + ext_suffix = ext_suffix.replace(ext, '') + idx = name.find(ext_suffix) + + if idx == -1: + return filename + else: + return name[:idx] + ext + +class BuildExtWithoutPlatformSuffix(build_ext): + def get_ext_filename(self, ext_name): + filename = super().get_ext_filename(ext_name) + return get_ext_filename_without_platform_suffix(filename) diff --git a/common/dbc.py b/common/dbc.py index 693b2985a4..e65ee38be8 100755 --- a/common/dbc.py +++ b/common/dbc.py @@ -17,10 +17,10 @@ DBCSignal = namedtuple( "factor", "offset", "tmin", "tmax", "units"]) -class dbc(object): +class dbc(): def __init__(self, fn): self.name, _ = os.path.splitext(os.path.basename(fn)) - with open(fn) as f: + with open(fn, encoding="ascii") as f: self.txt = f.readlines() self._warned_addresses = set() @@ -41,7 +41,7 @@ class dbc(object): self.def_vals = defaultdict(list) # lookup to bit reverse each byte - self.bits_index = [(i & ~0b111) + ((-i-1) & 0b111) for i in xrange(64)] + self.bits_index = [(i & ~0b111) + ((-i-1) & 0b111) for i in range(64)] for l in self.txt: l = l.strip() @@ -213,7 +213,7 @@ class dbc(object): if debug: print(name) - st = x[2].ljust(8, '\x00') + st = x[2].ljust(8, b'\x00') le, be = None, None for s in msg[1]: diff --git a/common/ffi_wrapper.py b/common/ffi_wrapper.py index f41115d1ce..2c169881ce 100644 --- a/common/ffi_wrapper.py +++ b/common/ffi_wrapper.py @@ -9,7 +9,7 @@ def ffi_wrap(name, c_code, c_header, tmpdir="/tmp/ccache", cflags="", libraries= if libraries is None: libraries = [] - cache = name + "_" + hashlib.sha1(c_code).hexdigest() + cache = name + "_" + hashlib.sha1(c_code.encode('utf-8')).hexdigest() try: os.mkdir(tmpdir) except OSError: diff --git a/common/file_helpers.py b/common/file_helpers.py index beeee89088..40c89fab0e 100644 --- a/common/file_helpers.py +++ b/common/file_helpers.py @@ -32,7 +32,7 @@ def get_tmpdir_on_same_filesystem(path): return "/{}/runner/tmp".format(parts[1]) return "/tmp" -class AutoMoveTempdir(object): +class AutoMoveTempdir(): def __init__(self, target_path, temp_dir=None): self._target_path = target_path self._path = tempfile.mkdtemp(dir=temp_dir) @@ -52,7 +52,7 @@ class AutoMoveTempdir(object): else: shutil.rmtree(self._path) -class NamedTemporaryDir(object): +class NamedTemporaryDir(): def __init__(self, temp_dir=None): self._path = tempfile.mkdtemp(dir=temp_dir) diff --git a/common/kalman/Makefile b/common/kalman/Makefile index 88b032af1b..df748197f2 100644 --- a/common/kalman/Makefile +++ b/common/kalman/Makefile @@ -1,7 +1,7 @@ all: simple_kalman_impl.so simple_kalman_impl.so: simple_kalman_impl.pyx simple_kalman_impl.pxd simple_kalman_setup.py - python2 simple_kalman_setup.py build_ext --inplace + python3 simple_kalman_setup.py build_ext --inplace rm -rf build rm simple_kalman_impl.c diff --git a/common/kalman/simple_kalman.py b/common/kalman/simple_kalman.py index 97b6d792dc..0b35555b61 100644 --- a/common/kalman/simple_kalman.py +++ b/common/kalman/simple_kalman.py @@ -5,6 +5,6 @@ import subprocess kalman_dir = os.path.dirname(os.path.abspath(__file__)) subprocess.check_call(["make", "simple_kalman_impl.so"], cwd=kalman_dir) -from simple_kalman_impl import KF1D as KF1D +from .simple_kalman_impl import KF1D as KF1D # Silence pyflakes assert KF1D diff --git a/common/kalman/simple_kalman_impl.pyx b/common/kalman/simple_kalman_impl.pyx index 43acf7e2a0..e65efd577e 100644 --- a/common/kalman/simple_kalman_impl.pyx +++ b/common/kalman/simple_kalman_impl.pyx @@ -1,3 +1,4 @@ +# cython: language_level=3 cdef class KF1D: def __init__(self, x0, A, C, K): @@ -32,4 +33,4 @@ cdef class KF1D: @x.setter def x(self, x): self.x0_0 = x[0][0] - self.x1_0 = x[1][0] \ No newline at end of file + self.x1_0 = x[1][0] diff --git a/common/kalman/simple_kalman_setup.py b/common/kalman/simple_kalman_setup.py index bccd9f888f..b7cad0ee2a 100644 --- a/common/kalman/simple_kalman_setup.py +++ b/common/kalman/simple_kalman_setup.py @@ -1,5 +1,9 @@ -from distutils.core import setup, Extension +from distutils.core import Extension, setup # pylint: disable=import-error,no-name-in-module + from Cython.Build import cythonize +from common.cython_hacks import BuildExtWithoutPlatformSuffix + setup(name='Simple Kalman Implementation', - ext_modules=cythonize(Extension("simple_kalman_impl", ["simple_kalman_impl.pyx"]))) \ No newline at end of file + cmdclass={'build_ext': BuildExtWithoutPlatformSuffix}, + ext_modules=cythonize(Extension("simple_kalman_impl", ["simple_kalman_impl.pyx"]))) diff --git a/common/numpy_fast.py b/common/numpy_fast.py index eb706a908f..a5d5ad3f50 100644 --- a/common/numpy_fast.py +++ b/common/numpy_fast.py @@ -12,7 +12,10 @@ def interp(x, xp, fp): hi += 1 low = hi - 1 return fp[-1] if hi == N and xv > xp[low] else ( - fp[0] if hi == 0 else + fp[0] if hi == 0 else (xv - xp[low]) * (fp[hi] - fp[low]) / (xp[hi] - xp[low]) + fp[low]) return [get_interp(v) for v in x] if hasattr( x, '__iter__') else get_interp(x) + +def mean(x): + return sum(x) / len(x) diff --git a/common/params.py b/common/params.py index 9af435361f..3c5a63786c 100755 --- a/common/params.py +++ b/common/params.py @@ -1,4 +1,4 @@ -#!/usr/bin/env python +#!/usr/bin/env python3 """ROS has a parameter server, we have files. The parameter store is a persistent key value store, implemented as a directory with a writer lock. @@ -59,29 +59,38 @@ keys = { "ControlsParams": [TxType.PERSISTENT], "DoUninstall": [TxType.CLEAR_ON_MANAGER_START], "DongleId": [TxType.PERSISTENT], - "GithubSshKeys": [TxType.PERSISTENT], "GitBranch": [TxType.PERSISTENT], "GitCommit": [TxType.PERSISTENT], "GitRemote": [TxType.PERSISTENT], + "GithubSshKeys": [TxType.PERSISTENT], "HasAcceptedTerms": [TxType.PERSISTENT], + "HasCompletedSetup": [TxType.PERSISTENT], "IsGeofenceEnabled": [TxType.PERSISTENT], "IsMetric": [TxType.PERSISTENT], "IsRHD": [TxType.PERSISTENT], "IsUpdateAvailable": [TxType.PERSISTENT], "IsUploadRawEnabled": [TxType.PERSISTENT], "IsUploadVideoOverCellularEnabled": [TxType.PERSISTENT], + "LastUpdateTime": [TxType.PERSISTENT], "LimitSetSpeed": [TxType.PERSISTENT], "LimitSetSpeedNeural": [TxType.PERSISTENT], "LiveParameters": [TxType.PERSISTENT], "LongitudinalControl": [TxType.PERSISTENT], + "OpenpilotEnabledToggle": [TxType.PERSISTENT], "Passive": [TxType.PERSISTENT], "RecordFront": [TxType.PERSISTENT], + "ReleaseNotes": [TxType.PERSISTENT], "ShouldDoUpdate": [TxType.CLEAR_ON_MANAGER_START], "SpeedLimitOffset": [TxType.PERSISTENT], "SubscriberInfo": [TxType.PERSISTENT], "TermsVersion": [TxType.PERSISTENT], "TrainingVersion": [TxType.PERSISTENT], + "UpdateAvailable": [TxType.CLEAR_ON_MANAGER_START], "Version": [TxType.PERSISTENT], + "Offroad_ChargeDisabled": [TxType.CLEAR_ON_MANAGER_START, TxType.CLEAR_ON_PANDA_DISCONNECT], + "Offroad_TemperatureTooHigh": [TxType.CLEAR_ON_MANAGER_START], + "Offroad_ConnectivityNeededPrompt": [TxType.CLEAR_ON_MANAGER_START], + "Offroad_ConnectivityNeeded": [TxType.CLEAR_ON_MANAGER_START], } @@ -93,7 +102,7 @@ def fsync_dir(path): os.close(fd) -class FileLock(object): +class FileLock(): def __init__(self, path, create): self._path = path self._create = create @@ -109,7 +118,7 @@ class FileLock(object): self._fd = None -class DBAccessor(object): +class DBAccessor(): def __init__(self, path): self._path = path self._vals = None @@ -279,6 +288,9 @@ def read_db(params_path, key): return None def write_db(params_path, key, value): + if isinstance(value, str): + value = value.encode('utf8') + prev_umask = os.umask(0) lock = FileLock(params_path+"/.lock", True) lock.acquire() @@ -297,7 +309,7 @@ def write_db(params_path, key, value): os.umask(prev_umask) lock.release() -class Params(object): +class Params(): def __init__(self, db='/data/params'): self.db = db @@ -328,7 +340,7 @@ class Params(object): with self.transaction(write=True) as txn: txn.delete(key) - def get(self, key, block=False): + def get(self, key, block=False, encoding=None): if key not in keys: raise UnknownKeyName(key) @@ -338,9 +350,21 @@ class Params(object): break # is polling really the best we can do? time.sleep(0.05) + + if ret is not None and encoding is not None: + ret = ret.decode(encoding) + return ret def put(self, key, dat): + """ + Warning: This function blocks until the param is written to disk! + In very rare cases this can take over a second, and your code will hang. + + Use the put_nonblocking helper function in time sensitive code, but + in general try to avoid writing params as much as possible. + """ + if key not in keys: raise UnknownKeyName(key) diff --git a/common/profiler.py b/common/profiler.py index 7f9b1a41ff..eab091c7fc 100644 --- a/common/profiler.py +++ b/common/profiler.py @@ -1,6 +1,6 @@ import time -class Profiler(object): +class Profiler(): def __init__(self, enabled=False): self.enabled = enabled self.cp = {} diff --git a/common/realtime.py b/common/realtime.py index 6fe64d0d72..f8e489612a 100644 --- a/common/realtime.py +++ b/common/realtime.py @@ -19,6 +19,7 @@ assert sec_since_boot DT_CTRL = 0.01 # controlsd DT_PLAN = 0.05 # mpc DT_MDL = 0.05 # model +DT_RDR = 0.05 # radar DT_DMON = 0.1 # driver monitoring DT_TRML = 0.5 # thermald and manager @@ -43,7 +44,7 @@ def set_realtime_priority(level): return subprocess.call(['chrt', '-f', '-p', str(level), str(tid)]) -class Ratekeeper(object): +class Ratekeeper(): def __init__(self, rate, print_delay_threshold=0.): """Rate in Hz for ratekeeping. print_delay_threshold must be nonnegative.""" self._interval = 1. / rate diff --git a/common/spinner.py b/common/spinner.py new file mode 100644 index 0000000000..7da31a9dda --- /dev/null +++ b/common/spinner.py @@ -0,0 +1,28 @@ +import os +import subprocess +from common.basedir import BASEDIR + +class Spinner(): + def __enter__(self): + self.spinner_proc = subprocess.Popen(["./spinner"], + stdin=subprocess.PIPE, + cwd=os.path.join(BASEDIR, "selfdrive", "ui", "spinner"), + close_fds=True) + return self + + def update(self, spinner_text): + self.spinner_proc.stdin.write(spinner_text.encode('utf8') + b"\n") + self.spinner_proc.stdin.flush() + + def __exit__(self, type, value, traceback): + self.spinner_proc.stdin.close() + self.spinner_proc.terminate() + + + +if __name__ == "__main__": + import time + with Spinner() as s: + s.update("Spinner text") + time.sleep(5.0) + diff --git a/common/stat_live.py b/common/stat_live.py index 06b4f993be..f392956efd 100644 --- a/common/stat_live.py +++ b/common/stat_live.py @@ -11,7 +11,7 @@ class RunningStat(): self.n = priors[2] self.M_last = self.M self.S_last = self.S - + else: self.reset() @@ -70,4 +70,4 @@ class RunningStatFilter(): pass # self.filtered_stat.push_data(self.filtered_stat.mean()) -# class SequentialBayesian(): \ No newline at end of file +# class SequentialBayesian(): diff --git a/common/sympy_helpers.py b/common/sympy_helpers.py index f20bdb08a7..78688a84cf 100644 --- a/common/sympy_helpers.py +++ b/common/sympy_helpers.py @@ -1,4 +1,4 @@ -#!/usr/bin/env python +#!/usr/bin/env python3 import sympy as sp import numpy as np diff --git a/common/transformations/camera.py b/common/transformations/camera.py index a5c14bb4bf..97c7c37d1b 100644 --- a/common/transformations/camera.py +++ b/common/transformations/camera.py @@ -125,7 +125,7 @@ def img_from_device(pt_device): #TODO please use generic img transform below def rotate_img(img, eulers, crop=None, intrinsics=eon_intrinsics): - import cv2 + import cv2 # pylint: disable=no-name-in-module, import-error size = img.shape[:2] rot = orient.rot_from_euler(eulers) @@ -138,8 +138,8 @@ def rotate_img(img, eulers, crop=None, intrinsics=eon_intrinsics): warped_quadrangle = np.column_stack((warped_quadrangle_full[:,0]/warped_quadrangle_full[:,2], warped_quadrangle_full[:,1]/warped_quadrangle_full[:,2])).astype(np.float32) if crop: - W_border = (size[1] - crop[0])/2 - H_border = (size[0] - crop[1])/2 + W_border = (size[1] - crop[0])//2 + H_border = (size[0] - crop[1])//2 outside_crop = (((warped_quadrangle[:,0] < W_border) | (warped_quadrangle[:,0] >= size[1] - W_border)) & ((warped_quadrangle[:,1] < H_border) | @@ -183,7 +183,8 @@ def transform_img(base_img, alpha=1.0, beta=0, blur=0): - import cv2 + import cv2 # pylint: disable=no-name-in-module, import-error + cv2.setNumThreads(1) if yuv: base_img = cv2.cvtColor(base_img, cv2.COLOR_YUV2RGB_I420) @@ -240,7 +241,7 @@ def transform_img(base_img, def yuv_crop(frame, output_size, center=None): # output_size in camera coordinates so u,v # center in array coordinates so row, column - import cv2 + import cv2 # pylint: disable=no-name-in-module, import-error rgb = cv2.cvtColor(frame, cv2.COLOR_YUV2RGB_I420) if not center: center = (rgb.shape[0]/2, rgb.shape[1]/2) diff --git a/common/transformations/coordinates.py b/common/transformations/coordinates.py index 568fb9bf2b..864bc4d807 100644 --- a/common/transformations/coordinates.py +++ b/common/transformations/coordinates.py @@ -65,7 +65,7 @@ def ecef2geodetic(ecef, radians=False): geodetic = np.column_stack((lat, lon, h)) return geodetic.reshape(input_shape) -class LocalCoord(object): +class LocalCoord(): """ Allows conversions to local frames. In this case NED. That is: North East Down from the start position in diff --git a/common/transformations/orientation.py b/common/transformations/orientation.py index 33a822ca3f..acbd4a2bf3 100644 --- a/common/transformations/orientation.py +++ b/common/transformations/orientation.py @@ -29,7 +29,7 @@ def euler2quat(eulers): np.sin(gamma / 2) * np.sin(theta / 2) * np.cos(psi / 2) quats = array([q0, q1, q2, q3]).T - for i in xrange(len(quats)): + for i in range(len(quats)): if quats[i,0] < 0: quats[i] = -quats[i] return quats.reshape(output_shape) @@ -99,7 +99,7 @@ def rot2quat(rots): K3[:, 3, 2] = K3[:, 2, 3] K3[:, 3, 3] = (rots[:, 0, 0] + rots[:, 1, 1] + rots[:, 2, 2]) / 3.0 q = np.empty((len(rots), 4)) - for i in xrange(len(rots)): + for i in range(len(rots)): _, eigvecs = linalg.eigh(K3[i].T) eigvecs = eigvecs[:,3:] q[i, 0] = eigvecs[-1] diff --git a/installer/updater/update.json b/installer/updater/update.json index 2715fcaccc..2ff5690fd5 100644 --- a/installer/updater/update.json +++ b/installer/updater/update.json @@ -1,7 +1,7 @@ { - "ota_url": "https://commadist.azureedge.net/neosupdate/ota-signed-4db25072191d24e204a816d73ac9e8c727822a26ed3baf01ecae18167fa2eb11.zip", - "ota_hash": "4db25072191d24e204a816d73ac9e8c727822a26ed3baf01ecae18167fa2eb11", - "recovery_url": "https://commadist.azureedge.net/neosupdate/recovery-31ef14206d3102edf18fb7417ef32ba2d9f37dd2f4443e234c374a70d1bf4662.img", - "recovery_len": 15136044, - "recovery_hash": "31ef14206d3102edf18fb7417ef32ba2d9f37dd2f4443e234c374a70d1bf4662" + "ota_url": "https://commadist.azureedge.net/neosupdate/ota-signed-7a0117425bc4a6673958d265312994e124654a566228f3cec2f0f9bc8120a9ab.zip", + "ota_hash": "7a0117425bc4a6673958d265312994e124654a566228f3cec2f0f9bc8120a9ab", + "recovery_url": "https://commadist.azureedge.net/neosupdate/recovery-3dc234d868c29a3739f6ca3bd47b1c2d3c570d9f478b6849a4fada129ee4af76.img", + "recovery_len": 15848748, + "recovery_hash": "3dc234d868c29a3739f6ca3bd47b1c2d3c570d9f478b6849a4fada129ee4af76" } diff --git a/launch_chffrplus.sh b/launch_chffrplus.sh index 287c72a78e..56c25e6ffe 100755 --- a/launch_chffrplus.sh +++ b/launch_chffrplus.sh @@ -32,6 +32,24 @@ function launch { echo 0-3 > /dev/cpuset/foreground/cpus echo 0-3 > /dev/cpuset/android/cpus + DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" >/dev/null && pwd )" + + # Remove old NEOS update file + if [ -d /data/neoupdate ]; then + rm -rf /data/neoupdate + fi + + # Check for NEOS update + if [ $(< /VERSION) != "12" ]; 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 + + # handle pythonpath ln -s /data/openpilot /data/pythonpath export PYTHONPATH="$PWD" diff --git a/models/driving_model.dlc b/models/driving_model.dlc index 340e2151b6..14d3f0d5ee 100644 --- a/models/driving_model.dlc +++ b/models/driving_model.dlc @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:7696d34bb06568c6a0a152404cff258e9ebd716aac3dc013287733940be58b2e -size 14488833 +oid sha256:cb1eea1b9d0e69da7aa07612bc6a72c36da9dd24b14eaa80191ecb8b9eca1841 +size 16950452 diff --git a/models/monitoring_model.dlc b/models/monitoring_model.dlc index c1880fccc3..6770be5d77 100644 --- a/models/monitoring_model.dlc +++ b/models/monitoring_model.dlc @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:9fdd514ad8a38876468adf9fa106f511150c077223c13eb1cc3ae939bc414564 +oid sha256:4e22c4152adf797e0b2aab048037871f1988ffce9339e618c9bb950f38b4ca99 size 601956 diff --git a/panda/board/obj/panda.bin.signed b/panda/board/obj/panda.bin.signed index a1e01931c6..3a6b76e664 100644 Binary files a/panda/board/obj/panda.bin.signed and b/panda/board/obj/panda.bin.signed differ diff --git a/run_docker_tests.sh b/run_docker_tests.sh index 21549ab089..3c4ef03e43 100755 --- a/run_docker_tests.sh +++ b/run_docker_tests.sh @@ -11,6 +11,6 @@ docker run --rm tmppilot /bin/sh -c 'cd /tmp/openpilot/ && make -C cereal && pyt docker run --rm tmppilot /bin/sh -c 'cd /tmp/openpilot/ && make -C cereal && python -m unittest discover selfdrive/boardd' docker run --rm tmppilot /bin/sh -c 'cd /tmp/openpilot/ && make -C cereal && python -m unittest discover selfdrive/controls' docker run --rm tmppilot /bin/sh -c 'cd /tmp/openpilot/ && python -m unittest discover selfdrive/loggerd' -docker run --rm -v "$(pwd)"/selfdrive/test/tests/plant/out:/tmp/openpilot/selfdrive/test/tests/plant/out tmppilot /bin/sh -c 'cd /tmp/openpilot/selfdrive/test/tests/plant && OPTEST=1 ./test_longitudinal.py' -docker run --rm tmppilot /bin/sh -c 'cd /tmp/openpilot/ && make -C cereal && cd /tmp/openpilot/selfdrive/test/tests/process_replay/ && ./test_processes.py' +docker run --rm -v "$(pwd)"/selfdrive/test/longitudinal_maneuvers/out:/tmp/openpilot/selfdrive/test/longitudinal_maneuvers/out tmppilot /bin/sh -c 'cd /tmp/openpilot/selfdrive/test/longitudinal_maneuvers && OPTEST=1 ./test_longitudinal.py' +docker run --rm tmppilot /bin/sh -c 'cd /tmp/openpilot/ && make -C cereal && cd /tmp/openpilot/selfdrive/test/process_replay/ && ./test_processes.py' docker run --rm tmppilot /bin/sh -c 'mkdir -p /data/params && cd /tmp/openpilot/ && make -C cereal && cd /tmp/openpilot/selfdrive/test/ && ./test_car_models.py' diff --git a/selfdrive/assets/sounds/warning_repeat.wav b/selfdrive/assets/sounds/warning_repeat.wav new file mode 100644 index 0000000000..745b05bda7 --- /dev/null +++ b/selfdrive/assets/sounds/warning_repeat.wav @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:5fbd825c3cd03355782763cdf1905981bccef6707b2a48c92e3adb1fadb4df28 +size 125604 diff --git a/selfdrive/athena/athenad.py b/selfdrive/athena/athenad.py index 4f1059930c..1474b346ba 100755 --- a/selfdrive/athena/athenad.py +++ b/selfdrive/athena/athenad.py @@ -1,6 +1,7 @@ -#!/usr/bin/env python2.7 +#!/usr/bin/env python3.7 import json import os +import io import random import re import select @@ -10,6 +11,7 @@ import time import threading import traceback import zmq +import base64 import requests import six.moves.queue from functools import partial @@ -24,6 +26,7 @@ from common.params import Params from selfdrive.services import service_list from selfdrive.swaglog import cloudlog from selfdrive.version import version, dirty +from functools import reduce ATHENA_HOST = os.getenv('ATHENA_HOST', 'wss://athena.comma.ai') HANDLER_THREADS = os.getenv('HANDLER_THREADS', 4) @@ -41,10 +44,11 @@ def handle_long_poll(ws): threading.Thread(target=ws_send, args=(ws, end_event)) ] + [ threading.Thread(target=jsonrpc_handler, args=(end_event,)) - for x in xrange(HANDLER_THREADS) + for x in range(HANDLER_THREADS) ] - map(lambda thread: thread.start(), threads) + for thread in threads: + thread.start() try: while not end_event.is_set(): time.sleep(0.1) @@ -101,7 +105,7 @@ def startLocalProxy(global_end_event, remote_ws_uri, local_port): raise Exception("Requested local port not whitelisted") params = Params() - dongle_id = params.get("DongleId") + dongle_id = params.get("DongleId").decode('utf8') identity_token = Api(dongle_id).get_token() ws = create_connection(remote_ws_uri, cookie="jwt=" + identity_token, @@ -117,8 +121,8 @@ def startLocalProxy(global_end_event, remote_ws_uri, local_port): threading.Thread(target=ws_proxy_recv, args=(ws, local_sock, ssock, proxy_end_event, global_end_event)), threading.Thread(target=ws_proxy_send, args=(ws, local_sock, csock, proxy_end_event)) ] - - map(lambda thread: thread.start(), threads) + for thread in threads: + thread.start() return {"success": 1} except Exception as e: @@ -135,16 +139,15 @@ def getPublicKey(): @dispatcher.add_method def getSshAuthorizedKeys(): - with open('/system/comma/home/.ssh/authorized_keys', 'r') as f: - return f.read() + return Params().get("GithubSshKeys", encoding='utf8') or '' @dispatcher.add_method def getSimInfo(): - sim_state = subprocess.check_output(['getprop', 'gsm.sim.state']).strip().split(',') - network_type = subprocess.check_output(['getprop', 'gsm.network.type']).strip().split(',') - mcc_mnc = subprocess.check_output(['getprop', 'gsm.sim.operator.numeric']).strip() or None + sim_state = subprocess.check_output(['getprop', 'gsm.sim.state'], encoding='utf8').strip().split(',') # pylint: disable=unexpected-keyword-arg + network_type = subprocess.check_output(['getprop', 'gsm.network.type'], encoding='utf8').strip().split(',') # pylint: disable=unexpected-keyword-arg + mcc_mnc = subprocess.check_output(['getprop', 'gsm.sim.operator.numeric'], encoding='utf8').strip() or None # pylint: disable=unexpected-keyword-arg - sim_id_aidl_out = subprocess.check_output(['service', 'call', 'iphonesubinfo', '11']) + sim_id_aidl_out = subprocess.check_output(['service', 'call', 'iphonesubinfo', '11'], encoding='utf8') # pylint: disable=unexpected-keyword-arg sim_id_aidl_lines = sim_id_aidl_out.split('\n') if len(sim_id_aidl_lines) > 3: sim_id_lines = sim_id_aidl_lines[1:4] @@ -160,6 +163,20 @@ def getSimInfo(): 'sim_state': sim_state } +@dispatcher.add_method +def takeSnapshot(): + from selfdrive.visiond.snapshot.snapshot import snapshot, jpeg_write + ret = snapshot() + if ret is not None: + def b64jpeg(x): + f = io.BytesIO() + jpeg_write(f, x) + return base64.b64encode(f.getvalue()).decode("utf-8") + return {'jpegBack': b64jpeg(ret[0]), + 'jpegFront': b64jpeg(ret[1])} + else: + raise Exception("not available while visiond is started") + def ws_proxy_recv(ws, local_sock, ssock, end_event, global_end_event): while not (end_event.is_set() or global_end_event.is_set()): try: @@ -221,7 +238,7 @@ def backoff(retries): def main(gctx=None): params = Params() - dongle_id = params.get("DongleId") + dongle_id = params.get("DongleId").decode('utf-8') ws_uri = ATHENA_HOST + "/ws/v2/" + dongle_id crash.bind_user(id=dongle_id) diff --git a/selfdrive/boardd/Makefile b/selfdrive/boardd/Makefile index c6b5f21910..67a315d8d9 100644 --- a/selfdrive/boardd/Makefile +++ b/selfdrive/boardd/Makefile @@ -78,7 +78,7 @@ boardd.o: boardd.cc boardd_api_impl.so: libcan_list_to_can_capnp.a boardd_api_impl.pyx boardd_setup.py - python2 boardd_setup.py build_ext --inplace + python3 boardd_setup.py build_ext --inplace rm -rf build rm -f boardd_api_impl.cpp diff --git a/selfdrive/boardd/boardd.cc b/selfdrive/boardd/boardd.cc index 8468238f31..bd60c34087 100644 --- a/selfdrive/boardd/boardd.cc +++ b/selfdrive/boardd/boardd.cc @@ -32,26 +32,9 @@ #define RECV_SIZE (0x1000) #define TIMEOUT 0 -#define SAFETY_NOOUTPUT 0 -#define SAFETY_HONDA 1 -#define SAFETY_TOYOTA 2 -#define SAFETY_GM 3 -#define SAFETY_HONDA_BOSCH 4 -#define SAFETY_FORD 5 -#define SAFETY_CADILLAC 6 -#define SAFETY_HYUNDAI 7 -#define SAFETY_TESLA 8 -#define SAFETY_CHRYSLER 9 -#define SAFETY_SUBARU 10 -#define SAFETY_GM_PASSIVE 11 -#define SAFETY_TOYOTA_IPAS 0x1335 -#define SAFETY_TOYOTA_NOLIMITS 0x1336 -#define SAFETY_ALLOUTPUT 0x1337 -#define SAFETY_ELM327 0xE327 // diagnostic only - namespace { -volatile int do_exit = 0; +volatile sig_atomic_t do_exit = 0; libusb_context *ctx = NULL; libusb_device_handle *dev_handle; @@ -62,6 +45,10 @@ bool fake_send = false; 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 * 24 * 3; // turn off charge after 3 days +uint32_t no_ignition_cnt = 0; +bool connected_once = false; +uint8_t ignition_last = 0; pthread_t safety_setter_thread_handle = -1; pthread_t pigeon_thread_handle = -1; @@ -89,8 +76,8 @@ void *safety_setter_thread(void *s) { pthread_mutex_lock(&usb_lock); - // VIN qury done, stop listening to OBDII - libusb_control_transfer(dev_handle, 0x40, 0xdc, SAFETY_NOOUTPUT, 0, NULL, 0, TIMEOUT); + // VIN query done, stop listening to OBDII + libusb_control_transfer(dev_handle, 0x40, 0xdc, (uint16_t)(cereal::CarParams::SafetyModel::NO_OUTPUT), 0, NULL, 0, TIMEOUT); pthread_mutex_unlock(&usb_lock); @@ -119,48 +106,6 @@ void *safety_setter_thread(void *s) { auto safety_param = car_params.getSafetyParam(); LOGW("setting safety model: %d with param %d", safety_model, safety_param); - int safety_setting = 0; - switch (safety_model) { - case cereal::CarParams::SafetyModel::NO_OUTPUT: - safety_setting = SAFETY_NOOUTPUT; - break; - case cereal::CarParams::SafetyModel::HONDA: - safety_setting = SAFETY_HONDA; - break; - case cereal::CarParams::SafetyModel::TOYOTA: - safety_setting = SAFETY_TOYOTA; - break; - case cereal::CarParams::SafetyModel::ELM327: - safety_setting = SAFETY_ELM327; - break; - case cereal::CarParams::SafetyModel::GM: - safety_setting = SAFETY_GM; - break; - case cereal::CarParams::SafetyModel::GM_PASSIVE: - safety_setting = SAFETY_GM_PASSIVE; - break; - case cereal::CarParams::SafetyModel::HONDA_BOSCH: - safety_setting = SAFETY_HONDA_BOSCH; - break; - case cereal::CarParams::SafetyModel::FORD: - safety_setting = SAFETY_FORD; - break; - case cereal::CarParams::SafetyModel::CADILLAC: - safety_setting = SAFETY_CADILLAC; - break; - case cereal::CarParams::SafetyModel::HYUNDAI: - safety_setting = SAFETY_HYUNDAI; - break; - case cereal::CarParams::SafetyModel::CHRYSLER: - safety_setting = SAFETY_CHRYSLER; - break; - case cereal::CarParams::SafetyModel::SUBARU: - safety_setting = SAFETY_SUBARU; - break; - default: - LOGE("unknown safety model: %d", safety_model); - } - pthread_mutex_lock(&usb_lock); // set in the mutex to avoid race @@ -169,7 +114,7 @@ void *safety_setter_thread(void *s) { // set if long_control is allowed by openpilot. Hardcoded to True for now libusb_control_transfer(dev_handle, 0x40, 0xdf, 1, 0, NULL, 0, TIMEOUT); - libusb_control_transfer(dev_handle, 0x40, 0xdc, safety_setting, safety_param, NULL, 0, TIMEOUT); + libusb_control_transfer(dev_handle, 0x40, 0xdc, (uint16_t)(cereal::CarParams::SafetyModel(safety_model)), safety_param, NULL, 0, TIMEOUT); pthread_mutex_unlock(&usb_lock); @@ -180,6 +125,7 @@ void *safety_setter_thread(void *s) { bool usb_connect() { int err; unsigned char hw_query[1] = {0}; + ignition_last = 0; dev_handle = libusb_open_device_with_vid_pid(ctx, 0xbbaa, 0xddcc); if (dev_handle == NULL) { goto fail; } @@ -197,25 +143,20 @@ bool usb_connect() { // power off ESP libusb_control_transfer(dev_handle, 0xc0, 0xd9, 0, 0, NULL, 0, TIMEOUT); - // power on charging (may trigger a reconnection, should be okay) - #ifndef __x86_64__ - libusb_control_transfer(dev_handle, 0xc0, 0xe6, 1, 0, NULL, 0, TIMEOUT); - #else - LOGW("not enabling charging on x86_64"); - #endif - - // diagnostic only is the default, needed for VIN query - libusb_control_transfer(dev_handle, 0x40, 0xdc, SAFETY_ELM327, 0, NULL, 0, TIMEOUT); - - if (safety_setter_thread_handle == -1) { - err = pthread_create(&safety_setter_thread_handle, NULL, safety_setter_thread, NULL); - assert(err == 0); + // power on charging, only the first time. Panda can also change mode and it causes a brief disconneciton +#ifndef __x86_64__ + if (!connected_once) { + libusb_control_transfer(dev_handle, 0xc0, 0xe6, (uint16_t)(cereal::HealthData::UsbPowerMode::CDP), 0, NULL, 0, TIMEOUT); } +#endif + connected_once = true; libusb_control_transfer(dev_handle, 0xc0, 0xc1, 0, 0, hw_query, 1, TIMEOUT); hw_type = (cereal::HealthData::HwType)(hw_query[0]); - is_pigeon = (hw_type == cereal::HealthData::HwType::GREY_PANDA) || (hw_type == cereal::HealthData::HwType::BLACK_PANDA); + is_pigeon = (hw_type == cereal::HealthData::HwType::GREY_PANDA) || + (hw_type == cereal::HealthData::HwType::BLACK_PANDA) || + (hw_type == cereal::HealthData::HwType::UNO); if (is_pigeon) { LOGW("panda with gps detected"); pigeon_needs_init = true; @@ -304,8 +245,9 @@ void can_recv(void *s) { void can_health(void *s) { int cnt; + int err; - // copied from board/main.c + // copied from panda/board/main.c struct __attribute__((packed)) health { uint32_t voltage; uint32_t current; @@ -315,7 +257,8 @@ void can_health(void *s) { uint8_t started; uint8_t controls_allowed; uint8_t gas_interceptor_detected; - uint8_t car_harness_status_pkt; + uint8_t car_harness_status; + uint8_t usb_power_mode; } health; // recv from board @@ -330,6 +273,42 @@ void can_health(void *s) { pthread_mutex_unlock(&usb_lock); + if (health.started == 0) { + no_ignition_cnt += 1; + } else { + no_ignition_cnt = 0; + } + +#ifndef __x86_64__ + if ((no_ignition_cnt > NO_IGNITION_CNT_MAX) && (health.usb_power_mode == (uint8_t)(cereal::HealthData::UsbPowerMode::CDP))) { + 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); + } +#endif + + // clear VIN, CarParams, and set new safety on car start + if ((health.started != 0) && (ignition_last == 0)) { + + 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)); + + // diagnostic only is the default, needed for VIN query + pthread_mutex_lock(&usb_lock); + libusb_control_transfer(dev_handle, 0x40, 0xdc, (uint16_t)(cereal::CarParams::SafetyModel::ELM327), 0, NULL, 0, TIMEOUT); + pthread_mutex_unlock(&usb_lock); + + if (safety_setter_thread_handle == -1) { + err = pthread_create(&safety_setter_thread_handle, NULL, safety_setter_thread, NULL); + assert(err == 0); + } + } + + ignition_last = health.started; + // create message capnp::MallocMessageBuilder msg; cereal::Event::Builder event = msg.initRoot(); @@ -351,6 +330,7 @@ void can_health(void *s) { healthData.setCanFwdErrs(health.can_fwd_errs); healthData.setGmlanSendErrs(health.gmlan_send_errs); healthData.setHwType(hw_type); + healthData.setUsbPowerMode(cereal::HealthData::UsbPowerMode(health.usb_power_mode)); // send to health auto words = capnp::messageToFlatArray(msg); @@ -481,7 +461,6 @@ void *can_recv_thread(void *crap) { void *can_health_thread(void *crap) { LOGD("start health thread"); - // health = 8011 void *context = zmq_ctx_new(); void *publisher = zmq_socket(context, ZMQ_PUB); @@ -628,7 +607,7 @@ void *pigeon_thread(void *crap) { } if (alen > 0) { if (dat[0] == (char)0x00){ - LOGW("received invalid ublox message, resetting pigeon"); + LOGW("received invalid ublox message, resetting panda GPS"); pigeon_init(); } else { pigeon_publish_raw(publisher, dat, alen); diff --git a/selfdrive/boardd/boardd_api_impl.pyx b/selfdrive/boardd/boardd_api_impl.pyx index eddf1fbf7a..3f50fbaab6 100644 --- a/selfdrive/boardd/boardd_api_impl.pyx +++ b/selfdrive/boardd/boardd_api_impl.pyx @@ -1,4 +1,5 @@ # distutils: language = c++ +# cython: language_level=3 from libcpp.vector cimport vector from libcpp.string cimport string from libcpp cimport bool diff --git a/selfdrive/boardd/boardd_setup.py b/selfdrive/boardd/boardd_setup.py index f614a06161..887a1b7105 100644 --- a/selfdrive/boardd/boardd_setup.py +++ b/selfdrive/boardd/boardd_setup.py @@ -1,25 +1,29 @@ import subprocess -from distutils.core import setup, Extension +from distutils.core import Extension, setup # pylint: disable=import-error,no-name-in-module + from Cython.Build import cythonize +from common.cython_hacks import BuildExtWithoutPlatformSuffix + PHONELIBS = '../../phonelibs' -ARCH = subprocess.check_output(["uname", "-m"]).rstrip() +ARCH = subprocess.check_output(["uname", "-m"], encoding='utf8').rstrip() # pylint: disable=unexpected-keyword-arg ARCH_DIR = 'x64' if ARCH == "x86_64" else 'aarch64' setup(name='Boardd API Implementation', - ext_modules=cythonize( - Extension( - "boardd_api_impl", - libraries=[':libcan_list_to_can_capnp.a', ':libcapnp.a', ':libcapnp.a', ':libkj.a'], - library_dirs=[ - './', - PHONELIBS + '/capnp-cpp/' + ARCH_DIR + '/lib/', - PHONELIBS + '/capnp-c/' + ARCH_DIR + '/lib/' - ], - sources=['boardd_api_impl.pyx'], - language="c++", - extra_compile_args=["-std=c++11"], - ) - ) + cmdclass={'build_ext': BuildExtWithoutPlatformSuffix}, + ext_modules=cythonize( + Extension( + "boardd_api_impl", + libraries=[':libcan_list_to_can_capnp.a', ':libcapnp.a', ':libcapnp.a', ':libkj.a'], + library_dirs=[ + './', + PHONELIBS + '/capnp-cpp/' + ARCH_DIR + '/lib/', + PHONELIBS + '/capnp-c/' + ARCH_DIR + '/lib/' + ], + sources=['boardd_api_impl.pyx'], + language="c++", + extra_compile_args=["-std=c++11"], + ) + ) ) diff --git a/selfdrive/boardd/tests/boardd_old.py b/selfdrive/boardd/tests/boardd_old.py index 5989e62746..3f5ad7cf6c 100755 --- a/selfdrive/boardd/tests/boardd_old.py +++ b/selfdrive/boardd/tests/boardd_old.py @@ -1,4 +1,4 @@ -#!/usr/bin/env python +#!/usr/bin/env python3 # This file is not used by OpenPilot. Only boardd.cc is used. # The python version is slower, but has more options for development. @@ -15,6 +15,9 @@ from common.realtime import Ratekeeper from selfdrive.services import service_list from selfdrive.swaglog import cloudlog from selfdrive.boardd.boardd import can_capnp_to_can_list +from cereal import car + +SafetyModel = car.CarParams.SafetyModel # USB is optional try: @@ -23,13 +26,6 @@ try: except Exception: pass -SAFETY_NOOUTPUT = 0 -SAFETY_HONDA = 1 -SAFETY_TOYOTA = 2 -SAFETY_CHRYSLER = 9 -SAFETY_TOYOTA_NOLIMITS = 0x1336 -SAFETY_ALLOUTPUT = 0x1337 - # *** serialization functions *** def can_list_to_can_capnp(can_msgs, msgtype='can'): dat = messaging.new_message() @@ -41,7 +37,7 @@ def can_list_to_can_capnp(can_msgs, msgtype='can'): cc = dat.can[i] cc.address = can_msg[0] cc.busTime = can_msg[1] - cc.dat = str(can_msg[2]) + cc.dat = bytes(can_msg[2]) cc.src = can_msg[3] return dat @@ -71,17 +67,17 @@ def can_send_many(arr): for addr, _, dat, alt in arr: if addr < 0x800: # only support 11 bit addr snd = struct.pack("II", ((addr << 21) | 1), len(dat) | (alt << 4)) + dat - snd = snd.ljust(0x10, '\x00') + snd = snd.ljust(0x10, b'\x00') snds.append(snd) while 1: try: - handle.bulkWrite(3, ''.join(snds)) + handle.bulkWrite(3, b''.join(snds)) break except (USBErrorIO, USBErrorOverflow): cloudlog.exception("CAN: BAD SEND MANY, RETRYING") def can_recv(): - dat = "" + dat = b"" while 1: try: dat = handle.bulkRead(1, 0x10*256) @@ -102,10 +98,10 @@ def can_init(): if device.getVendorID() == 0xbbaa and device.getProductID() == 0xddcc: handle = device.open() handle.claimInterface(0) - handle.controlWrite(0x40, 0xdc, SAFETY_ALLOUTPUT, 0, b'') + handle.controlWrite(0x40, 0xdc, SafetyModel.allOutput, 0, b'') if handle is None: - cloudlog.warn("CAN NOT FOUND") + cloudlog.warning("CAN NOT FOUND") exit(-1) cloudlog.info("got handle") @@ -113,7 +109,7 @@ def can_init(): def boardd_mock_loop(): can_init() - handle.controlWrite(0x40, 0xdc, SAFETY_ALLOUTPUT, 0, b'') + handle.controlWrite(0x40, 0xdc, SafetyModel.allOutput, 0, b'') logcan = messaging.sub_sock(service_list['can'].port) sendcan = messaging.pub_sock(service_list['sendcan'].port) @@ -124,17 +120,17 @@ def boardd_mock_loop(): snd = [] for s in snds: snd += s - snd = filter(lambda x: x[-1] <= 2, snd) - snd_0 = len(filter(lambda x: x[-1] == 0, snd)) - snd_1 = len(filter(lambda x: x[-1] == 1, snd)) - snd_2 = len(filter(lambda x: x[-1] == 2, snd)) + snd = list(filter(lambda x: x[-1] <= 2, snd)) + snd_0 = len(list(filter(lambda x: x[-1] == 0, snd))) + snd_1 = len(list(filter(lambda x: x[-1] == 1, snd))) + snd_2 = len(list(filter(lambda x: x[-1] == 2, snd))) can_send_many(snd) # recv @ 100hz can_msgs = can_recv() - got_0 = len(filter(lambda x: x[-1] == 0+0x80, can_msgs)) - got_1 = len(filter(lambda x: x[-1] == 1+0x80, can_msgs)) - got_2 = len(filter(lambda x: x[-1] == 2+0x80, can_msgs)) + got_0 = len(list(filter(lambda x: x[-1] == 0+0x80, can_msgs))) + got_1 = len(list(filter(lambda x: x[-1] == 1+0x80, can_msgs))) + got_2 = len(list(filter(lambda x: x[-1] == 2+0x80, can_msgs))) print("sent %3d (%3d/%3d/%3d) got %3d (%3d/%3d/%3d)" % (len(snd), snd_0, snd_1, snd_2, len(can_msgs), got_0, got_1, got_2)) m = can_list_to_can_capnp(can_msgs, msgtype='sendcan') diff --git a/selfdrive/boardd/tests/fuzzer.py b/selfdrive/boardd/tests/fuzzer.py index cb6edf7106..bb58f8b5e0 100755 --- a/selfdrive/boardd/tests/fuzzer.py +++ b/selfdrive/boardd/tests/fuzzer.py @@ -1,4 +1,5 @@ -#!/usr/bin/env python +#!/usr/bin/env python3 +from __future__ import print_function import time import random @@ -9,9 +10,9 @@ if __name__ == "__main__": while 1: c = random.randint(0, 3) if c == 0: - print can_recv() + print(can_recv()) elif c == 1: - print can_health() + print(can_health()) elif c == 2: many = [[0x123, 0, "abcdef", 0]] * random.randint(1, 10) can_send_many(many) diff --git a/selfdrive/boardd/tests/replay_many.py b/selfdrive/boardd/tests/replay_many.py index 710cfee486..548ec6549a 100755 --- a/selfdrive/boardd/tests/replay_many.py +++ b/selfdrive/boardd/tests/replay_many.py @@ -1,4 +1,4 @@ -#!/usr/bin/env python +#!/usr/bin/env python3 import sys import time import signal diff --git a/selfdrive/boardd/tests/test_boardd_api.py b/selfdrive/boardd/tests/test_boardd_api.py index 0e862b0952..f41e1e3571 100644 --- a/selfdrive/boardd/tests/test_boardd_api.py +++ b/selfdrive/boardd/tests/test_boardd_api.py @@ -1,7 +1,7 @@ import random import numpy as np -import boardd_old +import selfdrive.boardd.tests.boardd_old as boardd_old import selfdrive.boardd.boardd as boardd from common.realtime import sec_since_boot @@ -12,7 +12,7 @@ import unittest def generate_random_can_data_list(): can_list = [] cnt = random.randint(1, 64) - for j in xrange(cnt): + for j in range(cnt): can_data = np.random.bytes(random.randint(1, 8)) can_list.append([random.randint(0, 128), random.randint(0, 128), can_data, random.randint(0, 128)]) return can_list, cnt @@ -20,7 +20,7 @@ def generate_random_can_data_list(): class TestBoarddApiMethods(unittest.TestCase): def test_correctness(self): - for i in xrange(1000): + for i in range(1000): can_list, _ = generate_random_can_data_list() # Sendcan @@ -31,9 +31,10 @@ class TestBoarddApiMethods(unittest.TestCase): ev_old = log.Event.from_bytes(m_old) ev = log.Event.from_bytes(m) + self.assertEqual(ev_old.which(), ev.which()) self.assertEqual(len(ev.sendcan), len(ev_old.sendcan)) - for i in xrange(len(ev.sendcan)): + for i in range(len(ev.sendcan)): attrs = ['address', 'busTime', 'dat', 'src'] for attr in attrs: self.assertEqual(getattr(ev.sendcan[i], attr, 'new'), getattr(ev_old.sendcan[i], attr, 'old')) @@ -47,7 +48,7 @@ class TestBoarddApiMethods(unittest.TestCase): ev = log.Event.from_bytes(m) self.assertEqual(ev_old.which(), ev.which()) self.assertEqual(len(ev.can), len(ev_old.can)) - for i in xrange(len(ev.can)): + for i in range(len(ev.can)): attrs = ['address', 'busTime', 'dat', 'src'] for attr in attrs: self.assertEqual(getattr(ev.can[i], attr, 'new'), getattr(ev_old.can[i], attr, 'old')) @@ -57,14 +58,14 @@ class TestBoarddApiMethods(unittest.TestCase): recursions = 1000 n1 = sec_since_boot() - for i in xrange(recursions): + for i in range(recursions): boardd_old.can_list_to_can_capnp(can_list, 'sendcan').to_bytes() n2 = sec_since_boot() elapsed_old = n2 - n1 # print('Old API, elapsed time: {} secs'.format(elapsed_old)) n1 = sec_since_boot() - for i in xrange(recursions): + for i in range(recursions): boardd.can_list_to_can_capnp(can_list) n2 = sec_since_boot() elapsed_new = n2 - n1 diff --git a/selfdrive/boardd/tests/test_boardd_loopback.py b/selfdrive/boardd/tests/test_boardd_loopback.py index 042a5783a5..c3992bdac4 100755 --- a/selfdrive/boardd/tests/test_boardd_loopback.py +++ b/selfdrive/boardd/tests/test_boardd_loopback.py @@ -1,4 +1,4 @@ -#!/usr/bin/env python +#!/usr/bin/env python3 """Run boardd with the BOARDD_LOOPBACK envvar before running this test.""" import os diff --git a/selfdrive/can/Makefile b/selfdrive/can/Makefile index a9ee1e6861..85d2c494a9 100644 --- a/selfdrive/can/Makefile +++ b/selfdrive/can/Makefile @@ -36,7 +36,7 @@ endif OBJDIR = obj -OPENDBC_PATH := $(shell python2 -c 'import opendbc; print opendbc.DBC_PATH') +OPENDBC_PATH := $(shell python3 -c 'import opendbc; print(opendbc.DBC_PATH)') DBC_SOURCES := $(sort $(wildcard $(OPENDBC_PATH)/*.dbc)) DBC_OBJS := $(patsubst $(OPENDBC_PATH)/%.dbc,$(OBJDIR)/%.o,$(DBC_SOURCES)) @@ -70,12 +70,12 @@ libdbc.so:: $(LIBDBC_OBJS) $(DBC_OBJS) $(CEREAL_LIBS) packer_impl.so: packer_impl.pyx packer_setup.py - python2 packer_setup.py build_ext --inplace + python3 packer_setup.py build_ext --inplace rm -rf build rm -f packer_impl.cpp parser_pyx.so: parser_pyx_setup.py parser_pyx.pyx parser_pyx.pxd - python $< build_ext --inplace + python3 $< build_ext --inplace rm -rf build rm -f parser_pyx.cpp diff --git a/selfdrive/can/can_define.py b/selfdrive/can/can_define.py index 48ff8798fd..d6ffc5c669 100644 --- a/selfdrive/can/can_define.py +++ b/selfdrive/can/can_define.py @@ -1,11 +1,11 @@ from collections import defaultdict from selfdrive.can.libdbc_py import libdbc, ffi -class CANDefine(object): +class CANDefine(): def __init__(self, dbc_name): self.dv = defaultdict(dict) self.dbc_name = dbc_name - self.dbc = libdbc.dbc_lookup(dbc_name) + self.dbc = libdbc.dbc_lookup(dbc_name.encode('utf8')) num_vals = self.dbc[0].num_vals @@ -13,16 +13,16 @@ class CANDefine(object): num_msgs = self.dbc[0].num_msgs for i in range(num_msgs): msg = self.dbc[0].msgs[i] - name = ffi.string(msg.name) + name = ffi.string(msg.name).decode('utf8') address = msg.address self.address_to_msg_name[address] = name for i in range(num_vals): val = self.dbc[0].vals[i] - sgname = ffi.string(val.name) + sgname = ffi.string(val.name).decode('utf8') address = val.address - def_val = ffi.string(val.def_val) + def_val = ffi.string(val.def_val).decode('utf8') #separate definition/value pairs def_val = def_val.split() diff --git a/selfdrive/can/packer_impl.pyx b/selfdrive/can/packer_impl.pyx index 26ce8c1267..7429338ac4 100644 --- a/selfdrive/can/packer_impl.pyx +++ b/selfdrive/can/packer_impl.pyx @@ -1,4 +1,6 @@ # distutils: language = c++ +# cython: c_string_encoding=ascii, language_level=3 + from libc.stdint cimport uint32_t, uint64_t from libcpp.vector cimport vector from libcpp.map cimport map @@ -54,7 +56,7 @@ ctypedef uint64_t (*canpack_pack_vector_func)(void* inst, uint32_t address, cons ctypedef const DBC * (*dbc_lookup_func)(const char* dbc_name) -cdef class CANPacker(object): +cdef class CANPacker(): cdef void *packer cdef const DBC *dbc cdef map[string, (int, int)] name_to_address_and_size @@ -66,11 +68,14 @@ cdef class CANPacker(object): def __init__(self, dbc_name): can_dir = os.path.dirname(os.path.abspath(__file__)) libdbc_fn = os.path.join(can_dir, "libdbc.so") + libdbc_fn = str(libdbc_fn).encode('utf8') subprocess.check_call(["make"], cwd=can_dir) + cdef void *libdbc = dlopen(libdbc_fn, RTLD_LAZY) self.canpack_init = dlsym(libdbc, 'canpack_init') self.canpack_pack_vector = dlsym(libdbc, 'canpack_pack_vector') self.dbc_lookup = dlsym(libdbc, 'dbc_lookup') + self.packer = self.canpack_init(dbc_name) self.dbc = self.dbc_lookup(dbc_name) num_msgs = self.dbc[0].num_msgs @@ -82,8 +87,14 @@ cdef class CANPacker(object): cdef uint64_t pack(self, addr, values, counter): cdef vector[SignalPackValue] values_thing cdef SignalPackValue spv + + names = [] + for name, value in values.iteritems(): - spv.name = name + n = name.encode('utf8') + names.append(n) # TODO: find better way to keep reference to temp string arround + + spv.name = n spv.value = value values_thing.push_back(spv) @@ -105,7 +116,7 @@ cdef class CANPacker(object): addr = name_or_addr size = self.address_to_size[name_or_addr] else: - addr, size = self.name_to_address_and_size[name_or_addr] + addr, size = self.name_to_address_and_size[name_or_addr.encode('utf8')] cdef uint64_t val = self.pack(addr, values, counter) val = self.ReverseBytes(val) return [addr, 0, (&val)[:size], bus] diff --git a/selfdrive/can/packer_setup.py b/selfdrive/can/packer_setup.py index a36d4199ee..8b25e60580 100644 --- a/selfdrive/can/packer_setup.py +++ b/selfdrive/can/packer_setup.py @@ -1,5 +1,9 @@ -from distutils.core import setup, Extension +from distutils.core import Extension, setup # pylint: disable=import-error,no-name-in-module + from Cython.Build import cythonize +from common.cython_hacks import BuildExtWithoutPlatformSuffix + setup(name='CAN Packer API Implementation', + cmdclass={'build_ext': BuildExtWithoutPlatformSuffix}, ext_modules=cythonize(Extension("packer_impl", ["packer_impl.pyx"], language="c++", extra_compile_args=["-std=c++11"]))) diff --git a/selfdrive/can/parser_pyx.pxd b/selfdrive/can/parser_pyx.pxd index 6b1e50ce6d..28048bbc1f 100644 --- a/selfdrive/can/parser_pyx.pxd +++ b/selfdrive/can/parser_pyx.pxd @@ -1,4 +1,6 @@ # distutils: language = c++ +#cython: language_level=3 + from libc.stdint cimport uint32_t, uint64_t, uint16_t from libcpp.vector cimport vector from libcpp.map cimport map diff --git a/selfdrive/can/parser_pyx.pyx b/selfdrive/can/parser_pyx.pyx index 4f92970960..61a61aed60 100644 --- a/selfdrive/can/parser_pyx.pyx +++ b/selfdrive/can/parser_pyx.pyx @@ -1,4 +1,6 @@ # distutils: language = c++ +# cython: c_string_encoding=ascii, language_level=3 + from posix.dlfcn cimport dlopen, dlsym, RTLD_LAZY from libcpp cimport bool @@ -8,10 +10,11 @@ import numbers cdef int CAN_INVALID_CNT = 5 cdef class CANParser: - def __init__(self, dbc_name, signals, checks=None, bus=0, sendcan=False, tcp_addr="", timeout=-1): + def __init__(self, dbc_name, signals, checks=None, bus=0, sendcan=False, tcp_addr=b"", timeout=-1): self.test_mode_enabled = False can_dir = os.path.dirname(os.path.abspath(__file__)) libdbc_fn = os.path.join(can_dir, "libdbc.so") + libdbc_fn = str(libdbc_fn).encode('utf8') cdef void *libdbc = dlopen(libdbc_fn, RTLD_LAZY) self.can_init_with_vectors = dlsym(libdbc, 'can_init_with_vectors') @@ -33,24 +36,28 @@ cdef class CANParser: num_msgs = self.dbc[0].num_msgs for i in range(num_msgs): msg = self.dbc[0].msgs[i] - self.msg_name_to_address[string(msg.name)] = msg.address - self.address_to_msg_name[msg.address] = string(msg.name) + name = msg.name.decode('utf8') + + self.msg_name_to_address[name] = msg.address + self.address_to_msg_name[msg.address] = name self.vl[msg.address] = {} - self.vl[str(msg.name)] = {} + self.vl[name] = {} self.ts[msg.address] = {} - self.ts[str(msg.name)] = {} + self.ts[name] = {} # Convert message names into addresses for i in range(len(signals)): s = signals[i] if not isinstance(s[1], numbers.Number): - s = (s[0], self.msg_name_to_address[s[1]], s[2]) + name = s[1].encode('utf8') + s = (s[0], self.msg_name_to_address[name], s[2]) signals[i] = s for i in range(len(checks)): c = checks[i] if not isinstance(c[0], numbers.Number): - c = (self.msg_name_to_address[c[0]], c[1]) + name = c[0].encode('utf8') + c = (self.msg_name_to_address[name], c[1]) checks[i] = c cdef vector[SignalParseOptions] signal_options_v @@ -89,12 +96,15 @@ cdef class CANParser: for cv in self.can_values: - self.vl[cv.address][string(cv.name)] = cv.value - self.ts[cv.address][string(cv.name)] = cv.ts + # Cast char * directly to unicde + name = self.address_to_msg_name[cv.address].c_str() + cv_name = cv.name + + self.vl[cv.address][cv_name] = cv.value + self.ts[cv.address][cv_name] = cv.ts - sig_name = self.address_to_msg_name[cv.address] - self.vl[sig_name][string(cv.name)] = cv.value - self.ts[sig_name][string(cv.name)] = cv.ts + self.vl[name][cv_name] = cv.value + self.ts[name][cv_name] = cv.ts updated_val.insert(cv.address) diff --git a/selfdrive/can/parser_pyx_setup.py b/selfdrive/can/parser_pyx_setup.py index f20224f343..29102ee026 100644 --- a/selfdrive/can/parser_pyx_setup.py +++ b/selfdrive/can/parser_pyx_setup.py @@ -1,14 +1,19 @@ -from distutils.core import setup, Extension -from Cython.Build import cythonize import subprocess +from distutils.core import Extension, setup # pylint: disable=import-error,no-name-in-module + +from Cython.Build import cythonize + +from common.cython_hacks import BuildExtWithoutPlatformSuffix sourcefiles = ['parser_pyx.pyx'] extra_compile_args = ["-std=c++11"] -ARCH = subprocess.check_output(["uname", "-m"]).rstrip() +ARCH = subprocess.check_output(["uname", "-m"], encoding='utf8').rstrip() # pylint: disable=unexpected-keyword-arg + if ARCH == "aarch64": extra_compile_args += ["-Wno-deprecated-register"] setup(name='Radard Thread', + cmdclass={'build_ext': BuildExtWithoutPlatformSuffix}, ext_modules=cythonize( Extension( "parser_pyx", diff --git a/selfdrive/can/plant_can_parser.py b/selfdrive/can/plant_can_parser.py index e58906d7a0..65fcfc6415 100644 --- a/selfdrive/can/plant_can_parser.py +++ b/selfdrive/can/plant_can_parser.py @@ -7,7 +7,7 @@ from selfdrive.car.honda.hondacan import fix from common.realtime import sec_since_boot from common.dbc import dbc -class CANParser(object): +class CANParser(): def __init__(self, dbc_f, signals, checks=None): ### input: # dbc_f : dbc file @@ -73,7 +73,7 @@ class CANParser(object): self.ck[msg] = True if "CHECKSUM" in out.keys() and msg in self.msgs_ck: # remove checksum (half byte) - ck_portion = cdat[:-1] + chr(ord(cdat[-1]) & 0xF0) + ck_portion = cdat[:-1] + (cdat[-1] & 0xF0).to_bytes(1, 'little') # recalculate checksum msg_vl = fix(ck_portion, msg) # compare recalculated vs received checksum diff --git a/selfdrive/can/process_dbc.py b/selfdrive/can/process_dbc.py index 5f9237020b..187d413584 100755 --- a/selfdrive/can/process_dbc.py +++ b/selfdrive/can/process_dbc.py @@ -1,4 +1,5 @@ -#!/usr/bin/env python2 +#!/usr/bin/env python3 +from __future__ import print_function import os import glob import sys @@ -10,7 +11,7 @@ from common.dbc import dbc def main(): if len(sys.argv) != 3: - print "usage: %s dbc_directory output_directory" % (sys.argv[0],) + print("usage: %s dbc_directory output_directory" % (sys.argv[0],)) sys.exit(0) dbc_dir = sys.argv[1] @@ -38,7 +39,7 @@ def main(): if dbc_mtime < out_mtime and template_mtime < out_mtime and this_file_mtime < out_mtime: continue #skip output is newer than template and dbc - msgs = [(address, msg_name, msg_size, sorted(msg_sigs, key=lambda s: s.name not in ("COUNTER", "CHECKSUM"))) # process counter and checksums first + msgs = [(address, msg_name, msg_size, sorted(msg_sigs, key=lambda s: s.name not in (b"COUNTER", b"CHECKSUM"))) # process counter and checksums first for address, ((msg_name, msg_size), msg_sigs) in sorted(can_dbc.msgs.items()) if msg_sigs] def_vals = {a: set(b) for a,b in can_dbc.def_vals.items()} #remove duplicates @@ -55,22 +56,22 @@ def main(): for address, msg_name, msg_size, sigs in msgs: for sig in sigs: - if checksum_type is not None and sig.name == "CHECKSUM": + if checksum_type is not None and sig.name == b"CHECKSUM": if sig.size != checksum_size: sys.exit("CHECKSUM is not %d bits longs %s" % (checksum_size, msg_name)) if checksum_type == "honda" and sig.start_bit % 8 != 3: sys.exit("CHECKSUM starts at wrong bit %s" % msg_name) if checksum_type == "toyota" and sig.start_bit % 8 != 7: sys.exit("CHECKSUM starts at wrong bit %s" % msg_name) - if checksum_type == "honda" and sig.name == "COUNTER": + if checksum_type == "honda" and sig.name == b"COUNTER": if sig.size != 2: sys.exit("COUNTER is not 2 bits longs %s" % msg_name) if sig.start_bit % 8 != 5: sys.exit("COUNTER starts at wrong bit %s" % msg_name) if address in [0x200, 0x201]: - if sig.name == "COUNTER_PEDAL" and sig.size != 4: + if sig.name == b"COUNTER_PEDAL" and sig.size != 4: sys.exit("PEDAL COUNTER is not 4 bits longs %s" % msg_name) - if sig.name == "CHECKSUM_PEDAL" and sig.size != 8: + if sig.name == b"CHECKSUM_PEDAL" and sig.size != 8: sys.exit("PEDAL CHECKSUM is not 8 bits longs %s" % msg_name) # Fail on duplicate message names diff --git a/selfdrive/can/tests/packer_old.py b/selfdrive/can/tests/packer_old.py index 0c17c2e4d9..63120218e9 100644 --- a/selfdrive/can/tests/packer_old.py +++ b/selfdrive/can/tests/packer_old.py @@ -2,8 +2,9 @@ import struct from selfdrive.can.libdbc_py import libdbc, ffi -class CANPacker(object): +class CANPacker(): def __init__(self, dbc_name): + dbc_name = dbc_name.encode('utf8') self.packer = libdbc.canpack_init(dbc_name) self.dbc = libdbc.dbc_lookup(dbc_name) self.sig_names = {} @@ -13,16 +14,16 @@ class CANPacker(object): for i in range(num_msgs): msg = self.dbc[0].msgs[i] - name = ffi.string(msg.name) + name = ffi.string(msg.name).decode('utf8') address = msg.address self.name_to_address_and_size[name] = (address, msg.size) self.name_to_address_and_size[address] = (address, msg.size) def pack(self, addr, values, counter): values_thing = [] - for name, value in values.iteritems(): + for name, value in values.items(): if name not in self.sig_names: - self.sig_names[name] = ffi.new("char[]", name) + self.sig_names[name] = ffi.new("char[]", name.encode('utf8')) values_thing.append({ 'name': self.sig_names[name], diff --git a/selfdrive/can/tests/parser_old.py b/selfdrive/can/tests/parser_old.py index ccf613ab3c..1d2da57862 100644 --- a/selfdrive/can/tests/parser_old.py +++ b/selfdrive/can/tests/parser_old.py @@ -5,7 +5,7 @@ from selfdrive.can.libdbc_py import libdbc, ffi CAN_INVALID_CNT = 5 # after so many consecutive CAN data with wrong checksum, counter or frequency, flag CAN invalidity -class CANParser(object): +class CANParser(): def __init__(self, dbc_name, signals, checks=None, bus=0, sendcan=False, tcp_addr="127.0.0.1", timeout=-1): if checks is None: checks = [] @@ -16,7 +16,7 @@ class CANParser(object): self.ts = {} self.dbc_name = dbc_name - self.dbc = libdbc.dbc_lookup(dbc_name) + self.dbc = libdbc.dbc_lookup(dbc_name.encode('utf8')) self.msg_name_to_addres = {} self.address_to_msg_name = {} @@ -24,7 +24,7 @@ class CANParser(object): for i in range(num_msgs): msg = self.dbc[0].msgs[i] - name = ffi.string(msg.name) + name = ffi.string(msg.name).decode('utf8') address = msg.address self.msg_name_to_addres[name] = address @@ -48,7 +48,7 @@ class CANParser(object): c = (self.msg_name_to_addres[c[0]], c[1]) checks[i] = c - sig_names = dict((name, ffi.new("char[]", name)) for name, _, _ in signals) + sig_names = dict((name, ffi.new("char[]", name.encode('utf8'))) for name, _, _ in signals) signal_options_c = ffi.new("SignalParseOptions[]", [ { @@ -66,8 +66,8 @@ class CANParser(object): 'check_frequency': freq, } for msg_address, freq in message_options.items()]) - self.can = libdbc.can_init(bus, dbc_name, len(message_options_c), message_options_c, - len(signal_options_c), signal_options_c, sendcan, tcp_addr, timeout) + self.can = libdbc.can_init(bus, dbc_name.encode('utf8'), len(message_options_c), message_options_c, + len(signal_options_c), signal_options_c, sendcan, tcp_addr.encode('utf8'), timeout) self.p_can_valid = ffi.new("bool*") @@ -85,11 +85,11 @@ class CANParser(object): self.can_valid = self.can_invalid_cnt < CAN_INVALID_CNT ret = set() - for i in xrange(can_values_len): + for i in range(can_values_len): cv = self.can_values[i] address = cv.address # print("{0} {1}".format(hex(cv.address), ffi.string(cv.name))) - name = ffi.string(cv.name) + name = ffi.string(cv.name).decode('utf8') self.vl[address][name] = cv.value self.ts[address][name] = cv.ts diff --git a/selfdrive/can/tests/test_packer_chrysler.py b/selfdrive/can/tests/test_packer_chrysler.py index da85a98415..e566d87a72 100644 --- a/selfdrive/can/tests/test_packer_chrysler.py +++ b/selfdrive/can/tests/test_packer_chrysler.py @@ -13,7 +13,7 @@ class TestPackerMethods(unittest.TestCase): def test_correctness(self): # Test all commands, randomize the params. - for _ in xrange(1000): + for _ in range(1000): gear = ('drive', 'reverse', 'low')[random.randint(0, 3) % 3] lkas_active = (random.randint(0, 2) % 2 == 0) hud_alert = random.randint(0, 6) diff --git a/selfdrive/can/tests/test_packer_gm.py b/selfdrive/can/tests/test_packer_gm.py index db5631fbbe..54119fbe43 100644 --- a/selfdrive/can/tests/test_packer_gm.py +++ b/selfdrive/can/tests/test_packer_gm.py @@ -17,7 +17,7 @@ class TestPackerMethods(unittest.TestCase): def test_correctness(self): # Test all cars' commands, randomize the params. - for _ in xrange(1000): + for _ in range(1000): bus = random.randint(0, 65536) apply_steer = (random.randint(0, 2) % 2 == 0) idx = random.randint(0, 65536) diff --git a/selfdrive/can/tests/test_packer_honda.py b/selfdrive/can/tests/test_packer_honda.py index b6e779e40c..3a9548a3ef 100644 --- a/selfdrive/can/tests/test_packer_honda.py +++ b/selfdrive/can/tests/test_packer_honda.py @@ -15,8 +15,8 @@ class TestPackerMethods(unittest.TestCase): def test_correctness(self): # Test all commands, randomize the params. - for _ in xrange(1000): - is_panda_black = False + for _ in range(1000): + has_relay = False car_fingerprint = HONDA_BOSCH[0] apply_brake = (random.randint(0, 2) % 2 == 0) @@ -25,15 +25,15 @@ class TestPackerMethods(unittest.TestCase): pcm_cancel_cmd = (random.randint(0, 2) % 2 == 0) fcw = random.randint(0, 65536) idx = random.randint(0, 65536) - m_old = hondacan.create_brake_command(self.honda_cp_old, apply_brake, pump_on, pcm_override, pcm_cancel_cmd, fcw, idx, car_fingerprint, is_panda_black) - m = hondacan.create_brake_command(self.honda_cp, apply_brake, pump_on, pcm_override, pcm_cancel_cmd, fcw, idx, car_fingerprint, is_panda_black) + m_old = hondacan.create_brake_command(self.honda_cp_old, apply_brake, pump_on, pcm_override, pcm_cancel_cmd, fcw, idx, car_fingerprint, has_relay) + m = hondacan.create_brake_command(self.honda_cp, apply_brake, pump_on, pcm_override, pcm_cancel_cmd, fcw, idx, car_fingerprint, has_relay) self.assertEqual(m_old, m) apply_steer = (random.randint(0, 2) % 2 == 0) lkas_active = (random.randint(0, 2) % 2 == 0) idx = random.randint(0, 65536) - m_old = hondacan.create_steering_control(self.honda_cp_old, apply_steer, lkas_active, car_fingerprint, idx, is_panda_black) - m = hondacan.create_steering_control(self.honda_cp, apply_steer, lkas_active, car_fingerprint, idx, is_panda_black) + m_old = hondacan.create_steering_control(self.honda_cp_old, apply_steer, lkas_active, car_fingerprint, idx, has_relay) + m = hondacan.create_steering_control(self.honda_cp, apply_steer, lkas_active, car_fingerprint, idx, has_relay) self.assertEqual(m_old, m) pcm_speed = random.randint(0, 65536) @@ -41,14 +41,14 @@ class TestPackerMethods(unittest.TestCase): 0xc1, random.randint(0, 65536), random.randint(0, 65536), random.randint(0, 65536), random.randint(0, 65536)) idx = random.randint(0, 65536) is_metric = (random.randint(0, 2) % 2 == 0) - m_old = hondacan.create_ui_commands(self.honda_cp_old, pcm_speed, hud, car_fingerprint, is_metric, idx, is_panda_black) - m = hondacan.create_ui_commands(self.honda_cp, pcm_speed, hud, car_fingerprint, is_metric, idx, is_panda_black) + m_old = hondacan.create_ui_commands(self.honda_cp_old, pcm_speed, hud, car_fingerprint, is_metric, idx, has_relay) + m = hondacan.create_ui_commands(self.honda_cp, pcm_speed, hud, car_fingerprint, is_metric, idx, has_relay) self.assertEqual(m_old, m) button_val = random.randint(0, 65536) idx = random.randint(0, 65536) - m_old = hondacan.spam_buttons_command(self.honda_cp_old, button_val, idx, car_fingerprint, is_panda_black) - m = hondacan.spam_buttons_command(self.honda_cp, button_val, idx, car_fingerprint, is_panda_black) + m_old = hondacan.spam_buttons_command(self.honda_cp_old, button_val, idx, car_fingerprint, has_relay) + m = hondacan.spam_buttons_command(self.honda_cp, button_val, idx, car_fingerprint, has_relay) self.assertEqual(m_old, m) diff --git a/selfdrive/can/tests/test_packer_hyundai.py b/selfdrive/can/tests/test_packer_hyundai.py index d9e829ad59..c2bcb04014 100644 --- a/selfdrive/can/tests/test_packer_hyundai.py +++ b/selfdrive/can/tests/test_packer_hyundai.py @@ -14,7 +14,7 @@ class TestPackerMethods(unittest.TestCase): def test_correctness(self): # Test all commands, randomize the params. - for _ in xrange(1000): + for _ in range(1000): # Hyundai car_fingerprint = hyundai_checksum["crc8"][0] apply_steer = (random.randint(0, 2) % 2 == 0) diff --git a/selfdrive/can/tests/test_packer_subaru.py b/selfdrive/can/tests/test_packer_subaru.py index 65dd77cf83..0690806d7c 100644 --- a/selfdrive/can/tests/test_packer_subaru.py +++ b/selfdrive/can/tests/test_packer_subaru.py @@ -14,7 +14,7 @@ class TestPackerMethods(unittest.TestCase): def test_correctness(self): # Test all cars' commands, randomize the params. - for _ in xrange(1000): + for _ in range(1000): apply_steer = (random.randint(0, 2) % 2 == 0) frame = random.randint(1, 65536) steer_step = random.randint(1, 65536) diff --git a/selfdrive/can/tests/test_packer_toyota.py b/selfdrive/can/tests/test_packer_toyota.py index fc53399ee5..05f5eeed2e 100644 --- a/selfdrive/can/tests/test_packer_toyota.py +++ b/selfdrive/can/tests/test_packer_toyota.py @@ -17,7 +17,7 @@ class TestPackerMethods(unittest.TestCase): def test_correctness(self): # Test all commands, randomize the params. - for _ in xrange(1000): + for _ in range(1000): # Toyota steer = random.randint(-1, 1) enabled = (random.randint(0, 2) % 2 == 0) @@ -66,14 +66,14 @@ class TestPackerMethods(unittest.TestCase): left_lane_depart = (random.randint(0, 2) % 2 == 0) right_lane_depart = (random.randint(0, 2) % 2 == 0) - for _ in xrange(recursions): + for _ in range(recursions): create_ui_command(self.cp_old, steer, chime, left_line, right_line, left_lane_depart, right_lane_depart) n2 = sec_since_boot() elapsed_old = n2 - n1 # print('Old API, elapsed time: {} secs'.format(elapsed_old)) n1 = sec_since_boot() - for _ in xrange(recursions): + for _ in range(recursions): create_ui_command(self.cp, steer, chime, left_line, right_line, left_lane_depart, right_lane_depart) n2 = sec_since_boot() elapsed_new = n2 - n1 diff --git a/selfdrive/can/tests/test_parser.py b/selfdrive/can/tests/test_parser.py index ff3cbddf7e..09d153c1bd 100755 --- a/selfdrive/can/tests/test_parser.py +++ b/selfdrive/can/tests/test_parser.py @@ -1,4 +1,4 @@ -#!/usr/bin/env python2 +#!/usr/bin/env python3 import os import unittest @@ -45,7 +45,7 @@ def dicts_vals_differ(dict1, dict2): def run_route(route): can = messaging.pub_sock(service_list['can'].port) - CP = CarInterface.get_params(CAR.CIVIC, {}) + CP = CarInterface.get_params(CAR.CIVIC) signals, checks = get_can_signals(CP) parser_old = CANParserOld(DBC[CP.carFingerprint]['pt'], signals, checks, 0, timeout=-1, tcp_addr="127.0.0.1") parser_new = CANParserNew(DBC[CP.carFingerprint]['pt'], signals, checks, 0, timeout=-1, tcp_addr="127.0.0.1") @@ -95,7 +95,7 @@ class TestCanParser(unittest.TestCase): for route in self.routes.values(): route_filename = route + ".bz2" if not os.path.isfile(route_filename): - with open(route + ".bz2", "w") as f: + with open(route + ".bz2", "wb") as f: f.write(requests.get(BASE_URL + route_filename).content) def test_parser_civic(self): diff --git a/selfdrive/car/__init__.py b/selfdrive/car/__init__.py index 3b259c6222..b0232daf44 100644 --- a/selfdrive/car/__init__.py +++ b/selfdrive/car/__init__.py @@ -4,6 +4,9 @@ from common.numpy_fast import clip # kg of standard extra cargo to count for drive, gas, etc... STD_CARGO_KG = 136. +def gen_empty_fingerprint(): + return {i: {} for i in range(0, 4)} + # FIXME: hardcoding honda civic 2016 touring params so they can be used to # scale unknown params for other cars class CivicParams: @@ -31,7 +34,7 @@ def scale_tire_stiffness(mass, wheelbase, center_to_front, tire_stiffness_factor (center_to_front / wheelbase) / (CivicParams.CENTER_TO_FRONT / CivicParams.WHEELBASE) return tire_stiffness_front, tire_stiffness_rear - + def dbc_dict(pt_dbc, radar_dbc, chassis_dbc=None): return {'pt': pt_dbc, 'radar': radar_dbc, 'chassis': chassis_dbc} @@ -53,11 +56,10 @@ def apply_std_steer_torque_limits(apply_torque, apply_torque_last, driver_torque apply_torque = clip(apply_torque, apply_torque_last - LIMITS.STEER_DELTA_UP, min(apply_torque_last + LIMITS.STEER_DELTA_DOWN, LIMITS.STEER_DELTA_UP)) - return int(round(apply_torque)) + return int(round(float(apply_torque))) def apply_toyota_steer_torque_limits(apply_torque, apply_torque_last, motor_torque, LIMITS): - # limits due to comparison of commanded torque VS motor reported torque max_lim = min(max(motor_torque + LIMITS.STEER_ERROR_MAX, LIMITS.STEER_ERROR_MAX), LIMITS.STEER_MAX) min_lim = max(min(motor_torque - LIMITS.STEER_ERROR_MAX, -LIMITS.STEER_ERROR_MAX), -LIMITS.STEER_MAX) @@ -74,7 +76,7 @@ def apply_toyota_steer_torque_limits(apply_torque, apply_torque_last, motor_torq apply_torque_last - LIMITS.STEER_DELTA_UP, min(apply_torque_last + LIMITS.STEER_DELTA_DOWN, LIMITS.STEER_DELTA_UP)) - return int(round(apply_torque)) + return int(round(float(apply_torque))) def crc8_pedal(data): @@ -106,8 +108,19 @@ def create_gas_command(packer, gas_amount, idx): dat = packer.make_can_msg("GAS_COMMAND", 0, values)[2] - dat = [ord(i) for i in dat] checksum = crc8_pedal(dat[:-1]) values["CHECKSUM_PEDAL"] = checksum return packer.make_can_msg("GAS_COMMAND", 0, values) + + +def is_ecu_disconnected(fingerprint, fingerprint_list, ecu_fingerprint, car, ecu): + # check if a stock ecu is disconnected by looking for specific CAN msgs in the fingerprint + # return True if the reference car fingerprint contains the ecu fingerprint msg and + # fingerprint does not contains messages normally sent by a given ecu + ecu_in_car = False + for car_finger in fingerprint_list[car]: + if any(msg in car_finger for msg in ecu_fingerprint[ecu]): + ecu_in_car = True + + return ecu_in_car and not any(msg in fingerprint for msg in ecu_fingerprint[ecu]) diff --git a/selfdrive/car/car_helpers.py b/selfdrive/car/car_helpers.py index a90f8d43e0..eaff50a3b7 100644 --- a/selfdrive/car/car_helpers.py +++ b/selfdrive/car/car_helpers.py @@ -2,11 +2,12 @@ import os import zmq from cereal import car from common.params import Params -from common.vin import get_vin, VIN_UNKNOWN from common.basedir import BASEDIR -from common.fingerprints import eliminate_incompatible_cars, all_known_cars +from selfdrive.car.fingerprints import eliminate_incompatible_cars, all_known_cars +from selfdrive.car.vin import get_vin, VIN_UNKNOWN from selfdrive.swaglog import cloudlog import selfdrive.messaging as messaging +from selfdrive.car import gen_empty_fingerprint def get_one_can(logcan): @@ -67,12 +68,7 @@ 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, is_panda_black): - if os.getenv("SIMULATOR2") is not None: - return ("simulator2", None, "") - elif os.getenv("SIMULATOR") is not None: - return ("simulator", None, "") - +def fingerprint(logcan, sendcan, has_relay): params = Params() car_params = params.get("CarParams") @@ -80,7 +76,7 @@ def fingerprint(logcan, sendcan, is_panda_black): # use already stored VIN: a new VIN query cannot be done, since panda isn't in ELM327 mode car_params = car.CarParams.from_bytes(car_params) vin = VIN_UNKNOWN if car_params.carVin == "" else car_params.carVin - elif is_panda_black: + elif has_relay: # Vin query only reliably works thorugh OBDII vin = get_vin(logcan, sendcan, 1) else: @@ -89,7 +85,7 @@ def fingerprint(logcan, sendcan, is_panda_black): cloudlog.warning("VIN %s", vin) Params().put("CarVin", vin) - finger = {i: {} for i in range(0, 4)} # collect on all buses + finger = gen_empty_fingerprint() candidate_cars = {i: all_known_cars() for i in [0, 1]} # attempt fingerprint on both bus 0 and 1 frame = 0 frame_fingerprint = 10 # 0.1s @@ -105,10 +101,11 @@ def fingerprint(logcan, sendcan, is_panda_black): # and VIN query response. # Include bus 2 for toyotas to disambiguate cars using camera messages # (ideally should be done for all cars but we can't for Honda Bosch) + if can.src in range(0, 4): + finger[can.src][can.address] = len(can.dat) for b in candidate_cars: if (can.src == b or (only_toyota_left(candidate_cars[b]) and can.src == 2)) and \ can.address < 0x800 and can.address not in [0x7df, 0x7e0, 0x7e8]: - finger[can.src][can.address] = len(can.dat) candidate_cars[b] = eliminate_incompatible_cars(can, candidate_cars[b]) # if we only have one car choice and the time since we got our first @@ -123,7 +120,7 @@ def fingerprint(logcan, sendcan, is_panda_black): car_fingerprint = candidate_cars[b][0] # bail if no cars left or we've been waiting for more than 2s - failed = all(len(cc) == 0 for cc in candidate_cars.itervalues()) or frame > 200 + failed = all(len(cc) == 0 for cc in candidate_cars.values()) or frame > 200 succeeded = car_fingerprint is not None done = failed or succeeded @@ -133,15 +130,15 @@ def fingerprint(logcan, sendcan, is_panda_black): return car_fingerprint, finger, vin -def get_car(logcan, sendcan, is_panda_black=False): +def get_car(logcan, sendcan, has_relay=False): - candidate, fingerprints, vin = fingerprint(logcan, sendcan, is_panda_black) + candidate, fingerprints, vin = 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[0], vin, is_panda_black) + car_params = CarInterface.get_params(candidate, fingerprints, vin, has_relay) return CarInterface(car_params, CarController), car_params diff --git a/selfdrive/car/chrysler/carcontroller.py b/selfdrive/car/chrysler/carcontroller.py index 2ddc4bdaf3..7e2dc5cff6 100644 --- a/selfdrive/car/chrysler/carcontroller.py +++ b/selfdrive/car/chrysler/carcontroller.py @@ -11,7 +11,7 @@ class SteerLimitParams: STEER_ERROR_MAX = 80 -class CarController(object): +class CarController(): def __init__(self, dbc_name, car_fingerprint, enable_camera): self.braking = False # redundant safety check with the board diff --git a/selfdrive/car/chrysler/carstate.py b/selfdrive/car/chrysler/carstate.py index 13ea25b59c..93b354e0f8 100644 --- a/selfdrive/car/chrysler/carstate.py +++ b/selfdrive/car/chrysler/carstate.py @@ -77,7 +77,7 @@ def get_camera_parser(CP): return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 2) -class CarState(object): +class CarState(): def __init__(self, CP): self.CP = CP diff --git a/selfdrive/car/chrysler/chryslercan.py b/selfdrive/car/chrysler/chryslercan.py index d710262e91..047ae82580 100644 --- a/selfdrive/car/chrysler/chryslercan.py +++ b/selfdrive/car/chrysler/chryslercan.py @@ -1,6 +1,7 @@ from cereal import car +GearShifter = car.CarState.GearShifter VisualAlert = car.CarControl.HUDControl.VisualAlert def calc_checksum(data): @@ -48,7 +49,7 @@ def create_lkas_hud(packer, gear, lkas_active, hud_alert, hud_count, lkas_car_mo # LKAS_HUD 0x2a6 (678) Controls what lane-keeping icon is displayed. if hud_alert == VisualAlert.steerRequired: - msg = '0000000300000000'.decode('hex') + msg = b'\x00\x00\x00\x03\x00\x00\x00\x00' return make_can_msg(0x2a6, msg) color = 1 # default values are for park or neutral in 2017 are 0 0, but trying 1 1 for 2019 @@ -59,7 +60,7 @@ def create_lkas_hud(packer, gear, lkas_active, hud_alert, hud_count, lkas_car_mo alerts = 1 # CAR.PACIFICA_2018_HYBRID and CAR.PACIFICA_2019_HYBRID # had color = 1 and lines = 1 but trying 2017 hybrid style for now. - if gear in ('drive', 'reverse', 'low'): + if gear in (GearShifter.drive, GearShifter.reverse, GearShifter.low): if lkas_active: color = 2 # control active, display green. lines = 6 @@ -86,7 +87,7 @@ def create_lkas_command(packer, apply_steer, moving_fast, frame): } dat = packer.make_can_msg("LKAS_COMMAND", 0, values)[2] - dat = [ord(i) for i in dat][:-1] + dat = dat[:-1] checksum = calc_checksum(dat) values["CHECKSUM"] = checksum @@ -95,8 +96,8 @@ def create_lkas_command(packer, apply_steer, moving_fast, frame): def create_wheel_buttons(frame): # WHEEL_BUTTONS (571) Message sent to cancel ACC. - start = [0x01] # acc cancel set + start = b"\x01" # acc cancel set counter = (frame % 10) << 4 - dat = start + [counter] - dat = dat + [calc_checksum(dat)] - return make_can_msg(0x23b, str(bytearray(dat))) + dat = start + counter.to_bytes(1, 'little') + dat = dat + calc_checksum(dat).to_bytes(1, 'little') + return make_can_msg(0x23b, dat) diff --git a/selfdrive/car/chrysler/interface.py b/selfdrive/car/chrysler/interface.py index e470b3c1d8..8a3cd750a4 100755 --- a/selfdrive/car/chrysler/interface.py +++ b/selfdrive/car/chrysler/interface.py @@ -1,16 +1,17 @@ -#!/usr/bin/env python +#!/usr/bin/env python3 from cereal import car from selfdrive.config import Conversions as CV from selfdrive.controls.lib.drive_helpers import EventTypes as ET, create_event from selfdrive.controls.lib.vehicle_model import VehicleModel from selfdrive.car.chrysler.carstate import CarState, get_can_parser, get_camera_parser -from selfdrive.car.chrysler.values import ECU, check_ecu_msgs, CAR -from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness +from selfdrive.car.chrysler.values import ECU, ECU_FINGERPRINT, CAR, FINGERPRINTS +from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, is_ecu_disconnected, gen_empty_fingerprint +from selfdrive.car.interfaces import CarInterfaceBase GearShifter = car.CarState.GearShifter ButtonType = car.CarState.ButtonEvent.Type -class CarInterface(object): +class CarInterface(CarInterfaceBase): def __init__(self, CP, CarController): self.CP = CP self.VM = VehicleModel(CP) @@ -34,18 +35,14 @@ class CarInterface(object): return float(accel) / 3.0 @staticmethod - def calc_accel_override(a_ego, a_target, v_ego, v_target): - return 1.0 - - @staticmethod - def get_params(candidate, fingerprint, vin="", is_panda_black=False): + def get_params(candidate, fingerprint=gen_empty_fingerprint(), vin="", has_relay=False): ret = car.CarParams.new_message() ret.carName = "chrysler" ret.carFingerprint = candidate ret.carVin = vin - ret.isPandaBlack = is_panda_black + ret.isPandaBlack = has_relay ret.safetyModel = car.CarParams.SafetyModel.chrysler @@ -95,7 +92,7 @@ class CarInterface(object): ret.brakeMaxBP = [5., 20.] ret.brakeMaxV = [1., 0.8] - ret.enableCamera = not check_ecu_msgs(fingerprint, ECU.CAM) or is_panda_black + ret.enableCamera = is_ecu_disconnected(fingerprint[0], FINGERPRINTS, ECU_FINGERPRINT, candidate, ECU.CAM) or has_relay print("ECU Camera Simulated: {0}".format(ret.enableCamera)) ret.openpilotLongitudinalControl = False diff --git a/selfdrive/car/chrysler/radar_interface.py b/selfdrive/car/chrysler/radar_interface.py index 815d2c3e4c..3130c2ca71 100755 --- a/selfdrive/car/chrysler/radar_interface.py +++ b/selfdrive/car/chrysler/radar_interface.py @@ -1,10 +1,11 @@ -#!/usr/bin/env python +#!/usr/bin/env python3 import os from selfdrive.can.parser import CANParser from cereal import car +from selfdrive.car.interfaces import RadarInterfaceBase -RADAR_MSGS_C = range(0x2c2, 0x2d4+2, 2) # c_ messages 706,...,724 -RADAR_MSGS_D = range(0x2a2, 0x2b4+2, 2) # d_ messages +RADAR_MSGS_C = list(range(0x2c2, 0x2d4+2, 2)) # c_ messages 706,...,724 +RADAR_MSGS_D = list(range(0x2a2, 0x2b4+2, 2)) # d_ messages LAST_MSG = max(RADAR_MSGS_C + RADAR_MSGS_D) NUMBER_MSGS = len(RADAR_MSGS_C) + len(RADAR_MSGS_D) @@ -45,10 +46,10 @@ def _address_to_track(address): return (address - RADAR_MSGS_D[0]) // 2 raise ValueError("radar received unexpected address %d" % address) -class RadarInterface(object): +class RadarInterface(RadarInterfaceBase): def __init__(self, CP): self.pts = {} - self.delay = 0.0 # Delay of radar #TUNE + self.delay = 0 # Delay of radar #TUNE self.rcp = _create_radar_can_parser() self.updated_messages = set() self.trigger_msg = LAST_MSG diff --git a/selfdrive/car/chrysler/run_tests.sh b/selfdrive/car/chrysler/run_tests.sh deleted file mode 100755 index 1dcc8b2acf..0000000000 --- a/selfdrive/car/chrysler/run_tests.sh +++ /dev/null @@ -1 +0,0 @@ -PYTHONPATH=`realpath ../../../` python chryslercan_test.py diff --git a/selfdrive/car/chrysler/chryslercan_test.py b/selfdrive/car/chrysler/test_chryslercan.py similarity index 55% rename from selfdrive/car/chrysler/chryslercan_test.py rename to selfdrive/car/chrysler/test_chryslercan.py index 54b845342f..292fec5222 100644 --- a/selfdrive/car/chrysler/chryslercan_test.py +++ b/selfdrive/car/chrysler/test_chryslercan.py @@ -1,55 +1,57 @@ -from selfdrive.car.chrysler import chryslercan -from selfdrive.can.packer import CANPacker +import unittest from cereal import car +from selfdrive.can.packer import CANPacker +from selfdrive.car.chrysler import chryslercan + VisualAlert = car.CarControl.HUDControl.VisualAlert +GearShifter = car.CarState.GearShifter -import unittest class TestChryslerCan(unittest.TestCase): def test_checksum(self): - self.assertEqual(0x75, chryslercan.calc_checksum([0x01, 0x20])) - self.assertEqual(0xcc, chryslercan.calc_checksum([0x14, 0, 0, 0, 0x20])) + self.assertEqual(0x75, chryslercan.calc_checksum(b"\x01\x20")) + self.assertEqual(0xcc, chryslercan.calc_checksum(b"\x14\x00\x00\x00\x20")) def test_hud(self): packer = CANPacker('chrysler_pacifica_2017_hybrid') self.assertEqual( - [0x2a6, 0, '0100010100000000'.decode('hex'), 0], + [0x2a6, 0, b'\x01\x00\x01\x01\x00\x00\x00\x00', 0], chryslercan.create_lkas_hud( packer, - 'park', False, False, 1, 0)) + GearShifter.park, False, False, 1, 0)) self.assertEqual( - [0x2a6, 0, '0100010000000000'.decode('hex'), 0], + [0x2a6, 0, b'\x01\x00\x01\x00\x00\x00\x00\x00', 0], chryslercan.create_lkas_hud( packer, - 'park', False, False, 5*4, 0)) + GearShifter.park, False, False, 5*4, 0)) self.assertEqual( - [0x2a6, 0, '0100010000000000'.decode('hex'), 0], + [0x2a6, 0, b'\x01\x00\x01\x00\x00\x00\x00\x00', 0], chryslercan.create_lkas_hud( packer, - 'park', False, False, 99999, 0)) + GearShifter.park, False, False, 99999, 0)) self.assertEqual( - [0x2a6, 0, '0200060000000000'.decode('hex'), 0], + [0x2a6, 0, b'\x02\x00\x06\x00\x00\x00\x00\x00', 0], chryslercan.create_lkas_hud( packer, - 'drive', True, False, 99999, 0)) + GearShifter.drive, True, False, 99999, 0)) self.assertEqual( - [0x2a6, 0, '0264060000000000'.decode('hex'), 0], + [0x2a6, 0, b'\x02\x64\x06\x00\x00\x00\x00\x00', 0], chryslercan.create_lkas_hud( packer, - 'drive', True, False, 99999, 0x64)) + GearShifter.drive, True, False, 99999, 0x64)) def test_command(self): packer = CANPacker('chrysler_pacifica_2017_hybrid') self.assertEqual( - [0x292, 0, '140000001086'.decode('hex'), 0], + [0x292, 0, b'\x14\x00\x00\x00\x10\x86', 0], chryslercan.create_lkas_command( packer, 0, True, 1)) self.assertEqual( - [0x292, 0, '040000008083'.decode('hex'), 0], + [0x292, 0, b'\x04\x00\x00\x00\x80\x83', 0], chryslercan.create_lkas_command( packer, 0, False, 8)) diff --git a/selfdrive/car/chrysler/values.py b/selfdrive/car/chrysler/values.py index 2e74d70239..1832c8cd08 100644 --- a/selfdrive/car/chrysler/values.py +++ b/selfdrive/car/chrysler/values.py @@ -4,7 +4,7 @@ class CAR: PACIFICA_2017_HYBRID = "CHRYSLER PACIFICA HYBRID 2017" PACIFICA_2018_HYBRID = "CHRYSLER PACIFICA HYBRID 2018" PACIFICA_2019_HYBRID = "CHRYSLER PACIFICA HYBRID 2019" - PACIFICA_2018 = "CHRYSLER PACIFICA 2018" + PACIFICA_2018 = "CHRYSLER PACIFICA 2018" # Also covers Pacifica 2017. JEEP_CHEROKEE = "JEEP GRAND CHEROKEE V6 2018" # Also covers Tailhawk 2017. JEEP_CHEROKEE_2019 = "JEEP GRAND CHEROKEE 2019" @@ -25,14 +25,15 @@ FINGERPRINTS = { ], CAR.PACIFICA_2018: [ {55: 8, 257: 5, 258: 8, 264: 8, 268: 8, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 292: 8, 294: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 344: 8, 368: 8, 376: 3, 384: 8, 388: 4, 416: 7, 448: 6, 456: 4, 464: 8, 469: 8, 480: 8, 500: 8, 501: 8, 512: 8, 514: 8, 516: 7, 517: 7, 520: 8, 524: 8, 526: 6, 528: 8, 532: 8, 542: 8, 544: 8, 557: 8, 559: 8, 560: 4, 564: 8, 571: 3, 579: 8, 584: 8, 608: 8, 624: 8, 625: 8, 632: 8, 639: 8, 656: 4, 658: 6, 660: 8, 669: 3, 671: 8, 672: 8, 678: 8, 680: 8, 705: 8, 706: 8, 709: 8, 710: 8, 719: 8, 720: 6, 729: 5, 736: 8, 746: 5, 752: 2, 760: 8, 764: 8, 766: 8, 770: 8, 773: 8, 779: 8, 784: 8, 792: 8, 799: 8, 800: 8, 804: 8, 808: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 832: 8, 838: 2, 848: 8, 853: 8, 856: 4, 860: 6, 863: 8, 882: 8, 897: 8, 924: 8, 926: 3, 937: 8, 947: 8, 948: 8, 969: 4, 974: 5, 979: 8, 980: 8, 981: 8, 982: 8, 983: 8, 984: 8, 992: 8, 993: 7, 995: 8, 996: 8, 1000: 8, 1001: 8, 1002: 8, 1003: 8, 1008: 8, 1009: 8, 1010: 8, 1011: 8, 1012: 8, 1013: 8, 1014: 8, 1015: 8, 1024: 8, 1025: 8, 1026: 8, 1031: 8, 1033: 8, 1050: 8, 1059: 8, 1098: 8, 1100: 8, 1537: 8, 1538: 8, 1562: 8}, + {55: 8, 257: 5, 258: 8, 264: 8, 268: 8, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 292: 8, 294: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 344: 8, 368: 8, 376: 3, 384: 8, 388: 4, 416: 7, 448: 6, 456: 4, 464: 8, 469: 8, 480: 8, 500: 8, 501: 8, 512: 8, 514: 8, 516: 7, 517: 7, 520: 8, 524: 8, 526: 6, 528: 8, 532: 8, 542: 8, 544: 8, 557: 8, 559: 8, 560: 4, 564: 4, 571: 3, 584: 8, 608: 8, 624: 8, 625: 8, 632: 8, 639: 8, 656: 4, 658: 6, 660: 8, 669: 3, 671: 8, 672: 8, 678: 8, 680: 8, 705: 8, 706: 8, 709: 8, 710: 8, 719: 8, 720: 6, 729: 5, 736: 8, 746: 5, 752: 2, 760: 8, 764: 8, 766: 8, 770: 8, 773: 8, 779: 8, 784: 8, 792: 8, 799: 8, 800: 8, 804: 8, 808: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 832: 8, 838: 2, 848: 8, 853: 8, 856: 4, 860: 6, 863: 8, 882: 8, 897: 8, 924: 3, 926: 3, 937: 8, 947: 8, 948: 8, 969: 4, 974: 5, 979: 8, 980: 8, 981: 8, 982: 8, 983: 8, 984: 8, 992: 8, 993: 7, 995: 8, 996: 8, 1000: 8, 1001: 8, 1002: 8, 1003: 8, 1008: 8, 1009: 8, 1010: 8, 1011: 8, 1012: 8, 1013: 8, 1014: 8, 1015: 8, 1024: 8, 1025: 8, 1026: 8, 1031: 8, 1033: 8, 1050: 8, 1059: 8, 1098: 8, 1100: 8, 1537: 8, 1538: 8, 1562: 8}, ], CAR.PACIFICA_2018_HYBRID: [ - {68: 8, 168: 8, 257: 5, 258: 8, 264: 8, 268: 8, 270: 8, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 291: 8, 292: 8, 294: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 344: 8, 368: 8, 376: 3, 384: 8, 388: 4, 448: 6, 456: 4, 464: 8, 469: 8, 480: 8, 500: 8, 501: 8, 512: 8, 514: 8, 520: 8, 528: 8, 532: 8, 544: 8, 557: 8, 559: 8, 560: 4, 564: 8, 571: 3, 579: 8, 584: 8, 608: 8, 624: 8, 625: 8, 632: 8, 639: 8, 653: 8, 654: 8, 655: 8, 660: 8, 669: 3, 671: 8, 672: 8, 680: 8, 701: 8, 704: 8, 705: 8, 706: 8, 709: 8, 710: 8, 719: 8, 720: 6, 736: 8, 737: 8, 746: 5, 760: 8, 764: 8, 766: 8, 770: 8, 773: 8, 779: 8, 782: 8, 784: 8, 792: 8, 799: 8, 800: 8, 804: 8, 808: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 832: 8, 838: 2, 848: 8, 853: 8, 856: 4, 860: 6, 863: 8, 878: 8, 882: 8, 897: 8, 908: 8, 924: 8, 926: 3, 929: 8, 937: 8, 938: 8, 939: 8, 940: 8, 941: 8, 942: 8, 943: 8, 947: 8, 948: 8, 958: 8, 959: 8, 969: 4, 974: 5, 979: 8, 980: 8, 981: 8, 982: 8, 983: 8, 984: 8, 992: 8, 993: 7, 995: 8, 996: 8, 1000: 8, 1001: 8, 1002: 8, 1003: 8, 1008: 8, 1009: 8, 1010: 8, 1011: 8, 1012: 8, 1013: 8, 1014: 8, 1015: 8, 1024: 8, 1025: 8, 1026: 8, 1031: 8, 1033: 8, 1050: 8, 1059: 8, 1082: 8, 1083: 8, 1098: 8, 1100: 8}, + {68: 8, 168: 8, 257: 5, 258: 8, 264: 8, 268: 8, 270: 8, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 291: 8, 292: 8, 294: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 344: 8, 368: 8, 376: 3, 384: 8, 388: 4, 448: 6, 456: 4, 464: 8, 469: 8, 480: 8, 500: 8, 501: 8, 512: 8, 514: 8, 520: 8, 528: 8, 532: 8, 544: 8, 557: 8, 559: 8, 560: 4, 564: 8, 571: 3, 579: 8, 584: 8, 608: 8, 624: 8, 625: 8, 632: 8, 639: 8, 653: 8, 654: 8, 655: 8, 658: 6, 660: 8, 669: 3, 671: 8, 672: 8, 680: 8, 701: 8, 704: 8, 705: 8, 706: 8, 709: 8, 710: 8, 719: 8, 720: 6, 736: 8, 737: 8, 746: 5, 760: 8, 764: 8, 766: 8, 770: 8, 773: 8, 779: 8, 782: 8, 784: 8, 792: 8, 799: 8, 800: 8, 804: 8, 808: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 832: 8, 838: 2, 848: 8, 853: 8, 856: 4, 860: 6, 863: 8, 878: 8, 882: 8, 897: 8, 908: 8, 924: 8, 926: 3, 929: 8, 937: 8, 938: 8, 939: 8, 940: 8, 941: 8, 942: 8, 943: 8, 947: 8, 948: 8, 958: 8, 959: 8, 969: 4, 974: 5, 979: 8, 980: 8, 981: 8, 982: 8, 983: 8, 984: 8, 992: 8, 993: 7, 995: 8, 996: 8, 1000: 8, 1001: 8, 1002: 8, 1003: 8, 1008: 8, 1009: 8, 1010: 8, 1011: 8, 1012: 8, 1013: 8, 1014: 8, 1015: 8, 1024: 8, 1025: 8, 1026: 8, 1031: 8, 1033: 8, 1050: 8, 1059: 8, 1082: 8, 1083: 8, 1098: 8, 1100: 8}, # based on 9ae7821dc4e92455|2019-07-01--16-42-55 {168: 8, 257: 5, 258: 8, 264: 8, 268: 8, 270: 8, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 291: 8, 292: 8, 294: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 344: 8, 368: 8, 376: 3, 384: 8, 388: 4, 448: 6, 456: 4, 464: 8, 469: 8, 480: 8, 500: 8, 501: 8, 512: 8, 514: 8, 515: 7, 516: 7, 517: 7, 518: 7, 520: 8, 528: 8, 532: 8, 542: 8, 544: 8, 557: 8, 559: 8, 560: 4, 564: 8, 571: 3, 579: 8, 584: 8, 608: 8, 624: 8, 625: 8, 632: 8, 639: 8, 653: 8, 654: 8, 655: 8, 658: 6, 660: 8, 669: 3, 671: 8, 672: 8, 678: 8, 680: 8, 701: 8, 704: 8, 705: 8, 706: 8, 709: 8, 710: 8, 719: 8, 720: 6, 729: 5, 736: 8, 737: 8, 746: 5, 760: 8, 764: 8, 766: 8, 770: 8, 773: 8, 779: 8, 782: 8, 784: 8, 792: 8, 799: 8, 800: 8, 804: 8, 808: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 832: 8, 838: 2, 848: 8, 853: 8, 856: 4, 860: 6, 863: 8, 878: 8, 882: 8, 897: 8, 908: 8, 924: 8, 926: 3, 929: 8, 937: 8, 938: 8, 939: 8, 940: 8, 941: 8, 942: 8, 943: 8, 947: 8, 948: 8, 958: 8, 959: 8, 969: 4, 974: 5, 979: 8, 980: 8, 981: 8, 982: 8, 983: 8, 984: 8, 992: 8, 993: 7, 995: 8, 996: 8, 1000: 8, 1001: 8, 1002: 8, 1003: 8, 1008: 8, 1009: 8, 1010: 8, 1011: 8, 1012: 8, 1013: 8, 1014: 8, 1015: 8, 1024: 8, 1025: 8, 1026: 8, 1031: 8, 1033: 8, 1050: 8, 1059: 8, 1082: 8, 1083: 8, 1098: 8, 1100: 8, 1216: 8, 1218: 8, 1220: 8, 1225: 8, 1235: 8, 1242: 8, 1246: 8, 1250: 8, 1251: 8, 1252: 8, 1258: 8, 1259: 8, 1260: 8, 1262: 8, 1284: 8, 1537: 8, 1538: 8, 1562: 8, 1568: 8, 1856: 8, 1858: 8, 1860: 8, 1865: 8, 1875: 8, 1882: 8, 1886: 8, 1890: 8, 1891: 8, 1892: 8, 1898: 8, 1899: 8, 1900: 8, 1902: 8, 2016: 8, 2018: 8, 2019: 8, 2020: 8, 2023: 8, 2024: 8, 2026: 8, 2027: 8, 2028: 8, 2031: 8}, ], CAR.PACIFICA_2019_HYBRID: [ - {168: 8, 257: 5, 258: 8, 264: 8, 268: 8, 270: 8, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 291: 8, 292: 8, 294: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 344: 8, 368: 8, 376: 3, 384: 8, 388: 4, 448: 6, 456: 4, 464: 8, 469: 8, 480: 8, 500: 8, 501: 8, 512: 8, 514: 8, 515: 7, 516: 7, 517: 7, 518: 7, 520: 8, 528: 8, 532: 8, 542: 8, 544: 8, 557: 8, 559: 8, 560: 8, 564: 8, 571: 3, 579: 8, 584: 8, 608: 8, 624: 8, 625: 8, 632: 8, 639: 8, 653: 8, 654: 8, 655: 8, 660: 8, 669: 3, 671: 8, 672: 8, 680: 8, 701: 8, 703: 8, 704: 8, 705: 8, 706: 8, 709: 8, 710: 8, 719: 8, 720: 6, 736: 8, 737: 8, 746: 5, 752: 2, 754: 8, 760: 8, 764: 8, 766: 8, 770:8, 773: 8, 779: 8, 782: 8, 784: 8, 792: 8, 799: 8, 800: 8, 804: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 832: 8, 838: 2, 848: 8, 853: 8, 856: 4, 860: 6, 863: 8, 878: 8, 882: 8, 897: 8, 906: 8, 908: 8, 924: 8, 926: 3, 929: 8, 937: 8, 938: 8, 939: 8, 940: 8, 941: 8, 942: 8, 943: 8, 947: 8, 948: 8, 958: 8, 959: 8, 962: 8, 969: 4, 973: 8, 974: 5, 979: 8, 980: 8, 981: 8, 982: 8, 983: 8, 984: 8, 992: 8, 993: 7, 995: 8, 996: 8, 1000: 8, 1001: 8, 1002: 8, 1003: 8, 1008: 8, 1009: 8, 1010: 8, 1011: 8, 1012: 8, 1013: 8, 1014: 8, 1015: 8, 1024: 8, 1025: 8, 1026: 8, 1031: 8, 1033: 8, 1050: 8, 1059: 8, 1082: 8, 1083: 8, 1098: 8, 1100: 8, 1538: 8}, + {168: 8, 257: 5, 258: 8, 264: 8, 268: 8, 270: 8, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 291: 8, 292: 8, 294: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 344: 8, 368: 8, 376: 3, 384: 8, 388: 4, 448: 6, 456: 4, 464: 8, 469: 8, 480: 8, 500: 8, 501: 8, 512: 8, 514: 8, 515: 7, 516: 7, 517: 7, 518: 7, 520: 8, 528: 8, 532: 8, 542: 8, 544: 8, 557: 8, 559: 8, 560: 8, 564: 8, 571: 3, 579: 8, 584: 8, 608: 8, 624: 8, 625: 8, 632: 8, 639: 8, 653: 8, 654: 8, 655: 8, 658: 6, 660: 8, 669: 3, 671: 8, 672: 8, 680: 8, 701: 8, 703: 8, 704: 8, 705: 8, 706: 8, 709: 8, 710: 8, 719: 8, 720: 6, 736: 8, 737: 8, 746: 5, 752: 2, 754: 8, 760: 8, 764: 8, 766: 8, 770:8, 773: 8, 779: 8, 782: 8, 784: 8, 792: 8, 799: 8, 800: 8, 804: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 832: 8, 838: 2, 848: 8, 853: 8, 856: 4, 860: 6, 863: 8, 878: 8, 882: 8, 897: 8, 906: 8, 908: 8, 924: 8, 926: 3, 929: 8, 937: 8, 938: 8, 939: 8, 940: 8, 941: 8, 942: 8, 943: 8, 947: 8, 948: 8, 958: 8, 959: 8, 962: 8, 969: 4, 973: 8, 974: 5, 979: 8, 980: 8, 981: 8, 982: 8, 983: 8, 984: 8, 992: 8, 993: 7, 995: 8, 996: 8, 1000: 8, 1001: 8, 1002: 8, 1003: 8, 1008: 8, 1009: 8, 1010: 8, 1011: 8, 1012: 8, 1013: 8, 1014: 8, 1015: 8, 1024: 8, 1025: 8, 1026: 8, 1031: 8, 1033: 8, 1050: 8, 1059: 8, 1082: 8, 1083: 8, 1098: 8, 1100: 8, 1538: 8}, # Based on 0607d2516fc2148f|2019-02-13--23-03-16 { 168: 8, 257: 5, 258: 8, 264: 8, 268: 8, 270: 8, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 291: 8, 292: 8, 294: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 344: 8, 368: 8, 376: 3, 384: 8, 388: 4, 448: 6, 456: 4, 464: 8, 469: 8, 480: 8, 500: 8, 501: 8, 512: 8, 514: 8, 520: 8, 528: 8, 532: 8, 544: 8, 557: 8, 559: 8, 560: 8, 564: 8, 571: 3, 579: 8, 584: 8, 608: 8, 624: 8, 625: 8, 632: 8, 639: 8, 653: 8, 654: 8, 655: 8, 658: 6, 660: 8, 669: 3, 671: 8, 672: 8, 678: 8, 680: 8, 701: 8, 703: 8, 704: 8, 705: 8, 706: 8, 709: 8, 710: 8, 719: 8, 720: 6, 729: 5, 736: 8, 737: 8, 746: 5, 752: 2, 754: 8, 760: 8, 764: 8, 766: 8, 770: 8, 773: 8, 779: 8, 782: 8, 784: 8, 792: 8, 799: 8, 800: 8, 804: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 832: 8, 838: 2, 848: 8, 853: 8, 856: 4, 860: 6, 863: 8, 878: 8, 882: 8, 897: 8, 906: 8, 908: 8, 924: 8, 926: 3, 929: 8, 937: 8, 938: 8, 939: 8, 940: 8, 941: 8, 942: 8, 943: 8, 947: 8, 948: 8, 958: 8, 959: 8, 962: 8, 969: 4, 973: 8, 974: 5, 979: 8, 980: 8, 981: 8, 982: 8, 983: 8, 984: 8, 992: 8, 993: 7, 995: 8, 996: 8, 1000: 8, 1001: 8, 1002: 8, 1003: 8, 1008: 8, 1009: 8, 1010: 8, 1011: 8, 1012: 8, 1013: 8, 1014: 8, 1015: 8, 1024: 8, 1025: 8, 1026: 8, 1031: 8, 1033: 8, 1050: 8, 1059: 8, 1082: 8, 1083: 8, 1098: 8, 1100: 8, 1537: 8 @@ -46,12 +47,12 @@ FINGERPRINTS = { # JEEP GRAND CHEROKEE V6 2018 {55: 8, 168: 8, 181: 8, 256: 4, 257: 5, 258: 8, 264: 8, 268: 8, 272: 6, 273: 6, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 292: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 344: 8, 352: 8, 362: 8, 368: 8, 376: 3, 384: 8, 388: 4, 416: 7, 448: 6, 456: 4, 464: 8, 500: 8, 501: 8, 512: 8, 514: 8, 520: 8, 532: 8, 544: 8, 557: 8, 559: 8, 560: 4, 564: 4, 571: 3, 579: 8, 584: 8, 608: 8, 618: 8, 624: 8, 625: 8, 632: 8, 639: 8, 656: 4, 658: 6, 660: 8, 671: 8, 672: 8, 676: 8, 678: 8, 680: 8, 683: 8, 684: 8, 703: 8, 705: 8, 706: 8, 709: 8, 710: 8, 719: 8, 720: 6, 729: 5, 736: 8, 737: 8, 738: 8, 746: 5, 752: 2, 754: 8, 760: 8, 761: 8, 764: 8, 766: 8, 773: 8, 776: 8, 779: 8, 782: 8, 783: 8, 784: 8, 785: 8, 788: 3, 792: 8, 799: 8, 800: 8, 804: 8, 806: 2, 808: 8, 810: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 831: 6, 832: 8, 838: 2, 844: 5, 848: 8, 853: 8, 856: 4, 860: 6, 863: 8, 882: 8, 897: 8, 906: 8, 924: 8, 937: 8, 938: 8, 939: 8, 940: 8, 941: 8, 942: 8, 943: 8, 947: 8, 948: 8, 956: 8, 968: 8, 969: 4, 970: 8, 973: 8, 974: 5, 976: 8, 977: 4, 979: 8, 980: 8, 981: 8, 982: 8, 983: 8, 984: 8, 992: 8, 993: 7, 995: 8, 996: 8, 1000: 8, 1001: 8, 1002: 8, 1003: 8, 1008: 8, 1009: 8, 1010: 8, 1011: 8, 1012: 8, 1013: 8, 1014: 8, 1015: 8, 1024: 8, 1025: 8, 1026: 8, 1031: 8, 1033: 8, 1050: 8, 1059: 8, 1062: 8, 1098: 8, 1100: 8}, # Jeep Grand Cherokee 2017 Trailhawk - {257: 5, 258: 8, 264: 8, 268: 8, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 292: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 344: 8, 352: 8, 362: 8, 368: 8, 376: 3, 384: 8, 388: 4, 416: 7, 448: 6, 456: 4, 464: 8, 500: 8, 501: 8, 512: 8, 514: 8, 520: 8, 532: 8, 544: 8, 557: 8, 559: 8, 560: 4, 564: 4, 571: 3, 584: 8, 608: 8, 618: 8, 624: 8, 625: 8, 632: 8, 639: 8, 660: 8, 671: 8, 672: 8, 680: 8, 684: 8, 703: 8, 705: 8, 706: 8, 709: 8, 710: 8, 719: 8, 720: 6, 736: 8, 737: 8, 746: 5, 752: 2, 760: 8, 761: 8, 764: 8, 766: 8, 773: 8, 776: 8, 779: 8, 783: 8, 784: 8, 792: 8, 799: 8, 800: 8, 804: 8, 806: 2, 808: 8, 810: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 831: 6, 832: 8, 838: 2, 844: 5, 848: 8, 853: 8, 856: 4, 860: 6, 863: 8, 882: 8, 897: 8, 924: 3, 937: 8, 947: 8, 948: 8, 969: 4, 974: 5, 977: 4, 979: 8, 980: 8, 981: 8, 982: 8, 983: 8, 984: 8, 992: 8, 993: 7, 995: 8, 996: 8, 1000: 8, 1001: 8, 1002: 8, 1003: 8, 1008: 8, 1009: 8, 1010: 8, 1011: 8, 1012: 8, 1013: 8, 1014: 8, 1015: 8, 1024: 8, 1025: 8, 1026: 8, 1031: 8, 1033: 8, 1050: 8, 1059: 8, 1062: 8, 1098: 8, 1100: 8}, + {257: 5, 258: 8, 264: 8, 268: 8, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 292: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 344: 8, 352: 8, 362: 8, 368: 8, 376: 3, 384: 8, 388: 4, 416: 7, 448: 6, 456: 4, 464: 8, 500: 8, 501: 8, 512: 8, 514: 8, 520: 8, 532: 8, 544: 8, 557: 8, 559: 8, 560: 4, 564: 4, 571: 3, 584: 8, 608: 8, 618: 8, 624: 8, 625: 8, 632: 8, 639: 8, 658: 6, 660: 8, 671: 8, 672: 8, 680: 8, 684: 8, 703: 8, 705: 8, 706: 8, 709: 8, 710: 8, 719: 8, 720: 6, 736: 8, 737: 8, 746: 5, 752: 2, 760: 8, 761: 8, 764: 8, 766: 8, 773: 8, 776: 8, 779: 8, 783: 8, 784: 8, 792: 8, 799: 8, 800: 8, 804: 8, 806: 2, 808: 8, 810: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 831: 6, 832: 8, 838: 2, 844: 5, 848: 8, 853: 8, 856: 4, 860: 6, 863: 8, 882: 8, 897: 8, 924: 3, 937: 8, 947: 8, 948: 8, 969: 4, 974: 5, 977: 4, 979: 8, 980: 8, 981: 8, 982: 8, 983: 8, 984: 8, 992: 8, 993: 7, 995: 8, 996: 8, 1000: 8, 1001: 8, 1002: 8, 1003: 8, 1008: 8, 1009: 8, 1010: 8, 1011: 8, 1012: 8, 1013: 8, 1014: 8, 1015: 8, 1024: 8, 1025: 8, 1026: 8, 1031: 8, 1033: 8, 1050: 8, 1059: 8, 1062: 8, 1098: 8, 1100: 8}, ], CAR.JEEP_CHEROKEE_2019: [ # Jeep Grand Cherokee 2019 from Switzerland # 530: 8 is so far only in this Jeep. - {55: 8, 181: 8, 256: 4, 257: 5, 258: 8, 264: 8, 268: 8, 272: 6, 273: 6, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 292: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 344: 8, 352: 8, 362: 8, 368: 8, 376: 3, 384: 8, 388: 4, 416: 7, 448: 6, 456: 4, 464: 8, 500: 8, 501: 8, 512: 8, 514: 8, 520: 8, 530: 8, 532: 8, 544: 8, 557: 8, 559: 8, 560: 8, 564: 8, 571: 3, 579: 8, 584: 8, 608: 8, 618: 8, 624: 8, 625: 8, 632: 8, 639: 8, 660: 8, 671: 8, 672: 8, 676: 8, 680: 8, 683: 8, 684: 8, 703: 8, 705: 8, 706: 8, 709: 8, 710: 8, 719: 8, 720: 6, 736: 8, 737: 8, 738: 8, 746: 5, 752: 2, 754: 8, 760: 8, 761: 8, 764: 8, 773: 8, 776: 8, 779: 8, 782: 8, 783: 8, 784: 8, 792: 8, 799: 8, 804: 8, 806: 2, 808: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 831: 6, 832: 8, 838: 2, 844: 5, 848: 8, 853: 8, 856: 4, 860: 6, 882: 8, 897: 8, 906: 8, 924: 8, 937: 8, 938: 8, 939: 8, 940: 8, 941: 8, 942: 8, 943: 8, 947: 8, 948: 8, 968: 8, 969: 4, 970: 8, 973: 8, 974: 5, 977: 4, 979: 8, 980: 8, 981: 8, 982: 8, 983: 8, 984: 8, 992: 8, 993: 7, 995: 8, 996: 8, 1000: 8, 1001: 8, 1002: 8, 1003: 8, 1008: 8, 1009: 8, 1010: 8, 1011: 8, 1012: 8, 1013: 8, 1014: 8, 1015: 8, 1024: 8, 1025: 8, 1026: 8, 1031: 8, 1033: 8, 1050: 8, 1059: 8, 1062: 8, 1098: 8, 1100: 8}, + {55: 8, 181: 8, 256: 4, 257: 5, 258: 8, 264: 8, 268: 8, 272: 6, 273: 6, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 292: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 344: 8, 352: 8, 362: 8, 368: 8, 376: 3, 384: 8, 388: 4, 416: 7, 448: 6, 456: 4, 464: 8, 500: 8, 501: 8, 512: 8, 514: 8, 520: 8, 530: 8, 532: 8, 544: 8, 557: 8, 559: 8, 560: 8, 564: 8, 571: 3, 579: 8, 584: 8, 608: 8, 618: 8, 624: 8, 625: 8, 632: 8, 639: 8, 658: 6, 660: 8, 671: 8, 672: 8, 676: 8, 680: 8, 683: 8, 684: 8, 703: 8, 705: 8, 706: 8, 709: 8, 710: 8, 719: 8, 720: 6, 736: 8, 737: 8, 738: 8, 746: 5, 752: 2, 754: 8, 760: 8, 761: 8, 764: 8, 773: 8, 776: 8, 779: 8, 782: 8, 783: 8, 784: 8, 792: 8, 799: 8, 804: 8, 806: 2, 808: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 831: 6, 832: 8, 838: 2, 844: 5, 848: 8, 853: 8, 856: 4, 860: 6, 882: 8, 897: 8, 906: 8, 924: 8, 937: 8, 938: 8, 939: 8, 940: 8, 941: 8, 942: 8, 943: 8, 947: 8, 948: 8, 968: 8, 969: 4, 970: 8, 973: 8, 974: 5, 977: 4, 979: 8, 980: 8, 981: 8, 982: 8, 983: 8, 984: 8, 992: 8, 993: 7, 995: 8, 996: 8, 1000: 8, 1001: 8, 1002: 8, 1003: 8, 1008: 8, 1009: 8, 1010: 8, 1011: 8, 1012: 8, 1013: 8, 1014: 8, 1015: 8, 1024: 8, 1025: 8, 1026: 8, 1031: 8, 1033: 8, 1050: 8, 1059: 8, 1062: 8, 1098: 8, 1100: 8}, ], } @@ -85,10 +86,5 @@ class ECU: ECU_FINGERPRINT = { - ECU.CAM: 0x2d9, # steer torque cmd + ECU.CAM: [0x292], # lkas cmd } - - -def check_ecu_msgs(fingerprint, ecu): - # return True if fingerprint contains messages normally sent by a given ecu - return ECU_FINGERPRINT[ecu] in fingerprint diff --git a/common/fingerprints.py b/selfdrive/car/fingerprints.py similarity index 100% rename from common/fingerprints.py rename to selfdrive/car/fingerprints.py diff --git a/selfdrive/car/ford/carcontroller.py b/selfdrive/car/ford/carcontroller.py index 3566fd5e75..b466f03ea0 100644 --- a/selfdrive/car/ford/carcontroller.py +++ b/selfdrive/car/ford/carcontroller.py @@ -7,7 +7,7 @@ from selfdrive.can.packer import CANPacker MAX_STEER_DELTA = 1 TOGGLE_DEBUG = False -class CarController(object): +class CarController(): def __init__(self, dbc_name, enable_camera, vehicle_model): self.packer = CANPacker(dbc_name) self.enable_camera = enable_camera @@ -48,7 +48,7 @@ class CarController(object): if (frame % 100) == 0: - can_sends.append(make_can_msg(973, '\x00\x00\x00\x00\x00\x00\x00\x00', 0, False)) + can_sends.append(make_can_msg(973, b'\x00\x00\x00\x00\x00\x00\x00\x00', 0, False)) #can_sends.append(make_can_msg(984, '\x00\x00\x00\x00\x80\x45\x60\x30', 0, False)) if (frame % 100) == 0 or (self.enabled_last != enabled) or (self.main_on_last != CS.main_on) or \ @@ -56,29 +56,29 @@ class CarController(object): can_sends.append(create_lkas_ui(self.packer, CS.main_on, enabled, steer_alert)) if (frame % 200) == 0: - can_sends.append(make_can_msg(1875, '\x80\xb0\x55\x55\x78\x90\x00\x00', 1, False)) + can_sends.append(make_can_msg(1875, b'\x80\xb0\x55\x55\x78\x90\x00\x00', 1, False)) if (frame % 10) == 0: - can_sends.append(make_can_msg(1648, '\x00\x00\x00\x40\x00\x00\x50\x00', 1, False)) - can_sends.append(make_can_msg(1649, '\x10\x10\xf1\x70\x04\x00\x00\x00', 1, False)) + can_sends.append(make_can_msg(1648, b'\x00\x00\x00\x40\x00\x00\x50\x00', 1, False)) + can_sends.append(make_can_msg(1649, b'\x10\x10\xf1\x70\x04\x00\x00\x00', 1, False)) - can_sends.append(make_can_msg(1664, '\x00\x00\x03\xe8\x00\x01\xa9\xb2', 1, False)) - can_sends.append(make_can_msg(1674, '\x08\x00\x00\xff\x0c\xfb\x6a\x08', 1, False)) - can_sends.append(make_can_msg(1675, '\x00\x00\x3b\x60\x37\x00\x00\x00', 1, False)) - can_sends.append(make_can_msg(1690, '\x70\x00\x00\x55\x86\x1c\xe0\x00', 1, False)) + can_sends.append(make_can_msg(1664, b'\x00\x00\x03\xe8\x00\x01\xa9\xb2', 1, False)) + can_sends.append(make_can_msg(1674, b'\x08\x00\x00\xff\x0c\xfb\x6a\x08', 1, False)) + can_sends.append(make_can_msg(1675, b'\x00\x00\x3b\x60\x37\x00\x00\x00', 1, False)) + can_sends.append(make_can_msg(1690, b'\x70\x00\x00\x55\x86\x1c\xe0\x00', 1, False)) - can_sends.append(make_can_msg(1910, '\x06\x4b\x06\x4b\x42\xd3\x11\x30', 1, False)) - can_sends.append(make_can_msg(1911, '\x48\x53\x37\x54\x48\x53\x37\x54', 1, False)) - can_sends.append(make_can_msg(1912, '\x31\x34\x47\x30\x38\x31\x43\x42', 1, False)) - can_sends.append(make_can_msg(1913, '\x31\x34\x47\x30\x38\x32\x43\x42', 1, False)) - can_sends.append(make_can_msg(1969, '\xf4\x40\x00\x00\x00\x00\x00\x00', 1, False)) - can_sends.append(make_can_msg(1971, '\x0b\xc0\x00\x00\x00\x00\x00\x00', 1, False)) + can_sends.append(make_can_msg(1910, b'\x06\x4b\x06\x4b\x42\xd3\x11\x30', 1, False)) + can_sends.append(make_can_msg(1911, b'\x48\x53\x37\x54\x48\x53\x37\x54', 1, False)) + can_sends.append(make_can_msg(1912, b'\x31\x34\x47\x30\x38\x31\x43\x42', 1, False)) + can_sends.append(make_can_msg(1913, b'\x31\x34\x47\x30\x38\x32\x43\x42', 1, False)) + can_sends.append(make_can_msg(1969, b'\xf4\x40\x00\x00\x00\x00\x00\x00', 1, False)) + can_sends.append(make_can_msg(1971, b'\x0b\xc0\x00\x00\x00\x00\x00\x00', 1, False)) static_msgs = range(1653, 1658) for addr in static_msgs: cnt = (frame % 10) + 1 - can_sends.append(make_can_msg(addr, chr(cnt<<4) + '\x00\x00\x00\x00\x00\x00\x00', 1, False)) + can_sends.append(make_can_msg(addr, (cnt<<4).to_bytes(1, 'little') + b'\x00\x00\x00\x00\x00\x00\x00', 1, False)) self.enabled_last = enabled self.main_on_last = CS.main_on diff --git a/selfdrive/car/ford/carstate.py b/selfdrive/car/ford/carstate.py index 5e87a2c878..b6ddf45259 100644 --- a/selfdrive/car/ford/carstate.py +++ b/selfdrive/car/ford/carstate.py @@ -1,8 +1,8 @@ from selfdrive.can.parser import CANParser +from common.numpy_fast import mean from selfdrive.config import Conversions as CV from selfdrive.car.ford.values import DBC from common.kalman.simple_kalman import KF1D -import numpy as np WHEEL_RADIUS = 0.33 @@ -32,7 +32,7 @@ def get_can_parser(CP): return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 0) -class CarState(object): +class CarState(): def __init__(self, CP): self.CP = CP @@ -62,7 +62,7 @@ class CarState(object): self.v_wheel_fr = cp.vl["WheelSpeed_CG1"]['WhlRl_W_Meas'] * WHEEL_RADIUS self.v_wheel_rl = cp.vl["WheelSpeed_CG1"]['WhlFr_W_Meas'] * WHEEL_RADIUS self.v_wheel_rr = cp.vl["WheelSpeed_CG1"]['WhlFl_W_Meas'] * WHEEL_RADIUS - v_wheel = float(np.mean([self.v_wheel_fl, self.v_wheel_fr, self.v_wheel_rl, self.v_wheel_rr])) + v_wheel = mean([self.v_wheel_fl, self.v_wheel_fr, self.v_wheel_rl, self.v_wheel_rr]) # Kalman filter if abs(v_wheel - self.v_ego) > 2.0: # Prevent large accelerations when car starts at non zero speed diff --git a/selfdrive/car/ford/interface.py b/selfdrive/car/ford/interface.py index efc7301f36..a05144c133 100755 --- a/selfdrive/car/ford/interface.py +++ b/selfdrive/car/ford/interface.py @@ -1,15 +1,16 @@ -#!/usr/bin/env python +#!/usr/bin/env python3 from cereal import car from selfdrive.swaglog import cloudlog from selfdrive.config import Conversions as CV from selfdrive.controls.lib.drive_helpers import EventTypes as ET, create_event from selfdrive.controls.lib.vehicle_model import VehicleModel from selfdrive.car.ford.carstate import CarState, get_can_parser -from selfdrive.car.ford.values import MAX_ANGLE -from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness +from selfdrive.car.ford.values import MAX_ANGLE, ECU, ECU_FINGERPRINT, FINGERPRINTS +from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, is_ecu_disconnected, gen_empty_fingerprint +from selfdrive.car.interfaces import CarInterfaceBase -class CarInterface(object): +class CarInterface(CarInterfaceBase): def __init__(self, CP, CarController): self.CP = CP self.VM = VehicleModel(CP) @@ -33,18 +34,14 @@ class CarInterface(object): return float(accel) / 3.0 @staticmethod - def calc_accel_override(a_ego, a_target, v_ego, v_target): - return 1.0 - - @staticmethod - def get_params(candidate, fingerprint, vin="", is_panda_black=False): + def get_params(candidate, fingerprint=gen_empty_fingerprint(), vin="", has_relay=False): ret = car.CarParams.new_message() ret.carName = "ford" ret.carFingerprint = candidate ret.carVin = vin - ret.isPandaBlack = is_panda_black + ret.isPandaBlack = has_relay ret.safetyModel = car.CarParams.SafetyModel.ford ret.dashcamOnly = True @@ -88,9 +85,9 @@ class CarInterface(object): ret.brakeMaxBP = [5., 20.] ret.brakeMaxV = [1., 0.8] - ret.enableCamera = not any(x for x in [970, 973, 984] if x in fingerprint) or is_panda_black + ret.enableCamera = is_ecu_disconnected(fingerprint[0], FINGERPRINTS, ECU_FINGERPRINT, candidate, ECU.CAM) or has_relay ret.openpilotLongitudinalControl = False - cloudlog.warn("ECU Camera Simulated: %r", ret.enableCamera) + cloudlog.warning("ECU Camera Simulated: %r", ret.enableCamera) ret.steerLimitAlert = False ret.stoppingControl = False diff --git a/selfdrive/car/ford/radar_interface.py b/selfdrive/car/ford/radar_interface.py index be408da03f..2538a185de 100755 --- a/selfdrive/car/ford/radar_interface.py +++ b/selfdrive/car/ford/radar_interface.py @@ -1,32 +1,32 @@ -#!/usr/bin/env python -import os -import numpy as np -from selfdrive.can.parser import CANParser +#!/usr/bin/env python3 from cereal import car +from selfdrive.can.parser import CANParser +from selfdrive.car.ford.values import DBC +from selfdrive.config import Conversions as CV +from selfdrive.car.interfaces import RadarInterfaceBase -RADAR_MSGS = range(0x500, 0x540) +RADAR_MSGS = list(range(0x500, 0x540)) -def _create_radar_can_parser(): - dbc_f = 'ford_fusion_2018_adas.dbc' +def _create_radar_can_parser(car_fingerprint): + dbc_f = DBC[car_fingerprint]['radar'] msg_n = len(RADAR_MSGS) signals = list(zip(['X_Rel'] * msg_n + ['Angle'] * msg_n + ['V_Rel'] * msg_n, RADAR_MSGS * 3, [0] * msg_n + [0] * msg_n + [0] * msg_n)) checks = list(zip(RADAR_MSGS, [20]*msg_n)) - return CANParser(os.path.splitext(dbc_f)[0], signals, checks, 1) + return CANParser(dbc_f, signals, checks, 1) -class RadarInterface(object): +class RadarInterface(RadarInterfaceBase): def __init__(self, CP): # radar self.pts = {} self.validCnt = {key: 0 for key in RADAR_MSGS} self.track_id = 0 - self.delay = 0.0 # Delay of radar + self.delay = 0 # Delay of radar - # Nidec - self.rcp = _create_radar_can_parser() + self.rcp = _create_radar_can_parser(CP.carFingerprint) self.trigger_msg = 0x53f self.updated_messages = set() @@ -44,7 +44,7 @@ class RadarInterface(object): errors.append("canError") ret.errors = errors - for ii in self.updated_messages: + for ii in sorted(self.updated_messages): cpt = self.rcp.vl[ii] if cpt['X_Rel'] > 0.00001: @@ -63,7 +63,7 @@ class RadarInterface(object): self.pts[ii].trackId = self.track_id self.track_id += 1 self.pts[ii].dRel = cpt['X_Rel'] # from front of car - self.pts[ii].yRel = cpt['X_Rel'] * cpt['Angle'] * np.pi / 180. # in car frame's y axis, left is positive + self.pts[ii].yRel = cpt['X_Rel'] * cpt['Angle'] * CV.DEG_TO_RAD # in car frame's y axis, left is positive self.pts[ii].vRel = cpt['V_Rel'] self.pts[ii].aRel = float('nan') self.pts[ii].yvRel = float('nan') @@ -72,6 +72,6 @@ class RadarInterface(object): if ii in self.pts: del self.pts[ii] - ret.points = self.pts.values() + ret.points = list(self.pts.values()) self.updated_messages.clear() return ret diff --git a/selfdrive/car/ford/values.py b/selfdrive/car/ford/values.py index 6ab0b3750b..a3fb576a3b 100644 --- a/selfdrive/car/ford/values.py +++ b/selfdrive/car/ford/values.py @@ -11,6 +11,13 @@ FINGERPRINTS = { }], } +class ECU: + CAM = 0 + +ECU_FINGERPRINT = { + ECU.CAM: [970, 973, 984] +} + DBC = { CAR.FUSION: dbc_dict('ford_fusion_2018_pt', 'ford_fusion_2018_adas'), } diff --git a/selfdrive/car/gm/carcontroller.py b/selfdrive/car/gm/carcontroller.py index da820864f0..c4647c8228 100644 --- a/selfdrive/car/gm/carcontroller.py +++ b/selfdrive/car/gm/carcontroller.py @@ -68,7 +68,7 @@ def process_hud_alert(hud_alert): steer = 1 return steer -class CarController(object): +class CarController(): def __init__(self, canbus, car_fingerprint): self.pedal_steady = 0. self.start_time = 0. diff --git a/selfdrive/car/gm/carstate.py b/selfdrive/car/gm/carstate.py index 2501598dd6..7da8d65272 100644 --- a/selfdrive/car/gm/carstate.py +++ b/selfdrive/car/gm/carstate.py @@ -1,5 +1,5 @@ -import numpy as np from cereal import car +from common.numpy_fast import mean from common.kalman.simple_kalman import KF1D from selfdrive.config import Conversions as CV from selfdrive.can.parser import CANParser @@ -50,7 +50,7 @@ def get_powertrain_can_parser(CP, canbus): return CANParser(DBC[CP.carFingerprint]['pt'], signals, [], canbus.powertrain) -class CarState(object): +class CarState(): def __init__(self, CP, canbus): self.CP = CP # initialize can parser @@ -78,7 +78,7 @@ class CarState(object): self.v_wheel_fr = pt_cp.vl["EBCMWheelSpdFront"]['FRWheelSpd'] * CV.KPH_TO_MS self.v_wheel_rl = pt_cp.vl["EBCMWheelSpdRear"]['RLWheelSpd'] * CV.KPH_TO_MS self.v_wheel_rr = pt_cp.vl["EBCMWheelSpdRear"]['RRWheelSpd'] * CV.KPH_TO_MS - v_wheel = float(np.mean([self.v_wheel_fl, self.v_wheel_fr, self.v_wheel_rl, self.v_wheel_rr])) + v_wheel = mean([self.v_wheel_fl, self.v_wheel_fr, self.v_wheel_rl, self.v_wheel_rr]) if abs(v_wheel - self.v_ego) > 2.0: # Prevent large accelerations when car starts at non zero speed self.v_ego_kf.x = [[v_wheel], [0.0]] diff --git a/selfdrive/car/gm/gmcan.py b/selfdrive/car/gm/gmcan.py index 6919de3bd1..ada05658ba 100644 --- a/selfdrive/car/gm/gmcan.py +++ b/selfdrive/car/gm/gmcan.py @@ -24,7 +24,7 @@ def create_steering_control_ct6(packer, canbus, apply_steer, v_ego, idx, enabled dat = packer.make_can_msg("ASCMLKASteeringCmd", 0, values)[2] # the checksum logic is weird values['LKASteeringCmdChecksum'] = (0x2a + - sum([ord(i) for i in dat][:4]) + + sum(dat[:4]) + values['LKASMode']) & 0x3ff # pack again with checksum dat = packer.make_can_msg("ASCMLKASteeringCmd", 0, values)[2] @@ -36,7 +36,7 @@ def create_steering_control_ct6(packer, canbus, apply_steer, v_ego, idx, enabled def create_adas_keepalive(bus): - dat = "\x00\x00\x00\x00\x00\x00\x00" + dat = b"\x00\x00\x00\x00\x00\x00\x00" return [[0x409, 0, dat, bus], [0x40a, 0, dat, bus]] def create_gas_regen_command(packer, bus, throttle, idx, acc_engaged, at_full_stop): @@ -52,9 +52,9 @@ def create_gas_regen_command(packer, bus, throttle, idx, acc_engaged, at_full_st } dat = packer.make_can_msg("ASCMGasRegenCmd", bus, values)[2] - values["GasRegenChecksum"] = (((0xff - ord(dat[1])) & 0xff) << 16) | \ - (((0xff - ord(dat[2])) & 0xff) << 8) | \ - ((0x100 - ord(dat[3]) - idx) & 0xff) + values["GasRegenChecksum"] = (((0xff -dat[1]) & 0xff) << 16) | \ + (((0xff - dat[2]) & 0xff) << 8) | \ + ((0x100 - dat[3] - idx) & 0xff) return packer.make_can_msg("ASCMGasRegenCmd", bus, values) @@ -106,13 +106,13 @@ def create_adas_time_status(bus, tt, idx): chksum = 0x1000 - dat[0] - dat[1] - dat[2] - dat[3] chksum = chksum & 0xfff dat += [0x40 + (chksum >> 8), chksum & 0xff, 0x12] - return [0xa1, 0, "".join(map(chr, dat)), bus] + return [0xa1, 0, bytes(dat), bus] def create_adas_steering_status(bus, idx): dat = [idx << 6, 0xf0, 0x20, 0, 0, 0] chksum = 0x60 + sum(dat) dat += [chksum >> 8, chksum & 0xff] - return [0x306, 0, "".join(map(chr, dat)), bus] + return [0x306, 0, bytes(dat), bus] def create_adas_accelerometer_speed_status(bus, speed_ms, idx): spd = int(speed_ms * 16) & 0xfff @@ -125,24 +125,24 @@ def create_adas_accelerometer_speed_status(bus, speed_ms, idx): dat = [0x08, spd >> 4, ((spd & 0xf) << 4) | (accel >> 8), accel & 0xff, 0] chksum = 0x62 + far_range_mode + (idx << 2) + dat[0] + dat[1] + dat[2] + dat[3] + dat[4] dat += [(idx << 5) + (far_range_mode << 4) + (near_range_mode << 3) + (chksum >> 8), chksum & 0xff] - return [0x308, 0, "".join(map(chr, dat)), bus] + return [0x308, 0, bytes(dat), bus] def create_adas_headlights_status(bus): - return [0x310, 0, "\x42\x04", bus] + return [0x310, 0, b"\x42\x04", bus] def create_lka_icon_command(bus, active, critical, steer): if active and steer == 1: if critical: - dat = "\x50\xc0\x14" + dat = b"\x50\xc0\x14" else: - dat = "\x50\x40\x18" + dat = b"\x50\x40\x18" elif active: if critical: - dat = "\x40\xc0\x14" + dat = b"\x40\xc0\x14" else: - dat = "\x40\x40\x18" + dat = b"\x40\x40\x18" else: - dat = "\x00\x00\x00" + dat = b"\x00\x00\x00" return [0x104c006c, 0, dat, bus] # TODO: WIP diff --git a/selfdrive/car/gm/interface.py b/selfdrive/car/gm/interface.py index b3e82e5e61..2390997bcb 100755 --- a/selfdrive/car/gm/interface.py +++ b/selfdrive/car/gm/interface.py @@ -1,23 +1,24 @@ -#!/usr/bin/env python +#!/usr/bin/env python3 from cereal import car from selfdrive.config import Conversions as CV from selfdrive.controls.lib.drive_helpers import create_event, EventTypes as ET from selfdrive.controls.lib.vehicle_model import VehicleModel -from selfdrive.car.gm.values import DBC, CAR, STOCK_CONTROL_MSGS, \ - SUPERCRUISE_CARS, AccState +from selfdrive.car.gm.values import DBC, CAR, ECU, ECU_FINGERPRINT, \ + SUPERCRUISE_CARS, AccState, FINGERPRINTS from selfdrive.car.gm.carstate import CarState, CruiseButtons, get_powertrain_can_parser -from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness +from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, is_ecu_disconnected, gen_empty_fingerprint +from selfdrive.car.interfaces import CarInterfaceBase ButtonType = car.CarState.ButtonEvent.Type -class CanBus(object): +class CanBus(CarInterfaceBase): def __init__(self): self.powertrain = 0 self.obstacle = 1 self.chassis = 2 self.sw_gmlan = 3 -class CarInterface(object): +class CarInterface(CarInterfaceBase): def __init__(self, CP, CarController): self.CP = CP @@ -42,24 +43,22 @@ class CarInterface(object): return float(accel) / 4.0 @staticmethod - def calc_accel_override(a_ego, a_target, v_ego, v_target): - return 1.0 - - @staticmethod - def get_params(candidate, fingerprint, vin="", is_panda_black=False): + def get_params(candidate, fingerprint=gen_empty_fingerprint(), vin="", has_relay=False): ret = car.CarParams.new_message() ret.carName = "gm" ret.carFingerprint = candidate ret.carVin = vin - ret.isPandaBlack = is_panda_black + ret.isPandaBlack = has_relay ret.enableCruise = False # Presence of a camera on the object bus is ok. # Have to go to read_only if ASCM is online (ACC-enabled cars), # or camera is on powertrain bus (LKA cars without ACC). - ret.enableCamera = not any(x for x in STOCK_CONTROL_MSGS[candidate] if x in fingerprint) or is_panda_black + ret.enableCamera = is_ecu_disconnected(fingerprint[0], FINGERPRINTS, ECU_FINGERPRINT, candidate, ECU.CAM) or \ + has_relay or \ + candidate == CAR.CADILLAC_CT6 ret.openpilotLongitudinalControl = ret.enableCamera tire_stiffness_factor = 0.444 # not optimized yet ret.safetyModelPassive = car.CarParams.SafetyModel.gmPassive diff --git a/selfdrive/car/gm/radar_interface.py b/selfdrive/car/gm/radar_interface.py index 4f54c82756..1fb39c5c43 100755 --- a/selfdrive/car/gm/radar_interface.py +++ b/selfdrive/car/gm/radar_interface.py @@ -1,11 +1,13 @@ -#!/usr/bin/env python +#!/usr/bin/env python3 +from __future__ import print_function import math import time -import numpy as np from cereal import car from selfdrive.can.parser import CANParser from selfdrive.car.gm.interface import CanBus from selfdrive.car.gm.values import DBC, CAR +from selfdrive.config import Conversions as CV +from selfdrive.car.interfaces import RadarInterfaceBase RADAR_HEADER_MSG = 1120 SLOT_1_MSG = RADAR_HEADER_MSG + 1 @@ -20,7 +22,7 @@ def create_radar_can_parser(canbus, car_fingerprint): dbc_f = DBC[car_fingerprint]['radar'] if car_fingerprint in (CAR.VOLT, CAR.MALIBU, CAR.HOLDEN_ASTRA, CAR.ACADIA, CAR.CADILLAC_ATS): # C1A-ARS3-A by Continental - radar_targets = range(SLOT_1_MSG, SLOT_1_MSG + NUM_SLOTS) + radar_targets = list(range(SLOT_1_MSG, SLOT_1_MSG + NUM_SLOTS)) signals = list(zip(['FLRRNumValidTargets', 'FLRRSnsrBlckd', 'FLRRYawRtPlsblityFlt', 'FLRRHWFltPrsntInt', 'FLRRAntTngFltPrsnt', @@ -40,15 +42,15 @@ def create_radar_can_parser(canbus, car_fingerprint): else: return None -class RadarInterface(object): +class RadarInterface(RadarInterfaceBase): def __init__(self, CP): # radar self.pts = {} - self.delay = 0.0 # Delay of radar + self.delay = 0 # Delay of radar canbus = CanBus() - print "Using %d as obstacle CAN bus ID" % canbus.obstacle + print("Using %d as obstacle CAN bus ID" % canbus.obstacle) self.rcp = create_radar_can_parser(canbus, CP.carFingerprint) self.trigger_msg = LAST_RADAR_MSG @@ -65,7 +67,6 @@ class RadarInterface(object): if self.trigger_msg not in self.updated_messages: return None - ret = car.RadarData.new_message() header = self.rcp.vl[RADAR_HEADER_MSG] fault = header['FLRRSnsrBlckd'] or header['FLRRSnstvFltPrsntInt'] or \ @@ -101,16 +102,15 @@ class RadarInterface(object): distance = cpt['TrkRange'] self.pts[targetId].dRel = distance # from front of car # From driver's pov, left is positive - deg_to_rad = np.pi/180. - self.pts[targetId].yRel = math.sin(deg_to_rad * cpt['TrkAzimuth']) * distance + self.pts[targetId].yRel = math.sin(cpt['TrkAzimuth'] * CV.DEG_TO_RAD) * distance self.pts[targetId].vRel = cpt['TrkRangeRate'] self.pts[targetId].aRel = float('nan') self.pts[targetId].yvRel = float('nan') - for oldTarget in self.pts.keys(): + for oldTarget in list(self.pts.keys()): if not oldTarget in currentTargets: del self.pts[oldTarget] - ret.points = self.pts.values() + ret.points = list(self.pts.values()) self.updated_messages.clear() return ret diff --git a/selfdrive/car/gm/values.py b/selfdrive/car/gm/values.py index 1aa5a64b13..126ae9e40b 100644 --- a/selfdrive/car/gm/values.py +++ b/selfdrive/car/gm/values.py @@ -48,12 +48,12 @@ def parse_gear_shifter(can_gear): FINGERPRINTS = { # Astra BK MY17, ASCM unplugged CAR.HOLDEN_ASTRA: [{ - 190: 8, 193: 8, 197: 8, 199: 4, 201: 8, 209: 7, 211: 8, 241: 6, 249: 8, 288: 5, 298: 8, 304: 1, 309: 8, 311: 8, 313: 8, 320: 3, 328: 1, 352: 5, 381: 6, 386: 8, 388: 8, 393: 8, 398: 8, 401: 8, 413: 8, 417: 8, 419: 8, 422: 1, 426: 7, 431: 8, 442: 8, 451: 8, 452: 8, 453: 8, 455: 7, 456: 8, 458: 5, 479: 8, 481: 7, 485: 8, 489: 8, 497: 8, 499: 3, 500: 8, 501: 8, 508: 8, 528: 5, 532: 6, 554: 3, 560: 8, 562: 8, 563: 5, 564: 5, 565: 5, 567: 5, 647: 5, 707: 8, 723: 8, 753: 5, 761: 7, 806: 1, 810: 8, 840: 5, 842: 5, 844: 8, 866: 4, 961: 8, 969: 8, 977: 8, 979: 8, 985: 5, 1001: 8, 1009: 8, 1011: 6, 1017: 8, 1019: 3, 1020: 8, 1105: 6, 1217: 8, 1221: 5, 1225: 8, 1233: 8, 1249: 8, 1257: 6, 1259: 8, 1261: 7, 1263: 4, 1265: 8, 1267: 8, 1280: 4, 1300: 8, 1328: 4, 1417: 8, 1906: 7, 1907: 7, 1908: 7, 1912: 7, 1919: 7, + 190: 8, 193: 8, 197: 8, 199: 4, 201: 8, 209: 7, 211: 8, 241: 6, 249: 8, 288: 5, 298: 8, 304: 1, 309: 8, 311: 8, 313: 8, 320: 3, 328: 1, 352: 5, 381: 6, 384: 4, 386: 8, 388: 8, 393: 8, 398: 8, 401: 8, 413: 8, 417: 8, 419: 8, 422: 1, 426: 7, 431: 8, 442: 8, 451: 8, 452: 8, 453: 8, 455: 7, 456: 8, 458: 5, 479: 8, 481: 7, 485: 8, 489: 8, 497: 8, 499: 3, 500: 8, 501: 8, 508: 8, 528: 5, 532: 6, 554: 3, 560: 8, 562: 8, 563: 5, 564: 5, 565: 5, 567: 5, 647: 5, 707: 8, 715: 8, 723: 8, 753: 5, 761: 7, 806: 1, 810: 8, 840: 5, 842: 5, 844: 8, 866: 4, 961: 8, 969: 8, 977: 8, 979: 8, 985: 5, 1001: 8, 1009: 8, 1011: 6, 1017: 8, 1019: 3, 1020: 8, 1105: 6, 1217: 8, 1221: 5, 1225: 8, 1233: 8, 1249: 8, 1257: 6, 1259: 8, 1261: 7, 1263: 4, 1265: 8, 1267: 8, 1280: 4, 1300: 8, 1328: 4, 1417: 8, 1906: 7, 1907: 7, 1908: 7, 1912: 7, 1919: 7, }], CAR.VOLT: [ # Volt Premier w/ ACC 2017 { - 170: 8, 171: 8, 189: 7, 190: 6, 193: 8, 197: 8, 199: 4, 201: 8, 209: 7, 211: 2, 241: 6, 288: 5, 289: 8, 298: 8, 304: 1, 308: 4, 309: 8, 311: 8, 313: 8, 320: 3, 328: 1, 352: 5, 381: 6, 386: 8, 388: 8, 389: 2, 390: 7, 417: 7, 419: 1, 426: 7, 451: 8, 452: 8, 453: 6, 454: 8, 456: 8, 479: 3, 481: 7, 485: 8, 489: 8, 493: 8, 495: 4, 497: 8, 499: 3, 500: 6, 501: 8, 508: 8, 528: 4, 532: 6, 546: 7, 550: 8, 554: 3, 558: 8, 560: 8, 562: 8, 563: 5, 564: 5, 565: 5, 566: 5, 567: 3, 568: 1, 573: 1, 577: 8, 647: 3, 707: 8, 711: 6, 761: 7, 810: 8, 840: 5, 842: 5, 844: 8, 866: 4, 961: 8, 969: 8, 977: 8, 979: 7, 988: 6, 989: 8, 995: 7, 1001: 8, 1005: 6, 1009: 8, 1017: 8, 1019: 2, 1020: 8, 1105: 6, 1187: 4, 1217: 8, 1221: 5, 1223: 3, 1225: 7, 1227: 4, 1233: 8, 1249: 8, 1257: 6, 1265: 8, 1267: 1, 1273: 3, 1275: 3, 1280: 4, 1300: 8, 1322: 6, 1323: 4, 1328: 4, 1417: 8, 1601: 8, 1905: 7, 1906: 7, 1907: 7, 1910: 7, 1912: 7, 1922: 7, 1927: 7, 1928: 7, 2016: 8, 2020: 8, 2024: 8, 2028: 8 + 170: 8, 171: 8, 189: 7, 190: 6, 193: 8, 197: 8, 199: 4, 201: 8, 209: 7, 211: 2, 241: 6, 288: 5, 289: 8, 298: 8, 304: 1, 308: 4, 309: 8, 311: 8, 313: 8, 320: 3, 328: 1, 352: 5, 381: 6, 384: 4, 386: 8, 388: 8, 389: 2, 390: 7, 417: 7, 419: 1, 426: 7, 451: 8, 452: 8, 453: 6, 454: 8, 456: 8, 479: 3, 481: 7, 485: 8, 489: 8, 493: 8, 495: 4, 497: 8, 499: 3, 500: 6, 501: 8, 508: 8, 528: 4, 532: 6, 546: 7, 550: 8, 554: 3, 558: 8, 560: 8, 562: 8, 563: 5, 564: 5, 565: 5, 566: 5, 567: 3, 568: 1, 573: 1, 577: 8, 647: 3, 707: 8, 711: 6, 715: 8, 761: 7, 810: 8, 840: 5, 842: 5, 844: 8, 866: 4, 961: 8, 969: 8, 977: 8, 979: 7, 988: 6, 989: 8, 995: 7, 1001: 8, 1005: 6, 1009: 8, 1017: 8, 1019: 2, 1020: 8, 1105: 6, 1187: 4, 1217: 8, 1221: 5, 1223: 3, 1225: 7, 1227: 4, 1233: 8, 1249: 8, 1257: 6, 1265: 8, 1267: 1, 1273: 3, 1275: 3, 1280: 4, 1300: 8, 1322: 6, 1323: 4, 1328: 4, 1417: 8, 1601: 8, 1905: 7, 1906: 7, 1907: 7, 1910: 7, 1912: 7, 1922: 7, 1927: 7, 1928: 7, 2016: 8, 2020: 8, 2024: 8, 2028: 8 }, # Volt Premier w/ ACC 2018 { @@ -86,14 +86,11 @@ FINGERPRINTS = { STEER_THRESHOLD = 1.0 -STOCK_CONTROL_MSGS = { - CAR.HOLDEN_ASTRA: [384, 715], - CAR.VOLT: [384, 715], # 384 = "ASCMLKASteeringCmd", 715 = "ASCMGasRegenCmd" - CAR.MALIBU: [384, 715], # 384 = "ASCMLKASteeringCmd", 715 = "ASCMGasRegenCmd" - CAR.ACADIA: [384, 715], # 384 = "ASCMLKASteeringCmd", 715 = "ASCMGasRegenCmd" - CAR.CADILLAC_ATS: [384, 715], # 384 = "ASCMLKASteeringCmd", 715 = "ASCMGasRegenCmd" - CAR.BUICK_REGAL: [384, 715], # 384 = "ASCMLKASteeringCmd", 715 = "ASCMGasRegenCmd" - CAR.CADILLAC_CT6: [], # CT6 does not require ASCMs to be disconnected +class ECU: + CAM = 0 + +ECU_FINGERPRINT = { + ECU.CAM: [384, 715] # 384 = "ASCMLKASteeringCmd", 715 = "ASCMGasRegenCmd" } DBC = { diff --git a/selfdrive/car/honda/carcontroller.py b/selfdrive/car/honda/carcontroller.py index 8b96e511c2..1058e2f21a 100644 --- a/selfdrive/car/honda/carcontroller.py +++ b/selfdrive/car/honda/carcontroller.py @@ -73,7 +73,7 @@ HUDData = namedtuple("HUDData", "lanes", "fcw", "acc_alert", "steer_required"]) -class CarController(object): +class CarController(): def __init__(self, dbc_name): self.braking = False self.brake_steady = 0. diff --git a/selfdrive/car/honda/carstate.py b/selfdrive/car/honda/carstate.py index 87ecdb34ef..bb974b3d14 100644 --- a/selfdrive/car/honda/carstate.py +++ b/selfdrive/car/honda/carstate.py @@ -1,4 +1,5 @@ from cereal import car +from collections import defaultdict from common.numpy_fast import interp from common.kalman.simple_kalman import KF1D from selfdrive.can.can_define import CANDefine @@ -178,11 +179,12 @@ def get_cam_can_parser(CP): bus_cam = 1 if CP.carFingerprint in HONDA_BOSCH and not CP.isPandaBlack else 2 return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, bus_cam) -class CarState(object): +class CarState(): def __init__(self, CP): self.CP = CP self.can_define = CANDefine(DBC[CP.carFingerprint]['pt']) self.shifter_values = self.can_define.dv["GEARBOX"]["GEAR_SHIFTER"] + self.steer_status_values = defaultdict(lambda: "UNKNOWN", self.can_define.dv["STEER_STATUS"]["STEER_STATUS"]) self.user_gas, self.user_gas_pressed = 0., 0 self.brake_switch_prev = 0 @@ -224,7 +226,6 @@ class CarState(object): self.prev_right_blinker_on = self.right_blinker_on # ******************* parse out can ******************* - if self.CP.carFingerprint in (CAR.ACCORD, CAR.ACCORD_15, CAR.ACCORDH, CAR.CIVIC_BOSCH, CAR.CRV_HYBRID): # TODO: find wheels moving bit in dbc self.standstill = cp.vl["ENGINE_DATA"]['XMISSION_SPEED'] < 0.1 self.door_all_closed = not cp.vl["SCM_FEEDBACK"]['DRIVERS_DOOR_OPEN'] @@ -237,11 +238,13 @@ class CarState(object): cp.vl["DOORS_STATUS"]['DOOR_OPEN_RL'], cp.vl["DOORS_STATUS"]['DOOR_OPEN_RR']]) self.seatbelt = not cp.vl["SEATBELT_STATUS"]['SEATBELT_DRIVER_LAMP'] and cp.vl["SEATBELT_STATUS"]['SEATBELT_DRIVER_LATCHED'] - # 2 = temporary; 3 = TBD; 4 = significant steering wheel torque; 5 = (permanent); 6 = temporary; 7 = (permanent) - # TODO: Use values from DBC to parse this field - self.steer_error = cp.vl["STEER_STATUS"]['STEER_STATUS'] not in [0, 2, 3, 4, 6] - self.steer_not_allowed = cp.vl["STEER_STATUS"]['STEER_STATUS'] not in [0, 4] # 4 can be caused by bump OR steering nudge from driver - self.steer_warning = cp.vl["STEER_STATUS"]['STEER_STATUS'] not in [0, 3, 4] # 3 is low speed lockout, not worth a warning + steer_status = self.steer_status_values[cp.vl["STEER_STATUS"]['STEER_STATUS']] + self.steer_error = steer_status not in ['NORMAL', 'NO_TORQUE_ALERT_1', 'NO_TORQUE_ALERT_2', 'LOW_SPEED_LOCKOUT', 'TMP_FAULT'] + # NO_TORQUE_ALERT_2 can be caused by bump OR steering nudge from driver + self.steer_not_allowed = steer_status not in ['NORMAL', 'NO_TORQUE_ALERT_2'] + # LOW_SPEED_LOCKOUT is not worth a warning + self.steer_warning = steer_status not in ['NORMAL', 'LOW_SPEED_LOCKOUT', 'NO_TORQUE_ALERT_2'] + if self.CP.radarOffCan: self.brake_error = 0 else: @@ -357,7 +360,7 @@ if __name__ == '__main__': import zmq context = zmq.Context() - class CarParams(object): + class CarParams(): def __init__(self): self.carFingerprint = "HONDA CIVIC 2016 TOURING" self.enableGasInterceptor = 0 diff --git a/selfdrive/car/honda/hondacan.py b/selfdrive/car/honda/hondacan.py index b66b6c8c53..c6b9a12e83 100644 --- a/selfdrive/car/honda/hondacan.py +++ b/selfdrive/car/honda/hondacan.py @@ -6,7 +6,6 @@ from selfdrive.car.honda.values import CAR, HONDA_BOSCH def can_cksum(mm): s = 0 for c in mm: - c = ord(c) s += (c>>4) s += c & 0xF s = 8-s @@ -15,19 +14,19 @@ def can_cksum(mm): def fix(msg, addr): - msg2 = msg[0:-1] + chr(ord(msg[-1]) | can_cksum(struct.pack("I", addr)+msg)) + msg2 = msg[0:-1] + (msg[-1] | can_cksum(struct.pack("I", addr)+msg)).to_bytes(1, 'little') return msg2 -def get_pt_bus(car_fingerprint, is_panda_black): - return 1 if car_fingerprint in HONDA_BOSCH and is_panda_black else 0 +def get_pt_bus(car_fingerprint, has_relay): + return 1 if car_fingerprint in HONDA_BOSCH and has_relay else 0 -def get_lkas_cmd_bus(car_fingerprint, is_panda_black): - return 2 if car_fingerprint in HONDA_BOSCH and not is_panda_black else 0 +def get_lkas_cmd_bus(car_fingerprint, has_relay): + return 2 if car_fingerprint in HONDA_BOSCH and not has_relay else 0 -def create_brake_command(packer, apply_brake, pump_on, pcm_override, pcm_cancel_cmd, fcw, idx, car_fingerprint, is_panda_black): +def create_brake_command(packer, apply_brake, pump_on, pcm_override, pcm_cancel_cmd, fcw, idx, car_fingerprint, has_relay): # TODO: do we loose pressure if we keep pump off for long? brakelights = apply_brake > 0 brake_rq = apply_brake > 0 @@ -49,23 +48,23 @@ def create_brake_command(packer, apply_brake, pump_on, pcm_override, pcm_cancel_ "AEB_REQ_2": 0, "AEB_STATUS": 0, } - bus = get_pt_bus(car_fingerprint, is_panda_black) + bus = get_pt_bus(car_fingerprint, has_relay) return packer.make_can_msg("BRAKE_COMMAND", bus, values, idx) -def create_steering_control(packer, apply_steer, lkas_active, car_fingerprint, idx, is_panda_black): +def create_steering_control(packer, apply_steer, lkas_active, car_fingerprint, idx, has_relay): values = { "STEER_TORQUE": apply_steer if lkas_active else 0, "STEER_TORQUE_REQUEST": lkas_active, } - bus = get_lkas_cmd_bus(car_fingerprint, is_panda_black) + bus = get_lkas_cmd_bus(car_fingerprint, has_relay) return packer.make_can_msg("STEERING_CONTROL", bus, values, idx) -def create_ui_commands(packer, pcm_speed, hud, car_fingerprint, is_metric, idx, is_panda_black): +def create_ui_commands(packer, pcm_speed, hud, car_fingerprint, is_metric, idx, has_relay): commands = [] - bus_pt = get_pt_bus(car_fingerprint, is_panda_black) - bus_lkas = get_lkas_cmd_bus(car_fingerprint, is_panda_black) + bus_pt = get_pt_bus(car_fingerprint, has_relay) + bus_lkas = get_lkas_cmd_bus(car_fingerprint, has_relay) if car_fingerprint not in HONDA_BOSCH: acc_hud_values = { @@ -101,10 +100,10 @@ def create_ui_commands(packer, pcm_speed, hud, car_fingerprint, is_metric, idx, return commands -def spam_buttons_command(packer, button_val, idx, car_fingerprint, is_panda_black): +def spam_buttons_command(packer, button_val, idx, car_fingerprint, has_relay): values = { 'CRUISE_BUTTONS': button_val, 'CRUISE_SETTING': 0, } - bus = get_pt_bus(car_fingerprint, is_panda_black) + bus = get_pt_bus(car_fingerprint, has_relay) return packer.make_can_msg("SCM_BUTTONS", bus, values, idx) diff --git a/selfdrive/car/honda/interface.py b/selfdrive/car/honda/interface.py index 13f399310a..3c2ecfa8dd 100755 --- a/selfdrive/car/honda/interface.py +++ b/selfdrive/car/honda/interface.py @@ -1,5 +1,4 @@ -#!/usr/bin/env python -import os +#!/usr/bin/env python3 import numpy as np from cereal import car from common.numpy_fast import clip, interp @@ -9,11 +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, CAMERA_MSGS -from selfdrive.car import STD_CARGO_KG, CivicParams, scale_rot_inertia, scale_tire_stiffness -from selfdrive.controls.lib.planner import _A_CRUISE_MAX_V_FOLLOWING +from selfdrive.car.honda.values import CruiseButtons, CAR, HONDA_BOSCH, VISUAL_HUD, 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.car.interfaces import CarInterfaceBase -A_ACC_MAX = max(_A_CRUISE_MAX_V_FOLLOWING) +A_ACC_MAX = max(_A_CRUISE_MAX_V) ButtonType = car.CarState.ButtonEvent.Type GearShifter = car.CarState.GearShifter @@ -72,7 +72,7 @@ def get_compute_gb_acura(): return _compute_gb_acura -class CarInterface(object): +class CarInterface(CarInterfaceBase): def __init__(self, CP, CarController): self.CP = CP @@ -131,27 +131,28 @@ class CarInterface(object): return float(max(max_accel, a_target / A_ACC_MAX)) * min(speedLimiter, accelLimiter) @staticmethod - def get_params(candidate, fingerprint, vin="", is_panda_black=False): + def get_params(candidate, fingerprint=gen_empty_fingerprint(), vin="", has_relay=False): ret = car.CarParams.new_message() ret.carName = "honda" ret.carFingerprint = candidate ret.carVin = vin - ret.isPandaBlack = is_panda_black + ret.isPandaBlack = has_relay if candidate in HONDA_BOSCH: ret.safetyModel = car.CarParams.SafetyModel.hondaBosch - ret.enableCamera = True + 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.enableCamera = not any(x for x in CAMERA_MSGS if x in fingerprint) or is_panda_black - ret.enableGasInterceptor = 0x201 in fingerprint + 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 - cloudlog.warn("ECU Camera Simulated: %r", ret.enableCamera) - cloudlog.warn("ECU Gas Interceptor: %r", ret.enableGasInterceptor) + cloudlog.warning("ECU Camera Simulated: %r", ret.enableCamera) + cloudlog.warning("ECU Gas Interceptor: %r", ret.enableGasInterceptor) ret.enableCruise = not ret.enableGasInterceptor @@ -172,12 +173,8 @@ class CarInterface(object): ret.centerToFront = CivicParams.CENTER_TO_FRONT ret.steerRatio = 15.38 # 10.93 is end-to-end spec tire_stiffness_factor = 1. - # Civic at comma has modified steering FW, so different tuning for the Neo in that car - is_fw_modified = os.getenv("DONGLE_ID") in ['5b7c365c50084530'] - if is_fw_modified: - ret.lateralTuning.pid.kf = 0.00004 - ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.4], [0.12]] if is_fw_modified else [[0.8], [0.24]] + ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.8], [0.24]] ret.longitudinalTuning.kpBP = [0., 5., 35.] ret.longitudinalTuning.kpV = [3.6, 2.4, 1.5] ret.longitudinalTuning.kiBP = [0., 35.] diff --git a/selfdrive/car/honda/radar_interface.py b/selfdrive/car/honda/radar_interface.py index 6943d35634..2415e95a35 100755 --- a/selfdrive/car/honda/radar_interface.py +++ b/selfdrive/car/honda/radar_interface.py @@ -1,23 +1,25 @@ -#!/usr/bin/env python +#!/usr/bin/env python3 import os import time from cereal import car from selfdrive.can.parser import CANParser +from common.realtime import DT_RDR +from selfdrive.car.interfaces import RadarInterfaceBase def _create_nidec_can_parser(): dbc_f = 'acura_ilx_2016_nidec.dbc' - radar_messages = [0x400] + range(0x430, 0x43A) + range(0x440, 0x446) + radar_messages = [0x400] + list(range(0x430, 0x43A)) + list(range(0x440, 0x446)) signals = list(zip(['RADAR_STATE'] + ['LONG_DIST'] * 16 + ['NEW_TRACK'] * 16 + ['LAT_DIST'] * 16 + ['REL_SPEED'] * 16, [0x400] + radar_messages[1:] * 4, [0] + [255] * 16 + [1] * 16 + [0] * 16 + [0] * 16)) checks = list(zip([0x445], [20])) + fn = os.path.splitext(dbc_f)[0].encode('utf8') + return CANParser(fn, signals, checks, 1) - return CANParser(os.path.splitext(dbc_f)[0], signals, checks, 1) - -class RadarInterface(object): +class RadarInterface(RadarInterfaceBase): def __init__(self, CP): # radar self.pts = {} @@ -26,7 +28,7 @@ class RadarInterface(object): self.radar_wrong_config = False self.radar_off_can = CP.radarOffCan - self.delay = 0.1 # Delay of radar + self.delay = int(0.1 / DT_RDR) # 0.1s delay of radar # Nidec self.rcp = _create_nidec_can_parser() @@ -55,7 +57,7 @@ class RadarInterface(object): def _update(self, updated_messages): ret = car.RadarData.new_message() - for ii in updated_messages: + for ii in sorted(updated_messages): cpt = self.rcp.vl[ii] if ii == 0x400: # check for radar faults @@ -85,6 +87,6 @@ class RadarInterface(object): errors.append("wrongConfig") ret.errors = errors - ret.points = self.pts.values() + ret.points = list(self.pts.values()) return ret diff --git a/selfdrive/car/honda/values.py b/selfdrive/car/honda/values.py index baa2974c1a..ab94068130 100644 --- a/selfdrive/car/honda/values.py +++ b/selfdrive/car/honda/values.py @@ -30,6 +30,9 @@ VISUAL_HUD = { VisualAlert.seatbeltUnbuckled: AH.SEATBELT, VisualAlert.speedTooHigh: AH.SPEED_TOO_HIGH} +class ECU: + CAM = 0 + class CAR: ACCORD = "HONDA ACCORD 2018 SPORT 2T" ACCORD_15 = "HONDA ACCORD 2018 LX 1.5T" @@ -116,7 +119,7 @@ FINGERPRINTS = { }, # 2019 Ridgeline { - 57: 3, 145: 8, 229: 4, 308: 5, 316: 8, 339: 7, 342: 6, 344: 8, 380: 8, 392: 6, 399: 7, 419: 8, 420: 8, 422:8, 425: 8, 426: 8, 427: 3, 432: 7, 464: 8, 476: 4, 490: 8, 545: 5, 546: 3, 597: 8, 660: 8, 773: 7, 777: 8, 795: 8, 800: 8, 804: 8, 808: 8, 819: 7, 821: 5, 871: 8, 882: 2, 884: 7, 892: 8, 923: 2, 929: 8, 963: 8, 965: 8, 966: 8, 967: 8, 983: 8, 985: 3, 1027: 5, 1029: 8, 1036: 8, 1039: 8, 1064: 7, 1088: 8, 1089: 8, 1092: 1, 1108: 8, 1125: 8, 1296: 8, 1365: 5, 424: 5, 1613: 5, 1616: 5, 1618: 5, 1623: 5, 1668: 5 + 57: 3, 145: 8, 228: 5, 229: 4, 308: 5, 316: 8, 339: 7, 342: 6, 344: 8, 380: 8, 392: 6, 399: 7, 419: 8, 420: 8, 422:8, 425: 8, 426: 8, 427: 3, 432: 7, 464: 8, 476: 4, 490: 8, 545: 5, 546: 3, 597: 8, 660: 8, 773: 7, 777: 8, 795: 8, 800: 8, 804: 8, 808: 8, 819: 7, 821: 5, 871: 8, 882: 2, 884: 7, 892: 8, 923: 2, 929: 8, 963: 8, 965: 8, 966: 8, 967: 8, 983: 8, 985: 3, 1027: 5, 1029: 8, 1036: 8, 1039: 8, 1064: 7, 1088: 8, 1089: 8, 1092: 1, 1108: 8, 1125: 8, 1296: 8, 1365: 5, 424: 5, 1613: 5, 1616: 5, 1618: 5, 1623: 5, 1668: 5 }] } @@ -185,7 +188,8 @@ SPEED_FACTOR = { # msgs sent for steering controller by camera module on can 0. # those messages are mutually exclusive on CRV and non-CRV cars -CAMERA_MSGS = [0xe4, 0x194] +ECU_FINGERPRINT = { + ECU.CAM: [0xE4, 0x194], # steer torque cmd +} -# TODO: get these from dbc file HONDA_BOSCH = [CAR.ACCORD, CAR.ACCORD_15, CAR.ACCORDH, CAR.CIVIC_BOSCH, CAR.CRV_5G, CAR.CRV_HYBRID] diff --git a/selfdrive/car/hyundai/carcontroller.py b/selfdrive/car/hyundai/carcontroller.py index a1a688ff5e..729c853628 100644 --- a/selfdrive/car/hyundai/carcontroller.py +++ b/selfdrive/car/hyundai/carcontroller.py @@ -16,7 +16,7 @@ class SteerLimitParams: STEER_DRIVER_MULTIPLIER = 2 STEER_DRIVER_FACTOR = 1 -class CarController(object): +class CarController(): def __init__(self, dbc_name, car_fingerprint): self.apply_steer_last = 0 self.car_fingerprint = car_fingerprint diff --git a/selfdrive/car/hyundai/carstate.py b/selfdrive/car/hyundai/carstate.py index f993bd2a0f..a7e3448a0e 100644 --- a/selfdrive/car/hyundai/carstate.py +++ b/selfdrive/car/hyundai/carstate.py @@ -1,8 +1,10 @@ +from cereal import car from selfdrive.car.hyundai.values import DBC, STEER_THRESHOLD from selfdrive.can.parser import CANParser from selfdrive.config import Conversions as CV from common.kalman.simple_kalman import KF1D +GearShifter = car.CarState.GearShifter def get_can_parser(CP): @@ -31,7 +33,7 @@ def get_can_parser(CP): ("CYL_PRES", "ESP12", 0), ("CF_Clu_CruiseSwState", "CLU11", 0), - ("CF_Clu_CruiseSwMain" , "CLU11", 0), + ("CF_Clu_CruiseSwMain", "CLU11", 0), ("CF_Clu_SldMainSW", "CLU11", 0), ("CF_Clu_ParityBit1", "CLU11", 0), ("CF_Clu_VanzDecimal" , "CLU11", 0), @@ -48,7 +50,7 @@ def get_can_parser(CP): ("CF_Clu_InhibitN", "CLU15", 0), ("CF_Clu_InhibitR", "CLU15", 0), - ("CF_Lvr_Gear","LVR12",0), + ("CF_Lvr_Gear", "LVR12",0), ("CUR_GR", "TCU12",0), ("ACCEnable", "TCS13", 0), @@ -122,7 +124,7 @@ def get_camera_parser(CP): return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 2) -class CarState(object): +class CarState(): def __init__(self, CP): self.CP = CP @@ -211,38 +213,38 @@ class CarState(object): # Gear Selecton - This is not compatible with all Kia/Hyundai's, But is the best way for those it is compatible with gear = cp.vl["LVR12"]["CF_Lvr_Gear"] if gear == 5: - self.gear_shifter = "drive" + self.gear_shifter = GearShifter.drive elif gear == 6: - self.gear_shifter = "neutral" + self.gear_shifter = GearShifter.neutral elif gear == 0: - self.gear_shifter = "park" + self.gear_shifter = GearShifter.park elif gear == 7: - self.gear_shifter = "reverse" + self.gear_shifter = GearShifter.reverse else: - self.gear_shifter = "unknown" + self.gear_shifter = GearShifter.unknown # Gear Selection via Cluster - For those Kia/Hyundai which are not fully discovered, we can use the Cluster Indicator for Gear Selection, as this seems to be standard over all cars, but is not the preferred method. if cp.vl["CLU15"]["CF_Clu_InhibitD"] == 1: - self.gear_shifter_cluster = "drive" + self.gear_shifter_cluster = GearShifter.drive elif cp.vl["CLU15"]["CF_Clu_InhibitN"] == 1: - self.gear_shifter_cluster = "neutral" + self.gear_shifter_cluster = GearShifter.neutral elif cp.vl["CLU15"]["CF_Clu_InhibitP"] == 1: - self.gear_shifter_cluster = "park" + self.gear_shifter_cluster = GearShifter.park elif cp.vl["CLU15"]["CF_Clu_InhibitR"] == 1: - self.gear_shifter_cluster = "reverse" + self.gear_shifter_cluster = GearShifter.reverse else: - self.gear_shifter_cluster = "unknown" + self.gear_shifter_cluster = GearShifter.unknown # Gear Selecton via TCU12 gear2 = cp.vl["TCU12"]["CUR_GR"] if gear2 == 0: - self.gear_tcu = "park" + self.gear_tcu = GearShifter.park elif gear2 == 14: - self.gear_tcu = "reverse" + self.gear_tcu = GearShifter.reverse elif gear2 > 0 and gear2 < 9: # unaware of anything over 8 currently - self.gear_tcu = "drive" + self.gear_tcu = GearShifter.drive else: - self.gear_tcu = "unknown" + self.gear_tcu = GearShifter.unknown # save the entire LKAS11 and CLU11 self.lkas11 = cp_cam.vl["LKAS11"] diff --git a/selfdrive/car/hyundai/hyundaican.py b/selfdrive/car/hyundai/hyundaican.py index d9e7cf3a92..8783932055 100644 --- a/selfdrive/car/hyundai/hyundaican.py +++ b/selfdrive/car/hyundai/hyundaican.py @@ -34,15 +34,13 @@ def create_lkas11(packer, car_fingerprint, apply_steer, steer_req, cnt, enabled, if car_fingerprint in CHECKSUM["crc8"]: # CRC Checksum as seen on 2019 Hyundai Santa Fe - dat = dat[:6] + dat[7] + dat = dat[:6] + dat[7:8] checksum = hyundai_checksum(dat) elif car_fingerprint in CHECKSUM["6B"]: # Checksum of first 6 Bytes, as seen on 2018 Kia Sorento - dat = [ord(i) for i in dat] checksum = sum(dat[:6]) % 256 elif car_fingerprint in CHECKSUM["7B"]: # Checksum of first 6 Bytes and last Byte as seen on 2018 Kia Stinger - dat = [ord(i) for i in dat] checksum = (sum(dat[:6]) + dat[7]) % 256 values["CF_Lkas_Chksum"] = checksum @@ -50,15 +48,15 @@ def create_lkas11(packer, car_fingerprint, apply_steer, steer_req, cnt, enabled, return packer.make_can_msg("LKAS11", 0, values) def create_lkas12(): - return make_can_msg(1342, "\x00\x00\x00\x00\x60\x05", 0) + return make_can_msg(1342, b"\x00\x00\x00\x00\x60\x05", 0) def create_1191(): - return make_can_msg(1191, "\x01\x00", 0) + return make_can_msg(1191, b"\x01\x00", 0) def create_1156(): - return make_can_msg(1156, "\x08\x20\xfe\x3f\x00\xe0\xfd\x3f", 0) + return make_can_msg(1156, b"\x08\x20\xfe\x3f\x00\xe0\xfd\x3f", 0) def create_clu11(packer, clu11, button): values = { diff --git a/selfdrive/car/hyundai/interface.py b/selfdrive/car/hyundai/interface.py index b39f07d6f1..7d6e372cf6 100644 --- a/selfdrive/car/hyundai/interface.py +++ b/selfdrive/car/hyundai/interface.py @@ -1,16 +1,17 @@ -#!/usr/bin/env python +#!/usr/bin/env python3 from cereal import car from selfdrive.config import Conversions as CV from selfdrive.controls.lib.drive_helpers import EventTypes as ET, create_event from selfdrive.controls.lib.vehicle_model import VehicleModel from selfdrive.car.hyundai.carstate import CarState, get_can_parser, get_camera_parser -from selfdrive.car.hyundai.values import CAMERA_MSGS, CAR, get_hud_alerts, FEATURES -from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness +from selfdrive.car.hyundai.values import ECU, ECU_FINGERPRINT, CAR, get_hud_alerts, FEATURES, FINGERPRINTS +from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, is_ecu_disconnected, gen_empty_fingerprint +from selfdrive.car.interfaces import CarInterfaceBase GearShifter = car.CarState.GearShifter ButtonType = car.CarState.ButtonEvent.Type -class CarInterface(object): +class CarInterface(CarInterfaceBase): def __init__(self, CP, CarController): self.CP = CP self.VM = VehicleModel(CP) @@ -37,18 +38,14 @@ class CarInterface(object): return float(accel) / 3.0 @staticmethod - def calc_accel_override(a_ego, a_target, v_ego, v_target): - return 1.0 - - @staticmethod - def get_params(candidate, fingerprint, vin="", is_panda_black=False): + def get_params(candidate, fingerprint=gen_empty_fingerprint(), vin="", has_relay=False): ret = car.CarParams.new_message() ret.carName = "hyundai" ret.carFingerprint = candidate ret.carVin = vin - ret.isPandaBlack = is_panda_black + ret.isPandaBlack = has_relay ret.radarOffCan = True ret.safetyModel = car.CarParams.SafetyModel.hyundai ret.enableCruise = True # stock acc @@ -143,7 +140,7 @@ class CarInterface(object): ret.brakeMaxBP = [0.] ret.brakeMaxV = [1.] - ret.enableCamera = not any(x for x in CAMERA_MSGS if x in fingerprint) or is_panda_black + ret.enableCamera = is_ecu_disconnected(fingerprint[0], FINGERPRINTS, ECU_FINGERPRINT, candidate, ECU.CAM) or has_relay ret.openpilotLongitudinalControl = False ret.steerLimitAlert = False diff --git a/selfdrive/car/hyundai/radar_interface.py b/selfdrive/car/hyundai/radar_interface.py index 04c1005947..b2f7651136 100644 --- a/selfdrive/car/hyundai/radar_interface.py +++ b/selfdrive/car/hyundai/radar_interface.py @@ -1,18 +1,5 @@ -#!/usr/bin/env python -import os -import time -from cereal import car +#!/usr/bin/env python3 +from selfdrive.car.interfaces import RadarInterfaceBase -class RadarInterface(object): - def __init__(self, CP): - # radar - self.pts = {} - self.delay = 0.1 - - def update(self, can_strings): - ret = car.RadarData.new_message() - - if 'NO_RADAR_SLEEP' not in os.environ: - time.sleep(0.05) # radard runs on RI updates - - return ret +class RadarInterface(RadarInterfaceBase): + pass diff --git a/selfdrive/car/hyundai/values.py b/selfdrive/car/hyundai/values.py index 6380ca4463..e801260a95 100644 --- a/selfdrive/car/hyundai/values.py +++ b/selfdrive/car/hyundai/values.py @@ -25,7 +25,7 @@ class Buttons: FINGERPRINTS = { CAR.ELANTRA: [{ - 66: 8, 67: 8, 68: 8, 127: 8, 273: 8, 274: 8, 275: 8, 339: 8, 356: 4, 399: 8, 512: 6, 544: 8, 593: 8, 608: 8, 688: 5, 790: 8, 809: 8, 897: 8, 899: 8, 902: 8, 903: 8, 905: 8, 909: 8, 916: 8, 1040: 8, 1056: 8, 1057: 8, 1078: 4, 1170: 8, 1265: 4, 1280: 1, 1282: 4, 1287: 4, 1290: 8, 1292: 8, 1294: 8, 1312: 8, 1314: 8, 1322: 8, 1345: 8, 1349: 8, 1351: 8, 1353: 8, 1363: 8, 1366: 8, 1367: 8, 1369: 8, 1407: 8, 1415: 8, 1419: 8, 1425: 2, 1427: 6, 1440: 8, 1456: 4, 1472: 8, 1486: 8, 1487: 8, 1491: 8, 1530: 8, 1532: 5, 2001: 8, 2003: 8, 2004: 8, 2009: 8, 2012: 8, 2016: 8, 2017: 8, 2024: 8, 2025: 8 + 66: 8, 67: 8, 68: 8, 127: 8, 273: 8, 274: 8, 275: 8, 339: 8, 356: 4, 399: 8, 512: 6, 544: 8, 593: 8, 608: 8, 688: 5, 790: 8, 809: 8, 897: 8, 832: 8, 899: 8, 902: 8, 903: 8, 905: 8, 909: 8, 916: 8, 1040: 8, 1056: 8, 1057: 8, 1078: 4, 1170: 8, 1265: 4, 1280: 1, 1282: 4, 1287: 4, 1290: 8, 1292: 8, 1294: 8, 1312: 8, 1314: 8, 1322: 8, 1345: 8, 1349: 8, 1351: 8, 1353: 8, 1363: 8, 1366: 8, 1367: 8, 1369: 8, 1407: 8, 1415: 8, 1419: 8, 1425: 2, 1427: 6, 1440: 8, 1456: 4, 1472: 8, 1486: 8, 1487: 8, 1491: 8, 1530: 8, 1532: 5, 2001: 8, 2003: 8, 2004: 8, 2009: 8, 2012: 8, 2016: 8, 2017: 8, 2024: 8, 2025: 8 }], CAR.GENESIS: [{ 67: 8, 68: 8, 304: 8, 320: 8, 339: 8, 356: 4, 544: 7, 593: 8, 608: 8, 688: 5, 809: 8, 832: 8, 854: 7, 870: 7, 871: 8, 872: 5, 897: 8, 902: 8, 903: 6, 916: 8, 1024: 2, 1040: 8, 1056: 8, 1057: 8, 1078: 4, 1107: 5, 1136: 8, 1151: 6, 1168: 7, 1170: 8, 1173: 8, 1184: 8, 1265: 4, 1280: 1, 1287: 4, 1292: 8, 1312: 8, 1322: 8, 1331: 8, 1332: 8, 1333: 8, 1334: 8, 1335: 8, 1342: 6, 1345: 8, 1363: 8, 1369: 8, 1370: 8, 1371: 8, 1378: 4, 1384: 5, 1407: 8, 1419: 8, 1427: 6, 1434: 2, 1456: 4 @@ -46,12 +46,17 @@ FINGERPRINTS = { 67: 8, 127: 8, 304: 8, 320: 8, 339: 8, 356: 4, 544: 8, 593: 8, 608: 8, 688: 6, 809: 8, 832: 8, 854: 7, 870: 7, 871: 8, 872: 8, 897: 8, 902: 8, 903: 8, 905: 8, 909: 8, 916: 8, 1040: 8, 1042: 8, 1056: 8, 1057: 8, 1078: 4, 1107: 5, 1136: 8, 1151: 6, 1155: 8, 1156: 8, 1162: 8, 1164: 8, 1168: 7, 1170: 8, 1173: 8, 1183: 8, 1186: 2, 1191: 2, 1227: 8, 1265: 4, 1280: 1, 1287: 4, 1290: 8, 1292: 8, 1294: 8, 1312: 8, 1322: 8, 1342: 6, 1345: 8, 1348: 8, 1363: 8, 1369: 8, 1379: 8, 1384: 8, 1407: 8, 1414: 3, 1419: 8, 1427: 6, 1456: 4, 1470: 8 }, { - 67: 8, 127: 8, 304: 8, 320: 8, 339: 8, 356: 4, 544: 8, 593: 8, 608: 8, 688: 6, 764: 8, 809: 8, 854: 7, 870: 7, 871: 8, 872: 8, 897: 8, 902: 8, 903: 8, 905: 8, 909: 8, 916: 8, 1040: 8, 1042: 8, 1056: 8, 1057: 8, 1064: 8, 1078: 4, 1107: 5, 1136: 8, 1151: 6, 1155: 8, 1162: 8, 1164: 8, 1168: 7, 1170: 8, 1173: 8, 1180: 8, 1183: 8, 1186: 2, 1227: 8, 1265: 4, 1280: 1, 1287: 4, 1290: 8, 1292: 8, 1294: 8, 1312: 8, 1322: 8, 1345: 8, 1348: 8, 1363: 8, 1369: 8, 1371: 8, 1378: 8, 1384: 8, 1407: 8, 1414: 3, 1419: 8, 1427: 6, 1456: 4, 1470: 8, 1988: 8, 2000: 8, 2004: 8, 2008: 8, 2012: 8 + 67: 8, 127: 8, 304: 8, 320: 8, 339: 8, 356: 4, 544: 8, 593: 8, 608: 8, 688: 6, 764: 8, 809: 8, 832: 8, 854: 7, 870: 7, 871: 8, 872: 8, 897: 8, 902: 8, 903: 8, 905: 8, 909: 8, 916: 8, 1040: 8, 1042: 8, 1056: 8, 1057: 8, 1064: 8, 1078: 4, 1107: 5, 1136: 8, 1151: 6, 1155: 8, 1162: 8, 1164: 8, 1168: 7, 1170: 8, 1173: 8, 1180: 8, 1183: 8, 1186: 2, 1227: 8, 1265: 4, 1280: 1, 1287: 4, 1290: 8, 1292: 8, 1294: 8, 1312: 8, 1322: 8, 1345: 8, 1348: 8, 1363: 8, 1369: 8, 1371: 8, 1378: 8, 1384: 8, 1407: 8, 1414: 3, 1419: 8, 1427: 6, 1456: 4, 1470: 8, 1988: 8, 2000: 8, 2004: 8, 2008: 8, 2012: 8 } ], } -CAMERA_MSGS = [832, 1156, 1191, 1342] +class ECU: + CAM = 0 + +ECU_FINGERPRINT = { + ECU.CAM: [832, 1156, 1191, 1342] +} CHECKSUM = { "crc8": [CAR.SANTA_FE], diff --git a/selfdrive/car/interfaces.py b/selfdrive/car/interfaces.py new file mode 100644 index 0000000000..0d619cb950 --- /dev/null +++ b/selfdrive/car/interfaces.py @@ -0,0 +1,39 @@ +import os +import time +from cereal import car +from selfdrive.car import gen_empty_fingerprint + +# generic car and radar interfaces + +class CarInterfaceBase(): + def __init__(self, CP, CarController): + pass + + @staticmethod + def calc_accel_override(a_ego, a_target, v_ego, v_target): + return 1. + + @staticmethod + def get_params(candidate, fingerprint=gen_empty_fingerprint(), vin="", has_relay=False): + raise NotImplementedError + + # returns a car.CarState, pass in car.CarControl + def update(self, c, can_strings): + raise NotImplementedError + + # return sendcan, pass in a car.CarControl + def apply(self, c): + raise NotImplementedError + +class RadarInterfaceBase(): + def __init__(self, CP): + self.pts = {} + self.delay = 0 + + def update(self, can_strings): + ret = car.RadarData.new_message() + + if 'NO_RADAR_SLEEP' not in os.environ: + time.sleep(0.05) # radard runs on RI updates + + return ret diff --git a/selfdrive/car/mock/interface.py b/selfdrive/car/mock/interface.py index b703776c0b..3b768d2838 100755 --- a/selfdrive/car/mock/interface.py +++ b/selfdrive/car/mock/interface.py @@ -1,9 +1,11 @@ -#!/usr/bin/env python +#!/usr/bin/env python3 from cereal import car from selfdrive.config import Conversions as CV from selfdrive.services import service_list from selfdrive.swaglog import cloudlog import selfdrive.messaging as messaging +from selfdrive.car import gen_empty_fingerprint +from selfdrive.car.interfaces import CarInterfaceBase # mocked car interface to work with chffrplus TS = 0.01 # 100Hz @@ -12,7 +14,7 @@ YAW_FR = 0.2 # ~0.8s time constant on yaw rate filter LPG = 2 * 3.1415 * YAW_FR * TS / (1 + 2 * 3.1415 * YAW_FR * TS) -class CarInterface(object): +class CarInterface(CarInterfaceBase): def __init__(self, CP, CarController): self.CP = CP @@ -34,11 +36,7 @@ class CarInterface(object): return accel @staticmethod - def calc_accel_override(a_ego, a_target, v_ego, v_target): - return 1.0 - - @staticmethod - def get_params(candidate, fingerprint, vin="", is_panda_black=False): + def get_params(candidate, fingerprint=gen_empty_fingerprint(), vin="", has_relay=False): ret = car.CarParams.new_message() diff --git a/selfdrive/car/mock/radar_interface.py b/selfdrive/car/mock/radar_interface.py index 8e5f7b7fca..b2f7651136 100755 --- a/selfdrive/car/mock/radar_interface.py +++ b/selfdrive/car/mock/radar_interface.py @@ -1,15 +1,5 @@ -#!/usr/bin/env python -from cereal import car -import time +#!/usr/bin/env python3 +from selfdrive.car.interfaces import RadarInterfaceBase - -class RadarInterface(object): - def __init__(self, CP): - # radar - self.pts = {} - self.delay = 0.1 - - def update(self, can_strings): - ret = car.RadarData.new_message() - time.sleep(0.05) # radard runs on RI updates - return ret +class RadarInterface(RadarInterfaceBase): + pass diff --git a/selfdrive/car/subaru/carcontroller.py b/selfdrive/car/subaru/carcontroller.py index 95e0013faa..6005976fae 100644 --- a/selfdrive/car/subaru/carcontroller.py +++ b/selfdrive/car/subaru/carcontroller.py @@ -18,7 +18,7 @@ class CarControllerParams(): -class CarController(object): +class CarController(): def __init__(self, car_fingerprint): self.lkas_active = False self.steer_idx = 0 diff --git a/selfdrive/car/subaru/carstate.py b/selfdrive/car/subaru/carstate.py index 6c5f678266..cc24f63d2a 100644 --- a/selfdrive/car/subaru/carstate.py +++ b/selfdrive/car/subaru/carstate.py @@ -82,7 +82,7 @@ def get_camera_can_parser(CP): return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 2) -class CarState(object): +class CarState(): def __init__(self, CP): # initialize can parser self.CP = CP diff --git a/selfdrive/car/subaru/interface.py b/selfdrive/car/subaru/interface.py index 5c91c95463..0fa12cf7d9 100644 --- a/selfdrive/car/subaru/interface.py +++ b/selfdrive/car/subaru/interface.py @@ -1,15 +1,16 @@ -#!/usr/bin/env python +#!/usr/bin/env python3 from cereal import car from selfdrive.config import Conversions as CV from selfdrive.controls.lib.drive_helpers import create_event, EventTypes as ET from selfdrive.controls.lib.vehicle_model import VehicleModel -from selfdrive.car.subaru.values import CAR +from selfdrive.car.subaru.values import CAR, FINGERPRINTS, ECU_FINGERPRINT, ECU from selfdrive.car.subaru.carstate import CarState, get_powertrain_can_parser, get_camera_can_parser -from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness +from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint, is_ecu_disconnected +from selfdrive.car.interfaces import CarInterfaceBase ButtonType = car.CarState.ButtonEvent.Type -class CarInterface(object): +class CarInterface(CarInterfaceBase): def __init__(self, CP, CarController): self.CP = CP @@ -34,24 +35,20 @@ class CarInterface(object): return float(accel) / 4.0 @staticmethod - def calc_accel_override(a_ego, a_target, v_ego, v_target): - return 1.0 - - @staticmethod - def get_params(candidate, fingerprint, vin="", is_panda_black=False): + def get_params(candidate, fingerprint=gen_empty_fingerprint(), vin="", has_relay=False): ret = car.CarParams.new_message() ret.carName = "subaru" ret.radarOffCan = True ret.carFingerprint = candidate ret.carVin = vin - ret.isPandaBlack = is_panda_black + ret.isPandaBlack = has_relay ret.safetyModel = car.CarParams.SafetyModel.subaru ret.enableCruise = True ret.steerLimitAlert = True - ret.enableCamera = True + ret.enableCamera = is_ecu_disconnected(fingerprint[0], FINGERPRINTS, ECU_FINGERPRINT, candidate, ECU.CAM) or has_relay ret.steerRateCost = 0.7 diff --git a/selfdrive/car/subaru/radar_interface.py b/selfdrive/car/subaru/radar_interface.py index 6c4e7a16a1..b2f7651136 100644 --- a/selfdrive/car/subaru/radar_interface.py +++ b/selfdrive/car/subaru/radar_interface.py @@ -1,18 +1,5 @@ -#!/usr/bin/env python -import os -import time -from cereal import car +#!/usr/bin/env python3 +from selfdrive.car.interfaces import RadarInterfaceBase -class RadarInterface(object): - def __init__(self, CP): - # radar - self.pts = {} - self.delay = 0.1 - - def update(self, can_strings): - ret = car.RadarData.new_message() - - if 'NO_RADAR_SLEEP' not in os.environ: - time.sleep(0.05) # radard runs on RI updates - - return ret +class RadarInterface(RadarInterfaceBase): + pass diff --git a/selfdrive/car/subaru/subarucan.py b/selfdrive/car/subaru/subarucan.py index 922012f318..96abfaafb3 100644 --- a/selfdrive/car/subaru/subarucan.py +++ b/selfdrive/car/subaru/subarucan.py @@ -6,7 +6,6 @@ VisualAlert = car.CarControl.HUDControl.VisualAlert def subaru_checksum(packer, values, addr): dat = packer.make_can_msg(addr, 0, values)[2] - dat = [ord(i) for i in dat] return (sum(dat[1:]) + (addr >> 8) + addr) & 0xff def create_steering_control(packer, car_fingerprint, apply_steer, frame, steer_step): diff --git a/selfdrive/car/subaru/values.py b/selfdrive/car/subaru/values.py index 0e444d7361..9abbaef3c8 100644 --- a/selfdrive/car/subaru/values.py +++ b/selfdrive/car/subaru/values.py @@ -17,6 +17,13 @@ STEER_THRESHOLD = { CAR.IMPREZA: 80, } +class ECU: + CAM = 0 + +ECU_FINGERPRINT = { + ECU.CAM: [290, 356], # steer torque cmd +} + DBC = { CAR.IMPREZA: dbc_dict('subaru_global_2017', None), } diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py index 0cfd67b9ea..094588d57e 100644 --- a/selfdrive/car/toyota/carcontroller.py +++ b/selfdrive/car/toyota/carcontroller.py @@ -89,7 +89,7 @@ def ipas_state_transition(steer_angle_enabled, enabled, ipas_active, ipas_reset_ return False, 0 -class CarController(object): +class CarController(): def __init__(self, dbc_name, car_fingerprint, enable_camera, enable_dsu, enable_apg): self.braking = False # redundant safety check with the board @@ -257,14 +257,14 @@ class CarController(object): # special cases if fr_step == 5 and ecu == ECU.CAM and bus == 1: cnt = (((frame // 5) % 7) + 1) << 5 - vl = chr(cnt) + vl + vl = bytes([cnt]) + vl elif addr in (0x489, 0x48a) and bus == 0: # add counter for those 2 messages (last 4 bits) cnt = ((frame // 100) % 0xf) + 1 if addr == 0x48a: # 0x48a has a 8 preceding the counter cnt += 1 << 7 - vl += chr(cnt) + vl += bytes([cnt]) can_sends.append(make_can_msg(addr, vl, bus, False)) diff --git a/selfdrive/car/toyota/carstate.py b/selfdrive/car/toyota/carstate.py index f36738f8ee..fc0260ef56 100644 --- a/selfdrive/car/toyota/carstate.py +++ b/selfdrive/car/toyota/carstate.py @@ -1,5 +1,5 @@ -import numpy as np from cereal import car +from common.numpy_fast import mean from common.kalman.simple_kalman import KF1D from selfdrive.can.can_define import CANDefine from selfdrive.can.parser import CANParser @@ -15,7 +15,7 @@ def parse_gear_shifter(gear, vals): try: return val_to_capnp[vals[gear]] except KeyError: - return "unknown" + return GearShifter.unknown def get_can_parser(CP): @@ -94,7 +94,7 @@ def get_cam_can_parser(CP): return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 2) -class CarState(object): +class CarState(): def __init__(self, CP): self.CP = CP @@ -140,7 +140,7 @@ class CarState(object): self.v_wheel_fr = cp.vl["WHEEL_SPEEDS"]['WHEEL_SPEED_FR'] * CV.KPH_TO_MS self.v_wheel_rl = cp.vl["WHEEL_SPEEDS"]['WHEEL_SPEED_RL'] * CV.KPH_TO_MS self.v_wheel_rr = cp.vl["WHEEL_SPEEDS"]['WHEEL_SPEED_RR'] * CV.KPH_TO_MS - v_wheel = float(np.mean([self.v_wheel_fl, self.v_wheel_fr, self.v_wheel_rl, self.v_wheel_rr])) + v_wheel = mean([self.v_wheel_fl, self.v_wheel_fr, self.v_wheel_rl, self.v_wheel_rr]) # Kalman filter if abs(v_wheel - self.v_ego) > 2.0: # Prevent large accelerations when car starts at non zero speed diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py index 457169c4d6..1063f556a7 100755 --- a/selfdrive/car/toyota/interface.py +++ b/selfdrive/car/toyota/interface.py @@ -1,17 +1,18 @@ -#!/usr/bin/env python +#!/usr/bin/env python3 from cereal import car from selfdrive.config import Conversions as CV from selfdrive.controls.lib.drive_helpers import EventTypes as ET, create_event from selfdrive.controls.lib.vehicle_model import VehicleModel from selfdrive.car.toyota.carstate import CarState, get_can_parser, get_cam_can_parser -from selfdrive.car.toyota.values import ECU, check_ecu_msgs, CAR, NO_STOP_TIMER_CAR, TSS2_CAR -from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness +from selfdrive.car.toyota.values import ECU, ECU_FINGERPRINT, CAR, NO_STOP_TIMER_CAR, TSS2_CAR, FINGERPRINTS +from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, is_ecu_disconnected, gen_empty_fingerprint from selfdrive.swaglog import cloudlog +from selfdrive.car.interfaces import CarInterfaceBase ButtonType = car.CarState.ButtonEvent.Type GearShifter = car.CarState.GearShifter -class CarInterface(object): +class CarInterface(CarInterfaceBase): def __init__(self, CP, CarController): self.CP = CP self.VM = VehicleModel(CP) @@ -38,18 +39,14 @@ class CarInterface(object): return float(accel) / 3.0 @staticmethod - def calc_accel_override(a_ego, a_target, v_ego, v_target): - return 1.0 - - @staticmethod - def get_params(candidate, fingerprint, vin="", is_panda_black=False): + def get_params(candidate, fingerprint=gen_empty_fingerprint(), vin="", has_relay=False): ret = car.CarParams.new_message() ret.carName = "toyota" ret.carFingerprint = candidate ret.carVin = vin - ret.isPandaBlack = is_panda_black + ret.isPandaBlack = has_relay ret.safetyModel = car.CarParams.SafetyModel.toyota @@ -219,16 +216,19 @@ class CarInterface(object): ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.3], [0.05]] ret.lateralTuning.pid.kf = 0.00006 + elif candidate == CAR.LEXUS_CTH: + stop_and_go = True + ret.safetyParam = 100 + ret.wheelbase = 2.60 + ret.steerRatio = 18.6 + tire_stiffness_factor = 0.517 + ret.mass = 3108 * CV.LB_TO_KG + STD_CARGO_KG # mean between min and max + ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.3], [0.05]] + ret.lateralTuning.pid.kf = 0.00007 + ret.steerRateCost = 1. ret.centerToFront = ret.wheelbase * 0.44 - #detect the Pedal address - ret.enableGasInterceptor = 0x201 in fingerprint - - # min speed to enable ACC. if car can do stop and go, then set enabling speed - # to a negative value, so it won't matter. - ret.minEnableSpeed = -1. if (stop_and_go or ret.enableGasInterceptor) else 19. * CV.MPH_TO_MS - # TODO: get actual value, for now starting with reasonable value for # civic and scaling by mass and wheelbase ret.rotationalInertia = scale_rot_inertia(ret.mass, ret.wheelbase) @@ -248,14 +248,19 @@ class CarInterface(object): ret.brakeMaxBP = [0.] ret.brakeMaxV = [1.] - ret.enableCamera = not check_ecu_msgs(fingerprint, ECU.CAM, candidate) or is_panda_black - ret.enableDsu = not check_ecu_msgs(fingerprint, ECU.DSU, candidate) or (is_panda_black and candidate in TSS2_CAR) - ret.enableApgs = False #not check_ecu_msgs(fingerprint, ECU.APGS) + ret.enableCamera = is_ecu_disconnected(fingerprint[0], FINGERPRINTS, ECU_FINGERPRINT, candidate, ECU.CAM) or has_relay + ret.enableDsu = is_ecu_disconnected(fingerprint[0], FINGERPRINTS, ECU_FINGERPRINT, candidate, ECU.DSU) or (has_relay and candidate in TSS2_CAR) + ret.enableApgs = False # is_ecu_disconnected(fingerprint[0], FINGERPRINTS, ECU_FINGERPRINT, candidate, ECU.APGS) + ret.enableGasInterceptor = 0x201 in fingerprint[0] ret.openpilotLongitudinalControl = ret.enableCamera and ret.enableDsu - cloudlog.warn("ECU Camera Simulated: %r", ret.enableCamera) - cloudlog.warn("ECU DSU Simulated: %r", ret.enableDsu) - cloudlog.warn("ECU APGS Simulated: %r", ret.enableApgs) - cloudlog.warn("ECU Gas Interceptor: %r", ret.enableGasInterceptor) + cloudlog.warning("ECU Camera Simulated: %r", ret.enableCamera) + cloudlog.warning("ECU DSU Simulated: %r", ret.enableDsu) + cloudlog.warning("ECU APGS Simulated: %r", ret.enableApgs) + cloudlog.warning("ECU Gas Interceptor: %r", ret.enableGasInterceptor) + + # min speed to enable ACC. if car can do stop and go, then set enabling speed + # to a negative value, so it won't matter. + ret.minEnableSpeed = -1. if (stop_and_go or ret.enableGasInterceptor) else 19. * CV.MPH_TO_MS ret.steerLimitAlert = False @@ -283,10 +288,7 @@ class CarInterface(object): def update(self, c, can_strings): # ******************* do can recv ******************* self.cp.update_strings(can_strings) - - # run the cam can update for 10s as we just need to know if the camera is alive - if self.frame < 1000: - self.cp_cam.update_strings(can_strings) + self.cp_cam.update_strings(can_strings) self.CS.update(self.cp) @@ -371,6 +373,8 @@ class CarInterface(object): if self.cp_cam.can_valid: self.forwarding_camera = True + if self.cp_cam.can_invalid_cnt >= 100 and self.CP.enableCamera: + events.append(create_event('invalidGiraffeToyota', [ET.PERMANENT])) if not ret.gearShifter == GearShifter.drive and self.CP.enableDsu: events.append(create_event('wrongGear', [ET.NO_ENTRY, ET.SOFT_DISABLE])) if ret.doorOpen: diff --git a/selfdrive/car/toyota/radar_interface.py b/selfdrive/car/toyota/radar_interface.py index 0a1ce07ea2..a24a911a44 100755 --- a/selfdrive/car/toyota/radar_interface.py +++ b/selfdrive/car/toyota/radar_interface.py @@ -1,9 +1,10 @@ -#!/usr/bin/env python +#!/usr/bin/env python3 import os import time from selfdrive.can.parser import CANParser from cereal import car from selfdrive.car.toyota.values import NO_DSU_CAR, DBC, TSS2_CAR +from selfdrive.car.interfaces import RadarInterfaceBase def _create_radar_can_parser(car_fingerprint): dbc_f = DBC[car_fingerprint]['radar'] @@ -18,22 +19,22 @@ def _create_radar_can_parser(car_fingerprint): msg_a_n = len(RADAR_A_MSGS) msg_b_n = len(RADAR_B_MSGS) - signals = zip(['LONG_DIST'] * msg_a_n + ['NEW_TRACK'] * msg_a_n + ['LAT_DIST'] * msg_a_n + + signals = list(zip(['LONG_DIST'] * msg_a_n + ['NEW_TRACK'] * msg_a_n + ['LAT_DIST'] * msg_a_n + ['REL_SPEED'] * msg_a_n + ['VALID'] * msg_a_n + ['SCORE'] * msg_b_n, RADAR_A_MSGS * 5 + RADAR_B_MSGS, - [255] * msg_a_n + [1] * msg_a_n + [0] * msg_a_n + [0] * msg_a_n + [0] * msg_a_n + [0] * msg_b_n) + [255] * msg_a_n + [1] * msg_a_n + [0] * msg_a_n + [0] * msg_a_n + [0] * msg_a_n + [0] * msg_b_n)) - checks = zip(RADAR_A_MSGS + RADAR_B_MSGS, [20]*(msg_a_n + msg_b_n)) + checks = list(zip(RADAR_A_MSGS + RADAR_B_MSGS, [20]*(msg_a_n + msg_b_n))) return CANParser(os.path.splitext(dbc_f)[0], signals, checks, 1) -class RadarInterface(object): +class RadarInterface(RadarInterfaceBase): def __init__(self, CP): # radar self.pts = {} self.track_id = 0 - self.delay = 0.0 # Delay of radar + self.delay = 0 # Delay of radar if CP.carFingerprint in TSS2_CAR: self.RADAR_A_MSGS = list(range(0x180, 0x190)) @@ -75,7 +76,7 @@ class RadarInterface(object): errors.append("canError") ret.errors = errors - for ii in updated_messages: + for ii in sorted(updated_messages): if ii in self.RADAR_A_MSGS: cpt = self.rcp.vl[ii] @@ -105,5 +106,5 @@ class RadarInterface(object): if ii in self.pts: del self.pts[ii] - ret.points = self.pts.values() + ret.points = list(self.pts.values()) return ret diff --git a/selfdrive/car/toyota/toyotacan.py b/selfdrive/car/toyota/toyotacan.py index b886b1c675..7125ecbf3e 100644 --- a/selfdrive/car/toyota/toyotacan.py +++ b/selfdrive/car/toyota/toyotacan.py @@ -10,7 +10,7 @@ def fix(msg, addr): checksum = idh + idl + len(msg) + 1 for d_byte in msg: - checksum += ord(d_byte) + checksum += d_byte #return msg + chr(checksum & 0xFF) return msg + struct.pack("B", checksum & 0xFF) diff --git a/selfdrive/car/toyota/values.py b/selfdrive/car/toyota/values.py index ad7c85f8c0..4ec85d51e2 100644 --- a/selfdrive/car/toyota/values.py +++ b/selfdrive/car/toyota/values.py @@ -18,6 +18,7 @@ class CAR: LEXUS_ESH_TSS2 = "LEXUS ES 300H 2019" SIENNA = "TOYOTA SIENNA XLE 2018" LEXUS_IS = "LEXUS IS300 2018" + LEXUS_CTH = "LEXUS CT 200H 2018" class ECU: @@ -28,62 +29,56 @@ class ECU: # addr: (ecu, cars, bus, 1/freq*100, vl) STATIC_MSGS = [ - (0x130, ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON), 1, 100, '\x00\x00\x00\x00\x00\x00\x38'), - (0x240, ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON), 1, 5, '\x00\x10\x01\x00\x10\x01\x00'), - (0x241, ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON), 1, 5, '\x00\x10\x01\x00\x10\x01\x00'), - (0x244, ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON), 1, 5, '\x00\x10\x01\x00\x10\x01\x00'), - (0x245, ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON), 1, 5, '\x00\x10\x01\x00\x10\x01\x00'), - (0x248, ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON), 1, 5, '\x00\x00\x00\x00\x00\x00\x01'), - (0x367, ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON), 0, 40, '\x06\x00'), - (0x414, ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON), 0, 100, '\x00\x00\x00\x00\x00\x00\x17\x00'), - (0x466, ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.HIGHLANDER, CAR.HIGHLANDERH), 1, 100, '\x20\x20\xAD'), - (0x466, ECU.CAM, (CAR.COROLLA, CAR.AVALON), 1, 100, '\x24\x20\xB1'), - (0x489, ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON), 0, 100, '\x00\x00\x00\x00\x00\x00\x00'), - (0x48a, ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON), 0, 100, '\x00\x00\x00\x00\x00\x00\x00'), - (0x48b, ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON), 0, 100, '\x66\x06\x08\x0a\x02\x00\x00\x00'), - (0x4d3, ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.AVALON), 0, 100, '\x1C\x00\x00\x01\x00\x00\x00\x00'), + (0x130, ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON), 1, 100, b'\x00\x00\x00\x00\x00\x00\x38'), + (0x240, ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON), 1, 5, b'\x00\x10\x01\x00\x10\x01\x00'), + (0x241, ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON), 1, 5, b'\x00\x10\x01\x00\x10\x01\x00'), + (0x244, ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON), 1, 5, b'\x00\x10\x01\x00\x10\x01\x00'), + (0x245, ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON), 1, 5, b'\x00\x10\x01\x00\x10\x01\x00'), + (0x248, ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON), 1, 5, b'\x00\x00\x00\x00\x00\x00\x01'), + (0x367, ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON), 0, 40, b'\x06\x00'), + (0x414, ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON), 0, 100, b'\x00\x00\x00\x00\x00\x00\x17\x00'), + (0x466, ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.HIGHLANDER, CAR.HIGHLANDERH), 1, 100, b'\x20\x20\xAD'), + (0x466, ECU.CAM, (CAR.COROLLA, CAR.AVALON), 1, 100, b'\x24\x20\xB1'), + (0x489, ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON), 0, 100, b'\x00\x00\x00\x00\x00\x00\x00'), + (0x48a, ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON), 0, 100, b'\x00\x00\x00\x00\x00\x00\x00'), + (0x48b, ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON), 0, 100, b'\x66\x06\x08\x0a\x02\x00\x00\x00'), + (0x4d3, ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.AVALON), 0, 100, b'\x1C\x00\x00\x01\x00\x00\x00\x00'), - (0x128, ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.AVALON), 1, 3, '\xf4\x01\x90\x83\x00\x37'), - (0x128, ECU.DSU, (CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.SIENNA), 1, 3, '\x03\x00\x20\x00\x00\x52'), - (0x141, ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON, CAR.SIENNA), 1, 2, '\x00\x00\x00\x46'), - (0x160, ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON, CAR.SIENNA), 1, 7, '\x00\x00\x08\x12\x01\x31\x9c\x51'), - (0x161, ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.AVALON), 1, 7, '\x00\x1e\x00\x00\x00\x80\x07'), - (0X161, ECU.DSU, (CAR.HIGHLANDERH, CAR.HIGHLANDER, CAR.SIENNA), 1, 7, '\x00\x1e\x00\xd4\x00\x00\x5b'), - (0x283, ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON, CAR.SIENNA), 0, 3, '\x00\x00\x00\x00\x00\x00\x8c'), - (0x2E6, ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH), 0, 3, '\xff\xf8\x00\x08\x7f\xe0\x00\x4e'), - (0x2E7, ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH), 0, 3, '\xa8\x9c\x31\x9c\x00\x00\x00\x02'), - (0x33E, ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH), 0, 20, '\x0f\xff\x26\x40\x00\x1f\x00'), - (0x344, ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON, CAR.SIENNA), 0, 5, '\x00\x00\x01\x00\x00\x00\x00\x50'), - (0x365, ECU.DSU, (CAR.PRIUS, CAR.LEXUS_RXH, CAR.HIGHLANDERH), 0, 20, '\x00\x00\x00\x80\x03\x00\x08'), - (0x365, ECU.DSU, (CAR.RAV4, CAR.RAV4H, CAR.COROLLA, CAR.HIGHLANDER, CAR.AVALON, CAR.SIENNA), 0, 20, '\x00\x00\x00\x80\xfc\x00\x08'), - (0x366, ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.HIGHLANDERH), 0, 20, '\x00\x00\x4d\x82\x40\x02\x00'), - (0x366, ECU.DSU, (CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.AVALON, CAR.SIENNA), 0, 20, '\x00\x72\x07\xff\x09\xfe\x00'), - (0x470, ECU.DSU, (CAR.PRIUS, CAR.LEXUS_RXH), 1, 100, '\x00\x00\x02\x7a'), - (0x470, ECU.DSU, (CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.RAV4H, CAR.SIENNA), 1, 100, '\x00\x00\x01\x79'), - (0x4CB, ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDERH, CAR.HIGHLANDER, CAR.AVALON, CAR.SIENNA), 0, 100, '\x0c\x00\x00\x00\x00\x00\x00\x00'), + (0x128, ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.AVALON), 1, 3, b'\xf4\x01\x90\x83\x00\x37'), + (0x128, ECU.DSU, (CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.SIENNA, CAR.LEXUS_CTH), 1, 3, b'\x03\x00\x20\x00\x00\x52'), + (0x141, ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON, CAR.SIENNA, CAR.LEXUS_CTH), 1, 2, b'\x00\x00\x00\x46'), + (0x160, ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON, CAR.SIENNA, CAR.LEXUS_CTH), 1, 7, b'\x00\x00\x08\x12\x01\x31\x9c\x51'), + (0x161, ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.AVALON), 1, 7, b'\x00\x1e\x00\x00\x00\x80\x07'), + (0X161, ECU.DSU, (CAR.HIGHLANDERH, CAR.HIGHLANDER, CAR.SIENNA, CAR.LEXUS_CTH), 1, 7, b'\x00\x1e\x00\xd4\x00\x00\x5b'), + (0x283, ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON, CAR.SIENNA, CAR.LEXUS_CTH), 0, 3, b'\x00\x00\x00\x00\x00\x00\x8c'), + (0x2E6, ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH), 0, 3, b'\xff\xf8\x00\x08\x7f\xe0\x00\x4e'), + (0x2E7, ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH), 0, 3, b'\xa8\x9c\x31\x9c\x00\x00\x00\x02'), + (0x33E, ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH), 0, 20, b'\x0f\xff\x26\x40\x00\x1f\x00'), + (0x344, ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON, CAR.SIENNA, CAR.LEXUS_CTH), 0, 5, b'\x00\x00\x01\x00\x00\x00\x00\x50'), + (0x365, ECU.DSU, (CAR.PRIUS, CAR.LEXUS_RXH, CAR.HIGHLANDERH), 0, 20, b'\x00\x00\x00\x80\x03\x00\x08'), + (0x365, ECU.DSU, (CAR.RAV4, CAR.RAV4H, CAR.COROLLA, CAR.HIGHLANDER, CAR.AVALON, CAR.SIENNA, CAR.LEXUS_CTH), 0, 20, b'\x00\x00\x00\x80\xfc\x00\x08'), + (0x366, ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.HIGHLANDERH), 0, 20, b'\x00\x00\x4d\x82\x40\x02\x00'), + (0x366, ECU.DSU, (CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.AVALON, CAR.SIENNA, CAR.LEXUS_CTH), 0, 20, b'\x00\x72\x07\xff\x09\xfe\x00'), + (0x470, ECU.DSU, (CAR.PRIUS, CAR.LEXUS_RXH), 1, 100, b'\x00\x00\x02\x7a'), + (0x470, ECU.DSU, (CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.RAV4H, CAR.SIENNA, CAR.LEXUS_CTH), 1, 100, b'\x00\x00\x01\x79'), + (0x4CB, ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDERH, CAR.HIGHLANDER, CAR.AVALON, CAR.SIENNA, CAR.LEXUS_CTH), 0, 100, b'\x0c\x00\x00\x00\x00\x00\x00\x00'), - (0x292, ECU.APGS, (CAR.PRIUS), 0, 3, '\x00\x00\x00\x00\x00\x00\x00\x9e'), - (0x32E, ECU.APGS, (CAR.PRIUS), 0, 20, '\x00\x00\x00\x00\x00\x00\x00\x00'), - (0x396, ECU.APGS, (CAR.PRIUS), 0, 100, '\xBD\x00\x00\x00\x60\x0F\x02\x00'), - (0x43A, ECU.APGS, (CAR.PRIUS), 0, 100, '\x84\x00\x00\x00\x00\x00\x00\x00'), - (0x43B, ECU.APGS, (CAR.PRIUS), 0, 100, '\x00\x00\x00\x00\x00\x00\x00\x00'), - (0x497, ECU.APGS, (CAR.PRIUS), 0, 100, '\x00\x00\x00\x00\x00\x00\x00\x00'), - (0x4CC, ECU.APGS, (CAR.PRIUS), 0, 100, '\x0D\x00\x00\x00\x00\x00\x00\x00'), + (0x292, ECU.APGS, (CAR.PRIUS), 0, 3, b'\x00\x00\x00\x00\x00\x00\x00\x9e'), + (0x32E, ECU.APGS, (CAR.PRIUS), 0, 20, b'\x00\x00\x00\x00\x00\x00\x00\x00'), + (0x396, ECU.APGS, (CAR.PRIUS), 0, 100, b'\xBD\x00\x00\x00\x60\x0F\x02\x00'), + (0x43A, ECU.APGS, (CAR.PRIUS), 0, 100, b'\x84\x00\x00\x00\x00\x00\x00\x00'), + (0x43B, ECU.APGS, (CAR.PRIUS), 0, 100, b'\x00\x00\x00\x00\x00\x00\x00\x00'), + (0x497, ECU.APGS, (CAR.PRIUS), 0, 100, b'\x00\x00\x00\x00\x00\x00\x00\x00'), + (0x4CC, ECU.APGS, (CAR.PRIUS), 0, 100, b'\x0D\x00\x00\x00\x00\x00\x00\x00'), ] ECU_FINGERPRINT = { - ECU.CAM: 0x2e4, # steer torque cmd - ECU.DSU: 0x343, # accel cmd - ECU.APGS: 0x835, # angle cmd + ECU.CAM: [0x2e4], # steer torque cmd + ECU.DSU: [0x343], # accel cmd + ECU.APGS: [0x835], # angle cmd } -def check_ecu_msgs(fingerprint, ecu, car): - # return True if the reference car fingerprint doesn't contain the ecu fingerprint msg or - # fingerprint contains messages normally sent by a given ecu - return any(ECU_FINGERPRINT[ecu] not in finger for finger in FINGERPRINTS[car]) or ECU_FINGERPRINT[ecu] in fingerprint - - FINGERPRINTS = { CAR.RAV4: [{ 36: 8, 37: 8, 170: 8, 180: 8, 186: 4, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 512: 6, 513: 6, 547: 8, 548: 8, 552: 4, 562: 4, 608: 8, 610: 5, 643: 7, 705: 8, 725: 2, 740: 5, 800: 8, 835: 8, 836: 8, 849: 4, 869: 7, 870: 7, 871: 2, 896: 8, 897: 8, 900: 6, 902: 6, 905: 8, 911: 8, 916: 3, 918: 7, 921: 8, 933: 8, 944: 8, 945: 8, 951: 8, 955: 4, 956: 8, 979: 2, 998: 5, 999: 7, 1000: 8, 1001: 8, 1005: 2, 1008: 2, 1014: 8, 1017: 8, 1041: 8, 1042: 8, 1043: 8, 1044: 8, 1056: 8, 1059: 1, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1176: 8, 1177: 8, 1178: 8, 1179: 8, 1180: 8, 1181: 8, 1190: 8, 1191: 8, 1192: 8, 1196: 8, 1227: 8, 1228: 8, 1235: 8, 1237: 8, 1263: 8, 1264: 8, 1279: 8, 1408: 8, 1409: 8, 1410: 8, 1552: 8, 1553: 8, 1554: 8, 1555: 8, 1556: 8, 1557: 8, 1561: 8, 1562: 8, 1568: 8, 1569: 8, 1570: 8, 1571: 8, 1572: 8, 1584: 8, 1589: 8, 1592: 8, 1593: 8, 1595: 8, 1596: 8, 1597: 8, 1600: 8, 1656: 8, 1664: 8, 1728: 8, 1745: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8, 2015: 8, 2024: 8 @@ -153,7 +148,7 @@ FINGERPRINTS = { 36: 8, 37: 8, 166: 8, 170: 8, 180: 8, 295: 8, 296: 8, 426: 6, 452: 8, 466: 8, 467: 8, 550: 8, 552: 4, 560: 7, 562: 6, 581: 5, 608: 8, 610: 8, 643: 7, 658: 8, 713: 8, 728: 8, 740: 5, 761: 8, 764: 8, 800: 8, 810: 2, 812: 8, 814: 8, 818: 8, 824: 8, 829: 2, 830: 7, 835: 8, 836: 8, 869: 7, 870: 7, 871: 2, 888: 8, 889: 8, 898: 8, 900: 6, 902: 6, 905: 8, 918: 8, 921: 8, 933: 8, 934: 8, 935: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 8, 955: 8, 956: 8, 971: 7, 975: 5, 983: 8, 984: 8, 993: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1002: 8, 1011: 8, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1057: 8, 1059: 1, 1071: 8, 1076: 8, 1077: 8, 1082: 8, 1084: 8, 1085: 8, 1086: 8, 1114: 8, 1132: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1228: 8, 1235: 8, 1237: 8, 1264: 8, 1279: 8, 1541: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1595: 8, 1745: 8, 1779: 8, 1786: 8, 1787: 8, 1788: 8, 1789: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8 }], CAR.HIGHLANDER: [{ - 36: 8, 37: 8, 114: 5, 119: 6, 120: 4, 170: 8, 180: 8, 186: 4, 238: 4, 355: 5, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 544: 4, 545: 5, 550: 8, 552: 4, 608: 8, 610: 5, 643: 7, 705: 8, 725: 2, 740: 5, 800: 8, 835: 8, 836: 8, 849: 4, 869: 7, 870: 7, 871: 2, 896: 8, 900: 6, 902: 6, 905: 8, 911: 8, 916: 3, 921: 8, 922: 8, 933: 8, 944: 8, 945: 8, 951: 8, 955: 8, 956: 8, 979: 2, 998: 5, 999: 7, 1000: 8, 1001: 8, 1008: 2, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1043: 8, 1044: 8, 1056: 8, 1059: 1, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1176: 8, 1177: 8, 1178: 8, 1179: 8, 1180: 8, 1181: 8, 1182: 8, 1183: 8, 1189: 8, 1190: 8, 1191: 8, 1192: 8, 1196: 8, 1197: 8, 1198: 8, 1199: 8, 1206: 8, 1207: 8, 1212: 8, 1227: 8, 1235: 8, 1237: 8, 1279: 8, 1408: 8, 1409: 8, 1410: 8, 1552: 8, 1553: 8, 1554: 8, 1556: 8, 1557: 8, 1561: 8, 1562: 8, 1568: 8, 1569: 8, 1570: 8, 1571: 8, 1572: 8, 1584: 8, 1589: 8, 1592: 8, 1593: 8, 1595: 8, 1599: 8, 1656: 8, 1666: 8, 1667: 8, 1728: 8, 1745: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8 + 36: 8, 37: 8, 114: 5, 119: 6, 120: 4, 170: 8, 180: 8, 186: 4, 238: 4, 355: 5, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 544: 4, 545: 5, 550: 8, 552: 4, 608: 8, 610: 5, 643: 7, 705: 8, 725: 2, 740: 5, 800: 8, 835: 8, 836: 8, 849: 4, 869: 7, 870: 7, 871: 2, 896: 8, 900: 6, 902: 6, 905: 8, 911: 8, 916: 3, 921: 8, 922: 8, 933: 8, 944: 8, 945: 8, 951: 8, 955: 8, 956: 8, 979: 2, 998: 5, 999: 7, 1000: 8, 1001: 8, 1008: 2, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1043: 8, 1044: 8, 1056: 8, 1059: 1, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1176: 8, 1177: 8, 1178: 8, 1179: 8, 1180: 8, 1181: 8, 1182: 8, 1183: 8, 1189: 8, 1190: 8, 1191: 8, 1192: 8, 1196: 8, 1197: 8, 1198: 8, 1199: 8, 1206: 8, 1207: 8, 1212: 8, 1227: 8, 1235: 8, 1237: 8, 1279: 8, 1408: 8, 1409: 8, 1410: 8, 1552: 8, 1553: 8, 1554: 8, 1556: 8, 1557: 8, 1561: 8, 1562: 8, 1568: 8, 1569: 8, 1570: 8, 1571: 8, 1572: 8, 1584: 8, 1589: 8, 1592: 8, 1593: 8, 1595: 8, 1599: 8, 1656: 8, 1666: 8, 1667: 8, 1728: 8, 1745: 8, 1779: 8, 1872: 8, 1880: 8, 1904: 8, 1912: 8, 1984: 8, 1988: 8, 1992: 8, 1996: 8, 1990: 8, 1998: 8 }, # 2019 Highlander XLE { @@ -165,6 +160,9 @@ FINGERPRINTS = { }], CAR.HIGHLANDERH: [{ 36: 8, 37: 8, 170: 8, 180: 8, 296: 8, 426: 6, 452: 8, 466: 8, 467: 8, 550: 8, 552: 4, 560: 7, 581: 5, 608: 8, 610: 5, 643: 7, 713: 8, 740: 5, 800: 8, 835: 8, 836: 8, 849: 4, 869: 7, 870: 7, 871: 2, 896: 8, 897: 8, 900: 6, 902: 6, 905: 8, 911: 8, 916: 3, 918: 7, 921: 8, 933: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 3, 955: 8, 956: 8, 979: 2, 998: 5, 999: 7, 1000: 8, 1001: 8, 1005: 2, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1043: 8, 1044: 8, 1056: 8, 1059: 1, 1112: 8, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1176: 8, 1177: 8, 1178: 8, 1179: 8, 1180: 8, 1181: 8, 1184: 8, 1185: 8, 1186: 8, 1189: 8, 1190: 8, 1191: 8, 1192: 8, 1196: 8, 1197: 8, 1198: 8, 1199: 8, 1206: 8, 1212: 8, 1227: 8, 1232: 8, 1235: 8, 1237: 8, 1263: 8, 1264: 8, 1279: 8, 1552: 8, 1553: 8, 1554: 8, 1556: 8, 1557: 8, 1561: 8, 1562: 8, 1568: 8, 1569: 8, 1570: 8, 1571: 8, 1572: 8, 1584: 8, 1589: 8, 1592: 8, 1593: 8, 1595: 8, 1599: 8, 1656: 8, 1728: 8, 1745: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8 + }, + { + 36: 8, 37: 8, 170: 8, 180: 8, 296: 8, 426: 6, 452: 8, 466: 8, 467: 8, 550: 8, 552: 4, 560: 7, 581: 5, 608: 8, 610: 5, 643: 7, 713: 8, 740: 5, 800: 8, 835: 8, 836: 8, 849: 4, 869: 7, 870: 7, 871: 2, 896: 8, 897: 8, 900: 6, 902: 6, 905: 8, 911: 8, 916: 3, 921: 8, 933: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 3, 955: 8, 956: 8, 979: 2, 992: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1043: 8, 1044: 8, 1056: 8, 1057: 8, 1059: 1, 1076: 8, 1077: 8, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1176: 8, 1177: 8, 1178: 8, 1179: 8, 1180: 8, 1181: 8, 1184: 8, 1185: 8, 1186: 8, 1189: 8, 1190: 8, 1191: 8, 1192: 8, 1196: 8, 1197: 8, 1198: 8, 1199: 8, 1206: 8, 1212: 8, 1227: 8, 1232: 8, 1237: 8, 1279: 8, 1552: 8, 1553: 8, 1554: 8, 1556: 8, 1557: 8, 1561: 8, 1562: 8, 1568: 8, 1569: 8, 1570: 8, 1571: 8, 1572: 8, 1584: 8, 1589: 8, 1592: 8, 1593: 8, 1595: 8, 1599: 8, 1656: 8, 1728: 8, 1745: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8 }], CAR.AVALON: [{ 36: 8, 37: 8, 170: 8, 180: 8, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 547: 8, 550: 8, 552: 4, 562: 6, 608: 8, 610: 5, 643: 7, 705: 8, 740: 5, 800: 8, 835: 8, 836: 8, 849: 4, 869: 7, 870: 7, 871: 2, 896: 8, 897: 8, 905: 8, 911: 1, 916: 2, 921: 8, 933: 6, 944: 8, 945: 8, 951: 8, 955: 8, 956: 8, 979: 2, 1005: 2, 1014: 8, 1017: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1059: 1, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1176: 8, 1177: 8, 1178: 8, 1179: 8, 1180: 8, 1181: 8, 1190: 8, 1191: 8, 1192: 8, 1196: 8, 1200: 8, 1201: 8, 1202: 8, 1203: 8, 1206: 8, 1227: 8, 1235: 8, 1237: 8, 1264: 8, 1279: 8, 1552: 8, 1553: 8, 1554: 8, 1555: 8, 1556: 8, 1557: 8, 1558: 8, 1561: 8, 1562: 8, 1568: 8, 1569: 8, 1570: 8, 1571: 8, 1572: 8, 1584: 8, 1589: 8, 1592: 8, 1593: 8, 1595: 8, 1596: 8, 1597: 8, 1664: 8, 1728: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8 @@ -199,6 +197,9 @@ FINGERPRINTS = { { 36: 8, 37: 8, 170: 8, 180: 8, 295: 8, 296: 8, 400: 6, 426: 6, 452: 8, 466: 8, 467: 8, 550: 8, 552: 4, 560: 7, 581: 5, 608: 8, 610: 5, 643: 7, 713: 8, 740: 5, 800: 8, 836: 8, 845: 5, 849: 4, 869: 7, 870: 7, 871: 2, 896: 8, 897: 8, 900: 6, 902: 6, 905: 8, 911: 8, 913: 8, 916: 3, 918: 7, 921: 7, 933: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 3, 955: 8, 956: 8, 979: 2, 992: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1009: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1043: 8, 1044: 8, 1056: 8, 1057: 8, 1059: 1, 1112: 8, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1168: 1, 1176: 8, 1177: 8, 1178: 8, 1179: 8, 1180: 8, 1181: 8, 1184: 8, 1185: 8, 1186: 8, 1187: 8, 1189: 8, 1190: 8, 1191: 8, 1192: 8, 1196: 8, 1197: 8, 1198: 8, 1199: 8, 1206: 8, 1208: 8, 1212: 8, 1227: 8, 1232: 8, 1235: 8, 1279: 8, 1408: 8, 1409: 8, 1410: 8, 1552: 8, 1553: 8, 1554: 8, 1555: 8, 1556: 8, 1557: 8, 1561: 8, 1568: 8, 1569: 8, 1570: 8, 1571: 8, 1572: 8, 1575: 8, 1584: 8, 1589: 8, 1592: 8, 1593: 8, 1595: 8, 1599: 8, 1728: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8 }], + CAR.LEXUS_CTH: [{ + 36: 8, 37: 8, 170: 8, 180: 8, 288: 8, 426: 6, 452: 8, 466: 8, 467: 8, 548: 8, 552: 4, 560: 7, 581: 5, 608: 8, 610: 5, 643: 7, 713: 8, 740: 5, 800: 8, 810: 2, 832: 8, 835: 8, 836: 8, 849: 4, 869: 7, 870: 7, 871: 2, 897: 8, 900: 6, 902: 6, 905: 8, 911: 8, 916: 1, 921: 8, 933: 8, 944: 6, 945: 8, 950: 8, 951: 8, 953: 3, 955: 4, 956: 8, 979: 2, 992: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1017: 8, 1041: 8, 1042: 8, 1043: 8, 1056: 8, 1057: 8, 1059: 1, 1076: 8, 1077: 8, 1114: 8, 1116: 8, 1160: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1176: 8, 1177: 8, 1178: 8, 1179: 8, 1180: 8, 1181: 8, 1184: 8, 1185: 8, 1186: 8, 1190: 8, 1191: 8, 1192: 8, 1227: 8, 1235: 8, 1279: 8, 1552: 8, 1553: 8, 1554: 8, 1555: 8, 1556: 8, 1557: 8, 1558: 8, 1561: 8, 1562: 8, 1568: 8, 1569: 8, 1570: 8, 1571: 8, 1572: 8, 1575: 8, 1584: 8, 1589: 8, 1592: 8, 1593: 8, 1595: 8, 1664: 8, 1728: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8 + }] } STEER_THRESHOLD = 100 @@ -221,6 +222,7 @@ DBC = { CAR.LEXUS_ESH_TSS2: dbc_dict('toyota_nodsu_hybrid_pt_generated', 'toyota_tss2_adas'), CAR.SIENNA: dbc_dict('toyota_sienna_xle_2018_pt_generated', 'toyota_adas'), CAR.LEXUS_IS: dbc_dict('lexus_is_2018_pt_generated', 'toyota_adas'), + CAR.LEXUS_CTH: dbc_dict('lexus_ct200h_2018_pt_generated', 'toyota_adas'), } NO_DSU_CAR = [CAR.CHR, CAR.CHRH, CAR.CAMRY, CAR.CAMRYH, CAR.RAV4_TSS2, CAR.COROLLA_TSS2, CAR.LEXUS_ESH_TSS2] diff --git a/common/vin.py b/selfdrive/car/vin.py similarity index 83% rename from common/vin.py rename to selfdrive/car/vin.py index 989aa0fa13..76fd34ca6d 100755 --- a/common/vin.py +++ b/selfdrive/car/vin.py @@ -1,4 +1,4 @@ -#!/usr/bin/env python +#!/usr/bin/env python3 import selfdrive.messaging as messaging from selfdrive.boardd.boardd import can_list_to_can_capnp @@ -6,9 +6,6 @@ VIN_UNKNOWN = "0" * 17 # sanity checks on response messages from vin query def is_vin_response_valid(can_dat, step, cnt): - - can_dat = [ord(i) for i in can_dat] - if len(can_dat) != 8: # ISO-TP meesages are all 8 bytes return False @@ -36,17 +33,17 @@ class VinQuery(): 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, '\x02\x09\x02'.ljust(8, "\x00"), bus], - [0x18DA10f1, 0, '\x30'.ljust(8, "\x00"), bus]] - self.query_nor_msgs = [[0x7df, 0, '\x02\x09\x02'.ljust(8, "\x00"), bus], - [0x7e0, 0, '\x30'.ljust(8, "\x00"), bus]] + 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 = [] + self.dat = b"" self.vin = VIN_UNKNOWN def check_response(self, msg): @@ -55,7 +52,7 @@ class VinQuery(): self.never_responded = False # basic sanity checks on ISO-TP response if is_vin_response_valid(msg.dat, self.step, self.cnt): - self.dat += msg.dat[2:] if self.step == 0 else msg.dat[1:] + 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 @@ -73,7 +70,7 @@ class VinQuery(): def get_vin(self): # only report vin if procedure is finished if self.step == len(self.cnts) and self.cnt == self.cnts[-1]: - self.vin = "".join(self.dat[3:]) + self.vin = self.dat[3:].decode('utf8') return self.vin @@ -98,4 +95,4 @@ if __name__ == "__main__": from selfdrive.services import service_list logcan = messaging.sub_sock(service_list['can'].port) sendcan = messaging.pub_sock(service_list['sendcan'].port) - print get_vin(logcan, sendcan, 0) + print(get_vin(logcan, sendcan, 0)) diff --git a/selfdrive/common/params.cc b/selfdrive/common/params.cc index 723b06d921..0bdf6118a6 100644 --- a/selfdrive/common/params.cc +++ b/selfdrive/common/params.cc @@ -16,6 +16,7 @@ #include "common/util.h" #include "common/utilpp.h" + namespace { template @@ -28,8 +29,42 @@ static const char* default_params_path = null_coalesce( } // namespace +static int fsync_dir(const char* path){ + int result = 0; + int fd = open(path, O_RDONLY); + + if (fd < 0){ + result = -1; + goto cleanup; + } + + result = fsync(fd); + if (result < 0) { + goto cleanup; + } + + cleanup: + int result_close = 0; + if (fd >= 0){ + result_close = close(fd); + } + + if (result_close < 0) { + return result_close; + } else { + return result; + } +} + int write_db_value(const char* params_path, const char* key, const char* value, size_t value_size) { + // Information about safely and atomically writing a file: https://lwn.net/Articles/457667/ + // 1) Create temp file + // 2) Write data to temp file + // 3) fsync() the temp file + // 4) rename the temp file to the real name + // 5) fsync() the containing directory + int lock_fd = -1; int tmp_fd = -1; int result; @@ -55,12 +90,14 @@ int write_db_value(const char* params_path, const char* key, const char* value, goto cleanup; } + // Build lock path result = snprintf(path, sizeof(path), "%s/.lock", params_path); if (result < 0) { goto cleanup; } lock_fd = open(path, 0); + // Build key path result = snprintf(path, sizeof(path), "%s/d/%s", params_path, key); if (result < 0) { goto cleanup; @@ -80,6 +117,20 @@ int write_db_value(const char* params_path, const char* key, const char* value, // Move temp into place. result = rename(tmp_path, path); + if (result < 0) { + goto cleanup; + } + + // fsync parent directory + result = snprintf(path, sizeof(path), "%s/d", params_path); + if (result < 0) { + goto cleanup; + } + + result = fsync_dir(path); + if (result < 0) { + goto cleanup; + } cleanup: // Release lock. @@ -95,6 +146,60 @@ cleanup: return result; } +int delete_db_value(const char* params_path, const char* key) { + int lock_fd = -1; + int result; + char path[1024]; + + if (params_path == NULL) { + params_path = default_params_path; + } + + // Build lock path, and open lockfile + result = snprintf(path, sizeof(path), "%s/.lock", params_path); + if (result < 0) { + goto cleanup; + } + lock_fd = open(path, 0); + + // Take lock. + result = flock(lock_fd, LOCK_EX); + if (result < 0) { + goto cleanup; + } + + // Build key path + result = snprintf(path, sizeof(path), "%s/d/%s", params_path, key); + if (result < 0) { + goto cleanup; + } + + // Delete value. + result = remove(path); + if (result != 0) { + result = ERR_NO_VALUE; + goto cleanup; + } + + // fsync parent directory + result = snprintf(path, sizeof(path), "%s/d", params_path); + if (result < 0) { + goto cleanup; + } + + result = fsync_dir(path); + if (result < 0) { + goto cleanup; + } + +cleanup: + // Release lock. + if (lock_fd >= 0) { + close(lock_fd); + } + return result; +} + int read_db_value(const char* params_path, const char* key, char** value, size_t* value_sz) { int lock_fd = -1; diff --git a/selfdrive/common/params.h b/selfdrive/common/params.h index 299dcccd0a..6dcd36145e 100644 --- a/selfdrive/common/params.h +++ b/selfdrive/common/params.h @@ -7,6 +7,8 @@ extern "C" { #endif +#define ERR_NO_VALUE -33 + int write_db_value(const char* params_path, const char* key, const char* value, size_t value_size); @@ -23,6 +25,10 @@ int write_db_value(const char* params_path, const char* key, const char* value, int read_db_value(const char* params_path, const char* key, char** value, size_t* value_sz); +// Delete a value from the params database. +// Inputs are the same as read_db_value, without value and value_sz. +int delete_db_value(const char* params_path, const char* key); + // Reads a value from the params database, blocking until successful. // Inputs are the same as read_db_value. void read_db_value_blocking(const char* params_path, const char* key, diff --git a/selfdrive/common/spinner.c b/selfdrive/common/spinner.c index c8bfde8c7d..abae7cc135 100644 --- a/selfdrive/common/spinner.c +++ b/selfdrive/common/spinner.c @@ -4,6 +4,7 @@ #include #include #include +#include #include #include @@ -17,6 +18,8 @@ #include "framebuffer.h" #include "spinner.h" +#define SPINTEXT_LENGTH 128 + // external resources linked in extern const unsigned char _binary_opensans_semibold_ttf_start[]; extern const unsigned char _binary_opensans_semibold_ttf_end[]; @@ -27,14 +30,30 @@ extern const unsigned char _binary_img_spinner_track_png_end[]; extern const unsigned char _binary_img_spinner_comma_png_start[]; extern const unsigned char _binary_img_spinner_comma_png_end[]; +bool stdin_input_available() { + struct timeval timeout; + timeout.tv_sec = 0; + timeout.tv_usec = 0; + + fd_set fds; + FD_ZERO(&fds); + FD_SET(STDIN_FILENO, &fds); + select(STDIN_FILENO+1, &fds, NULL, NULL, &timeout); + return (FD_ISSET(0, &fds)); +} + int spin(int argc, char** argv) { int err; - const char* spintext = NULL; - if (argc == 1) { - spintext = argv[0]; - } else if (argc >= 2) { - spintext = argv[1]; + bool draw_progress = false; + float progress_val = 0.0; + + char spintext[SPINTEXT_LENGTH]; + spintext[0] = 0; + + const char* spintext_arg = NULL; + if (argc >= 2) { + strncpy(spintext, argv[1], SPINTEXT_LENGTH); } // spinner @@ -62,6 +81,30 @@ int spin(int argc, char** argv) { assert(spinner_comma_img >= 0); for (int cnt = 0; ; cnt++) { + // Check stdin for new text + if (stdin_input_available()){ + fgets(spintext, SPINTEXT_LENGTH, stdin); + spintext[strcspn(spintext, "\n")] = 0; + + // Check if number (update progress bar) + size_t len = strlen(spintext); + bool is_number = len > 0; + for (int i = 0; i < len; i++){ + if (!isdigit(spintext[i])){ + is_number = false; + break; + } + } + + if (is_number) { + progress_val = (float)(atoi(spintext)) / 100.0; + progress_val = fmin(1.0, progress_val); + progress_val = fmax(0.0, progress_val); + } + + draw_progress = is_number; + } + glClearColor(0.1, 0.1, 0.1, 1.0); glClear(GL_STENCIL_BUFFER_BIT | GL_COLOR_BUFFER_BIT); glEnable(GL_BLEND); @@ -97,8 +140,36 @@ int spin(int argc, char** argv) { nvgRect(vg, spinner_img_x, spinner_img_y, spinner_img_s, spinner_img_s); nvgFill(vg); - // message - if (spintext) { + if (draw_progress){ + // draw progress bar + int progress_width = 1000; + int progress_x = fb_w/2-progress_width/2; + int progress_y = 775; + int progress_height = 25; + + NVGpaint paint = nvgBoxGradient( + vg, progress_x + 1, progress_y + 1, + progress_width - 2, progress_height, 3, 4, nvgRGB(27, 27, 27), nvgRGB(27, 27, 27)); + nvgBeginPath(vg); + nvgRoundedRect(vg, progress_x, progress_y, progress_width, progress_height, 12); + nvgFillPaint(vg, paint); + nvgFill(vg); + + int bar_pos = ((progress_width - 2) * progress_val); + + paint = nvgBoxGradient( + vg, progress_x, progress_y, + bar_pos+1.5f, progress_height-1, 3, 4, + nvgRGB(245, 245, 245), nvgRGB(105, 105, 105)); + + nvgBeginPath(vg); + nvgRoundedRect( + vg, progress_x+1, progress_y+1, + bar_pos, progress_height-2, 12); + nvgFillPaint(vg, paint); + nvgFill(vg); + } else { + // message nvgTextAlign(vg, NVG_ALIGN_CENTER | NVG_ALIGN_TOP); nvgFontSize(vg, 96.0f); nvgText(vg, fb_w/2, (fb_h*2/3)+24, spintext, NULL); diff --git a/selfdrive/common/version.h b/selfdrive/common/version.h index 10517c6799..2c4cfa6a84 100644 --- a/selfdrive/common/version.h +++ b/selfdrive/common/version.h @@ -1 +1 @@ -#define COMMA_VERSION "0.6.4-release" +#define COMMA_VERSION "0.6.5-release" diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 558c1ba15b..2e7753614a 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -1,7 +1,9 @@ -#!/usr/bin/env python +#!/usr/bin/env python3 import os import gc +import json import capnp +import zmq from cereal import car, log from common.numpy_fast import clip from common.realtime import sec_since_boot, set_realtime_priority, Ratekeeper, DT_CTRL @@ -31,6 +33,7 @@ from selfdrive.locationd.calibration_helpers import Calibration, Filter ThermalStatus = log.ThermalData.ThermalStatus State = log.ControlsState.OpenpilotState +HwType = log.HealthData.HwType def isActive(state): @@ -52,12 +55,12 @@ def events_to_bytes(events): return ret -def data_sample(CI, CC, sm, can_sock, cal_status, cal_perc, overtemp, free_space, low_battery, +def data_sample(CI, CC, sm, can_poller, can_sock, cal_status, cal_perc, overtemp, free_space, low_battery, driver_status, state, mismatch_counter, params): """Receive data from sockets and create events for battery, temperature and disk space""" # Update carstate from CAN and create events - can_strs = messaging.drain_sock_raw(can_sock, wait_for_one=True) + can_strs = messaging.drain_sock_raw_poller(can_poller, can_sock, wait_for_one=True) CS = CI.update(CC, can_strs) sm.update(0) @@ -87,7 +90,7 @@ def data_sample(CI, CC, sm, can_sock, cal_status, cal_perc, overtemp, free_space is_rhd = is_rhd_region(sm['gpsLocation'].latitude, sm['gpsLocation'].longitude) driver_status.is_rhd_region = is_rhd driver_status.is_rhd_region_checked = True - put_nonblocking("IsRHD", str(int(is_rhd))) + put_nonblocking("IsRHD", "1" if is_rhd else "0") # Handle calibration if sm.updated['liveCalibration']: @@ -217,7 +220,7 @@ def state_transition(frame, CS, CP, state, events, soft_disable_timer, v_cruise_ def state_control(frame, rcv_frame, plan, path_plan, CS, CP, state, events, v_cruise_kph, v_cruise_kph_last, - AM, rk, driver_status, LaC, LoC, VM, read_only, is_metric, cal_perc): + AM, rk, driver_status, LaC, LoC, read_only, is_metric, cal_perc): """Given the state, this function returns an actuators packet""" actuators = car.CarControl.Actuators.new_message() @@ -264,7 +267,7 @@ def state_control(frame, rcv_frame, plan, path_plan, CS, CP, state, events, v_cr actuators.gas, actuators.brake = LoC.update(active, CS.vEgo, CS.brakePressed, CS.standstill, CS.cruiseState.standstill, v_cruise_kph, v_acc_sol, plan.vTargetFuture, a_acc_sol, CP) # Steering PID loop and lateral MPC - actuators.steer, actuators.steerAngle, lac_log = LaC.update(active, CS.vEgo, CS.steeringAngle, CS.steeringRate, CS.steeringTorqueEps, CS.steeringPressed, CP, VM, path_plan) + actuators.steer, actuators.steerAngle, lac_log = LaC.update(active, CS.vEgo, CS.steeringAngle, CS.steeringRate, CS.steeringTorqueEps, CS.steeringPressed, CP, path_plan) # Send a "steering required alert" if saturation count has reached the limit if LaC.sat_flag and CP.steerLimitAlert: @@ -342,7 +345,7 @@ def data_send(sm, pm, CS, CI, CP, VM, state, events, actuators, v_cruise_kph, rk "alertBlinkingRate": AM.alert_rate, "alertType": AM.alert_type, "alertSound": AM.audible_alert, - "awarenessStatus": max(driver_status.awareness, 0.0) if isEnabled(state) else 0.0, + "awarenessStatus": max(driver_status.awareness, -0.1) if isEnabled(state) else 1.0, "driverMonitoringOn": bool(driver_status.face_detected), "canMonoTimes": list(CS.canMonoTimes), "planMonoTime": sm.logMonoTime['plan'], @@ -424,8 +427,11 @@ def controlsd_thread(sm=None, pm=None, can_sock=None): params = Params() - is_metric = params.get("IsMetric") == "1" - passive = params.get("Passive") != "0" + is_metric = params.get("IsMetric", encoding='utf8') == "1" + passive = params.get("Passive", encoding='utf8') == "1" + openpilot_enabled_toggle = params.get("OpenpilotEnabledToggle", encoding='utf8') == "1" + + passive = passive or not openpilot_enabled_toggle # Pub/Sub Sockets if pm is None: @@ -435,17 +441,20 @@ def controlsd_thread(sm=None, pm=None, can_sock=None): sm = messaging.SubMaster(['thermal', 'health', 'liveCalibration', 'driverMonitoring', 'plan', 'pathPlan', \ 'gpsLocation'], ignore_alive=['gpsLocation']) + can_poller = zmq.Poller() + if can_sock is None: can_timeout = None if os.environ.get('NO_CAN_TIMEOUT', False) else 100 can_sock = messaging.sub_sock(service_list['can'].port, timeout=can_timeout) + can_poller.register(can_sock) # wait for health and CAN packets hw_type = messaging.recv_one(sm.sock['health']).health.hwType - is_panda_black = hw_type == log.HealthData.HwType.blackPanda + has_relay = hw_type in [HwType.blackPanda, HwType.uno] print("Waiting for CAN messages...") get_one_can(can_sock) - CI, CP = get_car(can_sock, pm.sock['sendcan'], is_panda_black) + CI, CP = get_car(can_sock, pm.sock['sendcan'], has_relay) car_recognized = CP.carName != 'mock' # If stock camera is disconnected, we loaded car controls and it's not chffrplus @@ -500,6 +509,10 @@ def controlsd_thread(sm=None, pm=None, can_sock=None): # controlsd is driven by can recv, expected at 100Hz rk = Ratekeeper(100, print_delay_threshold=None) + # FIXME: offroad alerts should not be created with negative severity + connectivity_alert = params.get("Offroad_ConnectivityNeeded", encoding='utf8') + internet_needed = connectivity_alert is not None and json.loads(connectivity_alert.replace("'", "\""))["severity"] >= 0 + prof = Profiler(False) # off by default while True: @@ -508,7 +521,7 @@ def controlsd_thread(sm=None, pm=None, can_sock=None): # Sample data and compute car events CS, events, cal_status, cal_perc, overtemp, free_space, low_battery, mismatch_counter =\ - data_sample(CI, CC, sm, can_sock, cal_status, cal_perc, overtemp, free_space, low_battery, + data_sample(CI, CC, sm, can_poller, can_sock, cal_status, cal_perc, overtemp, free_space, low_battery, driver_status, state, mismatch_counter, params) prof.checkpoint("Sample") @@ -531,6 +544,8 @@ def controlsd_thread(sm=None, pm=None, can_sock=None): events.append(create_event('canError', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) if not sounds_available: events.append(create_event('soundsUnavailable', [ET.NO_ENTRY, ET.PERMANENT])) + if internet_needed: + events.append(create_event('internetConnectivityNeeded', [ET.NO_ENTRY, 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: @@ -545,7 +560,7 @@ def controlsd_thread(sm=None, pm=None, can_sock=None): # Compute actuators (runs PID loops and lateral MPC) actuators, v_cruise_kph, driver_status, v_acc, a_acc, lac_log = \ state_control(sm.frame, sm.rcv_frame, sm['plan'], sm['pathPlan'], CS, CP, state, events, v_cruise_kph, v_cruise_kph_last, AM, rk, - driver_status, LaC, LoC, VM, read_only, is_metric, cal_perc) + driver_status, LaC, LoC, read_only, is_metric, cal_perc) prof.checkpoint("State Control") diff --git a/selfdrive/controls/lib/alertmanager.py b/selfdrive/controls/lib/alertmanager.py index 77196273b6..b6aa97b6fd 100644 --- a/selfdrive/controls/lib/alertmanager.py +++ b/selfdrive/controls/lib/alertmanager.py @@ -10,7 +10,7 @@ AlertStatus = log.ControlsState.AlertStatus VisualAlert = car.CarControl.HUDControl.VisualAlert AudibleAlert = car.CarControl.HUDControl.AudibleAlert -class AlertManager(object): +class AlertManager(): def __init__(self): self.activealerts = [] diff --git a/selfdrive/controls/lib/alerts.py b/selfdrive/controls/lib/alerts.py index fbef59fe4d..0b05498994 100644 --- a/selfdrive/controls/lib/alerts.py +++ b/selfdrive/controls/lib/alerts.py @@ -14,7 +14,7 @@ AlertStatus = log.ControlsState.AlertStatus AudibleAlert = car.CarControl.HUDControl.AudibleAlert VisualAlert = car.CarControl.HUDControl.VisualAlert -class Alert(object): +class Alert(): def __init__(self, alert_type, alert_text_1, @@ -124,7 +124,7 @@ ALERTS = [ Alert( "preDriverUnresponsive", - "TOUCH STEERING WHEEL: No Driver Monitoring", + "TOUCH STEERING WHEEL: No Face Detected", "", AlertStatus.normal, AlertSize.small, Priority.LOW, VisualAlert.steerRequired, AudibleAlert.none, .0, .1, .1, alert_rate=0.75), @@ -218,7 +218,7 @@ ALERTS = [ "TAKE CONTROL", "Steer Unavailable Below ", AlertStatus.userPrompt, AlertSize.mid, - Priority.MID, VisualAlert.steerRequired, AudibleAlert.chimeWarning1, 0., 0., .1), + Priority.MID, VisualAlert.steerRequired, AudibleAlert.none, 0., 0.4, .3), Alert( "debugAlert", @@ -633,11 +633,18 @@ ALERTS = [ Alert( "commIssueNoEntry", - "openpilot unavailable", + "openpilot Unavailable", "Communication Issue between Processes", AlertStatus.normal, AlertSize.mid, Priority.LOW, VisualAlert.none, AudibleAlert.chimeDisengage, .4, 2., 3.), + Alert( + "internetConnectivityNeededNoEntry", + "openpilot Unavailable", + "Internet Connectivity Needed", + AlertStatus.normal, AlertSize.mid, + Priority.LOW, VisualAlert.none, AudibleAlert.chimeDisengage, .4, 2., 3.), + # permanent alerts Alert( "steerUnavailablePermanent", @@ -674,6 +681,20 @@ ALERTS = [ AlertStatus.normal, AlertSize.mid, Priority.LOW_LOWEST, VisualAlert.none, AudibleAlert.none, 0., 0., .2), + Alert( + "invalidGiraffeToyotaPermanent", + "Unsupported Giraffe Configuration", + "Visit comma.ai/tg", + AlertStatus.normal, AlertSize.mid, + Priority.LOW_LOWEST, VisualAlert.none, AudibleAlert.none, 0., 0., .2), + + Alert( + "internetConnectivityNeededPermanent", + "Internet Connectivity Needed", + "Check for Updates to Be Able to Engage", + AlertStatus.normal, AlertSize.mid, + Priority.LOW_LOWEST, VisualAlert.none, AudibleAlert.none, 0., 0., .2), + Alert( "sensorDataInvalidPermanent", "No Data from EON Sensors", diff --git a/selfdrive/controls/lib/alerts_offroad.json b/selfdrive/controls/lib/alerts_offroad.json new file mode 100644 index 0000000000..52a55f23a6 --- /dev/null +++ b/selfdrive/controls/lib/alerts_offroad.json @@ -0,0 +1,19 @@ +{ + "Offroad_ChargeDisabled": { + "text": "EON charging disabled after car being off for more than 3 days. Turn ignition on to start charging again.", + "severity": 0 + }, + "Offroad_TemperatureTooHigh": { + "text": "EON temperature too high. System won't start.", + "severity": 1 + }, + "Offroad_ConnectivityNeededPrompt": { + "text": "Connect to internet to check for updates. openpilot won't engage in ", + "severity": 0, + "_comment": "Append the number of days at the end of the text" + }, + "Offroad_ConnectivityNeeded": { + "text": "Connect to internet to check for updates. openpilot won't engage.", + "severity": 1 + } +} diff --git a/selfdrive/controls/lib/driver_monitor.py b/selfdrive/controls/lib/driver_monitor.py index 263b03af34..9e94c4fe62 100644 --- a/selfdrive/controls/lib/driver_monitor.py +++ b/selfdrive/controls/lib/driver_monitor.py @@ -13,7 +13,7 @@ _DISTRACTED_PROMPT_TIME_TILL_TERMINAL = 6. _FACE_THRESHOLD = 0.4 _EYE_THRESHOLD = 0.4 -_BLINK_THRESHOLD = 0.2 # 0.225 +_BLINK_THRESHOLD = 0.5 # 0.225 _PITCH_WEIGHT = 1.35 # 1.5 # pitch matters a lot more _METRIC_THRESHOLD = 0.4 _PITCH_POS_ALLOWANCE = 0.04 # 0.08 # rad, to not be too sensitive on positive pitch @@ -29,13 +29,13 @@ _POSE_OFFSET_MAX_COUNT = 3600 # stop deweighting new data after 6 min, aka "shor _RECOVERY_FACTOR_MAX = 5. # relative to minus step change _RECOVERY_FACTOR_MIN = 1.25 # relative to minus step change -MAX_TERMINAL_ALERTS = 3 # not allowed to engage after 3 terminal alerts +MAX_TERMINAL_ALERTS = 3 # not allowed to engage after 3 terminal alerts # model output refers to center of cropped image, so need to apply the x displacement offset RESIZED_FOCAL = 320.0 H, W, FULL_W = 320, 160, 426 -class DistractedType(object): +class DistractedType(): NOT_DISTRACTED = 0 BAD_POSE = 1 BAD_BLINK = 2 @@ -49,8 +49,8 @@ def head_orientation_from_descriptor(angles_desc, pos_desc, rpy_calib): roll_prnet = angles_desc[2] face_pixel_position = ((pos_desc[0] + .5)*W - W + FULL_W, (pos_desc[1]+.5)*H) - yaw_focal_angle = np.arctan2(face_pixel_position[0] - FULL_W/2, RESIZED_FOCAL) - pitch_focal_angle = np.arctan2(face_pixel_position[1] - H/2, RESIZED_FOCAL) + yaw_focal_angle = np.arctan2(face_pixel_position[0] - FULL_W//2, RESIZED_FOCAL) + pitch_focal_angle = np.arctan2(face_pixel_position[1] - H//2, RESIZED_FOCAL) roll = roll_prnet pitch = pitch_prnet + pitch_focal_angle @@ -142,7 +142,7 @@ class DriverStatus(): if pose_metric > _METRIC_THRESHOLD: return DistractedType.BAD_POSE - elif blink.left_blink>_BLINK_THRESHOLD and blink.right_blink>_BLINK_THRESHOLD: + elif (blink.left_blink + blink.right_blink)*0.5 > _BLINK_THRESHOLD: return DistractedType.BAD_BLINK else: return DistractedType.NOT_DISTRACTED diff --git a/selfdrive/controls/lib/fcw.py b/selfdrive/controls/lib/fcw.py index c080fecb4a..93570a41db 100644 --- a/selfdrive/controls/lib/fcw.py +++ b/selfdrive/controls/lib/fcw.py @@ -7,7 +7,7 @@ _FCW_A_ACT_V = [-3., -2.] _FCW_A_ACT_BP = [0., 30.] -class FCWChecker(object): +class FCWChecker(): def __init__(self): self.reset_lead(0.0) diff --git a/selfdrive/controls/lib/lane_planner.py b/selfdrive/controls/lib/lane_planner.py index 38c39416d3..06c298e98e 100644 --- a/selfdrive/controls/lib/lane_planner.py +++ b/selfdrive/controls/lib/lane_planner.py @@ -26,13 +26,13 @@ def calc_d_poly(l_poly, r_poly, p_poly, l_prob, r_prob, lane_width): path_from_right_lane = r_poly.copy() path_from_right_lane[3] += lane_width / 2.0 - lr_prob = l_prob + r_prob - l_prob * r_prob + lr_prob = l_prob * r_prob d_poly_lane = (l_prob * path_from_left_lane + r_prob * path_from_right_lane) / (l_prob + r_prob + 0.0001) return lr_prob * d_poly_lane + (1.0 - lr_prob) * p_poly -class LanePlanner(object): +class LanePlanner(): def __init__(self): self.l_poly = [0., 0., 0., 0.] self.r_poly = [0., 0., 0., 0.] diff --git a/selfdrive/controls/lib/latcontrol_indi.py b/selfdrive/controls/lib/latcontrol_indi.py index 38ba8883b9..82bd6e6d79 100644 --- a/selfdrive/controls/lib/latcontrol_indi.py +++ b/selfdrive/controls/lib/latcontrol_indi.py @@ -9,7 +9,7 @@ from selfdrive.car import apply_toyota_steer_torque_limits from selfdrive.controls.lib.drive_helpers import get_steer_max -class LatControlINDI(object): +class LatControlINDI(): def __init__(self, CP): self.angle_steers_des = 0. @@ -47,7 +47,7 @@ class LatControlINDI(object): self.output_steer = 0. self.counter = 0 - def update(self, active, v_ego, angle_steers, angle_steers_rate, eps_torque, steer_override, CP, VM, path_plan): + def update(self, active, v_ego, angle_steers, angle_steers_rate, eps_torque, steer_override, CP, path_plan): # Update Kalman filter y = np.matrix([[math.radians(angle_steers)], [math.radians(angle_steers_rate)]]) self.x = np.dot(self.A_K, self.x) + np.dot(self.K, y) diff --git a/selfdrive/controls/lib/latcontrol_lqr.py b/selfdrive/controls/lib/latcontrol_lqr.py index 23d91cae43..1badf2d268 100644 --- a/selfdrive/controls/lib/latcontrol_lqr.py +++ b/selfdrive/controls/lib/latcontrol_lqr.py @@ -4,7 +4,7 @@ from common.numpy_fast import clip from cereal import log -class LatControlLQR(object): +class LatControlLQR(): def __init__(self, CP, rate=100): self.sat_flag = False self.scale = CP.lateralTuning.lqr.scale @@ -29,7 +29,7 @@ class LatControlLQR(object): self.i_lqr = 0.0 self.output_steer = 0.0 - def update(self, active, v_ego, angle_steers, angle_steers_rate, eps_torque, steer_override, CP, VM, path_plan): + def update(self, active, v_ego, angle_steers, angle_steers_rate, eps_torque, steer_override, CP, path_plan): lqr_log = log.ControlsState.LateralLQRState.new_message() steers_max = get_steer_max(CP, v_ego) diff --git a/selfdrive/controls/lib/latcontrol_pid.py b/selfdrive/controls/lib/latcontrol_pid.py index 5fe456d881..461203dec5 100644 --- a/selfdrive/controls/lib/latcontrol_pid.py +++ b/selfdrive/controls/lib/latcontrol_pid.py @@ -4,7 +4,7 @@ from cereal import car from cereal import log -class LatControlPID(object): +class LatControlPID(): def __init__(self, CP): self.pid = PIController((CP.lateralTuning.pid.kpBP, CP.lateralTuning.pid.kpV), (CP.lateralTuning.pid.kiBP, CP.lateralTuning.pid.kiV), @@ -14,7 +14,7 @@ class LatControlPID(object): def reset(self): self.pid.reset() - def update(self, active, v_ego, angle_steers, angle_steers_rate, eps_torque, steer_override, CP, VM, path_plan): + def update(self, active, v_ego, angle_steers, angle_steers_rate, eps_torque, steer_override, CP, path_plan): pid_log = log.ControlsState.LateralPIDState.new_message() pid_log.steerAngle = float(angle_steers) pid_log.steerRate = float(angle_steers_rate) diff --git a/selfdrive/controls/lib/lateral_mpc/lateral_mpc.d b/selfdrive/controls/lib/lateral_mpc/lateral_mpc.d new file mode 100644 index 0000000000..6cac7c2e14 --- /dev/null +++ b/selfdrive/controls/lib/lateral_mpc/lateral_mpc.d @@ -0,0 +1,3 @@ +lateral_mpc.o: lateral_mpc.c lib_mpc_export/acado_common.h \ + lib_mpc_export/acado_qpoases_interface.hpp \ + lib_mpc_export/acado_auxiliary_functions.h diff --git a/selfdrive/controls/lib/lateral_mpc/lateral_mpc.o b/selfdrive/controls/lib/lateral_mpc/lateral_mpc.o new file mode 100644 index 0000000000..0e19c553f7 --- /dev/null +++ b/selfdrive/controls/lib/lateral_mpc/lateral_mpc.o @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:79bd540099942480deacefb07bcc3077244ff5716f1f03351a685faf166f5508 +size 2008 diff --git a/selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_auxiliary_functions.d b/selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_auxiliary_functions.d new file mode 100644 index 0000000000..0d376ee632 --- /dev/null +++ b/selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_auxiliary_functions.d @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:9c5571edf4541f5966032a6964cdb3f1c3c26ac20c7fd8149c0ab6dd03c08c1f +size 219 diff --git a/selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_auxiliary_functions.o b/selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_auxiliary_functions.o new file mode 100644 index 0000000000..c56a6350a3 --- /dev/null +++ b/selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_auxiliary_functions.o @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:171419af40cd4d2dd983caba560f2a0b62cc1de1e91c7c0b90aee82c7b65f828 +size 5192 diff --git a/selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_integrator.d b/selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_integrator.d new file mode 100644 index 0000000000..87537719b2 --- /dev/null +++ b/selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_integrator.d @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:28e31ac55feb4c787b02ce8c7eee799b1b9f95a9ec9b25e395fa4d4ce5807e6c +size 150 diff --git a/selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_integrator.o b/selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_integrator.o new file mode 100644 index 0000000000..8b0c674f41 --- /dev/null +++ b/selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_integrator.o @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:c93a23d4ce9f810746e08284a34476dc4ea9d51a53f0228b5edfd547e4aa05a9 +size 5496 diff --git a/selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_qpoases_interface.d b/selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_qpoases_interface.d new file mode 100644 index 0000000000..ae2e81c22c --- /dev/null +++ b/selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_qpoases_interface.d @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:6480142ecf8a67001cfe5904fdabdbf05715901ae9e4d040673d55dc31d401ac +size 1260 diff --git a/selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_qpoases_interface.o b/selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_qpoases_interface.o new file mode 100644 index 0000000000..c413b8788b --- /dev/null +++ b/selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_qpoases_interface.o @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:4e35ce1faf130e036cd0523e9422e046389665b98e62b48c1f7288e8dc39c75e +size 3008 diff --git a/selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_solver.d b/selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_solver.d new file mode 100644 index 0000000000..8ae0395257 --- /dev/null +++ b/selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_solver.d @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:2fa7bde92a2d7d20c459eac2c10dadc0811492cd4cd45495ae78fa05843a9b73 +size 142 diff --git a/selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_solver.o b/selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_solver.o new file mode 100644 index 0000000000..a0fc95932b --- /dev/null +++ b/selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_solver.o @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:42e5a47c99cdbe9c34beec0208d2b9158e146c3722fd94ae9f50692b931a583c +size 272376 diff --git a/selfdrive/controls/lib/lateral_mpc/libmpc.so b/selfdrive/controls/lib/lateral_mpc/libmpc.so new file mode 100755 index 0000000000..2a9e16fe36 --- /dev/null +++ b/selfdrive/controls/lib/lateral_mpc/libmpc.so @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:88415f9187ccd260f520eec417d16b801239813d5a28a6c55ecd978374c844d5 +size 531640 diff --git a/selfdrive/controls/lib/long_mpc.py b/selfdrive/controls/lib/long_mpc.py index cb1ab94dc6..4c71683096 100644 --- a/selfdrive/controls/lib/long_mpc.py +++ b/selfdrive/controls/lib/long_mpc.py @@ -1,5 +1,5 @@ import os -import numpy as np +import math import selfdrive.messaging as messaging from selfdrive.swaglog import cloudlog @@ -11,7 +11,7 @@ from selfdrive.controls.lib.drive_helpers import MPC_COST_LONG LOG_MPC = os.environ.get('LOG_MPC', False) -class LongitudinalMpc(object): +class LongitudinalMpc(): def __init__(self, mpc_id): self.mpc_id = mpc_id @@ -104,10 +104,9 @@ class LongitudinalMpc(object): self.v_mpc_future = self.mpc_solution[0].v_ego[10] # Reset if NaN or goes through lead car - dls = np.array(list(self.mpc_solution[0].x_l)) - np.array(list(self.mpc_solution[0].x_ego)) - crashing = min(dls) < -50.0 - nans = np.any(np.isnan(list(self.mpc_solution[0].v_ego))) - backwards = min(list(self.mpc_solution[0].v_ego)) < -0.01 + crashing = any(lead - ego < -50 for (lead, ego) in zip(self.mpc_solution[0].x_l, self.mpc_solution[0].x_ego)) + nans = any(math.isnan(x) for x in self.mpc_solution[0].v_ego) + backwards = min(self.mpc_solution[0].v_ego) < -0.01 if ((backwards or crashing) and self.prev_lead_status) or nans: if t > self.last_cloudlog_t + 5.0: diff --git a/selfdrive/controls/lib/longcontrol.py b/selfdrive/controls/lib/longcontrol.py index 84c17ef374..8910747cdb 100644 --- a/selfdrive/controls/lib/longcontrol.py +++ b/selfdrive/controls/lib/longcontrol.py @@ -55,7 +55,7 @@ def long_control_state_trans(active, long_control_state, v_ego, v_target, v_pid, return long_control_state -class LongControl(object): +class LongControl(): def __init__(self, CP, compute_gb): self.long_control_state = LongCtrlState.off # initialized to off self.pid = PIController((CP.longitudinalTuning.kpBP, CP.longitudinalTuning.kpV), diff --git a/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_auxiliary_functions.d b/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_auxiliary_functions.d new file mode 100644 index 0000000000..0d376ee632 --- /dev/null +++ b/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_auxiliary_functions.d @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:9c5571edf4541f5966032a6964cdb3f1c3c26ac20c7fd8149c0ab6dd03c08c1f +size 219 diff --git a/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_auxiliary_functions.o b/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_auxiliary_functions.o new file mode 100644 index 0000000000..139912bfa4 --- /dev/null +++ b/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_auxiliary_functions.o @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:5a2f0c68a20c7c3a2eaff0010583389887c971be334bfb334c8cdc58a8f33a62 +size 5136 diff --git a/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_integrator.d b/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_integrator.d new file mode 100644 index 0000000000..87537719b2 --- /dev/null +++ b/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_integrator.d @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:28e31ac55feb4c787b02ce8c7eee799b1b9f95a9ec9b25e395fa4d4ce5807e6c +size 150 diff --git a/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_integrator.o b/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_integrator.o new file mode 100644 index 0000000000..9e445fed65 --- /dev/null +++ b/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_integrator.o @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:bcdecad1a69b573be3c70bb5271d7cf928592c0a82e002e515dfebd899c336ba +size 3576 diff --git a/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_qpoases_interface.d b/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_qpoases_interface.d new file mode 100644 index 0000000000..ae2e81c22c --- /dev/null +++ b/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_qpoases_interface.d @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:6480142ecf8a67001cfe5904fdabdbf05715901ae9e4d040673d55dc31d401ac +size 1260 diff --git a/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_qpoases_interface.o b/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_qpoases_interface.o new file mode 100644 index 0000000000..4b3c2303d1 --- /dev/null +++ b/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_qpoases_interface.o @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:7bfecf197fc15a276103a268f4448b293472af7534adb168be6fcc1385aa2747 +size 3008 diff --git a/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_solver.d b/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_solver.d new file mode 100644 index 0000000000..8ae0395257 --- /dev/null +++ b/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_solver.d @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:2fa7bde92a2d7d20c459eac2c10dadc0811492cd4cd45495ae78fa05843a9b73 +size 142 diff --git a/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_solver.o b/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_solver.o new file mode 100644 index 0000000000..217a707edb --- /dev/null +++ b/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_solver.o @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:52a0fb9d6feeb8182644a27c04821a5e1502c8e5a97b743f507d779461a692af +size 180712 diff --git a/selfdrive/controls/lib/longitudinal_mpc/libmpc1.so b/selfdrive/controls/lib/longitudinal_mpc/libmpc1.so new file mode 100755 index 0000000000..5665ad1ee2 --- /dev/null +++ b/selfdrive/controls/lib/longitudinal_mpc/libmpc1.so @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:3a649d81814d4476522e809c2ebfeefad7ce0bc84af5a525b6f1d5f96bd6e97c +size 437384 diff --git a/selfdrive/controls/lib/longitudinal_mpc/longitudinal_mpc.d b/selfdrive/controls/lib/longitudinal_mpc/longitudinal_mpc.d new file mode 100644 index 0000000000..34a2a0d20c --- /dev/null +++ b/selfdrive/controls/lib/longitudinal_mpc/longitudinal_mpc.d @@ -0,0 +1,3 @@ +longitudinal_mpc.o: longitudinal_mpc.c lib_mpc_export/acado_common.h \ + lib_mpc_export/acado_qpoases_interface.hpp \ + lib_mpc_export/acado_auxiliary_functions.h diff --git a/selfdrive/controls/lib/longitudinal_mpc/longitudinal_mpc.o b/selfdrive/controls/lib/longitudinal_mpc/longitudinal_mpc.o new file mode 100644 index 0000000000..ee5edddf2b --- /dev/null +++ b/selfdrive/controls/lib/longitudinal_mpc/longitudinal_mpc.o @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:4b204da3d4e4a763fc289cf2c539700b97260f7798df5a6ba707f58d4d12da60 +size 3152 diff --git a/selfdrive/controls/lib/pathplanner.py b/selfdrive/controls/lib/pathplanner.py index b62d9f1107..5678cf05f9 100644 --- a/selfdrive/controls/lib/pathplanner.py +++ b/selfdrive/controls/lib/pathplanner.py @@ -1,8 +1,5 @@ import os import math -import numpy as np - -# from common.numpy_fast import clip from common.realtime import sec_since_boot from selfdrive.swaglog import cloudlog from selfdrive.controls.lib.lateral_mpc import libmpc_py @@ -19,7 +16,7 @@ def calc_states_after_delay(states, v_ego, steer_angle, curvature_factor, steer_ return states -class PathPlanner(object): +class PathPlanner(): def __init__(self, CP): self.LP = LanePlanner() @@ -88,7 +85,7 @@ class PathPlanner(object): self.angle_steers_des_mpc = float(math.degrees(delta_desired * VM.sR) + angle_offset) # Check for infeasable MPC solution - mpc_nans = np.any(np.isnan(list(self.mpc_solution[0].delta))) + mpc_nans = any(math.isnan(x) for x in self.mpc_solution[0].delta) t = sec_since_boot() if mpc_nans: self.libmpc.init(MPC_COST_LAT.PATH, MPC_COST_LAT.LANE, MPC_COST_LAT.HEADING, CP.steerRateCost) diff --git a/selfdrive/controls/lib/pid.py b/selfdrive/controls/lib/pid.py index b68dc3810e..ce2d34b359 100644 --- a/selfdrive/controls/lib/pid.py +++ b/selfdrive/controls/lib/pid.py @@ -10,7 +10,7 @@ def apply_deadzone(error, deadzone): error = 0. return error -class PIController(object): +class PIController(): def __init__(self, k_p, k_i, k_f=1., pos_limit=None, neg_limit=None, rate=100, sat_limit=0.8, convert=None): self._k_p = k_p # proportional gain self._k_i = k_i # integral gain diff --git a/selfdrive/controls/lib/planner.py b/selfdrive/controls/lib/planner.py index 26230843a8..db3fe33217 100755 --- a/selfdrive/controls/lib/planner.py +++ b/selfdrive/controls/lib/planner.py @@ -1,4 +1,4 @@ -#!/usr/bin/env python +#!/usr/bin/env python3 import math import numpy as np from common.params import Params @@ -27,13 +27,12 @@ _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.1, 1.1, .8, .5, .3] -_A_CRUISE_MAX_V_FOLLOWING = [1.6, 1.6, 1.2, .7, .3] -_A_CRUISE_MAX_BP = [0., 5., 10., 20., 40.] +_A_CRUISE_MAX_V = [1.6, 1.6, 0.65, .4] +_A_CRUISE_MAX_BP = [0., 6.4, 22.5, 40.] # Lookup table for turns -_A_TOTAL_MAX_V = [1.5, 1.9, 3.2] -_A_TOTAL_MAX_BP = [0., 20., 40.] +_A_TOTAL_MAX_V = [1.7, 3.2] +_A_TOTAL_MAX_BP = [20., 40.] # Model speed kalman stuff @@ -46,13 +45,9 @@ _MODEL_V_K = [[0.07068858], [0.04826294]] SPEED_PERCENTILE_IDX = 7 -def calc_cruise_accel_limits(v_ego, following): +def calc_cruise_accel_limits(v_ego): a_cruise_min = interp(v_ego, _A_CRUISE_MIN_BP, _A_CRUISE_MIN_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) + a_cruise_max = interp(v_ego, _A_CRUISE_MAX_BP, _A_CRUISE_MAX_V) return np.vstack([a_cruise_min, a_cruise_max]) @@ -69,7 +64,7 @@ def limit_accel_in_turns(v_ego, angle_steers, a_target, CP): return [a_target[0], min(a_target[1], a_x_allowed)] -class Planner(object): +class Planner(): def __init__(self, CP): self.CP = CP @@ -95,7 +90,7 @@ class Planner(object): def choose_solution(self, v_cruise_setpoint, enabled): if enabled: - solutions = {'cruise': self.v_cruise, 'model': self.v_model} + solutions = {'model': self.v_model, 'cruise': self.v_cruise} if self.mpc1.prev_lead_status: solutions['mpc1'] = self.mpc1.v_mpc if self.mpc2.prev_lead_status: @@ -134,7 +129,6 @@ class Planner(object): 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,6 +136,7 @@ class Planner(object): # Curvature of polynomial https://en.wikipedia.org/wiki/Curvature#Curvature_of_the_graph_of_a_function # y = a x^3 + b x^2 + c x + d, y' = 3 a x^2 + 2 b x + c, y'' = 6 a x + 2 b # k = y'' / (1 + y'^2)^1.5 + # TODO: compute max speed without using a list of points and without numpy y_p = 3 * path[0] * self.path_x**2 + 2 * path[1] * self.path_x + path[2] y_pp = 6 * path[0] * self.path_x + 2 * path[1] curv = y_pp / (1. + y_p**2)**1.5 @@ -149,14 +144,13 @@ class Planner(object): a_y_max = 2.975 - v_ego * 0.0375 # ~1.85 @ 75mph, ~2.6 @ 25mph v_curvature = np.sqrt(a_y_max / np.clip(np.abs(curv), 1e-4, None)) model_speed = np.min(v_curvature) - # print(model_speed * CV.MS_TO_MPH, model_speed) model_speed = max(20.0 * CV.MPH_TO_MS, model_speed) # Don't slow down below 20mph else: 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, following)] + accel_limits = [float(x) for x in calc_cruise_accel_limits(v_ego)] 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) diff --git a/selfdrive/controls/lib/radar_helpers.py b/selfdrive/controls/lib/radar_helpers.py index d246d5d8ca..557679e5dc 100644 --- a/selfdrive/controls/lib/radar_helpers.py +++ b/selfdrive/controls/lib/radar_helpers.py @@ -22,7 +22,7 @@ _VLEAD_C = [1.0, 0.0] _VLEAD_K = [[0.1988689], [0.28555364]] -class Track(object): +class Track(): def __init__(self): self.ekf = None self.cnt = 0 @@ -67,7 +67,7 @@ def mean(l): return sum(l) / len(l) -class Cluster(object): +class Cluster(): def __init__(self): self.tracks = set() diff --git a/selfdrive/controls/lib/vehicle_model.py b/selfdrive/controls/lib/vehicle_model.py index 1dc77bf375..1559a893e8 100755 --- a/selfdrive/controls/lib/vehicle_model.py +++ b/selfdrive/controls/lib/vehicle_model.py @@ -1,4 +1,4 @@ -#!/usr/bin/env python +#!/usr/bin/env python3 import numpy as np from numpy.linalg import solve @@ -90,7 +90,7 @@ def calc_slip_factor(VM): return VM.m * (VM.cF * VM.aF - VM.cR * VM.aR) / (VM.l**2 * VM.cF * VM.cR) -class VehicleModel(object): +class VehicleModel(): def __init__(self, CP): """ Args: @@ -140,7 +140,7 @@ class VehicleModel(object): u: Speed [m/s] Returns: - Curvature factor [rad/m] + Curvature factor [1/m] """ return self.curvature_factor(u) * sa / self.sR @@ -161,7 +161,7 @@ class VehicleModel(object): """Calculates the required steering wheel angle for a given curvature Args: - curv: Desired curvature [rad/s] + curv: Desired curvature [1/m] u: Speed [m/s] Returns: @@ -170,6 +170,19 @@ class VehicleModel(object): return curv * self.sR * 1.0 / self.curvature_factor(u) + def get_steer_from_yaw_rate(self, yaw_rate, u): + """Calculates the required steering wheel angle for a given yaw_rate + + Args: + yaw_rate: Desired yaw rate [rad/s] + u: Speed [m/s] + + Returns: + Steering wheel angle [rad] + """ + curv = yaw_rate / u + return self.get_steer_from_curvature(curv, u) + def yaw_rate(self, sa, u): """Calculate yaw rate @@ -188,6 +201,6 @@ if __name__ == '__main__': from selfdrive.car.honda.interface import CarInterface from selfdrive.car.honda.values import CAR - CP = CarInterface.get_params(CAR.CIVIC, {}) + CP = CarInterface.get_params(CAR.CIVIC) VM = VehicleModel(CP) print(VM.yaw_rate(math.radians(20), 10.)) diff --git a/selfdrive/controls/plannerd.py b/selfdrive/controls/plannerd.py index 7a5742b3e5..59fe51aaca 100755 --- a/selfdrive/controls/plannerd.py +++ b/selfdrive/controls/plannerd.py @@ -1,4 +1,4 @@ -#!/usr/bin/env python +#!/usr/bin/env python3 import gc from cereal import car diff --git a/selfdrive/controls/radard.py b/selfdrive/controls/radard.py index d516316332..2a36d9a0ab 100755 --- a/selfdrive/controls/radard.py +++ b/selfdrive/controls/radard.py @@ -1,18 +1,20 @@ -#!/usr/bin/env python -import numpy as np -import numpy.matlib +#!/usr/bin/env python3 import importlib +import math from collections import defaultdict, deque +import zmq + import selfdrive.messaging as messaging -from selfdrive.services import service_list -from selfdrive.controls.lib.radar_helpers import Track, Cluster -from selfdrive.config import RADAR_TO_CENTER -from selfdrive.controls.lib.cluster.fastcluster_py import cluster_points_centroid -from selfdrive.swaglog import cloudlog from cereal import car from common.params import Params -from common.realtime import set_realtime_priority, Ratekeeper, DT_MDL +from common.realtime import DT_RDR, Ratekeeper, set_realtime_priority +from selfdrive.config import RADAR_TO_CAMERA +from selfdrive.controls.lib.cluster.fastcluster_py import \ + cluster_points_centroid +from selfdrive.controls.lib.radar_helpers import Cluster, Track +from selfdrive.services import service_list +from selfdrive.swaglog import cloudlog DEBUG = False @@ -22,33 +24,34 @@ XV, SPEEDV = 0, 1 VISION_POINT = -1 # Time-alignment -rate = 1. / DT_MDL # model and radar are both at 20Hz v_len = 20 # how many speed data points to remember for t alignment with rdr data def laplacian_cdf(x, mu, b): - b = np.max([b, 1e-4]) - return np.exp(-abs(x-mu)/b) + b = max(b, 1e-4) + return math.exp(-abs(x-mu)/b) def match_vision_to_cluster(v_ego, lead, clusters): # match vision point to best statistical cluster match - probs = [] - offset_vision_dist = lead.dist - RADAR_TO_CENTER - for c in clusters: + offset_vision_dist = lead.dist - RADAR_TO_CAMERA + + def prob(c): prob_d = laplacian_cdf(c.dRel, offset_vision_dist, lead.std) prob_y = laplacian_cdf(c.yRel, lead.relY, lead.relYStd) prob_v = laplacian_cdf(c.vRel, lead.relVel, lead.relVelStd) + # This is isn't exactly right, but good heuristic - combined_prob = prob_d * prob_y * prob_v - probs.append(combined_prob) - idx = np.argmax(probs) + return prob_d * prob_y * prob_v + + cluster = max(clusters, key=prob) + # if no 'sane' match is found return -1 # stationary radar points can be false positives - dist_sane = abs(clusters[idx].dRel - offset_vision_dist) < max([(offset_vision_dist)*.25, 5.0]) - vel_sane = (abs(clusters[idx].vRel - lead.relVel) < 10) or (v_ego + clusters[idx].vRel > 2) + dist_sane = abs(cluster.dRel - offset_vision_dist) < max([(offset_vision_dist)*.25, 5.0]) + vel_sane = (abs(cluster.vRel - lead.relVel) < 10) or (v_ego + cluster.vRel > 2) if dist_sane and vel_sane: - return idx + return cluster else: return None @@ -56,28 +59,30 @@ def match_vision_to_cluster(v_ego, lead, clusters): def get_lead(v_ego, ready, clusters, lead_msg, low_speed_override=True): # Determine leads, this is where the essential logic happens if len(clusters) > 0 and ready and lead_msg.prob > .5: - lead_idx = match_vision_to_cluster(v_ego, lead_msg, clusters) + cluster = match_vision_to_cluster(v_ego, lead_msg, clusters) else: - lead_idx = None + cluster = None lead_dict = {'status': False} - if lead_idx is not None: - lead_dict = clusters[lead_idx].get_RadarState(lead_msg.prob) - elif (lead_idx is None) and ready and (lead_msg.prob > .5): + if cluster is not None: + lead_dict = cluster.get_RadarState(lead_msg.prob) + elif (cluster is None) and ready and (lead_msg.prob > .5): lead_dict = Cluster().get_RadarState_from_vision(lead_msg, v_ego) if low_speed_override: low_speed_clusters = [c for c in clusters if c.potential_low_speed_lead(v_ego)] if len(low_speed_clusters) > 0: - lead_idx = np.argmin([c.dRel for c in low_speed_clusters]) - if (not lead_dict['status']) or (low_speed_clusters[lead_idx].dRel < lead_dict['dRel']): - lead_dict = low_speed_clusters[lead_idx].get_RadarState() + closest_cluster = min(low_speed_clusters, key=lambda c: c.dRel) + + # Only choose new cluster if it is actually closer than the previous one + if (not lead_dict['status']) or (closest_cluster.dRel < lead_dict['dRel']): + lead_dict = closest_cluster.get_RadarState() return lead_dict -class RadarD(object): - def __init__(self, mocked): +class RadarD(): + def __init__(self, mocked, delay=0): self.current_time = 0 self.mocked = mocked @@ -90,19 +95,18 @@ class RadarD(object): # v_ego self.v_ego = 0. - self.v_ego_hist_t = deque([0], maxlen=v_len) - self.v_ego_hist_v = deque([0], maxlen=v_len) + self.v_ego_hist = deque([0], maxlen=delay+1) + self.v_ego_t_aligned = 0. self.ready = False - def update(self, frame, delay, sm, rr, has_radar): + def update(self, frame, sm, rr, has_radar): self.current_time = 1e-9*max([sm.logMonoTime[key] for key in sm.logMonoTime.keys()]) if sm.updated['controlsState']: self.active = sm['controlsState'].active self.v_ego = sm['controlsState'].vEgo - self.v_ego_hist_v.append(self.v_ego) - self.v_ego_hist_t.append(float(frame)/rate) + self.v_ego_hist.append(self.v_ego) if sm.updated['model']: self.ready = True @@ -111,7 +115,7 @@ class RadarD(object): ar_pts[pt.trackId] = [pt.dRel, pt.yRel, pt.vRel, pt.measured] # *** remove missing points from meta data *** - for ids in self.tracks.keys(): + for ids in list(self.tracks.keys()): if ids not in ar_pts: self.tracks.pop(ids, None) @@ -120,16 +124,15 @@ class RadarD(object): rpt = ar_pts[ids] # align v_ego by a fixed time to align it with the radar measurement - cur_time = float(frame)/rate - self.v_ego_t_aligned = np.interp(cur_time - delay, self.v_ego_hist_t, self.v_ego_hist_v) + self.v_ego_t_aligned = self.v_ego_hist[0] # create the track if it doesn't exist or it's a new track if ids not in self.tracks: self.tracks[ids] = Track() self.tracks[ids].update(rpt[0], rpt[1], rpt[2], self.v_ego_t_aligned, rpt[3]) - idens = list(self.tracks.keys()) - track_pts = np.array([self.tracks[iden].get_key_for_cluster() for iden in idens]) + idens = list(sorted(self.tracks.keys())) + track_pts = list([self.tracks[iden].get_key_for_cluster() for iden in idens]) # If we have multiple points, cluster them @@ -137,7 +140,7 @@ class RadarD(object): cluster_idxs = cluster_points_centroid(track_pts, 2.5) clusters = [None] * (max(cluster_idxs) + 1) - for idx in xrange(len(track_pts)): + for idx in range(len(track_pts)): cluster_i = cluster_idxs[idx] if clusters[cluster_i] is None: clusters[cluster_i] = Cluster() @@ -151,7 +154,7 @@ class RadarD(object): clusters = [] # if a new point, reset accel to the rest of the cluster - for idx in xrange(len(track_pts)): + for idx in range(len(track_pts)): if self.tracks[idens[idx]].cnt <= 1: aLeadK = clusters[cluster_idxs[idx]].aLeadK aLeadTau = clusters[cluster_idxs[idx]].aLeadTau @@ -186,8 +189,11 @@ def radard_thread(sm=None, pm=None, can_sock=None): cloudlog.info("radard is importing %s", CP.carName) RadarInterface = importlib.import_module('selfdrive.car.%s.radar_interface' % CP.carName).RadarInterface + can_poller = zmq.Poller() + if can_sock is None: can_sock = messaging.sub_sock(service_list['can'].port) + can_poller.register(can_sock) if sm is None: sm = messaging.SubMaster(['model', 'controlsState', 'liveParameters']) @@ -198,13 +204,13 @@ def radard_thread(sm=None, pm=None, can_sock=None): RI = RadarInterface(CP) - rk = Ratekeeper(rate, print_delay_threshold=None) - RD = RadarD(mocked) + rk = Ratekeeper(1.0 / DT_RDR, print_delay_threshold=None) + RD = RadarD(mocked, RI.delay) has_radar = not CP.radarOffCan while 1: - can_strings = messaging.drain_sock_raw(can_sock, wait_for_one=True) + can_strings = messaging.drain_sock_raw_poller(can_poller, can_sock, wait_for_one=True) rr = RI.update(can_strings) if rr is None: @@ -212,7 +218,7 @@ def radard_thread(sm=None, pm=None, can_sock=None): sm.update(0) - dat = RD.update(rk.frame, RI.delay, sm, rr, has_radar) + dat = RD.update(rk.frame, sm, rr, has_radar) dat.radarState.cumLagMs = -rk.remaining*1000. pm.send('radarState', dat) @@ -222,7 +228,7 @@ def radard_thread(sm=None, pm=None, can_sock=None): dat = messaging.new_message() dat.init('liveTracks', len(tracks)) - for cnt, ids in enumerate(tracks.keys()): + for cnt, ids in enumerate(sorted(tracks.keys())): dat.liveTracks[cnt] = { "trackId": ids, "dRel": float(tracks[ids].dRel), diff --git a/selfdrive/controls/tests/test_clustering.py b/selfdrive/controls/tests/test_clustering.py index 14c0e23fe3..e899ff7d57 100644 --- a/selfdrive/controls/tests/test_clustering.py +++ b/selfdrive/controls/tests/test_clustering.py @@ -88,7 +88,7 @@ class TestClustering(unittest.TestCase): pts_ptr = ffi.cast("double *", pts.ctypes.data) n, m = pts.shape - out = np.zeros((n * (n - 1) / 2, ), dtype=np.float64) + out = np.zeros((n * (n - 1) // 2, ), dtype=np.float64) out_ptr = ffi.cast("double *", out.ctypes.data) hclust.hclust_pdist(n, m, pts_ptr, out_ptr) diff --git a/selfdrive/controls/tests/test_following_distance.py b/selfdrive/controls/tests/test_following_distance.py index bcc67c42a3..331cb14eac 100644 --- a/selfdrive/controls/tests/test_following_distance.py +++ b/selfdrive/controls/tests/test_following_distance.py @@ -15,7 +15,7 @@ def RW(v_ego, v_l): return (v_ego * TR - (v_l - v_ego) * TR + v_ego * v_ego / (2 * G) - v_l * v_l / (2 * G)) -class FakePubMaster(object): +class FakePubMaster(): def send(self, s, data): assert data @@ -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, False)] + accel_limits = [float(x) for x in calc_cruise_accel_limits(v_ego)] 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], diff --git a/selfdrive/controls/tests/test_lateral_mpc.py b/selfdrive/controls/tests/test_lateral_mpc.py index 4bd6571348..8dfff81ad4 100644 --- a/selfdrive/controls/tests/test_lateral_mpc.py +++ b/selfdrive/controls/tests/test_lateral_mpc.py @@ -27,15 +27,15 @@ def run_mpc(v_ref=30., x_init=0., y_init=0., psi_init=0., delta_init=0., d_poly = calc_d_poly(p_l, p_r, p_p, l_prob, r_prob, lane_width) - CP = CarInterface.get_params("HONDA CIVIC 2016 TOURING", {}) + CP = CarInterface.get_params("HONDA CIVIC 2016 TOURING") VM = VehicleModel(CP) v_ref = v_ref curvature_factor = VM.curvature_factor(v_ref) - l_poly = libmpc_py.ffi.new("double[4]", map(float, p_l)) - r_poly = libmpc_py.ffi.new("double[4]", map(float, p_r)) - d_poly = libmpc_py.ffi.new("double[4]", map(float, d_poly)) + l_poly = libmpc_py.ffi.new("double[4]", list(map(float, p_l))) + r_poly = libmpc_py.ffi.new("double[4]", list(map(float, p_r))) + d_poly = libmpc_py.ffi.new("double[4]", list(map(float, d_poly))) cur_state = libmpc_py.ffi.new("state_t *") cur_state[0].x = x_init diff --git a/selfdrive/controls/tests/test_monitoring.py b/selfdrive/controls/tests/test_monitoring.py index d543dfc9ac..d5c14d663d 100644 --- a/selfdrive/controls/tests/test_monitoring.py +++ b/selfdrive/controls/tests/test_monitoring.py @@ -196,5 +196,5 @@ class TestMonitoring(unittest.TestCase): self.assertEqual(events_output[int((_redlight_time+0.5)/DT_DMON)][0].name, 'promptDriverDistracted') if __name__ == "__main__": - print 'MAX_TERMINAL_ALERTS', MAX_TERMINAL_ALERTS + print('MAX_TERMINAL_ALERTS', MAX_TERMINAL_ALERTS) unittest.main() diff --git a/selfdrive/debug/can_printer.py b/selfdrive/debug/can_printer.py index daa0500b2f..6b5d188be3 100755 --- a/selfdrive/debug/can_printer.py +++ b/selfdrive/debug/can_printer.py @@ -1,9 +1,11 @@ -#!/usr/bin/env python +#!/usr/bin/env python3 +import binascii import os import sys from collections import defaultdict -from common.realtime import sec_since_boot + import selfdrive.messaging as messaging +from common.realtime import sec_since_boot from selfdrive.services import service_list @@ -24,9 +26,9 @@ def can_printer(bus=0, max_msg=None, addr="127.0.0.1"): if sec_since_boot() - lp > 0.1: dd = chr(27) + "[2J" dd += "%5.2f\n" % (sec_since_boot() - start) - for k,v in sorted(zip(msgs.keys(), map(lambda x: x[-1].encode("hex"), msgs.values()))): + for k,v in sorted(zip(msgs.keys(), map(lambda x: binascii.hexlify(x[-1]), msgs.values()))): if max_msg is None or k < max_msg: - dd += "%s(%6d) %s\n" % ("%04X(%4d)" % (k,k),len(msgs[k]), v) + dd += "%s(%6d) %s\n" % ("%04X(%4d)" % (k,k),len(msgs[k]), v.decode('ascii')) print(dd) lp = sec_since_boot() @@ -39,4 +41,3 @@ if __name__ == "__main__": can_printer(int(sys.argv[1])) else: can_printer() - diff --git a/selfdrive/debug/compare_fingerprints.py b/selfdrive/debug/compare_fingerprints.py new file mode 100755 index 0000000000..a0c80a740e --- /dev/null +++ b/selfdrive/debug/compare_fingerprints.py @@ -0,0 +1,18 @@ +#!/usr/bin/env python3 + +# put 2 fingeprints and print the diffs +f1 = { +168: 8, 257: 5, 258: 8, 264: 8, 268: 8, 270: 8, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 291: 8, 292: 8, 294: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 344: 8, 368: 8, 376: 3, 384: 8, 388: 4, 448: 6, 456: 4, 464: 8, 469: 8, 480: 8, 500: 8, 501: 8, 512: 8, 514: 8, 520: 8, 528: 8, 532: 8, 544: 8, 557: 8, 559: 8, 560: 8, 564: 8, 571: 3, 579: 8, 584: 8, 608: 8, 624: 8, 625: 8, 632: 8, 639: 8, 653: 8, 654: 8, 655: 8, 658: 6, 660: 8, 669: 3, 671: 8, 672: 8, 678: 8, 680: 8, 701: 8, 703: 8, 704: 8, 705: 8, 706: 8, 709: 8, 710: 8, 719: 8, 720: 6, 729: 5, 736: 8, 737: 8, 746: 5, 752: 2, 754: 8, 760: 8, 764: 8, 766: 8, 770: 8, 773: 8, 779: 8, 782: 8, 784: 8, 792: 8, 799: 8, 800: 8, 804: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 832: 8, 838: 2, 848: 8, 853: 8, 856: 4, 860: 6, 863: 8, 878: 8, 882: 8, 897: 8, 906: 8, 908: 8, 924: 8, 926: 3, 929: 8, 937: 8, 938: 8, 939: 8, 940: 8, 941: 8, 942: 8, 943: 8, 947: 8, 948: 8, 958: 8, 959: 8, 962: 8, 969: 4, 973: 8, 974: 5, 979: 8, 980: 8, 981: 8, 982: 8, 983: 8, 984: 8, 992: 8, 993: 7, 995: 8, 996: 8, 1000: 8, 1001: 8, 1002: 8, 1003: 8, 1008: 8, 1009: 8, 1010: 8, 1011: 8, 1012: 8, 1013: 8, 1014: 8, 1015: 8, 1024: 8, 1025: 8, 1026: 8, 1031: 8, 1033: 8, 1050: 8, 1059: 8, 1082: 8, 1083: 8, 1098: 8, 1100: 8, 1537: 8, 1538: 8, 1562: 8 +} + +f2 = { +168: 8, 257: 5, 258: 8, 264: 8, 268: 8, 270: 8, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 291: 8, 292: 8, 294: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 344: 8, 368: 8, 376: 3, 384: 8, 388: 4, 448: 6, 456: 4, 464: 8, 469: 8, 480: 8, 500: 8, 501: 8, 512: 8, 514: 8, 515: 7, 516: 7, 517: 7, 518: 7, 520: 8, 528: 8, 532: 8, 542: 8, 544: 8, 557: 8, 559: 8, 560: 8, 564: 8, 571: 3, 579: 8, 584: 8, 608: 8, 624: 8, 625: 8, 632: 8, 639: 8, 653: 8, 654: 8, 655: 8, 658: 6, 660: 8, 669: 3, 671: 8, 672: 8, 678: 8, 680: 8, 701: 8, 703: 8, 704: 8, 705: 8, 706: 8, 709: 8, 710: 8, 719: 8, 720: 6, 729: 5, 736: 8, 737: 8, 746: 5, 752: 2, 754: 8, 760: 8, 764: 8, 766: 8, 770: 8, 773: 8, 779: 8, 782: 8, 784: 8, 792: 8, 799: 8, 800: 8, 804: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 832: 8, 838: 2, 848: 8, 853: 8, 856: 4, 860: 6, 863: 8, 878: 8, 882: 8, 897: 8, 906: 8, 908: 8, 924: 8, 926: 3, 929: 8, 937: 8, 938: 8, 939: 8, 940: 8, 941: 8, 942: 8, 943: 8, 947: 8, 948: 8, 958: 8, 959: 8, 962: 8, 969: 4, 973: 8, 974: 5, 979: 8, 980: 8, 981: 8, 982: 8, 983: 8, 984: 8, 992: 8, 993: 7, 995: 8, 996: 8, 1000: 8, 1001: 8, 1002: 8, 1003: 8, 1008: 8, 1009: 8, 1010: 8, 1011: 8, 1012: 8, 1013: 8, 1014: 8, 1015: 8, 1024: 8, 1025: 8, 1026: 8, 1031: 8, 1033: 8, 1050: 8, 1059: 8, 1082: 8, 1083: 8, 1098: 8, 1100: 8 +} + +for k in f1: + if k not in f2 or f1[k] != f2[k]: + print(k, "not in f2") + +for k in f2: + if k not in f1 or f2[k] != f1[k]: + print(k, "not in f1") diff --git a/selfdrive/debug/cpu_usage_stat.py b/selfdrive/debug/cpu_usage_stat.py index 7adf7ef78e..e1a6ac35d4 100755 --- a/selfdrive/debug/cpu_usage_stat.py +++ b/selfdrive/debug/cpu_usage_stat.py @@ -1,4 +1,4 @@ -#!/usr/bin/env python2 +#!/usr/bin/env python3 import psutil import time import os @@ -94,7 +94,7 @@ if __name__ == "__main__": for k, stat in stats.items(): if len(stat['cpu_samples']) <= 0: continue - for name, samples in stat['cpu_samples'].iteritems(): + for name, samples in stat['cpu_samples'].items(): samples = np.array(samples) avg = samples.mean() c = samples.size diff --git a/selfdrive/debug/dump.py b/selfdrive/debug/dump.py index 7fd8a7cf4f..84846a17a6 100755 --- a/selfdrive/debug/dump.py +++ b/selfdrive/debug/dump.py @@ -1,4 +1,4 @@ -#!/usr/bin/env python2 +#!/usr/bin/env python3 import sys import argparse import zmq diff --git a/selfdrive/debug/get_fingerprint.py b/selfdrive/debug/get_fingerprint.py index e528d9826c..286b71a132 100755 --- a/selfdrive/debug/get_fingerprint.py +++ b/selfdrive/debug/get_fingerprint.py @@ -1,4 +1,4 @@ -#!/usr/bin/env python +#!/usr/bin/env python3 # simple script to get a vehicle fingerprint. diff --git a/selfdrive/debug/live_cpu_and_temp.py b/selfdrive/debug/live_cpu_and_temp.py new file mode 100755 index 0000000000..0cc62ef7c5 --- /dev/null +++ b/selfdrive/debug/live_cpu_and_temp.py @@ -0,0 +1,49 @@ +#!/usr/bin/env python3 +import numpy as np + +from selfdrive.messaging import SubMaster + +def cputime_total(ct): + return ct.user + ct.nice + ct.system + ct.idle + ct.iowait + ct.irq + ct.softirq + + +def cputime_busy(ct): + return ct.user + ct.nice + ct.system + ct.irq + ct.softirq + + + +sm = SubMaster(['thermal', 'procLog']) + +last_temp = 0.0 +total_times = [0., 0., 0., 0.] +busy_times = [0., 0., 0.0, 0.] + + +while True: + sm.update() + + if sm.updated['thermal']: + t = sm['thermal'] + last_temp = np.mean([t.cpu0, t.cpu1, t.cpu2, t.cpu3]) / 10. + + if sm.updated['procLog']: + m = sm['procLog'] + + cores = [0., 0., 0., 0.] + total_times_new = [0., 0., 0., 0.] + busy_times_new = [0., 0., 0.0, 0.] + + for c in m.cpuTimes: + n = c.cpuNum + total_times_new[n] = cputime_total(c) + busy_times_new[n] = cputime_busy(c) + + for n in range(4): + t_busy = busy_times_new[n] - busy_times[n] + t_total = total_times_new[n] - total_times[n] + cores[n] = t_busy / t_total + + total_times = total_times_new[:] + busy_times = busy_times_new[:] + + print("CPU %.2f%% - Temp %.2f" % (100. * np.mean(cores), last_temp )) diff --git a/selfdrive/debug/mpc/live_lateral_mpc.py b/selfdrive/debug/mpc/live_lateral_mpc.py new file mode 100755 index 0000000000..f4ee7a1843 --- /dev/null +++ b/selfdrive/debug/mpc/live_lateral_mpc.py @@ -0,0 +1,111 @@ +#!/usr/bin/env python3 +import matplotlib +matplotlib.use('TkAgg') + +import sys +from selfdrive.services import service_list +import selfdrive.messaging as messaging +import numpy as np +import matplotlib.pyplot as plt + +# debug liateral MPC by plotting its trajectory. To receive liveLongitudinalMpc packets, +# set on LOG_MPC env variable and run plannerd on a replay + + +def mpc_vwr_thread(addr="127.0.0.1"): + + plt.ion() + fig = plt.figure(figsize=(15, 20)) + ax = fig.add_subplot(131) + aa = fig.add_subplot(132, sharey=ax) + ap = fig.add_subplot(133, sharey=ax) + + ax.set_xlim([-10, 10]) + ax.set_ylim([0., 100.]) + aa.set_xlim([-20., 20]) + ap.set_xlim([-5, 5]) + + ax.set_xlabel('x [m]') + ax.set_ylabel('y [m]') + aa.set_xlabel('steer_angle [deg]') + ap.set_xlabel('asset angle [deg]') + ax.grid(True) + aa.grid(True) + ap.grid(True) + + path_x = np.arange(0, 100) + mpc_path_x = np.arange(0, 49) + + p_path_y = np.zeros(100) + + l_path_y = np.zeros(100) + r_path_y = np.zeros(100) + mpc_path_y = np.zeros(49) + mpc_steer_angle = np.zeros(49) + mpc_psi = np.zeros(49) + + line1, = ax.plot(mpc_path_y, mpc_path_x) + # line1b, = ax.plot(mpc_path_y, mpc_path_x, 'o') + + lineP, = ax.plot(p_path_y, path_x) + lineL, = ax.plot(l_path_y, path_x) + lineR, = ax.plot(r_path_y, path_x) + line3, = aa.plot(mpc_steer_angle, mpc_path_x) + line4, = ap.plot(mpc_psi, mpc_path_x) + ax.invert_xaxis() + aa.invert_xaxis() + plt.show() + + + # *** log *** + livempc = messaging.sub_sock(service_list['liveMpc'].port, addr=addr) + model = messaging.sub_sock(service_list['model'].port, addr=addr) + path_plan_sock = messaging.sub_sock(service_list['pathPlan'].port, addr=addr) + + while 1: + lMpc = messaging.recv_sock(livempc, wait=True) + md = messaging.recv_sock(model) + pp = messaging.recv_sock(path_plan_sock) + + if md is not None: + p_poly = np.array(md.model.path.poly) + l_poly = np.array(md.model.leftLane.poly) + r_poly = np.array(md.model.rightLane.poly) + + p_path_y = np.polyval(p_poly, path_x) + l_path_y = np.polyval(r_poly, path_x) + r_path_y = np.polyval(l_poly, path_x) + + if pp is not None: + p_path_y = np.polyval(pp.pathPlan.dPoly, path_x) + lineP.set_xdata(p_path_y) + lineP.set_ydata(path_x) + + if lMpc is not None: + mpc_path_x = list(lMpc.liveMpc.x)[1:] + mpc_path_y = list(lMpc.liveMpc.y)[1:] + mpc_steer_angle = list(lMpc.liveMpc.delta)[1:] + mpc_psi = list(lMpc.liveMpc.psi)[1:] + + line1.set_xdata(mpc_path_y) + line1.set_ydata(mpc_path_x) + lineL.set_xdata(l_path_y) + lineL.set_ydata(path_x) + lineR.set_xdata(r_path_y) + lineR.set_ydata(path_x) + line3.set_xdata(np.asarray(mpc_steer_angle)*180./np.pi * 14) + line3.set_ydata(mpc_path_x) + line4.set_xdata(np.asarray(mpc_psi)*180./np.pi) + line4.set_ydata(mpc_path_x) + + aa.relim() + aa.autoscale_view(True, scaley=True, scalex=True) + + fig.canvas.draw() + fig.canvas.flush_events() + +if __name__ == "__main__": + if len(sys.argv) > 1: + mpc_vwr_thread(sys.argv[1]) + else: + mpc_vwr_thread() diff --git a/selfdrive/debug/mpc/live_longitudinal_mpc.py b/selfdrive/debug/mpc/live_longitudinal_mpc.py new file mode 100755 index 0000000000..e72a37585f --- /dev/null +++ b/selfdrive/debug/mpc/live_longitudinal_mpc.py @@ -0,0 +1,110 @@ +#!/usr/bin/env python3 + +import matplotlib +matplotlib.use('TkAgg') + +import sys +from selfdrive.services import service_list +import selfdrive.messaging as messaging +import numpy as np +import matplotlib.pyplot as plt + +N = 21 + +# debug longitudinal MPC by plotting its trajectory. To receive liveLongitudinalMpc packets, +# set on LOG_MPC env variable and run plannerd on a replay + +def plot_longitudinal_mpc(addr="127.0.0.1"): + # *** log *** + livempc = messaging.sub_sock(service_list['liveLongitudinalMpc'].port, addr=addr, conflate=True) + radarstate = messaging.sub_sock(service_list['radarState'].port, addr=addr, conflate=True) + + plt.ion() + fig = plt.figure() + + t = np.hstack([np.arange(0.0, 0.8, 0.2), np.arange(0.8, 10.6, 0.6)]) + + p_x_ego = fig.add_subplot(3, 2, 1) + p_v_ego = fig.add_subplot(3, 2, 3) + p_a_ego = fig.add_subplot(3, 2, 5) + # p_x_l = fig.add_subplot(3, 2, 2) + # p_a_l = fig.add_subplot(3, 2, 6) + p_d_l = fig.add_subplot(3, 2, 2) + p_d_l_v = fig.add_subplot(3, 2, 4) + p_d_l_vv = fig.add_subplot(3, 2, 6) + + p_v_ego.set_ylim([0, 30]) + p_a_ego.set_ylim([-4, 4]) + p_d_l.set_ylim([-1, 10]) + + p_x_ego.set_title('x') + p_v_ego.set_title('v') + p_a_ego.set_title('a') + p_d_l.set_title('rel dist') + + l_x_ego, = p_x_ego.plot(t, np.zeros(N)) + l_v_ego, = p_v_ego.plot(t, np.zeros(N)) + l_a_ego, = p_a_ego.plot(t, np.zeros(N)) + l_x_l, = p_x_ego.plot(t, np.zeros(N)) + l_v_l, = p_v_ego.plot(t, np.zeros(N)) + l_a_l, = p_a_ego.plot(t, np.zeros(N)) + l_d_l, = p_d_l.plot(t, np.zeros(N)) + l_d_l_v, = p_d_l_v.plot(np.zeros(N)) + l_d_l_vv, = p_d_l_vv.plot(np.zeros(N)) + p_x_ego.legend(['ego', 'l']) + p_v_ego.legend(['ego', 'l']) + p_a_ego.legend(['ego', 'l']) + p_d_l_v.set_xlabel('d_rel') + p_d_l_v.set_ylabel('v_rel') + p_d_l_v.set_ylim([-20, 20]) + p_d_l_v.set_xlim([0, 100]) + p_d_l_vv.set_xlabel('d_rel') + p_d_l_vv.set_ylabel('v_rel') + p_d_l_vv.set_ylim([-5, 5]) + p_d_l_vv.set_xlim([10, 40]) + + while True: + lMpc = messaging.recv_sock(livempc, wait=True) + rs = messaging.recv_sock(radarstate, wait=True) + + if lMpc is not None: + + if lMpc.liveLongitudinalMpc.mpcId != 1: + continue + + x_ego = list(lMpc.liveLongitudinalMpc.xEgo) + v_ego = list(lMpc.liveLongitudinalMpc.vEgo) + a_ego = list(lMpc.liveLongitudinalMpc.aEgo) + x_l = list(lMpc.liveLongitudinalMpc.xLead) + v_l = list(lMpc.liveLongitudinalMpc.vLead) + # a_l = list(lMpc.liveLongitudinalMpc.aLead) + a_l = rs.radarState.leadOne.aLeadK * np.exp(-lMpc.liveLongitudinalMpc.aLeadTau * t**2 / 2) + #print(min(a_ego), lMpc.liveLongitudinalMpc.qpIterations) + + l_x_ego.set_ydata(x_ego) + l_v_ego.set_ydata(v_ego) + l_a_ego.set_ydata(a_ego) + + l_x_l.set_ydata(x_l) + l_v_l.set_ydata(v_l) + l_a_l.set_ydata(a_l) + + l_d_l.set_ydata(np.array(x_l) - np.array(x_ego)) + l_d_l_v.set_ydata(np.array(v_l) - np.array(v_ego)) + l_d_l_v.set_xdata(np.array(x_l) - np.array(x_ego)) + l_d_l_vv.set_ydata(np.array(v_l) - np.array(v_ego)) + l_d_l_vv.set_xdata(np.array(x_l) - np.array(x_ego)) + + p_x_ego.relim() + p_x_ego.autoscale_view(True, scaley=True, scalex=True) + fig.canvas.draw() + fig.canvas.flush_events() + + + + +if __name__ == "__main__": + if len(sys.argv) > 1: + plot_longitudinal_mpc(sys.argv[1]) + else: + plot_longitudinal_mpc() diff --git a/selfdrive/debug/mpc/test_mpc_wobble.py b/selfdrive/debug/mpc/test_mpc_wobble.py new file mode 100755 index 0000000000..5f9f1b3355 --- /dev/null +++ b/selfdrive/debug/mpc/test_mpc_wobble.py @@ -0,0 +1,129 @@ +#! /usr/bin/env python +import matplotlib.pyplot as plt +from selfdrive.controls.lib.lateral_mpc import libmpc_py +from selfdrive.controls.lib.drive_helpers import MPC_COST_LAT +import math + +libmpc = libmpc_py.libmpc +libmpc.init(MPC_COST_LAT.PATH, MPC_COST_LAT.LANE, MPC_COST_LAT.HEADING, 1.) + +cur_state = libmpc_py.ffi.new("state_t *") +cur_state[0].x = 0.0 +cur_state[0].y = 0.0 +cur_state[0].psi = 0.0 +cur_state[0].delta = 0.0 + +mpc_solution = libmpc_py.ffi.new("log_t *") +xx = [] +yy = [] +deltas = [] +psis = [] +times = [] + +curvature_factor = 0.3 +v_ref = 1.0 * 20.12 # 45 mph + +LANE_WIDTH = 3.7 +p = [0.0, 0.0, 0.0, 0.0] +p_l = p[:] +p_l[3] += LANE_WIDTH / 2.0 + +p_r = p[:] +p_r[3] -= LANE_WIDTH / 2.0 + + +l_poly = libmpc_py.ffi.new("double[4]", p_l) +r_poly = libmpc_py.ffi.new("double[4]", p_r) +p_poly = libmpc_py.ffi.new("double[4]", p) + +l_prob = 1.0 +r_prob = 1.0 +p_prob = 1.0 + +for i in range(1): + cur_state[0].delta = math.radians(510. / 13.) + libmpc.run_mpc(cur_state, mpc_solution, l_poly, r_poly, p_poly, l_prob, r_prob, + curvature_factor, v_ref, LANE_WIDTH) + +timesi = [] +ct = 0 +for i in range(21): + timesi.append(ct) + if i <= 4: + ct += 0.05 + else: + ct += 0.15 + + +xi = list(mpc_solution[0].x) +yi = list(mpc_solution[0].y) +psii = list(mpc_solution[0].psi) +deltai = list(mpc_solution[0].delta) +print("COST: ", mpc_solution[0].cost) + + +plt.figure(0) +plt.subplot(3, 1, 1) +plt.plot(timesi, psii) +plt.ylabel('psi') +plt.grid(True) +plt.subplot(3, 1, 2) +plt.plot(timesi, deltai) +plt.ylabel('delta') +plt.grid(True) +plt.subplot(3, 1, 3) +plt.plot(timesi, yi) +plt.ylabel('y') +plt.grid(True) +plt.show() + + +#### UNCOMMENT TO CHECK ITERATIVE SOLUTION +#### +####for i in range(100): +#### libmpc.run_mpc(cur_state, mpc_solution, l_poly, r_poly, p_poly, l_prob, r_prob, +#### curvature_factor, v_ref, LANE_WIDTH) +#### print "x", list(mpc_solution[0].x) +#### print "y", list(mpc_solution[0].y) +#### print "delta", list(mpc_solution[0].delta) +#### print "psi", list(mpc_solution[0].psi) +#### # cur_state[0].x = mpc_solution[0].x[1] +#### # cur_state[0].y = mpc_solution[0].y[1] +#### # cur_state[0].psi = mpc_solution[0].psi[1] +#### cur_state[0].delta = radians(200 / 13.)#mpc_solution[0].delta[1] +#### +#### xx.append(cur_state[0].x) +#### yy.append(cur_state[0].y) +#### psis.append(cur_state[0].psi) +#### deltas.append(cur_state[0].delta) +#### times.append(i * 0.05) +#### +#### +####def f(x): +#### return p_poly[0] * x**3 + p_poly[1] * x**2 + p_poly[2] * x + p_poly[3] +#### +#### +##### planned = map(f, xx) +##### plt.figure(1) +##### plt.plot(yy, xx, 'r-') +##### plt.plot(planned, xx, 'b--', linewidth=0.5) +##### plt.axes().set_aspect('equal', 'datalim') +##### plt.gca().invert_xaxis() +#### +##### planned = map(f, map(float, list(mpc_solution[0].x)[1:])) +##### plt.figure(1) +##### plt.plot(map(float, list(mpc_solution[0].y)[1:]), map(float, list(mpc_solution[0].x)[1:]), 'r-') +##### plt.plot(planned, map(float, list(mpc_solution[0].x)[1:]), 'b--', linewidth=0.5) +##### plt.axes().set_aspect('equal', 'datalim') +##### plt.gca().invert_xaxis() +#### +####plt.figure(2) +####plt.subplot(2, 1, 1) +####plt.plot(times, psis) +####plt.ylabel('psi') +####plt.subplot(2, 1, 2) +####plt.plot(times, deltas) +####plt.ylabel('delta') +#### +#### +####plt.show() diff --git a/selfdrive/debug/mpc/tune_lateral.py b/selfdrive/debug/mpc/tune_lateral.py new file mode 100755 index 0000000000..4a8ec2ce0b --- /dev/null +++ b/selfdrive/debug/mpc/tune_lateral.py @@ -0,0 +1,186 @@ +#! /usr/bin/env python +import numpy as np +from collections import OrderedDict +import matplotlib.pyplot as plt +from selfdrive.car.honda.interface import CarInterface +from selfdrive.controls.lib.lateral_mpc import libmpc_py +from selfdrive.controls.lib.vehicle_model import VehicleModel + +# plot lateral MPC trajectory by defining boundary conditions: +# lane lines, p_poly and vehicle states. Use this script to tune MPC costs + +libmpc = libmpc_py.libmpc + +mpc_solution = libmpc_py.ffi.new("log_t *") + +points_l = np.array([1.1049711, 1.1053879, 1.1073375, 1.1096942, 1.1124474, 1.1154714, 1.1192677, 1.1245866, 1.1321017, 1.1396152, 1.146443, 1.1555313, 1.1662073, 1.1774249, 1.1888939, 1.2009926, 1.2149779, 1.2300836, 1.2450289, 1.2617753, 1.2785473, 1.2974714, 1.3151019, 1.3331807, 1.3545501, 1.3763691, 1.3983455, 1.4215056, 1.4446729, 1.4691089, 1.4927692, 1.5175346, 1.5429921, 1.568854, 1.5968665, 1.6268958, 1.657122, 1.6853137, 1.7152609, 1.7477539, 1.7793678, 1.8098511, 1.8428392, 1.8746407, 1.9089606, 1.9426043, 1.9775689, 2.0136933, 2.0520134, 2.0891454]) + +points_r = np.array([-2.4442139, -2.4449506, -2.4448867, -2.44377, -2.4422617, -2.4393811, -2.4374201, -2.4334245, -2.4286852, -2.4238286, -2.4177458, -2.4094386, -2.3994849, -2.3904033, -2.380136, -2.3699453, -2.3594661, -2.3474073, -2.3342307, -2.3194637, -2.3046403, -2.2881098, -2.2706163, -2.2530098, -2.235604, -2.2160542, -2.1967411, -2.1758952, -2.1544619, -2.1325269, -2.1091819, -2.0850561, -2.0621953, -2.0364127, -2.0119917, -1.9851667, -1.9590458, -1.9306552, -1.9024918, -1.8745357, -1.8432863, -1.8131843, -1.7822732, -1.7507075, -1.7180918, -1.6845931, -1.650871, -1.6157099, -1.5787286, -1.5418037]) + + +points_c = (points_l + points_r) / 2.0 + +def compute_path_pinv(): + deg = 3 + x = np.arange(50.0) + X = np.vstack(tuple(x**n for n in range(deg, -1, -1))).T + pinv = np.linalg.pinv(X) + return pinv + + +def model_polyfit(points): + path_pinv = compute_path_pinv() + return np.dot(path_pinv, map(float, points)) + + +xx = [] +yy = [] +deltas = [] +psis = [] +times = [] + +CP = CarInterface.get_params("HONDA CIVIC 2016 TOURING") +VM = VehicleModel(CP) + +v_ref = 32.00 # 45 mph +curvature_factor = VM.curvature_factor(v_ref) +print(curvature_factor) + +LANE_WIDTH = 3.9 +p_l = map(float, model_polyfit(points_l)) +p_r = map(float, model_polyfit(points_r)) +p_p = map(float, model_polyfit(points_c)) + +l_poly = libmpc_py.ffi.new("double[4]", p_l) +r_poly = libmpc_py.ffi.new("double[4]", p_r) +p_poly = libmpc_py.ffi.new("double[4]", p_p) +l_prob = 1.0 +r_prob = 1.0 +p_prob = 1.0 # This is always 1 + + +mpc_x_points = np.linspace(0., 2.5*v_ref, num=50) +points_poly_l = np.polyval(p_l, mpc_x_points) +points_poly_r = np.polyval(p_r, mpc_x_points) +points_poly_p = np.polyval(p_p, mpc_x_points) +print(points_poly_l) + +lanes_x = np.linspace(0, 49) + +cur_state = libmpc_py.ffi.new("state_t *") +cur_state[0].x = 0.0 +cur_state[0].y = 0.5 +cur_state[0].psi = 0.0 +cur_state[0].delta = 0.0 + +xs = [] +ys = [] +deltas = [] +titles = [ + 'Steer rate cost', + 'Heading cost', + 'Lane cost', + 'Path cost', +] + +# Steer rate cost +sol_x = OrderedDict() +sol_y = OrderedDict() +delta = OrderedDict() +for cost in np.logspace(-1, 1.0, 5): + libmpc.init(1.0, 3.0, 1.0, cost) + for _ in range(10): + libmpc.run_mpc(cur_state, mpc_solution, l_poly, r_poly, p_poly, l_prob, r_prob, + curvature_factor, v_ref, LANE_WIDTH) + sol_x[cost] = map(float, list(mpc_solution[0].x)) + sol_y[cost] = map(float, list(mpc_solution[0].y)) + delta[cost] = map(float, list(mpc_solution[0].delta)) +xs.append(sol_x) +ys.append(sol_y) +deltas.append(delta) + +# Heading cost +sol_x = OrderedDict() +sol_y = OrderedDict() +delta = OrderedDict() +for cost in np.logspace(-1, 1.0, 5): + libmpc.init(1.0, 3.0, cost, 1.0) + for _ in range(10): + libmpc.run_mpc(cur_state, mpc_solution, l_poly, r_poly, p_poly, l_prob, r_prob, + curvature_factor, v_ref, LANE_WIDTH) + sol_x[cost] = map(float, list(mpc_solution[0].x)) + sol_y[cost] = map(float, list(mpc_solution[0].y)) + delta[cost] = map(float, list(mpc_solution[0].delta)) +xs.append(sol_x) +ys.append(sol_y) +deltas.append(delta) + +# Lane cost +sol_x = OrderedDict() +sol_y = OrderedDict() +delta = OrderedDict() +for cost in np.logspace(-1, 2.0, 5): + libmpc.init(1.0, cost, 1.0, 1.0) + for _ in range(10): + libmpc.run_mpc(cur_state, mpc_solution, l_poly, r_poly, p_poly, l_prob, r_prob, + curvature_factor, v_ref, LANE_WIDTH) + sol_x[cost] = map(float, list(mpc_solution[0].x)) + sol_y[cost] = map(float, list(mpc_solution[0].y)) + delta[cost] = map(float, list(mpc_solution[0].delta)) +xs.append(sol_x) +ys.append(sol_y) +deltas.append(delta) + + +# Path cost +sol_x = OrderedDict() +sol_y = OrderedDict() +delta = OrderedDict() +for cost in np.logspace(-1, 1.0, 5): + libmpc.init(cost, 3.0, 1.0, 1.0) + for _ in range(10): + libmpc.run_mpc(cur_state, mpc_solution, l_poly, r_poly, p_poly, l_prob, r_prob, + curvature_factor, v_ref, LANE_WIDTH) + sol_x[cost] = map(float, list(mpc_solution[0].x)) + sol_y[cost] = map(float, list(mpc_solution[0].y)) + delta[cost] = map(float, list(mpc_solution[0].delta)) +xs.append(sol_x) +ys.append(sol_y) +deltas.append(delta) + + + +plt.figure() + +for i in range(len(xs)): + ax = plt.subplot(2, 2, i + 1) + sol_x = xs[i] + sol_y = ys[i] + for cost in sol_x.keys(): + plt.plot(sol_x[cost], sol_y[cost]) + + plt.plot(lanes_x, points_r, '.b') + plt.plot(lanes_x, points_l, '.b') + plt.plot(lanes_x, (points_l + points_r) / 2.0, '--g') + plt.plot(mpc_x_points, points_poly_l, 'b') + plt.plot(mpc_x_points, points_poly_r, 'b') + plt.plot(mpc_x_points, (points_poly_l + points_poly_r) / 2.0, 'g') + plt.legend(map(lambda x: str(round(x, 2)), sol_x.keys()) + ['right', 'left', 'center'], loc=3) + plt.title(titles[i]) + plt.grid(True) + # ax.set_aspect('equal', 'datalim') + + +plt.figure() +for i in range(len(xs)): + plt.subplot(2, 2, i + 1) + sol_x = xs[i] + delta = deltas[i] + + for cost in sol_x.keys(): + plt.plot(delta[cost]) + plt.title(titles[i]) + plt.legend(map(lambda x: str(round(x, 2)), sol_x.keys()), loc=3) + plt.grid(True) + +plt.show() diff --git a/selfdrive/debug/mpc/tune_longitudinal.py b/selfdrive/debug/mpc/tune_longitudinal.py new file mode 100755 index 0000000000..0af444631d --- /dev/null +++ b/selfdrive/debug/mpc/tune_longitudinal.py @@ -0,0 +1,168 @@ +#! /usr/bin/env python +import numpy as np +import matplotlib.pyplot as plt +from selfdrive.controls.lib.longitudinal_mpc import libmpc_py +from selfdrive.controls.lib.drive_helpers import MPC_COST_LONG +import math + +# plot liongitudinal MPC trajectory by defining boundary conditions: +# ego and lead vehicles state. Use this script to tune MPC costs + +def RW(v_ego, v_l): + TR = 1.8 + G = 9.81 + return (v_ego * TR - (v_l - v_ego) * TR + v_ego*v_ego/(2*G) - v_l*v_l / (2*G)) + + +def NORM_RW_ERROR(v_ego, v_l, p): + return (RW(v_ego, v_l) + 4.0 - p) + return (RW(v_ego, v_l) + 4.0 - p) / (np.sqrt(v_ego + 0.5) + 0.1) + + +v_ego = 20.0 +a_ego = 0 + +x_lead = 10.0 +v_lead = 20.0 +a_lead = -3.0 +a_lead_tau = 0. + +# v_ego = 7.02661012716 +# a_ego = -1.26143024772 + +# x_lead = 29.625 + 20 +# v_lead = 0.725235462189 + 1 +# a_lead = -1.00025629997 + +# a_lead_tau = 2.90729817665 + +min_a_lead_tau = (a_lead**2 * math.pi) / (2 * (v_lead + 0.01)**2) +min_a_lead_tau = 0.0 + +print(a_lead_tau, min_a_lead_tau) +a_lead_tau = max(a_lead_tau, min_a_lead_tau) + +ffi, libmpc = libmpc_py.get_libmpc(1) +libmpc.init(MPC_COST_LONG.TTC, MPC_COST_LONG.DISTANCE, MPC_COST_LONG.ACCELERATION, MPC_COST_LONG.JERK) +libmpc.init_with_simulation(v_ego, x_lead, v_lead, a_lead, a_lead_tau) + +cur_state = ffi.new("state_t *") +cur_state[0].x_ego = 0.0 +cur_state[0].v_ego = v_ego +cur_state[0].a_ego = a_ego +cur_state[0].x_l = x_lead +cur_state[0].v_l = v_lead + +mpc_solution = ffi.new("log_t *") + +for _ in range(10): + print(libmpc.run_mpc(cur_state, mpc_solution, a_lead_tau, a_lead)) + + +for i in range(21): + print("t: %.2f\t x_e: %.2f\t v_e: %.2f\t a_e: %.2f\t" % (mpc_solution[0].t[i], mpc_solution[0].x_ego[i], mpc_solution[0].v_ego[i], mpc_solution[0].a_ego[i])) + print("x_l: %.2f\t v_l: %.2f\t \t" % (mpc_solution[0].x_l[i], mpc_solution[0].v_l[i])) + +t = np.hstack([np.arange(0., 1.0, 0.2), np.arange(1.0, 10.1, 0.6)]) + +print(map(float, mpc_solution[0].x_ego)[-1]) +print(map(float, mpc_solution[0].x_l)[-1] - map(float, mpc_solution[0].x_ego)[-1]) + +plt.figure(figsize=(8, 8)) + +plt.subplot(4, 1, 1) +x_l = np.array(map(float, mpc_solution[0].x_l)) +plt.plot(t, map(float, mpc_solution[0].x_ego)) +plt.plot(t, x_l) +plt.legend(['ego', 'lead']) +plt.title('x') +plt.grid() + +plt.subplot(4, 1, 2) +v_ego = np.array(map(float, mpc_solution[0].v_ego)) +v_l = np.array(map(float, mpc_solution[0].v_l)) +plt.plot(t, v_ego) +plt.plot(t, v_l) +plt.legend(['ego', 'lead']) +plt.ylim([-1, max(max(v_ego), max(v_l))]) +plt.title('v') +plt.grid() + +plt.subplot(4, 1, 3) +plt.plot(t, map(float, mpc_solution[0].a_ego)) +plt.plot(t, map(float, mpc_solution[0].a_l)) +plt.legend(['ego', 'lead']) +plt.title('a') +plt.grid() + + +plt.subplot(4, 1, 4) +d_l = np.array(map(float, mpc_solution[0].x_l)) - np.array(map(float, mpc_solution[0].x_ego)) +desired = 4.0 + RW(v_ego, v_l) + +plt.plot(t, d_l) +plt.plot(t, desired, '--') +plt.ylim(-1, max(max(desired), max(d_l))) +plt.legend(['relative distance', 'desired distance']) +plt.grid() + +plt.show() + +# c1 = np.exp(0.3 * NORM_RW_ERROR(v_ego, v_l, d_l)) +# c2 = np.exp(4.5 - d_l) +# print(c1) +# print(c2) + +# plt.figure() +# plt.plot(t, c1, label="NORM_RW_ERROR") +# plt.plot(t, c2, label="penalty function") +# plt.legend() + +# ## OLD MPC +# a_lead_tau = 1.5 +# a_lead_tau = max(a_lead_tau, -a_lead / (v_lead + 0.01)) + +# ffi, libmpc = libmpc_py.get_libmpc(1) +# libmpc.init(MPC_COST_LONG.TTC, MPC_COST_LONG.DISTANCE, MPC_COST_LONG.ACCELERATION, MPC_COST_LONG.JERK) +# libmpc.init_with_simulation(v_ego, x_lead, v_lead, a_lead, a_lead_tau) + +# cur_state = ffi.new("state_t *") +# cur_state[0].x_ego = 0.0 +# cur_state[0].v_ego = v_ego +# cur_state[0].a_ego = a_ego +# cur_state[0].x_lead = x_lead +# cur_state[0].v_lead = v_lead +# cur_state[0].a_lead = a_lead + +# mpc_solution = ffi.new("log_t *") + +# for _ in range(10): +# print libmpc.run_mpc(cur_state, mpc_solution, a_lead_tau) + +# t = np.hstack([np.arange(0., 1.0, 0.2), np.arange(1.0, 10.1, 0.6)]) + +# print(map(float, mpc_solution[0].x_ego)[-1]) +# print(map(float, mpc_solution[0].x_lead)[-1] - map(float, mpc_solution[0].x_ego)[-1]) +# plt.subplot(4, 2, 2) +# plt.plot(t, map(float, mpc_solution[0].x_ego)) +# plt.plot(t, map(float, mpc_solution[0].x_lead)) +# plt.legend(['ego', 'lead']) +# plt.title('x') + +# plt.subplot(4, 2, 4) +# plt.plot(t, map(float, mpc_solution[0].v_ego)) +# plt.plot(t, map(float, mpc_solution[0].v_lead)) +# plt.legend(['ego', 'lead']) +# plt.title('v') + +# plt.subplot(4, 2, 6) +# plt.plot(t, map(float, mpc_solution[0].a_ego)) +# plt.plot(t, map(float, mpc_solution[0].a_lead)) +# plt.legend(['ego', 'lead']) +# plt.title('a') + + +# plt.subplot(4, 2, 8) +# plt.plot(t, np.array(map(float, mpc_solution[0].x_lead)) - np.array(map(float, mpc_solution[0].x_ego))) + +# plt.show() diff --git a/selfdrive/debug/show_matching_cars.py b/selfdrive/debug/show_matching_cars.py new file mode 100755 index 0000000000..424e89887d --- /dev/null +++ b/selfdrive/debug/show_matching_cars.py @@ -0,0 +1,27 @@ +#!/usr/bin/env python3 +from selfdrive.car.fingerprints import eliminate_incompatible_cars, all_known_cars +import selfdrive.messaging as messaging + + +# Prius and Leuxs es 300H +fingerprint = {898: 8, 905: 8, 810: 2, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 921: 8, 800: 8, 944: 8, 1570: 8, 1059: 1, 36: 8, 37: 8, 550: 8, 295: 8, 296: 8, 170: 8, 1071: 8, 560: 7, 945: 8, 562: 6, 180: 8, 1077: 8, 950: 8, 951: 8, 953: 8, 1595: 8, 1084: 8, 829: 2, 1086: 8, 1568: 8, 452: 8, 581: 5, 1057: 8, 713: 8, 971: 7, 975: 5, 1571: 8, 466: 8, 467: 8, 1572: 8, 1114: 8, 933: 8, 863: 8, 608: 8, 993: 8, 610: 8, 955: 8, 166: 8, 1056: 8, 956: 8, 1132: 8, 1085: 8, 552: 4, 1779: 8, 1017: 8, 1020: 8, 426: 6, 1279: 8} + +# rav4 2019 and corolla tss2 +fingerprint = {896: 8, 898: 8, 976: 1, 1541: 8, 905: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1552: 8, 1553: 8, 1556: 8, 921: 8, 1056: 8, 544: 4, 1570: 8, 1059: 1, 36: 8, 37: 8, 550: 8, 552: 4, 170: 8, 812: 8, 944: 8, 945: 8, 562: 6, 180: 8, 1077: 8, 951: 8, 824: 8, 1076: 8, 186: 4, 955: 8, 956: 8, 705: 8, 452: 8, 1592: 8, 464: 8, 1571: 8, 466: 8, 467: 8, 761: 8, 728: 8, 1572: 8, 1114: 8, 933: 8, 800: 8, 608: 8, 865: 8, 610: 8, 1595: 8, 1745: 8, 764: 8, 1002: 8, 1649: 8, 1779: 8, 1568: 8, 1017: 8, 1279: 8, 1020: 8, 810: 2, 426: 6} + +# rav4 2019 and corolla tss2 +fingerprint = {896: 8, 898: 8, 900: 6, 976: 1, 1541: 8, 902: 6, 905: 8, 810: 2, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1552: 8, 1553: 8, 1556: 8, 1571: 8, 921: 8, 1056: 8, 544: 4, 1570: 8, 1059: 1, 36: 8, 37: 8, 550: 8, 935: 8, 552: 4, 170: 8, 812: 8, 944: 8, 945: 8, 562: 6, 180: 8, 1077: 8, 951: 8, 1592: 8, 1076: 8, 186: 4, 955: 8, 956: 8, 1001: 8, 705: 8, 452: 8, 1788: 8, 464: 8, 824: 8, 466: 8, 467: 8, 761: 8, 728: 8, 1572: 8, 1114: 8, 933: 8, 800: 8, 608: 8, 865: 8, 610: 8, 1595: 8, 934: 8, 998: 5, 1745: 8, 1000: 8, 764: 8, 1002: 8, 999: 7, 1789: 8, 1649: 8, 1779: 8, 1568: 8, 1017: 8, 1786: 8, 1787: 8, 1020: 8, 426: 6, 1279: 8} + +candidate_cars = all_known_cars() + + +for addr, l in fingerprint.items(): + dat = messaging.new_message() + dat.init('can', 1) + + msg = dat.can[0] + msg.address = addr + msg.dat = " " * l + + candidate_cars = eliminate_incompatible_cars(msg, candidate_cars) + print(candidate_cars) diff --git a/selfdrive/debug/tuner.py b/selfdrive/debug/tuner.py index 066b0841ab..b8a023ba29 100755 --- a/selfdrive/debug/tuner.py +++ b/selfdrive/debug/tuner.py @@ -1,4 +1,4 @@ -#!/usr/bin/env python +#!/usr/bin/env python3 """ This tool can be used to quickly changes the values in a JSON file used for tuning Keys like in vim: diff --git a/selfdrive/locationd/calibrationd.py b/selfdrive/locationd/calibrationd.py index db4b0c2266..50cc9d0157 100755 --- a/selfdrive/locationd/calibrationd.py +++ b/selfdrive/locationd/calibrationd.py @@ -1,4 +1,5 @@ -#!/usr/bin/env python +#!/usr/bin/env python3 + import os import copy import json @@ -6,7 +7,7 @@ import numpy as np import selfdrive.messaging as messaging from selfdrive.locationd.calibration_helpers import Calibration from selfdrive.swaglog import cloudlog -from common.params import Params +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 @@ -30,7 +31,7 @@ def is_calibration_valid(vp): vp[1] > VP_VALIDITY_CORNERS[0,1] and vp[1] < VP_VALIDITY_CORNERS[1,1] -class Calibrator(object): +class Calibrator(): def __init__(self, param_put=False): self.param_put = param_put self.vp = copy.copy(VP_INIT) @@ -38,8 +39,9 @@ class Calibrator(object): self.cal_status = Calibration.UNCALIBRATED self.write_counter = 0 self.just_calibrated = False - self.params = Params() - calibration_params = self.params.get("CalibrationParams") + + # Read calibration + calibration_params = Params().get("CalibrationParams") if calibration_params: try: calibration_params = json.loads(calibration_params) @@ -49,7 +51,6 @@ class Calibrator(object): 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: @@ -75,7 +76,7 @@ class Calibrator(object): if self.param_put and (self.write_counter % WRITE_CYCLES == 0 or self.just_calibrated): cal_params = {"vanishing_point": list(self.vp), "valid_points": len(self.vps)} - self.params.put("CalibrationParams", json.dumps(cal_params)) + put_nonblocking("CalibrationParams", json.dumps(cal_params).encode('utf8')) return new_vp else: return None @@ -109,7 +110,7 @@ def calibrationd_thread(sm=None, pm=None): new_vp = calibrator.handle_cam_odom(sm['cameraOdometry']) if DEBUG and new_vp is not None: - print 'got new vp', new_vp + print('got new vp', new_vp) calibrator.send_data(pm) diff --git a/selfdrive/locationd/paramsd.cc b/selfdrive/locationd/paramsd.cc index b9b4a9c1ad..a38729dcd1 100644 --- a/selfdrive/locationd/paramsd.cc +++ b/selfdrive/locationd/paramsd.cc @@ -1,17 +1,20 @@ +#include #include + #include #include #include -#include "locationd_yawrate.h" +#include "json11.hpp" #include "cereal/gen/cpp/log.capnp.h" #include "common/swaglog.h" #include "common/messaging.h" #include "common/params.h" #include "common/timing.h" + +#include "locationd_yawrate.h" #include "params_learner.h" -#include "json11.hpp" const int num_polls = 3; @@ -162,7 +165,6 @@ int main(int argc, char *argv[]) { zmq_send(live_parameters_sock_raw, bytes.begin(), bytes.size(), ZMQ_DONTWAIT); // Save parameters every minute - // TODO: Save in seperate thread if (save_counter % 6000 == 0) { json11::Json json = json11::Json::object { {"carVin", vin}, @@ -173,7 +175,10 @@ int main(int argc, char *argv[]) { }; std::string out = json.dump(); - write_db_value(NULL, "LiveParameters", out.c_str(), out.length()); + std::async(std::launch::async, + [out]{ + write_db_value(NULL, "LiveParameters", out.c_str(), out.length()); + }); } } } diff --git a/selfdrive/locationd/test/ci_test.py b/selfdrive/locationd/test/ci_test.py index 7e151093b4..847dcf635f 100755 --- a/selfdrive/locationd/test/ci_test.py +++ b/selfdrive/locationd/test/ci_test.py @@ -1,4 +1,4 @@ -#!/usr/bin/env python2 +#!/usr/bin/env python3 import subprocess import os import sys diff --git a/selfdrive/locationd/test/test_params_learner.py b/selfdrive/locationd/test/test_params_learner.py index b29f591e7e..c1b75c5af9 100755 --- a/selfdrive/locationd/test/test_params_learner.py +++ b/selfdrive/locationd/test/test_params_learner.py @@ -1,4 +1,4 @@ -#!/usr/bin/env python +#!/usr/bin/env python3 import numpy as np import unittest @@ -12,7 +12,7 @@ from selfdrive.locationd.liblocationd_py import liblocationd # pylint: disable= class TestParamsLearner(unittest.TestCase): def setUp(self): - self.CP = CarInterface.get_params(CAR.CIVIC, {}) + self.CP = CarInterface.get_params(CAR.CIVIC) bts = self.CP.to_bytes() self.params_learner = liblocationd.params_learner_init(len(bts), bts, 0.0, 1.0, self.CP.steerRatio, 1.0) diff --git a/selfdrive/locationd/test/ublox.py b/selfdrive/locationd/test/ublox.py index 175316fd81..82d7759301 100644 --- a/selfdrive/locationd/test/ublox.py +++ b/selfdrive/locationd/test/ublox.py @@ -1,4 +1,4 @@ -#!/usr/bin/env python +#!/usr/bin/env python3 ''' UBlox binary protocol handling @@ -10,6 +10,7 @@ ublox version 7, it has been adapted to work for ublox version 8, not all functions may work. ''' + import struct import time, os @@ -534,7 +535,7 @@ class UBloxMessage: '''UBlox message class - holds a UBX binary message''' def __init__(self): - self._buf = "" + self._buf = b"" self._fields = {} self._recs = [] self._unpacked = False @@ -613,11 +614,11 @@ class UBloxMessage: def msg_class(self): '''return the message class''' - return ord(self._buf[2]) + return self._buf[2] def msg_id(self): '''return the message id within the class''' - return ord(self._buf[3]) + return self._buf[3] def msg_type(self): '''return the message type tuple (class, id)''' @@ -630,9 +631,9 @@ class UBloxMessage: def valid_so_far(self): '''check if the message is valid so far''' - if len(self._buf) > 0 and ord(self._buf[0]) != PREAMBLE1: + if len(self._buf) > 0 and self._buf[0] != PREAMBLE1: return False - if len(self._buf) > 1 and ord(self._buf[1]) != PREAMBLE2: + if len(self._buf) > 1 and self._buf[1] != PREAMBLE2: self.debug(1, "bad pre2") return False if self.needed_bytes() == 0 and not self.valid(): @@ -661,7 +662,7 @@ class UBloxMessage: ck_a = 0 ck_b = 0 for i in data: - ck_a = (ck_a + ord(i)) & 0xFF + ck_a = (ck_a + i) & 0xFF ck_b = (ck_b + ck_a) & 0xFF return (ck_a, ck_b) @@ -713,7 +714,7 @@ class UBlox: self.dev = PandaSerial(self.panda, 1, self.baudrate) self.baudrate = 460800 - print "upping baud:",self.baudrate + print("upping baud:",self.baudrate) self.send_nmea("$PUBX,41,1,0007,0003,%u,0" % self.baudrate) time.sleep(0.1) @@ -722,7 +723,7 @@ class UBlox: from selfdrive.services import service_list import selfdrive.messaging as messaging - class BoarddSerial(object): + class BoarddSerial(): def __init__(self): self.ubloxRaw = messaging.sub_sock(service_list['ubloxRaw'].port) self.buf = "" diff --git a/selfdrive/locationd/test/ubloxd.py b/selfdrive/locationd/test/ubloxd.py index 388a69bf42..409bfc331c 100755 --- a/selfdrive/locationd/test/ubloxd.py +++ b/selfdrive/locationd/test/ubloxd.py @@ -1,4 +1,5 @@ -#!/usr/bin/env python +#!/usr/bin/env python3 + import os import serial from selfdrive.locationd.test import ublox @@ -210,7 +211,7 @@ def gen_raw(msg): cnos = {} for meas in measurements_parsed: cnos[meas['svId']] = meas['cno'] - print 'Carrier to noise ratio for each sat: \n', cnos, '\n' + print('Carrier to noise ratio for each sat: \n', cnos, '\n') receiverStatus_bools = int_to_bool_list(msg_meta_data['recStat']) receiverStatus = {'leapSecValid': receiverStatus_bools[0], 'clkReset': receiverStatus_bools[2]} @@ -254,7 +255,7 @@ def handle_msg(dev, msg, nav_frame_buffer): ubloxGnss.send(nav.to_bytes()) else: - print "UNKNNOWN MESSAGE:", msg.name() + print("UNKNNOWN MESSAGE:", msg.name()) except ublox.UBloxError as e: print(e) @@ -265,7 +266,7 @@ def main(gctx=None): global gpsLocationExternal, ubloxGnss nav_frame_buffer = {} nav_frame_buffer[0] = {} - for i in xrange(1,33): + for i in range(1,33): nav_frame_buffer[0][i] = {} diff --git a/selfdrive/locationd/test/ubloxd_easy.py b/selfdrive/locationd/test/ubloxd_easy.py index 8bb16d403a..b936badd57 100755 --- a/selfdrive/locationd/test/ubloxd_easy.py +++ b/selfdrive/locationd/test/ubloxd_easy.py @@ -1,4 +1,5 @@ -#!/usr/bin/env python +#!/usr/bin/env python3 + import os from selfdrive.locationd.test import ublox from common import realtime @@ -46,7 +47,7 @@ def main(gctx=None): raw.logMonoTime = int(realtime.sec_since_boot() * 1e9) ubloxGnss.send(raw.to_bytes()) else: - print "INVALID MESSAGE" + print("INVALID MESSAGE") if __name__ == "__main__": diff --git a/selfdrive/locationd/test/ubloxd_py_test.py b/selfdrive/locationd/test/ubloxd_py_test.py index 8104081183..bca9ddc421 100755 --- a/selfdrive/locationd/test/ubloxd_py_test.py +++ b/selfdrive/locationd/test/ubloxd_py_test.py @@ -17,7 +17,7 @@ def mkdirs_exists_ok(path): def parser_test(fn, prefix): nav_frame_buffer = {} nav_frame_buffer[0] = {} - for i in xrange(1, 33): + for i in range(1, 33): nav_frame_buffer[0][i] = {} if not os.path.exists(prefix): diff --git a/selfdrive/locationd/test/ubloxd_regression_test.py b/selfdrive/locationd/test/ubloxd_regression_test.py index 94d6b4fe20..29210bf545 100644 --- a/selfdrive/locationd/test/ubloxd_regression_test.py +++ b/selfdrive/locationd/test/ubloxd_regression_test.py @@ -1,4 +1,4 @@ -#!/usr/bin/env python +#!/usr/bin/env python3 import os import sys import argparse diff --git a/selfdrive/locationd/ubloxd_main.cc b/selfdrive/locationd/ubloxd_main.cc index 3ea080ad9d..df3b3aa8bc 100644 --- a/selfdrive/locationd/ubloxd_main.cc +++ b/selfdrive/locationd/ubloxd_main.cc @@ -25,7 +25,7 @@ #include "ublox_msg.h" -volatile int do_exit = 0; // Flag for process exit on signal +volatile sig_atomic_t do_exit = 0; // Flag for process exit on signal void set_do_exit(int sig) { do_exit = 1; diff --git a/selfdrive/loggerd/deleter.py b/selfdrive/loggerd/deleter.py index b1e1ffcd04..5669e2342d 100644 --- a/selfdrive/loggerd/deleter.py +++ b/selfdrive/loggerd/deleter.py @@ -1,10 +1,10 @@ -#!/usr/bin/env python +#!/usr/bin/env python3 import os import shutil import threading from selfdrive.swaglog import cloudlog from selfdrive.loggerd.config import ROOT, get_available_percent -from selfdrive.loggerd.uploader import listdir_by_creation_date +from selfdrive.loggerd.uploader import listdir_by_creation def deleter_thread(exit_event): @@ -13,7 +13,7 @@ def deleter_thread(exit_event): if available_percent < 10.0: # remove the earliest directory we can - dirs = listdir_by_creation_date(ROOT) + dirs = listdir_by_creation(ROOT) for delete_dir in dirs: delete_path = os.path.join(ROOT, delete_dir) diff --git a/selfdrive/loggerd/loggerd.cc b/selfdrive/loggerd/loggerd.cc index db59780f0b..ec4c1140e1 100644 --- a/selfdrive/loggerd/loggerd.cc +++ b/selfdrive/loggerd/loggerd.cc @@ -63,7 +63,7 @@ double randrange(double a, double b) { } -volatile int do_exit = 0; +volatile sig_atomic_t do_exit = 0; static void set_do_exit(int sig) { do_exit = 1; } @@ -126,7 +126,7 @@ void encoder_thread(bool is_streaming, bool raw_clips, bool front) { if (!encoder_inited) { LOGD("encoder init %dx%d", buf_info.width, buf_info.height); - encoder_init(&encoder, front ? "dcamera" : "fcamera", buf_info.width, buf_info.height, CAMERA_FPS, front ? 1000000 : 5000000); + encoder_init(&encoder, front ? "dcamera" : "fcamera", buf_info.width, buf_info.height, CAMERA_FPS, front ? 2500000 : 5000000); encoder_inited = true; if (is_streaming) { encoder.stream_sock_raw = zmq_socket(s.ctx, ZMQ_PUB); diff --git a/selfdrive/loggerd/tests/fill_eon.py b/selfdrive/loggerd/tests/fill_eon.py index 8ebc5ceb2a..f273ea89ec 100755 --- a/selfdrive/loggerd/tests/fill_eon.py +++ b/selfdrive/loggerd/tests/fill_eon.py @@ -1,4 +1,4 @@ -#!/usr/bin/env python +#!/usr/bin/env python3 """Script to fill up EON with fake data""" import os diff --git a/selfdrive/loggerd/tests/loggerd_tests_common.py b/selfdrive/loggerd/tests/loggerd_tests_common.py index dd48381d8e..9db75a7f59 100644 --- a/selfdrive/loggerd/tests/loggerd_tests_common.py +++ b/selfdrive/loggerd/tests/loggerd_tests_common.py @@ -7,7 +7,6 @@ import unittest import selfdrive.loggerd.uploader as uploader - def create_random_file(file_path, size_mb, lock=False): try: os.mkdir(os.path.dirname(file_path)) @@ -28,6 +27,30 @@ def create_random_file(file_path, size_mb, lock=False): if not lock: os.remove(lock_path) +class MockResponse(): + def __init__(self, text): + self.text = text + +class MockApi(): + def __init__(self, dongle_id): + pass + + def get(self, *args, **kwargs): + return MockResponse('{"url": "http://localhost/does/not/exist", "headers": {}}') + + def get_token(self): + return "fake-token" + +class MockParams(): + def __init__(self): + self.params = { + "DongleId": b"0000000000000000", + "IsUploadRawEnabled": b"1", + "IsUploadVideoOverCellularEnabled": b"1" + } + + def get(self, k): + return self.params[k] class UploaderTestCase(unittest.TestCase): f_type = "UNKNOWN" @@ -35,8 +58,14 @@ class UploaderTestCase(unittest.TestCase): def setUp(self): self.root = tempfile.mkdtemp() uploader.ROOT = self.root # Monkey patch root dir + uploader.Api = MockApi + uploader.Params = MockParams + uploader.fake_upload = 1 + uploader.is_on_hotspot = lambda *args: False + uploader.is_on_wifi = lambda *args: False self.seg_num = random.randint(1, 300) self.seg_format = "2019-04-18--12-52-54--{}" + self.seg_format2 = "2019-05-18--11-22-33--{}" self.seg_dir = self.seg_format.format(self.seg_num) def tearDown(self): @@ -48,6 +77,6 @@ class UploaderTestCase(unittest.TestCase): def make_file_with_data(self, f_dir, fn, size_mb=.1, lock=False): file_path = os.path.join(self.root, f_dir, fn) - create_random_file(file_path, size_mb) + create_random_file(file_path, size_mb, lock) return file_path diff --git a/selfdrive/loggerd/tests/test_deleter.py b/selfdrive/loggerd/tests/test_deleter.py index 6e62db600f..adbb0f0316 100644 --- a/selfdrive/loggerd/tests/test_deleter.py +++ b/selfdrive/loggerd/tests/test_deleter.py @@ -6,22 +6,19 @@ from collections import namedtuple import selfdrive.loggerd.deleter as deleter from common.timeout import Timeout, TimeoutException -from loggerd_tests_common import UploaderTestCase +from selfdrive.loggerd.tests.loggerd_tests_common import UploaderTestCase Stats = namedtuple("Stats", ['f_bavail', 'f_blocks']) -fake_stats = Stats(f_bavail=0, f_blocks=10) - - -def fake_statvfs(d): - return fake_stats - - class TestDeleter(UploaderTestCase): + def fake_statvfs(self, d): + return self.fake_stats + def setUp(self): self.f_type = "fcamera.hevc" super(TestDeleter, self).setUp() - deleter.os.statvfs = fake_statvfs + self.fake_stats = Stats(f_bavail=0, f_blocks=10) + deleter.os.statvfs = self.fake_statvfs deleter.ROOT = self.root def tearDown(self): @@ -69,10 +66,9 @@ class TestDeleter(UploaderTestCase): self.assertTrue(os.path.exists(f_path_2), "Newer file deleted before older file") def test_no_delete_when_available_space(self): - global fake_stats f_path = self.make_file_with_data(self.seg_dir, self.f_type) - fake_stats = Stats(f_bavail=10, f_blocks=10) + self.fake_stats = Stats(f_bavail=10, f_blocks=10) self.start_thread() diff --git a/selfdrive/loggerd/tests/test_uploader.py b/selfdrive/loggerd/tests/test_uploader.py new file mode 100644 index 0000000000..ed9a972220 --- /dev/null +++ b/selfdrive/loggerd/tests/test_uploader.py @@ -0,0 +1,116 @@ +import os +import time +import threading +import logging +import json + +from selfdrive.swaglog import cloudlog +import selfdrive.loggerd.uploader as uploader + +from common.timeout import Timeout + +from selfdrive.loggerd.tests.loggerd_tests_common import UploaderTestCase + +class TestLogHandler(logging.Handler): + def __init__(self): + logging.Handler.__init__(self) + self.reset() + + def reset(self): + self.upload_order = list() + + def emit(self, record): + try: + j = json.loads(record.message) + if j["event"] == "upload_success": + self.upload_order.append(j["key"]) + except BaseException: + pass + +log_handler = TestLogHandler() +cloudlog.addHandler(log_handler) + +class TestUploader(UploaderTestCase): + def setUp(self): + super(TestUploader, self).setUp() + log_handler.reset() + + def tearDown(self): + super(TestUploader, self).tearDown() + + def start_thread(self): + self.end_event = threading.Event() + self.up_thread = threading.Thread(target=uploader.uploader_fn, args=[self.end_event]) + self.up_thread.daemon = True + self.up_thread.start() + + def join_thread(self): + self.end_event.set() + self.up_thread.join() + + def gen_files(self, lock=False): + f_paths = list() + for t in ["bootlog.bz2", "qlog.bz2", "rlog.bz2", "dcamera.hevc", "fcamera.hevc"]: + f_paths.append(self.make_file_with_data(self.seg_dir, t, 1, lock=lock)) + return f_paths + + def gen_order(self, seg1, seg2): + keys = [f"{self.seg_format.format(i)}/qlog.bz2" for i in seg1] + keys += [f"{self.seg_format2.format(i)}/qlog.bz2" for i in seg2] + for i in seg1: + keys += [f"{self.seg_format.format(i)}/{f}" for f in ['rlog.bz2','fcamera.hevc','dcamera.hevc']] + for i in seg2: + keys += [f"{self.seg_format2.format(i)}/{f}" for f in ['rlog.bz2','fcamera.hevc','dcamera.hevc']] + keys += [f"{self.seg_format.format(i)}/bootlog.bz2" for i in seg1] + keys += [f"{self.seg_format2.format(i)}/bootlog.bz2" for i in seg2] + return keys + + def test_upload(self): + f_paths = self.gen_files(lock=False) + + self.start_thread() + + with Timeout(5, "Timeout waiting for file to be uploaded"): + while len(os.listdir(self.root)): + time.sleep(0.01) + self.join_thread() + + for f_path in f_paths: + self.assertFalse(os.path.exists(f_path), "All files not uploaded") + exp_order = self.gen_order([self.seg_num], []) + self.assertTrue(log_handler.upload_order == exp_order, "Files uploaded in wrong order") + + def test_upload_files_in_create_order(self): + f_paths = list() + seg1_nums = [0,1,2,10,20] + for i in seg1_nums: + self.seg_dir = self.seg_format.format(i) + f_paths += self.gen_files() + seg2_nums = [5,50,51] + for i in seg2_nums: + self.seg_dir = self.seg_format2.format(i) + f_paths += self.gen_files() + + self.start_thread() + + with Timeout(5, "Timeout waiting for file to be upload"): + while len(os.listdir(self.root)): + time.sleep(0.01) + + self.join_thread() + + for f_path in f_paths: + self.assertFalse(os.path.exists(f_path), "All files not uploaded") + exp_order = self.gen_order(seg1_nums, seg2_nums) + self.assertTrue(log_handler.upload_order == exp_order, "Files uploaded in wrong order") + + def test_no_upload_with_lock_file(self): + f_paths = self.gen_files(lock=True) + + self.start_thread() + # allow enough time that files should have been uploaded if they would be uploaded + time.sleep(5) + self.join_thread() + + for f_path in f_paths: + self.assertTrue(os.path.exists(f_path), "File upload when locked") diff --git a/selfdrive/loggerd/uploader.py b/selfdrive/loggerd/uploader.py index 38887f4ad2..dc9a2765f4 100644 --- a/selfdrive/loggerd/uploader.py +++ b/selfdrive/loggerd/uploader.py @@ -1,8 +1,7 @@ -#!/usr/bin/env python +#!/usr/bin/env python3 import os import re import time -import stat import json import random import ctypes @@ -12,7 +11,6 @@ import traceback import threading import subprocess -from collections import Counter from selfdrive.swaglog import cloudlog from selfdrive.loggerd.config import ROOT @@ -43,20 +41,17 @@ def raise_on_thread(t, exctype): ctypes.pythonapi.PyThreadState_SetAsyncExc(tid, 0) raise SystemError("PyThreadState_SetAsyncExc failed") -def listdir_with_creation_date(d): - lst = os.listdir(d) - for fn in lst: - try: - st = os.stat(os.path.join(d, fn)) - ctime = st[stat.ST_CTIME] - yield (ctime, fn) - except OSError: - cloudlog.exception("listdir_with_creation_date: stat failed?") - yield (None, fn) +def get_directory_sort(d): + return list(map(lambda s: s.rjust(10, '0'), d.rsplit('--', 1))) -def listdir_by_creation_date(d): - times_and_paths = list(listdir_with_creation_date(d)) - return [path for _, path in sorted(times_and_paths)] +def listdir_by_creation(d): + try: + paths = os.listdir(d) + paths = sorted(paths, key=get_directory_sort) + return paths + except OSError: + cloudlog.exception("listdir_by_creation failed") + return list() def clear_locks(root): for logname in os.listdir(root): @@ -71,17 +66,20 @@ def clear_locks(root): def is_on_wifi(): # ConnectivityManager.getActiveNetworkInfo() try: - result = subprocess.check_output(["service", "call", "connectivity", "2"]).strip().split("\n") + result = subprocess.check_output(["service", "call", "connectivity", "2"], encoding='utf8').strip().split("\n") # pylint: disable=unexpected-keyword-arg except subprocess.CalledProcessError: return False - data = ''.join(''.join(w.decode("hex")[::-1] for w in l[14:49].split()) for l in result[1:]) + # Concatenate all ascii parts + r = "" + for line in result[1:]: + r += line[51:67] - return "\x00".join("WIFI") in data + return "W.I.F.I" in r def is_on_hotspot(): try: - result = subprocess.check_output(["ifconfig", "wlan0"]) + result = subprocess.check_output(["ifconfig", "wlan0"], encoding='utf8') # pylint: disable=unexpected-keyword-arg result = re.findall(r"inet addr:((\d+\.){3}\d+)", result)[0][0] is_android = result.startswith('192.168.43.') @@ -92,7 +90,7 @@ def is_on_hotspot(): except: return False -class Uploader(object): +class Uploader(): def __init__(self, dongle_id, root): self.dongle_id = dongle_id self.api = Api(dongle_id) @@ -103,6 +101,9 @@ class Uploader(object): self.last_resp = None self.last_exc = None + self.immediate_priority = {"qlog.bz2": 0} + self.high_priority = {"rlog.bz2": 0, "fcamera.hevc": 1, "dcamera.hevc": 2} + def clean_dirs(self): try: for logname in os.listdir(self.root): @@ -113,10 +114,17 @@ class Uploader(object): except OSError: cloudlog.exception("clean_dirs failed") + def get_upload_sort(self, name): + if name in self.immediate_priority: + return self.immediate_priority[name] + if name in self.high_priority: + return self.high_priority[name] + 100 + return 1000 + def gen_upload_files(self): if not os.path.isdir(self.root): return - for logname in listdir_by_creation_date(self.root): + for logname in listdir_by_creation(self.root): path = os.path.join(self.root, logname) try: names = os.listdir(path) @@ -125,44 +133,32 @@ class Uploader(object): if any(name.endswith(".lock") for name in names): continue - for name in names: + for name in sorted(names, key=self.get_upload_sort): key = os.path.join(logname, name) fn = os.path.join(path, name) yield (name, key, fn) - def get_data_stats(self): - name_counts = Counter() - total_size = 0 - for name, key, fn in self.gen_upload_files(): - name_counts[name] += 1 - total_size += os.stat(fn).st_size - return dict(name_counts), total_size - def next_file_to_upload(self, with_raw): + upload_files = list(self.gen_upload_files()) # try to upload qlog files first - for name, key, fn in self.gen_upload_files(): - if name == "qlog.bz2": - return (key, fn, 0) + for name, key, fn in upload_files: + if name in self.immediate_priority: + return (key, fn) if with_raw: # then upload the full log files, rear and front camera files - for name, key, fn in self.gen_upload_files(): - if name == "rlog.bz2": - return (key, fn, 1) - elif name == "fcamera.hevc": - return (key, fn, 2) - elif name == "dcamera.hevc": - return (key, fn, 3) + for name, key, fn in upload_files: + if name in self.high_priority: + return (key, fn) # then upload other files - for name, key, fn in self.gen_upload_files(): + for name, key, fn in upload_files: if not name.endswith('.lock') and not name.endswith(".tmp"): - return (key, fn, 4) + return (key, fn) return None - def do_upload(self, key, fn): try: url_resp = self.api.get("v1.3/"+self.dongle_id+"/upload_url/", timeout=10, path=key, access_token=self.api.get_token()) @@ -173,7 +169,7 @@ class Uploader(object): if fake_upload: cloudlog.info("*** WARNING, THIS IS A FAKE UPLOAD TO %s ***" % url) - class FakeResponse(object): + class FakeResponse(): def __init__(self): self.status_code = 200 self.last_resp = FakeResponse() @@ -231,13 +227,11 @@ class Uploader(object): return success - - def uploader_fn(exit_event): cloudlog.info("uploader_fn") params = Params() - dongle_id = params.get("DongleId") + dongle_id = params.get("DongleId").decode('utf8') if dongle_id is None: cloudlog.info("uploader missing dongle_id") @@ -247,8 +241,8 @@ def uploader_fn(exit_event): backoff = 0.1 while True: - allow_raw_upload = (params.get("IsUploadRawEnabled") != "0") - allow_cellular = (params.get("IsUploadVideoOverCellularEnabled") != "0") + allow_raw_upload = (params.get("IsUploadRawEnabled") != b"0") + allow_cellular = (params.get("IsUploadVideoOverCellularEnabled") != b"0") on_hotspot = is_on_hotspot() on_wifi = is_on_wifi() should_upload = allow_cellular or (on_wifi and not on_hotspot) @@ -261,7 +255,7 @@ def uploader_fn(exit_event): time.sleep(5) continue - key, fn, _ = d + key, fn = d cloudlog.event("uploader_netcheck", allow_cellular=allow_cellular, is_on_hotspot=on_hotspot, is_on_wifi=on_wifi) cloudlog.info("to upload %r", d) diff --git a/selfdrive/logmessaged.py b/selfdrive/logmessaged.py index c2c583dfc5..57e5416aa5 100755 --- a/selfdrive/logmessaged.py +++ b/selfdrive/logmessaged.py @@ -1,4 +1,4 @@ -#!/usr/bin/env python +#!/usr/bin/env python3 import zmq from logentries import LogentriesHandler from selfdrive.services import service_list @@ -19,7 +19,8 @@ def main(gctx=None): pub_sock = messaging.pub_sock(service_list['logMessage'].port) while True: - dat = ''.join(sock.recv_multipart()) + dat = b''.join(sock.recv_multipart()) + dat = dat.decode('utf8') # print "RECV", repr(dat) diff --git a/selfdrive/manager.py b/selfdrive/manager.py index 62bf834b68..fb8b92e166 100755 --- a/selfdrive/manager.py +++ b/selfdrive/manager.py @@ -1,10 +1,12 @@ -#!/usr/bin/env python2.7 +#!/usr/bin/env python3.7 import os import sys import fcntl import errno import signal import subprocess +import datetime +from common.spinner import Spinner from common.basedir import BASEDIR sys.path.append(os.path.join(BASEDIR, "pyextra")) @@ -34,53 +36,24 @@ def unblock_stdout(): break try: - sys.stdout.write(dat) + sys.stdout.write(dat.decode('utf8')) except (OSError, IOError): pass os._exit(os.wait()[1]) if __name__ == "__main__": - is_neos = os.path.isfile("/init.qcom.rc") - neos_update_required = False - - if is_neos: - version = int(open("/VERSION").read()) if os.path.isfile("/VERSION") else 0 - revision = int(open("/REVISION").read()) if version >= 10 else 0 # Revision only present in NEOS 10 and up - neos_update_required = version < 10 or (version == 10 and revision < 4) - - if neos_update_required: - # update continue.sh before updating NEOS - if os.path.isfile(os.path.join(BASEDIR, "scripts", "continue.sh")): - from shutil import copyfile - copyfile(os.path.join(BASEDIR, "scripts", "continue.sh"), "/data/data/com.termux/files/continue.sh") - - # run the updater - print("Starting NEOS updater") - subprocess.check_call(["git", "clean", "-xdf"], cwd=BASEDIR) - updater_dir = os.path.join(BASEDIR, "installer", "updater") - manifest_path = os.path.realpath(os.path.join(updater_dir, "update.json")) - os.system(os.path.join(updater_dir, "updater") + " file://" + manifest_path) - raise Exception("NEOS outdated") - elif os.path.isdir("/data/neoupdate"): - from shutil import rmtree - rmtree("/data/neoupdate") - unblock_stdout() import glob import shutil import hashlib import importlib -import re -import stat -import subprocess import traceback from multiprocessing import Process from setproctitle import setproctitle #pylint: disable=no-name-in-module -from common.file_helpers import atomic_write_in_dir_neos from common.params import Params import cereal ThermalStatus = cereal.log.ThermalData.ThermalStatus @@ -134,7 +107,7 @@ unkillable_processes = ['visiond'] interrupt_processes = [] # processes to end with SIGKILL instead of SIGTERM -kill_processes = ['sensord'] +kill_processes = ['sensord', 'paramsd'] persistent_processes = [ 'thermald', @@ -405,7 +378,7 @@ def manager_thread(): break def get_installed_apks(): - dat = subprocess.check_output(["pm", "list", "packages", "-f"]).strip().split("\n") + dat = subprocess.check_output(["pm", "list", "packages", "-f"], encoding='utf8').strip().split("\n") # pylint: disable=unexpected-keyword-arg ret = {} for x in dat: if x.startswith("package:"): @@ -440,10 +413,10 @@ def update_apks(): if not os.path.exists(apk_path): continue - h1 = hashlib.sha1(open(apk_path).read()).hexdigest() + h1 = hashlib.sha1(open(apk_path, 'rb').read()).hexdigest() h2 = None if installed[app] is not None: - h2 = hashlib.sha1(open(installed[app]).read()).hexdigest() + h2 = hashlib.sha1(open(installed[app], 'rb').read()).hexdigest() cloudlog.info("comparing version of %s %s vs %s" % (app, h1, h2)) if h2 is None or h1 != h2: @@ -457,46 +430,7 @@ def update_apks(): assert success -def update_ssh(): - ssh_home_dirpath = "/system/comma/home/.ssh/" - auth_keys_path = os.path.join(ssh_home_dirpath, "authorized_keys") - auth_keys_persist_path = os.path.join(ssh_home_dirpath, "authorized_keys.persist") - auth_keys_mode = stat.S_IREAD | stat.S_IWRITE - - params = Params() - github_keys = params.get("GithubSshKeys") or '' - - old_keys = open(auth_keys_path).read() - has_persisted_keys = os.path.exists(auth_keys_persist_path) - if has_persisted_keys: - persisted_keys = open(auth_keys_persist_path).read() - else: - # add host filter - persisted_keys = re.sub(r'^(?!.+?from.+? )(ssh|ecdsa)', 'from="10.0.0.0/8,172.16.0.0/12,192.168.0.0/16" \\1', old_keys, flags=re.MULTILINE) - - new_keys = persisted_keys + '\n' + github_keys - - if has_persisted_keys and new_keys == old_keys and os.stat(auth_keys_path)[stat.ST_MODE] == auth_keys_mode: - # nothing to do - let's avoid remount - return - - try: - subprocess.check_call(["mount", "-o", "rw,remount", "/system"]) - if not has_persisted_keys: - atomic_write_in_dir_neos(auth_keys_persist_path, persisted_keys, mode=auth_keys_mode) - - atomic_write_in_dir_neos(auth_keys_path, new_keys, mode=auth_keys_mode) - finally: - try: - subprocess.check_call(["mount", "-o", "ro,remount", "/system"]) - except: - cloudlog.exception("Failed to remount as read-only") - # this can fail due to "Device busy" - reboot if so - os.system("reboot") - raise RuntimeError - def manager_update(): - update_ssh() update_apks() uninstall = [app for app in get_installed_apks().keys() if app in ("com.spotify.music", "com.waze")] @@ -504,13 +438,16 @@ def manager_update(): cloudlog.info("uninstalling %s" % app) os.system("pm uninstall % s" % app) -def manager_prepare(): +def manager_prepare(spinner=None): # build cereal first subprocess.check_call(["make", "-j4"], cwd=os.path.join(BASEDIR, "cereal")) # build all processes os.chdir(os.path.dirname(os.path.abspath(__file__))) - for p in managed_processes: + + for i, p in enumerate(managed_processes): + if spinner is not None: + spinner.update("%d" % (100.0 * (i + 1) / len(managed_processes),)) prepare_managed_process(p) def uninstall(): @@ -557,12 +494,16 @@ def main(): params.manager_start() # set unset params + if params.get("CompletedTrainingVersion") is None: + params.put("CompletedTrainingVersion", "0") if params.get("IsMetric") is None: params.put("IsMetric", "0") if params.get("RecordFront") is None: params.put("RecordFront", "0") if params.get("HasAcceptedTerms") is None: params.put("HasAcceptedTerms", "0") + if params.get("HasCompletedSetup") is None: + params.put("HasCompletedSetup", "0") if params.get("IsUploadRawEnabled") is None: params.put("IsUploadRawEnabled", "1") if params.get("IsUploadVideoOverCellularEnabled") is None: @@ -577,6 +518,11 @@ def main(): params.put("LimitSetSpeed", "0") if params.get("LimitSetSpeedNeural") is None: params.put("LimitSetSpeedNeural", "0") + if params.get("LastUpdateTime") is None: + t = datetime.datetime.now().isoformat() + params.put("LastUpdateTime", t.encode('utf8')) + if params.get("OpenpilotEnabledToggle") is None: + params.put("OpenpilotEnabledToggle", "1") # is this chffrplus? if os.getenv("PASSIVE") is not None: @@ -585,21 +531,11 @@ def main(): if params.get("Passive") is None: raise Exception("Passive must be set to continue") - # put something on screen while we set things up - if os.getenv("PREPAREONLY") is not None: - spinner_proc = None - else: - spinner_text = "chffrplus" if params.get("Passive")=="1" else "openpilot" - spinner_proc = subprocess.Popen(["./spinner", "loading %s"%spinner_text], - cwd=os.path.join(BASEDIR, "selfdrive", "ui", "spinner"), - close_fds=True) - try: - manager_update() - manager_init() - manager_prepare() - finally: - if spinner_proc: - spinner_proc.terminate() + with Spinner() as spinner: + spinner.update("0") # Show progress bar + manager_update() + manager_init() + manager_prepare(spinner) if os.getenv("PREPAREONLY") is not None: return diff --git a/selfdrive/messaging.py b/selfdrive/messaging.py index 13fb41efeb..3310ff0785 100644 --- a/selfdrive/messaging.py +++ b/selfdrive/messaging.py @@ -31,6 +31,24 @@ def sub_sock(port, poller=None, addr="127.0.0.1", conflate=False, timeout=None): poller.register(sock, zmq.POLLIN) return sock + +def drain_sock_raw_poller(poller, sock, wait_for_one=False): + ret = [] + + if wait_for_one: + try: + ret.append(sock.recv()) + except zmq.error.Again: # Thrown when there is timeout on the socket + return ret + + while True: + if not poller.poll(0): + break # Socket has no more messages + + ret.append(sock.recv()) + + return ret + def drain_sock_raw(sock, wait_for_one=False): ret = [] while 1: @@ -84,7 +102,7 @@ def recv_one_or_none(sock): return None -class SubMaster(object): +class SubMaster(): def __init__(self, services, ignore_alive=None, addr="127.0.0.1"): self.poller = zmq.Poller() self.frame = -1 @@ -108,8 +126,14 @@ class SubMaster(object): if addr is not None: self.sock[s] = sub_sock(service_list[s].port, poller=self.poller, addr=addr, conflate=True) self.freq[s] = service_list[s].frequency + data = new_message() - data.init(s) + if s in ['can', 'sensorEvents', 'liveTracks', 'sendCan', + 'ethernetData', 'cellInfo', 'wifiScan', + 'trafficEvents', 'orbObservation', 'carEvents']: + data.init(s, 0) + else: + data.init(s) self.data[s] = getattr(data, s) self.logMonoTime[s] = 0 self.valid[s] = data.valid @@ -168,6 +192,6 @@ class PubMaster(): def send(self, s, dat): # accept either bytes or capnp builder - if not isinstance(dat, str): + if not isinstance(dat, bytes): dat = dat.to_bytes() self.sock[s].send(dat) diff --git a/selfdrive/pandad.py b/selfdrive/pandad.py index 09f2e717f9..eac0ea92d1 100755 --- a/selfdrive/pandad.py +++ b/selfdrive/pandad.py @@ -1,4 +1,4 @@ -#!/usr/bin/env python2 +#!/usr/bin/env python3 # simple boardd wrapper that updates the panda first import os import time @@ -32,11 +32,15 @@ def update_panda(): panda_dfu = PandaDFU(panda_dfu[0]) panda_dfu.recover() - print "waiting for board..." + print("waiting for board...") time.sleep(1) - current_version = "bootstub" if panda.bootstub else str(panda.get_version()) - cloudlog.info("Panda connected, version: %s, expected %s" % (current_version, repo_version)) + 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): cloudlog.info("Panda firmware out of date, update required") @@ -60,7 +64,7 @@ def update_panda(): cloudlog.info("Panda still not booting, exiting") raise AssertionError - version = str(panda.get_version()) + version = panda.get_version() if not version.startswith(repo_version): cloudlog.info("Version mismatch after flashing, exiting") raise AssertionError diff --git a/selfdrive/registration.py b/selfdrive/registration.py index 5d45b11292..1c85e40b22 100644 --- a/selfdrive/registration.py +++ b/selfdrive/registration.py @@ -1,42 +1,50 @@ import os import json +import binascii import subprocess -import struct +import itertools from datetime import datetime, timedelta from selfdrive.swaglog import cloudlog from selfdrive.version import version, terms_version, training_version, get_git_commit, get_git_branch, get_git_remote from common.api import api_get from common.params import Params -from common.file_helpers import mkdirs_exists_ok def get_imei(): - ret = subprocess.check_output(["getprop", "oem.device.imeicache"]).strip() + ret = subprocess.check_output(["getprop", "oem.device.imeicache"], encoding='utf8').strip() # pylint: disable=unexpected-keyword-arg if ret == "": ret = "000000000000000" return ret def get_serial(): - return subprocess.check_output(["getprop", "ro.serialno"]).strip() + return subprocess.check_output(["getprop", "ro.serialno"], encoding='utf8').strip() # pylint: disable=unexpected-keyword-arg # TODO: move this to a library def parse_service_call(call): - ret = subprocess.check_output(call).strip() + ret = subprocess.check_output(call, encoding='utf8').strip() # pylint: disable=unexpected-keyword-arg if 'Parcel' not in ret: return None + try: - def fh(x): - if len(x) != 8: - return [] - return [x[6:8], x[4:6], x[2:4], x[0:2]] - hd = [] - for x in ret.split("\n")[1:]: - for k in map(fh, x.split(": ")[1].split(" '")[0].split(" ")): - hd.extend(k) - return ''.join([chr(int(x, 16)) for x in hd]) + r = b"" + for line in ret.split("\n")[1:]: # Skip 'Parcel(' + line_hex = line[14:49].replace(' ', '') + r += binascii.unhexlify(line_hex) + + r = r[8:] # Cut off length field + r = r.decode('utf_16_be') + + # All pairs of two characters seem to be swapped. Not sure why + result = "" + for a, b, in itertools.zip_longest(r[::2], r[1::2], fillvalue='\x00'): + result += b + a + + result = result.replace('\x00', '') + + return result except Exception: return None @@ -45,9 +53,7 @@ def get_subscriber_info(): ret = parse_service_call(["service", "call", "iphonesubinfo", "7"]) if ret is None or len(ret) < 8: return "" - if struct.unpack("I", ret[4:8]) == -1: - return "" - return ret[8:-2:2] + return ret def register(): @@ -60,22 +66,11 @@ 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) - dongle_id, access_token = params.get("DongleId"), params.get("AccessToken") + dongle_id, access_token = params.get("DongleId", encoding='utf8'), params.get("AccessToken", encoding='utf8') public_key = open("/persist/comma/id_rsa.pub").read() # create registration token @@ -91,7 +86,7 @@ def register(): resp = api_get("v2/pilotauth/", method='POST', timeout=15, imei=get_imei(), serial=get_serial(), public_key=public_key, register_token=register_token) dongleauth = json.loads(resp.text) - dongle_id, access_token = dongleauth["dongle_id"].encode('ascii'), dongleauth["access_token"].encode('ascii') + dongle_id, access_token = dongleauth["dongle_id"], dongleauth["access_token"] params.put("DongleId", dongle_id) params.put("AccessToken", access_token) diff --git a/selfdrive/sensord/gpsd.cc b/selfdrive/sensord/gpsd.cc index 6bf7a6a9e1..0a8cd1bffc 100644 --- a/selfdrive/sensord/gpsd.cc +++ b/selfdrive/sensord/gpsd.cc @@ -28,7 +28,7 @@ #include "rawgps.h" -volatile int do_exit = 0; +volatile sig_atomic_t do_exit = 0; namespace { @@ -219,7 +219,7 @@ void* clock_thread(void* args) { cereal::Event::Builder event = msg.initRoot(); event.setLogMonoTime(boottime); auto clocks = event.initClocks(); - + clocks.setBootTimeNanos(boottime); clocks.setMonotonicNanos(monotonic); clocks.setMonotonicRawNanos(monotonic_raw); @@ -248,7 +248,7 @@ int main() { signal(SIGTERM, (sighandler_t)set_do_exit); gps_init(); - + rawgps_init(); err = pthread_create(&clock_thread_handle, NULL, diff --git a/selfdrive/sensord/rawgps.cc b/selfdrive/sensord/rawgps.cc index 91e5562c7d..7f5206a06c 100644 --- a/selfdrive/sensord/rawgps.cc +++ b/selfdrive/sensord/rawgps.cc @@ -18,8 +18,6 @@ #include "libdiag.h" -extern volatile int do_exit; - #define NV_GNSS_OEM_FEATURE_MASK 7165 # define NV_GNSS_OEM_FEATURE_MASK_OEMDRE 1 #define NV_CGPS_DPO_CONTROL 5596 @@ -376,7 +374,7 @@ struct __attribute__((packed)) GNSSOemdreMeasurement { float doppler_acceleration; float fine_speed; float fine_speed_uncertainty; - + uint64_t carrier_phase; uint32_t f_count; @@ -428,7 +426,7 @@ struct __attribute__((packed)) GNSSOemdreMeasurementReportv2 { uint8_t source; GNSSOemdreMeasurement measurements[16]; - + }; _Static_assert(sizeof(GNSSOemdreMeasurementReportv2) == 1851, "error"); @@ -590,7 +588,7 @@ static void handle_log(uint8_t *ptr, int len) { assert(len >= sizeof(GNSSGpsMeasurementReportv0)); const GNSSGpsMeasurementReportv0* report = (const GNSSGpsMeasurementReportv0*)ptr; assert(len >= sizeof(sizeof(GNSSGpsMeasurementReportv0))+sizeof(GNSSGpsMeasurementReportv0_SV) * report->sv_count); - + auto lreport = qcomGnss.initMeasurementReport(); lreport.setSource(cereal::QcomGnss::MeasurementSource::GPS); lreport.setFCount(report->f_count); @@ -646,9 +644,9 @@ static void handle_log(uint8_t *ptr, int len) { #ifdef RAWGPS_TEST // if (sv->measurement_status & (1 << 27)) printf("%d\n", sv->unfiltered_measurement_integral); - printf("GPS %03d %d %d 0x%08X o: %02x go: %02x fs: %02x cn: %02x pd: %02x cs: %02x po: %02x ms: %08x ms2: %08x me: %08x az: %08x el: %08x fc: %08x\n", + printf("GPS %03d %d %d 0x%08X o: %02x go: %02x fs: %02x cn: %02x pd: %02x cs: %02x po: %02x ms: %08x ms2: %08x me: %08x az: %08x el: %08x fc: %08x\n", sv->sv_id, - !!(sv->measurement_status & (1 << 27)), + !!(sv->measurement_status & (1 << 27)), sv->unfiltered_measurement_integral, sv->unfiltered_measurement_integral, sv->observations, sv->good_observations, @@ -731,9 +729,9 @@ static void handle_log(uint8_t *ptr, int len) { #ifdef RAWGPS_TEST // if (sv->measurement_status & (1 << 27)) printf("%d\n", sv->unfiltered_measurement_integral); - printf("GLO %03d %02x %d %d 0x%08X o: %02x go: %02x fs: %02x cn: %02x pd: %02x cs: %02x po: %02x ms: %08x ms2: %08x me: %08x az: %08x el: %08x fc: %08x\n", + printf("GLO %03d %02x %d %d 0x%08X o: %02x go: %02x fs: %02x cn: %02x pd: %02x cs: %02x po: %02x ms: %08x ms2: %08x me: %08x az: %08x el: %08x fc: %08x\n", sv->sv_id, sv->frequency_index & 0xff, - !!(sv->measurement_status & (1 << 27)), + !!(sv->measurement_status & (1 << 27)), sv->unfiltered_measurement_integral, sv->unfiltered_measurement_integral, sv->observations, sv->good_observations, @@ -779,7 +777,7 @@ static void handle_log(uint8_t *ptr, int len) { lreport.setGpsLeapSecondsUncertainty(report->gps_leap_seconds_uncertainty); lreport.setGpsToGlonassTimeBiasMilliseconds(report->gps_to_glonass_time_bias_milliseconds); lreport.setGpsToGlonassTimeBiasMillisecondsUncertainty(report->gps_to_glonass_time_bias_milliseconds_uncertainty); - + lreport.setGpsWeek(report->gps_week); lreport.setGpsMilliseconds(report->gps_milliseconds); lreport.setGpsTimeBiasMs(report->gps_time_bias); @@ -869,7 +867,7 @@ static void handle_log(uint8_t *ptr, int len) { lreport.setSvId(report->sv_id); lreport.setFrequencyIndex(report->frequency_index); - + lreport.setHasPosition(report->flags & 1); lreport.setHasIono(report->flags & 2); lreport.setHasTropo(report->flags & 4); @@ -986,7 +984,7 @@ static int oemdre_on(int client_id) { int res_len = diag_send_sync(client_id, (unsigned char*)&req_pkt, sizeof(req_pkt), res_pkt, sizeof(res_pkt)); GpsOemControlResp *resp = (GpsOemControlResp*)res_pkt; - + if (res_len != sizeof(GpsOemControlResp) || resp->cmd_code != DIAG_SUBSYS_CMD || resp->subsys_id != DIAG_SUBSYS_GPS @@ -1060,7 +1058,7 @@ static bool nv_write_u32(int client_id, uint16_t nv_id, uint32_t val) { .nv_id = nv_id, }; *(uint32_t*)req.data = val; - + NvPacket resp = {0}; int res_len = diag_send_sync(client_id, (unsigned char*)&req, sizeof(req), (unsigned char*)&resp, sizeof(resp)); @@ -1072,7 +1070,7 @@ static bool nv_write_u32(int client_id, uint16_t nv_id, uint32_t val) { LOGW("nv_write_u32: diag command failed"); return false; } - + if (resp.status != NV_DONE) { LOGW("nv_write_u32: write failed: %d", resp.status); return false; @@ -1186,4 +1184,4 @@ int main() { rawgps_destroy(); return 0; } -#endif \ No newline at end of file +#endif diff --git a/selfdrive/sensord/sensors.cc b/selfdrive/sensord/sensors.cc index 5717458f72..ded2b88c44 100644 --- a/selfdrive/sensord/sensors.cc +++ b/selfdrive/sensord/sensors.cc @@ -38,7 +38,7 @@ #define SENSOR_PROXIMITY 6 #define SENSOR_LIGHT 7 -volatile int do_exit = 0; +volatile sig_atomic_t do_exit = 0; namespace { diff --git a/selfdrive/sensord/start_gpsd.py b/selfdrive/sensord/start_gpsd.py index ada2687158..287ead2959 100755 --- a/selfdrive/sensord/start_gpsd.py +++ b/selfdrive/sensord/start_gpsd.py @@ -1,4 +1,4 @@ -#!/usr/bin/env python2 +#!/usr/bin/env python3 import os assert os.system("make") == 0 diff --git a/selfdrive/sensord/start_sensord.py b/selfdrive/sensord/start_sensord.py index d6d6564113..bd0cbed82b 100755 --- a/selfdrive/sensord/start_sensord.py +++ b/selfdrive/sensord/start_sensord.py @@ -1,4 +1,4 @@ -#!/usr/bin/env python2 +#!/usr/bin/env python3 import os assert os.system("make") == 0 diff --git a/selfdrive/service_list.yaml b/selfdrive/service_list.yaml index ec8b0b9d44..a38c6ad714 100644 --- a/selfdrive/service_list.yaml +++ b/selfdrive/service_list.yaml @@ -40,7 +40,7 @@ lidarPts: [8030, true, 0.] procLog: [8031, true, 0.5] gpsLocationExternal: [8032, true, 10., 1] ubloxGnss: [8033, true, 10.] -clocks: [8034, true, 1.] +clocks: [8034, true, 1., 1] liveMpc: [8035, false, 20.] liveLongitudinalMpc: [8036, false, 20.] plusFrame: [8037, false, 0.] diff --git a/selfdrive/services.py b/selfdrive/services.py index 18d43ff76a..24a96c4eb2 100644 --- a/selfdrive/services.py +++ b/selfdrive/services.py @@ -1,15 +1,20 @@ import os import yaml -class Service(object): - def __init__(self, port, should_log, frequency): +class Service(): + def __init__(self, port, should_log, frequency, decimation=None): self.port = port self.should_log = should_log self.frequency = frequency + self.decimation = decimation service_list_path = os.path.join(os.path.dirname(__file__), "service_list.yaml") service_list = {} with open(service_list_path, "r") as f: for k, v in yaml.safe_load(f).items(): - service_list[k] = Service(v[0], v[1], v[2]) + decimation = None + if len(v) == 4: + decimation = v[3] + + service_list[k] = Service(v[0], v[1], v[2], decimation) diff --git a/selfdrive/swaglog.py b/selfdrive/swaglog.py index 169898637a..90962cb5d8 100644 --- a/selfdrive/swaglog.py +++ b/selfdrive/swaglog.py @@ -14,6 +14,7 @@ class LogMessageHandler(logging.Handler): def connect(self): self.zctx = zmq.Context() self.sock = self.zctx.socket(zmq.PUSH) + self.sock.setsockopt(zmq.LINGER, 10) self.sock.connect("ipc:///tmp/logmessage") self.pid = os.getpid() @@ -24,7 +25,8 @@ class LogMessageHandler(logging.Handler): msg = self.format(record).rstrip('\n') # print("SEND".format(repr(msg))) try: - self.sock.send(chr(record.levelno)+msg, zmq.NOBLOCK) + s = chr(record.levelno)+msg + self.sock.send(s.encode('utf8'), zmq.NOBLOCK) except zmq.error.Again: # drop :/ pass diff --git a/selfdrive/debug/getframes/__init__.py b/selfdrive/test/longitudinal_maneuvers/__init__.py similarity index 100% rename from selfdrive/debug/getframes/__init__.py rename to selfdrive/test/longitudinal_maneuvers/__init__.py diff --git a/selfdrive/test/plant/maneuver.py b/selfdrive/test/longitudinal_maneuvers/maneuver.py similarity index 94% rename from selfdrive/test/plant/maneuver.py rename to selfdrive/test/longitudinal_maneuvers/maneuver.py index ea5ebdd9ee..e79cf61d26 100644 --- a/selfdrive/test/plant/maneuver.py +++ b/selfdrive/test/longitudinal_maneuvers/maneuver.py @@ -1,10 +1,10 @@ from collections import defaultdict -from selfdrive.test.plant.maneuverplots import ManeuverPlot -from selfdrive.test.plant.plant import Plant +from selfdrive.test.longitudinal_maneuvers.maneuverplots import ManeuverPlot +from selfdrive.test.longitudinal_maneuvers.plant import Plant import numpy as np -class Maneuver(object): +class Maneuver(): def __init__(self, title, duration, **kwargs): # Was tempted to make a builder class self.distance_lead = kwargs.get("initial_distance_lead", 200.0) @@ -79,7 +79,7 @@ class Maneuver(object): for check in self.checks: c = check(logs) if not c: - print check.__name__ + " not valid!" + print(check.__name__ + " not valid!") valid = valid and c print("maneuver end", valid) diff --git a/selfdrive/test/plant/maneuverplots.py b/selfdrive/test/longitudinal_maneuvers/maneuverplots.py similarity index 99% rename from selfdrive/test/plant/maneuverplots.py rename to selfdrive/test/longitudinal_maneuvers/maneuverplots.py index 36b3e5bf83..3d52588100 100644 --- a/selfdrive/test/plant/maneuverplots.py +++ b/selfdrive/test/longitudinal_maneuvers/maneuverplots.py @@ -6,7 +6,7 @@ import pylab from selfdrive.config import Conversions as CV -class ManeuverPlot(object): +class ManeuverPlot(): def __init__(self, title = None): self.time_array = [] diff --git a/selfdrive/test/plant/plant.py b/selfdrive/test/longitudinal_maneuvers/plant.py similarity index 92% rename from selfdrive/test/plant/plant.py rename to selfdrive/test/longitudinal_maneuvers/plant.py index 531ad8ad26..1a57284697 100755 --- a/selfdrive/test/plant/plant.py +++ b/selfdrive/test/longitudinal_maneuvers/plant.py @@ -1,4 +1,5 @@ -#!/usr/bin/env python +#!/usr/bin/env python3 +import binascii import os import struct import time @@ -24,7 +25,7 @@ from common.dbc import dbc honda = dbc(os.path.join(DBC_PATH, "honda_civic_touring_2016_can_generated.dbc")) # Trick: set 0x201 (interceptor) in fingerprints for gas is controlled like if there was an interceptor -CP = CarInterface.get_params(CAR.CIVIC, {0x201}) +CP = CarInterface.get_params(CAR.CIVIC, {0: {0x201: 6}, 1: {}, 2: {}, 3: {}}) def car_plant(pos, speed, grade, gas, brake): @@ -81,12 +82,15 @@ def get_car_can_parser(): return CANParser(dbc_f, signals, checks) def to_3_byte(x): - return struct.pack("!H", int(x)).encode("hex")[1:] + # Convert into 12 bit value + s = struct.pack("!H", int(x)) + return binascii.hexlify(s)[1:] def to_3s_byte(x): - return struct.pack("!h", int(x)).encode("hex")[1:] + s = struct.pack("!h", int(x)) + return binascii.hexlify(s)[1:] -class Plant(object): +class Plant(): messaging_initialized = False def __init__(self, lead_relevancy=False, rate=100, speed=0.0, distance_lead=2.0): @@ -105,6 +109,7 @@ class Plant(object): Plant.plan = messaging.sub_sock(service_list['plan'].port) Plant.messaging_initialized = True + self.frame = 0 self.angle_steer = 0. self.gear_choice = 0 self.speed, self.speed_prev = 0., 0. @@ -221,7 +226,7 @@ class Plant(object): lateral_pos_rel = 0. # print at 5hz - if (self.rk.frame % (self.rate//5)) == 0: + if (self.frame % (self.rate//5)) == 0: print("%6.2f m %6.2f m/s %6.2f m/s2 %.2f ang gas: %.2f brake: %.2f steer: %5.2f lead_rel: %6.2f m %6.2f m/s" % (distance, speed, acceleration, self.angle_steer, gas, brake, steer_torque, d_rel, v_rel)) # ******** publish the car ******** @@ -301,10 +306,10 @@ class Plant(object): msg_struct[sgs[i]] = getattr(vls, sgs[i]) if "COUNTER" in honda.get_signals(msg): - msg_struct["COUNTER"] = self.rk.frame % 4 + msg_struct["COUNTER"] = self.frame % 4 if "COUNTER_PEDAL" in honda.get_signals(msg): - msg_struct["COUNTER_PEDAL"] = self.rk.frame % 0xf + msg_struct["COUNTER_PEDAL"] = self.frame % 0xf msg = honda.lookup_msg_id(msg) msg_data = honda.encode(msg, msg_struct) @@ -313,24 +318,26 @@ class Plant(object): msg_data = fix(msg_data, msg) if "CHECKSUM_PEDAL" in honda.get_signals(msg): - msg_struct["CHECKSUM_PEDAL"] = crc8_pedal([ord(i) for i in msg_data][:-1]) + msg_struct["CHECKSUM_PEDAL"] = crc8_pedal(msg_data[:-1]) msg_data = honda.encode(msg, msg_struct) can_msgs.append([msg, 0, msg_data, 0]) # add the radar message # TODO: use the DBC - if self.rk.frame % 5 == 0: - radar_state_msg = '\x79\x00\x00\x00\x00\x00\x00\x00' + if self.frame % 5 == 0: + radar_state_msg = b'\x79\x00\x00\x00\x00\x00\x00\x00' radar_msg = to_3_byte(d_rel*16.0) + \ to_3_byte(int(lateral_pos_rel*16.0)&0x3ff) + \ to_3s_byte(int(v_rel*32.0)) + \ - "0f00000" + b"0f00000" + + radar_msg = binascii.unhexlify(radar_msg) can_msgs.append([0x400, 0, radar_state_msg, 1]) - can_msgs.append([0x445, 0, radar_msg.decode("hex"), 1]) + can_msgs.append([0x445, 0, radar_msg, 1]) # add camera msg so controlsd thinks it's alive - msg_struct["COUNTER"] = self.rk.frame % 4 + msg_struct["COUNTER"] = self.frame % 4 msg = honda.lookup_msg_id(0xe4) msg_data = honda.encode(msg, msg_struct) msg_data = fix(msg_data, 0xe4) @@ -366,7 +373,7 @@ class Plant(object): # ******** publish a fake model going straight and fake calibration ******** # note that this is worst case for MPC, since model will delay long mpc by one time step - if publish_model and self.rk.frame % 5 == 0: + if publish_model and self.frame % 5 == 0: md = messaging.new_message() cal = messaging.new_message() md.init('model') @@ -406,18 +413,23 @@ class Plant(object): Plant.logcan.send(can_list_to_can_capnp(can_msgs)) # ******** update prevs ******** - self.speed = speed - self.distance = distance - self.distance_lead = distance_lead - - self.speed_prev = speed - self.distance_prev = distance - self.distance_lead_prev = distance_lead + self.frame += 1 if self.response_seen: self.rk.monitor_time() + + self.speed = speed + self.distance = distance + self.distance_lead = distance_lead + + self.speed_prev = speed + self.distance_prev = distance + self.distance_lead_prev = distance_lead + else: + # Don't advance time when controlsd is not yet ready self.rk.keep_time() + self.rk._frame = 0 return { "distance": distance, diff --git a/selfdrive/test/plant/plant_ui.py b/selfdrive/test/longitudinal_maneuvers/plant_ui.py similarity index 97% rename from selfdrive/test/plant/plant_ui.py rename to selfdrive/test/longitudinal_maneuvers/plant_ui.py index 5fa0ffad24..e9dd18f372 100755 --- a/selfdrive/test/plant/plant_ui.py +++ b/selfdrive/test/longitudinal_maneuvers/plant_ui.py @@ -1,6 +1,6 @@ -#!/usr/bin/env python +#!/usr/bin/env python3 import pygame # pylint: disable=import-error -from selfdrive.test.plant.plant import Plant +from selfdrive.test.longitudinal_maneuvers.plant import Plant from selfdrive.car.honda.values import CruiseButtons import numpy as np import selfdrive.messaging as messaging diff --git a/selfdrive/test/tests/plant/test_longitudinal.py b/selfdrive/test/longitudinal_maneuvers/test_longitudinal.py similarity index 98% rename from selfdrive/test/tests/plant/test_longitudinal.py rename to selfdrive/test/longitudinal_maneuvers/test_longitudinal.py index ca05924200..bfcbf67f7b 100755 --- a/selfdrive/test/tests/plant/test_longitudinal.py +++ b/selfdrive/test/longitudinal_maneuvers/test_longitudinal.py @@ -1,4 +1,4 @@ -#!/usr/bin/env python +#!/usr/bin/env python3 import os os.environ['OLD_CAN'] = '1' os.environ['NOCRASH'] = '1' @@ -11,7 +11,7 @@ matplotlib.use('svg') from selfdrive.config import Conversions as CV from selfdrive.car.honda.values import CruiseButtons as CB -from selfdrive.test.plant.maneuver import Maneuver +from selfdrive.test.longitudinal_maneuvers.maneuver import Maneuver import selfdrive.manager as manager from common.params import Params @@ -332,6 +332,7 @@ class LongitudinalControl(unittest.TestCase): shutil.rmtree('/data/params', ignore_errors=True) params = Params() params.put("Passive", "1" if os.getenv("PASSIVE") else "0") + params.put("OpenpilotEnabledToggle", "1") manager.gctx = {} manager.prepare_managed_process('radard') diff --git a/selfdrive/test/openpilotci_upload.py b/selfdrive/test/openpilotci_upload.py index 85509851de..df40715716 100755 --- a/selfdrive/test/openpilotci_upload.py +++ b/selfdrive/test/openpilotci_upload.py @@ -1,10 +1,11 @@ -#!/usr/bin/env python2 +#!/usr/bin/env python3 import os import sys import subprocess -from azure.storage.blob import BlockBlobService + def upload_file(path, name): + from azure.storage.blob import BlockBlobService # pylint: disable=no-name-in-module, import-error sas_token = os.getenv("TOKEN", None) if sas_token is not None: service = BlockBlobService(account_name="commadataci", sas_token=sas_token) @@ -18,5 +19,4 @@ if __name__ == "__main__": for f in sys.argv[1:]: name = os.path.basename(f) url = upload_file(f, name) - print url - + print(url) diff --git a/selfdrive/test/tests/process_replay/.gitignore b/selfdrive/test/process_replay/.gitignore similarity index 100% rename from selfdrive/test/tests/process_replay/.gitignore rename to selfdrive/test/process_replay/.gitignore diff --git a/selfdrive/test/tests/process_replay/README.md b/selfdrive/test/process_replay/README.md similarity index 100% rename from selfdrive/test/tests/process_replay/README.md rename to selfdrive/test/process_replay/README.md diff --git a/selfdrive/test/plant/__init__.py b/selfdrive/test/process_replay/__init__.py similarity index 100% rename from selfdrive/test/plant/__init__.py rename to selfdrive/test/process_replay/__init__.py diff --git a/selfdrive/test/tests/process_replay/compare_logs.py b/selfdrive/test/process_replay/compare_logs.py similarity index 98% rename from selfdrive/test/tests/process_replay/compare_logs.py rename to selfdrive/test/process_replay/compare_logs.py index 5a1bd3fd73..34f6ded602 100755 --- a/selfdrive/test/tests/process_replay/compare_logs.py +++ b/selfdrive/test/process_replay/compare_logs.py @@ -1,4 +1,4 @@ -#!/usr/bin/env python2 +#!/usr/bin/env python3 import bz2 import os import sys diff --git a/selfdrive/test/tests/process_replay/process_replay.py b/selfdrive/test/process_replay/process_replay.py similarity index 93% rename from selfdrive/test/tests/process_replay/process_replay.py rename to selfdrive/test/process_replay/process_replay.py index b360aed972..1676daa142 100755 --- a/selfdrive/test/tests/process_replay/process_replay.py +++ b/selfdrive/test/process_replay/process_replay.py @@ -1,7 +1,8 @@ -#!/usr/bin/env python2 +#!/usr/bin/env python3 import os import threading import importlib +import shutil import zmq if "CI" in os.environ: @@ -111,7 +112,7 @@ class FakePubMaster(messaging.PubMaster): def send(self, s, dat): self.last_updated = s - if isinstance(dat, str): + if isinstance(dat, bytes): self.data[s] = log.Event.from_bytes(dat) else: self.data[s] = dat.as_reader() @@ -127,11 +128,11 @@ class FakePubMaster(messaging.PubMaster): return dat def fingerprint(msgs, fsm, can_sock): - print "start fingerprinting" + print("start fingerprinting") fsm.wait_on_getitem = True # populate fake socket with data for fingerprinting - canmsgs = filter(lambda msg: msg.which() == "can", msgs) + canmsgs = [msg for msg in msgs if msg.which() == "can"] can_sock.recv_called.wait() can_sock.recv_called.clear() can_sock.data = [msg.as_builder().to_bytes() for msg in canmsgs[:300]] @@ -147,13 +148,13 @@ def fingerprint(msgs, fsm, can_sock): can_sock.data = [] fsm.update_ready.set() - print "finished fingerprinting" + print("finished fingerprinting") def get_car_params(msgs, fsm, can_sock): can = FakeSocket(wait=False) sendcan = FakeSocket(wait=False) - canmsgs = filter(lambda msg: msg.which() == 'can', msgs) + canmsgs = [msg for msg in msgs if msg.which() == 'can'] for m in canmsgs[:300]: can.send(m.as_builder().to_bytes()) _, CP = get_car(can, sendcan) @@ -225,15 +226,17 @@ def replay_process(cfg, lr): fsm = FakeSubMaster(pub_sockets) fpm = FakePubMaster(sub_sockets) args = (fsm, fpm) - if 'can' in cfg.pub_sub.keys(): + if 'can' in list(cfg.pub_sub.keys()): can_sock = FakeSocket() args = (fsm, fpm, can_sock) all_msgs = sorted(lr, key=lambda msg: msg.logMonoTime) - pub_msgs = filter(lambda msg: msg.which() in cfg.pub_sub.keys(), all_msgs) + pub_msgs = [msg for msg in all_msgs if msg.which() in list(cfg.pub_sub.keys())] + shutil.rmtree('/data/params', ignore_errors=True) params = Params() params.manager_start() + params.put("OpenpilotEnabledToggle", "1") params.put("Passive", "0") os.environ['NO_RADAR_SLEEP'] = "1" @@ -244,14 +247,14 @@ def replay_process(cfg, lr): thread.start() if cfg.init_callback is not None: - if 'can' not in cfg.pub_sub.keys(): + if 'can' not in list(cfg.pub_sub.keys()): can_sock = None cfg.init_callback(all_msgs, fsm, can_sock) CP = car.CarParams.from_bytes(params.get("CarParams", block=True)) # wait for started process to be ready - if 'can' in cfg.pub_sub.keys(): + if 'can' in list(cfg.pub_sub.keys()): can_sock.wait_for_recv() else: fsm.wait_for_update() diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit new file mode 100644 index 0000000000..557be7d88d --- /dev/null +++ b/selfdrive/test/process_replay/ref_commit @@ -0,0 +1 @@ +8def7e7391802f2e86498d764c953f487361f6a1 \ No newline at end of file diff --git a/selfdrive/test/tests/process_replay/test_processes.py b/selfdrive/test/process_replay/test_processes.py similarity index 78% rename from selfdrive/test/tests/process_replay/test_processes.py rename to selfdrive/test/process_replay/test_processes.py index 8d70c80a2b..6832aba33c 100755 --- a/selfdrive/test/tests/process_replay/test_processes.py +++ b/selfdrive/test/process_replay/test_processes.py @@ -1,11 +1,11 @@ -#!/usr/bin/env python2 +#!/usr/bin/env python3 import os import requests import sys import tempfile -from selfdrive.test.tests.process_replay.compare_logs import compare_logs -from selfdrive.test.tests.process_replay.process_replay import replay_process, CONFIGS +from selfdrive.test.process_replay.compare_logs import compare_logs +from selfdrive.test.process_replay.process_replay import replay_process, CONFIGS from tools.lib.logreader import LogReader segments = [ @@ -36,22 +36,22 @@ if __name__ == "__main__": ref_commit_fn = os.path.join(process_replay_dir, "ref_commit") if not os.path.isfile(ref_commit_fn): - print "couldn't find reference commit" + print("couldn't find reference commit") sys.exit(1) ref_commit = open(ref_commit_fn).read().strip() - print "***** testing against commit %s *****" % ref_commit + print("***** testing against commit %s *****" % ref_commit) results = {} for segment in segments: - print "***** testing route segment %s *****\n" % segment + print("***** testing route segment %s *****\n" % segment) results[segment] = {} rlog_fn = get_segment(segment) if rlog_fn is None: - print "failed to get segment %s" % segment + print("failed to get segment %s" % segment) sys.exit(1) lr = LogReader(rlog_fn) @@ -84,16 +84,16 @@ if __name__ == "__main__": with open(os.path.join(process_replay_dir, "diff.txt"), "w") as f: f.write("***** tested against commit %s *****\n" % ref_commit) - for segment, result in results.items(): + for segment, result in list(results.items()): f.write("***** differences for segment %s *****\n" % segment) - print "***** results for segment %s *****" % segment + print("***** results for segment %s *****" % segment) - for proc, diff in result.items(): + for proc, diff in list(result.items()): f.write("*** process: %s ***\n" % proc) - print "\t%s" % proc + print("\t%s" % proc) if isinstance(diff, str): - print "\t\t%s" % diff + print("\t\t%s" % diff) failed = True elif len(diff): cnt = {} @@ -104,15 +104,15 @@ if __name__ == "__main__": cnt[k] = 1 if k not in cnt else cnt[k] + 1 for k, v in sorted(cnt.items()): - print "\t\t%s: %s" % (k, v) + print("\t\t%s: %s" % (k, v)) failed = True if failed: - print "TEST FAILED" + print("TEST FAILED") else: - print "TEST SUCCEEDED" + print("TEST SUCCEEDED") - print "\n\nTo update the reference logs for this test run:" - print "./update_refs.py" + print("\n\nTo update the reference logs for this test run:") + print("./update_refs.py") sys.exit(int(failed)) diff --git a/selfdrive/test/tests/process_replay/update_refs.py b/selfdrive/test/process_replay/update_refs.py similarity index 73% rename from selfdrive/test/tests/process_replay/update_refs.py rename to selfdrive/test/process_replay/update_refs.py index 4bc2659391..e0da25133e 100755 --- a/selfdrive/test/tests/process_replay/update_refs.py +++ b/selfdrive/test/process_replay/update_refs.py @@ -1,11 +1,11 @@ -#!/usr/bin/env python2 +#!/usr/bin/env python3 import os import sys from selfdrive.test.openpilotci_upload import upload_file -from selfdrive.test.tests.process_replay.compare_logs import save_log -from selfdrive.test.tests.process_replay.process_replay import replay_process, CONFIGS -from selfdrive.test.tests.process_replay.test_processes import segments, get_segment +from selfdrive.test.process_replay.compare_logs import save_log +from selfdrive.test.process_replay.process_replay import replay_process, CONFIGS +from selfdrive.test.process_replay.test_processes import segments, get_segment from selfdrive.version import get_git_commit from tools.lib.logreader import LogReader @@ -24,7 +24,7 @@ if __name__ == "__main__": rlog_fn = get_segment(segment) if rlog_fn is None: - print "failed to get segment %s" % segment + print("failed to get segment %s" % segment) sys.exit(1) lr = LogReader(rlog_fn) @@ -39,4 +39,4 @@ if __name__ == "__main__": os.remove(log_fn) os.remove(rlog_fn) - print "done" + print("done") diff --git a/selfdrive/test/test_car_models.py b/selfdrive/test/test_car_models.py index 317f325918..fc4605f94a 100755 --- a/selfdrive/test/test_car_models.py +++ b/selfdrive/test/test_car_models.py @@ -1,4 +1,4 @@ -#!/usr/bin/env python2 +#!/usr/bin/env python3 import shutil import time import zmq @@ -14,7 +14,7 @@ from selfdrive.services import service_list import selfdrive.messaging as messaging from common.params import Params from common.basedir import BASEDIR -from common.fingerprints import all_known_cars +from selfdrive.car.fingerprints import all_known_cars from selfdrive.car.honda.values import CAR as HONDA from selfdrive.car.toyota.values import CAR as TOYOTA from selfdrive.car.gm.values import CAR as GM @@ -41,6 +41,8 @@ def wait_for_socket(name, timeout=10.0): except zmq.error.Again: pass time.sleep(0.5) + if r is None: + sys.exit(-1) return r def get_route_logs(route_name): @@ -52,10 +54,10 @@ def get_route_logs(route_name): r = requests.get(log_url) if r.status_code == 200: - with open(log_path, "w") as f: + with open(log_path, "wb") as f: f.write(r.content) else: - print "failed to download test log %s" % route_name + print("failed to download test log %s" % route_name) sys.exit(-1) routes = { @@ -435,7 +437,7 @@ if __name__ == "__main__": tested_cars = [keys["carFingerprint"] for route, keys in routes.items()] for car_model in all_known_cars(): if car_model not in tested_cars: - print "***** WARNING: %s not tested *****" % car_model + print("***** WARNING: %s not tested *****" % car_model) results = {} for route, checks in routes.items(): @@ -454,26 +456,26 @@ if __name__ == "__main__": else: params.put("Passive", "0") - print "testing ", route, " ", checks['carFingerprint'] - print "Preparing processes" + 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" + print("Starting processes") manager.start_managed_process("radard") manager.start_managed_process("controlsd") manager.start_managed_process("plannerd") time.sleep(2) # Start unlogger - print "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', '--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', '--no-interactive'] unlogger = subprocess.Popen(unlogger_cmd, preexec_fn=os.setsid) - print "Check sockets" + 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) @@ -485,7 +487,7 @@ if __name__ == "__main__": carstate_result = wait_for_socket('carState', timeout=30) - print "Check if everything is running" + print("Check if everything is running") running = manager.get_running() controlsd_running = running['controlsd'].is_alive() radard_running = running['radard'].is_alive() @@ -526,19 +528,20 @@ if __name__ == "__main__": params_ok = False if sockets_ok and params_ok: - print "Success" + print("Success") results[route] = True, failures break else: - print "Failure" + print("Failure") results[route] = False, failures time.sleep(2) - print results + for route in results: + print(results[route]) params.put("Passive", "0") # put back not passive to not leave the params in an unintended state if not all(passed for passed, _ in results.values()): - print "TEST FAILED" + print("TEST FAILED") sys.exit(1) else: - print "TEST SUCESSFUL" + print("TEST SUCESSFUL") diff --git a/selfdrive/test/test_fingerprints.py b/selfdrive/test/test_fingerprints.py index 721ea163fc..b291ff6a1c 100755 --- a/selfdrive/test/test_fingerprints.py +++ b/selfdrive/test/test_fingerprints.py @@ -1,4 +1,4 @@ -#!/usr/bin/env python2 +#!/usr/bin/env python3 import os import sys from common.basedir import BASEDIR diff --git a/selfdrive/test/test_openpilot.py b/selfdrive/test/test_openpilot.py index 13770f50a5..0176a7fcd0 100644 --- a/selfdrive/test/test_openpilot.py +++ b/selfdrive/test/test_openpilot.py @@ -43,7 +43,7 @@ def with_processes(processes): #@phone_only #@with_processes(['controlsd', 'radard']) #def test_controls(): -# from selfdrive.test.plant.plant import Plant +# from selfdrive.test.longitudinal_maneuvers.plant import Plant # # # start the fake car for 2 seconds # plant = Plant(100) diff --git a/selfdrive/test/tests/plant/__init__.py b/selfdrive/test/tests/plant/__init__.py deleted file mode 100644 index e69de29bb2..0000000000 diff --git a/selfdrive/test/tests/process_replay/__init__.py b/selfdrive/test/tests/process_replay/__init__.py deleted file mode 100644 index e69de29bb2..0000000000 diff --git a/selfdrive/test/tests/process_replay/ref_commit b/selfdrive/test/tests/process_replay/ref_commit deleted file mode 100644 index 94457fe328..0000000000 --- a/selfdrive/test/tests/process_replay/ref_commit +++ /dev/null @@ -1 +0,0 @@ -9124fa5837ead9b74d97aef85897478a5e8b92d2 \ No newline at end of file diff --git a/selfdrive/thermald.py b/selfdrive/thermald.py index 6a007558aa..a0548beb60 100755 --- a/selfdrive/thermald.py +++ b/selfdrive/thermald.py @@ -1,19 +1,29 @@ -#!/usr/bin/env python2.7 +#!/usr/bin/env python3.7 import os +import json +import copy +import datetime from smbus2 import SMBus from cereal import log +from common.basedir import BASEDIR +from common.params import Params +from common.realtime import sec_since_boot, DT_TRML +from common.numpy_fast import clip +from common.filter_simple import FirstOrderFilter from selfdrive.version import terms_version, training_version from selfdrive.swaglog import cloudlog import selfdrive.messaging as messaging from selfdrive.services import service_list from selfdrive.loggerd.config import get_available_percent -from common.params import Params -from common.realtime import sec_since_boot, DT_TRML -from common.numpy_fast import clip -from common.filter_simple import FirstOrderFilter ThermalStatus = log.ThermalData.ThermalStatus CURRENT_TAU = 15. # 15s time constant +DAYS_NO_CONNECTIVITY_MAX = 7 # do not allow to engage after a week without internet +DAYS_NO_CONNECTIVITY_PROMPT = 4 # send an offroad prompt after 4 days with no internet + + +with open(BASEDIR + "/selfdrive/controls/lib/alerts_offroad.json") as json_file: + OFFROAD_ALERTS = json.load(json_file) def read_tz(x): with open("/sys/devices/virtual/thermal/thermal_zone%d/temp" % x) as f: @@ -99,27 +109,11 @@ def handle_fan(max_cpu_temp, bat_temp, fan_speed): # no max fan speed unless battery is hot fan_speed = min(fan_speed, _FAN_SPEEDS[-2]) - set_eon_fan(fan_speed/16384) + set_eon_fan(fan_speed//16384) return fan_speed -def check_car_battery_voltage(should_start, health, charging_disabled): - - # charging disallowed if: - # - there are health packets from panda, and; - # - 12V battery voltage is too low, and; - # - onroad isn't started - if charging_disabled and (health is None or health.health.voltage > 11800): - charging_disabled = False - os.system('echo "1" > /sys/class/power_supply/battery/charging_enabled') - elif not charging_disabled and health is not None and health.health.voltage < 11500 and not should_start: - charging_disabled = True - os.system('echo "0" > /sys/class/power_supply/battery/charging_enabled') - - return charging_disabled - - def thermald_thread(): setup_eon_fan() @@ -138,13 +132,13 @@ def thermald_thread(): ignition_seen = False started_seen = False thermal_status = ThermalStatus.green + thermal_status_prev = ThermalStatus.green + usb_power = True + usb_power_prev = True health_sock.RCVTIMEO = int(1000 * 2 * DT_TRML) # 2x the expected health frequency current_filter = FirstOrderFilter(0., CURRENT_TAU, DT_TRML) health_prev = None - - # Make sure charging is enabled - charging_disabled = False - os.system('echo "1" > /sys/class/power_supply/battery/charging_enabled') + current_connectivity_alert = None params = Params() @@ -160,6 +154,9 @@ def thermald_thread(): ignition_seen = False health_prev = health + if health is not None: + usb_power = health.health.usbPowerMode != log.HealthData.UsbPowerMode.client + # loggerd is gated based on free space avail = get_available_percent() / 100.0 @@ -208,6 +205,32 @@ def thermald_thread(): # **** starting logic **** + # Check for last update time and display alerts if needed + now = datetime.datetime.now() + try: + last_update = datetime.datetime.fromisoformat(params.get("LastUpdateTime", encoding='utf8')) + except (TypeError, ValueError): + last_update = now + dt = now - last_update + + if dt.days > DAYS_NO_CONNECTIVITY_MAX: + if current_connectivity_alert != "expired": + current_connectivity_alert = "expired" + params.delete("Offroad_ConnectivityNeededPrompt") + params.put("Offroad_ConnectivityNeeded", json.dumps(OFFROAD_ALERTS["Offroad_ConnectivityNeeded"])) + elif dt.days > DAYS_NO_CONNECTIVITY_PROMPT: + remaining_time = str(DAYS_NO_CONNECTIVITY_MAX - dt.days) + if current_connectivity_alert != "prompt" + remaining_time: + current_connectivity_alert = "prompt" + remaining_time + alert_connectivity_prompt = copy.copy(OFFROAD_ALERTS["Offroad_ConnectivityNeededPrompt"]) + alert_connectivity_prompt["text"] += remaining_time + " days." + params.delete("Offroad_ConnectivityNeeded") + params.put("Offroad_ConnectivityNeededPrompt", json.dumps(alert_connectivity_prompt)) + elif current_connectivity_alert is not None: + current_connectivity_alert = None + params.delete("Offroad_ConnectivityNeeded") + params.delete("Offroad_ConnectivityNeededPrompt") + # start constellation of processes when the car starts ignition = health is not None and health.health.started ignition_seen = ignition_seen or ignition @@ -216,7 +239,7 @@ def thermald_thread(): if not ignition_seen and health is not None and health.health.voltage > 13500: ignition = True - do_uninstall = params.get("DoUninstall") == "1" + do_uninstall = params.get("DoUninstall") == b"1" accepted_terms = params.get("HasAcceptedTerms") == terms_version completed_training = params.get("CompletedTrainingVersion") == training_version @@ -234,8 +257,12 @@ def thermald_thread(): # if any CPU gets above 107 or the battery gets above 63, kill all processes # controls will warn with CPU above 95 or battery above 60 if thermal_status >= ThermalStatus.danger: - # TODO: Add a better warning when this is happening should_start = False + if thermal_status_prev < ThermalStatus.danger: + params.put("Offroad_TemperatureTooHigh", json.dumps(OFFROAD_ALERTS["Offroad_TemperatureTooHigh"])) + else: + if thermal_status_prev >= ThermalStatus.danger: + params.delete("Offroad_TemperatureTooHigh") if should_start: off_ts = None @@ -255,15 +282,21 @@ def thermald_thread(): started_seen and (sec_since_boot() - off_ts) > 60: os.system('LD_LIBRARY_PATH="" svc power shutdown') - #charging_disabled = check_car_battery_voltage(should_start, health, charging_disabled) - - msg.thermal.chargingDisabled = charging_disabled msg.thermal.chargingError = current_filter.x > 0. and msg.thermal.batteryPercent < 90 # if current is positive, then battery is being discharged msg.thermal.started = started_ts is not None msg.thermal.startedTs = int(1e9*(started_ts or 0)) msg.thermal.thermalStatus = thermal_status thermal_sock.send(msg.to_bytes()) + + if usb_power_prev and not usb_power: + params.put("Offroad_ChargeDisabled", json.dumps(OFFROAD_ALERTS["Offroad_ChargeDisabled"])) + elif usb_power and not usb_power_prev: + params.delete("Offroad_ChargeDisabled") + + thermal_status_prev = thermal_status + usb_power_prev = usb_power + print(msg) # report to server once per minute diff --git a/selfdrive/tombstoned.py b/selfdrive/tombstoned.py index 3af76818d4..292ec0408b 100644 --- a/selfdrive/tombstoned.py +++ b/selfdrive/tombstoned.py @@ -16,7 +16,7 @@ def get_tombstones(): def report_tombstone(fn, client): mtime = os.path.getmtime(fn) - with open(fn, "r") as f: + with open(fn, encoding='ISO-8859-1') as f: dat = f.read() # see system/core/debuggerd/tombstone.cpp diff --git a/selfdrive/ui/spinner/spinner b/selfdrive/ui/spinner/spinner index f11662e019..61da4d1c1c 100755 --- a/selfdrive/ui/spinner/spinner +++ b/selfdrive/ui/spinner/spinner @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:ca4dc718c959f0c5dc3d99bc8844ebe9c4366e7ab182144ad62bd7576b35b104 -size 518368 +oid sha256:619b54e73085e3f4ff3ba922765eff1c175f57336c8af7ba84f96d9a0328e979 +size 522464 diff --git a/selfdrive/ui/start.py b/selfdrive/ui/start.py index e3e629b545..a1d00ce145 100755 --- a/selfdrive/ui/start.py +++ b/selfdrive/ui/start.py @@ -1,4 +1,4 @@ -#!/usr/bin/env python2 +#!/usr/bin/env python3 import os assert os.system("make") == 0 diff --git a/selfdrive/ui/ui.c b/selfdrive/ui/ui.c index 1c88d21923..d09061ea63 100644 --- a/selfdrive/ui/ui.c +++ b/selfdrive/ui/ui.c @@ -331,7 +331,7 @@ static void set_volume(UIState *s, int volume) { int volume_changed = system(volume_change_cmd); } -volatile int do_exit = 0; +volatile sig_atomic_t do_exit = 0; static void set_do_exit(int sig) { do_exit = 1; } @@ -443,7 +443,7 @@ sound_file sound_table[] = { { 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_2.wav", true }, + { 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 }, @@ -2192,8 +2192,8 @@ int main(int argc, char* argv[]) { float smooth_brightness = BRIGHTNESS_B; - const int MIN_VOLUME = LEON ? 12 : 8; - const int MAX_VOLUME = LEON ? 15 : 13; + const int MIN_VOLUME = LEON ? 12 : 9; + const int MAX_VOLUME = LEON ? 15 : 12; set_volume(s, MIN_VOLUME); #ifdef DEBUG_FPS diff --git a/selfdrive/updated.py b/selfdrive/updated.py index 743eb6f589..d05a9a8fa7 100755 --- a/selfdrive/updated.py +++ b/selfdrive/updated.py @@ -1,13 +1,18 @@ -#!/usr/bin/env python +#!/usr/bin/env python3 # simple service that waits for network access and tries to update every hour -import time +import datetime import subprocess +import time + +from common.params import Params from selfdrive.swaglog import cloudlog NICE_LOW_PRIORITY = ["nice", "-n", "19"] def main(gctx=None): + params = Params() + while True: # try network ping_failed = subprocess.call(["ping", "-W", "4", "-c", "1", "8.8.8.8"]) @@ -17,7 +22,7 @@ def main(gctx=None): # download application update try: - r = subprocess.check_output(NICE_LOW_PRIORITY + ["git", "fetch"], stderr=subprocess.STDOUT) + 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, @@ -27,8 +32,26 @@ def main(gctx=None): continue cloudlog.info("git fetch success: %s", r) + # 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") + + # 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", "") + + t = datetime.datetime.now().isoformat() + params.put("LastUpdateTime", t.encode('utf8')) + time.sleep(60*60) if __name__ == "__main__": main() - diff --git a/selfdrive/version.py b/selfdrive/version.py index 2eb39dc976..dd78fa4e0b 100644 --- a/selfdrive/version.py +++ b/selfdrive/version.py @@ -1,28 +1,29 @@ +#!/usr/bin/env python3 import os import subprocess from selfdrive.swaglog import cloudlog def get_git_commit(): - return subprocess.check_output(["git", "rev-parse", "HEAD"]).strip() + return subprocess.check_output(["git", "rev-parse", "HEAD"], encoding='utf8').strip() # pylint: disable=unexpected-keyword-arg def get_git_branch(): - return subprocess.check_output(["git", "rev-parse", "--abbrev-ref", "HEAD"]).strip() + return subprocess.check_output(["git", "rev-parse", "--abbrev-ref", "HEAD"], encoding='utf8').strip() # pylint: disable=unexpected-keyword-arg def get_git_full_branchname(): - return subprocess.check_output(["git", "rev-parse", "--abbrev-ref", "--symbolic-full-name", "@{u}"]).strip() + return subprocess.check_output(["git", "rev-parse", "--abbrev-ref", "--symbolic-full-name", "@{u}"], encoding='utf8').strip() # pylint: disable=unexpected-keyword-arg def get_git_remote(): try: - local_branch = subprocess.check_output(["git", "name-rev", "--name-only", "HEAD"]).strip() - tracking_remote = subprocess.check_output(["git", "config", "branch." + local_branch + ".remote"]).strip() - return subprocess.check_output(["git", "config", "remote." + tracking_remote + ".url"]).strip() + local_branch = subprocess.check_output(["git", "name-rev", "--name-only", "HEAD"], encoding='utf8').strip() # pylint: disable=unexpected-keyword-arg + tracking_remote = subprocess.check_output(["git", "config", "branch." + local_branch + ".remote"], encoding='utf8').strip() # pylint: disable=unexpected-keyword-arg + return subprocess.check_output(["git", "config", "remote." + tracking_remote + ".url"], encoding='utf8').strip() # pylint: disable=unexpected-keyword-arg except subprocess.CalledProcessError: # Not on a branch, fallback - return subprocess.check_output(["git", "config", "--get", "remote.origin.url"]).strip() + return subprocess.check_output(["git", "config", "--get", "remote.origin.url"], encoding='utf8').strip() # pylint: disable=unexpected-keyword-arg with open(os.path.join(os.path.dirname(os.path.abspath(__file__)), "common", "version.h")) as _versionf: @@ -44,9 +45,9 @@ try: dirty = subprocess.call(["git", "diff-index", "--quiet", branch, "--"]) != 0 if dirty: - dirty_files = subprocess.check_output(["git", "diff-index", branch, "--"]) - commit = subprocess.check_output(["git", "rev-parse", "--verify", "HEAD"]).rstrip() - origin_commit = subprocess.check_output(["git", "rev-parse", "--verify", branch]).rstrip() + dirty_files = subprocess.check_output(["git", "diff-index", branch, "--"], encoding='utf8') # pylint: disable=unexpected-keyword-arg + commit = subprocess.check_output(["git", "rev-parse", "--verify", "HEAD"], encoding='utf8').rstrip() # pylint: disable=unexpected-keyword-arg + origin_commit = subprocess.check_output(["git", "rev-parse", "--verify", branch], encoding='utf8').rstrip() # pylint: disable=unexpected-keyword-arg cloudlog.event("dirty comma branch", vesion=version, dirty=dirty, origin=origin, branch=branch, dirty_files=dirty_files, commit=commit, origin_commit=origin_commit) else: @@ -58,8 +59,8 @@ except subprocess.CalledProcessError: pass dirty = True -training_version = "0.1.0" -terms_version = "2" +training_version = b"0.1.0" +terms_version = b"2" if __name__ == "__main__": print("Dirty: %s" % dirty) diff --git a/selfdrive/visiond/cameras/camera_frame_stream.cc b/selfdrive/visiond/cameras/camera_frame_stream.cc index 81b2bd4008..ae3c8b14ca 100644 --- a/selfdrive/visiond/cameras/camera_frame_stream.cc +++ b/selfdrive/visiond/cameras/camera_frame_stream.cc @@ -19,7 +19,7 @@ extern "C" { #include } -extern volatile int do_exit; +extern volatile sig_atomic_t do_exit; #define FRAME_WIDTH 1164 #define FRAME_HEIGHT 874 diff --git a/selfdrive/visiond/cameras/camera_qcom.c b/selfdrive/visiond/cameras/camera_qcom.c index ccaee8575f..39fd189b5a 100644 --- a/selfdrive/visiond/cameras/camera_qcom.c +++ b/selfdrive/visiond/cameras/camera_qcom.c @@ -44,7 +44,7 @@ typedef struct CameraMsg { float grey_frac; } CameraMsg; -extern volatile int do_exit; +extern volatile sig_atomic_t do_exit; CameraInfo cameras_supported[CAMERA_ID_MAX] = { [CAMERA_ID_IMX298] = { @@ -368,21 +368,6 @@ static void set_exposure(CameraState *s, float exposure_frac, float gain_frac) { integ_lines = min(integ_lines, frame_length-11); } - // done after exposure to not adjust it - if (s->using_pll) { - // can adjust frame length by up to +/- 1 - const int PHASE_DEADZONE = 20000; // 20 us - int phase_max = 1000000000 / s->fps; - int phase_diff = s->phase_actual - s->phase_request; - phase_diff = ((phase_diff + phase_max/2) % phase_max) - phase_max/2; - - if (phase_diff < -PHASE_DEADZONE) { - frame_length += 1; - } else if (phase_diff > PHASE_DEADZONE) { - frame_length -= 1; - } - } - if (gain_frac >= 0) { // ISO200 is minimum gain gain_frac = clamp(gain_frac, 1.0/64, 1.0); @@ -2019,41 +2004,6 @@ static bool acceleration_from_sensor_sock(void* sock, float* vs) { return ret; } -static bool gps_time_from_timing_sock(void* sock, uint64_t *mono_time, double* vs) { - int err; - - zmq_msg_t msg; - err = zmq_msg_init(&msg); - assert(err == 0); - - err = zmq_msg_recv(&msg, sock, 0); - assert(err >= 0); - - struct capn ctx; - capn_init_mem(&ctx, zmq_msg_data(&msg), zmq_msg_size(&msg), 0); - - cereal_Event_ptr eventp; - eventp.p = capn_getp(capn_root(&ctx), 0, 1); - struct cereal_Event eventd; - cereal_read_Event(&eventd, eventp); - - bool ret = false; - - if (eventd.which == cereal_Event_liveLocationTiming) { - struct cereal_LiveLocationData lld; - cereal_read_LiveLocationData(&lld, eventd.liveLocationTiming); - - *mono_time = lld.fixMonoTime; - *vs = lld.timeOfWeek; - ret = true; - } - - capn_free(&ctx); - zmq_msg_close(&msg); - - return ret; -} - static void ops_term() { zsock_t *ops_sock = zsock_new_push(">inproc://cameraops"); assert(ops_sock); @@ -2076,13 +2026,10 @@ static void* ops_thread(void* arg) { zsock_t *sensor_sock = zsock_new_sub(">tcp://127.0.0.1:8003", ""); assert(sensor_sock); - zsock_t *livelocationtiming_sock = zsock_new_sub(">tcp://127.0.0.1:8049", ""); - assert(livelocationtiming_sock); - zsock_t *terminate = zsock_new_sub(">inproc://terminate", ""); assert(terminate); - zpoller_t *poller = zpoller_new(cameraops, sensor_sock, livelocationtiming_sock, terminate, NULL); + zpoller_t *poller = zpoller_new(cameraops, sensor_sock, terminate, NULL); assert(poller); while (!do_exit) { @@ -2130,15 +2077,6 @@ static void* ops_thread(void* arg) { s->rear.last_sag_ts = ts; s->rear.last_sag_acc_z = -vs[2]; } - } else if (which == livelocationtiming_sock) { - uint64_t mono_time; - double gps_time; - if (gps_time_from_timing_sock(sockraw, &mono_time, &gps_time)) { - s->rear.global_time_offset = (uint64_t)(gps_time*1e9) - mono_time; - //LOGW("%f %lld = %lld", gps_time, mono_time, s->rear.global_time_offset); - s->rear.phase_request = 10000000; - s->rear.using_pll = true; - } } } @@ -2228,12 +2166,6 @@ void cameras_run(DualCameraState *s) { // printf("ISP_EVENT_EOF delta %f\n", (t-last_t)/1e6); c->last_t = t; - if (c->using_pll) { - int mod = ((int)1000000000 / c->fps); - c->phase_actual = (((timestamp + c->global_time_offset) % mod) + mod) % mod; - LOGD("phase is %12d request is %12d with offset %lld", c->phase_actual, c->phase_request, c->global_time_offset); - } - pthread_mutex_lock(&c->frame_info_lock); c->frame_metadata[c->frame_metadata_idx] = (FrameMetadata){ .frame_id = isp_event_data->frame_id, @@ -2270,4 +2202,3 @@ void cameras_close(DualCameraState *s) { camera_close(&s->rear); camera_close(&s->front); } - diff --git a/selfdrive/visiond/cameras/camera_qcom.h b/selfdrive/visiond/cameras/camera_qcom.h index 0cc8f844a5..1a9c31baa3 100644 --- a/selfdrive/visiond/cameras/camera_qcom.h +++ b/selfdrive/visiond/cameras/camera_qcom.h @@ -54,13 +54,6 @@ typedef struct CameraState { uint32_t line_length_pclk; unsigned int max_gain; - // PLL to sync cameras in time - // assumes at least 1 FPS - bool using_pll; - int phase_actual; - int phase_request; - int64_t global_time_offset; - int csid_fd; int csiphy_fd; int sensor_fd; diff --git a/selfdrive/debug/getframes/Makefile b/selfdrive/visiond/snapshot/Makefile similarity index 65% rename from selfdrive/debug/getframes/Makefile rename to selfdrive/visiond/snapshot/Makefile index 932673e208..6edde2f822 100644 --- a/selfdrive/debug/getframes/Makefile +++ b/selfdrive/visiond/snapshot/Makefile @@ -18,9 +18,15 @@ visionipc.o: ../../common/visionipc.c ../../common/visionipc.h -I../.. -I../../.. \ -c -o '$@' ../../common/visionipc.c -libvisionipc.so: visionipc.o - $(CC) -shared -fPIC -o '$@' visionipc.o +ipc.o: ../../common/ipc.c ../../common/ipc.h + @echo "[ CC ] $@" + $(CC) $(CFLAGS) -MMD \ + -I../.. -I../../.. \ + -c -o '$@' ../../common/ipc.c + +libvisionipc.so: visionipc.o ipc.o + $(CC) -shared -fPIC -o '$@' visionipc.o ipc.o .PHONY: clean clean: - rm visionipc.o libvisionipc.so + rm visionipc.o ipc.o libvisionipc.so diff --git a/selfdrive/test/tests/__init__.py b/selfdrive/visiond/snapshot/__init__.py similarity index 100% rename from selfdrive/test/tests/__init__.py rename to selfdrive/visiond/snapshot/__init__.py diff --git a/selfdrive/visiond/snapshot/snapshot.py b/selfdrive/visiond/snapshot/snapshot.py new file mode 100755 index 0000000000..cb24209655 --- /dev/null +++ b/selfdrive/visiond/snapshot/snapshot.py @@ -0,0 +1,46 @@ +#!/usr/bin/env python3 +import os +import subprocess +from PIL import Image +import time +import signal + +from selfdrive.visiond.snapshot.visionipc import VisionIPC + +def jpeg_write(fn, dat): + img = Image.fromarray(dat) + img.save(fn, "JPEG") + +def snapshot(): + # note: super sketch race condition if you start the car at the same time at this + # TODO: lock car starting? + ps = subprocess.Popen("ps | grep visiond", shell=True, stdout=subprocess.PIPE) + ret = list(filter(lambda x: 'grep ' not in x, ps.communicate()[0].decode('utf-8').strip().split("\n"))) + if len(ret) > 0: + return None + + proc = subprocess.Popen(os.path.join(os.getenv("HOME"), "one/selfdrive/visiond/visiond"), cwd=os.path.join(os.getenv("HOME"), "one/selfdrive/visiond")) + time.sleep(6.0) + + ret = None + try: + ipc = VisionIPC() + pic = ipc.get() + del ipc + + ipc_front = VisionIPC(front=True) + fpic = ipc_front.get() + del ipc_front + + ret = pic, fpic + finally: + proc.send_signal(signal.SIGINT) + proc.communicate() + + return ret + +if __name__ == "__main__": + pic, fpic = snapshot() + jpeg_write("/tmp/back.jpg", pic) + jpeg_write("/tmp/front.jpg", fpic) + diff --git a/selfdrive/debug/getframes/getframes.py b/selfdrive/visiond/snapshot/visionipc.py old mode 100755 new mode 100644 similarity index 65% rename from selfdrive/debug/getframes/getframes.py rename to selfdrive/visiond/snapshot/visionipc.py index 4964841750..1129025081 --- a/selfdrive/debug/getframes/getframes.py +++ b/selfdrive/visiond/snapshot/visionipc.py @@ -1,4 +1,4 @@ -#!/usr/bin/env python +#!/usr/bin/env python3 import os import subprocess from cffi import FFI @@ -9,7 +9,6 @@ gf_dir = os.path.dirname(os.path.abspath(__file__)) subprocess.check_call(["make"], cwd=gf_dir) - ffi = FFI() ffi.cdef(""" @@ -69,33 +68,19 @@ void visionstream_destroy(VisionStream *s); """ ) -clib = ffi.dlopen(os.path.join(gf_dir, "libvisionipc.so")) - - -def getframes(front=False): - s = ffi.new("VisionStream*") - buf_info = ffi.new("VisionStreamBufs*") +class VisionIPC(): + def __init__(self, front=False): + self.clib = ffi.dlopen(os.path.join(gf_dir, "libvisionipc.so")) - if front: - stream_type = clib.VISION_STREAM_RGB_FRONT - else: - stream_type = clib.VISION_STREAM_RGB_BACK + self.s = ffi.new("VisionStream*") + self.buf_info = ffi.new("VisionStreamBufs*") - err = clib.visionstream_init(s, stream_type, True, buf_info) - assert err == 0 - - w = buf_info.width - h = buf_info.height - assert buf_info.stride == w*3 - assert buf_info.buf_len == w*h*3 - - while True: - buf = clib.visionstream_get(s, ffi.NULL) + err = self.clib.visionstream_init(self.s, self.clib.VISION_STREAM_RGB_FRONT if front else self.clib.VISION_STREAM_RGB_BACK, True, self.buf_info) + assert err == 0 + def get(self): + buf = self.clib.visionstream_get(self.s, ffi.NULL) pbuf = ffi.buffer(buf.addr, buf.len) - yield np.frombuffer(pbuf, dtype=np.uint8).reshape((h, w, 3)) - + ret = np.frombuffer(pbuf, dtype=np.uint8).reshape((-1, self.buf_info.stride//3, 3)) + return ret[:self.buf_info.height, :self.buf_info.width, [2,1,0]] -if __name__ == "__main__": - for buf in getframes(): - print("{0} {1}".format(buf.shape, buf[101, 101])) diff --git a/selfdrive/visiond/visiond.cc b/selfdrive/visiond/visiond.cc index 53d1f82777..62682918c0 100644 --- a/selfdrive/visiond/visiond.cc +++ b/selfdrive/visiond/visiond.cc @@ -80,7 +80,7 @@ typedef void (*sighandler_t) (int); #endif extern "C" { -volatile int do_exit = 0; +volatile sig_atomic_t do_exit = 0; } namespace { @@ -738,8 +738,8 @@ void* monitoring_thread(void *arg) { } else // use default setting if no face { - s->front_meteringbox_ymin = s->rgb_front_height * 0; - s->front_meteringbox_ymax = s->rgb_front_height * 2 / 3; + s->front_meteringbox_ymin = s->rgb_front_height * 1 / 3; + s->front_meteringbox_ymax = s->rgb_front_height * 1; s->front_meteringbox_xmin = s->rgb_front_width * 3 / 5; s->front_meteringbox_xmax = s->rgb_front_width; } @@ -820,8 +820,8 @@ void* frontview_thread(void *arg) { } else { - y_start = s->rgb_front_height * 0; - y_end = s->rgb_front_height * 2 / 3; + y_start = s->rgb_front_height * 1 / 3; + y_end = s->rgb_front_height * 1; x_start = s->rgb_front_width * 3 / 5; x_end = s->rgb_front_width; } @@ -1301,7 +1301,7 @@ void set_do_exit(int sig) { do_exit = 1; } -void party(VisionState *s, bool nomodel) { +void party(VisionState *s) { int err; s->terminate_pub = zsock_new_pub("@inproc://terminate"); @@ -1390,11 +1390,6 @@ int main(int argc, char **argv) { test_run = true; } - bool no_model = false; - if (argc > 1 && strcmp(argv[1], "--no-model") == 0) { - no_model = true; - } - VisionState state = {0}; VisionState *s = &state; @@ -1443,7 +1438,7 @@ int main(int argc, char **argv) { if (test_run) { do_exit = true; } - party(s, no_model); + party(s); zsock_destroy(&s->recorder_sock); zsock_destroy(&s->monitoring_sock);