diff --git a/.gitignore b/.gitignore index d1af22dbb..64090c18d 100644 --- a/.gitignore +++ b/.gitignore @@ -142,6 +142,7 @@ TASK.md /iqpilot/selfdrive/iqlocd/models/generated/car.cpp /iqpilot/selfdrive/iqlocd/models/generated/car.h /iqpilot/tools/scripts/debug/ +/iqpilot/tools/scripts/plans/ !scripts/iqpilot/iqos_usrlocal_larch64.tar.zst # pretty-build capture logs (model compile / mpc + dbc codegen stdout+stderr) @@ -182,3 +183,12 @@ iqpilot/selfdrive/ui/tests/test_ui/nav_demo_report/ # fetcher: closed-source precompiled component — ship only the obfuscated .so /iqpilot/models_private_src/fetcher.py + +# build output: a signed .app is a 233-file sealed bundle, never track a subset +iqpilot/tools/iqmacd/macos/dist/IQ eMac.app/ +iqpilot/tools/iqmacd/macos/dist/.stage/ +iqpilot/tools/iqmacd/macos/dist/.DS_Store + +iqpilot/tools/iqmacd/macos/dist/ + +iqpilot/tools/scripts/iqemac/clips/README.md diff --git a/artifacts/iqpilot_alc_private/.source_sha b/artifacts/iqpilot_alc_private/.source_sha index 499bd083d..b3535496a 100644 --- a/artifacts/iqpilot_alc_private/.source_sha +++ b/artifacts/iqpilot_alc_private/.source_sha @@ -1 +1 @@ -0a3bb124deb8a9fdc8cea76764c690c7673a597f +076c1d057f97b067222d74180fbb63ba5467254a diff --git a/artifacts/iqpilot_alc_private/manifest.json b/artifacts/iqpilot_alc_private/manifest.json index 26f645a56..c1f1617d3 100644 --- a/artifacts/iqpilot_alc_private/manifest.json +++ b/artifacts/iqpilot_alc_private/manifest.json @@ -21,13 +21,13 @@ }, "python/iqpilot_private/konn3kt/iqlvbs/alc.cpython-312-aarch64-linux-gnu.so": { "mode": 493, - "sha256": "4d3eb2b63aa962bd764a9ee0086d3936846fb34a255321f7e95594412856f0fd", - "size": 402072 + "sha256": "268267fdfb6ae438f1b3c6f5f2c378a5b878e68357d5560f604f77d5c36a2cea", + "size": 402144 }, "python/iqpilot_private/konn3kt/iqlvbs/vehicle_state.cpython-312-aarch64-linux-gnu.so": { "mode": 493, - "sha256": "8e6829e7d8828e76258eba191144d70ae5b42dec620608dcf65431db25c0670f", - "size": 69688 + "sha256": "b3d33e86493408e5a02f2df753e1b257f003e5028480b5b040aa5a07eeed98be", + "size": 69712 }, "runtime": { "entries": { @@ -42,7 +42,7 @@ } }, "signatures": { - "python/iqpilot_private/konn3kt/iqlvbs/alc.cpython-312-aarch64-linux-gnu.so": "c/cZ53FdLJJLrMc8J40fW/CFkByPO0Dt2AMFIOY1AiT8/09piyULhhWAytgNaIejk8BP0aQhlk5xfaivI3bKCw==", - "python/iqpilot_private/konn3kt/iqlvbs/vehicle_state.cpython-312-aarch64-linux-gnu.so": "bbcjBEVBBQskXGTqTTQmLAdjcgtoL45mwGhRUwchrEkMNNeMJrvSCQ7ZXf8KpNUgFaiSU7L/5tnqHsrtKvU8Dw==" + "python/iqpilot_private/konn3kt/iqlvbs/alc.cpython-312-aarch64-linux-gnu.so": "v0mM7HrM8Ssbm3E/TxIUefAh5aCkzbZIKDQk5yFN6kr4EgqmVyPMNkqu7kCsn4ah0fTzr7xCrhs/gXjGf+6yBQ==", + "python/iqpilot_private/konn3kt/iqlvbs/vehicle_state.cpython-312-aarch64-linux-gnu.so": "5KQhJ61bt8tNmKuegPVMnMiqEHKcnjhqMtCDv2CphlSS54L/GmsOZjieHQEcgpOIDw7n/7qz2k6iFnwJ6clLCg==" } } diff --git a/artifacts/iqpilot_alc_private/manifest.json.sig b/artifacts/iqpilot_alc_private/manifest.json.sig index f7893e093..d514c4fa7 100644 --- a/artifacts/iqpilot_alc_private/manifest.json.sig +++ b/artifacts/iqpilot_alc_private/manifest.json.sig @@ -1 +1 @@ -M1n62PoNdW9Odr9dhsZgHsKupcmVorFMp8QU14sERQn2XkFWjOGDfjUX018m7IWOdxtbciDzTzHYOIHkBxqVAg== +Xr+Nn9b/ZoT2/S94B/I16aHVQI5dZZE3hGvpS6elgasC7e4JZouZvln/ue3Lh5Hg/NTJCCEStRmpEtNfgcjbAA== diff --git a/artifacts/iqpilot_alc_private/python/iqpilot_private/konn3kt/iqlvbs/alc.cpython-312-aarch64-linux-gnu.so b/artifacts/iqpilot_alc_private/python/iqpilot_private/konn3kt/iqlvbs/alc.cpython-312-aarch64-linux-gnu.so index c69dbf237..6ed51787e 100755 Binary files a/artifacts/iqpilot_alc_private/python/iqpilot_private/konn3kt/iqlvbs/alc.cpython-312-aarch64-linux-gnu.so and b/artifacts/iqpilot_alc_private/python/iqpilot_private/konn3kt/iqlvbs/alc.cpython-312-aarch64-linux-gnu.so differ diff --git a/artifacts/iqpilot_alc_private/python/iqpilot_private/konn3kt/iqlvbs/vehicle_state.cpython-312-aarch64-linux-gnu.so b/artifacts/iqpilot_alc_private/python/iqpilot_private/konn3kt/iqlvbs/vehicle_state.cpython-312-aarch64-linux-gnu.so index c45106e34..bae7e9767 100755 Binary files a/artifacts/iqpilot_alc_private/python/iqpilot_private/konn3kt/iqlvbs/vehicle_state.cpython-312-aarch64-linux-gnu.so and b/artifacts/iqpilot_alc_private/python/iqpilot_private/konn3kt/iqlvbs/vehicle_state.cpython-312-aarch64-linux-gnu.so differ diff --git a/artifacts/iqpilot_emac_private/manifest.json b/artifacts/iqpilot_emac_private/manifest.json new file mode 100644 index 000000000..3cf308598 --- /dev/null +++ b/artifacts/iqpilot_emac_private/manifest.json @@ -0,0 +1,561 @@ +{ + "python/_iqclosure/_vendored.json": { + "mode": 420, + "sha256": "c5dde802e17116d9759e661e7d80a1a86d396fb3040734c1eadb2a50c5a3cf4c", + "size": 3439 + }, + "python/_iqclosure/iqpilot/__init__.py": { + "mode": 420, + "sha256": "e3b0c44298fc1c149afbf4c8996fb92427ae41e4649b934ca495991b7852b855", + "size": 0 + }, + "python/_iqclosure/iqpilot/common/__init__.py": { + "mode": 420, + "sha256": "e3b0c44298fc1c149afbf4c8996fb92427ae41e4649b934ca495991b7852b855", + "size": 0 + }, + "python/_iqclosure/iqpilot/common/api/__init__.py": { + "mode": 420, + "sha256": "97d6e0d8cbd3e47797d734322b42d04efea4d95a2dae5a56f1baef3fd847aa0d", + "size": 939 + }, + "python/_iqclosure/iqpilot/common/api/base.py": { + "mode": 420, + "sha256": "be55c83869bad911ee0204e8779b4673653f7d29ae4f524c978a280bd84c7777", + "size": 3187 + }, + "python/_iqclosure/iqpilot/common/api/comma_connect.py": { + "mode": 420, + "sha256": "e7c2dd0b56fb8533c2cbc614e728a9ffc5ce7360524cfa62a02f5235bc23b9a8", + "size": 267 + }, + "python/_iqclosure/iqpilot/common/atlas_alerts.py": { + "mode": 420, + "sha256": "ef0d867f6213bb67ed6b7fd6ef4e694eb1705d9dc694163a80c8dd9572932954", + "size": 9448 + }, + "python/_iqclosure/iqpilot/common/basedir.py": { + "mode": 420, + "sha256": "58cfa5d6405903ee1acecc4544463bb8f47a66921b120c382143aa80d8539b28", + "size": 106 + }, + "python/_iqclosure/iqpilot/common/constants.py": { + "mode": 420, + "sha256": "fc439d59c24b95ed349b84f1a5862df3ea50e0891dbec4ea3cf5e1e0ccff6513", + "size": 434 + }, + "python/_iqclosure/iqpilot/common/file_chunker.py": { + "mode": 420, + "sha256": "0c6dfabdbf1aa737f4dc306b10d12e6dcc3a87720dbbfe23df2f40490453b06e", + "size": 1898 + }, + "python/_iqclosure/iqpilot/common/filter_simple.py": { + "mode": 420, + "sha256": "4b34753a3d3eea6d1578d88d21350045173a3809d38aed232875d1c5f34b4141", + "size": 1929 + }, + "python/_iqclosure/iqpilot/common/gpio.py": { + "mode": 420, + "sha256": "01ba8cb32a644a2df545e764ba19bf1c7a320a217f983f8bac73b45d3a6891cf", + "size": 2408 + }, + "python/_iqclosure/iqpilot/common/i2c.py": { + "mode": 420, + "sha256": "4af8f9da7504a548d15f18b91d71c1379cb7c05c84be031448764cc49920ce68", + "size": 2449 + }, + "python/_iqclosure/iqpilot/common/iq_perf.py": { + "mode": 420, + "sha256": "16b9a744fc1346d9660eab59c87c05a56015fea6bfc30317047e5f95aba2f7d2", + "size": 7218 + }, + "python/_iqclosure/iqpilot/common/issue_debug.py": { + "mode": 420, + "sha256": "73a10d846c735d4bcfda65f7828aa82192349b53920cb9d1b01cef8c211982fe", + "size": 1128 + }, + "python/_iqclosure/iqpilot/common/logging_extra.py": { + "mode": 420, + "sha256": "ce9b3b3d8498d76e1c4c0352a3e98e72759bb7f367ff8629c1329c46040595c3", + "size": 6628 + }, + "python/_iqclosure/iqpilot/common/realtime.py": { + "mode": 420, + "sha256": "f137ef62c1602a1f7c329731883dc518c62288bd25596891858d460ae3209ae2", + "size": 3966 + }, + "python/_iqclosure/iqpilot/common/spinner.py": { + "mode": 420, + "sha256": "33a2eb2aa3148492414035c5dc7150b3a69467905d7a3e58c8f65cfe70d3552c", + "size": 1371 + }, + "python/_iqclosure/iqpilot/common/steer_delay.py": { + "mode": 420, + "sha256": "933220f41a027fe73bfcf7af1d40a7b58d1a751dc9a4fe16ddccc66f2c58c549", + "size": 2541 + }, + "python/_iqclosure/iqpilot/common/swaglog.py": { + "mode": 420, + "sha256": "0a133b4ef49f80b8890f115e81273f3f3e21351ae79f92dc02abc033e6d24027", + "size": 5274 + }, + "python/_iqclosure/iqpilot/common/time_helpers.py": { + "mode": 420, + "sha256": "c14cda1f84149d5ffa63a2adec5ca24126f77c1412a78412e5e3e68a6fd12bcf", + "size": 474 + }, + "python/_iqclosure/iqpilot/common/utils.py": { + "mode": 420, + "sha256": "6475ad94bc89252398c41e9eabf7929ba5eb8fa23a08ad7545e7a67b400af9f3", + "size": 8090 + }, + "python/_iqclosure/iqpilot/konn3kt/__init__.py": { + "mode": 420, + "sha256": "e3b0c44298fc1c149afbf4c8996fb92427ae41e4649b934ca495991b7852b855", + "size": 0 + }, + "python/_iqclosure/iqpilot/konn3kt/cloud_client.py": { + "mode": 420, + "sha256": "4d58e62f8d628a8570881f2b87c82c801b46928fbb84f8dab8a81a9b55ba4e15", + "size": 493 + }, + "python/_iqclosure/iqpilot/konn3kt/registration.py": { + "mode": 420, + "sha256": "d8da8fa6373d54c3090b4466a975969317c9f5cd7c0ba20fd905e11ce9b95efd", + "size": 7948 + }, + "python/_iqclosure/iqpilot/selfdrive/__init__.py": { + "mode": 420, + "sha256": "e3b0c44298fc1c149afbf4c8996fb92427ae41e4649b934ca495991b7852b855", + "size": 0 + }, + "python/_iqclosure/iqpilot/selfdrive/controls/__init__.py": { + "mode": 420, + "sha256": "e3b0c44298fc1c149afbf4c8996fb92427ae41e4649b934ca495991b7852b855", + "size": 0 + }, + "python/_iqclosure/iqpilot/selfdrive/controls/lib/__init__.py": { + "mode": 420, + "sha256": "e3b0c44298fc1c149afbf4c8996fb92427ae41e4649b934ca495991b7852b855", + "size": 0 + }, + "python/_iqclosure/iqpilot/selfdrive/controls/lib/desire_helper.py": { + "mode": 420, + "sha256": "407b7fcdd2699f7f64404df420b9a2ccfef2cfa4a66d5634f6dd7cb2f23ff3f4", + "size": 11737 + }, + "python/_iqclosure/iqpilot/selfdrive/controls/lib/drive_helpers.py": { + "mode": 420, + "sha256": "4246e7ac3a1c554e068a967472b2fe2793fb291d6468308badb25f5769fd1fb9", + "size": 3599 + }, + "python/_iqclosure/iqpilot/selfdrive/controls/lib/helpers/__init__.py": { + "mode": 420, + "sha256": "e3b0c44298fc1c149afbf4c8996fb92427ae41e4649b934ca495991b7852b855", + "size": 0 + }, + "python/_iqclosure/iqpilot/selfdrive/controls/lib/helpers/lane_change.py": { + "mode": 420, + "sha256": "3e7557a578a971f45e704b4386f6ac386745c8d044f45dab4b9b6cdd492a7f42", + "size": 7875 + }, + "python/_iqclosure/iqpilot/selfdrive/controls/lib/helpers/lane_turn.py": { + "mode": 420, + "sha256": "faa6f87b227c805e8890a91139e8d16d7874c4b99b330dda08080392ce3479ec", + "size": 4618 + }, + "python/_iqclosure/iqpilot/selfdrive/iqmodeld/__init__.py": { + "mode": 420, + "sha256": "e3b0c44298fc1c149afbf4c8996fb92427ae41e4649b934ca495991b7852b855", + "size": 0 + }, + "python/_iqclosure/iqpilot/selfdrive/iqmodeld/big_catalog.py": { + "mode": 420, + "sha256": "7ec5df43ce10b47ce925467e61100123e828b8e72bdab20bef4c8ac0ffd3cc7a", + "size": 369 + }, + "python/_iqclosure/iqpilot/selfdrive/iqmodeld/camera.py": { + "mode": 420, + "sha256": "6d6b309a557a6f7a97e97dd0227d2ec66c215523f19c8e3fcf7256940115da5c", + "size": 2646 + }, + "python/_iqclosure/iqpilot/selfdrive/iqmodeld/config.py": { + "mode": 420, + "sha256": "056c744bbd9f1fe1053555d9b1fc1257d53093d69ddc94be3f05d840eb70781c", + "size": 3590 + }, + "python/_iqclosure/iqpilot/selfdrive/iqmodeld/daemon.py": { + "mode": 420, + "sha256": "bbfe0c839d22286c2293c3fac11c5cae2206c2e69f0ae2aa4bdf64a3c772fc4f", + "size": 30210 + }, + "python/_iqclosure/iqpilot/selfdrive/iqmodeld/driving_action.py": { + "mode": 420, + "sha256": "863a9b18e82c7b2ab821541f498228ca7f5904ee38d5ac282c66dfaab7fdc492", + "size": 2381 + }, + "python/_iqclosure/iqpilot/selfdrive/iqmodeld/egpu_helpers.py": { + "mode": 420, + "sha256": "5d00df0cf831ea75a91d1d817818dc3847d971e581170671f12e289461eccabc", + "size": 4846 + }, + "python/_iqclosure/iqpilot/selfdrive/iqmodeld/egpu_model.py": { + "mode": 420, + "sha256": "eb3ebc7065a863d51d5bb4359d68f26fc2f1929e1d7e044377f15cf5bac7f77d", + "size": 367 + }, + "python/_iqclosure/iqpilot/selfdrive/iqmodeld/emac_model_meta.py": { + "mode": 420, + "sha256": "e47406eabf9c96bdf393ddaba8936fbd01755b2728ebe9413b2ab24b94de5d48", + "size": 377 + }, + "python/_iqclosure/iqpilot/selfdrive/iqmodeld/messaging.py": { + "mode": 420, + "sha256": "17d48331eeeaf746ee6982a9c65f28d740a15a4c34c37617115c104894d49c06", + "size": 11920 + }, + "python/_iqclosure/iqpilot/selfdrive/iqmodeld/metadata.py": { + "mode": 420, + "sha256": "dd1559099407687ee2dc81e180e05b9954e712c39e1a82e39c1b00b70c43374a", + "size": 3016 + }, + "python/_iqclosure/iqpilot/selfdrive/iqmodeld/model_channel.py": { + "mode": 420, + "sha256": "2d706993df418501d93dc766ffe452131e565bfbc2e8412b527dc14915e92832", + "size": 1872 + }, + "python/_iqclosure/iqpilot/selfdrive/iqmodeld/model_warp.py": { + "mode": 420, + "sha256": "075c89f8740945b84d0c554cccf4b0198d0738c85bde52e64bab69b01e482987", + "size": 3325 + }, + "python/_iqclosure/iqpilot/selfdrive/iqmodeld/models/__init__.py": { + "mode": 420, + "sha256": "5865243a95ca4557d2a960302124a9898545cc3a8387e88ef569308e3d0f7077", + "size": 119 + }, + "python/_iqclosure/iqpilot/selfdrive/iqmodeld/models/combined_artifact.py": { + "mode": 420, + "sha256": "321e7223249dc7dba93effe8e834f225fb6067abf49da17fe9a6faeb71ba8e49", + "size": 2786 + }, + "python/_iqclosure/iqpilot/selfdrive/iqmodeld/models/fetcher.py": { + "mode": 420, + "sha256": "23622cdec9eed5649a05422ba19eae956ba16a63850927af4a222c3d9030a1bb", + "size": 399 + }, + "python/_iqclosure/iqpilot/selfdrive/iqmodeld/models/helpers.py": { + "mode": 420, + "sha256": "e4f524c7595754fd9db86724a7bd9b67524c630275ac78599ab6838d1aa7d809", + "size": 11037 + }, + "python/_iqclosure/iqpilot/selfdrive/iqmodeld/models/inference_state.py": { + "mode": 420, + "sha256": "bdfd1c1b538e2342b17d08aa6ebe0c041c2f721025bcd7334434d1139fe6063d", + "size": 268 + }, + "python/_iqclosure/iqpilot/selfdrive/iqmodeld/models/runners/__init__.py": { + "mode": 420, + "sha256": "e3b0c44298fc1c149afbf4c8996fb92427ae41e4649b934ca495991b7852b855", + "size": 0 + }, + "python/_iqclosure/iqpilot/selfdrive/iqmodeld/models/runners/model_runner.py": { + "mode": 420, + "sha256": "e0eb9d7bff1da89d52b09596f7e7109fc25c3b9b86a89674382a7dde9e0a4199", + "size": 8555 + }, + "python/_iqclosure/iqpilot/selfdrive/iqmodeld/models/runners/tinygrad/__init__.py": { + "mode": 420, + "sha256": "e3b0c44298fc1c149afbf4c8996fb92427ae41e4649b934ca495991b7852b855", + "size": 0 + }, + "python/_iqclosure/iqpilot/selfdrive/iqmodeld/models/runners/tinygrad/combined_split_runner.py": { + "mode": 420, + "sha256": "b7c15991f4fe9c6b4d7fb6a2a37256c142ab40cdd53f52bd3c59accc3a3618b1", + "size": 10929 + }, + "python/_iqclosure/iqpilot/selfdrive/iqmodeld/models/runners/tinygrad/fused_runner.py": { + "mode": 420, + "sha256": "25304fe7ae034d1bb8c9c86a2757704944539be6a0c69c322e75adc84f66e221", + "size": 7885 + }, + "python/_iqclosure/iqpilot/selfdrive/iqmodeld/models/runners/tinygrad/model_types.py": { + "mode": 420, + "sha256": "b68fe2c7f0013fe6780e765882b4a1ac7074834ea6d370cbca90a51d2f614091", + "size": 1981 + }, + "python/_iqclosure/iqpilot/selfdrive/iqmodeld/models/runners/tinygrad/supercombo_runner.py": { + "mode": 420, + "sha256": "10ae83948aba757a48a781fed90be1cce65e8853c8b67dcb7b4ec6928e312005", + "size": 14219 + }, + "python/_iqclosure/iqpilot/selfdrive/iqmodeld/models/runners/tinygrad/tinygrad_runner.py": { + "mode": 420, + "sha256": "58ffe0afb2cd9527d2a1d5bbebf0471af8c8210534e266ab80488a7f89bb06ba", + "size": 7684 + }, + "python/_iqclosure/iqpilot/selfdrive/iqmodeld/models/split_model_constants.py": { + "mode": 420, + "sha256": "1fe374d5eebef362923a5d90f1218bcd6b916eb60584d4a55f840de2ef6c4cf3", + "size": 2145 + }, + "python/_iqclosure/iqpilot/selfdrive/iqmodeld/parser.py": { + "mode": 420, + "sha256": "1dc47b2c167f1271c5c45d6ba0582cb906f4b59b0621b6a6d971da3f329d66e8", + "size": 9212 + }, + "python/_iqclosure/iqpilot/selfdrive/iqmodeld/runtime/__init__.py": { + "mode": 420, + "sha256": "e3b0c44298fc1c149afbf4c8996fb92427ae41e4649b934ca495991b7852b855", + "size": 0 + }, + "python/_iqclosure/iqpilot/selfdrive/iqmodeld/runtime/tinygrad.py": { + "mode": 420, + "sha256": "316988acf63f98aca83c5662ffb2c6028ef1f8b94fd8d6fcc15e96d9ac0d8181", + "size": 779 + }, + "python/_iqclosure/iqpilot/selfdrive/iqmodeld/temporal_state.py": { + "mode": 420, + "sha256": "06ab9349b786559037ce68d5882b54ffbe39f76737c4d19a213b251240023d4a", + "size": 5501 + }, + "python/_iqclosure/iqpilot/selfdrive/iqmodeld/tools/__init__.py": { + "mode": 420, + "sha256": "e3b0c44298fc1c149afbf4c8996fb92427ae41e4649b934ca495991b7852b855", + "size": 0 + }, + "python/_iqclosure/iqpilot/selfdrive/iqmodeld/tools/compile_supercombo.py": { + "mode": 420, + "sha256": "d8fbc18f67ea1e330130ce0b228af298919c5304d299da07eb94d557c0827d25", + "size": 17739 + }, + "python/_iqclosure/iqpilot/selfdrive/iqmodeld/tools/compile_warp.py": { + "mode": 420, + "sha256": "2cc299b2cfda88ee2dbc52658f145fb7840e2dfc49a7371e22fe671beed0e57f", + "size": 5436 + }, + "python/_iqclosure/iqpilot/selfdrive/locationd/__init__.py": { + "mode": 420, + "sha256": "e3b0c44298fc1c149afbf4c8996fb92427ae41e4649b934ca495991b7852b855", + "size": 0 + }, + "python/_iqclosure/iqpilot/selfdrive/locationd/calibration_helpers.py": { + "mode": 420, + "sha256": "1501058881da2720b35915f0949c2bc311020386c2549d48111ab32a34386a6c", + "size": 982 + }, + "python/_iqclosure/iqpilot/selfdrive/locationd/calibrationd.py": { + "mode": 420, + "sha256": "6bceebb5a15895d2b6d20a28cbd14b9112f9422e0a0cf466314a7e8779da39dc", + "size": 15698 + }, + "python/_iqclosure/iqpilot/selfdrive/locationd/helpers.py": { + "mode": 420, + "sha256": "be47384b3437670192f8bfd3dc679d57f0678f9f4fe34759419b0f2eb01ab117", + "size": 6828 + }, + "python/_iqclosure/iqpilot/selfdrive/locationd/locationd.py": { + "mode": 420, + "sha256": "8a7d7c89ea11998e418a52355a3d719ef3bf4ddfd0af13da2234d08348c25fe7", + "size": 15169 + }, + "python/_iqclosure/iqpilot/selfdrive/locationd/models/__init__.py": { + "mode": 420, + "sha256": "e3b0c44298fc1c149afbf4c8996fb92427ae41e4649b934ca495991b7852b855", + "size": 0 + }, + "python/_iqclosure/iqpilot/selfdrive/locationd/models/constants.py": { + "mode": 420, + "sha256": "276b20ca6187466efe6273e66100d92f4f54bd2b393c77329fb196a512b707bf", + "size": 1952 + }, + "python/_iqclosure/iqpilot/selfdrive/locationd/models/pose_kf.py": { + "mode": 420, + "sha256": "98aabe0a0b1e94f0c34af459ead9488901f08722ada3a87d6a33c00241dee211", + "size": 2930 + }, + "python/_iqclosure/iqpilot/selfdrive/selfdrived/__init__.py": { + "mode": 420, + "sha256": "e3b0c44298fc1c149afbf4c8996fb92427ae41e4649b934ca495991b7852b855", + "size": 0 + }, + "python/_iqclosure/iqpilot/selfdrive/selfdrived/alertmanager.py": { + "mode": 420, + "sha256": "c601bac30951a750d3ebf3d30f06c597bacd148210b1c405d5d3eca79bb7c4e3", + "size": 2045 + }, + "python/_iqclosure/iqpilot/selfdrive/selfdrived/events.py": { + "mode": 420, + "sha256": "e298175d13a99b55e183954d82346fa7adab6b6c161fc72b789f12601d1a9b43", + "size": 37038 + }, + "python/_iqclosure/iqpilot/selfdrive/state_estimation/__init__.py": { + "mode": 420, + "sha256": "85a0c22d638eb8f87b00ca2ea21aa4c0c76708fe3e2f3ccc1fa8f87f0c784450", + "size": 319 + }, + "python/_iqclosure/iqpilot/selfdrive/state_estimation/estimator.py": { + "mode": 420, + "sha256": "59ae4dd78c8f850a5acd28d2743abe1b9d19ebcaaf9c472f95b2766a73e056c6", + "size": 12577 + }, + "python/_iqclosure/iqpilot/selfdrive/ui/__init__.py": { + "mode": 420, + "sha256": "e3b0c44298fc1c149afbf4c8996fb92427ae41e4649b934ca495991b7852b855", + "size": 0 + }, + "python/_iqclosure/iqpilot/selfdrive/ui/feedback/__init__.py": { + "mode": 420, + "sha256": "e3b0c44298fc1c149afbf4c8996fb92427ae41e4649b934ca495991b7852b855", + "size": 0 + }, + "python/_iqclosure/iqpilot/selfdrive/ui/feedback/feedbackd.py": { + "mode": 420, + "sha256": "7c1e6cb2c3c2c6aae91210f6cbc5c372f33c28cc3edb292ef71439b83844615d", + "size": 2615 + }, + "python/_iqclosure/iqpilot/system/__init__.py": { + "mode": 420, + "sha256": "e3b0c44298fc1c149afbf4c8996fb92427ae41e4649b934ca495991b7852b855", + "size": 0 + }, + "python/_iqclosure/iqpilot/system/camerad/__init__.py": { + "mode": 420, + "sha256": "e3b0c44298fc1c149afbf4c8996fb92427ae41e4649b934ca495991b7852b855", + "size": 0 + }, + "python/_iqclosure/iqpilot/system/camerad/cameras/__init__.py": { + "mode": 420, + "sha256": "e3b0c44298fc1c149afbf4c8996fb92427ae41e4649b934ca495991b7852b855", + "size": 0 + }, + "python/_iqclosure/iqpilot/system/camerad/cameras/nv12_info.py": { + "mode": 420, + "sha256": "90b0b00df8a7baa638940c2e73cbe2e79059af0acd19812ca6d765793ce4fda0", + "size": 829 + }, + "python/_iqclosure/iqpilot/system/hardware/__init__.py": { + "mode": 420, + "sha256": "889391bffca493e74bfc108cae2976a53b644caca13ab600061fd29e9622c627", + "size": 695 + }, + "python/_iqclosure/iqpilot/system/hardware/base.py": { + "mode": 420, + "sha256": "a56293f7c23e503ff62d0d3ed39f1831b5544a65b37378165934361a5ddcd304", + "size": 4966 + }, + "python/_iqclosure/iqpilot/system/hardware/hw.py": { + "mode": 420, + "sha256": "c69185d8f6482c8b021f687b793246d2e2ede853e89a88748c64b85f60711b59", + "size": 3638 + }, + "python/_iqclosure/iqpilot/system/hardware/pc/__init__.py": { + "mode": 420, + "sha256": "e3b0c44298fc1c149afbf4c8996fb92427ae41e4649b934ca495991b7852b855", + "size": 0 + }, + "python/_iqclosure/iqpilot/system/hardware/pc/hardware.py": { + "mode": 420, + "sha256": "2520895597c375ae570bd04d5f189e874c89d642f3f07bdbeb20388cb6691433", + "size": 258 + }, + "python/_iqclosure/iqpilot/system/hardware/tici/__init__.py": { + "mode": 420, + "sha256": "e3b0c44298fc1c149afbf4c8996fb92427ae41e4649b934ca495991b7852b855", + "size": 0 + }, + "python/_iqclosure/iqpilot/system/hardware/tici/amplifier.py": { + "mode": 420, + "sha256": "98f46f8705259b00efae7274186b4e68201aad467a19de4f16d5f850d10ab0b5", + "size": 7585 + }, + "python/_iqclosure/iqpilot/system/hardware/tici/hardware.py": { + "mode": 420, + "sha256": "f1ddffa9bdb5071a49fd65eb4b49958b60b1873e1ec70e6f3f2e2b2501abce2f", + "size": 25584 + }, + "python/_iqclosure/iqpilot/system/hardware/tici/iwlist.py": { + "mode": 420, + "sha256": "d22858982705531732d5a20a45a73795496e5bbd9e23028c24609a7e58ac7aed", + "size": 890 + }, + "python/_iqclosure/iqpilot/system/hardware/tici/lpa.py": { + "mode": 420, + "sha256": "2bffb84aef35892c4788e80b6d937bf2eebd15e226b72a82c4f39217667c1ca0", + "size": 53692 + }, + "python/_iqclosure/iqpilot/system/hardware/tici/pins.py": { + "mode": 420, + "sha256": "b57ba4262e6a6a79f2db15c27356acf3e54f579a7edd13b483b2e16d140aca6d", + "size": 678 + }, + "python/_iqclosure/iqpilot/system/hardware/tici/usb_storage.py": { + "mode": 420, + "sha256": "3352f2cf70d38e7158ef8ac20bb736dba55c73239cbdd3ab7f431dbacfb5027e", + "size": 2189 + }, + "python/_iqclosure/iqpilot/system/hardware/usb.py": { + "mode": 420, + "sha256": "7c2cd7c6d5a859fb8454059da1b36e3b5ce4639aca6fea296b4f91096b46c1ba", + "size": 7257 + }, + "python/_iqclosure/iqpilot/system/micd.py": { + "mode": 420, + "sha256": "1a59cbe6b72fcca5723ab56dc09f52039817299071a8d8da37f04acea58776bc", + "size": 5977 + }, + "python/_iqclosure/iqpilot/system/sentry.py": { + "mode": 420, + "sha256": "c3da338feb533937b6a5258ee450b6a3ff7a77b69cc9e48e31d16b875855caf3", + "size": 5229 + }, + "python/_iqclosure/iqpilot/system/version.py": { + "mode": 420, + "sha256": "3a6bc7520dae7944a1da6f911cd090704a463e011f39949ea6c9df51b0e8c616", + "size": 6033 + }, + "python/iqpilot_private/__init__.py": { + "mode": 420, + "sha256": "e3b0c44298fc1c149afbf4c8996fb92427ae41e4649b934ca495991b7852b855", + "size": 0 + }, + "python/iqpilot_private/emac/__init__.py": { + "mode": 420, + "sha256": "5865243a95ca4557d2a960302124a9898545cc3a8387e88ef569308e3d0f7077", + "size": 119 + }, + "python/iqpilot_private/emac/bulk_transport.cpython-312-aarch64-linux-gnu.so": { + "mode": 493, + "sha256": "0b580306ea908ad46b280f2fd5f828083e736b83c951a33ab007a6fb8a642442", + "size": 202152 + }, + "python/iqpilot_private/emac/mac_client.cpython-312-aarch64-linux-gnu.so": { + "mode": 493, + "sha256": "b4d721d043cad44826a7b1ac304105ff65676a00570e1b1e0f845c2a4ab88d51", + "size": 267376 + }, + "python/iqpilot_private/emac/mac_protocol.cpython-312-aarch64-linux-gnu.so": { + "mode": 493, + "sha256": "c63d56f9a4329f8a16c4a2144ddc7761876fd20f7c51ac40005c516573f7b19e", + "size": 203544 + }, + "python/iqpilot_private/emac/maciqmodeld.cpython-312-aarch64-linux-gnu.so": { + "mode": 493, + "sha256": "311a13e5db0e6f8719e2b6383a2d9c74c2f70db28bdd424c5ceec4958d70f201", + "size": 333336 + }, + "runtime": { + "entries": { + "iqpilot_private.emac.maciqmodeld": { + "path": "python/iqpilot_private/emac/maciqmodeld.cpython-312-aarch64-linux-gnu.so", + "type": "python-module" + } + } + }, + "signatures": { + "python/iqpilot_private/emac/bulk_transport.cpython-312-aarch64-linux-gnu.so": "mxq4MaNi+2F0fpbVaypztLeUf9DqFo8LSohXAzb+1tXsdKZZYQmmU90hlqIRFv4PuwWRyVbXJE/Ge48amXDhBw==", + "python/iqpilot_private/emac/mac_client.cpython-312-aarch64-linux-gnu.so": "Zm41VmfRMMJkj3TvxrqLLyUy+0Xq71z9V18gpKkstM5hWtrZsdP4Q+G04eeiujl45Q8hO1ysW0MMP6gJFQGfAg==", + "python/iqpilot_private/emac/mac_protocol.cpython-312-aarch64-linux-gnu.so": "3quWJnVdRjcA+zrHojJIRcmxySkJAbskBYfVpXjpN3BHxFvjrC+TK9ReVEeTwQdp6b6xR0Fk+40Ngo0MphsbAQ==", + "python/iqpilot_private/emac/maciqmodeld.cpython-312-aarch64-linux-gnu.so": "wqTQKJ8HOYoBP6dLbdVLvMRZ81kMfvwRB77qSj3jikgyPtz2ARen/Eo/SydGqQByqn0FUR7mHWm65I7/flEEBw==" + } +} diff --git a/artifacts/iqpilot_emac_private/manifest.json.sig b/artifacts/iqpilot_emac_private/manifest.json.sig new file mode 100644 index 000000000..bae964284 --- /dev/null +++ b/artifacts/iqpilot_emac_private/manifest.json.sig @@ -0,0 +1 @@ +ML9DoBJeI+4hKgeyK5ssDKYFVeaAg1dVrqTrEjizxFHJtuAv1wYohJz8hxdJnvkQgvrA4pFVsOsu3kvEdIXWAg== diff --git a/artifacts/iqpilot_emac_private/python/_iqclosure/_vendored.json b/artifacts/iqpilot_emac_private/python/_iqclosure/_vendored.json new file mode 100644 index 000000000..9fcfa0148 --- /dev/null +++ b/artifacts/iqpilot_emac_private/python/_iqclosure/_vendored.json @@ -0,0 +1,84 @@ +[ + "iqpilot.common.api", + "iqpilot.common.api.base", + "iqpilot.common.api.comma_connect", + "iqpilot.common.atlas_alerts", + "iqpilot.common.basedir", + "iqpilot.common.constants", + "iqpilot.common.file_chunker", + "iqpilot.common.filter_simple", + "iqpilot.common.gpio", + "iqpilot.common.i2c", + "iqpilot.common.iq_perf", + "iqpilot.common.issue_debug", + "iqpilot.common.logging_extra", + "iqpilot.common.realtime", + "iqpilot.common.spinner", + "iqpilot.common.steer_delay", + "iqpilot.common.swaglog", + "iqpilot.common.time_helpers", + "iqpilot.common.utils", + "iqpilot.konn3kt.cloud_client", + "iqpilot.konn3kt.registration", + "iqpilot.selfdrive.controls.lib.desire_helper", + "iqpilot.selfdrive.controls.lib.drive_helpers", + "iqpilot.selfdrive.controls.lib.helpers.lane_change", + "iqpilot.selfdrive.controls.lib.helpers.lane_turn", + "iqpilot.selfdrive.iqmodeld.big_catalog", + "iqpilot.selfdrive.iqmodeld.camera", + "iqpilot.selfdrive.iqmodeld.config", + "iqpilot.selfdrive.iqmodeld.daemon", + "iqpilot.selfdrive.iqmodeld.driving_action", + "iqpilot.selfdrive.iqmodeld.egpu_helpers", + "iqpilot.selfdrive.iqmodeld.egpu_model", + "iqpilot.selfdrive.iqmodeld.emac_model_meta", + "iqpilot.selfdrive.iqmodeld.messaging", + "iqpilot.selfdrive.iqmodeld.metadata", + "iqpilot.selfdrive.iqmodeld.model_channel", + "iqpilot.selfdrive.iqmodeld.model_warp", + "iqpilot.selfdrive.iqmodeld.models", + "iqpilot.selfdrive.iqmodeld.models.combined_artifact", + "iqpilot.selfdrive.iqmodeld.models.fetcher", + "iqpilot.selfdrive.iqmodeld.models.helpers", + "iqpilot.selfdrive.iqmodeld.models.inference_state", + "iqpilot.selfdrive.iqmodeld.models.runners.model_runner", + "iqpilot.selfdrive.iqmodeld.models.runners.tinygrad.combined_split_runner", + "iqpilot.selfdrive.iqmodeld.models.runners.tinygrad.fused_runner", + "iqpilot.selfdrive.iqmodeld.models.runners.tinygrad.model_types", + "iqpilot.selfdrive.iqmodeld.models.runners.tinygrad.supercombo_runner", + "iqpilot.selfdrive.iqmodeld.models.runners.tinygrad.tinygrad_runner", + "iqpilot.selfdrive.iqmodeld.models.split_model_constants", + "iqpilot.selfdrive.iqmodeld.parser", + "iqpilot.selfdrive.iqmodeld.runtime.tinygrad", + "iqpilot.selfdrive.iqmodeld.temporal_state", + "iqpilot.selfdrive.iqmodeld.tools.compile_supercombo", + "iqpilot.selfdrive.iqmodeld.tools.compile_warp", + "iqpilot.selfdrive.locationd.calibration_helpers", + "iqpilot.selfdrive.locationd.calibrationd", + "iqpilot.selfdrive.locationd.helpers", + "iqpilot.selfdrive.locationd.locationd", + "iqpilot.selfdrive.locationd.models.constants", + "iqpilot.selfdrive.locationd.models.pose_kf", + "iqpilot.selfdrive.selfdrived.alertmanager", + "iqpilot.selfdrive.selfdrived.events", + "iqpilot.selfdrive.state_estimation", + "iqpilot.selfdrive.state_estimation.estimator", + "iqpilot.selfdrive.ui.feedback.feedbackd", + "iqpilot.system", + "iqpilot.system.camerad.cameras.nv12_info", + "iqpilot.system.hardware", + "iqpilot.system.hardware.base", + "iqpilot.system.hardware.hw", + "iqpilot.system.hardware.pc.hardware", + "iqpilot.system.hardware.tici", + "iqpilot.system.hardware.tici.amplifier", + "iqpilot.system.hardware.tici.hardware", + "iqpilot.system.hardware.tici.iwlist", + "iqpilot.system.hardware.tici.lpa", + "iqpilot.system.hardware.tici.pins", + "iqpilot.system.hardware.tici.usb_storage", + "iqpilot.system.hardware.usb", + "iqpilot.system.micd", + "iqpilot.system.sentry", + "iqpilot.system.version" +] diff --git a/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/__init__.py b/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/common/__init__.py b/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/common/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/common/api/__init__.py b/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/common/api/__init__.py new file mode 100644 index 000000000..7c7b26fc2 --- /dev/null +++ b/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/common/api/__init__.py @@ -0,0 +1,26 @@ +import iqpilot.common.api.comma_connect + + +class Api: + def __init__(self, dongle_id): + self.service = iqpilot.common.api.comma_connect.CommaConnectApi(dongle_id) + + def request(self, method, endpoint, **params): + return self.service.request(method, endpoint, **params) + + def get(self, *args, **kwargs): + return self.service.get(*args, **kwargs) + + def post(self, *args, **kwargs): + return self.service.post(*args, **kwargs) + + def get_token(self, payload_extra=None, expiry_hours=1): + return self.service.get_token(payload_extra, expiry_hours) + + +def api_get(endpoint, method='GET', timeout=None, access_token=None, session=None, **params): + return iqpilot.common.api.comma_connect.CommaConnectApi(None).api_get(endpoint, method, timeout, access_token, session, **params) + + +def get_key_pair() -> tuple[str, str, str] | tuple[None, None, None]: + return iqpilot.common.api.comma_connect.CommaConnectApi(None).get_key_pair() diff --git a/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/common/api/base.py b/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/common/api/base.py new file mode 100644 index 000000000..8170a19b2 --- /dev/null +++ b/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/common/api/base.py @@ -0,0 +1,84 @@ +import jwt +import os +import requests +import unicodedata +from datetime import datetime, timedelta, UTC +from functools import lru_cache +from iqpilot.system.hardware.hw import Paths +from iqpilot.system.version import get_version + +# name: jwt signature algorithm +KEYS = {"id_rsa": "RS256", + "id_ecdsa": "ES256"} + + +@lru_cache(maxsize=4) +def load_signing_key(private_key: str): + # PyJWT re-parses a PEM string on every encode; an RSA parse is ~40ms, so cache the key object + try: + from cryptography.hazmat.primitives.serialization import load_pem_private_key + return load_pem_private_key(private_key.encode(), password=None) + except Exception: + return private_key + + +class BaseApi: + def __init__(self, dongle_id, api_host, user_agent="openpilot-"): + self.dongle_id = dongle_id + self.api_host = api_host + self.user_agent = user_agent + self.jwt_algorithm, self.private_key, _ = self.get_key_pair() + + def get(self, *args, **kwargs): + return self.request('GET', *args, **kwargs) + + def post(self, *args, **kwargs): + return self.request('POST', *args, **kwargs) + + def request(self, method, endpoint, timeout=None, access_token=None, **params): + return self.api_get(endpoint, method=method, timeout=timeout, access_token=access_token, **params) + + def _get_token(self, payload_extra=None, expiry_hours=1, **extra_payload): + now = datetime.now(UTC).replace(tzinfo=None) + payload = { + 'identity': self.dongle_id, + 'nbf': now, + 'iat': now, + 'exp': now + timedelta(hours=expiry_hours), + **extra_payload + } + if payload_extra is not None: + payload.update(payload_extra) + key = load_signing_key(self.private_key) if self.private_key else self.private_key + token = jwt.encode(payload, key, algorithm=self.jwt_algorithm) + if isinstance(token, bytes): + token = token.decode('utf8') + return token + + def get_token(self, payload_extra=None, expiry_hours=1): + return self._get_token(payload_extra, expiry_hours) + + def remove_non_ascii_chars(self, text): + normalized_text = unicodedata.normalize('NFD', text) + ascii_encoded_text = normalized_text.encode('ascii', 'ignore') + return ascii_encoded_text.decode() + + def api_get(self, endpoint, method='GET', timeout=None, access_token=None, session=None, json=None, **params): + headers = {} + if access_token is not None: + headers['Authorization'] = "JWT " + access_token + + version = self.remove_non_ascii_chars(get_version()) + headers['User-Agent'] = self.user_agent + version + + # TODO: add session to Api + req = requests if session is None else session + return req.request(method, f"{self.api_host}/{endpoint}", timeout=timeout, headers=headers, json=json, params=params) + + @staticmethod + def get_key_pair() -> tuple[str, str, str] | tuple[None, None, None]: + for key in KEYS: + if os.path.isfile(Paths.persist_root() + f'/comma/{key}') and os.path.isfile(Paths.persist_root() + f'/comma/{key}.pub'): + with open(Paths.persist_root() + f'/comma/{key}') as private, open(Paths.persist_root() + f'/comma/{key}.pub') as public: + return KEYS[key], private.read(), public.read() + return None, None, None diff --git a/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/common/api/comma_connect.py b/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/common/api/comma_connect.py new file mode 100644 index 000000000..bc16d40eb --- /dev/null +++ b/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/common/api/comma_connect.py @@ -0,0 +1,11 @@ +import os + +from iqpilot.common.api.base import BaseApi + +API_HOST = os.getenv('API_HOST', 'https://api-iqlabs.konn3kt.com') + + +class CommaConnectApi(BaseApi): + def __init__(self, dongle_id): + super().__init__(dongle_id, API_HOST) + self.user_agent = "openpilot-" diff --git a/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/common/atlas_alerts.py b/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/common/atlas_alerts.py new file mode 100644 index 000000000..c261db844 --- /dev/null +++ b/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/common/atlas_alerts.py @@ -0,0 +1,281 @@ +from __future__ import annotations + +from abc import ABC, abstractmethod +from bisect import insort +from collections.abc import Callable, Iterable +from dataclasses import dataclass, field +from enum import IntEnum + +import iqpilot.cereal.messaging as messaging +from iqpilot.cereal import car, log +from iqpilot.common.realtime import DT_CTRL +from iqpilot.system.hardware import HARDWARE + +AlertSize = log.SelfdriveState.AlertSize +AlertStatus = log.SelfdriveState.AlertStatus +VisualAlert = car.CarControl.HUDControl.VisualAlert +AudibleAlert = car.CarControl.HUDControl.AudibleAlert + + +def _frames_for(seconds: float) -> int: + return int(seconds / DT_CTRL) + + +class Tier(IntEnum): + LOWEST = 0 + LOWER = 1 + LOW = 2 + MID = 3 + HIGH = 4 + HIGHEST = 5 + + +class Tags: + ENABLE = "enable" + PRE_ENABLE = "preEnable" + OVERRIDE_LATERAL = "overrideLateral" + OVERRIDE_LONGITUDINAL = "overrideLongitudinal" + NO_ENTRY = "noEntry" + WARNING = "warning" + USER_DISABLE = "userDisable" + SOFT_DISABLE = "softDisable" + IMMEDIATE_DISABLE = "immediateDisable" + PERMANENT = "permanent" + + +@dataclass(slots=True) +class AlertCard: + alert_text_1: str + alert_text_2: str + alert_status: log.SelfdriveState.AlertStatus + alert_size: log.SelfdriveState.AlertSize + priority: Tier + visual_alert: car.CarControl.HUDControl.VisualAlert + audible_alert: car.CarControl.HUDControl.AudibleAlert + duration: int + creation_delay: float = 0.0 + alert_type: str = field(default="", init=False) + event_type: str | None = field(default=None, init=False) + + def __init__(self, + alert_text_1: str, + alert_text_2: str, + alert_status: log.SelfdriveState.AlertStatus, + alert_size: log.SelfdriveState.AlertSize, + priority: Tier, + visual_alert: car.CarControl.HUDControl.VisualAlert, + audible_alert: car.CarControl.HUDControl.AudibleAlert, + duration: float, + creation_delay: float = 0.0): + self.alert_text_1 = alert_text_1 + self.alert_text_2 = alert_text_2 + self.alert_status = alert_status + self.alert_size = alert_size + self.priority = priority + self.visual_alert = visual_alert + self.audible_alert = audible_alert + self.duration = _frames_for(duration) + self.creation_delay = creation_delay + self.alert_type = "" + self.event_type = None + + def __str__(self) -> str: + return f"{self.alert_text_1}/{self.alert_text_2} {self.priority} {self.visual_alert} {self.audible_alert}" + + +AlertFactory = Callable[[car.CarParams, car.CarState, messaging.SubMaster, bool, int, log.ControlsState], AlertCard] + + +def car_mode_entry_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> AlertCard: + del CS, sm, metric, soft_disable_time, personality + headline = "Enable Adaptive Cruise to Engage" + if CP.brand == "honda": + headline = "Enable Main Switch to Engage" + return NoEntryCard(headline) + + +class EventBook(ABC): + def __init__(self): + self._live_names: list[int] = [] + self._latched_names: list[int] = [] + self.event_counters: dict[int, int] = {} + + @property + def events(self) -> list[int]: + return self._live_names + + @events.setter + def events(self, values: list[int]) -> None: + self._live_names = values + + @property + def static_events(self) -> list[int]: + return self._latched_names + + @static_events.setter + def static_events(self, values: list[int]) -> None: + self._latched_names = values + + @property + def names(self) -> list[int]: + return list(self._live_names) + + def __len__(self) -> int: + return len(self._live_names) + + def add(self, event_name: int, static: bool = False) -> None: + if static: + insort(self._latched_names, event_name) + insort(self._live_names, event_name) + + def clear(self) -> None: + refreshed: dict[int, int] = {} + for event_name, frames_seen in self.event_counters.items(): + refreshed[event_name] = frames_seen + 1 if event_name in self._live_names else 0 + self.event_counters = refreshed + self._live_names = list(self._latched_names) + + def contains(self, event_type: str) -> bool: + board = self.get_events_mapping() + return any(event_type in board.get(event_name, {}) for event_name in self._live_names) + + def has(self, event_name: int) -> bool: + return event_name in self._live_names + + def contains_in_list(self, events_list: list[int]) -> bool: + return any(event_name in self._live_names for event_name in events_list) + + def remove(self, event_name: int, static: bool = False) -> None: + if static and event_name in self._latched_names: + self._latched_names.remove(event_name) + + if event_name in self._live_names: + self.event_counters[event_name] = self.event_counters.get(event_name, 0) + 1 + self._live_names.remove(event_name) + + def add_from_msg(self, events: Iterable) -> None: + for event in events: + insort(self._live_names, event.name.raw) + + def to_msg(self): + board = self.get_events_mapping() + outbound = [] + for event_name in self._live_names: + msg = self.get_event_msg_type().new_message() + msg.name = event_name + for event_kind in board.get(event_name, {}): + setattr(msg, event_kind, True) + outbound.append(msg) + return outbound + + def create_alerts(self, event_types: list[str], callback_args=None): + callback_args = [] if callback_args is None else callback_args + board = self.get_events_mapping() + spawned: list[AlertCard] = [] + for event_name in self._live_names: + variants = board.get(event_name, {}) + for event_type in event_types: + chosen = variants.get(event_type) + if chosen is None: + continue + alert = self._realize(chosen, callback_args) + age_frames = self.event_counters.get(event_name, 0) + 1 + if age_frames * DT_CTRL < alert.creation_delay: + continue + alert.alert_type = f"{self.get_event_name(event_name)}/{event_type}" + alert.event_type = event_type + spawned.append(alert) + return spawned + + @staticmethod + def _realize(candidate: AlertCard | AlertFactory, callback_args: list) -> AlertCard: + return candidate if isinstance(candidate, AlertCard) else candidate(*callback_args) + + @abstractmethod + def get_events_mapping(self) -> dict[int, dict[str, AlertCard | AlertFactory]]: + raise NotImplementedError + + @abstractmethod + def get_event_name(self, event: int) -> str: + raise NotImplementedError + + @abstractmethod + def get_event_msg_type(self): + raise NotImplementedError + + +def _mici_reframe(primary: str, secondary: str) -> tuple[str, str, log.SelfdriveState.AlertSize]: + if HARDWARE.get_device_type() == "mici": + return secondary, primary, AlertSize.small + return primary, secondary, AlertSize.mid + + +class NoEntryCard(AlertCard): + def __init__(self, + alert_text_2: str, + alert_text_1: str = "IQ.Pilot Unavailable", + visual_alert: car.CarControl.HUDControl.VisualAlert = VisualAlert.none, + priority: Tier = Tier.LOW): + primary, secondary, size = _mici_reframe(alert_text_1, alert_text_2) + super().__init__(primary, secondary, AlertStatus.normal, size, priority, visual_alert, AudibleAlert.refuse, 3.0) + + +class GentleDisableCard(AlertCard): + def __init__(self, alert_text_2: str): + super().__init__( + "TAKE CONTROL IMMEDIATELY", + alert_text_2, + AlertStatus.userPrompt, + AlertSize.full, + Tier.MID, + VisualAlert.steerRequired, + AudibleAlert.warningSoft, + 2.0, + ) + + +class PendingDisableCard(GentleDisableCard): + def __init__(self, alert_text_2: str): + super().__init__(alert_text_2) + self.alert_text_1 = "IQ.Pilot will disengage" + + +class HardDisableCard(AlertCard): + def __init__(self, alert_text_2: str): + super().__init__( + "TAKE CONTROL IMMEDIATELY", + alert_text_2, + AlertStatus.critical, + AlertSize.full, + Tier.HIGHEST, + VisualAlert.steerRequired, + AudibleAlert.warningImmediate, + 4.0, + ) + + +class ChimeCard(AlertCard): + def __init__(self, audible_alert: car.CarControl.HUDControl.AudibleAlert): + super().__init__("", "", AlertStatus.normal, AlertSize.none, Tier.MID, VisualAlert.none, audible_alert, 0.2) + + +class BannerCard(AlertCard): + def __init__(self, alert_text_1: str, alert_text_2: str = "", duration: float = 0.2, priority: Tier = Tier.LOWER, creation_delay: float = 0.0): + size = AlertSize.mid if alert_text_2 else AlertSize.small + super().__init__(alert_text_1, alert_text_2, AlertStatus.normal, size, priority, VisualAlert.none, AudibleAlert.none, duration, creation_delay) + + +class BootCard(AlertCard): + def __init__(self, alert_text_1: str, alert_text_2: str = "Always keep hands on wheel and eyes on road", alert_status=AlertStatus.normal): + if HARDWARE.get_device_type() == "mici": + compact_secondary = "" if alert_text_2 == "Always keep hands on wheel and eyes on road" else alert_text_2 + super().__init__(alert_text_1, compact_secondary, alert_status, AlertSize.small, Tier.LOWER, VisualAlert.none, AudibleAlert.none, 5.0) + else: + super().__init__(alert_text_1, alert_text_2, alert_status, AlertSize.mid, Tier.LOWER, VisualAlert.none, AudibleAlert.none, 5.0) + + +class AlertBase(AlertCard): + pass + + +NULL_ALERT = AlertCard("", "", AlertStatus.normal, AlertSize.none, Tier.LOWEST, VisualAlert.none, AudibleAlert.none, 0.0) diff --git a/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/common/basedir.py b/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/common/basedir.py new file mode 100644 index 000000000..49b092d93 --- /dev/null +++ b/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/common/basedir.py @@ -0,0 +1,4 @@ +import os + + +BASEDIR = os.path.abspath(os.path.join(os.path.dirname(os.path.realpath(__file__)), "../..")) diff --git a/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/common/constants.py b/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/common/constants.py new file mode 100644 index 000000000..7ca425c4b --- /dev/null +++ b/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/common/constants.py @@ -0,0 +1,23 @@ +import numpy as np + +# conversions +class CV: + # Speed + MPH_TO_KPH = 1.609344 + KPH_TO_MPH = 1. / MPH_TO_KPH + MS_TO_KPH = 3.6 + KPH_TO_MS = 1. / MS_TO_KPH + MS_TO_MPH = MS_TO_KPH * KPH_TO_MPH + MPH_TO_MS = MPH_TO_KPH * KPH_TO_MS + MS_TO_KNOTS = 1.9438 + KNOTS_TO_MS = 1. / MS_TO_KNOTS + + # Angle + DEG_TO_RAD = np.pi / 180. + RAD_TO_DEG = 1. / DEG_TO_RAD + + # Mass + LB_TO_KG = 0.453592 + + +ACCELERATION_DUE_TO_GRAVITY = 9.81 # m/s^2 diff --git a/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/common/file_chunker.py b/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/common/file_chunker.py new file mode 100644 index 000000000..57dfc3553 --- /dev/null +++ b/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/common/file_chunker.py @@ -0,0 +1,55 @@ +#!/usr/bin/env python3 +import sys +import math +import os +from pathlib import Path + +CHUNK_SIZE = 45 * 1024 * 1024 # 45MB, under GitHub's 50MB limit + +def get_chunk_name(name, idx, num_chunks): + return f"{name}.chunk{idx+1:02d}of{num_chunks:02d}" + +def get_manifest_path(name): + return f"{name}.chunkmanifest" + +def _chunk_paths(path, num_chunks): + return [get_manifest_path(path)] + [get_chunk_name(path, i, num_chunks) for i in range(num_chunks)] + +def get_chunk_targets(path, file_size): + num_chunks = math.ceil(file_size / CHUNK_SIZE) + return _chunk_paths(path, num_chunks) + +def chunk_file(path, targets): + manifest_path, *chunk_paths = targets + with open(path, 'rb') as f: + data = f.read() + actual_num_chunks = max(1, math.ceil(len(data) / CHUNK_SIZE)) + assert len(chunk_paths) >= actual_num_chunks, f"Allowed {len(chunk_paths)} chunks but needs at least {actual_num_chunks}, for path {path}" + for i, chunk_path in enumerate(chunk_paths): + with open(chunk_path, 'wb') as f: + f.write(data[i * CHUNK_SIZE:(i + 1) * CHUNK_SIZE]) + Path(manifest_path).write_text(str(len(chunk_paths))) + os.remove(path) + +def get_existing_chunks(path): + if os.path.isfile(path): + return [path] + if os.path.isfile(manifest := get_manifest_path(path)): + num_chunks = int(Path(manifest).read_text().strip()) + return _chunk_paths(path, num_chunks) + raise FileNotFoundError(path) + +def read_file_chunked(path): + manifest_path = get_manifest_path(path) + if os.path.isfile(manifest_path): + num_chunks = int(Path(manifest_path).read_text().strip()) + return b''.join(Path(get_chunk_name(path, i, num_chunks)).read_bytes() for i in range(num_chunks)) + if os.path.isfile(path): + return Path(path).read_bytes() + raise FileNotFoundError(path) + + +if __name__ == "__main__": + path = sys.argv[1] + chunk_paths = get_chunk_targets(path, os.path.getsize(path)) + chunk_file(path, chunk_paths) diff --git a/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/common/filter_simple.py b/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/common/filter_simple.py new file mode 100644 index 000000000..30839f66e --- /dev/null +++ b/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/common/filter_simple.py @@ -0,0 +1,71 @@ +from collections import deque + +import numpy as np + + +class FirstOrderFilter: + def __init__(self, x0, rc, dt, initialized=True): + self.x = x0 + self.dt = dt + self.update_alpha(rc) + self.initialized = initialized + + def update_alpha(self, rc): + self.alpha = self.dt / (rc + self.dt) + + def update(self, x): + if self.initialized: + self.x = (1. - self.alpha) * self.x + self.alpha * x + else: + self.initialized = True + self.x = x + return self.x + + +class BounceFilter(FirstOrderFilter): + def __init__(self, x0, rc, dt, initialized=True, bounce=2): + self.velocity = FirstOrderFilter(0.0, 0.15, dt) + self.bounce = bounce + super().__init__(x0, rc, dt, initialized) + + def update(self, x): + super().update(x) + scale = self.dt / (1.0 / 60.0) # tuned at 60 fps + self.velocity.x += (x - self.x) * self.bounce * scale * self.dt + self.velocity.update(0.0) + if abs(self.velocity.x) < 1e-5: + self.velocity.x = 0.0 + self.x += self.velocity.x + return self.x + + +class MyMovingAverage: + def __init__(self, window_size, value=None): + self.window_size = window_size + if value is not None: + self.values = deque([value] * window_size, maxlen=window_size) + self.sum = value * window_size + self.result = value + else: + self.values = deque(maxlen=window_size) + self.sum = 0 + self.result = 0 + + def set(self, value): + self.values.clear() + self.values.append(value) + self.sum = value + self.result = value + return value + + def set_all(self, value): + self.values = deque([value] * self.window_size, maxlen=self.window_size) + self.sum = value * self.window_size + self.result = value + return value + + def process(self, value, median=False): + self.values.append(value) + self.sum = sum(self.values) + self.result = float(np.median(self.values)) if median else float(self.sum) / len(self.values) + return self.result diff --git a/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/common/gpio.py b/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/common/gpio.py new file mode 100644 index 000000000..8f025a2da --- /dev/null +++ b/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/common/gpio.py @@ -0,0 +1,89 @@ +import os +import fcntl +import ctypes +from functools import cache + +def gpio_init(pin: int, output: bool) -> None: + try: + with open(f"/sys/class/gpio/gpio{pin}/direction", 'wb') as f: + f.write(b"out" if output else b"in") + except Exception as e: + print(f"Failed to set gpio {pin} direction: {e}") + +def gpio_set(pin: int, high: bool) -> None: + try: + with open(f"/sys/class/gpio/gpio{pin}/value", 'wb') as f: + f.write(b"1" if high else b"0") + except Exception as e: + print(f"Failed to set gpio {pin} value: {e}") + +def gpio_read(pin: int) -> bool | None: + val = None + try: + with open(f"/sys/class/gpio/gpio{pin}/value", 'rb') as f: + val = bool(int(f.read().strip())) + except Exception as e: + print(f"Failed to set gpio {pin} value: {e}") + + return val + +def gpio_export(pin: int) -> None: + if os.path.isdir(f"/sys/class/gpio/gpio{pin}"): + return + + try: + with open("/sys/class/gpio/export", 'w') as f: + f.write(str(pin)) + except Exception: + print(f"Failed to export gpio {pin}") + +@cache +def get_irq_action(irq: int) -> list[str]: + try: + with open(f"/sys/kernel/irq/{irq}/actions") as f: + actions = f.read().strip().split(',') + return actions + except FileNotFoundError: + return [] + +def get_irqs_for_action(action: str) -> list[str]: + ret = [] + with open("/proc/interrupts") as f: + for l in f.readlines(): + irq = l.split(':')[0].strip() + if irq.isdigit() and action in get_irq_action(irq): + ret.append(irq) + return ret + +# *** gpiochip *** + +class gpioevent_data(ctypes.Structure): + _fields_ = [ + ("timestamp", ctypes.c_uint64), + ("id", ctypes.c_uint32), + ] + +class gpioevent_request(ctypes.Structure): + _fields_ = [ + ("lineoffset", ctypes.c_uint32), + ("handleflags", ctypes.c_uint32), + ("eventflags", ctypes.c_uint32), + ("label", ctypes.c_char * 32), + ("fd", ctypes.c_int) + ] + +def gpiochip_get_ro_value_fd(label: str, gpiochip_id: int, pin: int) -> int: + GPIOEVENT_REQUEST_BOTH_EDGES = 0x3 + GPIOHANDLE_REQUEST_INPUT = 0x1 + GPIO_GET_LINEEVENT_IOCTL = 0xc030b404 + + rq = gpioevent_request() + rq.lineoffset = pin + rq.handleflags = GPIOHANDLE_REQUEST_INPUT + rq.eventflags = GPIOEVENT_REQUEST_BOTH_EDGES + rq.label = label.encode('utf-8')[:31] + b'\0' + + fd = os.open(f"/dev/gpiochip{gpiochip_id}", os.O_RDONLY) + fcntl.ioctl(fd, GPIO_GET_LINEEVENT_IOCTL, rq) + os.close(fd) + return int(rq.fd) diff --git a/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/common/i2c.py b/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/common/i2c.py new file mode 100644 index 000000000..1dfaa659a --- /dev/null +++ b/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/common/i2c.py @@ -0,0 +1,81 @@ +import os +import fcntl +import ctypes + +# I2C constants from /usr/include/linux/i2c-dev.h +I2C_SLAVE = 0x0703 +I2C_SLAVE_FORCE = 0x0706 +I2C_SMBUS = 0x0720 + +# SMBus transfer types +I2C_SMBUS_READ = 1 +I2C_SMBUS_WRITE = 0 +I2C_SMBUS_BYTE_DATA = 2 +I2C_SMBUS_I2C_BLOCK_DATA = 8 + +I2C_SMBUS_BLOCK_MAX = 32 + + +class _I2cSmbusData(ctypes.Union): + _fields_ = [ + ("byte", ctypes.c_uint8), + ("word", ctypes.c_uint16), + ("block", ctypes.c_uint8 * (I2C_SMBUS_BLOCK_MAX + 2)), + ] + + +class _I2cSmbusIoctlData(ctypes.Structure): + _fields_ = [ + ("read_write", ctypes.c_uint8), + ("command", ctypes.c_uint8), + ("size", ctypes.c_uint32), + ("data", ctypes.POINTER(_I2cSmbusData)), + ] + + +class SMBus: + def __init__(self, bus: int): + self._fd = os.open(f'/dev/i2c-{bus}', os.O_RDWR) + + def __enter__(self) -> 'SMBus': + return self + + def __exit__(self, *args) -> None: + self.close() + + def close(self) -> None: + if hasattr(self, '_fd') and self._fd >= 0: + os.close(self._fd) + self._fd = -1 + + def _set_address(self, addr: int, force: bool = False) -> None: + ioctl_arg = I2C_SLAVE_FORCE if force else I2C_SLAVE + fcntl.ioctl(self._fd, ioctl_arg, addr) + + def _smbus_access(self, read_write: int, command: int, size: int, data: _I2cSmbusData) -> None: + ioctl_data = _I2cSmbusIoctlData(read_write, command, size, ctypes.pointer(data)) + fcntl.ioctl(self._fd, I2C_SMBUS, ioctl_data) + + def read_byte_data(self, addr: int, register: int, force: bool = False) -> int: + self._set_address(addr, force) + data = _I2cSmbusData() + self._smbus_access(I2C_SMBUS_READ, register, I2C_SMBUS_BYTE_DATA, data) + return int(data.byte) + + def write_byte_data(self, addr: int, register: int, value: int, force: bool = False) -> None: + self._set_address(addr, force) + data = _I2cSmbusData() + data.byte = value & 0xFF + self._smbus_access(I2C_SMBUS_WRITE, register, I2C_SMBUS_BYTE_DATA, data) + + def read_i2c_block_data(self, addr: int, register: int, length: int, force: bool = False) -> list[int]: + self._set_address(addr, force) + if not (0 <= length <= I2C_SMBUS_BLOCK_MAX): + raise ValueError(f"length must be 0..{I2C_SMBUS_BLOCK_MAX}") + + data = _I2cSmbusData() + data.block[0] = length + self._smbus_access(I2C_SMBUS_READ, register, I2C_SMBUS_I2C_BLOCK_DATA, data) + read_len = int(data.block[0]) or length + read_len = min(read_len, length) + return [int(b) for b in data.block[1 : read_len + 1]] diff --git a/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/common/iq_perf.py b/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/common/iq_perf.py new file mode 100644 index 000000000..f36f5edef --- /dev/null +++ b/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/common/iq_perf.py @@ -0,0 +1,187 @@ +#!/usr/bin/env python3 +from __future__ import annotations + +import time +from collections import deque +from dataclasses import dataclass +from typing import Any + +import iqpilot.cereal.messaging as messaging +from iqpilot.cereal import custom +from iqpilot.common.swaglog import cloudlog + + +TRACE_SERVICE = "iqPerfTrace" +MAX_TRACE_SAMPLES = 16 +_SHARED_PM: messaging.PubMaster | None = None + + +@dataclass(slots=True) +class PerfSample: + frame_id: int = 0 + loop_dt_us: int = 0 + update_us: int = 0 + state_control_us: int = 0 + publish_us: int = 0 + tail_work_us: int = 0 + rk_remaining_us: int = 0 + stale_carcontrol_us: int = 0 + stale_carcontrol_frames: int = 0 + sendcan_gap_us: int = 0 + model_eval_us: int = 0 + model_dropped_frames: int = 0 + model_backlog: int = 0 + texture_decode_us: int = 0 + texture_upload_us: int = 0 + texture_unload_us: int = 0 + texture_prune_us: int = 0 + texture_consume_us: int = 0 + texture_batch_size: int = 0 + texture_bytes: int = 0 + texture_cache_before: int = 0 + texture_cache_after: int = 0 + texture_unloaded: int = 0 + memory_usage_percent: int = 0 + gpu_usage_percent: int = 0 + cpu_usage_percent: int = 0 + flags: int = 0 + + +class PerfTraceRing: + def __init__(self, size: int = MAX_TRACE_SAMPLES): + self._samples: deque[PerfSample] = deque(maxlen=size) + + def push(self, sample: PerfSample) -> None: + self._samples.append(sample) + + def snapshot(self) -> list[PerfSample]: + return list(self._samples) + + +class PerfTraceEmitter: + _SEVERITY_MAP = { + "info": custom.IQPerfTrace.Severity.info, + "warning": custom.IQPerfTrace.Severity.warning, + "error": custom.IQPerfTrace.Severity.error, + "critical": custom.IQPerfTrace.Severity.critical, + } + + def __init__(self, process_name: str, pubmaster: messaging.PubMaster | None = None): + self.process_name = process_name + self._pm: messaging.PubMaster | None = pubmaster + self._last_emit_mono: dict[str, float] = {} + self._disabled = False + + def _pubmaster(self) -> messaging.PubMaster: + global _SHARED_PM + if self._pm is not None: + return self._pm + if _SHARED_PM is None: + _SHARED_PM = messaging.PubMaster([TRACE_SERVICE]) + self._pm = _SHARED_PM + return self._pm + + @staticmethod + def _clamp_uint(value: int, bits: int) -> int: + return max(0, min(value, (1 << bits) - 1)) + + @staticmethod + def _clamp_int(value: int, bits: int) -> int: + lo = -(1 << (bits - 1)) + hi = (1 << (bits - 1)) - 1 + return max(lo, min(value, hi)) + + def emit(self, event_class: str, *, + severity: str = "warning", + frame_id: int = 0, + total_time_us: int = 0, + rk_remaining_us: int = 0, + batch_size: int = 0, + dropped_frames: int = 0, + backlog: int = 0, + flags: int = 0, + samples: list[PerfSample] | None = None, + missing_services: list[str] | None = None, + top_processes: list[str] | None = None, + detail: str = "", + min_interval_s: float = 0.0, + mirror_cloudlog: bool = True) -> bool: + if self._disabled: + return False + now = time.monotonic() + last_emit = self._last_emit_mono.get(event_class, 0.0) + if min_interval_s > 0.0 and (now - last_emit) < min_interval_s: + return False + self._last_emit_mono[event_class] = now + + msg = messaging.new_message(TRACE_SERVICE) + trace = msg.iqPerfTrace + trace.process = self.process_name + trace.eventClass = event_class + trace.severity = self._SEVERITY_MAP.get(severity, custom.IQPerfTrace.Severity.warning) + trace.frameId = self._clamp_uint(int(frame_id), 32) + trace.totalTimeUs = self._clamp_uint(int(total_time_us), 32) + trace.rkRemainingUs = self._clamp_int(int(rk_remaining_us), 32) + trace.batchSize = self._clamp_uint(int(batch_size), 16) + trace.droppedFrames = self._clamp_uint(int(dropped_frames), 16) + trace.backlog = self._clamp_uint(int(backlog), 16) + trace.flags = self._clamp_uint(int(flags), 32) + trace.missingServices = list(missing_services or []) + trace.topProcesses = list(top_processes or []) + trace.detail = detail + + trace_samples = samples or [] + samples_builder = trace.init("samples", len(trace_samples)) + for i, sample in enumerate(trace_samples): + builder = samples_builder[i] + builder.frameId = self._clamp_uint(int(sample.frame_id), 32) + builder.loopDtUs = self._clamp_uint(int(sample.loop_dt_us), 32) + builder.updateUs = self._clamp_uint(int(sample.update_us), 32) + builder.stateControlUs = self._clamp_uint(int(sample.state_control_us), 32) + builder.publishUs = self._clamp_uint(int(sample.publish_us), 32) + builder.tailWorkUs = self._clamp_uint(int(sample.tail_work_us), 32) + builder.rkRemainingUs = self._clamp_int(int(sample.rk_remaining_us), 32) + builder.staleCarControlUs = self._clamp_uint(int(sample.stale_carcontrol_us), 32) + builder.staleCarControlFrames = self._clamp_uint(int(sample.stale_carcontrol_frames), 16) + builder.sendcanGapUs = self._clamp_uint(int(sample.sendcan_gap_us), 32) + builder.modelEvalUs = self._clamp_uint(int(sample.model_eval_us), 32) + builder.modelDroppedFrames = self._clamp_uint(int(sample.model_dropped_frames), 16) + builder.modelBacklog = self._clamp_uint(int(sample.model_backlog), 16) + builder.textureDecodeUs = self._clamp_uint(int(sample.texture_decode_us), 32) + builder.textureUploadUs = self._clamp_uint(int(sample.texture_upload_us), 32) + builder.textureUnloadUs = self._clamp_uint(int(sample.texture_unload_us), 32) + builder.texturePruneUs = self._clamp_uint(int(sample.texture_prune_us), 32) + builder.textureConsumeUs = self._clamp_uint(int(sample.texture_consume_us), 32) + builder.textureBatchSize = self._clamp_uint(int(sample.texture_batch_size), 16) + builder.textureBytes = self._clamp_uint(int(sample.texture_bytes), 32) + builder.textureCacheBefore = self._clamp_uint(int(sample.texture_cache_before), 16) + builder.textureCacheAfter = self._clamp_uint(int(sample.texture_cache_after), 16) + builder.textureUnloaded = self._clamp_uint(int(sample.texture_unloaded), 16) + builder.memoryUsagePercent = self._clamp_uint(int(sample.memory_usage_percent), 16) + builder.gpuUsagePercent = self._clamp_uint(int(sample.gpu_usage_percent), 16) + builder.cpuUsagePercent = self._clamp_uint(int(sample.cpu_usage_percent), 16) + builder.flags = self._clamp_uint(int(sample.flags), 32) + + try: + self._pubmaster().send(TRACE_SERVICE, msg) + except messaging.MultiplePublishersError: + self._disabled = True + cloudlog.error(f"iq_perf_trace disabled for {self.process_name}: duplicate publisher for {TRACE_SERVICE}") + return False + except Exception: + cloudlog.exception(f"iq_perf_trace publish failed for {self.process_name}") + return False + + if mirror_cloudlog: + cloudlog.event( + "iq_perf_trace", + process=self.process_name, + event_class=event_class, + severity=severity, + frame_id=int(frame_id), + total_time_us=int(total_time_us), + dropped_frames=int(dropped_frames), + flags=int(flags), + detail=detail, + ) + return True diff --git a/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/common/issue_debug.py b/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/common/issue_debug.py new file mode 100644 index 000000000..e8c517a7d --- /dev/null +++ b/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/common/issue_debug.py @@ -0,0 +1,44 @@ +import os +import threading +import time +from datetime import datetime +from pathlib import Path + +from iqpilot.system.hardware import PC +from iqpilot.system.hardware.hw import Paths + + +DEBUG_FILENAME = "iqpilot_issue_debug.txt" +DEBUG_PATH = Path(Paths.comma_home()) / "community" / DEBUG_FILENAME if PC else Path("/data/community") / DEBUG_FILENAME + +_lock = threading.Lock() +_last_log_times: dict[str, float] = {} + + +def log_issue(tag: str, message: str) -> None: + try: + DEBUG_PATH.parent.mkdir(parents=True, exist_ok=True) + with _lock: + with open(DEBUG_PATH, "a", encoding="utf-8") as f: + timestamp = datetime.now().strftime("%Y-%m-%d %H:%M:%S.%f")[:-3] + f.write(f"[{timestamp}] [{tag}] {message}\n") + except OSError: + pass + + +def log_issue_limited(key: str, tag: str, message: str, interval_sec: float = 1.0) -> None: + now = time.monotonic() + with _lock: + last = _last_log_times.get(key, 0.0) + if now - last < interval_sec: + return + _last_log_times[key] = now + + log_issue(tag, message) + + +def clear_issue_debug_log() -> None: + try: + os.remove(DEBUG_PATH) + except OSError: + pass diff --git a/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/common/logging_extra.py b/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/common/logging_extra.py new file mode 100644 index 000000000..ceaf083cd --- /dev/null +++ b/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/common/logging_extra.py @@ -0,0 +1,249 @@ +import io +import os +import sys +import copy +import json +import time +import uuid +import socket +import logging +import traceback +import numpy as np +from threading import local +from collections import OrderedDict +from contextlib import contextmanager + +LOG_TIMESTAMPS = "LOG_TIMESTAMPS" in os.environ + +def json_handler(obj): + if isinstance(obj, np.bool_): + return bool(obj) + # if isinstance(obj, (datetime.date, datetime.time)): + # return obj.isoformat() + return repr(obj) + +def json_robust_dumps(obj): + return json.dumps(obj, default=json_handler) + +class NiceOrderedDict(OrderedDict): + def __str__(self): + return json_robust_dumps(self) + +class SwagFormatter(logging.Formatter): + def __init__(self, swaglogger): + logging.Formatter.__init__(self, None, '%a %b %d %H:%M:%S %Z %Y') + + self.swaglogger = swaglogger + self.host = socket.gethostname() + + def format_dict(self, record): + record_dict = NiceOrderedDict() + + if isinstance(record.msg, dict): + record_dict['msg'] = record.msg + else: + try: + record_dict['msg'] = record.getMessage() + except (ValueError, TypeError): + record_dict['msg'] = [record.msg]+record.args + + record_dict['ctx'] = self.swaglogger.get_ctx() + + if record.exc_info: + record_dict['exc_info'] = self.formatException(record.exc_info) + + record_dict['level'] = record.levelname + record_dict['levelnum'] = record.levelno + record_dict['name'] = record.name + record_dict['filename'] = record.filename + record_dict['lineno'] = record.lineno + record_dict['pathname'] = record.pathname + record_dict['module'] = record.module + record_dict['funcName'] = record.funcName + record_dict['host'] = self.host + record_dict['process'] = record.process + record_dict['thread'] = record.thread + record_dict['threadName'] = record.threadName + record_dict['created'] = record.created + + return record_dict + + def format(self, record): + if self.swaglogger is None: + raise Exception("must set swaglogger before calling format()") + return json_robust_dumps(self.format_dict(record)) + +class SwagLogFileFormatter(SwagFormatter): + def fix_kv(self, k, v): + # append type to names to preserve legacy naming in logs + # avoids overlapping key namespaces with different types + # e.g. log.info() creates 'msg' -> 'msg$s' + # log.event() creates 'msg.health.logMonoTime' -> 'msg.health.logMonoTime$i' + # because overlapping namespace 'msg' caused problems + if isinstance(v, (str, bytes)): + k += "$s" + elif isinstance(v, float): + k += "$f" + elif isinstance(v, bool): + k += "$b" + elif isinstance(v, int): + k += "$i" + elif isinstance(v, dict): + nv = {} + for ik, iv in v.items(): + ik, iv = self.fix_kv(ik, iv) + nv[ik] = iv + v = nv + elif isinstance(v, list): + k += "$a" + return k, v + + def format(self, record): + if isinstance(record, str): + v = json.loads(record) + else: + v = self.format_dict(record) + + mk, mv = self.fix_kv('msg', v['msg']) + del v['msg'] + v[mk] = mv + v['id'] = uuid.uuid4().hex + + return json_robust_dumps(v) + +class SwagErrorFilter(logging.Filter): + def filter(self, record): + return record.levelno < logging.ERROR + +def _tmpfunc(): + return 0 + +def _srcfile(): + return os.path.normcase(_tmpfunc.__code__.co_filename) + +class SwagLogger(logging.Logger): + def __init__(self): + logging.Logger.__init__(self, "swaglog") + + self.global_ctx = {} + + self.log_local = local() + self.log_local.ctx = {} + + def local_ctx(self): + try: + return self.log_local.ctx + except AttributeError: + self.log_local.ctx = {} + return self.log_local.ctx + + def get_ctx(self): + return dict(self.local_ctx(), **self.global_ctx) + + @contextmanager + def ctx(self, **kwargs): + old_ctx = self.local_ctx() + self.log_local.ctx = copy.copy(old_ctx) or {} + self.log_local.ctx.update(kwargs) + try: + yield + finally: + self.log_local.ctx = old_ctx + + def bind(self, **kwargs): + self.local_ctx().update(kwargs) + + def bind_global(self, **kwargs): + self.global_ctx.update(kwargs) + + def event(self, event, *args, **kwargs): + evt = NiceOrderedDict() + evt['event'] = event + if args: + evt['args'] = args + evt.update(kwargs) + if 'error' in kwargs: + self.error(evt) + elif 'debug' in kwargs: + self.debug(evt) + else: + self.info(evt) + + def timestamp(self, event_name): + if LOG_TIMESTAMPS: + t = time.monotonic() + tstp = NiceOrderedDict() + tstp['timestamp'] = NiceOrderedDict() + tstp['timestamp']["event"] = event_name + tstp['timestamp']["time"] = t*1e9 + self.debug(tstp) + + def findCaller(self, stack_info=False, stacklevel=1): + """ + Find the stack frame of the caller so that we can note the source + file name, line number and function name. + """ + f = sys._getframe(3) + #On some versions of IronPython, currentframe() returns None if + #IronPython isn't run with -X:Frames. + if f is not None: + f = f.f_back + orig_f = f + while f and stacklevel > 1: + f = f.f_back + stacklevel -= 1 + if not f: + f = orig_f + rv = "(unknown file)", 0, "(unknown function)", None + while hasattr(f, "f_code"): + co = f.f_code + filename = os.path.normcase(co.co_filename) + + if filename == _srcfile: + f = f.f_back + continue + sinfo = None + if stack_info: + sio = io.StringIO() + sio.write('Stack (most recent call last):\n') + traceback.print_stack(f, file=sio) + sinfo = sio.getvalue() + if sinfo[-1] == '\n': + sinfo = sinfo[:-1] + sio.close() + rv = (co.co_filename, f.f_lineno, co.co_name, sinfo) + break + return rv + +if __name__ == "__main__": + log = SwagLogger() + + stdout_handler = logging.StreamHandler(sys.stdout) + stdout_handler.setLevel(logging.INFO) + stdout_handler.addFilter(SwagErrorFilter()) + log.addHandler(stdout_handler) + + stderr_handler = logging.StreamHandler(sys.stderr) + stderr_handler.setLevel(logging.ERROR) + log.addHandler(stderr_handler) + + log.info("asdasd %s", "a") + log.info({'wut': 1}) + log.warning("warning") + log.error("error") + log.critical("critical") + log.event("test", x="y") + + with log.ctx(): + stdout_handler.setFormatter(SwagFormatter(log)) + stderr_handler.setFormatter(SwagFormatter(log)) + log.bind(user="some user") + log.info("in req") + print("") + log.warning("warning") + print("") + log.error("error") + print("") + log.critical("critical") + print("") + log.event("do_req", a=1, b="c") diff --git a/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/common/realtime.py b/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/common/realtime.py new file mode 100644 index 000000000..9fd2f695d --- /dev/null +++ b/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/common/realtime.py @@ -0,0 +1,134 @@ +"""Utilities for reading real time clocks and keeping soft real time constraints.""" +import gc +import os +import sys +import time + +from setproctitle import getproctitle + +from iqpilot.common.utils import MovingAverage +from iqpilot.system.hardware import PC + + +# time step for each process +DT_CTRL = 0.01 # controlsd +DT_MDL = 0.05 # model +DT_HW = 0.5 # hardwared and manager +DT_DMON = 0.05 # driver monitoring + + +class Priority: + # CORE 2 + # - modeld = 55 + # - camerad = 54 + CTRL_LOW = 51 # plannerd & radard + + # CORE 3 + # - pandad = 55 + CTRL_HIGH = 53 + + +def set_core_affinity(cores: list[int]) -> None: + if sys.platform == 'linux' and not PC: + os.sched_setaffinity(0, cores) + + +def config_realtime_process(cores: int | list[int], priority: int) -> None: + gc.disable() + if sys.platform == 'linux' and not PC: + os.sched_setscheduler(0, os.SCHED_FIFO, os.sched_param(priority)) + c = cores if isinstance(cores, list) else [cores, ] + set_core_affinity(c) + + +def config_background_thread() -> None: + if sys.platform == 'linux' and not PC: + os.sched_setscheduler(0, os.SCHED_OTHER, os.sched_param(0)) + set_core_affinity(list(range(os.cpu_count() or 1))) + + +def lock_memory() -> None: + """mlockall this process so memory reclaim/compaction can't stall it. RT control + procs only (locking ui/modeld would worsen pressure). Best-effort.""" + if sys.platform != 'linux' or PC: + return + try: + import ctypes + import resource + resource.setrlimit(resource.RLIMIT_MEMLOCK, (resource.RLIM_INFINITY, resource.RLIM_INFINITY)) + MCL_CURRENT, MCL_FUTURE = 0x1, 0x2 + libc = ctypes.CDLL("libc.so.6", use_errno=True) + if libc.mlockall(MCL_CURRENT | MCL_FUTURE) != 0: + raise OSError(ctypes.get_errno(), os.strerror(ctypes.get_errno())) + except Exception as e: + try: + from iqpilot.common.swaglog import cloudlog + cloudlog.warning(f"lock_memory (mlockall) failed: {e}") + except Exception: + pass + + +class Ratekeeper: + def __init__(self, rate: float, print_delay_threshold: float | None = 0.0) -> None: + """Rate in Hz for ratekeeping. print_delay_threshold must be nonnegative.""" + self._interval = 1. / rate + self._print_delay_threshold = print_delay_threshold + self._frame = 0 + self._remaining = 0.0 + self._process_name = getproctitle() + self._last_monitor_time = -1. + self._next_frame_time = -1. + + self.avg_dt = MovingAverage(100) + self.avg_dt.add_value(self._interval) + + def reset(self) -> None: + self._remaining = 0.0 + self._last_monitor_time = -1. + self._next_frame_time = -1. + self.avg_dt = MovingAverage(100) + self.avg_dt.add_value(self._interval) + + @property + def frame(self) -> int: + return self._frame + + @property + def remaining(self) -> float: + return self._remaining + + @property + def lag(self) -> float: + return max(0., -self._remaining) + + @property + def lagging(self) -> bool: + expected_dt = self._interval * (1 / 0.9) + return self.avg_dt.get_average() > expected_dt + + # Maintain loop rate by calling this at the end of each loop + def keep_time(self) -> bool: + lagged = self.monitor_time() + if self._remaining > 0: + time.sleep(self._remaining) + return lagged + + # Monitors the cumulative lag, but does not enforce a rate + def monitor_time(self) -> bool: + if self._last_monitor_time < 0: + self._next_frame_time = time.monotonic() + self._interval + self._last_monitor_time = time.monotonic() + + prev = self._last_monitor_time + self._last_monitor_time = time.monotonic() + self.avg_dt.add_value(self._last_monitor_time - prev) + + lagged = False + remaining = self._next_frame_time - time.monotonic() + self._next_frame_time += self._interval + if self._print_delay_threshold is not None and remaining < -self._print_delay_threshold: + print(f"{self._process_name} lagging by {-remaining * 1000:.2f} ms") + lagged = True + self._frame += 1 + self._remaining = remaining + return lagged diff --git a/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/common/spinner.py b/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/common/spinner.py new file mode 100644 index 000000000..b823d09a4 --- /dev/null +++ b/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/common/spinner.py @@ -0,0 +1,52 @@ +import os +import subprocess +from iqpilot.common.basedir import BASEDIR + + +class Spinner: + def __init__(self): + try: + self.spinner_proc = subprocess.Popen(["./spinner.py"], + stdin=subprocess.PIPE, + cwd=os.path.join(BASEDIR, "iqpilot", "system", "ui"), + close_fds=True) + except OSError: + self.spinner_proc = None + + def __enter__(self): + return self + + def update(self, spinner_text: str): + if self.spinner_proc is not None: + self.spinner_proc.stdin.write(spinner_text.encode('utf8') + b"\n") + try: + self.spinner_proc.stdin.flush() + except BrokenPipeError: + pass + + def update_progress(self, cur: float, total: float): + self.update(str(round(100 * cur / total))) + + def close(self): + if self.spinner_proc is not None: + self.spinner_proc.kill() + try: + self.spinner_proc.communicate(timeout=2.) + except subprocess.TimeoutExpired: + print("WARNING: failed to kill spinner") + self.spinner_proc = None + + def __del__(self): + self.close() + + def __exit__(self, exc_type, exc_value, traceback): + self.close() + + +if __name__ == "__main__": + import time + with Spinner() as s: + s.update("Spinner text") + time.sleep(5.0) + print("gone") + time.sleep(5.0) diff --git a/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/common/steer_delay.py b/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/common/steer_delay.py new file mode 100644 index 000000000..349f975d3 --- /dev/null +++ b/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/common/steer_delay.py @@ -0,0 +1,60 @@ +""" +Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos + +Chooses which steer-actuator delay the lateral controllers run with: the value the +live estimator learned, or the driver's fixed software delay — gated by the +"IQLiveSteerDelay" param. The pick is mirrored into "IQSteerDelayCache" so consumers that do +not subscribe to lateralDelay can still read the current value. +""" +from iqpilot.cereal import car +from iqpilot.common.params import Params + +_ENABLE_KEY = "IQLiveSteerDelay" +_FIXED_KEY = "IQSoftwareSteerDelay" +_CACHE_KEY = "IQSteerDelayCache" + + +def fixed_steer_delay(params, stock_delay): + """The rack's own delay plus the driver's IQSoftwareSteerDelay offset, as the UI reports it.""" + return stock_delay + float(params.get(_FIXED_KEY, return_default=True)) + + +def resolve_steer_delay(params, stock_delay): + """Learned lateral delay while live-learning is enabled, otherwise the driver's fixed delay.""" + if not params.get_bool(_ENABLE_KEY): + return fixed_steer_delay(params, stock_delay) + return float(params.get(_CACHE_KEY, return_default=True)) + + +def lateral_action_delay(params, car_params, live_delay): + """Delay the lateral path should be planned against. + + Angle cars honour the IQLiveSteerDelay toggle so that with live learning off the + estimate never reaches the path: lagd cross-correlates against localizer lateral + accel, so it reports whole-vehicle response (~0.36 s measured on VW MQB, 0.44 s on + Tesla) where the lookahead wants actuator delay (~0.10 s). Torque cars keep the + live estimate. + """ + if car_params.steerControlType == car.CarParams.SteerControlType.angle: + return resolve_steer_delay(params, car_params.steerActuatorDelay) + return live_delay + + +def cached_steer_delay(): + """Last value SteerDelayPublisher mirrored into the param — usable without a + lateralDelay subscription (e.g. at process startup).""" + return Params().get(_CACHE_KEY, return_default=True) + + +class SteerDelayPublisher: + """Refreshes IQSteerDelayCache every lag message: the learned live delay when the + toggle is on, else the actuator delay plus the driver's fixed software offset.""" + + def __init__(self, car_params): + self._params = Params() + self._actuator_delay = car_params.steerActuatorDelay + + def update(self, lag_msg): + live = self._params.get_bool(_ENABLE_KEY) + value = lag_msg.lateralDelay.lateralDelay if live else fixed_steer_delay(self._params, self._actuator_delay) + self._params.put_nonblocking(_CACHE_KEY, value) diff --git a/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/common/swaglog.py b/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/common/swaglog.py new file mode 100644 index 000000000..f8fd9d392 --- /dev/null +++ b/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/common/swaglog.py @@ -0,0 +1,165 @@ +import logging +import os +import sys +import time +import warnings +from pathlib import Path +from logging.handlers import BaseRotatingHandler + +import zmq + +from iqpilot.common.logging_extra import SwagLogger, SwagFormatter, SwagLogFileFormatter +from iqpilot.system.hardware.hw import Paths + + +def get_file_handler(): + Path(Paths.swaglog_root()).mkdir(parents=True, exist_ok=True) + base_filename = os.path.join(Paths.swaglog_root(), "swaglog") + handler = SwaglogRotatingFileHandler(base_filename) + return handler + +class SwaglogRotatingFileHandler(BaseRotatingHandler): + def __init__(self, base_filename, interval=60, max_bytes=1024*256, backup_count=2500, encoding=None): + super().__init__(base_filename, mode="a", encoding=encoding, delay=True) + self.base_filename = base_filename + self.interval = interval # seconds + self.max_bytes = max_bytes + self.backup_count = backup_count + self.log_files = self.get_existing_logfiles() + log_indexes = [f.split(".")[-1] for f in self.log_files] + self.last_file_idx = max([int(i) for i in log_indexes if i.isdigit()] or [-1]) + self.last_rollover = None + self.doRollover() + + def _open(self): + self.last_rollover = time.monotonic() + self.last_file_idx += 1 + next_filename = f"{self.base_filename}.{self.last_file_idx:010}" + stream = open(next_filename, self.mode, encoding=self.encoding) + self.log_files.insert(0, next_filename) + return stream + + def get_existing_logfiles(self): + log_files = list() + base_dir = os.path.dirname(self.base_filename) + for fn in os.listdir(base_dir): + fp = os.path.join(base_dir, fn) + if fp.startswith(self.base_filename) and os.path.isfile(fp): + log_files.append(fp) + return sorted(log_files) + + def shouldRollover(self, record): + size_exceeded = self.max_bytes > 0 and self.stream.tell() >= self.max_bytes + time_exceeded = self.interval > 0 and self.last_rollover + self.interval <= time.monotonic() + return size_exceeded or time_exceeded + + def doRollover(self): + if self.stream: + self.stream.close() + self.stream = self._open() + + if self.backup_count > 0: + while len(self.log_files) > self.backup_count: + to_delete = self.log_files.pop() + if os.path.exists(to_delete): # just being safe, should always exist + os.remove(to_delete) + +class UnixDomainSocketHandler(logging.Handler): + def __init__(self, formatter): + logging.Handler.__init__(self) + self.setFormatter(formatter) + self.pid = None + + self.zctx = None + self.sock = None + + def __del__(self): + self.close() + + def close(self): + if self.sock is not None: + self.sock.close() + if self.zctx is not None: + self.zctx.term() + + def connect(self): + self.zctx = zmq.Context() + self.sock = self.zctx.socket(zmq.PUSH) + self.sock.setsockopt(zmq.LINGER, 10) + self.sock.connect(Paths.swaglog_ipc()) + self.pid = os.getpid() + + def emit(self, record): + if os.getpid() != self.pid: + # TODO suppresses warning about forking proc with zmq socket, fix root cause + warnings.filterwarnings("ignore", category=ResourceWarning, message="unclosed.*") + self.connect() + + msg = self.format(record).rstrip('\n') + # print("SEND".format(repr(msg))) + try: + s = chr(record.levelno)+msg + self.sock.send(s.encode('utf8'), zmq.NOBLOCK) + except zmq.error.Again: + # drop :/ + pass + + +class ForwardingHandler(logging.Handler): + def __init__(self, target_logger): + super().__init__() + self.target_logger = target_logger + + def emit(self, record): + self.target_logger.handle(record) + + +def add_file_handler(log): + """ + Function to add the file log handler to swaglog. + This can be used to store logs when logmessaged is not running. + """ + handler = get_file_handler() + handler.setFormatter(SwagLogFileFormatter(log)) + log.addHandler(handler) + + +cloudlog = log = SwagLogger() +log.setLevel(logging.DEBUG) + + +class PrettyConsoleFormatter(logging.Formatter): + # StreamHandler writes to stderr, so tty-gate on that + _COLOR = sys.stderr.isatty() and os.environ.get('NO_COLOR') is None + + def format(self, record): + msg = record.getMessage() + if not self._COLOR: + return f"{record.filename}: {msg}" + lvl = record.levelno + if lvl >= 50: lc, ln = "\033[1;38;5;196m", "CRIT" + elif lvl >= 40: lc, ln = "\033[1;38;5;203m", " ERR" + elif lvl >= 30: lc, ln = "\033[38;5;214m", "WARN" + elif lvl >= 20: lc, ln = "\033[38;5;110m", "info" + else: lc, ln = "\033[38;5;244m", " dbg" + body = f"\033[1;38;5;210m{msg}\033[0m" if lvl >= 40 else msg + src = "" if record.filename == "(unknown file)" else f"\033[2m{record.filename}\033[0m " + return f"{lc}{ln:>4}\033[0m {src}{body}" + + +outhandler = logging.StreamHandler() +outhandler.setFormatter(PrettyConsoleFormatter()) + +print_level = os.environ.get('LOGPRINT', 'warning') +if print_level == 'debug': + outhandler.setLevel(logging.DEBUG) +elif print_level == 'info': + outhandler.setLevel(logging.INFO) +elif print_level == 'warning': + outhandler.setLevel(logging.WARNING) + +ipchandler = UnixDomainSocketHandler(SwagFormatter(log)) + +log.addHandler(outhandler) +# logs are sent through IPC before writing to disk to prevent disk I/O blocking +log.addHandler(ipchandler) diff --git a/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/common/time_helpers.py b/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/common/time_helpers.py new file mode 100644 index 000000000..8564e270c --- /dev/null +++ b/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/common/time_helpers.py @@ -0,0 +1,15 @@ +import datetime +from pathlib import Path + +MIN_DATE = datetime.datetime(year=2025, month=2, day=21) + +def min_date(): + # on systemd systems, the default time is the systemd build time + systemd_path = Path("/lib/systemd/systemd") + if systemd_path.exists(): + d = datetime.datetime.fromtimestamp(systemd_path.stat().st_mtime) + return max(MIN_DATE, d + datetime.timedelta(days=1)) + return MIN_DATE + +def system_time_valid(): + return datetime.datetime.now() > min_date() diff --git a/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/common/utils.py b/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/common/utils.py new file mode 100644 index 000000000..e0aa1605d --- /dev/null +++ b/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/common/utils.py @@ -0,0 +1,271 @@ +import io +import os +import tempfile +import contextlib +import subprocess +import time +import functools +from subprocess import Popen, PIPE, TimeoutExpired +import zstandard as zstd + +LOG_COMPRESSION_LEVEL = 10 # little benefit up to level 15. level ~17 is a small step change + +class Timer: + """Simple lap timer for profiling sequential operations.""" + + def __init__(self): + self._start = self._lap = time.monotonic() + self._sections = {} + + def lap(self, name): + now = time.monotonic() + self._sections[name] = now - self._lap + self._lap = now + + @property + def total(self): + return time.monotonic() - self._start + + def fmt(self, duration): + parts = ", ".join(f"{k}={v:.2f}s" + (f" ({duration/v:.0f}x)" if k == 'render' and v > 0 else "") for k, v in self._sections.items()) + total = self.total + realtime = f"{duration/total:.1f}x realtime" if total > 0 else "N/A" + return f"{duration}s in {total:.1f}s ({realtime}) | {parts}" + +def sudo_write(val: str, path: str) -> None: + try: + with open(path, 'w') as f: + f.write(str(val)) + except PermissionError: + os.system(f"sudo chmod a+w {path}") + try: + with open(path, 'w') as f: + f.write(str(val)) + except PermissionError: + # fallback for debugfs files + os.system(f"sudo su -c 'echo {val} > {path}'") + + +def sudo_read(path: str) -> str: + try: + return subprocess.check_output(f"sudo cat {path}", shell=True, encoding='utf8').strip() + except Exception: + return "" + + +class MovingAverage: + def __init__(self, window_size: int): + self.window_size: int = window_size + self.buffer: list[float] = [0.0] * window_size + self.index: int = 0 + self.count: int = 0 + self.sum: float = 0.0 + + def add_value(self, new_value: float): + # Update the sum: subtract the value being replaced and add the new value + self.sum -= self.buffer[self.index] + self.buffer[self.index] = new_value + self.sum += new_value + + # Update the index in a circular manner + self.index = (self.index + 1) % self.window_size + + # Track the number of added values (for partial windows) + self.count = min(self.count + 1, self.window_size) + + def get_average(self) -> float: + if self.count == 0: + return float('nan') + return self.sum / self.count + + +class CallbackReader: + """Wraps a file, but overrides the read method to also + call a callback function with the number of bytes read so far.""" + + def __init__(self, f, callback, *args): + self.f = f + self.callback = callback + self.cb_args = args + self.total_read = 0 + + def __getattr__(self, attr): + return getattr(self.f, attr) + + def read(self, *args, **kwargs): + chunk = self.f.read(*args, **kwargs) + self.total_read += len(chunk) + self.callback(*self.cb_args, self.total_read) + return chunk + + +@contextlib.contextmanager +def atomic_write(path: str, mode: str = 'w', buffering: int = -1, encoding: str | None = None, newline: str | None = None, + overwrite: bool = False): + """Write to a file atomically using a temporary file in the same directory as the destination file.""" + dir_name = os.path.dirname(path) + + if not overwrite and os.path.exists(path): + raise FileExistsError(f"File '{path}' already exists. To overwrite it, set 'overwrite' to True.") + + with tempfile.NamedTemporaryFile(mode=mode, buffering=buffering, encoding=encoding, newline=newline, dir=dir_name, delete=False) as tmp_file: + yield tmp_file + tmp_file_name = tmp_file.name + os.replace(tmp_file_name, path) + + +def get_upload_stream(filepath: str, should_compress: bool) -> tuple[io.BufferedIOBase, int]: + if not should_compress: + file_size = os.path.getsize(filepath) + file_stream = open(filepath, "rb") + return file_stream, file_size + + # Compress the file on the fly + compressed_stream = io.BytesIO() + compressor = zstd.ZstdCompressor(level=LOG_COMPRESSION_LEVEL) + + with open(filepath, "rb") as f: + compressor.copy_stream(f, compressed_stream) + compressed_size = compressed_stream.tell() + compressed_stream.seek(0) + return compressed_stream, compressed_size + + +# remove all keys that end in DEPRECATED +def strip_deprecated_keys(d): + for k in list(d.keys()): + if isinstance(k, str): + if k.endswith('DEPRECATED'): + d.pop(k) + elif isinstance(d[k], dict): + strip_deprecated_keys(d[k]) + return d + + +def run_cmd(cmd: list[str], cwd=None, env=None) -> str: + return subprocess.check_output(cmd, encoding='utf8', cwd=cwd, env=env).strip() + + +def run_cmd_default(cmd: list[str], default: str = "", cwd=None, env=None) -> str: + try: + return run_cmd(cmd, cwd=cwd, env=env) + except subprocess.CalledProcessError: + return default + + +@contextlib.contextmanager +def managed_proc(cmd: list[str], env: dict[str, str]): + proc = Popen(cmd, env=env, stdout=PIPE, stderr=PIPE) + try: + yield proc + finally: + if proc.poll() is None: + proc.terminate() + try: + proc.wait(timeout=5) + except TimeoutExpired: + proc.kill() + + +def tabulate(tabular_data, headers=(), tablefmt="simple", floatfmt="g", stralign="left", numalign=None): + rows = [list(row) for row in tabular_data] + + def fmt(val): + if isinstance(val, str): + return val + if isinstance(val, (bool, int)): + return str(val) + try: + return format(val, floatfmt) + except (TypeError, ValueError): + return str(val) + + formatted = [[fmt(c) for c in row] for row in rows] + hdrs = [str(h) for h in headers] if headers else None + + ncols = max((len(r) for r in formatted), default=0) + if hdrs: + ncols = max(ncols, len(hdrs)) + if ncols == 0: + return "" + + for r in formatted: + r.extend([""] * (ncols - len(r))) + if hdrs: + hdrs.extend([""] * (ncols - len(hdrs))) + + widths = [0] * ncols + if hdrs: + for i in range(ncols): + widths[i] = len(hdrs[i]) + for row in formatted: + for i in range(ncols): + widths[i] = max(widths[i], max(len(ln) for ln in row[i].split('\n'))) + + def _align(s, w): + if stralign == "center": + return s.center(w) + return s.ljust(w) + + if tablefmt == "html": + parts = [""] + if hdrs: + parts.append("") + parts.append("" + "".join(f"" for h in hdrs) + "") + parts.append("") + parts.append("") + for row in formatted: + parts.append("" + "".join(f"" for c in row) + "") + parts.append("") + parts.append("
{h}
{c}
") + return "\n".join(parts) + + if tablefmt == "simple_grid": + def _sep(left, mid, right): + return left + mid.join("─" * (w + 2) for w in widths) + right + + top, mid_sep, bot = _sep("┌", "┬", "┐"), _sep("├", "┼", "┤"), _sep("└", "┴", "┘") + + def _fmt_row(cells): + split = [c.split('\n') for c in cells] + nlines = max(len(s) for s in split) + for s in split: + s.extend([""] * (nlines - len(s))) + return ["│" + "│".join(f" {_align(split[i][li], widths[i])} " for i in range(ncols)) + "│" for li in range(nlines)] + + lines = [top] + if hdrs: + lines.extend(_fmt_row(hdrs)) + lines.append(mid_sep) + for ri, row in enumerate(formatted): + lines.extend(_fmt_row(row)) + lines.append(mid_sep if ri < len(formatted) - 1 else bot) + return "\n".join(lines) + + gap = " " + lines = [] + if hdrs: + lines.append(gap.join(h.ljust(w) for h, w in zip(hdrs, widths, strict=True))) + lines.append(gap.join("-" * w for w in widths)) + for row in formatted: + lines.append(gap.join(_align(row[i], widths[i]) for i in range(ncols))) + return "\n".join(lines) + + +def retry(attempts=3, delay=1.0, ignore_failure=False): + def decorator(func): + @functools.wraps(func) + def wrapper(*args, **kwargs): + for _ in range(attempts): + try: + return func(*args, **kwargs) + except Exception: + print(f"{func.__name__} failed, trying again") + time.sleep(delay) + + if ignore_failure: + print(f"{func.__name__} failed after retry") + else: + raise Exception(f"{func.__name__} failed after retry") + return wrapper + return decorator diff --git a/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/konn3kt/__init__.py b/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/konn3kt/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/konn3kt/cloud_client.py b/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/konn3kt/cloud_client.py new file mode 100644 index 000000000..2897609eb --- /dev/null +++ b/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/konn3kt/cloud_client.py @@ -0,0 +1,17 @@ +""" +Copyright ©️ IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos +""" + + +import os +from iqpilot.common.api.base import BaseApi +API_HOST = os.getenv('KONN3KT_API_HOST', 'https://api-iqlabs.konn3kt.com') + +class Konn3ktApi(BaseApi): + + def __init__(self, dongle_id): + super().__init__(dongle_id, API_HOST) + self.user_agent = "konn3kt-device-" + + def get_token(self, expiry_hours=1): + return super()._get_token(expiry_hours=expiry_hours) diff --git a/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/konn3kt/registration.py b/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/konn3kt/registration.py new file mode 100644 index 000000000..1ef907dd9 --- /dev/null +++ b/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/konn3kt/registration.py @@ -0,0 +1,226 @@ +#!/usr/bin/env python3 +import os +import time +import json +import jwt +import re +import secrets +from typing import cast +from pathlib import Path + +from datetime import datetime, timedelta, UTC +from iqpilot.common.api import api_get, get_key_pair +from iqpilot.common.params import Params +from iqpilot.common.spinner import Spinner +from iqpilot.system.hardware import HARDWARE, PC +from iqpilot.system.hardware.hw import Paths +from iqpilot.common.swaglog import cloudlog + + +UNREGISTERED_DONGLE_ID = "UnregisteredDevice" + +_DONGLE_ID_RE = re.compile(r"^[a-fA-F0-9]{16}$") +IMEI_WAIT_TIMEOUT = 15.0 + + +def _read_persist_dongle_id() -> str | None: + p = Path(Paths.persist_root()) / "comma" / "dongle_id" + try: + if not p.is_file(): + return None + s = p.read_text().strip() + return s or None + except Exception: + cloudlog.exception("failed to read persist dongle_id") + return None + + +def get_cached_dongle_id(params: Params | None = None, prefer_readonly: bool = True) -> str | None: + ro = _read_persist_dongle_id() + if is_valid_dongle_id(ro): + ro = ro.lower() + if prefer_readonly and ro: + return ro + p = Params() if params is None else params + v = p.get("DongleId") + if v and v != UNREGISTERED_DONGLE_ID: + return v.lower() if is_valid_dongle_id(v) else v + return ro or None +def is_valid_dongle_id(dongle_id: str | None) -> bool: + return bool(dongle_id and _DONGLE_ID_RE.fullmatch(dongle_id)) +def get_or_create_dongle_id(params: Params | None = None, prefer_readonly: bool = True) -> str: + p = Params() if params is None else params + dongle_id = get_cached_dongle_id(p, prefer_readonly=prefer_readonly) + if dongle_id and dongle_id != UNREGISTERED_DONGLE_ID: + return dongle_id + dongle_id = secrets.token_hex(8) + p.put("DongleId", dongle_id) + cloudlog.warning(f"generated new DongleId={dongle_id} (no readonly dongle_id found)") + return dongle_id +def ensure_dev_pairing_identity(params: Params | None = None, force_reset: bool = False) -> dict[str, str]: + p = Params() if params is None else params + + persist_dir = Path(Paths.persist_root()) / "comma" + persist_dir.mkdir(parents=True, exist_ok=True) + + dongle_path = persist_dir / "dongle_id" + priv_path = persist_dir / "id_rsa" + pub_path = persist_dir / "id_rsa.pub" + + if force_reset: + for fp in (dongle_path, priv_path, pub_path): + try: + fp.unlink(missing_ok=True) + except Exception: + cloudlog.exception(f"failed to remove {fp}") + try: + (persist_dir / "konn3kt_prime_type").unlink(missing_ok=True) + except Exception: + pass + try: + p.remove("PrimeType") + except Exception: + pass + forced_dongle = os.getenv("KONN3KT_DEV_DONGLE_ID") + dongle_id = forced_dongle.strip().lower() if forced_dongle else None + if dongle_id and not is_valid_dongle_id(dongle_id): + cloudlog.error("KONN3KT_DEV_DONGLE_ID must be 16 hex chars") + dongle_id = None + if dongle_id is None: + existing = None + try: + existing = dongle_path.read_text().strip().lower() if dongle_path.is_file() else None + except Exception: + cloudlog.exception("failed reading existing dev dongle_id") + dongle_id = existing if is_valid_dongle_id(existing) else secrets.token_hex(8) + try: + dongle_path.write_text(dongle_id) + except Exception: + cloudlog.exception("failed writing dev dongle_id") + p.put("DongleId", dongle_id) + p.put("HardwareSerial", p.get("HardwareSerial") or f"DEV-{dongle_id}") + if force_reset or (not priv_path.is_file()) or (not pub_path.is_file()): + try: + from cryptography.hazmat.primitives import serialization + from cryptography.hazmat.primitives.asymmetric import rsa + key = rsa.generate_private_key(public_exponent=65537, key_size=2048) + priv_bytes = key.private_bytes( + encoding=serialization.Encoding.PEM, + format=serialization.PrivateFormat.TraditionalOpenSSL, + encryption_algorithm=serialization.NoEncryption(), + ) + pub_bytes = key.public_key().public_bytes( + encoding=serialization.Encoding.PEM, + format=serialization.PublicFormat.SubjectPublicKeyInfo, + ) + priv_path.write_bytes(priv_bytes) + pub_path.write_bytes(pub_bytes) + except Exception: + cloudlog.exception("failed generating dev RSA keys") + raise + return { + "dongle_id": dongle_id, + "serial": p.get("HardwareSerial") or f"DEV-{dongle_id}", + "persist_dir": str(persist_dir), + } +def is_registered_device() -> bool: + dongle = Params().get("DongleId") + return dongle not in (None, UNREGISTERED_DONGLE_ID) + + +def _normalize_imei(value: str | None) -> str: + return value or "" + + +def get_registration_identifiers(wait_timeout: float = IMEI_WAIT_TIMEOUT, show_spinner: bool = False) -> tuple[str, str, str]: + serial = HARDWARE.get_serial() + spinner = Spinner() if show_spinner else None + start_time = time.monotonic() + imei1: str | None = None + imei2: str | None = None + + while time.monotonic() - start_time < wait_timeout: + try: + imei1, imei2 = HARDWARE.get_imei(0), HARDWARE.get_imei(1) + if imei1 or imei2: + break + except RuntimeError as e: + if "no modems" in str(e).lower(): + cloudlog.warning("No cellular modem available, proceeding without IMEI") + break + cloudlog.exception("Error getting imei, trying again...") + except Exception: + cloudlog.exception("Error getting imei, trying again...") + time.sleep(1) + + imei1 = _normalize_imei(imei1) + imei2 = _normalize_imei(imei2) + + if not imei1 and not imei2: + cloudlog.warning(f"proceeding with serial-only registration for serial={serial}") + if spinner is not None: + spinner.update(f"registering device - serial: {serial}, IMEI: ({imei1 or None}, {imei2 or None})") + spinner.close() + + return serial, imei1, imei2 + + +def register(show_spinner=False) -> str | None: + params = Params() + + dongle_id: str | None = get_cached_dongle_id(params, prefer_readonly=True) + if dongle_id in ("", UNREGISTERED_DONGLE_ID): + dongle_id = None + + jwt_algo, private_key, public_key = get_key_pair() + + if not public_key: + dongle_id = UNREGISTERED_DONGLE_ID + cloudlog.warning("missing public key") + elif dongle_id is None: + if show_spinner: + spinner = Spinner() + spinner.update("registering device") + + serial, imei1, imei2 = get_registration_identifiers(wait_timeout=IMEI_WAIT_TIMEOUT, show_spinner=False) + + backoff = 0 + start_time = time.monotonic() + while True: + try: + register_token = jwt.encode({'register': True, 'exp': datetime.now(UTC).replace(tzinfo=None) + timedelta(hours=1)}, + cast(str, private_key), algorithm=jwt_algo) + cloudlog.info("getting pilotauth") + cloudlog.info("getting pilotauth") + resp = api_get("v2/pilotauth/", method='POST', timeout=15, + imei=imei1, imei2=imei2, serial=serial, public_key=public_key, register_token=register_token) + + if resp.status_code in (400, 402, 403): + cloudlog.info(f"Unable to register device, got {resp.status_code}") + dongle_id = UNREGISTERED_DONGLE_ID + else: + dongleauth = json.loads(resp.text) + dongle_id = dongleauth["dongle_id"] + break + except Exception: + cloudlog.exception("failed to authenticate") + backoff = min(backoff + 1, 15) + time.sleep(backoff) + + if time.monotonic() - start_time > 60 and show_spinner: + spinner.update(f"registering device - serial: {serial}, IMEI: ({imei1}, {imei2})") + return UNREGISTERED_DONGLE_ID + + if show_spinner: + spinner.update(f"registering device - serial: {serial}, IMEI: ({imei1 or None}, {imei2 or None})") + spinner.close() + + if dongle_id: + params.put("DongleId", dongle_id) + from iqpilot.selfdrive.selfdrived.alertmanager import set_offroad_alert + set_offroad_alert("Offroad_UnregisteredHardware", False) + return dongle_id + + +if __name__ == "__main__": + print(register()) diff --git a/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/__init__.py b/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/controls/__init__.py b/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/controls/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/controls/lib/__init__.py b/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/controls/lib/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/controls/lib/desire_helper.py b/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/controls/lib/desire_helper.py new file mode 100644 index 000000000..e19a2e24f --- /dev/null +++ b/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/controls/lib/desire_helper.py @@ -0,0 +1,286 @@ +""" +Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos/ +""" + +from __future__ import annotations + +from iqpilot.cereal import car, custom, log + +from iqpilot.common.constants import CV +from iqpilot.common.params import Params +from iqpilot.common.realtime import DT_MDL +from iqpilot.selfdrive.controls.lib.helpers.lane_change import ( + IQLaneSwapController, + AutoLaneChangeMode, + NavExitLaneChangeController, +) +from iqpilot.selfdrive.controls.lib.helpers.lane_turn import IQNavTurnController + +LaneChangeState = log.LaneChangeState +LaneChangeDirection = log.LaneChangeDirection +TurnDirection = custom.IQTurnSignalDirection +NavManeuverPhase = custom.IQNavState.ManeuverPhase + +LANE_CHANGE_SPEED_MIN = 20 * CV.MPH_TO_MS +LANE_CHANGE_TIME_MAX = 10.0 +TURN_DESIRE_STOP_HOLD_TIME = 3.4 +TURN_DESIRE_STOP_GAP_TIME = 0.2 +TURN_DESIRE_STOP_CYCLE_TIME = TURN_DESIRE_STOP_HOLD_TIME + TURN_DESIRE_STOP_GAP_TIME +TURN_DESIRE_CYCLE_SPEED_MAX = 5 * CV.MPH_TO_MS +TURN_DESIRE_COMMIT_YAW_RATE = 0.08 + +_LANE_CHANGE_DESIRES = { + (LaneChangeDirection.none, LaneChangeState.off): log.Desire.none, + (LaneChangeDirection.none, LaneChangeState.preLaneChange): log.Desire.none, + (LaneChangeDirection.none, LaneChangeState.laneChangeStarting): log.Desire.none, + (LaneChangeDirection.none, LaneChangeState.laneChangeFinishing): log.Desire.none, + (LaneChangeDirection.left, LaneChangeState.off): log.Desire.none, + (LaneChangeDirection.left, LaneChangeState.preLaneChange): log.Desire.none, + (LaneChangeDirection.left, LaneChangeState.laneChangeStarting): log.Desire.laneChangeLeft, + (LaneChangeDirection.left, LaneChangeState.laneChangeFinishing): log.Desire.laneChangeLeft, + (LaneChangeDirection.right, LaneChangeState.off): log.Desire.none, + (LaneChangeDirection.right, LaneChangeState.preLaneChange): log.Desire.none, + (LaneChangeDirection.right, LaneChangeState.laneChangeStarting): log.Desire.laneChangeRight, + (LaneChangeDirection.right, LaneChangeState.laneChangeFinishing): log.Desire.laneChangeRight, +} + +_TURN_DESIRES = { + TurnDirection.none: log.Desire.none, + TurnDirection.turnLeft: log.Desire.turnLeft, + TurnDirection.turnRight: log.Desire.turnRight, +} + +_STOP_CYCLING_TURN_DESIRES = { + log.Desire.turnLeft, + log.Desire.turnRight, +} + + +def turn_desire(turn_direction) -> log.Desire: + return _TURN_DESIRES[getattr(turn_direction, "raw", turn_direction)] + + +def _direction_from_blinkers(carstate) -> int: + if carstate.leftBlinker: + return LaneChangeDirection.left + if carstate.rightBlinker: + return LaneChangeDirection.right + return LaneChangeDirection.none + + +def _steering_nudge_matches(carstate, direction: int) -> bool: + if not carstate.steeringPressed: + return False + return ( + (direction == LaneChangeDirection.left and carstate.steeringTorque > 0) or + (direction == LaneChangeDirection.right and carstate.steeringTorque < 0) + ) + + +def _blindspot_matches(carstate, direction: int) -> bool: + return ( + (direction == LaneChangeDirection.left and carstate.leftBlindspot) or + (direction == LaneChangeDirection.right and carstate.rightBlindspot) + ) + + +def _read_enable_bsm() -> bool: + try: + with car.CarParams.from_bytes(Params().get("CarParams")) as cp: + return bool(cp.enableBsm) + except Exception: + return False + + +class DesireHelper: + def __init__(self): + self.lane_change_state = LaneChangeState.off + self.lane_change_direction = LaneChangeDirection.none + self.lane_change_timer = 0.0 + self.lane_change_ll_prob = 1.0 + self.prev_one_blinker = False + self.prev_nav_exit_active = False + self.desire = log.Desire.none + + self.alc = IQLaneSwapController(self) + self.lane_turn_controller = IQNavTurnController(self) + self.nav_exit = NavExitLaneChangeController(_read_enable_bsm()) + self.lane_turn_direction = TurnDirection.none + self.nav_turn_direction = TurnDirection.none + self.turn_desire_stop_timer = 0.0 + self.turn_desire_stop_active = False + self.turn_desire_cycle_input = log.Desire.none + self.turn_desire_committed = False + + @staticmethod + def get_lane_change_direction(carstate): + return _direction_from_blinkers(carstate) + + @staticmethod + def _nav_turn_desire(nav_state): + if nav_state is None or not getattr(nav_state, "active", False): + return TurnDirection.none + if getattr(nav_state, "maneuverPhase", NavManeuverPhase.none) != NavManeuverPhase.turnActive: + return TurnDirection.none + if not getattr(nav_state, "shouldSendTurnDesire", False): + return TurnDirection.none + return getattr(nav_state, "turnDesireDirection", TurnDirection.none) + + def _clear_lane_change(self) -> None: + self.lane_change_state = LaneChangeState.off + self.lane_change_direction = LaneChangeDirection.none + + def _refresh_turn_overrides(self, carstate, nav_state) -> bool: + speed_mps = carstate.vEgo + self.lane_turn_controller.update_params() + self.lane_turn_controller.update_lane_turn( + blindspot_left=carstate.leftBlindspot, + blindspot_right=carstate.rightBlindspot, + left_blinker=carstate.leftBlinker, + right_blinker=carstate.rightBlinker, + v_ego=speed_mps, + ) + self.lane_turn_direction = self.lane_turn_controller.get_turn_direction() + self.nav_turn_direction = self._nav_turn_desire(nav_state) + + self.nav_exit.update_params() + self.nav_exit.update(nav_state, carstate) + return bool(self.nav_exit.active) + + def _reset_required(self, lateral_active: bool, nav_exit_active: bool) -> bool: + timed_out = self.lane_change_timer > LANE_CHANGE_TIME_MAX + feature_disabled = self.alc.lane_change_set_timer == AutoLaneChangeMode.OFF and not nav_exit_active + return (not lateral_active) or timed_out or feature_disabled + + def _begin_from_idle(self, one_blinker: bool, nav_exit_active: bool, below_speed: bool) -> None: + if below_speed: + return + if one_blinker and not self.prev_one_blinker: + self.lane_change_state = LaneChangeState.preLaneChange + self.lane_change_direction = _direction_from_blinkers(self._last_carstate) + self.lane_change_ll_prob = 1.0 + return + if nav_exit_active and not self.prev_nav_exit_active: + self.lane_change_state = LaneChangeState.preLaneChange + self.lane_change_direction = self.nav_exit.direction + self.lane_change_ll_prob = 1.0 + + def _refresh_requested_direction(self, one_blinker: bool, nav_exit_active: bool) -> None: + if one_blinker: + self.lane_change_direction = _direction_from_blinkers(self._last_carstate) + elif nav_exit_active: + self.lane_change_direction = self.nav_exit.direction + + def _step_pre_lane_change(self, one_blinker: bool, nav_exit_active: bool, below_speed: bool) -> None: + self._refresh_requested_direction(one_blinker, nav_exit_active) + blindspot_detected = _blindspot_matches(self._last_carstate, self.lane_change_direction) + steering_ready = _steering_nudge_matches(self._last_carstate, self.lane_change_direction) + nav_auto_start = nav_exit_active and self.nav_exit.auto_allowed + + self.alc.update_lane_change(blindspot_detected=blindspot_detected, brake_pressed=self._last_carstate.brakePressed) + allowed_to_launch = steering_ready or self.alc.auto_lane_change_allowed or nav_auto_start + + if (not (one_blinker or nav_exit_active)) or below_speed: + self._clear_lane_change() + elif allowed_to_launch and not blindspot_detected: + self.lane_change_state = LaneChangeState.laneChangeStarting + + def _step_lane_change_starting(self, lane_change_prob: float) -> None: + self.lane_change_ll_prob = max(self.lane_change_ll_prob - (2.0 * DT_MDL), 0.0) + if lane_change_prob < 0.02 and self.lane_change_ll_prob < 0.01: + self.lane_change_state = LaneChangeState.laneChangeFinishing + + def _step_lane_change_finishing(self, one_blinker: bool) -> None: + self.lane_change_ll_prob = min(self.lane_change_ll_prob + DT_MDL, 1.0) + if self.lane_change_ll_prob <= 0.99: + return + self.lane_change_direction = LaneChangeDirection.none + self.lane_change_state = LaneChangeState.preLaneChange if one_blinker else LaneChangeState.off + + def _advance_lane_change_machine(self, one_blinker: bool, nav_exit_active: bool, below_speed: bool, lane_change_prob: float) -> None: + if self.lane_change_state == LaneChangeState.off: + self._begin_from_idle(one_blinker, nav_exit_active, below_speed) + return + if self.lane_change_state == LaneChangeState.preLaneChange: + self._step_pre_lane_change(one_blinker, nav_exit_active, below_speed) + return + if self.lane_change_state == LaneChangeState.laneChangeStarting: + self._step_lane_change_starting(lane_change_prob) + return + if self.lane_change_state == LaneChangeState.laneChangeFinishing: + self._step_lane_change_finishing(one_blinker) + + def _update_timer(self) -> None: + if self.lane_change_state in (LaneChangeState.off, LaneChangeState.preLaneChange): + self.lane_change_timer = 0.0 + else: + self.lane_change_timer += DT_MDL + + def _clear_turn_desire_stop_cycle(self) -> None: + self.turn_desire_stop_timer = 0.0 + self.turn_desire_stop_active = False + self.turn_desire_cycle_input = log.Desire.none + self.turn_desire_committed = False + + def _cycle_turn_desire_when_stopped(self, desired_output: log.Desire) -> log.Desire: + if desired_output not in _STOP_CYCLING_TURN_DESIRES: + self._clear_turn_desire_stop_cycle() + return desired_output + + if desired_output != self.turn_desire_cycle_input: + self.turn_desire_stop_timer = 0.0 + self.turn_desire_stop_active = False + self.turn_desire_cycle_input = desired_output + self.turn_desire_committed = False + + if abs(getattr(self._last_carstate, "yawRate", 0.0)) >= TURN_DESIRE_COMMIT_YAW_RATE: + self.turn_desire_committed = True + + if self.turn_desire_committed: + self.turn_desire_stop_timer = 0.0 + self.turn_desire_stop_active = False + return desired_output + + if self._last_carstate.vEgo > TURN_DESIRE_CYCLE_SPEED_MAX: + self.turn_desire_stop_timer = 0.0 + self.turn_desire_stop_active = False + return desired_output + + if not self.turn_desire_stop_active: + self.turn_desire_stop_active = True + self.turn_desire_stop_timer = 0.0 + + cycle_phase = self.turn_desire_stop_timer % TURN_DESIRE_STOP_CYCLE_TIME + self.turn_desire_stop_timer += DT_MDL + if cycle_phase >= TURN_DESIRE_STOP_HOLD_TIME: + return log.Desire.none + return desired_output + + def _pick_desire_output(self) -> None: + desired_output = log.Desire.none + if self.nav_turn_direction != TurnDirection.none: + desired_output = turn_desire(self.nav_turn_direction) + elif self.lane_turn_direction != TurnDirection.none: + desired_output = turn_desire(self.lane_turn_direction) + else: + desired_output = _LANE_CHANGE_DESIRES[(self.lane_change_direction, self.lane_change_state)] + + self.desire = self._cycle_turn_desire_when_stopped(desired_output) + + def update(self, carstate, lateral_active, lane_change_prob, nav_state=None, modeldata=None, radar_state=None): + self._last_carstate = carstate + one_blinker = carstate.leftBlinker != carstate.rightBlinker + below_speed = carstate.vEgo < LANE_CHANGE_SPEED_MIN + nav_exit_active = self._refresh_turn_overrides(carstate, nav_state) + + self.alc.update_params() + if self._reset_required(lateral_active, nav_exit_active): + self._clear_lane_change() + else: + self._advance_lane_change_machine(one_blinker, nav_exit_active, below_speed, lane_change_prob) + + self._update_timer() + self.prev_one_blinker = one_blinker and lateral_active + self.prev_nav_exit_active = nav_exit_active + self.alc.update_state() + self._pick_desire_output() diff --git a/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/controls/lib/drive_helpers.py b/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/controls/lib/drive_helpers.py new file mode 100644 index 000000000..c89027b03 --- /dev/null +++ b/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/controls/lib/drive_helpers.py @@ -0,0 +1,80 @@ +import numpy as np +from iqpilot.common.constants import ACCELERATION_DUE_TO_GRAVITY +from iqpilot.common.realtime import DT_CTRL, DT_MDL + +MIN_SPEED = 1.0 +CONTROL_N = 17 +CAR_ROTATION_RADIUS = 0.0 +# This is a turn radius smaller than most cars can achieve +MAX_CURVATURE = 0.4 +MAX_VEL_ERR = 5.0 # m/s + +MAX_LATERAL_JERK = 5.0 # m/s^3 +MAX_LATERAL_ACCEL_NO_ROLL = 5.0 # m/s^2 +MAX_LATERAL_ACCEL_NO_ROLL_OVERRIDE = 5.0 # m/s^2 +DEFAULT_STOPPING_SPEED = 0.25 # m/s + + +def should_stop(v_ego: float, a_target: float, stopping_speed: float = DEFAULT_STOPPING_SPEED) -> bool: + return bool(v_ego < stopping_speed and a_target < 0.1) + + +def clamp(val, min_val, max_val): + clamped_val = float(np.clip(val, min_val, max_val)) + return clamped_val, clamped_val != val + +def smooth_value(val, prev_val, tau, dt=DT_MDL): + alpha = 1 - np.exp(-dt/tau) if tau > 0 else 1 + return alpha * val + (1 - alpha) * prev_val + +# "Model smoothing": when the policy's own predicted uncertainty (plan_stds) for the +# 1s-ahead lateral position spikes, temporarily lengthen the desiredCurvature smoothing +# time constant so a noisy/uncertain model output doesn't jerk the wheel. +MODEL_SMOOTHING_STD_LOW = 0.15 # m, plan y_std at 1s below which no extra smoothing is added +MODEL_SMOOTHING_STD_HIGH = 0.25 # m, plan y_std at 1s at/above which the full max_extra_seconds is added +MODEL_SMOOTHING_MAX_TOTAL_SEC = 0.60 # hard ceiling on base + dynamic lat smoothing seconds + +def dynamic_lat_smooth_extra_seconds(y_std_1s: float, max_extra_seconds: float) -> float: + if max_extra_seconds <= 0.0: + return 0.0 + return float(np.interp(y_std_1s, [MODEL_SMOOTHING_STD_LOW, MODEL_SMOOTHING_STD_HIGH], [0.0, max_extra_seconds])) + +def clip_curvature(v_ego, prev_curvature, new_curvature, roll, override=False) -> tuple[float, bool]: + # This function respects ISO lateral jerk and acceleration limits + a max curvature + v_ego = max(v_ego, MIN_SPEED) + max_curvature_rate = MAX_LATERAL_JERK / (v_ego ** 2) # inexact calculation, check https://github.com/commaai/openpilot/pull/24755 + new_curvature = np.clip(new_curvature, + prev_curvature - max_curvature_rate * DT_CTRL, + prev_curvature + max_curvature_rate * DT_CTRL) + + max_lat_accel_no_roll = MAX_LATERAL_ACCEL_NO_ROLL_OVERRIDE if override else MAX_LATERAL_ACCEL_NO_ROLL + roll_compensation = roll * ACCELERATION_DUE_TO_GRAVITY + max_lat_accel = max_lat_accel_no_roll + roll_compensation + min_lat_accel = -max_lat_accel_no_roll + roll_compensation + new_curvature, limited_accel = clamp(new_curvature, min_lat_accel / v_ego ** 2, max_lat_accel / v_ego ** 2) + + new_curvature, limited_max_curv = clamp(new_curvature, -MAX_CURVATURE, MAX_CURVATURE) + return float(new_curvature), limited_accel or limited_max_curv + + +def get_accel_from_plan(speeds, accels, t_idxs, action_t=DT_MDL, stopping_speed=DEFAULT_STOPPING_SPEED): + if len(speeds) == len(t_idxs): + v_now = speeds[0] + a_now = accels[0] + v_target = np.interp(action_t, t_idxs, speeds) + a_target = 2 * (v_target - v_now) / (action_t) - a_now + else: + v_now = 0.0 + v_target = 0.0 + a_target = 0.0 + return a_target, should_stop(v_now, a_target, stopping_speed) + +def curv_from_psis(psi_target, psi_rate, vego, action_t): + vego = np.clip(vego, MIN_SPEED, np.inf) + curv_from_psi = psi_target / (vego * action_t) + return 2*curv_from_psi - psi_rate / vego + +def get_curvature_from_plan(yaws, yaw_rates, t_idxs, vego, action_t): + psi_target = np.interp(action_t, t_idxs, yaws) + psi_rate = yaw_rates[0] + return curv_from_psis(psi_target, psi_rate, vego, action_t) diff --git a/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/controls/lib/helpers/__init__.py b/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/controls/lib/helpers/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/controls/lib/helpers/lane_change.py b/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/controls/lib/helpers/lane_change.py new file mode 100644 index 000000000..58d826647 --- /dev/null +++ b/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/controls/lib/helpers/lane_change.py @@ -0,0 +1,293 @@ +""" +Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos +""" +from iqpilot.cereal import custom, log + +from iqpilot.common.params import Params +from iqpilot.common.realtime import DT_MDL + +NAV_EXIT_COMMIT_DISTANCE = 500.0 # m before a route exit to begin moving into the exit lane +_ManeuverType = custom.IQNavState.ManeuverType +_NavDirection = custom.NavDirection + + +class LaneSwapPreset: + DISABLED = -1 + STEERING_NUDGE = 0 + DIRECT = 1 + DELAY_HALF = 2 + DELAY_ONE = 3 + DELAY_TWO = 4 + DELAY_THREE = 5 + OFF = DISABLED + NUDGE = STEERING_NUDGE + NUDGELESS = DIRECT + HALF_SECOND = DELAY_HALF + ONE_SECOND = DELAY_ONE + TWO_SECONDS = DELAY_TWO + THREE_SECONDS = DELAY_THREE + + +PRESET_SECONDS = { + LaneSwapPreset.DISABLED: 0.0, + LaneSwapPreset.STEERING_NUDGE: 0.0, + LaneSwapPreset.DIRECT: 0.05, + LaneSwapPreset.DELAY_HALF: 0.5, + LaneSwapPreset.DELAY_ONE: 1.0, + LaneSwapPreset.DELAY_TWO: 2.0, + LaneSwapPreset.DELAY_THREE: 3.0, +} + +LANE_SWAP_SECONDS = dict(PRESET_SECONDS) +BLINDSPOT_WAIT_OFFSET = -1 + + +class LaneSwapEngine: + def __init__(self, desire_hub): + self._hub = desire_hub + self._kv = Params() + self._mem = { + "sec": 0.0, + "tick": 0, + "gate": 0.0, + "preset": self._kv.get("IQLaneChangeTimer", return_default=True), + "bsm_hold": False, + "braked": False, + "ready": False, + "used": False, + } + self.reload_setup() + + def _pull_setup(self) -> None: + self._mem["bsm_hold"] = self._kv.get_bool("IQLaneChangeBsmDelay") + self._mem["preset"] = self._kv.get("IQLaneChangeTimer", return_default=True) + + def _idle_phase(self) -> bool: + return ( + self._hub.lane_change_state == log.LaneChangeState.off and + self._hub.lane_change_direction == log.LaneChangeDirection.none + ) + + def _seconds_for_preset(self) -> float: + picked = self._mem["preset"] + return PRESET_SECONDS.get(picked, PRESET_SECONDS[LaneSwapPreset.STEERING_NUDGE]) + + def _auto_preset_active(self) -> bool: + picked = self._mem["preset"] + return picked not in (LaneSwapPreset.DISABLED, LaneSwapPreset.STEERING_NUDGE) + + def _advance_clock(self, blindspot_now: bool) -> None: + wait_s = self._seconds_for_preset() + self._mem["gate"] = wait_s + self._mem["sec"] += DT_MDL + if self._mem["bsm_hold"] and blindspot_now and wait_s > 0.0: + if wait_s == PRESET_SECONDS[LaneSwapPreset.DIRECT]: + self._mem["sec"] = BLINDSPOT_WAIT_OFFSET + else: + self._mem["sec"] = wait_s + BLINDSPOT_WAIT_OFFSET + + def _ready_to_fire(self) -> bool: + return ( + self._auto_preset_active() and + (not self._mem["braked"]) and + (not self._mem["used"]) and + (self._mem["sec"] > self._mem["gate"]) + ) + + def reload_setup(self) -> None: + self._pull_setup() + + def heartbeat(self) -> None: + if (self._mem["tick"] % 50) == 0: + self._pull_setup() + self._mem["tick"] += 1 + + def sample(self, blindspot_now: bool = False, brake_now: bool = False, **legacy) -> None: + blindspot_now = bool(legacy.get("blindspot_detected", blindspot_now)) + brake_now = bool(legacy.get("brake_pressed", brake_now)) + self._mem["braked"] = self._mem["braked"] or brake_now + self._advance_clock(blindspot_now) + self._mem["ready"] = self._ready_to_fire() + + def finalize(self) -> None: + started = self._hub.lane_change_state == log.LaneChangeState.laneChangeStarting + self._mem["used"] = self._mem["used"] or started + if self._idle_phase(): + self._mem["sec"] = 0.0 + self._mem["braked"] = False + self._mem["used"] = False + + @property + def ready(self): + return self._mem["ready"] + + @property + def delay(self): + return self._mem["gate"] + + @property + def elapsed(self): + return self._mem["sec"] + + @property + def preset(self): + return self._mem["preset"] + + @preset.setter + def preset(self, value): + self._mem["preset"] = value + + @property + def bsm_hold(self): + return self._mem["bsm_hold"] + + @bsm_hold.setter + def bsm_hold(self, value): + self._mem["bsm_hold"] = bool(value) + + @property + def braked(self): + return self._mem["braked"] + + @braked.setter + def braked(self, value): + self._mem["braked"] = bool(value) + + @property + def used(self): + return self._mem["used"] + + @used.setter + def used(self, value): + self._mem["used"] = bool(value) + + +class NavExitLaneChangeController: + def __init__(self, enable_bsm: bool): + self._params = Params() + self._enable_bsm = bool(enable_bsm) + self.enabled = self._read_enabled() + self._tick = 0 + self.active = False + self.direction = log.LaneChangeDirection.none + self.auto_allowed = False + + def _read_enabled(self) -> bool: + try: + return self._params.get_bool("NavExitLaneChange") + except Exception: + return False + + def update_params(self) -> None: + if self._tick % 50 == 0: + self.enabled = self._read_enabled() + self._tick += 1 + + @staticmethod + def _raw(value): + return getattr(value, "raw", value) + + def update(self, nav_state, carstate) -> None: + self.active = False + self.direction = log.LaneChangeDirection.none + self.auto_allowed = False + + if not self.enabled or nav_state is None or not getattr(nav_state, "active", False): + return + if not getattr(nav_state, "nextManeuverValid", False): + return + if self._raw(getattr(nav_state, "nextManeuverType", _ManeuverType.none)) != int(_ManeuverType.exit): + return + distance = float(getattr(nav_state, "nextManeuverDistance", 0.0)) + if not 0.0 < distance <= NAV_EXIT_COMMIT_DISTANCE: + return + + direction = self._raw(getattr(nav_state, "nextManeuverDirection", _NavDirection.none)) + if direction == int(_NavDirection.left): + self.direction = log.LaneChangeDirection.left + elif direction == int(_NavDirection.right): + self.direction = log.LaneChangeDirection.right + else: + return + + self.active = True + blindspot = carstate.leftBlindspot if self.direction == log.LaneChangeDirection.left else carstate.rightBlindspot + self.auto_allowed = (not blindspot) if self._enable_bsm else False + + +AutoLaneChangeMode = LaneSwapPreset +AUTO_LANE_CHANGE_TIMER = LANE_SWAP_SECONDS +ONE_SECOND_DELAY = BLINDSPOT_WAIT_OFFSET + + +class IQLaneSwapController(LaneSwapEngine): + def __init__(self, desire_helper): + super().__init__(desire_helper) + + def reset(self) -> None: + self.finalize() + + def update_params(self) -> None: + self.heartbeat() + + def update_lane_change(self, blindspot_detected: bool, brake_pressed: bool) -> None: + self.sample(blindspot_now=blindspot_detected, brake_now=brake_pressed) + + def update_state(self) -> None: + self.finalize() + + @property + def lane_change_wait_timer(self): + return self.elapsed + + @lane_change_wait_timer.setter + def lane_change_wait_timer(self, value): + self._mem["sec"] = float(value) + + @property + def lane_change_delay(self): + return self.delay + + @lane_change_delay.setter + def lane_change_delay(self, value): + self._mem["gate"] = float(value) + + @property + def lane_change_set_timer(self): + return self.preset + + @lane_change_set_timer.setter + def lane_change_set_timer(self, value): + self.preset = value + + @property + def lane_change_bsm_delay(self): + return self.bsm_hold + + @lane_change_bsm_delay.setter + def lane_change_bsm_delay(self, value): + self.bsm_hold = value + + @property + def prev_brake_pressed(self): + return self.braked + + @prev_brake_pressed.setter + def prev_brake_pressed(self, value): + self.braked = value + + @property + def auto_lane_change_allowed(self): + return self.ready + + @auto_lane_change_allowed.setter + def auto_lane_change_allowed(self, value): + self._mem["ready"] = bool(value) + + @property + def prev_lane_change(self): + return self.used + + @prev_lane_change.setter + def prev_lane_change(self, value): + self.used = value diff --git a/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/controls/lib/helpers/lane_turn.py b/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/controls/lib/helpers/lane_turn.py new file mode 100644 index 000000000..dd1c0598e --- /dev/null +++ b/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/controls/lib/helpers/lane_turn.py @@ -0,0 +1,157 @@ +""" +Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos/ +""" + +from __future__ import annotations + +from dataclasses import dataclass + +from iqpilot.cereal import custom + +from iqpilot.common.constants import CV +from iqpilot.common.params import Params + +TurnDirection = custom.IQTurnSignalDirection + +TURN_TRIGGER_MPS = 20 * CV.MPH_TO_MS +TURN_SPEED_GATE_MPS = TURN_TRIGGER_MPS +LANE_CHANGE_SPEED_MIN = TURN_SPEED_GATE_MPS + + +@dataclass +class _TurnGateState: + active: bool = False + speed_limit_mps: float = TURN_TRIGGER_MPS + outcome: int = TurnDirection.none + refresh_tick: int = 0 + + +def _mph_param_to_mps(raw_value) -> float: + try: + return float(raw_value) * CV.MPH_TO_MS + except (TypeError, ValueError): + return TURN_TRIGGER_MPS + + +def _resolve_signal_choice(speed_mps: float, + speed_limit_mps: float, + left_signal: bool, + right_signal: bool, + left_blocked: bool, + right_blocked: bool) -> int: + if speed_mps >= speed_limit_mps: + return TurnDirection.none + if left_signal and not right_signal and not left_blocked: + return TurnDirection.turnLeft + if right_signal and not left_signal and not right_blocked: + return TurnDirection.turnRight + return TurnDirection.none + + +class TurnSignalPlanner: + _REFRESH_STRIDE = 50 + + def __init__(self, desire_hub): + self._desire_hub = desire_hub + self._params = Params() + self._state = _TurnGateState() + self.reload_setup() + + def _refresh_from_params(self) -> None: + requested_gate = _mph_param_to_mps(self._params.get("IQLaneTurnValue", return_default=True)) + self._state.active = self._params.get_bool("IQLaneTurnDesire") + self._state.speed_limit_mps = min(TURN_TRIGGER_MPS, requested_gate) + + def _consume_legacy_kwargs(self, **legacy) -> tuple[bool, bool, bool, bool, float]: + return ( + bool(legacy.get("blindspot_left", False)), + bool(legacy.get("blindspot_right", False)), + bool(legacy.get("left_blinker", False)), + bool(legacy.get("right_blinker", False)), + float(legacy.get("v_ego", 0.0)), + ) + + def reload_setup(self): + self._refresh_from_params() + + def heartbeat(self) -> None: + if self._state.refresh_tick % self._REFRESH_STRIDE == 0: + self._refresh_from_params() + self._state.refresh_tick += 1 + + def sample(self, + blocked_l: bool = False, + blocked_r: bool = False, + blink_l: bool = False, + blink_r: bool = False, + speed_mps: float = 0.0, + **legacy) -> None: + if legacy: + blocked_l, blocked_r, blink_l, blink_r, speed_mps = self._consume_legacy_kwargs(**legacy) + self._state.outcome = _resolve_signal_choice(speed_mps, + self._state.speed_limit_mps, + blink_l, + blink_r, + blocked_l, + blocked_r) + + def output(self): + return self._state.outcome if self._state.active else TurnDirection.none + + @property + def enabled(self): + return self._state.active + + @enabled.setter + def enabled(self, value): + self._state.active = bool(value) + + @property + def speed_gate(self): + return self._state.speed_limit_mps + + @speed_gate.setter + def speed_gate(self, value): + self._state.speed_limit_mps = float(value) + + @property + def turn_direction(self): + return self._state.outcome + + @turn_direction.setter + def turn_direction(self, value): + self._state.outcome = value + + +class IQNavTurnController(TurnSignalPlanner): + def __init__(self, desire_helper): + super().__init__(desire_helper) + + def read_params(self): + self.reload_setup() + + def update_params(self) -> None: + self.heartbeat() + + def update_lane_turn(self, + blindspot_left: bool, + blindspot_right: bool, + left_blinker: bool, + right_blinker: bool, + v_ego: float) -> None: + self.sample(blocked_l=blindspot_left, + blocked_r=blindspot_right, + blink_l=left_blinker, + blink_r=right_blinker, + speed_mps=v_ego) + + def get_turn_direction(self): + return self.output() + + @property + def lane_turn_value(self): + return self.speed_gate + + @lane_turn_value.setter + def lane_turn_value(self, value): + self.speed_gate = value diff --git a/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/iqmodeld/__init__.py b/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/iqmodeld/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/iqmodeld/big_catalog.py b/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/iqmodeld/big_catalog.py new file mode 100644 index 000000000..972671971 --- /dev/null +++ b/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/iqmodeld/big_catalog.py @@ -0,0 +1,9 @@ +""" +Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos/ +""" +from iqpilot._proprietary_loader import ProprietaryModuleMissing, load_private_module + +try: + load_private_module(__name__, "iqpilot_private.models.big_catalog") +except ProprietaryModuleMissing: + from iqpilot.models_private_src.big_catalog import * diff --git a/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/iqmodeld/camera.py b/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/iqmodeld/camera.py new file mode 100644 index 000000000..a19e6ce51 --- /dev/null +++ b/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/iqmodeld/camera.py @@ -0,0 +1,70 @@ +""" +Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos/ +""" +import numpy as np + +from iqpilot.common.transformations.camera import DEVICE_CAMERAS + +MAX_CAMERA_OFFSET_METERS = 0.35 + + +class _OffsetSmoother: + def __init__(self, blend: float = 0.1): + self._blend = blend + self._value = 0.0 + + def step(self, target: float) -> float: + self._value = ((1.0 - self._blend) * self._value) + (self._blend * float(target)) + return self._value + + +def _clamped_offset(raw_offset) -> float: + try: + parsed = float(raw_offset) + except (TypeError, ValueError): + parsed = 0.0 + return float(np.clip(parsed, -MAX_CAMERA_OFFSET_METERS, MAX_CAMERA_OFFSET_METERS)) + + +def _camera_profile(sm): + return DEVICE_CAMERAS[(str(sm["deviceState"].deviceType), str(sm["roadCameraState"].sensor))] + + +def _calibration_height(sm) -> float: + from iqpilot.selfdrive.locationd.calibrationd import HEIGHT_SANE_MIN, HEIGHT_SANE_MAX + h = sm["extrinsicsCalibration"].height[0] if sm["extrinsicsCalibration"].height else 1.22 + return h if HEIGHT_SANE_MIN <= h <= HEIGHT_SANE_MAX else 1.22 + + +def _sheared_transform(model_transform, intrinsics, height: float, lateral_offset: float): + optical_center_y = intrinsics[1, 2] + projection_bias = np.eye(3, dtype=np.float32) + projection_bias[0, 1] = lateral_offset / height + projection_bias[0, 2] = -(lateral_offset / height) * optical_center_y + return (projection_bias @ model_transform).astype(np.float32) + + +class CameraOffsetHelper: + def __init__(self): + self.camera_offset = 0.0 + self.actual_camera_offset = 0.0 + self._smoother = _OffsetSmoother() + + @staticmethod + def apply_camera_offset(model_transform, intrinsics, height, offset_param): + return _sheared_transform(model_transform, intrinsics, height, offset_param) + + def set_offset(self, offset): + self.camera_offset = _clamped_offset(offset) + + def update(self, model_transform_main, model_transform_extra, sm, main_wide_camera, extra_uses_wide_camera=True): + self.actual_camera_offset = self._smoother.step(self.camera_offset) + camera_bundle = _camera_profile(sm) + camera_height = _calibration_height(sm) + main_intrinsics = camera_bundle.ecam.intrinsics if main_wide_camera else camera_bundle.fcam.intrinsics + extra_intrinsics = camera_bundle.ecam.intrinsics if extra_uses_wide_camera else camera_bundle.fcam.intrinsics + + return ( + self.apply_camera_offset(model_transform_main, main_intrinsics, camera_height, self.actual_camera_offset), + self.apply_camera_offset(model_transform_extra, extra_intrinsics, camera_height, self.actual_camera_offset), + ) diff --git a/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/iqmodeld/config.py b/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/iqmodeld/config.py new file mode 100644 index 000000000..5b73c0115 --- /dev/null +++ b/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/iqmodeld/config.py @@ -0,0 +1,133 @@ +""" +Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos/ +""" +from __future__ import annotations + +import numpy as np + + +def index_function(index: int, max_val: float = 192, max_idx: int = 32) -> float: + return max_val * ((index / max_idx) ** 2) + + +def _quadratic_series(limit: float, steps: int) -> list[float]: + return [index_function(index, max_val=limit, max_idx=steps - 1) for index in range(steps)] + + +def _probability_window(*values: float) -> np.ndarray: + return np.asarray(values, dtype=np.float32) + + +def _field_group(start: int, stop: int, stride: int) -> slice: + return slice(start, stop, stride) + + +_IDX_COUNT = 33 +_T_AXIS = _quadratic_series(10.0, _IDX_COUNT) +_X_AXIS = _quadratic_series(192.0, _IDX_COUNT) + + +class ModelConstants: + IDX_N = _IDX_COUNT + T_IDXS = _T_AXIS + X_IDXS = _X_AXIS + LEAD_T_IDXS = [0.0, 2.0, 4.0, 6.0, 8.0, 10.0] + LEAD_T_OFFSETS = [0.0, 2.0, 4.0] + META_T_IDXS = [2.0, 4.0, 6.0, 8.0, 10.0] + + MODEL_FREQ = 20 + FEATURE_LEN = 512 + FULL_HISTORY_BUFFER_LEN = 99 + HISTORY_BUFFER_LEN = FULL_HISTORY_BUFFER_LEN + DESIRE_LEN = 8 + TRAFFIC_CONVENTION_LEN = 2 + NAV_FEATURE_LEN = 256 + NAV_INSTRUCTION_LEN = 150 + LAT_PLANNER_STATE_LEN = 4 + LATERAL_CONTROL_PARAMS_LEN = 2 + PREV_DESIRED_CURV_LEN = 1 + + FCW_THRESHOLDS_5MS2 = _probability_window(0.05, 0.05, 0.15, 0.15, 0.15) + FCW_THRESHOLDS_3MS2 = _probability_window(0.7, 0.7) + FCW_5MS2_PROBS_WIDTH = 5 + FCW_3MS2_PROBS_WIDTH = 2 + + DISENGAGE_WIDTH = 5 + POSE_WIDTH = 6 + WIDE_FROM_DEVICE_WIDTH = 3 + SIM_POSE_WIDTH = 6 + LEAD_WIDTH = 4 + LANE_LINES_WIDTH = 2 + ROAD_EDGES_WIDTH = 2 + PLAN_WIDTH = 15 + DESIRE_PRED_WIDTH = 8 + LAT_PLANNER_SOLUTION_WIDTH = 4 + DESIRED_CURV_WIDTH = 1 + + NUM_LANE_LINES = 4 + NUM_ROAD_EDGES = 2 + LEAD_TRAJ_LEN = 6 + DESIRE_PRED_LEN = 4 + + PLAN_MHP_N = 5 + LEAD_MHP_N = 2 + PLAN_MHP_SELECTION = 1 + LEAD_MHP_SELECTION = 3 + + FCW_THRESHOLD_5MS2_HIGH = 0.15 + FCW_THRESHOLD_5MS2_LOW = 0.05 + FCW_THRESHOLD_3MS2 = 0.7 + + CONFIDENCE_BUFFER_LEN = 5 + RYG_GREEN = 0.01165 + RYG_YELLOW = 0.06157 + POLY_PATH_DEGREE = 4 + + +class Plan: + POSITION = slice(0, 3) + VELOCITY = slice(3, 6) + ACCELERATION = slice(6, 9) + T_FROM_CURRENT_EULER = slice(9, 12) + ORIENTATION_RATE = slice(12, 15) + + +class Meta: + ENGAGED = _field_group(0, 1, 1) + GAS_DISENGAGE = _field_group(1, 31, 6) + BRAKE_DISENGAGE = _field_group(2, 31, 6) + STEER_OVERRIDE = _field_group(3, 31, 6) + HARD_BRAKE_3 = _field_group(4, 31, 6) + HARD_BRAKE_4 = _field_group(5, 31, 6) + HARD_BRAKE_5 = _field_group(6, 31, 6) + GAS_PRESS = _field_group(31, 55, 4) + BRAKE_PRESS = _field_group(32, 55, 4) + LEFT_BLINKER = _field_group(33, 55, 4) + RIGHT_BLINKER = _field_group(34, 55, 4) + + +class MetaTombRaider: + ENGAGED = _field_group(0, 1, 1) + GAS_DISENGAGE = _field_group(1, 41, 8) + BRAKE_DISENGAGE = _field_group(2, 41, 8) + STEER_OVERRIDE = _field_group(3, 41, 8) + HARD_BRAKE_3 = _field_group(4, 41, 8) + HARD_BRAKE_4 = _field_group(5, 41, 8) + HARD_BRAKE_5 = _field_group(6, 41, 8) + GAS_PRESS = _field_group(7, 41, 8) + BRAKE_PRESS = _field_group(8, 41, 8) + LEFT_BLINKER = _field_group(41, 53, 2) + RIGHT_BLINKER = _field_group(42, 53, 2) + + +class MetaSimPose: + ENGAGED = _field_group(0, 1, 1) + GAS_DISENGAGE = _field_group(1, 36, 7) + BRAKE_DISENGAGE = _field_group(2, 36, 7) + STEER_OVERRIDE = _field_group(3, 36, 7) + HARD_BRAKE_3 = _field_group(4, 36, 7) + HARD_BRAKE_4 = _field_group(5, 36, 7) + HARD_BRAKE_5 = _field_group(6, 36, 7) + GAS_PRESS = _field_group(7, 36, 7) + LEFT_BLINKER = _field_group(36, 48, 2) + RIGHT_BLINKER = _field_group(37, 48, 2) diff --git a/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/iqmodeld/daemon.py b/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/iqmodeld/daemon.py new file mode 100644 index 000000000..2f1edd563 --- /dev/null +++ b/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/iqmodeld/daemon.py @@ -0,0 +1,766 @@ +#!/usr/bin/env python3 +from __future__ import annotations + +import sys +import time +from dataclasses import dataclass +from typing import Any + +import iqpilot.cereal.messaging as messaging +import numpy as np + +from iqpilot.cereal import car, custom, log +from iqpilot.cereal.messaging import PubMaster, SubMaster +from iqpilot.cereal.visionipc import VisionStreamType +from msgq.visionipc import VisionBuf, VisionIpcClient +from iqdbc.car.car_helpers import get_demo_car_params +from setproctitle import setproctitle + +from iqpilot.common.filter_simple import FirstOrderFilter +from iqpilot.common.iq_perf import PerfSample, PerfTraceEmitter, PerfTraceRing +from iqpilot.common.params import Params +from iqpilot.common.realtime import DT_MDL, config_realtime_process +from iqpilot.common.swaglog import cloudlog +from iqpilot.common.transformations.camera import DEVICE_CAMERAS +from iqpilot.common.transformations.model import get_warp_matrix +from iqpilot.selfdrive.controls.lib.desire_helper import DesireHelper +from iqpilot.selfdrive.controls.lib.drive_helpers import ( + MODEL_SMOOTHING_MAX_TOTAL_SEC, + dynamic_lat_smooth_extra_seconds, + get_accel_from_plan, + smooth_value, +) +from iqpilot.selfdrive.locationd.calibration_helpers import get_calibrated_rpy +from iqpilot.system import sentry + +from iqpilot.common.steer_delay import lateral_action_delay +from iqpilot.selfdrive.iqmodeld.models.helpers import get_active_bundle +from iqpilot.selfdrive.iqmodeld.models.inference_state import InferenceStateBase +from iqpilot.selfdrive.iqmodeld.models.runners.model_runner import get_model_runner +from iqpilot.selfdrive.iqmodeld.camera import CameraOffsetHelper +from iqpilot.selfdrive.iqmodeld.config import Plan +from iqpilot.selfdrive.iqmodeld.messaging import ( + DrivePacketMemory, + pick_curvature, + populate_drive_messages, + populate_odometry_message, +) +from iqpilot.selfdrive.iqmodeld.metadata import select_meta_layout + +try: + from iqpilot.selfdrive.iqmodeld.native.iqmodel_pyx import RoadProjector, WarpContext +except ModuleNotFoundError: + class WarpContext: + def __init__(self, *args, **kwargs): + raise ModuleNotFoundError("iqpilot.selfdrive.iqmodeld.native.iqmodel_pyx is not built") + + class RoadProjector: + def __init__(self, *args, **kwargs): + raise ModuleNotFoundError("iqpilot.selfdrive.iqmodeld.native.iqmodel_pyx is not built") + + +PROCESS_NAME = "iqpilot.selfdrive.iqmodeld.daemon" +IQP_NAV_MODEL_INFLUENCE_ENABLED = False +TurnDirection = custom.IQTurnSignalDirection +IQMODEL_EVAL_WARN_US = int(DT_MDL * 1_000_000) +IQMODEL_EVAL_ERROR_US = IQMODEL_EVAL_WARN_US * 2 +_FRAME_STARVED_BACKOFF_POLLS = 5 +_FRAME_STARVED_BACKOFF_SECONDS = 0.005 +_FRAME_STARVED_LOG_EVERY = 200 + + +def _plan_y_std_1s(outputs: dict[str, np.ndarray]) -> float: + # plan_stds is (batch, IDX_N, PLAN_WIDTH); index 10 ~= 1s ahead (see ModelConstants.T_IDXS), + # POSITION is an (x, y, z) slice within PLAN_WIDTH so [1] picks the lateral (y) std. + try: + return float(outputs["plan_stds"][0, 10, Plan.POSITION][1]) + except (KeyError, IndexError): + return 0.0 + + +def _model_lat_smooth_max_sec(params: Params) -> float: + if not params.get_bool("ModelSmoothingEnabled"): + return 0.0 + try: + raw = params.get("ModelLatSmoothSec", return_default=True) + raw = 0 if raw is None else int(raw) + except (ValueError, TypeError): + raw = 0 + return min(max(raw, 0), 30) * 0.01 + + +@dataclass +class CaptureStamp: + frame_id: int = 0 + timestamp_sof: int = 0 + timestamp_eof: int = 0 + + @classmethod + def from_vipc(cls, client: VisionIpcClient) -> "CaptureStamp": + return cls(client.frame_id, client.timestamp_sof, client.timestamp_eof) + + +@dataclass(frozen=True) +class StreamLayout: + dual_camera: bool + main_is_wide: bool + primary_stream: VisionStreamType + + +class ReplayLedger: + def __init__(self, tensor_shapes: dict[str, tuple[int, ...]], frame_inputs: list[str]): + self.inputs: dict[str, np.ndarray] = {} + self.archive: dict[str, np.ndarray] = {} + self.selectors: dict[str, np.ndarray] = {} + self._frame_inputs = set(frame_inputs) + self._pulse_name: str | None = None + self._pulse_memory: np.ndarray | None = None + + feature_shape = tensor_shapes.get("features_buffer") + for tensor_name, tensor_shape in tensor_shapes.items(): + if tensor_name in self._frame_inputs: + continue + + self.inputs[tensor_name] = np.zeros(tensor_shape, dtype=np.float32) + if len(tensor_shape) != 3 or tensor_shape[1] <= 1: + continue + + history_len = self._history_length(tensor_shape, feature_shape) + self.archive[tensor_name] = np.zeros((1, history_len, tensor_shape[2]), dtype=np.float32) + export_index = self._export_index(tensor_shape, history_len, feature_shape) + if export_index is not None: + self.selectors[tensor_name] = export_index + + if tensor_name.startswith("desire"): + self._pulse_name = tensor_name + self._pulse_memory = np.zeros(tensor_shape[2], dtype=np.float32) + + @staticmethod + def _history_length(tensor_shape: tuple[int, ...], feature_shape: tuple[int, ...] | None) -> int: + if tensor_shape[1] >= 99: + return tensor_shape[1] + if tensor_shape[1] in (24, 25) and feature_shape is not None and feature_shape[1] == 24: + return (feature_shape[1] + 1) * 4 + return tensor_shape[1] * 4 + + @staticmethod + def _export_index(tensor_shape: tuple[int, ...], history_len: int, + feature_shape: tuple[int, ...] | None) -> np.ndarray | None: + if tensor_shape[1] in (24, 25) and feature_shape is not None and feature_shape[1] == 24: + stride = int(-history_len / tensor_shape[1]) + return np.arange(stride, stride * (tensor_shape[1] + 1), stride)[::-1] + if tensor_shape[1] == 25: + skip = history_len // tensor_shape[1] + return np.arange(history_len)[-1 - (skip * (tensor_shape[1] - 1))::skip] + if tensor_shape[1] >= 99: + return np.arange(tensor_shape[1]) + return None + + @property + def pulse_name(self) -> str: + if self._pulse_name is None: + raise KeyError("No desire-like pulse input present in model inputs") + return self._pulse_name + + def _shift_archive(self, tensor_name: str) -> np.ndarray: + history = self.archive[tensor_name] + history[0, :-1] = history[0, 1:] + return history + + def inject_pulse(self, pulse_values: np.ndarray) -> None: + pulse = pulse_values.copy() + pulse[0] = 0 + assert self._pulse_memory is not None + rising = np.where(pulse - self._pulse_memory > 0.99, pulse, 0) + self._pulse_memory[:] = pulse + + history = self._shift_archive(self.pulse_name) + history[0, -1] = rising + exported_shape = self.inputs[self.pulse_name].shape + if history.shape[1] > exported_shape[1]: + stride = history.shape[1] // exported_shape[1] + self.inputs[self.pulse_name][:] = history[0].reshape( + exported_shape[0], exported_shape[1], stride, -1 + ).max(axis=2) + return + self.inputs[self.pulse_name][:] = history[0, self.selectors[self.pulse_name]] + + def merge_inputs(self, fresh_inputs: dict[str, np.ndarray]) -> None: + pulse_name = self.pulse_name + for tensor_name, tensor_value in fresh_inputs.items(): + if tensor_name in self.inputs and tensor_name != pulse_name: + self.inputs[tensor_name][:] = tensor_value + + def note_hidden_state(self, hidden_state: np.ndarray) -> None: + if "features_buffer" not in self.archive: + return + history = self._shift_archive("features_buffer") + history[0, -1] = hidden_state[0] + self.inputs["features_buffer"][:] = history[0, self.selectors["features_buffer"]] + + def note_feedback(self, tensor_name: str, values: np.ndarray, zero_export: bool = False) -> None: + if tensor_name not in self.archive: + return + history = self._shift_archive(tensor_name) + history[0, -1, :] = values[0] + exported = history[0, self.selectors[tensor_name]] + self.inputs[tensor_name][:] = 0 * exported if zero_export else exported + + +def _planplus_gain(vehicle_speed: float) -> float: + return 0.75 if vehicle_speed >= 25.0 else 1.0 + + +def _merged_plan(runtime_state: "NeuralEngineState", outputs: dict[str, np.ndarray], vehicle_speed: float) -> np.ndarray: + base_plan = outputs["plan"][0] + if "planplus" not in outputs: + return base_plan + return base_plan + (runtime_state.PLANPLUS_CONTROL * _planplus_gain(vehicle_speed)) * outputs["planplus"][0] + + +class NeuralEngineState(InferenceStateBase): + frames: dict[str, RoadProjector] + + def __init__(self, gpu_context: WarpContext): + super().__init__() + runner = get_model_runner() + bundle = get_active_bundle() + + self.model_runner = runner + self.constants = runner.constants + self.generation = bundle.generation if bundle is not None else None + + knob_values = {entry.key: entry.value for entry in bundle.overrides} if bundle is not None else {} + self.LAT_SMOOTH_SECONDS = float(knob_values.get("lat", ".0")) + self.LONG_SMOOTH_SECONDS = float(knob_values.get("long", ".0")) + self.MIN_LAT_CONTROL_SPEED = 0.3 + self.PLANPLUS_CONTROL = 1.0 + self.model_smoothing_max_extra_sec = 0.0 + + context_depth = 5 if runner.is_20hz else 2 + self.frames = { + stream_name: RoadProjector(gpu_context, context_depth) + for stream_name in runner.vision_input_names + } + + self._ledger = ReplayLedger(runner.input_shapes, runner.vision_input_names) + self.numpy_inputs = self._ledger.inputs + self.temporal_buffers = self._ledger.archive + self.temporal_idxs_map = self._ledger.selectors + + @property + def mlsim(self) -> bool: + return bool(self.generation is not None and self.generation >= 11) + + @property + def desire_key(self) -> str: + return self._ledger.pulse_name + + def _warp_frames(self, vision_bufs: dict[str, VisionBuf], + transform_map: dict[str, np.ndarray]) -> dict[str, Any]: + return { + stream_name: self.frames[stream_name].stage(vision_bufs[stream_name], transform_map[stream_name].flatten()) + for stream_name in self.model_runner.vision_input_names + } + + def _run_split_model(self) -> dict[str, np.ndarray]: + if hasattr(self.model_runner, "run_vision"): + vision_packet = self.model_runner.run_vision() + self._ledger.note_hidden_state(vision_packet["hidden_state"]) + self.model_runner.refresh_policy_features(self.numpy_inputs["features_buffer"]) + return {**vision_packet, **self.model_runner.run_policy()} + + result = self.model_runner.run_model() + if "hidden_state" in result: + self._ledger.note_hidden_state(result["hidden_state"]) + return result + + def _write_curvature_memory(self, outputs: dict[str, np.ndarray]) -> None: + if "desired_curvature" not in outputs: + return + + feedback_slot = None + if "prev_desired_curvs" in self.numpy_inputs: + feedback_slot = "prev_desired_curvs" + elif "prev_desired_curv" in self.numpy_inputs: + feedback_slot = "prev_desired_curv" + + if feedback_slot is not None: + self._ledger.note_feedback(feedback_slot, outputs["desired_curvature"], zero_export=self.mlsim) + + def run(self, vision_bufs: dict[str, VisionBuf], transform_map: dict[str, np.ndarray], + fresh_inputs: dict[str, np.ndarray]) -> dict[str, np.ndarray] | None: + if not getattr(self.model_runner, "uses_opencl_warp", True): + return self.model_runner.run_fused(vision_bufs, transform_map, fresh_inputs) + + self._ledger.inject_pulse(fresh_inputs[self.desire_key]) + self._ledger.merge_inputs(fresh_inputs) + warped_frames = self._warp_frames(vision_bufs, transform_map) + self.model_runner.prepare_inputs(warped_frames, self.numpy_inputs, self.frames) + + outputs = self._run_split_model() + self._write_curvature_memory(outputs) + return outputs + + def get_action_from_model(self, outputs: dict[str, np.ndarray], previous_action: log.ModelDataV2.Action, + lat_action_t: float, long_action_t: float, vehicle_speed: float, + lat_smooth_seconds: float | None = None) -> log.ModelDataV2.Action: + if lat_smooth_seconds is None: + lat_smooth_seconds = self.LAT_SMOOTH_SECONDS + + if "action" in outputs: + curvature_cmd = outputs["action"][0, 0] / (max(1.0, vehicle_speed)) ** 2 + accel_cmd = outputs["action"][0, 1] + should_stop = bool(vehicle_speed < 0.3 and accel_cmd < 0.1) + + accel_cmd = smooth_value(accel_cmd, previous_action.desiredAcceleration, self.LONG_SMOOTH_SECONDS) + if vehicle_speed > self.MIN_LAT_CONTROL_SPEED: + curvature_cmd = smooth_value(curvature_cmd, previous_action.desiredCurvature, lat_smooth_seconds) + else: + curvature_cmd = previous_action.desiredCurvature + + return log.ModelDataV2.Action( + desiredCurvature=float(curvature_cmd), + desiredAcceleration=float(accel_cmd), + shouldStop=should_stop, + ) + + plan_rows = _merged_plan(self, outputs, vehicle_speed) + accel_cmd, should_stop = get_accel_from_plan( + plan_rows[:, Plan.VELOCITY][:, 0], + plan_rows[:, Plan.ACCELERATION][:, 0], + self.constants.T_IDXS, + action_t=long_action_t, + ) + accel_cmd = smooth_value(accel_cmd, previous_action.desiredAcceleration, self.LONG_SMOOTH_SECONDS) + + curvature_cmd = pick_curvature(outputs, plan_rows, vehicle_speed, lat_action_t, self.mlsim) + if self.generation is not None and self.generation >= 10: + if vehicle_speed > self.MIN_LAT_CONTROL_SPEED: + curvature_cmd = smooth_value(curvature_cmd, previous_action.desiredCurvature, lat_smooth_seconds) + else: + curvature_cmd = previous_action.desiredCurvature + + return log.ModelDataV2.Action( + desiredCurvature=float(curvature_cmd), + desiredAcceleration=float(accel_cmd), + shouldStop=bool(should_stop), + ) + + +class CameraIngress: + def __init__(self, gpu_context: WarpContext): + self.layout = self._discover_layout() + self._primary = VisionIpcClient("camerad", self.layout.primary_stream, True, gpu_context) + self._secondary = VisionIpcClient("camerad", VisionStreamType.VISION_STREAM_WIDE_ROAD, False, gpu_context) + + while not self._primary.connect(False): + time.sleep(0.1) + while self.layout.dual_camera and not self._secondary.connect(False): + time.sleep(0.1) + + cloudlog.warning( + f"connected main cam with buffer size: {self._primary.buffer_len} ({self._primary.width} x {self._primary.height})" + ) + if self.layout.dual_camera: + cloudlog.warning( + f"connected extra cam with buffer size: {self._secondary.buffer_len} ({self._secondary.width} x {self._secondary.height})" + ) + + @staticmethod + def _discover_layout() -> StreamLayout: + while True: + available = VisionIpcClient.available_streams("camerad", block=False) + if available: + dual_camera = ( + VisionStreamType.VISION_STREAM_WIDE_ROAD in available + and VisionStreamType.VISION_STREAM_ROAD in available + ) + main_is_wide = VisionStreamType.VISION_STREAM_ROAD not in available + primary_stream = VisionStreamType.VISION_STREAM_WIDE_ROAD if main_is_wide else VisionStreamType.VISION_STREAM_ROAD + cloudlog.warning( + f"vision stream set up, main_wide_camera: {main_is_wide}, use_extra_client: {dual_camera}" + ) + return StreamLayout(dual_camera=dual_camera, main_is_wide=main_is_wide, primary_stream=primary_stream) + time.sleep(0.1) + + def pull(self) -> tuple[VisionBuf, VisionBuf, CaptureStamp, CaptureStamp] | None: + main_buf = None + wide_buf = None + main_stamp = CaptureStamp() + wide_stamp = CaptureStamp() + + while main_stamp.timestamp_sof < wide_stamp.timestamp_sof + 25000000: + main_buf = self._primary.recv() + main_stamp = CaptureStamp.from_vipc(self._primary) + if main_buf is None: + return None + + if not self.layout.dual_camera: + return main_buf, main_buf, main_stamp, main_stamp + + while True: + wide_buf = self._secondary.recv() + wide_stamp = CaptureStamp.from_vipc(self._secondary) + if wide_buf is None or main_stamp.timestamp_sof < wide_stamp.timestamp_sof + 25000000: + break + + if wide_buf is None: + return None + + if abs(main_stamp.timestamp_sof - wide_stamp.timestamp_sof) > 10000000: + cloudlog.error( + f"frames out of sync! main: {main_stamp.frame_id} ({main_stamp.timestamp_sof / 1e9:.5f})," + f" extra: {wide_stamp.frame_id} ({wide_stamp.timestamp_sof / 1e9:.5f})" + ) + return main_buf, wide_buf, main_stamp, wide_stamp + + +class CalibrationAtlas: + def __init__(self): + self.main_warp = np.zeros((3, 3), dtype=np.float32) + self.extra_warp = np.zeros((3, 3), dtype=np.float32) + self.ready = False + self._offset_tuner = CameraOffsetHelper() + + def set_offset(self, offset_value: Any) -> None: + self._offset_tuner.set_offset(offset_value) + + def refresh(self, sm: SubMaster, main_is_wide: bool, dual_camera: bool) -> tuple[np.ndarray, np.ndarray, bool]: + if not (sm.seen["extrinsicsCalibration"] and sm.seen["roadCameraState"] and sm.seen["deviceState"]): + return self.main_warp, self.extra_warp, self.ready + + rpy = get_calibrated_rpy(sm["extrinsicsCalibration"]) + if rpy is None: + live_calib = sm["extrinsicsCalibration"] + if len(live_calib.rpyCalib) == 3: + rpy = np.array(live_calib.rpyCalib, dtype=np.float32) + else: + rpy = np.zeros(3, dtype=np.float32) + + device_key = (str(sm["deviceState"].deviceType), str(sm["roadCameraState"].sensor)) + device_camera = DEVICE_CAMERAS[device_key] + main_intrinsics = device_camera.ecam.intrinsics if main_is_wide else device_camera.fcam.intrinsics + extra_uses_wide_camera = dual_camera or main_is_wide + extra_intrinsics = device_camera.ecam.intrinsics if extra_uses_wide_camera else device_camera.fcam.intrinsics + self.main_warp = get_warp_matrix(rpy, main_intrinsics, False).astype(np.float32) + self.extra_warp = get_warp_matrix(rpy, extra_intrinsics, True).astype(np.float32) + self.main_warp, self.extra_warp = self._offset_tuner.update( + self.main_warp, self.extra_warp, sm, main_is_wide, extra_uses_wide_camera + ) + self.ready = True + return self.main_warp, self.extra_warp, self.ready + + +class FrameDropMeter: + def __init__(self, model_freq: float): + self._smoother = FirstOrderFilter(0.0, 10.0, 1.0 / model_freq) + self._warm_frames = 0 + self._last_frame_id = 0 + + def sample(self, frame_id: int) -> tuple[int, float, bool]: + dropped = max(0, frame_id - self._last_frame_id - 1) + smooth = self._smoother.update(min(dropped, 10)) + if self._warm_frames < 10: + self._smoother.x = 0.0 + smooth = 0.0 + self._warm_frames += 1 + return dropped, smooth / (1 + smooth), dropped > 0 + + def commit(self, frame_id: int) -> None: + self._last_frame_id = frame_id + + +class InferenceDaemon: + def __init__(self, demo: bool = False, channel_path: str | None = None): + cloudlog.warning("iqmodeld init") + sentry.set_tag("daemon", PROCESS_NAME) + cloudlog.bind(daemon=PROCESS_NAME) + setproctitle(PROCESS_NAME) + config_realtime_process(7, 54) + + cloudlog.warning("setting up CL context") + self._gpu = WarpContext() + cloudlog.warning("CL context ready; loading model") + self._runtime = NeuralEngineState(self._gpu) + self._meta_layout = select_meta_layout() + cloudlog.warning("models loaded, iqmodeld starting") + + self._channel = None + if channel_path is not None: + from iqpilot.selfdrive.iqmodeld.model_channel import ModelChannel + self._channel = ModelChannel(channel_path, create=True) + + self._cameras = CameraIngress(self._gpu) + pub_services = ["iqPerfTrace"] if self._channel is not None else [ + "modelV2", "drivingModelData", "cameraOdometry", "iqDriveModelData", "iqPerfTrace"] + self._pub = PubMaster(pub_services) + self._sub = SubMaster([ + "deviceState", "carState", "roadCameraState", "extrinsicsCalibration", + "driverMonitoringState", "carControl", "lateralDelay", "iqNavState", "radarState", + ]) + self._message_memory = DrivePacketMemory() + self._params = Params() + self._frame_meter = FrameDropMeter(self._runtime.constants.MODEL_FREQ) + self._warps = CalibrationAtlas() + self._perf = PerfTraceEmitter("iqmodeld", pubmaster=self._pub) + self._perf_ring = PerfTraceRing() + + self._car_params = self._load_car_params(demo) + self._long_action_delay = self._car_params.longitudinalActuatorDelay + self._runtime.LONG_SMOOTH_SECONDS + self._previous_action = log.ModelDataV2.Action() + self._desire_logic = DesireHelper() + self._lat_smooth_extra_sec = 0.0 + + def _load_car_params(self, demo: bool): + car_params = get_demo_car_params() if demo else messaging.log_from_bytes( + self._params.get("CarParams", block=True), car.CarParams) + cloudlog.info("iqmodeld got CarParams: %s", car_params.brand) + return car_params + + def _refresh_tunables(self, tick: int) -> None: + if tick % 60 != 0: + return + from iqpilot.selfdrive.iqmodeld.egpu_helpers import egpu_selected + big_enabled = self._params.get_bool("IQEmacEnabled") or egpu_selected(self._params) + if big_enabled != (self._channel is not None): + # publish mode is fixed at startup: staying up would fight the selector for modelV2 + cloudlog.warning("iqmodeld: big backend toggled, restarting to switch publish mode") + sys.exit(0) + self._runtime.lat_delay = lateral_action_delay(self._params, self._car_params, self._sub["lateralDelay"].lateralDelay) + self._runtime.PLANPLUS_CONTROL = self._params.get("PlanplusControl", return_default=True) + self._runtime.model_smoothing_max_extra_sec = _model_lat_smooth_max_sec(self._params) + self._warps.set_offset(self._params.get("CameraOffset", return_default=True)) + + def _traffic_side(self) -> np.ndarray: + traffic = np.zeros(2, dtype=np.float32) + traffic[int(self._sub["driverMonitoringState"].isRHD)] = 1 + return traffic + + def _desire_pulse(self) -> np.ndarray: + pulse = np.zeros(self._runtime.constants.DESIRE_LEN, dtype=np.float32) + desire_idx = self._desire_logic.desire + if 0 <= desire_idx < self._runtime.constants.DESIRE_LEN: + pulse[desire_idx] = 1 + return pulse + + def _compose_inputs(self, vehicle_speed: float, lat_horizon: float, long_horizon: float) -> dict[str, np.ndarray]: + inputs: dict[str, np.ndarray] = { + self._runtime.desire_key: self._desire_pulse(), + "traffic_convention": self._traffic_side(), + } + if "lateral_control_params" in self._runtime.numpy_inputs: + inputs["lateral_control_params"] = np.array([vehicle_speed, lat_horizon], dtype=np.float32) + if "action_t" in self._runtime.numpy_inputs: + inputs["action_t"] = np.array([lat_horizon, long_horizon], dtype=np.float32) + return inputs + + def _publish(self, outputs: dict[str, np.ndarray], main_stamp: CaptureStamp, extra_stamp: CaptureStamp, + road_frame_id: int, frame_drop_ratio: float, dropped_frames: int, + execution_time: float, live_calib_seen: bool, + lat_horizon: float, long_horizon: float, vehicle_speed: float) -> None: + model_msg = messaging.new_message("modelV2") + driving_msg = messaging.new_message("drivingModelData") + pose_msg = messaging.new_message("cameraOdometry") + iq_msg = messaging.new_message("iqDriveModelData") + + self._lat_smooth_extra_sec = dynamic_lat_smooth_extra_seconds( + _plan_y_std_1s(outputs), self._runtime.model_smoothing_max_extra_sec + ) + lat_smooth_total_sec = min(self._runtime.LAT_SMOOTH_SECONDS + self._lat_smooth_extra_sec, MODEL_SMOOTHING_MAX_TOTAL_SEC) + action = self._runtime.get_action_from_model( + outputs, self._previous_action, lat_horizon, long_horizon, vehicle_speed, lat_smooth_total_sec + ) + self._previous_action = action + + populate_drive_messages( + driving_msg, + model_msg, + outputs, + action, + self._message_memory, + main_stamp.frame_id, + extra_stamp.frame_id, + road_frame_id, + frame_drop_ratio, + main_stamp.timestamp_eof, + execution_time, + live_calib_seen, + self._meta_layout, + ) + + desire_state = model_msg.modelV2.meta.desireState + lane_change_prob = desire_state[log.Desire.laneChangeLeft] + desire_state[log.Desire.laneChangeRight] + self._desire_logic.update( + self._sub["carState"], + self._sub["carControl"].latActive, + lane_change_prob, + self._sub["iqNavState"], + model_msg.modelV2, + self._sub["radarState"], + ) + model_msg.modelV2.meta.laneChangeState = self._desire_logic.lane_change_state + model_msg.modelV2.meta.laneChangeDirection = self._desire_logic.lane_change_direction + driving_msg.drivingModelData.meta.laneChangeState = self._desire_logic.lane_change_state + driving_msg.drivingModelData.meta.laneChangeDirection = self._desire_logic.lane_change_direction + iq_msg.iqDriveModelData.turnSignalDirection = self._desire_logic.lane_turn_direction + + populate_odometry_message( + pose_msg, + outputs, + main_stamp.frame_id, + dropped_frames, + main_stamp.timestamp_eof, + live_calib_seen, + ) + + if self._channel is not None: + self._channel.write(main_stamp.frame_id, { + "source": "small", + "frame_id": main_stamp.frame_id, + "timestamp_sof": int(main_stamp.timestamp_sof), + "live_calib_seen": bool(live_calib_seen), + "model_execution_time": float(execution_time), + "msgs": { + "modelV2": model_msg.to_bytes(), + "drivingModelData": driving_msg.to_bytes(), + "cameraOdometry": pose_msg.to_bytes(), + "iqDriveModelData": iq_msg.to_bytes(), + }, + }) + return + + self._pub.send("modelV2", model_msg) + self._pub.send("drivingModelData", driving_msg) + self._pub.send("cameraOdometry", pose_msg) + self._pub.send("iqDriveModelData", iq_msg) + + def serve(self) -> None: + tick = 0 + starved_polls = 0 + while True: + frame_pair = self._cameras.pull() + if frame_pair is None: + starved_polls += 1 + if starved_polls >= _FRAME_STARVED_BACKOFF_POLLS: + time.sleep(_FRAME_STARVED_BACKOFF_SECONDS) + if starved_polls % _FRAME_STARVED_LOG_EVERY == 0: + cloudlog.error(f"visionipc delivered no frames for {starved_polls} polls; model is not running") + continue + + if starved_polls: + cloudlog.warning(f"visionipc recovered after {starved_polls} frameless polls") + starved_polls = 0 + + main_buf, extra_buf, main_stamp, extra_stamp = frame_pair + self._sub.update(0) + self._refresh_tunables(tick) + + vehicle_speed = max(self._sub["carState"].vEgo, 0.0) + lat_horizon = self._runtime.lat_delay + self._runtime.LAT_SMOOTH_SECONDS + self._lat_smooth_extra_sec + DT_MDL + long_horizon = self._long_action_delay + DT_MDL + + main_warp, extra_warp, live_calib_seen = self._warps.refresh( + self._sub, self._cameras.layout.main_is_wide, self._cameras.layout.dual_camera + ) + dropped_frames, frame_drop_ratio, prepare_only = self._frame_meter.sample(main_stamp.frame_id) + + vision_bufs = { + stream_name: extra_buf if "big" in stream_name else main_buf + for stream_name in self._runtime.model_runner.vision_input_names + } + warp_map = { + stream_name: extra_warp if "big" in stream_name else main_warp + for stream_name in self._runtime.model_runner.vision_input_names + } + fresh_inputs = self._compose_inputs(vehicle_speed, lat_horizon, long_horizon) + + started_at = time.perf_counter() + outputs = self._runtime.run(vision_bufs, warp_map, fresh_inputs) + execution_time = time.perf_counter() - started_at + execution_us = int(execution_time * 1_000_000) + + sample = PerfSample( + frame_id=main_stamp.frame_id, + model_eval_us=execution_us, + model_dropped_frames=dropped_frames, + model_backlog=max(0, dropped_frames), + ) + self._perf_ring.push(sample) + if dropped_frames > 0 or execution_us >= IQMODEL_EVAL_WARN_US: + severity = "warning" + if dropped_frames > 0 or execution_us >= IQMODEL_EVAL_ERROR_US: + severity = "error" + self._perf.emit( + "iqmodeld_dropped_frames" if dropped_frames > 0 else "iqmodeld_slow_eval", + severity=severity, + frame_id=main_stamp.frame_id, + total_time_us=execution_us, + dropped_frames=dropped_frames, + backlog=max(0, dropped_frames), + samples=self._perf_ring.snapshot(), + detail=( + f"model_eval_us={execution_us} dropped_frames={dropped_frames} prepare_only={int(prepare_only)} " + f"road_frame_id={self._sub['roadCameraState'].frameId}" + ), + min_interval_s=0.25, + ) + + if outputs is not None: + self._publish( + outputs, + main_stamp, + extra_stamp, + self._sub["roadCameraState"].frameId, + frame_drop_ratio, + dropped_frames, + execution_time, + live_calib_seen, + lat_horizon, + long_horizon, + vehicle_speed, + ) + + self._frame_meter.commit(main_stamp.frame_id) + tick += 1 + + +def main(demo: bool = False, channel_path: str | None = "auto"): + if channel_path == "auto": + channel_path = None + params = Params() + from iqpilot.selfdrive.iqmodeld.egpu_helpers import egpu_selected + if params.get_bool("IQEmacEnabled") or egpu_selected(params): + from iqpilot.selfdrive.iqmodeld.model_channel import SMALL_CHANNEL + channel_path = SMALL_CHANNEL + InferenceDaemon(demo=demo, channel_path=channel_path).serve() + + +__all__ = [ + "PROCESS_NAME", + "IQP_NAV_MODEL_INFLUENCE_ENABLED", + "TurnDirection", + "CaptureStamp", + "ReplayLedger", + "NeuralEngineState", + "CameraIngress", + "CalibrationAtlas", + "FrameDropMeter", + "InferenceDaemon", + "main", +] + + +if __name__ == "__main__": + try: + import argparse + + parser = argparse.ArgumentParser() + parser.add_argument("--demo", action="store_true", help="Run iqmodeld in demo mode.") + args = parser.parse_args() + main(demo=args.demo) + except KeyboardInterrupt: + cloudlog.warning(f"child {PROCESS_NAME} got SIGINT") + except Exception: + sentry.capture_exception() + raise diff --git a/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/iqmodeld/driving_action.py b/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/iqmodeld/driving_action.py new file mode 100644 index 000000000..3e5f9750c --- /dev/null +++ b/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/iqmodeld/driving_action.py @@ -0,0 +1,46 @@ +""" +Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos/ +""" +from __future__ import annotations + +import numpy as np + +from iqpilot.cereal import log + +from iqpilot.selfdrive.controls.lib.drive_helpers import smooth_value + +LAT_SMOOTH_SECONDS = 0.0 +LONG_SMOOTH_SECONDS = 0.3 +MIN_LAT_CONTROL_SPEED = 0.3 +DESIRE_LEN = 8 + + +def get_action_from_model(outputs: dict[str, np.ndarray], prev_action: log.ModelDataV2.Action, + v_ego: float, lat_action_t: float, long_action_t: float, + lat_smooth_seconds: float | None = None) -> log.ModelDataV2.Action: + if "action" in outputs: + desired_accel = float(outputs["action"][0, 1]) + desired_curvature = float(outputs["action"][0, 0]) / (max(1.0, v_ego)) ** 2 + should_stop = bool(v_ego < 0.3 and desired_accel < 0.1) + else: + from iqpilot.selfdrive.controls.lib.drive_helpers import get_accel_from_plan, get_curvature_from_plan + from iqpilot.selfdrive.iqmodeld.config import ModelConstants, Plan + plan = outputs["plan"][0] + desired_accel, should_stop = get_accel_from_plan(plan[:, Plan.VELOCITY][:, 0], + plan[:, Plan.ACCELERATION][:, 0], + ModelConstants.T_IDXS, + action_t=long_action_t) + desired_curvature = get_curvature_from_plan(plan[:, Plan.T_FROM_CURRENT_EULER][:, 2], + plan[:, Plan.ORIENTATION_RATE][:, 2], + ModelConstants.T_IDXS, v_ego, lat_action_t) + desired_accel, should_stop = float(desired_accel), bool(should_stop) + desired_curvature = float(desired_curvature) + desired_accel = smooth_value(desired_accel, prev_action.desiredAcceleration, LONG_SMOOTH_SECONDS) + if v_ego > MIN_LAT_CONTROL_SPEED: + lat_smooth = LAT_SMOOTH_SECONDS if lat_smooth_seconds is None else lat_smooth_seconds + desired_curvature = smooth_value(desired_curvature, prev_action.desiredCurvature, lat_smooth) + else: + desired_curvature = prev_action.desiredCurvature + return log.ModelDataV2.Action(desiredCurvature=float(desired_curvature), + desiredAcceleration=float(desired_accel), + shouldStop=should_stop) diff --git a/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/iqmodeld/egpu_helpers.py b/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/iqmodeld/egpu_helpers.py new file mode 100644 index 000000000..d09895c86 --- /dev/null +++ b/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/iqmodeld/egpu_helpers.py @@ -0,0 +1,158 @@ +""" +Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos/ +""" +from __future__ import annotations + +import hashlib +import json +import os +import urllib.request +from pathlib import Path + +from iqpilot.system.hardware.usb import egpu_dock_ready + +USB_SYSFS_ROOT = "/sys/bus/usb/devices" + +COMMA_LFS_BATCH_URL = "https://gitlab.com/commaai/openpilot-lfs.git/info/lfs/objects/batch" + +DOWNLOAD_CHUNK = 4 * 1024 * 1024 + + +def usbgpu_present(sysfs_root: str = USB_SYSFS_ROOT) -> bool: + return egpu_dock_ready(Path(sysfs_root)) + + +def egpu_present_consented(params, sysfs_root: str = USB_SYSFS_ROOT) -> bool: + try: + if params is not None and params.get_bool("IQEgpuDisabled"): + return False + except Exception: + pass + return usbgpu_present(sysfs_root) + + +def egpu_selected(params, sysfs_root: str = USB_SYSFS_ROOT) -> bool: + try: + if params is not None and params.get_bool("IQEgpuDisabled"): + return False + if params is not None and params.get_bool("IQEgpuEnabled"): + return True + except Exception: + pass + return usbgpu_present(sysfs_root) + + +def resolve_backend(emac_enabled: bool, egpu_enabled: bool, egpu_present: bool = False) -> str | None: + if egpu_present: + return "egpu" + if emac_enabled: + return "emac" + if egpu_enabled: + return "egpu" + return None + + +def egpu_pkl_path(meta: dict) -> str: + from iqpilot.system.hardware.hw import Paths + return os.path.join(Paths.model_root(), f"egpu_{meta['key']}_{meta['sha256'][:8]}_amd_tinygrad.pkl") + + +def onnx_cache_path(meta: dict) -> str: + from iqpilot.system.hardware.hw import Paths + return os.path.join(Paths.model_root(), f"{meta['model_name']}_{meta['sha256'][:8]}.onnx") + + +def _sha256_file(path: str) -> str: + digest = hashlib.sha256() + with open(path, "rb") as f: + while chunk := f.read(DOWNLOAD_CHUNK): + digest.update(chunk) + return digest.hexdigest() + + +def quarantine_artifact(path: str, why: str) -> None: + try: + if os.path.isfile(path): + os.replace(path, path + ".unusable") + except OSError: + try: + os.remove(path) + except OSError: + pass + + +def local_onnx(meta: dict) -> str | None: + path = onnx_cache_path(meta) + if not os.path.isfile(path): + return None + size = int(meta.get("download", {}).get("size", 0)) + if size and os.path.getsize(path) != size: + quarantine_artifact(path, "onnx size mismatch") + return None + if _sha256_file(path) != meta["sha256"]: + quarantine_artifact(path, "onnx sha256 mismatch") + return None + return path + + +def resolve_download_url(download_url: str, sha256: str, size: int, timeout: float = 30.0) -> str: + if download_url.startswith("commalfs:"): + oid = download_url.split(":", 1)[1] + body = json.dumps({"operation": "download", "transfers": ["basic"], + "objects": [{"oid": oid, "size": size}]}).encode() + req = urllib.request.Request(COMMA_LFS_BATCH_URL, data=body, headers={ + "Accept": "application/vnd.git-lfs+json", "Content-Type": "application/vnd.git-lfs+json"}) + with urllib.request.urlopen(req, timeout=timeout) as r: + d = json.load(r) + return d["objects"][0]["actions"]["download"]["href"] + return download_url + + +def download_onnx(meta: dict, progress_cb=None) -> str: + from iqpilot.selfdrive.iqmodeld.egpu_model import download_descriptor + download_url, size = download_descriptor(meta) + if not download_url: + raise RuntimeError(f"model {meta['key']} has no download source; stage the onnx at {onnx_cache_path(meta)}") + + url = resolve_download_url(download_url, meta["sha256"], size) + path = onnx_cache_path(meta) + os.makedirs(os.path.dirname(path), exist_ok=True) + tmp = path + ".part" + digest = hashlib.sha256() + got = 0 + with urllib.request.urlopen(url, timeout=60) as r, open(tmp, "wb") as f: + while chunk := r.read(DOWNLOAD_CHUNK): + f.write(chunk) + digest.update(chunk) + got += len(chunk) + if progress_cb is not None and size: + progress_cb(got / size) + if size and got != size: + os.remove(tmp) + raise RuntimeError(f"onnx download truncated: {got}/{size} bytes") + if digest.hexdigest() != meta["sha256"]: + os.remove(tmp) + raise RuntimeError(f"onnx sha256 mismatch for {meta['key']}") + os.replace(tmp, path) + return path + + +def patch_tinygrad_fetch_fw() -> None: + import pathlib + + import zstandard + from tinygrad import helpers + if getattr(helpers.fetch_fw, "_iq_patched", False): + return + _orig = helpers.fetch_fw + + def fetch_fw(path, name, sha256): + p = pathlib.Path(f"/lib/firmware/{path}/{name}.zst") + if p.is_file(): + blob = zstandard.ZstdDecompressor().stream_reader(p.read_bytes()).read() + if hashlib.sha256(blob).hexdigest() == sha256: + return blob + return _orig(path, name, sha256) + + fetch_fw._iq_patched = True + helpers.fetch_fw = fetch_fw diff --git a/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/iqmodeld/egpu_model.py b/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/iqmodeld/egpu_model.py new file mode 100644 index 000000000..b9dc2c77e --- /dev/null +++ b/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/iqmodeld/egpu_model.py @@ -0,0 +1,9 @@ +""" +Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos/ +""" +from iqpilot._proprietary_loader import ProprietaryModuleMissing, load_private_module + +try: + load_private_module(__name__, "iqpilot_private.models.egpu_model") +except ProprietaryModuleMissing: + from iqpilot.models_private_src.egpu_model import * diff --git a/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/iqmodeld/emac_model_meta.py b/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/iqmodeld/emac_model_meta.py new file mode 100644 index 000000000..75f405d67 --- /dev/null +++ b/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/iqmodeld/emac_model_meta.py @@ -0,0 +1,9 @@ +""" +Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos/ +""" +from iqpilot._proprietary_loader import ProprietaryModuleMissing, load_private_module + +try: + load_private_module(__name__, "iqpilot_private.models.emac_model_meta") +except ProprietaryModuleMissing: + from iqpilot.models_private_src.emac_model_meta import * diff --git a/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/iqmodeld/messaging.py b/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/iqmodeld/messaging.py new file mode 100644 index 000000000..ca43b2702 --- /dev/null +++ b/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/iqmodeld/messaging.py @@ -0,0 +1,269 @@ +from __future__ import annotations + +import os +from dataclasses import dataclass, field + +import capnp +import numpy as np + +from iqpilot.cereal import log +from iqpilot.selfdrive.iqmodeld.models.helpers import plan_x_idxs_helper +from iqpilot.selfdrive.iqmodeld.config import ModelConstants, Plan +from iqpilot.selfdrive.controls.lib.drive_helpers import get_curvature_from_plan + +SEND_RAW_PRED = os.getenv("SEND_RAW_PRED") +ConfidenceClass = log.ModelDataV2.ConfidenceClass + + +def pick_curvature(outputs: dict[str, np.ndarray], plan_rows: np.ndarray, vehicle_speed: float, + action_horizon: float, synthetic_lane_logic: bool) -> float: + direct_signal = None if synthetic_lane_logic else outputs.get("desired_curvature") + if direct_signal is not None: + return float(direct_signal[0, 0]) + + yaw_track = plan_rows[:, Plan.T_FROM_CURRENT_EULER][:, 2] + yaw_rate_track = plan_rows[:, Plan.ORIENTATION_RATE][:, 2] + return float(get_curvature_from_plan(yaw_track, yaw_rate_track, ModelConstants.T_IDXS, vehicle_speed, action_horizon)) + + +@dataclass +class DrivePacketMemory: + disengage_rollup: np.ndarray = field(default_factory=lambda: np.zeros( + ModelConstants.CONFIDENCE_BUFFER_LEN * ModelConstants.DISENGAGE_WIDTH, dtype=np.float32)) + brake_watch_5: np.ndarray = field(default_factory=lambda: np.zeros( + ModelConstants.FCW_5MS2_PROBS_WIDTH, dtype=np.float32)) + brake_watch_3: np.ndarray = field(default_factory=lambda: np.zeros( + ModelConstants.FCW_3MS2_PROBS_WIDTH, dtype=np.float32)) + + +def _assign_xyz(builder, t_points, x_track, y_track, z_track, + x_std=None, y_std=None, z_std=None) -> None: + builder.t = t_points + builder.x = x_track.tolist() + builder.y = y_track.tolist() + builder.z = z_track.tolist() + if x_std is not None: + builder.xStd = x_std.tolist() + if y_std is not None: + builder.yStd = y_std.tolist() + if z_std is not None: + builder.zStd = z_std.tolist() + + +def _assign_xyva(builder, t_points, x_track, y_track, v_track, a_track, + x_std=None, y_std=None, v_std=None, a_std=None) -> None: + builder.t = t_points + builder.x = x_track.tolist() + builder.y = y_track.tolist() + builder.v = v_track.tolist() + builder.a = a_track.tolist() + if x_std is not None: + builder.xStd = x_std.tolist() + if y_std is not None: + builder.yStd = y_std.tolist() + if v_std is not None: + builder.vStd = v_std.tolist() + if a_std is not None: + builder.aStd = a_std.tolist() + + +def fill_xyz_poly(builder, degree: int, x_track: np.ndarray, y_track: np.ndarray, z_track: np.ndarray) -> None: + stacked = np.stack([x_track, y_track, z_track], axis=1) + coeffs = np.polynomial.polynomial.polyfit(ModelConstants.T_IDXS, stacked, deg=degree) + builder.xCoefficients = coeffs[:, 0].tolist() + builder.yCoefficients = coeffs[:, 1].tolist() + builder.zCoefficients = coeffs[:, 2].tolist() + + +def fill_lane_line_meta(builder, lane_lines, lane_probs: list[float]) -> None: + builder.leftY = lane_lines[1].y[0] + builder.leftProb = lane_probs[1] + builder.rightY = lane_lines[2].y[0] + builder.rightProb = lane_probs[2] + + +def _roll_brake_watch(outputs: dict[str, np.ndarray], memory: DrivePacketMemory, meta_layout) -> bool: + memory.brake_watch_5[:-1] = memory.brake_watch_5[1:] + memory.brake_watch_5[-1] = outputs["meta"][0, meta_layout.HARD_BRAKE_5][0] + memory.brake_watch_3[:-1] = memory.brake_watch_3[1:] + memory.brake_watch_3[-1] = outputs["meta"][0, meta_layout.HARD_BRAKE_3][0] + return bool( + (memory.brake_watch_5 > ModelConstants.FCW_THRESHOLDS_5MS2).all() + and (memory.brake_watch_3 > ModelConstants.FCW_THRESHOLDS_3MS2).all() + ) + + +def _confidence_bucket(outputs: dict[str, np.ndarray], memory: DrivePacketMemory, meta_layout, frame_id: int): + width = ModelConstants.DISENGAGE_WIDTH + if frame_id % (2 * ModelConstants.MODEL_FREQ) == 0: + brake_probs = outputs["meta"][0, meta_layout.BRAKE_DISENGAGE] + gas_probs = outputs["meta"][0, meta_layout.GAS_DISENGAGE] + steer_probs = outputs["meta"][0, meta_layout.STEER_OVERRIDE] + takeover_curve = 1 - ((1 - brake_probs) * (1 - gas_probs) * (1 - steer_probs)) + independent = np.r_[takeover_curve[0], np.diff(takeover_curve) / (1 - takeover_curve[:-1])] + memory.disengage_rollup[:-width] = memory.disengage_rollup[width:] + memory.disengage_rollup[-width:] = independent + + score = 0.0 + for idx in range(width): + score += memory.disengage_rollup[idx * width + width - 1 - idx].item() / width + + if score < ModelConstants.RYG_GREEN: + return ConfidenceClass.green + if score < ModelConstants.RYG_YELLOW: + return ConfidenceClass.yellow + return ConfidenceClass.red + + +def _write_plan_family(model_packet, driving_packet, outputs: dict[str, np.ndarray]) -> None: + plan_rows = outputs["plan"][0] + plan_stds = outputs["plan_stds"][0] + _assign_xyz(model_packet.position, ModelConstants.T_IDXS, *plan_rows[:, Plan.POSITION].T, *plan_stds[:, Plan.POSITION].T) + _assign_xyz(model_packet.velocity, ModelConstants.T_IDXS, *plan_rows[:, Plan.VELOCITY].T) + _assign_xyz(model_packet.acceleration, ModelConstants.T_IDXS, *plan_rows[:, Plan.ACCELERATION].T) + _assign_xyz(model_packet.orientation, ModelConstants.T_IDXS, *plan_rows[:, Plan.T_FROM_CURRENT_EULER].T) + _assign_xyz(model_packet.orientationRate, ModelConstants.T_IDXS, *plan_rows[:, Plan.ORIENTATION_RATE].T) + fill_xyz_poly(driving_packet.path, ModelConstants.POLY_PATH_DEGREE, *plan_rows[:, Plan.POSITION].T) + + +def _write_temporal_pose(model_packet, outputs: dict[str, np.ndarray]) -> None: + pose_packet = model_packet.temporalPoseDEPRECATED + if "sim_pose" in outputs: + half_width = ModelConstants.POSE_WIDTH // 2 + pose_packet.trans = outputs["sim_pose"][0, :half_width].tolist() + pose_packet.transStd = outputs["sim_pose_stds"][0, :half_width].tolist() + pose_packet.rot = outputs["sim_pose"][0, half_width:].tolist() + pose_packet.rotStd = outputs["sim_pose_stds"][0, half_width:].tolist() + return + + pose_packet.trans = outputs["plan"][0, 0, Plan.VELOCITY].tolist() + pose_packet.transStd = outputs["plan_stds"][0, 0, Plan.VELOCITY].tolist() + pose_packet.rot = outputs["plan"][0, 0, Plan.ORIENTATION_RATE].tolist() + pose_packet.rotStd = outputs["plan_stds"][0, 0, Plan.ORIENTATION_RATE].tolist() + + +def _write_lane_family(model_packet, driving_packet, outputs: dict[str, np.ndarray]) -> None: + time_axis = plan_x_idxs_helper(ModelConstants, Plan, outputs) + model_packet.init("laneLines", 4) + for lane_idx in range(4): + lane_builder = model_packet.laneLines[lane_idx] + _assign_xyz( + lane_builder, + time_axis, + np.array(ModelConstants.X_IDXS), + outputs["lane_lines"][0, lane_idx, :, 0], + outputs["lane_lines"][0, lane_idx, :, 1], + ) + model_packet.laneLineStds = outputs["lane_lines_stds"][0, :, 0, 0].tolist() + model_packet.laneLineProbs = outputs["lane_lines_prob"][0, 1::2].tolist() + fill_lane_line_meta(driving_packet.laneLineMeta, model_packet.laneLines, model_packet.laneLineProbs) + + model_packet.init("roadEdges", 2) + for edge_idx in range(2): + edge_builder = model_packet.roadEdges[edge_idx] + _assign_xyz( + edge_builder, + time_axis, + np.array(ModelConstants.X_IDXS), + outputs["road_edges"][0, edge_idx, :, 0], + outputs["road_edges"][0, edge_idx, :, 1], + ) + model_packet.roadEdgeStds = outputs["road_edges_stds"][0, :, 0, 0].tolist() + + +def _write_leads(model_packet, outputs: dict[str, np.ndarray]) -> None: + model_packet.init("leadsV3", 3) + for lead_idx in range(3): + lead_builder = model_packet.leadsV3[lead_idx] + _assign_xyva( + lead_builder, + ModelConstants.LEAD_T_IDXS, + *outputs["lead"][0, lead_idx].T, + *outputs["lead_stds"][0, lead_idx].T, + ) + lead_builder.prob = outputs["lead_prob"][0, lead_idx].tolist() + lead_builder.probTime = ModelConstants.LEAD_T_OFFSETS[lead_idx] + + +def _write_meta(model_packet, outputs: dict[str, np.ndarray], memory: DrivePacketMemory, meta_layout, frame_id: int) -> None: + meta = model_packet.meta + meta.desireState = outputs["desire_state"][0].reshape(-1).tolist() + meta.desirePrediction = outputs["desire_pred"][0].reshape(-1).tolist() + meta.engagedProb = outputs["meta"][0, meta_layout.ENGAGED].item() + meta.init("disengagePredictions") + + pred = meta.disengagePredictions + pred.t = ModelConstants.META_T_IDXS + pred.brakeDisengageProbs = outputs["meta"][0, meta_layout.BRAKE_DISENGAGE].tolist() + pred.gasDisengageProbs = outputs["meta"][0, meta_layout.GAS_DISENGAGE].tolist() + pred.steerOverrideProbs = outputs["meta"][0, meta_layout.STEER_OVERRIDE].tolist() + pred.brake3MetersPerSecondSquaredProbs = outputs["meta"][0, meta_layout.HARD_BRAKE_3].tolist() + pred.brake4MetersPerSecondSquaredProbs = outputs["meta"][0, meta_layout.HARD_BRAKE_4].tolist() + pred.brake5MetersPerSecondSquaredProbs = outputs["meta"][0, meta_layout.HARD_BRAKE_5].tolist() + + if hasattr(meta_layout, "GAS_PRESS") and hasattr(meta_layout, "BRAKE_PRESS"): + pred.gasPressProbs = outputs["meta"][0, meta_layout.GAS_PRESS].tolist() + pred.brakePressProbs = outputs["meta"][0, meta_layout.BRAKE_PRESS].tolist() + + meta.hardBrakePredicted = _roll_brake_watch(outputs, memory, meta_layout) + model_packet.confidence = _confidence_bucket(outputs, memory, meta_layout, frame_id) + + +def populate_drive_messages(primary_msg: capnp._DynamicStructBuilder, extended_msg: capnp._DynamicStructBuilder, + outputs: dict[str, np.ndarray], action: log.ModelDataV2.Action, + memory: DrivePacketMemory, vipc_frame_id: int, vipc_frame_id_extra: int, + frame_id: int, frame_drop: float, timestamp_eof: int, + model_execution_time: float, valid: bool, meta_layout) -> None: + frame_age = frame_id - vipc_frame_id if frame_id > vipc_frame_id else 0 + frame_drop_percent = frame_drop * 100 + primary_msg.valid = valid + extended_msg.valid = valid + + driving_packet = primary_msg.drivingModelData + driving_packet.frameId = vipc_frame_id + driving_packet.frameIdExtra = vipc_frame_id_extra + driving_packet.frameDropPerc = frame_drop_percent + driving_packet.modelExecutionTime = model_execution_time + driving_packet.action = action + + model_packet = extended_msg.modelV2 + model_packet.frameId = vipc_frame_id + model_packet.frameIdExtra = vipc_frame_id_extra + model_packet.frameAge = frame_age + model_packet.frameDropPerc = frame_drop_percent + model_packet.timestampEof = timestamp_eof + model_packet.modelExecutionTime = model_execution_time + model_packet.action = action + + _write_plan_family(model_packet, driving_packet, outputs) + _write_temporal_pose(model_packet, outputs) + _write_lane_family(model_packet, driving_packet, outputs) + _write_leads(model_packet, outputs) + _write_meta(model_packet, outputs, memory, meta_layout, vipc_frame_id) + + if SEND_RAW_PRED: + model_packet.rawPredictions = outputs["raw_pred"].tobytes() + + +def populate_odometry_message(msg: capnp._DynamicStructBuilder, outputs: dict[str, np.ndarray], + vipc_frame_id: int, vipc_dropped_frames: int, + timestamp_eof: int, live_calib_seen: bool) -> None: + msg.valid = live_calib_seen & (vipc_dropped_frames < 1) + odo = msg.cameraOdometry + odo.frameId = vipc_frame_id + odo.timestampEof = timestamp_eof + odo.trans = outputs["pose"][0, :3].tolist() + odo.rot = outputs["pose"][0, 3:].tolist() + odo.wideFromDeviceEuler = outputs["wide_from_device_euler"][0, :].tolist() + odo.roadTransformTrans = outputs["road_transform"][0, :3].tolist() + odo.transStd = outputs["pose_stds"][0, :3].tolist() + odo.rotStd = outputs["pose_stds"][0, 3:].tolist() + odo.wideFromDeviceEulerStd = outputs["wide_from_device_euler_stds"][0, :].tolist() + odo.roadTransformTransStd = outputs["road_transform_stds"][0, :3].tolist() + +__all__ = [ + "DrivePacketMemory", + "pick_curvature", + "populate_drive_messages", + "populate_odometry_message", +] diff --git a/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/iqmodeld/metadata.py b/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/iqmodeld/metadata.py new file mode 100644 index 000000000..c81ebe542 --- /dev/null +++ b/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/iqmodeld/metadata.py @@ -0,0 +1,96 @@ +#!/usr/bin/env python3 +import codecs +import pathlib +import pickle +import sys +from collections.abc import Iterable +from typing import Any + +from iqpilot.cereal import custom +from tinygrad.nn.onnx import OnnxPBParser + +from iqpilot.selfdrive.iqmodeld.models.helpers import get_active_bundle +from iqpilot.selfdrive.iqmodeld.config import Meta + + +ModelBundle = custom.IQModelManager.ModelBundle + + +def _blank_proto_doc() -> dict[str, Any]: + return {"graph": {"input": [], "output": []}, "metadata_props": []} + + +class TelemetryEnvelopeParser(OnnxPBParser): + def _parse_ModelProto(self) -> dict: + envelope = _blank_proto_doc() + for fid, wire_type in self._parse_message(self.reader.len): + if fid == 7: + envelope["graph"] = self._parse_GraphProto() + elif fid == 14: + envelope["metadata_props"].append(self._parse_StringStringEntryProto()) + else: + self.reader.skip_field(wire_type) + return envelope + + +def _shape_fingerprint(value_info: dict[str, Any]) -> tuple[str, tuple[int, ...]]: + resolved = [] + for axis in value_info["parsed_type"].shape: + resolved.append(int(axis) if isinstance(axis, int) else 0) + return value_info["name"], tuple(resolved) + + +def _lookup_metadata(props: Iterable[dict[str, Any]], wanted_key: str) -> str | Any: + for entry in props: + if entry["key"] == wanted_key: + return entry["value"] + return None + + +class Meta20hz(Meta): + ENGAGED = slice(0, 1) + GAS_DISENGAGE = slice(1, 31, 6) + BRAKE_DISENGAGE = slice(2, 31, 6) + STEER_OVERRIDE = slice(3, 31, 6) + HARD_BRAKE_3 = slice(4, 31, 6) + HARD_BRAKE_4 = slice(5, 31, 6) + HARD_BRAKE_5 = slice(6, 31, 6) + GAS_PRESS = slice(31, 55, 4) + BRAKE_PRESS = slice(32, 55, 4) + LEFT_BLINKER = slice(33, 55, 4) + RIGHT_BLINKER = slice(34, 55, 4) + + +def select_meta_layout(): + active_bundle = get_active_bundle() + return Meta20hz if active_bundle is not None and active_bundle.is20hz else Meta + + +def _decoded_slices(props: Iterable[dict[str, Any]]): + encoded = _lookup_metadata(props, "output_slices") + assert encoded is not None, "output_slices not found in metadata" + return pickle.loads(codecs.decode(encoded.encode(), "base64")) + + +def _graph_shape_table(graph_doc: dict[str, Any], field_name: str) -> dict[str, tuple[int, ...]]: + return dict(_shape_fingerprint(item) for item in graph_doc[field_name]) + + +def build_metadata_record(model_path): + parsed = TelemetryEnvelopeParser(model_path).parse() + props = parsed["metadata_props"] + graph = parsed["graph"] + return { + "model_checkpoint": _lookup_metadata(props, "model_checkpoint"), + "output_slices": _decoded_slices(props), + "input_shapes": _graph_shape_table(graph, "input"), + "output_shapes": _graph_shape_table(graph, "output"), + } + + +if __name__ == "__main__": + model_path = pathlib.Path(sys.argv[1]) + metadata_path = model_path.parent / f"{model_path.stem}_metadata.pkl" + with open(metadata_path, "wb") as handle: + pickle.dump(build_metadata_record(model_path), handle) + print(f"saved metadata to {metadata_path}") diff --git a/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/iqmodeld/model_channel.py b/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/iqmodeld/model_channel.py new file mode 100644 index 000000000..a9fa710bc --- /dev/null +++ b/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/iqmodeld/model_channel.py @@ -0,0 +1,58 @@ +""" +Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos/ +""" +from __future__ import annotations + +import mmap +import os +import pickle +import struct + +from iqpilot.common.swaglog import cloudlog + +SMALL_CHANNEL = "/dev/shm/iqpilot_smallmodel" +BIG_CHANNEL = "/dev/shm/iqpilot_bigmodel" +SHM_SIZE = 8 * 1024 * 1024 +HEADER = struct.Struct(" None: + data = pickle.dumps(payload, protocol=pickle.HIGHEST_PROTOCOL) + if HEADER.size + len(data) > SHM_SIZE: + cloudlog.error(f"model payload {len(data)} bytes exceeds shm {SHM_SIZE}, dropping frame {frame_id}") + return + seq = HEADER.unpack(self.mm[:HEADER.size])[0] + HEADER.pack_into(self.mm, 0, seq + 1, frame_id, len(data)) + self.mm[HEADER.size:HEADER.size + len(data)] = data + HEADER.pack_into(self.mm, 0, seq + 2, frame_id, len(data)) + + def peek_frame_id(self) -> int | None: + seq, frame_id, length = HEADER.unpack(self.mm[:HEADER.size]) + if seq == 0 or seq % 2 != 0 or length == 0: + return None + return frame_id + + def read(self) -> tuple[int, dict] | None: + seq1, frame_id, length = HEADER.unpack(self.mm[:HEADER.size]) + if seq1 == 0 or seq1 % 2 != 0 or length == 0: + return None + data = bytes(self.mm[HEADER.size:HEADER.size + length]) + seq2 = HEADER.unpack(self.mm[:HEADER.size])[0] + if seq1 != seq2: + return None + try: + return frame_id, pickle.loads(data) + except Exception: + return None diff --git a/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/iqmodeld/model_warp.py b/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/iqmodeld/model_warp.py new file mode 100644 index 000000000..50697728a --- /dev/null +++ b/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/iqmodeld/model_warp.py @@ -0,0 +1,80 @@ +""" +Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos/ +""" +from __future__ import annotations + +import os +import pickle + +import numpy as np + +from iqpilot.common.swaglog import cloudlog +from iqpilot.system.hardware.hw import Paths + + +def _load_bundle(pkl_path: str, cam_w: int, cam_h: int, frame_skip: int) -> dict: + with open(pkl_path, "rb") as f: + bundle = pickle.load(f) + if bundle.get("frame_skip") != frame_skip: + raise RuntimeError(f"frame_skip {bundle.get('frame_skip')} != {frame_skip}") + if (cam_w, cam_h) not in bundle: + raise RuntimeError(f"missing {cam_w}x{cam_h}; has {[k for k in bundle if isinstance(k, tuple)]}") + _verify_selftest(bundle, cam_w, cam_h) + return bundle + + +def _verify_selftest(bundle: dict, cam_w: int, cam_h: int) -> None: + want = bundle.get("selftest") + if not want: + raise RuntimeError("warp artifact predates the self-test; recompiling") + from iqpilot.system.camerad.cameras.nv12_info import get_nv12_info + from iqpilot.selfdrive.iqmodeld.tools.compile_warp import selftest_digest + nv12_size = get_nv12_info(cam_w, cam_h)[3] + got = selftest_digest(bundle[(cam_w, cam_h)], cam_w, cam_h, nv12_size) + if got != want: + raise RuntimeError(f"warp self-test {got[:12]} != {want[:12]}; artifact computes differently here") + + +class FrameWarp: + + def __init__(self, cam_w: int, cam_h: int, frame_skip: int): + from tinygrad.tensor import Tensor + + pkl_path = os.path.join(Paths.model_root(), f"emac_warp_{cam_w}x{cam_h}_tinygrad.pkl") + bundle = None + if os.path.isfile(pkl_path): + try: + bundle = _load_bundle(pkl_path, cam_w, cam_h, frame_skip) + except Exception as e: + cloudlog.warning(f"warp artifact unusable ({e}); discarding and recompiling") + os.remove(pkl_path) + if bundle is None: + cloudlog.warning(f"warp artifact missing; compiling for {cam_w}x{cam_h} (one-time)") + from iqpilot.selfdrive.iqmodeld.tools.compile_warp import compile_warp + compile_warp(cam_w, cam_h, pkl_path, frame_skip=frame_skip) + cloudlog.warning(f"warp compiled -> {pkl_path}") + bundle = _load_bundle(pkl_path, cam_w, cam_h, frame_skip) + self._jit = bundle[(cam_w, cam_h)] + + self._npy = {"tfm": np.zeros((3, 3), dtype=np.float32), "big_tfm": np.zeros((3, 3), dtype=np.float32)} + self._tensors = {k: Tensor(v, device="NPY").realize() for k, v in self._npy.items()} + self._blob_cache: dict[tuple[str, int], object] = {} + self._Tensor = Tensor + + def _frame_tensor(self, key: str, buf): + from tinygrad.device import Device + arr = np.frombuffer(buf.data, dtype=np.uint8) + ck = (key, arr.ctypes.data) + t = self._blob_cache.get(ck) + if t is None: + t = self._Tensor.from_blob(arr.ctypes.data, (arr.size,), dtype="uint8", device=Device.DEFAULT) + self._blob_cache[ck] = t + return t + + def run(self, main_buf, extra_buf, main_tfm: np.ndarray, extra_tfm: np.ndarray) -> np.ndarray: + self._npy["tfm"][:] = main_tfm + self._npy["big_tfm"][:] = extra_tfm + warped = self._jit(tfm=self._tensors["tfm"], big_tfm=self._tensors["big_tfm"], + frame=self._frame_tensor("img", main_buf), + big_frame=self._frame_tensor("big_img", extra_buf)) + return warped.numpy().astype(np.uint8, copy=False) diff --git a/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/iqmodeld/models/__init__.py b/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/iqmodeld/models/__init__.py new file mode 100644 index 000000000..5ece1c074 --- /dev/null +++ b/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/iqmodeld/models/__init__.py @@ -0,0 +1,3 @@ +""" +Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos/ +""" diff --git a/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/iqmodeld/models/combined_artifact.py b/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/iqmodeld/models/combined_artifact.py new file mode 100644 index 000000000..bf17800a7 --- /dev/null +++ b/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/iqmodeld/models/combined_artifact.py @@ -0,0 +1,100 @@ +""" +Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos/ +""" + +from __future__ import annotations + +import os +import re +from pathlib import Path + +from iqpilot.system.hardware.hw import Paths + + +_MODEL_ROOT = Path(Paths.model_root()) +_OVERRIDE_KEYS = ( + "combinedRuntimeArtifact", + "combinedSplitArtifact", + "iqCombinedArtifact", +) +_SPLIT_ROLE_PATTERN = re.compile(r"^driving_(vision|policy|off_policy|on_policy)_(.+)_tinygrad\.pkl$") + + +def _bundle_models(bundle) -> list: + models = getattr(bundle, "models", None) + return list(models) if models is not None else [] + + +def _bundle_override_map(bundle) -> dict[str, str]: + result: dict[str, str] = {} + for override in getattr(bundle, "overrides", None) or []: + key = getattr(override, "key", None) + value = getattr(override, "value", None) + if key and value: + result[str(key)] = str(value) + return result + + +def _artifact_name(model) -> str: + return getattr(getattr(model, "artifact", None), "fileName", "") or "" + + +def _split_suffixes(bundle) -> list[str]: + suffixes: list[str] = [] + for model in _bundle_models(bundle): + match = _SPLIT_ROLE_PATTERN.match(_artifact_name(model)) + if match: + suffixes.append(match.group(2)) + return suffixes + + +def _derived_candidates(bundle) -> list[str]: + seen: set[str] = set() + candidates: list[str] = [] + + for suffix in _split_suffixes(bundle): + for candidate in ( + f"driving_combined_{suffix}.pkl", + f"iqmodeld_combined_{suffix}.pkl", + ): + if candidate not in seen: + seen.add(candidate) + candidates.append(candidate) + + ref = getattr(bundle, "ref", None) + if ref: + short_ref = str(ref)[:8] + for candidate in ( + f"driving_combined_{short_ref}.pkl", + f"iqmodeld_combined_{short_ref}.pkl", + ): + if candidate not in seen: + seen.add(candidate) + candidates.append(candidate) + + return candidates + + +def combined_split_artifact_candidates(bundle) -> list[Path]: + explicit_env = os.getenv("IQMODEL_COMBINED_PKL") + if explicit_env: + explicit_path = Path(explicit_env) + return [explicit_path if explicit_path.is_absolute() else _MODEL_ROOT / explicit_path] + + overrides = _bundle_override_map(bundle) + explicit_names = [overrides[key] for key in _OVERRIDE_KEYS if key in overrides] + if explicit_names: + return [_MODEL_ROOT / name for name in explicit_names] + + return [_MODEL_ROOT / name for name in _derived_candidates(bundle)] + + +def resolve_combined_split_artifact(bundle) -> Path | None: + for candidate in combined_split_artifact_candidates(bundle): + if candidate.is_file(): + return candidate + return None + + +def has_combined_split_artifact(bundle) -> bool: + return resolve_combined_split_artifact(bundle) is not None diff --git a/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/iqmodeld/models/fetcher.py b/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/iqmodeld/models/fetcher.py new file mode 100644 index 000000000..d06c7c3dd --- /dev/null +++ b/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/iqmodeld/models/fetcher.py @@ -0,0 +1,11 @@ +#!/usr/bin/env python3 +""" +Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos/ +""" + +from iqpilot._proprietary_loader import ProprietaryModuleMissing, load_private_module + +try: + load_private_module(__name__, "iqpilot_private.models.fetcher") +except ProprietaryModuleMissing: + from iqpilot.models_private_src.fetcher import * # noqa: F403 diff --git a/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/iqmodeld/models/helpers.py b/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/iqmodeld/models/helpers.py new file mode 100644 index 000000000..4b9a2f4d4 --- /dev/null +++ b/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/iqmodeld/models/helpers.py @@ -0,0 +1,313 @@ +#!/usr/bin/env python3 +""" +Copyright (c) IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos +""" +import json +import os +import shutil +from pathlib import Path + +from iqpilot.cereal import custom +from iqpilot.common.params import Params +from iqpilot.common.swaglog import cloudlog +from iqpilot._proprietary_loader import ProprietaryModuleMissing, load_private_module +from iqpilot.system.hardware.hw import Paths + +try: + load_private_module(__name__, "iqpilot_private.models.helpers") +except ProprietaryModuleMissing: + try: + from iqpilot.models_private_src.helpers import * # noqa: F403 + except ImportError: + pass + + +ModelBundle = custom.IQModelManager.ModelBundle +Runner = custom.IQModelManager.Runner +_MODEL_ROOT = Path(Paths.model_root()) +_ACTIVE_BUNDLE_KEY = "ModelManager_ActiveBundle" +_MODELS_CACHE_KEY = "ModelManager_ModelsCache" +_RUNNER_CACHE_KEY = "ModelRunnerTypeCache" +_DOWNLOAD_INDEX_KEY = "ModelManager_DownloadIndex" +_PENDING_INDEX_KEY = "ModelManager_PendingIndex" +_PENDING_MODEL_RESTORE_FILE = "/data/k3_pending_model_restore" +_STOCK_RUNNER = int(Runner.stock) +_TINYGRAD_RUNNER = int(Runner.tinygrad) +_SNPE_RUNNER = int(Runner.snpe) + +_DEFAULT_MODEL_DIR = Path(__file__).resolve().parents[1] / "default_model" +_DEFAULT_BUNDLE_JSON = _DEFAULT_MODEL_DIR / "bundle.json" +_DEFAULT_BUNDLE_REF = "default" + + +def get_default_model_bundle(_bundles): + return None + + +def _coerce_runner_value(value) -> int | None: + raw = getattr(value, "raw", value) + try: + return int(raw) + except (TypeError, ValueError): + return None + + +def _bundle_models(bundle) -> list: + models = getattr(bundle, "models", None) + return list(models) if models is not None else [] + + +def _bundle_needs_runtime_upgrade(bundle) -> bool: + if bundle is None: + return False + + if _coerce_runner_value(getattr(bundle, "runner", None)) == _SNPE_RUNNER: + return True + + for model in _bundle_models(bundle): + file_name = getattr(getattr(model, "artifact", None), "fileName", "") or "" + if file_name.endswith(".thneed"): + return True + + return False + + +def _load_cached_manifest_bundles(params: Params): + cached = params.get(_MODELS_CACHE_KEY) or {} + bundles = [] + for raw_bundle in cached.get("bundles", []): + try: + min_selector_version = int(raw_bundle.get("minimumSelectorVersion", raw_bundle.get("minimum_selector_version", 0))) + compatibility_view = dict(raw_bundle) + compatibility_view["minimumSelectorVersion"] = min_selector_version + is_compatible = globals().get("is_bundle_version_compatible") + if is_compatible is not None and not is_compatible(compatibility_view): + continue + + if "short_name" in raw_bundle: + from iqpilot.selfdrive.iqmodeld.models.fetcher import ManifestDecoder + bundles.append(ManifestDecoder._decode_bundle(raw_bundle)) + continue + + if "internalName" in raw_bundle: + bundles.append(ModelBundle(**raw_bundle)) + continue + + bundle = ModelBundle() + bundle.index = int(raw_bundle["index"]) + bundle.internalName = raw_bundle.get("short_name") + bundle.displayName = raw_bundle.get("display_name") + bundle.status = 0 + bundle.generation = int(raw_bundle["generation"]) + bundle.environment = raw_bundle["environment"] + bundle.runner = raw_bundle.get("runner", Runner.tinygrad) + bundle.is20hz = raw_bundle.get("is_20hz", False) + bundle.minimumSelectorVersion = int(min_selector_version) + bundle.ref = raw_bundle.get("ref") + bundle.overrides = [] + for key, value in raw_bundle.get("overrides", {}).items(): + override = custom.IQModelManager.Override() + override.key = key + override.value = value + bundle.overrides.append(override) + + bundle.models = [] + for raw_model in raw_bundle.get("models", []): + model = custom.IQModelManager.Model() + model.type = raw_model.get("type") + for attr_name in ("artifact", "metadata"): + raw_artifact = raw_model.get(attr_name) + if not raw_artifact: + continue + artifact = custom.IQModelManager.Artifact() + artifact.fileName = raw_artifact.get("file_name") + download_uri = custom.IQModelManager.DownloadUri() + download_uri.uri = raw_artifact.get("download_uri", {}).get("url") + download_uri.sha256 = raw_artifact.get("download_uri", {}).get("sha256") + artifact.downloadUri = download_uri + setattr(model, attr_name, artifact) + bundle.models.append(model) + + bundles.append(bundle) + except Exception: + continue + return bundles + + +def _bundle_match_key(bundle) -> tuple[str | None, str | None, str | None]: + return ( + getattr(bundle, "ref", None), + getattr(bundle, "internalName", None), + getattr(bundle, "displayName", None), + ) + + +def _find_runtime_upgrade(bundle, params: Params, available_bundles=None): + if not _bundle_needs_runtime_upgrade(bundle): + return bundle + + candidate_bundles = available_bundles if available_bundles is not None else _load_cached_manifest_bundles(params) + ref, internal_name, display_name = _bundle_match_key(bundle) + + for candidate in candidate_bundles: + if getattr(candidate, "ref", None) and getattr(candidate, "ref", None) == ref: + return candidate + + for candidate in candidate_bundles: + if getattr(candidate, "internalName", None) == internal_name: + return candidate + + for candidate in candidate_bundles: + if getattr(candidate, "displayName", None) == display_name: + return candidate + + return None + + +def bundle_files_ready(bundle) -> bool: + if bundle is None: + return False + + for model in _bundle_models(bundle): + artifact = getattr(model, "artifact", None) + metadata = getattr(model, "metadata", None) + for file_name in (getattr(metadata, "fileName", None), getattr(artifact, "fileName", None)): + if file_name and not (_MODEL_ROOT / file_name).is_file(): + return False + return True + + +def persist_active_bundle(params: Params, bundle) -> None: + params.put(_ACTIVE_BUNDLE_KEY, bundle.to_dict()) + params.remove(_RUNNER_CACHE_KEY) + + +def _load_default_bundle_dict() -> dict: + return json.loads(_DEFAULT_BUNDLE_JSON.read_text()) + + +def _default_bundle_filenames(bundle_dict: dict) -> list[str]: + names = [] + for model in bundle_dict.get("models", []): + for artifact in (model.get("metadata"), model.get("artifact")): + file_name = artifact.get("fileName", "") if isinstance(artifact, dict) else "" + if file_name: + names.append(file_name) + return names + + +def is_default_bundle(bundle) -> bool: + return bool(bundle is not None and getattr(bundle, "ref", None) == _DEFAULT_BUNDLE_REF) + + +def ensure_default_model_files(bundle_dict: dict = None) -> None: + bundle_dict = bundle_dict if bundle_dict is not None else _load_default_bundle_dict() + try: + _MODEL_ROOT.mkdir(parents=True, exist_ok=True) + except OSError as e: + cloudlog.exception(f"default_model: cannot create model root: {e}") + return + for file_name in _default_bundle_filenames(bundle_dict): + src = _DEFAULT_MODEL_DIR / file_name + dst = _MODEL_ROOT / file_name + if not src.is_file(): + cloudlog.error(f"default_model: shipped asset missing {src}") + continue + if dst.is_file() and dst.stat().st_size == src.stat().st_size: + continue + try: + shutil.copy2(src, dst) + cloudlog.warning(f"default_model: staged {file_name} into model root") + except OSError as e: + cloudlog.exception(f"default_model: failed staging {file_name}: {e}") + + +def select_default_model(params: Params = None) -> None: + params = Params() if params is None else params + bundle_dict = _load_default_bundle_dict() + ensure_default_model_files(bundle_dict) + params.remove(_DOWNLOAD_INDEX_KEY) + params.remove(_PENDING_INDEX_KEY) + params.put(_ACTIVE_BUNDLE_KEY, bundle_dict) + params.remove(_RUNNER_CACHE_KEY) + params.put(_RUNNER_CACHE_KEY, _TINYGRAD_RUNNER) + try: + if os.path.isfile(_PENDING_MODEL_RESTORE_FILE): + os.remove(_PENDING_MODEL_RESTORE_FILE) + except OSError: + pass + + +def seed_default_bundle_if_unset(params: Params = None) -> None: + params = Params() if params is None else params + if params.get(_ACTIVE_BUNDLE_KEY): + return + queued_download = params.get(_DOWNLOAD_INDEX_KEY) + try: + select_default_model(params) + if queued_download is not None: + params.put(_DOWNLOAD_INDEX_KEY, queued_download) + cloudlog.warning("default_model: seeded Default (CD210) as active bundle") + except Exception as e: + cloudlog.exception(f"default_model: failed to seed default bundle: {e}") + + +def get_runtime_bundle_upgrade(bundle, params: Params = None, available_bundles=None): + params = Params() if params is None else params + return _find_runtime_upgrade(bundle, params, available_bundles) + + +def get_active_bundle(params: Params = None): + params = Params() if params is None else params + + try: + active_bundle = params.get(_ACTIVE_BUNDLE_KEY) or {} + if not active_bundle: + return None + is_compatible = globals().get("is_bundle_version_compatible") + if is_compatible is not None and not is_compatible(active_bundle): + return None + bundle = ModelBundle(**active_bundle) + except Exception: + return None + + replacement = _find_runtime_upgrade(bundle, params) + if replacement is not None and replacement is not bundle and bundle_files_ready(replacement): + persist_active_bundle(params, replacement) + return replacement + + return bundle + + +def get_active_model_runner(params: Params = None, force_check=False): + params = Params() if params is None else params + + active_bundle = get_active_bundle(params) + if not active_bundle: + seed_default_bundle_if_unset(params) + active_bundle = get_active_bundle(params) + if not active_bundle: + if params.get(_RUNNER_CACHE_KEY) != str(_TINYGRAD_RUNNER): + params.put(_RUNNER_CACHE_KEY, _TINYGRAD_RUNNER) + return _TINYGRAD_RUNNER + + cached_runner_type = params.get(_RUNNER_CACHE_KEY) + if cached_runner_type and not force_check and isinstance(cached_runner_type, str) and cached_runner_type.isdigit(): + return int(cached_runner_type) + + runner_type = _coerce_runner_value(active_bundle.runner) + if runner_type == _SNPE_RUNNER: + replacement = _find_runtime_upgrade(active_bundle, params) + if replacement is not None and replacement is not active_bundle and bundle_files_ready(replacement): + persist_active_bundle(params, replacement) + runner_type = _coerce_runner_value(replacement.runner) + else: + if replacement is not None and getattr(replacement, "index", None) is not None and params.get(_DOWNLOAD_INDEX_KEY) is None: + params.put(_DOWNLOAD_INDEX_KEY, int(replacement.index)) + cloudlog.warning(f"Queued tinygrad migration for retired bundle {getattr(active_bundle, 'internalName', '')}") + runner_type = _TINYGRAD_RUNNER + + if cached_runner_type != runner_type: + params.put(_RUNNER_CACHE_KEY, int(runner_type)) + + return runner_type diff --git a/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/iqmodeld/models/inference_state.py b/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/iqmodeld/models/inference_state.py new file mode 100644 index 000000000..1bc1a254a --- /dev/null +++ b/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/iqmodeld/models/inference_state.py @@ -0,0 +1,9 @@ +""" +Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos +""" +from iqpilot.common.steer_delay import cached_steer_delay + + +class InferenceStateBase: + def __init__(self): + self.lat_delay = cached_steer_delay() diff --git a/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/iqmodeld/models/runners/__init__.py b/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/iqmodeld/models/runners/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/iqmodeld/models/runners/model_runner.py b/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/iqmodeld/models/runners/model_runner.py new file mode 100644 index 000000000..8ede5f6c4 --- /dev/null +++ b/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/iqmodeld/models/runners/model_runner.py @@ -0,0 +1,231 @@ +""" +Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos +""" +import os +import pickle as _pk +from dataclasses import dataclass, field +from typing import TYPE_CHECKING, Any + +import numpy as np +from iqpilot.cereal import custom +from iqpilot.common.swaglog import cloudlog +from iqpilot.system.hardware import TICI +from iqpilot.system.hardware.hw import Paths as _hw_paths +from iqpilot.selfdrive.iqmodeld.models.helpers import get_active_bundle as _fetch_bundle +from iqpilot.selfdrive.iqmodeld.models.combined_artifact import has_combined_split_artifact + +# ---- runtime type surface (native OpenCL/frame handles resolve to Any off-device) ---- +if TYPE_CHECKING: + from iqpilot.selfdrive.iqmodeld.native.iqmodel_pyx import GpuMemorySlot, RoadProjector +else: + def _resolve_native_types() -> tuple[Any, Any]: + try: + from iqpilot.selfdrive.iqmodeld.native.iqmodel_pyx import GpuMemorySlot as iq_clmem + from iqpilot.selfdrive.iqmodeld.native.iqmodel_pyx import RoadProjector as iq_frame + return iq_clmem, iq_frame + except (ModuleNotFoundError, ImportError): + return Any, Any + + GpuMemorySlot, RoadProjector = _resolve_native_types() + +NumpyDict = dict[str, np.ndarray] +ShapeDict = dict[str, tuple[int, ...]] +SliceDict = dict[str, slice] +CLMemDict = dict[str, GpuMemorySlot] +FrameDict = dict[str, RoadProjector] + +ModelType = custom.IQModelManager.Model.Type +Model = custom.IQModelManager.Model + +SEND_RAW_PRED = os.getenv("SEND_RAW_PRED") +CUSTOM_MODEL_PATH = _hw_paths.model_root() + +_META_FIELDS = ("input_shapes", "output_slices") + +USBGPU = "USBGPU" in os.environ + + +def _configure_accelerator(): + """Point tinygrad at the right backend. Must run before tinygrad is imported, + which is why it fires at module import.""" + backend, extra = ("QCOM" if TICI else "CPU"), {} + if USBGPU: + backend, extra = "AMD", {"AMD_IFACE": "USB"} + elif TICI: + extra = {"QCOM_PRIORITY": "8"} + os.environ["DEV"] = backend + os.environ.update(extra) + + +_configure_accelerator() + + +# real metadata pkls are a few KB; anything bigger is a model artifact wrongly +# referenced as metadata (pre-fix manifests self-referenced the artifact), and +# unpickling it here double-loads the model onto the GPU +_META_MAX_BYTES = 1 << 20 + + +def load_artifact_metadata(metadata_filename): + """Read one artifact's metadata pkl: (input shapes, output slices).""" + try: + path = os.path.join(CUSTOM_MODEL_PATH, metadata_filename) + if os.path.getsize(path) > _META_MAX_BYTES: + cloudlog.error(f"metadata pkl {metadata_filename} is artifact-sized, refusing to unpickle it") + return tuple({} for _ in _META_FIELDS) + with open(path, 'rb') as fh: + blob = _pk.load(fh) + return tuple(blob.get(field, {}) for field in _META_FIELDS) + except Exception: + cloudlog.exception(f"unreadable metadata pkl {metadata_filename}, continuing without it") + return tuple({} for _ in _META_FIELDS) + + +@dataclass +class ArtifactSpec: + """One model of the active bundle plus its unpacked metadata.""" + model: Any + metadata: Any = None + input_shapes: ShapeDict = field(default_factory=dict) + output_slices: SliceDict = field(default_factory=dict) + + def __post_init__(self): + self.metadata = self.model.metadata + if self.metadata: + self.input_shapes, self.output_slices = load_artifact_metadata(self.metadata.fileName) + + +# kept name: some runners annotate against the old alias +ModelData = ArtifactSpec + + +class RunnerRoot: + """Shared root of the runner hierarchy. + + Both ModelRunner and the per-model parser mixins (model_types.py) inherit + this, so the concrete `TinygradRunner(ModelRunner, *Tinygrad)` diamond keeps + one consistent parser registry + slice implementation. + """ + + parser_method_dict: dict + _model_data: "ArtifactSpec | None" + + def _slice_outputs(self, model_outputs): + raise NotImplementedError + + +class ModelRunner(RunnerRoot): + """Base for the tinygrad/ONNX runners. + + Owns the active bundle's ArtifactSpecs and the shared slice/parse plumbing; + subclasses provide input staging (prepare_inputs) and execution (_run_model). + """ + + # False for fused runners, which warp + manage temporal buffers inside the JIT + uses_opencl_warp = True + + def __init__(self): + active = _fetch_bundle() + if not active: + raise ValueError("runner started without an active model bundle") + + self.models = {spec.type.raw: ArtifactSpec(spec) for spec in _qcom_models(active)} + self.is_20hz_3d = False + self.is_20hz = active.is20hz + self.inputs = {} + self.parser_method_dict = {} + self._model_data = None # active spec for the current operation + self._parser = self._constants = None + + def _active_spec(self): + spec = self._model_data + if spec is None: + raise ValueError("Model data is not available. Ensure the model is loaded correctly.") + return spec + + # views proxied straight off the active artifact spec; kept out of the class + # body (served via __getattr__) so the read surface stays data-driven + _SPEC_VIEW = frozenset(("input_shapes", "output_slices")) + + def __getattr__(self, name): + if name == "constants": + return self._constants + if name == "vision_input_names": + return list(self._active_spec().input_shapes) + if name in ModelRunner._SPEC_VIEW: + return getattr(self._active_spec(), name) + raise AttributeError(name) + + def prepare_inputs(self, imgs_cl, numpy_inputs, frames): + """Stage image + numpy inputs for inference; implemented per backend.""" + raise NotImplementedError + + def _run_model(self): + """Execute inference over the staged inputs; implemented per backend.""" + raise NotImplementedError + + def run_model(self): + # parsing happens inside each backend's _run_model + return self._run_model() + + def _slice_outputs(self, model_outputs): + """Split the flat output vector into named views per the artifact's slice table.""" + sliced = {} + for tag, span in self._active_spec().output_slices.items(): + sliced[tag] = model_outputs[np.newaxis, span] + if SEND_RAW_PRED: + sliced["raw_pred"] = model_outputs.copy() + return sliced + + +# ---- runner selection (which backend to build for the active bundle) ---------- + +def _qcom_models(bundle) -> list: + # usbeMac artifacts ride along in a bundle for the eGPU host; they are never + # loaded on QCOM and must not affect runner classification + return [m for m in bundle.models if m.type.raw != ModelType.usbeMac] + + +def _single_artifact_prefix(bundle, prefix: str) -> bool: + models = _qcom_models(bundle) + return len(models) == 1 and models[0].artifact.fileName.startswith(prefix) + + +def _is_fused_bundle(bundle) -> bool: + return _single_artifact_prefix(bundle, "driving_fused_") + + +def _is_supercombo_bundle(bundle) -> bool: + return _single_artifact_prefix(bundle, "driving_supercombo_") + + +def _is_split_bundle(bundle) -> bool: + present = {m.type.raw for m in _qcom_models(bundle)} + split_kinds = {ModelType.vision, ModelType.policy, ModelType.offPolicy, ModelType.onPolicy} + return not present.isdisjoint(split_kinds) + + +def get_model_runner() -> "ModelRunner": + """Build the runner backend that fits the active bundle (supercombo / fused / + combined-split / split / single). Concrete runners are imported lazily so one + backend failing to load can't take down the others at import time.""" + from iqpilot.selfdrive.iqmodeld.models.runners.tinygrad.tinygrad_runner import (TinygradRunner, + TinygradSplitRunner) + bundle = _fetch_bundle() + # an eMac-only bundle (no QCOM-loadable models) runs the stock default on + # device; the big host serves the bundle's precompiled artifact + if not (bundle and bundle.models and _qcom_models(bundle)): + return TinygradRunner(ModelType.supercombo) + + if _is_supercombo_bundle(bundle): + from iqpilot.selfdrive.iqmodeld.models.runners.tinygrad.supercombo_runner import TinygradSupercomboRunner + return TinygradSupercomboRunner() + if _is_fused_bundle(bundle): + from iqpilot.selfdrive.iqmodeld.models.runners.tinygrad.fused_runner import TinygradFusedRunner + return TinygradFusedRunner() + if _is_split_bundle(bundle) and has_combined_split_artifact(bundle): + from iqpilot.selfdrive.iqmodeld.models.runners.tinygrad.combined_split_runner import TinygradCombinedSplitRunner + return TinygradCombinedSplitRunner() + if _is_split_bundle(bundle): + return TinygradSplitRunner() + return TinygradRunner(_qcom_models(bundle)[0].type.raw) diff --git a/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/iqmodeld/models/runners/tinygrad/__init__.py b/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/iqmodeld/models/runners/tinygrad/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/iqmodeld/models/runners/tinygrad/combined_split_runner.py b/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/iqmodeld/models/runners/tinygrad/combined_split_runner.py new file mode 100644 index 000000000..71a99cd13 --- /dev/null +++ b/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/iqmodeld/models/runners/tinygrad/combined_split_runner.py @@ -0,0 +1,245 @@ +""" +Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos/ +""" + +from __future__ import annotations + +import os +import pickle +from pathlib import Path +from typing import Any + +import numpy as np + +from iqpilot.selfdrive.iqmodeld.models.combined_artifact import resolve_combined_split_artifact +from iqpilot.selfdrive.iqmodeld.models.helpers import get_active_bundle +from iqpilot.selfdrive.iqmodeld.models.runners.model_runner import NumpyDict, ShapeDict, SliceDict +from iqpilot.selfdrive.iqmodeld.models.runners.model_runner import ModelRunner +from iqpilot.selfdrive.iqmodeld.models.split_model_constants import SplitModelConstants +from iqpilot.selfdrive.iqmodeld.parser import PhaseParser + + +def _tinygrad_imports(): + from tinygrad.device import Device + from tinygrad.tensor import Tensor + return Tensor, Device + + +def _phase_roles(meta_by_role: dict[str, dict]) -> list[str]: + return [name for name in meta_by_role if name != "vision"] + + +def _phase_desire_key(policy_shapes: dict[str, tuple[int, ...]]) -> str: + for key in policy_shapes: + if key.startswith("desire"): + return key + raise KeyError("No desire-like key found in policy inputs") + + +def _phase_image_keys(vision_shapes: dict[str, tuple[int, ...]]) -> tuple[str, str]: + names = sorted(name for name in vision_shapes if "img" in name) + road_key = next((name for name in names if "big" not in name), None) + wide_key = next((name for name in names if "big" in name), None) + if road_key is None or wide_key is None: + raise ValueError(f"Unable to resolve road/wide image keys from {list(vision_shapes)}") + return road_key, wide_key + + +def _base_policy_keys(policy_shapes: dict[str, tuple[int, ...]]) -> set[str]: + desired_key = _phase_desire_key(policy_shapes) + return {desired_key, "features_buffer", "traffic_convention", "action_t"} + + +def _slice_map(raw_blob: np.ndarray, slices: dict[str, slice]) -> NumpyDict: + return {name: raw_blob[np.newaxis, section] for name, section in slices.items() if name != "pad"} + + +class TinygradCombinedSplitRunner(ModelRunner): + uses_opencl_warp: bool = False + + def __init__(self): + super().__init__() + self._constants = SplitModelConstants + self._parser = PhaseParser() + self._bundle = get_active_bundle() + self._artifact_path = resolve_combined_split_artifact(self._bundle) + if self._artifact_path is None: + raise FileNotFoundError("No IQ combined split artifact is available for the active bundle") + + with open(self._artifact_path, "rb") as artifact: + runtime_package: dict[Any, Any] = pickle.load(artifact) + + self._meta_by_role = runtime_package.get("meta_by_role", runtime_package.get("metadata", {})) + self._policy_roles = runtime_package.get("roles", _phase_roles(self._meta_by_role)) + self._camera_programs = { + camera_key: spec + for camera_key, spec in runtime_package.items() + if isinstance(camera_key, tuple) and isinstance(spec, dict) + } + self._execute_bundle = runtime_package.get("execute_bundle", runtime_package.get("run_policy")) + self._frame_stride = int(runtime_package.get("frame_stride", runtime_package.get("frame_skip", 1))) + + if "vision" not in self._meta_by_role: + raise ValueError("Combined split artifact is missing vision metadata") + if not self._policy_roles: + raise ValueError("Combined split artifact is missing policy roles") + if self._execute_bundle is None: + raise ValueError("Combined split artifact is missing execute_bundle") + + self._vision_meta = self._meta_by_role["vision"] + self._primary_policy_meta = self._meta_by_role[self._policy_roles[0]] + self._desired_key = _phase_desire_key(self._primary_policy_meta["input_shapes"]) + self._road_key, self._wide_key = _phase_image_keys(self._vision_meta["input_shapes"]) + self._extra_policy_keys = [ + key for key in self._primary_policy_meta["input_shapes"] + if key not in _base_policy_keys(self._primary_policy_meta["input_shapes"]) + ] + + self._queue_tensors: dict[str, Any] | None = None + self._numpy_state: dict[str, np.ndarray] | None = None + self._camera_shape: tuple[int, int] | None = None + self._blob_cache: dict[tuple[str, int], Any] = {} + self._last_desire = np.zeros(self._primary_policy_meta["input_shapes"][self._desired_key][2], dtype=np.float32) + + @property + def vision_input_names(self) -> list[str]: + return [self._road_key, self._wide_key] + + @property + def input_shapes(self) -> ShapeDict: + merged: ShapeDict = dict(self._vision_meta["input_shapes"]) + for role in self._policy_roles: + merged.update(self._meta_by_role[role]["input_shapes"]) + return merged + + @property + def output_slices(self) -> SliceDict: + merged: SliceDict = dict(self._vision_meta["output_slices"]) + for role in self._policy_roles: + merged.update(self._meta_by_role[role]["output_slices"]) + return merged + + def prepare_inputs(self, imgs_cl, numpy_inputs, frames): + raise RuntimeError("Combined split runner manages its own warp + queue state; use run_fused()") + + def _frame_blob(self, stream_name: str, buf): + Tensor, Device = _tinygrad_imports() + raw_frame = np.frombuffer(buf.data, dtype=np.uint8) + cache_key = (stream_name, raw_frame.ctypes.data) + tensor = self._blob_cache.get(cache_key) + if tensor is None: + tensor = Tensor.from_blob(raw_frame.ctypes.data, (raw_frame.size,), dtype="uint8", device=Device.DEFAULT) + self._blob_cache[cache_key] = tensor + return tensor + + def _allocate_runtime_state(self, camera_width: int, camera_height: int) -> None: + if self._queue_tensors is not None and self._camera_shape == (camera_width, camera_height): + return + if (camera_width, camera_height) not in self._camera_programs: + raise RuntimeError(f"No combined split kernels available for {camera_width}x{camera_height}") + + Tensor, Device = _tinygrad_imports() + vision_shapes = self._vision_meta["input_shapes"] + policy_shapes = self._primary_policy_meta["input_shapes"] + + image_shape = vision_shapes[self._road_key] + frame_history = image_shape[1] // 6 + queue_depth = self._frame_stride * (frame_history - 1) + 1 + frame_queue_shape = (queue_depth, 6, image_shape[2], image_shape[3]) + + feature_shape = policy_shapes["features_buffer"] + desired_shape = policy_shapes[self._desired_key] + traffic_shape = policy_shapes["traffic_convention"] + action_shape = policy_shapes.get("action_t", traffic_shape) + + numpy_state = { + "tfm": np.zeros((3, 3), dtype=np.float32), + "big_tfm": np.zeros((3, 3), dtype=np.float32), + "desire": np.zeros(desired_shape[2], dtype=np.float32), + "traffic_convention": np.zeros(traffic_shape, dtype=np.float32), + "action_t": np.zeros(action_shape, dtype=np.float32), + } + for key in self._extra_policy_keys: + numpy_state[key] = np.zeros(policy_shapes[key], dtype=np.float32) + + queue_tensors = { + "img_q": Tensor(np.zeros(frame_queue_shape, dtype=np.uint8), device=Device.DEFAULT).contiguous().realize(), + "big_img_q": Tensor(np.zeros(frame_queue_shape, dtype=np.uint8), device=Device.DEFAULT).contiguous().realize(), + "feat_q": Tensor( + np.zeros((self._frame_stride * (feature_shape[1] - 1) + 1, feature_shape[0], feature_shape[2]), dtype=np.float32), + device=Device.DEFAULT, + ).contiguous().realize(), + "desire_q": Tensor( + np.zeros((self._frame_stride * desired_shape[1], desired_shape[0], desired_shape[2]), dtype=np.float32), + device=Device.DEFAULT, + ).contiguous().realize(), + **{name: Tensor(value, device="NPY").realize() for name, value in numpy_state.items()}, + } + + self._queue_tensors = queue_tensors + self._numpy_state = numpy_state + self._camera_shape = (camera_width, camera_height) + + def _policy_inputs(self) -> dict[str, Any]: + assert self._queue_tensors is not None + tensor_names = ["feat_q", "desire_q", "desire", "traffic_convention", "action_t", *self._extra_policy_keys] + return {name: self._queue_tensors[name] for name in tensor_names if name in self._queue_tensors} + + def _merge_policy_outputs(self, raw_outputs: tuple[Any, ...]) -> NumpyDict: + outputs = self._parser.parse_vision_outputs( + _slice_map(raw_outputs[0].numpy().flatten(), self._vision_meta["output_slices"]) + ) + + has_on_policy = any(role == "on_policy" for role in self._policy_roles) + for role_name, tensor_out in zip(self._policy_roles, raw_outputs[1:], strict=True): + parsed = self._parser.parse_policy_outputs( + _slice_map(tensor_out.numpy().flatten(), self._meta_by_role[role_name]["output_slices"]) + ) + if role_name == "off_policy" and has_on_policy: + parsed.pop("plan", None) + outputs.update(parsed) + + if "planplus" in outputs and "plan" in outputs: + outputs["plan"] = outputs["plan"] + outputs["planplus"] + return outputs + + def run_fused(self, bufs: dict, transforms: dict[str, np.ndarray], numpy_inputs: NumpyDict) -> NumpyDict: + main_buf = bufs[self._road_key] + self._allocate_runtime_state(main_buf.width, main_buf.height) + assert self._queue_tensors is not None and self._numpy_state is not None and self._camera_shape is not None + + self._numpy_state["tfm"][:] = transforms[self._road_key] + self._numpy_state["big_tfm"][:] = transforms[self._wide_key] + + current_desire = numpy_inputs[self._desired_key].copy() + current_desire[0] = 0 + self._numpy_state["desire"][:] = np.where(current_desire - self._last_desire > 0.99, current_desire, 0) + self._last_desire[:] = current_desire + + if "traffic_convention" in numpy_inputs: + self._numpy_state["traffic_convention"][:] = numpy_inputs["traffic_convention"] + if "action_t" in numpy_inputs: + self._numpy_state["action_t"][:] = numpy_inputs["action_t"] + for key in self._extra_policy_keys: + if key in numpy_inputs: + self._numpy_state[key][:] = numpy_inputs[key] + + stage_inputs = self._camera_programs[self._camera_shape].get("stage_inputs", self._camera_programs[self._camera_shape].get("warp_enqueue")) + if stage_inputs is None: + raise RuntimeError("Combined split artifact camera entry is missing stage_inputs") + + staged_main, staged_wide = stage_inputs( + img_q=self._queue_tensors["img_q"], + big_img_q=self._queue_tensors["big_img_q"], + tfm=self._queue_tensors["tfm"], + big_tfm=self._queue_tensors["big_tfm"], + frame=self._frame_blob(self._road_key, bufs[self._road_key]), + big_frame=self._frame_blob(self._wide_key, bufs[self._wide_key]), + ) + raw_outputs = self._execute_bundle(img=staged_main, big_img=staged_wide, **self._policy_inputs()) + if not isinstance(raw_outputs, tuple): + raw_outputs = (raw_outputs,) + return self._merge_policy_outputs(raw_outputs) + + def _run_model(self) -> NumpyDict: + raise RuntimeError("Combined split runner executes through run_fused()") diff --git a/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/iqmodeld/models/runners/tinygrad/fused_runner.py b/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/iqmodeld/models/runners/tinygrad/fused_runner.py new file mode 100644 index 000000000..33beea423 --- /dev/null +++ b/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/iqmodeld/models/runners/tinygrad/fused_runner.py @@ -0,0 +1,178 @@ +""" +Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos/ +""" +from __future__ import annotations + +import os +import pickle +from typing import Any + +import numpy as np + +from iqpilot.selfdrive.iqmodeld.models.runners.model_runner import ( + CUSTOM_MODEL_PATH, NumpyDict, ShapeDict, SliceDict, +) +from iqpilot.selfdrive.iqmodeld.models.runners.model_runner import ModelRunner +from iqpilot.selfdrive.iqmodeld.models.split_model_constants import SplitModelConstants +from iqpilot.selfdrive.iqmodeld.parser import PhaseParser + + +def _tinygrad_imports(): + from tinygrad.tensor import Tensor + from tinygrad.device import Device + return Tensor, Device + + +WARP_DEV = os.getenv('WARP_DEV') + + +class TinygradFusedRunner(ModelRunner): + uses_opencl_warp: bool = False + + def __init__(self): + super().__init__() + + self._constants = SplitModelConstants + self._parser = PhaseParser() + + if len(self.models) != 1: + raise ValueError(f"fused bundle must have exactly one artifact, got {list(self.models)}") + self._model_data = next(iter(self.models.values())) + + pkl_path = os.path.join(CUSTOM_MODEL_PATH, self._model_data.model.artifact.fileName) + with open(pkl_path, 'rb') as f: + self._fused: dict[Any, Any] = pickle.load(f) + + self._vision_meta = self._fused['metadata']['vision'] + self._on_meta = self._fused['metadata']['on_policy'] + self._off_meta = self._fused['metadata']['off_policy'] + self._run_policy = self._fused['run_policy'] + self._warp_jits: dict[tuple[int, int], Any] = {k: v for k, v in self._fused.items() if isinstance(k, tuple)} + if not self._warp_jits: + raise ValueError("fused pkl has no warp JITs") + + self._frame_skip: int = int(self._fused.get('frame_skip', 4)) + + self._queues: dict[str, Any] | None = None + self._npy_buffers: dict[str, np.ndarray] | None = None + self._cam_resolution: tuple[int, int] | None = None + self._blob_cache: dict[tuple[str, int], Any] = {} + + def _frame_tensor(self, key, buf): + Tensor, Device = _tinygrad_imports() + arr = np.frombuffer(buf.data, dtype=np.uint8) + ck = (key, arr.ctypes.data) + t = self._blob_cache.get(ck) + if t is None: + t = Tensor.from_blob(arr.ctypes.data, (arr.size,), dtype='uint8', device=Device.DEFAULT) + self._blob_cache[ck] = t + return t + + @property + def vision_input_names(self) -> list[str]: + return ['img', 'big_img'] + + @property + def input_shapes(self) -> ShapeDict: + return {**self._vision_meta['input_shapes'], **self._on_meta['input_shapes']} + + @property + def output_slices(self) -> SliceDict: + merged: SliceDict = {} + for src in (self._vision_meta['output_slices'], self._on_meta['output_slices'], self._off_meta['output_slices']): + merged.update({k: v for k, v in src.items() if k != 'pad'}) + return merged + + def prepare_inputs(self, imgs_cl, numpy_inputs, frames): + raise RuntimeError("fused runner has no OpenCL path; use run_fused()") + + def _ensure_queues(self, cam_w: int, cam_h: int) -> None: + if self._queues is not None and self._cam_resolution == (cam_w, cam_h): + return + if (cam_w, cam_h) not in self._warp_jits: + raise RuntimeError(f"no warp JIT for {cam_w}x{cam_h}; have {sorted(self._warp_jits)}") + + Tensor, Device = _tinygrad_imports() + img_shape = self._vision_meta['input_shapes']['img'] + fb = self._on_meta['input_shapes']['features_buffer'] + dp = self._on_meta['input_shapes']['desire_pulse'] + n_frames = img_shape[1] // 6 + img_buf_shape = (self._frame_skip * (n_frames - 1) + 1, 6, img_shape[2], img_shape[3]) + + zeros_u8 = lambda shp: Tensor(np.zeros(shp, dtype=np.uint8), device=Device.DEFAULT).contiguous().realize() + zeros_f32 = lambda shp: Tensor(np.zeros(shp, dtype=np.float32), device=Device.DEFAULT).contiguous().realize() + + self._queues = { + 'img_q': zeros_u8(img_buf_shape), + 'big_img_q': zeros_u8(img_buf_shape), + 'feat_q': zeros_f32((self._frame_skip * (fb[1] - 1) + 1, fb[0], fb[2])), + 'desire_q': zeros_f32((self._frame_skip * dp[1], dp[0], dp[2])), + } + on_shapes = self._on_meta['input_shapes'] + captured = self._run_policy.captured + jit_shapes = { + name: tuple(int(s) for s in view.shape) + for name, (view, _vars, _dtype, _device) in zip(captured.expected_names, captured.expected_input_info) + } + + def policy_input_shape(name): + shape = on_shapes.get(name, jit_shapes.get(name)) + if shape is None: + raise ValueError(f"fused pkl declares no shape for policy input {name}") + return shape + + self._npy_buffers = { + 'desire': np.zeros(dp[2], dtype=np.float32), + 'traffic_convention': np.zeros(policy_input_shape('traffic_convention'), dtype=np.float32), + 'tfm': np.zeros((3, 3), dtype=np.float32), + 'big_tfm': np.zeros((3, 3), dtype=np.float32), + } + if 'action_t' in jit_shapes: + self._npy_buffers['action_t'] = np.zeros(policy_input_shape('action_t'), dtype=np.float32) + self._cam_resolution = (cam_w, cam_h) + + def run_fused(self, bufs: dict, transforms: dict[str, np.ndarray], numpy_inputs: NumpyDict) -> NumpyDict: + Tensor, Device = _tinygrad_imports() + + main_buf = bufs['img'] + self._ensure_queues(main_buf.width, main_buf.height) + assert self._queues is not None and self._npy_buffers is not None + + desire_key = next((k for k in numpy_inputs if k.startswith('desire')), None) + if desire_key is not None: + self._npy_buffers['desire'][:] = numpy_inputs[desire_key] + if 'traffic_convention' in numpy_inputs: + self._npy_buffers['traffic_convention'][:] = numpy_inputs['traffic_convention'] + if 'action_t' in numpy_inputs and 'action_t' in self._npy_buffers: + self._npy_buffers['action_t'][:] = numpy_inputs['action_t'] + self._npy_buffers['tfm'][:] = transforms['img'] + self._npy_buffers['big_tfm'][:] = transforms['big_img'] + + npy = lambda key: Tensor(self._npy_buffers[key], device='NPY') + + frame = self._frame_tensor('img', bufs['img']) + big_frame = self._frame_tensor('big_img', bufs['big_img']) + + warp_jit = self._warp_jits[self._cam_resolution] + img, big_img = warp_jit(img_q=self._queues['img_q'], big_img_q=self._queues['big_img_q'], + tfm=npy('tfm'), big_tfm=npy('big_tfm'), frame=frame, big_frame=big_frame) + + policy_inputs = dict( + img=img, big_img=big_img, feat_q=self._queues['feat_q'], desire_q=self._queues['desire_q'], + desire=npy('desire'), traffic_convention=npy('traffic_convention')) + if 'action_t' in self._npy_buffers: + policy_inputs['action_t'] = npy('action_t') + vision_out_t, on_out_t, off_out_t = self._run_policy(**policy_inputs) + + def _slice(tensor_out, meta) -> NumpyDict: + flat = tensor_out.numpy().flatten() + return {k: flat[np.newaxis, sl] for k, sl in meta['output_slices'].items() if k != 'pad'} + + parsed: NumpyDict = {} + parsed.update(self._parser.parse_vision_outputs(_slice(vision_out_t, self._vision_meta))) + parsed.update(self._parser.parse_policy_outputs(_slice(off_out_t, self._off_meta))) + parsed.update(self._parser.parse_policy_outputs(_slice(on_out_t, self._on_meta))) + return parsed + + def _run_model(self) -> NumpyDict: + raise RuntimeError("fused path goes through run_fused(), not _run_model()") diff --git a/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/iqmodeld/models/runners/tinygrad/model_types.py b/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/iqmodeld/models/runners/tinygrad/model_types.py new file mode 100644 index 000000000..a2f5ec5b6 --- /dev/null +++ b/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/iqmodeld/models/runners/tinygrad/model_types.py @@ -0,0 +1,60 @@ +""" +Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos/ +""" + +from __future__ import annotations + +from abc import ABC +from collections.abc import Callable + +import numpy as np + +from iqpilot.selfdrive.iqmodeld.models.runners.model_runner import ModelType, NumpyDict +from iqpilot.selfdrive.iqmodeld.models.runners.model_runner import RunnerRoot +from iqpilot.selfdrive.iqmodeld.parser import ArchiveParser, PhaseParser + + +class _ParserRole(RunnerRoot, ABC): + def _bind_parser_role(self, + selector: int, + parser_builder: Callable[[], object], + projector: Callable[[object, NumpyDict], NumpyDict]) -> None: + parser = parser_builder() + self.parser_method_dict[selector] = lambda model_blob: projector(parser, self._slice_outputs(model_blob)) + + +def _phase_policy(parser: PhaseParser, sliced_outputs: NumpyDict) -> NumpyDict: + return parser.parse_policy_outputs(sliced_outputs) + + +def _phase_vision(parser: PhaseParser, sliced_outputs: NumpyDict) -> NumpyDict: + return parser.parse_vision_outputs(sliced_outputs) + + +def _archive_combined(parser: ArchiveParser, sliced_outputs: NumpyDict) -> NumpyDict: + return parser.parse_outputs(sliced_outputs) + + +class OffPolicyTinygrad(_ParserRole, ABC): + def __init__(self): + self._bind_parser_role(ModelType.offPolicy, PhaseParser, _phase_policy) + + +class OnPolicyTinygrad(_ParserRole, ABC): + def __init__(self): + self._bind_parser_role(ModelType.onPolicy, PhaseParser, _phase_policy) + + +class PolicyTinygrad(_ParserRole, ABC): + def __init__(self): + self._bind_parser_role(ModelType.policy, PhaseParser, _phase_policy) + + +class VisionTinygrad(_ParserRole, ABC): + def __init__(self): + self._bind_parser_role(ModelType.vision, PhaseParser, _phase_vision) + + +class SupercomboTinygrad(_ParserRole, ABC): + def __init__(self): + self._bind_parser_role(ModelType.supercombo, ArchiveParser, _archive_combined) diff --git a/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/iqmodeld/models/runners/tinygrad/supercombo_runner.py b/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/iqmodeld/models/runners/tinygrad/supercombo_runner.py new file mode 100644 index 000000000..ccd4a5b1e --- /dev/null +++ b/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/iqmodeld/models/runners/tinygrad/supercombo_runner.py @@ -0,0 +1,339 @@ +""" +Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos/ +""" +from __future__ import annotations + +import hashlib +import math +import os +import pickle +import re +from typing import Any + +import numpy as np + +from iqpilot.common.params import Params +from iqpilot.selfdrive.iqmodeld.models.runners.model_runner import CUSTOM_MODEL_PATH, NumpyDict, ShapeDict, SliceDict +from iqpilot.selfdrive.iqmodeld.models.runners.model_runner import ModelRunner +from iqpilot.selfdrive.iqmodeld.models.split_model_constants import SplitModelConstants +from iqpilot.selfdrive.iqmodeld.parser import PhaseParser + + +def _tinygrad_imports(): + from tinygrad.tensor import Tensor + from tinygrad.device import Device + return Tensor, Device + + +def _captured_queue_depth(warp_jit: Any) -> int | None: + captured = getattr(warp_jit, "captured", None) + infos = getattr(captured, "expected_input_info", None) + if not infos or len(infos) < 2: + return None + + view_repr = repr(infos[1][0]) + dims = [int(val) for val in re.findall(r"arg=(\d+)", view_repr)] + return dims[0] if len(dims) >= 4 else None + + +def _captured_devices(warp_jit: Any) -> set[str]: + captured = getattr(warp_jit, "captured", None) + infos = getattr(captured, "expected_input_info", None) + if not infos: + return set() + + devices: set[str] = set() + for info in infos: + if isinstance(info, tuple) and len(info) >= 4 and isinstance(info[3], str): + devices.add(info[3]) + return devices + + +def _captured_expected_names(jit_obj: Any) -> list[str]: + captured = getattr(jit_obj, "captured", None) + names = getattr(captured, "expected_names", None) + return list(names) if names else [] + + +def _file_sha256(path: str) -> str: + digest = hashlib.sha256() + with open(path, "rb") as f: + for chunk in iter(lambda: f.read(1024 * 1024), b""): + digest.update(chunk) + return digest.hexdigest() + + +def _is_jit_arg_mismatch(err: BaseException) -> bool: + return "args mismatch in JIT" in str(err) + + +class TinygradSupercomboRunner(ModelRunner): + uses_opencl_warp: bool = False + + def __init__(self): + super().__init__() + self._constants = SplitModelConstants + self._parser = PhaseParser() + + if len(self.models) != 1: + raise ValueError(f"supercombo bundle must have exactly one artifact, got {list(self.models)}") + self._model_data = next(iter(self.models.values())) + + pkl_path = os.path.join(CUSTOM_MODEL_PATH, self._model_data.model.artifact.fileName) + self._pkl_path = pkl_path + self._expected_sha256 = getattr(getattr(self._model_data.model.artifact, "downloadUri", None), "sha256", "") or "" + self._verify_artifact_file() + with open(pkl_path, 'rb') as f: + self._m: dict[Any, Any] = pickle.load(f) + + self._meta = self._m['metadata'] + self._ish = self._meta['input_shapes'] + self._slices = {k: v for k, v in self._meta['output_slices'].items() if k != 'pad'} + self._hidden_slice = self._meta['output_slices']['hidden_state'] + self._run_policy = self._m['run_policy'] + self._warp_jits: dict[tuple[int, int], Any] = {k: v for k, v in self._m.items() if isinstance(k, tuple)} + if not self._warp_jits: + raise ValueError("supercombo pkl has no warp JITs") + self._frame_skip = int(self._m.get('frame_skip', 4)) + self._validate_warp_jits(pkl_path) + self._validate_jit_names() + + self._queues: dict[str, Any] | None = None + self._npy: dict[str, np.ndarray] | None = None + self._cam: tuple[int, int] | None = None + self._prev_desire = np.zeros(self._ish['desire_pulse'][2], dtype=np.float32) + self._blob_cache: dict[tuple[str, int], Any] = {} + + def _verify_artifact_file(self) -> None: + if not self._expected_sha256: + return + + actual_sha256 = _file_sha256(self._pkl_path) + if actual_sha256 == self._expected_sha256: + return + + try: + os.remove(self._pkl_path) + except OSError: + pass + redownload_msg = self._schedule_active_bundle_redownload() + + raise RuntimeError( + "supercombo artifact SHA mismatch: " + f"expected {self._expected_sha256}, got {actual_sha256} for {self._pkl_path}. " + f"Deleted the stale cached file{redownload_msg}." + ) + + def _validate_warp_jits(self, pkl_path: str) -> None: + img = self._ish['img'] + n_frames = img[1] // 6 + expected_depth = self._frame_skip * (n_frames - 1) + 1 + expected_device = os.getenv('DEV') + + mismatches: list[str] = [] + for cam, warp_jit in sorted(self._warp_jits.items()): + captured_depth = _captured_queue_depth(warp_jit) + captured_devices = _captured_devices(warp_jit) + if captured_depth is not None and captured_depth != expected_depth: + mismatches.append( + f"{cam[0]}x{cam[1]} queue-depth captured={captured_depth} expected={expected_depth}" + ) + if expected_device and captured_devices and expected_device not in captured_devices: + mismatches.append( + f"{cam[0]}x{cam[1]} device captured={sorted(captured_devices)} expected={expected_device}" + ) + + if mismatches: + details = "; ".join(mismatches) + raise RuntimeError( + "supercombo warp JIT compatibility mismatch: " + f"{details}. Bundle {pkl_path} was compiled with the wrong backend, frame_skip, or queue shape; " + "re-download or rebuild this model artifact." + ) + + def _validate_jit_names(self) -> None: + expected_warp_names = ['big_frame', 'big_tfm', 'frame', 'tfm'] + expected_policy_names = ['big_img_q', 'desire_q', 'feat_q', 'img_q', 'packed_npy_inputs', 'warped'] + + mismatches: list[str] = [] + + policy_names = sorted(_captured_expected_names(self._run_policy)) + if policy_names and policy_names != expected_policy_names: + mismatches.append(f"run_policy captured={policy_names} expected={expected_policy_names}") + + for cam, warp_jit in sorted(self._warp_jits.items()): + warp_names = sorted(_captured_expected_names(warp_jit)) + if warp_names and warp_names != expected_warp_names: + mismatches.append(f"{cam[0]}x{cam[1]} warp captured={warp_names} expected={expected_warp_names}") + + if mismatches: + details = "; ".join(mismatches) + actual_sha = None + try: + actual_sha = _file_sha256(self._pkl_path) + except OSError: + pass + + if actual_sha and self._expected_sha256 and actual_sha != self._expected_sha256: + try: + os.remove(self._pkl_path) + except OSError: + pass + redownload_msg = self._schedule_active_bundle_redownload() + raise RuntimeError( + "supercombo artifact contract mismatch with stale cached SHA: " + f"{details}. Expected SHA {self._expected_sha256}, got {actual_sha}. " + f"Deleted the stale cached file{redownload_msg}." + ) + + raise RuntimeError( + "supercombo artifact JIT argument mismatch: " + f"{details}. This model file does not match the current IQPilot runtime contract. " + "Re-download or rebuild this model artifact." + ) + + def _handle_runtime_jit_mismatch(self, err: BaseException) -> None: + if not _is_jit_arg_mismatch(err): + raise err + + actual_sha = None + try: + actual_sha = _file_sha256(self._pkl_path) + except OSError: + pass + + if actual_sha and self._expected_sha256 and actual_sha != self._expected_sha256: + try: + os.remove(self._pkl_path) + except OSError: + pass + redownload_msg = self._schedule_active_bundle_redownload() + raise RuntimeError( + "supercombo artifact runtime JIT mismatch with stale cached SHA: " + f"expected {self._expected_sha256}, got {actual_sha} for {self._pkl_path}. " + f"Deleted the stale cached file{redownload_msg}." + ) from err + + raise RuntimeError( + "supercombo artifact runtime JIT mismatch: " + f"{err}. This model file does not match the current IQPilot runtime contract. " + "Re-download or rebuild this model artifact." + ) from err + + def _schedule_active_bundle_redownload(self) -> str: + try: + params = Params() + active_bundle = params.get("ModelManager_ActiveBundle") or {} + index = active_bundle.get("index") if isinstance(active_bundle, dict) else None + if isinstance(index, str) and index.isdigit(): + index = int(index) + if isinstance(index, int) and index >= 0: + params.put("ModelManager_DownloadIndex", str(index)) + params.remove("ModelRunnerTypeCache") + return "; scheduled automatic re-download of the active model" + except Exception: + pass + + return "; unable to schedule automatic re-download" + + def _frame_tensor(self, key: str, buf): + Tensor, Device = _tinygrad_imports() + arr = np.frombuffer(buf.data, dtype=np.uint8) + ck = (key, arr.ctypes.data) + t = self._blob_cache.get(ck) + if t is None: + t = Tensor.from_blob(arr.ctypes.data, (arr.size,), dtype='uint8', device=Device.DEFAULT) + self._blob_cache[ck] = t + return t + + @property + def vision_input_names(self) -> list[str]: + return ['img', 'big_img'] + + @property + def input_shapes(self) -> ShapeDict: + return dict(self._ish) + + @property + def output_slices(self) -> SliceDict: + return dict(self._slices) + + def prepare_inputs(self, imgs_cl, numpy_inputs, frames): + raise RuntimeError("supercombo runner has no OpenCL path; use run_fused()") + + def _ensure_queues(self, cam_w: int, cam_h: int) -> None: + if self._queues is not None and self._cam == (cam_w, cam_h): + return + if (cam_w, cam_h) not in self._warp_jits: + raise RuntimeError(f"no warp JIT for {cam_w}x{cam_h}; have {sorted(self._warp_jits)}") + + Tensor, Device = _tinygrad_imports() + fs = self._frame_skip + img = self._ish['img'] + n_frames = img[1] // 6 + img_buf = (fs * (n_frames - 1) + 1, 6, img[2], img[3]) + fb = self._ish['features_buffer'] + dp = self._ish['desire_pulse'] + tc = self._ish['traffic_convention'] + at = self._ish['action_t'] + + zeros_u8 = lambda s: Tensor(np.zeros(s, dtype=np.uint8), device=Device.DEFAULT).contiguous().realize() + zeros_f32 = lambda s: Tensor(np.zeros(s, dtype=np.float32), device=Device.DEFAULT).contiguous().realize() + + shapes = {'desire': (dp[2],), 'traffic_convention': tuple(tc), 'action_t': tuple(at), 'prev_feat': (fb[0], fb[2])} + sizes = [math.prod(s) for s in shapes.values()] + packed = np.zeros(sum(sizes), dtype=np.float32) + views = {k: v.reshape(s) for (k, s), v in zip(shapes.items(), np.split(packed, np.cumsum(sizes[:-1])), strict=True)} + + self._npy = {'tfm': np.zeros((3, 3), dtype=np.float32), 'big_tfm': np.zeros((3, 3), dtype=np.float32), **views} + self._queues = { + 'img_q': zeros_u8(img_buf), + 'big_img_q': zeros_u8(img_buf), + 'feat_q': zeros_f32((fs * fb[1], fb[0], fb[2])), + 'desire_q': zeros_f32((fs * dp[1], dp[0], dp[2])), + 'tfm': Tensor(self._npy['tfm'], device='NPY'), + 'big_tfm': Tensor(self._npy['big_tfm'], device='NPY'), + 'packed_npy_inputs': Tensor(packed, device='NPY'), + } + self._cam = (cam_w, cam_h) + + def run_fused(self, bufs: dict, transforms: dict[str, np.ndarray], numpy_inputs: NumpyDict) -> NumpyDict: + Tensor, Device = _tinygrad_imports() + main_buf = bufs['img'] + self._ensure_queues(main_buf.width, main_buf.height) + assert self._queues is not None and self._npy is not None + + self._npy['tfm'][:] = transforms['img'] + self._npy['big_tfm'][:] = transforms['big_img'] + + desire_key = next((k for k in numpy_inputs if k.startswith('desire')), None) + cur = numpy_inputs[desire_key].copy() if desire_key is not None else np.zeros_like(self._prev_desire) + cur[0] = 0 + self._npy['desire'][:] = np.where(cur - self._prev_desire > .99, cur, 0) + self._prev_desire[:] = cur + if 'traffic_convention' in numpy_inputs: + self._npy['traffic_convention'][:] = numpy_inputs['traffic_convention'] + if 'action_t' in numpy_inputs: + self._npy['action_t'][:] = numpy_inputs['action_t'] + + frame = self._frame_tensor('img', bufs['img']) + big_frame = self._frame_tensor('big_img', bufs['big_img']) + + warp = self._warp_jits[self._cam] + try: + warped = warp(tfm=self._queues['tfm'], big_tfm=self._queues['big_tfm'], frame=frame, big_frame=big_frame) + out, = self._run_policy(warped=warped, img_q=self._queues['img_q'], big_img_q=self._queues['big_img_q'], + feat_q=self._queues['feat_q'], desire_q=self._queues['desire_q'], + packed_npy_inputs=self._queues['packed_npy_inputs']) + except Exception as err: + self._handle_runtime_jit_mismatch(err) + raise + flat = out.numpy().flatten() + + self._npy['prev_feat'][:] = flat[self._hidden_slice].reshape(self._npy['prev_feat'].shape) + + sliced = {k: flat[np.newaxis, sl] for k, sl in self._slices.items()} + return self._parser.parse_vision_outputs(sliced) + + def _run_model(self) -> NumpyDict: + raise RuntimeError("supercombo path goes through run_fused(), not _run_model()") diff --git a/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/iqmodeld/models/runners/tinygrad/tinygrad_runner.py b/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/iqmodeld/models/runners/tinygrad/tinygrad_runner.py new file mode 100644 index 000000000..2d91c3df1 --- /dev/null +++ b/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/iqmodeld/models/runners/tinygrad/tinygrad_runner.py @@ -0,0 +1,190 @@ +""" +Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos/ +""" + +from __future__ import annotations + +import pickle +from dataclasses import dataclass + +import numpy as np +from tinygrad.dtype import dtypes +from tinygrad.tensor import Tensor + +from iqpilot.selfdrive.iqmodeld.models.runners.model_runner import ( + CLMemDict, + CUSTOM_MODEL_PATH, + FrameDict, + ModelType, + NumpyDict, + ShapeDict, + SliceDict, +) +from iqpilot.selfdrive.iqmodeld.models.runners.model_runner import ModelRunner +from iqpilot.selfdrive.iqmodeld.models.runners.tinygrad.model_types import ( + OffPolicyTinygrad, + OnPolicyTinygrad, + PolicyTinygrad, + SupercomboTinygrad, + VisionTinygrad, +) +from iqpilot.selfdrive.iqmodeld.models.split_model_constants import SplitModelConstants +from iqpilot.selfdrive.iqmodeld.config import ModelConstants +from iqpilot.selfdrive.iqmodeld.runtime.tinygrad import qcom_tensor_from_opencl_address +from iqpilot.system.hardware import TICI + + +@dataclass(frozen=True) +class _TensorShapePlan: + dtype: object + device: str + + +def _artifact_path(filename: str) -> str: + return f"{CUSTOM_MODEL_PATH}/{filename}" + + +def _load_program_blob(filename: str): + with open(_artifact_path(filename), "rb") as artifact: + try: + return pickle.load(artifact) + except FileNotFoundError as exc: + assert "/dev/kgsl-3d0" not in str(exc), "Model was built on C3 or C3X, but is being loaded on PC" + raise + + +def _compile_input_plan(captured) -> dict[str, _TensorShapePlan]: + plan: dict[str, _TensorShapePlan] = {} + for name, info in zip(captured.expected_names, captured.expected_input_info, strict=True): + plan[name] = _TensorShapePlan(dtype=info[2], device=info[3]) + return plan + + +def _merge_step_outputs(output_groups: list[NumpyDict]) -> NumpyDict: + stitched: NumpyDict = {} + for payload in output_groups: + stitched.update(payload) + if "planplus" in stitched and "plan" in stitched: + stitched["plan"] = stitched["plan"] + stitched["planplus"] + return stitched + + +class TinygradRunner(ModelRunner, SupercomboTinygrad, PolicyTinygrad, VisionTinygrad, OffPolicyTinygrad, OnPolicyTinygrad): + def __init__(self, model_type: int = ModelType.supercombo): + ModelRunner.__init__(self) + for initializer in (SupercomboTinygrad, PolicyTinygrad, VisionTinygrad, OffPolicyTinygrad, OnPolicyTinygrad): + initializer.__init__(self) + + self._constants = ModelConstants + self._model_data = self.models.get(model_type) + if self._model_data is None or self._model_data.model is None: + raise ValueError(f"Model data for type {model_type} not available.") + + asset_name = self._model_data.model.artifact.fileName + assert asset_name.endswith("_tinygrad.pkl"), f"Invalid model file {asset_name} for TinygradRunner" + + self.model_run = _load_program_blob(asset_name) + self._input_plan = _compile_input_plan(self.model_run.captured) + for name, spec in self._input_plan.items(): + if "img" in name and spec.dtype is not dtypes.uint8: + raise ValueError(f"{asset_name}: image input {name} expects {spec.dtype}, incompatible with uint8 warp buffer") + self.input_to_dtype = {name: spec.dtype for name, spec in self._input_plan.items()} + self.input_to_device = {name: spec.device for name, spec in self._input_plan.items()} + + @property + def vision_input_names(self) -> list[str]: + return [stream_name for stream_name in self.input_shapes if "img" in stream_name] + + def _attach_vision_tensor(self, stream_name: str, frame_buffers: CLMemDict, frame_views: FrameDict) -> None: + spec = self._input_plan[stream_name] + frame_buffer = frame_buffers[stream_name] + if TICI: + self.inputs[stream_name] = qcom_tensor_from_opencl_address(frame_buffer.mem_address, + self.input_shapes[stream_name], + dtype=spec.dtype) + return + + mirrored = frame_views[stream_name].as_numpy(frame_buffer).reshape(self.input_shapes[stream_name]) + self.inputs[stream_name] = Tensor(mirrored, device=spec.device, dtype=spec.dtype).realize() + + def _attach_state_tensor(self, tensor_name: str, tensor_value: np.ndarray) -> None: + spec = self._input_plan[tensor_name] + self.inputs[tensor_name] = Tensor(tensor_value, device=spec.device, dtype=spec.dtype).realize() + + def prepare_vision_inputs(self, imgs_cl: CLMemDict, frames: FrameDict): + for stream_name in imgs_cl: + if stream_name not in self.inputs or not TICI: + self._attach_vision_tensor(stream_name, imgs_cl, frames) + + def prepare_policy_inputs(self, numpy_inputs: NumpyDict): + for tensor_name, tensor_value in numpy_inputs.items(): + self._attach_state_tensor(tensor_name, tensor_value) + + def prepare_inputs(self, imgs_cl: CLMemDict, numpy_inputs: NumpyDict, frames: FrameDict) -> dict: + self.prepare_vision_inputs(imgs_cl, frames) + self.prepare_policy_inputs(numpy_inputs) + return self.inputs + + def _parse_outputs(self, model_outputs: np.ndarray) -> NumpyDict: + if self._model_data is None: + raise ValueError("Model data is not available. Ensure the model is loaded correctly.") + return self.parser_method_dict[self._model_data.model.type.raw](model_outputs) + + def _run_model(self) -> NumpyDict: + raw_output = self.model_run(**self.inputs).numpy().reshape(-1) + return self._parse_outputs(raw_output) + + +class TinygradSplitRunner(ModelRunner): + def __init__(self): + super().__init__() + self.is_20hz_3d = True + self._constants = SplitModelConstants + self.vision_runner = TinygradRunner(ModelType.vision) + self.policy_runner = TinygradRunner(ModelType.policy) if self.models.get(ModelType.policy) else None + self.off_policy_runner = TinygradRunner(ModelType.offPolicy) if self.models.get(ModelType.offPolicy) else None + self.on_policy_runner = TinygradRunner(ModelType.onPolicy) if self.models.get(ModelType.onPolicy) else None + + def _policy_units(self) -> list[TinygradRunner]: + return [runner for runner in (self.policy_runner, self.off_policy_runner, self.on_policy_runner) if runner is not None] + + def run_vision(self) -> NumpyDict: + return self.vision_runner.run_model() + + def run_policy(self) -> NumpyDict: + return _merge_step_outputs([runner.run_model() for runner in self._policy_units()]) + + def refresh_policy_features(self, features_buffer: np.ndarray) -> None: + for runner in self._policy_units(): + if "features_buffer" in runner._input_plan: + runner._attach_state_tensor("features_buffer", features_buffer) + + def _run_model(self) -> NumpyDict: + return _merge_step_outputs([self.run_vision(), self.run_policy()]) + + @property + def vision_input_names(self) -> list[str]: + return list(self.vision_runner.vision_input_names) + + @property + def input_shapes(self) -> ShapeDict: + composite: ShapeDict = dict(self.vision_runner.input_shapes) + for runner in self._policy_units(): + composite.update(runner.input_shapes) + return composite + + @property + def output_slices(self) -> SliceDict: + composite: SliceDict = dict(self.vision_runner.output_slices) + for runner in self._policy_units(): + composite.update(runner.output_slices) + return composite + + def prepare_inputs(self, imgs_cl: CLMemDict, numpy_inputs: NumpyDict, frames: FrameDict) -> dict: + self.vision_runner.prepare_vision_inputs(imgs_cl, frames) + assembled_inputs = dict(self.vision_runner.inputs) + for runner in self._policy_units(): + runner.prepare_policy_inputs(numpy_inputs) + assembled_inputs.update(runner.inputs) + self.inputs = assembled_inputs + return assembled_inputs diff --git a/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/iqmodeld/models/split_model_constants.py b/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/iqmodeld/models/split_model_constants.py new file mode 100644 index 000000000..3f83ea806 --- /dev/null +++ b/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/iqmodeld/models/split_model_constants.py @@ -0,0 +1,89 @@ +import numpy as np + + +def index_function(idx, max_val=192, max_idx=32): + return max_val * ((idx/max_idx)**2) + + +class SplitModelConstants: + IDX_N = 33 + T_IDXS = [index_function(idx, max_val=10.0) for idx in range(IDX_N)] + X_IDXS = [index_function(idx, max_val=192.0) for idx in range(IDX_N)] + LEAD_T_IDXS = [0., 2., 4., 6., 8., 10.] + LEAD_T_OFFSETS = [0., 2., 4.] + META_T_IDXS = [2., 4., 6., 8., 10.] + + MODEL_FREQ = 20 + HISTORY_FREQ = 5 + HISTORY_LEN_SECONDS = 5 + TEMPORAL_SKIP = MODEL_FREQ // HISTORY_FREQ + FULL_HISTORY_BUFFER_LEN = MODEL_FREQ * HISTORY_LEN_SECONDS + INPUT_HISTORY_BUFFER_LEN = HISTORY_FREQ * HISTORY_LEN_SECONDS + + FEATURE_LEN = 512 + + DESIRE_LEN = 8 + TRAFFIC_CONVENTION_LEN = 2 + LAT_PLANNER_STATE_LEN = 4 + LATERAL_CONTROL_PARAMS_LEN = 2 + PREV_DESIRED_CURV_LEN = 1 + + FCW_THRESHOLDS_5MS2 = np.array([.05, .05, .15, .15, .15], dtype=np.float32) + FCW_THRESHOLDS_3MS2 = np.array([.7, .7], dtype=np.float32) + FCW_5MS2_PROBS_WIDTH = 5 + FCW_3MS2_PROBS_WIDTH = 2 + + DISENGAGE_WIDTH = 5 + POSE_WIDTH = 6 + WIDE_FROM_DEVICE_WIDTH = 3 + LEAD_WIDTH = 4 + LANE_LINES_WIDTH = 2 + ROAD_EDGES_WIDTH = 2 + PLAN_WIDTH = 15 + DESIRE_PRED_WIDTH = 8 + LAT_PLANNER_SOLUTION_WIDTH = 4 + DESIRED_CURV_WIDTH = 1 + ACTION_WIDTH = 2 + + NUM_LANE_LINES = 4 + NUM_ROAD_EDGES = 2 + + LEAD_TRAJ_LEN = 6 + DESIRE_PRED_LEN = 4 + + PLAN_MHP_N = 5 + LEAD_MHP_N = 2 + PLAN_MHP_SELECTION = 1 + LEAD_MHP_SELECTION = 3 + + FCW_THRESHOLD_5MS2_HIGH = 0.15 + FCW_THRESHOLD_5MS2_LOW = 0.05 + FCW_THRESHOLD_3MS2 = 0.7 + + CONFIDENCE_BUFFER_LEN = 5 + RYG_GREEN = 0.01165 + RYG_YELLOW = 0.06157 + + POLY_PATH_DEGREE = 4 + + +class Plan: + POSITION = slice(0, 3) + VELOCITY = slice(3, 6) + ACCELERATION = slice(6, 9) + T_FROM_CURRENT_EULER = slice(9, 12) + ORIENTATION_RATE = slice(12, 15) + + +class Meta: + ENGAGED = slice(0, 1) + GAS_DISENGAGE = slice(1, 31, 6) + BRAKE_DISENGAGE = slice(2, 31, 6) + STEER_OVERRIDE = slice(3, 31, 6) + HARD_BRAKE_3 = slice(4, 31, 6) + HARD_BRAKE_4 = slice(5, 31, 6) + HARD_BRAKE_5 = slice(6, 31, 6) + GAS_PRESS = slice(31, 55, 4) + BRAKE_PRESS = slice(32, 55, 4) + LEFT_BLINKER = slice(33, 55, 4) + RIGHT_BLINKER = slice(34, 55, 4) diff --git a/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/iqmodeld/parser.py b/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/iqmodeld/parser.py new file mode 100644 index 000000000..1b382b57f --- /dev/null +++ b/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/iqmodeld/parser.py @@ -0,0 +1,219 @@ +from __future__ import annotations + +from dataclasses import dataclass + +import numpy as np + +from iqpilot.selfdrive.iqmodeld.models.split_model_constants import SplitModelConstants +from iqpilot.selfdrive.iqmodeld.config import ModelConstants + + +def safe_exp(values, out=None): + return np.exp(np.clip(values, -np.inf, 11), out=out) + + +def sigmoid(values): + return 1.0 / (1.0 + safe_exp(-values)) + + +def _softmax_last(values, axis=-1): + values -= np.max(values, axis=axis, keepdims=True) + if values.dtype in (np.float32, np.float64): + safe_exp(values, out=values) + else: + values = safe_exp(values) + values /= np.sum(values, axis=axis, keepdims=True) + return values + + +@dataclass(frozen=True) +class _MixtureRecipe: + input_heads: int + output_heads: int + final_shape: tuple[int, ...] + + +class _TensorKitchen: + def __init__(self, ignore_missing: bool = False): + self.ignore_missing = ignore_missing + + def _grab(self, outputs: dict[str, np.ndarray], tensor_name: str) -> np.ndarray | None: + if tensor_name not in outputs: + if not self.ignore_missing: + raise ValueError(f"Missing output {tensor_name}") + return + return outputs[tensor_name] + + def categorical(self, outputs: dict[str, np.ndarray], tensor_name: str, shape=None) -> None: + raw = self._grab(outputs, tensor_name) + if raw is None: + return + if shape is not None: + raw = raw.reshape((raw.shape[0],) + shape) + outputs[tensor_name] = _softmax_last(raw, axis=-1) + + def binary(self, outputs: dict[str, np.ndarray], tensor_name: str) -> None: + raw = self._grab(outputs, tensor_name) + if raw is None: + return + outputs[tensor_name] = sigmoid(raw) + + def mixture(self, outputs: dict[str, np.ndarray], tensor_name: str, recipe: _MixtureRecipe) -> None: + raw = self._grab(outputs, tensor_name) + if raw is None: + return + + reshaped = raw.reshape((raw.shape[0], max(recipe.input_heads, 1), -1)) + value_count = (reshaped.shape[2] - recipe.output_heads) // 2 + means = reshaped[:, :, :value_count] + stds = safe_exp(reshaped[:, :, value_count:2 * value_count]) + + if recipe.input_heads > 1: + weights = np.zeros((reshaped.shape[0], recipe.input_heads, recipe.output_heads), dtype=reshaped.dtype) + for output_idx in range(recipe.output_heads): + weights[:, :, output_idx - recipe.output_heads] = _softmax_last( + reshaped[:, :, output_idx - recipe.output_heads], axis=-1 + ) + + if recipe.output_heads == 1: + for batch_idx in range(weights.shape[0]): + order = np.argsort(weights[batch_idx][:, 0])[::-1] + weights[batch_idx] = weights[batch_idx][order] + means[batch_idx] = means[batch_idx][order] + stds[batch_idx] = stds[batch_idx][order] + + hypothesis_shape = (reshaped.shape[0], recipe.input_heads, *recipe.final_shape) + outputs[f"{tensor_name}_weights"] = weights + outputs[f"{tensor_name}_hypotheses"] = means.reshape(hypothesis_shape) + outputs[f"{tensor_name}_stds_hypotheses"] = stds.reshape(hypothesis_shape) + + picked_means = np.zeros((reshaped.shape[0], recipe.output_heads, value_count), dtype=reshaped.dtype) + picked_stds = np.zeros((reshaped.shape[0], recipe.output_heads, value_count), dtype=reshaped.dtype) + for batch_idx in range(weights.shape[0]): + for output_idx in range(recipe.output_heads): + order = np.argsort(weights[batch_idx, :, output_idx])[::-1] + picked_means[batch_idx, output_idx] = means[batch_idx, order[0]] + picked_stds[batch_idx, output_idx] = stds[batch_idx, order[0]] + else: + picked_means = means + picked_stds = stds + + final_shape = ((reshaped.shape[0], recipe.output_heads, *recipe.final_shape) + if recipe.output_heads > 1 else (reshaped.shape[0], *recipe.final_shape)) + outputs[tensor_name] = picked_means.reshape(final_shape) + outputs[f"{tensor_name}_stds"] = picked_stds.reshape(final_shape) + + +class ArchiveParser(_TensorKitchen): + def __init__(self, ignore_missing: bool = False): + super().__init__(ignore_missing=ignore_missing) + self._c = ModelConstants + + def _recipes(self) -> list[tuple[str, _MixtureRecipe]]: + c = self._c + return [ + ("plan", _MixtureRecipe(c.PLAN_MHP_N, c.PLAN_MHP_SELECTION, (c.IDX_N, c.PLAN_WIDTH))), + ("lane_lines", _MixtureRecipe(0, 0, (c.NUM_LANE_LINES, c.IDX_N, c.LANE_LINES_WIDTH))), + ("road_edges", _MixtureRecipe(0, 0, (c.NUM_ROAD_EDGES, c.IDX_N, c.LANE_LINES_WIDTH))), + ("pose", _MixtureRecipe(0, 0, (c.POSE_WIDTH,))), + ("road_transform", _MixtureRecipe(0, 0, (c.POSE_WIDTH,))), + ("wide_from_device_euler", _MixtureRecipe(0, 0, (c.WIDE_FROM_DEVICE_WIDTH,))), + ("lead", _MixtureRecipe(c.LEAD_MHP_N, c.LEAD_MHP_SELECTION, (c.LEAD_TRAJ_LEN, c.LEAD_WIDTH))), + ] + + def parse_outputs(self, outputs: dict[str, np.ndarray]) -> dict[str, np.ndarray]: + c = self._c + for tensor_name, recipe in self._recipes(): + self.mixture(outputs, tensor_name, recipe) + if "sim_pose" in outputs: + self.mixture(outputs, "sim_pose", _MixtureRecipe(0, 0, (c.POSE_WIDTH,))) + if "lat_planner_solution" in outputs: + self.mixture(outputs, "lat_planner_solution", _MixtureRecipe(0, 0, (c.IDX_N, c.LAT_PLANNER_SOLUTION_WIDTH))) + if "desired_curvature" in outputs: + self.mixture(outputs, "desired_curvature", _MixtureRecipe(0, 0, (c.DESIRED_CURV_WIDTH,))) + for name in ("lead_prob", "lane_lines_prob", "meta"): + self.binary(outputs, name) + self.categorical(outputs, "desire_state", shape=(c.DESIRE_PRED_WIDTH,)) + self.categorical(outputs, "desire_pred", shape=(c.DESIRE_PRED_LEN, c.DESIRE_PRED_WIDTH)) + return outputs + + +class PhaseParser(_TensorKitchen): + def __init__(self, ignore_missing: bool = False): + super().__init__(ignore_missing=ignore_missing) + self._c = SplitModelConstants + + def _has_mixture_heads(self, outputs: dict[str, np.ndarray], tensor_name: str, flat_width: int) -> bool: + raw = self._grab(outputs, tensor_name) + if raw is None: + return False + return raw.shape[1] != 2 * flat_width + + def _decode_dynamic_family(self, outputs: dict[str, np.ndarray]) -> None: + c = self._c + if "lead" in outputs: + uses_heads = self._has_mixture_heads(outputs, "lead", c.LEAD_MHP_SELECTION * c.LEAD_TRAJ_LEN * c.LEAD_WIDTH) + self.mixture(outputs, "lead", _MixtureRecipe( + c.LEAD_MHP_N if uses_heads else 0, + c.LEAD_MHP_SELECTION if uses_heads else 0, + (c.LEAD_TRAJ_LEN, c.LEAD_WIDTH) if uses_heads else (c.LEAD_MHP_SELECTION, c.LEAD_TRAJ_LEN, c.LEAD_WIDTH), + )) + + if "plan" in outputs: + uses_heads = self._has_mixture_heads(outputs, "plan", c.IDX_N * c.PLAN_WIDTH) + self.mixture(outputs, "plan", _MixtureRecipe( + c.PLAN_MHP_N if uses_heads else 0, + c.PLAN_MHP_SELECTION if uses_heads else 0, + (c.IDX_N, c.PLAN_WIDTH), + )) + + if "planplus" in outputs: + self.mixture(outputs, "planplus", _MixtureRecipe(0, 0, (c.IDX_N, c.PLAN_WIDTH))) + + def _decode_policy_family(self, outputs: dict[str, np.ndarray]) -> None: + c = self._c + if "action" in outputs: + self.mixture(outputs, "action", _MixtureRecipe(0, 0, (c.ACTION_WIDTH,))) + if "desired_curvature" in outputs: + self.mixture(outputs, "desired_curvature", _MixtureRecipe(0, 0, (c.DESIRED_CURV_WIDTH,))) + if "desire_pred" in outputs: + self.categorical(outputs, "desire_pred", shape=(c.DESIRE_PRED_LEN, c.DESIRE_PRED_WIDTH)) + if "desire_state" in outputs: + self.categorical(outputs, "desire_state", shape=(c.DESIRE_PRED_WIDTH,)) + if "lane_lines" in outputs: + self.mixture(outputs, "lane_lines", _MixtureRecipe(0, 0, (c.NUM_LANE_LINES, c.IDX_N, c.LANE_LINES_WIDTH))) + if "lane_lines_prob" in outputs: + self.binary(outputs, "lane_lines_prob") + if "lead_prob" in outputs: + self.binary(outputs, "lead_prob") + if "lat_planner_solution" in outputs: + self.mixture(outputs, "lat_planner_solution", _MixtureRecipe(0, 0, (c.IDX_N, c.LAT_PLANNER_SOLUTION_WIDTH))) + if "meta" in outputs: + self.binary(outputs, "meta") + if "road_edges" in outputs: + self.mixture(outputs, "road_edges", _MixtureRecipe(0, 0, (c.NUM_ROAD_EDGES, c.IDX_N, c.LANE_LINES_WIDTH))) + if "sim_pose" in outputs: + self.mixture(outputs, "sim_pose", _MixtureRecipe(0, 0, (c.POSE_WIDTH,))) + + def parse_vision_outputs(self, outputs: dict[str, np.ndarray]) -> dict[str, np.ndarray]: + c = self._c + self.mixture(outputs, "pose", _MixtureRecipe(0, 0, (c.POSE_WIDTH,))) + self.mixture(outputs, "wide_from_device_euler", _MixtureRecipe(0, 0, (c.WIDE_FROM_DEVICE_WIDTH,))) + self.mixture(outputs, "road_transform", _MixtureRecipe(0, 0, (c.POSE_WIDTH,))) + self._decode_dynamic_family(outputs) + self._decode_policy_family(outputs) + return outputs + + def parse_policy_outputs(self, outputs: dict[str, np.ndarray]) -> dict[str, np.ndarray]: + self._decode_dynamic_family(outputs) + self._decode_policy_family(outputs) + return outputs + + def parse_outputs(self, outputs: dict[str, np.ndarray]) -> dict[str, np.ndarray]: + return self.parse_policy_outputs(self.parse_vision_outputs(outputs)) + + +__all__ = [ + "ArchiveParser", + "PhaseParser", +] diff --git a/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/iqmodeld/runtime/__init__.py b/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/iqmodeld/runtime/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/iqmodeld/runtime/tinygrad.py b/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/iqmodeld/runtime/tinygrad.py new file mode 100644 index 000000000..75c7e350e --- /dev/null +++ b/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/iqmodeld/runtime/tinygrad.py @@ -0,0 +1,23 @@ +""" +Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos/ +""" +from tinygrad.tensor import Tensor +from tinygrad.helpers import to_mv + +_PTR_STRIDE = 8 +_RAW_GPU_PTR_SLOT = 20 +_RAW_GPU_PTR_VIEW_BYTES = 0x100 + + +def _descriptor_pointer(opencl_address: int) -> int: + return to_mv(opencl_address, _PTR_STRIDE).cast("Q")[0] + + +def _raw_gpu_pointer(descriptor_pointer: int) -> int: + return to_mv(descriptor_pointer, _RAW_GPU_PTR_VIEW_BYTES).cast("Q")[_RAW_GPU_PTR_SLOT] + + +def qcom_tensor_from_opencl_address(opencl_address, shape, dtype): + descriptor_pointer = _descriptor_pointer(opencl_address) + device_pointer = _raw_gpu_pointer(descriptor_pointer) + return Tensor.from_blob(device_pointer, shape, dtype=dtype, device="QCOM") diff --git a/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/iqmodeld/temporal_state.py b/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/iqmodeld/temporal_state.py new file mode 100644 index 000000000..70cac9f64 --- /dev/null +++ b/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/iqmodeld/temporal_state.py @@ -0,0 +1,131 @@ +""" +Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos/ +""" + +from __future__ import annotations + +import math + +import numpy as np + +DEFAULT_FRAME_SKIP = 4 + +MODEL_INPUT_SPEC: dict[str, tuple[tuple[int, ...], str]] = { + "img": ((1, 12, 128, 256), "uint8"), + "big_img": ((1, 12, 128, 256), "uint8"), + "desire_pulse": ((1, 25, 8), "float32"), + "traffic_convention": ((1, 2), "float32"), + "features_buffer": ((1, 24, 512), "float32"), + "action_t": ((1, 2), "float32"), +} + + +def spec_from_meta(meta: dict) -> dict[str, tuple[tuple[int, ...], str]] | None: + shapes = meta.get("input_shapes") + if not shapes: + return None + return {name: (tuple(shape), "uint8" if name in ("img", "big_img") else "float32") + for name, shape in shapes.items()} + + +class TemporalInputState: + def __init__(self, frame_skip: int, spec: dict[str, tuple[tuple[int, ...], str]] = MODEL_INPUT_SPEC): + self.frame_skip = frame_skip + img = spec["img"][0] + fb = spec["features_buffer"][0] + dp = spec["desire_pulse"][0] + + self.n_frames = img[1] // 6 + img_q_shape = (frame_skip * (self.n_frames - 1) + 1, 6, img[2], img[3]) + self._img_shape = img + self._fb_shape = fb + self._dp_shape = dp + feat_dim = math.prod(fb[2:]) + + self.img_q = np.zeros(img_q_shape, dtype=np.uint8) + self.big_img_q = np.zeros(img_q_shape, dtype=np.uint8) + self.feat_q = np.zeros((frame_skip * fb[1], fb[0], feat_dim), dtype=np.float32) + self.desire_q = np.zeros((frame_skip * dp[1], dp[0], dp[2]), dtype=np.float32) + self.prev_desire = np.zeros(dp[2], dtype=np.float32) + self.prev_feat = np.zeros((fb[0], feat_dim), dtype=np.float32) + + @staticmethod + def _shift_append(q: np.ndarray, new_val: np.ndarray) -> None: + q[:-1] = q[1:] + q[-1] = new_val + + def push_and_materialize(self, warped: np.ndarray, desire_pulse: np.ndarray, + traffic_convention: np.ndarray, action_t: np.ndarray, + ) -> dict[str, np.ndarray]: + fs = self.frame_skip + + cur = desire_pulse.astype(np.float32).copy() + cur[0] = 0 + pulse = np.where(cur - self.prev_desire > 0.99, cur, 0).astype(np.float32) + self.prev_desire[:] = cur + + self._shift_append(self.img_q, warped[0]) + self._shift_append(self.big_img_q, warped[1]) + self._shift_append(self.desire_q, pulse.reshape(self._dp_shape[0], self._dp_shape[2])) + self._shift_append(self.feat_q, self.prev_feat) + + dp = self._dp_shape + return { + "img": np.ascontiguousarray(self.img_q[::fs]).reshape(self._img_shape), + "big_img": np.ascontiguousarray(self.big_img_q[::fs]).reshape(self._img_shape), + "features_buffer": np.ascontiguousarray(self.feat_q[::fs]).reshape(self._fb_shape), + "desire_pulse": self.desire_q.reshape(dp[1], fs, dp[0], dp[2]).max(axis=1).reshape(dp), + "traffic_convention": traffic_convention.astype(np.float32).reshape(1, -1), + "action_t": action_t.astype(np.float32).reshape(1, -1), + } + + def note_hidden_state(self, model_output: np.ndarray, hidden_slice: slice) -> None: + self.prev_feat[:] = model_output[hidden_slice].reshape(self.prev_feat.shape) + + +class SplitTemporalState: + + def __init__(self, frame_skip: int, img_shape: tuple[int, ...], + feature_shape: tuple[int, ...], desire_shape: tuple[int, ...]): + self.frame_skip = frame_skip + self._img_shape = tuple(img_shape) + self._fb_shape = tuple(feature_shape) + self._dp_shape = tuple(desire_shape) + + n_frames = img_shape[1] // 6 + img_q_shape = (frame_skip * (n_frames - 1) + 1, 6, img_shape[2], img_shape[3]) + self.img_q = np.zeros(img_q_shape, dtype=np.uint8) + self.big_img_q = np.zeros(img_q_shape, dtype=np.uint8) + self.feat_q = np.zeros((frame_skip * (feature_shape[1] - 1) + 1, feature_shape[0], feature_shape[2]), + dtype=np.float32) + self.desire_q = np.zeros((frame_skip * desire_shape[1], desire_shape[0], desire_shape[2]), dtype=np.float32) + self.prev_desire = np.zeros(desire_shape[2], dtype=np.float32) + + def materialize_vision(self, warped: np.ndarray, desire: np.ndarray) -> dict[str, np.ndarray]: + fs = self.frame_skip + cur = desire.astype(np.float32).copy() + cur[0] = 0 + pulse = np.where(cur - self.prev_desire > 0.99, cur, 0).astype(np.float32) + self.prev_desire[:] = cur + + TemporalInputState._shift_append(self.img_q, warped[0]) + TemporalInputState._shift_append(self.big_img_q, warped[1]) + TemporalInputState._shift_append(self.desire_q, pulse.reshape(self._dp_shape[0], self._dp_shape[2])) + return { + "img": np.ascontiguousarray(self.img_q[::fs]).reshape(self._img_shape), + "big_img": np.ascontiguousarray(self.big_img_q[::fs]).reshape(self._img_shape), + } + + def materialize_policy(self, vision_feature: np.ndarray, traffic_convention: np.ndarray, + action_t: np.ndarray | None = None) -> dict[str, np.ndarray]: + fs = self.frame_skip + TemporalInputState._shift_append(self.feat_q, vision_feature.reshape(self._fb_shape[0], self._fb_shape[2])) + dp = self._dp_shape + out = { + "features_buffer": np.ascontiguousarray(self.feat_q[::fs]).reshape(self._fb_shape), + "desire_pulse": self.desire_q.reshape(dp[1], fs, dp[0], dp[2]).max(axis=1).reshape(dp), + "traffic_convention": traffic_convention.astype(np.float32).reshape(1, -1), + } + if action_t is not None: + out["action_t"] = action_t.astype(np.float32).reshape(1, -1) + return out diff --git a/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/iqmodeld/tools/__init__.py b/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/iqmodeld/tools/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/iqmodeld/tools/compile_supercombo.py b/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/iqmodeld/tools/compile_supercombo.py new file mode 100644 index 000000000..56ce00b2e --- /dev/null +++ b/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/iqmodeld/tools/compile_supercombo.py @@ -0,0 +1,422 @@ +#!/usr/bin/env python3 +""" +Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos/ +""" + +import argparse +import atexit +import math +import os +import pickle +import re +import tempfile +import time +from functools import partial +from collections import namedtuple + +import numpy as np + +def _patch_tinygrad_fetch_fw(): + import hashlib + import pathlib + import zstandard + from tinygrad import helpers + _orig = helpers.fetch_fw + def fetch_fw(path, name, sha256): + p = pathlib.Path(f"/lib/firmware/{path}/{name}.zst") + if p.is_file(): + blob = zstandard.ZstdDecompressor().stream_reader(p.read_bytes()).read() + if hashlib.sha256(blob).hexdigest() == sha256: + return blob + return _orig(path, name, sha256) + helpers.fetch_fw = fetch_fw +_patch_tinygrad_fetch_fw() + +from tinygrad.tensor import Tensor +from tinygrad.helpers import Context +from tinygrad.device import Device +from tinygrad.engine.jit import TinyJit + + +NV12Frame = namedtuple("NV12Frame", ['width', 'height', 'stride', 'y_height', 'uv_height', 'size']) +WARP_INPUTS = ['tfm', 'big_tfm'] +POLICY_INPUTS = ['img_q', 'big_img_q', 'feat_q', 'desire_q', 'packed_npy_inputs'] + +UV_SCALE_MATRIX = np.array([[0.5, 0, 0], [0, 0.5, 0], [0, 0, 1]], dtype=np.float32) +UV_SCALE_MATRIX_INV = np.linalg.inv(UV_SCALE_MATRIX) + +WARP_DEV = os.getenv('WARP_DEV') + + +def make_random_images(keys, shape, device=None): + return {k: Tensor.randint(shape, low=0, high=256, dtype='uint8', device=device).realize() for k in keys} + + +def warp_perspective_tinygrad(src_flat, M_inv, dst_shape, src_shape, stride_pad, border_fill_val=None): + w_dst, h_dst = dst_shape + h_src, w_src = src_shape + + x = Tensor.arange(w_dst).reshape(1, w_dst).expand(h_dst, w_dst).reshape(-1) + y = Tensor.arange(h_dst).reshape(h_dst, 1).expand(h_dst, w_dst).reshape(-1) + + src_x = M_inv[0, 0] * x + M_inv[0, 1] * y + M_inv[0, 2] + src_y = M_inv[1, 0] * x + M_inv[1, 1] * y + M_inv[1, 2] + src_w = M_inv[2, 0] * x + M_inv[2, 1] * y + M_inv[2, 2] + + src_x = src_x / src_w + src_y = src_y / src_w + + x_round = Tensor.round(src_x) + y_round = Tensor.round(src_y) + x_nn_clipped = x_round.clip(0, w_src - 1).cast('int') + y_nn_clipped = y_round.clip(0, h_src - 1).cast('int') + idx = y_nn_clipped * (w_src + stride_pad) + x_nn_clipped + sampled = src_flat[idx] + + if border_fill_val is None: + return sampled + + in_bounds = ((x_round >= 0) & (x_round <= w_src - 1) & + (y_round >= 0) & (y_round <= h_src - 1)).cast(sampled.dtype) + return sampled * in_bounds + Tensor(border_fill_val, dtype=sampled.dtype) * (1 - in_bounds) + + +def frames_to_tensor(frames): + H = (frames.shape[0] * 2) // 3 + W = frames.shape[1] + in_img1 = Tensor.cat(frames[0:H:2, 0::2], + frames[1:H:2, 0::2], + frames[0:H:2, 1::2], + frames[1:H:2, 1::2], + frames[H:H+H//4].reshape((H//2, W//2)), + frames[H+H//4:H+H//2].reshape((H//2, W//2)), dim=0).reshape((6, H//2, W//2)) + return in_img1 + + +def make_frame_prepare(nv12: NV12Frame, model_w, model_h): + cam_w, cam_h, stride, y_height, uv_height, _ = nv12 + uv_offset = stride * y_height + stride_pad = stride - cam_w + + def frame_prepare_tinygrad(input_frame, M_inv): + M_inv_uv = M_inv * Tensor([[1.0, 1.0, 0.5], [1.0, 1.0, 0.5], [2.0, 2.0, 1.0]], device=WARP_DEV) + uv = input_frame[uv_offset:uv_offset + uv_height * stride].reshape(uv_height, stride) + with Context(SPLIT_REDUCEOP=0): + y = warp_perspective_tinygrad(input_frame[:cam_h*stride], + M_inv, (model_w, model_h), + (cam_h, cam_w), stride_pad).realize() + u = warp_perspective_tinygrad(uv[:cam_h//2, :cam_w:2].flatten(), + M_inv_uv, (model_w//2, model_h//2), + (cam_h//2, cam_w//2), 0).realize() + v = warp_perspective_tinygrad(uv[:cam_h//2, 1:cam_w:2].flatten(), + M_inv_uv, (model_w//2, model_h//2), + (cam_h//2, cam_w//2), 0).realize() + yuv = y.cat(u).cat(v).reshape((model_h * 3 // 2, model_w)) + tensor = frames_to_tensor(yuv) + return tensor + return frame_prepare_tinygrad + + +def make_warp_input_queues(vision_input_shapes, frame_skip, device): + img = vision_input_shapes['img'] # (1, 12, 128, 256) + n_frames = img[1] // 6 + img_buf_shape = (frame_skip * (n_frames - 1) + 1, 6, img[2], img[3]) + + npy = { + 'tfm': np.zeros((3, 3), dtype=np.float32), + 'big_tfm': np.zeros((3, 3), dtype=np.float32), + } + input_queues = { + 'img_q': Tensor(np.zeros(img_buf_shape, dtype=np.uint8), device=device).contiguous().realize(), + 'big_img_q': Tensor(np.zeros(img_buf_shape, dtype=np.uint8), device=device).contiguous().realize(), + **{k: Tensor(v, device='NPY').realize() for k, v in npy.items()}, + } + return input_queues, npy + + +def get_policy_npy_shapes(input_shapes): + dp = input_shapes['desire_pulse'] # (1, 25, 8) + tc = input_shapes['traffic_convention'] # (1, 2) + at = input_shapes['action_t'] # (1, 2) + fb = input_shapes['features_buffer'] # (1, 24, 512) + shapes = {'desire': (dp[2],), 'traffic_convention': tuple(tc), 'action_t': tuple(at), 'prev_feat': (fb[0], fb[2])} + return shapes, [math.prod(s) for s in shapes.values()] + + +def make_input_queues(input_shapes, frame_skip, device): + input_queues, npy = make_warp_input_queues(input_shapes, frame_skip, device) + + fb = input_shapes['features_buffer'] # (1, 24, 512), past features only; the model appends the current frame's feature + dp = input_shapes['desire_pulse'] # (1, 25, 8) + + shapes, sizes = get_policy_npy_shapes(input_shapes) + packed_npy_inputs = np.zeros(sum(sizes), dtype=np.float32) + npy.update({k: v.reshape(s) for (k, s), v in zip(shapes.items(), np.split(packed_npy_inputs, np.cumsum(sizes[:-1])), strict=True)}) + input_queues.update({ + 'feat_q': Tensor(np.zeros((frame_skip * fb[1], fb[0], fb[2]), dtype=np.float32), device=device).contiguous().realize(), + 'desire_q': Tensor(np.zeros((frame_skip * dp[1], dp[0], dp[2]), dtype=np.float32), device=device).contiguous().realize(), + 'packed_npy_inputs': Tensor(packed_npy_inputs, device='NPY').realize(), + }) + return input_queues, npy + + +def shift_and_sample(buf, new_val, sample_fn): + buf.assign(buf[1:].cat(new_val, dim=0).contiguous()) + return sample_fn(buf) + + +def sample_skip(buf, frame_skip): + return buf[::frame_skip].contiguous().flatten(0, 1).unsqueeze(0) + + +def sample_desire(buf, frame_skip): + return buf.reshape(-1, frame_skip, *buf.shape[1:]).max(1).flatten(0, 1).unsqueeze(0) + + +def make_warp(nv12, model_w, model_h, frame_skip): + frame_prepare = make_frame_prepare(nv12, model_w, model_h) + + def warp(tfm, big_tfm, frame, big_frame): + tfm = tfm.to(WARP_DEV) + big_tfm = big_tfm.to(WARP_DEV) + Tensor.realize(tfm, big_tfm) + + warped_frame = frame_prepare(frame, tfm).unsqueeze(0) + warped_big_frame = frame_prepare(big_frame, big_tfm).unsqueeze(0) + return Tensor.cat(warped_frame, warped_big_frame) + + return warp + + +def make_run_policy(model_runner, model_metadata, frame_skip): + sample_desire_fn = partial(sample_desire, frame_skip=frame_skip) + sample_skip_fn = partial(sample_skip, frame_skip=frame_skip) + npy_shapes, npy_sizes = get_policy_npy_shapes(model_metadata['input_shapes']) + + def run_policy(warped, img_q, big_img_q, feat_q, desire_q, packed_npy_inputs): + packed_npy_inputs = packed_npy_inputs.to(Device.DEFAULT) + warped = warped.to(Device.DEFAULT) + Tensor.realize(packed_npy_inputs, warped) + + img = shift_and_sample(img_q, warped[0:1], sample_skip_fn) + big_img = shift_and_sample(big_img_q, warped[1:2], sample_skip_fn) + + desire, traffic_convention, action_t, prev_feat = (t.reshape(s) for t, s in zip(packed_npy_inputs.split(npy_sizes), npy_shapes.values(), strict=True)) + desire_buf = shift_and_sample(desire_q, desire.reshape(1, 1, -1), sample_desire_fn) + feat_buf = shift_and_sample(feat_q, prev_feat.reshape(1, 1, -1), sample_skip_fn) + + inputs = { + 'img': img, + 'big_img': big_img, + 'features_buffer': feat_buf, + 'desire_pulse': desire_buf, + 'traffic_convention': traffic_convention, + 'action_t': action_t, + } + out = next(iter(model_runner(inputs).values())).cast('float32') + return out, + return run_policy + + +def compile_jit(jit, make_random_inputs, input_keys, make_queues): + SEED = 42 + def random_inputs_run(fn, seed, test_val=None, test_buffers=None, expect_match=True): + input_queues, npy = make_queues(Device.DEFAULT) + np.random.seed(seed) + Tensor.manual_seed(seed) + + testing = test_val is not None or test_buffers is not None + n_runs = 1 if testing else 3 + + for i in range(n_runs): + for v in npy.values(): + v[:] = np.random.randn(*v.shape).astype(v.dtype) + Device.default.synchronize() + random_inputs = make_random_inputs() + st = time.perf_counter() + outs = fn(**{k: input_queues[k] for k in input_keys}, **random_inputs) + mt = time.perf_counter() + Device.default.synchronize() + et = time.perf_counter() + print(f" [{i+1}/{n_runs}] enqueue {(mt-st)*1e3:6.2f} ms -- total {(et-st)*1e3:6.2f} ms") + + if i == 0: + val = [np.copy(v.numpy()) for v in outs] + buffers = [np.copy(v.numpy().copy()) for v in input_queues.values()] + + if test_val is not None: + match = all(np.array_equal(a, b) for a, b in zip(val, test_val, strict=True)) + assert match == expect_match, f"outputs {'differ from' if expect_match else 'match'} baseline (seed={seed})" + if test_buffers is not None: + match = all(np.array_equal(a, b) for a, b in zip(buffers, test_buffers, strict=True)) + assert match == expect_match, f"buffers {'differ from' if expect_match else 'match'} baseline (seed={seed})" + return val, buffers + + print('capture + replay') + test_val, test_buffers = random_inputs_run(jit, SEED) + print('pickle round trip') + jit = pickle.loads(pickle.dumps(jit)) + random_inputs_run(jit, SEED, test_val, test_buffers, expect_match=True) + random_inputs_run(jit, SEED+1, test_val, test_buffers, expect_match=False) + return jit + + +def _captured_devices(jit) -> set[str]: + captured = getattr(jit, 'captured', None) + infos = getattr(captured, 'expected_input_info', None) + if not infos: + return set() + + devices: set[str] = set() + for info in infos: + if isinstance(info, tuple) and len(info) >= 4 and isinstance(info[3], str): + devices.add(info[3]) + return devices + + +def _slice_outputs(model_outputs: np.ndarray, output_slices: dict[str, slice]) -> dict[str, np.ndarray]: + return {name: model_outputs[np.newaxis, tensor_slice] for name, tensor_slice in output_slices.items() if name != 'pad'} + + +def _validate_pose_outputs(parsed_outputs: dict[str, np.ndarray]) -> None: + from iqpilot.selfdrive.locationd.locationd import MIN_STD_SANITY_CHECK, ROTATION_SANITY_CHECK, TRANS_SANITY_CHECK + + required = ( + 'pose', 'pose_stds', 'wide_from_device_euler', 'wide_from_device_euler_stds', + 'road_transform', 'road_transform_stds', + ) + missing = [name for name in required if name not in parsed_outputs] + if missing: + raise AssertionError(f"parsed supercombo outputs missing required odometry tensors: {missing}") + + for name in required: + values = parsed_outputs[name] + if not np.isfinite(values).all(): + raise AssertionError(f"parsed supercombo output {name} contains non-finite values") + + pose = parsed_outputs['pose'][0] + pose_stds = parsed_outputs['pose_stds'][0] + road_transform_stds = parsed_outputs['road_transform_stds'][0] + wide_stds = parsed_outputs['wide_from_device_euler_stds'][0] + + if pose_stds.min() <= MIN_STD_SANITY_CHECK: + raise AssertionError(f"pose_stds min {pose_stds.min()} <= {MIN_STD_SANITY_CHECK}") + if road_transform_stds.min() <= MIN_STD_SANITY_CHECK: + raise AssertionError(f"road_transform_stds min {road_transform_stds.min()} <= {MIN_STD_SANITY_CHECK}") + if wide_stds.min() <= MIN_STD_SANITY_CHECK: + raise AssertionError(f"wide_from_device_euler_stds min {wide_stds.min()} <= {MIN_STD_SANITY_CHECK}") + + if np.linalg.norm(pose[:3]) > TRANS_SANITY_CHECK: + raise AssertionError(f"pose translation norm {np.linalg.norm(pose[:3])} exceeds {TRANS_SANITY_CHECK}") + if np.linalg.norm(pose[3:]) > ROTATION_SANITY_CHECK: + raise AssertionError(f"pose rotation norm {np.linalg.norm(pose[3:])} exceeds {ROTATION_SANITY_CHECK}") + if np.linalg.norm(pose_stds[:3]) > 10 * TRANS_SANITY_CHECK: + raise AssertionError( + f"pose translation std norm {np.linalg.norm(pose_stds[:3])} exceeds {10 * TRANS_SANITY_CHECK}" + ) + if np.linalg.norm(pose_stds[3:]) > 10 * ROTATION_SANITY_CHECK: + raise AssertionError( + f"pose rotation std norm {np.linalg.norm(pose_stds[3:])} exceeds {10 * ROTATION_SANITY_CHECK}" + ) + + +def validate_supercombo_release(run_policy_jit, model_runner, model_metadata, frame_skip, expected_device: str) -> None: + from iqpilot.selfdrive.iqmodeld.parser import PhaseParser + + direct_fn = make_run_policy(model_runner, model_metadata, frame_skip) + parser = PhaseParser() + queue_factory = partial(make_input_queues, model_metadata['input_shapes'], frame_skip) + image_shape = model_metadata['input_shapes']['img'] + + jit_queues, jit_npy = queue_factory(Device.DEFAULT) + direct_queues, direct_npy = queue_factory(Device.DEFAULT) + + for payload in (jit_npy, direct_npy): + for name, value in payload.items(): + value[:] = 0 if value.dtype.kind in ('i', 'u') else 0.0 + + zero_inputs = { + 'warped': Tensor(np.zeros((2, 6, *image_shape[2:]), dtype=np.uint8), device=Device.DEFAULT).realize(), + } + + direct_outs, = direct_fn(**{k: direct_queues[k] for k in POLICY_INPUTS}, **zero_inputs) + jit_outs, = run_policy_jit(**{k: jit_queues[k] for k in POLICY_INPUTS}, **zero_inputs) + + direct_flat = direct_outs.numpy().astype(np.float32).reshape(-1) + jit_flat = jit_outs.numpy().astype(np.float32).reshape(-1) + + if not np.allclose(direct_flat, jit_flat, atol=1e-4, rtol=1e-4): + max_delta = float(np.max(np.abs(direct_flat - jit_flat))) + raise AssertionError(f"JIT supercombo output diverges from direct ONNX execution; max abs delta {max_delta}") + + parsed = parser.parse_vision_outputs(_slice_outputs(jit_flat, model_metadata['output_slices'])) + _validate_pose_outputs(parsed) + + captured_devices = _captured_devices(run_policy_jit) + if expected_device and captured_devices and expected_device not in captured_devices: + raise AssertionError( + f"compiled run_policy backend mismatch: captured {sorted(captured_devices)} expected {expected_device}" + ) + + +def _parse_size(s): + w, h = s.lower().split('x') + return int(w), int(h) + + +def read_file_chunked_to_shm(path): + from iqpilot.common.file_chunker import read_file_chunked + from iqpilot.system.hardware.hw import Paths + with tempfile.NamedTemporaryFile(prefix='compile_modeld_', dir=Paths.shm_path(), delete=False) as f: + f.write(read_file_chunked(path)) + tmp_path = f.name + atexit.register(lambda: os.path.exists(tmp_path) and os.remove(tmp_path)) + return tmp_path + + +if __name__ == "__main__": + from tinygrad.nn.onnx import OnnxRunner + from iqpilot.system.camerad.cameras.nv12_info import get_nv12_info + from iqpilot.selfdrive.iqmodeld.metadata import build_metadata_record + p = argparse.ArgumentParser() + p.add_argument('--model-size', type=_parse_size, required=True, help='model input WxH') + p.add_argument('--camera-resolutions', type=_parse_size, nargs='+', required=True, + help='camera resolutions WxH (one or more)') + p.add_argument('--onnx', required=True) + p.add_argument('--output', required=True) + p.add_argument('--frame-skip', type=int, required=True) + p.add_argument('--expected-device', default='QCOM', help='expected tinygrad backend baked into the JIT') + args = p.parse_args() + + model_path = read_file_chunked_to_shm(args.onnx) + model_w, model_h = args.model_size + + model_runner = OnnxRunner(model_path) + out = { + 'metadata': build_metadata_record(model_path), + 'frame_skip': args.frame_skip, + } + + run_policy_jit = TinyJit(make_run_policy(model_runner, out['metadata'], args.frame_skip), prune=True) + + make_policy_queues = partial(make_input_queues, out['metadata']['input_shapes'], args.frame_skip) + make_random_model_inputs = partial(make_random_images, keys=['warped'], shape=(2, 6, *out['metadata']['input_shapes']['img'][2:])) + out['run_policy'] = compile_jit(run_policy_jit, make_random_model_inputs, POLICY_INPUTS, + make_policy_queues) + validate_supercombo_release(out['run_policy'], model_runner, out['metadata'], args.frame_skip, args.expected_device) + + for cam_w, cam_h in args.camera_resolutions: + nv12 = NV12Frame(cam_w, cam_h, *get_nv12_info(cam_w, cam_h)) + make_random_warp_inputs = partial(make_random_images, keys=['frame', 'big_frame'], shape=nv12.size, device=WARP_DEV) + warp_enqueue = TinyJit(make_warp(nv12, model_w, model_h, args.frame_skip), prune=True) + make_warp_queues = partial(make_warp_input_queues, out['metadata']['input_shapes'], args.frame_skip) + out[(cam_w,cam_h)] = compile_jit(warp_enqueue, make_random_warp_inputs, WARP_INPUTS, make_warp_queues) + captured_devices = _captured_devices(out[(cam_w,cam_h)]) + if args.expected_device and captured_devices and args.expected_device not in captured_devices: + raise AssertionError( + f"compiled warp backend mismatch for {cam_w}x{cam_h}: captured {sorted(captured_devices)} expected {args.expected_device}" + ) + + with open(args.output, "wb") as f: + pickle.dump(out, f) + print(f"Saved JITs to {args.output} ({os.path.getsize(args.output) / 1e6:.2f} MB)") diff --git a/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/iqmodeld/tools/compile_warp.py b/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/iqmodeld/tools/compile_warp.py new file mode 100644 index 000000000..07e4886eb --- /dev/null +++ b/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/iqmodeld/tools/compile_warp.py @@ -0,0 +1,119 @@ +#!/usr/bin/env python3 +""" +Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos/ + +Compile the backend-neutral warp-only artifact: NV12 camera frames + 3x3 +transforms -> (2, 6, model_h/2, model_w/2) uint8 warped tensor, on the device +GPU (QCOM). maciqmodeld runs this locally +and feed the output to their backend, so the big model's image pipeline is +bit-identical to comma's fused pkl warp stage. + +Run ON the device (needs the QCOM backend): + cd /data/openpilot && DEV=QCOM WARP_DEV=QCOM IMAGE=1 FLOAT16=1 NOLOCALS=1 JIT_BATCH_SIZE=0 \ + python3 iqpilot/selfdrive/iqmodeld/tools/compile_warp.py \ + --camera-resolutions 1928x1208 --output /data/models/emac_warp.pkl +The artifact is then split per-resolution into Paths.model_root(). +""" +from __future__ import annotations + +import argparse +import hashlib +import os +import pickle +from functools import partial + +import numpy as np + +SELFTEST_SEED = 20260817 + +from iqpilot.selfdrive.iqmodeld.temporal_state import DEFAULT_FRAME_SKIP, MODEL_INPUT_SPEC +from iqpilot.selfdrive.iqmodeld.tools.compile_supercombo import ( + NV12Frame, WARP_INPUTS, compile_jit, make_random_images, make_warp, make_warp_input_queues, +) + +MODEL_SIZE = (MODEL_INPUT_SPEC["img"][0][3] * 2, MODEL_INPUT_SPEC["img"][0][2] * 2) # (512, 256) + + +def _parse_size(s: str) -> tuple[int, int]: + w, h = s.lower().split("x") + return int(w), int(h) + + +def compile_warp(cam_w: int, cam_h: int, out_path: str | None = None, + frame_skip: int = DEFAULT_FRAME_SKIP) -> str: + """Compile the warp-only QCOM JIT for one camera resolution and write the pkl. + Returns the artifact path. Callable from the workers so a fresh device + self-provisions the warp instead of erroring — needs the QCOM backend.""" + # the QCOM warp env must be set before tinygrad is imported here + os.environ.setdefault("DEV", "QCOM") + os.environ.setdefault("WARP_DEV", "QCOM") + os.environ.setdefault("IMAGE", "1") + os.environ.setdefault("FLOAT16", "1") + os.environ.setdefault("NOLOCALS", "1") + os.environ.setdefault("JIT_BATCH_SIZE", "0") + from tinygrad.engine.jit import TinyJit + from iqpilot.system.camerad.cameras.nv12_info import get_nv12_info + from iqpilot.system.hardware.hw import Paths + + model_w, model_h = MODEL_SIZE + input_shapes = {name: shape for name, (shape, _) in MODEL_INPUT_SPEC.items()} + nv12 = NV12Frame(cam_w, cam_h, *get_nv12_info(cam_w, cam_h)) + make_random_warp_inputs = partial(make_random_images, keys=["frame", "big_frame"], + shape=nv12.size, device=os.getenv("WARP_DEV")) + warp_jit = TinyJit(make_warp(nv12, model_w, model_h, frame_skip), prune=True) + make_warp_queues = partial(make_warp_input_queues, input_shapes, frame_skip) + compiled = compile_jit(warp_jit, make_random_warp_inputs, WARP_INPUTS, make_warp_queues) + + # historical artifact name: already-provisioned devices keep their warp + out_path = out_path or os.path.join(Paths.model_root(), f"emac_warp_{cam_w}x{cam_h}_tinygrad.pkl") + os.makedirs(os.path.dirname(out_path), exist_ok=True) + tmp = out_path + ".part" + bundle = {(cam_w, cam_h): compiled, "frame_skip": frame_skip, "model_size": MODEL_SIZE} + bundle["selftest"] = selftest_digest(compiled, cam_w, cam_h, nv12.size) + with open(tmp, "wb") as f: + pickle.dump(bundle, f) + os.replace(tmp, out_path) # atomic: a reader never sees a half-written pkl + return out_path + + +def main() -> None: + p = argparse.ArgumentParser() + p.add_argument("--camera-resolutions", type=_parse_size, nargs="+", default=[(1928, 1208)]) + p.add_argument("--output", default=None) + p.add_argument("--frame-skip", type=int, default=DEFAULT_FRAME_SKIP) + args = p.parse_args() + for cam_w, cam_h in args.camera_resolutions: + out = compile_warp(cam_w, cam_h, args.output, frame_skip=args.frame_skip) + print(f"saved warp JIT to {out} ({os.path.getsize(out) / 1e6:.2f} MB)") + + +if __name__ == "__main__": + main() + + +def selftest_inputs(cam_w: int, cam_h: int, nv12_size: int): + """A fixed synthetic frame pair and pair of matrices. Deterministic so the + digest is reproducible on the device that compiled the artifact.""" + rng = np.random.default_rng(SELFTEST_SEED) + frame = rng.integers(0, 256, nv12_size, dtype=np.uint8) + big_frame = rng.integers(0, 256, nv12_size, dtype=np.uint8) + tfm = np.array([[0.7, 0.02, 300.0], [0.01, 0.7, 240.0], [0.0, 0.0, 1.0]], dtype=np.float32) + big_tfm = np.array([[0.5, 0.01, 380.0], [0.02, 0.5, 300.0], [0.0, 0.0, 1.0]], dtype=np.float32) + return frame, big_frame, tfm, big_tfm + + +def selftest_digest(compiled, cam_w: int, cam_h: int, nv12_size: int) -> str: + """Hash the warp's output for a fixed input. + + A warp artifact pinned to one tinygrad can still unpickle under another and + then compute silently wrong, which reaches the model as a garbage image and + looks like a bad model rather than a stale artifact. A version string cannot + see that; running it can.""" + from tinygrad.tensor import Tensor + frame, big_frame, tfm, big_tfm = selftest_inputs(cam_w, cam_h, nv12_size) + dev = os.getenv("WARP_DEV") or "QCOM" + out = compiled(tfm=Tensor(tfm, device="NPY").realize(), + big_tfm=Tensor(big_tfm, device="NPY").realize(), + frame=Tensor(frame, device=dev).realize(), + big_frame=Tensor(big_frame, device=dev).realize()) + return hashlib.sha256(out.numpy().astype(np.uint8).tobytes()).hexdigest() diff --git a/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/locationd/__init__.py b/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/locationd/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/locationd/calibration_helpers.py b/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/locationd/calibration_helpers.py new file mode 100644 index 000000000..11e39af3e --- /dev/null +++ b/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/locationd/calibration_helpers.py @@ -0,0 +1,32 @@ +import math + +import numpy as np +from iqpilot.cereal import log + +from iqpilot.selfdrive.locationd.calibrationd import HEIGHT_INIT, HEIGHT_SANE_MIN, HEIGHT_SANE_MAX + + +def get_calibrated_rpy(live_calib: log.ExtrinsicsCalibration) -> np.ndarray | None: + if live_calib.calStatus != log.ExtrinsicsCalibration.Status.calibrated: + return None + + if len(live_calib.rpyCalib) != 3: + return None + + calib_rpy = np.asarray(live_calib.rpyCalib, dtype=np.float32) + return calib_rpy if np.isfinite(calib_rpy).all() else None + + +def get_render_path_height(live_calib: log.ExtrinsicsCalibration) -> float: + if live_calib.calStatus != log.ExtrinsicsCalibration.Status.calibrated: + return float(HEIGHT_INIT[0]) + + if len(live_calib.height) != 1: + return float(HEIGHT_INIT[0]) + + height = float(live_calib.height[0]) + if not math.isfinite(height): + return float(HEIGHT_INIT[0]) + if not (HEIGHT_SANE_MIN <= height <= HEIGHT_SANE_MAX): + return float(HEIGHT_INIT[0]) + return height diff --git a/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/locationd/calibrationd.py b/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/locationd/calibrationd.py new file mode 100644 index 000000000..3e3ec6d45 --- /dev/null +++ b/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/locationd/calibrationd.py @@ -0,0 +1,367 @@ +#!/usr/bin/env python3 +''' +This process finds calibration values. More info on what these calibration values +are can be found here https://github.com/commaai/openpilot/tree/master/common/transformations +While the roll calibration is a real value that can be estimated, here we assume it's zero, +and the image input into the neural network is not corrected for roll. +''' + +import os +import capnp +import numpy as np +from typing import NoReturn + +from iqpilot.cereal import log, car +import iqpilot.cereal.messaging as messaging +from iqpilot.common.constants import CV +from iqpilot.common.params import Params +from iqpilot.common.issue_debug import log_issue_limited +from iqpilot.common.realtime import config_realtime_process +from iqpilot.common.transformations.orientation import rot_from_euler, euler_from_rot +from iqpilot.common.swaglog import cloudlog +from iqpilot.system.hardware import HARDWARE + +MIN_SPEED_FILTER = 15 * CV.MPH_TO_MS +MAX_VEL_ANGLE_STD = np.radians(0.25) +MAX_YAW_RATE_FILTER = np.radians(2) # per second + +MAX_HEIGHT_STD = np.exp(-3.5) + +# This is at model frequency, blocks needed for efficiency +SMOOTH_CYCLES = 10 +BLOCK_SIZE = 100 +INPUTS_NEEDED = 5 # Minimum blocks needed for valid calibration +INPUTS_WANTED = 50 # We want a little bit more than we need for stability +MAX_ALLOWED_YAW_SPREAD = np.radians(2) +MAX_ALLOWED_PITCH_SPREAD = np.radians(4) +TICI_FAMILY_PITCH_SPREAD_RESET = np.radians(3) +RPY_INIT = np.array([0.0,0.0,0.0]) +WIDE_FROM_DEVICE_EULER_INIT = np.array([0.0, 0.0, 0.0]) +HEIGHT_INIT = np.array([1.22]) +HEIGHT_SANE_MIN, HEIGHT_SANE_MAX = 0.9, 2.0 +DEVICE_IS_TICI_FAMILY = HARDWARE.get_device_type() in ("tici", "tizi") + +# These values are needed to accommodate the model frame in the narrow cam +if HARDWARE.get_device_type() == 'mici': + PITCH_LIMITS = np.array([-0.143101, 0.22235988]) +else: + PITCH_LIMITS = np.array([-0.09074112085129739, 0.17]) +YAW_LIMITS = np.array([-0.06912048084718224, 0.06912048084718235]) +DEBUG = os.getenv("DEBUG") is not None + +def is_calibration_valid(rpy: np.ndarray) -> bool: + return (PITCH_LIMITS[0] < rpy[1] < PITCH_LIMITS[1]) and (YAW_LIMITS[0] < rpy[2] < YAW_LIMITS[1]) + + +def sanity_clip(rpy: np.ndarray) -> np.ndarray: + if np.isnan(rpy).any(): + rpy = RPY_INIT + return np.array([rpy[0], + np.clip(rpy[1], PITCH_LIMITS[0] - .005, PITCH_LIMITS[1] + .005), + np.clip(rpy[2], YAW_LIMITS[0] - .005, YAW_LIMITS[1] + .005)]) + +def moving_avg_with_linear_decay(prev_mean: np.ndarray, new_val: np.ndarray, idx: int, block_size: float) -> np.ndarray: + return (idx*prev_mean + (block_size - idx) * new_val) / block_size + +class Calibrator: + def __init__(self, param_put: bool = False): + self.param_put = param_put + + self.not_car = False + self.stable_rpy = RPY_INIT.copy() + self.stable_wide_from_device_euler = WIDE_FROM_DEVICE_EULER_INIT.copy() + self.stable_height = HEIGHT_INIT.copy() + self.has_stable_snapshot = False + + # Read saved calibration + self.params = Params() + calibration_params = self.params.get("CalibrationParams") + rpy_init = RPY_INIT + wide_from_device_euler = WIDE_FROM_DEVICE_EULER_INIT + height = HEIGHT_INIT + valid_blocks = 0 + self.cal_status = log.ExtrinsicsCalibration.Status.uncalibrated + + if param_put and calibration_params: + try: + with log.Event.from_bytes(calibration_params) as msg: + rpy_init = np.array(msg.extrinsicsCalibration.rpyCalib) + valid_blocks = msg.extrinsicsCalibration.validBlocks + wide_from_device_euler = np.array(msg.extrinsicsCalibration.wideFromDeviceEuler) + height = np.array(msg.extrinsicsCalibration.height) + except Exception: + cloudlog.exception("Error reading cached CalibrationParams") + + self.reset(rpy_init, valid_blocks, wide_from_device_euler, height) + self.update_status() + + # If saved calibration is immediately invalid (e.g. bad params from a previous + # bootstrap bug or device remount), auto-clear it so we recalibrate from scratch + # instead of getting permanently stuck in the "Calibration Invalid" state. + if self.cal_status == log.ExtrinsicsCalibration.Status.invalid: + cloudlog.warning("calibrationd: saved CalibrationParams are invalid, clearing and starting fresh") + if param_put: + self.params.remove("CalibrationParams") + self.reset() + self.update_status() + + def _remember_stable_solution(self) -> None: + self.stable_rpy = self.rpy.copy() + self.stable_wide_from_device_euler = self.wide_from_device_euler.copy() + self.stable_height = self.height.copy() + self.has_stable_snapshot = True + + def reset(self, rpy_init: np.ndarray = RPY_INIT, + valid_blocks: int = 0, + wide_from_device_euler_init: np.ndarray = WIDE_FROM_DEVICE_EULER_INIT, + height_init: np.ndarray = HEIGHT_INIT, + smooth_from: np.ndarray | None = None) -> None: + if not np.isfinite(rpy_init).all(): + self.rpy = RPY_INIT.copy() + else: + self.rpy = rpy_init.copy() + + if not np.isfinite(height_init).all() or len(height_init) != 1: + self.height = HEIGHT_INIT.copy() + else: + self.height = height_init.copy() + + if not np.isfinite(wide_from_device_euler_init).all() or len(wide_from_device_euler_init) != 3: + self.wide_from_device_euler = WIDE_FROM_DEVICE_EULER_INIT.copy() + else: + self.wide_from_device_euler = wide_from_device_euler_init.copy() + + if not np.isfinite(valid_blocks) or valid_blocks < 0: + self.valid_blocks = 0 + else: + self.valid_blocks = valid_blocks + + self.rpys = np.tile(self.rpy, (INPUTS_WANTED, 1)) + self.wide_from_device_eulers = np.tile(self.wide_from_device_euler, (INPUTS_WANTED, 1)) + self.heights = np.tile(self.height, (INPUTS_WANTED, 1)) + + self.idx = 0 + self.block_idx = 0 + self.v_ego = 0.0 + + if smooth_from is None: + self.old_rpy = RPY_INIT + self.old_rpy_weight = 0.0 + else: + self.old_rpy = smooth_from + self.old_rpy_weight = 1.0 + + def get_valid_idxs(self) -> list[int]: + # exclude current block_idx from validity window + before_current = list(range(self.block_idx)) + after_current = list(range(min(self.valid_blocks, self.block_idx + 1), self.valid_blocks)) + return before_current + after_current + + def update_status(self) -> None: + valid_idxs = self.get_valid_idxs() + if valid_idxs: + self.wide_from_device_euler = np.mean(self.wide_from_device_eulers[valid_idxs], axis=0) + self.height = np.mean(self.heights[valid_idxs], axis=0) + rpys = self.rpys[valid_idxs] + self.rpy = np.mean(rpys, axis=0) + max_rpy_calib = np.array(np.max(rpys, axis=0)) + min_rpy_calib = np.array(np.min(rpys, axis=0)) + self.calib_spread = np.abs(max_rpy_calib - min_rpy_calib) + else: + self.calib_spread = np.zeros(3) + + if self.valid_blocks < INPUTS_NEEDED: + if self.cal_status == log.ExtrinsicsCalibration.Status.recalibrating: + self.cal_status = log.ExtrinsicsCalibration.Status.recalibrating + else: + self.cal_status = log.ExtrinsicsCalibration.Status.uncalibrated + elif is_calibration_valid(self.rpy): + self.cal_status = log.ExtrinsicsCalibration.Status.calibrated + else: + self.cal_status = log.ExtrinsicsCalibration.Status.invalid + + # If spread is too high, assume mounting was changed and reset to last block. + # Make the transition smooth. Abrupt transitions are not good for feedback loop through supercombo model. + # TODO: add height spread check with smooth transition too + pitch_spread_limit = TICI_FAMILY_PITCH_SPREAD_RESET if DEVICE_IS_TICI_FAMILY else MAX_ALLOWED_PITCH_SPREAD + spread_too_high = self.calib_spread[1] > pitch_spread_limit or self.calib_spread[2] > MAX_ALLOWED_YAW_SPREAD + if self.cal_status == log.ExtrinsicsCalibration.Status.calibrated and not spread_too_high: + self._remember_stable_solution() + + if spread_too_high and self.cal_status == log.ExtrinsicsCalibration.Status.calibrated: + use_stable_snapshot = DEVICE_IS_TICI_FAMILY and self.has_stable_snapshot + if use_stable_snapshot: + reset_rpy = self.stable_rpy + reset_wide = self.stable_wide_from_device_euler + reset_height = self.stable_height + else: + reset_rpy = self.rpys[self.block_idx - 1] + reset_wide = self.wide_from_device_eulers[self.block_idx - 1] + reset_height = self.heights[self.block_idx - 1] + + log_issue_limited( + "calibrationd_reset_spread", + "calibration", + f"calibrationd reset unstable solution pitchSpread={self.calib_spread[1]:.6f} " + f"yawSpread={self.calib_spread[2]:.6f} pitchLimit={pitch_spread_limit:.6f} " + f"use_stable_snapshot={use_stable_snapshot} rpy={self.rpy.tolist()}", + interval_sec=0.5, + ) + self.reset(reset_rpy, valid_blocks=1, wide_from_device_euler_init=reset_wide, + height_init=reset_height, smooth_from=self.stable_rpy if use_stable_snapshot else self.rpy) + self.cal_status = log.ExtrinsicsCalibration.Status.recalibrating + + write_this_cycle = (self.idx == 0) and (self.block_idx % (INPUTS_WANTED//5) == 5) + if self.param_put and write_this_cycle: + self.params.put_nonblocking("CalibrationParams", self.get_msg(True).to_bytes()) + + def handle_v_ego(self, v_ego: float) -> None: + self.v_ego = v_ego + + def get_smooth_rpy(self) -> np.ndarray: + if self.old_rpy_weight > 0: + return self.old_rpy_weight * self.old_rpy + (1.0 - self.old_rpy_weight) * self.rpy + else: + return self.rpy + + def handle_cam_odom(self, trans: list[float], + rot: list[float], + wide_from_device_euler: list[float], + trans_std: list[float], + road_transform_trans: list[float], + road_transform_trans_std: list[float]) -> np.ndarray | None: + self.old_rpy_weight = max(0.0, self.old_rpy_weight - 1/SMOOTH_CYCLES) + + fast_enough = self.v_ego > MIN_SPEED_FILTER + motion_speed = max(float(self.v_ego), float(trans[0])) + cam_fast_enough = motion_speed > MIN_SPEED_FILTER + yaw_ok = abs(rot[2]) < MAX_YAW_RATE_FILTER + straight_and_fast = fast_enough and cam_fast_enough and yaw_ok + angle_std_threshold = MAX_VEL_ANGLE_STD + height_std_threshold = MAX_HEIGHT_STD + rpy_certain = np.arctan2(trans_std[1], motion_speed) < angle_std_threshold + if len(road_transform_trans_std) == 3: + height_certain = road_transform_trans_std[2] < height_std_threshold + else: + height_certain = True + + certain_if_calib = rpy_certain + if not (straight_and_fast and certain_if_calib): + log_issue_limited( + "calibrationd_rejected_sample", + "calibration", + f"calibrationd rejected sample vEgo={self.v_ego:.2f} trans0={trans[0]:.2f} yawRate={rot[2]:.4f} " + f"fast_enough={fast_enough} cam_fast_enough={cam_fast_enough} motion_speed={motion_speed:.2f} yaw_ok={yaw_ok} " + f"rpy_certain={rpy_certain} height_certain={height_certain} valid_blocks={self.valid_blocks} idx={self.idx}", + interval_sec=1.0, + ) + return None + + observed_rpy = np.array([0, + -np.arctan2(trans[2], trans[0]), + np.arctan2(trans[1], trans[0])]) + new_rpy = euler_from_rot(rot_from_euler(self.get_smooth_rpy()).dot(rot_from_euler(observed_rpy))) + new_rpy = sanity_clip(new_rpy) + + if len(wide_from_device_euler) == 3: + new_wide_from_device_euler = np.array(wide_from_device_euler) + else: + new_wide_from_device_euler = WIDE_FROM_DEVICE_EULER_INIT + + if len(road_transform_trans) == 3 and HEIGHT_SANE_MIN <= road_transform_trans[2] <= HEIGHT_SANE_MAX: + new_height = np.array([road_transform_trans[2]]) + else: + new_height = HEIGHT_INIT + + self.rpys[self.block_idx] = moving_avg_with_linear_decay(self.rpys[self.block_idx], new_rpy, self.idx, float(BLOCK_SIZE)) + self.wide_from_device_eulers[self.block_idx] = moving_avg_with_linear_decay(self.wide_from_device_eulers[self.block_idx], + new_wide_from_device_euler, self.idx, float(BLOCK_SIZE)) + self.heights[self.block_idx] = moving_avg_with_linear_decay(self.heights[self.block_idx], new_height, self.idx, float(BLOCK_SIZE)) + + self.idx = (self.idx + 1) % BLOCK_SIZE + if self.idx == 0: + self.block_idx += 1 + self.valid_blocks = max(self.block_idx, self.valid_blocks) + self.block_idx = self.block_idx % INPUTS_WANTED + + self.update_status() + + if self.idx == 0: + log_issue_limited( + "calibrationd_progress_block", + "calibration", + f"calibrationd progress status={int(self.cal_status)} valid_blocks={self.valid_blocks} " + f"calPerc={min(100 * (self.valid_blocks * BLOCK_SIZE + self.idx) // (INPUTS_NEEDED * BLOCK_SIZE), 100)} " + f"rpy={self.rpy.tolist()} spread={self.calib_spread.tolist()}", + interval_sec=0.5, + ) + + return new_rpy + + def get_msg(self, valid: bool) -> capnp.lib.capnp._DynamicStructBuilder: + smooth_rpy = self.get_smooth_rpy() + + msg = messaging.new_message('extrinsicsCalibration') + msg.valid = valid + + extrinsicsCalibration = msg.extrinsicsCalibration + extrinsicsCalibration.validBlocks = self.valid_blocks + extrinsicsCalibration.calStatus = self.cal_status + extrinsicsCalibration.calPerc = min(100 * (self.valid_blocks * BLOCK_SIZE + self.idx) // (INPUTS_NEEDED * BLOCK_SIZE), 100) + extrinsicsCalibration.rpyCalib = smooth_rpy.tolist() + extrinsicsCalibration.rpyCalibSpread = self.calib_spread.tolist() + extrinsicsCalibration.wideFromDeviceEuler = self.wide_from_device_euler.tolist() + extrinsicsCalibration.height = self.height.tolist() + + return msg + + def send_data(self, pm: messaging.PubMaster, valid: bool) -> None: + pm.send('extrinsicsCalibration', self.get_msg(valid)) + + +def main() -> NoReturn: + config_realtime_process([0, 1, 2, 3], 5) + + pm = messaging.PubMaster(['extrinsicsCalibration']) + sm = messaging.SubMaster(['cameraOdometry', 'carState'], poll='cameraOdometry') + + params_reader = Params() + CP = messaging.log_from_bytes(params_reader.get("CarParams", block=True), car.CarParams) + + calibrator = Calibrator(param_put=True) + calibrator.not_car = CP.notCar + + while 1: + timeout = 0 if sm.frame == -1 else 100 + sm.update(timeout) + + if sm.updated['cameraOdometry']: + calibrator.handle_v_ego(sm['carState'].vEgo) + new_rpy = calibrator.handle_cam_odom(sm['cameraOdometry'].trans, + sm['cameraOdometry'].rot, + sm['cameraOdometry'].wideFromDeviceEuler, + sm['cameraOdometry'].transStd, + sm['cameraOdometry'].roadTransformTrans, + sm['cameraOdometry'].roadTransformTransStd) + + if DEBUG and new_rpy is not None: + print('got new rpy', new_rpy) + + # 4Hz driven by cameraOdometry + if sm.frame % 5 == 0: + checks_ok = sm.all_checks() + if not checks_ok: + ft = sm.freq_tracker + recv_hz = {s: (round(1.0 / ft[s].avg_dt.get_average(), 2) if ft[s].avg_dt.count else None) for s in sm.services} + log_issue_limited( + "calibrationd_checks_failed", + "calibration", + f"calibrationd all_checks failed alive={sm.alive} freq_ok={sm.freq_ok} valid={sm.valid} " + f"seen={sm.seen} recv_hz={recv_hz}", + interval_sec=5.0, + ) + calibrator.send_data(pm, checks_ok) + + +if __name__ == "__main__": + main() diff --git a/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/locationd/helpers.py b/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/locationd/helpers.py new file mode 100644 index 000000000..56a1c9039 --- /dev/null +++ b/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/locationd/helpers.py @@ -0,0 +1,188 @@ +import numpy as np +from typing import Any +from functools import cache + +from iqpilot.cereal import log +from iqpilot.common.transformations.orientation import rot_from_euler, euler_from_rot +from iqpilot.selfdrive.locationd.calibration_helpers import get_calibrated_rpy + + +@cache +def fft_next_good_size(n: int) -> int: + """ + smallest composite of 2, 3, 5, 7, 11 that is >= n + inspired by pocketfft + """ + if n <= 6: + return n + best, f2 = 2 * n, 1 + while f2 < best: + f23 = f2 + while f23 < best: + f235 = f23 + while f235 < best: + f2357 = f235 + while f2357 < best: + f235711 = f2357 + while f235711 < best: + best = f235711 if f235711 >= n else best + f235711 *= 11 + f2357 *= 7 + f235 *= 5 + f23 *= 3 + f2 *= 2 + return best + + +def parabolic_peak_interp(R, max_index): + if max_index == 0 or max_index == len(R) - 1: + return max_index + + y_m1, y_0, y_p1 = R[max_index - 1], R[max_index], R[max_index + 1] + offset = 0.5 * (y_p1 - y_m1) / (2 * y_0 - y_p1 - y_m1) + + return max_index + offset + + +def rotate_cov(rot_matrix, cov_in): + return rot_matrix @ cov_in @ rot_matrix.T + + +def rotate_std(rot_matrix, std_in): + return np.sqrt(np.diag(rotate_cov(rot_matrix, np.diag(std_in**2)))) + + +class NPQueue: + def __init__(self, maxlen: int, rowsize: int) -> None: + self.maxlen = maxlen + self.arr = np.empty((0, rowsize)) + + def __len__(self) -> int: + return len(self.arr) + + def append(self, pt: list[float]) -> None: + if len(self.arr) < self.maxlen: + self.arr = np.append(self.arr, [pt], axis=0) + else: + self.arr[:-1] = self.arr[1:] + self.arr[-1] = pt + + +class PointBuckets: + def __init__(self, x_bounds: list[tuple[float, float]], min_points: list[float], min_points_total: int, points_per_bucket: int, rowsize: int) -> None: + self.x_bounds = x_bounds + self.buckets = {bounds: NPQueue(maxlen=points_per_bucket, rowsize=rowsize) for bounds in x_bounds} + self.buckets_min_points = dict(zip(x_bounds, min_points, strict=True)) + self.min_points_total = min_points_total + + def __len__(self) -> int: + return sum([len(v) for v in self.buckets.values()]) + + def is_valid(self) -> bool: + individual_buckets_valid = all(len(v) >= min_pts for v, min_pts in zip(self.buckets.values(), self.buckets_min_points.values(), strict=True)) + total_points_valid = self.__len__() >= self.min_points_total + return individual_buckets_valid and total_points_valid + + def get_valid_percent(self) -> int: + total_points_perc = min(self.__len__() / self.min_points_total * 100, 100) + individual_buckets_perc = min(min(len(v) / min_pts * 100 for v, min_pts in + zip(self.buckets.values(), self.buckets_min_points.values(), strict=True)), 100) + return int((total_points_perc + individual_buckets_perc) / 2) + + def is_calculable(self) -> bool: + return all(len(v) > 0 for v in self.buckets.values()) + + def add_point(self, x: float, y: float) -> None: + raise NotImplementedError + + def get_points(self, num_points: int | None = None) -> Any: + points = np.vstack([x.arr for x in self.buckets.values()]) + if num_points is None: + return points + return points[np.random.choice(np.arange(len(points)), min(len(points), num_points), replace=False)] + + def load_points(self, points: list[list[float]]) -> None: + for point in points: + self.add_point(*point) + + +class ParameterEstimator: + """ Base class for parameter estimators """ + def reset(self) -> None: + raise NotImplementedError + + def handle_log(self, t: int, which: str, msg: log.Event) -> None: + raise NotImplementedError + + def get_msg(self, valid: bool, with_points: bool) -> log.Event: + raise NotImplementedError + + +class Measurement: + x, y, z = (property(lambda self: self.xyz[0]), property(lambda self: self.xyz[1]), property(lambda self: self.xyz[2])) + x_std, y_std, z_std = (property(lambda self: self.xyz_std[0]), property(lambda self: self.xyz_std[1]), property(lambda self: self.xyz_std[2])) + roll, pitch, yaw = x, y, z + roll_std, pitch_std, yaw_std = x_std, y_std, z_std + + def __init__(self, xyz: np.ndarray, xyz_std: np.ndarray): + self.xyz: np.ndarray = xyz + self.xyz_std: np.ndarray = xyz_std + + @classmethod + def from_measurement_xyz(cls, measurement: log.DeviceMotion.XYZMeasurement) -> 'Measurement': + return cls( + xyz=np.array([measurement.x, measurement.y, measurement.z]), + xyz_std=np.array([measurement.xStd, measurement.yStd, measurement.zStd]) + ) + + +class Pose: + def __init__(self, orientation: Measurement, velocity: Measurement, acceleration: Measurement, angular_velocity: Measurement): + self.orientation = orientation + self.velocity = velocity + self.acceleration = acceleration + self.angular_velocity = angular_velocity + + @classmethod + def from_live_pose(cls, live_pose: log.DeviceMotion) -> 'Pose': + return Pose( + orientation=Measurement.from_measurement_xyz(live_pose.orientationNED), + velocity=Measurement.from_measurement_xyz(live_pose.velocityDevice), + acceleration=Measurement.from_measurement_xyz(live_pose.accelerationDevice), + angular_velocity=Measurement.from_measurement_xyz(live_pose.angularVelocityDevice) + ) + + +class PoseCalibrator: + def __init__(self): + self.calib_valid = False + self.calib_from_device = np.eye(3) + + def _transform_calib_from_device(self, meas: Measurement): + new_xyz = self.calib_from_device @ meas.xyz + new_xyz_std = rotate_std(self.calib_from_device, meas.xyz_std) + return Measurement(new_xyz, new_xyz_std) + + def _ned_from_calib(self, orientation: Measurement): + ned_from_device = rot_from_euler(orientation.xyz) + ned_from_calib = ned_from_device @ self.calib_from_device.T + ned_from_calib_euler_meas = Measurement(euler_from_rot(ned_from_calib), np.full(3, np.nan)) + return ned_from_calib_euler_meas + + def build_calibrated_pose(self, pose: Pose) -> Pose: + ned_from_calib_euler = self._ned_from_calib(pose.orientation) + angular_velocity_calib = self._transform_calib_from_device(pose.angular_velocity) + acceleration_calib = self._transform_calib_from_device(pose.acceleration) + velocity_calib = self._transform_calib_from_device(pose.velocity) + + return Pose(ned_from_calib_euler, velocity_calib, acceleration_calib, angular_velocity_calib) + + def feed_live_calib(self, live_calib: log.ExtrinsicsCalibration): + calib_rpy = get_calibrated_rpy(live_calib) + if calib_rpy is not None: + self.calib_from_device = rot_from_euler(calib_rpy).T + self.calib_valid = True + else: + if not self.calib_valid: + self.calib_from_device = np.eye(3) + self.calib_valid = False diff --git a/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/locationd/locationd.py b/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/locationd/locationd.py new file mode 100644 index 000000000..bd7864b82 --- /dev/null +++ b/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/locationd/locationd.py @@ -0,0 +1,336 @@ +#!/usr/bin/env python3 +import os +import time +import capnp +import numpy as np +from enum import Enum +from collections import defaultdict + +from iqpilot.cereal import log, messaging +from iqpilot.cereal.services import SERVICE_LIST +from iqpilot.common.transformations.orientation import rot_from_euler +from iqpilot.common.realtime import config_realtime_process +from iqpilot.common.params import Params +from iqpilot.common.swaglog import cloudlog +from iqpilot.selfdrive.locationd.calibration_helpers import get_calibrated_rpy +from iqpilot.selfdrive.locationd.helpers import rotate_std +from iqpilot.selfdrive.locationd.models.pose_kf import PoseKalman, States +from iqpilot.selfdrive.locationd.models.constants import ObservationKind + +ACCEL_SANITY_CHECK = 100.0 # m/s^2 +ROTATION_SANITY_CHECK = 10.0 # rad/s +TRANS_SANITY_CHECK = 200.0 # m/s +CALIB_RPY_SANITY_CHECK = 0.5 # rad (+- 30 deg) +MIN_STD_SANITY_CHECK = 1e-5 # m or rad +MAX_FILTER_REWIND_TIME = 0.8 # s +MAX_SENSOR_TIME_DIFF = 0.1 # s +YAWRATE_CROSS_ERR_CHECK_FACTOR = 30 +INPUT_INVALID_LIMIT = 2.0 # 1 (camodo) / 9 (sensor) bad input[s] ignored +INPUT_INVALID_RECOVERY = 10.0 # ~10 secs to resume after exceeding allowed bad inputs by one +POSENET_STD_INITIAL_VALUE = 10.0 +POSENET_STD_HIST_HALF = 20 + + +def calculate_invalid_input_decay(invalid_limit, recovery_time, frequency): + return (1 - 1 / (2 * invalid_limit)) ** (1 / (recovery_time * frequency)) + + +def init_xyz_measurement(measurement: capnp._DynamicStructBuilder, values: np.ndarray, stds: np.ndarray, valid: bool): + assert len(values) == len(stds) == 3 + measurement.x, measurement.y, measurement.z = map(float, values) + measurement.xStd, measurement.yStd, measurement.zStd = map(float, stds) + measurement.valid = valid + + +class HandleLogResult(Enum): + SUCCESS = 0 + TIMING_INVALID = 1 + INPUT_INVALID = 2 + SENSOR_SOURCE_INVALID = 3 + + +class LocationEstimator: + def __init__(self, debug: bool): + self.kf = PoseKalman(MAX_FILTER_REWIND_TIME) + + self.debug = debug + + self.posenet_stds = np.array([POSENET_STD_INITIAL_VALUE] * (POSENET_STD_HIST_HALF * 2)) + self.car_speed = 0.0 + self.camodo_yawrate_distribution = np.array([0.0, 10.0]) # mean, std + self.device_from_calib = np.eye(3) + + obs_kinds = [ObservationKind.PHONE_ACCEL, ObservationKind.PHONE_GYRO, ObservationKind.CAMERA_ODO_ROTATION, ObservationKind.CAMERA_ODO_TRANSLATION] + self.observations = {kind: np.zeros(3, dtype=np.float32) for kind in obs_kinds} + self.observation_errors = {kind: np.zeros(3, dtype=np.float32) for kind in obs_kinds} + + def reset(self, t: float, x_initial: np.ndarray = PoseKalman.initial_x, P_initial: np.ndarray = PoseKalman.initial_P): + self.kf.init_state(x_initial, covs=P_initial, filter_time=t) + + def _validate_sensor_source(self, source: log.SensorEventData.SensorSource): + # some segments have two IMUs, ignore the second one + return source != log.SensorEventData.SensorSource.bmx055 + + def _validate_sensor_time(self, sensor_time: float, t: float): + # ignore empty readings + if sensor_time == 0: + return False + + # sensor time and log time should be close + sensor_time_invalid = abs(sensor_time - t) > MAX_SENSOR_TIME_DIFF + if sensor_time_invalid: + cloudlog.warning("Sensor reading ignored, sensor timestamp more than 100ms off from log time") + return not sensor_time_invalid + + def _validate_timestamp(self, t: float): + kf_t = self.kf.t + invalid = not np.isnan(kf_t) and (kf_t - t) > MAX_FILTER_REWIND_TIME + if invalid: + cloudlog.warning("Observation timestamp is older than the max rewind threshold of the filter") + return not invalid + + def _finite_check(self, t: float, new_x: np.ndarray, new_P: np.ndarray): + all_finite = np.isfinite(new_x).all() and np.isfinite(new_P).all() + if not all_finite: + cloudlog.error("Non-finite values detected, kalman reset") + self.reset(t) + + def handle_log(self, t: float, which: str, msg: capnp._DynamicStructReader) -> HandleLogResult: + new_x, new_P = None, None + if which == "accelerometer" and msg.which() == "acceleration": + sensor_time = msg.timestamp * 1e-9 + + if not self._validate_sensor_time(sensor_time, t) or not self._validate_timestamp(sensor_time): + return HandleLogResult.TIMING_INVALID + + if not self._validate_sensor_source(msg.source): + return HandleLogResult.SENSOR_SOURCE_INVALID + + v = msg.acceleration.v + meas = np.array([-v[2], -v[1], -v[0]]) + if np.linalg.norm(meas) >= ACCEL_SANITY_CHECK: + return HandleLogResult.INPUT_INVALID + + acc_res = self.kf.predict_and_observe(sensor_time, ObservationKind.PHONE_ACCEL, meas) + if acc_res is not None: + _, new_x, _, new_P, _, _, (acc_err,), _, _ = acc_res + self.observation_errors[ObservationKind.PHONE_ACCEL] = np.array(acc_err) + self.observations[ObservationKind.PHONE_ACCEL] = meas + + elif which == "gyroscope" and msg.which() == "gyroUncalibrated": + sensor_time = msg.timestamp * 1e-9 + + if not self._validate_sensor_time(sensor_time, t) or not self._validate_timestamp(sensor_time): + return HandleLogResult.TIMING_INVALID + + if not self._validate_sensor_source(msg.source): + return HandleLogResult.SENSOR_SOURCE_INVALID + + v = msg.gyroUncalibrated.v + meas = np.array([-v[2], -v[1], -v[0]]) + + gyro_bias = self.kf.x[States.GYRO_BIAS] + gyro_camodo_yawrate_err = np.abs((meas[2] - gyro_bias[2]) - self.camodo_yawrate_distribution[0]) + gyro_camodo_yawrate_err_threshold = YAWRATE_CROSS_ERR_CHECK_FACTOR * self.camodo_yawrate_distribution[1] + gyro_valid = gyro_camodo_yawrate_err < gyro_camodo_yawrate_err_threshold + + if np.linalg.norm(meas) >= ROTATION_SANITY_CHECK or not gyro_valid: + return HandleLogResult.INPUT_INVALID + + gyro_res = self.kf.predict_and_observe(sensor_time, ObservationKind.PHONE_GYRO, meas) + if gyro_res is not None: + _, new_x, _, new_P, _, _, (gyro_err,), _, _ = gyro_res + self.observation_errors[ObservationKind.PHONE_GYRO] = np.array(gyro_err) + self.observations[ObservationKind.PHONE_GYRO] = meas + + elif which == "carState": + self.car_speed = abs(msg.vEgo) + + elif which == "extrinsicsCalibration": + # Note that we use this message during calibration + calib = get_calibrated_rpy(msg) + if calib is None and len(msg.rpyCalib) > 0: + calib = np.array(msg.rpyCalib) + + if calib is not None: + if calib.min() < -CALIB_RPY_SANITY_CHECK or calib.max() > CALIB_RPY_SANITY_CHECK: + return HandleLogResult.INPUT_INVALID + + self.device_from_calib = rot_from_euler(calib) + + elif which == "cameraOdometry": + if not self._validate_timestamp(t): + return HandleLogResult.TIMING_INVALID + + rot_device = np.matmul(self.device_from_calib, np.array(msg.rot)) + trans_device = np.matmul(self.device_from_calib, np.array(msg.trans)) + + if np.linalg.norm(rot_device) > ROTATION_SANITY_CHECK or np.linalg.norm(trans_device) > TRANS_SANITY_CHECK: + return HandleLogResult.INPUT_INVALID + + rot_calib_std = np.array(msg.rotStd) + trans_calib_std = np.array(msg.transStd) + + if rot_calib_std.min() <= MIN_STD_SANITY_CHECK or trans_calib_std.min() <= MIN_STD_SANITY_CHECK: + return HandleLogResult.INPUT_INVALID + + if np.linalg.norm(rot_calib_std) > 10 * ROTATION_SANITY_CHECK or np.linalg.norm(trans_calib_std) > 10 * TRANS_SANITY_CHECK: + return HandleLogResult.INPUT_INVALID + + self.posenet_stds = np.roll(self.posenet_stds, -1) + self.posenet_stds[-1] = trans_calib_std[0] + + # Multiply by N to avoid to high certainty in kalman filter because of temporally correlated noise + rot_calib_std *= 10 + trans_calib_std *= 2 + + rot_device_std = rotate_std(self.device_from_calib, rot_calib_std) + trans_device_std = rotate_std(self.device_from_calib, trans_calib_std) + rot_device_noise = rot_device_std ** 2 + trans_device_noise = trans_device_std ** 2 + + cam_odo_rot_res = self.kf.predict_and_observe(t, ObservationKind.CAMERA_ODO_ROTATION, rot_device, np.array([np.diag(rot_device_noise)])) + cam_odo_trans_res = self.kf.predict_and_observe(t, ObservationKind.CAMERA_ODO_TRANSLATION, trans_device, np.array([np.diag(trans_device_noise)])) + self.camodo_yawrate_distribution = np.array([rot_device[2], rot_device_std[2]]) + if cam_odo_rot_res is not None: + _, new_x, _, new_P, _, _, (cam_odo_rot_err,), _, _ = cam_odo_rot_res + self.observation_errors[ObservationKind.CAMERA_ODO_ROTATION] = np.array(cam_odo_rot_err) + self.observations[ObservationKind.CAMERA_ODO_ROTATION] = rot_device + if cam_odo_trans_res is not None: + _, new_x, _, new_P, _, _, (cam_odo_trans_err,), _, _ = cam_odo_trans_res + self.observation_errors[ObservationKind.CAMERA_ODO_TRANSLATION] = np.array(cam_odo_trans_err) + self.observations[ObservationKind.CAMERA_ODO_TRANSLATION] = trans_device + + if new_x is not None and new_P is not None: + self._finite_check(t, new_x, new_P) + return HandleLogResult.SUCCESS + + def get_msg(self, sensors_valid: bool, inputs_valid: bool, filter_valid: bool): + state, cov = self.kf.x, self.kf.P + std = np.sqrt(np.diag(cov)) + + orientation_ned, orientation_ned_std = state[States.NED_ORIENTATION], std[States.NED_ORIENTATION] + velocity_device, velocity_device_std = state[States.DEVICE_VELOCITY], std[States.DEVICE_VELOCITY] + angular_velocity_device, angular_velocity_device_std = state[States.ANGULAR_VELOCITY], std[States.ANGULAR_VELOCITY] + acceleration_device, acceleration_device_std = state[States.ACCELERATION], std[States.ACCELERATION] + + msg = messaging.new_message("deviceMotion") + msg.valid = filter_valid + + deviceMotion = msg.deviceMotion + init_xyz_measurement(deviceMotion.orientationNED, orientation_ned, orientation_ned_std, filter_valid) + init_xyz_measurement(deviceMotion.velocityDevice, velocity_device, velocity_device_std, filter_valid) + init_xyz_measurement(deviceMotion.angularVelocityDevice, angular_velocity_device, angular_velocity_device_std, filter_valid) + init_xyz_measurement(deviceMotion.accelerationDevice, acceleration_device, acceleration_device_std, filter_valid) + if self.debug: + deviceMotion.debugFilterState.value = state.tolist() + deviceMotion.debugFilterState.std = std.tolist() + deviceMotion.debugFilterState.valid = filter_valid + deviceMotion.debugFilterState.observations = [ + {'kind': k, 'value': self.observations[k].tolist(), 'error': self.observation_errors[k].tolist()} + for k in self.observations.keys() + ] + + old_mean = np.mean(self.posenet_stds[:POSENET_STD_HIST_HALF]) + new_mean = np.mean(self.posenet_stds[POSENET_STD_HIST_HALF:]) + std_spike = (new_mean / old_mean) > 4.0 and new_mean > 7.0 + + deviceMotion.inputsOK = inputs_valid + deviceMotion.posenetOK = not std_spike or self.car_speed <= 5.0 + deviceMotion.sensorsOK = sensors_valid + + return msg + + +def sensor_all_checks(acc_msgs, gyro_msgs, sensor_valid, sensor_recv_time, sensor_alive, simulation): + cur_time = time.monotonic() + for which, msgs in [("accelerometer", acc_msgs), ("gyroscope", gyro_msgs)]: + if len(msgs) > 0: + sensor_valid[which] = msgs[-1].valid + sensor_recv_time[which] = cur_time + + if not simulation: + sensor_alive[which] = (cur_time - sensor_recv_time[which]) < 0.1 + else: + sensor_alive[which] = len(msgs) > 0 + + return all(sensor_alive.values()) and all(sensor_valid.values()) + + +def main(): + config_realtime_process([0, 1, 2, 3], 5) + + DEBUG = bool(int(os.getenv("DEBUG", "0"))) + SIMULATION = bool(int(os.getenv("SIMULATION", "0"))) + + pm = messaging.PubMaster(['deviceMotion']) + sm = messaging.SubMaster(['carState', 'extrinsicsCalibration', 'cameraOdometry'], poll='cameraOdometry') + # separate sensor sockets for efficiency + sensor_sockets = [messaging.sub_sock(which, timeout=20) for which in ['accelerometer', 'gyroscope']] + sensor_alive, sensor_valid, sensor_recv_time = defaultdict(bool), defaultdict(bool), defaultdict(float) + + params = Params() + + estimator = LocationEstimator(DEBUG) + + filter_initialized = False + critcal_services = ["accelerometer", "gyroscope", "cameraOdometry"] + observation_input_invalid = defaultdict(int) + + input_invalid_limit = {s: round(INPUT_INVALID_LIMIT * (SERVICE_LIST[s].frequency / 20.)) for s in critcal_services} + input_invalid_threshold = {s: input_invalid_limit[s] - 0.5 for s in critcal_services} + input_invalid_decay = {s: calculate_invalid_input_decay(input_invalid_limit[s], INPUT_INVALID_RECOVERY, SERVICE_LIST[s].frequency) for s in critcal_services} + + initial_pose_data = params.get("LocationFilterInitialState") + if initial_pose_data is not None: + with log.Event.from_bytes(initial_pose_data) as lp_msg: + filter_state = lp_msg.deviceMotion.debugFilterState + x_initial = np.array(filter_state.value, dtype=np.float64) if len(filter_state.value) != 0 else PoseKalman.initial_x + P_initial = np.diag(np.array(filter_state.std, dtype=np.float64)) if len(filter_state.std) != 0 else PoseKalman.initial_P + estimator.reset(None, x_initial, P_initial) + + while True: + sm.update() + + acc_msgs, gyro_msgs = (messaging.drain_sock(sock) for sock in sensor_sockets) + + if filter_initialized: + msgs = [] + for msg in acc_msgs + gyro_msgs: + t, valid, which, data = msg.logMonoTime, msg.valid, msg.which(), getattr(msg, msg.which()) + msgs.append((t, valid, which, data)) + for which, updated in sm.updated.items(): + if not updated: + continue + t, valid, data = sm.logMonoTime[which], sm.valid[which], sm[which] + msgs.append((t, valid, which, data)) + + for log_mono_time, valid, which, msg in sorted(msgs, key=lambda x: x[0]): + if valid: + t = log_mono_time * 1e-9 + res = estimator.handle_log(t, which, msg) + if which not in critcal_services: + continue + + if res == HandleLogResult.TIMING_INVALID: + cloudlog.warning(f"Observation {which} ignored due to failed timing check") + observation_input_invalid[which] += 1 + elif res == HandleLogResult.INPUT_INVALID: + cloudlog.warning(f"Observation {which} ignored due to failed sanity check") + observation_input_invalid[which] += 1 + elif res == HandleLogResult.SUCCESS: + observation_input_invalid[which] *= input_invalid_decay[which] + else: + filter_initialized = sm.all_checks() and sensor_all_checks(acc_msgs, gyro_msgs, sensor_valid, sensor_recv_time, sensor_alive, SIMULATION) + + if sm.updated["cameraOdometry"]: + critical_service_inputs_valid = all(observation_input_invalid[s] < input_invalid_threshold[s] for s in critcal_services) + inputs_valid = sm.all_valid() and critical_service_inputs_valid + sensors_valid = sensor_all_checks(acc_msgs, gyro_msgs, sensor_valid, sensor_recv_time, sensor_alive, SIMULATION) + + msg = estimator.get_msg(sensors_valid, inputs_valid, filter_initialized) + pm.send("deviceMotion", msg) + + +if __name__ == "__main__": + main() diff --git a/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/locationd/models/__init__.py b/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/locationd/models/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/locationd/models/constants.py b/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/locationd/models/constants.py new file mode 100644 index 000000000..8e95ff82e --- /dev/null +++ b/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/locationd/models/constants.py @@ -0,0 +1,88 @@ +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] diff --git a/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/locationd/models/pose_kf.py b/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/locationd/models/pose_kf.py new file mode 100644 index 000000000..6dbf0f091 --- /dev/null +++ b/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/locationd/models/pose_kf.py @@ -0,0 +1,67 @@ +""" +Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos +""" + +import numpy as np + +from iqpilot.common.transformations.orientation import euler_from_rot, rot_from_euler +from iqpilot.selfdrive.locationd.models.constants import ObservationKind +from iqpilot.selfdrive.state_estimation import EstimatorModel, ModelDefinition, StateEstimator +try: + from iqpilot.selfdrive.state_estimation.native_binding_pyx import pose_predict, pose_update +except ModuleNotFoundError: + pose_predict = None + pose_update = None + + +EARTH_G = 9.81 + + +class States: + NED_ORIENTATION = slice(0, 3) + DEVICE_VELOCITY = slice(3, 6) + ANGULAR_VELOCITY = slice(6, 9) + GYRO_BIAS = slice(9, 12) + ACCELERATION = slice(12, 15) + ACCEL_BIAS = slice(15, 18) + + +def _transition(state: np.ndarray, dt: float, _: dict[str, float]) -> np.ndarray: + result = state.copy() + result[States.DEVICE_VELOCITY] += dt * state[States.ACCELERATION] + rotation = rot_from_euler(state[States.NED_ORIENTATION]) @ rot_from_euler(dt * state[States.ANGULAR_VELOCITY]) + result[States.NED_ORIENTATION] = euler_from_rot(rotation) + return result + + +def _phone_acceleration(state: np.ndarray, _: dict[str, float]) -> np.ndarray: + device_from_ned = rot_from_euler(state[States.NED_ORIENTATION]).T + centripetal = np.cross(state[States.ANGULAR_VELOCITY], state[States.DEVICE_VELOCITY]) + return device_from_ned @ np.array([0.0, 0.0, -EARTH_G]) + state[States.ACCELERATION] + centripetal + state[States.ACCEL_BIAS] + + +class PoseKalman(EstimatorModel): + name = "pose" + initial_x = np.zeros(18) + initial_P = np.diag([0.01**2] * 3 + [10**2] * 3 + [1**2] * 6 + [100**2] * 3 + [0.01**2] * 3) + Q = np.diag([0.001**2] * 3 + [0.01**2] * 3 + [0.1**2] * 3 + [(0.005 / 100)**2] * 3 + [3**2] * 3 + [0.005**2] * 3) + obs_noise = { + ObservationKind.PHONE_GYRO: np.diag([0.025**2] * 3), + ObservationKind.PHONE_ACCEL: np.diag([0.5**2] * 3), + ObservationKind.CAMERA_ODO_TRANSLATION: np.diag([0.5**2] * 3), + ObservationKind.CAMERA_ODO_ROTATION: np.diag([0.05**2] * 3), + } + + def __init__(self, max_rewind_age: float): + measurements = { + ObservationKind.PHONE_GYRO: lambda state, _: state[States.ANGULAR_VELOCITY] + state[States.GYRO_BIAS], + ObservationKind.PHONE_ACCEL: _phone_acceleration, + ObservationKind.CAMERA_ODO_TRANSLATION: lambda state, _: state[States.DEVICE_VELOCITY], + ObservationKind.CAMERA_ODO_ROTATION: lambda state, _: state[States.ANGULAR_VELOCITY], + } + def native_predict(state, covariance, dt, process_noise, _): + pose_predict(state, covariance, process_noise, dt) + + model = ModelDefinition(18, 18, _transition, measurements, self.Q, self.obs_noise, + native_predict=native_predict if pose_predict is not None else None, native_update=pose_update) + super().__init__(StateEstimator(model, self.initial_x, self.initial_P, max_rewind_age=max_rewind_age)) diff --git a/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/selfdrived/__init__.py b/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/selfdrived/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/selfdrived/alertmanager.py b/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/selfdrived/alertmanager.py new file mode 100644 index 000000000..0ffe29431 --- /dev/null +++ b/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/selfdrived/alertmanager.py @@ -0,0 +1,68 @@ +import copy +import os +import json +from collections import defaultdict +from dataclasses import dataclass + +from iqpilot.common.basedir import BASEDIR +from iqpilot.common.params import Params +from iqpilot.selfdrive.selfdrived.events import Alert +from iqpilot.common.atlas_alerts import NULL_ALERT as EmptyAlert + + +with open(os.path.join(BASEDIR, "iqpilot/selfdrive/selfdrived/alerts_offroad.json")) as f: + OFFROAD_ALERTS = json.load(f) + + +def set_offroad_alert(alert: str, show_alert: bool, extra_text: str | None = None) -> None: + if show_alert: + a = copy.copy(OFFROAD_ALERTS[alert]) + a['extra'] = extra_text or '' + Params().put(alert, a) + else: + Params().remove(alert) + + +@dataclass +class AlertEntry: + alert: Alert | None = None + start_frame: int = -1 + end_frame: int = -1 + added_frame: int = -1 + + def active(self, frame: int) -> bool: + return frame <= self.end_frame + + def just_added(self, frame: int) -> bool: + return self.active(frame) and frame == (self.added_frame + 1) + +class AlertManager: + def __init__(self): + self.alerts: dict[str, AlertEntry] = defaultdict(AlertEntry) + self.current_alert = EmptyAlert + + def add_many(self, frame: int, alerts: list[Alert]) -> None: + for alert in alerts: + entry = self.alerts[alert.alert_type] + entry.alert = alert + if not entry.just_added(frame): + entry.start_frame = frame + min_end_frame = entry.start_frame + alert.duration + entry.end_frame = max(frame + 1, min_end_frame) + entry.added_frame = frame + + def process_alerts(self, frame: int, clear_event_types: set): + ae = AlertEntry() + for v in self.alerts.values(): + if not v.alert: + continue + + if v.alert.event_type in clear_event_types: + v.end_frame = -1 + + # sort by priority first and then by start_frame + greater = ae.alert is None or (v.alert.priority, v.start_frame) > (ae.alert.priority, ae.start_frame) + if v.active(frame) and greater: + ae = v + + self.current_alert = ae.alert if ae.alert is not None else EmptyAlert diff --git a/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/selfdrived/events.py b/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/selfdrived/events.py new file mode 100644 index 000000000..60a37668e --- /dev/null +++ b/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/selfdrived/events.py @@ -0,0 +1,991 @@ +#!/usr/bin/env python3 +import math + +from iqpilot.cereal import log, car +import iqpilot.cereal.messaging as messaging +from iqpilot.common.constants import CV +from iqpilot.common.realtime import DT_CTRL +from iqpilot.selfdrive.locationd.calibrationd import MIN_SPEED_FILTER +from iqpilot.system.micd import SAMPLE_RATE, SAMPLE_BUFFER +from iqpilot.selfdrive.ui.feedback.feedbackd import FEEDBACK_MAX_DURATION +from iqpilot.system.hardware import HARDWARE + +from iqpilot.common.atlas_alerts import EventBook as EventsBase, Tier as Priority, Tags as ET, AlertCard as Alert, \ + NoEntryCard as NoEntryAlert, GentleDisableCard as SoftDisableAlert, PendingDisableCard as UserSoftDisableAlert, \ + HardDisableCard as ImmediateDisableAlert, ChimeCard as EngagementAlert, BannerCard as NormalPermanentAlert, \ + BootCard as StartupAlert, AlertFactory as AlertCallbackType, car_mode_entry_alert as wrong_car_mode_alert + + +AlertSize = log.SelfdriveState.AlertSize +AlertStatus = log.SelfdriveState.AlertStatus +VisualAlert = car.CarControl.HUDControl.VisualAlert +AudibleAlert = car.CarControl.HUDControl.AudibleAlert +EventName = log.OnroadEvent.EventName + + +# get event name from enum +EVENT_NAME = {v: k for k, v in EventName.schema.enumerants.items()} + + +class Events(EventsBase): + def __init__(self): + super().__init__() + self.event_counters = dict.fromkeys(EVENTS.keys(), 0) + + def get_events_mapping(self) -> dict[int, dict[str, Alert | AlertCallbackType]]: + return EVENTS + + def get_event_name(self, event: int): + return EVENT_NAME[event] + + def get_event_msg_type(self): + return log.OnroadEvent + + + +# ********** helper functions ********** +def get_display_speed(speed_ms: float, metric: bool) -> str: + speed = int(round(speed_ms * (CV.MS_TO_KPH if metric else CV.MS_TO_MPH))) + unit = 'km/h' if metric else 'mph' + return f"{speed} {unit}" + + +# ********** alert callback functions ********** + + +def soft_disable_alert(alert_text_2: str) -> AlertCallbackType: + def func(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> Alert: + if soft_disable_time < int(0.5 / DT_CTRL): + return ImmediateDisableAlert(alert_text_2) + return SoftDisableAlert(alert_text_2) + return func + +def user_soft_disable_alert(alert_text_2: str) -> AlertCallbackType: + def func(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> Alert: + if soft_disable_time < int(0.5 / DT_CTRL): + return ImmediateDisableAlert(alert_text_2) + return UserSoftDisableAlert(alert_text_2) + return func + +def below_engage_speed_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> Alert: + return NoEntryAlert(f"Drive above {get_display_speed(CP.minEnableSpeed, metric)} to engage") + + +def below_steer_speed_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> Alert: + return Alert( + f"Steer Assist Unavailable Below {get_display_speed(CP.minSteerSpeed, metric)}", + "", + AlertStatus.userPrompt, AlertSize.small, + Priority.LOW, VisualAlert.none, AudibleAlert.prompt, 0.4) + + +def calibration_incomplete_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> Alert: + first_word = 'Recalibrating' if sm['extrinsicsCalibration'].calStatus == log.ExtrinsicsCalibration.Status.recalibrating else 'Calibrating' + return Alert( + f"{first_word}: {sm['extrinsicsCalibration'].calPerc:.0f}%", + f"Drive Above {get_display_speed(MIN_SPEED_FILTER, metric)}", + AlertStatus.normal, AlertSize.mid, + Priority.LOWEST, VisualAlert.none, AudibleAlert.none, .2) + + +def audio_feedback_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> Alert: + duration = FEEDBACK_MAX_DURATION - ((sm['audioFeedback'].blockNum + 1) * SAMPLE_BUFFER / SAMPLE_RATE) + return NormalPermanentAlert( + "Recording Audio Feedback", + f"{round(duration)} second{'s' if round(duration) != 1 else ''} remaining. Press again to save early.", + priority=Priority.LOW) + + +# *** debug alerts *** + +def out_of_space_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> Alert: + full_perc = round(100. - sm['deviceState'].freeSpacePercent) + return NormalPermanentAlert("Out of Storage", f"{full_perc}% full") + + +def posenet_invalid_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> Alert: + if sm.frame * DT_CTRL < 10.: + return NoEntryAlert("IQModel is starting up", alert_text_1="Please Wait") + mdl = sm['modelV2'].velocity.x[0] if len(sm['modelV2'].velocity.x) else math.nan + err = CS.vEgo - mdl + msg = f"Speed Error: {err:.1f} m/s" + return NoEntryAlert(msg, alert_text_1="Posenet Speed Invalid") + + +def process_not_running_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> Alert: + not_running = [p.name for p in sm['managerState'].processes if not p.running and p.shouldBeRunning] + msg = ', '.join(not_running) + return NoEntryAlert(msg, alert_text_1="Process Not Running") + + +def comm_issue_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> Alert: + bs = [s for s in sm.data.keys() if not sm.all_checks([s, ])] + msg = ', '.join(bs[:4]) # can't fit too many on one line + return NoEntryAlert(msg, alert_text_1="Communication Issue Between Processes") + + +def camera_malfunction_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> Alert: + all_cams = ('roadCameraState', 'driverCameraState', 'wideRoadCameraState') + bad_cams = [s.replace('State', '') for s in all_cams if s in sm.data.keys() and not sm.all_checks([s, ])] + return NormalPermanentAlert("Camera Malfunction", ', '.join(bad_cams)) + + +def calibration_invalid_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> Alert: + rpy = sm['extrinsicsCalibration'].rpyCalib + yaw = math.degrees(rpy[2] if len(rpy) == 3 else math.nan) + pitch = math.degrees(rpy[1] if len(rpy) == 3 else math.nan) + angles = f"Remount Device (Pitch: {pitch:.1f}°, Yaw: {yaw:.1f}°)" + return NormalPermanentAlert("Calibration Invalid", angles) + + +def paramsd_invalid_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> Alert: + if not sm['vehicleParameters'].angleOffsetValid: + angle_offset_deg = sm['vehicleParameters'].angleOffsetDeg + title = "Steering misalignment detected" + text = f"Angle offset too high (Offset: {angle_offset_deg:.1f}°)" + elif not sm['vehicleParameters'].steerRatioValid: + steer_ratio = sm['vehicleParameters'].steerRatio + title = "Steer ratio mismatch" + text = f"Steering rack geometry may be off (Ratio: {steer_ratio:.1f})" + elif not sm['vehicleParameters'].stiffnessFactorValid: + stiffness_factor = sm['vehicleParameters'].stiffnessFactor + title = "Abnormal tire stiffness" + text = f"Check tires, pressure, or alignment (Factor: {stiffness_factor:.1f})" + else: + return NoEntryAlert("paramsd Temporary Error") + + return NoEntryAlert(alert_text_1=title, alert_text_2=text) + +def overheat_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> Alert: + cpu = max(sm['deviceState'].cpuTempC, default=0.) + gpu = max(sm['deviceState'].gpuTempC, default=0.) + temp = max((cpu, gpu, sm['deviceState'].memoryTempC)) + return NormalPermanentAlert("System Overheated", f"{temp:.0f} °C") + + +def low_memory_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> Alert: + return NormalPermanentAlert("Low Memory", f"{sm['deviceState'].memoryUsagePercent}% used") + + +def high_cpu_usage_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> Alert: + x = max(sm['deviceState'].cpuUsagePercent, default=0.) + return NormalPermanentAlert("High CPU Usage", f"{x}% used") + + +def modeld_lagging_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> Alert: + return NormalPermanentAlert("Driving Model Lagging", f"{sm['modelV2'].frameDropPerc:.1f}% frames dropped") + + +def _joystick_axes(sm: messaging.SubMaster) -> tuple[float, float] | None: + if 'testJoystick' not in sm.data or sm.recv_frame['testJoystick'] == 0: + return None + + axes = list(sm['testJoystick'].axes) + if len(axes) < 2: + return None + + return float(axes[0]), float(axes[1]) + + +def joystick_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> Alert: + gb = sm['carControl'].actuators.accel / 4. + if CP.steerControlType in (car.CarParams.SteerControlType.angle, car.CarParams.SteerControlType.curvatureDEPRECATED): + steer = sm['carControl'].actuators.steeringAngleDeg + vals = f"Gas: {round(gb * 100.)}%, Angle: {round(steer, 1)}°" + else: + steer = sm['carControl'].actuators.torque + vals = f"Gas: {round(gb * 100.)}%, Steer: {round(steer * 100.)}%" + return NormalPermanentAlert("Joystick Mode", vals) + + +def longitudinal_maneuver_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> Alert: + ad = sm['alertDebug'] + audible_alert = AudibleAlert.prompt if 'Active' in ad.alertText1 else AudibleAlert.none + alert_status = AlertStatus.userPrompt if 'Active' in ad.alertText1 else AlertStatus.normal + alert_size = AlertSize.mid if ad.alertText2 else AlertSize.small + return Alert(ad.alertText1, ad.alertText2, + alert_status, alert_size, + Priority.LOW, VisualAlert.none, audible_alert, 0.2) + + +def personality_changed_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> Alert: + personality = str(personality).title() + return NormalPermanentAlert(f"Driving Personality: {personality}", duration=1.5) + + +def invalid_lkas_setting_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> Alert: + title = "Invalid LKAS setting" + text = "Toggle stock LKAS on or off to engage" + if CP.brand == "tesla": + title = "Dashcam Mode" + text = "FSD / Autosteer is active" + elif CP.brand == "mazda": + text = "Enable your car's LKAS to engage" + elif CP.brand == "nissan": + text = "Disable your car's stock LKAS to engage" + return NormalPermanentAlert(title, text) + + +def invalid_lkas_setting_no_entry_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, + metric: bool, soft_disable_time: int, personality) -> Alert: + if CP.brand == "tesla": + return NoEntryAlert("FSD / Autosteer is active", alert_text_1="Dashcam Mode") + return NoEntryAlert("Invalid LKAS setting") + + + +EVENTS: dict[int, dict[str, Alert | AlertCallbackType]] = { + # ********** events with no alerts ********** + + EventName.stockFcw: {}, + EventName.actuatorsApiUnavailable: {}, + + # ********** events only containing alerts displayed in all states ********** + + EventName.joystickDebug: { + ET.WARNING: joystick_alert, + ET.PERMANENT: NormalPermanentAlert("Joystick Mode"), + }, + + EventName.longitudinalManeuver: { + ET.WARNING: longitudinal_maneuver_alert, + ET.PERMANENT: NormalPermanentAlert("Longitudinal Maneuver Mode", + "Ensure road ahead is clear"), + }, + + EventName.bigModelLoading: { + ET.NO_ENTRY: NoEntryAlert("Big Model Loading"), + }, + + EventName.bigModelFailed: { + ET.SOFT_DISABLE: soft_disable_alert("Big Model Failed"), + ET.PERMANENT: NormalPermanentAlert("Big Model Failed ", "Restart the car to retry,\nsmall model is still available", duration=20.), + }, + + EventName.lateralManeuver: { + ET.WARNING: longitudinal_maneuver_alert, + ET.PERMANENT: NormalPermanentAlert("Lateral Maneuver Mode"), + }, + + EventName.selfdriveInitializing: { + ET.NO_ENTRY: NoEntryAlert("IQ.Pilot Initializing"), + }, + + EventName.startup: { + ET.PERMANENT: StartupAlert("Welcome to IQ.Pilot!") + }, + + EventName.startupMaster: { + ET.PERMANENT: StartupAlert("Welcome to IQ.Pilot!"), + }, + + EventName.startupNoControl: { + ET.PERMANENT: StartupAlert("Dashcam mode"), + ET.NO_ENTRY: NoEntryAlert("Dashcam mode"), + }, + + EventName.startupNoCar: { + ET.PERMANENT: StartupAlert("IQ.Pilot Dashcam mode: car unrecognized"), + }, + + EventName.startupNoSecOcKey: { + ET.PERMANENT: NormalPermanentAlert("Dashcam Mode", + "TSK Security Key Not Available", + priority=Priority.HIGH), + }, + + EventName.dashcamMode: { + ET.PERMANENT: NormalPermanentAlert("Dashcam Mode", + priority=Priority.LOWEST), + }, + + EventName.invalidLkasSetting: { + ET.PERMANENT: invalid_lkas_setting_alert, + ET.NO_ENTRY: invalid_lkas_setting_no_entry_alert, + }, + + EventName.cruiseMismatch: { + #ET.PERMANENT: ImmediateDisableAlert("openpilot failed to cancel cruise"), + }, + + # openpilot doesn't recognize the car. This switches openpilot into a + # read-only mode. This can be solved by adding your fingerprint. + # See https://github.com/commaai/openpilot/wiki/Fingerprinting for more information + EventName.carUnrecognized: { + ET.PERMANENT: NormalPermanentAlert("Dashcam Mode", + "Car Unrecognized", + priority=Priority.LOWEST), + }, + + EventName.aeb: { + ET.PERMANENT: Alert( + "BRAKE!", + "Emergency Braking: Risk of Collision", + AlertStatus.critical, AlertSize.full, + Priority.HIGHEST, VisualAlert.fcw, AudibleAlert.none, 2.), + ET.NO_ENTRY: NoEntryAlert("AEB: Risk of Collision"), + }, + + EventName.stockAeb: { + ET.PERMANENT: Alert( + "BRAKE!", + "Stock AEB: Risk of Collision", + AlertStatus.critical, AlertSize.full, + Priority.HIGHEST, VisualAlert.fcw, AudibleAlert.none, 2.), + ET.NO_ENTRY: NoEntryAlert("Stock AEB: Risk of Collision"), + }, + + EventName.stockLkas: { + ET.PERMANENT: Alert( + "Stock LKAS: Lane Departure Detected", + "", + AlertStatus.userPrompt, AlertSize.small, + Priority.LOW, VisualAlert.ldw, AudibleAlert.prompt, 3.), + ET.NO_ENTRY: NoEntryAlert("Stock LKAS: Lane Departure Detected"), + }, + + EventName.fcw: { + ET.PERMANENT: Alert( + "BRAKE!", + "Risk of Collision", + AlertStatus.critical, AlertSize.full, + Priority.HIGHEST, VisualAlert.fcw, AudibleAlert.warningSoft, 2.), + }, + + EventName.ldw: { + ET.PERMANENT: Alert( + "Lane Departure Detected", + "", + AlertStatus.userPrompt, AlertSize.small, + Priority.LOW, VisualAlert.ldw, AudibleAlert.prompt, 3.), + }, + + # ********** events only containing alerts that display while engaged ********** + + EventName.steerTempUnavailableSilent: { + ET.WARNING: Alert( + "Steering Assist Temporarily Unavailable", + "", + AlertStatus.userPrompt, AlertSize.small, + Priority.LOW, VisualAlert.steerRequired, AudibleAlert.prompt, 1.8), + }, + + EventName.preDriverDistracted: { + ET.PERMANENT: Alert( + "Pay Attention", + "", + AlertStatus.normal, AlertSize.small, + Priority.LOW, VisualAlert.none, AudibleAlert.none, .1), + }, + + EventName.promptDriverDistracted: { + ET.PERMANENT: Alert( + "Pay Attention", + "Driver Distracted", + AlertStatus.userPrompt, AlertSize.mid, + Priority.MID, VisualAlert.steerRequired, AudibleAlert.promptDistracted, .1), + }, + + EventName.driverDistracted: { + ET.PERMANENT: Alert( + "DISENGAGE IMMEDIATELY", + "Driver Distracted", + AlertStatus.critical, AlertSize.full, + Priority.HIGH, VisualAlert.steerRequired, AudibleAlert.warningImmediate, .1), + }, + + EventName.preDriverUnresponsive: { + ET.PERMANENT: Alert( + "Touch Steering Wheel: No Face Detected", + "", + AlertStatus.normal, AlertSize.small, + Priority.LOW, VisualAlert.steerRequired, AudibleAlert.none, .1), + }, + + EventName.promptDriverUnresponsive: { + ET.PERMANENT: Alert( + "Touch Steering Wheel", + "Driver Unresponsive", + AlertStatus.userPrompt, AlertSize.mid, + Priority.MID, VisualAlert.steerRequired, AudibleAlert.promptDistracted, .1), + }, + + EventName.driverUnresponsive: { + ET.PERMANENT: Alert( + "DISENGAGE IMMEDIATELY", + "Driver Unresponsive", + AlertStatus.critical, AlertSize.full, + Priority.HIGH, VisualAlert.steerRequired, AudibleAlert.warningImmediate, .1), + }, + + EventName.manualRestart: { + ET.WARNING: Alert( + "TAKE CONTROL", + "Resume Driving Manually", + AlertStatus.userPrompt, AlertSize.mid, + Priority.LOW, VisualAlert.none, AudibleAlert.none, .2), + }, + + EventName.resumeRequired: { + ET.WARNING: Alert( + "Press Resume to Exit Standstill", + "", + AlertStatus.userPrompt, AlertSize.small, + Priority.LOW, VisualAlert.none, AudibleAlert.none, .2), + }, + + EventName.belowSteerSpeed: { + ET.WARNING: below_steer_speed_alert, + }, + + EventName.preLaneChangeLeft: { + ET.WARNING: Alert( + "Steer Left to Start Lane Change Once Safe", + "", + AlertStatus.normal, AlertSize.small, + Priority.LOW, VisualAlert.none, AudibleAlert.none, .1), + }, + + EventName.preLaneChangeRight: { + ET.WARNING: Alert( + "Steer Right to Start Lane Change Once Safe", + "", + AlertStatus.normal, AlertSize.small, + Priority.LOW, VisualAlert.none, AudibleAlert.none, .1), + }, + + EventName.laneChangeBlocked: { + ET.WARNING: Alert( + "Car Detected in Blindspot", + "", + AlertStatus.userPrompt, AlertSize.small, + Priority.LOW, VisualAlert.none, AudibleAlert.prompt, .1), + }, + + EventName.laneChange: { + ET.WARNING: Alert( + "Changing Lanes", + "", + AlertStatus.normal, AlertSize.small, + Priority.LOW, VisualAlert.none, AudibleAlert.none, .1), + }, + + EventName.steerSaturated: { + ET.WARNING: Alert( + "Take Control", + "Turn Exceeds Steering Limit", + AlertStatus.userPrompt, AlertSize.mid, + Priority.LOW, VisualAlert.steerRequired, AudibleAlert.promptRepeat, 2.), + }, + + # Thrown when the fan is driven at >50% but is not rotating + EventName.fanMalfunction: { + ET.PERMANENT: NormalPermanentAlert("Fan Malfunction", "Likely Hardware Issue"), + }, + + # Camera is not outputting frames + EventName.cameraMalfunction: { + ET.PERMANENT: camera_malfunction_alert, + ET.SOFT_DISABLE: soft_disable_alert("Camera Malfunction"), + ET.NO_ENTRY: NoEntryAlert("Camera Malfunction: Reboot Your Device"), + }, + # Camera framerate too low + EventName.cameraFrameRate: { + ET.PERMANENT: NormalPermanentAlert("Camera Frame Rate Low", "Reboot your Device"), + ET.SOFT_DISABLE: soft_disable_alert("Camera Frame Rate Low"), + ET.NO_ENTRY: NoEntryAlert("Camera Frame Rate Low: Reboot Your Device"), + }, + + # Unused + + EventName.locationdTemporaryError: { + ET.NO_ENTRY: NoEntryAlert("locationd Temporary Error"), + ET.SOFT_DISABLE: soft_disable_alert("locationd Temporary Error"), + }, + + EventName.locationdPermanentError: { + ET.NO_ENTRY: NoEntryAlert("locationd Permanent Error"), + ET.IMMEDIATE_DISABLE: ImmediateDisableAlert("locationd Permanent Error"), + ET.PERMANENT: NormalPermanentAlert("locationd Permanent Error"), + }, + + # openpilot tries to learn certain parameters about your car by observing + # how the car behaves to steering inputs from both human and openpilot driving. + # This includes: + # - steer ratio: gear ratio of the steering rack. Steering angle divided by tire angle + # - tire stiffness: how much grip your tires have + # - angle offset: most steering angle sensors are offset and measure a non zero angle when driving straight + # This alert is thrown when any of these values exceed a sanity check. This can be caused by + # bad alignment or bad sensor data. If this happens consistently consider creating an issue on GitHub + EventName.paramsdTemporaryError: { + ET.NO_ENTRY: paramsd_invalid_alert, + ET.SOFT_DISABLE: soft_disable_alert("paramsd Temporary Error"), + }, + + EventName.paramsdPermanentError: { + ET.NO_ENTRY: NoEntryAlert("paramsd Permanent Error"), + ET.IMMEDIATE_DISABLE: ImmediateDisableAlert("paramsd Permanent Error"), + ET.PERMANENT: NormalPermanentAlert("paramsd Permanent Error"), + }, + + # ********** events that affect controls state transitions ********** + + EventName.pcmEnable: { + ET.ENABLE: EngagementAlert(AudibleAlert.engage), + }, + + EventName.buttonEnable: { + ET.ENABLE: EngagementAlert(AudibleAlert.engage), + }, + + EventName.pcmDisable: { + ET.USER_DISABLE: EngagementAlert(AudibleAlert.disengage), + }, + + EventName.buttonCancel: { + ET.USER_DISABLE: EngagementAlert(AudibleAlert.disengage), + ET.NO_ENTRY: NoEntryAlert("Cancel Pressed"), + }, + + EventName.brakeHold: { + ET.WARNING: Alert( + "Press Resume to Exit Brake Hold", + "", + AlertStatus.userPrompt, AlertSize.small, + Priority.LOW, VisualAlert.none, AudibleAlert.none, .2), + }, + + EventName.parkBrake: { + ET.USER_DISABLE: EngagementAlert(AudibleAlert.disengage), + ET.NO_ENTRY: NoEntryAlert("Parking Brake Engaged"), + }, + + EventName.pedalPressed: { + ET.USER_DISABLE: EngagementAlert(AudibleAlert.disengage), + ET.NO_ENTRY: NoEntryAlert("Pedal Pressed", + visual_alert=VisualAlert.brakePressed), + }, + + EventName.steerDisengage: { + ET.USER_DISABLE: EngagementAlert(AudibleAlert.disengage), + ET.NO_ENTRY: NoEntryAlert("Steering Pressed"), + }, + + EventName.preEnableStandstill: { + ET.PRE_ENABLE: Alert( + "Release Brake to Engage", + "", + AlertStatus.normal, AlertSize.small, + Priority.LOWEST, VisualAlert.none, AudibleAlert.none, .1, creation_delay=1.), + }, + + EventName.gasPressedOverride: { + ET.OVERRIDE_LONGITUDINAL: Alert( + "", + "", + AlertStatus.normal, AlertSize.none, + Priority.LOWEST, VisualAlert.none, AudibleAlert.none, .1), + }, + + EventName.steerOverride: { + ET.OVERRIDE_LATERAL: Alert( + "", + "", + AlertStatus.normal, AlertSize.none, + Priority.LOWEST, VisualAlert.none, AudibleAlert.none, .1), + }, + + EventName.wrongCarMode: { + ET.USER_DISABLE: EngagementAlert(AudibleAlert.disengage), + ET.NO_ENTRY: wrong_car_mode_alert, + }, + + EventName.resumeBlocked: { + ET.NO_ENTRY: NoEntryAlert("Press Set to Engage"), + }, + + EventName.wrongCruiseMode: { + ET.USER_DISABLE: EngagementAlert(AudibleAlert.disengage), + ET.NO_ENTRY: NoEntryAlert("Adaptive Cruise Disabled"), + }, + + EventName.steerTempUnavailable: { + ET.SOFT_DISABLE: soft_disable_alert("Steering Assist Temporarily Unavailable"), + ET.NO_ENTRY: NoEntryAlert("Steering Temporarily Unavailable"), + }, + + EventName.steerTimeLimit: { + ET.SOFT_DISABLE: soft_disable_alert("Vehicle Steering Time Limit"), + ET.NO_ENTRY: NoEntryAlert("Vehicle Steering Time Limit"), + }, + + EventName.outOfSpace: { + ET.PERMANENT: out_of_space_alert, + ET.NO_ENTRY: NoEntryAlert("Out of Storage"), + }, + + EventName.belowEngageSpeed: { + ET.NO_ENTRY: below_engage_speed_alert, + }, + + EventName.sensorDataInvalid: { + ET.PERMANENT: Alert( + "Sensor Data Invalid", + "Possible Hardware Issue", + AlertStatus.normal, AlertSize.mid, + Priority.LOWER, VisualAlert.none, AudibleAlert.none, .2, creation_delay=1.), + ET.NO_ENTRY: NoEntryAlert("Sensor Data Invalid"), + ET.SOFT_DISABLE: soft_disable_alert("Sensor Data Invalid"), + }, + + EventName.noGps: { + }, + + EventName.tooDistracted: { + ET.NO_ENTRY: NoEntryAlert("Distraction Level Too High"), + }, + + EventName.excessiveActuation: { + ET.SOFT_DISABLE: soft_disable_alert("Excessive Actuation"), + ET.NO_ENTRY: NoEntryAlert("Excessive Actuation"), + }, + + EventName.overheat: { + ET.PERMANENT: overheat_alert, + ET.SOFT_DISABLE: soft_disable_alert("System Overheated"), + ET.NO_ENTRY: NoEntryAlert("System Overheated"), + }, + + EventName.wrongGear: { + ET.SOFT_DISABLE: Alert( + "", + "", + AlertStatus.normal, AlertSize.none, + Priority.LOWEST, VisualAlert.none, AudibleAlert.none, 0.), + ET.NO_ENTRY: Alert( + "", + "", + AlertStatus.normal, AlertSize.none, + Priority.LOWEST, VisualAlert.none, AudibleAlert.none, 0.), + }, + + # This alert is thrown when the calibration angles are outside of the acceptable range. + # For example if the device is pointed too much to the left or the right. + # Usually this can only be solved by removing the mount from the windshield completely, + # and attaching while making sure the device is pointed straight forward and is level. + # See https://comma.ai/setup for more information + EventName.calibrationInvalid: { + ET.PERMANENT: calibration_invalid_alert, + ET.SOFT_DISABLE: soft_disable_alert("Calibration Invalid: Remount Device & Recalibrate"), + ET.NO_ENTRY: NoEntryAlert("Calibration Invalid: Remount Device & Recalibrate"), + }, + + EventName.calibrationIncomplete: { + ET.PERMANENT: calibration_incomplete_alert, + ET.SOFT_DISABLE: soft_disable_alert("Calibration Incomplete"), + ET.NO_ENTRY: NoEntryAlert("Calibration in Progress"), + }, + + EventName.calibrationRecalibrating: { + ET.PERMANENT: calibration_incomplete_alert, + ET.SOFT_DISABLE: soft_disable_alert("Device Remount Detected: Recalibrating"), + ET.NO_ENTRY: NoEntryAlert("Remount Detected: Recalibrating"), + }, + + EventName.doorOpen: { + ET.SOFT_DISABLE: user_soft_disable_alert("Door Open"), + ET.NO_ENTRY: NoEntryAlert("Door Open"), + }, + + EventName.seatbeltNotLatched: { + ET.SOFT_DISABLE: user_soft_disable_alert("Seatbelt Unlatched"), + ET.NO_ENTRY: NoEntryAlert("Seatbelt Unlatched"), + }, + + EventName.espDisabled: { + ET.SOFT_DISABLE: soft_disable_alert("Electronic Stability Control Disabled"), + ET.NO_ENTRY: NoEntryAlert("Electronic Stability Control Disabled"), + }, + + EventName.lowBattery: { + ET.SOFT_DISABLE: soft_disable_alert("Low Battery"), + ET.NO_ENTRY: NoEntryAlert("Low Battery"), + }, + + # Different openpilot services communicate between each other at a certain + # interval. If communication does not follow the regular schedule this alert + # is thrown. This can mean a service crashed, did not broadcast a message for + # ten times the regular interval, or the average interval is more than 10% too high. + # Soft warnings — no disable, no entry block. UI shows a silent yellow triangle instead. + EventName.commIssue: { + }, + EventName.commIssueAvgFreq: { + }, + + EventName.selfdrivedLagging: { + }, + + # Thrown when manager detects a service exited unexpectedly while driving + EventName.processNotRunning: { + ET.NO_ENTRY: process_not_running_alert, + ET.SOFT_DISABLE: soft_disable_alert("Process Not Running"), + }, + + EventName.radarFault: { + ET.SOFT_DISABLE: soft_disable_alert("Radar Error: Restart the Car"), + ET.NO_ENTRY: NoEntryAlert("Radar Error: Restart the Car"), + }, + + EventName.radarTempUnavailable: { + ET.SOFT_DISABLE: soft_disable_alert("Radar Temporarily Unavailable"), + ET.NO_ENTRY: NoEntryAlert("Radar Temporarily Unavailable"), + }, + + # Every frame from the camera should be processed by the model. If modeld + # is not processing frames fast enough they have to be dropped. This alert is + # thrown when over 20% of frames are dropped. + EventName.modeldLagging: { + ET.SOFT_DISABLE: soft_disable_alert("Driving Model Lagging"), + ET.NO_ENTRY: NoEntryAlert("Driving Model Lagging"), + ET.PERMANENT: modeld_lagging_alert, + }, + + # Besides predicting the path, lane lines and lead car data the model also + # predicts the current velocity and rotation speed of the car. If the model is + # very uncertain about the current velocity while the car is moving, this + # usually means the model has trouble understanding the scene. This is used + # as a heuristic to warn the driver. + EventName.posenetInvalid: { + ET.SOFT_DISABLE: soft_disable_alert("Posenet Speed Invalid"), + ET.NO_ENTRY: posenet_invalid_alert, + }, + + # When the localizer detects an acceleration of more than 40 m/s^2 (~4G) we + # alert the driver the device might have fallen from the windshield. + EventName.deviceFalling: { + ET.SOFT_DISABLE: soft_disable_alert("Device Fell Off Mount"), + ET.NO_ENTRY: NoEntryAlert("Device Fell Off Mount"), + }, + + EventName.lowMemory: { + ET.SOFT_DISABLE: soft_disable_alert("Low Memory: Reboot Your Device"), + ET.PERMANENT: low_memory_alert, + ET.NO_ENTRY: NoEntryAlert("Low Memory: Reboot Your Device"), + }, + + EventName.accFaulted: { + ET.IMMEDIATE_DISABLE: ImmediateDisableAlert("Cruise Fault: Restart the Car"), + ET.PERMANENT: NormalPermanentAlert("Cruise Fault: Restart the car to engage"), + ET.NO_ENTRY: NoEntryAlert("Cruise Fault: Restart the Car"), + }, + + EventName.cruiseFaultLateralAllowed: { + ET.PERMANENT: NormalPermanentAlert("Cruise Faulted", "Lane Assist will continue to work", priority=Priority.LOWEST), + }, + + EventName.espActive: { + ET.SOFT_DISABLE: soft_disable_alert("Electronic Stability Control Active"), + ET.NO_ENTRY: NoEntryAlert("Electronic Stability Control Active"), + }, + + EventName.controlsMismatch: { + ET.IMMEDIATE_DISABLE: ImmediateDisableAlert("Controls Mismatch"), + ET.NO_ENTRY: NoEntryAlert("Controls Mismatch"), + }, + + # Sometimes the USB stack on the device can get into a bad state + # causing the connection to the panda to be lost + EventName.usbError: { + ET.SOFT_DISABLE: soft_disable_alert("USB Error: Reboot Your Device"), + ET.PERMANENT: NormalPermanentAlert("USB Error: Reboot Your Device"), + ET.NO_ENTRY: NoEntryAlert("USB Error: Reboot Your Device"), + }, + + # This alert can be thrown for the following reasons: + # - No CAN data received at all + # - CAN data is received, but some message are not received at the right frequency + # If you're not writing a new car port, this is usually cause by faulty wiring + # Minor canError: low-priority silent HUD badge only — no disable, no entry block, no audible/visual alert + EventName.canError: { + ET.PERMANENT: Alert( + "CAN Error", + "", + AlertStatus.normal, AlertSize.small, + Priority.LOW, VisualAlert.none, AudibleAlert.none, 1., creation_delay=1.), + }, + + EventName.canBusMissing: { + ET.IMMEDIATE_DISABLE: ImmediateDisableAlert("CAN Bus Disconnected"), + ET.PERMANENT: Alert( + "CAN Bus Disconnected: Likely Faulty Cable", + "", + AlertStatus.normal, AlertSize.small, + Priority.LOW, VisualAlert.none, AudibleAlert.none, 1., creation_delay=1.), + ET.NO_ENTRY: NoEntryAlert("CAN Bus Disconnected: Check Connections"), + }, + + EventName.steerUnavailable: { + ET.IMMEDIATE_DISABLE: ImmediateDisableAlert("LKAS Fault: Restart the Car"), + ET.PERMANENT: NormalPermanentAlert("LKAS Fault: Restart the car to engage"), + ET.NO_ENTRY: NoEntryAlert("LKAS Fault: Restart the Car"), + }, + + EventName.reverseGear: { + ET.PERMANENT: Alert( + "Reverse\nGear", + "", + AlertStatus.normal, AlertSize.full, + Priority.LOWEST, VisualAlert.none, AudibleAlert.none, .2, creation_delay=0.5), + ET.USER_DISABLE: ImmediateDisableAlert("Reverse Gear"), + ET.NO_ENTRY: NoEntryAlert("Reverse Gear"), + }, + + # On cars that use stock ACC the car can decide to cancel ACC for various reasons. + # When this happens we can no long control the car so the user needs to be warned immediately. + EventName.cruiseDisabled: { + ET.IMMEDIATE_DISABLE: ImmediateDisableAlert("Cruise Is Off"), + }, + + # When the relay in the harness box opens the CAN bus between the LKAS camera + # and the rest of the car is separated. When messages from the LKAS camera + # are received on the car side this usually means the relay hasn't opened correctly + # and this alert is thrown. + EventName.relayMalfunction: { + ET.IMMEDIATE_DISABLE: ImmediateDisableAlert("Harness Relay Malfunction"), + ET.PERMANENT: NormalPermanentAlert("Harness Relay Malfunction", "Check Hardware"), + ET.NO_ENTRY: NoEntryAlert("Harness Relay Malfunction"), + }, + + EventName.speedTooLow: { + ET.IMMEDIATE_DISABLE: Alert( + "IQ.Pilot Canceled", + "Speed too low", + AlertStatus.normal, AlertSize.mid, + Priority.HIGH, VisualAlert.none, AudibleAlert.disengage, 3.), + }, + + # When the car is driving faster than most cars in the training data, the model outputs can be unpredictable. + EventName.speedTooHigh: { + ET.WARNING: Alert( + "Speed Too High", + "Model uncertain at this speed", + AlertStatus.userPrompt, AlertSize.mid, + Priority.HIGH, VisualAlert.steerRequired, AudibleAlert.promptRepeat, 4.), + ET.NO_ENTRY: NoEntryAlert("Slow down to engage"), + }, + + EventName.vehicleSensorsInvalid: { + ET.IMMEDIATE_DISABLE: ImmediateDisableAlert("Vehicle Sensors Invalid"), + ET.PERMANENT: NormalPermanentAlert("Vehicle Sensors Calibrating", "Drive to Calibrate"), + ET.NO_ENTRY: NoEntryAlert("Vehicle Sensors Calibrating"), + }, + + EventName.personalityChanged: { + ET.WARNING: personality_changed_alert, + }, + + EventName.userBookmark: { + ET.PERMANENT: NormalPermanentAlert("Bookmark Saved", duration=1.5), + }, + + EventName.audioFeedback: { + ET.PERMANENT: audio_feedback_alert, + }, +} + + +if HARDWARE.get_device_type() == 'mici': + EVENTS.update({ + EventName.preDriverDistracted: { + ET.PERMANENT: Alert( + "Pay Attention", + "", + AlertStatus.normal, AlertSize.small, + Priority.LOW, VisualAlert.none, AudibleAlert.none, 2), + }, + EventName.promptDriverDistracted: { + ET.PERMANENT: Alert( + "Pay Attention", + "Driver Distracted", + AlertStatus.userPrompt, AlertSize.mid, + Priority.MID, VisualAlert.steerRequired, AudibleAlert.promptDistracted, 1), + }, + EventName.resumeRequired: { + ET.WARNING: Alert( + "Press Resume", + "", + AlertStatus.userPrompt, AlertSize.small, + Priority.LOW, VisualAlert.none, AudibleAlert.none, .2), + }, + EventName.preLaneChangeLeft: { + ET.WARNING: Alert( + "Steer Left", + "Confirm Lane Change", + AlertStatus.normal, AlertSize.mid, + Priority.LOW, VisualAlert.none, AudibleAlert.none, .1), + }, + EventName.preLaneChangeRight: { + ET.WARNING: Alert( + "Steer Right", + "Confirm Lane Change", + AlertStatus.normal, AlertSize.mid, + Priority.LOW, VisualAlert.none, AudibleAlert.none, .1), + }, + EventName.laneChangeBlocked: { + ET.WARNING: Alert( + "Car in Blindspot", + "", + AlertStatus.userPrompt, AlertSize.small, + Priority.LOW, VisualAlert.none, AudibleAlert.prompt, .1), + }, + EventName.steerSaturated: { + ET.WARNING: Alert( + "take control", + "turn exceeds limit", + AlertStatus.userPrompt, AlertSize.mid, + Priority.LOW, VisualAlert.steerRequired, AudibleAlert.promptRepeat, 2.), + }, + EventName.calibrationIncomplete: { + ET.PERMANENT: calibration_incomplete_alert, + ET.SOFT_DISABLE: soft_disable_alert("Calibration Incomplete"), + ET.NO_ENTRY: NoEntryAlert("Calibrating"), + }, + EventName.reverseGear: { + ET.PERMANENT: Alert( + "Reverse", + "", + AlertStatus.normal, AlertSize.full, + Priority.LOWEST, VisualAlert.none, AudibleAlert.none, .2, creation_delay=0.5), + ET.USER_DISABLE: ImmediateDisableAlert("Reverse"), + ET.NO_ENTRY: NoEntryAlert("Reverse"), + }, + }) + + +if __name__ == '__main__': + # print all alerts by type and priority + from iqpilot.cereal.services import SERVICE_LIST + from collections import defaultdict + + event_names = {v: k for k, v in EventName.schema.enumerants.items()} + alerts_by_type: dict[str, dict[Priority, list[str]]] = defaultdict(lambda: defaultdict(list)) + + CP = car.CarParams.new_message() + CS = car.CarState.new_message() + sm = messaging.SubMaster(list(SERVICE_LIST.keys())) + + for i, alerts in EVENTS.items(): + for et, alert in alerts.items(): + if callable(alert): + alert = alert(CP, CS, sm, False, 1, log.LongitudinalPersonality.standard) + alerts_by_type[et][alert.priority].append(event_names[i]) + + all_alerts: dict[str, list[tuple[Priority, list[str]]]] = {} + for et, priority_alerts in alerts_by_type.items(): + all_alerts[et] = sorted(priority_alerts.items(), key=lambda x: x[0], reverse=True) + + for status, evs in sorted(all_alerts.items(), key=lambda x: x[0]): + print(f"**** {status} ****") + for p, alert_list in evs: + print(f" {repr(p)}:") + print(" ", ', '.join(alert_list), "\n") diff --git a/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/state_estimation/__init__.py b/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/state_estimation/__init__.py new file mode 100644 index 000000000..958a822d4 --- /dev/null +++ b/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/state_estimation/__init__.py @@ -0,0 +1,7 @@ +""" +Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos +""" + +from iqpilot.selfdrive.state_estimation.estimator import EstimatorModel, ModelDefinition, Observation, StateEstimator + +__all__ = ["EstimatorModel", "ModelDefinition", "Observation", "StateEstimator"] diff --git a/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/state_estimation/estimator.py b/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/state_estimation/estimator.py new file mode 100644 index 000000000..696bdd737 --- /dev/null +++ b/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/state_estimation/estimator.py @@ -0,0 +1,311 @@ +""" +Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos +""" + +from collections.abc import Callable +from dataclasses import dataclass +import math + +import numpy as np + + +Array = np.ndarray +Prediction = Callable[[Array, float, dict[str, float]], Array] +Measurement = Callable[[Array, dict[str, float]], Array] +Injection = Callable[[Array, Array], Array] +NativePrediction = Callable[[Array, Array, float, Array, dict[str, float]], None] +NativeUpdate = Callable[[Array, Array, int, Array, Array], Array] + + +@dataclass(frozen=True) +class Observation: + kind: int + values: Array + noise: Array + + +@dataclass(frozen=True) +class ModelDefinition: + state_size: int + error_size: int + transition: Prediction + measurements: dict[int, Measurement] + process_noise: Array + observation_noise: dict[int, Array] + inject_error: Injection | None = None + error_projection: Callable[[Array], Array] | None = None + normalize: Callable[[Array], Array] | None = None + native_predict: NativePrediction | None = None + native_update: NativeUpdate | None = None + + +@dataclass +class _Snapshot: + time: float + state: Array + covariance: Array + + +@dataclass +class _Event: + time: float + observation: Observation + order: int + + +class StateEstimator: + def __init__(self, model: ModelDefinition, initial_state: Array, initial_covariance: Array, + max_rewind_age: float = 0.0): + self.model = model + self.parameters: dict[str, float] = {} + self.max_rewind_age = max_rewind_age + self._order = 0 + self.init_state(initial_state, initial_covariance, None) + + @property + def x(self) -> Array: + return self._state.copy() + + @property + def P(self) -> Array: + return self._covariance.copy() + + @property + def t(self) -> float: + return self._time + + def set_global(self, name: str, value: float) -> None: + self.parameters[name] = float(value) + + def init_state(self, state: Array, covs: Array, filter_time: float | None) -> None: + state = np.asarray(state, dtype=np.float64).reshape(-1) + covariance = np.asarray(covs, dtype=np.float64) + self._validate_state(state, covariance) + self._state = self._normalize(state.copy()) + self._covariance = self._stabilize(covariance.copy()) + self._time = math.nan if filter_time is None else float(filter_time) + self._events: list[_Event] = [] + self._snapshots = [_Snapshot(self._time, self._state.copy(), self._covariance.copy())] + + def set_filter_time(self, filter_time: float | None) -> None: + self._time = math.nan if filter_time is None else float(filter_time) + + def reset_rewind(self) -> None: + self._events.clear() + self._snapshots = [_Snapshot(self._time, self._state.copy(), self._covariance.copy())] + + def predict(self, time: float) -> None: + time = float(time) + if math.isnan(self._time): + self._time = time + return + if time < self._time: + raise ValueError("prediction time precedes estimator time") + dt = time - self._time + if dt == 0.0: + return + if self.model.native_predict is not None: + self.model.native_predict(self._state, self._covariance, dt, self.model.process_noise, self.parameters) + self._time = time + return + previous = self._state.copy() + transition_jacobian = self._jacobian(lambda value: self.model.transition(value, dt, self.parameters), previous) + predicted = self.model.transition(previous, dt, self.parameters) + projection = self._error_projection(previous) + if self.model.error_size == self.model.state_size: + error_transition = transition_jacobian + else: + error_transition = np.linalg.pinv(self._error_projection(predicted)) @ transition_jacobian @ projection + self._state = self._normalize(predicted) + self._covariance = self._stabilize(error_transition @ self._covariance @ error_transition.T + dt * self.model.process_noise) + self._time = time + + def predict_and_observe(self, time: float, kind: int, measurements: Array, noise: Array | None = None): + values = self._measurement_batch(kind, measurements) + noises = self._noise_batch(kind, len(values), noise) + event = _Event(float(time), Observation(kind, values, noises), self._order) + self._order += 1 + if not math.isnan(self._time) and event.time < self._time: + if self.max_rewind_age <= 0.0 or self._time - event.time > self.max_rewind_age: + return None + return self._rewind(event) + result = self._apply_event(event) + self._events.append(event) + self._snapshots.append(_Snapshot(self._time, self._state.copy(), self._covariance.copy())) + self._trim_history() + return result + + def _apply_event(self, event: _Event): + self.predict(event.time) + prior_state = self._state.copy() + prior_covariance = self._covariance.copy() + innovations = [] + for measurement, noise in zip(event.observation.values, event.observation.noise, strict=True): + innovations.append(self._update(event.observation.kind, measurement, noise)) + return (event.time, self.x, prior_state, self.P, prior_covariance, event.observation.kind, + tuple(innovations), event.observation.values.copy(), event.observation.noise.copy()) + + def _update(self, kind: int, measurement: Array, noise: Array) -> Array: + measurement_function = self.model.measurements.get(kind) + if measurement_function is None: + raise KeyError(f"unknown observation kind {kind}") + measurement = np.asarray(measurement, dtype=np.float64).reshape(-1) + if noise.shape != (measurement.size, measurement.size): + raise ValueError("observation noise dimension mismatch") + if self.model.native_update is not None: + innovation = self.model.native_update(self._state, self._covariance, kind, measurement, noise) + return innovation + predicted = np.asarray(measurement_function(self._state, self.parameters), dtype=np.float64).reshape(-1) + if predicted.shape != measurement.shape: + raise ValueError("measurement dimension mismatch") + innovation = measurement - predicted + state_jacobian = self._jacobian(lambda value: measurement_function(value, self.parameters), self._state) + observation_jacobian = state_jacobian @ self._error_projection(self._state) + innovation_covariance = observation_jacobian @ self._covariance @ observation_jacobian.T + noise + gain = np.linalg.solve(innovation_covariance, observation_jacobian @ self._covariance).T + delta = gain @ innovation + self._state = self._normalize(self._inject(self._state, delta)) + identity = np.eye(self.model.error_size) + residual = identity - gain @ observation_jacobian + self._covariance = self._stabilize(residual @ self._covariance @ residual.T + gain @ noise @ gain.T) + self._require_finite() + return innovation + + def _rewind(self, new_event: _Event): + events = sorted(self._events + [new_event], key=lambda event: (event.time, event.order)) + base_index = max(i for i, snapshot in enumerate(self._snapshots) if math.isnan(snapshot.time) or snapshot.time <= new_event.time) + base = self._snapshots[base_index] + retained = self._events[:base_index] + retained_orders = {event.order for event in retained} + replay = [event for event in events if event.order not in retained_orders] + self._state = base.state.copy() + self._covariance = base.covariance.copy() + self._time = base.time + self._events = retained.copy() + self._snapshots = self._snapshots[:base_index + 1] + result = None + for event in replay: + current = self._apply_event(event) + self._events.append(event) + self._snapshots.append(_Snapshot(self._time, self._state.copy(), self._covariance.copy())) + if event is new_event: + result = current + self._trim_history() + return result + + def _trim_history(self) -> None: + if self.max_rewind_age <= 0.0 or math.isnan(self._time): + return + cutoff = self._time - self.max_rewind_age + remove = 0 + while remove < len(self._events) and self._events[remove].time < cutoff: + remove += 1 + if remove: + self._events = self._events[remove:] + self._snapshots = self._snapshots[remove:] + + def _measurement_batch(self, kind: int, measurements: Array) -> Array: + measurement_function = self.model.measurements.get(kind) + if measurement_function is None: + raise KeyError(f"unknown observation kind {kind}") + if self.model.native_update is not None and kind in self.model.observation_noise: + expected = self.model.observation_noise[kind].shape[0] + else: + expected = np.asarray(measurement_function(self._state, self.parameters)).size + values = np.asarray(measurements, dtype=np.float64) + if values.ndim == 1: + values = values.reshape(1, -1) + elif values.ndim != 2: + raise ValueError("measurements must be one or two dimensional") + if values.shape[1] != expected: + raise ValueError("measurement dimension mismatch") + return values + + def _noise_batch(self, kind: int, count: int, noise: Array | None) -> Array: + if noise is None: + base = self.model.observation_noise.get(kind) + if base is None: + raise KeyError(f"missing observation noise for kind {kind}") + return np.repeat(np.asarray(base, dtype=np.float64)[None, :, :], count, axis=0) + noises = np.asarray(noise, dtype=np.float64) + if noises.ndim == 2: + noises = noises[None, :, :] + if noises.shape[0] == 1 and count > 1: + noises = np.repeat(noises, count, axis=0) + if noises.shape[0] != count: + raise ValueError("observation noise batch mismatch") + return noises + + def _jacobian(self, function: Callable[[Array], Array], value: Array) -> Array: + output = np.asarray(function(value), dtype=np.float64).reshape(-1) + result = np.empty((output.size, value.size), dtype=np.float64) + for index in range(value.size): + step = np.cbrt(np.finfo(np.float64).eps) * max(1.0, abs(value[index])) + upper = value.copy() + lower = value.copy() + upper[index] += step + lower[index] -= step + result[:, index] = (np.asarray(function(upper)).reshape(-1) - np.asarray(function(lower)).reshape(-1)) / (2.0 * step) + return result + + def _inject(self, state: Array, delta: Array) -> Array: + if self.model.inject_error is None: + return state + delta + return self.model.inject_error(state, delta) + + def _error_projection(self, state: Array) -> Array: + if self.model.error_projection is None: + return np.eye(self.model.state_size, self.model.error_size) + return np.asarray(self.model.error_projection(state), dtype=np.float64) + + def _normalize(self, state: Array) -> Array: + if self.model.normalize is None: + return np.asarray(state, dtype=np.float64).reshape(-1) + return np.asarray(self.model.normalize(state), dtype=np.float64).reshape(-1) + + def _stabilize(self, covariance: Array) -> Array: + covariance = (covariance + covariance.T) * 0.5 + eigenvalues, eigenvectors = np.linalg.eigh(covariance) + if eigenvalues[0] < -1e-10: + raise FloatingPointError("covariance is not positive semidefinite") + return (eigenvectors * np.maximum(eigenvalues, 0.0)) @ eigenvectors.T + + def _validate_state(self, state: Array, covariance: Array) -> None: + if state.shape != (self.model.state_size,): + raise ValueError("state dimension mismatch") + if covariance.shape != (self.model.error_size, self.model.error_size): + raise ValueError("covariance dimension mismatch") + if self.model.process_noise.shape != covariance.shape: + raise ValueError("process noise dimension mismatch") + if not np.isfinite(state).all() or not np.isfinite(covariance).all(): + raise ValueError("state and covariance must be finite") + + def _require_finite(self) -> None: + if not np.isfinite(self._state).all() or not np.isfinite(self._covariance).all(): + raise FloatingPointError("estimator produced non-finite values") + + +class EstimatorModel: + def __init__(self, estimator: StateEstimator): + self.filter = estimator + + @property + def x(self) -> Array: + return self.filter.x + + @property + def P(self) -> Array: + return self.filter.P + + @property + def t(self) -> float: + return self.filter.t + + def init_state(self, state: Array, covs: Array, filter_time: float | None) -> None: + self.filter.init_state(state, covs, filter_time) + + def predict(self, time: float) -> None: + self.filter.predict(time) + + def predict_and_observe(self, time: float, kind: int, measurements: Array, noise: Array | None = None): + return self.filter.predict_and_observe(time, kind, measurements, noise) diff --git a/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/ui/__init__.py b/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/ui/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/ui/feedback/__init__.py b/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/ui/feedback/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/ui/feedback/feedbackd.py b/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/ui/feedback/feedbackd.py new file mode 100644 index 000000000..0412edd32 --- /dev/null +++ b/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/ui/feedback/feedbackd.py @@ -0,0 +1,70 @@ +#!/usr/bin/env python3 +import iqpilot.cereal.messaging as messaging +from iqpilot.common.params import Params +from iqpilot.common.swaglog import cloudlog +from iqpilot.cereal import car +from iqpilot.system.micd import SAMPLE_RATE, SAMPLE_BUFFER + +FEEDBACK_MAX_DURATION = 10.0 +ButtonType = car.CarState.ButtonEvent.Type + + +def main(): + params = Params() + pm = messaging.PubMaster(['userBookmark', 'audioFeedback']) + sm = messaging.SubMaster(['rawAudioData', 'bookmarkButton']) + should_record_audio = False + block_num = 0 + waiting_for_release = False + early_stop_triggered = False + + while True: + sm.update() + should_send_bookmark = False + + if False and sm.updated['carState'] and sm['carState'].canValid and not sm['iqState'].aol.available: + for be in sm['carState'].buttonEvents: + if be.type == ButtonType.lkas: + if be.pressed: + if not should_record_audio: + if params.get_bool("RecordAudioFeedback"): + should_record_audio = True + block_num = 0 + waiting_for_release = False + early_stop_triggered = False + cloudlog.info("LKAS button pressed - starting 10-second audio feedback") + else: + should_send_bookmark = True + cloudlog.info("LKAS button pressed - bookmarking") + elif should_record_audio and not waiting_for_release: + waiting_for_release = True + elif waiting_for_release: + waiting_for_release = False + early_stop_triggered = True + cloudlog.info("LKAS button released - ending recording early") + + if should_record_audio and sm.updated['rawAudioData']: + raw_audio = sm['rawAudioData'] + msg = messaging.new_message('audioFeedback', valid=True) + msg.audioFeedback.audio.data = raw_audio.data + msg.audioFeedback.audio.sampleRate = raw_audio.sampleRate + msg.audioFeedback.blockNum = block_num + block_num += 1 + if (block_num * SAMPLE_BUFFER / SAMPLE_RATE) >= FEEDBACK_MAX_DURATION or early_stop_triggered: + should_send_bookmark = True + should_record_audio = False + early_stop_triggered = False + cloudlog.info("10-second recording completed or second button press - stopping audio feedback") + pm.send('audioFeedback', msg) + + if sm.updated['bookmarkButton']: + cloudlog.info("Bookmark button pressed!") + should_send_bookmark = True + + if should_send_bookmark: + msg = messaging.new_message('userBookmark', valid=True) + pm.send('userBookmark', msg) + + +if __name__ == '__main__': + main() diff --git a/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/system/__init__.py b/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/system/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/system/camerad/__init__.py b/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/system/camerad/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/system/camerad/cameras/__init__.py b/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/system/camerad/cameras/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/system/camerad/cameras/nv12_info.py b/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/system/camerad/cameras/nv12_info.py new file mode 100644 index 000000000..bcb6312d2 --- /dev/null +++ b/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/system/camerad/cameras/nv12_info.py @@ -0,0 +1,21 @@ +# Python version of system/camerad/cameras/nv12_info.h +# Calculations from third_party/linux/include/msm_media_info.h (VENUS_BUFFER_SIZE) + +def align(val: int, alignment: int) -> int: + return ((val + alignment - 1) // alignment) * alignment + +def get_nv12_info(width: int, height: int) -> tuple[int, int, int, int]: + """Returns (stride, y_height, uv_height, buffer_size) for NV12 frame dimensions.""" + stride = align(width, 128) + y_height = align(height, 32) + uv_height = align(height // 2, 16) + + # VENUS_BUFFER_SIZE for NV12 + y_plane = stride * y_height + uv_plane = stride * uv_height + 4096 + size = y_plane + uv_plane + max(16 * 1024, 8 * stride) + size = align(size, 4096) + size += align(width, 512) * 512 # kernel padding for non-aligned frames + size = align(size, 4096) + + return stride, y_height, uv_height, size diff --git a/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/system/hardware/__init__.py b/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/system/hardware/__init__.py new file mode 100644 index 000000000..1338aa2cf --- /dev/null +++ b/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/system/hardware/__init__.py @@ -0,0 +1,22 @@ +import os +from typing import cast + +from iqpilot.system.hardware.base import HardwareBase +from iqpilot.system.hardware.tici.hardware import Tici +from iqpilot.system.hardware.pc.hardware import Pc + +TICI = os.path.isfile('/TICI') +AGNOS = os.path.isfile('/AGNOS') +PC = not TICI + + +if TICI: + HARDWARE = cast(HardwareBase, Tici()) +else: + HARDWARE = cast(HardwareBase, Pc()) + +# Only comma 3/3X expose the DMA-BUF EGL extensions used by the zero-copy +# camera renderer and the direct EGL frame-pacing calls. /TICI is also present +# on comma 4, so it identifies the AGNOS hardware family rather than this GPU +# capability. +EGL_DMA_BUF_SUPPORTED = TICI and HARDWARE.get_device_type() in ("tici", "tizi") diff --git a/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/system/hardware/base.py b/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/system/hardware/base.py new file mode 100644 index 000000000..217696e09 --- /dev/null +++ b/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/system/hardware/base.py @@ -0,0 +1,228 @@ +import os +from abc import abstractmethod, ABC +from dataclasses import dataclass, fields + +from iqpilot.cereal import log + +NetworkType = log.DeviceState.NetworkType +NetworkStrength = log.DeviceState.NetworkStrength + +class LPAError(RuntimeError): + pass + +class LPAProfileNotFoundError(LPAError): + pass + +@dataclass +class Profile: + iccid: str + nickname: str + enabled: bool + provider: str + +@dataclass +class ThermalZone: + # a zone from /sys/class/thermal/thermal_zone* + name: str # a.k.a type + scale: float = 1000. # scale to get degrees in C + zone_number = -1 + + def read(self) -> float: + if self.zone_number < 0: + for n in os.listdir("/sys/devices/virtual/thermal"): + if not n.startswith("thermal_zone"): + continue + with open(os.path.join("/sys/devices/virtual/thermal", n, "type")) as f: + if f.read().strip() == self.name: + self.zone_number = int(n.removeprefix("thermal_zone")) + break + + try: + with open(f"/sys/devices/virtual/thermal/thermal_zone{self.zone_number}/temp") as f: + return int(f.read()) / self.scale + except FileNotFoundError: + return 0 + +@dataclass +class ThermalConfig: + cpu: list[ThermalZone] | None = None + gpu: list[ThermalZone] | None = None + dsp: ThermalZone | None = None + pmic: list[ThermalZone] | None = None + memory: ThermalZone | None = None + intake: ThermalZone | None = None + exhaust: ThermalZone | None = None + case: ThermalZone | None = None + + def get_msg(self): + ret = {} + for f in fields(ThermalConfig): + v = getattr(self, f.name) + if v is not None: + if isinstance(v, list): + ret[f.name + "TempC"] = [x.read() for x in v] + else: + ret[f.name + "TempC"] = v.read() + return ret + +class LPABase(ABC): + @abstractmethod + def list_profiles(self) -> list[Profile]: + pass + + @abstractmethod + def get_active_profile(self) -> Profile | None: + pass + + @abstractmethod + def delete_profile(self, iccid: str) -> None: + pass + + @abstractmethod + def bootstrap(self) -> None: + pass + + @abstractmethod + def download_profile(self, qr: str, nickname: str | None = None) -> None: + pass + + @abstractmethod + def nickname_profile(self, iccid: str, nickname: str) -> None: + pass + + @abstractmethod + def switch_profile(self, iccid: str) -> None: + pass + + def is_comma_profile(self, iccid: str) -> bool: + return any(iccid.startswith(prefix) for prefix in ('8985235',)) + +class HardwareBase(ABC): + @staticmethod + def get_cmdline() -> dict[str, str]: + with open('/proc/cmdline') as f: + cmdline = f.read() + return {kv[0]: kv[1] for kv in [s.split('=') for s in cmdline.split(' ')] if len(kv) == 2} + + @staticmethod + def read_param_file(path, parser, default=0): + try: + with open(path) as f: + return parser(f.read()) + except Exception: + return default + + def booted(self) -> bool: + return True + + def reboot(self, reason=None): + print("REBOOT!") + + def uninstall(self): + print("uninstall") + + def get_os_version(self): + return None + + @abstractmethod + def get_device_type(self): + pass + + def get_imei(self, slot) -> str: + return "" + + def get_serial(self): + return "" + + def get_network_info(self): + return None + + def get_network_type(self): + return NetworkType.none + + def get_sim_info(self): + return { + 'sim_id': '', + 'mcc_mnc': None, + 'network_type': ["Unknown"], + 'sim_state': ["ABSENT"], + 'data_connected': False + } + + def get_sim_lpa(self) -> LPABase: + raise NotImplementedError("SIM LPA not available") + + def get_network_strength(self, network_type): + return NetworkStrength.unknown + + def get_network_metered(self, network_type) -> bool: + return network_type not in (NetworkType.none, NetworkType.wifi, NetworkType.ethernet) + + def get_current_power_draw(self): + return 0 + + def get_som_power_draw(self): + return 0 + + def shutdown(self): + print("SHUTDOWN!") + + def get_thermal_config(self): + return ThermalConfig() + + def set_display_power(self, on: bool): + pass + + def set_screen_brightness(self, percentage): + pass + + def get_screen_brightness(self): + return 0 + + def set_power_save(self, powersave_enabled): + pass + + def get_gpu_usage_percent(self): + return 0 + + def get_modem_version(self): + return None + + def get_modem_temperatures(self): + return [] + + def initialize_hardware(self): + pass + + def configure_modem(self): + pass + + def reboot_modem(self): + pass + + def recover_sim_detection(self) -> bool: + return False + + def get_networks(self): + return None + + def has_internal_panda(self) -> bool: + return False + + def reset_internal_panda(self): + pass + + def recover_internal_panda(self): + pass + + def get_modem_data_usage(self): + return -1, -1 + + def get_voltage(self) -> float: + return 0. + + def get_current(self) -> float: + return 0. + + def set_ir_power(self, percent: int): + pass diff --git a/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/system/hardware/hw.py b/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/system/hardware/hw.py new file mode 100644 index 000000000..7263ddbfc --- /dev/null +++ b/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/system/hardware/hw.py @@ -0,0 +1,137 @@ +import os +import platform +from pathlib import Path + +from iqpilot.system.hardware import PC + +DEFAULT_DOWNLOAD_CACHE_ROOT = "/tmp/comma_download_cache" + +class Paths: + _persist_root_cache: str | None = None + + @staticmethod + def _is_writable_persist_root(path: str) -> bool: + try: + os.makedirs(path, exist_ok=True) + comma_dir = os.path.join(path, "comma") + os.makedirs(comma_dir, exist_ok=True) + + probe_path = os.path.join(comma_dir, ".rw_probe") + with open(probe_path, "w") as f: + f.write("1") + os.remove(probe_path) + return True + except OSError: + return False + + @staticmethod + def comma_home() -> str: + return os.path.join(str(Path.home()), ".comma" + os.environ.get("OPENPILOT_PREFIX", "")) + + @staticmethod + def params() -> str: + if os.environ.get("PARAMS_ROOT"): + return os.environ["PARAMS_ROOT"] + return os.path.join(Paths.comma_home(), "params") if PC else "/data/params" + + @staticmethod + def log_root() -> str: + if os.environ.get('LOG_ROOT', False): + return os.environ['LOG_ROOT'] + elif PC: + return str(Path(Paths.comma_home()) / "media" / "0" / "realdata") + else: + return '/data/media/0/realdata/' + + @staticmethod + def log_root_external() -> str: + return '/mnt/external_realdata/' + + @staticmethod + def swaglog_root() -> str: + if PC: + return os.path.join(Paths.comma_home(), "log") + else: + return "/data/log/" + + @staticmethod + def swaglog_ipc() -> str: + return "ipc:///tmp/logmessage" + os.environ.get("OPENPILOT_PREFIX", "") + + @staticmethod + def download_cache_root() -> str: + if os.environ.get('COMMA_CACHE', False): + return os.environ['COMMA_CACHE'] + "/" + return DEFAULT_DOWNLOAD_CACHE_ROOT + os.environ.get("OPENPILOT_PREFIX", "") + "/" + + @staticmethod + def persist_root() -> str: + if PC: + return os.path.join(Paths.comma_home(), "persist") + + if Paths._persist_root_cache is not None: + return Paths._persist_root_cache + + for candidate in ("/persist", "/data/persist"): + if Paths._is_writable_persist_root(candidate): + Paths._persist_root_cache = candidate + return candidate + + # Keep previous behavior as a last resort. + Paths._persist_root_cache = "/persist" + return Paths._persist_root_cache + + @staticmethod + def stats_root() -> str: + if PC: + return str(Path(Paths.comma_home()) / "stats") + else: + return "/data/stats/" + + @staticmethod + def stats_iq_root() -> str: + if PC: + return str(Path(Paths.comma_home()) / "stats") + else: + return "/data/stats_iq/" + + @staticmethod + def config_root() -> str: + if PC: + return Paths.comma_home() + else: + return "/tmp/.comma" + + @staticmethod + def shm_path() -> str: + if PC and platform.system() == "Darwin": + return "/tmp" # This is not really shared memory on macOS, but it's the closest we can get + return "/dev/shm" + + @staticmethod + def model_root() -> str: + if PC: + return str(Path(Paths.comma_home()) / "media" / "0" / "models") + else: + return "/data/media/0/models" + + @staticmethod + def crash_log_root() -> str: + if PC: + return str(Path(Paths.comma_home()) / "community" / "crashes") + else: + return "/data/community/crashes" + + @staticmethod + def mapd_root() -> str: + if PC: + return str(Path(Paths.comma_home()) / "media" / "0" / "osm") + else: + return "/data/media/0/osm" + + @staticmethod + def screen_recordings_root() -> str: + if PC: + return str(Path(Paths.comma_home()) / "media" / "0" / "screen_recordings") + else: + return "/data/media/0/screen_recordings" diff --git a/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/system/hardware/pc/__init__.py b/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/system/hardware/pc/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/system/hardware/pc/hardware.py b/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/system/hardware/pc/hardware.py new file mode 100644 index 000000000..72ef2f235 --- /dev/null +++ b/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/system/hardware/pc/hardware.py @@ -0,0 +1,12 @@ +from iqpilot.cereal import log +from iqpilot.system.hardware.base import HardwareBase + +NetworkType = log.DeviceState.NetworkType + + +class Pc(HardwareBase): + def get_device_type(self): + return "pc" + + def get_network_type(self): + return NetworkType.wifi diff --git a/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/system/hardware/tici/__init__.py b/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/system/hardware/tici/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/system/hardware/tici/amplifier.py b/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/system/hardware/tici/amplifier.py new file mode 100644 index 000000000..48a34ea48 --- /dev/null +++ b/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/system/hardware/tici/amplifier.py @@ -0,0 +1,159 @@ +#!/usr/bin/env python3 +import time +from collections import namedtuple + +from iqpilot.common.i2c import SMBus + +# https://datasheets.maximintegrated.com/en/ds/MAX98089.pdf + +AmpConfig = namedtuple('AmpConfig', ['name', 'value', 'register', 'offset', 'mask']) +EQParams = namedtuple('EQParams', ['K', 'k1', 'k2', 'c1', 'c2']) + + +def configs_from_eq_params(base, eq_params): + return [ + AmpConfig("K (high)", (eq_params.K >> 8), base, 0, 0xFF), + AmpConfig("K (low)", (eq_params.K & 0xFF), base + 1, 0, 0xFF), + AmpConfig("k1 (high)", (eq_params.k1 >> 8), base + 2, 0, 0xFF), + AmpConfig("k1 (low)", (eq_params.k1 & 0xFF), base + 3, 0, 0xFF), + AmpConfig("k2 (high)", (eq_params.k2 >> 8), base + 4, 0, 0xFF), + AmpConfig("k2 (low)", (eq_params.k2 & 0xFF), base + 5, 0, 0xFF), + AmpConfig("c1 (high)", (eq_params.c1 >> 8), base + 6, 0, 0xFF), + AmpConfig("c1 (low)", (eq_params.c1 & 0xFF), base + 7, 0, 0xFF), + AmpConfig("c2 (high)", (eq_params.c2 >> 8), base + 8, 0, 0xFF), + AmpConfig("c2 (low)", (eq_params.c2 & 0xFF), base + 9, 0, 0xFF), + ] + + +BASE_CONFIG = [ + AmpConfig("MCLK prescaler", 0b01, 0x10, 4, 0b00110000), + AmpConfig("PM: enable speakers", 0b11, 0x4D, 4, 0b00110000), + AmpConfig("PM: enable DACs", 0b11, 0x4D, 0, 0b00000011), + AmpConfig("Enable PLL1", 0b1, 0x12, 7, 0b10000000), + AmpConfig("Enable PLL2", 0b1, 0x1A, 7, 0b10000000), + AmpConfig("DAI1: I2S mode", 0b00100, 0x14, 2, 0b01111100), + AmpConfig("DAI2: I2S mode", 0b00100, 0x1C, 2, 0b01111100), + AmpConfig("DAI1 Passband filtering: music mode", 0b1, 0x18, 7, 0b10000000), + AmpConfig("DAI1 voice mode gain (DV1G)", 0b00, 0x2F, 4, 0b00110000), + AmpConfig("DAI1 attenuation (DV1)", 0x0, 0x2F, 0, 0b00001111), + AmpConfig("DAI2 attenuation (DV2)", 0x0, 0x31, 0, 0b00001111), + AmpConfig("DAI2: DC blocking", 0b1, 0x20, 0, 0b00000001), + AmpConfig("DAI2: High sample rate", 0b0, 0x20, 3, 0b00001000), + AmpConfig("ALC enable", 0b1, 0x43, 7, 0b10000000), + AmpConfig("ALC/excursion limiter release time", 0b101, 0x43, 4, 0b01110000), + AmpConfig("ALC multiband enable", 0b1, 0x43, 3, 0b00001000), + AmpConfig("DAI1 EQ enable", 0b0, 0x49, 0, 0b00000001), + AmpConfig("DAI2 EQ clip detection disabled", 0b1, 0x32, 4, 0b00010000), + AmpConfig("DAI2 EQ attenuation", 0x5, 0x32, 0, 0b00001111), + AmpConfig("Excursion limiter upper corner freq", 0b100, 0x41, 4, 0b01110000), + AmpConfig("Excursion limiter lower corner freq", 0b00, 0x41, 0, 0b00000011), + AmpConfig("Excursion limiter threshold", 0b000, 0x42, 0, 0b00001111), + AmpConfig("Distortion limit (THDCLP)", 0x6, 0x46, 4, 0b11110000), + AmpConfig("Distortion limiter release time constant", 0b0, 0x46, 0, 0b00000001), + AmpConfig("Right DAC input mixer: DAI1 left", 0b0, 0x22, 3, 0b00001000), + AmpConfig("Right DAC input mixer: DAI1 right", 0b0, 0x22, 2, 0b00000100), + AmpConfig("Right DAC input mixer: DAI2 left", 0b1, 0x22, 1, 0b00000010), + AmpConfig("Right DAC input mixer: DAI2 right", 0b0, 0x22, 0, 0b00000001), + AmpConfig("DAI1 audio port selector", 0b10, 0x16, 6, 0b11000000), + AmpConfig("DAI2 audio port selector", 0b01, 0x1E, 6, 0b11000000), + AmpConfig("Enable left digital microphone", 0b1, 0x48, 5, 0b00100000), + AmpConfig("Enable right digital microphone", 0b1, 0x48, 4, 0b00010000), + AmpConfig("Enhanced volume smoothing disabled", 0b0, 0x49, 7, 0b10000000), + AmpConfig("Volume adjustment smoothing disabled", 0b0, 0x49, 6, 0b01000000), + AmpConfig("Zero-crossing detection disabled", 0b0, 0x49, 5, 0b00100000), +] + +CONFIGS = { + "tici": [ + AmpConfig("Right speaker output from right DAC", 0b1, 0x2C, 0, 0b11111111), + AmpConfig("Right Speaker Mixer Gain", 0b00, 0x2D, 2, 0b00001100), + AmpConfig("Right speaker output volume", 0x1c, 0x3E, 0, 0b00011111), + AmpConfig("DAI2 EQ enable", 0b1, 0x49, 1, 0b00000010), + *configs_from_eq_params(0x84, EQParams(0x274F, 0xC0FF, 0x3BF9, 0x0B3C, 0x1656)), + *configs_from_eq_params(0x8E, EQParams(0x1009, 0xC6BF, 0x2952, 0x1C97, 0x30DF)), + *configs_from_eq_params(0x98, EQParams(0x0F75, 0xCBE5, 0x0ED2, 0x2528, 0x3E42)), + *configs_from_eq_params(0xA2, EQParams(0x091F, 0x3D4C, 0xCE11, 0x1266, 0x2807)), + *configs_from_eq_params(0xAC, EQParams(0x0A9E, 0x3F20, 0xE573, 0x0A8B, 0x3A3B)), + ], + "tizi": [ + AmpConfig("Left speaker output from left DAC", 0b1, 0x2B, 0, 0b11111111), + AmpConfig("Right speaker output from right DAC", 0b1, 0x2C, 0, 0b11111111), + AmpConfig("Left Speaker Mixer Gain", 0b00, 0x2D, 0, 0b00000011), + AmpConfig("Right Speaker Mixer Gain", 0b00, 0x2D, 2, 0b00001100), + AmpConfig("Left speaker output volume", 0x17, 0x3D, 0, 0b00011111), + AmpConfig("Right speaker output volume", 0x17, 0x3E, 0, 0b00011111), + AmpConfig("DAI2 EQ enable", 0b0, 0x49, 1, 0b00000010), + AmpConfig("DAI2: DC blocking", 0b0, 0x20, 0, 0b00000001), + AmpConfig("ALC enable", 0b0, 0x43, 7, 0b10000000), + AmpConfig("DAI2 EQ attenuation", 0x2, 0x32, 0, 0b00001111), + AmpConfig("Excursion limiter upper corner freq", 0b001, 0x41, 4, 0b01110000), + AmpConfig("Excursion limiter threshold", 0b100, 0x42, 0, 0b00001111), + AmpConfig("Distortion limit (THDCLP)", 0x0, 0x46, 4, 0b11110000), + AmpConfig("Distortion limiter release time constant", 0b1, 0x46, 0, 0b00000001), + AmpConfig("Left DAC input mixer: DAI1 left", 0b0, 0x22, 7, 0b10000000), + AmpConfig("Left DAC input mixer: DAI1 right", 0b0, 0x22, 6, 0b01000000), + AmpConfig("Left DAC input mixer: DAI2 left", 0b1, 0x22, 5, 0b00100000), + AmpConfig("Left DAC input mixer: DAI2 right", 0b0, 0x22, 4, 0b00010000), + AmpConfig("Right DAC input mixer: DAI2 left", 0b0, 0x22, 1, 0b00000010), + AmpConfig("Right DAC input mixer: DAI2 right", 0b1, 0x22, 0, 0b00000001), + AmpConfig("Volume adjustment smoothing disabled", 0b1, 0x49, 6, 0b01000000), + ], +} + + +class Amplifier: + AMP_I2C_BUS = 0 + AMP_ADDRESS = 0x10 + + def __init__(self, debug=False): + self.debug = debug + + def _get_shutdown_config(self, amp_disabled: bool) -> AmpConfig: + return AmpConfig("Global shutdown", 0b0 if amp_disabled else 0b1, 0x51, 7, 0b10000000) + + def _set_configs(self, configs: list[AmpConfig]) -> None: + with SMBus(self.AMP_I2C_BUS) as bus: + for config in configs: + if self.debug: + print(f"Setting \"{config.name}\" to {config.value}:") + + old_value = bus.read_byte_data(self.AMP_ADDRESS, config.register, force=True) + new_value = (old_value & (~config.mask)) | ((config.value << config.offset) & config.mask) + bus.write_byte_data(self.AMP_ADDRESS, config.register, new_value, force=True) + + if self.debug: + print(f" Changed {hex(config.register)}: {hex(old_value)} -> {hex(new_value)}") + + def set_configs(self, configs: list[AmpConfig]) -> bool: + tries = 15 + backoff = 0. + for i in range(tries): + try: + self._set_configs(configs) + return True + except OSError: + backoff += 0.1 + time.sleep(backoff) + print(f"Failed to set amp config, {tries - i - 1} retries left") + return False + + def set_global_shutdown(self, amp_disabled: bool) -> bool: + return self.set_configs([self._get_shutdown_config(amp_disabled), ]) + + def initialize_configuration(self, model: str) -> bool: + cfgs = [ + self._get_shutdown_config(True), + *BASE_CONFIG, + *CONFIGS[model], + self._get_shutdown_config(False), + ] + return self.set_configs(cfgs) + + +if __name__ == "__main__": + with open("/sys/firmware/devicetree/base/model") as f: + model = f.read().strip('\x00') + model = model.split('comma ')[-1] + + amp = Amplifier() + amp.initialize_configuration(model) diff --git a/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/system/hardware/tici/hardware.py b/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/system/hardware/tici/hardware.py new file mode 100644 index 000000000..b9fa7fbff --- /dev/null +++ b/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/system/hardware/tici/hardware.py @@ -0,0 +1,740 @@ +import math +import os +import sys +import subprocess +import time +import tempfile +from enum import IntEnum +from functools import cached_property, lru_cache +from pathlib import Path + +from iqpilot.cereal import log +from iqpilot.common.utils import sudo_read, sudo_write +from iqpilot.common.gpio import gpio_set, gpio_init, get_irqs_for_action +from iqpilot.system.hardware.base import HardwareBase, LPABase, ThermalConfig, ThermalZone +from iqpilot.system.hardware.tici import iwlist +from iqpilot.system.hardware.tici.lpa import TiciLPA +from iqpilot.system.hardware.tici.pins import GPIO +from iqpilot.system.hardware.tici.amplifier import Amplifier + +NM = 'org.freedesktop.NetworkManager' +NM_CON_ACT = NM + '.Connection.Active' +NM_DEV = NM + '.Device' +NM_DEV_WL = NM + '.Device.Wireless' +NM_DEV_STATS = NM + '.Device.Statistics' +NM_AP = NM + '.AccessPoint' +DBUS_PROPS = 'org.freedesktop.DBus.Properties' + +MM = 'org.freedesktop.ModemManager1' +MM_MODEM = MM + ".Modem" +MM_MODEM_SIMPLE = MM + ".Modem.Simple" +MM_SIM = MM + ".Sim" + +class MM_MODEM_STATE(IntEnum): + FAILED = -1 + UNKNOWN = 0 + INITIALIZING = 1 + LOCKED = 2 + DISABLED = 3 + DISABLING = 4 + ENABLING = 5 + ENABLED = 6 + SEARCHING = 7 + REGISTERED = 8 + DISCONNECTING = 9 + CONNECTING = 10 + CONNECTED = 11 + +class NMActiveConnectionState(IntEnum): + UNKNOWN = 0 + ACTIVATING = 1 + ACTIVATED = 2 + DEACTIVATING = 3 + DEACTIVATED = 4 + +class NMMetered(IntEnum): + NM_METERED_UNKNOWN = 0 + NM_METERED_YES = 1 + NM_METERED_NO = 2 + NM_METERED_GUESS_YES = 3 + NM_METERED_GUESS_NO = 4 + +TIMEOUT = 0.1 +REFRESH_RATE_MS = 1000 + +NetworkType = log.DeviceState.NetworkType +NetworkStrength = log.DeviceState.NetworkStrength + +# https://developer.gnome.org/ModemManager/unstable/ModemManager-Flags-and-Enumerations.html#MMModemAccessTechnology +MM_MODEM_ACCESS_TECHNOLOGY_UMTS = 1 << 5 +MM_MODEM_ACCESS_TECHNOLOGY_LTE = 1 << 14 + +# MMModemStateFailedReason +MM_MODEM_STATE_FAILED_REASON_SIM_MISSING = 2 + + +def affine_irq(val, action): + irqs = get_irqs_for_action(action) + if len(irqs) == 0: + return + + for i in irqs: + sudo_write(str(val), f"/proc/irq/{i}/smp_affinity_list") + +@lru_cache +def get_device_type(): + # lru_cache and cache can cause memory leaks when used in classes + try: + with open("/sys/firmware/devicetree/base/model") as f: + model = f.read().strip('\x00') + except FileNotFoundError: + # off-device (e.g. the prebuilt build container fakes /TICI but has no + # devicetree); import must not crash. Not a real device type. + return "unknown" + return model.split('comma ')[-1] + +class Tici(HardwareBase): + @staticmethod + def _ensure_system_python_path() -> None: + system_site = "/usr/lib/python3/dist-packages" + if system_site not in sys.path and os.path.isdir(system_site): + sys.path.append(system_site) + + @staticmethod + def _run_direct_modem_command(command: str) -> None: + import serial + + last_error: Exception | None = None + for device in ("/dev/ttyUSB2", "/dev/ttyUSB3"): + if not os.path.exists(device): + continue + + try: + with serial.Serial(device, baudrate=9600, timeout=2) as modem: + modem.reset_input_buffer() + modem.write((command + "\r").encode("ascii")) + + deadline = time.monotonic() + 3.0 + while time.monotonic() < deadline: + line = modem.readline().decode(errors="ignore").strip() + if not line: + continue + if line == "OK": + return + if line == "ERROR" or "ERROR" in line: + raise RuntimeError(f"{device}: {line}") + raise TimeoutError(f"{device}: timed out waiting for modem response") + except Exception as e: + last_error = e + + if last_error is not None: + raise last_error + raise RuntimeError("No modem AT port available") + + @cached_property + def bus(self): + try: + import dbus + except ModuleNotFoundError: + self._ensure_system_python_path() + import dbus + return dbus.SystemBus() + + @cached_property + def nm(self): + return self.bus.get_object(NM, '/org/freedesktop/NetworkManager') + + @property # this should not be cached, in case the modemmanager restarts + def mm(self): + return self.bus.get_object(MM, '/org/freedesktop/ModemManager1') + + @cached_property + def amplifier(self): + if self.get_device_type() == "mici": + return None + if os.path.exists('/tmp/lite_hw') or os.environ.get('LITE') == '1': + return None + return Amplifier() + + def get_os_version(self): + with open("/VERSION") as f: + return f.read().strip() + + def get_device_type(self): + return get_device_type() + + def reboot(self, reason=None): + subprocess.check_output(["sudo", "reboot"]) + + def uninstall(self): + Path("/data/__system_reset__").touch() + os.sync() + self.reboot() + + def get_serial(self): + return self.get_cmdline()['androidboot.serialno'] + + def get_voltage(self): + with open("/sys/class/hwmon/hwmon1/in1_input") as f: + return int(f.read()) + + def get_current(self): + with open("/sys/class/hwmon/hwmon1/curr1_input") as f: + return int(f.read()) + + def set_ir_power(self, percent: int): + if self.get_device_type() in ("tici", "tizi"): + return + + value = int((percent / 100) * 300) + with open("/sys/class/leds/led:switch_2/brightness", "w") as f: + f.write("0\n") + with open("/sys/class/leds/led:torch_2/brightness", "w") as f: + f.write(f"{value}\n") + with open("/sys/class/leds/led:switch_2/brightness", "w") as f: + f.write(f"{value}\n") + + def get_network_type(self): + try: + primary_connection = self.nm.Get(NM, 'PrimaryConnection', dbus_interface=DBUS_PROPS, timeout=TIMEOUT) + primary_connection = self.bus.get_object(NM, primary_connection) + primary_type = primary_connection.Get(NM_CON_ACT, 'Type', dbus_interface=DBUS_PROPS, timeout=TIMEOUT) + + if primary_type == '802-3-ethernet': + return NetworkType.ethernet + elif primary_type == '802-11-wireless': + return NetworkType.wifi + else: + active_connections = self.nm.Get(NM, 'ActiveConnections', dbus_interface=DBUS_PROPS, timeout=TIMEOUT) + for conn in active_connections: + c = self.bus.get_object(NM, conn) + tp = c.Get(NM_CON_ACT, 'Type', dbus_interface=DBUS_PROPS, timeout=TIMEOUT) + if tp == 'gsm': + modem = self.get_modem() + modem_state = modem.Get(MM_MODEM, 'State', dbus_interface=DBUS_PROPS, timeout=TIMEOUT) + if modem_state < MM_MODEM_STATE.REGISTERED: + return NetworkType.none + access_t = modem.Get(MM_MODEM, 'AccessTechnologies', dbus_interface=DBUS_PROPS, timeout=TIMEOUT) + if access_t >= MM_MODEM_ACCESS_TECHNOLOGY_LTE: + return NetworkType.cell4G + elif access_t >= MM_MODEM_ACCESS_TECHNOLOGY_UMTS: + return NetworkType.cell3G + else: + return NetworkType.cell2G + except Exception: + pass + + return NetworkType.none + + def get_modem(self): + objects = self.mm.GetManagedObjects(dbus_interface="org.freedesktop.DBus.ObjectManager", timeout=TIMEOUT) + if not objects: + raise RuntimeError("ModemManager returned no modems") + modem_path = next(iter(objects)) + return self.bus.get_object(MM, modem_path) + + def get_wlan(self): + wlan_path = self.nm.GetDeviceByIpIface('wlan0', dbus_interface=NM, timeout=TIMEOUT) + return self.bus.get_object(NM, wlan_path) + + def get_wwan(self): + wwan_path = self.nm.GetDeviceByIpIface('wwan0', dbus_interface=NM, timeout=TIMEOUT) + return self.bus.get_object(NM, wwan_path) + + def get_sim_info(self): + modem = self.get_modem() + sim_path = modem.Get(MM_MODEM, 'Sim', dbus_interface=DBUS_PROPS, timeout=TIMEOUT) + + if sim_path == "/": + return { + 'sim_id': '', + 'mcc_mnc': None, + 'network_type': ["Unknown"], + 'sim_state': ["ABSENT"], + 'data_connected': False + } + else: + sim = self.bus.get_object(MM, sim_path) + return { + 'sim_id': str(sim.Get(MM_SIM, 'SimIdentifier', dbus_interface=DBUS_PROPS, timeout=TIMEOUT)), + 'mcc_mnc': str(sim.Get(MM_SIM, 'OperatorIdentifier', dbus_interface=DBUS_PROPS, timeout=TIMEOUT)), + 'network_type': ["Unknown"], + 'sim_state': ["READY"], + 'data_connected': modem.Get(MM_MODEM, 'State', dbus_interface=DBUS_PROPS, timeout=TIMEOUT) == MM_MODEM_STATE.CONNECTED, + } + + def get_sim_lpa(self) -> LPABase: + return TiciLPA() + + def get_imei(self, slot): + if slot != 0: + return "" + + return str(self.get_modem().Get(MM_MODEM, 'EquipmentIdentifier', dbus_interface=DBUS_PROPS, timeout=TIMEOUT)) + + def get_network_info(self): + if self.get_device_type() == "mici": + return None + try: + modem = self.get_modem() + info = modem.Command("AT+QNWINFO", math.ceil(TIMEOUT), dbus_interface=MM_MODEM, timeout=TIMEOUT) + extra = modem.Command('AT+QENG="servingcell"', math.ceil(TIMEOUT), dbus_interface=MM_MODEM, timeout=TIMEOUT) + state = modem.Get(MM_MODEM, 'State', dbus_interface=DBUS_PROPS, timeout=TIMEOUT) + except Exception: + return None + + if info and info.startswith('+QNWINFO: '): + info = info.replace('+QNWINFO: ', '').replace('"', '').split(',') + extra = "" if extra is None else extra.replace('+QENG: "servingcell",', '').replace('"', '') + state = "" if state is None else MM_MODEM_STATE(state).name + + if len(info) != 4: + return None + + technology, operator, band, channel = info + + return({ + 'technology': technology, + 'operator': operator, + 'band': band, + 'channel': int(channel), + 'extra': extra, + 'state': state, + }) + else: + return None + + def parse_strength(self, percentage): + if percentage < 25: + return NetworkStrength.poor + elif percentage < 50: + return NetworkStrength.moderate + elif percentage < 75: + return NetworkStrength.good + else: + return NetworkStrength.great + + def get_network_strength(self, network_type): + network_strength = NetworkStrength.unknown + + try: + if network_type == NetworkType.none: + pass + elif network_type == NetworkType.wifi: + wlan = self.get_wlan() + active_ap_path = wlan.Get(NM_DEV_WL, 'ActiveAccessPoint', dbus_interface=DBUS_PROPS, timeout=TIMEOUT) + if active_ap_path != "/": + active_ap = self.bus.get_object(NM, active_ap_path) + strength = int(active_ap.Get(NM_AP, 'Strength', dbus_interface=DBUS_PROPS, timeout=TIMEOUT)) + network_strength = self.parse_strength(strength) + else: # Cellular + modem = self.get_modem() + strength = int(modem.Get(MM_MODEM, 'SignalQuality', dbus_interface=DBUS_PROPS, timeout=TIMEOUT)[0]) + network_strength = self.parse_strength(strength) + except Exception: + pass + + return network_strength + + def get_network_metered(self, network_type) -> bool: + try: + primary_connection = self.nm.Get(NM, 'PrimaryConnection', dbus_interface=DBUS_PROPS, timeout=TIMEOUT) + primary_connection = self.bus.get_object(NM, primary_connection) + primary_devices = primary_connection.Get(NM_CON_ACT, 'Devices', dbus_interface=DBUS_PROPS, timeout=TIMEOUT) + + for dev in primary_devices: + dev_obj = self.bus.get_object(NM, str(dev)) + metered_prop = dev_obj.Get(NM_DEV, 'Metered', dbus_interface=DBUS_PROPS, timeout=TIMEOUT) + + if network_type == NetworkType.wifi: + if metered_prop in [NMMetered.NM_METERED_YES, NMMetered.NM_METERED_GUESS_YES]: + return True + elif network_type in [NetworkType.cell2G, NetworkType.cell3G, NetworkType.cell4G, NetworkType.cell5G]: + if metered_prop == NMMetered.NM_METERED_NO: + return False + except Exception: + pass + + return super().get_network_metered(network_type) + + def get_modem_version(self): + try: + modem = self.get_modem() + return modem.Get(MM_MODEM, 'Revision', dbus_interface=DBUS_PROPS, timeout=TIMEOUT) + except Exception: + return None + + def get_modem_temperatures(self): + timeout = 0.2 # Default timeout is too short + try: + modem = self.get_modem() + temps = modem.Command("AT+QTEMP", math.ceil(timeout), dbus_interface=MM_MODEM, timeout=timeout) + return list(filter(lambda t: t != 255, map(int, temps.split(' ')[1].split(',')))) + except Exception: + return [] + + + def get_current_power_draw(self): + return (self.read_param_file("/sys/class/hwmon/hwmon1/power1_input", int) / 1e6) + + def get_som_power_draw(self): + return (self.read_param_file("/sys/class/power_supply/bms/voltage_now", int) * self.read_param_file("/sys/class/power_supply/bms/current_now", int) / 1e12) + + def shutdown(self): + os.system("sudo poweroff") + + def get_thermal_config(self): + intake, exhaust, case = None, None, None + if self.get_device_type() == "mici": + case = ThermalZone("case") + intake = ThermalZone("intake") + exhaust = ThermalZone("exhaust") + return ThermalConfig(cpu=[ThermalZone(f"cpu{i}-silver-usr") for i in range(4)] + + [ThermalZone(f"cpu{i}-gold-usr") for i in range(4)], + gpu=[ThermalZone("gpu0-usr"), ThermalZone("gpu1-usr")], + dsp=ThermalZone("compute-hvx-usr"), + memory=ThermalZone("ddr-usr"), + pmic=[ThermalZone("pm8998_tz"), ThermalZone("pm8005_tz")], + intake=intake, + exhaust=exhaust, + case=case) + + def set_display_power(self, on): + try: + with open("/sys/class/backlight/panel0-backlight/bl_power", "w") as f: + f.write("0" if on else "4") + except Exception: + pass + + def set_screen_brightness(self, percentage): + try: + with open("/sys/class/backlight/panel0-backlight/max_brightness") as f: + max_brightness = float(f.read().strip()) + + val = int(percentage * (max_brightness / 100.)) + with open("/sys/class/backlight/panel0-backlight/brightness", "w") as f: + f.write(str(val)) + except Exception: + pass + + def get_screen_brightness(self): + try: + with open("/sys/class/backlight/panel0-backlight/max_brightness") as f: + max_brightness = float(f.read().strip()) + + with open("/sys/class/backlight/panel0-backlight/brightness") as f: + return int(float(f.read()) / (max_brightness / 100.)) + except Exception: + return 0 + + def set_power_save(self, powersave_enabled): + # amplifier, 100mW at idle + if self.amplifier is not None: + self.amplifier.set_global_shutdown(amp_disabled=powersave_enabled) + if not powersave_enabled: + self.amplifier.initialize_configuration(self.get_device_type()) + + # *** CPU config *** + + # offline big cluster + for i in range(4, 8): + val = '0' if powersave_enabled else '1' + sudo_write(val, f'/sys/devices/system/cpu/cpu{i}/online') + + for n in ('0', '4'): + if powersave_enabled and n == '4': + continue + gov = 'ondemand' if powersave_enabled else 'performance' + sudo_write(gov, f'/sys/devices/system/cpu/cpufreq/policy{n}/scaling_governor') + + # *** IRQ config *** + + # GPU, modeld core + affine_irq(7, "kgsl-3d0") + + # camerad core + camera_irqs = ("a5", "cci", "cpas_camnoc", "cpas-cdm", "csid", "ife", "csid-lite", "ife-lite") + for n in camera_irqs: + affine_irq(6, n) + + def get_gpu_usage_percent(self): + try: + with open('/sys/class/kgsl/kgsl-3d0/gpubusy') as f: + used, total = f.read().strip().split() + return 100.0 * int(used) / int(total) + except Exception: + return 0 + + def initialize_hardware(self): + if self.amplifier is not None: + self.amplifier.initialize_configuration(self.get_device_type()) + + # Allow hardwared to write engagement status to kmsg + os.system("sudo chmod a+w /dev/kmsg") + + # Ensure fan gpio is enabled so fan runs until shutdown, also turned on at boot by the ABL + gpio_init(GPIO.SOM_ST_IO, True) + gpio_set(GPIO.SOM_ST_IO, 1) + + # *** IRQ config *** + + # mask off big cluster from default affinity + sudo_write("f", "/proc/irq/default_smp_affinity") + + # move these off the default core + affine_irq(1, "msm_vidc") # encoders + affine_irq(1, "i2c_geni") # sensors + + # *** GPU config *** + # https://github.com/commaai/agnos-kernel-sdm845/blob/master/arch/arm64/boot/dts/qcom/sdm845-gpu.dtsi#L216 + affine_irq(5, "fts_ts") # touch + affine_irq(5, "msm_drm") # display + sudo_write("1", "/sys/class/kgsl/kgsl-3d0/min_pwrlevel") + sudo_write("1", "/sys/class/kgsl/kgsl-3d0/max_pwrlevel") + sudo_write("1", "/sys/class/kgsl/kgsl-3d0/force_bus_on") + sudo_write("1", "/sys/class/kgsl/kgsl-3d0/force_clk_on") + sudo_write("1", "/sys/class/kgsl/kgsl-3d0/force_rail_on") + sudo_write("1000", "/sys/class/kgsl/kgsl-3d0/idle_timer") + sudo_write("performance", "/sys/class/kgsl/kgsl-3d0/devfreq/governor") + sudo_write("710", "/sys/class/kgsl/kgsl-3d0/max_clock_mhz") + + # setup governors + sudo_write("performance", "/sys/class/devfreq/soc:qcom,cpubw/governor") + sudo_write("performance", "/sys/class/devfreq/soc:qcom,memlat-cpu0/governor") + sudo_write("performance", "/sys/class/devfreq/soc:qcom,memlat-cpu4/governor") + + # *** VIDC (encoder) config *** + sudo_write("N", "/sys/kernel/debug/msm_vidc/clock_scaling") + sudo_write("Y", "/sys/kernel/debug/msm_vidc/disable_thermal_mitigation") + + # pandad core + affine_irq(3, "spi_geni") # SPI + if "tici" in self.get_device_type(): + affine_irq(3, "xhci-hcd:usb3") + affine_irq(3, "xhci-hcd:usb1") + try: + pid = subprocess.check_output(["pgrep", "-f", "spi0"], encoding='utf8').strip() + subprocess.call(["sudo", "chrt", "-f", "-p", "1", pid]) + subprocess.call(["sudo", "taskset", "-pc", "3", pid], stdout=subprocess.DEVNULL, stderr=subprocess.DEVNULL) + except subprocess.CalledProcessException as e: + print(str(e)) + + def configure_modem(self): + from iqpilot.common.params import Params + + sim_info = self.get_sim_info() + sim_id = sim_info.get('sim_id', '') + params = Params() + manual_apn = params.get("GsmApn", encoding="utf-8") or "" + metered_enabled = params.get_bool("GsmMetered") + + modem = self.get_modem() + try: + manufacturer = str(modem.Get(MM_MODEM, 'Manufacturer', dbus_interface=DBUS_PROPS, timeout=TIMEOUT)) + except Exception: + manufacturer = None + + cmds = [] + is_comma_profile = self.get_sim_lpa().is_comma_profile(sim_id) + roaming_enabled = params.get_bool("GsmRoaming") + initial_eps_apn = "" if is_comma_profile else manual_apn + + if not is_comma_profile and params.get("GsmRoaming") is None: + params.put_bool("GsmRoaming", True) + roaming_enabled = True + + subprocess.call([ + "nmcli", "connection", "modify", "lte", + "gsm.auto-config", "no" if manual_apn else "yes", + "gsm.apn", manual_apn, + "gsm.home-only", "no" if roaming_enabled else "yes", + "gsm.network-id", "", + "gsm.initial-eps-bearer-configure", "yes" if initial_eps_apn else "no", + "gsm.initial-eps-bearer-apn", initial_eps_apn, + "connection.metered", "unknown" if metered_enabled else "no", + ], stdout=subprocess.DEVNULL, stderr=subprocess.DEVNULL) + + if self.get_device_type() in ("tici", "tizi"): + if initial_eps_apn: + subprocess.call(["mmcli", "-m", "any", f'--3gpp-set-initial-eps-bearer-settings=apn={initial_eps_apn}']) + else: + subprocess.call(["mmcli", "-m", "any", '--3gpp-set-initial-eps-bearer-settings=apn=']) + + cmds += [ + # configure modem as data-centric + 'AT+QNVW=5280,0,"0102000000000000"', + 'AT+QNVFW="/nv/item_files/ims/IMS_enable",00', + 'AT+QNVFW="/nv/item_files/modem/mmode/ue_usage_setting",01', + ] + if self.get_device_type() == "tizi": + cmds += [ + 'AT+QSIMDET=1,0', + 'AT+QSIMSTAT=1', + ] + elif manufacturer == 'Cavli Inc.': + cmds += [ + 'AT^SIMSWAP=1', # use SIM slot, instead of internal eSIM + 'AT$QCSIMSLEEP=0', # disable SIM sleep + 'AT$QCSIMCFG=SimPowerSave,0', # more sleep disable + + # ethernet config + 'AT$QCPCFG=usbNet,0', + 'AT$QCNETDEVCTL=3,1', + ] + else: + # this modem gets upset with too many AT commands + if sim_id is None or len(sim_id) == 0: + cmds += [ + # SIM sleep disable + 'AT$QCSIMSLEEP=0', + 'AT$QCSIMCFG=SimPowerSave,0', + + # ethernet config + 'AT$QCPCFG=usbNet,1', + ] + + for cmd in cmds: + try: + modem.Command(cmd, math.ceil(TIMEOUT), dbus_interface=MM_MODEM, timeout=TIMEOUT) + except Exception: + pass + + # eSIM prime + dest = "/etc/NetworkManager/system-connections/esim.nmconnection" + if self.get_sim_lpa().is_comma_profile(sim_id) and not os.path.exists(dest): + with open(Path(__file__).parent/'esim.nmconnection') as f, tempfile.NamedTemporaryFile(mode='w') as tf: + dat = f.read() + dat = dat.replace("sim-id=", f"sim-id={sim_id}") + tf.write(dat) + tf.flush() + + # needs to be root + os.system(f"sudo cp {tf.name} {dest}") + os.system(f"sudo nmcli con load {dest}") + + def recover_sim_detection(self) -> bool: + # A worn SIM-tray presence switch can read "removed" while the SIM pads still make + # contact; with hot-swap detect armed (AT+QSIMDET=1) the modem never powers the SIM + # and lands in failed/sim-missing. Disabling detect and rebooting the modem makes it + # probe the SIM electrically. Safe to retry on failure: firing disarms the QSIMDET + # gate, so a genuinely SIM-less device gets at most one extra modem reboot per boot. + if self.get_device_type() not in ("tici", "tizi"): + return False + + try: + modem = self.get_modem() + state = modem.Get(MM_MODEM, 'State', dbus_interface=DBUS_PROPS, timeout=TIMEOUT) + if state != MM_MODEM_STATE.FAILED: + return False + reason = modem.Get(MM_MODEM, 'StateFailedReason', dbus_interface=DBUS_PROPS, timeout=TIMEOUT) + if reason != MM_MODEM_STATE_FAILED_REASON_SIM_MISSING: + return False + detect = str(modem.Command('AT+QSIMDET?', math.ceil(TIMEOUT), dbus_interface=MM_MODEM, timeout=TIMEOUT)).strip() + if not detect.startswith('+QSIMDET: 1'): + return False + modem.Command('AT+QSIMDET=0,0', math.ceil(TIMEOUT), dbus_interface=MM_MODEM, timeout=TIMEOUT) + modem.Command('AT+CFUN=1,1', math.ceil(TIMEOUT), dbus_interface=MM_MODEM, timeout=TIMEOUT) + return True + except Exception: + return False + + def reboot_modem(self): + modem = None + try: + modem = self.get_modem() + except Exception: + pass + + if modem is not None: + for state in (0, 1): + try: + modem.Command(f'AT+CFUN={state}', math.ceil(TIMEOUT), dbus_interface=MM_MODEM, timeout=TIMEOUT) + except Exception: + pass + return + + for state in (0, 1): + try: + self._run_direct_modem_command(f"AT+CFUN={state}") + except Exception: + pass + + def get_networks(self): + r = {} + + wlan = iwlist.scan() + if wlan is not None: + r['wlan'] = wlan + + lte_info = self.get_network_info() + if lte_info is not None: + extra = lte_info['extra'] + + # ,"LTE",,,,,,,, + # ,,,,,,, + if 'LTE' in extra: + extra = extra.split(',') + try: + r['lte'] = [{ + "mcc": int(extra[3]), + "mnc": int(extra[4]), + "cid": int(extra[5], 16), + "nmr": [{"pci": int(extra[6]), "earfcn": int(extra[7])}], + }] + except (ValueError, IndexError): + pass + + return r + + def get_modem_data_usage(self): + try: + wwan = self.get_wwan() + + # Ensure refresh rate is set so values don't go stale + refresh_rate = wwan.Get(NM_DEV_STATS, 'RefreshRateMs', dbus_interface=DBUS_PROPS, timeout=TIMEOUT) + if refresh_rate != REFRESH_RATE_MS: + u = type(refresh_rate) + wwan.Set(NM_DEV_STATS, 'RefreshRateMs', u(REFRESH_RATE_MS), dbus_interface=DBUS_PROPS, timeout=TIMEOUT) + + tx = wwan.Get(NM_DEV_STATS, 'TxBytes', dbus_interface=DBUS_PROPS, timeout=TIMEOUT) + rx = wwan.Get(NM_DEV_STATS, 'RxBytes', dbus_interface=DBUS_PROPS, timeout=TIMEOUT) + return int(tx), int(rx) + except Exception: + return -1, -1 + + def has_internal_panda(self): + return True + + def reset_internal_panda(self): + gpio_init(GPIO.STM_RST_N, True) + gpio_init(GPIO.STM_BOOT0, True) + + gpio_set(GPIO.STM_RST_N, 1) + gpio_set(GPIO.STM_BOOT0, 0) + time.sleep(1) + gpio_set(GPIO.STM_RST_N, 0) + + def recover_internal_panda(self): + gpio_init(GPIO.STM_RST_N, True) + gpio_init(GPIO.STM_BOOT0, True) + + gpio_set(GPIO.STM_RST_N, 1) + gpio_set(GPIO.STM_BOOT0, 1) + time.sleep(0.5) + gpio_set(GPIO.STM_RST_N, 0) + time.sleep(0.5) + gpio_set(GPIO.STM_BOOT0, 0) + + def booted(self): + # this normally boots within 8s, but on rare occasions takes 30+s + encoder_state = sudo_read("/sys/kernel/debug/msm_vidc/core0/info") + if "Core state: 0" in encoder_state and (time.monotonic() < 60*2): + return False + return True + +if __name__ == "__main__": + t = Tici() + t.configure_modem() + t.initialize_hardware() + t.set_power_save(False) + print(t.get_sim_info()) diff --git a/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/system/hardware/tici/iwlist.py b/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/system/hardware/tici/iwlist.py new file mode 100644 index 000000000..1e7c428b4 --- /dev/null +++ b/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/system/hardware/tici/iwlist.py @@ -0,0 +1,35 @@ +import subprocess + + +def scan(interface="wlan0"): + result = [] + try: + r = subprocess.check_output(["iwlist", interface, "scan"], encoding='utf8') + + mac = None + for line in r.split('\n'): + if "Address" in line: + # Based on the adapter eithere a percentage or dBm is returned + # Add previous network in case no dBm signal level was seen + if mac is not None: + result.append({"mac": mac}) + mac = None + + mac = line.split(' ')[-1] + elif "dBm" in line: + try: + level = line.split('Signal level=')[1] + rss = int(level.split(' ')[0]) + result.append({"mac": mac, "rss": rss}) + mac = None + except ValueError: + continue + + # Add last network if no dBm was found + if mac is not None: + result.append({"mac": mac}) + + return result + + except Exception: + return None diff --git a/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/system/hardware/tici/lpa.py b/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/system/hardware/tici/lpa.py new file mode 100644 index 000000000..a636742d5 --- /dev/null +++ b/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/system/hardware/tici/lpa.py @@ -0,0 +1,1482 @@ +#!/usr/bin/env python3 +# SGP.22 v2.3: https://www.gsma.com/solutions-and-impact/technologies/esim/wp-content/uploads/2021/07/SGP.22-v2.3.pdf + +import argparse +import atexit +import base64 +import hashlib +import json +import math +import os +import re +import requests +import serial +import subprocess +import sys +import time + +from collections.abc import Generator +from contextlib import contextmanager +from pathlib import Path +from typing import Literal +from iqpilot.common.time_helpers import system_time_valid +from iqpilot.system.hardware.base import LPABase, LPAError, LPAProfileNotFoundError, Profile + +GSMA_CI_BUNDLE = str(Path(__file__).parent / 'gsma_ci_bundle.pem') + + +DEFAULT_DEVICE = "/dev/modem_at0" +DEFAULT_BAUD = 9600 +DEFAULT_TIMEOUT = 5.0 +# https://euicc-manual.osmocom.org/docs/lpa/applet-id/ +ISDR_AID = "A0000005591010FFFFFFFF8900000100" +ES10X_MSS = 120 +MM = "org.freedesktop.ModemManager1" +MM_MODEM = MM + ".Modem" + +# TLV Tags +TAG_ICCID = 0x5A +TAG_STATUS = 0x80 +TAG_EUICC_INFO = 0xBF20 +TAG_PREPARE_DOWNLOAD = 0xBF21 +TAG_PROFILE_INFO_LIST = 0xBF2D +TAG_EUICC_CHALLENGE = 0xBF2E +TAG_SET_NICKNAME = 0xBF29 +TAG_LIST_NOTIFICATION = 0xBF28 +TAG_RETRIEVE_NOTIFICATION = 0xBF2B +TAG_NOTIFICATION_METADATA = 0xBF2F +TAG_NOTIFICATION_SENT = 0xBF30 +TAG_ENABLE_PROFILE = 0xBF31 +TAG_DISABLE_PROFILE = 0xBF32 +TAG_DELETE_PROFILE = 0xBF33 +TAG_BPP = 0xBF36 +TAG_PROFILE_INSTALL_RESULT = 0xBF37 +TAG_AUTH_SERVER = 0xBF38 +TAG_CANCEL_SESSION = 0xBF41 + +STATE_LABELS = {0: "disabled", 1: "enabled", 255: "unknown"} +ICON_LABELS = {0: "jpeg", 1: "png", 255: "unknown"} +CLASS_LABELS = {0: "test", 1: "provisioning", 2: "operational", 255: "unknown"} +PROFILE_ERROR_CODES = { + 0x01: "iccidOrAidNotFound", 0x02: "profileNotInDisabledState", + 0x03: "disallowedByPolicy", 0x04: "wrongProfileReenabling", + 0x05: "catBusy", 0x06: "undefinedError", +} +AUTH_SERVER_ERROR_CODES = { + 0x01: "eUICCVerificationFailed", 0x02: "eUICCCertificateExpired", + 0x03: "eUICCCertificateRevoked", 0x05: "invalidServerSignature", + 0x06: "euiccCiPKUnknown", 0x0A: "matchingIdRefused", + 0x10: "insufficientMemory", +} +BPP_COMMAND_NAMES = { + 0: "initialiseSecureChannel", 1: "configureISDP", 2: "storeMetadata", + 3: "storeMetadata2", 4: "replaceSessionKeys", 5: "loadProfileElements", +} +BPP_ERROR_REASONS = { + 1: "incorrectInputValues", 2: "invalidSignature", 3: "invalidTransactionId", + 4: "unsupportedCrtValues", 5: "unsupportedRemoteOperationType", + 6: "unsupportedProfileClass", 7: "scp03tStructureError", 8: "scp03tSecurityError", + 9: "iccidAlreadyExistsOnEuicc", 10: "insufficientMemoryForProfile", + 11: "installInterrupted", 12: "peProcessingError", 13: "dataMismatch", + 14: "invalidNAA", +} +CANCEL_SESSION_REASON = { + 0: "endUserRejection", 1: "postponed", 2: "timeout", + 3: "pprNotAllowed", 127: "undefinedReason", +} + +ESIM_TLS_BUNDLE_ENV = "IQPILOT_ESIM_CA_BUNDLE" +ESIM_QESIM_INIT_ENV = "IQPILOT_ESIM_QESIM_INIT" +ESIM_EXCLUSIVE_MODEM_ENV = "IQPILOT_ESIM_EXCLUSIVE_MODEM" + + +def _ensure_dbus_import(): + try: + import dbus # noqa: F401 + return + except ModuleNotFoundError: + dist_packages = "/usr/lib/python3/dist-packages" + if dist_packages not in sys.path and os.path.isdir(dist_packages): + sys.path.append(dist_packages) + import dbus # noqa: F401 + + +def _tls_verify_option() -> bool | str: + # Default to the GSMA CI bundle used by the reference eSIM branch. + # Allow explicit override for local testing or forced system trust behavior. + override = os.getenv(ESIM_TLS_BUNDLE_ENV, "").strip() + if not override: + return GSMA_CI_BUNDLE + if override.lower() in ("0", "false", "off"): + return False + if override.lower() in ("1", "true", "on", "system"): + return True + return override + + +def b64e(data: bytes) -> str: + return base64.b64encode(data).decode("ascii") + + +def b64d(s: str) -> bytes: + return base64.b64decode(base64_trim(s)) + + +def _decode_apdu_response_blob(raw: str, label: str) -> bytes: + filtered = re.sub(r"[^0-9A-Fa-f]", "", raw) + if len(filtered) < 2 or len(filtered) % 2: + raise RuntimeError(f"Malformed {label} response payload") + return bytes.fromhex(filtered) + + +class AtClient: + def __init__(self, device: str, baud: int, timeout: float, verbose: bool) -> None: + self._device = device + self._baud = baud + self._timeout = timeout + self.verbose = verbose + self.channel: str | None = None + self._serial: serial.Serial | None = None + self._use_csim = False + self._has_qesim = False + self._qesim_lpa_enabled = False + try: + self._serial = serial.Serial(device, baudrate=baud, timeout=timeout) + self._disable_echo() + except (serial.SerialException, PermissionError, OSError): + self._serial = None + if self.verbose: + print(f"!! using ModemManager DBus AT transport for {device}", file=sys.stderr) + + def close(self) -> None: + try: + if self.channel: + try: + self.query(f"AT+CCHC={self.channel}") + except Exception as e: + if self.verbose: + print(f"!! ignoring channel close failure: {e}", file=sys.stderr) + finally: + self.channel = None + finally: + if self._serial is not None: + self._serial.close() + + @staticmethod + def _is_busy_open_error(error: Exception) -> bool: + text = str(error).lower() + return "errno 16" in text or "device or resource busy" in text + + def _disable_echo(self) -> None: + if self._serial is None: + return + self._serial.reset_input_buffer() + self._serial.write(b"ATE0\r") + time.sleep(0.1) + self._serial.reset_input_buffer() + + def _send(self, cmd: str) -> None: + if self.verbose: + print(f">> {cmd}", file=sys.stderr) + assert self._serial is not None + self._serial.write((cmd + "\r").encode("ascii")) + + def _expect(self) -> list[str]: + lines: list[str] = [] + while True: + assert self._serial is not None + raw = self._serial.readline() + if not raw: + raise TimeoutError("AT command timed out") + line = raw.decode(errors="ignore").strip() + if not line: + continue + if self.verbose: + print(f"<< {line}", file=sys.stderr) + if line == "OK": + return lines + if line == "ERROR": + raise RuntimeError("AT command failed") + if "ERROR" in line and line.startswith("+"): + # Catch +CME ERROR / +CMS ERROR and similar modem-specific failures. + raise RuntimeError(f"AT command failed: {line}") + lines.append(line) + + def _get_modem(self): + _ensure_dbus_import() + import dbus + + bus = dbus.SystemBus() + mm = bus.get_object(MM, '/org/freedesktop/ModemManager1') + objects = mm.GetManagedObjects(dbus_interface="org.freedesktop.DBus.ObjectManager", timeout=self._timeout) + modem_paths = [path for path in objects.keys() if "/Modem/" in str(path)] + if not modem_paths: + raise RuntimeError("ModemManager returned no modems") + return bus.get_object(MM, modem_paths[0]) + + def _dbus_query(self, cmd: str) -> list[str]: + if self.verbose: + print(f"DBUS >> {cmd}", file=sys.stderr) + try: + result = str(self._get_modem().Command(cmd, math.ceil(self._timeout), dbus_interface=MM_MODEM, timeout=self._timeout)) + except Exception as e: + raise RuntimeError(f"AT command failed: {e}") from e + lines = [line.strip() for line in result.splitlines() if line.strip()] + if self.verbose: + for line in lines: + print(f"DBUS << {line}", file=sys.stderr) + return lines + + def _reconnect_serial(self) -> None: + self.channel = None + try: + if self._serial is not None: + self._serial.close() + except Exception: + pass + self._serial = serial.Serial(self._device, baudrate=self._baud, timeout=self._timeout) + self._disable_echo() + + def query(self, cmd: str) -> list[str]: + if self._serial is None: + return self._dbus_query(cmd) + try: + self._send(cmd) + return self._expect() + except (OSError, serial.SerialException) as e: + try: + self._reconnect_serial() + self._send(cmd) + return self._expect() + except Exception: + raise RuntimeError(f"AT transport error ({cmd}): {e}") from e + except TimeoutError as e: + raise TimeoutError(f"AT command timed out ({cmd})") from e + except RuntimeError as e: + raise RuntimeError(f"AT command failed ({cmd}): {e}") from e + + def _supports_qesim(self) -> bool: + try: + lines = self.query("AT+QESIM=?") + except Exception: + return False + return any(line.startswith("+QESIM:") for line in lines) + + def _set_quectel_lpa_mode(self) -> None: + self._has_qesim = self._supports_qesim() + self._qesim_lpa_enabled = False + if not self._has_qesim: + return + + # Some Quectel firmwares require enabling internal LPA mode before + # ISO7816 channel open commands (CCHO/CGLA) will succeed. + lpa_enable_cmds = ( + 'AT+QESIM="lpa_enable",1', + 'AT+QESIM="lpa_enable"', + ) + for cmd in lpa_enable_cmds: + try: + self.query(cmd) + self._qesim_lpa_enabled = True + break + except Exception: + continue + + # Keep BIP auth enabled when supported; this is needed by some firmware + # revisions for profile download/auth paths. + try: + lines = self.query('AT+QCFG="bip/auth"?') + needs_enable = any('"bip/auth",0' in line for line in lines) + if needs_enable: + self.query('AT+QCFG="bip/auth",1') + except Exception: + pass + + def ensure_capabilities(self) -> None: + if self._serial is None: + return + + # Some modem firmwares reject command test forms (e.g. AT+CCHO=?) + # while still supporting real execution. Keep these probes non-fatal. + self.query("AT") + # Keep IQ-specific QESIM initialization opt-in for debugging only. + # Default behavior should match upstream greatgitsby/esim transport flow. + if os.getenv(ESIM_QESIM_INIT_ENV, "").strip() == "1": + try: + self.query("AT+CMEE=2") + except Exception: + pass + self._set_quectel_lpa_mode() + for command in ("AT+CCHO", "AT+CCHC", "AT+CGLA", "AT+CSIM"): + try: + self.query(f"{command}=?") + except Exception as e: + if self.verbose: + print(f"!! capability probe failed for {command}: {e}", file=sys.stderr) + + def open_isdr(self) -> None: + try: + self.query(f'AT+CCHC=1') + except Exception: + pass + ccho_candidates = [ + f'AT+CCHO="{ISDR_AID}"', + f"AT+CCHO={ISDR_AID}", + ] + for cmd in ccho_candidates: + try: + lines = self.query(cmd) + except Exception: + continue + for line in lines: + if line.startswith("+CCHO:") and (ch := line.split(":", 1)[1].strip()): + self.channel = ch + return + + # Fallback for modems that reject CCHO but allow APDUs over basic channel. + self.channel = "0" + aid_len = len(ISDR_AID) // 2 + select_isdr = bytes.fromhex(f"00A40400{aid_len:02X}{ISDR_AID}") + try: + _, sw1, sw2 = self.send_apdu(select_isdr) + if (sw1, sw2) == (0x90, 0x00): + return + raise RuntimeError(f"ISD-R SELECT failed on basic channel SW={sw1:02X}{sw2:02X}") + except Exception: + self.channel = None + # Final fallback for modems that only support APDU via CSIM. + try: + _, sw1, sw2 = self._send_csim_apdu(select_isdr) + if (sw1, sw2) == (0x90, 0x00): + self._use_csim = True + self.channel = "csim" + if self.verbose: + print("!! using CSIM APDU transport fallback", file=sys.stderr) + return + raise RuntimeError(f"ISD-R SELECT failed on CSIM SW={sw1:02X}{sw2:02X}") + except Exception: + self.channel = None + if self._has_qesim and not self._qesim_lpa_enabled and os.getenv(ESIM_QESIM_INIT_ENV, "").strip() == "1": + raise RuntimeError( + "Modem exposes AT+QESIM but rejects LPA enable and ISD-R access; " + "this firmware/SKU likely does not provide usable eSIM functionality" + ) + raise RuntimeError("Failed to access eUICC ISD-R (CCHO/CGLA/CSIM all failed)") + + def send_apdu(self, apdu: bytes) -> tuple[bytes, int, int]: + for attempt in range(3): + try: + if not self.channel: + self.open_isdr() + if self._use_csim: + return self._send_csim_apdu(apdu) + hex_payload = apdu.hex().upper() + cmd = f'AT+CGLA={self.channel},{len(hex_payload)},"{hex_payload}"' + for query_attempt in range(2): + for line in self.query(cmd): + if line.startswith("+CGLA:"): + parts = line.split(":", 1)[1].split(",", 1) + if len(parts) == 2: + data = _decode_apdu_response_blob(parts[1].strip().strip('"'), "+CGLA") + if len(data) >= 2: + return data[:-2], data[-2], data[-1] + if query_attempt == 0: + time.sleep(0.2) + raise RuntimeError("Missing +CGLA response") + except RuntimeError: + self.channel = None + self._use_csim = False + if attempt == 2: + raise + time.sleep(1 + attempt) + raise RuntimeError("send_apdu failed") + + def _send_csim_apdu(self, apdu: bytes) -> tuple[bytes, int, int]: + hex_payload = apdu.hex().upper() + cmd = f'AT+CSIM={len(hex_payload)},"{hex_payload}"' + for attempt in range(2): + for line in self.query(cmd): + if line.startswith("+CSIM:"): + parts = line.split(":", 1)[1].split(",", 1) + if len(parts) == 2: + data = _decode_apdu_response_blob(parts[1].strip().strip('"'), "+CSIM") + if len(data) >= 2: + return data[:-2], data[-2], data[-1] + if attempt == 0: + time.sleep(0.2) + raise RuntimeError("Missing +CSIM response") + + +# --- TLV utilities --- + +def iter_tlv(data: bytes, with_positions: bool = False) -> Generator: + idx, length = 0, len(data) + while idx < length: + start_pos = idx + tag = data[idx] + idx += 1 + if tag & 0x1F == 0x1F: # Multi-byte tag + tag_value = tag + while idx < length: + next_byte = data[idx] + idx += 1 + tag_value = (tag_value << 8) | next_byte + if not (next_byte & 0x80): + break + else: + tag_value = tag + if idx >= length: + break + size = data[idx] + idx += 1 + if size & 0x80: # Multi-byte length + num_bytes = size & 0x7F + if idx + num_bytes > length: + break + size = int.from_bytes(data[idx : idx + num_bytes], "big") + idx += num_bytes + if idx + size > length: + break + value = data[idx : idx + size] + idx += size + yield (tag_value, value, start_pos, idx) if with_positions else (tag_value, value) + + +def find_tag(data: bytes, target: int) -> bytes | None: + return next((v for t, v in iter_tlv(data) if t == target), None) + + +def encode_tlv(tag: int, value: bytes) -> bytes: + tag_bytes = bytes([(tag >> 8) & 0xFF, tag & 0xFF]) if tag > 255 else bytes([tag]) + vlen = len(value) + if vlen <= 127: + return tag_bytes + bytes([vlen]) + value + length_bytes = vlen.to_bytes((vlen.bit_length() + 7) // 8, "big") + return tag_bytes + bytes([0x80 | len(length_bytes)]) + length_bytes + value + + +def tbcd_to_string(raw: bytes) -> str: + return "".join(str(n) for b in raw for n in (b & 0x0F, b >> 4) if n <= 9) + + +def string_to_tbcd(s: str) -> bytes: + digits = [int(c) for c in s if c.isdigit()] + return bytes(digits[i] | ((digits[i + 1] if i + 1 < len(digits) else 0xF) << 4) for i in range(0, len(digits), 2)) + + +def base64_trim(s: str) -> str: + return "".join(c for c in s if c not in "\n\r \t") + + +# --- Shared helpers --- + +def _int_bytes(n: int) -> bytes: + """Encode a positive integer as minimal big-endian bytes (at least 1 byte).""" + return n.to_bytes((n.bit_length() + 7) // 8 or 1, "big") + + +def _extract_status(response: bytes, tag: int, name: str) -> int: + """Extract the status byte from a tagged ES10x response.""" + root = find_tag(response, tag) + if root is None: + raise RuntimeError(f"Missing {name}Response") + status = find_tag(root, TAG_STATUS) + if status is None: + raise RuntimeError(f"Missing status in {name}Response") + return status[0] + + +# Profile field decoders: TLV tag -> (field_name, decoder) +_PROFILE_FIELDS = { + TAG_ICCID: ("iccid", tbcd_to_string), + 0x4F: ("isdpAid", lambda v: v.hex().upper()), + 0x9F70: ("profileState", lambda v: STATE_LABELS.get(v[0], "unknown")), + 0x90: ("profileNickname", lambda v: v.decode("utf-8", errors="ignore") or None), + 0x91: ("serviceProviderName", lambda v: v.decode("utf-8", errors="ignore") or None), + 0x92: ("profileName", lambda v: v.decode("utf-8", errors="ignore") or None), + 0x93: ("iconType", lambda v: ICON_LABELS.get(v[0], "unknown")), + 0x94: ("icon", b64e), + 0x95: ("profileClass", lambda v: CLASS_LABELS.get(v[0], "unknown")), +} + + +def _decode_profile_fields(data: bytes) -> dict: + """Parse known profile metadata TLV fields into a dict.""" + result = {} + for tag, value in iter_tlv(data): + if (field := _PROFILE_FIELDS.get(tag)): + result[field[0]] = field[1](value) + return result + + +# Notification field decoders: TLV tag -> (field_name, decoder) +_NOTIF_FIELDS = { + TAG_STATUS: ("seqNumber", lambda v: int.from_bytes(v, "big")), + 0x81: ("profileManagementOperation", lambda v: next((m for m in [0x80, 0x40, 0x20, 0x10] if len(v) >= 2 and v[1] & m), 0xFF)), + 0x0C: ("notificationAddress", lambda v: v.decode("utf-8", errors="ignore")), + TAG_ICCID: ("iccid", tbcd_to_string), +} + + +# --- ES10x command transport --- + +def es10x_command(client: AtClient, data: bytes) -> bytes: + response = bytearray() + sequence = 0 + offset = 0 + while offset < len(data): + chunk = data[offset : offset + ES10X_MSS] + offset += len(chunk) + is_last = offset == len(data) + apdu = bytes([0x80, 0xE2, 0x91 if is_last else 0x11, sequence & 0xFF, len(chunk)]) + chunk + segment, sw1, sw2 = client.send_apdu(apdu) + response.extend(segment) + while True: + if sw1 == 0x61: # More data available + segment, sw1, sw2 = client.send_apdu(bytes([0x80, 0xC0, 0x00, 0x00, sw2 or 0])) + response.extend(segment) + continue + if (sw1 & 0xF0) == 0x90: + break + raise RuntimeError(f"APDU failed with SW={sw1:02X}{sw2:02X}") + sequence += 1 + return bytes(response) + + +# --- ES9P HTTP --- + +def es9p_request(smdp_address: str, endpoint: str, payload: dict, error_prefix: str = "Request") -> dict: + if not system_time_valid(): + raise RuntimeError("System time is not set; TLS certificate validation requires a valid clock") + url = f"https://{smdp_address}/gsma/rsp2/es9plus/{endpoint}" + headers = {"User-Agent": "gsma-rsp-lpad", "X-Admin-Protocol": "gsma/rsp/v2.3.0", "Content-Type": "application/json"} + resp = requests.post(url, json=payload, headers=headers, timeout=30, verify=_tls_verify_option()) + resp.raise_for_status() + if not resp.content: + return {} + data = resp.json() + if "header" in data and "functionExecutionStatus" in data["header"]: + status = data["header"]["functionExecutionStatus"] + if status.get("status") == "Failed": + sd = status.get("statusCodeData", {}) + raise RuntimeError(f"{error_prefix} failed: {sd.get('reasonCode', 'unknown')}/{sd.get('subjectCode', 'unknown')} - {sd.get('message', 'unknown')}") + return data + + +# --- Profile operations --- + +def decode_profiles(blob: bytes) -> list[dict]: + root = find_tag(blob, TAG_PROFILE_INFO_LIST) + if root is None: + raise RuntimeError("Missing ProfileInfoList") + list_ok = find_tag(root, 0xA0) + if list_ok is None: + return [] + defaults = {name: None for name, _ in _PROFILE_FIELDS.values()} + return [{**defaults, **_decode_profile_fields(value)} for tag, value in iter_tlv(list_ok) if tag == 0xE3] + + +def list_profiles(client: AtClient) -> list[dict]: + return decode_profiles(es10x_command(client, bytes.fromhex("BF2D00"))) + + +def _profile_op(client: AtClient, tag: int, iccid: str, refresh: bool, action: str) -> None: + inner = encode_tlv(TAG_ICCID, string_to_tbcd(iccid)) + if tag != TAG_DELETE_PROFILE: + if not refresh: + inner += encode_tlv(0x81, b'\x00') + inner = encode_tlv(0xA0, inner) + code = _extract_status(es10x_command(client, encode_tlv(tag, inner)), tag, f"{action.capitalize()}Profile") + if code == 0x00: + return + if code == 0x02 and tag != TAG_DELETE_PROFILE: + print(f"profile {iccid} already {action}d") + return + raise RuntimeError(f"{action.capitalize()}Profile failed: {PROFILE_ERROR_CODES.get(code, 'unknown')} (0x{code:02X})") + + +def enable_profile(client: AtClient, iccid: str, refresh: bool = True) -> None: + _profile_op(client, TAG_ENABLE_PROFILE, iccid, refresh, "enable") + + +def disable_profile(client: AtClient, iccid: str, refresh: bool = True) -> None: + _profile_op(client, TAG_DISABLE_PROFILE, iccid, refresh, "disable") + + +def delete_profile(client: AtClient, iccid: str) -> None: + _profile_op(client, TAG_DELETE_PROFILE, iccid, True, "delete") + + +def set_profile_nickname(client: AtClient, iccid: str, nickname: str) -> None: + nickname_bytes = nickname.encode("utf-8") + if len(nickname_bytes) > 64: + raise ValueError("Profile nickname must be 64 bytes or less") + content = encode_tlv(TAG_ICCID, string_to_tbcd(iccid)) + encode_tlv(0x90, nickname_bytes) + code = _extract_status(es10x_command(client, encode_tlv(TAG_SET_NICKNAME, content)), TAG_SET_NICKNAME, "SetNickname") + if code == 0x01: + raise RuntimeError(f"profile {iccid} not found") + if code != 0x00: + raise RuntimeError(f"SetNickname failed with status 0x{code:02X}") + + +# --- Notifications --- + +def list_notifications(client: AtClient) -> list[dict]: + response = es10x_command(client, encode_tlv(TAG_LIST_NOTIFICATION, b"")) + root = find_tag(response, TAG_LIST_NOTIFICATION) + if root is None: + raise RuntimeError("Missing ListNotificationResponse") + metadata_list = find_tag(root, 0xA0) + if metadata_list is None: + return [] + notifications: list[dict] = [] + for tag, value in iter_tlv(metadata_list): + if tag != TAG_NOTIFICATION_METADATA: + continue + notification = {"seqNumber": None, "profileManagementOperation": None, "notificationAddress": None, "iccid": None} + for t, v in iter_tlv(value): + if (field := _NOTIF_FIELDS.get(t)): + notification[field[0]] = field[1](v) + if notification["seqNumber"] is not None and notification["profileManagementOperation"] is not None and notification["notificationAddress"]: + notifications.append(notification) + return notifications + + +def retrieve_notification(client: AtClient, seq_number: int) -> dict: + request = encode_tlv(TAG_RETRIEVE_NOTIFICATION, encode_tlv(0xA0, encode_tlv(TAG_STATUS, _int_bytes(seq_number)))) + response = es10x_command(client, request) + root = find_tag(response, TAG_RETRIEVE_NOTIFICATION) + if root is None: + raise RuntimeError("Invalid RetrieveNotificationsListResponse") + a0_content = find_tag(root, 0xA0) + if a0_content is None: + raise RuntimeError("Invalid RetrieveNotificationsListResponse") + pending_notif, pending_tag = None, None + for tag, value in iter_tlv(a0_content): + if tag in (TAG_PROFILE_INSTALL_RESULT, 0x30): + pending_notif, pending_tag = value, tag + break + if pending_notif is None: + raise RuntimeError("Missing PendingNotification") + if pending_tag == TAG_PROFILE_INSTALL_RESULT: + result_data = find_tag(pending_notif, 0xBF27) + notif_meta = find_tag(result_data, TAG_NOTIFICATION_METADATA) if result_data else None + else: + notif_meta = find_tag(pending_notif, TAG_NOTIFICATION_METADATA) + if notif_meta is None: + raise RuntimeError("Missing NotificationMetadata") + addr = find_tag(notif_meta, 0x0C) + if addr is None: + raise RuntimeError("Missing notificationAddress") + return {"notificationAddress": addr.decode("utf-8", errors="ignore"), "b64_PendingNotification": b64e(pending_notif)} + + +def remove_notification(client: AtClient, seq_number: int) -> None: + response = es10x_command(client, encode_tlv(TAG_NOTIFICATION_SENT, encode_tlv(TAG_STATUS, _int_bytes(seq_number)))) + root = find_tag(response, TAG_NOTIFICATION_SENT) + if root is None: + raise RuntimeError("Invalid NotificationSentResponse") + status = find_tag(root, TAG_STATUS) + if status is None or int.from_bytes(status, "big") != 0: + raise RuntimeError("RemoveNotificationFromList failed") + + +def process_notifications(client: AtClient) -> None: + for notification in list_notifications(client): + seq_number, smdp_address = notification["seqNumber"], notification["notificationAddress"] + if not seq_number or not smdp_address: + continue + try: + notif_data = retrieve_notification(client, seq_number) + es9p_request(smdp_address, "handleNotification", {"pendingNotification": notif_data["b64_PendingNotification"]}, "HandleNotification") + remove_notification(client, seq_number) + except Exception: + pass + + +# --- Authentication & Download --- + +def get_challenge_and_info(client: AtClient) -> tuple[bytes, bytes]: + challenge_resp = es10x_command(client, encode_tlv(TAG_EUICC_CHALLENGE, b"")) + root = find_tag(challenge_resp, TAG_EUICC_CHALLENGE) + if root is None: + raise RuntimeError("Missing GetEuiccDataResponse") + challenge = find_tag(root, TAG_STATUS) + if challenge is None: + raise RuntimeError("Missing challenge in response") + info_resp = es10x_command(client, encode_tlv(TAG_EUICC_INFO, b"")) + if not info_resp.startswith(bytes([0xBF, 0x20])): + raise RuntimeError("Missing GetEuiccInfo1Response") + return challenge, info_resp + + +def authenticate_server(client: AtClient, b64_signed1: str, b64_sig1: str, b64_pk_id: str, b64_cert: str, matching_id: str | None = None) -> str: + # Build request + tac = bytes([0x35, 0x29, 0x06, 0x11]) + device_info = encode_tlv(TAG_STATUS, tac) + encode_tlv(0xA1, b"") + ctx_inner = b"" + if matching_id: + ctx_inner += encode_tlv(TAG_STATUS, matching_id.encode("utf-8")) + ctx_inner += encode_tlv(0xA1, device_info) + content = base64.b64decode(b64_signed1) + base64.b64decode(b64_sig1) + base64.b64decode(b64_pk_id) + base64.b64decode(b64_cert) + encode_tlv(0xA0, ctx_inner) + request = encode_tlv(TAG_AUTH_SERVER, content) + + response = es10x_command(client, request) + if not response.startswith(bytes([0xBF, 0x38])): + raise RuntimeError("Invalid AuthenticateServerResponse") + + # Check for eUICC-side errors + root = find_tag(response, TAG_AUTH_SERVER) + if root is not None: + error_tag = find_tag(root, 0xA1) + if error_tag is not None: + code = int.from_bytes(error_tag, "big") if error_tag else 0 + desc = AUTH_SERVER_ERROR_CODES.get(code, "unknown") + raise RuntimeError(f"AuthenticateServer rejected by eUICC: {desc} (0x{code:02X})") + + return b64e(response) + + +def prepare_download(client: AtClient, b64_signed2: str, b64_sig2: str, b64_cert: str, cc: str | None = None) -> str: + smdp_signed2 = base64.b64decode(b64_signed2) + smdp_signature2 = base64.b64decode(b64_sig2) + smdp_certificate = base64.b64decode(b64_cert) + smdp_signed2_root = find_tag(smdp_signed2, 0x30) + if smdp_signed2_root is None: + raise RuntimeError("Invalid smdpSigned2") + transaction_id = find_tag(smdp_signed2_root, TAG_STATUS) + cc_required_flag = find_tag(smdp_signed2_root, 0x01) + if transaction_id is None or cc_required_flag is None: + raise RuntimeError("Invalid smdpSigned2") + content = smdp_signed2 + smdp_signature2 + if int.from_bytes(cc_required_flag, "big") != 0: + if not cc: + raise RuntimeError("Confirmation code required but not provided") + content += encode_tlv(0x04, hashlib.sha256(hashlib.sha256(cc.encode("utf-8")).digest() + transaction_id).digest()) + content += smdp_certificate + response = es10x_command(client, encode_tlv(TAG_PREPARE_DOWNLOAD, content)) + if not response.startswith(bytes([0xBF, 0x21])): + raise RuntimeError("Invalid PrepareDownloadResponse") + return b64e(response) + + +def _parse_tlv_header_len(data: bytes) -> int: + """Return the combined tag + length header size for a TLV element.""" + tag_len = 2 if data[0] & 0x1F == 0x1F else 1 + length_byte = data[tag_len] + return tag_len + (1 + (length_byte & 0x7F) if length_byte & 0x80 else 1) + + +def load_bpp(client: AtClient, b64_bpp: str) -> dict: + bpp = b64d(b64_bpp) + if not bpp.startswith(bytes([0xBF, 0x36])): + raise RuntimeError("Invalid BoundProfilePackage") + + bpp_root_value = None + for tag, value, start, end in iter_tlv(bpp, with_positions=True): + if tag == TAG_BPP: + bpp_root_value = value + bpp_value_start = start + _parse_tlv_header_len(bpp[start:end]) + break + if bpp_root_value is None: + raise RuntimeError("Invalid BoundProfilePackage") + + chunks = [] + for tag, value, start, end in iter_tlv(bpp_root_value, with_positions=True): + if tag == 0xBF23: + chunks.append(bpp[0 : bpp_value_start + end]) + elif tag == 0xA0: + chunks.append(bpp[bpp_value_start + start : bpp_value_start + end]) + elif tag in (0xA1, 0xA3): + hdr_len = _parse_tlv_header_len(bpp_root_value[start:end]) + chunks.append(bpp[bpp_value_start + start : bpp_value_start + start + hdr_len]) + for _, _, child_start, child_end in iter_tlv(value, with_positions=True): + chunks.append(value[child_start:child_end]) + elif tag == 0xA2: + chunks.append(bpp[bpp_value_start + start : bpp_value_start + end]) + + result = {"seqNumber": 0, "success": False, "bppCommandId": None, "errorReason": None} + for chunk in chunks: + response = es10x_command(client, chunk) + if not response: + continue + root = find_tag(response, TAG_PROFILE_INSTALL_RESULT) + if not root: + continue + result_data = find_tag(root, 0xBF27) + if not result_data: + break + notif_meta = find_tag(result_data, TAG_NOTIFICATION_METADATA) + if notif_meta: + seq_num = find_tag(notif_meta, TAG_STATUS) + if seq_num: + result["seqNumber"] = int.from_bytes(seq_num, "big") + final_result = find_tag(result_data, 0xA2) + if final_result: + for tag, value in iter_tlv(final_result): + if tag == 0xA0: + result["success"] = True + elif tag == 0xA1: + bpp_cmd = find_tag(value, TAG_STATUS) + if bpp_cmd: + result["bppCommandId"] = int.from_bytes(bpp_cmd, "big") + err = find_tag(value, 0x81) + if err: + result["errorReason"] = int.from_bytes(err, "big") + break # ProfileInstallResult received — eUICC session is finalized + if not result["success"] and result["errorReason"] is not None: + cmd_name = BPP_COMMAND_NAMES.get(result["bppCommandId"], f"unknown({result['bppCommandId']})") + err_name = BPP_ERROR_REASONS.get(result["errorReason"], f"unknown({result['errorReason']})") + raise RuntimeError(f"Profile installation failed at {cmd_name}: {err_name} (bppCommandId={result['bppCommandId']}, errorReason={result['errorReason']})") + return result + + +def parse_metadata(b64_metadata: str) -> dict: + root = find_tag(b64d(b64_metadata), 0xBF25) + if root is None: + raise RuntimeError("Invalid profileMetadata") + defaults = {"iccid": None, "serviceProviderName": None, "profileName": None, "iconType": None, "icon": None, "profileClass": None} + return {**defaults, **_decode_profile_fields(root)} + + +def cancel_session(client: AtClient, transaction_id: bytes, reason: int = 127) -> str: + content = encode_tlv(0x80, transaction_id) + encode_tlv(0x81, bytes([reason])) + response = es10x_command(client, encode_tlv(TAG_CANCEL_SESSION, content)) + return b64e(response) + + +def parse_lpa_activation_code(activation_code: str) -> tuple[str, str, str]: + if not activation_code.startswith("LPA:"): + raise ValueError("Invalid activation code format") + + parts = activation_code[4:].split("$") + if len(parts) != 3: + raise ValueError("Invalid activation code format") + + version, smdp, matching_id = [p.strip() for p in parts] + if version != "1": + raise ValueError("Unsupported activation code version") + if len(smdp) == 0 or "." not in smdp: + raise ValueError("Invalid SM-DP+ address in activation code") + if len(matching_id) == 0: + raise ValueError("Missing matching ID in activation code") + return version, smdp, matching_id + + +def download_profile(client: AtClient, activation_code: str) -> str: + _, smdp, matching_id = parse_lpa_activation_code(activation_code) + + challenge, euicc_info = get_challenge_and_info(client) + + payload = {"smdpAddress": smdp, "euiccChallenge": b64e(challenge), "euiccInfo1": b64e(euicc_info)} + if matching_id: + payload["matchingId"] = matching_id + auth = es9p_request(smdp, "initiateAuthentication", payload, "Authentication") + tx_id = base64_trim(auth.get("transactionId", "")) + tx_id_bytes = base64.b64decode(tx_id) if tx_id else b"" + + try: + b64_auth_resp = authenticate_server( + client, base64_trim(auth.get("serverSigned1", "")), base64_trim(auth.get("serverSignature1", "")), + base64_trim(auth.get("euiccCiPKIdToBeUsed", "")), base64_trim(auth.get("serverCertificate", "")), + matching_id=matching_id) + + cli = es9p_request(smdp, "authenticateClient", {"transactionId": tx_id, "authenticateServerResponse": b64_auth_resp}, "Authentication") + metadata = parse_metadata(base64_trim(cli.get("profileMetadata", ""))) + iccid = str(metadata.get("iccid") or "") + print(f'Downloading profile: {iccid} - {metadata["serviceProviderName"]} - {metadata["profileName"]}') + + b64_prep = prepare_download( + client, base64_trim(cli.get("smdpSigned2", "")), base64_trim(cli.get("smdpSignature2", "")), base64_trim(cli.get("smdpCertificate", ""))) + + bpp = es9p_request(smdp, "getBoundProfilePackage", {"transactionId": tx_id, "prepareDownloadResponse": b64_prep}, "GetBoundProfilePackage") + + result = load_bpp(client, base64_trim(bpp.get("boundProfilePackage", ""))) + if result["success"]: + print(f"Profile installed successfully (seqNumber: {result['seqNumber']})") + return iccid + else: + raise RuntimeError(f"Profile installation failed: {result}") + except Exception: + if tx_id_bytes: + b64_cancel_resp = "" + try: + b64_cancel_resp = cancel_session(client, tx_id_bytes) + except Exception: + pass + try: + es9p_request(smdp, "cancelSession", { + "transactionId": tx_id, + "cancelSessionResponse": b64_cancel_resp, + }, "CancelSession") + except Exception: + pass + raise + + +class TiciLPA(LPABase): + handles_modem_restart = True + _instance = None + + def __new__(cls, *args, **kwargs): + if cls._instance is None: + cls._instance = super().__new__(cls) + return cls._instance + + def __init__(self, interface: Literal["qmi", "at"] = "at", device: str = DEFAULT_DEVICE, baud: int = DEFAULT_BAUD, + timeout: float = DEFAULT_TIMEOUT, verbose: bool = False): + if getattr(self, "_initialized", False): + self.verbose = verbose or self.verbose + return + # This backend is AT-only. Keep interface arg for compatibility with existing CLI/call sites. + self.interface = interface + self.device = device + self.baud = baud + self.timeout = timeout + self.verbose = verbose + self._last_good_device = device + self._euicc_present_cache: bool | None = None + from iqpilot.system.hardware.tici.hardware import get_device_type + self._is_eg25 = get_device_type() in ("tizi",) + self._client: AtClient | None = None + self._initialized = True + # _client is opened lazily via _ensure_client() on first actual eSIM operation. + # Do NOT call _open_ready_client() here: on pSIM devices (no eUICC) it retries + # for 10–30 s against the AT port, blocking hw_state_thread and starving the + # network-type polling loop that drives the WiFi/LTE sidebar indicator. + atexit.register(self.close) + + def close(self) -> None: + if self._client is not None: + try: + self._client.close() + finally: + self._client = None + + @staticmethod + def _set_modemmanager_running(running: bool) -> None: + action = "start" if running else "stop" + subprocess.check_call(["sudo", "systemctl", action, "ModemManager"]) + + @staticmethod + def _is_modemmanager_running() -> bool: + res = subprocess.run(["systemctl", "is-active", "ModemManager"], + capture_output=True, text=True, check=False) + return res.stdout.strip() == "active" + + def _probe_euicc_present(self) -> bool | None: + if self._euicc_present_cache is not None: + return self._euicc_present_cache + + if self._is_modemmanager_running(): + return None + + restarted_modemmanager = False + try: + res = subprocess.run( + ["sudo", "qmicli", "-p", "-d", "/dev/cdc-wdm0", "--uim-get-slot-status"], + capture_output=True, text=True, check=False, timeout=12, + ) + output = f"{res.stdout}\n{res.stderr}" + if "Is eUICC: yes" in output: + self._euicc_present_cache = True + elif "Is eUICC: no" in output: + self._euicc_present_cache = False + else: + self._euicc_present_cache = None + except Exception: + self._euicc_present_cache = None + finally: + if restarted_modemmanager: + self._set_modemmanager_running(True) + time.sleep(2.0) + + return self._euicc_present_cache + + @staticmethod + def _ttyusb2_mm_ignored() -> bool: + # AGNOS PR 521 marks ttyUSB2 with ID_MM_PORT_IGNORE=1 for EG25. + # If missing, ModemManager may contend with direct AT eSIM operations. + if not os.path.exists("/dev/ttyUSB2"): + return False + try: + res = subprocess.run( + ["udevadm", "info", "-q", "property", "-n", "/dev/ttyUSB2"], + capture_output=True, text=True, check=False, + ) + if res.returncode != 0: + return False + return "ID_MM_PORT_IGNORE=1" in res.stdout + except Exception: + return False + + def _ensure_client(self) -> AtClient: + if self._client is None: + self._client = self._open_ready_client() + return self._client + + @contextmanager + def _open_client(self): + yield self._ensure_client() + + def _open_ready_client(self) -> AtClient: + euicc_present = self._probe_euicc_present() + if euicc_present is False: + raise LPAError("Modem reports no eUICC support on this unit (QMI: Is eUICC: no)") + + last_error: Exception | None = None + recovery_attempted = False + for attempt in range(1, 6): + for device in self._candidate_devices(): + client: AtClient | None = None + try: + if self.verbose: + print(f"!! trying modem AT device: {device} (attempt {attempt})", file=sys.stderr) + client = AtClient(device, self.baud, self.timeout, self.verbose) + client.ensure_capabilities() + client.open_isdr() + self._last_good_device = device + if self.verbose: + print(f"!! selected modem AT device: {device}", file=sys.stderr) + return client + except Exception as e: + if self.verbose: + print(f"!! modem AT device failed: {device}: {e}", file=sys.stderr) + last_error = e + if client is not None: + try: + client.close() + except Exception: + pass + if not recovery_attempted and last_error is not None and self._should_recover_isdr(last_error): + recovery_attempted = self._recover_isdr_access() + if recovery_attempted: + continue + # Give modem a moment to finish startup / channel release. + time.sleep(min(0.4 * attempt, 1.5)) + assert last_error is not None + raise last_error + + def _candidate_devices(self) -> list[str]: + if self.device and not os.path.exists(self.device): + return [self.device] + + candidates = [ + self._last_good_device, + self.device, + "/dev/modem_at0", + "/dev/ttyUSB3", + "/dev/ttyUSB2", + "/dev/ttyUSB1", + "/dev/ttyUSB0", + ] + ordered: list[str] = [] + for dev in candidates: + should_include = dev in (self.device, "/dev/modem_at0") or os.path.exists(dev) + if dev and dev not in ordered and should_include: + ordered.append(dev) + return ordered if ordered else [self.device] + + @staticmethod + def _to_profile(data: dict) -> Profile: + return Profile( + iccid=str(data.get("iccid") or ""), + nickname=str(data.get("profileNickname") or ""), + enabled=str(data.get("profileState") or "") == "enabled", + provider=str(data.get("serviceProviderName") or ""), + ) + + def _clear_cat_busy(self) -> None: + time.sleep(3) + try: + self._ensure_client().query('AT+QSTKRSP=254') + except Exception: + pass + time.sleep(1) + try: + self._ensure_client().query('AT+CFUN=4') + except Exception: + pass + time.sleep(2) + try: + self._ensure_client().query('AT+CFUN=1') + except Exception: + pass + self._ensure_client().channel = None + time.sleep(5) + + def _wait_for_modem(self, timeout: float = 45.0) -> None: + start = time.monotonic() + last_error: Exception | None = None + while time.monotonic() - start < timeout: + try: + self.close() + self._client = self._open_ready_client() + return + except Exception as e: + last_error = e + time.sleep(2) + if last_error is not None: + raise RuntimeError("Modem did not recover after reboot") from last_error + raise RuntimeError("Modem did not recover after reboot") + + def _restart_modem(self) -> None: + client = self._ensure_client() + for cmd in ('AT+CFUN=0', 'AT+CFUN=1,1'): + try: + client.query(cmd) + except Exception: + pass + client.channel = None + client._use_csim = False + self._wait_for_modem() + + @staticmethod + def _should_recover_isdr(error: Exception) -> bool: + message = str(error).lower() + markers = ( + "failed to open isd-r", + "failed to access euicc isd-r", + "simfilenotfound", + "at+ccho", + "at+cgla", + ) + return any(marker in message for marker in markers) + + def _recover_isdr_access(self) -> bool: + was_modemmanager_running = self._is_modemmanager_running() + self.close() + try: + if was_modemmanager_running: + self._set_modemmanager_running(False) + time.sleep(2.0) + + for cmd in ( + ["sudo", "qmicli", "-p", "-d", "/dev/cdc-wdm0", "--uim-reset"], + ["sudo", "qmicli", "-p", "-d", "/dev/cdc-wdm0", "--uim-sim-power-off=1"], + ["sudo", "qmicli", "-p", "-d", "/dev/cdc-wdm0", "--uim-sim-power-on=1"], + ): + subprocess.check_call(cmd) + time.sleep(2.0) + + self._euicc_present_cache = None + return True + except Exception as e: + if self.verbose: + print(f"!! ISD-R recovery failed: {e}", file=sys.stderr) + return False + finally: + if was_modemmanager_running: + self._set_modemmanager_running(True) + time.sleep(2.0) + + def _with_lpa_error(self, fn): + last_error: Exception | None = None + recovery_attempted = False + for attempt in range(3): + try: + return fn() + except LPAError: + raise + except RuntimeError as e: + last_error = e + if "does not provide usable esim functionality" in str(e).lower(): + raise LPAError(str(e)) from e + if not recovery_attempted and self._should_recover_isdr(e): + recovery_attempted = self._recover_isdr_access() + if recovery_attempted: + continue + if self._is_transient_transport_error(str(e)) and attempt == 0: + self._wait_for_modem() + continue + raise LPAError(str(e)) from e + except (OSError, serial.SerialException) as e: + last_error = e + if not recovery_attempted and self._should_recover_isdr(e): + recovery_attempted = self._recover_isdr_access() + if recovery_attempted: + continue + if self._is_transient_transport_error(str(e)) and attempt == 0: + self._wait_for_modem() + continue + raise LPAError(f"AT transport error: {e}") from e + except TimeoutError as e: + raise LPAError(f"AT modem timeout: {e}") from e + except ValueError as e: + raise LPAError(str(e)) from e + assert last_error is not None + raise LPAError(str(last_error)) + + @staticmethod + def _is_transient_transport_error(message: str) -> bool: + lower = message.lower() + transient_markers = ( + "broken pipe", + "resource busy", + "input/output error", + "device disconnected", + "device reports readiness to read but returned no data", + "transport error", + "missing +cgla response", + "missing +csim response", + "malformed +cgla response payload", + "malformed +csim response payload", + "non-hexadecimal number found in fromhex", + ) + return any(m in lower for m in transient_markers) + + def list_profiles(self) -> list[Profile]: + def _run() -> list[Profile]: + return [self._to_profile(p) for p in list_profiles(self._ensure_client())] + return self._with_lpa_error(_run) + + def get_active_profile(self) -> Profile | None: + return next((p for p in self.list_profiles() if p.enabled), None) + + def _validate_profile_exists(self, iccid: str) -> None: + if not any(p.iccid == iccid for p in self.list_profiles()): + raise LPAProfileNotFoundError(f"profile {iccid} does not exist") + + def _is_bootstrapped(self) -> bool: + return not any(self.is_comma_profile(p.iccid) for p in self.list_profiles()) + + def _ensure_switchable_profile(self, iccid: str) -> None: + if self.is_comma_profile(iccid): + return + + if not self._is_bootstrapped(): + raise LPAError( + "Delete the existing Comma pSIM profile before activating a user eSIM profile" + ) + + def _process_notifications_after_delete(self, client: AtClient, iccid: str) -> None: + try: + process_notifications(client) + return + except Exception as error: + notification_error = error + + self._restart_modem() + + try: + deleted = not any(p.iccid == iccid for p in self.list_profiles()) + except Exception: + raise RuntimeError( + "Profile delete may have succeeded, but modem notification cleanup failed; " + "refresh profiles and retry if needed" + ) from notification_error + + if deleted: + return + + raise RuntimeError( + "Profile delete did not finish cleanly; refresh profiles and retry" + ) from notification_error + + def _process_notifications_after_state_change( + self, + validator, + recovery_message: str, + failure_message: str, + ) -> None: + try: + process_notifications(self._ensure_client()) + return + except Exception as notification_error: + pass + + self._wait_for_modem() + + try: + if validator(): + return + except Exception: + raise RuntimeError(recovery_message) from notification_error + + raise RuntimeError(failure_message) from notification_error + + def delete_profile(self, iccid: str) -> None: + self._validate_profile_exists(iccid) + active = self.get_active_profile() + if active is not None and active.iccid == iccid: + raise LPAError("cannot delete active profile, switch to another profile first") + + def _run() -> None: + client = self._ensure_client() + try: + delete_profile(client, iccid) + except RuntimeError as error: + if "catbusy" in str(error).lower(): + self._clear_cat_busy() + delete_profile(client, iccid) + else: + raise + self._process_notifications_after_delete(client, iccid) + + self._with_lpa_error(_run) + + def bootstrap(self) -> None: + comma_profiles = [p for p in self.list_profiles() if self.is_comma_profile(p.iccid)] + if not comma_profiles: + return + + def _run() -> None: + client = self._ensure_client() + for profile in comma_profiles: + if profile.enabled: + disable_profile(client, profile.iccid, refresh=True) + process_notifications(client) + delete_profile(client, profile.iccid) + process_notifications(client) + self._restart_modem() + + self._with_lpa_error(_run) + + def download_profile(self, qr: str, nickname: str | None = None) -> None: + # Strict upfront validation to provide immediate user feedback. + parse_lpa_activation_code(qr) + before = {p.iccid for p in self.list_profiles()} + + def _run() -> None: + client = self._ensure_client() + iccid = download_profile(client, qr) + self._process_notifications_after_state_change( + lambda: any(profile.iccid == iccid for profile in self.list_profiles()), + "Profile add may have succeeded, but modem notification cleanup failed; refresh profiles", + "Profile add did not finish cleanly; refresh profiles and retry", + ) + + self._with_lpa_error(_run) + + if nickname: + after = self.list_profiles() + new_profile = next((p for p in after if p.iccid not in before), None) + if new_profile is not None and new_profile.iccid: + self.nickname_profile(new_profile.iccid, nickname) + + def nickname_profile(self, iccid: str, nickname: str) -> None: + self._validate_profile_exists(iccid) + + def _run() -> None: + set_profile_nickname(self._ensure_client(), iccid, nickname) + self._with_lpa_error(_run) + + def switch_profile(self, iccid: str) -> None: + self._validate_profile_exists(iccid) + self._ensure_switchable_profile(iccid) + active = self.get_active_profile() + if active is not None and active.iccid == iccid: + return + + def _run() -> None: + client = self._ensure_client() + use_refresh = self._is_eg25 + try: + enable_profile(client, iccid, refresh=use_refresh) + except RuntimeError as error: + if "catbusy" in str(error).lower(): + self._clear_cat_busy() + enable_profile(client, iccid, refresh=use_refresh) + else: + raise + client.channel = None + client._use_csim = False + if use_refresh: + self._wait_for_modem() + else: + self._restart_modem() + self._process_notifications_after_state_change( + lambda: any(profile.iccid == iccid and profile.enabled for profile in self.list_profiles()), + "Profile switch may have succeeded, but modem notification cleanup failed; refresh profiles", + "Profile switch did not finish cleanly; refresh profiles and retry", + ) + self._with_lpa_error(_run) + + # Compatibility methods retained for old tests/scripts. + def process_notifications(self) -> None: + def _run() -> None: + process_notifications(self._ensure_client()) + self._with_lpa_error(_run) + + def enable_profile(self, iccid: str, refresh: bool = True) -> None: + self._validate_profile_exists(iccid) + + def _run() -> None: + client = self._ensure_client() + enable_profile(client, iccid, refresh=refresh) + client.channel = None + client._use_csim = False + self._wait_for_modem() + self._process_notifications_after_state_change( + lambda: any(profile.iccid == iccid and profile.enabled for profile in self.list_profiles()), + "Profile enable may have succeeded, but modem notification cleanup failed; refresh profiles", + "Profile enable did not finish cleanly; refresh profiles and retry", + ) + self._with_lpa_error(_run) + + def disable_profile(self, iccid: str, refresh: bool = True) -> None: + self._validate_profile_exists(iccid) + + def _run() -> None: + client = self._ensure_client() + disable_profile(client, iccid, refresh=refresh) + client.channel = None + client._use_csim = False + self._wait_for_modem() + self._process_notifications_after_state_change( + lambda: any(profile.iccid == iccid and not profile.enabled for profile in self.list_profiles()), + "Profile disable may have succeeded, but modem notification cleanup failed; refresh profiles", + "Profile disable did not finish cleanly; refresh profiles and retry", + ) + self._with_lpa_error(_run) + + +# --- CLI --- + +def main() -> None: + parser = argparse.ArgumentParser(description="Minimal AT-only LPA implementation") + parser.add_argument("--device", default=DEFAULT_DEVICE) + parser.add_argument("--baud", type=int, default=DEFAULT_BAUD) + parser.add_argument("--timeout", type=float, default=DEFAULT_TIMEOUT) + parser.add_argument("--verbose", action="store_true") + parser.add_argument("--enable", type=str) + parser.add_argument("--disable", type=str) + parser.add_argument("--delete", type=str, metavar="ICCID", help="Delete a disabled profile") + parser.add_argument("--no-refresh", action="store_true", help="Skip REFRESH after enable/disable") + parser.add_argument("--set-nickname", nargs=2, metavar=("ICCID", "NICKNAME")) + parser.add_argument("--list-notifications", action="store_true") + parser.add_argument("--process-notifications", action="store_true") + parser.add_argument("--download", type=str, metavar="CODE") + args = parser.parse_args() + + lpa = TiciLPA(device=args.device, baud=args.baud, timeout=args.timeout, verbose=args.verbose) + + if args.enable: + lpa.enable_profile(args.enable, refresh=not args.no_refresh) + elif args.disable: + lpa.disable_profile(args.disable, refresh=not args.no_refresh) + elif args.delete: + lpa.delete_profile(args.delete) + elif args.set_nickname: + lpa.nickname_profile(args.set_nickname[0], args.set_nickname[1]) + elif args.list_notifications: + with lpa._open_client() as client: + print(json.dumps(list_notifications(client), indent=2)) + return + elif args.process_notifications: + lpa.process_notifications() + return + elif args.download: + lpa.download_profile(args.download) + + profiles_out = [{ + "iccid": p.iccid, + "profileNickname": p.nickname, + "profileState": "enabled" if p.enabled else "disabled", + "serviceProviderName": p.provider, + } for p in lpa.list_profiles()] + print(json.dumps(profiles_out, indent=2)) + + +if __name__ == "__main__": + main() diff --git a/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/system/hardware/tici/pins.py b/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/system/hardware/tici/pins.py new file mode 100644 index 000000000..d53183f7a --- /dev/null +++ b/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/system/hardware/tici/pins.py @@ -0,0 +1,30 @@ +# GPIO pin definitions +class GPIO: + # both GPIO_STM_RST_N and GPIO_LTE_RST_N are misnamed, they are high to reset + HUB_RST_N = 30 + UBLOX_RST_N = 32 + UBLOX_SAFEBOOT_N = 33 + GNSS_PWR_EN = 34 # SCHEMATIC LABEL: GPIO_UBLOX_PWR_EN + + STM_RST_N = 124 + STM_BOOT0 = 134 + STM_PWR_EN_N = 41 # because STM32H7 RST doesn't generate a full power-on-reset + + SIREN = 42 + SOM_ST_IO = 49 + + LTE_RST_N = 50 + LTE_PWRKEY = 116 + LTE_BOOT = 52 + + # GPIO_CAM0_DVDD_EN = /sys/kernel/debug/regulator/camera_rear_ldo + CAM0_AVDD_EN = 8 + CAM0_RSTN = 9 + CAM1_RSTN = 7 + CAM2_RSTN = 12 + + # Sensor interrupts + BMX055_ACCEL_INT = 21 + BMX055_GYRO_INT = 23 + BMX055_MAGN_INT = 87 + LSM_INT = 84 diff --git a/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/system/hardware/tici/usb_storage.py b/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/system/hardware/tici/usb_storage.py new file mode 100644 index 000000000..7dcb52214 --- /dev/null +++ b/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/system/hardware/tici/usb_storage.py @@ -0,0 +1,71 @@ +import os +import subprocess + +from iqpilot.common.params import Params + +SCRIPT_PATH = os.path.join(os.path.dirname(os.path.abspath(__file__)), "set_usb_storage.sh") + + +def apply_usb_storage_state(state: bool): + Params().put_bool("UsbStorageEnabled", state) + try: + args = ["sudo", SCRIPT_PATH] + if state: + args.append("--rebuild") + subprocess.Popen(args) + except OSError: + pass + + +NCM_TRIED_MARKER = "/tmp/.iqemac_ncm_provisioned" +MAX_NCM_ATTEMPTS = 3 + + +def _ncm_attempts() -> int: + try: + with open(NCM_TRIED_MARKER) as f: + return int(f.read().strip() or 0) + except (OSError, ValueError): + return 0 + + +def ensure_ncm_gadget() -> bool: + # configfs is RAM backed, so the gadget must be rebuilt every boot; binding it + # enumerates on the host, which must not happen mid-drive + if os.path.isdir("/sys/class/net/usb0"): + return True + params = Params() + from iqpilot.selfdrive.iqmodeld.egpu_helpers import egpu_selected + if not (params.get_bool("IQEmacEnabled") or egpu_selected(params)): + return False + attempts = _ncm_attempts() + if attempts >= MAX_NCM_ATTEMPTS: + return False + try: + # stamped before the run: the gadget build can wedge configfs in + # uninterruptible D state, and retrying that forever helps nobody + with open(NCM_TRIED_MARKER, "w") as f: + f.write(str(attempts + 1)) + subprocess.run(["sudo", "-n", SCRIPT_PATH], timeout=120, check=False) + except (OSError, subprocess.SubprocessError): + return False + return os.path.isdir("/sys/class/net/usb0") + + +INPUT_SUSPEND = "/sys/class/power_supply/battery/input_suspend" + + +def suspend_usb_input(suspend: bool = True) -> bool: + # a host on the data port makes the PMIC sink USB-PD while OBD-C feeds the same + # rail; the SOM browns out. This closes the charge path only, data is untouched + if not os.path.exists(INPUT_SUSPEND): + return False + try: + with open(INPUT_SUSPEND) as f: + if f.read().strip() == ("1" if suspend else "0"): + return True + except OSError: + pass + rc = subprocess.run(["sudo", "-n", "sh", "-c", f"echo {int(suspend)} > {INPUT_SUSPEND}"], + check=False, capture_output=True) + return rc.returncode == 0 diff --git a/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/system/hardware/usb.py b/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/system/hardware/usb.py new file mode 100644 index 000000000..15be6f295 --- /dev/null +++ b/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/system/hardware/usb.py @@ -0,0 +1,184 @@ +""" +Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos/ + +USB bus snapshot for deviceState: every enumerated device with its negotiated +speed and its controller's link-error count. Landing this in every rlog makes +cable/hub/link regressions diagnosable from a recorded route instead of only +live. + +Link errors come from `portli` on the ssusb controller (IQ.OS 4.9.1+); on older +builds the file is absent and the counts read 0. + +The USB eGPU dock is identified by VID/PID only. comma's internal codename for +it is deliberately not used here: IQ.Pilot runs these models on several +backends (eGPU dock, eMac), so the naming stays about the role, not the vendor. +""" +from pathlib import Path + +# comma's USB eGPU dock, both shipped USB IDs. The ROM ids are the same board +# sitting in its bootloader (ASMedia) before vendor firmware is flashed — it +# enumerates but cannot serve a GPU in that state. +EGPU_DOCK_USB_IDS = ((0xADD1, 0x0001), (0x3801, 0x0001)) +EGPU_DOCK_ROM_USB_IDS = ((0x174C, 0x2464), (0x174C, 0x2463)) +# must equal image_product() of the bundled firmware; test_egpu_dock_flash pins them together +EGPU_DOCK_FW_PRODUCT = "custom ed4e39b7-CLEAN" + + +def is_egpu_usb_device(vendor_id: int, product_id: int, include_bootloader: bool = False) -> bool: + ids = EGPU_DOCK_USB_IDS + EGPU_DOCK_ROM_USB_IDS if include_bootloader else EGPU_DOCK_USB_IDS + return (vendor_id, product_id) in ids +USB_DEVICES_PATH = Path("/sys/bus/usb/devices") +UDC_PATH = Path("/sys/class/udc") +TYPEC_CC_ORIENTATION_PATH = Path("/sys/class/power_supply/usb/typec_cc_orientation") +USB3_LANES = {1: "a", 2: "b"} # 0 = unattached +SOC_PLATFORM_PATH = Path("/sys/devices/platform/soc") +CONTROLLER_SUFFIX = ".ssusb" +LINK_ERRORS_FILE = "portli" + + +def read(path: Path) -> str | None: + # a controller in peripheral mode fails portli's show(); that surfaces as TypeError, not OSError + try: + return path.read_text().strip() + except Exception: + return None + + +def read_int(path: Path, base: int = 10) -> int: + try: + return int(path.read_text(), base) + except Exception: + return 0 + + +def read_hex_counter(path: Path) -> int: + """sysfs counter printed as '0x0000002a' (portli), tolerating a bare hex value.""" + raw = read(path) + if raw is None: + return 0 + try: + return int(raw, 0) if raw.lower().startswith("0x") else int(raw, 16) + except ValueError: + return 0 + + +def get_usb_topology(root: Path = USB_DEVICES_PATH) -> set[str]: + """Names of everything on the bus; a cheap way to detect hotplug without + re-reading every attribute.""" + try: + return {p.name for p in root.iterdir()} + except Exception: + return set() + + +def usb_devices(root: Path = USB_DEVICES_PATH) -> list[Path]: + try: + return sorted((d for d in root.glob("*") if (d / "idVendor").exists()), key=lambda p: p.name) + except Exception: + return [] + + +def controller(device: Path) -> Path | None: + """The SuperSpeed controller a device hangs off (…/a800000.ssusb).""" + try: + return next((p for p in device.resolve().parents if p.name.endswith(CONTROLLER_SUFFIX)), None) + except Exception: + return None + + +def usb_controllers(soc: Path = SOC_PLATFORM_PATH) -> list[Path]: + try: + return sorted(soc.glob(f"*{CONTROLLER_SUFFIX}")) + except Exception: + return [] + + +def link_controller(udc_root: Path = UDC_PATH) -> str: + """Name of the Type-C port's controller, derived from the UDC rather than + hardcoded: the gadget exposes `.dwc3`, whose address prefix is the + `.ssusb` controller behind the same connector. comma pins the 3X value + directly, which would be wrong on any other board.""" + try: + udc = next(iter(sorted(p.name for p in udc_root.iterdir())), "") + except Exception: + return "" + return f"{udc.split('.')[0]}{CONTROLLER_SUFFIX}" if udc else "" + + +def usb3_lane(orientation: int | None = None) -> str: + """Which SuperSpeed lane the Type-C connector landed on. Unattached reads 0, + which is 'unknown' rather than a lane.""" + if orientation is None: + orientation = read_int(TYPEC_CC_ORIENTATION_PATH) + return USB3_LANES.get(orientation, "unknown") + + +def link_errors(ctrl: Path | None) -> int: + return read_hex_counter(ctrl / LINK_ERRORS_FILE) if ctrl is not None else 0 + + +def get_link_error_count(soc: Path = SOC_PLATFORM_PATH) -> int: + """Cumulative SS port link errors, read off the controller rather than a + device: in peripheral mode (eMac gadget link) the peer never enumerates on + our side, so there is no device row to carry the count.""" + return sum(link_errors(c) for c in usb_controllers(soc)) + + +def egpu_dock_present(root: Path = USB_DEVICES_PATH) -> bool: + """A dock in ROM/bootloader state is deliberately NOT counted as present: it + enumerates but cannot serve a GPU until vendor firmware is flashed.""" + return any((read_int(d / "idVendor", 16), read_int(d / "idProduct", 16)) in EGPU_DOCK_USB_IDS + for d in usb_devices(root)) + + +def egpu_dock_ready(root: Path = USB_DEVICES_PATH) -> bool: + """Present AND running the exact firmware we ship. A dock on any other + firmware enumerates fine but has not been validated with this stack, so the + runtime refuses it; the flasher still sees it via egpu_dock_present.""" + return any((read_int(d / "idVendor", 16), read_int(d / "idProduct", 16)) in EGPU_DOCK_USB_IDS + and (read(d / "product") or "").strip() == EGPU_DOCK_FW_PRODUCT + for d in usb_devices(root)) + + +def get_usb_state(root: Path = USB_DEVICES_PATH, udc_root: Path = UDC_PATH) -> list[dict]: + devices = [] + lane, link_ctrl = usb3_lane(), link_controller(udc_root) + for device in usb_devices(root): + ctrl = controller(device) + devices.append({ + "usb3Lane": lane if ctrl is not None and ctrl.name == link_ctrl else "unknown", + "busnum": read_int(device / "busnum"), + "devnum": read_int(device / "devnum"), + "vendorId": read_int(device / "idVendor", 16), + "productId": read_int(device / "idProduct", 16), + "speedMbps": read_int(device / "speed"), + "manufacturer": read(device / "manufacturer") or "", + "product": read(device / "product") or "", + # 16-bit field upstream, so mask rather than let a wrapped counter overflow it + "linkErrorCount": link_errors(ctrl) & 0xFFFF, + }) + return devices + + +def set_usb_state(device_state, devices: list[dict], link_error_count: int = 0, + lane: str | None = None) -> None: + entries = device_state.usbState.init('devices', len(devices)) + + dock_present = False + for entry, device in zip(entries, devices, strict=True): + entry.busnum = device["busnum"] + entry.devnum = device["devnum"] + entry.vendorId = device["vendorId"] + entry.productId = device["productId"] + entry.speedMbps = device["speedMbps"] + entry.manufacturer = device["manufacturer"] + entry.product = device["product"] + entry.linkErrorCount = device.get("linkErrorCount", 0) & 0xFFFF + entry.usb3Lane = device.get("usb3Lane", "unknown") + + if (entry.vendorId, entry.productId) in EGPU_DOCK_USB_IDS: + dock_present = True + + device_state.usbState.linkErrorCount = link_error_count + device_state.usbState.usb3Lane = lane if lane is not None else usb3_lane() + device_state.egpuDockPresent = dock_present diff --git a/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/system/micd.py b/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/system/micd.py new file mode 100644 index 000000000..5c8d05594 --- /dev/null +++ b/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/system/micd.py @@ -0,0 +1,160 @@ +#!/usr/bin/env python3 +import numpy as np +import os +import time +from functools import cache +import threading + +from iqpilot.cereal import messaging +from iqpilot.common.params import Params +from iqpilot.common.realtime import Ratekeeper +from iqpilot.common.utils import retry +from iqpilot.common.swaglog import cloudlog + +RATE = 10 +FFT_SAMPLES = 1600 # 100ms +REFERENCE_SPL = 2e-5 # newtons/m^2 +SAMPLE_RATE = 16000 +SAMPLE_BUFFER = 800 # 50ms + + +@cache +def get_a_weighting_filter(): + # Calculate the A-weighting filter + # https://en.wikipedia.org/wiki/A-weighting + freqs = np.fft.fftfreq(FFT_SAMPLES, d=1 / SAMPLE_RATE) + A = 12194 ** 2 * freqs ** 4 / ((freqs ** 2 + 20.6 ** 2) * (freqs ** 2 + 12194 ** 2) * np.sqrt((freqs ** 2 + 107.7 ** 2) * (freqs ** 2 + 737.9 ** 2))) + return A / np.max(A) + + +def calculate_spl(measurements): + # https://www.engineeringtoolbox.com/sound-pressure-d_711.html + sound_pressure = np.sqrt(np.mean(measurements ** 2)) # RMS of amplitudes + if sound_pressure > 0: + sound_pressure_level = 20 * np.log10(sound_pressure / REFERENCE_SPL) # dB + else: + sound_pressure_level = 0 + return sound_pressure, sound_pressure_level + + +def apply_a_weighting(measurements: np.ndarray) -> np.ndarray: + # Generate a Hanning window of the same length as the audio measurements + measurements_windowed = measurements * np.hanning(len(measurements)) + + # Apply the A-weighting filter to the signal + return np.abs(np.fft.ifft(np.fft.fft(measurements_windowed) * get_a_weighting_filter())) + + +class Mic: + def __init__(self): + self.rk = Ratekeeper(RATE) + self.pm = messaging.PubMaster(['soundPressure', 'rawAudioData']) + self.params = Params() + + self.measurements = np.empty(0) + + self.sound_pressure = 0 + self.sound_pressure_weighted = 0 + self.sound_pressure_level_weighted = 0 + + self.lock = threading.Lock() + self.callback_count = 0 + self.last_audio_rms = 0.0 + self.last_audio_peak = 0.0 + self.last_device = None + self.last_status = None + + def update(self): + with self.lock: + sound_pressure = self.sound_pressure + sound_pressure_weighted = self.sound_pressure_weighted + sound_pressure_level_weighted = self.sound_pressure_level_weighted + callback_count = self.callback_count + audio_rms = self.last_audio_rms + audio_peak = self.last_audio_peak + device_name = self.last_device + status = self.last_status + + msg = messaging.new_message('soundPressure', valid=True) + msg.soundPressure.soundPressure = float(sound_pressure) + msg.soundPressure.soundPressureWeighted = float(sound_pressure_weighted) + msg.soundPressure.soundPressureWeightedDb = float(sound_pressure_level_weighted) + + self.pm.send('soundPressure', msg) + if callback_count % RATE == 0: + cloudlog.info( + f"micd health: callbacks={callback_count} rms={audio_rms:.6f} peak={audio_peak:.6f} " + f"device={device_name} status={status!r} livestream={self.params.get_bool('IsLiveStreaming')}" + ) + self.rk.keep_time() + + def callback(self, indata, frames, time, status): + """ + Using amplitude measurements, calculate an uncalibrated sound pressure and sound pressure level. + Then apply A-weighting to the raw amplitudes and run the same calculations again. + + Logged A-weighted equivalents are rough approximations of the human-perceived loudness. + """ + msg = messaging.new_message('rawAudioData', valid=True) + audio_data_int_16 = (indata[:, 0] * 32767).astype(np.int16) + msg.rawAudioData.data = audio_data_int_16.tobytes() + msg.rawAudioData.sampleRate = SAMPLE_RATE + self.pm.send('rawAudioData', msg) + + with self.lock: + self.callback_count += 1 + self.last_audio_rms = float(np.sqrt(np.mean(np.square(indata[:, 0])))) + self.last_audio_peak = float(np.max(np.abs(indata[:, 0]))) + self.last_status = str(status) if status else None + self.measurements = np.concatenate((self.measurements, indata[:, 0])) + + while self.measurements.size >= FFT_SAMPLES: + measurements = self.measurements[:FFT_SAMPLES] + + self.sound_pressure, _ = calculate_spl(measurements) + measurements_weighted = apply_a_weighting(measurements) + self.sound_pressure_weighted, self.sound_pressure_level_weighted = calculate_spl(measurements_weighted) + + self.measurements = self.measurements[FFT_SAMPLES:] + + @retry(attempts=10, delay=3) + def get_stream(self, sd): + # reload sounddevice to reinitialize portaudio + sd._terminate() + sd._initialize() + requested_device = os.environ.get("MICD_DEVICE") + device = int(requested_device) if requested_device is not None else None + return sd.InputStream(channels=1, samplerate=SAMPLE_RATE, callback=self.callback, blocksize=SAMPLE_BUFFER, device=device) + + def micd_thread(self): + # sounddevice must be imported after forking processes + import sounddevice as sd + + device = sd.default.device + if os.environ.get("MICD_DEVICE") is not None: + device = int(os.environ["MICD_DEVICE"]) + sd.default.device = (device, device) + self.last_device = f"{device}: {sd.query_devices(device)['name']}" if isinstance(device, int) else str(device) + cloudlog.info(f"micd selecting input device {self.last_device}") + + while True: + try: + with self.get_stream(sd) as stream: + cloudlog.info(f"micd stream started: {stream.samplerate=} {stream.channels=} {stream.dtype=} {stream.device=}, {stream.blocksize=}") + while True: + self.update() + except Exception: + # Some A1s wedge the audio DSP (ALSA EINVAL / ADSP_EFAILED until reboot). Dying here + # crash-loops the process and selfdrived raises a takeover alert mid-drive over a + # microphone - stay alive and keep retrying instead; recovers if the DSP comes back. + cloudlog.exception("micd: audio stream unavailable, retrying") + time.sleep(10) + + +def main(): + mic = Mic() + mic.micd_thread() + + +if __name__ == "__main__": + main() diff --git a/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/system/sentry.py b/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/system/sentry.py new file mode 100644 index 000000000..0205903d5 --- /dev/null +++ b/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/system/sentry.py @@ -0,0 +1,171 @@ +"""Install exception handler for process crash.""" +import os +import shutil +import traceback +from datetime import datetime +import sentry_sdk +from enum import Enum +from sentry_sdk.integrations.threading import ThreadingIntegration + +from iqpilot.common.params import Params +from iqpilot.konn3kt.registration import UNREGISTERED_DONGLE_ID +from iqpilot.system.hardware import HARDWARE +from iqpilot.system.hardware.hw import Paths +from iqpilot.common.swaglog import cloudlog +from iqpilot.system.version import get_build_metadata, get_version + +CRASHES_DIR = Paths.crash_log_root() +CRASH_UPLOADS_DIR = os.path.join(Paths.log_root(), "crash") + + +class SentryProject(Enum): + # python project + SELFDRIVE = "https://186a6736b7927e5ae9b92c869ba81b6b@o1138119.ingest.us.sentry.io/4508660076052480" + # native project + SELFDRIVE_NATIVE = SELFDRIVE + + +def _sentry_enabled() -> bool: + return os.getenv("IQPILOT_ENABLE_SENTRY", "0") == "1" + + +def _ensure_dir(path: str) -> None: + os.makedirs(path, exist_ok=True) + + +def _queue_crash_upload(src: str, name: str | None = None) -> None: + try: + _ensure_dir(CRASH_UPLOADS_DIR) + dest_name = name or os.path.basename(src) + shutil.copyfile(src, os.path.join(CRASH_UPLOADS_DIR, dest_name)) + except Exception: + cloudlog.exception("error when attempting to queue crash upload") + + +def report_tombstone(fn: str, message: str, contents: str) -> None: + cloudlog.error({'tombstone': message}) + + if not _sentry_enabled(): + return + + with sentry_sdk.configure_scope() as scope: + set_user() + scope.set_extra("tombstone_fn", fn) + scope.set_extra("tombstone", contents) + sentry_sdk.capture_message(message=message) + sentry_sdk.flush() + + +def capture_exception(*args, **kwargs) -> None: + cloudlog.error("crash", exc_info=kwargs.get('exc_info', 1)) + + try: + save_exception(traceback.format_exc()) + + if not _sentry_enabled(): + return + + set_user() + sentry_sdk.capture_exception(*args, **kwargs) + sentry_sdk.flush() # https://github.com/getsentry/sentry-python/issues/291 + except Exception: + cloudlog.exception("sentry exception") + + +def save_exception(content: str) -> None: + try: + _ensure_dir(CRASHES_DIR) + commit = (get_build_metadata().openpilot.git_commit or "nocommit")[:8] + + dated_fn = os.path.join(CRASHES_DIR, datetime.now().strftime("%Y-%m-%d--%H-%M-%S.log")) + files = [ + dated_fn, + os.path.join(CRASHES_DIR, "error.log") + ] + + for fn in files: + with open(fn, 'w') as f: + if os.path.basename(fn) == "error.log": + lines = content.splitlines()[-3:] + f.write("\n".join(lines)) + else: + f.write(content) + + upload_name = f"{os.path.splitext(os.path.basename(dated_fn))[0]}_{commit}_python.log" + _queue_crash_upload(dated_fn, upload_name) + cloudlog.error(f"logged crash to {files}") + except Exception: + cloudlog.exception("error when attempting to save exception") + + +def capture_fingerprint_mock() -> None: + try: + set_user() + message = "car doesn't match any fingerprints" + sentry_sdk.capture_message(message=message, level="error") + sentry_sdk.flush() + except Exception as e: + cloudlog.exception(f"sentry fingerprint MOCK exception: {e}") + + +def capture_fingerprint(candidate: str, car_name: str) -> None: + try: + set_user() + sentry_sdk.set_tag("carFingerprint", candidate) + sentry_sdk.set_tag("carName", car_name) + + message = f"Fingerprinted {candidate}" + sentry_sdk.capture_message(message=message, level="info") + sentry_sdk.flush() + except Exception as e: + cloudlog.exception(f"sentry fingerprint exception: {e}") + + +def set_tag(key: str, value: str) -> None: + sentry_sdk.set_tag(key, value) + + +def set_user() -> None: + dongle_id, git_username = get_properties() + sentry_sdk.set_user({"id": dongle_id, "name": git_username}) + + +def get_properties() -> tuple[str, str]: + params = Params() + hardware_serial: str = params.get("HardwareSerial") or "" + git_username: str = params.get("GithubUsername") or "" + dongle_id: str = params.get("DongleId") or f"{UNREGISTERED_DONGLE_ID}-{hardware_serial}" + + return dongle_id, git_username + + +def init(project: SentryProject) -> bool: + if not _sentry_enabled(): + cloudlog.info("Sentry disabled, using local crash logging + konn3kt uploader") + return False + + build_metadata = get_build_metadata() + + env = build_metadata.channel_type + dongle_id, git_username = get_properties() + + integrations = [] + if project == SentryProject.SELFDRIVE: + integrations.append(ThreadingIntegration(propagate_hub=True)) + + sentry_sdk.init(project.value, + default_integrations=False, + release=get_version(), + integrations=integrations, + traces_sample_rate=1.0, + max_value_length=8192, + environment=env) + + sentry_sdk.set_user({"id": dongle_id, "name": git_username}) + sentry_sdk.set_tag("dirty", build_metadata.openpilot.is_dirty) + sentry_sdk.set_tag("origin", build_metadata.openpilot.git_origin) + sentry_sdk.set_tag("branch", build_metadata.channel) + sentry_sdk.set_tag("commit", build_metadata.openpilot.git_commit) + sentry_sdk.set_tag("device", HARDWARE.get_device_type()) + + return True diff --git a/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/system/version.py b/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/system/version.py new file mode 100644 index 000000000..cb731d009 --- /dev/null +++ b/artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/system/version.py @@ -0,0 +1,200 @@ +#!/usr/bin/env python3 +from dataclasses import dataclass +from functools import cache +import json +import os +import pathlib +import subprocess + +from iqpilot.common.basedir import BASEDIR +from iqpilot.common.swaglog import cloudlog +from iqpilot.common.git import get_commit, get_origin, get_branch, get_short_branch, get_commit_date + +RELEASE_IQ_BRANCHES = ['release', 'release-tici', 'release-new'] +TESTED_BRANCHES = RELEASE_IQ_BRANCHES +IQ_BRANCH_MIGRATIONS: dict[tuple[str, str], str] = {} + +BUILD_METADATA_FILENAME = "build.json" + +training_version: str = "0.2.0" +terms_version: str = "2" + + +UNKNOWN_VERSION = "0.0.0" + + +@cache +def get_version(path: str = BASEDIR) -> str: + try: + with open(os.path.join(path, "iqpilot", "common", "version.h")) as _versionf: + return _versionf.read().split('"')[1] + except (OSError, IndexError): + return UNKNOWN_VERSION + + +def get_release_notes(path: str = BASEDIR) -> str: + for rel in (("iqpilot", "docs", "CHANGELOG.md"), ("docs", "CHANGELOG.md")): + try: + with open(os.path.join(path, *rel)) as f: + return f.read().split('\n\n', 1)[0] + except OSError: + continue + return "" + + +@cache +def is_prebuilt(path: str = BASEDIR) -> bool: + return os.path.exists(os.path.join(path, 'prebuilt')) + + +@cache +def is_dirty(cwd: str = BASEDIR) -> bool: + if not get_origin() or not get_short_branch(): + return True + + dirty = False + try: + # Actually check dirty files + if not is_prebuilt(cwd): + # This is needed otherwise touched files might show up as modified + try: + subprocess.check_call(["git", "update-index", "--refresh"], cwd=cwd) + except subprocess.CalledProcessError: + pass + + branch = get_branch() + if not branch: + return True + dirty = (subprocess.call(["git", "diff-index", "--quiet", branch, "--"], cwd=cwd)) != 0 + except subprocess.CalledProcessError: + cloudlog.exception("git subprocess failed while checking dirty") + dirty = True + + return dirty + + +@dataclass +class OpenpilotMetadata: + version: str + release_notes: str + git_commit: str + git_origin: str + git_commit_date: str + build_style: str + is_dirty: bool # whether there are local changes + + @property + def short_version(self) -> str: + return self.version.split('-')[0] + + @property + def comma_remote(self) -> bool: + # note to fork maintainers, this is used for release metrics. please do not + # touch this to get rid of the orange startup alert. there's better ways to do that + return self.git_normalized_origin == "github.com/commaai/openpilot" + + @property + def iqpilot_remote(self) -> bool: + return self.git_normalized_origin in ("github.com/iqpilot/iqpilot", + "github.com/iqpilot/openpilot") + + @property + def git_normalized_origin(self) -> str: + return self.git_origin \ + .replace("git@", "", 1) \ + .replace(".git", "", 1) \ + .replace("https://", "", 1) \ + .replace(":", "/", 1) + + +@dataclass +class BuildMetadata: + channel: str + openpilot: OpenpilotMetadata + + @property + def tested_channel(self) -> bool: + return self.channel in TESTED_BRANCHES + + @property + def release_channel(self) -> bool: + return self.channel in RELEASE_IQ_BRANCHES + + @property + def canonical(self) -> str: + return f"{self.openpilot.version}-{self.openpilot.git_commit}-{self.openpilot.build_style}" + + @property + def ui_description(self) -> str: + return f"{self.openpilot.version} / {self.openpilot.git_commit[:6]} / {self.channel}" + + @property + def master_channel(self) -> bool: + return self.channel in RELEASE_IQ_BRANCHES + + @property + def development_channel(self) -> bool: + return self.channel == "dev" or self.channel.startswith("dev-") or self.channel.endswith("-prebuilt") + + @property + def channel_type(self) -> str: + if "-tici" in self.channel or self.channel in ("release-new" or "master-mici"): + return "tici" + elif self.development_channel: + return "development" + elif self.tested_channel: + return "staging" + elif self.master_channel: + return "master" + elif self.release_channel: + return "release" + else: + return "feature" + + +def build_metadata_from_dict(build_metadata: dict) -> BuildMetadata: + channel = build_metadata.get("channel", "unknown") + openpilot_metadata = build_metadata.get("openpilot", {}) + version = openpilot_metadata.get("version", "unknown") + release_notes = openpilot_metadata.get("release_notes", "unknown") + git_commit = openpilot_metadata.get("git_commit", "unknown") + git_origin = openpilot_metadata.get("git_origin", "unknown") + git_commit_date = openpilot_metadata.get("git_commit_date", "unknown") + build_style = openpilot_metadata.get("build_style", "unknown") + return BuildMetadata(channel, + OpenpilotMetadata( + version=version, + release_notes=release_notes, + git_commit=git_commit, + git_origin=git_origin, + git_commit_date=git_commit_date, + build_style=build_style, + is_dirty=False)) + + +def get_build_metadata(path: str = BASEDIR) -> BuildMetadata: + build_metadata_path = pathlib.Path(path) / BUILD_METADATA_FILENAME + + if build_metadata_path.exists(): + build_metadata = json.loads(build_metadata_path.read_text()) + return build_metadata_from_dict(build_metadata) + + git_folder = pathlib.Path(path) / ".git" + + if git_folder.exists(): + return BuildMetadata(get_short_branch(path), + OpenpilotMetadata( + version=get_version(path), + release_notes=get_release_notes(path), + git_commit=get_commit(path), + git_origin=get_origin(path), + git_commit_date=get_commit_date(path), + build_style="unknown", + is_dirty=is_dirty(path))) + + cloudlog.exception("unable to get build metadata") + raise Exception("invalid build metadata") + + +if __name__ == "__main__": + print(get_build_metadata()) diff --git a/artifacts/iqpilot_emac_private/python/iqpilot_private/__init__.py b/artifacts/iqpilot_emac_private/python/iqpilot_private/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/artifacts/iqpilot_emac_private/python/iqpilot_private/emac/__init__.py b/artifacts/iqpilot_emac_private/python/iqpilot_private/emac/__init__.py new file mode 100644 index 000000000..5ece1c074 --- /dev/null +++ b/artifacts/iqpilot_emac_private/python/iqpilot_private/emac/__init__.py @@ -0,0 +1,3 @@ +""" +Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos/ +""" diff --git a/artifacts/iqpilot_emac_private/python/iqpilot_private/emac/bulk_transport.cpython-312-aarch64-linux-gnu.so b/artifacts/iqpilot_emac_private/python/iqpilot_private/emac/bulk_transport.cpython-312-aarch64-linux-gnu.so new file mode 100755 index 000000000..5f5fc962e Binary files /dev/null and b/artifacts/iqpilot_emac_private/python/iqpilot_private/emac/bulk_transport.cpython-312-aarch64-linux-gnu.so differ diff --git a/artifacts/iqpilot_emac_private/python/iqpilot_private/emac/mac_client.cpython-312-aarch64-linux-gnu.so b/artifacts/iqpilot_emac_private/python/iqpilot_private/emac/mac_client.cpython-312-aarch64-linux-gnu.so new file mode 100755 index 000000000..2c3698a90 Binary files /dev/null and b/artifacts/iqpilot_emac_private/python/iqpilot_private/emac/mac_client.cpython-312-aarch64-linux-gnu.so differ diff --git a/artifacts/iqpilot_emac_private/python/iqpilot_private/emac/mac_protocol.cpython-312-aarch64-linux-gnu.so b/artifacts/iqpilot_emac_private/python/iqpilot_private/emac/mac_protocol.cpython-312-aarch64-linux-gnu.so new file mode 100755 index 000000000..90bb87e16 Binary files /dev/null and b/artifacts/iqpilot_emac_private/python/iqpilot_private/emac/mac_protocol.cpython-312-aarch64-linux-gnu.so differ diff --git a/artifacts/iqpilot_emac_private/python/iqpilot_private/emac/maciqmodeld.cpython-312-aarch64-linux-gnu.so b/artifacts/iqpilot_emac_private/python/iqpilot_private/emac/maciqmodeld.cpython-312-aarch64-linux-gnu.so new file mode 100755 index 000000000..1b142e069 Binary files /dev/null and b/artifacts/iqpilot_emac_private/python/iqpilot_private/emac/maciqmodeld.cpython-312-aarch64-linux-gnu.so differ diff --git a/artifacts/iqpilot_model_selector_private/manifest.json b/artifacts/iqpilot_model_selector_private/manifest.json index 218dc9785..94dc9c020 100644 --- a/artifacts/iqpilot_model_selector_private/manifest.json +++ b/artifacts/iqpilot_model_selector_private/manifest.json @@ -176,7 +176,7 @@ }, "python/iqpilot_private/models/__init__.cpython-312-aarch64-linux-gnu.so": { "mode": 493, - "sha256": "c0d8a584bfeb79f0a109108ff463a95a57db24635f48d936461dddf4673e25b4", + "sha256": "b7ae194bc219d344d92e77e7ad56232c48e75bab4d0bdcb2ebf20dccc7ba2c4d", "size": 67616 }, "python/iqpilot_private/models/__init__.py": { @@ -184,24 +184,39 @@ "sha256": "5128931b54d4f1ff131185e7711d56a788b5e2cb71e3c8154a2f79fef184b452", "size": 118 }, + "python/iqpilot_private/models/big_catalog.cpython-312-aarch64-linux-gnu.so": { + "mode": 493, + "sha256": "447634f6ee186df629e3fc03556fab3222d84037da0e9061aa44e7238b0f9b5e", + "size": 133840 + }, + "python/iqpilot_private/models/egpu_model.cpython-312-aarch64-linux-gnu.so": { + "mode": 493, + "sha256": "5a69a9967bdeaf8258b26487be40420c8f37935161699527f2bafc3ddfa5e710", + "size": 68112 + }, + "python/iqpilot_private/models/emac_model_meta.cpython-312-aarch64-linux-gnu.so": { + "mode": 493, + "sha256": "24785c0b8eee854e1da2f03ee961de6650fcd7b975797c45d5d1d8ea995b8273", + "size": 68264 + }, "python/iqpilot_private/models/fetcher.cpython-312-aarch64-linux-gnu.so": { "mode": 493, - "sha256": "fe34e89be0539fd173457cadc1fdf3a1467d6fefd965031c37cc3d5566f8b1bb", + "sha256": "37042bfe3e87db35e8abb8a03562c90be5ce835e832b2b4159f57e6f2643764e", "size": 136200 }, "python/iqpilot_private/models/git_auth.cpython-312-aarch64-linux-gnu.so": { "mode": 493, - "sha256": "9e7dd5fbf72f7737130fb36718a2b212afa55757b39ad6ad99d7e4643fa852fa", + "sha256": "9818f5eb5c850cdee9ec6d62d239de0451659cd62b891c01083433f41ca8dbde", "size": 67864 }, "python/iqpilot_private/models/helpers.cpython-312-aarch64-linux-gnu.so": { "mode": 493, - "sha256": "8bedb7406b942dbee7274cd97bb448ded69284911f9760dd702bb90e44325d20", + "sha256": "6e6c9e9e66579d1f03861f1535da3a960f53f50e734a6030b41b9d8f8516c981", "size": 137944 }, "python/iqpilot_private/models/manager.cpython-312-aarch64-linux-gnu.so": { "mode": 493, - "sha256": "65f503a85a00b729f0f4eeb033601f7978a0aba81fd4ba3ea58c0a3d18e45dad", + "sha256": "ca16d91129cf70227a92b81dd4204760681fc6f2ca7bd9ed84e58b7cd2efd2de", "size": 340000 }, "runtime": { @@ -213,10 +228,13 @@ } }, "signatures": { - "python/iqpilot_private/models/__init__.cpython-312-aarch64-linux-gnu.so": "9lPKvj8ZTe6wKhWcX4Bha8oHtb/BbWQThP610EWKYWHYNDm6mkUVKGFcyhkH+tmy+PSk5eGDjBnxS0tq/TstBw==", - "python/iqpilot_private/models/fetcher.cpython-312-aarch64-linux-gnu.so": "BbwoBwc9jhIgfty30cSgr1mD0u633HyT9MmO7Um7/CrcF9bO5uyhE5hDHsotvHaQmwKnTBGW4+D9hLs8xNo0Bw==", - "python/iqpilot_private/models/git_auth.cpython-312-aarch64-linux-gnu.so": "D4SIcunDU2v7PQ+DExObejMtufrJVn8LQDl6SwyRr9EzoemViY0dS/Ff7tTz0KwbvzpHz10ksOCYn2/3t6uxAg==", - "python/iqpilot_private/models/helpers.cpython-312-aarch64-linux-gnu.so": "g2oM0JvMIJOvhat23cMmWq8QY+jAAsrQFEFZSk9sC/Kq4aqDZZ/KthrmlLQaNclXtkATJShOxfIlPuEVFzpzAg==", - "python/iqpilot_private/models/manager.cpython-312-aarch64-linux-gnu.so": "ZGu3WKX4AELSUXdXpPu4IF+gjzLeRqx1jdi+WGcnCJK17dKtvui9OQ0s3uF3j37DnvBMjpjxnnvwwIQjcEjoAQ==" + "python/iqpilot_private/models/__init__.cpython-312-aarch64-linux-gnu.so": "lSYzYS2zoycLPzqpLu8RE5hnpVFgSA0e6ZXgH57Ve2f3jwV9OSCbM+JHqxy5fDDiPW1ksRM6j01vFE+6SY5hCw==", + "python/iqpilot_private/models/big_catalog.cpython-312-aarch64-linux-gnu.so": "WHHAjMetD6q1tsUdG54ulOLADQImspgfqr7p7lqPp0NMmvy+MYVJLQPmxrjrSokZTwtGivlLNEy8wo4EZ6OuCQ==", + "python/iqpilot_private/models/egpu_model.cpython-312-aarch64-linux-gnu.so": "+QTJvOd9zd1UfXcwn0+EhoMHhGMvPyBOB59GglVi5fx9j1TUqDWu00Ip/30IlbxWTbTMZQzDNFWR3puQZ/tgCA==", + "python/iqpilot_private/models/emac_model_meta.cpython-312-aarch64-linux-gnu.so": "e9ZmfuFD/FSWPAt/9nmyFafkGojmCuRxmaHixSCMPLaFVDrwwomuDoaq4ogaQ6HmHDdCosBkNh7w/6ZIHgpzDg==", + "python/iqpilot_private/models/fetcher.cpython-312-aarch64-linux-gnu.so": "ImIktR4cuqUzWtoZe1nSA0sIygmpuVW76jNZwerd+qG1qmJojkW7mfQzZ6yiwRTV51FybjLjLzhZgAA5kqY9Cw==", + "python/iqpilot_private/models/git_auth.cpython-312-aarch64-linux-gnu.so": "qGUpnjJFC67qdIuc3JeTtDHVKRuK9oEe59f/43IJF3avDTzbFTHnLWM3AfvO+j3i/m4vuuRjAlKVCQEFRlNWDA==", + "python/iqpilot_private/models/helpers.cpython-312-aarch64-linux-gnu.so": "rB3MDS2Ue0p8889DbKn+gpzyaM1IeR4cb38uy3taitPwV4Em2Zbyr47W2hbvlKL32m2pBWwiJCM8EeqlafEtDg==", + "python/iqpilot_private/models/manager.cpython-312-aarch64-linux-gnu.so": "qsLTZana4Y5dxS0m8gGvtYVTFeXixkPUawT2SGwQF8ccxRQogAUt1bQJfJLZA4xmOnTG2Da0qhxFQ13VsFBeCA==" } } diff --git a/artifacts/iqpilot_model_selector_private/manifest.json.sig b/artifacts/iqpilot_model_selector_private/manifest.json.sig index 64d92df48..72a9da140 100644 --- a/artifacts/iqpilot_model_selector_private/manifest.json.sig +++ b/artifacts/iqpilot_model_selector_private/manifest.json.sig @@ -1 +1 @@ -rL5ip/O3aYwgySr2d0kyzVfFnyOTfAAwVbDl8lxmNJIjPkpSLsEDrgglLQ5MWjmL4s2oHeJA70f2ahx31WgVBw== +ZZZzIcyCSoU2lVTy3IXBcSS//qvFxL5C0ivJT9huKojaRWV7cBqJDBRD457NwknzAaUHiz9HmAwguFLjvWgxCA== diff --git a/artifacts/iqpilot_model_selector_private/python/iqpilot_private/models/__init__.cpython-312-aarch64-linux-gnu.so b/artifacts/iqpilot_model_selector_private/python/iqpilot_private/models/__init__.cpython-312-aarch64-linux-gnu.so index ed886630d..189743499 100755 Binary files a/artifacts/iqpilot_model_selector_private/python/iqpilot_private/models/__init__.cpython-312-aarch64-linux-gnu.so and b/artifacts/iqpilot_model_selector_private/python/iqpilot_private/models/__init__.cpython-312-aarch64-linux-gnu.so differ diff --git a/artifacts/iqpilot_model_selector_private/python/iqpilot_private/models/big_catalog.cpython-312-aarch64-linux-gnu.so b/artifacts/iqpilot_model_selector_private/python/iqpilot_private/models/big_catalog.cpython-312-aarch64-linux-gnu.so new file mode 100755 index 000000000..cd3fa6b43 Binary files /dev/null and b/artifacts/iqpilot_model_selector_private/python/iqpilot_private/models/big_catalog.cpython-312-aarch64-linux-gnu.so differ diff --git a/artifacts/iqpilot_model_selector_private/python/iqpilot_private/models/egpu_model.cpython-312-aarch64-linux-gnu.so b/artifacts/iqpilot_model_selector_private/python/iqpilot_private/models/egpu_model.cpython-312-aarch64-linux-gnu.so new file mode 100755 index 000000000..ee7176c44 Binary files /dev/null and b/artifacts/iqpilot_model_selector_private/python/iqpilot_private/models/egpu_model.cpython-312-aarch64-linux-gnu.so differ diff --git a/artifacts/iqpilot_model_selector_private/python/iqpilot_private/models/emac_model_meta.cpython-312-aarch64-linux-gnu.so b/artifacts/iqpilot_model_selector_private/python/iqpilot_private/models/emac_model_meta.cpython-312-aarch64-linux-gnu.so new file mode 100755 index 000000000..422e0d5a1 Binary files /dev/null and b/artifacts/iqpilot_model_selector_private/python/iqpilot_private/models/emac_model_meta.cpython-312-aarch64-linux-gnu.so differ diff --git a/artifacts/iqpilot_model_selector_private/python/iqpilot_private/models/fetcher.cpython-312-aarch64-linux-gnu.so b/artifacts/iqpilot_model_selector_private/python/iqpilot_private/models/fetcher.cpython-312-aarch64-linux-gnu.so index 208c82d5c..90de4ba7a 100755 Binary files a/artifacts/iqpilot_model_selector_private/python/iqpilot_private/models/fetcher.cpython-312-aarch64-linux-gnu.so and b/artifacts/iqpilot_model_selector_private/python/iqpilot_private/models/fetcher.cpython-312-aarch64-linux-gnu.so differ diff --git a/artifacts/iqpilot_model_selector_private/python/iqpilot_private/models/git_auth.cpython-312-aarch64-linux-gnu.so b/artifacts/iqpilot_model_selector_private/python/iqpilot_private/models/git_auth.cpython-312-aarch64-linux-gnu.so index 342b9ae9f..622d8bb7c 100755 Binary files a/artifacts/iqpilot_model_selector_private/python/iqpilot_private/models/git_auth.cpython-312-aarch64-linux-gnu.so and b/artifacts/iqpilot_model_selector_private/python/iqpilot_private/models/git_auth.cpython-312-aarch64-linux-gnu.so differ diff --git a/artifacts/iqpilot_model_selector_private/python/iqpilot_private/models/helpers.cpython-312-aarch64-linux-gnu.so b/artifacts/iqpilot_model_selector_private/python/iqpilot_private/models/helpers.cpython-312-aarch64-linux-gnu.so index c3a061ecd..89887192c 100755 Binary files a/artifacts/iqpilot_model_selector_private/python/iqpilot_private/models/helpers.cpython-312-aarch64-linux-gnu.so and b/artifacts/iqpilot_model_selector_private/python/iqpilot_private/models/helpers.cpython-312-aarch64-linux-gnu.so differ diff --git a/artifacts/iqpilot_model_selector_private/python/iqpilot_private/models/manager.cpython-312-aarch64-linux-gnu.so b/artifacts/iqpilot_model_selector_private/python/iqpilot_private/models/manager.cpython-312-aarch64-linux-gnu.so index 249ae9b9a..a7bf20fae 100755 Binary files a/artifacts/iqpilot_model_selector_private/python/iqpilot_private/models/manager.cpython-312-aarch64-linux-gnu.so and b/artifacts/iqpilot_model_selector_private/python/iqpilot_private/models/manager.cpython-312-aarch64-linux-gnu.so differ diff --git a/artifacts/package_sources/iqdbc/iqdbc/car/hyundai/carcontroller.py b/artifacts/package_sources/iqdbc/iqdbc/car/hyundai/carcontroller.py index 83ae1d661..25fb87a10 100644 --- a/artifacts/package_sources/iqdbc/iqdbc/car/hyundai/carcontroller.py +++ b/artifacts/package_sources/iqdbc/iqdbc/car/hyundai/carcontroller.py @@ -454,10 +454,10 @@ class CarController(CarControllerBase): hda2 = self.CP.flags & HyundaiFlags.CANFD_HDA2 hda2_long = hda2 and self.CP.openpilotLongitudinalControl # steering control - if camera_scc: - can_sends.extend(hyundaicanfd.create_steering_messages_camera_scc(self.frame, self.packer, self.CP, self.CAN, CC, apply_steer_req, apply_torque, CS, apply_angle, self.lkas_max_torque, angle_control)) - else: - can_sends.extend(hyundaicanfd.create_steering_messages(self.packer, self.CP, self.CAN, CC.enabled, apply_steer_req, apply_torque, apply_angle, self.lkas_max_torque, angle_control)) + can_sends.extend(hyundaicanfd.create_steering_messages( + self.packer, self.CP, self.CAN, CC.enabled, apply_steer_req, apply_torque, + apply_angle, self.lkas_max_torque, angle_control, + )) # prevent LFA from activating on HDA2 by sending "no lane lines detected" to ADAS ECU if self.frame % 5 == 0 and hda2 and not camera_scc: diff --git a/artifacts/package_sources/iqdbc/iqdbc/car/hyundai/hyundaicanfd.py b/artifacts/package_sources/iqdbc/iqdbc/car/hyundai/hyundaicanfd.py index b9c5a5186..003a56e91 100644 --- a/artifacts/package_sources/iqdbc/iqdbc/car/hyundai/hyundaicanfd.py +++ b/artifacts/package_sources/iqdbc/iqdbc/car/hyundai/hyundaicanfd.py @@ -12,20 +12,6 @@ LaneChangeDirection = log.LaneChangeDirection TurnDirection = log.Desire -def hyundai_crc8(data: bytes) -> int: - poly = 0x2F - crc = 0xFF - - for byte in data: - crc ^= byte - for _ in range(8): - if crc & 0x80: - crc = ((crc << 1) ^ poly) & 0xFF - else: - crc = (crc << 1) & 0xFF - - return crc ^ 0xFF - class CanBus(CanBusBase): def __init__(self, CP, fingerprint=None, lka_steering=None) -> None: super().__init__(CP, fingerprint) @@ -84,95 +70,6 @@ class CanBus(CanBusBase): # LKAS # 201 - 2a0 - - -def create_steering_messages_camera_scc(frame, packer, CP, CAN, CC, lat_active, apply_steer, CS, apply_angle, max_torque, angle_control): - - emergency_steering = False - if CS.adrv_0x161 is not None: - values = CS.adrv_0x161 - emergency_steering = values["ALERTS_1"] in [11, 12, 13, 14, 15, 21, 22, 23, 24, 25, 26] - - - ret = [] - if CS.mdps is not None: - values = copy.copy(CS.mdps) - #rx_counter = values.pop("COUNTER", None) - if angle_control: - if CS.lfa_alt is not None: - values["LFA2_ACTIVE"] = CS.lfa_alt["LKAS_ANGLE_ACTIVE"] - else: - if CS.lfa is not None: - values["LKA_ACTIVE"] = 1 if CS.lfa["STEER_REQ"] == 1 else 0 - - if frame % 1000 < 40: - values["STEERING_COL_TORQUE"] += 220 - #ret.append(packer.make_can_msg("MDPS", CAN.CAM, values, rx_counter = rx_counter)) - ret.append(packer.make_can_msg("MDPS", CAN.CAM, values)) - - if frame % 10 == 0: - if CS.steer_touch_2af is not None: - values = copy.copy(CS.steer_touch_2af) - if frame % 1000 < 40: - values["TOUCH_DETECT"] = 3 - values["TOUCH1"] = 50 - values["TOUCH2"] = 50 - values["CHECKSUM_"] = 0 - dat = packer.make_can_msg("STEER_TOUCH_2AF", 0, values)[1] - values["CHECKSUM_"] = hyundai_crc8(dat[1:8]) - - ret.append(packer.make_can_msg("STEER_TOUCH_2AF", CAN.CAM, values)) - - if angle_control: - if CS.lfa_alt is not None: - values = copy.copy(CS.lfa_alt) - rx_counter = values.pop("COUNTER", None) - if emergency_steering: - pass - else: - #values = {} #CS.lfa_alt - values["LKAS_ANGLE_ACTIVE"] = 2 if CC.latActive else 1 - values["LKAS_ANGLE_CMD"] = -apply_angle - values["LKAS_ANGLE_MAX_TORQUE"] = max_torque if CC.latActive else 0 - ret.append(packer.make_can_msg("LFA_ALT", CAN.ECAN, values, rx_counter = rx_counter)) - - if CS.lfa is not None: - values = copy.copy(CS.lfa) - rx_counter = values.pop("COUNTER", None) - if not emergency_steering: - values["LKA_MODE"] = 0 - values["LKA_ICON"] = 2 if CC.latActive else 1 - values["TORQUE_REQUEST"] = -1024 # apply_steer, - values["VALUE63"] = 0 # LKA_ASSIST - values["STEER_REQ"] = 0 # 1 if lat_active else 0, - values["HAS_LANE_SAFETY"] = 0 # hide LKAS settings - values["LKA_ACTIVE"] = 3 if CC.latActive else 0 # this changes sometimes, 3 seems to indicate engaged - values["VALUE64"] = 0 #STEER_MODE, NEW_SIGNAL_2 - values["LKAS_ANGLE_CMD"] = -25.6 #-apply_angle, - values["LKAS_ANGLE_ACTIVE"] = 0 #2 if lat_active else 1, - values["LKAS_ANGLE_MAX_TORQUE"] = 0 #max_torque if lat_active else 0, - values["NEW_SIGNAL_1"] = 10 - ret.append(packer.make_can_msg("LFA", CAN.ECAN, values, rx_counter = rx_counter)) - - elif CS.lfa is not None: - values = {} - values["LKA_MODE"] = 2 - values["LKA_ICON"] = 2 if lat_active else 1 - values["TORQUE_REQUEST"] = apply_steer - values["STEER_REQ"] = 1 if lat_active else 0 - values["VALUE64"] = 0 # STEER_MODE, NEW_SIGNAL_2 - values["HAS_LANE_SAFETY"] = 0 - values["LKA_ACTIVE"] = 0 # NEW_SIGNAL_1 - - values["DampingGain"] = 0 if lat_active else 100 - #values["VALUE63"] = 0 - - #values["VALUE82_SET256"] = 0 - - ret.append(packer.make_can_msg("LFA", CAN.ECAN, values)) - - return ret - def create_steering_messages(packer, CP, CAN, enabled, lat_active, apply_steer, apply_angle, max_torque, angle_control): ret = [] diff --git a/artifacts/package_sources/iqdbc/iqdbc/car/volkswagen/carcontroller.py b/artifacts/package_sources/iqdbc/iqdbc/car/volkswagen/carcontroller.py index 6f87a30e6..dbdd6bd36 100644 --- a/artifacts/package_sources/iqdbc/iqdbc/car/volkswagen/carcontroller.py +++ b/artifacts/package_sources/iqdbc/iqdbc/car/volkswagen/carcontroller.py @@ -193,6 +193,22 @@ def accel_during_driver_override(accel: float, gas_pressed: bool, keep_long_acti return 0.0 if gas_pressed and keep_long_active else accel +def ea_send_ready(stock_values, last_counter): + return bool(stock_values) and stock_values["COUNTER"] != last_counter + + +EA_BLINKER_STEP = 2 + + +def next_ea_counter(tx_counter, stock_counter): + return ((stock_counter if tx_counter is None else tx_counter) + 1) % 16 + + +def ea_blinker_command(left_request, right_request, left_active, right_active): + blinker_active = left_active or right_active + return left_request and not blinker_active, right_request and not blinker_active + + class CarController(CarControllerBase): def __init__(self, dbc_names, CP, CP_IQ): from iqpilot.system.proprietary_runtime._verified_import import import_verified_module @@ -241,6 +257,8 @@ class CarController(CarControllerBase): self.sng_handoff_active = False self.acc_counter_seeded = False self.klr_counter_last = None + self.ea_counter_last = None + self.ea_tx_counter = None self.eps_timer_soft_disable_alert = False self.hca_frame_timer_running = 0 self.hca_frame_same_torque = 0 @@ -462,9 +480,14 @@ class CarController(CarControllerBase): self.klr_counter_last = CS.klr_stock_values["COUNTER"] if self.CP.flags & (VolkswagenFlags.MEB | VolkswagenFlags.MQB_EVO): - if self.frame % 2 == 0: + if CS.ea_hud_stock_values and self.frame % EA_BLINKER_STEP == 0: + self.ea_tx_counter = next_ea_counter(self.ea_tx_counter, CS.ea_hud_stock_values["COUNTER"]) + left_blinker, right_blinker = ea_blinker_command( + CC.leftBlinker, CC.rightBlinker, CS.left_blinker_active, CS.right_blinker_active, + ) can_sends.append(self.CCS.create_blinker_control(self.packer_pt, self.CAN.pt, CS.ea_hud_stock_values, CS.ea_control_stock_values, - CC.leftBlinker, CC.rightBlinker, self.hide_ea_error)) + left_blinker, right_blinker, self.hide_ea_error, self.ea_tx_counter)) + self.ea_counter_last = CS.ea_hud_stock_values["COUNTER"] if self.CP.openpilotLongitudinalControl and self.CCS in (mqbcan, mlbcan) and not self.acc_counter_seeded and CS.acc_stock_counters: seed_msgs = ("ACC_01", "ACC_02") if self.CCS is mlbcan else ("ACC_02", "ACC_06", "ACC_07", "ACC_10") diff --git a/artifacts/package_sources/iqdbc/iqdbc/car/volkswagen/mebcan.py b/artifacts/package_sources/iqdbc/iqdbc/car/volkswagen/mebcan.py index 6d2a031ee..05de04a8a 100644 --- a/artifacts/package_sources/iqdbc/iqdbc/car/volkswagen/mebcan.py +++ b/artifacts/package_sources/iqdbc/iqdbc/car/volkswagen/mebcan.py @@ -55,8 +55,9 @@ def create_eps_update(packer, bus, eps_stock_values, ea_simulated_torque): return packer.make_can_msg("LH_EPS_03", bus, values) -def create_blinker_control(packer, bus, ea_hud_stock_values, ea_control_stock_values, left_blinker, right_blinker, hide_error): +def create_blinker_control(packer, bus, ea_hud_stock_values, ea_control_stock_values, left_blinker, right_blinker, hide_error, counter=None): values = {s: ea_hud_stock_values[s] for s in [ + "COUNTER", "EA_Texte", "ACF_Lampe_Hands_Off", "EA_Infotainment_Anf", @@ -69,6 +70,9 @@ def create_blinker_control(packer, bus, ea_hud_stock_values, ea_control_stock_va "EA_Unknown", ]} + if counter is not None: + values["COUNTER"] = counter + if ea_hud_stock_values["EA_Blinken"] == 0: values.update({ "EA_Blinken": 1 if left_blinker else (2 if right_blinker else ea_hud_stock_values["EA_Blinken"]), diff --git a/artifacts/package_sources/iqdbc/iqdbc/car/volkswagen/tests/test_meb_nav_blinker.py b/artifacts/package_sources/iqdbc/iqdbc/car/volkswagen/tests/test_meb_nav_blinker.py index 9c126d7a6..ff567fa38 100644 --- a/artifacts/package_sources/iqdbc/iqdbc/car/volkswagen/tests/test_meb_nav_blinker.py +++ b/artifacts/package_sources/iqdbc/iqdbc/car/volkswagen/tests/test_meb_nav_blinker.py @@ -2,9 +2,11 @@ from iqdbc.can import CANPacker, CANParser from iqdbc.car.volkswagen import mebcan +from iqdbc.car.volkswagen.carcontroller import ea_blinker_command, ea_send_ready, next_ea_counter EA_HUD_VALUES = { + "COUNTER": 0, "EA_Texte": 0, "ACF_Lampe_Hands_Off": 0, "EA_Infotainment_Anf": 0, @@ -23,21 +25,74 @@ def blinker_values(dbc, requests): packer = CANPacker(dbc) parser = CANParser(dbc, [("EA_02", 50)], 0) values = [] - for left, right in requests: + for counter, stock_blinker, left, right in requests: + ea_hud_values = {**EA_HUD_VALUES, "COUNTER": counter, "EA_Blinken": stock_blinker} msg = mebcan.create_blinker_control( - packer, 0, EA_HUD_VALUES, EA_CONTROL_VALUES, left, right, False, + packer, 0, ea_hud_values, EA_CONTROL_VALUES, left, right, False, ) parser.update([0, [msg]]) - values.append(int(parser.vl["EA_02"]["EA_Blinken"])) + values.append((int(parser.vl["EA_02"]["COUNTER"]), int(parser.vl["EA_02"]["EA_Blinken"]))) return values +def blinker_value(dbc, counter, stock_blinker, left, right, override_counter=None): + packer = CANPacker(dbc) + parser = CANParser(dbc, [("EA_02", 50)], 0) + ea_hud_values = {**EA_HUD_VALUES, "COUNTER": counter, "EA_Blinken": stock_blinker} + msg = mebcan.create_blinker_control( + packer, 0, ea_hud_values, EA_CONTROL_VALUES, left, right, False, override_counter, + ) + parser.update([0, [msg]]) + return int(parser.vl["EA_02"]["COUNTER"]), int(parser.vl["EA_02"]["EA_Blinken"]) + + def test_nav_blinker_request_remains_asserted_until_released(): - requests = [(True, False)] * 100 + [(False, False)] - assert blinker_values("vw_meb", requests) == [1] * 100 + [0] + requests = [(counter % 16, 0, True, False) for counter in range(100)] + [(4, 0, False, False)] + assert blinker_values("vw_meb", requests) == [(counter % 16, 1) for counter in range(100)] + [(4, 0)] def test_nav_blinker_directions_on_meb_variants(): - requests = [(True, False), (False, True), (False, False)] + requests = [(7, 0, True, False), (8, 0, False, True), (9, 0, False, False)] for dbc in ("vw_meb", "vw_meb_2024", "vw_mqbevo"): - assert blinker_values(dbc, requests) == [1, 2, 0] + assert blinker_values(dbc, requests) == [(7, 1), (8, 2), (9, 0)] + + +def test_stock_blinker_has_priority_over_nav_request(): + requests = [(10, 1, False, True), (11, 2, True, False), (12, 3, True, False)] + assert blinker_values("vw_meb", requests) == [(10, 1), (11, 2), (12, 3)] + + +def test_ea_frame_is_sent_once_per_stock_counter(): + last_counter = None + sends = [] + for counter in (5, 5, 5, 6, 6, 7): + stock_values = {"COUNTER": counter} + if ea_send_ready(stock_values, last_counter): + sends.append(counter) + last_counter = counter + assert sends == [5, 6, 7] + + +def test_blinker_counter_can_continue_between_stock_frames(): + assert blinker_value("vw_meb", 8, 0, True, False, 9) == (9, 1) + assert blinker_value("vw_meb", 8, 0, True, False, 10) == (10, 1) + + +def test_stock_blinker_priority_survives_counter_override(): + assert blinker_value("vw_meb", 8, 2, True, False, 9) == (9, 2) + + +def test_nav_blinker_retriggers_only_between_lamp_cycles(): + assert ea_blinker_command(True, False, False, False) == (True, False) + assert ea_blinker_command(True, False, True, False) == (False, False) + assert ea_blinker_command(True, False, False, False) == (True, False) + assert ea_blinker_command(False, True, False, True) == (False, False) + + +def test_blinker_counter_runs_at_20_ms(): + tx_counter = None + counters = [] + for _ in range(10): + tx_counter = next_ea_counter(tx_counter, 14) + counters.append(tx_counter) + assert counters == [15, 0, 1, 2, 3, 4, 5, 6, 7, 8] diff --git a/artifacts/package_sources/iqdbc/iqdbc/car/volkswagen/tests/test_volkswagen.py b/artifacts/package_sources/iqdbc/iqdbc/car/volkswagen/tests/test_volkswagen.py index b791b8fb3..96e44387e 100644 --- a/artifacts/package_sources/iqdbc/iqdbc/car/volkswagen/tests/test_volkswagen.py +++ b/artifacts/package_sources/iqdbc/iqdbc/car/volkswagen/tests/test_volkswagen.py @@ -210,6 +210,7 @@ def _a4_mk4_frames(packer, reverse=False, eps_torque=None): packer.make_can_msg("Kombi_02", 1, {"KBI_Inhalt_Tank": 40, "KBI_Kilometerstand": 100000}), packer.make_can_msg("Airbag_02", 1, {"AB_Gurtschloss_FA": 3}), packer.make_can_msg("Gateway_05", 1, {"BCM1_Rueckfahrlicht_Schalter": int(reverse)}), + packer.make_can_msg("LH_EPS_01", 1, {}), ] if eps_torque is not None: msgs.append(packer.make_can_msg("LH_EPS_03", 1, {"EPS_Lenkmoment": abs(eps_torque), diff --git a/artifacts/package_sources/iqdbc/iqdbc/safety/modes/hyundai_canfd.h b/artifacts/package_sources/iqdbc/iqdbc/safety/modes/hyundai_canfd.h index 3a7d8c8df..34937a977 100644 --- a/artifacts/package_sources/iqdbc/iqdbc/safety/modes/hyundai_canfd.h +++ b/artifacts/package_sources/iqdbc/iqdbc/safety/modes/hyundai_canfd.h @@ -7,7 +7,7 @@ #pragma GCC diagnostic ignored "-Wunused-function" const TorqueSteeringLimits HYUNDAI_CANFD_STEERING_LIMITS = { - .max_torque = 512, //270, + .max_torque = 270, .max_rt_delta = 112, .max_rate_up = 2, @@ -85,34 +85,34 @@ const CanMsg HYUNDAI_CANFD_HDA2_LONG_TX_MSGS[] = { {203, 0, 24}, // CB {373, 2, 24}, // TCS(0x175) {506, 2, 32}, // CLUSTER_SPEED_LIMIT - {234, 2, 24}, // MDPS - {687, 2, 8}, // STEER_TOUCH_2AF - {0x4BE, 2, 8}, // NEW_MSG_4BE (may be corner radar enabler x) {0x4B9, 2, 8}, // NEW_MSG_4B9 (may be corner radar enabler) }; +#define HYUNDAI_CANFD_HDA1_TX_MSGS_COMMON(longitudinal) \ + {0x12A, 0, 16, .check_relay = true}, \ + {0x1A0, 0, 32, .check_relay = (longitudinal)}, \ + {0x1CF, 0, 8, .check_relay = false}, \ + {0x1CF, 2, 8, .check_relay = (longitudinal)}, \ + {0x1E0, 0, 16, .check_relay = true}, \ + {0x160, 0, 16, .check_relay = (longitudinal)}, \ + {0x7D0, 0, 8, .check_relay = false}, \ + {0x1AA, 2, 16, .check_relay = (longitudinal)}, \ + {0x0CB, 0, 24, .check_relay = (longitudinal)}, \ + {0x175, 2, 24, .check_relay = (longitudinal)}, \ + {0x161, 0, 32, .check_relay = (longitudinal)}, \ + {0x162, 0, 32, .check_relay = (longitudinal)}, \ + {0x1EA, 0, 32, .check_relay = (longitudinal)}, \ + {0x200, 0, 8, .check_relay = (longitudinal)}, \ + {0x4A3, 2, 8, .check_relay = false}, \ + {0x4B4, 2, 8, .check_relay = false}, \ + const CanMsg HYUNDAI_CANFD_HDA1_TX_MSGS[] = { - {0x12A, 0, 16}, // LFA - {0x1A0, 0, 32}, // CRUISE_INFO - {0x1CF, 0, 8}, // CRUISE_BUTTON - {0x1CF, 2, 8}, // CRUISE_BUTTON - {0x1E0, 0, 16}, // LFAHDA_CLUSTER - {0x160, 0, 16}, // ADRV_0x160 - {0x7D0, 0, 8}, // tester present for radar ECU disable - {0x1AA, 2, 16}, // CRUISE_ALT_BUTTONS , carrot - {203, 0, 24}, // CB - {373, 2, 24}, // TCS(0x175) - - {353, 0, 32}, // ADRV_353 - {354, 0, 32}, // CORNER_RADAR_HIGHWAY - {512, 0, 8}, // ADRV_0x200 - {1187, 2, 8}, // 4A3 - {1204, 2, 8}, // 4B4 - {373, 2, 24}, // TCS(0x175) - {234, 2, 24}, // MDPS - {687, 2, 8}, // STEER_TOUCH_2AF + HYUNDAI_CANFD_HDA1_TX_MSGS_COMMON(false) +}; +const CanMsg HYUNDAI_CANFD_HDA1_LONG_TX_MSGS[] = { + HYUNDAI_CANFD_HDA1_TX_MSGS_COMMON(true) }; @@ -221,7 +221,6 @@ const int HYUNDAI_PARAM_CANFD_ALT_BUTTONS = 32; const int HYUNDAI_PARAM_CANFD_HDA2_ALT_STEERING = 128; bool hyundai_canfd_alt_buttons = false; bool hyundai_canfd_hda2_alt_steering = false; -bool hyundai_canfd_buffered_fwd = false; int hyundai_canfd_hda2_get_lkas_addr(void) { return hyundai_canfd_hda2_alt_steering ? 0x110 : 0x50; @@ -275,8 +274,6 @@ CanfdTxState canfd_tx_states[] = { {0x1AA, 2, 50, 0U, 0U}, // 426: CRUISE_ALT_BUTTONS {0x1CF, 2, 50, 0U, 0U}, // 463: CRUISE_BUTTON {0x1FA, 2, 10, 0U, 0U}, // 506: CLUSTER_SPEED_LIMIT - {0x0EA, 2, 100, 0U, 0U}, // 234: MDPS - {0x2AF, 2, 10, 0U, 0U}, // 687: STEER_TOUCH_2AF {0x4A3, 2, 5, 0U, 0U}, // 1187: HDA_INFO_4A3 {0x4B4, 2, 10, 0U, 0U}, // 1204: NEW_MSG_4B4 {0x4BE, 2, 10, 0U, 0U}, // 1214: NEW_MSG_4BE @@ -295,30 +292,6 @@ static CanfdTxState* find_canfd_tx_state(int bus, int addr) { } -static void hyundai_canfd_set_counter(CANPacket_t* to_push, uint8_t counter) { - if (GET_LEN(to_push) == 8U) { - to_push->data[1] = (to_push->data[1] & 0x0FU) | ((counter & 0x0FU) << 4); - } - else { - to_push->data[2] = counter; - } -} - -static void hyundai_canfd_set_checksum(CANPacket_t* to_push, uint16_t checksum) { - to_push->data[0] = (uint8_t)(checksum & 0xFFU); - to_push->data[1] = (uint8_t)((checksum >> 8U) & 0xFFU); -} - -static void hyundai_canfd_update_checksum(CANPacket_t* to_push) { - to_push->data[0] = 0U; - to_push->data[1] = 0U; - uint32_t checksum = hyundai_common_canfd_compute_checksum(to_push); - hyundai_canfd_set_checksum(to_push, (uint16_t)checksum); -} -static void canfd_apply_counter_and_update_checksum(CANPacket_t* dst, uint8_t counter) { - hyundai_canfd_set_counter(dst, counter); - hyundai_canfd_update_checksum(dst); -} static void canfd_record_tx_time(int bus, int addr, bool tx) { CanfdTxState* st = find_canfd_tx_state(bus, addr); if (st != NULL) { @@ -334,126 +307,6 @@ static bool canfd_should_block_fwd(int tx_bus, int addr, uint32_t now) { return (now - st->last_tx_us) < st->timeout_us; } -#define CANFD_BFWD_MAX_QUEUE 2 -#define CANFD_BFWD_REUSE_MAX 2 - -typedef struct { - int addr; - int dst_bus; - bool enabled; - - bool started; - uint8_t head; - uint8_t tail; - uint8_t count; - - uint8_t reuse_left; - bool has_last_pkt; - CANPacket_t last_pkt; - - CANPacket_t q[CANFD_BFWD_MAX_QUEUE]; -} CanfdBufferedFwd; - -CanfdBufferedFwd canfd_bfwd[] = { - {.addr = 0x1A0, .dst_bus = 0, .enabled = true }, // SCC_CONTROL - {.addr = 0x12A, .dst_bus = 0, .enabled = true }, // LFA - {.addr = 0x0CB, .dst_bus = 0, .enabled = true }, // LFA_ALT - {.addr = 0x0EA, .dst_bus = 2, .enabled = true }, // MDPS - {.addr = 0x1AA, .dst_bus = 2, .enabled = true }, // CRUISE_ALT_BUTTONS - // {.addr = 0x1CF, .dst_bus = 2, .enabled = true }, // CRUISE_BUTTON - {.addr = 0x175, .dst_bus = 2, .enabled = true }, // TCS - { 0 }, -}; - -static void canfd_copy_packet(CANPacket_t* dst, const CANPacket_t* src) { - dst->fd = src->fd; - dst->returned = 0U; - dst->rejected = 0U; - dst->extended = src->extended; - dst->addr = src->addr; - dst->bus = src->bus; - dst->data_len_code = src->data_len_code; - for (uint8_t i = 0U; i < GET_LEN(src); i++) { - dst->data[i] = src->data[i]; - } -} -static CanfdBufferedFwd* canfd_bfwd_find(int addr, int dst_bus) { - for (int i = 0; canfd_bfwd[i].addr > 0; i++) { - if (canfd_bfwd[i].enabled && - (canfd_bfwd[i].addr == addr) && - (canfd_bfwd[i].dst_bus == dst_bus)) { - return &canfd_bfwd[i]; - } - } - return NULL; -} - -static void canfd_bfwd_reset(CanfdBufferedFwd* st) { - st->started = false; - st->head = 0U; - st->tail = 0U; - st->count = 0U; - st->reuse_left = 0U; - st->has_last_pkt = false; - st->last_pkt = (CANPacket_t){0}; -} -static void canfd_bfwd_push(CanfdBufferedFwd* st, const CANPacket_t* pkt) { - if ((st == NULL) || !st->enabled) return; - if (GET_BUS(pkt) != st->dst_bus) return; - - // queue�� �̹� 2���� �̹� �� packet�� ���� - if (st->count >= CANFD_BFWD_MAX_QUEUE) { - return; - } - - canfd_copy_packet(&st->q[st->tail], pkt); - st->tail = (st->tail + 1U) % CANFD_BFWD_MAX_QUEUE; - st->count++; - st->started = true; -} - -static bool canfd_bfwd_pop(CanfdBufferedFwd* st, CANPacket_t* pkt) { - if ((st == NULL) || !st->enabled) { - return false; - } - - if (!st->started || (st->count == 0U)) { - return false; - } - - canfd_copy_packet(pkt, &st->q[st->head]); - st->head = (st->head + 1U) % CANFD_BFWD_MAX_QUEUE; - st->count--; - - // ������ ���� packet ���� - canfd_copy_packet(&st->last_pkt, pkt); - st->has_last_pkt = true; - st->reuse_left = CANFD_BFWD_REUSE_MAX; - - if (st->count == 0U) { - st->started = false; - } - - return true; -} -static bool canfd_bfwd_reuse_last(CanfdBufferedFwd* st, CANPacket_t* pkt) { - if ((st == NULL) || !st->enabled) { - return false; - } - - if (!st->has_last_pkt || (st->reuse_left == 0U)) { - return false; - } - - canfd_copy_packet(pkt, &st->last_pkt); - st->reuse_left--; - - return true; -} - - - - static void hyundai_canfd_rx_hook(const CANPacket_t *to_push) { int bus = GET_BUS(to_push); int addr = GET_ADDR(to_push); @@ -538,26 +391,7 @@ static void hyundai_canfd_rx_hook(const CANPacket_t *to_push) { } -static bool hyundai_canfd_tx_hook(const CANPacket_t *to_send_const) { - CANPacket_t* to_send = (CANPacket_t*)to_send_const; - - const TorqueSteeringLimits HYUNDAI_CANFD_STEERING_LIMITS = { - .max_torque = 512, - .max_rt_delta = 112, - - .max_rate_up = 10, - .max_rate_down = 10, - .driver_torque_allowance = 250, - .driver_torque_multiplier = 2, - .type = TorqueDriverLimited, - - // the EPS faults when the steering angle is above a certain threshold for too long. to prevent this, - // we allow setting torque actuation bit to 0 while maintaining the requested torque value for two consecutive frames - .min_valid_request_frames = 89, - .max_invalid_request_frames = 2, - .min_valid_request_rt_interval = 810000, // 810ms; a ~10% buffer on cutting every 90 frames - .has_steer_req_tolerance = true, - }; +static bool hyundai_canfd_tx_hook(const CANPacket_t *to_send) { bool tx = true; int addr = GET_ADDR(to_send); @@ -618,14 +452,6 @@ static bool hyundai_canfd_tx_hook(const CANPacket_t *to_send_const) { if (violation) { tx = false; } - else if (hyundai_canfd_buffered_fwd) { - CanfdBufferedFwd* bfwd = canfd_bfwd_find(addr, GET_BUS(to_send)); - if (bfwd != NULL) { - canfd_bfwd_push(bfwd, to_send); - //tx = false; - return true; - } - } canfd_record_tx_time(GET_BUS(to_send), addr, tx); @@ -633,6 +459,10 @@ static bool hyundai_canfd_tx_hook(const CANPacket_t *to_send_const) { } static bool hyundai_canfd_fwd_hook(int bus_num, int addr) { + if (!hyundai_canfd_hda2) { + return (bus_num == 0) && (addr == 0x4B9); + } + uint32_t now = microsecond_timer_get(); if (bus_num == 0) { if (canfd_should_block_fwd(2, addr, now)) { @@ -653,16 +483,11 @@ static safety_config hyundai_canfd_init(uint16_t param) { canfd_tx_states[i].last_tx_us = 0U; } - for (int i = 0; canfd_bfwd[i].addr > 0; i++) { - canfd_bfwd_reset(&canfd_bfwd[i]); - } - hyundai_common_init(param); gen_crc_lookup_table_16(0x1021, hyundai_canfd_crc_lut); hyundai_canfd_alt_buttons = GET_FLAG(param, HYUNDAI_PARAM_CANFD_ALT_BUTTONS); hyundai_canfd_hda2_alt_steering = GET_FLAG(param, HYUNDAI_PARAM_CANFD_HDA2_ALT_STEERING); - hyundai_canfd_buffered_fwd = hyundai_camera_scc; // no long for radar-SCC HDA1 yet //if (!hyundai_canfd_hda2 && !hyundai_camera_scc) { @@ -680,8 +505,8 @@ static safety_config hyundai_canfd_init(uint16_t param) { else ret = BUILD_SAFETY_CFG(hyundai_canfd_hda2_long_rx_checks, HYUNDAI_CANFD_HDA2_LONG_TX_MSGS); } } else { - ret = hyundai_canfd_alt_buttons ? BUILD_SAFETY_CFG(hyundai_canfd_long_alt_buttons_rx_checks, HYUNDAI_CANFD_HDA1_TX_MSGS) : \ - BUILD_SAFETY_CFG(hyundai_canfd_long_rx_checks, HYUNDAI_CANFD_HDA1_TX_MSGS); + ret = hyundai_canfd_alt_buttons ? BUILD_SAFETY_CFG(hyundai_canfd_long_alt_buttons_rx_checks, HYUNDAI_CANFD_HDA1_LONG_TX_MSGS) : \ + BUILD_SAFETY_CFG(hyundai_canfd_long_rx_checks, HYUNDAI_CANFD_HDA1_LONG_TX_MSGS); } } else { if (hyundai_canfd_hda2 && hyundai_camera_scc) { diff --git a/artifacts/package_sources/iqdbc/iqdbc/safety/tests/test_hyundai_canfd.py b/artifacts/package_sources/iqdbc/iqdbc/safety/tests/test_hyundai_canfd.py index 8b84f8169..a30c2e0f6 100644 --- a/artifacts/package_sources/iqdbc/iqdbc/safety/tests/test_hyundai_canfd.py +++ b/artifacts/package_sources/iqdbc/iqdbc/safety/tests/test_hyundai_canfd.py @@ -44,12 +44,12 @@ def test_canfd_steering_limits(safety, param, addr, length): assert not safety.safety_tx_hook(canfd_steer(addr, length, 1)) safety.set_controls_allowed(True) - assert safety.safety_tx_hook(canfd_steer(addr, length, 10)) - safety.set_desired_torque_last(512) - safety.set_rt_torque_last(512) - assert safety.safety_tx_hook(canfd_steer(addr, length, 512)) - assert not safety.safety_tx_hook(canfd_steer(addr, length, 513)) - assert not safety.safety_tx_hook(canfd_steer(addr, length, -513)) + assert safety.safety_tx_hook(canfd_steer(addr, length, 2)) + safety.set_desired_torque_last(270) + safety.set_rt_torque_last(270) + assert safety.safety_tx_hook(canfd_steer(addr, length, 270)) + assert not safety.safety_tx_hook(canfd_steer(addr, length, 271)) + assert not safety.safety_tx_hook(canfd_steer(addr, length, -271)) def test_canfd_tx_whitelist_and_buttons(safety): @@ -90,6 +90,31 @@ def test_canfd_camera_scc_button_passthrough(safety): assert safety.safety_tx_hook(packet(0x1CF, 2, 8, {2: 4})) +def test_canfd_camera_scc_single_owner_forwarding(safety): + safety.set_safety_hooks(CarParams.SafetyModel.hyundaiCanfd, HyundaiSafetyFlags.CAMERA_SCC) + safety.init_tests() + safety.set_timer(1_000_000) + + assert safety.safety_fwd_hook(2, 0x12A) == -1 + assert safety.safety_fwd_hook(2, 0x1E0) == -1 + assert safety.safety_fwd_hook(2, 0x1A0) == 0 + assert safety.safety_fwd_hook(0, 0xEA) == 2 + assert not safety.safety_tx_hook(packet(0xEA, 2, 24)) + assert not safety.safety_tx_hook(packet(0x2AF, 2, 8)) + + +def test_canfd_camera_scc_longitudinal_single_owner_forwarding(safety): + param = HyundaiSafetyFlags.CAMERA_SCC | HyundaiSafetyFlags.LONG + safety.set_safety_hooks(CarParams.SafetyModel.hyundaiCanfd, param) + safety.init_tests() + safety.set_timer(1_000_000) + + assert safety.safety_fwd_hook(2, 0x12A) == -1 + assert safety.safety_fwd_hook(2, 0x1A0) == -1 + assert safety.safety_fwd_hook(0, 0x175) == -1 + assert safety.safety_fwd_hook(0, 0xEA) == 2 + + def test_canfd_stock_longitudinal_only_allows_cancel(safety): safety.set_safety_hooks(CarParams.SafetyModel.hyundaiCanfd, 0) safety.init_tests() diff --git a/artifacts/package_sources/iqdbc/iqdbc/safety/tests/test_hyundai_controller.py b/artifacts/package_sources/iqdbc/iqdbc/safety/tests/test_hyundai_controller.py index 71c14b5b1..810665a48 100644 --- a/artifacts/package_sources/iqdbc/iqdbc/safety/tests/test_hyundai_controller.py +++ b/artifacts/package_sources/iqdbc/iqdbc/safety/tests/test_hyundai_controller.py @@ -2,7 +2,7 @@ import pytest from iqdbc.car import gen_empty_fingerprint, structs from iqdbc.car.hyundai.interface import CarInterface -from iqdbc.car.hyundai.values import CAR +from iqdbc.car.hyundai.values import CAR, HyundaiFlags from iqdbc.safety.tests.libsafety import libsafety_py @@ -37,3 +37,24 @@ def test_controller_frames_match_configured_safety(candidate, alpha_long, monkey packet = libsafety_py.make_CANPacket(address, bus, data) rejection = f"{candidate.value} frame {frame}: safety rejected address={address:#x} bus={bus} data={data.hex()}" assert safety.safety_tx_hook(packet), rejection + + +def test_ev6_camera_scc_has_one_lfa_sender(monkeypatch, tmp_path): + monkeypatch.setenv("PARAMS_ROOT", str(tmp_path)) + fingerprint = gen_empty_fingerprint() + cp = CarInterface.get_params(CAR.KIA_EV6, fingerprint, [], False, False, False) + cp.flags &= ~HyundaiFlags.CANFD_HDA2.value + cp.flags |= HyundaiFlags.CANFD_CAMERA_SCC.value + cp_iq = CarInterface.get_params_iq(cp, CAR.KIA_EV6, fingerprint, [], False, False, False) + interface = CarInterface(cp, cp_iq) + interface.update([]) + + control = structs.CarControl.new_message() + control.enabled = True + control.latActive = True + control.actuators.torque = 0.01 + + for _ in range(20): + _, can_sends = interface.apply(control.as_reader(), structs.IQCarControl()) + assert [(address, bus) for address, _, bus in can_sends if address == 0x12A] == [(0x12A, 0)] + assert all(address not in (0xEA, 0x2AF) for address, _, _ in can_sends) diff --git a/artifacts/package_sources/panda/board/drivers/can_common.h b/artifacts/package_sources/panda/board/drivers/can_common.h index 7d4b57575..6d0385a53 100644 --- a/artifacts/package_sources/panda/board/drivers/can_common.h +++ b/artifacts/package_sources/panda/board/drivers/can_common.h @@ -307,7 +307,7 @@ void ignition_can_hook(CANPacket_t *msg) { vw_meb_gateway_out_of_p = (fahrstufe >= 6) && (fahrstufe <= 14); } - if ((msg->addr == 0xC0U) && (len == 32)) { + if (((msg->addr == 0xC0U) && (len == 32)) || ((msg->addr == 0x20AU) && (len == 64))) { vw_meb_seen = true; } diff --git a/iqpilot/cereal/custom.capnp b/iqpilot/cereal/custom.capnp index 1e813ec0c..cc561146a 100644 --- a/iqpilot/cereal/custom.capnp +++ b/iqpilot/cereal/custom.capnp @@ -131,6 +131,7 @@ struct IQModelManager @0xe91d6987759290bb { policy @3; offPolicy @4; onPolicy @5; + usbeMac @6; } } @@ -440,6 +441,9 @@ struct IQCarState @0xb1c39318bb6bc2b3 { # VW PQ stock ACC radar feedback for the IQ.Dynamics radar_manager (Blend feature) accRadarStaAdr @4 :UInt8; # ACC_System.ACS_Sta_ADR (0 not-active, 1 active, 2 passive, 3 irrev_Fehler) accRadarFehler @5 :Bool; # ACC_System.ACS_Fehler (stored fault -> radar dead for the drive) + slcSetSpeedRequestId @6 :UInt32; + slcSetSpeedGestureId @7 :UInt32; + slcSetSpeedRequestKph @8 :Float32; } struct IQLiveData @0xf2e2b608e51f4b0e { diff --git a/iqpilot/cereal/log.capnp b/iqpilot/cereal/log.capnp index 549a05f45..fb080df1c 100644 --- a/iqpilot/cereal/log.capnp +++ b/iqpilot/cereal/log.capnp @@ -135,6 +135,8 @@ struct OnroadEvent @0xc4fa6047f024e718 { userBookmark @95; excessiveActuation @96; audioFeedback @97; + bigModelLoading @101; + bigModelFailed @102; soundsUnavailableDEPRECATED @47; } @@ -491,6 +493,13 @@ struct DeviceState @0xa4d8b5af2aa492eb { started @11 :Bool; startedMonoTime @13 :UInt64; + # ordinals track commaai/openpilot exactly (@50 bottomSocTempC, @51 dock + # presence, @52 usbState) so upstream tooling stays able to read our logs; + # only the field NAME differs, we do not use comma's internal dock codename. + bottomSocTempC @50 :Float32; + egpuDockPresent @51 :Bool; + usbState @52 :UsbState; + # system utilization freeSpacePercent @7 :Float32; memoryUsagePercent @19 :Int8; @@ -586,6 +595,37 @@ struct DeviceState @0xa4d8b5af2aa492eb { nvmeTempCDEPRECATED @35 :List(Float32); } +struct UsbState { + devices @0 :List(Device); + # IQ extension (no upstream equivalent): controller-level link errors, needed + # because portli is a CONTROLLER counter and in peripheral mode (eMac gadget + # link) the peer never enumerates, so no Device row can carry it. + linkErrorCount @1 :UInt32; + # IQ extension, same reason: CC orientation describes the PORT, so it is + # readable while the link is in peripheral mode. The per-device field below + # matches upstream but only populates when something enumerates on that + # controller (host mode, e.g. the eGPU dock) — never during an eMac session. + usb3Lane @2 :Device.Usb3Lane; + + struct Device { + busnum @0 :UInt8; + devnum @1 :UInt8; + vendorId @2 :UInt16; + productId @3 :UInt16; + speedMbps @4 :UInt16; + manufacturer @6 :Text; + product @5 :Text; + linkErrorCount @7 :UInt16; + usb3Lane @8 :Usb3Lane; + + enum Usb3Lane { + unknown @0; + a @1; + b @2; + } + } +} + struct PandaState @0xa7649e2575e4591e { ignitionLine @2 :Bool; rxBufferOverflow @7 :UInt32; @@ -745,6 +785,20 @@ struct PeripheralState { } } +struct EgpuDockState { + tempC @0 :Float32; + memoryTempC @1 :Float32; + powerDrawW @2 :Float32; + powerLimitW @3 :Float32; + gpuUsagePercent @4 :UInt8; + gpuClockMhz @5 :UInt16; + fanSpeedRpm @6 :UInt16; + pcieLtssm @7 :UInt8; + supplyVoltage @8 :UInt16; # mV + supplyCurrent @9 :Int16; # mA + supplyFault @10 :Bool; +} + struct RadarState @0x9a185389d6fdd05f { mdMonoTime @6 :UInt64; carStateMonoTime @11 :UInt64; @@ -1078,6 +1132,7 @@ struct ModelDataV2 { timestampEof @3 :UInt64; modelExecutionTime @15 :Float32; rawPredictions @16 :Data; + big @27 :Bool; # predicted future position, orientation, etc.. position @4 :XYZTData; @@ -2668,6 +2723,7 @@ struct Event { iqState @153 :Custom.IQState; iqModelManager @154 :Custom.IQModelManager; iqPlan @155 :Custom.IQPlan; + egpuDockState @166 :EgpuDockState; iqOnroadEvents @156 :Custom.IQOnroadEvent; iqCarParams @157 :Custom.IQCarParams; iqCarControl @158 :Custom.IQCarControl; diff --git a/iqpilot/cereal/services.py b/iqpilot/cereal/services.py index 1f6fa9c50..4bca8a3e4 100755 --- a/iqpilot/cereal/services.py +++ b/iqpilot/cereal/services.py @@ -108,6 +108,7 @@ _services: dict[str, tuple] = { "iqRoadIncidentFeed": (True, 0.2, 1), "iqNavRenderState": (True, 5., 10, QueueSize.MEDIUM), "iqState": (True, 100., 10), + "egpuDockState": (True, 10., 10), "iqPlan": (True, 20., 10), "iqOnroadEvents": (True, 1., 1), "iqDriveModelData": (True, 20., None, QueueSize.MEDIUM), diff --git a/iqpilot/common/params_keys.h b/iqpilot/common/params_keys.h index f3b8a2de8..8dfab24e2 100644 --- a/iqpilot/common/params_keys.h +++ b/iqpilot/common/params_keys.h @@ -105,6 +105,13 @@ inline static std::unordered_map keys = { {"ObdMultiplexingChanged", {CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION, BOOL}}, {"ObdMultiplexingEnabled", {CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION, BOOL}}, {"Offroad_CarUnrecognized", {CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION, JSON}}, + {"Offroad_EgpuNotDetected", {CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION, JSON}}, + {"Offroad_EgpuFansObstructed", {CLEAR_ON_MANAGER_START, JSON}}, + {"Offroad_EgpuOverheated", {CLEAR_ON_MANAGER_START, JSON}}, + {"Offroad_EgpuPcieUnavailable", {CLEAR_ON_MANAGER_START, JSON}}, + {"Offroad_EgpuUncompiled", {CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION, JSON}}, + {"Offroad_EgpuUpdateFailed", {CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION, JSON}}, + {"Offroad_EgpuUsbSlow", {CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION, JSON}}, {"Offroad_ConnectivityNeeded", {CLEAR_ON_MANAGER_START, JSON}}, {"Offroad_ConnectivityNeededPrompt", {CLEAR_ON_MANAGER_START, JSON}}, {"Offroad_ExcessiveActuation", {PERSISTENT, JSON}}, @@ -224,6 +231,34 @@ inline static std::unordered_map keys = { {"ModelManager_LastSyncTime", {CLEAR_ON_MANAGER_START | CLEAR_ON_OFFROAD_TRANSITION, INT, "0"}}, {"ModelManager_ModelsCache", {PERSISTENT, JSON}}, + {"IQEmacEnabled", {PERSISTENT, BOOL, "0"}}, + {"IQEmacHost", {PERSISTENT, STRING}}, + {"IQEmacModel", {PERSISTENT, STRING}}, + {"IQEmacCatalogCache", {PERSISTENT, STRING}}, + {"MacModelDownloadProgress", {CLEAR_ON_MANAGER_START, STRING, "1.0"}}, + {"MacModelStatus", {CLEAR_ON_MANAGER_START, STRING}}, + {"MacModelPresent", {CLEAR_ON_MANAGER_START, BOOL}}, + {"MacModelReachable", {CLEAR_ON_MANAGER_START, BOOL}}, + {"MacModelReady", {CLEAR_ON_MANAGER_START, BOOL}}, + {"MacModelActive", {CLEAR_ON_MANAGER_START, BOOL}}, + {"MacModelFailed", {CLEAR_ON_MANAGER_START, BOOL}}, + {"MacModelLastError", {CLEAR_ON_MANAGER_START, STRING}}, + {"MacModelLatencyMs", {CLEAR_ON_MANAGER_START, FLOAT, "0.0"}}, + + // comma USB eGPU big-model backend. Mutually exclusive with eMac at + // runtime (eMac wins). UsbGpu* naming/flags mirror comma's handover branch + {"IQEgpuEnabled", {PERSISTENT, BOOL, "0"}}, + {"IQEgpuDisabled", {PERSISTENT, BOOL, "0"}}, + {"UsbGpuPresent", {CLEAR_ON_MANAGER_START | CLEAR_ON_OFFROAD_TRANSITION, BOOL}}, + {"UsbGpuCompiled", {CLEAR_ON_MANAGER_START | CLEAR_ON_OFFROAD_TRANSITION, BOOL}}, + {"UsbGpuLoading", {CLEAR_ON_MANAGER_START | CLEAR_ON_OFFROAD_TRANSITION | CLEAR_ON_IGNITION_ON, BOOL}}, + {"UsbGpuActive", {CLEAR_ON_MANAGER_START | CLEAR_ON_OFFROAD_TRANSITION | CLEAR_ON_IGNITION_ON, BOOL}}, + {"UsbGpuFailed", {CLEAR_ON_MANAGER_START | CLEAR_ON_OFFROAD_TRANSITION | CLEAR_ON_IGNITION_ON, BOOL}}, + {"UsbGpuLastError", {CLEAR_ON_MANAGER_START, STRING}}, + {"UsbGpuLatencyMs", {CLEAR_ON_MANAGER_START, FLOAT, "0.0"}}, + {"UsbGpuStatus", {CLEAR_ON_MANAGER_START, STRING}}, + {"UsbGpuSetupProgress", {CLEAR_ON_MANAGER_START, STRING, "1.0"}}, + // Neural Network Feed Forward {"NeuralNetworkFeedForward", {PERSISTENT, BOOL, "0"}}, diff --git a/iqpilot/selfdrive/car/card.py b/iqpilot/selfdrive/car/card.py index 96f12c692..786be2ffb 100755 --- a/iqpilot/selfdrive/car/card.py +++ b/iqpilot/selfdrive/car/card.py @@ -253,6 +253,9 @@ class Car: # TODO: mirror the carState.cruiseState struct? CS.vCruise = float(self.v_cruise_helper.v_cruise_kph) CS.vCruiseCluster = float(self.v_cruise_helper.v_cruise_cluster_kph) + CS_IQ.slcSetSpeedRequestId = self.v_cruise_helper.slc_set_speed_request_id + CS_IQ.slcSetSpeedGestureId = self.v_cruise_helper.slc_set_speed_gesture_id + CS_IQ.slcSetSpeedRequestKph = self.v_cruise_helper.slc_set_speed_request_kph return CS, CS_IQ, RD diff --git a/iqpilot/selfdrive/car/cruise.py b/iqpilot/selfdrive/car/cruise.py index f54864f75..79881caee 100644 --- a/iqpilot/selfdrive/car/cruise.py +++ b/iqpilot/selfdrive/car/cruise.py @@ -6,6 +6,7 @@ from iqpilot.common.constants import CV from iqpilot.cereal import car, custom from iqdbc.car import structs from iqpilot.common.params import Params +from iqpilot.common.realtime import DT_CTRL from iqpilot.selfdrive.car.long_increments import LongIncrementConfig, read_long_increment_config, resolve_button_step @@ -237,6 +238,35 @@ class VCruiseHelper(VCruiseHelperIQ): self.v_cruise_kph_last = 0 self.button_timers = {ButtonType.decelCruise: 0, ButtonType.accelCruise: 0} self.button_change_states = {btn: {"standstill": False, "enabled": False} for btn in self.button_timers} + self.slc_set_speed_request_id = 0 + self.slc_set_speed_gesture_id = 0 + self.slc_set_speed_request_kph = 0.0 + self._slc_accel_held = False + self._slc_accel_release_frames = 0 + self._slc_pcm_speed_last = None + + def _update_slc_accel_gesture(self, CS): + self._slc_accel_release_frames = max(0, self._slc_accel_release_frames - 1) + for button in CS.buttonEvents: + if button.type in (ButtonType.accelCruise, ButtonType.resumeCruise): + if button.pressed: + self.slc_set_speed_gesture_id = (self.slc_set_speed_gesture_id + 1) % (1 << 32) + self._slc_accel_release_frames = 0 + else: + self._slc_accel_release_frames = int(0.5 / DT_CTRL) + self._slc_accel_held = button.pressed + elif button.pressed: + self._slc_accel_held = False + self._slc_accel_release_frames = 0 + if not CS.cruiseState.available: + self._slc_accel_held = False + self._slc_accel_release_frames = 0 + + def _record_slc_set_speed_increase(self, previous_kph, enabled): + if enabled and (self._slc_accel_held or self._slc_accel_release_frames > 0) and \ + previous_kph is not None and 0 < previous_kph < self.v_cruise_kph <= V_CRUISE_MAX: + self.slc_set_speed_request_id = (self.slc_set_speed_request_id + 1) % (1 << 32) + self.slc_set_speed_request_kph = float(self.v_cruise_kph) @property def v_cruise_initialized(self): @@ -248,6 +278,7 @@ class VCruiseHelper(VCruiseHelperIQ): def update_v_cruise(self, CS, enabled, is_metric): self.v_cruise_kph_last = self.v_cruise_kph + self._update_slc_accel_gesture(CS) self.get_minimum_set_speed(is_metric) @@ -256,12 +287,15 @@ class VCruiseHelper(VCruiseHelperIQ): if not self.CP.pcmCruise or (not self.CP_IQ.pcmCruiseSpeed and _enabled): # if stock cruise is completely disabled, then we can use our own set speed logic self._update_v_cruise_non_pcm(CS, _enabled, is_metric, self.volkswagen_standby_set_speed) + self._record_slc_set_speed_increase(self.v_cruise_kph_last, _enabled) self.update_speed_limit_assist_v_cruise_non_pcm() self.v_cruise_cluster_kph = self.v_cruise_kph self.update_button_timers(CS, enabled) else: self.v_cruise_kph = CS.cruiseState.speed * CV.MS_TO_KPH self.v_cruise_cluster_kph = CS.cruiseState.speedCluster * CV.MS_TO_KPH + self._record_slc_set_speed_increase(self._slc_pcm_speed_last, _enabled) + self._slc_pcm_speed_last = self.v_cruise_kph if CS.cruiseState.speed == 0: self.v_cruise_kph = V_CRUISE_UNSET self.v_cruise_cluster_kph = V_CRUISE_UNSET @@ -273,6 +307,7 @@ class VCruiseHelper(VCruiseHelperIQ): else: self.v_cruise_kph = V_CRUISE_UNSET self.v_cruise_cluster_kph = V_CRUISE_UNSET + self._slc_pcm_speed_last = None def _update_v_cruise_non_pcm(self, CS, enabled, is_metric, allow_standby_adjustment=False): # handle button presses. TODO: this should be in state_control, but a decelCruise press diff --git a/iqpilot/selfdrive/car/tests/test_speed_limit_set_speed.py b/iqpilot/selfdrive/car/tests/test_speed_limit_set_speed.py index a872cbae1..ea1a7d3c3 100644 --- a/iqpilot/selfdrive/car/tests/test_speed_limit_set_speed.py +++ b/iqpilot/selfdrive/car/tests/test_speed_limit_set_speed.py @@ -73,6 +73,65 @@ class TestSpeedLimitSetSpeedMirror: assert self.v_cruise_helper.v_cruise_cluster_kph == pytest.approx(13.41 * CV.MS_TO_KPH, abs=0.1) +@pytest.mark.parametrize("pcm_cruise", [False, True]) +@pytest.mark.parametrize("is_metric", [False, True]) +def test_driver_increase_marker_survives_button_release(pcm_cruise, is_metric): + helper = VCruiseHelper(car.CarParams(pcmCruise=pcm_cruise), custom.IQCarParams(pcmCruiseSpeed=True)) + helper.set_speed_to_limit = False + helper.v_cruise_kph = helper.v_cruise_cluster_kph = 80.0 + state = car.CarState(cruiseState={"available": True, "speed": 80 * CV.KPH_TO_MS, "speedCluster": 80 * CV.KPH_TO_MS}) + helper.update_v_cruise(state, True, is_metric) + state.buttonEvents = [car.CarState.ButtonEvent(type="accelCruise", pressed=True)] + helper.update_v_cruise(state, True, is_metric) + assert helper.slc_set_speed_request_id == 0 + state.buttonEvents = [car.CarState.ButtonEvent(type="accelCruise", pressed=False)] + if pcm_cruise: + state.cruiseState.speed = state.cruiseState.speedCluster = 81 * CV.KPH_TO_MS + helper.update_v_cruise(state, True, is_metric) + request = custom.IQCarState.new_message( + slcSetSpeedRequestId=helper.slc_set_speed_request_id, + slcSetSpeedGestureId=helper.slc_set_speed_gesture_id, + slcSetSpeedRequestKph=helper.slc_set_speed_request_kph, + ) + assert request.slcSetSpeedRequestId == 1 + assert request.slcSetSpeedRequestKph == pytest.approx(helper.v_cruise_kph) + state.buttonEvents = [] + for _ in range(100): + helper.update_v_cruise(state, True, is_metric) + assert helper.slc_set_speed_request_id == request.slcSetSpeedRequestId + assert helper.slc_set_speed_gesture_id == request.slcSetSpeedGestureId + + +def test_stock_cruise_sync_is_not_a_driver_increase(): + helper = VCruiseHelper(car.CarParams(pcmCruise=True, openpilotLongitudinalControl=True), + custom.IQCarParams(pcmCruiseSpeed=True)) + helper.set_speed_to_limit = True + helper.update_speed_limit_assist(True, TestSpeedLimitSetSpeedMirror._iq_plan( + 50 * CV.KPH_TO_MS, custom.IQPlan.SpeedLimit.AssistState.active)) + state = car.CarState(cruiseState={"available": True, "speed": 80 * CV.KPH_TO_MS, "speedCluster": 80 * CV.KPH_TO_MS}) + for _ in range(10): + helper.update_v_cruise(state, True, True) + assert helper.slc_set_speed_request_id == 0 + state.cruiseState.speed = state.cruiseState.speedCluster = 90 * CV.KPH_TO_MS + helper.update_v_cruise(state, True, True) + assert helper.slc_set_speed_request_id == 0 + + +def test_held_increase_uses_one_gesture_and_multiple_requests(): + helper = VCruiseHelper(car.CarParams(pcmCruise=False), custom.IQCarParams(pcmCruiseSpeed=True)) + helper.set_speed_to_limit = False + helper.v_cruise_kph = 80.0 + state = car.CarState(cruiseState={"available": True}) + state.buttonEvents = [car.CarState.ButtonEvent(type="accelCruise", pressed=True)] + helper.update_v_cruise(state, True, True) + state.buttonEvents = [] + for _ in range(110): + helper.update_v_cruise(state, True, True) + assert helper.slc_set_speed_gesture_id == 1 + assert helper.slc_set_speed_request_id >= 2 + assert helper.slc_set_speed_request_kph == pytest.approx(helper.v_cruise_kph) + + def test_set_speed_does_not_follow_limit_when_feature_off(): # Default off: set speed must stay the driver's value (limiter-only via planner min-blend). CP = car.CarParams(pcmCruise=True, openpilotLongitudinalControl=True) diff --git a/iqpilot/selfdrive/controls/controlsd.py b/iqpilot/selfdrive/controls/controlsd.py index b067ecfdf..9a6f43ff4 100755 --- a/iqpilot/selfdrive/controls/controlsd.py +++ b/iqpilot/selfdrive/controls/controlsd.py @@ -24,6 +24,7 @@ from iqpilot.selfdrive.controls.lib.latcontrol_torque import LatControlTorque from iqpilot.selfdrive.controls.lib.latcontrol_torque_pq import LatControlTorquePQ from iqpilot.selfdrive.controls.lib.latcontrol_torque_v0 import LatControlTorqueV0, is_vw_mqb_torque from iqpilot.selfdrive.controls.lib.longcontrol import LongControl +from iqpilot.selfdrive.controls.steering_fault_recovery import SteeringFaultRecovery from iqpilot.system.proprietary_runtime._verified_import import import_verified_module from iqpilot.selfdrive.locationd.helpers import PoseCalibrator, Pose @@ -73,6 +74,7 @@ class Controls(IQControlsLayer): self.pm = messaging.PubMaster(['carControl', 'controlsState', 'iqPerfTrace'] + self.iq_pub_services) self.steer_limited_by_safety = False + self.steering_fault_recovery = SteeringFaultRecovery() self.curvature = 0.0 self.desired_curvature = 0.0 self.roll_compensation = 0.0 @@ -208,7 +210,8 @@ class Controls(IQControlsLayer): # Get which state to use for active lateral control _lat_active = self.iq_lateral_allowed(self.sm) - CC.latActive = _lat_active and not CS.steerFaultTemporary and not CS.steerFaultPermanent and \ + steering_fault_recovered = self.steering_fault_recovery.update(CS.steerFaultTemporary, CS.steerFaultPermanent) + CC.latActive = _lat_active and steering_fault_recovered and \ (not standstill or self.CP.steerAtStandstill) # long control may stay active through a gas override on platforms that opt in override_longitudinal = any(e.overrideLongitudinal for e in self.sm['onroadEvents']) diff --git a/iqpilot/selfdrive/controls/lib/slc_vcruise.py b/iqpilot/selfdrive/controls/lib/slc_vcruise.py index 0af9facff..7482f6237 100644 --- a/iqpilot/selfdrive/controls/lib/slc_vcruise.py +++ b/iqpilot/selfdrive/controls/lib/slc_vcruise.py @@ -189,6 +189,8 @@ class SLCVCruise: self._user_max_speed = v_cruise_cluster else: self._user_max_speed = 0.0 + if not slc_params["speed_limit_controller"]: + self.slc.reset_override(sm) if slc_params["speed_limit_controller"]: self.slc.update_limits(dashboard_speed_limit, now, time_validated, v_cruise, v_ego, sm, slc_params) self.pending_events = list(getattr(self.slc, 'pending_events', [])) diff --git a/iqpilot/selfdrive/controls/lib/speed_limit_controller.py b/iqpilot/selfdrive/controls/lib/speed_limit_controller.py index 0fd6bdeea..3cdf72ecf 100644 --- a/iqpilot/selfdrive/controls/lib/speed_limit_controller.py +++ b/iqpilot/selfdrive/controls/lib/speed_limit_controller.py @@ -261,10 +261,11 @@ class IQSpeedLimitAssist: for btn in sm["carState"].buttonEvents: if btn.pressed: continue - if is_lower and btn.type in CONFIRM_LOWER_BUTTONS: + button_type = getattr(btn.type, "raw", btn.type) + if is_lower and button_type in CONFIRM_LOWER_BUTTONS: confirmed = True break - elif not is_lower and btn.type in CONFIRM_HIGHER_BUTTONS: + elif not is_lower and button_type in CONFIRM_HIGHER_BUTTONS: confirmed = True break except (AttributeError, TypeError): @@ -314,6 +315,10 @@ class SpeedLimitController: self.override_slc = False self.overridden_speed = 0.0 + self._last_override_request_id = 0 + self._blocked_override_gesture = 0 + self._override_limit = None + self._override_set_speed = False self._resolved_limit = 0.0 self._resolved_source = "None" @@ -807,9 +812,44 @@ class SpeedLimitController: self.pending_events.append(EventNameIQ.constructionZoneDetected) self._czone_was_limiting = czone_limiting + def reset_override(self, sm): + self.override_slc = False + self.overridden_speed = 0.0 + self._last_override_request_id = int(getattr(sm["iqCarState"], "slcSetSpeedRequestId", 0)) + self._blocked_override_gesture = int(getattr(sm["iqCarState"], "slcSetSpeedGestureId", 0)) + self._override_limit = None + def update_override(self, v_cruise, v_cruise_diff, v_ego, v_ego_diff, sm, slc_params, is_metric): offset = self.get_offset(is_metric) target = self._assist.target + set_speed_override = slc_params.get("speed_limit_controller_override_set_speed", False) + mode_changed = set_speed_override != self._override_set_speed + self._override_set_speed = set_speed_override + + if set_speed_override: + request_id = int(getattr(sm["iqCarState"], "slcSetSpeedRequestId", 0)) + gesture_id = int(getattr(sm["iqCarState"], "slcSetSpeedGestureId", 0)) + request_speed = float(getattr(sm["iqCarState"], "slcSetSpeedRequestKph", 0.0)) * CV.KPH_TO_MS + new_request = request_id != self._last_override_request_id + limit = (target, self._assist.source) + reset = (mode_changed or limit != self._override_limit or self._assist.just_confirmed or + self._assist.state == SpeedLimitAssistState.preActive or + not bool(getattr(sm["selfdriveState"], "enabled", False)) or target <= 0 or self._resolved_source == "Construction") + cruise_speed = v_cruise + v_cruise_diff + above_limit = cruise_speed > target + offset + 1e-3 + if reset or (self.override_slc and not above_limit): + self.reset_override(sm) + elif above_limit: + driver_increase = new_request and gesture_id != self._blocked_override_gesture and request_speed > target + offset + 1e-3 + gas_override = sm["carState"].gasPressed and v_ego > target + offset + self.override_slc = self.override_slc or driver_increase or gas_override + self.overridden_speed = cruise_speed if self.override_slc else 0.0 + self._last_override_request_id = request_id + self._override_limit = limit + return + + if mode_changed: + self.reset_override(sm) self.override_slc = self.overridden_speed > target + offset > 0 self.override_slc |= sm["carState"].gasPressed and v_ego > target + offset > 0 @@ -820,7 +860,5 @@ class SpeedLimitController: if sm["carState"].gasPressed: self.overridden_speed = max(v_ego + v_ego_diff, self.overridden_speed) self.overridden_speed = float(np.clip(self.overridden_speed, target + offset, v_cruise + v_cruise_diff)) - elif slc_params.get("speed_limit_controller_override_set_speed", False): - self.overridden_speed = v_cruise + v_cruise_diff else: self.overridden_speed = 0.0 diff --git a/iqpilot/selfdrive/controls/lib/tests/test_slc_speed_limit_control.py b/iqpilot/selfdrive/controls/lib/tests/test_slc_speed_limit_control.py index 2fd7a2b63..2e2acf9c4 100644 --- a/iqpilot/selfdrive/controls/lib/tests/test_slc_speed_limit_control.py +++ b/iqpilot/selfdrive/controls/lib/tests/test_slc_speed_limit_control.py @@ -5,8 +5,13 @@ Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed from datetime import datetime from types import SimpleNamespace +import pytest + +from iqpilot.cereal import car, custom from iqpilot.common.constants import CV from iqpilot.common.slc_variables import OFFSET_MAP_IMPERIAL +from iqpilot.selfdrive.car.cruise import VCruiseHelper +from iqpilot.selfdrive.controls.lib.iq_longitudinal_planner import LongitudinalPlannerIQ from iqpilot.selfdrive.controls.lib.slc_vcruise import SLCVCruise, CRUISING_SPEED from iqpilot.selfdrive.controls.lib.speed_limit_controller import SpeedLimitController, POLICY_MAP_DATA_PRIORITY, POLICY_COMBINED @@ -61,6 +66,9 @@ class _FakeSLC: def update_override(self, *_args, **_kwargs): self.update_override_calls += 1 + def reset_override(self, _sm): + self.overridden_speed = 0.0 + def get_offset(self, _is_metric): return self._offset @@ -261,6 +269,200 @@ def test_slc_vcruise_does_not_auto_raise_when_higher_confirmation_enabled(): assert out == v_cruise +@pytest.fixture(params=[True, False], ids=["metric", "imperial"]) +def set_speed_slc(request, monkeypatch): + monkeypatch.setattr("iqpilot.selfdrive.controls.lib.slc_vcruise.Params", FakeParams) + slc = SLCVCruise() + slc._maybe_log_debug = lambda *_args: None + slc.slc.update_gps = lambda _sm: None + slc.slc._resolver.update_map_data = lambda *_args: None + params = _base_slc_params_controller() | { + "speed_limit_controller": True, + "speed_limit_mode": 3, + "show_speed_limits": False, + "is_metric": request.param, + "slc_online_filler": False, + "slc_fallback_experimental_mode": False, + "speed_limit_controller_override_manual": False, + "speed_limit_controller_override_set_speed": True, + } + slc._get_slc_params = lambda: params + unit = CV.KPH_TO_MS if request.param else CV.MPH_TO_MS + slc.slc._resolver.map_speed_limit = 50 * unit + sm = _FakeSM(_build_sm(v_ego_cluster=50 * unit)) + sm["iqCarState"].slcSetSpeedRequestId = 0 + sm["iqCarState"].slcSetSpeedGestureId = 0 + sm["iqCarState"].slcSetSpeedRequestKph = 0.0 + + def step(speed, increase=False, new_gesture=False): + sm["carState"].vCruiseCluster = speed * unit * CV.MS_TO_KPH + if new_gesture: + sm["iqCarState"].slcSetSpeedGestureId += 1 + if increase: + sm["iqCarState"].slcSetSpeedRequestId += 1 + sm["iqCarState"].slcSetSpeedRequestKph = sm["carState"].vCruiseCluster + target = slc.update(sm["selfdriveState"].enabled, None, True, speed * unit, 50 * unit, sm) + return min(speed * unit, target) / unit + + step(50) + return SimpleNamespace(slc=slc, params=params, sm=sm, unit=unit, step=step) + + +@pytest.mark.parametrize("confirm_higher", [False, True]) +def test_set_speed_override_tracks_driver_adjustments(set_speed_slc, confirm_higher): + system = set_speed_slc + system.params["speed_limit_confirmation_higher"] = confirm_higher + assert system.step(50, new_gesture=True) == pytest.approx(50) + assert system.step(55, increase=True) == pytest.approx(55) + assert system.step(60, increase=True) == pytest.approx(60) + assert system.step(60) == pytest.approx(60) + assert system.step(55) == pytest.approx(55) + assert system.step(50) == pytest.approx(50) + assert not system.slc.slc.override_slc + assert system.step(45) == pytest.approx(45) + assert system.step(60) == pytest.approx(50) + assert system.step(61, increase=True, new_gesture=True) == pytest.approx(61) + + +def test_set_speed_override_ignores_automatic_speed_changes(set_speed_slc): + system = set_speed_slc + assert system.step(80) == pytest.approx(50) + system.slc.slc._resolver.map_speed_limit = 60 * system.unit + assert system.step(60) == pytest.approx(60) + assert system.step(80) == pytest.approx(60) + assert system.slc.slc.overridden_speed == 0 + + +@pytest.mark.parametrize("limit", [40, 55]) +def test_set_speed_override_resets_on_accepted_limit(set_speed_slc, limit): + system = set_speed_slc + assert system.step(60, increase=True, new_gesture=True) == pytest.approx(60) + system.slc.slc._resolver.map_speed_limit = limit * system.unit + assert system.step(65, increase=True) == pytest.approx(limit) + assert system.step(70, increase=True) == pytest.approx(limit) + assert system.step(71, increase=True, new_gesture=True) == pytest.approx(71) + + +@pytest.mark.parametrize("limit,button", [(40, "decelCruise"), (55, "accelCruise")]) +def test_set_speed_override_does_not_reuse_confirmation_gesture(set_speed_slc, limit, button): + system = set_speed_slc + system.params["speed_limit_confirmation_higher"] = True + system.params["speed_limit_confirmation_lower"] = True + system.slc.slc._resolver.map_speed_limit = limit * system.unit + system.step(50, new_gesture=True) + assert system.slc.assist_state == custom.IQPlan.SpeedLimit.AssistState.preActive + assert system.step(60, increase=True) == pytest.approx(50) + system.sm["carState"].buttonEvents = [car.CarState.ButtonEvent(type=button, pressed=False)] + assert system.step(61, increase=True) == pytest.approx(limit) + system.sm["carState"].buttonEvents = [] + assert system.step(65, increase=True) == pytest.approx(limit) + assert system.step(66, increase=True, new_gesture=True) == pytest.approx(66) + + +@pytest.mark.parametrize("reset", ["disengage", "information", "off", "missing_limit"]) +def test_set_speed_override_cannot_survive_reset(set_speed_slc, reset): + system = set_speed_slc + assert system.step(60, increase=True, new_gesture=True) == pytest.approx(60) + if reset == "disengage": + system.sm["selfdriveState"].enabled = False + elif reset in ("information", "off"): + system.params["speed_limit_controller"] = False + system.params["show_speed_limits"] = reset == "information" + else: + system.slc.slc._resolver.map_speed_limit = 0 + system.step(60) + assert system.slc.slc.overridden_speed == 0 + system.sm["selfdriveState"].enabled = True + system.params["speed_limit_controller"] = True + system.slc.slc._resolver.map_speed_limit = 50 * system.unit + assert system.step(60) == pytest.approx(50) + assert system.step(65, increase=True) == pytest.approx(50) + assert system.step(66, increase=True, new_gesture=True) == pytest.approx(66) + + +def test_set_speed_override_respects_offset(set_speed_slc): + system = set_speed_slc + system.slc.slc.params.put("speed_limit_offset1", 10) + system.slc.slc.params.put("speed_limit_offset2", 10) + system.slc.slc.params.put("speed_limit_offset3", 10) + system.slc.slc._offset_cache.clear() + system.step(50, new_gesture=True) + assert system.step(52, increase=True) == pytest.approx(52) + assert not system.slc.slc.override_slc + assert system.step(56, increase=True) == pytest.approx(56) + assert system.step(55) == pytest.approx(55) + assert not system.slc.slc.override_slc + + +def test_manual_override_still_requires_accelerator(set_speed_slc): + system = set_speed_slc + system.params["speed_limit_controller_override_set_speed"] = False + system.params["speed_limit_controller_override_manual"] = True + assert system.step(60, increase=True, new_gesture=True) == pytest.approx(50) + system.sm["carState"].gasPressed = True + system.sm["carState"].vEgoCluster = 55 * system.unit + system.slc.slc.update_override(60 * system.unit, 0, 55 * system.unit, 0, system.sm, system.params, system.params["is_metric"]) + assert system.slc.slc.overridden_speed == pytest.approx(55 * system.unit) + system.sm["carState"].gasPressed = False + system.slc.slc.update_override(60 * system.unit, 0, 55 * system.unit, 0, system.sm, system.params, system.params["is_metric"]) + assert system.slc.slc.overridden_speed == pytest.approx(55 * system.unit) + + +@pytest.mark.parametrize("pcm_cruise", [False, True]) +def test_driver_increase_reaches_slc_without_transient_button_events(set_speed_slc, pcm_cruise): + system = set_speed_slc + helper = VCruiseHelper(car.CarParams(pcmCruise=pcm_cruise), custom.IQCarParams(pcmCruiseSpeed=True)) + helper.set_speed_to_limit = False + helper.v_cruise_kph = helper.v_cruise_cluster_kph = 50 * system.unit * CV.MS_TO_KPH + state = car.CarState(cruiseState={"available": True, "speed": 50 * system.unit, "speedCluster": 50 * system.unit}) + helper.update_v_cruise(state, True, system.params["is_metric"]) + state.buttonEvents = [car.CarState.ButtonEvent(type="accelCruise", pressed=True)] + helper.update_v_cruise(state, True, system.params["is_metric"]) + state.buttonEvents = [car.CarState.ButtonEvent(type="accelCruise", pressed=False)] + if pcm_cruise: + state.cruiseState.speed = state.cruiseState.speedCluster = 51 * system.unit + helper.update_v_cruise(state, True, system.params["is_metric"]) + state.buttonEvents = [] + for _ in range(5): + helper.update_v_cruise(state, True, system.params["is_metric"]) + system.sm["iqCarState"] = custom.IQCarState.new_message( + slcSetSpeedRequestId=helper.slc_set_speed_request_id, + slcSetSpeedGestureId=helper.slc_set_speed_gesture_id, + slcSetSpeedRequestKph=helper.slc_set_speed_request_kph, + ) + requested = helper.v_cruise_kph * CV.KPH_TO_MS / system.unit + assert requested > 50 + assert system.step(requested) == pytest.approx(requested) + + +def test_set_speed_override_keeps_navigation_constraint(set_speed_slc): + system = set_speed_slc + assert system.step(60, increase=True, new_gesture=True) == pytest.approx(60) + planner = LongitudinalPlannerIQ.__new__(LongitudinalPlannerIQ) + planner.slimit = system.slc + planner.iq_dynamic = SimpleNamespace( + set_slc_experimental_mode=lambda _mode: None, update=lambda _sm: None, force_stop_requested=lambda: False) + planner.force_stop_timer = 0.0 + planner.override_force_stop_timer = 0.0 + planner.override_force_stop = False + system.sm["iqNavState"] = SimpleNamespace(longitudinalEngaged=False, valid=False) + assert planner.update_targets(system.sm, 50 * system.unit, 60 * system.unit) == pytest.approx(60 * system.unit) + system.sm["iqNavState"] = SimpleNamespace(longitudinalEngaged=True, valid=True, speedTarget=40 * system.unit) + assert planner.update_targets(system.sm, 50 * system.unit, 60 * system.unit) == pytest.approx(40 * system.unit) + + +def test_set_speed_override_cannot_bypass_construction_zone(set_speed_slc): + system = set_speed_slc + assert system.step(60, increase=True, new_gesture=True) == pytest.approx(60) + system.params["construction_zone_assist"] = True + system.params["construction_zone_speed"] = 40 + system.sm["iqConstructionZone"] = SimpleNamespace(active=True) + system.sm.alive["iqConstructionZone"] = True + assert system.step(60) == pytest.approx(40) + assert system.step(65, increase=True, new_gesture=True) == pytest.approx(40) + assert not system.slc.slc.override_slc + + class _FakeSM(dict): def __init__(self, services, alive=None): super().__init__(services) diff --git a/iqpilot/selfdrive/controls/steering_fault_recovery.py b/iqpilot/selfdrive/controls/steering_fault_recovery.py new file mode 100644 index 000000000..191161e3e --- /dev/null +++ b/iqpilot/selfdrive/controls/steering_fault_recovery.py @@ -0,0 +1,18 @@ +""" +Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos +""" +from iqpilot.common.realtime import DT_CTRL + +STEER_FAULT_RECOVERY_FRAMES = int(1.0 / DT_CTRL) + + +class SteeringFaultRecovery: + def __init__(self) -> None: + self.clear_frames = STEER_FAULT_RECOVERY_FRAMES + + def update(self, temporary: bool, permanent: bool) -> bool: + if temporary or permanent: + self.clear_frames = 0 + else: + self.clear_frames = min(self.clear_frames + 1, STEER_FAULT_RECOVERY_FRAMES) + return self.clear_frames == STEER_FAULT_RECOVERY_FRAMES diff --git a/iqpilot/selfdrive/controls/tests/test_steering_fault_recovery.py b/iqpilot/selfdrive/controls/tests/test_steering_fault_recovery.py new file mode 100644 index 000000000..d6cf76ce3 --- /dev/null +++ b/iqpilot/selfdrive/controls/tests/test_steering_fault_recovery.py @@ -0,0 +1,28 @@ +""" +Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos +""" +from iqpilot.selfdrive.controls.steering_fault_recovery import STEER_FAULT_RECOVERY_FRAMES, SteeringFaultRecovery + + +def test_steering_fault_recovery_starts_ready(): + recovery = SteeringFaultRecovery() + assert recovery.update(False, False) + + +def test_temporary_fault_requires_continuous_clear_interval(): + recovery = SteeringFaultRecovery() + assert not recovery.update(True, False) + for _ in range(STEER_FAULT_RECOVERY_FRAMES - 1): + assert not recovery.update(False, False) + assert recovery.update(False, False) + + +def test_repeated_fault_restarts_recovery_interval(): + recovery = SteeringFaultRecovery() + assert not recovery.update(False, True) + for _ in range(STEER_FAULT_RECOVERY_FRAMES - 1): + assert not recovery.update(False, False) + assert not recovery.update(True, False) + for _ in range(STEER_FAULT_RECOVERY_FRAMES - 1): + assert not recovery.update(False, False) + assert recovery.update(False, False) diff --git a/iqpilot/selfdrive/dmonitoringmodeld/dmonitoringmodeld.py b/iqpilot/selfdrive/dmonitoringmodeld/dmonitoringmodeld.py index d17aa62ca..bcaa13712 100755 --- a/iqpilot/selfdrive/dmonitoringmodeld/dmonitoringmodeld.py +++ b/iqpilot/selfdrive/dmonitoringmodeld/dmonitoringmodeld.py @@ -109,6 +109,13 @@ def get_driverstate_packet(model_output, frame_id: int, location_ts: int, exec_t def main(): config_realtime_process(7, 5) + # Set in the child, not at import: manager preimports every process module in the parent, + # so an import-time write lands in one shared env that all children inherit (and setdefault + # in a child is then a guaranteed no-op). tinygrad reads this lazily at QCOMDevice init. + # KGSL: lower value = higher priority. DM has no 50ms deadline; at the driving contexts' + # default 8 its kernels interleave with the warp and blow its submit tail 16ms -> 72ms p90. + os.environ['QCOM_PRIORITY'] = os.getenv('DMON_QCOM_PRIORITY', '12') + cl_context = CLContext() model = ModelState(cl_context) cloudlog.warning("models loaded, dmonitoringmodeld starting") diff --git a/iqpilot/selfdrive/iqmodeld/big_catalog.py b/iqpilot/selfdrive/iqmodeld/big_catalog.py new file mode 100644 index 000000000..972671971 --- /dev/null +++ b/iqpilot/selfdrive/iqmodeld/big_catalog.py @@ -0,0 +1,9 @@ +""" +Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos/ +""" +from iqpilot._proprietary_loader import ProprietaryModuleMissing, load_private_module + +try: + load_private_module(__name__, "iqpilot_private.models.big_catalog") +except ProprietaryModuleMissing: + from iqpilot.models_private_src.big_catalog import * diff --git a/iqpilot/selfdrive/iqmodeld/daemon.py b/iqpilot/selfdrive/iqmodeld/daemon.py index bf3e0001f..2f1edd563 100755 --- a/iqpilot/selfdrive/iqmodeld/daemon.py +++ b/iqpilot/selfdrive/iqmodeld/daemon.py @@ -1,6 +1,7 @@ #!/usr/bin/env python3 from __future__ import annotations +import sys import time from dataclasses import dataclass from typing import Any @@ -471,7 +472,7 @@ class FrameDropMeter: class InferenceDaemon: - def __init__(self, demo: bool = False): + def __init__(self, demo: bool = False, channel_path: str | None = None): cloudlog.warning("iqmodeld init") sentry.set_tag("daemon", PROCESS_NAME) cloudlog.bind(daemon=PROCESS_NAME) @@ -485,8 +486,15 @@ class InferenceDaemon: self._meta_layout = select_meta_layout() cloudlog.warning("models loaded, iqmodeld starting") + self._channel = None + if channel_path is not None: + from iqpilot.selfdrive.iqmodeld.model_channel import ModelChannel + self._channel = ModelChannel(channel_path, create=True) + self._cameras = CameraIngress(self._gpu) - self._pub = PubMaster(["modelV2", "drivingModelData", "cameraOdometry", "iqDriveModelData", "iqPerfTrace"]) + pub_services = ["iqPerfTrace"] if self._channel is not None else [ + "modelV2", "drivingModelData", "cameraOdometry", "iqDriveModelData", "iqPerfTrace"] + self._pub = PubMaster(pub_services) self._sub = SubMaster([ "deviceState", "carState", "roadCameraState", "extrinsicsCalibration", "driverMonitoringState", "carControl", "lateralDelay", "iqNavState", "radarState", @@ -513,6 +521,12 @@ class InferenceDaemon: def _refresh_tunables(self, tick: int) -> None: if tick % 60 != 0: return + from iqpilot.selfdrive.iqmodeld.egpu_helpers import egpu_selected + big_enabled = self._params.get_bool("IQEmacEnabled") or egpu_selected(self._params) + if big_enabled != (self._channel is not None): + # publish mode is fixed at startup: staying up would fight the selector for modelV2 + cloudlog.warning("iqmodeld: big backend toggled, restarting to switch publish mode") + sys.exit(0) self._runtime.lat_delay = lateral_action_delay(self._params, self._car_params, self._sub["lateralDelay"].lateralDelay) self._runtime.PLANPLUS_CONTROL = self._params.get("PlanplusControl", return_default=True) self._runtime.model_smoothing_max_extra_sec = _model_lat_smooth_max_sec(self._params) @@ -600,6 +614,22 @@ class InferenceDaemon: live_calib_seen, ) + if self._channel is not None: + self._channel.write(main_stamp.frame_id, { + "source": "small", + "frame_id": main_stamp.frame_id, + "timestamp_sof": int(main_stamp.timestamp_sof), + "live_calib_seen": bool(live_calib_seen), + "model_execution_time": float(execution_time), + "msgs": { + "modelV2": model_msg.to_bytes(), + "drivingModelData": driving_msg.to_bytes(), + "cameraOdometry": pose_msg.to_bytes(), + "iqDriveModelData": iq_msg.to_bytes(), + }, + }) + return + self._pub.send("modelV2", model_msg) self._pub.send("drivingModelData", driving_msg) self._pub.send("cameraOdometry", pose_msg) @@ -695,8 +725,15 @@ class InferenceDaemon: tick += 1 -def main(demo: bool = False): - InferenceDaemon(demo=demo).serve() +def main(demo: bool = False, channel_path: str | None = "auto"): + if channel_path == "auto": + channel_path = None + params = Params() + from iqpilot.selfdrive.iqmodeld.egpu_helpers import egpu_selected + if params.get_bool("IQEmacEnabled") or egpu_selected(params): + from iqpilot.selfdrive.iqmodeld.model_channel import SMALL_CHANNEL + channel_path = SMALL_CHANNEL + InferenceDaemon(demo=demo, channel_path=channel_path).serve() __all__ = [ diff --git a/iqpilot/selfdrive/iqmodeld/driving_action.py b/iqpilot/selfdrive/iqmodeld/driving_action.py new file mode 100644 index 000000000..3e5f9750c --- /dev/null +++ b/iqpilot/selfdrive/iqmodeld/driving_action.py @@ -0,0 +1,46 @@ +""" +Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos/ +""" +from __future__ import annotations + +import numpy as np + +from iqpilot.cereal import log + +from iqpilot.selfdrive.controls.lib.drive_helpers import smooth_value + +LAT_SMOOTH_SECONDS = 0.0 +LONG_SMOOTH_SECONDS = 0.3 +MIN_LAT_CONTROL_SPEED = 0.3 +DESIRE_LEN = 8 + + +def get_action_from_model(outputs: dict[str, np.ndarray], prev_action: log.ModelDataV2.Action, + v_ego: float, lat_action_t: float, long_action_t: float, + lat_smooth_seconds: float | None = None) -> log.ModelDataV2.Action: + if "action" in outputs: + desired_accel = float(outputs["action"][0, 1]) + desired_curvature = float(outputs["action"][0, 0]) / (max(1.0, v_ego)) ** 2 + should_stop = bool(v_ego < 0.3 and desired_accel < 0.1) + else: + from iqpilot.selfdrive.controls.lib.drive_helpers import get_accel_from_plan, get_curvature_from_plan + from iqpilot.selfdrive.iqmodeld.config import ModelConstants, Plan + plan = outputs["plan"][0] + desired_accel, should_stop = get_accel_from_plan(plan[:, Plan.VELOCITY][:, 0], + plan[:, Plan.ACCELERATION][:, 0], + ModelConstants.T_IDXS, + action_t=long_action_t) + desired_curvature = get_curvature_from_plan(plan[:, Plan.T_FROM_CURRENT_EULER][:, 2], + plan[:, Plan.ORIENTATION_RATE][:, 2], + ModelConstants.T_IDXS, v_ego, lat_action_t) + desired_accel, should_stop = float(desired_accel), bool(should_stop) + desired_curvature = float(desired_curvature) + desired_accel = smooth_value(desired_accel, prev_action.desiredAcceleration, LONG_SMOOTH_SECONDS) + if v_ego > MIN_LAT_CONTROL_SPEED: + lat_smooth = LAT_SMOOTH_SECONDS if lat_smooth_seconds is None else lat_smooth_seconds + desired_curvature = smooth_value(desired_curvature, prev_action.desiredCurvature, lat_smooth) + else: + desired_curvature = prev_action.desiredCurvature + return log.ModelDataV2.Action(desiredCurvature=float(desired_curvature), + desiredAcceleration=float(desired_accel), + shouldStop=should_stop) diff --git a/iqpilot/selfdrive/iqmodeld/egpu_helpers.py b/iqpilot/selfdrive/iqmodeld/egpu_helpers.py new file mode 100644 index 000000000..d09895c86 --- /dev/null +++ b/iqpilot/selfdrive/iqmodeld/egpu_helpers.py @@ -0,0 +1,158 @@ +""" +Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos/ +""" +from __future__ import annotations + +import hashlib +import json +import os +import urllib.request +from pathlib import Path + +from iqpilot.system.hardware.usb import egpu_dock_ready + +USB_SYSFS_ROOT = "/sys/bus/usb/devices" + +COMMA_LFS_BATCH_URL = "https://gitlab.com/commaai/openpilot-lfs.git/info/lfs/objects/batch" + +DOWNLOAD_CHUNK = 4 * 1024 * 1024 + + +def usbgpu_present(sysfs_root: str = USB_SYSFS_ROOT) -> bool: + return egpu_dock_ready(Path(sysfs_root)) + + +def egpu_present_consented(params, sysfs_root: str = USB_SYSFS_ROOT) -> bool: + try: + if params is not None and params.get_bool("IQEgpuDisabled"): + return False + except Exception: + pass + return usbgpu_present(sysfs_root) + + +def egpu_selected(params, sysfs_root: str = USB_SYSFS_ROOT) -> bool: + try: + if params is not None and params.get_bool("IQEgpuDisabled"): + return False + if params is not None and params.get_bool("IQEgpuEnabled"): + return True + except Exception: + pass + return usbgpu_present(sysfs_root) + + +def resolve_backend(emac_enabled: bool, egpu_enabled: bool, egpu_present: bool = False) -> str | None: + if egpu_present: + return "egpu" + if emac_enabled: + return "emac" + if egpu_enabled: + return "egpu" + return None + + +def egpu_pkl_path(meta: dict) -> str: + from iqpilot.system.hardware.hw import Paths + return os.path.join(Paths.model_root(), f"egpu_{meta['key']}_{meta['sha256'][:8]}_amd_tinygrad.pkl") + + +def onnx_cache_path(meta: dict) -> str: + from iqpilot.system.hardware.hw import Paths + return os.path.join(Paths.model_root(), f"{meta['model_name']}_{meta['sha256'][:8]}.onnx") + + +def _sha256_file(path: str) -> str: + digest = hashlib.sha256() + with open(path, "rb") as f: + while chunk := f.read(DOWNLOAD_CHUNK): + digest.update(chunk) + return digest.hexdigest() + + +def quarantine_artifact(path: str, why: str) -> None: + try: + if os.path.isfile(path): + os.replace(path, path + ".unusable") + except OSError: + try: + os.remove(path) + except OSError: + pass + + +def local_onnx(meta: dict) -> str | None: + path = onnx_cache_path(meta) + if not os.path.isfile(path): + return None + size = int(meta.get("download", {}).get("size", 0)) + if size and os.path.getsize(path) != size: + quarantine_artifact(path, "onnx size mismatch") + return None + if _sha256_file(path) != meta["sha256"]: + quarantine_artifact(path, "onnx sha256 mismatch") + return None + return path + + +def resolve_download_url(download_url: str, sha256: str, size: int, timeout: float = 30.0) -> str: + if download_url.startswith("commalfs:"): + oid = download_url.split(":", 1)[1] + body = json.dumps({"operation": "download", "transfers": ["basic"], + "objects": [{"oid": oid, "size": size}]}).encode() + req = urllib.request.Request(COMMA_LFS_BATCH_URL, data=body, headers={ + "Accept": "application/vnd.git-lfs+json", "Content-Type": "application/vnd.git-lfs+json"}) + with urllib.request.urlopen(req, timeout=timeout) as r: + d = json.load(r) + return d["objects"][0]["actions"]["download"]["href"] + return download_url + + +def download_onnx(meta: dict, progress_cb=None) -> str: + from iqpilot.selfdrive.iqmodeld.egpu_model import download_descriptor + download_url, size = download_descriptor(meta) + if not download_url: + raise RuntimeError(f"model {meta['key']} has no download source; stage the onnx at {onnx_cache_path(meta)}") + + url = resolve_download_url(download_url, meta["sha256"], size) + path = onnx_cache_path(meta) + os.makedirs(os.path.dirname(path), exist_ok=True) + tmp = path + ".part" + digest = hashlib.sha256() + got = 0 + with urllib.request.urlopen(url, timeout=60) as r, open(tmp, "wb") as f: + while chunk := r.read(DOWNLOAD_CHUNK): + f.write(chunk) + digest.update(chunk) + got += len(chunk) + if progress_cb is not None and size: + progress_cb(got / size) + if size and got != size: + os.remove(tmp) + raise RuntimeError(f"onnx download truncated: {got}/{size} bytes") + if digest.hexdigest() != meta["sha256"]: + os.remove(tmp) + raise RuntimeError(f"onnx sha256 mismatch for {meta['key']}") + os.replace(tmp, path) + return path + + +def patch_tinygrad_fetch_fw() -> None: + import pathlib + + import zstandard + from tinygrad import helpers + if getattr(helpers.fetch_fw, "_iq_patched", False): + return + _orig = helpers.fetch_fw + + def fetch_fw(path, name, sha256): + p = pathlib.Path(f"/lib/firmware/{path}/{name}.zst") + if p.is_file(): + blob = zstandard.ZstdDecompressor().stream_reader(p.read_bytes()).read() + if hashlib.sha256(blob).hexdigest() == sha256: + return blob + return _orig(path, name, sha256) + + fetch_fw._iq_patched = True + helpers.fetch_fw = fetch_fw diff --git a/iqpilot/selfdrive/iqmodeld/egpu_model.py b/iqpilot/selfdrive/iqmodeld/egpu_model.py new file mode 100644 index 000000000..b9dc2c77e --- /dev/null +++ b/iqpilot/selfdrive/iqmodeld/egpu_model.py @@ -0,0 +1,9 @@ +""" +Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos/ +""" +from iqpilot._proprietary_loader import ProprietaryModuleMissing, load_private_module + +try: + load_private_module(__name__, "iqpilot_private.models.egpu_model") +except ProprietaryModuleMissing: + from iqpilot.models_private_src.egpu_model import * diff --git a/iqpilot/selfdrive/iqmodeld/egpu_pipeline.py b/iqpilot/selfdrive/iqmodeld/egpu_pipeline.py new file mode 100644 index 000000000..e0cd2f20b --- /dev/null +++ b/iqpilot/selfdrive/iqmodeld/egpu_pipeline.py @@ -0,0 +1,47 @@ +""" +Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos/ +""" +from __future__ import annotations + +import numpy as np + +from iqpilot.selfdrive.iqmodeld.temporal_state import MODEL_INPUT_SPEC, TemporalInputState, spec_from_meta + + +class EgpuPipelineError(RuntimeError): + pass + + +class EgpuPipeline: + + def __init__(self, meta: dict, infer_fn): + if meta.get("split"): + raise EgpuPipelineError(f"model {meta['key']} is a split model; eGPU v1 runs fused models only") + self.meta = meta + self.infer_fn = infer_fn + self.state = TemporalInputState(meta["frame_skip"], spec_from_meta(meta) or MODEL_INPUT_SPEC) + self.hidden_slice = meta["output_slices"]["hidden_state"] + self.output_len = int(meta["output_len"]) + + def run(self, warped: np.ndarray, desire_vec: np.ndarray, traffic_convention: np.ndarray, + action_t: np.ndarray) -> np.ndarray: + inputs = self.state.push_and_materialize(warped, desire_vec, traffic_convention, action_t) + out = np.asarray(self.infer_fn(inputs), dtype=np.float32).reshape(-1) + if out.shape[0] != self.output_len: + raise EgpuPipelineError(f"eGPU output length {out.shape[0]} != {self.output_len}") + if not np.isfinite(out).all(): + raise EgpuPipelineError("eGPU output contains non-finite values") + self.state.note_hidden_state(out, self.hidden_slice) + return out + + +def make_big_channel_payload(frame_id: int, live_calib_seen: bool, execution_time: float, + egpu_exec_ms: float, msgs: dict[str, bytes]) -> dict: + return { + "source": "egpu_big", + "frame_id": int(frame_id), + "live_calib_seen": bool(live_calib_seen), + "model_execution_time": float(execution_time), + "egpu_exec_ms": float(egpu_exec_ms), + "msgs": msgs, + } diff --git a/iqpilot/selfdrive/iqmodeld/egpu_telemetry.py b/iqpilot/selfdrive/iqmodeld/egpu_telemetry.py new file mode 100644 index 000000000..13936312f --- /dev/null +++ b/iqpilot/selfdrive/iqmodeld/egpu_telemetry.py @@ -0,0 +1,106 @@ +""" +Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos/ +""" +from __future__ import annotations + +import struct + +from iqpilot.cereal import messaging +from iqpilot.common.swaglog import cloudlog + +METRICS_REFRESH_EVERY = 100 + + +class EgpuDockTelemetry: + + def __init__(self, pm, big: bool): + self.pm = pm + self.big = big + self.valid = True + self.sends = 0 + self.metrics: dict[str, float] = {} + self._power_limit: int | None = None + self._asm_usb = None + + def _device(self): + from tinygrad.device import Device + return Device + + def _open_asm_usb(self): + import usb1 + from iqpilot.system.hardware.usb import EGPU_DOCK_USB_IDS + context = usb1.USBContext() + for vendor_id, product_id in EGPU_DOCK_USB_IDS: + handle = context.openByVendorIDAndProductID(vendor_id, product_id, skip_on_error=True) + if handle is not None: + return handle + context.close() + return None + + def _read_ina(self): + Device = self._device() + if "AMD" in Device._opened_devices and self._asm_usb is None: + try: + raw = Device["AMD"].iface.pci_dev.usb.usb.control_read(0xC0, 5) + return struct.unpack(" int: + if self._power_limit is None: + self._power_limit = smu._send_msg(smu.smu_mod.PPSMC_MSG_GetPptLimit, 0, read_back_arg=True, timeout=100) + return self._power_limit + + def send(self) -> None: + Device = self._device() + msg = messaging.new_message("egpuDockState") + state = msg.egpuDockState + self.sends += 1 + + if self.big and "AMD" in Device._opened_devices and self.sends % METRICS_REFRESH_EVERY == 1: + try: + smu = Device["AMD"].iface.dev_impl.smu + smu._send_msg(smu.smu_mod.PPSMC_MSG_TransferTableSmu2Dram, smu.smu_mod.TABLE_SMU_METRICS, timeout=100) + metrics = smu.read_table(smu.smu_mod.SmuMetricsExternal_t, smu.smu_mod.TABLE_SMU_METRICS).SmuMetrics + self.metrics = {"tempC": metrics.AvgTemperature[smu.smu_mod.TEMP_HOTSPOT], + "memoryTempC": metrics.AvgTemperature[smu.smu_mod.TEMP_MEM], + "powerDrawW": metrics.AverageSocketPower, + "powerLimitW": self.power_limit(smu), + "gpuUsagePercent": metrics.AverageGfxActivity, + "gpuClockMhz": metrics.AverageGfxclkFrequencyPostDs, + "fanSpeedRpm": metrics.AvgFanRpm} + self.valid = True + except Exception: + if self.valid: + cloudlog.exception("egpu dock state read failed") + self.valid = False + self.metrics.clear() + + if self.big: + for k, v in self.metrics.items(): + setattr(state, k, v) + + asm_valid = False + try: + state.supplyVoltage, state.supplyCurrent, state.supplyFault = self._read_ina() + asm_valid = True + except Exception: + pass + if "AMD" in Device._opened_devices: + try: + state.pcieLtssm = Device["AMD"].iface.pci_dev.usb.read(0xB450, 1)[0] + except Exception: + pass + + msg.valid = asm_valid and (not self.big or self.valid) + self.pm.send("egpuDockState", msg) diff --git a/iqpilot/selfdrive/iqmodeld/emac_input_state.py b/iqpilot/selfdrive/iqmodeld/emac_input_state.py new file mode 100644 index 000000000..698e4f639 --- /dev/null +++ b/iqpilot/selfdrive/iqmodeld/emac_input_state.py @@ -0,0 +1,10 @@ +""" +Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos/ +""" + +from iqpilot.selfdrive.iqmodeld.temporal_state import ( + SplitTemporalState as SplitInputState, + TemporalInputState as EmacInputState, +) + +__all__ = ["EmacInputState", "SplitInputState"] diff --git a/iqpilot/selfdrive/iqmodeld/emac_model_meta.py b/iqpilot/selfdrive/iqmodeld/emac_model_meta.py new file mode 100644 index 000000000..75f405d67 --- /dev/null +++ b/iqpilot/selfdrive/iqmodeld/emac_model_meta.py @@ -0,0 +1,9 @@ +""" +Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos/ +""" +from iqpilot._proprietary_loader import ProprietaryModuleMissing, load_private_module + +try: + load_private_module(__name__, "iqpilot_private.models.emac_model_meta") +except ProprietaryModuleMissing: + from iqpilot.models_private_src.emac_model_meta import * diff --git a/iqpilot/selfdrive/iqmodeld/iqegpumodeld.py b/iqpilot/selfdrive/iqmodeld/iqegpumodeld.py new file mode 100644 index 000000000..0a48111c0 --- /dev/null +++ b/iqpilot/selfdrive/iqmodeld/iqegpumodeld.py @@ -0,0 +1,377 @@ +""" +Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos/ +""" +from __future__ import annotations + +import os +import pickle +import subprocess +import sys +import time + +from iqpilot.system.hardware import TICI + +os.environ.setdefault("GMMU", "0") +if TICI: + os.environ.setdefault("DEV", "QCOM") +else: + os.environ.setdefault("DEV", "CPU") + +import numpy as np +from setproctitle import setproctitle + +import iqpilot.cereal.messaging as messaging +from iqpilot.cereal import car, log +from iqpilot.cereal.messaging import SubMaster +from iqpilot.cereal.services import SERVICE_LIST +from iqdbc.car.car_helpers import get_demo_car_params + +from iqpilot.common.params import Params +from iqpilot.common.realtime import DT_MDL +from iqpilot.common.swaglog import cloudlog +from iqpilot.selfdrive.controls.lib.desire_helper import DesireHelper +from iqpilot.system import sentry + +from iqpilot.common.steer_delay import lateral_action_delay +from iqpilot.selfdrive.iqmodeld.daemon import CalibrationAtlas, CameraIngress, FrameDropMeter +from iqpilot.selfdrive.iqmodeld.driving_action import ( + DESIRE_LEN, LAT_SMOOTH_SECONDS, LONG_SMOOTH_SECONDS, get_action_from_model, +) +from iqpilot.selfdrive.iqmodeld.egpu_helpers import ( + download_onnx, egpu_pkl_path, egpu_present_consented, egpu_selected, local_onnx, patch_tinygrad_fetch_fw, + quarantine_artifact, resolve_backend, usbgpu_present, +) +from iqpilot.selfdrive.iqmodeld.egpu_model import resolve_egpu_model +from iqpilot.selfdrive.iqmodeld.egpu_pipeline import EgpuPipeline, EgpuPipelineError, make_big_channel_payload +from iqpilot.selfdrive.iqmodeld.egpu_telemetry import EgpuDockTelemetry +from iqpilot.selfdrive.iqmodeld.messaging import DrivePacketMemory, populate_drive_messages, populate_odometry_message +from iqpilot.selfdrive.iqmodeld.metadata import Meta20hz +from iqpilot.selfdrive.iqmodeld.model_channel import BIG_CHANNEL, ModelChannel +from iqpilot.selfdrive.iqmodeld.model_warp import FrameWarp +from iqpilot.selfdrive.iqmodeld.parser import PhaseParser + +PROCESS_NAME = "iqpilot.selfdrive.iqmodeld.iqegpumodeld" + +PRESENCE_POLL_S = 5.0 +COMPILE_TIMEOUT_S = 3600 +LINK_UP_TIMEOUT_S = 10.0 +SETUP_RETRY_BASE_S = 3.0 +SETUP_RETRY_MAX_S = 30.0 + + +def park(reason: str) -> None: + cloudlog.warning(f"iqegpumodeld parked: {reason}") + params = Params() + params.put_bool("UsbGpuFailed", True) + params.put("UsbGpuLastError", reason[:512]) + while True: + time.sleep(1) + + +def _wait_for_egpu(params: Params) -> None: + while not usbgpu_present(): + params.put_bool("UsbGpuPresent", False) + time.sleep(PRESENCE_POLL_S) + params.put_bool("UsbGpuPresent", True) + try: + from iqpilot.system.hardware.egpu_dock.flash import link_up + except Exception: + return + deadline = time.monotonic() + LINK_UP_TIMEOUT_S + while time.monotonic() < deadline: + try: + if link_up(): + return + except Exception: + return + time.sleep(0.5) + + +def _compile_in_subprocess(meta: dict, onnx_path: str, pkl_path: str) -> None: + cmd = [sys.executable, "-m", "iqpilot.selfdrive.iqmodeld.tools.compile_egpu_model", + "--model", meta["key"], "--onnx", onnx_path, "--output", pkl_path] + compile_env = {**os.environ, "DEV": "USB+AMD:LLVM", "FLOAT16": "1", + "JIT_BATCH_SIZE": "0", "GMMU": "0"} + proc = subprocess.run(cmd, timeout=COMPILE_TIMEOUT_S, capture_output=True, text=True, + env=compile_env, preexec_fn=lambda: os.nice(20)) + if proc.returncode != 0: + tail = (proc.stderr or proc.stdout or "").strip()[-800:] + raise RuntimeError(f"eGPU model compile failed (rc={proc.returncode}): {tail}") + + +def _ensure_artifact(params: Params, meta: dict) -> str: + pkl_path = egpu_pkl_path(meta) + if os.path.isfile(pkl_path): + return pkl_path + + params.put_bool("UsbGpuCompiled", False) + onnx_path = local_onnx(meta) + if onnx_path is None: + params.put("UsbGpuSetupProgress", "0.0") + cloudlog.warning(f"iqegpumodeld downloading {meta['key']} onnx ({meta.get('download', {}).get('size', 0) / 1e6:.0f}MB)") + last = [-1.0] + + def _prog(p: float) -> None: + if p - last[0] >= 0.02 or p >= 1.0: + last[0] = p + params.put("UsbGpuSetupProgress", f"{p:.3f}") + + onnx_path = download_onnx(meta, progress_cb=_prog) + + cloudlog.warning(f"iqegpumodeld compiling {meta['key']} for USB-AMD (one-time, can take minutes)") + _compile_in_subprocess(meta, onnx_path, pkl_path) + cloudlog.warning(f"iqegpumodeld compiled -> {pkl_path}") + return pkl_path + + +def _load_infer_fn(pkl_path: str, meta: dict): + patch_tinygrad_fetch_fw() + from tinygrad.tensor import Tensor + + with open(pkl_path, "rb") as f: + bundle = pickle.load(f) + if bundle.get("model_sha256") != meta["sha256"]: + quarantine_artifact(pkl_path, "pkl model sha mismatch") + raise RuntimeError(f"artifact model sha {bundle.get('model_sha256')} != {meta['sha256']}") + if int(bundle.get("output_len", -1)) != int(meta["output_len"]): + quarantine_artifact(pkl_path, "pkl output_len mismatch") + raise RuntimeError(f"artifact output_len {bundle.get('output_len')} != {meta['output_len']}") + jit = bundle["run_model"] + input_dev = bundle.get("input_device", "AMD") + input_spec = bundle["input_spec"] + + def infer(inputs: dict[str, np.ndarray]) -> np.ndarray: + tensors = {name: Tensor(np.ascontiguousarray(inputs[name]), device=input_dev).realize() + for name in input_spec} + out, = jit(**tensors) + return out.numpy().reshape(-1) + + return infer, input_spec + + +def _warmup(infer_fn, input_spec: dict, output_len: int) -> float: + zeros = {name: np.zeros(shape, dtype=dtype) for name, (shape, dtype) in input_spec.items()} + t0 = time.perf_counter() + out = infer_fn(zeros) + dt = time.perf_counter() - t0 + if out.shape[0] != output_len or not np.isfinite(out).all(): + raise RuntimeError(f"warmup produced invalid output (len={out.shape[0]})") + return dt + + +def main(demo: bool = False) -> None: + cloudlog.warning("iqegpumodeld init") + sentry.set_tag("daemon", PROCESS_NAME) + cloudlog.bind(daemon=PROCESS_NAME) + setproctitle(PROCESS_NAME) + try: + os.sched_setaffinity(0, {4, 5, 6}) + os.nice(-10) + except OSError as e: + cloudlog.warning(f"iqegpumodeld affinity/nice failed ({e}); continuing at defaults") + + params = Params() + backend = resolve_backend(params.get_bool("IQEmacEnabled"), egpu_selected(params), egpu_present_consented(params)) + if backend != "egpu": + park(f"backend resolution is {backend!r}, not egpu; refusing to own the big channel") + + channel = ModelChannel(BIG_CHANNEL, create=True) + + cloudlog.warning("iqegpumodeld waiting for camerad") + cameras = CameraIngress(None) + layout = cameras.layout + + _wait_for_egpu(params) + params.put_bool("UsbGpuLoading", True) + attempt = 0 + while True: + try: + meta = resolve_egpu_model(params) + if meta is None: + raise RuntimeError("selected big model is not in the catalog; check connectivity or pick another model") + if meta.get("split"): + params.put_bool("UsbGpuLoading", False) + park(f"model {meta['key']} needs the Mac backend; the eGPU runs fused models only") + warp = FrameWarp(cameras._primary.width, cameras._primary.height, meta["frame_skip"]) + pkl_path = _ensure_artifact(params, meta) + infer_fn, input_spec = _load_infer_fn(pkl_path, meta) + warm_s = _warmup(infer_fn, input_spec, meta["output_len"]) + break + except Exception as e: + attempt += 1 + params.put("UsbGpuLastError", str(e)[:512]) + cloudlog.warning(f"iqegpumodeld setup attempt {attempt} failed: {e}; retrying") + if not usbgpu_present(): + _wait_for_egpu(params) + time.sleep(min(SETUP_RETRY_MAX_S, SETUP_RETRY_BASE_S * attempt)) + + params.put_bool("UsbGpuLoading", False) + params.put_bool("UsbGpuCompiled", True) + params.put("UsbGpuSetupProgress", "1.0") + cloudlog.warning(f"iqegpumodeld model: {meta['key']} ({meta['model_name']})") + cloudlog.warning(f"iqegpumodeld model up (warmup {warm_s * 1e3:.0f}ms)") + + pipeline = EgpuPipeline(meta, infer_fn) + telemetry_pm = messaging.PubMaster(["egpuDockState"]) + telemetry = EgpuDockTelemetry(telemetry_pm, big=True) + telemetry_every = max(1, round((1.0 / DT_MDL) / SERVICE_LIST["egpuDockState"].frequency)) + + sub = SubMaster(["deviceState", "carState", "roadCameraState", "extrinsicsCalibration", + "driverMonitoringState", "carControl", "lateralDelay", "iqNavState", "radarState"]) + if demo: + CP = get_demo_car_params() + else: + CP = messaging.log_from_bytes(params.get("CarParams", block=True), car.CarParams) + long_delay = CP.longitudinalActuatorDelay + LONG_SMOOTH_SECONDS + + parser = PhaseParser() + memory = DrivePacketMemory() + desire_logic = DesireHelper() + frame_meter = FrameDropMeter(20.0) + warps = CalibrationAtlas() + prev_action = log.ModelDataV2.Action() + slices = {k: v for k, v in meta["output_slices"].items() if k != "pad"} + + produced = 0 + stats: dict[str, list[float]] = {k: [] for k in ("pull", "warp", "infer", "publish", "loop")} + iter_count = 0 + skip_count = 0 + last_pulled_fid = -1 + last_frame_mono = time.monotonic() + t_loop = time.perf_counter() + cloudlog.warning("iqegpumodeld starting") + + while True: + frame_pair = cameras.pull() + t_pull = time.perf_counter() + if frame_pair is None: + if time.monotonic() - last_frame_mono > 2.0: + cloudlog.warning("iqegpumodeld camera stream silent >2s; reconnecting VisionIPC") + cameras = CameraIngress(None) + last_frame_mono = time.monotonic() + continue + last_frame_mono = time.monotonic() + main_buf, extra_buf, main_stamp, extra_stamp = frame_pair + + stats["pull"].append(t_pull - t_loop) + stats["loop"].append(time.perf_counter() - t_loop) + t_loop = time.perf_counter() + if last_pulled_fid >= 0 and main_stamp.frame_id > last_pulled_fid + 1: + skip_count += main_stamp.frame_id - last_pulled_fid - 1 + last_pulled_fid = main_stamp.frame_id + iter_count += 1 + if iter_count % 200 == 0: + pcts = {k: {"p50": round(sorted(v)[len(v) // 2] * 1e3, 1), + "p90": round(sorted(v)[int(len(v) * 0.9)] * 1e3, 1)} + for k, v in stats.items() if v} + cloudlog.event("iqegpu_stats", **pcts, cam_skips=skip_count, window=iter_count) + msg = " ".join(f"{k}=p50:{v['p50']:.0f}/p90:{v['p90']:.0f}ms" for k, v in pcts.items()) + cloudlog.warning(f"iqegpumodeld stages: {msg} cam_skips={skip_count} over {iter_count}") + for v in stats.values(): + v.clear() + skip_count = 0 + + sub.update(0) + + v_ego = max(sub["carState"].vEgo, 0.0) + lat_delay = lateral_action_delay(params, CP, sub["lateralDelay"].lateralDelay) + LAT_SMOOTH_SECONDS + main_tfm, extra_tfm, live_calib_seen = warps.refresh(sub, layout.main_is_wide, layout.dual_camera) + dropped_frames, frame_drop_ratio, _ = frame_meter.sample(main_stamp.frame_id) + + traffic = np.zeros(2, dtype=np.float32) + traffic[int(sub["driverMonitoringState"].isRHD)] = 1 + desire_vec = np.zeros(DESIRE_LEN, dtype=np.float32) + if 0 <= desire_logic.desire < DESIRE_LEN: + desire_vec[desire_logic.desire] = 1 + + frame_delay = DT_MDL + action_delay = DT_MDL / 2 + lat_action_t = lat_delay + frame_delay + action_delay + long_action_t = long_delay + frame_delay + action_delay + action_t = np.array([lat_action_t, long_action_t], dtype=np.float32) + + started_at = time.perf_counter() + try: + warped = warp.run(main_buf, extra_buf, main_tfm, extra_tfm) + except Exception as e: + park(f"warp run failed: {e}") + t_warp = time.perf_counter() + stats["warp"].append(t_warp - started_at) + + try: + output = pipeline.run(warped, desire_vec, traffic, action_t) + except EgpuPipelineError as e: + park(str(e)) + except Exception as e: + park(f"eGPU inference failed: {e}") + t_infer = time.perf_counter() + stats["infer"].append(t_infer - t_warp) + + execution_time = time.perf_counter() - started_at + sliced = {k: output[np.newaxis, sl] for k, sl in slices.items()} + outputs = parser.parse_vision_outputs(sliced) + + action = get_action_from_model(outputs, prev_action, v_ego, float(lat_action_t), float(long_action_t), + lat_smooth_seconds=meta.get("lat_smooth_seconds")) + prev_action = action + + model_msg = messaging.new_message("modelV2") + driving_msg = messaging.new_message("drivingModelData") + pose_msg = messaging.new_message("cameraOdometry") + iq_msg = messaging.new_message("iqDriveModelData") + + populate_drive_messages( + driving_msg, model_msg, outputs, action, memory, + main_stamp.frame_id, extra_stamp.frame_id, sub["roadCameraState"].frameId, + frame_drop_ratio, main_stamp.timestamp_eof, execution_time, + live_calib_seen, Meta20hz, + ) + + model_msg.modelV2.big = True + + desire_state = model_msg.modelV2.meta.desireState + lane_change_prob = desire_state[log.Desire.laneChangeLeft] + desire_state[log.Desire.laneChangeRight] + desire_logic.update(sub["carState"], sub["carControl"].latActive, lane_change_prob, + sub["iqNavState"], model_msg.modelV2, sub["radarState"]) + model_msg.modelV2.meta.laneChangeState = desire_logic.lane_change_state + model_msg.modelV2.meta.laneChangeDirection = desire_logic.lane_change_direction + driving_msg.drivingModelData.meta.laneChangeState = desire_logic.lane_change_state + driving_msg.drivingModelData.meta.laneChangeDirection = desire_logic.lane_change_direction + iq_msg.iqDriveModelData.turnSignalDirection = desire_logic.lane_turn_direction + + populate_odometry_message(pose_msg, outputs, main_stamp.frame_id, dropped_frames, + main_stamp.timestamp_eof, live_calib_seen) + + channel.write(main_stamp.frame_id, make_big_channel_payload( + main_stamp.frame_id, live_calib_seen, execution_time, (t_infer - t_warp) * 1e3, { + "modelV2": model_msg.to_bytes(), + "drivingModelData": driving_msg.to_bytes(), + "cameraOdometry": pose_msg.to_bytes(), + "iqDriveModelData": iq_msg.to_bytes(), + })) + stats["publish"].append(time.perf_counter() - t_infer) + produced += 1 + if produced == 1 or produced % 100 == 0: + infer_ms = (t_infer - t_warp) * 1e3 + cloudlog.warning(f"iqegpumodeld producing: frame={main_stamp.frame_id} total={execution_time * 1e3:.0f}ms infer={infer_ms:.0f}ms count={produced}") + + if produced % telemetry_every == 0: + telemetry.send() + + frame_meter.commit(main_stamp.frame_id) + + +if __name__ == "__main__": + try: + import argparse + ap = argparse.ArgumentParser() + ap.add_argument("--demo", action="store_true") + args = ap.parse_args() + main(demo=args.demo) + except KeyboardInterrupt: + cloudlog.warning("iqegpumodeld got SIGINT") + except Exception: + import traceback + sentry.capture_exception() + cloudlog.exception("iqegpumodeld crashed, parking") + park(f"crashed: {traceback.format_exc(limit=8)}") diff --git a/iqpilot/selfdrive/iqmodeld/model_channel.py b/iqpilot/selfdrive/iqmodeld/model_channel.py new file mode 100644 index 000000000..a9fa710bc --- /dev/null +++ b/iqpilot/selfdrive/iqmodeld/model_channel.py @@ -0,0 +1,58 @@ +""" +Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos/ +""" +from __future__ import annotations + +import mmap +import os +import pickle +import struct + +from iqpilot.common.swaglog import cloudlog + +SMALL_CHANNEL = "/dev/shm/iqpilot_smallmodel" +BIG_CHANNEL = "/dev/shm/iqpilot_bigmodel" +SHM_SIZE = 8 * 1024 * 1024 +HEADER = struct.Struct(" None: + data = pickle.dumps(payload, protocol=pickle.HIGHEST_PROTOCOL) + if HEADER.size + len(data) > SHM_SIZE: + cloudlog.error(f"model payload {len(data)} bytes exceeds shm {SHM_SIZE}, dropping frame {frame_id}") + return + seq = HEADER.unpack(self.mm[:HEADER.size])[0] + HEADER.pack_into(self.mm, 0, seq + 1, frame_id, len(data)) + self.mm[HEADER.size:HEADER.size + len(data)] = data + HEADER.pack_into(self.mm, 0, seq + 2, frame_id, len(data)) + + def peek_frame_id(self) -> int | None: + seq, frame_id, length = HEADER.unpack(self.mm[:HEADER.size]) + if seq == 0 or seq % 2 != 0 or length == 0: + return None + return frame_id + + def read(self) -> tuple[int, dict] | None: + seq1, frame_id, length = HEADER.unpack(self.mm[:HEADER.size]) + if seq1 == 0 or seq1 % 2 != 0 or length == 0: + return None + data = bytes(self.mm[HEADER.size:HEADER.size + length]) + seq2 = HEADER.unpack(self.mm[:HEADER.size])[0] + if seq1 != seq2: + return None + try: + return frame_id, pickle.loads(data) + except Exception: + return None diff --git a/iqpilot/selfdrive/iqmodeld/model_warp.py b/iqpilot/selfdrive/iqmodeld/model_warp.py new file mode 100644 index 000000000..50697728a --- /dev/null +++ b/iqpilot/selfdrive/iqmodeld/model_warp.py @@ -0,0 +1,80 @@ +""" +Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos/ +""" +from __future__ import annotations + +import os +import pickle + +import numpy as np + +from iqpilot.common.swaglog import cloudlog +from iqpilot.system.hardware.hw import Paths + + +def _load_bundle(pkl_path: str, cam_w: int, cam_h: int, frame_skip: int) -> dict: + with open(pkl_path, "rb") as f: + bundle = pickle.load(f) + if bundle.get("frame_skip") != frame_skip: + raise RuntimeError(f"frame_skip {bundle.get('frame_skip')} != {frame_skip}") + if (cam_w, cam_h) not in bundle: + raise RuntimeError(f"missing {cam_w}x{cam_h}; has {[k for k in bundle if isinstance(k, tuple)]}") + _verify_selftest(bundle, cam_w, cam_h) + return bundle + + +def _verify_selftest(bundle: dict, cam_w: int, cam_h: int) -> None: + want = bundle.get("selftest") + if not want: + raise RuntimeError("warp artifact predates the self-test; recompiling") + from iqpilot.system.camerad.cameras.nv12_info import get_nv12_info + from iqpilot.selfdrive.iqmodeld.tools.compile_warp import selftest_digest + nv12_size = get_nv12_info(cam_w, cam_h)[3] + got = selftest_digest(bundle[(cam_w, cam_h)], cam_w, cam_h, nv12_size) + if got != want: + raise RuntimeError(f"warp self-test {got[:12]} != {want[:12]}; artifact computes differently here") + + +class FrameWarp: + + def __init__(self, cam_w: int, cam_h: int, frame_skip: int): + from tinygrad.tensor import Tensor + + pkl_path = os.path.join(Paths.model_root(), f"emac_warp_{cam_w}x{cam_h}_tinygrad.pkl") + bundle = None + if os.path.isfile(pkl_path): + try: + bundle = _load_bundle(pkl_path, cam_w, cam_h, frame_skip) + except Exception as e: + cloudlog.warning(f"warp artifact unusable ({e}); discarding and recompiling") + os.remove(pkl_path) + if bundle is None: + cloudlog.warning(f"warp artifact missing; compiling for {cam_w}x{cam_h} (one-time)") + from iqpilot.selfdrive.iqmodeld.tools.compile_warp import compile_warp + compile_warp(cam_w, cam_h, pkl_path, frame_skip=frame_skip) + cloudlog.warning(f"warp compiled -> {pkl_path}") + bundle = _load_bundle(pkl_path, cam_w, cam_h, frame_skip) + self._jit = bundle[(cam_w, cam_h)] + + self._npy = {"tfm": np.zeros((3, 3), dtype=np.float32), "big_tfm": np.zeros((3, 3), dtype=np.float32)} + self._tensors = {k: Tensor(v, device="NPY").realize() for k, v in self._npy.items()} + self._blob_cache: dict[tuple[str, int], object] = {} + self._Tensor = Tensor + + def _frame_tensor(self, key: str, buf): + from tinygrad.device import Device + arr = np.frombuffer(buf.data, dtype=np.uint8) + ck = (key, arr.ctypes.data) + t = self._blob_cache.get(ck) + if t is None: + t = self._Tensor.from_blob(arr.ctypes.data, (arr.size,), dtype="uint8", device=Device.DEFAULT) + self._blob_cache[ck] = t + return t + + def run(self, main_buf, extra_buf, main_tfm: np.ndarray, extra_tfm: np.ndarray) -> np.ndarray: + self._npy["tfm"][:] = main_tfm + self._npy["big_tfm"][:] = extra_tfm + warped = self._jit(tfm=self._tensors["tfm"], big_tfm=self._tensors["big_tfm"], + frame=self._frame_tensor("img", main_buf), + big_frame=self._frame_tensor("big_img", extra_buf)) + return warped.numpy().astype(np.uint8, copy=False) diff --git a/iqpilot/selfdrive/iqmodeld/modeld_selector.py b/iqpilot/selfdrive/iqmodeld/modeld_selector.py new file mode 100644 index 000000000..5d0c37f5d --- /dev/null +++ b/iqpilot/selfdrive/iqmodeld/modeld_selector.py @@ -0,0 +1,373 @@ +""" +Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos/ +""" +from __future__ import annotations + +import json +import os +import threading +import time +from collections import deque + +from iqpilot.cereal.messaging import PubMaster, log_from_bytes +from setproctitle import setproctitle + +from iqpilot.common.filter_simple import FirstOrderFilter +from iqpilot.common.params import Params +from iqpilot.common.realtime import config_realtime_process +from iqpilot.common.swaglog import cloudlog +from iqpilot.selfdrive.iqmodeld.model_channel import BIG_CHANNEL, SMALL_CHANNEL, ModelChannel + +PROCESS_NAME = "iqpilot.selfdrive.iqmodeld.modeld_selector" + +BIG_MODEL_DEADLINE = float(os.getenv("IQEMAC_BIG_DEADLINE_MS", "45")) / 1000.0 +BIG_MAX_LAG_FRAMES = int(os.getenv("IQEMAC_MAX_BIG_LAG_FRAMES", "6")) +BIG_FUTURE_ACCEPT = int(os.getenv("IQEMAC_BIG_FUTURE_ACCEPT", "2")) +BIG_ANCHOR_MS = float(os.getenv("IQEMAC_BIG_ANCHOR_MS", "90")) +BIG_WAIT_FLOOR_S = 0.002 +BIG_WAIT_CEIL_S = float(os.getenv("IQEMAC_BIG_WAIT_CEIL_MS", "58")) / 1000.0 +BIG_MISS_LIMIT = int(os.getenv("IQEMAC_BIG_MISS_LIMIT", "80")) +ACTIVATE_WINDOW = int(os.getenv("IQEMAC_ACTIVATE_WINDOW", "50")) +ACTIVATE_FRAC = float(os.getenv("IQEMAC_ACTIVATE_FRAC", "0.7")) +REARM_LIMIT = int(os.getenv("IQEMAC_REARM_LIMIT", "2")) +MODEL_FREQ = 20.0 +WARMUP_FRAMES = 40 +STATUS_WINDOW = int(os.getenv("IQEMAC_STATUS_EVERY", "20")) + +SELECTOR_SERVICES = ["modelV2", "drivingModelData", "cameraOdometry", "iqDriveModelData"] + +EMAC_STATUS_KEYS = { + "active": "MacModelActive", "failed": "MacModelFailed", "last_error": "MacModelLastError", + "latency_ms": "MacModelLatencyMs", "status": "MacModelStatus", + "reachable": "MacModelReachable", "progress": "MacModelDownloadProgress", +} +EGPU_STATUS_KEYS = { + "active": "UsbGpuActive", "failed": "UsbGpuFailed", "last_error": "UsbGpuLastError", + "latency_ms": "UsbGpuLatencyMs", "status": "UsbGpuStatus", + "reachable": "UsbGpuPresent", "progress": "UsbGpuSetupProgress", +} + + +def backend_status_keys(emac_enabled: bool, egpu_enabled: bool, egpu_present: bool = False) -> dict[str, str]: + from iqpilot.selfdrive.iqmodeld.egpu_helpers import resolve_backend + return EGPU_STATUS_KEYS if resolve_backend(emac_enabled, egpu_enabled, egpu_present) == "egpu" else EMAC_STATUS_KEYS + + +def resolve_status_keys(params): + from iqpilot.selfdrive.iqmodeld.egpu_helpers import egpu_present_consented, egpu_selected + return backend_status_keys(params.get_bool("IQEmacEnabled"), egpu_selected(params), egpu_present_consented(params)) + + +def resolve_model_name(params, keys) -> str: + if keys is EGPU_STATUS_KEYS: + from iqpilot.selfdrive.iqmodeld.egpu_model import DEFAULT_EGPU_MODEL, resolve_egpu_model + resolved = resolve_egpu_model(params, allow_refresh=False) + return resolved["key"] if resolved else DEFAULT_EGPU_MODEL + name = params.get("IQEmacModel") or b"lebrowski" + return name.decode() if isinstance(name, bytes) else name + + +class AsyncParamWriter: + + def __init__(self, params: Params): + self._params = params + self._pending: dict[str, object] = {} + self._lock = threading.Lock() + self._event = threading.Event() + threading.Thread(target=self._drain, daemon=True).start() + + def put(self, key: str, value) -> None: + with self._lock: + self._pending[key] = value + self._event.set() + + def put_bool(self, key: str, value: bool) -> None: + self.put(key, bool(value)) + + def _drain(self) -> None: + while True: + self._event.wait() + self._event.clear() + with self._lock: + batch, self._pending = self._pending, {} + for key, value in batch.items(): + try: + if isinstance(value, bool): + self._params.put_bool(key, value) + else: + self._params.put(key, value) + except Exception: + cloudlog.exception(f"async param write failed: {key}") + + +def wait_for_big(big_channel, target: int, deadline: float, min_frame: int = -1, + max_lag_frames: int = BIG_MAX_LAG_FRAMES) -> tuple[dict | None, int | None]: + big_peek = None + grab_at = deadline - 0.004 + while time.perf_counter() < deadline: + bfid = big_channel.peek_frame_id() + big_peek = bfid + if bfid == target - 1 and time.perf_counter() < grab_at: + time.sleep(0.0005) + continue + if bfid is not None and min_frame < bfid <= target + BIG_FUTURE_ACCEPT and target - bfid <= max_lag_frames: + got = big_channel.read() + if got is not None and got[0] == bfid: + return got[1], big_peek + break + if bfid is None or bfid <= min_frame or bfid > target + BIG_FUTURE_ACCEPT or target - bfid > max_lag_frames: + break + time.sleep(0.0005) + return None, big_peek + + +class BigLatch: + + def __init__(self, miss_limit: int = BIG_MISS_LIMIT, activate_window: int = ACTIVATE_WINDOW, + activate_frac: float = ACTIVATE_FRAC, rearm_limit: int = REARM_LIMIT): + self.miss_limit = miss_limit + self.activate_window = activate_window + self.activate_need = int(round(activate_window * activate_frac)) + self.rearm_limit = rearm_limit + self.active = False + self.done = False + self._miss = 0 + self._window: deque[bool] = deque(maxlen=activate_window) + self._retires = 0 + + def update(self, used_big: bool) -> tuple[bool, bool]: + if self.done: + return False, False + if not self.active: + self._window.append(used_big) + if len(self._window) >= self.activate_window and sum(self._window) >= self.activate_need: + self.active = True + self._miss = 0 + self._window.clear() + return True, False + if used_big: + self._miss = 0 + elif self.active: + self._miss += 1 + if self._miss >= self.miss_limit: + self.active = False + self._miss = 0 + self._window.clear() + self._retires += 1 + self.done = self._retires > self.rearm_limit + return False, True + return False, False + + +def _patch_and_send(pm: PubMaster, payload: dict, frame_drop_perc: float, selector_dropped: int, + target: int, source_lag: int, mismatch: bool | None = None) -> None: + msgs = payload["msgs"] + if mismatch is None: + mismatch = source_lag > 0 + + model_msg = log_from_bytes(msgs["modelV2"]).as_builder() + if mismatch: + model_msg.modelV2.frameId = target + model_msg.modelV2.frameAge = max(model_msg.modelV2.frameAge, source_lag) + model_msg.modelV2.frameDropPerc = frame_drop_perc + pm.send("modelV2", model_msg) + + driving_msg = log_from_bytes(msgs["drivingModelData"]).as_builder() + if mismatch: + driving_msg.drivingModelData.frameId = target + driving_msg.drivingModelData.frameDropPerc = frame_drop_perc + pm.send("drivingModelData", driving_msg) + + pose_msg = log_from_bytes(msgs["cameraOdometry"]).as_builder() + if mismatch: + pose_msg.cameraOdometry.frameId = target + pose_msg.valid = bool(payload["live_calib_seen"]) and selector_dropped < 1 and not mismatch + pm.send("cameraOdometry", pose_msg) + + pm.send("iqDriveModelData", msgs["iqDriveModelData"]) + + +def _read_float(params, key: str, default: float) -> float: + v = params.get(key) + try: + return float(v) if v is not None else default + except (TypeError, ValueError): + return default + + +def main() -> None: + cloudlog.warning("modeld_selector init") + cloudlog.bind(daemon=PROCESS_NAME) + setproctitle(PROCESS_NAME) + config_realtime_process([0, 1, 2, 3], 54) + + params = Params() + keys = resolve_status_keys(params) + pwriter = AsyncParamWriter(params) + pwriter.put_bool(keys["active"], False) + pwriter.put_bool(keys["failed"], False) + pm = PubMaster(SELECTOR_SERVICES) + + small_channel: ModelChannel | None = None + big_channel: ModelChannel | None = None + latch = BigLatch() + big_used_count = 0 + run_count = 0 + last_published = -1 + last_big_published = -1 + frame_dropped_filter = FirstOrderFilter(0.0, 10.0, 1.0 / MODEL_FREQ) + recent_big = deque(maxlen=STATUS_WINDOW) + model_name = resolve_model_name(params, keys) + last_backend_check = 0.0 + last_latency_ms = 0.0 + last_source_lag = 0 + miss_reasons = {"no_head": 0, "already_used": 0, "far_future": 0, + "too_stale": 0, "head_prev_timeout": 0, "read_race": 0} + + cloudlog.warning(f"modeld_selector starting (max_big_lag_frames={BIG_MAX_LAG_FRAMES})") + while True: + if small_channel is None: + try: + small_channel = ModelChannel(SMALL_CHANNEL, create=False) + except OSError: + time.sleep(0.05) + continue + if big_channel is None: + try: + big_channel = ModelChannel(BIG_CHANNEL, create=False) + except OSError: + big_channel = None + + fid = small_channel.peek_frame_id() + if fid is None or fid == last_published: + time.sleep(0.0005) + continue + if last_published >= 0 and fid < last_published - 1: + cloudlog.warning(f"modeld_selector frame reset {last_published} -> {fid}; re-arming") + last_published = -1 + last_big_published = -1 + big_used_count = 0 + run_count = 0 + latch = BigLatch() + pwriter.put_bool(keys["active"], False) + pwriter.put_bool(keys["failed"], False) + + now_mono = time.monotonic() + if now_mono - last_backend_check > 1.0: + last_backend_check = now_mono + new_keys = resolve_status_keys(params) + if new_keys is not keys: + cloudlog.warning(f"modeld_selector backend changed {keys['active']} -> {new_keys['active']}; re-arming") + pwriter.put_bool(keys["active"], False) + pwriter.put_bool(keys["failed"], False) + keys = new_keys + model_name = resolve_model_name(params, keys) + recent_big.clear() + last_big_published = -1 + big_used_count = 0 + run_count = 0 + latch = BigLatch() + pwriter.put_bool(keys["active"], False) + pwriter.put_bool(keys["failed"], False) + target = fid + t_start = time.perf_counter() + + small_payload = None + got = small_channel.read() + if got is not None and got[0] == target: + small_payload = got[1] + + payload = None + used_big = False + big_peek = None + if big_channel is not None and not latch.done: + deadline = t_start + BIG_MODEL_DEADLINE + sof_ns = (small_payload or {}).get("timestamp_sof") + if sof_ns: + remaining = (BIG_ANCHOR_MS / 1000.0) - (time.clock_gettime(time.CLOCK_BOOTTIME) - sof_ns / 1e9) + deadline = t_start + min(max(remaining, BIG_WAIT_FLOOR_S), BIG_WAIT_CEIL_S) + payload, big_peek = wait_for_big(big_channel, target, deadline, + last_big_published, BIG_MAX_LAG_FRAMES) + used_big = payload is not None + if not used_big: + if big_peek is None: + miss_reasons["no_head"] += 1 + elif big_peek <= last_big_published: + miss_reasons["already_used"] += 1 + elif big_peek > target + BIG_FUTURE_ACCEPT: + miss_reasons["far_future"] += 1 + elif target - big_peek > BIG_MAX_LAG_FRAMES: + miss_reasons["too_stale"] += 1 + elif big_peek == target - 1: + miss_reasons["head_prev_timeout"] += 1 + else: + miss_reasons["read_race"] += 1 + + if payload is None: + payload = small_payload + if payload is None: + got = small_channel.read() + if got is not None and got[0] == target: + payload = got[1] + + activated_now, failed_now = latch.update(used_big) + if activated_now: + pwriter.put_bool(keys["active"], True) + pwriter.put_bool(keys["failed"], False) + cloudlog.warning(f"modeld_selector switched to BIG model at frame {target}") + elif failed_now: + pwriter.put_bool(keys["active"], False) + pwriter.put_bool(keys["failed"], latch.done) + pwriter.put(keys["last_error"], "big model stalled onroad; local fallback latched" + if latch.done else "big model stalled onroad; small active, big may re-arm") + if latch.done: + cloudlog.warning(f"modeld_selector big stalled, staying on small until next ignition (frame {target})") + else: + cloudlog.warning(f"modeld_selector big stalled, small active; big may re-arm after a clean streak (frame {target})") + + if payload is not None: + selector_dropped = max(0, target - last_published - 1) if last_published >= 0 else 0 + frames_dropped = frame_dropped_filter.update(min(selector_dropped, 10)) + if run_count < WARMUP_FRAMES: + frame_dropped_filter.x = 0.0 + frames_dropped = 0.0 + run_count += 1 + recent_big.append(used_big) + if used_big: + big_used_count += 1 + big_fid = int(payload.get("frame_id", big_peek if big_peek is not None else target)) + last_big_published = min(big_fid, target) + last_latency_ms = float(payload.get("model_execution_time", 0.0)) * 1e3 + source_lag = max(0, target - int(payload.get("frame_id", target))) + frame_mismatch = int(payload.get("frame_id", target)) != target + last_source_lag = source_lag + if run_count % STATUS_WINDOW == 0: + hit_rate = (sum(recent_big) / len(recent_big)) if recent_big else 0.0 + pwriter.put(keys["latency_ms"], last_latency_ms) + pwriter.put(keys["status"], json.dumps({ + "active": latch.active, + "failed": latch.done, + "hit_rate": round(hit_rate, 3), + "latency_ms": round(last_latency_ms, 1), + "source_lag_frames": last_source_lag, + "model": model_name, + "reachable": params.get_bool(keys["reachable"]), + "download_progress": _read_float(params, keys["progress"], 1.0), + "ts_mono": round(time.monotonic(), 1), + })) + if run_count % 100 == 0: + cloudlog.warning(f"modeld_selector misses: {miss_reasons}") + cloudlog.warning(f"modeld_selector: big_used={big_used_count}/{run_count} " + f"last_big_peek={big_peek} target={target} active={latch.active} " + f"max_big_lag={BIG_MAX_LAG_FRAMES}") + + frame_drop_perc = 100.0 * frames_dropped / (1.0 + frames_dropped) + _patch_and_send(pm, payload, frame_drop_perc, selector_dropped, target, source_lag, frame_mismatch) + last_published = target + + +if __name__ == "__main__": + try: + main() + except KeyboardInterrupt: + cloudlog.warning("modeld_selector got SIGINT") diff --git a/iqpilot/selfdrive/iqmodeld/models/runners/model_runner.py b/iqpilot/selfdrive/iqmodeld/models/runners/model_runner.py index 56f7883bd..8ede5f6c4 100644 --- a/iqpilot/selfdrive/iqmodeld/models/runners/model_runner.py +++ b/iqpilot/selfdrive/iqmodeld/models/runners/model_runner.py @@ -129,7 +129,7 @@ class ModelRunner(RunnerRoot): if not active: raise ValueError("runner started without an active model bundle") - self.models = {spec.type.raw: ArtifactSpec(spec) for spec in active.models} + self.models = {spec.type.raw: ArtifactSpec(spec) for spec in _qcom_models(active)} self.is_20hz_3d = False self.is_20hz = active.is20hz self.inputs = {} @@ -180,8 +180,15 @@ class ModelRunner(RunnerRoot): # ---- runner selection (which backend to build for the active bundle) ---------- +def _qcom_models(bundle) -> list: + # usbeMac artifacts ride along in a bundle for the eGPU host; they are never + # loaded on QCOM and must not affect runner classification + return [m for m in bundle.models if m.type.raw != ModelType.usbeMac] + + def _single_artifact_prefix(bundle, prefix: str) -> bool: - return len(bundle.models) == 1 and bundle.models[0].artifact.fileName.startswith(prefix) + models = _qcom_models(bundle) + return len(models) == 1 and models[0].artifact.fileName.startswith(prefix) def _is_fused_bundle(bundle) -> bool: @@ -193,7 +200,7 @@ def _is_supercombo_bundle(bundle) -> bool: def _is_split_bundle(bundle) -> bool: - present = {m.type.raw for m in bundle.models} + present = {m.type.raw for m in _qcom_models(bundle)} split_kinds = {ModelType.vision, ModelType.policy, ModelType.offPolicy, ModelType.onPolicy} return not present.isdisjoint(split_kinds) @@ -205,7 +212,9 @@ def get_model_runner() -> "ModelRunner": from iqpilot.selfdrive.iqmodeld.models.runners.tinygrad.tinygrad_runner import (TinygradRunner, TinygradSplitRunner) bundle = _fetch_bundle() - if not (bundle and bundle.models): + # an eMac-only bundle (no QCOM-loadable models) runs the stock default on + # device; the big host serves the bundle's precompiled artifact + if not (bundle and bundle.models and _qcom_models(bundle)): return TinygradRunner(ModelType.supercombo) if _is_supercombo_bundle(bundle): @@ -219,4 +228,4 @@ def get_model_runner() -> "ModelRunner": return TinygradCombinedSplitRunner() if _is_split_bundle(bundle): return TinygradSplitRunner() - return TinygradRunner(bundle.models[0].type.raw) + return TinygradRunner(_qcom_models(bundle)[0].type.raw) diff --git a/iqpilot/selfdrive/iqmodeld/temporal_state.py b/iqpilot/selfdrive/iqmodeld/temporal_state.py new file mode 100644 index 000000000..70cac9f64 --- /dev/null +++ b/iqpilot/selfdrive/iqmodeld/temporal_state.py @@ -0,0 +1,131 @@ +""" +Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos/ +""" + +from __future__ import annotations + +import math + +import numpy as np + +DEFAULT_FRAME_SKIP = 4 + +MODEL_INPUT_SPEC: dict[str, tuple[tuple[int, ...], str]] = { + "img": ((1, 12, 128, 256), "uint8"), + "big_img": ((1, 12, 128, 256), "uint8"), + "desire_pulse": ((1, 25, 8), "float32"), + "traffic_convention": ((1, 2), "float32"), + "features_buffer": ((1, 24, 512), "float32"), + "action_t": ((1, 2), "float32"), +} + + +def spec_from_meta(meta: dict) -> dict[str, tuple[tuple[int, ...], str]] | None: + shapes = meta.get("input_shapes") + if not shapes: + return None + return {name: (tuple(shape), "uint8" if name in ("img", "big_img") else "float32") + for name, shape in shapes.items()} + + +class TemporalInputState: + def __init__(self, frame_skip: int, spec: dict[str, tuple[tuple[int, ...], str]] = MODEL_INPUT_SPEC): + self.frame_skip = frame_skip + img = spec["img"][0] + fb = spec["features_buffer"][0] + dp = spec["desire_pulse"][0] + + self.n_frames = img[1] // 6 + img_q_shape = (frame_skip * (self.n_frames - 1) + 1, 6, img[2], img[3]) + self._img_shape = img + self._fb_shape = fb + self._dp_shape = dp + feat_dim = math.prod(fb[2:]) + + self.img_q = np.zeros(img_q_shape, dtype=np.uint8) + self.big_img_q = np.zeros(img_q_shape, dtype=np.uint8) + self.feat_q = np.zeros((frame_skip * fb[1], fb[0], feat_dim), dtype=np.float32) + self.desire_q = np.zeros((frame_skip * dp[1], dp[0], dp[2]), dtype=np.float32) + self.prev_desire = np.zeros(dp[2], dtype=np.float32) + self.prev_feat = np.zeros((fb[0], feat_dim), dtype=np.float32) + + @staticmethod + def _shift_append(q: np.ndarray, new_val: np.ndarray) -> None: + q[:-1] = q[1:] + q[-1] = new_val + + def push_and_materialize(self, warped: np.ndarray, desire_pulse: np.ndarray, + traffic_convention: np.ndarray, action_t: np.ndarray, + ) -> dict[str, np.ndarray]: + fs = self.frame_skip + + cur = desire_pulse.astype(np.float32).copy() + cur[0] = 0 + pulse = np.where(cur - self.prev_desire > 0.99, cur, 0).astype(np.float32) + self.prev_desire[:] = cur + + self._shift_append(self.img_q, warped[0]) + self._shift_append(self.big_img_q, warped[1]) + self._shift_append(self.desire_q, pulse.reshape(self._dp_shape[0], self._dp_shape[2])) + self._shift_append(self.feat_q, self.prev_feat) + + dp = self._dp_shape + return { + "img": np.ascontiguousarray(self.img_q[::fs]).reshape(self._img_shape), + "big_img": np.ascontiguousarray(self.big_img_q[::fs]).reshape(self._img_shape), + "features_buffer": np.ascontiguousarray(self.feat_q[::fs]).reshape(self._fb_shape), + "desire_pulse": self.desire_q.reshape(dp[1], fs, dp[0], dp[2]).max(axis=1).reshape(dp), + "traffic_convention": traffic_convention.astype(np.float32).reshape(1, -1), + "action_t": action_t.astype(np.float32).reshape(1, -1), + } + + def note_hidden_state(self, model_output: np.ndarray, hidden_slice: slice) -> None: + self.prev_feat[:] = model_output[hidden_slice].reshape(self.prev_feat.shape) + + +class SplitTemporalState: + + def __init__(self, frame_skip: int, img_shape: tuple[int, ...], + feature_shape: tuple[int, ...], desire_shape: tuple[int, ...]): + self.frame_skip = frame_skip + self._img_shape = tuple(img_shape) + self._fb_shape = tuple(feature_shape) + self._dp_shape = tuple(desire_shape) + + n_frames = img_shape[1] // 6 + img_q_shape = (frame_skip * (n_frames - 1) + 1, 6, img_shape[2], img_shape[3]) + self.img_q = np.zeros(img_q_shape, dtype=np.uint8) + self.big_img_q = np.zeros(img_q_shape, dtype=np.uint8) + self.feat_q = np.zeros((frame_skip * (feature_shape[1] - 1) + 1, feature_shape[0], feature_shape[2]), + dtype=np.float32) + self.desire_q = np.zeros((frame_skip * desire_shape[1], desire_shape[0], desire_shape[2]), dtype=np.float32) + self.prev_desire = np.zeros(desire_shape[2], dtype=np.float32) + + def materialize_vision(self, warped: np.ndarray, desire: np.ndarray) -> dict[str, np.ndarray]: + fs = self.frame_skip + cur = desire.astype(np.float32).copy() + cur[0] = 0 + pulse = np.where(cur - self.prev_desire > 0.99, cur, 0).astype(np.float32) + self.prev_desire[:] = cur + + TemporalInputState._shift_append(self.img_q, warped[0]) + TemporalInputState._shift_append(self.big_img_q, warped[1]) + TemporalInputState._shift_append(self.desire_q, pulse.reshape(self._dp_shape[0], self._dp_shape[2])) + return { + "img": np.ascontiguousarray(self.img_q[::fs]).reshape(self._img_shape), + "big_img": np.ascontiguousarray(self.big_img_q[::fs]).reshape(self._img_shape), + } + + def materialize_policy(self, vision_feature: np.ndarray, traffic_convention: np.ndarray, + action_t: np.ndarray | None = None) -> dict[str, np.ndarray]: + fs = self.frame_skip + TemporalInputState._shift_append(self.feat_q, vision_feature.reshape(self._fb_shape[0], self._fb_shape[2])) + dp = self._dp_shape + out = { + "features_buffer": np.ascontiguousarray(self.feat_q[::fs]).reshape(self._fb_shape), + "desire_pulse": self.desire_q.reshape(dp[1], fs, dp[0], dp[2]).max(axis=1).reshape(dp), + "traffic_convention": traffic_convention.astype(np.float32).reshape(1, -1), + } + if action_t is not None: + out["action_t"] = action_t.astype(np.float32).reshape(1, -1) + return out diff --git a/iqpilot/selfdrive/iqmodeld/tests/test_egpu_stock_parity.py b/iqpilot/selfdrive/iqmodeld/tests/test_egpu_stock_parity.py new file mode 100644 index 000000000..3481018e1 --- /dev/null +++ b/iqpilot/selfdrive/iqmodeld/tests/test_egpu_stock_parity.py @@ -0,0 +1,163 @@ +""" +Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos/ +""" +import types + +import pytest + +from iqpilot.cereal import log, messaging +from iqpilot.cereal.services import SERVICE_LIST + + +class TestTelemetryContract: + def test_service_is_published_at_stock_cadence(self): + assert "egpuDockState" in SERVICE_LIST + assert SERVICE_LIST["egpuDockState"].frequency == 10. + + def test_message_carries_every_stock_field(self): + msg = messaging.new_message("egpuDockState") + state = msg.egpuDockState + for field in ("tempC", "memoryTempC", "powerDrawW", "powerLimitW", "gpuUsagePercent", + "gpuClockMhz", "fanSpeedRpm", "pcieLtssm", "supplyVoltage", "supplyCurrent"): + setattr(state, field, 1) + assert getattr(state, field) == 1 + + def test_metrics_refresh_matches_stock(self): + from iqpilot.selfdrive.iqmodeld.egpu_telemetry import METRICS_REFRESH_EVERY + assert METRICS_REFRESH_EVERY == 100 + + def test_send_without_a_gpu_publishes_an_invalid_message(self): + from iqpilot.selfdrive.iqmodeld import egpu_telemetry + sent = [] + telemetry = egpu_telemetry.EgpuDockTelemetry(types.SimpleNamespace(send=lambda n, m: sent.append((n, m))), big=True) + telemetry._device = lambda: types.SimpleNamespace(_opened_devices=set()) + telemetry.send() + assert sent and sent[0][0] == "egpuDockState" + assert sent[0][1].valid is False + + +class TestBigFrameFlag: + def test_model_message_carries_the_big_flag(self): + msg = messaging.new_message("modelV2") + msg.modelV2.big = True + assert msg.modelV2.big + + +class TestStatusParams: + def test_loading_param_exists_and_is_cleared_like_stock(self): + from pathlib import Path + root = Path(__file__).resolve().parents[3] + keys = (root / "common" / "params_keys.h").read_text() + assert '{"UsbGpuLoading"' in keys + line = next(ln for ln in keys.splitlines() if '"UsbGpuLoading"' in ln) + for flag in ("CLEAR_ON_MANAGER_START", "CLEAR_ON_OFFROAD_TRANSITION", "CLEAR_ON_IGNITION_ON"): + assert flag in line + + +class TestAlerts: + def test_both_stock_big_model_events_exist(self): + assert hasattr(log.OnroadEvent.EventName, "bigModelLoading") + assert hasattr(log.OnroadEvent.EventName, "bigModelFailed") + + def test_alerts_are_wired_with_stock_severities(self): + from iqpilot.selfdrive.selfdrived.events import EVENTS, ET + EventName = log.OnroadEvent.EventName + loading = EVENTS[EventName.bigModelLoading] + failed = EVENTS[EventName.bigModelFailed] + assert ET.NO_ENTRY in loading + assert ET.SOFT_DISABLE in failed and ET.PERMANENT in failed + + +class TestFirmwareGate: + def test_runtime_refuses_a_dock_on_other_firmware(self, tmp_path): + from iqpilot.selfdrive.iqmodeld.egpu_helpers import usbgpu_present + from iqpilot.system.hardware.usb import EGPU_DOCK_FW_PRODUCT, EGPU_DOCK_USB_IDS + vid, pid = EGPU_DOCK_USB_IDS[0] + d = tmp_path / "1-1" + d.mkdir() + (d / "idVendor").write_text(f"{vid:04x}\n") + (d / "idProduct").write_text(f"{pid:04x}\n") + (d / "product").write_text("custom deadbeef-CLEAN\n") + assert not usbgpu_present(str(tmp_path)) + (d / "product").write_text(EGPU_DOCK_FW_PRODUCT + "\n") + assert usbgpu_present(str(tmp_path)) + + +class TestAutoFlash: + def test_hardwared_drives_the_flasher_offroad_only(self): + from iqpilot.system.hardware.hardwared import EgpuDockFlasher + f = EgpuDockFlasher() + calls = [] + f.flash = lambda: calls.append(1) + stale = [{"vendorId": 0xADD1, "productId": 0x0001, "product": "custom deadbeef-CLEAN"}] + f.update(False, stale) + assert f.attempts == 0, "must not flash onroad" + f.update(True, stale) + assert f.attempts == 1 + if f.thread is not None: + f.thread.join(timeout=5) + + def test_matching_firmware_is_never_flashed(self): + from iqpilot.system.hardware.egpu_dock.flash import bundled_version + from iqpilot.system.hardware.hardwared import EgpuDockFlasher + f = EgpuDockFlasher() + f.flash = lambda: pytest.fail("flashed a dock that already matches") + f.update(True, [{"vendorId": 0xADD1, "productId": 0x0001, "product": bundled_version()}]) + assert f.attempts == 0 + + def test_attempts_are_bounded_like_stock(self): + from iqpilot.system.hardware.hardwared import EgpuDockFlasher + assert EgpuDockFlasher.MAX_ATTEMPTS == 3 + assert EgpuDockFlasher.RETRY_INTERVAL == 20. + + +class TestDockIsItsOwnConsent: + + def _params(self, **flags): + class P: + def get_bool(self, k): + return bool(flags.get(k, False)) + def get(self, k, *a, **kw): + return None + return P() + + def _sysfs_with_dock(self, tmp_path, product=None): + from iqpilot.system.hardware.usb import EGPU_DOCK_FW_PRODUCT, EGPU_DOCK_USB_IDS + vid, pid = EGPU_DOCK_USB_IDS[0] + d = tmp_path / "1-1" + d.mkdir() + (d / "idVendor").write_text(f"{vid:04x}\n") + (d / "idProduct").write_text(f"{pid:04x}\n") + (d / "product").write_text((product or EGPU_DOCK_FW_PRODUCT) + "\n") + return str(tmp_path) + + def test_a_plugged_in_dock_selects_itself(self, tmp_path): + from iqpilot.selfdrive.iqmodeld.egpu_helpers import egpu_selected + assert egpu_selected(self._params(), self._sysfs_with_dock(tmp_path)) + + def test_nothing_plugged_in_selects_nothing(self, tmp_path): + from iqpilot.selfdrive.iqmodeld.egpu_helpers import egpu_selected + assert not egpu_selected(self._params(), str(tmp_path)) + + def test_a_dock_on_foreign_firmware_does_not_select_itself(self, tmp_path): + from iqpilot.selfdrive.iqmodeld.egpu_helpers import egpu_selected + assert not egpu_selected(self._params(), self._sysfs_with_dock(tmp_path, "custom deadbeef-CLEAN")) + + def test_the_user_can_force_it_off(self, tmp_path): + from iqpilot.selfdrive.iqmodeld.egpu_helpers import egpu_selected + root = self._sysfs_with_dock(tmp_path) + assert not egpu_selected(self._params(IQEgpuDisabled=True), root) + + def test_the_param_can_force_it_on_without_hardware(self, tmp_path): + from iqpilot.selfdrive.iqmodeld.egpu_helpers import egpu_selected + assert egpu_selected(self._params(IQEgpuEnabled=True), str(tmp_path)) + + def test_present_dock_wins_even_with_emac_enabled(self, tmp_path): + from iqpilot.selfdrive.iqmodeld.egpu_helpers import egpu_selected, resolve_backend, usbgpu_present + root = self._sysfs_with_dock(tmp_path) + assert resolve_backend(True, egpu_selected(self._params(), root), usbgpu_present(root)) == "egpu" + + def test_force_param_without_hardware_yields_to_emac(self, tmp_path): + from iqpilot.selfdrive.iqmodeld.egpu_helpers import egpu_selected, resolve_backend, usbgpu_present + assert resolve_backend(True, egpu_selected(self._params(IQEgpuEnabled=True), str(tmp_path)), + usbgpu_present(str(tmp_path))) == "emac" diff --git a/iqpilot/selfdrive/iqmodeld/tests/test_egpu_worker.py b/iqpilot/selfdrive/iqmodeld/tests/test_egpu_worker.py new file mode 100644 index 000000000..96fae1a8b --- /dev/null +++ b/iqpilot/selfdrive/iqmodeld/tests/test_egpu_worker.py @@ -0,0 +1,509 @@ +""" +Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos/ +""" +from __future__ import annotations + +import io +import json +import time +import urllib.request + +import numpy as np +import pytest + +from iqpilot.selfdrive.iqmodeld.egpu_helpers import ( + resolve_backend, resolve_download_url, usbgpu_present, +) +from iqpilot.selfdrive.iqmodeld.egpu_pipeline import ( + EgpuPipeline, EgpuPipelineError, make_big_channel_payload, +) +from iqpilot.selfdrive.iqmodeld.egpu_model import EGPU_MODELS, get_egpu_model +from iqpilot.selfdrive.iqmodeld.temporal_state import MODEL_INPUT_SPEC as INPUT_SPEC + + +class FakeParams: + def __init__(self, **flags): + self._flags = {k: bool(v) for k, v in flags.items()} + + def get_bool(self, key: str) -> bool: + return self._flags.get(key, False) + + +def _fake_usb_device(root, vid: str, pid: str, name: str = "1-1", product: str | None = None): + from iqpilot.system.hardware.usb import EGPU_DOCK_FW_PRODUCT + d = root / name + d.mkdir() + (d / "idVendor").write_text(vid + "\n") + (d / "idProduct").write_text(pid + "\n") + (d / "product").write_text((product if product is not None else EGPU_DOCK_FW_PRODUCT) + "\n") + + +class TestPresence: + def test_present(self, tmp_path): + _fake_usb_device(tmp_path, "add1", "0001") + assert usbgpu_present(str(tmp_path)) + + def test_foreign_firmware_absent(self, tmp_path): + _fake_usb_device(tmp_path, "add1", "0001", product="custom deadbeef-CLEAN") + assert not usbgpu_present(str(tmp_path)) + + def test_wrong_ids_absent(self, tmp_path): + _fake_usb_device(tmp_path, "05ac", "12a8") + assert not usbgpu_present(str(tmp_path)) + + def test_empty_bus_absent(self, tmp_path): + assert not usbgpu_present(str(tmp_path)) + + def test_unreadable_entries_skipped(self, tmp_path): + (tmp_path / "usb1").mkdir() + _fake_usb_device(tmp_path, "add1", "0001", name="1-2") + assert usbgpu_present(str(tmp_path)) + + +class TestBackendResolution: + def test_none(self): + assert resolve_backend(False, False) is None + + def test_emac_only(self): + assert resolve_backend(True, False) == "emac" + + def test_egpu_only(self): + assert resolve_backend(False, True) == "egpu" + + def test_force_param_yields_to_emac_without_hardware(self): + assert resolve_backend(True, True) == "emac" + + def test_present_dock_wins_over_emac(self): + assert resolve_backend(True, True, True) == "egpu" + + +class TestManagerGating: + @pytest.fixture + def pc(self): + return pytest.importorskip("iqpilot.system.manager.process_config") + + def test_egpu_needs_presence(self, pc, monkeypatch): + monkeypatch.setattr(pc, "usbgpu_present", lambda: True) + assert pc.egpu_enabled(True, FakeParams(IQEgpuEnabled=True), None) + assert pc.egpu_enabled(True, FakeParams(), None) + monkeypatch.setattr(pc, "usbgpu_present", lambda: False) + assert not pc.egpu_enabled(True, FakeParams(IQEgpuEnabled=True), None) + assert not pc.egpu_enabled(True, FakeParams(), None) + + def test_present_dock_wins_over_left_on_emac(self, pc, monkeypatch): + monkeypatch.setattr(pc, "usbgpu_present", lambda: True) + both = FakeParams(IQEmacEnabled=True, IQEgpuEnabled=True) + assert not pc.emac_enabled(True, both, None) + assert pc.egpu_enabled(True, both, None) + + def test_emac_runs_when_no_dock(self, pc, monkeypatch): + monkeypatch.setattr(pc, "usbgpu_present", lambda: False) + assert pc.emac_enabled(True, FakeParams(IQEmacEnabled=True), None) + + def test_disabled_dock_yields_to_emac(self, pc, monkeypatch): + monkeypatch.setattr(pc, "usbgpu_present", lambda: True) + both = FakeParams(IQEmacEnabled=True, IQEgpuDisabled=True) + assert pc.emac_enabled(True, both, None) + assert not pc.egpu_enabled(True, both, None) + + def test_disabled_dock_runs_no_backend_when_no_emac(self, pc, monkeypatch): + monkeypatch.setattr(pc, "usbgpu_present", lambda: True) + off = FakeParams(IQEgpuDisabled=True) + assert not pc.egpu_enabled(True, off, None) + assert not pc.emac_enabled(True, off, None) + + def test_selector_runs_for_either_backend(self, pc): + assert pc.big_model_enabled(True, FakeParams(IQEmacEnabled=True), None) + assert pc.big_model_enabled(True, FakeParams(IQEgpuEnabled=True), None) + assert not pc.big_model_enabled(True, FakeParams(), None) + + def test_iqegpumodeld_registered(self, pc): + assert "iqegpumodeld" in pc.managed_processes + assert "maciqmodeld" in pc.managed_processes + + +class TestDownloadResolve: + def test_direct_url_passthrough(self): + assert resolve_download_url("https://x/y.onnx", "0" * 64, 5) == "https://x/y.onnx" + + def test_commalfs_batch(self, monkeypatch): + seen = {} + + def fake_urlopen(req, timeout=0): + seen["url"] = req.full_url + seen["body"] = json.loads(req.data) + return io.BytesIO(json.dumps( + {"objects": [{"actions": {"download": {"href": "https://signed/url"}}}]}).encode()) + + monkeypatch.setattr(urllib.request, "urlopen", fake_urlopen) + sha = "a5" * 32 + url = resolve_download_url(f"commalfs:{sha}", sha, 1234) + assert url == "https://signed/url" + assert seen["body"]["objects"] == [{"oid": sha, "size": 1234}] + assert seen["url"].endswith("/info/lfs/objects/batch") + + +def _zero_infer(output_len: int, fill=None): + calls = [] + + def infer(inputs): + for name, (shape, dtype) in INPUT_SPEC.items(): + assert tuple(inputs[name].shape) == shape, name + assert inputs[name].dtype == np.dtype(dtype), name + calls.append({k: v.copy() for k, v in inputs.items()}) + out = np.zeros(output_len, dtype=np.float32) + if fill is not None: + out[:] = fill + return out + + infer.calls = calls + return infer + + +def _frame_inputs(seed=0): + rng = np.random.default_rng(seed) + warped = rng.integers(0, 256, (2, 6, 128, 256)).astype(np.uint8) + desire = np.zeros(8, dtype=np.float32) + traffic = np.array([1.0, 0.0], dtype=np.float32) + action_t = np.array([0.25, 0.55], dtype=np.float32) + return warped, desire, traffic, action_t + + +class TestEgpuPipeline: + def setup_method(self): + self.meta = get_egpu_model() + + def test_split_model_rejected(self): + split_meta = {**get_egpu_model(), "key": "some_split", "split": True} + with pytest.raises(EgpuPipelineError, match="split"): + EgpuPipeline(split_meta, _zero_infer(split_meta["output_len"])) + + def test_registry_is_fused_only(self): + assert not any(m.get("split") for m in EGPU_MODELS.values()) + + def test_run_shapes_and_output(self): + infer = _zero_infer(self.meta["output_len"]) + pipe = EgpuPipeline(self.meta, infer) + out = pipe.run(*_frame_inputs()) + assert out.shape == (self.meta["output_len"],) + assert len(infer.calls) == 1 + + def test_hidden_state_feeds_next_features_buffer(self): + output_len = self.meta["output_len"] + hidden = self.meta["output_slices"]["hidden_state"] + + def infer(inputs): + out = np.zeros(output_len, dtype=np.float32) + out[hidden] = np.arange(hidden.stop - hidden.start, dtype=np.float32) + return out + + pipe = EgpuPipeline(self.meta, infer) + pipe.run(*_frame_inputs(1)) + np.testing.assert_array_equal( + pipe.state.prev_feat.reshape(-1), np.arange(hidden.stop - hidden.start, dtype=np.float32)) + pipe.run(*_frame_inputs(2)) + np.testing.assert_array_equal( + pipe.state.feat_q[-1].reshape(-1), np.arange(hidden.stop - hidden.start, dtype=np.float32)) + + def test_desire_rising_edge_pulse(self): + infer = _zero_infer(self.meta["output_len"]) + pipe = EgpuPipeline(self.meta, infer) + warped, _, traffic, action_t = _frame_inputs() + desire_on = np.zeros(8, dtype=np.float32) + desire_on[3] = 1.0 + pipe.run(warped, desire_on, traffic, action_t) + assert infer.calls[-1]["desire_pulse"][0, -1, 3] == 1.0 + for _ in range(5): + pipe.run(warped, desire_on, traffic, action_t) + assert infer.calls[-1]["desire_pulse"][0, :, 3].sum() == 1.0 + + def test_wrong_output_len_raises(self): + pipe = EgpuPipeline(self.meta, _zero_infer(self.meta["output_len"] - 1)) + with pytest.raises(EgpuPipelineError, match="length"): + pipe.run(*_frame_inputs()) + + def test_non_finite_output_raises(self): + pipe = EgpuPipeline(self.meta, _zero_infer(self.meta["output_len"], fill=np.nan)) + with pytest.raises(EgpuPipelineError, match="finite"): + pipe.run(*_frame_inputs()) + + +class TestChannelContract: + def _real_msgs(self): + import iqpilot.cereal.messaging as messaging + msgs = {} + for svc in ("modelV2", "drivingModelData", "cameraOdometry", "iqDriveModelData"): + m = messaging.new_message(svc) + msgs[svc] = m.to_bytes() + return msgs + + def test_payload_keys_match_selector_contract(self): + payload = make_big_channel_payload(7, True, 0.031, 24.0, {"modelV2": b"x"}) + assert payload["source"] == "egpu_big" + for key in ("frame_id", "live_calib_seen", "model_execution_time", "msgs"): + assert key in payload + + def test_selector_consumes_egpu_payload(self, tmp_path): + from iqpilot.selfdrive.iqmodeld.model_channel import ModelChannel + from iqpilot.selfdrive.iqmodeld.modeld_selector import wait_for_big + + chan = ModelChannel(str(tmp_path / "big"), create=True) + payload = make_big_channel_payload(100, True, 0.03, 25.0, self._real_msgs()) + chan.write(100, payload) + + got, peek = wait_for_big(chan, 100, time.perf_counter() + 0.01) + assert peek == 100 + assert got is not None + assert got["source"] == "egpu_big" + assert got["frame_id"] == 100 + + def test_selector_patch_and_send_parses_egpu_msgs(self): + from iqpilot.selfdrive.iqmodeld.modeld_selector import _patch_and_send + + sent = {} + + class PM: + def send(self, service, msg): + sent[service] = msg + + payload = make_big_channel_payload(42, True, 0.03, 25.0, self._real_msgs()) + _patch_and_send(PM(), payload, frame_drop_perc=0.0, selector_dropped=0, target=42, source_lag=0) + assert set(sent) == {"modelV2", "drivingModelData", "cameraOdometry", "iqDriveModelData"} + assert sent["modelV2"].modelV2.frameDropPerc == 0.0 + assert sent["cameraOdometry"].valid + + def test_selector_lag_patches_frame_id(self): + from iqpilot.selfdrive.iqmodeld.modeld_selector import _patch_and_send + + sent = {} + + class PM: + def send(self, service, msg): + sent[service] = msg + + payload = make_big_channel_payload(40, True, 0.03, 25.0, self._real_msgs()) + _patch_and_send(PM(), payload, frame_drop_perc=0.0, selector_dropped=0, target=42, source_lag=2) + assert sent["modelV2"].modelV2.frameId == 42 + assert not sent["cameraOdometry"].valid + + +def _import_worker(): + try: + import iqpilot.selfdrive.iqmodeld.iqegpumodeld as w + return w + except ImportError as e: + if any(tag in str(e) for tag in ("pyx", "visionipc", "proprietary_runtime")): + pytest.skip(f"device-only import chain unavailable on this host: {e}") + raise + + +class TestWorkerModule: + def test_module_imports_off_device(self): + w = _import_worker() + assert w.PROCESS_NAME.endswith("iqegpumodeld") + assert callable(w.main) + + def test_warmup_validates_output(self): + w = _import_worker() + spec = {name: (shape, dtype) for name, (shape, dtype) in INPUT_SPEC.items()} + def good(inputs): + return np.zeros(10, dtype=np.float32) + assert w._warmup(good, spec, 10) >= 0.0 + with pytest.raises(RuntimeError, match="invalid"): + w._warmup(good, spec, 11) + + +class TestSelectorBackendKeys: + def test_emac_default(self): + from iqpilot.selfdrive.iqmodeld.modeld_selector import EMAC_STATUS_KEYS, backend_status_keys + assert backend_status_keys(False, False) is EMAC_STATUS_KEYS + assert backend_status_keys(True, False) is EMAC_STATUS_KEYS + + def test_egpu_selected(self): + from iqpilot.selfdrive.iqmodeld.modeld_selector import EGPU_STATUS_KEYS, backend_status_keys + assert backend_status_keys(False, True) is EGPU_STATUS_KEYS + assert backend_status_keys(False, True)["active"] == "UsbGpuActive" + assert backend_status_keys(False, True)["failed"] == "UsbGpuFailed" + + def test_emac_wins_when_both(self): + from iqpilot.selfdrive.iqmodeld.modeld_selector import EMAC_STATUS_KEYS, backend_status_keys + assert backend_status_keys(True, True) is EMAC_STATUS_KEYS + + def test_key_maps_cover_same_roles(self): + from iqpilot.selfdrive.iqmodeld.modeld_selector import EGPU_STATUS_KEYS, EMAC_STATUS_KEYS + assert set(EGPU_STATUS_KEYS) == set(EMAC_STATUS_KEYS) + + +class TestBackendSeparation: + EGPU_SOURCES = ( + "egpu_helpers.py", "egpu_pipeline.py", "egpu_model.py", "iqegpumodeld.py", + "big_catalog.py", "tools/compile_egpu_model.py", + ) + BANNED_IMPORTS = ("emac_input_state", "emac_model_meta", "maciqmodeld", "mac_protocol", "mac_client") + + def _sources(self): + import pathlib + root = pathlib.Path(__file__).resolve().parents[1] + return {name: (root / name).read_text() for name in self.EGPU_SOURCES} + + def test_no_emac_module_imports(self): + for name, src in self._sources().items(): + for banned in self.BANNED_IMPORTS: + assert f"import {banned}" not in src and f"iqmodeld.{banned}" not in src, f"{name} imports {banned}" + + def test_no_macmodel_params(self): + for name, src in self._sources().items(): + assert "MacModel" not in src, f"{name} references MacModel* params" + + def test_emac_shim_reexports_temporal_state(self): + from iqpilot.selfdrive.iqmodeld import emac_input_state, temporal_state + assert emac_input_state.EmacInputState is temporal_state.TemporalInputState + assert emac_input_state.SplitInputState is temporal_state.SplitTemporalState + + def test_emac_modules_are_not_in_the_public_tree(self): + import pathlib + root = pathlib.Path(__file__).resolve().parents[1] + for gone in ("mac_protocol.py", "mac_client.py", "maciqmodeld.py", "bulk_transport.py"): + assert not (root / gone).exists(), f"{gone} must live only in konn3kt_private" + + +class TestMetaDrivenInputSpec: + + def _run_one(self, meta): + seen = {} + def infer(inputs): + seen.update({k: v.shape for k, v in inputs.items()}) + return np.zeros(meta["output_len"], dtype=np.float32) + pipe = EgpuPipeline(meta, infer) + pipe.run(np.zeros((2, 6, 128, 256), np.uint8), np.zeros(8, np.float32), + np.array([1, 0], np.float32), np.zeros(2, np.float32)) + return seen + + def test_default_contract_unchanged(self): + meta = get_egpu_model() + seen = self._run_one(meta) + assert seen["features_buffer"] == (1, 24, 512) + assert seen["desire_pulse"] == (1, 25, 8) + + def test_registry_shapes_drive_the_state(self): + meta = dict(get_egpu_model()) + meta["output_len"] = 18452 + meta["output_slices"] = dict(meta["output_slices"], hidden_state=slice(2066, 18450)) + meta["input_shapes"] = { + "img": (1, 12, 128, 256), "big_img": (1, 12, 128, 256), + "desire_pulse": (1, 33, 8), "traffic_convention": (1, 2), + "action_t": (1, 2), "features_buffer": (1, 32, 32, 512), + } + seen = self._run_one(meta) + assert seen["features_buffer"] == (1, 32, 32, 512) + assert seen["desire_pulse"] == (1, 33, 8) + + +class TestCatalogResolution: + + def _params(self, model, doc=None): + class P: + def get(self, k): + if k == "IQEmacModel": + return model + if k == "IQEmacCatalogCache": + return json.dumps(doc) if doc else None + return None + return P() + + def _doc(self): + return {"schema": 1, "bundles": [{ + "short_name": "ttx", "display_name": "TTx", "index": 1, + "model_name": "big_driving_supercombo", + "wire": {"output_len": 2580, "frame_skip": 4, "pipeline": True, + "output_slices": {"plan": [917, 1907], "hidden_state": [2066, 2578], "pad": [-2, None]}, + "input_shapes": {"img": [1, 12, 128, 256], "big_img": [1, 12, 128, 256], + "desire_pulse": [1, 33, 8], "traffic_convention": [1, 2], + "action_t": [1, 2], "features_buffer": [1, 32, 512]}, + "lat_smooth_seconds": 0.1}, + "source": {"kind": "comma_lfs", "sha256": "c" * 64, "size": 1}, + }]} + + def test_unset_selection_is_the_builtin_default(self): + from iqpilot.selfdrive.iqmodeld.egpu_model import resolve_egpu_model + m = resolve_egpu_model(self._params(None)) + assert m["key"] == "lebrowski" and m["sha256"].startswith("a501760a") + + def test_catalog_selection_resolves_with_shapes_and_smoothing(self): + from iqpilot.selfdrive.iqmodeld.egpu_model import resolve_egpu_model + m = resolve_egpu_model(self._params("ttx", self._doc())) + assert m["key"] == "ttx" + assert m["input_shapes"]["features_buffer"] == (1, 32, 512) + assert m["input_shapes"]["desire_pulse"] == (1, 33, 8) + assert m["lat_smooth_seconds"] == 0.1 + assert m["output_slices"]["pad"] == slice(-2, None) + + def test_unknown_selection_is_a_park_not_a_silent_default(self): + from iqpilot.selfdrive.iqmodeld.egpu_model import resolve_egpu_model + assert resolve_egpu_model(self._params("ghost", self._doc()), allow_refresh=False) is None + + def test_bench_model_is_not_selectable(self): + from iqpilot.selfdrive.iqmodeld.egpu_model import resolve_egpu_model + assert resolve_egpu_model(self._params("comma_small", self._doc()), allow_refresh=False) is None + + def test_registry_carries_no_model_list(self): + from iqpilot.selfdrive.iqmodeld.egpu_model import EGPU_MODELS + assert set(EGPU_MODELS) == {"lebrowski", "comma_small"} + + +class TestConsentAndIntegrity: + def test_disabled_param_denies_present_dock(self, monkeypatch): + from iqpilot.selfdrive.iqmodeld import egpu_helpers + monkeypatch.setattr(egpu_helpers, "usbgpu_present", lambda sysfs_root=egpu_helpers.USB_SYSFS_ROOT: True) + assert egpu_helpers.egpu_present_consented(FakeParams()) is True + assert egpu_helpers.egpu_present_consented(FakeParams(IQEgpuDisabled=True)) is False + + def test_local_onnx_quarantines_bad_content(self, tmp_path, monkeypatch): + import hashlib + from iqpilot.selfdrive.iqmodeld import egpu_helpers + onnx = tmp_path / "m.onnx" + onnx.write_bytes(b"good") + meta = {"sha256": hashlib.sha256(b"good").hexdigest(), "download": {"size": 4}} + monkeypatch.setattr(egpu_helpers, "onnx_cache_path", lambda m: str(onnx)) + assert egpu_helpers.local_onnx(meta) == str(onnx) + onnx.write_bytes(b"bad!") + assert egpu_helpers.local_onnx(meta) is None + assert not onnx.exists() + assert (tmp_path / "m.onnx.unusable").exists() + + +class TestEgpuDockStatus: + def _run(self, seq): + from iqpilot.system.hardware.egpu_dock.status import EgpuDockStatus + st = EgpuDockStatus() + fired = {} + def set_alert(name, cond, extra=None): + fired[name] = (bool(cond), extra) + for args in seq: + st.update(*args, set_alert) + return {k: v for k, v in fired.items() if v[0]} + + def _dock(self, speed=10000, product="custom ed4e39b7-CLEAN"): + return [{"vendorId": 0xADD1, "productId": 0x0001, "product": product, "speedMbps": speed}] + + def test_no_dock_no_alerts(self): + assert self._run([(True, [], False, False, None, True, None)]) == {} + + def test_usb2_dock_warns_slow(self): + fired = self._run([(True, self._dock(speed=480), False, False, None, True, None)]) + assert fired.get("Offroad_EgpuUsbSlow") == (True, "480 Mbps") + + def test_power_fault_reports_pcie_unavailable(self): + class St: + supplyFault = True + supplyVoltage = 0 + pcieLtssm = 0x78 + tempC = memoryTempC = 40.0 + fanSpeedRpm = 1500 + d = self._dock() + fired = self._run([ + (True, d, False, False, None, True, None), + (False, d, False, True, None, True, None), + (False, d, False, False, b"1", True, St()), + ]) + assert "Offroad_EgpuPcieUnavailable" in fired diff --git a/iqpilot/selfdrive/iqmodeld/tests/test_emac_input_state.py b/iqpilot/selfdrive/iqmodeld/tests/test_emac_input_state.py new file mode 100644 index 000000000..ac661b607 --- /dev/null +++ b/iqpilot/selfdrive/iqmodeld/tests/test_emac_input_state.py @@ -0,0 +1,111 @@ +""" +Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos/ +""" +from __future__ import annotations + +import os + +import numpy as np +import pytest + +os.environ.setdefault("DEV", "CPU") + +from iqpilot.selfdrive.iqmodeld.emac_input_state import EmacInputState +from iqpilot.selfdrive.iqmodeld.emac_model_meta import FRAME_SKIP, OUTPUT_LEN, OUTPUT_SLICES +from iqpilot.selfdrive.iqmodeld.temporal_state import MODEL_INPUT_SPEC as INPUT_SPEC + +N_FRAMES_TEST = 30 +IMG_SHAPE = INPUT_SPEC["img"][0] +DESIRE_LEN = INPUT_SPEC["desire_pulse"][0][2] + + +class _CaptureRunner: + + def __init__(self): + self.captured: dict[str, np.ndarray] | None = None + + def __call__(self, inputs): + from tinygrad import Tensor + self.captured = {k: v.numpy().copy() for k, v in inputs.items()} + return {"outputs": Tensor(np.zeros((1, OUTPUT_LEN), dtype=np.float32))} + + +@pytest.fixture(scope="module") +def reference(): + from iqpilot.selfdrive.iqmodeld.tools.compile_supercombo import ( + POLICY_INPUTS, make_input_queues, make_run_policy, + ) + + input_shapes = {name: shape for name, (shape, _) in INPUT_SPEC.items()} + metadata = {"input_shapes": input_shapes} + capture = _CaptureRunner() + run_policy = make_run_policy(capture, metadata, FRAME_SKIP) + queues, npy = make_input_queues(input_shapes, FRAME_SKIP, device="CPU") + return run_policy, queues, npy, capture, POLICY_INPUTS + + +def _rising_edge(raw_desire: np.ndarray, prev: np.ndarray) -> np.ndarray: + cur = raw_desire.astype(np.float32).copy() + cur[0] = 0 + pulse = np.where(cur - prev > 0.99, cur, 0).astype(np.float32) + prev[:] = cur + return pulse + + +def test_materialized_inputs_match_tinygrad_reference(reference): + from tinygrad import Tensor + + run_policy, queues, npy, capture, policy_inputs = reference + rng = np.random.default_rng(1234) + state = EmacInputState(FRAME_SKIP) + ref_prev_desire = np.zeros(DESIRE_LEN, dtype=np.float32) + + hidden = np.zeros((1, 512), dtype=np.float32) + for frame in range(N_FRAMES_TEST): + warped = rng.integers(0, 256, (2, 6, IMG_SHAPE[2], IMG_SHAPE[3]), dtype=np.int64).astype(np.uint8) + raw_desire = np.zeros(DESIRE_LEN, dtype=np.float32) + if frame % 3: + raw_desire[int(rng.integers(0, DESIRE_LEN))] = 1.0 + traffic = rng.standard_normal(2).astype(np.float32) + action_t = rng.standard_normal(2).astype(np.float32) + + npy["desire"][:] = _rising_edge(raw_desire, ref_prev_desire) + npy["traffic_convention"][:] = traffic + npy["action_t"][:] = action_t + npy["prev_feat"][:] = hidden + run_policy(warped=Tensor(warped), **{k: queues[k] for k in policy_inputs}) + ref_inputs = capture.captured + + state.prev_feat[:] = hidden + mat = state.push_and_materialize(warped, raw_desire, traffic, action_t) + + for name in INPUT_SPEC: + assert ref_inputs[name].shape == tuple(INPUT_SPEC[name][0]), name + np.testing.assert_array_equal( + mat[name].astype(ref_inputs[name].dtype), ref_inputs[name], + err_msg=f"frame {frame}: materialized {name} diverges from tinygrad reference") + + fake_output = rng.standard_normal(OUTPUT_LEN).astype(np.float32) + state.note_hidden_state(fake_output, OUTPUT_SLICES["hidden_state"]) + hidden = fake_output[OUTPUT_SLICES["hidden_state"]].reshape(1, 512).copy() + + +def test_note_hidden_state_slice(): + state = EmacInputState(FRAME_SKIP) + out = np.arange(OUTPUT_LEN, dtype=np.float32) + state.note_hidden_state(out, OUTPUT_SLICES["hidden_state"]) + np.testing.assert_array_equal(state.prev_feat.reshape(-1), out[OUTPUT_SLICES["hidden_state"]]) + + +def test_desire_pulse_rising_edge_only_once(): + state = EmacInputState(FRAME_SKIP) + held = np.zeros(DESIRE_LEN, dtype=np.float32) + held[3] = 1.0 + warped = np.zeros((2, 6, IMG_SHAPE[2], IMG_SHAPE[3]), dtype=np.uint8) + zeros2 = np.zeros(2, dtype=np.float32) + + first = state.push_and_materialize(warped, held, zeros2, zeros2) + assert first["desire_pulse"][0, -1, 3] == 1.0 + second = state.push_and_materialize(warped, held, zeros2, zeros2) + assert state.desire_q[-1].max() == 0.0 + assert second["desire_pulse"][0, -1, 3] == 1.0 diff --git a/iqpilot/selfdrive/iqmodeld/tests/test_lat_delay_source.py b/iqpilot/selfdrive/iqmodeld/tests/test_lat_delay_source.py index bd4fcba79..de48f580c 100644 --- a/iqpilot/selfdrive/iqmodeld/tests/test_lat_delay_source.py +++ b/iqpilot/selfdrive/iqmodeld/tests/test_lat_delay_source.py @@ -34,6 +34,7 @@ def _daemon(params, steer_control_type): return SimpleNamespace( _params=params, _car_params=car_params, + _channel=None, _sub={"lateralDelay": SimpleNamespace(lateralDelay=LIVE_DELAY)}, _runtime=SimpleNamespace(lat_delay=None, PLANPLUS_CONTROL=None, model_smoothing_max_extra_sec=None), _warps=SimpleNamespace(set_offset=lambda _: None), diff --git a/iqpilot/selfdrive/iqmodeld/tests/test_selector_version_gate.py b/iqpilot/selfdrive/iqmodeld/tests/test_selector_version_gate.py new file mode 100644 index 000000000..73a9fe23b --- /dev/null +++ b/iqpilot/selfdrive/iqmodeld/tests/test_selector_version_gate.py @@ -0,0 +1,31 @@ +""" +Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos/ + +The eMac bundles ship `minimum_selector_version = 17`, and the version +gate lives in the COMPILED private selector bundle, not in this repo. If that +bundle is rebuilt from stale source the gate still reads 16, every eMac bundle +is silently dropped as "too new", and the selector simply shows no eMac models +— with no error anywhere. Assert the effective gate instead, so a stale +private bundle fails here rather than on a device. +""" +from iqpilot.selfdrive.iqmodeld.emac_model_meta import EMAC_BUNDLE_MIN_SELECTOR_VERSION +from iqpilot.selfdrive.iqmodeld.models.helpers import is_bundle_version_compatible + + +def test_gate_accepts_the_version_our_emac_bundles_ship(): + assert is_bundle_version_compatible({"minimumSelectorVersion": EMAC_BUNDLE_MIN_SELECTOR_VERSION}), ( + f"the effective selector gate rejects minimumSelectorVersion=" + f"{EMAC_BUNDLE_MIN_SELECTOR_VERSION}; the private selector bundle is stale. " + f"Rebuild it from BOTH iqpilot/models_private_src/helpers.py " + f"(CURRENT_SELECTOR_VERSION) and fetcher.py (MANIFEST_VERSION)." + ) + + +def test_gate_still_accepts_older_bundles(): + # the window is a range, not a floor: bumping it must not orphan the existing catalogue + assert is_bundle_version_compatible({"minimumSelectorVersion": 12}) + assert is_bundle_version_compatible({"minimumSelectorVersion": 16}) + + +def test_gate_rejects_a_bundle_from_the_future(): + assert not is_bundle_version_compatible({"minimumSelectorVersion": EMAC_BUNDLE_MIN_SELECTOR_VERSION + 5}) diff --git a/iqpilot/selfdrive/iqmodeld/tests/test_split_input_state.py b/iqpilot/selfdrive/iqmodeld/tests/test_split_input_state.py new file mode 100644 index 000000000..e2d54c017 --- /dev/null +++ b/iqpilot/selfdrive/iqmodeld/tests/test_split_input_state.py @@ -0,0 +1,131 @@ +""" +Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos/ + +eMac split-model "prepared input equivalence": SplitInputState must reproduce, +byte-exact, the queue semantics of compile_split_runtime's execute_bundle — +the real tinygrad reference graph run on CPU with stub vision/policy runners, +over a multi-frame random sequence with desire rising edges. +""" +from __future__ import annotations + +import os + +import numpy as np +import pytest + +os.environ.setdefault("DEV", "CPU") + +from iqpilot.selfdrive.iqmodeld.emac_input_state import EmacInputState, SplitInputState + +N_FRAMES_TEST = 30 +FRAME_SKIP = 4 +IMG_SHAPE = (1, 12, 16, 32) # small spatial dims: queue math is shape-generic +FB_SHAPE = (1, 25, 512) +DP_SHAPE = (1, 25, 8) +VISION_OUT_LEN = 1576 +HIDDEN_SLICE = slice(1064, 1576) + +VISION_SHAPES = {"img": IMG_SHAPE, "big_img": IMG_SHAPE} +POLICY_SHAPES = {"desire_pulse": DP_SHAPE, "traffic_convention": (1, 2), "features_buffer": FB_SHAPE} + + +class _StubRunner: + """Stands in for OnnxRunner inside execute_bundle: returns a preset output + and records the materialized inputs it was fed.""" + + def __init__(self, out_len: int): + self.out_len = out_len + self.next_output: np.ndarray | None = None + self.captured: dict[str, np.ndarray] | None = None + + def __call__(self, inputs): + from tinygrad import Tensor + self.captured = {k: v.numpy().copy() for k, v in inputs.items()} + out = self.next_output if self.next_output is not None else np.zeros((1, self.out_len), dtype=np.float32) + return {"outputs": Tensor(out.astype(np.float32))} + + +@pytest.fixture(scope="module") +def reference(): + from tinygrad import Tensor + from iqpilot.selfdrive.iqmodeld.tools.compile_split_runtime import _role_executor + + meta_by_role = { + "vision": {"input_shapes": dict(VISION_SHAPES), "output_slices": {"hidden_state": HIDDEN_SLICE}}, + "policy": {"input_shapes": dict(POLICY_SHAPES), "output_slices": {}}, + } + vision, policy = _StubRunner(VISION_OUT_LEN), _StubRunner(1000) + execute_bundle = _role_executor({"vision": vision, "policy": policy}, meta_by_role, FRAME_SKIP) + + feat_q = Tensor(np.zeros((FRAME_SKIP * (FB_SHAPE[1] - 1) + 1, FB_SHAPE[0], FB_SHAPE[2]), dtype=np.float32), + device="CPU").contiguous().realize() + desire_q = Tensor(np.zeros((FRAME_SKIP * DP_SHAPE[1], DP_SHAPE[0], DP_SHAPE[2]), dtype=np.float32), + device="CPU").contiguous().realize() + return execute_bundle, feat_q, desire_q, vision, policy + + +def test_split_inputs_match_tinygrad_reference(reference): + from tinygrad import Tensor + + execute_bundle, feat_q, desire_q, vision_stub, policy_stub = reference + rng = np.random.default_rng(4321) + state = SplitInputState(FRAME_SKIP, IMG_SHAPE, FB_SHAPE, DP_SHAPE) + ref_prev_desire = np.zeros(DP_SHAPE[2], dtype=np.float32) + + for frame in range(N_FRAMES_TEST): + warped = rng.integers(0, 256, (2, 6, IMG_SHAPE[2], IMG_SHAPE[3]), dtype=np.int64).astype(np.uint8) + raw_desire = np.zeros(DP_SHAPE[2], dtype=np.float32) + if frame % 3: + raw_desire[int(rng.integers(0, DP_SHAPE[2]))] = 1.0 + traffic = rng.standard_normal((1, 2)).astype(np.float32) + vision_out = rng.standard_normal((1, VISION_OUT_LEN)).astype(np.float32) + vision_stub.next_output = vision_out + + # --- ours --- + vis_inputs = state.materialize_vision(warped, raw_desire) + pol_inputs = state.materialize_policy(vision_out[0, HIDDEN_SLICE], traffic[0]) + + # --- reference graph: rising edge happens outside execute_bundle (run_fused) --- + cur = raw_desire.copy() + cur[0] = 0 + ref_pulse = np.where(cur - ref_prev_desire > 0.99, cur, 0).astype(np.float32) + ref_prev_desire[:] = cur + + execute_bundle( + img=Tensor(vis_inputs["img"], device="CPU").realize(), + big_img=Tensor(vis_inputs["big_img"], device="CPU").realize(), + feat_q=feat_q, desire_q=desire_q, + desire=Tensor(ref_pulse, device="CPU").realize(), + traffic_convention=Tensor(traffic, device="CPU").realize(), + action_t=Tensor(np.zeros((1, 2), dtype=np.float32), device="CPU").realize(), + ) + ref = policy_stub.captured + assert ref is not None + + assert ref["features_buffer"].tobytes() == pol_inputs["features_buffer"].tobytes(), f"features frame {frame}" + assert ref["desire_pulse"].tobytes() == pol_inputs["desire_pulse"].tobytes(), f"desire frame {frame}" + assert ref["traffic_convention"].tobytes() == pol_inputs["traffic_convention"].tobytes() + # vision saw exactly what our img queues materialized + vref = vision_stub.captured + assert vref["img"].tobytes() == vis_inputs["img"].tobytes(), f"img frame {frame}" + assert vref["big_img"].tobytes() == vis_inputs["big_img"].tobytes(), f"big_img frame {frame}" + + +def test_split_img_queue_matches_fused_state(): + # img/desire mechanics are shared with the fused mirror: same warps must + # materialize identical img/big_img in both states + rng = np.random.default_rng(7) + fused_spec = { + "img": (IMG_SHAPE, "uint8"), "big_img": (IMG_SHAPE, "uint8"), + "desire_pulse": (DP_SHAPE, "float32"), "traffic_convention": ((1, 2), "float32"), + "features_buffer": ((1, 24, 512), "float32"), "action_t": ((1, 2), "float32"), + } + fused = EmacInputState(FRAME_SKIP, fused_spec) + split = SplitInputState(FRAME_SKIP, IMG_SHAPE, FB_SHAPE, DP_SHAPE) + for _ in range(12): + warped = rng.integers(0, 256, (2, 6, IMG_SHAPE[2], IMG_SHAPE[3]), dtype=np.int64).astype(np.uint8) + desire = np.zeros(DP_SHAPE[2], dtype=np.float32) + f = fused.push_and_materialize(warped, desire, np.zeros(2, dtype=np.float32), np.zeros(2, dtype=np.float32)) + s = split.materialize_vision(warped, desire) + assert f["img"].tobytes() == s["img"].tobytes() + assert f["big_img"].tobytes() == s["big_img"].tobytes() diff --git a/iqpilot/selfdrive/iqmodeld/tools/compile_egpu_model.py b/iqpilot/selfdrive/iqmodeld/tools/compile_egpu_model.py new file mode 100644 index 000000000..7402a36a9 --- /dev/null +++ b/iqpilot/selfdrive/iqmodeld/tools/compile_egpu_model.py @@ -0,0 +1,144 @@ +""" +Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos/ +""" +from __future__ import annotations + +import argparse +import os +import pickle +import time + +os.environ.setdefault("DEV", "USB+AMD:LLVM") +os.environ.setdefault("FLOAT16", "1") +os.environ.setdefault("JIT_BATCH_SIZE", "0") +os.environ.setdefault("GMMU", "0") + +import numpy as np + +from iqpilot.selfdrive.iqmodeld.egpu_helpers import egpu_pkl_path, local_onnx, patch_tinygrad_fetch_fw +from iqpilot.selfdrive.iqmodeld.egpu_model import EGPU_MODELS, get_egpu_model, resolve_egpu_model +from iqpilot.selfdrive.iqmodeld.temporal_state import MODEL_INPUT_SPEC, spec_from_meta + +INPUT_SPEC = dict(MODEL_INPUT_SPEC) + +patch_tinygrad_fetch_fw() + +SEED = 42 + + +def set_input_spec(meta: dict) -> None: + spec = spec_from_meta(meta) + if spec is not None: + INPUT_SPEC.clear() + INPUT_SPEC.update(spec) + + +def make_run_model(model_runner): + def run_model(**inputs): + out = next(iter(model_runner({k: inputs[k] for k in INPUT_SPEC}).values())).cast("float32") + return out.reshape(-1), + return run_model + + +def _random_inputs(seed: int): + from tinygrad.device import Device + from tinygrad.tensor import Tensor + rng = np.random.default_rng(seed) + out = {} + for name, (shape, dtype) in INPUT_SPEC.items(): + if dtype == "uint8": + arr = rng.integers(0, 256, shape).astype(np.uint8) + else: + arr = rng.standard_normal(shape).astype(np.float32) + out[name] = Tensor(arr, device=Device.DEFAULT).realize() + return out + + +def _run(fn, seed: int) -> np.ndarray: + from tinygrad.device import Device + st = time.perf_counter() + outs = fn(**_random_inputs(seed)) + Device.default.synchronize() + print(f" run(seed={seed}) {(time.perf_counter() - st) * 1e3:6.1f} ms") + return outs[0].numpy().reshape(-1) + + +def compile_model(meta: dict, onnx_path: str, out_path: str) -> str: + from tinygrad.device import Device + from tinygrad.engine.jit import TinyJit + from tinygrad.nn.onnx import OnnxRunner + + if meta.get("split"): + raise RuntimeError(f"model {meta['key']} is a split model; eGPU v1 compiles fused models only") + + jit = TinyJit(make_run_model(OnnxRunner(onnx_path)), prune=True) + + print("capture + replay") + for _ in range(2): + baseline = _run(jit, SEED) + if baseline.shape[0] != meta["output_len"]: + raise RuntimeError(f"model output length {baseline.shape[0]} != registry {meta['output_len']}") + if not np.isfinite(baseline).all(): + raise RuntimeError("compiled model produced non-finite outputs") + + print("pickle round trip") + jit = pickle.loads(pickle.dumps(jit)) + if not np.array_equal(_run(jit, SEED), baseline): + raise RuntimeError("outputs differ from baseline after pickle round trip") + if np.array_equal(_run(jit, SEED + 1), baseline): + raise RuntimeError("outputs insensitive to inputs after pickle round trip") + + from tinygrad.tensor import Tensor + zeros = {name: Tensor(np.zeros(shape, dtype=dtype), device=Device.DEFAULT).realize() + for name, (shape, dtype) in INPUT_SPEC.items()} + flat = jit(**zeros)[0].numpy().reshape(-1) + from iqpilot.selfdrive.iqmodeld.parser import PhaseParser + from iqpilot.selfdrive.iqmodeld.tools.compile_supercombo import _slice_outputs, _validate_pose_outputs + _validate_pose_outputs(PhaseParser().parse_vision_outputs(_slice_outputs(flat, meta["output_slices"]))) + + bundle = { + "run_model": jit, + "model_key": meta["key"], + "model_sha256": meta["sha256"], + "output_len": int(meta["output_len"]), + "frame_skip": int(meta["frame_skip"]), + "input_spec": {name: (tuple(shape), dtype) for name, (shape, dtype) in INPUT_SPEC.items()}, + "input_device": Device.DEFAULT, + } + os.makedirs(os.path.dirname(out_path), exist_ok=True) + tmp = out_path + ".part" + with open(tmp, "wb") as f: + pickle.dump(bundle, f, protocol=pickle.HIGHEST_PROTOCOL) + os.replace(tmp, out_path) + return out_path + + +def main() -> None: + p = argparse.ArgumentParser() + p.add_argument("--model", default=None, help=f"registry key, one of {sorted(EGPU_MODELS)}") + p.add_argument("--onnx", default=None) + p.add_argument("--output", default=None) + args = p.parse_args() + + if args.model is not None: + if args.model in EGPU_MODELS: + meta = get_egpu_model(args.model) + else: + from iqpilot.common.params import Params + meta = resolve_egpu_model(Params(), args.model) + if meta is None: + raise SystemExit(f"unknown model {args.model!r}: not a built-in ({sorted(EGPU_MODELS)}) and not in the synced catalog") + else: + meta = get_egpu_model() + set_input_spec(meta) + + onnx_path = args.onnx or local_onnx(meta) + if onnx_path is None or not os.path.isfile(onnx_path): + raise SystemExit(f"onnx not found for {meta['key']}; pass --onnx or let iqegpumodeld download it first") + + out = compile_model(meta, onnx_path, args.output or egpu_pkl_path(meta)) + print(f"saved eGPU jit to {out} ({os.path.getsize(out) / 1e6:.2f} MB)") + + +if __name__ == "__main__": + main() diff --git a/iqpilot/selfdrive/iqmodeld/tools/compile_emac_warp.py b/iqpilot/selfdrive/iqmodeld/tools/compile_emac_warp.py new file mode 100644 index 000000000..4da3425e1 --- /dev/null +++ b/iqpilot/selfdrive/iqmodeld/tools/compile_emac_warp.py @@ -0,0 +1,9 @@ +""" +Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos/ +""" +from iqpilot.selfdrive.iqmodeld.tools.compile_warp import MODEL_SIZE, compile_warp, main + +__all__ = ["MODEL_SIZE", "compile_warp", "main"] + +if __name__ == "__main__": + main() diff --git a/iqpilot/selfdrive/iqmodeld/tools/compile_warp.py b/iqpilot/selfdrive/iqmodeld/tools/compile_warp.py new file mode 100644 index 000000000..07e4886eb --- /dev/null +++ b/iqpilot/selfdrive/iqmodeld/tools/compile_warp.py @@ -0,0 +1,119 @@ +#!/usr/bin/env python3 +""" +Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos/ + +Compile the backend-neutral warp-only artifact: NV12 camera frames + 3x3 +transforms -> (2, 6, model_h/2, model_w/2) uint8 warped tensor, on the device +GPU (QCOM). maciqmodeld runs this locally +and feed the output to their backend, so the big model's image pipeline is +bit-identical to comma's fused pkl warp stage. + +Run ON the device (needs the QCOM backend): + cd /data/openpilot && DEV=QCOM WARP_DEV=QCOM IMAGE=1 FLOAT16=1 NOLOCALS=1 JIT_BATCH_SIZE=0 \ + python3 iqpilot/selfdrive/iqmodeld/tools/compile_warp.py \ + --camera-resolutions 1928x1208 --output /data/models/emac_warp.pkl +The artifact is then split per-resolution into Paths.model_root(). +""" +from __future__ import annotations + +import argparse +import hashlib +import os +import pickle +from functools import partial + +import numpy as np + +SELFTEST_SEED = 20260817 + +from iqpilot.selfdrive.iqmodeld.temporal_state import DEFAULT_FRAME_SKIP, MODEL_INPUT_SPEC +from iqpilot.selfdrive.iqmodeld.tools.compile_supercombo import ( + NV12Frame, WARP_INPUTS, compile_jit, make_random_images, make_warp, make_warp_input_queues, +) + +MODEL_SIZE = (MODEL_INPUT_SPEC["img"][0][3] * 2, MODEL_INPUT_SPEC["img"][0][2] * 2) # (512, 256) + + +def _parse_size(s: str) -> tuple[int, int]: + w, h = s.lower().split("x") + return int(w), int(h) + + +def compile_warp(cam_w: int, cam_h: int, out_path: str | None = None, + frame_skip: int = DEFAULT_FRAME_SKIP) -> str: + """Compile the warp-only QCOM JIT for one camera resolution and write the pkl. + Returns the artifact path. Callable from the workers so a fresh device + self-provisions the warp instead of erroring — needs the QCOM backend.""" + # the QCOM warp env must be set before tinygrad is imported here + os.environ.setdefault("DEV", "QCOM") + os.environ.setdefault("WARP_DEV", "QCOM") + os.environ.setdefault("IMAGE", "1") + os.environ.setdefault("FLOAT16", "1") + os.environ.setdefault("NOLOCALS", "1") + os.environ.setdefault("JIT_BATCH_SIZE", "0") + from tinygrad.engine.jit import TinyJit + from iqpilot.system.camerad.cameras.nv12_info import get_nv12_info + from iqpilot.system.hardware.hw import Paths + + model_w, model_h = MODEL_SIZE + input_shapes = {name: shape for name, (shape, _) in MODEL_INPUT_SPEC.items()} + nv12 = NV12Frame(cam_w, cam_h, *get_nv12_info(cam_w, cam_h)) + make_random_warp_inputs = partial(make_random_images, keys=["frame", "big_frame"], + shape=nv12.size, device=os.getenv("WARP_DEV")) + warp_jit = TinyJit(make_warp(nv12, model_w, model_h, frame_skip), prune=True) + make_warp_queues = partial(make_warp_input_queues, input_shapes, frame_skip) + compiled = compile_jit(warp_jit, make_random_warp_inputs, WARP_INPUTS, make_warp_queues) + + # historical artifact name: already-provisioned devices keep their warp + out_path = out_path or os.path.join(Paths.model_root(), f"emac_warp_{cam_w}x{cam_h}_tinygrad.pkl") + os.makedirs(os.path.dirname(out_path), exist_ok=True) + tmp = out_path + ".part" + bundle = {(cam_w, cam_h): compiled, "frame_skip": frame_skip, "model_size": MODEL_SIZE} + bundle["selftest"] = selftest_digest(compiled, cam_w, cam_h, nv12.size) + with open(tmp, "wb") as f: + pickle.dump(bundle, f) + os.replace(tmp, out_path) # atomic: a reader never sees a half-written pkl + return out_path + + +def main() -> None: + p = argparse.ArgumentParser() + p.add_argument("--camera-resolutions", type=_parse_size, nargs="+", default=[(1928, 1208)]) + p.add_argument("--output", default=None) + p.add_argument("--frame-skip", type=int, default=DEFAULT_FRAME_SKIP) + args = p.parse_args() + for cam_w, cam_h in args.camera_resolutions: + out = compile_warp(cam_w, cam_h, args.output, frame_skip=args.frame_skip) + print(f"saved warp JIT to {out} ({os.path.getsize(out) / 1e6:.2f} MB)") + + +if __name__ == "__main__": + main() + + +def selftest_inputs(cam_w: int, cam_h: int, nv12_size: int): + """A fixed synthetic frame pair and pair of matrices. Deterministic so the + digest is reproducible on the device that compiled the artifact.""" + rng = np.random.default_rng(SELFTEST_SEED) + frame = rng.integers(0, 256, nv12_size, dtype=np.uint8) + big_frame = rng.integers(0, 256, nv12_size, dtype=np.uint8) + tfm = np.array([[0.7, 0.02, 300.0], [0.01, 0.7, 240.0], [0.0, 0.0, 1.0]], dtype=np.float32) + big_tfm = np.array([[0.5, 0.01, 380.0], [0.02, 0.5, 300.0], [0.0, 0.0, 1.0]], dtype=np.float32) + return frame, big_frame, tfm, big_tfm + + +def selftest_digest(compiled, cam_w: int, cam_h: int, nv12_size: int) -> str: + """Hash the warp's output for a fixed input. + + A warp artifact pinned to one tinygrad can still unpickle under another and + then compute silently wrong, which reaches the model as a garbage image and + looks like a bad model rather than a stale artifact. A version string cannot + see that; running it can.""" + from tinygrad.tensor import Tensor + frame, big_frame, tfm, big_tfm = selftest_inputs(cam_w, cam_h, nv12_size) + dev = os.getenv("WARP_DEV") or "QCOM" + out = compiled(tfm=Tensor(tfm, device="NPY").realize(), + big_tfm=Tensor(big_tfm, device="NPY").realize(), + frame=Tensor(frame, device=dev).realize(), + big_frame=Tensor(big_frame, device=dev).realize()) + return hashlib.sha256(out.numpy().astype(np.uint8).tobytes()).hexdigest() diff --git a/iqpilot/selfdrive/selfdrived/alerts_offroad.json b/iqpilot/selfdrive/selfdrived/alerts_offroad.json index 65a8577eb..6fcc7fe6f 100644 --- a/iqpilot/selfdrive/selfdrived/alerts_offroad.json +++ b/iqpilot/selfdrive/selfdrived/alerts_offroad.json @@ -54,5 +54,33 @@ "text": "Unsupported branch! - The current version of %1 is not marked as compatible with the Comma Three (3|tici). Please go to [Device > Software] and install a supported branch such as release or beta for the comma three.", "severity": 1, "_comment": "Set extra field to the current branch name." + }, + "Offroad_EgpuNotDetected": { + "text": "eGPU dock not detected. Check USB and 12V connections.", + "severity": 0 + }, + "Offroad_EgpuFansObstructed": { + "text": "eGPU dock fans obstructed. Check the fans.", + "severity": 0 + }, + "Offroad_EgpuOverheated": { + "text": "eGPU dock overheated. Allow it to cool.", + "severity": 0 + }, + "Offroad_EgpuPcieUnavailable": { + "text": "eGPU dock PCIe unavailable. %1", + "severity": 0 + }, + "Offroad_EgpuUncompiled": { + "text": "eGPU big model not compiled. Keep ignition on and reboot the device.", + "severity": 0 + }, + "Offroad_EgpuUpdateFailed": { + "text": "eGPU dock update failed. Check the USB cable.", + "severity": 0 + }, + "Offroad_EgpuUsbSlow": { + "text": "eGPU dock USB link is slow. Check the USB cable. The current speed is %1.", + "severity": 0 } } diff --git a/iqpilot/selfdrive/selfdrived/events.py b/iqpilot/selfdrive/selfdrived/events.py index 2e36bfe34..60a37668e 100755 --- a/iqpilot/selfdrive/selfdrived/events.py +++ b/iqpilot/selfdrive/selfdrived/events.py @@ -253,6 +253,15 @@ EVENTS: dict[int, dict[str, Alert | AlertCallbackType]] = { "Ensure road ahead is clear"), }, + EventName.bigModelLoading: { + ET.NO_ENTRY: NoEntryAlert("Big Model Loading"), + }, + + EventName.bigModelFailed: { + ET.SOFT_DISABLE: soft_disable_alert("Big Model Failed"), + ET.PERMANENT: NormalPermanentAlert("Big Model Failed ", "Restart the car to retry,\nsmall model is still available", duration=20.), + }, + EventName.lateralManeuver: { ET.WARNING: longitudinal_maneuver_alert, ET.PERMANENT: NormalPermanentAlert("Lateral Maneuver Mode"), diff --git a/iqpilot/selfdrive/selfdrived/selfdrived.py b/iqpilot/selfdrive/selfdrived/selfdrived.py index 10176b9ac..6d861d37f 100755 --- a/iqpilot/selfdrive/selfdrived/selfdrived.py +++ b/iqpilot/selfdrive/selfdrived/selfdrived.py @@ -143,6 +143,9 @@ class SelfdriveD(GapButtonActions): self.events = Events() self.initialized = False + self.big_model_loading = False + self.big_model_active = False + self.big_model_failed = False self.enabled = False self.active = False self.mismatch_counter = 0 @@ -270,6 +273,28 @@ class SelfdriveD(GapButtonActions): self.events.add(EventName.joystickDebug) self.startup_event = None + if self.sm['deviceState'].egpuDockPresent or self.params.get_bool("IQEgpuEnabled") or self.big_model_active: + loading = self.params.get_bool("UsbGpuLoading") + self.big_model_loading = loading + if self.big_model_loading: + self.events.add(EventName.bigModelLoading) + + big_active = self.params.get("UsbGpuActive") + dock_present = self.sm['deviceState'].egpuDockPresent + mac_active = self.params.get_bool("MacModelActive") + model_unavailable = big_active is True and self.sm.seen['modelV2'] and not self.sm.alive['modelV2'] + big_failed = (big_active is False or model_unavailable + or (self.big_model_active and not dock_present)) and not mac_active + if big_failed: + self.events.add(EventName.bigModelFailed) + self.big_model_failed = big_failed + + # soft disable if the big model fails + if big_active: + self.big_model_active = True + if mac_active or (not self.enabled and not model_unavailable): + self.big_model_active = False + if self.sm.recv_frame['lateralManeuverPlan'] > 0: self.events.add(EventName.lateralManeuver) self.startup_event = None diff --git a/iqpilot/selfdrive/test/process_replay/migration.py b/iqpilot/selfdrive/test/process_replay/migration.py index 09038a748..a32bc5485 100644 --- a/iqpilot/selfdrive/test/process_replay/migration.py +++ b/iqpilot/selfdrive/test/process_replay/migration.py @@ -135,7 +135,9 @@ def migrate_drivingModelData(msgs): setattr(dmd.drivingModelData, field, getattr(msg.modelV2, field)) for meta_field in ["laneChangeState", "laneChangeState"]: setattr(dmd.drivingModelData.meta, meta_field, getattr(msg.modelV2.meta, meta_field)) - if len(msg.modelV2.laneLines) and len(msg.modelV2.laneLineProbs): + lane_lines = msg.modelV2.laneLines + lane_probs = msg.modelV2.laneLineProbs + if len(lane_lines) > 2 and len(lane_probs) > 2 and len(lane_lines[1].y) and len(lane_lines[2].y): fill_lane_line_meta(dmd.drivingModelData.laneLineMeta, msg.modelV2.laneLines, msg.modelV2.laneLineProbs) if all(len(a) for a in [msg.modelV2.position.x, msg.modelV2.position.y, msg.modelV2.position.z]): fill_xyz_poly(dmd.drivingModelData.path, ModelConstants.POLY_PATH_DEGREE, msg.modelV2.position.x, msg.modelV2.position.y, msg.modelV2.position.z) diff --git a/iqpilot/selfdrive/test/process_replay/test_migration.py b/iqpilot/selfdrive/test/process_replay/test_migration.py new file mode 100644 index 000000000..68261faea --- /dev/null +++ b/iqpilot/selfdrive/test/process_replay/test_migration.py @@ -0,0 +1,19 @@ +""" +Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos +""" +from iqpilot.cereal import messaging +from iqpilot.selfdrive.test.process_replay.migration import migrate_drivingModelData + + +def test_driving_model_migration_ignores_incomplete_lane_metadata(): + msg = messaging.new_message("modelV2") + msg.modelV2.init("laneLines", 4) + for lane_line in msg.modelV2.laneLines: + lane_line.y = [1.0] + msg.modelV2.laneLineProbs = [0.5] + + _, added, _ = migrate_drivingModelData([(0, msg.as_reader())]) + + assert len(added) == 1 + assert added[0].drivingModelData.laneLineMeta.leftProb == 0.0 + assert added[0].drivingModelData.laneLineMeta.rightProb == 0.0 diff --git a/iqpilot/system/hardware/egpu_dock/TESTING.md b/iqpilot/system/hardware/egpu_dock/TESTING.md new file mode 100644 index 000000000..38542e401 --- /dev/null +++ b/iqpilot/system/hardware/egpu_dock/TESTING.md @@ -0,0 +1,47 @@ +# eGPU dock bring-up runbook + +Our flasher is a port of comma's known-working one, byte-identical firmware +bundle, but it has never touched real hardware. Order matters: everything +read-only first, evidence at every step. + +Run everything as root from the repo root on the device, dock on the USB-C +port, car ignition off. + +## 1. Read-only probe (safe, run first, share the output) + + sudo python3 iqpilot/system/hardware/egpu_dock/dock_probe.py + +Expected on a dock previously flashed by stock openpilot: product matches the +bundled `custom ed4e39b7-CLEAN`, USB3 speed, PCIe link L0, stable config read. +Any other result: stop and send the output before proceeding. + +## 2. Flash-path validation (writes, but writes the same bytes) + +A stock-flashed dock already runs our exact bundled firmware, so the +no-op path proves version detection: + + sudo python3 iqpilot/system/hardware/egpu_dock/flash.py + +Expected: "firmware matches" and no write. Then exercise the full write path +by reflashing the identical image: + + sudo python3 iqpilot/system/hardware/egpu_dock/flash.py --force + +This backs up the per-unit config page to /data/egpu_dock_config/ first and +verifies every sector; identical bytes make it the lowest-risk possible +full-path test. Re-run step 1 after; product string and config sha must be +unchanged. + +## 3. Runtime + +Set `IQEgpuEnabled`, go onroad (bench is fine), and confirm iqegpumodeld +downloads/compiles and the selector reports UsbGpu* status. The runtime gate +requires the exact bundled firmware product string, so a dock that failed +step 2 will be treated as absent by design. + +## If anything goes wrong + +The dock falling back to the ROM bootloader (product "USB 3.2 PCIe +TinyEnclosure" or AS2462*) is recoverable: flash.py handles ROM recovery, and +the config backup from step 2 is on disk. Do not improvise register writes; +capture output and stop. diff --git a/iqpilot/system/hardware/egpu_dock/__init__.py b/iqpilot/system/hardware/egpu_dock/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/iqpilot/system/hardware/egpu_dock/dock_probe.py b/iqpilot/system/hardware/egpu_dock/dock_probe.py new file mode 100644 index 000000000..748e01e21 --- /dev/null +++ b/iqpilot/system/hardware/egpu_dock/dock_probe.py @@ -0,0 +1,46 @@ +""" +Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos/ +""" +import hashlib +import sys + +from iqpilot.system.hardware.egpu_dock.flash import ( + Flash, RomFallback, bundled_version, find_dock, in_rom_bootloader, link_up, stable_read, +) + + +def main() -> int: + path, vid_pid, product = find_dock() + if path is None: + print("no eGPU dock enumerated") + return 1 + print(f"dock at {path}") + print(f" vid:pid {vid_pid[0]}:{vid_pid[1]}") + print(f" product {product!r}") + print(f" bundled {bundled_version()!r}") + print(f" match {product == bundled_version()}") + with open(path + "/speed") as fs: + speed = int(fs.read()) + print(f" usb speed {speed} Mbps ({'USB3' if speed >= 5000 else 'USB2 - register reads capped at 64B'})") + if in_rom_bootloader(vid_pid, product): + print(" state ROM bootloader (config page lost or firmware invalid)") + return 2 + print(f" pcie link {'L0 (trained)' if link_up() else 'not trained'}") + + flash = Flash() + try: + flash.connect() + config = stable_read(flash, 0, 0x100, 3) + print(f" config sha256={hashlib.sha256(config).hexdigest()[:16]} " + f"(stable over 3 reads, {sum(1 for b in config if b != 0xFF)} non-blank bytes)") + except (RomFallback, OSError, RuntimeError, TimeoutError) as e: + print(f" config read failed: {type(e).__name__}: {e}") + return 3 + finally: + flash.close() + print("all read-only checks passed") + return 0 + + +if __name__ == "__main__": + sys.exit(main()) diff --git a/iqpilot/system/hardware/egpu_dock/firmware_wrapped.bin b/iqpilot/system/hardware/egpu_dock/firmware_wrapped.bin new file mode 100644 index 000000000..1d738e4a7 Binary files /dev/null and b/iqpilot/system/hardware/egpu_dock/firmware_wrapped.bin differ diff --git a/iqpilot/system/hardware/egpu_dock/flash.py b/iqpilot/system/hardware/egpu_dock/flash.py new file mode 100644 index 000000000..263a42dd0 --- /dev/null +++ b/iqpilot/system/hardware/egpu_dock/flash.py @@ -0,0 +1,617 @@ +""" +Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos/ +""" +import argparse +import ctypes +import errno +import fcntl +import glob +import hashlib +import os +import re +import signal +import struct +import sys +import time +import zlib +from pathlib import Path + +VID_PIDS = (("add1", "0001"), ("3801", "0001")) +ROM_VID_PIDS = (("174c", "2464"), ("174c", "2463")) +ROM_PRODUCT = "USB 3.2 PCIe TinyEnclosure" +FIRMWARE_PATH = Path(__file__).with_name("firmware_wrapped.bin") +CONFIG_DIR = "/data/egpu_dock_config" +LEGACY_CONFIG_DIR = "/data/chestnut_config" +PM_PATHS = ("/sys/bus/platform/devices/a800000.ssusb", "/sys/bus/platform/devices/a600000.ssusb", + "/sys/bus/usb/devices/usb4") +VBUS_PATH = "/sys/kernel/debug/regulator/smb2-vbus/enable" +IMAGE_OFFSET = 0x100 +SECTOR, PAGE = 4096, 128 +MAX_REGISTER_READ_SIZE = 255 +MAX_CODE_SIZE = 0x10000 +FLASH_BUDGET = 600.0 +USBDEVFS_CONTROL = 0xC0185500 +USBDEVFS_BULK = 0xC0185502 +USBDEVFS_SETINTERFACE = 0x80085504 +USBDEVFS_SETCONFIGURATION = 0x80045505 +USBDEVFS_CLAIMINTERFACE = 0x8004550F +USBDEVFS_RESET = 0x5514 +USBDEVFS_CLEAR_HALT = 0x80045515 + +_deadline = float("inf") + + +def check_budget(): + if time.monotonic() > _deadline: + raise TimeoutError(f"flash did not converge within {FLASH_BUDGET:g}s") + + +class Ctrl(ctypes.Structure): + _fields_ = [("request_type", ctypes.c_uint8), ("request", ctypes.c_uint8), + ("value", ctypes.c_uint16), ("index", ctypes.c_uint16), + ("length", ctypes.c_uint16), ("timeout", ctypes.c_uint32), + ("data", ctypes.c_void_p)] + + +class Bulk(ctypes.Structure): + _fields_ = [("ep", ctypes.c_uint), ("len", ctypes.c_uint), + ("timeout", ctypes.c_uint), ("data", ctypes.c_void_p)] + + +class RomFallback(Exception): + pass + + +def find_dock(): + found = [] + for d in glob.glob("/sys/bus/usb/devices/*"): + try: + with open(d + "/idVendor") as fv, open(d + "/idProduct") as fp: + vid_pid = (fv.read().strip(), fp.read().strip()) + if vid_pid in VID_PIDS + ROM_VID_PIDS: + with open(d + "/product") as fpr: + found.append((d, vid_pid, fpr.read().strip())) + except OSError: + pass + if len(found) > 1: + raise RuntimeError(f"expected one eGPU dock, found {len(found)}") + return found[0] if found else (None, None, None) + + +def in_rom_bootloader(vid_pid, product): + return vid_pid in ROM_VID_PIDS or product == ROM_PRODUCT or (product or "").startswith("AS2462") + + +def disable_runtime_pm(path): + control = os.path.join(path, "power/control") + if not os.path.exists(control): + return + with open(control, "w") as f: + f.write("on\n") + with open(control) as fh: + applied = fh.read().strip() + if applied != "on": + raise RuntimeError(f"could not disable USB runtime PM: {control}") + delay = os.path.join(path, "power/autosuspend_delay_ms") + if os.path.exists(delay): + with open(delay, "w") as f: + f.write("-1\n") + + +def unbind_drivers(path): + for interface in glob.glob(path + ":*"): + driver = interface + "/driver" + if os.path.islink(driver): + with open(os.path.realpath(driver) + "/unbind", "w") as f: + f.write(os.path.basename(interface)) + + +def open_device(path): + with open(path + "/busnum") as fb, open(path + "/devnum") as fd_: + bus, dev = int(fb.read()), int(fd_.read()) + return os.open(f"/dev/bus/usb/{bus:03d}/{dev:03d}", os.O_RDWR) + + +def link_up() -> bool: + try: + path, _, _ = find_dock() + if path is None: + return False + fd = open_device(path) + except (OSError, RuntimeError): + return False + try: + fcntl.ioctl(fd, USBDEVFS_CONTROL, Ctrl(0x40, 0xF3, 1, 0, 0, 2000, None)) + buf = (ctypes.c_ubyte * 1)() + fcntl.ioctl(fd, USBDEVFS_CONTROL, Ctrl(0xC0, 0xE4, 0xB450, 0, 1, 1000, ctypes.cast(buf, ctypes.c_void_p))) + return buf[0] == 0x78 + except OSError: + return False + finally: + os.close(fd) + + +def claim_interface(path, setup=False): + disable_runtime_pm(path) + unbind_drivers(path) + fd = open_device(path) + try: + if setup: + fcntl.ioctl(fd, USBDEVFS_SETCONFIGURATION, struct.pack("I", 1)) + fcntl.ioctl(fd, USBDEVFS_CLAIMINTERFACE, struct.pack("I", 0)) + if setup: + fcntl.ioctl(fd, USBDEVFS_SETINTERFACE, struct.pack("II", 0, 0)) + except OSError as e: + os.close(fd) + if e.errno == errno.EBUSY: + raise RuntimeError("eGPU dock is in use, stop the model/GPU processes before flashing") from e + raise + return fd + + +class Flash: + def __init__(self): + self.fd = -1 + self.max_register_read_size = MAX_REGISTER_READ_SIZE + + def close(self): + if self.fd >= 0: + os.close(self.fd) + self.fd = -1 + + def connect(self, timeout=5.0): + self.close() + deadline = time.monotonic() + timeout + while time.monotonic() < deadline: + path, vid_pid, product = find_dock() + if in_rom_bootloader(vid_pid, product): + raise RomFallback("eGPU dock fell back to the ROM bootloader") + if path is not None: + try: + with open(path + "/speed") as fs: + speed = int(fs.read()) + except (OSError, ValueError): + speed = 0 + self.max_register_read_size = 64 if speed < 5000 else MAX_REGISTER_READ_SIZE + self.fd = claim_interface(path) + return + time.sleep(0.1) + raise RuntimeError(f"eGPU dock did not enumerate within {timeout:g}s") + + def reg_write(self, addr, value): + fcntl.ioctl(self.fd, USBDEVFS_CONTROL, + Ctrl(0x40, 0xE5, addr & 0xFFFF, value & 0xFFFF, 0, 2000, None)) + + def reg_read(self, addr, length=1): + buf = (ctypes.c_ubyte * length)() + fcntl.ioctl(self.fd, USBDEVFS_CONTROL, + Ctrl(0xC0, 0xE4, addr & 0xFFFF, 0, length, 2000, ctypes.cast(buf, ctypes.c_void_p))) + return bytes(buf) + + def write_buffer(self, data): + for i, value in enumerate(data): + self.reg_write(0x7000 + i, value) + + def wait_controller(self, timeout=2.0): + deadline = time.monotonic() + timeout + while time.monotonic() < deadline: + if not self.reg_read(0xC8A9)[0] & 1: + return + raise TimeoutError("flash controller timeout") + + def transaction(self, command, addr=0, length=0, addr_len=0x07, mode=0): + for reg, value in ((0xC8AD, mode), (0xC8AE, 0), (0xC8AF, 0), (0xC8AA, command), (0xC8AC, addr_len), + (0xC8A1, addr), (0xC8A2, addr >> 8), (0xC8AB, addr >> 16), (0xC8A3, length >> 8), (0xC8A4, length)): + self.reg_write(reg, value & 0xFF) + self.reg_write(0xC8A9, 1) + self.wait_controller() + for _ in range(4): + self.reg_write(0xC8AD, 0) + + def write_enable(self): + for reg, value in ((0xC8AD, 0), (0xC8AA, 0x06), (0xC8AC, 0x04), (0xC8A3, 0), (0xC8A4, 0), (0xC8A9, 1)): + self.reg_write(reg, value) + self.wait_controller() + + def status(self): + self.transaction(0x05, length=1, addr_len=0x04) + return self.reg_read(0x7000)[0] + + def wait_write_done(self, timeout=10.0): + deadline = time.monotonic() + timeout + while time.monotonic() < deadline: + if not self.status() & 1: + return + time.sleep(0.005) + raise TimeoutError("SPI flash WIP timeout") + + def init(self): + self.reg_write(0xCC33, 0x04) + self.reg_write(0xCA81, self.reg_read(0xCA81)[0] | 1) + self.reg_write(0xC805, 0x02) + self.reg_write(0xC8A6, 0x04) + for _ in range(5): + self.write_enable() + self.write_buffer(bytes(4)) + self.transaction(0x01, length=1, addr_len=0x04, mode=1) + time.sleep(0.01) + if not self.status() & 0x1C: + return + raise RuntimeError("could not clear SPI block protection") + + def read(self, addr, length): + out = bytearray() + while len(out) < length: + n = min(4096, length - len(out)) + self.transaction(0x03, addr + len(out), max(4096, n)) + for off in range(0, n, self.max_register_read_size): + out += self.reg_read(0x7000 + off, min(self.max_register_read_size, n - off)) + return bytes(out) + + def erase_sector(self, addr): + self.write_enable() + self.transaction(0x20, addr) + self.wait_write_done() + + def program(self, addr, data): + self.write_buffer(data + bytes((-len(data)) % 4)) + self.write_enable() + self.transaction(0x02, addr, len(data), mode=1) + self.wait_write_done() + + +def validate_image(data): + if len(data) < 10: + raise ValueError("wrapped firmware is too short") + body_len = int.from_bytes(data[:4], "little") + if body_len > MAX_CODE_SIZE: + raise ValueError(f"wrapped firmware body exceeds {MAX_CODE_SIZE} bytes") + if len(data) != body_len + 10 or data[4 + body_len] != 0xA5: + raise ValueError("invalid wrapped firmware length or magic") + body = data[4:4 + body_len] + if data[5 + body_len] != sum(body) & 0xFF: + raise ValueError("invalid wrapped firmware checksum") + if data[6 + body_len:] != zlib.crc32(body).to_bytes(4, "little"): + raise ValueError("invalid wrapped firmware CRC") + + +def image_product(image): + match = re.search(rb"custom [0-9a-f]{8}-CLEAN", image) + if match is None: + raise ValueError("no product string in wrapped firmware") + return match.group().decode() + + +def reconnect(flash): + attempt = 0 + while True: + attempt += 1 + check_budget() + try: + flash.connect() + flash.init() + return + except (OSError, TimeoutError, RuntimeError) as e: + print(f"waiting for eGPU dock (attempt {attempt}): {e}", flush=True) + time.sleep(1) + + +def with_retries(flash, label, operation): + attempt = 0 + while True: + attempt += 1 + try: + return operation() + except (OSError, TimeoutError, RuntimeError) as e: + check_budget() + print(f"{label} attempt {attempt}: {e}", flush=True) + reconnect(flash) + + +def stable_read(flash, addr, length, count=2): + def read(): + reads = [flash.read(addr, length) for _ in range(count)] + if any(x != reads[0] for x in reads[1:]): + raise RuntimeError(f"unstable flash read at 0x{addr:05x}") + return reads[0] + return with_retries(flash, f"read 0x{addr:05x}", read) + + +def program_sector(flash, addr, target): + def program(): + flash.erase_sector(addr) + if flash.read(addr, SECTOR) != bytes([0xFF]) * SECTOR: + raise RuntimeError("sector erase verification failed") + for off in range(0, SECTOR, PAGE): + chunk = target[off:off + PAGE] + if chunk != bytes([0xFF]) * len(chunk): + flash.program(addr + off, chunk) + if flash.read(addr + off, len(chunk)) != chunk: + raise RuntimeError(f"page verify failed at 0x{addr + off:05x}") + if flash.read(addr, SECTOR) != target: + raise RuntimeError("sector verification failed") + with_retries(flash, f"sector 0x{addr:05x}", program) + + +def config_path(): + return os.path.join(CONFIG_DIR, f"{os.uname().nodename}.bin") + + +def legacy_config_path(): + return os.path.join(LEGACY_CONFIG_DIR, f"{os.uname().nodename}.bin") + + +def saved_config(path, data): + os.makedirs(os.path.dirname(path), exist_ok=True) + try: + fd = os.open(path, os.O_WRONLY | os.O_CREAT | os.O_EXCL, 0o600) + except FileExistsError as e: + with open(path, "rb") as fh: + backup = fh.read() + if len(backup) != 0x100: + raise RuntimeError(f"invalid config backup: {path}") from e + if backup != data: + print(f"restoring config from {path}", flush=True) + return backup + with os.fdopen(fd, "wb") as f: + f.write(data) + f.flush() + os.fsync(f.fileno()) + return data + + +def rom_write(image, config): + path, _, _ = find_dock() + if path is None: + raise RuntimeError("eGPU dock disappeared before recovery") + unbind_drivers(path) + fd = open_device(path) + try: + fcntl.ioctl(fd, USBDEVFS_RESET) + finally: + os.close(fd) + time.sleep(3) + path, _, _ = find_dock() + if path is None: + raise RuntimeError("eGPU dock did not re-enumerate after reset") + fd = claim_interface(path, setup=True) + for ep in (0x02, 0x81): + fcntl.ioctl(fd, USBDEVFS_CLEAR_HALT, struct.pack("I", ep)) + tag = 0 + + def bulk(ep, payload, timeout): + buf = ctypes.create_string_buffer(bytes(payload), len(payload)) + fcntl.ioctl(fd, USBDEVFS_BULK, Bulk(ep, len(payload), timeout, ctypes.cast(buf, ctypes.c_void_p))) + return buf.raw + + def cmd(cdb, data=b"", timeout=30000): + nonlocal tag + tag += 1 + bulk(0x02, struct.pack("BBB12x", 0xE1, 0x50, 0), config[:0x80]) + cmd(struct.pack(">BBB12x", 0xE1, 0x50, 1), config[0x80:]) + cmd(struct.pack(">BBI", 0xE3, 0x50, min(len(image), 0xFF00)), image[:0xFF00]) + if len(image) > 0xFF00: + cmd(struct.pack(">BBI", 0xE3, 0xD0, len(image) - 0xFF00), image[0xFF00:]) + cmd(struct.pack(">BB13x", 0xE8, 0x51)) + finally: + os.close(fd) + print("recovery flash done", flush=True) + + +def vbus_write(value): + try: + with open(VBUS_PATH, "w") as f: + f.write(value + "\n") + except OSError: + pass + + +def vbus_cycle(): + if os.path.exists(VBUS_PATH): + vbus_write("0") + time.sleep(2) + vbus_write("1") + time.sleep(5) + + +def activate(expected_product): + if not os.path.exists(VBUS_PATH): + print("no VBUS control, firmware activates on the next dock power cycle", flush=True) + return + print("power-cycling the eGPU dock VBUS", flush=True) + vbus_write("0") + disconnected = False + deadline = time.monotonic() + 5.0 + while time.monotonic() < deadline: + path, _, _ = find_dock() + if path is None: + disconnected = True + break + time.sleep(0.2) + time.sleep(1) + vbus_write("1") + if not disconnected: + print("dock stayed powered, firmware activates on its next power cycle", flush=True) + return + deadline = time.monotonic() + 15.0 + while time.monotonic() < deadline: + _, _, product = find_dock() + if product is not None: + if product == expected_product: + print(f"activated {expected_product}", flush=True) + else: + print(f"dock re-enumerated with {product!r}, firmware activates on its next power cycle", flush=True) + return + time.sleep(0.2) + print("dock did not re-enumerate, firmware activates on its next power cycle", flush=True) + + +def defer_signal(signum, _frame): + os.write(1, f"signal {signum} deferred until the dock is powered back up\n".encode()) + + +def flash_dock(expected_version=None, force=False): + global _deadline + + image = FIRMWARE_PATH.read_bytes() + validate_image(image) + expected_product = image_product(image) + if expected_version is not None and expected_product != f"custom {expected_version}-CLEAN": + raise RuntimeError(f"bundled firmware is {expected_product!r}, expected version {expected_version}") + + path, vid_pid, product = find_dock() + if path is None: + print("no eGPU dock connected", flush=True) + return + if product == expected_product and not force: + print(f"eGPU dock firmware is up to date ({expected_product})", flush=True) + return + + _deadline = time.monotonic() + FLASH_BUDGET + for pm_path in PM_PATHS: + disable_runtime_pm(pm_path) + + previous = {sig: signal.signal(sig, defer_signal) for sig in (signal.SIGINT, signal.SIGTERM, signal.SIGHUP)} + try: + if in_rom_bootloader(vid_pid, product): + if not recover_from_rom(image, expected_product): + return + force, product = True, None + write_image(image, expected_product, product, force) + finally: + for sig, handler in previous.items(): + signal.signal(sig, handler) + + +def recover_from_rom(image, expected_product): + backup = config_path() + if not os.path.isfile(backup) and os.path.isfile(legacy_config_path()): + backup = legacy_config_path() + if not os.path.isfile(backup): + raise RuntimeError(f"cannot recover from the ROM bootloader without a config backup at {config_path()}") + with open(backup, "rb") as fh: + config = fh.read() + if len(config) != 0x100: + raise RuntimeError(f"invalid config backup: {backup}") + + committed = False + while True: + check_budget() + path, vid_pid, product = find_dock() + if path is None: + if committed: + print("dock is offline, recovered firmware boots on its next power cycle", flush=True) + return False + vbus_cycle() + continue + if not in_rom_bootloader(vid_pid, product): + return True + if committed: + print("dock stayed powered, recovered firmware boots on its next power cycle", flush=True) + return False + try: + rom_write(image, config) + committed = True + except (OSError, TimeoutError, RuntimeError) as e: + print(f"ROM recovery failed, retrying: {e}", flush=True) + vbus_cycle() + continue + activate(expected_product) + + +def write_image(image, expected_product, product, force): + if force: + print(f"forced reflash of {expected_product}", flush=True) + else: + print(f"eGPU dock firmware mismatch: {product!r}; expected {expected_product!r}", flush=True) + + flash = Flash() + try: + reconnect(flash) + config = stable_read(flash, 0, 0x100, 3) + config = saved_config(config_path(), config) + image_end = IMAGE_OFFSET + len(image) + first_sector = IMAGE_OFFSET & ~(SECTOR - 1) + span = (image_end + SECTOR - 1) & ~(SECTOR - 1) + current = stable_read(flash, first_sector, span - first_sector) + target = bytearray(current) + target[:len(config)] = config + target[IMAGE_OFFSET - first_sector:image_end - first_sector] = image + target = bytes(target) + print(f"target {len(image)} bytes at 0x{IMAGE_OFFSET:05x}, sha256={hashlib.sha256(image).hexdigest()}", flush=True) + + if not _still_offroad(): + raise RuntimeError("device went onroad before any sector was written; aborting flash") + + for addr in range(first_sector, span, SECTOR): + off = addr - first_sector + wanted = target[off:off + SECTOR] + if current[off:off + SECTOR] == wanted: + print(f"sector 0x{addr:05x}: unchanged", flush=True) + else: + print(f"sector 0x{addr:05x}: programming", flush=True) + program_sector(flash, addr, wanted) + + verified = stable_read(flash, first_sector, span - first_sector, 3) + if verified != target: + raise RuntimeError("final full-image verification failed") + print(f"verified sha256={hashlib.sha256(verified).hexdigest()}", flush=True) + finally: + flash.close() + + activate(expected_product) + + +def bundled_version() -> str: + return image_product(FIRMWARE_PATH.read_bytes()) + + +def _still_offroad() -> bool: + try: + from iqpilot.common.params import Params + return bool(Params().get_bool("IsOffroad")) + except Exception: + return True + + +def dock_needs_flash(usb_devices: list[dict]) -> bool: + try: + expected = bundled_version() + except (OSError, ValueError): + return False + ids = tuple(tuple(int(x, 16) for x in p) for p in VID_PIDS + ROM_VID_PIDS) + return any((d.get("vendorId"), d.get("productId")) in ids and d.get("product") != expected + for d in usb_devices) + + +def main(): + parser = argparse.ArgumentParser(description="check and flash the bundled eGPU dock firmware") + parser.add_argument("version", nargs="?", help="expected firmware version hash") + parser.add_argument("--force", action="store_true", help="reflash even when the version matches") + args = parser.parse_args() + if os.geteuid() != 0: + raise RuntimeError("flash.py must run as root") + flash_dock(expected_version=args.version, force=args.force) + + +if __name__ == "__main__": + try: + main() + except Exception as e: + print(f"FAIL: {type(e).__name__}: {e}", file=sys.stderr) + sys.exit(1) diff --git a/iqpilot/system/hardware/egpu_dock/status.py b/iqpilot/system/hardware/egpu_dock/status.py new file mode 100644 index 000000000..97077776f --- /dev/null +++ b/iqpilot/system/hardware/egpu_dock/status.py @@ -0,0 +1,89 @@ +""" +Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos/ +""" +import time + +from iqpilot.system.hardware.usb import EGPU_DOCK_FW_PRODUCT, is_egpu_usb_device + +EGPU_POWERED_VOLTAGE = 5000 +GPU_TEMP_LIMIT = 110. +MEMORY_TEMP_LIMIT = 108. +TEMP_HYSTERESIS = 5. +FAN_START_GPU_TEMP = 60. +FAN_STOP_GPU_TEMP = 50. +FAN_START_MEMORY_TEMP = 70. +FAN_STOP_MEMORY_TEMP = 60. +FAN_STALLED_RPM = 250 + + +class EgpuDockStatus: + def __init__(self): + self.offroad = True + self.pcie_failed = False + self.power_lost = False + self.power_restored = False + self.link_failures = 0 + self.model_loading_seen = False + self.model_attempted = False + self.overheated = False + self.fans_obstructed = False + self.usb_seen = False + self.usb_failed = False + + def update(self, offroad, usb_state, firmware_failed, model_loading, model_active, compiled, state, set_alert): + detected = [d for d in usb_state if is_egpu_usb_device(d["vendorId"], d["productId"], include_bootloader=True)] + devices = [d for d in detected if is_egpu_usb_device(d["vendorId"], d["productId"])] + firmware_ok = len(devices) == 1 and devices[0]["product"] == EGPU_DOCK_FW_PRODUCT + + if self.offroad and not offroad: + self.pcie_failed = False + self.power_lost = False + self.power_restored = False + self.link_failures = 0 + self.model_loading_seen = False + self.model_attempted = False + self.usb_seen = firmware_ok + self.usb_failed = False + + self.model_loading_seen |= model_loading + self.model_attempted |= self.model_loading_seen and not model_loading and model_active is not None + + if not offroad and self.usb_seen and not firmware_ok: + self.usb_failed = True + + if not offroad and self.model_attempted and state is not None: + power_lost = state.supplyFault or state.supplyVoltage < EGPU_POWERED_VOLTAGE + self.link_failures = self.link_failures + 1 if state.pcieLtssm != 0x78 else 0 + self.pcie_failed |= self.link_failures >= 2 or power_lost + self.power_lost |= power_lost + + if self.pcie_failed and self.power_lost and state is not None: + self.power_restored |= not state.supplyFault and state.supplyVoltage >= EGPU_POWERED_VOLTAGE + if self.usb_failed: + self.pcie_failed = False + self.power_lost = False + self.power_restored = False + + if state is not None: + gpu_limit = GPU_TEMP_LIMIT - (TEMP_HYSTERESIS if self.overheated else 0.) + memory_limit = MEMORY_TEMP_LIMIT - (TEMP_HYSTERESIS if self.overheated else 0.) + self.overheated = state.tempC >= gpu_limit or state.memoryTempC >= memory_limit + fan_hot = (state.tempC >= (FAN_STOP_GPU_TEMP if self.fans_obstructed else FAN_START_GPU_TEMP) or + state.memoryTempC >= (FAN_STOP_MEMORY_TEMP if self.fans_obstructed else FAN_START_MEMORY_TEMP)) + self.fans_obstructed = fan_hot and state.fanSpeedRpm < FAN_STALLED_RPM + + slow_usb = offroad and len(devices) == 1 and devices[0]["speedMbps"] < 5000 + set_alert("Offroad_EgpuNotDetected", self.usb_failed) + set_alert("Offroad_EgpuFansObstructed", self.fans_obstructed) + set_alert("Offroad_EgpuOverheated", self.overheated) + set_alert("Offroad_EgpuUsbSlow", slow_usb, f"{devices[0]['speedMbps']} Mbps" if slow_usb else None) + if self.power_lost: + pcie_action = "12V power was interrupted, possibly by engine start-stop. " + pcie_action += ("Cycle ignition to reload the model." if self.power_restored else + "Check 12V, then cycle ignition to reload the model.") + else: + pcie_action = "Check 12V connection." + set_alert("Offroad_EgpuPcieUnavailable", self.pcie_failed, pcie_action) + set_alert("Offroad_EgpuUncompiled", offroad and firmware_ok and not compiled) + set_alert("Offroad_EgpuUpdateFailed", offroad and firmware_failed) + self.offroad = offroad diff --git a/iqpilot/system/hardware/fan_controller.py b/iqpilot/system/hardware/fan_controller.py index e4560bfb2..cc66c83e5 100755 --- a/iqpilot/system/hardware/fan_controller.py +++ b/iqpilot/system/hardware/fan_controller.py @@ -3,24 +3,13 @@ import numpy as np class FanController: - def __init__(self) -> None: - self.last_ignition = False - def update(self, cur_temp: float, ignition: bool, max_cool: bool = False) -> int: if max_cool: - self.last_ignition = ignition return 100 - if cur_temp < 70.0: - fan_pwr_out = 0 - elif cur_temp > 85.0: - fan_pwr_out = 100 - else: - # 70°C → 0%, 85°C → 80%, target 75°C - fan_pwr_out = int(np.interp(cur_temp, [70.0, 85.0], [0, 80])) + fan_pwr_out = int(np.interp(cur_temp, [70.0, 85.0, 90.0], [0, 80, 100])) if not ignition: fan_pwr_out = min(fan_pwr_out, 30) - self.last_ignition = ignition return fan_pwr_out diff --git a/iqpilot/system/hardware/hardwared.py b/iqpilot/system/hardware/hardwared.py index d57db2f81..c918d704b 100755 --- a/iqpilot/system/hardware/hardwared.py +++ b/iqpilot/system/hardware/hardwared.py @@ -1,6 +1,8 @@ #!/usr/bin/env python3 import fcntl import os +import subprocess +import sys import queue import struct import threading @@ -16,10 +18,14 @@ from iqpilot.cereal.services import SERVICE_LIST from iqpilot.common.iq_perf import PerfSample, PerfTraceEmitter from iqpilot.common.utils import strip_deprecated_keys from iqpilot.common.filter_simple import FirstOrderFilter +from iqpilot.common.basedir import BASEDIR from iqpilot.common.params import Params from iqpilot.common.realtime import DT_HW from iqpilot.selfdrive.selfdrived.alertmanager import set_offroad_alert from iqpilot.system.hardware import HARDWARE, TICI, AGNOS +from iqpilot.system.hardware.egpu_dock.flash import dock_needs_flash +from iqpilot.system.hardware.egpu_dock.status import EgpuDockStatus +from iqpilot.system.hardware.usb import get_link_error_count, get_usb_state, set_usb_state, usb3_lane from iqpilot.system.loggerd.config import get_available_percent from iqpilot.common.swaglog import cloudlog from iqpilot.system.hardware.power_monitoring import PowerMonitoring, VBATT_LOW_POWER_EXIT @@ -41,7 +47,8 @@ CAN_STARTUP_RECOVERY_MAX_ATTEMPTS = 2 ThermalBand = namedtuple("ThermalBand", ['min_temp', 'max_temp']) HardwareState = namedtuple("HardwareState", ['network_type', 'network_info', 'network_strength', 'network_stats', - 'network_metered', 'modem_temps']) + 'network_metered', 'modem_temps', 'usb_state', 'usb_link_errors', + 'usb3_lane']) # List of thermal bands. We will stay within this region as long as we are within the bounds. # When exiting the bounds, we'll jump to the lower or higher band. Bands are ordered in the dict. @@ -161,6 +168,56 @@ class _CarParamsCache: +class EgpuDockFlasher: + """Flash the dock's firmware offroad when it does not match what we ship. + + Same policy as stock: the model runtime ignores a dock until its product + string matches, so a mismatched dock is unusable until this runs. Bounded + attempts, offroad only, one flash in flight at a time. + """ + MAX_ATTEMPTS = 3 + RETRY_INTERVAL = 20. + + def __init__(self): + self.thread: threading.Thread | None = None + self.attempts = 0 + self.last_attempt = 0. + self.flashed = False + self.mismatch = False + + @property + def failed(self) -> bool: + return (self.mismatch and self.attempts >= self.MAX_ATTEMPTS + and self.thread is not None and not self.thread.is_alive() and not self.flashed) + + def flash(self) -> None: + ret = subprocess.run(["sudo", sys.executable, + os.path.join(BASEDIR, "iqpilot/system/hardware/egpu_dock/flash.py")], + stdout=subprocess.PIPE, stderr=subprocess.STDOUT, text=True, check=False) + cloudlog.event("egpu dock flash done", returncode=ret.returncode, output=ret.stdout[-1000:], + error=ret.returncode != 0) + self.flashed = ret.returncode == 0 + + def update(self, offroad: bool, usb_state: list[dict]) -> None: + self.mismatch = dock_needs_flash(usb_state) + if not self.mismatch: + self.flashed = False + return + + if not offroad or self.flashed or self.attempts >= self.MAX_ATTEMPTS: + return + if self.thread is not None and self.thread.is_alive(): + return + if time.monotonic() - self.last_attempt < self.RETRY_INTERVAL: + return + + self.attempts += 1 + self.last_attempt = time.monotonic() + cloudlog.warning(f"egpu dock firmware out of date, flashing (attempt {self.attempts})") + self.thread = threading.Thread(target=self.flash, daemon=True) + self.thread.start() + + def set_offroad_alert_if_changed(offroad_alert: str, show_alert: bool, extra_text: str | None=None): if prev_offroad_states.get(offroad_alert, None) == (show_alert, extra_text): return @@ -254,6 +311,9 @@ def hw_state_thread(end_event, hw_queue): network_stats={'wwanTx': tx, 'wwanRx': rx}, network_metered=HARDWARE.get_network_metered(network_type), modem_temps=modem_temps, + usb_state=get_usb_state(), + usb_link_errors=get_link_error_count(), + usb3_lane=usb3_lane(), ) try: @@ -280,7 +340,7 @@ def hw_state_thread(end_event, hw_queue): def hardware_thread(end_event, hw_queue) -> None: pm = messaging.PubMaster(['deviceState', 'iqPerfTrace']) - sm = messaging.SubMaster(["peripheralState", "gpsLocationExternal", "selfdriveState", "pandaStates", "carState"], poll="pandaStates") + sm = messaging.SubMaster(["peripheralState", "gpsLocationExternal", "selfdriveState", "pandaStates", "carState", "egpuDockState"], poll="pandaStates") perf = PerfTraceEmitter("hardwared", pubmaster=pm) count = 0 @@ -306,6 +366,9 @@ def hardware_thread(end_event, hw_queue) -> None: network_strength=NetworkStrength.unknown, network_stats={'wwanTx': -1, 'wwanRx': -1}, modem_temps=[], + usb_state=[], + usb_link_errors=0, + usb3_lane="unknown", ) all_temp_filter = FirstOrderFilter(0., TEMP_TAU, DT_HW, initialized=False) @@ -332,6 +395,8 @@ def hardware_thread(end_event, hw_queue) -> None: thermal_config = HARDWARE.get_thermal_config() fan_controller = FanController() + egpu_dock_flasher = EgpuDockFlasher() + egpu_dock_status = EgpuDockStatus() while not end_event.is_set(): sm.update(PANDA_STATES_TIMEOUT) @@ -427,6 +492,14 @@ def hardware_thread(end_event, hw_queue) -> None: msg.deviceState.networkInfo = last_hw_state.network_info msg.deviceState.modemTempC = last_hw_state.modem_temps + set_usb_state(msg.deviceState, last_hw_state.usb_state, last_hw_state.usb_link_errors, + last_hw_state.usb3_lane) + egpu_dock_flasher.update(started_ts is None, last_hw_state.usb_state) + egpu_valid = sm.alive["egpuDockState"] and sm.valid["egpuDockState"] + egpu_dock_status.update(started_ts is None, last_hw_state.usb_state, egpu_dock_flasher.failed, + params.get_bool("UsbGpuLoading"), params.get("UsbGpuActive"), + params.get_bool("UsbGpuCompiled"), + sm["egpuDockState"] if egpu_valid else None, set_offroad_alert_if_changed) msg.deviceState.screenBrightnessPercent = HARDWARE.get_screen_brightness() diff --git a/iqpilot/system/hardware/tests/test_egpu_dock_flash.py b/iqpilot/system/hardware/tests/test_egpu_dock_flash.py new file mode 100644 index 000000000..694fdd8b4 --- /dev/null +++ b/iqpilot/system/hardware/tests/test_egpu_dock_flash.py @@ -0,0 +1,132 @@ +""" +Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos/ + +The eGPU dock flasher writes SPI flash, so the parts that decide WHETHER and +WHAT to write are pinned here. The transfer path itself needs the hardware; the +image validator, product parsing, config preservation and the needs-flash +decision do not, and those are what stop a bad write. +""" +import os +import zlib + +import pytest + +from iqpilot.system.hardware.egpu_dock import flash as f + + +def _wrap(body: bytes, *, magic=0xA5, checksum=None, crc=None, body_len=None) -> bytes: + n = len(body) if body_len is None else body_len + cs = (sum(body) & 0xFF) if checksum is None else checksum + c = zlib.crc32(body).to_bytes(4, "little") if crc is None else crc + return n.to_bytes(4, "little") + body + bytes([magic, cs]) + c + + +def test_bundled_firmware_is_valid_and_named(): + image = f.FIRMWARE_PATH.read_bytes() + f.validate_image(image) # raises if the shipped blob is corrupt + assert f.image_product(image).startswith("custom ") + assert f.image_product(image).endswith("-CLEAN") + assert f.bundled_version() == f.image_product(image) + + +def test_validate_rejects_corruption(): + body = b"custom deadbeef-CLEAN" + bytes(64) + f.validate_image(_wrap(body)) # the good case + with pytest.raises(ValueError): + f.validate_image(b"\x00" * 4) # too short + with pytest.raises(ValueError): + f.validate_image(_wrap(body, magic=0x00)) # bad magic + with pytest.raises(ValueError): + f.validate_image(_wrap(body, checksum=0x00)) + with pytest.raises(ValueError): + f.validate_image(_wrap(body, crc=b"\x00\x00\x00\x00")) + with pytest.raises(ValueError): + f.validate_image(_wrap(body, body_len=len(body) + 1)) # length disagrees + with pytest.raises(ValueError): + f.validate_image((f.MAX_CODE_SIZE + 1).to_bytes(4, "little") + bytes(16)) + + +def test_image_product_requires_a_version_string(): + with pytest.raises(ValueError): + f.image_product(b"no version here") + + +def test_saved_config_preserves_the_first_backup(tmp_path): + # the config page is per-unit; a reflash must rewrite the ORIGINAL, never the + # bytes read back from a half-written dock + p = str(tmp_path / "dock.bin") + original = bytes(range(256)) + assert f.saved_config(p, original) == original + # later flash reads something different -> the stored original wins + assert f.saved_config(p, bytes(256)) == original + with open(p, "rb") as fh: + assert fh.read() == original + + +def test_saved_config_rejects_wrong_size_backup(tmp_path): + p = str(tmp_path / "dock.bin") + with open(p, "wb") as fh: + fh.write(b"\x00" * 8) + with pytest.raises(RuntimeError): + f.saved_config(p, bytes(256)) + + +def test_needs_flash_only_for_a_dock_on_wrong_firmware(): + expected = f.bundled_version() + vid, pid = (int(x, 16) for x in f.VID_PIDS[0]) + rom_vid, rom_pid = (int(x, 16) for x in f.ROM_VID_PIDS[0]) + + assert not f.dock_needs_flash([]) + assert not f.dock_needs_flash([{"vendorId": 0x1234, "productId": 0x5678, "product": "something else"}]) + assert not f.dock_needs_flash([{"vendorId": vid, "productId": pid, "product": expected}]) + assert f.dock_needs_flash([{"vendorId": vid, "productId": pid, "product": "custom 00000000-CLEAN"}]) + # a ROM-mode board always needs flashing + assert f.dock_needs_flash([{"vendorId": rom_vid, "productId": rom_pid, "product": f.ROM_PRODUCT}]) + + +def test_both_shipped_ids_trigger_the_check(): + for pair in f.VID_PIDS: + vid, pid = (int(x, 16) for x in pair) + assert f.dock_needs_flash([{"vendorId": vid, "productId": pid, "product": "custom 00000000-CLEAN"}]) + + +def test_rom_detection(): + assert f.in_rom_bootloader(f.ROM_VID_PIDS[0], "anything") + assert f.in_rom_bootloader(("add1", "0001"), f.ROM_PRODUCT) + assert f.in_rom_bootloader(("add1", "0001"), "AS2462something") + assert not f.in_rom_bootloader(("add1", "0001"), f.bundled_version()) + assert not f.in_rom_bootloader(("add1", "0001"), None) + + +def test_config_paths_are_per_host_and_have_a_legacy_fallback(): + host = os.uname().nodename + assert f.config_path().endswith(f"{host}.bin") + assert f.config_path().startswith(f.CONFIG_DIR) + # a dock flashed on this device by stock openpilot left its backup elsewhere + assert f.legacy_config_path().startswith(f.LEGACY_CONFIG_DIR) + assert f.legacy_config_path() != f.config_path() + + +def test_we_do_not_autoflash(): + # upstream flashes from hardwared automatically; ours must stay deliberate + # until it has been validated against a real dock + import subprocess + root = os.path.join(os.path.dirname(f.__file__), "..", "..", "..") + hits = subprocess.run(["grep", "-rnI", "--exclude-dir=__pycache__", "flash_dock", os.path.join(root, "system"), + os.path.join(root, "iqpilot")], capture_output=True, text=True).stdout + callers = [ln for ln in hits.splitlines() if "egpu_dock/flash.py" not in ln and "test_" not in ln] + assert callers == [], f"unexpected automatic flash caller: {callers}" + + +def test_runtime_fw_gate_is_pinned_to_the_bundled_firmware(): + from iqpilot.system.hardware.usb import EGPU_DOCK_FW_PRODUCT + assert EGPU_DOCK_FW_PRODUCT == f.bundled_version() + + +def test_register_reads_default_to_superspeed_size(): + assert f.MAX_REGISTER_READ_SIZE == 255 + assert f.Flash().max_register_read_size == f.MAX_REGISTER_READ_SIZE + + +def test_link_up_is_false_without_a_dock(): + assert f.link_up() is False diff --git a/iqpilot/system/hardware/tests/test_fan_controller.py b/iqpilot/system/hardware/tests/test_fan_controller.py index 8e0492790..3284f5625 100644 --- a/iqpilot/system/hardware/tests/test_fan_controller.py +++ b/iqpilot/system/hardware/tests/test_fan_controller.py @@ -1,57 +1,41 @@ -import pytest +import numpy as np from iqpilot.system.hardware.fan_controller import FanController -ALL_CONTROLLERS = [FanController] - -def patched_controller(mocker, controller_class): - mocker.patch("os.system", new=mocker.Mock()) - return controller_class() class TestFanController: - def wind_up(self, controller, ignition=True): - for _ in range(1000): - controller.update(100, ignition) + def test_ramp_anchors(self): + c = FanController() + assert c.update(60, True) == 0 + assert c.update(70, True) == 0 + assert c.update(85, True) == 80 + assert c.update(90, True) == 100 + assert c.update(100, True) == 100 - def wind_down(self, controller, ignition=False): - for _ in range(1000): - controller.update(10, ignition) + def test_ramp_is_monotonic_and_continuous(self): + c = FanController() + temps = np.arange(50.0, 105.0, 0.25) + outs = [c.update(t, True) for t in temps] + assert all(b >= a for a, b in zip(outs, outs[1:])) + # no step may exceed the steepest segment's slope (4 %/deg) over a 0.25 deg move + assert max(b - a for a, b in zip(outs, outs[1:])) <= 2 - @pytest.mark.parametrize("controller_class", ALL_CONTROLLERS) - def test_hot_onroad(self, mocker, controller_class): - controller = patched_controller(mocker, controller_class) - self.wind_up(controller) - assert controller.update(100, True) >= 70 + def test_hot_onroad(self): + assert FanController().update(100, True) >= 70 - @pytest.mark.parametrize("controller_class", ALL_CONTROLLERS) - def test_offroad_limits(self, mocker, controller_class): - controller = patched_controller(mocker, controller_class) - self.wind_up(controller) - assert controller.update(100, False) <= 30 + def test_offroad_capped(self): + c = FanController() + for t in (60, 75, 85, 100): + assert c.update(t, False) <= 30 - @pytest.mark.parametrize("controller_class", ALL_CONTROLLERS) - def test_no_fan_wear(self, mocker, controller_class): - controller = patched_controller(mocker, controller_class) - self.wind_down(controller) - assert controller.update(10, False) == 0 + def test_no_fan_wear(self): + assert FanController().update(10, False) == 0 - @pytest.mark.parametrize("controller_class", ALL_CONTROLLERS) - def test_limited(self, mocker, controller_class): - controller = patched_controller(mocker, controller_class) - self.wind_up(controller, True) - assert controller.update(100, True) == 100 + def test_max_cool(self): + c = FanController() + assert c.update(80, True, True) == 100 + assert c.update(80, False, True) == 100 - @pytest.mark.parametrize("controller_class", ALL_CONTROLLERS) - def test_max_cool(self, mocker, controller_class): - controller = patched_controller(mocker, controller_class) - self.wind_down(controller) - assert controller.update(80, True, True) == 100 - assert controller.update(80, False, True) == 100 - - @pytest.mark.parametrize("controller_class", ALL_CONTROLLERS) - def test_windup_speed(self, mocker, controller_class): - controller = patched_controller(mocker, controller_class) - self.wind_down(controller, True) - for _ in range(10): - controller.update(90, True) - assert controller.update(90, True) >= 60 + def test_target_band_has_airflow(self): + # the design centers on 75 C; the curve must actually move air there + assert 20 <= FanController().update(75, True) <= 40 diff --git a/iqpilot/system/hardware/tests/test_usb_state.py b/iqpilot/system/hardware/tests/test_usb_state.py new file mode 100644 index 000000000..ab4e6c148 --- /dev/null +++ b/iqpilot/system/hardware/tests/test_usb_state.py @@ -0,0 +1,267 @@ +""" +Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos/ + +deviceState.usbState carries a per-device USB bus snapshot plus the ssusb +link-error counter (IQ.OS 4.9.1+ `portli`) into every rlog. Drive it off a +synthetic sysfs tree so parsing, eGPU dock presence and the link-error +plumbing are pinned without hardware. +""" +from iqpilot.cereal import messaging +from iqpilot.system.hardware.usb import ( + EGPU_DOCK_FW_PRODUCT, EGPU_DOCK_ROM_USB_IDS, EGPU_DOCK_USB_IDS, controller, egpu_dock_present, + egpu_dock_ready, get_link_error_count, + get_usb_topology, get_usb_state, link_controller, read_hex_counter, set_usb_state, usb3_lane, +) + + +def _mkctrl(root, name="a800000.ssusb", portli="0x00000000"): + """Platform controller dir, mirroring /sys/devices/platform/soc/.ssusb.""" + ctrl = root / "soc" / name + ctrl.mkdir(parents=True) + if portli is not None: + (ctrl / "portli").write_text(portli + "\n") + return ctrl + + +def _mkdev(root, name, *, vid, pid, busnum=1, devnum=2, speed=5000, + manufacturer="ACME", product="Widget", ctrl=None): + """USB device under the controller, symlinked into the bus view like sysfs.""" + real = (ctrl / "usb1" / name) if ctrl is not None else (root / "bus" / name) + real.mkdir(parents=True) + (real / "idVendor").write_text(f"{vid:04x}\n") + (real / "idProduct").write_text(f"{pid:04x}\n") + (real / "busnum").write_text(f"{busnum}\n") + (real / "devnum").write_text(f"{devnum}\n") + (real / "speed").write_text(f"{speed}\n") + (real / "manufacturer").write_text(manufacturer + "\n") + (real / "product").write_text(product + "\n") + + bus = root / "bus" + bus.mkdir(parents=True, exist_ok=True) + link = bus / name + if real != link: + link.symlink_to(real) + return real + + +def test_missing_sysfs_is_empty(tmp_path): + assert get_usb_state(tmp_path / "nope") == [] + + +def test_entries_without_idvendor_are_skipped(tmp_path): + (tmp_path / "bus" / "usb1").mkdir(parents=True) # a root hub dir with no idVendor + _mkdev(tmp_path, "1-2", vid=0x1234, pid=0x5678) + state = get_usb_state(tmp_path / "bus") + assert len(state) == 1 and state[0]["vendorId"] == 0x1234 + + +def test_fields_parsed_with_hex_ids(tmp_path): + _mkdev(tmp_path, "1-2", vid=0x0BDA, pid=0x8153, busnum=3, devnum=7, + speed=480, manufacturer="Realtek", product="USB 10/100 LAN") + (dev,) = get_usb_state(tmp_path / "bus") + assert dev == { + "busnum": 3, "devnum": 7, + "vendorId": 0x0BDA, "productId": 0x8153, + "speedMbps": 480, + "manufacturer": "Realtek", "product": "USB 10/100 LAN", + "linkErrorCount": 0, # no controller in this device's path + "usb3Lane": "unknown", # not on the type-C port's controller + } + + +def test_unreadable_strings_default_empty(tmp_path): + real = _mkdev(tmp_path, "1-2", vid=0x1, pid=0x2) + (real / "manufacturer").unlink() + (real / "product").unlink() + (dev,) = get_usb_state(tmp_path / "bus") + assert dev["manufacturer"] == "" and dev["product"] == "" + + +def test_hex_counter_parsing(tmp_path): + f = tmp_path / "portli" + f.write_text("0x00000000\n") + assert read_hex_counter(f) == 0 + f.write_text("0x0000002a\n") + assert read_hex_counter(f) == 42 + f.write_text("0000002a\n") # bare hex, no 0x prefix + assert read_hex_counter(f) == 42 + f.write_text("garbage\n") + assert read_hex_counter(f) == 0 + assert read_hex_counter(tmp_path / "absent") == 0 # pre-4.9.1 IQ.OS + + +def test_controller_resolved_from_device(tmp_path): + ctrl = _mkctrl(tmp_path) + _mkdev(tmp_path, "1-2", vid=0x1, pid=0x2, ctrl=ctrl) + assert controller(tmp_path / "bus" / "1-2") == ctrl.resolve() + + +def test_device_carries_its_controllers_link_errors(tmp_path): + ctrl = _mkctrl(tmp_path, portli="0x0000000c") + _mkdev(tmp_path, "1-2", vid=0x1234, pid=0x5678, ctrl=ctrl) + (dev,) = get_usb_state(tmp_path / "bus") + assert dev["linkErrorCount"] == 12 + + +def test_controller_count_without_any_enumerated_device(tmp_path): + # peripheral mode (eMac gadget link): the peer never enumerates on our side, + # so the counter must still be readable off the controller + _mkctrl(tmp_path, portli="0x00000005") + assert get_usb_state(tmp_path / "bus") == [] + assert get_link_error_count(tmp_path / "soc") == 5 + + +def test_link_errors_summed_across_controllers(tmp_path): + _mkctrl(tmp_path, name="a800000.ssusb", portli="0x00000002") + _mkctrl(tmp_path, name="a600000.ssusb", portli="0x00000003") + assert get_link_error_count(tmp_path / "soc") == 5 + + +def test_missing_portli_reads_zero(tmp_path): + _mkctrl(tmp_path, portli=None) # pre-4.9.1 kernel: file absent + assert get_link_error_count(tmp_path / "soc") == 0 + + +def test_set_usb_state_populates_message_and_flags_dock(tmp_path): + ctrl = _mkctrl(tmp_path, portli="0x00000007") + _mkdev(tmp_path, "1-1", vid=0x1234, pid=0x5678, speed=480, ctrl=ctrl) + _mkdev(tmp_path, "1-2", vid=EGPU_DOCK_USB_IDS[0][0], pid=EGPU_DOCK_USB_IDS[0][1], speed=5000, ctrl=ctrl) + + msg = messaging.new_message('deviceState') + set_usb_state(msg.deviceState, get_usb_state(tmp_path / "bus"), get_link_error_count(tmp_path / "soc")) + + devices = list(msg.deviceState.usbState.devices) + assert len(devices) == 2 + assert {d.speedMbps for d in devices} == {480, 5000} + assert all(d.linkErrorCount == 7 for d in devices) + assert msg.deviceState.usbState.linkErrorCount == 7 + assert msg.deviceState.egpuDockPresent + + +def test_dock_absent_when_not_plugged(tmp_path): + _mkdev(tmp_path, "1-1", vid=0x1234, pid=0x5678) + msg = messaging.new_message('deviceState') + set_usb_state(msg.deviceState, get_usb_state(tmp_path / "bus")) + assert not msg.deviceState.egpuDockPresent + + +def test_both_shipped_dock_usb_ids_detected(tmp_path): + # comma ships the dock under two VID/PIDs; only the first was known before + for i, (vid, pid) in enumerate(EGPU_DOCK_USB_IDS): + root = tmp_path / f"v{i}" + _mkdev(root, "1-1", vid=vid, pid=pid) + assert egpu_dock_present(root / "bus"), f"{vid:#06x}:{pid:#06x} not detected" + + +def test_dock_in_rom_mode_is_not_present(tmp_path): + # bootloader/ROM state enumerates but cannot serve a GPU until flashed + vid, pid = EGPU_DOCK_ROM_USB_IDS[0] + _mkdev(tmp_path, "1-1", vid=vid, pid=pid) + assert not egpu_dock_present(tmp_path / "bus") + + +def test_empty_bus_clears_flag(): + msg = messaging.new_message('deviceState') + set_usb_state(msg.deviceState, []) + assert len(msg.deviceState.usbState.devices) == 0 + assert msg.deviceState.usbState.linkErrorCount == 0 + assert not msg.deviceState.egpuDockPresent + + +def test_link_error_count_masked_to_16_bits(tmp_path): + # the per-device field is UInt16 upstream; a wrapped counter must not overflow it + ctrl = _mkctrl(tmp_path, portli="0x0001ffff") + _mkdev(tmp_path, "1-2", vid=0x1, pid=0x2, ctrl=ctrl) + (dev,) = get_usb_state(tmp_path / "bus") + assert dev["linkErrorCount"] == 0xFFFF + + msg = messaging.new_message('deviceState') + set_usb_state(msg.deviceState, get_usb_state(tmp_path / "bus")) + assert list(msg.deviceState.usbState.devices)[0].linkErrorCount == 0xFFFF + + +def test_usb_topology_lists_bus_entries(tmp_path): + _mkdev(tmp_path, "1-1", vid=0x1, pid=0x2) + _mkdev(tmp_path, "1-2", vid=0x3, pid=0x4) + assert {"1-1", "1-2"} <= get_usb_topology(tmp_path / "bus") + assert get_usb_topology(tmp_path / "nope") == set() + + +def _mkudc(root, name="a600000.dwc3"): + udc = root / "udc" / name + udc.mkdir(parents=True, exist_ok=True) + (udc / "state").write_text("not attached\n") + return root / "udc" + + +def test_link_controller_derived_from_udc_not_hardcoded(tmp_path): + # comma pins "a600000.ssusb"; we derive it, so another board still resolves + assert link_controller(_mkudc(tmp_path)) == "a600000.ssusb" + assert link_controller(_mkudc(tmp_path / "other", "a800000.dwc3")) == "a800000.ssusb" + + +def test_link_controller_absent_udc_is_empty(tmp_path): + assert link_controller(tmp_path / "nope") == "" + + +def test_usb3_lane_mapping(): + assert usb3_lane(1) == "a" + assert usb3_lane(2) == "b" + assert usb3_lane(0) == "unknown" # unattached + assert usb3_lane(None if False else 7) == "unknown" + + +def test_port_lane_survives_gadget_mode(tmp_path): + """The case upstream cannot report: in peripheral mode nothing enumerates on + the link controller, so every Device row is 'unknown' while the eMac link is + up. The port-level field still carries it.""" + ctrl = _mkctrl(tmp_path, name="a800000.ssusb") + _mkdev(tmp_path, "1-1", vid=0x1234, pid=0x5678, ctrl=ctrl) # panda, host controller + _mkudc(tmp_path) # gadget is a600000 + + devices = get_usb_state(tmp_path / "bus", tmp_path / "udc") + assert all(d["usb3Lane"] == "unknown" for d in devices), "no device sits on the gadget controller" + + msg = messaging.new_message('deviceState') + set_usb_state(msg.deviceState, devices, 0, lane="b") + assert msg.deviceState.usbState.usb3Lane == "b" + assert all(d.usb3Lane == "unknown" for d in msg.deviceState.usbState.devices) + + +def test_device_on_link_controller_gets_the_lane(tmp_path): + # host mode on the type-C port (eGPU dock): upstream's per-device field populates + ctrl = _mkctrl(tmp_path, name="a600000.ssusb") + _mkdev(tmp_path, "1-1", vid=EGPU_DOCK_USB_IDS[0][0], pid=EGPU_DOCK_USB_IDS[0][1], ctrl=ctrl) + _mkudc(tmp_path) + import iqpilot.system.hardware.usb as usbmod + orig = usbmod.usb3_lane + usbmod.usb3_lane = lambda orientation=None: "a" + try: + devices = get_usb_state(tmp_path / "bus", tmp_path / "udc") + finally: + usbmod.usb3_lane = orig + assert devices[0]["usb3Lane"] == "a" + + +def test_dock_ready_requires_the_bundled_firmware_product(tmp_path): + root = tmp_path + vid, pid = EGPU_DOCK_USB_IDS[0] + _mkdev(root, "1-1", vid=vid, pid=pid, product=EGPU_DOCK_FW_PRODUCT) + assert egpu_dock_present(root / "bus") + assert egpu_dock_ready(root / "bus") + + +def test_dock_on_foreign_firmware_is_present_but_not_ready(tmp_path): + root = tmp_path + vid, pid = EGPU_DOCK_USB_IDS[0] + _mkdev(root, "1-1", vid=vid, pid=pid, product="custom deadbeef-CLEAN") + assert egpu_dock_present(root / "bus") + assert not egpu_dock_ready(root / "bus") + + +def test_rom_mode_dock_is_neither_present_nor_ready(tmp_path): + root = tmp_path + vid, pid = EGPU_DOCK_ROM_USB_IDS[0] + _mkdev(root, "1-1", vid=vid, pid=pid, product="USB 3.2 PCIe TinyEnclosure") + assert not egpu_dock_present(root / "bus") + assert not egpu_dock_ready(root / "bus") diff --git a/iqpilot/system/hardware/tici/set_usb_storage.sh b/iqpilot/system/hardware/tici/set_usb_storage.sh index 75e2d4f90..8c3ba1b8c 100755 --- a/iqpilot/system/hardware/tici/set_usb_storage.sh +++ b/iqpilot/system/hardware/tici/set_usb_storage.sh @@ -78,32 +78,38 @@ unbind() { fi } +set_attr() { + [ "$(cat "$1" 2>/dev/null)" = "$2" ] && return 0 + echo "$2" | sudo tee "$1" >/dev/null 2>&1 || true +} + ensure_base() { if ! mountpoint -q /config; then sudo mount -t configfs none /config fi sudo mkdir -p "$GADGET/strings/0x409" "$GADGET/configs/c.1/strings/0x409" cd "$GADGET" - [ -s idVendor ] || echo 0x04D8 | sudo tee idVendor >/dev/null - [ -s idProduct ] || echo 0x1235 | sudo tee idProduct >/dev/null - [ -s strings/0x409/serialnumber ] || echo "$(cat /proc/cmdline | sed -e 's/^.*androidboot.serialno=//' -e 's/ .*$//')" | sudo tee strings/0x409/serialnumber >/dev/null - [ -s strings/0x409/manufacturer ] || echo "comma.ai" | sudo tee strings/0x409/manufacturer >/dev/null - [ -s strings/0x409/product ] || echo "IQ.Pilot" | sudo tee strings/0x409/product >/dev/null - [ -s configs/c.1/MaxPower ] || echo 250 | sudo tee configs/c.1/MaxPower >/dev/null - [ -s configs/c.1/strings/0x409/configuration ] || echo "IQ.Pilot" | sudo tee configs/c.1/strings/0x409/configuration >/dev/null + # `[ -s ]` never guards a configfs attribute: an unset idVendor still reads back + # as "0x0000", so those writes were all skipped and the gadget stayed nameless + set_attr idVendor 0x04D8 + set_attr idProduct 0x1235 + set_attr strings/0x409/serialnumber "$(sed -e 's/^.*androidboot.serialno=//' -e 's/ .*$//' /proc/cmdline)" + set_attr strings/0x409/manufacturer "comma.ai" + set_attr strings/0x409/product "IQ.Pilot" + set_attr configs/c.1/MaxPower 250 + set_attr configs/c.1/strings/0x409/configuration "IQ.Pilot" } add_adb() { # same rationale as add_mass_storage: start from a clean slate to avoid stale busy attributes remove_adb cd "$GADGET" - sudo mkdir -p functions/ncm.0 functions/ffs.adb + sudo mkdir -p functions/ffs.adb sudo mkdir -p /dev/usb-ffs/adb if ! mountpoint -q /dev/usb-ffs/adb; then sudo mount -t functionfs adb /dev/usb-ffs/adb fi - sudo rm -f configs/c.1/ncm.0 configs/c.1/ffs.adb - sudo ln -s functions/ncm.0 configs/c.1/ + sudo rm -f configs/c.1/ffs.adb sudo ln -s functions/ffs.adb configs/c.1/ setprop service.adb.tcp.port -1 2>/dev/null || true sudo systemctl start adbd @@ -115,9 +121,42 @@ remove_adb() { sudo systemctl stop adbd || true if [ -d "$GADGET" ]; then cd "$GADGET" - sudo rm -f configs/c.1/ncm.0 configs/c.1/ffs.adb + sudo rm -f configs/c.1/ffs.adb sudo umount /dev/usb-ffs/adb 2>/dev/null || true - sudo rmdir functions/ncm.0 functions/ffs.adb 2>/dev/null || true + sudo rmdir functions/ffs.adb 2>/dev/null || true + fi +} + +# ncm carries the usb0 ethernet link. ADB needs it, but so does the Mac-backed +# model worker with ADB off, so it is enabled independently of either. +# the kernel randomises the ncm MACs every boot, so macOS sees a new adapter each +# time and orphans the network service holding the link's static address +ncm_id() { + local id + id=$(tr -dc '0-9a-f' < /data/params/d/DongleId 2>/dev/null | tail -c 6) + [ ${#id} -eq 6 ] || id="000001" + echo "$id" +} + +add_ncm() { + remove_ncm + cd "$GADGET" + sudo mkdir -p functions/ncm.0 + local id + id=$(ncm_id) + # best effort: some kernels create the ncm netdev lazily and fail these writes + # with ENODEV, and a pinned MAC is never worth losing the whole gadget over + echo "02:49:51:${id:0:2}:${id:2:2}:${id:4:2}" | sudo tee functions/ncm.0/host_addr >/dev/null 2>&1 || true + echo "06:49:51:${id:0:2}:${id:2:2}:${id:4:2}" | sudo tee functions/ncm.0/dev_addr >/dev/null 2>&1 || true + sudo rm -f configs/c.1/ncm.0 + sudo ln -s functions/ncm.0 configs/c.1/ +} + +remove_ncm() { + if [ -d "$GADGET" ]; then + cd "$GADGET" + sudo rm -f configs/c.1/ncm.0 + sudo rmdir functions/ncm.0 2>/dev/null || true fi } @@ -162,10 +201,18 @@ USB_STORAGE_ENABLE=0 read_bool_param "/data/params/d/UsbStorageEnabled" && USB_STORAGE_ENABLE=1 ADB_ENABLE=0 read_bool_param "/data/params/d/AdbEnabled" && ADB_ENABLE=1 +EMAC_ENABLE=0 +read_bool_param "/data/params/d/IQEmacEnabled" && EMAC_ENABLE=1 unbind ensure_base +if [ "$ADB_ENABLE" == "1" ] || [ "$EMAC_ENABLE" == "1" ]; then + add_ncm +else + remove_ncm +fi + if [ "$ADB_ENABLE" == "1" ]; then add_adb else diff --git a/iqpilot/system/hardware/tici/usb_storage.py b/iqpilot/system/hardware/tici/usb_storage.py index d6f6ed835..7dcb52214 100644 --- a/iqpilot/system/hardware/tici/usb_storage.py +++ b/iqpilot/system/hardware/tici/usb_storage.py @@ -15,3 +15,57 @@ def apply_usb_storage_state(state: bool): subprocess.Popen(args) except OSError: pass + + +NCM_TRIED_MARKER = "/tmp/.iqemac_ncm_provisioned" +MAX_NCM_ATTEMPTS = 3 + + +def _ncm_attempts() -> int: + try: + with open(NCM_TRIED_MARKER) as f: + return int(f.read().strip() or 0) + except (OSError, ValueError): + return 0 + + +def ensure_ncm_gadget() -> bool: + # configfs is RAM backed, so the gadget must be rebuilt every boot; binding it + # enumerates on the host, which must not happen mid-drive + if os.path.isdir("/sys/class/net/usb0"): + return True + params = Params() + from iqpilot.selfdrive.iqmodeld.egpu_helpers import egpu_selected + if not (params.get_bool("IQEmacEnabled") or egpu_selected(params)): + return False + attempts = _ncm_attempts() + if attempts >= MAX_NCM_ATTEMPTS: + return False + try: + # stamped before the run: the gadget build can wedge configfs in + # uninterruptible D state, and retrying that forever helps nobody + with open(NCM_TRIED_MARKER, "w") as f: + f.write(str(attempts + 1)) + subprocess.run(["sudo", "-n", SCRIPT_PATH], timeout=120, check=False) + except (OSError, subprocess.SubprocessError): + return False + return os.path.isdir("/sys/class/net/usb0") + + +INPUT_SUSPEND = "/sys/class/power_supply/battery/input_suspend" + + +def suspend_usb_input(suspend: bool = True) -> bool: + # a host on the data port makes the PMIC sink USB-PD while OBD-C feeds the same + # rail; the SOM browns out. This closes the charge path only, data is untouched + if not os.path.exists(INPUT_SUSPEND): + return False + try: + with open(INPUT_SUSPEND) as f: + if f.read().strip() == ("1" if suspend else "0"): + return True + except OSError: + pass + rc = subprocess.run(["sudo", "-n", "sh", "-c", f"echo {int(suspend)} > {INPUT_SUSPEND}"], + check=False, capture_output=True) + return rc.returncode == 0 diff --git a/iqpilot/system/hardware/usb.py b/iqpilot/system/hardware/usb.py new file mode 100644 index 000000000..15be6f295 --- /dev/null +++ b/iqpilot/system/hardware/usb.py @@ -0,0 +1,184 @@ +""" +Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos/ + +USB bus snapshot for deviceState: every enumerated device with its negotiated +speed and its controller's link-error count. Landing this in every rlog makes +cable/hub/link regressions diagnosable from a recorded route instead of only +live. + +Link errors come from `portli` on the ssusb controller (IQ.OS 4.9.1+); on older +builds the file is absent and the counts read 0. + +The USB eGPU dock is identified by VID/PID only. comma's internal codename for +it is deliberately not used here: IQ.Pilot runs these models on several +backends (eGPU dock, eMac), so the naming stays about the role, not the vendor. +""" +from pathlib import Path + +# comma's USB eGPU dock, both shipped USB IDs. The ROM ids are the same board +# sitting in its bootloader (ASMedia) before vendor firmware is flashed — it +# enumerates but cannot serve a GPU in that state. +EGPU_DOCK_USB_IDS = ((0xADD1, 0x0001), (0x3801, 0x0001)) +EGPU_DOCK_ROM_USB_IDS = ((0x174C, 0x2464), (0x174C, 0x2463)) +# must equal image_product() of the bundled firmware; test_egpu_dock_flash pins them together +EGPU_DOCK_FW_PRODUCT = "custom ed4e39b7-CLEAN" + + +def is_egpu_usb_device(vendor_id: int, product_id: int, include_bootloader: bool = False) -> bool: + ids = EGPU_DOCK_USB_IDS + EGPU_DOCK_ROM_USB_IDS if include_bootloader else EGPU_DOCK_USB_IDS + return (vendor_id, product_id) in ids +USB_DEVICES_PATH = Path("/sys/bus/usb/devices") +UDC_PATH = Path("/sys/class/udc") +TYPEC_CC_ORIENTATION_PATH = Path("/sys/class/power_supply/usb/typec_cc_orientation") +USB3_LANES = {1: "a", 2: "b"} # 0 = unattached +SOC_PLATFORM_PATH = Path("/sys/devices/platform/soc") +CONTROLLER_SUFFIX = ".ssusb" +LINK_ERRORS_FILE = "portli" + + +def read(path: Path) -> str | None: + # a controller in peripheral mode fails portli's show(); that surfaces as TypeError, not OSError + try: + return path.read_text().strip() + except Exception: + return None + + +def read_int(path: Path, base: int = 10) -> int: + try: + return int(path.read_text(), base) + except Exception: + return 0 + + +def read_hex_counter(path: Path) -> int: + """sysfs counter printed as '0x0000002a' (portli), tolerating a bare hex value.""" + raw = read(path) + if raw is None: + return 0 + try: + return int(raw, 0) if raw.lower().startswith("0x") else int(raw, 16) + except ValueError: + return 0 + + +def get_usb_topology(root: Path = USB_DEVICES_PATH) -> set[str]: + """Names of everything on the bus; a cheap way to detect hotplug without + re-reading every attribute.""" + try: + return {p.name for p in root.iterdir()} + except Exception: + return set() + + +def usb_devices(root: Path = USB_DEVICES_PATH) -> list[Path]: + try: + return sorted((d for d in root.glob("*") if (d / "idVendor").exists()), key=lambda p: p.name) + except Exception: + return [] + + +def controller(device: Path) -> Path | None: + """The SuperSpeed controller a device hangs off (…/a800000.ssusb).""" + try: + return next((p for p in device.resolve().parents if p.name.endswith(CONTROLLER_SUFFIX)), None) + except Exception: + return None + + +def usb_controllers(soc: Path = SOC_PLATFORM_PATH) -> list[Path]: + try: + return sorted(soc.glob(f"*{CONTROLLER_SUFFIX}")) + except Exception: + return [] + + +def link_controller(udc_root: Path = UDC_PATH) -> str: + """Name of the Type-C port's controller, derived from the UDC rather than + hardcoded: the gadget exposes `.dwc3`, whose address prefix is the + `.ssusb` controller behind the same connector. comma pins the 3X value + directly, which would be wrong on any other board.""" + try: + udc = next(iter(sorted(p.name for p in udc_root.iterdir())), "") + except Exception: + return "" + return f"{udc.split('.')[0]}{CONTROLLER_SUFFIX}" if udc else "" + + +def usb3_lane(orientation: int | None = None) -> str: + """Which SuperSpeed lane the Type-C connector landed on. Unattached reads 0, + which is 'unknown' rather than a lane.""" + if orientation is None: + orientation = read_int(TYPEC_CC_ORIENTATION_PATH) + return USB3_LANES.get(orientation, "unknown") + + +def link_errors(ctrl: Path | None) -> int: + return read_hex_counter(ctrl / LINK_ERRORS_FILE) if ctrl is not None else 0 + + +def get_link_error_count(soc: Path = SOC_PLATFORM_PATH) -> int: + """Cumulative SS port link errors, read off the controller rather than a + device: in peripheral mode (eMac gadget link) the peer never enumerates on + our side, so there is no device row to carry the count.""" + return sum(link_errors(c) for c in usb_controllers(soc)) + + +def egpu_dock_present(root: Path = USB_DEVICES_PATH) -> bool: + """A dock in ROM/bootloader state is deliberately NOT counted as present: it + enumerates but cannot serve a GPU until vendor firmware is flashed.""" + return any((read_int(d / "idVendor", 16), read_int(d / "idProduct", 16)) in EGPU_DOCK_USB_IDS + for d in usb_devices(root)) + + +def egpu_dock_ready(root: Path = USB_DEVICES_PATH) -> bool: + """Present AND running the exact firmware we ship. A dock on any other + firmware enumerates fine but has not been validated with this stack, so the + runtime refuses it; the flasher still sees it via egpu_dock_present.""" + return any((read_int(d / "idVendor", 16), read_int(d / "idProduct", 16)) in EGPU_DOCK_USB_IDS + and (read(d / "product") or "").strip() == EGPU_DOCK_FW_PRODUCT + for d in usb_devices(root)) + + +def get_usb_state(root: Path = USB_DEVICES_PATH, udc_root: Path = UDC_PATH) -> list[dict]: + devices = [] + lane, link_ctrl = usb3_lane(), link_controller(udc_root) + for device in usb_devices(root): + ctrl = controller(device) + devices.append({ + "usb3Lane": lane if ctrl is not None and ctrl.name == link_ctrl else "unknown", + "busnum": read_int(device / "busnum"), + "devnum": read_int(device / "devnum"), + "vendorId": read_int(device / "idVendor", 16), + "productId": read_int(device / "idProduct", 16), + "speedMbps": read_int(device / "speed"), + "manufacturer": read(device / "manufacturer") or "", + "product": read(device / "product") or "", + # 16-bit field upstream, so mask rather than let a wrapped counter overflow it + "linkErrorCount": link_errors(ctrl) & 0xFFFF, + }) + return devices + + +def set_usb_state(device_state, devices: list[dict], link_error_count: int = 0, + lane: str | None = None) -> None: + entries = device_state.usbState.init('devices', len(devices)) + + dock_present = False + for entry, device in zip(entries, devices, strict=True): + entry.busnum = device["busnum"] + entry.devnum = device["devnum"] + entry.vendorId = device["vendorId"] + entry.productId = device["productId"] + entry.speedMbps = device["speedMbps"] + entry.manufacturer = device["manufacturer"] + entry.product = device["product"] + entry.linkErrorCount = device.get("linkErrorCount", 0) & 0xFFFF + entry.usb3Lane = device.get("usb3Lane", "unknown") + + if (entry.vendorId, entry.productId) in EGPU_DOCK_USB_IDS: + dock_present = True + + device_state.usbState.linkErrorCount = link_error_count + device_state.usbState.usb3Lane = lane if lane is not None else usb3_lane() + device_state.egpuDockPresent = dock_present diff --git a/iqpilot/system/manager/manager.py b/iqpilot/system/manager/manager.py index 5edb2b626..c72e65329 100755 --- a/iqpilot/system/manager/manager.py +++ b/iqpilot/system/manager/manager.py @@ -53,6 +53,16 @@ def manager_init() -> None: except Exception: cloudlog.exception("recover_unclean_segments failed") + try: + from iqpilot.system.hardware import TICI + if TICI: + from iqpilot.system.hardware.tici.usb_storage import ensure_ncm_gadget, suspend_usb_input + ensure_ncm_gadget() + if Params().get_bool("IQEmacEnabled") or Params().get_bool("IQEgpuEnabled"): + suspend_usb_input(True) + except Exception: + cloudlog.exception("emac usb setup failed") + build_metadata = get_build_metadata() params = Params() diff --git a/iqpilot/system/manager/process_config.py b/iqpilot/system/manager/process_config.py index 7e1492154..449fa70e5 100644 --- a/iqpilot/system/manager/process_config.py +++ b/iqpilot/system/manager/process_config.py @@ -8,6 +8,7 @@ from iqpilot.system.hardware import HARDWARE, PC, TICI from iqpilot.system.hardware.hw import Paths from iqpilot.system.manager.process import PythonProcess, NativeProcess, BundleProcess +from iqpilot.selfdrive.iqmodeld.egpu_helpers import egpu_selected, resolve_backend, usbgpu_present from iqpilot.selfdrive.iqmodeld.models.helpers import get_active_model_runner from iqpilot.konn3kt.service_health import hephaestus_ready @@ -124,6 +125,22 @@ def is_tinygrad_model(started, params, CP: car.CarParams) -> bool: """Check if the active model runner is tinygrad.""" return bool(get_active_model_runner(params, not started) == custom.IQModelManager.Runner.tinygrad) +def _egpu_present(params) -> bool: + if params.get_bool("IQEgpuDisabled"): + return False + return usbgpu_present() + + +def emac_enabled(started, params, CP: car.CarParams) -> bool: + return resolve_backend(params.get_bool("IQEmacEnabled"), egpu_selected(params), _egpu_present(params)) == "emac" + +def egpu_enabled(started, params, CP: car.CarParams) -> bool: + return (resolve_backend(params.get_bool("IQEmacEnabled"), egpu_selected(params), _egpu_present(params)) == "egpu" + and _egpu_present(params)) + +def big_model_enabled(started, params, CP: car.CarParams) -> bool: + return params.get_bool("IQEmacEnabled") or egpu_selected(params) + def hephaestus_ready_shim(started, params, CP: car.CarParams) -> bool: return hephaestus_ready(params) @@ -196,6 +213,15 @@ procs += [ # Models BundleProcess("models_manager", "iqpilot_model_selector_private", "iqpilot_private.models.manager", and_(only_offroad, not_low_power)), NativeProcess("iqmodeld", "iqpilot/selfdrive/iqmodeld", ["./iqmodeld"], and_(only_onroad, is_tinygrad_model), restart_if_crash=True), + # big-model backends: iqmodeld self-demotes to the small channel worker when + # either backend is enabled; the selector publishes, and exactly one big + # worker (Mac or eGPU, eMac wins) feeds the BIG channel + PythonProcess("modeld_selector", "iqpilot.selfdrive.iqmodeld.modeld_selector", + and_(only_onroad, and_(is_tinygrad_model, big_model_enabled)), restart_if_crash=True), + BundleProcess("maciqmodeld", "iqpilot_emac_private", "iqpilot_private.emac.maciqmodeld", + and_(only_onroad, and_(is_tinygrad_model, emac_enabled)), restart_if_crash=True), + PythonProcess("iqegpumodeld", "iqpilot.selfdrive.iqmodeld.iqegpumodeld", + and_(only_onroad, and_(is_tinygrad_model, egpu_enabled)), restart_if_crash=True), BundleProcess("backup_manager_k3", "iqpilot_hephaestusd_private", "iqpilot_private.konn3kt.backups.backup_orchestrator", and_(only_offroad, hephaestus_ready_shim, not_low_power)), diff --git a/iqpilot/tools/lib/url_file.py b/iqpilot/tools/lib/url_file.py index de86026f1..5200bb019 100644 --- a/iqpilot/tools/lib/url_file.py +++ b/iqpilot/tools/lib/url_file.py @@ -21,6 +21,10 @@ CACHE_SIZE = 10 * 1024 * 1024 * 1024 # total cache size in GB logging.getLogger("urllib3").setLevel(logging.WARNING) +USER_AGENT = os.getenv("IQPILOT_HTTP_USER_AGENT", "Mozilla/5.0 (Macintosh; Intel Mac OS X 10_15_7) " + "AppleWebKit/537.36 (KHTML, like Gecko) Chrome/128.0.0.0 Safari/537.36") + + def _env_int(name: str, default: int) -> int: raw = os.getenv(name) if raw is None: @@ -113,6 +117,10 @@ class URLFile: pass def _request(self, method: str, url: str, headers: dict[str, str] | None = None) -> BaseHTTPResponse: + # the data host is behind cloudflare, which answers a default urllib3 agent + # with a 1010 block. It reads as 403 Forbidden on a correctly signed url, so + # it looks like an auth problem and is not one. + headers = {**(headers or {}), "User-Agent": USER_AGENT} try: return URLFile.pool_manager().request(method, url, timeout=self._timeout, headers=headers) except MaxRetryError as e: diff --git a/iqpilot/tools/replay/camera.cc b/iqpilot/tools/replay/camera.cc index 671c0320b..715ad0a79 100644 --- a/iqpilot/tools/replay/camera.cc +++ b/iqpilot/tools/replay/camera.cc @@ -70,6 +70,9 @@ void CameraServer::cameraThread(Camera &cam) { .timestamp_eof = eidx.getTimestampEof(), }; vipc_server_->send(yuv, &extra); + if (++sent_count_ % 100 == 1) { + rInfo("camera[%d] vipc send #%lu frame_id=%u seg_frame=%d", cam.type, sent_count_, frame_id, segment_id); + } } else { rError("camera[%d] failed to get frame: %lu", cam.type, segment_id); } diff --git a/iqpilot/tools/replay/camera.h b/iqpilot/tools/replay/camera.h index 1d6967265..732c126d4 100644 --- a/iqpilot/tools/replay/camera.h +++ b/iqpilot/tools/replay/camera.h @@ -38,5 +38,6 @@ protected: {.type = WideRoadCam, .stream_type = VISION_STREAM_WIDE_ROAD}, }; std::atomic publishing_ = 0; + uint64_t sent_count_ = 0; std::unique_ptr vipc_server_; }; diff --git a/iqpilot/tools/replay/framereader.cc b/iqpilot/tools/replay/framereader.cc index c3c60acf6..8a2bcc45f 100644 --- a/iqpilot/tools/replay/framereader.cc +++ b/iqpilot/tools/replay/framereader.cc @@ -107,11 +107,21 @@ bool FrameReader::loadFromFile(CameraType type, const std::string &file, bool no packets_info.reserve(60 * 20); // 20fps, one minute while (!(abort && *abort) && av_read_frame(input_ctx, &pkt) == 0) { if (pkt.stream_index == video_stream_idx_) { - packets_info.emplace_back(PacketInfo{.flags = pkt.flags, .pos = pkt.pos}); + packets_info.emplace_back(PacketInfo{.flags = pkt.flags, .pos = pkt.pos, .ts = pkt.dts}); } av_packet_unref(&pkt); } - avio_seek(input_ctx->pb, 0, SEEK_SET); + // IQ.Pilot camera files are (fragmented) MP4: rewinding the raw pb leaves the + // mov demuxer's sample cursor at EOF, so rewind through the demuxer instead. + // comma's raw HEVC bitstreams have no index; only the pb rewind works there. + if (!packets_info.empty() && + avformat_seek_file(input_ctx, video_stream_idx_, INT64_MIN, + packets_info.front().ts, packets_info.front().ts, 0) < 0) { + avformat_flush(input_ctx); + avio_seek(input_ctx->pb, 0, SEEK_SET); + } + rInfo("frame index built: %zu packets, fmt=%s", packets_info.size(), + input_ctx->iformat ? input_ctx->iformat->name : "?"); return !packets_info.empty(); } @@ -147,6 +157,10 @@ bool FFmpegVideoDecoder::open(AVCodecParameters *codecpar, bool hw_decoder) { } width = (decoder_ctx->width + 3) & ~3; height = decoder_ctx->height; + // frame-threaded software decode: single-threaded can't hold 2x1928x1208@20 + // on this SoC. The added output delay is absorbed by the EAGAIN-aware loop. + decoder_ctx->thread_count = 3; + decoder_ctx->thread_type = FF_THREAD_FRAME; if (hw_decoder && !initHardwareDecoder(HW_DEVICE_TYPE)) { rWarning("No device with hardware decoder found. fallback to CPU decoding."); @@ -188,6 +202,14 @@ bool FFmpegVideoDecoder::initHardwareDecoder(AVHWDeviceType hw_device_type) { bool FFmpegVideoDecoder::decode(FrameReader *reader, int idx, VisionBuf *buf) { int current_idx = idx; if (idx != reader->prev_idx + 1) { + if (idx > reader->prev_idx && idx - reader->prev_idx <= 300) { + // forward catch-up: the decoder is already positioned at prev_idx+1, and + // sequential decode is cheaper and (for raw H.264) more reliable than a + // byte seek plus keyframe re-decode + current_idx = reader->prev_idx + 1; + reader->prev_idx = idx; + goto read_packets; + } // seeking to the nearest key frame for (int i = idx; i >= 0; --i) { if (reader->packets_info[i].flags & AV_PKT_FLAG_KEY) { @@ -198,6 +220,12 @@ bool FFmpegVideoDecoder::decode(FrameReader *reader, int idx, VisionBuf *buf) { auto pos = reader->packets_info[current_idx].pos; int ret = avformat_seek_file(reader->input_ctx, 0, pos, pos, pos, AVSEEK_FLAG_BYTE); + if (ret < 0) { + // mp4 containers reject byte seeks; seek the keyframe by timestamp + // through the mov index instead + auto ts = reader->packets_info[current_idx].ts; + ret = avformat_seek_file(reader->input_ctx, reader->video_stream_idx_, INT64_MIN, ts, ts, 0); + } if (ret < 0) { rError("Failed to seek to byte position %lld: %d", pos, AVERROR(ret)); return false; @@ -206,26 +234,42 @@ bool FFmpegVideoDecoder::decode(FrameReader *reader, int idx, VisionBuf *buf) { } reader->prev_idx = idx; +read_packets: + // H.264 has decoder delay: the first packets may legitimately yield no frame + // yet (EAGAIN), and one packet can release several buffered frames. comma's + // zero-delay HEVC never exercised either case. AVPacket pkt; - while (av_read_frame(reader->input_ctx, &pkt) >= 0) { - // Skip non-video packets + int rf_ret; + while ((rf_ret = av_read_frame(reader->input_ctx, &pkt)) >= 0) { if (pkt.stream_index != reader->video_stream_idx_) { av_packet_unref(&pkt); continue; } - - AVFrame *frame = decodeFrame(&pkt); + int ret = avcodec_send_packet(decoder_ctx, &pkt); av_packet_unref(&pkt); - if (!frame) { - rError("Failed to decode frame at index %d", current_idx); + if (ret < 0) { + rError("Error sending a packet for decoding: %d", ret); return false; } - - if (current_idx++ == idx) { - return copyBuffer(frame, buf); + while ((ret = avcodec_receive_frame(decoder_ctx, av_frame_)) == 0) { + AVFrame *frame = av_frame_; + if (av_frame_->format == hw_pix_fmt) { + if (av_hwframe_transfer_data(hw_frame_, av_frame_, 0) < 0) { + rError("error transferring frame data from GPU to CPU"); + return false; + } + frame = hw_frame_; + } + if (current_idx++ == idx) { + return copyBuffer(frame, buf); + } + } + if (ret != AVERROR(EAGAIN)) { + rError("avcodec_receive_frame error: %d", ret); + return false; } } - rError("Failed to find frame at index %d", idx); + rError("Failed to find frame at index %d (read ret=%d)", idx, rf_ret); return false; } @@ -268,20 +312,46 @@ bool FFmpegVideoDecoder::copyBuffer(AVFrame *f, VisionBuf *buf) { } #ifndef __APPLE__ +QcomVideoDecoder::~QcomVideoDecoder() { + if (bsf_) av_bsf_free(&bsf_); +} + bool QcomVideoDecoder::open(AVCodecParameters *codecpar, bool hw_decoder) { - if (codecpar->codec_id != AV_CODEC_ID_HEVC) { - rError("Hardware decoder only supports HEVC codec"); + // msm_vidc decodes both; IQ.Pilot recordings are H.264 while comma's are HEVC + uint32_t v4l2_fmt; + if (codecpar->codec_id == AV_CODEC_ID_HEVC) { + v4l2_fmt = V4L2_PIX_FMT_HEVC; + } else if (codecpar->codec_id == AV_CODEC_ID_H264) { + v4l2_fmt = V4L2_PIX_FMT_H264; + if (codecpar->extradata && codecpar->extradata_size > 0) { + // mp4 carries AVCC (length-prefixed NALs, headers out-of-band); the V4L2 + // decoder wants an Annex-B bitstream with in-band SPS/PPS + const AVBitStreamFilter *f = av_bsf_get_by_name("h264_mp4toannexb"); + if (!f || av_bsf_alloc(f, &bsf_) < 0 || + avcodec_parameters_copy(bsf_->par_in, codecpar) < 0 || av_bsf_init(bsf_) < 0) { + rError("failed to set up h264_mp4toannexb filter"); + return false; + } + } + } else { + rError("Hardware decoder only supports HEVC and H.264 codecs"); return false; } width = codecpar->width; height = codecpar->height; - msm_vidc.init(VIDEO_DEVICE, width, height, V4L2_PIX_FMT_HEVC); + msm_vidc.init(VIDEO_DEVICE, width, height, v4l2_fmt); return true; } bool QcomVideoDecoder::decode(FrameReader *reader, int idx, VisionBuf *buf) { int from_idx = idx; if (idx != reader->prev_idx + 1) { + if (idx > reader->prev_idx && idx - reader->prev_idx <= 300) { + // forward catch-up: the decoder is already positioned at prev_idx+1, and + // sequential decode is cheaper and (for raw H.264) more reliable than a + // byte seek plus keyframe re-decode + from_idx = reader->prev_idx + 1; + } else { // seeking to the nearest key frame for (int i = idx; i >= 0; --i) { if (reader->packets_info[i].flags & AV_PKT_FLAG_KEY) { @@ -292,10 +362,17 @@ bool QcomVideoDecoder::decode(FrameReader *reader, int idx, VisionBuf *buf) { auto pos = reader->packets_info[from_idx].pos; int ret = avformat_seek_file(reader->input_ctx, 0, pos, pos, pos, AVSEEK_FLAG_BYTE); + if (ret < 0) { + // mp4 containers reject byte seeks; seek the keyframe by timestamp + // through the mov index instead + auto ts = reader->packets_info[from_idx].ts; + ret = avformat_seek_file(reader->input_ctx, reader->video_stream_idx_, INT64_MIN, ts, ts, 0); + } if (ret < 0) { rError("Failed to seek to byte position %lld: %d", pos, AVERROR(ret)); return false; } + } } reader->prev_idx = idx; bool result = false; @@ -303,6 +380,13 @@ bool QcomVideoDecoder::decode(FrameReader *reader, int idx, VisionBuf *buf) { msm_vidc.avctx = reader->input_ctx; for (int i = from_idx; i <= idx; ++i) { if (av_read_frame(reader->input_ctx, &pkt) == 0) { + if (bsf_ != nullptr) { + if (av_bsf_send_packet(bsf_, &pkt) < 0 || av_bsf_receive_packet(bsf_, &pkt) < 0) { + rError("h264_mp4toannexb failed at index %d", i); + av_packet_unref(&pkt); + return false; + } + } result = msm_vidc.decodeFrame(&pkt, buf) && (i == idx); av_packet_unref(&pkt); } diff --git a/iqpilot/tools/replay/framereader.h b/iqpilot/tools/replay/framereader.h index d8e86fce0..98c7ef1cc 100644 --- a/iqpilot/tools/replay/framereader.h +++ b/iqpilot/tools/replay/framereader.h @@ -37,6 +37,7 @@ public: struct PacketInfo { int flags; int64_t pos; + int64_t ts; // dts; byte pos is useless for seeking in mp4 containers }; std::vector packets_info; }; @@ -72,11 +73,12 @@ private: class QcomVideoDecoder : public VideoDecoder { public: QcomVideoDecoder() {}; - ~QcomVideoDecoder() override {}; + ~QcomVideoDecoder() override; bool open(AVCodecParameters *codecpar, bool hw_decoder) override; bool decode(FrameReader *reader, int idx, VisionBuf *buf) override; private: MsmVidc msm_vidc = MsmVidc(); + AVBSFContext *bsf_ = nullptr; // AVCC (mp4) -> Annex-B for msm_vidc }; #endif diff --git a/iqpilot/tools/replay/main.cc b/iqpilot/tools/replay/main.cc index 30f85b170..7cc737946 100644 --- a/iqpilot/tools/replay/main.cc +++ b/iqpilot/tools/replay/main.cc @@ -1,4 +1,5 @@ #include +#include #include #include @@ -176,6 +177,15 @@ int main(int argc, char *argv[]) { return 0; } + // REPLAY_HEADLESS: skip ncurses, which needs a real TTY and swallows all + // replay log output into its UI — required when driven from a service + if (getenv("REPLAY_HEADLESS") != nullptr) { + replay.start(config.start_seconds); + while (true) { + pause(); + } + } + ConsoleUI console_ui(&replay); replay.start(config.start_seconds); return console_ui.exec(); diff --git a/iqpilot/tools/replay/qcom_decoder.cc b/iqpilot/tools/replay/qcom_decoder.cc index eb5409daa..675db1a8f 100644 --- a/iqpilot/tools/replay/qcom_decoder.cc +++ b/iqpilot/tools/replay/qcom_decoder.cc @@ -43,7 +43,7 @@ bool MsmVidc::init(const char* dev, size_t width, size_t height, uint64_t codec) } subscribeEvents(); v4l2_buf_type out_type = V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE; - setPlaneFormat(out_type, V4L2_PIX_FMT_HEVC); // Also allocates the output buffer + setPlaneFormat(out_type, codec); // Also allocates the output buffer setFPS(FPS); request_buffers(fd, out_type, OUTPUT_BUFFER_COUNT); util::safe_ioctl(fd, VIDIOC_STREAMON, &out_type, "VIDIOC_STREAMON OUTPUT failed"); diff --git a/iqpilot/ui/layouts/settings/iq_panels.py b/iqpilot/ui/layouts/settings/iq_panels.py index 512e7b06e..529f4c93f 100644 --- a/iqpilot/ui/layouts/settings/iq_panels.py +++ b/iqpilot/ui/layouts/settings/iq_panels.py @@ -1590,6 +1590,31 @@ _ACTIVE_BUNDLE_KEY = "ModelManager_ActiveBundle" _DOWNLOAD_INDEX_KEY = "ModelManager_DownloadIndex" _RUNNER_CACHE_KEY = "ModelRunnerTypeCache" +def _big_model_options() -> list[tuple[str, str]]: + try: + from iqpilot.selfdrive.iqmodeld.emac_model_meta import big_models + return big_models(ui_state.params) + except Exception: + return [] + + +def _big_model_label(key: str) -> str: + for name, display in _big_model_options(): + if name == key: + return display + return key + + +def _refresh_big_catalog() -> None: + def worker(): + try: + from iqpilot.selfdrive.iqmodeld.emac_model_meta import refresh_catalog + refresh_catalog(ui_state.params) + except Exception: + pass + threading.Thread(target=worker, daemon=True).start() + + class ModelsLayout(Widget): def __init__(self): super().__init__() @@ -1599,6 +1624,7 @@ class ModelsLayout(Widget): self.download_status = None self.prev_download_status = None self.model_dialog = None + self._big_model_dialog = None self.last_cache_calc_time = 0 self._initialize_items() @@ -1614,6 +1640,14 @@ class ModelsLayout(Widget): callback=self._handle_current_model_clicked ) + self.big_model_item = button_item( + lambda: tr("Big Model"), + lambda: tr("CHANGE"), + tr("Only works with external compute connected over USB."), + self._handle_big_model_clicked, + ) + self.big_model_item.action_item.set_value(self._big_model_value()) + self.supercombo_label = progress_item(tr("Combined Model")) self.vision_label = progress_item(tr("Vision Weights")) self.policy_label = progress_item(tr("Policy Weights")) @@ -1630,7 +1664,7 @@ class ModelsLayout(Widget): self.redownload_item = button_item(lambda: tr("Redownload Current Model"), lambda: tr("REDOWNLOAD"), "", self._redownload_model) self.cancel_download_item = button_item(tr("Stop Download"), tr("Cancel"), "", self._cancel_model_request) - self.items = [self.current_model_item, self.cancel_download_item, self.supercombo_label, self.vision_label, + self.items = [self.current_model_item, self.big_model_item, self.cancel_download_item, self.supercombo_label, self.vision_label, self.policy_label, self.redownload_item, self.refresh_item, self.clear_cache_item] def _is_downloading(self): @@ -1880,7 +1914,40 @@ class ModelsLayout(Widget): get_folders_fn=self._get_folders, on_exit=self._on_model_selected) gui_app.set_modal_overlay(self.model_dialog, callback=self._on_model_selected) + @staticmethod + def _big_model_value() -> str: + if not ui_state.params.get_bool("IQEmacEnabled"): + return tr("Off") + key = ui_state.params.get("IQEmacModel") + key = key.decode() if isinstance(key, bytes) else (key or "") + return _big_model_label(key) if key in [n for n, _ in _big_model_options()] else tr("Off") + + def _handle_big_model_clicked(self): + options = _big_model_options() + if len(options) <= 1: + _refresh_big_catalog() + keys = [n for n, _ in options] + labels = [tr("Off")] + [d for _, d in options] + self._big_model_dialog = MultiOptionDialog(tr("Big Model"), labels, self._big_model_value()) + + def handle_selection(result): + if result == DialogResult.CONFIRM and self._big_model_dialog is not None and self._big_model_dialog.selection: + selected = self._big_model_dialog.selection + if selected == tr("Off"): + ui_state.params.put_bool("IQEmacEnabled", False) + else: + for key in keys: + if _big_model_label(key) == selected: + ui_state.params.put("IQEmacModel", key) + ui_state.params.put_bool("IQEmacEnabled", True) + break + self.big_model_item.action_item.set_value(self._big_model_value()) + self._big_model_dialog = None + + gui_app.set_modal_overlay(self._big_model_dialog, callback=handle_selection) + def _on_refresh_models(self): + _refresh_big_catalog() ui_state.params.put("ModelManager_LastSyncTime", 0) self._refreshing = True self._refresh_start = time.monotonic() diff --git a/iqpilot/ui/mici/onroad/emac_source.py b/iqpilot/ui/mici/onroad/emac_source.py new file mode 100644 index 000000000..4adb1801a --- /dev/null +++ b/iqpilot/ui/mici/onroad/emac_source.py @@ -0,0 +1,43 @@ +""" +Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos/ +""" + +import time + +import pyray as rl + +from iqpilot.common.params import Params +from iqpilot.ui.onroad.big_model_status import SourceState, draw_source_label, resolve_source +from iqpilot.selfdrive.ui.ui_state import ui_state +from iqpilot.system.ui.lib.application import FontWeight, gui_app +from iqpilot.system.ui.lib.text_measure import measure_text_cached + +_POLL_S = 1.0 +_FONT_SIZE = 44 +_WHEEL_H = 50 +_MARGIN_R = 12 +_MARGIN_B = 14 + + +class EmacSourceIndicator: + def __init__(self): + self._params = Params() + self._font = gui_app.font(FontWeight.SEMI_BOLD) + self._last_poll = 0.0 + self._label = "" + self._state = SourceState.HIDDEN + + def update(self) -> None: + now = time.monotonic() + if now - self._last_poll < _POLL_S: + return + self._last_poll = now + self._label, self._state = resolve_source(self._params, ui_state.engaged) + + def render(self, rect: rl.Rectangle) -> None: + if self._state == SourceState.HIDDEN: + return + size = measure_text_cached(self._font, self._label, _FONT_SIZE) + x = rect.x + rect.width - _MARGIN_R - size.x + y = rect.y + rect.height - _MARGIN_B - (_WHEEL_H + size.y) / 2 + draw_source_label(self._font, self._label, self._state, rl.Vector2(x, y), _FONT_SIZE) diff --git a/iqpilot/ui/mici/onroad/hud_renderer.py b/iqpilot/ui/mici/onroad/hud_renderer.py index f5fc67f4c..4dfc6fa21 100644 --- a/iqpilot/ui/mici/onroad/hud_renderer.py +++ b/iqpilot/ui/mici/onroad/hud_renderer.py @@ -5,11 +5,12 @@ import pyray as rl from iqpilot.selfdrive.ui.mici.onroad.hud_renderer import HudRenderer from iqpilot.ui.onroad.hud_overlays import IQBlindSpotOverlay +from iqpilot.ui.mici.onroad.emac_source import EmacSourceIndicator class IQMiciHudRenderer(HudRenderer): def __init__(self): super().__init__() - self._overlays = [IQBlindSpotOverlay()] + self._overlays = [IQBlindSpotOverlay(), EmacSourceIndicator()] def _update_state(self) -> None: super()._update_state() diff --git a/iqpilot/ui/onroad/big_model_status.py b/iqpilot/ui/onroad/big_model_status.py new file mode 100644 index 000000000..a4e1537d3 --- /dev/null +++ b/iqpilot/ui/onroad/big_model_status.py @@ -0,0 +1,71 @@ +""" +Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos/ +""" +import math +from enum import IntEnum + +import pyray as rl + +from iqpilot.common.params import Params +from iqpilot.system.ui.lib.text_measure import measure_text_cached + +class SourceState(IntEnum): + HIDDEN = 0 + LOADING = 1 + ACTIVE = 2 + FAILED = 3 + CROSSED = 4 + + +_GREEN = rl.Color(46, 204, 113, 255) +_ORANGE = rl.Color(255, 115, 0, 255) +_WHITE = rl.Color(255, 255, 255, 255) + + +def _emac_state(params: Params, engaged: bool) -> SourceState: + if not params.get_bool("MacModelReachable"): + return SourceState.HIDDEN + if params.get_bool("MacModelActive"): + return SourceState.ACTIVE + if params.get_bool("MacModelFailed"): + return SourceState.CROSSED if engaged else SourceState.FAILED + return SourceState.LOADING + + +def _egpu_state(params: Params, engaged: bool) -> SourceState: + if not params.get_bool("UsbGpuPresent"): + return SourceState.HIDDEN + if params.get_bool("UsbGpuActive"): + return SourceState.ACTIVE + if params.get_bool("UsbGpuFailed"): + return SourceState.CROSSED if engaged else SourceState.FAILED + return SourceState.LOADING + + +def resolve_source(params: Params, engaged: bool) -> tuple[str, SourceState]: + if params.get_bool("IQEmacEnabled"): + return "MAC", _emac_state(params, engaged) + if params.get_bool("UsbGpuPresent") or params.get_bool("IQEgpuEnabled"): + return "GPU", _egpu_state(params, engaged) + return "", SourceState.HIDDEN + + +def draw_source_label(font: rl.Font, label: str, state: SourceState, + pos: rl.Vector2, font_size: int) -> None: + if state == SourceState.HIDDEN or not label: + return + if state == SourceState.ACTIVE: + color, opacity, strike = _GREEN, 1.0, False + elif state == SourceState.FAILED: + color, opacity, strike = _ORANGE, 1.0, False + elif state == SourceState.CROSSED: + color, opacity, strike = _WHITE, 0.65, True + else: + color, opacity, strike = _WHITE, 0.35 + 0.65 * (0.5 - 0.5 * math.cos(rl.get_time() * 6.0)), False + + col = rl.Color(color.r, color.g, color.b, int(255 * opacity)) + rl.draw_text_ex(font, label, pos, font_size, 0, col) + if strike: + size = measure_text_cached(font, label, font_size) + y = int(pos.y + size.y / 2) + rl.draw_line_ex(rl.Vector2(pos.x - 2, y), rl.Vector2(pos.x + size.x + 2, y), 4, col) diff --git a/iqpilot/ui/onroad/emac_status.py b/iqpilot/ui/onroad/emac_status.py new file mode 100644 index 000000000..efcb65955 --- /dev/null +++ b/iqpilot/ui/onroad/emac_status.py @@ -0,0 +1,44 @@ +""" +Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos/ +""" + +import time + +import pyray as rl + +from iqpilot.common.params import Params +from iqpilot.ui.onroad.big_model_status import SourceState, draw_source_label, resolve_source +from iqpilot.selfdrive.ui import UI_BORDER_SIZE +from iqpilot.selfdrive.ui.onroad.driver_state import BTN_SIZE +from iqpilot.selfdrive.ui.ui_state import ui_state +from iqpilot.system.ui.lib.application import FontWeight, gui_app +from iqpilot.system.ui.lib.text_measure import measure_text_cached +from iqpilot.system.ui.widgets import Widget + +_POLL_S = 1.0 +_FONT_SIZE = 70 + + +class EmacStatusRenderer(Widget): + def __init__(self): + super().__init__() + self._params = Params() + self._font = gui_app.font(FontWeight.SEMI_BOLD) + self._last_poll = 0.0 + self._label = "" + self._state = SourceState.HIDDEN + + def update(self): + now = time.monotonic() + if now - self._last_poll < _POLL_S: + return + self._last_poll = now + self._label, self._state = resolve_source(self._params, ui_state.engaged) + + def _render(self, rect: rl.Rectangle): + if self._state == SourceState.HIDDEN: + return + size = measure_text_cached(self._font, self._label, _FONT_SIZE) + x = rect.x + UI_BORDER_SIZE + BTN_SIZE // 2 - size.x / 2 + y = rect.y + rect.height / 2 - size.y / 2 + draw_source_label(self._font, self._label, self._state, rl.Vector2(x, y), _FONT_SIZE) diff --git a/iqpilot/ui/onroad/hud_renderer.py b/iqpilot/ui/onroad/hud_renderer.py index 55d9bffe1..c4c6d9b76 100644 --- a/iqpilot/ui/onroad/hud_renderer.py +++ b/iqpilot/ui/onroad/hud_renderer.py @@ -16,6 +16,7 @@ from iqpilot.ui.onroad.hud_overlays import ( ) from iqpilot.ui.onroad.nav_map_panel import NavMapPanel from iqpilot.ui.onroad.soft_warning import SoftWarningRenderer +from iqpilot.ui.onroad.emac_status import EmacStatusRenderer ENABLE_FLOATING_NAV_MAP_PANEL = False ENABLE_SPLIT_NAV_MAP_PANEL = True @@ -25,6 +26,7 @@ class IQHudRenderer(HudRenderer): def __init__(self): super().__init__() self.developer_ui = IQDevMetricsOverlay() + self.emac_status = EmacStatusRenderer() self.nav_map_panel = NavMapPanel() self.road_name_renderer = RoadNameRenderer() self.rocket_fuel = IQAccelBar() @@ -38,6 +40,7 @@ class IQHudRenderer(HudRenderer): super()._update_state() if ENABLE_FLOATING_NAV_MAP_PANEL or ENABLE_SPLIT_NAV_MAP_PANEL: self.nav_map_panel.update() + self.emac_status.update() self.road_name_renderer.update() self.speed_limit_renderer.update() has_limit = self.speed_limit_renderer.speed_limit_valid or self.speed_limit_renderer.speed_limit_last_valid @@ -65,6 +68,7 @@ class IQHudRenderer(HudRenderer): self.developer_ui.render(rect) if ENABLE_FLOATING_NAV_MAP_PANEL: self.nav_map_panel.render(rect) + self.emac_status.render(rect) self.road_name_renderer.render(torque_rect) self.turn_signal_controller.render(rect) self.soft_warning_renderer.render(rect) diff --git a/iqpilot/ui/tests/test_big_model_status.py b/iqpilot/ui/tests/test_big_model_status.py new file mode 100644 index 000000000..46af2707a --- /dev/null +++ b/iqpilot/ui/tests/test_big_model_status.py @@ -0,0 +1,66 @@ +""" +Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos/ + +The BIG (tici/tizi) and SMALL (mici) onroad source indicators share one +resolver (comma PR #38492 states, backend-aware label). Pin its truth table. +""" +from iqpilot.ui.onroad.big_model_status import SourceState, resolve_source + + +class _FakeParams: + def __init__(self, **flags): + self._flags = flags + + def get_bool(self, key: str) -> bool: + return bool(self._flags.get(key, False)) + + +def _resolve(engaged=False, **flags): + return resolve_source(_FakeParams(**flags), engaged) + + +def test_no_backend_enabled_is_hidden(): + assert _resolve() == ("", SourceState.HIDDEN) + + +def test_emac_label_is_mac(): + label, _ = _resolve(IQEmacEnabled=True, MacModelReachable=True, MacModelActive=True) + assert label == "MAC" + + +def test_egpu_label_is_gpu(): + label, _ = _resolve(IQEgpuEnabled=True, UsbGpuPresent=True, UsbGpuActive=True) + assert label == "GPU" + + +def test_emac_wins_when_both_enabled(): + label, _ = _resolve(IQEmacEnabled=True, IQEgpuEnabled=True, + MacModelReachable=True, UsbGpuPresent=True) + assert label == "MAC" + + +def test_egpu_states_mirror_emac(): + assert _resolve(IQEgpuEnabled=True)[1] == SourceState.HIDDEN # not present + assert _resolve(IQEgpuEnabled=True, UsbGpuPresent=True)[1] == SourceState.LOADING + assert _resolve(IQEgpuEnabled=True, UsbGpuPresent=True, UsbGpuActive=True)[1] == SourceState.ACTIVE + assert _resolve(IQEgpuEnabled=True, UsbGpuPresent=True, UsbGpuFailed=True)[1] == SourceState.FAILED + assert _resolve(engaged=True, IQEgpuEnabled=True, UsbGpuPresent=True, + UsbGpuFailed=True)[1] == SourceState.CROSSED + + +def test_emac_states(): + assert _resolve(IQEmacEnabled=True)[1] == SourceState.HIDDEN # unreachable + assert _resolve(IQEmacEnabled=True, MacModelReachable=True)[1] == SourceState.LOADING + assert _resolve(IQEmacEnabled=True, MacModelReachable=True, MacModelActive=True)[1] == SourceState.ACTIVE + assert _resolve(IQEmacEnabled=True, MacModelReachable=True, MacModelFailed=True)[1] == SourceState.FAILED + # engaged on small because big failed -> crossed + assert _resolve(engaged=True, IQEmacEnabled=True, MacModelReachable=True, + MacModelFailed=True)[1] == SourceState.CROSSED + # active always wins over a stale failed flag + assert _resolve(IQEmacEnabled=True, MacModelReachable=True, + MacModelActive=True, MacModelFailed=True)[1] == SourceState.ACTIVE + + +def test_failed_but_disconnected_is_hidden(): + # unplugged mid-drive: nothing to show, not a red/orange "failed" + assert _resolve(IQEmacEnabled=True, MacModelReachable=False, MacModelFailed=True)[1] == SourceState.HIDDEN