diff --git a/.github/workflows/selfdrive_tests.yaml b/.github/workflows/selfdrive_tests.yaml
index 03dd4f4686..752a5ec357 100644
--- a/.github/workflows/selfdrive_tests.yaml
+++ b/.github/workflows/selfdrive_tests.yaml
@@ -69,6 +69,19 @@ jobs:
rm -rf /tmp/scons_cache/* && \
scons -j$(nproc) --cache-populate"
+ build_all:
+ name: build all
+ runs-on: ubuntu-20.04
+ timeout-minutes: 50
+ steps:
+ - uses: actions/checkout@v2
+ with:
+ submodules: true
+ - name: Build Docker image
+ run: eval "$BUILD"
+ - name: Build openpilot with all flags
+ run: ${{ env.RUN }} "scons -j$(nproc) --extras --test"
+
#build_mac:
# name: build macos
# runs-on: macos-latest
@@ -257,6 +270,7 @@ jobs:
$UNIT_TEST selfdrive/athena && \
$UNIT_TEST selfdrive/thermald && \
$UNIT_TEST selfdrive/hardware/tici && \
+ $UNIT_TEST selfdrive/modeld && \
$UNIT_TEST tools/lib/tests && \
./selfdrive/boardd/tests/test_boardd_usbprotocol && \
./selfdrive/common/tests/test_util && \
diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml
index 3fc5596568..dd53a4200c 100644
--- a/.pre-commit-config.yaml
+++ b/.pre-commit-config.yaml
@@ -22,6 +22,8 @@ repos:
exclude: '^(pyextra/)|(cereal/)|(rednose/)|(panda/)|(laika/)|(opendbc/)|(laika_repo/)|(rednose_repo/)/'
additional_dependencies: ['git+https://github.com/numpy/numpy-stubs', 'types-requests', 'types-atomicwrites',
'types-pycurl']
+ args:
+ - --warn-unreachable
- repo: https://github.com/PyCQA/flake8
rev: 4.0.1
hooks:
diff --git a/Dockerfile.openpilot_base b/Dockerfile.openpilot_base
index 2ea26b854f..c5a024db82 100644
--- a/Dockerfile.openpilot_base
+++ b/Dockerfile.openpilot_base
@@ -25,8 +25,9 @@ RUN cd /tmp && \
rm -rf /tmp/* && \
rm -rf /root/.cache && \
pip uninstall -y pipenv && \
-
# remove unused architectures from gcc for panda
cd /usr/lib/gcc/arm-none-eabi/9.2.1 && \
rm -rf arm/ && \
rm -rf thumb/nofp thumb/v6* thumb/v8* thumb/v7+fp thumb/v7-r+fp.sp
+RUN sudo git config --global --add safe.directory /tmp/openpilot
+
diff --git a/Jenkinsfile b/Jenkinsfile
index fcfc17a86f..b188539b14 100644
--- a/Jenkinsfile
+++ b/Jenkinsfile
@@ -18,18 +18,6 @@ fi
ln -snf ${env.TEST_DIR} /data/pythonpath
-if [ -f /EON ]; then
- # kill all old procs in the openpilot cpuset
- while read p; do
- kill "\$p" || true
- done < /dev/cpuset/app/tasks
-
- echo \$\$ > /dev/cpuset/app/tasks || true
-
- mkdir -p /dev/shm
- chmod 777 /dev/shm
-fi
-
cd ${env.TEST_DIR} || true
${cmd}
exit 0
@@ -62,29 +50,15 @@ pipeline {
}
stages {
- stage('build releases') {
+ stage('build release3') {
+ agent { docker { image 'ghcr.io/commaai/alpine-ssh'; args '--user=root' } }
when {
branch 'devel-staging'
}
-
- parallel {
- stage('release2') {
- agent { docker { image 'ghcr.io/commaai/alpine-ssh'; args '--user=root' } }
- steps {
- phone_steps("eon-build", [
- ["build release2-staging & dashcam-staging", "PUSH=1 $SOURCE_DIR/release/build_release.sh"],
- ])
- }
- }
-
- stage('release3') {
- agent { docker { image 'ghcr.io/commaai/alpine-ssh'; args '--user=root' } }
- steps {
- phone_steps("tici", [
- ["build release3-staging & dashcam3-staging", "PUSH=1 $SOURCE_DIR/release/build_release.sh"],
- ])
- }
- }
+ steps {
+ phone_steps("tici", [
+ ["build release3-staging & dashcam3-staging", "PUSH=1 $SOURCE_DIR/release/build_release.sh"],
+ ])
}
}
@@ -106,41 +80,6 @@ pipeline {
stages {
stage('parallel tests') {
parallel {
- stage('C2: build') {
- steps {
- phone_steps("eon-build", [
- ["build master-ci", "cd $SOURCE_DIR/release && EXTRA_FILES='tools/' ./build_devel.sh"],
- ["build openpilot", "cd selfdrive/manager && ./build.py"],
- ["test manager", "python selfdrive/manager/test/test_manager.py"],
- ["onroad tests", "cd selfdrive/test/ && ./test_onroad.py"],
- ["test car interfaces", "cd selfdrive/car/tests/ && ./test_car_interfaces.py"],
- ])
- }
- }
-
- stage('C2: replay') {
- steps {
- phone_steps("eon2", [
- ["build", "cd selfdrive/manager && ./build.py"],
- ["model replay", "cd selfdrive/test/process_replay && ./model_replay.py"],
- ])
- }
- }
-
- stage('C2: HW + Unit Tests') {
- steps {
- phone_steps("eon", [
- ["build", "cd selfdrive/manager && ./build.py"],
- ["test sounds", "python selfdrive/ui/tests/test_soundd.py"],
- ["test boardd loopback", "python selfdrive/boardd/tests/test_boardd_loopback.py"],
- ["test loggerd", "python selfdrive/loggerd/tests/test_loggerd.py"],
- ["test encoder", "python selfdrive/loggerd/tests/test_encoder.py"],
- ["test logcatd", "python selfdrive/logcatd/tests/test_logcatd_android.py"],
- ["test updater", "python selfdrive/hardware/eon/test_neos_updater.py"],
- ])
- }
- }
-
/*
stage('Power Consumption Tests') {
steps {
@@ -162,7 +101,7 @@ pipeline {
}
*/
- stage('C3: build') {
+ stage('build') {
environment {
R3_PUSH = "${env.BRANCH_NAME == 'master' ? '1' : ' '}"
}
@@ -177,7 +116,7 @@ pipeline {
}
}
- stage('C3: HW + Unit Tests') {
+ stage('HW + Unit Tests') {
steps {
phone_steps("tici2", [
["build", "cd selfdrive/manager && ./build.py"],
@@ -188,17 +127,7 @@ pipeline {
}
}
- stage('C2: camerad') {
- steps {
- phone_steps("eon-party", [
- ["build", "cd selfdrive/manager && ./build.py"],
- ["test camerad", "python selfdrive/camerad/test/test_camerad.py"],
- ["test exposure", "python selfdrive/camerad/test/test_exposure.py"],
- ])
- }
- }
-
- stage('C3: camerad') {
+ stage('camerad') {
steps {
phone_steps("tici-party", [
["build", "cd selfdrive/manager && ./build.py"],
@@ -208,7 +137,7 @@ pipeline {
}
}
- stage('C3: replay') {
+ stage('replay') {
steps {
phone_steps("tici3", [
["build", "cd selfdrive/manager && ./build.py"],
@@ -225,7 +154,7 @@ pipeline {
branch 'master'
}
steps {
- phone_steps("eon-build", [
+ phone_steps("tici-build", [
["push devel", "cd $SOURCE_DIR/release && PUSH='master-ci' ./build_devel.sh"],
])
}
diff --git a/Pipfile b/Pipfile
index 02e8c602d1..6d30179b9d 100644
--- a/Pipfile
+++ b/Pipfile
@@ -37,6 +37,7 @@ breathe = "*"
subprocess32 = "*"
tenacity = "*"
mpld3 = "*"
+carla = "==0.9.12"
[packages]
atomicwrites = "*"
diff --git a/Pipfile.lock b/Pipfile.lock
index da2dae92aa..2c62e13dfa 100644
--- a/Pipfile.lock
+++ b/Pipfile.lock
@@ -1,7 +1,7 @@
{
"_meta": {
"hash": {
- "sha256": "7288746fb2afc988e4de2a7d1d3bc2fbef09ce65ca135b6762f3d722c156661b"
+ "sha256": "41e285b10b9b5a353b3eb9c202886e52d22403c369784877aaa35e099aa203a8"
},
"pipfile-spec": 6,
"requires": {
@@ -1143,6 +1143,19 @@
"index": "pypi",
"version": "==4.33.1"
},
+ "carla": {
+ "hashes": [
+ "sha256:1ed11b56c781cd15cbd540cacfb59ad348e0a021d898cfd0ff89a585f144da0b",
+ "sha256:20c1e1b72034175824d89b2d86b09ae72b4aca09ea25874dc6251f239297251d",
+ "sha256:6d1122c24af4f6375dc6858fbb0309b61c219b101d8c5a540def4c36c4563fe1",
+ "sha256:9c19ebf6cbbc535bde4baf9e18c72ab349657b34c4202b9751541e4c0d20b3cc",
+ "sha256:a69f6d84b59e2f805b2a417de98f977fe9efe0cfa733da8d75e20d28892da915",
+ "sha256:c3ae0dce3f1354b6311fee21a365947b0ff169249993a913904f676046d2d69f",
+ "sha256:dd392a267e14b785a8f65dafef86e05a92201253e9fb4a01e1e262834f20bed2"
+ ],
+ "index": "pypi",
+ "version": "==0.9.12"
+ },
"certifi": {
"hashes": [
"sha256:78884e7c1d4b00ce3cea67b44566851c4343c120abd683433ce934a68ea58872",
diff --git a/README.md b/README.md
index b012883d78..5af02de8d9 100755
--- a/README.md
+++ b/README.md
@@ -41,8 +41,8 @@ Running in a car
To use openpilot in a car, you need four things
* This software. It's free and available right here.
* One of [the 150+ supported cars](docs/CARS.md). We support Honda, Toyota, Hyundai, Nissan, Kia, Chrysler, Lexus, Acura, Audi, VW, and more. If your car is not supported, but has adaptive cruise control and lane keeping assist, it's likely able to run openpilot.
-* A supported device to run this software. This can be a [comma two](https://comma.ai/shop/products/two), [comma three](https://comma.ai/shop/products/three), or if you like to experiment, a [Ubuntu computer with webcams](https://github.com/commaai/openpilot/tree/master/tools/webcam).
-* A way to connect to your car. With a comma two or three, you need only a [car harness](https://comma.ai/shop/products/car-harness). With an EON Gold or PC, you also need a [black panda](https://comma.ai/shop/products/panda).
+* A supported device to run this software: a [comma three](https://comma.ai/shop/products/three), or if you like to experiment, a [Ubuntu computer with webcams](https://github.com/commaai/openpilot/tree/master/tools/webcam).
+* A way to connect to your car. With a comma three, you need only a [car harness](https://comma.ai/shop/products/car-harness). With a PC, you also need a [black panda](https://comma.ai/shop/products/panda).
We have detailed instructions for [how to install the device in a car](https://comma.ai/setup).
diff --git a/RELEASES.md b/RELEASES.md
index 304df61cba..8d8ee06d63 100644
--- a/RELEASES.md
+++ b/RELEASES.md
@@ -2,6 +2,7 @@ Version 0.8.14 (2022-0X-XX)
========================
* bigmodel!
* comma body support
+ * Hyundai Ioniq Plug-in Hybrid 2019 support thanks to sunnyhaibin!
* Hyundai Tucson Diesel 2019 support thanks to sunnyhaibin!
* Toyota Alphard Hybrid 2021 support
* Toyota Avalon Hybrid 2022 support
diff --git a/SConstruct b/SConstruct
index 6b520bee41..ec76448235 100644
--- a/SConstruct
+++ b/SConstruct
@@ -73,14 +73,9 @@ lenv = {
rpath = lenv["LD_LIBRARY_PATH"].copy()
-if arch == "aarch64" or arch == "larch64":
+if arch == "larch64":
lenv["LD_LIBRARY_PATH"] += ['/data/data/com.termux/files/usr/lib']
- if arch == "aarch64":
- # android
- lenv["ANDROID_DATA"] = os.environ['ANDROID_DATA']
- lenv["ANDROID_ROOT"] = os.environ['ANDROID_ROOT']
-
cpppath = [
"#third_party/opencl/include",
]
@@ -92,27 +87,17 @@ if arch == "aarch64" or arch == "larch64":
f"#third_party/acados/{arch}/lib",
]
- if arch == "larch64":
- libpath += [
- "#third_party/snpe/larch64",
- "#third_party/libyuv/larch64/lib",
- "/usr/lib/aarch64-linux-gnu"
- ]
- cpppath += [
- "#selfdrive/camerad/include",
- ]
- cflags = ["-DQCOM2", "-mcpu=cortex-a57"]
- cxxflags = ["-DQCOM2", "-mcpu=cortex-a57"]
- rpath += ["/usr/local/lib"]
- else:
- rpath = []
- libpath += [
- "#third_party/snpe/aarch64",
- "#third_party/libyuv/lib",
- "/system/vendor/lib64"
- ]
- cflags = ["-DQCOM", "-D_USING_LIBCXX", "-mcpu=cortex-a57"]
- cxxflags = ["-DQCOM", "-D_USING_LIBCXX", "-mcpu=cortex-a57"]
+ libpath += [
+ "#third_party/snpe/larch64",
+ "#third_party/libyuv/larch64/lib",
+ "/usr/lib/aarch64-linux-gnu"
+ ]
+ cpppath += [
+ "#selfdrive/camerad/include",
+ ]
+ cflags = ["-DQCOM2", "-mcpu=cortex-a57"]
+ cxxflags = ["-DQCOM2", "-mcpu=cortex-a57"]
+ rpath += ["/usr/local/lib"]
else:
cflags = []
cxxflags = []
@@ -259,6 +244,7 @@ if os.environ.get('SCONS_PROGRESS'):
SHARED = False
+# TODO: this can probably be removed
def abspath(x):
if arch == 'aarch64':
pth = os.path.join("/data/pythonpath", x[0].path)
@@ -287,9 +273,7 @@ Export('envCython')
# Qt build environment
qt_env = env.Clone()
-qt_modules = ["Widgets", "Gui", "Core", "Network", "Concurrent", "Multimedia", "Quick", "Qml", "QuickWidgets", "Location", "Positioning"]
-if arch != "aarch64":
- qt_modules += ["DBus"]
+qt_modules = ["Widgets", "Gui", "Core", "Network", "Concurrent", "Multimedia", "Quick", "Qml", "QuickWidgets", "Location", "Positioning", "DBus"]
qt_libs = []
if arch == "Darwin":
@@ -304,15 +288,6 @@ if arch == "Darwin":
qt_env["LINKFLAGS"] += ["-F" + os.path.join(qt_env['QTDIR'], "lib")]
qt_env["FRAMEWORKS"] += [f"Qt{m}" for m in qt_modules] + ["OpenGL"]
qt_env.AppendENVPath('PATH', os.path.join(qt_env['QTDIR'], "bin"))
-elif arch == "aarch64":
- qt_env['QTDIR'] = "/usr"
- qt_dirs = [
- f"/usr/include/qt",
- ]
- qt_dirs += [f"/usr/include/qt/Qt{m}" for m in qt_modules]
-
- qt_libs = [f"Qt5{m}" for m in qt_modules]
- qt_libs += ['EGL', 'GLESv3', 'c++_shared']
else:
qt_env['QTDIR'] = "/usr"
qt_dirs = [
@@ -390,7 +365,7 @@ rednose_config = {
},
}
-if arch not in ["aarch64", "larch64"]:
+if arch != "larch64":
rednose_config['to_build'].update({
'gnss': ('#selfdrive/locationd/models/gnss_kf.py', True, []),
'loc_4': ('#selfdrive/locationd/models/loc_kf.py', True, []),
diff --git a/cereal b/cereal
index d6c3cf6b33..29f4fe89ef 160000
--- a/cereal
+++ b/cereal
@@ -1 +1 @@
-Subproject commit d6c3cf6b33e699f82e5d78ae22c74cad978830b6
+Subproject commit 29f4fe89ef710ff86a5aeb998a357187d0619fb8
diff --git a/common/file_helpers.py b/common/file_helpers.py
index 592b2a199a..8a45fa313c 100644
--- a/common/file_helpers.py
+++ b/common/file_helpers.py
@@ -81,25 +81,12 @@ def _get_fileobject_func(writer, temp_dir):
return writer.get_fileobject(dir=temp_dir)
return _get_fileobject
-def monkeypatch_os_link():
- # This is neccesary on EON/C2, where os.link is patched out of python
- if not hasattr(os, 'link'):
- from cffi import FFI
- ffi = FFI()
- ffi.cdef("int link(const char *oldpath, const char *newpath);")
- libc = ffi.dlopen(None)
-
- def link(src, dest):
- return libc.link(src.encode(), dest.encode())
- os.link = link
-
def atomic_write_on_fs_tmp(path, **kwargs):
"""Creates an atomic writer using a temporary file in a temporary directory
on the same filesystem as path.
"""
# TODO(mgraczyk): This use of AtomicWriter relies on implementation details to set the temp
# directory.
- monkeypatch_os_link()
writer = AtomicWriter(path, **kwargs)
return writer._open(_get_fileobject_func(writer, get_tmpdir_on_same_filesystem(path)))
@@ -108,6 +95,5 @@ def atomic_write_in_dir(path, **kwargs):
"""Creates an atomic writer using a temporary file in the same directory
as the destination file.
"""
- monkeypatch_os_link()
writer = AtomicWriter(path, **kwargs)
return writer._open(_get_fileobject_func(writer, os.path.dirname(path)))
diff --git a/common/realtime.py b/common/realtime.py
index d577680aee..4a37efadab 100644
--- a/common/realtime.py
+++ b/common/realtime.py
@@ -2,8 +2,9 @@
import gc
import os
import time
-import multiprocessing
-from typing import Optional
+from typing import Optional, List, Union
+
+from setproctitle import getproctitle # pylint: disable=no-name-in-module
from common.clock import sec_since_boot # pylint: disable=no-name-in-module, import-error
from selfdrive.hardware import PC, TICI
@@ -37,15 +38,16 @@ def set_realtime_priority(level: int) -> None:
os.sched_setscheduler(0, os.SCHED_FIFO, os.sched_param(level)) # type: ignore[attr-defined]
-def set_core_affinity(core: int) -> None:
+def set_core_affinity(cores: List[int]) -> None:
if not PC:
- os.sched_setaffinity(0, [core,]) # type: ignore[attr-defined]
+ os.sched_setaffinity(0, cores)
-def config_realtime_process(core: int, priority: int) -> None:
+def config_realtime_process(cores: Union[int, List[int]], priority: int) -> None:
gc.disable()
set_realtime_priority(priority)
- set_core_affinity(core)
+ c = cores if isinstance(cores, list) else [cores, ]
+ set_core_affinity(c)
class Ratekeeper:
@@ -56,7 +58,7 @@ class Ratekeeper:
self._print_delay_threshold = print_delay_threshold
self._frame = 0
self._remaining = 0.0
- self._process_name = multiprocessing.current_process().name
+ self._process_name = getproctitle()
@property
def frame(self) -> int:
diff --git a/docs/CARS.md b/docs/CARS.md
index b5c0128d9c..520359596f 100644
--- a/docs/CARS.md
+++ b/docs/CARS.md
@@ -49,7 +49,7 @@ How We Rate The Cars
|Lexus|ES 2019-21|All|
|
|
|
|
|
|Lexus|ES Hybrid 2019-21|All|
|
|
|
|
|
|Lexus|NX 2020|All|
|
|
|
|
|
-|Lexus|RX 2020-21|All|
|
|
|
|
|
+|Lexus|RX 2020-22|All|
|
|
|
|
|
|Lexus|RX Hybrid 2020-21|All|
|
|
|
|
|
|Lexus|UX Hybrid 2019-21|All|
|
|
|
|
|
|Toyota|Alphard 2019-20|All|
|
|
|
|
|
@@ -81,7 +81,7 @@ How We Rate The Cars
|Genesis|G70 2018|All|
|
|
|
|
|
|Genesis|G80 2018|All|
|
|
|
|
|
|Hyundai|Elantra 2021-22|SCC + LKAS|
|
|
|
|
|
-|Hyundai|Elantra Hybrid 2021|SCC + LKAS|
|
|
|
|
|
+|Hyundai|Elantra Hybrid 2021-22|SCC + LKAS|
|
|
|
|
|
|Hyundai|Ioniq Electric 2020|SCC + LKAS|
|
|
|
|
|
|Hyundai|Ioniq Hybrid 2020-22|SCC + LFA|
|
|
|
|
|
|Hyundai|Ioniq Plug-in Hybrid 2020-21|SCC + LKAS|
|
|
|
|
|
@@ -132,7 +132,7 @@ How We Rate The Cars
|Toyota|Sienna 2018-20|All|
[3](#footnotes)|
|
|
|
|
|Volkswagen|Arteon 2018, 2021[8](#footnotes)|Driver Assistance|
|
|
|
|
|
|Volkswagen|Atlas 2018-19, 2022[8](#footnotes)|Driver Assistance|
|
|
|
|
|
-|Volkswagen|e-Golf 2014, 2019-20|Driver Assistance|
|
|
|
|
|
+|Volkswagen|e-Golf 2014, 2018-20|Driver Assistance|
|
|
|
|
|
|Volkswagen|Golf 2015-20|Driver Assistance|
|
|
|
|
|
|Volkswagen|Golf Alltrack 2017-18|Driver Assistance|
|
|
|
|
|
|Volkswagen|Golf GTE 2016|Driver Assistance|
|
|
|
|
|
@@ -187,6 +187,7 @@ How We Rate The Cars
|Hyundai|Genesis 2015-16|SCC + LKAS|
|
|
|
|
|
|Hyundai|Ioniq Electric 2019|SCC + LKAS|
|
|
|
|
|
|Hyundai|Ioniq Hybrid 2017-19|SCC + LKAS|
|
|
|
|
|
+|Hyundai|Ioniq Plug-in Hybrid 2019|SCC + LKAS|
|
|
|
|
|
|Hyundai|Veloster 2019-20|SCC + LKAS|
|
|
|
|
|
|Jeep|Grand Cherokee 2016-18|Adaptive Cruise|
|
|
|
|
|
|Jeep|Grand Cherokee 2019-20|Adaptive Cruise|
|
|
|
|
|
diff --git a/installer/updater/updater b/installer/updater/updater
deleted file mode 100755
index 8bf40708a6..0000000000
--- a/installer/updater/updater
+++ /dev/null
@@ -1,2 +0,0 @@
-#!/usr/bin/bash
-echo "this is a compatability shim for old updaters"
diff --git a/launch_chffrplus.sh b/launch_chffrplus.sh
index 29f58ad002..28e3801c28 100755
--- a/launch_chffrplus.sh
+++ b/launch_chffrplus.sh
@@ -8,96 +8,6 @@ source "$BASEDIR/launch_env.sh"
DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" >/dev/null && pwd )"
-function two_init {
-
- # set IO scheduler
- setprop sys.io.scheduler noop
- for f in /sys/block/*/queue/scheduler; do
- echo noop > $f
- done
-
- # *** shield cores 2-3 ***
-
- # TODO: should we enable this?
- # offline cores 2-3 to force recurring timers onto the other cores
- #echo 0 > /sys/devices/system/cpu/cpu2/online
- #echo 0 > /sys/devices/system/cpu/cpu3/online
- #echo 1 > /sys/devices/system/cpu/cpu2/online
- #echo 1 > /sys/devices/system/cpu/cpu3/online
-
- # android gets two cores
- echo 0-1 > /dev/cpuset/background/cpus
- echo 0-1 > /dev/cpuset/system-background/cpus
- echo 0-1 > /dev/cpuset/foreground/cpus
- echo 0-1 > /dev/cpuset/foreground/boost/cpus
- echo 0-1 > /dev/cpuset/android/cpus
-
- # openpilot gets all the cores
- echo 0-3 > /dev/cpuset/app/cpus
-
- # mask off 2-3 from RPS and XPS - Receive/Transmit Packet Steering
- echo 3 | tee /sys/class/net/*/queues/*/rps_cpus
- echo 3 | tee /sys/class/net/*/queues/*/xps_cpus
-
- # *** set up governors ***
-
- # +50mW offroad, +500mW onroad for 30% more RAM bandwidth
- echo "performance" > /sys/class/devfreq/soc:qcom,cpubw/governor
- echo 1056000 > /sys/class/devfreq/soc:qcom,m4m/max_freq
- echo "performance" > /sys/class/devfreq/soc:qcom,m4m/governor
-
- # unclear if these help, but they don't seem to hurt
- echo "performance" > /sys/class/devfreq/soc:qcom,memlat-cpu0/governor
- echo "performance" > /sys/class/devfreq/soc:qcom,memlat-cpu2/governor
-
- # GPU
- echo "performance" > /sys/class/devfreq/b00000.qcom,kgsl-3d0/governor
-
- # /sys/class/devfreq/soc:qcom,mincpubw is the only one left at "powersave"
- # it seems to gain nothing but a wasted 500mW
-
- # *** set up IRQ affinities ***
-
- # Collect RIL and other possibly long-running I/O interrupts onto CPU 1
- echo 1 > /proc/irq/78/smp_affinity_list # qcom,smd-modem (LTE radio)
- echo 1 > /proc/irq/33/smp_affinity_list # ufshcd (flash storage)
- echo 1 > /proc/irq/35/smp_affinity_list # wifi (wlan_pci)
- echo 1 > /proc/irq/6/smp_affinity_list # MDSS
-
- # USB traffic needs realtime handling on cpu 3
- [ -d "/proc/irq/733" ] && echo 3 > /proc/irq/733/smp_affinity_list
-
- # GPU and camera get cpu 2
- CAM_IRQS="177 178 179 180 181 182 183 184 185 186 192"
- for irq in $CAM_IRQS; do
- echo 2 > /proc/irq/$irq/smp_affinity_list
- done
- echo 2 > /proc/irq/193/smp_affinity_list # GPU
-
- # give GPU threads RT priority
- for pid in $(pgrep "kgsl"); do
- chrt -f -p 52 $pid
- done
-
- # the flippening!
- LD_LIBRARY_PATH="" content insert --uri content://settings/system --bind name:s:user_rotation --bind value:i:1
-
- # disable bluetooth
- service call bluetooth_manager 8
-
- # wifi scan
- wpa_cli IFNAME=wlan0 SCAN
-
- # Check for NEOS update
- if [ $(< /VERSION) != "$REQUIRED_NEOS_VERSION" ]; then
- echo "Installing NEOS update"
- NEOS_PY="$DIR/selfdrive/hardware/eon/neos.py"
- MANIFEST="$DIR/selfdrive/hardware/eon/neos.json"
- $NEOS_PY --swap-if-ready $MANIFEST
- $DIR/selfdrive/hardware/eon/updater $NEOS_PY $MANIFEST
- fi
-}
-
function tici_init {
# wait longer for weston to come up
if [ -f "$BASEDIR/prebuilt" ]; then
@@ -168,9 +78,7 @@ function launch {
export PYTHONPATH="$PWD:$PWD/pyextra"
# hardware specific init
- if [ -f /EON ]; then
- two_init
- elif [ -f /TICI ]; then
+ if [ -f /TICI ]; then
tici_init
fi
diff --git a/launch_env.sh b/launch_env.sh
index cd0c27f643..d4b57c909a 100755
--- a/launch_env.sh
+++ b/launch_env.sh
@@ -6,10 +6,6 @@ export NUMEXPR_NUM_THREADS=1
export OPENBLAS_NUM_THREADS=1
export VECLIB_MAXIMUM_THREADS=1
-if [ -z "$REQUIRED_NEOS_VERSION" ]; then
- export REQUIRED_NEOS_VERSION="19.1"
-fi
-
if [ -z "$AGNOS_VERSION" ]; then
export AGNOS_VERSION="4"
fi
diff --git a/opendbc b/opendbc
index eb56fff37a..e19ba095c3 160000
--- a/opendbc
+++ b/opendbc
@@ -1 +1 @@
-Subproject commit eb56fff37a4a2738df7add08779db51a0a6f38e2
+Subproject commit e19ba095c3ee288d629284e24ec7e0deaf645f3f
diff --git a/panda b/panda
index c925407461..7dd9493eb1 160000
--- a/panda
+++ b/panda
@@ -1 +1 @@
-Subproject commit c925407461e46adf3282494af6daa00b2626ebb8
+Subproject commit 7dd9493eb102ba8b96362c7265b06851ec1d4bac
diff --git a/release/build_release.sh b/release/build_release.sh
index 95fcfea1a1..79ab4fb087 100755
--- a/release/build_release.sh
+++ b/release/build_release.sh
@@ -13,10 +13,6 @@ if [ -f /TICI ]; then
FILES_SRC="release/files_tici"
RELEASE_BRANCH=release3-staging
DASHCAM_BRANCH=dashcam3-staging
-elif [ -f /EON ]; then
- FILES_SRC="release/files_eon"
- RELEASE_BRANCH=release2-staging
- DASHCAM_BRANCH=dashcam-staging
else
exit 0
fi
diff --git a/release/files_common b/release/files_common
index ec0b8370c0..8c1214f73d 100644
--- a/release/files_common
+++ b/release/files_common
@@ -64,8 +64,7 @@ models/dmonitoring_model_q.dlc
release/*
tools/lib/*
-
-installer/updater/updater
+tools/joystick/*
selfdrive/version.py
@@ -239,7 +238,7 @@ selfdrive/controls/lib/events.py
selfdrive/controls/lib/lane_planner.py
selfdrive/controls/lib/latcontrol_angle.py
selfdrive/controls/lib/latcontrol_indi.py
-selfdrive/controls/lib/latcontrol_lqr.py
+selfdrive/controls/lib/latcontrol_torque.py
selfdrive/controls/lib/latcontrol_pid.py
selfdrive/controls/lib/latcontrol.py
selfdrive/controls/lib/lateral_planner.py
@@ -260,14 +259,6 @@ selfdrive/hardware/__init__.py
selfdrive/hardware/base.h
selfdrive/hardware/base.py
selfdrive/hardware/hw.h
-selfdrive/hardware/eon/__init__.py
-selfdrive/hardware/eon/androidd.py
-selfdrive/hardware/eon/shutdownd.py
-selfdrive/hardware/eon/hardware.h
-selfdrive/hardware/eon/hardware.py
-selfdrive/hardware/eon/neos.py
-selfdrive/hardware/eon/neos.json
-selfdrive/hardware/eon/updater
selfdrive/hardware/tici/__init__.py
selfdrive/hardware/tici/hardware.py
selfdrive/hardware/tici/amplifier.py
@@ -299,7 +290,6 @@ selfdrive/locationd/models/live_kf.cc
selfdrive/locationd/calibrationd.py
selfdrive/logcatd/SConscript
-selfdrive/logcatd/logcatd_android.cc
selfdrive/logcatd/logcatd_systemd.cc
selfdrive/proclogd/SConscript
@@ -311,6 +301,8 @@ selfdrive/loggerd/SConscript
selfdrive/loggerd/encoder.h
selfdrive/loggerd/omx_encoder.cc
selfdrive/loggerd/omx_encoder.h
+selfdrive/loggerd/video_writer.cc
+selfdrive/loggerd/video_writer.h
selfdrive/loggerd/logger.cc
selfdrive/loggerd/logger.h
selfdrive/loggerd/loggerd.cc
@@ -329,7 +321,6 @@ selfdrive/loggerd/xattr_cache.py
selfdrive/sensord/SConscript
selfdrive/sensord/libdiag.h
-selfdrive/sensord/sensors_qcom.cc
selfdrive/sensord/sensors_qcom2.cc
selfdrive/sensord/sensors/*.cc
selfdrive/sensord/sensors/*.h
@@ -363,8 +354,6 @@ selfdrive/ui/qt/offroad/*.h
selfdrive/ui/qt/offroad/*.qml
selfdrive/ui/qt/widgets/*.cc
selfdrive/ui/qt/widgets/*.h
-selfdrive/ui/qt/spinner_aarch64
-selfdrive/ui/qt/text_aarch64
selfdrive/ui/replay/*.cc
selfdrive/ui/replay/*.h
@@ -376,11 +365,8 @@ selfdrive/camerad/snapshot/*
selfdrive/camerad/include/*
selfdrive/camerad/cameras/camera_common.h
selfdrive/camerad/cameras/camera_common.cc
-selfdrive/camerad/cameras/camera_qcom.cc
-selfdrive/camerad/cameras/camera_qcom.h
selfdrive/camerad/cameras/camera_replay.cc
selfdrive/camerad/cameras/camera_replay.h
-selfdrive/camerad/cameras/debayer.cl
selfdrive/camerad/cameras/sensor_i2c.h
selfdrive/camerad/cameras/sensor2_i2c.h
@@ -456,7 +442,6 @@ selfdrive/assets/training/*
third_party/SConscript
-third_party/libgralloc/**
third_party/linux/**
third_party/opencl/**
third_party/zlib/*
@@ -477,19 +462,12 @@ third_party/libyuv/lib/**
third_party/libyuv/larch64/**
third_party/snpe/include/**
-third_party/snpe/aarch64**
-third_party/snpe/larch64**
third_party/snpe/dsp**
third_party/acados/x86_64/**
-third_party/acados/aarch64/**
third_party/acados/larch64/**
third_party/acados/include/**
-third_party/android_frameworks_native/**
-third_party/android_hardware_libhardware/**
-third_party/android_system_core/**
-
scripts/update_now.sh
scripts/stop_updater.sh
@@ -565,8 +543,8 @@ opendbc/gm_global_a_powertrain_generated.dbc
opendbc/gm_global_a_object.dbc
opendbc/gm_global_a_chassis.dbc
-opendbc/ford_fusion_2018_pt.dbc
opendbc/ford_fusion_2018_adas.dbc
+opendbc/ford_lincoln_base_pt.dbc
opendbc/honda_accord_2018_can_generated.dbc
opendbc/acura_ilx_2016_can_generated.dbc
diff --git a/release/files_eon b/release/files_eon
deleted file mode 100644
index b43bf86b50..0000000000
--- a/release/files_eon
+++ /dev/null
@@ -1 +0,0 @@
-README.md
diff --git a/release/files_tici b/release/files_tici
index 59cc41918f..68e075c971 100644
--- a/release/files_tici
+++ b/release/files_tici
@@ -1,3 +1,5 @@
+third_party/snpe/larch64**
+third_party/snpe/aarch64-ubuntu-gcc7.5/*
third_party/mapbox-gl-native-qt/include/*
selfdrive/timezoned.py
diff --git a/scripts/launch_corolla.sh b/scripts/launch_corolla.sh
new file mode 100755
index 0000000000..0801938e71
--- /dev/null
+++ b/scripts/launch_corolla.sh
@@ -0,0 +1,6 @@
+#!/usr/bin/bash
+
+DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" >/dev/null && pwd)"
+
+export FINGERPRINT="TOYOTA COROLLA TSS2 2019"
+$DIR/../launch_openpilot.sh
diff --git a/selfdrive/athena/athenad.py b/selfdrive/athena/athenad.py
index 71a18aff73..919cf0c04e 100755
--- a/selfdrive/athena/athenad.py
+++ b/selfdrive/athena/athenad.py
@@ -255,12 +255,12 @@ def getMessage(service=None, timeout=1000):
@dispatcher.add_method
-def getVersion():
+def getVersion() -> Dict[str, str]:
return {
"version": get_version(),
- "remote": get_origin(),
- "branch": get_short_branch(),
- "commit": get_commit(),
+ "remote": get_origin(''),
+ "branch": get_short_branch(''),
+ "commit": get_commit(default=''),
}
diff --git a/selfdrive/athena/registration.py b/selfdrive/athena/registration.py
index 10f5d46f77..cc4bd7e647 100755
--- a/selfdrive/athena/registration.py
+++ b/selfdrive/athena/registration.py
@@ -3,6 +3,7 @@ import time
import json
import jwt
from pathlib import Path
+from typing import Optional
from datetime import datetime, timedelta
from common.api import api_get
@@ -48,7 +49,8 @@ def register(show_spinner=False) -> str:
# Block until we get the imei
serial = HARDWARE.get_serial()
start_time = time.monotonic()
- imei1, imei2 = None, None
+ imei1: Optional[str] = None
+ imei2: Optional[str] = None
while imei1 is None and imei2 is None:
try:
imei1, imei2 = HARDWARE.get_imei(0), HARDWARE.get_imei(1)
diff --git a/selfdrive/boardd/boardd.cc b/selfdrive/boardd/boardd.cc
index cbd0609f0d..5b44f4d60b 100644
--- a/selfdrive/boardd/boardd.cc
+++ b/selfdrive/boardd/boardd.cc
@@ -156,7 +156,7 @@ bool safety_setter_thread(std::vector pandas) {
capnp::FlatArrayMessageReader cmsg(aligned_buf.align(params.data(), params.size()));
cereal::CarParams::Reader car_params = cmsg.getRoot();
cereal::CarParams::SafetyModel safety_model;
- int safety_param;
+ uint32_t safety_param;
auto safety_configs = car_params.getSafetyConfigs();
uint16_t alternative_experience = car_params.getAlternativeExperience();
@@ -169,7 +169,7 @@ bool safety_setter_thread(std::vector pandas) {
} else {
// If no safety mode is specified, default to silent
safety_model = cereal::CarParams::SafetyModel::SILENT;
- safety_param = 0;
+ safety_param = 0U;
}
LOGW("panda %d: setting safety model: %d, param: %d, alternative experience: %d", i, (int)safety_model, safety_param, alternative_experience);
diff --git a/selfdrive/boardd/panda.cc b/selfdrive/boardd/panda.cc
index 7b4e81f9ae..394437902f 100644
--- a/selfdrive/boardd/panda.cc
+++ b/selfdrive/boardd/panda.cc
@@ -247,7 +247,7 @@ int Panda::usb_bulk_read(unsigned char endpoint, unsigned char* data, int length
return transferred;
}
-void Panda::set_safety_model(cereal::CarParams::SafetyModel safety_model, int safety_param) {
+void Panda::set_safety_model(cereal::CarParams::SafetyModel safety_model, uint32_t safety_param) {
usb_write(0xdc, (uint16_t)safety_model, safety_param);
}
diff --git a/selfdrive/boardd/panda.h b/selfdrive/boardd/panda.h
index 294553b413..9bbeee86b4 100644
--- a/selfdrive/boardd/panda.h
+++ b/selfdrive/boardd/panda.h
@@ -73,7 +73,7 @@ class Panda {
// Panda functionality
cereal::PandaState::PandaType get_hw_type();
- void set_safety_model(cereal::CarParams::SafetyModel safety_model, int safety_param=0);
+ void set_safety_model(cereal::CarParams::SafetyModel safety_model, uint32_t safety_param=0U);
void set_alternative_experience(uint16_t alternative_experience);
void set_rtc(struct tm sys_time);
struct tm get_rtc();
diff --git a/selfdrive/camerad/SConscript b/selfdrive/camerad/SConscript
index 85bc756bc1..bb55c825d4 100644
--- a/selfdrive/camerad/SConscript
+++ b/selfdrive/camerad/SConscript
@@ -2,10 +2,7 @@ Import('env', 'arch', 'cereal', 'messaging', 'common', 'gpucommon', 'visionipc',
libs = ['m', 'pthread', common, 'jpeg', 'OpenCL', 'yuv', cereal, messaging, 'zmq', 'capnp', 'kj', visionipc, gpucommon]
-if arch == "aarch64":
- libs += ['gsl', 'CB', 'adreno_utils', 'EGL', 'GLESv3', 'cutils', 'ui']
- cameras = ['cameras/camera_qcom.cc']
-elif arch == "larch64":
+if arch == "larch64":
libs += ['atomic']
cameras = ['cameras/camera_qcom2.cc']
else:
diff --git a/selfdrive/camerad/cameras/camera_common.cc b/selfdrive/camerad/cameras/camera_common.cc
index f3879bd2fe..be9d0724ff 100644
--- a/selfdrive/camerad/cameras/camera_common.cc
+++ b/selfdrive/camerad/cameras/camera_common.cc
@@ -18,10 +18,7 @@
#include "selfdrive/common/util.h"
#include "selfdrive/hardware/hw.h"
-#ifdef QCOM
-#include "CL/cl_ext_qcom.h"
-#include "selfdrive/camerad/cameras/camera_qcom.h"
-#elif QCOM2
+#if QCOM2
#include "CL/cl_ext_qcom.h"
#include "selfdrive/camerad/cameras/camera_qcom2.h"
#elif WEBCAM
@@ -46,7 +43,7 @@ public:
ci->frame_width, ci->frame_height, ci->frame_stride,
b->rgb_width, b->rgb_height, b->rgb_stride,
ci->bayer_flip, ci->hdr, s->camera_num);
- const char *cl_file = Hardware::TICI() ? "cameras/real_debayer.cl" : "cameras/debayer.cl";
+ const char *cl_file = "cameras/real_debayer.cl";
cl_program prg_debayer = cl_program_from_file(context, device_id, cl_file, args);
krnl_ = CL_CHECK_ERR(clCreateKernel(prg_debayer, "debayer10", &err));
CL_CHECK(clReleaseProgram(prg_debayer));
@@ -56,30 +53,13 @@ public:
CL_CHECK(clSetKernelArg(krnl_, 0, sizeof(cl_mem), &cam_buf_cl));
CL_CHECK(clSetKernelArg(krnl_, 1, sizeof(cl_mem), &buf_cl));
- if (Hardware::TICI()) {
- const int debayer_local_worksize = 16;
- constexpr int localMemSize = (debayer_local_worksize + 2 * (3 / 2)) * (debayer_local_worksize + 2 * (3 / 2)) * sizeof(short int);
- const size_t globalWorkSize[] = {size_t(width), size_t(height)};
- const size_t localWorkSize[] = {debayer_local_worksize, debayer_local_worksize};
- CL_CHECK(clSetKernelArg(krnl_, 2, localMemSize, 0));
- CL_CHECK(clSetKernelArg(krnl_, 3, sizeof(float), &black_level));
- CL_CHECK(clEnqueueNDRangeKernel(q, krnl_, 2, NULL, globalWorkSize, localWorkSize, 0, 0, debayer_event));
- } else {
- if (hdr_) {
- // HDR requires a 1-D kernel due to the DPCM compression
- const size_t debayer_local_worksize = 128;
- const size_t debayer_work_size = height; // doesn't divide evenly, is this okay?
- CL_CHECK(clSetKernelArg(krnl_, 2, sizeof(float), &gain));
- CL_CHECK(clEnqueueNDRangeKernel(q, krnl_, 1, NULL, &debayer_work_size, &debayer_local_worksize, 0, 0, debayer_event));
- } else {
- const int debayer_local_worksize = 32;
- assert(width % 2 == 0);
- const size_t globalWorkSize[] = {size_t(height), size_t(width / 2)};
- const size_t localWorkSize[] = {debayer_local_worksize, debayer_local_worksize};
- CL_CHECK(clSetKernelArg(krnl_, 2, sizeof(float), &gain));
- CL_CHECK(clEnqueueNDRangeKernel(q, krnl_, 2, NULL, globalWorkSize, localWorkSize, 0, 0, debayer_event));
- }
- }
+ const int debayer_local_worksize = 16;
+ constexpr int localMemSize = (debayer_local_worksize + 2 * (3 / 2)) * (debayer_local_worksize + 2 * (3 / 2)) * sizeof(short int);
+ const size_t globalWorkSize[] = {size_t(width), size_t(height)};
+ const size_t localWorkSize[] = {debayer_local_worksize, debayer_local_worksize};
+ CL_CHECK(clSetKernelArg(krnl_, 2, localMemSize, 0));
+ CL_CHECK(clSetKernelArg(krnl_, 3, sizeof(float), &black_level));
+ CL_CHECK(clEnqueueNDRangeKernel(q, krnl_, 2, NULL, globalWorkSize, localWorkSize, 0, 0, debayer_event));
}
~Debayer() {
@@ -176,7 +156,6 @@ bool CameraBuf::acquire() {
#else
if (camera_state->camera_id == CAMERA_ID_IMX390) black_level = 64.0;
#endif
-
debayer->queue(q, camrabuf_cl, cur_rgb_buf->buf_cl, rgb_width, rgb_height, gain, black_level, &event);
} else {
assert(rgb_stride == camera_state->ci.frame_stride);
@@ -418,17 +397,6 @@ static void driver_cam_auto_exposure(CameraState *c, SubMaster &sm) {
}
static ExpRect rect = def_rect;
- // use driver face crop for AE
- if (Hardware::EON() && sm.updated("driverState")) {
- if (auto state = sm["driverState"].getDriverState(); state.getFaceProb() > 0.4) {
- auto face_position = state.getFacePosition();
- int x = is_rhd ? 0 : frame_width - (0.5 * frame_height);
- x += (face_position[0] * (is_rhd ? -1.0 : 1.0) + 0.5) * (0.5 * frame_height) + x_offset;
- int y = (face_position[1] + 0.5) * frame_height + y_offset;
- rect = {std::max(0, x - 72), std::min(b->rgb_width - 1, x + 72), 2,
- std::max(0, y - 72), std::min(b->rgb_height - 1, y + 72), 1};
- }
- }
camera_autoexposure(c, set_exposure_target(b, rect.x1, rect.x2, rect.x_skip, rect.y1, rect.y2, rect.y_skip));
}
@@ -452,7 +420,7 @@ void common_process_driver_camera(MultiCameraState *s, CameraState *c, int cnt)
void camerad_thread() {
cl_device_id device_id = cl_get_device_id(CL_DEVICE_TYPE_DEFAULT);
-#if defined(QCOM) || defined(QCOM2)
+#ifdef QCOM2
const cl_context_properties props[] = {CL_CONTEXT_PRIORITY_HINT_QCOM, CL_PRIORITY_HINT_HIGH_QCOM, 0};
cl_context context = CL_CHECK_ERR(clCreateContext(props, 1, &device_id, NULL, NULL, &err));
#else
diff --git a/selfdrive/camerad/cameras/camera_common.h b/selfdrive/camerad/cameras/camera_common.h
index 6fafa0e59d..e415f80982 100644
--- a/selfdrive/camerad/cameras/camera_common.h
+++ b/selfdrive/camerad/cameras/camera_common.h
@@ -29,7 +29,7 @@
#define CAMERA_ID_MAX 10
const int UI_BUF_COUNT = 4;
-const int YUV_BUFFER_COUNT = Hardware::EON() ? 100 : 40;
+const int YUV_BUFFER_COUNT = 40;
enum CameraType {
RoadCam = 0,
diff --git a/selfdrive/camerad/cameras/camera_qcom.cc b/selfdrive/camerad/cameras/camera_qcom.cc
deleted file mode 100644
index 411ff0aec4..0000000000
--- a/selfdrive/camerad/cameras/camera_qcom.cc
+++ /dev/null
@@ -1,1164 +0,0 @@
-#include "selfdrive/camerad/cameras/camera_qcom.h"
-
-#include
-#include
-#include
-#include
-
-#include
-#include
-#include
-#include
-#include
-
-#include
-#include
-
-#include "selfdrive/camerad/cameras/sensor_i2c.h"
-#include "selfdrive/camerad/include/msm_cam_sensor.h"
-#include "selfdrive/camerad/include/msmb_camera.h"
-#include "selfdrive/camerad/include/msmb_isp.h"
-#include "selfdrive/camerad/include/msmb_ispif.h"
-#include "selfdrive/common/clutil.h"
-#include "selfdrive/common/params.h"
-#include "selfdrive/common/swaglog.h"
-#include "selfdrive/common/timing.h"
-#include "selfdrive/common/util.h"
-
-// leeco actuator (DW9800W H-Bridge Driver IC)
-// from sniff
-const uint16_t INFINITY_DAC = 364;
-
-extern ExitHandler do_exit;
-
-static int cam_ioctl(int fd, unsigned long int request, void *arg, const char *log_msg = nullptr) {
- int err = HANDLE_EINTR(ioctl(fd, request, arg));
- if (err != 0 && log_msg) {
- LOG(util::string_format("%s: %d", log_msg, err).c_str());
- }
- return err;
-}
-// global var for AE/AF ops
-std::atomic road_cam_exp{{0}};
-std::atomic driver_cam_exp{{0}};
-
-CameraInfo cameras_supported[CAMERA_ID_MAX] = {
- [CAMERA_ID_IMX298] = {
- .frame_width = 2328,
- .frame_height = 1748,
- .frame_stride = 2912,
- .bayer = true,
- .bayer_flip = 3,
- .hdr = true
- },
- [CAMERA_ID_OV8865] = {
- .frame_width = 1632,
- .frame_height = 1224,
- .frame_stride = 2040, // seems right
- .bayer = true,
- .bayer_flip = 3,
- .hdr = false
- },
- // this exists to get the kernel to build for the LeEco in release
- [CAMERA_ID_IMX298_FLIPPED] = {
- .frame_width = 2328,
- .frame_height = 1748,
- .frame_stride = 2912,
- .bayer = true,
- .bayer_flip = 3,
- .hdr = true
- },
- [CAMERA_ID_OV10640] = {
- .frame_width = 1280,
- .frame_height = 1080,
- .frame_stride = 2040,
- .bayer = true,
- .bayer_flip = 0,
- .hdr = true
- },
-};
-
-static void camera_release_buffer(void* cookie, int buf_idx) {
- CameraState *s = (CameraState *)cookie;
- // printf("camera_release_buffer %d\n", buf_idx);
- s->ss[0].qbuf_info[buf_idx].dirty_buf = 1;
- HANDLE_EINTR(ioctl(s->isp_fd, VIDIOC_MSM_ISP_ENQUEUE_BUF, &s->ss[0].qbuf_info[buf_idx]));
-}
-
-int sensor_write_regs(CameraState *s, struct msm_camera_i2c_reg_array* arr, size_t size, msm_camera_i2c_data_type data_type) {
- struct msm_camera_i2c_reg_setting out_settings = {
- .reg_setting = arr,
- .size = (uint16_t)size,
- .addr_type = MSM_CAMERA_I2C_WORD_ADDR,
- .data_type = data_type,
- .delay = 0,
- };
- sensorb_cfg_data cfg_data = {.cfgtype = CFG_WRITE_I2C_ARRAY, .cfg.setting = &out_settings};
- return HANDLE_EINTR(ioctl(s->sensor_fd, VIDIOC_MSM_SENSOR_CFG, &cfg_data));
-}
-
-static int imx298_apply_exposure(CameraState *s, int gain, int integ_lines, uint32_t frame_length) {
- int analog_gain = std::min(gain, 448);
- s->digital_gain = gain > 448 ? (512.0/(512-(gain))) / 8.0 : 1.0;
- //printf("%5d/%5d %5d %f\n", s->cur_integ_lines, s->frame_length, analog_gain, s->digital_gain);
-
- struct msm_camera_i2c_reg_array reg_array[] = {
- // REG_HOLD
- {0x104,0x1,0},
- {0x3002,0x0,0}, // long autoexposure off
-
- // FRM_LENGTH
- {0x340, (uint16_t)(frame_length >> 8), 0}, {0x341, (uint16_t)(frame_length & 0xff), 0},
- // INTEG_TIME aka coarse_int_time_addr aka shutter speed
- {0x202, (uint16_t)(integ_lines >> 8), 0}, {0x203, (uint16_t)(integ_lines & 0xff),0},
- // global_gain_addr
- // if you assume 1x gain is 32, 448 is 14x gain, aka 2^14=16384
- {0x204, (uint16_t)(analog_gain >> 8), 0}, {0x205, (uint16_t)(analog_gain & 0xff),0},
-
- // digital gain for colors: gain_greenR, gain_red, gain_blue, gain_greenB
- /*{0x20e, digital_gain_gr >> 8, 0}, {0x20f,digital_gain_gr & 0xFF,0},
- {0x210, digital_gain_r >> 8, 0}, {0x211,digital_gain_r & 0xFF,0},
- {0x212, digital_gain_b >> 8, 0}, {0x213,digital_gain_b & 0xFF,0},
- {0x214, digital_gain_gb >> 8, 0}, {0x215,digital_gain_gb & 0xFF,0},*/
-
- // REG_HOLD
- {0x104,0x0,0},
- };
- return sensor_write_regs(s, reg_array, std::size(reg_array), MSM_CAMERA_I2C_BYTE_DATA);
-}
-
-static int ov8865_apply_exposure(CameraState *s, int gain, int integ_lines, uint32_t frame_length) {
- //printf("driver camera: %d %d %d\n", gain, integ_lines, frame_length);
- int coarse_gain_bitmap, fine_gain_bitmap;
-
- // get bitmaps from iso
- static const int gains[] = {0, 100, 200, 400, 800};
- int i;
- for (i = 1; i < std::size(gains); i++) {
- if (gain >= gains[i - 1] && gain < gains[i])
- break;
- }
- int coarse_gain = i - 1;
- float fine_gain = (gain - gains[coarse_gain])/(float)(gains[coarse_gain+1]-gains[coarse_gain]);
- coarse_gain_bitmap = (1 << coarse_gain) - 1;
- fine_gain_bitmap = ((int)(16*fine_gain) << 3) + 128; // 7th is always 1, 0-2nd are always 0
-
- integ_lines *= 16; // The exposure value in reg is in 16ths of a line
-
- struct msm_camera_i2c_reg_array reg_array[] = {
- //{0x104,0x1,0},
-
- // FRM_LENGTH
- {0x380e, (uint16_t)(frame_length >> 8), 0}, {0x380f, (uint16_t)(frame_length & 0xff), 0},
- // AEC EXPO
- {0x3500, (uint16_t)(integ_lines >> 16), 0}, {0x3501, (uint16_t)(integ_lines >> 8), 0}, {0x3502, (uint16_t)(integ_lines & 0xff),0},
- // AEC MANUAL
- {0x3503, 0x4, 0},
- // AEC GAIN
- {0x3508, (uint16_t)(coarse_gain_bitmap), 0}, {0x3509, (uint16_t)(fine_gain_bitmap), 0},
-
- //{0x104,0x0,0},
- };
- return sensor_write_regs(s, reg_array, std::size(reg_array), MSM_CAMERA_I2C_BYTE_DATA);
-}
-
-static void camera_init(VisionIpcServer *v, CameraState *s, int camera_id, int camera_num,
- uint32_t pixel_clock, uint32_t line_length_pclk,
- uint32_t max_gain, uint32_t fps, cl_device_id device_id, cl_context ctx,
- VisionStreamType rgb_type, VisionStreamType yuv_type) {
- s->camera_num = camera_num;
- s->camera_id = camera_id;
-
- assert(camera_id < std::size(cameras_supported));
- s->ci = cameras_supported[camera_id];
- assert(s->ci.frame_width != 0);
-
- s->pixel_clock = pixel_clock;
- s->max_gain = max_gain;
- s->fps = fps;
- s->frame_length = s->pixel_clock / line_length_pclk / s->fps;
- s->self_recover = 0;
-
- s->apply_exposure = (camera_id == CAMERA_ID_IMX298) ? imx298_apply_exposure : ov8865_apply_exposure;
- s->buf.init(device_id, ctx, s, v, FRAME_BUF_COUNT, rgb_type, yuv_type, camera_release_buffer);
-}
-
-void cameras_init(VisionIpcServer *v, MultiCameraState *s, cl_device_id device_id, cl_context ctx) {
- char project_name[1024] = {0};
- property_get("ro.boot.project_name", project_name, "");
- assert(strlen(project_name) == 0);
-
- // sensor is flipped in LP3
- // IMAGE_ORIENT = 3
- init_array_imx298[0].reg_data = 3;
-
- // 0 = ISO 100
- // 256 = ISO 200
- // 384 = ISO 400
- // 448 = ISO 800
- // 480 = ISO 1600
- // 496 = ISO 3200
- // 504 = ISO 6400, 8x digital gain
- // 508 = ISO 12800, 16x digital gain
- // 510 = ISO 25600, 32x digital gain
-
- camera_init(v, &s->road_cam, CAMERA_ID_IMX298, 0,
- /*pixel_clock=*/600000000, /*line_length_pclk=*/5536,
- /*max_gain=*/510, //0 (ISO 100)- 448 (ISO 800, max analog gain) - 511 (super noisy)
-#ifdef HIGH_FPS
- /*fps*/ 60,
-#else
- /*fps*/ 20,
-#endif
- device_id, ctx,
- VISION_STREAM_RGB_ROAD, VISION_STREAM_ROAD);
-
- camera_init(v, &s->driver_cam, CAMERA_ID_OV8865, 1,
- /*pixel_clock=*/72000000, /*line_length_pclk=*/1602,
- /*max_gain=*/510, 10, device_id, ctx,
- VISION_STREAM_RGB_DRIVER, VISION_STREAM_DRIVER);
-
- s->sm = new SubMaster({"driverState"});
- s->pm = new PubMaster({"roadCameraState", "driverCameraState", "thumbnail"});
-
- for (int i = 0; i < FRAME_BUF_COUNT; i++) {
- // TODO: make lengths correct
- s->focus_bufs[i].allocate(0xb80);
- s->stats_bufs[i].allocate(0xb80);
- }
- std::fill_n(s->lapres, std::size(s->lapres), 16160);
- s->lap_conv = new LapConv(device_id, ctx, s->road_cam.buf.rgb_width, s->road_cam.buf.rgb_height, s->road_cam.buf.rgb_stride, 3);
-}
-
-static void set_exposure(CameraState *s, float exposure_frac, float gain_frac) {
- int err = 0;
- uint32_t gain = s->cur_gain;
- uint32_t integ_lines = s->cur_integ_lines;
-
- if (exposure_frac >= 0) {
- exposure_frac = std::clamp(exposure_frac, 2.0f / s->frame_length, 1.0f);
- integ_lines = s->frame_length * exposure_frac;
-
- // See page 79 of the datasheet, this is the max allowed (-1 for phase adjust)
- integ_lines = std::min(integ_lines, s->frame_length - 11);
- }
-
- if (gain_frac >= 0) {
- // ISO200 is minimum gain
- gain_frac = std::clamp(gain_frac, 1.0f/64, 1.0f);
-
- // linearize gain response
- // TODO: will be wrong for driver camera
- // 0.125 -> 448
- // 0.25 -> 480
- // 0.5 -> 496
- // 1.0 -> 504
- // 512 - 512/(128*gain_frac)
- gain = (s->max_gain/510) * (512 - 512/(256*gain_frac));
- }
-
- if (gain != s->cur_gain || integ_lines != s->cur_integ_lines) {
- if (s->apply_exposure == ov8865_apply_exposure) {
- gain = 800 * gain_frac; // ISO
- }
- err = s->apply_exposure(s, gain, integ_lines, s->frame_length);
- if (err == 0) {
- std::lock_guard lk(s->frame_info_lock);
- s->cur_gain = gain;
- s->cur_integ_lines = integ_lines;
- } else {
- LOGE("camera %d apply_exposure err: %d", s->camera_num, err);
- }
- }
-
- if (err == 0) {
- s->cur_exposure_frac = exposure_frac;
- std::lock_guard lk(s->frame_info_lock);
- s->cur_gain_frac = gain_frac;
- }
-
- //LOGD("set exposure: %f %f - %d", exposure_frac, gain_frac, err);
-}
-
-static void do_autoexposure(CameraState *s, float grey_frac) {
- const float target_grey = 0.3;
-
- s->frame_info_lock.lock();
- s->measured_grey_fraction = grey_frac;
- s->target_grey_fraction = target_grey;
- s->frame_info_lock.unlock();
-
- if (s->apply_exposure == ov8865_apply_exposure) {
- // gain limits downstream
- const float gain_frac_min = 0.015625;
- const float gain_frac_max = 1.0;
- // exposure time limits
- const uint32_t exposure_time_min = 16;
- const uint32_t exposure_time_max = s->frame_length - 11; // copied from set_exposure()
-
- float cur_gain_frac = s->cur_gain_frac;
- float exposure_factor = pow(1.05, (target_grey - grey_frac) / 0.05);
- if (cur_gain_frac > 0.125 && exposure_factor < 1) {
- cur_gain_frac *= exposure_factor;
- } else if (s->cur_integ_lines * exposure_factor <= exposure_time_max && s->cur_integ_lines * exposure_factor >= exposure_time_min) { // adjust exposure time first
- s->cur_exposure_frac *= exposure_factor;
- } else if (cur_gain_frac * exposure_factor <= gain_frac_max && cur_gain_frac * exposure_factor >= gain_frac_min) {
- cur_gain_frac *= exposure_factor;
- }
- s->frame_info_lock.lock();
- s->cur_gain_frac = cur_gain_frac;
- s->frame_info_lock.unlock();
-
- set_exposure(s, s->cur_exposure_frac, cur_gain_frac);
- } else { // keep the old for others
- float new_exposure = s->cur_exposure_frac;
- new_exposure *= pow(1.05, (target_grey - grey_frac) / 0.05 );
- //LOGD("diff %f: %f to %f", target_grey - grey_frac, s->cur_exposure_frac, new_exposure);
-
- float new_gain = s->cur_gain_frac;
- if (new_exposure < 0.10) {
- new_gain *= 0.95;
- } else if (new_exposure > 0.40) {
- new_gain *= 1.05;
- }
-
- set_exposure(s, new_exposure, new_gain);
- }
-}
-
-static void sensors_init(MultiCameraState *s) {
- msm_camera_sensor_slave_info slave_infos[2] = {
- (msm_camera_sensor_slave_info){ // road camera
- .sensor_name = "imx298",
- .eeprom_name = "sony_imx298",
- .actuator_name = "dw9800w",
- .ois_name = "",
- .flash_name = "pmic",
- .camera_id = CAMERA_0,
- .slave_addr = 32,
- .i2c_freq_mode = I2C_FAST_MODE,
- .addr_type = MSM_CAMERA_I2C_WORD_ADDR,
- .sensor_id_info = {.sensor_id_reg_addr = 22, .sensor_id = 664, .module_id = 9, .vcm_id = 6},
- .power_setting_array = {
- .power_setting_a = {
- {.seq_type = SENSOR_GPIO, .delay = 1},
- {.seq_type = SENSOR_VREG, .seq_val = 2},
- {.seq_type = SENSOR_GPIO, .seq_val = 5, .config_val = 2},
- {.seq_type = SENSOR_VREG, .seq_val = 1},
- {.seq_type = SENSOR_VREG, .seq_val = 3, .delay = 1},
- {.seq_type = SENSOR_CLK, .config_val = 24000000, .delay = 1},
- {.seq_type = SENSOR_GPIO, .config_val = 2, .delay = 10},
- },
- .size = 7,
- .power_down_setting_a = {
- {.seq_type = SENSOR_CLK, .delay = 1},
- {.seq_type = SENSOR_GPIO, .delay = 1},
- {.seq_type = SENSOR_VREG, .seq_val = 1},
- {.seq_type = SENSOR_GPIO, .seq_val = 5},
- {.seq_type = SENSOR_VREG, .seq_val = 2},
- {.seq_type = SENSOR_VREG, .seq_val = 3, .delay = 1},
- },
- .size_down = 6,
- },
- .is_init_params_valid = 0,
- .sensor_init_params = {.modes_supported = 1, .position = BACK_CAMERA_B, .sensor_mount_angle = 90},
- .output_format = MSM_SENSOR_BAYER,
- },
- (msm_camera_sensor_slave_info){ // driver camera
- .sensor_name = "ov8865_sunny",
- .eeprom_name = "ov8865_plus",
- .actuator_name = "",
- .ois_name = "",
- .flash_name = "",
- .camera_id = CAMERA_2,
- .slave_addr = 108,
- .i2c_freq_mode = I2C_FAST_MODE,
- .addr_type = MSM_CAMERA_I2C_WORD_ADDR,
- .sensor_id_info = {.sensor_id_reg_addr = 12299, .sensor_id = 34917, .module_id = 2},
- .power_setting_array = {
- .power_setting_a = {
- {.seq_type = SENSOR_GPIO, .delay = 5},
- {.seq_type = SENSOR_VREG, .seq_val = 1},
- {.seq_type = SENSOR_VREG, .seq_val = 2},
- {.seq_type = SENSOR_VREG},
- {.seq_type = SENSOR_CLK, .config_val = 24000000, .delay = 1},
- {.seq_type = SENSOR_GPIO, .config_val = 2, .delay = 1},
- },
- .size = 6,
- .power_down_setting_a = {
- {.seq_type = SENSOR_GPIO, .delay = 5},
- {.seq_type = SENSOR_CLK, .delay = 1},
- {.seq_type = SENSOR_VREG},
- {.seq_type = SENSOR_VREG, .seq_val = 1},
- {.seq_type = SENSOR_VREG, .seq_val = 2, .delay = 1},
- },
- .size_down = 5,
- },
- .is_init_params_valid = 0,
- .sensor_init_params = {.modes_supported = 1, .position = FRONT_CAMERA_B, .sensor_mount_angle = 270},
- .output_format = MSM_SENSOR_BAYER,
- }};
-
- unique_fd sensorinit_fd = open_v4l_by_name_and_index("msm_sensor_init");
- assert(sensorinit_fd >= 0);
- for (auto &info : slave_infos) {
- info.power_setting_array.power_setting = &info.power_setting_array.power_setting_a[0];
- info.power_setting_array.power_down_setting = &info.power_setting_array.power_down_setting_a[0];
- sensor_init_cfg_data sensor_init_cfg = {.cfgtype = CFG_SINIT_PROBE, .cfg.setting = &info};
- int err = cam_ioctl(sensorinit_fd, VIDIOC_MSM_SENSOR_INIT_CFG, &sensor_init_cfg, "sensor init cfg");
- assert(err >= 0);
- }
-}
-
-static void camera_open(CameraState *s, bool is_road_cam) {
- struct csid_cfg_data csid_cfg_data = {};
- struct v4l2_event_subscription sub = {};
-
- struct msm_actuator_cfg_data actuator_cfg_data = {};
-
- // open devices
- s->csid_fd = open_v4l_by_name_and_index("msm_csid", is_road_cam ? 0 : 2);
- assert(s->csid_fd >= 0);
- s->csiphy_fd = open_v4l_by_name_and_index("msm_csiphy", is_road_cam ? 0 : 2);
- assert(s->csiphy_fd >= 0);
- s->isp_fd = open_v4l_by_name_and_index("vfe", is_road_cam ? 0 : 1);
- assert(s->isp_fd >= 0);
-
- if (is_road_cam) {
- s->actuator_fd = open_v4l_by_name_and_index("msm_actuator");
- assert(s->actuator_fd >= 0);
- }
-
- // wait for sensor device
- // on first startup, these devices aren't present yet
- for (int i = 0; i < 10; i++) {
- s->sensor_fd = open_v4l_by_name_and_index(is_road_cam ? "imx298" : "ov8865_sunny");
- if (s->sensor_fd >= 0) break;
- LOGW("waiting for sensors...");
- util::sleep_for(1000); // sleep one second
- }
- assert(s->sensor_fd >= 0);
-
- // *** SHUTDOWN ALL ***
-
- // CSIPHY: release csiphy
- struct msm_camera_csi_lane_params csi_lane_params = {0};
- csi_lane_params.csi_lane_mask = 0x1f;
- csiphy_cfg_data csiphy_cfg_data = { .cfg.csi_lane_params = &csi_lane_params, .cfgtype = CSIPHY_RELEASE};
- int err = cam_ioctl(s->csiphy_fd, VIDIOC_MSM_CSIPHY_IO_CFG, &csiphy_cfg_data, "release csiphy");
-
- // CSID: release csid
- csid_cfg_data.cfgtype = CSID_RELEASE;
- cam_ioctl(s->csid_fd, VIDIOC_MSM_CSID_IO_CFG, &csid_cfg_data, "release csid");
-
- // SENSOR: send power down
- struct sensorb_cfg_data sensorb_cfg_data = {.cfgtype = CFG_POWER_DOWN};
- cam_ioctl(s->sensor_fd, VIDIOC_MSM_SENSOR_CFG, &sensorb_cfg_data, "sensor power down");
-
- // actuator powerdown
- actuator_cfg_data.cfgtype = CFG_ACTUATOR_POWERDOWN;
- cam_ioctl(s->actuator_fd, VIDIOC_MSM_ACTUATOR_CFG, &actuator_cfg_data, "actuator powerdown");
-
- // reset isp
- // struct msm_vfe_axi_halt_cmd halt_cmd = {
- // .stop_camif = 1,
- // .overflow_detected = 1,
- // .blocking_halt = 1,
- // };
- // err = ioctl(s->isp_fd, VIDIOC_MSM_ISP_AXI_HALT, &halt_cmd);
- // printf("axi halt: %d\n", err);
-
- // struct msm_vfe_axi_reset_cmd reset_cmd = {
- // .blocking = 1,
- // .frame_id = 1,
- // };
- // err = ioctl(s->isp_fd, VIDIOC_MSM_ISP_AXI_RESET, &reset_cmd);
- // printf("axi reset: %d\n", err);
-
- // struct msm_vfe_axi_restart_cmd restart_cmd = {
- // .enable_camif = 1,
- // };
- // err = ioctl(s->isp_fd, VIDIOC_MSM_ISP_AXI_RESTART, &restart_cmd);
- // printf("axi restart: %d\n", err);
-
- // **** GO GO GO ****
- LOG("******************** GO GO GO ************************");
-
- // CSID: init csid
- csid_cfg_data.cfgtype = CSID_INIT;
- cam_ioctl(s->csid_fd, VIDIOC_MSM_CSID_IO_CFG, &csid_cfg_data, "init csid");
-
- // CSIPHY: init csiphy
- csiphy_cfg_data = {.cfgtype = CSIPHY_INIT};
- cam_ioctl(s->csiphy_fd, VIDIOC_MSM_CSIPHY_IO_CFG, &csiphy_cfg_data, "init csiphy");
-
- // SENSOR: stop stream
- struct msm_camera_i2c_reg_setting stop_settings = {
- .reg_setting = stop_reg_array,
- .size = std::size(stop_reg_array),
- .addr_type = MSM_CAMERA_I2C_WORD_ADDR,
- .data_type = MSM_CAMERA_I2C_BYTE_DATA,
- .delay = 0
- };
- sensorb_cfg_data.cfgtype = CFG_SET_STOP_STREAM_SETTING;
- sensorb_cfg_data.cfg.setting = &stop_settings;
- cam_ioctl(s->sensor_fd, VIDIOC_MSM_SENSOR_CFG, &sensorb_cfg_data, "stop stream");
-
- // SENSOR: send power up
- sensorb_cfg_data = {.cfgtype = CFG_POWER_UP};
- cam_ioctl(s->sensor_fd, VIDIOC_MSM_SENSOR_CFG, &sensorb_cfg_data, "sensor power up");
-
- // **** configure the sensor ****
-
- // SENSOR: send i2c configuration
- if (s->camera_id == CAMERA_ID_IMX298) {
- err = sensor_write_regs(s, init_array_imx298, std::size(init_array_imx298), MSM_CAMERA_I2C_BYTE_DATA);
- } else if (s->camera_id == CAMERA_ID_OV8865) {
- err = sensor_write_regs(s, init_array_ov8865, std::size(init_array_ov8865), MSM_CAMERA_I2C_BYTE_DATA);
- } else {
- assert(false);
- }
- LOG("sensor init i2c: %d", err);
-
- if (is_road_cam) {
- // init the actuator
- actuator_cfg_data.cfgtype = CFG_ACTUATOR_POWERUP;
- cam_ioctl(s->actuator_fd, VIDIOC_MSM_ACTUATOR_CFG, &actuator_cfg_data, "actuator powerup");
-
- actuator_cfg_data.cfgtype = CFG_ACTUATOR_INIT;
- cam_ioctl(s->actuator_fd, VIDIOC_MSM_ACTUATOR_CFG, &actuator_cfg_data, "actuator init");
-
- struct msm_actuator_reg_params_t actuator_reg_params[] = {
- {
- .reg_write_type = MSM_ACTUATOR_WRITE_DAC,
- // MSB here at address 3
- .reg_addr = 3,
- .data_type = 9,
- .addr_type = 4,
- },
- };
-
- struct reg_settings_t actuator_init_settings[] = {
- { .reg_addr=2, .addr_type=MSM_ACTUATOR_BYTE_ADDR, .reg_data=1, .data_type = MSM_ACTUATOR_BYTE_DATA, .i2c_operation = MSM_ACT_WRITE, .delay = 0 }, // PD = power down
- { .reg_addr=2, .addr_type=MSM_ACTUATOR_BYTE_ADDR, .reg_data=0, .data_type = MSM_ACTUATOR_BYTE_DATA, .i2c_operation = MSM_ACT_WRITE, .delay = 2 }, // 0 = power up
- { .reg_addr=2, .addr_type=MSM_ACTUATOR_BYTE_ADDR, .reg_data=2, .data_type = MSM_ACTUATOR_BYTE_DATA, .i2c_operation = MSM_ACT_WRITE, .delay = 2 }, // RING = SAC mode
- { .reg_addr=6, .addr_type=MSM_ACTUATOR_BYTE_ADDR, .reg_data=64, .data_type = MSM_ACTUATOR_BYTE_DATA, .i2c_operation = MSM_ACT_WRITE, .delay = 0 }, // 0x40 = SAC3 mode
- { .reg_addr=7, .addr_type=MSM_ACTUATOR_BYTE_ADDR, .reg_data=113, .data_type = MSM_ACTUATOR_BYTE_DATA, .i2c_operation = MSM_ACT_WRITE, .delay = 0 },
- // 0x71 = DIV1 | DIV0 | SACT0 -- Tvib x 1/4 (quarter)
- // SAC Tvib = 6.3 ms + 0.1 ms = 6.4 ms / 4 = 1.6 ms
- // LSC 1-step = 252 + 1*4 = 256 ms / 4 = 64 ms
- };
-
- struct region_params_t region_params[] = {
- {.step_bound = {238, 0,}, .code_per_step = 235, .qvalue = 128}
- };
-
- actuator_cfg_data.cfgtype = CFG_SET_ACTUATOR_INFO;
- actuator_cfg_data.cfg.set_info = (struct msm_actuator_set_info_t){
- .actuator_params = {
- .act_type = ACTUATOR_BIVCM,
- .reg_tbl_size = 1,
- .data_size = 10,
- .init_setting_size = 5,
- .i2c_freq_mode = I2C_STANDARD_MODE,
- .i2c_addr = 24,
- .i2c_addr_type = MSM_ACTUATOR_BYTE_ADDR,
- .i2c_data_type = MSM_ACTUATOR_WORD_DATA,
- .reg_tbl_params = &actuator_reg_params[0],
- .init_settings = &actuator_init_settings[0],
- .park_lens = {.damping_step = 1023, .damping_delay = 14000, .hw_params = 11, .max_step = 20},
- },
- .af_tuning_params = {
- .initial_code = INFINITY_DAC,
- .pwd_step = 0,
- .region_size = 1,
- .total_steps = 238,
- .region_params = ®ion_params[0],
- },
- };
-
- cam_ioctl(s->actuator_fd, VIDIOC_MSM_ACTUATOR_CFG, &actuator_cfg_data, "actuator set info");
- }
-
- if (s->camera_id == CAMERA_ID_IMX298) {
- err = sensor_write_regs(s, mode_setting_array_imx298, std::size(mode_setting_array_imx298), MSM_CAMERA_I2C_BYTE_DATA);
- LOG("sensor setup: %d", err);
- }
-
- // CSIPHY: configure csiphy
- struct msm_camera_csiphy_params csiphy_params = {};
- if (s->camera_id == CAMERA_ID_IMX298) {
- csiphy_params = {.lane_cnt = 4, .settle_cnt = 14, .lane_mask = 0x1f, .csid_core = 0};
- } else if (s->camera_id == CAMERA_ID_OV8865) {
- // guess!
- csiphy_params = {.lane_cnt = 4, .settle_cnt = 24, .lane_mask = 0x1f, .csid_core = 2};
- }
- csiphy_cfg_data.cfgtype = CSIPHY_CFG;
- csiphy_cfg_data.cfg.csiphy_params = &csiphy_params;
- cam_ioctl(s->csiphy_fd, VIDIOC_MSM_CSIPHY_IO_CFG, &csiphy_cfg_data, "csiphy configure");
-
- // CSID: configure csid
-#define CSI_STATS 0x35
-#define CSI_PD 0x36
- struct msm_camera_csid_params csid_params = {
- .lane_cnt = 4,
- .lane_assign = 0x4320,
- .phy_sel = (uint8_t)(is_road_cam ? 0 : 2),
- .lut_params.num_cid = (uint8_t)(is_road_cam ? 3 : 1),
- .lut_params.vc_cfg_a = {
- {.cid = 0, .dt = CSI_RAW10, .decode_format = CSI_DECODE_10BIT},
- {.cid = 1, .dt = CSI_PD, .decode_format = CSI_DECODE_10BIT},
- {.cid = 2, .dt = CSI_STATS, .decode_format = CSI_DECODE_10BIT},
- },
- };
-
- csid_params.lut_params.vc_cfg[0] = &csid_params.lut_params.vc_cfg_a[0];
- csid_params.lut_params.vc_cfg[1] = &csid_params.lut_params.vc_cfg_a[1];
- csid_params.lut_params.vc_cfg[2] = &csid_params.lut_params.vc_cfg_a[2];
-
- csid_cfg_data.cfgtype = CSID_CFG;
- csid_cfg_data.cfg.csid_params = &csid_params;
- cam_ioctl(s->csid_fd, VIDIOC_MSM_CSID_IO_CFG, &csid_cfg_data, "csid configure");
-
- // ISP: SMMU_ATTACH
- msm_vfe_smmu_attach_cmd smmu_attach_cmd = {.security_mode = 0, .iommu_attach_mode = IOMMU_ATTACH};
- cam_ioctl(s->isp_fd, VIDIOC_MSM_ISP_SMMU_ATTACH, &smmu_attach_cmd, "isp smmu attach");
-
- // ******************* STREAM RAW *****************************
-
- // configure QMET input
- struct msm_vfe_input_cfg input_cfg = {};
- for (int i = 0; i < (is_road_cam ? 3 : 1); i++) {
- StreamState *ss = &s->ss[i];
-
- memset(&input_cfg, 0, sizeof(struct msm_vfe_input_cfg));
- input_cfg.input_src = (msm_vfe_input_src)(VFE_RAW_0+i);
- input_cfg.input_pix_clk = s->pixel_clock;
- input_cfg.d.rdi_cfg.cid = i;
- input_cfg.d.rdi_cfg.frame_based = 1;
- err = ioctl(s->isp_fd, VIDIOC_MSM_ISP_INPUT_CFG, &input_cfg);
- LOG("configure input(%d): %d", i, err);
-
- // ISP: REQUEST_STREAM
- ss->stream_req.axi_stream_handle = 0;
- if (is_road_cam) {
- ss->stream_req.session_id = 2;
- ss->stream_req.stream_id = /*ISP_META_CHANNEL_BIT | */ISP_NATIVE_BUF_BIT | (1+i);
- } else {
- ss->stream_req.session_id = 3;
- ss->stream_req.stream_id = ISP_NATIVE_BUF_BIT | 1;
- }
-
- if (i == 0) {
- ss->stream_req.output_format = v4l2_fourcc('R', 'G', '1', '0');
- } else {
- ss->stream_req.output_format = v4l2_fourcc('Q', 'M', 'E', 'T');
- }
- ss->stream_req.stream_src = (msm_vfe_axi_stream_src)(RDI_INTF_0+i);
-
-#ifdef HIGH_FPS
- if (is_road_cam) {
- ss->stream_req.frame_skip_pattern = EVERY_3FRAME;
- }
-#endif
-
- ss->stream_req.frame_base = 1;
- ss->stream_req.buf_divert = 1; //i == 0;
-
- // setup stream plane. doesn't even matter?
- /*s->stream_req.plane_cfg[0].output_plane_format = Y_PLANE;
- s->stream_req.plane_cfg[0].output_width = s->ci.frame_width;
- s->stream_req.plane_cfg[0].output_height = s->ci.frame_height;
- s->stream_req.plane_cfg[0].output_stride = s->ci.frame_width;
- s->stream_req.plane_cfg[0].output_scan_lines = s->ci.frame_height;
- s->stream_req.plane_cfg[0].rdi_cid = 0;*/
-
- err = ioctl(s->isp_fd, VIDIOC_MSM_ISP_REQUEST_STREAM, &ss->stream_req);
- LOG("isp request stream: %d -> 0x%x", err, ss->stream_req.axi_stream_handle);
-
- // ISP: REQUEST_BUF
- ss->buf_request.session_id = ss->stream_req.session_id;
- ss->buf_request.stream_id = ss->stream_req.stream_id;
- ss->buf_request.num_buf = FRAME_BUF_COUNT;
- ss->buf_request.buf_type = ISP_PRIVATE_BUF;
- ss->buf_request.handle = 0;
- cam_ioctl(s->isp_fd, VIDIOC_MSM_ISP_REQUEST_BUF, &ss->buf_request, "isp request buf");
- LOG("got buf handle: 0x%x", ss->buf_request.handle);
-
- // ENQUEUE all buffers
- for (int j = 0; j < ss->buf_request.num_buf; j++) {
- ss->qbuf_info[j].handle = ss->buf_request.handle;
- ss->qbuf_info[j].buf_idx = j;
- ss->qbuf_info[j].buffer.num_planes = 1;
- ss->qbuf_info[j].buffer.planes[0].addr = ss->bufs[j].fd;
- ss->qbuf_info[j].buffer.planes[0].length = ss->bufs[j].len;
- err = ioctl(s->isp_fd, VIDIOC_MSM_ISP_ENQUEUE_BUF, &ss->qbuf_info[j]);
- }
-
- // ISP: UPDATE_STREAM
- struct msm_vfe_axi_stream_update_cmd update_cmd = {};
- update_cmd.num_streams = 1;
- update_cmd.update_info[0].user_stream_id = ss->stream_req.stream_id;
- update_cmd.update_info[0].stream_handle = ss->stream_req.axi_stream_handle;
- update_cmd.update_type = UPDATE_STREAM_ADD_BUFQ;
- cam_ioctl(s->isp_fd, VIDIOC_MSM_ISP_UPDATE_STREAM, &update_cmd, "isp update stream");
- }
-
- LOG("******** START STREAMS ********");
-
- sub.id = 0;
- sub.type = 0x1ff;
- cam_ioctl(s->isp_fd, VIDIOC_SUBSCRIBE_EVENT, &sub, "isp subscribe");
-
- // ISP: START_STREAM
- s->stream_cfg.cmd = START_STREAM;
- s->stream_cfg.num_streams = is_road_cam ? 3 : 1;
- for (int i = 0; i < s->stream_cfg.num_streams; i++) {
- s->stream_cfg.stream_handle[i] = s->ss[i].stream_req.axi_stream_handle;
- }
- cam_ioctl(s->isp_fd, VIDIOC_MSM_ISP_CFG_STREAM, &s->stream_cfg, "isp start stream");
-}
-
-static void road_camera_start(CameraState *s) {
- set_exposure(s, 1.0, 1.0);
-
- int err = sensor_write_regs(s, start_reg_array, std::size(start_reg_array), MSM_CAMERA_I2C_BYTE_DATA);
- LOG("sensor start regs: %d", err);
-
- int inf_step = 512 - INFINITY_DAC;
-
- // initial guess
- s->lens_true_pos = 400;
-
- // reset lens position
- struct msm_actuator_cfg_data actuator_cfg_data = {};
- actuator_cfg_data.cfgtype = CFG_SET_POSITION;
- actuator_cfg_data.cfg.setpos = (struct msm_actuator_set_position_t){
- .number_of_steps = 1,
- .hw_params = (uint32_t)7,
- .pos = {INFINITY_DAC, 0},
- .delay = {0,}
- };
- cam_ioctl(s->actuator_fd, VIDIOC_MSM_ACTUATOR_CFG, &actuator_cfg_data, "actuator set pos");
-
- // TODO: confirm this isn't needed
- /*memset(&actuator_cfg_data, 0, sizeof(actuator_cfg_data));
- actuator_cfg_data.cfgtype = CFG_MOVE_FOCUS;
- actuator_cfg_data.cfg.move = (struct msm_actuator_move_params_t){
- .dir = 0,
- .sign_dir = 1,
- .dest_step_pos = inf_step,
- .num_steps = inf_step,
- .curr_lens_pos = 0,
- .ringing_params = &actuator_ringing_params,
- };
- err = ioctl(s->actuator_fd, VIDIOC_MSM_ACTUATOR_CFG, &actuator_cfg_data); // should be ~332 at startup ?
- LOG("init actuator move focus: %d", err);*/
- //actuator_cfg_data.cfg.move.curr_lens_pos;
-
- s->cur_lens_pos = 0;
- s->cur_step_pos = inf_step;
-
- actuator_move(s, s->cur_lens_pos);
- LOG("init lens pos: %d", s->cur_lens_pos);
-}
-
-void actuator_move(CameraState *s, uint16_t target) {
- // LP3 moves only on even positions. TODO: use proper sensor params
-
- // focus on infinity assuming phone is perpendicular
- static struct damping_params_t actuator_ringing_params = {
- .damping_step = 1023,
- .damping_delay = 20000,
- .hw_params = 13,
- };
-
- int step = (target - s->cur_lens_pos) / 2;
-
- int dest_step_pos = s->cur_step_pos + step;
- dest_step_pos = std::clamp(dest_step_pos, 0, 255);
-
- struct msm_actuator_cfg_data actuator_cfg_data = {0};
- actuator_cfg_data.cfgtype = CFG_MOVE_FOCUS;
- actuator_cfg_data.cfg.move = (struct msm_actuator_move_params_t){
- .dir = (int8_t)((step > 0) ? MOVE_NEAR : MOVE_FAR),
- .sign_dir = (int8_t)((step > 0) ? MSM_ACTUATOR_MOVE_SIGNED_NEAR : MSM_ACTUATOR_MOVE_SIGNED_FAR),
- .dest_step_pos = (int16_t)dest_step_pos,
- .num_steps = abs(step),
- .curr_lens_pos = s->cur_lens_pos,
- .ringing_params = &actuator_ringing_params,
- };
- HANDLE_EINTR(ioctl(s->actuator_fd, VIDIOC_MSM_ACTUATOR_CFG, &actuator_cfg_data));
-
- s->cur_step_pos = dest_step_pos;
- s->cur_lens_pos = actuator_cfg_data.cfg.move.curr_lens_pos;
- //LOGD("step %d target: %d lens pos: %d", dest_step_pos, target, s->cur_lens_pos);
-}
-
-static void parse_autofocus(CameraState *s, uint8_t *d) {
- int good_count = 0;
- int16_t max_focus = -32767;
- int avg_focus = 0;
-
- /*printf("FOCUS: ");
- for (int i = 0; i < 0x10; i++) {
- printf("%2.2X ", d[i]);
- }*/
-
- for (int i = 0; i < NUM_FOCUS; i++) {
- int doff = i*5+5;
- s->confidence[i] = d[doff];
- // this should just be a 10-bit signed int instead of 11
- // TODO: write it in a nicer way
- int16_t focus_t = (d[doff+1] << 3) | (d[doff+2] >> 5);
- if (focus_t >= 1024) focus_t = -(2048-focus_t);
- s->focus[i] = focus_t;
- //printf("%x->%d ", d[doff], focus_t);
- if (s->confidence[i] > 0x20) {
- good_count++;
- max_focus = std::max(max_focus, s->focus[i]);
- avg_focus += s->focus[i];
- }
- }
- // self recover override
- if (s->self_recover > 1) {
- s->focus_err = 200 * ((s->self_recover % 2 == 0) ? 1:-1); // far for even numbers, close for odd
- s->self_recover -= 2;
- return;
- }
-
- if (good_count < 4) {
- s->focus_err = nan("");
- return;
- }
-
- avg_focus /= good_count;
-
- // outlier rejection
- if (abs(avg_focus - max_focus) > 200) {
- s->focus_err = nan("");
- return;
- }
-
- s->focus_err = max_focus*1.0;
-}
-
-static void do_autofocus(CameraState *s) {
- float lens_true_pos = s->lens_true_pos.load();
- if (!isnan(s->focus_err)) {
- // learn lens_true_pos
- const float focus_kp = 0.005;
- lens_true_pos -= s->focus_err*focus_kp;
- }
-
- // stay off the walls
- lens_true_pos = std::clamp(lens_true_pos, float(LP3_AF_DAC_DOWN), float(LP3_AF_DAC_UP));
- s->lens_true_pos.store(lens_true_pos);
- actuator_move(s, lens_true_pos);
-}
-
-void camera_autoexposure(CameraState *s, float grey_frac) {
- if (s->camera_num == 0) {
- CameraExpInfo tmp = road_cam_exp.load();
- tmp.op_id++;
- tmp.grey_frac = grey_frac;
- road_cam_exp.store(tmp);
- } else {
- CameraExpInfo tmp = driver_cam_exp.load();
- tmp.op_id++;
- tmp.grey_frac = grey_frac;
- driver_cam_exp.store(tmp);
- }
-}
-
-static void driver_camera_start(CameraState *s) {
- set_exposure(s, 1.0, 1.0);
- int err = sensor_write_regs(s, start_reg_array, std::size(start_reg_array), MSM_CAMERA_I2C_BYTE_DATA);
- LOG("sensor start regs: %d", err);
-}
-
-void cameras_open(MultiCameraState *s) {
- struct msm_ispif_param_data ispif_params = {
- .num = 4,
- .entries = {
- // road camera
- {.vfe_intf = VFE0, .intftype = RDI0, .num_cids = 1, .cids[0] = CID0, .csid = CSID0},
- // driver camera
- {.vfe_intf = VFE1, .intftype = RDI0, .num_cids = 1, .cids[0] = CID0, .csid = CSID2},
- // road camera (focus)
- {.vfe_intf = VFE0, .intftype = RDI1, .num_cids = 1, .cids[0] = CID1, .csid = CSID0},
- // road camera (stats, for AE)
- {.vfe_intf = VFE0, .intftype = RDI2, .num_cids = 1, .cids[0] = CID2, .csid = CSID0},
- },
- };
- s->msmcfg_fd = HANDLE_EINTR(open("/dev/media0", O_RDWR | O_NONBLOCK));
- assert(s->msmcfg_fd >= 0);
-
- sensors_init(s);
-
- s->v4l_fd = HANDLE_EINTR(open("/dev/video0", O_RDWR | O_NONBLOCK));
- assert(s->v4l_fd >= 0);
-
- s->ispif_fd = open_v4l_by_name_and_index("msm_ispif");
- assert(s->ispif_fd >= 0);
-
- // ISPIF: stop
- // memset(&ispif_cfg_data, 0, sizeof(ispif_cfg_data));
- // ispif_cfg_data.cfg_type = ISPIF_STOP_FRAME_BOUNDARY;
- // ispif_cfg_data.params = ispif_params;
- // err = ioctl(s->ispif_fd, VIDIOC_MSM_ISPIF_CFG, &ispif_cfg_data);
- // LOG("ispif stop: %d", err);
-
- LOG("*** open driver camera ***");
- s->driver_cam.ss[0].bufs = s->driver_cam.buf.camera_bufs.get();
- camera_open(&s->driver_cam, false);
-
- LOG("*** open road camera ***");
- s->road_cam.ss[0].bufs = s->road_cam.buf.camera_bufs.get();
- s->road_cam.ss[1].bufs = s->focus_bufs;
- s->road_cam.ss[2].bufs = s->stats_bufs;
- camera_open(&s->road_cam, true);
-
- if (getenv("CAMERA_TEST")) {
- cameras_close(s);
- exit(0);
- }
-
- // ISPIF: set vfe info
- struct ispif_cfg_data ispif_cfg_data = {.cfg_type = ISPIF_SET_VFE_INFO, .vfe_info.num_vfe = 2};
- int err = HANDLE_EINTR(ioctl(s->ispif_fd, VIDIOC_MSM_ISPIF_CFG, &ispif_cfg_data));
- LOG("ispif set vfe info: %d", err);
-
- // ISPIF: setup
- ispif_cfg_data = {.cfg_type = ISPIF_INIT, .csid_version = 0x30050000 /* CSID_VERSION_V35*/};
- cam_ioctl(s->ispif_fd, VIDIOC_MSM_ISPIF_CFG, &ispif_cfg_data, "ispif setup");
-
- ispif_cfg_data = {.cfg_type = ISPIF_CFG, .params = ispif_params};
- cam_ioctl(s->ispif_fd, VIDIOC_MSM_ISPIF_CFG, &ispif_cfg_data, "ispif cfg");
-
- ispif_cfg_data.cfg_type = ISPIF_START_FRAME_BOUNDARY;
- cam_ioctl(s->ispif_fd, VIDIOC_MSM_ISPIF_CFG, &ispif_cfg_data, "ispif start_frame_boundary");
-
- driver_camera_start(&s->driver_cam);
- road_camera_start(&s->road_cam);
-}
-
-
-static void camera_close(CameraState *s) {
- // ISP: STOP_STREAM
- s->stream_cfg.cmd = STOP_STREAM;
- cam_ioctl(s->isp_fd, VIDIOC_MSM_ISP_CFG_STREAM, &s->stream_cfg, "isp stop stream");
-
- for (int i = 0; i < 3; i++) {
- StreamState *ss = &s->ss[i];
- if (ss->stream_req.axi_stream_handle != 0) {
- cam_ioctl(s->isp_fd, VIDIOC_MSM_ISP_RELEASE_BUF, &ss->buf_request, "isp release buf");
-
- struct msm_vfe_axi_stream_release_cmd stream_release = {
- .stream_handle = ss->stream_req.axi_stream_handle,
- };
- cam_ioctl(s->isp_fd, VIDIOC_MSM_ISP_RELEASE_STREAM, &stream_release, "isp release stream");
- }
- }
-}
-
-const char* get_isp_event_name(uint32_t type) {
- switch (type) {
- case ISP_EVENT_REG_UPDATE: return "ISP_EVENT_REG_UPDATE";
- case ISP_EVENT_EPOCH_0: return "ISP_EVENT_EPOCH_0";
- case ISP_EVENT_EPOCH_1: return "ISP_EVENT_EPOCH_1";
- case ISP_EVENT_START_ACK: return "ISP_EVENT_START_ACK";
- case ISP_EVENT_STOP_ACK: return "ISP_EVENT_STOP_ACK";
- case ISP_EVENT_IRQ_VIOLATION: return "ISP_EVENT_IRQ_VIOLATION";
- case ISP_EVENT_STATS_OVERFLOW: return "ISP_EVENT_STATS_OVERFLOW";
- case ISP_EVENT_ERROR: return "ISP_EVENT_ERROR";
- case ISP_EVENT_SOF: return "ISP_EVENT_SOF";
- case ISP_EVENT_EOF: return "ISP_EVENT_EOF";
- case ISP_EVENT_BUF_DONE: return "ISP_EVENT_BUF_DONE";
- case ISP_EVENT_BUF_DIVERT: return "ISP_EVENT_BUF_DIVERT";
- case ISP_EVENT_STATS_NOTIFY: return "ISP_EVENT_STATS_NOTIFY";
- case ISP_EVENT_COMP_STATS_NOTIFY: return "ISP_EVENT_COMP_STATS_NOTIFY";
- case ISP_EVENT_FE_READ_DONE: return "ISP_EVENT_FE_READ_DONE";
- case ISP_EVENT_IOMMU_P_FAULT: return "ISP_EVENT_IOMMU_P_FAULT";
- case ISP_EVENT_HW_FATAL_ERROR: return "ISP_EVENT_HW_FATAL_ERROR";
- case ISP_EVENT_PING_PONG_MISMATCH: return "ISP_EVENT_PING_PONG_MISMATCH";
- case ISP_EVENT_REG_UPDATE_MISSING: return "ISP_EVENT_REG_UPDATE_MISSING";
- case ISP_EVENT_BUF_FATAL_ERROR: return "ISP_EVENT_BUF_FATAL_ERROR";
- case ISP_EVENT_STREAM_UPDATE_DONE: return "ISP_EVENT_STREAM_UPDATE_DONE";
- default: return "unknown";
- }
-}
-
-static FrameMetadata get_frame_metadata(CameraState *s, uint32_t frame_id) {
- std::lock_guard lk(s->frame_info_lock);
- for (auto &i : s->frame_metadata) {
- if (i.frame_id == frame_id) {
- return i;
- }
- }
- // should never happen
- return (FrameMetadata){
- .frame_id = (uint32_t)-1,
- };
-}
-
-static void ops_thread(MultiCameraState *s) {
- int last_road_cam_op_id = 0;
- int last_driver_cam_op_id = 0;
-
- CameraExpInfo road_cam_op;
- CameraExpInfo driver_cam_op;
-
- util::set_thread_name("camera_settings");
- while(!do_exit) {
- road_cam_op = road_cam_exp.load();
- if (road_cam_op.op_id != last_road_cam_op_id) {
- do_autoexposure(&s->road_cam, road_cam_op.grey_frac);
- do_autofocus(&s->road_cam);
- last_road_cam_op_id = road_cam_op.op_id;
- }
-
- driver_cam_op = driver_cam_exp.load();
- if (driver_cam_op.op_id != last_driver_cam_op_id) {
- do_autoexposure(&s->driver_cam, driver_cam_op.grey_frac);
- last_driver_cam_op_id = driver_cam_op.op_id;
- }
-
- util::sleep_for(50);
- }
-}
-
-static void setup_self_recover(CameraState *c, const uint16_t *lapres, size_t lapres_size) {
- const float lens_true_pos = c->lens_true_pos.load();
- int self_recover = c->self_recover.load();
- if (self_recover < 2 && (lens_true_pos < (LP3_AF_DAC_DOWN + 1) || lens_true_pos > (LP3_AF_DAC_UP - 1)) && is_blur(lapres, lapres_size)) {
- // truly stuck, needs help
- if (--self_recover < -FOCUS_RECOVER_PATIENCE) {
- LOGD("road camera bad state detected. attempting recovery from %.1f, recover state is %d", lens_true_pos, self_recover);
- // parity determined by which end is stuck at
- self_recover = FOCUS_RECOVER_STEPS + (lens_true_pos < LP3_AF_DAC_M ? 1 : 0);
- }
- } else if (self_recover < 2 && (lens_true_pos < (LP3_AF_DAC_M - LP3_AF_DAC_3SIG) || lens_true_pos > (LP3_AF_DAC_M + LP3_AF_DAC_3SIG))) {
- // in suboptimal position with high prob, but may still recover by itself
- if (--self_recover < -(FOCUS_RECOVER_PATIENCE * 3)) {
- self_recover = FOCUS_RECOVER_STEPS / 2 + (lens_true_pos < LP3_AF_DAC_M ? 1 : 0);
- }
- } else if (self_recover < 0) {
- self_recover += 1; // reset if fine
- }
- c->self_recover.store(self_recover);
-}
-
-// called by processing_thread
-void process_road_camera(MultiCameraState *s, CameraState *c, int cnt) {
- const CameraBuf *b = &c->buf;
- const int roi_id = cnt % std::size(s->lapres); // rolling roi
- s->lapres[roi_id] = s->lap_conv->Update(b->q, (uint8_t *)b->cur_rgb_buf->addr, roi_id);
- setup_self_recover(c, &s->lapres[0], std::size(s->lapres));
-
- MessageBuilder msg;
- auto framed = msg.initEvent().initRoadCameraState();
- fill_frame_data(framed, b->cur_frame_data);
- if (env_send_road) {
- framed.setImage(get_frame_image(b));
- }
- framed.setFocusVal(s->road_cam.focus);
- framed.setFocusConf(s->road_cam.confidence);
- framed.setRecoverState(s->road_cam.self_recover);
- framed.setSharpnessScore(s->lapres);
- framed.setTransform(b->yuv_transform.v);
- s->pm->send("roadCameraState", msg);
-
- if (cnt % 3 == 0) {
- const int x = 290, y = 322, width = 560, height = 314;
- const int skip = 1;
- camera_autoexposure(c, set_exposure_target(b, x, x + width, skip, y, y + height, skip));
- }
-}
-
-void cameras_run(MultiCameraState *s) {
- std::vector threads;
- threads.push_back(std::thread(ops_thread, s));
- threads.push_back(start_process_thread(s, &s->road_cam, process_road_camera));
- threads.push_back(start_process_thread(s, &s->driver_cam, common_process_driver_camera));
-
- CameraState* cameras[2] = {&s->road_cam, &s->driver_cam};
-
- while (!do_exit) {
- struct pollfd fds[2] = {{.fd = cameras[0]->isp_fd, .events = POLLPRI},
- {.fd = cameras[1]->isp_fd, .events = POLLPRI}};
- int ret = poll(fds, std::size(fds), 1000);
- if (ret < 0) {
- if (errno == EINTR || errno == EAGAIN) continue;
- LOGE("poll failed (%d - %d)", ret, errno);
- break;
- }
-
- // process cameras
- for (int i=0; i<2; i++) {
- if (!fds[i].revents) continue;
-
- CameraState *c = cameras[i];
-
- struct v4l2_event ev = {};
- ret = HANDLE_EINTR(ioctl(c->isp_fd, VIDIOC_DQEVENT, &ev));
- const msm_isp_event_data *isp_event_data = (const msm_isp_event_data *)ev.u.data;
-
- if (ev.type == ISP_EVENT_BUF_DIVERT) {
- const int buf_idx = isp_event_data->u.buf_done.buf_idx;
- const int buffer = (isp_event_data->u.buf_done.stream_id & 0xFFFF) - 1;
- if (buffer == 0) {
- c->buf.camera_bufs_metadata[buf_idx] = get_frame_metadata(c, isp_event_data->frame_id);
- c->buf.queue(buf_idx);
- } else {
- auto &ss = c->ss[buffer];
- if (buffer == 1) {
- parse_autofocus(c, (uint8_t *)(ss.bufs[buf_idx].addr));
- }
- ss.qbuf_info[buf_idx].dirty_buf = 1;
- HANDLE_EINTR(ioctl(c->isp_fd, VIDIOC_MSM_ISP_ENQUEUE_BUF, &ss.qbuf_info[buf_idx]));
- }
-
- } else if (ev.type == ISP_EVENT_EOF) {
- const uint64_t timestamp = (isp_event_data->mono_timestamp.tv_sec * 1000000000ULL + isp_event_data->mono_timestamp.tv_usec * 1000);
- std::lock_guard lk(c->frame_info_lock);
- c->frame_metadata[c->frame_metadata_idx] = (FrameMetadata){
- .frame_id = isp_event_data->frame_id,
- .timestamp_eof = timestamp,
- .frame_length = (uint32_t)c->frame_length,
- .integ_lines = (uint32_t)c->cur_integ_lines,
- .lens_pos = c->cur_lens_pos,
- .lens_err = c->focus_err,
- .lens_true_pos = c->lens_true_pos,
- .gain = c->cur_gain_frac,
- .measured_grey_fraction = c->measured_grey_fraction,
- .target_grey_fraction = c->target_grey_fraction,
- .high_conversion_gain = false,
- };
- c->frame_metadata_idx = (c->frame_metadata_idx + 1) % METADATA_BUF_COUNT;
-
- } else if (ev.type == ISP_EVENT_ERROR) {
- LOGE("ISP_EVENT_ERROR! err type: 0x%08x", isp_event_data->u.error_info.err_type);
- }
- }
- }
-
- LOG(" ************** STOPPING **************");
-
- for (auto &t : threads) t.join();
-
- cameras_close(s);
-}
-
-void cameras_close(MultiCameraState *s) {
- camera_close(&s->road_cam);
- camera_close(&s->driver_cam);
- for (int i = 0; i < FRAME_BUF_COUNT; i++) {
- s->focus_bufs[i].free();
- s->stats_bufs[i].free();
- }
-
- delete s->lap_conv;
- delete s->sm;
- delete s->pm;
-}
diff --git a/selfdrive/camerad/cameras/camera_qcom.h b/selfdrive/camerad/cameras/camera_qcom.h
deleted file mode 100644
index 87bbe4b7e7..0000000000
--- a/selfdrive/camerad/cameras/camera_qcom.h
+++ /dev/null
@@ -1,106 +0,0 @@
-#pragma once
-
-#include
-#include
-#include
-
-#include "cereal/messaging/messaging.h"
-#include "cereal/visionipc/visionbuf.h"
-#include "selfdrive/camerad/cameras/camera_common.h"
-#include "selfdrive/camerad/imgproc/utils.h"
-#include "selfdrive/camerad/include/msm_cam_sensor.h"
-#include "selfdrive/camerad/include/msmb_camera.h"
-#include "selfdrive/camerad/include/msmb_isp.h"
-#include "selfdrive/camerad/include/msmb_ispif.h"
-#include "selfdrive/common/mat.h"
-#include "selfdrive/common/util.h"
-
-#define FRAME_BUF_COUNT 4
-#define METADATA_BUF_COUNT 4
-
-#define NUM_FOCUS 8
-
-#define LP3_AF_DAC_DOWN 366
-#define LP3_AF_DAC_UP 634
-#define LP3_AF_DAC_M 440
-#define LP3_AF_DAC_3SIG 52
-
-#define FOCUS_RECOVER_PATIENCE 50 // 2.5 seconds of complete blur
-#define FOCUS_RECOVER_STEPS 240 // 6 seconds
-
-typedef struct CameraState CameraState;
-
-typedef int (*camera_apply_exposure_func)(CameraState *s, int gain, int integ_lines, uint32_t frame_length);
-
-typedef struct StreamState {
- struct msm_isp_buf_request buf_request;
- struct msm_vfe_axi_stream_request_cmd stream_req;
- struct msm_isp_qbuf_info qbuf_info[FRAME_BUF_COUNT];
- VisionBuf *bufs;
-} StreamState;
-
-typedef struct CameraState {
- int camera_num;
- int camera_id;
-
- int fps;
- CameraInfo ci;
-
- unique_fd csid_fd;
- unique_fd csiphy_fd;
- unique_fd sensor_fd;
- unique_fd isp_fd;
-
- struct msm_vfe_axi_stream_cfg_cmd stream_cfg;
-
- StreamState ss[3];
- CameraBuf buf;
-
- std::mutex frame_info_lock;
- FrameMetadata frame_metadata[METADATA_BUF_COUNT];
- int frame_metadata_idx;
-
- // exposure
- uint32_t pixel_clock, line_length_pclk;
- uint32_t frame_length;
- unsigned int max_gain;
- float cur_exposure_frac, cur_gain_frac;
- int cur_gain, cur_integ_lines;
-
- float measured_grey_fraction;
- float target_grey_fraction;
-
- std::atomic digital_gain;
- camera_apply_exposure_func apply_exposure;
-
- // rear camera only,used for focusing
- unique_fd actuator_fd;
- std::atomic focus_err;
- std::atomic lens_true_pos;
- std::atomic self_recover; // af recovery counter, neg is patience, pos is active
- uint16_t cur_step_pos;
- uint16_t cur_lens_pos;
- int16_t focus[NUM_FOCUS];
- uint8_t confidence[NUM_FOCUS];
-} CameraState;
-
-
-typedef struct MultiCameraState {
- unique_fd ispif_fd;
- unique_fd msmcfg_fd;
- unique_fd v4l_fd;
- uint16_t lapres[(ROI_X_MAX-ROI_X_MIN+1)*(ROI_Y_MAX-ROI_Y_MIN+1)];
-
- VisionBuf focus_bufs[FRAME_BUF_COUNT];
- VisionBuf stats_bufs[FRAME_BUF_COUNT];
-
- CameraState road_cam;
- CameraState driver_cam;
-
- SubMaster *sm;
- PubMaster *pm;
- LapConv *lap_conv;
-} MultiCameraState;
-
-void actuator_move(CameraState *s, uint16_t target);
-int sensor_write_regs(CameraState *s, struct msm_camera_i2c_reg_array* arr, size_t size, int data_type);
diff --git a/selfdrive/camerad/cameras/camera_qcom2.cc b/selfdrive/camerad/cameras/camera_qcom2.cc
index 291dd261d3..8986351fc1 100644
--- a/selfdrive/camerad/cameras/camera_qcom2.cc
+++ b/selfdrive/camerad/cameras/camera_qcom2.cc
@@ -1092,10 +1092,10 @@ void process_road_camera(MultiCameraState *s, CameraState *c, int cnt) {
if ((c == &s->road_cam && env_send_road) || (c == &s->wide_road_cam && env_send_wide_road)) {
framed.setImage(get_frame_image(b));
}
- LOGT("%s: Image set", c == &s->road_cam ? "RoadCamera" : "WideRoadCamera");
+ LOGT(c->buf.cur_frame_data.frame_id, "%s: Image set", c == &s->road_cam ? "RoadCamera" : "WideRoadCamera");
if (c == &s->road_cam) {
framed.setTransform(b->yuv_transform.v);
- LOGT("%s: Transformed", c == &s->road_cam ? "RoadCamera" : "WideRoadCamera");
+ LOGT(c->buf.cur_frame_data.frame_id, "%s: Transformed", "RoadCamera");
}
s->pm->send(c == &s->road_cam ? "roadCameraState" : "wideRoadCameraState", msg);
diff --git a/selfdrive/camerad/cameras/debayer.cl b/selfdrive/camerad/cameras/debayer.cl
deleted file mode 100644
index 4e4b832203..0000000000
--- a/selfdrive/camerad/cameras/debayer.cl
+++ /dev/null
@@ -1,140 +0,0 @@
-const __constant float3 color_correction[3] = {
- // Matrix from WBraw -> sRGBD65 (normalized)
- (float3)( 1.62393627, -0.2092988, 0.00119886),
- (float3)(-0.45734315, 1.5534676, -0.59296798),
- (float3)(-0.16659312, -0.3441688, 1.59176912),
-};
-
-float3 color_correct(float3 x) {
- float3 ret = (0,0,0);
-
- // white balance of daylight
- x /= (float3)(0.4609375, 1.0, 0.546875);
- x = max(0.0, min(1.0, x));
-
- // fix up the colors
- ret += x.x * color_correction[0];
- ret += x.y * color_correction[1];
- ret += x.z * color_correction[2];
- return ret;
-}
-
-float3 srgb_gamma(float3 p) {
- // go all out and add an sRGB gamma curve
- const float3 ph = (1.0f + 0.055f)*pow(p, 1/2.4f) - 0.055f;
- const float3 pl = p*12.92f;
- return select(ph, pl, islessequal(p, 0.0031308f));
-}
-
-#if HDR
-
-__constant int dpcm_lookup[512] = {0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29, 30, 31, 0, -1, -2, -3, -4, -5, -6, -7, -8, -9, -10, -11, -12, -13, -14, -15, -16, -17, -18, -19, -20, -21, -22, -23, -24, -25, -26, -27, -28, -29, -30, -31, 935, 951, 967, 983, 999, 1015, 1031, 1047, 1063, 1079, 1095, 1111, 1127, 1143, 1159, 1175, 1191, 1207, 1223, 1239, 1255, 1271, 1287, 1303, 1319, 1335, 1351, 1367, 1383, 1399, 1415, 1431, -935, -951, -967, -983, -999, -1015, -1031, -1047, -1063, -1079, -1095, -1111, -1127, -1143, -1159, -1175, -1191, -1207, -1223, -1239, -1255, -1271, -1287, -1303, -1319, -1335, -1351, -1367, -1383, -1399, -1415, -1431, 419, 427, 435, 443, 451, 459, 467, 475, 483, 491, 499, 507, 515, 523, 531, 539, 547, 555, 563, 571, 579, 587, 595, 603, 611, 619, 627, 635, 643, 651, 659, 667, 675, 683, 691, 699, 707, 715, 723, 731, 739, 747, 755, 763, 771, 779, 787, 795, 803, 811, 819, 827, 835, 843, 851, 859, 867, 875, 883, 891, 899, 907, 915, 923, -419, -427, -435, -443, -451, -459, -467, -475, -483, -491, -499, -507, -515, -523, -531, -539, -547, -555, -563, -571, -579, -587, -595, -603, -611, -619, -627, -635, -643, -651, -659, -667, -675, -683, -691, -699, -707, -715, -723, -731, -739, -747, -755, -763, -771, -779, -787, -795, -803, -811, -819, -827, -835, -843, -851, -859, -867, -875, -883, -891, -899, -907, -915, -923, 161, 165, 169, 173, 177, 181, 185, 189, 193, 197, 201, 205, 209, 213, 217, 221, 225, 229, 233, 237, 241, 245, 249, 253, 257, 261, 265, 269, 273, 277, 281, 285, 289, 293, 297, 301, 305, 309, 313, 317, 321, 325, 329, 333, 337, 341, 345, 349, 353, 357, 361, 365, 369, 373, 377, 381, 385, 389, 393, 397, 401, 405, 409, 413, -161, -165, -169, -173, -177, -181, -185, -189, -193, -197, -201, -205, -209, -213, -217, -221, -225, -229, -233, -237, -241, -245, -249, -253, -257, -261, -265, -269, -273, -277, -281, -285, -289, -293, -297, -301, -305, -309, -313, -317, -321, -325, -329, -333, -337, -341, -345, -349, -353, -357, -361, -365, -369, -373, -377, -381, -385, -389, -393, -397, -401, -405, -409, -413, 32, 34, 36, 38, 40, 42, 44, 46, 48, 50, 52, 54, 56, 58, 60, 62, 64, 66, 68, 70, 72, 74, 76, 78, 80, 82, 84, 86, 88, 90, 92, 94, 96, 98, 100, 102, 104, 106, 108, 110, 112, 114, 116, 118, 120, 122, 124, 126, 128, 130, 132, 134, 136, 138, 140, 142, 144, 146, 148, 150, 152, 154, 156, 158, -32, -34, -36, -38, -40, -42, -44, -46, -48, -50, -52, -54, -56, -58, -60, -62, -64, -66, -68, -70, -72, -74, -76, -78, -80, -82, -84, -86, -88, -90, -92, -94, -96, -98, -100, -102, -104, -106, -108, -110, -112, -114, -116, -118, -120, -122, -124, -126, -128, -130, -132, -134, -136, -138, -140, -142, -144, -146, -148, -150, -152, -154, -156, -158};
-
-inline uint4 decompress(uint4 p, uint4 pl) {
- uint4 r1 = (pl + (uint4)(dpcm_lookup[p.s0], dpcm_lookup[p.s1], dpcm_lookup[p.s2], dpcm_lookup[p.s3]));
- uint4 r2 = ((p-0x200)<<5) | 0xF;
- r2 += select((uint4)(0,0,0,0), (uint4)(1,1,1,1), r2 <= pl);
- return select(r2, r1, p < 0x200);
-}
-
-#endif
-
-__kernel void debayer10(__global uchar const * const in,
- __global uchar * out, float digital_gain)
-{
- const int oy = get_global_id(0);
- if (oy >= RGB_HEIGHT) return;
- const int iy = oy * 2;
-
-#if HDR
- uint4 pint_last;
- for (int ox = 0; ox < RGB_WIDTH; ox += 2) {
-#else
- int ox = get_global_id(1) * 2;
- {
-#endif
- const int ix = (ox/2) * 5;
-
- // TODO: why doesn't this work for the frontview
- /*const uchar8 v1 = vload8(0, &in[iy * FRAME_STRIDE + ix]);
- const uchar ex1 = v1.s4;
- const uchar8 v2 = vload8(0, &in[(iy+1) * FRAME_STRIDE + ix]);
- const uchar ex2 = v2.s4;*/
-
- const uchar4 v1 = vload4(0, &in[iy * FRAME_STRIDE + ix]);
- const uchar ex1 = in[iy * FRAME_STRIDE + ix + 4];
- const uchar4 v2 = vload4(0, &in[(iy+1) * FRAME_STRIDE + ix]);
- const uchar ex2 = in[(iy+1) * FRAME_STRIDE + ix + 4];
-
- uint4 pinta[2];
- pinta[0] = (uint4)(
- (((uint)v1.s0 << 2) + ( (ex1 >> 0) & 3)),
- (((uint)v1.s1 << 2) + ( (ex1 >> 2) & 3)),
- (((uint)v2.s0 << 2) + ( (ex2 >> 0) & 3)),
- (((uint)v2.s1 << 2) + ( (ex2 >> 2) & 3)));
- pinta[1] = (uint4)(
- (((uint)v1.s2 << 2) + ( (ex1 >> 4) & 3)),
- (((uint)v1.s3 << 2) + ( (ex1 >> 6) & 3)),
- (((uint)v2.s2 << 2) + ( (ex2 >> 4) & 3)),
- (((uint)v2.s3 << 2) + ( (ex2 >> 6) & 3)));
-
- #pragma unroll
- for (uint px = 0; px < 2; px++) {
- uint4 pint = pinta[px];
-
-#if HDR
- // decompress HDR
- pint = (ox == 0 && px == 0) ? ((pint<<4) | 8) : decompress(pint, pint_last);
- pint_last = pint;
-#endif
-
- float4 p = convert_float4(pint);
-
- // 64 is the black level of the sensor, remove
- // (changed to 56 for HDR)
- const float black_level = 56.0f;
- // TODO: switch to max here?
- p = (p - black_level);
-
- // correct vignetting (no pow function?)
- // see https://www.eecis.udel.edu/~jye/lab_research/09/JiUp.pdf the A (4th order)
- const float r = ((oy - RGB_HEIGHT/2)*(oy - RGB_HEIGHT/2) + (ox - RGB_WIDTH/2)*(ox - RGB_WIDTH/2));
- const float fake_f = 700.0f; // should be 910, but this fits...
- const float lil_a = (1.0f + r/(fake_f*fake_f));
- p = p * lil_a * lil_a;
-
- // rescale to 1.0
-#if HDR
- p /= (16384.0f-black_level);
-#else
- p /= (1024.0f-black_level);
-#endif
-
- // digital gain
- p *= digital_gain;
-
- // use both green channels
-#if BAYER_FLIP == 3
- float3 c1 = (float3)(p.s3, (p.s1+p.s2)/2.0f, p.s0);
-#elif BAYER_FLIP == 2
- float3 c1 = (float3)(p.s2, (p.s0+p.s3)/2.0f, p.s1);
-#elif BAYER_FLIP == 1
- float3 c1 = (float3)(p.s1, (p.s0+p.s3)/2.0f, p.s2);
-#elif BAYER_FLIP == 0
- float3 c1 = (float3)(p.s0, (p.s1+p.s2)/2.0f, p.s3);
-#endif
-
- // color correction
- c1 = color_correct(c1);
-
-#if HDR
- // srgb gamma isn't right for YUV, so it's disabled for now
- c1 = srgb_gamma(c1);
-#endif
-
- // output BGR
- const int ooff = oy * RGB_STRIDE/3 + ox;
- vstore3(convert_uchar3_sat(c1.zyx * 255.0f), ooff+px, out);
- }
- }
-}
diff --git a/selfdrive/camerad/main.cc b/selfdrive/camerad/main.cc
index 668410d6f7..58edaedd9b 100644
--- a/selfdrive/camerad/main.cc
+++ b/selfdrive/camerad/main.cc
@@ -7,11 +7,11 @@
#include "selfdrive/hardware/hw.h"
int main(int argc, char *argv[]) {
- if (!Hardware::PC()) {
+ if (Hardware::TICI()) {
int ret;
ret = util::set_realtime_priority(53);
assert(ret == 0);
- ret = util::set_core_affinity({Hardware::EON() ? 2 : 6});
+ ret = util::set_core_affinity({6});
assert(ret == 0 || Params().getBool("IsOffroad")); // failure ok while offroad due to offlining cores
}
diff --git a/selfdrive/camerad/test/test_camerad.py b/selfdrive/camerad/test/test_camerad.py
index ff37ad1f31..c311c17169 100755
--- a/selfdrive/camerad/test/test_camerad.py
+++ b/selfdrive/camerad/test/test_camerad.py
@@ -4,11 +4,9 @@ import time
import unittest
import cereal.messaging as messaging
+from selfdrive.hardware import TICI
from selfdrive.test.helpers import with_processes
-# only tests for EON and TICI
-from selfdrive.hardware import EON, TICI
-
TEST_TIMESPAN = 30 # random.randint(60, 180) # seconds
SKIP_FRAME_TOLERANCE = 0
LAG_FRAME_TOLERANCE = 2 # ms
@@ -26,7 +24,7 @@ if TICI:
class TestCamerad(unittest.TestCase):
@classmethod
def setUpClass(cls):
- if not (EON or TICI):
+ if not TICI:
raise unittest.SkipTest
@with_processes(['camerad'])
diff --git a/selfdrive/camerad/test/test_exposure.py b/selfdrive/camerad/test/test_exposure.py
index 6ed69627b5..31bcc28681 100755
--- a/selfdrive/camerad/test/test_exposure.py
+++ b/selfdrive/camerad/test/test_exposure.py
@@ -6,7 +6,7 @@ import numpy as np
from selfdrive.test.helpers import with_processes
from selfdrive.camerad.snapshot.snapshot import get_snapshots
-from selfdrive.hardware import EON, TICI
+from selfdrive.hardware import TICI
TEST_TIME = 45
REPEAT = 5
@@ -14,7 +14,7 @@ REPEAT = 5
class TestCamerad(unittest.TestCase):
@classmethod
def setUpClass(cls):
- if not (EON or TICI):
+ if not TICI:
raise unittest.SkipTest
def _numpy_rgb2gray(self, im):
diff --git a/selfdrive/car/body/interface.py b/selfdrive/car/body/interface.py
index 379e89f624..2b44200397 100644
--- a/selfdrive/car/body/interface.py
+++ b/selfdrive/car/body/interface.py
@@ -37,13 +37,9 @@ class CarInterface(CarInterfaceBase):
return ret
- def update(self, c, can_strings):
- self.cp.update_strings(can_strings)
-
+ def _update(self, c):
ret = self.CS.update(self.cp)
- ret.canValid = self.cp.can_valid
-
# wait for everything to init first
if self.frame > int(5. / DT_CTRL):
# body always wants to enable
@@ -52,8 +48,7 @@ class CarInterface(CarInterfaceBase):
ret.events[0].enable = True
self.frame += 1
- self.CS.out = ret.as_reader()
- return self.CS.out
+ return ret
def apply(self, c):
return self.CC.update(c, self.CS)
diff --git a/selfdrive/car/chrysler/interface.py b/selfdrive/car/chrysler/interface.py
index 1bc34dff4d..7751bd151b 100755
--- a/selfdrive/car/chrysler/interface.py
+++ b/selfdrive/car/chrysler/interface.py
@@ -47,16 +47,9 @@ class CarInterface(CarInterfaceBase):
return ret
# returns a car.CarState
- def update(self, c, can_strings):
- # ******************* do can recv *******************
- self.cp.update_strings(can_strings)
- self.cp_cam.update_strings(can_strings)
-
+ def _update(self, c):
ret = self.CS.update(self.cp, self.cp_cam)
- ret.canValid = self.cp.can_valid and self.cp_cam.can_valid
-
- # speeds
ret.steeringRateLimited = self.CC.steer_rate_limited if self.CC is not None else False
# events
@@ -67,10 +60,7 @@ class CarInterface(CarInterfaceBase):
ret.events = events.to_msg()
- # copy back carState packet to CS
- self.CS.out = ret.as_reader()
-
- return self.CS.out
+ return ret
# pass in a car.CarControl
# to be called @ 100hz
diff --git a/selfdrive/car/ford/carcontroller.py b/selfdrive/car/ford/carcontroller.py
index 389d3d8d8b..c2f87a4806 100644
--- a/selfdrive/car/ford/carcontroller.py
+++ b/selfdrive/car/ford/carcontroller.py
@@ -1,86 +1,90 @@
-import math
from cereal import car
-from selfdrive.car import make_can_msg
-from selfdrive.car.ford.fordcan import create_steer_command, create_lkas_ui, spam_cancel_button
+from common.numpy_fast import clip, interp
+from selfdrive.car.ford import fordcan
+from selfdrive.car.ford.values import CarControllerParams
from opendbc.can.packer import CANPacker
VisualAlert = car.CarControl.HUDControl.VisualAlert
-MAX_STEER_DELTA = 1
-TOGGLE_DEBUG = False
+
+def apply_ford_steer_angle_limits(apply_steer, apply_steer_last, vEgo):
+ # rate limit
+ steer_up = apply_steer * apply_steer_last > 0. and abs(apply_steer) > abs(apply_steer_last)
+ rate_limit = CarControllerParams.STEER_RATE_LIMIT_UP if steer_up else CarControllerParams.STEER_RATE_LIMIT_DOWN
+ max_angle_diff = interp(vEgo, rate_limit.speed_points, rate_limit.max_angle_diff_points)
+ apply_steer = clip(apply_steer, (apply_steer_last - max_angle_diff), (apply_steer_last + max_angle_diff))
+
+ return apply_steer
+
class CarController():
def __init__(self, dbc_name, CP, VM):
+ self.CP = CP
+ self.VM = VM
self.packer = CANPacker(dbc_name)
- self.enabled_last = False
+
+ self.apply_steer_last = 0
+ self.steer_rate_limited = False
self.main_on_last = False
- self.vehicle_model = VM
- self.generic_toggle_last = 0
+ self.lkas_enabled_last = False
self.steer_alert_last = False
- self.lkas_action = 0
-
- def update(self, enabled, CS, frame, actuators, visual_alert, pcm_cancel):
+ def update(self, CC, CS, frame):
can_sends = []
- steer_alert = visual_alert in (VisualAlert.steerRequired, VisualAlert.ldw)
-
- apply_steer = actuators.steer
- if pcm_cancel:
- #print "CANCELING!!!!"
- can_sends.append(spam_cancel_button(self.packer))
+ actuators = CC.actuators
+ hud_control = CC.hudControl
- if (frame % 3) == 0:
+ main_on = CS.out.cruiseState.available
+ steer_alert = hud_control.visualAlert in (VisualAlert.steerRequired, VisualAlert.ldw)
- curvature = self.vehicle_model.calc_curvature(math.radians(actuators.steeringAngleDeg), CS.out.vEgo, 0.0)
+ if CC.cruiseControl.cancel:
+ # cancel stock ACC
+ can_sends.append(fordcan.spam_cancel_button(self.packer))
- # The use of the toggle below is handy for trying out the various LKAS modes
- if TOGGLE_DEBUG:
- self.lkas_action += int(CS.out.genericToggle and not self.generic_toggle_last)
- self.lkas_action &= 0xf
- else:
- self.lkas_action = 5 # 4 and 5 seem the best. 8 and 9 seem to aggressive and laggy
+ # apply rate limits
+ new_steer = actuators.steeringAngleDeg
+ apply_steer = apply_ford_steer_angle_limits(new_steer, self.apply_steer_last, CS.out.vEgo)
+ self.steer_rate_limited = new_steer != apply_steer
- can_sends.append(create_steer_command(self.packer, apply_steer, enabled,
- CS.lkas_state, CS.out.steeringAngleDeg, curvature, self.lkas_action))
- self.generic_toggle_last = CS.out.genericToggle
+ # send steering commands at 20Hz
+ if (frame % CarControllerParams.LKAS_STEER_STEP) == 0:
+ lca_rq = 1 if CC.latActive else 0
- if (frame % 100) == 0:
+ # use LatCtlPath_An_Actl to actuate steering for now until curvature control is implemented
+ path_angle = apply_steer
- can_sends.append(make_can_msg(973, b'\x00\x00\x00\x00\x00\x00\x00\x00', 0))
- #can_sends.append(make_can_msg(984, b'\x00\x00\x00\x00\x80\x45\x60\x30', 0))
+ # convert steer angle to curvature
+ curvature = self.VM.calc_curvature(apply_steer, CS.out.vEgo, 0.0)
- if (frame % 100) == 0 or (self.enabled_last != enabled) or (self.main_on_last != CS.out.cruiseState.available) or \
- (self.steer_alert_last != steer_alert):
- can_sends.append(create_lkas_ui(self.packer, CS.out.cruiseState.available, enabled, steer_alert))
+ # TODO: get other actuators
+ curvature_rate = 0
+ path_offset = 0
- if (frame % 200) == 0:
- can_sends.append(make_can_msg(1875, b'\x80\xb0\x55\x55\x78\x90\x00\x00', 1))
+ ramp_type = 3 # 0=Slow, 1=Medium, 2=Fast, 3=Immediately
+ precision = 0 # 0=Comfortable, 1=Precise
- if (frame % 10) == 0:
+ self.apply_steer_last = apply_steer
+ can_sends.append(fordcan.create_lkas_command(self.packer, apply_steer, curvature))
+ can_sends.append(fordcan.create_tja_command(self.packer, lca_rq, ramp_type, precision,
+ path_offset, path_angle, curvature_rate, curvature))
- can_sends.append(make_can_msg(1648, b'\x00\x00\x00\x40\x00\x00\x50\x00', 1))
- can_sends.append(make_can_msg(1649, b'\x10\x10\xf1\x70\x04\x00\x00\x00', 1))
- can_sends.append(make_can_msg(1664, b'\x00\x00\x03\xe8\x00\x01\xa9\xb2', 1))
- can_sends.append(make_can_msg(1674, b'\x08\x00\x00\xff\x0c\xfb\x6a\x08', 1))
- can_sends.append(make_can_msg(1675, b'\x00\x00\x3b\x60\x37\x00\x00\x00', 1))
- can_sends.append(make_can_msg(1690, b'\x70\x00\x00\x55\x86\x1c\xe0\x00', 1))
+ send_ui = (self.main_on_last != main_on) or (self.lkas_enabled_last != CC.latActive) or (self.steer_alert_last != steer_alert)
- can_sends.append(make_can_msg(1910, b'\x06\x4b\x06\x4b\x42\xd3\x11\x30', 1))
- can_sends.append(make_can_msg(1911, b'\x48\x53\x37\x54\x48\x53\x37\x54', 1))
- can_sends.append(make_can_msg(1912, b'\x31\x34\x47\x30\x38\x31\x43\x42', 1))
- can_sends.append(make_can_msg(1913, b'\x31\x34\x47\x30\x38\x32\x43\x42', 1))
- can_sends.append(make_can_msg(1969, b'\xf4\x40\x00\x00\x00\x00\x00\x00', 1))
- can_sends.append(make_can_msg(1971, b'\x0b\xc0\x00\x00\x00\x00\x00\x00', 1))
+ # send lkas ui command at 1Hz or if ui state changes
+ if (frame % CarControllerParams.LKAS_UI_STEP) == 0 or send_ui:
+ can_sends.append(fordcan.create_lkas_ui_command(self.packer, main_on, CC.latActive, steer_alert, CS.lkas_status_stock_values))
- static_msgs = range(1653, 1658)
- for addr in static_msgs:
- cnt = (frame % 10) + 1
- can_sends.append(make_can_msg(addr, (cnt << 4).to_bytes(1, 'little') + b'\x00\x00\x00\x00\x00\x00\x00', 1))
+ # send acc ui command at 20Hz or if ui state changes
+ if (frame % CarControllerParams.ACC_UI_STEP) == 0 or send_ui:
+ can_sends.append(fordcan.create_acc_ui_command(self.packer, main_on, CC.latActive, CS.acc_tja_status_stock_values))
- self.enabled_last = enabled
- self.main_on_last = CS.out.cruiseState.available
+ self.main_on_last = main_on
+ self.lkas_enabled_last = CC.latActive
self.steer_alert_last = steer_alert
- return actuators, can_sends
+ new_actuators = actuators.copy()
+ new_actuators.steeringAngleDeg = apply_steer
+
+ return new_actuators, can_sends
diff --git a/selfdrive/car/ford/carstate.py b/selfdrive/car/ford/carstate.py
index ff82e585b4..67a72fd67c 100644
--- a/selfdrive/car/ford/carstate.py
+++ b/selfdrive/car/ford/carstate.py
@@ -1,58 +1,225 @@
+from typing import Dict
+
from cereal import car
from common.conversions import Conversions as CV
-from common.numpy_fast import mean
+from opendbc.can.can_define import CANDefine
from opendbc.can.parser import CANParser
from selfdrive.car.interfaces import CarStateBase
-from selfdrive.car.ford.values import DBC
+from selfdrive.car.ford.values import CANBUS, DBC
+
+GearShifter = car.CarState.GearShifter
+TransmissionType = car.CarParams.TransmissionType
-WHEEL_RADIUS = 0.33
class CarState(CarStateBase):
- def update(self, cp):
+ def __init__(self, CP):
+ super().__init__(CP)
+ can_define = CANDefine(DBC[CP.carFingerprint]["pt"])
+ if CP.transmissionType == TransmissionType.automatic:
+ self.shifter_values = can_define.dv["Gear_Shift_by_Wire_FD1"]["TrnGear_D_RqDrv"]
+
+ def update(self, cp, cp_cam):
ret = car.CarState.new_message()
- ret.wheelSpeeds = self.get_wheel_speeds(
- cp.vl["WheelSpeed_CG1"]["WhlFl_W_Meas"],
- cp.vl["WheelSpeed_CG1"]["WhlFr_W_Meas"],
- cp.vl["WheelSpeed_CG1"]["WhlRl_W_Meas"],
- cp.vl["WheelSpeed_CG1"]["WhlRr_W_Meas"],
- unit=WHEEL_RADIUS,
- )
- ret.vEgoRaw = mean([ret.wheelSpeeds.rr, ret.wheelSpeeds.rl, ret.wheelSpeeds.fr, ret.wheelSpeeds.fl])
+ # car speed
+ ret.vEgoRaw = cp.vl["EngVehicleSpThrottle2"]["Veh_V_ActlEng"] * CV.KPH_TO_MS
ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw)
- ret.standstill = not ret.vEgoRaw > 0.001
- ret.steeringAngleDeg = cp.vl["Steering_Wheel_Data_CG1"]["SteWhlRelInit_An_Sns"]
- ret.steeringPressed = not cp.vl["Lane_Keep_Assist_Status"]["LaHandsOff_B_Actl"]
- ret.steerFaultPermanent = cp.vl["Lane_Keep_Assist_Status"]["LaActDeny_B_Actl"] == 1
- ret.cruiseState.speed = cp.vl["Cruise_Status"]["Set_Speed"] * CV.MPH_TO_MS
- ret.cruiseState.enabled = not (cp.vl["Cruise_Status"]["Cruise_State"] in (0, 3))
- ret.cruiseState.available = cp.vl["Cruise_Status"]["Cruise_State"] != 0
- ret.gas = cp.vl["EngineData_14"]["ApedPosScal_Pc_Actl"] / 100.
+ ret.yawRate = cp.vl["Yaw_Data_FD1"]["VehYaw_W_Actl"] * CV.RAD_TO_DEG
+ ret.standstill = cp.vl["DesiredTorqBrk"]["VehStop_D_Stat"] == 1
+
+ # gas pedal
+ ret.gas = cp.vl["EngVehicleSpThrottle"]["ApedPos_Pc_ActlArb"] / 100.
ret.gasPressed = ret.gas > 1e-6
- ret.brakePressed = bool(cp.vl["Cruise_Status"]["Brake_Drv_Appl"])
- ret.genericToggle = bool(cp.vl["Steering_Buttons"]["Dist_Incr"])
- # TODO: we also need raw driver torque, needed for Assisted Lane Change
- self.lkas_state = cp.vl["Lane_Keep_Assist_Status"]["LaActAvail_D_Actl"]
+
+ # brake pedal
+ ret.brake = cp.vl["BrakeSnData_4"]["BrkTot_Tq_Actl"] / 32756. # torque in Nm
+ ret.brakePressed = cp.vl["EngBrakeData"]["BpedDrvAppl_D_Actl"] == 2
+ ret.parkingBrake = cp.vl["DesiredTorqBrk"]["PrkBrkStatus"] in (1, 2)
+
+ # steering wheel
+ ret.steeringAngleDeg = cp.vl["SteeringPinion_Data"]["StePinComp_An_Est"]
+ ret.steeringTorque = cp.vl["EPAS_INFO"]["SteeringColumnTorque"]
+ ret.steeringPressed = cp.vl["Lane_Assist_Data3_FD1"]["LaHandsOff_B_Actl"] == 0
+ ret.steerFaultTemporary = cp.vl["EPAS_INFO"]["EPAS_Failure"] == 1
+ ret.steerFaultPermanent = cp.vl["EPAS_INFO"]["EPAS_Failure"] in (2, 3)
+ # ret.espDisabled = False # TODO: find traction control signal
+
+ # cruise state
+ ret.cruiseState.speed = cp.vl["EngBrakeData"]["Veh_V_DsplyCcSet"] * CV.MPH_TO_MS
+ ret.cruiseState.enabled = cp.vl["EngBrakeData"]["CcStat_D_Actl"] in (4, 5)
+ ret.cruiseState.available = cp.vl["EngBrakeData"]["CcStat_D_Actl"] in (3, 4, 5)
+
+ # gear
+ if self.CP.transmissionType == TransmissionType.automatic:
+ gear = int(cp.vl["Gear_Shift_by_Wire_FD1"]["TrnGear_D_RqDrv"])
+ ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(gear, None))
+ elif self.CP.transmissionType == TransmissionType.manual:
+ ret.clutchPressed = cp.vl["Engine_Clutch_Data"]["CluPdlPos_Pc_Meas"] > 0
+ if bool(cp.vl["BCM_Lamp_Stat_FD1"]["RvrseLghtOn_B_Stat"]):
+ ret.gearShifter = GearShifter.reverse
+ else:
+ ret.gearShifter = GearShifter.drive
+
+ # safety
+ ret.stockFcw = bool(cp_cam.vl["ACCDATA_3"]["FcwVisblWarn_B_Rq"])
+ ret.stockAeb = ret.stockFcw and ret.cruiseState.enabled
+
+ # button presses
+ ret.leftBlinker = cp.vl["Steering_Data_FD1"]["TurnLghtSwtch_D_Stat"] == 1
+ ret.rightBlinker = cp.vl["Steering_Data_FD1"]["TurnLghtSwtch_D_Stat"] == 2
+ ret.genericToggle = bool(cp.vl["Steering_Data_FD1"]["TjaButtnOnOffPress"])
+
+ # lock info
+ ret.doorOpen = any([cp.vl["BodyInfo_3_FD1"]["DrStatDrv_B_Actl"], cp.vl["BodyInfo_3_FD1"]["DrStatPsngr_B_Actl"],
+ cp.vl["BodyInfo_3_FD1"]["DrStatRl_B_Actl"], cp.vl["BodyInfo_3_FD1"]["DrStatRr_B_Actl"]])
+ ret.seatbeltUnlatched = cp.vl["RCMStatusMessage2_FD1"]["FirstRowBuckleDriver"] == 2
+
+ # blindspot sensors
+ if self.CP.enableBsm:
+ ret.leftBlindspot = cp.vl["Side_Detect_L_Stat"]["SodDetctLeft_D_Stat"] != 0
+ ret.rightBlindspot = cp.vl["Side_Detect_R_Stat"]["SodDetctRight_D_Stat"] != 0
+
+ # Stock values from IPMA so that we can retain some stock functionality
+ self.acc_tja_status_stock_values = cp_cam.vl["ACCDATA_3"]
+ self.lkas_status_stock_values = cp_cam.vl["IPMA_Data"]
return ret
+ @staticmethod
+ def parse_gear_shifter(gear: str) -> car.CarState.GearShifter:
+ d: Dict[str, car.CarState.GearShifter] = {
+ 'Park': GearShifter.park, 'Reverse': GearShifter.reverse, 'Neutral': GearShifter.neutral,
+ 'Manual': GearShifter.manumatic, 'Drive': GearShifter.drive,
+ }
+ return d.get(gear, GearShifter.unknown)
+
@staticmethod
def get_can_parser(CP):
signals = [
# sig_name, sig_address
- ("WhlRr_W_Meas", "WheelSpeed_CG1"),
- ("WhlRl_W_Meas", "WheelSpeed_CG1"),
- ("WhlFr_W_Meas", "WheelSpeed_CG1"),
- ("WhlFl_W_Meas", "WheelSpeed_CG1"),
- ("SteWhlRelInit_An_Sns", "Steering_Wheel_Data_CG1"),
- ("Cruise_State", "Cruise_Status"),
- ("Set_Speed", "Cruise_Status"),
- ("LaActAvail_D_Actl", "Lane_Keep_Assist_Status"),
- ("LaHandsOff_B_Actl", "Lane_Keep_Assist_Status"),
- ("LaActDeny_B_Actl", "Lane_Keep_Assist_Status"),
- ("ApedPosScal_Pc_Actl", "EngineData_14"),
- ("Dist_Incr", "Steering_Buttons"),
- ("Brake_Drv_Appl", "Cruise_Status"),
+ ("Veh_V_ActlEng", "EngVehicleSpThrottle2"), # ABS vehicle speed (kph)
+ ("VehYaw_W_Actl", "Yaw_Data_FD1"), # ABS vehicle yaw rate (rad/s)
+ ("VehStop_D_Stat", "DesiredTorqBrk"), # ABS vehicle stopped
+ ("PrkBrkStatus", "DesiredTorqBrk"), # ABS park brake status
+ ("ApedPos_Pc_ActlArb", "EngVehicleSpThrottle"), # PCM throttle (pct)
+ ("BrkTot_Tq_Actl", "BrakeSnData_4"), # ABS brake torque (Nm)
+ ("BpedDrvAppl_D_Actl", "EngBrakeData"), # PCM driver brake pedal pressed
+ ("Veh_V_DsplyCcSet", "EngBrakeData"), # PCM ACC set speed (mph)
+ # The units might change with IPC settings?
+ ("CcStat_D_Actl", "EngBrakeData"), # PCM ACC status
+ ("StePinComp_An_Est", "SteeringPinion_Data"), # PSCM estimated steering angle (deg)
+ # Calculates steering angle (and offset) from pinion
+ # angle and driving measurements.
+ # StePinRelInit_An_Sns is the pinion angle, initialised
+ # to zero at the beginning of the drive.
+ ("SteeringColumnTorque", "EPAS_INFO"), # PSCM steering column torque (Nm)
+ ("EPAS_Failure", "EPAS_INFO"), # PSCM EPAS status
+ ("LaHandsOff_B_Actl", "Lane_Assist_Data3_FD1"), # PSCM LKAS hands off wheel
+ ("TurnLghtSwtch_D_Stat", "Steering_Data_FD1"), # SCCM Turn signal switch
+ ("TjaButtnOnOffPress", "Steering_Data_FD1"), # SCCM ACC button, lane-centering/traffic jam assist toggle
+ ("DrStatDrv_B_Actl", "BodyInfo_3_FD1"), # BCM Door open, driver
+ ("DrStatPsngr_B_Actl", "BodyInfo_3_FD1"), # BCM Door open, passenger
+ ("DrStatRl_B_Actl", "BodyInfo_3_FD1"), # BCM Door open, rear left
+ ("DrStatRr_B_Actl", "BodyInfo_3_FD1"), # BCM Door open, rear right
+ ("FirstRowBuckleDriver", "RCMStatusMessage2_FD1"), # RCM Seatbelt status, driver
]
- checks = []
- return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, 0, enforce_checks=False)
+
+ checks = [
+ # sig_address, frequency
+ ("EngVehicleSpThrottle2", 50),
+ ("Yaw_Data_FD1", 100),
+ ("DesiredTorqBrk", 50),
+ ("EngVehicleSpThrottle", 100),
+ ("BrakeSnData_4", 50),
+ ("EngBrakeData", 10),
+ ("SteeringPinion_Data", 100),
+ ("EPAS_INFO", 50),
+ ("Lane_Assist_Data3_FD1", 33),
+ ("Steering_Data_FD1", 10),
+ ("BodyInfo_3_FD1", 2),
+ ("RCMStatusMessage2_FD1", 10),
+ ]
+
+ if CP.transmissionType == TransmissionType.automatic:
+ signals += [
+ ("TrnGear_D_RqDrv", "Gear_Shift_by_Wire_FD1"), # GWM transmission gear position
+ ]
+ checks += [
+ ("Gear_Shift_by_Wire_FD1", 10),
+ ]
+ elif CP.transmissionType == TransmissionType.manual:
+ signals += [
+ ("CluPdlPos_Pc_Meas", "Engine_Clutch_Data"), # PCM clutch (pct)
+ ("RvrseLghtOn_B_Stat", "BCM_Lamp_Stat_FD1"), # BCM reverse light
+ ]
+ checks += [
+ ("Engine_Clutch_Data", 33),
+ ("BCM_Lamp_Stat_FD1", 1),
+ ]
+
+ if CP.enableBsm:
+ signals += [
+ ("SodDetctLeft_D_Stat", "Side_Detect_L_Stat"), # Blindspot sensor, left
+ ("SodDetctRight_D_Stat", "Side_Detect_R_Stat"), # Blindspot sensor, right
+ ]
+ checks += [
+ ("Side_Detect_L_Stat", 5),
+ ("Side_Detect_R_Stat", 5),
+ ]
+
+ return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, CANBUS.main)
+
+ @staticmethod
+ def get_cam_can_parser(CP):
+ signals = [
+ # sig_name, sig_address
+ ("HaDsply_No_Cs", "ACCDATA_3"),
+ ("HaDsply_No_Cnt", "ACCDATA_3"),
+ ("AccStopStat_D_Dsply", "ACCDATA_3"), # ACC stopped status message
+ ("AccTrgDist2_D_Dsply", "ACCDATA_3"), # ACC target distance
+ ("AccStopRes_B_Dsply", "ACCDATA_3"),
+ ("TjaWarn_D_Rq", "ACCDATA_3"), # TJA warning
+ ("Tja_D_Stat", "ACCDATA_3"), # TJA status
+ ("TjaMsgTxt_D_Dsply", "ACCDATA_3"), # TJA text
+ ("IaccLamp_D_Rq", "ACCDATA_3"), # iACC status icon
+ ("AccMsgTxt_D2_Rq", "ACCDATA_3"), # ACC text
+ ("FcwDeny_B_Dsply", "ACCDATA_3"), # FCW disabled
+ ("FcwMemStat_B_Actl", "ACCDATA_3"), # FCW enabled setting
+ ("AccTGap_B_Dsply", "ACCDATA_3"), # ACC time gap display setting
+ ("CadsAlignIncplt_B_Actl", "ACCDATA_3"),
+ ("AccFllwMde_B_Dsply", "ACCDATA_3"), # ACC follow mode display setting
+ ("CadsRadrBlck_B_Actl", "ACCDATA_3"),
+ ("CmbbPostEvnt_B_Dsply", "ACCDATA_3"), # AEB event status
+ ("AccStopMde_B_Dsply", "ACCDATA_3"), # ACC stop mode display setting
+ ("FcwMemSens_D_Actl", "ACCDATA_3"), # FCW sensitivity setting
+ ("FcwMsgTxt_D_Rq", "ACCDATA_3"), # FCW text
+ ("AccWarn_D_Dsply", "ACCDATA_3"), # ACC warning
+ ("FcwVisblWarn_B_Rq", "ACCDATA_3"), # FCW visible alert
+ ("FcwAudioWarn_B_Rq", "ACCDATA_3"), # FCW audio alert
+ ("AccTGap_D_Dsply", "ACCDATA_3"), # ACC time gap
+ ("AccMemEnbl_B_RqDrv", "ACCDATA_3"), # ACC adaptive/normal setting
+ ("FdaMem_B_Stat", "ACCDATA_3"), # FDA enabled setting
+
+ ("FeatConfigIpmaActl", "IPMA_Data"),
+ ("FeatNoIpmaActl", "IPMA_Data"),
+ ("PersIndexIpma_D_Actl", "IPMA_Data"),
+ ("AhbcRampingV_D_Rq", "IPMA_Data"), # AHB ramping
+ ("LaActvStats_D_Dsply", "IPMA_Data"), # LKAS status (lines)
+ ("LaDenyStats_B_Dsply", "IPMA_Data"), # LKAS error
+ ("LaHandsOff_D_Dsply", "IPMA_Data"), # LKAS hands on chime
+ ("CamraDefog_B_Req", "IPMA_Data"), # Windshield heater?
+ ("CamraStats_D_Dsply", "IPMA_Data"), # Camera status
+ ("DasAlrtLvl_D_Dsply", "IPMA_Data"), # DAS alert level
+ ("DasStats_D_Dsply", "IPMA_Data"), # DAS status
+ ("DasWarn_D_Dsply", "IPMA_Data"), # DAS warning
+ ("AhbHiBeam_D_Rq", "IPMA_Data"), # AHB status
+ ("Set_Me_X1", "IPMA_Data"),
+ ]
+
+ checks = [
+ # sig_address, frequency
+ ("ACCDATA_3", 5),
+ ("IPMA_Data", 1),
+ ]
+
+ return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, CANBUS.camera)
diff --git a/selfdrive/car/ford/fordcan.py b/selfdrive/car/ford/fordcan.py
index 5a8b0b2dec..29fec1f2de 100644
--- a/selfdrive/car/ford/fordcan.py
+++ b/selfdrive/car/ford/fordcan.py
@@ -1,50 +1,144 @@
from common.numpy_fast import clip
-from selfdrive.car.ford.values import MAX_ANGLE
-def create_steer_command(packer, angle_cmd, enabled, lkas_state, angle_steers, curvature, lkas_action):
- """Creates a CAN message for the Ford Steer Command."""
+def create_lkas_command(packer, angle_deg: float, curvature: float):
+ """
+ Creates a CAN message for the Ford LKAS Command.
- #if enabled and lkas available:
- if enabled and lkas_state in (2, 3): # and (frame % 500) >= 3:
- action = lkas_action
- else:
- action = 0xf
- angle_cmd = angle_steers/MAX_ANGLE
+ This command can apply "Lane Keeping Aid" manoeuvres, which are subject to the
+ PSCM lockout.
- angle_cmd = clip(angle_cmd * MAX_ANGLE, - MAX_ANGLE, MAX_ANGLE)
+ Frequency is 20Hz.
+ """
values = {
- "Lkas_Action": action,
- "Lkas_Alert": 0xf, # no alerts
- "Lane_Curvature": clip(curvature, -0.01, 0.01), # is it just for debug?
- #"Lane_Curvature": 0, # is it just for debug?
- "Steer_Angle_Req": angle_cmd
+ "LkaDrvOvrrd_D_Rq": 0, # driver override level? [0|3]
+ "LkaActvStats_D2_Req": 0, # action [0|7]
+ "LaRefAng_No_Req": angle_deg, # angle [-102.4|102.3] degrees
+ "LaRampType_B_Req": 0, # Ramp speed: 0=Smooth, 1=Quick
+ "LaCurvature_No_Calc": curvature, # curvature [-0.01024|0.01023] 1/meter
+ "LdwActvStats_D_Req": 0, # LDW status [0|7]
+ "LdwActvIntns_D_Req": 0, # LDW intensity [0|3], shake alert strength
}
- return packer.make_can_msg("Lane_Keep_Assist_Control", 0, values)
+ return packer.make_can_msg("Lane_Assist_Data1", 0, values)
+
+
+def create_tja_command(packer, lca_rq: int, ramp_type: int, precision: int, path_offset: float, path_angle: float, curvature_rate: float, curvature: float):
+ """
+ Creates a CAN message for the Ford TJA/LCA Command.
+
+ This command can apply "Lane Centering" manoeuvres: continuous lane centering
+ for traffic jam assist and highway driving. It is not subject to the PSCM
+ lockout.
+
+ The PSCM should be configured to accept TJA/LCA commands before these
+ commands will be processed. This can be done using tools such as Forscan.
+
+ Frequency is 20Hz.
+ """
+
+ values = {
+ "LatCtlRng_L_Max": 0, # Unknown [0|126] meter
+ "HandsOffCnfm_B_Rq": 0, # Unknown: 0=Inactive, 1=Active [0|1]
+ "LatCtl_D_Rq": lca_rq, # Mode: 0=None, 1=ContinuousPathFollowing, 2=InterventionLeft, 3=InterventionRight, 4-7=NotUsed [0|7]
+ "LatCtlRampType_D_Rq": ramp_type, # Ramp speed: 0=Slow, 1=Medium, 2=Fast, 3=Immediate [0|3]
+ "LatCtlPrecision_D_Rq": precision, # Precision: 0=Comfortable, 1=Precise, 2/3=NotUsed [0|3]
+ "LatCtlPathOffst_L_Actl": clip(path_offset, -5.12, 5.11), # Path offset [-5.12|5.11] meter
+ "LatCtlPath_An_Actl": clip(path_angle, -0.5, 0.5235), # Path angle [-0.5|0.5235] radians
+ "LatCtlCurv_NoRate_Actl": clip(curvature_rate, -0.001024, 0.00102375), # Curvature rate [-0.001024|0.00102375] 1/meter^2
+ "LatCtlCurv_No_Actl": clip(curvature, -0.02, 0.02094), # Curvature [-0.02|0.02094] 1/meter
+ }
+ return packer.make_can_msg("LateralMotionControl", 0, values)
+
+
+def create_lkas_ui_command(packer, main_on: bool, enabled: bool, steer_alert: bool, stock_values):
+ """
+ Creates a CAN message for the Ford IPC IPMA/LKAS status.
+ Show the LKAS status with the "driver assist" lines in the IPC.
-def create_lkas_ui(packer, main_on, enabled, steer_alert):
- """Creates a CAN message for the Ford Steer Ui."""
+ Stock functionality is maintained by passing through unmodified signals.
- if not main_on:
- lines = 0xf
- elif enabled:
- lines = 0x3
+ Frequency is 1Hz.
+ """
+
+ # LaActvStats_D_Dsply
+ # TODO: get LDW state from OP
+ if enabled:
+ lines = 6
+ elif main_on:
+ lines = 0
else:
- lines = 0x6
+ lines = 30
values = {
- "Set_Me_X80": 0x80,
- "Set_Me_X45": 0x45,
- "Set_Me_X30": 0x30,
- "Lines_Hud": lines,
- "Hands_Warning_W_Chime": steer_alert,
+ "FeatConfigIpmaActl": stock_values["FeatConfigIpmaActl"], # [0|65535]
+ "FeatNoIpmaActl": stock_values["FeatNoIpmaActl"], # [0|65535]
+ "PersIndexIpma_D_Actl": stock_values["PersIndexIpma_D_Actl"], # [0|7]
+ "AhbcRampingV_D_Rq": stock_values["AhbcRampingV_D_Rq"], # AHB ramping [0|3]
+ "LaActvStats_D_Dsply": lines, # LKAS status (lines) [0|31]
+ "LaDenyStats_B_Dsply": stock_values["LaDenyStats_B_Dsply"], # LKAS error [0|1]
+ "LaHandsOff_D_Dsply": 2 if steer_alert else 0, # 0=HandsOn, 1=Level1 (w/o chime), 2=Level2 (w/ chime), 3=Suppressed
+ "CamraDefog_B_Req": stock_values["CamraDefog_B_Req"], # Windshield heater? [0|1]
+ "CamraStats_D_Dsply": stock_values["CamraStats_D_Dsply"], # Camera status [0|3]
+ "DasAlrtLvl_D_Dsply": stock_values["DasAlrtLvl_D_Dsply"], # DAS alert level [0|7]
+ "DasStats_D_Dsply": stock_values["DasStats_D_Dsply"], # DAS status [0|3]
+ "DasWarn_D_Dsply": stock_values["DasWarn_D_Dsply"], # DAS warning [0|3]
+ "AhbHiBeam_D_Rq": stock_values["AhbHiBeam_D_Rq"], # AHB status [0|3]
+ "Set_Me_X1": stock_values["Set_Me_X1"], # [0|15]
}
- return packer.make_can_msg("Lane_Keep_Assist_Ui", 0, values)
+ return packer.make_can_msg("IPMA_Data", 0, values)
+
+
+def create_acc_ui_command(packer, main_on: bool, enabled: bool, stock_values):
+ """
+ Creates a CAN message for the Ford IPC adaptive cruise, forward collision
+ warning and traffic jam assist status.
+
+ Stock functionality is maintained by passing through unmodified signals.
+
+ Frequency is 20Hz.
+ """
+
+ values = {
+ "HaDsply_No_Cs": stock_values["HaDsply_No_Cs"], # [0|255]
+ "HaDsply_No_Cnt": stock_values["HaDsply_No_Cnt"], # [0|15]
+ "AccStopStat_D_Dsply": stock_values["AccStopStat_D_Dsply"], # ACC stopped status message: 0=NoDisplay, 1=ResumeReady, 2=Stopped, 3=PressResume [0|3]
+ "AccTrgDist2_D_Dsply": stock_values["AccTrgDist2_D_Dsply"], # ACC target distance [0|15]
+ "AccStopRes_B_Dsply": stock_values["AccStopRes_B_Dsply"], # [0|1]
+ "TjaWarn_D_Rq": stock_values["TjaWarn_D_Rq"], # TJA warning: 0=NoWarning, 1=Cancel, 2=HardTakeOverLevel1, 3=HardTakeOverLevel2 [0|7]
+ "Tja_D_Stat": 2 if enabled else (1 if main_on else 0), # TJA status: 0=Off, 1=Standby, 2=Active, 3=InterventionLeft, 4=InterventionRight, 5=WarningLeft, 6=WarningRight, 7=NotUsed [0|7]
+ "TjaMsgTxt_D_Dsply": stock_values["TjaMsgTxt_D_Dsply"], # TJA text [0|7]
+ "IaccLamp_D_Rq": stock_values["IaccLamp_D_Rq"], # iACC status icon [0|3]
+ "AccMsgTxt_D2_Rq": stock_values["AccMsgTxt_D2_Rq"], # ACC text [0|15]
+ "FcwDeny_B_Dsply": stock_values["FcwDeny_B_Dsply"], # FCW disabled [0|1]
+ "FcwMemStat_B_Actl": stock_values["FcwMemStat_B_Actl"], # FCW enabled setting [0|1]
+ "AccTGap_B_Dsply": stock_values["AccTGap_B_Dsply"], # ACC time gap display setting [0|1]
+ "CadsAlignIncplt_B_Actl": stock_values["CadsAlignIncplt_B_Actl"], # Radar alignment? [0|1]
+ "AccFllwMde_B_Dsply": stock_values["AccFllwMde_B_Dsply"], # ACC follow mode display setting [0|1]
+ "CadsRadrBlck_B_Actl": stock_values["CadsRadrBlck_B_Actl"], # Radar blocked? [0|1]
+ "CmbbPostEvnt_B_Dsply": stock_values["CmbbPostEvnt_B_Dsply"], # AEB event status [0|1]
+ "AccStopMde_B_Dsply": stock_values["AccStopMde_B_Dsply"], # ACC stop mode display setting [0|1]
+ "FcwMemSens_D_Actl": stock_values["FcwMemSens_D_Actl"], # FCW sensitivity setting [0|3]
+ "FcwMsgTxt_D_Rq": stock_values["FcwMsgTxt_D_Rq"], # FCW text [0|7]
+ "AccWarn_D_Dsply": stock_values["AccWarn_D_Dsply"], # ACC warning [0|3]
+ "FcwVisblWarn_B_Rq": stock_values["FcwVisblWarn_B_Rq"], # FCW alert: 0=Off, 1=On [0|1]
+ "FcwAudioWarn_B_Rq": stock_values["FcwAudioWarn_B_Rq"], # FCW audio: 0=Off, 1=On [0|1]
+ "AccTGap_D_Dsply": stock_values["AccTGap_D_Dsply"], # ACC time gap: 1=Time_Gap_1, 2=Time_Gap_2, ..., 5=Time_Gap_5 [0|7]
+ "AccMemEnbl_B_RqDrv": stock_values["AccMemEnbl_B_RqDrv"], # ACC setting: 0=NormalCruise, 1=AdaptiveCruise [0|1]
+ "FdaMem_B_Stat": stock_values["FdaMem_B_Stat"], # FDA enabled setting [0|1]
+ }
+ return packer.make_can_msg("ACCDATA_3", 0, values)
+
+
+def spam_cancel_button(packer, cancel=1):
+ """
+ Creates a CAN message for the Ford SCCM buttons/switches.
+
+ Includes cruise control buttons, turn lights and more.
+ """
-def spam_cancel_button(packer):
values = {
- "Cancel": 1
+ "CcAslButtnCnclPress": cancel, # CC cancel button
}
- return packer.make_can_msg("Steering_Buttons", 0, values)
+ return packer.make_can_msg("Steering_Data_FD1", 0, values)
diff --git a/selfdrive/car/ford/interface.py b/selfdrive/car/ford/interface.py
old mode 100755
new mode 100644
index 3b15f03c91..ec7c723669
--- a/selfdrive/car/ford/interface.py
+++ b/selfdrive/car/ford/interface.py
@@ -1,72 +1,84 @@
#!/usr/bin/env python3
from cereal import car
from common.conversions import Conversions as CV
-from selfdrive.car.ford.values import MAX_ANGLE
-from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint, get_safety_config
+from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint
+from selfdrive.car.ford.values import TransmissionType, CAR
from selfdrive.car.interfaces import CarInterfaceBase
+EventName = car.CarEvent.EventName
+
+
class CarInterface(CarInterfaceBase):
@staticmethod
def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=None, disable_radar=False):
ret = CarInterfaceBase.get_std_params(candidate, fingerprint)
+
ret.carName = "ford"
- ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.ford)]
+ #ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.ford)]
ret.dashcamOnly = True
- ret.wheelbase = 2.85
- ret.steerRatio = 14.8
- ret.mass = 3045. * CV.LB_TO_KG + STD_CARGO_KG
- ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]]
- ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.01], [0.005]] # TODO: tune this
- ret.lateralTuning.pid.kf = 1. / MAX_ANGLE # MAX Steer angle to normalize FF
- ret.steerActuatorDelay = 0.1 # Default delay, not measured yet
+ # Angle-based steering
+ # TODO: use curvature control when ready
+ ret.steerControlType = car.CarParams.SteerControlType.angle
+ ret.steerActuatorDelay = 0.1
ret.steerLimitTimer = 1.0
+
+ # TODO: detect stop-and-go vehicles
+ stop_and_go = False
+
+ if candidate == CAR.ESCAPE_MK4:
+ ret.wheelbase = 2.71
+ ret.steerRatio = 14.3 # Copied from Focus
+ tire_stiffness_factor = 0.5328 # Copied from Focus
+ ret.mass = 1750 + STD_CARGO_KG
+
+ elif candidate == CAR.FOCUS_MK4:
+ ret.wheelbase = 2.7
+ ret.steerRatio = 14.3
+ tire_stiffness_factor = 0.5328
+ ret.mass = 1350 + STD_CARGO_KG
+
+ else:
+ raise ValueError(f"Unsupported car: ${candidate}")
+
+ # Auto Transmission: Gear_Shift_by_Wire_FD1
+ # TODO: detect transmission in car_fw?
+ if 0x5A in fingerprint[0]:
+ ret.transmissionType = TransmissionType.automatic
+ else:
+ ret.transmissionType = TransmissionType.manual
+
+ # BSM: Side_Detect_L_Stat, Side_Detect_R_Stat
+ # TODO: detect bsm in car_fw?
+ ret.enableBsm = 0x3A6 in fingerprint[0] and 0x3A7 in fingerprint[0]
+
+ # 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) else 20. * CV.MPH_TO_MS
+ # LCA can steer down to zero
+ ret.minSteerSpeed = 0.
+
ret.steerRateCost = 1.0
ret.centerToFront = ret.wheelbase * 0.44
- tire_stiffness_factor = 0.5328
- # TODO: add minSteerSpeed
- ret.minEnableSpeed = 12. * 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)
-
- # TODO: start from empirically derived lateral slip stiffness for the civic and scale by
- # mass and CG position, so all cars will have approximately similar dyn behaviors
ret.tireStiffnessFront, ret.tireStiffnessRear = scale_tire_stiffness(ret.mass, ret.wheelbase, ret.centerToFront,
tire_stiffness_factor=tire_stiffness_factor)
- ret.steerControlType = car.CarParams.SteerControlType.angle
-
return ret
- # returns a car.CarState
- def update(self, c, can_strings):
- # ******************* do can recv *******************
- self.cp.update_strings(can_strings)
-
- ret = self.CS.update(self.cp)
+ def _update(self, c):
+ ret = self.CS.update(self.cp, self.cp_cam)
- ret.canValid = self.cp.can_valid
+ ret.steeringRateLimited = self.CC.steer_rate_limited if self.CC is not None else False
- # events
events = self.create_common_events(ret)
-
- if self.CS.lkas_state not in (2, 3) and ret.vEgo > 13. * CV.MPH_TO_MS and ret.cruiseState.enabled:
- events.add(car.CarEvent.EventName.steerTempUnavailable)
-
ret.events = events.to_msg()
- self.CS.out = ret.as_reader()
- return self.CS.out
+ return ret
- # pass in a car.CarControl
- # to be called @ 100hz
def apply(self, c):
-
- ret = self.CC.update(c.enabled, self.CS, self.frame, c.actuators,
- c.hudControl.visualAlert, c.cruiseControl.cancel)
-
+ ret = self.CC.update(c, self.CS, self.frame)
self.frame += 1
return ret
diff --git a/selfdrive/car/ford/radar_interface.py b/selfdrive/car/ford/radar_interface.py
old mode 100755
new mode 100644
index f8477951cd..1dfc27e672
--- a/selfdrive/car/ford/radar_interface.py
+++ b/selfdrive/car/ford/radar_interface.py
@@ -2,7 +2,7 @@
from cereal import car
from common.conversions import Conversions as CV
from opendbc.can.parser import CANParser
-from selfdrive.car.ford.values import DBC
+from selfdrive.car.ford.values import CANBUS, DBC
from selfdrive.car.interfaces import RadarInterfaceBase
RADAR_MSGS = list(range(0x500, 0x540))
@@ -14,7 +14,7 @@ def _create_radar_can_parser(car_fingerprint):
RADAR_MSGS * 3))
checks = list(zip(RADAR_MSGS, [20] * msg_n))
- return CANParser(DBC[car_fingerprint]['radar'], signals, checks, 1)
+ return CANParser(DBC[car_fingerprint]['radar'], signals, checks, CANBUS.radar)
class RadarInterface(RadarInterfaceBase):
def __init__(self, CP):
diff --git a/selfdrive/car/ford/values.py b/selfdrive/car/ford/values.py
index dd76f48b39..cee7b979ec 100644
--- a/selfdrive/car/ford/values.py
+++ b/selfdrive/car/ford/values.py
@@ -1,21 +1,82 @@
+from collections import namedtuple
from typing import Dict, List, Union
from cereal import car
from selfdrive.car import dbc_dict
from selfdrive.car.docs_definitions import CarInfo
+
Ecu = car.CarParams.Ecu
+TransmissionType = car.CarParams.TransmissionType
+
+AngleRateLimit = namedtuple('AngleRateLimit', ['speed_points', 'max_angle_diff_points'])
+
+
+class CarControllerParams:
+ # Messages: Lane_Assist_Data1, LateralMotionControl
+ LKAS_STEER_STEP = 5
+ # Message: IPMA_Data
+ LKAS_UI_STEP = 100
+ # Message: ACCDATA_3
+ ACC_UI_STEP = 5
+
+ STEER_RATE_LIMIT_UP = AngleRateLimit(speed_points=[0., 5., 15.], max_angle_diff_points=[5., .8, .15])
+ STEER_RATE_LIMIT_DOWN = AngleRateLimit(speed_points=[0., 5., 15.], max_angle_diff_points=[5., 3.5, 0.4])
-MAX_ANGLE = 87. # make sure we never command the extremes (0xfff) which cause latching fault
+
+class CANBUS:
+ main = 0
+ radar = 1
+ camera = 2
class CAR:
- FUSION = "FORD FUSION 2018"
+ ESCAPE_MK4 = "FORD ESCAPE 4TH GEN"
+ FOCUS_MK4 = "FORD FOCUS 4TH GEN"
CAR_INFO: Dict[str, Union[CarInfo, List[CarInfo]]] = {
- CAR.FUSION: CarInfo("Ford Fusion 2018", "All")
}
+
+FW_VERSIONS = {
+ CAR.ESCAPE_MK4: {
+ (Ecu.eps, 0x730, None): [
+ b'LX6C-14D003-AH\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
+ ],
+ (Ecu.esp, 0x760, None): [
+ b'LX6C-2D053-NS\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
+ ],
+ (Ecu.fwdRadar, 0x764, None): [
+ b'LB5T-14D049-AB\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
+ ],
+ (Ecu.fwdCamera, 0x706, None): [
+ b'LJ6T-14F397-AD\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
+ ],
+ (Ecu.engine, 0x7E0, None): [
+ b'LX6A-14C204-ESG\x00\x00\x00\x00\x00\x00\x00\x00\x00',
+ ],
+ },
+ CAR.FOCUS_MK4: {
+ (Ecu.eps, 0x730, None): [
+ b'JX6C-14D003-AH\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
+ ],
+ (Ecu.esp, 0x760, None): [
+ b'JX61-2D053-CJ\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
+ ],
+ (Ecu.fwdRadar, 0x764, None): [
+ b'JX7T-14D049-AC\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
+ ],
+ (Ecu.fwdCamera, 0x706, None): [
+ b'JX7T-14F397-AH\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
+ ],
+ (Ecu.engine, 0x7E0, None): [
+ b'JX6A-14C204-BPL\x00\x00\x00\x00\x00\x00\x00\x00\x00',
+ ],
+ },
+}
+
+
DBC = {
- CAR.FUSION: dbc_dict('ford_fusion_2018_pt', 'ford_fusion_2018_adas'),
+ CAR.ESCAPE_MK4: dbc_dict('ford_lincoln_base_pt', 'ford_fusion_2018_adas'),
+ CAR.FOCUS_MK4: dbc_dict('ford_lincoln_base_pt', 'ford_fusion_2018_adas'),
}
diff --git a/selfdrive/car/gm/interface.py b/selfdrive/car/gm/interface.py
index 5787dc5ae8..64ec9d4242 100755
--- a/selfdrive/car/gm/interface.py
+++ b/selfdrive/car/gm/interface.py
@@ -155,13 +155,9 @@ class CarInterface(CarInterfaceBase):
return ret
# returns a car.CarState
- def update(self, c, can_strings):
- self.cp.update_strings(can_strings)
- self.cp_loopback.update_strings(can_strings)
-
+ def _update(self, c):
ret = self.CS.update(self.cp, self.cp_loopback)
- ret.canValid = self.cp.can_valid and self.cp_loopback.can_valid
ret.steeringRateLimited = self.CC.steer_rate_limited if self.CC is not None else False
buttonEvents = []
@@ -210,10 +206,7 @@ class CarInterface(CarInterfaceBase):
ret.events = events.to_msg()
- # copy back carState packet to CS
- self.CS.out = ret.as_reader()
-
- return self.CS.out
+ return ret
def apply(self, c):
ret = self.CC.update(c, self.CS)
diff --git a/selfdrive/car/honda/carstate.py b/selfdrive/car/honda/carstate.py
index 31a641e90d..5314fe375e 100644
--- a/selfdrive/car/honda/carstate.py
+++ b/selfdrive/car/honda/carstate.py
@@ -181,18 +181,14 @@ class CarState(CarStateBase):
self.prev_cruise_setting = self.cruise_setting
# ******************* parse out can *******************
- # TODO: find wheels moving bit in dbc
+ # STANDSTILL->WHEELS_MOVING bit can be noisy around zero, so use XMISSION_SPEED
+ # panda checks if the signal is non-zero
+ ret.standstill = cp.vl["ENGINE_DATA"]["XMISSION_SPEED"] < 1e-5
if self.CP.carFingerprint in (CAR.ACCORD, CAR.ACCORDH, CAR.CIVIC_BOSCH, CAR.CIVIC_BOSCH_DIESEL, CAR.CRV_HYBRID, CAR.INSIGHT, CAR.ACURA_RDX_3G, CAR.HONDA_E):
- ret.standstill = cp.vl["ENGINE_DATA"]["XMISSION_SPEED"] < 0.1
ret.doorOpen = bool(cp.vl["SCM_FEEDBACK"]["DRIVERS_DOOR_OPEN"])
- elif self.CP.carFingerprint == CAR.ODYSSEY_CHN:
- ret.standstill = cp.vl["ENGINE_DATA"]["XMISSION_SPEED"] < 0.1
- ret.doorOpen = bool(cp.vl["SCM_BUTTONS"]["DRIVERS_DOOR_OPEN"])
- elif self.CP.carFingerprint in (CAR.FREED, CAR.HRV):
- ret.standstill = not cp.vl["STANDSTILL"]["WHEELS_MOVING"]
+ elif self.CP.carFingerprint in (CAR.ODYSSEY_CHN, CAR.FREED, CAR.HRV):
ret.doorOpen = bool(cp.vl["SCM_BUTTONS"]["DRIVERS_DOOR_OPEN"])
else:
- ret.standstill = not cp.vl["STANDSTILL"]["WHEELS_MOVING"]
ret.doorOpen = any([cp.vl["DOORS_STATUS"]["DOOR_OPEN_FL"], cp.vl["DOORS_STATUS"]["DOOR_OPEN_FR"],
cp.vl["DOORS_STATUS"]["DOOR_OPEN_RL"], cp.vl["DOORS_STATUS"]["DOOR_OPEN_RR"]])
ret.seatbeltUnlatched = bool(cp.vl["SEATBELT_STATUS"]["SEATBELT_DRIVER_LAMP"] or not cp.vl["SEATBELT_STATUS"]["SEATBELT_DRIVER_LATCHED"])
diff --git a/selfdrive/car/honda/interface.py b/selfdrive/car/honda/interface.py
index 8a23154bc9..265175d940 100755
--- a/selfdrive/car/honda/interface.py
+++ b/selfdrive/car/honda/interface.py
@@ -328,17 +328,9 @@ class CarInterface(CarInterfaceBase):
disable_ecu(logcan, sendcan, bus=1, addr=0x18DAB0F1, com_cont_req=b'\x28\x83\x03')
# returns a car.CarState
- def update(self, c, can_strings):
- # ******************* do can recv *******************
- self.cp.update_strings(can_strings)
- self.cp_cam.update_strings(can_strings)
- if self.cp_body:
- self.cp_body.update_strings(can_strings)
-
+ def _update(self, c):
ret = self.CS.update(self.cp, self.cp_cam, self.cp_body)
- ret.canValid = self.cp.can_valid and self.cp_cam.can_valid and (self.cp_body is None or self.cp_body.can_valid)
-
buttonEvents = []
if self.CS.cruise_buttons != self.CS.prev_cruise_buttons:
@@ -412,11 +404,9 @@ class CarInterface(CarInterfaceBase):
ret.events = events.to_msg()
- self.CS.out = ret.as_reader()
- return self.CS.out
+ return ret
# pass in a car.CarControl
# to be called @ 100hz
def apply(self, c):
- ret = self.CC.update(c, self.CS)
- return ret
+ return self.CC.update(c, self.CS)
diff --git a/selfdrive/car/hyundai/interface.py b/selfdrive/car/hyundai/interface.py
index 4bb24ba4ea..00cd4507f9 100644
--- a/selfdrive/car/hyundai/interface.py
+++ b/selfdrive/car/hyundai/interface.py
@@ -138,6 +138,20 @@ class CarInterface(CarInterfaceBase):
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25], [0.05]]
if candidate not in (CAR.IONIQ_EV_2020, CAR.IONIQ_PHEV, CAR.IONIQ_HEV_2022):
ret.minSteerSpeed = 32 * CV.MPH_TO_MS
+ elif candidate == CAR.IONIQ_PHEV_2019:
+ ret.mass = 1550. + STD_CARGO_KG # weight per hyundai site https://www.hyundaiusa.com/us/en/vehicles/2019-ioniq-plug-in-hybrid/compare-specs
+ ret.wheelbase = 2.7
+ ret.steerRatio = 13.73
+ ret.lateralTuning.init('indi')
+ ret.lateralTuning.indi.innerLoopGainBP = [0.]
+ ret.lateralTuning.indi.innerLoopGainV = [2.5]
+ ret.lateralTuning.indi.outerLoopGainBP = [0.]
+ ret.lateralTuning.indi.outerLoopGainV = [3.5]
+ ret.lateralTuning.indi.timeConstantBP = [0.]
+ ret.lateralTuning.indi.timeConstantV = [1.4]
+ ret.lateralTuning.indi.actuatorEffectivenessBP = [0.]
+ ret.lateralTuning.indi.actuatorEffectivenessV = [1.8]
+ ret.minSteerSpeed = 32 * CV.MPH_TO_MS
elif candidate == CAR.VELOSTER:
ret.lateralTuning.pid.kf = 0.00005
ret.mass = 3558. * CV.LB_TO_KG
@@ -297,12 +311,8 @@ class CarInterface(CarInterfaceBase):
if CP.openpilotLongitudinalControl:
disable_ecu(logcan, sendcan, addr=0x7d0, com_cont_req=b'\x28\x83\x01')
- def update(self, c, can_strings):
- self.cp.update_strings(can_strings)
- self.cp_cam.update_strings(can_strings)
-
+ def _update(self, c):
ret = self.CS.update(self.cp, self.cp_cam)
- ret.canValid = self.cp.can_valid and self.cp_cam.can_valid
ret.steeringRateLimited = self.CC.steer_rate_limited if self.CC is not None else False
events = self.create_common_events(ret, pcm_enable=self.CS.CP.pcmCruise)
@@ -352,8 +362,7 @@ class CarInterface(CarInterfaceBase):
ret.events = events.to_msg()
- self.CS.out = ret.as_reader()
- return self.CS.out
+ return ret
def apply(self, c):
ret = self.CC.update(c, self.CS)
diff --git a/selfdrive/car/hyundai/values.py b/selfdrive/car/hyundai/values.py
index 0263daee3a..d88d60496a 100644
--- a/selfdrive/car/hyundai/values.py
+++ b/selfdrive/car/hyundai/values.py
@@ -39,6 +39,7 @@ class CAR:
IONIQ_HEV_2022 = "HYUNDAI IONIQ HYBRID 2020-2022"
IONIQ_EV_LTD = "HYUNDAI IONIQ ELECTRIC LIMITED 2019"
IONIQ_EV_2020 = "HYUNDAI IONIQ ELECTRIC 2020"
+ IONIQ_PHEV_2019 = "HYUNDAI IONIQ PLUG-IN HYBRID 2019"
IONIQ_PHEV = "HYUNDAI IONIQ PHEV 2020"
KONA = "HYUNDAI KONA 2020"
KONA_EV = "HYUNDAI KONA ELECTRIC 2019"
@@ -83,12 +84,13 @@ class HyundaiCarInfo(CarInfo):
CAR_INFO: Dict[str, Union[HyundaiCarInfo, List[HyundaiCarInfo]]] = {
CAR.ELANTRA: HyundaiCarInfo("Hyundai Elantra 2017-19", min_enable_speed=19 * CV.MPH_TO_MS),
CAR.ELANTRA_2021: HyundaiCarInfo("Hyundai Elantra 2021-22", video_link="https://youtu.be/_EdYQtV52-c"),
- CAR.ELANTRA_HEV_2021: HyundaiCarInfo("Hyundai Elantra Hybrid 2021", video_link="https://youtu.be/_EdYQtV52-c"),
+ CAR.ELANTRA_HEV_2021: HyundaiCarInfo("Hyundai Elantra Hybrid 2021-22", video_link="https://youtu.be/_EdYQtV52-c"),
CAR.HYUNDAI_GENESIS: HyundaiCarInfo("Hyundai Genesis 2015-16", min_enable_speed=19 * CV.MPH_TO_MS),
CAR.IONIQ: HyundaiCarInfo("Hyundai Ioniq Hybrid 2017-19"),
CAR.IONIQ_HEV_2022: HyundaiCarInfo("Hyundai Ioniq Hybrid 2020-22", "SCC + LFA"),
CAR.IONIQ_EV_LTD: HyundaiCarInfo("Hyundai Ioniq Electric 2019"),
CAR.IONIQ_EV_2020: HyundaiCarInfo("Hyundai Ioniq Electric 2020"),
+ CAR.IONIQ_PHEV_2019: HyundaiCarInfo("Hyundai Ioniq Plug-in Hybrid 2019", "SCC + LKAS"),
CAR.IONIQ_PHEV: HyundaiCarInfo("Hyundai Ioniq Plug-in Hybrid 2020-21"),
CAR.KONA: HyundaiCarInfo("Hyundai Kona 2020"),
CAR.KONA_EV: HyundaiCarInfo("Hyundai Kona Electric 2018-19"),
@@ -242,6 +244,24 @@ FW_VERSIONS = {
b'\xf1\x816U3H1051\x00\x00\xf1\x006U3H0_C2\x00\x006U3H1051\x00\x00HAE0G16US2\x00\x00\x00\x00',
],
},
+ CAR.IONIQ_PHEV_2019: {
+ (Ecu.fwdRadar, 0x7d0, None): [
+ b'\xf1\x00AEhe SCC H-CUP 1.01 1.01 96400-G2100 ',
+ ],
+ (Ecu.eps, 0x7d4, None): [
+ b'\xf1\x00AE MDPS C 1.00 1.07 56310/G2501 4AEHC107',
+ ],
+ (Ecu.fwdCamera, 0x7c4, None): [
+ b'\xf1\x00AEP MFC AT USA LHD 1.00 1.00 95740-G2400 180222',
+ ],
+ (Ecu.engine, 0x7e0, None): [
+ b'\xf1\x816H6F6051\x00\x00\x00\x00\x00\x00\x00\x00',
+ ],
+ (Ecu.transmission, 0x7e1, None): [
+ b'\xf1\x816U3J2051\x00\x00\xf1\x006U3H0_C2\x00\x006U3J2051\x00\x00PAE0G16NS1\xdbD\r\x81',
+ b'\xf1\x816U3J2051\x00\x00\xf1\x006U3H0_C2\x00\x006U3J2051\x00\x00PAE0G16NS1\x00\x00\x00\x00',
+ ],
+ },
CAR.IONIQ_PHEV: {
(Ecu.fwdRadar, 0x7d0, None): [
b'\xf1\000AEhe SCC FHCUP 1.00 1.02 99110-G2100 ',
@@ -1034,6 +1054,7 @@ FW_VERSIONS = {
},
CAR.ELANTRA_HEV_2021: {
(Ecu.fwdCamera, 0x7c4, None) : [
+ b'\xf1\x00CN7HMFC AT USA LHD 1.00 1.05 99210-AA000 210930',
b'\xf1\000CN7HMFC AT USA LHD 1.00 1.03 99210-AA000 200819',
],
(Ecu.fwdRadar, 0x7d0, None) : [
@@ -1041,6 +1062,7 @@ FW_VERSIONS = {
b'\xf1\x8799110BY000\xf1\x00CNhe SCC FHCUP 1.00 1.01 99110-BY000 ',
],
(Ecu.eps, 0x7d4, None) :[
+ b'\xf1\x8756310/BY050\xf1\x00CN7 MDPS C 1.00 1.03 56310/BY050 4CNHC103',
b'\xf1\x8756310/BY050\xf1\000CN7 MDPS C 1.00 1.02 56310/BY050 4CNHC102',
],
(Ecu.transmission, 0x7e1, None) :[
@@ -1135,13 +1157,13 @@ FEATURES = {
# which message has the gear
"use_cluster_gears": {CAR.ELANTRA, CAR.ELANTRA_GT_I30, CAR.KONA},
"use_tcu_gears": {CAR.KIA_OPTIMA, CAR.SONATA_LF, CAR.VELOSTER, CAR.TUCSON_DIESEL_2019},
- "use_elect_gears": {CAR.KIA_NIRO_EV, CAR.KIA_NIRO_HEV, CAR.KIA_NIRO_HEV_2021, CAR.KIA_OPTIMA_H, CAR.IONIQ_EV_LTD, CAR.KONA_EV, CAR.IONIQ, CAR.IONIQ_EV_2020, CAR.IONIQ_PHEV, CAR.ELANTRA_HEV_2021,CAR.SONATA_HYBRID, CAR.KONA_HEV, CAR.IONIQ_HEV_2022, CAR.SANTA_FE_HEV_2022, CAR.SANTA_FE_PHEV_2022},
+ "use_elect_gears": {CAR.KIA_NIRO_EV, CAR.KIA_NIRO_HEV, CAR.KIA_NIRO_HEV_2021, CAR.KIA_OPTIMA_H, CAR.IONIQ_EV_LTD, CAR.KONA_EV, CAR.IONIQ, CAR.IONIQ_EV_2020, CAR.IONIQ_PHEV, CAR.ELANTRA_HEV_2021, CAR.SONATA_HYBRID, CAR.KONA_HEV, CAR.IONIQ_HEV_2022, CAR.SANTA_FE_HEV_2022, CAR.SANTA_FE_PHEV_2022, CAR.IONIQ_PHEV_2019},
# these cars use the FCA11 message for the AEB and FCW signals, all others use SCC12
"use_fca": {CAR.SONATA, CAR.SONATA_HYBRID, CAR.ELANTRA, CAR.ELANTRA_2021, CAR.ELANTRA_HEV_2021, CAR.ELANTRA_GT_I30, CAR.KIA_STINGER, CAR.IONIQ_EV_2020, CAR.IONIQ_PHEV, CAR.KONA_EV, CAR.KIA_FORTE, CAR.KIA_NIRO_EV, CAR.PALISADE, CAR.GENESIS_G70, CAR.GENESIS_G70_2020, CAR.KONA, CAR.SANTA_FE, CAR.KIA_SELTOS, CAR.KONA_HEV, CAR.SANTA_FE_2022, CAR.KIA_K5_2021, CAR.IONIQ_HEV_2022, CAR.SANTA_FE_HEV_2022, CAR.SANTA_FE_PHEV_2022, CAR.TUCSON_DIESEL_2019},
}
-HYBRID_CAR = {CAR.IONIQ_PHEV, CAR.ELANTRA_HEV_2021, CAR.KIA_NIRO_HEV, CAR.KIA_NIRO_HEV_2021, CAR.SONATA_HYBRID, CAR.KONA_HEV, CAR.IONIQ, CAR.IONIQ_HEV_2022, CAR.SANTA_FE_HEV_2022, CAR.SANTA_FE_PHEV_2022} # these cars use a different gas signal
+HYBRID_CAR = {CAR.IONIQ_PHEV, CAR.ELANTRA_HEV_2021, CAR.KIA_NIRO_HEV, CAR.KIA_NIRO_HEV_2021, CAR.SONATA_HYBRID, CAR.KONA_HEV, CAR.IONIQ, CAR.IONIQ_HEV_2022, CAR.SANTA_FE_HEV_2022, CAR.SANTA_FE_PHEV_2022, CAR.IONIQ_PHEV_2019} # these cars use a different gas signal
EV_CAR = {CAR.IONIQ_EV_2020, CAR.IONIQ_EV_LTD, CAR.KONA_EV, CAR.KIA_NIRO_EV}
# these cars require a special panda safety mode due to missing counters and checksums in the messages
@@ -1159,6 +1181,7 @@ DBC = {
CAR.GENESIS_G80: dbc_dict('hyundai_kia_generic', None),
CAR.GENESIS_G90: dbc_dict('hyundai_kia_generic', None),
CAR.HYUNDAI_GENESIS: dbc_dict('hyundai_kia_generic', None),
+ CAR.IONIQ_PHEV_2019: dbc_dict('hyundai_kia_generic', None),
CAR.IONIQ_PHEV: dbc_dict('hyundai_kia_generic', None),
CAR.IONIQ_EV_2020: dbc_dict('hyundai_kia_generic', None),
CAR.IONIQ_EV_LTD: dbc_dict('hyundai_kia_generic', 'hyundai_kia_mando_front_radar'),
diff --git a/selfdrive/car/interfaces.py b/selfdrive/car/interfaces.py
index ddf67e4d62..2d4b3416a4 100644
--- a/selfdrive/car/interfaces.py
+++ b/selfdrive/car/interfaces.py
@@ -33,12 +33,17 @@ class CarInterfaceBase(ABC):
self.low_speed_alert = False
self.silent_steer_warning = True
+ self.CS = None
+ self.can_parsers = []
if CarState is not None:
self.CS = CarState(CP)
+
self.cp = self.CS.get_can_parser(CP)
self.cp_cam = self.CS.get_cam_can_parser(CP)
+ self.cp_adas = self.CS.get_adas_can_parser(CP)
self.cp_body = self.CS.get_body_can_parser(CP)
self.cp_loopback = self.CS.get_loopback_can_parser(CP)
+ self.can_parsers = [self.cp, self.cp_cam, self.cp_adas, self.cp_body, self.cp_loopback]
self.CC = None
if CarController is not None:
@@ -100,9 +105,28 @@ class CarInterfaceBase(ABC):
return ret
@abstractmethod
- def update(self, c: car.CarControl, can_strings: List[bytes]) -> car.CarState:
+ def _update(self, c: car.CarControl) -> car.CarState:
pass
+ def update(self, c: car.CarControl, can_strings: List[bytes]) -> car.CarState:
+ # parse can
+ for cp in self.can_parsers:
+ if cp is not None:
+ cp.update_strings(can_strings)
+
+ # get CarState
+ ret = self._update(c)
+
+ ret.canValid = all(cp.can_valid for cp in self.can_parsers if cp is not None)
+ ret.canTimeout = any(cp.bus_timeout for cp in self.can_parsers if cp is not None)
+
+ # copy back for next iteration
+ reader = ret.as_reader()
+ if self.CS is not None:
+ self.CS.out = reader
+
+ return reader
+
@abstractmethod
def apply(self, c: car.CarControl) -> Tuple[car.CarControl.Actuators, List[bytes]]:
pass
@@ -254,6 +278,10 @@ class CarStateBase(ABC):
def get_cam_can_parser(CP):
return None
+ @staticmethod
+ def get_adas_can_parser(CP):
+ return None
+
@staticmethod
def get_body_can_parser(CP):
return None
diff --git a/selfdrive/car/mazda/interface.py b/selfdrive/car/mazda/interface.py
index b9a3d66ff2..0ef573c6b6 100755
--- a/selfdrive/car/mazda/interface.py
+++ b/selfdrive/car/mazda/interface.py
@@ -75,13 +75,8 @@ class CarInterface(CarInterfaceBase):
return ret
# returns a car.CarState
- def update(self, c, can_strings):
-
- self.cp.update_strings(can_strings)
- self.cp_cam.update_strings(can_strings)
-
+ def _update(self, c):
ret = self.CS.update(self.cp, self.cp_cam)
- ret.canValid = self.cp.can_valid and self.cp_cam.can_valid
# events
events = self.create_common_events(ret)
@@ -93,8 +88,7 @@ class CarInterface(CarInterfaceBase):
ret.events = events.to_msg()
- self.CS.out = ret.as_reader()
- return self.CS.out
+ return ret
def apply(self, c):
ret = self.CC.update(c, self.CS, self.frame)
diff --git a/selfdrive/car/mock/interface.py b/selfdrive/car/mock/interface.py
index 79f37097c8..65e886751d 100755
--- a/selfdrive/car/mock/interface.py
+++ b/selfdrive/car/mock/interface.py
@@ -48,7 +48,7 @@ class CarInterface(CarInterfaceBase):
return ret
# returns a car.CarState
- def update(self, c, can_strings):
+ def _update(self, c):
# get basic data from phone and gps since CAN isn't connected
sensors = messaging.recv_sock(self.sensor)
if sensors is not None:
@@ -63,7 +63,6 @@ class CarInterface(CarInterfaceBase):
# create message
ret = car.CarState.new_message()
- ret.canValid = True
# speeds
ret.vEgo = self.speed
@@ -83,7 +82,7 @@ class CarInterface(CarInterfaceBase):
curvature = self.yaw_rate / max(self.speed, 1.)
ret.steeringAngleDeg = curvature * self.CP.steerRatio * self.CP.wheelbase * CV.RAD_TO_DEG
- return ret.as_reader()
+ return ret
def apply(self, c):
# in mock no carcontrols
diff --git a/selfdrive/car/nissan/interface.py b/selfdrive/car/nissan/interface.py
index c078556434..3bf34e8266 100644
--- a/selfdrive/car/nissan/interface.py
+++ b/selfdrive/car/nissan/interface.py
@@ -5,9 +5,6 @@ from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness,
from selfdrive.car.interfaces import CarInterfaceBase
class CarInterface(CarInterfaceBase):
- def __init__(self, CP, CarController, CarState):
- super().__init__(CP, CarController, CarState)
- self.cp_adas = self.CS.get_adas_can_parser(CP)
@staticmethod
def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=None, disable_radar=False):
@@ -53,15 +50,9 @@ class CarInterface(CarInterfaceBase):
return ret
# returns a car.CarState
- def update(self, c, can_strings):
- self.cp.update_strings(can_strings)
- self.cp_cam.update_strings(can_strings)
- self.cp_adas.update_strings(can_strings)
-
+ def _update(self, c):
ret = self.CS.update(self.cp, self.cp_adas, self.cp_cam)
- ret.canValid = self.cp.can_valid and self.cp_adas.can_valid and self.cp_cam.can_valid
-
buttonEvents = []
be = car.CarState.ButtonEvent.new_message()
be.type = car.CarState.ButtonEvent.Type.accelCruise
@@ -74,8 +65,7 @@ class CarInterface(CarInterfaceBase):
ret.events = events.to_msg()
- self.CS.out = ret.as_reader()
- return self.CS.out
+ return ret
def apply(self, c):
hud_control = c.hudControl
diff --git a/selfdrive/car/subaru/interface.py b/selfdrive/car/subaru/interface.py
index 37330f0e30..d0d8e91ce1 100644
--- a/selfdrive/car/subaru/interface.py
+++ b/selfdrive/car/subaru/interface.py
@@ -107,19 +107,15 @@ class CarInterface(CarInterfaceBase):
return ret
# returns a car.CarState
- def update(self, c, can_strings):
- self.cp.update_strings(can_strings)
- self.cp_cam.update_strings(can_strings)
+ def _update(self, c):
ret = self.CS.update(self.cp, self.cp_cam)
- ret.canValid = self.cp.can_valid and self.cp_cam.can_valid
ret.steeringRateLimited = self.CC.steer_rate_limited if self.CC is not None else False
ret.events = self.create_common_events(ret).to_msg()
- self.CS.out = ret.as_reader()
- return self.CS.out
+ return ret
def apply(self, c):
hud_control = c.hudControl
diff --git a/selfdrive/car/subaru/values.py b/selfdrive/car/subaru/values.py
index 2ae3b3f17d..76f7e89660 100644
--- a/selfdrive/car/subaru/values.py
+++ b/selfdrive/car/subaru/values.py
@@ -40,7 +40,7 @@ CAR_INFO: Dict[str, Union[SubaruCarInfo, List[SubaruCarInfo]]] = {
CAR.ASCENT: SubaruCarInfo("Subaru Ascent 2019-20"),
CAR.IMPREZA: [
SubaruCarInfo("Subaru Impreza 2017-19", good_torque=True),
- SubaruCarInfo("Subaru Crosstrek 2018-19", good_torque=True),
+ SubaruCarInfo("Subaru Crosstrek 2018-19", video_link="https://youtu.be/Agww7oE1k-s?t=26", good_torque=True),
],
CAR.IMPREZA_2020: [
SubaruCarInfo("Subaru Impreza 2020-21"),
@@ -54,18 +54,6 @@ CAR_INFO: Dict[str, Union[SubaruCarInfo, List[SubaruCarInfo]]] = {
}
-FINGERPRINTS = {
- CAR.IMPREZA_2020: [{
- 2: 8, 64: 8, 65: 8, 72: 8, 73: 8, 280: 8, 281: 8, 282: 8, 290: 8, 312: 8, 313: 8, 314: 8, 315: 8, 316: 8, 326: 8, 372: 8, 544: 8, 545: 8, 546: 8, 552: 8, 554: 8, 557: 8, 576: 8, 577: 8, 722: 8, 801: 8, 802: 8, 803: 8, 805: 8, 808: 8, 816: 8, 826: 8, 837: 8, 838: 8, 839: 8, 842: 8, 912: 8, 915: 8, 940: 8, 1617: 8, 1632: 8, 1650: 8, 1677: 8, 1697: 8, 1722: 8, 1743: 8, 1759: 8, 1786: 5, 1787: 5, 1788: 8, 1809: 8, 1813: 8, 1817: 8, 1821: 8, 1840: 8, 1848: 8, 1924: 8, 1932: 8, 1952: 8, 1960: 8, 1968: 8, 1976: 8, 2015: 8, 2016: 8, 2024: 8
- },
- {
- 2: 8, 64: 8, 65: 8, 72: 8, 73: 8, 280: 8, 281: 8, 282: 8, 290: 8, 312: 8, 313: 8, 314: 8, 315: 8, 316: 8, 326: 8, 544: 8, 545: 8, 546: 8, 554: 8, 557: 8, 576: 8, 577: 8, 801: 8, 802: 8, 803: 8, 805: 8, 808: 8, 816: 8, 826: 8, 837: 8, 838: 8, 839: 8, 842: 8, 912: 8, 915: 8, 940: 8, 1614: 8, 1617: 8, 1632: 8, 1657: 8, 1658: 8, 1677: 8, 1697: 8, 1743: 8, 1759: 8, 1786: 5, 1787: 5, 1788: 8, 1809: 8, 1813: 8, 1817: 8, 1821: 8, 1840: 8, 1848: 8, 1924: 8, 1932: 8, 1952: 8, 1960: 8
- }],
- CAR.FORESTER: [{
- 2: 8, 64: 8, 65: 8, 72: 8, 73: 8, 280: 8, 281: 8, 282: 8, 290: 8, 312: 8, 313: 8, 314: 8, 315: 8, 316: 8, 326: 8, 372: 8, 544: 8, 545: 8, 546: 8, 552: 8, 554: 8, 557: 8, 576: 8, 577: 8, 722: 8, 801: 8, 802: 8, 803: 8, 805: 8, 808: 8, 811: 8, 816: 8, 826: 8, 837: 8, 838: 8, 839: 8, 842: 8, 912: 8, 915: 8, 940: 8, 961: 8, 984: 8, 1614: 8, 1617: 8, 1632: 8, 1650: 8, 1651: 8, 1657: 8, 1658: 8, 1677: 8, 1697: 8, 1698: 8, 1722: 8, 1743: 8, 1759: 8, 1787: 5, 1788: 8, 1809: 8, 1813: 8, 1817: 8, 1821: 8, 1840: 8, 1848: 8, 1924: 8, 1932: 8, 1952: 8, 1960: 8
- }],
-}
-
FW_VERSIONS = {
CAR.ASCENT: {
(Ecu.esp, 0x7b0, None): [
@@ -111,10 +99,11 @@ FW_VERSIONS = {
b'z\x9c\x19\x80\x01',
b'z\x94\x08\x90\x00',
b'z\x84\x19\x90\x00',
+ b'\xf1\x00\xb2\x06\x04',
],
(Ecu.eps, 0x746, None): [
b'\x7a\xc0\x0c\x00',
- b'z\xc0\b\x00',
+ b'z\xc0\x08\x00',
b'\x8a\xc0\x00\x00',
b'z\xc0\x04\x00',
b'z\xc0\x00\x00',
@@ -131,6 +120,10 @@ FW_VERSIONS = {
b'\000\000e\002\037@ \024',
b'\x00\x00d)\x00\x00\x00\x00',
b'\x00\x00c\xf4\x00\x00\x00\x00',
+ b'\x00\x00d\xdc\x00\x00\x00\x00',
+ ],
+ (Ecu.fwdCamera, 0x7c4, None): [
+ b'\xf1\x00\xac\x02\x00',
],
(Ecu.engine, 0x7e0, None): [
b'\xaa\x61\x66\x73\x07',
diff --git a/selfdrive/car/tesla/interface.py b/selfdrive/car/tesla/interface.py
index 14d8e32de4..71594cecb6 100755
--- a/selfdrive/car/tesla/interface.py
+++ b/selfdrive/car/tesla/interface.py
@@ -57,18 +57,12 @@ class CarInterface(CarInterfaceBase):
return ret
- def update(self, c, can_strings):
- self.cp.update_strings(can_strings)
- self.cp_cam.update_strings(can_strings)
-
+ def _update(self, c):
ret = self.CS.update(self.cp, self.cp_cam)
- ret.canValid = self.cp.can_valid and self.cp_cam.can_valid
- events = self.create_common_events(ret)
+ ret.events = self.create_common_events(ret).to_msg()
- ret.events = events.to_msg()
- self.CS.out = ret.as_reader()
- return self.CS.out
+ return ret
def apply(self, c):
ret = self.CC.update(c, self.CS)
diff --git a/selfdrive/car/tests/routes.py b/selfdrive/car/tests/routes.py
index b7b110c051..a26d6dcd04 100644
--- a/selfdrive/car/tests/routes.py
+++ b/selfdrive/car/tests/routes.py
@@ -3,6 +3,7 @@ from collections import namedtuple
from selfdrive.car.chrysler.values import CAR as CHRYSLER
from selfdrive.car.gm.values import CAR as GM
+from selfdrive.car.ford.values import CAR as FORD
from selfdrive.car.honda.values import CAR as HONDA
from selfdrive.car.hyundai.values import CAR as HYUNDAI
from selfdrive.car.nissan.values import CAR as NISSAN
@@ -15,6 +16,8 @@ from selfdrive.car.body.values import CAR as COMMA
# TODO: add routes for these cars
non_tested_cars = [
+ FORD.ESCAPE_MK4,
+ FORD.FOCUS_MK4,
GM.CADILLAC_ATS,
GM.HOLDEN_ASTRA,
GM.MALIBU,
@@ -45,8 +48,6 @@ routes = [
TestRoute("0e7a2ba168465df5|2020-10-18--14-14-22", HONDA.ACURA_RDX_3G),
TestRoute("a74b011b32b51b56|2020-07-26--17-09-36", HONDA.CIVIC),
- # Checks there's no controls mismatches due to pedal thresholds
- TestRoute("cfb32f0fb91b173b|2022-04-06--14-54-45", HONDA.CIVIC, segment=21),
TestRoute("a859a044a447c2b0|2020-03-03--18-42-45", HONDA.CRV_EU),
TestRoute("68aac44ad69f838e|2021-05-18--20-40-52", HONDA.CRV),
TestRoute("14fed2e5fa0aa1a5|2021-05-25--14-59-42", HONDA.CRV_HYBRID),
@@ -86,6 +87,7 @@ routes = [
TestRoute("fb3fd42f0baaa2f8|2022-03-30--15-25-05", HYUNDAI.TUCSON_DIESEL_2019),
TestRoute("5875672fc1d4bf57|2020-07-23--21-33-28", HYUNDAI.KIA_SORENTO),
TestRoute("9c917ba0d42ffe78|2020-04-17--12-43-19", HYUNDAI.PALISADE),
+ TestRoute("3f29334d6134fcd4|2022-03-30--22-00-50", HYUNDAI.IONIQ_PHEV_2019),
TestRoute("fa8db5869167f821|2021-06-10--22-50-10", HYUNDAI.IONIQ_PHEV),
TestRoute("2c5cf2dd6102e5da|2020-12-17--16-06-44", HYUNDAI.IONIQ_EV_2020),
TestRoute("610ebb9faaad6b43|2020-06-13--15-28-36", HYUNDAI.IONIQ_EV_LTD),
@@ -203,4 +205,11 @@ routes = [
TestRoute("6c14ee12b74823ce|2021-06-30--11-49-02", TESLA.AP1_MODELS),
TestRoute("bb50caf5f0945ab1|2021-06-19--17-20-18", TESLA.AP2_MODELS),
+
+ # Segments that test specific issues
+ # Controls mismatch due to interceptor threshold
+ TestRoute("cfb32f0fb91b173b|2022-04-06--14-54-45", HONDA.CIVIC, segment=21),
+ TestRoute("5a8762b91fc70467|2022-04-14--21-26-20", TOYOTA.RAV4, segment=2),
+ # Controls mismatch due to standstill threshold
+ TestRoute("bec2dcfde6a64235|2022-04-08--14-21-32", HONDA.CRV_HYBRID, segment=22),
]
diff --git a/selfdrive/car/tests/test_car_interfaces.py b/selfdrive/car/tests/test_car_interfaces.py
index 2f4984c3d9..d97b16e682 100755
--- a/selfdrive/car/tests/test_car_interfaces.py
+++ b/selfdrive/car/tests/test_car_interfaces.py
@@ -38,8 +38,8 @@ class TestCarInterfaces(unittest.TestCase):
tuning = car_params.lateralTuning.which()
if tuning == 'pid':
self.assertTrue(len(car_params.lateralTuning.pid.kpV))
- elif tuning == 'lqr':
- self.assertTrue(len(car_params.lateralTuning.lqr.a))
+ elif tuning == 'torque':
+ self.assertTrue(car_params.lateralTuning.torque.kf > 0)
elif tuning == 'indi':
self.assertTrue(len(car_params.lateralTuning.indi.outerLoopGainV))
diff --git a/selfdrive/car/tests/test_models.py b/selfdrive/car/tests/test_models.py
index 7e02fd2a07..4368619926 100755
--- a/selfdrive/car/tests/test_models.py
+++ b/selfdrive/car/tests/test_models.py
@@ -115,8 +115,8 @@ class TestCarModel(unittest.TestCase):
tuning = self.CP.lateralTuning.which()
if tuning == 'pid':
self.assertTrue(len(self.CP.lateralTuning.pid.kpV))
- elif tuning == 'lqr':
- self.assertTrue(len(self.CP.lateralTuning.lqr.a))
+ elif tuning == 'torque':
+ self.assertTrue(self.CP.lateralTuning.torque.kf > 0)
elif tuning == 'indi':
self.assertTrue(len(self.CP.lateralTuning.indi.outerLoopGainV))
else:
@@ -209,13 +209,7 @@ class TestCarModel(unittest.TestCase):
# TODO: check rest of panda's carstate (steering, ACC main on, etc.)
- # TODO: make the interceptor thresholds in openpilot and panda match, then remove this exception
- gas_pressed = CS.gasPressed
- if self.CP.enableGasInterceptor and gas_pressed and not self.safety.get_gas_pressed_prev():
- # panda intentionally has a higher threshold
- if self.CP.carName == "toyota" and 15 < CS.gas < 15*1.5:
- gas_pressed = False
- checks['gasPressed'] += gas_pressed != self.safety.get_gas_pressed_prev()
+ checks['gasPressed'] += CS.gasPressed != self.safety.get_gas_pressed_prev()
# TODO: remove this exception once this mismatch is resolved
brake_pressed = CS.brakePressed
@@ -245,6 +239,8 @@ class TestCarModel(unittest.TestCase):
if self.CP.carName == "honda":
checks['mainOn'] += CS.cruiseState.available != self.safety.get_acc_main_on()
+ # TODO: fix standstill mismatches for other makes
+ checks['standstill'] += CS.standstill == self.safety.get_vehicle_moving()
CS_prev = CS
@@ -255,5 +251,6 @@ class TestCarModel(unittest.TestCase):
failed_checks = {k: v for k, v in checks.items() if v > 0}
self.assertFalse(len(failed_checks), f"panda safety doesn't agree with openpilot: {failed_checks}")
+
if __name__ == "__main__":
unittest.main()
diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py
index c0868f3288..1c01703d7e 100644
--- a/selfdrive/car/toyota/carcontroller.py
+++ b/selfdrive/car/toyota/carcontroller.py
@@ -14,6 +14,7 @@ VisualAlert = car.CarControl.HUDControl.VisualAlert
class CarController:
def __init__(self, dbc_name, CP, VM):
self.CP = CP
+ self.torque_rate_limits = CarControllerParams(self.CP)
self.frame = 0
self.last_steer = 0
self.alert_active = False
@@ -50,7 +51,7 @@ class CarController:
# steer torque
new_steer = int(round(actuators.steer * CarControllerParams.STEER_MAX))
- apply_steer = apply_toyota_steer_torque_limits(new_steer, self.last_steer, CS.out.steeringTorqueEps, CarControllerParams)
+ apply_steer = apply_toyota_steer_torque_limits(new_steer, self.last_steer, CS.out.steeringTorqueEps, self.torque_rate_limits)
self.steer_rate_limited = new_steer != apply_steer
# Cut steering while we're in a known fault state (2s)
diff --git a/selfdrive/car/toyota/carstate.py b/selfdrive/car/toyota/carstate.py
index a951955505..dea4263d7e 100644
--- a/selfdrive/car/toyota/carstate.py
+++ b/selfdrive/car/toyota/carstate.py
@@ -36,8 +36,8 @@ class CarState(CarStateBase):
ret.brakePressed = cp.vl["BRAKE_MODULE"]["BRAKE_PRESSED"] != 0
ret.brakeHoldActive = cp.vl["ESP_CONTROL"]["BRAKE_HOLD_ACTIVE"] == 1
if self.CP.enableGasInterceptor:
- ret.gas = (cp.vl["GAS_SENSOR"]["INTERCEPTOR_GAS"] + cp.vl["GAS_SENSOR"]["INTERCEPTOR_GAS2"]) / 2.
- ret.gasPressed = ret.gas > 15
+ ret.gas = (cp.vl["GAS_SENSOR"]["INTERCEPTOR_GAS"] + cp.vl["GAS_SENSOR"]["INTERCEPTOR_GAS2"]) // 2
+ ret.gasPressed = ret.gas > 805
else:
# TODO: find a new, common signal
msg = "GAS_PEDAL_HYBRID" if (self.CP.flags & ToyotaFlags.HYBRID) else "GAS_PEDAL"
diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py
index dc0c64ad1b..c20110b8b5 100644
--- a/selfdrive/car/toyota/interface.py
+++ b/selfdrive/car/toyota/interface.py
@@ -34,7 +34,6 @@ class CarInterface(CarInterfaceBase):
ret.steerRatio = 15.74 # unknown end-to-end spec
tire_stiffness_factor = 0.6371 # hand-tune
ret.mass = 3045. * CV.LB_TO_KG + STD_CARGO_KG
-
set_lat_tune(ret.lateralTuning, LatTunes.INDI_PRIUS)
ret.steerActuatorDelay = 0.3
@@ -44,7 +43,7 @@ class CarInterface(CarInterfaceBase):
ret.steerRatio = 17.4
tire_stiffness_factor = 0.5533
ret.mass = 4387. * CV.LB_TO_KG + STD_CARGO_KG
- set_lat_tune(ret.lateralTuning, LatTunes.LQR_RAV4)
+ set_lat_tune(ret.lateralTuning, LatTunes.TORQUE)
elif candidate in (CAR.RAV4, CAR.RAV4H):
stop_and_go = True if (candidate in CAR.RAV4H) else False
@@ -52,7 +51,7 @@ class CarInterface(CarInterfaceBase):
ret.steerRatio = 16.88 # 14.5 is spec end-to-end
tire_stiffness_factor = 0.5533
ret.mass = 3650. * CV.LB_TO_KG + STD_CARGO_KG # mean between normal and hybrid
- set_lat_tune(ret.lateralTuning, LatTunes.LQR_RAV4)
+ set_lat_tune(ret.lateralTuning, LatTunes.TORQUE, MAX_LAT_ACCEL=2.5, FRICTION=0.06)
elif candidate == CAR.COROLLA:
ret.wheelbase = 2.70
@@ -133,7 +132,7 @@ class CarInterface(CarInterfaceBase):
ret.steerRatio = 13.9
tire_stiffness_factor = 0.444 # not optimized yet
ret.mass = 3060. * CV.LB_TO_KG + STD_CARGO_KG
- set_lat_tune(ret.lateralTuning, LatTunes.PID_D)
+ set_lat_tune(ret.lateralTuning, LatTunes.TORQUE, MAX_LAT_ACCEL=3.0, FRICTION=0.08)
elif candidate in (CAR.LEXUS_ES_TSS2, CAR.LEXUS_ESH_TSS2, CAR.LEXUS_ESH):
stop_and_go = True
@@ -240,14 +239,9 @@ class CarInterface(CarInterfaceBase):
return ret
# returns a car.CarState
- def update(self, c, can_strings):
- # ******************* do can recv *******************
- self.cp.update_strings(can_strings)
- self.cp_cam.update_strings(can_strings)
-
+ def _update(self, c):
ret = self.CS.update(self.cp, self.cp_cam)
- ret.canValid = self.cp.can_valid and self.cp_cam.can_valid
ret.steeringRateLimited = self.CC.steer_rate_limited if self.CC is not None else False
# events
@@ -266,8 +260,7 @@ class CarInterface(CarInterfaceBase):
ret.events = events.to_msg()
- self.CS.out = ret.as_reader()
- return self.CS.out
+ return ret
# pass in a car.CarControl
# to be called @ 100hz
diff --git a/selfdrive/car/toyota/tunes.py b/selfdrive/car/toyota/tunes.py
index f26fc72a09..8ea1d6547f 100644
--- a/selfdrive/car/toyota/tunes.py
+++ b/selfdrive/car/toyota/tunes.py
@@ -24,6 +24,7 @@ class LatTunes(Enum):
PID_L = 13
PID_M = 14
PID_N = 15
+ TORQUE = 16
###### LONG ######
@@ -49,8 +50,15 @@ def set_long_tune(tune, name):
###### LAT ######
-def set_lat_tune(tune, name):
- if name == LatTunes.INDI_PRIUS:
+def set_lat_tune(tune, name, MAX_LAT_ACCEL=2.5, FRICTION=.1):
+ if name == LatTunes.TORQUE:
+ tune.init('torque')
+ tune.torque.useSteeringAngle = True
+ tune.torque.kp = 2.0 / MAX_LAT_ACCEL
+ tune.torque.kf = 1.0 / MAX_LAT_ACCEL
+ tune.torque.ki = 0.5 / MAX_LAT_ACCEL
+ tune.torque.friction = FRICTION
+ elif name == LatTunes.INDI_PRIUS:
tune.init('indi')
tune.indi.innerLoopGainBP = [0.]
tune.indi.innerLoopGainV = [4.0]
@@ -60,18 +68,6 @@ def set_lat_tune(tune, name):
tune.indi.timeConstantV = [1.0]
tune.indi.actuatorEffectivenessBP = [0.]
tune.indi.actuatorEffectivenessV = [1.0]
-
- elif name == LatTunes.LQR_RAV4:
- tune.init('lqr')
- tune.lqr.scale = 1500.0
- tune.lqr.ki = 0.05
- tune.lqr.a = [0., 1., -0.22619643, 1.21822268]
- tune.lqr.b = [-1.92006585e-04, 3.95603032e-05]
- tune.lqr.c = [1., 0.]
- tune.lqr.k = [-110.73572306, 451.22718255]
- tune.lqr.l = [0.3233671, 0.3185757]
- tune.lqr.dcGain = 0.002237852961363602
-
elif 'PID' in str(name):
tune.init('pid')
tune.pid.kiBP = [0.0]
diff --git a/selfdrive/car/toyota/values.py b/selfdrive/car/toyota/values.py
index 04157bf080..324d3b2b66 100644
--- a/selfdrive/car/toyota/values.py
+++ b/selfdrive/car/toyota/values.py
@@ -18,10 +18,16 @@ class CarControllerParams:
ACCEL_MIN = -3.5 # m/s2
STEER_MAX = 1500
- STEER_DELTA_UP = 10 # 1.5s time to peak torque
- STEER_DELTA_DOWN = 25 # always lower than 45 otherwise the Rav4 faults (Prius seems ok with 50)
STEER_ERROR_MAX = 350 # max delta between torque cmd and torque motor
+ def __init__(self, CP):
+ if CP.lateralTuning.which == 'torque':
+ self.STEER_DELTA_UP = 15 # 1.0s time to peak torque
+ self.STEER_DELTA_DOWN = 25 # always lower than 45 otherwise the Rav4 faults (Prius seems ok with 50)
+ else:
+ self.STEER_DELTA_UP = 10 # 1.5s time to peak torque
+ self.STEER_DELTA_DOWN = 25 # always lower than 45 otherwise the Rav4 faults (Prius seems ok with 50)
+
class ToyotaFlags(IntFlag):
HYBRID = 1
@@ -151,7 +157,7 @@ CAR_INFO: Dict[str, Union[ToyotaCarInfo, List[ToyotaCarInfo]]] = {
CAR.LEXUS_RC: ToyotaCarInfo("Lexus RC 2020"),
CAR.LEXUS_RX: ToyotaCarInfo("Lexus RX 2016-18", footnotes=[Footnote.DSU]),
CAR.LEXUS_RXH: ToyotaCarInfo("Lexus RX Hybrid 2016-19", footnotes=[Footnote.DSU]),
- CAR.LEXUS_RX_TSS2: ToyotaCarInfo("Lexus RX 2020-21"),
+ CAR.LEXUS_RX_TSS2: ToyotaCarInfo("Lexus RX 2020-22"),
CAR.LEXUS_RXH_TSS2: ToyotaCarInfo("Lexus RX Hybrid 2020-21"),
}
@@ -260,6 +266,7 @@ FW_VERSIONS = {
},
CAR.AVALON_TSS2: {
(Ecu.esp, 0x7b0, None): [
+ b'\x01F152607240\x00\x00\x00\x00\x00\x00',
b'\x01F152607280\x00\x00\x00\x00\x00\x00',
],
(Ecu.eps, 0x7a1, None): [
@@ -270,9 +277,11 @@ FW_VERSIONS = {
],
(Ecu.fwdRadar, 0x750, 0xf): [
b'\x018821F6201200\x00\x00\x00\x00',
+ b'\x018821F6201300\x00\x00\x00\x00',
],
(Ecu.fwdCamera, 0x750, 0x6d): [
b'\x028646F4104100\x00\x00\x00\x008646G5301200\x00\x00\x00\x00',
+ b'\x028646F4104100\x00\x00\x00\x008646G3304000\x00\x00\x00\x00',
],
},
CAR.AVALONH_TSS2: {
@@ -290,6 +299,7 @@ FW_VERSIONS = {
],
(Ecu.fwdCamera, 0x750, 0x6d): [
b'\x028646F4104100\x00\x00\x00\x008646G5301200\x00\x00\x00\x00',
+ b'\x028646F4104100\x00\x00\x00\x008646G3304000\x00\x00\x00\x00',
],
},
CAR.CAMRY: {
@@ -932,6 +942,7 @@ FW_VERSIONS = {
b'\x01F15264872500\x00\x00\x00\x00',
b'\x01F15264873500\x00\x00\x00\x00',
b'\x01F152648C6300\x00\x00\x00\x00',
+ b'\x01F152648J4000\x00\x00\x00\x00',
],
(Ecu.engine, 0x700, None): [
b'\x01896630E67000\x00\x00\x00\x00',
@@ -1492,8 +1503,8 @@ FW_VERSIONS = {
CAR.LEXUS_NX_TSS2: {
(Ecu.engine, 0x700, None): [
b'\x018966378B2100\x00\x00\x00\x00',
+ b'\x018966378B3000\x00\x00\x00\x00',
b'\x018966378G3000\x00\x00\x00\x00',
- b'\x018966378B3000\x00\x00\x00\x00'
],
(Ecu.esp, 0x7b0, None): [
b'\x01F152678221\x00\x00\x00\x00\x00\x00',
@@ -1504,6 +1515,7 @@ FW_VERSIONS = {
(Ecu.fwdRadar, 0x750, 0xf): [
b"\x018821F3301400\x00\x00\x00\x00",
b'\x018821F3301200\x00\x00\x00\x00',
+ b'\x018821F3301300\x00\x00\x00\x00',
],
(Ecu.fwdCamera, 0x750, 0x6d): [
b'\x028646F78030A0\x00\x00\x00\x008646G2601200\x00\x00\x00\x00',
diff --git a/selfdrive/car/volkswagen/carstate.py b/selfdrive/car/volkswagen/carstate.py
index 858b5e1440..dfd6a0031f 100644
--- a/selfdrive/car/volkswagen/carstate.py
+++ b/selfdrive/car/volkswagen/carstate.py
@@ -74,11 +74,6 @@ class CarState(CarStateBase):
# Update seatbelt fastened status.
ret.seatbeltUnlatched = pt_cp.vl["Airbag_02"]["AB_Gurtschloss_FA"] != 3
- # Update driver preference for metric. VW stores many different unit
- # preferences, including separate units for for distance vs. speed.
- # We use the speed preference for OP.
- self.displayMetricUnits = not pt_cp.vl["Einheiten_01"]["KBI_MFA_v_Einheit_02"]
-
# Consume blind-spot monitoring info/warning LED states, if available.
# Infostufe: BSM LED on, Warnung: BSM LED flashing
if self.CP.enableBsm:
@@ -176,7 +171,6 @@ class CarState(CarStateBase):
("EPS_HCA_Status", "LH_EPS_03"), # EPS HCA control status
("ESP_Tastung_passiv", "ESP_21"), # Stability control disabled
("ESP_Haltebestaetigung", "ESP_21"), # ESP hold confirmation
- ("KBI_MFA_v_Einheit_02", "Einheiten_01"), # MPH vs KMH speed display
("KBI_Handbremse", "Kombi_01"), # Manual handbrake applied
("TSK_Status", "TSK_06"), # ACC engagement status from drivetrain coordinator
("GRA_Hauptschalter", "GRA_ACC_01"), # ACC button, on/off
@@ -207,7 +201,6 @@ class CarState(CarStateBase):
("Airbag_02", 5), # From J234 Airbag control module
("Kombi_01", 2), # From J285 Instrument cluster
("Blinkmodi_02", 1), # From J519 BCM (sent at 1Hz when no lights active, 50Hz when active)
- ("Einheiten_01", 1), # From J??? not known if gateway, cluster, or BCM
]
if CP.transmissionType == TransmissionType.automatic:
diff --git a/selfdrive/car/volkswagen/interface.py b/selfdrive/car/volkswagen/interface.py
index ef42d63e96..f849d912a0 100644
--- a/selfdrive/car/volkswagen/interface.py
+++ b/selfdrive/car/volkswagen/interface.py
@@ -10,7 +10,6 @@ class CarInterface(CarInterfaceBase):
def __init__(self, CP, CarController, CarState):
super().__init__(CP, CarController, CarState)
- self.displayMetricUnitsPrev = None
self.buttonStatesPrev = BUTTON_STATES.copy()
if CP.networkLocation == NetworkLocation.fwdCamera:
@@ -161,17 +160,10 @@ class CarInterface(CarInterfaceBase):
return ret
# returns a car.CarState
- def update(self, c, can_strings):
+ def _update(self, c):
buttonEvents = []
- # Process the most recent CAN message traffic, and check for validity
- # The camera CAN has no signals we use at this time, but we process it
- # anyway so we can test connectivity with can_valid
- self.cp.update_strings(can_strings)
- self.cp_cam.update_strings(can_strings)
-
ret = self.CS.update(self.cp, self.cp_cam, self.cp_ext, self.CP.transmissionType)
- ret.canValid = self.cp.can_valid and self.cp_cam.can_valid
ret.steeringRateLimited = self.CC.steer_rate_limited if self.CC is not None else False
# Check for and process state-change events (button press or release) from
@@ -201,11 +193,9 @@ class CarInterface(CarInterfaceBase):
ret.buttonEvents = buttonEvents
# update previous car states
- self.displayMetricUnitsPrev = self.CS.displayMetricUnits
self.buttonStatesPrev = self.CS.buttonStates.copy()
- self.CS.out = ret.as_reader()
- return self.CS.out
+ return ret
def apply(self, c):
hud_control = c.hudControl
diff --git a/selfdrive/car/volkswagen/values.py b/selfdrive/car/volkswagen/values.py
index f27d7785cc..b2b43c98f3 100755
--- a/selfdrive/car/volkswagen/values.py
+++ b/selfdrive/car/volkswagen/values.py
@@ -116,7 +116,7 @@ CAR_INFO: Dict[str, Union[VWCarInfo, List[VWCarInfo]]] = {
CAR.ARTEON_MK1: VWCarInfo("Volkswagen Arteon 2018, 2021", footnotes=[Footnote.VW_HARNESS]),
CAR.ATLAS_MK1: VWCarInfo("Volkswagen Atlas 2018-19, 2022", footnotes=[Footnote.VW_HARNESS]),
CAR.GOLF_MK7: [
- VWCarInfo("Volkswagen e-Golf 2014, 2019-20"),
+ VWCarInfo("Volkswagen e-Golf 2014, 2018-20"),
VWCarInfo("Volkswagen Golf 2015-20"),
VWCarInfo("Volkswagen Golf Alltrack 2017-18"),
VWCarInfo("Volkswagen Golf GTE 2016"),
@@ -252,7 +252,9 @@ FW_VERSIONS = {
b'\xf1\x8704L906056HN\xf1\x896590',
b'\xf1\x870EA906016A \xf1\x898343',
b'\xf1\x870EA906016E \xf1\x894219',
+ b'\xf1\x870EA906016F \xf1\x894238',
b'\xf1\x870EA906016F \xf1\x895002',
+ b'\xf1\x870EA906016Q \xf1\x895993',
b'\xf1\x870EA906016S \xf1\x897207',
b'\xf1\x875G0906259 \xf1\x890007',
b'\xf1\x875G0906259J \xf1\x890002',
@@ -340,6 +342,7 @@ FW_VERSIONS = {
b'\xf1\x873Q0909144M \xf1\x895082\xf1\x82\00571A0JA16A1',
b'\xf1\x875Q0909143K \xf1\x892033\xf1\x820519A9040203',
b'\xf1\x875Q0909144AA\xf1\x891081\xf1\x82\00521A00441A1',
+ b'\xf1\x875Q0909144AA\xf1\x891081\xf1\x82\x0521A00608A1',
b'\xf1\x875Q0909144AA\xf1\x891081\xf1\x82\x0521A00641A1',
b'\xf1\x875Q0909144AB\xf1\x891082\xf1\x82\00521A00442A1',
b'\xf1\x875Q0909144AB\xf1\x891082\xf1\x82\00521A00642A1',
diff --git a/selfdrive/clocksd/clocksd.cc b/selfdrive/clocksd/clocksd.cc
index c24ffa41d9..a6a92fc058 100644
--- a/selfdrive/clocksd/clocksd.cc
+++ b/selfdrive/clocksd/clocksd.cc
@@ -21,16 +21,6 @@
ExitHandler do_exit;
-#ifdef QCOM
-namespace {
- int64_t arm_cntpct() {
- int64_t v;
- asm volatile("mrs %0, cntpct_el0" : "=r"(v));
- return v;
- }
-}
-#endif
-
int main() {
setpriority(PRIO_PROCESS, 0, -13);
PubMaster pm({"clocks"});
@@ -65,10 +55,6 @@ int main() {
uint64_t monotonic_raw = nanos_monotonic_raw();
uint64_t wall_time = nanos_since_epoch();
-#ifdef QCOM
- uint64_t modem_uptime_v = arm_cntpct() / 19200ULL; // 19.2 mhz clock
-#endif
-
MessageBuilder msg;
auto clocks = msg.initEvent().initClocks();
@@ -76,9 +62,6 @@ int main() {
clocks.setMonotonicNanos(monotonic);
clocks.setMonotonicRawNanos(monotonic_raw);
clocks.setWallTimeNanos(wall_time);
-#ifdef QCOM
- clocks.setModemUptimeMillis(modem_uptime_v);
-#endif
pm.send("clocks", msg);
}
diff --git a/selfdrive/common/SConscript b/selfdrive/common/SConscript
index 8dfeecbdd7..e53cb7615d 100644
--- a/selfdrive/common/SConscript
+++ b/selfdrive/common/SConscript
@@ -22,9 +22,7 @@ files = [
'visionimg.cc',
]
-if arch == "aarch64":
- _gpu_libs = ['gui', 'adreno_utils']
-elif arch == "larch64":
+if arch == "larch64":
_gpu_libs = ["GLESv2"]
else:
_gpu_libs = ["GL"]
diff --git a/selfdrive/common/modeldata.h b/selfdrive/common/modeldata.h
index 32b21338d8..8e044ecadc 100644
--- a/selfdrive/common/modeldata.h
+++ b/selfdrive/common/modeldata.h
@@ -26,13 +26,9 @@ constexpr auto X_IDXS_FLOAT = build_idxs(192.0);
const int TICI_CAM_WIDTH = 1928;
-const mat3 fcam_intrinsic_matrix =
- Hardware::EON() ? (mat3){{910., 0., 1164.0 / 2,
- 0., 910., 874.0 / 2,
- 0., 0., 1.}}
- : (mat3){{2648.0, 0.0, 1928.0 / 2,
- 0.0, 2648.0, 1208.0 / 2,
- 0.0, 0.0, 1.0}};
+const mat3 fcam_intrinsic_matrix = (mat3){{2648.0, 0.0, 1928.0 / 2,
+ 0.0, 2648.0, 1208.0 / 2,
+ 0.0, 0.0, 1.0}};
// tici ecam focal probably wrong? magnification is not consistent across frame
// Need to retrain model before this can be changed
@@ -41,7 +37,7 @@ const mat3 ecam_intrinsic_matrix = (mat3){{567.0, 0.0, 1928.0 / 2,
0.0, 0.0, 1.0}};
static inline mat3 get_model_yuv_transform(bool bayer = true) {
- float db_s = Hardware::EON() ? 0.5 : 1.0; // debayering does a 2x downscale on EON
+ float db_s = 1.0;
const mat3 transform = (mat3){{
1.0, 0.0, 0.0,
0.0, 1.0, 0.0,
diff --git a/selfdrive/common/params.cc b/selfdrive/common/params.cc
index 0c36511be7..29687b1a0e 100644
--- a/selfdrive/common/params.cc
+++ b/selfdrive/common/params.cc
@@ -166,7 +166,6 @@ std::unordered_map keys = {
{"ApiCache_Owner", PERSISTENT},
{"Offroad_BadNvme", CLEAR_ON_MANAGER_START},
{"Offroad_CarUnrecognized", CLEAR_ON_MANAGER_START | CLEAR_ON_IGNITION_ON},
- {"Offroad_ChargeDisabled", CLEAR_ON_MANAGER_START },
{"Offroad_ConnectivityNeeded", CLEAR_ON_MANAGER_START},
{"Offroad_ConnectivityNeededPrompt", CLEAR_ON_MANAGER_START},
{"Offroad_InvalidTime", CLEAR_ON_MANAGER_START},
diff --git a/selfdrive/common/statlog.cc b/selfdrive/common/statlog.cc
index 27dfc2ca9d..54463e9816 100644
--- a/selfdrive/common/statlog.cc
+++ b/selfdrive/common/statlog.cc
@@ -17,6 +17,9 @@ class StatlogState : public LogState {
static StatlogState s = {};
static void log(const char* metric_type, const char* metric, const char* fmt, ...) {
+ std::lock_guard lk(s.lock);
+ if (!s.initialized) s.initialize();
+
char* value_buf = nullptr;
va_list args;
va_start(args, fmt);
diff --git a/selfdrive/common/swaglog.cc b/selfdrive/common/swaglog.cc
index 618421194c..21115da10f 100644
--- a/selfdrive/common/swaglog.cc
+++ b/selfdrive/common/swaglog.cc
@@ -6,6 +6,7 @@
#include
#include
+#include
#include
#include
@@ -20,7 +21,6 @@ class SwaglogState : public LogState {
public:
SwaglogState() : LogState("ipc:///tmp/logmessage") {}
- bool initialized = false;
json11::Json::object ctx_j;
inline void initialize() {
@@ -50,20 +50,18 @@ class SwaglogState : public LogState {
ctx_j["dirty"] = !getenv("CLEAN");
// device type
- if (Hardware::EON()) {
- ctx_j["device"] = "eon";
- } else if (Hardware::TICI()) {
+ if (Hardware::TICI()) {
ctx_j["device"] = "tici";
} else {
ctx_j["device"] = "pc";
}
-
- initialized = true;
+ LogState::initialize();
}
};
static SwaglogState s = {};
bool LOG_TIMESTAMPS = getenv("LOG_TIMESTAMPS");
+uint32_t NO_FRAME_ID = std::numeric_limits::max();
static void log(int levelnum, const char* filename, int lineno, const char* func, const char* msg, const std::string& log_s) {
if (levelnum >= s.print_level) {
@@ -72,14 +70,9 @@ static void log(int levelnum, const char* filename, int lineno, const char* func
char levelnum_c = levelnum;
zmq_send(s.sock, (levelnum_c + log_s).c_str(), log_s.length() + 1, ZMQ_NOBLOCK);
}
-static void cloudlog_common(int levelnum, bool is_timestamp, const char* filename, int lineno, const char* func,
- const char* fmt, va_list args) {
- char* msg_buf = nullptr;
- int ret = vasprintf(&msg_buf, fmt, args);
- if (ret <= 0 || !msg_buf) return;
-
+static void cloudlog_common(int levelnum, const char* filename, int lineno, const char* func,
+ char* msg_buf, json11::Json::object msg_j={}) {
std::lock_guard lk(s.lock);
-
if (!s.initialized) s.initialize();
json11::Json::object log_j = json11::Json::object {
@@ -90,17 +83,10 @@ static void cloudlog_common(int levelnum, bool is_timestamp, const char* filenam
{"funcname", func},
{"created", seconds_since_epoch()}
};
-
- if (is_timestamp) {
- json11::Json::object tspt_j = json11::Json::object {
- {"timestamp", json11::Json::object{
- {"event", msg_buf},
- {"time", std::to_string(nanos_since_boot())}}
- }
- };
- log_j["msg"] = tspt_j;
- } else {
+ if (msg_j.empty()) {
log_j["msg"] = msg_buf;
+ } else {
+ log_j["msg"] = msg_j;
}
std::string log_s = ((json11::Json)log_j).dump();
@@ -109,18 +95,46 @@ static void cloudlog_common(int levelnum, bool is_timestamp, const char* filenam
}
void cloudlog_e(int levelnum, const char* filename, int lineno, const char* func,
- const char* fmt, ...){
+ const char* fmt, ...) {
va_list args;
va_start(args, fmt);
- cloudlog_common(levelnum, false, filename, lineno, func, fmt, args);
+ char* msg_buf = nullptr;
+ int ret = vasprintf(&msg_buf, fmt, args);
va_end(args);
+ if (ret <= 0 || !msg_buf) return;
+ cloudlog_common(levelnum, filename, lineno, func, msg_buf);
}
-void cloudlog_t(int levelnum, const char* filename, int lineno, const char* func,
- const char* fmt, ...){
+void cloudlog_t_common(int levelnum, const char* filename, int lineno, const char* func,
+ uint32_t frame_id, const char* fmt, va_list args) {
if (!LOG_TIMESTAMPS) return;
+ char* msg_buf = nullptr;
+ int ret = vasprintf(&msg_buf, fmt, args);
+ if (ret <= 0 || !msg_buf) return;
+ json11::Json::object tspt_j = json11::Json::object{
+ {"event", msg_buf},
+ {"time", std::to_string(nanos_since_boot())}
+ };
+ if (frame_id < NO_FRAME_ID) {
+ tspt_j["frame_id"] = std::to_string(frame_id);
+ }
+ tspt_j = json11::Json::object{{"timestamp", tspt_j}};
+ cloudlog_common(levelnum, filename, lineno, func, msg_buf, tspt_j);
+}
+
+
+void cloudlog_te(int levelnum, const char* filename, int lineno, const char* func,
+ const char* fmt, ...) {
+ va_list args;
+ va_start(args, fmt);
+ cloudlog_t_common(levelnum, filename, lineno, func, NO_FRAME_ID, fmt, args);
+ va_end(args);
+}
+void cloudlog_te(int levelnum, const char* filename, int lineno, const char* func,
+ uint32_t frame_id, const char* fmt, ...) {
va_list args;
va_start(args, fmt);
- cloudlog_common(levelnum, true, filename, lineno, func, fmt, args);
+ cloudlog_t_common(levelnum, filename, lineno, func, frame_id, fmt, args);
va_end(args);
}
+
diff --git a/selfdrive/common/swaglog.h b/selfdrive/common/swaglog.h
index 1973724880..edae9fdc34 100644
--- a/selfdrive/common/swaglog.h
+++ b/selfdrive/common/swaglog.h
@@ -12,16 +12,21 @@
void cloudlog_e(int levelnum, const char* filename, int lineno, const char* func,
const char* fmt, ...) /*__attribute__ ((format (printf, 6, 7)))*/;
-void cloudlog_t(int levelnum, const char* filename, int lineno, const char* func,
- const char* fmt, ...) /*__attribute__ ((format (printf, 6, 7)))*/;
+void cloudlog_te(int levelnum, const char* filename, int lineno, const char* func,
+ const char* fmt, ...) /*__attribute__ ((format (printf, 6, 7)))*/;
+
+void cloudlog_te(int levelnum, const char* filename, int lineno, const char* func,
+ uint32_t frame_id, const char* fmt, ...) /*__attribute__ ((format (printf, 6, 7)))*/;
+
#define cloudlog(lvl, fmt, ...) cloudlog_e(lvl, __FILE__, __LINE__, \
__func__, \
fmt, ## __VA_ARGS__);
-#define cloudlog_t_m(lvl, fmt, ...) cloudlog_t(lvl, __FILE__, __LINE__, \
- __func__, \
- fmt, ## __VA_ARGS__);
+#define cloudlog_t(lvl, ...) cloudlog_te(lvl, __FILE__, __LINE__, \
+ __func__, \
+ __VA_ARGS__);
+
#define cloudlog_rl(burst, millis, lvl, fmt, ...) \
{ \
@@ -52,7 +57,8 @@ void cloudlog_t(int levelnum, const char* filename, int lineno, const char* func
} \
}
-#define LOGT(fmt, ...) cloudlog_t_m(CLOUDLOG_DEBUG, fmt,## __VA_ARGS__)
+
+#define LOGT(...) cloudlog_t(CLOUDLOG_DEBUG, __VA_ARGS__)
#define LOGD(fmt, ...) cloudlog(CLOUDLOG_DEBUG, fmt, ## __VA_ARGS__)
#define LOG(fmt, ...) cloudlog(CLOUDLOG_INFO, fmt, ## __VA_ARGS__)
#define LOGW(fmt, ...) cloudlog(CLOUDLOG_WARNING, fmt, ## __VA_ARGS__)
diff --git a/selfdrive/common/tests/test_swaglog.cc b/selfdrive/common/tests/test_swaglog.cc
index 1d00def63d..47c5504638 100644
--- a/selfdrive/common/tests/test_swaglog.cc
+++ b/selfdrive/common/tests/test_swaglog.cc
@@ -57,9 +57,7 @@ void recv_log(int thread_cnt, int thread_msg_cnt) {
REQUIRE(ctx["version"].string_value() == COMMA_VERSION);
std::string device = "pc";
- if (Hardware::EON()) {
- device = "eon";
- } else if (Hardware::TICI()) {
+ if (Hardware::TICI()) {
device = "tici";
}
REQUIRE(ctx["device"].string_value() == device);
diff --git a/selfdrive/common/util.h b/selfdrive/common/util.h
index bf0df3bcaa..f3a24723b4 100644
--- a/selfdrive/common/util.h
+++ b/selfdrive/common/util.h
@@ -168,12 +168,18 @@ void update_max_atomic(std::atomic& max, T const& value) {
class LogState {
public:
+ bool initialized = false;
std::mutex lock;
- void *zctx;
- void *sock;
+ void *zctx = nullptr;
+ void *sock = nullptr;
int print_level;
+ const char* endpoint;
- LogState(const char* endpoint) {
+ LogState(const char* _endpoint) {
+ endpoint = _endpoint;
+ }
+
+ inline void initialize() {
zctx = zmq_ctx_new();
sock = zmq_socket(zctx, ZMQ_PUSH);
@@ -182,9 +188,13 @@ class LogState {
zmq_setsockopt(sock, ZMQ_LINGER, &timeout, sizeof(timeout));
zmq_connect(sock, endpoint);
- };
+ initialized = true;
+ }
+
~LogState() {
- zmq_close(sock);
- zmq_ctx_destroy(zctx);
+ if (initialized) {
+ zmq_close(sock);
+ zmq_ctx_destroy(zctx);
+ }
}
};
diff --git a/selfdrive/common/visionimg.cc b/selfdrive/common/visionimg.cc
index a98aabc36c..b67f0e202d 100644
--- a/selfdrive/common/visionimg.cc
+++ b/selfdrive/common/visionimg.cc
@@ -2,54 +2,6 @@
#include
-#ifdef QCOM
-#include
-#include
-#include
-#include
-#define GL_GLEXT_PROTOTYPES
-#include
-using namespace android;
-
-EGLImageTexture::EGLImageTexture(const VisionBuf *buf) {
- const int bpp = 3;
- assert((buf->len % buf->stride) == 0);
- assert((buf->stride % bpp) == 0);
-
- const int format = HAL_PIXEL_FORMAT_RGB_888;
- private_handle = new private_handle_t(buf->fd, buf->len,
- private_handle_t::PRIV_FLAGS_USES_ION|private_handle_t::PRIV_FLAGS_FRAMEBUFFER,
- 0, format,
- buf->stride/bpp, buf->len/buf->stride,
- buf->width, buf->height);
-
- // GraphicBuffer is ref counted by EGLClientBuffer(ANativeWindowBuffer), no need and not possible to release.
- GraphicBuffer* gb = new GraphicBuffer(buf->width, buf->height, (PixelFormat)format,
- GraphicBuffer::USAGE_HW_TEXTURE, buf->stride/bpp, (private_handle_t*)private_handle, false);
-
- EGLDisplay display = eglGetDisplay(EGL_DEFAULT_DISPLAY);
- assert(display != EGL_NO_DISPLAY);
-
- EGLint img_attrs[] = {EGL_IMAGE_PRESERVED_KHR, EGL_TRUE, EGL_NONE};
- img_khr = eglCreateImageKHR(display, EGL_NO_CONTEXT,
- EGL_NATIVE_BUFFER_ANDROID, gb->getNativeBuffer(), img_attrs);
- assert(img_khr != EGL_NO_IMAGE_KHR);
-
- glGenTextures(1, &frame_tex);
- glBindTexture(GL_TEXTURE_2D, frame_tex);
- glEGLImageTargetTexture2DOES(GL_TEXTURE_2D, img_khr);
-}
-
-EGLImageTexture::~EGLImageTexture() {
- glDeleteTextures(1, &frame_tex);
- EGLDisplay display = eglGetDisplay(EGL_DEFAULT_DISPLAY);
- assert(display != EGL_NO_DISPLAY);
- eglDestroyImageKHR(display, img_khr);
- delete (private_handle_t*)private_handle;
-}
-
-#else // ifdef QCOM
-
EGLImageTexture::EGLImageTexture(const VisionBuf *buf) {
glGenTextures(1, &frame_tex);
glBindTexture(GL_TEXTURE_2D, frame_tex);
@@ -60,4 +12,3 @@ EGLImageTexture::EGLImageTexture(const VisionBuf *buf) {
EGLImageTexture::~EGLImageTexture() {
glDeleteTextures(1, &frame_tex);
}
-#endif // ifdef QCOM
diff --git a/selfdrive/common/visionimg.h b/selfdrive/common/visionimg.h
index e8917f3bd6..0cb41a32b8 100644
--- a/selfdrive/common/visionimg.h
+++ b/selfdrive/common/visionimg.h
@@ -8,20 +8,9 @@
#include
#endif
-#ifdef QCOM
-#include
-#define EGL_EGLEXT_PROTOTYPES
-#include
-#undef Status
-#endif
-
class EGLImageTexture {
public:
EGLImageTexture(const VisionBuf *buf);
~EGLImageTexture();
GLuint frame_tex = 0;
-#ifdef QCOM
- void *private_handle = nullptr;
- EGLImageKHR img_khr = 0;
-#endif
};
diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py
index 25b0c41419..5bd75548f0 100755
--- a/selfdrive/controls/controlsd.py
+++ b/selfdrive/controls/controlsd.py
@@ -20,13 +20,13 @@ from selfdrive.controls.lib.drive_helpers import get_lag_adjusted_curvature
from selfdrive.controls.lib.longcontrol import LongControl
from selfdrive.controls.lib.latcontrol_pid import LatControlPID
from selfdrive.controls.lib.latcontrol_indi import LatControlINDI
-from selfdrive.controls.lib.latcontrol_lqr import LatControlLQR
from selfdrive.controls.lib.latcontrol_angle import LatControlAngle
+from selfdrive.controls.lib.latcontrol_torque import LatControlTorque
from selfdrive.controls.lib.events import Events, ET
from selfdrive.controls.lib.alertmanager import AlertManager, set_offroad_alert
from selfdrive.controls.lib.vehicle_model import VehicleModel
from selfdrive.locationd.calibrationd import Calibration
-from selfdrive.hardware import HARDWARE, TICI, EON
+from selfdrive.hardware import HARDWARE, TICI
from selfdrive.manager.process_config import managed_processes
SOFT_DISABLE_TIME = 3 # seconds
@@ -36,9 +36,8 @@ LANE_DEPARTURE_THRESHOLD = 0.1
REPLAY = "REPLAY" in os.environ
SIMULATION = "SIMULATION" in os.environ
NOSENSOR = "NOSENSOR" in os.environ
-IGNORE_PROCESSES = {"rtshield", "uploader", "deleter", "loggerd", "logmessaged", "tombstoned",
- "logcatd", "proclogd", "clocksd", "updated", "timezoned", "manage_athenad",
- "statsd", "shutdownd"} | \
+IGNORE_PROCESSES = {"uploader", "deleter", "loggerd", "logmessaged", "tombstoned", "statsd",
+ "logcatd", "proclogd", "clocksd", "updated", "timezoned", "manage_athenad"} | \
{k for k, v in managed_processes.items() if not v.enabled}
ThermalStatus = log.DeviceState.ThermalStatus
@@ -52,7 +51,7 @@ ButtonEvent = car.CarState.ButtonEvent
SafetyModel = car.CarParams.SafetyModel
IGNORED_SAFETY_MODES = (SafetyModel.silent, SafetyModel.noOutput)
-CSID_MAP = {"0": EventName.roadCameraError, "1": EventName.wideRoadCameraError, "2": EventName.driverCameraError}
+CSID_MAP = {"1": EventName.roadCameraError, "2": EventName.wideRoadCameraError, "0": EventName.driverCameraError}
ACTUATOR_FIELDS = tuple(car.CarControl.Actuators.schema.fields.keys())
ACTIVE_STATES = (State.enabled, State.softDisabling, State.overriding)
ENABLED_STATES = (State.preEnabled, *ACTIVE_STATES)
@@ -145,8 +144,8 @@ class Controls:
self.LaC = LatControlPID(self.CP, self.CI)
elif self.CP.lateralTuning.which() == 'indi':
self.LaC = LatControlINDI(self.CP, self.CI)
- elif self.CP.lateralTuning.which() == 'lqr':
- self.LaC = LatControlLQR(self.CP, self.CI)
+ elif self.CP.lateralTuning.which() == 'torque':
+ self.LaC = LatControlTorque(self.CP, self.CI)
self.initialized = False
self.state = State.disabled
@@ -220,11 +219,7 @@ class Controls:
if not self.CP.notCar:
self.events.add_from_msg(self.sm['driverMonitoringState'].events)
- # Create events for battery, temperature, disk space, and memory
- if EON and (self.sm['peripheralState'].pandaType != PandaType.uno) and \
- self.sm['deviceState'].batteryPercent < 1 and self.sm['deviceState'].chargingError:
- # at zero percent battery, while discharging, OP should not allowed
- self.events.add(EventName.lowBattery)
+ # Create events for temperature, disk space, and memory
if self.sm['deviceState'].thermalStatus >= ThermalStatus.red:
self.events.add(EventName.overheat)
if self.sm['deviceState'].freeSpacePercent < 7 and not SIMULATION:
@@ -235,7 +230,7 @@ class Controls:
self.events.add(EventName.lowMemory)
# TODO: enable this once loggerd CPU usage is more reasonable
- #cpus = list(self.sm['deviceState'].cpuUsagePercent)[:(-1 if EON else None)]
+ #cpus = list(self.sm['deviceState'].cpuUsagePercent)
#if max(cpus, default=0) > 95 and not SIMULATION:
# self.events.add(EventName.highCpuUsage)
@@ -270,7 +265,9 @@ class Controls:
LaneChangeState.laneChangeFinishing):
self.events.add(EventName.laneChange)
- if not CS.canValid:
+ if CS.canTimeout:
+ self.events.add(EventName.canBusMissing)
+ elif not CS.canValid:
self.events.add(EventName.canError)
for i, pandaState in enumerate(self.sm['pandaStates']):
@@ -367,7 +364,7 @@ class Controls:
self.events.add(EventName.localizerMalfunction)
# Check if all manager processes are running
- not_running = {p.name for p in self.sm['managerState'].processes if not p.running}
+ not_running = {p.name for p in self.sm['managerState'].processes if not p.running and p.shouldBeRunning}
if self.sm.rcv_frame['managerState'] and (not_running - IGNORE_PROCESSES):
self.events.add(EventName.processNotRunning)
@@ -569,7 +566,7 @@ class Controls:
lat_plan.curvatureRates)
actuators.steer, actuators.steeringAngleDeg, lac_log = self.LaC.update(CC.latActive, CS, self.CP, self.VM,
params, self.last_actuators, desired_curvature,
- desired_curvature_rate)
+ desired_curvature_rate, self.sm['liveLocationKalman'])
else:
lac_log = log.ControlsState.LateralDebugState.new_message()
if self.sm.rcv_frame['testJoystick'] > 0:
@@ -733,8 +730,8 @@ class Controls:
controlsState.lateralControlState.angleState = lac_log
elif lat_tuning == 'pid':
controlsState.lateralControlState.pidState = lac_log
- elif lat_tuning == 'lqr':
- controlsState.lateralControlState.lqrState = lac_log
+ elif lat_tuning == 'torque':
+ controlsState.lateralControlState.torqueState = lac_log
elif lat_tuning == 'indi':
controlsState.lateralControlState.indiState = lac_log
diff --git a/selfdrive/controls/lib/alerts_offroad.json b/selfdrive/controls/lib/alerts_offroad.json
index 4f0ffa794d..2f85ea917a 100644
--- a/selfdrive/controls/lib/alerts_offroad.json
+++ b/selfdrive/controls/lib/alerts_offroad.json
@@ -1,8 +1,4 @@
{
- "Offroad_ChargeDisabled": {
- "text": "EON charging disabled after car being off for more than 30 hours. Turn ignition on to start charging again.",
- "severity": 0
- },
"Offroad_TemperatureTooHigh": {
"text": "Device temperature too high. System won't start.",
"severity": 1
diff --git a/selfdrive/controls/lib/events.py b/selfdrive/controls/lib/events.py
index a355f14623..df22d97d6b 100644
--- a/selfdrive/controls/lib/events.py
+++ b/selfdrive/controls/lib/events.py
@@ -724,6 +724,7 @@ EVENTS: Dict[int, Dict[str, Union[Alert, AlertCallbackType]]] = {
EventName.controlsMismatch: {
ET.IMMEDIATE_DISABLE: ImmediateDisableAlert("Controls Mismatch"),
+ ET.NO_ENTRY: NoEntryAlert("Controls Mismatch"),
},
EventName.roadCameraError: {
@@ -732,13 +733,13 @@ EVENTS: Dict[int, Dict[str, Union[Alert, AlertCallbackType]]] = {
creation_delay=30.),
},
- EventName.driverCameraError: {
+ EventName.wideRoadCameraError: {
ET.PERMANENT: NormalPermanentAlert("Camera CRC Error - Road Fisheye",
duration=1.,
creation_delay=30.),
},
- EventName.wideRoadCameraError: {
+ EventName.driverCameraError: {
ET.PERMANENT: NormalPermanentAlert("Camera CRC Error - Driver",
duration=1.,
creation_delay=30.),
@@ -757,7 +758,7 @@ EVENTS: Dict[int, Dict[str, Union[Alert, AlertCallbackType]]] = {
# - CAN data is received, but some message are not received at the right frequency
# If you're not writing a new car port, this is usually cause by faulty wiring
EventName.canError: {
- ET.IMMEDIATE_DISABLE: ImmediateDisableAlert("CAN Error: Check Connections"),
+ ET.IMMEDIATE_DISABLE: ImmediateDisableAlert("CAN Error"),
ET.PERMANENT: Alert(
"CAN Error: Check Connections",
"",
@@ -766,7 +767,15 @@ EVENTS: Dict[int, Dict[str, Union[Alert, AlertCallbackType]]] = {
ET.NO_ENTRY: NoEntryAlert("CAN Error: Check Connections"),
},
- EventName.canBusMissing: {},
+ EventName.canBusMissing: {
+ ET.IMMEDIATE_DISABLE: ImmediateDisableAlert("CAN Bus Disconnected"),
+ ET.PERMANENT: Alert(
+ "CAN Bus Disconnected: Likely Faulty Cable",
+ "",
+ AlertStatus.normal, AlertSize.small,
+ Priority.LOW, VisualAlert.none, AudibleAlert.none, 1., creation_delay=1.),
+ ET.NO_ENTRY: NoEntryAlert("CAN Bus Disconnected: Check Connections"),
+ },
EventName.steerUnavailable: {
ET.IMMEDIATE_DISABLE: ImmediateDisableAlert("LKAS Fault: Restart the Car"),
diff --git a/selfdrive/controls/lib/lane_planner.py b/selfdrive/controls/lib/lane_planner.py
index 0230255ef9..aedf61a073 100644
--- a/selfdrive/controls/lib/lane_planner.py
+++ b/selfdrive/controls/lib/lane_planner.py
@@ -3,7 +3,7 @@ from cereal import log
from common.filter_simple import FirstOrderFilter
from common.numpy_fast import interp
from common.realtime import DT_MDL
-from selfdrive.hardware import EON, TICI
+from selfdrive.hardware import TICI
from selfdrive.swaglog import cloudlog
@@ -13,9 +13,7 @@ TRAJECTORY_SIZE = 33
# the model knows the difference between TICI and EON
# so a path offset is not needed
PATH_OFFSET = 0.00
-if EON:
- CAMERA_OFFSET = -0.06
-elif TICI:
+if TICI:
CAMERA_OFFSET = 0.04
else:
CAMERA_OFFSET = 0.0
diff --git a/selfdrive/controls/lib/latcontrol.py b/selfdrive/controls/lib/latcontrol.py
index 4a67715fcb..0d137df733 100644
--- a/selfdrive/controls/lib/latcontrol.py
+++ b/selfdrive/controls/lib/latcontrol.py
@@ -16,7 +16,7 @@ class LatControl(ABC):
self.steer_max = 1.0
@abstractmethod
- def update(self, active, CS, CP, VM, params, last_actuators, desired_curvature, desired_curvature_rate):
+ def update(self, active, CS, CP, VM, params, last_actuators, desired_curvature, desired_curvature_rate, llk):
pass
def reset(self):
diff --git a/selfdrive/controls/lib/latcontrol_angle.py b/selfdrive/controls/lib/latcontrol_angle.py
index c935356311..8d09432b6a 100644
--- a/selfdrive/controls/lib/latcontrol_angle.py
+++ b/selfdrive/controls/lib/latcontrol_angle.py
@@ -7,7 +7,7 @@ STEER_ANGLE_SATURATION_THRESHOLD = 2.5 # Degrees
class LatControlAngle(LatControl):
- def update(self, active, CS, CP, VM, params, last_actuators, desired_curvature, desired_curvature_rate):
+ def update(self, active, CS, CP, VM, params, last_actuators, desired_curvature, desired_curvature_rate, llk):
angle_log = log.ControlsState.LateralAngleState.new_message()
if CS.vEgo < MIN_STEER_SPEED or not active:
diff --git a/selfdrive/controls/lib/latcontrol_indi.py b/selfdrive/controls/lib/latcontrol_indi.py
index 2db2d43881..4d2a727623 100644
--- a/selfdrive/controls/lib/latcontrol_indi.py
+++ b/selfdrive/controls/lib/latcontrol_indi.py
@@ -63,7 +63,7 @@ class LatControlINDI(LatControl):
self.steer_filter.x = 0.
self.speed = 0.
- def update(self, active, CS, CP, VM, params, last_actuators, desired_curvature, desired_curvature_rate):
+ def update(self, active, CS, CP, VM, params, last_actuators, desired_curvature, desired_curvature_rate, llk):
self.speed = CS.vEgo
# Update Kalman filter
y = np.array([[math.radians(CS.steeringAngleDeg)], [math.radians(CS.steeringRateDeg)]])
diff --git a/selfdrive/controls/lib/latcontrol_lqr.py b/selfdrive/controls/lib/latcontrol_lqr.py
deleted file mode 100644
index 583244882c..0000000000
--- a/selfdrive/controls/lib/latcontrol_lqr.py
+++ /dev/null
@@ -1,84 +0,0 @@
-import math
-import numpy as np
-
-from common.numpy_fast import clip
-from common.realtime import DT_CTRL
-from cereal import log
-from selfdrive.controls.lib.latcontrol import LatControl, MIN_STEER_SPEED
-
-
-class LatControlLQR(LatControl):
- def __init__(self, CP, CI):
- super().__init__(CP, CI)
- self.scale = CP.lateralTuning.lqr.scale
- self.ki = CP.lateralTuning.lqr.ki
-
- self.A = np.array(CP.lateralTuning.lqr.a).reshape((2, 2))
- self.B = np.array(CP.lateralTuning.lqr.b).reshape((2, 1))
- self.C = np.array(CP.lateralTuning.lqr.c).reshape((1, 2))
- self.K = np.array(CP.lateralTuning.lqr.k).reshape((1, 2))
- self.L = np.array(CP.lateralTuning.lqr.l).reshape((2, 1))
- self.dc_gain = CP.lateralTuning.lqr.dcGain
-
- self.x_hat = np.array([[0], [0]])
- self.i_unwind_rate = 0.3 * DT_CTRL
- self.i_rate = 1.0 * DT_CTRL
-
- self.reset()
-
- def reset(self):
- super().reset()
- self.i_lqr = 0.0
-
- def update(self, active, CS, CP, VM, params, last_actuators, desired_curvature, desired_curvature_rate):
- lqr_log = log.ControlsState.LateralLQRState.new_message()
-
- torque_scale = (0.45 + CS.vEgo / 60.0)**2 # Scale actuator model with speed
-
- # Subtract offset. Zero angle should correspond to zero torque
- steering_angle_no_offset = CS.steeringAngleDeg - params.angleOffsetAverageDeg
-
- desired_angle = math.degrees(VM.get_steer_from_curvature(-desired_curvature, CS.vEgo, params.roll))
-
- instant_offset = params.angleOffsetDeg - params.angleOffsetAverageDeg
- desired_angle += instant_offset # Only add offset that originates from vehicle model errors
- lqr_log.steeringAngleDesiredDeg = desired_angle
-
- # Update Kalman filter
- angle_steers_k = float(self.C.dot(self.x_hat))
- e = steering_angle_no_offset - angle_steers_k
- self.x_hat = self.A.dot(self.x_hat) + self.B.dot(CS.steeringTorqueEps / torque_scale) + self.L.dot(e)
-
- if CS.vEgo < MIN_STEER_SPEED or not active:
- lqr_log.active = False
- lqr_output = 0.
- output_steer = 0.
- self.reset()
- else:
- lqr_log.active = True
-
- # LQR
- u_lqr = float(desired_angle / self.dc_gain - self.K.dot(self.x_hat))
- lqr_output = torque_scale * u_lqr / self.scale
-
- # Integrator
- if CS.steeringPressed:
- self.i_lqr -= self.i_unwind_rate * float(np.sign(self.i_lqr))
- else:
- error = desired_angle - angle_steers_k
- i = self.i_lqr + self.ki * self.i_rate * error
- control = lqr_output + i
-
- if (error >= 0 and (control <= self.steer_max or i < 0.0)) or \
- (error <= 0 and (control >= -self.steer_max or i > 0.0)):
- self.i_lqr = i
-
- output_steer = lqr_output + self.i_lqr
- output_steer = clip(output_steer, -self.steer_max, self.steer_max)
-
- lqr_log.steeringAngleDeg = angle_steers_k
- lqr_log.i = self.i_lqr
- lqr_log.output = output_steer
- lqr_log.lqrOutput = lqr_output
- lqr_log.saturated = self._check_saturation(self.steer_max - abs(output_steer) < 1e-3, CS)
- return output_steer, desired_angle, lqr_log
diff --git a/selfdrive/controls/lib/latcontrol_pid.py b/selfdrive/controls/lib/latcontrol_pid.py
index 67d17e088c..813ba13aba 100644
--- a/selfdrive/controls/lib/latcontrol_pid.py
+++ b/selfdrive/controls/lib/latcontrol_pid.py
@@ -17,7 +17,7 @@ class LatControlPID(LatControl):
super().reset()
self.pid.reset()
- def update(self, active, CS, CP, VM, params, last_actuators, desired_curvature, desired_curvature_rate):
+ def update(self, active, CS, CP, VM, params, last_actuators, desired_curvature, desired_curvature_rate, llk):
pid_log = log.ControlsState.LateralPIDState.new_message()
pid_log.steeringAngleDeg = float(CS.steeringAngleDeg)
pid_log.steeringRateDeg = float(CS.steeringRateDeg)
diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py
new file mode 100644
index 0000000000..2816b20149
--- /dev/null
+++ b/selfdrive/controls/lib/latcontrol_torque.py
@@ -0,0 +1,79 @@
+import math
+from selfdrive.controls.lib.pid import PIDController
+from common.numpy_fast import interp
+from selfdrive.controls.lib.latcontrol import LatControl, MIN_STEER_SPEED
+from selfdrive.controls.lib.vehicle_model import ACCELERATION_DUE_TO_GRAVITY
+from cereal import log
+
+# At higher speeds (25+mph) we can assume:
+# Lateral acceleration achieved by a specific car correlates to
+# torque applied to the steering rack. It does not correlate to
+# wheel slip, or to speed.
+
+# This controller applies torque to achieve desired lateral
+# accelerations. To compensate for the low speed effects we
+# use a LOW_SPEED_FACTOR in the error. Additionally there is
+# friction in the steering wheel that needs to be overcome to
+# move it at all, this is compensated for too.
+
+
+LOW_SPEED_FACTOR = 200
+JERK_THRESHOLD = 0.2
+
+
+class LatControlTorque(LatControl):
+ def __init__(self, CP, CI):
+ super().__init__(CP, CI)
+ self.pid = PIDController(CP.lateralTuning.torque.kp, CP.lateralTuning.torque.ki,
+ k_f=CP.lateralTuning.torque.kf, pos_limit=1.0, neg_limit=-1.0)
+ self.get_steer_feedforward = CI.get_steer_feedforward_function()
+ self.steer_max = 1.0
+ self.pid.pos_limit = self.steer_max
+ self.pid.neg_limit = -self.steer_max
+ self.use_steering_angle = CP.lateralTuning.torque.useSteeringAngle
+ self.friction = CP.lateralTuning.torque.friction
+
+ def reset(self):
+ super().reset()
+ self.pid.reset()
+
+ def update(self, active, CS, CP, VM, params, last_actuators, desired_curvature, desired_curvature_rate, llk):
+ pid_log = log.ControlsState.LateralTorqueState.new_message()
+
+ if CS.vEgo < MIN_STEER_SPEED or not active:
+ output_torque = 0.0
+ pid_log.active = False
+ self.pid.reset()
+ else:
+ if self.use_steering_angle:
+ actual_curvature = -VM.calc_curvature(math.radians(CS.steeringAngleDeg - params.angleOffsetDeg), CS.vEgo, params.roll)
+ else:
+ actual_curvature = llk.angularVelocityCalibrated.value[2] / CS.vEgo
+ desired_lateral_accel = desired_curvature * CS.vEgo**2
+ desired_lateral_jerk = desired_curvature_rate * CS.vEgo**2
+ actual_lateral_accel = actual_curvature * CS.vEgo**2
+
+ setpoint = desired_lateral_accel + LOW_SPEED_FACTOR * desired_curvature
+ measurement = actual_lateral_accel + LOW_SPEED_FACTOR * actual_curvature
+ error = setpoint - measurement
+ pid_log.error = error
+
+ ff = desired_lateral_accel - params.roll * ACCELERATION_DUE_TO_GRAVITY
+ output_torque = self.pid.update(error,
+ override=CS.steeringPressed, feedforward=ff,
+ speed=CS.vEgo,
+ freeze_integrator=CS.steeringRateLimited)
+
+ friction_compensation = interp(desired_lateral_jerk, [-JERK_THRESHOLD, JERK_THRESHOLD], [-self.friction, self.friction])
+ output_torque += friction_compensation
+
+ pid_log.active = True
+ pid_log.p = self.pid.p
+ pid_log.i = self.pid.i
+ pid_log.d = self.pid.d
+ pid_log.f = self.pid.f
+ pid_log.output = -output_torque
+ pid_log.saturated = self._check_saturation(self.steer_max - abs(output_torque) < 1e-3, CS)
+
+ #TODO left is positive in this convention
+ return -output_torque, 0.0, pid_log
diff --git a/selfdrive/controls/lib/lateral_mpc_lib/SConscript b/selfdrive/controls/lib/lateral_mpc_lib/SConscript
index 76f7fadce7..df1e2a2a1a 100644
--- a/selfdrive/controls/lib/lateral_mpc_lib/SConscript
+++ b/selfdrive/controls/lib/lateral_mpc_lib/SConscript
@@ -48,7 +48,6 @@ acados_templates_dir = '#pyextra/acados_template/c_templates_tera'
source_list = ['lat_mpc.py',
f'{acados_dir}/include/acados_c/ocp_nlp_interface.h',
- f'{acados_dir}/aarch64/lib/libacados.so',
f'{acados_dir}/x86_64/lib/libacados.so',
f'{acados_dir}/larch64/lib/libacados.so',
f'{acados_templates_dir}/acados_solver.in.c',
diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/SConscript b/selfdrive/controls/lib/longitudinal_mpc_lib/SConscript
index fd0da63a37..5a9e69c297 100644
--- a/selfdrive/controls/lib/longitudinal_mpc_lib/SConscript
+++ b/selfdrive/controls/lib/longitudinal_mpc_lib/SConscript
@@ -55,7 +55,6 @@ acados_templates_dir = '#pyextra/acados_template/c_templates_tera'
source_list = ['long_mpc.py',
f'{acados_dir}/include/acados_c/ocp_nlp_interface.h',
- f'{acados_dir}/aarch64/lib/libacados.so',
f'{acados_dir}/x86_64/lib/libacados.so',
f'{acados_dir}/larch64/lib/libacados.so',
f'{acados_templates_dir}/acados_solver.in.c',
diff --git a/selfdrive/controls/lib/tests/test_latcontrol.py b/selfdrive/controls/lib/tests/test_latcontrol.py
index 8345840eca..503eaaa6a4 100755
--- a/selfdrive/controls/lib/tests/test_latcontrol.py
+++ b/selfdrive/controls/lib/tests/test_latcontrol.py
@@ -9,7 +9,7 @@ from selfdrive.car.honda.values import CAR as HONDA
from selfdrive.car.toyota.values import CAR as TOYOTA
from selfdrive.car.nissan.values import CAR as NISSAN
from selfdrive.controls.lib.latcontrol_pid import LatControlPID
-from selfdrive.controls.lib.latcontrol_lqr import LatControlLQR
+from selfdrive.controls.lib.latcontrol_torque import LatControlTorque
from selfdrive.controls.lib.latcontrol_indi import LatControlINDI
from selfdrive.controls.lib.latcontrol_angle import LatControlAngle
from selfdrive.controls.lib.vehicle_model import VehicleModel
@@ -17,7 +17,7 @@ from selfdrive.controls.lib.vehicle_model import VehicleModel
class TestLatControl(unittest.TestCase):
- @parameterized.expand([(HONDA.CIVIC, LatControlPID), (TOYOTA.RAV4, LatControlLQR), (TOYOTA.PRIUS, LatControlINDI), (NISSAN.LEAF, LatControlAngle)])
+ @parameterized.expand([(HONDA.CIVIC, LatControlPID), (TOYOTA.RAV4, LatControlTorque), (TOYOTA.PRIUS, LatControlINDI), (NISSAN.LEAF, LatControlAngle)])
def test_saturation(self, car_name, controller):
CarInterface, CarController, CarState = interfaces[car_name]
CP = CarInterface.get_params(car_name)
diff --git a/selfdrive/debug/README.md b/selfdrive/debug/README.md
index d49db25d09..f6903170a2 100644
--- a/selfdrive/debug/README.md
+++ b/selfdrive/debug/README.md
@@ -52,7 +52,7 @@ optional arguments:
-h, --help show this help message and exit
--debug enable ISO-TP/UDS stack debugging output
-This tool is meant to run directly on a vehicle-installed comma two or comma three, with
+This tool is meant to run directly on a vehicle-installed comma three, with
the openpilot/tmux processes stopped. It should also work on a separate PC with a USB-
attached comma panda. Vehicle ignition must be on. Recommend engine not be running when
making changes. Must turn ignition off and on again for any changes to take effect.
diff --git a/selfdrive/debug/adb.sh b/selfdrive/debug/adb.sh
index 8a04d4aefd..919a82fefc 100755
--- a/selfdrive/debug/adb.sh
+++ b/selfdrive/debug/adb.sh
@@ -4,12 +4,7 @@ set -e
PORT=5555
setprop service.adb.tcp.port $PORT
-if [ -f /EON ]; then
- stop adbd
- start adbd
-else
- sudo systemctl start adbd
-fi
+sudo systemctl start adbd
IP=$(echo $SSH_CONNECTION | awk '{ print $3}')
echo "then, connect on your computer:"
diff --git a/selfdrive/debug/filter_log_message.py b/selfdrive/debug/filter_log_message.py
index 98e7d87246..452358cd5c 100755
--- a/selfdrive/debug/filter_log_message.py
+++ b/selfdrive/debug/filter_log_message.py
@@ -56,7 +56,7 @@ if __name__ == "__main__":
logs = None
if len(args.route):
r = Route(args.route[0])
- logs = r.log_paths()
+ logs = [q_log if r_log is None else r_log for (q_log, r_log) in zip(r.qlog_paths(), r.log_paths())]
if len(args.route) == 2 and logs:
n = int(args.route[1])
@@ -71,6 +71,8 @@ if __name__ == "__main__":
for m in lr:
if m.which() == 'logMessage':
print_logmessage(m.logMonoTime, m.logMessage, min_level)
+ elif m.which() == 'errorLogMessage' and 'qlog' in log:
+ print_logmessage(m.logMonoTime, m.errorLogMessage, min_level)
elif m.which() == 'androidLog':
print_androidlog(m.logMonoTime, m.androidLog)
else:
diff --git a/selfdrive/debug/internal/measure_modeld_packet_drop.py b/selfdrive/debug/internal/measure_modeld_packet_drop.py
index d5f65d06b1..6b7f7dbd13 100755
--- a/selfdrive/debug/internal/measure_modeld_packet_drop.py
+++ b/selfdrive/debug/internal/measure_modeld_packet_drop.py
@@ -1,11 +1,12 @@
#!/usr/bin/env python3
import cereal.messaging as messaging
+from typing import Optional
if __name__ == "__main__":
modeld_sock = messaging.sub_sock("modelV2")
last_frame_id = None
- start_t = None
+ start_t: Optional[int] = None
frame_cnt = 0
dropped = 0
diff --git a/selfdrive/debug/live_cpu_and_temp.py b/selfdrive/debug/live_cpu_and_temp.py
index e46c0b0a1f..b244f7cb0e 100755
--- a/selfdrive/debug/live_cpu_and_temp.py
+++ b/selfdrive/debug/live_cpu_and_temp.py
@@ -1,9 +1,10 @@
#!/usr/bin/env python3
import argparse
+import capnp
from cereal.messaging import SubMaster
from common.numpy_fast import mean
-
+from typing import Optional
def cputime_total(ct):
return ct.user + ct.nice + ct.system + ct.idle + ct.iowait + ct.irq + ct.softirq
@@ -40,8 +41,8 @@ if __name__ == "__main__":
total_times = [0.]*8
busy_times = [0.]*8
- prev_proclog = None
- prev_proclog_t = None
+ prev_proclog: Optional[capnp._DynamicStructReader] = None
+ prev_proclog_t: Optional[int] = None
while True:
sm.update()
@@ -73,7 +74,7 @@ if __name__ == "__main__":
print(f"CPU {100.0 * mean(cores):.2f}% - RAM: {last_mem:.2f}% - Temp {last_temp:.2f}C")
- if args.cpu and prev_proclog is not None:
+ if args.cpu and prev_proclog is not None and prev_proclog_t is not None:
procs = {}
dt = (sm.logMonoTime['procLog'] - prev_proclog_t) / 1e9
for proc in m.procs:
diff --git a/selfdrive/debug/profiling/snapdragon/setup-agnos.sh b/selfdrive/debug/profiling/snapdragon/setup-agnos.sh
index 5099fb09cb..f036ca2111 100755
--- a/selfdrive/debug/profiling/snapdragon/setup-agnos.sh
+++ b/selfdrive/debug/profiling/snapdragon/setup-agnos.sh
@@ -4,4 +4,4 @@
cd SnapdragonProfiler/service
mv android real_android
-ln -s iot_rb5_lu/ android
+ln -s agl/ android
diff --git a/selfdrive/debug/test_fw_query_on_routes.py b/selfdrive/debug/test_fw_query_on_routes.py
index 6b29ae820d..0f9e316cd0 100755
--- a/selfdrive/debug/test_fw_query_on_routes.py
+++ b/selfdrive/debug/test_fw_query_on_routes.py
@@ -25,6 +25,7 @@ SUPPORTED_CARS |= set(interface_names['hyundai'])
SUPPORTED_CARS |= set(interface_names['volkswagen'])
SUPPORTED_CARS |= set(interface_names['mazda'])
SUPPORTED_CARS |= set(interface_names['subaru'])
+SUPPORTED_CARS |= set(interface_names['nissan'])
try:
from xx.pipeline.c.CarState import migration
@@ -74,7 +75,8 @@ if __name__ == "__main__":
for msg in lr:
if msg.which() == "pandaStates":
- if msg.pandaStates[0].pandaType not in ['uno', 'blackPanda', 'dos']:
+ if msg.pandaStates[0].pandaType not in ('uno', 'blackPanda', 'dos'):
+ print("wrong panda type")
break
elif msg.which() == "carParams":
@@ -82,6 +84,7 @@ if __name__ == "__main__":
car_fw = msg.carParams.carFw
if len(car_fw) == 0:
+ print("no fw")
break
live_fingerprint = msg.carParams.carFingerprint
@@ -91,6 +94,7 @@ if __name__ == "__main__":
live_fingerprint = args.car
if live_fingerprint not in SUPPORTED_CARS:
+ print("not in supported cars")
break
fw_versions_dict = build_fw_dict(car_fw)
diff --git a/selfdrive/debug/uiview.py b/selfdrive/debug/uiview.py
index 29465ecc8d..93d901f7c9 100755
--- a/selfdrive/debug/uiview.py
+++ b/selfdrive/debug/uiview.py
@@ -1,11 +1,15 @@
#!/usr/bin/env python3
import time
-from cereal import messaging, log
+
+from cereal import car, log, messaging
+from common.params import Params
from selfdrive.manager.process_config import managed_processes
if __name__ == "__main__":
- procs = ['camerad', 'ui', 'modeld', 'calibrationd']
+ CP = car.CarParams(notCar=True)
+ Params().put("CarParams", CP.to_bytes())
+ procs = ['camerad', 'ui', 'modeld', 'calibrationd']
for p in procs:
managed_processes[p].start()
diff --git a/selfdrive/debug/vw_mqb_config.py b/selfdrive/debug/vw_mqb_config.py
index 1dd28d371f..c55a058159 100755
--- a/selfdrive/debug/vw_mqb_config.py
+++ b/selfdrive/debug/vw_mqb_config.py
@@ -23,7 +23,7 @@ if __name__ == "__main__":
desc_text = "Shows Volkswagen EPS software and coding info, and enables or disables Heading Control Assist " + \
"(Lane Assist). Useful for enabling HCA on cars without factory Lane Assist that want to use " + \
"openpilot integrated at the CAN gateway (J533)."
- epilog_text = "This tool is meant to run directly on a vehicle-installed comma two or comma three, with the " + \
+ epilog_text = "This tool is meant to run directly on a vehicle-installed comma three, with the " + \
"openpilot/tmux processes stopped. It should also work on a separate PC with a USB-attached comma " + \
"panda. Vehicle ignition must be on. Recommend engine not be running when making changes. Must " + \
"turn ignition off and on again for any changes to take effect."
diff --git a/selfdrive/hardware/__init__.py b/selfdrive/hardware/__init__.py
index 3babf1bb5d..03dfce86d7 100644
--- a/selfdrive/hardware/__init__.py
+++ b/selfdrive/hardware/__init__.py
@@ -2,18 +2,14 @@ import os
from typing import cast
from selfdrive.hardware.base import HardwareBase
-from selfdrive.hardware.eon.hardware import Android
from selfdrive.hardware.tici.hardware import Tici
from selfdrive.hardware.pc.hardware import Pc
-EON = os.path.isfile('/EON')
TICI = os.path.isfile('/TICI')
-PC = not (EON or TICI)
+PC = not TICI
-if EON:
- HARDWARE = cast(HardwareBase, Android())
-elif TICI:
+if TICI:
HARDWARE = cast(HardwareBase, Tici())
else:
HARDWARE = cast(HardwareBase, Pc())
diff --git a/selfdrive/hardware/base.h b/selfdrive/hardware/base.h
index 05e55cc034..2826d09700 100644
--- a/selfdrive/hardware/base.h
+++ b/selfdrive/hardware/base.h
@@ -20,6 +20,5 @@ public:
static void set_ssh_enabled(bool enabled) {}
static bool PC() { return false; }
- static bool EON() { return false; }
static bool TICI() { return false; }
};
diff --git a/selfdrive/hardware/base.py b/selfdrive/hardware/base.py
index c05c75bbb2..2f251d3364 100644
--- a/selfdrive/hardware/base.py
+++ b/selfdrive/hardware/base.py
@@ -44,7 +44,7 @@ class HardwareBase(ABC):
pass
@abstractmethod
- def get_imei(self, slot):
+ def get_imei(self, slot) -> str:
pass
@abstractmethod
diff --git a/selfdrive/hardware/eon/__init__.py b/selfdrive/hardware/eon/__init__.py
deleted file mode 100644
index e69de29bb2..0000000000
diff --git a/selfdrive/hardware/eon/androidd.py b/selfdrive/hardware/eon/androidd.py
deleted file mode 100755
index 3d91468b90..0000000000
--- a/selfdrive/hardware/eon/androidd.py
+++ /dev/null
@@ -1,91 +0,0 @@
-#!/usr/bin/env python3
-import os
-import time
-import psutil
-from typing import Optional
-
-import cereal.messaging as messaging
-from common.realtime import set_core_affinity, set_realtime_priority
-from selfdrive.swaglog import cloudlog
-
-
-MAX_MODEM_CRASHES = 3
-MODEM_PATH = "/sys/devices/soc/2080000.qcom,mss/subsys5"
-WATCHED_PROCS = ["zygote", "zygote64", "system_server", "/system/bin/servicemanager", "/system/bin/surfaceflinger"]
-
-
-def get_modem_crash_count() -> Optional[int]:
- try:
- with open(os.path.join(MODEM_PATH, "crash_count")) as f:
- return int(f.read())
- except Exception:
- cloudlog.exception("Error reading modem crash count")
- return None
-
-def get_modem_state() -> str:
- try:
- with open(os.path.join(MODEM_PATH, "state")) as f:
- return f.read().strip()
- except Exception:
- cloudlog.exception("Error reading modem state")
- return ""
-
-def main():
- set_core_affinity(1)
- set_realtime_priority(1)
-
- procs = {}
- crash_count = 0
- modem_killed = False
- modem_state = "ONLINE"
- androidLog = messaging.sub_sock('androidLog')
-
- while True:
- # check critical android services
- if any(p is None or not p.is_running() for p in procs.values()) or not len(procs):
- cur = {p: None for p in WATCHED_PROCS}
- for p in psutil.process_iter(attrs=['cmdline']):
- cmdline = None if not len(p.info['cmdline']) else p.info['cmdline'][0]
- if cmdline in WATCHED_PROCS:
- cur[cmdline] = p
-
- if len(procs):
- for p in WATCHED_PROCS:
- if cur[p] != procs[p]:
- cloudlog.event("android service pid changed", proc=p, cur=cur[p], prev=procs[p], error=True)
- procs.update(cur)
-
- # log caught NetworkPolicy exceptions
- msgs = messaging.drain_sock(androidLog)
- for m in msgs:
- try:
- if m.androidLog.tag == "NetworkPolicy" and m.androidLog.message.startswith("problem with advise persist threshold"):
- cloudlog.event("network policy exception caught", androidLog=m.androidLog, error=True)
- except UnicodeDecodeError:
- pass
-
- if os.path.exists(MODEM_PATH):
- # check modem state
- state = get_modem_state()
- if state != modem_state and not modem_killed:
- cloudlog.event("modem state changed", state=state)
- modem_state = state
-
- # check modem crashes
- cnt = get_modem_crash_count()
- if cnt is not None:
- if cnt > crash_count:
- cloudlog.event("modem crash", count=cnt)
- crash_count = cnt
-
- # handle excessive modem crashes
- if crash_count > MAX_MODEM_CRASHES and not modem_killed:
- cloudlog.event("killing modem", error=True)
- with open("/sys/kernel/debug/msm_subsys/modem", "w") as f:
- f.write("put")
- modem_killed = True
-
- time.sleep(1)
-
-if __name__ == "__main__":
- main()
diff --git a/selfdrive/hardware/eon/hardware.h b/selfdrive/hardware/eon/hardware.h
deleted file mode 100644
index bcd1aaba74..0000000000
--- a/selfdrive/hardware/eon/hardware.h
+++ /dev/null
@@ -1,73 +0,0 @@
-#pragma once
-
-#include
-#include
-
-#include
-#include
-#include
-
-#include "selfdrive/common/util.h"
-#include "selfdrive/hardware/base.h"
-
-class HardwareEon : public HardwareNone {
-public:
- static constexpr float MAX_VOLUME = 1.0;
- static constexpr float MIN_VOLUME = 0.5;
-
- static bool EON() { return true; }
- static std::string get_os_version() {
- return "NEOS " + util::read_file("/VERSION");
- };
-
- static void reboot() { std::system("reboot"); };
- static void poweroff() { std::system("LD_LIBRARY_PATH= svc power shutdown"); };
- static void set_brightness(int percent) {
- std::ofstream brightness_control("/sys/class/leds/lcd-backlight/brightness");
- if (brightness_control.is_open()) {
- brightness_control << (int)(percent * (255/100.)) << "\n";
- brightness_control.close();
- }
- };
- static void set_display_power(bool on) {
- auto dtoken = android::SurfaceComposerClient::getBuiltInDisplay(android::ISurfaceComposer::eDisplayIdMain);
- android::SurfaceComposerClient::setDisplayPowerMode(dtoken, on ? HWC_POWER_MODE_NORMAL : HWC_POWER_MODE_OFF);
- };
-
- static bool get_ssh_enabled() {
- return std::system("getprop persist.neos.ssh | grep -qF '1'") == 0;
- };
- static void set_ssh_enabled(bool enabled) {
- std::string cmd = util::string_format("setprop persist.neos.ssh %d", enabled ? 1 : 0);
- std::system(cmd.c_str());
- };
-
- // android only
- inline static bool launched_activity = false;
- static void check_activity() {
- int ret = std::system("dumpsys SurfaceFlinger --list | grep -Fq 'com.android.settings'");
- launched_activity = ret == 0;
- }
-
- static void close_activities() {
- if(launched_activity) {
- std::system("pm disable com.android.settings && pm enable com.android.settings");
- }
- }
-
- static void launch_activity(std::string activity, std::string opts = "") {
- if (!launched_activity) {
- std::string cmd = "am start -n " + activity + " " + opts +
- " --ez extra_prefs_show_button_bar true \
- --es extra_prefs_set_next_text ''";
- std::system(cmd.c_str());
- }
- launched_activity = true;
- }
- static void launch_wifi() {
- launch_activity("com.android.settings/.wifi.WifiPickerActivity", "-a android.net.wifi.PICK_WIFI_NETWORK");
- }
- static void launch_tethering() {
- launch_activity("com.android.settings/.TetherSettings");
- }
-};
diff --git a/selfdrive/hardware/eon/hardware.py b/selfdrive/hardware/eon/hardware.py
deleted file mode 100644
index bd2a01cf1e..0000000000
--- a/selfdrive/hardware/eon/hardware.py
+++ /dev/null
@@ -1,425 +0,0 @@
-import binascii
-import itertools
-import os
-import re
-import serial
-import struct
-import subprocess
-from typing import List, Union
-
-from cereal import log
-from selfdrive.hardware.base import HardwareBase, ThermalConfig
-
-try:
- from common.params import Params
-except Exception:
- # openpilot is not built yet
- Params = None
-
-NetworkType = log.DeviceState.NetworkType
-NetworkStrength = log.DeviceState.NetworkStrength
-
-MODEM_PATH = "/dev/smd11"
-
-def service_call(call: List[str]) -> Union[bytes, None]:
- try:
- ret = subprocess.check_output(["service", "call", *call], encoding='utf8').strip()
- if 'Parcel' not in ret:
- return None
- return parse_service_call_bytes(ret)
- except subprocess.CalledProcessError:
- return None
-
-
-def parse_service_call_unpack(r, fmt) -> Union[bytes, None]:
- try:
- return struct.unpack(fmt, r)[0]
- except Exception:
- return None
-
-
-def parse_service_call_string(r: bytes) -> Union[str, None]:
- try:
- r = r[8:] # Cut off length field
- r_str = 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_str[::2], r_str[1::2], fillvalue='\x00'):
- result += b + a
-
- return result.replace('\x00', '')
- except Exception:
- return None
-
-
-def parse_service_call_bytes(ret: str) -> Union[bytes, None]:
- try:
- r = b""
- for hex_part in re.findall(r'[ (]([0-9a-f]{8})', ret):
- r += binascii.unhexlify(hex_part)
- return r
- except Exception:
- return None
-
-
-def getprop(key: str) -> Union[str, None]:
- try:
- return subprocess.check_output(["getprop", key], encoding='utf8').strip()
- except subprocess.CalledProcessError:
- return None
-
-
-class Android(HardwareBase):
- def get_os_version(self):
- with open("/VERSION") as f:
- return f.read().strip()
-
- def get_device_type(self):
- try:
- if int(Params().get("LastPeripheralPandaType")) == log.PandaState.PandaType.uno:
- return "two"
- except Exception:
- pass
- return "eon"
-
- def get_sound_card_online(self):
- return (os.path.isfile('/proc/asound/card0/state') and
- open('/proc/asound/card0/state').read().strip() == 'ONLINE')
-
- def get_imei(self, slot):
- slot = str(slot)
- if slot not in ("0", "1"):
- raise ValueError("SIM slot must be 0 or 1")
-
- return parse_service_call_string(service_call(["iphonesubinfo", "3", "i32", str(slot)]))
-
- def get_serial(self):
- ret = getprop("ro.serialno")
- if len(ret) == 0:
- ret = "cccccccc"
- return ret
-
- def get_subscriber_info(self):
- ret = parse_service_call_string(service_call(["iphonesubinfo", "7"]))
- if ret is None or len(ret) < 8:
- return ""
- return ret
-
- def reboot(self, reason=None):
- # e.g. reason="recovery" to go into recover mode
- if reason is None:
- reason_args = ["null"]
- else:
- reason_args = ["s16", reason]
-
- subprocess.check_output([
- "service", "call", "power", "16", # IPowerManager.reboot
- "i32", "0", # no confirmation,
- *reason_args,
- "i32", "1" # wait
- ])
-
- def uninstall(self):
- with open('/cache/recovery/command', 'w') as f:
- f.write('--wipe_data\n')
- # IPowerManager.reboot(confirm=false, reason="recovery", wait=true)
- self.reboot(reason="recovery")
-
- def get_sim_info(self):
- # Used for athena
- # TODO: build using methods from this class
- sim_state = getprop("gsm.sim.state").split(",")
- network_type = getprop("gsm.network.type").split(',')
- mcc_mnc = getprop("gsm.sim.operator.numeric") or None
-
- sim_id = parse_service_call_string(service_call(['iphonesubinfo', '11']))
- cell_data_state = parse_service_call_unpack(service_call(['phone', '46']), ">q")
- cell_data_connected = (cell_data_state == 2)
-
- return {
- 'sim_id': sim_id,
- 'mcc_mnc': mcc_mnc,
- 'network_type': network_type,
- 'sim_state': sim_state,
- 'data_connected': cell_data_connected
- }
-
- def get_network_info(self):
- msg = log.DeviceState.NetworkInfo.new_message()
- msg.state = getprop("gsm.sim.state") or ""
- msg.technology = getprop("gsm.network.type") or ""
- msg.operator = getprop("gsm.sim.operator.numeric") or ""
-
- try:
- modem = serial.Serial(MODEM_PATH, 115200, timeout=0.1)
- modem.write(b"AT$QCRSRP?\r")
- msg.extra = modem.read_until(b"OK\r\n").decode('utf-8')
-
- rsrp = msg.extra.split("$QCRSRP: ")[1].split("\r")[0].split(",")
- msg.channel = int(rsrp[1])
- except Exception:
- pass
-
- return msg
-
- def get_network_type(self):
- wifi_check = parse_service_call_string(service_call(["connectivity", "2"]))
- if wifi_check is None:
- return NetworkType.none
- elif 'WIFI' in wifi_check:
- return NetworkType.wifi
- else:
- cell_check = parse_service_call_unpack(service_call(['phone', '59']), ">q")
- # from TelephonyManager.java
- cell_networks = {
- 0: NetworkType.none,
- 1: NetworkType.cell2G,
- 2: NetworkType.cell2G,
- 3: NetworkType.cell3G,
- 4: NetworkType.cell2G,
- 5: NetworkType.cell3G,
- 6: NetworkType.cell3G,
- 7: NetworkType.cell3G,
- 8: NetworkType.cell3G,
- 9: NetworkType.cell3G,
- 10: NetworkType.cell3G,
- 11: NetworkType.cell2G,
- 12: NetworkType.cell3G,
- 13: NetworkType.cell4G,
- 14: NetworkType.cell4G,
- 15: NetworkType.cell3G,
- 16: NetworkType.cell2G,
- 17: NetworkType.cell3G,
- 18: NetworkType.cell4G,
- 19: NetworkType.cell4G
- }
- return cell_networks.get(cell_check, NetworkType.none)
-
- def get_network_strength(self, network_type):
- network_strength = NetworkStrength.unknown
-
- # from SignalStrength.java
- def get_lte_level(rsrp, rssnr):
- INT_MAX = 2147483647
- if rsrp == INT_MAX:
- lvl_rsrp = NetworkStrength.unknown
- elif rsrp >= -95:
- lvl_rsrp = NetworkStrength.great
- elif rsrp >= -105:
- lvl_rsrp = NetworkStrength.good
- elif rsrp >= -115:
- lvl_rsrp = NetworkStrength.moderate
- else:
- lvl_rsrp = NetworkStrength.poor
- if rssnr == INT_MAX:
- lvl_rssnr = NetworkStrength.unknown
- elif rssnr >= 45:
- lvl_rssnr = NetworkStrength.great
- elif rssnr >= 10:
- lvl_rssnr = NetworkStrength.good
- elif rssnr >= -30:
- lvl_rssnr = NetworkStrength.moderate
- else:
- lvl_rssnr = NetworkStrength.poor
- return max(lvl_rsrp, lvl_rssnr)
-
- def get_tdscdma_level(tdscmadbm):
- lvl = NetworkStrength.unknown
- if tdscmadbm > -25:
- lvl = NetworkStrength.unknown
- elif tdscmadbm >= -49:
- lvl = NetworkStrength.great
- elif tdscmadbm >= -73:
- lvl = NetworkStrength.good
- elif tdscmadbm >= -97:
- lvl = NetworkStrength.moderate
- elif tdscmadbm >= -110:
- lvl = NetworkStrength.poor
- return lvl
-
- def get_gsm_level(asu):
- if asu <= 2 or asu == 99:
- lvl = NetworkStrength.unknown
- elif asu >= 12:
- lvl = NetworkStrength.great
- elif asu >= 8:
- lvl = NetworkStrength.good
- elif asu >= 5:
- lvl = NetworkStrength.moderate
- else:
- lvl = NetworkStrength.poor
- return lvl
-
- def get_evdo_level(evdodbm, evdosnr):
- lvl_evdodbm = NetworkStrength.unknown
- lvl_evdosnr = NetworkStrength.unknown
- if evdodbm >= -65:
- lvl_evdodbm = NetworkStrength.great
- elif evdodbm >= -75:
- lvl_evdodbm = NetworkStrength.good
- elif evdodbm >= -90:
- lvl_evdodbm = NetworkStrength.moderate
- elif evdodbm >= -105:
- lvl_evdodbm = NetworkStrength.poor
- if evdosnr >= 7:
- lvl_evdosnr = NetworkStrength.great
- elif evdosnr >= 5:
- lvl_evdosnr = NetworkStrength.good
- elif evdosnr >= 3:
- lvl_evdosnr = NetworkStrength.moderate
- elif evdosnr >= 1:
- lvl_evdosnr = NetworkStrength.poor
- return max(lvl_evdodbm, lvl_evdosnr)
-
- def get_cdma_level(cdmadbm, cdmaecio):
- lvl_cdmadbm = NetworkStrength.unknown
- lvl_cdmaecio = NetworkStrength.unknown
- if cdmadbm >= -75:
- lvl_cdmadbm = NetworkStrength.great
- elif cdmadbm >= -85:
- lvl_cdmadbm = NetworkStrength.good
- elif cdmadbm >= -95:
- lvl_cdmadbm = NetworkStrength.moderate
- elif cdmadbm >= -100:
- lvl_cdmadbm = NetworkStrength.poor
- if cdmaecio >= -90:
- lvl_cdmaecio = NetworkStrength.great
- elif cdmaecio >= -110:
- lvl_cdmaecio = NetworkStrength.good
- elif cdmaecio >= -130:
- lvl_cdmaecio = NetworkStrength.moderate
- elif cdmaecio >= -150:
- lvl_cdmaecio = NetworkStrength.poor
- return max(lvl_cdmadbm, lvl_cdmaecio)
-
- if network_type == NetworkType.none:
- return network_strength
- if network_type == NetworkType.wifi:
- out = subprocess.check_output('dumpsys connectivity', shell=True).decode('utf-8')
- network_strength = NetworkStrength.unknown
- for line in out.split('\n'):
- signal_str = "SignalStrength: "
- if signal_str in line:
- lvl_idx_start = line.find(signal_str) + len(signal_str)
- lvl_idx_end = line.find(']', lvl_idx_start)
- lvl = int(line[lvl_idx_start : lvl_idx_end])
- if lvl >= -50:
- network_strength = NetworkStrength.great
- elif lvl >= -60:
- network_strength = NetworkStrength.good
- elif lvl >= -70:
- network_strength = NetworkStrength.moderate
- else:
- network_strength = NetworkStrength.poor
- return network_strength
- else:
- # check cell strength
- out = subprocess.check_output('dumpsys telephony.registry', shell=True).decode('utf-8')
- for line in out.split('\n'):
- if "mSignalStrength" in line:
- arr = line.split(' ')
- ns = 0
- if ("gsm" in arr[14]):
- rsrp = int(arr[9])
- rssnr = int(arr[11])
- ns = get_lte_level(rsrp, rssnr)
- if ns == NetworkStrength.unknown:
- tdscmadbm = int(arr[13])
- ns = get_tdscdma_level(tdscmadbm)
- if ns == NetworkStrength.unknown:
- asu = int(arr[1])
- ns = get_gsm_level(asu)
- else:
- cdmadbm = int(arr[3])
- cdmaecio = int(arr[4])
- evdodbm = int(arr[5])
- evdosnr = int(arr[7])
- lvl_cdma = get_cdma_level(cdmadbm, cdmaecio)
- lvl_edmo = get_evdo_level(evdodbm, evdosnr)
- if lvl_edmo == NetworkStrength.unknown:
- ns = lvl_cdma
- elif lvl_cdma == NetworkStrength.unknown:
- ns = lvl_edmo
- else:
- ns = min(lvl_cdma, lvl_edmo)
- network_strength = max(network_strength, ns)
-
- return network_strength
-
- def get_battery_capacity(self):
- return self.read_param_file("/sys/class/power_supply/battery/capacity", int, 100)
-
- def get_battery_status(self):
- # This does not correspond with actual charging or not.
- # If a USB cable is plugged in, it responds with 'Charging', even when charging is disabled
- return self.read_param_file("/sys/class/power_supply/battery/status", lambda x: x.strip(), '')
-
- def get_battery_current(self):
- return self.read_param_file("/sys/class/power_supply/battery/current_now", int)
-
- def get_battery_voltage(self):
- return self.read_param_file("/sys/class/power_supply/battery/voltage_now", int)
-
- def get_battery_charging(self):
- # This does correspond with actually charging
- return self.read_param_file("/sys/class/power_supply/battery/charge_type", lambda x: x.strip() != "N/A", True)
-
- def set_battery_charging(self, on):
- with open('/sys/class/power_supply/battery/charging_enabled', 'w') as f:
- f.write(f"{1 if on else 0}\n")
-
- def get_usb_present(self):
- return self.read_param_file("/sys/class/power_supply/usb/present", lambda x: bool(int(x)), False)
-
- def get_current_power_draw(self):
- if self.get_battery_status() == 'Discharging':
- # If the battery is discharging, we can use this measurement
- # On C2: this is low by about 10-15%, probably mostly due to UNO draw not being factored in
- return ((self.get_battery_voltage() / 1000000) * (self.get_battery_current() / 1000000))
- else:
- # We don't have a good direct way to measure this if it's not "discharging"
- return None
-
- def shutdown(self):
- os.system('LD_LIBRARY_PATH="" svc power shutdown')
-
- def get_thermal_config(self):
- # the thermal sensors on the 820 don't have meaningful names
- return ThermalConfig(cpu=((5, 7, 10, 12), 10), gpu=((16,), 10), mem=(2, 10),
- bat=("battery", 1000), ambient=("pa_therm0", 1), pmic=(("pm8994_tz",), 1000))
-
- def set_screen_brightness(self, percentage):
- with open("/sys/class/leds/lcd-backlight/brightness", "w") as f:
- f.write(str(int(percentage * 2.55)))
-
- def get_screen_brightness(self):
- try:
- with open("/sys/class/leds/lcd-backlight/brightness") as f:
- return int(float(f.read()) / 2.55)
- except Exception:
- return 0
-
- def set_power_save(self, powersave_enabled):
- pass
-
- def get_gpu_usage_percent(self):
- try:
- used, total = open('/sys/devices/soc/b00000.qcom,kgsl-3d0/kgsl/kgsl-3d0/gpubusy').read().strip().split()
- perc = 100.0 * int(used) / int(total)
- return min(max(perc, 0), 100)
- except Exception:
- return 0
-
- def get_modem_temperatures(self):
- # Not sure if we can get this on the LeEco
- return []
-
- def get_nvme_temperatures(self):
- return []
-
- def initialize_hardware(self):
- pass
-
- def get_networks(self):
- return None
diff --git a/selfdrive/hardware/eon/neos.json b/selfdrive/hardware/eon/neos.json
deleted file mode 100644
index 4010f7126a..0000000000
--- a/selfdrive/hardware/eon/neos.json
+++ /dev/null
@@ -1,7 +0,0 @@
-{
- "ota_url": "https://commadist.azureedge.net/neosupdate/ota-signed-50da8800caa5cbc224acbaa21f3a83d21802a31d89cccfc62a898903a8eb19e7.zip",
- "ota_hash": "50da8800caa5cbc224acbaa21f3a83d21802a31d89cccfc62a898903a8eb19e7",
- "recovery_url": "https://commadist.azureedge.net/neosupdate/recovery-fe76739438d28ea6111853a737b616afdf340ba75c01c0ee99e6a28c19ecc29f.img",
- "recovery_len": 15222060,
- "recovery_hash": "fe76739438d28ea6111853a737b616afdf340ba75c01c0ee99e6a28c19ecc29f"
-}
diff --git a/selfdrive/hardware/eon/neos.py b/selfdrive/hardware/eon/neos.py
deleted file mode 100755
index 6f290fbcd1..0000000000
--- a/selfdrive/hardware/eon/neos.py
+++ /dev/null
@@ -1,130 +0,0 @@
-#!/usr/bin/env python3
-import argparse
-import hashlib
-import json
-import logging
-import os
-import requests
-
-NEOSUPDATE_DIR = "/data/neoupdate"
-
-RECOVERY_DEV = "/dev/block/bootdevice/by-name/recovery"
-RECOVERY_COMMAND = "/cache/recovery/command"
-
-
-def get_fn(url: str):
- return os.path.join(NEOSUPDATE_DIR, os.path.basename(url))
-
-
-def download_file(url: str, fn: str, sha256: str, display_name: str, cloudlog=logging) -> None:
- # check if already downloaded
- if check_hash(fn, sha256):
- cloudlog.info(f"{display_name} already cached")
- return
-
- try:
- with open(fn, "ab+") as f:
- headers = {"Range": f"bytes={f.tell()}-"}
- r = requests.get(url, stream=True, allow_redirects=True, headers=headers)
- r.raise_for_status()
-
- total = int(r.headers['Content-Length'])
- if 'Content-Range' in r.headers:
- total = int(r.headers['Content-Range'].split('/')[-1])
-
- for chunk in r.iter_content(chunk_size=1024 * 1024):
- f.write(chunk)
- print(f"Downloading {display_name}: {f.tell() / total * 100}", flush=True)
- except Exception:
- cloudlog.error("download error")
- if os.path.isfile(fn):
- os.unlink(fn)
- raise
-
- if not check_hash(fn, sha256):
- if os.path.isfile(fn):
- os.unlink(fn)
- raise Exception("downloaded update failed hash check")
-
-
-def check_hash(fn: str, sha256: str, length: int = -1) -> bool:
- if not os.path.exists(fn):
- return False
-
- h = hashlib.sha256()
- with open(fn, "rb") as f:
- while f.tell() != length:
- r = min(max(0, length - f.tell()), 1024 * 1024) if length > 0 else 1024 * 1024
- dat = f.read(r)
- if not dat:
- break
- h.update(dat)
- return h.hexdigest().lower() == sha256.lower()
-
-
-def flash_update(update_fn: str, out_path: str) -> None:
- with open(update_fn, "rb") as update, open(out_path, "w+b") as out:
- while True:
- dat = update.read(8192)
- if len(dat) == 0:
- break
- out.write(dat)
-
-
-def download_neos_update(manifest_path: str, cloudlog=logging) -> None:
- with open(manifest_path) as f:
- m = json.load(f)
-
- os.makedirs(NEOSUPDATE_DIR, exist_ok=True)
-
- # handle recovery updates
- if not check_hash(RECOVERY_DEV, m['recovery_hash'], m['recovery_len']):
- cloudlog.info("recovery needs update")
- recovery_fn = os.path.join(NEOSUPDATE_DIR, os.path.basename(m['recovery_url']))
- download_file(m['recovery_url'], recovery_fn, m['recovery_hash'], "recovery", cloudlog)
-
- flash_update(recovery_fn, RECOVERY_DEV)
- assert check_hash(RECOVERY_DEV, m['recovery_hash'], m['recovery_len']), "recovery flash corrupted"
- cloudlog.info("recovery successfully flashed")
-
- # download OTA update
- download_file(m['ota_url'], get_fn(m['ota_url']), m['ota_hash'], "system", cloudlog)
-
-
-def verify_update_ready(manifest_path: str) -> bool:
- with open(manifest_path) as f:
- m = json.load(f)
-
- ota_downloaded = check_hash(get_fn(m['ota_url']), m['ota_hash'])
- recovery_flashed = check_hash(RECOVERY_DEV, m['recovery_hash'], m['recovery_len'])
- return ota_downloaded and recovery_flashed
-
-
-def perform_ota_update(manifest_path: str) -> None:
- with open(manifest_path) as f:
- m = json.load(f)
-
- # reboot into recovery
- ota_fn = get_fn(m['ota_url'])
- with open(RECOVERY_COMMAND, "w") as rf:
- rf.write(f"--update_package={ota_fn}\n")
- os.system("service call power 16 i32 0 s16 recovery i32 1")
-
-
-if __name__ == "__main__":
- parser = argparse.ArgumentParser(description="NEOS update utility",
- formatter_class=argparse.ArgumentDefaultsHelpFormatter)
- parser.add_argument("--swap", action="store_true", help="Peform update after downloading")
- parser.add_argument("--swap-if-ready", action="store_true", help="Perform update if already downloaded")
- parser.add_argument("manifest", help="Manifest json")
- args = parser.parse_args()
-
- logging.basicConfig(level=logging.INFO)
-
- if args.swap_if_ready:
- if verify_update_ready(args.manifest):
- perform_ota_update(args.manifest)
- else:
- download_neos_update(args.manifest, logging)
- if args.swap:
- perform_ota_update(args.manifest)
diff --git a/selfdrive/hardware/eon/shutdownd.py b/selfdrive/hardware/eon/shutdownd.py
deleted file mode 100755
index 15112e7d5e..0000000000
--- a/selfdrive/hardware/eon/shutdownd.py
+++ /dev/null
@@ -1,34 +0,0 @@
-#!/usr/bin/env python3
-import os
-import time
-import datetime
-
-from common.params import Params
-from selfdrive.hardware.eon.hardware import getprop
-from selfdrive.swaglog import cloudlog
-
-def main():
- prev = b""
- params = Params()
- while True:
- with open("/dev/__properties__", 'rb') as f:
- cur = f.read()
-
- if cur != prev:
- prev = cur
-
- # 0 for shutdown, 1 for reboot
- prop = getprop("sys.shutdown.requested")
- if prop is not None and len(prop) > 0:
- os.system("pkill -9 loggerd")
- params.put("LastSystemShutdown", f"'{prop}' {datetime.datetime.now()}")
- os.sync()
-
- time.sleep(120)
- cloudlog.error('shutdown false positive')
- break
-
- time.sleep(0.1)
-
-if __name__ == "__main__":
- main()
diff --git a/selfdrive/hardware/eon/test_neos_updater.py b/selfdrive/hardware/eon/test_neos_updater.py
deleted file mode 100755
index e258f943d2..0000000000
--- a/selfdrive/hardware/eon/test_neos_updater.py
+++ /dev/null
@@ -1,145 +0,0 @@
-#!/usr/bin/env python3
-import hashlib
-import http.server
-import json
-import os
-import unittest
-import random
-import requests
-import shutil
-import socketserver
-import tempfile
-import multiprocessing
-from pathlib import Path
-
-from selfdrive.hardware.eon.neos import RECOVERY_DEV, NEOSUPDATE_DIR, get_fn, download_file, \
- verify_update_ready, download_neos_update
-
-EON_DIR = os.path.join(os.path.dirname(os.path.abspath(__file__)))
-MANIFEST = os.path.join(EON_DIR, "neos.json")
-
-PORT = 8000
-
-def server_thread(port):
- socketserver.TCPServer.allow_reuse_address = True
- httpd = socketserver.TCPServer(("", port), http.server.SimpleHTTPRequestHandler)
- httpd.serve_forever()
-
-
-class TestNeosUpdater(unittest.TestCase):
-
- @classmethod
- def setUpClass(cls):
- # generate a fake manifest
- cls.manifest = {}
- for i in ('ota', 'recovery'):
- with tempfile.NamedTemporaryFile(delete=False, dir=os.getcwd()) as f:
- dat = os.urandom(random.randint(1000, 100000))
- f.write(dat)
- cls.manifest[f"{i}_url"] = f"http://localhost:{PORT}/" + os.path.relpath(f.name)
- cls.manifest[F"{i}_hash"] = hashlib.sha256(dat).hexdigest()
- if i == "recovery":
- cls.manifest["recovery_len"] = len(dat)
-
- with tempfile.NamedTemporaryFile(delete=False, mode='w') as f:
- f.write(json.dumps(cls.manifest))
- cls.fake_manifest = f.name
-
- @classmethod
- def tearDownClass(cls):
- os.unlink(cls.fake_manifest)
- os.unlink(os.path.basename(cls.manifest['ota_url']))
- os.unlink(os.path.basename(cls.manifest['recovery_url']))
-
- def setUp(self):
- # server for update files
- self.server = multiprocessing.Process(target=server_thread, args=(PORT, ))
- self.server.start()
-
- # clean up
- if os.path.exists(NEOSUPDATE_DIR):
- shutil.rmtree(NEOSUPDATE_DIR)
-
- def tearDown(self):
- self.server.kill()
- self.server.join(1)
-
- def _corrupt_recovery(self):
- with open(RECOVERY_DEV, "wb") as f:
- f.write(b'\x00'*100)
-
- def test_manifest(self):
- with open(MANIFEST) as f:
- m = json.load(f)
-
- assert m['ota_hash'] in m['ota_url']
- assert m['recovery_hash'] in m['recovery_url']
- assert m['recovery_len'] > 0
-
- for url in (m['ota_url'], m['recovery_url']):
- r = requests.head(m['recovery_url'])
- r.raise_for_status()
- self.assertEqual(r.headers['Content-Type'], "application/octet-stream")
- if url == m['recovery_url']:
- self.assertEqual(int(r.headers['Content-Length']), m['recovery_len'])
-
- def test_download_hash_check(self):
- os.makedirs(NEOSUPDATE_DIR, exist_ok=True)
- Path(get_fn(self.manifest['ota_url'])).touch()
- with self.assertRaisesRegex(Exception, "failed hash check"):
- download_file(self.manifest['ota_url'], get_fn(self.manifest['ota_url']),
- self.manifest['ota_hash']+'a', "system")
-
- # should've unlinked after the failed hash check, should succeed now
- download_file(self.manifest['ota_url'], get_fn(self.manifest['ota_url']),
- self.manifest['ota_hash'], "system")
-
- # TODO: needs an http server that supports Content-Range
- #def test_download_resume(self):
- # os.makedirs(NEOSUPDATE_DIR, exist_ok=True)
- # with open(os.path.basename(self.manifest['ota_url']), "rb") as src, \
- # open(get_fn(self.manifest['ota_url']), "wb") as dest:
- # l = dest.write(src.read(8192))
- # assert l == 8192
- # download_file(self.manifest['ota_url'], get_fn(self.manifest['ota_url']),
- # self.manifest['ota_hash'], "system")
-
- def test_download_no_internet(self):
- self.server.kill()
- os.makedirs(NEOSUPDATE_DIR, exist_ok=True)
- # fail, no internet
- with self.assertRaises(requests.exceptions.ConnectionError):
- download_file(self.manifest['ota_url'], get_fn(self.manifest['ota_url']),
- self.manifest['ota_hash'], "system")
-
- # already cached, ensure we don't hit the server
- shutil.copyfile(os.path.basename(self.manifest['ota_url']), get_fn(self.manifest['ota_url']))
- download_file(self.manifest['ota_url'], get_fn(self.manifest['ota_url']),
- self.manifest['ota_hash'], "system")
-
-
- def test_download_update(self):
- download_neos_update(self.fake_manifest)
- self.assertTrue(verify_update_ready(self.fake_manifest))
-
- def test_verify_update(self):
- # good state
- download_neos_update(self.fake_manifest)
- self.assertTrue(verify_update_ready(self.fake_manifest))
-
- # corrupt recovery
- self._corrupt_recovery()
- self.assertFalse(verify_update_ready(self.fake_manifest))
-
- # back to good state
- download_neos_update(self.fake_manifest)
- self.assertTrue(verify_update_ready(self.fake_manifest))
-
- # corrupt ota
- self._corrupt_recovery()
- with open(os.path.join(NEOSUPDATE_DIR, os.path.basename(self.manifest['ota_url'])), "ab") as f:
- f.write(b'\x00')
- self.assertFalse(verify_update_ready(self.fake_manifest))
-
-if __name__ == "__main__":
- unittest.main()
diff --git a/selfdrive/hardware/eon/update_neos.sh b/selfdrive/hardware/eon/update_neos.sh
deleted file mode 100755
index ccc6ecce44..0000000000
--- a/selfdrive/hardware/eon/update_neos.sh
+++ /dev/null
@@ -1,4 +0,0 @@
-#!/usr/bin/bash
-
-ROOT=$PWD/../../..
-$ROOT/installer/updater/updater "file://$ROOT/installer/updater/update.json"
diff --git a/selfdrive/hardware/eon/updater b/selfdrive/hardware/eon/updater
deleted file mode 100755
index eaf34e957c..0000000000
Binary files a/selfdrive/hardware/eon/updater and /dev/null differ
diff --git a/selfdrive/hardware/hw.h b/selfdrive/hardware/hw.h
index f18ede01ec..b221b63db2 100644
--- a/selfdrive/hardware/hw.h
+++ b/selfdrive/hardware/hw.h
@@ -3,10 +3,7 @@
#include "selfdrive/hardware/base.h"
#include "selfdrive/common/util.h"
-#ifdef QCOM
-#include "selfdrive/hardware/eon/hardware.h"
-#define Hardware HardwareEon
-#elif QCOM2
+#if QCOM2
#include "selfdrive/hardware/tici/hardware.h"
#define Hardware HardwareTici
#else
diff --git a/selfdrive/hardware/tici/pins.py b/selfdrive/hardware/tici/pins.py
index 7139f5e955..5000e6ff2a 100644
--- a/selfdrive/hardware/tici/pins.py
+++ b/selfdrive/hardware/tici/pins.py
@@ -1,8 +1,22 @@
# TODO: these are also defined in a header
# GPIO pin definitions
-GPIO_HUB_RST_N = 30
-GPIO_UBLOX_RST_N = 32
-GPIO_UBLOX_SAFEBOOT_N = 33
-GPIO_UBLOX_PWR_EN = 34
-GPIO_STM_RST_N = 124
-GPIO_STM_BOOT0 = 134
+class GPIO:
+ # both GPIO_STM_RST_N and GPIO_LTE_RST_N are misnamed, they are high to reset
+ HUB_RST_N = 30
+ UBLOX_RST_N = 32
+ UBLOX_SAFEBOOT_N = 33
+ UBLOX_PWR_EN = 34
+ STM_RST_N = 124
+ STM_BOOT0 = 134
+
+ LTE_RST_N = 50
+ LTE_PWRKEY = 116
+ LTE_BOOT = 52
+
+ # GPIO_CAM0_DVDD_EN = /sys/kernel/debug/regulator/camera_rear_ldo
+ CAM0_AVDD_EN = 8
+ CAM0_RSTN = 9
+ CAM1_RSTN = 7
+ CAM2_RSTN = 12
+
+
diff --git a/selfdrive/hardware/tici/power_draw_test.py b/selfdrive/hardware/tici/power_draw_test.py
new file mode 100755
index 0000000000..1af51fc018
--- /dev/null
+++ b/selfdrive/hardware/tici/power_draw_test.py
@@ -0,0 +1,112 @@
+#!/usr/bin/env python3
+import os
+import time
+import numpy as np
+from selfdrive.hardware.tici.hardware import Tici
+from selfdrive.hardware.tici.pins import GPIO
+from common.gpio import gpio_init, gpio_set
+
+def read_power():
+ with open("/sys/bus/i2c/devices/0-0040/hwmon/hwmon1/in1_input") as f:
+ voltage_total = int(f.read()) / 1000.
+
+ with open("/sys/bus/i2c/devices/0-0040/hwmon/hwmon1/curr1_input") as f:
+ current_total = int(f.read())
+
+ with open("/sys/class/power_supply/bms/voltage_now") as f:
+ voltage = int(f.read()) / 1e6 # volts
+
+ with open("/sys/class/power_supply/bms/current_now") as f:
+ current = int(f.read()) / 1e3 # ma
+
+ power_som = voltage*current
+ power_total = voltage_total*current_total
+
+ return power_total, power_som
+
+def read_power_avg():
+ pwrs = []
+ for _ in range(100):
+ pwrs.append(read_power())
+ time.sleep(0.01)
+ power_total, power_som = np.mean([x[0] for x in pwrs]), np.mean([x[1] for x in pwrs])
+ return "total %7.2f mW SOM %7.2f mW" % (power_total, power_som)
+
+
+def gpio_export(pin):
+ try:
+ with open("/sys/class/gpio/export", 'w') as f:
+ f.write(str(pin))
+ except Exception:
+ print(f"Failed to export gpio {pin}")
+
+if __name__ == "__main__":
+ gpio_export(GPIO.CAM0_AVDD_EN)
+ gpio_export(GPIO.CAM0_RSTN)
+ gpio_export(GPIO.CAM1_RSTN)
+ gpio_export(GPIO.CAM2_RSTN)
+ print("hello")
+ os.system('kill $(pgrep -f "manager.py")')
+ os.system('kill $(pgrep -f "python -m selfdrive.athena.manage_athenad")')
+ os.system('kill $(pgrep -f "selfdrive.athena.athenad")')
+ # stopping weston turns off lcd3v3
+ os.system("sudo service weston stop")
+ os.system("sudo service ModemManager stop")
+ print("services stopped")
+
+ t = Tici()
+ t.initialize_hardware()
+ t.set_power_save(True)
+ t.set_screen_brightness(0)
+ gpio_init(GPIO.STM_RST_N, True)
+ gpio_init(GPIO.HUB_RST_N, True)
+ gpio_init(GPIO.UBLOX_PWR_EN, True)
+ gpio_init(GPIO.LTE_RST_N, True)
+ gpio_init(GPIO.LTE_PWRKEY, True)
+ gpio_init(GPIO.CAM0_AVDD_EN, True)
+ gpio_init(GPIO.CAM0_RSTN, True)
+ gpio_init(GPIO.CAM1_RSTN, True)
+ gpio_init(GPIO.CAM2_RSTN, True)
+
+
+ os.system("sudo su -c 'echo 0 > /sys/kernel/debug/regulator/camera_rear_ldo/enable'") # cam 1v2 off
+ gpio_set(GPIO.CAM0_AVDD_EN, False) # cam 2v8 off
+ gpio_set(GPIO.LTE_RST_N, True) # quectel off
+ gpio_set(GPIO.UBLOX_PWR_EN, False) # gps off
+ gpio_set(GPIO.STM_RST_N, True) # panda off
+ gpio_set(GPIO.HUB_RST_N, False) # hub off
+ # cameras in reset
+ gpio_set(GPIO.CAM0_RSTN, False)
+ gpio_set(GPIO.CAM1_RSTN, False)
+ gpio_set(GPIO.CAM2_RSTN, False)
+ time.sleep(8)
+
+ print("baseline: ", read_power_avg())
+ gpio_set(GPIO.CAM0_AVDD_EN, True)
+ time.sleep(2)
+ print("cam avdd: ", read_power_avg())
+ os.system("sudo su -c 'echo 1 > /sys/kernel/debug/regulator/camera_rear_ldo/enable'")
+ time.sleep(2)
+ print("cam dvdd: ", read_power_avg())
+ gpio_set(GPIO.CAM0_RSTN, True)
+ gpio_set(GPIO.CAM1_RSTN, True)
+ gpio_set(GPIO.CAM2_RSTN, True)
+ time.sleep(2)
+ print("cams up: ", read_power_avg())
+ gpio_set(GPIO.HUB_RST_N, True)
+ time.sleep(2)
+ print("usb hub: ", read_power_avg())
+ gpio_set(GPIO.STM_RST_N, False)
+ time.sleep(5)
+ print("panda: ", read_power_avg())
+ gpio_set(GPIO.UBLOX_PWR_EN, True)
+ time.sleep(5)
+ print("gps: ", read_power_avg())
+ gpio_set(GPIO.LTE_RST_N, False)
+ time.sleep(1)
+ gpio_set(GPIO.LTE_PWRKEY, True)
+ time.sleep(1)
+ gpio_set(GPIO.LTE_PWRKEY, False)
+ time.sleep(5)
+ print("quectel: ", read_power_avg())
+
diff --git a/selfdrive/hardware/tici/precise_power_measure.py b/selfdrive/hardware/tici/precise_power_measure.py
new file mode 100755
index 0000000000..f1fb254b26
--- /dev/null
+++ b/selfdrive/hardware/tici/precise_power_measure.py
@@ -0,0 +1,17 @@
+#!/usr/bin/env python3
+import numpy as np
+from common.realtime import Ratekeeper
+
+if __name__ == '__main__':
+ RATE = 123
+ print("measuring for 5 seconds at %dhz 3 times" % RATE)
+ rk = Ratekeeper(RATE, print_delay_threshold=None)
+
+ for _ in range(3):
+ pwrs = []
+ for _ in range(RATE*5):
+ with open("/sys/bus/i2c/devices/0-0040/hwmon/hwmon1/power1_input") as f:
+ pwrs.append(int(f.read()) / 1000.)
+ rk.keep_time()
+ print("mean %.2f std %.2f" % (np.mean(pwrs), np.std(pwrs)))
+
diff --git a/selfdrive/locationd/models/constants.py b/selfdrive/locationd/models/constants.py
index f22f503ee0..6d328ce6f5 100644
--- a/selfdrive/locationd/models/constants.py
+++ b/selfdrive/locationd/models/constants.py
@@ -27,9 +27,10 @@ class ObservationKind:
PSEUDORANGE_RATE_GLONASS = 21
PSEUDORANGE = 22
PSEUDORANGE_RATE = 23
- ECEF_VEL = 31
+ ECEF_VEL = 35
ECEF_ORIENTATION_FROM_GPS = 32
NO_ACCEL = 33
+ ORB_FEATURES_WIDE = 34
ROAD_FRAME_XY_SPEED = 24 # (x, y) [m/s]
ROAD_FRAME_YAW_RATE = 25 # [rad/s]
@@ -63,6 +64,8 @@ class ObservationKind:
'imu frame eulers',
'GLONASS pseudorange',
'GLONASS pseudorange rate',
+ 'pseudorange',
+ 'pseudorange rate',
'Road Frame x,y speed',
'Road Frame yaw rate',
@@ -72,6 +75,10 @@ class ObservationKind:
'Steer Ratio',
'Road Frame x speed',
'Road Roll',
+ 'ECEF orientation from GPS',
+ 'NO accel',
+ 'ORB features wide camera',
+ 'ECEF_VEL',
]
@classmethod
diff --git a/selfdrive/locationd/models/loc_kf.py b/selfdrive/locationd/models/loc_kf.py
index b6293d60db..8391426dd1 100755
--- a/selfdrive/locationd/models/loc_kf.py
+++ b/selfdrive/locationd/models/loc_kf.py
@@ -50,6 +50,8 @@ class States():
CLOCK_ACCELERATION = slice(28, 29) # clock acceleration in light-meters/s**2,
ACCELEROMETER_SCALE_UNUSED = slice(29, 30) # scale of mems accelerometer
ACCELEROMETER_BIAS = slice(30, 33) # bias of mems accelerometer
+ # TODO the offset is likely a translation of the sensor, not a rotation of the camera
+ WIDE_CAM_OFFSET = slice(33, 36) # wide camera offset angles in radians (tici only)
# We curently do not use ACCELEROMETER_SCALE to avoid instability due to too many free variables (ACCELEROMETER_SCALE, ACCELEROMETER_BIAS, IMU_OFFSET).
# From experiments we see that ACCELEROMETER_BIAS is more correct than ACCELEROMETER_SCALE
@@ -70,6 +72,7 @@ class States():
CLOCK_ACCELERATION_ERR = slice(27, 28)
ACCELEROMETER_SCALE_ERR_UNUSED = slice(28, 29)
ACCELEROMETER_BIAS_ERR = slice(29, 32)
+ WIDE_CAM_OFFSET_ERR = slice(32, 35)
class LocKalman():
@@ -87,6 +90,7 @@ class LocKalman():
0, 0,
0,
1,
+ 0, 0, 0,
0, 0, 0], dtype=np.float64)
# state covariance
@@ -99,11 +103,12 @@ class LocKalman():
0.02**2,
2**2, 2**2, 2**2,
0.01**2,
- (0.01)**2, (0.01)**2, (0.01)**2,
+ 0.01**2, 0.01**2, 0.01**2,
10**2, 1**2,
0.2**2,
0.05**2,
- 0.05**2, 0.05**2, 0.05**2])
+ 0.05**2, 0.05**2, 0.05**2,
+ 0.01**2, 0.01**2, 0.01**2])
# process noise
Q = np.diag([0.03**2, 0.03**2, 0.03**2,
@@ -119,10 +124,11 @@ class LocKalman():
(.1)**2, (.01)**2,
0.005**2,
(0.02 / 100)**2,
- (0.005 / 100)**2, (0.005 / 100)**2, (0.005 / 100)**2])
+ (0.005 / 100)**2, (0.005 / 100)**2, (0.005 / 100)**2,
+ (0.05 / 60)**2, (0.05 / 60)**2, (0.05 / 60)**2])
# measurements that need to pass mahalanobis distance outlier rejector
- maha_test_kinds = [ObservationKind.ORB_FEATURES] # , ObservationKind.PSEUDORANGE, ObservationKind.PSEUDORANGE_RATE]
+ maha_test_kinds = [ObservationKind.ORB_FEATURES, ObservationKind.ORB_FEATURES_WIDE] # , ObservationKind.PSEUDORANGE, ObservationKind.PSEUDORANGE_RATE]
dim_augment = 7
dim_augment_err = 6
@@ -154,12 +160,14 @@ class LocKalman():
roll_bias, pitch_bias, yaw_bias = state[States.GYRO_BIAS, :]
acceleration = state[States.ACCELERATION, :]
imu_angles = state[States.IMU_OFFSET, :]
- imu_angles[0, 0] = 0
- imu_angles[2, 0] = 0
+ imu_angles[0, 0] = 0 # not observable enough
+ imu_angles[2, 0] = 0 # not observable enough
glonass_bias = state[States.GLONASS_BIAS, :]
glonass_freq_slope = state[States.GLONASS_FREQ_SLOPE, :]
ca = state[States.CLOCK_ACCELERATION, :]
accel_bias = state[States.ACCELEROMETER_BIAS, :]
+ wide_cam_angles = state[States.WIDE_CAM_OFFSET, :]
+ wide_cam_angles[0, 0] = 0 # not observable enough
dt = sp.Symbol('dt')
@@ -308,22 +316,29 @@ class LocKalman():
[h_phone_rot_sym, ObservationKind.CAMERA_ODO_ROTATION, None],
[h_acc_stationary_sym, ObservationKind.NO_ACCEL, None]]
+ wide_cam_rot = euler_rotate(*wide_cam_angles)
# MSCKF configuration
if N > 0:
# experimentally found this is correct value for imx298 with 910 focal length
# this is a variable so it can change with focus, but we disregard that for now
+ # TODO: this isn't correct for tici
focal_scale = 1.01
# Add observation functions for orb feature tracks
track_epos_sym = sp.MatrixSymbol('track_epos_sym', 3, 1)
track_x, track_y, track_z = track_epos_sym
h_track_sym = sp.Matrix(np.zeros(((1 + N) * 2, 1)))
+ h_track_wide_cam_sym = sp.Matrix(np.zeros(((1 + N) * 2, 1)))
+
track_pos_sym = sp.Matrix([track_x - x, track_y - y, track_z - z])
track_pos_rot_sym = quat_rot.T * track_pos_sym
+ track_pos_rot_wide_cam_sym = wide_cam_rot * track_pos_rot_sym
h_track_sym[-2:, :] = sp.Matrix([focal_scale * (track_pos_rot_sym[1] / track_pos_rot_sym[0]),
- focal_scale * (track_pos_rot_sym[2] / track_pos_rot_sym[0])])
+ focal_scale * (track_pos_rot_sym[2] / track_pos_rot_sym[0])])
+ h_track_wide_cam_sym[-2:, :] = sp.Matrix([focal_scale * (track_pos_rot_wide_cam_sym[1] / track_pos_rot_wide_cam_sym[0]),
+ focal_scale * (track_pos_rot_wide_cam_sym[2] / track_pos_rot_wide_cam_sym[0])])
h_msckf_test_sym = sp.Matrix(np.zeros(((1 + N) * 3, 1)))
- h_msckf_test_sym[-3:, :] = sp.Matrix([track_x - x, track_y - y, track_z - z])
+ h_msckf_test_sym[-3:, :] = track_pos_sym
for n in range(N):
idx = dim_main + n * dim_augment
@@ -333,19 +348,23 @@ class LocKalman():
quat_rot = quat_rotate(*q)
track_pos_sym = sp.Matrix([track_x - x, track_y - y, track_z - z])
track_pos_rot_sym = quat_rot.T * track_pos_sym
+ track_pos_rot_wide_cam_sym = wide_cam_rot * track_pos_rot_sym
h_track_sym[n * 2:n * 2 + 2, :] = sp.Matrix([focal_scale * (track_pos_rot_sym[1] / track_pos_rot_sym[0]),
focal_scale * (track_pos_rot_sym[2] / track_pos_rot_sym[0])])
- h_msckf_test_sym[n * 3:n * 3 + 3, :] = sp.Matrix([track_x - x, track_y - y, track_z - z])
+ h_track_wide_cam_sym[n * 2: n * 2 + 2, :] = sp.Matrix([focal_scale * (track_pos_rot_wide_cam_sym[1] / track_pos_rot_wide_cam_sym[0]),
+ focal_scale * (track_pos_rot_wide_cam_sym[2] / track_pos_rot_wide_cam_sym[0])])
+ h_msckf_test_sym[n * 3:n * 3 + 3, :] = track_pos_sym
obs_eqs.append([h_msckf_test_sym, ObservationKind.MSCKF_TEST, track_epos_sym])
obs_eqs.append([h_track_sym, ObservationKind.ORB_FEATURES, track_epos_sym])
+ obs_eqs.append([h_track_wide_cam_sym, ObservationKind.ORB_FEATURES_WIDE, track_epos_sym])
obs_eqs.append([h_track_sym, ObservationKind.FEATURE_TRACK_TEST, track_epos_sym])
- msckf_params = [dim_main, dim_augment, dim_main_err, dim_augment_err, N, [ObservationKind.MSCKF_TEST, ObservationKind.ORB_FEATURES]]
+ msckf_params = [dim_main, dim_augment, dim_main_err, dim_augment_err, N, [ObservationKind.MSCKF_TEST, ObservationKind.ORB_FEATURES, ObservationKind.ORB_FEATURES_WIDE]]
else:
msckf_params = None
gen_code(generated_dir, name, f_sym, dt, state_sym, obs_eqs, dim_state, dim_state_err, eskf_params, msckf_params, maha_test_kinds)
- def __init__(self, generated_dir, N=4, max_tracks=3000):
+ def __init__(self, generated_dir, N=4):
name = f"{self.name}_{N}"
self.obs_noise = {ObservationKind.ODOMETRIC_SPEED: np.atleast_2d(0.2**2),
@@ -367,7 +386,6 @@ class LocKalman():
if self.N > 0:
x_initial, P_initial, Q = self.pad_augmented(self.x_initial, self.P_initial, self.Q) # lgtm[py/mismatched-multiple-assignment] pylint: disable=unbalanced-tuple-unpacking
self.computer = LstSqComputer(generated_dir, N)
- self.max_tracks = max_tracks
self.quaternion_idxs = [3, ] + [(self.dim_main + i * self.dim_augment + 3)for i in range(self.N)]
@@ -427,7 +445,7 @@ class LocKalman():
r = self.predict_and_update_pseudorange(data, t, kind)
elif kind == ObservationKind.PSEUDORANGE_RATE_GPS or kind == ObservationKind.PSEUDORANGE_RATE_GLONASS:
r = self.predict_and_update_pseudorange_rate(data, t, kind)
- elif kind == ObservationKind.ORB_FEATURES:
+ elif kind == ObservationKind.ORB_FEATURES or kind == ObservationKind.ORB_FEATURES_WIDE:
r = self.predict_and_update_orb_features(data, t, kind)
elif kind == ObservationKind.MSCKF_TEST:
r = self.predict_and_update_msckf_test(data, t, kind)
@@ -492,8 +510,12 @@ class LocKalman():
ecef_pos[:] = np.nan
poses = self.x[self.dim_main:].reshape((-1, 7))
times = tracks.reshape((len(tracks), self.N + 1, 4))[:, :, 0]
- good_counter = 0
- if times.any() and np.allclose(times[0, :-1], self.filter.get_augment_times(), rtol=1e-6):
+ if kind==ObservationKind.ORB_FEATURES:
+ pt_std = 0.005
+ else:
+ pt_std = 0.02
+ if times.any():
+ assert np.allclose(times[0, :-1], self.filter.get_augment_times(), atol=1e-7, rtol=0.0)
for i, track in enumerate(tracks):
img_positions = track.reshape((self.N + 1, 4))[:, 2:]
@@ -502,20 +524,21 @@ class LocKalman():
ecef_pos[i] = self.computer.compute_pos(poses, img_positions[:-1])
z[i] = img_positions.flatten()
- R[i, :, :] = np.diag([0.005**2] * (k))
- if np.isfinite(ecef_pos[i][0]):
- good_counter += 1
- if good_counter > self.max_tracks:
- break
+ R[i, :, :] = np.diag([pt_std**2] * (k))
+
good_idxs = np.all(np.isfinite(ecef_pos), axis=1)
+
+ # This code relies on wide and narrow orb features being captured at the same time,
+ # and wide features to be processed first.
+ ret = self.filter.predict_and_update_batch(t, kind, z[good_idxs], R[good_idxs], ecef_pos[good_idxs],
+ augment=kind==ObservationKind.ORB_FEATURES)
+ if ret is None:
+ return
+
# have to do some weird stuff here to keep
# to have the observations input from mesh3d
# consistent with the outputs of the filter
# Probably should be replaced, not sure how.
- ret = self.filter.predict_and_update_batch(t, kind, z[good_idxs], R[good_idxs], ecef_pos[good_idxs], augment=True)
- if ret is None:
- return
-
y_full = np.zeros((z.shape[0], z.shape[1] - 3))
if sum(good_idxs) > 0:
y_full[good_idxs] = np.array(ret[6])
diff --git a/selfdrive/locationd/paramsd.py b/selfdrive/locationd/paramsd.py
index 55479f255c..0e83728e5c 100755
--- a/selfdrive/locationd/paramsd.py
+++ b/selfdrive/locationd/paramsd.py
@@ -1,14 +1,12 @@
#!/usr/bin/env python3
-import gc
import math
-
import json
import numpy as np
import cereal.messaging as messaging
from cereal import car
from common.params import Params, put_nonblocking
-from common.realtime import set_realtime_priority, DT_MDL
+from common.realtime import config_realtime_process, DT_MDL
from common.numpy_fast import clip
from selfdrive.locationd.models.car_kf import CarKalman, ObservationKind, States
from selfdrive.locationd.models.constants import GENERATED_DIR
@@ -103,8 +101,7 @@ class ParamsLearner:
def main(sm=None, pm=None):
- gc.disable()
- set_realtime_priority(5)
+ config_realtime_process([0, 1, 2, 3], 5)
if sm is None:
sm = messaging.SubMaster(['liveLocationKalman', 'carState'], poll=['liveLocationKalman'])
@@ -178,7 +175,6 @@ def main(sm=None, pm=None):
angle_offset = clip(math.degrees(x[States.ANGLE_OFFSET] + x[States.ANGLE_OFFSET_FAST]), angle_offset - MAX_ANGLE_OFFSET_DELTA, angle_offset + MAX_ANGLE_OFFSET_DELTA)
msg = messaging.new_message('liveParameters')
- msg.logMonoTime = sm.logMonoTime['carState']
liveParameters = msg.liveParameters
liveParameters.posenetValid = True
diff --git a/selfdrive/logcatd/SConscript b/selfdrive/logcatd/SConscript
index 8811f32fe6..6bd7c6ff3e 100644
--- a/selfdrive/logcatd/SConscript
+++ b/selfdrive/logcatd/SConscript
@@ -1,6 +1,3 @@
-Import('env', 'cereal', 'messaging', 'common', 'arch')
+Import('env', 'cereal', 'messaging', 'common')
-if arch == "aarch64":
- env.Program('logcatd', 'logcatd_android.cc', LIBS=[cereal, messaging, common, 'cutils', 'zmq', 'capnp', 'kj'])
-else:
- env.Program('logcatd', 'logcatd_systemd.cc', LIBS=[cereal, messaging, common, 'zmq', 'capnp', 'kj', 'systemd', 'json11'])
+env.Program('logcatd', 'logcatd_systemd.cc', LIBS=[cereal, messaging, common, 'zmq', 'capnp', 'kj', 'systemd', 'json11'])
diff --git a/selfdrive/logcatd/logcatd_android.cc b/selfdrive/logcatd/logcatd_android.cc
deleted file mode 100644
index 8c2524d94a..0000000000
--- a/selfdrive/logcatd/logcatd_android.cc
+++ /dev/null
@@ -1,51 +0,0 @@
-#include
-#include
-#include
-#include
-
-#include
-
-#include "cereal/messaging/messaging.h"
-
-#undef LOG_ID_KERNEL
-#define LOG_ID_KERNEL 5
-
-int main() {
- std::signal(SIGINT, exit);
- std::signal(SIGTERM, exit);
- setpriority(PRIO_PROCESS, 0, -15);
-
- // setup android logging
- logger_list *logger_list = android_logger_list_alloc(ANDROID_LOG_RDONLY, 0, 0);
- assert(logger_list);
- for (auto log_id : {LOG_ID_MAIN, LOG_ID_RADIO, LOG_ID_SYSTEM, LOG_ID_CRASH, (log_id_t)LOG_ID_KERNEL}) {
- logger *logger = android_logger_open(logger_list, log_id);
- assert(logger);
- }
-
- PubMaster pm({"androidLog"});
-
- while (true) {
- log_msg log_msg;
- int err = android_logger_list_read(logger_list, &log_msg);
- if (err <= 0) break;
-
- AndroidLogEntry entry;
- err = android_log_processLogBuffer(&log_msg.entry_v1, &entry);
- if (err < 0) continue;
-
- MessageBuilder msg;
- auto androidEntry = msg.initEvent().initAndroidLog();
- androidEntry.setId(log_msg.id());
- androidEntry.setTs(entry.tv_sec * NS_PER_SEC + entry.tv_nsec);
- androidEntry.setPriority(entry.priority);
- androidEntry.setPid(entry.pid);
- androidEntry.setTid(entry.tid);
- androidEntry.setTag(entry.tag);
- androidEntry.setMessage(entry.message);
- pm.send("androidLog", msg);
- }
-
- android_logger_list_free(logger_list);
- return 0;
-}
diff --git a/selfdrive/logcatd/tests/test_logcatd_android.py b/selfdrive/logcatd/tests/test_logcatd_android.py
deleted file mode 100755
index 4e0c903dfa..0000000000
--- a/selfdrive/logcatd/tests/test_logcatd_android.py
+++ /dev/null
@@ -1,45 +0,0 @@
-#!/usr/bin/env python3
-import os
-import random
-import string
-import time
-import unittest
-import uuid
-
-import cereal.messaging as messaging
-from selfdrive.test.helpers import with_processes
-
-class TestLogcatdAndroid(unittest.TestCase):
-
- @with_processes(['logcatd'])
- def test_log(self):
- sock = messaging.sub_sock("androidLog", conflate=False)
-
- # make sure sockets are ready
- time.sleep(1)
- messaging.drain_sock(sock)
-
- sent_msgs = {}
- for _ in range(random.randint(2, 10)):
- # write some log messages
- for __ in range(random.randint(5, 50)):
- tag = uuid.uuid4().hex
- msg = ''.join(random.choice(string.ascii_letters) for _ in range(random.randrange(2, 50)))
- sent_msgs[tag] = {'recv_cnt': 0, 'msg': msg}
- os.system(f"log -t '{tag}' '{msg}'")
-
- time.sleep(1)
- msgs = messaging.drain_sock(sock)
- for m in msgs:
- self.assertTrue(m.valid)
- self.assertLess(time.monotonic() - (m.logMonoTime / 1e9), 30)
- tag = m.androidLog.tag
- if tag in sent_msgs:
- sent_msgs[tag]['recv_cnt'] += 1
- self.assertEqual(m.androidLog.message.strip(), sent_msgs[tag]['msg'])
-
- for v in sent_msgs.values():
- self.assertEqual(v['recv_cnt'], 1)
-
-if __name__ == "__main__":
- unittest.main()
diff --git a/selfdrive/loggerd/SConscript b/selfdrive/loggerd/SConscript
index 76fbcae6c4..92cc37a5fb 100644
--- a/selfdrive/loggerd/SConscript
+++ b/selfdrive/loggerd/SConscript
@@ -4,19 +4,14 @@ Import('env', 'arch', 'cereal', 'messaging', 'common', 'visionipc', 'gpucommon')
libs = [common, cereal, messaging, visionipc,
'zmq', 'capnp', 'kj', 'z',
'avformat', 'avcodec', 'swscale', 'avutil',
- 'yuv', 'bz2', 'OpenCL']
+ 'yuv', 'bz2', 'OpenCL', 'pthread']
-src = ['logger.cc', 'loggerd.cc']
-if arch in ["aarch64", "larch64"]:
+src = ['logger.cc', 'loggerd.cc', 'video_writer.cc']
+if arch == "larch64":
src += ['omx_encoder.cc']
libs += ['OmxCore', 'gsl', 'CB'] + gpucommon
- if arch == "aarch64":
- libs += ['OmxVenc', 'cutils']
- else:
- libs += ['pthread']
else:
src += ['raw_logger.cc']
- libs += ['pthread']
if arch == "Darwin":
# fix OpenCL
diff --git a/selfdrive/loggerd/bootlog.cc b/selfdrive/loggerd/bootlog.cc
index a723124847..e7ce308cec 100644
--- a/selfdrive/loggerd/bootlog.cc
+++ b/selfdrive/loggerd/bootlog.cc
@@ -11,8 +11,6 @@ static kj::Array build_boot_log() {
if (Hardware::TICI()) {
bootlog_commands.push_back("journalctl");
bootlog_commands.push_back("sudo nvme smart-log --output-format=json /dev/nvme0");
- } else if (Hardware::EON()) {
- bootlog_commands.push_back("logcat -d");
}
MessageBuilder msg;
diff --git a/selfdrive/loggerd/logger.cc b/selfdrive/loggerd/logger.cc
index 6ba520d162..f2c34554e3 100644
--- a/selfdrive/loggerd/logger.cc
+++ b/selfdrive/loggerd/logger.cc
@@ -14,9 +14,6 @@
#include
#include
#include
-#ifdef QCOM
-#include
-#endif
#include "selfdrive/common/params.h"
#include "selfdrive/common/swaglog.h"
@@ -36,9 +33,7 @@ kj::Array logger_build_init_data() {
MessageBuilder msg;
auto init = msg.initEvent().initInitData();
- if (Hardware::EON()) {
- init.setDeviceType(cereal::InitData::DeviceType::NEO);
- } else if (Hardware::TICI()) {
+ if (Hardware::TICI()) {
init.setDeviceType(cereal::InitData::DeviceType::TICI);
} else {
init.setDeviceType(cereal::InitData::DeviceType::PC);
@@ -61,20 +56,6 @@ kj::Array logger_build_init_data() {
init.setKernelVersion(util::read_file("/proc/version"));
init.setOsVersion(util::read_file("/VERSION"));
-#ifdef QCOM
- {
- std::vector > properties;
- property_list(append_property, (void*)&properties);
-
- auto lentries = init.initAndroidProperties().initEntries(properties.size());
- for (int i=0; i
int main(int argc, char** argv) {
- if (Hardware::EON()) {
- setpriority(PRIO_PROCESS, 0, -20);
- } else if (Hardware::TICI()) {
+ if (Hardware::TICI()) {
int ret;
ret = util::set_core_affinity({0, 1, 2, 3});
assert(ret == 0);
diff --git a/selfdrive/loggerd/omx_encoder.cc b/selfdrive/loggerd/omx_encoder.cc
index 42637357ba..31e035955a 100644
--- a/selfdrive/loggerd/omx_encoder.cc
+++ b/selfdrive/loggerd/omx_encoder.cc
@@ -154,16 +154,15 @@ static const char* omx_color_fomat_name(uint32_t format) {
// ***** encoder functions *****
-OmxEncoder::OmxEncoder(const char* filename, CameraType type, int width, int height, int fps, int bitrate, bool h265, bool downscale, bool write) {
+OmxEncoder::OmxEncoder(const char* filename, CameraType type, int in_width, int in_height, int fps, int bitrate, bool h265, int out_width, int out_height, bool write)
+ : in_width_(in_width), in_height_(in_height), width(out_width), height(out_height) {
this->filename = filename;
this->type = type;
this->write = write;
- this->width = width;
- this->height = height;
this->fps = fps;
this->remuxing = !h265;
- this->downscale = downscale;
+ this->downscale = in_width != out_width || in_height != out_height;
if (this->downscale) {
this->y_ptr2 = (uint8_t *)malloc(this->width*this->height);
this->u_ptr2 = (uint8_t *)malloc(this->width*this->height/4);
@@ -233,13 +232,8 @@ OmxEncoder::OmxEncoder(const char* filename, CameraType type, int width, int hei
if (h265) {
// setup HEVC
- #ifndef QCOM2
- OMX_VIDEO_PARAM_HEVCTYPE hevc_type = {0};
- OMX_INDEXTYPE index_type = (OMX_INDEXTYPE) OMX_IndexParamVideoHevc;
- #else
OMX_VIDEO_PARAM_PROFILELEVELTYPE hevc_type = {0};
OMX_INDEXTYPE index_type = OMX_IndexParamVideoProfileLevelCurrent;
- #endif
hevc_type.nSize = sizeof(hevc_type);
hevc_type.nPortIndex = (OMX_U32) PORT_INDEX_OUT;
OMX_CHECK(OMX_GetParameter(this->handle, index_type, (OMX_PTR) &hevc_type));
@@ -326,6 +320,11 @@ OmxEncoder::OmxEncoder(const char* filename, CameraType type, int width, int hei
}
LOGE("omx initialized - in: %d - out %d", this->in_buf_headers.size(), this->out_buf_headers.size());
+
+ service_name = this->type == DriverCam ? "driverEncodeData" :
+ (this->type == WideRoadCam ? "wideRoadEncodeData" :
+ (this->remuxing ? "qRoadEncodeData" : "roadEncodeData"));
+ pm.reset(new PubMaster({service_name}));
}
void OmxEncoder::callback_handler(OmxEncoder *e) {
@@ -366,20 +365,22 @@ void OmxEncoder::callback_handler(OmxEncoder *e) {
void OmxEncoder::write_and_broadcast_handler(OmxEncoder *e){
bool exit = false;
- const char *service_name = e->type == DriverCam ? "driverEncodeData" : (e->type == WideRoadCam ? "wideRoadEncodeData" : "roadEncodeData");
- PubMaster pm({service_name});
+ e->segment_num++;
uint32_t idx = 0;
while (!exit) {
OmxBuffer *out_buf = e->to_write.pop();
MessageBuilder msg;
auto edata = (e->type == DriverCam) ? msg.initEvent(true).initDriverEncodeData() :
- ((e->type == WideRoadCam) ? msg.initEvent(true).initWideRoadEncodeData() : msg.initEvent(true).initRoadEncodeData());
+ ((e->type == WideRoadCam) ? msg.initEvent(true).initWideRoadEncodeData() :
+ (e->remuxing ? msg.initEvent(true).initQRoadEncodeData() : msg.initEvent(true).initRoadEncodeData()));
edata.setData(kj::heapArray(out_buf->data, out_buf->header.nFilledLen));
edata.setTimestampEof(out_buf->header.nTimeStamp);
edata.setIdx(idx++);
- pm.send(service_name, msg);
+ edata.setSegmentNum(e->segment_num);
+ edata.setFlags(out_buf->header.nFlags);
+ e->pm->send(e->service_name, msg);
OmxEncoder::handle_out_buf(e, out_buf);
if (out_buf->header.nFlags & OMX_BUFFERFLAG_EOS) {
@@ -391,87 +392,20 @@ void OmxEncoder::write_and_broadcast_handler(OmxEncoder *e){
}
-void OmxEncoder::write_handler(OmxEncoder *e){
- bool exit = false;
- while (!exit) {
- OmxBuffer *out_buf = e->to_write.pop();
- OmxEncoder::handle_out_buf(e, out_buf);
-
- if (out_buf->header.nFlags & OMX_BUFFERFLAG_EOS) {
- exit = true;
- }
-
- free(out_buf);
- }
-}
-
-
void OmxEncoder::handle_out_buf(OmxEncoder *e, OmxBuffer *out_buf) {
- int err;
-
- if (out_buf->header.nFlags & OMX_BUFFERFLAG_CODECCONFIG) {
- if (e->codec_config_len < out_buf->header.nFilledLen) {
- e->codec_config = (uint8_t *)realloc(e->codec_config, out_buf->header.nFilledLen);
- }
- e->codec_config_len = out_buf->header.nFilledLen;
- memcpy(e->codec_config, out_buf->data, out_buf->header.nFilledLen);
-
- // TODO: is still needed?
-#ifdef QCOM2
- out_buf->header.nTimeStamp = 0;
-#endif
- }
-
- if (e->of) {
- //printf("write %d flags 0x%x\n", out_buf->nFilledLen, out_buf->nFlags);
- size_t written = util::safe_fwrite(out_buf->data, 1, out_buf->header.nFilledLen, e->of);
- if (written != out_buf->header.nFilledLen) {
- LOGE("failed to write file.errno=%d", errno);
- }
- }
-
- if (e->remuxing) {
- if (!e->wrote_codec_config && e->codec_config_len > 0) {
- // extradata will be freed by av_free() in avcodec_free_context()
- e->codec_ctx->extradata = (uint8_t*)av_mallocz(e->codec_config_len + AV_INPUT_BUFFER_PADDING_SIZE);
- e->codec_ctx->extradata_size = e->codec_config_len;
- memcpy(e->codec_ctx->extradata, e->codec_config, e->codec_config_len);
-
- err = avcodec_parameters_from_context(e->out_stream->codecpar, e->codec_ctx);
- assert(err >= 0);
- err = avformat_write_header(e->ofmt_ctx, NULL);
- assert(err >= 0);
-
- e->wrote_codec_config = true;
- }
-
- if (out_buf->header.nTimeStamp > 0) {
- // input timestamps are in microseconds
- AVRational in_timebase = {1, 1000000};
-
- AVPacket pkt;
- av_init_packet(&pkt);
- pkt.data = out_buf->data;
- pkt.size = out_buf->header.nFilledLen;
-
- enum AVRounding rnd = static_cast(AV_ROUND_NEAR_INF|AV_ROUND_PASS_MINMAX);
- pkt.pts = pkt.dts = av_rescale_q_rnd(out_buf->header.nTimeStamp, in_timebase, e->ofmt_ctx->streams[0]->time_base, rnd);
- pkt.duration = av_rescale_q(50*1000, in_timebase, e->ofmt_ctx->streams[0]->time_base);
-
- if (out_buf->header.nFlags & OMX_BUFFERFLAG_SYNCFRAME) {
- pkt.flags |= AV_PKT_FLAG_KEY;
- }
-
- err = av_write_frame(e->ofmt_ctx, &pkt);
- if (err < 0) { LOGW("ts encoder write issue"); }
-
- av_free_packet(&pkt);
- }
+ if (!(out_buf->header.nFlags & OMX_BUFFERFLAG_EOS) && e->writer) {
+ e->writer->write(out_buf->data,
+ out_buf->header.nFilledLen,
+ out_buf->header.nTimeStamp,
+ out_buf->header.nFlags & OMX_BUFFERFLAG_CODECCONFIG,
+ out_buf->header.nFlags & OMX_BUFFERFLAG_SYNCFRAME);
}
}
int OmxEncoder::encode_frame(const uint8_t *y_ptr, const uint8_t *u_ptr, const uint8_t *v_ptr,
int in_width, int in_height, uint64_t ts) {
+ assert(in_width == this->in_width_);
+ assert(in_height == this->in_height_);
int err;
if (!this->is_open) {
return -1;
@@ -539,57 +473,13 @@ int OmxEncoder::encode_frame(const uint8_t *y_ptr, const uint8_t *u_ptr, const u
}
void OmxEncoder::encoder_open(const char* path) {
- int err;
-
- snprintf(this->vid_path, sizeof(this->vid_path), "%s/%s", path, this->filename);
- LOGD("encoder_open %s remuxing:%d", this->vid_path, this->remuxing);
-
- if (this->remuxing) {
- avformat_alloc_output_context2(&this->ofmt_ctx, NULL, NULL, this->vid_path);
- assert(this->ofmt_ctx);
-
- this->out_stream = avformat_new_stream(this->ofmt_ctx, NULL);
- assert(this->out_stream);
-
- // set codec correctly
- av_register_all();
-
- AVCodec *codec = NULL;
- codec = avcodec_find_encoder(AV_CODEC_ID_H264);
- assert(codec);
-
- this->codec_ctx = avcodec_alloc_context3(codec);
- assert(this->codec_ctx);
- this->codec_ctx->width = this->width;
- this->codec_ctx->height = this->height;
- this->codec_ctx->pix_fmt = AV_PIX_FMT_YUV420P;
- this->codec_ctx->time_base = (AVRational){ 1, this->fps };
-
- err = avio_open(&this->ofmt_ctx->pb, this->vid_path, AVIO_FLAG_WRITE);
- assert(err >= 0);
-
- this->wrote_codec_config = false;
- } else {
- if (this->write) {
- this->of = util::safe_fopen(this->vid_path, "wb");
- assert(this->of);
-#ifndef QCOM2
- if (this->codec_config_len > 0) {
- util::safe_fwrite(this->codec_config, 1, this->codec_config_len, this->of);
- }
-#endif
- }
+ if (this->write) {
+ writer.reset(new VideoWriter(path, this->filename, this->remuxing, this->width, this->height, this->fps, !this->remuxing, false));
}
- // create camera lock file
- snprintf(this->lock_path, sizeof(this->lock_path), "%s/%s.lock", path, this->filename);
- int lock_fd = HANDLE_EINTR(open(this->lock_path, O_RDWR | O_CREAT, 0664));
- assert(lock_fd >= 0);
- close(lock_fd);
-
// start writer threads
callback_handler_thread = std::thread(OmxEncoder::callback_handler, this);
- write_handler_thread = std::thread(this->remuxing ? OmxEncoder::write_handler : OmxEncoder::write_and_broadcast_handler, this);
+ write_handler_thread = std::thread(OmxEncoder::write_and_broadcast_handler, this);
this->is_open = true;
this->counter = 0;
@@ -614,19 +504,7 @@ void OmxEncoder::encoder_close() {
callback_handler_thread.join();
write_handler_thread.join();
- if (this->remuxing) {
- av_write_trailer(this->ofmt_ctx);
- avcodec_free_context(&this->codec_ctx);
- avio_closep(&this->ofmt_ctx->pb);
- avformat_free_context(this->ofmt_ctx);
- } else {
- if (this->of) {
- util::safe_fflush(this->of);
- fclose(this->of);
- this->of = nullptr;
- }
- }
- unlink(this->lock_path);
+ writer.reset();
}
this->is_open = false;
}
@@ -661,10 +539,6 @@ OmxEncoder::~OmxEncoder() {
free(write_buf);
};
- if (this->codec_config) {
- free(this->codec_config);
- }
-
if (this->downscale) {
free(this->y_ptr2);
free(this->u_ptr2);
diff --git a/selfdrive/loggerd/omx_encoder.h b/selfdrive/loggerd/omx_encoder.h
index 65f2c0b9be..e57dccbe11 100644
--- a/selfdrive/loggerd/omx_encoder.h
+++ b/selfdrive/loggerd/omx_encoder.h
@@ -6,12 +6,10 @@
#include
#include
-extern "C" {
-#include
-}
#include "selfdrive/common/queue.h"
#include "selfdrive/loggerd/encoder.h"
+#include "selfdrive/loggerd/video_writer.h"
struct OmxBuffer {
OMX_BUFFERHEADERTYPE header;
@@ -22,7 +20,7 @@ struct OmxBuffer {
// OmxEncoder, lossey codec using hardware hevc
class OmxEncoder : public VideoEncoder {
public:
- OmxEncoder(const char* filename, CameraType type, int width, int height, int fps, int bitrate, bool h265, bool downscale, bool write = true);
+ OmxEncoder(const char* filename, CameraType type, int width, int height, int fps, int bitrate, bool h265, int out_width, int out_height, bool write = true);
~OmxEncoder();
int encode_frame(const uint8_t *y_ptr, const uint8_t *u_ptr, const uint8_t *v_ptr,
int in_width, int in_height, uint64_t ts);
@@ -40,28 +38,24 @@ public:
private:
void wait_for_state(OMX_STATETYPE state);
static void callback_handler(OmxEncoder *e);
- static void write_handler(OmxEncoder *e);
static void write_and_broadcast_handler(OmxEncoder *e);
static void handle_out_buf(OmxEncoder *e, OmxBuffer *out_buf);
+ int in_width_, in_height_;
int width, height, fps;
- char vid_path[1024];
- char lock_path[1024];
bool is_open = false;
bool dirty = false;
bool write = false;
int counter = 0;
std::thread callback_handler_thread;
std::thread write_handler_thread;
+ int segment_num = -1;
+ std::unique_ptr pm;
+ const char *service_name;
const char* filename;
- FILE *of = nullptr;
CameraType type;
- size_t codec_config_len;
- uint8_t *codec_config = NULL;
- bool wrote_codec_config;
-
std::mutex state_lock;
std::condition_variable state_cv;
OMX_STATETYPE state = OMX_StateLoaded;
@@ -77,10 +71,8 @@ private:
SafeQueue done_out;
SafeQueue to_write;
- AVFormatContext *ofmt_ctx;
- AVCodecContext *codec_ctx;
- AVStream *out_stream;
bool remuxing;
+ std::unique_ptr writer;
bool downscale;
uint8_t *y_ptr2, *u_ptr2, *v_ptr2;
diff --git a/selfdrive/loggerd/raw_logger.cc b/selfdrive/loggerd/raw_logger.cc
index 6da0d59ad3..29d421a881 100644
--- a/selfdrive/loggerd/raw_logger.cc
+++ b/selfdrive/loggerd/raw_logger.cc
@@ -22,123 +22,59 @@ extern "C" {
#include "selfdrive/common/swaglog.h"
#include "selfdrive/common/util.h"
-RawLogger::RawLogger(const char* filename, CameraType type, int width, int height, int fps,
- int bitrate, bool h265, bool downscale, bool write)
- : filename(filename), fps(fps) {
-
+RawLogger::RawLogger(const char* filename, CameraType type, int in_width, int in_height, int fps,
+ int bitrate, bool h265, int out_width, int out_height, bool write)
+ : in_width_(in_width), in_height_(in_height), filename(filename), fps(fps) {
// TODO: respect write arg
-
- codec = avcodec_find_encoder(AV_CODEC_ID_FFVHUFF);
- // codec = avcodec_find_encoder(AV_CODEC_ID_FFV1);
- assert(codec);
-
- codec_ctx = avcodec_alloc_context3(codec);
- assert(codec_ctx);
- codec_ctx->width = width;
- codec_ctx->height = height;
- codec_ctx->pix_fmt = AV_PIX_FMT_YUV420P;
-
- // codec_ctx->thread_count = 2;
-
- // ffv1enc doesn't respect AV_PICTURE_TYPE_I. make every frame a key frame for now.
- // codec_ctx->gop_size = 0;
-
- codec_ctx->time_base = (AVRational){ 1, fps };
-
- int err = avcodec_open2(codec_ctx, codec, NULL);
- assert(err >= 0);
-
frame = av_frame_alloc();
assert(frame);
- frame->format = codec_ctx->pix_fmt;
- frame->width = width;
- frame->height = height;
- frame->linesize[0] = width;
- frame->linesize[1] = width/2;
- frame->linesize[2] = width/2;
-
- if (downscale) {
- downscale_buf.resize(width * height * 3 / 2);
+ frame->format = AV_PIX_FMT_YUV420P;
+ frame->width = out_width;
+ frame->height = out_height;
+ frame->linesize[0] = out_width;
+ frame->linesize[1] = out_width/2;
+ frame->linesize[2] = out_width/2;
+
+ if (in_width != out_width || in_height != out_height) {
+ downscale_buf.resize(out_width * out_height * 3 / 2);
}
}
RawLogger::~RawLogger() {
+ encoder_close();
av_frame_free(&frame);
- avcodec_close(codec_ctx);
- av_free(codec_ctx);
}
void RawLogger::encoder_open(const char* path) {
- vid_path = util::string_format("%s/%s", path, filename);
-
- // create camera lock file
- lock_path = util::string_format("%s/%s.lock", path, filename);
-
- LOG("open %s\n", lock_path.c_str());
-
- int lock_fd = HANDLE_EINTR(open(lock_path.c_str(), O_RDWR | O_CREAT, 0664));
- assert(lock_fd >= 0);
- close(lock_fd);
-
- format_ctx = NULL;
- avformat_alloc_output_context2(&format_ctx, NULL, "matroska", vid_path.c_str());
- assert(format_ctx);
-
- stream = avformat_new_stream(format_ctx, codec);
- // AVStream *stream = avformat_new_stream(format_ctx, NULL);
- assert(stream);
- stream->id = 0;
- stream->time_base = (AVRational){ 1, fps };
- // codec_ctx->time_base = stream->time_base;
-
- int err = avcodec_parameters_from_context(stream->codecpar, codec_ctx);
- assert(err >= 0);
-
- err = avio_open(&format_ctx->pb, vid_path.c_str(), AVIO_FLAG_WRITE);
- assert(err >= 0);
-
- err = avformat_write_header(format_ctx, NULL);
- assert(err >= 0);
-
+ writer = new VideoWriter(path, this->filename, true, frame->width, frame->height, this->fps, false, true);
+ // write the header
+ writer->write(NULL, 0, 0, true, false);
is_open = true;
- counter = 0;
}
void RawLogger::encoder_close() {
if (!is_open) return;
-
- int err = av_write_trailer(format_ctx);
- assert(err == 0);
-
- err = avio_closep(&format_ctx->pb);
- assert(err == 0);
-
- avformat_free_context(format_ctx);
- format_ctx = NULL;
-
- unlink(lock_path.c_str());
+ delete writer;
is_open = false;
}
int RawLogger::encode_frame(const uint8_t *y_ptr, const uint8_t *u_ptr, const uint8_t *v_ptr,
int in_width, int in_height, uint64_t ts) {
- AVPacket pkt;
- av_init_packet(&pkt);
- pkt.data = NULL;
- pkt.size = 0;
+ assert(in_width == this->in_width_);
+ assert(in_height == this->in_height_);
if (downscale_buf.size() > 0) {
uint8_t *out_y = downscale_buf.data();
- uint8_t *out_u = out_y + codec_ctx->width * codec_ctx->height;
- uint8_t *out_v = out_u + (codec_ctx->width / 2) * (codec_ctx->height / 2);
+ uint8_t *out_u = out_y + frame->width * frame->height;
+ uint8_t *out_v = out_u + (frame->width / 2) * (frame->height / 2);
libyuv::I420Scale(y_ptr, in_width,
u_ptr, in_width/2,
v_ptr, in_width/2,
in_width, in_height,
- out_y, codec_ctx->width,
- out_u, codec_ctx->width/2,
- out_v, codec_ctx->width/2,
- codec_ctx->width, codec_ctx->height,
+ out_y, frame->width,
+ out_u, frame->width/2,
+ out_v, frame->width/2,
+ frame->width, frame->height,
libyuv::kFilterNone);
frame->data[0] = out_y;
frame->data[1] = out_u;
@@ -148,18 +84,22 @@ int RawLogger::encode_frame(const uint8_t *y_ptr, const uint8_t *u_ptr, const ui
frame->data[1] = (uint8_t*)u_ptr;
frame->data[2] = (uint8_t*)v_ptr;
}
- frame->pts = counter;
+ frame->pts = counter*50*1000; // 50ms per frame
int ret = counter;
- int err = avcodec_send_frame(codec_ctx, frame);
- if (ret < 0) {
- LOGE("avcode_send_frame error %d", err);
+ int err = avcodec_send_frame(writer->codec_ctx, frame);
+ if (err < 0) {
+ LOGE("avcodec_send_frame error %d", err);
ret = -1;
}
- while (ret >= 0){
- err = avcodec_receive_packet(codec_ctx, &pkt);
+ AVPacket pkt;
+ av_init_packet(&pkt);
+ pkt.data = NULL;
+ pkt.size = 0;
+ while (ret >= 0) {
+ err = avcodec_receive_packet(writer->codec_ctx, &pkt);
if (err == AVERROR_EOF) {
break;
} else if (err == AVERROR(EAGAIN)) {
@@ -172,18 +112,9 @@ int RawLogger::encode_frame(const uint8_t *y_ptr, const uint8_t *u_ptr, const ui
break;
}
- av_packet_rescale_ts(&pkt, codec_ctx->time_base, stream->time_base);
- pkt.stream_index = 0;
-
- err = av_interleaved_write_frame(format_ctx, &pkt);
- if (err < 0) {
- LOGE("av_interleaved_write_frame %d", err);
- ret = -1;
- } else {
- counter++;
- }
+ writer->write(pkt.data, pkt.size, pkt.pts, false, pkt.flags & AV_PKT_FLAG_KEY);
+ counter++;
}
-
av_packet_unref(&pkt);
return ret;
}
diff --git a/selfdrive/loggerd/raw_logger.h b/selfdrive/loggerd/raw_logger.h
index 2fa23f83cd..56bd08ff1f 100644
--- a/selfdrive/loggerd/raw_logger.h
+++ b/selfdrive/loggerd/raw_logger.h
@@ -12,11 +12,12 @@ extern "C" {
}
#include "selfdrive/loggerd/encoder.h"
+#include "selfdrive/loggerd/video_writer.h"
class RawLogger : public VideoEncoder {
public:
- RawLogger(const char* filename, CameraType type, int width, int height, int fps,
- int bitrate, bool h265, bool downscale, bool write = true);
+ RawLogger(const char* filename, CameraType type, int in_width, int in_height, int fps,
+ int bitrate, bool h265, int out_width, int out_height, bool write = true);
~RawLogger();
int encode_frame(const uint8_t *y_ptr, const uint8_t *u_ptr, const uint8_t *v_ptr,
int in_width, int in_height, uint64_t ts);
@@ -30,14 +31,10 @@ private:
int counter = 0;
bool is_open = false;
- std::string vid_path, lock_path;
-
- const AVCodec *codec = NULL;
- AVCodecContext *codec_ctx = NULL;
-
- AVStream *stream = NULL;
- AVFormatContext *format_ctx = NULL;
+ int in_width_, in_height_;
AVFrame *frame = NULL;
std::vector downscale_buf;
+
+ VideoWriter *writer = NULL;
};
diff --git a/selfdrive/loggerd/tests/test_encoder.py b/selfdrive/loggerd/tests/test_encoder.py
index 97c142652f..9479b6256a 100755
--- a/selfdrive/loggerd/tests/test_encoder.py
+++ b/selfdrive/loggerd/tests/test_encoder.py
@@ -13,27 +13,19 @@ from tqdm import trange
from common.params import Params
from common.timeout import Timeout
-from selfdrive.hardware import EON, TICI
+from selfdrive.hardware import TICI
from selfdrive.loggerd.config import ROOT
from selfdrive.manager.process_config import managed_processes
from tools.lib.logreader import LogReader
SEGMENT_LENGTH = 2
-if EON:
- FULL_SIZE = 1253786 # file size for a 2s segment in bytes
- CAMERAS = [
- ("fcamera.hevc", 20, FULL_SIZE, "roadEncodeIdx"),
- ("dcamera.hevc", 10, 770920, "driverEncodeIdx"),
- ("qcamera.ts", 20, 77066, None),
- ]
-else:
- FULL_SIZE = 2507572
- CAMERAS = [
- ("fcamera.hevc", 20, FULL_SIZE, "roadEncodeIdx"),
- ("dcamera.hevc", 20, FULL_SIZE, "driverEncodeIdx"),
- ("ecamera.hevc", 20, FULL_SIZE, "wideRoadEncodeIdx"),
- ("qcamera.ts", 20, 77066, None),
- ]
+FULL_SIZE = 2507572
+CAMERAS = [
+ ("fcamera.hevc", 20, FULL_SIZE, "roadEncodeIdx"),
+ ("dcamera.hevc", 20, FULL_SIZE, "driverEncodeIdx"),
+ ("ecamera.hevc", 20, FULL_SIZE, "wideRoadEncodeIdx"),
+ ("qcamera.ts", 20, 77066, None),
+]
# we check frame count, so we don't have to be too strict on size
FILE_SIZE_TOLERANCE = 0.5
@@ -44,7 +36,7 @@ class TestEncoder(unittest.TestCase):
# TODO: all of loggerd should work on PC
@classmethod
def setUpClass(cls):
- if not (EON or TICI):
+ if not TICI:
raise unittest.SkipTest
def setUp(self):
@@ -93,8 +85,6 @@ class TestEncoder(unittest.TestCase):
if not record_front and "dcamera" in camera:
continue
- eon_dcam = EON and (camera == 'dcamera.hevc')
-
file_path = f"{route_prefix_path}--{i}/{camera}"
# check file exists
@@ -107,7 +97,7 @@ class TestEncoder(unittest.TestCase):
cmd = "LD_LIBRARY_PATH=/usr/local/lib " + cmd
expected_frames = fps * SEGMENT_LENGTH
- frame_tolerance = 1 if eon_dcam else 0
+ frame_tolerance = 0
probe = subprocess.check_output(cmd, shell=True, encoding='utf8')
frame_count = int(probe.split('\n')[0].strip())
counts.append(frame_count)
@@ -140,9 +130,8 @@ class TestEncoder(unittest.TestCase):
self.assertTrue(all(valid))
- if not eon_dcam:
- self.assertEqual(expected_frames * i, encode_idxs[0])
- first_frames.append(frame_idxs[0])
+ self.assertEqual(expected_frames * i, encode_idxs[0])
+ first_frames.append(frame_idxs[0])
self.assertEqual(len(set(encode_idxs)), len(encode_idxs))
self.assertEqual(1, len(set(first_frames)))
diff --git a/selfdrive/loggerd/video_writer.cc b/selfdrive/loggerd/video_writer.cc
new file mode 100644
index 0000000000..f9034f6912
--- /dev/null
+++ b/selfdrive/loggerd/video_writer.cc
@@ -0,0 +1,118 @@
+#pragma clang diagnostic ignored "-Wdeprecated-declarations"
+
+#include
+#include
+
+#include "selfdrive/loggerd/video_writer.h"
+#include "selfdrive/common/swaglog.h"
+#include "selfdrive/common/util.h"
+
+VideoWriter::VideoWriter(const char *path, const char *filename, bool remuxing, int width, int height, int fps, bool h265, bool raw)
+ : remuxing(remuxing), raw(raw) {
+ vid_path = util::string_format("%s/%s", path, filename);
+ lock_path = util::string_format("%s/%s.lock", path, filename);
+
+ int lock_fd = HANDLE_EINTR(open(lock_path.c_str(), O_RDWR | O_CREAT, 0664));
+ assert(lock_fd >= 0);
+ close(lock_fd);
+
+ LOGD("encoder_open %s remuxing:%d", this->vid_path.c_str(), this->remuxing);
+ if (this->remuxing) {
+ avformat_alloc_output_context2(&this->ofmt_ctx, NULL, raw ? "matroska" : NULL, this->vid_path.c_str());
+ assert(this->ofmt_ctx);
+
+ // set codec correctly. needed?
+ av_register_all();
+
+ AVCodec *codec = NULL;
+ assert(!h265);
+ codec = avcodec_find_encoder(raw ? AV_CODEC_ID_FFVHUFF : AV_CODEC_ID_H264);
+ assert(codec);
+
+ this->codec_ctx = avcodec_alloc_context3(codec);
+ assert(this->codec_ctx);
+ this->codec_ctx->width = width;
+ this->codec_ctx->height = height;
+ this->codec_ctx->pix_fmt = AV_PIX_FMT_YUV420P;
+ this->codec_ctx->time_base = (AVRational){ 1, fps };
+
+ if (raw) {
+ // since the codec is actually used, we open it
+ int err = avcodec_open2(this->codec_ctx, codec, NULL);
+ assert(err >= 0);
+ }
+
+ this->out_stream = avformat_new_stream(this->ofmt_ctx, raw ? codec : NULL);
+ assert(this->out_stream);
+
+ int err = avio_open(&this->ofmt_ctx->pb, this->vid_path.c_str(), AVIO_FLAG_WRITE);
+ assert(err >= 0);
+
+ this->wrote_codec_config = false;
+ } else {
+ this->of = util::safe_fopen(this->vid_path.c_str(), "wb");
+ assert(this->of);
+ }
+}
+
+void VideoWriter::write(uint8_t *data, int len, long long timestamp, bool codecconfig, bool keyframe) {
+ if (of && data) {
+ size_t written = util::safe_fwrite(data, 1, len, of);
+ if (written != len) {
+ LOGE("failed to write file.errno=%d", errno);
+ }
+ }
+
+ if (remuxing) {
+ if (codecconfig) {
+ if (data) {
+ codec_ctx->extradata = (uint8_t*)av_mallocz(len + AV_INPUT_BUFFER_PADDING_SIZE);
+ codec_ctx->extradata_size = len;
+ memcpy(codec_ctx->extradata, data, len);
+ }
+ int err = avcodec_parameters_from_context(out_stream->codecpar, codec_ctx);
+ assert(err >= 0);
+ err = avformat_write_header(ofmt_ctx, NULL);
+ assert(err >= 0);
+ } else {
+ // input timestamps are in microseconds
+ AVRational in_timebase = {1, 1000000};
+
+ AVPacket pkt;
+ av_init_packet(&pkt);
+ pkt.data = data;
+ pkt.size = len;
+
+ enum AVRounding rnd = static_cast(AV_ROUND_NEAR_INF|AV_ROUND_PASS_MINMAX);
+ pkt.pts = pkt.dts = av_rescale_q_rnd(timestamp, in_timebase, ofmt_ctx->streams[0]->time_base, rnd);
+ pkt.duration = av_rescale_q(50*1000, in_timebase, ofmt_ctx->streams[0]->time_base);
+
+ if (keyframe) {
+ pkt.flags |= AV_PKT_FLAG_KEY;
+ }
+
+ // TODO: can use av_write_frame for non raw?
+ int err = av_interleaved_write_frame(ofmt_ctx, &pkt);
+ if (err < 0) { LOGW("ts encoder write issue"); }
+
+ av_free_packet(&pkt);
+ }
+ }
+}
+
+VideoWriter::~VideoWriter() {
+ if (this->remuxing) {
+ if (this->raw) { avcodec_close(this->codec_ctx); }
+ int err = av_write_trailer(this->ofmt_ctx);
+ if (err != 0) LOGE("av_write_trailer failed %d", err);
+ avcodec_free_context(&this->codec_ctx);
+ err = avio_closep(&this->ofmt_ctx->pb);
+ if (err != 0) LOGE("avio_closep failed %d", err);
+ avformat_free_context(this->ofmt_ctx);
+ } else {
+ util::safe_fflush(this->of);
+ fclose(this->of);
+ this->of = nullptr;
+ }
+ unlink(this->lock_path.c_str());
+}
diff --git a/selfdrive/loggerd/video_writer.h b/selfdrive/loggerd/video_writer.h
new file mode 100644
index 0000000000..8217b859bf
--- /dev/null
+++ b/selfdrive/loggerd/video_writer.h
@@ -0,0 +1,25 @@
+#pragma once
+
+#include
+
+extern "C" {
+#include
+}
+
+class VideoWriter {
+public:
+ VideoWriter(const char *path, const char *filename, bool remuxing, int width, int height, int fps, bool h265, bool raw);
+ void write(uint8_t *data, int len, long long timestamp, bool codecconfig, bool keyframe);
+ ~VideoWriter();
+ AVCodecContext *codec_ctx;
+private:
+ std::string vid_path, lock_path;
+
+ FILE *of = nullptr;
+
+ AVFormatContext *ofmt_ctx;
+ AVStream *out_stream;
+ bool remuxing, raw;
+
+ bool wrote_codec_config;
+};
\ No newline at end of file
diff --git a/selfdrive/manager/build.py b/selfdrive/manager/build.py
index 4b97855f34..806aa58bb7 100755
--- a/selfdrive/manager/build.py
+++ b/selfdrive/manager/build.py
@@ -29,7 +29,7 @@ def build(spinner: Spinner, dirty: bool = False) -> None:
j_flag = "" if nproc is None else f"-j{nproc - 1}"
for retry in [True, False]:
- scons = subprocess.Popen(["scons", j_flag, "--cache-populate"], cwd=BASEDIR, env=env, stderr=subprocess.PIPE)
+ scons: subprocess.Popen = subprocess.Popen(["scons", j_flag, "--cache-populate"], cwd=BASEDIR, env=env, stderr=subprocess.PIPE)
assert scons.stderr is not None
compile_output = []
diff --git a/selfdrive/manager/manager.py b/selfdrive/manager/manager.py
index b5fa439671..fca5020297 100755
--- a/selfdrive/manager/manager.py
+++ b/selfdrive/manager/manager.py
@@ -131,16 +131,15 @@ def manager_thread() -> None:
ensure_running(managed_processes.values(), started=False, not_run=ignore)
started_prev = False
- sm = messaging.SubMaster(['deviceState'])
+ sm = messaging.SubMaster(['deviceState', 'carParams'], poll=['deviceState'])
pm = messaging.PubMaster(['managerState'])
while True:
sm.update()
- not_run = ignore[:]
started = sm['deviceState'].started
driverview = params.get_bool("IsDriverViewEnabled")
- ensure_running(managed_processes.values(), started, driverview, not_run)
+ ensure_running(managed_processes.values(), started=started, driverview=driverview, notcar=sm['carParams'].notCar, not_run=ignore)
# trigger an update after going offroad
if started_prev and not started and 'updated' in managed_processes:
diff --git a/selfdrive/manager/process.py b/selfdrive/manager/process.py
index 4cd9783b2b..8bb160961f 100644
--- a/selfdrive/manager/process.py
+++ b/selfdrive/manager/process.py
@@ -69,14 +69,16 @@ class ManagerProcess(ABC):
unkillable = False
daemon = False
sigkill = False
- persistent = False
+ onroad = True
+ offroad = False
driverview = False
+ notcar = False
proc: Optional[Process] = None
enabled = True
name = ""
last_watchdog_time = 0
- watchdog_max_dt = None
+ watchdog_max_dt: Optional[int] = None
watchdog_seen = False
shutting_down = False
@@ -182,13 +184,15 @@ class ManagerProcess(ABC):
class NativeProcess(ManagerProcess):
- def __init__(self, name, cwd, cmdline, enabled=True, persistent=False, driverview=False, unkillable=False, sigkill=False, watchdog_max_dt=None):
+ def __init__(self, name, cwd, cmdline, enabled=True, onroad=True, offroad=False, driverview=False, notcar=False, unkillable=False, sigkill=False, watchdog_max_dt=None):
self.name = name
self.cwd = cwd
self.cmdline = cmdline
self.enabled = enabled
- self.persistent = persistent
+ self.onroad = onroad
+ self.offroad = offroad
self.driverview = driverview
+ self.notcar = notcar
self.unkillable = unkillable
self.sigkill = sigkill
self.watchdog_max_dt = watchdog_max_dt
@@ -213,12 +217,14 @@ class NativeProcess(ManagerProcess):
class PythonProcess(ManagerProcess):
- def __init__(self, name, module, enabled=True, persistent=False, driverview=False, unkillable=False, sigkill=False, watchdog_max_dt=None):
+ def __init__(self, name, module, enabled=True, onroad=True, offroad=False, driverview=False, notcar=False, unkillable=False, sigkill=False, watchdog_max_dt=None):
self.name = name
self.module = module
self.enabled = enabled
- self.persistent = persistent
+ self.onroad = onroad
+ self.offroad = offroad
self.driverview = driverview
+ self.notcar = notcar
self.unkillable = unkillable
self.sigkill = sigkill
self.watchdog_max_dt = watchdog_max_dt
@@ -251,7 +257,8 @@ class DaemonProcess(ManagerProcess):
self.module = module
self.param_name = param_name
self.enabled = enabled
- self.persistent = True
+ self.onroad = True
+ self.offroad = True
def prepare(self) -> None:
pass
@@ -284,23 +291,29 @@ class DaemonProcess(ManagerProcess):
pass
-def ensure_running(procs: ValuesView[ManagerProcess], started: bool, driverview: bool=False, not_run: Optional[List[str]]=None) -> None:
+def ensure_running(procs: ValuesView[ManagerProcess], started: bool, driverview: bool=False, notcar: bool=False,
+ not_run: Optional[List[str]]=None) -> None:
if not_run is None:
not_run = []
for p in procs:
- if p.name in not_run:
- p.stop(block=False)
- elif not p.enabled:
- p.stop(block=False)
- elif p.persistent:
- p.start()
- elif p.driverview and driverview:
- p.start()
- elif started:
+ # Conditions that make a process run
+ run = any((
+ p.offroad and not started,
+ p.onroad and started,
+ p.driverview and driverview,
+ p.notcar and notcar,
+ ))
+
+ # Conditions that block a process from starting
+ run = run and not any((
+ not p.enabled,
+ p.name in not_run,
+ ))
+
+ if run:
p.start()
else:
p.stop(block=False)
p.check_watchdog(started)
-
diff --git a/selfdrive/manager/process_config.py b/selfdrive/manager/process_config.py
index da4284571f..6568e1980e 100644
--- a/selfdrive/manager/process_config.py
+++ b/selfdrive/manager/process_config.py
@@ -1,6 +1,6 @@
import os
-from selfdrive.hardware import EON, TICI, PC
+from selfdrive.hardware import TICI, PC
from selfdrive.manager.process import PythonProcess, NativeProcess, DaemonProcess
WEBCAM = os.getenv("USE_WEBCAM") is not None
@@ -14,34 +14,32 @@ procs = [
NativeProcess("logcatd", "selfdrive/logcatd", ["./logcatd"]),
NativeProcess("loggerd", "selfdrive/loggerd", ["./loggerd"]),
NativeProcess("modeld", "selfdrive/modeld", ["./modeld"]),
- NativeProcess("navd", "selfdrive/ui/navd", ["./navd"], enabled=(PC or TICI), persistent=True),
+ NativeProcess("navd", "selfdrive/ui/navd", ["./navd"], offroad=True),
NativeProcess("proclogd", "selfdrive/proclogd", ["./proclogd"]),
- NativeProcess("sensord", "selfdrive/sensord", ["./sensord"], enabled=not PC, persistent=EON, sigkill=EON),
+ NativeProcess("sensord", "selfdrive/sensord", ["./sensord"], enabled=not PC),
NativeProcess("ubloxd", "selfdrive/locationd", ["./ubloxd"], enabled=(not PC or WEBCAM)),
- NativeProcess("ui", "selfdrive/ui", ["./ui"], persistent=True, watchdog_max_dt=(5 if TICI else None)),
- NativeProcess("soundd", "selfdrive/ui/soundd", ["./soundd"], persistent=True),
+ NativeProcess("ui", "selfdrive/ui", ["./ui"], offroad=True, watchdog_max_dt=(5 if TICI else None)),
+ NativeProcess("soundd", "selfdrive/ui/soundd", ["./soundd"], offroad=True),
NativeProcess("locationd", "selfdrive/locationd", ["./locationd"]),
NativeProcess("boardd", "selfdrive/boardd", ["./boardd"], enabled=False),
PythonProcess("calibrationd", "selfdrive.locationd.calibrationd"),
PythonProcess("controlsd", "selfdrive.controls.controlsd"),
- PythonProcess("deleter", "selfdrive.loggerd.deleter", persistent=True),
+ PythonProcess("deleter", "selfdrive.loggerd.deleter", offroad=True),
PythonProcess("dmonitoringd", "selfdrive.monitoring.dmonitoringd", enabled=(not PC or WEBCAM), driverview=True),
- PythonProcess("logmessaged", "selfdrive.logmessaged", persistent=True),
- PythonProcess("pandad", "selfdrive.boardd.pandad", persistent=True),
+ PythonProcess("logmessaged", "selfdrive.logmessaged", offroad=True),
+ PythonProcess("pandad", "selfdrive.boardd.pandad", offroad=True),
PythonProcess("paramsd", "selfdrive.locationd.paramsd"),
PythonProcess("plannerd", "selfdrive.controls.plannerd"),
PythonProcess("radard", "selfdrive.controls.radard"),
- PythonProcess("thermald", "selfdrive.thermald.thermald", persistent=True),
- PythonProcess("timezoned", "selfdrive.timezoned", enabled=TICI, persistent=True),
- PythonProcess("tombstoned", "selfdrive.tombstoned", enabled=not PC, persistent=True),
- PythonProcess("updated", "selfdrive.updated", enabled=not PC, persistent=True),
- PythonProcess("uploader", "selfdrive.loggerd.uploader", persistent=True),
- PythonProcess("statsd", "selfdrive.statsd", persistent=True),
+ PythonProcess("thermald", "selfdrive.thermald.thermald", offroad=True),
+ PythonProcess("timezoned", "selfdrive.timezoned", enabled=TICI, offroad=True),
+ PythonProcess("tombstoned", "selfdrive.tombstoned", enabled=not PC, offroad=True),
+ PythonProcess("updated", "selfdrive.updated", enabled=not PC, offroad=True),
+ PythonProcess("uploader", "selfdrive.loggerd.uploader", offroad=True),
+ PythonProcess("statsd", "selfdrive.statsd", offroad=True),
- # EON only
- PythonProcess("rtshield", "selfdrive.rtshield", enabled=EON),
- PythonProcess("shutdownd", "selfdrive.hardware.eon.shutdownd", enabled=EON),
- PythonProcess("androidd", "selfdrive.hardware.eon.androidd", enabled=EON, persistent=True),
+ NativeProcess("bridge", "cereal/messaging", ["./bridge"], onroad=False, notcar=True),
+ PythonProcess("webjoystick", "tools.joystick.web", onroad=False, notcar=True),
# Experimental
PythonProcess("rawgpsd", "selfdrive.sensord.rawgps.rawgpsd", enabled=os.path.isfile("/persist/comma/use-quectel-rawgps")),
diff --git a/selfdrive/manager/test/test_manager.py b/selfdrive/manager/test/test_manager.py
index d16a145031..1750c81b2e 100755
--- a/selfdrive/manager/test/test_manager.py
+++ b/selfdrive/manager/test/test_manager.py
@@ -5,14 +5,14 @@ import time
import unittest
import selfdrive.manager.manager as manager
-from selfdrive.hardware import EON, TICI, HARDWARE
+from selfdrive.hardware import TICI, HARDWARE
from selfdrive.manager.process import DaemonProcess
from selfdrive.manager.process_config import managed_processes
os.environ['FAKEUPLOAD'] = "1"
# TODO: make eon fast
-MAX_STARTUP_TIME = 30 if EON else 15
+MAX_STARTUP_TIME = 15
ALL_PROCESSES = [p.name for p in managed_processes.values() if (type(p) is not DaemonProcess) and p.enabled and (p.name not in ['updated', 'pandad'])]
@@ -50,7 +50,7 @@ class TestManager(unittest.TestCase):
self.assertTrue(state.running, f"{p} not running")
exit_code = managed_processes[p].stop(retry=False)
- if (TICI and p in ['ui', 'navd']) or (EON and p == 'logcatd'):
+ if (TICI and p in ['ui', 'navd']):
# TODO: make Qt UI exit gracefully
continue
diff --git a/selfdrive/modeld/SConscript b/selfdrive/modeld/SConscript
index f7ad6d1192..b7e2ea40c1 100644
--- a/selfdrive/modeld/SConscript
+++ b/selfdrive/modeld/SConscript
@@ -4,7 +4,7 @@ Import('env', 'arch', 'cereal', 'messaging', 'common', 'gpucommon', 'visionipc')
lenv = env.Clone()
libs = [cereal, messaging, common, visionipc, gpucommon,
- 'OpenCL', 'SNPE', 'symphony-cpu', 'capnp', 'zmq', 'kj', 'yuv']
+ 'OpenCL', 'SNPE', 'capnp', 'zmq', 'kj', 'yuv']
def get_dlsym_offset():
"""Returns the offset between dlopen and dlsym in libdl.so"""
@@ -31,9 +31,8 @@ thneed_src = [
use_thneed = not GetOption('no_thneed')
-if arch == "aarch64" or arch == "larch64":
- libs += ['gsl', 'CB', 'jpeg']
- libs += ['gnustl_shared'] if arch == "aarch64" else ['pthread', 'dl']
+if arch == "larch64":
+ libs += ['gsl', 'CB', 'pthread', 'dl', 'jpeg']
if use_thneed:
common_src += thneed_src
@@ -58,13 +57,12 @@ else:
# no SNPE on Mac
del libs[libs.index('SNPE')]
- del libs[libs.index('symphony-cpu')]
del common_src[common_src.index('runners/snpemodel.cc')]
common_model = lenv.Object(common_src)
# build thneed model
-if use_thneed and arch in ("aarch64", "larch64"):
+if use_thneed and arch == "larch64":
fn = File("#models/supercombo").abspath
compiler = lenv.Program('thneed/compile', ["thneed/compile.cc"]+common_model, LIBS=libs)
cmd = f"cd {Dir('.').abspath} && {compiler[0].abspath} {fn}.dlc {fn}_badweights.thneed --binary"
diff --git a/selfdrive/modeld/dmonitoringmodeld b/selfdrive/modeld/dmonitoringmodeld
index 9acd7bcc33..f292fe4c0b 100755
--- a/selfdrive/modeld/dmonitoringmodeld
+++ b/selfdrive/modeld/dmonitoringmodeld
@@ -3,15 +3,10 @@
DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" >/dev/null && pwd)"
cd $DIR
-if [ -d /system ]; then
- if [ -f /TICI ]; then # QCOM2
- export LD_LIBRARY_PATH="/usr/lib/aarch64-linux-gnu:/data/pythonpath/third_party/snpe/larch64:$LD_LIBRARY_PATH"
- else # QCOM
- export LD_LIBRARY_PATH="/data/pythonpath/third_party/snpe/aarch64/:$LD_LIBRARY_PATH"
- fi
+if [ -f /TICI ]; then
+ export LD_LIBRARY_PATH="/usr/lib/aarch64-linux-gnu:/data/pythonpath/third_party/snpe/larch64:$LD_LIBRARY_PATH"
export ADSP_LIBRARY_PATH="/data/pythonpath/third_party/snpe/dsp/"
else
- # PC
export LD_LIBRARY_PATH="$DIR/../../third_party/snpe/x86_64-linux-clang:$DIR/../../openpilot/third_party/snpe/x86_64:$LD_LIBRARY_PATH"
fi
exec ./_dmonitoringmodeld
diff --git a/selfdrive/modeld/modeld b/selfdrive/modeld/modeld
index fcf2812cb7..c067cf3d62 100755
--- a/selfdrive/modeld/modeld
+++ b/selfdrive/modeld/modeld
@@ -3,14 +3,9 @@
DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" >/dev/null && pwd)"
cd $DIR
-if [ -d /system ]; then
- if [ -f /TICI ]; then # QCOM2
- export LD_LIBRARY_PATH="/usr/lib/aarch64-linux-gnu:/data/pythonpath/third_party/snpe/larch64:$LD_LIBRARY_PATH"
- else # QCOM
- export LD_LIBRARY_PATH="/data/pythonpath/third_party/snpe/aarch64/:$LD_LIBRARY_PATH"
- fi
+if [ -f /TICI ]; then
+ export LD_LIBRARY_PATH="/usr/lib/aarch64-linux-gnu:/data/pythonpath/third_party/snpe/larch64:$LD_LIBRARY_PATH"
else
- # PC
export LD_LIBRARY_PATH="$DIR/../../third_party/snpe/x86_64-linux-clang:$DIR/../../openpilot/third_party/snpe/x86_64:$LD_LIBRARY_PATH"
fi
exec ./_modeld
diff --git a/selfdrive/modeld/modeld.cc b/selfdrive/modeld/modeld.cc
index 2714370106..639acd2d22 100644
--- a/selfdrive/modeld/modeld.cc
+++ b/selfdrive/modeld/modeld.cc
@@ -51,10 +51,6 @@ mat3 update_calibration(Eigen::Matrix &extrinsics, bool wide_camera
return matmul3(yuv_transform, transform);
}
-static uint64_t get_ts(const VisionIpcBufExtra &extra) {
- return Hardware::TICI() ? extra.timestamp_sof : extra.timestamp_eof;
-}
-
void run_model(ModelState &model, VisionIpcClient &vipc_client_main, VisionIpcClient &vipc_client_extra, bool main_wide_camera, bool use_extra_client) {
// messaging
@@ -80,7 +76,7 @@ void run_model(ModelState &model, VisionIpcClient &vipc_client_main, VisionIpcCl
while (!do_exit) {
// Keep receiving frames until we are at least 1 frame ahead of previous extra frame
- while (get_ts(meta_main) < get_ts(meta_extra) + 25000000ULL) {
+ while (meta_main.timestamp_sof < meta_extra.timestamp_sof + 25000000ULL) {
buf_main = vipc_client_main.recv(&meta_main);
if (buf_main == nullptr) break;
}
@@ -94,7 +90,7 @@ void run_model(ModelState &model, VisionIpcClient &vipc_client_main, VisionIpcCl
// Keep receiving extra frames until frame id matches main camera
do {
buf_extra = vipc_client_extra.recv(&meta_extra);
- } while (buf_extra != nullptr && get_ts(meta_main) > get_ts(meta_extra) + 25000000ULL);
+ } while (buf_extra != nullptr && meta_main.timestamp_sof > meta_extra.timestamp_sof + 25000000ULL);
if (buf_extra == nullptr) {
LOGE("vipc_client_extra no frame");
@@ -124,7 +120,7 @@ void run_model(ModelState &model, VisionIpcClient &vipc_client_main, VisionIpcCl
}
model_transform_main = update_calibration(extrinsic_matrix_eigen, main_wide_camera, false);
- model_transform_extra = update_calibration(extrinsic_matrix_eigen, Hardware::TICI(), true);
+ model_transform_extra = update_calibration(extrinsic_matrix_eigen, true, true);
live_calib_seen = true;
}
@@ -160,16 +156,16 @@ void run_model(ModelState &model, VisionIpcClient &vipc_client_main, VisionIpcCl
}
int main(int argc, char **argv) {
- if (!Hardware::PC()) {
+ if (Hardware::TICI()) {
int ret;
ret = util::set_realtime_priority(54);
assert(ret == 0);
- util::set_core_affinity({Hardware::EON() ? 2 : 7});
+ util::set_core_affinity({7});
assert(ret == 0);
}
bool main_wide_camera = Hardware::TICI() ? Params().getBool("EnableWideCamera") : false;
- bool use_extra_client = Hardware::TICI() && !main_wide_camera;
+ bool use_extra_client = !main_wide_camera;
// cl init
cl_device_id device_id = cl_get_device_id(CL_DEVICE_TYPE_DEFAULT);
diff --git a/selfdrive/modeld/runners/snpemodel.cc b/selfdrive/modeld/runners/snpemodel.cc
index 0d394ca511..44fa6ce298 100644
--- a/selfdrive/modeld/runners/snpemodel.cc
+++ b/selfdrive/modeld/runners/snpemodel.cc
@@ -18,7 +18,7 @@ SNPEModel::SNPEModel(const char *path, float *loutput, size_t loutput_size, int
output = loutput;
output_size = loutput_size;
use_extra = luse_extra;
-#if defined(QCOM) || defined(QCOM2)
+#ifdef QCOM2
if (runtime==USE_GPU_RUNTIME) {
Runtime = zdl::DlSystem::Runtime_t::GPU;
} else if (runtime==USE_DSP_RUNTIME) {
@@ -39,7 +39,7 @@ SNPEModel::SNPEModel(const char *path, float *loutput, size_t loutput_size, int
// create model runner
zdl::SNPE::SNPEBuilder snpeBuilder(container.get());
while (!snpe) {
-#if defined(QCOM) || defined(QCOM2)
+#ifdef QCOM2
snpe = snpeBuilder.setOutputLayers({})
.setRuntimeProcessor(Runtime)
.setUseUserSuppliedBuffers(true)
diff --git a/selfdrive/modeld/runners/snpemodel.h b/selfdrive/modeld/runners/snpemodel.h
index 584c636802..6e9c33f89c 100644
--- a/selfdrive/modeld/runners/snpemodel.h
+++ b/selfdrive/modeld/runners/snpemodel.h
@@ -38,7 +38,7 @@ public:
private:
std::string model_data;
-#if defined(QCOM) || defined(QCOM2)
+#ifdef QCOM2
zdl::DlSystem::Runtime_t Runtime;
#endif
diff --git a/selfdrive/modeld/test/test_modeld.py b/selfdrive/modeld/test/test_modeld.py
new file mode 100755
index 0000000000..93ad0f5608
--- /dev/null
+++ b/selfdrive/modeld/test/test_modeld.py
@@ -0,0 +1,72 @@
+#!/usr/bin/env python3
+import time
+import unittest
+import numpy as np
+
+import cereal.messaging as messaging
+from cereal.visionipc.visionipc_pyx import VisionIpcServer, VisionStreamType # pylint: disable=no-name-in-module, import-error
+from common.transformations.camera import tici_f_frame_size
+from common.realtime import DT_MDL
+from selfdrive.manager.process_config import managed_processes
+
+
+VIPC_STREAM = {"roadCameraState": VisionStreamType.VISION_STREAM_ROAD, "driverCameraState": VisionStreamType.VISION_STREAM_DRIVER,
+ "wideRoadCameraState": VisionStreamType.VISION_STREAM_WIDE_ROAD}
+
+IMG = np.zeros(int(tici_f_frame_size[0]*tici_f_frame_size[1]*(3/2)), dtype=np.uint8)
+IMG_BYTES = IMG.flatten().tobytes()
+
+class TestModeld(unittest.TestCase):
+
+ def setUp(self):
+ self.vipc_server = VisionIpcServer("camerad")
+ self.vipc_server.create_buffers(VisionStreamType.VISION_STREAM_ROAD, 40, False, *tici_f_frame_size)
+ self.vipc_server.create_buffers(VisionStreamType.VISION_STREAM_DRIVER, 40, False, *tici_f_frame_size)
+ self.vipc_server.create_buffers(VisionStreamType.VISION_STREAM_WIDE_ROAD, 40, False, *tici_f_frame_size)
+ self.vipc_server.start_listener()
+
+ self.sm = messaging.SubMaster(['modelV2', 'cameraOdometry'])
+ self.pm = messaging.PubMaster(['roadCameraState', 'wideRoadCameraState', 'driverCameraState', 'liveCalibration', 'lateralPlan'])
+
+ managed_processes['modeld'].start()
+ time.sleep(0.2)
+ self.sm.update(1000)
+
+ def tearDown(self):
+ managed_processes['modeld'].stop()
+ del self.vipc_server
+
+ def test_modeld(self):
+ for n in range(1, 500):
+ for cam in ('roadCameraState', 'wideRoadCameraState'):
+ msg = messaging.new_message(cam)
+ cs = getattr(msg, cam)
+ cs.frameId = n
+ cs.timestampSof = int((n * DT_MDL) * 1e9)
+ cs.timestampEof = int(cs.timestampSof + (DT_MDL * 1e9))
+
+ self.pm.send(msg.which(), msg)
+ self.vipc_server.send(VIPC_STREAM[msg.which()], IMG_BYTES, cs.frameId,
+ cs.timestampSof, cs.timestampEof)
+
+ self.sm.update(5000)
+ if self.sm['modelV2'].frameId != self.sm['cameraOdometry'].frameId:
+ self.sm.update(1000)
+
+ mdl = self.sm['modelV2']
+ self.assertEqual(mdl.frameId, n)
+ self.assertEqual(mdl.frameIdExtra, n)
+ self.assertEqual(mdl.timestampEof, cs.timestampEof)
+ self.assertEqual(mdl.frameAge, 0)
+ self.assertEqual(mdl.frameDropPerc, 0)
+
+ odo = self.sm['cameraOdometry']
+ self.assertEqual(odo.frameId, n)
+ self.assertEqual(odo.timestampEof, cs.timestampEof)
+
+ def test_skipped_frames(self):
+ pass
+
+
+if __name__ == "__main__":
+ unittest.main()
diff --git a/selfdrive/modeld/thneed/thneed.cc b/selfdrive/modeld/thneed/thneed.cc
index e2dc9b72f2..470ff219b0 100644
--- a/selfdrive/modeld/thneed/thneed.cc
+++ b/selfdrive/modeld/thneed/thneed.cc
@@ -433,7 +433,7 @@ cl_program thneed_clCreateProgramWithSource(cl_context context, cl_uint count, c
}
void *dlsym(void *handle, const char *symbol) {
-#if defined(QCOM) || defined(QCOM2)
+#ifdef QCOM2
void *(*my_dlsym)(void *handle, const char *symbol) = (void *(*)(void *handle, const char *symbol))((uintptr_t)dlopen + DLSYM_OFFSET);
#else
#error "Unsupported platform for thneed"
diff --git a/selfdrive/modeld/visiontest.c b/selfdrive/modeld/visiontest.c
deleted file mode 100644
index fcc3539040..0000000000
--- a/selfdrive/modeld/visiontest.c
+++ /dev/null
@@ -1,111 +0,0 @@
-#include
-#include
-#include
-
-#define CL_USE_DEPRECATED_OPENCL_1_2_APIS
-#ifdef __APPLE__
-#include
-#else
-#include
-#endif
-
-#include "selfdrive/common/clutil.h"
-#include "selfdrive/modeld/transforms/transform.h"
-
-typedef struct {
- int disable_model;
- Transform transform;
-
- int in_width;
- int in_height;
- int out_width;
- int out_height;
-
- cl_context context;
- cl_command_queue command_queue;
- cl_device_id device_id;
-
- size_t in_yuv_size;
- cl_mem in_yuv_cl;
-
- cl_mem out_y_cl, out_u_cl, out_v_cl;
-} VisionTest;
-
-void initialize_opencl(VisionTest* visiontest) {
- // init cl
- cl_device_id device_id = cl_get_device_id(CL_DEVICE_TYPE_CPU);
- visiontest->context = CL_CHECK_ERR(clCreateContext(NULL, 1, &device_id, NULL, NULL, &err));
- visiontest->device_id = device_id;
-}
-
-VisionTest* visiontest_create(int temporal_model, int disable_model,
- int input_width, int input_height,
- int model_input_width, int model_input_height) {
- VisionTest* const vt = calloc(1, sizeof(*vt));
- assert(vt);
-
- vt->disable_model = disable_model;
- vt->in_width = input_width;
- vt->in_height = input_height;
- vt->out_width = model_input_width;
- vt->out_height = model_input_height;
-
- initialize_opencl(vt);
-
- transform_init(&vt->transform, vt->context, vt->device_id);
-
-
- assert((vt->in_width%2) == 0 && (vt->in_height%2) == 0);
- vt->in_yuv_size = vt->in_width*vt->in_height*3/2;
- vt->in_yuv_cl = CL_CHECK_ERR(clCreateBuffer(vt->context, CL_MEM_READ_WRITE,
- vt->in_yuv_size, NULL, &err));
- vt->out_y_cl = CL_CHECK_ERR(clCreateBuffer(vt->context, CL_MEM_READ_WRITE,
- vt->out_width*vt->out_width, NULL, &err));
- vt->out_u_cl = CL_CHECK_ERR(clCreateBuffer(vt->context, CL_MEM_READ_WRITE,
- vt->out_width*vt->out_width/4, NULL, &err));
- vt->out_v_cl = CL_CHECK_ERR(clCreateBuffer(vt->context, CL_MEM_READ_WRITE,
- vt->out_width*vt->out_width/4, NULL, &err));
- vt->command_queue = CL_CHECK_ERR(clCreateCommandQueue(vt->context, vt->device_id, 0, &err));
- return vt;
-}
-
-void visiontest_destroy(VisionTest* vt) {
- transform_destroy(&vt->transform);
-
- CL_CHECK(clReleaseMemObject(vt->in_yuv_cl));
- CL_CHECK(clReleaseMemObject(vt->out_y_cl));
- CL_CHECK(clReleaseMemObject(vt->out_u_cl));
- CL_CHECK(clReleaseMemObject(vt->out_v_cl));
- CL_CHECK(clReleaseCommandQueue(vt->command_queue));
- CL_CHECK(clReleaseContext(vt->context));
-
- free(vt);
-}
-
-void visiontest_transform(VisionTest* vt, const uint8_t* yuv_data,
- uint8_t* out_y, uint8_t* out_u, uint8_t* out_v,
- const float* transform) {
- CL_CHECK(clEnqueueWriteBuffer(vt->command_queue, vt->in_yuv_cl, CL_FALSE,
- 0, vt->in_yuv_size, yuv_data, 0, NULL, NULL));
-
- mat3 transform_m = *(const mat3*)transform;
-
- transform_queue(&vt->transform, vt->command_queue,
- vt->in_yuv_cl, vt->in_width, vt->in_height,
- vt->out_y_cl, vt->out_u_cl, vt->out_v_cl,
- vt->out_width, vt->out_height,
- transform_m);
-
- CL_CHECK(clEnqueueReadBuffer(vt->command_queue, vt->out_y_cl, CL_FALSE,
- 0, vt->out_width*vt->out_height, out_y,
- 0, NULL, NULL));
- CL_CHECK(clEnqueueReadBuffer(vt->command_queue, vt->out_u_cl, CL_FALSE,
- 0, vt->out_width*vt->out_height/4, out_u,
- 0, NULL, NULL));
- CL_CHECK(clEnqueueReadBuffer(vt->command_queue, vt->out_v_cl, CL_FALSE,
- 0, vt->out_width*vt->out_height/4, out_v,
- 0, NULL, NULL));
-
- clFinish(vt->command_queue);
-}
-
diff --git a/selfdrive/modeld/visiontest.mk b/selfdrive/modeld/visiontest.mk
deleted file mode 100644
index 1df504e93a..0000000000
--- a/selfdrive/modeld/visiontest.mk
+++ /dev/null
@@ -1,104 +0,0 @@
-CC:=clang
-CXX:=clang++
-OPT_FLAGS:=-O2 -g -ggdb3
-
-UNAME_S := $(shell uname -s)
-ifeq ($(UNAME_S),Linux)
- SHARED_FLAGS=-Wl,--whole-archive $^ -Wl,--no-whole-archive
-endif
-ifeq ($(UNAME_S),Darwin)
- SHARED_FLAGS=-Wl,-force_load $^
-endif
-
-PHONELIBS := ../../third_party
-BASEDIR := ../..
-
-WARN_FLAGS = -Werror=implicit-function-declaration \
- -Werror=incompatible-pointer-types \
- -Werror=int-conversion \
- -Werror=return-type \
- -Werror=format-extra-args
-
-CFLAGS = -std=gnu11 -g -fPIC $(OPT_FLAGS) $(WARN_FLAGS)
-CXXFLAGS = -std=c++1z -fPIC $(OPT_FLAGS) $(WARN_FLAGS)
-
-EIGEN_FLAGS = -I$(PHONELIBS)/eigen
-
-CEREAL_LIBS = $(BASEDIR)/cereal/libmessaging.a
-
-OPENCV_LIBS = -lopencv_video -lopencv_core -lopencv_imgproc
-
-ifeq ($(UNAME_S),Darwin)
- VT_LDFLAGS += $(PHONELIBS)/capnp-c/mac/lib/libcapnp_c.a \
- $(PHONELIBS)/zmq/mac/lib/libzmq.a \
- -framework OpenCL
-
- OPENCV_LIBS += -L/usr/local/opt/opencv@2/lib
- OPENCV_FLAGS += -I/usr/local/opt/opencv@2/include
-
-else
- VT_LDFLAGS += $(CEREAL_LIBS) \
- -L/system/vendor/lib64 \
- -L$(BASEDIR)/external/zmq/lib/ \
- -l:libzmq.a \
- -lOpenCL
-endif
-
-.PHONY: all visiontest clean test
-all: visiontest
-
-libvisiontest_inputs := visiontest.c \
- transforms/transform.cc \
- transforms/loadyuv.cc \
- ../common/clutil.cc \
- $(BASEDIR)/selfdrive/common/util.c \
- $(CEREAL_OBJS)
-
-visiontest: libvisiontest.so
-all-tests := $(addsuffix .test, $(basename $(wildcard test_*)))
-
-%.o: %.cc
- @echo "[ CXX ] $@"
- $(CXX) $(CXXFLAGS) -MMD \
- -I. -I.. -I../.. \
- -Wall \
- -I$(BASEDIR)/ -I$(BASEDIR)/selfdrive -I$(BASEDIR)/selfdrive/common \
- $(EIGEN_FLAGS) \
- $(OPENCV_FLAGS) \
- $(CEREAL_CXXFLAGS) \
- -c -o '$@' '$<'
-
-%.o: %.c
- @echo "[ CXX ] $@"
- $(CC) $(CFLAGS) -MMD \
- -I. -I.. -I../.. \
- -Wall \
- -I$(BASEDIR)/ -I$(BASEDIR)/selfdrive -I$(BASEDIR)/selfdrive/common \
- $(CEREAL_CFLAGS) \
- -c -o '$@' '$<'
-
-libvisiontest.so: $(libvisiontest_inputs)
- $(eval $@_TMP := $(shell mktemp))
- $(CC) -std=gnu11 -shared -fPIC -O2 -g \
- -Werror=implicit-function-declaration -Werror=incompatible-pointer-types \
- -Werror=int-conversion -Wno-pointer-to-int-cast \
- -I. \
- $^ -o $($@_TMP) \
- -I$(PHONELIBS)/opencl/include \
- -I$(BASEDIR)/selfdrive/common \
- $(CEREAL_CXXFLAGS) \
- $(CEREAL_CFLAGS) \
- -I$(BASEDIR)/external/zmq/include \
- -I$(BASEDIR)/ -I$(BASEDIR)/selfdrive \
- -lstdc++ \
- $(VT_LDFLAGS) \
- -lm -lpthread
- mv $($@_TMP) $@
-
-test : $(all-tests)
-
-test_%.test : test_%
- @./'$<' || echo FAIL
-
-clean:
- rm -rf *.o *.so *.a
diff --git a/selfdrive/modeld/visiontest.py b/selfdrive/modeld/visiontest.py
deleted file mode 100644
index 1e30c05449..0000000000
--- a/selfdrive/modeld/visiontest.py
+++ /dev/null
@@ -1,135 +0,0 @@
-import os
-import subprocess
-from cffi import FFI
-from common.basedir import BASEDIR
-
-# Initialize visiontest. Ignore output.
-_visiond_dir = os.path.dirname(os.path.abspath(__file__))
-_libvisiontest = "libvisiontest.so"
-try: # because this crashes sometimes when running pipeline
- subprocess.check_output(["make", "-C", _visiond_dir, "-f",
- os.path.join(_visiond_dir, "visiontest.mk"),
- _libvisiontest])
-except Exception:
- pass
-
-
-class VisionTest():
- """A version of the vision model that can be run on a desktop.
-
- WARNING: This class is not thread safe. VisionTest objects cannot be
- created or used on multiple threads simultaneously.
- """
-
- ffi = FFI()
- ffi.cdef("""
- typedef unsigned char uint8_t;
-
- struct VisionTest;
- typedef struct VisionTest VisionTest;
-
- VisionTest* visiontest_create(int temporal_model, int disable_model,
- int input_width, int input_height,
- int model_input_width, int model_input_height);
- void visiontest_destroy(VisionTest* visiontest);
-
- void visiontest_transform(VisionTest* vt, const uint8_t* yuv_data,
- uint8_t* out_y, uint8_t* out_u, uint8_t* out_v,
- const float* transform);
- """)
-
- clib = ffi.dlopen(os.path.join(_visiond_dir, _libvisiontest))
-
- def __init__(self, input_size, model_input_size, model):
- """Create a wrapper around visiond for off-device python code.
-
- Inputs:
- input_size: The size of YUV images passed to transform.
- model_input_size: The size of YUV images passed to the model.
- model: The name of the model to use. "temporal", "yuv", or None to disable the
- model (used to disable OpenCL).
- """
- self._input_size = input_size
- self._model_input_size = model_input_size
-
- if model is None:
- disable_model = 1
- temporal_model = 0
- elif model == "yuv":
- disable_model = 0
- temporal_model = 0
- elif model == "temporal":
- disable_model = 0
- temporal_model = 1
- else:
- raise ValueError(f"Bad model name: {model}")
-
- prevdir = os.getcwd()
- os.chdir(_visiond_dir) # tmp hack to find kernels
- os.environ['BASEDIR'] = BASEDIR
- self._visiontest_c = self.clib.visiontest_create(
- temporal_model, disable_model, self._input_size[0], self._input_size[1],
- self._model_input_size[0], self._model_input_size[1])
- os.chdir(prevdir)
-
- @property
- def input_size(self):
- return self._input_size
-
- @property
- def model_input_size(self):
- return self._model_input_size
-
- def transform(self, yuv_data, transform):
- y_len = self.model_input_size[0] * self.model_input_size[1]
- t_y_ptr = bytearray(y_len)
- t_u_ptr = bytearray(y_len // 4)
- t_v_ptr = bytearray(y_len // 4)
-
- self.transform_output_buffer(yuv_data, t_y_ptr, t_u_ptr, t_v_ptr,
- transform)
-
- return t_y_ptr, t_u_ptr, t_v_ptr
-
- def transform_contiguous(self, yuv_data, transform):
- y_ol = self.model_input_size[0] * self.model_input_size[1]
- uv_ol = y_ol // 4
- result = bytearray(y_ol * 3 // 2)
- result_view = memoryview(result)
- t_y_ptr = result_view[:y_ol]
- t_u_ptr = result_view[y_ol:y_ol + uv_ol]
- t_v_ptr = result_view[y_ol + uv_ol:]
-
- self.transform_output_buffer(yuv_data, t_y_ptr, t_u_ptr, t_v_ptr,
- transform)
- return result
-
- def transform_output_buffer(self, yuv_data, y_out, u_out, v_out,
- transform):
- assert len(yuv_data) == self.input_size[0] * self.input_size[1] * 3 / 2
-
- cast = self.ffi.cast
- from_buffer = self.ffi.from_buffer
- yuv_ptr = cast("unsigned char*", from_buffer(yuv_data))
- transform_ptr = self.ffi.new("float[]", transform)
-
- y_out_ptr = cast("unsigned char*", from_buffer(y_out))
- u_out_ptr = cast("unsigned char*", from_buffer(u_out))
- v_out_ptr = cast("unsigned char*", from_buffer(v_out))
-
- self.clib.visiontest_transform(self._visiontest_c, yuv_ptr, y_out_ptr,
- u_out_ptr, v_out_ptr, transform_ptr)
-
- def close(self):
- self.clib.visiontest_destroy(self._visiontest_c)
- self._visiontest_c = None
-
- def __enter__(self):
- return self
-
- def __exit__(self, exc_type, exc_value, traceback):
- self.close()
-
-
-if __name__ == "__main__":
- VisionTest((560, 304), (320, 160), "temporal")
diff --git a/selfdrive/rtshield.py b/selfdrive/rtshield.py
index ad7538d7a8..45571fe2db 100755
--- a/selfdrive/rtshield.py
+++ b/selfdrive/rtshield.py
@@ -11,7 +11,7 @@ from common.realtime import set_core_affinity, set_realtime_priority
# openpilot processes
def main() -> NoReturn:
- set_core_affinity(int(os.getenv("CORE", "3")))
+ set_core_affinity([int(os.getenv("CORE", "3")), ])
set_realtime_priority(1)
while True:
diff --git a/selfdrive/sensord/SConscript b/selfdrive/sensord/SConscript
index 186e302bc3..db32887e7f 100644
--- a/selfdrive/sensord/SConscript
+++ b/selfdrive/sensord/SConscript
@@ -1,22 +1,19 @@
Import('env', 'arch', 'common', 'cereal', 'messaging')
-if arch == "aarch64":
- env.Program('_sensord', 'sensors_qcom.cc', LIBS=['hardware', common, cereal, messaging, 'capnp', 'zmq', 'kj'])
-else:
- sensors = [
- 'sensors/file_sensor.cc',
- 'sensors/i2c_sensor.cc',
- 'sensors/light_sensor.cc',
- 'sensors/bmx055_accel.cc',
- 'sensors/bmx055_gyro.cc',
- 'sensors/bmx055_magn.cc',
- 'sensors/bmx055_temp.cc',
- 'sensors/lsm6ds3_accel.cc',
- 'sensors/lsm6ds3_gyro.cc',
- 'sensors/lsm6ds3_temp.cc',
- 'sensors/mmc5603nj_magn.cc',
- ]
- libs = [common, cereal, messaging, 'capnp', 'zmq', 'kj']
- if arch == "larch64":
- libs.append('i2c')
- env.Program('_sensord', ['sensors_qcom2.cc'] + sensors, LIBS=libs)
+sensors = [
+ 'sensors/file_sensor.cc',
+ 'sensors/i2c_sensor.cc',
+ 'sensors/light_sensor.cc',
+ 'sensors/bmx055_accel.cc',
+ 'sensors/bmx055_gyro.cc',
+ 'sensors/bmx055_magn.cc',
+ 'sensors/bmx055_temp.cc',
+ 'sensors/lsm6ds3_accel.cc',
+ 'sensors/lsm6ds3_gyro.cc',
+ 'sensors/lsm6ds3_temp.cc',
+ 'sensors/mmc5603nj_magn.cc',
+]
+libs = [common, cereal, messaging, 'capnp', 'zmq', 'kj']
+if arch == "larch64":
+ libs.append('i2c')
+env.Program('_sensord', ['sensors_qcom2.cc'] + sensors, LIBS=libs)
diff --git a/selfdrive/sensord/sensors_qcom.cc b/selfdrive/sensord/sensors_qcom.cc
deleted file mode 100644
index fcdb9a72d7..0000000000
--- a/selfdrive/sensord/sensors_qcom.cc
+++ /dev/null
@@ -1,228 +0,0 @@
-#include
-#include
-#include
-#include
-#include
-#include
-#include
-#include
-
-#include
-#include
-#include
-#include
-#include
-#include