mirror of
https://github.com/sunnypilot/sunnypilot.git
synced 2026-11-22 22:53:42 +08:00
Compare commits
57 Commits
| Author | SHA1 | Date | |
|---|---|---|---|
| ba0f1859eb | |||
| 571a2fcb29 | |||
| 093c73e785 | |||
| 1f20b4bc79 | |||
| 20fcf35696 | |||
| 08bf9b3511 | |||
| c9316cc3d2 | |||
| 424311491c | |||
| a8795674b8 | |||
| 823b447fff | |||
| c36db99c2d | |||
| 5426b6af86 | |||
| bf0cdd667b | |||
| 9af59caf9c | |||
| 1a75c53ea4 | |||
| 83999585ec | |||
| 138353adb4 | |||
| 40dc4e1591 | |||
| cce4810904 | |||
| 5b58f9c1f5 | |||
| 815ac7732b | |||
| 2321f9d8f5 | |||
| b9020e0003 | |||
| ceb466578a | |||
| e1cfea4924 | |||
| dd8a2bf83a | |||
| ba36a28f2a | |||
| cf2010389a | |||
| 43522c4922 | |||
| 1cb5f24439 | |||
| b6a3f21a52 | |||
| 2ecc26b623 | |||
| 145f75fe06 | |||
| bd0578cbf9 | |||
| 40f24cc0b6 | |||
| 754efac063 | |||
| 132cc156f4 | |||
| 95ad932efb | |||
| ae30f4119f | |||
| d251ae4976 | |||
| 33324e590b | |||
| fea10ffd22 | |||
| f1e11e3f06 | |||
| c562eca8a1 | |||
| d9a690dac3 | |||
| a0eed058d1 | |||
| abc9f47a6f | |||
| b064f6fcaf | |||
| 0c7d7df2ec | |||
| dc86f35957 | |||
| 9685b0aa9b | |||
| f23cc408e3 | |||
| cea00a6c14 | |||
| f2949e1dc2 | |||
| 5f1f34fa7f | |||
| 2c041f9025 | |||
| 74b2d519b9 |
@@ -42,7 +42,7 @@ jobs:
|
||||
WARP_PKL="openpilot/sunnypilot/modeld_v2/models/driving_warp_${res}_tinygrad.pkl"
|
||||
echo "Compiling $WARP_PKL on QCOM"
|
||||
taskset -c 7 env DEV=QCOM IMAGE=1 FLOAT16=1 NOLOCALS=1 JIT_BATCH_SIZE=0 OPENPILOT_HACKS=1 python3 openpilot/sunnypilot/modeld_v2/compile_warp.py \
|
||||
--frame ${FRAME_PARAM} --warp-to ${MODEL_SIZE} --split-frames --output "${WARP_PKL}"
|
||||
--frame ${FRAME_PARAM} --warp-to ${MODEL_SIZE} --output "${WARP_PKL}"
|
||||
|
||||
BIG_WARP_PKL="openpilot/sunnypilot/modeld_v2/models/big_driving_warp_${res}_tinygrad.pkl"
|
||||
echo "Compiling $BIG_WARP_PKL on AMD"
|
||||
|
||||
@@ -168,7 +168,7 @@ jobs:
|
||||
echo "Building locationd..."
|
||||
# -j1: parallel rednose generators OOM the device
|
||||
scons -j1 cache_dir="$SCONS_CACHE" --minimal \
|
||||
openpilot/selfdrive/locationd
|
||||
openpilot/selfdrive/locationd openpilot/sunnypilot/selfdrive/locationd
|
||||
echo "Building rest of sunnypilot"
|
||||
sudo rm -rf /tmp/* || true
|
||||
mkdir -p "$BUILD_DIR/tmp"
|
||||
|
||||
@@ -46,23 +46,6 @@ jobs:
|
||||
CHECK_OOB: 0
|
||||
GMMU: 0
|
||||
run: tools/op.sh test -v openpilot/sunnypilot/modeld_v2/tests/test_convert_upstream_pkl.py
|
||||
- name: Resolve commaai's tinygrad pin, which their precompiled chestnut model is built and converted with
|
||||
id: upstream-tg
|
||||
env:
|
||||
GH_TOKEN: ${{ github.token }}
|
||||
run: |
|
||||
read -r sha url < <(gh api repos/commaai/openpilot/contents/tinygrad_repo --jq '.sha + " " + .submodule_git_url')
|
||||
echo "commaai/openpilot master pins $url@$sha, ours is $(git rev-parse HEAD:tinygrad_repo)"
|
||||
dir="$RUNNER_TEMP/upstream_tinygrad"
|
||||
git init -q "$dir" && git -C "$dir" fetch -q --depth 1 "$url" "$sha" && git -C "$dir" checkout -q FETCH_HEAD
|
||||
echo "path=$dir" >> "$GITHUB_OUTPUT"
|
||||
- name: Convert a pickled USB+AMD jit under commaai's tinygrad, as the chestnut model build does
|
||||
env:
|
||||
DEV: MOCKUSB+AMD
|
||||
CHECK_OOB: 0
|
||||
GMMU: 0
|
||||
PYTHONPATH: ${{ steps.upstream-tg.outputs.path }}:${{ github.workspace }}
|
||||
run: tools/op.sh test -v openpilot/sunnypilot/modeld_v2/tests/test_convert_upstream_pkl.py
|
||||
- name: Where tinygrad moved, for the helper or failure point named above
|
||||
if: failure() && github.event_name == 'pull_request'
|
||||
run: |
|
||||
|
||||
@@ -293,6 +293,8 @@ SConscript([
|
||||
'openpilot/selfdrive/ui/SConscript',
|
||||
])
|
||||
|
||||
SConscript(['openpilot/sunnypilot/SConscript'])
|
||||
|
||||
# Build desktop-only tools
|
||||
if GetOption('extras') and arch != "comma_arm64":
|
||||
SConscript([
|
||||
|
||||
+109
-123
@@ -4,11 +4,10 @@
|
||||
|
||||
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.
|
||||
|
||||
# 358 Supported Cars
|
||||
# 345 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|
|
||||
|---|---|---|:---:|:---:|:---:|:---:|:---:|:---:|:---:|:---:|
|
||||
|Acura|ADX 2025-26|All|openpilot available[<sup>1,5</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Honda Bosch B 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 ADX 2025-26">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|MDX 2022-24|All|openpilot available[<sup>1,5</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=Acura MDX 2022-24">Buy Here</a></sub></details>|||
|
||||
@@ -17,12 +16,12 @@ A supported vehicle is one that just works when you install a comma device. All
|
||||
|Acura|RDX 2019-21|All|openpilot available[<sup>1,5</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-22|All|openpilot available[<sup>1,5</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-22">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[<sup>13</sup>](#footnotes)|A3 2014-19|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,17</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[<sup>13</sup>](#footnotes)|A3 Sportback e-tron 2017-18|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,17</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[<sup>13</sup>](#footnotes)|Q2 2018|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,17</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[<sup>13</sup>](#footnotes)|Q3 2019-24|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,17</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[<sup>13</sup>](#footnotes)|RS3 2018|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,17</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[<sup>13</sup>](#footnotes)|S3 2015-17|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,17</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>|||
|
||||
|Audi[<sup>12</sup>](#footnotes)|A3 2014-19|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,16</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[<sup>12</sup>](#footnotes)|A3 Sportback e-tron 2017-18|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,16</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[<sup>12</sup>](#footnotes)|Q2 2018|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,16</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[<sup>12</sup>](#footnotes)|Q3 2019-24|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,16</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[<sup>12</sup>](#footnotes)|RS3 2018|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,16</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[<sup>12</sup>](#footnotes)|S3 2015-17|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,16</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" /></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|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>|||
|
||||
@@ -34,8 +33,8 @@ 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 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" /></a>||
|
||||
|CUPRA[<sup>13</sup>](#footnotes)|Ateca 2018-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,17</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|Born 2021-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot[<sup>17</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 OBD-C cable (2 ft)<br>- 1 VW MEB 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 Born 2021-23">Buy Here</a></sub></details>|||
|
||||
|CUPRA[<sup>12</sup>](#footnotes)|Ateca 2018-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,16</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|Born 2021-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot[<sup>16</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 OBD-C cable (2 ft)<br>- 1 VW MEB 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 Born 2021-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>|||
|
||||
|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>|||
|
||||
@@ -87,7 +86,7 @@ A supported vehicle is one that just works when you install a comma device. All
|
||||
|Honda|Civic 2022-26|All|openpilot available[<sup>1,5</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Honda Bosch B 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 Civic 2022-26">Buy Here</a></sub></details>|<a href="https://youtu.be/ytiOT5lcp6Q" target="_blank"><img height="18px" src="assets/icon-youtube.svg" /></a>||
|
||||
|Honda|Civic Hatchback 2017-18|Honda Sensing|openpilot available[<sup>1,5</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 Civic Hatchback 2017-18">Buy Here</a></sub></details>|||
|
||||
|Honda|Civic Hatchback 2019-21|All|openpilot available[<sup>1,5</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 Civic Hatchback 2019-21">Buy Here</a></sub></details>|||
|
||||
|Honda|Civic Hatchback 2022-26|All|openpilot available[<sup>1,5</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Honda Bosch B 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 Civic Hatchback 2022-26">Buy Here</a></sub></details>|<a href="https://youtu.be/ytiOT5lcp6Q" target="_blank"><img height="18px" src="assets/icon-youtube.svg" /></a>||
|
||||
|Honda|Civic Hatchback 2022-24|All|openpilot available[<sup>1,5</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Honda Bosch B 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 Civic Hatchback 2022-24">Buy Here</a></sub></details>|<a href="https://youtu.be/ytiOT5lcp6Q" target="_blank"><img height="18px" src="assets/icon-youtube.svg" /></a>||
|
||||
|Honda|Civic Hatchback Hybrid 2025-26|All|openpilot available[<sup>1,5</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Honda Bosch B 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 Civic Hatchback Hybrid 2025-26">Buy Here</a></sub></details>|||
|
||||
|Honda|Civic Hatchback Hybrid (Europe only) 2023|All|openpilot available[<sup>1,5</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Honda Bosch B 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 Civic Hatchback Hybrid (Europe only) 2023">Buy Here</a></sub></details>|||
|
||||
|Honda|Civic Hybrid 2025-26|All|openpilot available[<sup>1,5</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Honda Bosch B 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 Civic Hybrid 2025-26">Buy Here</a></sub></details>|||
|
||||
@@ -95,7 +94,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,5</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 Hybrid 2017-22|Honda Sensing|openpilot available[<sup>1,5</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-27|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-27">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|e 2020|All|openpilot available[<sup>1,5</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|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>|||
|
||||
@@ -112,9 +111,7 @@ A supported vehicle is one that just works when you install a comma device. All
|
||||
|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 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 Pilot 2023-25">Buy Here</a></sub></details>|||
|
||||
|Honda|Pilot 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 Pilot 2026">Buy Here</a></sub></details>|||
|
||||
|Honda|Prelude 2026|All|openpilot available[<sup>1,5</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Honda Bosch B 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 Prelude 2026">Buy Here</a></sub></details>|||
|
||||
|Honda|Ridgeline 2017-26|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 Ridgeline 2017-26">Buy Here</a></sub></details>|||
|
||||
|Honda|Ridgeline 2017-25|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 Ridgeline 2017-25">Buy Here</a></sub></details>|||
|
||||
|Hyundai|Azera 2022|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Hyundai K 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=Hyundai Azera 2022">Buy Here</a></sub></details>|||
|
||||
|Hyundai|Azera Hybrid 2019|All|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=Hyundai Azera Hybrid 2019">Buy Here</a></sub></details>|||
|
||||
|Hyundai|Azera Hybrid 2020|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Hyundai K 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=Hyundai Azera Hybrid 2020">Buy Here</a></sub></details>|||
|
||||
@@ -139,12 +136,10 @@ A supported vehicle is one that just works when you install a comma device. All
|
||||
|Hyundai|Ioniq Plug-in Hybrid 2020-22|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=Hyundai Ioniq Plug-in Hybrid 2020-22">Buy Here</a></sub></details>|||
|
||||
|Hyundai|Kona 2020|Smart Cruise Control (SCC)|openpilot available[<sup>1</sup>](#footnotes)|6 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Hyundai B 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=Hyundai Kona 2020">Buy Here</a></sub></details>|||
|
||||
|Hyundai|Kona 2022-23|Smart Cruise Control (SCC)|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Hyundai O 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=Hyundai Kona 2022-23">Buy Here</a></sub></details>|||
|
||||
|Hyundai|Kona (without HDA II) 2024-25|Smart Cruise Control (SCC)|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=Hyundai Kona (without HDA II) 2024-25">Buy Here</a></sub></details>|||
|
||||
|Hyundai|Kona Electric 2018-21|Smart Cruise Control (SCC)|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Hyundai G 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=Hyundai Kona Electric 2018-21">Buy Here</a></sub></details>|||
|
||||
|Hyundai|Kona Electric 2022-23|Smart Cruise Control (SCC)|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Hyundai O 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=Hyundai Kona Electric 2022-23">Buy Here</a></sub></details>|||
|
||||
|Hyundai|Kona Electric (with HDA II, Korea only) 2023|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Hyundai R 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=Hyundai Kona Electric (with HDA II, Korea only) 2023">Buy Here</a></sub></details>|<a href="https://www.youtube.com/watch?v=U2fOCmcQ8hw" target="_blank"><img height="18px" src="assets/icon-youtube.svg" /></a>||
|
||||
|Hyundai|Kona Hybrid 2020|Smart Cruise Control (SCC)|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Hyundai I 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=Hyundai Kona Hybrid 2020">Buy Here</a></sub></details>|||
|
||||
|Hyundai|Kona Hybrid (without HDA II) 2024-26|Smart Cruise Control (SCC)|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=Hyundai Kona Hybrid (without HDA II) 2024-26">Buy Here</a></sub></details>|||
|
||||
|Hyundai|Nexo 2021|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=Hyundai Nexo 2021">Buy Here</a></sub></details>|||
|
||||
|Hyundai|Palisade 2020-22|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=Hyundai Palisade 2020-22">Buy Here</a></sub></details>|<a href="https://youtu.be/TAnDqjF4fDY?t=456" target="_blank"><img height="18px" src="assets/icon-youtube.svg" /></a>||
|
||||
|Hyundai|Santa Cruz 2022-24|Smart Cruise Control (SCC)|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Hyundai N 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=Hyundai Santa Cruz 2022-24">Buy Here</a></sub></details>|||
|
||||
@@ -154,25 +149,20 @@ A supported vehicle is one that just works when you install a comma device. All
|
||||
|Hyundai|Santa Fe Plug-in Hybrid 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=Hyundai Santa Fe Plug-in Hybrid 2022-23">Buy Here</a></sub></details>|||
|
||||
|Hyundai|Sonata 2018-19|Smart Cruise Control (SCC)|Stock|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=Hyundai Sonata 2018-19">Buy Here</a></sub></details>|||
|
||||
|Hyundai|Sonata 2020-23|All|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=Hyundai Sonata 2020-23">Buy Here</a></sub></details>|<a href="https://www.youtube.com/watch?v=ix63r9kE3Fw" target="_blank"><img height="18px" src="assets/icon-youtube.svg" /></a>||
|
||||
|Hyundai|Sonata (without HDA II) 2024-26|All|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=Hyundai Sonata (without HDA II) 2024-26">Buy Here</a></sub></details>|||
|
||||
|Hyundai|Sonata Hybrid 2020-23|All|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=Hyundai Sonata Hybrid 2020-23">Buy Here</a></sub></details>|||
|
||||
|Hyundai|Sonata Hybrid (without HDA II) 2024-26|All|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=Hyundai Sonata Hybrid (without HDA II) 2024-26">Buy Here</a></sub></details>|||
|
||||
|Hyundai|Staria 2023|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Hyundai K 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=Hyundai Staria 2023">Buy Here</a></sub></details>|||
|
||||
|Hyundai|Tucson 2021|Smart Cruise Control (SCC)|openpilot available[<sup>1</sup>](#footnotes)|19 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=Hyundai Tucson 2021">Buy Here</a></sub></details>|||
|
||||
|Hyundai|Tucson 2022|Smart Cruise Control (SCC)|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Hyundai N 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=Hyundai Tucson 2022">Buy Here</a></sub></details>|||
|
||||
|Hyundai|Tucson 2023-24|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Hyundai N 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=Hyundai Tucson 2023-24">Buy Here</a></sub></details>|||
|
||||
|Hyundai|Tucson Diesel 2019|Smart Cruise Control (SCC)|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=Hyundai Tucson Diesel 2019">Buy Here</a></sub></details>|||
|
||||
|Hyundai|Tucson Hybrid 2022-24|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Hyundai N 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=Hyundai Tucson Hybrid 2022-24">Buy Here</a></sub></details>|||
|
||||
|Hyundai|Tucson Hybrid (without HDA II) 2025-26|Smart Cruise Control (SCC)|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Hyundai N 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=Hyundai Tucson Hybrid (without HDA II) 2025-26">Buy Here</a></sub></details>|||
|
||||
|Hyundai|Tucson Plug-in Hybrid 2023-24|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Hyundai N 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=Hyundai Tucson Plug-in Hybrid 2023-24">Buy Here</a></sub></details>|||
|
||||
|Hyundai|Tucson Plug-in Hybrid 2024|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Hyundai N 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=Hyundai Tucson Plug-in Hybrid 2024">Buy Here</a></sub></details>|||
|
||||
|Hyundai|Veloster 2019-20|Smart Cruise Control (SCC)|Stock|5 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=Hyundai Veloster 2019-20">Buy Here</a></sub></details>|||
|
||||
|Jeep|Grand Cherokee 2016-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=Jeep Grand Cherokee 2016-18">Buy Here</a></sub></details>|<a href="https://www.youtube.com/watch?v=eLR9o2JkuRk" target="_blank"><img height="18px" src="assets/icon-youtube.svg" /></a>||
|
||||
|Jeep|Grand Cherokee 2019-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=Jeep Grand Cherokee 2019-21">Buy Here</a></sub></details>|||
|
||||
|Kia|Carnival 2022-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 Carnival 2022-24">Buy Here</a></sub></details>|||
|
||||
|Kia|Carnival (China only) 2023|Smart Cruise Control (SCC)|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Hyundai K 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 Carnival (China only) 2023">Buy Here</a></sub></details>|||
|
||||
|Kia|Carnival Hybrid (with HDA II) 2025-26|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 Carnival Hybrid (with HDA II) 2025-26">Buy Here</a></sub></details>|||
|
||||
|Kia|Ceed 2019-21|Smart Cruise Control (SCC)|Stock|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 Ceed 2019-21">Buy Here</a></sub></details>|||
|
||||
|Kia|Ceed Plug-in Hybrid 2021|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Hyundai B 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 Ceed Plug-in Hybrid 2021">Buy Here</a></sub></details>|||
|
||||
|Kia|EV6 (Southeast Asia only) 2022-24|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Hyundai P 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 EV6 (Southeast Asia only) 2022-24">Buy Here</a></sub></details>|||
|
||||
|Kia|EV6 (with HDA II) 2022-24|Highway Driving Assist II|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Hyundai P 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 EV6 (with HDA II) 2022-24">Buy Here</a></sub></details>|||
|
||||
|Kia|EV6 (without HDA II) 2022-24|Highway Driving Assist|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=Kia EV6 (without HDA II) 2022-24">Buy Here</a></sub></details>|||
|
||||
@@ -196,10 +186,9 @@ A supported vehicle is one that just works when you install a comma device. All
|
||||
|Kia|Niro Plug-in Hybrid 2020|Smart Cruise Control (SCC)|Stock|0 mph|32 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Hyundai D 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 Plug-in Hybrid 2020">Buy Here</a></sub></details>|||
|
||||
|Kia|Niro Plug-in Hybrid 2021|Smart Cruise Control (SCC)|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Hyundai D 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 Plug-in Hybrid 2021">Buy Here</a></sub></details>|||
|
||||
|Kia|Niro Plug-in Hybrid 2022|Smart Cruise Control (SCC)|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 Plug-in Hybrid 2022">Buy Here</a></sub></details>|||
|
||||
|Kia|Niro Plus EV 2024|Smart Cruise Control (SCC)|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 Plus EV 2024">Buy Here</a></sub></details>|||
|
||||
|Kia|Optima 2017|Advanced Smart Cruise Control|Stock|0 mph|32 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Hyundai B 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 Optima 2017">Buy Here</a></sub></details>|||
|
||||
|Kia|Optima 2019-20|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Hyundai G 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 Optima 2019-20">Buy Here</a></sub></details>|||
|
||||
|Kia|Optima Hybrid 2019|Smart Cruise Control (SCC)|Stock|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 Optima Hybrid 2019">Buy Here</a></sub></details>|||
|
||||
|Kia|Optima Hybrid 2019|Smart Cruise Control (SCC)|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 Optima Hybrid 2019">Buy Here</a></sub></details>|||
|
||||
|Kia|Seltos 2021|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 Seltos 2021">Buy Here</a></sub></details>|||
|
||||
|Kia|Sorento 2018|Advanced Smart Cruise Control & LKAS|Stock|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 Sorento 2018">Buy Here</a></sub></details>|<a href="https://www.youtube.com/watch?v=Fkh3s6WHJz8" target="_blank"><img height="18px" src="assets/icon-youtube.svg" /></a>||
|
||||
|Kia|Sorento 2019|Smart Cruise Control (SCC)|Stock|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 Sorento 2019">Buy Here</a></sub></details>|<a href="https://www.youtube.com/watch?v=Fkh3s6WHJz8" target="_blank"><img height="18px" src="assets/icon-youtube.svg" /></a>||
|
||||
@@ -208,7 +197,6 @@ A supported vehicle is one that just works when you install a comma device. All
|
||||
|Kia|Sorento Plug-in Hybrid 2022-23|All|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 Sorento Plug-in Hybrid 2022-23">Buy Here</a></sub></details>|||
|
||||
|Kia|Sportage 2023-24|Smart Cruise Control (SCC)|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Hyundai N 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 Sportage 2023-24">Buy Here</a></sub></details>|||
|
||||
|Kia|Sportage Hybrid 2023|Smart Cruise Control (SCC)|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Hyundai N 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 Sportage Hybrid 2023">Buy Here</a></sub></details>|||
|
||||
|Kia|Sportage Plug-in Hybrid 2023|Smart Cruise Control (SCC)|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Hyundai N 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 Sportage Plug-in Hybrid 2023">Buy Here</a></sub></details>|||
|
||||
|Kia|Stinger 2018-20|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 Stinger 2018-20">Buy Here</a></sub></details>|<a href="https://www.youtube.com/watch?v=MJ94qoofYw0" target="_blank"><img height="18px" src="assets/icon-youtube.svg" /></a>||
|
||||
|Kia|Stinger 2022-23|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 Hyundai K 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 Stinger 2022-23">Buy Here</a></sub></details>|||
|
||||
|Kia|Telluride 2020-22|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 Telluride 2020-22">Buy Here</a></sub></details>|||
|
||||
@@ -237,52 +225,51 @@ 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>|||
|
||||
|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>|||
|
||||
|MAN[<sup>13</sup>](#footnotes)|eTGE 2020-24|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,17</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" /></a>||
|
||||
|MAN[<sup>13</sup>](#footnotes)|TGE 2017-24|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,17</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" /></a>||
|
||||
|Mazda[<sup>6</sup>](#footnotes)|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[<sup>6</sup>](#footnotes)|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" /></a>||
|
||||
|Nissan[<sup>7</sup>](#footnotes)|Altima 2019-24|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-24">Buy Here</a></sub></details>|||
|
||||
|Nissan[<sup>7</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" /></a>||
|
||||
|Nissan[<sup>7</sup>](#footnotes)|Leaf IC 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 IC 2018-23">Buy Here</a></sub></details>|<a href="https://youtu.be/vaMbtAh_0cY" target="_blank"><img height="18px" src="assets/icon-youtube.svg" /></a>||
|
||||
|Nissan[<sup>7</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>7</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>|||
|
||||
|MAN[<sup>12</sup>](#footnotes)|eTGE 2020-24|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,16</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" /></a>||
|
||||
|MAN[<sup>12</sup>](#footnotes)|TGE 2017-24|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,16</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" /></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-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" /></a>||
|
||||
|Nissan[<sup>6</sup>](#footnotes)|Altima 2019-24|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-24">Buy Here</a></sub></details>|||
|
||||
|Nissan[<sup>6</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" /></a>||
|
||||
|Nissan[<sup>6</sup>](#footnotes)|Leaf IC 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 IC 2018-23">Buy Here</a></sub></details>|<a href="https://youtu.be/vaMbtAh_0cY" target="_blank"><img height="18px" src="assets/icon-youtube.svg" /></a>||
|
||||
|Nissan[<sup>6</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>6</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>|||
|
||||
|Ram|1500 2019-24|Adaptive Cruise Control (ACC)|Stock|32 mph|1 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|2500 2020-24|Adaptive Cruise Control (ACC)|Stock|0 mph|36 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 2500 2020-24">Buy Here</a></sub></details>|||
|
||||
|Ram|3500 2019-24|Adaptive Cruise Control (ACC)|Stock|0 mph|36 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 3500 2019-24">Buy Here</a></sub></details>|||
|
||||
|Rivian|R1S 2022-24|All|openpilot available[<sup>1</sup>](#footnotes)|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/dflSSGQwYNc" target="_blank"><img height="18px" src="assets/icon-youtube.svg" /></a>|<a href="https://youtu.be/uaISd1j7Z4U" target="_blank"><img height="18px" src="assets/icon-youtube.svg" /></a>|
|
||||
|Rivian|R1S 2025|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 OBD-C cable (2 ft)<br>- 1 Rivian B 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 2025">Buy Here</a></sub></details>|||
|
||||
|Rivian|R1T 2022-24|All|openpilot available[<sup>1</sup>](#footnotes)|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/dflSSGQwYNc" target="_blank"><img height="18px" src="assets/icon-youtube.svg" /></a>|<a href="https://youtu.be/uaISd1j7Z4U" target="_blank"><img height="18px" src="assets/icon-youtube.svg" /></a>|
|
||||
|Rivian|R1T 2025|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 OBD-C cable (2 ft)<br>- 1 Rivian B 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 2025">Buy Here</a></sub></details>|||
|
||||
|SEAT[<sup>13</sup>](#footnotes)|Ateca 2016-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,17</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[<sup>13</sup>](#footnotes)|Leon 2014-20|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,17</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>|||
|
||||
|Subaru|Ascent 2019-21|All[<sup>8</sup>](#footnotes)|Stock|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>8</sup>](#footnotes)|Stock|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" /></a>||
|
||||
|Subaru|Crosstrek 2020-23|EyeSight Driver Assistance[<sup>8</sup>](#footnotes)|Stock|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|Forester 2017-18|EyeSight Driver Assistance[<sup>8</sup>](#footnotes)|Stock|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 Forester 2017-18">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|Forester 2019-21|All[<sup>8</sup>](#footnotes)|Stock|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 Forester 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|Impreza 2017-19|EyeSight Driver Assistance[<sup>8</sup>](#footnotes)|Stock|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 Impreza 2017-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>|||
|
||||
|Subaru|Impreza 2020-22|EyeSight Driver Assistance[<sup>8</sup>](#footnotes)|Stock|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 Impreza 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|Legacy 2015-18|EyeSight Driver Assistance[<sup>8</sup>](#footnotes)|Stock|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 Legacy 2015-18">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|Legacy 2020-22|All[<sup>8</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 Legacy 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 2015-17|EyeSight Driver Assistance[<sup>8</sup>](#footnotes)|Stock|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 Outback 2015-17">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 2018-19|EyeSight Driver Assistance[<sup>8</sup>](#footnotes)|Stock|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 Outback 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>|||
|
||||
|Subaru|Outback 2020-22|All[<sup>8</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>8</sup>](#footnotes)|Stock|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" /></a>||
|
||||
|Subaru|XV 2020-21|EyeSight Driver Assistance[<sup>8</sup>](#footnotes)|Stock|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>16</sup>](#footnotes)|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,17</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>18</sup>](#footnotes)|||
|
||||
|Škoda|Kamiq 2021-23[<sup>14,16</sup>](#footnotes)|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,17</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>18</sup>](#footnotes)|||
|
||||
|Škoda[<sup>13</sup>](#footnotes)|Karoq 2019-23[<sup>16</sup>](#footnotes)|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,17</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[<sup>13</sup>](#footnotes)|Kodiaq 2017-23[<sup>16</sup>](#footnotes)|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,17</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[<sup>13</sup>](#footnotes)|Octavia 2015-19[<sup>16</sup>](#footnotes)|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,17</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[<sup>13</sup>](#footnotes)|Octavia RS 2016[<sup>16</sup>](#footnotes)|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,17</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[<sup>13</sup>](#footnotes)|Octavia Scout 2017-19[<sup>16</sup>](#footnotes)|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,17</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|Scala 2020-23[<sup>16</sup>](#footnotes)|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,17</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>18</sup>](#footnotes)|||
|
||||
|Škoda[<sup>13</sup>](#footnotes)|Superb 2015-22[<sup>16</sup>](#footnotes)|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,17</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>|||
|
||||
|Tesla[<sup>11</sup>](#footnotes)|Model 3 (with HW2.5) 2019[<sup>10</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 HW2.5) 2019">Buy Here</a></sub></details>|||
|
||||
|Tesla[<sup>11</sup>](#footnotes)|Model 3 (with HW3) 2019-23[<sup>10</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>11</sup>](#footnotes)|Model 3 (with HW4) 2024-25[<sup>10</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>11</sup>](#footnotes)|Model Y (with HW3) 2020-24[<sup>10</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-24">Buy Here</a></sub></details>|||
|
||||
|Tesla[<sup>11</sup>](#footnotes)|Model Y (with HW4) 2023-25[<sup>10</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) 2023-25">Buy Here</a></sub></details>|||
|
||||
|Ram|3500 2019-22|Adaptive Cruise Control (ACC)|Stock|0 mph|36 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 3500 2019-22">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/dflSSGQwYNc" target="_blank"><img height="18px" src="assets/icon-youtube.svg" /></a>|<a href="https://youtu.be/uaISd1j7Z4U" target="_blank"><img height="18px" src="assets/icon-youtube.svg" /></a>|
|
||||
|Rivian|R1S 2025|All|openpilot|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 OBD-C cable (2 ft)<br>- 1 Rivian B 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 2025">Buy Here</a></sub></details>|||
|
||||
|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/dflSSGQwYNc" target="_blank"><img height="18px" src="assets/icon-youtube.svg" /></a>|<a href="https://youtu.be/uaISd1j7Z4U" target="_blank"><img height="18px" src="assets/icon-youtube.svg" /></a>|
|
||||
|Rivian|R1T 2025|All|openpilot|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 OBD-C cable (2 ft)<br>- 1 Rivian B 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 2025">Buy Here</a></sub></details>|||
|
||||
|SEAT[<sup>12</sup>](#footnotes)|Ateca 2016-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,16</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[<sup>12</sup>](#footnotes)|Leon 2014-20|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,16</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>|||
|
||||
|Subaru|Ascent 2019-21|All[<sup>7</sup>](#footnotes)|Stock|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>7</sup>](#footnotes)|Stock|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" /></a>||
|
||||
|Subaru|Crosstrek 2020-23|EyeSight Driver Assistance[<sup>7</sup>](#footnotes)|Stock|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|Forester 2017-18|EyeSight Driver Assistance[<sup>7</sup>](#footnotes)|Stock|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 Forester 2017-18">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|Forester 2019-21|All[<sup>7</sup>](#footnotes)|Stock|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 Forester 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|Impreza 2017-19|EyeSight Driver Assistance[<sup>7</sup>](#footnotes)|Stock|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 Impreza 2017-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>|||
|
||||
|Subaru|Impreza 2020-22|EyeSight Driver Assistance[<sup>7</sup>](#footnotes)|Stock|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 Impreza 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|Legacy 2015-18|EyeSight Driver Assistance[<sup>7</sup>](#footnotes)|Stock|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 Legacy 2015-18">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|Legacy 2020-22|All[<sup>7</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 Legacy 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 2015-17|EyeSight Driver Assistance[<sup>7</sup>](#footnotes)|Stock|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 Outback 2015-17">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 2018-19|EyeSight Driver Assistance[<sup>7</sup>](#footnotes)|Stock|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 Outback 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>|||
|
||||
|Subaru|Outback 2020-22|All[<sup>7</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>7</sup>](#footnotes)|Stock|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" /></a>||
|
||||
|Subaru|XV 2020-21|EyeSight Driver Assistance[<sup>7</sup>](#footnotes)|Stock|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>15</sup>](#footnotes)|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,16</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>17</sup>](#footnotes)|||
|
||||
|Škoda|Kamiq 2021-23[<sup>13,15</sup>](#footnotes)|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,16</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>17</sup>](#footnotes)|||
|
||||
|Škoda[<sup>12</sup>](#footnotes)|Karoq 2019-23[<sup>15</sup>](#footnotes)|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,16</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[<sup>12</sup>](#footnotes)|Kodiaq 2017-23[<sup>15</sup>](#footnotes)|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,16</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[<sup>12</sup>](#footnotes)|Octavia 2015-19[<sup>15</sup>](#footnotes)|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,16</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[<sup>12</sup>](#footnotes)|Octavia RS 2016[<sup>15</sup>](#footnotes)|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,16</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[<sup>12</sup>](#footnotes)|Octavia Scout 2017-19[<sup>15</sup>](#footnotes)|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,16</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|Scala 2020-23[<sup>15</sup>](#footnotes)|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,16</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>17</sup>](#footnotes)|||
|
||||
|Škoda[<sup>12</sup>](#footnotes)|Superb 2015-22[<sup>15</sup>](#footnotes)|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,16</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>|||
|
||||
|Tesla[<sup>10</sup>](#footnotes)|Model 3 (with HW3) 2019-23[<sup>9</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>10</sup>](#footnotes)|Model 3 (with HW4) 2024-25[<sup>9</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>10</sup>](#footnotes)|Model Y (with HW3) 2020-24[<sup>9</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-24">Buy Here</a></sub></details>|||
|
||||
|Tesla[<sup>10</sup>](#footnotes)|Model Y (with HW4) 2023-25[<sup>9</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) 2023-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 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>|||
|
||||
@@ -295,8 +282,8 @@ A supported vehicle is one that just works when you install a comma device. All
|
||||
|Toyota|C-HR 2021|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 C-HR 2021">Buy Here</a></sub></details>|||
|
||||
|Toyota|C-HR Hybrid 2017-20|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=Toyota C-HR Hybrid 2017-20">Buy Here</a></sub></details>|||
|
||||
|Toyota|C-HR Hybrid 2021-22|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 C-HR Hybrid 2021-22">Buy Here</a></sub></details>|||
|
||||
|Toyota|Camry 2018-20|All|Stock|0 mph[<sup>12</sup>](#footnotes)|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 Camry 2018-20">Buy Here</a></sub></details>|<a href="https://www.youtube.com/watch?v=fkcjviZY9CM" target="_blank"><img height="18px" src="assets/icon-youtube.svg" /></a>||
|
||||
|Toyota|Camry 2021-24|All|openpilot|0 mph[<sup>12</sup>](#footnotes)|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 Camry 2021-24">Buy Here</a></sub></details>|||
|
||||
|Toyota|Camry 2018-20|All|Stock|0 mph[<sup>11</sup>](#footnotes)|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 Camry 2018-20">Buy Here</a></sub></details>|<a href="https://www.youtube.com/watch?v=fkcjviZY9CM" target="_blank"><img height="18px" src="assets/icon-youtube.svg" /></a>||
|
||||
|Toyota|Camry 2021-24|All|openpilot|0 mph[<sup>11</sup>](#footnotes)|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 Camry 2021-24">Buy Here</a></sub></details>|||
|
||||
|Toyota|Camry Hybrid 2018-20|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=Toyota Camry Hybrid 2018-20">Buy Here</a></sub></details>|<a href="https://www.youtube.com/watch?v=Q2DYY0AWKgk" target="_blank"><img height="18px" src="assets/icon-youtube.svg" /></a>||
|
||||
|Toyota|Camry Hybrid 2021-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=Toyota Camry Hybrid 2021-24">Buy Here</a></sub></details>|||
|
||||
|Toyota|Corolla 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=Toyota Corolla 2017-19">Buy Here</a></sub></details>|||
|
||||
@@ -328,44 +315,44 @@ 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" /></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" /></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" /></a>||
|
||||
|Volkswagen[<sup>13</sup>](#footnotes)|Arteon 2018-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,17</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" /></a>||
|
||||
|Volkswagen[<sup>13</sup>](#footnotes)|Arteon eHybrid 2020-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,17</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" /></a>||
|
||||
|Volkswagen[<sup>13</sup>](#footnotes)|Arteon R 2020-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,17</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" /></a>||
|
||||
|Volkswagen[<sup>13</sup>](#footnotes)|Arteon Shooting Brake 2020-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,17</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" /></a>||
|
||||
|Volkswagen[<sup>13</sup>](#footnotes)|Atlas 2018-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,17</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[<sup>13</sup>](#footnotes)|Atlas Cross Sport 2020-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,17</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[<sup>13</sup>](#footnotes)|California 2021-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,17</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[<sup>13</sup>](#footnotes)|Caravelle 2020|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,17</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[<sup>13</sup>](#footnotes)|CC 2018-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,17</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" /></a>||
|
||||
|Volkswagen[<sup>13</sup>](#footnotes)|Crafter 2017-24|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,17</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" /></a>||
|
||||
|Volkswagen[<sup>13</sup>](#footnotes)|e-Crafter 2018-24|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,17</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" /></a>||
|
||||
|Volkswagen[<sup>13</sup>](#footnotes)|e-Golf 2014-20|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,17</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[<sup>13</sup>](#footnotes)|Golf 2015-20|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,17</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[<sup>13</sup>](#footnotes)|Golf Alltrack 2015-19|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,17</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[<sup>13</sup>](#footnotes)|Golf GTD 2015-20|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,17</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[<sup>13</sup>](#footnotes)|Golf GTE 2015-20|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,17</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[<sup>13</sup>](#footnotes)|Golf GTI 2015-21|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,17</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[<sup>13</sup>](#footnotes)|Golf R 2015-19|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,17</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[<sup>13</sup>](#footnotes)|Golf SportsVan 2015-20|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,17</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[<sup>13</sup>](#footnotes)|Grand California 2019-24|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,17</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" /></a>||
|
||||
|Volkswagen|ID.4 2021-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot[<sup>17</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 OBD-C cable (2 ft)<br>- 1 VW MEB 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 ID.4 2021-23">Buy Here</a></sub></details>|||
|
||||
|Volkswagen|ID.4 2024-25|Adaptive Cruise Control (ACC) & Lane Assist|openpilot[<sup>17</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 OBD-C cable (2 ft)<br>- 1 VW MEB 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 ID.4 2024-25">Buy Here</a></sub></details>|||
|
||||
|Volkswagen[<sup>13</sup>](#footnotes)|Jetta 2019-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,17</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[<sup>13</sup>](#footnotes)|Jetta GLI 2021-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,17</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|Passat 2015-22[<sup>15</sup>](#footnotes)|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,17</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[<sup>13</sup>](#footnotes)|Passat Alltrack 2015-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,17</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[<sup>13</sup>](#footnotes)|Passat GTE 2015-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,17</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|Polo 2018-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,17</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>18</sup>](#footnotes)|||
|
||||
|Volkswagen|Polo GTI 2018-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,17</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>18</sup>](#footnotes)|||
|
||||
|Volkswagen|T-Cross 2021|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,17</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>18</sup>](#footnotes)|||
|
||||
|Volkswagen[<sup>13</sup>](#footnotes)|T-Roc 2018-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,17</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[<sup>13</sup>](#footnotes)|Taos 2022-24|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,17</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[<sup>13</sup>](#footnotes)|Teramont 2018-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,17</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[<sup>13</sup>](#footnotes)|Teramont Cross Sport 2021-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,17</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[<sup>13</sup>](#footnotes)|Teramont X 2021-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,17</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[<sup>13</sup>](#footnotes)|Tiguan 2018-24|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,17</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[<sup>13</sup>](#footnotes)|Tiguan eHybrid 2021-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,17</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[<sup>13</sup>](#footnotes)|Touran 2016-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,17</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[<sup>12</sup>](#footnotes)|Arteon 2018-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,16</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" /></a>||
|
||||
|Volkswagen[<sup>12</sup>](#footnotes)|Arteon eHybrid 2020-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,16</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" /></a>||
|
||||
|Volkswagen[<sup>12</sup>](#footnotes)|Arteon R 2020-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,16</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" /></a>||
|
||||
|Volkswagen[<sup>12</sup>](#footnotes)|Arteon Shooting Brake 2020-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,16</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" /></a>||
|
||||
|Volkswagen[<sup>12</sup>](#footnotes)|Atlas 2018-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,16</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[<sup>12</sup>](#footnotes)|Atlas Cross Sport 2020-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,16</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[<sup>12</sup>](#footnotes)|California 2021-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,16</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[<sup>12</sup>](#footnotes)|Caravelle 2020|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,16</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[<sup>12</sup>](#footnotes)|CC 2018-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,16</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" /></a>||
|
||||
|Volkswagen[<sup>12</sup>](#footnotes)|Crafter 2017-24|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,16</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" /></a>||
|
||||
|Volkswagen[<sup>12</sup>](#footnotes)|e-Crafter 2018-24|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,16</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" /></a>||
|
||||
|Volkswagen[<sup>12</sup>](#footnotes)|e-Golf 2014-20|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,16</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[<sup>12</sup>](#footnotes)|Golf 2015-20|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,16</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[<sup>12</sup>](#footnotes)|Golf Alltrack 2015-19|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,16</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[<sup>12</sup>](#footnotes)|Golf GTD 2015-20|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,16</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[<sup>12</sup>](#footnotes)|Golf GTE 2015-20|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,16</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[<sup>12</sup>](#footnotes)|Golf GTI 2015-21|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,16</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[<sup>12</sup>](#footnotes)|Golf R 2015-19|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,16</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[<sup>12</sup>](#footnotes)|Golf SportsVan 2015-20|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,16</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[<sup>12</sup>](#footnotes)|Grand California 2019-24|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,16</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" /></a>||
|
||||
|Volkswagen|ID.4 2021-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot[<sup>16</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 OBD-C cable (2 ft)<br>- 1 VW MEB 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 ID.4 2021-23">Buy Here</a></sub></details>|||
|
||||
|Volkswagen|ID.4 2024-25|Adaptive Cruise Control (ACC) & Lane Assist|openpilot[<sup>16</sup>](#footnotes)|0 mph|0 mph|[](##)|[](##)|<details><summary>Parts</summary><sub>- 1 OBD-C cable (2 ft)<br>- 1 VW MEB 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 ID.4 2024-25">Buy Here</a></sub></details>|||
|
||||
|Volkswagen[<sup>12</sup>](#footnotes)|Jetta 2019-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,16</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[<sup>12</sup>](#footnotes)|Jetta GLI 2021-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,16</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|Passat 2015-22[<sup>14</sup>](#footnotes)|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,16</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[<sup>12</sup>](#footnotes)|Passat Alltrack 2015-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,16</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[<sup>12</sup>](#footnotes)|Passat GTE 2015-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,16</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|Polo 2018-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,16</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>17</sup>](#footnotes)|||
|
||||
|Volkswagen|Polo GTI 2018-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,16</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>17</sup>](#footnotes)|||
|
||||
|Volkswagen|T-Cross 2021|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,16</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>17</sup>](#footnotes)|||
|
||||
|Volkswagen[<sup>12</sup>](#footnotes)|T-Roc 2018-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,16</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[<sup>12</sup>](#footnotes)|Taos 2022-24|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,16</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[<sup>12</sup>](#footnotes)|Teramont 2018-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,16</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[<sup>12</sup>](#footnotes)|Teramont Cross Sport 2021-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,16</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[<sup>12</sup>](#footnotes)|Teramont X 2021-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,16</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[<sup>12</sup>](#footnotes)|Tiguan 2018-24|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,16</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[<sup>12</sup>](#footnotes)|Tiguan eHybrid 2021-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,16</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[<sup>12</sup>](#footnotes)|Touran 2016-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[<sup>1,16</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>|||
|
||||
|
||||
### Footnotes
|
||||
<sup>1</sup>openpilot Longitudinal Control (Alpha) is available behind a toggle; the toggle is only available in non-release branches such as `nightly-dev`. <br />
|
||||
@@ -373,19 +360,18 @@ A supported vehicle is one that just works when you install a comma device. All
|
||||
<sup>3</sup>See more setup details for <a href="https://github.com/commaai/openpilot/wiki/gm" target="_blank">GM</a>. <br />
|
||||
<sup>4</sup>2019 Honda Civic 1.6L Diesel Sedan does not have ALC below 12mph. <br />
|
||||
<sup>5</sup>Enabling longitudinal control (alpha) will disable all CMBS functionality, including AEB and FCW. <br />
|
||||
<sup>6</sup>Mazda vehicles <b>require</b> comma power installation. <br />
|
||||
<sup>7</sup>See more setup details for <a href="https://github.com/commaai/openpilot/wiki/nissan" target="_blank">Nissan</a>. <br />
|
||||
<sup>8</sup>In the non-US market, openpilot requires the car to come equipped with EyeSight with Lane Keep Assistance. <br />
|
||||
<sup>9</sup>Enabling longitudinal control (alpha) will disable all EyeSight functionality, including AEB, LDW, and RAB. <br />
|
||||
<sup>10</sup>Model year 2019 can have HW2.5 or HW3, and model years 2023 and 2024 can have HW3 or HW4, depending on build date and factory. To check which hardware type your vehicle has, look for <b>Autopilot computer</b> under <b>Software -> Additional Vehicle Information</b> on your vehicle's touchscreen. See <a href="https://www.notateslaapp.com/news/2173/how-to-check-if-your-tesla-has-hardware-4-ai4-or-hardware-3">this page</a> for more information. <br />
|
||||
<sup>11</sup>See more setup details for <a href="https://github.com/commaai/openpilot/wiki/tesla" target="_blank">Tesla</a>. <br />
|
||||
<sup>12</sup>openpilot operates above 28mph for Camry 4CYL L, 4CYL LE and 4CYL SE which don't have Full-Speed Range Dynamic Radar Cruise Control. <br />
|
||||
<sup>13</sup>The J533 harness plugs in at the CAN gateway under the dashboard, just above the steering column. More information can be found at <a href="https://docs.howtocomma.com/docs/j533-harness-install" target="_blank">this guide</a>. <br />
|
||||
<sup>14</sup>Not including the China market Kamiq, which is based on the (currently) unsupported PQ34 platform. <br />
|
||||
<sup>15</sup>Refers only to the MQB-based European B8 Passat, not the NMS Passat in the USA/China/Mideast markets. <br />
|
||||
<sup>16</sup>Some Škoda vehicles are equipped with heated windshields, which are known to block GPS signal needed for some comma four functionality. <br />
|
||||
<sup>17</sup>Only available for vehicles using a gateway (J533) harness. At this time, vehicles using a camera harness are limited to using stock ACC. <br />
|
||||
<sup>18</sup>Model-years 2022 and beyond may have a combined CAN gateway and BCM, which is supported by openpilot in software, but doesn't yet have a harness available from the comma store. <br />
|
||||
<sup>6</sup>See more setup details for <a href="https://github.com/commaai/openpilot/wiki/nissan" target="_blank">Nissan</a>. <br />
|
||||
<sup>7</sup>In the non-US market, openpilot requires the car to come equipped with EyeSight with Lane Keep Assistance. <br />
|
||||
<sup>8</sup>Enabling longitudinal control (alpha) will disable all EyeSight functionality, including AEB, LDW, and RAB. <br />
|
||||
<sup>9</sup>Model years 2023 and 2024 can have either hardware type, depending on build date and factory. To check which hardware type your vehicle has, look for <b>Autopilot computer</b> under <b>Software -> Additional Vehicle Information</b> on your vehicle's touchscreen. See <a href="https://www.notateslaapp.com/news/2173/how-to-check-if-your-tesla-has-hardware-4-ai4-or-hardware-3">this page</a> for more information. <br />
|
||||
<sup>10</sup>See more setup details for <a href="https://github.com/commaai/openpilot/wiki/tesla" target="_blank">Tesla</a>. <br />
|
||||
<sup>11</sup>openpilot operates above 28mph for Camry 4CYL L, 4CYL LE and 4CYL SE which don't have Full-Speed Range Dynamic Radar Cruise Control. <br />
|
||||
<sup>12</sup>The J533 harness plugs in at the CAN gateway under the dashboard, just above the steering column. More information can be found at <a href="https://docs.howtocomma.com/docs/j533-harness-install" target="_blank">this guide</a>. <br />
|
||||
<sup>13</sup>Not including the China market Kamiq, which is based on the (currently) unsupported PQ34 platform. <br />
|
||||
<sup>14</sup>Refers only to the MQB-based European B8 Passat, not the NMS Passat in the USA/China/Mideast markets. <br />
|
||||
<sup>15</sup>Some Škoda vehicles are equipped with heated windshields, which are known to block GPS signal needed for some comma four functionality. <br />
|
||||
<sup>16</sup>Only available for vehicles using a gateway (J533) harness. At this time, vehicles using a camera harness are limited to using stock ACC. <br />
|
||||
<sup>17</sup>Model-years 2022 and beyond may have a combined CAN gateway and BCM, which is supported by openpilot in software, but doesn't yet have a harness available from the comma store. <br />
|
||||
|
||||
## Community Maintained Cars
|
||||
Although they're not upstream, the community has openpilot running on other makes and models. See the 'Community Supported Models' section of each make [on our wiki](https://wiki.comma.ai/).
|
||||
|
||||
+1
-1
Submodule opendbc_repo updated: 426784cec8...15c326f465
@@ -483,16 +483,7 @@ struct ModelDataV2SP @0xa1680744031fdb2d {
|
||||
}
|
||||
}
|
||||
|
||||
struct GpsLocationSP @0xcb9fd56c7057593a {
|
||||
latitude @0 :Float64;
|
||||
longitude @1 :Float64;
|
||||
speed @2 :Float32;
|
||||
bearingDeg @3 :Float32;
|
||||
horizontalAccuracy @4 :Float32; # m
|
||||
bearingAccuracyDeg @5 :Float32;
|
||||
hasFix @6 :Bool;
|
||||
gpsOK @7 :Bool; # a raw fix arrived in the last 2 s
|
||||
fused @8 :Bool; # false while passing the raw fix through
|
||||
struct CustomReserved10 @0xcb9fd56c7057593a {
|
||||
}
|
||||
|
||||
struct CustomReserved11 @0xc2243c65e0340384 {
|
||||
|
||||
@@ -2648,7 +2648,7 @@ struct Event {
|
||||
carStateSP @114 :Custom.CarStateSP;
|
||||
liveMapDataSP @115 :Custom.LiveMapDataSP;
|
||||
modelDataV2SP @116 :Custom.ModelDataV2SP;
|
||||
gpsLocationSP @136 :Custom.GpsLocationSP;
|
||||
customReserved10 @136 :Custom.CustomReserved10;
|
||||
customReserved11 @137 :Custom.CustomReserved11;
|
||||
customReserved12 @138 :Custom.CustomReserved12;
|
||||
customReserved13 @139 :Custom.CustomReserved13;
|
||||
@@ -2701,7 +2701,7 @@ struct Event {
|
||||
lateralPlanDEPRECATED @64 :LateralPlan;
|
||||
navModelDEPRECATED @104 :Deprecated.NavModelData;
|
||||
uiPlanDEPRECATED @106 :UiPlan;
|
||||
liveLocationKalmanDEPRECATED @72 :LiveLocationKalman;
|
||||
liveLocationKalman @72 :LiveLocationKalman;
|
||||
liveTracksDEPRECATED @16 :List(Deprecated.LiveTracksDEPRECATED);
|
||||
onroadEventsDEPRECATED @68: List(Car.OnroadEventDEPRECATED);
|
||||
gyroscope2DEPRECATED @100 :SensorEventData;
|
||||
|
||||
@@ -95,7 +95,7 @@ _services: dict[str, tuple] = {
|
||||
"carStateSP": (True, 100., 10),
|
||||
"liveMapDataSP": (True, 1., 1),
|
||||
"modelDataV2SP": (True, 20., 10, QueueSize.BIG),
|
||||
"gpsLocationSP": (True, 20., 20),
|
||||
"liveLocationKalman": (True, 20.),
|
||||
|
||||
# debug
|
||||
"uiDebug": (True, 0., 1),
|
||||
|
||||
@@ -149,7 +149,6 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
|
||||
{"Version", {PERSISTENT, STRING}},
|
||||
|
||||
// --- sunnypilot params --- //
|
||||
{"AccelBoostSP", {PERSISTENT | BACKUP, BOOL, "1"}},
|
||||
{"ApiCache_DriveStats", {PERSISTENT, JSON}},
|
||||
{"AutoLaneChangeBsmDelay", {PERSISTENT | BACKUP, BOOL, "0"}},
|
||||
{"AutoLaneChangeTimer", {PERSISTENT | BACKUP, INT, "0"}},
|
||||
@@ -172,13 +171,14 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
|
||||
{"EnableCopyparty", {PERSISTENT | BACKUP, BOOL}},
|
||||
{"EnableGithubRunner", {PERSISTENT | BACKUP, BOOL}},
|
||||
{"GreenLightAlert", {PERSISTENT | BACKUP, BOOL, "0"}},
|
||||
{"GithubRunnerSufficientVoltage", {CLEAR_ON_MANAGER_START , BOOL}},
|
||||
{"HasAcceptedTermsSP", {PERSISTENT, STRING, "0"}},
|
||||
{"HideVEgoUI", {PERSISTENT | BACKUP, BOOL, "0"}},
|
||||
{"IntelligentCruiseButtonManagement", {PERSISTENT | BACKUP , BOOL}},
|
||||
{"InteractivityTimeout", {PERSISTENT | BACKUP, INT, "0"}},
|
||||
{"IsDevelopmentBranch", {CLEAR_ON_MANAGER_START, BOOL}},
|
||||
{"IsReleaseSpBranch", {CLEAR_ON_MANAGER_START, BOOL}},
|
||||
{"LastGPSPositionFused", {PERSISTENT, STRING}},
|
||||
{"LastGPSPositionLLK", {PERSISTENT, STRING}},
|
||||
{"LeadDepartAlert", {PERSISTENT | BACKUP, BOOL, "0"}},
|
||||
{"MaxTimeOffroad", {PERSISTENT | BACKUP, INT, "1800"}},
|
||||
{"ModelRunnerTypeCache", {CLEAR_ON_ONROAD_TRANSITION, INT}},
|
||||
@@ -239,6 +239,7 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
|
||||
{"BackupManager_RestoreVersion", {PERSISTENT, STRING}},
|
||||
|
||||
// sunnypilot car specific params
|
||||
{"HyundaiCustomButtonAction", {PERSISTENT | BACKUP, INT, "0"}},
|
||||
{"HyundaiLongitudinalTuning", {PERSISTENT | BACKUP, INT, "0"}},
|
||||
{"SubaruStopAndGo", {PERSISTENT | BACKUP, BOOL, "0"}},
|
||||
{"SubaruStopAndGoManualParkingBrake", {PERSISTENT | BACKUP, BOOL, "0"}},
|
||||
|
||||
@@ -0,0 +1,38 @@
|
||||
"""
|
||||
Copyright (c) 2021-, sunnypilot, and a number of other contributors.
|
||||
|
||||
This file is part of sunnypilot and is licensed under the MIT License.
|
||||
See the LICENSE.md file in the root directory for more details.
|
||||
"""
|
||||
from collections.abc import Callable
|
||||
from enum import IntEnum
|
||||
|
||||
from opendbc.car.structs import car
|
||||
from openpilot.selfdrive.ui.ui_state import ui_state
|
||||
|
||||
ALT_BUTTON_2_BIT = int(car.CarState.ButtonEvent.Type.altButton2)
|
||||
|
||||
|
||||
class CustomButtonAction(IntEnum):
|
||||
NONE = 0
|
||||
BOOKMARK = 1
|
||||
CYCLE_UI = 2
|
||||
|
||||
|
||||
class CustomButton:
|
||||
"""Runs the selected action on each custom button release, read from selfdriveStateSP.buttonsReleaseToggle."""
|
||||
def __init__(self, callbacks: dict[int, Callable[[], None]]):
|
||||
self._callbacks = callbacks
|
||||
self._prev_toggle: int | None = None
|
||||
|
||||
def update(self) -> None:
|
||||
if not (ui_state.started and ui_state.has_custom_button):
|
||||
self._prev_toggle = None
|
||||
return
|
||||
|
||||
toggle = ui_state.sm["selfdriveStateSP"].buttonsReleaseToggle
|
||||
released = self._prev_toggle is not None and (self._prev_toggle ^ toggle) >> ALT_BUTTON_2_BIT & 1
|
||||
self._prev_toggle = toggle
|
||||
|
||||
if released and (callback := self._callbacks.get(ui_state.params.get("HyundaiCustomButtonAction", return_default=True))):
|
||||
callback()
|
||||
@@ -0,0 +1,29 @@
|
||||
"""
|
||||
Copyright (c) 2021-, sunnypilot, and a number of other contributors.
|
||||
|
||||
This file is part of sunnypilot and is licensed under the MIT License.
|
||||
See the LICENSE.md file in the root directory for more details.
|
||||
"""
|
||||
from openpilot.selfdrive.ui.layouts.main import MainLayout, MainState
|
||||
from openpilot.selfdrive.ui.sunnypilot.custom_button import CustomButton, CustomButtonAction
|
||||
|
||||
|
||||
class MainLayoutSP(MainLayout):
|
||||
def __init__(self):
|
||||
super().__init__()
|
||||
self._custom_button = CustomButton({
|
||||
CustomButtonAction.BOOKMARK: self._on_bookmark_clicked,
|
||||
CustomButtonAction.CYCLE_UI: self._cycle_ui,
|
||||
})
|
||||
|
||||
def _render(self, rect):
|
||||
self._custom_button.update()
|
||||
super()._render(rect)
|
||||
|
||||
def _cycle_ui(self):
|
||||
if self._current_mode == MainState.ONROAD and not self._sidebar.is_visible:
|
||||
self._sidebar.set_visible(True)
|
||||
elif self._current_mode == MainState.SETTINGS:
|
||||
self._set_mode_for_state()
|
||||
else:
|
||||
self._on_settings_clicked()
|
||||
@@ -57,12 +57,6 @@ class CruiseLayout(Widget):
|
||||
description=tr("Use map data to estimate the appropriate speed to drive through turns ahead."),
|
||||
param="SmartCruiseControlMap")
|
||||
|
||||
self.accel_boost_toggle = toggle_item_sp(
|
||||
title=tr("Acceleration Boost"),
|
||||
description=tr("Adds a small push on top of sunnypilot's own acceleration, stronger in Aggressive and gentler in Relaxed. " +
|
||||
"Eases off in curves and when sunnypilot slows or stops. Never goes past your set speed, and lead-car following still comes first."),
|
||||
param="AccelBoostSP")
|
||||
|
||||
self.custom_acc_toggle = toggle_item_sp(
|
||||
title=tr("Custom ACC Speed Increments"),
|
||||
description="",
|
||||
@@ -92,7 +86,6 @@ class CruiseLayout(Widget):
|
||||
self.icbm_toggle,
|
||||
self.scc_v_toggle,
|
||||
self.scc_m_toggle,
|
||||
self.accel_boost_toggle,
|
||||
self.custom_acc_toggle,
|
||||
self.custom_acc_short_increment,
|
||||
self.custom_acc_long_increment,
|
||||
@@ -143,8 +136,6 @@ class CruiseLayout(Widget):
|
||||
self.icbm_toggle.set_description(new_desc)
|
||||
self.icbm_toggle.show_description(True)
|
||||
|
||||
self.accel_boost_toggle.action_item.set_enabled(has_long)
|
||||
|
||||
if has_long or has_icbm:
|
||||
self.custom_acc_toggle.action_item.set_enabled(((has_long and not ui_state.CP.pcmCruise) or has_icbm) and ui_state.is_offroad())
|
||||
self.scc_v_toggle.action_item.set_enabled(True)
|
||||
@@ -160,7 +151,6 @@ class CruiseLayout(Widget):
|
||||
else:
|
||||
has_icbm = has_long = False
|
||||
self.icbm_toggle.action_item.set_enabled(False)
|
||||
self.accel_boost_toggle.action_item.set_enabled(False)
|
||||
self.icbm_toggle.set_description(tr(ONROAD_ONLY_DESCRIPTION))
|
||||
|
||||
show_custom_acc_desc = False
|
||||
|
||||
@@ -102,9 +102,10 @@ class ModelsLayout(Widget):
|
||||
|
||||
self.lagd_toggle = toggle_item_sp(tr("Live Learning Steer Delay"), "", param="LagdToggle")
|
||||
|
||||
self._blendful_desc = tr("Adds a gentle nudge toward lane center on top of the model's own steering, stronger when lane lines are clear." +
|
||||
" Yields around obstacles. Not available with the Default model.")
|
||||
self.blendful_toggle = toggle_item_sp(tr("Blendful (Alpha)"), self._blendful_desc, param="Blendful")
|
||||
self.blendful_toggle = toggle_item_sp(tr("Blendful (Alpha)"),
|
||||
tr("Adds a gentle nudge toward lane center on top of the model's own steering, stronger when lane lines are clear." +
|
||||
" Yields around obstacles. Not available with the Default model."),
|
||||
param="Blendful")
|
||||
|
||||
self.camera_offset = option_item_sp(tr("Adjust Camera Offset"), "CameraOffset", -35, 35,
|
||||
tr("Virtually shift camera's perspective to move model's center to Left(+ values) or Right (- values)"),
|
||||
@@ -249,11 +250,7 @@ class ModelsLayout(Widget):
|
||||
return {"name": name, "downloading": True, "progress": progress, "status_text": tr("verifying")}
|
||||
if ds.downloading in statuses:
|
||||
speed = max((p.speed for p in progresses), default=0.0)
|
||||
speed_text = f"{speed / 1024 ** 2:.1f} MB/s" if speed > 0 else "--.- MB/s"
|
||||
if any(p.resuming for p in progresses):
|
||||
status_text = tr("resuming...") + f" {speed_text}"
|
||||
else:
|
||||
status_text = speed_text
|
||||
status_text = f"{speed / 1024 ** 2:.1f} MB/s" if speed > 0 else "--.- MB/s"
|
||||
return {"name": name, "downloading": True, "progress": progress, "status_text": status_text}
|
||||
if statuses <= {ds.downloaded, ds.cached}:
|
||||
return {"name": name, "text_color": ON_COLOR, "icon": "icons/checkmark.png"}
|
||||
@@ -340,12 +337,6 @@ class ModelsLayout(Widget):
|
||||
self.blendful_toggle.action_item.set_state(False)
|
||||
self.blendful_toggle.action_item.set_enabled(False)
|
||||
|
||||
blendful_desc = self._blendful_desc if blendful_allowed else \
|
||||
f"<b>{tr('Select a non-Default model to use Blendful.')}</b><br><br>{self._blendful_desc}"
|
||||
if self.blendful_toggle.description != blendful_desc:
|
||||
self.blendful_toggle.set_description(blendful_desc)
|
||||
self.blendful_toggle.show_description(not blendful_allowed)
|
||||
|
||||
self.delay_control.set_visible(not live_delay and advanced_controls)
|
||||
new_step = int(round(100 / CV.MPH_TO_KPH)) if ui_state.is_metric else 100
|
||||
if self.lane_turn_value_control.action_item is not None and self.lane_turn_value_control.action_item.value_change_step != new_step:
|
||||
|
||||
@@ -20,7 +20,13 @@ class HyundaiSettings(BrandSettings):
|
||||
self.longitudinal_tuning_item = multiple_button_item_sp(tr("Custom Longitudinal Tuning"), "", tuning_texts,
|
||||
button_width=300, callback=self._on_tuning_selected,
|
||||
param="HyundaiLongitudinalTuning", inline=False)
|
||||
self.items = [self.longitudinal_tuning_item]
|
||||
|
||||
custom_button_texts = [tr("None"), tr("Bookmark"), tr("Cycle UI")]
|
||||
self.custom_button_item = multiple_button_item_sp(tr("Steering Custom Button"),
|
||||
tr("Choose the sunnypilot action for the steering wheel custom/star button."),
|
||||
custom_button_texts, button_width=300,
|
||||
param="HyundaiCustomButtonAction", inline=False)
|
||||
self.items = [self.custom_button_item, self.longitudinal_tuning_item]
|
||||
|
||||
@staticmethod
|
||||
def _on_tuning_selected(index):
|
||||
@@ -57,3 +63,7 @@ class HyundaiSettings(BrandSettings):
|
||||
self.longitudinal_tuning_item.show_description(True)
|
||||
self.longitudinal_tuning_item.action_item.set_selected_button(tuning_param)
|
||||
self.longitudinal_tuning_item.set_visible(self.alpha_long_available)
|
||||
|
||||
self.custom_button_item.action_item.set_selected_button(ui_state.params.get("HyundaiCustomButtonAction", return_default=True))
|
||||
self.custom_button_item.show_description(True)
|
||||
self.custom_button_item.set_visible(ui_state.has_custom_button)
|
||||
|
||||
@@ -0,0 +1,39 @@
|
||||
"""
|
||||
Copyright (c) 2021-, sunnypilot, and a number of other contributors.
|
||||
|
||||
This file is part of sunnypilot and is licensed under the MIT License.
|
||||
See the LICENSE.md file in the root directory for more details.
|
||||
"""
|
||||
from openpilot.selfdrive.ui.mici.layouts.main import MiciMainLayout
|
||||
from openpilot.selfdrive.ui.sunnypilot.custom_button import CustomButton, CustomButtonAction
|
||||
from openpilot.system.ui.lib.application import gui_app
|
||||
from openpilot.system.ui.widgets import Widget
|
||||
|
||||
|
||||
class MiciMainLayoutSP(MiciMainLayout):
|
||||
def __init__(self):
|
||||
super().__init__()
|
||||
self._custom_button = CustomButton({
|
||||
CustomButtonAction.BOOKMARK: self._on_bookmark_clicked,
|
||||
CustomButtonAction.CYCLE_UI: self._cycle_ui,
|
||||
})
|
||||
|
||||
def _render(self, rect):
|
||||
self._custom_button.update()
|
||||
super()._render(rect)
|
||||
|
||||
def _show_layout(self, layout: Widget):
|
||||
if gui_app.widget_in_stack(self._onboarding_window):
|
||||
return
|
||||
gui_app.pop_widgets_to(self, lambda: self._scroll_to(layout))
|
||||
|
||||
def _layout_visible(self, layout: Widget) -> bool:
|
||||
return abs(layout.rect.x - self._rect.x) < self._rect.width / 2
|
||||
|
||||
def _cycle_ui(self):
|
||||
if gui_app.widget_in_stack(self._settings_layout):
|
||||
self._show_layout(self._onroad_layout)
|
||||
elif gui_app.get_active_widget() is self and self._layout_visible(self._home_layout):
|
||||
gui_app.push_widget(self._settings_layout)
|
||||
else:
|
||||
self._show_layout(self._home_layout)
|
||||
@@ -229,9 +229,6 @@ class ModelsLayoutMici(NavScroller):
|
||||
else:
|
||||
self.blendful_toggle.set_checked(False)
|
||||
self.blendful_toggle.set_enabled(False)
|
||||
blendful_value = "" if blendful_allowed else tr("custom model only")
|
||||
if self.blendful_toggle.value != blendful_value:
|
||||
self.blendful_toggle.set_value(blendful_value)
|
||||
|
||||
manager = self.model_manager
|
||||
self._download_frame += 1
|
||||
@@ -267,8 +264,8 @@ class ModelsLayoutMici(NavScroller):
|
||||
self.current_model_info.info_text.set_text(info_text)
|
||||
|
||||
if manager.selectedBundle and manager.selectedBundle.status == custom.ModelManagerSP.DownloadStatus.failed:
|
||||
self.current_model_info.info_header.set_text(tr("error"))
|
||||
self.current_model_info.info_text.set_text(f"{manager.selectedBundle.internalName.lower()} | {tr('download failed')}")
|
||||
self.current_model_info.info_header.set_text(tr("error") + self._download_progress)
|
||||
self.current_model_info.info_text.set_text(tr("download failed"))
|
||||
|
||||
elif manager.selectedBundle and manager.selectedBundle.status == custom.ModelManagerSP.DownloadStatus.downloading:
|
||||
self.cancel_download_btn.set_visible(ui_state.params.get("ModelManager_DownloadRef") is not None)
|
||||
@@ -277,7 +274,6 @@ class ModelsLayoutMici(NavScroller):
|
||||
count = 0
|
||||
verifying = False
|
||||
canceling = False
|
||||
resuming = False
|
||||
speed = 0.0
|
||||
for model in manager.selectedBundle.models:
|
||||
count += 1
|
||||
@@ -287,13 +283,12 @@ class ModelsLayoutMici(NavScroller):
|
||||
progress += p.progress
|
||||
verifying = verifying or p.status == custom.ModelManagerSP.DownloadStatus.verifying
|
||||
canceling = canceling or p.canceling
|
||||
resuming = resuming or p.resuming
|
||||
speed = max(speed, p.speed) # only one file downloads at a time, so the non-zero speed is the live one
|
||||
elif p.status in (custom.ModelManagerSP.DownloadStatus.downloaded,
|
||||
custom.ModelManagerSP.DownloadStatus.cached):
|
||||
progress += 100.0
|
||||
|
||||
header = tr("canceling...") if canceling else tr("verifying") if verifying else tr("resuming...") if resuming else tr("downloading")
|
||||
header = tr("canceling...") if canceling else tr("verifying") if verifying else tr("downloading")
|
||||
self.current_model_info.current_model_header.set_text(header)
|
||||
self.cancel_download_btn.set_text(tr("cancel verification") if verifying else tr("cancel download"))
|
||||
self.current_model_info.current_model_header._shimmer = True
|
||||
|
||||
@@ -9,6 +9,7 @@ from enum import Enum
|
||||
from openpilot.cereal import messaging, log, custom
|
||||
from opendbc.car.structs import car
|
||||
from opendbc.sunnypilot.car.interfaces import has_bsm
|
||||
from opendbc.sunnypilot.car.hyundai.values import HyundaiFlagsSP
|
||||
from openpilot.common.params import Params
|
||||
from openpilot.selfdrive.ui.sunnypilot.layouts.settings.display import OnroadBrightness
|
||||
from openpilot.sunnypilot.models.helpers import ACTIVE_BUNDLE_KEYS, get_active_source
|
||||
@@ -33,6 +34,7 @@ class UIStateSP:
|
||||
self.params = Params()
|
||||
self.CP_SP: custom.CarParamsSP | None = None
|
||||
self.has_icbm: bool = False
|
||||
self.has_custom_button: bool = False
|
||||
self.is_sp_release: bool = self.params.get_bool("IsReleaseSpBranch")
|
||||
self.sm_services_ext = [
|
||||
"modelManagerSP", "selfdriveStateSP", "longitudinalPlanSP", "backupManagerSP",
|
||||
@@ -160,6 +162,8 @@ class UIStateSP:
|
||||
if CP_SP_bytes is not None:
|
||||
self.CP_SP = messaging.log_from_bytes(CP_SP_bytes, custom.CarParamsSP)
|
||||
self.has_icbm = self.CP_SP.intelligentCruiseButtonManagementAvailable and self.params.get_bool("IntelligentCruiseButtonManagement")
|
||||
# flagsSP bits are per brand
|
||||
self.has_custom_button = self.CP is not None and self.CP.brand == "hyundai" and bool(self.CP_SP.flags & HyundaiFlagsSP.HAS_CUSTOM_BUTTON)
|
||||
|
||||
self._enforce_constraints()
|
||||
source = get_active_source(chestnut=self.chestnut_present, chestnut_active=self.chestnut_active,
|
||||
|
||||
@@ -0,0 +1,41 @@
|
||||
from types import SimpleNamespace
|
||||
from unittest.mock import Mock
|
||||
|
||||
from openpilot.common.test import OpenpilotTestCase
|
||||
import openpilot.selfdrive.ui.sunnypilot.custom_button as custom_button
|
||||
from openpilot.selfdrive.ui.sunnypilot.custom_button import ALT_BUTTON_2_BIT, CustomButton, CustomButtonAction
|
||||
|
||||
|
||||
def make_ui_state(monkeypatch, action, has_custom_button=True):
|
||||
state = SimpleNamespace(started=True, has_custom_button=has_custom_button, params=Mock(),
|
||||
sm={"selfdriveStateSP": SimpleNamespace(buttonsReleaseToggle=0)})
|
||||
state.params.get.return_value = action
|
||||
monkeypatch.setattr(custom_button, "ui_state", state)
|
||||
return state
|
||||
|
||||
|
||||
def run(button, state, toggles):
|
||||
for toggle in toggles:
|
||||
state.sm["selfdriveStateSP"].buttonsReleaseToggle = toggle
|
||||
button.update()
|
||||
|
||||
|
||||
class TestCustomButton(OpenpilotTestCase):
|
||||
def test_actions_on_release(self, monkeypatch):
|
||||
for action in (CustomButtonAction.BOOKMARK, CustomButtonAction.CYCLE_UI):
|
||||
state = make_ui_state(monkeypatch, action)
|
||||
callbacks = {a: Mock() for a in (CustomButtonAction.BOOKMARK, CustomButtonAction.CYCLE_UI)}
|
||||
# first read seeds, then two releases of altButton2 and one of another button
|
||||
run(CustomButton(callbacks), state, [0, 1 << ALT_BUTTON_2_BIT, 1 << ALT_BUTTON_2_BIT, 0, 0b1])
|
||||
assert callbacks[action].call_count == 2
|
||||
assert sum(cb.call_count for cb in callbacks.values()) == 2
|
||||
|
||||
|
||||
def test_no_action_when_unavailable(self, monkeypatch):
|
||||
callback = Mock()
|
||||
state = make_ui_state(monkeypatch, CustomButtonAction.BOOKMARK, has_custom_button=False)
|
||||
run(CustomButton({CustomButtonAction.BOOKMARK: callback}), state, [0, 1 << ALT_BUTTON_2_BIT])
|
||||
|
||||
state = make_ui_state(monkeypatch, CustomButtonAction.BOOKMARK)
|
||||
run(CustomButton({CustomButtonAction.BOOKMARK: callback}), state, [1 << ALT_BUTTON_2_BIT]) # stale toggle on first read
|
||||
assert callback.call_count == 0
|
||||
@@ -10,6 +10,10 @@ from openpilot.selfdrive.ui.layouts.main import MainLayout
|
||||
from openpilot.selfdrive.ui.mici.layouts.main import MiciMainLayout
|
||||
from openpilot.selfdrive.ui.ui_state import ui_state
|
||||
|
||||
if gui_app.sunnypilot_ui():
|
||||
from openpilot.selfdrive.ui.sunnypilot.layouts.main import MainLayoutSP as MainLayout
|
||||
from openpilot.selfdrive.ui.sunnypilot.mici.layouts.main import MiciMainLayoutSP as MiciMainLayout
|
||||
|
||||
BIG_UI = gui_app.big_ui()
|
||||
|
||||
|
||||
|
||||
@@ -0,0 +1,2 @@
|
||||
SConscript(['common/transformations/SConscript'])
|
||||
SConscript(['selfdrive/locationd/SConscript'])
|
||||
@@ -0,0 +1,4 @@
|
||||
Import('env')
|
||||
|
||||
transformations = env.Library('transformations', ['orientation.cc', 'coordinates.cc'])
|
||||
Export('transformations')
|
||||
@@ -0,0 +1,100 @@
|
||||
#define _USE_MATH_DEFINES
|
||||
|
||||
#include "sunnypilot/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,143 @@
|
||||
#define _USE_MATH_DEFINES
|
||||
|
||||
#include <iostream>
|
||||
#include <cmath>
|
||||
#include <eigen3/Eigen/Dense>
|
||||
|
||||
#include "sunnypilot/common/transformations/orientation.hpp"
|
||||
#include "sunnypilot/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 "sunnypilot/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);
|
||||
@@ -139,7 +139,7 @@ class ModularAssistiveDrivingSystem:
|
||||
self.transition_paused_state()
|
||||
|
||||
if self.steering_mode_on_brake == MadsSteeringModeOnBrake.PAUSE:
|
||||
if CS.brakePressed or CS.regenBraking or self.pedal_pressed_non_gas_pressed(CS):
|
||||
if self.pedal_pressed_non_gas_pressed(CS):
|
||||
self.transition_paused_state()
|
||||
|
||||
self.events.remove(EventName.preEnableStandstill)
|
||||
|
||||
@@ -107,22 +107,6 @@ class TestShouldSilentLkasEnable(OpenpilotTestCase):
|
||||
# pause
|
||||
|
||||
class TestPauseMode(OpenpilotTestCase):
|
||||
def test_pauses_at_standstill_brake_already_held(self, mocker):
|
||||
mads, sd = make_mads(mocker, MadsSteeringModeOnBrake.PAUSE)
|
||||
mads.state_machine.state = State.enabled
|
||||
mads.enabled = True
|
||||
mads.active = True
|
||||
sd.CS_prev = make_car_state(brake_pressed=True, standstill=True)
|
||||
sd.sm['pandaStates'] = [make_panda_state(mocker, False)]
|
||||
cs = make_car_state(brake_pressed=True, standstill=True)
|
||||
|
||||
run_frames(mads, sd, cs, n=199)
|
||||
mads.update(cs)
|
||||
|
||||
assert not sd.events_sp.has(EventNameSP.controlsMismatchLateral)
|
||||
assert mads.state_machine.state == State.paused
|
||||
assert mads.lateral_mismatch_counter == 0
|
||||
|
||||
def test_stays_paused_at_standstill_brake_held(self, mocker):
|
||||
mads, sd = make_mads(mocker, MadsSteeringModeOnBrake.PAUSE)
|
||||
mads.state_machine.state = State.enabled
|
||||
|
||||
@@ -19,12 +19,12 @@ class BaseMapData(ABC):
|
||||
def __init__(self):
|
||||
self.params = Params()
|
||||
|
||||
self.sm = messaging.SubMaster(['gpsLocationSP'])
|
||||
self.sm = messaging.SubMaster(['liveLocationKalman'])
|
||||
self.pm = messaging.PubMaster(['liveMapDataSP'])
|
||||
|
||||
self.localizer_valid = False
|
||||
self.last_bearing = None
|
||||
self.last_position = coordinate_from_param("LastGPSPositionFused", self.params)
|
||||
self.last_position = coordinate_from_param("LastGPSPositionLLK", self.params)
|
||||
|
||||
@abstractmethod
|
||||
def update_location(self) -> None:
|
||||
@@ -47,7 +47,7 @@ class BaseMapData(ABC):
|
||||
next_speed_limit, next_speed_limit_distance = self.get_next_speed_limit_and_distance()
|
||||
|
||||
mapd_sp_send = messaging.new_message('liveMapDataSP')
|
||||
mapd_sp_send.valid = self.sm['gpsLocationSP'].gpsOK
|
||||
mapd_sp_send.valid = self.sm['liveLocationKalman'].gpsOK
|
||||
live_map_data = mapd_sp_send.liveMapDataSP
|
||||
|
||||
live_map_data.speedLimitValid = bool(MAX_SPEED_LIMIT > speed_limit > 0)
|
||||
|
||||
@@ -5,8 +5,10 @@ This file is part of sunnypilot and is licensed under the MIT License.
|
||||
See the LICENSE.md file in the root directory for more details.
|
||||
"""
|
||||
import json
|
||||
import math
|
||||
import platform
|
||||
|
||||
from openpilot.cereal import log
|
||||
from openpilot.common.params import Params
|
||||
from openpilot.sunnypilot.mapd.live_map_data.base_map_data import BaseMapData
|
||||
from openpilot.sunnypilot.navd.helpers import Coordinate
|
||||
@@ -18,12 +20,12 @@ class OsmMapData(BaseMapData):
|
||||
self.mem_params = Params("/dev/shm/params") if platform.system() != "Darwin" else self.params
|
||||
|
||||
def update_location(self) -> None:
|
||||
location = self.sm['gpsLocationSP']
|
||||
self.localizer_valid = self.sm.valid['gpsLocationSP'] and location.hasFix
|
||||
location = self.sm['liveLocationKalman']
|
||||
self.localizer_valid = (location.status == log.LiveLocationKalman.Status.valid) and location.positionGeodetic.valid
|
||||
|
||||
if self.localizer_valid:
|
||||
self.last_bearing = location.bearingDeg
|
||||
self.last_position = Coordinate(location.latitude, location.longitude)
|
||||
self.last_bearing = math.degrees(location.calibratedOrientationNED.value[2])
|
||||
self.last_position = Coordinate(location.positionGeodetic.value[0], location.positionGeodetic.value[1])
|
||||
|
||||
if self.last_position is None:
|
||||
return
|
||||
|
||||
@@ -5,9 +5,11 @@ This file is part of sunnypilot and is licensed under the MIT License.
|
||||
See the LICENSE.md file in the root directory for more details.
|
||||
"""
|
||||
import json
|
||||
import math
|
||||
import platform
|
||||
|
||||
import openpilot.cereal.messaging as messaging
|
||||
from openpilot.cereal import log
|
||||
from openpilot.common.params import Params
|
||||
from openpilot.common.test import OpenpilotTestCase
|
||||
from openpilot.sunnypilot.mapd.live_map_data.osm_map_data import OsmMapData
|
||||
@@ -18,26 +20,28 @@ class TestOsmMapData(OpenpilotTestCase):
|
||||
|
||||
LATITUDE = 32.875134
|
||||
LONGITUDE = -117.209777
|
||||
ALTITUDE = 42.0
|
||||
BEARING = 90.0
|
||||
|
||||
def setup_method(self):
|
||||
self.mem_params = Params("/dev/shm/params") if platform.system() != "Darwin" else Params()
|
||||
self.mem_params.remove("LastGPSPosition")
|
||||
self.map_data = OsmMapData()
|
||||
self.pm = messaging.PubMaster(['gpsLocationSP'])
|
||||
self.pm = messaging.PubMaster(['liveLocationKalman'])
|
||||
|
||||
def location_message(self, has_fix: bool):
|
||||
msg = messaging.new_message('gpsLocationSP', valid=has_fix)
|
||||
location = msg.gpsLocationSP
|
||||
location.hasFix = has_fix
|
||||
location.latitude, location.longitude, location.bearingDeg = self.LATITUDE, self.LONGITUDE, self.BEARING
|
||||
def location_message(self, status: log.LiveLocationKalman.Status, position_valid: bool):
|
||||
msg = messaging.new_message('liveLocationKalman', valid=True)
|
||||
location = msg.liveLocationKalman
|
||||
location.status = status
|
||||
location.positionGeodetic = {'value': [self.LATITUDE, self.LONGITUDE, self.ALTITUDE], 'valid': position_valid}
|
||||
location.calibratedOrientationNED = {'value': [0.0, 0.0, math.radians(self.BEARING)], 'valid': True}
|
||||
return msg
|
||||
|
||||
def send_location(self, has_fix: bool) -> None:
|
||||
def send_location(self, status: log.LiveLocationKalman.Status, position_valid: bool) -> None:
|
||||
# the first exchange synchronizes the PUB/SUB connection, the second one is actually received
|
||||
self.pm.send('gpsLocationSP', self.location_message(has_fix))
|
||||
self.pm.send('liveLocationKalman', self.location_message(status, position_valid))
|
||||
self.map_data.sm.update(1000)
|
||||
self.pm.send('gpsLocationSP', self.location_message(has_fix))
|
||||
self.pm.send('liveLocationKalman', self.location_message(status, position_valid))
|
||||
self.map_data.sm.update(1000)
|
||||
|
||||
def read_position(self) -> dict:
|
||||
@@ -46,7 +50,7 @@ class TestOsmMapData(OpenpilotTestCase):
|
||||
return json.loads(value)
|
||||
|
||||
def test_writes_position_for_mapd(self):
|
||||
self.send_location(True)
|
||||
self.send_location(log.LiveLocationKalman.Status.valid, True)
|
||||
self.map_data.tick()
|
||||
|
||||
position = self.read_position()
|
||||
@@ -55,10 +59,10 @@ class TestOsmMapData(OpenpilotTestCase):
|
||||
self.assertAlmostEqual(position['bearing'], self.BEARING, delta=1e-6)
|
||||
|
||||
def test_invalid_localizer_keeps_last_known_position(self):
|
||||
self.send_location(True)
|
||||
self.send_location(log.LiveLocationKalman.Status.valid, True)
|
||||
self.map_data.tick()
|
||||
|
||||
self.send_location(False)
|
||||
self.send_location(log.LiveLocationKalman.Status.uninitialized, False)
|
||||
self.map_data.tick()
|
||||
|
||||
position = self.read_position()
|
||||
|
||||
@@ -17,25 +17,16 @@ def main():
|
||||
parser.add_argument("--frame", required=True, help="width,height,stride,y_height,uv_height,buffer_size")
|
||||
parser.add_argument("--warp-to", required=True, help="model input WxH")
|
||||
parser.add_argument("--output", required=True)
|
||||
parser.add_argument("--split-frames", action="store_true", help="one input per camera (frame, big_frame), bound to the camera buffers by pointer")
|
||||
args = parser.parse_args()
|
||||
|
||||
from examples.openpilot.compile_warp import make_frame_prepare, parse_frame, parse_size
|
||||
from tinygrad import Device, Tensor, TinyJit
|
||||
frame = parse_frame(args.frame)
|
||||
warp = make_frame_prepare(frame, *parse_size(args.warp_to))
|
||||
run = TinyJit(lambda input_frame, M_inv: Tensor.stack(*(warp(input_frame[i], M_inv[i]) for i in range(2))), prune=True)
|
||||
rng = np.random.default_rng(42)
|
||||
frames = rng.integers(0, 256, (2, frame.size), dtype=np.uint8)
|
||||
m_inv = Tensor((rng.standard_normal((2, 3, 3)) * 8).astype(np.float32), device=Device.DEFAULT).realize()
|
||||
if args.split_frames:
|
||||
run = TinyJit(lambda frame, big_frame, M_inv: Tensor.stack(warp(frame, M_inv[0]), warp(big_frame, M_inv[1])), prune=True)
|
||||
inputs = {"frame": Tensor(frames[0], device=Device.DEFAULT).realize(), "big_frame": Tensor(frames[1], device=Device.DEFAULT).realize(), "M_inv": m_inv}
|
||||
specs = {"frame": ((frame.size,), "|u1", Device.DEFAULT), "big_frame": ((frame.size,), "|u1", Device.DEFAULT),
|
||||
"M_inv": ((2, 3, 3), "<f4", Device.DEFAULT)}
|
||||
else:
|
||||
run = TinyJit(lambda input_frame, M_inv: Tensor.stack(*(warp(input_frame[i], M_inv[i]) for i in range(2))), prune=True)
|
||||
inputs = {"input_frame": Tensor(frames, device=Device.DEFAULT).realize(), "M_inv": m_inv}
|
||||
specs = {"input_frame": ((2, frame.size), "|u1", Device.DEFAULT), "M_inv": ((2, 3, 3), "<f4", Device.DEFAULT)}
|
||||
inputs = {"input_frame": Tensor(rng.integers(0, 256, (2, frame.size), dtype=np.uint8), device=Device.DEFAULT).realize(),
|
||||
"M_inv": Tensor((rng.standard_normal((2, 3, 3)) * 8).astype(np.float32), device=Device.DEFAULT).realize()}
|
||||
with record_kernel_linear() as recorded:
|
||||
run(**inputs)
|
||||
run(**inputs)
|
||||
@@ -43,6 +34,7 @@ def main():
|
||||
assert len(recorded) == 1, f"expected one captured kernel list, got {len(recorded)}"
|
||||
part = serialize_jit(run, recorded[0], check)
|
||||
PlainJit(*part)
|
||||
specs = {"input_frame": ((2, frame.size), "|u1", Device.DEFAULT), "M_inv": ((2, 3, 3), "<f4", Device.DEFAULT)}
|
||||
write_bundle(args.output, {"run": part}, {"input_specs": specs, "metadata": {}})
|
||||
print(f"Saved warp to {args.output} ({len(part[0]['calls'])} kernels)")
|
||||
|
||||
|
||||
@@ -66,75 +66,47 @@ def _patched(buf, topo) -> tuple[bytearray, dict]:
|
||||
return blob, rows
|
||||
|
||||
|
||||
def _packed_args(program) -> dict[int, int]:
|
||||
from tinygrad.device import TinyELF
|
||||
params = getattr(program.arg, "params", None) or ()
|
||||
return {off: p.arg.slot for (off, _), p in zip(TinyELF.iter_sig(tuple((None, p.addrspace, p.dtype, ()) for p in params)), params, strict=True)}
|
||||
|
||||
|
||||
def _outer_slot(u, packed: dict[int, int]) -> int | None:
|
||||
from tinygrad.uop.ops import Ops
|
||||
if u.op is Ops.PARAM:
|
||||
return None if packed else u.arg.slot
|
||||
if u.op.name == "REFPARAM":
|
||||
return u.arg.slot
|
||||
if packed and u.op is Ops.LOAD and u.src[0].op is Ops.INDEX and _root(u.src[0].src[0]).op is Ops.PARAM:
|
||||
index = _evaluate(u.src[0].src[1], {}, packed)
|
||||
return None if index is None else packed.get(index * u.dtype.itemsize)
|
||||
return None
|
||||
|
||||
|
||||
def _evaluate(u, env, packed: dict[int, int]):
|
||||
def _evaluate(u, env):
|
||||
from tinygrad.uop.ops import GroupOp, Ops, exec_alu
|
||||
if u.op in (Ops.AFTER, Ops.CAST, Ops.BITCAST):
|
||||
return _evaluate(u.src[0], env, packed)
|
||||
return _evaluate(u.src[0], env)
|
||||
if u.op is Ops.CONST:
|
||||
return u.arg
|
||||
if u.op is Ops.RANGE:
|
||||
return env.get(u)
|
||||
if (slot := _outer_slot(u, packed)) is not None:
|
||||
return env.get(slot)
|
||||
if u.op in (Ops.PARAM, Ops.RANGE):
|
||||
return env.get(u.arg.slot if u.op is Ops.PARAM else u)
|
||||
if u.op is Ops.LOAD:
|
||||
table, index = _root(u.src[0].src[0]), _evaluate(u.src[0].src[1], env, packed)
|
||||
table, index = _root(u.src[0].src[0]), _evaluate(u.src[0].src[1], env)
|
||||
return struct.unpack_from(u.dtype.fmt, table.arg, index * u.dtype.itemsize)[0] if table.op is Ops.BINARY and index is not None else None
|
||||
if u.op in GroupOp.ALU and None not in (values := [_evaluate(x, env, packed) for x in u.src]):
|
||||
if u.op in GroupOp.ALU and None not in (values := [_evaluate(x, env) for x in u.src]):
|
||||
return exec_alu(u.op, u.dtype, values, *([False] if "truncate_output" in inspect.signature(exec_alu).parameters else []))
|
||||
return None
|
||||
|
||||
|
||||
def _input_pokes(submit, kernargs) -> dict[int, tuple[int, int]]:
|
||||
from tinygrad.uop.ops import Ops
|
||||
outer, packed = dict(enumerate(submit.src[1:])), _packed_args(submit.src[0])
|
||||
outer = dict(enumerate(submit.src[1:]))
|
||||
kernargs_params = {i for i, a in outer.items() if a.op is Ops.GETADDR and _root(a.src[0]) is kernargs}
|
||||
inputs = {i: _root(a.src[0]).arg.slot for i, a in outer.items() if a.op is Ops.GETADDR and _root(a.src[0]).op is Ops.PARAM}
|
||||
pokes = {}
|
||||
for call in (u for u in submit.src[0].src[0].toposort() if u.op is Ops.CALL and len(u.src) > 3):
|
||||
_, address, value, *_ = call.src[1:]
|
||||
used = list({slot for x in value.toposort() if (slot := _outer_slot(x, packed)) is not None} & inputs.keys())
|
||||
if len(used) != 1 or not kernargs_params & {_outer_slot(x, packed) for x in address.toposort()}:
|
||||
used = list({x.arg.slot for x in value.toposort() if x.op is Ops.PARAM} & inputs.keys())
|
||||
if len(used) != 1 or not kernargs_params & {x.arg.slot for x in address.toposort() if x.op is Ops.PARAM}:
|
||||
continue
|
||||
ranges = list(dict.fromkeys(x for x in (*address.toposort(), *value.toposort()) if x.op is Ops.RANGE))
|
||||
for trip in itertools.product(*[range(int(r.vmax) + 1) for r in ranges]):
|
||||
env = dict.fromkeys(outer, 0) | dict(zip(ranges, trip, strict=True))
|
||||
if (offset := _evaluate(address, env, packed)) % 8 == 0:
|
||||
pokes[offset] = (inputs[used[0]], _evaluate(value, env, packed))
|
||||
if (offset := _evaluate(address, env)) % 8 == 0:
|
||||
pokes[offset] = (inputs[used[0]], _evaluate(value, env))
|
||||
return pokes
|
||||
|
||||
|
||||
def _elf(image: bytes, rodata: int, entry: int) -> bytes:
|
||||
spans = [(".text", 0, rodata), (".rodata", rodata, len(image))] if entry < rodata else \
|
||||
[(".data", 0, rodata), (".rodata", rodata, entry), (".text", entry, len(image))]
|
||||
sections, names = [(0, 0, 0, 0, b"")], b"\0"
|
||||
for name, start, end in (span for span in spans if span[1] < span[2]):
|
||||
sections.append((len(names), 1, 6 if name == ".text" else 2, ALIGNMENT + start, image[start:end]))
|
||||
names += name.encode() + b"\0"
|
||||
sections.append((len(names), 3, 0, 0, names + b".shstrtab\0"))
|
||||
offset, data, headers = 64, b"", []
|
||||
for name, kind, flags, address, content in sections:
|
||||
headers.append(struct.pack("<IIQQQQIIQQ", name, kind, flags, address, offset, len(content), 0, 0, 1, 0))
|
||||
def _elf(image: bytes, rodata: int) -> bytes:
|
||||
names, offset, data, headers = b"\0.text\0.rodata\0.shstrtab\0", 64, b"", []
|
||||
for name, kind, address, content in [(0, 0, 0, b""), (1, 1, ALIGNMENT, image[:rodata]), (7, 1, ALIGNMENT + rodata, image[rodata:]), (15, 3, 0, names)]:
|
||||
headers.append(struct.pack("<IIQQQQIIQQ", name, kind, 2 if kind == 1 else 0, address, offset, len(content), 0, 0, 1, 0))
|
||||
data, offset = data + content, offset + len(content)
|
||||
header = struct.pack("<HHIQQQIHHHHHH", 1, 0xe0, 1, 0, 0, offset, 0, 64, 0, 0, 64, len(sections), len(sections) - 1)
|
||||
return b"\x7fELF\x02\x01\x01" + bytes(9) + header + data + b"".join(headers)
|
||||
return b"\x7fELF\x02\x01\x01" + bytes(9) + struct.pack("<HHIQQQIHHHHHH", 1, 0xe0, 1, 0, 0, offset, 0, 64, 0, 0, 64, 4, 3) + data + b"".join(headers)
|
||||
|
||||
|
||||
def convert(jit, kwargs: dict, frame: dict, target) -> tuple[dict, memoryview]:
|
||||
@@ -183,7 +155,7 @@ def convert(jit, kwargs: dict, frame: dict, target) -> tuple[dict, memoryview]:
|
||||
if image_alloc not in programs:
|
||||
image = bytes(_patched(image_alloc, topo)[0])
|
||||
rodata = next(r for r in range(0, len(image) - 24, 64) if r + struct.unpack_from("<q", image, r + 16)[0] == entry)
|
||||
programs[image_alloc] = blob.add(_elf(image, rodata, entry))
|
||||
programs[image_alloc] = blob.add(_elf(image, rodata))
|
||||
start, size = _view(arguments.src[0])
|
||||
args = []
|
||||
for slot in range(start, start + unwrap(size), 8):
|
||||
|
||||
@@ -12,8 +12,6 @@ from functools import partial
|
||||
from openpilot.common.basedir import BASEDIR
|
||||
from openpilot.sunnypilot.modeld_v2.helpers import _dtype, read_parts
|
||||
|
||||
SPLIT_FRAMES = ("big_frame", "frame")
|
||||
|
||||
|
||||
def pack_layout(shapes: dict[str, tuple], frame_size: int) -> tuple[dict[str, int], int]:
|
||||
offsets, offset = {}, 0
|
||||
@@ -41,30 +39,29 @@ def frame_part(warp_path: str, parts: dict, extras: dict) -> dict:
|
||||
(model, model_blob), (warp, warp_blob) = parts["run"], read_parts(warp_path)[0]["run"]
|
||||
states = sorted(name for name in extras["input_specs"] if f"next_{name}" in extras["metadata"]["output_shapes"])
|
||||
host = {name: shape for name, (shape, _, _) in extras["input_specs"].items() if name not in states and name != "new_img"}
|
||||
frame_specs = {s["name"]: s for s in warp["inputs"] if s["name"] in SPLIT_FRAMES}
|
||||
offsets, packed = pack_layout(host, 0 if frame_specs else next(s["numel"] for s in warp["inputs"] if s["name"] == "input_frame") // 2)
|
||||
frame_size = next(s["numel"] for s in warp["inputs"] if s["name"] == "input_frame") // 2
|
||||
offsets, packed = pack_layout(host, frame_size)
|
||||
shift, upload, u8 = len(model_blob), len(model["buffers"]) + len(warp["buffers"]), repr(_dtype("dtypes.uint8"))
|
||||
|
||||
def view(name: str, nbytes: int, dtype: str) -> dict:
|
||||
return {"buffer": upload, "offset": offsets[name], "nbytes": nbytes, "dtype": dtype}
|
||||
outputs = sorted(s["slot"] for s in model["inputs"] if s["name"] is None)
|
||||
names = sorted(["host", *states, *frame_specs])
|
||||
names = sorted(["host", *states])
|
||||
slots = {s["slot"]: names.index(s["name"]) for s in model["inputs"] if s["name"] in states} | \
|
||||
{slot: len(names) + i for i, slot in enumerate(outputs)}
|
||||
returned = warp["returns"]["tensors"][0]
|
||||
model_inputs = {s["slot"]: {"input": slots[s["slot"]]} if s["slot"] in slots else
|
||||
{k: v for k, v in returned.items() if k != "shape"} | {"buffer": returned["buffer"] + len(model["buffers"])} if s["name"] == "new_img" else
|
||||
view(s["name"], s["numel"] * _dtype(s["dtype"]).itemsize, s["dtype"]) for s in model["inputs"]}
|
||||
warp_inputs = {s["slot"]: {"input": names.index(s["name"])} if s["name"] in frame_specs else
|
||||
view("frames" if s["name"] == "input_frame" else "tfm", s["numel"] * _dtype(s["dtype"]).itemsize, s["dtype"]) for s in warp["inputs"]}
|
||||
warp_inputs = {s["slot"]: view("frames" if s["name"] == "input_frame" else "tfm", s["numel"] * _dtype(s["dtype"]).itemsize, s["dtype"])
|
||||
for s in warp["inputs"]}
|
||||
|
||||
def remap(calls: list, inputs: dict, buffers: int, lib: int) -> list:
|
||||
return [call | ({"lib": [call["lib"][0] + lib, call["lib"][1] + lib]} if "lib" in call else {}) |
|
||||
{"args": [dict(inputs[a["input"]]) if "input" in a else a | {"buffer": a["buffer"] + buffers} for a in call["args"]]} for call in calls]
|
||||
header = {"input_names": names, "returns": {"kind": "NoneType", "tensors": []}, "check": None,
|
||||
"inputs": [{"slot": names.index("host"), "name": "host", "dtype": u8, "numel": packed, "device": "NPY"}] +
|
||||
[s | {"slot": slots[s["slot"]]} for s in model["inputs"] if s["slot"] in slots] +
|
||||
[s | {"slot": names.index(name)} for name, s in frame_specs.items()],
|
||||
[s | {"slot": slots[s["slot"]]} for s in model["inputs"] if s["slot"] in slots],
|
||||
"buffers": model["buffers"] + [b | ({"init": [b["init"][0] + shift, b["init"][1] + shift]} if "init" in b else {}) for b in warp["buffers"]] +
|
||||
[{"nbytes": packed, "device": model["inputs"][0]["device"], "dtype": u8}],
|
||||
"calls": [{"kind": "copy", "args": [{"buffer": upload, "offset": 0, "nbytes": packed, "dtype": u8}, {"input": names.index("host")}]}] +
|
||||
|
||||
@@ -264,38 +264,21 @@ def _program_uop(call: dict, lib: bytes):
|
||||
parameter = UOp.param(call["globals"][compact_slot], _dtype(dtype), math.prod(shape))
|
||||
image = (shape[0], shape[1]) if len(shape) == 3 and shape[2] == 4 else None
|
||||
parameters.append(parameter.replace(arg=_fields(parameter.arg, name=name, image=image)))
|
||||
info = _fields(ProgramInfo, global_size=tuple(call["global_size"]), local_size=tuple(call["local_size"]), params=tuple(parameters),
|
||||
globals=tuple(call["globals"]), outs=tuple(call["outs"]), ins=tuple(call["ins"]), target=_fields(Target, **call["target"]))
|
||||
info = _fields(ProgramInfo, global_size=tuple(call["global_size"]), local_size=tuple(call["local_size"]), globals=tuple(call["globals"]),
|
||||
outs=tuple(call["outs"]), ins=tuple(call["ins"]), target=_fields(Target, **call["target"]))
|
||||
return UOp(Ops.PROGRAM, src=(UOp(Ops.SINK, arg=KernelInfo(name=call["name"])), UOp(Ops.LINEAR, src=tuple(parameters)),
|
||||
UOp(Ops.SOURCE, arg=""), UOp(Ops.BINARY, arg=lib)), arg=info)
|
||||
|
||||
|
||||
def _input_tensors(kwargs: dict) -> list: # in _prepare_jit_inputs' order
|
||||
from tinygrad import Tensor
|
||||
tensors = [t for _, t in sorted(kwargs.items()) if t.__class__ is Tensor]
|
||||
for value in kwargs.values():
|
||||
tensors += [t for t in (value.values() if isinstance(value, dict) else value if isinstance(value, (tuple, list)) else ())
|
||||
if t.__class__ is Tensor and not any(t is y for y in tensors)]
|
||||
return tensors
|
||||
|
||||
|
||||
class _Jit:
|
||||
def __init__(self, captured, input_names: list, buffers: list):
|
||||
self.captured, self.input_names, self.buffers = captured, input_names, buffers
|
||||
|
||||
def __call__(self, *args, **kwargs):
|
||||
import tinygrad.engine.jit as jit
|
||||
from tinygrad.uop.ops import Ops
|
||||
# same names, no variables: the binding is the tensors' own uops, without _prepare_jit_inputs' graph rewrites
|
||||
if not args and self.__dict__.get("_names") == sorted(kwargs):
|
||||
tensors = _input_tensors(kwargs)
|
||||
if all(t.uop.is_realized for t in tensors):
|
||||
return self.captured([t.uop.src[0] if t.uop.op is Ops.UNSHARD else t.uop for t in tensors], {})
|
||||
input_uops, var_vals, names, _ = jit._prepare_jit_inputs(args, kwargs)
|
||||
if names != self.input_names:
|
||||
raise ValueError(f"expected inputs {self.input_names}, got {names}")
|
||||
if not args and not var_vals:
|
||||
self._names = sorted(kwargs)
|
||||
return self.captured(input_uops, var_vals)
|
||||
|
||||
|
||||
@@ -328,7 +311,7 @@ class PlainJit(_Jit):
|
||||
calls.append(_program_uop(call, bytes(blob[slice(*call["lib"])])).call(*arguments))
|
||||
else:
|
||||
calls.append(arguments[0].store_call(arguments[1]))
|
||||
# allocate gpu buffers
|
||||
# stand-ins for JIT inputs: copy staging to GPU is decided from the input buffers, the real ones bind per call
|
||||
stand_ins = {slot: UOp.from_buffer(Buffer(_device(spec["device"], device_map), spec["numel"] * _dtype(spec["dtype"]).itemsize).ensure_allocated(),
|
||||
_dtype(spec["dtype"])) for slot, spec in inputs.items()}
|
||||
input_uops = tuple(stand_ins.get(slot, next(iter(stand_ins.values()), None)) for slot in range(max(stand_ins, default=-1) + 1))
|
||||
|
||||
@@ -11,7 +11,7 @@ import pickle
|
||||
import numpy as np
|
||||
|
||||
from openpilot.common.basedir import BASEDIR
|
||||
from openpilot.sunnypilot.modeld_v2.frame_fusion import pack_layout, SPLIT_FRAMES
|
||||
from openpilot.sunnypilot.modeld_v2.frame_fusion import pack_layout
|
||||
from openpilot.sunnypilot.modeld_v2.helpers import load_model
|
||||
from openpilot.sunnypilot.modeld_v2.compile_modeld import (POLICY_INPUTS, derive_frame_skip,
|
||||
make_split_input_queues, make_supercombo_input_queues)
|
||||
@@ -169,18 +169,16 @@ class NativeTinygradAdapter(BaseModelAdapter):
|
||||
self.warp_frame_size = self.frame_copy_size
|
||||
self._init_common()
|
||||
self.run_model = self.jits['run']
|
||||
self.split = [name for name in SPLIT_FRAMES if name in self.run_model.input_names]
|
||||
self.frame_inputs: dict[str, Tensor] = {}
|
||||
|
||||
self.input_queues = {name: Tensor(np.zeros(shape, dtype=dtype), device=self.model_device).realize()
|
||||
for name, (shape, dtype) in self.input_shapes.items() if name in self.state_pairs}
|
||||
shapes = {name: shape for name, (shape, _) in self.input_shapes.items() if name not in self.state_pairs and name != 'new_img'}
|
||||
offsets, packed = pack_layout(shapes, 0 if self.split else self.warp_frame_size)
|
||||
offsets, packed = pack_layout(shapes, self.warp_frame_size)
|
||||
self.packed_input = np.zeros(packed, dtype=np.uint8)
|
||||
self.host = Tensor(self.packed_input, device='NPY')
|
||||
self.numpy_inputs = {name: np.ndarray(shape, dtype=np.float32, buffer=self.packed_input, offset=offsets[name])
|
||||
for name, shape in ({'tfm': (2, 3, 3)} | shapes).items()}
|
||||
self.frames = self.packed_input[offsets['frames']:].reshape(2, -1)
|
||||
self.frames = self.packed_input[offsets['frames']:].reshape(2, self.warp_frame_size)
|
||||
|
||||
self.outputs = {name: Tensor(np.zeros(shape, dtype=dtype), device=device).realize() for name, (shape, dtype, device) in self.jits['output_specs'].items()}
|
||||
for name, next_name in self.state_pairs.items():
|
||||
@@ -189,29 +187,17 @@ class NativeTinygradAdapter(BaseModelAdapter):
|
||||
|
||||
def copy_frames(self, bufs):
|
||||
for i, key in enumerate(self._vision_input_names):
|
||||
if key not in bufs:
|
||||
continue
|
||||
if not self.split:
|
||||
if key in bufs:
|
||||
data = bufs[key].data if hasattr(bufs[key], 'data') else bufs[key]
|
||||
np.copyto(self.frames[i], np.frombuffer(data, dtype=np.uint8, count=self.warp_frame_size))
|
||||
elif (name := ('frame', 'big_frame')[i]) in self.split:
|
||||
if isinstance(bufs[key], np.ndarray): # cached memory: the GPU would read stale lines in place
|
||||
self.frame_inputs[name] = Tensor(np.ascontiguousarray(bufs[key][:self.warp_frame_size]), device=self.model_device).realize()
|
||||
continue
|
||||
ptr = np.frombuffer(bufs[key].data, dtype=np.uint8).ctypes.data # visionipc buffers are coherent
|
||||
if (key, ptr) not in self._blob_cache:
|
||||
self._blob_cache[(key, ptr)] = Tensor.from_blob(ptr, (self.warp_frame_size,), dtype='uint8', device=self.model_device)
|
||||
self.frame_inputs[name] = self._blob_cache[(key, ptr)]
|
||||
|
||||
def reset_warmup_buffers(self) -> None:
|
||||
self.packed_input[:] = 0
|
||||
self.frame_inputs.clear()
|
||||
self._blob_cache.clear()
|
||||
for key in self.state_pairs:
|
||||
self.input_queues[key].assign(0).realize()
|
||||
|
||||
def run(self):
|
||||
self.run_model(host=self.host, **self.input_queues, **self.frame_inputs, output_buffers=self.outputs)
|
||||
self.run_model(host=self.host, **self.input_queues, output_buffers=self.outputs)
|
||||
return self.outputs['outputs']
|
||||
|
||||
|
||||
|
||||
@@ -242,7 +242,7 @@ class ModelState(ModelStateBase):
|
||||
desired_curvature = get_curvature_from_output(model_output, curvature_plan, v_ego, lat_action_t, self.mlsim)
|
||||
else:
|
||||
desired_accel = model_output['action'][0, 1]
|
||||
desired_curvature = model_output['action'][0, 0] / (max(2.5, v_ego))**2
|
||||
desired_curvature = model_output['action'][0, 0] / (max(1.0, v_ego))**2
|
||||
if blendful is not None:
|
||||
desired_curvature += blendful(desired_curvature)
|
||||
|
||||
|
||||
@@ -1,3 +1,3 @@
|
||||
version https://git-lfs.github.com/spec/v1
|
||||
oid sha256:d776c72125156b5dd3eb59768409401e28c8a52a3a619cd3b9f02c10a156fda6
|
||||
size 3699072
|
||||
oid sha256:4b22fd6bcb3a3e8a642e6866449dd9c30c12e694ff0741cad9e34e4b00d8c5fa
|
||||
size 3698799
|
||||
|
||||
@@ -1,3 +1,3 @@
|
||||
version https://git-lfs.github.com/spec/v1
|
||||
oid sha256:a3f13ed12e087817a86afd48b2496c66c4f61a1925065b676b2945e81fabaacd
|
||||
size 7924864
|
||||
oid sha256:9a17fd6acece4fab23784ef0f3f8bba28a049e9599e5e9e5ee7b36e4b1d24a6f
|
||||
size 7924847
|
||||
|
||||
@@ -6,7 +6,6 @@ See the LICENSE.md file in the root directory for more details.
|
||||
"""
|
||||
import os
|
||||
import re
|
||||
import struct
|
||||
import sys
|
||||
import tempfile
|
||||
import unittest
|
||||
@@ -26,8 +25,8 @@ if sys.platform == "darwin" and os.environ.get("DEV", "").startswith("MOCKUSB"):
|
||||
|
||||
from openpilot.common.test import OpenpilotTestCase
|
||||
from openpilot.sunnypilot.modeld_v2 import convert_upstream_pkl
|
||||
from openpilot.sunnypilot.modeld_v2.convert_upstream_pkl import _elf, convert
|
||||
from openpilot.sunnypilot.modeld_v2.helpers import ALIGNMENT, PlainJit, load_model
|
||||
from openpilot.sunnypilot.modeld_v2.convert_upstream_pkl import convert
|
||||
from openpilot.sunnypilot.modeld_v2.helpers import PlainJit, load_model
|
||||
from openpilot.sunnypilot.modeld_v2.plain_artifact_compiler import capture_frame
|
||||
|
||||
IMG, DESIRE, STATE, OUT = (6, 8, 8), (8,), (4, 16), (1, 32)
|
||||
@@ -120,18 +119,3 @@ class TestConvertUpstreamPkl(OpenpilotTestCase):
|
||||
bundle = load_model(out, cache=False, verify=True)
|
||||
assert bundle["metadata"] == {"name": "tiny"} and bundle["input_specs"] == self.input_specs and bundle["output_specs"] == self.output_specs
|
||||
self.assert_matches_eager(bundle["run"], 6)
|
||||
|
||||
|
||||
class TestKernelElf(OpenpilotTestCase):
|
||||
def test_sections_follow_the_code(self):
|
||||
from tinygrad.runtime.support.elf import elf_loader
|
||||
code = bytes(range(1, 129))
|
||||
for rodata, entry in ((len(code), 0), (0, 256)):
|
||||
image = bytearray(code + bytes(64) if entry < rodata else bytes(256) + code)
|
||||
struct.pack_into("<q", image, rodata + 16, entry - rodata)
|
||||
loaded, sections, _ = elf_loader(_elf(bytes(image), rodata, entry))
|
||||
headers = {section.name: section.header for section in sections}
|
||||
assert bytes(loaded[ALIGNMENT:]) == bytes(image), "the uploaded program differs from the pickle's"
|
||||
assert headers[".rodata"].sh_addr == ALIGNMENT + rodata, "the loader reads the kernel descriptor at .rodata"
|
||||
text = headers[".text"]
|
||||
assert text.sh_addr <= ALIGNMENT + entry < text.sh_addr + text.sh_size, "the amd mock sizes the code from the entry to the end of .text"
|
||||
|
||||
@@ -144,21 +144,6 @@ class TestPlainArtifact(OpenpilotTestCase):
|
||||
for name in ("outputs", "next_state_feat_q"):
|
||||
assert np.array_equal(inputs["output_buffers"][name].numpy(), expected["output_buffers"][name].numpy()), name
|
||||
|
||||
split_path = os.path.join(self.tmp.name, "warp_split_tinygrad.pkl")
|
||||
with mock.patch.object(sys, "argv", ["compile_warp", "--frame", FRAME, "--warp-to", f"{MODEL_W}x{MODEL_H}", "--output", split_path, "--split-frames"]):
|
||||
compile_warp.main()
|
||||
split = load_model(path, cache=False, extend=functools.partial(frame_part, split_path))
|
||||
assert split["run"].input_names == ["big_frame", "frame", "host", "state_feat_q"]
|
||||
offsets, packed = pack_layout(HOST, 0)
|
||||
host = np.zeros(packed, np.uint8)
|
||||
for name, shape in ({"tfm": (2, 3, 3)} | HOST).items():
|
||||
np.ndarray(shape, np.float32, host, offsets[name])[...] = tfm.numpy() if name == "tfm" else inputs[name].numpy()
|
||||
out = {"outputs": Tensor.zeros(OUT).contiguous().realize(), "next_state_feat_q": Tensor.zeros(STATE).contiguous().realize()}
|
||||
split["run"](host=Tensor(host, device="NPY"), frame=Tensor(frames.numpy()[0]).realize(), big_frame=Tensor(frames.numpy()[1]).realize(),
|
||||
state_feat_q=expected["state_feat_q"], output_buffers=out)
|
||||
for name in ("outputs", "next_state_feat_q"):
|
||||
assert np.array_equal(out[name].numpy(), expected["output_buffers"][name].numpy()), name
|
||||
|
||||
def test_tinygrad_api_the_loader_reads(self):
|
||||
import tinygrad.engine.jit as jit
|
||||
from tinygrad.device import Buffer, TinyELF
|
||||
|
||||
@@ -6,7 +6,6 @@ See the LICENSE.md file in the root directory for more details.
|
||||
"""
|
||||
|
||||
import asyncio
|
||||
import json
|
||||
import os
|
||||
import re
|
||||
import threading
|
||||
@@ -29,11 +28,8 @@ DOWNLOAD_TIMEOUT = (30, 30)
|
||||
# HuggingFace throttles each connection to ~1-2 MB/s but not the connection count; 12 saturates the device link
|
||||
MAX_CONCURRENT_PIECES = 12
|
||||
PIECE_SIZE = 8 * 1024 * 1024
|
||||
PIECE_ATTEMPTS = 3 # per piece, before the transfer as a whole is retried
|
||||
PIECE_ATTEMPTS = 3
|
||||
PIECE_BACKOFF = 1.0 # seconds, growing linearly: waits 1 s then 2 s between a piece's three attempts
|
||||
DOWNLOAD_ATTEMPTS = 4 # whole-transfer attempts, each resuming from the sidecar
|
||||
RETRY_BACKOFF = 2.0 # seconds, doubling: waits 2, 4 then 8 s between the four transfer attempts
|
||||
RETRY_POLL = 0.25 # how often a backoff checks for a cancel
|
||||
TRANSIENT_HTTP_STATUS = frozenset({408, 429, 500, 502, 503, 504})
|
||||
REPORT_INTERVAL = 0.5 # seconds between progress publications
|
||||
# byte ranges count encoded bytes but iter_content yields decoded ones, so ask for an unencoded body
|
||||
@@ -53,69 +49,26 @@ def _is_transient(e: BaseException) -> bool:
|
||||
|
||||
|
||||
class _Progress:
|
||||
def __init__(self, done: set[int], done_bytes: int):
|
||||
self._bytes = done_bytes
|
||||
self._done = set(done)
|
||||
def __init__(self):
|
||||
self._bytes = 0
|
||||
self._lock = threading.Lock()
|
||||
|
||||
def add_bytes(self, n: int) -> None:
|
||||
with self._lock:
|
||||
self._bytes += n
|
||||
|
||||
def mark_done(self, index: int) -> None:
|
||||
def snapshot(self) -> int:
|
||||
with self._lock:
|
||||
self._done.add(index)
|
||||
|
||||
def snapshot(self) -> tuple[int, set[int]]:
|
||||
with self._lock:
|
||||
return self._bytes, set(self._done)
|
||||
return self._bytes
|
||||
|
||||
|
||||
def _piece_ranges(total: int, piece_size: int) -> list[tuple[int, int]]:
|
||||
return [(start, min(start + piece_size, total)) for start in range(0, total, piece_size)]
|
||||
|
||||
|
||||
def _sidecar_path(path: str) -> str:
|
||||
return f"{path}.download"
|
||||
|
||||
|
||||
def _download_in_progress(path: str) -> bool:
|
||||
return os.path.isfile(_sidecar_path(path))
|
||||
|
||||
|
||||
def _save_resume_state(path: str, layout: dict, done: set[int]) -> None:
|
||||
tmp = _sidecar_path(path) + ".tmp"
|
||||
with open(tmp, "w") as f:
|
||||
json.dump({**layout, "done": sorted(done)}, f)
|
||||
f.flush()
|
||||
os.fsync(f.fileno())
|
||||
os.replace(tmp, _sidecar_path(path)) # a power loss leaves the old sidecar or the new one, never a torn file
|
||||
dir_fd = os.open(os.path.dirname(path) or ".", os.O_RDONLY)
|
||||
try:
|
||||
os.fsync(dir_fd)
|
||||
finally:
|
||||
os.close(dir_fd)
|
||||
|
||||
|
||||
def _remove_download_state(path: str) -> None:
|
||||
for stale in (_sidecar_path(path), _sidecar_path(path) + ".tmp"):
|
||||
if os.path.isfile(stale):
|
||||
os.remove(stale)
|
||||
|
||||
|
||||
def _prepare_target(path: str, layout: dict) -> set[int]:
|
||||
total = layout["total"]
|
||||
try:
|
||||
with open(_sidecar_path(path)) as f:
|
||||
state = json.load(f)
|
||||
if isinstance(state, dict) and all(state.get(k) == v for k, v in layout.items()) and os.path.getsize(path) == total:
|
||||
return {i for i in state.get("done", []) if isinstance(i, int)}
|
||||
except (OSError, ValueError, TypeError): # missing, unreadable or malformed sidecar: start over
|
||||
pass
|
||||
_save_resume_state(path, layout, set())
|
||||
def _prepare_target(path: str, total: int) -> None:
|
||||
with open(path, "wb") as f:
|
||||
f.truncate(total)
|
||||
return set()
|
||||
|
||||
|
||||
def _content_range_total(response: requests.Response) -> int | None:
|
||||
@@ -157,9 +110,6 @@ def _fetch_piece(url: str, path: str, index: int, start: int, end: int | None, p
|
||||
f.truncate() # unknown length: the body defines the file size
|
||||
if end is not None and written != end - start:
|
||||
raise requests.exceptions.ConnectionError(f"short read: {written} of {end - start} bytes")
|
||||
f.flush()
|
||||
os.fsync(f.fileno()) # before it is recorded, so a power loss never leaves the sidecar naming unsynced bytes
|
||||
progress.mark_done(index)
|
||||
return
|
||||
except requests.RequestException as e:
|
||||
progress.add_bytes(-written) # the piece restarts from scratch
|
||||
@@ -170,11 +120,6 @@ def _fetch_piece(url: str, path: str, index: int, start: int, end: int | None, p
|
||||
raise DownloadCancelled("Download cancelled") from None
|
||||
|
||||
|
||||
def _remove_file(path: str) -> None:
|
||||
if os.path.isfile(path):
|
||||
os.remove(path)
|
||||
|
||||
|
||||
class ModelManagerSP:
|
||||
"""Manages model downloads and status reporting"""
|
||||
|
||||
@@ -241,12 +186,11 @@ class ModelManagerSP:
|
||||
else:
|
||||
eta = self._calculate_eta(artifact.fileName, progress)
|
||||
self._set_progress(artifact, custom.ModelManagerSP.DownloadStatus.downloading, progress, eta, speed)
|
||||
artifact.downloadProgress.resuming = False # shown only for the first report, when the bar jumps to the resumed point
|
||||
|
||||
async def _report_until_done(self, tasks: list[asyncio.Future], artifact, progress: _Progress, path: str, layout: dict,
|
||||
started: tuple[int, set[int], float]) -> None:
|
||||
async def _report_until_done(self, tasks: list[asyncio.Future], artifact, progress: _Progress, total: int,
|
||||
started: tuple[int, float]) -> None:
|
||||
pending = set(tasks)
|
||||
last_bytes, saved, last_time = started
|
||||
last_bytes, last_time = started
|
||||
speed = 0.0
|
||||
while pending:
|
||||
done, pending = await asyncio.wait(pending, timeout=REPORT_INTERVAL)
|
||||
@@ -255,17 +199,14 @@ class ModelManagerSP:
|
||||
if self._download_interrupted():
|
||||
raise DownloadCancelled("Download cancelled")
|
||||
now = time.monotonic()
|
||||
done_bytes, done_pieces = progress.snapshot()
|
||||
if done_pieces != saved:
|
||||
_save_resume_state(path, layout, done_pieces)
|
||||
saved = done_pieces
|
||||
done_bytes = progress.snapshot()
|
||||
if (dt := now - last_time) > 0:
|
||||
instant = max(0.0, (done_bytes - last_bytes) / dt) # bytes/s this tick; clamped since retried pieces can lower the byte count
|
||||
speed = instant if speed == 0 else SPEED_SMOOTHING * speed + (1 - SPEED_SMOOTHING) * instant
|
||||
if speed < SPEED_FLOOR: # a stall decays toward 0 without reaching it, and a near-0 speed overflows the eta
|
||||
speed = 0.0
|
||||
last_time, last_bytes = now, done_bytes
|
||||
self._publish_progress(artifact, done_bytes, layout["total"], speed)
|
||||
self._publish_progress(artifact, done_bytes, total, speed)
|
||||
|
||||
async def _download_file(self, url: str, path: str, artifact) -> None:
|
||||
self._download_start_times[artifact.fileName] = time.monotonic()
|
||||
@@ -273,18 +214,16 @@ class ModelManagerSP:
|
||||
|
||||
total, ranged = await loop.run_in_executor(None, _probe_size, url)
|
||||
pieces: list[tuple[int, int | None]] = list(_piece_ranges(total, PIECE_SIZE)) if ranged else [(0, None)]
|
||||
layout = {"total": total, "piece_size": PIECE_SIZE, "ranged": ranged, "sha256": artifact.downloadUri.sha256}
|
||||
done = await loop.run_in_executor(None, _prepare_target, path, layout)
|
||||
resumed_bytes = sum(end - start for i, (start, end) in enumerate(pieces) if i in done and end is not None)
|
||||
progress = _Progress(done, resumed_bytes) # the bar and eta start from the resumed pieces
|
||||
await loop.run_in_executor(None, _prepare_target, path, total)
|
||||
progress = _Progress()
|
||||
cancel = threading.Event()
|
||||
started = *progress.snapshot(), time.monotonic() # before any worker can land bytes
|
||||
started = progress.snapshot(), time.monotonic() # before any worker can land bytes
|
||||
|
||||
with ThreadPoolExecutor(max_workers=MAX_CONCURRENT_PIECES) as pool:
|
||||
tasks = [loop.run_in_executor(pool, _fetch_piece, url, path, i, start, end, progress, cancel, self._block_size)
|
||||
for i, (start, end) in enumerate(pieces) if i not in done]
|
||||
for i, (start, end) in enumerate(pieces)]
|
||||
try:
|
||||
await self._report_until_done(tasks, artifact, progress, path, layout, started)
|
||||
await self._report_until_done(tasks, artifact, progress, total, started)
|
||||
except BaseException as e:
|
||||
cancel.set()
|
||||
if isinstance(e, DownloadCancelled):
|
||||
@@ -293,32 +232,9 @@ class ModelManagerSP:
|
||||
for task in tasks:
|
||||
task.cancel() # drops queued pieces; running ones see the event at their next block
|
||||
await asyncio.gather(*tasks, return_exceptions=True)
|
||||
await loop.run_in_executor(None, pool.shutdown) # let running pieces stop so none lands after the save
|
||||
_save_resume_state(path, layout, progress.snapshot()[1])
|
||||
raise
|
||||
|
||||
_remove_download_state(path)
|
||||
artifact.downloadProgress.resuming = False
|
||||
del self._download_start_times[artifact.fileName]
|
||||
|
||||
async def _download_with_retries(self, url: str, path: str, artifact) -> None:
|
||||
for attempt in range(DOWNLOAD_ATTEMPTS):
|
||||
try:
|
||||
await self._download_file(url, path, artifact)
|
||||
return
|
||||
except Exception as e:
|
||||
if not _is_transient(e) or attempt == DOWNLOAD_ATTEMPTS - 1:
|
||||
raise
|
||||
delay = RETRY_BACKOFF * 2 ** attempt
|
||||
cloudlog.warning(f"Retrying {artifact.fileName} in {delay:g}s after {type(e).__name__}: {e}")
|
||||
progress = artifact.downloadProgress
|
||||
self._set_progress(artifact, progress.status, progress.progress, progress.eta, 0.0) # stalled: no speed
|
||||
deadline = time.monotonic() + delay
|
||||
while (remaining := deadline - time.monotonic()) > 0:
|
||||
if self._download_interrupted():
|
||||
raise DownloadCancelled("Download cancelled") from e
|
||||
await asyncio.sleep(min(RETRY_POLL, remaining))
|
||||
|
||||
async def _process_artifact(self, artifact, destination_path: str) -> None:
|
||||
if not artifact.downloadUri.uri:
|
||||
return None
|
||||
@@ -330,39 +246,37 @@ class ModelManagerSP:
|
||||
filename = artifact.fileName
|
||||
full_path = os.path.join(destination_path, filename)
|
||||
status = custom.ModelManagerSP.DownloadStatus
|
||||
artifact.downloadProgress.resuming = False
|
||||
artifact.downloadProgress.canceling = False
|
||||
|
||||
try:
|
||||
if _download_in_progress(full_path):
|
||||
cloudlog.info(f"Resuming interrupted download of {filename}")
|
||||
artifact.downloadProgress.resuming = True
|
||||
else:
|
||||
self._set_progress(artifact, status.verifying, 0)
|
||||
if await verify_file(full_path, expected_hash):
|
||||
self._set_progress(artifact, status.cached, 100)
|
||||
return
|
||||
self._set_progress(artifact, status.verifying, 0)
|
||||
if await verify_file(full_path, expected_hash):
|
||||
self._set_progress(artifact, status.cached, 100)
|
||||
return
|
||||
|
||||
self._set_progress(artifact, status.downloading, 0)
|
||||
await self._download_with_retries(url, full_path, artifact)
|
||||
await self._download_file(url, full_path, artifact)
|
||||
self._set_progress(artifact, status.verifying, 99)
|
||||
if not await verify_file(full_path, expected_hash):
|
||||
_remove_file(full_path)
|
||||
_remove_download_state(full_path)
|
||||
raise ValueError(f"Hash validation failed for {filename}")
|
||||
|
||||
self._set_progress(artifact, status.downloaded, 100)
|
||||
|
||||
except DownloadCancelled: # not a failure: the request handler clears the row
|
||||
except DownloadCancelled:
|
||||
self._download_start_times.pop(artifact.fileName, None)
|
||||
artifact.downloadProgress.status = status.failed
|
||||
artifact.downloadProgress.eta = 0
|
||||
artifact.downloadProgress.speed = 0
|
||||
self._sync_artifact_progress(artifact)
|
||||
if self.selected_bundle:
|
||||
self.selected_bundle.status = status.failed
|
||||
self._report_status()
|
||||
raise
|
||||
|
||||
except Exception as e:
|
||||
cloudlog.error(f"Error downloading {filename}: {str(e)}")
|
||||
if os.path.isfile(full_path): # noqa: ASYNC240
|
||||
os.remove(full_path)
|
||||
artifact.downloadProgress.status = status.failed
|
||||
artifact.downloadProgress.eta = 0
|
||||
artifact.downloadProgress.speed = 0
|
||||
@@ -419,8 +333,6 @@ class ModelManagerSP:
|
||||
self.params.put(ACTIVE_BUNDLE_KEYS[source], model_bundle.to_dict(), block=True)
|
||||
self.active_bundle = get_active_bundle(self.params, chestnut=self.chestnut_present)
|
||||
|
||||
except DownloadCancelled:
|
||||
raise # not a failure
|
||||
except Exception:
|
||||
if self.selected_bundle is not None:
|
||||
self.selected_bundle.status = custom.ModelManagerSP.DownloadStatus.failed
|
||||
@@ -448,14 +360,11 @@ class ModelManagerSP:
|
||||
self._download_ref = ref_to_download
|
||||
try:
|
||||
self.download(model_to_download, Paths.model_root(), source)
|
||||
except DownloadCancelled:
|
||||
self.selected_bundle = None
|
||||
except Exception as e:
|
||||
cloudlog.exception(e) # a failure stays on the row until the next request
|
||||
else:
|
||||
self.selected_bundle = None
|
||||
cloudlog.exception(e)
|
||||
finally:
|
||||
self._release_download_ref()
|
||||
self.selected_bundle = None
|
||||
|
||||
def main_thread(self) -> None:
|
||||
"""Main thread for model management"""
|
||||
|
||||
@@ -11,13 +11,11 @@ import contextlib
|
||||
import dataclasses
|
||||
import hashlib
|
||||
import http.server
|
||||
import json
|
||||
import math
|
||||
import os
|
||||
import random
|
||||
import re
|
||||
import requests
|
||||
import stat
|
||||
import tempfile
|
||||
import threading
|
||||
import time
|
||||
@@ -57,8 +55,6 @@ class ServerState:
|
||||
fail_ranges: dict[tuple[int, int], int] = dataclasses.field(default_factory=dict) # range -> HTTP status, every time
|
||||
fail_times: dict[tuple[int, int], int] = dataclasses.field(default_factory=dict) # range -> how many more requests 503
|
||||
short_once: set[tuple[int, int]] = dataclasses.field(default_factory=set) # ranges whose first answer is cut short
|
||||
fail_after_others: bool = False # hold each failure until every other piece has been sent
|
||||
sent: set[tuple[int, int]] = dataclasses.field(default_factory=set) # ranges sent in full
|
||||
stall_ranges: set[tuple[int, int]] = dataclasses.field(default_factory=set)
|
||||
stall_event: threading.Event | None = None
|
||||
stall_released_by_event: bool | None = None
|
||||
@@ -92,10 +88,6 @@ class DownloadHandler(http.server.BaseHTTPRequestHandler):
|
||||
state.fail_times[rng] -= 1
|
||||
status = 503
|
||||
if status is not None:
|
||||
others = {piece_range(i) for i in range(NUM_PIECES)} - {rng}
|
||||
deadline = time.monotonic() + 5
|
||||
while state.fail_after_others and not others <= state.sent and time.monotonic() < deadline:
|
||||
time.sleep(0.01)
|
||||
self.send_response(status)
|
||||
self.end_headers()
|
||||
return
|
||||
@@ -123,8 +115,6 @@ class DownloadHandler(http.server.BaseHTTPRequestHandler):
|
||||
self.wfile.write(payload[100:])
|
||||
else:
|
||||
self.wfile.write(payload)
|
||||
if rng is not None:
|
||||
state.sent.add(rng)
|
||||
|
||||
|
||||
@contextlib.contextmanager
|
||||
@@ -143,10 +133,7 @@ def threaded_server(handler):
|
||||
class InlineExecutor(concurrent.futures.Executor):
|
||||
def submit(self, fn, /, *args, **kwargs):
|
||||
future: concurrent.futures.Future = concurrent.futures.Future()
|
||||
try:
|
||||
future.set_result(fn(*args, **kwargs)) # the piece lands before the reporter starts
|
||||
except BaseException as e:
|
||||
future.set_exception(e)
|
||||
future.set_result(fn(*args, **kwargs)) # the piece lands before the reporter starts
|
||||
return future
|
||||
|
||||
|
||||
@@ -160,12 +147,11 @@ class ManagerDownloadTestBase(OpenpilotTestCase):
|
||||
self.addCleanup(self._tmp.cleanup)
|
||||
self.dest = self._tmp.name
|
||||
|
||||
for name, value in (('PIECE_SIZE', PIECE_SIZE), ('REPORT_INTERVAL', 0.05), ('PIECE_BACKOFF', 0.01), ('SPEED_FLOOR', 100), ('RETRY_BACKOFF', 0.01)):
|
||||
for name, value in (('PIECE_SIZE', PIECE_SIZE), ('REPORT_INTERVAL', 0.05), ('PIECE_BACKOFF', 0.01), ('SPEED_FLOOR', 100)):
|
||||
self.enterContext(mock.patch.object(manager_module, name, value))
|
||||
|
||||
self.reported: list[float] = []
|
||||
self.reported_statuses: list[int] = []
|
||||
self.reported_resuming: list[bool] = []
|
||||
|
||||
self.manager = ModelManagerSP.__new__(ModelManagerSP)
|
||||
self.params = self.manager.params = mock.MagicMock()
|
||||
@@ -186,7 +172,6 @@ class ManagerDownloadTestBase(OpenpilotTestCase):
|
||||
self.reported.append(float(artifact.downloadProgress.progress))
|
||||
status = artifact.downloadProgress.status
|
||||
self.reported_statuses.append(getattr(status, 'raw', status)) # .raw: _DynamicEnum equals its int but does not hash like it
|
||||
self.reported_resuming.append(bool(artifact.downloadProgress.resuming))
|
||||
|
||||
def make_artifact(self):
|
||||
bundle = custom.ModelManagerSP.ModelBundle.new_message()
|
||||
@@ -203,24 +188,6 @@ class ManagerDownloadTestBase(OpenpilotTestCase):
|
||||
def path(self) -> str:
|
||||
return os.path.join(self.dest, FILE_NAME)
|
||||
|
||||
@property
|
||||
def sidecar(self) -> str:
|
||||
return manager_module._sidecar_path(self.path)
|
||||
|
||||
def resume_state(self) -> dict:
|
||||
with open(self.sidecar) as f:
|
||||
return json.load(f)
|
||||
|
||||
def write_resume_state(self, done: list[int], total: int = len(WHOLE_BODY), file_size: int | None = None,
|
||||
file_sha256: str = sha256(WHOLE_BODY)) -> None:
|
||||
with open(self.path, 'wb') as f:
|
||||
f.truncate(len(WHOLE_BODY) if file_size is None else file_size)
|
||||
for i in done:
|
||||
f.seek(i * PIECE_SIZE)
|
||||
f.write(WHOLE_BODY[i * PIECE_SIZE:(i + 1) * PIECE_SIZE])
|
||||
with open(self.sidecar, 'w') as f:
|
||||
json.dump({"total": total, "piece_size": PIECE_SIZE, "ranged": True, "sha256": file_sha256, "done": done}, f)
|
||||
|
||||
def piece_requests(self) -> list[tuple[int, int]]:
|
||||
return [r for r in self.server.request_ranges if r is not None and r != (0, 0)]
|
||||
|
||||
@@ -242,17 +209,6 @@ class ManagerDownloadTestBase(OpenpilotTestCase):
|
||||
def download_bundle(self, source: str):
|
||||
asyncio.run(self.manager._download_bundle(self._bundle, self.dest, source))
|
||||
|
||||
def request_download(self) -> dict:
|
||||
self.make_artifact()
|
||||
self._bundle.ref = "ref"
|
||||
self._bundle.minimumSelectorVersion = helpers.REQUIRED_JSON_VERSION
|
||||
self.manager.source_models = {"qcom": [self._bundle], "chestnut": []}
|
||||
self.manager.params, store = self._make_params_with_store()
|
||||
store["ModelManager_DownloadRef"] = "ref"
|
||||
with mock.patch.object(manager_module.Paths, 'model_root', return_value=self.dest):
|
||||
self.manager._process_download_requests()
|
||||
return store
|
||||
|
||||
def read_path(self) -> bytes:
|
||||
with open(self.path, 'rb') as f:
|
||||
return f.read()
|
||||
@@ -269,7 +225,6 @@ class ManagerDownloadTestBase(OpenpilotTestCase):
|
||||
|
||||
params.get.side_effect = get
|
||||
params.put.side_effect = put
|
||||
params.remove.side_effect = lambda key: put(key, None)
|
||||
return params, store
|
||||
|
||||
|
||||
@@ -283,7 +238,6 @@ class TestManagerDownload(ManagerDownloadTestBase):
|
||||
assert max(self.reported) <= 99.0, "progress holds below 100 until the hash check"
|
||||
assert artifact.fileName not in self.manager._download_start_times
|
||||
assert artifact.downloadProgress.speed > 0, "a running download shows its speed"
|
||||
assert not os.path.exists(self.sidecar), "the resume state goes once every piece has landed"
|
||||
|
||||
def test_speed_counts_bytes_landed_before_the_first_report(self):
|
||||
self.enterContext(mock.patch.object(manager_module, 'ThreadPoolExecutor', lambda max_workers: InlineExecutor()))
|
||||
@@ -342,42 +296,9 @@ class TestManagerDownload(ManagerDownloadTestBase):
|
||||
|
||||
def test_permanent_failure_is_not_retried(self):
|
||||
self.server.fail_ranges = {piece_range(1): 404}
|
||||
self.enterContext(mock.patch.object(manager_module, 'ThreadPoolExecutor', lambda max_workers: InlineExecutor()))
|
||||
with self.assertRaises(requests.exceptions.HTTPError):
|
||||
self.process_artifact()
|
||||
self.download_file()
|
||||
assert self.piece_requests().count(piece_range(1)) == 1
|
||||
assert os.path.isfile(self.path)
|
||||
assert self.resume_state()["done"] == [i for i in range(NUM_PIECES) if i != 1]
|
||||
|
||||
def test_failed_transfer_resumes_on_retry(self):
|
||||
self.server.fail_times = {piece_range(1): manager_module.PIECE_ATTEMPTS} # the first transfer gives up on piece 1
|
||||
self.server.fail_after_others = True # by then every other piece is saved
|
||||
self.process_artifact()
|
||||
assert self.read_path() == WHOLE_BODY
|
||||
assert self.piece_requests().count(piece_range(1)) == manager_module.PIECE_ATTEMPTS + 1, "the retry fetched piece 1"
|
||||
others = [i for i in range(NUM_PIECES) if i != 1]
|
||||
assert all(self.piece_requests().count(piece_range(i)) == 1 for i in others), "the retry skipped every saved piece"
|
||||
|
||||
def test_lasting_transient_failure_gives_up(self):
|
||||
self.server.fail_ranges = {piece_range(1): 503}
|
||||
with self.assertRaises(requests.exceptions.HTTPError):
|
||||
self.process_artifact()
|
||||
assert self.piece_requests().count(piece_range(1)) == manager_module.PIECE_ATTEMPTS * manager_module.DOWNLOAD_ATTEMPTS
|
||||
|
||||
def test_cancel_ends_retry_backoff(self):
|
||||
self.enterContext(mock.patch.object(manager_module, 'RETRY_BACKOFF', 30.0))
|
||||
self.manager._download_ref = b"ref"
|
||||
|
||||
def fail_then_cancel(*args):
|
||||
self.params.get.return_value = None # the user cancels while the manager waits to retry
|
||||
raise requests.exceptions.ConnectionError("dropped")
|
||||
transfer = self.enterContext(mock.patch.object(self.manager, '_download_file', side_effect=fail_then_cancel))
|
||||
started = time.monotonic()
|
||||
with self.assertRaises(DownloadCancelled):
|
||||
self.process_artifact()
|
||||
assert time.monotonic() - started < 5, "the wait must end at the cancel, not after 30 s"
|
||||
assert transfer.call_count == 1, "no second transfer after a cancel"
|
||||
assert self.artifact.downloadProgress.status != custom.ModelManagerSP.DownloadStatus.failed
|
||||
|
||||
def test_removing_download_ref_cancels(self):
|
||||
stall_event = self.stall(3)
|
||||
@@ -395,51 +316,6 @@ class TestManagerDownload(ManagerDownloadTestBase):
|
||||
assert self.server.stall_released_by_event is True, "the cancel must have caught the piece mid-transfer"
|
||||
end = piece_range(3)[1] + 1
|
||||
assert self.read_path()[end - 100:end] == bytes(100), "the cancelled piece must stop before its last bytes land"
|
||||
done = self.resume_state()["done"]
|
||||
assert done and 3 not in done, "a cancel records the finished pieces, and only those, for resume"
|
||||
|
||||
def test_resume_skips_complete_pieces(self):
|
||||
self.write_resume_state(done=[0, 4])
|
||||
self.download_file()
|
||||
assert piece_range(0) not in self.piece_requests() and piece_range(4) not in self.piece_requests()
|
||||
assert self.read_path() == WHOLE_BODY
|
||||
assert min(self.reported) >= 2 * PIECE_SIZE / len(WHOLE_BODY) * 100 - 1, "progress must not restart below the resumed share"
|
||||
|
||||
def test_unusable_resume_state_starts_over(self):
|
||||
def malformed(content: bytes):
|
||||
self.write_resume_state(done=[0])
|
||||
with open(self.sidecar, 'wb') as f:
|
||||
f.write(content)
|
||||
|
||||
layout = {"total": len(WHOLE_BODY), "piece_size": PIECE_SIZE, "ranged": True, "sha256": sha256(WHOLE_BODY)}
|
||||
cases = {
|
||||
"other size": lambda: self.write_resume_state(done=[0], total=len(WHOLE_BODY) + 1),
|
||||
"other bytes": lambda: self.write_resume_state(done=[0], file_sha256="0" * 64),
|
||||
"truncated file": lambda: self.write_resume_state(done=[0], file_size=PIECE_SIZE),
|
||||
"not an object": lambda: malformed(b'[]'),
|
||||
"cut-off json": lambda: malformed(b'{"total": '),
|
||||
"done not a list": lambda: malformed(json.dumps({**layout, "done": 3}).encode()),
|
||||
"done not ints": lambda: malformed(json.dumps({**layout, "done": ["0", None]}).encode()),
|
||||
}
|
||||
for name, write_state in cases.items():
|
||||
with self.subTest(name):
|
||||
self.server.request_ranges = []
|
||||
write_state()
|
||||
self.download_file()
|
||||
assert sorted(self.piece_requests()) == [piece_range(i) for i in range(NUM_PIECES)], "every piece is fetched again"
|
||||
assert self.read_path() == WHOLE_BODY
|
||||
|
||||
def test_pieces_and_resume_state_are_synced_to_disk(self):
|
||||
real_fsync, synced = os.fsync, []
|
||||
|
||||
def record_sync(fd):
|
||||
st = os.fstat(fd)
|
||||
synced.append("dir" if stat.S_ISDIR(st.st_mode) else "model" if os.path.exists(self.path) and st.st_ino == os.stat(self.path).st_ino else "sidecar")
|
||||
real_fsync(fd)
|
||||
with mock.patch.object(manager_module.os, 'fsync', side_effect=record_sync):
|
||||
self.download_file()
|
||||
assert synced.count("model") == NUM_PIECES, f"expected one fsync per piece, got {synced.count('model')}"
|
||||
assert "sidecar" in synced and "dir" in synced, "sidecar file and directory must be fsync'd"
|
||||
|
||||
def test_replaced_download_ref_queues_instead_of_canceling(self):
|
||||
self.params.get.return_value = b"other-ref"
|
||||
@@ -469,26 +345,9 @@ class TestProcessArtifact(ManagerDownloadTestBase):
|
||||
self.server.corrupt = True
|
||||
with self.assertRaises(ValueError):
|
||||
self.process_artifact()
|
||||
assert not os.path.isfile(self.path) and not os.path.exists(self.sidecar), "a corrupt file must not be resumed from"
|
||||
assert not os.path.isfile(self.path)
|
||||
assert self.artifact.downloadProgress.status == custom.ModelManagerSP.DownloadStatus.failed
|
||||
|
||||
def test_interrupted_download_resumes_without_reverifying(self):
|
||||
self.write_resume_state(done=[0, 1, 2])
|
||||
artifact = self.process_artifact()
|
||||
ds = custom.ModelManagerSP.DownloadStatus
|
||||
assert self.reported_statuses[0] == ds.downloading, "an in-progress download is not verified first"
|
||||
assert piece_range(0) not in self.piece_requests()
|
||||
assert artifact.downloadProgress.status == ds.downloaded
|
||||
assert any(self.reported_resuming), "a resumed download is flagged resuming until its first progress report"
|
||||
assert not artifact.downloadProgress.resuming
|
||||
|
||||
def test_resume_with_every_piece_done_clears_resuming(self):
|
||||
self.write_resume_state(done=list(range(NUM_PIECES)))
|
||||
artifact = self.process_artifact()
|
||||
assert self.piece_requests() == []
|
||||
assert artifact.downloadProgress.status == custom.ModelManagerSP.DownloadStatus.downloaded
|
||||
assert not artifact.downloadProgress.resuming
|
||||
|
||||
def test_probe_failure_moves_past_verifying(self):
|
||||
self.server.fail_ranges = {(0, 0): 404} # the size probe
|
||||
with self.assertRaises(requests.exceptions.HTTPError):
|
||||
@@ -515,23 +374,6 @@ class TestProcessArtifact(ManagerDownloadTestBase):
|
||||
self.download_bundle("qcom")
|
||||
assert ACTIVE_BUNDLE_KEYS["qcom"] not in store
|
||||
assert os.path.isfile(self.path), "cancel must not delete a cached model"
|
||||
assert self.manager.selected_bundle.status != custom.ModelManagerSP.DownloadStatus.failed, "a cancel is not a failure"
|
||||
|
||||
def test_failed_download_stays_on_the_row(self):
|
||||
self.server.fail_ranges = {piece_range(1): 404}
|
||||
store = self.request_download()
|
||||
selected = self.manager.selected_bundle
|
||||
assert selected is not None and selected.status == custom.ModelManagerSP.DownloadStatus.failed, "a failure stays on the row"
|
||||
assert store["ModelManager_DownloadRef"] is None, "the failed request is released"
|
||||
|
||||
def test_finished_download_clears_the_row(self):
|
||||
self.request_download()
|
||||
assert self.manager.selected_bundle is None
|
||||
|
||||
def test_cancelled_download_clears_the_row(self):
|
||||
self.pm.send.side_effect = lambda *args: self.manager.params.remove("ModelManager_DownloadRef") # cancel at the first report
|
||||
self.request_download()
|
||||
assert self.manager.selected_bundle is None
|
||||
|
||||
def test_download_ref_is_released_only_if_unchanged(self):
|
||||
for current, removed in ((b"new-ref", False), (b"ref", True)):
|
||||
|
||||
@@ -1,67 +0,0 @@
|
||||
"""
|
||||
Copyright (c) 2021-, sunnypilot, and a number of other contributors.
|
||||
|
||||
This file is part of sunnypilot and is licensed under the MIT License.
|
||||
See the LICENSE.md file in the root directory for more details.
|
||||
"""
|
||||
import numpy as np
|
||||
|
||||
from openpilot.cereal import log
|
||||
from openpilot.common.params import Params
|
||||
from openpilot.common.realtime import DT_MDL
|
||||
from openpilot.sunnypilot import PARAMS_UPDATE_PERIOD
|
||||
|
||||
ACCEL_BOOST_MIN_SPEED = 1.0
|
||||
ACCEL_BOOST_MAX = (0.4, 0.25, 0.1) # aggressive, standard, relaxed, m/s^2
|
||||
ACCEL_BOOST_BUILD, ACCEL_BOOST_RELEASE = 0.2, 1.0 # m/s^3
|
||||
BRAKE_PROB = (0.1, 0.3) # the model's brake press in 4 s: it wants to slow
|
||||
GAS_PROB = (0.6, 0.8) # predicts a human gas press at this hypothesis at about 23%
|
||||
CURVE_LAT_ACCEL = (1.5, 2.5)
|
||||
|
||||
|
||||
def get_boost_max(personality=log.LongitudinalPersonality.standard):
|
||||
if personality == log.LongitudinalPersonality.relaxed:
|
||||
return ACCEL_BOOST_MAX[2]
|
||||
elif personality == log.LongitudinalPersonality.standard:
|
||||
return ACCEL_BOOST_MAX[1]
|
||||
elif personality == log.LongitudinalPersonality.aggressive:
|
||||
return ACCEL_BOOST_MAX[0]
|
||||
else:
|
||||
raise NotImplementedError("Longitudinal personality not supported")
|
||||
|
||||
|
||||
class AccelBoostSP:
|
||||
def __init__(self):
|
||||
self.params = Params()
|
||||
self.frame = -1
|
||||
self.enabled = False
|
||||
self.boost = 0.0
|
||||
self.total_boost = 0.0
|
||||
|
||||
def update_params(self):
|
||||
self.frame += 1
|
||||
if self.frame % int(PARAMS_UPDATE_PERIOD / DT_MDL) != 0:
|
||||
return False
|
||||
enabled = self.params.get_bool("AccelBoostSP")
|
||||
if enabled == self.enabled:
|
||||
return False
|
||||
self.enabled = enabled
|
||||
self.boost = self.total_boost = 0.0
|
||||
return True
|
||||
|
||||
def update(self, sm, output_a_target_e2e, output_a_target_mpc, a_cruise):
|
||||
model = sm['modelV2']
|
||||
brake_probs = model.meta.disengagePredictions.brakePressProbs
|
||||
gas_probs = model.meta.disengagePredictions.gasPressProbs
|
||||
if not sm['selfdriveState'].enabled or len(brake_probs) < 2 or len(gas_probs) < 2:
|
||||
self.boost = self.total_boost = 0.0
|
||||
return output_a_target_e2e
|
||||
|
||||
want = max(float(np.interp(brake_probs[1], BRAKE_PROB, [1.0, 0.0])), float(np.interp(gas_probs[1], GAS_PROB, [0.0, 1.0])))
|
||||
want = 0.0 if model.action.shouldStop else want
|
||||
want *= float(np.interp(abs(model.action.desiredCurvature) * sm['carState'].vEgo ** 2, CURVE_LAT_ACCEL, [1.0, 0.0]))
|
||||
target = want * get_boost_max(sm['selfdriveState'].personality)
|
||||
self.boost = float(np.clip(target, self.boost - ACCEL_BOOST_RELEASE * DT_MDL, self.boost + ACCEL_BOOST_BUILD * DT_MDL))
|
||||
speed_scale = np.interp(sm['carState'].vEgo, [ACCEL_BOOST_MIN_SPEED, 2 * ACCEL_BOOST_MIN_SPEED], [0.0, 1.0])
|
||||
self.total_boost = self.boost * speed_scale * np.interp(output_a_target_e2e, [-0.7, -0.2, 5.0], [0.0, 1.0, 1.0], right=0.0)
|
||||
return output_a_target_e2e + self.total_boost
|
||||
@@ -9,8 +9,6 @@ from openpilot.cereal import messaging, custom
|
||||
from opendbc.car import structs
|
||||
from openpilot.common.constants import CV
|
||||
from openpilot.selfdrive.car.cruise import V_CRUISE_MAX
|
||||
from openpilot.selfdrive.controls.lib.accel_boost import AccelBoost
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.accel_boost import AccelBoostSP
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.e2e_alerts_helper import E2EAlertsHelper
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.smart_cruise_control.smart_cruise_control import SmartCruiseControl
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit.speed_limit_assist import SpeedLimitAssist
|
||||
@@ -31,8 +29,6 @@ class LongitudinalPlannerSP:
|
||||
self.generation = int(model_bundle.generation) if (model_bundle := get_active_bundle()) else None
|
||||
self.source = LongitudinalPlanSource.cruise
|
||||
self.e2e_alerts_helper = E2EAlertsHelper()
|
||||
self.accel_boost_sp = AccelBoostSP()
|
||||
self.openpilot_longitudinal = CP.openpilotLongitudinalControl
|
||||
|
||||
self.output_v_target = 0.
|
||||
self.output_a_target = 0.
|
||||
@@ -68,9 +64,6 @@ class LongitudinalPlannerSP:
|
||||
return self.output_v_target, self.output_a_target
|
||||
|
||||
def update(self, sm: messaging.SubMaster) -> None:
|
||||
if self.accel_boost_sp.update_params():
|
||||
self.accel_boost = self.accel_boost_sp if self.accel_boost_sp.enabled and self.openpilot_longitudinal else AccelBoost()
|
||||
|
||||
self.events_sp.clear()
|
||||
self.e2e_alerts_helper.update(sm, self.events_sp)
|
||||
|
||||
|
||||
@@ -1,81 +0,0 @@
|
||||
"""
|
||||
Copyright (c) 2021-, sunnypilot, and a number of other contributors.
|
||||
|
||||
This file is part of sunnypilot and is licensed under the MIT License.
|
||||
See the LICENSE.md file in the root directory for more details.
|
||||
"""
|
||||
from types import SimpleNamespace
|
||||
|
||||
from openpilot.common.parameterized import parameterized
|
||||
from openpilot.common.params import Params
|
||||
from openpilot.common.realtime import DT_MDL
|
||||
from openpilot.common.test import OpenpilotTestCase
|
||||
|
||||
from openpilot.sunnypilot import PARAMS_UPDATE_PERIOD
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.accel_boost import AccelBoostSP, ACCEL_BOOST_MAX
|
||||
|
||||
|
||||
def make_sm(brake_prob=0.05, gas_prob=0.3, lat_accel=0.0, v_ego=30.0, personality=0, enabled=True, should_stop=False):
|
||||
meta = SimpleNamespace(disengagePredictions=SimpleNamespace(brakePressProbs=[0.0, brake_prob, 0.0, 0.0, 0.0, 0.0],
|
||||
gasPressProbs=[0.0, gas_prob, 0.0, 0.0, 0.0, 0.0]))
|
||||
action = SimpleNamespace(shouldStop=should_stop, desiredCurvature=lat_accel / v_ego ** 2)
|
||||
return {'modelV2': SimpleNamespace(meta=meta, action=action), 'carState': SimpleNamespace(vEgo=v_ego),
|
||||
'selfdriveState': SimpleNamespace(enabled=enabled, personality=personality)}
|
||||
|
||||
|
||||
def settle(sm, frames=100):
|
||||
boost = AccelBoostSP()
|
||||
for _ in range(frames):
|
||||
boost.update(sm, 0.0, 9.0, 9.0)
|
||||
return boost.total_boost
|
||||
|
||||
|
||||
class TestAccelBoostSP(OpenpilotTestCase):
|
||||
@parameterized.expand([(0,), (1,), (2,)])
|
||||
def test_personality_max(self, personality):
|
||||
assert abs(settle(make_sm(personality=personality)) - ACCEL_BOOST_MAX[personality]) < 1e-6
|
||||
|
||||
def test_brake_gate_closes(self):
|
||||
assert settle(make_sm(brake_prob=0.5)) == 0.0
|
||||
|
||||
def test_gas_reopens(self):
|
||||
assert abs(settle(make_sm(brake_prob=0.5, gas_prob=0.9)) - ACCEL_BOOST_MAX[0]) < 1e-6
|
||||
|
||||
def test_should_stop(self):
|
||||
assert settle(make_sm(gas_prob=0.9, should_stop=True)) == 0.0
|
||||
|
||||
@parameterized.expand([(1.5, 1.0), (2.0, 0.5), (2.5, 0.0), (-2.0, 0.5)])
|
||||
def test_curve_fade(self, lat_accel, scale):
|
||||
assert abs(settle(make_sm(lat_accel=lat_accel)) - scale * ACCEL_BOOST_MAX[0]) < 1e-6
|
||||
|
||||
def test_low_speed(self):
|
||||
assert settle(make_sm(v_ego=0.5)) == 0.0
|
||||
|
||||
def test_disabled_resets(self):
|
||||
boost = AccelBoostSP()
|
||||
for _ in range(100):
|
||||
boost.update(make_sm(), 0.0, 9.0, 9.0)
|
||||
assert boost.update(make_sm(enabled=False), 0.0, 9.0, 9.0) == 0.0
|
||||
assert boost.boost == 0.0 and boost.total_boost == 0.0
|
||||
|
||||
def test_build_and_release_rates(self):
|
||||
boost = AccelBoostSP()
|
||||
boost.update(make_sm(), 0.0, 9.0, 9.0)
|
||||
assert abs(boost.total_boost - 0.01) < 1e-6
|
||||
for _ in range(100):
|
||||
boost.update(make_sm(), 0.0, 9.0, 9.0)
|
||||
boost.update(make_sm(brake_prob=0.5), 0.0, 9.0, 9.0)
|
||||
assert abs(boost.total_boost - (ACCEL_BOOST_MAX[0] - 0.05)) < 1e-6
|
||||
|
||||
def test_param_switch(self):
|
||||
params = Params()
|
||||
boost = AccelBoostSP()
|
||||
params.put_bool("AccelBoostSP", True, block=True)
|
||||
assert boost.update_params() and boost.enabled
|
||||
for _ in range(100):
|
||||
boost.update(make_sm(), 0.0, 9.0, 9.0)
|
||||
params.put_bool("AccelBoostSP", False, block=True)
|
||||
for _ in range(int(PARAMS_UPDATE_PERIOD / DT_MDL) - 1):
|
||||
assert not boost.update_params()
|
||||
assert boost.update_params() and not boost.enabled
|
||||
assert boost.boost == 0.0 and boost.total_boost == 0.0
|
||||
@@ -0,0 +1,3 @@
|
||||
params_learner
|
||||
paramsd
|
||||
locationd
|
||||
@@ -0,0 +1,30 @@
|
||||
Import('env', 'arch', 'common', 'messaging', 'rednose', 'transformations')
|
||||
|
||||
loc_libs = [messaging, common, 'pthread', 'dl']
|
||||
|
||||
# build ekf models
|
||||
rednose_gen_dir = 'models/generated'
|
||||
rednose_gen_deps = [
|
||||
"models/constants.py",
|
||||
]
|
||||
live_ekf = env.RednoseCompileFilter(
|
||||
target='live',
|
||||
filter_gen_script='models/live_kf.py',
|
||||
output_dir=rednose_gen_dir,
|
||||
extra_gen_artifacts=['live_kf_constants.h'],
|
||||
gen_script_deps=rednose_gen_deps,
|
||||
)
|
||||
|
||||
# locationd build
|
||||
locationd_sources = ["locationd.cc", "models/live_kf.cc"]
|
||||
|
||||
lenv = env.Clone()
|
||||
# ekf filter libraries need to be linked, even if no symbols are used
|
||||
if arch != "Darwin":
|
||||
lenv["LINKFLAGS"] += ["-Wl,--no-as-needed"]
|
||||
|
||||
lenv["LIBPATH"].append(Dir(rednose_gen_dir).abspath)
|
||||
lenv["RPATH"].append(Dir(rednose_gen_dir).abspath)
|
||||
locationd = lenv.Program("locationd", locationd_sources, LIBS=["live", "ekf_sym"] + loc_libs + transformations)
|
||||
lenv.Depends(locationd, rednose)
|
||||
lenv.Depends(locationd, live_ekf)
|
||||
@@ -1,181 +0,0 @@
|
||||
"""
|
||||
Copyright (c) 2021-, sunnypilot, and a number of other contributors.
|
||||
|
||||
This file is part of sunnypilot and is licensed under the MIT License.
|
||||
See the LICENSE.md file in the root directory for more details.
|
||||
"""
|
||||
import json
|
||||
import math
|
||||
import time
|
||||
from collections import deque
|
||||
|
||||
import numpy as np
|
||||
|
||||
import openpilot.cereal.messaging as messaging
|
||||
from openpilot.common.gps import get_gps_location_service
|
||||
from openpilot.common.params import Params
|
||||
from openpilot.common.transformations.orientation import rot_from_euler
|
||||
|
||||
EARTH_RADIUS = 6378137.0
|
||||
GPS_LATENCY = {'gpsLocation': 0.630, 'gpsLocationExternal': 0.095} # s, empirical fix latency per receiver
|
||||
RESET_DISTANCE = 100.0 # m
|
||||
MAX_REJECTS = 5
|
||||
MAX_POS_STD = 50.0 # m
|
||||
MIN_BEARING_SPEED = 5.0 # m/s
|
||||
MAX_BEARING_ACCURACY = 10.0 # deg
|
||||
GPS_TIMEOUT = 2.0 # s
|
||||
SEED_INTERVAL = 60.0 # s
|
||||
|
||||
|
||||
def wrap(a: float) -> float:
|
||||
return (a + math.pi) % (2 * math.pi) - math.pi
|
||||
|
||||
|
||||
class GpsFuser:
|
||||
def __init__(self):
|
||||
self.t: float | None = None
|
||||
self.lat = self.lon = self.heading = math.nan
|
||||
self.pos_var = self.heading_var = math.inf
|
||||
self.history: deque = deque(maxlen=40)
|
||||
self.rejects = 0
|
||||
|
||||
@property
|
||||
def initialized(self) -> bool:
|
||||
return math.isfinite(self.lat)
|
||||
|
||||
@property
|
||||
def valid(self) -> bool:
|
||||
return self.initialized and math.isfinite(self.heading) and self.pos_var < MAX_POS_STD ** 2
|
||||
|
||||
def _move(self, north: float, east: float) -> None:
|
||||
self.lat += math.degrees(north / EARTH_RADIUS)
|
||||
self.lon += math.degrees(east / (EARTH_RADIUS * math.cos(math.radians(self.lat))))
|
||||
|
||||
@staticmethod
|
||||
def _offset(lat: float, lon: float, ref_lat: float, ref_lon: float) -> tuple[float, float]:
|
||||
north = math.radians(lat - ref_lat) * EARTH_RADIUS
|
||||
east = math.radians(lon - ref_lon) * EARTH_RADIUS * math.cos(math.radians(ref_lat))
|
||||
return north, east
|
||||
|
||||
def _state_at(self, t: float) -> tuple[float, float, float]:
|
||||
for ht, lat, lon, heading in reversed(self.history):
|
||||
if ht <= t:
|
||||
return lat, lon, heading
|
||||
return self.lat, self.lon, self.heading
|
||||
|
||||
def predict(self, t: float, speed: float, yaw_rate: float) -> None:
|
||||
dt = 0.0 if self.t is None else t - self.t
|
||||
self.t = t
|
||||
if not 0.0 < dt < 1.0 or not self.initialized:
|
||||
return
|
||||
if math.isfinite(self.heading):
|
||||
self.heading = (self.heading + yaw_rate * dt) % (2 * math.pi)
|
||||
self._move(speed * dt * math.cos(self.heading), speed * dt * math.sin(self.heading))
|
||||
# dead reckoning error grows with distance driven (speed scale and heading error), not as a random walk
|
||||
drift = (0.03 + math.sqrt(min(self.heading_var, 1.0))) * abs(speed) * dt
|
||||
self.pos_var = (math.sqrt(self.pos_var) + drift) ** 2 + 0.01 * dt
|
||||
self.heading_var += 1e-5 * dt
|
||||
self.history.append((t, self.lat, self.lon, self.heading))
|
||||
|
||||
def update_gps(self, t: float, gps, latency: float) -> None:
|
||||
# qcom reports no horizontal accuracy
|
||||
accuracy = gps.horizontalAccuracy if gps.horizontalAccuracy > 0 else math.sqrt(3) * gps.verticalAccuracy
|
||||
if not gps.hasFix or not 0 < accuracy < 1500 or gps.speedAccuracy <= 0:
|
||||
return
|
||||
if abs(gps.latitude) > 90 or abs(gps.longitude) > 180:
|
||||
return
|
||||
|
||||
then_lat, then_lon, then_heading = self._state_at(t - latency)
|
||||
moved = self._offset(self.lat, self.lon, then_lat, then_lon) if self.initialized else (0.0, 0.0)
|
||||
bearing = math.radians(gps.bearingDeg)
|
||||
bearing_ok = gps.speed > MIN_BEARING_SPEED and 0 < gps.bearingAccuracyDeg < MAX_BEARING_ACCURACY
|
||||
pos_r = accuracy ** 2
|
||||
|
||||
innov = self._offset(gps.latitude, gps.longitude, then_lat, then_lon) if self.initialized else (math.inf, math.inf)
|
||||
dist = math.hypot(*innov)
|
||||
if not self.initialized or dist > RESET_DISTANCE or self.rejects >= MAX_REJECTS:
|
||||
self.lat, self.lon = gps.latitude, gps.longitude
|
||||
self._move(*moved)
|
||||
self.pos_var = pos_r
|
||||
self.rejects = 0
|
||||
self.history.clear()
|
||||
if bearing_ok:
|
||||
self.heading, self.heading_var = bearing, math.radians(gps.bearingAccuracyDeg) ** 2
|
||||
return
|
||||
|
||||
s = self.pos_var + pos_r
|
||||
if dist ** 2 > 9.0 * s and dist > 10.0:
|
||||
self.rejects += 1
|
||||
return
|
||||
self.rejects = 0
|
||||
k = self.pos_var / s
|
||||
lat0, lon0 = self.lat, self.lon
|
||||
self._move(k * innov[0], k * innov[1])
|
||||
dlat, dlon = self.lat - lat0, self.lon - lon0
|
||||
self.history = deque(((ht, lat + dlat, lon + dlon, h) for ht, lat, lon, h in self.history), maxlen=self.history.maxlen)
|
||||
self.pos_var *= 1 - k
|
||||
|
||||
if bearing_ok:
|
||||
head_r = math.radians(gps.bearingAccuracyDeg) ** 2
|
||||
if not math.isfinite(self.heading):
|
||||
self.heading, self.heading_var = bearing, head_r
|
||||
else:
|
||||
kh = self.heading_var / (self.heading_var + head_r)
|
||||
self.heading = (self.heading + kh * wrap(bearing - then_heading)) % (2 * math.pi)
|
||||
self.heading_var *= 1 - kh
|
||||
|
||||
|
||||
def main():
|
||||
params = Params()
|
||||
gps_service = get_gps_location_service(params)
|
||||
latency = GPS_LATENCY[gps_service]
|
||||
sm = messaging.SubMaster(['deviceMotion', 'carState', 'extrinsicsCalibration', gps_service], poll='deviceMotion')
|
||||
pm = messaging.PubMaster(['gpsLocationSP'])
|
||||
fuser = GpsFuser()
|
||||
calib_from_device = np.eye(3)
|
||||
last_seed = 0.0
|
||||
|
||||
while True:
|
||||
sm.update()
|
||||
t = sm.logMonoTime['deviceMotion'] * 1e-9
|
||||
|
||||
rpy = list(sm['extrinsicsCalibration'].rpyCalib)
|
||||
if sm.updated['extrinsicsCalibration'] and len(rpy) == 3:
|
||||
calib_from_device = rot_from_euler(rpy).T
|
||||
|
||||
gps = sm[gps_service]
|
||||
gps_t = sm.logMonoTime[gps_service] * 1e-9
|
||||
if sm.updated[gps_service] and sm.valid[gps_service]:
|
||||
fuser.update_gps(gps_t, gps, latency)
|
||||
|
||||
if sm.valid['deviceMotion']:
|
||||
w = sm['deviceMotion'].angularVelocityDevice
|
||||
yaw_rate = float((calib_from_device @ np.array([w.x, w.y, w.z]))[2])
|
||||
fuser.predict(t, sm['carState'].vEgo, yaw_rate)
|
||||
|
||||
gps_ok = gps.hasFix and 0 <= t - gps_t < GPS_TIMEOUT
|
||||
msg = messaging.new_message('gpsLocationSP', valid=fuser.valid or gps_ok)
|
||||
out = msg.gpsLocationSP
|
||||
out.gpsOK = gps_ok
|
||||
out.fused = fuser.valid
|
||||
if fuser.valid:
|
||||
out.hasFix = True
|
||||
out.latitude, out.longitude = fuser.lat, fuser.lon
|
||||
out.bearingDeg = math.degrees(fuser.heading) % 360
|
||||
out.bearingAccuracyDeg = math.degrees(math.sqrt(fuser.heading_var))
|
||||
out.horizontalAccuracy = math.sqrt(fuser.pos_var)
|
||||
out.speed = sm['carState'].vEgo
|
||||
elif gps_ok:
|
||||
out.hasFix = True
|
||||
out.latitude, out.longitude = gps.latitude, gps.longitude
|
||||
out.bearingDeg, out.bearingAccuracyDeg = gps.bearingDeg, gps.bearingAccuracyDeg
|
||||
out.horizontalAccuracy, out.speed = gps.horizontalAccuracy, gps.speed
|
||||
pm.send('gpsLocationSP', msg)
|
||||
|
||||
if fuser.valid and time.monotonic() - last_seed > SEED_INTERVAL:
|
||||
params.put("LastGPSPositionFused", json.dumps({"latitude": fuser.lat, "longitude": fuser.lon}))
|
||||
last_seed = time.monotonic()
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@@ -0,0 +1,750 @@
|
||||
#include "sunnypilot/selfdrive/locationd/locationd.h"
|
||||
|
||||
#include <sys/time.h>
|
||||
#include <sys/resource.h>
|
||||
|
||||
#include <algorithm>
|
||||
#include <cmath>
|
||||
#include <vector>
|
||||
|
||||
using namespace EKFS;
|
||||
using namespace Eigen;
|
||||
|
||||
ExitHandler do_exit;
|
||||
const double ACCEL_SANITY_CHECK = 100.0; // m/s^2
|
||||
const double ROTATION_SANITY_CHECK = 10.0; // rad/s
|
||||
const double TRANS_SANITY_CHECK = 200.0; // m/s
|
||||
const double CALIB_RPY_SANITY_CHECK = 0.5; // rad (+- 30 deg)
|
||||
const double ALTITUDE_SANITY_CHECK = 10000; // m
|
||||
const double MIN_STD_SANITY_CHECK = 1e-5; // m or rad
|
||||
const double VALID_TIME_SINCE_RESET = 1.0; // s
|
||||
const double VALID_POS_STD = 50.0; // m
|
||||
const double MAX_RESET_TRACKER = 5.0;
|
||||
const double SANE_GPS_UNCERTAINTY = 1500.0; // m
|
||||
const double INPUT_INVALID_THRESHOLD = 0.5; // same as reset tracker
|
||||
const double RESET_TRACKER_DECAY = 0.99995;
|
||||
const double DECAY = 0.9993; // ~10 secs to resume after a bad input
|
||||
const double MAX_FILTER_REWIND_TIME = 0.8; // s
|
||||
const double YAWRATE_CROSS_ERR_CHECK_FACTOR = 30;
|
||||
|
||||
// TODO: GPS sensor time offsets are empirically calculated
|
||||
// They should be replaced with synced time from a real clock
|
||||
const double GPS_QUECTEL_SENSOR_TIME_OFFSET = 0.630; // s
|
||||
const double GPS_UBLOX_SENSOR_TIME_OFFSET = 0.095; // s
|
||||
const float GPS_POS_STD_THRESHOLD = 50.0;
|
||||
const float GPS_VEL_STD_THRESHOLD = 5.0;
|
||||
const float GPS_POS_ERROR_RESET_THRESHOLD = 300.0;
|
||||
const float GPS_POS_STD_RESET_THRESHOLD = 2.0;
|
||||
const float GPS_VEL_STD_RESET_THRESHOLD = 0.5;
|
||||
const float GPS_ORIENTATION_ERROR_RESET_THRESHOLD = 1.0;
|
||||
const int GPS_ORIENTATION_ERROR_RESET_CNT = 3;
|
||||
|
||||
const bool DEBUG = getenv("DEBUG") != nullptr && std::string(getenv("DEBUG")) != "0";
|
||||
|
||||
static VectorXd floatlist2vector(const capnp::List<float, capnp::Kind::PRIMITIVE>::Reader& floatlist) {
|
||||
VectorXd res(floatlist.size());
|
||||
for (int i = 0; i < floatlist.size(); i++) {
|
||||
res[i] = floatlist[i];
|
||||
}
|
||||
return res;
|
||||
}
|
||||
|
||||
static Vector4d quat2vector(const Quaterniond& quat) {
|
||||
return Vector4d(quat.w(), quat.x(), quat.y(), quat.z());
|
||||
}
|
||||
|
||||
static Quaterniond vector2quat(const VectorXd& vec) {
|
||||
return Quaterniond(vec(0), vec(1), vec(2), vec(3));
|
||||
}
|
||||
|
||||
static void init_measurement(cereal::LiveLocationKalman::Measurement::Builder meas, const VectorXd& val, const VectorXd& std, bool valid) {
|
||||
meas.setValue(kj::arrayPtr(val.data(), val.size()));
|
||||
meas.setStd(kj::arrayPtr(std.data(), std.size()));
|
||||
meas.setValid(valid);
|
||||
}
|
||||
|
||||
|
||||
static MatrixXdr rotate_cov(const MatrixXdr& rot_matrix, const MatrixXdr& cov_in) {
|
||||
// To rotate a covariance matrix, the cov matrix needs to multiplied left and right by the transform matrix
|
||||
return ((rot_matrix * cov_in) * rot_matrix.transpose());
|
||||
}
|
||||
|
||||
static VectorXd rotate_std(const MatrixXdr& rot_matrix, const VectorXd& std_in) {
|
||||
// Stds cannot be rotated like values, only covariances can be rotated
|
||||
return rotate_cov(rot_matrix, std_in.array().square().matrix().asDiagonal()).diagonal().array().sqrt();
|
||||
}
|
||||
|
||||
Localizer::Localizer(LocalizerGnssSource gnss_source) {
|
||||
this->kf = std::make_unique<LiveKalman>();
|
||||
this->reset_kalman();
|
||||
|
||||
this->calib = Vector3d(0.0, 0.0, 0.0);
|
||||
this->device_from_calib = MatrixXdr::Identity(3, 3);
|
||||
this->calib_from_device = MatrixXdr::Identity(3, 3);
|
||||
|
||||
for (int i = 0; i < POSENET_STD_HIST_HALF * 2; i++) {
|
||||
this->posenet_stds.push_back(10.0);
|
||||
}
|
||||
|
||||
VectorXd ecef_pos = this->kf->get_x().segment<STATE_ECEF_POS_LEN>(STATE_ECEF_POS_START);
|
||||
this->converter = std::make_unique<LocalCoord>((ECEF) { .x = ecef_pos[0], .y = ecef_pos[1], .z = ecef_pos[2] });
|
||||
this->configure_gnss_source(gnss_source);
|
||||
}
|
||||
|
||||
void Localizer::build_live_location(cereal::LiveLocationKalman::Builder& fix) {
|
||||
VectorXd predicted_state = this->kf->get_x();
|
||||
MatrixXdr predicted_cov = this->kf->get_P();
|
||||
VectorXd predicted_std = predicted_cov.diagonal().array().sqrt();
|
||||
|
||||
VectorXd fix_ecef = predicted_state.segment<STATE_ECEF_POS_LEN>(STATE_ECEF_POS_START);
|
||||
ECEF fix_ecef_ecef = { .x = fix_ecef(0), .y = fix_ecef(1), .z = fix_ecef(2) };
|
||||
VectorXd fix_ecef_std = predicted_std.segment<STATE_ECEF_POS_ERR_LEN>(STATE_ECEF_POS_ERR_START);
|
||||
VectorXd vel_ecef = predicted_state.segment<STATE_ECEF_VELOCITY_LEN>(STATE_ECEF_VELOCITY_START);
|
||||
VectorXd vel_ecef_std = predicted_std.segment<STATE_ECEF_VELOCITY_ERR_LEN>(STATE_ECEF_VELOCITY_ERR_START);
|
||||
VectorXd fix_pos_geo_vec = this->get_position_geodetic();
|
||||
VectorXd orientation_ecef = quat2euler(vector2quat(predicted_state.segment<STATE_ECEF_ORIENTATION_LEN>(STATE_ECEF_ORIENTATION_START)));
|
||||
VectorXd orientation_ecef_std = predicted_std.segment<STATE_ECEF_ORIENTATION_ERR_LEN>(STATE_ECEF_ORIENTATION_ERR_START);
|
||||
MatrixXdr orientation_ecef_cov = predicted_cov.block<STATE_ECEF_ORIENTATION_ERR_LEN, STATE_ECEF_ORIENTATION_ERR_LEN>(STATE_ECEF_ORIENTATION_ERR_START, STATE_ECEF_ORIENTATION_ERR_START);
|
||||
MatrixXdr device_from_ecef = euler2rot(orientation_ecef).transpose();
|
||||
VectorXd calibrated_orientation_ecef = rot2euler((this->calib_from_device * device_from_ecef).transpose());
|
||||
|
||||
VectorXd acc_calib = this->calib_from_device * predicted_state.segment<STATE_ACCELERATION_LEN>(STATE_ACCELERATION_START);
|
||||
MatrixXdr acc_calib_cov = predicted_cov.block<STATE_ACCELERATION_ERR_LEN, STATE_ACCELERATION_ERR_LEN>(STATE_ACCELERATION_ERR_START, STATE_ACCELERATION_ERR_START);
|
||||
VectorXd acc_calib_std = rotate_cov(this->calib_from_device, acc_calib_cov).diagonal().array().sqrt();
|
||||
VectorXd ang_vel_calib = this->calib_from_device * predicted_state.segment<STATE_ANGULAR_VELOCITY_LEN>(STATE_ANGULAR_VELOCITY_START);
|
||||
|
||||
MatrixXdr vel_angular_cov = predicted_cov.block<STATE_ANGULAR_VELOCITY_ERR_LEN, STATE_ANGULAR_VELOCITY_ERR_LEN>(STATE_ANGULAR_VELOCITY_ERR_START, STATE_ANGULAR_VELOCITY_ERR_START);
|
||||
VectorXd ang_vel_calib_std = rotate_cov(this->calib_from_device, vel_angular_cov).diagonal().array().sqrt();
|
||||
|
||||
VectorXd vel_device = device_from_ecef * vel_ecef;
|
||||
VectorXd device_from_ecef_eul = quat2euler(vector2quat(predicted_state.segment<STATE_ECEF_ORIENTATION_LEN>(STATE_ECEF_ORIENTATION_START))).transpose();
|
||||
MatrixXdr condensed_cov(STATE_ECEF_ORIENTATION_ERR_LEN + STATE_ECEF_VELOCITY_ERR_LEN, STATE_ECEF_ORIENTATION_ERR_LEN + STATE_ECEF_VELOCITY_ERR_LEN);
|
||||
condensed_cov.topLeftCorner<STATE_ECEF_ORIENTATION_ERR_LEN, STATE_ECEF_ORIENTATION_ERR_LEN>() =
|
||||
predicted_cov.block<STATE_ECEF_ORIENTATION_ERR_LEN, STATE_ECEF_ORIENTATION_ERR_LEN>(STATE_ECEF_ORIENTATION_ERR_START, STATE_ECEF_ORIENTATION_ERR_START);
|
||||
condensed_cov.topRightCorner<STATE_ECEF_ORIENTATION_ERR_LEN, STATE_ECEF_VELOCITY_ERR_LEN>() =
|
||||
predicted_cov.block<STATE_ECEF_ORIENTATION_ERR_LEN, STATE_ECEF_VELOCITY_ERR_LEN>(STATE_ECEF_ORIENTATION_ERR_START, STATE_ECEF_VELOCITY_ERR_START);
|
||||
condensed_cov.bottomRightCorner<STATE_ECEF_VELOCITY_ERR_LEN, STATE_ECEF_VELOCITY_ERR_LEN>() =
|
||||
predicted_cov.block<STATE_ECEF_VELOCITY_ERR_LEN, STATE_ECEF_VELOCITY_ERR_LEN>(STATE_ECEF_VELOCITY_ERR_START, STATE_ECEF_VELOCITY_ERR_START);
|
||||
condensed_cov.bottomLeftCorner<STATE_ECEF_VELOCITY_ERR_LEN, STATE_ECEF_ORIENTATION_ERR_LEN>() =
|
||||
predicted_cov.block<STATE_ECEF_VELOCITY_ERR_LEN, STATE_ECEF_ORIENTATION_ERR_LEN>(STATE_ECEF_VELOCITY_ERR_START, STATE_ECEF_ORIENTATION_ERR_START);
|
||||
VectorXd H_input(device_from_ecef_eul.size() + vel_ecef.size());
|
||||
H_input << device_from_ecef_eul, vel_ecef;
|
||||
MatrixXdr HH = this->kf->H(H_input);
|
||||
MatrixXdr vel_device_cov = (HH * condensed_cov) * HH.transpose();
|
||||
VectorXd vel_device_std = vel_device_cov.diagonal().array().sqrt();
|
||||
|
||||
VectorXd vel_calib = this->calib_from_device * vel_device;
|
||||
VectorXd vel_calib_std = rotate_cov(this->calib_from_device, vel_device_cov).diagonal().array().sqrt();
|
||||
|
||||
VectorXd orientation_ned = ned_euler_from_ecef(fix_ecef_ecef, orientation_ecef);
|
||||
VectorXd orientation_ned_std = rotate_cov(this->converter->ecef2ned_matrix, orientation_ecef_cov).diagonal().array().sqrt();
|
||||
VectorXd calibrated_orientation_ned = ned_euler_from_ecef(fix_ecef_ecef, calibrated_orientation_ecef);
|
||||
VectorXd nextfix_ecef = fix_ecef + vel_ecef;
|
||||
VectorXd ned_vel = this->converter->ecef2ned((ECEF) { .x = nextfix_ecef(0), .y = nextfix_ecef(1), .z = nextfix_ecef(2) }).to_vector() - converter->ecef2ned(fix_ecef_ecef).to_vector();
|
||||
|
||||
VectorXd accDevice = predicted_state.segment<STATE_ACCELERATION_LEN>(STATE_ACCELERATION_START);
|
||||
VectorXd accDeviceErr = predicted_std.segment<STATE_ACCELERATION_ERR_LEN>(STATE_ACCELERATION_ERR_START);
|
||||
|
||||
VectorXd angVelocityDevice = predicted_state.segment<STATE_ANGULAR_VELOCITY_LEN>(STATE_ANGULAR_VELOCITY_START);
|
||||
VectorXd angVelocityDeviceErr = predicted_std.segment<STATE_ANGULAR_VELOCITY_ERR_LEN>(STATE_ANGULAR_VELOCITY_ERR_START);
|
||||
|
||||
Vector3d nans = Vector3d(NAN, NAN, NAN);
|
||||
|
||||
// TODO fill in NED and Calibrated stds
|
||||
// write measurements to msg
|
||||
init_measurement(fix.initPositionGeodetic(), fix_pos_geo_vec, nans, this->gps_mode);
|
||||
init_measurement(fix.initPositionECEF(), fix_ecef, fix_ecef_std, this->gps_mode);
|
||||
init_measurement(fix.initVelocityECEF(), vel_ecef, vel_ecef_std, this->gps_mode);
|
||||
init_measurement(fix.initVelocityNED(), ned_vel, nans, this->gps_mode);
|
||||
init_measurement(fix.initVelocityDevice(), vel_device, vel_device_std, true);
|
||||
init_measurement(fix.initAccelerationDevice(), accDevice, accDeviceErr, true);
|
||||
init_measurement(fix.initOrientationECEF(), orientation_ecef, orientation_ecef_std, this->gps_mode);
|
||||
init_measurement(fix.initCalibratedOrientationECEF(), calibrated_orientation_ecef, nans, this->calibrated && this->gps_mode);
|
||||
init_measurement(fix.initOrientationNED(), orientation_ned, orientation_ned_std, this->gps_mode);
|
||||
init_measurement(fix.initCalibratedOrientationNED(), calibrated_orientation_ned, nans, this->calibrated && this->gps_mode);
|
||||
init_measurement(fix.initAngularVelocityDevice(), angVelocityDevice, angVelocityDeviceErr, true);
|
||||
init_measurement(fix.initVelocityCalibrated(), vel_calib, vel_calib_std, this->calibrated);
|
||||
init_measurement(fix.initAngularVelocityCalibrated(), ang_vel_calib, ang_vel_calib_std, this->calibrated);
|
||||
init_measurement(fix.initAccelerationCalibrated(), acc_calib, acc_calib_std, this->calibrated);
|
||||
if (DEBUG) {
|
||||
init_measurement(fix.initFilterState(), predicted_state, predicted_std, true);
|
||||
}
|
||||
|
||||
double old_mean = 0.0, new_mean = 0.0;
|
||||
int i = 0;
|
||||
for (double x : this->posenet_stds) {
|
||||
if (i < POSENET_STD_HIST_HALF) {
|
||||
old_mean += x;
|
||||
} else {
|
||||
new_mean += x;
|
||||
}
|
||||
i++;
|
||||
}
|
||||
old_mean /= POSENET_STD_HIST_HALF;
|
||||
new_mean /= POSENET_STD_HIST_HALF;
|
||||
// experimentally found these values, no false positives in 20k minutes of driving
|
||||
bool std_spike = (new_mean / old_mean > 4.0 && new_mean > 7.0);
|
||||
|
||||
fix.setPosenetOK(!(std_spike && this->car_speed > 5.0));
|
||||
fix.setDeviceStable(!this->device_fell);
|
||||
fix.setExcessiveResets(this->reset_tracker > MAX_RESET_TRACKER);
|
||||
fix.setTimeToFirstFix(std::isnan(this->ttff) ? -1. : this->ttff);
|
||||
this->device_fell = false;
|
||||
|
||||
//fix.setGpsWeek(this->time.week);
|
||||
//fix.setGpsTimeOfWeek(this->time.tow);
|
||||
fix.setUnixTimestampMillis(this->unix_timestamp_millis);
|
||||
|
||||
double time_since_reset = this->kf->get_filter_time() - this->last_reset_time;
|
||||
fix.setTimeSinceReset(time_since_reset);
|
||||
if (fix_ecef_std.norm() < VALID_POS_STD && this->calibrated && time_since_reset > VALID_TIME_SINCE_RESET) {
|
||||
fix.setStatus(cereal::LiveLocationKalman::Status::VALID);
|
||||
} else if (fix_ecef_std.norm() < VALID_POS_STD && time_since_reset > VALID_TIME_SINCE_RESET) {
|
||||
fix.setStatus(cereal::LiveLocationKalman::Status::UNCALIBRATED);
|
||||
} else {
|
||||
fix.setStatus(cereal::LiveLocationKalman::Status::UNINITIALIZED);
|
||||
}
|
||||
}
|
||||
|
||||
VectorXd Localizer::get_position_geodetic() {
|
||||
VectorXd fix_ecef = this->kf->get_x().segment<STATE_ECEF_POS_LEN>(STATE_ECEF_POS_START);
|
||||
ECEF fix_ecef_ecef = { .x = fix_ecef(0), .y = fix_ecef(1), .z = fix_ecef(2) };
|
||||
Geodetic fix_pos_geo = ecef2geodetic(fix_ecef_ecef);
|
||||
return Vector3d(fix_pos_geo.lat, fix_pos_geo.lon, fix_pos_geo.alt);
|
||||
}
|
||||
|
||||
VectorXd Localizer::get_state() {
|
||||
return this->kf->get_x();
|
||||
}
|
||||
|
||||
VectorXd Localizer::get_stdev() {
|
||||
return this->kf->get_P().diagonal().array().sqrt();
|
||||
}
|
||||
|
||||
bool Localizer::are_inputs_ok() {
|
||||
return this->critical_services_valid(this->observation_values_invalid) && !this->observation_timings_invalid;
|
||||
}
|
||||
|
||||
void Localizer::observation_timings_invalid_reset(){
|
||||
this->observation_timings_invalid = false;
|
||||
}
|
||||
|
||||
void Localizer::handle_sensor(double current_time, const cereal::SensorEventData::Reader& log) {
|
||||
// TODO does not yet account for double sensor readings in the log
|
||||
|
||||
// Ignore empty readings (e.g. in case the magnetometer had no data ready)
|
||||
if (log.getTimestamp() == 0) {
|
||||
return;
|
||||
}
|
||||
|
||||
double sensor_time = 1e-9 * log.getTimestamp();
|
||||
|
||||
// sensor time and log time should be close
|
||||
if (std::abs(current_time - sensor_time) > 0.1) {
|
||||
LOGE("Sensor reading ignored, sensor timestamp more than 100ms off from log time");
|
||||
this->observation_timings_invalid = true;
|
||||
return;
|
||||
} else if (!this->is_timestamp_valid(sensor_time)) {
|
||||
this->observation_timings_invalid = true;
|
||||
return;
|
||||
}
|
||||
|
||||
// TODO: handle messages from two IMUs at the same time
|
||||
if (log.getSource() == cereal::SensorEventData::SensorSource::BMX055) {
|
||||
return;
|
||||
}
|
||||
|
||||
// Gyro Uncalibrated
|
||||
if (log.which() == cereal::SensorEventData::GYRO_UNCALIBRATED) {
|
||||
auto v = log.getGyroUncalibrated().getV();
|
||||
auto meas = Vector3d(-v[2], -v[1], -v[0]);
|
||||
|
||||
VectorXd gyro_bias = this->kf->get_x().segment<STATE_GYRO_BIAS_LEN>(STATE_GYRO_BIAS_START);
|
||||
float gyro_camodo_yawrate_err = std::abs((meas[2] - gyro_bias[2]) - this->camodo_yawrate_distribution[0]);
|
||||
float gyro_camodo_yawrate_err_threshold = YAWRATE_CROSS_ERR_CHECK_FACTOR * this->camodo_yawrate_distribution[1];
|
||||
bool gyro_valid = gyro_camodo_yawrate_err < gyro_camodo_yawrate_err_threshold;
|
||||
|
||||
if ((meas.norm() < ROTATION_SANITY_CHECK) && gyro_valid) {
|
||||
this->kf->predict_and_observe(sensor_time, OBSERVATION_PHONE_GYRO, { meas });
|
||||
this->observation_values_invalid["gyroscope"] *= DECAY;
|
||||
} else {
|
||||
this->observation_values_invalid["gyroscope"] += 1.0;
|
||||
}
|
||||
}
|
||||
|
||||
// Accelerometer
|
||||
if (log.which() == cereal::SensorEventData::ACCELERATION) {
|
||||
auto v = log.getAcceleration().getV();
|
||||
|
||||
// TODO: reduce false positives and re-enable this check
|
||||
// check if device fell, estimate 10 for g
|
||||
// 40m/s**2 is a good filter for falling detection, no false positives in 20k minutes of driving
|
||||
// this->device_fell |= (floatlist2vector(v) - Vector3d(10.0, 0.0, 0.0)).norm() > 40.0;
|
||||
|
||||
auto meas = Vector3d(-v[2], -v[1], -v[0]);
|
||||
if (meas.norm() < ACCEL_SANITY_CHECK) {
|
||||
this->kf->predict_and_observe(sensor_time, OBSERVATION_PHONE_ACCEL, { meas });
|
||||
this->observation_values_invalid["accelerometer"] *= DECAY;
|
||||
} else {
|
||||
this->observation_values_invalid["accelerometer"] += 1.0;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void Localizer::input_fake_gps_observations(double current_time) {
|
||||
// This is done to make sure that the error estimate of the position does not blow up
|
||||
// when the filter is in no-gps mode
|
||||
// Steps : first predict -> observe current obs with reasonable STD
|
||||
this->kf->predict(current_time);
|
||||
|
||||
VectorXd current_x = this->kf->get_x();
|
||||
VectorXd ecef_pos = current_x.segment<STATE_ECEF_POS_LEN>(STATE_ECEF_POS_START);
|
||||
VectorXd ecef_vel = current_x.segment<STATE_ECEF_VELOCITY_LEN>(STATE_ECEF_VELOCITY_START);
|
||||
const MatrixXdr &ecef_pos_R = this->kf->get_fake_gps_pos_cov();
|
||||
const MatrixXdr &ecef_vel_R = this->kf->get_fake_gps_vel_cov();
|
||||
|
||||
this->kf->predict_and_observe(current_time, OBSERVATION_ECEF_POS, { ecef_pos }, { ecef_pos_R });
|
||||
this->kf->predict_and_observe(current_time, OBSERVATION_ECEF_VEL, { ecef_vel }, { ecef_vel_R });
|
||||
}
|
||||
|
||||
void Localizer::handle_gps(double current_time, const cereal::GpsLocationData::Reader& log, const double sensor_time_offset) {
|
||||
bool gps_unreasonable = (Vector2d(log.getHorizontalAccuracy(), log.getVerticalAccuracy()).norm() >= SANE_GPS_UNCERTAINTY);
|
||||
bool gps_accuracy_insane = ((log.getVerticalAccuracy() <= 0) || (log.getSpeedAccuracy() <= 0) || (log.getBearingAccuracyDeg() <= 0));
|
||||
bool gps_lat_lng_alt_insane = ((std::abs(log.getLatitude()) > 90) || (std::abs(log.getLongitude()) > 180) || (std::abs(log.getAltitude()) > ALTITUDE_SANITY_CHECK));
|
||||
bool gps_vel_insane = (floatlist2vector(log.getVNED()).norm() > TRANS_SANITY_CHECK);
|
||||
|
||||
if (!log.getHasFix() || gps_unreasonable || gps_accuracy_insane || gps_lat_lng_alt_insane || gps_vel_insane) {
|
||||
//this->gps_valid = false;
|
||||
this->determine_gps_mode(current_time);
|
||||
return;
|
||||
}
|
||||
|
||||
double sensor_time = current_time - sensor_time_offset;
|
||||
|
||||
// Process message
|
||||
//this->gps_valid = true;
|
||||
this->gps_mode = true;
|
||||
Geodetic geodetic = { log.getLatitude(), log.getLongitude(), log.getAltitude() };
|
||||
this->converter = std::make_unique<LocalCoord>(geodetic);
|
||||
|
||||
VectorXd ecef_pos = this->converter->ned2ecef({ 0.0, 0.0, 0.0 }).to_vector();
|
||||
VectorXd ecef_vel = this->converter->ned2ecef({ log.getVNED()[0], log.getVNED()[1], log.getVNED()[2] }).to_vector() - ecef_pos;
|
||||
float ecef_pos_std = std::sqrt(this->gps_variance_factor * std::pow(log.getHorizontalAccuracy(), 2) + this->gps_vertical_variance_factor * std::pow(log.getVerticalAccuracy(), 2));
|
||||
MatrixXdr ecef_pos_R = Vector3d::Constant(std::pow(this->gps_std_factor * ecef_pos_std, 2)).asDiagonal();
|
||||
MatrixXdr ecef_vel_R = Vector3d::Constant(std::pow(this->gps_std_factor * log.getSpeedAccuracy(), 2)).asDiagonal();
|
||||
|
||||
this->unix_timestamp_millis = log.getUnixTimestampMillis();
|
||||
double gps_est_error = (this->kf->get_x().segment<STATE_ECEF_POS_LEN>(STATE_ECEF_POS_START) - ecef_pos).norm();
|
||||
|
||||
VectorXd orientation_ecef = quat2euler(vector2quat(this->kf->get_x().segment<STATE_ECEF_ORIENTATION_LEN>(STATE_ECEF_ORIENTATION_START)));
|
||||
VectorXd orientation_ned = ned_euler_from_ecef({ ecef_pos(0), ecef_pos(1), ecef_pos(2) }, orientation_ecef);
|
||||
VectorXd orientation_ned_gps = Vector3d(0.0, 0.0, DEG2RAD(log.getBearingDeg()));
|
||||
VectorXd orientation_error = (orientation_ned - orientation_ned_gps).array() - M_PI;
|
||||
for (int i = 0; i < orientation_error.size(); i++) {
|
||||
orientation_error(i) = std::fmod(orientation_error(i), 2.0 * M_PI);
|
||||
if (orientation_error(i) < 0.0) {
|
||||
orientation_error(i) += 2.0 * M_PI;
|
||||
}
|
||||
orientation_error(i) -= M_PI;
|
||||
}
|
||||
VectorXd initial_pose_ecef_quat = quat2vector(euler2quat(ecef_euler_from_ned({ ecef_pos(0), ecef_pos(1), ecef_pos(2) }, orientation_ned_gps)));
|
||||
|
||||
if (ecef_vel.norm() > 5.0 && orientation_error.norm() > 1.0) {
|
||||
LOGE("Locationd vs ubloxLocation orientation difference too large, kalman reset");
|
||||
this->reset_kalman(NAN, initial_pose_ecef_quat, ecef_pos, ecef_vel, ecef_pos_R, ecef_vel_R);
|
||||
this->kf->predict_and_observe(sensor_time, OBSERVATION_ECEF_ORIENTATION_FROM_GPS, { initial_pose_ecef_quat });
|
||||
} else if (gps_est_error > 100.0) {
|
||||
LOGE("Locationd vs ubloxLocation position difference too large, kalman reset");
|
||||
this->reset_kalman(NAN, initial_pose_ecef_quat, ecef_pos, ecef_vel, ecef_pos_R, ecef_vel_R);
|
||||
}
|
||||
|
||||
this->last_gps_msg = sensor_time;
|
||||
this->kf->predict_and_observe(sensor_time, OBSERVATION_ECEF_POS, { ecef_pos }, { ecef_pos_R });
|
||||
this->kf->predict_and_observe(sensor_time, OBSERVATION_ECEF_VEL, { ecef_vel }, { ecef_vel_R });
|
||||
}
|
||||
|
||||
void Localizer::handle_gnss(double current_time, const cereal::GnssMeasurements::Reader& log) {
|
||||
|
||||
if (!log.getPositionECEF().getValid() || !log.getVelocityECEF().getValid()) {
|
||||
this->determine_gps_mode(current_time);
|
||||
return;
|
||||
}
|
||||
|
||||
double sensor_time = log.getMeasTime() * 1e-9;
|
||||
sensor_time -= this->gps_time_offset;
|
||||
|
||||
auto ecef_pos_v = log.getPositionECEF().getValue();
|
||||
VectorXd ecef_pos = Vector3d(ecef_pos_v[0], ecef_pos_v[1], ecef_pos_v[2]);
|
||||
|
||||
// indexed at 0 cause all std values are the same MAE
|
||||
auto ecef_pos_std = log.getPositionECEF().getStd()[0];
|
||||
MatrixXdr ecef_pos_R = Vector3d::Constant(pow(this->gps_std_factor*ecef_pos_std, 2)).asDiagonal();
|
||||
|
||||
auto ecef_vel_v = log.getVelocityECEF().getValue();
|
||||
VectorXd ecef_vel = Vector3d(ecef_vel_v[0], ecef_vel_v[1], ecef_vel_v[2]);
|
||||
|
||||
// indexed at 0 cause all std values are the same MAE
|
||||
auto ecef_vel_std = log.getVelocityECEF().getStd()[0];
|
||||
MatrixXdr ecef_vel_R = Vector3d::Constant(pow(this->gps_std_factor*ecef_vel_std, 2)).asDiagonal();
|
||||
|
||||
double gps_est_error = (this->kf->get_x().segment<STATE_ECEF_POS_LEN>(STATE_ECEF_POS_START) - ecef_pos).norm();
|
||||
|
||||
VectorXd orientation_ecef = quat2euler(vector2quat(this->kf->get_x().segment<STATE_ECEF_ORIENTATION_LEN>(STATE_ECEF_ORIENTATION_START)));
|
||||
VectorXd orientation_ned = ned_euler_from_ecef({ ecef_pos[0], ecef_pos[1], ecef_pos[2] }, orientation_ecef);
|
||||
|
||||
LocalCoord convs((ECEF){ .x = ecef_pos[0], .y = ecef_pos[1], .z = ecef_pos[2] });
|
||||
ECEF next_ecef = {.x = ecef_pos[0] + ecef_vel[0], .y = ecef_pos[1] + ecef_vel[1], .z = ecef_pos[2] + ecef_vel[2]};
|
||||
VectorXd ned_vel = convs.ecef2ned(next_ecef).to_vector();
|
||||
double bearing_rad = atan2(ned_vel[1], ned_vel[0]);
|
||||
|
||||
VectorXd orientation_ned_gps = Vector3d(0.0, 0.0, bearing_rad);
|
||||
VectorXd orientation_error = (orientation_ned - orientation_ned_gps).array() - M_PI;
|
||||
for (int i = 0; i < orientation_error.size(); i++) {
|
||||
orientation_error(i) = std::fmod(orientation_error(i), 2.0 * M_PI);
|
||||
if (orientation_error(i) < 0.0) {
|
||||
orientation_error(i) += 2.0 * M_PI;
|
||||
}
|
||||
orientation_error(i) -= M_PI;
|
||||
}
|
||||
VectorXd initial_pose_ecef_quat = quat2vector(euler2quat(ecef_euler_from_ned({ ecef_pos(0), ecef_pos(1), ecef_pos(2) }, orientation_ned_gps)));
|
||||
|
||||
if (ecef_pos_std > GPS_POS_STD_THRESHOLD || ecef_vel_std > GPS_VEL_STD_THRESHOLD) {
|
||||
this->determine_gps_mode(current_time);
|
||||
return;
|
||||
}
|
||||
|
||||
// prevent jumping gnss measurements (covered lots, standstill...)
|
||||
bool orientation_reset = ecef_vel_std < GPS_VEL_STD_RESET_THRESHOLD;
|
||||
orientation_reset &= orientation_error.norm() > GPS_ORIENTATION_ERROR_RESET_THRESHOLD;
|
||||
orientation_reset &= !this->standstill;
|
||||
if (orientation_reset) {
|
||||
this->orientation_reset_count++;
|
||||
} else {
|
||||
this->orientation_reset_count = 0;
|
||||
}
|
||||
|
||||
if ((gps_est_error > GPS_POS_ERROR_RESET_THRESHOLD && ecef_pos_std < GPS_POS_STD_RESET_THRESHOLD) || this->last_gps_msg == 0) {
|
||||
// always reset on first gps message and if the location is off but the accuracy is high
|
||||
LOGE("Locationd vs gnssMeasurement position difference too large, kalman reset");
|
||||
this->reset_kalman(NAN, initial_pose_ecef_quat, ecef_pos, ecef_vel, ecef_pos_R, ecef_vel_R);
|
||||
} else if (orientation_reset_count > GPS_ORIENTATION_ERROR_RESET_CNT) {
|
||||
LOGE("Locationd vs gnssMeasurement orientation difference too large, kalman reset");
|
||||
this->reset_kalman(NAN, initial_pose_ecef_quat, ecef_pos, ecef_vel, ecef_pos_R, ecef_vel_R);
|
||||
this->kf->predict_and_observe(sensor_time, OBSERVATION_ECEF_ORIENTATION_FROM_GPS, { initial_pose_ecef_quat });
|
||||
this->orientation_reset_count = 0;
|
||||
}
|
||||
|
||||
this->gps_mode = true;
|
||||
this->last_gps_msg = sensor_time;
|
||||
this->kf->predict_and_observe(sensor_time, OBSERVATION_ECEF_POS, { ecef_pos }, { ecef_pos_R });
|
||||
this->kf->predict_and_observe(sensor_time, OBSERVATION_ECEF_VEL, { ecef_vel }, { ecef_vel_R });
|
||||
}
|
||||
|
||||
void Localizer::handle_car_state(double current_time, const cereal::CarState::Reader& log) {
|
||||
this->car_speed = std::abs(log.getVEgo());
|
||||
this->standstill = log.getStandstill();
|
||||
if (this->standstill) {
|
||||
this->kf->predict_and_observe(current_time, OBSERVATION_NO_ROT, { Vector3d(0.0, 0.0, 0.0) });
|
||||
this->kf->predict_and_observe(current_time, OBSERVATION_NO_ACCEL, { Vector3d(0.0, 0.0, 0.0) });
|
||||
}
|
||||
}
|
||||
|
||||
void Localizer::handle_cam_odo(double current_time, const cereal::CameraOdometry::Reader& log) {
|
||||
VectorXd rot_device = this->device_from_calib * floatlist2vector(log.getRot());
|
||||
VectorXd trans_device = this->device_from_calib * floatlist2vector(log.getTrans());
|
||||
|
||||
if (!this->is_timestamp_valid(current_time)) {
|
||||
this->observation_timings_invalid = true;
|
||||
return;
|
||||
}
|
||||
|
||||
if ((rot_device.norm() > ROTATION_SANITY_CHECK) || (trans_device.norm() > TRANS_SANITY_CHECK)) {
|
||||
this->observation_values_invalid["cameraOdometry"] += 1.0;
|
||||
return;
|
||||
}
|
||||
|
||||
VectorXd rot_calib_std = floatlist2vector(log.getRotStd());
|
||||
VectorXd trans_calib_std = floatlist2vector(log.getTransStd());
|
||||
|
||||
if ((rot_calib_std.minCoeff() <= MIN_STD_SANITY_CHECK) || (trans_calib_std.minCoeff() <= MIN_STD_SANITY_CHECK)) {
|
||||
this->observation_values_invalid["cameraOdometry"] += 1.0;
|
||||
return;
|
||||
}
|
||||
|
||||
if ((rot_calib_std.norm() > 10 * ROTATION_SANITY_CHECK) || (trans_calib_std.norm() > 10 * TRANS_SANITY_CHECK)) {
|
||||
this->observation_values_invalid["cameraOdometry"] += 1.0;
|
||||
return;
|
||||
}
|
||||
|
||||
this->posenet_stds.pop_front();
|
||||
this->posenet_stds.push_back(trans_calib_std[0]);
|
||||
|
||||
// Multiply by 10 to avoid to high certainty in kalman filter because of temporally correlated noise
|
||||
trans_calib_std *= 10.0;
|
||||
rot_calib_std *= 10.0;
|
||||
MatrixXdr rot_device_cov = rotate_std(this->device_from_calib, rot_calib_std).array().square().matrix().asDiagonal();
|
||||
MatrixXdr trans_device_cov = rotate_std(this->device_from_calib, trans_calib_std).array().square().matrix().asDiagonal();
|
||||
this->kf->predict_and_observe(current_time, OBSERVATION_CAMERA_ODO_ROTATION,
|
||||
{ rot_device }, { rot_device_cov });
|
||||
this->kf->predict_and_observe(current_time, OBSERVATION_CAMERA_ODO_TRANSLATION,
|
||||
{ trans_device }, { trans_device_cov });
|
||||
this->observation_values_invalid["cameraOdometry"] *= DECAY;
|
||||
this->camodo_yawrate_distribution = Vector2d(rot_device[2], rotate_std(this->device_from_calib, rot_calib_std)[2]);
|
||||
}
|
||||
|
||||
void Localizer::handle_live_calib(double current_time, const cereal::ExtrinsicsCalibration::Reader& log) {
|
||||
if (!this->is_timestamp_valid(current_time)) {
|
||||
this->observation_timings_invalid = true;
|
||||
return;
|
||||
}
|
||||
|
||||
if (log.getRpyCalib().size() > 0) {
|
||||
auto live_calib = floatlist2vector(log.getRpyCalib());
|
||||
if ((live_calib.minCoeff() < -CALIB_RPY_SANITY_CHECK) || (live_calib.maxCoeff() > CALIB_RPY_SANITY_CHECK)) {
|
||||
this->observation_values_invalid["extrinsicsCalibration"] += 1.0;
|
||||
return;
|
||||
}
|
||||
|
||||
this->calib = live_calib;
|
||||
this->device_from_calib = euler2rot(this->calib);
|
||||
this->calib_from_device = this->device_from_calib.transpose();
|
||||
this->calibrated = log.getCalStatus() == cereal::ExtrinsicsCalibration::Status::CALIBRATED;
|
||||
this->observation_values_invalid["extrinsicsCalibration"] *= DECAY;
|
||||
}
|
||||
}
|
||||
|
||||
void Localizer::reset_kalman(double current_time) {
|
||||
const VectorXd &init_x = this->kf->get_initial_x();
|
||||
const MatrixXdr &init_P = this->kf->get_initial_P();
|
||||
this->reset_kalman(current_time, init_x, init_P);
|
||||
}
|
||||
|
||||
void Localizer::finite_check(double current_time) {
|
||||
bool all_finite = this->kf->get_x().array().isFinite().all() or this->kf->get_P().array().isFinite().all();
|
||||
if (!all_finite) {
|
||||
LOGE("Non-finite values detected, kalman reset");
|
||||
this->reset_kalman(current_time);
|
||||
}
|
||||
}
|
||||
|
||||
void Localizer::time_check(double current_time) {
|
||||
if (std::isnan(this->last_reset_time)) {
|
||||
this->last_reset_time = current_time;
|
||||
}
|
||||
if (std::isnan(this->first_valid_log_time)) {
|
||||
this->first_valid_log_time = current_time;
|
||||
}
|
||||
double filter_time = this->kf->get_filter_time();
|
||||
bool big_time_gap = !std::isnan(filter_time) && (current_time - filter_time > 10);
|
||||
if (big_time_gap) {
|
||||
LOGE("Time gap of over 10s detected, kalman reset");
|
||||
this->reset_kalman(current_time);
|
||||
}
|
||||
}
|
||||
|
||||
void Localizer::update_reset_tracker() {
|
||||
// reset tracker is tuned to trigger when over 1reset/10s over 2min period
|
||||
if (this->is_gps_ok()) {
|
||||
this->reset_tracker *= RESET_TRACKER_DECAY;
|
||||
} else {
|
||||
this->reset_tracker = 0.0;
|
||||
}
|
||||
}
|
||||
|
||||
void Localizer::reset_kalman(double current_time, const VectorXd &init_orient, const VectorXd &init_pos, const VectorXd &init_vel, const MatrixXdr &init_pos_R, const MatrixXdr &init_vel_R) {
|
||||
// too nonlinear to init on completely wrong
|
||||
VectorXd current_x = this->kf->get_x();
|
||||
MatrixXdr current_P = this->kf->get_P();
|
||||
MatrixXdr init_P = this->kf->get_initial_P();
|
||||
const MatrixXdr &reset_orientation_P = this->kf->get_reset_orientation_P();
|
||||
int non_ecef_state_err_len = init_P.rows() - (STATE_ECEF_POS_ERR_LEN + STATE_ECEF_ORIENTATION_ERR_LEN + STATE_ECEF_VELOCITY_ERR_LEN);
|
||||
|
||||
current_x.segment<STATE_ECEF_ORIENTATION_LEN>(STATE_ECEF_ORIENTATION_START) = init_orient;
|
||||
current_x.segment<STATE_ECEF_VELOCITY_LEN>(STATE_ECEF_VELOCITY_START) = init_vel;
|
||||
current_x.segment<STATE_ECEF_POS_LEN>(STATE_ECEF_POS_START) = init_pos;
|
||||
|
||||
init_P.block<STATE_ECEF_POS_ERR_LEN, STATE_ECEF_POS_ERR_LEN>(STATE_ECEF_POS_ERR_START, STATE_ECEF_POS_ERR_START).diagonal() = init_pos_R.diagonal();
|
||||
init_P.block<STATE_ECEF_ORIENTATION_ERR_LEN, STATE_ECEF_ORIENTATION_ERR_LEN>(STATE_ECEF_ORIENTATION_ERR_START, STATE_ECEF_ORIENTATION_ERR_START).diagonal() = reset_orientation_P.diagonal();
|
||||
init_P.block<STATE_ECEF_VELOCITY_ERR_LEN, STATE_ECEF_VELOCITY_ERR_LEN>(STATE_ECEF_VELOCITY_ERR_START, STATE_ECEF_VELOCITY_ERR_START).diagonal() = init_vel_R.diagonal();
|
||||
init_P.block(STATE_ANGULAR_VELOCITY_ERR_START, STATE_ANGULAR_VELOCITY_ERR_START, non_ecef_state_err_len, non_ecef_state_err_len).diagonal() = current_P.block(STATE_ANGULAR_VELOCITY_ERR_START,
|
||||
STATE_ANGULAR_VELOCITY_ERR_START, non_ecef_state_err_len, non_ecef_state_err_len).diagonal();
|
||||
|
||||
this->reset_kalman(current_time, current_x, init_P);
|
||||
}
|
||||
|
||||
void Localizer::reset_kalman(double current_time, const VectorXd &init_x, const MatrixXdr &init_P) {
|
||||
this->kf->init_state(init_x, init_P, current_time);
|
||||
this->last_reset_time = current_time;
|
||||
this->reset_tracker += 1.0;
|
||||
}
|
||||
|
||||
void Localizer::handle_msg_bytes(const char *data, const size_t size) {
|
||||
AlignedBuffer aligned_buf;
|
||||
|
||||
capnp::FlatArrayMessageReader cmsg(aligned_buf.align(data, size));
|
||||
cereal::Event::Reader event = cmsg.getRoot<cereal::Event>();
|
||||
|
||||
this->handle_msg(event);
|
||||
}
|
||||
|
||||
void Localizer::handle_msg(const cereal::Event::Reader& log) {
|
||||
double t = log.getLogMonoTime() * 1e-9;
|
||||
this->time_check(t);
|
||||
if (log.isAccelerometer()) {
|
||||
this->handle_sensor(t, log.getAccelerometer());
|
||||
} else if (log.isGyroscope()) {
|
||||
this->handle_sensor(t, log.getGyroscope());
|
||||
} else if (log.isGpsLocation()) {
|
||||
this->handle_gps(t, log.getGpsLocation(), GPS_QUECTEL_SENSOR_TIME_OFFSET);
|
||||
} else if (log.isGpsLocationExternal()) {
|
||||
this->handle_gps(t, log.getGpsLocationExternal(), GPS_UBLOX_SENSOR_TIME_OFFSET);
|
||||
//} else if (log.isGnssMeasurements()) {
|
||||
// this->handle_gnss(t, log.getGnssMeasurements());
|
||||
} else if (log.isCarState()) {
|
||||
this->handle_car_state(t, log.getCarState());
|
||||
} else if (log.isCameraOdometry()) {
|
||||
this->handle_cam_odo(t, log.getCameraOdometry());
|
||||
} else if (log.isExtrinsicsCalibration()) {
|
||||
this->handle_live_calib(t, log.getExtrinsicsCalibration());
|
||||
}
|
||||
this->finite_check();
|
||||
this->update_reset_tracker();
|
||||
}
|
||||
|
||||
kj::ArrayPtr<capnp::byte> Localizer::get_message_bytes(MessageBuilder& msg_builder, bool inputsOK,
|
||||
bool sensorsOK, bool gpsOK, bool msgValid) {
|
||||
cereal::Event::Builder evt = msg_builder.initEvent();
|
||||
evt.setValid(msgValid);
|
||||
cereal::LiveLocationKalman::Builder liveLoc = evt.initLiveLocationKalman();
|
||||
this->build_live_location(liveLoc);
|
||||
liveLoc.setSensorsOK(sensorsOK);
|
||||
liveLoc.setGpsOK(gpsOK);
|
||||
liveLoc.setInputsOK(inputsOK);
|
||||
return msg_builder.toBytes();
|
||||
}
|
||||
|
||||
bool Localizer::is_gps_ok() {
|
||||
return (this->kf->get_filter_time() - this->last_gps_msg) < 2.0;
|
||||
}
|
||||
|
||||
bool Localizer::critical_services_valid(const std::map<std::string, double> &critical_services) {
|
||||
for (auto &kv : critical_services){
|
||||
if (kv.second >= INPUT_INVALID_THRESHOLD){
|
||||
return false;
|
||||
}
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
bool Localizer::is_timestamp_valid(double current_time) {
|
||||
double filter_time = this->kf->get_filter_time();
|
||||
if (!std::isnan(filter_time) && ((filter_time - current_time) > MAX_FILTER_REWIND_TIME)) {
|
||||
LOGE("Observation timestamp is older than the max rewind threshold of the filter");
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
void Localizer::determine_gps_mode(double current_time) {
|
||||
// 1. If the pos_std is greater than what's not acceptable and localizer is in gps-mode, reset to no-gps-mode
|
||||
// 2. If the pos_std is greater than what's not acceptable and localizer is in no-gps-mode, fake obs
|
||||
// 3. If the pos_std is smaller than what's not acceptable, let gps-mode be whatever it is
|
||||
VectorXd current_pos_std = this->kf->get_P().block<STATE_ECEF_POS_ERR_LEN, STATE_ECEF_POS_ERR_LEN>(STATE_ECEF_POS_ERR_START, STATE_ECEF_POS_ERR_START).diagonal().array().sqrt();
|
||||
if (current_pos_std.norm() > SANE_GPS_UNCERTAINTY){
|
||||
if (this->gps_mode){
|
||||
this->gps_mode = false;
|
||||
this->reset_kalman(current_time);
|
||||
} else {
|
||||
this->input_fake_gps_observations(current_time);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void Localizer::configure_gnss_source(const LocalizerGnssSource &source) {
|
||||
this->gnss_source = source;
|
||||
if (source == LocalizerGnssSource::UBLOX) {
|
||||
this->gps_std_factor = 10.0;
|
||||
this->gps_variance_factor = 1.0;
|
||||
this->gps_vertical_variance_factor = 1.0;
|
||||
this->gps_time_offset = GPS_UBLOX_SENSOR_TIME_OFFSET;
|
||||
} else {
|
||||
this->gps_std_factor = 2.0;
|
||||
this->gps_variance_factor = 0.0;
|
||||
this->gps_vertical_variance_factor = 3.0;
|
||||
this->gps_time_offset = GPS_QUECTEL_SENSOR_TIME_OFFSET;
|
||||
}
|
||||
}
|
||||
|
||||
int Localizer::locationd_thread() {
|
||||
Params params;
|
||||
LocalizerGnssSource source;
|
||||
const char* gps_location_socket;
|
||||
if (params.getBool("UbloxAvailable")) {
|
||||
source = LocalizerGnssSource::UBLOX;
|
||||
gps_location_socket = "gpsLocationExternal";
|
||||
} else {
|
||||
source = LocalizerGnssSource::QCOM;
|
||||
gps_location_socket = "gpsLocation";
|
||||
}
|
||||
|
||||
this->configure_gnss_source(source);
|
||||
const std::initializer_list<const char *> service_list = {gps_location_socket, "cameraOdometry", "extrinsicsCalibration",
|
||||
"carState", "accelerometer", "gyroscope"};
|
||||
|
||||
SubMaster sm(service_list, {}, nullptr, {gps_location_socket});
|
||||
PubMaster pm({"liveLocationKalman"});
|
||||
|
||||
uint64_t cnt = 0;
|
||||
bool filterInitialized = false;
|
||||
const std::vector<std::string> critical_input_services = {"cameraOdometry", "extrinsicsCalibration", "accelerometer", "gyroscope"};
|
||||
for (std::string service : critical_input_services) {
|
||||
this->observation_values_invalid.insert({service, 0.0});
|
||||
}
|
||||
|
||||
while (!do_exit) {
|
||||
sm.update();
|
||||
if (filterInitialized){
|
||||
this->observation_timings_invalid_reset();
|
||||
for (const char* service : service_list) {
|
||||
if (sm.updated(service) && sm.valid(service)){
|
||||
const cereal::Event::Reader log = sm[service];
|
||||
this->handle_msg(log);
|
||||
}
|
||||
}
|
||||
} else {
|
||||
filterInitialized = sm.allAliveAndValid();
|
||||
}
|
||||
|
||||
const char* trigger_msg = "cameraOdometry";
|
||||
if (sm.updated(trigger_msg)) {
|
||||
bool inputsOK = sm.allValid() && this->are_inputs_ok();
|
||||
bool gpsOK = this->is_gps_ok();
|
||||
bool sensorsOK = sm.allAliveAndValid({"accelerometer", "gyroscope"});
|
||||
|
||||
// Log time to first fix
|
||||
if (gpsOK && std::isnan(this->ttff) && !std::isnan(this->first_valid_log_time)) {
|
||||
this->ttff = std::max(1e-3, (sm[trigger_msg].getLogMonoTime() * 1e-9) - this->first_valid_log_time);
|
||||
}
|
||||
|
||||
MessageBuilder msg_builder;
|
||||
kj::ArrayPtr<capnp::byte> bytes = this->get_message_bytes(msg_builder, inputsOK, sensorsOK, gpsOK, filterInitialized);
|
||||
pm.send("liveLocationKalman", bytes.begin(), bytes.size());
|
||||
|
||||
if (cnt % 1200 == 0 && gpsOK) { // once a minute
|
||||
VectorXd posGeo = this->get_position_geodetic();
|
||||
std::string lastGPSPosJSON = util::string_format(
|
||||
"{\"latitude\": %.15f, \"longitude\": %.15f, \"altitude\": %.15f}", posGeo(0), posGeo(1), posGeo(2));
|
||||
params.putNonBlocking("LastGPSPositionLLK", lastGPSPosJSON);
|
||||
}
|
||||
cnt++;
|
||||
}
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
int main() {
|
||||
util::set_realtime_priority(5);
|
||||
|
||||
Localizer localizer;
|
||||
return localizer.locationd_thread();
|
||||
}
|
||||
@@ -0,0 +1,100 @@
|
||||
#pragma once
|
||||
|
||||
#include <eigen3/Eigen/Dense>
|
||||
#include <deque>
|
||||
#include <fstream>
|
||||
#include <memory>
|
||||
#include <map>
|
||||
#include <string>
|
||||
|
||||
#include "cereal/messaging/messaging.h"
|
||||
#include "common/params.h"
|
||||
#include "common/swaglog.h"
|
||||
#include "common/timing.h"
|
||||
#include "common/util.h"
|
||||
|
||||
#include "sunnypilot/common/transformations/coordinates.hpp"
|
||||
#include "sunnypilot/common/transformations/orientation.hpp"
|
||||
#include "sunnypilot/system/sensord/sensors/constants.h"
|
||||
#include "sunnypilot/selfdrive/locationd/models/live_kf.h"
|
||||
|
||||
#define VISION_DECIMATION 2
|
||||
#define SENSOR_DECIMATION 10
|
||||
#define POSENET_STD_HIST_HALF 20
|
||||
|
||||
enum LocalizerGnssSource {
|
||||
UBLOX, QCOM
|
||||
};
|
||||
|
||||
class Localizer {
|
||||
public:
|
||||
Localizer(LocalizerGnssSource gnss_source = LocalizerGnssSource::UBLOX);
|
||||
|
||||
int locationd_thread();
|
||||
|
||||
void reset_kalman(double current_time = NAN);
|
||||
void reset_kalman(double current_time, const Eigen::VectorXd &init_orient, const Eigen::VectorXd &init_pos, const Eigen::VectorXd &init_vel, const MatrixXdr &init_pos_R, const MatrixXdr &init_vel_R);
|
||||
void reset_kalman(double current_time, const Eigen::VectorXd &init_x, const MatrixXdr &init_P);
|
||||
void finite_check(double current_time = NAN);
|
||||
void time_check(double current_time = NAN);
|
||||
void update_reset_tracker();
|
||||
bool is_gps_ok();
|
||||
bool critical_services_valid(const std::map<std::string, double> &critical_services);
|
||||
bool is_timestamp_valid(double current_time);
|
||||
void determine_gps_mode(double current_time);
|
||||
bool are_inputs_ok();
|
||||
void observation_timings_invalid_reset();
|
||||
|
||||
kj::ArrayPtr<capnp::byte> get_message_bytes(MessageBuilder& msg_builder,
|
||||
bool inputsOK, bool sensorsOK, bool gpsOK, bool msgValid);
|
||||
void build_live_location(cereal::LiveLocationKalman::Builder& fix);
|
||||
|
||||
Eigen::VectorXd get_position_geodetic();
|
||||
Eigen::VectorXd get_state();
|
||||
Eigen::VectorXd get_stdev();
|
||||
|
||||
void handle_msg_bytes(const char *data, const size_t size);
|
||||
void handle_msg(const cereal::Event::Reader& log);
|
||||
void handle_sensor(double current_time, const cereal::SensorEventData::Reader& log);
|
||||
void handle_gps(double current_time, const cereal::GpsLocationData::Reader& log, const double sensor_time_offset);
|
||||
void handle_gnss(double current_time, const cereal::GnssMeasurements::Reader& log);
|
||||
void handle_car_state(double current_time, const cereal::CarState::Reader& log);
|
||||
void handle_cam_odo(double current_time, const cereal::CameraOdometry::Reader& log);
|
||||
void handle_live_calib(double current_time, const cereal::ExtrinsicsCalibration::Reader& log);
|
||||
|
||||
void input_fake_gps_observations(double current_time);
|
||||
|
||||
private:
|
||||
std::unique_ptr<LiveKalman> kf;
|
||||
|
||||
Eigen::VectorXd calib;
|
||||
MatrixXdr device_from_calib;
|
||||
MatrixXdr calib_from_device;
|
||||
bool calibrated = false;
|
||||
|
||||
double car_speed = 0.0;
|
||||
double last_reset_time = NAN;
|
||||
std::deque<double> posenet_stds;
|
||||
|
||||
std::unique_ptr<LocalCoord> converter;
|
||||
|
||||
int64_t unix_timestamp_millis = 0;
|
||||
double reset_tracker = 0.0;
|
||||
bool device_fell = false;
|
||||
bool gps_mode = false;
|
||||
double first_valid_log_time = NAN;
|
||||
double ttff = NAN;
|
||||
double last_gps_msg = 0;
|
||||
LocalizerGnssSource gnss_source;
|
||||
bool observation_timings_invalid = false;
|
||||
std::map<std::string, double> observation_values_invalid;
|
||||
bool standstill = true;
|
||||
int32_t orientation_reset_count = 0;
|
||||
float gps_std_factor;
|
||||
float gps_variance_factor;
|
||||
float gps_vertical_variance_factor;
|
||||
double gps_time_offset;
|
||||
Eigen::VectorXd camodo_yawrate_distribution = Eigen::Vector2d(0.0, 10.0); // mean, std
|
||||
|
||||
void configure_gnss_source(const LocalizerGnssSource &source);
|
||||
};
|
||||
@@ -0,0 +1 @@
|
||||
generated/
|
||||
+180
@@ -0,0 +1,180 @@
|
||||
#!/usr/bin/env python3
|
||||
import math
|
||||
import sys
|
||||
from typing import Any
|
||||
|
||||
import numpy as np
|
||||
|
||||
from openpilot.common.constants import ACCELERATION_DUE_TO_GRAVITY
|
||||
from openpilot.sunnypilot.selfdrive.locationd.models.constants import ObservationKind
|
||||
from openpilot.common.swaglog import cloudlog
|
||||
|
||||
from rednose.helpers.kalmanfilter import KalmanFilter
|
||||
|
||||
if __name__ == '__main__': # Generating sympy
|
||||
import sympy as sp
|
||||
from rednose.helpers.ekf_sym import gen_code
|
||||
else:
|
||||
from rednose.helpers.ekf_sym_pyx import EKF_sym_pyx
|
||||
|
||||
|
||||
i = 0
|
||||
|
||||
def _slice(n):
|
||||
global i
|
||||
s = slice(i, i + n)
|
||||
i += n
|
||||
|
||||
return s
|
||||
|
||||
|
||||
class States:
|
||||
# Vehicle model params
|
||||
STIFFNESS = _slice(1) # [-]
|
||||
STEER_RATIO = _slice(1) # [-]
|
||||
ANGLE_OFFSET = _slice(1) # [rad]
|
||||
ANGLE_OFFSET_FAST = _slice(1) # [rad]
|
||||
|
||||
VELOCITY = _slice(2) # (x, y) [m/s]
|
||||
YAW_RATE = _slice(1) # [rad/s]
|
||||
STEER_ANGLE = _slice(1) # [rad]
|
||||
ROAD_ROLL = _slice(1) # [rad]
|
||||
|
||||
|
||||
class CarKalman(KalmanFilter):
|
||||
name = 'car'
|
||||
|
||||
initial_x = np.array([
|
||||
1.0,
|
||||
15.0,
|
||||
0.0,
|
||||
0.0,
|
||||
|
||||
10.0, 0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0
|
||||
])
|
||||
|
||||
# process noise
|
||||
Q = np.diag([
|
||||
(.05 / 100)**2,
|
||||
.01**2,
|
||||
math.radians(0.02)**2,
|
||||
math.radians(0.25)**2,
|
||||
|
||||
.1**2, .01**2,
|
||||
math.radians(0.1)**2,
|
||||
math.radians(0.1)**2,
|
||||
math.radians(1)**2,
|
||||
])
|
||||
P_initial = Q.copy()
|
||||
|
||||
obs_noise: dict[int, Any] = {
|
||||
ObservationKind.STEER_ANGLE: np.atleast_2d(math.radians(0.05)**2),
|
||||
ObservationKind.ANGLE_OFFSET_FAST: np.atleast_2d(math.radians(10.0)**2),
|
||||
ObservationKind.ROAD_ROLL: np.atleast_2d(math.radians(1.0)**2),
|
||||
ObservationKind.STEER_RATIO: np.atleast_2d(5.0**2),
|
||||
ObservationKind.STIFFNESS: np.atleast_2d(0.5**2),
|
||||
ObservationKind.ROAD_FRAME_X_SPEED: np.atleast_2d(0.1**2),
|
||||
}
|
||||
|
||||
global_vars = [
|
||||
'mass',
|
||||
'rotational_inertia',
|
||||
'center_to_front',
|
||||
'center_to_rear',
|
||||
'stiffness_front',
|
||||
'stiffness_rear',
|
||||
]
|
||||
|
||||
@staticmethod
|
||||
def generate_code(generated_dir):
|
||||
dim_state = CarKalman.initial_x.shape[0]
|
||||
name = CarKalman.name
|
||||
|
||||
# Linearized single-track lateral dynamics, equations 7.211-7.213
|
||||
# Massimo Guiggiani, The Science of Vehicle Dynamics: Handling, Braking, and Ride of Road and Race Cars
|
||||
# Springer Cham, 2023. doi: https://doi.org/10.1007/978-3-031-06461-6
|
||||
|
||||
# globals
|
||||
global_vars = [sp.Symbol(name) for name in CarKalman.global_vars]
|
||||
m, j, aF, aR, cF_orig, cR_orig = global_vars
|
||||
|
||||
# make functions and jacobians with sympy
|
||||
# state variables
|
||||
state_sym = sp.MatrixSymbol('state', dim_state, 1)
|
||||
state = sp.Matrix(state_sym)
|
||||
|
||||
# Vehicle model constants
|
||||
sf = state[States.STIFFNESS, :][0, 0]
|
||||
|
||||
cF, cR = sf * cF_orig, sf * cR_orig
|
||||
angle_offset = state[States.ANGLE_OFFSET, :][0, 0]
|
||||
angle_offset_fast = state[States.ANGLE_OFFSET_FAST, :][0, 0]
|
||||
theta = state[States.ROAD_ROLL, :][0, 0]
|
||||
sa = state[States.STEER_ANGLE, :][0, 0]
|
||||
|
||||
sR = state[States.STEER_RATIO, :][0, 0]
|
||||
u, v = state[States.VELOCITY, :]
|
||||
r = state[States.YAW_RATE, :][0, 0]
|
||||
|
||||
A = sp.Matrix(np.zeros((2, 2)))
|
||||
A[0, 0] = -(cF + cR) / (m * u)
|
||||
A[0, 1] = -(cF * aF - cR * aR) / (m * u) - u
|
||||
A[1, 0] = -(cF * aF - cR * aR) / (j * u)
|
||||
A[1, 1] = -(cF * aF**2 + cR * aR**2) / (j * u)
|
||||
|
||||
B = sp.Matrix(np.zeros((2, 1)))
|
||||
B[0, 0] = cF / m / sR
|
||||
B[1, 0] = (cF * aF) / j / sR
|
||||
|
||||
C = sp.Matrix(np.zeros((2, 1)))
|
||||
C[0, 0] = ACCELERATION_DUE_TO_GRAVITY
|
||||
C[1, 0] = 0
|
||||
|
||||
x = sp.Matrix([v, r]) # lateral velocity, yaw rate
|
||||
x_dot = A * x + B * (sa - angle_offset - angle_offset_fast) - C * theta
|
||||
|
||||
dt = sp.Symbol('dt')
|
||||
state_dot = sp.Matrix(np.zeros((dim_state, 1)))
|
||||
state_dot[States.VELOCITY.start + 1, 0] = x_dot[0]
|
||||
state_dot[States.YAW_RATE.start, 0] = x_dot[1]
|
||||
|
||||
# Basic descretization, 1st order integrator
|
||||
# Can be pretty bad if dt is big
|
||||
f_sym = state + dt * state_dot
|
||||
|
||||
#
|
||||
# Observation functions
|
||||
#
|
||||
obs_eqs = [
|
||||
[sp.Matrix([r]), ObservationKind.ROAD_FRAME_YAW_RATE, None],
|
||||
[sp.Matrix([u, v]), ObservationKind.ROAD_FRAME_XY_SPEED, None],
|
||||
[sp.Matrix([u]), ObservationKind.ROAD_FRAME_X_SPEED, None],
|
||||
[sp.Matrix([sa]), ObservationKind.STEER_ANGLE, None],
|
||||
[sp.Matrix([angle_offset_fast]), ObservationKind.ANGLE_OFFSET_FAST, None],
|
||||
[sp.Matrix([sR]), ObservationKind.STEER_RATIO, None],
|
||||
[sp.Matrix([sf]), ObservationKind.STIFFNESS, None],
|
||||
[sp.Matrix([theta]), ObservationKind.ROAD_ROLL, None],
|
||||
]
|
||||
|
||||
gen_code(generated_dir, name, f_sym, dt, state_sym, obs_eqs, dim_state, dim_state, global_vars=global_vars)
|
||||
|
||||
def __init__(self, generated_dir):
|
||||
dim_state, dim_state_err = CarKalman.initial_x.shape[0], CarKalman.P_initial.shape[0]
|
||||
self.filter = EKF_sym_pyx(generated_dir, CarKalman.name, CarKalman.Q, CarKalman.initial_x, CarKalman.P_initial,
|
||||
dim_state, dim_state_err, global_vars=CarKalman.global_vars, logger=cloudlog)
|
||||
|
||||
def set_globals(self, mass, rotational_inertia, center_to_front, center_to_rear, stiffness_front, stiffness_rear):
|
||||
self.filter.set_global("mass", mass)
|
||||
self.filter.set_global("rotational_inertia", rotational_inertia)
|
||||
self.filter.set_global("center_to_front", center_to_front)
|
||||
self.filter.set_global("center_to_rear", center_to_rear)
|
||||
self.filter.set_global("stiffness_front", stiffness_front)
|
||||
self.filter.set_global("stiffness_rear", stiffness_rear)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
generated_dir = sys.argv[2]
|
||||
CarKalman.generate_code(generated_dir)
|
||||
@@ -0,0 +1,92 @@
|
||||
import os
|
||||
|
||||
GENERATED_DIR = os.path.abspath(os.path.join(os.path.dirname(__file__), 'generated'))
|
||||
|
||||
class ObservationKind:
|
||||
UNKNOWN = 0
|
||||
NO_OBSERVATION = 1
|
||||
GPS_NED = 2
|
||||
ODOMETRIC_SPEED = 3
|
||||
PHONE_GYRO = 4
|
||||
GPS_VEL = 5
|
||||
PSEUDORANGE_GPS = 6
|
||||
PSEUDORANGE_RATE_GPS = 7
|
||||
SPEED = 8
|
||||
NO_ROT = 9
|
||||
PHONE_ACCEL = 10
|
||||
ORB_POINT = 11
|
||||
ECEF_POS = 12
|
||||
CAMERA_ODO_TRANSLATION = 13
|
||||
CAMERA_ODO_ROTATION = 14
|
||||
ORB_FEATURES = 15
|
||||
MSCKF_TEST = 16
|
||||
FEATURE_TRACK_TEST = 17
|
||||
LANE_PT = 18
|
||||
IMU_FRAME = 19
|
||||
PSEUDORANGE_GLONASS = 20
|
||||
PSEUDORANGE_RATE_GLONASS = 21
|
||||
PSEUDORANGE = 22
|
||||
PSEUDORANGE_RATE = 23
|
||||
ECEF_VEL = 35
|
||||
ECEF_ORIENTATION_FROM_GPS = 32
|
||||
NO_ACCEL = 33
|
||||
ORB_FEATURES_WIDE = 34
|
||||
|
||||
ROAD_FRAME_XY_SPEED = 24 # (x, y) [m/s]
|
||||
ROAD_FRAME_YAW_RATE = 25 # [rad/s]
|
||||
STEER_ANGLE = 26 # [rad]
|
||||
ANGLE_OFFSET_FAST = 27 # [rad]
|
||||
STIFFNESS = 28 # [-]
|
||||
STEER_RATIO = 29 # [-]
|
||||
ROAD_FRAME_X_SPEED = 30 # (x) [m/s]
|
||||
ROAD_ROLL = 31 # [rad]
|
||||
|
||||
names = [
|
||||
'Unknown',
|
||||
'No observation',
|
||||
'GPS NED',
|
||||
'Odometric speed',
|
||||
'Phone gyro',
|
||||
'GPS velocity',
|
||||
'GPS pseudorange',
|
||||
'GPS pseudorange rate',
|
||||
'Speed',
|
||||
'No rotation',
|
||||
'Phone acceleration',
|
||||
'ORB point',
|
||||
'ECEF pos',
|
||||
'camera odometric translation',
|
||||
'camera odometric rotation',
|
||||
'ORB features',
|
||||
'MSCKF test',
|
||||
'Feature track test',
|
||||
'Lane ecef point',
|
||||
'imu frame eulers',
|
||||
'GLONASS pseudorange',
|
||||
'GLONASS pseudorange rate',
|
||||
'pseudorange',
|
||||
'pseudorange rate',
|
||||
|
||||
'Road Frame x,y speed',
|
||||
'Road Frame yaw rate',
|
||||
'Steer Angle',
|
||||
'Fast Angle Offset',
|
||||
'Stiffness',
|
||||
'Steer Ratio',
|
||||
'Road Frame x speed',
|
||||
'Road Roll',
|
||||
'ECEF orientation from GPS',
|
||||
'NO accel',
|
||||
'ORB features wide camera',
|
||||
'ECEF_VEL',
|
||||
]
|
||||
|
||||
@classmethod
|
||||
def to_string(cls, kind):
|
||||
return cls.names[kind]
|
||||
|
||||
|
||||
SAT_OBS = [ObservationKind.PSEUDORANGE_GPS,
|
||||
ObservationKind.PSEUDORANGE_RATE_GPS,
|
||||
ObservationKind.PSEUDORANGE_GLONASS,
|
||||
ObservationKind.PSEUDORANGE_RATE_GLONASS]
|
||||
@@ -0,0 +1,122 @@
|
||||
#include "sunnypilot/selfdrive/locationd/models/live_kf.h"
|
||||
|
||||
using namespace EKFS;
|
||||
using namespace Eigen;
|
||||
|
||||
Eigen::Map<Eigen::VectorXd> get_mapvec(const Eigen::VectorXd &vec) {
|
||||
return Eigen::Map<Eigen::VectorXd>((double*)vec.data(), vec.rows(), vec.cols());
|
||||
}
|
||||
|
||||
Eigen::Map<MatrixXdr> get_mapmat(const MatrixXdr &mat) {
|
||||
return Eigen::Map<MatrixXdr>((double*)mat.data(), mat.rows(), mat.cols());
|
||||
}
|
||||
|
||||
std::vector<Eigen::Map<Eigen::VectorXd>> get_vec_mapvec(const std::vector<Eigen::VectorXd> &vec_vec) {
|
||||
std::vector<Eigen::Map<Eigen::VectorXd>> res;
|
||||
for (const Eigen::VectorXd &vec : vec_vec) {
|
||||
res.push_back(get_mapvec(vec));
|
||||
}
|
||||
return res;
|
||||
}
|
||||
|
||||
std::vector<Eigen::Map<MatrixXdr>> get_vec_mapmat(const std::vector<MatrixXdr> &mat_vec) {
|
||||
std::vector<Eigen::Map<MatrixXdr>> res;
|
||||
for (const MatrixXdr &mat : mat_vec) {
|
||||
res.push_back(get_mapmat(mat));
|
||||
}
|
||||
return res;
|
||||
}
|
||||
|
||||
LiveKalman::LiveKalman() {
|
||||
this->dim_state = live_initial_x.rows();
|
||||
this->dim_state_err = live_initial_P_diag.rows();
|
||||
|
||||
this->initial_x = live_initial_x;
|
||||
this->initial_P = live_initial_P_diag.asDiagonal();
|
||||
this->fake_gps_pos_cov = live_fake_gps_pos_cov_diag.asDiagonal();
|
||||
this->fake_gps_vel_cov = live_fake_gps_vel_cov_diag.asDiagonal();
|
||||
this->reset_orientation_P = live_reset_orientation_diag.asDiagonal();
|
||||
this->Q = live_Q_diag.asDiagonal();
|
||||
for (auto& pair : live_obs_noise_diag) {
|
||||
this->obs_noise[pair.first] = pair.second.asDiagonal();
|
||||
}
|
||||
|
||||
// init filter
|
||||
this->filter = std::make_shared<EKFSym>(this->name, get_mapmat(this->Q), get_mapvec(this->initial_x),
|
||||
get_mapmat(initial_P), this->dim_state, this->dim_state_err, 0, 0, 0, std::vector<int>(),
|
||||
std::vector<int>{3}, std::vector<std::string>(), 0.8);
|
||||
}
|
||||
|
||||
void LiveKalman::init_state(const VectorXd &state, const VectorXd &covs_diag, double filter_time) {
|
||||
MatrixXdr covs = covs_diag.asDiagonal();
|
||||
this->filter->init_state(get_mapvec(state), get_mapmat(covs), filter_time);
|
||||
}
|
||||
|
||||
void LiveKalman::init_state(const VectorXd &state, const MatrixXdr &covs, double filter_time) {
|
||||
this->filter->init_state(get_mapvec(state), get_mapmat(covs), filter_time);
|
||||
}
|
||||
|
||||
void LiveKalman::init_state(const VectorXd &state, double filter_time) {
|
||||
MatrixXdr covs = this->filter->covs();
|
||||
this->filter->init_state(get_mapvec(state), get_mapmat(covs), filter_time);
|
||||
}
|
||||
|
||||
VectorXd LiveKalman::get_x() {
|
||||
return this->filter->state();
|
||||
}
|
||||
|
||||
MatrixXdr LiveKalman::get_P() {
|
||||
return this->filter->covs();
|
||||
}
|
||||
|
||||
double LiveKalman::get_filter_time() {
|
||||
return this->filter->get_filter_time();
|
||||
}
|
||||
|
||||
std::vector<MatrixXdr> LiveKalman::get_R(int kind, int n) {
|
||||
std::vector<MatrixXdr> R;
|
||||
for (int i = 0; i < n; i++) {
|
||||
R.push_back(this->obs_noise[kind]);
|
||||
}
|
||||
return R;
|
||||
}
|
||||
|
||||
std::optional<Estimate> LiveKalman::predict_and_observe(double t, int kind, const std::vector<VectorXd> &meas, std::vector<MatrixXdr> R) {
|
||||
std::optional<Estimate> r;
|
||||
if (R.size() == 0) {
|
||||
R = this->get_R(kind, meas.size());
|
||||
}
|
||||
r = this->filter->predict_and_update_batch(t, kind, get_vec_mapvec(meas), get_vec_mapmat(R));
|
||||
return r;
|
||||
}
|
||||
|
||||
void LiveKalman::predict(double t) {
|
||||
this->filter->predict(t);
|
||||
}
|
||||
|
||||
const Eigen::VectorXd &LiveKalman::get_initial_x() {
|
||||
return this->initial_x;
|
||||
}
|
||||
|
||||
const MatrixXdr &LiveKalman::get_initial_P() {
|
||||
return this->initial_P;
|
||||
}
|
||||
|
||||
const MatrixXdr &LiveKalman::get_fake_gps_pos_cov() {
|
||||
return this->fake_gps_pos_cov;
|
||||
}
|
||||
|
||||
const MatrixXdr &LiveKalman::get_fake_gps_vel_cov() {
|
||||
return this->fake_gps_vel_cov;
|
||||
}
|
||||
|
||||
const MatrixXdr &LiveKalman::get_reset_orientation_P() {
|
||||
return this->reset_orientation_P;
|
||||
}
|
||||
|
||||
MatrixXdr LiveKalman::H(const VectorXd &in) {
|
||||
assert(in.size() == 6);
|
||||
Matrix<double, 3, 6, Eigen::RowMajor> res;
|
||||
this->filter->get_extra_routine("H")((double*)in.data(), res.data());
|
||||
return res;
|
||||
}
|
||||
@@ -0,0 +1,66 @@
|
||||
#pragma once
|
||||
|
||||
#include <string>
|
||||
#include <cmath>
|
||||
#include <memory>
|
||||
#include <unordered_map>
|
||||
#include <vector>
|
||||
|
||||
#include <eigen3/Eigen/Core>
|
||||
#include <eigen3/Eigen/Dense>
|
||||
|
||||
#include "generated/live_kf_constants.h"
|
||||
#include "rednose/helpers/ekf_sym.h"
|
||||
|
||||
#define EARTH_GM 3.986005e14 // m^3/s^2 (gravitational constant * mass of earth)
|
||||
|
||||
using namespace EKFS;
|
||||
|
||||
Eigen::Map<Eigen::VectorXd> get_mapvec(const Eigen::VectorXd &vec);
|
||||
Eigen::Map<MatrixXdr> get_mapmat(const MatrixXdr &mat);
|
||||
std::vector<Eigen::Map<Eigen::VectorXd>> get_vec_mapvec(const std::vector<Eigen::VectorXd> &vec_vec);
|
||||
std::vector<Eigen::Map<MatrixXdr>> get_vec_mapmat(const std::vector<MatrixXdr> &mat_vec);
|
||||
|
||||
class LiveKalman {
|
||||
public:
|
||||
LiveKalman();
|
||||
|
||||
void init_state(const Eigen::VectorXd &state, const Eigen::VectorXd &covs_diag, double filter_time);
|
||||
void init_state(const Eigen::VectorXd &state, const MatrixXdr &covs, double filter_time);
|
||||
void init_state(const Eigen::VectorXd &state, double filter_time);
|
||||
|
||||
Eigen::VectorXd get_x();
|
||||
MatrixXdr get_P();
|
||||
double get_filter_time();
|
||||
std::vector<MatrixXdr> get_R(int kind, int n);
|
||||
|
||||
std::optional<Estimate> predict_and_observe(double t, int kind, const std::vector<Eigen::VectorXd> &meas, std::vector<MatrixXdr> R = {});
|
||||
std::optional<Estimate> predict_and_update_odo_speed(std::vector<Eigen::VectorXd> speed, double t, int kind);
|
||||
std::optional<Estimate> predict_and_update_odo_trans(std::vector<Eigen::VectorXd> trans, double t, int kind);
|
||||
std::optional<Estimate> predict_and_update_odo_rot(std::vector<Eigen::VectorXd> rot, double t, int kind);
|
||||
void predict(double t);
|
||||
|
||||
const Eigen::VectorXd &get_initial_x();
|
||||
const MatrixXdr &get_initial_P();
|
||||
const MatrixXdr &get_fake_gps_pos_cov();
|
||||
const MatrixXdr &get_fake_gps_vel_cov();
|
||||
const MatrixXdr &get_reset_orientation_P();
|
||||
|
||||
MatrixXdr H(const Eigen::VectorXd &in);
|
||||
|
||||
private:
|
||||
std::string name = "live";
|
||||
|
||||
std::shared_ptr<EKFSym> filter;
|
||||
|
||||
int dim_state;
|
||||
int dim_state_err;
|
||||
|
||||
Eigen::VectorXd initial_x;
|
||||
MatrixXdr initial_P;
|
||||
MatrixXdr fake_gps_pos_cov;
|
||||
MatrixXdr fake_gps_vel_cov;
|
||||
MatrixXdr reset_orientation_P;
|
||||
MatrixXdr Q; // process noise
|
||||
std::unordered_map<int, MatrixXdr> obs_noise;
|
||||
};
|
||||
+242
@@ -0,0 +1,242 @@
|
||||
#!/usr/bin/env python3
|
||||
|
||||
import sys
|
||||
import os
|
||||
import numpy as np
|
||||
|
||||
from openpilot.sunnypilot.selfdrive.locationd.models.constants import ObservationKind
|
||||
|
||||
import sympy as sp
|
||||
import inspect
|
||||
from rednose.helpers.sympy_helpers import euler_rotate, quat_matrix_r, quat_rotate
|
||||
from rednose.helpers.ekf_sym import gen_code
|
||||
|
||||
EARTH_GM = 3.986005e14 # m^3/s^2 (gravitational constant * mass of earth)
|
||||
|
||||
|
||||
def numpy2eigenstring(arr):
|
||||
assert(len(arr.shape) == 1)
|
||||
arr_str = np.array2string(arr, precision=20, separator=',')[1:-1].replace(' ', '').replace('\n', '')
|
||||
return f"(Eigen::VectorXd({len(arr)}) << {arr_str}).finished()"
|
||||
|
||||
|
||||
class States:
|
||||
ECEF_POS = slice(0, 3) # x, y and z in ECEF in meters
|
||||
ECEF_ORIENTATION = slice(3, 7) # quat for pose of phone in ecef
|
||||
ECEF_VELOCITY = slice(7, 10) # ecef velocity in m/s
|
||||
ANGULAR_VELOCITY = slice(10, 13) # roll, pitch and yaw rates in device frame in radians/s
|
||||
GYRO_BIAS = slice(13, 16) # roll, pitch and yaw biases
|
||||
ACCELERATION = slice(16, 19) # Acceleration in device frame in m/s**2
|
||||
ACC_BIAS = slice(19, 22) # Acceletometer bias in m/s**2
|
||||
|
||||
# Error-state has different slices because it is an ESKF
|
||||
ECEF_POS_ERR = slice(0, 3)
|
||||
ECEF_ORIENTATION_ERR = slice(3, 6) # euler angles for orientation error
|
||||
ECEF_VELOCITY_ERR = slice(6, 9)
|
||||
ANGULAR_VELOCITY_ERR = slice(9, 12)
|
||||
GYRO_BIAS_ERR = slice(12, 15)
|
||||
ACCELERATION_ERR = slice(15, 18)
|
||||
ACC_BIAS_ERR = slice(18, 21)
|
||||
|
||||
|
||||
class LiveKalman:
|
||||
name = 'live'
|
||||
|
||||
initial_x = np.array([3.88e6, -3.37e6, 3.76e6,
|
||||
0.42254641, -0.31238054, -0.83602975, -0.15788347, # NED [0,0,0] -> ECEF Quat
|
||||
0, 0, 0,
|
||||
0, 0, 0,
|
||||
0, 0, 0,
|
||||
0, 0, 0,
|
||||
0, 0, 0])
|
||||
|
||||
# state covariance
|
||||
initial_P_diag = np.array([10**2, 10**2, 10**2,
|
||||
0.01**2, 0.01**2, 0.01**2,
|
||||
10**2, 10**2, 10**2,
|
||||
1**2, 1**2, 1**2,
|
||||
1**2, 1**2, 1**2,
|
||||
100**2, 100**2, 100**2,
|
||||
0.01**2, 0.01**2, 0.01**2])
|
||||
|
||||
# state covariance when resetting midway in a segment
|
||||
reset_orientation_diag = np.array([1**2, 1**2, 1**2])
|
||||
|
||||
# fake observation covariance, to ensure the uncertainty estimate of the filter is under control
|
||||
fake_gps_pos_cov_diag = np.array([1000**2, 1000**2, 1000**2])
|
||||
fake_gps_vel_cov_diag = np.array([10**2, 10**2, 10**2])
|
||||
|
||||
# process noise
|
||||
Q_diag = np.array([0.03**2, 0.03**2, 0.03**2,
|
||||
0.001**2, 0.001**2, 0.001**2,
|
||||
0.01**2, 0.01**2, 0.01**2,
|
||||
0.1**2, 0.1**2, 0.1**2,
|
||||
(0.005 / 100)**2, (0.005 / 100)**2, (0.005 / 100)**2,
|
||||
3**2, 3**2, 3**2,
|
||||
0.005**2, 0.005**2, 0.005**2])
|
||||
|
||||
obs_noise_diag = {ObservationKind.PHONE_GYRO: np.array([0.025**2, 0.025**2, 0.025**2]),
|
||||
ObservationKind.PHONE_ACCEL: np.array([.5**2, .5**2, .5**2]),
|
||||
ObservationKind.CAMERA_ODO_ROTATION: np.array([0.05**2, 0.05**2, 0.05**2]),
|
||||
ObservationKind.NO_ROT: np.array([0.005**2, 0.005**2, 0.005**2]),
|
||||
ObservationKind.NO_ACCEL: np.array([0.05**2, 0.05**2, 0.05**2]),
|
||||
ObservationKind.ECEF_POS: np.array([5**2, 5**2, 5**2]),
|
||||
ObservationKind.ECEF_VEL: np.array([.5**2, .5**2, .5**2]),
|
||||
ObservationKind.ECEF_ORIENTATION_FROM_GPS: np.array([.2**2, .2**2, .2**2, .2**2])}
|
||||
|
||||
@staticmethod
|
||||
def generate_code(generated_dir):
|
||||
name = LiveKalman.name
|
||||
dim_state = LiveKalman.initial_x.shape[0]
|
||||
dim_state_err = LiveKalman.initial_P_diag.shape[0]
|
||||
|
||||
state_sym = sp.MatrixSymbol('state', dim_state, 1)
|
||||
state = sp.Matrix(state_sym)
|
||||
x, y, z = state[States.ECEF_POS, :]
|
||||
q = state[States.ECEF_ORIENTATION, :]
|
||||
v = state[States.ECEF_VELOCITY, :]
|
||||
vx, vy, vz = v
|
||||
omega = state[States.ANGULAR_VELOCITY, :]
|
||||
vroll, vpitch, vyaw = omega
|
||||
roll_bias, pitch_bias, yaw_bias = state[States.GYRO_BIAS, :]
|
||||
acceleration = state[States.ACCELERATION, :]
|
||||
acc_bias = state[States.ACC_BIAS, :]
|
||||
|
||||
dt = sp.Symbol('dt')
|
||||
|
||||
# calibration and attitude rotation matrices
|
||||
quat_rot = quat_rotate(*q)
|
||||
|
||||
# Got the quat predict equations from here
|
||||
# A New Quaternion-Based Kalman Filter for
|
||||
# Real-Time Attitude Estimation Using the Two-Step
|
||||
# Geometrically-Intuitive Correction Algorithm
|
||||
A = 0.5 * sp.Matrix([[0, -vroll, -vpitch, -vyaw],
|
||||
[vroll, 0, vyaw, -vpitch],
|
||||
[vpitch, -vyaw, 0, vroll],
|
||||
[vyaw, vpitch, -vroll, 0]])
|
||||
q_dot = A * q
|
||||
|
||||
# Time derivative of the state as a function of state
|
||||
state_dot = sp.Matrix(np.zeros((dim_state, 1)))
|
||||
state_dot[States.ECEF_POS, :] = v
|
||||
state_dot[States.ECEF_ORIENTATION, :] = q_dot
|
||||
state_dot[States.ECEF_VELOCITY, 0] = quat_rot * acceleration
|
||||
|
||||
# Basic descretization, 1st order intergrator
|
||||
# Can be pretty bad if dt is big
|
||||
f_sym = state + dt * state_dot
|
||||
|
||||
state_err_sym = sp.MatrixSymbol('state_err', dim_state_err, 1)
|
||||
state_err = sp.Matrix(state_err_sym)
|
||||
quat_err = state_err[States.ECEF_ORIENTATION_ERR, :]
|
||||
v_err = state_err[States.ECEF_VELOCITY_ERR, :]
|
||||
omega_err = state_err[States.ANGULAR_VELOCITY_ERR, :]
|
||||
acceleration_err = state_err[States.ACCELERATION_ERR, :]
|
||||
|
||||
# Time derivative of the state error as a function of state error and state
|
||||
quat_err_matrix = euler_rotate(quat_err[0], quat_err[1], quat_err[2])
|
||||
q_err_dot = quat_err_matrix * quat_rot * (omega + omega_err)
|
||||
state_err_dot = sp.Matrix(np.zeros((dim_state_err, 1)))
|
||||
state_err_dot[States.ECEF_POS_ERR, :] = v_err
|
||||
state_err_dot[States.ECEF_ORIENTATION_ERR, :] = q_err_dot
|
||||
state_err_dot[States.ECEF_VELOCITY_ERR, :] = quat_err_matrix * quat_rot * (acceleration + acceleration_err)
|
||||
f_err_sym = state_err + dt * state_err_dot
|
||||
|
||||
# Observation matrix modifier
|
||||
H_mod_sym = sp.Matrix(np.zeros((dim_state, dim_state_err)))
|
||||
H_mod_sym[States.ECEF_POS, States.ECEF_POS_ERR] = np.eye(States.ECEF_POS.stop - States.ECEF_POS.start)
|
||||
H_mod_sym[States.ECEF_ORIENTATION, States.ECEF_ORIENTATION_ERR] = 0.5 * quat_matrix_r(state[3:7])[:, 1:]
|
||||
H_mod_sym[States.ECEF_ORIENTATION.stop:, States.ECEF_ORIENTATION_ERR.stop:] = np.eye(dim_state - States.ECEF_ORIENTATION.stop)
|
||||
|
||||
# these error functions are defined so that say there
|
||||
# is a nominal x and true x:
|
||||
# true x = err_function(nominal x, delta x)
|
||||
# delta x = inv_err_function(nominal x, true x)
|
||||
nom_x = sp.MatrixSymbol('nom_x', dim_state, 1)
|
||||
true_x = sp.MatrixSymbol('true_x', dim_state, 1)
|
||||
delta_x = sp.MatrixSymbol('delta_x', dim_state_err, 1)
|
||||
|
||||
err_function_sym = sp.Matrix(np.zeros((dim_state, 1)))
|
||||
delta_quat = sp.Matrix(np.ones(4))
|
||||
delta_quat[1:, :] = sp.Matrix(0.5 * delta_x[States.ECEF_ORIENTATION_ERR, :])
|
||||
err_function_sym[States.ECEF_POS, :] = sp.Matrix(nom_x[States.ECEF_POS, :] + delta_x[States.ECEF_POS_ERR, :])
|
||||
err_function_sym[States.ECEF_ORIENTATION, 0] = quat_matrix_r(nom_x[States.ECEF_ORIENTATION, 0]) * delta_quat
|
||||
err_function_sym[States.ECEF_ORIENTATION.stop:, :] = sp.Matrix(nom_x[States.ECEF_ORIENTATION.stop:, :] + delta_x[States.ECEF_ORIENTATION_ERR.stop:, :])
|
||||
|
||||
inv_err_function_sym = sp.Matrix(np.zeros((dim_state_err, 1)))
|
||||
inv_err_function_sym[States.ECEF_POS_ERR, 0] = sp.Matrix(-nom_x[States.ECEF_POS, 0] + true_x[States.ECEF_POS, 0])
|
||||
delta_quat = quat_matrix_r(nom_x[States.ECEF_ORIENTATION, 0]).T * true_x[States.ECEF_ORIENTATION, 0]
|
||||
inv_err_function_sym[States.ECEF_ORIENTATION_ERR, 0] = sp.Matrix(2 * delta_quat[1:])
|
||||
inv_err_function_sym[States.ECEF_ORIENTATION_ERR.stop:, 0] = sp.Matrix(-nom_x[States.ECEF_ORIENTATION.stop:, 0] + true_x[States.ECEF_ORIENTATION.stop:, 0])
|
||||
|
||||
eskf_params = [[err_function_sym, nom_x, delta_x],
|
||||
[inv_err_function_sym, nom_x, true_x],
|
||||
H_mod_sym, f_err_sym, state_err_sym]
|
||||
#
|
||||
# Observation functions
|
||||
#
|
||||
h_gyro_sym = sp.Matrix([
|
||||
vroll + roll_bias,
|
||||
vpitch + pitch_bias,
|
||||
vyaw + yaw_bias])
|
||||
|
||||
pos = sp.Matrix([x, y, z])
|
||||
gravity = quat_rot.T * ((EARTH_GM / ((x**2 + y**2 + z**2)**(3.0 / 2.0))) * pos)
|
||||
h_acc_sym = (gravity + acceleration + acc_bias)
|
||||
h_acc_stationary_sym = acceleration
|
||||
h_phone_rot_sym = sp.Matrix([vroll, vpitch, vyaw])
|
||||
h_pos_sym = sp.Matrix([x, y, z])
|
||||
h_vel_sym = sp.Matrix([vx, vy, vz])
|
||||
h_orientation_sym = q
|
||||
h_relative_motion = sp.Matrix(quat_rot.T * v)
|
||||
|
||||
obs_eqs = [[h_gyro_sym, ObservationKind.PHONE_GYRO, None],
|
||||
[h_phone_rot_sym, ObservationKind.NO_ROT, None],
|
||||
[h_acc_sym, ObservationKind.PHONE_ACCEL, None],
|
||||
[h_pos_sym, ObservationKind.ECEF_POS, None],
|
||||
[h_vel_sym, ObservationKind.ECEF_VEL, None],
|
||||
[h_orientation_sym, ObservationKind.ECEF_ORIENTATION_FROM_GPS, None],
|
||||
[h_relative_motion, ObservationKind.CAMERA_ODO_TRANSLATION, None],
|
||||
[h_phone_rot_sym, ObservationKind.CAMERA_ODO_ROTATION, None],
|
||||
[h_acc_stationary_sym, ObservationKind.NO_ACCEL, None]]
|
||||
|
||||
# this returns a sympy routine for the jacobian of the observation function of the local vel
|
||||
in_vec = sp.MatrixSymbol('in_vec', 6, 1) # roll, pitch, yaw, vx, vy, vz
|
||||
h = euler_rotate(in_vec[0], in_vec[1], in_vec[2]).T * (sp.Matrix([in_vec[3], in_vec[4], in_vec[5]]))
|
||||
extra_routines = [('H', h.jacobian(in_vec), [in_vec])]
|
||||
|
||||
gen_code(generated_dir, name, f_sym, dt, state_sym, obs_eqs, dim_state, dim_state_err, eskf_params, extra_routines=extra_routines)
|
||||
|
||||
# write constants to extra header file for use in cpp
|
||||
live_kf_header = "#pragma once\n\n"
|
||||
live_kf_header += "#include <unordered_map>\n"
|
||||
live_kf_header += "#include <eigen3/Eigen/Dense>\n\n"
|
||||
for state, slc in inspect.getmembers(States, lambda x: isinstance(x, slice)):
|
||||
assert(slc.step is None) # unsupported
|
||||
live_kf_header += f'#define STATE_{state}_START {slc.start}\n'
|
||||
live_kf_header += f'#define STATE_{state}_END {slc.stop}\n'
|
||||
live_kf_header += f'#define STATE_{state}_LEN {slc.stop - slc.start}\n'
|
||||
live_kf_header += "\n"
|
||||
|
||||
for kind, val in inspect.getmembers(ObservationKind, lambda x: isinstance(x, int)):
|
||||
live_kf_header += f'#define OBSERVATION_{kind} {val}\n'
|
||||
live_kf_header += "\n"
|
||||
|
||||
live_kf_header += f"static const Eigen::VectorXd live_initial_x = {numpy2eigenstring(LiveKalman.initial_x)};\n"
|
||||
live_kf_header += f"static const Eigen::VectorXd live_initial_P_diag = {numpy2eigenstring(LiveKalman.initial_P_diag)};\n"
|
||||
live_kf_header += f"static const Eigen::VectorXd live_fake_gps_pos_cov_diag = {numpy2eigenstring(LiveKalman.fake_gps_pos_cov_diag)};\n"
|
||||
live_kf_header += f"static const Eigen::VectorXd live_fake_gps_vel_cov_diag = {numpy2eigenstring(LiveKalman.fake_gps_vel_cov_diag)};\n"
|
||||
live_kf_header += f"static const Eigen::VectorXd live_reset_orientation_diag = {numpy2eigenstring(LiveKalman.reset_orientation_diag)};\n"
|
||||
live_kf_header += f"static const Eigen::VectorXd live_Q_diag = {numpy2eigenstring(LiveKalman.Q_diag)};\n"
|
||||
live_kf_header += "static const std::unordered_map<int, Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>> live_obs_noise_diag = {\n"
|
||||
for kind, noise in LiveKalman.obs_noise_diag.items():
|
||||
live_kf_header += f" {{ {kind}, {numpy2eigenstring(noise)} }},\n"
|
||||
live_kf_header += "};\n\n"
|
||||
|
||||
open(os.path.join(generated_dir, "live_kf_constants.h"), 'w').write(live_kf_header)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
generated_dir = sys.argv[2]
|
||||
LiveKalman.generate_code(generated_dir)
|
||||
@@ -0,0 +1 @@
|
||||
out/
|
||||
@@ -1,73 +0,0 @@
|
||||
"""
|
||||
Copyright (c) 2021-, sunnypilot, and a number of other contributors.
|
||||
|
||||
This file is part of sunnypilot and is licensed under the MIT License.
|
||||
See the LICENSE.md file in the root directory for more details.
|
||||
"""
|
||||
import math
|
||||
from types import SimpleNamespace
|
||||
|
||||
from openpilot.common.test import OpenpilotTestCase
|
||||
from openpilot.sunnypilot.selfdrive.locationd.gps_fuser import EARTH_RADIUS, GPS_LATENCY, GpsFuser
|
||||
|
||||
LAT0, LON0 = 39.0, -77.0
|
||||
LATENCY = GPS_LATENCY['gpsLocation']
|
||||
|
||||
|
||||
def fix(north, east, speed=30.0, bearing=90.0, h_acc=0.0, v_acc=1.5):
|
||||
return SimpleNamespace(latitude=LAT0 + math.degrees(north / EARTH_RADIUS),
|
||||
longitude=LON0 + math.degrees(east / (EARTH_RADIUS * math.cos(math.radians(LAT0)))),
|
||||
speed=speed, bearingDeg=bearing, horizontalAccuracy=h_acc, verticalAccuracy=v_acc,
|
||||
speedAccuracy=0.3, bearingAccuracyDeg=0.5, hasFix=True)
|
||||
|
||||
|
||||
def position(f: GpsFuser) -> tuple[float, float]:
|
||||
north = math.radians(f.lat - LAT0) * EARTH_RADIUS
|
||||
east = math.radians(f.lon - LON0) * EARTH_RADIUS * math.cos(math.radians(LAT0))
|
||||
return north, east
|
||||
|
||||
|
||||
class TestGpsFuser(OpenpilotTestCase):
|
||||
def drive(self, f, seconds, speed=30.0, start=0.0, gps=lambda t, true_east: fix(0.0, true_east)):
|
||||
t = start
|
||||
while t < start + seconds:
|
||||
t = round(t + 0.05, 2)
|
||||
f.predict(t, speed, 0.0)
|
||||
if abs(t - round(t)) < 1e-6:
|
||||
f.update_gps(t, gps(t, speed * (t - LATENCY)), LATENCY)
|
||||
return t
|
||||
|
||||
def test_compensates_latency(self):
|
||||
f = GpsFuser()
|
||||
t = self.drive(f, 30)
|
||||
self.assertTrue(f.valid)
|
||||
north, east = position(f)
|
||||
self.assertAlmostEqual(east, 30.0 * t, delta=3.0)
|
||||
self.assertAlmostEqual(north, 0.0, delta=1.0)
|
||||
|
||||
def test_rejects_single_jump(self):
|
||||
f = GpsFuser()
|
||||
t = self.drive(f, 20)
|
||||
f.predict(t + 0.05, 30.0, 0.0)
|
||||
f.update_gps(t + 0.05, fix(40.0, 30.0 * (t + 0.05 - LATENCY)), LATENCY)
|
||||
self.assertAlmostEqual(position(f)[0], 0.0, delta=2.0)
|
||||
|
||||
def test_follows_persistent_offset(self):
|
||||
f = GpsFuser()
|
||||
t = self.drive(f, 20)
|
||||
self.drive(f, 10, start=t, gps=lambda _, e: fix(150.0, e))
|
||||
self.assertAlmostEqual(position(f)[0], 150.0, delta=3.0)
|
||||
|
||||
def test_ignores_bearing_when_slow(self):
|
||||
f = GpsFuser()
|
||||
t = self.drive(f, 20)
|
||||
self.drive(f, 5, speed=1.0, start=t, gps=lambda _, e: fix(0.0, e, speed=1.0, bearing=270.0))
|
||||
self.assertAlmostEqual(math.degrees(f.heading), 90.0, delta=1.0)
|
||||
|
||||
def test_invalid_after_long_outage(self):
|
||||
f = GpsFuser()
|
||||
t = self.drive(f, 20)
|
||||
self.drive(f, 120, start=t, gps=lambda _, e: SimpleNamespace(hasFix=False, horizontalAccuracy=0.0, verticalAccuracy=0.0,
|
||||
speedAccuracy=0.0, latitude=0.0, longitude=0.0))
|
||||
self.assertFalse(f.valid)
|
||||
|
||||
@@ -0,0 +1,96 @@
|
||||
import platform
|
||||
import unittest
|
||||
import json
|
||||
import random
|
||||
import time
|
||||
import capnp
|
||||
|
||||
import openpilot.cereal.messaging as messaging
|
||||
from openpilot.cereal.services import SERVICE_LIST
|
||||
from openpilot.common.params import Params
|
||||
from openpilot.common.transformations.coordinates import ecef2geodetic
|
||||
|
||||
from openpilot.system.manager.process_config import managed_processes
|
||||
from openpilot.common.test import OpenpilotTestCase
|
||||
|
||||
|
||||
@unittest.skipIf(platform.system() == 'Darwin', "msgq unsupported on macOS")
|
||||
class TestLocationdProc(OpenpilotTestCase):
|
||||
LLD_MSGS = ['gpsLocationExternal', 'cameraOdometry', 'carState', 'extrinsicsCalibration',
|
||||
'accelerometer', 'gyroscope']
|
||||
|
||||
def setup_method(self):
|
||||
self.pm = messaging.PubMaster(self.LLD_MSGS)
|
||||
|
||||
self.params = Params()
|
||||
self.params.put_bool("UbloxAvailable", True)
|
||||
managed_processes['locationd_llk'].start()
|
||||
|
||||
def teardown_method(self):
|
||||
managed_processes['locationd_llk'].stop()
|
||||
|
||||
def get_msg(self, name, t):
|
||||
try:
|
||||
msg = messaging.new_message(name)
|
||||
except capnp.lib.capnp.KjException:
|
||||
msg = messaging.new_message(name, 0)
|
||||
|
||||
if name == "gpsLocationExternal":
|
||||
msg.gpsLocationExternal.flags = 1
|
||||
msg.gpsLocationExternal.hasFix = True
|
||||
msg.gpsLocationExternal.verticalAccuracy = 1.0
|
||||
msg.gpsLocationExternal.speedAccuracy = 1.0
|
||||
msg.gpsLocationExternal.bearingAccuracyDeg = 1.0
|
||||
msg.gpsLocationExternal.vNED = [0.0, 0.0, 0.0]
|
||||
msg.gpsLocationExternal.latitude = float(self.lat)
|
||||
msg.gpsLocationExternal.longitude = float(self.lon)
|
||||
msg.gpsLocationExternal.unixTimestampMillis = t * 1e6
|
||||
msg.gpsLocationExternal.altitude = float(self.alt)
|
||||
#if name == "gnssMeasurements":
|
||||
# msg.gnssMeasurements.measTime = t
|
||||
# msg.gnssMeasurements.positionECEF.value = [self.x , self.y, self.z]
|
||||
# msg.gnssMeasurements.positionECEF.std = [0,0,0]
|
||||
# msg.gnssMeasurements.positionECEF.valid = True
|
||||
# msg.gnssMeasurements.velocityECEF.value = []
|
||||
# msg.gnssMeasurements.velocityECEF.std = [0,0,0]
|
||||
# msg.gnssMeasurements.velocityECEF.valid = True
|
||||
elif name == 'cameraOdometry':
|
||||
msg.cameraOdometry.rot = [0.0, 0.0, 0.0]
|
||||
msg.cameraOdometry.rotStd = [0.0, 0.0, 0.0]
|
||||
msg.cameraOdometry.trans = [0.0, 0.0, 0.0]
|
||||
msg.cameraOdometry.transStd = [0.0, 0.0, 0.0]
|
||||
msg.logMonoTime = t
|
||||
msg.valid = True
|
||||
return msg
|
||||
|
||||
def test_params_gps(self):
|
||||
random.seed(123489234)
|
||||
self.params.remove('LastGPSPositionLLK')
|
||||
|
||||
self.x = -2710700 + (random.random() * 1e5)
|
||||
self.y = -4280600 + (random.random() * 1e5)
|
||||
self.z = 3850300 + (random.random() * 1e5)
|
||||
self.lat, self.lon, self.alt = ecef2geodetic([self.x, self.y, self.z])
|
||||
|
||||
# get fake messages at the correct frequency, listed in services.py
|
||||
msgs = []
|
||||
for sec in range(65):
|
||||
for name in self.LLD_MSGS:
|
||||
for j in range(int(SERVICE_LIST[name].frequency)):
|
||||
msgs.append(self.get_msg(name, int((sec + j / SERVICE_LIST[name].frequency) * 1e9)))
|
||||
|
||||
for msg in sorted(msgs, key=lambda x: x.logMonoTime):
|
||||
self.pm.send(msg.which(), msg)
|
||||
if msg.which() == "cameraOdometry":
|
||||
self.pm.wait_for_readers_to_update(msg.which(), timeout=1, dt=0.005)
|
||||
for _ in range(50):
|
||||
val = self.params.get('LastGPSPositionLLK')
|
||||
if val is not None:
|
||||
break
|
||||
time.sleep(0.1)
|
||||
|
||||
self.assertIsNotNone(val, "LastGPSPositionLLK not written within 5s")
|
||||
lastGPS = json.loads(val)
|
||||
self.assertAlmostEqual(lastGPS['latitude'], self.lat, delta=0.001)
|
||||
self.assertAlmostEqual(lastGPS['longitude'], self.lon, delta=0.001)
|
||||
self.assertAlmostEqual(lastGPS['altitude'], self.alt, delta=0.001)
|
||||
@@ -32,7 +32,7 @@ from openpilot.sunnypilot.models.model_name import DEFAULT_MODEL, DEFAULT_BIG_MO
|
||||
from openpilot.sunnypilot.selfdrive.car.sync_sunnylink_params import update_car_list_param
|
||||
from openpilot.sunnypilot.sunnylink.api import SunnylinkApi
|
||||
from openpilot.sunnypilot.sunnylink.utils import sunnylink_need_register, sunnylink_ready, get_param_as_byte, save_param_from_base64_encoded_string
|
||||
from openpilot.sunnypilot.sunnylink.capabilities import generate_capabilities, CAPABILITY_LABELS, CAR_CAPABILITY_FIELDS
|
||||
from openpilot.sunnypilot.sunnylink.capabilities import generate_capabilities, CAPABILITY_LABELS
|
||||
from openpilot.sunnypilot.sunnylink.tools.generate_settings_schema import generate_schema
|
||||
|
||||
SUNNYLINK_ATHENA_HOST = os.getenv('SUNNYLINK_ATHENA_HOST', 'wss://athena.sunnylink.ai')
|
||||
@@ -181,7 +181,6 @@ def getParamsMetadata() -> str:
|
||||
schema = generate_schema()
|
||||
schema["capabilities"] = generate_capabilities()
|
||||
schema["capability_labels"] = CAPABILITY_LABELS
|
||||
schema["car_capability_fields"] = CAR_CAPABILITY_FIELDS
|
||||
schema["default_model"] = DEFAULT_MODEL
|
||||
schema["default_big_model"] = DEFAULT_BIG_MODEL
|
||||
schema["chestnut_active"] = params.get_bool("ChestnutActive")
|
||||
|
||||
@@ -11,6 +11,7 @@ from opendbc.car.structs import car
|
||||
from opendbc.car.hyundai.values import CAR as HYUNDAI_CAR, UNSUPPORTED_LONGITUDINAL_CAR
|
||||
from opendbc.car.subaru.values import CAR as SUBARU_CAR, SubaruFlags
|
||||
from opendbc.car.toyota.values import CAR as TOYOTA_CAR, ToyotaFlags
|
||||
from opendbc.sunnypilot.car.hyundai.values import HyundaiFlagsSP
|
||||
from opendbc.sunnypilot.car.interfaces import has_bsm
|
||||
from opendbc.sunnypilot.car.tesla.values import TeslaFlagsSP
|
||||
from openpilot.common.params import Params
|
||||
@@ -44,6 +45,7 @@ CAPABILITY_FIELDS = (
|
||||
"device_type",
|
||||
"subaru_has_sng",
|
||||
"hyundai_alpha_long_available",
|
||||
"hyundai_has_custom_button",
|
||||
"toyota_tss2",
|
||||
"custom_model_active",
|
||||
)
|
||||
@@ -68,29 +70,11 @@ CAPABILITY_LABELS: dict[str, str] = {
|
||||
"device_type": "Device type",
|
||||
"subaru_has_sng": "Subaru Stop-and-Go available",
|
||||
"hyundai_alpha_long_available": "Hyundai Alpha Longitudinal available",
|
||||
"hyundai_has_custom_button": "Hyundai steering wheel custom button",
|
||||
"toyota_tss2": "Toyota TSS2 platform",
|
||||
"custom_model_active": "Non-Default model selected",
|
||||
}
|
||||
|
||||
# Fields read from the car (CarParams*, CarPlatformBundle). Until a car is seen these stay at
|
||||
# their defaults, so a rule failing on one of them means "start the vehicle", not a real reason.
|
||||
CAR_CAPABILITY_FIELDS = (
|
||||
"has_longitudinal_control",
|
||||
"has_icbm",
|
||||
"icbm_available",
|
||||
"torque_allowed",
|
||||
"brand",
|
||||
"pcm_cruise",
|
||||
"alpha_long_available",
|
||||
"steer_control_type",
|
||||
"has_bsm",
|
||||
"tesla_has_vehicle_bus",
|
||||
"has_stop_and_go",
|
||||
"subaru_has_sng",
|
||||
"hyundai_alpha_long_available",
|
||||
"toyota_tss2",
|
||||
)
|
||||
|
||||
# Explicit defaults for non-boolean capability fields
|
||||
CAPABILITY_DEFAULTS: dict[str, bool | str | int] = {
|
||||
"brand": "",
|
||||
@@ -211,6 +195,7 @@ def generate_capabilities(params: Params | None = None) -> dict:
|
||||
caps["icbm_available"] = bool(CP_SP.intelligentCruiseButtonManagementAvailable)
|
||||
caps["has_icbm"] = bool(CP_SP.intelligentCruiseButtonManagementAvailable) and params.get_bool("IntelligentCruiseButtonManagement")
|
||||
caps["tesla_has_vehicle_bus"] = bool(CP_SP.flags & TeslaFlagsSP.HAS_VEHICLE_BUS)
|
||||
caps["hyundai_has_custom_button"] = caps["brand"] == "hyundai" and bool(CP_SP.flags & HyundaiFlagsSP.HAS_CUSTOM_BUTTON)
|
||||
except Exception:
|
||||
cloudlog.exception("capabilities: failed to deserialize CarParamsSPPersistent")
|
||||
|
||||
|
||||
@@ -620,19 +620,6 @@
|
||||
}
|
||||
]
|
||||
},
|
||||
{
|
||||
"key": "AccelBoostSP",
|
||||
"widget": "toggle",
|
||||
"title": "Acceleration Boost",
|
||||
"description": "Adds a small push on top of sunnypilot's own acceleration, stronger in Aggressive and gentler in Relaxed. Eases off in curves and when sunnypilot slows or stops. Never goes past your set speed, and lead-car following still comes first.",
|
||||
"enablement": [
|
||||
{
|
||||
"type": "capability",
|
||||
"field": "has_longitudinal_control",
|
||||
"equals": true
|
||||
}
|
||||
]
|
||||
},
|
||||
{
|
||||
"key": "IntelligentCruiseButtonManagement",
|
||||
"widget": "toggle",
|
||||
@@ -1362,17 +1349,7 @@
|
||||
"key": "TorqueBar",
|
||||
"widget": "toggle",
|
||||
"title": "Steering Arc",
|
||||
"description": "Display steering arc on the driving screen when lateral control is enabled.",
|
||||
"visibility": [
|
||||
{
|
||||
"type": "not",
|
||||
"condition": {
|
||||
"type": "capability",
|
||||
"field": "device_type",
|
||||
"equals": "mici"
|
||||
}
|
||||
}
|
||||
]
|
||||
"description": "Display steering arc on the driving screen when lateral control is enabled."
|
||||
},
|
||||
{
|
||||
"key": "ShowTurnSignals",
|
||||
@@ -2197,6 +2174,33 @@
|
||||
"title": "Hyundai / Kia / Genesis Settings",
|
||||
"description": "",
|
||||
"items": [
|
||||
{
|
||||
"key": "HyundaiCustomButtonAction",
|
||||
"widget": "multiple_button",
|
||||
"title": "Steering Custom Button",
|
||||
"description": "Choose the sunnypilot action for the steering wheel custom/star button.",
|
||||
"options": [
|
||||
{
|
||||
"value": 0,
|
||||
"label": "None"
|
||||
},
|
||||
{
|
||||
"value": 1,
|
||||
"label": "Bookmark"
|
||||
},
|
||||
{
|
||||
"value": 2,
|
||||
"label": "Cycle UI"
|
||||
}
|
||||
],
|
||||
"visibility": [
|
||||
{
|
||||
"type": "capability",
|
||||
"field": "hyundai_has_custom_button",
|
||||
"equals": true
|
||||
}
|
||||
]
|
||||
},
|
||||
{
|
||||
"key": "HyundaiLongitudinalTuning",
|
||||
"widget": "multiple_button",
|
||||
|
||||
@@ -30,12 +30,6 @@ sections:
|
||||
label: Relaxed
|
||||
enablement:
|
||||
- $ref: '#/macros/longitudinal'
|
||||
- key: AccelBoostSP
|
||||
widget: toggle
|
||||
title: Acceleration Boost
|
||||
description: "Adds a small push on top of sunnypilot's own acceleration, stronger in Aggressive and gentler in Relaxed. Eases off in curves and when sunnypilot slows or stops. Never goes past your set speed, and lead-car following still comes first."
|
||||
enablement:
|
||||
- $ref: '#/macros/longitudinal'
|
||||
- key: IntelligentCruiseButtonManagement
|
||||
widget: toggle
|
||||
title: Intelligent Cruise Button Management (ICBM) (Alpha)
|
||||
|
||||
@@ -10,6 +10,21 @@ sections:
|
||||
title: Hyundai / Kia / Genesis Settings
|
||||
description: ''
|
||||
items:
|
||||
- key: HyundaiCustomButtonAction
|
||||
widget: multiple_button
|
||||
title: Steering Custom Button
|
||||
description: Choose the sunnypilot action for the steering wheel custom/star button.
|
||||
options:
|
||||
- value: 0
|
||||
label: None
|
||||
- value: 1
|
||||
label: Bookmark
|
||||
- value: 2
|
||||
label: Cycle UI
|
||||
visibility:
|
||||
- type: capability
|
||||
field: hyundai_has_custom_button
|
||||
equals: true
|
||||
- key: HyundaiLongitudinalTuning
|
||||
widget: multiple_button
|
||||
title: Custom Longitudinal Tuning
|
||||
|
||||
@@ -20,8 +20,6 @@ sections:
|
||||
widget: toggle
|
||||
title: Steering Arc
|
||||
description: Display steering arc on the driving screen when lateral control is enabled.
|
||||
visibility:
|
||||
- $ref: '#/macros/hide_on_mici'
|
||||
- key: ShowTurnSignals
|
||||
widget: toggle
|
||||
title: Display Turn Signals
|
||||
|
||||
@@ -17,7 +17,6 @@ from openpilot.sunnypilot.sunnylink.capabilities import (
|
||||
CAPABILITY_DEFAULTS,
|
||||
CAPABILITY_FIELDS,
|
||||
CAPABILITY_LABELS,
|
||||
CAR_CAPABILITY_FIELDS,
|
||||
PROTOCOL_VERSION,
|
||||
generate_capabilities,
|
||||
)
|
||||
@@ -92,10 +91,6 @@ class TestCapabilitiesShape(OpenpilotTestCase):
|
||||
for field in CAPABILITY_FIELDS:
|
||||
assert field in CAPABILITY_LABELS, f"CAPABILITY_LABELS missing {field}"
|
||||
|
||||
def test_car_fields_are_capabilities(self):
|
||||
assert set(CAR_CAPABILITY_FIELDS) <= set(CAPABILITY_FIELDS)
|
||||
assert "custom_model_active" not in CAR_CAPABILITY_FIELDS
|
||||
|
||||
def test_string_defaults_are_strings(self, caps):
|
||||
assert isinstance(caps["brand"], str)
|
||||
assert isinstance(caps["steer_control_type"], str)
|
||||
|
||||
@@ -19,9 +19,10 @@ from openpilot.tools.lib.route import Route
|
||||
REPO_ID = os.environ.get("SUNNYPILOT_CAR_SEGMENTS_REPO_ID", "sunnypilot/sunnypilotCarSegments")
|
||||
|
||||
|
||||
def upload_route(route_name: str, dry_run: bool = False, local_dir: str | None = None) -> None:
|
||||
log_paths = Route(route_name, data_dir=local_dir).log_paths()
|
||||
valid_segments = [(i, path) for i, path in enumerate(log_paths) if path is not None]
|
||||
def upload_route(route_name: str, dry_run: bool = False) -> None:
|
||||
route = Route(route_name)
|
||||
log_paths = route.log_paths()
|
||||
valid_segments = [(i, url) for i, url in enumerate(log_paths) if url is not None]
|
||||
|
||||
print(f"Route: {route_name}")
|
||||
print(f"Segments: {len(valid_segments)}/{len(log_paths)}")
|
||||
@@ -33,17 +34,14 @@ def upload_route(route_name: str, dry_run: bool = False, local_dir: str | None =
|
||||
api = HfApi()
|
||||
|
||||
with tempfile.TemporaryDirectory() as tmpdir:
|
||||
for seg_idx, src in tqdm(valid_segments, desc="Uploading"):
|
||||
filename = src.split("?")[0].rsplit("/", 1)[-1]
|
||||
if local_dir is not None:
|
||||
local_path = src
|
||||
else:
|
||||
local_path = os.path.join(tmpdir, f"{seg_idx}_{filename}")
|
||||
resp = requests.get(src, stream=True)
|
||||
resp.raise_for_status()
|
||||
with open(local_path, "wb") as f:
|
||||
for chunk in resp.iter_content(chunk_size=8192):
|
||||
f.write(chunk)
|
||||
for seg_idx, url in tqdm(valid_segments, desc="Uploading"):
|
||||
filename = url.split("?")[0].rsplit("/", 1)[-1]
|
||||
local_path = os.path.join(tmpdir, f"{seg_idx}_{filename}")
|
||||
resp = requests.get(url, stream=True)
|
||||
resp.raise_for_status()
|
||||
with open(local_path, "wb") as f:
|
||||
for chunk in resp.iter_content(chunk_size=8192):
|
||||
f.write(chunk)
|
||||
|
||||
repo_path = f"segments/{route_name.replace('|', '/')}/{seg_idx}/{filename}"
|
||||
|
||||
@@ -65,7 +63,6 @@ if __name__ == "__main__":
|
||||
parser = argparse.ArgumentParser(description="Upload route rlogs to sunnypilot HuggingFace dataset")
|
||||
parser.add_argument("route", help="Route ID (e.g. 5beb9b58bd12b691/0000010a--a51155e496)")
|
||||
parser.add_argument("--dry-run", action="store_true", help="Download and show sizes without uploading")
|
||||
parser.add_argument("--local", metavar="DIR", help="Upload rlogs from DIR (<dongle id>|<log id>--<segment>/) instead of the comma API")
|
||||
args = parser.parse_args()
|
||||
|
||||
upload_route(args.route, dry_run=args.dry_run, local_dir=args.local)
|
||||
upload_route(args.route, dry_run=args.dry_run)
|
||||
|
||||
@@ -253,12 +253,6 @@ def hardware_thread(end_event, hw_queue) -> None:
|
||||
chestnut_status = ChestnutStatus()
|
||||
branch = get_short_branch()
|
||||
|
||||
build_metadata = get_build_metadata()
|
||||
is_unsupported_combo = COMMA_HARDWARE and HARDWARE.get_device_type() == "tici" and build_metadata.channel_type != "tici"
|
||||
startup_conditions["not_tici"] = not is_unsupported_combo
|
||||
onroad_conditions["not_tici"] = not is_unsupported_combo
|
||||
set_offroad_alert_if_changed("Offroad_TiciSupport", is_unsupported_combo, extra_text=build_metadata.channel)
|
||||
|
||||
while not end_event.is_set():
|
||||
sm.update(PANDA_STATES_TIMEOUT)
|
||||
|
||||
@@ -381,6 +375,16 @@ def hardware_thread(end_event, hw_queue) -> None:
|
||||
startup_conditions["not_always_offroad"] = not offroad_mode
|
||||
onroad_conditions["not_always_offroad"] = not offroad_mode
|
||||
|
||||
# if an unsupported device and branch is detected, going onroad is blocked
|
||||
# only allow going onroad when:
|
||||
# - TIZI, or
|
||||
# - TICI and channel_type is "tici"
|
||||
build_metadata = get_build_metadata()
|
||||
is_unsupported_combo = COMMA_HARDWARE and HARDWARE.get_device_type() == "tici" and build_metadata.channel_type != "tici"
|
||||
startup_conditions["not_tici"] = not is_unsupported_combo
|
||||
onroad_conditions["not_tici"] = not is_unsupported_combo
|
||||
set_offroad_alert("Offroad_TiciSupport", is_unsupported_combo, extra_text=build_metadata.channel)
|
||||
|
||||
# if the temperature enters the danger zone, go offroad to cool down
|
||||
onroad_conditions["device_temp_good"] = thermal_status < ThermalStatus.critical
|
||||
extra_text = f"{offroad_comp_temp:.1f}C"
|
||||
@@ -439,6 +443,10 @@ def hardware_thread(end_event, hw_queue) -> None:
|
||||
# Offroad power monitoring
|
||||
voltage = None if peripheralState.pandaType == log.PandaState.PandaType.unknown else peripheralState.voltage
|
||||
|
||||
# GitHub runner auto off: 9V is used as the threshold because most desktop runners
|
||||
# will rarely exceed 5V so 9V is set as our buffer between desk use and car use.
|
||||
params.put_bool("GithubRunnerSufficientVoltage", ((voltage or 0) and voltage > 9000))
|
||||
|
||||
power_monitor.calculate(voltage, onroad_conditions["ignition"])
|
||||
msg.deviceState.offroadPowerUsageUwh = power_monitor.get_power_used()
|
||||
msg.deviceState.carBatteryCapacityUwh = max(0, power_monitor.get_car_battery_capacity())
|
||||
|
||||
@@ -184,7 +184,8 @@ procs += [
|
||||
NativeProcess("mapd", Paths.mapd_root(), ["bash", "-c", f"{MAPD_PATH} > /dev/null 2>&1"], mapd_ready),
|
||||
PythonProcess("mapd_manager", "openpilot.sunnypilot.mapd.mapd_manager", always_run),
|
||||
|
||||
PythonProcess("gps_fuser", "openpilot.sunnypilot.selfdrive.locationd.gps_fuser", only_onroad),
|
||||
# locationd
|
||||
NativeProcess("locationd_llk", "openpilot/sunnypilot/selfdrive/locationd", ["./locationd"], only_onroad),
|
||||
]
|
||||
|
||||
if os.path.exists("../../sunnypilot/sunnylink/uploader.py"):
|
||||
|
||||
@@ -13,41 +13,26 @@ from pathlib import Path
|
||||
from datetime import datetime, UTC
|
||||
|
||||
|
||||
_VERSION_REGEX = re.compile(r'^(?:\d+[a-zA-Z]+|\d+[vVbB]\d+|[vVbB]\d+|\d{4})$')
|
||||
_DECIMAL_VERSION_REGEX = re.compile(r'^([vVbB]?)(\d+(?:\.\d+)+)$')
|
||||
|
||||
|
||||
def _version(word: str) -> str | None:
|
||||
match = _DECIMAL_VERSION_REGEX.match(word.strip('()[]{}.,;:'))
|
||||
if match:
|
||||
return match.group(1).upper() + match.group(2)
|
||||
stripped = re.sub(r'[^a-zA-Z0-9]', '', word)
|
||||
return stripped.upper() if _VERSION_REGEX.match(stripped) else None
|
||||
|
||||
|
||||
def _truncate(short_name: str) -> str:
|
||||
return short_name[:8].rstrip('.')
|
||||
|
||||
|
||||
def create_short_name(full_name: str) -> str:
|
||||
# Remove parentheses and extract alphanumeric words, keeping decimals for versioning
|
||||
# Remove parentheses and extract alphanumeric words
|
||||
clean_name = re.sub(r'\([^)]*\)', '', full_name)
|
||||
words = [re.sub(r'[^a-zA-Z0-9.]', '', word) for word in clean_name.split()]
|
||||
words = [word for word in words if word]
|
||||
words = [re.sub(r'[^a-zA-Z0-9]', '', word) for word in clean_name.split() if re.sub(r'[^a-zA-Z0-9]', '', word)]
|
||||
|
||||
if len(words) == 1:
|
||||
return _truncate((_version(words[0]) or words[0]).upper())
|
||||
return words[0][:8].upper()
|
||||
|
||||
# Handle special case: Name + Version (e.g., "Word A1" -> "WordA1", "Model v1.1" -> "ModelV1.1")
|
||||
if len(words) == 2 and re.match(r'^[A-Za-z]?\d+$', words[1]):
|
||||
return _truncate((words[0] + (_version(words[1]) or words[1])).upper())
|
||||
# Handle special case: Name + Version (e.g., "Word A1" -> "WordA1")
|
||||
if len(words) == 2 and re.match(r'^[A-Za-z]\d+$', words[1]):
|
||||
return (words[0] + words[1])[:8].upper()
|
||||
|
||||
result = ""
|
||||
for word in words:
|
||||
# Version or number patterns
|
||||
version = _version(word)
|
||||
if version:
|
||||
result += version
|
||||
if (re.match(r'^\d+[a-zA-Z]+$', word) or
|
||||
re.match(r'^\d+[vVbB]\d+$', word) or
|
||||
re.match(r'^[vVbB]\d+$', word) or
|
||||
re.match(r'^\d{4}$', word)):
|
||||
result += word.upper()
|
||||
# All uppercase abbreviations (2-3 letters)
|
||||
elif re.match(r'^[A-Z]{2,3}$', word):
|
||||
result += word
|
||||
@@ -60,7 +45,7 @@ def create_short_name(full_name: str) -> str:
|
||||
result += word
|
||||
else:
|
||||
result += word[0].upper()
|
||||
return _truncate(result)
|
||||
return result[:8]
|
||||
|
||||
|
||||
def create_pkl_name(full_name: str) -> str:
|
||||
|
||||
Reference in New Issue
Block a user