Compare commits
16 Commits
dragonpilot
..
pre
| Author | SHA1 | Date | |
|---|---|---|---|
| 317d51566b | |||
| d606f7d723 | |||
| 1986516b32 | |||
| 6f274a1e59 | |||
| e889c3bf19 | |||
| 3346e4d281 | |||
| 80ac65a3e2 | |||
| e5ffea4734 | |||
| 3ef072024f | |||
| b5c149dfed | |||
| bdf701af10 | |||
| 0634122d32 | |||
| 5b95afd3dc | |||
| 01e9d51093 | |||
| 0076d30d62 | |||
| 6928314c89 |
@@ -13,13 +13,13 @@ venv/
|
|||||||
a.out
|
a.out
|
||||||
.hypothesis
|
.hypothesis
|
||||||
.cache/
|
.cache/
|
||||||
bin/
|
|
||||||
|
/docs_site/
|
||||||
|
|
||||||
*.mp4
|
*.mp4
|
||||||
*.dylib
|
*.dylib
|
||||||
*.DSYM
|
*.DSYM
|
||||||
*.d
|
*.d
|
||||||
*.pem
|
|
||||||
*.pyc
|
*.pyc
|
||||||
*.pyo
|
*.pyo
|
||||||
.*.swp
|
.*.swp
|
||||||
@@ -39,13 +39,11 @@ bin/
|
|||||||
*.mo
|
*.mo
|
||||||
*_pyx.cpp
|
*_pyx.cpp
|
||||||
*.stats
|
*.stats
|
||||||
*.pkl
|
|
||||||
*.pkl*
|
|
||||||
config.json
|
config.json
|
||||||
|
clcache
|
||||||
compile_commands.json
|
compile_commands.json
|
||||||
compare_runtime*.html
|
compare_runtime*.html
|
||||||
|
|
||||||
# build artifacts
|
|
||||||
selfdrive/pandad/pandad
|
selfdrive/pandad/pandad
|
||||||
cereal/services.h
|
cereal/services.h
|
||||||
cereal/gen
|
cereal/gen
|
||||||
@@ -58,34 +56,45 @@ system/camerad/test/ae_gray_test
|
|||||||
.coverage*
|
.coverage*
|
||||||
coverage.xml
|
coverage.xml
|
||||||
htmlcov
|
htmlcov
|
||||||
|
pandaextra
|
||||||
|
|
||||||
|
.mypy_cache/
|
||||||
|
flycheck_*
|
||||||
|
|
||||||
|
cppcheck_report.txt
|
||||||
|
comma*.sh
|
||||||
|
|
||||||
|
selfdrive/modeld/models/*.pkl
|
||||||
|
|
||||||
# openpilot log files
|
# openpilot log files
|
||||||
*.bz2
|
*.bz2
|
||||||
*.zst
|
*.zst
|
||||||
*.rlog
|
|
||||||
|
|
||||||
build/
|
build/
|
||||||
|
|
||||||
!**/.gitkeep
|
!**/.gitkeep
|
||||||
|
|
||||||
|
poetry.toml
|
||||||
|
Pipfile
|
||||||
|
|
||||||
### VisualStudioCode ###
|
### VisualStudioCode ###
|
||||||
*.vsix
|
|
||||||
.history
|
|
||||||
.ionide
|
|
||||||
.vscode/*
|
.vscode/*
|
||||||
.history/
|
|
||||||
!.vscode/settings.json
|
!.vscode/settings.json
|
||||||
!.vscode/tasks.json
|
!.vscode/tasks.json
|
||||||
!.vscode/launch.json
|
!.vscode/launch.json
|
||||||
!.vscode/extensions.json
|
!.vscode/extensions.json
|
||||||
!.vscode/*.code-snippets
|
!.vscode/*.code-snippets
|
||||||
|
|
||||||
# agents
|
# Local History for Visual Studio Code
|
||||||
.claude/
|
.history/
|
||||||
.context/
|
|
||||||
PLAN.md
|
# Built Visual Studio Code Extensions
|
||||||
TASK.md
|
*.vsix
|
||||||
|
|
||||||
|
### VisualStudioCode Patch ###
|
||||||
|
# Ignore all local history of files
|
||||||
|
.history
|
||||||
|
.ionide
|
||||||
|
|
||||||
# rick - keep panda_tici standalone
|
# rick - keep panda_tici standalone
|
||||||
panda_tici/
|
panda_tici/
|
||||||
|
|||||||
@@ -0,0 +1,91 @@
|
|||||||
|
```mermaid
|
||||||
|
flowchart TD
|
||||||
|
B000["devel-staging"] ---> CORE["core"]
|
||||||
|
CORE ---> CORE_001["core-feat/params"]
|
||||||
|
CORE_001 ---> CORE_002["core-feat/panel"]
|
||||||
|
CORE_002 ---> CORE_003["core-feat/safety-ext"]
|
||||||
|
CORE_003 ---> MIN["min"]
|
||||||
|
MIN ---> MIN_001["min-feat/ui/display-mode"]
|
||||||
|
MIN ---> MIN_002["min-feat/dev/model-selector"]
|
||||||
|
MIN ---> MIN_003["min-feat/lat/lca"]
|
||||||
|
MIN ---> MIN_004["min-feat/dev/on-off-road"]
|
||||||
|
MIN ---> MIN_005["min-feat/ui/hide-hud"]
|
||||||
|
MIN ---> MIN_006["min-feat/lon/ext-radar"]
|
||||||
|
MIN ---> MIN_007["min-feat/lat/road-edge-detection"]
|
||||||
|
MIN ---> MIN_008["min-feat/ui/rainbow-path"]
|
||||||
|
MIN ---> MIN_009["min-feat/lon/acm"]
|
||||||
|
MIN ---> MIN_010["min-feat/lon/aem"]
|
||||||
|
MIN ---> MIN_011["min-feat/lon/dtsc"]
|
||||||
|
MIN ---> MIN_012["min-feat/lon/apm"]
|
||||||
|
MIN ---> MIN_013["min-feat/lon/dasr"]
|
||||||
|
MIN ---> MIN_014["min-feat/dev/alert-mode"]
|
||||||
|
MIN ---> MIN_015["min-feat/dev/auto-shutdown"]
|
||||||
|
MIN ---> MIN_016["min-feat/ui/lead-stats"]
|
||||||
|
MIN ---> MIN_017["min-feat/ui/border-indicator"]
|
||||||
|
MIN ---> MIN_018["min-feat/dev/delay-loggerd"]
|
||||||
|
MIN ---> MIN_019["min-feat/dev/disable-connect"]
|
||||||
|
MIN ---> MIN_020["min-feat/dev/tether-on-boot"]
|
||||||
|
MIN ---> MIN_021["min-feat/ui/torque-bar"]
|
||||||
|
MIN ---> MIN_022["min-feat/ui/mici-ui-mode"]
|
||||||
|
MIN ---> MIN_023["min-feat/lat/lat-offset"]
|
||||||
|
MIN_001 ---> FULL["full"]
|
||||||
|
MIN_002 ---> FULL
|
||||||
|
MIN_003 ---> FULL
|
||||||
|
MIN_004 ---> FULL
|
||||||
|
MIN_005 ---> FULL
|
||||||
|
MIN_006 ---> FULL
|
||||||
|
MIN_007 ---> FULL
|
||||||
|
MIN_008 ---> FULL
|
||||||
|
MIN_009 ---> FULL
|
||||||
|
MIN_010 ---> FULL
|
||||||
|
MIN_011 ---> FULL
|
||||||
|
MIN_012 ---> FULL
|
||||||
|
MIN_013 ---> FULL
|
||||||
|
MIN_014 ---> FULL
|
||||||
|
MIN_015 ---> FULL
|
||||||
|
MIN_016 ---> FULL
|
||||||
|
MIN_017 ---> FULL
|
||||||
|
MIN_018 ---> FULL
|
||||||
|
MIN_019 ---> FULL
|
||||||
|
MIN_020 ---> FULL
|
||||||
|
MIN_021 ---> FULL
|
||||||
|
FULL ---> TOYOTA_001[brand/toyota/safety-common]
|
||||||
|
FULL ---> TOYOTA_002[brand/toyota/door-auto-lock-unlock]
|
||||||
|
FULL ---> TOYOTA_003[brand/toyota/tss1-sng]
|
||||||
|
FULL ---> TOYOTA_004[brand/toyota/radar-filter]
|
||||||
|
FULL ---> TOYOTA_005[brand/toyota/sdsu]
|
||||||
|
FULL ---> TOYOTA_006[brand/toyota/dsu-bypass]
|
||||||
|
FULL ---> TOYOTA_007[brand/toyota/zss]
|
||||||
|
FULL ---> TOYOTA_008[brand/toyota/stock-lon]
|
||||||
|
FULL ---> VAG_001[brand/vag/a0-sng]
|
||||||
|
FULL ---> VAG_002[brand/vag/pq-steering-patch]
|
||||||
|
FULL ---> VAG_003[brand/vag/pq-no-dashcam]
|
||||||
|
FULL ---> VAG_004[brand/vag/avoid-eps-lockout]
|
||||||
|
FULL ---> HKG_001[brand/hkg/smdps]
|
||||||
|
FULL ---> HONDA_001[brand/honda/eps-mod]
|
||||||
|
FULL ---> HONDA_002[brand/honda/nidec-stock-long]
|
||||||
|
FULL ---> SUBARU_001[brand/subaru/torque-3071]
|
||||||
|
TOYOTA_001 ---> TOYOTA[pre-toyota]
|
||||||
|
TOYOTA_002 ---> TOYOTA
|
||||||
|
TOYOTA_003 ---> TOYOTA
|
||||||
|
TOYOTA_004 ---> TOYOTA
|
||||||
|
TOYOTA_005 ---> TOYOTA
|
||||||
|
TOYOTA_006 ---> TOYOTA
|
||||||
|
TOYOTA_007 ---> TOYOTA
|
||||||
|
TOYOTA_008 ---> TOYOTA
|
||||||
|
VAG_001 ---> VAG[pre-vag]
|
||||||
|
VAG_002 ---> VAG
|
||||||
|
VAG_003 ---> VAG
|
||||||
|
VAG_004 ---> VAG
|
||||||
|
HKG_001 ---> HKG[pre-hkg]
|
||||||
|
HONDA_001 ---> HONDA[pre-honda]
|
||||||
|
SUBARU_001 ---> SUBARU[pre-subaru]
|
||||||
|
TOYOTA ---> PRE[pre]
|
||||||
|
VAG ---> PRE
|
||||||
|
HKG ---> PRE
|
||||||
|
HONDA ---> PRE
|
||||||
|
SUBARU ---> PRE
|
||||||
|
PRE ---> PRE_PATCH[pre-patch]
|
||||||
|
PRE_PATCH ---> PREBUILD[pre-build]
|
||||||
|
PREBUILD ---> VERSION[x.x.x]
|
||||||
|
```
|
||||||
@@ -1,38 +1,14 @@
|
|||||||
FROM ubuntu:24.04
|
FROM ghcr.io/commaai/openpilot-base:latest
|
||||||
|
|
||||||
ENV PYTHONUNBUFFERED=1
|
ENV PYTHONUNBUFFERED=1
|
||||||
|
|
||||||
ENV DEBIAN_FRONTEND=noninteractive
|
ENV OPENPILOT_PATH=/home/batman/openpilot
|
||||||
RUN apt-get update && \
|
|
||||||
apt-get install -y --no-install-recommends sudo tzdata locales && \
|
|
||||||
rm -rf /var/lib/apt/lists/*
|
|
||||||
|
|
||||||
RUN sed -i -e 's/# en_US.UTF-8 UTF-8/en_US.UTF-8 UTF-8/' /etc/locale.gen && locale-gen
|
|
||||||
ENV LANG=en_US.UTF-8
|
|
||||||
ENV LANGUAGE=en_US:en
|
|
||||||
ENV LC_ALL=en_US.UTF-8
|
|
||||||
|
|
||||||
ENV NVIDIA_VISIBLE_DEVICES=all
|
|
||||||
ENV NVIDIA_DRIVER_CAPABILITIES=graphics,utility,compute
|
|
||||||
|
|
||||||
ARG USER=batman
|
|
||||||
ARG USER_UID=1001
|
|
||||||
RUN useradd -m -s /bin/bash -u $USER_UID $USER
|
|
||||||
RUN usermod -aG sudo $USER
|
|
||||||
RUN echo '%sudo ALL=(ALL) NOPASSWD:ALL' >> /etc/sudoers
|
|
||||||
USER $USER
|
|
||||||
|
|
||||||
ENV OPENPILOT_PATH=/home/$USER/openpilot
|
|
||||||
RUN mkdir -p ${OPENPILOT_PATH}
|
RUN mkdir -p ${OPENPILOT_PATH}
|
||||||
WORKDIR ${OPENPILOT_PATH}
|
WORKDIR ${OPENPILOT_PATH}
|
||||||
|
|
||||||
COPY --chown=$USER . ${OPENPILOT_PATH}/
|
COPY . ${OPENPILOT_PATH}/
|
||||||
|
|
||||||
ENV UV_BIN="/home/$USER/.local/bin/"
|
ENV UV_BIN="/home/batman/.local/bin/"
|
||||||
ENV VIRTUAL_ENV=${OPENPILOT_PATH}/.venv
|
ENV PATH="$UV_BIN:$PATH"
|
||||||
ENV PATH="$UV_BIN:$VIRTUAL_ENV/bin:$PATH"
|
RUN UV_PROJECT_ENVIRONMENT=$VIRTUAL_ENV uv run scons --cache-readonly -j$(nproc)
|
||||||
RUN tools/setup_dependencies.sh && \
|
|
||||||
sudo rm -rf /var/lib/apt/lists/*
|
|
||||||
|
|
||||||
USER root
|
|
||||||
RUN git config --global --add safe.directory '*'
|
|
||||||
|
|||||||
@@ -0,0 +1,81 @@
|
|||||||
|
FROM ubuntu:24.04
|
||||||
|
|
||||||
|
ENV PYTHONUNBUFFERED=1
|
||||||
|
|
||||||
|
ENV DEBIAN_FRONTEND=noninteractive
|
||||||
|
RUN apt-get update && \
|
||||||
|
apt-get install -y --no-install-recommends sudo tzdata locales ssh pulseaudio xvfb x11-xserver-utils gnome-screenshot python3-tk python3-dev && \
|
||||||
|
rm -rf /var/lib/apt/lists/*
|
||||||
|
|
||||||
|
RUN sed -i -e 's/# en_US.UTF-8 UTF-8/en_US.UTF-8 UTF-8/' /etc/locale.gen && locale-gen
|
||||||
|
ENV LANG=en_US.UTF-8
|
||||||
|
ENV LANGUAGE=en_US:en
|
||||||
|
ENV LC_ALL=en_US.UTF-8
|
||||||
|
|
||||||
|
COPY tools/install_ubuntu_dependencies.sh /tmp/tools/
|
||||||
|
RUN /tmp/tools/install_ubuntu_dependencies.sh && \
|
||||||
|
rm -rf /var/lib/apt/lists/* /tmp/* && \
|
||||||
|
cd /usr/lib/gcc/arm-none-eabi/* && \
|
||||||
|
rm -rf arm/ thumb/nofp thumb/v6* thumb/v8* thumb/v7+fp thumb/v7-r+fp.sp
|
||||||
|
|
||||||
|
# Add OpenCL
|
||||||
|
RUN apt-get update && apt-get install -y --no-install-recommends \
|
||||||
|
apt-utils \
|
||||||
|
alien \
|
||||||
|
unzip \
|
||||||
|
tar \
|
||||||
|
curl \
|
||||||
|
xz-utils \
|
||||||
|
dbus \
|
||||||
|
gcc-arm-none-eabi \
|
||||||
|
tmux \
|
||||||
|
vim \
|
||||||
|
libx11-6 \
|
||||||
|
wget \
|
||||||
|
&& rm -rf /var/lib/apt/lists/*
|
||||||
|
|
||||||
|
RUN mkdir -p /tmp/opencl-driver-intel && \
|
||||||
|
cd /tmp/opencl-driver-intel && \
|
||||||
|
wget https://github.com/intel/llvm/releases/download/2024-WW14/oclcpuexp-2024.17.3.0.09_rel.tar.gz && \
|
||||||
|
wget https://github.com/oneapi-src/oneTBB/releases/download/v2021.12.0/oneapi-tbb-2021.12.0-lin.tgz && \
|
||||||
|
mkdir -p /opt/intel/oclcpuexp_2024.17.3.0.09_rel && \
|
||||||
|
cd /opt/intel/oclcpuexp_2024.17.3.0.09_rel && \
|
||||||
|
tar -zxvf /tmp/opencl-driver-intel/oclcpuexp-2024.17.3.0.09_rel.tar.gz && \
|
||||||
|
mkdir -p /etc/OpenCL/vendors && \
|
||||||
|
echo /opt/intel/oclcpuexp_2024.17.3.0.09_rel/x64/libintelocl.so > /etc/OpenCL/vendors/intel_expcpu.icd && \
|
||||||
|
cd /opt/intel && \
|
||||||
|
tar -zxvf /tmp/opencl-driver-intel/oneapi-tbb-2021.12.0-lin.tgz && \
|
||||||
|
ln -s /opt/intel/oneapi-tbb-2021.12.0/lib/intel64/gcc4.8/libtbb.so /opt/intel/oclcpuexp_2024.17.3.0.09_rel/x64 && \
|
||||||
|
ln -s /opt/intel/oneapi-tbb-2021.12.0/lib/intel64/gcc4.8/libtbbmalloc.so /opt/intel/oclcpuexp_2024.17.3.0.09_rel/x64 && \
|
||||||
|
ln -s /opt/intel/oneapi-tbb-2021.12.0/lib/intel64/gcc4.8/libtbb.so.12 /opt/intel/oclcpuexp_2024.17.3.0.09_rel/x64 && \
|
||||||
|
ln -s /opt/intel/oneapi-tbb-2021.12.0/lib/intel64/gcc4.8/libtbbmalloc.so.2 /opt/intel/oclcpuexp_2024.17.3.0.09_rel/x64 && \
|
||||||
|
mkdir -p /etc/ld.so.conf.d && \
|
||||||
|
echo /opt/intel/oclcpuexp_2024.17.3.0.09_rel/x64 > /etc/ld.so.conf.d/libintelopenclexp.conf && \
|
||||||
|
ldconfig -f /etc/ld.so.conf.d/libintelopenclexp.conf && \
|
||||||
|
cd / && \
|
||||||
|
rm -rf /tmp/opencl-driver-intel
|
||||||
|
|
||||||
|
ENV NVIDIA_VISIBLE_DEVICES=all
|
||||||
|
ENV NVIDIA_DRIVER_CAPABILITIES=graphics,utility,compute
|
||||||
|
ENV QTWEBENGINE_DISABLE_SANDBOX=1
|
||||||
|
|
||||||
|
RUN dbus-uuidgen > /etc/machine-id
|
||||||
|
|
||||||
|
ARG USER=batman
|
||||||
|
ARG USER_UID=1001
|
||||||
|
RUN useradd -m -s /bin/bash -u $USER_UID $USER
|
||||||
|
RUN usermod -aG sudo $USER
|
||||||
|
RUN echo '%sudo ALL=(ALL) NOPASSWD:ALL' >> /etc/sudoers
|
||||||
|
USER $USER
|
||||||
|
|
||||||
|
COPY --chown=$USER pyproject.toml uv.lock /home/$USER
|
||||||
|
COPY --chown=$USER tools/install_python_dependencies.sh /home/$USER/tools/
|
||||||
|
|
||||||
|
ENV VIRTUAL_ENV=/home/$USER/.venv
|
||||||
|
ENV PATH="$VIRTUAL_ENV/bin:$PATH"
|
||||||
|
RUN cd /home/$USER && \
|
||||||
|
tools/install_python_dependencies.sh && \
|
||||||
|
rm -rf tools/ pyproject.toml uv.lock .cache
|
||||||
|
|
||||||
|
USER root
|
||||||
|
RUN sudo git config --global --add safe.directory /tmp/openpilot
|
||||||
@@ -22,7 +22,7 @@ shopt -s huponexit # kill all child processes when the shell exits
|
|||||||
|
|
||||||
export CI=1
|
export CI=1
|
||||||
export PYTHONWARNINGS=error
|
export PYTHONWARNINGS=error
|
||||||
#export LOGPRINT=debug # this has gotten too spammy...
|
export LOGPRINT=debug
|
||||||
export TEST_DIR=${env.TEST_DIR}
|
export TEST_DIR=${env.TEST_DIR}
|
||||||
export SOURCE_DIR=${env.SOURCE_DIR}
|
export SOURCE_DIR=${env.SOURCE_DIR}
|
||||||
export GIT_BRANCH=${env.GIT_BRANCH}
|
export GIT_BRANCH=${env.GIT_BRANCH}
|
||||||
@@ -210,23 +210,30 @@ node {
|
|||||||
'HW + Unit Tests': {
|
'HW + Unit Tests': {
|
||||||
deviceStage("tizi-hardware", "tizi-common", ["UNSAFE=1"], [
|
deviceStage("tizi-hardware", "tizi-common", ["UNSAFE=1"], [
|
||||||
step("build", "cd system/manager && ./build.py"),
|
step("build", "cd system/manager && ./build.py"),
|
||||||
|
step("test pandad", "pytest selfdrive/pandad/tests/test_pandad.py", [diffPaths: ["panda", "selfdrive/pandad/"]]),
|
||||||
step("test power draw", "pytest -s system/hardware/tici/tests/test_power_draw.py"),
|
step("test power draw", "pytest -s system/hardware/tici/tests/test_power_draw.py"),
|
||||||
step("test encoder", "LD_LIBRARY_PATH=/usr/local/lib pytest system/loggerd/tests/test_encoder.py", [diffPaths: ["system/loggerd/"]]),
|
step("test encoder", "LD_LIBRARY_PATH=/usr/local/lib pytest system/loggerd/tests/test_encoder.py", [diffPaths: ["system/loggerd/"]]),
|
||||||
step("test manager", "pytest system/manager/test/test_manager.py"),
|
step("test manager", "pytest system/manager/test/test_manager.py"),
|
||||||
])
|
])
|
||||||
},
|
},
|
||||||
|
'loopback': {
|
||||||
|
deviceStage("loopback", "tizi-loopback", ["UNSAFE=1"], [
|
||||||
|
step("build openpilot", "cd system/manager && ./build.py"),
|
||||||
|
step("test pandad loopback", "pytest selfdrive/pandad/tests/test_pandad_loopback.py"),
|
||||||
|
])
|
||||||
|
},
|
||||||
'camerad OX03C10': {
|
'camerad OX03C10': {
|
||||||
deviceStage("OX03C10", "tizi-ox03c10", ["UNSAFE=1"], [
|
deviceStage("OX03C10", "tizi-ox03c10", ["UNSAFE=1"], [
|
||||||
step("build", "cd system/manager && ./build.py"),
|
step("build", "cd system/manager && ./build.py"),
|
||||||
step("test pandad", "pytest selfdrive/pandad/tests/test_pandad.py"),
|
step("test camerad", "pytest system/camerad/test/test_camerad.py", [timeout: 60]),
|
||||||
step("test camerad", "pytest system/camerad/test/test_camerad.py", [timeout: 90]),
|
step("test exposure", "pytest system/camerad/test/test_exposure.py"),
|
||||||
])
|
])
|
||||||
},
|
},
|
||||||
'camerad OS04C10': {
|
'camerad OS04C10': {
|
||||||
deviceStage("OS04C10", "tici-os04c10", ["UNSAFE=1"], [
|
deviceStage("OS04C10", "tici-os04c10", ["UNSAFE=1"], [
|
||||||
step("build", "cd system/manager && ./build.py"),
|
step("build", "cd system/manager && ./build.py"),
|
||||||
step("test pandad", "pytest selfdrive/pandad/tests/test_pandad.py"),
|
step("test camerad", "pytest system/camerad/test/test_camerad.py", [timeout: 60]),
|
||||||
step("test camerad", "pytest system/camerad/test/test_camerad.py", [timeout: 90]),
|
step("test exposure", "pytest system/camerad/test/test_exposure.py"),
|
||||||
])
|
])
|
||||||
},
|
},
|
||||||
'sensord': {
|
'sensord': {
|
||||||
@@ -244,9 +251,11 @@ node {
|
|||||||
'tizi': {
|
'tizi': {
|
||||||
deviceStage("tizi", "tizi", ["UNSAFE=1"], [
|
deviceStage("tizi", "tizi", ["UNSAFE=1"], [
|
||||||
step("build openpilot", "cd system/manager && ./build.py"),
|
step("build openpilot", "cd system/manager && ./build.py"),
|
||||||
step("test pandad loopback", "pytest selfdrive/pandad/tests/test_pandad_loopback.py"),
|
step("test pandad loopback", "SINGLE_PANDA=1 pytest selfdrive/pandad/tests/test_pandad_loopback.py"),
|
||||||
step("test pandad spi", "pytest selfdrive/pandad/tests/test_pandad_spi.py"),
|
step("test pandad spi", "pytest selfdrive/pandad/tests/test_pandad_spi.py"),
|
||||||
step("test amp", "pytest system/hardware/tici/tests/test_amplifier.py"),
|
step("test amp", "pytest system/hardware/tici/tests/test_amplifier.py"),
|
||||||
|
// TODO: enable once new AGNOS is available
|
||||||
|
// step("test esim", "pytest system/hardware/tici/tests/test_esim.py"),
|
||||||
step("test qcomgpsd", "pytest system/qcomgpsd/tests/test_qcomgpsd.py", [diffPaths: ["system/qcomgpsd/"]]),
|
step("test qcomgpsd", "pytest system/qcomgpsd/tests/test_qcomgpsd.py", [diffPaths: ["system/qcomgpsd/"]]),
|
||||||
])
|
])
|
||||||
},
|
},
|
||||||
|
|||||||
@@ -1,12 +1,3 @@
|
|||||||
Version 0.11.0 (2026-03-17)
|
|
||||||
========================
|
|
||||||
* New driving model #36798
|
|
||||||
* Fully trained using a learned simulator
|
|
||||||
* Improved longitudinal performance in Experimental mode
|
|
||||||
* Reduce comma four standby power usage by 77% to 52 mW
|
|
||||||
* Kia K7 2017 support thanks to royjr!
|
|
||||||
* Lexus LS 2018 support thanks to Hacheoy!
|
|
||||||
|
|
||||||
Version 0.10.3 (2025-12-17)
|
Version 0.10.3 (2025-12-17)
|
||||||
========================
|
========================
|
||||||
* New driving model #36249
|
* New driving model #36249
|
||||||
|
|||||||
@@ -4,7 +4,6 @@ import sys
|
|||||||
import sysconfig
|
import sysconfig
|
||||||
import platform
|
import platform
|
||||||
import shlex
|
import shlex
|
||||||
import importlib
|
|
||||||
import numpy as np
|
import numpy as np
|
||||||
|
|
||||||
import SCons.Errors
|
import SCons.Errors
|
||||||
@@ -15,11 +14,11 @@ Decider('MD5-timestamp')
|
|||||||
|
|
||||||
SetOption('num_jobs', max(1, int(os.cpu_count()/2)))
|
SetOption('num_jobs', max(1, int(os.cpu_count()/2)))
|
||||||
|
|
||||||
|
AddOption('--kaitai', action='store_true', help='Regenerate kaitai struct parsers')
|
||||||
AddOption('--asan', action='store_true', help='turn on ASAN')
|
AddOption('--asan', action='store_true', help='turn on ASAN')
|
||||||
AddOption('--ubsan', action='store_true', help='turn on UBSan')
|
AddOption('--ubsan', action='store_true', help='turn on UBSan')
|
||||||
AddOption('--mutation', action='store_true', help='generate mutation-ready code')
|
AddOption('--mutation', action='store_true', help='generate mutation-ready code')
|
||||||
AddOption('--ccflags', action='store', type='string', default='', help='pass arbitrary flags over the command line')
|
AddOption('--ccflags', action='store', type='string', default='', help='pass arbitrary flags over the command line')
|
||||||
AddOption('--verbose', action='store_true', default=False, help='show full build commands')
|
|
||||||
AddOption('--minimal',
|
AddOption('--minimal',
|
||||||
action='store_false',
|
action='store_false',
|
||||||
dest='extras',
|
dest='extras',
|
||||||
@@ -30,6 +29,7 @@ AddOption('--minimal',
|
|||||||
arch = subprocess.check_output(["uname", "-m"], encoding='utf8').rstrip()
|
arch = subprocess.check_output(["uname", "-m"], encoding='utf8').rstrip()
|
||||||
if platform.system() == "Darwin":
|
if platform.system() == "Darwin":
|
||||||
arch = "Darwin"
|
arch = "Darwin"
|
||||||
|
brew_prefix = subprocess.check_output(['brew', '--prefix'], encoding='utf8').strip()
|
||||||
elif arch == "aarch64" and os.path.isfile('/TICI'):
|
elif arch == "aarch64" and os.path.isfile('/TICI'):
|
||||||
arch = "larch64"
|
arch = "larch64"
|
||||||
assert arch in [
|
assert arch in [
|
||||||
@@ -39,10 +39,6 @@ assert arch in [
|
|||||||
"Darwin", # macOS arm64 (x86 not supported)
|
"Darwin", # macOS arm64 (x86 not supported)
|
||||||
]
|
]
|
||||||
|
|
||||||
pkg_names = ['bzip2', 'capnproto', 'eigen', 'ffmpeg', 'libjpeg', 'libyuv', 'ncurses', 'zeromq', 'zstd']
|
|
||||||
pkgs = [importlib.import_module(name) for name in pkg_names]
|
|
||||||
py_include = importlib.import_module('python3_dev').INCLUDE_DIR
|
|
||||||
|
|
||||||
env = Environment(
|
env = Environment(
|
||||||
ENV={
|
ENV={
|
||||||
"PATH": os.environ['PATH'],
|
"PATH": os.environ['PATH'],
|
||||||
@@ -51,13 +47,15 @@ env = Environment(
|
|||||||
"ACADOS_PYTHON_INTERFACE_PATH": Dir("#third_party/acados/acados_template").abspath,
|
"ACADOS_PYTHON_INTERFACE_PATH": Dir("#third_party/acados/acados_template").abspath,
|
||||||
"TERA_PATH": Dir("#").abspath + f"/third_party/acados/{arch}/t_renderer"
|
"TERA_PATH": Dir("#").abspath + f"/third_party/acados/{arch}/t_renderer"
|
||||||
},
|
},
|
||||||
|
CC='clang',
|
||||||
|
CXX='clang++',
|
||||||
CCFLAGS=[
|
CCFLAGS=[
|
||||||
"-g",
|
"-g",
|
||||||
"-fPIC",
|
"-fPIC",
|
||||||
"-O2",
|
"-O2",
|
||||||
"-Wunused",
|
"-Wunused",
|
||||||
"-Werror",
|
"-Werror",
|
||||||
"-Wshadow" if arch in ("Darwin", "larch64") else "-Wshadow=local",
|
"-Wshadow",
|
||||||
"-Wno-unknown-warning-option",
|
"-Wno-unknown-warning-option",
|
||||||
"-Wno-inconsistent-missing-override",
|
"-Wno-inconsistent-missing-override",
|
||||||
"-Wno-c99-designator",
|
"-Wno-c99-designator",
|
||||||
@@ -76,16 +74,16 @@ env = Environment(
|
|||||||
"#third_party/acados/include/blasfeo/include",
|
"#third_party/acados/include/blasfeo/include",
|
||||||
"#third_party/acados/include/hpipm/include",
|
"#third_party/acados/include/hpipm/include",
|
||||||
"#third_party/catch2/include",
|
"#third_party/catch2/include",
|
||||||
[x.INCLUDE_DIR for x in pkgs],
|
"#third_party/libyuv/include",
|
||||||
],
|
],
|
||||||
LIBPATH=[
|
LIBPATH=[
|
||||||
"#common",
|
"#common",
|
||||||
"#msgq_repo",
|
"#msgq_repo",
|
||||||
"#third_party",
|
"#third_party",
|
||||||
"#selfdrive/pandad_tici" if "TICI_DOS" in os.environ else "#selfdrive/pandad",
|
"#selfdrive/pandad",
|
||||||
"#rednose/helpers",
|
"#rednose/helpers",
|
||||||
|
f"#third_party/libyuv/{arch}/lib",
|
||||||
f"#third_party/acados/{arch}/lib",
|
f"#third_party/acados/{arch}/lib",
|
||||||
[x.LIB_DIR for x in pkgs],
|
|
||||||
],
|
],
|
||||||
RPATH=[],
|
RPATH=[],
|
||||||
CYTHONCFILESUFFIX=".cpp",
|
CYTHONCFILESUFFIX=".cpp",
|
||||||
@@ -97,8 +95,7 @@ env = Environment(
|
|||||||
|
|
||||||
# Arch-specific flags and paths
|
# Arch-specific flags and paths
|
||||||
if arch == "larch64":
|
if arch == "larch64":
|
||||||
env["CC"] = "clang"
|
env.Append(CPPPATH=["#third_party/opencl/include"])
|
||||||
env["CXX"] = "clang++"
|
|
||||||
env.Append(LIBPATH=[
|
env.Append(LIBPATH=[
|
||||||
"/usr/local/lib",
|
"/usr/local/lib",
|
||||||
"/system/vendor/lib64",
|
"/system/vendor/lib64",
|
||||||
@@ -109,10 +106,17 @@ if arch == "larch64":
|
|||||||
env.Append(CXXFLAGS=arch_flags)
|
env.Append(CXXFLAGS=arch_flags)
|
||||||
elif arch == "Darwin":
|
elif arch == "Darwin":
|
||||||
env.Append(LIBPATH=[
|
env.Append(LIBPATH=[
|
||||||
|
f"{brew_prefix}/lib",
|
||||||
|
f"{brew_prefix}/opt/openssl@3.0/lib",
|
||||||
|
f"{brew_prefix}/opt/llvm/lib/c++",
|
||||||
"/System/Library/Frameworks/OpenGL.framework/Libraries",
|
"/System/Library/Frameworks/OpenGL.framework/Libraries",
|
||||||
])
|
])
|
||||||
env.Append(CCFLAGS=["-DGL_SILENCE_DEPRECATION"])
|
env.Append(CCFLAGS=["-DGL_SILENCE_DEPRECATION"])
|
||||||
env.Append(CXXFLAGS=["-DGL_SILENCE_DEPRECATION"])
|
env.Append(CXXFLAGS=["-DGL_SILENCE_DEPRECATION"])
|
||||||
|
env.Append(CPPPATH=[
|
||||||
|
f"{brew_prefix}/include",
|
||||||
|
f"{brew_prefix}/opt/openssl@3.0/include",
|
||||||
|
])
|
||||||
else:
|
else:
|
||||||
env.Append(LIBPATH=[
|
env.Append(LIBPATH=[
|
||||||
"/usr/lib",
|
"/usr/lib",
|
||||||
@@ -135,22 +139,6 @@ if _extra_cc:
|
|||||||
if arch != "Darwin":
|
if arch != "Darwin":
|
||||||
env.Append(LINKFLAGS=["-Wl,--as-needed", "-Wl,--no-undefined"])
|
env.Append(LINKFLAGS=["-Wl,--as-needed", "-Wl,--no-undefined"])
|
||||||
|
|
||||||
# Shorter build output: show brief descriptions instead of full commands.
|
|
||||||
# Full command lines are still printed on failure by scons.
|
|
||||||
if not GetOption('verbose'):
|
|
||||||
for action, short in (
|
|
||||||
("CC", "CC"),
|
|
||||||
("CXX", "CXX"),
|
|
||||||
("LINK", "LINK"),
|
|
||||||
("SHCC", "CC"),
|
|
||||||
("SHCXX", "CXX"),
|
|
||||||
("SHLINK", "LINK"),
|
|
||||||
("AR", "AR"),
|
|
||||||
("RANLIB", "RANLIB"),
|
|
||||||
("AS", "AS"),
|
|
||||||
):
|
|
||||||
env[f"{action}COMSTR"] = f" [{short}] $TARGET"
|
|
||||||
|
|
||||||
# progress output
|
# progress output
|
||||||
node_interval = 5
|
node_interval = 5
|
||||||
node_count = 0
|
node_count = 0
|
||||||
@@ -162,9 +150,10 @@ if os.environ.get('SCONS_PROGRESS'):
|
|||||||
Progress(progress_function, interval=node_interval)
|
Progress(progress_function, interval=node_interval)
|
||||||
|
|
||||||
# ********** Cython build environment **********
|
# ********** Cython build environment **********
|
||||||
|
py_include = sysconfig.get_paths()['include']
|
||||||
envCython = env.Clone()
|
envCython = env.Clone()
|
||||||
envCython["CPPPATH"] += [py_include, np.get_include()]
|
envCython["CPPPATH"] += [py_include, np.get_include()]
|
||||||
envCython["CCFLAGS"] += ["-Wno-#warnings", "-Wno-cpp", "-Wno-shadow", "-Wno-deprecated-declarations"]
|
envCython["CCFLAGS"] += ["-Wno-#warnings", "-Wno-shadow", "-Wno-deprecated-declarations"]
|
||||||
envCython["CCFLAGS"].remove("-Werror")
|
envCython["CCFLAGS"].remove("-Werror")
|
||||||
|
|
||||||
envCython["LIBS"] = []
|
envCython["LIBS"] = []
|
||||||
@@ -183,12 +172,6 @@ cache_dir = '/data/scons_cache' if arch == "larch64" else '/tmp/scons_cache'
|
|||||||
CacheDir(cache_dir)
|
CacheDir(cache_dir)
|
||||||
Clean(["."], cache_dir)
|
Clean(["."], cache_dir)
|
||||||
|
|
||||||
# dragonpilot settings generation — runs every scons invocation, idempotent.
|
|
||||||
# Writes common/params_keys.h in place; we don't declare a target so scons
|
|
||||||
# treats it purely as a pre-build side effect.
|
|
||||||
if env.Execute('./generate_settings.py') != 0:
|
|
||||||
Exit('generate_settings.py failed')
|
|
||||||
|
|
||||||
# ********** start building stuff **********
|
# ********** start building stuff **********
|
||||||
|
|
||||||
# Build common module
|
# Build common module
|
||||||
@@ -220,6 +203,7 @@ SConscript(['rednose/SConscript'])
|
|||||||
|
|
||||||
# Build system services
|
# Build system services
|
||||||
SConscript([
|
SConscript([
|
||||||
|
'system/ubloxd/SConscript',
|
||||||
'system/loggerd/SConscript',
|
'system/loggerd/SConscript',
|
||||||
])
|
])
|
||||||
|
|
||||||
@@ -231,8 +215,10 @@ SConscript(['third_party/SConscript'])
|
|||||||
|
|
||||||
SConscript(['selfdrive/SConscript'])
|
SConscript(['selfdrive/SConscript'])
|
||||||
|
|
||||||
if Dir('#tools/cabana/').exists() and arch != "larch64":
|
if Dir('#tools/cabana/').exists() and GetOption('extras'):
|
||||||
SConscript(['tools/cabana/SConscript'])
|
SConscript(['tools/replay/SConscript'])
|
||||||
|
if arch != "larch64":
|
||||||
|
SConscript(['tools/cabana/SConscript'])
|
||||||
|
|
||||||
|
|
||||||
env.CompilationDatabase('compile_commands.json')
|
env.CompilationDatabase('compile_commands.json')
|
||||||
|
|||||||
@@ -0,0 +1,26 @@
|
|||||||
|
# Sponsors 贊助者
|
||||||
|
|
||||||
|
我們誠摯感謝以下贊助者提供的硬體資源,讓專案能夠在多種平台上進行測試與驗證。
|
||||||
|
|
||||||
|
We sincerely thank the following sponsors for providing hardware resources, which enable the project to be tested and validated across multiple platforms.
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
## 贊助者列表 Sponsors
|
||||||
|
|
||||||
|
| 贊助者 Sponsor | 設備 Deices | 備註 Notes |
|
||||||
|
| -------------- | ----------- | ---------- |
|
||||||
|
| BlueGood | <ul><li>Radar Filter x 2</li><li>sDSU x1</li></ul> | - |
|
||||||
|
| Chia Chun Lee | <ul><li>C3 Quick Mount x1</li></ul> | - |
|
||||||
|
| CloudJ | <ul><li>C3X x1</li></ul> | - |
|
||||||
|
| FareWay | <ul><li>EON Quick Mount x1</li><li>C2/C3 Quick Mount x1</li></ul> | Special thanks for fixing my good old EON |
|
||||||
|
| Fred Wang | <ul><li>Oneplus 3t x1</li></ul> | - |
|
||||||
|
| Saber Huang | <ul><li>O3L x1</li></ul> | - |
|
||||||
|
| [馬威 Mr. One](https://shop61532546.taobao.com/) | <ul><li>O3 x 1</li><li>O3 (Dev) x1</li><li>O3XL x1</li><li>Red Panda x1</li><li>Panda Jungle v1 x1</li></ul> | - |
|
||||||
|
| 門文梁 | <ul><li>C1.5 x1</li></ul> | - |
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
🙏 沒有你們的支持,我們無法讓專案在這麼多硬體平台上持續成長與驗證。
|
||||||
|
|
||||||
|
Without your support, this project could not continue to grow and be validated across so many hardware platforms.
|
||||||
@@ -13,7 +13,7 @@ cereal = env.Library('cereal', [f'gen/cpp/{s}.c++' for s in schema_files])
|
|||||||
|
|
||||||
# Build messaging
|
# Build messaging
|
||||||
services_h = env.Command(['services.h'], ['services.py'], 'python3 ' + cereal_dir.path + '/services.py > $TARGET')
|
services_h = env.Command(['services.h'], ['services.py'], 'python3 ' + cereal_dir.path + '/services.py > $TARGET')
|
||||||
env.Program('messaging/bridge', ['messaging/bridge.cc', 'messaging/msgq_to_zmq.cc', 'messaging/bridge_zmq.cc'], LIBS=[msgq, common, 'pthread'])
|
env.Program('messaging/bridge', ['messaging/bridge.cc', 'messaging/msgq_to_zmq.cc'], LIBS=[msgq, common, 'pthread'])
|
||||||
|
|
||||||
socketmaster = env.Library('socketmaster', ['messaging/socketmaster.cc'])
|
socketmaster = env.Library('socketmaster', ['messaging/socketmaster.cc'])
|
||||||
|
|
||||||
|
|||||||
@@ -24,19 +24,87 @@ struct ModelExt @0xf35cc4560bbf6ec2 {
|
|||||||
rightEdgeDetected @1 :Bool;
|
rightEdgeDetected @1 :Bool;
|
||||||
}
|
}
|
||||||
|
|
||||||
struct DashyState @0xda96579883444c35 {
|
struct LiveGPS @0xda96579883444c35 {
|
||||||
|
# Position
|
||||||
|
latitude @0 :Float64; # degrees
|
||||||
|
longitude @1 :Float64; # degrees
|
||||||
|
altitude @2 :Float64; # meters (WGS84)
|
||||||
|
|
||||||
|
# Motion
|
||||||
|
speed @3 :Float32; # m/s (horizontal speed)
|
||||||
|
bearingDeg @4 :Float32; # degrees (heading)
|
||||||
|
|
||||||
|
# Accuracy
|
||||||
|
horizontalAccuracy @5 :Float32; # meters
|
||||||
|
verticalAccuracy @6 :Float32; # meters
|
||||||
|
|
||||||
|
# Status
|
||||||
|
gpsOK @7 :Bool; # livePose valid + GPS fresh
|
||||||
|
status @8 :Status;
|
||||||
|
|
||||||
|
enum Status {
|
||||||
|
uninitialized @0; # no GPS data yet
|
||||||
|
uncalibrated @1; # has GPS but fusion not ready (raw passthrough)
|
||||||
|
valid @2; # fusion active with calibrated bearing
|
||||||
|
}
|
||||||
|
|
||||||
|
# Metadata
|
||||||
|
unixTimestampMillis @9 :Int64;
|
||||||
|
lastGpsTimestamp @10 :UInt64; # logMonoTime of last GPS
|
||||||
|
|
||||||
|
# livePose health (for debugging fusion issues)
|
||||||
|
livePoseOk @11 :Bool; # livePose valid and providing orientation/velocity
|
||||||
|
}
|
||||||
|
|
||||||
|
struct MaaControl @0x80ae746ee2596b11 {
|
||||||
|
# Map-Aware Assist control signals
|
||||||
|
|
||||||
|
# Curvature data (for lateral control)
|
||||||
|
curvature @0 :Float32; # current nav curvature (1/m)
|
||||||
|
curvatureValid @1 :Bool; # curvature data is valid
|
||||||
|
|
||||||
|
# Turn speed data (for longitudinal control)
|
||||||
|
turnSpeedLimit @2 :Float32; # target speed for turn (m/s)
|
||||||
|
turnDistance @3 :Float32; # distance to turn (m)
|
||||||
|
turnDirection @4 :TurnDirection;
|
||||||
|
turnValid @5 :Bool; # turn data is valid
|
||||||
|
maneuverType @6 :ManeuverType; # type of maneuver (turn vs lane change)
|
||||||
|
turnAngle @7 :Float32; # expected turn angle in degrees (positive=left, negative=right)
|
||||||
|
turnCurvature @8 :Float32; # curvature at turn point (1/m), used for speed limit calc
|
||||||
|
|
||||||
|
# Turn execution (heading-based tracking)
|
||||||
|
desireActive @9 :Bool; # true when turn desire should be sent to model
|
||||||
|
turnState @10 :UInt8; # TurnState enum: 0=none, 1=approaching, 2=executing, 3=complete
|
||||||
|
turnProgress @11 :Float32; # accumulated heading change during turn (degrees)
|
||||||
|
|
||||||
|
# Driver acknowledgment (blinker = commit to turn)
|
||||||
|
driverAcknowledged @12 :Bool; # driver turned on blinker matching turn direction
|
||||||
|
speedLimitActive @13 :Bool; # turn speed limit should be enforced (blinker on)
|
||||||
|
blockLaneChange @14 :Bool; # within commit distance, block lane change desire
|
||||||
|
|
||||||
|
enum TurnDirection {
|
||||||
|
none @0;
|
||||||
|
left @1;
|
||||||
|
right @2;
|
||||||
|
}
|
||||||
|
|
||||||
|
enum ManeuverType {
|
||||||
|
none @0;
|
||||||
|
turn @1; # intersection turn - use turnLeft/Right desire
|
||||||
|
laneChange @2; # highway exit/fork - use laneChangeLeft/Right desire
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
struct DashyState @0xa5cd762cd951a455 {
|
||||||
# Pre-serialized JSON bytes for dashy UI
|
# Pre-serialized JSON bytes for dashy UI
|
||||||
# Aggregates all topics needed by dashy into single message
|
# Aggregates all topics needed by dashy into single message
|
||||||
json @0 :Data;
|
json @0 :Data;
|
||||||
}
|
}
|
||||||
|
|
||||||
struct CustomReserved4 @0x80ae746ee2596b11 {
|
struct NavInstructionExt @0xf98d843bfd7004a3 {
|
||||||
}
|
# Extension fields for NavInstruction (not in upstream)
|
||||||
|
turnAngle @0 :Float32; # degrees, positive=left, negative=right
|
||||||
struct CustomReserved5 @0xa5cd762cd951a455 {
|
turnCurvature @1 :Float32; # 1/m, positive=left, negative=right
|
||||||
}
|
|
||||||
|
|
||||||
struct CustomReserved6 @0xf98d843bfd7004a3 {
|
|
||||||
}
|
}
|
||||||
|
|
||||||
struct CustomReserved7 @0xb86e6369214c01c8 {
|
struct CustomReserved7 @0xb86e6369214c01c8 {
|
||||||
|
|||||||
@@ -87,7 +87,6 @@ struct OnroadEvent @0xc4fa6047f024e718 {
|
|||||||
laneChange @50;
|
laneChange @50;
|
||||||
lowMemory @51;
|
lowMemory @51;
|
||||||
stockAeb @52;
|
stockAeb @52;
|
||||||
stockLkas @98;
|
|
||||||
ldw @53;
|
ldw @53;
|
||||||
carUnrecognized @54;
|
carUnrecognized @54;
|
||||||
invalidLkasSetting @55;
|
invalidLkasSetting @55;
|
||||||
@@ -499,8 +498,7 @@ struct DeviceState @0xa4d8b5af2aa492eb {
|
|||||||
pmicTempC @39 :List(Float32);
|
pmicTempC @39 :List(Float32);
|
||||||
intakeTempC @46 :Float32;
|
intakeTempC @46 :Float32;
|
||||||
exhaustTempC @47 :Float32;
|
exhaustTempC @47 :Float32;
|
||||||
gnssTempC @48 :Float32;
|
caseTempC @48 :Float32;
|
||||||
bottomSocTempC @50 :Float32;
|
|
||||||
maxTempC @44 :Float32; # max of other temps, used to control fan
|
maxTempC @44 :Float32; # max of other temps, used to control fan
|
||||||
thermalZones @38 :List(ThermalZone);
|
thermalZones @38 :List(ThermalZone);
|
||||||
thermalStatus @14 :ThermalStatus;
|
thermalStatus @14 :ThermalStatus;
|
||||||
@@ -593,7 +591,6 @@ struct PandaState @0xa7649e2575e4591e {
|
|||||||
harnessStatus @21 :HarnessStatus;
|
harnessStatus @21 :HarnessStatus;
|
||||||
sbu1Voltage @35 :Float32;
|
sbu1Voltage @35 :Float32;
|
||||||
sbu2Voltage @36 :Float32;
|
sbu2Voltage @36 :Float32;
|
||||||
soundOutputLevel @37 :UInt16;
|
|
||||||
|
|
||||||
# can health
|
# can health
|
||||||
canState0 @29 :PandaCanState;
|
canState0 @29 :PandaCanState;
|
||||||
@@ -1480,11 +1477,6 @@ struct ProcLog {
|
|||||||
|
|
||||||
cmdline @15 :List(Text);
|
cmdline @15 :List(Text);
|
||||||
exe @16 :Text;
|
exe @16 :Text;
|
||||||
|
|
||||||
# from /proc/<pid>/smaps_rollup (proportional/private memory)
|
|
||||||
memPss @17 :UInt64; # Pss — shared pages split by mapper count
|
|
||||||
memPssAnon @18 :UInt64; # Pss_Anon — private anonymous (heap, stack)
|
|
||||||
memPssShmem @19 :UInt64; # Pss_Shmem — proportional MSGQ/tmpfs share
|
|
||||||
}
|
}
|
||||||
|
|
||||||
struct CPUTimes {
|
struct CPUTimes {
|
||||||
@@ -2636,10 +2628,10 @@ struct Event {
|
|||||||
controlsStateExt @107 :Custom.ControlsStateExt;
|
controlsStateExt @107 :Custom.ControlsStateExt;
|
||||||
carStateExt @108 :Custom.CarStateExt;
|
carStateExt @108 :Custom.CarStateExt;
|
||||||
modelExt @109 :Custom.ModelExt;
|
modelExt @109 :Custom.ModelExt;
|
||||||
dashyState @110 :Custom.DashyState;
|
liveGPS @110 :Custom.LiveGPS;
|
||||||
customReserved4 @111 :Custom.CustomReserved4;
|
maaControl @111 :Custom.MaaControl;
|
||||||
customReserved5 @112 :Custom.CustomReserved5;
|
dashyState @112 :Custom.DashyState;
|
||||||
customReserved6 @113 :Custom.CustomReserved6;
|
navInstructionExt @113 :Custom.NavInstructionExt;
|
||||||
customReserved7 @114 :Custom.CustomReserved7;
|
customReserved7 @114 :Custom.CustomReserved7;
|
||||||
customReserved8 @115 :Custom.CustomReserved8;
|
customReserved8 @115 :Custom.CustomReserved8;
|
||||||
customReserved9 @116 :Custom.CustomReserved9;
|
customReserved9 @116 :Custom.CustomReserved9;
|
||||||
|
|||||||
@@ -1,8 +1,10 @@
|
|||||||
# must be built with scons
|
# must be built with scons
|
||||||
from msgq import fake_event_handle, drain_sock_raw, MultiplePublishersError, IpcError, \
|
from msgq.ipc_pyx import Context, Poller, SubSocket, PubSocket, SocketEventHandle, toggle_fake_events, \
|
||||||
Context, Poller, SubSocket, PubSocket, SocketEventHandle, toggle_fake_events, \
|
set_fake_prefix, get_fake_prefix, delete_fake_prefix, wait_for_one_event
|
||||||
set_fake_prefix, get_fake_prefix, delete_fake_prefix, wait_for_one_event
|
from msgq.ipc_pyx import MultiplePublishersError, IpcError
|
||||||
|
from msgq import fake_event_handle, drain_sock_raw
|
||||||
import msgq
|
import msgq
|
||||||
|
|
||||||
import os
|
import os
|
||||||
import capnp
|
import capnp
|
||||||
import time
|
import time
|
||||||
@@ -11,7 +13,7 @@ from typing import Optional, List, Union, Dict
|
|||||||
|
|
||||||
from cereal import log
|
from cereal import log
|
||||||
from cereal.services import SERVICE_LIST
|
from cereal.services import SERVICE_LIST
|
||||||
from openpilot.common.utils import MovingAverage
|
from openpilot.common.util import MovingAverage
|
||||||
|
|
||||||
NO_TRAVERSAL_LIMIT = 2**64-1
|
NO_TRAVERSAL_LIMIT = 2**64-1
|
||||||
|
|
||||||
|
|||||||
@@ -25,16 +25,15 @@ void msgq_to_zmq(const std::vector<std::string> &endpoints, const std::string &i
|
|||||||
}
|
}
|
||||||
|
|
||||||
void zmq_to_msgq(const std::vector<std::string> &endpoints, const std::string &ip) {
|
void zmq_to_msgq(const std::vector<std::string> &endpoints, const std::string &ip) {
|
||||||
auto poller = std::make_unique<BridgeZmqPoller>();
|
auto poller = std::make_unique<ZMQPoller>();
|
||||||
auto pub_context = std::make_unique<Context>();
|
auto pub_context = std::make_unique<MSGQContext>();
|
||||||
auto sub_context = std::make_unique<BridgeZmqContext>();
|
auto sub_context = std::make_unique<ZMQContext>();
|
||||||
std::map<BridgeZmqSubSocket *, PubSocket *> sub2pub;
|
std::map<SubSocket *, PubSocket *> sub2pub;
|
||||||
|
|
||||||
for (auto endpoint : endpoints) {
|
for (auto endpoint : endpoints) {
|
||||||
auto pub_sock = new PubSocket();
|
auto pub_sock = new MSGQPubSocket();
|
||||||
auto sub_sock = new BridgeZmqSubSocket();
|
auto sub_sock = new ZMQSubSocket();
|
||||||
size_t queue_size = services.at(endpoint).queue_size;
|
pub_sock->connect(pub_context.get(), endpoint);
|
||||||
pub_sock->connect(pub_context.get(), endpoint, true, queue_size);
|
|
||||||
sub_sock->connect(sub_context.get(), endpoint, ip, false);
|
sub_sock->connect(sub_context.get(), endpoint, ip, false);
|
||||||
|
|
||||||
poller->registerSocket(sub_sock);
|
poller->registerSocket(sub_sock);
|
||||||
|
|||||||
@@ -1,170 +0,0 @@
|
|||||||
#include "cereal/messaging/bridge_zmq.h"
|
|
||||||
|
|
||||||
#include <cassert>
|
|
||||||
#include <cstring>
|
|
||||||
#include <unistd.h>
|
|
||||||
|
|
||||||
static size_t fnv1a_hash(const std::string &str) {
|
|
||||||
const size_t fnv_prime = 0x100000001b3;
|
|
||||||
size_t hash_value = 0xcbf29ce484222325;
|
|
||||||
for (char c : str) {
|
|
||||||
hash_value ^= (unsigned char)c;
|
|
||||||
hash_value *= fnv_prime;
|
|
||||||
}
|
|
||||||
return hash_value;
|
|
||||||
}
|
|
||||||
|
|
||||||
// FIXME: This is a hack to get the port number from the socket name, might have collisions.
|
|
||||||
static int get_port(std::string endpoint) {
|
|
||||||
size_t hash_value = fnv1a_hash(endpoint);
|
|
||||||
int start_port = 8023;
|
|
||||||
int max_port = 65535;
|
|
||||||
return start_port + (hash_value % (max_port - start_port));
|
|
||||||
}
|
|
||||||
|
|
||||||
BridgeZmqContext::BridgeZmqContext() {
|
|
||||||
context = zmq_ctx_new();
|
|
||||||
}
|
|
||||||
|
|
||||||
BridgeZmqContext::~BridgeZmqContext() {
|
|
||||||
if (context != nullptr) {
|
|
||||||
zmq_ctx_term(context);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
void BridgeZmqMessage::init(size_t sz) {
|
|
||||||
size = sz;
|
|
||||||
data = new char[size];
|
|
||||||
}
|
|
||||||
|
|
||||||
void BridgeZmqMessage::init(char *d, size_t sz) {
|
|
||||||
size = sz;
|
|
||||||
data = new char[size];
|
|
||||||
memcpy(data, d, size);
|
|
||||||
}
|
|
||||||
|
|
||||||
void BridgeZmqMessage::close() {
|
|
||||||
if (size > 0) {
|
|
||||||
delete[] data;
|
|
||||||
}
|
|
||||||
data = nullptr;
|
|
||||||
size = 0;
|
|
||||||
}
|
|
||||||
|
|
||||||
BridgeZmqMessage::~BridgeZmqMessage() {
|
|
||||||
close();
|
|
||||||
}
|
|
||||||
|
|
||||||
int BridgeZmqSubSocket::connect(BridgeZmqContext *context, std::string endpoint, std::string address, bool conflate, bool check_endpoint) {
|
|
||||||
sock = zmq_socket(context->getRawContext(), ZMQ_SUB);
|
|
||||||
if (sock == nullptr) {
|
|
||||||
return -1;
|
|
||||||
}
|
|
||||||
|
|
||||||
zmq_setsockopt(sock, ZMQ_SUBSCRIBE, "", 0);
|
|
||||||
|
|
||||||
if (conflate) {
|
|
||||||
int arg = 1;
|
|
||||||
zmq_setsockopt(sock, ZMQ_CONFLATE, &arg, sizeof(int));
|
|
||||||
}
|
|
||||||
|
|
||||||
int reconnect_ivl = 500;
|
|
||||||
zmq_setsockopt(sock, ZMQ_RECONNECT_IVL_MAX, &reconnect_ivl, sizeof(reconnect_ivl));
|
|
||||||
|
|
||||||
full_endpoint = "tcp://" + address + ":";
|
|
||||||
if (check_endpoint) {
|
|
||||||
full_endpoint += std::to_string(get_port(endpoint));
|
|
||||||
} else {
|
|
||||||
full_endpoint += endpoint;
|
|
||||||
}
|
|
||||||
|
|
||||||
return zmq_connect(sock, full_endpoint.c_str());
|
|
||||||
}
|
|
||||||
|
|
||||||
void BridgeZmqSubSocket::setTimeout(int timeout) {
|
|
||||||
zmq_setsockopt(sock, ZMQ_RCVTIMEO, &timeout, sizeof(int));
|
|
||||||
}
|
|
||||||
|
|
||||||
Message *BridgeZmqSubSocket::receive(bool non_blocking) {
|
|
||||||
zmq_msg_t msg;
|
|
||||||
assert(zmq_msg_init(&msg) == 0);
|
|
||||||
|
|
||||||
int flags = non_blocking ? ZMQ_DONTWAIT : 0;
|
|
||||||
int rc = zmq_msg_recv(&msg, sock, flags);
|
|
||||||
|
|
||||||
Message *ret = nullptr;
|
|
||||||
if (rc >= 0) {
|
|
||||||
ret = new BridgeZmqMessage;
|
|
||||||
ret->init((char *)zmq_msg_data(&msg), zmq_msg_size(&msg));
|
|
||||||
}
|
|
||||||
|
|
||||||
zmq_msg_close(&msg);
|
|
||||||
return ret;
|
|
||||||
}
|
|
||||||
|
|
||||||
BridgeZmqSubSocket::~BridgeZmqSubSocket() {
|
|
||||||
if (sock != nullptr) {
|
|
||||||
zmq_close(sock);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
int BridgeZmqPubSocket::connect(BridgeZmqContext *context, std::string endpoint, bool check_endpoint) {
|
|
||||||
sock = zmq_socket(context->getRawContext(), ZMQ_PUB);
|
|
||||||
if (sock == nullptr) {
|
|
||||||
return -1;
|
|
||||||
}
|
|
||||||
|
|
||||||
full_endpoint = "tcp://*:";
|
|
||||||
if (check_endpoint) {
|
|
||||||
full_endpoint += std::to_string(get_port(endpoint));
|
|
||||||
} else {
|
|
||||||
full_endpoint += endpoint;
|
|
||||||
}
|
|
||||||
|
|
||||||
// ZMQ pub sockets cannot be shared between processes, so we need to ensure pid stays the same.
|
|
||||||
pid = getpid();
|
|
||||||
|
|
||||||
return zmq_bind(sock, full_endpoint.c_str());
|
|
||||||
}
|
|
||||||
|
|
||||||
int BridgeZmqPubSocket::sendMessage(Message *message) {
|
|
||||||
assert(pid == getpid());
|
|
||||||
return zmq_send(sock, message->getData(), message->getSize(), ZMQ_DONTWAIT);
|
|
||||||
}
|
|
||||||
|
|
||||||
int BridgeZmqPubSocket::send(char *data, size_t size) {
|
|
||||||
assert(pid == getpid());
|
|
||||||
return zmq_send(sock, data, size, ZMQ_DONTWAIT);
|
|
||||||
}
|
|
||||||
|
|
||||||
BridgeZmqPubSocket::~BridgeZmqPubSocket() {
|
|
||||||
if (sock != nullptr) {
|
|
||||||
zmq_close(sock);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
void BridgeZmqPoller::registerSocket(BridgeZmqSubSocket *socket) {
|
|
||||||
assert(num_polls + 1 < (sizeof(polls) / sizeof(polls[0])));
|
|
||||||
polls[num_polls].socket = socket->getRawSocket();
|
|
||||||
polls[num_polls].events = ZMQ_POLLIN;
|
|
||||||
|
|
||||||
sockets.push_back(socket);
|
|
||||||
num_polls++;
|
|
||||||
}
|
|
||||||
|
|
||||||
std::vector<BridgeZmqSubSocket *> BridgeZmqPoller::poll(int timeout) {
|
|
||||||
std::vector<BridgeZmqSubSocket *> ret;
|
|
||||||
|
|
||||||
int rc = zmq_poll(polls, num_polls, timeout);
|
|
||||||
if (rc < 0) {
|
|
||||||
return ret;
|
|
||||||
}
|
|
||||||
|
|
||||||
for (size_t i = 0; i < num_polls; i++) {
|
|
||||||
if (polls[i].revents) {
|
|
||||||
ret.push_back(sockets[i]);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
return ret;
|
|
||||||
}
|
|
||||||
@@ -1,72 +0,0 @@
|
|||||||
#pragma once
|
|
||||||
|
|
||||||
#include <cstddef>
|
|
||||||
#include <string>
|
|
||||||
#include <vector>
|
|
||||||
|
|
||||||
#include <zmq.h>
|
|
||||||
|
|
||||||
#include "msgq/ipc.h"
|
|
||||||
|
|
||||||
class BridgeZmqContext {
|
|
||||||
public:
|
|
||||||
BridgeZmqContext();
|
|
||||||
void *getRawContext() { return context; }
|
|
||||||
~BridgeZmqContext();
|
|
||||||
|
|
||||||
private:
|
|
||||||
void *context = nullptr;
|
|
||||||
};
|
|
||||||
|
|
||||||
class BridgeZmqMessage : public Message {
|
|
||||||
public:
|
|
||||||
void init(size_t size);
|
|
||||||
void init(char *data, size_t size);
|
|
||||||
void close();
|
|
||||||
size_t getSize() { return size; }
|
|
||||||
char *getData() { return data; }
|
|
||||||
~BridgeZmqMessage();
|
|
||||||
|
|
||||||
private:
|
|
||||||
char *data = nullptr;
|
|
||||||
size_t size = 0;
|
|
||||||
};
|
|
||||||
|
|
||||||
class BridgeZmqSubSocket {
|
|
||||||
public:
|
|
||||||
int connect(BridgeZmqContext *context, std::string endpoint, std::string address, bool conflate = false, bool check_endpoint = true);
|
|
||||||
void setTimeout(int timeout);
|
|
||||||
Message *receive(bool non_blocking = false);
|
|
||||||
void *getRawSocket() { return sock; }
|
|
||||||
~BridgeZmqSubSocket();
|
|
||||||
|
|
||||||
private:
|
|
||||||
void *sock = nullptr;
|
|
||||||
std::string full_endpoint;
|
|
||||||
};
|
|
||||||
|
|
||||||
class BridgeZmqPubSocket {
|
|
||||||
public:
|
|
||||||
int connect(BridgeZmqContext *context, std::string endpoint, bool check_endpoint = true);
|
|
||||||
int sendMessage(Message *message);
|
|
||||||
int send(char *data, size_t size);
|
|
||||||
void *getRawSocket() { return sock; }
|
|
||||||
~BridgeZmqPubSocket();
|
|
||||||
|
|
||||||
private:
|
|
||||||
void *sock = nullptr;
|
|
||||||
std::string full_endpoint;
|
|
||||||
int pid = -1;
|
|
||||||
};
|
|
||||||
|
|
||||||
class BridgeZmqPoller {
|
|
||||||
public:
|
|
||||||
void registerSocket(BridgeZmqSubSocket *socket);
|
|
||||||
std::vector<BridgeZmqSubSocket *> poll(int timeout);
|
|
||||||
|
|
||||||
private:
|
|
||||||
static constexpr size_t MAX_BRIDGE_ZMQ_POLLERS = 128;
|
|
||||||
std::vector<BridgeZmqSubSocket *> sockets;
|
|
||||||
zmq_pollitem_t polls[MAX_BRIDGE_ZMQ_POLLERS] = {};
|
|
||||||
size_t num_polls = 0;
|
|
||||||
};
|
|
||||||
@@ -2,7 +2,6 @@
|
|||||||
|
|
||||||
#include <cassert>
|
#include <cassert>
|
||||||
|
|
||||||
#include "cereal/services.h"
|
|
||||||
#include "common/util.h"
|
#include "common/util.h"
|
||||||
|
|
||||||
extern ExitHandler do_exit;
|
extern ExitHandler do_exit;
|
||||||
@@ -22,14 +21,14 @@ static std::string recv_zmq_msg(void *sock) {
|
|||||||
}
|
}
|
||||||
|
|
||||||
void MsgqToZmq::run(const std::vector<std::string> &endpoints, const std::string &ip) {
|
void MsgqToZmq::run(const std::vector<std::string> &endpoints, const std::string &ip) {
|
||||||
zmq_context = std::make_unique<BridgeZmqContext>();
|
zmq_context = std::make_unique<ZMQContext>();
|
||||||
msgq_context = std::make_unique<Context>();
|
msgq_context = std::make_unique<MSGQContext>();
|
||||||
|
|
||||||
// Create ZMQPubSockets for each endpoint
|
// Create ZMQPubSockets for each endpoint
|
||||||
for (const auto &endpoint : endpoints) {
|
for (const auto &endpoint : endpoints) {
|
||||||
auto &socket_pair = socket_pairs.emplace_back();
|
auto &socket_pair = socket_pairs.emplace_back();
|
||||||
socket_pair.endpoint = endpoint;
|
socket_pair.endpoint = endpoint;
|
||||||
socket_pair.pub_sock = std::make_unique<BridgeZmqPubSocket>();
|
socket_pair.pub_sock = std::make_unique<ZMQPubSocket>();
|
||||||
int ret = socket_pair.pub_sock->connect(zmq_context.get(), endpoint);
|
int ret = socket_pair.pub_sock->connect(zmq_context.get(), endpoint);
|
||||||
if (ret != 0) {
|
if (ret != 0) {
|
||||||
printf("Failed to create ZMQ publisher for [%s]: %s\n", endpoint.c_str(), zmq_strerror(zmq_errno()));
|
printf("Failed to create ZMQ publisher for [%s]: %s\n", endpoint.c_str(), zmq_strerror(zmq_errno()));
|
||||||
@@ -49,7 +48,7 @@ void MsgqToZmq::run(const std::vector<std::string> &endpoints, const std::string
|
|||||||
|
|
||||||
for (auto sub_sock : msgq_poller->poll(100)) {
|
for (auto sub_sock : msgq_poller->poll(100)) {
|
||||||
// Process messages for each socket
|
// Process messages for each socket
|
||||||
BridgeZmqPubSocket *pub_sock = sub2pub.at(sub_sock);
|
ZMQPubSocket *pub_sock = sub2pub.at(sub_sock);
|
||||||
for (int i = 0; i < MAX_MESSAGES_PER_SOCKET; ++i) {
|
for (int i = 0; i < MAX_MESSAGES_PER_SOCKET; ++i) {
|
||||||
auto msg = std::unique_ptr<Message>(sub_sock->receive(true));
|
auto msg = std::unique_ptr<Message>(sub_sock->receive(true));
|
||||||
if (!msg) break;
|
if (!msg) break;
|
||||||
@@ -72,7 +71,7 @@ void MsgqToZmq::zmqMonitorThread() {
|
|||||||
// Set up ZMQ monitor for each pub socket
|
// Set up ZMQ monitor for each pub socket
|
||||||
for (int i = 0; i < socket_pairs.size(); ++i) {
|
for (int i = 0; i < socket_pairs.size(); ++i) {
|
||||||
std::string addr = "inproc://op-bridge-monitor-" + std::to_string(i);
|
std::string addr = "inproc://op-bridge-monitor-" + std::to_string(i);
|
||||||
zmq_socket_monitor(socket_pairs[i].pub_sock->getRawSocket(), addr.c_str(), ZMQ_EVENT_ACCEPTED | ZMQ_EVENT_DISCONNECTED);
|
zmq_socket_monitor(socket_pairs[i].pub_sock->sock, addr.c_str(), ZMQ_EVENT_ACCEPTED | ZMQ_EVENT_DISCONNECTED);
|
||||||
|
|
||||||
void *monitor_socket = zmq_socket(zmq_context->getRawContext(), ZMQ_PAIR);
|
void *monitor_socket = zmq_socket(zmq_context->getRawContext(), ZMQ_PAIR);
|
||||||
zmq_connect(monitor_socket, addr.c_str());
|
zmq_connect(monitor_socket, addr.c_str());
|
||||||
@@ -109,8 +108,7 @@ void MsgqToZmq::zmqMonitorThread() {
|
|||||||
if (++pair.connected_clients == 1) {
|
if (++pair.connected_clients == 1) {
|
||||||
// Create new MSGQ subscriber socket and map to ZMQ publisher
|
// Create new MSGQ subscriber socket and map to ZMQ publisher
|
||||||
pair.sub_sock = std::make_unique<MSGQSubSocket>();
|
pair.sub_sock = std::make_unique<MSGQSubSocket>();
|
||||||
size_t queue_size = services.at(pair.endpoint).queue_size;
|
pair.sub_sock->connect(msgq_context.get(), pair.endpoint, "127.0.0.1");
|
||||||
pair.sub_sock->connect(msgq_context.get(), pair.endpoint, "127.0.0.1", false, true, queue_size);
|
|
||||||
sub2pub[pair.sub_sock.get()] = pair.pub_sock.get();
|
sub2pub[pair.sub_sock.get()] = pair.pub_sock.get();
|
||||||
registerSockets();
|
registerSockets();
|
||||||
}
|
}
|
||||||
@@ -130,7 +128,7 @@ void MsgqToZmq::zmqMonitorThread() {
|
|||||||
|
|
||||||
// Clean up monitor sockets
|
// Clean up monitor sockets
|
||||||
for (int i = 0; i < pollitems.size(); ++i) {
|
for (int i = 0; i < pollitems.size(); ++i) {
|
||||||
zmq_socket_monitor(socket_pairs[i].pub_sock->getRawSocket(), nullptr, 0);
|
zmq_socket_monitor(socket_pairs[i].pub_sock->sock, nullptr, 0);
|
||||||
zmq_close(pollitems[i].socket);
|
zmq_close(pollitems[i].socket);
|
||||||
}
|
}
|
||||||
cv.notify_one();
|
cv.notify_one();
|
||||||
|
|||||||
@@ -7,8 +7,9 @@
|
|||||||
#include <string>
|
#include <string>
|
||||||
#include <vector>
|
#include <vector>
|
||||||
|
|
||||||
|
#define private public
|
||||||
#include "msgq/impl_msgq.h"
|
#include "msgq/impl_msgq.h"
|
||||||
#include "cereal/messaging/bridge_zmq.h"
|
#include "msgq/impl_zmq.h"
|
||||||
|
|
||||||
class MsgqToZmq {
|
class MsgqToZmq {
|
||||||
public:
|
public:
|
||||||
@@ -21,16 +22,16 @@ protected:
|
|||||||
|
|
||||||
struct SocketPair {
|
struct SocketPair {
|
||||||
std::string endpoint;
|
std::string endpoint;
|
||||||
std::unique_ptr<BridgeZmqPubSocket> pub_sock;
|
std::unique_ptr<ZMQPubSocket> pub_sock;
|
||||||
std::unique_ptr<MSGQSubSocket> sub_sock;
|
std::unique_ptr<MSGQSubSocket> sub_sock;
|
||||||
int connected_clients = 0;
|
int connected_clients = 0;
|
||||||
};
|
};
|
||||||
|
|
||||||
std::unique_ptr<Context> msgq_context;
|
std::unique_ptr<MSGQContext> msgq_context;
|
||||||
std::unique_ptr<BridgeZmqContext> zmq_context;
|
std::unique_ptr<ZMQContext> zmq_context;
|
||||||
std::mutex mutex;
|
std::mutex mutex;
|
||||||
std::condition_variable cv;
|
std::condition_variable cv;
|
||||||
std::unique_ptr<MSGQPoller> msgq_poller;
|
std::unique_ptr<MSGQPoller> msgq_poller;
|
||||||
std::map<SubSocket *, BridgeZmqPubSocket *> sub2pub;
|
std::map<SubSocket *, ZMQPubSocket *> sub2pub;
|
||||||
std::vector<SocketPair> socket_pairs;
|
std::vector<SocketPair> socket_pairs;
|
||||||
};
|
};
|
||||||
|
|||||||
@@ -5,7 +5,7 @@ import numbers
|
|||||||
import random
|
import random
|
||||||
import threading
|
import threading
|
||||||
import time
|
import time
|
||||||
from openpilot.common.parameterized import parameterized
|
from parameterized import parameterized
|
||||||
import pytest
|
import pytest
|
||||||
|
|
||||||
from cereal import log, car
|
from cereal import log, car
|
||||||
|
|||||||
@@ -1,7 +1,7 @@
|
|||||||
import os
|
import os
|
||||||
import tempfile
|
import tempfile
|
||||||
from typing import Dict
|
from typing import Dict
|
||||||
from openpilot.common.parameterized import parameterized
|
from parameterized import parameterized
|
||||||
|
|
||||||
import cereal.services as services
|
import cereal.services as services
|
||||||
from cereal.services import SERVICE_LIST
|
from cereal.services import SERVICE_LIST
|
||||||
|
|||||||
@@ -75,7 +75,7 @@ _services: dict[str, tuple] = {
|
|||||||
"modelV2": (True, 20., None, QueueSize.BIG),
|
"modelV2": (True, 20., None, QueueSize.BIG),
|
||||||
"managerState": (True, 2., 1),
|
"managerState": (True, 2., 1),
|
||||||
"uploaderState": (True, 0., 1),
|
"uploaderState": (True, 0., 1),
|
||||||
"navInstruction": (True, 1., 10),
|
# "navInstruction": (True, 1., 10), # dp - make it 0 hz
|
||||||
"navRoute": (True, 0.),
|
"navRoute": (True, 0.),
|
||||||
"navThumbnail": (True, 0.),
|
"navThumbnail": (True, 0.),
|
||||||
"qRoadEncodeIdx": (False, 20.),
|
"qRoadEncodeIdx": (False, 20.),
|
||||||
@@ -105,6 +105,11 @@ _services: dict[str, tuple] = {
|
|||||||
"controlsStateExt": (True, 100.),
|
"controlsStateExt": (True, 100.),
|
||||||
"carStateExt": (True, 100.),
|
"carStateExt": (True, 100.),
|
||||||
"modelExt": (True, 20.),
|
"modelExt": (True, 20.),
|
||||||
|
# dashy
|
||||||
|
"navInstruction": (True, 0.),
|
||||||
|
"navInstructionExt": (True, 0.),
|
||||||
|
"liveGPS": (True, 0.), # GPS fusion from gpsd (optional)
|
||||||
|
"maaControl": (True, 0.), # Map-Aware Assist control signals (optional)
|
||||||
"dashyState": (True, 0.), # Aggregated dashy UI state (optional)
|
"dashyState": (True, 0.), # Aggregated dashy UI state (optional)
|
||||||
}
|
}
|
||||||
SERVICE_LIST = {name: Service(*vals) for
|
SERVICE_LIST = {name: Service(*vals) for
|
||||||
|
|||||||
@@ -0,0 +1 @@
|
|||||||
|
*.cpp
|
||||||
@@ -5,6 +5,7 @@ common_libs = [
|
|||||||
'swaglog.cc',
|
'swaglog.cc',
|
||||||
'util.cc',
|
'util.cc',
|
||||||
'ratekeeper.cc',
|
'ratekeeper.cc',
|
||||||
|
'clutil.cc',
|
||||||
]
|
]
|
||||||
|
|
||||||
_common = env.Library('common', common_libs, LIBS="json11")
|
_common = env.Library('common', common_libs, LIBS="json11")
|
||||||
@@ -18,6 +19,11 @@ if GetOption('extras'):
|
|||||||
# Cython bindings
|
# Cython bindings
|
||||||
params_python = envCython.Program('params_pyx.so', 'params_pyx.pyx', LIBS=envCython['LIBS'] + [_common, 'zmq', 'json11'])
|
params_python = envCython.Program('params_pyx.so', 'params_pyx.pyx', LIBS=envCython['LIBS'] + [_common, 'zmq', 'json11'])
|
||||||
|
|
||||||
common_python = [params_python]
|
SConscript([
|
||||||
|
'transformations/SConscript',
|
||||||
|
])
|
||||||
|
|
||||||
|
Import('transformations_python')
|
||||||
|
common_python = [params_python, transformations_python]
|
||||||
|
|
||||||
Export('common_python')
|
Export('common_python')
|
||||||
|
|||||||
@@ -0,0 +1,98 @@
|
|||||||
|
#include "common/clutil.h"
|
||||||
|
|
||||||
|
#include <cassert>
|
||||||
|
#include <iostream>
|
||||||
|
#include <memory>
|
||||||
|
|
||||||
|
#include "common/util.h"
|
||||||
|
#include "common/swaglog.h"
|
||||||
|
|
||||||
|
namespace { // helper functions
|
||||||
|
|
||||||
|
template <typename Func, typename Id, typename Name>
|
||||||
|
std::string get_info(Func get_info_func, Id id, Name param_name) {
|
||||||
|
size_t size = 0;
|
||||||
|
CL_CHECK(get_info_func(id, param_name, 0, NULL, &size));
|
||||||
|
std::string info(size, '\0');
|
||||||
|
CL_CHECK(get_info_func(id, param_name, size, info.data(), NULL));
|
||||||
|
return info;
|
||||||
|
}
|
||||||
|
inline std::string get_platform_info(cl_platform_id id, cl_platform_info name) { return get_info(&clGetPlatformInfo, id, name); }
|
||||||
|
inline std::string get_device_info(cl_device_id id, cl_device_info name) { return get_info(&clGetDeviceInfo, id, name); }
|
||||||
|
|
||||||
|
void cl_print_info(cl_platform_id platform, cl_device_id device) {
|
||||||
|
size_t work_group_size = 0;
|
||||||
|
cl_device_type device_type = 0;
|
||||||
|
clGetDeviceInfo(device, CL_DEVICE_MAX_WORK_GROUP_SIZE, sizeof(work_group_size), &work_group_size, NULL);
|
||||||
|
clGetDeviceInfo(device, CL_DEVICE_TYPE, sizeof(device_type), &device_type, NULL);
|
||||||
|
const char *type_str = "Other...";
|
||||||
|
switch (device_type) {
|
||||||
|
case CL_DEVICE_TYPE_CPU: type_str ="CL_DEVICE_TYPE_CPU"; break;
|
||||||
|
case CL_DEVICE_TYPE_GPU: type_str = "CL_DEVICE_TYPE_GPU"; break;
|
||||||
|
case CL_DEVICE_TYPE_ACCELERATOR: type_str = "CL_DEVICE_TYPE_ACCELERATOR"; break;
|
||||||
|
}
|
||||||
|
|
||||||
|
LOGD("vendor: %s", get_platform_info(platform, CL_PLATFORM_VENDOR).c_str());
|
||||||
|
LOGD("platform version: %s", get_platform_info(platform, CL_PLATFORM_VERSION).c_str());
|
||||||
|
LOGD("profile: %s", get_platform_info(platform, CL_PLATFORM_PROFILE).c_str());
|
||||||
|
LOGD("extensions: %s", get_platform_info(platform, CL_PLATFORM_EXTENSIONS).c_str());
|
||||||
|
LOGD("name: %s", get_device_info(device, CL_DEVICE_NAME).c_str());
|
||||||
|
LOGD("device version: %s", get_device_info(device, CL_DEVICE_VERSION).c_str());
|
||||||
|
LOGD("max work group size: %zu", work_group_size);
|
||||||
|
LOGD("type = %d, %s", (int)device_type, type_str);
|
||||||
|
}
|
||||||
|
|
||||||
|
void cl_print_build_errors(cl_program program, cl_device_id device) {
|
||||||
|
cl_build_status status;
|
||||||
|
clGetProgramBuildInfo(program, device, CL_PROGRAM_BUILD_STATUS, sizeof(status), &status, NULL);
|
||||||
|
size_t log_size;
|
||||||
|
clGetProgramBuildInfo(program, device, CL_PROGRAM_BUILD_LOG, 0, NULL, &log_size);
|
||||||
|
std::string log(log_size, '\0');
|
||||||
|
clGetProgramBuildInfo(program, device, CL_PROGRAM_BUILD_LOG, log_size, &log[0], NULL);
|
||||||
|
|
||||||
|
LOGE("build failed; status=%d, log: %s", status, log.c_str());
|
||||||
|
}
|
||||||
|
|
||||||
|
} // namespace
|
||||||
|
|
||||||
|
cl_device_id cl_get_device_id(cl_device_type device_type) {
|
||||||
|
cl_uint num_platforms = 0;
|
||||||
|
CL_CHECK(clGetPlatformIDs(0, NULL, &num_platforms));
|
||||||
|
std::unique_ptr<cl_platform_id[]> platform_ids = std::make_unique<cl_platform_id[]>(num_platforms);
|
||||||
|
CL_CHECK(clGetPlatformIDs(num_platforms, &platform_ids[0], NULL));
|
||||||
|
|
||||||
|
for (size_t i = 0; i < num_platforms; ++i) {
|
||||||
|
LOGD("platform[%zu] CL_PLATFORM_NAME: %s", i, get_platform_info(platform_ids[i], CL_PLATFORM_NAME).c_str());
|
||||||
|
|
||||||
|
// Get first device
|
||||||
|
if (cl_device_id device_id = NULL; clGetDeviceIDs(platform_ids[i], device_type, 1, &device_id, NULL) == 0 && device_id) {
|
||||||
|
cl_print_info(platform_ids[i], device_id);
|
||||||
|
return device_id;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
LOGE("No valid openCL platform found");
|
||||||
|
assert(0);
|
||||||
|
return nullptr;
|
||||||
|
}
|
||||||
|
|
||||||
|
cl_context cl_create_context(cl_device_id device_id) {
|
||||||
|
return CL_CHECK_ERR(clCreateContext(NULL, 1, &device_id, NULL, NULL, &err));
|
||||||
|
}
|
||||||
|
|
||||||
|
void cl_release_context(cl_context context) {
|
||||||
|
clReleaseContext(context);
|
||||||
|
}
|
||||||
|
|
||||||
|
cl_program cl_program_from_file(cl_context ctx, cl_device_id device_id, const char* path, const char* args) {
|
||||||
|
return cl_program_from_source(ctx, device_id, util::read_file(path), args);
|
||||||
|
}
|
||||||
|
|
||||||
|
cl_program cl_program_from_source(cl_context ctx, cl_device_id device_id, const std::string& src, const char* args) {
|
||||||
|
const char *csrc = src.c_str();
|
||||||
|
cl_program prg = CL_CHECK_ERR(clCreateProgramWithSource(ctx, 1, &csrc, NULL, &err));
|
||||||
|
if (int err = clBuildProgram(prg, 1, &device_id, args, NULL, NULL); err != 0) {
|
||||||
|
cl_print_build_errors(prg, device_id);
|
||||||
|
assert(0);
|
||||||
|
}
|
||||||
|
return prg;
|
||||||
|
}
|
||||||
@@ -0,0 +1,28 @@
|
|||||||
|
#pragma once
|
||||||
|
|
||||||
|
#ifdef __APPLE__
|
||||||
|
#include <OpenCL/cl.h>
|
||||||
|
#else
|
||||||
|
#include <CL/cl.h>
|
||||||
|
#endif
|
||||||
|
|
||||||
|
#include <string>
|
||||||
|
|
||||||
|
#define CL_CHECK(_expr) \
|
||||||
|
do { \
|
||||||
|
assert(CL_SUCCESS == (_expr)); \
|
||||||
|
} while (0)
|
||||||
|
|
||||||
|
#define CL_CHECK_ERR(_expr) \
|
||||||
|
({ \
|
||||||
|
cl_int err = CL_INVALID_VALUE; \
|
||||||
|
__typeof__(_expr) _ret = _expr; \
|
||||||
|
assert(_ret&& err == CL_SUCCESS); \
|
||||||
|
_ret; \
|
||||||
|
})
|
||||||
|
|
||||||
|
cl_device_id cl_get_device_id(cl_device_type device_type);
|
||||||
|
cl_context cl_create_context(cl_device_id device_id);
|
||||||
|
void cl_release_context(cl_context context);
|
||||||
|
cl_program cl_program_from_source(cl_context ctx, cl_device_id device_id, const std::string& src, const char* args = nullptr);
|
||||||
|
cl_program cl_program_from_file(cl_context ctx, cl_device_id device_id, const char* path, const char* args);
|
||||||
@@ -1,37 +0,0 @@
|
|||||||
import math
|
|
||||||
import os
|
|
||||||
from pathlib import Path
|
|
||||||
|
|
||||||
CHUNK_SIZE = 45 * 1024 * 1024 # 45MB, under GitHub's 50MB limit
|
|
||||||
|
|
||||||
def get_chunk_name(name, idx, num_chunks):
|
|
||||||
return f"{name}.chunk{idx+1:02d}of{num_chunks:02d}"
|
|
||||||
|
|
||||||
def get_manifest_path(name):
|
|
||||||
return f"{name}.chunkmanifest"
|
|
||||||
|
|
||||||
def get_chunk_paths(path, file_size):
|
|
||||||
num_chunks = math.ceil(file_size / CHUNK_SIZE)
|
|
||||||
return [get_manifest_path(path)] + [get_chunk_name(path, i, num_chunks) for i in range(num_chunks)]
|
|
||||||
|
|
||||||
def chunk_file(path, targets):
|
|
||||||
manifest_path, *chunk_paths = targets
|
|
||||||
with open(path, 'rb') as f:
|
|
||||||
data = f.read()
|
|
||||||
actual_num_chunks = max(1, math.ceil(len(data) / CHUNK_SIZE))
|
|
||||||
assert len(chunk_paths) >= actual_num_chunks, f"Allowed {len(chunk_paths)} chunks but needs at least {actual_num_chunks}, for path {path}"
|
|
||||||
for i, chunk_path in enumerate(chunk_paths):
|
|
||||||
with open(chunk_path, 'wb') as f:
|
|
||||||
f.write(data[i * CHUNK_SIZE:(i + 1) * CHUNK_SIZE])
|
|
||||||
Path(manifest_path).write_text(str(len(chunk_paths)))
|
|
||||||
os.remove(path)
|
|
||||||
|
|
||||||
|
|
||||||
def read_file_chunked(path):
|
|
||||||
manifest_path = get_manifest_path(path)
|
|
||||||
if os.path.isfile(manifest_path):
|
|
||||||
num_chunks = int(Path(manifest_path).read_text().strip())
|
|
||||||
return b''.join(Path(get_chunk_name(path, i, num_chunks)).read_bytes() for i in range(num_chunks))
|
|
||||||
if os.path.isfile(path):
|
|
||||||
return Path(path).read_bytes()
|
|
||||||
raise FileNotFoundError(path)
|
|
||||||
@@ -28,7 +28,7 @@ class BounceFilter(FirstOrderFilter):
|
|||||||
scale = self.dt / (1.0 / 60.0) # tuned at 60 fps
|
scale = self.dt / (1.0 / 60.0) # tuned at 60 fps
|
||||||
self.velocity.x += (x - self.x) * self.bounce * scale * self.dt
|
self.velocity.x += (x - self.x) * self.bounce * scale * self.dt
|
||||||
self.velocity.update(0.0)
|
self.velocity.update(0.0)
|
||||||
if abs(self.velocity.x) < 1e-3:
|
if abs(self.velocity.x) < 1e-5:
|
||||||
self.velocity.x = 0.0
|
self.velocity.x = 0.0
|
||||||
self.x += self.velocity.x
|
self.x += self.velocity.x
|
||||||
return self.x
|
return self.x
|
||||||
|
|||||||
@@ -4,27 +4,27 @@ from openpilot.common.utils import run_cmd, run_cmd_default
|
|||||||
|
|
||||||
|
|
||||||
@cache
|
@cache
|
||||||
def get_commit(cwd: str | None = None, branch: str = "HEAD") -> str:
|
def get_commit(cwd: str = None, branch: str = "HEAD") -> str:
|
||||||
return run_cmd_default(["git", "rev-parse", branch], cwd=cwd)
|
return run_cmd_default(["git", "rev-parse", branch], cwd=cwd)
|
||||||
|
|
||||||
|
|
||||||
@cache
|
@cache
|
||||||
def get_commit_date(cwd: str | None = None, commit: str = "HEAD") -> str:
|
def get_commit_date(cwd: str = None, commit: str = "HEAD") -> str:
|
||||||
return run_cmd_default(["git", "show", "--no-patch", "--format='%ct %ci'", commit], cwd=cwd)
|
return run_cmd_default(["git", "show", "--no-patch", "--format='%ct %ci'", commit], cwd=cwd)
|
||||||
|
|
||||||
|
|
||||||
@cache
|
@cache
|
||||||
def get_short_branch(cwd: str | None = None) -> str:
|
def get_short_branch(cwd: str = None) -> str:
|
||||||
return run_cmd_default(["git", "rev-parse", "--abbrev-ref", "HEAD"], cwd=cwd)
|
return run_cmd_default(["git", "rev-parse", "--abbrev-ref", "HEAD"], cwd=cwd)
|
||||||
|
|
||||||
|
|
||||||
@cache
|
@cache
|
||||||
def get_branch(cwd: str | None = None) -> str:
|
def get_branch(cwd: str = None) -> str:
|
||||||
return run_cmd_default(["git", "rev-parse", "--abbrev-ref", "--symbolic-full-name", "@{u}"], cwd=cwd)
|
return run_cmd_default(["git", "rev-parse", "--abbrev-ref", "--symbolic-full-name", "@{u}"], cwd=cwd)
|
||||||
|
|
||||||
|
|
||||||
@cache
|
@cache
|
||||||
def get_origin(cwd: str | None = None) -> str:
|
def get_origin(cwd: str = None) -> str:
|
||||||
try:
|
try:
|
||||||
local_branch = run_cmd(["git", "name-rev", "--name-only", "HEAD"], cwd=cwd)
|
local_branch = run_cmd(["git", "name-rev", "--name-only", "HEAD"], cwd=cwd)
|
||||||
tracking_remote = run_cmd(["git", "config", "branch." + local_branch + ".remote"], cwd=cwd)
|
tracking_remote = run_cmd(["git", "config", "branch." + local_branch + ".remote"], cwd=cwd)
|
||||||
@@ -34,7 +34,7 @@ def get_origin(cwd: str | None = None) -> str:
|
|||||||
|
|
||||||
|
|
||||||
@cache
|
@cache
|
||||||
def get_normalized_origin(cwd: str | None = None) -> str:
|
def get_normalized_origin(cwd: str = None) -> str:
|
||||||
return get_origin(cwd) \
|
return get_origin(cwd) \
|
||||||
.replace("git@", "", 1) \
|
.replace("git@", "", 1) \
|
||||||
.replace(".git", "", 1) \
|
.replace(".git", "", 1) \
|
||||||
|
|||||||
@@ -1,81 +0,0 @@
|
|||||||
import os
|
|
||||||
import fcntl
|
|
||||||
import ctypes
|
|
||||||
|
|
||||||
# I2C constants from /usr/include/linux/i2c-dev.h
|
|
||||||
I2C_SLAVE = 0x0703
|
|
||||||
I2C_SLAVE_FORCE = 0x0706
|
|
||||||
I2C_SMBUS = 0x0720
|
|
||||||
|
|
||||||
# SMBus transfer types
|
|
||||||
I2C_SMBUS_READ = 1
|
|
||||||
I2C_SMBUS_WRITE = 0
|
|
||||||
I2C_SMBUS_BYTE_DATA = 2
|
|
||||||
I2C_SMBUS_I2C_BLOCK_DATA = 8
|
|
||||||
|
|
||||||
I2C_SMBUS_BLOCK_MAX = 32
|
|
||||||
|
|
||||||
|
|
||||||
class _I2cSmbusData(ctypes.Union):
|
|
||||||
_fields_ = [
|
|
||||||
("byte", ctypes.c_uint8),
|
|
||||||
("word", ctypes.c_uint16),
|
|
||||||
("block", ctypes.c_uint8 * (I2C_SMBUS_BLOCK_MAX + 2)),
|
|
||||||
]
|
|
||||||
|
|
||||||
|
|
||||||
class _I2cSmbusIoctlData(ctypes.Structure):
|
|
||||||
_fields_ = [
|
|
||||||
("read_write", ctypes.c_uint8),
|
|
||||||
("command", ctypes.c_uint8),
|
|
||||||
("size", ctypes.c_uint32),
|
|
||||||
("data", ctypes.POINTER(_I2cSmbusData)),
|
|
||||||
]
|
|
||||||
|
|
||||||
|
|
||||||
class SMBus:
|
|
||||||
def __init__(self, bus: int):
|
|
||||||
self._fd = os.open(f'/dev/i2c-{bus}', os.O_RDWR)
|
|
||||||
|
|
||||||
def __enter__(self) -> 'SMBus':
|
|
||||||
return self
|
|
||||||
|
|
||||||
def __exit__(self, *args) -> None:
|
|
||||||
self.close()
|
|
||||||
|
|
||||||
def close(self) -> None:
|
|
||||||
if hasattr(self, '_fd') and self._fd >= 0:
|
|
||||||
os.close(self._fd)
|
|
||||||
self._fd = -1
|
|
||||||
|
|
||||||
def _set_address(self, addr: int, force: bool = False) -> None:
|
|
||||||
ioctl_arg = I2C_SLAVE_FORCE if force else I2C_SLAVE
|
|
||||||
fcntl.ioctl(self._fd, ioctl_arg, addr)
|
|
||||||
|
|
||||||
def _smbus_access(self, read_write: int, command: int, size: int, data: _I2cSmbusData) -> None:
|
|
||||||
ioctl_data = _I2cSmbusIoctlData(read_write, command, size, ctypes.pointer(data))
|
|
||||||
fcntl.ioctl(self._fd, I2C_SMBUS, ioctl_data)
|
|
||||||
|
|
||||||
def read_byte_data(self, addr: int, register: int, force: bool = False) -> int:
|
|
||||||
self._set_address(addr, force)
|
|
||||||
data = _I2cSmbusData()
|
|
||||||
self._smbus_access(I2C_SMBUS_READ, register, I2C_SMBUS_BYTE_DATA, data)
|
|
||||||
return int(data.byte)
|
|
||||||
|
|
||||||
def write_byte_data(self, addr: int, register: int, value: int, force: bool = False) -> None:
|
|
||||||
self._set_address(addr, force)
|
|
||||||
data = _I2cSmbusData()
|
|
||||||
data.byte = value & 0xFF
|
|
||||||
self._smbus_access(I2C_SMBUS_WRITE, register, I2C_SMBUS_BYTE_DATA, data)
|
|
||||||
|
|
||||||
def read_i2c_block_data(self, addr: int, register: int, length: int, force: bool = False) -> list[int]:
|
|
||||||
self._set_address(addr, force)
|
|
||||||
if not (0 <= length <= I2C_SMBUS_BLOCK_MAX):
|
|
||||||
raise ValueError(f"length must be 0..{I2C_SMBUS_BLOCK_MAX}")
|
|
||||||
|
|
||||||
data = _I2cSmbusData()
|
|
||||||
data.block[0] = length
|
|
||||||
self._smbus_access(I2C_SMBUS_READ, register, I2C_SMBUS_I2C_BLOCK_DATA, data)
|
|
||||||
read_len = int(data.block[0]) or length
|
|
||||||
read_len = min(read_len, length)
|
|
||||||
return [int(b) for b in data.block[1 : read_len + 1]]
|
|
||||||
@@ -0,0 +1,85 @@
|
|||||||
|
#pragma once
|
||||||
|
|
||||||
|
typedef struct vec3 {
|
||||||
|
float v[3];
|
||||||
|
} vec3;
|
||||||
|
|
||||||
|
typedef struct vec4 {
|
||||||
|
float v[4];
|
||||||
|
} vec4;
|
||||||
|
|
||||||
|
typedef struct mat3 {
|
||||||
|
float v[3*3];
|
||||||
|
} mat3;
|
||||||
|
|
||||||
|
typedef struct mat4 {
|
||||||
|
float v[4*4];
|
||||||
|
} mat4;
|
||||||
|
|
||||||
|
static inline mat3 matmul3(const mat3 &a, const mat3 &b) {
|
||||||
|
mat3 ret = {{0.0}};
|
||||||
|
for (int r=0; r<3; r++) {
|
||||||
|
for (int c=0; c<3; c++) {
|
||||||
|
float v = 0.0;
|
||||||
|
for (int k=0; k<3; k++) {
|
||||||
|
v += a.v[r*3+k] * b.v[k*3+c];
|
||||||
|
}
|
||||||
|
ret.v[r*3+c] = v;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
return ret;
|
||||||
|
}
|
||||||
|
|
||||||
|
static inline vec3 matvecmul3(const mat3 &a, const vec3 &b) {
|
||||||
|
vec3 ret = {{0.0}};
|
||||||
|
for (int r=0; r<3; r++) {
|
||||||
|
for (int c=0; c<3; c++) {
|
||||||
|
ret.v[r] += a.v[r*3+c] * b.v[c];
|
||||||
|
}
|
||||||
|
}
|
||||||
|
return ret;
|
||||||
|
}
|
||||||
|
|
||||||
|
static inline mat4 matmul(const mat4 &a, const mat4 &b) {
|
||||||
|
mat4 ret = {{0.0}};
|
||||||
|
for (int r=0; r<4; r++) {
|
||||||
|
for (int c=0; c<4; c++) {
|
||||||
|
float v = 0.0;
|
||||||
|
for (int k=0; k<4; k++) {
|
||||||
|
v += a.v[r*4+k] * b.v[k*4+c];
|
||||||
|
}
|
||||||
|
ret.v[r*4+c] = v;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
return ret;
|
||||||
|
}
|
||||||
|
|
||||||
|
static inline vec4 matvecmul(const mat4 &a, const vec4 &b) {
|
||||||
|
vec4 ret = {{0.0}};
|
||||||
|
for (int r=0; r<4; r++) {
|
||||||
|
for (int c=0; c<4; c++) {
|
||||||
|
ret.v[r] += a.v[r*4+c] * b.v[c];
|
||||||
|
}
|
||||||
|
}
|
||||||
|
return ret;
|
||||||
|
}
|
||||||
|
|
||||||
|
// scales the input and output space of a transformation matrix
|
||||||
|
// that assumes pixel-center origin.
|
||||||
|
static inline mat3 transform_scale_buffer(const mat3 &in, float s) {
|
||||||
|
// in_pt = ( transform(out_pt/s + 0.5) - 0.5) * s
|
||||||
|
|
||||||
|
mat3 transform_out = {{
|
||||||
|
1.0f/s, 0.0f, 0.5f,
|
||||||
|
0.0f, 1.0f/s, 0.5f,
|
||||||
|
0.0f, 0.0f, 1.0f,
|
||||||
|
}};
|
||||||
|
|
||||||
|
mat3 transform_in = {{
|
||||||
|
s, 0.0f, -0.5f*s,
|
||||||
|
0.0f, s, -0.5f*s,
|
||||||
|
0.0f, 0.0f, 1.0f,
|
||||||
|
}};
|
||||||
|
|
||||||
|
return matmul3(transform_in, matmul3(in, transform_out));
|
||||||
|
}
|
||||||
@@ -1,47 +0,0 @@
|
|||||||
import sys
|
|
||||||
import pytest
|
|
||||||
import inspect
|
|
||||||
|
|
||||||
|
|
||||||
class parameterized:
|
|
||||||
@staticmethod
|
|
||||||
def expand(cases):
|
|
||||||
cases = list(cases)
|
|
||||||
|
|
||||||
if not cases:
|
|
||||||
return lambda func: pytest.mark.skip("no parameterized cases")(func)
|
|
||||||
|
|
||||||
def decorator(func):
|
|
||||||
params = [p for p in inspect.signature(func).parameters if p != 'self']
|
|
||||||
normalized = [c if isinstance(c, tuple) else (c,) for c in cases]
|
|
||||||
# Infer arg count from first case so extra params (e.g. from @given) are left untouched
|
|
||||||
expand_params = params[: len(normalized[0])]
|
|
||||||
if len(expand_params) == 1:
|
|
||||||
return pytest.mark.parametrize(expand_params[0], [c[0] for c in normalized])(func)
|
|
||||||
return pytest.mark.parametrize(', '.join(expand_params), normalized)(func)
|
|
||||||
|
|
||||||
return decorator
|
|
||||||
|
|
||||||
|
|
||||||
def parameterized_class(attrs, input_list=None):
|
|
||||||
if isinstance(attrs, list) and (not attrs or isinstance(attrs[0], dict)):
|
|
||||||
params_list = attrs
|
|
||||||
else:
|
|
||||||
assert input_list is not None
|
|
||||||
attr_names = (attrs,) if isinstance(attrs, str) else tuple(attrs)
|
|
||||||
params_list = [dict(zip(attr_names, v if isinstance(v, (tuple, list)) else (v,), strict=False)) for v in input_list]
|
|
||||||
|
|
||||||
def decorator(cls):
|
|
||||||
globs = sys._getframe(1).f_globals
|
|
||||||
for i, params in enumerate(params_list):
|
|
||||||
name = f"{cls.__name__}_{i}"
|
|
||||||
new_cls = type(name, (cls,), dict(params))
|
|
||||||
new_cls.__module__ = cls.__module__
|
|
||||||
new_cls.__test__ = True # override inherited False so pytest collects this subclass
|
|
||||||
globs[name] = new_cls
|
|
||||||
# Don't collect the un-parametrised base, but return it so outer decorators
|
|
||||||
# (e.g. @pytest.mark.skip) land on it and propagate to subclasses via MRO.
|
|
||||||
cls.__test__ = False
|
|
||||||
return cls
|
|
||||||
|
|
||||||
return decorator
|
|
||||||
@@ -131,5 +131,42 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
|
|||||||
{"Version", {PERSISTENT, STRING}},
|
{"Version", {PERSISTENT, STRING}},
|
||||||
{"dp_dev_last_log", {CLEAR_ON_ONROAD_TRANSITION, STRING}},
|
{"dp_dev_last_log", {CLEAR_ON_ONROAD_TRANSITION, STRING}},
|
||||||
{"dp_dev_reset_conf", {CLEAR_ON_MANAGER_START, BOOL, "0"}},
|
{"dp_dev_reset_conf", {CLEAR_ON_MANAGER_START, BOOL, "0"}},
|
||||||
{"dp_dev_go_off_road", {CLEAR_ON_MANAGER_START, BOOL, "0"}},
|
{"dp_dev_beep", {PERSISTENT, BOOL, "0"}},
|
||||||
|
{"dp_dev_is_rhd", {PERSISTENT, BOOL, "0"}},
|
||||||
|
{"dp_lat_alka", {PERSISTENT, BOOL, "0"}},
|
||||||
|
{"dp_ui_display_mode", {PERSISTENT, INT, "0"}},
|
||||||
|
{"dp_dev_model_selected", {PERSISTENT, STRING}},
|
||||||
|
{"dp_dev_model_list", {PERSISTENT, STRING}},
|
||||||
|
{"dp_lat_lca_speed", {PERSISTENT, INT, "20"}},
|
||||||
|
{"dp_lat_lca_auto_sec", {PERSISTENT, FLOAT, "0.0"}},
|
||||||
|
{"dp_dev_go_off_road", {CLEAR_ON_MANAGER_START, BOOL}},
|
||||||
|
{"dp_ui_hide_hud_speed_kph", {PERSISTENT, INT, "0"}},
|
||||||
|
{"dp_lon_ext_radar", {PERSISTENT, BOOL, "0"}},
|
||||||
|
{"dp_lat_road_edge_detection", {PERSISTENT, BOOL, "0"}},
|
||||||
|
{"dp_ui_rainbow", {PERSISTENT, BOOL, "0"}},
|
||||||
|
{"dp_lon_acm", {PERSISTENT, BOOL, "0"}},
|
||||||
|
{"dp_lon_aem", {PERSISTENT, BOOL, "0"}},
|
||||||
|
{"dp_lon_dtsc", {PERSISTENT, BOOL, "0"}},
|
||||||
|
{"dp_lon_apm", {PERSISTENT, BOOL, "0"}},
|
||||||
|
{"dp_lon_dasr", {PERSISTENT, BOOL, "0"}},
|
||||||
|
{"dp_dev_audible_alert_mode", {PERSISTENT, INT, "0"}},
|
||||||
|
{"dp_dev_auto_shutdown_in", {PERSISTENT, INT, "-5"}},
|
||||||
|
{"dp_ui_lead", {PERSISTENT, INT, "0"}},
|
||||||
|
{"dp_dev_opview", {PERSISTENT, BOOL, "0"}},
|
||||||
|
{"dp_dev_dashy", {PERSISTENT, BOOL, "0"}},
|
||||||
|
{"dp_maa_route", {CLEAR_ON_MANAGER_START, JSON}},
|
||||||
|
{"dp_maa_destination", {PERSISTENT, JSON}},
|
||||||
|
{"dp_maa_places", {PERSISTENT, JSON}},
|
||||||
|
{"dp_dev_delay_loggerd", {PERSISTENT, INT, "0"}},
|
||||||
|
{"dp_dev_disable_connect", {PERSISTENT, BOOL, "0"}},
|
||||||
|
{"dp_dev_tethering", {PERSISTENT, BOOL, "0"}},
|
||||||
|
{"dp_ui_mici", {PERSISTENT, BOOL, "0"}},
|
||||||
|
{"dp_lat_offset_cm", {PERSISTENT, INT, "0"}},
|
||||||
|
{"dp_toyota_door_auto_lock_unlock", {PERSISTENT, BOOL, "0"}},
|
||||||
|
{"dp_toyota_tss1_sng", {PERSISTENT, BOOL, "0"}},
|
||||||
|
{"dp_toyota_stock_lon", {PERSISTENT, BOOL, "0"}},
|
||||||
|
{"dp_vag_a0_sng", {PERSISTENT, BOOL, "0"}},
|
||||||
|
{"dp_vag_pq_steering_patch", {PERSISTENT, BOOL, "0"}},
|
||||||
|
{"dp_vag_avoid_eps_lockout", {PERSISTENT, BOOL, "0"}},
|
||||||
|
{"dp_honda_nidec_stock_long", {PERSISTENT, BOOL, "0"}},
|
||||||
};
|
};
|
||||||
|
|||||||
@@ -3,9 +3,15 @@ from numbers import Number
|
|||||||
|
|
||||||
class PIDController:
|
class PIDController:
|
||||||
def __init__(self, k_p, k_i, k_d=0., pos_limit=1e308, neg_limit=-1e308, rate=100):
|
def __init__(self, k_p, k_i, k_d=0., pos_limit=1e308, neg_limit=-1e308, rate=100):
|
||||||
self._k_p: list[list[float]] = [[0], [k_p]] if isinstance(k_p, Number) else k_p
|
self._k_p = k_p
|
||||||
self._k_i: list[list[float]] = [[0], [k_i]] if isinstance(k_i, Number) else k_i
|
self._k_i = k_i
|
||||||
self._k_d: list[list[float]] = [[0], [k_d]] if isinstance(k_d, Number) else k_d
|
self._k_d = k_d
|
||||||
|
if isinstance(self._k_p, Number):
|
||||||
|
self._k_p = [[0], [self._k_p]]
|
||||||
|
if isinstance(self._k_i, Number):
|
||||||
|
self._k_i = [[0], [self._k_i]]
|
||||||
|
if isinstance(self._k_d, Number):
|
||||||
|
self._k_d = [[0], [self._k_d]]
|
||||||
|
|
||||||
self.set_limits(pos_limit, neg_limit)
|
self.set_limits(pos_limit, neg_limit)
|
||||||
|
|
||||||
|
|||||||
@@ -13,11 +13,7 @@ public:
|
|||||||
if (prefix.empty()) {
|
if (prefix.empty()) {
|
||||||
prefix = util::random_string(15);
|
prefix = util::random_string(15);
|
||||||
}
|
}
|
||||||
#ifdef __APPLE__
|
msgq_path = Path::shm_path() + "/" + prefix;
|
||||||
msgq_path = "/tmp/msgq_" + prefix;
|
|
||||||
#else
|
|
||||||
msgq_path = "/dev/shm/msgq_" + prefix;
|
|
||||||
#endif
|
|
||||||
bool ret = util::create_directories(msgq_path, 0777);
|
bool ret = util::create_directories(msgq_path, 0777);
|
||||||
assert(ret);
|
assert(ret);
|
||||||
setenv("OPENPILOT_PREFIX", prefix.c_str(), 1);
|
setenv("OPENPILOT_PREFIX", prefix.c_str(), 1);
|
||||||
@@ -27,14 +23,14 @@ public:
|
|||||||
auto param_path = Params().getParamPath();
|
auto param_path = Params().getParamPath();
|
||||||
if (util::file_exists(param_path)) {
|
if (util::file_exists(param_path)) {
|
||||||
std::string real_path = util::readlink(param_path);
|
std::string real_path = util::readlink(param_path);
|
||||||
util::check_system(util::string_format("rm %s -rf", real_path.c_str()));
|
system(util::string_format("rm %s -rf", real_path.c_str()).c_str());
|
||||||
unlink(param_path.c_str());
|
unlink(param_path.c_str());
|
||||||
}
|
}
|
||||||
if (getenv("COMMA_CACHE") == nullptr) {
|
if (getenv("COMMA_CACHE") == nullptr) {
|
||||||
util::check_system(util::string_format("rm %s -rf", Path::download_cache_root().c_str()));
|
system(util::string_format("rm %s -rf", Path::download_cache_root().c_str()).c_str());
|
||||||
}
|
}
|
||||||
util::check_system(util::string_format("rm %s -rf", Path::comma_home().c_str()));
|
system(util::string_format("rm %s -rf", Path::comma_home().c_str()).c_str());
|
||||||
util::check_system(util::string_format("rm %s -rf", msgq_path.c_str()));
|
system(util::string_format("rm %s -rf", msgq_path.c_str()).c_str());
|
||||||
unsetenv("OPENPILOT_PREFIX");
|
unsetenv("OPENPILOT_PREFIX");
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|||||||
@@ -1,5 +1,4 @@
|
|||||||
import os
|
import os
|
||||||
import platform
|
|
||||||
import shutil
|
import shutil
|
||||||
import uuid
|
import uuid
|
||||||
|
|
||||||
@@ -10,10 +9,9 @@ from openpilot.system.hardware.hw import Paths
|
|||||||
from openpilot.system.hardware.hw import DEFAULT_DOWNLOAD_CACHE_ROOT
|
from openpilot.system.hardware.hw import DEFAULT_DOWNLOAD_CACHE_ROOT
|
||||||
|
|
||||||
class OpenpilotPrefix:
|
class OpenpilotPrefix:
|
||||||
def __init__(self, prefix: str | None = None, create_dirs_on_enter: bool = True, clean_dirs_on_exit: bool = True, shared_download_cache: bool = False):
|
def __init__(self, prefix: str = None, create_dirs_on_enter: bool = True, clean_dirs_on_exit: bool = True, shared_download_cache: bool = False):
|
||||||
self.prefix = prefix if prefix else str(uuid.uuid4().hex[0:15])
|
self.prefix = prefix if prefix else str(uuid.uuid4().hex[0:15])
|
||||||
shm_path = "/tmp" if platform.system() == "Darwin" else "/dev/shm"
|
self.msgq_path = os.path.join(Paths.shm_path(), "msgq_" + self.prefix)
|
||||||
self.msgq_path = os.path.join(shm_path, "msgq_" + self.prefix)
|
|
||||||
self.create_dirs_on_enter = create_dirs_on_enter
|
self.create_dirs_on_enter = create_dirs_on_enter
|
||||||
self.clean_dirs_on_exit = clean_dirs_on_exit
|
self.clean_dirs_on_exit = clean_dirs_on_exit
|
||||||
self.shared_download_cache = shared_download_cache
|
self.shared_download_cache = shared_download_cache
|
||||||
|
|||||||
@@ -6,9 +6,9 @@
|
|||||||
#include "common/timing.h"
|
#include "common/timing.h"
|
||||||
#include "common/util.h"
|
#include "common/util.h"
|
||||||
|
|
||||||
RateKeeper::RateKeeper(const std::string &name_, float rate, float print_delay_threshold_)
|
RateKeeper::RateKeeper(const std::string &name, float rate, float print_delay_threshold)
|
||||||
: name(name_),
|
: name(name),
|
||||||
print_delay_threshold(std::max(0.f, print_delay_threshold_)) {
|
print_delay_threshold(std::max(0.f, print_delay_threshold)) {
|
||||||
interval = 1 / rate;
|
interval = 1 / rate;
|
||||||
last_monitor_time = seconds_since_boot();
|
last_monitor_time = seconds_since_boot();
|
||||||
next_frame_time = last_monitor_time + interval;
|
next_frame_time = last_monitor_time + interval;
|
||||||
|
|||||||
@@ -6,7 +6,7 @@ import time
|
|||||||
|
|
||||||
from setproctitle import getproctitle
|
from setproctitle import getproctitle
|
||||||
|
|
||||||
from openpilot.common.utils import MovingAverage
|
from openpilot.common.util import MovingAverage
|
||||||
from openpilot.system.hardware import PC
|
from openpilot.system.hardware import PC
|
||||||
|
|
||||||
|
|
||||||
|
|||||||
@@ -36,7 +36,7 @@ TEST_CASE("util::read_file") {
|
|||||||
REQUIRE(util::read_file(filename).empty());
|
REQUIRE(util::read_file(filename).empty());
|
||||||
|
|
||||||
std::string content = random_bytes(64 * 1024);
|
std::string content = random_bytes(64 * 1024);
|
||||||
REQUIRE(write(fd, content.c_str(), content.size()) == (ssize_t)content.size());
|
write(fd, content.c_str(), content.size());
|
||||||
std::string ret = util::read_file(filename);
|
std::string ret = util::read_file(filename);
|
||||||
bool equal = (ret == content);
|
bool equal = (ret == content);
|
||||||
REQUIRE(equal);
|
REQUIRE(equal);
|
||||||
@@ -114,12 +114,12 @@ TEST_CASE("util::safe_fwrite") {
|
|||||||
}
|
}
|
||||||
|
|
||||||
TEST_CASE("util::create_directories") {
|
TEST_CASE("util::create_directories") {
|
||||||
REQUIRE(system("rm /tmp/test_create_directories -rf") == 0);
|
system("rm /tmp/test_create_directories -rf");
|
||||||
std::string dir = "/tmp/test_create_directories/a/b/c/d/e/f";
|
std::string dir = "/tmp/test_create_directories/a/b/c/d/e/f";
|
||||||
|
|
||||||
auto check_dir_permissions = [](const std::string &path, mode_t mode) -> bool {
|
auto check_dir_permissions = [](const std::string &dir, mode_t mode) -> bool {
|
||||||
struct stat st = {};
|
struct stat st = {};
|
||||||
return stat(path.c_str(), &st) == 0 && (st.st_mode & S_IFMT) == S_IFDIR && (st.st_mode & (S_IRWXU | S_IRWXG | S_IRWXO)) == mode;
|
return stat(dir.c_str(), &st) == 0 && (st.st_mode & S_IFMT) == S_IFDIR && (st.st_mode & (S_IRWXU | S_IRWXG | S_IRWXO)) == mode;
|
||||||
};
|
};
|
||||||
|
|
||||||
SECTION("create_directories") {
|
SECTION("create_directories") {
|
||||||
@@ -132,7 +132,7 @@ TEST_CASE("util::create_directories") {
|
|||||||
}
|
}
|
||||||
SECTION("a file exists with the same name") {
|
SECTION("a file exists with the same name") {
|
||||||
REQUIRE(util::create_directories(dir, 0755));
|
REQUIRE(util::create_directories(dir, 0755));
|
||||||
int f = open((dir + "/file").c_str(), O_RDWR | O_CREAT, 0644);
|
int f = open((dir + "/file").c_str(), O_RDWR | O_CREAT);
|
||||||
REQUIRE(f != -1);
|
REQUIRE(f != -1);
|
||||||
close(f);
|
close(f);
|
||||||
REQUIRE(util::create_directories(dir + "/file", 0755) == false);
|
REQUIRE(util::create_directories(dir + "/file", 0755) == false);
|
||||||
|
|||||||
@@ -0,0 +1,2 @@
|
|||||||
|
transformations
|
||||||
|
transformations.cpp
|
||||||
@@ -0,0 +1,5 @@
|
|||||||
|
Import('env', 'envCython')
|
||||||
|
|
||||||
|
transformations = env.Library('transformations', ['orientation.cc', 'coordinates.cc'])
|
||||||
|
transformations_python = envCython.Program('transformations.so', 'transformations.pyx')
|
||||||
|
Export('transformations', 'transformations_python')
|
||||||
@@ -0,0 +1,100 @@
|
|||||||
|
#define _USE_MATH_DEFINES
|
||||||
|
|
||||||
|
#include "common/transformations/coordinates.hpp"
|
||||||
|
|
||||||
|
#include <iostream>
|
||||||
|
#include <cmath>
|
||||||
|
#include <eigen3/Eigen/Dense>
|
||||||
|
|
||||||
|
double a = 6378137; // lgtm [cpp/short-global-name]
|
||||||
|
double b = 6356752.3142; // lgtm [cpp/short-global-name]
|
||||||
|
double esq = 6.69437999014 * 0.001; // lgtm [cpp/short-global-name]
|
||||||
|
double e1sq = 6.73949674228 * 0.001;
|
||||||
|
|
||||||
|
|
||||||
|
static Geodetic to_degrees(Geodetic geodetic){
|
||||||
|
geodetic.lat = RAD2DEG(geodetic.lat);
|
||||||
|
geodetic.lon = RAD2DEG(geodetic.lon);
|
||||||
|
return geodetic;
|
||||||
|
}
|
||||||
|
|
||||||
|
static Geodetic to_radians(Geodetic geodetic){
|
||||||
|
geodetic.lat = DEG2RAD(geodetic.lat);
|
||||||
|
geodetic.lon = DEG2RAD(geodetic.lon);
|
||||||
|
return geodetic;
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
ECEF geodetic2ecef(const Geodetic &geodetic) {
|
||||||
|
auto g = to_radians(geodetic);
|
||||||
|
double xi = sqrt(1.0 - esq * pow(sin(g.lat), 2));
|
||||||
|
double x = (a / xi + g.alt) * cos(g.lat) * cos(g.lon);
|
||||||
|
double y = (a / xi + g.alt) * cos(g.lat) * sin(g.lon);
|
||||||
|
double z = (a / xi * (1.0 - esq) + g.alt) * sin(g.lat);
|
||||||
|
return {x, y, z};
|
||||||
|
}
|
||||||
|
|
||||||
|
Geodetic ecef2geodetic(const ECEF &e) {
|
||||||
|
// Convert from ECEF to geodetic using Ferrari's methods
|
||||||
|
// https://en.wikipedia.org/wiki/Geographic_coordinate_conversion#Ferrari.27s_solution
|
||||||
|
double x = e.x;
|
||||||
|
double y = e.y;
|
||||||
|
double z = e.z;
|
||||||
|
|
||||||
|
double r = sqrt(x * x + y * y);
|
||||||
|
double Esq = a * a - b * b;
|
||||||
|
double F = 54 * b * b * z * z;
|
||||||
|
double G = r * r + (1 - esq) * z * z - esq * Esq;
|
||||||
|
double C = (esq * esq * F * r * r) / (pow(G, 3));
|
||||||
|
double S = cbrt(1 + C + sqrt(C * C + 2 * C));
|
||||||
|
double P = F / (3 * pow((S + 1 / S + 1), 2) * G * G);
|
||||||
|
double Q = sqrt(1 + 2 * esq * esq * P);
|
||||||
|
double r_0 = -(P * esq * r) / (1 + Q) + sqrt(0.5 * a * a*(1 + 1.0 / Q) - P * (1 - esq) * z * z / (Q * (1 + Q)) - 0.5 * P * r * r);
|
||||||
|
double U = sqrt(pow((r - esq * r_0), 2) + z * z);
|
||||||
|
double V = sqrt(pow((r - esq * r_0), 2) + (1 - esq) * z * z);
|
||||||
|
double Z_0 = b * b * z / (a * V);
|
||||||
|
double h = U * (1 - b * b / (a * V));
|
||||||
|
|
||||||
|
double lat = atan((z + e1sq * Z_0) / r);
|
||||||
|
double lon = atan2(y, x);
|
||||||
|
|
||||||
|
return to_degrees({lat, lon, h});
|
||||||
|
}
|
||||||
|
|
||||||
|
LocalCoord::LocalCoord(const Geodetic &geodetic, const ECEF &e) {
|
||||||
|
init_ecef << e.x, e.y, e.z;
|
||||||
|
|
||||||
|
auto g = to_radians(geodetic);
|
||||||
|
|
||||||
|
ned2ecef_matrix <<
|
||||||
|
-sin(g.lat)*cos(g.lon), -sin(g.lon), -cos(g.lat)*cos(g.lon),
|
||||||
|
-sin(g.lat)*sin(g.lon), cos(g.lon), -cos(g.lat)*sin(g.lon),
|
||||||
|
cos(g.lat), 0, -sin(g.lat);
|
||||||
|
ecef2ned_matrix = ned2ecef_matrix.transpose();
|
||||||
|
}
|
||||||
|
|
||||||
|
NED LocalCoord::ecef2ned(const ECEF &e) {
|
||||||
|
Eigen::Vector3d ecef;
|
||||||
|
ecef << e.x, e.y, e.z;
|
||||||
|
|
||||||
|
Eigen::Vector3d ned = (ecef2ned_matrix * (ecef - init_ecef));
|
||||||
|
return {ned[0], ned[1], ned[2]};
|
||||||
|
}
|
||||||
|
|
||||||
|
ECEF LocalCoord::ned2ecef(const NED &n) {
|
||||||
|
Eigen::Vector3d ned;
|
||||||
|
ned << n.n, n.e, n.d;
|
||||||
|
|
||||||
|
Eigen::Vector3d ecef = (ned2ecef_matrix * ned) + init_ecef;
|
||||||
|
return {ecef[0], ecef[1], ecef[2]};
|
||||||
|
}
|
||||||
|
|
||||||
|
NED LocalCoord::geodetic2ned(const Geodetic &g) {
|
||||||
|
ECEF e = ::geodetic2ecef(g);
|
||||||
|
return ecef2ned(e);
|
||||||
|
}
|
||||||
|
|
||||||
|
Geodetic LocalCoord::ned2geodetic(const NED &n) {
|
||||||
|
ECEF e = ned2ecef(n);
|
||||||
|
return ::ecef2geodetic(e);
|
||||||
|
}
|
||||||
@@ -0,0 +1,43 @@
|
|||||||
|
#pragma once
|
||||||
|
|
||||||
|
#include <eigen3/Eigen/Dense>
|
||||||
|
|
||||||
|
#define DEG2RAD(x) ((x) * M_PI / 180.0)
|
||||||
|
#define RAD2DEG(x) ((x) * 180.0 / M_PI)
|
||||||
|
|
||||||
|
struct ECEF {
|
||||||
|
double x, y, z;
|
||||||
|
Eigen::Vector3d to_vector() const {
|
||||||
|
return Eigen::Vector3d(x, y, z);
|
||||||
|
}
|
||||||
|
};
|
||||||
|
|
||||||
|
struct NED {
|
||||||
|
double n, e, d;
|
||||||
|
Eigen::Vector3d to_vector() const {
|
||||||
|
return Eigen::Vector3d(n, e, d);
|
||||||
|
}
|
||||||
|
};
|
||||||
|
|
||||||
|
struct Geodetic {
|
||||||
|
double lat, lon, alt;
|
||||||
|
bool radians=false;
|
||||||
|
};
|
||||||
|
|
||||||
|
ECEF geodetic2ecef(const Geodetic &g);
|
||||||
|
Geodetic ecef2geodetic(const ECEF &e);
|
||||||
|
|
||||||
|
class LocalCoord {
|
||||||
|
public:
|
||||||
|
Eigen::Matrix3d ned2ecef_matrix;
|
||||||
|
Eigen::Matrix3d ecef2ned_matrix;
|
||||||
|
Eigen::Vector3d init_ecef;
|
||||||
|
LocalCoord(const Geodetic &g, const ECEF &e);
|
||||||
|
LocalCoord(const Geodetic &g) : LocalCoord(g, ::geodetic2ecef(g)) {}
|
||||||
|
LocalCoord(const ECEF &e) : LocalCoord(::ecef2geodetic(e), e) {}
|
||||||
|
|
||||||
|
NED ecef2ned(const ECEF &e);
|
||||||
|
ECEF ned2ecef(const NED &n);
|
||||||
|
NED geodetic2ned(const Geodetic &g);
|
||||||
|
Geodetic ned2geodetic(const NED &n);
|
||||||
|
};
|
||||||
@@ -0,0 +1,144 @@
|
|||||||
|
#define _USE_MATH_DEFINES
|
||||||
|
|
||||||
|
#include <iostream>
|
||||||
|
#include <cmath>
|
||||||
|
#include <eigen3/Eigen/Dense>
|
||||||
|
|
||||||
|
#include "common/transformations/orientation.hpp"
|
||||||
|
#include "common/transformations/coordinates.hpp"
|
||||||
|
|
||||||
|
Eigen::Quaterniond ensure_unique(const Eigen::Quaterniond &quat) {
|
||||||
|
if (quat.w() > 0){
|
||||||
|
return quat;
|
||||||
|
} else {
|
||||||
|
return Eigen::Quaterniond(-quat.w(), -quat.x(), -quat.y(), -quat.z());
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
Eigen::Quaterniond euler2quat(const Eigen::Vector3d &euler) {
|
||||||
|
Eigen::Quaterniond q;
|
||||||
|
|
||||||
|
q = Eigen::AngleAxisd(euler(2), Eigen::Vector3d::UnitZ())
|
||||||
|
* Eigen::AngleAxisd(euler(1), Eigen::Vector3d::UnitY())
|
||||||
|
* Eigen::AngleAxisd(euler(0), Eigen::Vector3d::UnitX());
|
||||||
|
return ensure_unique(q);
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
Eigen::Vector3d quat2euler(const Eigen::Quaterniond &quat) {
|
||||||
|
// TODO: switch to eigen implementation if the range of the Euler angles doesn't matter anymore
|
||||||
|
// Eigen::Vector3d euler = quat.toRotationMatrix().eulerAngles(2, 1, 0);
|
||||||
|
// return {euler(2), euler(1), euler(0)};
|
||||||
|
double gamma = atan2(2 * (quat.w() * quat.x() + quat.y() * quat.z()), 1 - 2 * (quat.x()*quat.x() + quat.y()*quat.y()));
|
||||||
|
double asin_arg_clipped = std::clamp(2 * (quat.w() * quat.y() - quat.z() * quat.x()), -1.0, 1.0);
|
||||||
|
double theta = asin(asin_arg_clipped);
|
||||||
|
double psi = atan2(2 * (quat.w() * quat.z() + quat.x() * quat.y()), 1 - 2 * (quat.y()*quat.y() + quat.z()*quat.z()));
|
||||||
|
return {gamma, theta, psi};
|
||||||
|
}
|
||||||
|
|
||||||
|
Eigen::Matrix3d quat2rot(const Eigen::Quaterniond &quat) {
|
||||||
|
return quat.toRotationMatrix();
|
||||||
|
}
|
||||||
|
|
||||||
|
Eigen::Quaterniond rot2quat(const Eigen::Matrix3d &rot) {
|
||||||
|
return ensure_unique(Eigen::Quaterniond(rot));
|
||||||
|
}
|
||||||
|
|
||||||
|
Eigen::Matrix3d euler2rot(const Eigen::Vector3d &euler) {
|
||||||
|
return quat2rot(euler2quat(euler));
|
||||||
|
}
|
||||||
|
|
||||||
|
Eigen::Vector3d rot2euler(const Eigen::Matrix3d &rot) {
|
||||||
|
return quat2euler(rot2quat(rot));
|
||||||
|
}
|
||||||
|
|
||||||
|
Eigen::Matrix3d rot_matrix(double roll, double pitch, double yaw) {
|
||||||
|
return euler2rot({roll, pitch, yaw});
|
||||||
|
}
|
||||||
|
|
||||||
|
Eigen::Matrix3d rot(const Eigen::Vector3d &axis, double angle) {
|
||||||
|
Eigen::Quaterniond q;
|
||||||
|
q = Eigen::AngleAxisd(angle, axis);
|
||||||
|
return q.toRotationMatrix();
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
Eigen::Vector3d ecef_euler_from_ned(const ECEF &ecef_init, const Eigen::Vector3d &ned_pose) {
|
||||||
|
/*
|
||||||
|
Using Rotations to Build Aerospace Coordinate Systems
|
||||||
|
Don Koks
|
||||||
|
https://apps.dtic.mil/dtic/tr/fulltext/u2/a484864.pdf
|
||||||
|
*/
|
||||||
|
LocalCoord converter = LocalCoord(ecef_init);
|
||||||
|
Eigen::Vector3d zero = ecef_init.to_vector();
|
||||||
|
|
||||||
|
Eigen::Vector3d x0 = converter.ned2ecef({1, 0, 0}).to_vector() - zero;
|
||||||
|
Eigen::Vector3d y0 = converter.ned2ecef({0, 1, 0}).to_vector() - zero;
|
||||||
|
Eigen::Vector3d z0 = converter.ned2ecef({0, 0, 1}).to_vector() - zero;
|
||||||
|
|
||||||
|
Eigen::Vector3d x1 = rot(z0, ned_pose(2)) * x0;
|
||||||
|
Eigen::Vector3d y1 = rot(z0, ned_pose(2)) * y0;
|
||||||
|
Eigen::Vector3d z1 = rot(z0, ned_pose(2)) * z0;
|
||||||
|
|
||||||
|
Eigen::Vector3d x2 = rot(y1, ned_pose(1)) * x1;
|
||||||
|
Eigen::Vector3d y2 = rot(y1, ned_pose(1)) * y1;
|
||||||
|
Eigen::Vector3d z2 = rot(y1, ned_pose(1)) * z1;
|
||||||
|
|
||||||
|
Eigen::Vector3d x3 = rot(x2, ned_pose(0)) * x2;
|
||||||
|
Eigen::Vector3d y3 = rot(x2, ned_pose(0)) * y2;
|
||||||
|
|
||||||
|
|
||||||
|
x0 = Eigen::Vector3d(1, 0, 0);
|
||||||
|
y0 = Eigen::Vector3d(0, 1, 0);
|
||||||
|
z0 = Eigen::Vector3d(0, 0, 1);
|
||||||
|
|
||||||
|
double psi = atan2(x3.dot(y0), x3.dot(x0));
|
||||||
|
double theta = atan2(-x3.dot(z0), sqrt(pow(x3.dot(x0), 2) + pow(x3.dot(y0), 2)));
|
||||||
|
|
||||||
|
y2 = rot(z0, psi) * y0;
|
||||||
|
z2 = rot(y2, theta) * z0;
|
||||||
|
|
||||||
|
double phi = atan2(y3.dot(z2), y3.dot(y2));
|
||||||
|
|
||||||
|
return {phi, theta, psi};
|
||||||
|
}
|
||||||
|
|
||||||
|
Eigen::Vector3d ned_euler_from_ecef(const ECEF &ecef_init, const Eigen::Vector3d &ecef_pose) {
|
||||||
|
/*
|
||||||
|
Using Rotations to Build Aerospace Coordinate Systems
|
||||||
|
Don Koks
|
||||||
|
https://apps.dtic.mil/dtic/tr/fulltext/u2/a484864.pdf
|
||||||
|
*/
|
||||||
|
LocalCoord converter = LocalCoord(ecef_init);
|
||||||
|
|
||||||
|
Eigen::Vector3d x0 = Eigen::Vector3d(1, 0, 0);
|
||||||
|
Eigen::Vector3d y0 = Eigen::Vector3d(0, 1, 0);
|
||||||
|
Eigen::Vector3d z0 = Eigen::Vector3d(0, 0, 1);
|
||||||
|
|
||||||
|
Eigen::Vector3d x1 = rot(z0, ecef_pose(2)) * x0;
|
||||||
|
Eigen::Vector3d y1 = rot(z0, ecef_pose(2)) * y0;
|
||||||
|
Eigen::Vector3d z1 = rot(z0, ecef_pose(2)) * z0;
|
||||||
|
|
||||||
|
Eigen::Vector3d x2 = rot(y1, ecef_pose(1)) * x1;
|
||||||
|
Eigen::Vector3d y2 = rot(y1, ecef_pose(1)) * y1;
|
||||||
|
Eigen::Vector3d z2 = rot(y1, ecef_pose(1)) * z1;
|
||||||
|
|
||||||
|
Eigen::Vector3d x3 = rot(x2, ecef_pose(0)) * x2;
|
||||||
|
Eigen::Vector3d y3 = rot(x2, ecef_pose(0)) * y2;
|
||||||
|
|
||||||
|
Eigen::Vector3d zero = ecef_init.to_vector();
|
||||||
|
x0 = converter.ned2ecef({1, 0, 0}).to_vector() - zero;
|
||||||
|
y0 = converter.ned2ecef({0, 1, 0}).to_vector() - zero;
|
||||||
|
z0 = converter.ned2ecef({0, 0, 1}).to_vector() - zero;
|
||||||
|
|
||||||
|
double psi = atan2(x3.dot(y0), x3.dot(x0));
|
||||||
|
double theta = atan2(-x3.dot(z0), sqrt(pow(x3.dot(x0), 2) + pow(x3.dot(y0), 2)));
|
||||||
|
|
||||||
|
y2 = rot(z0, psi) * y0;
|
||||||
|
z2 = rot(y2, theta) * z0;
|
||||||
|
|
||||||
|
double phi = atan2(y3.dot(z2), y3.dot(y2));
|
||||||
|
|
||||||
|
return {phi, theta, psi};
|
||||||
|
}
|
||||||
|
|
||||||
@@ -0,0 +1,17 @@
|
|||||||
|
#pragma once
|
||||||
|
#include <eigen3/Eigen/Dense>
|
||||||
|
#include "common/transformations/coordinates.hpp"
|
||||||
|
|
||||||
|
|
||||||
|
Eigen::Quaterniond ensure_unique(const Eigen::Quaterniond &quat);
|
||||||
|
|
||||||
|
Eigen::Quaterniond euler2quat(const Eigen::Vector3d &euler);
|
||||||
|
Eigen::Vector3d quat2euler(const Eigen::Quaterniond &quat);
|
||||||
|
Eigen::Matrix3d quat2rot(const Eigen::Quaterniond &quat);
|
||||||
|
Eigen::Quaterniond rot2quat(const Eigen::Matrix3d &rot);
|
||||||
|
Eigen::Matrix3d euler2rot(const Eigen::Vector3d &euler);
|
||||||
|
Eigen::Vector3d rot2euler(const Eigen::Matrix3d &rot);
|
||||||
|
Eigen::Matrix3d rot_matrix(double roll, double pitch, double yaw);
|
||||||
|
Eigen::Matrix3d rot(const Eigen::Vector3d &axis, double angle);
|
||||||
|
Eigen::Vector3d ecef_euler_from_ned(const ECEF &ecef_init, const Eigen::Vector3d &ned_pose);
|
||||||
|
Eigen::Vector3d ned_euler_from_ecef(const ECEF &ecef_init, const Eigen::Vector3d &ecef_pose);
|
||||||
@@ -102,36 +102,3 @@ class TestNED:
|
|||||||
np.testing.assert_allclose(converter.ned2ecef(ned_offsets_batch),
|
np.testing.assert_allclose(converter.ned2ecef(ned_offsets_batch),
|
||||||
ecef_positions_offset_batch,
|
ecef_positions_offset_batch,
|
||||||
rtol=1e-9, atol=1e-7)
|
rtol=1e-9, atol=1e-7)
|
||||||
|
|
||||||
def test_errors(self):
|
|
||||||
# Test wrong shape/type for geodetic2ecef
|
|
||||||
# numpy_wrap raises IndexError for scalar input
|
|
||||||
with np.testing.assert_raises(IndexError):
|
|
||||||
coord.geodetic2ecef(1.0)
|
|
||||||
|
|
||||||
with np.testing.assert_raises_regex(ValueError, "Geodetic must be size 3"):
|
|
||||||
coord.geodetic2ecef([0, 0])
|
|
||||||
|
|
||||||
with np.testing.assert_raises_regex(ValueError, "Geodetic must be size 3"):
|
|
||||||
coord.geodetic2ecef([0, 0, 0, 0])
|
|
||||||
|
|
||||||
with np.testing.assert_raises(TypeError):
|
|
||||||
coord.geodetic2ecef(['a', 'b', 'c'])
|
|
||||||
|
|
||||||
# Test LocalCoord constructor errors
|
|
||||||
with np.testing.assert_raises(ValueError):
|
|
||||||
coord.LocalCoord.from_geodetic([0, 0])
|
|
||||||
|
|
||||||
with np.testing.assert_raises(ValueError):
|
|
||||||
coord.LocalCoord.from_geodetic(1)
|
|
||||||
|
|
||||||
with np.testing.assert_raises(TypeError):
|
|
||||||
coord.LocalCoord.from_geodetic(['a', 'b', 'c'])
|
|
||||||
|
|
||||||
# Test wrong shape/type for ecef2geodetic
|
|
||||||
with np.testing.assert_raises(ValueError):
|
|
||||||
coord.ecef2geodetic([1, 2])
|
|
||||||
with np.testing.assert_raises(ValueError):
|
|
||||||
coord.ecef2geodetic([1, 2, 3, 4])
|
|
||||||
with np.testing.assert_raises(IndexError):
|
|
||||||
coord.ecef2geodetic(1.0)
|
|
||||||
|
|||||||
@@ -1,5 +1,4 @@
|
|||||||
import numpy as np
|
import numpy as np
|
||||||
import pytest
|
|
||||||
|
|
||||||
from openpilot.common.transformations.orientation import euler2quat, quat2euler, euler2rot, rot2euler, \
|
from openpilot.common.transformations.orientation import euler2quat, quat2euler, euler2rot, rot2euler, \
|
||||||
rot2quat, quat2rot, \
|
rot2quat, quat2rot, \
|
||||||
@@ -60,32 +59,3 @@ class TestOrientation:
|
|||||||
np.testing.assert_allclose(ned_eulers[i], ned_euler_from_ecef(ecef_positions[i], eulers[i]), rtol=1e-7)
|
np.testing.assert_allclose(ned_eulers[i], ned_euler_from_ecef(ecef_positions[i], eulers[i]), rtol=1e-7)
|
||||||
#np.testing.assert_allclose(eulers[i], ecef_euler_from_ned(ecef_positions[i], ned_eulers[i]), rtol=1e-7)
|
#np.testing.assert_allclose(eulers[i], ecef_euler_from_ned(ecef_positions[i], ned_eulers[i]), rtol=1e-7)
|
||||||
# np.testing.assert_allclose(ned_eulers, ned_euler_from_ecef(ecef_positions, eulers), rtol=1e-7)
|
# np.testing.assert_allclose(ned_eulers, ned_euler_from_ecef(ecef_positions, eulers), rtol=1e-7)
|
||||||
|
|
||||||
def test_inputs(self):
|
|
||||||
with pytest.raises(ValueError):
|
|
||||||
euler2quat([1, 2])
|
|
||||||
|
|
||||||
with pytest.raises(ValueError):
|
|
||||||
quat2rot([1, 2, 3])
|
|
||||||
|
|
||||||
with pytest.raises(IndexError):
|
|
||||||
rot2quat(np.zeros((2, 2)))
|
|
||||||
|
|
||||||
def test_euler_rot_consistency(self):
|
|
||||||
rpy = [0.1, 0.2, 0.3]
|
|
||||||
R = euler2rot(rpy)
|
|
||||||
|
|
||||||
# R -> q -> R
|
|
||||||
q = rot2quat(R)
|
|
||||||
R_new = quat2rot(q)
|
|
||||||
np.testing.assert_allclose(R, R_new, atol=1e-15)
|
|
||||||
|
|
||||||
# q -> R -> Euler (quat2euler) -> R
|
|
||||||
rpy_new = quat2euler(q)
|
|
||||||
R_new2 = euler2rot(rpy_new)
|
|
||||||
np.testing.assert_allclose(R, R_new2, atol=1e-15)
|
|
||||||
|
|
||||||
# R -> Euler (rot2euler) -> R
|
|
||||||
rpy_from_rot = rot2euler(R)
|
|
||||||
R_new3 = euler2rot(rpy_from_rot)
|
|
||||||
np.testing.assert_allclose(R, R_new3, atol=1e-15)
|
|
||||||
|
|||||||
@@ -0,0 +1,72 @@
|
|||||||
|
# cython: language_level=3
|
||||||
|
from libcpp cimport bool
|
||||||
|
|
||||||
|
cdef extern from "orientation.cc":
|
||||||
|
pass
|
||||||
|
|
||||||
|
cdef extern from "orientation.hpp":
|
||||||
|
cdef cppclass Quaternion "Eigen::Quaterniond":
|
||||||
|
Quaternion()
|
||||||
|
Quaternion(double, double, double, double)
|
||||||
|
double w()
|
||||||
|
double x()
|
||||||
|
double y()
|
||||||
|
double z()
|
||||||
|
|
||||||
|
cdef cppclass Vector3 "Eigen::Vector3d":
|
||||||
|
Vector3()
|
||||||
|
Vector3(double, double, double)
|
||||||
|
double operator()(int)
|
||||||
|
|
||||||
|
cdef cppclass Matrix3 "Eigen::Matrix3d":
|
||||||
|
Matrix3()
|
||||||
|
Matrix3(double*)
|
||||||
|
|
||||||
|
double operator()(int, int)
|
||||||
|
|
||||||
|
Quaternion euler2quat(const Vector3 &)
|
||||||
|
Vector3 quat2euler(const Quaternion &)
|
||||||
|
Matrix3 quat2rot(const Quaternion &)
|
||||||
|
Quaternion rot2quat(const Matrix3 &)
|
||||||
|
Vector3 rot2euler(const Matrix3 &)
|
||||||
|
Matrix3 euler2rot(const Vector3 &)
|
||||||
|
Matrix3 rot_matrix(double, double, double)
|
||||||
|
Vector3 ecef_euler_from_ned(const ECEF &, const Vector3 &)
|
||||||
|
Vector3 ned_euler_from_ecef(const ECEF &, const Vector3 &)
|
||||||
|
|
||||||
|
|
||||||
|
cdef extern from "coordinates.cc":
|
||||||
|
cdef struct ECEF:
|
||||||
|
double x
|
||||||
|
double y
|
||||||
|
double z
|
||||||
|
|
||||||
|
cdef struct NED:
|
||||||
|
double n
|
||||||
|
double e
|
||||||
|
double d
|
||||||
|
|
||||||
|
cdef struct Geodetic:
|
||||||
|
double lat
|
||||||
|
double lon
|
||||||
|
double alt
|
||||||
|
bool radians
|
||||||
|
|
||||||
|
ECEF geodetic2ecef(const Geodetic &)
|
||||||
|
Geodetic ecef2geodetic(const ECEF &)
|
||||||
|
|
||||||
|
cdef cppclass LocalCoord_c "LocalCoord":
|
||||||
|
Matrix3 ned2ecef_matrix
|
||||||
|
Matrix3 ecef2ned_matrix
|
||||||
|
|
||||||
|
LocalCoord_c(const Geodetic &, const ECEF &)
|
||||||
|
LocalCoord_c(const Geodetic &)
|
||||||
|
LocalCoord_c(const ECEF &)
|
||||||
|
|
||||||
|
NED ecef2ned(const ECEF &)
|
||||||
|
ECEF ned2ecef(const NED &)
|
||||||
|
NED geodetic2ned(const Geodetic &)
|
||||||
|
Geodetic ned2geodetic(const NED &)
|
||||||
|
|
||||||
|
cdef extern from "coordinates.hpp":
|
||||||
|
pass
|
||||||
@@ -1,342 +0,0 @@
|
|||||||
import numpy as np
|
|
||||||
|
|
||||||
|
|
||||||
# Constants
|
|
||||||
a = 6378137.0
|
|
||||||
b = 6356752.3142
|
|
||||||
esq = 6.69437999014e-3
|
|
||||||
e1sq = 6.73949674228e-3
|
|
||||||
|
|
||||||
|
|
||||||
def geodetic2ecef_single(g):
|
|
||||||
"""
|
|
||||||
Convert geodetic coordinates (latitude, longitude, altitude) to ECEF.
|
|
||||||
"""
|
|
||||||
try:
|
|
||||||
if len(g) != 3:
|
|
||||||
raise ValueError("Geodetic must be size 3")
|
|
||||||
except TypeError:
|
|
||||||
raise ValueError("Geodetic must be a sequence of length 3") from None
|
|
||||||
|
|
||||||
lat, lon, alt = g
|
|
||||||
lat = np.radians(lat)
|
|
||||||
lon = np.radians(lon)
|
|
||||||
xi = np.sqrt(1.0 - esq * np.sin(lat)**2)
|
|
||||||
x = (a / xi + alt) * np.cos(lat) * np.cos(lon)
|
|
||||||
y = (a / xi + alt) * np.cos(lat) * np.sin(lon)
|
|
||||||
z = (a / xi * (1.0 - esq) + alt) * np.sin(lat)
|
|
||||||
return np.array([x, y, z])
|
|
||||||
|
|
||||||
|
|
||||||
def ecef2geodetic_single(e):
|
|
||||||
"""
|
|
||||||
Convert ECEF to geodetic coordinates using Ferrari's solution.
|
|
||||||
"""
|
|
||||||
x, y, z = e
|
|
||||||
r = np.sqrt(x**2 + y**2)
|
|
||||||
Esq = a**2 - b**2
|
|
||||||
F = 54 * b**2 * z**2
|
|
||||||
G = r**2 + (1 - esq) * z**2 - esq * Esq
|
|
||||||
C = (esq**2 * F * r**2) / (G**3)
|
|
||||||
S = np.cbrt(1 + C + np.sqrt(C**2 + 2 * C))
|
|
||||||
P = F / (3 * (S + 1 / S + 1)**2 * G**2)
|
|
||||||
Q = np.sqrt(1 + 2 * esq**2 * P)
|
|
||||||
r_0 = -(P * esq * r) / (1 + Q) + np.sqrt(0.5 * a**2 * (1 + 1.0 / Q) - P * (1 - esq) * z**2 / (Q * (1 + Q)) - 0.5 * P * r**2)
|
|
||||||
U = np.sqrt((r - esq * r_0)**2 + z**2)
|
|
||||||
V = np.sqrt((r - esq * r_0)**2 + (1 - esq) * z**2)
|
|
||||||
Z_0 = b**2 * z / (a * V)
|
|
||||||
h = U * (1 - b**2 / (a * V))
|
|
||||||
lat = np.arctan((z + e1sq * Z_0) / r)
|
|
||||||
lon = np.arctan2(y, x)
|
|
||||||
return np.array([np.degrees(lat), np.degrees(lon), h])
|
|
||||||
|
|
||||||
|
|
||||||
def euler2quat_single(euler):
|
|
||||||
"""
|
|
||||||
Convert Euler angles (roll, pitch, yaw) to a quaternion.
|
|
||||||
Rotation order: Z-Y-X (yaw, pitch, roll).
|
|
||||||
"""
|
|
||||||
phi, theta, psi = euler
|
|
||||||
|
|
||||||
c_phi, s_phi = np.cos(phi / 2), np.sin(phi / 2)
|
|
||||||
c_theta, s_theta = np.cos(theta / 2), np.sin(theta / 2)
|
|
||||||
c_psi, s_psi = np.cos(psi / 2), np.sin(psi / 2)
|
|
||||||
|
|
||||||
w = c_phi * c_theta * c_psi + s_phi * s_theta * s_psi
|
|
||||||
x = s_phi * c_theta * c_psi - c_phi * s_theta * s_psi
|
|
||||||
y = c_phi * s_theta * c_psi + s_phi * c_theta * s_psi
|
|
||||||
z = c_phi * c_theta * s_psi - s_phi * s_theta * c_psi
|
|
||||||
|
|
||||||
if w < 0:
|
|
||||||
return np.array([-w, -x, -y, -z])
|
|
||||||
return np.array([w, x, y, z])
|
|
||||||
|
|
||||||
|
|
||||||
def quat2euler_single(q):
|
|
||||||
"""
|
|
||||||
Convert a quaternion to Euler angles (roll, pitch, yaw).
|
|
||||||
"""
|
|
||||||
w, x, y, z = q
|
|
||||||
gamma = np.arctan2(2 * (w * x + y * z), 1 - 2 * (x**2 + y**2))
|
|
||||||
sin_arg = 2 * (w * y - z * x)
|
|
||||||
sin_arg = np.clip(sin_arg, -1.0, 1.0)
|
|
||||||
theta = np.arcsin(sin_arg)
|
|
||||||
psi = np.arctan2(2 * (w * z + x * y), 1 - 2 * (y**2 + z**2))
|
|
||||||
return np.array([gamma, theta, psi])
|
|
||||||
|
|
||||||
|
|
||||||
def quat2rot_single(q):
|
|
||||||
"""
|
|
||||||
Convert a quaternion to a 3x3 rotation matrix.
|
|
||||||
"""
|
|
||||||
w, x, y, z = q
|
|
||||||
xx, yy, zz = x * x, y * y, z * z
|
|
||||||
xy, xz, yz = x * y, x * z, y * z
|
|
||||||
wx, wy, wz = w * x, w * y, w * z
|
|
||||||
|
|
||||||
mat = np.array([
|
|
||||||
[1 - 2 * (yy + zz), 2 * (xy - wz), 2 * (xz + wy)],
|
|
||||||
[2 * (xy + wz), 1 - 2 * (xx + zz), 2 * (yz - wx)],
|
|
||||||
[2 * (xz - wy), 2 * (yz + wx), 1 - 2 * (xx + yy)]
|
|
||||||
])
|
|
||||||
return mat
|
|
||||||
|
|
||||||
|
|
||||||
def rot2quat_single(rot):
|
|
||||||
"""
|
|
||||||
Convert a 3x3 rotation matrix to a quaternion.
|
|
||||||
"""
|
|
||||||
trace = np.trace(rot)
|
|
||||||
if trace > 0:
|
|
||||||
s = 0.5 / np.sqrt(trace + 1.0)
|
|
||||||
w = 0.25 / s
|
|
||||||
x = (rot[2, 1] - rot[1, 2]) * s
|
|
||||||
y = (rot[0, 2] - rot[2, 0]) * s
|
|
||||||
z = (rot[1, 0] - rot[0, 1]) * s
|
|
||||||
else:
|
|
||||||
if rot[0, 0] > rot[1, 1] and rot[0, 0] > rot[2, 2]:
|
|
||||||
s = 2.0 * np.sqrt(1.0 + rot[0, 0] - rot[1, 1] - rot[2, 2])
|
|
||||||
w = (rot[2, 1] - rot[1, 2]) / s
|
|
||||||
x = 0.25 * s
|
|
||||||
y = (rot[0, 1] + rot[1, 0]) / s
|
|
||||||
z = (rot[0, 2] + rot[2, 0]) / s
|
|
||||||
elif rot[1, 1] > rot[2, 2]:
|
|
||||||
s = 2.0 * np.sqrt(1.0 + rot[1, 1] - rot[0, 0] - rot[2, 2])
|
|
||||||
w = (rot[0, 2] - rot[2, 0]) / s
|
|
||||||
x = (rot[0, 1] + rot[1, 0]) / s
|
|
||||||
y = 0.25 * s
|
|
||||||
z = (rot[1, 2] + rot[2, 1]) / s
|
|
||||||
else:
|
|
||||||
s = 2.0 * np.sqrt(1.0 + rot[2, 2] - rot[0, 0] - rot[1, 1])
|
|
||||||
w = (rot[1, 0] - rot[0, 1]) / s
|
|
||||||
x = (rot[0, 2] + rot[2, 0]) / s
|
|
||||||
y = (rot[1, 2] + rot[2, 1]) / s
|
|
||||||
z = 0.25 * s
|
|
||||||
|
|
||||||
if w < 0:
|
|
||||||
return np.array([-w, -x, -y, -z])
|
|
||||||
return np.array([w, x, y, z])
|
|
||||||
|
|
||||||
|
|
||||||
def euler2rot_single(euler):
|
|
||||||
"""
|
|
||||||
Convert Euler angles (roll, pitch, yaw) to a 3x3 rotation matrix.
|
|
||||||
Rotation order: Z-Y-X (yaw, pitch, roll).
|
|
||||||
"""
|
|
||||||
phi, theta, psi = euler
|
|
||||||
|
|
||||||
cx, sx = np.cos(phi), np.sin(phi)
|
|
||||||
cy, sy = np.cos(theta), np.sin(theta)
|
|
||||||
cz, sz = np.cos(psi), np.sin(psi)
|
|
||||||
|
|
||||||
Rx = np.array([[1, 0, 0], [0, cx, -sx], [0, sx, cx]])
|
|
||||||
Ry = np.array([[cy, 0, sy], [0, 1, 0], [-sy, 0, cy]])
|
|
||||||
Rz = np.array([[cz, -sz, 0], [sz, cz, 0], [0, 0, 1]])
|
|
||||||
|
|
||||||
return Rz @ Ry @ Rx
|
|
||||||
|
|
||||||
|
|
||||||
def rot2euler_single(rot):
|
|
||||||
"""
|
|
||||||
Convert a 3x3 rotation matrix to Euler angles (roll, pitch, yaw).
|
|
||||||
"""
|
|
||||||
return quat2euler_single(rot2quat_single(rot))
|
|
||||||
|
|
||||||
|
|
||||||
def rot_matrix(roll, pitch, yaw):
|
|
||||||
"""
|
|
||||||
Create a 3x3 rotation matrix from roll, pitch, and yaw angles.
|
|
||||||
"""
|
|
||||||
return euler2rot_single([roll, pitch, yaw])
|
|
||||||
|
|
||||||
|
|
||||||
def axis_angle_to_rot(axis, angle):
|
|
||||||
"""
|
|
||||||
Convert an axis-angle representation to a 3x3 rotation matrix.
|
|
||||||
"""
|
|
||||||
c = np.cos(angle / 2)
|
|
||||||
s = np.sin(angle / 2)
|
|
||||||
q = np.array([c, s*axis[0], s*axis[1], s*axis[2]])
|
|
||||||
return quat2rot_single(q)
|
|
||||||
|
|
||||||
|
|
||||||
class LocalCoord:
|
|
||||||
"""
|
|
||||||
A class to handle conversions between ECEF and local NED coordinates.
|
|
||||||
"""
|
|
||||||
def __init__(self, geodetic=None, ecef=None):
|
|
||||||
"""
|
|
||||||
Initialize LocalCoord with either geodetic or ECEF coordinates.
|
|
||||||
"""
|
|
||||||
if geodetic is not None:
|
|
||||||
self.init_ecef = geodetic2ecef_single(geodetic)
|
|
||||||
lat, lon, _ = geodetic
|
|
||||||
elif ecef is not None:
|
|
||||||
self.init_ecef = np.array(ecef)
|
|
||||||
lat, lon, _ = ecef2geodetic_single(ecef)
|
|
||||||
else:
|
|
||||||
raise ValueError("Must provide geodetic or ecef")
|
|
||||||
|
|
||||||
lat = np.radians(lat)
|
|
||||||
lon = np.radians(lon)
|
|
||||||
|
|
||||||
self.ned2ecef_matrix = np.array([
|
|
||||||
[-np.sin(lat) * np.cos(lon), -np.sin(lon), -np.cos(lat) * np.cos(lon)],
|
|
||||||
[-np.sin(lat) * np.sin(lon), np.cos(lon), -np.cos(lat) * np.sin(lon)],
|
|
||||||
[np.cos(lat), 0, -np.sin(lat)]
|
|
||||||
])
|
|
||||||
self.ecef2ned_matrix = self.ned2ecef_matrix.T
|
|
||||||
|
|
||||||
@classmethod
|
|
||||||
def from_geodetic(cls, geodetic):
|
|
||||||
"""
|
|
||||||
Create a LocalCoord instance from geodetic coordinates.
|
|
||||||
"""
|
|
||||||
return cls(geodetic=geodetic)
|
|
||||||
|
|
||||||
@classmethod
|
|
||||||
def from_ecef(cls, ecef):
|
|
||||||
"""
|
|
||||||
Create a LocalCoord instance from ECEF coordinates.
|
|
||||||
"""
|
|
||||||
return cls(ecef=ecef)
|
|
||||||
|
|
||||||
def ecef2ned_single(self, ecef):
|
|
||||||
"""
|
|
||||||
Convert a single ECEF point to NED coordinates relative to the origin.
|
|
||||||
"""
|
|
||||||
return self.ecef2ned_matrix @ (ecef - self.init_ecef)
|
|
||||||
|
|
||||||
def ned2ecef_single(self, ned):
|
|
||||||
"""
|
|
||||||
Convert a single NED point to ECEF coordinates.
|
|
||||||
"""
|
|
||||||
return self.ned2ecef_matrix @ ned + self.init_ecef
|
|
||||||
|
|
||||||
def geodetic2ned_single(self, geodetic):
|
|
||||||
"""
|
|
||||||
Convert a single geodetic point to NED coordinates.
|
|
||||||
"""
|
|
||||||
ecef = geodetic2ecef_single(geodetic)
|
|
||||||
return self.ecef2ned_single(ecef)
|
|
||||||
|
|
||||||
def ned2geodetic_single(self, ned):
|
|
||||||
"""
|
|
||||||
Convert a single NED point to geodetic coordinates.
|
|
||||||
"""
|
|
||||||
ecef = self.ned2ecef_single(ned)
|
|
||||||
return ecef2geodetic_single(ecef)
|
|
||||||
|
|
||||||
@property
|
|
||||||
def ned_from_ecef_matrix(self):
|
|
||||||
"""
|
|
||||||
Returns the rotation matrix from ECEF to NED coordinates.
|
|
||||||
"""
|
|
||||||
return self.ecef2ned_matrix
|
|
||||||
|
|
||||||
@property
|
|
||||||
def ecef_from_ned_matrix(self):
|
|
||||||
"""
|
|
||||||
Returns the rotation matrix from NED to ECEF coordinates.
|
|
||||||
"""
|
|
||||||
return self.ned2ecef_matrix
|
|
||||||
|
|
||||||
|
|
||||||
def ecef_euler_from_ned_single(ecef_init, ned_pose):
|
|
||||||
"""
|
|
||||||
Convert NED Euler angles (roll, pitch, yaw) at a given ECEF origin
|
|
||||||
to equivalent ECEF Euler angles.
|
|
||||||
"""
|
|
||||||
converter = LocalCoord(ecef=ecef_init)
|
|
||||||
zero = np.array(ecef_init)
|
|
||||||
|
|
||||||
x0 = converter.ned2ecef_single([1, 0, 0]) - zero
|
|
||||||
y0 = converter.ned2ecef_single([0, 1, 0]) - zero
|
|
||||||
z0 = converter.ned2ecef_single([0, 0, 1]) - zero
|
|
||||||
|
|
||||||
phi, theta, psi = ned_pose
|
|
||||||
|
|
||||||
x1 = axis_angle_to_rot(z0, psi) @ x0
|
|
||||||
y1 = axis_angle_to_rot(z0, psi) @ y0
|
|
||||||
z1 = axis_angle_to_rot(z0, psi) @ z0
|
|
||||||
|
|
||||||
x2 = axis_angle_to_rot(y1, theta) @ x1
|
|
||||||
y2 = axis_angle_to_rot(y1, theta) @ y1
|
|
||||||
z2 = axis_angle_to_rot(y1, theta) @ z1
|
|
||||||
|
|
||||||
x3 = axis_angle_to_rot(x2, phi) @ x2
|
|
||||||
y3 = axis_angle_to_rot(x2, phi) @ y2
|
|
||||||
|
|
||||||
x0 = np.array([1.0, 0, 0])
|
|
||||||
y0 = np.array([0, 1.0, 0])
|
|
||||||
z0 = np.array([0, 0, 1.0])
|
|
||||||
|
|
||||||
psi_out = np.arctan2(np.dot(x3, y0), np.dot(x3, x0))
|
|
||||||
theta_out = np.arctan2(-np.dot(x3, z0), np.sqrt(np.dot(x3, x0)**2 + np.dot(x3, y0)**2))
|
|
||||||
|
|
||||||
y2 = axis_angle_to_rot(z0, psi_out) @ y0
|
|
||||||
z2 = axis_angle_to_rot(y2, theta_out) @ z0
|
|
||||||
|
|
||||||
phi_out = np.arctan2(np.dot(y3, z2), np.dot(y3, y2))
|
|
||||||
|
|
||||||
return np.array([phi_out, theta_out, psi_out])
|
|
||||||
|
|
||||||
|
|
||||||
def ned_euler_from_ecef_single(ecef_init, ecef_pose):
|
|
||||||
"""
|
|
||||||
Convert ECEF Euler angles (roll, pitch, yaw) at a given ECEF origin
|
|
||||||
to equivalent NED Euler angles.
|
|
||||||
"""
|
|
||||||
converter = LocalCoord(ecef=ecef_init)
|
|
||||||
|
|
||||||
x0 = np.array([1.0, 0, 0])
|
|
||||||
y0 = np.array([0, 1.0, 0])
|
|
||||||
z0 = np.array([0, 0, 1.0])
|
|
||||||
|
|
||||||
phi, theta, psi = ecef_pose
|
|
||||||
|
|
||||||
x1 = axis_angle_to_rot(z0, psi) @ x0
|
|
||||||
y1 = axis_angle_to_rot(z0, psi) @ y0
|
|
||||||
z1 = axis_angle_to_rot(z0, psi) @ z0
|
|
||||||
|
|
||||||
x2 = axis_angle_to_rot(y1, theta) @ x1
|
|
||||||
y2 = axis_angle_to_rot(y1, theta) @ y1
|
|
||||||
z2 = axis_angle_to_rot(y1, theta) @ z1
|
|
||||||
|
|
||||||
x3 = axis_angle_to_rot(x2, phi) @ x2
|
|
||||||
y3 = axis_angle_to_rot(x2, phi) @ y2
|
|
||||||
|
|
||||||
zero = np.array(ecef_init)
|
|
||||||
x0 = converter.ned2ecef_single([1, 0, 0]) - zero
|
|
||||||
y0 = converter.ned2ecef_single([0, 1, 0]) - zero
|
|
||||||
z0 = converter.ned2ecef_single([0, 0, 1]) - zero
|
|
||||||
|
|
||||||
psi_out = np.arctan2(np.dot(x3, y0), np.dot(x3, x0))
|
|
||||||
theta_out = np.arctan2(-np.dot(x3, z0), np.sqrt(np.dot(x3, x0)**2 + np.dot(x3, y0)**2))
|
|
||||||
|
|
||||||
y2 = axis_angle_to_rot(z0, psi_out) @ y0
|
|
||||||
z2 = axis_angle_to_rot(y2, theta_out) @ z0
|
|
||||||
|
|
||||||
phi_out = np.arctan2(np.dot(y3, z2), np.dot(y3, y2))
|
|
||||||
|
|
||||||
return np.array([phi_out, theta_out, psi_out])
|
|
||||||
@@ -0,0 +1,173 @@
|
|||||||
|
# distutils: language = c++
|
||||||
|
# cython: language_level = 3
|
||||||
|
from openpilot.common.transformations.transformations cimport Matrix3, Vector3, Quaternion
|
||||||
|
from openpilot.common.transformations.transformations cimport ECEF, NED, Geodetic
|
||||||
|
|
||||||
|
from openpilot.common.transformations.transformations cimport euler2quat as euler2quat_c
|
||||||
|
from openpilot.common.transformations.transformations cimport quat2euler as quat2euler_c
|
||||||
|
from openpilot.common.transformations.transformations cimport quat2rot as quat2rot_c
|
||||||
|
from openpilot.common.transformations.transformations cimport rot2quat as rot2quat_c
|
||||||
|
from openpilot.common.transformations.transformations cimport euler2rot as euler2rot_c
|
||||||
|
from openpilot.common.transformations.transformations cimport rot2euler as rot2euler_c
|
||||||
|
from openpilot.common.transformations.transformations cimport rot_matrix as rot_matrix_c
|
||||||
|
from openpilot.common.transformations.transformations cimport ecef_euler_from_ned as ecef_euler_from_ned_c
|
||||||
|
from openpilot.common.transformations.transformations cimport ned_euler_from_ecef as ned_euler_from_ecef_c
|
||||||
|
from openpilot.common.transformations.transformations cimport geodetic2ecef as geodetic2ecef_c
|
||||||
|
from openpilot.common.transformations.transformations cimport ecef2geodetic as ecef2geodetic_c
|
||||||
|
from openpilot.common.transformations.transformations cimport LocalCoord_c
|
||||||
|
|
||||||
|
|
||||||
|
import numpy as np
|
||||||
|
cimport numpy as np
|
||||||
|
|
||||||
|
cdef np.ndarray[double, ndim=2] matrix2numpy(Matrix3 m):
|
||||||
|
return np.array([
|
||||||
|
[m(0, 0), m(0, 1), m(0, 2)],
|
||||||
|
[m(1, 0), m(1, 1), m(1, 2)],
|
||||||
|
[m(2, 0), m(2, 1), m(2, 2)],
|
||||||
|
])
|
||||||
|
|
||||||
|
cdef Matrix3 numpy2matrix(np.ndarray[double, ndim=2, mode="fortran"] m):
|
||||||
|
assert m.shape[0] == 3
|
||||||
|
assert m.shape[1] == 3
|
||||||
|
return Matrix3(<double*>m.data)
|
||||||
|
|
||||||
|
cdef ECEF list2ecef(ecef):
|
||||||
|
cdef ECEF e
|
||||||
|
e.x = ecef[0]
|
||||||
|
e.y = ecef[1]
|
||||||
|
e.z = ecef[2]
|
||||||
|
return e
|
||||||
|
|
||||||
|
cdef NED list2ned(ned):
|
||||||
|
cdef NED n
|
||||||
|
n.n = ned[0]
|
||||||
|
n.e = ned[1]
|
||||||
|
n.d = ned[2]
|
||||||
|
return n
|
||||||
|
|
||||||
|
cdef Geodetic list2geodetic(geodetic):
|
||||||
|
cdef Geodetic g
|
||||||
|
g.lat = geodetic[0]
|
||||||
|
g.lon = geodetic[1]
|
||||||
|
g.alt = geodetic[2]
|
||||||
|
return g
|
||||||
|
|
||||||
|
def euler2quat_single(euler):
|
||||||
|
cdef Vector3 e = Vector3(euler[0], euler[1], euler[2])
|
||||||
|
cdef Quaternion q = euler2quat_c(e)
|
||||||
|
return [q.w(), q.x(), q.y(), q.z()]
|
||||||
|
|
||||||
|
def quat2euler_single(quat):
|
||||||
|
cdef Quaternion q = Quaternion(quat[0], quat[1], quat[2], quat[3])
|
||||||
|
cdef Vector3 e = quat2euler_c(q)
|
||||||
|
return [e(0), e(1), e(2)]
|
||||||
|
|
||||||
|
def quat2rot_single(quat):
|
||||||
|
cdef Quaternion q = Quaternion(quat[0], quat[1], quat[2], quat[3])
|
||||||
|
cdef Matrix3 r = quat2rot_c(q)
|
||||||
|
return matrix2numpy(r)
|
||||||
|
|
||||||
|
def rot2quat_single(rot):
|
||||||
|
cdef Matrix3 r = numpy2matrix(np.asfortranarray(rot, dtype=np.double))
|
||||||
|
cdef Quaternion q = rot2quat_c(r)
|
||||||
|
return [q.w(), q.x(), q.y(), q.z()]
|
||||||
|
|
||||||
|
def euler2rot_single(euler):
|
||||||
|
cdef Vector3 e = Vector3(euler[0], euler[1], euler[2])
|
||||||
|
cdef Matrix3 r = euler2rot_c(e)
|
||||||
|
return matrix2numpy(r)
|
||||||
|
|
||||||
|
def rot2euler_single(rot):
|
||||||
|
cdef Matrix3 r = numpy2matrix(np.asfortranarray(rot, dtype=np.double))
|
||||||
|
cdef Vector3 e = rot2euler_c(r)
|
||||||
|
return [e(0), e(1), e(2)]
|
||||||
|
|
||||||
|
def rot_matrix(roll, pitch, yaw):
|
||||||
|
return matrix2numpy(rot_matrix_c(roll, pitch, yaw))
|
||||||
|
|
||||||
|
def ecef_euler_from_ned_single(ecef_init, ned_pose):
|
||||||
|
cdef ECEF init = list2ecef(ecef_init)
|
||||||
|
cdef Vector3 pose = Vector3(ned_pose[0], ned_pose[1], ned_pose[2])
|
||||||
|
|
||||||
|
cdef Vector3 e = ecef_euler_from_ned_c(init, pose)
|
||||||
|
return [e(0), e(1), e(2)]
|
||||||
|
|
||||||
|
def ned_euler_from_ecef_single(ecef_init, ecef_pose):
|
||||||
|
cdef ECEF init = list2ecef(ecef_init)
|
||||||
|
cdef Vector3 pose = Vector3(ecef_pose[0], ecef_pose[1], ecef_pose[2])
|
||||||
|
|
||||||
|
cdef Vector3 e = ned_euler_from_ecef_c(init, pose)
|
||||||
|
return [e(0), e(1), e(2)]
|
||||||
|
|
||||||
|
def geodetic2ecef_single(geodetic):
|
||||||
|
cdef Geodetic g = list2geodetic(geodetic)
|
||||||
|
cdef ECEF e = geodetic2ecef_c(g)
|
||||||
|
return [e.x, e.y, e.z]
|
||||||
|
|
||||||
|
def ecef2geodetic_single(ecef):
|
||||||
|
cdef ECEF e = list2ecef(ecef)
|
||||||
|
cdef Geodetic g = ecef2geodetic_c(e)
|
||||||
|
return [g.lat, g.lon, g.alt]
|
||||||
|
|
||||||
|
|
||||||
|
cdef class LocalCoord:
|
||||||
|
cdef LocalCoord_c * lc
|
||||||
|
|
||||||
|
def __init__(self, geodetic=None, ecef=None):
|
||||||
|
assert (geodetic is not None) or (ecef is not None)
|
||||||
|
if geodetic is not None:
|
||||||
|
self.lc = new LocalCoord_c(list2geodetic(geodetic))
|
||||||
|
elif ecef is not None:
|
||||||
|
self.lc = new LocalCoord_c(list2ecef(ecef))
|
||||||
|
|
||||||
|
@property
|
||||||
|
def ned2ecef_matrix(self):
|
||||||
|
return matrix2numpy(self.lc.ned2ecef_matrix)
|
||||||
|
|
||||||
|
@property
|
||||||
|
def ecef2ned_matrix(self):
|
||||||
|
return matrix2numpy(self.lc.ecef2ned_matrix)
|
||||||
|
|
||||||
|
@property
|
||||||
|
def ned_from_ecef_matrix(self):
|
||||||
|
return self.ecef2ned_matrix
|
||||||
|
|
||||||
|
@property
|
||||||
|
def ecef_from_ned_matrix(self):
|
||||||
|
return self.ned2ecef_matrix
|
||||||
|
|
||||||
|
@classmethod
|
||||||
|
def from_geodetic(cls, geodetic):
|
||||||
|
return cls(geodetic=geodetic)
|
||||||
|
|
||||||
|
@classmethod
|
||||||
|
def from_ecef(cls, ecef):
|
||||||
|
return cls(ecef=ecef)
|
||||||
|
|
||||||
|
def ecef2ned_single(self, ecef):
|
||||||
|
assert self.lc
|
||||||
|
cdef ECEF e = list2ecef(ecef)
|
||||||
|
cdef NED n = self.lc.ecef2ned(e)
|
||||||
|
return [n.n, n.e, n.d]
|
||||||
|
|
||||||
|
def ned2ecef_single(self, ned):
|
||||||
|
assert self.lc
|
||||||
|
cdef NED n = list2ned(ned)
|
||||||
|
cdef ECEF e = self.lc.ned2ecef(n)
|
||||||
|
return [e.x, e.y, e.z]
|
||||||
|
|
||||||
|
def geodetic2ned_single(self, geodetic):
|
||||||
|
assert self.lc
|
||||||
|
cdef Geodetic g = list2geodetic(geodetic)
|
||||||
|
cdef NED n = self.lc.geodetic2ned(g)
|
||||||
|
return [n.n, n.e, n.d]
|
||||||
|
|
||||||
|
def ned2geodetic_single(self, ned):
|
||||||
|
assert self.lc
|
||||||
|
cdef NED n = list2ned(ned)
|
||||||
|
cdef Geodetic g = self.lc.ned2geodetic(n)
|
||||||
|
return [g.lat, g.lon, g.alt]
|
||||||
|
|
||||||
|
def __dealloc__(self):
|
||||||
|
del self.lc
|
||||||
@@ -181,9 +181,9 @@ bool file_exists(const std::string& fn) {
|
|||||||
}
|
}
|
||||||
|
|
||||||
static bool createDirectory(std::string dir, mode_t mode) {
|
static bool createDirectory(std::string dir, mode_t mode) {
|
||||||
auto verify_dir = [](const std::string& path) -> bool {
|
auto verify_dir = [](const std::string& dir) -> bool {
|
||||||
struct stat st = {};
|
struct stat st = {};
|
||||||
return (stat(path.c_str(), &st) == 0 && (st.st_mode & S_IFMT) == S_IFDIR);
|
return (stat(dir.c_str(), &st) == 0 && (st.st_mode & S_IFMT) == S_IFDIR);
|
||||||
};
|
};
|
||||||
// remove trailing /'s
|
// remove trailing /'s
|
||||||
while (dir.size() > 1 && dir.back() == '/') {
|
while (dir.size() > 1 && dir.back() == '/') {
|
||||||
@@ -288,7 +288,7 @@ std::string strip(const std::string &str) {
|
|||||||
std::string check_output(const std::string& command) {
|
std::string check_output(const std::string& command) {
|
||||||
char buffer[128];
|
char buffer[128];
|
||||||
std::string result;
|
std::string result;
|
||||||
std::unique_ptr<FILE, int(*)(FILE*)> pipe(popen(command.c_str(), "r"), pclose);
|
std::unique_ptr<FILE, decltype(&pclose)> pipe(popen(command.c_str(), "r"), pclose);
|
||||||
|
|
||||||
if (!pipe) {
|
if (!pipe) {
|
||||||
return "";
|
return "";
|
||||||
@@ -303,7 +303,7 @@ std::string check_output(const std::string& command) {
|
|||||||
|
|
||||||
bool system_time_valid() {
|
bool system_time_valid() {
|
||||||
// Default to August 26, 2024
|
// Default to August 26, 2024
|
||||||
tm min_tm = {.tm_mday = 26, .tm_mon = 7, .tm_year = 2024 - 1900};
|
tm min_tm = {.tm_year = 2024 - 1900, .tm_mon = 7, .tm_mday = 26};
|
||||||
time_t min_date = mktime(&min_tm);
|
time_t min_date = mktime(&min_tm);
|
||||||
|
|
||||||
struct stat st;
|
struct stat st;
|
||||||
|
|||||||
@@ -96,13 +96,6 @@ bool create_directories(const std::string &dir, mode_t mode);
|
|||||||
|
|
||||||
std::string check_output(const std::string& command);
|
std::string check_output(const std::string& command);
|
||||||
|
|
||||||
inline void check_system(const std::string& cmd) {
|
|
||||||
int ret = std::system(cmd.c_str());
|
|
||||||
if (ret != 0) {
|
|
||||||
fprintf(stderr, "system command failed (%d): %s\n", ret, cmd.c_str());
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
bool system_time_valid();
|
bool system_time_valid();
|
||||||
|
|
||||||
inline void sleep_for(const int milliseconds) {
|
inline void sleep_for(const int milliseconds) {
|
||||||
|
|||||||
@@ -0,0 +1,46 @@
|
|||||||
|
import os
|
||||||
|
import subprocess
|
||||||
|
|
||||||
|
def sudo_write(val: str, path: str) -> None:
|
||||||
|
try:
|
||||||
|
with open(path, 'w') as f:
|
||||||
|
f.write(str(val))
|
||||||
|
except PermissionError:
|
||||||
|
os.system(f"sudo chmod a+w {path}")
|
||||||
|
try:
|
||||||
|
with open(path, 'w') as f:
|
||||||
|
f.write(str(val))
|
||||||
|
except PermissionError:
|
||||||
|
# fallback for debugfs files
|
||||||
|
os.system(f"sudo su -c 'echo {val} > {path}'")
|
||||||
|
|
||||||
|
def sudo_read(path: str) -> str:
|
||||||
|
try:
|
||||||
|
return subprocess.check_output(f"sudo cat {path}", shell=True, encoding='utf8').strip()
|
||||||
|
except Exception:
|
||||||
|
return ""
|
||||||
|
|
||||||
|
class MovingAverage:
|
||||||
|
def __init__(self, window_size: int):
|
||||||
|
self.window_size: int = window_size
|
||||||
|
self.buffer: list[float] = [0.0] * window_size
|
||||||
|
self.index: int = 0
|
||||||
|
self.count: int = 0
|
||||||
|
self.sum: float = 0.0
|
||||||
|
|
||||||
|
def add_value(self, new_value: float):
|
||||||
|
# Update the sum: subtract the value being replaced and add the new value
|
||||||
|
self.sum -= self.buffer[self.index]
|
||||||
|
self.buffer[self.index] = new_value
|
||||||
|
self.sum += new_value
|
||||||
|
|
||||||
|
# Update the index in a circular manner
|
||||||
|
self.index = (self.index + 1) % self.window_size
|
||||||
|
|
||||||
|
# Track the number of added values (for partial windows)
|
||||||
|
self.count = min(self.count + 1, self.window_size)
|
||||||
|
|
||||||
|
def get_average(self) -> float:
|
||||||
|
if self.count == 0:
|
||||||
|
return float('nan')
|
||||||
|
return self.sum / self.count
|
||||||
@@ -7,82 +7,14 @@ import time
|
|||||||
import functools
|
import functools
|
||||||
from subprocess import Popen, PIPE, TimeoutExpired
|
from subprocess import Popen, PIPE, TimeoutExpired
|
||||||
import zstandard as zstd
|
import zstandard as zstd
|
||||||
|
from openpilot.common.swaglog import cloudlog
|
||||||
|
|
||||||
LOG_COMPRESSION_LEVEL = 10 # little benefit up to level 15. level ~17 is a small step change
|
LOG_COMPRESSION_LEVEL = 10 # little benefit up to level 15. level ~17 is a small step change
|
||||||
|
|
||||||
class Timer:
|
|
||||||
"""Simple lap timer for profiling sequential operations."""
|
|
||||||
|
|
||||||
def __init__(self):
|
|
||||||
self._start = self._lap = time.monotonic()
|
|
||||||
self._sections = {}
|
|
||||||
|
|
||||||
def lap(self, name):
|
|
||||||
now = time.monotonic()
|
|
||||||
self._sections[name] = now - self._lap
|
|
||||||
self._lap = now
|
|
||||||
|
|
||||||
@property
|
|
||||||
def total(self):
|
|
||||||
return time.monotonic() - self._start
|
|
||||||
|
|
||||||
def fmt(self, duration):
|
|
||||||
parts = ", ".join(f"{k}={v:.2f}s" + (f" ({duration/v:.0f}x)" if k == 'render' and v > 0 else "") for k, v in self._sections.items())
|
|
||||||
total = self.total
|
|
||||||
realtime = f"{duration/total:.1f}x realtime" if total > 0 else "N/A"
|
|
||||||
return f"{duration}s in {total:.1f}s ({realtime}) | {parts}"
|
|
||||||
|
|
||||||
def sudo_write(val: str, path: str) -> None:
|
|
||||||
try:
|
|
||||||
with open(path, 'w') as f:
|
|
||||||
f.write(str(val))
|
|
||||||
except PermissionError:
|
|
||||||
os.system(f"sudo chmod a+w {path}")
|
|
||||||
try:
|
|
||||||
with open(path, 'w') as f:
|
|
||||||
f.write(str(val))
|
|
||||||
except PermissionError:
|
|
||||||
# fallback for debugfs files
|
|
||||||
os.system(f"sudo su -c 'echo {val} > {path}'")
|
|
||||||
|
|
||||||
|
|
||||||
def sudo_read(path: str) -> str:
|
|
||||||
try:
|
|
||||||
return subprocess.check_output(f"sudo cat {path}", shell=True, encoding='utf8').strip()
|
|
||||||
except Exception:
|
|
||||||
return ""
|
|
||||||
|
|
||||||
|
|
||||||
class MovingAverage:
|
|
||||||
def __init__(self, window_size: int):
|
|
||||||
self.window_size: int = window_size
|
|
||||||
self.buffer: list[float] = [0.0] * window_size
|
|
||||||
self.index: int = 0
|
|
||||||
self.count: int = 0
|
|
||||||
self.sum: float = 0.0
|
|
||||||
|
|
||||||
def add_value(self, new_value: float):
|
|
||||||
# Update the sum: subtract the value being replaced and add the new value
|
|
||||||
self.sum -= self.buffer[self.index]
|
|
||||||
self.buffer[self.index] = new_value
|
|
||||||
self.sum += new_value
|
|
||||||
|
|
||||||
# Update the index in a circular manner
|
|
||||||
self.index = (self.index + 1) % self.window_size
|
|
||||||
|
|
||||||
# Track the number of added values (for partial windows)
|
|
||||||
self.count = min(self.count + 1, self.window_size)
|
|
||||||
|
|
||||||
def get_average(self) -> float:
|
|
||||||
if self.count == 0:
|
|
||||||
return float('nan')
|
|
||||||
return self.sum / self.count
|
|
||||||
|
|
||||||
|
|
||||||
class CallbackReader:
|
class CallbackReader:
|
||||||
"""Wraps a file, but overrides the read method to also
|
"""Wraps a file, but overrides the read method to also
|
||||||
call a callback function with the number of bytes read so far."""
|
call a callback function with the number of bytes read so far."""
|
||||||
|
|
||||||
def __init__(self, f, callback, *args):
|
def __init__(self, f, callback, *args):
|
||||||
self.f = f
|
self.f = f
|
||||||
self.callback = callback
|
self.callback = callback
|
||||||
@@ -167,92 +99,6 @@ def managed_proc(cmd: list[str], env: dict[str, str]):
|
|||||||
proc.kill()
|
proc.kill()
|
||||||
|
|
||||||
|
|
||||||
def tabulate(tabular_data, headers=(), tablefmt="simple", floatfmt="g", stralign="left", numalign=None):
|
|
||||||
rows = [list(row) for row in tabular_data]
|
|
||||||
|
|
||||||
def fmt(val):
|
|
||||||
if isinstance(val, str):
|
|
||||||
return val
|
|
||||||
if isinstance(val, (bool, int)):
|
|
||||||
return str(val)
|
|
||||||
try:
|
|
||||||
return format(val, floatfmt)
|
|
||||||
except (TypeError, ValueError):
|
|
||||||
return str(val)
|
|
||||||
|
|
||||||
formatted = [[fmt(c) for c in row] for row in rows]
|
|
||||||
hdrs = [str(h) for h in headers] if headers else None
|
|
||||||
|
|
||||||
ncols = max((len(r) for r in formatted), default=0)
|
|
||||||
if hdrs:
|
|
||||||
ncols = max(ncols, len(hdrs))
|
|
||||||
if ncols == 0:
|
|
||||||
return ""
|
|
||||||
|
|
||||||
for r in formatted:
|
|
||||||
r.extend([""] * (ncols - len(r)))
|
|
||||||
if hdrs:
|
|
||||||
hdrs.extend([""] * (ncols - len(hdrs)))
|
|
||||||
|
|
||||||
widths = [0] * ncols
|
|
||||||
if hdrs:
|
|
||||||
for i in range(ncols):
|
|
||||||
widths[i] = len(hdrs[i])
|
|
||||||
for row in formatted:
|
|
||||||
for i in range(ncols):
|
|
||||||
widths[i] = max(widths[i], max(len(ln) for ln in row[i].split('\n')))
|
|
||||||
|
|
||||||
def _align(s, w):
|
|
||||||
if stralign == "center":
|
|
||||||
return s.center(w)
|
|
||||||
return s.ljust(w)
|
|
||||||
|
|
||||||
if tablefmt == "html":
|
|
||||||
parts = ["<table>"]
|
|
||||||
if hdrs:
|
|
||||||
parts.append("<thead>")
|
|
||||||
parts.append("<tr>" + "".join(f"<th>{h}</th>" for h in hdrs) + "</tr>")
|
|
||||||
parts.append("</thead>")
|
|
||||||
parts.append("<tbody>")
|
|
||||||
for row in formatted:
|
|
||||||
parts.append("<tr>" + "".join(f"<td>{c}</td>" for c in row) + "</tr>")
|
|
||||||
parts.append("</tbody>")
|
|
||||||
parts.append("</table>")
|
|
||||||
return "\n".join(parts)
|
|
||||||
|
|
||||||
if tablefmt == "simple_grid":
|
|
||||||
def _sep(left, mid, right):
|
|
||||||
return left + mid.join("\u2500" * (w + 2) for w in widths) + right
|
|
||||||
|
|
||||||
top, mid_sep, bot = _sep("\u250c", "\u252c", "\u2510"), _sep("\u251c", "\u253c", "\u2524"), _sep("\u2514", "\u2534", "\u2518")
|
|
||||||
|
|
||||||
def _fmt_row(cells):
|
|
||||||
split = [c.split('\n') for c in cells]
|
|
||||||
nlines = max(len(s) for s in split)
|
|
||||||
for s in split:
|
|
||||||
s.extend([""] * (nlines - len(s)))
|
|
||||||
return ["\u2502" + "\u2502".join(f" {_align(split[i][li], widths[i])} " for i in range(ncols)) + "\u2502" for li in range(nlines)]
|
|
||||||
|
|
||||||
lines = [top]
|
|
||||||
if hdrs:
|
|
||||||
lines.extend(_fmt_row(hdrs))
|
|
||||||
lines.append(mid_sep)
|
|
||||||
for ri, row in enumerate(formatted):
|
|
||||||
lines.extend(_fmt_row(row))
|
|
||||||
lines.append(mid_sep if ri < len(formatted) - 1 else bot)
|
|
||||||
return "\n".join(lines)
|
|
||||||
|
|
||||||
# simple
|
|
||||||
gap = " "
|
|
||||||
lines = []
|
|
||||||
if hdrs:
|
|
||||||
lines.append(gap.join(h.ljust(w) for h, w in zip(hdrs, widths, strict=True)))
|
|
||||||
lines.append(gap.join("-" * w for w in widths))
|
|
||||||
for row in formatted:
|
|
||||||
lines.append(gap.join(_align(row[i], widths[i]) for i in range(ncols)))
|
|
||||||
return "\n".join(lines)
|
|
||||||
|
|
||||||
|
|
||||||
def retry(attempts=3, delay=1.0, ignore_failure=False):
|
def retry(attempts=3, delay=1.0, ignore_failure=False):
|
||||||
def decorator(func):
|
def decorator(func):
|
||||||
@functools.wraps(func)
|
@functools.wraps(func)
|
||||||
@@ -261,11 +107,11 @@ def retry(attempts=3, delay=1.0, ignore_failure=False):
|
|||||||
try:
|
try:
|
||||||
return func(*args, **kwargs)
|
return func(*args, **kwargs)
|
||||||
except Exception:
|
except Exception:
|
||||||
print(f"{func.__name__} failed, trying again")
|
cloudlog.exception(f"{func.__name__} failed, trying again")
|
||||||
time.sleep(delay)
|
time.sleep(delay)
|
||||||
|
|
||||||
if ignore_failure:
|
if ignore_failure:
|
||||||
print(f"{func.__name__} failed after retry")
|
cloudlog.error(f"{func.__name__} failed after retry")
|
||||||
else:
|
else:
|
||||||
raise Exception(f"{func.__name__} failed after retry")
|
raise Exception(f"{func.__name__} failed after retry")
|
||||||
return wrapper
|
return wrapper
|
||||||
|
|||||||
@@ -1 +1 @@
|
|||||||
#define COMMA_VERSION "0.11.0"
|
#define COMMA_VERSION "0.10.3"
|
||||||
|
|||||||
@@ -10,6 +10,7 @@ from openpilot.system.hardware import TICI, HARDWARE
|
|||||||
# TODO: pytest-cpp doesn't support FAIL, and we need to create test translations in sessionstart
|
# TODO: pytest-cpp doesn't support FAIL, and we need to create test translations in sessionstart
|
||||||
# pending https://github.com/pytest-dev/pytest-cpp/pull/147
|
# pending https://github.com/pytest-dev/pytest-cpp/pull/147
|
||||||
collect_ignore = [
|
collect_ignore = [
|
||||||
|
"selfdrive/ui/tests/test_translations",
|
||||||
"selfdrive/test/process_replay/test_processes.py",
|
"selfdrive/test/process_replay/test_processes.py",
|
||||||
"selfdrive/test/process_replay/test_regen.py",
|
"selfdrive/test/process_replay/test_regen.py",
|
||||||
]
|
]
|
||||||
|
|||||||
@@ -4,23 +4,22 @@
|
|||||||
|
|
||||||
A supported vehicle is one that just works when you install a comma device. All supported cars provide a better experience than any stock system. Supported vehicles reference the US market unless otherwise specified.
|
A supported vehicle is one that just works when you install a comma device. All supported cars provide a better experience than any stock system. Supported vehicles reference the US market unless otherwise specified.
|
||||||
|
|
||||||
# 329 Supported Cars
|
# 325 Supported Cars
|
||||||
|
|
||||||
|Make|Model|Supported Package|ACC|No ACC accel below|No ALC below|Steering Torque|Resume from stop|<a href="##"><img width=2000></a>Hardware Needed<br> |Video|Setup Video|
|
|Make|Model|Supported Package|ACC|No ACC accel below|No ALC below|Steering Torque|Resume from stop|<a href="##"><img width=2000></a>Hardware Needed<br> |Video|Setup Video|
|
||||||
|---|---|---|:---:|:---:|:---:|:---:|:---:|:---:|:---:|:---:|
|
|---|---|---|:---:|:---:|:---:|:---:|:---:|:---:|:---:|:---:|
|
||||||
|Acura|ILX 2016-18|Technology Plus Package or AcuraWatch Plus|openpilot|26 mph|25 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Honda Nidec connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Acura ILX 2016-18">Buy Here</a></sub></details>|||
|
|Acura|ILX 2016-18|Technology Plus Package or AcuraWatch Plus|openpilot|26 mph|25 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Honda Nidec connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Acura ILX 2016-18">Buy Here</a></sub></details>|||
|
||||||
|Acura|ILX 2019|All|openpilot|26 mph|25 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Honda Nidec connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Acura ILX 2019">Buy Here</a></sub></details>|||
|
|Acura|ILX 2019|All|openpilot|26 mph|25 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Honda Nidec connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Acura ILX 2019">Buy Here</a></sub></details>|||
|
||||||
|Acura|MDX 2025-26|All except Type S|Stock|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Honda Bosch C connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Acura MDX 2025-26">Buy Here</a></sub></details>|||
|
|Acura|MDX 2025|All except Type S|Stock|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Honda Bosch C connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Acura MDX 2025">Buy Here</a></sub></details>|||
|
||||||
|Acura|RDX 2016-18|AcuraWatch Plus or Advance Package|openpilot|26 mph|12 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Honda Nidec connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Acura RDX 2016-18">Buy Here</a></sub></details>|||
|
|Acura|RDX 2016-18|AcuraWatch Plus or Advance Package|openpilot|26 mph|12 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Honda Nidec connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Acura RDX 2016-18">Buy Here</a></sub></details>|||
|
||||||
|Acura|RDX 2019-21|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|3 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Honda Bosch A connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Acura RDX 2019-21">Buy Here</a></sub></details>|||
|
|Acura|RDX 2019-21|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|3 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Honda Bosch A connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Acura RDX 2019-21">Buy Here</a></sub></details>|||
|
||||||
|Acura|TLX 2021|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Honda Bosch A connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Acura TLX 2021">Buy Here</a></sub></details>|||
|
|Acura|TLX 2021|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Honda Bosch A connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Acura TLX 2021">Buy Here</a></sub></details>|||
|
||||||
|Acura|TLX 2025|All|Stock|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Honda Bosch C connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Acura TLX 2025">Buy Here</a></sub></details>|||
|
|Audi|A3 2014-19|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,14</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 OBD-C cable (2 ft)<br>- 1 USB-C coupler<br>- 1 VW J533 connector<br>- 1 comma four<br>- 1 harness box<br>- 1 long OBD-C cable (9.5 ft)<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Audi A3 2014-19">Buy Here</a></sub></details>|||
|
||||||
|Audi|A3 2014-19|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,14</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 OBD-C cable (2 ft)<br>- 1 VW J533 connector<br>- 1 comma four<br>- 1 harness box<br>- 1 long OBD-C cable (9.5 ft)<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Audi A3 2014-19">Buy Here</a></sub></details>|||
|
|Audi|A3 Sportback e-tron 2017-18|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,14</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 OBD-C cable (2 ft)<br>- 1 USB-C coupler<br>- 1 VW J533 connector<br>- 1 comma four<br>- 1 harness box<br>- 1 long OBD-C cable (9.5 ft)<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Audi A3 Sportback e-tron 2017-18">Buy Here</a></sub></details>|||
|
||||||
|Audi|A3 Sportback e-tron 2017-18|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,14</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 OBD-C cable (2 ft)<br>- 1 VW J533 connector<br>- 1 comma four<br>- 1 harness box<br>- 1 long OBD-C cable (9.5 ft)<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Audi A3 Sportback e-tron 2017-18">Buy Here</a></sub></details>|||
|
|Audi|Q2 2018|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,14</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 OBD-C cable (2 ft)<br>- 1 USB-C coupler<br>- 1 VW J533 connector<br>- 1 comma four<br>- 1 harness box<br>- 1 long OBD-C cable (9.5 ft)<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Audi Q2 2018">Buy Here</a></sub></details>|||
|
||||||
|Audi|Q2 2018|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,14</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 OBD-C cable (2 ft)<br>- 1 VW J533 connector<br>- 1 comma four<br>- 1 harness box<br>- 1 long OBD-C cable (9.5 ft)<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Audi Q2 2018">Buy Here</a></sub></details>|||
|
|Audi|Q3 2019-24|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,14</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 OBD-C cable (2 ft)<br>- 1 USB-C coupler<br>- 1 VW J533 connector<br>- 1 comma four<br>- 1 harness box<br>- 1 long OBD-C cable (9.5 ft)<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Audi Q3 2019-24">Buy Here</a></sub></details>|||
|
||||||
|Audi|Q3 2019-24|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,14</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 OBD-C cable (2 ft)<br>- 1 VW J533 connector<br>- 1 comma four<br>- 1 harness box<br>- 1 long OBD-C cable (9.5 ft)<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Audi Q3 2019-24">Buy Here</a></sub></details>|||
|
|Audi|RS3 2018|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,14</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 OBD-C cable (2 ft)<br>- 1 USB-C coupler<br>- 1 VW J533 connector<br>- 1 comma four<br>- 1 harness box<br>- 1 long OBD-C cable (9.5 ft)<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Audi RS3 2018">Buy Here</a></sub></details>|||
|
||||||
|Audi|RS3 2018|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,14</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 OBD-C cable (2 ft)<br>- 1 VW J533 connector<br>- 1 comma four<br>- 1 harness box<br>- 1 long OBD-C cable (9.5 ft)<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Audi RS3 2018">Buy Here</a></sub></details>|||
|
|Audi|S3 2015-17|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,14</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 OBD-C cable (2 ft)<br>- 1 USB-C coupler<br>- 1 VW J533 connector<br>- 1 comma four<br>- 1 harness box<br>- 1 long OBD-C cable (9.5 ft)<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Audi S3 2015-17">Buy Here</a></sub></details>|||
|
||||||
|Audi|S3 2015-17|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,14</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 OBD-C cable (2 ft)<br>- 1 VW J533 connector<br>- 1 comma four<br>- 1 harness box<br>- 1 long OBD-C cable (9.5 ft)<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Audi S3 2015-17">Buy Here</a></sub></details>|||
|
|
||||||
|Chevrolet|Bolt EUV 2022-23|Premier or Premier Redline Trim, without Super Cruise Package|openpilot available[<sup>1</sup>](#footnotes)|3 mph|6 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 GM connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Chevrolet Bolt EUV 2022-23">Buy Here</a></sub></details>|<a href="https://youtu.be/xvwzGMUA210" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>||
|
|Chevrolet|Bolt EUV 2022-23|Premier or Premier Redline Trim, without Super Cruise Package|openpilot available[<sup>1</sup>](#footnotes)|3 mph|6 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 GM connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Chevrolet Bolt EUV 2022-23">Buy Here</a></sub></details>|<a href="https://youtu.be/xvwzGMUA210" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>||
|
||||||
|Chevrolet|Bolt EV 2022-23|2LT Trim with Adaptive Cruise Control Package|openpilot available[<sup>1</sup>](#footnotes)|3 mph|6 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 GM connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Chevrolet Bolt EV 2022-23">Buy Here</a></sub></details>|||
|
|Chevrolet|Bolt EV 2022-23|2LT Trim with Adaptive Cruise Control Package|openpilot available[<sup>1</sup>](#footnotes)|3 mph|6 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 GM connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Chevrolet Bolt EV 2022-23">Buy Here</a></sub></details>|||
|
||||||
|Chevrolet|Equinox 2019-22|Adaptive Cruise Control (ACC)|openpilot available[<sup>1</sup>](#footnotes)|3 mph|6 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 GM connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Chevrolet Equinox 2019-22">Buy Here</a></sub></details>|||
|
|Chevrolet|Equinox 2019-22|Adaptive Cruise Control (ACC)|openpilot available[<sup>1</sup>](#footnotes)|3 mph|6 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 GM connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Chevrolet Equinox 2019-22">Buy Here</a></sub></details>|||
|
||||||
@@ -32,33 +31,33 @@ A supported vehicle is one that just works when you install a comma device. All
|
|||||||
|Chrysler|Pacifica Hybrid 2017-18|Adaptive Cruise Control (ACC)|Stock|0 mph|9 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 FCA connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Chrysler Pacifica Hybrid 2017-18">Buy Here</a></sub></details>|||
|
|Chrysler|Pacifica Hybrid 2017-18|Adaptive Cruise Control (ACC)|Stock|0 mph|9 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 FCA connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Chrysler Pacifica Hybrid 2017-18">Buy Here</a></sub></details>|||
|
||||||
|Chrysler|Pacifica Hybrid 2019-25|Adaptive Cruise Control (ACC)|Stock|0 mph|39 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 FCA connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Chrysler Pacifica Hybrid 2019-25">Buy Here</a></sub></details>|||
|
|Chrysler|Pacifica Hybrid 2019-25|Adaptive Cruise Control (ACC)|Stock|0 mph|39 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 FCA connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Chrysler Pacifica Hybrid 2019-25">Buy Here</a></sub></details>|||
|
||||||
|comma|body|All|openpilot|0 mph|0 mph|[](##)|[](##)|None|<a href="https://youtu.be/VT-i3yRsX2s?t=2736" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>||
|
|comma|body|All|openpilot|0 mph|0 mph|[](##)|[](##)|None|<a href="https://youtu.be/VT-i3yRsX2s?t=2736" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>||
|
||||||
|CUPRA|Ateca 2018-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,14</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 OBD-C cable (2 ft)<br>- 1 VW J533 connector<br>- 1 comma four<br>- 1 harness box<br>- 1 long OBD-C cable (9.5 ft)<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=CUPRA Ateca 2018-23">Buy Here</a></sub></details>|||
|
|CUPRA|Ateca 2018-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,14</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 OBD-C cable (2 ft)<br>- 1 USB-C coupler<br>- 1 VW J533 connector<br>- 1 comma four<br>- 1 harness box<br>- 1 long OBD-C cable (9.5 ft)<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=CUPRA Ateca 2018-23">Buy Here</a></sub></details>|||
|
||||||
|Dodge|Durango 2020-21|Adaptive Cruise Control (ACC)|Stock|0 mph|39 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 FCA connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Dodge Durango 2020-21">Buy Here</a></sub></details>|||
|
|Dodge|Durango 2020-21|Adaptive Cruise Control (ACC)|Stock|0 mph|39 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 FCA connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Dodge Durango 2020-21">Buy Here</a></sub></details>|||
|
||||||
|Ford|Bronco Sport 2021-24|Co-Pilot360 Assist+|openpilot|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Ford Q3 connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Ford Bronco Sport 2021-24">Buy Here</a></sub></details>|||
|
|Ford|Bronco Sport 2021-24|Co-Pilot360 Assist+|openpilot|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Ford Q3 connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Ford Bronco Sport 2021-24">Buy Here</a></sub></details>|||
|
||||||
|Ford|Escape 2020-22|Co-Pilot360 Assist+|openpilot|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Ford Q3 connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Ford Escape 2020-22">Buy Here</a></sub></details>|||
|
|Ford|Escape 2020-22|Co-Pilot360 Assist+|openpilot|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Ford Q3 connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Ford Escape 2020-22">Buy Here</a></sub></details>|||
|
||||||
|Ford|Escape 2023-24|Co-Pilot360 Assist+|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Ford Q4 connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 long OBD-C cable (9.5 ft)<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Ford Escape 2023-24">Buy Here</a></sub></details>||<a href="https://www.youtube.com/watch?v=uUGkH6C_EQU" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
|
|Ford|Escape 2023-24|Co-Pilot360 Assist+|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Ford Q4 connector<br>- 1 OBD-C cable (2 ft)<br>- 1 USB-C coupler<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 long OBD-C cable (9.5 ft)<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Ford Escape 2023-24">Buy Here</a></sub></details>||<a href="https://www.youtube.com/watch?v=uUGkH6C_EQU" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
|
||||||
|Ford|Escape Hybrid 2020-22|Co-Pilot360 Assist+|openpilot|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Ford Q3 connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Ford Escape Hybrid 2020-22">Buy Here</a></sub></details>|||
|
|Ford|Escape Hybrid 2020-22|Co-Pilot360 Assist+|openpilot|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Ford Q3 connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Ford Escape Hybrid 2020-22">Buy Here</a></sub></details>|||
|
||||||
|Ford|Escape Hybrid 2023-24|Co-Pilot360 Assist+|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Ford Q4 connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 long OBD-C cable (9.5 ft)<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Ford Escape Hybrid 2023-24">Buy Here</a></sub></details>||<a href="https://www.youtube.com/watch?v=uUGkH6C_EQU" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
|
|Ford|Escape Hybrid 2023-24|Co-Pilot360 Assist+|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Ford Q4 connector<br>- 1 OBD-C cable (2 ft)<br>- 1 USB-C coupler<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 long OBD-C cable (9.5 ft)<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Ford Escape Hybrid 2023-24">Buy Here</a></sub></details>||<a href="https://www.youtube.com/watch?v=uUGkH6C_EQU" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
|
||||||
|Ford|Escape Plug-in Hybrid 2020-22|Co-Pilot360 Assist+|openpilot|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Ford Q3 connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Ford Escape Plug-in Hybrid 2020-22">Buy Here</a></sub></details>|||
|
|Ford|Escape Plug-in Hybrid 2020-22|Co-Pilot360 Assist+|openpilot|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Ford Q3 connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Ford Escape Plug-in Hybrid 2020-22">Buy Here</a></sub></details>|||
|
||||||
|Ford|Escape Plug-in Hybrid 2023-24|Co-Pilot360 Assist+|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Ford Q4 connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 long OBD-C cable (9.5 ft)<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Ford Escape Plug-in Hybrid 2023-24">Buy Here</a></sub></details>||<a href="https://www.youtube.com/watch?v=uUGkH6C_EQU" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
|
|Ford|Escape Plug-in Hybrid 2023-24|Co-Pilot360 Assist+|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Ford Q4 connector<br>- 1 OBD-C cable (2 ft)<br>- 1 USB-C coupler<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 long OBD-C cable (9.5 ft)<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Ford Escape Plug-in Hybrid 2023-24">Buy Here</a></sub></details>||<a href="https://www.youtube.com/watch?v=uUGkH6C_EQU" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
|
||||||
|Ford|Expedition 2022-24|Co-Pilot360 Assist 2.0|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Ford Q4 connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 long OBD-C cable (9.5 ft)<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Ford Expedition 2022-24">Buy Here</a></sub></details>||<a href="https://www.youtube.com/watch?v=MewJc9LYp9M" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
|
|Ford|Expedition 2022-24|Co-Pilot360 Assist 2.0|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Ford Q4 connector<br>- 1 OBD-C cable (2 ft)<br>- 1 USB-C coupler<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 long OBD-C cable (9.5 ft)<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Ford Expedition 2022-24">Buy Here</a></sub></details>||<a href="https://www.youtube.com/watch?v=MewJc9LYp9M" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
|
||||||
|Ford|Explorer 2020-24|Co-Pilot360 Assist+|openpilot|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Ford Q3 connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Ford Explorer 2020-24">Buy Here</a></sub></details>|||
|
|Ford|Explorer 2020-24|Co-Pilot360 Assist+|openpilot|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Ford Q3 connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Ford Explorer 2020-24">Buy Here</a></sub></details>|||
|
||||||
|Ford|Explorer Hybrid 2020-24|Co-Pilot360 Assist+|openpilot|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Ford Q3 connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Ford Explorer Hybrid 2020-24">Buy Here</a></sub></details>|||
|
|Ford|Explorer Hybrid 2020-24|Co-Pilot360 Assist+|openpilot|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Ford Q3 connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Ford Explorer Hybrid 2020-24">Buy Here</a></sub></details>|||
|
||||||
|Ford|F-150 2021-23|Co-Pilot360 Assist 2.0|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Ford Q4 connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 long OBD-C cable (9.5 ft)<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Ford F-150 2021-23">Buy Here</a></sub></details>||<a href="https://www.youtube.com/watch?v=MewJc9LYp9M" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
|
|Ford|F-150 2021-23|Co-Pilot360 Assist 2.0|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Ford Q4 connector<br>- 1 OBD-C cable (2 ft)<br>- 1 USB-C coupler<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 long OBD-C cable (9.5 ft)<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Ford F-150 2021-23">Buy Here</a></sub></details>||<a href="https://www.youtube.com/watch?v=MewJc9LYp9M" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
|
||||||
|Ford|F-150 Hybrid 2021-23|Co-Pilot360 Assist 2.0|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Ford Q4 connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 long OBD-C cable (9.5 ft)<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Ford F-150 Hybrid 2021-23">Buy Here</a></sub></details>||<a href="https://www.youtube.com/watch?v=MewJc9LYp9M" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
|
|Ford|F-150 Hybrid 2021-23|Co-Pilot360 Assist 2.0|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Ford Q4 connector<br>- 1 OBD-C cable (2 ft)<br>- 1 USB-C coupler<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 long OBD-C cable (9.5 ft)<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Ford F-150 Hybrid 2021-23">Buy Here</a></sub></details>||<a href="https://www.youtube.com/watch?v=MewJc9LYp9M" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
|
||||||
|Ford|Focus 2018[<sup>2</sup>](#footnotes)|Adaptive Cruise Control with Lane Centering|openpilot|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Ford Q3 connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Ford Focus 2018">Buy Here</a></sub></details>|||
|
|Ford|Focus 2018[<sup>2</sup>](#footnotes)|Adaptive Cruise Control with Lane Centering|openpilot|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Ford Q3 connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Ford Focus 2018">Buy Here</a></sub></details>|||
|
||||||
|Ford|Focus Hybrid 2018[<sup>2</sup>](#footnotes)|Adaptive Cruise Control with Lane Centering|openpilot|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Ford Q3 connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Ford Focus Hybrid 2018">Buy Here</a></sub></details>|||
|
|Ford|Focus Hybrid 2018[<sup>2</sup>](#footnotes)|Adaptive Cruise Control with Lane Centering|openpilot|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Ford Q3 connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Ford Focus Hybrid 2018">Buy Here</a></sub></details>|||
|
||||||
|Ford|Kuga 2020-23|Adaptive Cruise Control with Lane Centering|openpilot|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Ford Q3 connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Ford Kuga 2020-23">Buy Here</a></sub></details>|||
|
|Ford|Kuga 2020-23|Adaptive Cruise Control with Lane Centering|openpilot|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Ford Q3 connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Ford Kuga 2020-23">Buy Here</a></sub></details>|||
|
||||||
|Ford|Kuga Hybrid 2020-23|Adaptive Cruise Control with Lane Centering|openpilot|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Ford Q3 connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Ford Kuga Hybrid 2020-23">Buy Here</a></sub></details>|||
|
|Ford|Kuga Hybrid 2020-23|Adaptive Cruise Control with Lane Centering|openpilot|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Ford Q3 connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Ford Kuga Hybrid 2020-23">Buy Here</a></sub></details>|||
|
||||||
|Ford|Kuga Hybrid 2024|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Ford Q4 connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 long OBD-C cable (9.5 ft)<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Ford Kuga Hybrid 2024">Buy Here</a></sub></details>||<a href="https://www.youtube.com/watch?v=uUGkH6C_EQU" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
|
|Ford|Kuga Hybrid 2024|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Ford Q4 connector<br>- 1 OBD-C cable (2 ft)<br>- 1 USB-C coupler<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 long OBD-C cable (9.5 ft)<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Ford Kuga Hybrid 2024">Buy Here</a></sub></details>||<a href="https://www.youtube.com/watch?v=uUGkH6C_EQU" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
|
||||||
|Ford|Kuga Plug-in Hybrid 2020-23|Adaptive Cruise Control with Lane Centering|openpilot|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Ford Q3 connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Ford Kuga Plug-in Hybrid 2020-23">Buy Here</a></sub></details>|||
|
|Ford|Kuga Plug-in Hybrid 2020-23|Adaptive Cruise Control with Lane Centering|openpilot|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Ford Q3 connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Ford Kuga Plug-in Hybrid 2020-23">Buy Here</a></sub></details>|||
|
||||||
|Ford|Kuga Plug-in Hybrid 2024|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Ford Q4 connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 long OBD-C cable (9.5 ft)<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Ford Kuga Plug-in Hybrid 2024">Buy Here</a></sub></details>||<a href="https://www.youtube.com/watch?v=uUGkH6C_EQU" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
|
|Ford|Kuga Plug-in Hybrid 2024|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Ford Q4 connector<br>- 1 OBD-C cable (2 ft)<br>- 1 USB-C coupler<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 long OBD-C cable (9.5 ft)<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Ford Kuga Plug-in Hybrid 2024">Buy Here</a></sub></details>||<a href="https://www.youtube.com/watch?v=uUGkH6C_EQU" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
|
||||||
|Ford|Maverick 2022|LARIAT Luxury|openpilot|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Ford Q3 connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Ford Maverick 2022">Buy Here</a></sub></details>|||
|
|Ford|Maverick 2022|LARIAT Luxury|openpilot|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Ford Q3 connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Ford Maverick 2022">Buy Here</a></sub></details>|||
|
||||||
|Ford|Maverick 2023-24|Co-Pilot360 Assist|openpilot|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Ford Q3 connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Ford Maverick 2023-24">Buy Here</a></sub></details>|||
|
|Ford|Maverick 2023-24|Co-Pilot360 Assist|openpilot|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Ford Q3 connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Ford Maverick 2023-24">Buy Here</a></sub></details>|||
|
||||||
|Ford|Maverick Hybrid 2022|LARIAT Luxury|openpilot|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Ford Q3 connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Ford Maverick Hybrid 2022">Buy Here</a></sub></details>|||
|
|Ford|Maverick Hybrid 2022|LARIAT Luxury|openpilot|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Ford Q3 connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Ford Maverick Hybrid 2022">Buy Here</a></sub></details>|||
|
||||||
|Ford|Maverick Hybrid 2023-24|Co-Pilot360 Assist|openpilot|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Ford Q3 connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Ford Maverick Hybrid 2023-24">Buy Here</a></sub></details>|||
|
|Ford|Maverick Hybrid 2023-24|Co-Pilot360 Assist|openpilot|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Ford Q3 connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Ford Maverick Hybrid 2023-24">Buy Here</a></sub></details>|||
|
||||||
|Ford|Mustang Mach-E 2021-24|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Ford Q4 connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 long OBD-C cable (9.5 ft)<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Ford Mustang Mach-E 2021-24">Buy Here</a></sub></details>||<a href="https://www.youtube.com/watch?v=uUGkH6C_EQU" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
|
|Ford|Mustang Mach-E 2021-24|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Ford Q4 connector<br>- 1 OBD-C cable (2 ft)<br>- 1 USB-C coupler<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 long OBD-C cable (9.5 ft)<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Ford Mustang Mach-E 2021-24">Buy Here</a></sub></details>||<a href="https://www.youtube.com/watch?v=uUGkH6C_EQU" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
|
||||||
|Ford|Ranger 2024|Adaptive Cruise Control with Lane Centering|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Ford Q4 connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 long OBD-C cable (9.5 ft)<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Ford Ranger 2024">Buy Here</a></sub></details>||<a href="https://www.youtube.com/watch?v=uUGkH6C_EQU" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
|
|Ford|Ranger 2024|Adaptive Cruise Control with Lane Centering|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Ford Q4 connector<br>- 1 OBD-C cable (2 ft)<br>- 1 USB-C coupler<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 long OBD-C cable (9.5 ft)<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Ford Ranger 2024">Buy Here</a></sub></details>||<a href="https://www.youtube.com/watch?v=uUGkH6C_EQU" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
|
||||||
|Genesis|G70 2018|All|Stock|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Hyundai F connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Genesis G70 2018">Buy Here</a></sub></details>|||
|
|Genesis|G70 2018|All|Stock|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Hyundai F connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Genesis G70 2018">Buy Here</a></sub></details>|||
|
||||||
|Genesis|G70 2019-21|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Hyundai F connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Genesis G70 2019-21">Buy Here</a></sub></details>|||
|
|Genesis|G70 2019-21|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Hyundai F connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Genesis G70 2019-21">Buy Here</a></sub></details>|||
|
||||||
|Genesis|G70 2022-23|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Hyundai L connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Genesis G70 2022-23">Buy Here</a></sub></details>|||
|
|Genesis|G70 2022-23|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Hyundai L connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Genesis G70 2022-23">Buy Here</a></sub></details>|||
|
||||||
@@ -92,7 +91,7 @@ A supported vehicle is one that just works when you install a comma device. All
|
|||||||
|Honda|CR-V 2017-22|Honda Sensing|openpilot available[<sup>1</sup>](#footnotes)|0 mph|15 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Honda Bosch A connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Honda CR-V 2017-22">Buy Here</a></sub></details>|||
|
|Honda|CR-V 2017-22|Honda Sensing|openpilot available[<sup>1</sup>](#footnotes)|0 mph|15 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Honda Bosch A connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Honda CR-V 2017-22">Buy Here</a></sub></details>|||
|
||||||
|Honda|CR-V 2023-26|All|Stock|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Honda Bosch C connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Honda CR-V 2023-26">Buy Here</a></sub></details>|||
|
|Honda|CR-V 2023-26|All|Stock|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Honda Bosch C connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Honda CR-V 2023-26">Buy Here</a></sub></details>|||
|
||||||
|Honda|CR-V Hybrid 2017-22|Honda Sensing|openpilot available[<sup>1</sup>](#footnotes)|0 mph|12 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Honda Bosch A connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Honda CR-V Hybrid 2017-22">Buy Here</a></sub></details>|||
|
|Honda|CR-V Hybrid 2017-22|Honda Sensing|openpilot available[<sup>1</sup>](#footnotes)|0 mph|12 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Honda Bosch A connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Honda CR-V Hybrid 2017-22">Buy Here</a></sub></details>|||
|
||||||
|Honda|CR-V Hybrid 2023-26|All|Stock|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Honda Bosch C connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Honda CR-V Hybrid 2023-26">Buy Here</a></sub></details>|||
|
|Honda|CR-V Hybrid 2023-25|All|Stock|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Honda Bosch C connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Honda CR-V Hybrid 2023-25">Buy Here</a></sub></details>|||
|
||||||
|Honda|e 2020|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|3 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Honda Bosch A connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Honda e 2020">Buy Here</a></sub></details>|||
|
|Honda|e 2020|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|3 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Honda Bosch A connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Honda e 2020">Buy Here</a></sub></details>|||
|
||||||
|Honda|Fit 2018-20|Honda Sensing|openpilot|26 mph|12 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Honda Nidec connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Honda Fit 2018-20">Buy Here</a></sub></details>|||
|
|Honda|Fit 2018-20|Honda Sensing|openpilot|26 mph|12 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Honda Nidec connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Honda Fit 2018-20">Buy Here</a></sub></details>|||
|
||||||
|Honda|Freed 2020|Honda Sensing|openpilot|26 mph|12 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Honda Nidec connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Honda Freed 2020">Buy Here</a></sub></details>|||
|
|Honda|Freed 2020|Honda Sensing|openpilot|26 mph|12 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Honda Nidec connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Honda Freed 2020">Buy Here</a></sub></details>|||
|
||||||
@@ -103,7 +102,6 @@ A supported vehicle is one that just works when you install a comma device. All
|
|||||||
|Honda|N-Box 2018|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|11 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Honda Bosch A connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Honda N-Box 2018">Buy Here</a></sub></details>|||
|
|Honda|N-Box 2018|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|11 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Honda Bosch A connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Honda N-Box 2018">Buy Here</a></sub></details>|||
|
||||||
|Honda|Odyssey 2018-20|Honda Sensing|openpilot|26 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Honda Nidec connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Honda Odyssey 2018-20">Buy Here</a></sub></details>|||
|
|Honda|Odyssey 2018-20|Honda Sensing|openpilot|26 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Honda Nidec connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Honda Odyssey 2018-20">Buy Here</a></sub></details>|||
|
||||||
|Honda|Odyssey 2021-26|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|43 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Honda Bosch A connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Honda Odyssey 2021-26">Buy Here</a></sub></details>|||
|
|Honda|Odyssey 2021-26|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|43 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Honda Bosch A connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Honda Odyssey 2021-26">Buy Here</a></sub></details>|||
|
||||||
|Honda|Odyssey (Taiwan) 2018-19|Honda Sensing|openpilot|19 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Honda Nidec connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Honda Odyssey (Taiwan) 2018-19">Buy Here</a></sub></details>|||
|
|
||||||
|Honda|Passport 2019-25|All|openpilot|26 mph|12 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Honda Nidec connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Honda Passport 2019-25">Buy Here</a></sub></details>|||
|
|Honda|Passport 2019-25|All|openpilot|26 mph|12 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Honda Nidec connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Honda Passport 2019-25">Buy Here</a></sub></details>|||
|
||||||
|Honda|Passport 2026|All|Stock|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Honda Bosch C connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Honda Passport 2026">Buy Here</a></sub></details>|||
|
|Honda|Passport 2026|All|Stock|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Honda Bosch C connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Honda Passport 2026">Buy Here</a></sub></details>|||
|
||||||
|Honda|Pilot 2016-22|Honda Sensing|openpilot|26 mph|12 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Honda Nidec connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Honda Pilot 2016-22">Buy Here</a></sub></details>|||
|
|Honda|Pilot 2016-22|Honda Sensing|openpilot|26 mph|12 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Honda Nidec connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Honda Pilot 2016-22">Buy Here</a></sub></details>|||
|
||||||
@@ -165,7 +163,6 @@ A supported vehicle is one that just works when you install a comma device. All
|
|||||||
|Kia|Forte 2022-23|Smart Cruise Control (SCC)|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Hyundai E connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Kia Forte 2022-23">Buy Here</a></sub></details>|||
|
|Kia|Forte 2022-23|Smart Cruise Control (SCC)|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Hyundai E connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Kia Forte 2022-23">Buy Here</a></sub></details>|||
|
||||||
|Kia|K5 2021-24|Smart Cruise Control (SCC)|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Hyundai A connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Kia K5 2021-24">Buy Here</a></sub></details>|||
|
|Kia|K5 2021-24|Smart Cruise Control (SCC)|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Hyundai A connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Kia K5 2021-24">Buy Here</a></sub></details>|||
|
||||||
|Kia|K5 Hybrid 2020-22|Smart Cruise Control (SCC)|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Hyundai A connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Kia K5 Hybrid 2020-22">Buy Here</a></sub></details>|||
|
|Kia|K5 Hybrid 2020-22|Smart Cruise Control (SCC)|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Hyundai A connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Kia K5 Hybrid 2020-22">Buy Here</a></sub></details>|||
|
||||||
|Kia|K7 2017|Smart Cruise Control (SCC)|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Hyundai C connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Kia K7 2017">Buy Here</a></sub></details>|||
|
|
||||||
|Kia|K8 Hybrid (with HDA II) 2023|Highway Driving Assist II|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Hyundai Q connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Kia K8 Hybrid (with HDA II) 2023">Buy Here</a></sub></details>|||
|
|Kia|K8 Hybrid (with HDA II) 2023|Highway Driving Assist II|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Hyundai Q connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Kia K8 Hybrid (with HDA II) 2023">Buy Here</a></sub></details>|||
|
||||||
|Kia|Niro EV 2019|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Hyundai H connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Kia Niro EV 2019">Buy Here</a></sub></details>|<a href="https://www.youtube.com/watch?v=lT7zcG6ZpGo" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>||
|
|Kia|Niro EV 2019|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Hyundai H connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Kia Niro EV 2019">Buy Here</a></sub></details>|<a href="https://www.youtube.com/watch?v=lT7zcG6ZpGo" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>||
|
||||||
|Kia|Niro EV 2020|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Hyundai F connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Kia Niro EV 2020">Buy Here</a></sub></details>|<a href="https://www.youtube.com/watch?v=lT7zcG6ZpGo" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>||
|
|Kia|Niro EV 2020|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Hyundai F connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Kia Niro EV 2020">Buy Here</a></sub></details>|<a href="https://www.youtube.com/watch?v=lT7zcG6ZpGo" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>||
|
||||||
@@ -204,7 +201,6 @@ A supported vehicle is one that just works when you install a comma device. All
|
|||||||
|Lexus|IS 2017-19|All|Stock|19 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 OBD-C cable (2 ft)<br>- 1 Toyota A connector<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Lexus IS 2017-19">Buy Here</a></sub></details>|||
|
|Lexus|IS 2017-19|All|Stock|19 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 OBD-C cable (2 ft)<br>- 1 Toyota A connector<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Lexus IS 2017-19">Buy Here</a></sub></details>|||
|
||||||
|Lexus|IS 2022-24|All|openpilot|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 OBD-C cable (2 ft)<br>- 1 Toyota A connector<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Lexus IS 2022-24">Buy Here</a></sub></details>|||
|
|Lexus|IS 2022-24|All|openpilot|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 OBD-C cable (2 ft)<br>- 1 Toyota A connector<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Lexus IS 2022-24">Buy Here</a></sub></details>|||
|
||||||
|Lexus|LC 2024-25|All|openpilot|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 OBD-C cable (2 ft)<br>- 1 Toyota A connector<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Lexus LC 2024-25">Buy Here</a></sub></details>|||
|
|Lexus|LC 2024-25|All|openpilot|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 OBD-C cable (2 ft)<br>- 1 Toyota A connector<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Lexus LC 2024-25">Buy Here</a></sub></details>|||
|
||||||
|Lexus|LS 2018|All except Lexus Safety System+ A|Stock|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 OBD-C cable (2 ft)<br>- 1 Toyota A connector<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Lexus LS 2018">Buy Here</a></sub></details>|||
|
|
||||||
|Lexus|NX 2018-19|All|Stock|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 OBD-C cable (2 ft)<br>- 1 Toyota A connector<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Lexus NX 2018-19">Buy Here</a></sub></details>|||
|
|Lexus|NX 2018-19|All|Stock|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 OBD-C cable (2 ft)<br>- 1 Toyota A connector<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Lexus NX 2018-19">Buy Here</a></sub></details>|||
|
||||||
|Lexus|NX 2020-21|All|openpilot|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 OBD-C cable (2 ft)<br>- 1 Toyota A connector<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Lexus NX 2020-21">Buy Here</a></sub></details>|||
|
|Lexus|NX 2020-21|All|openpilot|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 OBD-C cable (2 ft)<br>- 1 Toyota A connector<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Lexus NX 2020-21">Buy Here</a></sub></details>|||
|
||||||
|Lexus|NX Hybrid 2018-19|All|Stock|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 OBD-C cable (2 ft)<br>- 1 Toyota A connector<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Lexus NX Hybrid 2018-19">Buy Here</a></sub></details>|||
|
|Lexus|NX Hybrid 2018-19|All|Stock|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 OBD-C cable (2 ft)<br>- 1 Toyota A connector<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Lexus NX Hybrid 2018-19">Buy Here</a></sub></details>|||
|
||||||
@@ -220,19 +216,19 @@ A supported vehicle is one that just works when you install a comma device. All
|
|||||||
|Lexus|UX Hybrid 2019-24|All|openpilot|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 OBD-C cable (2 ft)<br>- 1 Toyota A connector<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Lexus UX Hybrid 2019-24">Buy Here</a></sub></details>|||
|
|Lexus|UX Hybrid 2019-24|All|openpilot|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 OBD-C cable (2 ft)<br>- 1 Toyota A connector<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Lexus UX Hybrid 2019-24">Buy Here</a></sub></details>|||
|
||||||
|Lincoln|Aviator 2020-24|Co-Pilot360 Plus|openpilot|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Ford Q3 connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Lincoln Aviator 2020-24">Buy Here</a></sub></details>|||
|
|Lincoln|Aviator 2020-24|Co-Pilot360 Plus|openpilot|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Ford Q3 connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Lincoln Aviator 2020-24">Buy Here</a></sub></details>|||
|
||||||
|Lincoln|Aviator Plug-in Hybrid 2020-24|Co-Pilot360 Plus|openpilot|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Ford Q3 connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Lincoln Aviator Plug-in Hybrid 2020-24">Buy Here</a></sub></details>|||
|
|Lincoln|Aviator Plug-in Hybrid 2020-24|Co-Pilot360 Plus|openpilot|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Ford Q3 connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Lincoln Aviator Plug-in Hybrid 2020-24">Buy Here</a></sub></details>|||
|
||||||
|MAN|eTGE 2020-24|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,14</sup>](#footnotes)|0 mph|31 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 OBD-C cable (2 ft)<br>- 1 VW J533 connector<br>- 1 comma four<br>- 1 harness box<br>- 1 long OBD-C cable (9.5 ft)<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=MAN eTGE 2020-24">Buy Here</a></sub></details>|<a href="https://youtu.be/4100gLeabmo" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>||
|
|MAN|eTGE 2020-24|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,14</sup>](#footnotes)|0 mph|31 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 OBD-C cable (2 ft)<br>- 1 USB-C coupler<br>- 1 VW J533 connector<br>- 1 comma four<br>- 1 harness box<br>- 1 long OBD-C cable (9.5 ft)<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=MAN eTGE 2020-24">Buy Here</a></sub></details>|<a href="https://youtu.be/4100gLeabmo" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>||
|
||||||
|MAN|TGE 2017-24|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,14</sup>](#footnotes)|0 mph|31 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 OBD-C cable (2 ft)<br>- 1 VW J533 connector<br>- 1 comma four<br>- 1 harness box<br>- 1 long OBD-C cable (9.5 ft)<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=MAN TGE 2017-24">Buy Here</a></sub></details>|<a href="https://youtu.be/4100gLeabmo" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>||
|
|MAN|TGE 2017-24|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,14</sup>](#footnotes)|0 mph|31 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 OBD-C cable (2 ft)<br>- 1 USB-C coupler<br>- 1 VW J533 connector<br>- 1 comma four<br>- 1 harness box<br>- 1 long OBD-C cable (9.5 ft)<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=MAN TGE 2017-24">Buy Here</a></sub></details>|<a href="https://youtu.be/4100gLeabmo" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>||
|
||||||
|Mazda|CX-5 2022-25|All|Stock|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Mazda connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Mazda CX-5 2022-25">Buy Here</a></sub></details>|||
|
|Mazda|CX-5 2022-25|All|Stock|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Mazda connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Mazda CX-5 2022-25">Buy Here</a></sub></details>|||
|
||||||
|Mazda|CX-9 2021-23|All|Stock|0 mph|28 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Mazda connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Mazda CX-9 2021-23">Buy Here</a></sub></details>|<a href="https://youtu.be/dA3duO4a0O4" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>||
|
|Mazda|CX-9 2021-23|All|Stock|0 mph|28 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Mazda connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Mazda CX-9 2021-23">Buy Here</a></sub></details>|<a href="https://youtu.be/dA3duO4a0O4" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>||
|
||||||
|Nissan[<sup>5</sup>](#footnotes)|Altima 2019-20, 2024|ProPILOT Assist|Stock|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Nissan B connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 long OBD-C cable (9.5 ft)<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Nissan Altima 2019-20, 2024">Buy Here</a></sub></details>|||
|
|Nissan[<sup>5</sup>](#footnotes)|Altima 2019-20, 2024|ProPILOT Assist|Stock|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Nissan B connector<br>- 1 OBD-C cable (2 ft)<br>- 1 USB-C coupler<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 long OBD-C cable (9.5 ft)<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Nissan Altima 2019-20, 2024">Buy Here</a></sub></details>|||
|
||||||
|Nissan[<sup>5</sup>](#footnotes)|Leaf 2018-23|ProPILOT Assist|Stock|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Nissan A connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 long OBD-C cable (9.5 ft)<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Nissan Leaf 2018-23">Buy Here</a></sub></details>|<a href="https://youtu.be/vaMbtAh_0cY" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>||
|
|Nissan[<sup>5</sup>](#footnotes)|Leaf 2018-23|ProPILOT Assist|Stock|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Nissan A connector<br>- 1 OBD-C cable (2 ft)<br>- 1 USB-C coupler<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 long OBD-C cable (9.5 ft)<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Nissan Leaf 2018-23">Buy Here</a></sub></details>|<a href="https://youtu.be/vaMbtAh_0cY" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>||
|
||||||
|Nissan[<sup>5</sup>](#footnotes)|Rogue 2018-20|ProPILOT Assist|Stock|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Nissan A connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 long OBD-C cable (9.5 ft)<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Nissan Rogue 2018-20">Buy Here</a></sub></details>|||
|
|Nissan[<sup>5</sup>](#footnotes)|Rogue 2018-20|ProPILOT Assist|Stock|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Nissan A connector<br>- 1 OBD-C cable (2 ft)<br>- 1 USB-C coupler<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 long OBD-C cable (9.5 ft)<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Nissan Rogue 2018-20">Buy Here</a></sub></details>|||
|
||||||
|Nissan[<sup>5</sup>](#footnotes)|X-Trail 2017|ProPILOT Assist|Stock|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Nissan A connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 long OBD-C cable (9.5 ft)<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Nissan X-Trail 2017">Buy Here</a></sub></details>|||
|
|Nissan[<sup>5</sup>](#footnotes)|X-Trail 2017|ProPILOT Assist|Stock|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Nissan A connector<br>- 1 OBD-C cable (2 ft)<br>- 1 USB-C coupler<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 long OBD-C cable (9.5 ft)<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Nissan X-Trail 2017">Buy Here</a></sub></details>|||
|
||||||
|Ram|1500 2019-24|Adaptive Cruise Control (ACC)|Stock|0 mph|32 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 OBD-C cable (2 ft)<br>- 1 Ram connector<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Ram 1500 2019-24">Buy Here</a></sub></details>|||
|
|Ram|1500 2019-24|Adaptive Cruise Control (ACC)|Stock|0 mph|32 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 OBD-C cable (2 ft)<br>- 1 Ram connector<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Ram 1500 2019-24">Buy Here</a></sub></details>|||
|
||||||
|Rivian|R1S 2022-24|All|openpilot|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 OBD-C cable (2 ft)<br>- 1 Rivian A connector<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 long OBD-C cable (9.5 ft)<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Rivian R1S 2022-24">Buy Here</a></sub></details>||<a href="https://youtu.be/uaISd1j7Z4U" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
|
|Rivian|R1S 2022-24|All|openpilot|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 OBD-C cable (2 ft)<br>- 1 Rivian A connector<br>- 1 USB-C coupler<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 long OBD-C cable (9.5 ft)<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Rivian R1S 2022-24">Buy Here</a></sub></details>||<a href="https://youtu.be/uaISd1j7Z4U" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
|
||||||
|Rivian|R1T 2022-24|All|openpilot|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 OBD-C cable (2 ft)<br>- 1 Rivian A connector<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 long OBD-C cable (9.5 ft)<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Rivian R1T 2022-24">Buy Here</a></sub></details>||<a href="https://youtu.be/uaISd1j7Z4U" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
|
|Rivian|R1T 2022-24|All|openpilot|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 OBD-C cable (2 ft)<br>- 1 Rivian A connector<br>- 1 USB-C coupler<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 long OBD-C cable (9.5 ft)<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Rivian R1T 2022-24">Buy Here</a></sub></details>||<a href="https://youtu.be/uaISd1j7Z4U" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>|
|
||||||
|SEAT|Ateca 2016-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,14</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 OBD-C cable (2 ft)<br>- 1 VW J533 connector<br>- 1 comma four<br>- 1 harness box<br>- 1 long OBD-C cable (9.5 ft)<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=SEAT Ateca 2016-23">Buy Here</a></sub></details>|||
|
|SEAT|Ateca 2016-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,14</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 OBD-C cable (2 ft)<br>- 1 USB-C coupler<br>- 1 VW J533 connector<br>- 1 comma four<br>- 1 harness box<br>- 1 long OBD-C cable (9.5 ft)<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=SEAT Ateca 2016-23">Buy Here</a></sub></details>|||
|
||||||
|SEAT|Leon 2014-20|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,14</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 OBD-C cable (2 ft)<br>- 1 VW J533 connector<br>- 1 comma four<br>- 1 harness box<br>- 1 long OBD-C cable (9.5 ft)<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=SEAT Leon 2014-20">Buy Here</a></sub></details>|||
|
|SEAT|Leon 2014-20|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,14</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 OBD-C cable (2 ft)<br>- 1 USB-C coupler<br>- 1 VW J533 connector<br>- 1 comma four<br>- 1 harness box<br>- 1 long OBD-C cable (9.5 ft)<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=SEAT Leon 2014-20">Buy Here</a></sub></details>|||
|
||||||
|Subaru|Ascent 2019-21|All[<sup>6</sup>](#footnotes)|openpilot available[<sup>1,7</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 OBD-C cable (2 ft)<br>- 1 Subaru A connector<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Subaru Ascent 2019-21">Buy Here</a></sub></details><details><summary>Tools</summary><sub>- 1 Pry Tool<br>- 1 Socket Wrench 8mm or 5/16" (deep)</sub></details>|||
|
|Subaru|Ascent 2019-21|All[<sup>6</sup>](#footnotes)|openpilot available[<sup>1,7</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 OBD-C cable (2 ft)<br>- 1 Subaru A connector<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Subaru Ascent 2019-21">Buy Here</a></sub></details><details><summary>Tools</summary><sub>- 1 Pry Tool<br>- 1 Socket Wrench 8mm or 5/16" (deep)</sub></details>|||
|
||||||
|Subaru|Crosstrek 2018-19|EyeSight Driver Assistance[<sup>6</sup>](#footnotes)|openpilot available[<sup>1,7</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 OBD-C cable (2 ft)<br>- 1 Subaru A connector<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Subaru Crosstrek 2018-19">Buy Here</a></sub></details><details><summary>Tools</summary><sub>- 1 Pry Tool<br>- 1 Socket Wrench 8mm or 5/16" (deep)</sub></details>|<a href="https://youtu.be/Agww7oE1k-s?t=26" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>||
|
|Subaru|Crosstrek 2018-19|EyeSight Driver Assistance[<sup>6</sup>](#footnotes)|openpilot available[<sup>1,7</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 OBD-C cable (2 ft)<br>- 1 Subaru A connector<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Subaru Crosstrek 2018-19">Buy Here</a></sub></details><details><summary>Tools</summary><sub>- 1 Pry Tool<br>- 1 Socket Wrench 8mm or 5/16" (deep)</sub></details>|<a href="https://youtu.be/Agww7oE1k-s?t=26" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>||
|
||||||
|Subaru|Crosstrek 2020-23|EyeSight Driver Assistance[<sup>6</sup>](#footnotes)|openpilot available[<sup>1,7</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 OBD-C cable (2 ft)<br>- 1 Subaru A connector<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Subaru Crosstrek 2020-23">Buy Here</a></sub></details><details><summary>Tools</summary><sub>- 1 Pry Tool<br>- 1 Socket Wrench 8mm or 5/16" (deep)</sub></details>|||
|
|Subaru|Crosstrek 2020-23|EyeSight Driver Assistance[<sup>6</sup>](#footnotes)|openpilot available[<sup>1,7</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 OBD-C cable (2 ft)<br>- 1 Subaru A connector<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Subaru Crosstrek 2020-23">Buy Here</a></sub></details><details><summary>Tools</summary><sub>- 1 Pry Tool<br>- 1 Socket Wrench 8mm or 5/16" (deep)</sub></details>|||
|
||||||
@@ -243,19 +239,19 @@ A supported vehicle is one that just works when you install a comma device. All
|
|||||||
|Subaru|Outback 2020-22|All[<sup>6</sup>](#footnotes)|Stock|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 OBD-C cable (2 ft)<br>- 1 Subaru B connector<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Subaru Outback 2020-22">Buy Here</a></sub></details><details><summary>Tools</summary><sub>- 1 Pry Tool<br>- 1 Socket Wrench 8mm or 5/16" (deep)</sub></details>|||
|
|Subaru|Outback 2020-22|All[<sup>6</sup>](#footnotes)|Stock|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 OBD-C cable (2 ft)<br>- 1 Subaru B connector<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Subaru Outback 2020-22">Buy Here</a></sub></details><details><summary>Tools</summary><sub>- 1 Pry Tool<br>- 1 Socket Wrench 8mm or 5/16" (deep)</sub></details>|||
|
||||||
|Subaru|XV 2018-19|EyeSight Driver Assistance[<sup>6</sup>](#footnotes)|openpilot available[<sup>1,7</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 OBD-C cable (2 ft)<br>- 1 Subaru A connector<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Subaru XV 2018-19">Buy Here</a></sub></details><details><summary>Tools</summary><sub>- 1 Pry Tool<br>- 1 Socket Wrench 8mm or 5/16" (deep)</sub></details>|<a href="https://youtu.be/Agww7oE1k-s?t=26" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>||
|
|Subaru|XV 2018-19|EyeSight Driver Assistance[<sup>6</sup>](#footnotes)|openpilot available[<sup>1,7</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 OBD-C cable (2 ft)<br>- 1 Subaru A connector<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Subaru XV 2018-19">Buy Here</a></sub></details><details><summary>Tools</summary><sub>- 1 Pry Tool<br>- 1 Socket Wrench 8mm or 5/16" (deep)</sub></details>|<a href="https://youtu.be/Agww7oE1k-s?t=26" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>||
|
||||||
|Subaru|XV 2020-21|EyeSight Driver Assistance[<sup>6</sup>](#footnotes)|openpilot available[<sup>1,7</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 OBD-C cable (2 ft)<br>- 1 Subaru A connector<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Subaru XV 2020-21">Buy Here</a></sub></details><details><summary>Tools</summary><sub>- 1 Pry Tool<br>- 1 Socket Wrench 8mm or 5/16" (deep)</sub></details>|||
|
|Subaru|XV 2020-21|EyeSight Driver Assistance[<sup>6</sup>](#footnotes)|openpilot available[<sup>1,7</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 OBD-C cable (2 ft)<br>- 1 Subaru A connector<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Subaru XV 2020-21">Buy Here</a></sub></details><details><summary>Tools</summary><sub>- 1 Pry Tool<br>- 1 Socket Wrench 8mm or 5/16" (deep)</sub></details>|||
|
||||||
|Škoda|Fabia 2022-23[<sup>13</sup>](#footnotes)|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,14</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 OBD-C cable (2 ft)<br>- 1 VW J533 connector<br>- 1 comma four<br>- 1 harness box<br>- 1 long OBD-C cable (9.5 ft)<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Škoda Fabia 2022-23">Buy Here</a></sub></details>[<sup>15</sup>](#footnotes)|||
|
|Škoda|Fabia 2022-23[<sup>13</sup>](#footnotes)|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,14</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 OBD-C cable (2 ft)<br>- 1 USB-C coupler<br>- 1 VW J533 connector<br>- 1 comma four<br>- 1 harness box<br>- 1 long OBD-C cable (9.5 ft)<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Škoda Fabia 2022-23">Buy Here</a></sub></details>[<sup>15</sup>](#footnotes)|||
|
||||||
|Škoda|Kamiq 2021-23[<sup>11,13</sup>](#footnotes)|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,14</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 OBD-C cable (2 ft)<br>- 1 VW J533 connector<br>- 1 comma four<br>- 1 harness box<br>- 1 long OBD-C cable (9.5 ft)<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Škoda Kamiq 2021-23">Buy Here</a></sub></details>[<sup>15</sup>](#footnotes)|||
|
|Škoda|Kamiq 2021-23[<sup>11,13</sup>](#footnotes)|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,14</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 OBD-C cable (2 ft)<br>- 1 USB-C coupler<br>- 1 VW J533 connector<br>- 1 comma four<br>- 1 harness box<br>- 1 long OBD-C cable (9.5 ft)<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Škoda Kamiq 2021-23">Buy Here</a></sub></details>[<sup>15</sup>](#footnotes)|||
|
||||||
|Škoda|Karoq 2019-23[<sup>13</sup>](#footnotes)|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,14</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 OBD-C cable (2 ft)<br>- 1 VW J533 connector<br>- 1 comma four<br>- 1 harness box<br>- 1 long OBD-C cable (9.5 ft)<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Škoda Karoq 2019-23">Buy Here</a></sub></details>|||
|
|Škoda|Karoq 2019-23[<sup>13</sup>](#footnotes)|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,14</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 OBD-C cable (2 ft)<br>- 1 USB-C coupler<br>- 1 VW J533 connector<br>- 1 comma four<br>- 1 harness box<br>- 1 long OBD-C cable (9.5 ft)<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Škoda Karoq 2019-23">Buy Here</a></sub></details>|||
|
||||||
|Škoda|Kodiaq 2017-23[<sup>13</sup>](#footnotes)|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,14</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 OBD-C cable (2 ft)<br>- 1 VW J533 connector<br>- 1 comma four<br>- 1 harness box<br>- 1 long OBD-C cable (9.5 ft)<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Škoda Kodiaq 2017-23">Buy Here</a></sub></details>|||
|
|Škoda|Kodiaq 2017-23[<sup>13</sup>](#footnotes)|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,14</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 OBD-C cable (2 ft)<br>- 1 USB-C coupler<br>- 1 VW J533 connector<br>- 1 comma four<br>- 1 harness box<br>- 1 long OBD-C cable (9.5 ft)<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Škoda Kodiaq 2017-23">Buy Here</a></sub></details>|||
|
||||||
|Škoda|Octavia 2015-19[<sup>13</sup>](#footnotes)|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,14</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 OBD-C cable (2 ft)<br>- 1 VW J533 connector<br>- 1 comma four<br>- 1 harness box<br>- 1 long OBD-C cable (9.5 ft)<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Škoda Octavia 2015-19">Buy Here</a></sub></details>|||
|
|Škoda|Octavia 2015-19[<sup>13</sup>](#footnotes)|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,14</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 OBD-C cable (2 ft)<br>- 1 USB-C coupler<br>- 1 VW J533 connector<br>- 1 comma four<br>- 1 harness box<br>- 1 long OBD-C cable (9.5 ft)<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Škoda Octavia 2015-19">Buy Here</a></sub></details>|||
|
||||||
|Škoda|Octavia RS 2016[<sup>13</sup>](#footnotes)|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,14</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 OBD-C cable (2 ft)<br>- 1 VW J533 connector<br>- 1 comma four<br>- 1 harness box<br>- 1 long OBD-C cable (9.5 ft)<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Škoda Octavia RS 2016">Buy Here</a></sub></details>|||
|
|Škoda|Octavia RS 2016[<sup>13</sup>](#footnotes)|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,14</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 OBD-C cable (2 ft)<br>- 1 USB-C coupler<br>- 1 VW J533 connector<br>- 1 comma four<br>- 1 harness box<br>- 1 long OBD-C cable (9.5 ft)<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Škoda Octavia RS 2016">Buy Here</a></sub></details>|||
|
||||||
|Škoda|Octavia Scout 2017-19[<sup>13</sup>](#footnotes)|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,14</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 OBD-C cable (2 ft)<br>- 1 VW J533 connector<br>- 1 comma four<br>- 1 harness box<br>- 1 long OBD-C cable (9.5 ft)<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Škoda Octavia Scout 2017-19">Buy Here</a></sub></details>|||
|
|Škoda|Octavia Scout 2017-19[<sup>13</sup>](#footnotes)|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,14</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 OBD-C cable (2 ft)<br>- 1 USB-C coupler<br>- 1 VW J533 connector<br>- 1 comma four<br>- 1 harness box<br>- 1 long OBD-C cable (9.5 ft)<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Škoda Octavia Scout 2017-19">Buy Here</a></sub></details>|||
|
||||||
|Škoda|Scala 2020-23[<sup>13</sup>](#footnotes)|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,14</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 OBD-C cable (2 ft)<br>- 1 VW J533 connector<br>- 1 comma four<br>- 1 harness box<br>- 1 long OBD-C cable (9.5 ft)<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Škoda Scala 2020-23">Buy Here</a></sub></details>[<sup>15</sup>](#footnotes)|||
|
|Škoda|Scala 2020-23[<sup>13</sup>](#footnotes)|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,14</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 OBD-C cable (2 ft)<br>- 1 USB-C coupler<br>- 1 VW J533 connector<br>- 1 comma four<br>- 1 harness box<br>- 1 long OBD-C cable (9.5 ft)<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Škoda Scala 2020-23">Buy Here</a></sub></details>[<sup>15</sup>](#footnotes)|||
|
||||||
|Škoda|Superb 2015-22[<sup>13</sup>](#footnotes)|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,14</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 OBD-C cable (2 ft)<br>- 1 VW J533 connector<br>- 1 comma four<br>- 1 harness box<br>- 1 long OBD-C cable (9.5 ft)<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Škoda Superb 2015-22">Buy Here</a></sub></details>|||
|
|Škoda|Superb 2015-22[<sup>13</sup>](#footnotes)|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,14</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 OBD-C cable (2 ft)<br>- 1 USB-C coupler<br>- 1 VW J533 connector<br>- 1 comma four<br>- 1 harness box<br>- 1 long OBD-C cable (9.5 ft)<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Škoda Superb 2015-22">Buy Here</a></sub></details>|||
|
||||||
|Tesla[<sup>9</sup>](#footnotes)|Model 3 (with HW3) 2019-23[<sup>8</sup>](#footnotes)|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 OBD-C cable (2 ft)<br>- 1 Tesla A connector<br>- 1 comma four<br>- 1 harness box<br>- 1 long OBD-C cable (9.5 ft)<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Tesla Model 3 (with HW3) 2019-23">Buy Here</a></sub></details>|||
|
|Tesla[<sup>9</sup>](#footnotes)|Model 3 (with HW3) 2019-23[<sup>8</sup>](#footnotes)|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 OBD-C cable (2 ft)<br>- 1 Tesla A connector<br>- 1 USB-C coupler<br>- 1 comma four<br>- 1 harness box<br>- 1 long OBD-C cable (9.5 ft)<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Tesla Model 3 (with HW3) 2019-23">Buy Here</a></sub></details>|||
|
||||||
|Tesla[<sup>9</sup>](#footnotes)|Model 3 (with HW4) 2024-25[<sup>8</sup>](#footnotes)|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 OBD-C cable (2 ft)<br>- 1 Tesla B connector<br>- 1 comma four<br>- 1 harness box<br>- 1 long OBD-C cable (9.5 ft)<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Tesla Model 3 (with HW4) 2024-25">Buy Here</a></sub></details>|||
|
|Tesla[<sup>9</sup>](#footnotes)|Model 3 (with HW4) 2024-25[<sup>8</sup>](#footnotes)|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 OBD-C cable (2 ft)<br>- 1 Tesla B connector<br>- 1 USB-C coupler<br>- 1 comma four<br>- 1 harness box<br>- 1 long OBD-C cable (9.5 ft)<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Tesla Model 3 (with HW4) 2024-25">Buy Here</a></sub></details>|||
|
||||||
|Tesla[<sup>9</sup>](#footnotes)|Model Y (with HW3) 2020-23[<sup>8</sup>](#footnotes)|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 OBD-C cable (2 ft)<br>- 1 Tesla A connector<br>- 1 comma four<br>- 1 harness box<br>- 1 long OBD-C cable (9.5 ft)<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Tesla Model Y (with HW3) 2020-23">Buy Here</a></sub></details>|||
|
|Tesla[<sup>9</sup>](#footnotes)|Model Y (with HW3) 2020-23[<sup>8</sup>](#footnotes)|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 OBD-C cable (2 ft)<br>- 1 Tesla A connector<br>- 1 USB-C coupler<br>- 1 comma four<br>- 1 harness box<br>- 1 long OBD-C cable (9.5 ft)<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Tesla Model Y (with HW3) 2020-23">Buy Here</a></sub></details>|||
|
||||||
|Tesla[<sup>9</sup>](#footnotes)|Model Y (with HW4) 2024-25[<sup>8</sup>](#footnotes)|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 OBD-C cable (2 ft)<br>- 1 Tesla B connector<br>- 1 comma four<br>- 1 harness box<br>- 1 long OBD-C cable (9.5 ft)<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Tesla Model Y (with HW4) 2024-25">Buy Here</a></sub></details>|||
|
|Tesla[<sup>9</sup>](#footnotes)|Model Y (with HW4) 2024-25[<sup>8</sup>](#footnotes)|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 OBD-C cable (2 ft)<br>- 1 Tesla B connector<br>- 1 USB-C coupler<br>- 1 comma four<br>- 1 harness box<br>- 1 long OBD-C cable (9.5 ft)<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Tesla Model Y (with HW4) 2024-25">Buy Here</a></sub></details>|||
|
||||||
|Toyota|Alphard 2019-20|All|openpilot|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 OBD-C cable (2 ft)<br>- 1 Toyota A connector<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Toyota Alphard 2019-20">Buy Here</a></sub></details>|||
|
|Toyota|Alphard 2019-20|All|openpilot|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 OBD-C cable (2 ft)<br>- 1 Toyota A connector<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Toyota Alphard 2019-20">Buy Here</a></sub></details>|||
|
||||||
|Toyota|Alphard Hybrid 2021|All|openpilot|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 OBD-C cable (2 ft)<br>- 1 Toyota A connector<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Toyota Alphard Hybrid 2021">Buy Here</a></sub></details>|||
|
|Toyota|Alphard Hybrid 2021|All|openpilot|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 OBD-C cable (2 ft)<br>- 1 Toyota A connector<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Toyota Alphard Hybrid 2021">Buy Here</a></sub></details>|||
|
||||||
|Toyota|Avalon 2016|Toyota Safety Sense P|Stock|19 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 OBD-C cable (2 ft)<br>- 1 Toyota A connector<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Toyota Avalon 2016">Buy Here</a></sub></details>|||
|
|Toyota|Avalon 2016|Toyota Safety Sense P|Stock|19 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 OBD-C cable (2 ft)<br>- 1 Toyota A connector<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Toyota Avalon 2016">Buy Here</a></sub></details>|||
|
||||||
@@ -301,42 +297,42 @@ A supported vehicle is one that just works when you install a comma device. All
|
|||||||
|Toyota|RAV4 Hybrid 2022|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 OBD-C cable (2 ft)<br>- 1 Toyota A connector<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Toyota RAV4 Hybrid 2022">Buy Here</a></sub></details>|<a href="https://youtu.be/U0nH9cnrFB0" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>||
|
|Toyota|RAV4 Hybrid 2022|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 OBD-C cable (2 ft)<br>- 1 Toyota A connector<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Toyota RAV4 Hybrid 2022">Buy Here</a></sub></details>|<a href="https://youtu.be/U0nH9cnrFB0" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>||
|
||||||
|Toyota|RAV4 Hybrid 2023-25|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 OBD-C cable (2 ft)<br>- 1 Toyota A connector<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Toyota RAV4 Hybrid 2023-25">Buy Here</a></sub></details>|<a href="https://youtu.be/4eIsEq4L4Ng" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>||
|
|Toyota|RAV4 Hybrid 2023-25|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 OBD-C cable (2 ft)<br>- 1 Toyota A connector<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Toyota RAV4 Hybrid 2023-25">Buy Here</a></sub></details>|<a href="https://youtu.be/4eIsEq4L4Ng" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>||
|
||||||
|Toyota|Sienna 2018-20|All|Stock|19 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 OBD-C cable (2 ft)<br>- 1 Toyota A connector<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Toyota Sienna 2018-20">Buy Here</a></sub></details>|<a href="https://www.youtube.com/watch?v=q1UPOo4Sh68" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>||
|
|Toyota|Sienna 2018-20|All|Stock|19 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 OBD-C cable (2 ft)<br>- 1 Toyota A connector<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Toyota Sienna 2018-20">Buy Here</a></sub></details>|<a href="https://www.youtube.com/watch?v=q1UPOo4Sh68" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>||
|
||||||
|Volkswagen|Arteon 2018-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,14</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 OBD-C cable (2 ft)<br>- 1 VW J533 connector<br>- 1 comma four<br>- 1 harness box<br>- 1 long OBD-C cable (9.5 ft)<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Volkswagen Arteon 2018-23">Buy Here</a></sub></details>|<a href="https://youtu.be/FAomFKPFlDA" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>||
|
|Volkswagen|Arteon 2018-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,14</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 OBD-C cable (2 ft)<br>- 1 USB-C coupler<br>- 1 VW J533 connector<br>- 1 comma four<br>- 1 harness box<br>- 1 long OBD-C cable (9.5 ft)<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Volkswagen Arteon 2018-23">Buy Here</a></sub></details>|<a href="https://youtu.be/FAomFKPFlDA" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>||
|
||||||
|Volkswagen|Arteon eHybrid 2020-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,14</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 OBD-C cable (2 ft)<br>- 1 VW J533 connector<br>- 1 comma four<br>- 1 harness box<br>- 1 long OBD-C cable (9.5 ft)<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Volkswagen Arteon eHybrid 2020-23">Buy Here</a></sub></details>|<a href="https://youtu.be/FAomFKPFlDA" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>||
|
|Volkswagen|Arteon eHybrid 2020-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,14</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 OBD-C cable (2 ft)<br>- 1 USB-C coupler<br>- 1 VW J533 connector<br>- 1 comma four<br>- 1 harness box<br>- 1 long OBD-C cable (9.5 ft)<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Volkswagen Arteon eHybrid 2020-23">Buy Here</a></sub></details>|<a href="https://youtu.be/FAomFKPFlDA" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>||
|
||||||
|Volkswagen|Arteon R 2020-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,14</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 OBD-C cable (2 ft)<br>- 1 VW J533 connector<br>- 1 comma four<br>- 1 harness box<br>- 1 long OBD-C cable (9.5 ft)<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Volkswagen Arteon R 2020-23">Buy Here</a></sub></details>|<a href="https://youtu.be/FAomFKPFlDA" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>||
|
|Volkswagen|Arteon R 2020-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,14</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 OBD-C cable (2 ft)<br>- 1 USB-C coupler<br>- 1 VW J533 connector<br>- 1 comma four<br>- 1 harness box<br>- 1 long OBD-C cable (9.5 ft)<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Volkswagen Arteon R 2020-23">Buy Here</a></sub></details>|<a href="https://youtu.be/FAomFKPFlDA" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>||
|
||||||
|Volkswagen|Arteon Shooting Brake 2020-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,14</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 OBD-C cable (2 ft)<br>- 1 VW J533 connector<br>- 1 comma four<br>- 1 harness box<br>- 1 long OBD-C cable (9.5 ft)<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Volkswagen Arteon Shooting Brake 2020-23">Buy Here</a></sub></details>|<a href="https://youtu.be/FAomFKPFlDA" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>||
|
|Volkswagen|Arteon Shooting Brake 2020-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,14</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 OBD-C cable (2 ft)<br>- 1 USB-C coupler<br>- 1 VW J533 connector<br>- 1 comma four<br>- 1 harness box<br>- 1 long OBD-C cable (9.5 ft)<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Volkswagen Arteon Shooting Brake 2020-23">Buy Here</a></sub></details>|<a href="https://youtu.be/FAomFKPFlDA" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>||
|
||||||
|Volkswagen|Atlas 2018-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,14</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 OBD-C cable (2 ft)<br>- 1 VW J533 connector<br>- 1 comma four<br>- 1 harness box<br>- 1 long OBD-C cable (9.5 ft)<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Volkswagen Atlas 2018-23">Buy Here</a></sub></details>|||
|
|Volkswagen|Atlas 2018-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,14</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 OBD-C cable (2 ft)<br>- 1 USB-C coupler<br>- 1 VW J533 connector<br>- 1 comma four<br>- 1 harness box<br>- 1 long OBD-C cable (9.5 ft)<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Volkswagen Atlas 2018-23">Buy Here</a></sub></details>|||
|
||||||
|Volkswagen|Atlas Cross Sport 2020-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,14</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 OBD-C cable (2 ft)<br>- 1 VW J533 connector<br>- 1 comma four<br>- 1 harness box<br>- 1 long OBD-C cable (9.5 ft)<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Volkswagen Atlas Cross Sport 2020-22">Buy Here</a></sub></details>|||
|
|Volkswagen|Atlas Cross Sport 2020-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,14</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 OBD-C cable (2 ft)<br>- 1 USB-C coupler<br>- 1 VW J533 connector<br>- 1 comma four<br>- 1 harness box<br>- 1 long OBD-C cable (9.5 ft)<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Volkswagen Atlas Cross Sport 2020-22">Buy Here</a></sub></details>|||
|
||||||
|Volkswagen|California 2021-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,14</sup>](#footnotes)|0 mph|31 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 OBD-C cable (2 ft)<br>- 1 VW J533 connector<br>- 1 comma four<br>- 1 harness box<br>- 1 long OBD-C cable (9.5 ft)<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Volkswagen California 2021-23">Buy Here</a></sub></details>|||
|
|Volkswagen|California 2021-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,14</sup>](#footnotes)|0 mph|31 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 OBD-C cable (2 ft)<br>- 1 USB-C coupler<br>- 1 VW J533 connector<br>- 1 comma four<br>- 1 harness box<br>- 1 long OBD-C cable (9.5 ft)<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Volkswagen California 2021-23">Buy Here</a></sub></details>|||
|
||||||
|Volkswagen|Caravelle 2020|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,14</sup>](#footnotes)|0 mph|31 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 OBD-C cable (2 ft)<br>- 1 VW J533 connector<br>- 1 comma four<br>- 1 harness box<br>- 1 long OBD-C cable (9.5 ft)<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Volkswagen Caravelle 2020">Buy Here</a></sub></details>|||
|
|Volkswagen|Caravelle 2020|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,14</sup>](#footnotes)|0 mph|31 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 OBD-C cable (2 ft)<br>- 1 USB-C coupler<br>- 1 VW J533 connector<br>- 1 comma four<br>- 1 harness box<br>- 1 long OBD-C cable (9.5 ft)<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Volkswagen Caravelle 2020">Buy Here</a></sub></details>|||
|
||||||
|Volkswagen|CC 2018-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,14</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 OBD-C cable (2 ft)<br>- 1 VW J533 connector<br>- 1 comma four<br>- 1 harness box<br>- 1 long OBD-C cable (9.5 ft)<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Volkswagen CC 2018-22">Buy Here</a></sub></details>|<a href="https://youtu.be/FAomFKPFlDA" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>||
|
|Volkswagen|CC 2018-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,14</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 OBD-C cable (2 ft)<br>- 1 USB-C coupler<br>- 1 VW J533 connector<br>- 1 comma four<br>- 1 harness box<br>- 1 long OBD-C cable (9.5 ft)<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Volkswagen CC 2018-22">Buy Here</a></sub></details>|<a href="https://youtu.be/FAomFKPFlDA" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>||
|
||||||
|Volkswagen|Crafter 2017-24|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,14</sup>](#footnotes)|0 mph|31 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 OBD-C cable (2 ft)<br>- 1 VW J533 connector<br>- 1 comma four<br>- 1 harness box<br>- 1 long OBD-C cable (9.5 ft)<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Volkswagen Crafter 2017-24">Buy Here</a></sub></details>|<a href="https://youtu.be/4100gLeabmo" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>||
|
|Volkswagen|Crafter 2017-24|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,14</sup>](#footnotes)|0 mph|31 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 OBD-C cable (2 ft)<br>- 1 USB-C coupler<br>- 1 VW J533 connector<br>- 1 comma four<br>- 1 harness box<br>- 1 long OBD-C cable (9.5 ft)<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Volkswagen Crafter 2017-24">Buy Here</a></sub></details>|<a href="https://youtu.be/4100gLeabmo" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>||
|
||||||
|Volkswagen|e-Crafter 2018-24|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,14</sup>](#footnotes)|0 mph|31 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 OBD-C cable (2 ft)<br>- 1 VW J533 connector<br>- 1 comma four<br>- 1 harness box<br>- 1 long OBD-C cable (9.5 ft)<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Volkswagen e-Crafter 2018-24">Buy Here</a></sub></details>|<a href="https://youtu.be/4100gLeabmo" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>||
|
|Volkswagen|e-Crafter 2018-24|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,14</sup>](#footnotes)|0 mph|31 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 OBD-C cable (2 ft)<br>- 1 USB-C coupler<br>- 1 VW J533 connector<br>- 1 comma four<br>- 1 harness box<br>- 1 long OBD-C cable (9.5 ft)<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Volkswagen e-Crafter 2018-24">Buy Here</a></sub></details>|<a href="https://youtu.be/4100gLeabmo" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>||
|
||||||
|Volkswagen|e-Golf 2014-20|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,14</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 OBD-C cable (2 ft)<br>- 1 VW J533 connector<br>- 1 comma four<br>- 1 harness box<br>- 1 long OBD-C cable (9.5 ft)<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Volkswagen e-Golf 2014-20">Buy Here</a></sub></details>|||
|
|Volkswagen|e-Golf 2014-20|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,14</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 OBD-C cable (2 ft)<br>- 1 USB-C coupler<br>- 1 VW J533 connector<br>- 1 comma four<br>- 1 harness box<br>- 1 long OBD-C cable (9.5 ft)<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Volkswagen e-Golf 2014-20">Buy Here</a></sub></details>|||
|
||||||
|Volkswagen|Golf 2015-20|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,14</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 OBD-C cable (2 ft)<br>- 1 VW J533 connector<br>- 1 comma four<br>- 1 harness box<br>- 1 long OBD-C cable (9.5 ft)<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Volkswagen Golf 2015-20">Buy Here</a></sub></details>|||
|
|Volkswagen|Golf 2015-20|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,14</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 OBD-C cable (2 ft)<br>- 1 USB-C coupler<br>- 1 VW J533 connector<br>- 1 comma four<br>- 1 harness box<br>- 1 long OBD-C cable (9.5 ft)<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Volkswagen Golf 2015-20">Buy Here</a></sub></details>|||
|
||||||
|Volkswagen|Golf Alltrack 2015-19|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,14</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 OBD-C cable (2 ft)<br>- 1 VW J533 connector<br>- 1 comma four<br>- 1 harness box<br>- 1 long OBD-C cable (9.5 ft)<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Volkswagen Golf Alltrack 2015-19">Buy Here</a></sub></details>|||
|
|Volkswagen|Golf Alltrack 2015-19|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,14</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 OBD-C cable (2 ft)<br>- 1 USB-C coupler<br>- 1 VW J533 connector<br>- 1 comma four<br>- 1 harness box<br>- 1 long OBD-C cable (9.5 ft)<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Volkswagen Golf Alltrack 2015-19">Buy Here</a></sub></details>|||
|
||||||
|Volkswagen|Golf GTD 2015-20|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,14</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 OBD-C cable (2 ft)<br>- 1 VW J533 connector<br>- 1 comma four<br>- 1 harness box<br>- 1 long OBD-C cable (9.5 ft)<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Volkswagen Golf GTD 2015-20">Buy Here</a></sub></details>|||
|
|Volkswagen|Golf GTD 2015-20|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,14</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 OBD-C cable (2 ft)<br>- 1 USB-C coupler<br>- 1 VW J533 connector<br>- 1 comma four<br>- 1 harness box<br>- 1 long OBD-C cable (9.5 ft)<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Volkswagen Golf GTD 2015-20">Buy Here</a></sub></details>|||
|
||||||
|Volkswagen|Golf GTE 2015-20|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,14</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 OBD-C cable (2 ft)<br>- 1 VW J533 connector<br>- 1 comma four<br>- 1 harness box<br>- 1 long OBD-C cable (9.5 ft)<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Volkswagen Golf GTE 2015-20">Buy Here</a></sub></details>|||
|
|Volkswagen|Golf GTE 2015-20|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,14</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 OBD-C cable (2 ft)<br>- 1 USB-C coupler<br>- 1 VW J533 connector<br>- 1 comma four<br>- 1 harness box<br>- 1 long OBD-C cable (9.5 ft)<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Volkswagen Golf GTE 2015-20">Buy Here</a></sub></details>|||
|
||||||
|Volkswagen|Golf GTI 2015-21|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,14</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 OBD-C cable (2 ft)<br>- 1 VW J533 connector<br>- 1 comma four<br>- 1 harness box<br>- 1 long OBD-C cable (9.5 ft)<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Volkswagen Golf GTI 2015-21">Buy Here</a></sub></details>|||
|
|Volkswagen|Golf GTI 2015-21|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,14</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 OBD-C cable (2 ft)<br>- 1 USB-C coupler<br>- 1 VW J533 connector<br>- 1 comma four<br>- 1 harness box<br>- 1 long OBD-C cable (9.5 ft)<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Volkswagen Golf GTI 2015-21">Buy Here</a></sub></details>|||
|
||||||
|Volkswagen|Golf R 2015-19|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,14</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 OBD-C cable (2 ft)<br>- 1 VW J533 connector<br>- 1 comma four<br>- 1 harness box<br>- 1 long OBD-C cable (9.5 ft)<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Volkswagen Golf R 2015-19">Buy Here</a></sub></details>|||
|
|Volkswagen|Golf R 2015-19|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,14</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 OBD-C cable (2 ft)<br>- 1 USB-C coupler<br>- 1 VW J533 connector<br>- 1 comma four<br>- 1 harness box<br>- 1 long OBD-C cable (9.5 ft)<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Volkswagen Golf R 2015-19">Buy Here</a></sub></details>|||
|
||||||
|Volkswagen|Golf SportsVan 2015-20|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,14</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 OBD-C cable (2 ft)<br>- 1 VW J533 connector<br>- 1 comma four<br>- 1 harness box<br>- 1 long OBD-C cable (9.5 ft)<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Volkswagen Golf SportsVan 2015-20">Buy Here</a></sub></details>|||
|
|Volkswagen|Golf SportsVan 2015-20|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,14</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 OBD-C cable (2 ft)<br>- 1 USB-C coupler<br>- 1 VW J533 connector<br>- 1 comma four<br>- 1 harness box<br>- 1 long OBD-C cable (9.5 ft)<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Volkswagen Golf SportsVan 2015-20">Buy Here</a></sub></details>|||
|
||||||
|Volkswagen|Grand California 2019-24|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,14</sup>](#footnotes)|0 mph|31 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 OBD-C cable (2 ft)<br>- 1 VW J533 connector<br>- 1 comma four<br>- 1 harness box<br>- 1 long OBD-C cable (9.5 ft)<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Volkswagen Grand California 2019-24">Buy Here</a></sub></details>|<a href="https://youtu.be/4100gLeabmo" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>||
|
|Volkswagen|Grand California 2019-24|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,14</sup>](#footnotes)|0 mph|31 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 OBD-C cable (2 ft)<br>- 1 USB-C coupler<br>- 1 VW J533 connector<br>- 1 comma four<br>- 1 harness box<br>- 1 long OBD-C cable (9.5 ft)<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Volkswagen Grand California 2019-24">Buy Here</a></sub></details>|<a href="https://youtu.be/4100gLeabmo" target="_blank"><img height="18px" src="assets/icon-youtube.svg"></img></a>||
|
||||||
|Volkswagen|Jetta 2019-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,14</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 OBD-C cable (2 ft)<br>- 1 VW J533 connector<br>- 1 comma four<br>- 1 harness box<br>- 1 long OBD-C cable (9.5 ft)<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Volkswagen Jetta 2019-23">Buy Here</a></sub></details>|||
|
|Volkswagen|Jetta 2018-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,14</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 OBD-C cable (2 ft)<br>- 1 USB-C coupler<br>- 1 VW J533 connector<br>- 1 comma four<br>- 1 harness box<br>- 1 long OBD-C cable (9.5 ft)<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Volkswagen Jetta 2018-23">Buy Here</a></sub></details>|||
|
||||||
|Volkswagen|Jetta GLI 2021-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,14</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 OBD-C cable (2 ft)<br>- 1 VW J533 connector<br>- 1 comma four<br>- 1 harness box<br>- 1 long OBD-C cable (9.5 ft)<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Volkswagen Jetta GLI 2021-23">Buy Here</a></sub></details>|||
|
|Volkswagen|Jetta GLI 2021-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,14</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 OBD-C cable (2 ft)<br>- 1 USB-C coupler<br>- 1 VW J533 connector<br>- 1 comma four<br>- 1 harness box<br>- 1 long OBD-C cable (9.5 ft)<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Volkswagen Jetta GLI 2021-23">Buy Here</a></sub></details>|||
|
||||||
|Volkswagen|Passat 2015-22[<sup>12</sup>](#footnotes)|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,14</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 OBD-C cable (2 ft)<br>- 1 VW J533 connector<br>- 1 comma four<br>- 1 harness box<br>- 1 long OBD-C cable (9.5 ft)<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Volkswagen Passat 2015-22">Buy Here</a></sub></details>|||
|
|Volkswagen|Passat 2015-22[<sup>12</sup>](#footnotes)|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,14</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 OBD-C cable (2 ft)<br>- 1 USB-C coupler<br>- 1 VW J533 connector<br>- 1 comma four<br>- 1 harness box<br>- 1 long OBD-C cable (9.5 ft)<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Volkswagen Passat 2015-22">Buy Here</a></sub></details>|||
|
||||||
|Volkswagen|Passat Alltrack 2015-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,14</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 OBD-C cable (2 ft)<br>- 1 VW J533 connector<br>- 1 comma four<br>- 1 harness box<br>- 1 long OBD-C cable (9.5 ft)<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Volkswagen Passat Alltrack 2015-22">Buy Here</a></sub></details>|||
|
|Volkswagen|Passat Alltrack 2015-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,14</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 OBD-C cable (2 ft)<br>- 1 USB-C coupler<br>- 1 VW J533 connector<br>- 1 comma four<br>- 1 harness box<br>- 1 long OBD-C cable (9.5 ft)<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Volkswagen Passat Alltrack 2015-22">Buy Here</a></sub></details>|||
|
||||||
|Volkswagen|Passat GTE 2015-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,14</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 OBD-C cable (2 ft)<br>- 1 VW J533 connector<br>- 1 comma four<br>- 1 harness box<br>- 1 long OBD-C cable (9.5 ft)<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Volkswagen Passat GTE 2015-22">Buy Here</a></sub></details>|||
|
|Volkswagen|Passat GTE 2015-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,14</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 OBD-C cable (2 ft)<br>- 1 USB-C coupler<br>- 1 VW J533 connector<br>- 1 comma four<br>- 1 harness box<br>- 1 long OBD-C cable (9.5 ft)<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Volkswagen Passat GTE 2015-22">Buy Here</a></sub></details>|||
|
||||||
|Volkswagen|Polo 2018-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,14</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 OBD-C cable (2 ft)<br>- 1 VW J533 connector<br>- 1 comma four<br>- 1 harness box<br>- 1 long OBD-C cable (9.5 ft)<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Volkswagen Polo 2018-23">Buy Here</a></sub></details>[<sup>15</sup>](#footnotes)|||
|
|Volkswagen|Polo 2018-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,14</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 OBD-C cable (2 ft)<br>- 1 USB-C coupler<br>- 1 VW J533 connector<br>- 1 comma four<br>- 1 harness box<br>- 1 long OBD-C cable (9.5 ft)<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Volkswagen Polo 2018-23">Buy Here</a></sub></details>[<sup>15</sup>](#footnotes)|||
|
||||||
|Volkswagen|Polo GTI 2018-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,14</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 OBD-C cable (2 ft)<br>- 1 VW J533 connector<br>- 1 comma four<br>- 1 harness box<br>- 1 long OBD-C cable (9.5 ft)<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Volkswagen Polo GTI 2018-23">Buy Here</a></sub></details>[<sup>15</sup>](#footnotes)|||
|
|Volkswagen|Polo GTI 2018-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,14</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 OBD-C cable (2 ft)<br>- 1 USB-C coupler<br>- 1 VW J533 connector<br>- 1 comma four<br>- 1 harness box<br>- 1 long OBD-C cable (9.5 ft)<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Volkswagen Polo GTI 2018-23">Buy Here</a></sub></details>[<sup>15</sup>](#footnotes)|||
|
||||||
|Volkswagen|T-Cross 2021|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,14</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 OBD-C cable (2 ft)<br>- 1 VW J533 connector<br>- 1 comma four<br>- 1 harness box<br>- 1 long OBD-C cable (9.5 ft)<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Volkswagen T-Cross 2021">Buy Here</a></sub></details>[<sup>15</sup>](#footnotes)|||
|
|Volkswagen|T-Cross 2021|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,14</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 OBD-C cable (2 ft)<br>- 1 USB-C coupler<br>- 1 VW J533 connector<br>- 1 comma four<br>- 1 harness box<br>- 1 long OBD-C cable (9.5 ft)<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Volkswagen T-Cross 2021">Buy Here</a></sub></details>[<sup>15</sup>](#footnotes)|||
|
||||||
|Volkswagen|T-Roc 2018-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,14</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 OBD-C cable (2 ft)<br>- 1 VW J533 connector<br>- 1 comma four<br>- 1 harness box<br>- 1 long OBD-C cable (9.5 ft)<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Volkswagen T-Roc 2018-23">Buy Here</a></sub></details>|||
|
|Volkswagen|T-Roc 2018-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,14</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 OBD-C cable (2 ft)<br>- 1 USB-C coupler<br>- 1 VW J533 connector<br>- 1 comma four<br>- 1 harness box<br>- 1 long OBD-C cable (9.5 ft)<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Volkswagen T-Roc 2018-23">Buy Here</a></sub></details>|||
|
||||||
|Volkswagen|Taos 2022-24|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,14</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 OBD-C cable (2 ft)<br>- 1 VW J533 connector<br>- 1 comma four<br>- 1 harness box<br>- 1 long OBD-C cable (9.5 ft)<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Volkswagen Taos 2022-24">Buy Here</a></sub></details>|||
|
|Volkswagen|Taos 2022-24|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,14</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 OBD-C cable (2 ft)<br>- 1 USB-C coupler<br>- 1 VW J533 connector<br>- 1 comma four<br>- 1 harness box<br>- 1 long OBD-C cable (9.5 ft)<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Volkswagen Taos 2022-24">Buy Here</a></sub></details>|||
|
||||||
|Volkswagen|Teramont 2018-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,14</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 OBD-C cable (2 ft)<br>- 1 VW J533 connector<br>- 1 comma four<br>- 1 harness box<br>- 1 long OBD-C cable (9.5 ft)<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Volkswagen Teramont 2018-22">Buy Here</a></sub></details>|||
|
|Volkswagen|Teramont 2018-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,14</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 OBD-C cable (2 ft)<br>- 1 USB-C coupler<br>- 1 VW J533 connector<br>- 1 comma four<br>- 1 harness box<br>- 1 long OBD-C cable (9.5 ft)<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Volkswagen Teramont 2018-22">Buy Here</a></sub></details>|||
|
||||||
|Volkswagen|Teramont Cross Sport 2021-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,14</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 OBD-C cable (2 ft)<br>- 1 VW J533 connector<br>- 1 comma four<br>- 1 harness box<br>- 1 long OBD-C cable (9.5 ft)<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Volkswagen Teramont Cross Sport 2021-22">Buy Here</a></sub></details>|||
|
|Volkswagen|Teramont Cross Sport 2021-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,14</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 OBD-C cable (2 ft)<br>- 1 USB-C coupler<br>- 1 VW J533 connector<br>- 1 comma four<br>- 1 harness box<br>- 1 long OBD-C cable (9.5 ft)<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Volkswagen Teramont Cross Sport 2021-22">Buy Here</a></sub></details>|||
|
||||||
|Volkswagen|Teramont X 2021-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,14</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 OBD-C cable (2 ft)<br>- 1 VW J533 connector<br>- 1 comma four<br>- 1 harness box<br>- 1 long OBD-C cable (9.5 ft)<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Volkswagen Teramont X 2021-22">Buy Here</a></sub></details>|||
|
|Volkswagen|Teramont X 2021-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,14</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 OBD-C cable (2 ft)<br>- 1 USB-C coupler<br>- 1 VW J533 connector<br>- 1 comma four<br>- 1 harness box<br>- 1 long OBD-C cable (9.5 ft)<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Volkswagen Teramont X 2021-22">Buy Here</a></sub></details>|||
|
||||||
|Volkswagen|Tiguan 2018-24|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,14</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 OBD-C cable (2 ft)<br>- 1 VW J533 connector<br>- 1 comma four<br>- 1 harness box<br>- 1 long OBD-C cable (9.5 ft)<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Volkswagen Tiguan 2018-24">Buy Here</a></sub></details>|||
|
|Volkswagen|Tiguan 2018-24|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,14</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 OBD-C cable (2 ft)<br>- 1 USB-C coupler<br>- 1 VW J533 connector<br>- 1 comma four<br>- 1 harness box<br>- 1 long OBD-C cable (9.5 ft)<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Volkswagen Tiguan 2018-24">Buy Here</a></sub></details>|||
|
||||||
|Volkswagen|Tiguan eHybrid 2021-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,14</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 OBD-C cable (2 ft)<br>- 1 VW J533 connector<br>- 1 comma four<br>- 1 harness box<br>- 1 long OBD-C cable (9.5 ft)<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Volkswagen Tiguan eHybrid 2021-23">Buy Here</a></sub></details>|||
|
|Volkswagen|Tiguan eHybrid 2021-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,14</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 OBD-C cable (2 ft)<br>- 1 USB-C coupler<br>- 1 VW J533 connector<br>- 1 comma four<br>- 1 harness box<br>- 1 long OBD-C cable (9.5 ft)<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Volkswagen Tiguan eHybrid 2021-23">Buy Here</a></sub></details>|||
|
||||||
|Volkswagen|Touran 2016-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,14</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 OBD-C cable (2 ft)<br>- 1 VW J533 connector<br>- 1 comma four<br>- 1 harness box<br>- 1 long OBD-C cable (9.5 ft)<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Volkswagen Touran 2016-23">Buy Here</a></sub></details>|||
|
|Volkswagen|Touran 2016-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,14</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 OBD-C cable (2 ft)<br>- 1 USB-C coupler<br>- 1 VW J533 connector<br>- 1 comma four<br>- 1 harness box<br>- 1 long OBD-C cable (9.5 ft)<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Volkswagen Touran 2016-23">Buy Here</a></sub></details>|||
|
||||||
|
|
||||||
### Footnotes
|
### Footnotes
|
||||||
<sup>1</sup>openpilot Longitudinal Control (Alpha) is available behind a toggle; the toggle is only available in non-release branches such as `devel` or `nightly-dev`. <br />
|
<sup>1</sup>openpilot Longitudinal Control (Alpha) is available behind a toggle; the toggle is only available in non-release branches such as `devel` or `nightly-dev`. <br />
|
||||||
|
|||||||
@@ -13,13 +13,13 @@ Development is coordinated through [Discord](https://discord.comma.ai) and GitHu
|
|||||||
## What contributions are we looking for?
|
## What contributions are we looking for?
|
||||||
|
|
||||||
**openpilot's priorities are [safety](SAFETY.md), stability, quality, and features, in that order.**
|
**openpilot's priorities are [safety](SAFETY.md), stability, quality, and features, in that order.**
|
||||||
openpilot is part of comma's mission to *solve self-driving cars while delivering shippable intermediaries*, and all development is towards that goal.
|
openpilot is part of comma's mission to *solve self-driving cars while delivering shippable intermediaries*, and all development is towards that goal.
|
||||||
|
|
||||||
### What gets merged?
|
### What gets merged?
|
||||||
|
|
||||||
The probability of a pull request being merged is a function of its value to the project and the effort it will take us to get it merged.
|
The probability of a pull request being merged is a function of its value to the project and the effort it will take us to get it merged.
|
||||||
If a PR offers *some* value but will take lots of time to get merged, it will be closed.
|
If a PR offers *some* value but will take lots of time to get merged, it will be closed.
|
||||||
Simple, well-tested bug fixes are the easiest to merge, and new features are the hardest to get merged.
|
Simple, well-tested bug fixes are the easiest to merge, and new features are the hardest to get merged.
|
||||||
|
|
||||||
All of these are examples of good PRs:
|
All of these are examples of good PRs:
|
||||||
* typo fix: https://github.com/commaai/openpilot/pull/30678
|
* typo fix: https://github.com/commaai/openpilot/pull/30678
|
||||||
@@ -29,17 +29,17 @@ All of these are examples of good PRs:
|
|||||||
|
|
||||||
### What doesn't get merged?
|
### What doesn't get merged?
|
||||||
|
|
||||||
* **style changes**: code is art, and it's up to the author to make it beautiful
|
* **style changes**: code is art, and it's up to the author to make it beautiful
|
||||||
* **500+ line PRs**: clean it up, break it up into smaller PRs, or both
|
* **500+ line PRs**: clean it up, break it up into smaller PRs, or both
|
||||||
* **PRs without a clear goal**: every PR must have a singular and clear goal
|
* **PRs without a clear goal**: every PR must have a singular and clear goal
|
||||||
* **UI design**: we do not have a good review process for this yet
|
* **UI design**: we do not have a good review process for this yet
|
||||||
* **New features**: We believe openpilot is mostly feature-complete, and the rest is a matter of refinement and fixing bugs. As a result of this, most feature PRs will be immediately closed, however the beauty of open source is that forks can and do offer features that upstream openpilot doesn't.
|
* **New features**: We believe openpilot is mostly feature-complete, and the rest is a matter of refinement and fixing bugs. As a result of this, most feature PRs will be immediately closed, however the beauty of open source is that forks can and do offer features that upstream openpilot doesn't.
|
||||||
* **Negative expected value**: This is a class of PRs that makes an improvement, but the risk or validation costs more than the improvement. The risk can be mitigated by first getting a failing test merged.
|
* **Negative expected value**: This a class of PRs that makes an improvement, but the risk or validation costs more than the improvement. The risk can be mitigated by first getting a failing test merged.
|
||||||
|
|
||||||
### First contribution
|
### First contribution
|
||||||
|
|
||||||
[Projects / openpilot bounties](https://github.com/orgs/commaai/projects/26/views/1?pane=info) is the best place to get started and goes in-depth on what's expected when working on a bounty.
|
[Projects / openpilot bounties](https://github.com/orgs/commaai/projects/26/views/1?pane=info) is the best place to get started and goes in-depth on what's expected when working on a bounty.
|
||||||
There are a lot of bounties that don't require a comma 3X or a car.
|
There's lot of bounties that don't require a comma 3X or a car.
|
||||||
|
|
||||||
## Pull Requests
|
## Pull Requests
|
||||||
|
|
||||||
|
|||||||
@@ -8,7 +8,7 @@ NOTE: Those commands must be run in the root directory of openpilot, **not /docs
|
|||||||
|
|
||||||
**1. Install the docs dependencies**
|
**1. Install the docs dependencies**
|
||||||
``` bash
|
``` bash
|
||||||
uv pip install .[docs]
|
pip install .[docs]
|
||||||
```
|
```
|
||||||
|
|
||||||
**2. Build the new site**
|
**2. Build the new site**
|
||||||
|
|||||||
@@ -21,10 +21,10 @@ Each car brand is supported by a standard interface structure in `opendbc/car/[b
|
|||||||
* `values.py`: Limits for actuation, general constants for cars, and supported car documentation
|
* `values.py`: Limits for actuation, general constants for cars, and supported car documentation
|
||||||
* `radar_interface.py`: Interface for parsing radar points from the car, if applicable
|
* `radar_interface.py`: Interface for parsing radar points from the car, if applicable
|
||||||
|
|
||||||
## safety
|
## panda
|
||||||
|
|
||||||
* `opendbc_repo/opendbc/safety/modes/[brand].h`: Brand-specific safety logic
|
* `board/safety/safety_[brand].h`: Brand-specific safety logic
|
||||||
* `opendbc_repo/opendbc/safety/tests/test_[brand].py`: Brand-specific safety CI tests
|
* `tests/safety/test_[brand].py`: Brand-specific safety CI tests
|
||||||
|
|
||||||
## openpilot
|
## openpilot
|
||||||
|
|
||||||
|
|||||||
@@ -24,8 +24,8 @@ SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
|
|||||||
Dashy State Aggregation Daemon
|
Dashy State Aggregation Daemon
|
||||||
|
|
||||||
Aggregates all cereal topics needed by dashy UI into a single dashyState message.
|
Aggregates all cereal topics needed by dashy UI into a single dashyState message.
|
||||||
serverd then forwards that one message over WebSocket, avoiding per-topic
|
This reduces CPU overhead in webrtcd by doing JSON serialization once here
|
||||||
serialization for every connected client.
|
instead of serializing 14+ topics separately.
|
||||||
|
|
||||||
All display formatting (units, distances, times) is done here so the frontend
|
All display formatting (units, distances, times) is done here so the frontend
|
||||||
can be a pure display layer with no conversion logic.
|
can be a pure display layer with no conversion logic.
|
||||||
@@ -46,6 +46,8 @@ LOOP_RATE = 15 # Hz
|
|||||||
DOWNSAMPLE_FACTOR = 2
|
DOWNSAMPLE_FACTOR = 2
|
||||||
|
|
||||||
# Unit conversion constants
|
# Unit conversion constants
|
||||||
|
M_TO_KM = 0.001
|
||||||
|
M_TO_MI = 0.000621371
|
||||||
M_TO_FT = 3.28084
|
M_TO_FT = 3.28084
|
||||||
|
|
||||||
# Global state (refreshed periodically)
|
# Global state (refreshed periodically)
|
||||||
@@ -97,6 +99,42 @@ def format_speed(speed_ms: float) -> str:
|
|||||||
return f"{max(0, speed_ms * Conversions.MS_TO_MPH):.0f}"
|
return f"{max(0, speed_ms * Conversions.MS_TO_MPH):.0f}"
|
||||||
|
|
||||||
|
|
||||||
|
def format_speed_value(speed_ms: float) -> float:
|
||||||
|
"""Convert speed to display units (m/s -> km/h or mph)."""
|
||||||
|
if _is_metric:
|
||||||
|
return max(0, speed_ms * Conversions.MS_TO_KPH)
|
||||||
|
return max(0, speed_ms * Conversions.MS_TO_MPH)
|
||||||
|
|
||||||
|
|
||||||
|
def format_distance(meters: float) -> str:
|
||||||
|
"""Format distance for display."""
|
||||||
|
if meters <= 0:
|
||||||
|
return ""
|
||||||
|
if _is_metric:
|
||||||
|
if meters >= 1000:
|
||||||
|
return f"{meters * M_TO_KM:.1f} km"
|
||||||
|
return f"{meters:.0f} m"
|
||||||
|
else:
|
||||||
|
miles = meters * M_TO_MI
|
||||||
|
if miles >= 0.1:
|
||||||
|
return f"{miles:.1f} mi"
|
||||||
|
return f"{meters * M_TO_FT:.0f} ft"
|
||||||
|
|
||||||
|
|
||||||
|
def format_time(seconds: float) -> str:
|
||||||
|
"""Format time duration for display."""
|
||||||
|
if seconds <= 0:
|
||||||
|
return ""
|
||||||
|
minutes = int(seconds / 60)
|
||||||
|
if minutes < 60:
|
||||||
|
return f"{minutes} min"
|
||||||
|
hours = minutes // 60
|
||||||
|
mins = minutes % 60
|
||||||
|
if mins == 0:
|
||||||
|
return f"{hours} hr"
|
||||||
|
return f"{hours} hr {mins} min"
|
||||||
|
|
||||||
|
|
||||||
def get_speed_unit() -> str:
|
def get_speed_unit() -> str:
|
||||||
"""Get current speed unit string."""
|
"""Get current speed unit string."""
|
||||||
return "km/h" if _is_metric else "mph"
|
return "km/h" if _is_metric else "mph"
|
||||||
@@ -146,14 +184,10 @@ def extract_car_state(sm):
|
|||||||
v_ego = float(cs.vEgo)
|
v_ego = float(cs.vEgo)
|
||||||
v_ego_cluster = float(cs.vEgoCluster)
|
v_ego_cluster = float(cs.vEgoCluster)
|
||||||
|
|
||||||
# Set speed: prefer the modern carState.vCruiseCluster; only fall back
|
# Get set speed from controlsState.vCruiseDEPRECATED (fallback to carState.vCruiseCluster)
|
||||||
# to controlsState.vCruiseDEPRECATED if the cluster value isn't populated.
|
|
||||||
v_cruise = float(cs.vCruiseCluster)
|
v_cruise = float(cs.vCruiseCluster)
|
||||||
if not (0 < v_cruise < SET_SPEED_NA) and 'controlsState' in sm.updated and sm.updated['controlsState']:
|
if 'controlsState' in sm.updated and sm.updated['controlsState']:
|
||||||
try:
|
v_cruise = float(sm['controlsState'].vCruiseDEPRECATED)
|
||||||
v_cruise = float(sm['controlsState'].vCruiseDEPRECATED)
|
|
||||||
except Exception:
|
|
||||||
pass
|
|
||||||
set_speed = get_cruise_speed(v_cruise)
|
set_speed = get_cruise_speed(v_cruise)
|
||||||
|
|
||||||
return {
|
return {
|
||||||
@@ -162,13 +196,10 @@ def extract_car_state(sm):
|
|||||||
'gearShifter': str(cs.gearShifter),
|
'gearShifter': str(cs.gearShifter),
|
||||||
'aEgo': float(cs.aEgo),
|
'aEgo': float(cs.aEgo),
|
||||||
'steeringAngleDeg': float(cs.steeringAngleDeg),
|
'steeringAngleDeg': float(cs.steeringAngleDeg),
|
||||||
'steeringPressed': bool(cs.steeringPressed),
|
|
||||||
'gasPressed': bool(cs.gasPressed),
|
|
||||||
'leftBlinker': bool(cs.leftBlinker),
|
'leftBlinker': bool(cs.leftBlinker),
|
||||||
'rightBlinker': bool(cs.rightBlinker),
|
'rightBlinker': bool(cs.rightBlinker),
|
||||||
'leftBlindspot': bool(cs.leftBlindspot),
|
'leftBlindspot': bool(cs.leftBlindspot),
|
||||||
'rightBlindspot': bool(cs.rightBlindspot),
|
'rightBlindspot': bool(cs.rightBlindspot),
|
||||||
'cruiseEnabled': bool(cs.cruiseState.enabled),
|
|
||||||
# Pre-formatted display values
|
# Pre-formatted display values
|
||||||
'speedDisplay': format_speed(v_ego),
|
'speedDisplay': format_speed(v_ego),
|
||||||
'speedClusterDisplay': format_speed(v_ego_cluster) if v_ego_cluster > 0 else format_speed(v_ego),
|
'speedClusterDisplay': format_speed(v_ego_cluster) if v_ego_cluster > 0 else format_speed(v_ego),
|
||||||
@@ -203,13 +234,8 @@ def extract_device_state(sm):
|
|||||||
temp_display = f"{temp_f:.0f}°" if temp_c > 0 else "--"
|
temp_display = f"{temp_f:.0f}°" if temp_c > 0 else "--"
|
||||||
return {
|
return {
|
||||||
'cpuUsagePercent': list(ds.cpuUsagePercent) if ds.cpuUsagePercent else [],
|
'cpuUsagePercent': list(ds.cpuUsagePercent) if ds.cpuUsagePercent else [],
|
||||||
'gpuUsagePercent': int(ds.gpuUsagePercent),
|
|
||||||
'memoryUsagePercent': int(ds.memoryUsagePercent),
|
'memoryUsagePercent': int(ds.memoryUsagePercent),
|
||||||
'freeSpacePercent': float(ds.freeSpacePercent),
|
|
||||||
'maxTempC': temp_c,
|
'maxTempC': temp_c,
|
||||||
'thermalStatus': str(ds.thermalStatus), # 'green' | 'yellow' | 'red' | 'danger'
|
|
||||||
'fanSpeedPercentDesired': int(ds.fanSpeedPercentDesired),
|
|
||||||
'powerDrawW': float(safe_get(ds, 'powerDrawW', 0)),
|
|
||||||
'deviceType': str(ds.deviceType),
|
'deviceType': str(ds.deviceType),
|
||||||
'tempDisplay': temp_display,
|
'tempDisplay': temp_display,
|
||||||
}
|
}
|
||||||
@@ -222,43 +248,23 @@ def extract_lead(lead, sm):
|
|||||||
y_rel = float(lead.yRel)
|
y_rel = float(lead.yRel)
|
||||||
has_lead = bool(lead.status)
|
has_lead = bool(lead.status)
|
||||||
|
|
||||||
# Pre-format lead display values. Each metric ships as a
|
# Pre-format lead display values
|
||||||
# (value, unit) pair so the HUD can tabular-align numbers without
|
|
||||||
# regex-parsing on the JS side.
|
|
||||||
dist_value = "--"
|
|
||||||
dist_unit = ""
|
|
||||||
speed_value = "--"
|
|
||||||
speed_unit_str = ""
|
|
||||||
ttc_value = "—"
|
|
||||||
ttc_unit = "s"
|
|
||||||
ttc_urgent = False
|
|
||||||
if has_lead:
|
if has_lead:
|
||||||
speed_unit_str = "km/h" if _is_metric else "mph"
|
|
||||||
dist_unit = "m" if _is_metric else "ft"
|
|
||||||
conv = Conversions.MS_TO_KPH if _is_metric else Conversions.MS_TO_MPH
|
|
||||||
dist_value = f"{d_rel:.1f}" if _is_metric else f"{d_rel * M_TO_FT:.1f}"
|
|
||||||
v_ego = float(sm['carState'].vEgo) if sm.valid['carState'] else 0
|
v_ego = float(sm['carState'].vEgo) if sm.valid['carState'] else 0
|
||||||
# Lead's absolute speed = ego + relative (clamped to 0).
|
lead_speed_ms = max(0, v_ego + v_rel)
|
||||||
lead_speed_disp = max(0.0, v_ego + v_rel) * conv
|
speed_display = format_speed(lead_speed_ms)
|
||||||
speed_value = f"{lead_speed_disp:.1f}"
|
distance_display = f"{d_rel:.1f}m" if _is_metric else f"{d_rel * M_TO_FT:.0f}ft"
|
||||||
if v_ego > 0:
|
else:
|
||||||
ttc = d_rel / v_ego
|
speed_display = "--"
|
||||||
if ttc < 5.0:
|
distance_display = "--"
|
||||||
ttc_value = f"{ttc:.1f}"
|
|
||||||
ttc_urgent = True
|
|
||||||
|
|
||||||
return {
|
return {
|
||||||
'status': has_lead,
|
'status': has_lead,
|
||||||
'dRel': d_rel,
|
'dRel': d_rel,
|
||||||
'yRel': y_rel,
|
'yRel': y_rel,
|
||||||
'vRel': v_rel,
|
'vRel': v_rel,
|
||||||
'distValue': dist_value,
|
'speedDisplay': speed_display,
|
||||||
'distUnit': dist_unit,
|
'distanceDisplay': distance_display,
|
||||||
'speedValue': speed_value,
|
|
||||||
'speedUnit': speed_unit_str,
|
|
||||||
'ttcValue': ttc_value,
|
|
||||||
'ttcUnit': ttc_unit,
|
|
||||||
'ttcUrgent': ttc_urgent,
|
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
@@ -298,14 +304,6 @@ def extract_live_tracks(sm):
|
|||||||
# Skip if this track is already shown as a lead vehicle
|
# Skip if this track is already shown as a lead vehicle
|
||||||
if pt.trackId in lead_track_ids:
|
if pt.trackId in lead_track_ids:
|
||||||
continue
|
continue
|
||||||
# Drop stale tracks — radar's predicting, not measuring
|
|
||||||
if not pt.measured:
|
|
||||||
continue
|
|
||||||
# Drop stationary clutter (sign posts, guardrails,
|
|
||||||
# parked cars). |vRel| < 0.5 m/s ≈ standing still
|
|
||||||
# relative to ego; not relevant traffic.
|
|
||||||
if abs(pt.vRel) < 0.5:
|
|
||||||
continue
|
|
||||||
|
|
||||||
points.append({
|
points.append({
|
||||||
'd': float(pt.dRel),
|
'd': float(pt.dRel),
|
||||||
@@ -319,6 +317,85 @@ def extract_live_tracks(sm):
|
|||||||
return {'points': []}
|
return {'points': []}
|
||||||
|
|
||||||
|
|
||||||
|
def extract_live_gps(sm):
|
||||||
|
"""Extract liveGPS fields used by dashy."""
|
||||||
|
gps = sm['liveGPS']
|
||||||
|
# Skip if no coordinates yet
|
||||||
|
if gps.latitude == 0 and gps.longitude == 0:
|
||||||
|
return None
|
||||||
|
return {
|
||||||
|
'latitude': float(gps.latitude),
|
||||||
|
'longitude': float(gps.longitude),
|
||||||
|
'bearingDeg': float(gps.bearingDeg),
|
||||||
|
'speed': float(gps.speed),
|
||||||
|
'gpsOK': bool(gps.gpsOK),
|
||||||
|
'horizontalAccuracy': float(gps.horizontalAccuracy),
|
||||||
|
'status': str(gps.status),
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
def extract_nav_instruction(sm):
|
||||||
|
"""Extract navInstruction fields used by dashy."""
|
||||||
|
nav = sm['navInstruction']
|
||||||
|
maneuver_dist = float(safe_get(nav, 'maneuverDistance', 0))
|
||||||
|
dist_remaining = float(safe_get(nav, 'distanceRemaining', 0))
|
||||||
|
time_remaining = float(safe_get(nav, 'timeRemaining', 0))
|
||||||
|
speed_limit_ms = float(safe_get(nav, 'speedLimit', 0))
|
||||||
|
|
||||||
|
return {
|
||||||
|
'valid': sm.valid['navInstruction'],
|
||||||
|
'maneuverDistance': maneuver_dist,
|
||||||
|
'maneuverPrimaryText': str(safe_get(nav, 'maneuverPrimaryText', '')),
|
||||||
|
'maneuverSecondaryText': str(safe_get(nav, 'maneuverSecondaryText', '')),
|
||||||
|
'maneuverType': str(safe_get(nav, 'maneuverType', '')),
|
||||||
|
'maneuverModifier': str(safe_get(nav, 'maneuverModifier', '')),
|
||||||
|
'distanceRemaining': dist_remaining,
|
||||||
|
'timeRemaining': time_remaining,
|
||||||
|
'timeRemainingTypical': float(safe_get(nav, 'timeRemainingTypical', 0)),
|
||||||
|
'speedLimit': speed_limit_ms,
|
||||||
|
'speedLimitSign': str(safe_get(nav, 'speedLimitSign', '')),
|
||||||
|
# Pre-formatted display values
|
||||||
|
'maneuverDistanceDisplay': format_distance(maneuver_dist),
|
||||||
|
'distanceRemainingDisplay': format_distance(dist_remaining),
|
||||||
|
'timeRemainingDisplay': format_time(time_remaining),
|
||||||
|
'speedLimitDisplay': format_speed(speed_limit_ms) if speed_limit_ms > 0 else '',
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
def extract_nav_instruction_ext(sm):
|
||||||
|
"""Extract navInstructionExt fields used by dashy (extended nav data)."""
|
||||||
|
nav_ext = sm['navInstructionExt']
|
||||||
|
# Extract allManeuvers list (with name field and formatted distance)
|
||||||
|
all_maneuvers = []
|
||||||
|
if hasattr(nav_ext, 'allManeuvers'):
|
||||||
|
for m in nav_ext.allManeuvers:
|
||||||
|
dist = float(safe_get(m, 'distance', 0))
|
||||||
|
all_maneuvers.append({
|
||||||
|
'distance': dist,
|
||||||
|
'distanceDisplay': format_distance(dist),
|
||||||
|
'type': str(safe_get(m, 'type', '')),
|
||||||
|
'modifier': str(safe_get(m, 'modifier', '')),
|
||||||
|
'name': str(safe_get(m, 'name', '')),
|
||||||
|
})
|
||||||
|
return {
|
||||||
|
'turnAngle': float(safe_get(nav_ext, 'turnAngle', 0)),
|
||||||
|
'turnCurvature': float(safe_get(nav_ext, 'turnCurvature', 0)),
|
||||||
|
'allManeuvers': all_maneuvers,
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
def extract_nav_route(sm):
|
||||||
|
"""Extract navRoute coordinates used by dashy."""
|
||||||
|
route = sm['navRoute']
|
||||||
|
coords = []
|
||||||
|
if hasattr(route, 'coordinates'):
|
||||||
|
for c in route.coordinates:
|
||||||
|
coords.append([float(c.longitude), float(c.latitude)])
|
||||||
|
return {
|
||||||
|
'coordinates': coords,
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
def extract_model_v2(sm):
|
def extract_model_v2(sm):
|
||||||
"""Extract modelV2 fields used by dashy (downsampled)."""
|
"""Extract modelV2 fields used by dashy (downsampled)."""
|
||||||
model = sm['modelV2']
|
model = sm['modelV2']
|
||||||
@@ -414,10 +491,14 @@ TOPICS = {
|
|||||||
'liveTracks': {'extractor': extract_live_tracks, 'rate': 'fast'},
|
'liveTracks': {'extractor': extract_live_tracks, 'rate': 'fast'},
|
||||||
'modelV2': {'extractor': extract_model_v2, 'rate': 'fast'},
|
'modelV2': {'extractor': extract_model_v2, 'rate': 'fast'},
|
||||||
'longitudinalPlan': {'extractor': extract_longitudinal_plan, 'rate': 'fast'},
|
'longitudinalPlan': {'extractor': extract_longitudinal_plan, 'rate': 'fast'},
|
||||||
|
'liveGPS': {'extractor': extract_live_gps, 'rate': 'fast'},
|
||||||
|
|
||||||
# Slow topics - poll at fixed intervals
|
# Slow topics - poll at fixed intervals
|
||||||
'deviceState': {'extractor': extract_device_state, 'rate': LOOP_RATE // 2},
|
'deviceState': {'extractor': extract_device_state, 'rate': LOOP_RATE // 2},
|
||||||
'liveCalibration': {'extractor': extract_live_calibration, 'rate': LOOP_RATE},
|
'liveCalibration': {'extractor': extract_live_calibration, 'rate': LOOP_RATE},
|
||||||
|
'navInstruction': {'extractor': extract_nav_instruction, 'rate': LOOP_RATE},
|
||||||
|
'navInstructionExt': {'extractor': extract_nav_instruction_ext, 'rate': LOOP_RATE},
|
||||||
|
'navRoute': {'extractor': extract_nav_route, 'rate': LOOP_RATE},
|
||||||
'carParams': {'extractor': extract_car_params, 'rate': LOOP_RATE * 2},
|
'carParams': {'extractor': extract_car_params, 'rate': LOOP_RATE * 2},
|
||||||
|
|
||||||
# Valid-only topics - just track valid state
|
# Valid-only topics - just track valid state
|
||||||
@@ -431,55 +512,27 @@ TOPICS = {
|
|||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
def _available_topics(topics_cfg):
|
|
||||||
"""Filter TOPICS to only services this cereal schema knows about.
|
|
||||||
|
|
||||||
Lets dragonpilot-specific topics like controlsStateExt drop out
|
|
||||||
cleanly on a vanilla openpilot schema instead of crashing SubMaster.
|
|
||||||
Topics that drop out keep their default cache value (see 'default'
|
|
||||||
in the TOPICS entry), which the frontend already null-checks.
|
|
||||||
"""
|
|
||||||
try:
|
|
||||||
from cereal.services import SERVICE_LIST as _services
|
|
||||||
except ImportError:
|
|
||||||
try:
|
|
||||||
from cereal.services import services as _services
|
|
||||||
except ImportError:
|
|
||||||
return topics_cfg
|
|
||||||
|
|
||||||
out = {}
|
|
||||||
for name, cfg in topics_cfg.items():
|
|
||||||
if name in _services:
|
|
||||||
out[name] = cfg
|
|
||||||
else:
|
|
||||||
cloudlog.info(f"dashyd: cereal service '{name}' not available, skipping")
|
|
||||||
return out
|
|
||||||
|
|
||||||
|
|
||||||
def main():
|
def main():
|
||||||
cloudlog.info("dashyd: starting")
|
cloudlog.info("dashyd: starting")
|
||||||
|
|
||||||
# Initialize metric preference
|
# Initialize metric preference
|
||||||
refresh_metric_preference()
|
refresh_metric_preference()
|
||||||
|
|
||||||
topics = _available_topics(TOPICS)
|
# Derive services list from TOPICS config
|
||||||
|
services = list(TOPICS.keys())
|
||||||
# Derive services list from filtered topics
|
|
||||||
services = list(topics.keys())
|
|
||||||
sm = messaging.SubMaster(services)
|
sm = messaging.SubMaster(services)
|
||||||
pm = messaging.PubMaster(['dashyState'])
|
pm = messaging.PubMaster(['dashyState'])
|
||||||
rk = Ratekeeper(LOOP_RATE)
|
rk = Ratekeeper(LOOP_RATE)
|
||||||
|
|
||||||
# Initialize cache from TOPICS defaults (always include all topics so
|
# Initialize cache from TOPICS defaults (exclude subscribe-only topics)
|
||||||
# the frontend gets default values for dropped optional ones too).
|
|
||||||
cache = {t: cfg.get('default') for t, cfg in TOPICS.items() if cfg.get('rate') != 'subscribe'}
|
cache = {t: cfg.get('default') for t, cfg in TOPICS.items() if cfg.get('rate') != 'subscribe'}
|
||||||
cache['carParams'] = get_car_params_from_params() # special: init from Params
|
cache['carParams'] = get_car_params_from_params() # special: init from Params
|
||||||
|
|
||||||
# Build topic lists from the filtered topics (only subscribed ones run their extractors)
|
# Build topic lists from TOPICS config
|
||||||
fast_topics = {t: cfg['extractor'] for t, cfg in topics.items() if cfg.get('rate') == 'fast'}
|
fast_topics = {t: cfg['extractor'] for t, cfg in TOPICS.items() if cfg.get('rate') == 'fast'}
|
||||||
slow_topics = {t: (cfg['extractor'], cfg['rate']) for t, cfg in topics.items()
|
slow_topics = {t: (cfg['extractor'], cfg['rate']) for t, cfg in TOPICS.items()
|
||||||
if isinstance(cfg.get('rate'), int)}
|
if isinstance(cfg.get('rate'), int)}
|
||||||
valid_topics = [t for t, cfg in topics.items() if cfg.get('rate') == 'valid']
|
valid_topics = [t for t, cfg in TOPICS.items() if cfg.get('rate') == 'valid']
|
||||||
|
|
||||||
cache_dirty = True
|
cache_dirty = True
|
||||||
frame_count = 0
|
frame_count = 0
|
||||||
@@ -515,7 +568,7 @@ def main():
|
|||||||
|
|
||||||
# Only serialize and publish if something changed
|
# Only serialize and publish if something changed
|
||||||
if cache_dirty:
|
if cache_dirty:
|
||||||
# Only publish when critical openpilot data exists
|
# Only publish when critical data exists (nav data can be null)
|
||||||
critical_ready = (
|
critical_ready = (
|
||||||
cache.get('carState') is not None and
|
cache.get('carState') is not None and
|
||||||
cache.get('modelV2') is not None and
|
cache.get('modelV2') is not None and
|
||||||
|
|||||||
@@ -0,0 +1,423 @@
|
|||||||
|
#!/usr/bin/env python3
|
||||||
|
"""
|
||||||
|
Copyright (c) 2026, Rick Lan
|
||||||
|
|
||||||
|
Permission is hereby granted, free of charge, to any person obtaining a copy
|
||||||
|
of this software and associated documentation files (the "Software"), to deal
|
||||||
|
in the Software without restriction, including without limitation the rights
|
||||||
|
to use, copy, modify, merge, publish, distribute, and/or sublicense,
|
||||||
|
for non-commercial purposes only, subject to the following conditions:
|
||||||
|
|
||||||
|
- The above copyright notice and this permission notice shall be included in
|
||||||
|
all copies or substantial portions of the Software.
|
||||||
|
- Commercial use (e.g. use in a product, service, or activity intended to
|
||||||
|
generate revenue) is prohibited without explicit written permission from
|
||||||
|
the copyright holder.
|
||||||
|
|
||||||
|
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED,
|
||||||
|
INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A
|
||||||
|
PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
|
||||||
|
HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
|
||||||
|
OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
|
||||||
|
SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
|
||||||
|
|
||||||
|
GPS Location Service - GPS + livePose fusion with Kalman filter.
|
||||||
|
- Position: 2D Kalman filter fusing GPS with livePose velocity
|
||||||
|
- Bearing: livePose yaw + GPS-calibrated offset (with slow drift correction)
|
||||||
|
"""
|
||||||
|
import numpy as np
|
||||||
|
from cereal import messaging, custom
|
||||||
|
|
||||||
|
from openpilot.common.params import Params
|
||||||
|
from openpilot.common.realtime import Ratekeeper
|
||||||
|
from openpilot.common.transformations.coordinates import LocalCoord
|
||||||
|
from openpilot.common.swaglog import cloudlog
|
||||||
|
from openpilot.common.gps import get_gps_location_service
|
||||||
|
|
||||||
|
|
||||||
|
def wrap_angle(x):
|
||||||
|
return np.arctan2(np.sin(x), np.cos(x))
|
||||||
|
|
||||||
|
|
||||||
|
class GPSKalman:
|
||||||
|
"""
|
||||||
|
3D Kalman filter for GPS fusion.
|
||||||
|
State: [north, east, yaw_offset] where yaw_offset calibrates livePose to true north.
|
||||||
|
Adapts automatically to GPS accuracy (ublox vs qcom).
|
||||||
|
"""
|
||||||
|
# Process noise
|
||||||
|
POS_NOISE = 0.5 # m²/s - position uncertainty growth
|
||||||
|
YAW_NOISE = 0.0001 # rad²/s - yaw offset drift (~0.6°/min)
|
||||||
|
|
||||||
|
def __init__(self):
|
||||||
|
self.x = np.zeros(3) # [north, east, yaw_offset]
|
||||||
|
self.P = np.diag([100.0, 100.0, 1.0]) # uncertainty (position high, yaw moderate)
|
||||||
|
|
||||||
|
def get_yaw(self, pose_yaw):
|
||||||
|
"""Get calibrated yaw from pose yaw + estimated offset."""
|
||||||
|
return wrap_angle(pose_yaw + self.x[2])
|
||||||
|
|
||||||
|
def predict(self, vel_ned, dt):
|
||||||
|
"""Predict state using velocity from livePose."""
|
||||||
|
# Position prediction
|
||||||
|
self.x[0] += vel_ned[0] * dt
|
||||||
|
self.x[1] += vel_ned[1] * dt
|
||||||
|
# yaw_offset: no change (constant), just add process noise
|
||||||
|
|
||||||
|
# Process noise - more yaw drift when stopped (gyro drift accumulates)
|
||||||
|
speed = np.linalg.norm(vel_ned[:2])
|
||||||
|
yaw_noise = self.YAW_NOISE if speed > 1.0 else self.YAW_NOISE * 10
|
||||||
|
Q = np.diag([self.POS_NOISE * dt, self.POS_NOISE * dt, yaw_noise * dt])
|
||||||
|
self.P += Q
|
||||||
|
|
||||||
|
def update_position(self, gps_ned, gps_accuracy):
|
||||||
|
"""Update position with GPS measurement."""
|
||||||
|
# Observation matrix: observe [north, east], not yaw_offset
|
||||||
|
H = np.array([[1, 0, 0],
|
||||||
|
[0, 1, 0]])
|
||||||
|
|
||||||
|
# Measurement noise from GPS accuracy
|
||||||
|
R = np.eye(2) * (gps_accuracy ** 2)
|
||||||
|
|
||||||
|
# Innovation
|
||||||
|
z = gps_ned[:2]
|
||||||
|
y = z - H @ self.x
|
||||||
|
|
||||||
|
# Kalman gain
|
||||||
|
S = H @ self.P @ H.T + R
|
||||||
|
det = S[0, 0] * S[1, 1] - S[0, 1] * S[1, 0]
|
||||||
|
if abs(det) < 1e-10:
|
||||||
|
return
|
||||||
|
S_inv = np.array([[S[1, 1], -S[0, 1]],
|
||||||
|
[-S[1, 0], S[0, 0]]]) / det
|
||||||
|
K = self.P @ H.T @ S_inv
|
||||||
|
|
||||||
|
# Update
|
||||||
|
self.x += K @ y
|
||||||
|
self.P = (np.eye(3) - K @ H) @ self.P
|
||||||
|
self._ensure_positive_definite()
|
||||||
|
|
||||||
|
def update_yaw(self, gps_bearing, pose_yaw, bearing_std):
|
||||||
|
"""Update yaw_offset with GPS bearing measurement."""
|
||||||
|
# Observation: yaw_offset = gps_bearing - pose_yaw
|
||||||
|
# H = [0, 0, 1] - we observe yaw_offset directly
|
||||||
|
H = np.array([[0, 0, 1]])
|
||||||
|
|
||||||
|
# Measurement noise from GPS bearing uncertainty
|
||||||
|
R = np.array([[bearing_std ** 2]])
|
||||||
|
|
||||||
|
# Expected yaw_offset from GPS
|
||||||
|
observed_offset = wrap_angle(gps_bearing - pose_yaw)
|
||||||
|
|
||||||
|
# Innovation (handle angle wrapping)
|
||||||
|
predicted_offset = self.x[2]
|
||||||
|
y = np.array([wrap_angle(observed_offset - predicted_offset)])
|
||||||
|
|
||||||
|
# Kalman gain
|
||||||
|
S = H @ self.P @ H.T + R
|
||||||
|
K = self.P @ H.T / S[0, 0]
|
||||||
|
|
||||||
|
# Update
|
||||||
|
self.x += (K @ y).flatten()
|
||||||
|
self.x[2] = wrap_angle(self.x[2]) # keep yaw_offset wrapped
|
||||||
|
self.P = (np.eye(3) - K @ H) @ self.P
|
||||||
|
self._ensure_positive_definite()
|
||||||
|
|
||||||
|
def _ensure_positive_definite(self):
|
||||||
|
"""Ensure covariance stays positive definite. Reinit if corrupted."""
|
||||||
|
self.P = (self.P + self.P.T) / 2
|
||||||
|
if np.any(np.diag(self.P) < 0):
|
||||||
|
cloudlog.warning("gpsd: negative covariance detected, reinitializing filter")
|
||||||
|
self.P = np.diag([100.0, 100.0, 1.0])
|
||||||
|
return
|
||||||
|
min_var = np.array([0.01, 0.01, 0.0001]) # minimum variances
|
||||||
|
for i in range(3):
|
||||||
|
self.P[i, i] = max(self.P[i, i], min_var[i])
|
||||||
|
|
||||||
|
def reset(self, pos, yaw_offset=None):
|
||||||
|
"""Reset to known position, optionally with yaw offset."""
|
||||||
|
self.x[0] = pos[0]
|
||||||
|
self.x[1] = pos[1]
|
||||||
|
if yaw_offset is not None:
|
||||||
|
self.x[2] = yaw_offset
|
||||||
|
self.P = np.diag([1.0, 1.0, 0.1]) # low uncertainty
|
||||||
|
else:
|
||||||
|
self.P = np.diag([1.0, 1.0, self.P[2, 2]]) # keep yaw uncertainty
|
||||||
|
|
||||||
|
@property
|
||||||
|
def pos(self):
|
||||||
|
"""Position [north, east]."""
|
||||||
|
return self.x[:2]
|
||||||
|
|
||||||
|
@property
|
||||||
|
def yaw_offset(self):
|
||||||
|
"""Yaw offset estimate."""
|
||||||
|
return self.x[2]
|
||||||
|
|
||||||
|
@property
|
||||||
|
def pos_uncertainty(self):
|
||||||
|
"""Position uncertainty (meters)."""
|
||||||
|
return np.sqrt(max(0.0, (self.P[0, 0] + self.P[1, 1]) / 2))
|
||||||
|
|
||||||
|
@property
|
||||||
|
def yaw_uncertainty(self):
|
||||||
|
"""Yaw offset uncertainty (radians)."""
|
||||||
|
return np.sqrt(max(0.0, self.P[2, 2]))
|
||||||
|
|
||||||
|
|
||||||
|
class LiveGPS:
|
||||||
|
"""
|
||||||
|
GPS + livePose fusion with 3D Kalman filter.
|
||||||
|
- Position: Kalman filter fusing GPS with livePose velocity
|
||||||
|
- Bearing: Kalman-estimated yaw_offset + livePose yaw
|
||||||
|
"""
|
||||||
|
GPS_MIN_SPEED = 5.0 # m/s (18 km/h) - need speed for reliable GPS bearing
|
||||||
|
GPS_MAX_ACCURACY = 50.0 # m - reject very bad GPS
|
||||||
|
BEARING_STD_BASE = 0.1 # rad (~6°) - base GPS bearing uncertainty
|
||||||
|
BEARING_STD_PER_ACC = 0.02 # rad per meter of GPS accuracy
|
||||||
|
|
||||||
|
def __init__(self):
|
||||||
|
# pose inputs
|
||||||
|
self.orientation_ned = np.zeros(3)
|
||||||
|
self.vel_device = np.zeros(3)
|
||||||
|
|
||||||
|
# gps inputs
|
||||||
|
self.gps = None
|
||||||
|
self.last_gps_t = 0.0
|
||||||
|
self.unix_timestamp_millis = 0
|
||||||
|
|
||||||
|
# Kalman filter: [north, east, yaw_offset]
|
||||||
|
self.origin = None # LocalCoord of first GPS fix
|
||||||
|
self.kf = GPSKalman() # 3D Kalman filter
|
||||||
|
self.altitude = 0.0 # altitude tracked separately (1D)
|
||||||
|
self.last_gps_update_t = 0.0 # track when we last updated Kalman with GPS
|
||||||
|
|
||||||
|
# timing
|
||||||
|
self.last_t = None
|
||||||
|
self.last_pose_yaw = None # for yaw rate calculation
|
||||||
|
self.live_pose_ok = False # for monitoring
|
||||||
|
|
||||||
|
# -----------------------------
|
||||||
|
# inputs
|
||||||
|
# -----------------------------
|
||||||
|
|
||||||
|
def handle_pose(self, pose):
|
||||||
|
if pose.orientationNED.valid:
|
||||||
|
self.orientation_ned[:] = [
|
||||||
|
pose.orientationNED.x,
|
||||||
|
pose.orientationNED.y,
|
||||||
|
pose.orientationNED.z,
|
||||||
|
]
|
||||||
|
if pose.velocityDevice.valid:
|
||||||
|
self.vel_device[:] = [
|
||||||
|
pose.velocityDevice.x,
|
||||||
|
pose.velocityDevice.y,
|
||||||
|
pose.velocityDevice.z,
|
||||||
|
]
|
||||||
|
# For monitoring
|
||||||
|
self.live_pose_ok = pose.orientationNED.valid and pose.velocityDevice.valid
|
||||||
|
|
||||||
|
def handle_gps(self, t, gps):
|
||||||
|
if gps.horizontalAccuracy > 0 and gps.horizontalAccuracy > self.GPS_MAX_ACCURACY:
|
||||||
|
return
|
||||||
|
if abs(gps.latitude) < 0.1 or abs(gps.longitude) < 0.1:
|
||||||
|
return
|
||||||
|
|
||||||
|
self.gps = gps
|
||||||
|
self.last_gps_t = t
|
||||||
|
self.unix_timestamp_millis = gps.unixTimestampMillis
|
||||||
|
|
||||||
|
# -----------------------------
|
||||||
|
# core update
|
||||||
|
# -----------------------------
|
||||||
|
|
||||||
|
def update(self, t):
|
||||||
|
dt = (t - self.last_t) if self.last_t else 0.05
|
||||||
|
self.last_t = t
|
||||||
|
|
||||||
|
if self.gps is None:
|
||||||
|
return
|
||||||
|
|
||||||
|
# initialize origin on first GPS
|
||||||
|
if self.origin is None:
|
||||||
|
self.origin = LocalCoord.from_geodetic([self.gps.latitude, self.gps.longitude, self.gps.altitude])
|
||||||
|
self.kf.reset(np.zeros(2))
|
||||||
|
self.altitude = self.gps.altitude
|
||||||
|
cloudlog.info(f"gpsd: origin set at {self.gps.latitude:.6f}, {self.gps.longitude:.6f}")
|
||||||
|
return
|
||||||
|
|
||||||
|
# get current yaw from Kalman (pose_yaw + estimated yaw_offset)
|
||||||
|
pose_yaw = self.orientation_ned[2]
|
||||||
|
yaw = self.kf.get_yaw(pose_yaw)
|
||||||
|
|
||||||
|
# transform velocity from device frame to NED
|
||||||
|
cos_yaw, sin_yaw = np.cos(yaw), np.sin(yaw)
|
||||||
|
vel_ned = np.array([
|
||||||
|
self.vel_device[0] * cos_yaw - self.vel_device[1] * sin_yaw,
|
||||||
|
self.vel_device[0] * sin_yaw + self.vel_device[1] * cos_yaw,
|
||||||
|
-self.vel_device[2]
|
||||||
|
])
|
||||||
|
|
||||||
|
# Kalman predict: propagate position using livePose velocity
|
||||||
|
# Skip prediction when stationary (GPS wanders, IMU drifts)
|
||||||
|
# Threshold 0.1 m/s - above noise (~0.04) but catches actual stops
|
||||||
|
speed = np.linalg.norm(self.vel_device[:2])
|
||||||
|
is_moving = speed > 0.1
|
||||||
|
if is_moving:
|
||||||
|
self.kf.predict(vel_ned, dt)
|
||||||
|
|
||||||
|
# Kalman update: only on NEW GPS data (not stale)
|
||||||
|
# Skip position update when stopped - prevents GPS wander from moving position
|
||||||
|
new_gps = self.last_gps_t > self.last_gps_update_t
|
||||||
|
if new_gps and is_moving:
|
||||||
|
self.last_gps_update_t = self.last_gps_t
|
||||||
|
gps_ned = self.origin.geodetic2ned([self.gps.latitude, self.gps.longitude, self.gps.altitude])
|
||||||
|
gps_accuracy = self.gps.horizontalAccuracy if self.gps.horizontalAccuracy > 0 else 15.0
|
||||||
|
|
||||||
|
# Check for hard reset (large error after tunnel/GPS loss)
|
||||||
|
error = np.linalg.norm(gps_ned[:2] - self.kf.pos)
|
||||||
|
if error > 100.0 and gps_accuracy < 20.0:
|
||||||
|
# GPS is good but we're far off - full reset (new origin, fresh Kalman)
|
||||||
|
yaw_err_deg = np.degrees(self.kf.yaw_uncertainty)
|
||||||
|
cloudlog.warning(f"gpsd: hard reset, error={error:.1f}m, yaw_unc={yaw_err_deg:.1f}°, gps_acc={gps_accuracy:.1f}m, livePoseOk={self.live_pose_ok}")
|
||||||
|
# Reset origin to current GPS - starts fresh
|
||||||
|
self.origin = LocalCoord.from_geodetic([self.gps.latitude, self.gps.longitude, self.gps.altitude])
|
||||||
|
self.kf = GPSKalman() # fresh Kalman filter
|
||||||
|
self.kf.reset(np.zeros(2)) # position (0,0) at new origin
|
||||||
|
self.altitude = self.gps.altitude
|
||||||
|
return # skip normal update this frame
|
||||||
|
else:
|
||||||
|
# Position update - adapts to GPS accuracy:
|
||||||
|
# - ublox (2-5m): high gain, trusts GPS
|
||||||
|
# - qcom (10-30m): low gain, trusts IMU more
|
||||||
|
self.kf.update_position(gps_ned, gps_accuracy)
|
||||||
|
|
||||||
|
# simple altitude tracking (no Kalman needed)
|
||||||
|
self.altitude = 0.9 * self.altitude + 0.1 * self.gps.altitude
|
||||||
|
|
||||||
|
# Yaw update (need speed for reliable GPS bearing)
|
||||||
|
if self.gps.speed > self.GPS_MIN_SPEED:
|
||||||
|
# compute yaw rate to check if driving straight
|
||||||
|
yaw_rate = 0.0
|
||||||
|
if self.last_pose_yaw is not None and dt > 0:
|
||||||
|
yaw_rate = abs(wrap_angle(pose_yaw - self.last_pose_yaw)) / dt
|
||||||
|
|
||||||
|
# GPS bearing is unreliable during turns - increase uncertainty
|
||||||
|
gps_bearing = np.radians(self.gps.bearingDeg)
|
||||||
|
bearing_std = self.BEARING_STD_BASE + self.BEARING_STD_PER_ACC * gps_accuracy
|
||||||
|
if yaw_rate > 0.1: # turning - GPS bearing lags
|
||||||
|
bearing_std *= 3.0
|
||||||
|
|
||||||
|
self.kf.update_yaw(gps_bearing, pose_yaw, bearing_std)
|
||||||
|
|
||||||
|
self.last_pose_yaw = pose_yaw
|
||||||
|
|
||||||
|
# -----------------------------
|
||||||
|
# output
|
||||||
|
# -----------------------------
|
||||||
|
|
||||||
|
def get_msg(self, log_mono_time):
|
||||||
|
msg = messaging.new_message("liveGPS", valid=True)
|
||||||
|
msg.logMonoTime = log_mono_time
|
||||||
|
out = msg.liveGPS
|
||||||
|
|
||||||
|
t = log_mono_time * 1e-9
|
||||||
|
gps_fresh = self.gps is not None and (t - self.last_gps_t) < 5.0
|
||||||
|
pos_initialized = self.origin is not None
|
||||||
|
# yaw is calibrated when uncertainty < 0.5 rad (~30°)
|
||||||
|
yaw_calibrated = self.kf.yaw_uncertainty < 0.5
|
||||||
|
|
||||||
|
if pos_initialized:
|
||||||
|
# position from Kalman filter (NED -> geodetic)
|
||||||
|
pos_ned = np.array([self.kf.pos[0], self.kf.pos[1], self.altitude])
|
||||||
|
geodetic = self.origin.ned2geodetic(pos_ned)
|
||||||
|
out.latitude = float(geodetic[0])
|
||||||
|
out.longitude = float(geodetic[1])
|
||||||
|
out.altitude = float(geodetic[2])
|
||||||
|
out.speed = float(np.linalg.norm(self.vel_device[:2]))
|
||||||
|
|
||||||
|
# horizontalAccuracy from Kalman uncertainty
|
||||||
|
out.horizontalAccuracy = float(self.kf.pos_uncertainty)
|
||||||
|
out.verticalAccuracy = float(self.gps.verticalAccuracy) if hasattr(self.gps, 'verticalAccuracy') and self.gps.verticalAccuracy > 0 else 15.0
|
||||||
|
|
||||||
|
# bearing from Kalman (pose_yaw + estimated yaw_offset)
|
||||||
|
has_livePose = np.any(self.orientation_ned != 0)
|
||||||
|
if yaw_calibrated and has_livePose and gps_fresh:
|
||||||
|
yaw = self.kf.get_yaw(self.orientation_ned[2])
|
||||||
|
out.bearingDeg = float(np.degrees(yaw) % 360)
|
||||||
|
out.status = custom.LiveGPS.Status.valid
|
||||||
|
else:
|
||||||
|
# fallback to raw GPS bearing
|
||||||
|
out.bearingDeg = float(self.gps.bearingDeg)
|
||||||
|
out.status = custom.LiveGPS.Status.uncalibrated
|
||||||
|
elif self.gps is not None:
|
||||||
|
# have GPS but not initialized yet - pass through raw
|
||||||
|
out.latitude = float(self.gps.latitude)
|
||||||
|
out.longitude = float(self.gps.longitude)
|
||||||
|
out.altitude = float(self.gps.altitude)
|
||||||
|
out.speed = float(self.gps.speed)
|
||||||
|
out.bearingDeg = float(self.gps.bearingDeg)
|
||||||
|
out.horizontalAccuracy = float(self.gps.horizontalAccuracy) if self.gps.horizontalAccuracy > 0 else 20.0
|
||||||
|
out.status = custom.LiveGPS.Status.uncalibrated
|
||||||
|
else:
|
||||||
|
out.status = custom.LiveGPS.Status.uninitialized
|
||||||
|
|
||||||
|
# gpsOK = position is usable (bearing calibration tracked separately via status)
|
||||||
|
out.gpsOK = gps_fresh and pos_initialized
|
||||||
|
out.unixTimestampMillis = self.unix_timestamp_millis
|
||||||
|
out.lastGpsTimestamp = int(self.last_gps_t * 1e9)
|
||||||
|
|
||||||
|
# livePose health - for monitoring
|
||||||
|
out.livePoseOk = self.live_pose_ok
|
||||||
|
|
||||||
|
return msg
|
||||||
|
|
||||||
|
def main():
|
||||||
|
import os
|
||||||
|
params = Params()
|
||||||
|
|
||||||
|
# EXT=1 forces gpsLocationExternal (ublox), EXT=0 forces gpsLocation (qcom)
|
||||||
|
ext_override = os.environ.get("EXT")
|
||||||
|
if ext_override == "1":
|
||||||
|
gps_service = "gpsLocationExternal"
|
||||||
|
cloudlog.info("gpsd: EXT=1, using gpsLocationExternal (ublox)")
|
||||||
|
elif ext_override == "0":
|
||||||
|
gps_service = "gpsLocation"
|
||||||
|
cloudlog.info("gpsd: EXT=0, using gpsLocation (qcom)")
|
||||||
|
else:
|
||||||
|
gps_service = get_gps_location_service(params)
|
||||||
|
|
||||||
|
pm = messaging.PubMaster(["liveGPS"])
|
||||||
|
sm = messaging.SubMaster([gps_service, "livePose"], ignore_alive=[gps_service])
|
||||||
|
|
||||||
|
gps = LiveGPS()
|
||||||
|
rk = Ratekeeper(20)
|
||||||
|
|
||||||
|
while True:
|
||||||
|
try:
|
||||||
|
sm.update(0)
|
||||||
|
|
||||||
|
if sm.logMonoTime["livePose"] > 0:
|
||||||
|
t = sm.logMonoTime["livePose"] * 1e-9
|
||||||
|
log_mono_time = sm.logMonoTime["livePose"]
|
||||||
|
else:
|
||||||
|
log_mono_time = int(rk.frame * 1e9 / 20)
|
||||||
|
t = log_mono_time * 1e-9
|
||||||
|
|
||||||
|
if sm.updated[gps_service]:
|
||||||
|
gps.handle_gps(t, sm[gps_service])
|
||||||
|
|
||||||
|
if sm.updated["livePose"] and sm.valid["livePose"]:
|
||||||
|
gps.handle_pose(sm["livePose"])
|
||||||
|
|
||||||
|
gps.update(t)
|
||||||
|
pm.send("liveGPS", gps.get_msg(log_mono_time))
|
||||||
|
|
||||||
|
except Exception:
|
||||||
|
cloudlog.exception("gpsd: error in main loop")
|
||||||
|
|
||||||
|
rk.keep_time()
|
||||||
|
|
||||||
|
|
||||||
|
if __name__ == "__main__":
|
||||||
|
main()
|
||||||
@@ -0,0 +1,30 @@
|
|||||||
|
"""
|
||||||
|
Copyright (c) 2026, Rick Lan
|
||||||
|
|
||||||
|
Permission is hereby granted, free of charge, to any person obtaining a copy
|
||||||
|
of this software and associated documentation files (the "Software"), to deal
|
||||||
|
in the Software without restriction, including without limitation the rights
|
||||||
|
to use, copy, modify, merge, publish, distribute, and/or sublicense,
|
||||||
|
for non-commercial purposes only, subject to the following conditions:
|
||||||
|
|
||||||
|
- The above copyright notice and this permission notice shall be included in
|
||||||
|
all copies or substantial portions of the Software.
|
||||||
|
- Commercial use (e.g. use in a product, service, or activity intended to
|
||||||
|
generate revenue) is prohibited without explicit written permission from
|
||||||
|
the copyright holder.
|
||||||
|
|
||||||
|
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED,
|
||||||
|
INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A
|
||||||
|
PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
|
||||||
|
HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
|
||||||
|
OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
|
||||||
|
SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
|
||||||
|
|
||||||
|
dragonpilot Map-Aware Assist (MAA)
|
||||||
|
|
||||||
|
Navigation-assisted driving module that provides:
|
||||||
|
- Route tracking and turn-by-turn navigation
|
||||||
|
- Turn speed limits based on road curvature
|
||||||
|
- Blinker-based turn desire for lane changes
|
||||||
|
- Physics-based acceleration limiting
|
||||||
|
"""
|
||||||
@@ -0,0 +1,657 @@
|
|||||||
|
"""
|
||||||
|
Copyright (c) 2026, Rick Lan
|
||||||
|
|
||||||
|
Permission is hereby granted, free of charge, to any person obtaining a copy
|
||||||
|
of this software and associated documentation files (the "Software"), to deal
|
||||||
|
in the Software without restriction, including without limitation the rights
|
||||||
|
to use, copy, modify, merge, publish, distribute, and/or sublicense,
|
||||||
|
for non-commercial purposes only, subject to the following conditions:
|
||||||
|
|
||||||
|
- The above copyright notice and this permission notice shall be included in
|
||||||
|
all copies or substantial portions of the Software.
|
||||||
|
- Commercial use (e.g. use in a product, service, or activity intended to
|
||||||
|
generate revenue) is prohibited without explicit written permission from
|
||||||
|
the copyright holder.
|
||||||
|
|
||||||
|
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED,
|
||||||
|
INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A
|
||||||
|
PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
|
||||||
|
HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
|
||||||
|
OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
|
||||||
|
SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
|
||||||
|
|
||||||
|
Navigation helpers for dragonpilot MAA.
|
||||||
|
|
||||||
|
Coordinate math, route parsing, and curvature computation utilities.
|
||||||
|
"""
|
||||||
|
from __future__ import annotations
|
||||||
|
|
||||||
|
import json
|
||||||
|
import math
|
||||||
|
from typing import Any, cast
|
||||||
|
|
||||||
|
from opendbc.car.common.conversions import Conversions
|
||||||
|
from openpilot.common.params import Params
|
||||||
|
|
||||||
|
DIRECTIONS = ('left', 'right', 'straight')
|
||||||
|
MODIFIABLE_DIRECTIONS = ('left', 'right')
|
||||||
|
EARTH_MEAN_RADIUS = 6371007.2
|
||||||
|
|
||||||
|
# Speed unit conversions to m/s
|
||||||
|
SPEED_CONVERSIONS = {
|
||||||
|
'km/h': Conversions.KPH_TO_MS,
|
||||||
|
'mph': Conversions.MPH_TO_MS,
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
class Coordinate:
|
||||||
|
def __init__(self, latitude: float, longitude: float) -> None:
|
||||||
|
self.latitude = latitude
|
||||||
|
self.longitude = longitude
|
||||||
|
self.annotations: dict[str, float] = {}
|
||||||
|
|
||||||
|
@classmethod
|
||||||
|
def from_mapbox_tuple(cls, t: tuple[float, float]) -> Coordinate:
|
||||||
|
return cls(t[1], t[0])
|
||||||
|
|
||||||
|
@classmethod
|
||||||
|
def from_osrm_tuple(cls, t: list[float]) -> Coordinate:
|
||||||
|
"""OSRM uses [lon, lat] order."""
|
||||||
|
return cls(t[1], t[0])
|
||||||
|
|
||||||
|
def as_dict(self) -> dict[str, float]:
|
||||||
|
return {'latitude': self.latitude, 'longitude': self.longitude}
|
||||||
|
|
||||||
|
def __str__(self) -> str:
|
||||||
|
return f'Coordinate({self.latitude}, {self.longitude})'
|
||||||
|
|
||||||
|
def __repr__(self) -> str:
|
||||||
|
return self.__str__()
|
||||||
|
|
||||||
|
def __eq__(self, other) -> bool:
|
||||||
|
if not isinstance(other, Coordinate):
|
||||||
|
return False
|
||||||
|
return self.latitude == other.latitude and self.longitude == other.longitude
|
||||||
|
|
||||||
|
def __hash__(self) -> int:
|
||||||
|
return hash((self.latitude, self.longitude))
|
||||||
|
|
||||||
|
def __sub__(self, other: Coordinate) -> Coordinate:
|
||||||
|
return Coordinate(self.latitude - other.latitude, self.longitude - other.longitude)
|
||||||
|
|
||||||
|
def __add__(self, other: Coordinate) -> Coordinate:
|
||||||
|
return Coordinate(self.latitude + other.latitude, self.longitude + other.longitude)
|
||||||
|
|
||||||
|
def __mul__(self, c: float) -> Coordinate:
|
||||||
|
return Coordinate(self.latitude * c, self.longitude * c)
|
||||||
|
|
||||||
|
def dot(self, other: Coordinate) -> float:
|
||||||
|
return self.latitude * other.latitude + self.longitude * other.longitude
|
||||||
|
|
||||||
|
def distance_to(self, other: Coordinate) -> float:
|
||||||
|
"""Haversine distance in meters."""
|
||||||
|
dlat = math.radians(other.latitude - self.latitude)
|
||||||
|
dlon = math.radians(other.longitude - self.longitude)
|
||||||
|
|
||||||
|
haversine_dlat = math.sin(dlat / 2.0)
|
||||||
|
haversine_dlat *= haversine_dlat
|
||||||
|
haversine_dlon = math.sin(dlon / 2.0)
|
||||||
|
haversine_dlon *= haversine_dlon
|
||||||
|
|
||||||
|
y = haversine_dlat \
|
||||||
|
+ math.cos(math.radians(self.latitude)) \
|
||||||
|
* math.cos(math.radians(other.latitude)) \
|
||||||
|
* haversine_dlon
|
||||||
|
x = 2 * math.asin(math.sqrt(y))
|
||||||
|
return x * EARTH_MEAN_RADIUS
|
||||||
|
|
||||||
|
def bearing_to(self, other: Coordinate) -> float:
|
||||||
|
"""Bearing to other coordinate in degrees (0-360)."""
|
||||||
|
lat1, lat2 = math.radians(self.latitude), math.radians(other.latitude)
|
||||||
|
dlon = math.radians(other.longitude - self.longitude)
|
||||||
|
x = math.sin(dlon) * math.cos(lat2)
|
||||||
|
y = math.cos(lat1) * math.sin(lat2) - math.sin(lat1) * math.cos(lat2) * math.cos(dlon)
|
||||||
|
bearing = math.degrees(math.atan2(x, y))
|
||||||
|
return (bearing + 360) % 360
|
||||||
|
|
||||||
|
|
||||||
|
def minimum_distance(a: Coordinate, b: Coordinate, p: Coordinate) -> float:
|
||||||
|
"""Minimum distance from point p to line segment ab."""
|
||||||
|
if a.distance_to(b) < 0.01:
|
||||||
|
return a.distance_to(p)
|
||||||
|
|
||||||
|
ap = p - a
|
||||||
|
ab = b - a
|
||||||
|
t = max(0.0, min(1.0, ap.dot(ab) / ab.dot(ab)))
|
||||||
|
projection = a + ab * t
|
||||||
|
return projection.distance_to(p)
|
||||||
|
|
||||||
|
|
||||||
|
def project_onto_segment(a: Coordinate, b: Coordinate, p: Coordinate) -> tuple[Coordinate, float, float]:
|
||||||
|
"""Project point p onto line segment ab (snap to road).
|
||||||
|
|
||||||
|
Returns:
|
||||||
|
(projected_point, t, distance) where:
|
||||||
|
- projected_point: the closest point on segment ab to p
|
||||||
|
- t: parameter 0-1 indicating position along segment (0=a, 1=b)
|
||||||
|
- distance: distance from p to the projected point
|
||||||
|
"""
|
||||||
|
seg_len = a.distance_to(b)
|
||||||
|
if seg_len < 0.01:
|
||||||
|
return a, 0.0, a.distance_to(p)
|
||||||
|
|
||||||
|
ap = p - a
|
||||||
|
ab = b - a
|
||||||
|
t = max(0.0, min(1.0, ap.dot(ab) / ab.dot(ab)))
|
||||||
|
projection = a + ab * t
|
||||||
|
return projection, t, projection.distance_to(p)
|
||||||
|
|
||||||
|
|
||||||
|
def distance_along_geometry(geometry: list[Coordinate], pos: Coordinate) -> float:
|
||||||
|
"""Calculate distance traveled along geometry from start to closest point to pos."""
|
||||||
|
if len(geometry) <= 2:
|
||||||
|
return geometry[0].distance_to(pos)
|
||||||
|
|
||||||
|
total_distance = 0.0
|
||||||
|
total_distance_closest = 0.0
|
||||||
|
closest_distance = 1e9
|
||||||
|
|
||||||
|
for i in range(len(geometry) - 1):
|
||||||
|
d = minimum_distance(geometry[i], geometry[i + 1], pos)
|
||||||
|
|
||||||
|
if d < closest_distance:
|
||||||
|
closest_distance = d
|
||||||
|
total_distance_closest = total_distance + geometry[i].distance_to(pos)
|
||||||
|
|
||||||
|
total_distance += geometry[i].distance_to(geometry[i + 1])
|
||||||
|
|
||||||
|
return total_distance_closest
|
||||||
|
|
||||||
|
|
||||||
|
def normalize_angle(angle: float) -> float:
|
||||||
|
"""Normalize angle to -180 to 180 degrees."""
|
||||||
|
while angle > 180:
|
||||||
|
angle -= 360
|
||||||
|
while angle < -180:
|
||||||
|
angle += 360
|
||||||
|
return angle
|
||||||
|
|
||||||
|
|
||||||
|
def calculate_turn_angle(
|
||||||
|
current_geometry: list[Coordinate],
|
||||||
|
next_geometry: list[Coordinate],
|
||||||
|
samples: int = 3
|
||||||
|
) -> float:
|
||||||
|
"""
|
||||||
|
Calculate turn angle between two road segments.
|
||||||
|
|
||||||
|
Uses the bearing of the end of current geometry vs the bearing
|
||||||
|
of the start of next geometry to determine turn angle.
|
||||||
|
|
||||||
|
Args:
|
||||||
|
current_geometry: Coordinates of current road segment (before turn)
|
||||||
|
next_geometry: Coordinates of next road segment (after turn)
|
||||||
|
samples: Number of points to use for bearing calculation (for stability)
|
||||||
|
|
||||||
|
Returns:
|
||||||
|
Turn angle in degrees. Positive = left turn, negative = right turn.
|
||||||
|
Example: 90 = 90° left turn, -90 = 90° right turn
|
||||||
|
"""
|
||||||
|
if len(current_geometry) < 2 or len(next_geometry) < 2:
|
||||||
|
return 0.0
|
||||||
|
|
||||||
|
# Get bearing of current road (last segment)
|
||||||
|
# Use last few points for stability
|
||||||
|
start_idx = max(0, len(current_geometry) - samples)
|
||||||
|
current_bearing = current_geometry[start_idx].bearing_to(current_geometry[-1])
|
||||||
|
|
||||||
|
# Get bearing of next road (first segment)
|
||||||
|
# Use first few points for stability
|
||||||
|
end_idx = min(samples, len(next_geometry) - 1)
|
||||||
|
next_bearing = next_geometry[0].bearing_to(next_geometry[end_idx])
|
||||||
|
|
||||||
|
# Calculate turn angle (positive = left, negative = right)
|
||||||
|
# This matches openpilot convention where left is positive curvature
|
||||||
|
angle = normalize_angle(current_bearing - next_bearing)
|
||||||
|
|
||||||
|
return angle
|
||||||
|
|
||||||
|
|
||||||
|
def coordinate_from_param(key: str, params: Params = None) -> Coordinate | None:
|
||||||
|
"""Read coordinate from params.
|
||||||
|
|
||||||
|
Handles both JSON type params (returns dict) and STRING type params (returns string).
|
||||||
|
"""
|
||||||
|
if params is None:
|
||||||
|
params = Params()
|
||||||
|
|
||||||
|
try:
|
||||||
|
value = params.get(key)
|
||||||
|
if value is None:
|
||||||
|
return None
|
||||||
|
|
||||||
|
# JSON type params return dict directly, STRING type needs json.loads()
|
||||||
|
if isinstance(value, str):
|
||||||
|
pos = json.loads(value)
|
||||||
|
else:
|
||||||
|
pos = value
|
||||||
|
|
||||||
|
if 'latitude' not in pos or 'longitude' not in pos:
|
||||||
|
return None
|
||||||
|
|
||||||
|
return Coordinate(pos['latitude'], pos['longitude'])
|
||||||
|
except (json.JSONDecodeError, KeyError, TypeError):
|
||||||
|
return None
|
||||||
|
|
||||||
|
|
||||||
|
def find_closest_point_on_route(pos: Coordinate, route_coords: list[Coordinate]) -> tuple[int, float]:
|
||||||
|
"""Find closest point index and distance on route."""
|
||||||
|
if not route_coords:
|
||||||
|
return 0, float('inf')
|
||||||
|
|
||||||
|
min_dist = float('inf')
|
||||||
|
closest_idx = 0
|
||||||
|
|
||||||
|
for i in range(len(route_coords) - 1):
|
||||||
|
# Check distance to segment, not just point
|
||||||
|
d = minimum_distance(route_coords[i], route_coords[i + 1], pos)
|
||||||
|
if d < min_dist:
|
||||||
|
min_dist = d
|
||||||
|
closest_idx = i
|
||||||
|
|
||||||
|
return closest_idx, min_dist
|
||||||
|
|
||||||
|
|
||||||
|
def calculate_remaining_distance(route_coords: list[Coordinate], from_idx: int) -> float:
|
||||||
|
"""Calculate remaining distance along route from index."""
|
||||||
|
if from_idx >= len(route_coords) - 1:
|
||||||
|
return 0.0
|
||||||
|
|
||||||
|
total = 0.0
|
||||||
|
for i in range(from_idx, len(route_coords) - 1):
|
||||||
|
total += route_coords[i].distance_to(route_coords[i + 1])
|
||||||
|
return total
|
||||||
|
|
||||||
|
|
||||||
|
# --- Instruction Parsing ---
|
||||||
|
|
||||||
|
def string_to_direction(direction: str) -> str:
|
||||||
|
"""Convert direction string to standard format."""
|
||||||
|
for d in DIRECTIONS:
|
||||||
|
if d in direction:
|
||||||
|
if 'slight' in direction and d in MODIFIABLE_DIRECTIONS:
|
||||||
|
return 'slight' + d.capitalize()
|
||||||
|
return d
|
||||||
|
return 'none'
|
||||||
|
|
||||||
|
|
||||||
|
def maxspeed_to_ms(maxspeed: dict[str, str | float]) -> float:
|
||||||
|
"""Convert speed limit dict to m/s."""
|
||||||
|
unit = cast(str, maxspeed['unit'])
|
||||||
|
speed = cast(float, maxspeed['speed'])
|
||||||
|
return SPEED_CONVERSIONS.get(unit, 1.0) * speed
|
||||||
|
|
||||||
|
|
||||||
|
def field_valid(dat: dict, field: str) -> bool:
|
||||||
|
"""Check if field exists and is not None."""
|
||||||
|
return field in dat and dat[field] is not None
|
||||||
|
|
||||||
|
|
||||||
|
def parse_banner_instructions(banners: Any, distance_to_maneuver: float = 0.0) -> dict[str, Any] | None:
|
||||||
|
"""Parse Mapbox/OSRM banner instructions."""
|
||||||
|
if not banners or not len(banners):
|
||||||
|
return None
|
||||||
|
|
||||||
|
instruction = {}
|
||||||
|
|
||||||
|
# A segment can contain multiple banners, find one that we need to show now
|
||||||
|
current_banner = banners[0]
|
||||||
|
for banner in banners:
|
||||||
|
if distance_to_maneuver < banner.get('distanceAlongGeometry', 0):
|
||||||
|
current_banner = banner
|
||||||
|
|
||||||
|
# Only show banner when close enough to maneuver
|
||||||
|
instruction['showFull'] = distance_to_maneuver < current_banner.get('distanceAlongGeometry', 0)
|
||||||
|
|
||||||
|
# Primary
|
||||||
|
p = current_banner.get('primary', {})
|
||||||
|
if field_valid(p, 'text'):
|
||||||
|
instruction['maneuverPrimaryText'] = p['text']
|
||||||
|
if field_valid(p, 'type'):
|
||||||
|
instruction['maneuverType'] = p['type']
|
||||||
|
if field_valid(p, 'modifier'):
|
||||||
|
instruction['maneuverModifier'] = p['modifier']
|
||||||
|
|
||||||
|
# Secondary
|
||||||
|
if field_valid(current_banner, 'secondary'):
|
||||||
|
instruction['maneuverSecondaryText'] = current_banner['secondary']['text']
|
||||||
|
|
||||||
|
# Lane lines
|
||||||
|
if field_valid(current_banner, 'sub'):
|
||||||
|
lanes = []
|
||||||
|
for component in current_banner['sub'].get('components', []):
|
||||||
|
if component.get('type') != 'lane':
|
||||||
|
continue
|
||||||
|
|
||||||
|
lane = {
|
||||||
|
'active': component.get('active', False),
|
||||||
|
'directions': [string_to_direction(d) for d in component.get('directions', [])],
|
||||||
|
}
|
||||||
|
|
||||||
|
if field_valid(component, 'active_direction'):
|
||||||
|
lane['activeDirection'] = string_to_direction(component['active_direction'])
|
||||||
|
|
||||||
|
lanes.append(lane)
|
||||||
|
instruction['lanes'] = lanes
|
||||||
|
|
||||||
|
return instruction
|
||||||
|
|
||||||
|
|
||||||
|
def parse_osrm_step(step: dict) -> dict[str, Any]:
|
||||||
|
"""Parse OSRM route step into instruction format."""
|
||||||
|
maneuver = step.get('maneuver', {})
|
||||||
|
instruction = {
|
||||||
|
'distance': step.get('distance', 0),
|
||||||
|
'duration': step.get('duration', 0),
|
||||||
|
'name': step.get('name', ''),
|
||||||
|
'maneuverType': maneuver.get('type', ''),
|
||||||
|
'maneuverModifier': maneuver.get('modifier', ''),
|
||||||
|
'location': maneuver.get('location', []), # [lon, lat]
|
||||||
|
}
|
||||||
|
return instruction
|
||||||
|
|
||||||
|
|
||||||
|
def classify_maneuver(maneuver_type: str, maneuver_modifier: str) -> str:
|
||||||
|
"""
|
||||||
|
Classify OSRM maneuver as 'turn' or 'laneChange'.
|
||||||
|
|
||||||
|
Highway exits/forks use laneChange desires (smoother).
|
||||||
|
Intersection turns use turn desires (sharper).
|
||||||
|
|
||||||
|
OSRM maneuver types:
|
||||||
|
- turn: regular intersection turn
|
||||||
|
- fork: highway split/junction
|
||||||
|
- off ramp: highway exit
|
||||||
|
- on ramp: highway entrance
|
||||||
|
- merge: merging lanes
|
||||||
|
- roundabout turn: roundabout exit
|
||||||
|
- exit roundabout: leaving roundabout
|
||||||
|
- continue: straight (no maneuver)
|
||||||
|
- depart/arrive: start/end
|
||||||
|
|
||||||
|
Returns:
|
||||||
|
'turn' for intersection turns
|
||||||
|
'laneChange' for highway exits/forks
|
||||||
|
'none' for straight/no maneuver
|
||||||
|
"""
|
||||||
|
maneuver_type = maneuver_type.lower()
|
||||||
|
maneuver_modifier = maneuver_modifier.lower()
|
||||||
|
|
||||||
|
# Highway exits and forks -> lane change desire
|
||||||
|
LANE_CHANGE_TYPES = {
|
||||||
|
'fork', # highway fork/split
|
||||||
|
'off ramp', # highway exit
|
||||||
|
'on ramp', # highway entrance
|
||||||
|
'merge', # merging
|
||||||
|
'exit rotary', # leaving rotary
|
||||||
|
'exit roundabout',# leaving roundabout
|
||||||
|
}
|
||||||
|
|
||||||
|
# Intersection turns -> turn desire
|
||||||
|
TURN_TYPES = {
|
||||||
|
'turn', # regular turn
|
||||||
|
'end of road', # forced turn at end of road
|
||||||
|
'rotary', # entering rotary
|
||||||
|
'roundabout', # entering roundabout
|
||||||
|
'roundabout turn',# turn within roundabout
|
||||||
|
}
|
||||||
|
|
||||||
|
# No maneuver
|
||||||
|
NO_MANEUVER_TYPES = {
|
||||||
|
'continue',
|
||||||
|
'depart',
|
||||||
|
'arrive',
|
||||||
|
'new name',
|
||||||
|
'notification',
|
||||||
|
}
|
||||||
|
|
||||||
|
if maneuver_type in LANE_CHANGE_TYPES:
|
||||||
|
return 'laneChange'
|
||||||
|
|
||||||
|
if maneuver_type in TURN_TYPES:
|
||||||
|
# For turns, check modifier - slight turns at highway speeds might be lane changes
|
||||||
|
if 'slight' in maneuver_modifier:
|
||||||
|
# Slight turns could be either - default to turn but could be lane change
|
||||||
|
# CarrotPilot uses additional context like road speed limit
|
||||||
|
return 'turn'
|
||||||
|
return 'turn'
|
||||||
|
|
||||||
|
if maneuver_type in NO_MANEUVER_TYPES:
|
||||||
|
return 'none'
|
||||||
|
|
||||||
|
# Unknown type - default to turn
|
||||||
|
return 'turn'
|
||||||
|
|
||||||
|
|
||||||
|
def get_turn_direction(maneuver_modifier: str) -> str:
|
||||||
|
"""
|
||||||
|
Get turn direction from OSRM maneuver modifier.
|
||||||
|
|
||||||
|
Returns:
|
||||||
|
'left', 'right', or 'none'
|
||||||
|
"""
|
||||||
|
modifier = maneuver_modifier.lower()
|
||||||
|
if 'left' in modifier:
|
||||||
|
return 'left'
|
||||||
|
if 'right' in modifier:
|
||||||
|
return 'right'
|
||||||
|
return 'none'
|
||||||
|
|
||||||
|
|
||||||
|
# --- Curvature Computation ---
|
||||||
|
|
||||||
|
def compute_path_curvature(pos: Coordinate, bearing: float, route_coords: list[Coordinate],
|
||||||
|
closest_idx: int, v_ego: float, lookahead_time: float = 2.5) -> float:
|
||||||
|
"""
|
||||||
|
Compute desired curvature from route geometry using pure pursuit.
|
||||||
|
|
||||||
|
Args:
|
||||||
|
pos: Current position
|
||||||
|
bearing: Current heading in degrees
|
||||||
|
route_coords: List of route coordinates
|
||||||
|
closest_idx: Index of closest point on route
|
||||||
|
v_ego: Current vehicle speed m/s
|
||||||
|
lookahead_time: How far ahead to look in seconds
|
||||||
|
|
||||||
|
Returns:
|
||||||
|
Desired curvature in 1/m (positive = left turn, negative = right turn)
|
||||||
|
"""
|
||||||
|
if not route_coords or closest_idx >= len(route_coords) - 1:
|
||||||
|
return 0.0
|
||||||
|
|
||||||
|
# Calculate lookahead distance (min 30m, based on speed)
|
||||||
|
lookahead_dist = max(v_ego * lookahead_time, 30.0)
|
||||||
|
|
||||||
|
# Find lookahead point along route
|
||||||
|
dist_traveled = 0.0
|
||||||
|
lookahead_idx = closest_idx
|
||||||
|
|
||||||
|
for i in range(closest_idx, len(route_coords) - 1):
|
||||||
|
segment_dist = route_coords[i].distance_to(route_coords[i + 1])
|
||||||
|
if dist_traveled + segment_dist >= lookahead_dist:
|
||||||
|
# Interpolate within segment
|
||||||
|
remaining = lookahead_dist - dist_traveled
|
||||||
|
ratio = remaining / segment_dist if segment_dist > 0 else 0
|
||||||
|
lookahead_idx = i
|
||||||
|
# Could interpolate here, but using next point is simpler
|
||||||
|
if ratio > 0.5:
|
||||||
|
lookahead_idx = i + 1
|
||||||
|
break
|
||||||
|
dist_traveled += segment_dist
|
||||||
|
lookahead_idx = i + 1
|
||||||
|
|
||||||
|
if lookahead_idx >= len(route_coords):
|
||||||
|
lookahead_idx = len(route_coords) - 1
|
||||||
|
|
||||||
|
lookahead_point = route_coords[lookahead_idx]
|
||||||
|
|
||||||
|
# Calculate desired heading to lookahead point
|
||||||
|
desired_bearing = pos.bearing_to(lookahead_point)
|
||||||
|
|
||||||
|
# Calculate heading error (normalized to -180 to 180)
|
||||||
|
heading_error = desired_bearing - bearing
|
||||||
|
if heading_error > 180:
|
||||||
|
heading_error -= 360
|
||||||
|
elif heading_error < -180:
|
||||||
|
heading_error += 360
|
||||||
|
|
||||||
|
# Convert heading error to yaw (radians)
|
||||||
|
yaw_error = math.radians(heading_error)
|
||||||
|
|
||||||
|
# Distance to lookahead point
|
||||||
|
dist_to_lookahead = pos.distance_to(lookahead_point)
|
||||||
|
if dist_to_lookahead < 1.0:
|
||||||
|
return 0.0
|
||||||
|
|
||||||
|
# Pure pursuit curvature: 2 * sin(yaw_error) / lookahead_distance
|
||||||
|
# Note: Negative because heading_error > 0 means target is to the RIGHT (clockwise),
|
||||||
|
# and right turn should produce negative curvature
|
||||||
|
curvature = -2.0 * math.sin(yaw_error) / dist_to_lookahead
|
||||||
|
|
||||||
|
# Clamp to reasonable values (max ~7m radius turn)
|
||||||
|
MAX_CURVATURE = 0.15
|
||||||
|
return max(-MAX_CURVATURE, min(MAX_CURVATURE, curvature))
|
||||||
|
|
||||||
|
|
||||||
|
def smooth_curvature(new_curv: float, prev_curv: float, alpha: float = 0.3) -> float:
|
||||||
|
"""Exponential smoothing for curvature."""
|
||||||
|
return alpha * new_curv + (1 - alpha) * prev_curv
|
||||||
|
|
||||||
|
|
||||||
|
def curvature_to_radius(curvature: float) -> float:
|
||||||
|
"""Convert curvature to turn radius in meters."""
|
||||||
|
if abs(curvature) < 0.001:
|
||||||
|
return float('inf')
|
||||||
|
return 1.0 / abs(curvature)
|
||||||
|
|
||||||
|
|
||||||
|
def compute_turn_angle_at_index(route_coords: list[Coordinate], turn_idx: int,
|
||||||
|
sample_dist: float = 20.0) -> float:
|
||||||
|
"""
|
||||||
|
Compute turn angle at a specific point on the route.
|
||||||
|
Uses points sample_dist meters before and after for stability.
|
||||||
|
Returns angle in degrees (positive = left, negative = right).
|
||||||
|
"""
|
||||||
|
if turn_idx < 1 or turn_idx >= len(route_coords) - 1:
|
||||||
|
return 0.0
|
||||||
|
|
||||||
|
# Find points ~sample_dist before and after the turn for stable bearing
|
||||||
|
before_idx = turn_idx
|
||||||
|
after_idx = turn_idx
|
||||||
|
|
||||||
|
# Walk backwards to find before point
|
||||||
|
dist = 0.0
|
||||||
|
for i in range(turn_idx, 0, -1):
|
||||||
|
dist += route_coords[i].distance_to(route_coords[i - 1])
|
||||||
|
if dist >= sample_dist:
|
||||||
|
before_idx = i - 1
|
||||||
|
break
|
||||||
|
else:
|
||||||
|
before_idx = 0
|
||||||
|
|
||||||
|
# Walk forwards to find after point
|
||||||
|
dist = 0.0
|
||||||
|
for i in range(turn_idx, len(route_coords) - 1):
|
||||||
|
dist += route_coords[i].distance_to(route_coords[i + 1])
|
||||||
|
if dist >= sample_dist:
|
||||||
|
after_idx = i + 1
|
||||||
|
break
|
||||||
|
else:
|
||||||
|
after_idx = len(route_coords) - 1
|
||||||
|
|
||||||
|
if before_idx == turn_idx or after_idx == turn_idx:
|
||||||
|
return 0.0
|
||||||
|
|
||||||
|
# Calculate bearings using the sampled points
|
||||||
|
p1 = route_coords[before_idx]
|
||||||
|
p2 = route_coords[turn_idx]
|
||||||
|
p3 = route_coords[after_idx]
|
||||||
|
|
||||||
|
bearing1 = p1.bearing_to(p2)
|
||||||
|
bearing2 = p2.bearing_to(p3)
|
||||||
|
|
||||||
|
# Angle difference (positive = left, negative = right)
|
||||||
|
angle = bearing1 - bearing2
|
||||||
|
|
||||||
|
# Normalize to -180 to 180
|
||||||
|
while angle > 180:
|
||||||
|
angle -= 360
|
||||||
|
while angle < -180:
|
||||||
|
angle += 360
|
||||||
|
|
||||||
|
return angle
|
||||||
|
|
||||||
|
|
||||||
|
def compute_turn_curvature_at_index(route_coords: list[Coordinate], turn_idx: int,
|
||||||
|
sample_dist: float = 15.0) -> float:
|
||||||
|
"""
|
||||||
|
Compute curvature at turn point using three-point circle fitting.
|
||||||
|
Returns curvature in 1/m (positive = left, negative = right).
|
||||||
|
"""
|
||||||
|
if turn_idx < 1 or turn_idx >= len(route_coords) - 1:
|
||||||
|
return 0.0
|
||||||
|
|
||||||
|
# Find points sample_dist before and after
|
||||||
|
before_idx = turn_idx
|
||||||
|
after_idx = turn_idx
|
||||||
|
|
||||||
|
dist = 0.0
|
||||||
|
for i in range(turn_idx, 0, -1):
|
||||||
|
dist += route_coords[i].distance_to(route_coords[i - 1])
|
||||||
|
if dist >= sample_dist:
|
||||||
|
before_idx = i - 1
|
||||||
|
break
|
||||||
|
|
||||||
|
dist = 0.0
|
||||||
|
for i in range(turn_idx, len(route_coords) - 1):
|
||||||
|
dist += route_coords[i].distance_to(route_coords[i + 1])
|
||||||
|
if dist >= sample_dist:
|
||||||
|
after_idx = i + 1
|
||||||
|
break
|
||||||
|
|
||||||
|
if before_idx == turn_idx or after_idx == turn_idx:
|
||||||
|
return 0.0
|
||||||
|
|
||||||
|
# Three points for circle fitting
|
||||||
|
p1 = route_coords[before_idx]
|
||||||
|
p2 = route_coords[turn_idx]
|
||||||
|
p3 = route_coords[after_idx]
|
||||||
|
|
||||||
|
# Convert to local meters (approximate)
|
||||||
|
lat_center = p2.latitude
|
||||||
|
lon_scale = math.cos(math.radians(lat_center))
|
||||||
|
m_per_deg = 111319.5 # meters per degree latitude
|
||||||
|
|
||||||
|
x1 = (p1.longitude - p2.longitude) * lon_scale * m_per_deg
|
||||||
|
y1 = (p1.latitude - p2.latitude) * m_per_deg
|
||||||
|
x2 = 0.0
|
||||||
|
y2 = 0.0
|
||||||
|
x3 = (p3.longitude - p2.longitude) * lon_scale * m_per_deg
|
||||||
|
y3 = (p3.latitude - p2.latitude) * m_per_deg
|
||||||
|
|
||||||
|
# Calculate curvature using cross product / (product of distances)
|
||||||
|
d12 = math.sqrt((x2 - x1)**2 + (y2 - y1)**2)
|
||||||
|
d13 = math.sqrt((x3 - x1)**2 + (y3 - y1)**2)
|
||||||
|
d23 = math.sqrt((x3 - x2)**2 + (y3 - y2)**2)
|
||||||
|
|
||||||
|
if d12 < 0.1 or d13 < 0.1 or d23 < 0.1:
|
||||||
|
return 0.0
|
||||||
|
|
||||||
|
# Cross product (p2-p1) x (p3-p1) - z component only
|
||||||
|
cross = (x2 - x1) * (y3 - y1) - (y2 - y1) * (x3 - x1)
|
||||||
|
|
||||||
|
curvature = 2.0 * cross / (d12 * d13 * d23)
|
||||||
|
|
||||||
|
# Clamp to reasonable values
|
||||||
|
MAX_CURVATURE = 0.2 # ~5m radius
|
||||||
|
return max(-MAX_CURVATURE, min(MAX_CURVATURE, curvature))
|
||||||
@@ -0,0 +1,47 @@
|
|||||||
|
"""
|
||||||
|
Copyright (c) 2026, Rick Lan
|
||||||
|
|
||||||
|
Permission is hereby granted, free of charge, to any person obtaining a copy
|
||||||
|
of this software and associated documentation files (the "Software"), to deal
|
||||||
|
in the Software without restriction, including without limitation the rights
|
||||||
|
to use, copy, modify, merge, publish, distribute, and/or sublicense,
|
||||||
|
for non-commercial purposes only, subject to the following conditions:
|
||||||
|
|
||||||
|
- The above copyright notice and this permission notice shall be included in
|
||||||
|
all copies or substantial portions of the Software.
|
||||||
|
- Commercial use (e.g. use in a product, service, or activity intended to
|
||||||
|
generate revenue) is prohibited without explicit written permission from
|
||||||
|
the copyright holder.
|
||||||
|
|
||||||
|
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED,
|
||||||
|
INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A
|
||||||
|
PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
|
||||||
|
HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
|
||||||
|
OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
|
||||||
|
SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
|
||||||
|
|
||||||
|
dragonpilot Map-Aware Assist Library
|
||||||
|
|
||||||
|
Core modules:
|
||||||
|
- maa_desire: Simple turn desire logic (blinker confirmation, lane change blocking, RHD/LHD)
|
||||||
|
- model_helper: Helper class for modeld integration
|
||||||
|
- longitudinal_helper: Planner integration for speed/accel limiting
|
||||||
|
"""
|
||||||
|
|
||||||
|
from dragonpilot.dashy.maa.lib.maa_desire import (
|
||||||
|
should_block_lane_change,
|
||||||
|
get_turn_desire,
|
||||||
|
is_crossing_turn,
|
||||||
|
get_turn_trigger_distance,
|
||||||
|
)
|
||||||
|
from dragonpilot.dashy.maa.lib.model_helper import ModelHelper
|
||||||
|
from dragonpilot.dashy.maa.lib.longitudinal_helper import LongitudinalHelper
|
||||||
|
|
||||||
|
__all__ = [
|
||||||
|
'should_block_lane_change',
|
||||||
|
'get_turn_desire',
|
||||||
|
'is_crossing_turn',
|
||||||
|
'get_turn_trigger_distance',
|
||||||
|
'ModelHelper',
|
||||||
|
'LongitudinalHelper',
|
||||||
|
]
|
||||||
@@ -0,0 +1,238 @@
|
|||||||
|
#!/usr/bin/env python3
|
||||||
|
"""
|
||||||
|
Copyright (c) 2026, Rick Lan
|
||||||
|
|
||||||
|
Permission is hereby granted, free of charge, to any person obtaining a copy
|
||||||
|
of this software and associated documentation files (the "Software"), to deal
|
||||||
|
in the Software without restriction, including without limitation the rights
|
||||||
|
to use, copy, modify, merge, publish, distribute, and/or sublicense,
|
||||||
|
for non-commercial purposes only, subject to the following conditions:
|
||||||
|
|
||||||
|
- The above copyright notice and this permission notice shall be included in
|
||||||
|
all copies or substantial portions of the Software.
|
||||||
|
- Commercial use (e.g. use in a product, service, or activity intended to
|
||||||
|
generate revenue) is prohibited without explicit written permission from
|
||||||
|
the copyright holder.
|
||||||
|
|
||||||
|
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED,
|
||||||
|
INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A
|
||||||
|
PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
|
||||||
|
HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
|
||||||
|
OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
|
||||||
|
SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
|
||||||
|
|
||||||
|
Acceleration Limiter
|
||||||
|
|
||||||
|
Physics-based acceleration limiting for turns.
|
||||||
|
|
||||||
|
Algorithm adapted from CarrotPilot (https://github.com/ajouatom/openpilot)
|
||||||
|
Credit: carrotpilot team for the physics-based lateral acceleration approach.
|
||||||
|
|
||||||
|
The key insight is that total acceleration is limited by tire grip:
|
||||||
|
a_total² = a_x² + a_y² ≤ a_max²
|
||||||
|
|
||||||
|
Where:
|
||||||
|
a_x = longitudinal acceleration (throttle/brake)
|
||||||
|
a_y = lateral acceleration (from turning) = v² × curvature
|
||||||
|
|
||||||
|
This means during turns, we must reduce longitudinal acceleration
|
||||||
|
to stay within the grip circle.
|
||||||
|
"""
|
||||||
|
|
||||||
|
import math
|
||||||
|
from dataclasses import dataclass
|
||||||
|
from typing import Tuple, Optional
|
||||||
|
|
||||||
|
|
||||||
|
@dataclass
|
||||||
|
class AccelLimits:
|
||||||
|
"""Acceleration limits."""
|
||||||
|
min_accel: float # m/s² (negative = braking)
|
||||||
|
max_accel: float # m/s² (positive = acceleration)
|
||||||
|
|
||||||
|
|
||||||
|
@dataclass
|
||||||
|
class AccelLimiterState:
|
||||||
|
"""Current state of acceleration limiter."""
|
||||||
|
v_ego: float = 0.0
|
||||||
|
curvature: float = 0.0
|
||||||
|
lateral_accel: float = 0.0
|
||||||
|
available_long_accel: float = 0.0
|
||||||
|
is_limiting: bool = False
|
||||||
|
|
||||||
|
|
||||||
|
class AccelLimiter:
|
||||||
|
"""
|
||||||
|
Physics-based acceleration limiter for turns.
|
||||||
|
|
||||||
|
Uses the friction circle concept: total acceleration magnitude
|
||||||
|
is limited by tire grip. During turns, lateral acceleration
|
||||||
|
consumes part of this budget, leaving less for longitudinal
|
||||||
|
acceleration.
|
||||||
|
|
||||||
|
Adapted from CarrotPilot's limit_accel_in_turns function.
|
||||||
|
"""
|
||||||
|
|
||||||
|
# Default lateral acceleration limit (m/s²)
|
||||||
|
# Comfortable limit is ~2-3 m/s², sporty is ~4 m/s²
|
||||||
|
DEFAULT_LAT_ACCEL_MAX = 2.5
|
||||||
|
|
||||||
|
# Lookup table for max total acceleration vs speed
|
||||||
|
# Higher speeds = lower max lateral accel for comfort
|
||||||
|
A_TOTAL_MAX_BP = [0., 10., 20., 30., 40.] # m/s
|
||||||
|
A_TOTAL_MAX_V = [3.0, 2.8, 2.5, 2.2, 2.0] # m/s²
|
||||||
|
|
||||||
|
def __init__(
|
||||||
|
self,
|
||||||
|
lat_accel_max: float = DEFAULT_LAT_ACCEL_MAX,
|
||||||
|
comfort_mode: bool = True
|
||||||
|
):
|
||||||
|
"""
|
||||||
|
Initialize acceleration limiter.
|
||||||
|
|
||||||
|
Args:
|
||||||
|
lat_accel_max: Maximum allowed lateral acceleration (m/s²)
|
||||||
|
comfort_mode: If True, use speed-dependent limits
|
||||||
|
"""
|
||||||
|
self.lat_accel_max = lat_accel_max
|
||||||
|
self.comfort_mode = comfort_mode
|
||||||
|
self.state = AccelLimiterState()
|
||||||
|
|
||||||
|
def get_max_total_accel(self, v_ego: float) -> float:
|
||||||
|
"""
|
||||||
|
Get maximum total acceleration for current speed.
|
||||||
|
|
||||||
|
In comfort mode, reduces limit at higher speeds.
|
||||||
|
"""
|
||||||
|
if not self.comfort_mode:
|
||||||
|
return self.lat_accel_max
|
||||||
|
|
||||||
|
# Interpolate from lookup table
|
||||||
|
import numpy as np
|
||||||
|
return np.interp(v_ego, self.A_TOTAL_MAX_BP, self.A_TOTAL_MAX_V)
|
||||||
|
|
||||||
|
def compute_lateral_accel(self, v_ego: float, curvature: float) -> float:
|
||||||
|
"""
|
||||||
|
Compute lateral acceleration from speed and curvature.
|
||||||
|
|
||||||
|
a_y = v² × κ
|
||||||
|
|
||||||
|
Args:
|
||||||
|
v_ego: Vehicle speed in m/s
|
||||||
|
curvature: Road curvature in 1/m
|
||||||
|
|
||||||
|
Returns:
|
||||||
|
Lateral acceleration in m/s²
|
||||||
|
"""
|
||||||
|
return v_ego * v_ego * abs(curvature)
|
||||||
|
|
||||||
|
def compute_available_long_accel(
|
||||||
|
self,
|
||||||
|
v_ego: float,
|
||||||
|
curvature: float,
|
||||||
|
a_max: Optional[float] = None
|
||||||
|
) -> float:
|
||||||
|
"""
|
||||||
|
Compute available longitudinal acceleration given current turn.
|
||||||
|
|
||||||
|
Uses friction circle: a_x² + a_y² ≤ a_max²
|
||||||
|
Solving for a_x: a_x = sqrt(a_max² - a_y²)
|
||||||
|
|
||||||
|
Adapted from CarrotPilot's limit_accel_in_turns.
|
||||||
|
|
||||||
|
Args:
|
||||||
|
v_ego: Vehicle speed in m/s
|
||||||
|
curvature: Road curvature in 1/m
|
||||||
|
a_max: Override for max total acceleration
|
||||||
|
|
||||||
|
Returns:
|
||||||
|
Maximum available longitudinal acceleration in m/s²
|
||||||
|
"""
|
||||||
|
if a_max is None:
|
||||||
|
a_max = self.get_max_total_accel(v_ego)
|
||||||
|
|
||||||
|
# Compute lateral acceleration
|
||||||
|
a_y = self.compute_lateral_accel(v_ego, curvature)
|
||||||
|
a_y_abs = abs(a_y)
|
||||||
|
|
||||||
|
# Update state
|
||||||
|
self.state.v_ego = v_ego
|
||||||
|
self.state.curvature = curvature
|
||||||
|
self.state.lateral_accel = a_y
|
||||||
|
|
||||||
|
# Check if lateral accel exceeds limit
|
||||||
|
if a_y_abs >= a_max:
|
||||||
|
# Already at or over limit - no longitudinal accel available
|
||||||
|
self.state.available_long_accel = 0.0
|
||||||
|
self.state.is_limiting = True
|
||||||
|
return 0.0
|
||||||
|
|
||||||
|
# Compute remaining budget for longitudinal acceleration
|
||||||
|
a_x_available = math.sqrt(a_max * a_max - a_y_abs * a_y_abs)
|
||||||
|
|
||||||
|
self.state.available_long_accel = a_x_available
|
||||||
|
self.state.is_limiting = a_x_available < a_max * 0.9 # Limiting if < 90% available
|
||||||
|
|
||||||
|
return a_x_available
|
||||||
|
|
||||||
|
def limit_accel(
|
||||||
|
self,
|
||||||
|
v_ego: float,
|
||||||
|
curvature: float,
|
||||||
|
accel_limits: AccelLimits,
|
||||||
|
a_max: Optional[float] = None
|
||||||
|
) -> AccelLimits:
|
||||||
|
"""
|
||||||
|
Apply turn-based acceleration limiting.
|
||||||
|
|
||||||
|
Args:
|
||||||
|
v_ego: Vehicle speed in m/s
|
||||||
|
curvature: Road curvature in 1/m
|
||||||
|
accel_limits: Current acceleration limits
|
||||||
|
a_max: Override for max total acceleration
|
||||||
|
|
||||||
|
Returns:
|
||||||
|
New AccelLimits with turn limiting applied
|
||||||
|
"""
|
||||||
|
a_x_available = self.compute_available_long_accel(v_ego, curvature, a_max)
|
||||||
|
|
||||||
|
# Clamp max acceleration to available budget
|
||||||
|
new_max = min(accel_limits.max_accel, a_x_available)
|
||||||
|
|
||||||
|
# Don't limit braking as much - we may need to slow down
|
||||||
|
# But still apply some limit for comfort
|
||||||
|
new_min = max(accel_limits.min_accel, -a_x_available * 1.5)
|
||||||
|
|
||||||
|
return AccelLimits(
|
||||||
|
min_accel=new_min,
|
||||||
|
max_accel=new_max
|
||||||
|
)
|
||||||
|
|
||||||
|
def limit_accel_tuple(
|
||||||
|
self,
|
||||||
|
v_ego: float,
|
||||||
|
curvature: float,
|
||||||
|
accel_limits: Tuple[float, float],
|
||||||
|
a_max: Optional[float] = None
|
||||||
|
) -> Tuple[float, float]:
|
||||||
|
"""
|
||||||
|
Apply turn-based acceleration limiting (tuple interface).
|
||||||
|
|
||||||
|
For compatibility with existing planner code.
|
||||||
|
|
||||||
|
Args:
|
||||||
|
v_ego: Vehicle speed in m/s
|
||||||
|
curvature: Road curvature in 1/m
|
||||||
|
accel_limits: (min_accel, max_accel) tuple
|
||||||
|
a_max: Override for max total acceleration
|
||||||
|
|
||||||
|
Returns:
|
||||||
|
(min_accel, max_accel) tuple with turn limiting applied
|
||||||
|
"""
|
||||||
|
limits = self.limit_accel(
|
||||||
|
v_ego,
|
||||||
|
curvature,
|
||||||
|
AccelLimits(accel_limits[0], accel_limits[1]),
|
||||||
|
a_max
|
||||||
|
)
|
||||||
|
return (limits.min_accel, limits.max_accel)
|
||||||
@@ -0,0 +1,360 @@
|
|||||||
|
#!/usr/bin/env python3
|
||||||
|
"""
|
||||||
|
Copyright (c) 2026, Rick Lan
|
||||||
|
|
||||||
|
Permission is hereby granted, free of charge, to any person obtaining a copy
|
||||||
|
of this software and associated documentation files (the "Software"), to deal
|
||||||
|
in the Software without restriction, including without limitation the rights
|
||||||
|
to use, copy, modify, merge, publish, distribute, and/or sublicense,
|
||||||
|
for non-commercial purposes only, subject to the following conditions:
|
||||||
|
|
||||||
|
- The above copyright notice and this permission notice shall be included in
|
||||||
|
all copies or substantial portions of the Software.
|
||||||
|
- Commercial use (e.g. use in a product, service, or activity intended to
|
||||||
|
generate revenue) is prohibited without explicit written permission from
|
||||||
|
the copyright holder.
|
||||||
|
|
||||||
|
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED,
|
||||||
|
INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A
|
||||||
|
PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
|
||||||
|
HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
|
||||||
|
OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
|
||||||
|
SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
|
||||||
|
|
||||||
|
Map-Aware Assist Longitudinal Helper
|
||||||
|
|
||||||
|
Provides navigation-based speed and acceleration limiting for the longitudinal planner.
|
||||||
|
This module keeps all nav-related planner logic isolated from the core planner code.
|
||||||
|
|
||||||
|
Features:
|
||||||
|
- Turn speed limiting based on maaControl cereal message
|
||||||
|
- Physics-based acceleration limiting using friction circle
|
||||||
|
- Smooth slowdown/resume transitions
|
||||||
|
- Driver acknowledgment (blinker) required to activate speed limiting
|
||||||
|
|
||||||
|
Adapted from CarrotPilot (https://github.com/ajouatom/openpilot)
|
||||||
|
Credit: carrotpilot team for curvature-based speed and physics-based accel limiting.
|
||||||
|
|
||||||
|
Usage in longitudinal_planner.py:
|
||||||
|
from dragonpilot.dashy.maa.lib.longitudinal_helper import LongitudinalHelper
|
||||||
|
|
||||||
|
# Add 'maaControl' to SubMaster
|
||||||
|
# In update:
|
||||||
|
maa = sm['maaControl']
|
||||||
|
if sm.valid['maaControl'] and maa.turnValid and maa.speedLimitActive:
|
||||||
|
# Only apply speed limit when driver acknowledged (blinker on)
|
||||||
|
turn_speed = maa.turnSpeedLimit
|
||||||
|
turn_distance = maa.turnDistance
|
||||||
|
else:
|
||||||
|
turn_speed, turn_distance = None, None
|
||||||
|
v_cruise = self.maa_helper.apply_nav_speed_limit(v_cruise, turn_speed, turn_distance)
|
||||||
|
"""
|
||||||
|
|
||||||
|
from dataclasses import dataclass
|
||||||
|
from typing import Optional, List
|
||||||
|
|
||||||
|
# Import nav modules
|
||||||
|
try:
|
||||||
|
from dragonpilot.dashy.maa.lib.accel_limiter import AccelLimiter
|
||||||
|
ACCEL_LIMITER_AVAILABLE = True
|
||||||
|
except ImportError:
|
||||||
|
ACCEL_LIMITER_AVAILABLE = False
|
||||||
|
|
||||||
|
|
||||||
|
@dataclass
|
||||||
|
class NavPlannerState:
|
||||||
|
"""Current state of nav planner integration."""
|
||||||
|
nav_limited_speed: Optional[float] = None
|
||||||
|
is_limiting_speed: bool = False
|
||||||
|
is_limiting_accel: bool = False
|
||||||
|
|
||||||
|
|
||||||
|
@dataclass
|
||||||
|
class VirtualLead:
|
||||||
|
"""Virtual lead car for turn deceleration."""
|
||||||
|
status: bool = False
|
||||||
|
dRel: float = 100.0 # distance to virtual lead
|
||||||
|
vLead: float = 0.0 # speed of virtual lead
|
||||||
|
aLeadK: float = 0.0 # acceleration
|
||||||
|
aLeadTau: float = 0.3
|
||||||
|
|
||||||
|
|
||||||
|
class RadarStateWrapper:
|
||||||
|
"""
|
||||||
|
Wrapper to inject virtual lead into radarState for MPC.
|
||||||
|
|
||||||
|
This allows the MPC to "see" a fake slow car at the turn point,
|
||||||
|
triggering natural deceleration using the well-tuned lead following logic.
|
||||||
|
"""
|
||||||
|
def __init__(self, radar_state, virtual_lead=None):
|
||||||
|
self._radar_state = radar_state
|
||||||
|
self._virtual_lead = virtual_lead
|
||||||
|
|
||||||
|
@property
|
||||||
|
def leadOne(self):
|
||||||
|
real_lead = self._radar_state.leadOne
|
||||||
|
# If no virtual lead, use real lead
|
||||||
|
if self._virtual_lead is None or not self._virtual_lead.status:
|
||||||
|
return real_lead
|
||||||
|
# If real lead exists and is closer, use real lead
|
||||||
|
if real_lead.status and real_lead.dRel < self._virtual_lead.dRel:
|
||||||
|
return real_lead
|
||||||
|
# Use virtual lead (turn point)
|
||||||
|
return self._virtual_lead
|
||||||
|
|
||||||
|
@property
|
||||||
|
def leadTwo(self):
|
||||||
|
return self._radar_state.leadTwo
|
||||||
|
|
||||||
|
def __getattr__(self, name):
|
||||||
|
return getattr(self._radar_state, name)
|
||||||
|
|
||||||
|
|
||||||
|
class LongitudinalHelper:
|
||||||
|
"""
|
||||||
|
Navigation helper for longitudinal planner.
|
||||||
|
|
||||||
|
Provides turn speed limiting and physics-based acceleration limiting
|
||||||
|
based on navigation data. Keeps all nav logic isolated from core planner.
|
||||||
|
|
||||||
|
Design rationale (based on automotive research):
|
||||||
|
- Stopping distance at 50 km/h is ~48m (25m braking + 15m reaction)
|
||||||
|
- Intersection approach speeds typically 30-50 km/h
|
||||||
|
- Comfortable deceleration: 1.5-2.0 m/s² for normal driving
|
||||||
|
"""
|
||||||
|
|
||||||
|
# Configuration - Speed limiting
|
||||||
|
# Physics-based: calculate braking distance from speed difference
|
||||||
|
# More natural driving: maintain speed, brake closer to turn
|
||||||
|
SLOWDOWN_END_DIST = 10.0 # meters - be at turn speed by this distance (buffer before turn)
|
||||||
|
SLOWDOWN_BUFFER = 15.0 # meters - extra buffer added to braking distance
|
||||||
|
|
||||||
|
# Physics-based accel limiting
|
||||||
|
ACCEL_LIMIT_ENABLED = True
|
||||||
|
LAT_ACCEL_MAX = 2.5 # m/s² - max comfortable lateral acceleration
|
||||||
|
|
||||||
|
# Comfortable deceleration target (m/s²)
|
||||||
|
# Research shows 1.5-2.0 m/s² is comfortable for passengers
|
||||||
|
# Using 2.0 for more natural, later braking
|
||||||
|
COMFORT_DECEL = 2.0
|
||||||
|
|
||||||
|
def __init__(self):
|
||||||
|
"""Initialize nav planner helper."""
|
||||||
|
self.state = NavPlannerState()
|
||||||
|
|
||||||
|
# Physics-based acceleration limiter
|
||||||
|
if ACCEL_LIMITER_AVAILABLE and self.ACCEL_LIMIT_ENABLED:
|
||||||
|
self.accel_limiter = AccelLimiter(
|
||||||
|
lat_accel_max=self.LAT_ACCEL_MAX,
|
||||||
|
comfort_mode=True
|
||||||
|
)
|
||||||
|
else:
|
||||||
|
self.accel_limiter = None
|
||||||
|
|
||||||
|
def apply_nav_speed_limit(
|
||||||
|
self,
|
||||||
|
v_cruise: float,
|
||||||
|
turn_speed: Optional[float] = None,
|
||||||
|
distance: Optional[float] = None
|
||||||
|
) -> float:
|
||||||
|
"""
|
||||||
|
Apply navigation-based speed limiting with physics-based braking.
|
||||||
|
|
||||||
|
Uses kinematic equation: d = (v² - v_target²) / (2 * decel)
|
||||||
|
Starts braking at calculated distance + buffer for natural driving feel.
|
||||||
|
|
||||||
|
Args:
|
||||||
|
v_cruise: Current cruise speed in m/s
|
||||||
|
turn_speed: Turn speed limit from maaControl (None if invalid)
|
||||||
|
distance: Distance to turn from maaControl (None if invalid)
|
||||||
|
|
||||||
|
Returns:
|
||||||
|
Limited cruise speed
|
||||||
|
"""
|
||||||
|
if turn_speed is None or distance is None:
|
||||||
|
self.state.nav_limited_speed = None
|
||||||
|
self.state.is_limiting_speed = False
|
||||||
|
return v_cruise
|
||||||
|
|
||||||
|
# No need to slow if already at or below turn speed
|
||||||
|
if v_cruise <= turn_speed:
|
||||||
|
self.state.nav_limited_speed = None
|
||||||
|
self.state.is_limiting_speed = False
|
||||||
|
return v_cruise
|
||||||
|
|
||||||
|
# Calculate required braking distance using kinematics
|
||||||
|
# d = (v² - v_target²) / (2 * decel)
|
||||||
|
speed_diff_sq = v_cruise ** 2 - turn_speed ** 2
|
||||||
|
braking_distance = speed_diff_sq / (2 * self.COMFORT_DECEL)
|
||||||
|
|
||||||
|
# Start braking at: braking_distance + buffer + end_distance
|
||||||
|
slowdown_start = braking_distance + self.SLOWDOWN_BUFFER + self.SLOWDOWN_END_DIST
|
||||||
|
|
||||||
|
# Outside slowdown zone - no limit
|
||||||
|
if distance > slowdown_start:
|
||||||
|
self.state.nav_limited_speed = None
|
||||||
|
self.state.is_limiting_speed = False
|
||||||
|
return v_cruise
|
||||||
|
|
||||||
|
# In slowdown zone - calculate target speed at this distance
|
||||||
|
# v² = v_target² + 2 * decel * (distance - end_dist)
|
||||||
|
if distance > self.SLOWDOWN_END_DIST:
|
||||||
|
remaining = distance - self.SLOWDOWN_END_DIST
|
||||||
|
# Target speed that allows comfortable braking to turn_speed
|
||||||
|
target_speed_sq = turn_speed ** 2 + 2 * self.COMFORT_DECEL * remaining
|
||||||
|
limited_speed = min(v_cruise, target_speed_sq ** 0.5)
|
||||||
|
else:
|
||||||
|
# Very close to turn - calculate achievable speed with comfort decel
|
||||||
|
# This handles late blinker: never brake harder than COMFORT_DECEL
|
||||||
|
# If we can't reach turn_speed in time, accept entering turn faster
|
||||||
|
achievable_speed_sq = turn_speed ** 2 + 2 * self.COMFORT_DECEL * max(0, distance)
|
||||||
|
achievable_speed = achievable_speed_sq ** 0.5
|
||||||
|
# Never target lower than achievable (prevents harsh braking)
|
||||||
|
limited_speed = min(v_cruise, max(turn_speed, achievable_speed))
|
||||||
|
|
||||||
|
self.state.nav_limited_speed = limited_speed
|
||||||
|
self.state.is_limiting_speed = True
|
||||||
|
return limited_speed
|
||||||
|
|
||||||
|
# Minimum distance for virtual lead - below this, don't use virtual lead
|
||||||
|
# to avoid MPC braking to stop
|
||||||
|
VIRTUAL_LEAD_MIN_DIST = 15.0 # meters
|
||||||
|
|
||||||
|
def get_virtual_lead(
|
||||||
|
self,
|
||||||
|
v_ego: float,
|
||||||
|
turn_speed: Optional[float] = None,
|
||||||
|
distance: Optional[float] = None
|
||||||
|
) -> Optional[VirtualLead]:
|
||||||
|
"""
|
||||||
|
Create a virtual lead car at the turn point for natural deceleration.
|
||||||
|
|
||||||
|
The virtual lead "drives" at turn_speed, positioned at the turn point.
|
||||||
|
This makes the MPC decelerate naturally as if following a slow car.
|
||||||
|
|
||||||
|
Args:
|
||||||
|
v_ego: Current vehicle speed in m/s
|
||||||
|
turn_speed: Turn speed limit from maaControl (None if invalid)
|
||||||
|
distance: Distance to turn from maaControl (None if invalid)
|
||||||
|
|
||||||
|
Returns:
|
||||||
|
VirtualLead object if turn is approaching, None otherwise
|
||||||
|
"""
|
||||||
|
if turn_speed is None or distance is None:
|
||||||
|
return None
|
||||||
|
|
||||||
|
# Only create virtual lead when we need to slow down
|
||||||
|
if v_ego <= turn_speed:
|
||||||
|
return None
|
||||||
|
|
||||||
|
# Don't use virtual lead when very close to turn - avoids brake-to-stop
|
||||||
|
# At this point, we should already be at turn speed from earlier braking
|
||||||
|
if distance < self.VIRTUAL_LEAD_MIN_DIST:
|
||||||
|
return None
|
||||||
|
|
||||||
|
# Calculate when to start showing virtual lead
|
||||||
|
# Use same physics: braking distance + buffer
|
||||||
|
speed_diff_sq = v_ego ** 2 - turn_speed ** 2
|
||||||
|
braking_distance = speed_diff_sq / (2 * self.COMFORT_DECEL)
|
||||||
|
activation_distance = braking_distance + self.SLOWDOWN_BUFFER + self.SLOWDOWN_END_DIST
|
||||||
|
|
||||||
|
if distance > activation_distance:
|
||||||
|
return None
|
||||||
|
|
||||||
|
# Create virtual lead at turn point, moving at turn speed
|
||||||
|
# The MPC will naturally decelerate to follow it
|
||||||
|
return VirtualLead(
|
||||||
|
status=True,
|
||||||
|
dRel=distance, # distance to virtual lead
|
||||||
|
vLead=turn_speed, # virtual lead moves at turn speed
|
||||||
|
aLeadK=0.0, # no acceleration (constant speed)
|
||||||
|
aLeadTau=0.3 # response time constant
|
||||||
|
)
|
||||||
|
|
||||||
|
def apply_nav_accel_limit(
|
||||||
|
self,
|
||||||
|
v_ego: float,
|
||||||
|
curvature: float,
|
||||||
|
accel_clip: List[float]
|
||||||
|
) -> List[float]:
|
||||||
|
"""
|
||||||
|
Apply physics-based acceleration limiting for turns.
|
||||||
|
|
||||||
|
Uses friction circle: a_x² + a_y² ≤ a_max²
|
||||||
|
|
||||||
|
Args:
|
||||||
|
v_ego: Current vehicle speed in m/s
|
||||||
|
curvature: Current road curvature (from nav or model)
|
||||||
|
accel_clip: Current [min_accel, max_accel] limits
|
||||||
|
|
||||||
|
Returns:
|
||||||
|
Updated [min_accel, max_accel] with turn limiting applied
|
||||||
|
"""
|
||||||
|
if self.accel_limiter is None:
|
||||||
|
return accel_clip
|
||||||
|
|
||||||
|
if abs(curvature) < 0.001: # Essentially straight
|
||||||
|
self.state.is_limiting_accel = False
|
||||||
|
return accel_clip
|
||||||
|
|
||||||
|
limited = list(self.accel_limiter.limit_accel_tuple(
|
||||||
|
v_ego, curvature, tuple(accel_clip)
|
||||||
|
))
|
||||||
|
|
||||||
|
self.state.is_limiting_accel = self.accel_limiter.state.is_limiting
|
||||||
|
return limited
|
||||||
|
|
||||||
|
# Staleness threshold for maaControl message (nanoseconds)
|
||||||
|
STALE_THRESHOLD_NS = 5e8 # 0.5 seconds
|
||||||
|
|
||||||
|
def process(
|
||||||
|
self,
|
||||||
|
sm,
|
||||||
|
v_ego: float,
|
||||||
|
v_cruise: float,
|
||||||
|
accel_clip: List[float]
|
||||||
|
) -> tuple:
|
||||||
|
"""
|
||||||
|
Process maaControl and return updated planner values.
|
||||||
|
|
||||||
|
Encapsulates all MAA logic:
|
||||||
|
- Validity and staleness checking
|
||||||
|
- Speed limiting (when driver acknowledged via blinker)
|
||||||
|
- Virtual lead creation for natural deceleration
|
||||||
|
- Curvature-based acceleration limiting
|
||||||
|
|
||||||
|
Args:
|
||||||
|
sm: SubMaster with 'maaControl' and 'carState'
|
||||||
|
v_ego: Current vehicle speed in m/s
|
||||||
|
v_cruise: Current cruise speed in m/s
|
||||||
|
accel_clip: Current [min_accel, max_accel] limits
|
||||||
|
|
||||||
|
Returns:
|
||||||
|
tuple: (v_cruise, accel_clip, virtual_lead)
|
||||||
|
"""
|
||||||
|
virtual_lead = None
|
||||||
|
|
||||||
|
maa = sm['maaControl']
|
||||||
|
|
||||||
|
# Check valid and not stale
|
||||||
|
maa_valid = sm.valid['maaControl'] and maa.turnValid
|
||||||
|
if maa_valid and (sm.logMonoTime['carState'] - sm.logMonoTime['maaControl']) > self.STALE_THRESHOLD_NS:
|
||||||
|
maa_valid = False
|
||||||
|
|
||||||
|
if not maa_valid:
|
||||||
|
self.state.nav_limited_speed = None
|
||||||
|
self.state.is_limiting_speed = False
|
||||||
|
self.state.is_limiting_accel = False
|
||||||
|
return v_cruise, accel_clip, virtual_lead
|
||||||
|
|
||||||
|
# Speed limiting only when driver acknowledged (blinker on)
|
||||||
|
# Without blinker: informational only, no speed reduction
|
||||||
|
if maa.speedLimitActive:
|
||||||
|
virtual_lead = self.get_virtual_lead(v_ego, maa.turnSpeedLimit, maa.turnDistance)
|
||||||
|
v_cruise = self.apply_nav_speed_limit(v_cruise, maa.turnSpeedLimit, maa.turnDistance)
|
||||||
|
|
||||||
|
# Curvature-based acceleration limiting (always active when valid)
|
||||||
|
if maa.curvatureValid and abs(maa.curvature) > 0.001:
|
||||||
|
accel_clip = self.apply_nav_accel_limit(v_ego, maa.curvature, accel_clip)
|
||||||
|
|
||||||
|
return v_cruise, accel_clip, virtual_lead
|
||||||
@@ -0,0 +1,119 @@
|
|||||||
|
#!/usr/bin/env python3
|
||||||
|
"""
|
||||||
|
Copyright (c) 2026, Rick Lan
|
||||||
|
|
||||||
|
Permission is hereby granted, free of charge, to any person obtaining a copy
|
||||||
|
of this software and associated documentation files (the "Software"), to deal
|
||||||
|
in the Software without restriction, including without limitation the rights
|
||||||
|
to use, copy, modify, merge, publish, distribute, and/or sublicense,
|
||||||
|
for non-commercial purposes only, subject to the following conditions:
|
||||||
|
|
||||||
|
- The above copyright notice and this permission notice shall be included in
|
||||||
|
all copies or substantial portions of the Software.
|
||||||
|
- Commercial use (e.g. use in a product, service, or activity intended to
|
||||||
|
generate revenue) is prohibited without explicit written permission from
|
||||||
|
the copyright holder.
|
||||||
|
|
||||||
|
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED,
|
||||||
|
INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A
|
||||||
|
PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
|
||||||
|
HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
|
||||||
|
OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
|
||||||
|
SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
|
||||||
|
|
||||||
|
MAA Desire Helper
|
||||||
|
|
||||||
|
Turn execution flow:
|
||||||
|
1. APPROACHING (200m): Start dead reckoning, show turn suggestion
|
||||||
|
2. Driver turns on blinker: acknowledged
|
||||||
|
3. At 100m with blinker: COMMIT - block lane change, slow down, send desire
|
||||||
|
4. EXECUTING (30m): Track heading change
|
||||||
|
5. COMPLETE: When heading change ≈ expected turn angle
|
||||||
|
|
||||||
|
Turn desire is sent when:
|
||||||
|
- desireActive is true (from maa_controld)
|
||||||
|
- This requires: driver acknowledged (blinker) + committed (<100m) + EXECUTING state
|
||||||
|
|
||||||
|
Without blinker:
|
||||||
|
- System shows turn info but doesn't intervene
|
||||||
|
- No speed reduction, no desire sent
|
||||||
|
- Driver maintains full control
|
||||||
|
"""
|
||||||
|
|
||||||
|
from cereal import log, custom
|
||||||
|
|
||||||
|
TurnDirection = custom.MaaControl.TurnDirection
|
||||||
|
ManeuverType = custom.MaaControl.ManeuverType
|
||||||
|
TurnState = {
|
||||||
|
'NONE': 0,
|
||||||
|
'APPROACHING': 1,
|
||||||
|
'EXECUTING': 2,
|
||||||
|
'COMPLETE': 3,
|
||||||
|
'MISSED': 4,
|
||||||
|
}
|
||||||
|
|
||||||
|
# Configuration
|
||||||
|
MAX_TURN_SPEED = 50.0 / 3.6 # m/s (50 km/h) - no turn assist above this
|
||||||
|
SHARP_TURN_ANGLE = 45.0 # degrees - above: turnLeft/Right, below: laneChangeLeft/Right
|
||||||
|
|
||||||
|
|
||||||
|
def should_block_lane_change(maa_control, v_ego: float) -> bool:
|
||||||
|
"""
|
||||||
|
Check if lane change should be blocked due to approaching turn.
|
||||||
|
|
||||||
|
Uses blockLaneChange from maa_controld which is set when:
|
||||||
|
- Driver acknowledged (blinker on matching direction)
|
||||||
|
- Within commit distance (100m)
|
||||||
|
"""
|
||||||
|
if maa_control is None:
|
||||||
|
return False
|
||||||
|
if v_ego > MAX_TURN_SPEED:
|
||||||
|
return False
|
||||||
|
# Use the pre-computed blockLaneChange from maa_controld
|
||||||
|
return maa_control.blockLaneChange
|
||||||
|
|
||||||
|
|
||||||
|
def get_turn_desire(maa_control, carstate, is_rhd: bool = False) -> log.Desire:
|
||||||
|
"""
|
||||||
|
Get turn desire based on maaControl.
|
||||||
|
|
||||||
|
desireActive from maa_controld is true when:
|
||||||
|
- Driver acknowledged (blinker matches turn direction)
|
||||||
|
- Committed (<100m) OR in EXECUTING state
|
||||||
|
|
||||||
|
This function adds additional checks:
|
||||||
|
- Speed < 50 km/h
|
||||||
|
- maneuverType == turn
|
||||||
|
|
||||||
|
Args:
|
||||||
|
maa_control: MaaControl message
|
||||||
|
carstate: CarState message
|
||||||
|
is_rhd: True if right-hand drive (UK/Japan), False for left-hand drive (US/Taiwan)
|
||||||
|
|
||||||
|
Returns:
|
||||||
|
- turnLeft/Right if angle >= 45°
|
||||||
|
- laneChangeLeft/Right if angle < 45°
|
||||||
|
- none if conditions not met
|
||||||
|
"""
|
||||||
|
if maa_control is None or not maa_control.turnValid:
|
||||||
|
return log.Desire.none
|
||||||
|
|
||||||
|
if maa_control.maneuverType != ManeuverType.turn:
|
||||||
|
return log.Desire.none
|
||||||
|
|
||||||
|
if carstate.vEgo > MAX_TURN_SPEED:
|
||||||
|
return log.Desire.none
|
||||||
|
|
||||||
|
# desireActive encapsulates: acknowledged + (committed OR executing)
|
||||||
|
if not maa_control.desireActive:
|
||||||
|
return log.Desire.none
|
||||||
|
|
||||||
|
# Sharp turn (>= 45°) uses turnLeft/Right, gentle uses laneChange
|
||||||
|
is_sharp = abs(maa_control.turnAngle) >= SHARP_TURN_ANGLE
|
||||||
|
|
||||||
|
if maa_control.turnDirection == TurnDirection.left:
|
||||||
|
return log.Desire.turnLeft if is_sharp else log.Desire.laneChangeLeft
|
||||||
|
elif maa_control.turnDirection == TurnDirection.right:
|
||||||
|
return log.Desire.turnRight if is_sharp else log.Desire.laneChangeRight
|
||||||
|
|
||||||
|
return log.Desire.none
|
||||||
@@ -0,0 +1,118 @@
|
|||||||
|
#!/usr/bin/env python3
|
||||||
|
"""
|
||||||
|
Copyright (c) 2026, Rick Lan
|
||||||
|
|
||||||
|
Permission is hereby granted, free of charge, to any person obtaining a copy
|
||||||
|
of this software and associated documentation files (the "Software"), to deal
|
||||||
|
in the Software without restriction, including without limitation the rights
|
||||||
|
to use, copy, modify, merge, publish, distribute, and/or sublicense,
|
||||||
|
for non-commercial purposes only, subject to the following conditions:
|
||||||
|
|
||||||
|
- The above copyright notice and this permission notice shall be included in
|
||||||
|
all copies or substantial portions of the Software.
|
||||||
|
- Commercial use (e.g. use in a product, service, or activity intended to
|
||||||
|
generate revenue) is prohibited without explicit written permission from
|
||||||
|
the copyright holder.
|
||||||
|
|
||||||
|
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED,
|
||||||
|
INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A
|
||||||
|
PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
|
||||||
|
HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
|
||||||
|
OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
|
||||||
|
SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
|
||||||
|
|
||||||
|
Model Helper for modeld integration.
|
||||||
|
|
||||||
|
Provides turn desire logic for modeld.py.
|
||||||
|
"""
|
||||||
|
|
||||||
|
from cereal import log
|
||||||
|
from dragonpilot.dashy.maa.lib.maa_desire import (
|
||||||
|
should_block_lane_change,
|
||||||
|
get_turn_desire,
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
|
class ModelHelper:
|
||||||
|
"""
|
||||||
|
Helper class for MAA integration with modeld.
|
||||||
|
|
||||||
|
Usage in modeld.py:
|
||||||
|
from dragonpilot.dashy.maa.lib import ModelHelper
|
||||||
|
model_helper = ModelHelper()
|
||||||
|
|
||||||
|
# In the loop:
|
||||||
|
is_rhd = sm["driverMonitoringState"].isRHD
|
||||||
|
desire = model_helper.get_desire(sm, lane_change_desire, is_rhd)
|
||||||
|
"""
|
||||||
|
|
||||||
|
def __init__(self):
|
||||||
|
self.active = False
|
||||||
|
self.last_desire = log.Desire.none
|
||||||
|
|
||||||
|
def get_desire(self, sm, lane_change_desire: int, is_rhd: bool = False) -> int:
|
||||||
|
"""
|
||||||
|
Get combined desire from MAA turn logic and lane change.
|
||||||
|
|
||||||
|
Priority:
|
||||||
|
1. Active lane change (driver initiated) - don't interrupt
|
||||||
|
2. MAA turn desire (navigation turn)
|
||||||
|
3. Lane change desire (pre-lane-change, keep, etc.)
|
||||||
|
|
||||||
|
Args:
|
||||||
|
sm: SubMaster with maaControl and carState
|
||||||
|
lane_change_desire: Desire from DesireHelper (lane change logic)
|
||||||
|
is_rhd: True if right-hand drive
|
||||||
|
|
||||||
|
Returns:
|
||||||
|
Final desire value
|
||||||
|
"""
|
||||||
|
# Check if MAA data is available
|
||||||
|
if not sm.valid.get('maaControl', False) or not sm.valid.get('carState', False):
|
||||||
|
return lane_change_desire
|
||||||
|
|
||||||
|
# Check for stale maaControl (wait for 0.5s)
|
||||||
|
if (sm.logMonoTime['carState'] - sm.logMonoTime['maaControl']) > 5e8:
|
||||||
|
return lane_change_desire
|
||||||
|
|
||||||
|
maa_control = sm['maaControl']
|
||||||
|
carstate = sm['carState']
|
||||||
|
|
||||||
|
# Don't interrupt active lane change
|
||||||
|
if lane_change_desire in (log.Desire.laneChangeLeft, log.Desire.laneChangeRight):
|
||||||
|
self.active = False
|
||||||
|
return lane_change_desire
|
||||||
|
|
||||||
|
# Check if lane change should be blocked due to approaching turn
|
||||||
|
if should_block_lane_change(maa_control, carstate.vEgo):
|
||||||
|
# Block lane change desires, but allow none/keep
|
||||||
|
if lane_change_desire in (log.Desire.laneChangeLeft, log.Desire.laneChangeRight):
|
||||||
|
lane_change_desire = log.Desire.none
|
||||||
|
|
||||||
|
# Get MAA turn desire
|
||||||
|
maa_desire = get_turn_desire(maa_control, carstate, is_rhd)
|
||||||
|
|
||||||
|
# MAA turn desire takes priority over none/keep
|
||||||
|
if maa_desire != log.Desire.none:
|
||||||
|
self.active = True
|
||||||
|
self.last_desire = maa_desire
|
||||||
|
return maa_desire
|
||||||
|
|
||||||
|
# If MAA was active but now returns none, we completed the turn
|
||||||
|
if self.active and maa_desire == log.Desire.none:
|
||||||
|
self.active = False
|
||||||
|
|
||||||
|
return lane_change_desire
|
||||||
|
|
||||||
|
def update(self, modelv2, desire_state):
|
||||||
|
"""
|
||||||
|
Update helper with model output (for future use).
|
||||||
|
|
||||||
|
Can be used to detect turn completion via desireState probabilities.
|
||||||
|
|
||||||
|
Args:
|
||||||
|
modelv2: ModelV2 message
|
||||||
|
desire_state: Model's desire state output
|
||||||
|
"""
|
||||||
|
# Reserved for future use - detecting turn completion via model output
|
||||||
|
pass
|
||||||
@@ -0,0 +1,919 @@
|
|||||||
|
#!/usr/bin/env python3
|
||||||
|
"""
|
||||||
|
Copyright (c) 2026, Rick Lan
|
||||||
|
|
||||||
|
Permission is hereby granted, free of charge, to any person obtaining a copy
|
||||||
|
of this software and associated documentation files (the "Software"), to deal
|
||||||
|
in the Software without restriction, including without limitation the rights
|
||||||
|
to use, copy, modify, merge, publish, distribute, and/or sublicense,
|
||||||
|
for non-commercial purposes only, subject to the following conditions:
|
||||||
|
|
||||||
|
- The above copyright notice and this permission notice shall be included in
|
||||||
|
all copies or substantial portions of the Software.
|
||||||
|
- Commercial use (e.g. use in a product, service, or activity intended to
|
||||||
|
generate revenue) is prohibited without explicit written permission from
|
||||||
|
the copyright holder.
|
||||||
|
|
||||||
|
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED,
|
||||||
|
INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A
|
||||||
|
PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
|
||||||
|
HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
|
||||||
|
OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
|
||||||
|
SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
|
||||||
|
|
||||||
|
MAA Control Daemon - Turn Assist with Dead Reckoning
|
||||||
|
|
||||||
|
================================================================================
|
||||||
|
OVERVIEW
|
||||||
|
================================================================================
|
||||||
|
|
||||||
|
This daemon provides high-frequency (20Hz) turn assist signals for navigation.
|
||||||
|
The key insight is that navInstruction updates at only 1Hz (too slow for smooth
|
||||||
|
turn tracking), so we use "snapshot and coast" - capture turn info once, then
|
||||||
|
dead reckon using the car's own sensors.
|
||||||
|
|
||||||
|
Subscribes to:
|
||||||
|
- navInstruction: turn info from maad (1Hz) - used as TRIGGER only
|
||||||
|
- liveGPS: position, bearing (1Hz)
|
||||||
|
- navRoute: route geometry - reset on route change
|
||||||
|
- carState: vEgo, blinker, yawRate (100Hz) - for dead reckoning
|
||||||
|
|
||||||
|
Publishes:
|
||||||
|
- maaControl: turnDistance, turnDirection, desireActive, turnState, etc.
|
||||||
|
|
||||||
|
================================================================================
|
||||||
|
DEAD RECKONING APPROACH
|
||||||
|
================================================================================
|
||||||
|
|
||||||
|
Problem:
|
||||||
|
Navigation updates come in slow (1Hz). At 60 km/h, that's 17m between updates.
|
||||||
|
This causes jerky distance countdown and imprecise turn detection.
|
||||||
|
|
||||||
|
Solution: "Snapshot and Coast"
|
||||||
|
1. TRIGGER at ~200m: capture turn params (angle, distance, direction)
|
||||||
|
2. IGNORE subsequent navInstruction updates for this turn
|
||||||
|
3. DEAD RECKON distance: integrate vEgo from carState (100Hz)
|
||||||
|
4. TRACK HEADING: integrate yawRate from carState during turn execution
|
||||||
|
|
||||||
|
Distance remaining = initial_distance - ∫(vEgo × dt)
|
||||||
|
Heading change = ∫(yawRate × dt)
|
||||||
|
|
||||||
|
================================================================================
|
||||||
|
STATE MACHINE
|
||||||
|
================================================================================
|
||||||
|
|
||||||
|
NONE ──────────────────────────────────────────────────────────────┐
|
||||||
|
│ │
|
||||||
|
│ navInstruction shows turn within 200m │
|
||||||
|
│ (significant angle ≥20°, has direction) │
|
||||||
|
▼ │
|
||||||
|
APPROACHING ───────────────────────────────────────────────────────┤
|
||||||
|
│ • Show turn suggestion at <150m │
|
||||||
|
│ • Wait for driver blinker (acknowledgment) │
|
||||||
|
│ • Dead reckon distance using vEgo │
|
||||||
|
│ • NOT tracking yaw yet (allows overtaking) │
|
||||||
|
│ │
|
||||||
|
│ At <100m WITH blinker: COMMIT │
|
||||||
|
│ • blockLaneChange = true │
|
||||||
|
│ • speedLimitActive = true │
|
||||||
|
│ • desireActive = true │
|
||||||
|
│ │
|
||||||
|
│ Dead reckoned distance < 30m │
|
||||||
|
▼ │
|
||||||
|
EXECUTING ─────────────────────────────────────────────────────────┤
|
||||||
|
│ • Now tracking yaw (accumulated heading change) │
|
||||||
|
│ • Compare accumulated vs expected turn angle │
|
||||||
|
│ • desireActive = true (only if acknowledged) │
|
||||||
|
│ │
|
||||||
|
│ |accumulated_yaw| >= |expected_angle| - tolerance │
|
||||||
|
▼ │
|
||||||
|
COMPLETE ──────► (2s cooldown) ──────► NONE │
|
||||||
|
│
|
||||||
|
MISSED ◄─── (any abort condition) ◄────────────────────────────────┘
|
||||||
|
│
|
||||||
|
│ Wait for route change (navRoute coords change)
|
||||||
|
▼
|
||||||
|
NONE
|
||||||
|
|
||||||
|
================================================================================
|
||||||
|
DRIVER ACKNOWLEDGMENT (Two-Step, Like Lane Change)
|
||||||
|
================================================================================
|
||||||
|
|
||||||
|
Two confirmation steps:
|
||||||
|
1. Blinker matching turn direction = approach confirmed (speed limit, block lane change)
|
||||||
|
2. Steering torque in turn direction = turn execution confirmed (desire sent)
|
||||||
|
|
||||||
|
Blinker is the main confirmation for slowing down. Steering is required to
|
||||||
|
actually send the turn desire to the model (like lane change).
|
||||||
|
|
||||||
|
Distance | Without Blinker | With Blinker | + Steering
|
||||||
|
-----------|--------------------------|----------------------------|------------------
|
||||||
|
200m-150m | Informational only | Informational only | (same)
|
||||||
|
150m-100m | Show suggestion | speedLimitActive = true | (same)
|
||||||
|
<100m | Show suggestion | slow down, block LC | + desireActive
|
||||||
|
<30m | Enter EXECUTING | slow down, block LC | + desireActive
|
||||||
|
|
||||||
|
Key behaviors:
|
||||||
|
- speedLimitActive: When blinker on, enforce turn speed limit
|
||||||
|
- blockLaneChange: At <100m with blinker, block lane change desire
|
||||||
|
- desireActive: Only sent when blinker AND steering confirmed (at turn)
|
||||||
|
|
||||||
|
If driver has blinker but doesn't steer:
|
||||||
|
- System slows down for the turn
|
||||||
|
- But doesn't send turn desire (driver steers manually)
|
||||||
|
- Once driver steers in turn direction, desire activates
|
||||||
|
|
||||||
|
================================================================================
|
||||||
|
ABORT/MISS DETECTION
|
||||||
|
================================================================================
|
||||||
|
|
||||||
|
Driver can override at any time. We detect "missed turn" via:
|
||||||
|
|
||||||
|
1. DROVE TOO FAR (no turn)
|
||||||
|
- Condition: distance_traveled > 2× initial_distance
|
||||||
|
- Meaning: drove way past expected turn point without turning
|
||||||
|
- Example: triggered at 200m, drove 400m, barely any yaw change
|
||||||
|
|
||||||
|
2. WRONG DIRECTION
|
||||||
|
- Condition: accumulated_yaw > 20° in opposite direction
|
||||||
|
- Meaning: driver turned the wrong way
|
||||||
|
- Example: expected right turn, driver turned 25° left
|
||||||
|
|
||||||
|
3. INSUFFICIENT TURN
|
||||||
|
- Condition: drove 2× total distance but <30% of expected yaw
|
||||||
|
- Meaning: drove through intersection without completing turn
|
||||||
|
- Example: expected 90° turn, only turned 20° after driving 500m
|
||||||
|
|
||||||
|
4. TIMEOUT
|
||||||
|
- Condition: 30 seconds of MOVING time (v_ego > 1 m/s)
|
||||||
|
- Meaning: something went wrong, taking too long
|
||||||
|
- Note: stopped time (traffic light) doesn't count
|
||||||
|
|
||||||
|
5. PASSED TURN (nav jumped to next)
|
||||||
|
- Condition: dead_reckon < 50m but nav says > 150m
|
||||||
|
- Meaning: nav is now showing NEXT turn, we passed this one
|
||||||
|
- Example: we think 30m to turn, nav says 400m (next turn)
|
||||||
|
|
||||||
|
6. DIRECTION CHANGED
|
||||||
|
- Condition: turnAngle sign flipped (left ↔ right)
|
||||||
|
- Meaning: route recalculated or nav corrected itself
|
||||||
|
- Example: was +90° (left), now -45° (right)
|
||||||
|
|
||||||
|
7. TURN DISAPPEARED
|
||||||
|
- Condition: turnAngle dropped below 20° threshold
|
||||||
|
- Meaning: no longer a significant turn (route changed or passed)
|
||||||
|
|
||||||
|
================================================================================
|
||||||
|
DRIVER OVERRIDE SCENARIOS
|
||||||
|
================================================================================
|
||||||
|
|
||||||
|
Overtaking another car:
|
||||||
|
- During APPROACHING: No problem! We only track distance, not yaw.
|
||||||
|
Driver can swerve left/right to overtake, doesn't affect tracking.
|
||||||
|
- During EXECUTING: Brief swerves (<20°) won't trigger wrong direction.
|
||||||
|
Only sustained opposite turn triggers abort.
|
||||||
|
|
||||||
|
Stopping at traffic light:
|
||||||
|
- moving_time only increments when v_ego > 1 m/s
|
||||||
|
- Can wait indefinitely at red light without timeout
|
||||||
|
- Distance tracking pauses when stopped (vEgo ≈ 0)
|
||||||
|
|
||||||
|
Taking a different route intentionally:
|
||||||
|
- System correctly detects as MISSED
|
||||||
|
- Waits for navRoute to change (reroute)
|
||||||
|
- Then ready to track new turn
|
||||||
|
|
||||||
|
================================================================================
|
||||||
|
DATA FLOW
|
||||||
|
================================================================================
|
||||||
|
|
||||||
|
navInstruction (1Hz) carState (100Hz)
|
||||||
|
│ │
|
||||||
|
│ turnAngle, maneuverDistance │ vEgo, yawRate, blinker
|
||||||
|
│ maneuverType, modifier │
|
||||||
|
▼ ▼
|
||||||
|
┌─────────────────────────────────────────────────────────┐
|
||||||
|
│ TurnTracker │
|
||||||
|
│ │
|
||||||
|
│ trigger() ◄── at 200m, capture params │
|
||||||
|
│ │ │
|
||||||
|
│ ▼ │
|
||||||
|
│ update(vEgo, yawRate) ◄── every 50ms (20Hz) │
|
||||||
|
│ │ │
|
||||||
|
│ ├── distance_traveled += vEgo × dt │
|
||||||
|
│ ├── accumulated_yaw += yawRate × dt (if EXECUTING)│
|
||||||
|
│ └── check abort conditions │
|
||||||
|
│ │
|
||||||
|
└─────────────────────────────────────────────────────────┘
|
||||||
|
│
|
||||||
|
▼
|
||||||
|
maaControl (20Hz)
|
||||||
|
• turnDistance (dead reckoned)
|
||||||
|
• turnState (NONE/APPROACHING/EXECUTING/COMPLETE/MISSED)
|
||||||
|
• desireActive (for blinker/lane change desire)
|
||||||
|
• turnProgress (accumulated yaw in degrees)
|
||||||
|
|
||||||
|
================================================================================
|
||||||
|
CONFIGURATION
|
||||||
|
================================================================================
|
||||||
|
|
||||||
|
TURN_TRIGGER_DISTANCE = 200m # Start dead reckoning
|
||||||
|
TURN_DESIRE_DISTANCE = 150m # Show turn suggestion, wait for blinker
|
||||||
|
TURN_COMMIT_DISTANCE = 100m # With blinker: commit (block lane change, slow)
|
||||||
|
TURN_EXECUTE_DISTANCE = 30m # Start tracking yaw
|
||||||
|
TURN_ANGLE_TOLERANCE = 15° # Turn complete within this
|
||||||
|
TURN_MIN_ANGLE = 20° # Minimum to be "significant"
|
||||||
|
TURN_TIMEOUT = 30s # Max moving time
|
||||||
|
|
||||||
|
================================================================================
|
||||||
|
OUTPUT FIELDS (maaControl)
|
||||||
|
================================================================================
|
||||||
|
|
||||||
|
turnDistance - Dead reckoned distance to turn (m)
|
||||||
|
turnDirection - left/right/none
|
||||||
|
turnAngle - Expected turn angle (deg, + = left)
|
||||||
|
turnState - 0=none, 1=approaching, 2=executing, 3=complete, 4=missed
|
||||||
|
turnProgress - Accumulated yaw during turn (deg)
|
||||||
|
desireActive - Send turn desire to model (blinker + steering confirmed)
|
||||||
|
driverAcknowledged - Driver turned on matching blinker
|
||||||
|
speedLimitActive - Enforce turn speed limit (blinker on)
|
||||||
|
blockLaneChange - Block lane change desire (blinker + committed)
|
||||||
|
turnSpeedLimit - Target speed for turn (m/s)
|
||||||
|
|
||||||
|
================================================================================
|
||||||
|
"""
|
||||||
|
|
||||||
|
import json
|
||||||
|
import math
|
||||||
|
import time
|
||||||
|
from enum import IntEnum
|
||||||
|
|
||||||
|
import numpy as np
|
||||||
|
from cereal import messaging, log, custom
|
||||||
|
from openpilot.common.params import Params
|
||||||
|
from openpilot.common.realtime import Ratekeeper
|
||||||
|
from openpilot.common.swaglog import cloudlog
|
||||||
|
|
||||||
|
from dragonpilot.dashy.maa.helpers import (
|
||||||
|
Coordinate,
|
||||||
|
compute_path_curvature,
|
||||||
|
find_closest_point_on_route,
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
|
# CarrotPilot curvature-to-speed lookup table
|
||||||
|
# Adapted from https://github.com/ajouatom/openpilot
|
||||||
|
# Maps curvature (1/m) to recommended speed (km/h)
|
||||||
|
# Based on physics: v = sqrt(a_lat / κ) where a_lat ≈ 2.5 m/s² for comfort
|
||||||
|
V_CURVE_LOOKUP_BP = [0., 1./800., 1./670., 1./560., 1./440., 1./360., 1./265., 1./190., 1./135., 1./85., 1./55., 1./30., 1./25.]
|
||||||
|
V_CURVE_LOOKUP_VALS = [300., 150., 120., 110., 100., 90., 80., 70., 60., 50., 40., 15., 5.] # km/h
|
||||||
|
|
||||||
|
|
||||||
|
# Configuration
|
||||||
|
CURVATURE_ASSIST_ENABLED = False # Disable continuous curvature steering assist
|
||||||
|
CURVATURE_LOOKAHEAD = 2.5 # seconds ahead for curvature calculation
|
||||||
|
TURN_VALID_DISTANCE = 500.0 # meters - turn is valid if within this distance
|
||||||
|
MIN_SPEED_FOR_CURVATURE = 1.0 # m/s - minimum speed to use curvature
|
||||||
|
|
||||||
|
# Turn execution - dead reckoning based
|
||||||
|
TURN_TRIGGER_DISTANCE = 200.0 # meters - capture turn info and start dead reckoning
|
||||||
|
TURN_DESIRE_DISTANCE = 150.0 # meters - show turn suggestion, wait for blinker
|
||||||
|
TURN_COMMIT_DISTANCE = 100.0 # meters - if blinker on, commit (block lane change, slow down)
|
||||||
|
TURN_EXECUTE_DISTANCE = 30.0 # meters - start tracking heading (entering intersection)
|
||||||
|
TURN_ANGLE_TOLERANCE = 15.0 # degrees - turn complete when within this of target
|
||||||
|
TURN_MIN_ANGLE = 20.0 # degrees - minimum angle to consider a "turn"
|
||||||
|
|
||||||
|
# Abort detection thresholds
|
||||||
|
TURN_MISS_DISTANCE_FACTOR = 2.0 # drove 2x expected distance without turning = missed
|
||||||
|
TURN_MISS_YAW_THRESHOLD = 0.3 # must achieve at least 30% of turn angle
|
||||||
|
TURN_WRONG_DIRECTION_ANGLE = 20.0 # degrees in wrong direction = missed
|
||||||
|
TURN_TIMEOUT = 30.0 # seconds - max time in APPROACHING/EXECUTING
|
||||||
|
|
||||||
|
|
||||||
|
class TurnState(IntEnum):
|
||||||
|
NONE = 0 # no turn pending
|
||||||
|
APPROACHING = 1 # turn ahead, dead reckoning distance
|
||||||
|
EXECUTING = 2 # in intersection, tracking heading change
|
||||||
|
COMPLETE = 3 # turn done, cooldown before next
|
||||||
|
MISSED = 4 # turn missed/aborted, wait for reroute
|
||||||
|
|
||||||
|
|
||||||
|
class TurnTracker:
|
||||||
|
"""
|
||||||
|
Tracks turn execution using dead reckoning.
|
||||||
|
|
||||||
|
Once triggered at ~200m, ignores navInstruction updates and purely
|
||||||
|
uses vEgo (distance) and yawRate (heading) from carState.
|
||||||
|
|
||||||
|
Driver acknowledgment flow (two-step, like lane change):
|
||||||
|
- System shows turn suggestion at 150m
|
||||||
|
- Driver turns on blinker = acknowledged (speed limit, block lane change)
|
||||||
|
- Driver steers in turn direction = steering confirmed (desire sent)
|
||||||
|
- Blinker gates the approach, steering gates the turn execution
|
||||||
|
"""
|
||||||
|
|
||||||
|
def __init__(self):
|
||||||
|
self.state = TurnState.NONE
|
||||||
|
# Captured at trigger
|
||||||
|
self.expected_angle = 0.0 # degrees (positive=left, negative=right)
|
||||||
|
self.initial_distance = 0.0 # distance to turn when triggered
|
||||||
|
self.direction = 'none' # 'left' or 'right'
|
||||||
|
self.maneuver_type = 0 # MaaControl.ManeuverType
|
||||||
|
self.turn_speed_limit = 0.0 # m/s
|
||||||
|
# Dead reckoning state
|
||||||
|
self.distance_traveled = 0.0 # meters since trigger
|
||||||
|
self.accumulated_yaw = 0.0 # degrees turned since execute start
|
||||||
|
self.moving_time = 0.0 # accumulated time while moving (for timeout)
|
||||||
|
self.execute_start_time = 0.0 # when EXECUTING started
|
||||||
|
self.complete_time = 0.0 # when turn completed
|
||||||
|
self.last_update_time = 0.0 # for dt calculation
|
||||||
|
# Driver acknowledgment (two-step like lane change)
|
||||||
|
self.driver_acknowledged = False # blinker matches turn direction
|
||||||
|
self.steering_confirmed = False # steering torque matches turn direction
|
||||||
|
|
||||||
|
def reset(self):
|
||||||
|
self.state = TurnState.NONE
|
||||||
|
self.expected_angle = 0.0
|
||||||
|
self.initial_distance = 0.0
|
||||||
|
self.direction = 'none'
|
||||||
|
self.maneuver_type = 0
|
||||||
|
self.turn_speed_limit = 0.0
|
||||||
|
self.distance_traveled = 0.0
|
||||||
|
self.accumulated_yaw = 0.0
|
||||||
|
self.moving_time = 0.0
|
||||||
|
self.execute_start_time = 0.0
|
||||||
|
self.complete_time = 0.0
|
||||||
|
self.last_update_time = 0.0
|
||||||
|
self.driver_acknowledged = False
|
||||||
|
self.steering_confirmed = False
|
||||||
|
|
||||||
|
def get_estimated_distance(self) -> float:
|
||||||
|
"""Get estimated distance to turn based on dead reckoning."""
|
||||||
|
if self.state in (TurnState.APPROACHING, TurnState.EXECUTING):
|
||||||
|
return max(0.0, self.initial_distance - self.distance_traveled)
|
||||||
|
return 9999.0
|
||||||
|
|
||||||
|
def trigger(self, t: float, turn_distance: float, turn_angle: float,
|
||||||
|
maneuver_type: int, turn_direction: int, turn_speed_limit: float):
|
||||||
|
"""Capture turn parameters and start dead reckoning."""
|
||||||
|
self.state = TurnState.APPROACHING
|
||||||
|
self.expected_angle = turn_angle
|
||||||
|
self.initial_distance = turn_distance
|
||||||
|
self.maneuver_type = maneuver_type
|
||||||
|
self.turn_speed_limit = turn_speed_limit
|
||||||
|
self.direction = 'left' if turn_direction == custom.MaaControl.TurnDirection.left else 'right'
|
||||||
|
self.distance_traveled = 0.0
|
||||||
|
self.accumulated_yaw = 0.0
|
||||||
|
self.moving_time = 0.0
|
||||||
|
self.last_update_time = t
|
||||||
|
cloudlog.info(f"maa: turn triggered, angle={turn_angle:.1f}°, dist={turn_distance:.0f}m, dir={self.direction}")
|
||||||
|
|
||||||
|
def check_blinker(self, left_blinker: bool, right_blinker: bool):
|
||||||
|
"""Check if driver turned on blinker matching turn direction."""
|
||||||
|
if self.direction == 'left' and left_blinker:
|
||||||
|
self.driver_acknowledged = True
|
||||||
|
elif self.direction == 'right' and right_blinker:
|
||||||
|
self.driver_acknowledged = True
|
||||||
|
# Note: we don't reset acknowledged if blinker turns off
|
||||||
|
# (driver may have tapped blinker briefly to acknowledge)
|
||||||
|
|
||||||
|
def check_steering(self, steering_pressed: bool, steering_torque: float):
|
||||||
|
"""Check if driver is steering in the turn direction (like lane change confirmation)."""
|
||||||
|
if not steering_pressed:
|
||||||
|
return
|
||||||
|
# Same logic as lane change: positive torque = left, negative = right
|
||||||
|
if self.direction == 'left' and steering_torque > 0:
|
||||||
|
self.steering_confirmed = True
|
||||||
|
elif self.direction == 'right' and steering_torque < 0:
|
||||||
|
self.steering_confirmed = True
|
||||||
|
# Once confirmed, stays confirmed (one-time check)
|
||||||
|
|
||||||
|
def update(self, t: float, v_ego: float, yaw_rate: float) -> bool:
|
||||||
|
"""
|
||||||
|
Update turn state with dead reckoning.
|
||||||
|
|
||||||
|
Args:
|
||||||
|
t: current time (monotonic)
|
||||||
|
v_ego: vehicle speed (m/s) from carState
|
||||||
|
yaw_rate: yaw rate (rad/s) from carState
|
||||||
|
|
||||||
|
Returns True if desire should be sent (acknowledged + within commit distance).
|
||||||
|
"""
|
||||||
|
if self.state == TurnState.NONE:
|
||||||
|
return False
|
||||||
|
|
||||||
|
# Calculate dt
|
||||||
|
dt = t - self.last_update_time if self.last_update_time > 0 else 0.05
|
||||||
|
dt = min(dt, 0.2) # Cap dt to handle timing glitches
|
||||||
|
self.last_update_time = t
|
||||||
|
|
||||||
|
# Integrate distance traveled (and moving time for timeout)
|
||||||
|
self.distance_traveled += v_ego * dt
|
||||||
|
if v_ego > 1.0: # Only count time when actually moving
|
||||||
|
self.moving_time += dt
|
||||||
|
estimated_dist = self.get_estimated_distance()
|
||||||
|
|
||||||
|
# Timeout check - based on moving time (allows waiting at traffic lights)
|
||||||
|
if self.moving_time > TURN_TIMEOUT:
|
||||||
|
cloudlog.warning(f"maa: turn timeout after {self.moving_time:.0f}s moving")
|
||||||
|
self.state = TurnState.MISSED
|
||||||
|
return False
|
||||||
|
|
||||||
|
if self.state == TurnState.APPROACHING:
|
||||||
|
# Check if we've reached the intersection
|
||||||
|
if estimated_dist <= TURN_EXECUTE_DISTANCE:
|
||||||
|
self.state = TurnState.EXECUTING
|
||||||
|
self.execute_start_time = t
|
||||||
|
self.accumulated_yaw = 0.0
|
||||||
|
cloudlog.info(f"maa: turn executing, traveled={self.distance_traveled:.0f}m")
|
||||||
|
|
||||||
|
# Miss detection: drove way past without entering execute
|
||||||
|
if self.distance_traveled > self.initial_distance * TURN_MISS_DISTANCE_FACTOR:
|
||||||
|
cloudlog.warning(f"maa: turn missed (drove past), traveled={self.distance_traveled:.0f}m")
|
||||||
|
self.state = TurnState.MISSED
|
||||||
|
return False
|
||||||
|
|
||||||
|
elif self.state == TurnState.EXECUTING:
|
||||||
|
# Integrate yaw rate (convert rad/s to deg)
|
||||||
|
yaw_change_deg = math.degrees(yaw_rate) * dt
|
||||||
|
self.accumulated_yaw += yaw_change_deg
|
||||||
|
|
||||||
|
# Fix 3: Early abort if going straight through the "turn" (map error)
|
||||||
|
# If we've traveled 20m past the turn point with no significant yaw change,
|
||||||
|
# the turn doesn't exist - abort quickly instead of extended false braking
|
||||||
|
distance_past_turn = self.distance_traveled - self.initial_distance
|
||||||
|
if distance_past_turn > 20.0 and abs(self.accumulated_yaw) < 5.0:
|
||||||
|
cloudlog.warning(f"maa: no turn detected (past {distance_past_turn:.0f}m, yaw={self.accumulated_yaw:.1f}°), aborting")
|
||||||
|
self.state = TurnState.MISSED
|
||||||
|
return False
|
||||||
|
|
||||||
|
# Check completion: achieved target heading
|
||||||
|
# For left turn: expected_angle > 0, accumulated should go positive
|
||||||
|
# For right turn: expected_angle < 0, accumulated should go negative
|
||||||
|
progress = abs(self.accumulated_yaw) / abs(self.expected_angle) if self.expected_angle != 0 else 0
|
||||||
|
angle_remaining = abs(self.expected_angle) - abs(self.accumulated_yaw)
|
||||||
|
|
||||||
|
if angle_remaining <= TURN_ANGLE_TOLERANCE:
|
||||||
|
self.state = TurnState.COMPLETE
|
||||||
|
self.complete_time = t
|
||||||
|
cloudlog.info(f"maa: turn complete, yaw={self.accumulated_yaw:.1f}°, expected={self.expected_angle:.1f}°")
|
||||||
|
return False
|
||||||
|
|
||||||
|
# Wrong direction detection: significant yaw in opposite direction
|
||||||
|
if self.expected_angle > 0 and self.accumulated_yaw < -TURN_WRONG_DIRECTION_ANGLE:
|
||||||
|
cloudlog.warning(f"maa: turn wrong direction (expected left, went right)")
|
||||||
|
self.state = TurnState.MISSED
|
||||||
|
return False
|
||||||
|
if self.expected_angle < 0 and self.accumulated_yaw > TURN_WRONG_DIRECTION_ANGLE:
|
||||||
|
cloudlog.warning(f"maa: turn wrong direction (expected right, went left)")
|
||||||
|
self.state = TurnState.MISSED
|
||||||
|
return False
|
||||||
|
|
||||||
|
# Miss detection: drove 2x expected total distance without sufficient turn
|
||||||
|
total_expected = self.initial_distance + 50.0 # Add some buffer for turn itself
|
||||||
|
if self.distance_traveled > total_expected * TURN_MISS_DISTANCE_FACTOR:
|
||||||
|
if progress < TURN_MISS_YAW_THRESHOLD:
|
||||||
|
cloudlog.warning(f"maa: turn missed (insufficient yaw), progress={progress:.1%}")
|
||||||
|
self.state = TurnState.MISSED
|
||||||
|
return False
|
||||||
|
|
||||||
|
elif self.state == TurnState.COMPLETE:
|
||||||
|
# Short cooldown then reset
|
||||||
|
if t - self.complete_time > 2.0:
|
||||||
|
self.reset()
|
||||||
|
return False
|
||||||
|
|
||||||
|
elif self.state == TurnState.MISSED:
|
||||||
|
# Stay in missed state until route changes (reset called externally)
|
||||||
|
return False
|
||||||
|
|
||||||
|
# Desire active when:
|
||||||
|
# Driver acknowledged (blinker on) AND steering confirmed AND either:
|
||||||
|
# 1. APPROACHING and within commit distance (<100m), OR
|
||||||
|
# 2. EXECUTING (in the turn)
|
||||||
|
# Without both confirmations, NO desire is sent (driver maintains control)
|
||||||
|
if not self.driver_acknowledged or not self.steering_confirmed:
|
||||||
|
return False
|
||||||
|
if self.state == TurnState.APPROACHING:
|
||||||
|
return estimated_dist <= TURN_COMMIT_DISTANCE
|
||||||
|
if self.state == TurnState.EXECUTING:
|
||||||
|
return True
|
||||||
|
return False
|
||||||
|
|
||||||
|
|
||||||
|
def get_turn_speed_from_curvature(curvature: float) -> float:
|
||||||
|
"""
|
||||||
|
Compute recommended speed for turn using CarrotPilot's curvature lookup table.
|
||||||
|
This is physics-based: v = sqrt(a_lat / κ) where a_lat ≈ 2.5 m/s² for comfort.
|
||||||
|
|
||||||
|
Args:
|
||||||
|
curvature: Road curvature in 1/m (positive or negative)
|
||||||
|
|
||||||
|
Returns:
|
||||||
|
Recommended speed in m/s
|
||||||
|
"""
|
||||||
|
abs_curv = abs(curvature)
|
||||||
|
speed_kph = np.interp(abs_curv, V_CURVE_LOOKUP_BP, V_CURVE_LOOKUP_VALS)
|
||||||
|
return speed_kph / 3.6 # Convert to m/s
|
||||||
|
|
||||||
|
|
||||||
|
def get_turn_speed_limit(modifier: str, turn_angle: float, nav_type: str = '', curvature: float = 0.0) -> float:
|
||||||
|
"""
|
||||||
|
Compute recommended speed for turn based on curvature (preferred) or heuristics.
|
||||||
|
|
||||||
|
Uses CarrotPilot's curvature lookup table when curvature is available.
|
||||||
|
Falls back to heuristic-based calculation when curvature is not available.
|
||||||
|
|
||||||
|
Args:
|
||||||
|
modifier: OSRM maneuver modifier (e.g., 'left', 'right', 'sharp left')
|
||||||
|
turn_angle: Turn angle in degrees (positive=left, negative=right)
|
||||||
|
nav_type: OSRM maneuver type (e.g., 'turn', 'off ramp')
|
||||||
|
curvature: Road curvature in 1/m (from geometry calculation)
|
||||||
|
|
||||||
|
Returns:
|
||||||
|
Recommended speed in m/s
|
||||||
|
"""
|
||||||
|
# Use curvature-based calculation when curvature is available
|
||||||
|
# This is more accurate than heuristics as it's physics-based
|
||||||
|
if abs(curvature) > 0.001: # Meaningful curvature
|
||||||
|
speed_from_curv = get_turn_speed_from_curvature(curvature)
|
||||||
|
# Clamp to reasonable range: 5 km/h to 100 km/h for turns
|
||||||
|
return max(5.0 / 3.6, min(100.0 / 3.6, speed_from_curv))
|
||||||
|
|
||||||
|
# Fallback: Heuristic-based calculation when curvature not available
|
||||||
|
|
||||||
|
# Highway maneuvers (exits, ramps, merges) - much higher speeds
|
||||||
|
if nav_type in ('depart', 'off ramp', 'on ramp', 'merge', 'fork'):
|
||||||
|
if 'sharp' in modifier:
|
||||||
|
base_speed = 50.0 # km/h - sharp highway exit
|
||||||
|
elif 'slight' in modifier:
|
||||||
|
base_speed = 80.0 # km/h - gentle curve
|
||||||
|
else:
|
||||||
|
base_speed = 60.0 # km/h - normal exit ramp
|
||||||
|
return base_speed / 3.6 # Don't apply angle adjustments to highway maneuvers
|
||||||
|
|
||||||
|
# Intersection turns - lower speeds
|
||||||
|
if 'sharp' in modifier:
|
||||||
|
base_speed = 15.0 # km/h
|
||||||
|
elif 'slight' in modifier:
|
||||||
|
base_speed = 45.0 # km/h
|
||||||
|
elif modifier in ('left', 'right'):
|
||||||
|
base_speed = 25.0 # km/h
|
||||||
|
elif 'uturn' in modifier:
|
||||||
|
base_speed = 10.0 # km/h
|
||||||
|
else:
|
||||||
|
base_speed = 50.0 # km/h default
|
||||||
|
|
||||||
|
# Adjust based on actual angle if available (only for intersection turns)
|
||||||
|
abs_angle = abs(turn_angle)
|
||||||
|
if abs_angle > 90:
|
||||||
|
base_speed = min(base_speed, 15.0)
|
||||||
|
elif abs_angle > 60:
|
||||||
|
base_speed = min(base_speed, 25.0)
|
||||||
|
elif abs_angle > 30:
|
||||||
|
base_speed = min(base_speed, 35.0)
|
||||||
|
|
||||||
|
return base_speed / 3.6 # Convert to m/s
|
||||||
|
|
||||||
|
|
||||||
|
def get_maneuver_type(nav_type: str, turn_angle: float = 0.0) -> int:
|
||||||
|
"""Determine maneuver type primarily from geometry, with OSRM hints.
|
||||||
|
|
||||||
|
Geometry is the source of truth - OSRM labels can be wrong.
|
||||||
|
"""
|
||||||
|
# Geometry-first: significant turn angle = it's a turn
|
||||||
|
if abs(turn_angle) >= TURN_MIN_ANGLE:
|
||||||
|
return custom.MaaControl.ManeuverType.turn
|
||||||
|
|
||||||
|
# Highway maneuvers (even with small angle) - use laneChange desire
|
||||||
|
if nav_type in ('off ramp', 'on ramp', 'merge', 'fork'):
|
||||||
|
return custom.MaaControl.ManeuverType.laneChange
|
||||||
|
|
||||||
|
# OSRM says turn but geometry is minor - still treat as turn
|
||||||
|
if nav_type in ('turn', 'end of road'):
|
||||||
|
return custom.MaaControl.ManeuverType.turn
|
||||||
|
|
||||||
|
# Everything else: no action needed
|
||||||
|
return custom.MaaControl.ManeuverType.none
|
||||||
|
|
||||||
|
|
||||||
|
def get_last_gps_position(params: Params) -> tuple:
|
||||||
|
"""Get last known GPS position from params. Returns (lat, lon, bearing) or None."""
|
||||||
|
try:
|
||||||
|
data = params.get("LastGPSPosition")
|
||||||
|
if data:
|
||||||
|
pos = json.loads(data)
|
||||||
|
return pos.get('latitude'), pos.get('longitude'), pos.get('bearing', 0.0)
|
||||||
|
except Exception:
|
||||||
|
pass
|
||||||
|
return None
|
||||||
|
|
||||||
|
|
||||||
|
def main():
|
||||||
|
cloudlog.info("maa_controld: starting")
|
||||||
|
|
||||||
|
params = Params()
|
||||||
|
sm = messaging.SubMaster(
|
||||||
|
['navRoute', 'navInstruction', 'liveGPS', 'carState'],
|
||||||
|
ignore_alive=['navRoute', 'navInstruction', 'liveGPS', 'carState']
|
||||||
|
)
|
||||||
|
pm = messaging.PubMaster(['maaControl'])
|
||||||
|
|
||||||
|
rk = Ratekeeper(20)
|
||||||
|
|
||||||
|
# State
|
||||||
|
route_coords: list[Coordinate] = []
|
||||||
|
last_route_len = 0
|
||||||
|
last_gps_time = 0.0
|
||||||
|
closest_idx = 0
|
||||||
|
|
||||||
|
# Turn tracker (dead reckoning based)
|
||||||
|
turn_tracker = TurnTracker()
|
||||||
|
|
||||||
|
# Fallback position from params (used before liveGPS is ready)
|
||||||
|
fallback_pos = get_last_gps_position(params)
|
||||||
|
if fallback_pos:
|
||||||
|
cloudlog.info(f"maa_controld: fallback position {fallback_pos[0]:.6f}, {fallback_pos[1]:.6f}")
|
||||||
|
|
||||||
|
while True:
|
||||||
|
sm.update(0)
|
||||||
|
t = time.monotonic()
|
||||||
|
|
||||||
|
# Get current position - prefer liveGPS, fall back to LastGPSPosition
|
||||||
|
if sm.updated['liveGPS']:
|
||||||
|
last_gps_time = time.monotonic()
|
||||||
|
|
||||||
|
gps = sm['liveGPS']
|
||||||
|
gps_stale = (time.monotonic() - last_gps_time) > 2.0
|
||||||
|
gps_valid = gps.status == custom.LiveGPS.Status.valid and not gps_stale
|
||||||
|
|
||||||
|
# Use fallback if liveGPS not ready
|
||||||
|
use_fallback = not gps_valid and fallback_pos is not None
|
||||||
|
if gps_valid:
|
||||||
|
current_lat = gps.latitude
|
||||||
|
current_lon = gps.longitude
|
||||||
|
current_bearing = gps.bearingDeg
|
||||||
|
elif use_fallback:
|
||||||
|
current_lat, current_lon, current_bearing = fallback_pos
|
||||||
|
else:
|
||||||
|
current_lat = current_lon = current_bearing = None
|
||||||
|
|
||||||
|
# Get turn info from navInstruction
|
||||||
|
nav = sm['navInstruction']
|
||||||
|
nav_valid = sm.valid['navInstruction']
|
||||||
|
|
||||||
|
# Always update route coordinates (needed for turn angle calculation)
|
||||||
|
nav_route = sm['navRoute']
|
||||||
|
if sm.valid['navRoute'] and nav_route.coordinates:
|
||||||
|
new_len = len(nav_route.coordinates)
|
||||||
|
if new_len != last_route_len:
|
||||||
|
route_coords = [
|
||||||
|
Coordinate(c.latitude, c.longitude)
|
||||||
|
for c in nav_route.coordinates
|
||||||
|
]
|
||||||
|
last_route_len = new_len
|
||||||
|
closest_idx = 0
|
||||||
|
turn_tracker.reset() # Reset turn state when route changes
|
||||||
|
cloudlog.debug(f"maa_controld: route updated, {new_len} points")
|
||||||
|
|
||||||
|
# Find current position on route
|
||||||
|
has_position = current_lat is not None
|
||||||
|
if route_coords and has_position:
|
||||||
|
current_pos = Coordinate(current_lat, current_lon)
|
||||||
|
try:
|
||||||
|
# Optimization: Search locally around last known position
|
||||||
|
search_start = max(0, closest_idx - 10)
|
||||||
|
search_end = min(len(route_coords), closest_idx + 50)
|
||||||
|
|
||||||
|
if closest_idx == 0:
|
||||||
|
search_start = 0
|
||||||
|
search_end = len(route_coords)
|
||||||
|
|
||||||
|
subset = route_coords[search_start:search_end]
|
||||||
|
local_idx, _ = find_closest_point_on_route(current_pos, subset)
|
||||||
|
closest_idx = search_start + local_idx
|
||||||
|
except Exception as e:
|
||||||
|
cloudlog.warning(f"maa_controld: position error: {e}")
|
||||||
|
|
||||||
|
# Get speed, blinker, and yawRate from carState
|
||||||
|
cs = sm['carState']
|
||||||
|
cs_valid = sm.valid['carState']
|
||||||
|
v_ego = cs.vEgo if cs_valid else 0.0
|
||||||
|
left_blinker = cs.leftBlinker if cs_valid else False
|
||||||
|
right_blinker = cs.rightBlinker if cs_valid else False
|
||||||
|
yaw_rate = cs.yawRate if cs_valid else 0.0
|
||||||
|
|
||||||
|
# Continuous curvature assist (optional - for steering)
|
||||||
|
curvature = 0.0
|
||||||
|
curvature_valid = False
|
||||||
|
if CURVATURE_ASSIST_ENABLED and route_coords and has_position and v_ego > MIN_SPEED_FOR_CURVATURE:
|
||||||
|
try:
|
||||||
|
curvature = compute_path_curvature(
|
||||||
|
current_pos, current_bearing, route_coords, closest_idx, v_ego, CURVATURE_LOOKAHEAD
|
||||||
|
)
|
||||||
|
curvature_valid = True
|
||||||
|
except Exception as e:
|
||||||
|
cloudlog.warning(f"maa_controld: curvature error: {e}")
|
||||||
|
|
||||||
|
# Build maaControl message
|
||||||
|
msg = messaging.new_message('maaControl', valid=True)
|
||||||
|
maa = msg.maaControl
|
||||||
|
|
||||||
|
maa.curvature = float(curvature)
|
||||||
|
maa.curvatureValid = curvature_valid
|
||||||
|
|
||||||
|
# Get maneuver info from navInstruction (1Hz, used only for trigger)
|
||||||
|
maneuver_dist = getattr(nav, 'maneuverDistance', None) if nav_valid else None
|
||||||
|
|
||||||
|
# If turn tracker is active (including MISSED/COMPLETE), handle accordingly
|
||||||
|
if turn_tracker.state in (TurnState.APPROACHING, TurnState.EXECUTING):
|
||||||
|
# Safety checks: detect if turn info changed significantly
|
||||||
|
if nav_valid and maneuver_dist is not None:
|
||||||
|
# Use turnAngle from navInstructionExt - this is geometry-based (reliable)
|
||||||
|
turn_angle = getattr(nav, 'turnAngle', 0.0) or 0.0
|
||||||
|
estimated_dist = turn_tracker.get_estimated_distance()
|
||||||
|
|
||||||
|
# Check 1: Did we pass the turn? (nav distance jumped up = now showing NEXT turn)
|
||||||
|
# If we think we're close (<50m) but nav says far (>150m), we probably passed it
|
||||||
|
if estimated_dist < 50.0 and maneuver_dist > 150.0:
|
||||||
|
cloudlog.warning(f"maa: likely passed turn (est={estimated_dist:.0f}m, nav={maneuver_dist:.0f}m), resetting")
|
||||||
|
turn_tracker.reset()
|
||||||
|
|
||||||
|
# Check 2: Did direction flip? (route recalculated or now showing different turn)
|
||||||
|
else:
|
||||||
|
current_nav_dir = None
|
||||||
|
if turn_angle > TURN_MIN_ANGLE:
|
||||||
|
current_nav_dir = 'left'
|
||||||
|
elif turn_angle < -TURN_MIN_ANGLE:
|
||||||
|
current_nav_dir = 'right'
|
||||||
|
|
||||||
|
if current_nav_dir and current_nav_dir != turn_tracker.direction:
|
||||||
|
cloudlog.warning(f"maa: turn direction changed ({turn_tracker.direction} → {current_nav_dir}), angle={turn_angle:.1f}°, resetting")
|
||||||
|
turn_tracker.reset()
|
||||||
|
|
||||||
|
# Check 3: Turn disappeared (angle now below threshold)
|
||||||
|
elif abs(turn_angle) < TURN_MIN_ANGLE:
|
||||||
|
nav_type = getattr(nav, 'maneuverType', '') or ''
|
||||||
|
if get_maneuver_type(nav_type, turn_angle) == custom.MaaControl.ManeuverType.none:
|
||||||
|
cloudlog.warning(f"maa: turn no longer valid (angle={turn_angle:.1f}°), resetting")
|
||||||
|
turn_tracker.reset()
|
||||||
|
|
||||||
|
if turn_tracker.state in (TurnState.APPROACHING, TurnState.EXECUTING):
|
||||||
|
# Check blinker and steering for driver acknowledgment (like lane change)
|
||||||
|
turn_tracker.check_blinker(left_blinker, right_blinker)
|
||||||
|
turn_tracker.check_steering(cs.steeringPressed, cs.steeringTorque)
|
||||||
|
|
||||||
|
# Dead reckon distance - ignore navInstruction updates
|
||||||
|
estimated_dist = turn_tracker.get_estimated_distance()
|
||||||
|
|
||||||
|
# Fix 2: Allow abort if blinker turns off before commitment (>100m)
|
||||||
|
# User can change their mind if not yet committed to the turn
|
||||||
|
if turn_tracker.state == TurnState.APPROACHING and turn_tracker.driver_acknowledged:
|
||||||
|
blinker_matches = (turn_tracker.direction == 'left' and left_blinker) or \
|
||||||
|
(turn_tracker.direction == 'right' and right_blinker)
|
||||||
|
if not blinker_matches and estimated_dist > TURN_COMMIT_DISTANCE:
|
||||||
|
turn_tracker.driver_acknowledged = False
|
||||||
|
turn_tracker.steering_confirmed = False
|
||||||
|
cloudlog.info("maa: blinker canceled before commit, aborting turn assist")
|
||||||
|
maa.turnDistance = float(estimated_dist)
|
||||||
|
maa.turnValid = True
|
||||||
|
maa.turnAngle = float(turn_tracker.expected_angle)
|
||||||
|
maa.maneuverType = turn_tracker.maneuver_type
|
||||||
|
maa.turnSpeedLimit = float(turn_tracker.turn_speed_limit)
|
||||||
|
|
||||||
|
# Direction from captured state
|
||||||
|
if turn_tracker.direction == 'left':
|
||||||
|
maa.turnDirection = custom.MaaControl.TurnDirection.left
|
||||||
|
elif turn_tracker.direction == 'right':
|
||||||
|
maa.turnDirection = custom.MaaControl.TurnDirection.right
|
||||||
|
else:
|
||||||
|
maa.turnDirection = custom.MaaControl.TurnDirection.none
|
||||||
|
|
||||||
|
# Update tracker with dead reckoning
|
||||||
|
desire_active = turn_tracker.update(t, v_ego, yaw_rate)
|
||||||
|
maa.turnState = int(turn_tracker.state)
|
||||||
|
maa.turnProgress = float(turn_tracker.accumulated_yaw)
|
||||||
|
|
||||||
|
# Driver acknowledgment status
|
||||||
|
maa.driverAcknowledged = turn_tracker.driver_acknowledged
|
||||||
|
|
||||||
|
# Speed limit active when blinker on (driver acknowledged)
|
||||||
|
maa.speedLimitActive = turn_tracker.driver_acknowledged
|
||||||
|
|
||||||
|
# Block lane change when committed (blinker + within commit distance)
|
||||||
|
maa.blockLaneChange = turn_tracker.driver_acknowledged and estimated_dist <= TURN_COMMIT_DISTANCE
|
||||||
|
|
||||||
|
# desireActive: send turn desire to model (requires blinker)
|
||||||
|
maa.desireActive = desire_active
|
||||||
|
|
||||||
|
# Curvature from captured state (not recalculated)
|
||||||
|
maa.curvature = 0.0
|
||||||
|
maa.curvatureValid = False
|
||||||
|
maa.turnCurvature = 0.0
|
||||||
|
|
||||||
|
elif nav_valid and maneuver_dist is not None:
|
||||||
|
# No active turn tracking - check if we should trigger
|
||||||
|
maa.turnDistance = float(maneuver_dist)
|
||||||
|
maa.turnValid = maneuver_dist < TURN_VALID_DISTANCE
|
||||||
|
|
||||||
|
nav_type = getattr(nav, 'maneuverType', '') or ''
|
||||||
|
modifier = getattr(nav, 'maneuverModifier', '') or ''
|
||||||
|
|
||||||
|
# Get pre-computed turn geometry from navInstruction
|
||||||
|
turn_angle = getattr(nav, 'turnAngle', 0.0) or 0.0
|
||||||
|
turn_curvature = getattr(nav, 'turnCurvature', 0.0) or 0.0
|
||||||
|
|
||||||
|
maa.turnAngle = float(turn_angle)
|
||||||
|
maa.turnCurvature = float(turn_curvature)
|
||||||
|
|
||||||
|
# Compute maneuver type
|
||||||
|
maa.maneuverType = get_maneuver_type(nav_type, turn_angle)
|
||||||
|
|
||||||
|
# Set turn direction
|
||||||
|
if maa.maneuverType != custom.MaaControl.ManeuverType.none:
|
||||||
|
if 'left' in modifier.lower():
|
||||||
|
maa.turnDirection = custom.MaaControl.TurnDirection.left
|
||||||
|
elif 'right' in modifier.lower():
|
||||||
|
maa.turnDirection = custom.MaaControl.TurnDirection.right
|
||||||
|
elif turn_angle > TURN_MIN_ANGLE:
|
||||||
|
maa.turnDirection = custom.MaaControl.TurnDirection.left
|
||||||
|
elif turn_angle < -TURN_MIN_ANGLE:
|
||||||
|
maa.turnDirection = custom.MaaControl.TurnDirection.right
|
||||||
|
else:
|
||||||
|
maa.turnDirection = custom.MaaControl.TurnDirection.none
|
||||||
|
else:
|
||||||
|
maa.turnDirection = custom.MaaControl.TurnDirection.none
|
||||||
|
|
||||||
|
# Compute turn speed limit using CarrotPilot curvature lookup table
|
||||||
|
# Curvature-based is more accurate than heuristics when available
|
||||||
|
turn_speed_limit = get_turn_speed_limit(modifier, turn_angle, nav_type, turn_curvature)
|
||||||
|
maa.turnSpeedLimit = float(turn_speed_limit)
|
||||||
|
|
||||||
|
# Check if we should trigger dead reckoning
|
||||||
|
# Don't trigger if already in MISSED/COMPLETE state (wait for route change)
|
||||||
|
can_trigger = turn_tracker.state == TurnState.NONE
|
||||||
|
is_actionable = maa.maneuverType != custom.MaaControl.ManeuverType.none
|
||||||
|
has_direction = maa.turnDirection != custom.MaaControl.TurnDirection.none
|
||||||
|
is_significant = abs(turn_angle) >= TURN_MIN_ANGLE
|
||||||
|
in_trigger_range = maneuver_dist <= TURN_TRIGGER_DISTANCE
|
||||||
|
|
||||||
|
if can_trigger and is_actionable and has_direction and is_significant and in_trigger_range:
|
||||||
|
# Trigger! Capture turn params and start dead reckoning
|
||||||
|
turn_tracker.trigger(
|
||||||
|
t, maneuver_dist, turn_angle,
|
||||||
|
maa.maneuverType, maa.turnDirection, turn_speed_limit
|
||||||
|
)
|
||||||
|
# Check blinker and steering immediately after trigger
|
||||||
|
turn_tracker.check_blinker(left_blinker, right_blinker)
|
||||||
|
turn_tracker.check_steering(cs.steeringPressed, cs.steeringTorque)
|
||||||
|
# Blinker = approach confirmation, steering = turn execution confirmation
|
||||||
|
maa.desireActive = turn_tracker.driver_acknowledged and turn_tracker.steering_confirmed and maneuver_dist <= TURN_COMMIT_DISTANCE
|
||||||
|
maa.turnState = int(turn_tracker.state)
|
||||||
|
maa.turnProgress = 0.0
|
||||||
|
maa.driverAcknowledged = turn_tracker.driver_acknowledged
|
||||||
|
maa.speedLimitActive = turn_tracker.driver_acknowledged
|
||||||
|
maa.blockLaneChange = turn_tracker.driver_acknowledged and maneuver_dist <= TURN_COMMIT_DISTANCE
|
||||||
|
elif turn_tracker.state in (TurnState.COMPLETE, TurnState.MISSED):
|
||||||
|
# In cooldown - call update to handle state transitions
|
||||||
|
turn_tracker.update(t, v_ego, yaw_rate)
|
||||||
|
maa.desireActive = False
|
||||||
|
maa.turnState = int(turn_tracker.state)
|
||||||
|
maa.turnProgress = float(turn_tracker.accumulated_yaw)
|
||||||
|
maa.driverAcknowledged = False
|
||||||
|
maa.speedLimitActive = False
|
||||||
|
maa.blockLaneChange = False
|
||||||
|
else:
|
||||||
|
# Not triggered yet (turn too far away)
|
||||||
|
maa.desireActive = False
|
||||||
|
maa.turnState = int(TurnState.NONE)
|
||||||
|
maa.turnProgress = 0.0
|
||||||
|
maa.driverAcknowledged = False
|
||||||
|
maa.speedLimitActive = False
|
||||||
|
maa.blockLaneChange = False
|
||||||
|
|
||||||
|
else:
|
||||||
|
# No valid nav instruction
|
||||||
|
maa.turnValid = False
|
||||||
|
maa.turnDirection = custom.MaaControl.TurnDirection.none
|
||||||
|
maa.turnSpeedLimit = 50.0 / 3.6
|
||||||
|
maa.maneuverType = custom.MaaControl.ManeuverType.none
|
||||||
|
maa.turnAngle = 0.0
|
||||||
|
maa.turnCurvature = 0.0
|
||||||
|
maa.turnDistance = 9999.0
|
||||||
|
maa.desireActive = False
|
||||||
|
maa.turnState = int(TurnState.NONE)
|
||||||
|
maa.turnProgress = 0.0
|
||||||
|
maa.driverAcknowledged = False
|
||||||
|
maa.speedLimitActive = False
|
||||||
|
maa.blockLaneChange = False
|
||||||
|
|
||||||
|
# Only reset if not in MISSED state (wait for route change)
|
||||||
|
if turn_tracker.state != TurnState.MISSED:
|
||||||
|
turn_tracker.reset()
|
||||||
|
|
||||||
|
pm.send('maaControl', msg)
|
||||||
|
rk.keep_time()
|
||||||
|
|
||||||
|
|
||||||
|
if __name__ == "__main__":
|
||||||
|
main()
|
||||||
@@ -0,0 +1,566 @@
|
|||||||
|
#!/usr/bin/env python3
|
||||||
|
"""
|
||||||
|
Copyright (c) 2026, Rick Lan
|
||||||
|
|
||||||
|
Permission is hereby granted, free of charge, to any person obtaining a copy
|
||||||
|
of this software and associated documentation files (the "Software"), to deal
|
||||||
|
in the Software without restriction, including without limitation the rights
|
||||||
|
to use, copy, modify, merge, publish, distribute, and/or sublicense,
|
||||||
|
for non-commercial purposes only, subject to the following conditions:
|
||||||
|
|
||||||
|
- The above copyright notice and this permission notice shall be included in
|
||||||
|
all copies or substantial portions of the Software.
|
||||||
|
- Commercial use (e.g. use in a product, service, or activity intended to
|
||||||
|
generate revenue) is prohibited without explicit written permission from
|
||||||
|
the copyright holder.
|
||||||
|
|
||||||
|
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED,
|
||||||
|
INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A
|
||||||
|
PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
|
||||||
|
HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
|
||||||
|
OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
|
||||||
|
SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
|
||||||
|
|
||||||
|
dragonpilot Map-Aware Assist Daemon (maad)
|
||||||
|
|
||||||
|
Handles route calculation and navigation instructions using OSRM routing.
|
||||||
|
Similar to openpilot's navd but uses free OSRM API instead of Mapbox.
|
||||||
|
|
||||||
|
Control signals (maaControl) are published by maa_controld.py separately
|
||||||
|
for low-latency, deterministic control.
|
||||||
|
|
||||||
|
Flow:
|
||||||
|
1. Reads dp_maa_destination from params (set by dashy)
|
||||||
|
2. Fetches route from OSRM (free routing API) - async to avoid blocking
|
||||||
|
3. Subscribes to liveGPS for position updates
|
||||||
|
4. Publishes navInstruction (for UI) and navRoute (for map display)
|
||||||
|
"""
|
||||||
|
import json
|
||||||
|
import time
|
||||||
|
import threading
|
||||||
|
import queue
|
||||||
|
from dataclasses import dataclass
|
||||||
|
from typing import Optional
|
||||||
|
|
||||||
|
import cereal.messaging as messaging
|
||||||
|
from cereal import custom, log
|
||||||
|
from openpilot.common.params import Params
|
||||||
|
from openpilot.common.realtime import Ratekeeper
|
||||||
|
from openpilot.common.swaglog import cloudlog
|
||||||
|
|
||||||
|
from dragonpilot.dashy.maa.helpers import (
|
||||||
|
Coordinate,
|
||||||
|
coordinate_from_param,
|
||||||
|
find_closest_point_on_route,
|
||||||
|
distance_along_geometry,
|
||||||
|
compute_turn_angle_at_index,
|
||||||
|
compute_turn_curvature_at_index,
|
||||||
|
)
|
||||||
|
from dragonpilot.dashy.maa.providers import MapService
|
||||||
|
from dragonpilot.dashy.maa.providers.models import Coordinate as ProviderCoordinate
|
||||||
|
from dragonpilot.dashy.maa.route_tracker import RouteTracker, REROUTE_DISTANCE_BASE, REROUTE_DEBOUNCE_TIME
|
||||||
|
|
||||||
|
MANEUVER_TRANSITION_THRESHOLD = 10 # meters past maneuver to transition
|
||||||
|
NAV_RATE = 1.0 # Hz - navigation update rate
|
||||||
|
|
||||||
|
|
||||||
|
@dataclass
|
||||||
|
class Step:
|
||||||
|
"""Represents a navigation step/segment."""
|
||||||
|
distance: float # total distance of step in meters
|
||||||
|
duration: float # duration in seconds
|
||||||
|
duration_typical: Optional[float]
|
||||||
|
name: str
|
||||||
|
maneuver_type: str
|
||||||
|
maneuver_modifier: str
|
||||||
|
geometry: list[Coordinate] # coordinates for this step
|
||||||
|
speed_limit: Optional[float] # m/s
|
||||||
|
speed_limit_sign: str # 'mutcd' or 'vienna'
|
||||||
|
# Pre-computed turn geometry (computed once when route is fetched)
|
||||||
|
turn_angle: float = 0.0 # degrees, positive=left, negative=right
|
||||||
|
turn_curvature: float = 0.0 # 1/m, positive=left, negative=right
|
||||||
|
# Explicit maneuver point from OSRM (for fork/turn detection)
|
||||||
|
maneuver_point: Optional[Coordinate] = None
|
||||||
|
|
||||||
|
|
||||||
|
class RouteEngine:
|
||||||
|
def __init__(self, sm: messaging.SubMaster, pm: messaging.PubMaster):
|
||||||
|
self.sm = sm
|
||||||
|
self.pm = pm
|
||||||
|
self.params = Params()
|
||||||
|
self.map_service = MapService(self.params)
|
||||||
|
|
||||||
|
# Get last GPS position from params
|
||||||
|
self.last_position = coordinate_from_param("LastGPSPosition", self.params)
|
||||||
|
if self.last_position is None:
|
||||||
|
# Default to Taipei 101 for bench testing
|
||||||
|
self.last_position = Coordinate(25.033976, 121.564472)
|
||||||
|
self.last_bearing: Optional[float] = None
|
||||||
|
|
||||||
|
self.gps_ok = False
|
||||||
|
self.gps_speed = 0.0
|
||||||
|
self.gps_accuracy = 0.0 # horizontal accuracy in meters
|
||||||
|
self.last_gps_time = 0.0
|
||||||
|
|
||||||
|
# Route state
|
||||||
|
self.nav_destination: Optional[Coordinate] = None
|
||||||
|
self.route: Optional[list[Step]] = None
|
||||||
|
self.tracker = RouteTracker() # OsmAnd-style route tracking
|
||||||
|
|
||||||
|
# Recompute state
|
||||||
|
self.recompute_backoff = 0
|
||||||
|
self.recompute_countdown = 0
|
||||||
|
|
||||||
|
# Async route calculation
|
||||||
|
self._route_queue: queue.Queue = queue.Queue()
|
||||||
|
self._route_thread: Optional[threading.Thread] = None
|
||||||
|
self._route_calculating = False
|
||||||
|
|
||||||
|
# Timing diagnostics
|
||||||
|
self._frame_count = 0
|
||||||
|
self._last_timing_log = time.monotonic()
|
||||||
|
self._gps_save_counter = 0
|
||||||
|
self._route_send_counter = 0
|
||||||
|
|
||||||
|
# Params cache - avoid reading params every frame
|
||||||
|
self._cached_destination: Optional[Coordinate] = None
|
||||||
|
self._last_destination_check = 0.0
|
||||||
|
self._destination_check_interval = 3.0 # seconds
|
||||||
|
|
||||||
|
# First valid GPS flag - triggers immediate route calculation
|
||||||
|
self._had_first_valid_gps = False
|
||||||
|
|
||||||
|
def update(self):
|
||||||
|
t0 = time.monotonic()
|
||||||
|
|
||||||
|
self.sm.update(0)
|
||||||
|
t1 = time.monotonic()
|
||||||
|
|
||||||
|
self._update_location()
|
||||||
|
self._check_route_result() # Non-blocking check for async route result
|
||||||
|
t2 = time.monotonic()
|
||||||
|
|
||||||
|
try:
|
||||||
|
self._recompute_route()
|
||||||
|
t3 = time.monotonic()
|
||||||
|
self._send_instruction()
|
||||||
|
# Resend route periodically (every 1s) so new UI clients can see it
|
||||||
|
if self.route is not None:
|
||||||
|
self.send_route()
|
||||||
|
t4 = time.monotonic()
|
||||||
|
except Exception:
|
||||||
|
cloudlog.exception("maad.failed_to_compute")
|
||||||
|
t3 = t4 = time.monotonic()
|
||||||
|
|
||||||
|
# Log timing every 10 seconds
|
||||||
|
self._frame_count += 1
|
||||||
|
total_time = t4 - t0
|
||||||
|
if total_time > 0.05: # Log if frame took > 50ms
|
||||||
|
cloudlog.warning(f"maad slow frame: total={total_time*1000:.0f}ms "
|
||||||
|
f"(sm={1000*(t1-t0):.0f}, loc={1000*(t2-t1):.0f}, "
|
||||||
|
f"route={1000*(t3-t2):.0f}, instr={1000*(t4-t3):.0f})")
|
||||||
|
|
||||||
|
if t4 - self._last_timing_log > 10.0:
|
||||||
|
actual_rate = self._frame_count / (t4 - self._last_timing_log)
|
||||||
|
cloudlog.info(f"maad rate: {actual_rate:.1f} Hz (target {NAV_RATE} Hz), frames={self._frame_count}")
|
||||||
|
self._frame_count = 0
|
||||||
|
self._last_timing_log = t4
|
||||||
|
|
||||||
|
def _check_route_result(self):
|
||||||
|
"""Check for async route calculation result (non-blocking)."""
|
||||||
|
try:
|
||||||
|
result = self._route_queue.get_nowait()
|
||||||
|
self._route_calculating = False
|
||||||
|
|
||||||
|
if result is None:
|
||||||
|
# Route calculation failed
|
||||||
|
self._clear_route(clear_destination=False)
|
||||||
|
return
|
||||||
|
|
||||||
|
# Apply the calculated route
|
||||||
|
self.route, route_data = result
|
||||||
|
self.nav_curvature_valid = True
|
||||||
|
|
||||||
|
# Start at first step (simple, like navd.py)
|
||||||
|
self.tracker.set_step(0)
|
||||||
|
self._reset_recompute_limits() # Reset backoff on successful route
|
||||||
|
cloudlog.warning(f"maad: route calculated - {route_data['distance']/1000:.1f}km, "
|
||||||
|
f"{route_data['duration']/60:.0f}min, {len(self.route)} steps")
|
||||||
|
|
||||||
|
# Debug: log each step's maneuver info
|
||||||
|
for i, s in enumerate(self.route):
|
||||||
|
mp_str = f"({s.maneuver_point.latitude:.6f},{s.maneuver_point.longitude:.6f})" if s.maneuver_point else "None"
|
||||||
|
cloudlog.info(f"maad step {i}: {s.maneuver_type} {s.maneuver_modifier} -> '{s.name}' ({s.distance:.0f}m) mp={mp_str}")
|
||||||
|
|
||||||
|
self.send_route()
|
||||||
|
|
||||||
|
except queue.Empty:
|
||||||
|
pass # No result yet
|
||||||
|
|
||||||
|
def _update_location(self):
|
||||||
|
"""Update position from GPS."""
|
||||||
|
# Debug: log GPS reception status every 50 frames
|
||||||
|
self._gps_debug_counter = getattr(self, '_gps_debug_counter', 0) + 1
|
||||||
|
if self._gps_debug_counter >= 50:
|
||||||
|
self._gps_debug_counter = 0
|
||||||
|
gps = self.sm['liveGPS']
|
||||||
|
cloudlog.info(f"maad GPS: updated={self.sm.updated['liveGPS']} valid={self.sm.valid['liveGPS']} "
|
||||||
|
f"gpsOK={gps.gpsOK} pos=({gps.latitude:.6f},{gps.longitude:.6f})")
|
||||||
|
|
||||||
|
if self.sm.updated['liveGPS']:
|
||||||
|
gps = self.sm['liveGPS']
|
||||||
|
|
||||||
|
# Always update position and speed from GPS (needed for route calculation)
|
||||||
|
if gps.gpsOK:
|
||||||
|
self.last_position = Coordinate(gps.latitude, gps.longitude)
|
||||||
|
self.gps_speed = gps.speed
|
||||||
|
self.gps_accuracy = gps.horizontalAccuracy # Track for OsmAnd-style tolerance
|
||||||
|
self.last_gps_time = time.monotonic()
|
||||||
|
|
||||||
|
# Save last position every ~60 frames (12 seconds at 5Hz) to reduce I/O
|
||||||
|
self._gps_save_counter += 1
|
||||||
|
if self._gps_save_counter >= 60:
|
||||||
|
self._gps_save_counter = 0
|
||||||
|
self.params.put("LastGPSPosition", json.dumps({
|
||||||
|
'latitude': gps.latitude,
|
||||||
|
'longitude': gps.longitude
|
||||||
|
}))
|
||||||
|
|
||||||
|
# GPS is valid when OK, good accuracy, AND fully calibrated
|
||||||
|
was_gps_ok = self.gps_ok
|
||||||
|
is_calibrated = gps.status == custom.LiveGPS.Status.valid
|
||||||
|
self.gps_ok = gps.gpsOK and gps.horizontalAccuracy <= 20.0 and is_calibrated
|
||||||
|
|
||||||
|
# Only use bearing when fusion is fully calibrated
|
||||||
|
if is_calibrated:
|
||||||
|
self.last_bearing = gps.bearingDeg
|
||||||
|
else:
|
||||||
|
self.last_bearing = None # clear stale bearing
|
||||||
|
|
||||||
|
# Detect first valid GPS - triggers immediate route check
|
||||||
|
if self.gps_ok and not was_gps_ok and not self._had_first_valid_gps:
|
||||||
|
self._had_first_valid_gps = True
|
||||||
|
cloudlog.info("maad: first valid GPS fix (calibrated), checking for destination")
|
||||||
|
|
||||||
|
# Staleness check
|
||||||
|
if time.monotonic() - self.last_gps_time > 2.0:
|
||||||
|
self.gps_ok = False
|
||||||
|
|
||||||
|
def _recompute_route(self):
|
||||||
|
"""Check if we need to recompute route."""
|
||||||
|
# Don't start route until GPS is valid (OK + calibrated)
|
||||||
|
if not self.gps_ok:
|
||||||
|
return
|
||||||
|
|
||||||
|
# Skip if route calculation is already in progress
|
||||||
|
if self._route_calculating:
|
||||||
|
return
|
||||||
|
|
||||||
|
# Check params - immediately on first valid GPS, otherwise every 3 seconds
|
||||||
|
now = time.monotonic()
|
||||||
|
first_gps_check = self._had_first_valid_gps and self.route is None and self._cached_destination is None
|
||||||
|
if first_gps_check or now - self._last_destination_check >= self._destination_check_interval:
|
||||||
|
self._last_destination_check = now
|
||||||
|
self._cached_destination = coordinate_from_param("dp_maa_destination", self.params)
|
||||||
|
|
||||||
|
new_destination = self._cached_destination
|
||||||
|
if new_destination is None:
|
||||||
|
if self.nav_destination is not None or self.route is not None:
|
||||||
|
self._clear_route()
|
||||||
|
self._reset_recompute_limits()
|
||||||
|
return
|
||||||
|
|
||||||
|
should_recompute = self._should_recompute()
|
||||||
|
if should_recompute and self.route is not None:
|
||||||
|
cloudlog.warning(f"maad: reroute triggered, countdown={self.recompute_countdown}, backoff={self.recompute_backoff}")
|
||||||
|
|
||||||
|
# New destination
|
||||||
|
if new_destination != self.nav_destination:
|
||||||
|
cloudlog.warning(f"Got new destination from dp_maa_destination param {new_destination}")
|
||||||
|
self.nav_destination = new_destination
|
||||||
|
should_recompute = True
|
||||||
|
|
||||||
|
# Don't recompute when GPS drifts in tunnels
|
||||||
|
if not self.gps_ok and self.tracker.step_idx is not None:
|
||||||
|
return
|
||||||
|
|
||||||
|
# First route calculation (no existing route) - start immediately without backoff
|
||||||
|
is_first_route = self.route is None and should_recompute
|
||||||
|
if is_first_route or (self.recompute_countdown == 0 and should_recompute):
|
||||||
|
if not is_first_route:
|
||||||
|
self.recompute_countdown = 2 ** self.recompute_backoff
|
||||||
|
self.recompute_backoff = min(3, self.recompute_backoff + 1) # Max 8 second backoff
|
||||||
|
self._start_route_calculation(new_destination)
|
||||||
|
else:
|
||||||
|
self.recompute_countdown = max(0, self.recompute_countdown - 1)
|
||||||
|
|
||||||
|
def _start_route_calculation(self, destination: Coordinate):
|
||||||
|
"""Start async route calculation in a separate thread."""
|
||||||
|
start_pos = self.last_position
|
||||||
|
bearing = self.last_bearing
|
||||||
|
|
||||||
|
self._route_calculating = True
|
||||||
|
self.nav_destination = destination
|
||||||
|
|
||||||
|
cloudlog.info(f"maad: starting async route calculation {start_pos} -> {destination}")
|
||||||
|
|
||||||
|
def calculate():
|
||||||
|
try:
|
||||||
|
result = self._fetch_route(start_pos, destination, bearing)
|
||||||
|
self._route_queue.put(result)
|
||||||
|
except Exception as e:
|
||||||
|
cloudlog.exception(f"maad: route calculation failed: {e}")
|
||||||
|
self._route_queue.put(None)
|
||||||
|
|
||||||
|
self._route_thread = threading.Thread(target=calculate, daemon=True)
|
||||||
|
self._route_thread.start()
|
||||||
|
|
||||||
|
def _fetch_route(self, start: Coordinate, destination: Coordinate,
|
||||||
|
bearing: Optional[float]) -> Optional[tuple]:
|
||||||
|
"""Fetch route using MapService (runs in thread). Returns (route, route_data) or None."""
|
||||||
|
origin = ProviderCoordinate(start.latitude, start.longitude)
|
||||||
|
dest = ProviderCoordinate(destination.latitude, destination.longitude)
|
||||||
|
|
||||||
|
provider_route = self.map_service.route_provider.get_route_sync(
|
||||||
|
origin=origin,
|
||||||
|
destination=dest,
|
||||||
|
bearing=bearing
|
||||||
|
)
|
||||||
|
|
||||||
|
if provider_route is None:
|
||||||
|
cloudlog.warning("maad: route provider returned None")
|
||||||
|
return None
|
||||||
|
|
||||||
|
# Convert provider Route to local Step format
|
||||||
|
# Filter out depart/arrive - merge their geometry with adjacent steps
|
||||||
|
route = []
|
||||||
|
all_coords = [] # Full route geometry for turn angle computation
|
||||||
|
pending_geometry = [] # Geometry from depart to merge with first real step
|
||||||
|
|
||||||
|
for provider_step in provider_route.steps:
|
||||||
|
# Convert provider Coordinates to helper Coordinates
|
||||||
|
geometry = [
|
||||||
|
Coordinate(c.latitude, c.longitude)
|
||||||
|
for c in provider_step.geometry
|
||||||
|
]
|
||||||
|
all_coords.extend(geometry)
|
||||||
|
|
||||||
|
# Skip depart/arrive steps but keep their geometry
|
||||||
|
if provider_step.maneuver_type in ('depart', 'arrive'):
|
||||||
|
if provider_step.maneuver_type == 'depart':
|
||||||
|
pending_geometry = geometry # Save for merging with first real step
|
||||||
|
elif provider_step.maneuver_type == 'arrive' and route:
|
||||||
|
# Merge arrive geometry with last step
|
||||||
|
route[-1].geometry.extend(geometry)
|
||||||
|
route[-1].distance += provider_step.distance
|
||||||
|
route[-1].duration += provider_step.duration
|
||||||
|
continue
|
||||||
|
|
||||||
|
# Merge pending depart geometry with this step
|
||||||
|
if pending_geometry:
|
||||||
|
geometry = pending_geometry + geometry
|
||||||
|
pending_geometry = []
|
||||||
|
|
||||||
|
# Convert provider maneuver_point to helpers Coordinate
|
||||||
|
maneuver_pt = None
|
||||||
|
if provider_step.maneuver_point:
|
||||||
|
maneuver_pt = Coordinate(
|
||||||
|
provider_step.maneuver_point.latitude,
|
||||||
|
provider_step.maneuver_point.longitude
|
||||||
|
)
|
||||||
|
|
||||||
|
route_step = Step(
|
||||||
|
distance=provider_step.distance,
|
||||||
|
duration=provider_step.duration,
|
||||||
|
duration_typical=provider_step.duration_typical or provider_step.duration,
|
||||||
|
name=provider_step.name,
|
||||||
|
maneuver_type=provider_step.maneuver_type,
|
||||||
|
maneuver_modifier=provider_step.maneuver_modifier,
|
||||||
|
geometry=geometry,
|
||||||
|
speed_limit=provider_step.speed_limit,
|
||||||
|
speed_limit_sign=provider_step.speed_limit_sign,
|
||||||
|
maneuver_point=maneuver_pt,
|
||||||
|
)
|
||||||
|
route.append(route_step)
|
||||||
|
|
||||||
|
# Pre-compute turn geometry at each step's maneuver point (end of step)
|
||||||
|
coord_idx = 0
|
||||||
|
for step in route:
|
||||||
|
coord_idx += len(step.geometry)
|
||||||
|
# Turn point is at the end of this step (start of next)
|
||||||
|
turn_idx = min(coord_idx - 1, len(all_coords) - 2)
|
||||||
|
if turn_idx > 0:
|
||||||
|
step.turn_angle = compute_turn_angle_at_index(all_coords, turn_idx)
|
||||||
|
step.turn_curvature = compute_turn_curvature_at_index(all_coords, turn_idx)
|
||||||
|
|
||||||
|
# Build route_data dict for compatibility
|
||||||
|
route_data = {
|
||||||
|
'distance': provider_route.distance,
|
||||||
|
'duration': provider_route.duration,
|
||||||
|
}
|
||||||
|
|
||||||
|
cloudlog.info(f"maad: route from {provider_route.provider} - "
|
||||||
|
f"{provider_route.distance/1000:.1f}km, {len(route)} steps")
|
||||||
|
|
||||||
|
return (route, route_data)
|
||||||
|
|
||||||
|
def _send_instruction(self):
|
||||||
|
"""Send navInstruction message ."""
|
||||||
|
msg = messaging.new_message('navInstruction', valid=True)
|
||||||
|
|
||||||
|
if self.tracker.step_idx is None or self.route is None or self.last_position is None or not self.gps_ok:
|
||||||
|
# Debug: log why we're sending invalid
|
||||||
|
reasons = []
|
||||||
|
if self.tracker.step_idx is None:
|
||||||
|
reasons.append("step_idx=None")
|
||||||
|
if self.route is None:
|
||||||
|
reasons.append("route=None")
|
||||||
|
if self.last_position is None:
|
||||||
|
reasons.append("position=None")
|
||||||
|
if not self.gps_ok:
|
||||||
|
reasons.append(f"gps_ok=False")
|
||||||
|
cloudlog.info(f"maad: sending invalid navInstruction: {', '.join(reasons)}")
|
||||||
|
msg.valid = False
|
||||||
|
self.pm.send('navInstruction', msg)
|
||||||
|
return
|
||||||
|
|
||||||
|
# Sanity check: ensure step_idx is valid
|
||||||
|
if self.tracker.step_idx >= len(self.route):
|
||||||
|
cloudlog.error(f"maad: step_idx {self.tracker.step_idx} >= route length {len(self.route)}, resetting to 0")
|
||||||
|
self.tracker.set_step(0)
|
||||||
|
|
||||||
|
step = self.route[self.tracker.step_idx]
|
||||||
|
geometry = step.geometry
|
||||||
|
|
||||||
|
# Calculate distance along current step geometry
|
||||||
|
along_geometry = distance_along_geometry(geometry, self.last_position)
|
||||||
|
distance_to_maneuver = step.distance - along_geometry
|
||||||
|
|
||||||
|
# Current instruction (depart/arrive already filtered out during route build)
|
||||||
|
msg.navInstruction.maneuverDistance = distance_to_maneuver
|
||||||
|
msg.navInstruction.maneuverPrimaryText = step.name or step.maneuver_type
|
||||||
|
|
||||||
|
# Override maneuver type/modifier based on geometry
|
||||||
|
# Geometry-first: always use turn_angle for direction when significant
|
||||||
|
TURN_MIN_ANGLE = 20.0
|
||||||
|
if abs(step.turn_angle) >= TURN_MIN_ANGLE:
|
||||||
|
# Significant turn - use geometry for both type and direction
|
||||||
|
if step.maneuver_type in ('continue', 'new name'):
|
||||||
|
msg.navInstruction.maneuverType = 'turn'
|
||||||
|
else:
|
||||||
|
msg.navInstruction.maneuverType = step.maneuver_type
|
||||||
|
# Always use geometry-based direction for significant turns
|
||||||
|
msg.navInstruction.maneuverModifier = 'left' if step.turn_angle > 0 else 'right'
|
||||||
|
else:
|
||||||
|
msg.navInstruction.maneuverType = step.maneuver_type
|
||||||
|
msg.navInstruction.maneuverModifier = step.maneuver_modifier
|
||||||
|
|
||||||
|
# Next step's road name (the road to turn onto)
|
||||||
|
if self.tracker.step_idx + 1 < len(self.route):
|
||||||
|
next_step = self.route[self.tracker.step_idx + 1]
|
||||||
|
msg.navInstruction.maneuverSecondaryText = next_step.name or ""
|
||||||
|
|
||||||
|
# Compute total remaining time and distance
|
||||||
|
remaining_ratio = 1.0 - along_geometry / max(step.distance, 1)
|
||||||
|
total_distance = step.distance * remaining_ratio
|
||||||
|
total_time = step.duration * remaining_ratio
|
||||||
|
total_time_typical = (step.duration_typical or step.duration) * remaining_ratio
|
||||||
|
|
||||||
|
for i in range(self.tracker.step_idx + 1, len(self.route)):
|
||||||
|
total_distance += self.route[i].distance
|
||||||
|
total_time += self.route[i].duration
|
||||||
|
total_time_typical += self.route[i].duration_typical or self.route[i].duration
|
||||||
|
|
||||||
|
msg.navInstruction.distanceRemaining = total_distance
|
||||||
|
msg.navInstruction.timeRemaining = total_time
|
||||||
|
msg.navInstruction.timeRemainingTypical = total_time_typical
|
||||||
|
|
||||||
|
# Speed limit from closest coordinate
|
||||||
|
if geometry:
|
||||||
|
closest_idx, _ = find_closest_point_on_route(self.last_position, geometry)
|
||||||
|
if closest_idx < len(geometry):
|
||||||
|
closest = geometry[closest_idx]
|
||||||
|
if 'maxspeed' in closest.annotations and self.gps_ok:
|
||||||
|
msg.navInstruction.speedLimit = closest.annotations['maxspeed']
|
||||||
|
|
||||||
|
if step.speed_limit_sign == 'mutcd':
|
||||||
|
msg.navInstruction.speedLimitSign = log.NavInstruction.SpeedLimitSign.mutcd
|
||||||
|
else:
|
||||||
|
msg.navInstruction.speedLimitSign = log.NavInstruction.SpeedLimitSign.vienna
|
||||||
|
|
||||||
|
self.pm.send('navInstruction', msg)
|
||||||
|
|
||||||
|
# Send extended nav instruction (turn geometry)
|
||||||
|
msg_ext = messaging.new_message('navInstructionExt')
|
||||||
|
msg_ext.navInstructionExt.turnAngle = step.turn_angle
|
||||||
|
msg_ext.navInstructionExt.turnCurvature = step.turn_curvature
|
||||||
|
self.pm.send('navInstructionExt', msg_ext)
|
||||||
|
|
||||||
|
# Transition to next step
|
||||||
|
if self.tracker.update_step(self.route, self.last_position, self.last_bearing, self.gps_speed):
|
||||||
|
self._reset_recompute_limits()
|
||||||
|
|
||||||
|
# Check if arrived at destination
|
||||||
|
if self.nav_destination:
|
||||||
|
dist = self.nav_destination.distance_to(self.last_position)
|
||||||
|
if dist < 30: # Within 30m of destination
|
||||||
|
cloudlog.warning("maad: destination reached")
|
||||||
|
self.params.remove("dp_maa_destination")
|
||||||
|
self._clear_route()
|
||||||
|
|
||||||
|
def send_route(self):
|
||||||
|
"""Send navRoute message for dashy to display route on map."""
|
||||||
|
coords = []
|
||||||
|
|
||||||
|
if self.route is not None:
|
||||||
|
for step in self.route:
|
||||||
|
coords.extend([[c.longitude, c.latitude] for c in step.geometry])
|
||||||
|
|
||||||
|
msg = messaging.new_message('navRoute', valid=True)
|
||||||
|
msg.navRoute.coordinates = [{"longitude": c[0], "latitude": c[1]} for c in coords]
|
||||||
|
self.pm.send('navRoute', msg)
|
||||||
|
|
||||||
|
def _clear_route(self, clear_destination=True):
|
||||||
|
"""Clear navigation state."""
|
||||||
|
self.route = None
|
||||||
|
self.tracker.reset()
|
||||||
|
if clear_destination:
|
||||||
|
self.nav_destination = None
|
||||||
|
|
||||||
|
# Send empty navRoute to clear map display
|
||||||
|
msg = messaging.new_message('navRoute', valid=False)
|
||||||
|
msg.navRoute.coordinates = []
|
||||||
|
self.pm.send('navRoute', msg)
|
||||||
|
|
||||||
|
def _reset_recompute_limits(self):
|
||||||
|
"""Reset recompute backoff and deviation timer."""
|
||||||
|
self.recompute_backoff = 0
|
||||||
|
self.recompute_countdown = 0
|
||||||
|
self.tracker.deviation_start_time = None # Reset OsmAnd-style deviation timer
|
||||||
|
|
||||||
|
def _should_recompute(self) -> bool:
|
||||||
|
"""Check if route should be recomputed (delegates to RouteTracker)."""
|
||||||
|
if self.route is None:
|
||||||
|
return True
|
||||||
|
return self.tracker.should_reroute(self.route, self.last_position, self.gps_accuracy)
|
||||||
|
|
||||||
|
|
||||||
|
def main():
|
||||||
|
cloudlog.info("maad starting")
|
||||||
|
|
||||||
|
pm = messaging.PubMaster(['navInstruction', 'navInstructionExt', 'navRoute'])
|
||||||
|
sm = messaging.SubMaster(['liveGPS'], ignore_alive=['liveGPS'])
|
||||||
|
|
||||||
|
rk = Ratekeeper(NAV_RATE)
|
||||||
|
route_engine = RouteEngine(sm, pm)
|
||||||
|
|
||||||
|
while True:
|
||||||
|
try:
|
||||||
|
route_engine.update()
|
||||||
|
except Exception:
|
||||||
|
cloudlog.exception("maad: error in main loop")
|
||||||
|
rk.keep_time()
|
||||||
|
|
||||||
|
|
||||||
|
if __name__ == "__main__":
|
||||||
|
main()
|
||||||
@@ -0,0 +1,54 @@
|
|||||||
|
"""
|
||||||
|
Copyright (c) 2026, Rick Lan
|
||||||
|
|
||||||
|
Permission is hereby granted, free of charge, to any person obtaining a copy
|
||||||
|
of this software and associated documentation files (the "Software"), to deal
|
||||||
|
in the Software without restriction, including without limitation the rights
|
||||||
|
to use, copy, modify, merge, publish, distribute, and/or sublicense,
|
||||||
|
for non-commercial purposes only, subject to the following conditions:
|
||||||
|
|
||||||
|
- The above copyright notice and this permission notice shall be included in
|
||||||
|
all copies or substantial portions of the Software.
|
||||||
|
- Commercial use (e.g. use in a product, service, or activity intended to
|
||||||
|
generate revenue) is prohibited without explicit written permission from
|
||||||
|
the copyright holder.
|
||||||
|
|
||||||
|
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED,
|
||||||
|
INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A
|
||||||
|
PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
|
||||||
|
HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
|
||||||
|
OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
|
||||||
|
SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
|
||||||
|
|
||||||
|
Map Provider Abstraction Layer
|
||||||
|
|
||||||
|
Uses api.dragonpilot.org for search and routing with device JWT authentication.
|
||||||
|
Falls back to free providers (Photon/OSRM) when not authenticated.
|
||||||
|
|
||||||
|
Usage:
|
||||||
|
from dragonpilot.dashy.maa.providers import MapService
|
||||||
|
|
||||||
|
map_service = MapService()
|
||||||
|
route = map_service.route_provider.get_route_sync(origin, dest)
|
||||||
|
results = await map_service.search_provider.search("Taipei 101")
|
||||||
|
"""
|
||||||
|
|
||||||
|
from .map_service import MapService
|
||||||
|
from .models import Coordinate, SearchResult, Route, Step, TileConfig
|
||||||
|
from .base import SearchProvider, RouteProvider, TileProvider
|
||||||
|
from .dragonpilot import DragonpilotSearchProvider, DragonpilotRouteProvider, DragonpilotApiClient
|
||||||
|
|
||||||
|
__all__ = [
|
||||||
|
'MapService',
|
||||||
|
'Coordinate',
|
||||||
|
'SearchResult',
|
||||||
|
'Route',
|
||||||
|
'Step',
|
||||||
|
'TileConfig',
|
||||||
|
'SearchProvider',
|
||||||
|
'RouteProvider',
|
||||||
|
'TileProvider',
|
||||||
|
'DragonpilotSearchProvider',
|
||||||
|
'DragonpilotRouteProvider',
|
||||||
|
'DragonpilotApiClient',
|
||||||
|
]
|
||||||
@@ -0,0 +1,177 @@
|
|||||||
|
"""
|
||||||
|
Copyright (c) 2026, Rick Lan
|
||||||
|
|
||||||
|
Permission is hereby granted, free of charge, to any person obtaining a copy
|
||||||
|
of this software and associated documentation files (the "Software"), to deal
|
||||||
|
in the Software without restriction, including without limitation the rights
|
||||||
|
to use, copy, modify, merge, publish, distribute, and/or sublicense,
|
||||||
|
for non-commercial purposes only, subject to the following conditions:
|
||||||
|
|
||||||
|
- The above copyright notice and this permission notice shall be included in
|
||||||
|
all copies or substantial portions of the Software.
|
||||||
|
- Commercial use (e.g. use in a product, service, or activity intended to
|
||||||
|
generate revenue) is prohibited without explicit written permission from
|
||||||
|
the copyright holder.
|
||||||
|
|
||||||
|
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED,
|
||||||
|
INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A
|
||||||
|
PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
|
||||||
|
HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
|
||||||
|
OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
|
||||||
|
SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
|
||||||
|
|
||||||
|
Abstract base classes for map providers.
|
||||||
|
|
||||||
|
These interfaces define the contract that all provider implementations must follow.
|
||||||
|
"""
|
||||||
|
|
||||||
|
from abc import ABC, abstractmethod
|
||||||
|
from typing import Optional
|
||||||
|
import asyncio
|
||||||
|
|
||||||
|
from .models import Coordinate, SearchResult, Route, TileConfig
|
||||||
|
|
||||||
|
|
||||||
|
class SearchProvider(ABC):
|
||||||
|
"""
|
||||||
|
Abstract search/geocoding provider.
|
||||||
|
|
||||||
|
Implementations: PhotonSearchProvider, MapboxSearchProvider, etc.
|
||||||
|
"""
|
||||||
|
|
||||||
|
name: str = "base"
|
||||||
|
requires_api_key: bool = False
|
||||||
|
|
||||||
|
@abstractmethod
|
||||||
|
async def search(
|
||||||
|
self,
|
||||||
|
query: str,
|
||||||
|
proximity: Optional[Coordinate] = None,
|
||||||
|
limit: int = 10
|
||||||
|
) -> list[SearchResult]:
|
||||||
|
"""
|
||||||
|
Search for places by query string.
|
||||||
|
|
||||||
|
Args:
|
||||||
|
query: Search query (address, place name, etc.)
|
||||||
|
proximity: Optional coordinate to bias results toward
|
||||||
|
limit: Maximum number of results
|
||||||
|
|
||||||
|
Returns:
|
||||||
|
List of SearchResult objects sorted by relevance/distance
|
||||||
|
"""
|
||||||
|
pass
|
||||||
|
|
||||||
|
@abstractmethod
|
||||||
|
async def reverse_geocode(
|
||||||
|
self,
|
||||||
|
coord: Coordinate
|
||||||
|
) -> Optional[SearchResult]:
|
||||||
|
"""
|
||||||
|
Get address/place information from coordinates.
|
||||||
|
|
||||||
|
Args:
|
||||||
|
coord: Coordinate to reverse geocode
|
||||||
|
|
||||||
|
Returns:
|
||||||
|
SearchResult with address info, or None if not found
|
||||||
|
"""
|
||||||
|
pass
|
||||||
|
|
||||||
|
def search_sync(
|
||||||
|
self,
|
||||||
|
query: str,
|
||||||
|
proximity: Optional[Coordinate] = None,
|
||||||
|
limit: int = 10
|
||||||
|
) -> list[SearchResult]:
|
||||||
|
"""Synchronous wrapper for search()."""
|
||||||
|
loop = asyncio.new_event_loop()
|
||||||
|
try:
|
||||||
|
return loop.run_until_complete(self.search(query, proximity, limit))
|
||||||
|
finally:
|
||||||
|
loop.close()
|
||||||
|
|
||||||
|
|
||||||
|
class RouteProvider(ABC):
|
||||||
|
"""
|
||||||
|
Abstract routing provider.
|
||||||
|
|
||||||
|
Implementations: OSRMRouteProvider, MapboxRouteProvider, etc.
|
||||||
|
"""
|
||||||
|
|
||||||
|
name: str = "base"
|
||||||
|
requires_api_key: bool = False
|
||||||
|
supports_traffic: bool = False
|
||||||
|
|
||||||
|
@abstractmethod
|
||||||
|
async def get_route(
|
||||||
|
self,
|
||||||
|
origin: Coordinate,
|
||||||
|
destination: Coordinate,
|
||||||
|
waypoints: Optional[list[Coordinate]] = None,
|
||||||
|
bearing: Optional[float] = None
|
||||||
|
) -> Optional[Route]:
|
||||||
|
"""
|
||||||
|
Calculate route between points.
|
||||||
|
|
||||||
|
Args:
|
||||||
|
origin: Starting coordinate
|
||||||
|
destination: Ending coordinate
|
||||||
|
waypoints: Optional intermediate waypoints
|
||||||
|
bearing: Optional current heading in degrees (for better route start)
|
||||||
|
|
||||||
|
Returns:
|
||||||
|
Route object with steps and geometry, or None if routing fails
|
||||||
|
"""
|
||||||
|
pass
|
||||||
|
|
||||||
|
def get_route_sync(
|
||||||
|
self,
|
||||||
|
origin: Coordinate,
|
||||||
|
destination: Coordinate,
|
||||||
|
waypoints: Optional[list[Coordinate]] = None,
|
||||||
|
bearing: Optional[float] = None
|
||||||
|
) -> Optional[Route]:
|
||||||
|
"""
|
||||||
|
Synchronous wrapper for get_route().
|
||||||
|
|
||||||
|
Use this in maad.py where async is not available.
|
||||||
|
"""
|
||||||
|
loop = asyncio.new_event_loop()
|
||||||
|
try:
|
||||||
|
return loop.run_until_complete(
|
||||||
|
self.get_route(origin, destination, waypoints, bearing)
|
||||||
|
)
|
||||||
|
finally:
|
||||||
|
loop.close()
|
||||||
|
|
||||||
|
|
||||||
|
class TileProvider(ABC):
|
||||||
|
"""
|
||||||
|
Abstract map tile provider.
|
||||||
|
|
||||||
|
Implementations: OpenFreeMapTileProvider, MapboxTileProvider, etc.
|
||||||
|
"""
|
||||||
|
|
||||||
|
name: str = "base"
|
||||||
|
requires_api_key: bool = False
|
||||||
|
|
||||||
|
@abstractmethod
|
||||||
|
def get_tile_config(self) -> TileConfig:
|
||||||
|
"""
|
||||||
|
Get tile URL template and configuration.
|
||||||
|
|
||||||
|
Returns:
|
||||||
|
TileConfig with URL template and attribution
|
||||||
|
"""
|
||||||
|
pass
|
||||||
|
|
||||||
|
@abstractmethod
|
||||||
|
def get_style_json(self) -> dict:
|
||||||
|
"""
|
||||||
|
Get MapLibre GL style JSON for this provider.
|
||||||
|
|
||||||
|
Returns:
|
||||||
|
Style JSON dict for MapLibre GL JS
|
||||||
|
"""
|
||||||
|
pass
|
||||||
@@ -0,0 +1,45 @@
|
|||||||
|
"""
|
||||||
|
Copyright (c) 2026, Rick Lan
|
||||||
|
|
||||||
|
Permission is hereby granted, free of charge, to any person obtaining a copy
|
||||||
|
of this software and associated documentation files (the "Software"), to deal
|
||||||
|
in the Software without restriction, including without limitation the rights
|
||||||
|
to use, copy, modify, merge, publish, distribute, and/or sublicense,
|
||||||
|
for non-commercial purposes only, subject to the following conditions:
|
||||||
|
|
||||||
|
- The above copyright notice and this permission notice shall be included in
|
||||||
|
all copies or substantial portions of the Software.
|
||||||
|
- Commercial use (e.g. use in a product, service, or activity intended to
|
||||||
|
generate revenue) is prohibited without explicit written permission from
|
||||||
|
the copyright holder.
|
||||||
|
|
||||||
|
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED,
|
||||||
|
INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A
|
||||||
|
PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
|
||||||
|
HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
|
||||||
|
OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
|
||||||
|
SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
|
||||||
|
|
||||||
|
Configuration for map providers.
|
||||||
|
|
||||||
|
Uses api.dragonpilot.org for search and routing with device serial authentication.
|
||||||
|
Falls back to free providers (Photon/OSRM) when not authenticated.
|
||||||
|
All providers use WGS-84 coordinates (standard GPS).
|
||||||
|
"""
|
||||||
|
|
||||||
|
from dataclasses import dataclass
|
||||||
|
|
||||||
|
|
||||||
|
@dataclass
|
||||||
|
class ProviderConfig:
|
||||||
|
"""
|
||||||
|
Provider configuration.
|
||||||
|
|
||||||
|
Uses Dragonpilot API with automatic fallback to free providers.
|
||||||
|
All providers use WGS-84 coordinates (standard GPS).
|
||||||
|
"""
|
||||||
|
|
||||||
|
@classmethod
|
||||||
|
def from_params(cls, params) -> 'ProviderConfig':
|
||||||
|
"""Load configuration from openpilot Params."""
|
||||||
|
return cls()
|
||||||
@@ -0,0 +1,32 @@
|
|||||||
|
"""
|
||||||
|
Copyright (c) 2026, Rick Lan
|
||||||
|
|
||||||
|
Permission is hereby granted, free of charge, to any person obtaining a copy
|
||||||
|
of this software and associated documentation files (the "Software"), to deal
|
||||||
|
in the Software without restriction, including without limitation the rights
|
||||||
|
to use, copy, modify, merge, publish, distribute, and/or sublicense,
|
||||||
|
for non-commercial purposes only, subject to the following conditions:
|
||||||
|
|
||||||
|
- The above copyright notice and this permission notice shall be included in
|
||||||
|
all copies or substantial portions of the Software.
|
||||||
|
- Commercial use (e.g. use in a product, service, or activity intended to
|
||||||
|
generate revenue) is prohibited without explicit written permission from
|
||||||
|
the copyright holder.
|
||||||
|
|
||||||
|
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED,
|
||||||
|
INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A
|
||||||
|
PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
|
||||||
|
HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
|
||||||
|
OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
|
||||||
|
SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
|
||||||
|
|
||||||
|
Dragonpilot API Provider
|
||||||
|
|
||||||
|
Uses api.dragonpilot.org for search and routing with device JWT authentication.
|
||||||
|
"""
|
||||||
|
|
||||||
|
from .client import DragonpilotApiClient
|
||||||
|
from .search import DragonpilotSearchProvider
|
||||||
|
from .routing import DragonpilotRouteProvider
|
||||||
|
|
||||||
|
__all__ = ['DragonpilotApiClient', 'DragonpilotSearchProvider', 'DragonpilotRouteProvider']
|
||||||
@@ -0,0 +1,123 @@
|
|||||||
|
"""
|
||||||
|
Copyright (c) 2026, Rick Lan
|
||||||
|
|
||||||
|
Permission is hereby granted, free of charge, to any person obtaining a copy
|
||||||
|
of this software and associated documentation files (the "Software"), to deal
|
||||||
|
in the Software without restriction, including without limitation the rights
|
||||||
|
to use, copy, modify, merge, publish, distribute, and/or sublicense,
|
||||||
|
for non-commercial purposes only, subject to the following conditions:
|
||||||
|
|
||||||
|
- The above copyright notice and this permission notice shall be included in
|
||||||
|
all copies or substantial portions of the Software.
|
||||||
|
- Commercial use (e.g. use in a product, service, or activity intended to
|
||||||
|
generate revenue) is prohibited without explicit written permission from
|
||||||
|
the copyright holder.
|
||||||
|
|
||||||
|
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED,
|
||||||
|
INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A
|
||||||
|
PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
|
||||||
|
HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
|
||||||
|
OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
|
||||||
|
SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
|
||||||
|
|
||||||
|
Dragonpilot API Client
|
||||||
|
|
||||||
|
Simple HTTP client using device serial for authentication.
|
||||||
|
"""
|
||||||
|
|
||||||
|
import os
|
||||||
|
from typing import Optional
|
||||||
|
import aiohttp
|
||||||
|
import requests
|
||||||
|
|
||||||
|
from openpilot.system.hardware import HARDWARE
|
||||||
|
|
||||||
|
API_HOST = os.getenv('DRAGONPILOT_API_HOST', 'https://api.dragonpilot.org')
|
||||||
|
|
||||||
|
# Module-level serial cache - queried once from HARDWARE
|
||||||
|
_serial: Optional[str] = None
|
||||||
|
|
||||||
|
|
||||||
|
def _get_serial() -> Optional[str]:
|
||||||
|
"""Get device serial (cached)."""
|
||||||
|
global _serial
|
||||||
|
if _serial is None:
|
||||||
|
try:
|
||||||
|
_serial = HARDWARE.get_serial()
|
||||||
|
except Exception:
|
||||||
|
pass
|
||||||
|
return _serial
|
||||||
|
|
||||||
|
|
||||||
|
class DragonpilotApiClient:
|
||||||
|
"""
|
||||||
|
API client for api.dragonpilot.org.
|
||||||
|
|
||||||
|
Uses device serial from HARDWARE for authentication.
|
||||||
|
"""
|
||||||
|
|
||||||
|
def __init__(self, serial: str = None):
|
||||||
|
self.api_host = API_HOST
|
||||||
|
self.serial = serial if serial is not None else _get_serial()
|
||||||
|
|
||||||
|
@property
|
||||||
|
def is_authenticated(self) -> bool:
|
||||||
|
return self.serial is not None
|
||||||
|
|
||||||
|
def _headers(self) -> dict:
|
||||||
|
headers = {'Content-Type': 'application/json'}
|
||||||
|
if self.serial:
|
||||||
|
headers['X-Device-Serial'] = self.serial
|
||||||
|
return headers
|
||||||
|
|
||||||
|
def get_sync(self, endpoint: str, params: dict = None, timeout: int = 10) -> Optional[dict]:
|
||||||
|
try:
|
||||||
|
resp = requests.get(f"{self.api_host}{endpoint}", params=params, headers=self._headers(), timeout=timeout)
|
||||||
|
return resp.json() if resp.status_code == 200 else None
|
||||||
|
except Exception:
|
||||||
|
return None
|
||||||
|
|
||||||
|
def post_sync(self, endpoint: str, data: dict = None, timeout: int = 10) -> Optional[dict]:
|
||||||
|
try:
|
||||||
|
resp = requests.post(f"{self.api_host}{endpoint}", json=data, headers=self._headers(), timeout=timeout)
|
||||||
|
return resp.json() if resp.status_code == 200 else None
|
||||||
|
except Exception:
|
||||||
|
return None
|
||||||
|
|
||||||
|
async def get(self, endpoint: str, params: dict = None, timeout: int = 10) -> Optional[dict]:
|
||||||
|
try:
|
||||||
|
async with aiohttp.ClientSession() as session:
|
||||||
|
async with session.get(
|
||||||
|
f"{self.api_host}{endpoint}",
|
||||||
|
params=params,
|
||||||
|
headers=self._headers(),
|
||||||
|
timeout=aiohttp.ClientTimeout(total=timeout)
|
||||||
|
) as resp:
|
||||||
|
return await resp.json() if resp.status == 200 else None
|
||||||
|
except Exception:
|
||||||
|
return None
|
||||||
|
|
||||||
|
async def post(self, endpoint: str, data: dict = None, timeout: int = 10) -> Optional[dict]:
|
||||||
|
try:
|
||||||
|
async with aiohttp.ClientSession() as session:
|
||||||
|
async with session.post(
|
||||||
|
f"{self.api_host}{endpoint}",
|
||||||
|
json=data,
|
||||||
|
headers=self._headers(),
|
||||||
|
timeout=aiohttp.ClientTimeout(total=timeout)
|
||||||
|
) as resp:
|
||||||
|
return await resp.json() if resp.status == 200 else None
|
||||||
|
except Exception:
|
||||||
|
return None
|
||||||
|
|
||||||
|
|
||||||
|
# Singleton client instance
|
||||||
|
_client: Optional[DragonpilotApiClient] = None
|
||||||
|
|
||||||
|
|
||||||
|
def get_client() -> DragonpilotApiClient:
|
||||||
|
"""Get the shared API client instance."""
|
||||||
|
global _client
|
||||||
|
if _client is None:
|
||||||
|
_client = DragonpilotApiClient()
|
||||||
|
return _client
|
||||||
@@ -0,0 +1,269 @@
|
|||||||
|
"""
|
||||||
|
Copyright (c) 2026, Rick Lan
|
||||||
|
|
||||||
|
Permission is hereby granted, free of charge, to any person obtaining a copy
|
||||||
|
of this software and associated documentation files (the "Software"), to deal
|
||||||
|
in the Software without restriction, including without limitation the rights
|
||||||
|
to use, copy, modify, merge, publish, distribute, and/or sublicense,
|
||||||
|
for non-commercial purposes only, subject to the following conditions:
|
||||||
|
|
||||||
|
- The above copyright notice and this permission notice shall be included in
|
||||||
|
all copies or substantial portions of the Software.
|
||||||
|
- Commercial use (e.g. use in a product, service, or activity intended to
|
||||||
|
generate revenue) is prohibited without explicit written permission from
|
||||||
|
the copyright holder.
|
||||||
|
|
||||||
|
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED,
|
||||||
|
INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A
|
||||||
|
PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
|
||||||
|
HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
|
||||||
|
OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
|
||||||
|
SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
|
||||||
|
|
||||||
|
Dragonpilot Routing Provider
|
||||||
|
|
||||||
|
Uses api.dragonpilot.org routing endpoint with device serial authentication.
|
||||||
|
Falls back to OSRM if not authenticated or on error.
|
||||||
|
"""
|
||||||
|
|
||||||
|
from typing import Optional
|
||||||
|
|
||||||
|
from openpilot.common.swaglog import cloudlog
|
||||||
|
|
||||||
|
from ..base import RouteProvider
|
||||||
|
from ..models import Coordinate, Route, Step
|
||||||
|
from .client import get_client
|
||||||
|
|
||||||
|
|
||||||
|
def decode_polyline(polyline: str) -> list[tuple[float, float]]:
|
||||||
|
"""
|
||||||
|
Decode a Google/HERE Encoded Polyline into (lat, lon) tuples.
|
||||||
|
|
||||||
|
Algorithm: https://developers.google.com/maps/documentation/utilities/polylinealgorithm
|
||||||
|
"""
|
||||||
|
coordinates = []
|
||||||
|
index = 0
|
||||||
|
lat = 0
|
||||||
|
lng = 0
|
||||||
|
|
||||||
|
while index < len(polyline):
|
||||||
|
# Decode latitude
|
||||||
|
result = 0
|
||||||
|
shift = 0
|
||||||
|
while True:
|
||||||
|
b = ord(polyline[index]) - 63
|
||||||
|
index += 1
|
||||||
|
result |= (b & 0x1f) << shift
|
||||||
|
shift += 5
|
||||||
|
if b < 0x20:
|
||||||
|
break
|
||||||
|
lat += (~(result >> 1) if result & 1 else result >> 1)
|
||||||
|
|
||||||
|
# Decode longitude
|
||||||
|
result = 0
|
||||||
|
shift = 0
|
||||||
|
while True:
|
||||||
|
b = ord(polyline[index]) - 63
|
||||||
|
index += 1
|
||||||
|
result |= (b & 0x1f) << shift
|
||||||
|
shift += 5
|
||||||
|
if b < 0x20:
|
||||||
|
break
|
||||||
|
lng += (~(result >> 1) if result & 1 else result >> 1)
|
||||||
|
|
||||||
|
coordinates.append((lat / 1e5, lng / 1e5))
|
||||||
|
|
||||||
|
return coordinates
|
||||||
|
|
||||||
|
|
||||||
|
# Map action to OSRM-style maneuver type/modifier
|
||||||
|
ACTION_MAP = {
|
||||||
|
'depart': ('depart', ''),
|
||||||
|
'arrive': ('arrive', ''),
|
||||||
|
'turn': ('turn', ''),
|
||||||
|
'turn-left': ('turn', 'left'),
|
||||||
|
'turn-right': ('turn', 'right'),
|
||||||
|
'turn-slight-left': ('turn', 'slight left'),
|
||||||
|
'turn-slight-right': ('turn', 'slight right'),
|
||||||
|
'turn-sharp-left': ('turn', 'sharp left'),
|
||||||
|
'turn-sharp-right': ('turn', 'sharp right'),
|
||||||
|
'continue': ('continue', 'straight'),
|
||||||
|
'keep': ('continue', ''),
|
||||||
|
'merge': ('merge', ''),
|
||||||
|
'roundabout': ('roundabout', ''),
|
||||||
|
'roundaboutExit': ('roundabout', 'exit'),
|
||||||
|
'ferry': ('ferry', ''),
|
||||||
|
'uturn': ('turn', 'uturn'),
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
class DragonpilotRouteProvider(RouteProvider):
|
||||||
|
"""
|
||||||
|
Dragonpilot API routing provider.
|
||||||
|
|
||||||
|
Uses device serial for authentication. Falls back to OSRM on auth failure.
|
||||||
|
|
||||||
|
API Response format:
|
||||||
|
{
|
||||||
|
"route": {
|
||||||
|
"route_id": "...",
|
||||||
|
"distance_m": 6837,
|
||||||
|
"duration_s": 759,
|
||||||
|
"duration_traffic_s": 1595,
|
||||||
|
"polyline": "...",
|
||||||
|
"maneuvers": [{
|
||||||
|
"instruction": "Turn left onto Main Street",
|
||||||
|
"distance_m": 500,
|
||||||
|
"duration_s": 60,
|
||||||
|
"position": {"lat": 25.03, "lon": 121.56},
|
||||||
|
"action": "turn"
|
||||||
|
}],
|
||||||
|
"provider": "here"
|
||||||
|
},
|
||||||
|
"cached": false,
|
||||||
|
"provider": "here"
|
||||||
|
}
|
||||||
|
"""
|
||||||
|
|
||||||
|
name = "dragonpilot"
|
||||||
|
requires_api_key = False
|
||||||
|
supports_traffic = True
|
||||||
|
|
||||||
|
def __init__(self):
|
||||||
|
self._client = get_client()
|
||||||
|
self._fallback = None
|
||||||
|
|
||||||
|
def _get_fallback(self) -> RouteProvider:
|
||||||
|
"""Get fallback OSRM provider."""
|
||||||
|
if self._fallback is None:
|
||||||
|
from ..routing.osrm import OSRMRouteProvider
|
||||||
|
self._fallback = OSRMRouteProvider()
|
||||||
|
return self._fallback
|
||||||
|
|
||||||
|
async def get_route(
|
||||||
|
self,
|
||||||
|
origin: Coordinate,
|
||||||
|
destination: Coordinate,
|
||||||
|
waypoints: Optional[list[Coordinate]] = None,
|
||||||
|
bearing: Optional[float] = None
|
||||||
|
) -> Optional[Route]:
|
||||||
|
"""Calculate route using Dragonpilot API (async)."""
|
||||||
|
if not self._client.is_authenticated:
|
||||||
|
cloudlog.warning("dragonpilot routing: no serial, using osrm fallback")
|
||||||
|
return await self._get_fallback().get_route(origin, destination, waypoints, bearing)
|
||||||
|
|
||||||
|
data = self._build_request(origin, destination, waypoints)
|
||||||
|
response = await self._client.post('/v1/route', data=data, timeout=30)
|
||||||
|
|
||||||
|
if response is None:
|
||||||
|
cloudlog.warning("dragonpilot routing: API error, using osrm fallback")
|
||||||
|
return await self._get_fallback().get_route(origin, destination, waypoints, bearing)
|
||||||
|
|
||||||
|
return self._parse_response(response)
|
||||||
|
|
||||||
|
def get_route_sync(
|
||||||
|
self,
|
||||||
|
origin: Coordinate,
|
||||||
|
destination: Coordinate,
|
||||||
|
waypoints: Optional[list[Coordinate]] = None,
|
||||||
|
bearing: Optional[float] = None
|
||||||
|
) -> Optional[Route]:
|
||||||
|
"""Calculate route using Dragonpilot API (synchronous)."""
|
||||||
|
if not self._client.is_authenticated:
|
||||||
|
cloudlog.warning("dragonpilot routing: no serial, using osrm fallback")
|
||||||
|
return self._get_fallback().get_route_sync(origin, destination, waypoints, bearing)
|
||||||
|
|
||||||
|
data = self._build_request(origin, destination, waypoints)
|
||||||
|
response = self._client.post_sync('/v1/route', data=data, timeout=30)
|
||||||
|
|
||||||
|
if response is None:
|
||||||
|
cloudlog.warning("dragonpilot routing: API error, using osrm fallback")
|
||||||
|
return self._get_fallback().get_route_sync(origin, destination, waypoints, bearing)
|
||||||
|
|
||||||
|
return self._parse_response(response)
|
||||||
|
|
||||||
|
def _build_request(
|
||||||
|
self,
|
||||||
|
origin: Coordinate,
|
||||||
|
destination: Coordinate,
|
||||||
|
waypoints: Optional[list[Coordinate]]
|
||||||
|
) -> dict:
|
||||||
|
"""Build API request body."""
|
||||||
|
origin = origin.to_wgs84() if hasattr(origin, 'to_wgs84') else origin
|
||||||
|
destination = destination.to_wgs84() if hasattr(destination, 'to_wgs84') else destination
|
||||||
|
|
||||||
|
data = {
|
||||||
|
'origin': {'lat': origin.latitude, 'lon': origin.longitude},
|
||||||
|
'destination': {'lat': destination.latitude, 'lon': destination.longitude},
|
||||||
|
}
|
||||||
|
|
||||||
|
if waypoints:
|
||||||
|
data['waypoints'] = []
|
||||||
|
for wp in waypoints:
|
||||||
|
wp = wp.to_wgs84() if hasattr(wp, 'to_wgs84') else wp
|
||||||
|
data['waypoints'].append({'lat': wp.latitude, 'lon': wp.longitude})
|
||||||
|
|
||||||
|
return data
|
||||||
|
|
||||||
|
def _parse_response(self, data: dict) -> Optional[Route]:
|
||||||
|
"""Parse API response into Route object."""
|
||||||
|
if not data:
|
||||||
|
return None
|
||||||
|
|
||||||
|
# Unwrap route object
|
||||||
|
route_data = data.get('route', data)
|
||||||
|
|
||||||
|
# Decode full route geometry
|
||||||
|
full_geometry = []
|
||||||
|
polyline = route_data.get('polyline', '')
|
||||||
|
if polyline:
|
||||||
|
try:
|
||||||
|
for lat, lon in decode_polyline(polyline):
|
||||||
|
coord = Coordinate(lat, lon)
|
||||||
|
full_geometry.append(coord)
|
||||||
|
except Exception:
|
||||||
|
pass
|
||||||
|
|
||||||
|
# Parse maneuvers into steps
|
||||||
|
steps = []
|
||||||
|
maneuvers = route_data.get('maneuvers', [])
|
||||||
|
|
||||||
|
for maneuver in maneuvers:
|
||||||
|
pos = maneuver.get('position', {})
|
||||||
|
action = maneuver.get('action', 'continue')
|
||||||
|
|
||||||
|
# Map action to maneuver type/modifier
|
||||||
|
maneuver_type, maneuver_modifier = ACTION_MAP.get(action, ('continue', ''))
|
||||||
|
|
||||||
|
# Get maneuver point
|
||||||
|
maneuver_point = None
|
||||||
|
if pos.get('lat') is not None and pos.get('lon') is not None:
|
||||||
|
maneuver_point = Coordinate(pos['lat'], pos['lon'])
|
||||||
|
|
||||||
|
# Use full instruction text as the step name
|
||||||
|
instruction = maneuver.get('instruction', '')
|
||||||
|
|
||||||
|
steps.append(Step(
|
||||||
|
distance=maneuver.get('distance_m', 0),
|
||||||
|
duration=maneuver.get('duration_s', 0),
|
||||||
|
name=instruction,
|
||||||
|
maneuver_type=maneuver_type,
|
||||||
|
maneuver_modifier=maneuver_modifier,
|
||||||
|
geometry=[maneuver_point] if maneuver_point else [],
|
||||||
|
speed_limit=None,
|
||||||
|
speed_limit_sign='vienna',
|
||||||
|
maneuver_point=maneuver_point,
|
||||||
|
))
|
||||||
|
|
||||||
|
# Use traffic duration if available
|
||||||
|
duration = route_data.get('duration_traffic_s') or route_data.get('duration_s', 0)
|
||||||
|
|
||||||
|
return Route(
|
||||||
|
steps=steps,
|
||||||
|
distance=route_data.get('distance_m', 0),
|
||||||
|
duration=duration,
|
||||||
|
geometry=full_geometry,
|
||||||
|
provider=self.name,
|
||||||
|
has_traffic=route_data.get('duration_traffic_s') is not None,
|
||||||
|
raw=data,
|
||||||
|
)
|
||||||
@@ -0,0 +1,191 @@
|
|||||||
|
"""
|
||||||
|
Copyright (c) 2026, Rick Lan
|
||||||
|
|
||||||
|
Permission is hereby granted, free of charge, to any person obtaining a copy
|
||||||
|
of this software and associated documentation files (the "Software"), to deal
|
||||||
|
in the Software without restriction, including without limitation the rights
|
||||||
|
to use, copy, modify, merge, publish, distribute, and/or sublicense,
|
||||||
|
for non-commercial purposes only, subject to the following conditions:
|
||||||
|
|
||||||
|
- The above copyright notice and this permission notice shall be included in
|
||||||
|
all copies or substantial portions of the Software.
|
||||||
|
- Commercial use (e.g. use in a product, service, or activity intended to
|
||||||
|
generate revenue) is prohibited without explicit written permission from
|
||||||
|
the copyright holder.
|
||||||
|
|
||||||
|
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED,
|
||||||
|
INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A
|
||||||
|
PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
|
||||||
|
HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
|
||||||
|
OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
|
||||||
|
SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
|
||||||
|
|
||||||
|
Dragonpilot Search Provider
|
||||||
|
|
||||||
|
Uses api.dragonpilot.org geocoding endpoint for address search.
|
||||||
|
Falls back to Photon if not authenticated or on error.
|
||||||
|
"""
|
||||||
|
|
||||||
|
from typing import Optional
|
||||||
|
|
||||||
|
from openpilot.common.swaglog import cloudlog
|
||||||
|
|
||||||
|
from ..base import SearchProvider
|
||||||
|
from ..models import Coordinate, SearchResult
|
||||||
|
from .client import get_client
|
||||||
|
|
||||||
|
|
||||||
|
class DragonpilotSearchProvider(SearchProvider):
|
||||||
|
"""
|
||||||
|
Dragonpilot API search provider.
|
||||||
|
|
||||||
|
Uses device serial for authentication. Falls back to Photon on auth failure.
|
||||||
|
|
||||||
|
API Response format:
|
||||||
|
{
|
||||||
|
"results": [{
|
||||||
|
"id": "here:...",
|
||||||
|
"title": "Taipei 101",
|
||||||
|
"address": "No. 7, Section 5, Xinyi Road",
|
||||||
|
"position": {"lat": 25.033, "lon": 121.565},
|
||||||
|
"category": "landmark",
|
||||||
|
"distance_m": 150
|
||||||
|
}],
|
||||||
|
"provider": "here"
|
||||||
|
}
|
||||||
|
"""
|
||||||
|
|
||||||
|
name = "dragonpilot"
|
||||||
|
requires_api_key = False
|
||||||
|
|
||||||
|
def __init__(self):
|
||||||
|
self._client = get_client()
|
||||||
|
self._fallback = None
|
||||||
|
|
||||||
|
def _get_fallback(self) -> SearchProvider:
|
||||||
|
"""Get fallback Photon provider."""
|
||||||
|
if self._fallback is None:
|
||||||
|
from ..search.photon import PhotonSearchProvider
|
||||||
|
self._fallback = PhotonSearchProvider()
|
||||||
|
return self._fallback
|
||||||
|
|
||||||
|
async def search(
|
||||||
|
self,
|
||||||
|
query: str,
|
||||||
|
proximity: Optional[Coordinate] = None,
|
||||||
|
limit: int = 10
|
||||||
|
) -> list[SearchResult]:
|
||||||
|
"""Search for places using Dragonpilot API (async)."""
|
||||||
|
if not query or len(query) < 1:
|
||||||
|
return []
|
||||||
|
|
||||||
|
# Fall back to Photon if not authenticated
|
||||||
|
if not self._client.is_authenticated:
|
||||||
|
cloudlog.debug("dragonpilot search: no serial, using photon fallback")
|
||||||
|
return await self._get_fallback().search(query, proximity, limit)
|
||||||
|
|
||||||
|
params = {'q': query, 'limit': min(limit, 10)}
|
||||||
|
|
||||||
|
if proximity:
|
||||||
|
prox = proximity.to_wgs84() if hasattr(proximity, 'to_wgs84') else proximity
|
||||||
|
params['lat'] = prox.latitude
|
||||||
|
params['lon'] = prox.longitude
|
||||||
|
|
||||||
|
data = await self._client.get('/v1/geocode/autocomplete', params=params)
|
||||||
|
|
||||||
|
if data is None:
|
||||||
|
cloudlog.debug("dragonpilot search: API error, using photon fallback")
|
||||||
|
return await self._get_fallback().search(query, proximity, limit)
|
||||||
|
|
||||||
|
return self._parse_results(data, proximity, limit)
|
||||||
|
|
||||||
|
def search_sync(
|
||||||
|
self,
|
||||||
|
query: str,
|
||||||
|
proximity: Optional[Coordinate] = None,
|
||||||
|
limit: int = 10
|
||||||
|
) -> list[SearchResult]:
|
||||||
|
"""Search for places using Dragonpilot API (synchronous)."""
|
||||||
|
if not query or len(query) < 1:
|
||||||
|
return []
|
||||||
|
|
||||||
|
# Fall back to Photon if not authenticated
|
||||||
|
if not self._client.is_authenticated:
|
||||||
|
cloudlog.debug("dragonpilot search: no serial, using photon fallback")
|
||||||
|
return self._get_fallback().search_sync(query, proximity, limit)
|
||||||
|
|
||||||
|
params = {'q': query, 'limit': min(limit, 10)}
|
||||||
|
|
||||||
|
if proximity:
|
||||||
|
prox = proximity.to_wgs84() if hasattr(proximity, 'to_wgs84') else proximity
|
||||||
|
params['lat'] = prox.latitude
|
||||||
|
params['lon'] = prox.longitude
|
||||||
|
|
||||||
|
data = self._client.get_sync('/v1/geocode/autocomplete', params=params)
|
||||||
|
|
||||||
|
if data is None:
|
||||||
|
cloudlog.debug("dragonpilot search: API error, using photon fallback")
|
||||||
|
return self._get_fallback().search_sync(query, proximity, limit)
|
||||||
|
|
||||||
|
return self._parse_results(data, proximity, limit)
|
||||||
|
|
||||||
|
async def reverse_geocode(self, coord: Coordinate) -> Optional[SearchResult]:
|
||||||
|
"""Get address from coordinates. Falls back to Photon."""
|
||||||
|
return await self._get_fallback().reverse_geocode(coord)
|
||||||
|
|
||||||
|
def _parse_results(
|
||||||
|
self,
|
||||||
|
data: dict,
|
||||||
|
proximity: Optional[Coordinate],
|
||||||
|
limit: int
|
||||||
|
) -> list[SearchResult]:
|
||||||
|
"""Parse API response into SearchResult list."""
|
||||||
|
results = []
|
||||||
|
|
||||||
|
# Handle both GeoJSON format (features) and simple format (results)
|
||||||
|
items = data.get('features', data.get('results', []))
|
||||||
|
|
||||||
|
for item in items:
|
||||||
|
try:
|
||||||
|
# GeoJSON format
|
||||||
|
if 'geometry' in item:
|
||||||
|
coords = item['geometry']['coordinates']
|
||||||
|
lon, lat = coords[0], coords[1] # GeoJSON is [lon, lat]
|
||||||
|
props = item.get('properties', {})
|
||||||
|
title = props.get('title', 'Unknown')
|
||||||
|
address = props.get('address', '')
|
||||||
|
place_id = props.get('id')
|
||||||
|
distance = props.get('distance_m')
|
||||||
|
# Simple format
|
||||||
|
else:
|
||||||
|
pos = item.get('position', {})
|
||||||
|
lat = pos.get('lat')
|
||||||
|
lon = pos.get('lon')
|
||||||
|
title = item.get('title', 'Unknown')
|
||||||
|
address = item.get('address', '')
|
||||||
|
place_id = item.get('id')
|
||||||
|
distance = item.get('distance_m')
|
||||||
|
|
||||||
|
if lat is None or lon is None:
|
||||||
|
continue
|
||||||
|
|
||||||
|
coord = Coordinate(lat, lon)
|
||||||
|
|
||||||
|
# Calculate distance if not provided
|
||||||
|
if distance is None and proximity:
|
||||||
|
prox = proximity.to_wgs84() if hasattr(proximity, 'to_wgs84') else proximity
|
||||||
|
distance = Coordinate(lat, lon).distance_to(prox)
|
||||||
|
|
||||||
|
results.append(SearchResult(
|
||||||
|
name=title,
|
||||||
|
address=address,
|
||||||
|
coordinate=coord,
|
||||||
|
distance=distance,
|
||||||
|
place_id=place_id,
|
||||||
|
provider=self.name,
|
||||||
|
raw=item,
|
||||||
|
))
|
||||||
|
except (KeyError, TypeError, IndexError):
|
||||||
|
continue
|
||||||
|
|
||||||
|
return results[:limit]
|
||||||
@@ -0,0 +1,56 @@
|
|||||||
|
"""
|
||||||
|
Copyright (c) 2026, Rick Lan
|
||||||
|
|
||||||
|
Permission is hereby granted, free of charge, to any person obtaining a copy
|
||||||
|
of this software and associated documentation files (the "Software"), to deal
|
||||||
|
in the Software without restriction, including without limitation the rights
|
||||||
|
to use, copy, modify, merge, publish, distribute, and/or sublicense,
|
||||||
|
for non-commercial purposes only, subject to the following conditions:
|
||||||
|
|
||||||
|
- The above copyright notice and this permission notice shall be included in
|
||||||
|
all copies or substantial portions of the Software.
|
||||||
|
- Commercial use (e.g. use in a product, service, or activity intended to
|
||||||
|
generate revenue) is prohibited without explicit written permission from
|
||||||
|
the copyright holder.
|
||||||
|
|
||||||
|
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED,
|
||||||
|
INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A
|
||||||
|
PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
|
||||||
|
HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
|
||||||
|
OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
|
||||||
|
SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
|
||||||
|
|
||||||
|
Provider factory for creating map service instances.
|
||||||
|
|
||||||
|
Uses api.dragonpilot.org for search and routing.
|
||||||
|
"""
|
||||||
|
|
||||||
|
from .config import ProviderConfig
|
||||||
|
from .base import SearchProvider, RouteProvider, TileProvider
|
||||||
|
|
||||||
|
|
||||||
|
class ProviderFactory:
|
||||||
|
"""
|
||||||
|
Factory for creating map providers.
|
||||||
|
|
||||||
|
Uses Dragonpilot API providers which have built-in fallback
|
||||||
|
to free providers (Photon/OSRM) when not authenticated.
|
||||||
|
"""
|
||||||
|
|
||||||
|
@classmethod
|
||||||
|
def create_search_provider(cls, config: ProviderConfig) -> SearchProvider:
|
||||||
|
"""Create search provider."""
|
||||||
|
from .dragonpilot.search import DragonpilotSearchProvider
|
||||||
|
return DragonpilotSearchProvider()
|
||||||
|
|
||||||
|
@classmethod
|
||||||
|
def create_route_provider(cls, config: ProviderConfig) -> RouteProvider:
|
||||||
|
"""Create route provider."""
|
||||||
|
from .dragonpilot.routing import DragonpilotRouteProvider
|
||||||
|
return DragonpilotRouteProvider()
|
||||||
|
|
||||||
|
@classmethod
|
||||||
|
def create_tile_provider(cls, config: ProviderConfig) -> TileProvider:
|
||||||
|
"""Create tile provider."""
|
||||||
|
from .tiles.openfreemap import OpenFreeMapTileProvider
|
||||||
|
return OpenFreeMapTileProvider()
|
||||||
@@ -0,0 +1,188 @@
|
|||||||
|
"""
|
||||||
|
Copyright (c) 2026, Rick Lan
|
||||||
|
|
||||||
|
Permission is hereby granted, free of charge, to any person obtaining a copy
|
||||||
|
of this software and associated documentation files (the "Software"), to deal
|
||||||
|
in the Software without restriction, including without limitation the rights
|
||||||
|
to use, copy, modify, merge, publish, distribute, and/or sublicense,
|
||||||
|
for non-commercial purposes only, subject to the following conditions:
|
||||||
|
|
||||||
|
- The above copyright notice and this permission notice shall be included in
|
||||||
|
all copies or substantial portions of the Software.
|
||||||
|
- Commercial use (e.g. use in a product, service, or activity intended to
|
||||||
|
generate revenue) is prohibited without explicit written permission from
|
||||||
|
the copyright holder.
|
||||||
|
|
||||||
|
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED,
|
||||||
|
INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A
|
||||||
|
PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
|
||||||
|
HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
|
||||||
|
OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
|
||||||
|
SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
|
||||||
|
|
||||||
|
Map Service - Main entry point for all map operations.
|
||||||
|
|
||||||
|
Usage:
|
||||||
|
from dragonpilot.dashy.maa.providers import MapService
|
||||||
|
|
||||||
|
# In maad.py (sync):
|
||||||
|
map_service = MapService()
|
||||||
|
route = map_service.route_provider.get_route_sync(origin, dest)
|
||||||
|
|
||||||
|
# In server.py (async):
|
||||||
|
results = await MapService.get_instance().search_provider.search("Taipei 101")
|
||||||
|
|
||||||
|
# Get tile config for frontend:
|
||||||
|
tile_config = map_service.get_tile_config_for_js()
|
||||||
|
"""
|
||||||
|
|
||||||
|
from typing import Optional
|
||||||
|
|
||||||
|
from .config import ProviderConfig
|
||||||
|
from .factory import ProviderFactory
|
||||||
|
from .base import SearchProvider, RouteProvider, TileProvider
|
||||||
|
|
||||||
|
|
||||||
|
class MapService:
|
||||||
|
"""
|
||||||
|
Main entry point for all map operations.
|
||||||
|
|
||||||
|
Provides lazy-loaded access to search, routing, and tile providers.
|
||||||
|
Configuration is read from openpilot Params.
|
||||||
|
"""
|
||||||
|
|
||||||
|
_instance: Optional['MapService'] = None
|
||||||
|
|
||||||
|
def __init__(self, params=None):
|
||||||
|
"""
|
||||||
|
Initialize MapService.
|
||||||
|
|
||||||
|
Args:
|
||||||
|
params: Optional openpilot Params instance.
|
||||||
|
If None, will be created when needed.
|
||||||
|
"""
|
||||||
|
self._params = params
|
||||||
|
self._config: Optional[ProviderConfig] = None
|
||||||
|
self._search_provider: Optional[SearchProvider] = None
|
||||||
|
self._route_provider: Optional[RouteProvider] = None
|
||||||
|
self._tile_provider: Optional[TileProvider] = None
|
||||||
|
|
||||||
|
@classmethod
|
||||||
|
def get_instance(cls, params=None) -> 'MapService':
|
||||||
|
"""
|
||||||
|
Get singleton instance of MapService.
|
||||||
|
|
||||||
|
Args:
|
||||||
|
params: Optional Params instance for first-time initialization
|
||||||
|
"""
|
||||||
|
if cls._instance is None:
|
||||||
|
cls._instance = cls(params)
|
||||||
|
return cls._instance
|
||||||
|
|
||||||
|
@classmethod
|
||||||
|
def reset_instance(cls):
|
||||||
|
"""Reset singleton instance. Useful for testing or config reload."""
|
||||||
|
cls._instance = None
|
||||||
|
|
||||||
|
def _get_params(self):
|
||||||
|
"""Get or create Params instance."""
|
||||||
|
if self._params is None:
|
||||||
|
from openpilot.common.params import Params
|
||||||
|
self._params = Params()
|
||||||
|
return self._params
|
||||||
|
|
||||||
|
def reload_config(self):
|
||||||
|
"""
|
||||||
|
Reload configuration from params and recreate providers.
|
||||||
|
|
||||||
|
Call this after changing provider settings in params.
|
||||||
|
"""
|
||||||
|
self._config = ProviderConfig.from_params(self._get_params())
|
||||||
|
self._search_provider = None
|
||||||
|
self._route_provider = None
|
||||||
|
self._tile_provider = None
|
||||||
|
|
||||||
|
@property
|
||||||
|
def config(self) -> ProviderConfig:
|
||||||
|
"""Get current configuration, loading from params if needed."""
|
||||||
|
if self._config is None:
|
||||||
|
self._config = ProviderConfig.from_params(self._get_params())
|
||||||
|
return self._config
|
||||||
|
|
||||||
|
@property
|
||||||
|
def search_provider(self) -> SearchProvider:
|
||||||
|
"""Get search provider, creating if needed."""
|
||||||
|
if self._search_provider is None:
|
||||||
|
# Import providers to trigger registration
|
||||||
|
self._ensure_providers_imported()
|
||||||
|
self._search_provider = ProviderFactory.create_search_provider(self.config)
|
||||||
|
return self._search_provider
|
||||||
|
|
||||||
|
@property
|
||||||
|
def route_provider(self) -> RouteProvider:
|
||||||
|
"""Get route provider, creating if needed."""
|
||||||
|
if self._route_provider is None:
|
||||||
|
# Import providers to trigger registration
|
||||||
|
self._ensure_providers_imported()
|
||||||
|
self._route_provider = ProviderFactory.create_route_provider(self.config)
|
||||||
|
return self._route_provider
|
||||||
|
|
||||||
|
@property
|
||||||
|
def tile_provider(self) -> TileProvider:
|
||||||
|
"""Get tile provider, creating if needed."""
|
||||||
|
if self._tile_provider is None:
|
||||||
|
# Import providers to trigger registration
|
||||||
|
self._ensure_providers_imported()
|
||||||
|
self._tile_provider = ProviderFactory.create_tile_provider(self.config)
|
||||||
|
return self._tile_provider
|
||||||
|
|
||||||
|
def _ensure_providers_imported(self):
|
||||||
|
"""Import provider modules to trigger registration."""
|
||||||
|
try:
|
||||||
|
from . import search
|
||||||
|
from . import routing
|
||||||
|
from . import tiles
|
||||||
|
except ImportError:
|
||||||
|
pass
|
||||||
|
|
||||||
|
def get_tile_config_for_js(self) -> dict:
|
||||||
|
"""
|
||||||
|
Get tile configuration as dict for JavaScript frontend.
|
||||||
|
|
||||||
|
Returns dict suitable for sending to frontend via API.
|
||||||
|
"""
|
||||||
|
tile_config = self.tile_provider.get_tile_config()
|
||||||
|
style = self.tile_provider.get_style_json()
|
||||||
|
return {
|
||||||
|
'provider': self.config.tile_provider.value,
|
||||||
|
'url_template': tile_config.url_template,
|
||||||
|
'style': style,
|
||||||
|
'attribution': tile_config.attribution,
|
||||||
|
'min_zoom': tile_config.min_zoom,
|
||||||
|
'max_zoom': tile_config.max_zoom,
|
||||||
|
}
|
||||||
|
|
||||||
|
def get_provider_info(self) -> dict:
|
||||||
|
"""
|
||||||
|
Get information about current providers.
|
||||||
|
|
||||||
|
Useful for debugging and UI display.
|
||||||
|
"""
|
||||||
|
return {
|
||||||
|
'search': {
|
||||||
|
'provider': self.config.search_provider.value,
|
||||||
|
'name': self.search_provider.name,
|
||||||
|
'requires_api_key': self.search_provider.requires_api_key,
|
||||||
|
},
|
||||||
|
'route': {
|
||||||
|
'provider': self.config.route_provider.value,
|
||||||
|
'name': self.route_provider.name,
|
||||||
|
'requires_api_key': self.route_provider.requires_api_key,
|
||||||
|
'supports_traffic': self.route_provider.supports_traffic,
|
||||||
|
},
|
||||||
|
'tile': {
|
||||||
|
'provider': self.config.tile_provider.value,
|
||||||
|
'name': self.tile_provider.name,
|
||||||
|
'requires_api_key': self.tile_provider.requires_api_key,
|
||||||
|
},
|
||||||
|
}
|
||||||
@@ -0,0 +1,180 @@
|
|||||||
|
"""
|
||||||
|
Copyright (c) 2026, Rick Lan
|
||||||
|
|
||||||
|
Permission is hereby granted, free of charge, to any person obtaining a copy
|
||||||
|
of this software and associated documentation files (the "Software"), to deal
|
||||||
|
in the Software without restriction, including without limitation the rights
|
||||||
|
to use, copy, modify, merge, publish, distribute, and/or sublicense,
|
||||||
|
for non-commercial purposes only, subject to the following conditions:
|
||||||
|
|
||||||
|
- The above copyright notice and this permission notice shall be included in
|
||||||
|
all copies or substantial portions of the Software.
|
||||||
|
- Commercial use (e.g. use in a product, service, or activity intended to
|
||||||
|
generate revenue) is prohibited without explicit written permission from
|
||||||
|
the copyright holder.
|
||||||
|
|
||||||
|
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED,
|
||||||
|
INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A
|
||||||
|
PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
|
||||||
|
HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
|
||||||
|
OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
|
||||||
|
SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
|
||||||
|
|
||||||
|
Data models for map providers.
|
||||||
|
|
||||||
|
These models provide a common interface for search results, routes, and coordinates
|
||||||
|
across different providers (OSRM, Mapbox, Google, AMap).
|
||||||
|
"""
|
||||||
|
|
||||||
|
from __future__ import annotations
|
||||||
|
import math
|
||||||
|
from dataclasses import dataclass, field
|
||||||
|
from typing import Optional
|
||||||
|
from enum import Enum
|
||||||
|
|
||||||
|
|
||||||
|
EARTH_MEAN_RADIUS = 6371007.2 # meters
|
||||||
|
|
||||||
|
|
||||||
|
class CoordinateSystem(Enum):
|
||||||
|
"""Coordinate reference system."""
|
||||||
|
WGS84 = "wgs84" # Standard GPS coordinates (used globally)
|
||||||
|
GCJ02 = "gcj02" # China encrypted coordinates (required for China maps)
|
||||||
|
|
||||||
|
|
||||||
|
@dataclass
|
||||||
|
class Coordinate:
|
||||||
|
"""
|
||||||
|
Geographic coordinate with optional coordinate system awareness.
|
||||||
|
|
||||||
|
Supports WGS-84 (standard GPS) and GCJ-02 (China) coordinate systems.
|
||||||
|
Provides distance and bearing calculations.
|
||||||
|
"""
|
||||||
|
latitude: float
|
||||||
|
longitude: float
|
||||||
|
system: CoordinateSystem = CoordinateSystem.WGS84
|
||||||
|
annotations: dict = field(default_factory=dict)
|
||||||
|
|
||||||
|
def distance_to(self, other: Coordinate) -> float:
|
||||||
|
"""Calculate Haversine distance to another coordinate in meters."""
|
||||||
|
dlat = math.radians(other.latitude - self.latitude)
|
||||||
|
dlon = math.radians(other.longitude - self.longitude)
|
||||||
|
|
||||||
|
haversine_dlat = math.sin(dlat / 2.0)
|
||||||
|
haversine_dlat *= haversine_dlat
|
||||||
|
haversine_dlon = math.sin(dlon / 2.0)
|
||||||
|
haversine_dlon *= haversine_dlon
|
||||||
|
|
||||||
|
y = haversine_dlat + \
|
||||||
|
math.cos(math.radians(self.latitude)) * \
|
||||||
|
math.cos(math.radians(other.latitude)) * \
|
||||||
|
haversine_dlon
|
||||||
|
x = 2 * math.asin(math.sqrt(y))
|
||||||
|
return x * EARTH_MEAN_RADIUS
|
||||||
|
|
||||||
|
def bearing_to(self, other: Coordinate) -> float:
|
||||||
|
"""Calculate bearing to another coordinate in degrees (0-360)."""
|
||||||
|
lat1, lat2 = math.radians(self.latitude), math.radians(other.latitude)
|
||||||
|
dlon = math.radians(other.longitude - self.longitude)
|
||||||
|
x = math.sin(dlon) * math.cos(lat2)
|
||||||
|
y = math.cos(lat1) * math.sin(lat2) - math.sin(lat1) * math.cos(lat2) * math.cos(dlon)
|
||||||
|
bearing = math.degrees(math.atan2(x, y))
|
||||||
|
return (bearing + 360) % 360
|
||||||
|
|
||||||
|
def to_wgs84(self) -> Coordinate:
|
||||||
|
"""Convert to WGS-84 coordinate system."""
|
||||||
|
if self.system == CoordinateSystem.WGS84:
|
||||||
|
return self
|
||||||
|
from .utils.coordinates import gcj02_to_wgs84
|
||||||
|
lat, lon = gcj02_to_wgs84(self.latitude, self.longitude)
|
||||||
|
return Coordinate(lat, lon, CoordinateSystem.WGS84, self.annotations.copy())
|
||||||
|
|
||||||
|
def to_gcj02(self) -> Coordinate:
|
||||||
|
"""Convert to GCJ-02 coordinate system (for China maps)."""
|
||||||
|
if self.system == CoordinateSystem.GCJ02:
|
||||||
|
return self
|
||||||
|
from .utils.coordinates import wgs84_to_gcj02
|
||||||
|
lat, lon = wgs84_to_gcj02(self.latitude, self.longitude)
|
||||||
|
return Coordinate(lat, lon, CoordinateSystem.GCJ02, self.annotations.copy())
|
||||||
|
|
||||||
|
def as_dict(self) -> dict:
|
||||||
|
"""Convert to dictionary."""
|
||||||
|
return {'latitude': self.latitude, 'longitude': self.longitude}
|
||||||
|
|
||||||
|
def __str__(self) -> str:
|
||||||
|
return f'Coordinate({self.latitude:.6f}, {self.longitude:.6f})'
|
||||||
|
|
||||||
|
def __repr__(self) -> str:
|
||||||
|
return self.__str__()
|
||||||
|
|
||||||
|
def __eq__(self, other) -> bool:
|
||||||
|
if not isinstance(other, Coordinate):
|
||||||
|
return False
|
||||||
|
return self.latitude == other.latitude and self.longitude == other.longitude
|
||||||
|
|
||||||
|
def __hash__(self) -> int:
|
||||||
|
return hash((self.latitude, self.longitude))
|
||||||
|
|
||||||
|
|
||||||
|
@dataclass
|
||||||
|
class SearchResult:
|
||||||
|
"""
|
||||||
|
Normalized search/geocoding result.
|
||||||
|
|
||||||
|
All search providers return results in this format.
|
||||||
|
"""
|
||||||
|
name: str # Display name (e.g., "Taipei 101")
|
||||||
|
address: str # Full address string
|
||||||
|
coordinate: Coordinate # Location
|
||||||
|
distance: Optional[float] = None # Distance from search proximity (meters)
|
||||||
|
place_id: Optional[str] = None # Provider-specific place ID
|
||||||
|
provider: str = "" # Provider name (e.g., "photon", "mapbox")
|
||||||
|
raw: dict = field(default_factory=dict) # Raw provider response for debugging
|
||||||
|
|
||||||
|
|
||||||
|
@dataclass
|
||||||
|
class Step:
|
||||||
|
"""
|
||||||
|
Single navigation step/maneuver in a route.
|
||||||
|
|
||||||
|
Represents one turn or road segment with its geometry.
|
||||||
|
"""
|
||||||
|
distance: float # Step distance in meters
|
||||||
|
duration: float # Step duration in seconds
|
||||||
|
duration_typical: Optional[float] = None # Typical duration (with traffic)
|
||||||
|
name: str = "" # Road/street name
|
||||||
|
maneuver_type: str = "" # Type: turn, fork, off ramp, merge, etc.
|
||||||
|
maneuver_modifier: str = "" # Direction: left, right, slight left, etc.
|
||||||
|
geometry: list[Coordinate] = field(default_factory=list) # Path coordinates
|
||||||
|
speed_limit: Optional[float] = None # Speed limit in m/s
|
||||||
|
speed_limit_sign: str = "vienna" # Sign type: vienna or mutcd
|
||||||
|
maneuver_point: Optional[Coordinate] = None # Explicit maneuver location from OSRM
|
||||||
|
|
||||||
|
|
||||||
|
@dataclass
|
||||||
|
class Route:
|
||||||
|
"""
|
||||||
|
Complete navigation route.
|
||||||
|
|
||||||
|
Contains all steps from origin to destination with total distance/duration.
|
||||||
|
"""
|
||||||
|
steps: list[Step] # List of navigation steps
|
||||||
|
distance: float # Total distance in meters
|
||||||
|
duration: float # Total duration in seconds
|
||||||
|
duration_typical: Optional[float] = None # Typical duration (with traffic)
|
||||||
|
geometry: list[Coordinate] = field(default_factory=list) # Full route polyline
|
||||||
|
provider: str = "" # Provider name
|
||||||
|
has_traffic: bool = False # Whether duration includes traffic
|
||||||
|
raw: dict = field(default_factory=dict) # Raw provider response
|
||||||
|
|
||||||
|
|
||||||
|
@dataclass
|
||||||
|
class TileConfig:
|
||||||
|
"""
|
||||||
|
Map tile configuration for frontend display.
|
||||||
|
"""
|
||||||
|
url_template: str # URL template with {z}/{x}/{y}
|
||||||
|
style_url: Optional[str] = None # MapLibre style URL
|
||||||
|
attribution: str = "" # Map attribution text
|
||||||
|
min_zoom: int = 0
|
||||||
|
max_zoom: int = 22
|
||||||
@@ -0,0 +1,5 @@
|
|||||||
|
"""Routing providers."""
|
||||||
|
|
||||||
|
from .osrm import OSRMRouteProvider
|
||||||
|
|
||||||
|
__all__ = ['OSRMRouteProvider']
|
||||||
@@ -0,0 +1,190 @@
|
|||||||
|
"""
|
||||||
|
Copyright (c) 2026, Rick Lan
|
||||||
|
|
||||||
|
Permission is hereby granted, free of charge, to any person obtaining a copy
|
||||||
|
of this software and associated documentation files (the "Software"), to deal
|
||||||
|
in the Software without restriction, including without limitation the rights
|
||||||
|
to use, copy, modify, merge, publish, distribute, and/or sublicense,
|
||||||
|
for non-commercial purposes only, subject to the following conditions:
|
||||||
|
|
||||||
|
- The above copyright notice and this permission notice shall be included in
|
||||||
|
all copies or substantial portions of the Software.
|
||||||
|
- Commercial use (e.g. use in a product, service, or activity intended to
|
||||||
|
generate revenue) is prohibited without explicit written permission from
|
||||||
|
the copyright holder.
|
||||||
|
|
||||||
|
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED,
|
||||||
|
INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A
|
||||||
|
PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
|
||||||
|
HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
|
||||||
|
OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
|
||||||
|
SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
|
||||||
|
|
||||||
|
OSRM Route Provider.
|
||||||
|
|
||||||
|
Uses the free OSRM (Open Source Routing Machine) API for routing.
|
||||||
|
No API key required.
|
||||||
|
|
||||||
|
OSRM API Docs: http://project-osrm.org/docs/v5.24.0/api/
|
||||||
|
"""
|
||||||
|
|
||||||
|
from typing import Optional
|
||||||
|
import aiohttp
|
||||||
|
import requests
|
||||||
|
|
||||||
|
from ..base import RouteProvider
|
||||||
|
from ..models import Coordinate, Route, Step
|
||||||
|
|
||||||
|
OSRM_URL = 'https://router.project-osrm.org'
|
||||||
|
|
||||||
|
|
||||||
|
class OSRMRouteProvider(RouteProvider):
|
||||||
|
"""
|
||||||
|
Free OSRM routing provider.
|
||||||
|
|
||||||
|
No API key required. Uses public OSRM demo server.
|
||||||
|
Does not support traffic data.
|
||||||
|
"""
|
||||||
|
|
||||||
|
name = "osrm"
|
||||||
|
requires_api_key = False
|
||||||
|
supports_traffic = False
|
||||||
|
|
||||||
|
def __init__(self):
|
||||||
|
"""Initialize OSRM provider."""
|
||||||
|
pass
|
||||||
|
|
||||||
|
async def get_route(
|
||||||
|
self,
|
||||||
|
origin: Coordinate,
|
||||||
|
destination: Coordinate,
|
||||||
|
waypoints: Optional[list[Coordinate]] = None,
|
||||||
|
bearing: Optional[float] = None
|
||||||
|
) -> Optional[Route]:
|
||||||
|
"""Calculate route using OSRM API (async)."""
|
||||||
|
url, params = self._build_request(origin, destination, waypoints, bearing)
|
||||||
|
|
||||||
|
try:
|
||||||
|
async with aiohttp.ClientSession() as session:
|
||||||
|
async with session.get(url, params=params, timeout=aiohttp.ClientTimeout(total=10)) as resp:
|
||||||
|
if resp.status != 200:
|
||||||
|
return None
|
||||||
|
data = await resp.json()
|
||||||
|
except Exception:
|
||||||
|
return None
|
||||||
|
|
||||||
|
return self._parse_response(data)
|
||||||
|
|
||||||
|
def get_route_sync(
|
||||||
|
self,
|
||||||
|
origin: Coordinate,
|
||||||
|
destination: Coordinate,
|
||||||
|
waypoints: Optional[list[Coordinate]] = None,
|
||||||
|
bearing: Optional[float] = None
|
||||||
|
) -> Optional[Route]:
|
||||||
|
"""Calculate route using OSRM API (synchronous for maad.py)."""
|
||||||
|
url, params = self._build_request(origin, destination, waypoints, bearing)
|
||||||
|
|
||||||
|
try:
|
||||||
|
resp = requests.get(url, params=params, timeout=10)
|
||||||
|
if resp.status_code != 200:
|
||||||
|
return None
|
||||||
|
data = resp.json()
|
||||||
|
if data.get('code') != 'Ok':
|
||||||
|
return None
|
||||||
|
except Exception:
|
||||||
|
return None
|
||||||
|
|
||||||
|
return self._parse_response(data)
|
||||||
|
|
||||||
|
def _build_request(
|
||||||
|
self,
|
||||||
|
origin: Coordinate,
|
||||||
|
destination: Coordinate,
|
||||||
|
waypoints: Optional[list[Coordinate]],
|
||||||
|
bearing: Optional[float]
|
||||||
|
) -> tuple[str, dict]:
|
||||||
|
"""Build OSRM API request URL and params."""
|
||||||
|
# Convert to WGS-84 if needed (OSRM uses WGS-84)
|
||||||
|
origin = origin.to_wgs84() if hasattr(origin, 'to_wgs84') and origin.system.value != 'wgs84' else origin
|
||||||
|
destination = destination.to_wgs84() if hasattr(destination, 'to_wgs84') and destination.system.value != 'wgs84' else destination
|
||||||
|
|
||||||
|
# Build coordinate string: lon,lat;lon,lat;...
|
||||||
|
all_coords = [(origin.longitude, origin.latitude)]
|
||||||
|
if waypoints:
|
||||||
|
for wp in waypoints:
|
||||||
|
wp = wp.to_wgs84() if hasattr(wp, 'to_wgs84') and wp.system.value != 'wgs84' else wp
|
||||||
|
all_coords.append((wp.longitude, wp.latitude))
|
||||||
|
all_coords.append((destination.longitude, destination.latitude))
|
||||||
|
|
||||||
|
# Limit coordinate precision to 6 decimal places (about 0.1m accuracy)
|
||||||
|
coords_str = ';'.join([f'{lon:.6f},{lat:.6f}' for lon, lat in all_coords])
|
||||||
|
url = f"{OSRM_URL}/route/v1/driving/{coords_str}"
|
||||||
|
|
||||||
|
params = {
|
||||||
|
'overview': 'full',
|
||||||
|
'geometries': 'geojson',
|
||||||
|
'steps': 'true',
|
||||||
|
}
|
||||||
|
|
||||||
|
# Add bearing if provided (helps with route direction at start)
|
||||||
|
# OSRM bearings format: bearing,range for each coord, separated by ;
|
||||||
|
# Note: Disabled for now due to URL encoding issues with semicolons
|
||||||
|
# TODO: Re-enable once we properly handle the bearings parameter
|
||||||
|
# if bearing is not None:
|
||||||
|
# bearing_parts = [f"{int(bearing) % 360},90"] + [''] * (len(all_coords) - 1)
|
||||||
|
# url += f"?bearings={';'.join(bearing_parts)}"
|
||||||
|
# return url, params
|
||||||
|
|
||||||
|
return url, params
|
||||||
|
|
||||||
|
def _parse_response(self, data: dict) -> Optional[Route]:
|
||||||
|
"""Parse OSRM API response into Route object."""
|
||||||
|
if data.get('code') != 'Ok' or not data.get('routes'):
|
||||||
|
return None
|
||||||
|
|
||||||
|
route_data = data['routes'][0]
|
||||||
|
steps = []
|
||||||
|
full_geometry = []
|
||||||
|
|
||||||
|
for leg in route_data.get('legs', []):
|
||||||
|
for step in leg.get('steps', []):
|
||||||
|
maneuver = step.get('maneuver', {})
|
||||||
|
|
||||||
|
# Parse geometry coordinates
|
||||||
|
geometry = []
|
||||||
|
for coord in step.get('geometry', {}).get('coordinates', []):
|
||||||
|
# OSRM uses [lon, lat] order
|
||||||
|
c = Coordinate(coord[1], coord[0])
|
||||||
|
geometry.append(c)
|
||||||
|
full_geometry.append(c)
|
||||||
|
|
||||||
|
step_name = step.get('name', '')
|
||||||
|
|
||||||
|
# Extract explicit maneuver location from OSRM
|
||||||
|
maneuver_location = maneuver.get('location') # [lon, lat]
|
||||||
|
maneuver_point = None
|
||||||
|
if maneuver_location and len(maneuver_location) == 2:
|
||||||
|
maneuver_point = Coordinate(maneuver_location[1], maneuver_location[0])
|
||||||
|
|
||||||
|
steps.append(Step(
|
||||||
|
distance=step.get('distance', 0),
|
||||||
|
duration=step.get('duration', 0),
|
||||||
|
name=step_name,
|
||||||
|
maneuver_type=maneuver.get('type', ''),
|
||||||
|
maneuver_modifier=maneuver.get('modifier', ''),
|
||||||
|
geometry=geometry,
|
||||||
|
speed_limit=None,
|
||||||
|
speed_limit_sign='vienna',
|
||||||
|
maneuver_point=maneuver_point,
|
||||||
|
))
|
||||||
|
|
||||||
|
return Route(
|
||||||
|
steps=steps,
|
||||||
|
distance=route_data.get('distance', 0),
|
||||||
|
duration=route_data.get('duration', 0),
|
||||||
|
geometry=full_geometry,
|
||||||
|
provider=self.name,
|
||||||
|
has_traffic=False,
|
||||||
|
raw=route_data,
|
||||||
|
)
|
||||||
@@ -0,0 +1,5 @@
|
|||||||
|
"""Search providers."""
|
||||||
|
|
||||||
|
from .photon import PhotonSearchProvider
|
||||||
|
|
||||||
|
__all__ = ['PhotonSearchProvider']
|
||||||
@@ -0,0 +1,198 @@
|
|||||||
|
"""
|
||||||
|
Copyright (c) 2026, Rick Lan
|
||||||
|
|
||||||
|
Permission is hereby granted, free of charge, to any person obtaining a copy
|
||||||
|
of this software and associated documentation files (the "Software"), to deal
|
||||||
|
in the Software without restriction, including without limitation the rights
|
||||||
|
to use, copy, modify, merge, publish, distribute, and/or sublicense,
|
||||||
|
for non-commercial purposes only, subject to the following conditions:
|
||||||
|
|
||||||
|
- The above copyright notice and this permission notice shall be included in
|
||||||
|
all copies or substantial portions of the Software.
|
||||||
|
- Commercial use (e.g. use in a product, service, or activity intended to
|
||||||
|
generate revenue) is prohibited without explicit written permission from
|
||||||
|
the copyright holder.
|
||||||
|
|
||||||
|
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED,
|
||||||
|
INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A
|
||||||
|
PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
|
||||||
|
HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
|
||||||
|
OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
|
||||||
|
SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
|
||||||
|
|
||||||
|
Photon Search Provider.
|
||||||
|
|
||||||
|
Uses the free Photon (Komoot) geocoding API for address search.
|
||||||
|
No API key required, no rate limits.
|
||||||
|
|
||||||
|
Photon API: https://photon.komoot.io/
|
||||||
|
"""
|
||||||
|
|
||||||
|
from typing import Optional
|
||||||
|
import aiohttp
|
||||||
|
import requests
|
||||||
|
|
||||||
|
from ..base import SearchProvider
|
||||||
|
from ..models import Coordinate, SearchResult
|
||||||
|
|
||||||
|
PHOTON_URL = 'https://photon.komoot.io/api'
|
||||||
|
|
||||||
|
|
||||||
|
class PhotonSearchProvider(SearchProvider):
|
||||||
|
"""
|
||||||
|
Free Photon geocoding provider.
|
||||||
|
|
||||||
|
No API key required. Fast, reliable, uses OpenStreetMap data.
|
||||||
|
"""
|
||||||
|
|
||||||
|
name = "photon"
|
||||||
|
requires_api_key = False
|
||||||
|
|
||||||
|
def __init__(self):
|
||||||
|
"""Initialize Photon provider."""
|
||||||
|
pass
|
||||||
|
|
||||||
|
async def search(
|
||||||
|
self,
|
||||||
|
query: str,
|
||||||
|
proximity: Optional[Coordinate] = None,
|
||||||
|
limit: int = 10
|
||||||
|
) -> list[SearchResult]:
|
||||||
|
"""Search for places using Photon API (async)."""
|
||||||
|
if not query or len(query) < 2:
|
||||||
|
return []
|
||||||
|
|
||||||
|
params = {'q': query, 'limit': min(limit + 5, 15)} # Request extra for filtering
|
||||||
|
|
||||||
|
if proximity:
|
||||||
|
prox = proximity.to_wgs84() if hasattr(proximity, 'to_wgs84') else proximity
|
||||||
|
params['lat'] = prox.latitude
|
||||||
|
params['lon'] = prox.longitude
|
||||||
|
|
||||||
|
try:
|
||||||
|
async with aiohttp.ClientSession() as session:
|
||||||
|
async with session.get(PHOTON_URL, params=params, timeout=aiohttp.ClientTimeout(total=10)) as resp:
|
||||||
|
if resp.status != 200:
|
||||||
|
return []
|
||||||
|
data = await resp.json()
|
||||||
|
except Exception:
|
||||||
|
return []
|
||||||
|
|
||||||
|
return self._parse_results(data, proximity, limit)
|
||||||
|
|
||||||
|
def search_sync(
|
||||||
|
self,
|
||||||
|
query: str,
|
||||||
|
proximity: Optional[Coordinate] = None,
|
||||||
|
limit: int = 10
|
||||||
|
) -> list[SearchResult]:
|
||||||
|
"""Search for places using Photon API (synchronous)."""
|
||||||
|
if not query or len(query) < 2:
|
||||||
|
return []
|
||||||
|
|
||||||
|
params = {'q': query, 'limit': min(limit + 5, 15)}
|
||||||
|
|
||||||
|
if proximity:
|
||||||
|
prox = proximity.to_wgs84() if hasattr(proximity, 'to_wgs84') else proximity
|
||||||
|
params['lat'] = prox.latitude
|
||||||
|
params['lon'] = prox.longitude
|
||||||
|
|
||||||
|
try:
|
||||||
|
resp = requests.get(PHOTON_URL, params=params, timeout=10)
|
||||||
|
if resp.status_code != 200:
|
||||||
|
return []
|
||||||
|
data = resp.json()
|
||||||
|
except Exception:
|
||||||
|
return []
|
||||||
|
|
||||||
|
return self._parse_results(data, proximity, limit)
|
||||||
|
|
||||||
|
async def reverse_geocode(
|
||||||
|
self,
|
||||||
|
coord: Coordinate
|
||||||
|
) -> Optional[SearchResult]:
|
||||||
|
"""Get address from coordinates using Photon reverse API."""
|
||||||
|
wgs_coord = coord.to_wgs84() if hasattr(coord, 'to_wgs84') else coord
|
||||||
|
url = f"{PHOTON_URL}/reverse"
|
||||||
|
params = {'lat': wgs_coord.latitude, 'lon': wgs_coord.longitude}
|
||||||
|
|
||||||
|
try:
|
||||||
|
async with aiohttp.ClientSession() as session:
|
||||||
|
async with session.get(url, params=params, timeout=aiohttp.ClientTimeout(total=10)) as resp:
|
||||||
|
if resp.status != 200:
|
||||||
|
return None
|
||||||
|
data = await resp.json()
|
||||||
|
except Exception:
|
||||||
|
return None
|
||||||
|
|
||||||
|
features = data.get('features', [])
|
||||||
|
if not features:
|
||||||
|
return None
|
||||||
|
|
||||||
|
feature = features[0]
|
||||||
|
props = feature.get('properties', {})
|
||||||
|
parts = [props.get('name'), props.get('street'), props.get('city')]
|
||||||
|
address = ', '.join(filter(None, parts))
|
||||||
|
|
||||||
|
return SearchResult(
|
||||||
|
name=address.split(',')[0] if address else 'Unknown',
|
||||||
|
address=address or 'Unknown location',
|
||||||
|
coordinate=coord,
|
||||||
|
provider=self.name,
|
||||||
|
raw=feature,
|
||||||
|
)
|
||||||
|
|
||||||
|
def _parse_results(
|
||||||
|
self,
|
||||||
|
data: dict,
|
||||||
|
proximity: Optional[Coordinate],
|
||||||
|
limit: int
|
||||||
|
) -> list[SearchResult]:
|
||||||
|
"""Parse Photon GeoJSON response into SearchResult list."""
|
||||||
|
results = []
|
||||||
|
|
||||||
|
for feature in data.get('features', []):
|
||||||
|
try:
|
||||||
|
coords = feature['geometry']['coordinates']
|
||||||
|
props = feature.get('properties', {})
|
||||||
|
|
||||||
|
# Create coordinate (Photon uses [lon, lat] GeoJSON order)
|
||||||
|
coord = Coordinate(coords[1], coords[0])
|
||||||
|
|
||||||
|
# Build address from properties
|
||||||
|
parts = [
|
||||||
|
props.get('name'),
|
||||||
|
props.get('street'),
|
||||||
|
props.get('city'),
|
||||||
|
props.get('state'),
|
||||||
|
props.get('country')
|
||||||
|
]
|
||||||
|
address = ', '.join(filter(None, parts))
|
||||||
|
|
||||||
|
# Calculate distance if proximity provided
|
||||||
|
distance = None
|
||||||
|
if proximity:
|
||||||
|
prox = proximity.to_wgs84() if hasattr(proximity, 'to_wgs84') else proximity
|
||||||
|
search_coord = Coordinate(coords[1], coords[0]) # Use WGS84 for distance
|
||||||
|
distance = search_coord.distance_to(prox)
|
||||||
|
|
||||||
|
# Determine display name
|
||||||
|
name = props.get('name') or props.get('street') or (address.split(',')[0] if address else 'Unknown')
|
||||||
|
|
||||||
|
results.append(SearchResult(
|
||||||
|
name=name,
|
||||||
|
address=address or 'Unknown location',
|
||||||
|
coordinate=coord,
|
||||||
|
distance=distance,
|
||||||
|
place_id=props.get('osm_id'),
|
||||||
|
provider=self.name,
|
||||||
|
raw=feature,
|
||||||
|
))
|
||||||
|
except (KeyError, IndexError):
|
||||||
|
continue
|
||||||
|
|
||||||
|
# Sort by distance if proximity provided
|
||||||
|
if proximity:
|
||||||
|
results.sort(key=lambda r: r.distance if r.distance is not None else float('inf'))
|
||||||
|
|
||||||
|
return results[:limit]
|
||||||
@@ -0,0 +1,6 @@
|
|||||||
|
"""Tile providers."""
|
||||||
|
|
||||||
|
# Import providers to register them with the factory
|
||||||
|
from .openfreemap import OpenFreeMapTileProvider
|
||||||
|
|
||||||
|
__all__ = ['OpenFreeMapTileProvider']
|
||||||
@@ -0,0 +1,88 @@
|
|||||||
|
"""
|
||||||
|
Copyright (c) 2026, Rick Lan
|
||||||
|
|
||||||
|
Permission is hereby granted, free of charge, to any person obtaining a copy
|
||||||
|
of this software and associated documentation files (the "Software"), to deal
|
||||||
|
in the Software without restriction, including without limitation the rights
|
||||||
|
to use, copy, modify, merge, publish, distribute, and/or sublicense,
|
||||||
|
for non-commercial purposes only, subject to the following conditions:
|
||||||
|
|
||||||
|
- The above copyright notice and this permission notice shall be included in
|
||||||
|
all copies or substantial portions of the Software.
|
||||||
|
- Commercial use (e.g. use in a product, service, or activity intended to
|
||||||
|
generate revenue) is prohibited without explicit written permission from
|
||||||
|
the copyright holder.
|
||||||
|
|
||||||
|
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED,
|
||||||
|
INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A
|
||||||
|
PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
|
||||||
|
HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
|
||||||
|
OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
|
||||||
|
SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
|
||||||
|
|
||||||
|
OpenFreeMap Tile Provider.
|
||||||
|
|
||||||
|
Uses free OpenFreeMap tiles. No API key required.
|
||||||
|
|
||||||
|
OpenFreeMap: https://openfreemap.org/
|
||||||
|
"""
|
||||||
|
|
||||||
|
from ..base import TileProvider
|
||||||
|
from ..models import TileConfig
|
||||||
|
from ..factory import ProviderFactory
|
||||||
|
from ..config import TileProviderType
|
||||||
|
|
||||||
|
|
||||||
|
@ProviderFactory.register_tile(TileProviderType.OPENFREEMAP)
|
||||||
|
class OpenFreeMapTileProvider(TileProvider):
|
||||||
|
"""
|
||||||
|
Free OpenFreeMap tile provider.
|
||||||
|
|
||||||
|
No API key required. Uses OpenStreetMap data with nice styling.
|
||||||
|
"""
|
||||||
|
|
||||||
|
name = "openfreemap"
|
||||||
|
requires_api_key = False
|
||||||
|
|
||||||
|
def __init__(self, api_key: str = None):
|
||||||
|
"""Initialize OpenFreeMap provider."""
|
||||||
|
pass # No API key needed
|
||||||
|
|
||||||
|
def get_tile_config(self) -> TileConfig:
|
||||||
|
"""Get tile configuration."""
|
||||||
|
return TileConfig(
|
||||||
|
url_template="https://tiles.openfreemap.org/planet/{z}/{x}/{y}.pbf",
|
||||||
|
style_url="https://tiles.openfreemap.org/styles/liberty",
|
||||||
|
attribution='<a href="https://openfreemap.org">OpenFreeMap</a> | <a href="https://www.openstreetmap.org/copyright">OpenStreetMap</a>',
|
||||||
|
min_zoom=0,
|
||||||
|
max_zoom=20,
|
||||||
|
)
|
||||||
|
|
||||||
|
def get_style_json(self) -> dict:
|
||||||
|
"""
|
||||||
|
Get MapLibre GL style JSON.
|
||||||
|
|
||||||
|
This style is optimized for navigation display with good contrast
|
||||||
|
and road visibility.
|
||||||
|
"""
|
||||||
|
return {
|
||||||
|
"version": 8,
|
||||||
|
"name": "OpenFreeMap Liberty",
|
||||||
|
"sources": {
|
||||||
|
"openmaptiles": {
|
||||||
|
"type": "vector",
|
||||||
|
"url": "https://tiles.openfreemap.org/planet"
|
||||||
|
}
|
||||||
|
},
|
||||||
|
"glyphs": "https://tiles.openfreemap.org/fonts/{fontstack}/{range}.pbf",
|
||||||
|
"sprite": "https://tiles.openfreemap.org/styles/liberty/sprite",
|
||||||
|
"layers": [
|
||||||
|
# Simplified layer config - the actual style is loaded from style_url
|
||||||
|
# This is a fallback/reference
|
||||||
|
{
|
||||||
|
"id": "background",
|
||||||
|
"type": "background",
|
||||||
|
"paint": {"background-color": "#f8f4f0"}
|
||||||
|
}
|
||||||
|
]
|
||||||
|
}
|
||||||
@@ -0,0 +1,5 @@
|
|||||||
|
"""Utility functions for map providers."""
|
||||||
|
|
||||||
|
from .coordinates import wgs84_to_gcj02, gcj02_to_wgs84
|
||||||
|
|
||||||
|
__all__ = ['wgs84_to_gcj02', 'gcj02_to_wgs84']
|
||||||
@@ -0,0 +1,114 @@
|
|||||||
|
"""
|
||||||
|
Copyright (c) 2026, Rick Lan
|
||||||
|
|
||||||
|
Permission is hereby granted, free of charge, to any person obtaining a copy
|
||||||
|
of this software and associated documentation files (the "Software"), to deal
|
||||||
|
in the Software without restriction, including without limitation the rights
|
||||||
|
to use, copy, modify, merge, publish, distribute, and/or sublicense,
|
||||||
|
for non-commercial purposes only, subject to the following conditions:
|
||||||
|
|
||||||
|
- The above copyright notice and this permission notice shall be included in
|
||||||
|
all copies or substantial portions of the Software.
|
||||||
|
- Commercial use (e.g. use in a product, service, or activity intended to
|
||||||
|
generate revenue) is prohibited without explicit written permission from
|
||||||
|
the copyright holder.
|
||||||
|
|
||||||
|
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED,
|
||||||
|
INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A
|
||||||
|
PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
|
||||||
|
HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
|
||||||
|
OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
|
||||||
|
SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
|
||||||
|
|
||||||
|
WGS-84 <-> GCJ-02 coordinate transformations.
|
||||||
|
|
||||||
|
GCJ-02 (Chinese: 火星坐标系 "Mars Coordinates") is the coordinate system
|
||||||
|
mandated for maps in China. All maps in China must use GCJ-02, including
|
||||||
|
AMap (Gaode/高德) and Baidu Maps.
|
||||||
|
|
||||||
|
GPS devices output WGS-84 coordinates, which must be converted to GCJ-02
|
||||||
|
for use with Chinese map services, and vice versa.
|
||||||
|
"""
|
||||||
|
|
||||||
|
import math
|
||||||
|
|
||||||
|
# Krasovsky 1940 ellipsoid parameters
|
||||||
|
_A = 6378245.0 # Semi-major axis
|
||||||
|
_EE = 0.00669342162296594323 # Eccentricity squared
|
||||||
|
|
||||||
|
|
||||||
|
def _out_of_china(lat: float, lon: float) -> bool:
|
||||||
|
"""Check if coordinates are outside China's approximate bounds."""
|
||||||
|
return not (72.004 <= lon <= 137.8347 and 0.8293 <= lat <= 55.8271)
|
||||||
|
|
||||||
|
|
||||||
|
def _transform_lat(x: float, y: float) -> float:
|
||||||
|
"""Transform latitude offset."""
|
||||||
|
ret = -100.0 + 2.0 * x + 3.0 * y + 0.2 * y * y + 0.1 * x * y + 0.2 * math.sqrt(abs(x))
|
||||||
|
ret += (20.0 * math.sin(6.0 * x * math.pi) + 20.0 * math.sin(2.0 * x * math.pi)) * 2.0 / 3.0
|
||||||
|
ret += (20.0 * math.sin(y * math.pi) + 40.0 * math.sin(y / 3.0 * math.pi)) * 2.0 / 3.0
|
||||||
|
ret += (160.0 * math.sin(y / 12.0 * math.pi) + 320 * math.sin(y * math.pi / 30.0)) * 2.0 / 3.0
|
||||||
|
return ret
|
||||||
|
|
||||||
|
|
||||||
|
def _transform_lon(x: float, y: float) -> float:
|
||||||
|
"""Transform longitude offset."""
|
||||||
|
ret = 300.0 + x + 2.0 * y + 0.1 * x * x + 0.1 * x * y + 0.1 * math.sqrt(abs(x))
|
||||||
|
ret += (20.0 * math.sin(6.0 * x * math.pi) + 20.0 * math.sin(2.0 * x * math.pi)) * 2.0 / 3.0
|
||||||
|
ret += (20.0 * math.sin(x * math.pi) + 40.0 * math.sin(x / 3.0 * math.pi)) * 2.0 / 3.0
|
||||||
|
ret += (150.0 * math.sin(x / 12.0 * math.pi) + 300.0 * math.sin(x / 30.0 * math.pi)) * 2.0 / 3.0
|
||||||
|
return ret
|
||||||
|
|
||||||
|
|
||||||
|
def wgs84_to_gcj02(lat: float, lon: float) -> tuple[float, float]:
|
||||||
|
"""
|
||||||
|
Convert WGS-84 coordinates to GCJ-02.
|
||||||
|
|
||||||
|
Args:
|
||||||
|
lat: WGS-84 latitude
|
||||||
|
lon: WGS-84 longitude
|
||||||
|
|
||||||
|
Returns:
|
||||||
|
Tuple of (GCJ-02 latitude, GCJ-02 longitude)
|
||||||
|
"""
|
||||||
|
if _out_of_china(lat, lon):
|
||||||
|
return lat, lon
|
||||||
|
|
||||||
|
dlat = _transform_lat(lon - 105.0, lat - 35.0)
|
||||||
|
dlon = _transform_lon(lon - 105.0, lat - 35.0)
|
||||||
|
|
||||||
|
radlat = lat / 180.0 * math.pi
|
||||||
|
magic = math.sin(radlat)
|
||||||
|
magic = 1 - _EE * magic * magic
|
||||||
|
sqrtmagic = math.sqrt(magic)
|
||||||
|
|
||||||
|
dlat = (dlat * 180.0) / ((_A * (1 - _EE)) / (magic * sqrtmagic) * math.pi)
|
||||||
|
dlon = (dlon * 180.0) / (_A / sqrtmagic * math.cos(radlat) * math.pi)
|
||||||
|
|
||||||
|
return lat + dlat, lon + dlon
|
||||||
|
|
||||||
|
|
||||||
|
def gcj02_to_wgs84(lat: float, lon: float) -> tuple[float, float]:
|
||||||
|
"""
|
||||||
|
Convert GCJ-02 coordinates to WGS-84 (approximate inverse).
|
||||||
|
|
||||||
|
Uses iterative approach for better accuracy (~0.5m error).
|
||||||
|
|
||||||
|
Args:
|
||||||
|
lat: GCJ-02 latitude
|
||||||
|
lon: GCJ-02 longitude
|
||||||
|
|
||||||
|
Returns:
|
||||||
|
Tuple of (WGS-84 latitude, WGS-84 longitude)
|
||||||
|
"""
|
||||||
|
if _out_of_china(lat, lon):
|
||||||
|
return lat, lon
|
||||||
|
|
||||||
|
# Iterative approach for better accuracy
|
||||||
|
wgs_lat, wgs_lon = lat, lon
|
||||||
|
for _ in range(5):
|
||||||
|
gcj_lat, gcj_lon = wgs84_to_gcj02(wgs_lat, wgs_lon)
|
||||||
|
wgs_lat += lat - gcj_lat
|
||||||
|
wgs_lon += lon - gcj_lon
|
||||||
|
|
||||||
|
return wgs_lat, wgs_lon
|
||||||
@@ -0,0 +1,447 @@
|
|||||||
|
"""
|
||||||
|
Copyright (c) 2026, Rick Lan
|
||||||
|
|
||||||
|
Permission is hereby granted, free of charge, to any person obtaining a copy
|
||||||
|
of this software and associated documentation files (the "Software"), to deal
|
||||||
|
in the Software without restriction, including without limitation the rights
|
||||||
|
to use, copy, modify, merge, publish, distribute, and/or sublicense,
|
||||||
|
for non-commercial purposes only, subject to the following conditions:
|
||||||
|
|
||||||
|
- The above copyright notice and this permission notice shall be included in
|
||||||
|
all copies or substantial portions of the Software.
|
||||||
|
- Commercial use (e.g. use in a product, service, or activity intended to
|
||||||
|
generate revenue) is prohibited without explicit written permission from
|
||||||
|
the copyright holder.
|
||||||
|
|
||||||
|
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED,
|
||||||
|
INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A
|
||||||
|
PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
|
||||||
|
HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
|
||||||
|
OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
|
||||||
|
SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
|
||||||
|
|
||||||
|
Route Tracker - OsmAnd-inspired route tracking logic
|
||||||
|
|
||||||
|
Handles:
|
||||||
|
- Step transition detection (finding closest segment among lookahead)
|
||||||
|
- Reroute detection with time-based debounce
|
||||||
|
- GPS accuracy-aware tolerances
|
||||||
|
|
||||||
|
References:
|
||||||
|
- OsmAnd RoutingHelper.java: lookAheadFindMinOrthogonalDistance (line 562)
|
||||||
|
- OsmAnd RoutingHelperUtils.java: bearing thresholds (lines 175, 200)
|
||||||
|
- OsmAnd AnnounceTimeDistances.java: positioning tolerance (line 25)
|
||||||
|
"""
|
||||||
|
import time
|
||||||
|
from typing import Optional, TYPE_CHECKING
|
||||||
|
|
||||||
|
from openpilot.common.swaglog import cloudlog
|
||||||
|
|
||||||
|
from dragonpilot.dashy.maa.helpers import (
|
||||||
|
Coordinate,
|
||||||
|
minimum_distance,
|
||||||
|
normalize_angle,
|
||||||
|
project_onto_segment,
|
||||||
|
)
|
||||||
|
|
||||||
|
if TYPE_CHECKING:
|
||||||
|
from dragonpilot.dashy.maa.maad import Step
|
||||||
|
|
||||||
|
|
||||||
|
# Reroute parameters (OsmAnd-inspired)
|
||||||
|
REROUTE_DISTANCE_BASE = 25 # meters off route before considering reroute
|
||||||
|
REROUTE_DEBOUNCE_TIME = 3.0 # seconds of sustained deviation before reroute
|
||||||
|
POSITIONING_TOLERANCE = 12 # meters GPS error buffer
|
||||||
|
|
||||||
|
# Bearing thresholds (OsmAnd: RoutingHelperUtils.java)
|
||||||
|
WRONG_DIRECTION_THRESHOLD = 90.0 # degrees - wrong movement direction
|
||||||
|
UTURN_THRESHOLD = 135.0 # degrees - U-turn needed
|
||||||
|
|
||||||
|
# Speed-based lookahead (OsmAnd: RoutingHelper.java:562)
|
||||||
|
LOOKAHEAD_SLOW = 8 # segments for slow speed
|
||||||
|
LOOKAHEAD_FAST = 15 # segments for fast speed (highway)
|
||||||
|
FAST_SPEED_THRESHOLD = 20.0 # m/s (~72 km/h) to switch to fast lookahead
|
||||||
|
|
||||||
|
# Step transition - snap zone around maneuver point
|
||||||
|
MANEUVER_SNAP_ZONE = 100.0 # meters before/after maneuver to check
|
||||||
|
|
||||||
|
# Maneuver point detection (bearing-based)
|
||||||
|
MANEUVER_PROXIMITY_THRESHOLD = 50.0 # meters - must be this close to check bearing
|
||||||
|
MANEUVER_BEHIND_ANGLE = 90.0 # degrees - point is "behind" if angle > this
|
||||||
|
|
||||||
|
|
||||||
|
class RouteTracker:
|
||||||
|
"""Tracks position along a route with snap-to-road logic."""
|
||||||
|
|
||||||
|
def __init__(self):
|
||||||
|
self.step_idx: Optional[int] = None
|
||||||
|
self.segment_idx: int = 0 # Current segment within step
|
||||||
|
self.deviation_start_time: Optional[float] = None
|
||||||
|
|
||||||
|
def reset(self):
|
||||||
|
"""Reset tracking state."""
|
||||||
|
self.step_idx = None
|
||||||
|
self.segment_idx = 0
|
||||||
|
self.deviation_start_time = None
|
||||||
|
|
||||||
|
def set_step(self, idx: int):
|
||||||
|
"""Set current step index."""
|
||||||
|
self.step_idx = idx
|
||||||
|
self.segment_idx = 0
|
||||||
|
self.deviation_start_time = None
|
||||||
|
|
||||||
|
def get_lookahead(self, speed: float) -> int:
|
||||||
|
"""Get lookahead distance based on speed (OsmAnd: RoutingHelper.java:562)."""
|
||||||
|
return LOOKAHEAD_FAST if speed > FAST_SPEED_THRESHOLD else LOOKAHEAD_SLOW
|
||||||
|
|
||||||
|
def snap_to_route(
|
||||||
|
self,
|
||||||
|
route: list['Step'],
|
||||||
|
position: Coordinate,
|
||||||
|
bearing: Optional[float] = None,
|
||||||
|
speed: float = 0.0,
|
||||||
|
) -> tuple[int, int, float, float, float]:
|
||||||
|
"""Snap position to route geometry.
|
||||||
|
|
||||||
|
Returns:
|
||||||
|
(step_idx, segment_idx, t, distance_to_route, distance_along_step) where:
|
||||||
|
- step_idx: which step we're on
|
||||||
|
- segment_idx: which segment within the step
|
||||||
|
- t: position along segment (0-1)
|
||||||
|
- distance_to_route: perpendicular distance to route
|
||||||
|
- distance_along_step: how far along the current step (meters)
|
||||||
|
"""
|
||||||
|
if not route or self.step_idx is None:
|
||||||
|
return self.step_idx or 0, 0, 0.0, float('inf'), 0.0
|
||||||
|
|
||||||
|
lookahead = self.get_lookahead(speed)
|
||||||
|
best_dist = float('inf')
|
||||||
|
best_step = self.step_idx
|
||||||
|
best_seg = 0
|
||||||
|
best_t = 0.0
|
||||||
|
|
||||||
|
# Search current step and lookahead steps
|
||||||
|
end_idx = min(self.step_idx + lookahead, len(route))
|
||||||
|
for step_idx in range(self.step_idx, end_idx):
|
||||||
|
step = route[step_idx]
|
||||||
|
|
||||||
|
for seg_idx in range(len(step.geometry) - 1):
|
||||||
|
a = step.geometry[seg_idx]
|
||||||
|
b = step.geometry[seg_idx + 1]
|
||||||
|
seg_len = a.distance_to(b)
|
||||||
|
if seg_len < 1.0:
|
||||||
|
continue
|
||||||
|
|
||||||
|
_, t, dist = project_onto_segment(a, b, position)
|
||||||
|
|
||||||
|
# If we have bearing, validate direction for non-current steps
|
||||||
|
if bearing is not None and step_idx > self.step_idx:
|
||||||
|
route_bearing = a.bearing_to(b)
|
||||||
|
bearing_diff = abs(normalize_angle(bearing - route_bearing))
|
||||||
|
if bearing_diff > WRONG_DIRECTION_THRESHOLD:
|
||||||
|
continue # Skip segments going wrong direction
|
||||||
|
|
||||||
|
if dist < best_dist:
|
||||||
|
best_dist = dist
|
||||||
|
best_step = step_idx
|
||||||
|
best_seg = seg_idx
|
||||||
|
best_t = t
|
||||||
|
|
||||||
|
# Calculate distance along the best step
|
||||||
|
dist_along = 0.0
|
||||||
|
if best_step < len(route):
|
||||||
|
step = route[best_step]
|
||||||
|
for i in range(best_seg):
|
||||||
|
if i < len(step.geometry) - 1:
|
||||||
|
dist_along += step.geometry[i].distance_to(step.geometry[i + 1])
|
||||||
|
# Add partial distance in current segment
|
||||||
|
if best_seg < len(step.geometry) - 1:
|
||||||
|
seg_len = step.geometry[best_seg].distance_to(step.geometry[best_seg + 1])
|
||||||
|
dist_along += seg_len * best_t
|
||||||
|
|
||||||
|
return best_step, best_seg, best_t, best_dist, dist_along
|
||||||
|
|
||||||
|
def find_closest_step(
|
||||||
|
self,
|
||||||
|
route: list['Step'],
|
||||||
|
position: Coordinate,
|
||||||
|
bearing: Optional[float],
|
||||||
|
speed: float = 0.0,
|
||||||
|
) -> int:
|
||||||
|
"""Find closest step among current and lookahead steps (OsmAnd-style).
|
||||||
|
|
||||||
|
Uses orthogonal distance to segments with bearing validation.
|
||||||
|
Returns the step index with minimum distance that matches travel direction.
|
||||||
|
"""
|
||||||
|
if not route or position is None or self.step_idx is None:
|
||||||
|
return self.step_idx or 0
|
||||||
|
|
||||||
|
lookahead = self.get_lookahead(speed)
|
||||||
|
|
||||||
|
if bearing is None:
|
||||||
|
return self._find_closest_by_distance(route, position, lookahead)
|
||||||
|
|
||||||
|
min_dist = float('inf')
|
||||||
|
closest_idx = self.step_idx
|
||||||
|
|
||||||
|
end_idx = min(self.step_idx + lookahead, len(route))
|
||||||
|
for step_idx in range(self.step_idx, end_idx):
|
||||||
|
step = route[step_idx]
|
||||||
|
|
||||||
|
for i in range(len(step.geometry) - 1):
|
||||||
|
a = step.geometry[i]
|
||||||
|
b = step.geometry[i + 1]
|
||||||
|
seg_len = a.distance_to(b)
|
||||||
|
if seg_len < 1.0:
|
||||||
|
continue
|
||||||
|
|
||||||
|
d = minimum_distance(a, b, position)
|
||||||
|
if d < min_dist:
|
||||||
|
# Check bearing match before accepting
|
||||||
|
route_bearing = a.bearing_to(b)
|
||||||
|
bearing_diff = abs(normalize_angle(bearing - route_bearing))
|
||||||
|
|
||||||
|
# Only accept if bearing within 90 degrees (not going backwards)
|
||||||
|
if bearing_diff < WRONG_DIRECTION_THRESHOLD or step_idx == self.step_idx:
|
||||||
|
min_dist = d
|
||||||
|
closest_idx = step_idx
|
||||||
|
|
||||||
|
return closest_idx
|
||||||
|
|
||||||
|
def _find_closest_by_distance(
|
||||||
|
self,
|
||||||
|
route: list['Step'],
|
||||||
|
position: Coordinate,
|
||||||
|
lookahead: int,
|
||||||
|
) -> int:
|
||||||
|
"""Fallback: find closest step by distance only (no bearing check)."""
|
||||||
|
min_dist = float('inf')
|
||||||
|
closest_idx = self.step_idx
|
||||||
|
|
||||||
|
end_idx = min(self.step_idx + lookahead, len(route))
|
||||||
|
for step_idx in range(self.step_idx, end_idx):
|
||||||
|
step = route[step_idx]
|
||||||
|
for i in range(len(step.geometry) - 1):
|
||||||
|
a = step.geometry[i]
|
||||||
|
b = step.geometry[i + 1]
|
||||||
|
if a.distance_to(b) < 1.0:
|
||||||
|
continue
|
||||||
|
d = minimum_distance(a, b, position)
|
||||||
|
if d < min_dist:
|
||||||
|
min_dist = d
|
||||||
|
closest_idx = step_idx
|
||||||
|
|
||||||
|
return closest_idx
|
||||||
|
|
||||||
|
def should_reroute(
|
||||||
|
self,
|
||||||
|
route: list['Step'],
|
||||||
|
position: Coordinate,
|
||||||
|
gps_accuracy: float = 0.0,
|
||||||
|
) -> bool:
|
||||||
|
"""Check if route should be recomputed (OsmAnd-style time-based debounce).
|
||||||
|
|
||||||
|
Returns True if vehicle has been off-route for REROUTE_DEBOUNCE_TIME seconds.
|
||||||
|
Uses GPS accuracy-aware tolerance for distance threshold.
|
||||||
|
"""
|
||||||
|
if self.step_idx is None or not route:
|
||||||
|
return True
|
||||||
|
|
||||||
|
# Don't reroute in last segment
|
||||||
|
if self.step_idx == len(route) - 1:
|
||||||
|
return False
|
||||||
|
|
||||||
|
# GPS accuracy-aware tolerance (OsmAnd: AnnounceTimeDistances.java:25)
|
||||||
|
tolerance = POSITIONING_TOLERANCE / 2 + gps_accuracy
|
||||||
|
reroute_threshold = max(REROUTE_DISTANCE_BASE, tolerance)
|
||||||
|
|
||||||
|
# Find minimum distance to current step geometry
|
||||||
|
min_d = reroute_threshold + 1
|
||||||
|
step = route[self.step_idx]
|
||||||
|
|
||||||
|
for i in range(len(step.geometry) - 1):
|
||||||
|
a = step.geometry[i]
|
||||||
|
b = step.geometry[i + 1]
|
||||||
|
if a.distance_to(b) < 1.0:
|
||||||
|
continue
|
||||||
|
min_d = min(min_d, minimum_distance(a, b, position))
|
||||||
|
|
||||||
|
now = time.monotonic()
|
||||||
|
|
||||||
|
# Time-based debounce (OsmAnd: 10 seconds of sustained deviation)
|
||||||
|
if min_d > reroute_threshold:
|
||||||
|
if self.deviation_start_time is None:
|
||||||
|
self.deviation_start_time = now
|
||||||
|
cloudlog.info(f"maad: deviation detected, dist={min_d:.0f}m > threshold={reroute_threshold:.0f}m")
|
||||||
|
elif now - self.deviation_start_time > REROUTE_DEBOUNCE_TIME:
|
||||||
|
cloudlog.warning(f"maad: rerouting after {REROUTE_DEBOUNCE_TIME}s deviation")
|
||||||
|
return True
|
||||||
|
else:
|
||||||
|
# Back on route - reset timer
|
||||||
|
if self.deviation_start_time is not None:
|
||||||
|
cloudlog.info("maad: back on route, resetting deviation timer")
|
||||||
|
self.deviation_start_time = None
|
||||||
|
|
||||||
|
return False
|
||||||
|
|
||||||
|
def _get_geometry_tail(self, geometry: list[Coordinate], max_dist: float) -> list[Coordinate]:
|
||||||
|
"""Get the last max_dist meters of geometry (before maneuver point)."""
|
||||||
|
if len(geometry) < 2:
|
||||||
|
return geometry
|
||||||
|
|
||||||
|
# Walk backwards from end
|
||||||
|
result = [geometry[-1]]
|
||||||
|
dist = 0.0
|
||||||
|
for i in range(len(geometry) - 2, -1, -1):
|
||||||
|
seg_dist = geometry[i].distance_to(geometry[i + 1])
|
||||||
|
if dist + seg_dist > max_dist:
|
||||||
|
break
|
||||||
|
result.insert(0, geometry[i])
|
||||||
|
dist += seg_dist
|
||||||
|
|
||||||
|
return result
|
||||||
|
|
||||||
|
def _get_geometry_head(self, geometry: list[Coordinate], max_dist: float) -> list[Coordinate]:
|
||||||
|
"""Get the first max_dist meters of geometry (after maneuver point)."""
|
||||||
|
if len(geometry) < 2:
|
||||||
|
return geometry
|
||||||
|
|
||||||
|
# Walk forwards from start
|
||||||
|
result = [geometry[0]]
|
||||||
|
dist = 0.0
|
||||||
|
for i in range(1, len(geometry)):
|
||||||
|
seg_dist = geometry[i - 1].distance_to(geometry[i])
|
||||||
|
if dist + seg_dist > max_dist:
|
||||||
|
break
|
||||||
|
result.append(geometry[i])
|
||||||
|
dist += seg_dist
|
||||||
|
|
||||||
|
return result
|
||||||
|
|
||||||
|
def _snap_to_geometry(self, geometry: list[Coordinate], position: Coordinate) -> float:
|
||||||
|
"""Find minimum distance from position to geometry segments."""
|
||||||
|
min_dist = float('inf')
|
||||||
|
for i in range(len(geometry) - 1):
|
||||||
|
a, b = geometry[i], geometry[i + 1]
|
||||||
|
if a.distance_to(b) < 1.0:
|
||||||
|
continue
|
||||||
|
_, _, dist = project_onto_segment(a, b, position)
|
||||||
|
min_dist = min(min_dist, dist)
|
||||||
|
return min_dist
|
||||||
|
|
||||||
|
def _check_passed_maneuver_point(
|
||||||
|
self,
|
||||||
|
maneuver_pt: Coordinate,
|
||||||
|
position: Coordinate,
|
||||||
|
bearing: Optional[float],
|
||||||
|
) -> bool:
|
||||||
|
"""Check if vehicle has passed the maneuver point using bearing.
|
||||||
|
|
||||||
|
A vehicle has "passed" a point when that point is behind it (>90° off heading).
|
||||||
|
|
||||||
|
Before fork: Vehicle → → → [Fork Point]
|
||||||
|
bearing_to_fork ≈ 0° (ahead)
|
||||||
|
|
||||||
|
After fork: [Fork Point] Vehicle → → →
|
||||||
|
bearing_to_fork ≈ 180° (behind)
|
||||||
|
"""
|
||||||
|
dist = position.distance_to(maneuver_pt)
|
||||||
|
|
||||||
|
if bearing is not None:
|
||||||
|
# Calculate bearing FROM vehicle TO maneuver point
|
||||||
|
bearing_to_pt = position.bearing_to(maneuver_pt)
|
||||||
|
angle_diff = abs(normalize_angle(bearing - bearing_to_pt))
|
||||||
|
|
||||||
|
# Debug logging
|
||||||
|
if dist < 500:
|
||||||
|
cloudlog.info(f"maad: maneuver check step={self.step_idx} dist={dist:.1f}m "
|
||||||
|
f"bearing={bearing:.1f} to_pt={bearing_to_pt:.1f} angle_diff={angle_diff:.1f}")
|
||||||
|
|
||||||
|
# If maneuver point is behind us (>90° off heading), we've passed it
|
||||||
|
# This works regardless of distance - if it's behind us, we passed it
|
||||||
|
if angle_diff > MANEUVER_BEHIND_ANGLE:
|
||||||
|
cloudlog.info(f"maad: passed maneuver point, advancing step {self.step_idx} -> {self.step_idx + 1} "
|
||||||
|
f"(dist={dist:.1f}m, angle_diff={angle_diff:.1f}°)")
|
||||||
|
self.step_idx += 1
|
||||||
|
self.segment_idx = 0
|
||||||
|
self.deviation_start_time = None
|
||||||
|
return True
|
||||||
|
else:
|
||||||
|
# No bearing - use distance only (very close = passed)
|
||||||
|
if dist < 15.0:
|
||||||
|
cloudlog.info(f"maad: passed maneuver point (no bearing), advancing step {self.step_idx} -> {self.step_idx + 1} "
|
||||||
|
f"(dist={dist:.1f}m)")
|
||||||
|
self.step_idx += 1
|
||||||
|
self.segment_idx = 0
|
||||||
|
self.deviation_start_time = None
|
||||||
|
return True
|
||||||
|
|
||||||
|
return False
|
||||||
|
|
||||||
|
def update_step(
|
||||||
|
self,
|
||||||
|
route: list['Step'],
|
||||||
|
position: Coordinate,
|
||||||
|
bearing: Optional[float],
|
||||||
|
speed: float = 0.0,
|
||||||
|
) -> bool:
|
||||||
|
"""Update current step by detecting when maneuver point is passed.
|
||||||
|
|
||||||
|
Uses explicit maneuver point with bearing-based detection if available,
|
||||||
|
otherwise falls back to geometry comparison.
|
||||||
|
"""
|
||||||
|
if not route or self.step_idx is None:
|
||||||
|
return False
|
||||||
|
|
||||||
|
# Need a next step to transition to
|
||||||
|
if self.step_idx + 1 >= len(route):
|
||||||
|
return False
|
||||||
|
|
||||||
|
current_step = route[self.step_idx]
|
||||||
|
next_step = route[self.step_idx + 1]
|
||||||
|
|
||||||
|
# TODO: Re-enable maneuver point detection after fixing display issues
|
||||||
|
# # Use explicit maneuver point if available (preferred - works at forks)
|
||||||
|
# # Note: OSRM's maneuver.location is at the START of each step, so we check
|
||||||
|
# # the NEXT step's maneuver_point (where the turn/fork happens)
|
||||||
|
# if next_step.maneuver_point is not None:
|
||||||
|
# return self._check_passed_maneuver_point(next_step.maneuver_point, position, bearing)
|
||||||
|
# else:
|
||||||
|
# cloudlog.warning(f"maad: step {self.step_idx + 1} has no maneuver_point, using geometry fallback")
|
||||||
|
|
||||||
|
# Geometry-based comparison (standard OSRM/OsmAnd approach)
|
||||||
|
if not current_step.geometry or not next_step.geometry:
|
||||||
|
return False
|
||||||
|
|
||||||
|
# Get geometry around the maneuver point
|
||||||
|
# Before: last 100m of current step
|
||||||
|
before_geom = self._get_geometry_tail(current_step.geometry, MANEUVER_SNAP_ZONE)
|
||||||
|
# After: first 100m of next step
|
||||||
|
after_geom = self._get_geometry_head(next_step.geometry, MANEUVER_SNAP_ZONE)
|
||||||
|
|
||||||
|
# Snap to both geometries
|
||||||
|
dist_before = self._snap_to_geometry(before_geom, position)
|
||||||
|
dist_after = self._snap_to_geometry(after_geom, position)
|
||||||
|
|
||||||
|
# Debug logging
|
||||||
|
cloudlog.debug(f"maad: step transition check step={self.step_idx} "
|
||||||
|
f"dist_before={dist_before:.1f}m dist_after={dist_after:.1f}m")
|
||||||
|
|
||||||
|
# If closer to "after" geometry, we've passed the maneuver
|
||||||
|
if dist_after < dist_before:
|
||||||
|
# Additional bearing check if available
|
||||||
|
if bearing is not None and len(after_geom) >= 2:
|
||||||
|
route_bearing = after_geom[0].bearing_to(after_geom[1])
|
||||||
|
bearing_diff = abs(normalize_angle(bearing - route_bearing))
|
||||||
|
if bearing_diff > WRONG_DIRECTION_THRESHOLD:
|
||||||
|
# Going wrong direction on next step - don't transition
|
||||||
|
return False
|
||||||
|
|
||||||
|
cloudlog.info(f"maad: maneuver passed {self.step_idx} -> {self.step_idx + 1} "
|
||||||
|
f"(dist_before={dist_before:.1f}m, dist_after={dist_after:.1f}m)")
|
||||||
|
self.step_idx += 1
|
||||||
|
self.segment_idx = 0
|
||||||
|
self.deviation_start_time = None
|
||||||
|
return True
|
||||||
|
|
||||||
|
return False
|
||||||
@@ -26,15 +26,15 @@ Dashy HTTP Server
|
|||||||
|
|
||||||
Provides REST API and static file serving for the dashy web UI.
|
Provides REST API and static file serving for the dashy web UI.
|
||||||
- Settings management (read/write params)
|
- Settings management (read/write params)
|
||||||
|
- Navigation API (destination, search, places, tiles)
|
||||||
- File browser for drive logs
|
- File browser for drive logs
|
||||||
|
- WebRTC stream proxy
|
||||||
- Static file serving for web UI
|
- Static file serving for web UI
|
||||||
"""
|
"""
|
||||||
|
|
||||||
import argparse
|
import argparse
|
||||||
import ast
|
|
||||||
import asyncio
|
import asyncio
|
||||||
import json
|
import json
|
||||||
import operator
|
|
||||||
import os
|
import os
|
||||||
import logging
|
import logging
|
||||||
import time
|
import time
|
||||||
@@ -42,7 +42,7 @@ from datetime import datetime
|
|||||||
from functools import wraps
|
from functools import wraps
|
||||||
from urllib.parse import quote
|
from urllib.parse import quote
|
||||||
|
|
||||||
from aiohttp import web
|
from aiohttp import web, ClientSession, ClientTimeout, ClientConnectorError
|
||||||
|
|
||||||
from cereal import messaging
|
from cereal import messaging
|
||||||
|
|
||||||
@@ -50,15 +50,13 @@ from openpilot.common.params import Params
|
|||||||
from openpilot.system.hardware import PC, HARDWARE
|
from openpilot.system.hardware import PC, HARDWARE
|
||||||
from openpilot.system.ui.lib.multilang import multilang as base_multilang
|
from openpilot.system.ui.lib.multilang import multilang as base_multilang
|
||||||
from dragonpilot.settings import SETTINGS
|
from dragonpilot.settings import SETTINGS
|
||||||
|
from dragonpilot.dashy.maa.providers import MapService
|
||||||
try:
|
from dragonpilot.dashy.maa.providers.models import Coordinate
|
||||||
from openpilot.system.version import get_build_metadata as _get_build_metadata
|
|
||||||
except Exception:
|
|
||||||
_get_build_metadata = None
|
|
||||||
|
|
||||||
# --- Configuration ---
|
# --- Configuration ---
|
||||||
DEFAULT_DIR = os.path.realpath(os.path.join(os.path.dirname(__file__), '..') if PC else '/data/media/0/realdata')
|
DEFAULT_DIR = os.path.realpath(os.path.join(os.path.dirname(__file__), '..') if PC else '/data/media/0/realdata')
|
||||||
WEB_DIST_PATH = os.path.join(os.path.dirname(__file__), "web", "dist")
|
WEB_DIST_PATH = os.path.join(os.path.dirname(__file__), "web", "dist")
|
||||||
|
WEBRTC_TIMEOUT = ClientTimeout(total=10)
|
||||||
CAR_PARAMS_CACHE_TTL = 30 # seconds
|
CAR_PARAMS_CACHE_TTL = 30 # seconds
|
||||||
|
|
||||||
logger = logging.getLogger("dashy")
|
logger = logging.getLogger("dashy")
|
||||||
@@ -130,10 +128,7 @@ class AppCache:
|
|||||||
'brand': car_params['brand'],
|
'brand': car_params['brand'],
|
||||||
'openpilotLongitudinalControl': car_params['openpilot_longitudinal_control'],
|
'openpilotLongitudinalControl': car_params['openpilot_longitudinal_control'],
|
||||||
'LITE': os.getenv("LITE") is not None,
|
'LITE': os.getenv("LITE") is not None,
|
||||||
'MICI': self._check_mici(),
|
'MICI': self._check_mici()
|
||||||
# Upstream-mirror items gate on these.
|
|
||||||
'DASHY': True,
|
|
||||||
'IS_RELEASE': self._is_release_channel(),
|
|
||||||
}
|
}
|
||||||
self._context_time = now
|
self._context_time = now
|
||||||
return self._context
|
return self._context
|
||||||
@@ -145,14 +140,6 @@ class AppCache:
|
|||||||
except Exception:
|
except Exception:
|
||||||
return False
|
return False
|
||||||
|
|
||||||
def _is_release_channel(self):
|
|
||||||
if _get_build_metadata is None:
|
|
||||||
return False
|
|
||||||
try:
|
|
||||||
return bool(_get_build_metadata().release_channel)
|
|
||||||
except Exception:
|
|
||||||
return False
|
|
||||||
|
|
||||||
def get_bool_safe(self, key, default=False):
|
def get_bool_safe(self, key, default=False):
|
||||||
"""Safely get a boolean param with default."""
|
"""Safely get a boolean param with default."""
|
||||||
try:
|
try:
|
||||||
@@ -191,54 +178,12 @@ def get_safe_path(requested_path):
|
|||||||
return None
|
return None
|
||||||
|
|
||||||
|
|
||||||
_CMP_OPS = {
|
|
||||||
ast.Eq: operator.eq,
|
|
||||||
ast.NotEq: operator.ne,
|
|
||||||
ast.Lt: operator.lt,
|
|
||||||
ast.LtE: operator.le,
|
|
||||||
ast.Gt: operator.gt,
|
|
||||||
ast.GtE: operator.ge,
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
def _eval_node(node, context):
|
|
||||||
"""Evaluate a tightly restricted AST node against a context dict.
|
|
||||||
|
|
||||||
Only the operators that SETTINGS conditions actually use are supported:
|
|
||||||
Name lookup, literal Constants, and / or / not, and the six numeric
|
|
||||||
comparisons. No function calls, attribute access, subscripts, or
|
|
||||||
arithmetic — those would re-open the eval-sandbox escape paths.
|
|
||||||
"""
|
|
||||||
if isinstance(node, ast.Expression):
|
|
||||||
return _eval_node(node.body, context)
|
|
||||||
if isinstance(node, ast.Constant):
|
|
||||||
return node.value
|
|
||||||
if isinstance(node, ast.Name):
|
|
||||||
return context.get(node.id, False)
|
|
||||||
if isinstance(node, ast.UnaryOp) and isinstance(node.op, ast.Not):
|
|
||||||
return not _eval_node(node.operand, context)
|
|
||||||
if isinstance(node, ast.BoolOp):
|
|
||||||
values = [_eval_node(v, context) for v in node.values]
|
|
||||||
if isinstance(node.op, ast.And):
|
|
||||||
return all(values)
|
|
||||||
if isinstance(node.op, ast.Or):
|
|
||||||
return any(values)
|
|
||||||
if isinstance(node, ast.Compare) and len(node.ops) == 1 and len(node.comparators) == 1:
|
|
||||||
op_type = type(node.ops[0])
|
|
||||||
if op_type in _CMP_OPS:
|
|
||||||
left = _eval_node(node.left, context)
|
|
||||||
right = _eval_node(node.comparators[0], context)
|
|
||||||
return _CMP_OPS[op_type](left, right)
|
|
||||||
raise ValueError(f"Unsupported node: {type(node).__name__}")
|
|
||||||
|
|
||||||
|
|
||||||
def eval_condition(condition, context):
|
def eval_condition(condition, context):
|
||||||
"""Evaluate a SETTINGS condition expression in a sandboxed AST walker."""
|
"""Safely evaluate a condition string."""
|
||||||
if not condition:
|
if not condition:
|
||||||
return True
|
return True
|
||||||
try:
|
try:
|
||||||
tree = ast.parse(condition, mode='eval')
|
return eval(condition, {"__builtins__": {}}, context)
|
||||||
return bool(_eval_node(tree, context))
|
|
||||||
except Exception as e:
|
except Exception as e:
|
||||||
logger.debug(f"Condition evaluation failed: {condition}, error: {e}")
|
logger.debug(f"Condition evaluation failed: {condition}, error: {e}")
|
||||||
return False
|
return False
|
||||||
@@ -249,41 +194,6 @@ def resolve_value(value):
|
|||||||
return value() if callable(value) else value
|
return value() if callable(value) else value
|
||||||
|
|
||||||
|
|
||||||
# Map of settings-declared param keys to their setting dict.
|
|
||||||
# Used as an allowlist for /api/settings/params/{name} read/write so
|
|
||||||
# LAN clients can only touch keys that the UI knowingly exposes.
|
|
||||||
def _build_param_setting_map():
|
|
||||||
out = {}
|
|
||||||
for section in SETTINGS:
|
|
||||||
for setting in section.get('settings', []):
|
|
||||||
key = setting.get('key')
|
|
||||||
if not key:
|
|
||||||
continue
|
|
||||||
# action_item entries use `key` as the action name, not a real
|
|
||||||
# param — skip so they don't leak into the param read/write
|
|
||||||
# allowlist.
|
|
||||||
if setting.get('type') == 'action_item':
|
|
||||||
continue
|
|
||||||
out[key] = setting
|
|
||||||
return out
|
|
||||||
|
|
||||||
|
|
||||||
_PARAM_SETTINGS = _build_param_setting_map()
|
|
||||||
|
|
||||||
# Control-tab / one-off params the UI legitimately reads or writes that
|
|
||||||
# are not part of the SETTINGS schema. Kept as an explicit allowlist so
|
|
||||||
# the broader 'unknown param' guard still blocks arbitrary writes.
|
|
||||||
_CONTROL_PARAMS = {
|
|
||||||
'dp_dev_go_off_road', # Controls tab: force-offroad toggle
|
|
||||||
'DoReboot', # Controls tab: reboot button
|
|
||||||
'ExperimentalMode', # Tesla HUD: tap set-speed circle to toggle
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
def _param_allowed(key):
|
|
||||||
return key in _PARAM_SETTINGS or key in _CONTROL_PARAMS
|
|
||||||
|
|
||||||
|
|
||||||
# --- API Endpoints ---
|
# --- API Endpoints ---
|
||||||
@api_handler
|
@api_handler
|
||||||
async def init_api(request):
|
async def init_api(request):
|
||||||
@@ -291,7 +201,6 @@ async def init_api(request):
|
|||||||
cache: AppCache = request.app['cache']
|
cache: AppCache = request.app['cache']
|
||||||
return web.json_response({
|
return web.json_response({
|
||||||
'dp_dev_dashy': cache.get_bool_safe("dp_dev_dashy", True),
|
'dp_dev_dashy': cache.get_bool_safe("dp_dev_dashy", True),
|
||||||
'isOffroad': cache.get_bool_safe("IsOffroad", False),
|
|
||||||
})
|
})
|
||||||
|
|
||||||
|
|
||||||
@@ -307,12 +216,6 @@ async def list_files_api(request):
|
|||||||
items = []
|
items = []
|
||||||
for entry in os.listdir(safe_path):
|
for entry in os.listdir(safe_path):
|
||||||
full_path = os.path.join(safe_path, entry)
|
full_path = os.path.join(safe_path, entry)
|
||||||
# Skip entries whose real target escapes DEFAULT_DIR (e.g., symlinks).
|
|
||||||
# get_safe_path only validates the requested directory itself; each
|
|
||||||
# child has to be re-checked to prevent listing files outside the tree.
|
|
||||||
real_full = os.path.realpath(full_path)
|
|
||||||
if os.path.commonpath((real_full, DEFAULT_DIR)) != DEFAULT_DIR:
|
|
||||||
continue
|
|
||||||
try:
|
try:
|
||||||
stat = os.stat(full_path)
|
stat = os.stat(full_path)
|
||||||
is_dir = os.path.isdir(full_path)
|
is_dir = os.path.isdir(full_path)
|
||||||
@@ -342,8 +245,6 @@ async def serve_player_api(request):
|
|||||||
file_path = request.query.get('file')
|
file_path = request.query.get('file')
|
||||||
if not file_path:
|
if not file_path:
|
||||||
return web.Response(text="File parameter is required.", status=400)
|
return web.Response(text="File parameter is required.", status=400)
|
||||||
if get_safe_path(file_path) is None:
|
|
||||||
return web.Response(text="Invalid file path.", status=400)
|
|
||||||
|
|
||||||
player_html_path = os.path.join(WEB_DIST_PATH, 'pages', 'player.html')
|
player_html_path = os.path.join(WEB_DIST_PATH, 'pages', 'player.html')
|
||||||
try:
|
try:
|
||||||
@@ -352,7 +253,7 @@ async def serve_player_api(request):
|
|||||||
except FileNotFoundError:
|
except FileNotFoundError:
|
||||||
return web.Response(text="Player HTML not found.", status=500)
|
return web.Response(text="Player HTML not found.", status=500)
|
||||||
|
|
||||||
html = html_template.replace('{{FILE_PATH}}', quote(file_path, safe=''))
|
html = html_template.replace('{{FILE_PATH}}', quote(file_path))
|
||||||
return web.Response(text=html, content_type='text/html')
|
return web.Response(text=html, content_type='text/html')
|
||||||
|
|
||||||
|
|
||||||
@@ -362,8 +263,6 @@ async def serve_manifest_api(request):
|
|||||||
file_path = request.query.get('file', '').lstrip('/')
|
file_path = request.query.get('file', '').lstrip('/')
|
||||||
if not file_path:
|
if not file_path:
|
||||||
return web.Response(text="File parameter is required.", status=400)
|
return web.Response(text="File parameter is required.", status=400)
|
||||||
if get_safe_path(file_path) is None:
|
|
||||||
return web.Response(text="Invalid file path.", status=400)
|
|
||||||
|
|
||||||
encoded_path = quote(file_path)
|
encoded_path = quote(file_path)
|
||||||
manifest = f"#EXTM3U\n#EXT-X-VERSION:3\n#EXT-X-TARGETDURATION:60\n#EXT-X-PLAYLIST-TYPE:VOD\n#EXTINF:60.0,\n/media/{encoded_path}\n#EXT-X-ENDLIST\n"
|
manifest = f"#EXTM3U\n#EXT-X-VERSION:3\n#EXT-X-TARGETDURATION:60\n#EXT-X-PLAYLIST-TYPE:VOD\n#EXTINF:60.0,\n/media/{encoded_path}\n#EXT-X-ENDLIST\n"
|
||||||
@@ -441,15 +340,6 @@ def _get_setting_value(params, setting):
|
|||||||
elif setting_type == 'double_spin_button_item':
|
elif setting_type == 'double_spin_button_item':
|
||||||
value = params.get(key)
|
value = params.get(key)
|
||||||
return float(value) if value is not None else float(default)
|
return float(value) if value is not None else float(default)
|
||||||
elif setting_type in ('text_input_item', 'text_display_item'):
|
|
||||||
value = params.get(key)
|
|
||||||
if value is None:
|
|
||||||
return ''
|
|
||||||
return value.decode('utf-8', errors='replace') if isinstance(value, bytes) else str(value)
|
|
||||||
elif setting_type == 'action_item':
|
|
||||||
# Pure action buttons have no stored value; return None so the
|
|
||||||
# UI treats it as display-only.
|
|
||||||
return None
|
|
||||||
else: # spin_button_item, text_spin_button_item
|
else: # spin_button_item, text_spin_button_item
|
||||||
value = params.get(key)
|
value = params.get(key)
|
||||||
return int(value) if value is not None else int(default)
|
return int(value) if value is not None else int(default)
|
||||||
@@ -459,10 +349,6 @@ def _get_setting_value(params, setting):
|
|||||||
return False
|
return False
|
||||||
elif setting_type == 'double_spin_button_item':
|
elif setting_type == 'double_spin_button_item':
|
||||||
return float(default)
|
return float(default)
|
||||||
elif setting_type in ('text_input_item', 'text_display_item'):
|
|
||||||
return ''
|
|
||||||
elif setting_type == 'action_item':
|
|
||||||
return None
|
|
||||||
return int(default)
|
return int(default)
|
||||||
|
|
||||||
|
|
||||||
@@ -476,12 +362,6 @@ async def save_param_api(request):
|
|||||||
param_name = request.match_info.get('param_name')
|
param_name = request.match_info.get('param_name')
|
||||||
if not param_name:
|
if not param_name:
|
||||||
return web.json_response({'error': 'param_name is required'}, status=400)
|
return web.json_response({'error': 'param_name is required'}, status=400)
|
||||||
if not _param_allowed(param_name):
|
|
||||||
return web.json_response({'error': 'Unknown param'}, status=403)
|
|
||||||
|
|
||||||
setting = _PARAM_SETTINGS.get(param_name)
|
|
||||||
if setting is not None and setting.get('type') == 'text_display_item':
|
|
||||||
return web.json_response({'error': 'Read-only param'}, status=403)
|
|
||||||
|
|
||||||
cache: AppCache = request.app['cache']
|
cache: AppCache = request.app['cache']
|
||||||
params = cache.params
|
params = cache.params
|
||||||
@@ -520,17 +400,22 @@ def _save_param(params, key, value):
|
|||||||
|
|
||||||
|
|
||||||
def _get_param_value(params, key):
|
def _get_param_value(params, key):
|
||||||
"""Get a single param value via its declared setting type, or as a
|
"""Get a single param value with proper type handling."""
|
||||||
bool for control-only params that have no SETTINGS entry."""
|
try:
|
||||||
setting = _PARAM_SETTINGS.get(key)
|
# Try get_bool first for boolean params
|
||||||
if setting is not None:
|
return params.get_bool(key)
|
||||||
return _get_setting_value(params, setting)
|
except Exception:
|
||||||
if key in _CONTROL_PARAMS:
|
pass
|
||||||
try:
|
|
||||||
return params.get_bool(key)
|
try:
|
||||||
except Exception:
|
raw_value = params.get(key)
|
||||||
return False
|
if raw_value is None:
|
||||||
return None
|
return None
|
||||||
|
elif isinstance(raw_value, bytes):
|
||||||
|
return raw_value.decode('utf-8')
|
||||||
|
return raw_value
|
||||||
|
except Exception:
|
||||||
|
return None
|
||||||
|
|
||||||
|
|
||||||
@api_handler
|
@api_handler
|
||||||
@@ -539,8 +424,6 @@ async def get_param_api(request):
|
|||||||
param_name = request.match_info.get('param_name')
|
param_name = request.match_info.get('param_name')
|
||||||
if not param_name:
|
if not param_name:
|
||||||
return web.json_response({'error': 'param_name is required'}, status=400)
|
return web.json_response({'error': 'param_name is required'}, status=400)
|
||||||
if not _param_allowed(param_name):
|
|
||||||
return web.json_response({'error': 'Unknown param'}, status=403)
|
|
||||||
|
|
||||||
cache: AppCache = request.app['cache']
|
cache: AppCache = request.app['cache']
|
||||||
try:
|
try:
|
||||||
@@ -551,118 +434,18 @@ async def get_param_api(request):
|
|||||||
return web.json_response({'key': param_name, 'value': value})
|
return web.json_response({'key': param_name, 'value': value})
|
||||||
|
|
||||||
|
|
||||||
# --- Action endpoints ---
|
|
||||||
# Named side-effectful operations declared by settings items via the
|
|
||||||
# `action` field (text_input_item / action_item). Each handler receives
|
|
||||||
# the parsed JSON body and the AppCache; it returns a dict that is
|
|
||||||
# serialized as the JSON response. Errors should be raised — the wrapper
|
|
||||||
# converts them to 502/500 responses.
|
|
||||||
SSH_KEY_FETCH_TIMEOUT_S = 10
|
|
||||||
SSH_KEY_MAX_BYTES = 16 * 1024 # plenty for any realistic ~/.ssh/authorized_keys
|
|
||||||
GITHUB_USERNAME_MAX_LEN = 39 # github's own limit
|
|
||||||
|
|
||||||
|
|
||||||
def _validate_github_username(username):
|
|
||||||
"""GitHub username: 1-39 chars, alnum or single hyphen, no leading/trailing hyphen."""
|
|
||||||
if not username or len(username) > GITHUB_USERNAME_MAX_LEN:
|
|
||||||
return False
|
|
||||||
if username.startswith('-') or username.endswith('-'):
|
|
||||||
return False
|
|
||||||
if '--' in username:
|
|
||||||
return False
|
|
||||||
return all(c.isalnum() or c == '-' for c in username)
|
|
||||||
|
|
||||||
|
|
||||||
async def _fetch_github_ssh_keys(username):
|
|
||||||
"""Fetch https://github.com/{username}.keys. Returns the body text on
|
|
||||||
HTTP 200; raises web.HTTPException with an upstream-derived status on
|
|
||||||
failure so the action endpoint surfaces the real reason."""
|
|
||||||
import aiohttp
|
|
||||||
url = f"https://github.com/{quote(username, safe='')}.keys"
|
|
||||||
timeout = aiohttp.ClientTimeout(total=SSH_KEY_FETCH_TIMEOUT_S)
|
|
||||||
async with aiohttp.ClientSession(timeout=timeout) as session:
|
|
||||||
async with session.get(url) as resp:
|
|
||||||
if resp.status == 404:
|
|
||||||
raise web.HTTPNotFound(reason=f"GitHub user '{username}' not found")
|
|
||||||
if resp.status != 200:
|
|
||||||
raise web.HTTPBadGateway(reason=f"github.com returned HTTP {resp.status}")
|
|
||||||
body = await resp.content.read(SSH_KEY_MAX_BYTES + 1)
|
|
||||||
if len(body) > SSH_KEY_MAX_BYTES:
|
|
||||||
raise web.HTTPBadGateway(reason="SSH key response too large")
|
|
||||||
return body.decode('utf-8', errors='replace')
|
|
||||||
|
|
||||||
|
|
||||||
async def _action_ssh_key_set(request, payload, cache):
|
|
||||||
"""Fetch the user's GitHub SSH keys and write both params atomically.
|
|
||||||
Body: { "value": "<github-username>" }. On success the device's
|
|
||||||
sshd_config drop-in is updated by openpilot's own SSH manager."""
|
|
||||||
username = (payload.get('value') or '').strip()
|
|
||||||
if not _validate_github_username(username):
|
|
||||||
raise web.HTTPBadRequest(reason="Invalid GitHub username")
|
|
||||||
|
|
||||||
keys_body = await _fetch_github_ssh_keys(username)
|
|
||||||
if not keys_body.strip():
|
|
||||||
raise web.HTTPBadRequest(reason=f"GitHub user '{username}' has no public SSH keys")
|
|
||||||
|
|
||||||
params = cache.params
|
|
||||||
# Write keys first; only commit the username if keys were stored
|
|
||||||
# successfully — keeps the two params consistent.
|
|
||||||
params.put('GithubSshKeys', keys_body)
|
|
||||||
params.put('GithubUsername', username)
|
|
||||||
cache.invalidate()
|
|
||||||
logger.info(f"SSH keys set from github.com/{username} ({len(keys_body)} bytes)")
|
|
||||||
return {'status': 'ok', 'username': username, 'key_bytes': len(keys_body)}
|
|
||||||
|
|
||||||
|
|
||||||
async def _action_ssh_key_clear(request, payload, cache):
|
|
||||||
params = cache.params
|
|
||||||
params.put('GithubSshKeys', '')
|
|
||||||
params.put('GithubUsername', '')
|
|
||||||
cache.invalidate()
|
|
||||||
logger.info("SSH keys cleared")
|
|
||||||
return {'status': 'ok'}
|
|
||||||
|
|
||||||
|
|
||||||
_ACTION_HANDLERS = {
|
|
||||||
'ssh_key_set': _action_ssh_key_set,
|
|
||||||
'ssh_key_clear': _action_ssh_key_clear,
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
@api_handler
|
|
||||||
async def run_action_api(request):
|
|
||||||
"""Dispatch /api/action/{name} → registered handler."""
|
|
||||||
name = request.match_info.get('name', '')
|
|
||||||
handler = _ACTION_HANDLERS.get(name)
|
|
||||||
if handler is None:
|
|
||||||
return web.json_response({'error': f'Unknown action: {name}'}, status=404)
|
|
||||||
|
|
||||||
try:
|
|
||||||
payload = await request.json()
|
|
||||||
except Exception:
|
|
||||||
payload = {}
|
|
||||||
|
|
||||||
cache: AppCache = request.app['cache']
|
|
||||||
result = await handler(request, payload, cache)
|
|
||||||
return web.json_response(result)
|
|
||||||
|
|
||||||
|
|
||||||
@api_handler
|
@api_handler
|
||||||
async def get_model_list_api(request):
|
async def get_model_list_api(request):
|
||||||
"""Get the model list and current selection."""
|
"""Get the model list and current selection."""
|
||||||
cache: AppCache = request.app['cache']
|
cache: AppCache = request.app['cache']
|
||||||
params = cache.params
|
params = cache.params
|
||||||
|
|
||||||
# Get model list. JSON-typed params come back already-parsed in
|
# Get model list
|
||||||
# newer dragonpilot; older builds returned bytes/str — handle both.
|
|
||||||
model_list = {}
|
model_list = {}
|
||||||
try:
|
try:
|
||||||
raw = params.get("dp_dev_model_list")
|
model_list_raw = params.get("dp_dev_model_list")
|
||||||
if raw:
|
if model_list_raw:
|
||||||
if isinstance(raw, (bytes, str)):
|
model_list = json.loads(model_list_raw)
|
||||||
model_list = json.loads(raw)
|
|
||||||
elif isinstance(raw, dict):
|
|
||||||
model_list = raw
|
|
||||||
except Exception as e:
|
except Exception as e:
|
||||||
logger.debug(f"Could not parse dp_dev_model_list: {e}")
|
logger.debug(f"Could not parse dp_dev_model_list: {e}")
|
||||||
|
|
||||||
@@ -700,82 +483,423 @@ async def save_model_selection_api(request):
|
|||||||
return web.json_response({'status': 'success'})
|
return web.json_response({'status': 'success'})
|
||||||
|
|
||||||
|
|
||||||
# --- WebSocket endpoint for data streaming ---
|
@api_handler
|
||||||
# One shared publisher task polls the dashyState SubMaster and fans out
|
async def webrtc_stream_proxy(request):
|
||||||
# to every connected client. The previous per-connection design ran
|
"""Proxy WebRTC stream requests to webrtcd."""
|
||||||
# blocking ZMQ I/O on the event loop, which starved every other request
|
host = request.host.split(':')[0]
|
||||||
# under multi-client load.
|
body = await request.read()
|
||||||
async def _publisher_loop(app):
|
session: ClientSession = request.app['http_session']
|
||||||
# IMPORTANT: ZMQ sockets are thread-affined. Construct the SubMaster on
|
|
||||||
# the asyncio main thread and call update() on the same thread — using
|
|
||||||
# asyncio.to_thread bounces between worker threads and silently breaks
|
|
||||||
# the receive. The 0-timeout update is cheap enough on the event loop;
|
|
||||||
# the per-client send is what we actually need to be async for.
|
|
||||||
try:
|
try:
|
||||||
sm = messaging.SubMaster(['dashyState'])
|
async with session.post(
|
||||||
|
f'http://{host}:5001/stream',
|
||||||
|
data=body,
|
||||||
|
headers={'Content-Type': 'application/json'}
|
||||||
|
) as resp:
|
||||||
|
response_body = await resp.read()
|
||||||
|
return web.Response(
|
||||||
|
body=response_body,
|
||||||
|
status=resp.status,
|
||||||
|
content_type=resp.content_type
|
||||||
|
)
|
||||||
|
except ClientConnectorError:
|
||||||
|
# webrtcd not running - return 503 Service Unavailable
|
||||||
|
return web.json_response(
|
||||||
|
{'error': 'Stream service unavailable', 'code': 'WEBRTCD_UNAVAILABLE'},
|
||||||
|
status=503
|
||||||
|
)
|
||||||
|
|
||||||
|
# --- Navigation API Endpoints ---
|
||||||
|
@api_handler
|
||||||
|
async def nav_get_destination_api(request):
|
||||||
|
"""Get current navigation destination.
|
||||||
|
|
||||||
|
GET /api/nav/destination
|
||||||
|
Returns: { latitude, longitude, name } or {}
|
||||||
|
"""
|
||||||
|
cache: AppCache = request.app['cache']
|
||||||
|
params = cache.params
|
||||||
|
|
||||||
|
destination = {}
|
||||||
|
try:
|
||||||
|
# JSON type params return dict directly
|
||||||
|
dest = params.get("dp_maa_destination")
|
||||||
|
if dest:
|
||||||
|
destination = dest
|
||||||
except Exception as e:
|
except Exception as e:
|
||||||
logger.warning(f"Publisher disabled (SubMaster init failed): {e}")
|
logger.debug(f"Could not get dp_maa_destination: {e}")
|
||||||
return
|
|
||||||
|
|
||||||
logger.info("dashyState publisher loop started")
|
return web.json_response(destination)
|
||||||
|
|
||||||
while True:
|
|
||||||
|
@api_handler
|
||||||
|
async def nav_set_destination_api(request):
|
||||||
|
"""Set navigation destination.
|
||||||
|
|
||||||
|
POST /api/nav/destination
|
||||||
|
Body: { "latitude": float, "longitude": float, "name": string (optional) }
|
||||||
|
"""
|
||||||
|
cache: AppCache = request.app['cache']
|
||||||
|
params = cache.params
|
||||||
|
data = await request.json()
|
||||||
|
|
||||||
|
if 'latitude' not in data or 'longitude' not in data:
|
||||||
|
return web.json_response({'error': 'latitude and longitude required'}, status=400)
|
||||||
|
|
||||||
|
destination = {
|
||||||
|
'latitude': float(data['latitude']),
|
||||||
|
'longitude': float(data['longitude']),
|
||||||
|
'name': data.get('name', '')
|
||||||
|
}
|
||||||
|
|
||||||
|
try:
|
||||||
|
# Use native dict with put_nonblocking for JSON types
|
||||||
|
params.put_nonblocking("dp_maa_destination", destination)
|
||||||
|
logger.info(f"Nav destination set: {destination['latitude']:.6f}, {destination['longitude']:.6f}")
|
||||||
|
except Exception as e:
|
||||||
|
logger.warning(f"Could not save NavDestination to params: {e}")
|
||||||
|
|
||||||
|
return web.json_response({'status': 'success', 'destination': destination})
|
||||||
|
|
||||||
|
|
||||||
|
@api_handler
|
||||||
|
async def nav_clear_destination_api(request):
|
||||||
|
"""Clear navigation destination.
|
||||||
|
|
||||||
|
DELETE /api/nav/destination
|
||||||
|
"""
|
||||||
|
cache: AppCache = request.app['cache']
|
||||||
|
params = cache.params
|
||||||
|
try:
|
||||||
|
params.remove("dp_maa_destination")
|
||||||
|
logger.info("Nav destination cleared")
|
||||||
|
except Exception as e:
|
||||||
|
logger.warning(f"Could not remove NavDestination from params: {e}")
|
||||||
|
return web.json_response({'status': 'success'})
|
||||||
|
|
||||||
|
|
||||||
|
@api_handler
|
||||||
|
async def nav_search_api(request):
|
||||||
|
"""Search for places/addresses.
|
||||||
|
|
||||||
|
GET /api/nav/search?q=<query>&lat=<lat>&lon=<lon>&limit=<limit>
|
||||||
|
Returns: [{ name, address, latitude, longitude, distance }, ...]
|
||||||
|
"""
|
||||||
|
query = request.query.get('q', '').strip()
|
||||||
|
if not query or len(query) < 2:
|
||||||
|
return web.json_response([])
|
||||||
|
|
||||||
|
# Parse optional proximity
|
||||||
|
proximity = None
|
||||||
|
lat_str = request.query.get('lat')
|
||||||
|
lon_str = request.query.get('lon')
|
||||||
|
if lat_str and lon_str:
|
||||||
try:
|
try:
|
||||||
sm.update(0)
|
proximity = Coordinate(float(lat_str), float(lon_str))
|
||||||
if sm.updated['dashyState']:
|
except ValueError:
|
||||||
json_data = sm['dashyState'].json
|
pass
|
||||||
if isinstance(json_data, bytes):
|
|
||||||
json_data = json_data.decode('utf-8')
|
|
||||||
|
|
||||||
clients = list(app['ws_clients'])
|
limit = min(int(request.query.get('limit', 10)), 20)
|
||||||
for ws in clients:
|
|
||||||
if ws.closed:
|
|
||||||
app['ws_clients'].discard(ws)
|
|
||||||
continue
|
|
||||||
try:
|
|
||||||
await ws.send_str(json_data)
|
|
||||||
except Exception as e:
|
|
||||||
logger.debug(f"WebSocket send failed, dropping client: {e}")
|
|
||||||
app['ws_clients'].discard(ws)
|
|
||||||
|
|
||||||
await asyncio.sleep(0.01)
|
# Get map service from app cache
|
||||||
except asyncio.CancelledError:
|
map_service: MapService = request.app.get('map_service')
|
||||||
raise
|
if not map_service:
|
||||||
except Exception as e:
|
cache: AppCache = request.app['cache']
|
||||||
# Don't let a transient error tear down the loop silently.
|
map_service = MapService(cache.params)
|
||||||
logger.exception(f"Publisher loop error: {e}")
|
request.app['map_service'] = map_service
|
||||||
await asyncio.sleep(0.1)
|
|
||||||
|
try:
|
||||||
|
results = await map_service.search_provider.search(query, proximity, limit)
|
||||||
|
# Log which provider was used
|
||||||
|
if results:
|
||||||
|
provider = results[0].provider if hasattr(results[0], 'provider') else 'unknown'
|
||||||
|
logger.info(f"Search '{query}' returned {len(results)} results via {provider}")
|
||||||
|
return web.json_response([
|
||||||
|
{
|
||||||
|
'name': r.name,
|
||||||
|
'address': r.address,
|
||||||
|
'latitude': r.coordinate.latitude,
|
||||||
|
'longitude': r.coordinate.longitude,
|
||||||
|
'distance': r.distance,
|
||||||
|
}
|
||||||
|
for r in results
|
||||||
|
])
|
||||||
|
except Exception as e:
|
||||||
|
logger.error(f"Search error: {e}")
|
||||||
|
return web.json_response([])
|
||||||
|
|
||||||
|
|
||||||
|
@api_handler
|
||||||
|
async def nav_route_api(request):
|
||||||
|
"""Calculate route between two points.
|
||||||
|
|
||||||
|
POST /api/nav/route
|
||||||
|
Body: { "start": {"lat": float, "lon": float}, "end": {"lat": float, "lon": float} }
|
||||||
|
Returns: { distance_m, duration_s, polyline, maneuvers, has_traffic }
|
||||||
|
"""
|
||||||
|
data = await request.json()
|
||||||
|
|
||||||
|
start = data.get('start', {})
|
||||||
|
end = data.get('end', {})
|
||||||
|
|
||||||
|
if not all([start.get('lat'), start.get('lon'), end.get('lat'), end.get('lon')]):
|
||||||
|
return web.json_response({'error': 'start and end coordinates required'}, status=400)
|
||||||
|
|
||||||
|
origin = Coordinate(float(start['lat']), float(start['lon']))
|
||||||
|
destination = Coordinate(float(end['lat']), float(end['lon']))
|
||||||
|
|
||||||
|
# Get map service
|
||||||
|
map_service: MapService = request.app.get('map_service')
|
||||||
|
if not map_service:
|
||||||
|
cache: AppCache = request.app['cache']
|
||||||
|
map_service = MapService(cache.params)
|
||||||
|
request.app['map_service'] = map_service
|
||||||
|
|
||||||
|
try:
|
||||||
|
route = await map_service.route_provider.get_route(origin, destination)
|
||||||
|
if not route:
|
||||||
|
return web.json_response({'error': 'No route found'}, status=404)
|
||||||
|
|
||||||
|
logger.info(f"Route calculated: {route.distance/1000:.1f}km via {route.provider}")
|
||||||
|
return web.json_response({
|
||||||
|
'distance_m': route.distance,
|
||||||
|
'duration_s': route.duration,
|
||||||
|
'polyline': _encode_polyline(route.geometry) if route.geometry else '',
|
||||||
|
'geometry': [[c.latitude, c.longitude] for c in route.geometry] if route.geometry else [],
|
||||||
|
'maneuvers': [
|
||||||
|
{
|
||||||
|
'instruction': step.name or '',
|
||||||
|
'distance_m': step.distance,
|
||||||
|
'duration_s': step.duration,
|
||||||
|
'position': {
|
||||||
|
'lat': step.maneuver_point.latitude,
|
||||||
|
'lon': step.maneuver_point.longitude
|
||||||
|
} if step.maneuver_point else None,
|
||||||
|
'type': step.maneuver_type,
|
||||||
|
'modifier': step.maneuver_modifier,
|
||||||
|
}
|
||||||
|
for step in route.steps
|
||||||
|
],
|
||||||
|
'has_traffic': route.has_traffic,
|
||||||
|
'provider': route.provider,
|
||||||
|
})
|
||||||
|
except Exception as e:
|
||||||
|
logger.error(f"Route error: {e}")
|
||||||
|
return web.json_response({'error': str(e)}, status=500)
|
||||||
|
|
||||||
|
|
||||||
|
def _encode_polyline(coordinates: list) -> str:
|
||||||
|
"""Encode coordinates to Google polyline format."""
|
||||||
|
if not coordinates:
|
||||||
|
return ''
|
||||||
|
|
||||||
|
result = []
|
||||||
|
prev_lat = 0
|
||||||
|
prev_lon = 0
|
||||||
|
|
||||||
|
for coord in coordinates:
|
||||||
|
lat = int(round(coord.latitude * 1e5))
|
||||||
|
lon = int(round(coord.longitude * 1e5))
|
||||||
|
|
||||||
|
d_lat = lat - prev_lat
|
||||||
|
d_lon = lon - prev_lon
|
||||||
|
|
||||||
|
for val in [d_lat, d_lon]:
|
||||||
|
val = ~(val << 1) if val < 0 else (val << 1)
|
||||||
|
while val >= 0x20:
|
||||||
|
result.append(chr((0x20 | (val & 0x1f)) + 63))
|
||||||
|
val >>= 5
|
||||||
|
result.append(chr(val + 63))
|
||||||
|
|
||||||
|
prev_lat = lat
|
||||||
|
prev_lon = lon
|
||||||
|
|
||||||
|
return ''.join(result)
|
||||||
|
|
||||||
|
|
||||||
|
@api_handler
|
||||||
|
async def nav_tiles_config_api(request):
|
||||||
|
"""Get tile provider configuration.
|
||||||
|
|
||||||
|
GET /api/nav/tiles/config
|
||||||
|
Returns: { url_template, style_url, attribution, min_zoom, max_zoom }
|
||||||
|
"""
|
||||||
|
map_service: MapService = request.app.get('map_service')
|
||||||
|
if not map_service:
|
||||||
|
cache: AppCache = request.app['cache']
|
||||||
|
map_service = MapService(cache.params)
|
||||||
|
request.app['map_service'] = map_service
|
||||||
|
|
||||||
|
try:
|
||||||
|
config = map_service.tile_provider.get_tile_config()
|
||||||
|
return web.json_response({
|
||||||
|
'url_template': config.url_template,
|
||||||
|
'style_url': config.style_url,
|
||||||
|
'attribution': config.attribution,
|
||||||
|
'min_zoom': config.min_zoom,
|
||||||
|
'max_zoom': config.max_zoom,
|
||||||
|
})
|
||||||
|
except Exception as e:
|
||||||
|
logger.error(f"Tile config error: {e}")
|
||||||
|
return web.json_response({'error': str(e)}, status=500)
|
||||||
|
|
||||||
|
|
||||||
|
# --- Places API (Favorites + Recent) ---
|
||||||
|
# In-memory cache (persists for server session even if params fails)
|
||||||
|
_places_cache = {"home": None, "work": None, "recent": []}
|
||||||
|
|
||||||
|
def _get_places(params) -> dict:
|
||||||
|
"""Get places data from dp_maa_places param or memory cache."""
|
||||||
|
global _places_cache
|
||||||
|
try:
|
||||||
|
# JSON type params return dict/list directly
|
||||||
|
data = params.get("dp_maa_places")
|
||||||
|
if data:
|
||||||
|
_places_cache = data # sync to memory
|
||||||
|
return data
|
||||||
|
except Exception as e:
|
||||||
|
logger.debug(f"Could not parse dp_maa_places: {e}")
|
||||||
|
return _places_cache
|
||||||
|
|
||||||
|
|
||||||
|
def _save_places(params, places: dict):
|
||||||
|
"""Save places data to dp_maa_places param and memory cache."""
|
||||||
|
global _places_cache
|
||||||
|
_places_cache = places # always save to memory first
|
||||||
|
try:
|
||||||
|
# JSON type params accept dict/list directly
|
||||||
|
params.put("dp_maa_places", places)
|
||||||
|
except Exception as e:
|
||||||
|
logger.warning(f"Failed to save places to params: {e}")
|
||||||
|
|
||||||
|
|
||||||
|
def _haversine_distance(lat1, lon1, lat2, lon2) -> float:
|
||||||
|
"""Calculate distance between two points in meters."""
|
||||||
|
import math
|
||||||
|
R = 6371000
|
||||||
|
phi1, phi2 = math.radians(lat1), math.radians(lat2)
|
||||||
|
d_phi = math.radians(lat2 - lat1)
|
||||||
|
d_lambda = math.radians(lon2 - lon1)
|
||||||
|
a = math.sin(d_phi / 2) ** 2 + math.cos(phi1) * math.cos(phi2) * math.sin(d_lambda / 2) ** 2
|
||||||
|
return R * 2 * math.atan2(math.sqrt(a), math.sqrt(1 - a))
|
||||||
|
|
||||||
|
|
||||||
|
def _add_to_recent(places: dict, place: dict) -> dict:
|
||||||
|
"""Add a place to recent list with deduplication."""
|
||||||
|
recent = [r for r in places.get("recent", [])
|
||||||
|
if _haversine_distance(r["lat"], r["lon"], place["lat"], place["lon"]) > 100]
|
||||||
|
recent.insert(0, place)
|
||||||
|
places["recent"] = recent[:5]
|
||||||
|
return places
|
||||||
|
|
||||||
|
|
||||||
|
@api_handler
|
||||||
|
async def nav_get_places_api(request):
|
||||||
|
"""GET /api/nav/places - Get all places."""
|
||||||
|
cache: AppCache = request.app['cache']
|
||||||
|
return web.json_response(_get_places(cache.params))
|
||||||
|
|
||||||
|
|
||||||
|
@api_handler
|
||||||
|
async def nav_set_place_api(request):
|
||||||
|
"""POST /api/nav/places/{place_type} - Set home or work."""
|
||||||
|
place_type = request.match_info.get('place_type')
|
||||||
|
if place_type not in ('home', 'work'):
|
||||||
|
return web.json_response({'error': 'Invalid place type'}, status=400)
|
||||||
|
|
||||||
|
cache: AppCache = request.app['cache']
|
||||||
|
data = await request.json()
|
||||||
|
if 'lat' not in data or 'lon' not in data:
|
||||||
|
return web.json_response({'error': 'lat and lon required'}, status=400)
|
||||||
|
|
||||||
|
places = _get_places(cache.params)
|
||||||
|
places[place_type] = {
|
||||||
|
'name': data.get('name', place_type.capitalize()),
|
||||||
|
'address': data.get('address', ''),
|
||||||
|
'lat': float(data['lat']),
|
||||||
|
'lon': float(data['lon'])
|
||||||
|
}
|
||||||
|
_save_places(cache.params, places)
|
||||||
|
return web.json_response({'success': True, place_type: places[place_type]})
|
||||||
|
|
||||||
|
|
||||||
|
@api_handler
|
||||||
|
async def nav_delete_place_api(request):
|
||||||
|
"""DELETE /api/nav/places/{place_type} - Delete home or work."""
|
||||||
|
place_type = request.match_info.get('place_type')
|
||||||
|
if place_type not in ('home', 'work'):
|
||||||
|
return web.json_response({'error': 'Invalid place type'}, status=400)
|
||||||
|
|
||||||
|
cache: AppCache = request.app['cache']
|
||||||
|
places = _get_places(cache.params)
|
||||||
|
places[place_type] = None
|
||||||
|
_save_places(cache.params, places)
|
||||||
|
return web.json_response({'success': True})
|
||||||
|
|
||||||
|
|
||||||
|
@api_handler
|
||||||
|
async def nav_add_recent_api(request):
|
||||||
|
"""POST /api/nav/places/recent - Add to recent."""
|
||||||
|
cache: AppCache = request.app['cache']
|
||||||
|
data = await request.json()
|
||||||
|
if 'lat' not in data or 'lon' not in data:
|
||||||
|
return web.json_response({'error': 'lat and lon required'}, status=400)
|
||||||
|
|
||||||
|
places = _get_places(cache.params)
|
||||||
|
place = {'name': data.get('name', 'Unknown'), 'address': data.get('address', ''),
|
||||||
|
'lat': float(data['lat']), 'lon': float(data['lon'])}
|
||||||
|
places = _add_to_recent(places, place)
|
||||||
|
_save_places(cache.params, places)
|
||||||
|
return web.json_response({'success': True, 'recent': places['recent']})
|
||||||
|
|
||||||
|
|
||||||
|
@api_handler
|
||||||
|
async def nav_clear_recent_api(request):
|
||||||
|
"""DELETE /api/nav/places/recent - Clear recent."""
|
||||||
|
cache: AppCache = request.app['cache']
|
||||||
|
places = _get_places(cache.params)
|
||||||
|
places['recent'] = []
|
||||||
|
_save_places(cache.params, places)
|
||||||
|
return web.json_response({'success': True})
|
||||||
|
|
||||||
|
|
||||||
|
# --- WebSocket endpoint for data streaming ---
|
||||||
async def websocket_handler(request):
|
async def websocket_handler(request):
|
||||||
"""WebSocket endpoint for data-only connections - streams dashyState directly."""
|
"""WebSocket endpoint for data-only connections - streams dashyState directly."""
|
||||||
ws = web.WebSocketResponse()
|
ws = web.WebSocketResponse()
|
||||||
await ws.prepare(request)
|
await ws.prepare(request)
|
||||||
|
|
||||||
logger.info("WebSocket client connected")
|
logger.info("WebSocket client connected")
|
||||||
request.app['ws_clients'].add(ws)
|
|
||||||
|
# Create a SubMaster for this connection
|
||||||
|
sm = messaging.SubMaster(['dashyState'])
|
||||||
|
|
||||||
try:
|
try:
|
||||||
# Wait until the client disconnects; no inbound traffic expected.
|
while not ws.closed:
|
||||||
async for _ in ws:
|
sm.update(0)
|
||||||
pass
|
if sm.updated['dashyState']:
|
||||||
|
json_data = sm['dashyState'].json
|
||||||
|
if isinstance(json_data, bytes):
|
||||||
|
json_data = json_data.decode('utf-8')
|
||||||
|
await ws.send_str(json_data)
|
||||||
|
await asyncio.sleep(0.01)
|
||||||
except Exception as e:
|
except Exception as e:
|
||||||
logger.warning(f"WebSocket error: {e}")
|
logger.warning(f"WebSocket error: {e}")
|
||||||
finally:
|
finally:
|
||||||
request.app['ws_clients'].discard(ws)
|
|
||||||
logger.info("WebSocket client disconnected")
|
logger.info("WebSocket client disconnected")
|
||||||
|
|
||||||
return ws
|
return ws
|
||||||
|
|
||||||
|
|
||||||
# --- No-cache middleware for web assets ---
|
# --- CORS Middleware ---
|
||||||
# Dashy is a same-origin LAN app; no CORS headers are emitted so that
|
|
||||||
# browsers will block cross-origin JS from mutating settings via the
|
|
||||||
# JSON endpoints (the preflight will fail for non-simple requests).
|
|
||||||
@web.middleware
|
@web.middleware
|
||||||
async def no_cache_middleware(request, handler):
|
async def cors_middleware(request, handler):
|
||||||
response = await handler(request)
|
response = await handler(request)
|
||||||
|
response.headers['Access-Control-Allow-Origin'] = '*'
|
||||||
|
response.headers['Access-Control-Allow-Methods'] = 'GET, POST, PUT, DELETE, OPTIONS'
|
||||||
|
response.headers['Access-Control-Allow-Headers'] = 'Content-Type, Authorization'
|
||||||
|
|
||||||
|
# Disable caching for web assets
|
||||||
path = request.path.lower()
|
path = request.path.lower()
|
||||||
if path.endswith(('.html', '.js', '.css')) or path == '/':
|
if path.endswith(('.html', '.js', '.css')) or path == '/':
|
||||||
response.headers['Cache-Control'] = 'no-cache, no-store, must-revalidate'
|
response.headers['Cache-Control'] = 'no-cache, no-store, must-revalidate'
|
||||||
@@ -785,24 +909,26 @@ async def no_cache_middleware(request, handler):
|
|||||||
return response
|
return response
|
||||||
|
|
||||||
|
|
||||||
|
async def handle_cors_preflight(request):
|
||||||
|
return web.Response(status=200, headers={
|
||||||
|
'Access-Control-Allow-Origin': '*',
|
||||||
|
'Access-Control-Allow-Methods': 'GET, POST, PUT, DELETE, OPTIONS',
|
||||||
|
'Access-Control-Allow-Headers': 'Content-Type, Authorization',
|
||||||
|
'Access-Control-Max-Age': '86400',
|
||||||
|
})
|
||||||
|
|
||||||
|
|
||||||
# --- Application Setup ---
|
# --- Application Setup ---
|
||||||
async def on_startup(app):
|
async def on_startup(app):
|
||||||
"""Initialize app-level resources."""
|
"""Initialize app-level resources."""
|
||||||
app['cache'] = AppCache()
|
app['cache'] = AppCache()
|
||||||
app['ws_clients'] = set()
|
app['http_session'] = ClientSession(timeout=WEBRTC_TIMEOUT)
|
||||||
app['publisher_task'] = asyncio.create_task(_publisher_loop(app))
|
|
||||||
logger.info("Dashy server started")
|
logger.info("Dashy server started")
|
||||||
|
|
||||||
|
|
||||||
async def on_cleanup(app):
|
async def on_cleanup(app):
|
||||||
"""Cleanup app-level resources."""
|
"""Cleanup app-level resources."""
|
||||||
task = app.get('publisher_task')
|
await app['http_session'].close()
|
||||||
if task and not task.done():
|
|
||||||
task.cancel()
|
|
||||||
try:
|
|
||||||
await task
|
|
||||||
except (asyncio.CancelledError, Exception):
|
|
||||||
pass
|
|
||||||
logger.info("Dashy server stopped")
|
logger.info("Dashy server stopped")
|
||||||
|
|
||||||
|
|
||||||
@@ -812,7 +938,8 @@ def setup_aiohttp_app(host: str, port: int, debug: bool):
|
|||||||
format='%(asctime)s - %(name)s - %(levelname)s - %(message)s'
|
format='%(asctime)s - %(name)s - %(levelname)s - %(message)s'
|
||||||
)
|
)
|
||||||
|
|
||||||
app = web.Application(middlewares=[no_cache_middleware])
|
app = web.Application(middlewares=[cors_middleware])
|
||||||
|
app['port'] = port
|
||||||
|
|
||||||
# API routes
|
# API routes
|
||||||
app.router.add_get("/api/init", init_api)
|
app.router.add_get("/api/init", init_api)
|
||||||
@@ -824,9 +951,26 @@ def setup_aiohttp_app(host: str, port: int, debug: bool):
|
|||||||
app.router.add_post("/api/settings/params/{param_name}", save_param_api)
|
app.router.add_post("/api/settings/params/{param_name}", save_param_api)
|
||||||
app.router.add_get("/api/models", get_model_list_api)
|
app.router.add_get("/api/models", get_model_list_api)
|
||||||
app.router.add_post("/api/models/select", save_model_selection_api)
|
app.router.add_post("/api/models/select", save_model_selection_api)
|
||||||
app.router.add_post("/api/action/{name}", run_action_api)
|
app.router.add_post("/api/stream", webrtc_stream_proxy)
|
||||||
app.router.add_get("/api/ws", websocket_handler) # WebSocket for data streaming
|
app.router.add_get("/api/ws", websocket_handler) # WebSocket for data streaming
|
||||||
|
|
||||||
|
# Navigation routes
|
||||||
|
app.router.add_get("/api/nav/destination", nav_get_destination_api)
|
||||||
|
app.router.add_post("/api/nav/destination", nav_set_destination_api)
|
||||||
|
app.router.add_delete("/api/nav/destination", nav_clear_destination_api)
|
||||||
|
app.router.add_get("/api/nav/search", nav_search_api)
|
||||||
|
app.router.add_post("/api/nav/route", nav_route_api)
|
||||||
|
app.router.add_get("/api/nav/tiles/config", nav_tiles_config_api)
|
||||||
|
|
||||||
|
# Places routes (favorites + recent) - specific routes before parametrized
|
||||||
|
app.router.add_get("/api/nav/places", nav_get_places_api)
|
||||||
|
app.router.add_post("/api/nav/places/recent", nav_add_recent_api)
|
||||||
|
app.router.add_delete("/api/nav/places/recent", nav_clear_recent_api)
|
||||||
|
app.router.add_post("/api/nav/places/{place_type}", nav_set_place_api)
|
||||||
|
app.router.add_delete("/api/nav/places/{place_type}", nav_delete_place_api)
|
||||||
|
|
||||||
|
app.router.add_route('OPTIONS', '/{tail:.*}', handle_cors_preflight)
|
||||||
|
|
||||||
# Static files
|
# Static files
|
||||||
app.router.add_static('/media', path=DEFAULT_DIR, name='media', show_index=False, follow_symlinks=False)
|
app.router.add_static('/media', path=DEFAULT_DIR, name='media', show_index=False, follow_symlinks=False)
|
||||||
app.router.add_static('/download', path=DEFAULT_DIR, name='download', show_index=False, follow_symlinks=False)
|
app.router.add_static('/download', path=DEFAULT_DIR, name='download', show_index=False, follow_symlinks=False)
|
||||||
|
|||||||
|
After Width: | Height: | Size: 40 KiB |
|
After Width: | Height: | Size: 40 KiB |
|
After Width: | Height: | Size: 30 KiB |
|
After Width: | Height: | Size: 34 KiB |
@@ -0,0 +1,4 @@
|
|||||||
|
<svg id="WORKING_ICONS" data-name="WORKING ICONS" xmlns="http://www.w3.org/2000/svg" width="20" height="20" viewBox="0 0 20 20">
|
||||||
|
<title>direction</title>
|
||||||
|
<path fill="#FFFFFF" d="M10,5a2,2,0,1,1,2-2A2,2,0,0,1,10,5Zm4.91284,8.35114L10.00916,7.0083,5.10547,13.35114a0.38659,0.38659,0,0,0,.40942.62354l2.95184-1.34375A0.35542,0.35542,0,0,1,9.00769,13H9v5.50006A0.49992,0.49992,0,0,0,9.49994,19h1.00012A0.49992,0.49992,0,0,0,11,18.50006V13.0083h0.00916a0.35757,0.35757,0,0,1,.54242-0.37738l2.95184,1.34375A0.3866,0.3866,0,0,0,14.91284,13.35114Z"/>
|
||||||
|
</svg>
|
||||||
|
After Width: | Height: | Size: 554 B |
@@ -0,0 +1,4 @@
|
|||||||
|
<svg id="WORKING_ICONS" data-name="WORKING ICONS" xmlns="http://www.w3.org/2000/svg" width="20" height="20" viewBox="0 0 20 20">
|
||||||
|
<title>direction</title>
|
||||||
|
<path fill="#FFFFFF" d="M3,12a2,2,0,1,1,2-2A2,2,0,0,1,3,12Zm10.00293-.96332a4.05782,4.05782,0,0,1,3.98877,4.07324H17v1.37775A0.51232,0.51232,0,0,0,17.51233,17h0.97534A0.51232,0.51232,0,0,0,19,16.48767V15H18.98615a6.05607,6.05607,0,0,0-5.9834-5.96332l-0.011-.00183,0.00012,0.02008H12V9.04584a0.35757,0.35757,0,0,1-.37738-0.54242l1.34375-2.95184a0.38659,0.38659,0,0,0-.62354-0.40942L6,10.04584l6.34283,4.90369a0.3866,0.3866,0,0,0,.62354-0.40942l-1.34375-2.95184A0.35757,0.35757,0,0,1,12,11.04584v0.00909h1"/>
|
||||||
|
</svg>
|
||||||
|
After Width: | Height: | Size: 672 B |
@@ -0,0 +1,4 @@
|
|||||||
|
<svg id="WORKING_ICONS" data-name="WORKING ICONS" xmlns="http://www.w3.org/2000/svg" width="20" height="20" viewBox="0 0 20 20">
|
||||||
|
<title>direction</title>
|
||||||
|
<path fill="#FFFFFF" d="M15,10a2,2,0,1,1,2,2A2,2,0,0,1,15,10ZM7,11.05493H8V11.04584a0.35757,0.35757,0,0,1,.37738.54242L7.03363,14.5401a0.3866,0.3866,0,0,0,.62354.40942L14,10.04584,7.65717,5.14215a0.38659,0.38659,0,0,0-.62354.40942L8.37738,8.50342A0.35757,0.35757,0,0,1,8,9.04584V9.05493H7.00818L7.0083,9.03485l-0.011.00183A6.05607,6.05607,0,0,0,1.01385,15H1v1.48767A0.51232,0.51232,0,0,0,1.51233,17H2.48767A0.51232,0.51232,0,0,0,3,16.48767V15.10992H3.0083a4.05782,4.05782,0,0,1,3.98877-4.07324"/>
|
||||||
|
</svg>
|
||||||
|
After Width: | Height: | Size: 662 B |
@@ -0,0 +1,4 @@
|
|||||||
|
<svg id="WORKING_ICONS" data-name="WORKING ICONS" xmlns="http://www.w3.org/2000/svg" width="20" height="20" viewBox="0 0 20 20">
|
||||||
|
<title>direction</title>
|
||||||
|
<path fill="#FFFFFF" d="M10,5a2,2,0,1,1,2-2A2,2,0,0,1,10,5Zm4.91284,8.35114L10.00916,7.0083,5.10547,13.35114a0.38659,0.38659,0,0,0,.40942.62354l2.95184-1.34375A0.35542,0.35542,0,0,1,9.00769,13H9v5.50006A0.49992,0.49992,0,0,0,9.49994,19h1.00012A0.49992,0.49992,0,0,0,11,18.50006V13.0083h0.00916a0.35757,0.35757,0,0,1,.54242-0.37738l2.95184,1.34375A0.3866,0.3866,0,0,0,14.91284,13.35114Z"/>
|
||||||
|
</svg>
|
||||||
|
After Width: | Height: | Size: 554 B |