diff --git a/.github/workflows/selfdrive_tests.yaml b/.github/workflows/selfdrive_tests.yaml
index 03dd4f4686413a..752a5ec3572e07 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 3fc559656820fb..dd53a4200c24b1 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 2ea26b854f4a90..c5a024db820d09 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 fcfc17a86fe127..aa73ccfc4952c1 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,10 +116,11 @@ pipeline {
}
}
- stage('C3: HW + Unit Tests') {
+ stage('HW + Unit Tests') {
steps {
phone_steps("tici2", [
["build", "cd selfdrive/manager && ./build.py"],
+ ["test power draw", "python selfdrive/hardware/tici/test_power_draw.py"],
["test boardd loopback", "python selfdrive/boardd/tests/test_boardd_loopback.py"],
["test loggerd", "python selfdrive/loggerd/tests/test_loggerd.py"],
["test encoder", "LD_LIBRARY_PATH=/usr/local/lib python selfdrive/loggerd/tests/test_encoder.py"],
@@ -188,17 +128,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 +138,7 @@ pipeline {
}
}
- stage('C3: replay') {
+ stage('replay') {
steps {
phone_steps("tici3", [
["build", "cd selfdrive/manager && ./build.py"],
@@ -225,7 +155,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 02e8c602d19719..6d30179b9deb35 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 da2dae92aa929f..2c62e13dfaee7e 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 b012883d78428d..5af02de8d9277d 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 304df61cba30ec..8d8ee06d632fab 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 6b520bee413f1f..ec76448235d461 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 a91dfedc0d11b2..29f4fe89ef710f 160000
--- a/cereal
+++ b/cereal
@@ -1 +1 @@
-Subproject commit a91dfedc0d11b27623541c32e6321d51c6a7fb35
+Subproject commit 29f4fe89ef710ff86a5aeb998a357187d0619fb8
diff --git a/common/file_helpers.py b/common/file_helpers.py
index 592b2a199a75e5..8a45fa313c455b 100644
--- a/common/file_helpers.py
+++ b/common/file_helpers.py
@@ -81,25 +81,12 @@ def _get_fileobject():
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 e05eb8f2c36a3c..4a37efadabff63 100644
--- a/common/realtime.py
+++ b/common/realtime.py
@@ -2,7 +2,7 @@
import gc
import os
import time
-from typing import Optional
+from typing import Optional, List, Union
from setproctitle import getproctitle # pylint: disable=no-name-in-module
@@ -38,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:
diff --git a/docs/CARS.md b/docs/CARS.md
index dc24560ad45a4d..1c134b38690533 100644
--- a/docs/CARS.md
+++ b/docs/CARS.md
@@ -81,12 +81,12 @@ 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||||||
|Hyundai|Kona 2020|SCC + LKAS||||||
-|Hyundai|Kona Electric 2018-19|SCC + LKAS||||||
+|Hyundai|Kona Electric 2018-21|SCC + LKAS||||||
|Hyundai|Kona Hybrid 2020|SCC + LKAS||||||
|Hyundai|Santa Fe 2021-22|All||||||
|Hyundai|Santa Fe Hybrid 2022|All||||||
@@ -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 8bf40708a64510..00000000000000
--- 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 29f58ad00243c7..503ebb5d41662b 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
@@ -152,7 +62,6 @@ function launch {
cd $BASEDIR
echo "Restarting launch script ${LAUNCHER_LOCATION}"
- unset REQUIRED_NEOS_VERSION
unset AGNOS_VERSION
exec "${LAUNCHER_LOCATION}"
else
@@ -168,9 +77,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 cd0c27f64336b8..d4b57c909af912 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 35cf5c9e5dbaeb..e19ba095c3ee28 160000
--- a/opendbc
+++ b/opendbc
@@ -1 +1 @@
-Subproject commit 35cf5c9e5dbaeb8b8a36c65a396036fa7d4d1055
+Subproject commit e19ba095c3ee288d629284e24ec7e0deaf645f3f
diff --git a/panda b/panda
index c925407461e46a..7dd9493eb102ba 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 95fcfea1a1bbac..79ab4fb0876982 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 ff7669084e8937..8c1214f73d4be9 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
diff --git a/release/files_eon b/release/files_eon
deleted file mode 100644
index b43bf86b50fd8d..00000000000000
--- a/release/files_eon
+++ /dev/null
@@ -1 +0,0 @@
-README.md
diff --git a/release/files_tici b/release/files_tici
index 59cc41918ff983..68e075c97170be 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 00000000000000..0801938e71bee1
--- /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 71a18aff73d49a..919cf0c04ec47c 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 10f5d46f77f2ba..cc4bd7e6471863 100755
--- a/selfdrive/athena/registration.py
+++ b/selfdrive/athena/registration.py
@@ -3,6 +3,7 @@
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 cbd0609f0dc724..5b44f4d60bca52 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 7b4e81f9ae0d1a..394437902f6da8 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 294553b413dafc..9bbeee86b4ec48 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 85bc756bc1319c..bb55c825d48a59 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 7c4d2748f16c08..c37846870edba5 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 @@ class Debayer {
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 @@ class Debayer {
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 6fafa0e59dced3..e415f809821d2a 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 411ff0aec4ca3b..00000000000000
--- 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 87bbe4b7e72618..00000000000000
--- 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 291dd261d31cb5..8986351fc10a7f 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 4e4b832203d3de..00000000000000
--- 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 668410d6f7c2ae..58edaedd9bb692 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 ff37ad1f311936..c311c17169ac57 100755
--- a/selfdrive/camerad/test/test_camerad.py
+++ b/selfdrive/camerad/test/test_camerad.py
@@ -4,11 +4,9 @@
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 @@
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 6ed69627b50d27..31bcc286819f63 100755
--- a/selfdrive/camerad/test/test_exposure.py
+++ b/selfdrive/camerad/test/test_exposure.py
@@ -6,7 +6,7 @@
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 @@
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/honda/carstate.py b/selfdrive/car/honda/carstate.py
index 31a641e90d8b9c..5314fe375ebe44 100644
--- a/selfdrive/car/honda/carstate.py
+++ b/selfdrive/car/honda/carstate.py
@@ -181,18 +181,14 @@ def update(self, cp, cp_cam, cp_body):
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/hyundai/interface.py b/selfdrive/car/hyundai/interface.py
index ba74c23e4731eb..00cd4507f9ddfa 100644
--- a/selfdrive/car/hyundai/interface.py
+++ b/selfdrive/car/hyundai/interface.py
@@ -138,6 +138,20 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=[], disabl
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
diff --git a/selfdrive/car/hyundai/values.py b/selfdrive/car/hyundai/values.py
index 0263daee3ae2b4..1a20ab6d6bd723 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,16 +84,17 @@ 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"),
- CAR.KONA_HEV: HyundaiCarInfo("Hyundai Kona Hybrid 2020", video_link="https://youtu.be/_EdYQtV52-c"),
+ CAR.KONA_EV: HyundaiCarInfo("Hyundai Kona Electric 2018-21"),
+ CAR.KONA_HEV: HyundaiCarInfo("Hyundai Kona Hybrid 2020"),
CAR.SANTA_FE: HyundaiCarInfo("Hyundai Santa Fe 2019-20", "All"),
CAR.SANTA_FE_2022: HyundaiCarInfo("Hyundai Santa Fe 2021-22", "All"),
CAR.SANTA_FE_HEV_2022: HyundaiCarInfo("Hyundai Santa Fe Hybrid 2022", "All"),
@@ -242,6 +244,24 @@ class Buttons:
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 @@ class Buttons:
},
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 @@ class Buttons:
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 @@ class Buttons:
# 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 @@ class Buttons:
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/subaru/values.py b/selfdrive/car/subaru/values.py
index 1c524747d3e6d0..76f7e896602daa 100644
--- a/selfdrive/car/subaru/values.py
+++ b/selfdrive/car/subaru/values.py
@@ -40,7 +40,7 @@ class SubaruCarInfo(CarInfo):
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"),
diff --git a/selfdrive/car/tests/routes.py b/selfdrive/car/tests/routes.py
index 5d99c731fee74b..a26d6dcd0400e4 100644
--- a/selfdrive/car/tests/routes.py
+++ b/selfdrive/car/tests/routes.py
@@ -48,8 +48,6 @@
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),
@@ -89,6 +87,7 @@
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),
@@ -206,4 +205,11 @@
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 2f4984c3d9ad28..d97b16e682147a 100755
--- a/selfdrive/car/tests/test_car_interfaces.py
+++ b/selfdrive/car/tests/test_car_interfaces.py
@@ -38,8 +38,8 @@ def test_car_interfaces(self, car_name):
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 7e02fd2a07e6a2..436861992659e2 100755
--- a/selfdrive/car/tests/test_models.py
+++ b/selfdrive/car/tests/test_models.py
@@ -115,8 +115,8 @@ def test_car_params(self):
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 @@ def test_panda_safety_carstate(self):
# 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 @@ def test_panda_safety_carstate(self):
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 @@ def test_panda_safety_carstate(self):
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 c0868f3288218e..1c01703d7ee2f6 100644
--- a/selfdrive/car/toyota/carcontroller.py
+++ b/selfdrive/car/toyota/carcontroller.py
@@ -14,6 +14,7 @@
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 @@ def update(self, CC, CS):
# 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 a9519555050d93..dea4263d7ebf04 100644
--- a/selfdrive/car/toyota/carstate.py
+++ b/selfdrive/car/toyota/carstate.py
@@ -36,8 +36,8 @@ def update(self, cp, cp_cam):
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 f7d5aba7abfc40..c20110b8b55e67 100644
--- a/selfdrive/car/toyota/interface.py
+++ b/selfdrive/car/toyota/interface.py
@@ -34,7 +34,6 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=[], disabl
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 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=[], disabl
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 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=[], disabl
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 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=[], disabl
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
diff --git a/selfdrive/car/toyota/tunes.py b/selfdrive/car/toyota/tunes.py
index f26fc72a092bb9..8ea1d6547f31c3 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 da4161195b6dd1..0e74be24540ab9 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
@@ -260,6 +266,7 @@ class ToyotaCarInfo(CarInfo):
},
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 @@ class ToyotaCarInfo(CarInfo):
],
(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 @@ class ToyotaCarInfo(CarInfo):
],
(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 @@ class ToyotaCarInfo(CarInfo):
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',
@@ -1673,6 +1684,7 @@ class ToyotaCarInfo(CarInfo):
b'\x018966348W9000\x00\x00\x00\x00',
b'\x01896634D12000\x00\x00\x00\x00',
b'\x01896634D12100\x00\x00\x00\x00',
+ b'\x01896634D43000\x00\x00\x00\x00',
],
(Ecu.esp, 0x7b0, None): [
b'\x01F15260E031\x00\x00\x00\x00\x00\x00',
@@ -1693,6 +1705,7 @@ class ToyotaCarInfo(CarInfo):
b'\x028646F4810100\x00\x00\x00\x008646G2601200\x00\x00\x00\x00',
b'\x028646F4810200\x00\x00\x00\x008646G2601400\x00\x00\x00\x00',
b'\x028646F4810300\x00\x00\x00\x008646G2601400\x00\x00\x00\x00',
+ b'\x028646F4810400\x00\x00\x00\x008646G2601400\x00\x00\x00\x00',
],
},
CAR.LEXUS_RXH_TSS2: {
diff --git a/selfdrive/car/volkswagen/carstate.py b/selfdrive/car/volkswagen/carstate.py
index 858b5e14402727..dfd6a0031fde29 100644
--- a/selfdrive/car/volkswagen/carstate.py
+++ b/selfdrive/car/volkswagen/carstate.py
@@ -74,11 +74,6 @@ def update(self, pt_cp, cam_cp, ext_cp, trans_type):
# 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 @@ def get_can_parser(CP):
("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 @@ def get_can_parser(CP):
("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 b0fd08ccfc244f..f849d912a0476c 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:
@@ -194,7 +193,6 @@ def _update(self, c):
ret.buttonEvents = buttonEvents
# update previous car states
- self.displayMetricUnitsPrev = self.CS.displayMetricUnits
self.buttonStatesPrev = self.CS.buttonStates.copy()
return ret
diff --git a/selfdrive/car/volkswagen/values.py b/selfdrive/car/volkswagen/values.py
index 5a65c9a91679d9..b2b43c98f3a8a3 100755
--- a/selfdrive/car/volkswagen/values.py
+++ b/selfdrive/car/volkswagen/values.py
@@ -254,6 +254,7 @@ class VWCarInfo(CarInfo):
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',
diff --git a/selfdrive/clocksd/clocksd.cc b/selfdrive/clocksd/clocksd.cc
index c24ffa41d94763..a6a92fc058c152 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 8dfeecbdd7b4b3..e53cb7615db8b4 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 9ebf7d5ff05348..6b30e605060453 100644
--- a/selfdrive/common/modeldata.h
+++ b/selfdrive/common/modeldata.h
@@ -32,13 +32,9 @@ namespace tici_dm_crop {
const int width = 954;
};
-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
@@ -47,7 +43,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 0c36511be7b394..bea12aca11ea3c 100644
--- a/selfdrive/common/params.cc
+++ b/selfdrive/common/params.cc
@@ -134,7 +134,7 @@ std::unordered_map keys = {
{"LastPeripheralPandaType", PERSISTENT},
{"LastPowerDropDetected", CLEAR_ON_MANAGER_START},
{"LastSystemShutdown", CLEAR_ON_MANAGER_START},
- {"LastUpdateException", PERSISTENT},
+ {"LastUpdateException", CLEAR_ON_MANAGER_START},
{"LastUpdateTime", PERSISTENT},
{"LiveParameters", PERSISTENT},
{"NavDestination", CLEAR_ON_MANAGER_START | CLEAR_ON_IGNITION_OFF},
@@ -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 27dfc2ca9d10b7..54463e9816f123 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 618421194cb1e1..21115da10fa58b 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 197372488022b6..edae9fdc34d6bd 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 1d00def63d02e5..47c550463826e3 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 bf0df3bcaa6889..f3a24723b4f51e 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 a98aabc36c4ace..b67f0e202d5c38 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 e8917f3bd618ca..0cb41a32b8046e 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 a1346a4393c8c0..ab23af7b88e880 100755
--- a/selfdrive/controls/controlsd.py
+++ b/selfdrive/controls/controlsd.py
@@ -20,13 +20,13 @@
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 @@
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
@@ -145,8 +144,8 @@ def __init__(self, sm=None, pm=None, can_sock=None, CI=None):
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 @@ def update_events(self, CS):
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 @@ def update_events(self, CS):
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)
@@ -369,7 +364,7 @@ def update_events(self, CS):
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)
@@ -543,7 +538,7 @@ def state_control(self, CS):
# Check which actuators can be enabled
CC.latActive = self.active and not CS.steerFaultTemporary and not CS.steerFaultPermanent and \
CS.vEgo > self.CP.minSteerSpeed and not CS.standstill
- CC.longActive = self.active and not self.events.any(ET.OVERRIDE)
+ CC.longActive = self.active and not self.events.any(ET.OVERRIDE) and self.CP.openpilotLongitudinalControl
actuators = CC.actuators
actuators.longControlState = self.LoC.long_control_state
@@ -571,7 +566,7 @@ def state_control(self, CS):
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:
@@ -735,8 +730,8 @@ def publish_logs(self, CS, start_time, CC, lac_log):
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 4f0ffa794d9dc3..2f85ea917ae6b4 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 abe4111d0a032a..19620cbf4b989d 100644
--- a/selfdrive/controls/lib/events.py
+++ b/selfdrive/controls/lib/events.py
@@ -251,6 +251,32 @@ def no_gps_alert(CP: car.CarParams, sm: messaging.SubMaster, metric: bool, soft_
AlertStatus.normal, AlertSize.mid,
Priority.LOWER, VisualAlert.none, AudibleAlert.none, .2, creation_delay=300.)
+# *** debug alerts ***
+
+def out_of_space_alert(CP: car.CarParams, sm: messaging.SubMaster, metric: bool, soft_disable_time: int) -> Alert:
+ full_perc = round(100. - sm['deviceState'].freeSpacePercent)
+ return NormalPermanentAlert("Out of Storage", f"{full_perc}% full")
+
+
+def overheat_alert(CP: car.CarParams, sm: messaging.SubMaster, metric: bool, soft_disable_time: int) -> Alert:
+ cpu = max(sm['deviceState'].cpuTempC, default=0.)
+ gpu = max(sm['deviceState'].gpuTempC, default=0.)
+ temp = max((cpu, gpu, sm['deviceState'].memoryTempC))
+ return NormalPermanentAlert("System Overheated", f"{temp} °C")
+
+
+def low_memory_alert(CP: car.CarParams, sm: messaging.SubMaster, metric: bool, soft_disable_time: int) -> Alert:
+ return NormalPermanentAlert("Low Memory", f"{sm['deviceState'].memoryUsagePercent}% used")
+
+
+def high_cpu_usage_alert(CP: car.CarParams, sm: messaging.SubMaster, metric: bool, soft_disable_time: int) -> Alert:
+ x = max(sm['deviceState'].cpuUsagePercent, default=0.)
+ return NormalPermanentAlert("High CPU Usage", f"{x}% used")
+
+
+def modeld_lagging_alert(CP: car.CarParams, sm: messaging.SubMaster, metric: bool, soft_disable_time: int) -> Alert:
+ return NormalPermanentAlert("Driving model lagging", f"{sm['modelV2'].frameDropPerc}% frames dropped")
+
def wrong_car_mode_alert(CP: car.CarParams, sm: messaging.SubMaster, metric: bool, soft_disable_time: int) -> Alert:
text = "Cruise Mode Disabled"
@@ -578,7 +604,7 @@ def joystick_alert(CP: car.CarParams, sm: messaging.SubMaster, metric: bool, sof
},
EventName.outOfSpace: {
- ET.PERMANENT: NormalPermanentAlert("Out of Storage"),
+ ET.PERMANENT: out_of_space_alert,
ET.NO_ENTRY: NoEntryAlert("Out of Storage"),
},
@@ -609,7 +635,7 @@ def joystick_alert(CP: car.CarParams, sm: messaging.SubMaster, metric: bool, sof
},
EventName.overheat: {
- ET.PERMANENT: NormalPermanentAlert("System Overheated"),
+ ET.PERMANENT: overheat_alert,
ET.SOFT_DISABLE: soft_disable_alert("System Overheated"),
ET.NO_ENTRY: NoEntryAlert("System Overheated"),
},
@@ -685,6 +711,7 @@ def joystick_alert(CP: car.CarParams, sm: messaging.SubMaster, metric: bool, sof
EventName.modeldLagging: {
ET.SOFT_DISABLE: soft_disable_alert("Driving model lagging"),
ET.NO_ENTRY: NoEntryAlert("Driving model lagging"),
+ ET.PERMANENT: modeld_lagging_alert,
},
# Besides predicting the path, lane lines and lead car data the model also
@@ -706,14 +733,14 @@ def joystick_alert(CP: car.CarParams, sm: messaging.SubMaster, metric: bool, sof
EventName.lowMemory: {
ET.SOFT_DISABLE: soft_disable_alert("Low Memory: Reboot Your Device"),
- ET.PERMANENT: NormalPermanentAlert("Low Memory", "Reboot your Device"),
+ ET.PERMANENT: low_memory_alert,
ET.NO_ENTRY: NoEntryAlert("Low Memory: Reboot Your Device"),
},
EventName.highCpuUsage: {
#ET.SOFT_DISABLE: soft_disable_alert("System Malfunction: Reboot Your Device"),
#ET.PERMANENT: NormalPermanentAlert("System Malfunction", "Reboot your Device"),
- ET.NO_ENTRY: NoEntryAlert("System Malfunction: Reboot Your Device"),
+ ET.NO_ENTRY: high_cpu_usage_alert,
},
EventName.accFaulted: {
@@ -724,6 +751,7 @@ def joystick_alert(CP: car.CarParams, sm: messaging.SubMaster, metric: bool, sof
EventName.controlsMismatch: {
ET.IMMEDIATE_DISABLE: ImmediateDisableAlert("Controls Mismatch"),
+ ET.NO_ENTRY: NoEntryAlert("Controls Mismatch"),
},
EventName.roadCameraError: {
diff --git a/selfdrive/controls/lib/lane_planner.py b/selfdrive/controls/lib/lane_planner.py
index 0230255ef983f2..aedf61a073eff5 100644
--- a/selfdrive/controls/lib/lane_planner.py
+++ b/selfdrive/controls/lib/lane_planner.py
@@ -3,7 +3,7 @@
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 @@
# 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 4a67715fcb547f..0d137df7338ab7 100644
--- a/selfdrive/controls/lib/latcontrol.py
+++ b/selfdrive/controls/lib/latcontrol.py
@@ -16,7 +16,7 @@ def __init__(self, CP, CI):
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 c9353563113a28..8d09432b6a6cd9 100644
--- a/selfdrive/controls/lib/latcontrol_angle.py
+++ b/selfdrive/controls/lib/latcontrol_angle.py
@@ -7,7 +7,7 @@
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 2db2d43881cc33..4d2a7276236d4c 100644
--- a/selfdrive/controls/lib/latcontrol_indi.py
+++ b/selfdrive/controls/lib/latcontrol_indi.py
@@ -63,7 +63,7 @@ def reset(self):
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 583244882caac3..00000000000000
--- 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 67d17e088ce083..813ba13abaa301 100644
--- a/selfdrive/controls/lib/latcontrol_pid.py
+++ b/selfdrive/controls/lib/latcontrol_pid.py
@@ -17,7 +17,7 @@ def reset(self):
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 00000000000000..2816b20149794b
--- /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 76f7fadce7362b..df1e2a2a1ae6a6 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 fd0da63a37ba49..5a9e69c29781e8 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 8345840eca6063..503eaaa6a465a2 100755
--- a/selfdrive/controls/lib/tests/test_latcontrol.py
+++ b/selfdrive/controls/lib/tests/test_latcontrol.py
@@ -9,7 +9,7 @@
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 @@
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 d49db25d0920ef..f6903170a284ea 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 8a04d4aefd583d..919a82fefc726b 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/cycle_alerts.py b/selfdrive/debug/cycle_alerts.py
index f28d5373f44c7f..d097367474d29f 100755
--- a/selfdrive/debug/cycle_alerts.py
+++ b/selfdrive/debug/cycle_alerts.py
@@ -1,15 +1,11 @@
#!/usr/bin/env python3
-# flake8: noqa
-# pylint: skip-file
-# type: ignore
-
import time
from cereal import car, log
import cereal.messaging as messaging
from common.realtime import DT_CTRL
from selfdrive.car.honda.interface import CarInterface
-from selfdrive.controls.lib.events import ET, EVENTS, Events
+from selfdrive.controls.lib.events import ET, Events
from selfdrive.controls.lib.alertmanager import AlertManager
EventName = car.CarEvent.EventName
@@ -33,10 +29,26 @@ def cycle_alerts(duration=200, is_metric=False):
(EventName.driverDistracted, ET.WARNING),
]
+ # debug alerts
+ alerts = [
+ (EventName.highCpuUsage, ET.NO_ENTRY),
+ (EventName.lowMemory, ET.PERMANENT),
+ (EventName.overheat, ET.PERMANENT),
+ (EventName.outOfSpace, ET.PERMANENT),
+ (EventName.modeldLagging, ET.PERMANENT),
+ ]
+
CP = CarInterface.get_params("HONDA CIVIC 2016")
sm = messaging.SubMaster(['deviceState', 'pandaStates', 'roadCameraState', 'modelV2', 'liveCalibration',
'driverMonitoringState', 'longitudinalPlan', 'lateralPlan', 'liveLocationKalman'])
+ sm['deviceState'].freeSpacePercent = 55
+ sm['deviceState'].memoryUsagePercent = 55
+ sm['deviceState'].cpuTempC = [1, 2, 100]
+ sm['deviceState'].gpuTempC = [211, 2, 100]
+ sm['deviceState'].cpuUsagePercent = [23, 54]
+ sm['modelV2'].frameDropPerc = 20
+
pm = messaging.PubMaster(['controlsState', 'pandaStates', 'deviceState'])
events = Events()
@@ -44,30 +56,27 @@ def cycle_alerts(duration=200, is_metric=False):
frame = 0
while True:
- current_alert_types = [ET.PERMANENT, ET.USER_DISABLE, ET.IMMEDIATE_DISABLE,
- ET.SOFT_DISABLE, ET.PRE_ENABLE, ET.NO_ENTRY,
- ET.ENABLE, ET.WARNING]
-
for alert, et in alerts:
events.clear()
events.add(alert)
a = events.create_alerts([et, ], [CP, sm, is_metric, 0])
AM.add_many(frame, a)
- AM.process_alerts(frame)
- print(AM.alert)
+ alert = AM.process_alerts(frame, [])
+ print(alert)
for _ in range(duration):
dat = messaging.new_message()
dat.init('controlsState')
- dat.controlsState.enabled = True
-
- dat.controlsState.alertText1 = AM.alert_text_1
- dat.controlsState.alertText2 = AM.alert_text_2
- dat.controlsState.alertSize = AM.alert_size
- dat.controlsState.alertStatus = AM.alert_status
- dat.controlsState.alertBlinkingRate = AM.alert_rate
- dat.controlsState.alertType = AM.alert_type
- dat.controlsState.alertSound = AM.audible_alert
+ dat.controlsState.enabled = False
+
+ if alert:
+ dat.controlsState.alertText1 = alert.alert_text_1
+ dat.controlsState.alertText2 = alert.alert_text_2
+ dat.controlsState.alertSize = alert.alert_size
+ dat.controlsState.alertStatus = alert.alert_status
+ dat.controlsState.alertBlinkingRate = alert.alert_rate
+ dat.controlsState.alertType = alert.alert_type
+ dat.controlsState.alertSound = alert.audible_alert
pm.send('controlsState', dat)
dat = messaging.new_message()
diff --git a/selfdrive/debug/internal/measure_modeld_packet_drop.py b/selfdrive/debug/internal/measure_modeld_packet_drop.py
index d5f65d06b1e8e5..6b7f7dbd13f8ac 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 e46c0b0a1f801d..b244f7cb0ed028 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 @@ def proc_name(proc):
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 @@ def proc_name(proc):
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/vw_mqb_config.py b/selfdrive/debug/vw_mqb_config.py
index 1dd28d371fae64..c55a058159b147 100755
--- a/selfdrive/debug/vw_mqb_config.py
+++ b/selfdrive/debug/vw_mqb_config.py
@@ -23,7 +23,7 @@ class ACCESS_TYPE_LEVEL_1(IntEnum):
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 3babf1bb5d3abb..03dfce86d76213 100644
--- a/selfdrive/hardware/__init__.py
+++ b/selfdrive/hardware/__init__.py
@@ -2,18 +2,14 @@
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 05e55cc0340c66..2826d09700589d 100644
--- a/selfdrive/hardware/base.h
+++ b/selfdrive/hardware/base.h
@@ -20,6 +20,5 @@ class HardwareNone {
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 c05c75bbb2dbac..2f251d33643db1 100644
--- a/selfdrive/hardware/base.py
+++ b/selfdrive/hardware/base.py
@@ -44,7 +44,7 @@ def get_sound_card_online(self):
pass
@abstractmethod
- def get_imei(self, slot):
+ def get_imei(self, slot) -> str:
pass
@abstractmethod
diff --git a/selfdrive/hardware/eon/androidd.py b/selfdrive/hardware/eon/androidd.py
deleted file mode 100755
index 3d91468b908505..00000000000000
--- 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 bcd1aaba740302..00000000000000
--- 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 bd2a01cf1e4aae..00000000000000
--- 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 4010f7126a0892..00000000000000
--- 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 6f290fbcd19e5d..00000000000000
--- 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 15112e7d5ee300..00000000000000
--- 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 e258f943d20432..00000000000000
--- 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 ccc6ecce4496e7..00000000000000
--- 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 eaf34e957ceb32..00000000000000
Binary files a/selfdrive/hardware/eon/updater and /dev/null differ
diff --git a/selfdrive/hardware/hw.h b/selfdrive/hardware/hw.h
index f18ede01ec25cc..b221b63db25756 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/hardware.py b/selfdrive/hardware/tici/hardware.py
index 77f32653864d0b..1cffe7c287cc27 100644
--- a/selfdrive/hardware/tici/hardware.py
+++ b/selfdrive/hardware/tici/hardware.py
@@ -415,12 +415,12 @@ def set_power_save(self, powersave_enabled):
# offline big cluster, leave core 4 online for boardd
for i in range(5, 8):
- val = "0" if powersave_enabled else "1"
- sudo_write(val, f"/sys/devices/system/cpu/cpu{i}/online")
+ val = '0' if powersave_enabled else '1'
+ sudo_write(val, f'/sys/devices/system/cpu/cpu{i}/online')
for n in ('0', '4'):
gov = 'ondemand' if powersave_enabled else 'performance'
- sudo_write(gov, f"/sys/devices/system/cpu/cpufreq/policy{n}/scaling_governor")
+ sudo_write(gov, f'/sys/devices/system/cpu/cpufreq/policy{n}/scaling_governor')
# *** IRQ config ***
affine_irq(5, 565) # kgsl-3d0
@@ -450,13 +450,15 @@ def initialize_hardware(self):
sudo_write("f", "/proc/irq/default_smp_affinity")
# *** GPU config ***
- sudo_write("0", "/sys/class/kgsl/kgsl-3d0/min_pwrlevel")
- sudo_write("0", "/sys/class/kgsl/kgsl-3d0/max_pwrlevel")
+ # https://github.com/commaai/agnos-kernel-sdm845/blob/master/arch/arm64/boot/dts/qcom/sdm845-gpu.dtsi#L216
+ sudo_write("1", "/sys/class/kgsl/kgsl-3d0/min_pwrlevel")
+ sudo_write("1", "/sys/class/kgsl/kgsl-3d0/max_pwrlevel")
sudo_write("1", "/sys/class/kgsl/kgsl-3d0/force_bus_on")
sudo_write("1", "/sys/class/kgsl/kgsl-3d0/force_clk_on")
sudo_write("1", "/sys/class/kgsl/kgsl-3d0/force_rail_on")
sudo_write("1000000", "/sys/class/kgsl/kgsl-3d0/idle_timer")
sudo_write("performance", "/sys/class/kgsl/kgsl-3d0/devfreq/governor")
+ sudo_write("596", "/sys/class/kgsl/kgsl-3d0/max_clock_mhz")
# setup governors
sudo_write("performance", "/sys/class/devfreq/soc:qcom,cpubw/governor")
diff --git a/selfdrive/hardware/tici/power_monitor.py b/selfdrive/hardware/tici/power_monitor.py
index d7f113cf2c5227..07362c83987bf8 100755
--- a/selfdrive/hardware/tici/power_monitor.py
+++ b/selfdrive/hardware/tici/power_monitor.py
@@ -1,11 +1,32 @@
#!/usr/bin/env python3
import sys
import time
+import numpy as np
+from typing import List
+
+from common.realtime import Ratekeeper
def average(avg, sample):
# Weighted avg between existing value and new sample
return ((avg[0] * avg[1] + sample) / (avg[1] + 1), avg[1] + 1)
+
+def sample_power(seconds=5) -> List[float]:
+ rate = 123
+ rk = Ratekeeper(rate, print_delay_threshold=None)
+
+ pwrs = []
+ for _ in range(rate*seconds):
+ with open("/sys/bus/i2c/devices/0-0040/hwmon/hwmon1/power1_input") as f:
+ pwrs.append(int(f.read()) / 1e6)
+ rk.keep_time()
+ return pwrs
+
+def get_power(seconds=5):
+ pwrs = sample_power(seconds)
+ return np.mean(pwrs)
+
+
if __name__ == '__main__':
sample_time = None
@@ -17,7 +38,6 @@ def average(avg, sample):
voltage_average = (0, 0) # average, count
current_average = (0, 0)
power_average = (0, 0)
- power_total_average = (0, 0)
while sample_time is None or time.monotonic() - start_time < sample_time:
with open("/sys/bus/i2c/devices/0-0040/hwmon/hwmon1/in1_input") as f:
voltage_total = int(f.read()) / 1000.
@@ -25,23 +45,23 @@ def average(avg, sample):
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
+ # SOM measurements are questionable
+ #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 = voltage*current
power_total = voltage_total*current_total
# compute averages
- voltage_average = average(voltage_average, voltage)
- current_average = average(current_average, current)
- power_average = average(power_average, power)
- power_total_average = average(power_total_average, power_total)
+ voltage_average = average(voltage_average, voltage_total)
+ current_average = average(current_average, current_total)
+ power_average = average(power_average, power_total)
- print(f"{power:12.2f} mW {power_total:12.2f} mW {power_total - power:12.2f} mW")
+ print(f"now: {power_total:.2f} mW, avg: {power_average[0]:.2f} mW")
time.sleep(0.25)
+ except KeyboardInterrupt:
+ pass
finally:
stop_time = time.monotonic()
print("\n----------------------Average-----------------------------------")
diff --git a/selfdrive/hardware/tici/precise_power_measure.py b/selfdrive/hardware/tici/precise_power_measure.py
new file mode 100755
index 00000000000000..c66936aef871df
--- /dev/null
+++ b/selfdrive/hardware/tici/precise_power_measure.py
@@ -0,0 +1,9 @@
+#!/usr/bin/env python3
+import numpy as np
+from selfdrive.hardware.tici.power_monitor import sample_power
+
+if __name__ == '__main__':
+ print("measuring for 5 seconds")
+ for _ in range(3):
+ pwrs = sample_power()
+ print("mean %.2f std %.2f" % (np.mean(pwrs), np.std(pwrs)))
diff --git a/selfdrive/hardware/tici/test_power_draw.py b/selfdrive/hardware/tici/test_power_draw.py
new file mode 100755
index 00000000000000..fee799503877bb
--- /dev/null
+++ b/selfdrive/hardware/tici/test_power_draw.py
@@ -0,0 +1,61 @@
+#!/usr/bin/env python3
+import unittest
+import time
+import math
+from collections import OrderedDict
+
+from selfdrive.hardware import HARDWARE, TICI
+from selfdrive.hardware.tici.power_monitor import get_power
+from selfdrive.manager.process_config import managed_processes
+from selfdrive.manager.manager import manager_cleanup
+
+POWER = OrderedDict(
+ camerad=2.58,
+ modeld=0.90,
+ dmonitoringmodeld=0.25,
+ loggerd=0.45,
+)
+
+
+class TestPowerDraw(unittest.TestCase):
+
+ @classmethod
+ def setUpClass(cls):
+ if not TICI:
+ raise unittest.SkipTest
+
+ def setUp(self):
+ HARDWARE.initialize_hardware()
+ HARDWARE.set_power_save(False)
+
+ def tearDown(self):
+ manager_cleanup()
+
+ def test_camera_procs(self):
+ baseline = get_power()
+
+ prev = baseline
+ used = {}
+ for proc in POWER.keys():
+ managed_processes[proc].start()
+ time.sleep(6)
+
+ now = get_power(8)
+ used[proc] = now - prev
+ prev = now
+
+ manager_cleanup()
+
+ print("-"*35)
+ print(f"Baseline {baseline:.2f}W\n")
+ for proc in POWER.keys():
+ cur = used[proc]
+ expected = POWER[proc]
+ print(f"{proc.ljust(20)} {expected:.2f}W {cur:.2f}W")
+ with self.subTest(proc=proc):
+ self.assertTrue(math.isclose(cur, expected, rel_tol=0.10, abs_tol=0.1))
+ print("-"*35)
+
+
+if __name__ == "__main__":
+ unittest.main()
diff --git a/selfdrive/locationd/models/constants.py b/selfdrive/locationd/models/constants.py
index f22f503ee0cd74..6d328ce6f52793 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 b6293d60db8dcc..8391426dd1cd10 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 @@ def generate_code(generated_dir, N=4):
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 @@ def generate_code(generated_dir, N=4):
[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 @@ def generate_code(generated_dir, N=4):
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 @@ def __init__(self, generated_dir, N=4, max_tracks=3000):
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 @@ def predict_and_observe(self, t, kind, data):
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 @@ def predict_and_update_orb_features(self, tracks, t, kind):
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 @@ def predict_and_update_orb_features(self, tracks, t, kind):
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 55479f255c274c..0e83728e5c87f3 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 @@ def handle_log(self, t, which, msg):
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 8811f32fe65118..6bd7c6ff3e8561 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 8c2524d94a0949..00000000000000
--- 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 4e0c903dfa9959..00000000000000
--- 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 76fbcae6c4e1f0..92cc37a5fbe058 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 a72312484748e1..e7ce308cec6f49 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 6ba520d162670a..f2c34554e329b2 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 96c205225803a3..31e035955ae917 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));
@@ -330,7 +324,7 @@ OmxEncoder::OmxEncoder(const char* filename, CameraType type, int width, int hei
service_name = this->type == DriverCam ? "driverEncodeData" :
(this->type == WideRoadCam ? "wideRoadEncodeData" :
(this->remuxing ? "qRoadEncodeData" : "roadEncodeData"));
- pm = new PubMaster({service_name});
+ pm.reset(new PubMaster({service_name}));
}
void OmxEncoder::callback_handler(OmxEncoder *e) {
@@ -399,71 +393,19 @@ void OmxEncoder::write_and_broadcast_handler(OmxEncoder *e){
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;
@@ -531,54 +473,10 @@ 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(OmxEncoder::write_and_broadcast_handler, this);
@@ -606,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;
}
@@ -653,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 fdaeee71652205..e57dccbe11c3bd 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);
@@ -43,9 +41,8 @@ class OmxEncoder : public VideoEncoder {
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;
@@ -53,17 +50,12 @@ class OmxEncoder : public VideoEncoder {
std::thread callback_handler_thread;
std::thread write_handler_thread;
int segment_num = -1;
- PubMaster *pm;
+ 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;
@@ -79,10 +71,8 @@ class OmxEncoder : public VideoEncoder {
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 6da0d59ad3bf35..29d421a8816f1b 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 2fa23f83cdca9e..56bd08ff1fd1a2 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 @@ class RawLogger : public VideoEncoder {
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 97c142652f3cab..9479b6256a064a 100755
--- a/selfdrive/loggerd/tests/test_encoder.py
+++ b/selfdrive/loggerd/tests/test_encoder.py
@@ -13,27 +13,19 @@
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 @@ def check_seg(i):
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 @@ def check_seg(i):
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 @@ def check_seg(i):
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 00000000000000..f9034f691253b2
--- /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 00000000000000..8217b859bf349c
--- /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 4b97855f348c6a..806aa58bb70887 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 b5fa43967167ee..1934da67015fcc 100755
--- a/selfdrive/manager/manager.py
+++ b/selfdrive/manager/manager.py
@@ -130,24 +130,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)
-
- # trigger an update after going offroad
- if started_prev and not started and 'updated' in managed_processes:
- os.sync()
- managed_processes['updated'].signal(signal.SIGHUP)
-
- started_prev = started
+ ensure_running(managed_processes.values(), started=started, driverview=driverview, notcar=sm['carParams'].notCar, not_run=ignore)
running = ' '.join("%s%s\u001b[0m" % ("\u001b[32m" if p.proc.is_alive() else "\u001b[31m", p.name)
for p in managed_processes.values() if p.proc)
diff --git a/selfdrive/manager/process.py b/selfdrive/manager/process.py
index 4cd9783b2b0e07..8bb160961f8afc 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 @@ def get_process_state_msg(self):
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 @@ def start(self) -> None:
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 @@ def __init__(self, name, module, param_name, enabled=True):
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 @@ def stop(self, retry=True, block=True) -> None:
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 da4284571ffadc..2e2747fecc7651 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 @@
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, onroad=False, 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 d16a145031df08..1750c81b2e1b25 100755
--- a/selfdrive/manager/test/test_manager.py
+++ b/selfdrive/manager/test/test_manager.py
@@ -5,14 +5,14 @@
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 @@ def test_clean_exit(self):
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 20d3fb8acc6c8f..c6855e8c38a249 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']
- libs += ['gnustl_shared'] if arch == "aarch64" else ['pthread', 'dl']
+if arch == "larch64":
+ libs += ['gsl', 'CB', 'pthread', 'dl']
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 9acd7bcc33cf40..f292fe4c0bd2a4 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 fcf2812cb7305a..c067cf3d622489 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 2714370106a178..6ee07b3d021dd4 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;
}
@@ -133,11 +129,6 @@ void run_model(ModelState &model, VisionIpcClient &vipc_client_main, VisionIpcCl
vec_desire[desire] = 1.0;
}
- double mt1 = millis_since_boot();
- ModelOutput *model_output = model_eval_frame(&model, buf_main, buf_extra, model_transform_main, model_transform_extra, vec_desire);
- double mt2 = millis_since_boot();
- float model_execution_time = (mt2 - mt1) / 1000.0;
-
// tracked dropped frames
uint32_t vipc_dropped_frames = meta_main.frame_id - last_vipc_frame_id - 1;
float frames_dropped = frame_dropped_filter.update((float)std::min(vipc_dropped_frames, 10U));
@@ -148,10 +139,22 @@ void run_model(ModelState &model, VisionIpcClient &vipc_client_main, VisionIpcCl
run_count++;
float frame_drop_ratio = frames_dropped / (1 + frames_dropped);
+ bool prepare_only = vipc_dropped_frames > 0;
- model_publish(pm, meta_main.frame_id, meta_extra.frame_id, frame_id, frame_drop_ratio, *model_output, meta_main.timestamp_eof, model_execution_time,
- kj::ArrayPtr(model.output.data(), model.output.size()), live_calib_seen);
- posenet_publish(pm, meta_main.frame_id, vipc_dropped_frames, *model_output, meta_main.timestamp_eof, live_calib_seen);
+ if (prepare_only) {
+ LOGE("skipping model eval. Dropped %d frames", vipc_dropped_frames);
+ }
+
+ double mt1 = millis_since_boot();
+ ModelOutput *model_output = model_eval_frame(&model, buf_main, buf_extra, model_transform_main, model_transform_extra, vec_desire, prepare_only);
+ double mt2 = millis_since_boot();
+ float model_execution_time = (mt2 - mt1) / 1000.0;
+
+ if (model_output != nullptr) {
+ model_publish(pm, meta_main.frame_id, meta_extra.frame_id, frame_id, frame_drop_ratio, *model_output, meta_main.timestamp_eof, model_execution_time,
+ kj::ArrayPtr(model.output.data(), model.output.size()), live_calib_seen);
+ posenet_publish(pm, meta_main.frame_id, vipc_dropped_frames, *model_output, meta_main.timestamp_eof, live_calib_seen);
+ }
//printf("model process: %.2fms, from last %.2fms, vipc_frame_id %u, frame_id, %u, frame_drop %.3f\n", mt2 - mt1, mt1 - last, extra.frame_id, frame_id, frame_drop_ratio);
last = mt1;
@@ -160,16 +163,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 main_wide_camera = Params().getBool("EnableWideCamera");
+ bool use_extra_client = !main_wide_camera; // set for single camera mode
// cl init
cl_device_id device_id = cl_get_device_id(CL_DEVICE_TYPE_DEFAULT);
diff --git a/selfdrive/modeld/models/driving.cc b/selfdrive/modeld/models/driving.cc
index 422ad21cdeb530..e14411df787b1f 100644
--- a/selfdrive/modeld/models/driving.cc
+++ b/selfdrive/modeld/models/driving.cc
@@ -56,7 +56,7 @@ void model_init(ModelState* s, cl_device_id device_id, cl_context context) {
}
ModelOutput* model_eval_frame(ModelState* s, VisionBuf* buf, VisionBuf* wbuf,
- const mat3 &transform, const mat3 &transform_wide, float *desire_in) {
+ const mat3 &transform, const mat3 &transform_wide, float *desire_in, bool prepare_only) {
#ifdef DESIRE
if (desire_in != NULL) {
for (int i = 1; i < DESIRE_LEN; i++) {
@@ -82,6 +82,11 @@ ModelOutput* model_eval_frame(ModelState* s, VisionBuf* buf, VisionBuf* wbuf,
s->m->addExtra(net_extra_buf, s->wide_frame->buf_size);
LOGT("Extra image added");
}
+
+ if (prepare_only) {
+ return nullptr;
+ }
+
s->m->execute();
LOGT("Execution finished");
diff --git a/selfdrive/modeld/models/driving.h b/selfdrive/modeld/models/driving.h
index f83cb869392bb8..47dec0d9d740d5 100644
--- a/selfdrive/modeld/models/driving.h
+++ b/selfdrive/modeld/models/driving.h
@@ -268,7 +268,7 @@ struct ModelState {
void model_init(ModelState* s, cl_device_id device_id, cl_context context);
ModelOutput *model_eval_frame(ModelState* s, VisionBuf* buf, VisionBuf* buf_wide,
- const mat3 &transform, const mat3 &transform_wide, float *desire_in);
+ const mat3 &transform, const mat3 &transform_wide, float *desire_in, bool prepare_only);
void model_free(ModelState* s);
void model_publish(PubMaster &pm, uint32_t vipc_frame_id, uint32_t vipc_frame_id_extra, uint32_t frame_id, float frame_drop,
const ModelOutput &net_outputs, uint64_t timestamp_eof,
diff --git a/selfdrive/modeld/runners/snpemodel.cc b/selfdrive/modeld/runners/snpemodel.cc
index 0d394ca511e4b1..44fa6ce298c389 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 584c6368026e81..6e9c33f89c8f6d 100644
--- a/selfdrive/modeld/runners/snpemodel.h
+++ b/selfdrive/modeld/runners/snpemodel.h
@@ -38,7 +38,7 @@ class SNPEModel : public RunModel {
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 00000000000000..f09aec578c94fb
--- /dev/null
+++ b/selfdrive/modeld/test/test_modeld.py
@@ -0,0 +1,107 @@
+#!/usr/bin/env python3
+import time
+import unittest
+import numpy as np
+import random
+
+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', '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 _send_frames(self, frame_id, cams=None):
+ if cams is None:
+ cams = ('roadCameraState', 'wideRoadCameraState')
+
+ cs = None
+ for cam in cams:
+ msg = messaging.new_message(cam)
+ cs = getattr(msg, cam)
+ cs.frameId = frame_id
+ cs.timestampSof = int((frame_id * 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)
+ return cs
+
+ def _wait(self):
+ self.sm.update(5000)
+ if self.sm['modelV2'].frameId != self.sm['cameraOdometry'].frameId:
+ self.sm.update(1000)
+
+ def test_modeld(self):
+ for n in range(1, 500):
+ cs = self._send_frames(n)
+ self._wait()
+
+ 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_dropped_frames(self):
+ """
+ modeld should only run on consecutive road frames
+ """
+ frame_id = -1
+ road_frames = list()
+ for n in range(1, 50):
+ if (random.random() < 0.1) and n > 3:
+ cams = random.choice([(), ('wideRoadCameraState', )])
+ self._send_frames(n, cams)
+ else:
+ self._send_frames(n)
+ road_frames.append(n)
+ self._wait()
+
+ if len(road_frames) < 3 or road_frames[-1] - road_frames[-2] == 1:
+ frame_id = road_frames[-1]
+
+ mdl = self.sm['modelV2']
+ odo = self.sm['cameraOdometry']
+ self.assertEqual(mdl.frameId, frame_id)
+ self.assertEqual(mdl.frameIdExtra, frame_id)
+ self.assertEqual(odo.frameId, frame_id)
+ if n != frame_id:
+ self.assertFalse(self.sm.updated['modelV2'])
+ self.assertFalse(self.sm.updated['cameraOdometry'])
+
+
+if __name__ == "__main__":
+ unittest.main()
diff --git a/selfdrive/modeld/thneed/thneed.cc b/selfdrive/modeld/thneed/thneed.cc
index e2dc9b72f22d0a..470ff219b0e431 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/rtshield.py b/selfdrive/rtshield.py
index ad7538d7a8bc2c..45571fe2db3841 100755
--- a/selfdrive/rtshield.py
+++ b/selfdrive/rtshield.py
@@ -11,7 +11,7 @@
# 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 186e302bc3d4ea..db32887e7f4073 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 fcdb9a72d78fee..00000000000000
--- 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