diff --git a/CHANGELOG.md b/CHANGELOG.md index db37ec8..6aa47db 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -21,6 +21,10 @@ SLC can now also raise your cruise speed automatically when the speed limit incr IQ.Pilot now detects upcoming speed cameras, red light cameras, and ALPR/surveillance cameras (including Flock Safety cameras) sourced from OpenStreetMap's and alerts you before you reach them. Each camera type has its own toggle so you can pick what you want to be warned about. Speed cameras can also trigger a speed reduction to the limit when detected if enabled. Camera data is sourced from OSM and is updated periodically. +**Direct Flock / ALPR Camera Detection (Bluetooth & WiFi)** + +Beyond map data, IQ.Pilot can now spot Flock Safety and similar ALPR cameras directly over the air by their Bluetooth and WiFi signatures as you approach them. Because it's sensing the actual hardware rather than relying on a map, this works anywhere, including fully offline and even for cameras that haven't been mapped yet, so you get a heads-up the moment one is nearby. When IQ.Pilot picks up a camera directly, that live detection takes priority over map data, so you see a single clear "Flock Camera Detected" alert instead of a duplicate. It shares the same Flock camera alert toggle, runs quietly in the background only when that's enabled, and is built to stay out of the way of your Bluetooth (phone link, game controllers) and WiFi connections. + **IQ.Dynamic and Driving Behavior** In IQ.Dynamic blended mode, when IQ.Pilot sees a stop light ahead, and the model agrees you need to stop, and there's no lead car to track, it will now commit (force) to stopping on its own, no lead car required. Gas pedal overrides it instantly. The stop prediction horizon is adjustable in IQ.Dynamic settings. Behavior for curves, low-speed driving, stopped leads, speed-limit fallbacks, and vision-based stops is now configurable. On-device IQ.Dynamic tuning is accessible by double-tapping IQ.Dynamic in longitudinal mode selection. diff --git a/artifacts/iqpilot_alc_private/manifest.json b/artifacts/iqpilot_alc_private/manifest.json index 7130c64..fb42059 100644 --- a/artifacts/iqpilot_alc_private/manifest.json +++ b/artifacts/iqpilot_alc_private/manifest.json @@ -16,13 +16,13 @@ }, "python/iqpilot_private/konn3kt/iqlvbs/alc.cpython-312-aarch64-linux-gnu.so": { "mode": 493, - "sha256": "77a601a12a498f09f01c4b1618e164638d5d9702913b6d5bb30a3c729a1c7d54", + "sha256": "16990383c7ba1d2f3d33e5e4c1746019101bc79bed867721b51de853a68002ed", "size": 401688 }, "python/iqpilot_private/konn3kt/iqlvbs/vehicle_state.cpython-312-aarch64-linux-gnu.so": { "mode": 493, - "sha256": "ca91f99d94c8bb7842332a69896d4b3a31c109c027426d1d7386876f11710d80", - "size": 69696 + "sha256": "bd060a8527d21501ce866f24d055f1c8bb03209b2730eb745b786780aace0aae", + "size": 69688 }, "runtime": { "entries": { @@ -37,7 +37,7 @@ } }, "signatures": { - "python/iqpilot_private/konn3kt/iqlvbs/alc.cpython-312-aarch64-linux-gnu.so": "4KY001fMJhdkiQNoUXbvtOKQ2nWocmg9ZL0umWnDjAztueIstXBoc1iqW83plm2Xq0jR9NLk1Jd4yl3AydWFBA==", - "python/iqpilot_private/konn3kt/iqlvbs/vehicle_state.cpython-312-aarch64-linux-gnu.so": "JqSaxsDJT9q0UYgVJUo9Pqm0J0Aqwy5gIa2C8n9/ioOg/fV6PLn2KiVGn6470U4F5oNaaOgAmm+AM9jwYrZXAA==" + "python/iqpilot_private/konn3kt/iqlvbs/alc.cpython-312-aarch64-linux-gnu.so": "IVz01J4HpdUHDrjPP9yB+9ksGFberB9MeFiCNLcv8hMzXCLIjIATl3JXTwpBqU8Kad/q683nkhgL5IJ3tCKdCw==", + "python/iqpilot_private/konn3kt/iqlvbs/vehicle_state.cpython-312-aarch64-linux-gnu.so": "/aW/StEMRpge+zzihQDK7VPLmKhgwCucX8JielvPnPTud/mk0pmCy5pE8fB+TbwtWZCBfVhEd1bidRgsBqZfBA==" } } 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 e27935a..ffc6160 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 f0abb2a..c86c5cd 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_hephaestusd_private/manifest.json b/artifacts/iqpilot_hephaestusd_private/manifest.json index 3249777..c56ced4 100644 --- a/artifacts/iqpilot_hephaestusd_private/manifest.json +++ b/artifacts/iqpilot_hephaestusd_private/manifest.json @@ -16,27 +16,27 @@ }, "python/iqpilot_private/konn3kt/backups/archive_codec.cpython-312-aarch64-linux-gnu.so": { "mode": 493, - "sha256": "dfc76d7b791879501396d4de4891718d0052d23b1e6cb217b0be01f3e3e6d857", + "sha256": "1ca35a437eaf1c50ce6f58eb38cdfb56eacab6f7d5201eef446495a68d90d69b", "size": 135536 }, "python/iqpilot_private/konn3kt/backups/backup_keys.cpython-312-aarch64-linux-gnu.so": { "mode": 493, - "sha256": "d1a7fd4e222b08c1e574ed1188943acddecc8d3f5ac02a1983bf90bde5e00a2c", + "sha256": "98039925b5104eb48b69d2ea6a34600daab8251d926fecab666ac51e565d1571", "size": 67664 }, "python/iqpilot_private/konn3kt/backups/backup_orchestrator.cpython-312-aarch64-linux-gnu.so": { "mode": 493, - "sha256": "5393274eb99f50917fef30f994b1ad5fab66860bd535b7d270a86d144d4e25b7", + "sha256": "85edb71bacde7958772f85a07e56506e974214756198b0fc8b91a21188d336e6", "size": 204624 }, "python/iqpilot_private/konn3kt/backups/cbc_vault.cpython-312-aarch64-linux-gnu.so": { "mode": 493, - "sha256": "9c08c3f6e3dc3b73f1a28d50d5aa76fa0894e9cd83bf02b7e9cc83bda5e84b4e", + "sha256": "1c8a4bf98aa378c3a4d924725a6f8f42c4de13452a18064bc76539354659d84e", "size": 69752 }, "python/iqpilot_private/konn3kt/backups/imahelper.cpython-312-aarch64-linux-gnu.so": { "mode": 493, - "sha256": "fb417b7fed56353caad6d5a96429952ff0b87ee1bab7d6a8f0856d1ca40e4b4a", + "sha256": "a5ce7828c559a5841317d79e01a7e571debbce4d4992827aedab27d6d2b2eb15", "size": 203072 }, "python/iqpilot_private/konn3kt/flockd/__init__.py": { @@ -46,12 +46,12 @@ }, "python/iqpilot_private/konn3kt/flockd/flockd.cpython-312-aarch64-linux-gnu.so": { "mode": 493, - "sha256": "73164bb5c3eafe0a5409508140c093503ec40c1c58d0a8e79281bac3d22d811c", + "sha256": "2fe267d00d04e9f94ef16885cb88b4b28ad7f6e1b1ca41caf23883d46ab03961", "size": 202168 }, "python/iqpilot_private/konn3kt/flockd/signatures.cpython-312-aarch64-linux-gnu.so": { "mode": 493, - "sha256": "fb6d56fbec20965d988f936c46e7a70f31def71a2fba27a6506567b3c13574cf", + "sha256": "d3ff0a6d4d9f1653418de8fc3fa52baed0067c48c02438b30e1d27cf92e5aeb4", "size": 68080 }, "python/iqpilot_private/konn3kt/hephaestus/__init__.py": { @@ -61,67 +61,67 @@ }, "python/iqpilot_private/konn3kt/hephaestus/_vendor/localapi_runtime.zip": { "mode": 420, - "sha256": "52026a3b96c4e7eee5cb33d70a323896ab0de02557196980653afb99498af84d", + "sha256": "1854ddf5a9e969ce210acf55fd4e53cc158e0117c67f65132e9c21fb49d1b53e", "size": 2100238 }, "python/iqpilot_private/konn3kt/hephaestus/ble_auth.cpython-312-aarch64-linux-gnu.so": { "mode": 493, - "sha256": "aa3530a7f7a44ba1de0d8c1b5de3d26405bf0ee0b9599190bfbbea55d2ed2071", + "sha256": "983a5825aaf5178081c5f44f1ba7e950b117774fcf751881c013dd05796b70d4", "size": 268064 }, "python/iqpilot_private/konn3kt/hephaestus/ble_gatt.cpython-312-aarch64-linux-gnu.so": { "mode": 493, - "sha256": "5a6bf50d4f8e3ec43603c2b3d9743dd650b37c6dd2075ec19266d0f1368b8cff", + "sha256": "87586aec493893ccae8c67fc89fc71006762f87100d3d396d9735d6603b86cb9", "size": 340584 }, "python/iqpilot_private/konn3kt/hephaestus/ble_rpc_dispatch.cpython-312-aarch64-linux-gnu.so": { "mode": 493, - "sha256": "ff36bfb143294790a6ab0e2fe3d14852f3b04374185ef19a614231ca56a40999", + "sha256": "cff0554eebc045f8d46d5153d88949123ddc26cc251dec1fe6303c1911b9c8ad", "size": 68048 }, "python/iqpilot_private/konn3kt/hephaestus/ble_transportd.cpython-312-aarch64-linux-gnu.so": { "mode": 493, - "sha256": "ea54bc8d8a2ba2d803ce782898a6987457d248945da81cb9ab08bc32f6763fe1", + "sha256": "a1f10df44d7a49066b2383ad06dde00337f9dc2126f5eeeeca5a7d6c4689da07", "size": 335728 }, "python/iqpilot_private/konn3kt/hephaestus/bt_gamepad.cpython-312-aarch64-linux-gnu.so": { "mode": 493, - "sha256": "f7706a6cabf49660c48609ca4a85c840c1e22e5e4b17d250114e25b56a7b9c34", + "sha256": "d0f12e27574b34033a86e29e186d4a551cd8c9f03c2e478ac656003ce164cbc9", "size": 271680 }, "python/iqpilot_private/konn3kt/hephaestus/cloud_routes.cpython-312-aarch64-linux-gnu.so": { "mode": 493, - "sha256": "5fc96f8ac2c67f3ff660b8c060d14b0e526cba7eaa5ce64fae94f3d0e784d417", + "sha256": "ab76c8af88512b3839150810f9083f26f0238ac644585a1cb9d4e462a2ea92b2", "size": 134408 }, "python/iqpilot_private/konn3kt/hephaestus/hephaestusd.cpython-312-aarch64-linux-gnu.so": { "mode": 493, - "sha256": "85ecde61b3fcc0d18dc260855a94eb8d30ab2e02adbfea67e209b3fe320608b1", - "size": 3514416 + "sha256": "80183a42c221b9ac60681ad585d629fe22313f655e8dcb955dc05128bcf16147", + "size": 3647464 }, "python/iqpilot_private/konn3kt/hephaestus/kwp2000.cpython-312-aarch64-linux-gnu.so": { "mode": 493, - "sha256": "c7b1b7a441f9c4ad395efb3de26d972379d9927f9c7ea9c121d1d06b8170139b", + "sha256": "05bac25363b7dc2b1cab18fbf54511c98fc8806034eba759f56008451efd1c64", "size": 136448 }, "python/iqpilot_private/konn3kt/hephaestus/manage_hephaestusd.cpython-312-aarch64-linux-gnu.so": { "mode": 493, - "sha256": "a7fddf4e77d804f255ae4821fe64eada65ee6f2cc41cfb5c995acd497a0caca4", + "sha256": "0e3104b80f8eb480caa673fee837f9b8159e479f8761c443caa3e88f6d65fc82", "size": 68136 }, "python/iqpilot_private/konn3kt/hephaestus/motd.cpython-312-aarch64-linux-gnu.so": { "mode": 493, - "sha256": "2ff9ebdb8887bb71a971a4d06fd362c51f5a4446595a073a45edc8bb630a642a", + "sha256": "3ce3d559312252ab44fd4b2d68b25393751d74e14d0f5142ab2d14e5dd2a466b", "size": 67840 }, "python/iqpilot_private/konn3kt/hephaestus/tp20.cpython-312-aarch64-linux-gnu.so": { "mode": 493, - "sha256": "789481c2f41a6a38492f4f6e62d90c18c59276ff7a9b60fd8a5063b1754efe82", + "sha256": "b85ce7db3fbd4879067c068175d0c40f4c1eb9aa54811396519ce2f8fff7ad3c", "size": 135648 }, "python/iqpilot_private/konn3kt/hephaestus/vw_pq_flasher.cpython-312-aarch64-linux-gnu.so": { "mode": 493, - "sha256": "280bee6ecfcde50d1856835a319aaa3354ecfdc4c9d552243493ae7070f9f169", + "sha256": "2e7b7f1b4ed526c924c06aa63ecfaffc1ed726e3a31ba267712a1502e0f30020", "size": 269568 }, "python/iqpilot_private/konn3kt/uploaderd/__init__.py": { @@ -131,7 +131,7 @@ }, "python/iqpilot_private/konn3kt/uploaderd/iquploaderd.cpython-312-aarch64-linux-gnu.so": { "mode": 493, - "sha256": "b1df9786798beaf80ae110b20b79c2e88b7bbdf38c576fa249fc896e360f9ad1", + "sha256": "66cec9701718c377178142f92912d8093dd62a58bb00fa2f865b76457db959db", "size": 204184 }, "runtime": { @@ -171,25 +171,25 @@ } }, "signatures": { - "python/iqpilot_private/konn3kt/backups/archive_codec.cpython-312-aarch64-linux-gnu.so": "/YL6W76Q7ejmJvQraTclgzsGxYGpYbSBF3qql0E8Z40MZBQcm8wy+9mrMm1en5W9xGebuaT5xN2+Rji5MUCrDQ==", - "python/iqpilot_private/konn3kt/backups/backup_keys.cpython-312-aarch64-linux-gnu.so": "0/MsPs9cfZy51uqtyCEK8/U8paw+eQXgqVnglRdUM3Xx6WaPTku3DXfx7MquCTXl+ia9XlM1xdaH/ax6zxhdAw==", - "python/iqpilot_private/konn3kt/backups/backup_orchestrator.cpython-312-aarch64-linux-gnu.so": "1i1wEWih2NTsQ9jgK/cpb8V/B0y5prTk/OHG1wu4qoaMx4Qm1gVm6KAWU7rkvaQHMqC5eZG5lgL7O6tGflgOBA==", - "python/iqpilot_private/konn3kt/backups/cbc_vault.cpython-312-aarch64-linux-gnu.so": "Q0TBxMRrSRv99t5NIpcYgwRcj26IycOSUSX+Yh4f4S82hY3dXwME3XSqWFLaL2wNF+uG7dMZr0pZkZnHBHmaDg==", - "python/iqpilot_private/konn3kt/backups/imahelper.cpython-312-aarch64-linux-gnu.so": "wFMRv5A2/dCAjlBC/x5uN876dq/kMrgMg5F7nmOKjDlrC5lZ014MbZXU0BKzI49CSch9uU3TLbvUR1p9gWPcBA==", - "python/iqpilot_private/konn3kt/flockd/flockd.cpython-312-aarch64-linux-gnu.so": "de2W5b0SVEEAhjf9r1Ht9tg6W4RWldp7Tc/IzUQdphK4XBOzqouXIYCItN5xgEbc1tFTmGgKu2pfMXAoXkgYDw==", - "python/iqpilot_private/konn3kt/flockd/signatures.cpython-312-aarch64-linux-gnu.so": "yEkA8O096lTrh5YTvZgmuGpyuZ0vTnZDvLnLGR7dhBfypiNPsF4dxp2bcgDpz14GRKPHKa3v8O2v/G/06lk4CQ==", - "python/iqpilot_private/konn3kt/hephaestus/ble_auth.cpython-312-aarch64-linux-gnu.so": "fpzjKBczfi9su9/BzPINkMJ8+Qx2KljBmYg/lhpaCRZDQX9kIT2qgcgbu5CqahfEQAjWDH2YKDwRhAGzkx1LBw==", - "python/iqpilot_private/konn3kt/hephaestus/ble_gatt.cpython-312-aarch64-linux-gnu.so": "mUUGiu3/nIkcp9McjoX+qOTzBwuJlFzxxZdbW3uU2C4ppAlQ+bYMz9p33qnmkUiJUGfjcBwFnJebL8AXo/kXBQ==", - "python/iqpilot_private/konn3kt/hephaestus/ble_rpc_dispatch.cpython-312-aarch64-linux-gnu.so": "Nge3RnzUjW2gPUjEyBRxh+D6frwx9dJ/N9W0e0vzFWLLvbo+cT5zjrNAT7buA7XAKGfHSy14ssOxL8Jz+uZBBw==", - "python/iqpilot_private/konn3kt/hephaestus/ble_transportd.cpython-312-aarch64-linux-gnu.so": "KXjqWoG0lsVWqK8uRhOOChREx/ThGtcdKoFxaDD2WpGtEoFBTz2fDsNP8lgyLcrH+9ETT6ggnXqHMf2SnEabBw==", - "python/iqpilot_private/konn3kt/hephaestus/bt_gamepad.cpython-312-aarch64-linux-gnu.so": "+0ZLw/tXrJtcempSJZFTKz9vlzs7q13YrnzSkuSpHzaGUm4Z2l4H3Syy0rPj3iJ8OLbkMAE9LegxScVq9mb1Bg==", - "python/iqpilot_private/konn3kt/hephaestus/cloud_routes.cpython-312-aarch64-linux-gnu.so": "Q44rxaPzw1QoSLoHyoaCMtW9N45YeYUNhtgPETz6Z1zLJ5o9NPec6yP06673sX9xdOnRXr0maeLfI2ji7iFiBg==", - "python/iqpilot_private/konn3kt/hephaestus/hephaestusd.cpython-312-aarch64-linux-gnu.so": "MUg6v8qvb7Rhhk5LPoU7krD0CKb39e8wsoh5ER+VvzWYJZ2fnra7Od14xSGfhFZoHIItPwtOlASUbqlbPKReCg==", - "python/iqpilot_private/konn3kt/hephaestus/kwp2000.cpython-312-aarch64-linux-gnu.so": "ERRMJ3hrOhetsuiNyWrsXH6hwtKzw5CAjg8kB/025JkDyA4OR3yLGMnqXqryygc4r4onUUF0Q55SzTbxxHWzDA==", - "python/iqpilot_private/konn3kt/hephaestus/manage_hephaestusd.cpython-312-aarch64-linux-gnu.so": "+gyiBvn9Uz56mpHovmDtUsNxvlnPYLH8/v5J+f0o8nr/E26f1xvSk2yQbYwYrCeZZ8AOxgY6GmdGRnkly4WCCg==", - "python/iqpilot_private/konn3kt/hephaestus/motd.cpython-312-aarch64-linux-gnu.so": "Y/cinSdkb/kWX44T/w4woXX/Ex9+Q1KPtUC318dtygZeTiHDjCM+0IUjfS/BXwbdKETQPEpyIoKLsJaP2Gk1Cw==", - "python/iqpilot_private/konn3kt/hephaestus/tp20.cpython-312-aarch64-linux-gnu.so": "N+JrGIiTUzESxzH9fdgub5y01psxzlmEw75Y1AuxQqFx7mG4Z+Tx20UeeyQtAw27UuOctiKpGPHq9trtijJwBA==", - "python/iqpilot_private/konn3kt/hephaestus/vw_pq_flasher.cpython-312-aarch64-linux-gnu.so": "1MxRuPAWgjtsNORklvPZBml0OOVqIu8vMluY0QlK/pqE3S8EoC28/Gn39Bhb6hMjk8yTAUaTnTn4HlwkENpvBA==", - "python/iqpilot_private/konn3kt/uploaderd/iquploaderd.cpython-312-aarch64-linux-gnu.so": "Uxa3OMIwXvFEYz1LP+UDWB1sVhi4z8yC9bP+Xypzw26LNRAMkJZta2r+drXGPEe1TFg4zgVWrb9Q1+Y3pA9HAQ==" + "python/iqpilot_private/konn3kt/backups/archive_codec.cpython-312-aarch64-linux-gnu.so": "S6pnKIiETehDkX4x+40+Q+fBQ04N9x1OI7NQS5+j6JVNNIE0PA4MuMB+Vl9rt3o+fJrFoyIL1g+tMWT2gMluDA==", + "python/iqpilot_private/konn3kt/backups/backup_keys.cpython-312-aarch64-linux-gnu.so": "NckVEdkvdS5EmHJKOIe1EakeyNUbYFxDG2tohT0MMzKM3Tk47f0k4cs/QOqFRNsb612WwPH9PaPdhOmNXUKLCQ==", + "python/iqpilot_private/konn3kt/backups/backup_orchestrator.cpython-312-aarch64-linux-gnu.so": "3byD6wJefryFq1S6jV0TT+Ui/Yn+pyj3Iu+RI2vxIuue9qIknyIy0zc156kXSotiYkip0XOuzwB1HNWo4qTPCg==", + "python/iqpilot_private/konn3kt/backups/cbc_vault.cpython-312-aarch64-linux-gnu.so": "0S7cE4HK9ZoATxXvrWcQF9Hf+iy8JKdBd/OAQJt6IHrmgDeExkn5NQD5VjMpOejsGBTCQEgL/xx7JpYxlr5wCQ==", + "python/iqpilot_private/konn3kt/backups/imahelper.cpython-312-aarch64-linux-gnu.so": "3adGsr3oPmcldMROx5pjkCxa2Vrbaj2lRhFjNgz151VHRULAOTuxyX7nAZQmxNuDEumyji5TW5d1zTqAoMFnBg==", + "python/iqpilot_private/konn3kt/flockd/flockd.cpython-312-aarch64-linux-gnu.so": "gIbpR3VZKr2n0sXnV6PJhHiF6spttO6PFPgxxGoo7ml8tmYFx7SoUhUiEEhNQXGpcN27iW/5IT7RGIE4m8kPCg==", + "python/iqpilot_private/konn3kt/flockd/signatures.cpython-312-aarch64-linux-gnu.so": "iLZnAzgx87+P3adKMeOOJswOKmCdt4gqG4EJ/lEHjjWZC+GZlnTr/qQ3fiQ1R1qp6Jvb3MrFI4FyFnsbURfPCQ==", + "python/iqpilot_private/konn3kt/hephaestus/ble_auth.cpython-312-aarch64-linux-gnu.so": "QTiHED4yIIVzPcU6BapHNU/nlX1g6cP8AaxteooJDR/DqCnUbW/3YUsVSmqvwTLNTF6YWNOLa2dgvybyYRBSAQ==", + "python/iqpilot_private/konn3kt/hephaestus/ble_gatt.cpython-312-aarch64-linux-gnu.so": "MPoy7t15DrxRSJBi+ZFRqHU/ahIc03h8XxuOiho/zaU4I6LIy4QwP18duc+YkeJ5R6jkPduoWgLyHKbAXPTYCg==", + "python/iqpilot_private/konn3kt/hephaestus/ble_rpc_dispatch.cpython-312-aarch64-linux-gnu.so": "I9uohiZn6lrerZFWee6cf2yXkeDxZTSBxKVQWSGKq5woFAK5fZ94FMf8qKfow8dOw8etLXC8XOYA2oH5TfK+AQ==", + "python/iqpilot_private/konn3kt/hephaestus/ble_transportd.cpython-312-aarch64-linux-gnu.so": "smEWcWBel8zd78q5YvTnVKlM3EMh15FxywK/Dlx6lCHJ5eLAPsoYo+YimWd9qUFBOF/rmkO5k/9TlqsKE31vBw==", + "python/iqpilot_private/konn3kt/hephaestus/bt_gamepad.cpython-312-aarch64-linux-gnu.so": "wrvw7RvBfiEt0dz2TKMuDb03FIIaPLEclAYYmWObzwF1NAmyiBRqup0F0y3k7vtmwbubjns7+j2ZvA4j+k18AQ==", + "python/iqpilot_private/konn3kt/hephaestus/cloud_routes.cpython-312-aarch64-linux-gnu.so": "/IBbuQ0E7ym7EGLV9EYDqcWLaSgBa6IImVgapC50F7LhNXmuUhOZJc9sVyV50Xa/e+60toBSPsJETud6sSeTCA==", + "python/iqpilot_private/konn3kt/hephaestus/hephaestusd.cpython-312-aarch64-linux-gnu.so": "iqiqKn2zwfgBtsiqKtNTMgfnCfek9F41ZLVi3GTOzJucXsyKMCCLfyn4zImGf1w9zG+XWPWwudDBYEYWKr8AAQ==", + "python/iqpilot_private/konn3kt/hephaestus/kwp2000.cpython-312-aarch64-linux-gnu.so": "uzsNWnyFDbCTEYVdDEF2dS+Hk/WnB/Je4bT5Htge8TX/1O/pEpgCKpLE1j1H5Nt+/M5HDvgSXJScQxmlWJBaDQ==", + "python/iqpilot_private/konn3kt/hephaestus/manage_hephaestusd.cpython-312-aarch64-linux-gnu.so": "TwgqVE8NrG59tVrr9z1Lvo7yh1gVyI8548cP7PT44F4bnDVx9oJvqaZOqF1vEV4xWdoYKSIBPVA3YnINnwEICw==", + "python/iqpilot_private/konn3kt/hephaestus/motd.cpython-312-aarch64-linux-gnu.so": "uBZA3ZSrCNfpy/vWbtYApt9UOStxuBBe0LP6AhEZEHWfebrM0CeCK1qimb+kfGDoQyTXLmie6v8PiKWYeTmODQ==", + "python/iqpilot_private/konn3kt/hephaestus/tp20.cpython-312-aarch64-linux-gnu.so": "OwU3WbDPCT0ggbPBWjuYvTiPuPW+j557MyciaBFq1C5+xElg94DHWH6DpV/WzUFkRwjb4226epgYjSCQHva/Ag==", + "python/iqpilot_private/konn3kt/hephaestus/vw_pq_flasher.cpython-312-aarch64-linux-gnu.so": "j7lzNRlIQ0kr0Pw4C8Oo/fw1i/ihgmNTXP+cro9J3ivRm1xpe5aPPY22I5cw763tGHo2h9YCAO5EBAHz+FSsCg==", + "python/iqpilot_private/konn3kt/uploaderd/iquploaderd.cpython-312-aarch64-linux-gnu.so": "Z3vtFdwz1cLaTACNhZoaO5UXG4HCzEOB/2/RCdii1epsXE9amtWEbwx2miS2qlN03gC28Ieak9SECjxMyGafAg==" } } diff --git a/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/backups/archive_codec.cpython-312-aarch64-linux-gnu.so b/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/backups/archive_codec.cpython-312-aarch64-linux-gnu.so index fd5714b..eed3bd6 100755 Binary files a/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/backups/archive_codec.cpython-312-aarch64-linux-gnu.so and b/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/backups/archive_codec.cpython-312-aarch64-linux-gnu.so differ diff --git a/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/backups/backup_keys.cpython-312-aarch64-linux-gnu.so b/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/backups/backup_keys.cpython-312-aarch64-linux-gnu.so index 6ca54cd..d6b80b4 100755 Binary files a/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/backups/backup_keys.cpython-312-aarch64-linux-gnu.so and b/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/backups/backup_keys.cpython-312-aarch64-linux-gnu.so differ diff --git a/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/backups/backup_orchestrator.cpython-312-aarch64-linux-gnu.so b/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/backups/backup_orchestrator.cpython-312-aarch64-linux-gnu.so index d3289e5..002e5cc 100755 Binary files a/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/backups/backup_orchestrator.cpython-312-aarch64-linux-gnu.so and b/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/backups/backup_orchestrator.cpython-312-aarch64-linux-gnu.so differ diff --git a/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/backups/cbc_vault.cpython-312-aarch64-linux-gnu.so b/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/backups/cbc_vault.cpython-312-aarch64-linux-gnu.so index e5e41c0..1b6da19 100755 Binary files a/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/backups/cbc_vault.cpython-312-aarch64-linux-gnu.so and b/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/backups/cbc_vault.cpython-312-aarch64-linux-gnu.so differ diff --git a/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/backups/imahelper.cpython-312-aarch64-linux-gnu.so b/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/backups/imahelper.cpython-312-aarch64-linux-gnu.so index 9d49c2e..27ea6b8 100755 Binary files a/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/backups/imahelper.cpython-312-aarch64-linux-gnu.so and b/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/backups/imahelper.cpython-312-aarch64-linux-gnu.so differ diff --git a/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/flockd/flockd.cpython-312-aarch64-linux-gnu.so b/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/flockd/flockd.cpython-312-aarch64-linux-gnu.so index fa473b5..aa8334b 100755 Binary files a/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/flockd/flockd.cpython-312-aarch64-linux-gnu.so and b/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/flockd/flockd.cpython-312-aarch64-linux-gnu.so differ diff --git a/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/flockd/signatures.cpython-312-aarch64-linux-gnu.so b/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/flockd/signatures.cpython-312-aarch64-linux-gnu.so index 2c9d5c6..7174afd 100755 Binary files a/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/flockd/signatures.cpython-312-aarch64-linux-gnu.so and b/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/flockd/signatures.cpython-312-aarch64-linux-gnu.so differ diff --git a/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/hephaestus/_vendor/localapi_runtime.zip b/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/hephaestus/_vendor/localapi_runtime.zip index 1ec16bf..8d85a6e 100644 Binary files a/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/hephaestus/_vendor/localapi_runtime.zip and b/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/hephaestus/_vendor/localapi_runtime.zip differ diff --git a/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/hephaestus/ble_auth.cpython-312-aarch64-linux-gnu.so b/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/hephaestus/ble_auth.cpython-312-aarch64-linux-gnu.so index 38cd773..509009c 100755 Binary files a/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/hephaestus/ble_auth.cpython-312-aarch64-linux-gnu.so and b/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/hephaestus/ble_auth.cpython-312-aarch64-linux-gnu.so differ diff --git a/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/hephaestus/ble_gatt.cpython-312-aarch64-linux-gnu.so b/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/hephaestus/ble_gatt.cpython-312-aarch64-linux-gnu.so index 36dad6a..3df4f4a 100755 Binary files a/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/hephaestus/ble_gatt.cpython-312-aarch64-linux-gnu.so and b/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/hephaestus/ble_gatt.cpython-312-aarch64-linux-gnu.so differ diff --git a/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/hephaestus/ble_rpc_dispatch.cpython-312-aarch64-linux-gnu.so b/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/hephaestus/ble_rpc_dispatch.cpython-312-aarch64-linux-gnu.so index 4d3b251..9f0093f 100755 Binary files a/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/hephaestus/ble_rpc_dispatch.cpython-312-aarch64-linux-gnu.so and b/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/hephaestus/ble_rpc_dispatch.cpython-312-aarch64-linux-gnu.so differ diff --git a/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/hephaestus/ble_transportd.cpython-312-aarch64-linux-gnu.so b/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/hephaestus/ble_transportd.cpython-312-aarch64-linux-gnu.so index a835701..8334826 100755 Binary files a/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/hephaestus/ble_transportd.cpython-312-aarch64-linux-gnu.so and b/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/hephaestus/ble_transportd.cpython-312-aarch64-linux-gnu.so differ diff --git a/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/hephaestus/bt_gamepad.cpython-312-aarch64-linux-gnu.so b/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/hephaestus/bt_gamepad.cpython-312-aarch64-linux-gnu.so index 93cc0c0..1d9c92e 100755 Binary files a/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/hephaestus/bt_gamepad.cpython-312-aarch64-linux-gnu.so and b/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/hephaestus/bt_gamepad.cpython-312-aarch64-linux-gnu.so differ diff --git a/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/hephaestus/cloud_routes.cpython-312-aarch64-linux-gnu.so b/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/hephaestus/cloud_routes.cpython-312-aarch64-linux-gnu.so index 1b74130..6fe3cbc 100755 Binary files a/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/hephaestus/cloud_routes.cpython-312-aarch64-linux-gnu.so and b/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/hephaestus/cloud_routes.cpython-312-aarch64-linux-gnu.so differ diff --git a/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/hephaestus/hephaestusd.cpython-312-aarch64-linux-gnu.so b/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/hephaestus/hephaestusd.cpython-312-aarch64-linux-gnu.so index 448a7ba..f0d71b1 100755 Binary files a/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/hephaestus/hephaestusd.cpython-312-aarch64-linux-gnu.so and b/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/hephaestus/hephaestusd.cpython-312-aarch64-linux-gnu.so differ diff --git a/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/hephaestus/kwp2000.cpython-312-aarch64-linux-gnu.so b/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/hephaestus/kwp2000.cpython-312-aarch64-linux-gnu.so index d7538b5..e7cc75d 100755 Binary files a/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/hephaestus/kwp2000.cpython-312-aarch64-linux-gnu.so and b/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/hephaestus/kwp2000.cpython-312-aarch64-linux-gnu.so differ diff --git a/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/hephaestus/manage_hephaestusd.cpython-312-aarch64-linux-gnu.so b/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/hephaestus/manage_hephaestusd.cpython-312-aarch64-linux-gnu.so index 9f88ebd..209a630 100755 Binary files a/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/hephaestus/manage_hephaestusd.cpython-312-aarch64-linux-gnu.so and b/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/hephaestus/manage_hephaestusd.cpython-312-aarch64-linux-gnu.so differ diff --git a/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/hephaestus/motd.cpython-312-aarch64-linux-gnu.so b/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/hephaestus/motd.cpython-312-aarch64-linux-gnu.so index 1abab6f..18a5090 100755 Binary files a/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/hephaestus/motd.cpython-312-aarch64-linux-gnu.so and b/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/hephaestus/motd.cpython-312-aarch64-linux-gnu.so differ diff --git a/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/hephaestus/tp20.cpython-312-aarch64-linux-gnu.so b/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/hephaestus/tp20.cpython-312-aarch64-linux-gnu.so index 70c0105..b88ee48 100755 Binary files a/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/hephaestus/tp20.cpython-312-aarch64-linux-gnu.so and b/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/hephaestus/tp20.cpython-312-aarch64-linux-gnu.so differ diff --git a/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/hephaestus/vw_pq_flasher.cpython-312-aarch64-linux-gnu.so b/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/hephaestus/vw_pq_flasher.cpython-312-aarch64-linux-gnu.so index 8736d63..4733e5a 100755 Binary files a/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/hephaestus/vw_pq_flasher.cpython-312-aarch64-linux-gnu.so and b/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/hephaestus/vw_pq_flasher.cpython-312-aarch64-linux-gnu.so differ diff --git a/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/uploaderd/iquploaderd.cpython-312-aarch64-linux-gnu.so b/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/uploaderd/iquploaderd.cpython-312-aarch64-linux-gnu.so index 06a0c1d..ae42981 100755 Binary files a/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/uploaderd/iquploaderd.cpython-312-aarch64-linux-gnu.so and b/artifacts/iqpilot_hephaestusd_private/python/iqpilot_private/konn3kt/uploaderd/iquploaderd.cpython-312-aarch64-linux-gnu.so differ diff --git a/cereal/custom.capnp b/cereal/custom.capnp index 4b65a94..a63212c 100644 --- a/cereal/custom.capnp +++ b/cereal/custom.capnp @@ -315,6 +315,9 @@ struct IQOnroadEvent @0xf4621d3ee9233bc9 { # construction zone assist constructionZoneDetected @30; + + # model management + modelUpdating @31; } } @@ -329,6 +332,7 @@ struct IQCarParams @0xd4189b5c8aca9f78 { iqLateralNet @4 :LateralNet; longitudinalStoppingSpeedOverride @5 :Float32; # m/s; zero keeps the upstream default + stoppingDecelRateOverride @6 :Float32; # m/s^3; zero keeps the upstream default struct LateralNet { fuzzyFingerprint @0 :Bool; diff --git a/cereal/log.capnp b/cereal/log.capnp index 39e9f50..590c92a 100644 --- a/cereal/log.capnp +++ b/cereal/log.capnp @@ -70,12 +70,12 @@ struct OnroadEvent @0xc4fa6047f024e718 { longitudinalManeuver @30; steerTempUnavailableSilent @31; resumeRequired @32; - driverDistracted1 @33; - driverDistracted2 @34; - driverDistracted3 @35; - driverUnresponsive1 @36; - driverUnresponsive2 @37; - driverUnresponsive3 @38; + preDriverDistracted @33; + promptDriverDistracted @34; + driverDistracted @35; + preDriverUnresponsive @36; + promptDriverUnresponsive @37; + driverUnresponsive @38; belowSteerSpeed @39; lowBattery @40; accFaulted @41; @@ -2183,7 +2183,6 @@ struct DriverStateV2 { rightBlinkProb @8 :Float32; sunglassesProb @9 :Float32; phoneProb @13 :Float32; - sleepProb @14 :Float32; notReadyProbDEPRECATED @12 :List(Float32); occludedProbDEPRECATED @10 :Float32; readyProbDEPRECATED @11 :List(Float32); @@ -2225,7 +2224,7 @@ struct DriverStateDEPRECATED @0xb83c6cc593ed0a00 { stdDEPRECATED @2 :Float32; } -struct DriverMonitoringStateDEPRECATED @0xb83cda094a1da284 { +struct DriverMonitoringState @0xb83cda094a1da284 { events @18 :List(OnroadEvent); faceDetected @1 :Bool; isDistracted @2 :Bool; @@ -2251,81 +2250,6 @@ struct DriverMonitoringStateDEPRECATED @0xb83cda094a1da284 { eventsDEPRECATED @0 :List(Car.OnroadEventDEPRECATED); } -struct DriverMonitoringState { - lockout @0 :Bool; - lockoutCount @15 :Int8; - lockoutMinutesRemaining @11 :Int8; - alert3Count @12 :Int8; - noResponseCount @13 :Int8; - noResponseForceDecel @14 :Bool; - - alwaysOn @3 :Bool; - alwaysOnLockout @4 :Bool; - - alertLevel @5 :AlertLevel; - activePolicy @6 :MonitoringPolicy; - isRHD @7 :Bool; - rhdCalibration @8 :CalibrationState; - - visionPolicyState @9 :VisionPolicyState; - wheeltouchPolicyState @10 :WheeltouchPolicyState; - - enum AlertLevel { - none @0; - one @1; - two @2; - three @3; - } - - enum MonitoringPolicy { - wheeltouch @0; - vision @1; - } - - struct VisionPolicyState { - awarenessPercent @0 :Int8; - awarenessStep @1 :Float32; - isDistracted @2 :Bool; - distractedTypes @3 :DistractedTypes; - - faceDetected @4 :Bool; - pose @5 :Pose; - wheeltouchFallbackPercent @6 :Int8; - uncertainOffroadAlertPercent @7 :Int8; - - struct DistractedTypes { - pose @0: Bool; - eye @1: Bool; - phone @2: Bool; - } - - struct Pose { - pitch @0 :Float32; - yaw @1 :Float32; - pitchCalib @2 :CalibrationState; - yawCalib @3 :CalibrationState; - calibrated @4 :Bool; - uncertainty @5 :Float32; - } - } - - struct WheeltouchPolicyState { - awarenessPercent @0 :Int8; - awarenessStep @1 :Float32; - driverInteracting @2 :Bool; - } - - struct CalibrationState { - calibratedPercent @0 :Int8; - offset @1 :Float32; - } - - deprecated :group { - alertCountLockoutPercent @1 :Int8; - alertTimeLockoutPercent @2 :Int8; - } -} - struct Boot { wallTimeNanos @0 :UInt64; pstore @4 :Map(Text, Data); @@ -2646,7 +2570,7 @@ struct Event { thumbnail @66: Thumbnail; onroadEvents @134: List(OnroadEvent); carParams @69: Car.CarParams; - driverMonitoringState @165: DriverMonitoringState; + driverMonitoringState @71: DriverMonitoringState; livePose @129 :LivePose; modelV2 @75 :ModelDataV2; drivingModelData @128 :DrivingModelData; @@ -2769,7 +2693,6 @@ struct Event { wifiScanDEPRECATED @29 :List(Legacy.WifiScan); uiNavigationEventDEPRECATED @50 :Legacy.UiNavigationEvent; liveMapDataDEPRECATED @62 :LiveMapDataDEPRECATED; - driverMonitoringStateDEPRECATED @71 :DriverMonitoringStateDEPRECATED; gpsPlannerPointsDEPRECATED @40 :Legacy.GPSPlannerPoints; gpsPlannerPlanDEPRECATED @41 :Legacy.GPSPlannerPlan; applanixRawDEPRECATED @42 :Data; diff --git a/common/api/base.py b/common/api/base.py index 5cb5934..7e53329 100644 --- a/common/api/base.py +++ b/common/api/base.py @@ -3,6 +3,7 @@ import os import requests import unicodedata from datetime import datetime, timedelta, UTC +from functools import lru_cache from openpilot.system.hardware.hw import Paths from openpilot.system.version import get_version @@ -11,6 +12,16 @@ 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 @@ -38,7 +49,8 @@ class BaseApi: } if payload_extra is not None: payload.update(payload_extra) - token = jwt.encode(payload, self.private_key, algorithm=self.jwt_algorithm) + 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 diff --git a/common/params_keys.h b/common/params_keys.h index de2194c..1a6bfc9 100644 --- a/common/params_keys.h +++ b/common/params_keys.h @@ -41,7 +41,6 @@ inline static std::unordered_map keys = { {"DoShutdown", {CLEAR_ON_MANAGER_START, BOOL}}, {"DoUninstall", {CLEAR_ON_MANAGER_START, BOOL}}, {"DriverTooDistracted", {CLEAR_ON_MANAGER_START | CLEAR_ON_IGNITION_ON, BOOL}}, - {"DriverLockoutCount", {CLEAR_ON_MANAGER_START | CLEAR_ON_IGNITION_ON, INT, "0"}}, {"AlphaLongitudinalEnabled", {PERSISTENT, BOOL}}, {"ExperimentalMode", {PERSISTENT, BOOL}}, {"ExperimentalModeConfirmed", {PERSISTENT, BOOL}}, @@ -132,7 +131,7 @@ inline static std::unordered_map keys = { {"SnoozeUpdate", {CLEAR_ON_MANAGER_START | CLEAR_ON_OFFROAD_TRANSITION, BOOL}}, {"SshEnabled", {PERSISTENT, BOOL}}, {"TermsVersion", {PERSISTENT, STRING}}, - {"TorqueBar", {PERSISTENT, BOOL, "0"}}, + {"IQSteerEffortArc", {PERSISTENT, BOOL, "0"}}, {"TrainingVersion", {PERSISTENT, STRING}}, {"UbloxAvailable", {PERSISTENT, BOOL}}, {"UsbStorageEnabled", {PERSISTENT, BOOL}}, @@ -167,7 +166,7 @@ inline static std::unordered_map keys = { {"CarPlatformBundle", {PERSISTENT, JSON}}, {"Konn3ktVwOdometers", {PERSISTENT, JSON}}, {"Konn3ktVehicleOdometers", {PERSISTENT, JSON}}, - {"ChevronInfo", {PERSISTENT, INT, "4"}}, + {"IQLeadReadouts", {PERSISTENT, INT, "4"}}, {"DeviceBootMode", {PERSISTENT, INT, "0"}}, {"IQDevUIInfo", {PERSISTENT, INT, "0"}}, {"EnableEsimProvisioning", {PERSISTENT, BOOL, "1"}}, @@ -201,9 +200,8 @@ inline static std::unordered_map keys = { {"OfflineTilesBaseUrl", {PERSISTENT, STRING}}, {"OnroadUploads", {PERSISTENT, BOOL, "1"}}, {"IQAlertSilence", {PERSISTENT, BOOL, "0"}}, - {"RainbowMode", {PERSISTENT, BOOL, "0"}}, - {"RocketFuel", {PERSISTENT, BOOL, "0"}}, - {"ShowTurnSignals", {PERSISTENT, BOOL, "0"}}, + {"IQAccelMeter", {PERSISTENT, BOOL, "0"}}, + {"IQBlinkerIndicators", {PERSISTENT, BOOL, "0"}}, {"StandstillTimer", {PERSISTENT, BOOL, "0"}}, // AOL (Always On Lateral) params {"AolEnabled", {PERSISTENT, BOOL, "1"}}, @@ -215,7 +213,7 @@ inline static std::unordered_map keys = { {"ModelManager_ActiveBundle", {PERSISTENT, JSON}}, {"ModelManager_ClearCache", {CLEAR_ON_MANAGER_START, BOOL}}, {"ModelManager_DownloadIndex", {CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION, INT, "-1"}}, - {"ModelManager_Favs", {PERSISTENT, STRING}}, + {"IQModelFavorites", {PERSISTENT, STRING}}, {"ModelManager_LastSyncTime", {CLEAR_ON_MANAGER_START | CLEAR_ON_OFFROAD_TRANSITION, INT, "0"}}, {"ModelManager_ModelsCache", {PERSISTENT, JSON}}, @@ -227,7 +225,7 @@ inline static std::unordered_map keys = { {"BackupManager_RestoreVersion", {PERSISTENT, STRING}}, // iqpilot car specific params - {"HyundaiLongitudinalTuning", {PERSISTENT, INT, "0"}}, + {"IQHyundaiLongTune", {PERSISTENT, INT, "0"}}, {"AutoCruiseControl", {PERSISTENT, INT, "0"}}, {"AutoEngage", {PERSISTENT, INT, "0"}}, {"CanfdDebug", {PERSISTENT, INT, "0"}}, @@ -254,10 +252,10 @@ inline static std::unordered_map keys = { {"LongitudinalPersonalityMax", {PERSISTENT, INT, "3"}}, {"MaxAngleFrames", {PERSISTENT, INT, "89"}}, {"SpeedFromPCM", {PERSISTENT, INT, "2"}}, - {"SubaruStopAndGo", {PERSISTENT, BOOL, "0"}}, - {"SubaruStopAndGoManualParkingBrake", {PERSISTENT, BOOL, "0"}}, - {"TeslaCoopSteering", {PERSISTENT, BOOL, "0"}}, - {"ToyotaEnforceStockLongitudinal", {PERSISTENT, BOOL, "0"}}, + {"IQSubaruCreepAssist", {PERSISTENT, BOOL, "0"}}, + {"IQSubaruCreepAssistManualBrake", {PERSISTENT, BOOL, "0"}}, + {"IQTeslaTorqueBlend", {PERSISTENT, BOOL, "0"}}, + {"IQToyotaFactoryLong", {PERSISTENT, BOOL, "0"}}, {"VwPqEpsPatched", {PERSISTENT, BOOL}}, {"ToyotaSnGHack", {PERSISTENT, BOOL, "0"}}, {"pqhca5or7Toggle", {PERSISTENT, BOOL}}, @@ -278,7 +276,7 @@ inline static std::unordered_map keys = { {"IQDynamicMinimumForceStopLength", {PERSISTENT, FLOAT, "0.0"}}, {"IQForceStops", {PERSISTENT, BOOL, "1"}}, {"IQCustomStopDistance", {PERSISTENT, INT, "0"}}, // meters, -2..2; negative = stop closer, positive = stop further back; independent of IQForceStops - {"BlindSpot", {PERSISTENT, BOOL, "0"}}, + {"IQBlindSpotAlerts", {PERSISTENT, BOOL, "0"}}, {"IQExpandedStatus", {PERSISTENT, BOOL, "0"}}, {"HomePanelWidget", {PERSISTENT, STRING, "changelog"}}, @@ -342,7 +340,7 @@ inline static std::unordered_map keys = { {"OsmStateNames", {PERSISTENT, JSON}}, {"OsmWayTest", {PERSISTENT, STRING}}, {"RoadName", {CLEAR_ON_ONROAD_TRANSITION, STRING}}, - {"RoadNameToggle", {PERSISTENT, BOOL, "0"}}, + {"IQRoadNameOverlay", {PERSISTENT, BOOL, "0"}}, {"IQSpeedAssistMode", {PERSISTENT, INT, "1"}}, {"IQSpeedAssistOffsetType", {PERSISTENT, INT, "0"}}, {"IQSpeedAssistPolicy", {PERSISTENT, INT, "3"}}, diff --git a/common/utils.py b/common/utils.py index ccc6719..e0aa160 100644 --- a/common/utils.py +++ b/common/utils.py @@ -167,6 +167,91 @@ def managed_proc(cmd: list[str], env: dict[str, str]): 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) diff --git a/iqdbc_repo/iqdbc/car/honda/fingerprints.py b/iqdbc_repo/iqdbc/car/honda/fingerprints.py index ddf4331..89b1da0 100644 --- a/iqdbc_repo/iqdbc/car/honda/fingerprints.py +++ b/iqdbc_repo/iqdbc/car/honda/fingerprints.py @@ -994,6 +994,7 @@ FW_VERSIONS = { b'8S102-T56-A070\x00\x00', b'8S102-T60-AA10\x00\x00', b'8S102-T64-A040\x00\x00', + b'8S102-T64-A050\x00\x00', ], (Ecu.vsa, 0x18da28f1, None): [ b'57114-T20-AB40\x00\x00', diff --git a/iqdbc_repo/iqdbc/car/honda/interface.py b/iqdbc_repo/iqdbc/car/honda/interface.py index 133741f..cf24f09 100755 --- a/iqdbc_repo/iqdbc/car/honda/interface.py +++ b/iqdbc_repo/iqdbc/car/honda/interface.py @@ -88,9 +88,11 @@ class CarInterface(CarInterfaceBase): ret.steerActuatorDelay = 0.1 if candidate in HONDA_BOSCH: - ret.longitudinalActuatorDelay = 0.5 # s if candidate in HONDA_BOSCH_RADARLESS: ret.stopAccel = CarControllerParams.BOSCH_ACCEL_MIN # stock uses -4.0 m/s^2 once stopped but limited by safety model + ret.longitudinalActuatorDelay = 0.25 # s + else: + ret.longitudinalActuatorDelay = 0.5 # s else: # default longitudinal tuning for all hondas ret.longitudinalTuning.kiBP = [0., 5., 35.] @@ -335,6 +337,12 @@ class CarInterface(CarInterfaceBase): stock_cp.autoResumeSng = stock_cp.autoResumeSng or ret.enableGasInterceptor + if candidate == CAR.HONDA_CITY_7G: + ret.longitudinalStoppingSpeedOverride = 2.0 + ret.stoppingDecelRateOverride = 0.3 + else: + ret.longitudinalStoppingSpeedOverride = 0.5 + ret.stoppingDecelRateOverride = 0.1 return ret diff --git a/iqdbc_repo/iqdbc/car/structs.py b/iqdbc_repo/iqdbc/car/structs.py index 583dc22..f3dabdf 100644 --- a/iqdbc_repo/iqdbc/car/structs.py +++ b/iqdbc_repo/iqdbc/car/structs.py @@ -52,6 +52,7 @@ class IQCarParams: pcmCruiseSpeed: bool = auto_field() enableGasInterceptor: bool = auto_field() longitudinalStoppingSpeedOverride: float = auto_field() + stoppingDecelRateOverride: float = auto_field() iqLateralNet: 'IQCarParams.LateralNet' = field(default_factory=lambda: IQCarParams.LateralNet()) diff --git a/iqdbc_repo/iqdbc/car/subaru/carcontroller.py b/iqdbc_repo/iqdbc/car/subaru/carcontroller.py index 18ba9e5..3a1cab6 100644 --- a/iqdbc_repo/iqdbc/car/subaru/carcontroller.py +++ b/iqdbc_repo/iqdbc/car/subaru/carcontroller.py @@ -6,7 +6,7 @@ from iqdbc.car.interfaces import CarControllerBase from iqdbc.car.subaru import subarucan from iqdbc.car.subaru.values import DBC, GLOBAL_ES_ADDR, CanBus, CarControllerParams, SubaruFlags -from iqdbc.lvbs.car.subaru.stop_and_go import IQStopAndGoController +from iqdbc.lvbs.car.subaru.creep_assist import IQStopAndGoController # FIXME: These limits aren't exact. The real limit is more than likely over a larger time period and # involves the total steering angle change rather than rate, but these limits work well for now @@ -139,7 +139,7 @@ class CarController(CarControllerBase, IQStopAndGoController): if self.frame % 2 == 0: can_sends.append(subarucan.create_es_static_2(self.packer)) - can_sends.extend(IQStopAndGoController.create_stop_and_go(self, self.packer, CC, CS, self.frame)) + can_sends.extend(IQStopAndGoController.create_creep_assist(self, self.packer, CC, CS, self.frame)) new_actuators = actuators.as_builder() new_actuators.torque = self.apply_torque_last / self.p.STEER_MAX diff --git a/iqdbc_repo/iqdbc/car/subaru/carstate.py b/iqdbc_repo/iqdbc/car/subaru/carstate.py index 5fbfc87..5276c4d 100644 --- a/iqdbc_repo/iqdbc/car/subaru/carstate.py +++ b/iqdbc_repo/iqdbc/car/subaru/carstate.py @@ -7,7 +7,7 @@ from iqdbc.car.subaru.values import DBC, CanBus, SubaruFlags from iqdbc.car import CanSignalRateCalculator from iqdbc.lvbs.car.subaru.aol import AolCarState -from iqdbc.lvbs.car.subaru.stop_and_go import IQStopAndGoState +from iqdbc.lvbs.car.subaru.creep_assist import IQStopAndGoState class CarState(CarStateBase, AolCarState, IQStopAndGoState): diff --git a/iqdbc_repo/iqdbc/car/tesla/carcontroller.py b/iqdbc_repo/iqdbc/car/tesla/carcontroller.py index 216a181..59ca117 100644 --- a/iqdbc_repo/iqdbc/car/tesla/carcontroller.py +++ b/iqdbc_repo/iqdbc/car/tesla/carcontroller.py @@ -8,7 +8,7 @@ from iqdbc.car.tesla.teslacan import TeslaCAN from iqdbc.car.tesla.values import CarControllerParams from iqdbc.car.vehicle_model import VehicleModel from openpilot.iqpilot.selfdrive.car.enhanced_stock_longitudinal_control import get_set_speed_kph_from_params -from iqdbc.lvbs.car.tesla.coop_steering import CoopSteeringCarController +from iqdbc.lvbs.car.tesla.torque_blend import TorqueBlendController from iqdbc.lvbs.car.tesla.values import TeslaFlagsIQ @@ -22,7 +22,7 @@ def get_safety_CP(): class CarController(CarControllerBase): def __init__(self, dbc_names, CP, CP_IQ): CarControllerBase.__init__(self, dbc_names, CP, CP_IQ) - self.coop_steer = CoopSteeringCarController() + self.coop_steer = TorqueBlendController() self.apply_angle_last = 0 self.packer = CANPacker(dbc_names[Bus.party]) self.tesla_can = TeslaCAN(CP, self.packer) @@ -99,7 +99,7 @@ class CarController(CarControllerBase): # TODO: HUD control new_actuators = actuators.as_builder() new_actuators.steeringAngleDeg = self.apply_angle_last - new_actuators.accel = self.coop_steer.coop_apply_angle_last_sat # debug + new_actuators.accel = self.coop_steer.blend_apply_angle_last_sat # debug new_actuators.curvature = float(self.coop_steer.debug_angle_desired_limited) # debug new_actuators.torque = float(self.coop_steer.override_angle_accu) # debug diff --git a/iqdbc_repo/iqdbc/car/tesla/teslacan.py b/iqdbc_repo/iqdbc/car/tesla/teslacan.py index b50544f..35e5f66 100644 --- a/iqdbc_repo/iqdbc/car/tesla/teslacan.py +++ b/iqdbc_repo/iqdbc/car/tesla/teslacan.py @@ -10,7 +10,7 @@ class TeslaCAN: self.l_jerk = 0.0 def create_steering_control(self, angle, enabled, control_type): - # control_type comes from coop_steering: ANGLE_CONTROL (1) normally, LANE_KEEP_ASSIST (2) when cooperative steering is enabled + # control_type comes from torque_blend: ANGLE_CONTROL (1) normally, LANE_KEEP_ASSIST (2) when cooperative steering is enabled control_type = control_type if enabled else 0 if self.CP.flags & TeslaFlags.LEGACY_DAS_STEERING: control_type <<= 1 # legacy firmware uses a 2-bit field, one bit up from the 3-bit signal diff --git a/iqdbc_repo/iqdbc/dbc/generator/honda/honda_bosch_radarless.dbc b/iqdbc_repo/iqdbc/dbc/generator/honda/honda_bosch_radarless.dbc index 2b86a33..f57265f 100644 --- a/iqdbc_repo/iqdbc/dbc/generator/honda/honda_bosch_radarless.dbc +++ b/iqdbc_repo/iqdbc/dbc/generator/honda/honda_bosch_radarless.dbc @@ -7,7 +7,7 @@ CM_ "IMPORT _gearbox_common.dbc"; BO_ 456 ACC_CONTROL: 8 XXX SG_ ACCEL_COMMAND : 7|12@0- (0.01,0) [0|0] "m/s^2" XXX - SG_ IDLESTOP_ALLOW : 8|1@0+ (1,0) [0|1] "" XXX + SG_ BRAKE_REQUEST : 8|1@0+ (1,0) [0|1] "" XXX SG_ STANDSTILL : 9|1@0+ (1,0) [0|1] "" XXX SG_ CONTROL_ON : 10|1@0+ (1,0) [0|1] "" XXX SG_ BOH : 23|1@0+ (1,0) [0|1] "" XXX diff --git a/iqdbc_repo/iqdbc/lvbs/car/platform_list.py b/iqdbc_repo/iqdbc/lvbs/car/car_catalog.py similarity index 92% rename from iqdbc_repo/iqdbc/lvbs/car/platform_list.py rename to iqdbc_repo/iqdbc/lvbs/car/car_catalog.py index 002bd22..3da55cb 100644 --- a/iqdbc_repo/iqdbc/lvbs/car/platform_list.py +++ b/iqdbc_repo/iqdbc/lvbs/car/car_catalog.py @@ -5,7 +5,7 @@ from iqdbc.car.docs import get_all_footnotes, get_params_for_docs from iqdbc.car.values import PLATFORMS -def get_car_list() -> dict[str, dict[str, list[str] | str]]: +def build_car_catalog() -> dict[str, dict[str, list[str] | str]]: collected_footnote = get_all_footnotes() sorted_list: dict[str, dict[str, list[str] | str]] = collect_car_docs(PLATFORMS, collected_footnote) return sorted_list @@ -57,6 +57,6 @@ def collect_car_docs(platforms, footnotes) -> dict[str, dict[str, list[str] | st if __name__ == "__main__": - # get_car_list() is the raw platform source; the shipped catalog is generated + # build_car_catalog() is the raw platform source; the shipped catalog is generated # (and encoded to its on-disk envelope) by the main-repo entry point: print("run: python -m openpilot.iqpilot.selfdrive.car.vehicle_catalog") diff --git a/iqdbc_repo/iqdbc/lvbs/car/interfaces.py b/iqdbc_repo/iqdbc/lvbs/car/interfaces.py index f000d63..59905ed 100644 --- a/iqdbc_repo/iqdbc/lvbs/car/interfaces.py +++ b/iqdbc_repo/iqdbc/lvbs/car/interfaces.py @@ -75,43 +75,43 @@ def apply_iq_car_config(CI, CP: structs.CarParams, CP_IQ: structs.IQCarParams, params_dict = {k: v for param in params_list for k, v in param.items()} - _initialize_custom_longitudinal_tuning(CI, CP, CP_IQ, params_dict) - _initialize_coop_steering(CP, CP_IQ, params_dict) - _initialize_stop_and_go(CP, CP_IQ, params_dict) - _initialize_toyota(CP, CP_IQ, params_dict) + _apply_long_tuning(CI, CP, CP_IQ, params_dict) + _apply_torque_blend(CP, CP_IQ, params_dict) + _apply_creep_assist(CP, CP_IQ, params_dict) + _apply_toyota_options(CP, CP_IQ, params_dict) -def _initialize_custom_longitudinal_tuning(CI, CP: structs.CarParams, CP_IQ: structs.IQCarParams, +def _apply_long_tuning(CI, CP: structs.CarParams, CP_IQ: structs.IQCarParams, params_dict: dict[str, str]) -> None: _ = CI.get_longitudinal_tuning_iq(CP, CP_IQ) -def _initialize_coop_steering(CP: structs.CarParams, CP_IQ: structs.IQCarParams, +def _apply_torque_blend(CP: structs.CarParams, CP_IQ: structs.IQCarParams, params_dict: dict[str, str]) -> None: if CP.brand == 'tesla': - coop_steering = int(params_dict.get("TeslaCoopSteering", 0)) == 1 - if coop_steering: + torque_blend = int(params_dict.get("IQTeslaTorqueBlend", 0)) == 1 + if torque_blend: CP_IQ.flags |= TeslaFlagsIQ.COOP_STEERING.value -def _initialize_stop_and_go(CP: structs.CarParams, CP_IQ: structs.IQCarParams, params_dict: dict[str, str]) -> None: +def _apply_creep_assist(CP: structs.CarParams, CP_IQ: structs.IQCarParams, params_dict: dict[str, str]) -> None: # Subaru stop-and-go; unsupported on gen2-global and hybrid platforms. if CP.brand != 'subaru' or CP.flags & (SubaruFlags.GLOBAL_GEN2 | SubaruFlags.HYBRID): return - if int(params_dict.get("SubaruStopAndGo", 0)) == 1: + if int(params_dict.get("IQSubaruCreepAssist", 0)) == 1: CP_IQ.flags |= SubaruFlagsIQ.STOP_AND_GO.value - if int(params_dict.get("SubaruStopAndGoManualParkingBrake", 0)) == 1: + if int(params_dict.get("IQSubaruCreepAssistManualBrake", 0)) == 1: CP_IQ.flags |= SubaruFlagsIQ.STOP_AND_GO_MANUAL_PARKING_BRAKE.value if CP_IQ.flags & (SubaruFlagsIQ.STOP_AND_GO | SubaruFlagsIQ.STOP_AND_GO_MANUAL_PARKING_BRAKE): CP_IQ.iqSafetyFlags |= SubaruSafetyFlagsIQ.STOP_AND_GO -def _initialize_toyota(CP: structs.CarParams, CP_IQ: structs.IQCarParams, params_dict: dict[str, str]) -> None: +def _apply_toyota_options(CP: structs.CarParams, CP_IQ: structs.IQCarParams, params_dict: dict[str, str]) -> None: if CP.brand == 'toyota': - toyota_stock_long = int(params_dict.get("ToyotaEnforceStockLongitudinal", 0)) == 1 + toyota_stock_long = int(params_dict.get("IQToyotaFactoryLong", 0)) == 1 toyota_sng_hack = int(params_dict.get("ToyotaSnGHack", 0)) == 1 if toyota_stock_long: diff --git a/iqdbc_repo/iqdbc/lvbs/car/subaru/stop_and_go.py b/iqdbc_repo/iqdbc/lvbs/car/subaru/creep_assist.py similarity index 96% rename from iqdbc_repo/iqdbc/lvbs/car/subaru/stop_and_go.py rename to iqdbc_repo/iqdbc/lvbs/car/subaru/creep_assist.py index 5256a85..29417e1 100644 --- a/iqdbc_repo/iqdbc/lvbs/car/subaru/stop_and_go.py +++ b/iqdbc_repo/iqdbc/lvbs/car/subaru/creep_assist.py @@ -68,7 +68,7 @@ class IQStopAndGoController: return held_long_enough return self._epb_pulse(standing and lead_pulling_away) - def create_stop_and_go(self, packer, CC: structs.CarControl, CS: CarStateBase, frame: int) -> list[CanData]: + def create_creep_assist(self, packer, CC: structs.CarControl, CS: CarStateBase, frame: int) -> list[CanData]: if not self.enabled: return [] diff --git a/iqdbc_repo/iqdbc/lvbs/car/tesla/coop_steering.py b/iqdbc_repo/iqdbc/lvbs/car/tesla/torque_blend.py similarity index 93% rename from iqdbc_repo/iqdbc/lvbs/car/tesla/coop_steering.py rename to iqdbc_repo/iqdbc/lvbs/car/tesla/torque_blend.py index e67d443..c8c4092 100644 --- a/iqdbc_repo/iqdbc/lvbs/car/tesla/coop_steering.py +++ b/iqdbc_repo/iqdbc/lvbs/car/tesla/torque_blend.py @@ -15,7 +15,7 @@ from iqdbc.lvbs.car.tesla.values import TeslaFlagsIQ DT_LAT_CTRL = DT_CTRL * CarControllerParams.STEER_STEP -class CoopSteeringCarControllerParams(CarControllerParams): +class TorqueBlendParams(CarControllerParams): ANGLE_LIMITS = replace(CarControllerParams.ANGLE_LIMITS, MAX_ANGLE_RATE=5) STEERING_DEG_PHASE_LEAD_COEFF = 8.0 @@ -28,7 +28,7 @@ STEER_OVERRIDE_LAT_ACCEL_GAIN_LIMIT = 10 # deg/Nm stability and smoothness for a # angle ramping STEER_OVERRIDE_MAX_LAT_JERK = 2.0 # m/s^3 - determines angle ramping rate - speed dependent -STEER_OVERRIDE_MAX_LAT_JERK_CENTERING = CoopSteeringCarControllerParams.ANGLE_LIMITS.MAX_LATERAL_JERK # m/s^3 - for low speed angle ramp down +STEER_OVERRIDE_MAX_LAT_JERK_CENTERING = TorqueBlendParams.ANGLE_LIMITS.MAX_LATERAL_JERK # m/s^3 - for low speed angle ramp down # stability and smoothness for angle ramp control - at very low speeds this takes precedence over jerk settings STEER_OVERRIDE_LAT_JERK_GAIN_LIMIT = 100 # deg/s/Nm - should be less than CarControllerParams.ANGLE_LIMITS.MAX_ANGLE_RATE / DT_CTRL / STEER_OVERRIDE_TORQUE_RANGE STEER_OVERRIDE_TORQUE_RANGE = STEER_OVERRIDE_MAX_TORQUE - STEER_OVERRIDE_MIN_TORQUE @@ -42,7 +42,7 @@ STEER_DESIRED_LIMITER_OVERRIDE_ACTIVE_COUNTER = 0.7 # second STEER_RESUME_RATE_LIMIT_RAMP_RATE = 500 # deg/s^2 - controls rate of rise of angle rate limit, not angle directly -CoopSteeringDataIQ = namedtuple("CoopSteeringDataIQ", +TorqueBlendDataIQ = namedtuple("TorqueBlendDataIQ", ["steeringAngleDeg", "lat_active", "control_type"]) def get_steer_from_lat_accel(lat_accel, v_ego: float, VM: VehicleModel): @@ -84,7 +84,7 @@ def calc_override_angle_delta_limited(torque: float, vEgo: float, VM: VehicleMod """ # prevents windup in carcontroller rate limiter - lat_jerk = min(lat_jerk, CoopSteeringCarControllerParams.ANGLE_LIMITS.MAX_LATERAL_JERK) + lat_jerk = min(lat_jerk, TorqueBlendParams.ANGLE_LIMITS.MAX_LATERAL_JERK) # lateral accel is linear in respect to angle so it's fine to interpolate it with torque torque_to_angle = get_steer_from_lat_accel(lat_jerk, vEgo, VM) / STEER_OVERRIDE_TORQUE_RANGE @@ -93,7 +93,7 @@ def calc_override_angle_delta_limited(torque: float, vEgo: float, VM: VehicleMod override_angle_rate = torque * min(torque_to_angle, gain_limit) # prevent windup in angle rate limiter - return apply_bounds(override_angle_rate * DT_LAT_CTRL, CoopSteeringCarControllerParams.ANGLE_LIMITS.MAX_ANGLE_RATE) + return apply_bounds(override_angle_rate * DT_LAT_CTRL, TorqueBlendParams.ANGLE_LIMITS.MAX_ANGLE_RATE) class SteerRateLimiter: @@ -160,10 +160,10 @@ class SteerAccelLimiter: return angle_out -class CoopSteeringCarController: +class TorqueBlendController: def __init__(self): self.coop_apply_angle_last = 0 - self.coop_apply_angle_last_sat = 0 + self.blend_apply_angle_last_sat = 0 self.override_angle_accu = 0 self.override_active_counter = 0 # Counter for how many cycles torque is below threshold self.resume_rate_limiter_delta = SteerRateLimiter() @@ -201,7 +201,7 @@ class CoopSteeringCarController: return 0 # unwind accumulator toward zero if the previous loop saturated (apply_steer_angle_limits_vm) - unwind = (self.coop_apply_angle_last - self.coop_apply_angle_last_sat) * unwind_weight + unwind = (self.coop_apply_angle_last - self.blend_apply_angle_last_sat) * unwind_weight if self.override_angle_accu * unwind > 0: unwind = apply_bounds(unwind, abs(self.override_angle_accu)) self.override_angle_accu -= unwind @@ -289,7 +289,7 @@ class CoopSteeringCarController: apply_angle_lim = self.resume_rate_limiter.update(apply_angle, angle_rate_delta_lim) return apply_angle_lim - def update(self, apply_angle, lat_active, CP_IQ: structs.IQCarParams, CS: structs.CarState, VM: VehicleModel) -> CoopSteeringDataIQ: + def update(self, apply_angle, lat_active, CP_IQ: structs.IQCarParams, CS: structs.CarState, VM: VehicleModel) -> TorqueBlendDataIQ: # estimate real steering angle by adding rate to the tesla filtered angle steeringAngleDegPhaseLead = CS.out.steeringAngleDeg + CS.out.steeringRateDeg / STEERING_DEG_PHASE_LEAD_COEFF @@ -306,7 +306,7 @@ class CoopSteeringCarController: # final rate limit - matching panda safety self.coop_apply_angle_last = apply_angle - self.coop_apply_angle_last_sat = apply_steer_angle_limits_vm(apply_angle, self.coop_apply_angle_last_sat, CS.out.vEgoRaw, - CS.out.steeringAngleDeg, lat_active, CoopSteeringCarControllerParams, VM) + self.blend_apply_angle_last_sat = apply_steer_angle_limits_vm(apply_angle, self.blend_apply_angle_last_sat, CS.out.vEgoRaw, + CS.out.steeringAngleDeg, lat_active, TorqueBlendParams, VM) - return CoopSteeringDataIQ(self.coop_apply_angle_last_sat, lat_active, 1) # 1 = angle control + return TorqueBlendDataIQ(self.blend_apply_angle_last_sat, lat_active, 1) # 1 = angle control diff --git a/iqdbc_repo/iqdbc/lvbs/car/tests/test_car_list.py b/iqdbc_repo/iqdbc/lvbs/car/tests/test_car_catalog.py similarity index 88% rename from iqdbc_repo/iqdbc/lvbs/car/tests/test_car_list.py rename to iqdbc_repo/iqdbc/lvbs/car/tests/test_car_catalog.py index d016349..5ce5da3 100644 --- a/iqdbc_repo/iqdbc/lvbs/car/tests/test_car_list.py +++ b/iqdbc_repo/iqdbc/lvbs/car/tests/test_car_catalog.py @@ -2,7 +2,7 @@ import json import os from iqdbc.car.common.basedir import BASEDIR -from iqdbc.lvbs.car.platform_list import get_car_list +from iqdbc.lvbs.car.car_catalog import build_car_catalog CATALOG_JSON = os.path.join(BASEDIR, "..", "..", "iqpilot", "selfdrive", "car", "vehicle_catalog.json") @@ -18,7 +18,7 @@ def _decode(envelope) -> dict: class TestCarList: def test_generator(self): - generated = get_car_list() + generated = build_car_catalog() with open(CATALOG_JSON) as f: shipped = _decode(json.load(f)) diff --git a/iqpilot/selfdrive/car/interfaces.py b/iqpilot/selfdrive/car/interfaces.py index bff71f9..7243529 100644 --- a/iqpilot/selfdrive/car/interfaces.py +++ b/iqpilot/selfdrive/car/interfaces.py @@ -12,11 +12,11 @@ _ANGLE = _dbc.CarParams.SteerControlType.angle # Port tunables surfaced to the fingerprint step, flat so the read is one pass. _TUNABLES = ( - "HyundaiLongitudinalTuning", - "SubaruStopAndGo", - "SubaruStopAndGoManualParkingBrake", - "TeslaCoopSteering", - "ToyotaEnforceStockLongitudinal", + "IQHyundaiLongTune", + "IQSubaruCreepAssist", + "IQSubaruCreepAssistManualBrake", + "IQTeslaTorqueBlend", + "IQToyotaFactoryLong", "ToyotaSnGHack", ) diff --git a/iqpilot/selfdrive/car/vehicle_catalog.py b/iqpilot/selfdrive/car/vehicle_catalog.py index 42d6e5d..dedd794 100644 --- a/iqpilot/selfdrive/car/vehicle_catalog.py +++ b/iqpilot/selfdrive/car/vehicle_catalog.py @@ -81,5 +81,5 @@ def _write(vehicles: dict[str, dict], basedir: str = BASEDIR) -> str: if __name__ == "__main__": - from iqdbc.lvbs.car.platform_list import get_car_list - print("wrote", _write(get_car_list())) + from iqdbc.lvbs.car.car_catalog import build_car_catalog + print("wrote", _write(build_car_catalog())) diff --git a/iqpilot/selfdrive/iqmodeld/models/manager.py b/iqpilot/selfdrive/iqmodeld/models/manager.py index 623eed1..1596d2e 100644 --- a/iqpilot/selfdrive/iqmodeld/models/manager.py +++ b/iqpilot/selfdrive/iqmodeld/models/manager.py @@ -43,6 +43,7 @@ from openpilot.iqpilot.selfdrive.iqmodeld.models.helpers import ( bundle_files_ready, get_active_bundle, get_runtime_bundle_upgrade, + is_default_bundle, persist_active_bundle, ) @@ -56,6 +57,7 @@ class IQModelManager(_BaseIQModelManager): def __init__(self): super().__init__() self._validated_active_key: tuple[tuple[str, str], ...] | None = None + self._manifest_refresh_key: tuple[tuple[str, str], ...] | None = None @staticmethod def _bundle_index(bundle) -> int | None: @@ -190,6 +192,58 @@ class IQModelManager(_BaseIQModelManager): if bundle_index is not None and self._download_index() is None: self.params.put(_DOWNLOAD_INDEX_KEY, bundle_index) + def _find_manifest_counterpart(self, target): + # never match by index: indexes shift between manifest generations, and a + # positional match could redownload a different model than the user selected + for attr in ("ref", "internalName", "displayName"): + value = getattr(target, attr, None) + if not value: + continue + for bundle in self.available_models: + if getattr(bundle, attr, None) == value: + return bundle + return None + + def _queue_active_manifest_refresh(self) -> None: + active = self.active_bundle + if active is None or is_default_bundle(active): + return + if self._download_index() is not None: + return + + counterpart = self._find_manifest_counterpart(active) + if counterpart is None: + return + counterpart_index = self._bundle_index(counterpart) + if counterpart_index is None: + return + + active_files = dict(self._bundle_files(active)) + stale = False + for filename, sha in self._bundle_files(counterpart): + if not sha: + continue + active_sha = active_files.get(filename) + # an empty recorded hash can't prove a mismatch, so it never triggers a redownload + if active_sha is None or (active_sha and active_sha.lower() != sha.lower()): + stale = True + break + if not stale: + self._manifest_refresh_key = None + return + + # the manifest may be an expired offline cache, so keep the active bundle and its + # files in place: the download flow replaces artifacts atomically and only persists + # the counterpart as active once everything landed. One attempt per bundle per run + # so a dead network doesn't turn the 1Hz loop into a download-retry storm. + key = self._bundle_validation_key(active) + if key == self._manifest_refresh_key: + return + self._manifest_refresh_key = key + + cloudlog.warning(f"Active model {_display_bundle_name(active)} artifacts are stale vs current manifest; queueing redownload") + self.params.put(_DOWNLOAD_INDEX_KEY, counterpart_index) + async def _download_file(self, url: str, path: str, model) -> None: temp_path = f"{path}.download" self._download_start_times[model.fileName] = time.monotonic() @@ -302,6 +356,7 @@ class IQModelManager(_BaseIQModelManager): self.active_bundle = get_active_bundle(self.params) self._queue_active_redownload_if_invalid() self._queue_tinygrad_upgrade() + self._queue_active_manifest_refresh() if (index_to_download := self._download_index()) is not None: if model_to_download := next((model for model in self.available_models if model.index == index_to_download), None): diff --git a/iqpilot/selfdrive/iqmodeld/tests/test_manager_manifest_refresh.py b/iqpilot/selfdrive/iqmodeld/tests/test_manager_manifest_refresh.py new file mode 100644 index 0000000..19e20af --- /dev/null +++ b/iqpilot/selfdrive/iqmodeld/tests/test_manager_manifest_refresh.py @@ -0,0 +1,150 @@ +""" +Copyright (c) IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos +""" +from dataclasses import dataclass, field + +from openpilot.iqpilot.selfdrive.iqmodeld.models.manager import IQModelManager, _DOWNLOAD_INDEX_KEY + + +@dataclass +class _DownloadUri: + sha256: str = "" + uri: str = "" + + +@dataclass +class _Artifact: + fileName: str = "" + downloadUri: _DownloadUri = field(default_factory=_DownloadUri) + + +@dataclass +class _Model: + artifact: _Artifact = field(default_factory=_Artifact) + metadata: _Artifact | None = None + + +@dataclass +class _Bundle: + index: int = 0 + ref: str = "" + internalName: str = "" + displayName: str = "" + models: list = field(default_factory=list) + + +class _FakeParams: + def __init__(self): + self.store = {} + + def get(self, key): + return self.store.get(key) + + def put(self, key, value): + self.store[key] = value + + def remove(self, key): + self.store.pop(key, None) + + +def _bundle(index, name, sha, filename="driving_vision_test_tinygrad.pkl"): + return _Bundle( + index=index, + ref=f"ref-{name}", + internalName=name, + displayName=f"{name} display", + models=[_Model(artifact=_Artifact(fileName=filename, downloadUri=_DownloadUri(sha256=sha)))], + ) + + +def _manager(active, available): + mgr = IQModelManager.__new__(IQModelManager) + mgr.params = _FakeParams() + mgr.active_bundle = active + mgr.available_models = available + mgr._validated_active_key = None + mgr._manifest_refresh_key = None + return mgr + + +def test_stale_active_bundle_queues_redownload_at_current_index(): + active = _bundle(55, "WMIV12", "a" * 64) + counterpart = _bundle(12, "WMIV12", "b" * 64) + mgr = _manager(active, [counterpart]) + + mgr._queue_active_manifest_refresh() + + assert mgr.params.get(_DOWNLOAD_INDEX_KEY) == 12 + assert mgr.active_bundle is active + + +def test_matching_shas_do_not_queue(): + active = _bundle(55, "WMIV12", "a" * 64) + counterpart = _bundle(12, "WMIV12", "A" * 64) + mgr = _manager(active, [counterpart]) + + mgr._queue_active_manifest_refresh() + + assert mgr.params.get(_DOWNLOAD_INDEX_KEY) is None + + +def test_retired_bundle_is_left_alone(): + active = _bundle(55, "WMIV12", "a" * 64) + mgr = _manager(active, [_bundle(12, "OtherModel", "b" * 64)]) + + mgr._queue_active_manifest_refresh() + + assert mgr.params.get(_DOWNLOAD_INDEX_KEY) is None + assert mgr.active_bundle is active + + +def test_default_bundle_is_never_refreshed(): + active = _bundle(0, "Default", "a" * 64) + active.ref = "default" + mgr = _manager(active, [_bundle(0, "Default", "b" * 64)]) + + mgr._queue_active_manifest_refresh() + + assert mgr.params.get(_DOWNLOAD_INDEX_KEY) is None + + +def test_pending_download_blocks_refresh(): + active = _bundle(55, "WMIV12", "a" * 64) + mgr = _manager(active, [_bundle(12, "WMIV12", "b" * 64)]) + mgr.params.put(_DOWNLOAD_INDEX_KEY, 3) + + mgr._queue_active_manifest_refresh() + + assert mgr.params.get(_DOWNLOAD_INDEX_KEY) == 3 + + +def test_empty_manifest_hash_never_triggers(): + active = _bundle(55, "WMIV12", "a" * 64) + mgr = _manager(active, [_bundle(12, "WMIV12", "")]) + + mgr._queue_active_manifest_refresh() + + assert mgr.params.get(_DOWNLOAD_INDEX_KEY) is None + + +def test_refresh_queued_once_per_run(): + active = _bundle(55, "WMIV12", "a" * 64) + mgr = _manager(active, [_bundle(12, "WMIV12", "b" * 64)]) + + mgr._queue_active_manifest_refresh() + assert mgr.params.get(_DOWNLOAD_INDEX_KEY) == 12 + + mgr.params.remove(_DOWNLOAD_INDEX_KEY) + mgr._queue_active_manifest_refresh() + assert mgr.params.get(_DOWNLOAD_INDEX_KEY) is None + + +def test_counterpart_matched_by_name_not_index(): + active = _bundle(55, "WMIV12", "a" * 64) + imposter = _bundle(55, "OtherModel", "c" * 64) + counterpart = _bundle(12, "WMIV12", "b" * 64) + mgr = _manager(active, [imposter, counterpart]) + + mgr._queue_active_manifest_refresh() + + assert mgr.params.get(_DOWNLOAD_INDEX_KEY) == 12 diff --git a/iqpilot/selfdrive/selfdrived/events.py b/iqpilot/selfdrive/selfdrived/events.py index 76d81ae..674e551 100644 --- a/iqpilot/selfdrive/selfdrived/events.py +++ b/iqpilot/selfdrive/selfdrived/events.py @@ -337,6 +337,13 @@ _NOTICE_EVENTS: EVENTS_IQ_TYPE = { Priority.MID, VisualAlert.none, AudibleAlert.prompt, 3.), }, + # outranks the generic processNotRunning alert so the driver sees why engagement is blocked + EventNameIQ.modelUpdating: { + ET.NO_ENTRY: NoEntryAlert("Update finishes while parked with internet", + alert_text_1="Driving Model Updating", + priority=Priority.MID), + }, + } EVENTS_IQ: EVENTS_IQ_TYPE = {**_GUIDANCE_EVENTS, **_ENGAGE_EVENTS, **_CABIN_BLOCK_EVENTS, **_NOTICE_EVENTS} diff --git a/iqpilot/ui/layouts/settings/trips.py b/iqpilot/ui/layouts/settings/drive_history.py similarity index 100% rename from iqpilot/ui/layouts/settings/trips.py rename to iqpilot/ui/layouts/settings/drive_history.py diff --git a/iqpilot/ui/layouts/settings/iq_panels.py b/iqpilot/ui/layouts/settings/iq_panels.py index 0f023af..529efd8 100644 --- a/iqpilot/ui/layouts/settings/iq_panels.py +++ b/iqpilot/ui/layouts/settings/iq_panels.py @@ -194,7 +194,7 @@ CHEVRON_INFO_DESCRIPTION = { # param key -> (title fn, description fn) _HUD_TOGGLES = { - "BlindSpot": ( + "IQBlindSpotAlerts": ( lambda: tr("Blind Spot Alerts"), lambda: tr("Flashes a side warning whenever the car reports something sitting in your blind spot (BSM-equipped cars only)."), ), @@ -202,20 +202,20 @@ _HUD_TOGGLES = { lambda: tr("Expanded Status Bar"), lambda: tr("Bring back the classic UI's wide offroad status strip: temperature, vehicle, and Konn3kt state at a glance."), ), - "TorqueBar": ( + "IQSteerEffortArc": ( lambda: tr("Steering Effort Arc"), lambda: tr("Trace an arc over the road view showing how much steering IQ.Pilot is applying while lateral control runs."), ), - "RoadNameToggle": ( + "IQRoadNameOverlay": ( lambda: tr("Road Name Overlay"), lambda: tr("Show the current road's name over the driving view." "
Requires offline map data for your region to be installed."), ), - "ShowTurnSignals": ( + "IQBlinkerIndicators": ( lambda: tr("Blinker Indicators"), lambda: tr("Mirror the car's blinkers as arrows on the driving screen."), ), - "RocketFuel": ( + "IQAccelMeter": ( lambda: tr("Acceleration Meter"), lambda: tr("Draw a bar along the left edge tracking measured acceleration and braking — what the car is actually " "doing right now, not the planner's request."), @@ -243,7 +243,7 @@ class VisualsLayout(Widget): title=lambda: tr("Lead Vehicle Readouts"), description="", buttons=[lambda: tr("Off"), lambda: tr("Distance"), lambda: tr("Speed"), lambda: tr("Time"), lambda: tr("All")], - param="ChevronInfo", + param="IQLeadReadouts", inline=False, ) self._dev_ui_info = toggle_item( @@ -270,12 +270,12 @@ class VisualsLayout(Widget): def _sync_chevron_row(self): if ui_state.has_longitudinal_control: self._chevron_info.set_description(tr(CHEVRON_INFO_DESCRIPTION["enabled"])) - self._chevron_info.action_item.set_selected_button(ui_state.params.get("ChevronInfo", return_default=True)) + self._chevron_info.action_item.set_selected_button(ui_state.params.get("IQLeadReadouts", return_default=True)) self._chevron_info.action_item.set_enabled(True) else: self._chevron_info.set_description(tr(CHEVRON_INFO_DESCRIPTION["disabled"])) self._chevron_info.action_item.set_enabled(False) - ui_state.params.put("ChevronInfo", 0) + ui_state.params.put("IQLeadReadouts", 0) def _update_state(self): super()._update_state() @@ -1388,7 +1388,7 @@ class IQDeviceLayout(DeviceLayout): DeviceLayout._initialize_items(self) # Using dual button with no right button for better alignment - self._always_offroad_btn = self._left_button(lambda: tr("Force Offroad Mode"), self._handle_always_offroad) + self._always_offroad_btn = self._left_button(lambda: tr("Keep Device Offroad"), self._handle_always_offroad) self._force_onroad_btn = self._left_button(lambda: tr("Force On-Road (10 min)"), self._handle_force_onroad) self._max_time_offroad = option_item( @@ -1592,7 +1592,7 @@ class IQDeviceLayout(DeviceLayout): force_onroad_active = force_onroad_until > now # Text & Color - offroad_mode_btn_text = tr("Exit Offroad Mode") if always_offroad else tr("Force Offroad Mode") + offroad_mode_btn_text = tr("Exit Offroad Mode") if always_offroad else tr("Keep Device Offroad") offroad_mode_btn_style = ButtonStyle.PRIMARY if always_offroad else ButtonStyle.DANGER self._always_offroad_btn.action_item.left_button.set_text(offroad_mode_btn_text) self._always_offroad_btn.action_item.left_button.set_button_style(offroad_mode_btn_style) @@ -1957,12 +1957,12 @@ class ModelsLayout(Widget): return folders_list def _handle_current_model_clicked(self): - favs = ui_state.params.get("ModelManager_Favs") + favs = ui_state.params.get("IQModelFavorites") favorites = set(favs.split(';')) if favs else set() folders_list = self._get_folders(favorites) active_ref = self.model_manager.activeBundle.ref if self._has_active_bundle_param() and self.model_manager.activeBundle else "Default" - self.model_dialog = PickerDialog(tr("Choose a Model"), folders_list, active_ref, "ModelManager_Favs", + self.model_dialog = PickerDialog(tr("Choose a Model"), folders_list, active_ref, "IQModelFavorites", 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) @@ -2191,8 +2191,8 @@ class HyundaiSettings(BrandPanel): self.longitudinal_tuning_item = multiple_button_item( tr("Longitudinal Tune Profile"), "", [tr("Off"), tr("Dynamic"), tr("Predictive")], - button_width=300, param="HyundaiLongitudinalTuning", inline=False, - callback=lambda index: ui_state.params.put("HyundaiLongitudinalTuning", index)) + button_width=300, param="IQHyundaiLongTune", inline=False, + callback=lambda index: ui_state.params.put("IQHyundaiLongTune", index)) self.items = [self.longitudinal_tuning_item] def _alpha_long_supported(self) -> bool: @@ -2204,7 +2204,7 @@ class HyundaiSettings(BrandPanel): def update_settings(self): self.alpha_long_available = self._alpha_long_supported() - selected = int(ui_state.params.get("HyundaiLongitudinalTuning") or "0") + selected = int(ui_state.params.get("IQHyundaiLongTune") or "0") if not ui_state.is_offroad(): desc, usable = tr("Unavailable while the car is onroad."), False @@ -2231,11 +2231,11 @@ class SubaruSettings(BrandPanel): def __init__(self): super().__init__() self._supported = False - self.stop_and_go_toggle = toggle_item(tr("Creep from Standstill (Beta)"), "", param="SubaruStopAndGo", + self.stop_and_go_toggle = toggle_item(tr("Creep from Standstill (Beta)"), "", param="IQSubaruCreepAssist", callback=lambda _: self.update_settings()) self.stop_and_go_manual_parking_brake_toggle = toggle_item( tr("Creep from Standstill — Manual Handbrake (Beta)"), "", - param="SubaruStopAndGoManualParkingBrake", callback=lambda _: self.update_settings()) + param="IQSubaruCreepAssistManualBrake", callback=lambda _: self.update_settings()) self.items = [self.stop_and_go_toggle, self.stop_and_go_manual_parking_brake_toggle] def _platform_flags(self) -> int: @@ -2286,8 +2286,8 @@ def _speed_text(kmh: int) -> str: class TeslaSettings(BrandPanel): def __init__(self): super().__init__() - self.coop_steering_toggle = toggle_item(tr("VTB (Virtual Torque Blending)"), "", param="TeslaCoopSteering") - self.items = [self.coop_steering_toggle] + self.torque_blend_toggle = toggle_item(tr("VTB (Virtual Torque Blending)"), "", param="IQTeslaTorqueBlend") + self.items = [self.torque_blend_toggle] def update_settings(self): caution = tr("Warning: steering may oscillate in turns below {}; turn this off if you feel it.").format( @@ -2300,8 +2300,8 @@ class TeslaSettings(BrandPanel): blocker = tr("Flip on Always Offroad from the Device panel, or power the car down, to change this.") body = f"{blocker}

{body}" - self.coop_steering_toggle.set_description(body) - self.coop_steering_toggle.action_item.set_enabled(ui_state.is_offroad()) + self.torque_blend_toggle.set_description(body) + self.torque_blend_toggle.action_item.set_enabled(ui_state.is_offroad()) # ===== vehicle_brands_toyota ===== @@ -2312,7 +2312,7 @@ class ToyotaSettings(BrandPanel): self.enforce_stock_longitudinal = toggle_item( lambda: tr("Keep Factory Gas and Brake"), description=lambda: tr("Keeps gas and brakes with the factory Toyota system; IQ.Pilot steers only."), - initial_state=ui_state.params.get_bool("ToyotaEnforceStockLongitudinal"), + initial_state=ui_state.params.get_bool("IQToyotaFactoryLong"), callback=self._on_toggled, enabled=lambda: not ui_state.engaged, ) @@ -2320,7 +2320,7 @@ class ToyotaSettings(BrandPanel): @staticmethod def _apply(enabled: bool): - ui_state.params.put_bool("ToyotaEnforceStockLongitudinal", enabled) + ui_state.params.put_bool("IQToyotaFactoryLong", enabled) if enabled and ui_state.params.get_bool("AlphaLongitudinalEnabled"): ui_state.params.put_bool("AlphaLongitudinalEnabled", False) ui_state.params.put_bool("OnroadCycleRequested", True) diff --git a/iqpilot/ui/mici/onroad/road_name.py b/iqpilot/ui/mici/onroad/road_label.py similarity index 100% rename from iqpilot/ui/mici/onroad/road_name.py rename to iqpilot/ui/mici/onroad/road_label.py diff --git a/iqpilot/ui/onroad/model_renderer.py b/iqpilot/ui/onroad/model_renderer.py deleted file mode 100644 index e5e57ac..0000000 --- a/iqpilot/ui/onroad/model_renderer.py +++ /dev/null @@ -1,10 +0,0 @@ -""" -Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos -""" -from openpilot.iqpilot.ui.onroad.hud_overlays import ChevronMetrics -from openpilot.iqpilot.ui.onroad.rainbow_path import RainbowPath - - -class IQModelRenderer: - def __init__(self): - self.rainbow_path = RainbowPath() diff --git a/iqpilot/ui/onroad/rainbow_path.py b/iqpilot/ui/onroad/rainbow_path.py deleted file mode 100644 index bd6bf6e..0000000 --- a/iqpilot/ui/onroad/rainbow_path.py +++ /dev/null @@ -1,41 +0,0 @@ -""" -Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos -""" -import colorsys -import time - -import pyray as rl -from openpilot.system.ui.lib.shader_polygon import draw_polygon, Gradient - -# Scrolling spectrum along the driving path: a fixed set of stops from the -# bottom (1.0) to the top (0.0) of the path, each a full-saturation swatch whose -# hue advances with time and whose opacity thins toward the horizon. -_SEGMENTS = 8 -_SCROLL_DEG_PER_S = 50.0 -_SATURATION = 0.9 -_LIGHTNESS = 0.6 -_ALPHA_NEAR = 0.8 # opacity at the bottom of the path -_ALPHA_HORIZON_FRACTION = 0.3 # how much of that opacity is shed by the top - -# Stop offsets are constant, so resolve them once. -_STOP_OFFSETS = tuple(i / (_SEGMENTS - 1) for i in range(_SEGMENTS)) - - -def _swatch(hue_turns: float, alpha: float) -> rl.Color: - r, g, b = colorsys.hls_to_rgb(hue_turns, _LIGHTNESS, _SATURATION) - return rl.Color(int(r * 255), int(g * 255), int(b * 255), int(alpha * 255)) - - -def _spectrum_gradient() -> Gradient: - scroll_deg = (time.monotonic() * _SCROLL_DEG_PER_S) % 360.0 - colors = [] - for offset in _STOP_OFFSETS: - hue_deg = (scroll_deg + offset * 360.0) % 360.0 - alpha = _ALPHA_NEAR * (1.0 - offset * _ALPHA_HORIZON_FRACTION) - colors.append(_swatch(hue_deg / 360.0, alpha)) - return Gradient(start=(0.0, 1.0), end=(0.0, 0.0), colors=colors, stops=list(_STOP_OFFSETS)) - - -class RainbowPath: - def draw_rainbow_path(self, rect, path): - draw_polygon(rect, path.projected_points, gradient=_spectrum_gradient()) diff --git a/selfdrive/assets/sounds/pre_alert.wav b/selfdrive/assets/sounds/pre_alert.wav deleted file mode 100644 index f83b710..0000000 Binary files a/selfdrive/assets/sounds/pre_alert.wav and /dev/null differ diff --git a/selfdrive/assets/sounds/prompt_distracted.wav b/selfdrive/assets/sounds/prompt_distracted.wav index 6cad556..c3d4475 100644 Binary files a/selfdrive/assets/sounds/prompt_distracted.wav and b/selfdrive/assets/sounds/prompt_distracted.wav differ diff --git a/selfdrive/assets/sounds/warning_immediate.wav b/selfdrive/assets/sounds/warning_immediate.wav index 99b4b03..b1815a9 100644 Binary files a/selfdrive/assets/sounds/warning_immediate.wav and b/selfdrive/assets/sounds/warning_immediate.wav differ diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 3def978..1bd7b31 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -260,7 +260,7 @@ class Controls(IQControlsLayer): hudControl.leadFollowTime = 1.45 hudControl.visualAlert = self.sm['selfdriveState'].alertHudVisual hudControl.audibleAlert = self.sm['selfdriveState'].alertSound - hudControl.driverUnresponsive = self.sm['driverMonitoringState'].noResponseForceDecel + hudControl.driverUnresponsive = self.sm['selfdriveState'].alertType.split('/', 1)[0] == 'driverUnresponsive' hudControl.rightLaneVisible = True hudControl.leftLaneVisible = True @@ -292,7 +292,7 @@ class Controls(IQControlsLayer): cs.upAccelCmd = float(self.LoC.pid.p) cs.uiAccelCmd = float(self.LoC.pid.i) cs.ufAccelCmd = float(self.LoC.pid.f) - cs.forceDecel = bool(self.sm['driverMonitoringState'].noResponseForceDecel or + cs.forceDecel = bool((self.sm['driverMonitoringState'].awarenessStatus < 0.) or (self.sm['selfdriveState'].state == State.softDisabling)) lat_tuning = self.CP.lateralTuning.which() diff --git a/selfdrive/controls/lib/longcontrol.py b/selfdrive/controls/lib/longcontrol.py index c304425..4be1953 100644 --- a/selfdrive/controls/lib/longcontrol.py +++ b/selfdrive/controls/lib/longcontrol.py @@ -47,6 +47,7 @@ class LongControl: (CP.longitudinalTuning.kiBP, CP.longitudinalTuning.kiV), rate=1 / DT_CTRL) self.last_output_accel = 0.0 + self.stopping_decel_rate = CP_IQ.stoppingDecelRateOverride or 1.0 self.smooth = SmoothStopController() def reset(self): @@ -75,7 +76,7 @@ class LongControl: if output_accel > self.CP.stopAccel: output_accel = min(output_accel, 0.0) # TODO: can we just go straight to stopAccel? - output_accel -= 1.0 * DT_CTRL # m/s^2/s while trying to stop + output_accel -= self.stopping_decel_rate * DT_CTRL # m/s^2/s while trying to stop self.reset() self.smooth.reset() diff --git a/selfdrive/debug/cycle_alerts.py b/selfdrive/debug/cycle_alerts.py index c1f30bc..5b25d8c 100755 --- a/selfdrive/debug/cycle_alerts.py +++ b/selfdrive/debug/cycle_alerts.py @@ -30,9 +30,9 @@ def cycle_alerts(duration=200, is_metric=False): (EventName.accFaulted, ET.IMMEDIATE_DISABLE), # DM sequence - (EventName.driverDistracted1, ET.WARNING), - (EventName.driverDistracted2, ET.WARNING), - (EventName.driverDistracted3, ET.WARNING), + (EventName.preDriverDistracted, ET.WARNING), + (EventName.promptDriverDistracted, ET.WARNING), + (EventName.driverDistracted, ET.WARNING), ] # debug alerts diff --git a/selfdrive/modeld/SConscript b/selfdrive/modeld/SConscript index 71989c8..40e7401 100644 --- a/selfdrive/modeld/SConscript +++ b/selfdrive/modeld/SConscript @@ -60,7 +60,7 @@ def host_tinygrad_flags(*, float16=False): return f"{base} FLOAT16=1" if float16 else base # Compile small models -for model_name in ['dmonitoring_model', 'dmonitoring_model_mici']: +for model_name in ['dmonitoring_model']: # The optimization flags are mandatory on QCOM: without FLOAT16/NOLOCALS/JIT_BATCH_SIZE/OPENPILOT_HACKS these # models compile to unoptimized QCOM kernels and run ~20x slower (dmonitoring_model: ~300ms -> ~14ms), # which starves the driving model on the shared Adreno. IMAGE=2 (not upstream's IMAGE=1) because the diff --git a/selfdrive/modeld/dmonitoringmodeld.py b/selfdrive/modeld/dmonitoringmodeld.py index 422a4a9..0e50932 100755 --- a/selfdrive/modeld/dmonitoringmodeld.py +++ b/selfdrive/modeld/dmonitoringmodeld.py @@ -20,25 +20,19 @@ from openpilot.selfdrive.locationd.calibration_helpers import get_calibrated_rpy from openpilot.selfdrive.modeld.models.commonmodel_pyx import CLContext, MonitoringModelFrame from openpilot.selfdrive.modeld.parse_model_outputs import sigmoid, safe_exp from openpilot.selfdrive.modeld.runners.tinygrad_helpers import qcom_tensor_from_opencl_address -from openpilot.system.hardware import HARDWARE PROCESS_NAME = "selfdrive.modeld.dmonitoringmodeld" SEND_RAW_PRED = os.getenv('SEND_RAW_PRED') -MODELS_PATH = Path(__file__).parent / 'models' - - -def get_model_paths(device_type: str) -> tuple[Path, Path]: - model_name = 'dmonitoring_model_mici' if device_type == 'mici' else 'dmonitoring_model' - return MODELS_PATH / f'{model_name}_tinygrad.pkl', MODELS_PATH / f'{model_name}_metadata.pkl' +MODEL_PKL_PATH = Path(__file__).parent / 'models/dmonitoring_model_tinygrad.pkl' +METADATA_PATH = Path(__file__).parent / 'models/dmonitoring_model_metadata.pkl' class ModelState: inputs: dict[str, np.ndarray] output: np.ndarray - def __init__(self, cl_ctx, device_type=None): - model_path, metadata_path = get_model_paths(device_type or HARDWARE.get_device_type()) - with open(metadata_path, 'rb') as f: + def __init__(self, cl_ctx): + with open(METADATA_PATH, 'rb') as f: model_metadata = pickle.load(f) self.input_shapes = model_metadata['input_shapes'] self.output_slices = model_metadata['output_slices'] @@ -49,7 +43,7 @@ class ModelState: } self.tensor_inputs = {k: Tensor(v, device='NPY').realize() for k,v in self.numpy_inputs.items()} - with open(model_path, "rb") as f: + with open(MODEL_PKL_PATH, "rb") as f: self.model_run = pickle.load(f) def run(self, buf: VisionBuf, calib: np.ndarray, transform: np.ndarray) -> tuple[np.ndarray, float]: @@ -81,10 +75,8 @@ def parse_model_output(model_output): face_descs = model_output[f'face_descs_{ds_suffix}'] parsed[f'face_descs_{ds_suffix}'] = face_descs[:, :-6] parsed[f'face_descs_{ds_suffix}_std'] = safe_exp(face_descs[:, -6:]) - for key in ['face_prob', 'left_eye_prob', 'right_eye_prob','left_blink_prob', 'right_blink_prob', 'sunglasses_prob', 'using_phone_prob', 'sleep_prob']: - output_key = f'{key}_{ds_suffix}' - if output_key in model_output: - parsed[output_key] = sigmoid(model_output[output_key]) + for key in ['face_prob', 'left_eye_prob', 'right_eye_prob','left_blink_prob', 'right_blink_prob', 'sunglasses_prob', 'using_phone_prob']: + parsed[f'{key}_{ds_suffix}'] = sigmoid(model_output[f'{key}_{ds_suffix}']) return parsed def fill_driver_data(msg, model_output, ds_suffix): @@ -99,8 +91,6 @@ def fill_driver_data(msg, model_output, ds_suffix): msg.rightBlinkProb = model_output[f'right_blink_prob_{ds_suffix}'][0, 0].item() msg.sunglassesProb = model_output[f'sunglasses_prob_{ds_suffix}'][0, 0].item() msg.phoneProb = model_output[f'using_phone_prob_{ds_suffix}'][0, 0].item() - sleep_prob = model_output.get(f'sleep_prob_{ds_suffix}') - msg.sleepProb = sleep_prob[0, 0].item() if sleep_prob is not None else 0. def get_driverstate_packet(model_output, frame_id: int, location_ts: int, exec_time: float, gpu_exec_time: float): msg = messaging.new_message('driverStateV2', valid=True) diff --git a/selfdrive/modeld/models/dmonitoring_model.onnx b/selfdrive/modeld/models/dmonitoring_model.onnx index 873c74b..51d1849 100644 Binary files a/selfdrive/modeld/models/dmonitoring_model.onnx and b/selfdrive/modeld/models/dmonitoring_model.onnx differ diff --git a/selfdrive/modeld/models/dmonitoring_model_metadata.pkl b/selfdrive/modeld/models/dmonitoring_model_metadata.pkl index 9aad8d4..7869f9d 100644 Binary files a/selfdrive/modeld/models/dmonitoring_model_metadata.pkl and b/selfdrive/modeld/models/dmonitoring_model_metadata.pkl differ diff --git a/selfdrive/modeld/models/dmonitoring_model_mici.onnx b/selfdrive/modeld/models/dmonitoring_model_mici.onnx deleted file mode 100644 index 51d1849..0000000 Binary files a/selfdrive/modeld/models/dmonitoring_model_mici.onnx and /dev/null differ diff --git a/selfdrive/modeld/models/dmonitoring_model_mici_metadata.pkl b/selfdrive/modeld/models/dmonitoring_model_mici_metadata.pkl deleted file mode 100644 index 7869f9d..0000000 Binary files a/selfdrive/modeld/models/dmonitoring_model_mici_metadata.pkl and /dev/null differ diff --git a/selfdrive/modeld/models/dmonitoring_model_mici_tinygrad.pkl b/selfdrive/modeld/models/dmonitoring_model_mici_tinygrad.pkl deleted file mode 100644 index a89f4f1..0000000 Binary files a/selfdrive/modeld/models/dmonitoring_model_mici_tinygrad.pkl and /dev/null differ diff --git a/selfdrive/modeld/models/dmonitoring_model_tinygrad.pkl b/selfdrive/modeld/models/dmonitoring_model_tinygrad.pkl index 8f33bbb..a89f4f1 100644 Binary files a/selfdrive/modeld/models/dmonitoring_model_tinygrad.pkl and b/selfdrive/modeld/models/dmonitoring_model_tinygrad.pkl differ diff --git a/selfdrive/modeld/models/prebuilt_check.json b/selfdrive/modeld/models/prebuilt_check.json index 1014aa4..6d0839d 100644 --- a/selfdrive/modeld/models/prebuilt_check.json +++ b/selfdrive/modeld/models/prebuilt_check.json @@ -1,15 +1,8 @@ { "dmonitoring_model": { "outputs": { - "dmonitoring_model_metadata.pkl": "5999c262b1c25c62e485fb4ced5806d20a8ca59e3ae94e0ff499c0fe3497fedc", - "dmonitoring_model_tinygrad.pkl": "5aca89a35b42376d56f67ccd28a1080806706d546dd8c63f6e1b4c681f8e2c01" - }, - "signature": "2364ebd4bb95c4b4e539c9b1ba68324b713cbb56617396b73262accf9cdcfbfc" - }, - "dmonitoring_model_mici": { - "outputs": { - "dmonitoring_model_mici_metadata.pkl": "31a86ab7a92dc0af088b15787a440dd3b210aa662e445a15145900e559a1b5c3", - "dmonitoring_model_mici_tinygrad.pkl": "806c0ea75df6bf6dfeb81b832314c68e31df5865a52d0359e6eeb76d93ad2b52" + "dmonitoring_model_metadata.pkl": "31a86ab7a92dc0af088b15787a440dd3b210aa662e445a15145900e559a1b5c3", + "dmonitoring_model_tinygrad.pkl": "806c0ea75df6bf6dfeb81b832314c68e31df5865a52d0359e6eeb76d93ad2b52" }, "signature": "e1eeb5ce45774a816c8da2394e6ee35ebf700b71dff345e0141dbab8ff592349" } diff --git a/selfdrive/modeld/prebuilt_models.py b/selfdrive/modeld/prebuilt_models.py index 384939f..73a16de 100644 --- a/selfdrive/modeld/prebuilt_models.py +++ b/selfdrive/modeld/prebuilt_models.py @@ -11,7 +11,7 @@ BASEDIR = MODELD_DIR.parents[1] TINYGRAD_DIR = BASEDIR / 'tinygrad_repo' METADATA_SCRIPT = MODELD_DIR / 'get_model_metadata.py' -MODEL_NAMES = ['dmonitoring_model', 'dmonitoring_model_mici'] +MODEL_NAMES = ['dmonitoring_model'] def _hash_file(h, path: Path) -> None: diff --git a/selfdrive/modeld/test_dmonitoringmodeld.py b/selfdrive/modeld/test_dmonitoringmodeld.py deleted file mode 100644 index daf7be6..0000000 --- a/selfdrive/modeld/test_dmonitoringmodeld.py +++ /dev/null @@ -1,37 +0,0 @@ -import pickle - -import numpy as np -import pytest - -from openpilot.selfdrive.modeld.dmonitoringmodeld import get_driverstate_packet, get_model_paths, parse_model_output, slice_outputs - - -@pytest.mark.parametrize("device_type, model_name", [ - ("mici", "dmonitoring_model_mici"), - ("tici", "dmonitoring_model"), - ("tizi", "dmonitoring_model"), -]) -def test_model_paths(device_type, model_name): - model_path, metadata_path = get_model_paths(device_type) - - assert model_path.name == f"{model_name}_tinygrad.pkl" - assert metadata_path.name == f"{model_name}_metadata.pkl" - - -@pytest.mark.parametrize("device_type, expected_sleep_prob", [ - ("mici", 0.), - ("tici", 0.5), - ("tizi", 0.5), -]) -def test_sleep_probability_output(device_type, expected_sleep_prob): - _, metadata_path = get_model_paths(device_type) - with open(metadata_path, 'rb') as f: - metadata = pickle.load(f) - - output = np.zeros(metadata['output_shapes']['outputs'][1], dtype=np.float32) - parsed = parse_model_output(slice_outputs(output, metadata['output_slices'])) - parsed['raw_pred'] = b'' - msg = get_driverstate_packet(parsed, 1, 0, 0., 0.) - - assert msg.driverStateV2.leftDriverData.sleepProb == expected_sleep_prob - assert msg.driverStateV2.rightDriverData.sleepProb == expected_sleep_prob diff --git a/selfdrive/modeld/test_prebuilt_models.py b/selfdrive/modeld/test_prebuilt_models.py index 1b05e51..a994982 100644 --- a/selfdrive/modeld/test_prebuilt_models.py +++ b/selfdrive/modeld/test_prebuilt_models.py @@ -1,52 +1,51 @@ import hashlib import json -import pytest - from openpilot.selfdrive.modeld import prebuilt_models -def write_outputs(models_dir, check_path, model_names): - checks = {} - for model_name in model_names: - outputs = {} - for suffix, contents in { - 'tinygrad.pkl': b'tinygrad', - 'metadata.pkl': b'metadata', - }.items(): - name = f'{model_name}_{suffix}' - (models_dir / name).write_bytes(contents) - outputs[name] = hashlib.sha256(contents).hexdigest() - checks[model_name] = {'outputs': outputs} - check_path.write_text(json.dumps(checks)) +def write_outputs(models_dir, check_path): + outputs = {} + for name, contents in { + 'dmonitoring_model_tinygrad.pkl': b'tinygrad', + 'dmonitoring_model_metadata.pkl': b'metadata', + }.items(): + (models_dir / name).write_bytes(contents) + outputs[name] = hashlib.sha256(contents).hexdigest() + check_path.write_text(json.dumps({'dmonitoring_model': {'outputs': outputs}})) -@pytest.fixture -def packaged_models(tmp_path, monkeypatch): +def test_packaged_prebuilt_without_onnx(tmp_path, monkeypatch): models_dir = tmp_path / 'models' models_dir.mkdir() check_path = models_dir / 'prebuilt_check.json' - write_outputs(models_dir, check_path, prebuilt_models.MODEL_NAMES) + write_outputs(models_dir, check_path) monkeypatch.setattr(prebuilt_models, 'MODELS_DIR', models_dir) monkeypatch.setattr(prebuilt_models, 'CHECK_PATH', check_path) - return models_dir + + assert prebuilt_models.packaged_prebuilt_matches('dmonitoring_model') + assert not prebuilt_models.verify_prebuilt('dmonitoring_model', 'flags') -@pytest.mark.parametrize("model_name", prebuilt_models.MODEL_NAMES) -def test_packaged_prebuilt_without_onnx(packaged_models, model_name): - assert prebuilt_models.packaged_prebuilt_matches(model_name) - assert not prebuilt_models.verify_prebuilt(model_name, 'flags') +def test_packaged_prebuilt_rejects_corrupt_output(tmp_path, monkeypatch): + models_dir = tmp_path / 'models' + models_dir.mkdir() + check_path = models_dir / 'prebuilt_check.json' + write_outputs(models_dir, check_path) + (models_dir / 'dmonitoring_model_tinygrad.pkl').write_bytes(b'corrupt') + monkeypatch.setattr(prebuilt_models, 'MODELS_DIR', models_dir) + monkeypatch.setattr(prebuilt_models, 'CHECK_PATH', check_path) + + assert not prebuilt_models.packaged_prebuilt_matches('dmonitoring_model') -@pytest.mark.parametrize("model_name", prebuilt_models.MODEL_NAMES) -def test_packaged_prebuilt_rejects_corrupt_output(packaged_models, model_name): - (packaged_models / f'{model_name}_tinygrad.pkl').write_bytes(b'corrupt') +def test_source_checkout_is_not_packaged_prebuilt(tmp_path, monkeypatch): + models_dir = tmp_path / 'models' + models_dir.mkdir() + check_path = models_dir / 'prebuilt_check.json' + write_outputs(models_dir, check_path) + (models_dir / 'dmonitoring_model.onnx').write_bytes(b'onnx') + monkeypatch.setattr(prebuilt_models, 'MODELS_DIR', models_dir) + monkeypatch.setattr(prebuilt_models, 'CHECK_PATH', check_path) - assert not prebuilt_models.packaged_prebuilt_matches(model_name) - - -@pytest.mark.parametrize("model_name", prebuilt_models.MODEL_NAMES) -def test_source_checkout_is_not_packaged_prebuilt(packaged_models, model_name): - (packaged_models / f'{model_name}.onnx').write_bytes(b'onnx') - - assert not prebuilt_models.packaged_prebuilt_matches(model_name) + assert not prebuilt_models.packaged_prebuilt_matches('dmonitoring_model') diff --git a/selfdrive/monitoring/README.md b/selfdrive/monitoring/README.md new file mode 100644 index 0000000..2a29ea0 --- /dev/null +++ b/selfdrive/monitoring/README.md @@ -0,0 +1,15 @@ +# driver monitoring (DM) + +Uploading driver-facing camera footage is opt-in, but it is encouraged to opt-in to improve the DM model. You can always change your preference using the "Record and Upload Driver Camera" toggle. + +## Troubleshooting + +Before creating a bug report, go through these troubleshooting steps. + +* Ensure the driver-facing camera has a good view of the driver in normal driving positions. + * This can be checked in Settings -> Device -> Preview Driver Camera (when car is off). +* If the camera can't see the driver, the device should be re-mounted. + +## Bug report + +In order for us to look into DM bug reports, we'll need the driver-facing camera footage. If you don't normally have this enabled, simply enable the toggle for a single drive. Also ensure the "Upload Raw Logs" toggle is enabled before going for a drive. diff --git a/selfdrive/monitoring/dmonitoringd.py b/selfdrive/monitoring/dmonitoringd.py index 552b8fb..022415a 100755 --- a/selfdrive/monitoring/dmonitoringd.py +++ b/selfdrive/monitoring/dmonitoringd.py @@ -1,33 +1,8 @@ #!/usr/bin/env python3 -from types import SimpleNamespace - import cereal.messaging as messaging from openpilot.common.params import Params from openpilot.common.realtime import config_realtime_process -from openpilot.selfdrive.monitoring.legacy_policy import DRIVER_MONITOR_SETTINGS as LegacySettings -from openpilot.selfdrive.monitoring.legacy_policy import DriverMonitoring as LegacyDriverMonitoring -from openpilot.selfdrive.monitoring.policy import DriverMonitoring as UpstreamDriverMonitoring -from openpilot.system.hardware import HARDWARE - - -def use_legacy_dm(device_type: str) -> bool: - return device_type == 'mici' - - -def create_driver_monitoring(device_type: str, rhd_saved: bool, always_on: bool): - if use_legacy_dm(device_type): - return LegacyDriverMonitoring(rhd_saved=rhd_saved, settings=LegacySettings(device_type), always_on=always_on) - return UpstreamDriverMonitoring(rhd_saved=rhd_saved, always_on=always_on) - - -def get_dm_inputs(sm): - return { - 'driverStateV2': sm['driverStateV2'], - 'liveCalibration': sm['liveCalibration'], - 'carState': sm['carState'], - 'selfdriveState': SimpleNamespace(enabled=sm['selfdriveState'].enabled or sm['carControl'].latActive), - 'modelV2': sm['modelV2'], - } +from openpilot.selfdrive.monitoring.helpers import DriverMonitoring def dmonitoringd_thread(): @@ -38,9 +13,7 @@ def dmonitoringd_thread(): sm = messaging.SubMaster(['driverStateV2', 'liveCalibration', 'carState', 'selfdriveState', 'modelV2', 'carControl'], poll='driverStateV2') - device_type = HARDWARE.get_device_type() - legacy_dm = use_legacy_dm(device_type) - DM = create_driver_monitoring(device_type, params.get_bool("IsRhdDetected"), params.get_bool("AlwaysOnDM")) + DM = DriverMonitoring(rhd_saved=params.get_bool("IsRhdDetected"), always_on=params.get_bool("AlwaysOnDM")) demo_mode=False # 20Hz <- dmonitoringmodeld @@ -52,9 +25,9 @@ def dmonitoringd_thread(): valid = sm.all_checks() if demo_mode and sm.valid['driverStateV2']: - DM.run_step(sm if legacy_dm else get_dm_inputs(sm), demo=True) + DM.run_step(sm, demo=demo_mode) elif valid: - DM.run_step(sm if legacy_dm else get_dm_inputs(sm), demo=demo_mode) + DM.run_step(sm, demo=demo_mode) # publish dat = DM.get_state_packet(valid=valid) @@ -66,11 +39,10 @@ def dmonitoringd_thread(): demo_mode = params.get_bool("IsDriverViewEnabled") # save rhd virtual toggle every 5 mins - wheelpos_offsetter = DM.wheelpos.prob_offseter if legacy_dm else DM.wheelpos_offsetter if (sm['driverStateV2'].frameId % 6000 == 0 and not demo_mode and - wheelpos_offsetter.filtered_stat.n > DM.settings._WHEELPOS_FILTER_MIN_COUNT and - DM.wheel_on_right == (wheelpos_offsetter.filtered_stat.M > DM.settings._WHEELPOS_THRESHOLD)): - params.put_bool("IsRhdDetected", DM.wheel_on_right) + DM.wheelpos.prob_offseter.filtered_stat.n > DM.settings._WHEELPOS_FILTER_MIN_COUNT and + DM.wheel_on_right == (DM.wheelpos.prob_offseter.filtered_stat.M > DM.settings._WHEELPOS_THRESHOLD)): + params.put_bool_nonblocking("IsRhdDetected", DM.wheel_on_right) def main(): dmonitoringd_thread() diff --git a/selfdrive/monitoring/legacy_policy.py b/selfdrive/monitoring/helpers.py similarity index 86% rename from selfdrive/monitoring/legacy_policy.py rename to selfdrive/monitoring/helpers.py index c450fd7..6f8752d 100644 --- a/selfdrive/monitoring/legacy_policy.py +++ b/selfdrive/monitoring/helpers.py @@ -13,12 +13,6 @@ from openpilot.selfdrive.locationd.calibration_helpers import get_calibrated_rpy from openpilot.system.hardware import HARDWARE EventName = log.OnroadEvent.EventName -AlertLevel = log.DriverMonitoringState.AlertLevel -MonitoringPolicy = log.DriverMonitoringState.MonitoringPolicy - - -def to_percent(v): - return int(min(max(v * 100., 0.), 100.)) # ****************************************************************************************** # NOTE: To fork maintainers. @@ -397,61 +391,44 @@ class DriverMonitoring: alert = None if self.awareness <= 0.: # terminal red alert: disengagement required - alert = EventName.driverDistracted3 if self.active_monitoring_mode else EventName.driverUnresponsive3 + alert = EventName.driverDistracted if self.active_monitoring_mode else EventName.driverUnresponsive self.terminal_time += 1 if awareness_prev > 0.: self.terminal_alert_cnt += 1 elif self.awareness <= self.threshold_prompt: # prompt orange alert - alert = EventName.driverDistracted2 if self.active_monitoring_mode else EventName.driverUnresponsive2 + alert = EventName.promptDriverDistracted if self.active_monitoring_mode else EventName.promptDriverUnresponsive elif self.awareness <= self.threshold_pre and not always_on_lowspeed_exemption: # pre green alert - alert = EventName.driverDistracted1 if self.active_monitoring_mode else EventName.driverUnresponsive1 + alert = EventName.preDriverDistracted if self.active_monitoring_mode else EventName.preDriverUnresponsive if alert is not None: self.current_events.add(alert) def get_state_packet(self, valid=True): + # build driverMonitoringState packet dat = messaging.new_message('driverMonitoringState', valid=valid) - dm = dat.driverMonitoringState - - dm.lockout = self.too_distracted - dm.alert3Count = self.terminal_alert_cnt - dm.noResponseCount = int(self.terminal_time >= self.settings._MAX_TERMINAL_DURATION) - dm.noResponseForceDecel = self.awareness <= 0. - dm.alwaysOn = self.always_on - dm.alwaysOnLockout = self.always_on and self.awareness <= self.threshold_prompt - if self.awareness <= 0.: - dm.alertLevel = AlertLevel.three - elif self.awareness <= self.threshold_prompt: - dm.alertLevel = AlertLevel.two - elif self.awareness <= self.threshold_pre: - dm.alertLevel = AlertLevel.one - dm.activePolicy = MonitoringPolicy.vision if self.active_monitoring_mode else MonitoringPolicy.wheeltouch - dm.isRHD = self.wheel_on_right - dm.rhdCalibration.calibratedPercent = to_percent(self.wheelpos.prob_offseter.filtered_stat.n / self.settings._WHEELPOS_FILTER_MIN_COUNT) - dm.rhdCalibration.offset = self.wheelpos.prob_offseter.filtered_stat.M - - dm.visionPolicyState.awarenessPercent = to_percent(self.awareness if self.active_monitoring_mode else self.awareness_active) - dm.visionPolicyState.awarenessStep = self.step_change if self.active_monitoring_mode else 0. - dm.visionPolicyState.isDistracted = self.driver_distracted - dm.visionPolicyState.distractedTypes.pose = DistractedType.DISTRACTED_POSE in self.distracted_types - dm.visionPolicyState.distractedTypes.eye = DistractedType.DISTRACTED_BLINK in self.distracted_types - dm.visionPolicyState.distractedTypes.phone = DistractedType.DISTRACTED_PHONE in self.distracted_types - dm.visionPolicyState.faceDetected = self.face_detected - dm.visionPolicyState.pose.pitch = self.pose.pitch - dm.visionPolicyState.pose.yaw = self.pose.yaw - dm.visionPolicyState.pose.calibrated = self.pose.calibrated - dm.visionPolicyState.pose.pitchCalib.calibratedPercent = to_percent(self.pose.pitch_offseter.filtered_stat.n / self.settings._POSE_OFFSET_MIN_COUNT) - dm.visionPolicyState.pose.pitchCalib.offset = self.pose.pitch_offseter.filtered_stat.M - dm.visionPolicyState.pose.yawCalib.calibratedPercent = to_percent(self.pose.yaw_offseter.filtered_stat.n / self.settings._POSE_OFFSET_MIN_COUNT) - dm.visionPolicyState.pose.yawCalib.offset = self.pose.yaw_offseter.filtered_stat.M - dm.visionPolicyState.pose.uncertainty = max(self.pose.pitch_std, self.pose.yaw_std) - dm.visionPolicyState.wheeltouchFallbackPercent = to_percent(self.hi_stds / self.settings._HI_STD_FALLBACK_TIME) - dm.visionPolicyState.uncertainOffroadAlertPercent = to_percent(self.dcam_uncertain_cnt / int(60 / self.settings._DT_DMON)) - - dm.wheeltouchPolicyState.awarenessPercent = to_percent(self.awareness if not self.active_monitoring_mode else self.awareness_passive) - dm.wheeltouchPolicyState.awarenessStep = self.step_change if not self.active_monitoring_mode else 0. + dat.driverMonitoringState = { + "events": self.current_events.to_msg(), + "faceDetected": self.face_detected, + "isDistracted": self.driver_distracted, + "distractedType": sum(self.distracted_types), + "awarenessStatus": self.awareness, + "posePitchOffset": self.pose.pitch_offseter.filtered_stat.mean(), + "posePitchValidCount": self.pose.pitch_offseter.filtered_stat.n, + "poseYawOffset": self.pose.yaw_offseter.filtered_stat.mean(), + "poseYawValidCount": self.pose.yaw_offseter.filtered_stat.n, + "phoneProbOffset": self.phone.prob_offseter.filtered_stat.mean(), + "phoneProbValidCount": self.phone.prob_offseter.filtered_stat.n, + "stepChange": self.step_change, + "awarenessActive": self.awareness_active, + "awarenessPassive": self.awareness_passive, + "isLowStd": self.pose.low_std, + "hiStdCount": self.hi_stds, + "isActiveMode": self.active_monitoring_mode, + "isRHD": self.wheel_on_right, + "uncertainCount": self.dcam_uncertain_cnt, + } return dat def run_step(self, sm, demo=False): diff --git a/selfdrive/monitoring/policy.py b/selfdrive/monitoring/policy.py deleted file mode 100644 index 1c55b4d..0000000 --- a/selfdrive/monitoring/policy.py +++ /dev/null @@ -1,467 +0,0 @@ -from collections import defaultdict -from math import atan2, radians -import numpy as np - -from cereal import car, log -import cereal.messaging as messaging -from openpilot.common.realtime import DT_DMON -from openpilot.common.filter_simple import FirstOrderFilter -from openpilot.common.params import Params -from openpilot.common.stat_live import RunningStatFilter -from openpilot.common.transformations.camera import DEVICE_CAMERAS - -AlertLevel = log.DriverMonitoringState.AlertLevel -MonitoringPolicy = log.DriverMonitoringState.MonitoringPolicy - -def to_percent(v): - return int(min(max(v * 100., 0.), 100.)) - -# ****************************************************************************************** -# NOTE: To fork maintainers. -# Disabling or nerfing safety features will get you and your users banned from our servers. -# We recommend that you do not change these numbers from the defaults. -# ****************************************************************************************** - -class DRIVER_MONITOR_SETTINGS: - def __init__(self): - # https://eur-lex.europa.eu/legal-content/EN/TXT/PDF/?uri=OJ:L_202501899 - self._ALERT_MIN_SPEED = 2.8 # 10 km/h - - self._WHEELTOUCH_POLICY_ALERT_1_TIMEOUT = 5. - self._WHEELTOUCH_POLICY_ALERT_2_TIMEOUT = 15. - self._WHEELTOUCH_POLICY_ALERT_3_TIMEOUT = 25. - self._VISION_POLICY_ALERT_1_TIMEOUT = 5. - self._VISION_POLICY_ALERT_2_TIMEOUT = 8. - self._VISION_POLICY_ALERT_3_TIMEOUT = 13. - - # no response = alert_3 sustained for certain amount of time - self._NO_RESPONSE_TIMEOUT = 5. - - # lockout specs - self._MAX_ALERT_3 = 2 - self._MAX_NO_RESPONSE = 1 - self._LOCKOUT_TIMES = [int(60 * n_min / DT_DMON) for n_min in [1, 5, 15, 30]] - - self._TIMEOUT_RECOVERY_FACTOR_MAX = 5. - self._TIMEOUT_RECOVERY_FACTOR_MIN = 1.25 - - self._FACE_THRESHOLD = 0.7 - self._EYE_THRESHOLD = 0.65 - self._SG_THRESHOLD = 0.9 - self._BLINK_THRESHOLD = 0.865 - self._PHONE_THRESH = 0.5 - self._POSE_PITCH_THRESHOLD = 0.3133 - self._POSE_PITCH_THRESHOLD_SLACK = 0.3237 - self._POSE_PITCH_THRESHOLD_STRICT = self._POSE_PITCH_THRESHOLD - self._POSE_YAW_THRESHOLD = 0.4020 - self._POSE_YAW_THRESHOLD_SLACK = 0.5042 - self._POSE_YAW_THRESHOLD_STRICT = self._POSE_YAW_THRESHOLD - self._POSE_YAW_MIN_STEER_DEG = 30 - self._POSE_YAW_STEER_FACTOR = 0.15 - self._POSE_YAW_STEER_MAX_OFFSET = 0.3927 - self._PITCH_NATURAL_OFFSET = 0.011 # initial value before offset is learned - self._PITCH_NATURAL_THRESHOLD = 0.449 - self._YAW_NATURAL_OFFSET = 0.075 # initial value before offset is learned - self._PITCH_NATURAL_VAR = 3*0.01 - self._YAW_NATURAL_VAR = 3*0.05 - self._PITCH_MAX_OFFSET = 0.124 - self._PITCH_MIN_OFFSET = -0.0881 - self._YAW_MAX_OFFSET = 0.289 - self._YAW_MIN_OFFSET = -0.0246 - - self._DCAM_UNCERTAIN_ALERT_THRESHOLD = 0.1 - self._DCAM_UNCERTAIN_ALERT_COUNT = int(60 / DT_DMON) - self._DCAM_UNCERTAIN_RESET_COUNT = int(2 / DT_DMON) - self._HI_STD_THRESHOLD = 0.3 - self._HI_STD_FALLBACK_TIME = int(10 / DT_DMON) # fall back to wheel touch if model is uncertain for 10s - self._DISTRACTED_FILTER_TS = 0.25 # 0.6Hz - - self._POSE_CALIB_MIN_SPEED = 13 # 30 mph - self._POSE_OFFSET_MIN_COUNT = int(60 / DT_DMON) # valid data counts before calibration completes, 1min cumulative - self._POSE_OFFSET_MAX_COUNT = int(360 / DT_DMON) # stop deweighting new data after 6 min, aka "short term memory" - self._WHEELPOS_CALIB_MIN_SPEED = 11 - self._WHEELPOS_THRESHOLD = 0.5 - self._WHEELPOS_FILTER_MIN_COUNT = int(15 / DT_DMON) # allow 15 seconds to converge wheel side - self._WHEELPOS_DATA_AVG = 0.03 - self._WHEELPOS_DATA_VAR = 3*5.5e-5 - self._WHEELPOS_MAX_COUNT = -1 - -class DriverPose: - def __init__(self, settings): - pitch_filter_raw_priors = (settings._PITCH_NATURAL_OFFSET, settings._PITCH_NATURAL_VAR, 2) - yaw_filter_raw_priors = (settings._YAW_NATURAL_OFFSET, settings._YAW_NATURAL_VAR, 2) - self.yaw = 0. - self.pitch = 0. - self.pitch_offsetter = RunningStatFilter(raw_priors=pitch_filter_raw_priors, max_trackable=settings._POSE_OFFSET_MAX_COUNT) - self.yaw_offsetter = RunningStatFilter(raw_priors=yaw_filter_raw_priors, max_trackable=settings._POSE_OFFSET_MAX_COUNT) - self.calibrated = False - self.low_std = True - self.cfactor_pitch = 1. - self.cfactor_yaw = 1. - self.steer_yaw_offset = 0. - -class DriverBlink: - def __init__(self): - self.left = 0. - self.right = 0. - -# model output refers to center of undistorted+leveled image -ref_undistorted_cam = DEVICE_CAMERAS[("tici", "ar0231")].dcam -dcam_undistorted_FL = 598.0 -dcam_undistorted_W, dcam_undistorted_H = (ref_undistorted_cam.width, ref_undistorted_cam.height) - -def face_orientation_from_model(orient_model, pos_model, rpy_calib): - pitch_model = orient_model[0] - yaw_model = orient_model[1] - - face_pixel_position = ((pos_model[0]+0.5)*dcam_undistorted_W, (pos_model[1]+0.5)*dcam_undistorted_H) - yaw_focal_angle = atan2(face_pixel_position[0] - dcam_undistorted_W//2, dcam_undistorted_FL) - pitch_focal_angle = atan2(face_pixel_position[1] - dcam_undistorted_H//2, dcam_undistorted_FL) - - pitch = pitch_model + pitch_focal_angle - yaw = -yaw_model + yaw_focal_angle - - pitch -= rpy_calib[1] - yaw -= rpy_calib[2] - return pitch, yaw - - -class DriverMonitoring: - def __init__(self, rhd_saved=False, settings=None, always_on=False): - # init policy settings - self.settings = settings if settings is not None else DRIVER_MONITOR_SETTINGS() - - # init driver status - wheelpos_filter_raw_priors = (self.settings._WHEELPOS_DATA_AVG, self.settings._WHEELPOS_DATA_VAR, 2) - self.wheelpos_offsetter = RunningStatFilter(raw_priors=wheelpos_filter_raw_priors, max_trackable=self.settings._WHEELPOS_MAX_COUNT) - self.pose = DriverPose(settings=self.settings) - self.blink = DriverBlink() - self.phone_prob = 0. - - self.alert_level = AlertLevel.none - self.always_on = always_on - self.distracted_types = defaultdict(bool) - self.driver_distracted = False - self.driver_distraction_filter = FirstOrderFilter(0., self.settings._DISTRACTED_FILTER_TS, DT_DMON) - self.wheel_on_right = False - self.wheel_on_right_last = None - self.wheel_on_right_default = rhd_saved - self.face_detected = False - self.alert_3_cnt = 0 - self.cnt_since_alert_3 = 0 - self.no_response_timeout = int(self.settings._NO_RESPONSE_TIMEOUT / DT_DMON) - self.no_response_cnt = 0 - self.lockout_active = Params().get_bool("DriverTooDistracted") - self.lockout_count = Params().get("DriverLockoutCount") or 0 - self.lockout_duration = self.settings._LOCKOUT_TIMES[min(max(self.lockout_count - 1, 0), len(self.settings._LOCKOUT_TIMES) - 1)] - self.lockout_time_elapsed = 0 - self.step_change = 0. - self.active_policy = MonitoringPolicy.vision - self.driver_interacting = False - self.is_model_uncertain = False - self.hi_stds = 0 - self.model_std_max = 0. - self.threshold_alert_1 = 0. - self.threshold_alert_2 = 0. - self.dcam_uncertain_cnt = 0 - self.dcam_reset_cnt = 0 - - self._reset_awareness() - self._set_policy(MonitoringPolicy.vision) - - def _reset_awareness(self): - self.awareness = 1. - self.last_vision_awareness = 1. - self.last_wheeltouch_awareness = 1. - - def _set_policy(self, target_policy): - if self.active_policy == MonitoringPolicy.vision and self.awareness <= self.threshold_alert_2: - if target_policy == MonitoringPolicy.vision: - self.step_change = DT_DMON / self.settings._VISION_POLICY_ALERT_3_TIMEOUT - else: - self.step_change = 0. - return # no exploit after orange alert - elif self.awareness <= 0.: - return - - if target_policy == MonitoringPolicy.vision: - # when falling back from passive mode to active mode, reset awareness to avoid false alert - if self.active_policy != MonitoringPolicy.vision: - self.last_wheeltouch_awareness = self.awareness - self.awareness = self.last_vision_awareness - - self.threshold_alert_1 = 1. - self.settings._VISION_POLICY_ALERT_1_TIMEOUT / self.settings._VISION_POLICY_ALERT_3_TIMEOUT - self.threshold_alert_2 = 1. - self.settings._VISION_POLICY_ALERT_2_TIMEOUT / self.settings._VISION_POLICY_ALERT_3_TIMEOUT - self.step_change = DT_DMON / self.settings._VISION_POLICY_ALERT_3_TIMEOUT - self.active_policy = MonitoringPolicy.vision - else: - if self.active_policy == MonitoringPolicy.vision: - self.last_vision_awareness = self.awareness - self.awareness = self.last_wheeltouch_awareness - - self.threshold_alert_1 = 1. - self.settings._WHEELTOUCH_POLICY_ALERT_1_TIMEOUT / self.settings._WHEELTOUCH_POLICY_ALERT_3_TIMEOUT - self.threshold_alert_2 = 1. - self.settings._WHEELTOUCH_POLICY_ALERT_2_TIMEOUT / self.settings._WHEELTOUCH_POLICY_ALERT_3_TIMEOUT - self.step_change = DT_DMON / self.settings._WHEELTOUCH_POLICY_ALERT_3_TIMEOUT - self.active_policy = MonitoringPolicy.wheeltouch - - def _set_pose_strictness(self, brake_disengage_prob, car_speed): - bp = brake_disengage_prob - k1 = max(-0.00156*((car_speed-16)**2)+0.6, 0.2) - bp_normal = max(min(bp / k1, 0.5),0) - self.pose.cfactor_pitch = np.interp(bp_normal, [0, 0.5], - [self.settings._POSE_PITCH_THRESHOLD_SLACK, - self.settings._POSE_PITCH_THRESHOLD_STRICT]) / self.settings._POSE_PITCH_THRESHOLD - self.pose.cfactor_yaw = np.interp(bp_normal, [0, 0.5], - [self.settings._POSE_YAW_THRESHOLD_SLACK, - self.settings._POSE_YAW_THRESHOLD_STRICT]) / self.settings._POSE_YAW_THRESHOLD - - def _get_distracted_types(self): - self.distracted_types = defaultdict(bool) - - if not self.pose.calibrated: - pitch_error = self.pose.pitch - self.settings._PITCH_NATURAL_OFFSET - yaw_error = self.pose.yaw - self.settings._YAW_NATURAL_OFFSET - else: - pitch_error = self.pose.pitch - min(max(self.pose.pitch_offsetter.filtered_stat.mean(), - self.settings._PITCH_MIN_OFFSET), self.settings._PITCH_MAX_OFFSET) - yaw_error = self.pose.yaw - min(max(self.pose.yaw_offsetter.filtered_stat.mean(), - self.settings._YAW_MIN_OFFSET), self.settings._YAW_MAX_OFFSET) - pitch_error = 0 if pitch_error > 0 else abs(pitch_error) # no positive pitch limit - - if yaw_error * self.pose.steer_yaw_offset > 0: # unidirectional - yaw_error = max(abs(yaw_error) - min(abs(self.pose.steer_yaw_offset), self.settings._POSE_YAW_STEER_MAX_OFFSET), 0.) - else: - yaw_error = abs(yaw_error) - - pitch_threshold = self.settings._POSE_PITCH_THRESHOLD * self.pose.cfactor_pitch if self.pose.calibrated else self.settings._PITCH_NATURAL_THRESHOLD - yaw_threshold = self.settings._POSE_YAW_THRESHOLD * self.pose.cfactor_yaw - - self.distracted_types['pose'] = bool((pitch_error > pitch_threshold) or (yaw_error > yaw_threshold)) - self.distracted_types['eye'] = bool((self.blink.left + self.blink.right)*0.5 > self.settings._BLINK_THRESHOLD) - self.distracted_types['phone'] = bool(self.phone_prob > self.settings._PHONE_THRESH) - - def _update_states(self, driver_state, cal_rpy, car_speed, op_engaged, lowspeed, demo_mode=False, steering_angle_deg=0.): - rhd_pred = driver_state.wheelOnRightProb - # calibrates only when there's movement and either face detected - if car_speed > self.settings._WHEELPOS_CALIB_MIN_SPEED and (driver_state.leftDriverData.faceProb > self.settings._FACE_THRESHOLD or - driver_state.rightDriverData.faceProb > self.settings._FACE_THRESHOLD): - self.wheelpos_offsetter.push_and_update(rhd_pred) - - wheelpos_calibrated = self.wheelpos_offsetter.filtered_stat.n >= self.settings._WHEELPOS_FILTER_MIN_COUNT - - if wheelpos_calibrated or demo_mode: - self.wheel_on_right = self.wheelpos_offsetter.filtered_stat.M > self.settings._WHEELPOS_THRESHOLD - else: - self.wheel_on_right = self.wheel_on_right_default # use default/saved if calibration is unfinished - # make sure no switching when engaged - if op_engaged and self.wheel_on_right_last is not None and self.wheel_on_right_last != self.wheel_on_right and not demo_mode: - self.wheel_on_right = self.wheel_on_right_last - driver_data = driver_state.rightDriverData if self.wheel_on_right else driver_state.leftDriverData - if not all(len(x) > 0 for x in (driver_data.faceOrientation, driver_data.facePosition, - driver_data.faceOrientationStd, driver_data.facePositionStd)): - return - - self.face_detected = driver_data.faceProb > self.settings._FACE_THRESHOLD - self.pose.pitch, self.pose.yaw = face_orientation_from_model(driver_data.faceOrientation, driver_data.facePosition, cal_rpy) - steer_d = max(abs(steering_angle_deg) - self.settings._POSE_YAW_MIN_STEER_DEG, 0.) - self.pose.steer_yaw_offset = radians(steer_d) * -np.sign(steering_angle_deg) * self.settings._POSE_YAW_STEER_FACTOR - if self.wheel_on_right: - self.pose.yaw *= -1 - self.pose.steer_yaw_offset *= -1 - self.wheel_on_right_last = self.wheel_on_right - self.model_std_max = max(driver_data.faceOrientationStd[0], driver_data.faceOrientationStd[1]) - self.pose.low_std = self.model_std_max < self.settings._HI_STD_THRESHOLD - self.blink.left = driver_data.leftBlinkProb * (driver_data.leftEyeProb > self.settings._EYE_THRESHOLD) \ - * (driver_data.sunglassesProb < self.settings._SG_THRESHOLD) - self.blink.right = driver_data.rightBlinkProb * (driver_data.rightEyeProb > self.settings._EYE_THRESHOLD) \ - * (driver_data.sunglassesProb < self.settings._SG_THRESHOLD) - self.phone_prob = driver_data.phoneProb - - self._get_distracted_types() - self.driver_distracted = any(self.distracted_types.values()) and driver_data.faceProb > self.settings._FACE_THRESHOLD and self.pose.low_std - self.driver_distraction_filter.update(self.driver_distracted) - - # only update offsetter when driver is actively driving the car above a certain speed - if self.face_detected and car_speed > self.settings._POSE_CALIB_MIN_SPEED and self.pose.low_std and (not op_engaged or not self.driver_distracted): - self.pose.pitch_offsetter.push_and_update(self.pose.pitch) - self.pose.yaw_offsetter.push_and_update(self.pose.yaw) - - self.pose.calibrated = self.pose.pitch_offsetter.filtered_stat.n >= self.settings._POSE_OFFSET_MIN_COUNT and \ - self.pose.yaw_offsetter.filtered_stat.n >= self.settings._POSE_OFFSET_MIN_COUNT - - if self.face_detected and not self.driver_distracted: - dcam_uncertain = self.model_std_max > self.settings._DCAM_UNCERTAIN_ALERT_THRESHOLD - if dcam_uncertain and not lowspeed: - self.dcam_uncertain_cnt += 1 - self.dcam_reset_cnt = 0 - else: - self.dcam_reset_cnt += 1 - if self.dcam_reset_cnt > self.settings._DCAM_UNCERTAIN_RESET_COUNT: - self.dcam_uncertain_cnt = 0 - - self.is_model_uncertain = self.hi_stds >= self.settings._HI_STD_FALLBACK_TIME - self._set_policy(MonitoringPolicy.vision if self.face_detected and not self.is_model_uncertain else MonitoringPolicy.wheeltouch) - if self.face_detected and not self.pose.low_std and not self.driver_distracted: - self.hi_stds += 1 - elif self.face_detected and self.pose.low_std: - self.hi_stds = 0 - - def _update_events(self, driver_engaged, op_engaged, lowspeed, wrong_gear): - self.alert_level = AlertLevel.none - self.driver_interacting = driver_engaged - - if self.alert_3_cnt >= self.settings._MAX_ALERT_3 or self.no_response_cnt >= self.settings._MAX_NO_RESPONSE: - if not self.lockout_active: - self.lockout_count += 1 - self.lockout_duration = self.settings._LOCKOUT_TIMES[min(self.lockout_count - 1, len(self.settings._LOCKOUT_TIMES) - 1)] - Params().put("DriverLockoutCount", self.lockout_count) - self.lockout_active = True - - if self.lockout_active: - self.lockout_time_elapsed += 1 - if self.lockout_time_elapsed > self.lockout_duration: - self.lockout_active = False - self.alert_3_cnt = 0 - self.cnt_since_alert_3 = 0 - self.no_response_cnt = 0 - self.lockout_time_elapsed = 0 - - always_on_valid = self.always_on and not wrong_gear - if (self.driver_interacting and self.awareness > 0 and self.active_policy == MonitoringPolicy.wheeltouch) or \ - (not always_on_valid and not op_engaged) or \ - (always_on_valid and not op_engaged and self.awareness <= 0): - # always reset on disengage with normal mode; disengage resets only on red if always on - self._reset_awareness() - return - - awareness_prev = self.awareness - _reaching_alert_1 = self.awareness - self.step_change <= self.threshold_alert_1 - _reaching_alert_3 = self.awareness - self.step_change <= 0 - lowspeed_exemption = lowspeed and _reaching_alert_1 - always_on_exemption = always_on_valid and not op_engaged and _reaching_alert_3 - - if self.awareness > 0 and \ - ((self.driver_distraction_filter.x < 0.37 and self.face_detected and self.pose.low_std) or lowspeed_exemption): - if self.driver_interacting: - self._reset_awareness() - return - # only restore awareness when paying attention and alert is not red - self.awareness = min(self.awareness + ((self.settings._TIMEOUT_RECOVERY_FACTOR_MAX-self.settings._TIMEOUT_RECOVERY_FACTOR_MIN)* - (1.-self.awareness)+self.settings._TIMEOUT_RECOVERY_FACTOR_MIN)*self.step_change, 1.) - if self.awareness == 1.: - self.last_wheeltouch_awareness = min(self.last_wheeltouch_awareness + self.step_change, 1.) - # don't display alert banner when awareness is recovering and has cleared orange - if self.awareness > self.threshold_alert_2: - return - - certainly_distracted = self.driver_distraction_filter.x > 0.63 and self.driver_distracted and self.face_detected - maybe_distracted = self.is_model_uncertain or not self.face_detected - - if certainly_distracted or maybe_distracted: - # should always be counting if distracted unless at low speed and reaching green - # also will not be reaching 0 if DM is active when not engaged - if not (lowspeed_exemption or always_on_exemption): - self.awareness = max(self.awareness - self.step_change, -0.1) - - if self.awareness <= 0.: - # terminal alert: disengagement required - self.alert_level = AlertLevel.three - if awareness_prev > 0.: - self.alert_3_cnt += 1 - self.cnt_since_alert_3 = 0 - else: - self.cnt_since_alert_3 += 1 - if self.cnt_since_alert_3 == self.no_response_timeout: - self.no_response_cnt += 1 - else: - if self.awareness <= self.threshold_alert_2: - self.alert_level = AlertLevel.two - elif self.awareness <= self.threshold_alert_1: - self.alert_level = AlertLevel.one - - def get_state_packet(self, valid=True): - # build driverMonitoringState packet - dat = messaging.new_message('driverMonitoringState', valid=valid) - dm = dat.driverMonitoringState - - dm.lockout = self.lockout_active - dm.lockoutCount = self.lockout_count - if self.lockout_active: - dm.lockoutMinutesRemaining = max(1, round((self.lockout_duration - self.lockout_time_elapsed) * DT_DMON / 60.)) - dm.alert3Count = self.alert_3_cnt - dm.noResponseCount = self.no_response_cnt - dm.noResponseForceDecel = self.alert_level == AlertLevel.three and self.cnt_since_alert_3 >= self.no_response_timeout - dm.alwaysOn = self.always_on - dm.alwaysOnLockout = self.always_on and self.awareness <= self.threshold_alert_2 - dm.alertLevel = self.alert_level - dm.activePolicy = self.active_policy - dm.isRHD = self.wheel_on_right - dm.rhdCalibration.calibratedPercent = to_percent(self.wheelpos_offsetter.filtered_stat.n / self.settings._WHEELPOS_FILTER_MIN_COUNT) - dm.rhdCalibration.offset = self.wheelpos_offsetter.filtered_stat.M - - dm.visionPolicyState.awarenessPercent = to_percent(self.last_vision_awareness if self.active_policy != MonitoringPolicy.vision else self.awareness) - dm.visionPolicyState.awarenessStep = self.step_change if self.active_policy == MonitoringPolicy.vision else 0. - dm.visionPolicyState.isDistracted = self.driver_distracted - dm.visionPolicyState.distractedTypes.pose = self.distracted_types['pose'] - dm.visionPolicyState.distractedTypes.eye = self.distracted_types['eye'] - dm.visionPolicyState.distractedTypes.phone = self.distracted_types['phone'] - dm.visionPolicyState.faceDetected = self.face_detected - dm.visionPolicyState.pose.pitch = self.pose.pitch - dm.visionPolicyState.pose.yaw = self.pose.yaw - dm.visionPolicyState.pose.calibrated = self.pose.calibrated - dm.visionPolicyState.pose.pitchCalib.calibratedPercent = to_percent(self.pose.pitch_offsetter.filtered_stat.n / self.settings._POSE_OFFSET_MIN_COUNT) - dm.visionPolicyState.pose.pitchCalib.offset = self.pose.pitch_offsetter.filtered_stat.M - dm.visionPolicyState.pose.yawCalib.calibratedPercent = to_percent(self.pose.yaw_offsetter.filtered_stat.n / self.settings._POSE_OFFSET_MIN_COUNT) - dm.visionPolicyState.pose.yawCalib.offset = self.pose.yaw_offsetter.filtered_stat.M - dm.visionPolicyState.pose.uncertainty = self.model_std_max - dm.visionPolicyState.wheeltouchFallbackPercent = to_percent(self.hi_stds / self.settings._HI_STD_FALLBACK_TIME) - dm.visionPolicyState.uncertainOffroadAlertPercent = to_percent(self.dcam_uncertain_cnt / self.settings._DCAM_UNCERTAIN_ALERT_COUNT) - - dm.wheeltouchPolicyState.awarenessPercent = to_percent(self.last_wheeltouch_awareness if self.active_policy == MonitoringPolicy.vision else self.awareness) - dm.wheeltouchPolicyState.awarenessStep = 0. if self.active_policy == MonitoringPolicy.vision else self.step_change - dm.wheeltouchPolicyState.driverInteracting = self.driver_interacting - return dat - - def run_step(self, sm, demo=False): - if demo: - car_speed = 30 - enabled = True - wrong_gear = False - lowspeed = False - driver_engaged = False - brake_disengage_prob = 1.0 - steering_angle_deg = 0.0 - rpyCalib = [0., 0., 0.] - else: - car_speed = sm['carState'].vEgo - enabled = sm['selfdriveState'].enabled - wrong_gear = sm['carState'].gearShifter not in (car.CarState.GearShifter.drive, car.CarState.GearShifter.low) - lowspeed = car_speed < self.settings._ALERT_MIN_SPEED - driver_engaged = sm['carState'].steeringPressed or sm['carState'].gasPressed - brake_disengage_prob = sm['modelV2'].meta.disengagePredictions.brakeDisengageProbs[0] # brake disengage prob in next 2s - steering_angle_deg = sm['carState'].steeringAngleDeg - rpyCalib = sm['liveCalibration'].rpyCalib - - self._set_pose_strictness( - brake_disengage_prob=brake_disengage_prob, - car_speed=car_speed, - ) - - # Parse data from dmonitoringmodeld - self._update_states( - driver_state=sm['driverStateV2'], - cal_rpy=rpyCalib, - car_speed=car_speed, - op_engaged=enabled, - lowspeed=lowspeed, - demo_mode=demo, - steering_angle_deg=steering_angle_deg, - ) - - # Update distraction events - self._update_events( - driver_engaged=driver_engaged, - op_engaged=enabled, - lowspeed=lowspeed, - wrong_gear=wrong_gear, - ) diff --git a/selfdrive/monitoring/test_dmonitoringd.py b/selfdrive/monitoring/test_dmonitoringd.py deleted file mode 100644 index 750489d..0000000 --- a/selfdrive/monitoring/test_dmonitoringd.py +++ /dev/null @@ -1,61 +0,0 @@ -from types import SimpleNamespace - -import pytest - -from cereal import log -from openpilot.selfdrive.monitoring.dmonitoringd import create_driver_monitoring, get_dm_inputs, use_legacy_dm -from openpilot.selfdrive.monitoring.legacy_policy import DriverMonitoring as LegacyDriverMonitoring -from openpilot.selfdrive.monitoring.policy import DriverMonitoring as UpstreamDriverMonitoring - - -@pytest.mark.parametrize("device_type, expected_type", [ - ("mici", LegacyDriverMonitoring), - ("tici", UpstreamDriverMonitoring), - ("tizi", UpstreamDriverMonitoring), -]) -def test_policy_selection(device_type, expected_type): - assert use_legacy_dm(device_type) == (device_type == "mici") - assert isinstance(create_driver_monitoring(device_type, False, False), expected_type) - - -def test_mici_uses_legacy_thresholds(): - dm = create_driver_monitoring("mici", False, False) - - assert dm.settings._DISTRACTED_TIME == 11. - assert dm.settings._AWARENESS_TIME == 30. - assert dm.settings._PHONE_THRESH == 0.75 - - -@pytest.mark.parametrize("enabled, lat_active, expected", [ - (False, False, False), - (True, False, True), - (False, True, True), - (True, True, True), -]) -def test_iq_enabled_adapter(enabled, lat_active, expected): - sm = { - 'driverStateV2': object(), - 'liveCalibration': object(), - 'carState': object(), - 'selfdriveState': SimpleNamespace(enabled=enabled), - 'modelV2': object(), - 'carControl': SimpleNamespace(latActive=lat_active), - } - - assert get_dm_inputs(sm)['selfdriveState'].enabled == expected - - -def test_legacy_policy_packet_uses_shared_schema(): - dm = create_driver_monitoring("mici", False, False) - dm.awareness = 0. - dm.terminal_alert_cnt = 2 - dm.terminal_time = dm.settings._MAX_TERMINAL_DURATION - dm.too_distracted = True - - state = dm.get_state_packet().driverMonitoringState - - assert state.lockout - assert state.alert3Count == 2 - assert state.noResponseCount == 1 - assert state.noResponseForceDecel - assert state.alertLevel == log.DriverMonitoringState.AlertLevel.three diff --git a/selfdrive/monitoring/test_monitoring.py b/selfdrive/monitoring/test_monitoring.py index 37606fe..fef6a7e 100644 --- a/selfdrive/monitoring/test_monitoring.py +++ b/selfdrive/monitoring/test_monitoring.py @@ -1,15 +1,19 @@ -from cereal import log +import numpy as np +import pytest + +from cereal import log, car from openpilot.common.realtime import DT_DMON -from openpilot.selfdrive.monitoring.policy import DriverMonitoring, DRIVER_MONITOR_SETTINGS +from openpilot.selfdrive.monitoring.helpers import DriverMonitoring, DRIVER_MONITOR_SETTINGS +from openpilot.system.hardware import HARDWARE EventName = log.OnroadEvent.EventName -dm_settings = DRIVER_MONITOR_SETTINGS() +dm_settings = DRIVER_MONITOR_SETTINGS(device_type=HARDWARE.get_device_type()) TEST_TIMESPAN = 120 # seconds -DISTRACTED_SECONDS_TO_ORANGE = dm_settings._VISION_POLICY_ALERT_2_TIMEOUT + 1 -DISTRACTED_SECONDS_TO_RED = dm_settings._VISION_POLICY_ALERT_3_TIMEOUT + 1 -INVISIBLE_SECONDS_TO_ORANGE = dm_settings._WHEELTOUCH_POLICY_ALERT_2_TIMEOUT + 1 -INVISIBLE_SECONDS_TO_RED = dm_settings._WHEELTOUCH_POLICY_ALERT_3_TIMEOUT + 1 +DISTRACTED_SECONDS_TO_ORANGE = dm_settings._DISTRACTED_TIME - dm_settings._DISTRACTED_PROMPT_TIME_TILL_TERMINAL + 1 +DISTRACTED_SECONDS_TO_RED = dm_settings._DISTRACTED_TIME + 1 +INVISIBLE_SECONDS_TO_ORANGE = dm_settings._AWARENESS_TIME - dm_settings._AWARENESS_PROMPT_TIME_TILL_TERMINAL + 1 +INVISIBLE_SECONDS_TO_RED = dm_settings._AWARENESS_TIME + 1 def make_msg(face_detected, distracted=False, model_uncertain=False): ds = log.DriverStateV2.new_message() @@ -33,7 +37,7 @@ msg_ATTENTIVE = make_msg(True) msg_DISTRACTED = make_msg(True, distracted=True) msg_ATTENTIVE_UNCERTAIN = make_msg(True, model_uncertain=True) msg_DISTRACTED_UNCERTAIN = make_msg(True, distracted=True, model_uncertain=True) -msg_DISTRACTED_BUT_SOMEHOW_UNCERTAIN = make_msg(True, distracted=True, model_uncertain=dm_settings._HI_STD_THRESHOLD*1.5) +msg_DISTRACTED_BUT_SOMEHOW_UNCERTAIN = make_msg(True, distracted=True, model_uncertain=dm_settings._POSESTD_THRESHOLD*1.5) # driver interaction with car car_interaction_DETECTED = True @@ -47,66 +51,51 @@ always_true = [True] * int(TEST_TIMESPAN / DT_DMON) always_false = [False] * int(TEST_TIMESPAN / DT_DMON) class TestMonitoring: - def _run_seq(self, msgs, interaction, engaged, lowspeed): + def _run_seq(self, msgs, interaction, engaged, standstill): DM = DriverMonitoring() - alert_lvls = [] + events = [] for idx in range(len(msgs)): - DM._update_states(msgs[idx], [0, 0, 0], 0, engaged[idx], lowspeed[idx]) + DM._update_states(msgs[idx], [0, 0, 0], 0, engaged[idx], standstill[idx]) # cal_rpy and car_speed don't matter here # evaluate events at 10Hz for tests - DM._update_events(interaction[idx], engaged[idx], lowspeed[idx], 0) - alert_lvls.append(DM.alert_level) - assert len(alert_lvls) == len(msgs), f"got {len(alert_lvls)} for {len(msgs)} driverState input msgs" - return alert_lvls, DM + DM._update_events(interaction[idx], engaged[idx], standstill[idx], 0, 0) + events.append(DM.current_events) + assert len(events) == len(msgs), f"got {len(events)} for {len(msgs)} driverState input msgs" + return events, DM + def _assert_no_events(self, events): + assert all(not len(e) for e in events) # engaged, driver is attentive all the time def test_fully_aware_driver(self): - alert_lvls, d_status = self._run_seq(always_attentive, always_false, always_true, always_false) - assert all(a == 0 for a in alert_lvls) - assert d_status.active_policy == log.DriverMonitoringState.MonitoringPolicy.vision + events, _ = self._run_seq(always_attentive, always_false, always_true, always_false) + self._assert_no_events(events) # engaged, driver is distracted and does nothing def test_fully_distracted_driver(self): - alert_lvls, d_status = self._run_seq(always_distracted, always_false, always_true, always_false) - s = d_status.settings - assert alert_lvls[int(s._VISION_POLICY_ALERT_1_TIMEOUT / 2 / DT_DMON)] == 0 - assert alert_lvls[int((s._VISION_POLICY_ALERT_1_TIMEOUT + \ - (s._VISION_POLICY_ALERT_2_TIMEOUT - s._VISION_POLICY_ALERT_1_TIMEOUT) / 2) / DT_DMON)] == 1 - assert alert_lvls[int((s._VISION_POLICY_ALERT_2_TIMEOUT + \ - (s._VISION_POLICY_ALERT_3_TIMEOUT - s._VISION_POLICY_ALERT_2_TIMEOUT) / 2) / DT_DMON)] == 2 - assert alert_lvls[int((s._VISION_POLICY_ALERT_3_TIMEOUT + \ - (TEST_TIMESPAN - 10 - s._VISION_POLICY_ALERT_3_TIMEOUT) / 2) / DT_DMON)] == 3 + events, d_status = self._run_seq(always_distracted, always_false, always_true, always_false) + assert len(events[int((d_status.settings._DISTRACTED_TIME-d_status.settings._DISTRACTED_PRE_TIME_TILL_TERMINAL)/2/DT_DMON)]) == 0 + assert events[int((d_status.settings._DISTRACTED_TIME-d_status.settings._DISTRACTED_PRE_TIME_TILL_TERMINAL + \ + ((d_status.settings._DISTRACTED_PRE_TIME_TILL_TERMINAL-d_status.settings._DISTRACTED_PROMPT_TIME_TILL_TERMINAL)/2))/DT_DMON)].names[0] == \ + EventName.preDriverDistracted + assert events[int((d_status.settings._DISTRACTED_TIME-d_status.settings._DISTRACTED_PROMPT_TIME_TILL_TERMINAL + \ + ((d_status.settings._DISTRACTED_PROMPT_TIME_TILL_TERMINAL)/2))/DT_DMON)].names[0] == EventName.promptDriverDistracted + assert events[int((d_status.settings._DISTRACTED_TIME + \ + ((TEST_TIMESPAN-10-d_status.settings._DISTRACTED_TIME)/2))/DT_DMON)].names[0] == EventName.driverDistracted assert isinstance(d_status.awareness, float) - # engaged, distracted past red and beyond the no-response window -> unavailability response + lockout - def test_distracted_lockout(self): - alert_lvls, d_status = self._run_seq(always_distracted, always_false, always_true, always_false) - assert alert_lvls[int(DISTRACTED_SECONDS_TO_RED / DT_DMON)] == 3 - assert d_status.lockout_active - assert d_status.lockout_time_elapsed > 0 - assert d_status.lockout_count >= 1 - - # no face -> wheeltouch red, sustained past the no-response timeout -> unavailability response + lockout - def test_invisible_lockout(self): - _, d_status = self._run_seq(always_no_face, always_false, always_true, always_false) - assert d_status.active_policy == log.DriverMonitoringState.MonitoringPolicy.wheeltouch - assert d_status.lockout_active - assert d_status.lockout_count >= 1 - # engaged, no face detected the whole time, no action def test_fully_invisible_driver(self): - alert_lvls, d_status = self._run_seq(always_no_face, always_false, always_true, always_false) - s = d_status.settings - assert alert_lvls[int(s._WHEELTOUCH_POLICY_ALERT_1_TIMEOUT / 2 / DT_DMON)] == 0 - assert alert_lvls[int((s._WHEELTOUCH_POLICY_ALERT_1_TIMEOUT + \ - (s._WHEELTOUCH_POLICY_ALERT_2_TIMEOUT - s._WHEELTOUCH_POLICY_ALERT_1_TIMEOUT) / 2) / DT_DMON)] == 1 - assert alert_lvls[int((s._WHEELTOUCH_POLICY_ALERT_2_TIMEOUT + \ - (s._WHEELTOUCH_POLICY_ALERT_3_TIMEOUT - s._WHEELTOUCH_POLICY_ALERT_2_TIMEOUT) / 2) / DT_DMON)] == 2 - assert alert_lvls[int((s._WHEELTOUCH_POLICY_ALERT_3_TIMEOUT + \ - (TEST_TIMESPAN - 10 - s._WHEELTOUCH_POLICY_ALERT_3_TIMEOUT) / 2) / DT_DMON)] == 3 - assert d_status.active_policy == log.DriverMonitoringState.MonitoringPolicy.wheeltouch + events, d_status = self._run_seq(always_no_face, always_false, always_true, always_false) + assert len(events[int((d_status.settings._AWARENESS_TIME-d_status.settings._AWARENESS_PRE_TIME_TILL_TERMINAL)/2/DT_DMON)]) == 0 + assert events[int((d_status.settings._AWARENESS_TIME-d_status.settings._AWARENESS_PRE_TIME_TILL_TERMINAL + \ + ((d_status.settings._AWARENESS_PRE_TIME_TILL_TERMINAL-d_status.settings._AWARENESS_PROMPT_TIME_TILL_TERMINAL)/2))/DT_DMON)].names[0] == \ + EventName.preDriverUnresponsive + assert events[int((d_status.settings._AWARENESS_TIME-d_status.settings._AWARENESS_PROMPT_TIME_TILL_TERMINAL + \ + ((d_status.settings._AWARENESS_PROMPT_TIME_TILL_TERMINAL)/2))/DT_DMON)].names[0] == EventName.promptDriverUnresponsive + assert events[int((d_status.settings._AWARENESS_TIME + \ + ((TEST_TIMESPAN-10-d_status.settings._AWARENESS_TIME)/2))/DT_DMON)].names[0] == EventName.driverUnresponsive # engaged, down to orange, driver pays attention, back to normal; then down to orange, driver touches wheel # - should have short orange recovery time and no green afterwards; wheel touch only recovers when paying attention @@ -117,13 +106,13 @@ class TestMonitoring: [msg_ATTENTIVE] * (int(TEST_TIMESPAN/DT_DMON)-int((DISTRACTED_SECONDS_TO_ORANGE*3+2)/DT_DMON)) interaction_vector = [car_interaction_NOT_DETECTED] * int(DISTRACTED_SECONDS_TO_ORANGE*3/DT_DMON) + \ [car_interaction_DETECTED] * (int(TEST_TIMESPAN/DT_DMON)-int(DISTRACTED_SECONDS_TO_ORANGE*3/DT_DMON)) - alert_lvls, _ = self._run_seq(ds_vector, interaction_vector, always_true, always_false) - assert alert_lvls[int(DISTRACTED_SECONDS_TO_ORANGE*0.5/DT_DMON)] == 0 - assert alert_lvls[int((DISTRACTED_SECONDS_TO_ORANGE-0.1)/DT_DMON)] == 2 - assert alert_lvls[int(DISTRACTED_SECONDS_TO_ORANGE*1.5/DT_DMON)] == 0 - assert alert_lvls[int((DISTRACTED_SECONDS_TO_ORANGE*3-0.1)/DT_DMON)] == 2 - assert alert_lvls[int((DISTRACTED_SECONDS_TO_ORANGE*3+0.1)/DT_DMON)] == 2 - assert alert_lvls[int((DISTRACTED_SECONDS_TO_ORANGE*3+2.5)/DT_DMON)] == 0 + events, _ = self._run_seq(ds_vector, interaction_vector, always_true, always_false) + assert len(events[int(DISTRACTED_SECONDS_TO_ORANGE*0.5/DT_DMON)]) == 0 + assert events[int((DISTRACTED_SECONDS_TO_ORANGE-0.1)/DT_DMON)].names[0] == EventName.promptDriverDistracted + assert len(events[int(DISTRACTED_SECONDS_TO_ORANGE*1.5/DT_DMON)]) == 0 + assert events[int((DISTRACTED_SECONDS_TO_ORANGE*3-0.1)/DT_DMON)].names[0] == EventName.promptDriverDistracted + assert events[int((DISTRACTED_SECONDS_TO_ORANGE*3+0.1)/DT_DMON)].names[0] == EventName.promptDriverDistracted + assert len(events[int((DISTRACTED_SECONDS_TO_ORANGE*3+2.5)/DT_DMON)]) == 0 # engaged, down to orange, driver dodges camera, then comes back still distracted, down to red, \ # driver dodges, and then touches wheel to no avail, disengages and reengages @@ -141,31 +130,31 @@ class TestMonitoring: = [True] * int(1/DT_DMON) op_vector[int((DISTRACTED_SECONDS_TO_RED+2*_invisible_time+2.5)/DT_DMON):int((DISTRACTED_SECONDS_TO_RED+2*_invisible_time+3)/DT_DMON)] \ = [False] * int(0.5/DT_DMON) - alert_lvls, _ = self._run_seq(ds_vector, interaction_vector, op_vector, always_false) - assert alert_lvls[int((DISTRACTED_SECONDS_TO_ORANGE+0.5*_invisible_time)/DT_DMON)] == 2 - assert alert_lvls[int((DISTRACTED_SECONDS_TO_RED+1.5*_invisible_time)/DT_DMON)] == 3 - assert alert_lvls[int((DISTRACTED_SECONDS_TO_RED+2*_invisible_time+1.5)/DT_DMON)] == 3 - assert alert_lvls[int((DISTRACTED_SECONDS_TO_RED+2*_invisible_time+3.5)/DT_DMON)] == 0 + events, _ = self._run_seq(ds_vector, interaction_vector, op_vector, always_false) + assert events[int((DISTRACTED_SECONDS_TO_ORANGE+0.5*_invisible_time)/DT_DMON)].names[0] == EventName.promptDriverDistracted + assert events[int((DISTRACTED_SECONDS_TO_RED+1.5*_invisible_time)/DT_DMON)].names[0] == EventName.driverDistracted + assert events[int((DISTRACTED_SECONDS_TO_RED+2*_invisible_time+1.5)/DT_DMON)].names[0] == EventName.driverDistracted + assert len(events[int((DISTRACTED_SECONDS_TO_RED+2*_invisible_time+3.5)/DT_DMON)]) == 0 # engaged, invisible driver, down to orange, driver touches wheel; then down to orange again, driver appears # - both actions should clear the alert, but momentary appearance should not def test_sometimes_transparent_commuter(self): - for _visible_time in (0.5, 10): - ds_vector = always_no_face[:]*2 - interaction_vector = always_false[:]*2 - ds_vector[int((2*INVISIBLE_SECONDS_TO_ORANGE+1)/DT_DMON):int((2*INVISIBLE_SECONDS_TO_ORANGE+1+_visible_time)/DT_DMON)] = \ - [msg_ATTENTIVE] * int(_visible_time/DT_DMON) - interaction_vector[int((INVISIBLE_SECONDS_TO_ORANGE)/DT_DMON):int((INVISIBLE_SECONDS_TO_ORANGE+1)/DT_DMON)] = [True] * int(1/DT_DMON) - alert_lvls, _ = self._run_seq(ds_vector, interaction_vector, 2*always_true, 2*always_false) - assert alert_lvls[int(dm_settings._WHEELTOUCH_POLICY_ALERT_1_TIMEOUT/2/DT_DMON)] == 0 - assert alert_lvls[int((INVISIBLE_SECONDS_TO_ORANGE-0.1)/DT_DMON)] == 2 - assert alert_lvls[int((INVISIBLE_SECONDS_TO_ORANGE+0.1)/DT_DMON)] == 0 - if _visible_time == 0.5: - assert alert_lvls[int((INVISIBLE_SECONDS_TO_ORANGE*2+1-0.1)/DT_DMON)] == 2 - assert alert_lvls[int((INVISIBLE_SECONDS_TO_ORANGE*2+1+0.1+_visible_time)/DT_DMON)] == 2 - elif _visible_time == 10: - assert alert_lvls[int((INVISIBLE_SECONDS_TO_ORANGE*2+1-0.1)/DT_DMON)] == 2 - assert alert_lvls[int((INVISIBLE_SECONDS_TO_ORANGE*2+1+0.1+_visible_time)/DT_DMON)] == 0 + _visible_time = np.random.choice([0.5, 10]) + ds_vector = always_no_face[:]*2 + interaction_vector = always_false[:]*2 + ds_vector[int((2*INVISIBLE_SECONDS_TO_ORANGE+1)/DT_DMON):int((2*INVISIBLE_SECONDS_TO_ORANGE+1+_visible_time)/DT_DMON)] = \ + [msg_ATTENTIVE] * int(_visible_time/DT_DMON) + interaction_vector[int((INVISIBLE_SECONDS_TO_ORANGE)/DT_DMON):int((INVISIBLE_SECONDS_TO_ORANGE+1)/DT_DMON)] = [True] * int(1/DT_DMON) + events, _ = self._run_seq(ds_vector, interaction_vector, 2*always_true, 2*always_false) + assert len(events[int(INVISIBLE_SECONDS_TO_ORANGE*0.5/DT_DMON)]) == 0 + assert events[int((INVISIBLE_SECONDS_TO_ORANGE-0.1)/DT_DMON)].names[0] == EventName.promptDriverUnresponsive + assert len(events[int((INVISIBLE_SECONDS_TO_ORANGE+0.1)/DT_DMON)]) == 0 + if _visible_time == 0.5: + assert events[int((INVISIBLE_SECONDS_TO_ORANGE*2+1-0.1)/DT_DMON)].names[0] == EventName.promptDriverUnresponsive + assert events[int((INVISIBLE_SECONDS_TO_ORANGE*2+1+0.1+_visible_time)/DT_DMON)].names[0] == EventName.preDriverUnresponsive + elif _visible_time == 10: + assert events[int((INVISIBLE_SECONDS_TO_ORANGE*2+1-0.1)/DT_DMON)].names[0] == EventName.promptDriverUnresponsive + assert len(events[int((INVISIBLE_SECONDS_TO_ORANGE*2+1+0.1+_visible_time)/DT_DMON)]) == 0 # engaged, invisible driver, down to red, driver appears and then touches wheel, then disengages/reengages # - only disengage will clear the alert @@ -177,51 +166,105 @@ class TestMonitoring: ds_vector[int(INVISIBLE_SECONDS_TO_RED/DT_DMON):int((INVISIBLE_SECONDS_TO_RED+_visible_time)/DT_DMON)] = [msg_ATTENTIVE] * int(_visible_time/DT_DMON) interaction_vector[int((INVISIBLE_SECONDS_TO_RED+_visible_time)/DT_DMON):int((INVISIBLE_SECONDS_TO_RED+_visible_time+1)/DT_DMON)] = [True] * int(1/DT_DMON) op_vector[int((INVISIBLE_SECONDS_TO_RED+_visible_time+1)/DT_DMON):int((INVISIBLE_SECONDS_TO_RED+_visible_time+0.5)/DT_DMON)] = [False] * int(0.5/DT_DMON) - alert_lvls, _ = self._run_seq(ds_vector, interaction_vector, op_vector, always_false) - assert alert_lvls[int(dm_settings._WHEELTOUCH_POLICY_ALERT_1_TIMEOUT/2/DT_DMON)] == 0 - assert alert_lvls[int((INVISIBLE_SECONDS_TO_ORANGE-0.1)/DT_DMON)] == 2 - assert alert_lvls[int((INVISIBLE_SECONDS_TO_RED-0.1)/DT_DMON)] == 3 - assert alert_lvls[int((INVISIBLE_SECONDS_TO_RED+0.5*_visible_time)/DT_DMON)] == 3 - assert alert_lvls[int((INVISIBLE_SECONDS_TO_RED+_visible_time+0.5)/DT_DMON)] == 3 - assert alert_lvls[int((INVISIBLE_SECONDS_TO_RED+_visible_time+1+0.1)/DT_DMON)] == 0 + events, _ = self._run_seq(ds_vector, interaction_vector, op_vector, always_false) + assert len(events[int(INVISIBLE_SECONDS_TO_ORANGE*0.5/DT_DMON)]) == 0 + assert events[int((INVISIBLE_SECONDS_TO_ORANGE-0.1)/DT_DMON)].names[0] == EventName.promptDriverUnresponsive + assert events[int((INVISIBLE_SECONDS_TO_RED-0.1)/DT_DMON)].names[0] == EventName.driverUnresponsive + assert events[int((INVISIBLE_SECONDS_TO_RED+0.5*_visible_time)/DT_DMON)].names[0] == EventName.driverUnresponsive + assert events[int((INVISIBLE_SECONDS_TO_RED+_visible_time+0.5)/DT_DMON)].names[0] == EventName.driverUnresponsive + assert len(events[int((INVISIBLE_SECONDS_TO_RED+_visible_time+1+0.1)/DT_DMON)]) == 0 # disengaged, always distracted driver # - dm should stay quiet when not engaged def test_pure_dashcam_user(self): - alert_lvls, _ = self._run_seq(always_distracted, always_false, always_false, always_false) - assert all(a == 0 for a in alert_lvls) + events, _ = self._run_seq(always_distracted, always_false, always_false, always_false) + assert sum(len(event) for event in events) == 0 # engaged, car stops at traffic light, down to orange, no action, then car starts moving # - should only reach green when stopped, but continues counting down on launch def test_long_traffic_light_victim(self): _redlight_time = 60 # seconds - lowspeed_vector = always_true[:] - lowspeed_vector[int(_redlight_time/DT_DMON):] = [False] * int((TEST_TIMESPAN-_redlight_time)/DT_DMON) - alert_lvls, d_status = self._run_seq(always_distracted, always_false, always_true, lowspeed_vector) - s = d_status.settings - assert alert_lvls[int((_redlight_time-0.1)/DT_DMON)] == 0 - _alert_1_to_2 = s._VISION_POLICY_ALERT_2_TIMEOUT - s._VISION_POLICY_ALERT_1_TIMEOUT - assert alert_lvls[int((_redlight_time+0.5)/DT_DMON)] == 1 - assert alert_lvls[int((_redlight_time+_alert_1_to_2+0.5)/DT_DMON)] == 2 - - # engaged, distracted while moving, then car stops after reaching orange - # - should reset timer to pre green at low speed - def test_distracted_then_stops(self): - _stop_time = DISTRACTED_SECONDS_TO_ORANGE + 1 # stop 1 second after reaching orange - lowspeed_vector = always_false[:] - lowspeed_vector[int(_stop_time/DT_DMON):] = [True] * int((TEST_TIMESPAN-_stop_time)/DT_DMON) - alert_lvls, _ = self._run_seq(always_distracted, always_false, always_true, lowspeed_vector) - # just before and briefly after stopping: orange alert; goes away quickly after stopped - assert alert_lvls[int((_stop_time+0.1)/DT_DMON)] == 2 - assert alert_lvls[int((_stop_time+0.5)/DT_DMON)] == 0 + standstill_vector = always_true[:] + standstill_vector[int(_redlight_time/DT_DMON):] = [False] * int((TEST_TIMESPAN-_redlight_time)/DT_DMON) + events, d_status = self._run_seq(always_distracted, always_false, always_true, standstill_vector) + assert events[int((d_status.settings._DISTRACTED_TIME-d_status.settings._DISTRACTED_PRE_TIME_TILL_TERMINAL+1)/DT_DMON)].names[0] == \ + EventName.preDriverDistracted + assert events[int((_redlight_time-0.1)/DT_DMON)].names[0] == EventName.preDriverDistracted + assert events[int((_redlight_time+0.5)/DT_DMON)].names[0] == EventName.promptDriverDistracted # engaged, model is somehow uncertain and driver is distracted # - should fall back to wheel touch after uncertain alert def test_somehow_indecisive_model(self): ds_vector = [msg_DISTRACTED_BUT_SOMEHOW_UNCERTAIN] * int(TEST_TIMESPAN/DT_DMON) interaction_vector = always_false[:] - alert_lvls, d_status = self._run_seq(ds_vector, interaction_vector, always_true, always_false) - s = d_status.settings - assert alert_lvls[int((INVISIBLE_SECONDS_TO_ORANGE-1+DT_DMON*s._HI_STD_FALLBACK_TIME-0.1)/DT_DMON)] == 1 - assert alert_lvls[int((INVISIBLE_SECONDS_TO_ORANGE-1+DT_DMON*s._HI_STD_FALLBACK_TIME+0.1)/DT_DMON)] == 2 - assert alert_lvls[int((INVISIBLE_SECONDS_TO_RED-1+DT_DMON*s._HI_STD_FALLBACK_TIME+0.1)/DT_DMON)] == 3 + events, d_status = self._run_seq(ds_vector, interaction_vector, always_true, always_false) + assert EventName.preDriverUnresponsive in \ + events[int((INVISIBLE_SECONDS_TO_ORANGE-1+DT_DMON*d_status.settings._HI_STD_FALLBACK_TIME-0.1)/DT_DMON)].names + assert EventName.promptDriverUnresponsive in \ + events[int((INVISIBLE_SECONDS_TO_ORANGE-1+DT_DMON*d_status.settings._HI_STD_FALLBACK_TIME+0.1)/DT_DMON)].names + assert EventName.driverUnresponsive in \ + events[int((INVISIBLE_SECONDS_TO_RED-1+DT_DMON*d_status.settings._HI_STD_FALLBACK_TIME+0.1)/DT_DMON)].names + + +@pytest.mark.parametrize("enabled_state, lat_active_state, expected", [ + (False, False, False), # Both Disabled + (True, False, True), # OP Enabled, Lat Inactive + (False, True, True), # OP Disabled, Lat Active (e.g. AOL) + (True, True, True) # Both Active +]) +def test_enabled_states(enabled_state, lat_active_state, expected): + """ + Test DriverMonitoring.run_step with all 4 combinations of: + - selfdriveState.enabled (True/False) + - carControl.latActive (True/False) + """ + cs = car.CarState.new_message() + cs.vEgo = 30.0 + cs.gearShifter = car.CarState.GearShifter.drive + cs.standstill = False + cs.steeringPressed = False + cs.gasPressed = False + + ss = log.SelfdriveState.new_message() + ss.enabled = enabled_state + + cc = car.CarControl.new_message() + cc.latActive = lat_active_state + + mv2 = log.ModelDataV2.new_message() + mv2.meta.disengagePredictions.brakeDisengageProbs = [0.0] + + lc = log.LiveCalibrationData.new_message() + lc.rpyCalib = [0.0, 0.0, 0.0] + + ds = make_msg(False) + + sm = { + 'carState': cs, + 'selfdriveState': ss, + 'carControl': cc, + 'modelV2': mv2, + 'liveCalibration': lc, + 'driverStateV2': ds + } + + driver_monitoring = DriverMonitoring() + + # run_test doesn't assign enabled to a variable, so we need to spy on _update_events to see its value + captured_args = [] + original_update_events = driver_monitoring._update_events + + def spy_update_events(driver_engaged, op_engaged, standstill, wrong_gear, car_speed): + captured_args.append(op_engaged) + return original_update_events(driver_engaged, op_engaged, standstill, wrong_gear, car_speed) + + driver_monitoring._update_events = spy_update_events + + driver_monitoring.run_step(sm, demo=False) + + # Assertion + assert len(captured_args) == 1, "Expected _update_events to be called exactly once" + actual_enabled = captured_args[0] + + assert actual_enabled == expected, f"Expected op_engaged={expected}, but got {actual_enabled}" + diff --git a/selfdrive/selfdrived/events.py b/selfdrive/selfdrived/events.py index 5f47508..1085e11 100755 --- a/selfdrive/selfdrived/events.py +++ b/selfdrive/selfdrived/events.py @@ -79,15 +79,6 @@ def below_steer_speed_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.S Priority.LOW, VisualAlert.none, AudibleAlert.prompt, 0.4) -def too_distracted_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> Alert: - if sm['driverMonitoringState'].lockout: - mins_left = sm['driverMonitoringState'].lockoutMinutesRemaining - if mins_left <= 0: - return NoEntryAlert("Distraction Level Too High", priority=Priority.HIGH) - return NoEntryAlert("Too Distracted", f"{mins_left} minute{'s' if mins_left != 1 else ''} Left", priority=Priority.HIGH) - return NoEntryAlert("Pay Attention to Engage", priority=Priority.HIGH) - - 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['liveCalibration'].calStatus == log.LiveCalibrationData.Status.recalibrating else 'Calibrating' return Alert( @@ -235,8 +226,7 @@ def invalid_lkas_setting_alert(CP: car.CarParams, CS: car.CarState, sm: messagin 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: +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") @@ -370,15 +360,15 @@ EVENTS: dict[int, dict[str, Alert | AlertCallbackType]] = { Priority.LOW, VisualAlert.steerRequired, AudibleAlert.prompt, 1.8), }, - EventName.driverDistracted1: { + EventName.preDriverDistracted: { ET.PERMANENT: Alert( "Pay Attention", "", AlertStatus.normal, AlertSize.small, - Priority.LOW, VisualAlert.none, AudibleAlert.preAlert, .1), + Priority.LOW, VisualAlert.none, AudibleAlert.none, .1), }, - EventName.driverDistracted2: { + EventName.promptDriverDistracted: { ET.PERMANENT: Alert( "Pay Attention", "Driver Distracted", @@ -386,7 +376,7 @@ EVENTS: dict[int, dict[str, Alert | AlertCallbackType]] = { Priority.MID, VisualAlert.steerRequired, AudibleAlert.promptDistracted, .1), }, - EventName.driverDistracted3: { + EventName.driverDistracted: { ET.PERMANENT: Alert( "DISENGAGE IMMEDIATELY", "Driver Distracted", @@ -394,7 +384,7 @@ EVENTS: dict[int, dict[str, Alert | AlertCallbackType]] = { Priority.HIGH, VisualAlert.steerRequired, AudibleAlert.warningImmediate, .1), }, - EventName.driverUnresponsive1: { + EventName.preDriverUnresponsive: { ET.PERMANENT: Alert( "Touch Steering Wheel: No Face Detected", "", @@ -402,7 +392,7 @@ EVENTS: dict[int, dict[str, Alert | AlertCallbackType]] = { Priority.LOW, VisualAlert.steerRequired, AudibleAlert.none, .1), }, - EventName.driverUnresponsive2: { + EventName.promptDriverUnresponsive: { ET.PERMANENT: Alert( "Touch Steering Wheel", "Driver Unresponsive", @@ -410,7 +400,7 @@ EVENTS: dict[int, dict[str, Alert | AlertCallbackType]] = { Priority.MID, VisualAlert.steerRequired, AudibleAlert.promptDistracted, .1), }, - EventName.driverUnresponsive3: { + EventName.driverUnresponsive: { ET.PERMANENT: Alert( "DISENGAGE IMMEDIATELY", "Driver Unresponsive", @@ -642,7 +632,7 @@ EVENTS: dict[int, dict[str, Alert | AlertCallbackType]] = { }, EventName.tooDistracted: { - ET.NO_ENTRY: too_distracted_alert, + ET.NO_ENTRY: NoEntryAlert("Distraction Level Too High"), }, EventName.excessiveActuation: { @@ -888,14 +878,14 @@ EVENTS: dict[int, dict[str, Alert | AlertCallbackType]] = { if HARDWARE.get_device_type() == 'mici': EVENTS.update({ - EventName.driverDistracted1: { + EventName.preDriverDistracted: { ET.PERMANENT: Alert( "Pay Attention", "", AlertStatus.normal, AlertSize.small, - Priority.LOW, VisualAlert.none, AudibleAlert.preAlert, 2), + Priority.LOW, VisualAlert.none, AudibleAlert.none, 2), }, - EventName.driverDistracted2: { + EventName.promptDriverDistracted: { ET.PERMANENT: Alert( "Pay Attention", "Driver Distracted", diff --git a/selfdrive/selfdrived/selfdrived.py b/selfdrive/selfdrived/selfdrived.py index 15da7cc..5c13610 100755 --- a/selfdrive/selfdrived/selfdrived.py +++ b/selfdrive/selfdrived/selfdrived.py @@ -54,11 +54,11 @@ EventName = log.OnroadEvent.EventName ButtonType = car.CarState.ButtonEvent.Type SafetyModel = car.CarParams.SafetyModel TurnDirection = custom.IQTurnSignalDirection -AlertLevel = log.DriverMonitoringState.AlertLevel -MonitoringPolicy = log.DriverMonitoringState.MonitoringPolicy IGNORED_SAFETY_MODES = (SafetyModel.silent, SafetyModel.noOutput) +NON_BLOCKING_PROCESSES = {'mapd', 'iqmapd', 'navd', 'navrenderd'} + def _cleanup_startup_params(CP: car.CarParams, params: Params) -> None: if not CP.alphaLongitudinalAvailable or not CP.openpilotLongitudinalControl: @@ -129,6 +129,7 @@ class SelfdriveD(GapButtonActions): self.is_ldw_enabled = self.params.get_bool("IsLdwEnabled") self.disengage_on_accelerator = self.params.get_bool("DisengageOnAccelerator") self.nav_exit_lane_change = self._read_nav_exit_lane_change() + self.model_download_pending = self.params.get("ModelManager_DownloadIndex") is not None car_recognized = self.CP.brand != 'mock' @@ -149,7 +150,6 @@ class SelfdriveD(GapButtonActions): self.events_prev = [] self.logged_comm_issue = None self.not_running_prev = None - self.dm_lockout_set = False self.experimental_mode = False self.personality = get_sanitize_int_param( "LongitudinalPersonality", @@ -161,10 +161,10 @@ class SelfdriveD(GapButtonActions): self.state_machine = StateMachine() self.rk = Ratekeeper(100, print_delay_threshold=None) - self.ignored_processes = set() + self.ignored_processes = set(NON_BLOCKING_PROCESSES) nvme_expected = os.path.exists('/dev/nvme0n1') or (not os.path.isfile("/persist/comma/living-in-the-moment")) if HARDWARE.get_device_type() == 'tici' and nvme_expected: - self.ignored_processes = {'loggerd', } + self.ignored_processes.add('loggerd') # Determine startup event is_remote = build_metadata.openpilot.comma_remote or build_metadata.openpilot.iqpilot_remote @@ -186,6 +186,7 @@ class SelfdriveD(GapButtonActions): self.events_iq = IQEvents() self.events_iq_prev = [] + self._cached_dm_event_names: tuple[int, ...] = () self._cached_plan_event_names: tuple[int, ...] = () self._cached_model_event_names: tuple[int, ...] = () self._cached_nav_event_names: tuple[int, ...] = () @@ -207,6 +208,10 @@ class SelfdriveD(GapButtonActions): if self.sm.updated['iqPlan']: self._cached_plan_event_names = tuple(event.name.raw for event in self._get_longitudinal_plan_ext().events) + def _refresh_cached_dm_events(self) -> None: + if self.sm.updated['driverMonitoringState']: + self._cached_dm_event_names = tuple(event.name.raw for event in self.sm['driverMonitoringState'].events) + def _refresh_cached_model_events(self) -> None: if not self.sm.updated['iqDriveModelData']: return @@ -295,24 +300,8 @@ class SelfdriveD(GapButtonActions): self.events.add(EventName.resumeBlocked) if not self.CP.notCar: - if self.sm['driverMonitoringState'].lockout and not self.dm_lockout_set: - self.params.put_bool("DriverTooDistracted", True) - self.dm_lockout_set = True - elif not self.sm['driverMonitoringState'].lockout and self.dm_lockout_set: - self.params.remove("DriverTooDistracted") - self.dm_lockout_set = False - - if self.sm['driverMonitoringState'].lockout or self.sm['driverMonitoringState'].alwaysOnLockout: - self.events.add(EventName.tooDistracted) - - vision_dm = self.sm['driverMonitoringState'].activePolicy == MonitoringPolicy.vision - if self.sm['driverMonitoringState'].alertLevel == AlertLevel.one: - self.events.add(EventName.driverDistracted1 if vision_dm else EventName.driverUnresponsive1) - elif self.sm['driverMonitoringState'].alertLevel == AlertLevel.two: - self.events.add(EventName.driverDistracted2 if vision_dm else EventName.driverUnresponsive2) - elif self.sm['driverMonitoringState'].alertLevel == AlertLevel.three: - self.events.add(EventName.driverDistracted3 if vision_dm else EventName.driverUnresponsive3) - + self._refresh_cached_dm_events() + self._add_event_names(self._cached_dm_event_names) self._refresh_cached_plan_events() self._add_iq_event_names(self._cached_plan_event_names) @@ -443,6 +432,8 @@ class SelfdriveD(GapButtonActions): self.not_running_prev = not_running if self.sm.recv_frame['managerState'] and (not_running - self.ignored_processes): self.events.add(EventName.processNotRunning) + if 'iqmodeld' in not_running and self.model_download_pending: + self.events_iq.add(custom.IQOnroadEvent.EventName.modelUpdating) else: if not SIMULATION and not self.rk.lagging: if not self.sm.all_alive(self.camera_packets): @@ -754,8 +745,18 @@ class SelfdriveD(GapButtonActions): self.is_ldw_enabled = self.params.get_bool("IsLdwEnabled") self.disengage_on_accelerator = self.params.get_bool("DisengageOnAccelerator") self.experimental_mode = self.params.get_bool("ExperimentalMode") and self.CP.openpilotLongitudinalControl - self.personality = self.params.get("LongitudinalPersonality", return_default=True) + # Params can be changed while selfdrived is running. Keep the live value in + # the same valid enum range enforced during startup; otherwise a stale value + # (for example 3) makes the alert callback lookup raise KeyError and kills + # selfdrived. + self.personality = get_sanitize_int_param( + "LongitudinalPersonality", + min(log.LongitudinalPersonality.schema.enumerants.values()), + max(log.LongitudinalPersonality.schema.enumerants.values()), + self.params, + ) self.nav_exit_lane_change = self._read_nav_exit_lane_change() + self.model_download_pending = self.params.get("ModelManager_DownloadIndex") is not None self.aol.read_params() time.sleep(0.1) diff --git a/selfdrive/selfdrived/tests/test_driver_monitoring.py b/selfdrive/selfdrived/tests/test_driver_monitoring.py deleted file mode 100644 index 71bf8dd..0000000 --- a/selfdrive/selfdrived/tests/test_driver_monitoring.py +++ /dev/null @@ -1,40 +0,0 @@ -from types import SimpleNamespace - -from cereal import car, log - -from openpilot.selfdrive.selfdrived.events import EVENTS, ET - - -EventName = log.OnroadEvent.EventName -AudibleAlert = car.CarControl.HUDControl.AudibleAlert - - -def test_driver_monitoring_alert_stages(): - expected_sounds = { - EventName.driverDistracted1: AudibleAlert.preAlert, - EventName.driverDistracted2: AudibleAlert.promptDistracted, - EventName.driverDistracted3: AudibleAlert.warningImmediate, - EventName.driverUnresponsive1: AudibleAlert.none, - EventName.driverUnresponsive2: AudibleAlert.promptDistracted, - EventName.driverUnresponsive3: AudibleAlert.warningImmediate, - } - - for event_name, audible_alert in expected_sounds.items(): - assert EVENTS[event_name][ET.PERMANENT].audible_alert == audible_alert - - -def test_driver_monitoring_lockout_alert(): - callback = EVENTS[EventName.tooDistracted][ET.NO_ENTRY] - sm = {'driverMonitoringState': SimpleNamespace(lockout=True, lockoutMinutesRemaining=5)} - alert = callback(None, None, sm, False, 0, None) - - assert alert.alert_text_1 == "5 minutes Left" - assert alert.alert_text_2 == "Too Distracted" - - -def test_legacy_driver_monitoring_lockout_alert(): - callback = EVENTS[EventName.tooDistracted][ET.NO_ENTRY] - sm = {'driverMonitoringState': SimpleNamespace(lockout=True, lockoutMinutesRemaining=0)} - alert = callback(None, None, sm, False, 0, None) - - assert alert.alert_text_2 == "Distraction Level Too High" diff --git a/selfdrive/selfdrived/tests/test_longitudinal_pref_persistence.py b/selfdrive/selfdrived/tests/test_longitudinal_pref_persistence.py index b8e1c58..ba128f7 100644 --- a/selfdrive/selfdrived/tests/test_longitudinal_pref_persistence.py +++ b/selfdrive/selfdrived/tests/test_longitudinal_pref_persistence.py @@ -1,6 +1,6 @@ from cereal import car -from openpilot.selfdrive.selfdrived.selfdrived import _cleanup_startup_params +from openpilot.selfdrive.selfdrived.selfdrived import _cleanup_startup_params, get_sanitize_int_param class DummyParams: @@ -21,3 +21,20 @@ class TestLongitudinalPrefPersistence: _cleanup_startup_params(cp, params) assert params.removed == [] + + def test_invalid_personality_is_clamped_before_use(self): + class ParamsWithInvalidPersonality: + def __init__(self): + self.value = 3 + + def get(self, key: str, return_default: bool = False) -> int: + assert key == "LongitudinalPersonality" + return self.value + + def put(self, key: str, value: int) -> None: + assert key == "LongitudinalPersonality" + self.value = value + + params = ParamsWithInvalidPersonality() + assert get_sanitize_int_param("LongitudinalPersonality", 0, 2, params) == 2 + assert params.value == 2 diff --git a/selfdrive/test/process_replay/migration.py b/selfdrive/test/process_replay/migration.py index b135a68..010ce3f 100644 --- a/selfdrive/test/process_replay/migration.py +++ b/selfdrive/test/process_replay/migration.py @@ -455,46 +455,21 @@ def migrate_onroadEvents(msgs): return ops, [], [] -@migration(inputs=["driverMonitoringStateDEPRECATED"]) +@migration(inputs=["driverMonitoringState"]) def migrate_driverMonitoringState(msgs): ops = [] for index, msg in msgs: - old = msg.driverMonitoringStateDEPRECATED - new_msg = messaging.new_message('driverMonitoringState', valid=msg.valid, logMonoTime=msg.logMonoTime) - dm = new_msg.driverMonitoringState - dm.isRHD = old.isRHD - dm.activePolicy = log.DriverMonitoringState.MonitoringPolicy.vision if old.isActiveMode else \ - log.DriverMonitoringState.MonitoringPolicy.wheeltouch + msg = msg.as_builder() + events = [] + for event in msg.driverMonitoringState.eventsDEPRECATED: + try: + if not str(event.name).endswith('DEPRECATED'): + # dict converts name enum into string representation + events.append(log.OnroadEvent(**event.to_dict())) + except RuntimeError: # Member was null + traceback.print_exc() - AlertLevel = log.DriverMonitoringState.AlertLevel - event_to_alert_level = { - 'driverDistracted1': AlertLevel.one, 'driverUnresponsive1': AlertLevel.one, - 'driverDistracted2': AlertLevel.two, 'driverUnresponsive2': AlertLevel.two, - 'driverDistracted3': AlertLevel.three, 'driverUnresponsive3': AlertLevel.three, - } - for event in old.events: - level = event_to_alert_level.get(str(event.name)) - if level is not None: - dm.alertLevel = level - break - dm.lockout = any(str(event.name) == 'tooDistracted' for event in old.events) - - dm.visionPolicyState.awarenessPercent = int(max(0, min(100, (old.awarenessStatus if old.isActiveMode else old.awarenessActive) * 100))) - dm.visionPolicyState.awarenessStep = old.stepChange if old.isActiveMode else 0. - dm.visionPolicyState.isDistracted = old.isDistracted - dm.visionPolicyState.distractedTypes.pose = bool(old.distractedType & 1) - dm.visionPolicyState.distractedTypes.eye = bool(old.distractedType & 2) - dm.visionPolicyState.distractedTypes.phone = bool(old.distractedType & 4) - dm.visionPolicyState.faceDetected = old.faceDetected - dm.visionPolicyState.pose.pitchCalib.offset = old.posePitchOffset - dm.visionPolicyState.pose.pitchCalib.calibratedPercent = int(min(100, old.posePitchValidCount / 1200 * 100)) - dm.visionPolicyState.pose.yawCalib.offset = old.poseYawOffset - dm.visionPolicyState.pose.yawCalib.calibratedPercent = int(min(100, old.poseYawValidCount / 1200 * 100)) - dm.visionPolicyState.pose.calibrated = old.posePitchValidCount >= 1200 and old.poseYawValidCount >= 1200 - dm.visionPolicyState.wheeltouchFallbackPercent = int(min(100, old.hiStdCount / 200 * 100)) - dm.visionPolicyState.uncertainOffroadAlertPercent = int(min(100, old.uncertainCount / 1200 * 100)) - dm.wheeltouchPolicyState.awarenessPercent = int(max(0, min(100, (old.awarenessPassive if old.isActiveMode else old.awarenessStatus) * 100))) - dm.wheeltouchPolicyState.awarenessStep = 0. if old.isActiveMode else old.stepChange - ops.append((index, new_msg.as_reader())) + msg.driverMonitoringState.events = events + ops.append((index, msg.as_reader())) return ops, [], [] diff --git a/selfdrive/ui/layouts/home.py b/selfdrive/ui/layouts/home.py index 2000a01..bf910af 100644 --- a/selfdrive/ui/layouts/home.py +++ b/selfdrive/ui/layouts/home.py @@ -10,7 +10,7 @@ from openpilot.selfdrive.ui.widgets.offroad_alerts import UpdateAlert, OffroadAl from openpilot.selfdrive.ui.widgets.setup import SetupWidget from openpilot.selfdrive.ui.widgets.inspire_widget import InspireWidget from openpilot.selfdrive.ui.widgets.map_panel_widget import MapPanelWidget -from openpilot.iqpilot.ui.layouts.settings.trips import TripsLayout +from openpilot.iqpilot.ui.layouts.settings.drive_history import TripsLayout from openpilot.selfdrive.ui.layouts.sidebar import NETWORK_TYPES from openpilot.selfdrive.ui.lib.wifi_ssid import current_ssid from openpilot.selfdrive.ui.ui_state import ui_state diff --git a/selfdrive/ui/layouts/settings/developer.py b/selfdrive/ui/layouts/settings/developer.py index 7a45da0..c19e086 100644 --- a/selfdrive/ui/layouts/settings/developer.py +++ b/selfdrive/ui/layouts/settings/developer.py @@ -24,6 +24,14 @@ DESCRIPTIONS = { "Exposes a snapshot of recent dashcam clips and logs as a USB drive when connected to a computer. " + "IQ.Pilot keeps running while this is enabled." ), + 'long_maneuver': tr_noop( + "Commands a scripted sequence of acceleration steps to measure longitudinal actuator response. " + + "Requires IQ.Pilot longitudinal control. Only use on a clear, closed road." + ), + 'lat_maneuver': tr_noop( + "Commands a scripted sequence of lateral acceleration steps to measure steering actuator response. " + + "Only use on a straight, flat, clear road." + ), } @@ -59,6 +67,20 @@ class DeveloperLayout(Widget): ) self._ssh_keys = ssh_key_item(lambda: tr("SSH Keys"), description=lambda: tr(DESCRIPTIONS["ssh_key"])) + self._long_maneuver_toggle = toggle_item( + lambda: tr("Longitudinal Maneuver Mode"), + description=lambda: tr(DESCRIPTIONS["long_maneuver"]), + initial_state=self._params.get_bool("LongitudinalManeuverMode"), + callback=self._on_long_maneuver_mode, + ) + + self._lat_maneuver_toggle = toggle_item( + lambda: tr("Lateral Maneuver Mode"), + description=lambda: tr(DESCRIPTIONS["lat_maneuver"]), + initial_state=self._params.get_bool("LateralManeuverMode"), + callback=self._on_lat_maneuver_mode, + ) + self._on_enable_ui_debug(self._params.get_bool("ShowDebugInfo")) self._scroller = Scroller([ @@ -66,6 +88,8 @@ class DeveloperLayout(Widget): self._usb_storage_toggle, self._ssh_toggle, self._ssh_keys, + self._long_maneuver_toggle, + self._lat_maneuver_toggle, ], line_separator=True, spacing=0) # Toggles should be not available to change in onroad state @@ -81,12 +105,24 @@ class DeveloperLayout(Widget): def _update_toggles(self): ui_state.update_params() + for item in (self._long_maneuver_toggle, self._lat_maneuver_toggle): + item.set_visible(not self._is_release) + + if ui_state.CP is not None: + self._long_maneuver_toggle.action_item.set_enabled(ui_state.has_longitudinal_control and ui_state.is_offroad()) + self._lat_maneuver_toggle.action_item.set_enabled(ui_state.is_offroad()) + else: + self._long_maneuver_toggle.action_item.set_enabled(False) + self._lat_maneuver_toggle.action_item.set_enabled(False) + # TODO: make a param control list item so we don't need to manage internal state as much here # refresh toggles from params to mirror external changes for key, item in ( ("AdbEnabled", self._adb_toggle), ("UsbStorageEnabled", self._usb_storage_toggle), ("SshEnabled", self._ssh_toggle), + ("LongitudinalManeuverMode", self._long_maneuver_toggle), + ("LateralManeuverMode", self._lat_maneuver_toggle), ): item.action_item.set_state(self._params.get_bool(key)) @@ -108,7 +144,12 @@ class DeveloperLayout(Widget): def _on_long_maneuver_mode(self, state: bool): self._params.put_bool("LongitudinalManeuverMode", state) self._params.put_bool("JoystickDebugMode", False) + self._params.put_bool("LateralManeuverMode", False) + self._lat_maneuver_toggle.action_item.set_state(False) def _on_lat_maneuver_mode(self, state: bool): self._params.put_bool("LateralManeuverMode", state) self._params.put_bool("JoystickDebugMode", False) + self._params.put_bool("ExperimentalMode", False) + self._params.put_bool("LongitudinalManeuverMode", False) + self._long_maneuver_toggle.action_item.set_state(False) diff --git a/selfdrive/ui/layouts/settings/toggles.py b/selfdrive/ui/layouts/settings/toggles.py index eda7e3b..69145bb 100644 --- a/selfdrive/ui/layouts/settings/toggles.py +++ b/selfdrive/ui/layouts/settings/toggles.py @@ -214,7 +214,7 @@ class TogglesLayout(Widget): toyota_stock_long_forced = bool( ui_state.CP is not None and ui_state.CP.brand == "toyota" and - self._params.get_bool("ToyotaEnforceStockLongitudinal") + self._params.get_bool("IQToyotaFactoryLong") ) iq_modes_selectable = alpha_available or alpha_requested or toyota_stock_long_forced availability_note = "" @@ -268,7 +268,7 @@ class TogglesLayout(Widget): def _apply_longitudinal_control_mode(self, button_index: int): # 0 = Stock ACC, 1 = IQ.Standard, 2 = IQ.Dynamic, 3 = IQ.Pilot previous_alpha = self._params.get_bool("AlphaLongitudinalEnabled") - previous_toyota_stock_long = self._params.get_bool("ToyotaEnforceStockLongitudinal") + previous_toyota_stock_long = self._params.get_bool("IQToyotaFactoryLong") if button_index == 0: self._params.put_bool("AlphaLongitudinalEnabled", False) @@ -289,9 +289,9 @@ class TogglesLayout(Widget): self._params.put_bool("IQDynamicMode", False) if button_index != 0 and previous_toyota_stock_long: - self._params.put_bool("ToyotaEnforceStockLongitudinal", False) + self._params.put_bool("IQToyotaFactoryLong", False) - if previous_alpha != self._params.get_bool("AlphaLongitudinalEnabled") or previous_toyota_stock_long != self._params.get_bool("ToyotaEnforceStockLongitudinal"): + if previous_alpha != self._params.get_bool("AlphaLongitudinalEnabled") or previous_toyota_stock_long != self._params.get_bool("IQToyotaFactoryLong"): self._params.put_bool("OnroadCycleRequested", True) def _toggle_callback(self, state: bool, param: str): diff --git a/selfdrive/ui/layouts/stats.py b/selfdrive/ui/layouts/stats.py index bd2861c..ea52c7f 100644 --- a/selfdrive/ui/layouts/stats.py +++ b/selfdrive/ui/layouts/stats.py @@ -1,7 +1,7 @@ import pyray as rl from collections.abc import Callable -from openpilot.iqpilot.ui.layouts.settings.trips import TripsLayout +from openpilot.iqpilot.ui.layouts.settings.drive_history import TripsLayout from openpilot.selfdrive.ui.widgets.screen_header import ScreenHeader, HEADER_HEIGHT from openpilot.system.ui.lib.multilang import tr from openpilot.system.ui.widgets import Widget diff --git a/selfdrive/ui/mici/layouts/onboarding.py b/selfdrive/ui/mici/layouts/onboarding.py index 8efcdd9..11a8154 100644 --- a/selfdrive/ui/mici/layouts/onboarding.py +++ b/selfdrive/ui/mici/layouts/onboarding.py @@ -200,7 +200,7 @@ class TrainingGuideDMTutorial(Widget): looking_center = False # stay at 100% once reached - if (dm_state.visionPolicyState.faceDetected and looking_center) or self._progress.x > 0.99: + if (dm_state.faceDetected and looking_center) or self._progress.x > 0.99: slow = self._progress.x < 0.25 duration = self.PROGRESS_DURATION * 2 if slow else self.PROGRESS_DURATION self._progress.x += 1.0 / (duration * gui_app.target_fps) diff --git a/selfdrive/ui/mici/layouts/settings/trips.py b/selfdrive/ui/mici/layouts/settings/drive_history.py similarity index 100% rename from selfdrive/ui/mici/layouts/settings/trips.py rename to selfdrive/ui/mici/layouts/settings/drive_history.py diff --git a/selfdrive/ui/mici/layouts/settings/models.py b/selfdrive/ui/mici/layouts/settings/models.py index 23fd338..86ddb98 100644 --- a/selfdrive/ui/mici/layouts/settings/models.py +++ b/selfdrive/ui/mici/layouts/settings/models.py @@ -223,7 +223,7 @@ class ModelsLayoutMici(NavScroller): @staticmethod def _read_favorites() -> set: - favs = ui_state.params.get("ModelManager_Favs") + favs = ui_state.params.get("IQModelFavorites") return set(favs.split(';')) if favs else set() def _toggle_favorite(self, bundle) -> bool: @@ -232,7 +232,7 @@ class ModelsLayoutMici(NavScroller): favs.discard(bundle.ref) else: favs.add(bundle.ref) - ui_state.params.put("ModelManager_Favs", ';'.join(sorted(favs))) + ui_state.params.put("IQModelFavorites", ';'.join(sorted(favs))) return bundle.ref in favs def _confirm_clear_cache(self): diff --git a/selfdrive/ui/mici/layouts/settings/settings.py b/selfdrive/ui/mici/layouts/settings/settings.py index 90dc4e3..a2aab7e 100644 --- a/selfdrive/ui/mici/layouts/settings/settings.py +++ b/selfdrive/ui/mici/layouts/settings/settings.py @@ -11,7 +11,7 @@ from openpilot.selfdrive.ui.mici.layouts.settings.cruise import CruiseLayoutMici from openpilot.selfdrive.ui.mici.layouts.settings.visuals import VisualsLayoutMici from openpilot.selfdrive.ui.mici.layouts.settings.models import ModelsLayoutMici from openpilot.selfdrive.ui.mici.layouts.settings.display import DisplayLayoutMici -from openpilot.selfdrive.ui.mici.layouts.settings.trips import TripsLayoutMici +from openpilot.selfdrive.ui.mici.layouts.settings.drive_history import TripsLayoutMici from openpilot.selfdrive.ui.mici.layouts.settings.vehicle import VehicleLayoutMici from openpilot.selfdrive.ui.mici.layouts.settings.dashcam import DashcamLayoutMici from openpilot.selfdrive.ui.mici.layouts.settings.network.network_layout import NetworkLayoutMici diff --git a/selfdrive/ui/mici/layouts/settings/vehicle.py b/selfdrive/ui/mici/layouts/settings/vehicle.py index be0f29d..a228155 100644 --- a/selfdrive/ui/mici/layouts/settings/vehicle.py +++ b/selfdrive/ui/mici/layouts/settings/vehicle.py @@ -69,17 +69,17 @@ class VehicleLayoutMici(NavScroller): self._vehicle_btn = BigButton("vehicle") self._vehicle_btn.set_click_callback(self._on_vehicle_clicked) - self._toyota_long = BigParamControl("enforce factory long.", "ToyotaEnforceStockLongitudinal", + self._toyota_long = BigParamControl("enforce factory long.", "IQToyotaFactoryLong", toggle_callback=self._on_toyota_long) - self._hyundai_tuning = MappedParamToggle("hyundai long. tuning", "HyundaiLongitudinalTuning", + self._hyundai_tuning = MappedParamToggle("hyundai long. tuning", "IQHyundaiLongTune", ["off", "dynamic", "predictive"], [0, 1, 2]) - self._subaru_snag = BigParamControl("creep from standstill (beta)", "SubaruStopAndGo") - self._subaru_manual = BigParamControl("stop and go manual brake", "SubaruStopAndGoManualParkingBrake") + self._subaru_snag = BigParamControl("creep from standstill (beta)", "IQSubaruCreepAssist") + self._subaru_manual = BigParamControl("stop and go manual brake", "IQSubaruCreepAssistManualBrake") self._vw_pq_hca = BigParamControl("PQ HCA status 7 mode", "pqhca5or7Toggle") self._vw_lateral = BigParamControl("lateral when cruise faulted", "AllowLateralWhenLongUnavailable") self._vw_mqb_acc_resume = BigParamControl("MQB ACC resume", "iqMqbAccResume") self._vw_mqb_steering_lockout = BigParamControl("MQB steering lockout", "iqMqbSteeringLockout") - self._tesla_vtb = BigParamControl("virtual torque blending", "TeslaCoopSteering") + self._tesla_vtb = BigParamControl("virtual torque blending", "IQTeslaTorqueBlend") self._brand_widgets = { "toyota": [self._toyota_long], diff --git a/selfdrive/ui/mici/layouts/settings/visuals.py b/selfdrive/ui/mici/layouts/settings/visuals.py index 67f7cce..35dac74 100644 --- a/selfdrive/ui/mici/layouts/settings/visuals.py +++ b/selfdrive/ui/mici/layouts/settings/visuals.py @@ -9,11 +9,11 @@ from openpilot.system.ui.widgets.scroller import NavScroller class VisualsLayoutMici(NavScroller): def __init__(self): super().__init__() - self._blind_spot = BigParamControl("Blind Spot Warnings", "BlindSpot") - self._steering_arc = BigParamControl("Steering Effort Arc", "TorqueBar") - self._road_name = BigParamControl("Road Name", "RoadNameToggle") - self._turn_signals = BigParamControl("Turn Signals", "ShowTurnSignals") - self._accel_bar = BigParamControl("Acceleration Bar", "RocketFuel") + self._blind_spot = BigParamControl("Blind Spot Warnings", "IQBlindSpotAlerts") + self._steering_arc = BigParamControl("Steering Effort Arc", "IQSteerEffortArc") + self._road_name = BigParamControl("Road Name", "IQRoadNameOverlay") + self._turn_signals = BigParamControl("Turn Signals", "IQBlinkerIndicators") + self._accel_bar = BigParamControl("Acceleration Bar", "IQAccelMeter") self._toggles = [self._blind_spot, self._steering_arc, self._road_name, self._turn_signals, self._accel_bar] diff --git a/selfdrive/ui/mici/onroad/augmented_road_view.py b/selfdrive/ui/mici/onroad/augmented_road_view.py index 0d32f6c..7ca48b1 100644 --- a/selfdrive/ui/mici/onroad/augmented_road_view.py +++ b/selfdrive/ui/mici/onroad/augmented_road_view.py @@ -29,7 +29,7 @@ from openpilot.iqpilot.ui.onroad.augmented_road_view import BORDER_COLORS_IQ if gui_app.iqpilot_ui(): from openpilot.iqpilot.ui.mici.onroad.hud_renderer import IQMiciHudRenderer as HudRenderer - from openpilot.iqpilot.ui.mici.onroad.road_name import RoadNameRendererMici + from openpilot.iqpilot.ui.mici.onroad.road_label import RoadNameRendererMici from openpilot.selfdrive.ui.ui_state import OnroadTimerStatus OpState = log.SelfdriveState.OpenpilotState diff --git a/selfdrive/ui/mici/onroad/driver_camera_dialog.py b/selfdrive/ui/mici/onroad/driver_camera_dialog.py index 1ad90a2..68ff785 100644 --- a/selfdrive/ui/mici/onroad/driver_camera_dialog.py +++ b/selfdrive/ui/mici/onroad/driver_camera_dialog.py @@ -1,14 +1,20 @@ import pyray as rl -from cereal import car, log, messaging +from cereal import log, messaging from msgq.visionipc import VisionStreamType from openpilot.selfdrive.ui.mici.onroad.cameraview import CameraView from openpilot.selfdrive.ui.mici.onroad.driver_state import DriverStateRenderer from openpilot.selfdrive.ui.ui_state import ui_state, device +from openpilot.selfdrive.selfdrived.events import EVENTS, ET from openpilot.system.ui.lib.application import gui_app, FontWeight from openpilot.system.ui.lib.multilang import tr from openpilot.system.ui.widgets.nav_widget import NavWidget from openpilot.system.ui.widgets.label import gui_label +EventName = log.OnroadEvent.EventName + +EVENT_TO_INT = EventName.schema.enumerants + + class DriverCameraView(CameraView): def _calc_frame_matrix(self, rect: rl.Rectangle): base = super()._calc_frame_matrix(rect) @@ -110,14 +116,10 @@ class DriverCameraDialog(NavWidget): return msg = messaging.new_message('selfdriveState') - if dm_state is not None: - AudibleAlert = car.CarControl.HUDControl.AudibleAlert - alert_sounds = { - 'one': AudibleAlert.preAlert, - 'two': AudibleAlert.promptDistracted, - 'three': AudibleAlert.warningImmediate, - } - msg.selfdriveState.alertSound = alert_sounds.get(str(dm_state.alertLevel), AudibleAlert.none) + if dm_state is not None and len(dm_state.events): + event_name = EVENT_TO_INT[dm_state.events[0].name] + if event_name is not None and event_name in EVENTS and ET.PERMANENT in EVENTS[event_name]: + msg.selfdriveState.alertSound = EVENTS[event_name][ET.PERMANENT].audible_alert self._pm.send('selfdriveState', msg) def _render_dm_alerts(self, rect: rl.Rectangle): @@ -125,30 +127,29 @@ class DriverCameraDialog(NavWidget): dm_state = ui_state.sm["driverMonitoringState"] self._publish_alert_sound(dm_state) - is_vision = dm_state.activePolicy == log.DriverMonitoringState.MonitoringPolicy.vision - awareness_pct = dm_state.visionPolicyState.awarenessPercent if is_vision else dm_state.wheeltouchPolicyState.awarenessPercent gui_label(rl.Rectangle(rect.x + 2, rect.y + 2, rect.width, rect.height), - f"Awareness: {awareness_pct:.0f}%", font_size=44, font_weight=FontWeight.MEDIUM, + f"Awareness: {dm_state.awarenessStatus * 100:.0f}%", font_size=44, font_weight=FontWeight.MEDIUM, alignment=rl.GuiTextAlignment.TEXT_ALIGN_RIGHT, alignment_vertical=rl.GuiTextAlignmentVertical.TEXT_ALIGN_TOP, color=rl.Color(0, 0, 0, 180)) - gui_label(rect, f"Awareness: {awareness_pct:.0f}%", font_size=44, font_weight=FontWeight.MEDIUM, + gui_label(rect, f"Awareness: {dm_state.awarenessStatus * 100:.0f}%", font_size=44, font_weight=FontWeight.MEDIUM, alignment=rl.GuiTextAlignment.TEXT_ALIGN_RIGHT, alignment_vertical=rl.GuiTextAlignmentVertical.TEXT_ALIGN_TOP, color=rl.Color(255, 255, 255, int(255 * 0.9))) - if dm_state.alertLevel == log.DriverMonitoringState.AlertLevel.none: + if not dm_state.events: return - alert_level_str = f"{'Pay Attention' if is_vision else 'Touch Wheel'} - level {dm_state.alertLevel}" + # Show first event (only one should be active at a time) + event_name_str = str(dm_state.events[0].name).split('.')[-1] alignment = rl.GuiTextAlignment.TEXT_ALIGN_RIGHT if self.driver_state_renderer.is_rhd else rl.GuiTextAlignment.TEXT_ALIGN_LEFT shadow_rect = rl.Rectangle(rect.x + 2, rect.y + 2, rect.width, rect.height) - gui_label(shadow_rect, alert_level_str, font_size=40, font_weight=FontWeight.BOLD, + gui_label(shadow_rect, event_name_str, font_size=40, font_weight=FontWeight.BOLD, alignment=alignment, alignment_vertical=rl.GuiTextAlignmentVertical.TEXT_ALIGN_BOTTOM, color=rl.Color(0, 0, 0, 180)) - gui_label(rect, alert_level_str, font_size=40, font_weight=FontWeight.BOLD, + gui_label(rect, event_name_str, font_size=40, font_weight=FontWeight.BOLD, alignment=alignment, alignment_vertical=rl.GuiTextAlignmentVertical.TEXT_ALIGN_BOTTOM, color=rl.Color(255, 255, 255, int(255 * 0.9))) @@ -165,7 +166,7 @@ class DriverCameraDialog(NavWidget): def _draw_face_detection(self, rect: rl.Rectangle): dm_state = ui_state.sm["driverMonitoringState"] driver_data = self.driver_state_renderer.get_driver_data() - if not dm_state.visionPolicyState.faceDetected: + if not dm_state.faceDetected: return # Get face position and orientation diff --git a/selfdrive/ui/mici/onroad/driver_state.py b/selfdrive/ui/mici/onroad/driver_state.py index bd2e694..8a20137 100644 --- a/selfdrive/ui/mici/onroad/driver_state.py +++ b/selfdrive/ui/mici/onroad/driver_state.py @@ -6,12 +6,12 @@ from openpilot.common.filter_simple import FirstOrderFilter from openpilot.system.ui.lib.application import gui_app from openpilot.system.ui.widgets import Widget from openpilot.selfdrive.ui.ui_state import ui_state +from openpilot.selfdrive.monitoring.helpers import face_orientation_from_net AlertSize = log.SelfdriveState.AlertSize DEBUG = False ACTIVE_ACCENT = rl.Color(0x0C, 0x94, 0x96, 0xFF) -CONE_COLOR_ORANGE = (255, 115, 0) LOOKING_CENTER_THRESHOLD_UPPER = math.radians(6) LOOKING_CENTER_THRESHOLD_LOWER = math.radians(3) @@ -21,7 +21,6 @@ class DriverStateRenderer(Widget): BASE_SIZE = 60 LINES_ANGLE_INCREMENT = 5 LINES_STALE_ANGLES = 3.0 # seconds - AWARENESS_UNFULL_PERCENT = 95 def __init__(self, lines: bool = False, inset: bool = False): super().__init__() @@ -36,15 +35,11 @@ class DriverStateRenderer(Widget): self._is_active = False self._is_rhd = False self._face_detected = False - self._face_pitch = 0. - self._face_yaw = 0. self._should_draw = False self._force_active = False self._looking_center = False - self._awareness_unfull = False self._fade_filter = FirstOrderFilter(0.0, 0.05, 1 / gui_app.target_fps) - self._color_fade_filter = FirstOrderFilter(1.0, 0.05, 1 / gui_app.target_fps) self._pitch_filter = FirstOrderFilter(0.0, 0.05, 1 / gui_app.target_fps, initialized=False) self._yaw_filter = FirstOrderFilter(0.0, 0.05, 1 / gui_app.target_fps, initialized=False) self._rotation_filter = FirstOrderFilter(0.0, 0.1, 1 / gui_app.target_fps, initialized=False) @@ -100,7 +95,6 @@ class DriverStateRenderer(Widget): rl.Color(255, 255, 255, int(255 * 0.9 * self._fade_filter.x))) if self.effective_active: - active_amount = self._color_fade_filter.update(0.0 if self._awareness_unfull else 1.0) source_rect = rl.Rectangle(0, 0, self._dm_cone.width, self._dm_cone.height) dest_rect = rl.Rectangle( self._rect.x + self._rect.width / 2, @@ -110,16 +104,13 @@ class DriverStateRenderer(Widget): ) if not self._lines: - r = int(round(ACTIVE_ACCENT.r * active_amount + CONE_COLOR_ORANGE[0] * (1 - active_amount))) - g = int(round(ACTIVE_ACCENT.g * active_amount + CONE_COLOR_ORANGE[1] * (1 - active_amount))) - b = int(round(ACTIVE_ACCENT.b * active_amount + CONE_COLOR_ORANGE[2] * (1 - active_amount))) rl.draw_texture_pro( self._dm_cone, source_rect, dest_rect, rl.Vector2(dest_rect.width / 2, dest_rect.height / 2), self._rotation_filter.x - 90, - rl.Color(r, g, b, int(255 * self._fade_filter.x)), + rl.Color(ACTIVE_ACCENT.r, ACTIVE_ACCENT.g, ACTIVE_ACCENT.b, int(255 * self._fade_filter.x)), ) else: @@ -159,12 +150,9 @@ class DriverStateRenderer(Widget): sm = ui_state.sm dm_state = sm["driverMonitoringState"] - self._is_active = dm_state.activePolicy == log.DriverMonitoringState.MonitoringPolicy.vision + self._is_active = dm_state.isActiveMode self._is_rhd = dm_state.isRHD - self._face_detected = dm_state.visionPolicyState.faceDetected - self._awareness_unfull = self.effective_active and dm_state.visionPolicyState.awarenessPercent < self.AWARENESS_UNFULL_PERCENT - self._face_pitch = dm_state.visionPolicyState.pose.pitch + math.radians(6) - self._face_yaw = -dm_state.visionPolicyState.pose.yaw + self._face_detected = dm_state.faceDetected driverstate = sm["driverStateV2"] driver_data = driverstate.rightDriverData if self._is_rhd else driverstate.leftDriverData @@ -172,9 +160,24 @@ class DriverStateRenderer(Widget): def _update_state(self): # Get monitoring state - _ = self.get_driver_data() - pitch = self._pitch_filter.update(self._face_pitch) - yaw = self._yaw_filter.update(self._face_yaw) + driver_data = self.get_driver_data() + driver_orient = driver_data.faceOrientation + + if len(driver_orient) != 3: + return + + # Calibrate orientation so looking straight ahead at the road (instead of at the device) reads + # (0, 0), using live calibration. Makes the cone point in the correct direction. (stock PR #37149) + sm = ui_state.sm + if sm.valid['liveCalibration'] and len(sm['liveCalibration'].rpyCalib) == 3: + cal_rpy = sm['liveCalibration'].rpyCalib + else: + cal_rpy = [0.0, 0.0, 0.0] + _, pitch, yaw = face_orientation_from_net(driver_orient, driver_data.facePosition, cal_rpy) + yaw = -yaw # undo sign flip in face_orientation_from_net to match UI convention + + pitch = self._pitch_filter.update(pitch) + yaw = self._yaw_filter.update(yaw) # hysteresis on looking center if abs(pitch) < LOOKING_CENTER_THRESHOLD_LOWER and abs(yaw) < LOOKING_CENTER_THRESHOLD_LOWER: @@ -195,8 +198,9 @@ class DriverStateRenderer(Widget): rl.draw_circle(int(pitch_x), 100, 5, rl.GREEN) rl.draw_circle(int(yaw_x), 120, 5, rl.GREEN) - # filter head rotation, handling wrap-around - rotation = math.degrees(math.atan2(pitch * 2, yaw)) + # filter head rotation, handling wrap-around (bias pitch up since calib/DM pose isn't exact, + # and halve yaw sensitivity) + rotation = math.degrees(math.atan2((pitch + math.radians(6)) * 2, yaw)) angle_diff = rotation - self._rotation_filter.x angle_diff = ((angle_diff + 180) % 360) - 180 self._rotation_filter.update(self._rotation_filter.x + angle_diff) diff --git a/selfdrive/ui/onroad/driver_state.py b/selfdrive/ui/onroad/driver_state.py index e34b2c1..39aa6d1 100644 --- a/selfdrive/ui/onroad/driver_state.py +++ b/selfdrive/ui/onroad/driver_state.py @@ -114,7 +114,7 @@ class DriverStateRenderer(Widget): # Get monitoring state dm_state = sm["driverMonitoringState"] - self.is_active = dm_state.activePolicy == log.DriverMonitoringState.MonitoringPolicy.vision + self.is_active = dm_state.isActiveMode self.is_rhd = dm_state.isRHD # Update fade state (smoother transition between active/inactive) diff --git a/selfdrive/ui/onroad/model_renderer.py b/selfdrive/ui/onroad/model_renderer.py index ec10c64..e979e73 100644 --- a/selfdrive/ui/onroad/model_renderer.py +++ b/selfdrive/ui/onroad/model_renderer.py @@ -12,7 +12,7 @@ from openpilot.system.ui.lib.application import gui_app from openpilot.system.ui.lib.shader_polygon import draw_polygon, Gradient from openpilot.system.ui.widgets import Widget -from openpilot.iqpilot.ui.onroad.model_renderer import ChevronMetrics, IQModelRenderer +from openpilot.iqpilot.ui.onroad.hud_overlays import ChevronMetrics from openpilot.iqpilot.ui.onroad.lead_confidence import driving_confidence CLIP_MARGIN = 500 @@ -68,10 +68,9 @@ class VisionDot: _VD_EASE = 0.4 -class ModelRenderer(Widget, IQModelRenderer): +class ModelRenderer(Widget): def __init__(self): Widget.__init__(self) - IQModelRenderer.__init__(self) self.chevron_metrics = ChevronMetrics() self._lead_orb = gui_app.texture("icons/lead_orb.png", 256, 256) self._longitudinal_control = False @@ -433,9 +432,6 @@ class ModelRenderer(Widget, IQModelRenderer): allow_throttle = sm['longitudinalPlan'].allowThrottle or not self._longitudinal_control self._blend_filter.update(int(allow_throttle)) - if ui_state.rainbow_path: - self.rainbow_path.draw_rainbow_path(self._rect, self._path) - return if self._experimental_mode: # Draw with acceleration coloring @@ -455,18 +451,19 @@ class ModelRenderer(Widget, IQModelRenderer): ) draw_polygon(self._rect, self._path.projected_points, gradient=gradient) + # concentric layers (outer faint -> inner bright) build a soft center-out glow using only + # draw_circle, which is signature-stable across raylib versions (draw_circle_gradient is not) + _VD_GLOW = ((1.0, 26), (0.72, 38), (0.48, 54), (0.26, 78)) + def _draw_vision_dots(self): for dot in self._vision_dots: a = dot.alpha if a <= 0.02: continue - x, y, r = int(dot.x), int(dot.y), dot.radius - if dot.rgb is None: - # soft teal glow, brighter in the center fading to transparent at the edge - rl.draw_circle_gradient(x, y, r, rl.Color(120, 235, 225, int(165 * a)), rl.Color(0, 150, 150, 0)) - else: - rl.draw_circle_gradient(x, y, r, rl.Color(dot.rgb[0], dot.rgb[1], dot.rgb[2], int(210 * a)), - rl.Color(dot.rgb[0], dot.rgb[1], dot.rgb[2], 0)) + x, y = int(dot.x), int(dot.y) + cr, cg, cb = (40, 210, 200) if dot.rgb is None else dot.rgb + for frac, base in self._VD_GLOW: + rl.draw_circle(x, y, dot.radius * frac, rl.Color(cr, cg, cb, int(base * a))) def _draw_track_dots(self): src = rl.Rectangle(0, 0, self._lead_orb.width, self._lead_orb.height) diff --git a/selfdrive/ui/soundd.py b/selfdrive/ui/soundd.py index 9d92e80..e956e73 100644 --- a/selfdrive/ui/soundd.py +++ b/selfdrive/ui/soundd.py @@ -45,28 +45,26 @@ sound_list_iq: dict[int, tuple[str, int | None, float]] = { AudibleAlertIQ.promptSingleHigh: ("prompt_single_high.wav", 1, MAX_VOLUME), } -def get_sound_list(device_type: str) -> dict[int, tuple[str, int | None, float]]: - sounds = { +sound_list: dict[int, tuple[str, int | None, float]] = { + # AudibleAlert, file name, play count (none for infinite) + AudibleAlert.engage: ("engage.wav", 1, MAX_VOLUME), + AudibleAlert.disengage: ("disengage.wav", 1, MAX_VOLUME), + AudibleAlert.refuse: ("refuse.wav", 1, MAX_VOLUME), + + AudibleAlert.prompt: ("prompt.wav", 1, MAX_VOLUME), + AudibleAlert.promptRepeat: ("prompt.wav", None, MAX_VOLUME), + AudibleAlert.promptDistracted: ("prompt_distracted.wav", None, MAX_VOLUME), + + AudibleAlert.warningSoft: ("warning_soft.wav", None, MAX_VOLUME), + AudibleAlert.warningImmediate: ("warning_immediate.wav", None, MAX_VOLUME), + + **sound_list_iq, +} +if HARDWARE.get_device_type() in ("tizi", "tici"): + sound_list.update({ AudibleAlert.engage: ("engage.wav", 1, MAX_VOLUME), AudibleAlert.disengage: ("disengage.wav", 1, MAX_VOLUME), - AudibleAlert.refuse: ("refuse.wav", 1, MAX_VOLUME), - AudibleAlert.prompt: ("prompt.wav", 1, MAX_VOLUME), - AudibleAlert.promptRepeat: ("prompt.wav", None, MAX_VOLUME), - AudibleAlert.promptDistracted: ("prompt_distracted.wav", None, MAX_VOLUME), - AudibleAlert.preAlert: ("pre_alert.wav", 1, MAX_VOLUME), - AudibleAlert.warningSoft: ("warning_soft.wav", None, MAX_VOLUME), - AudibleAlert.warningImmediate: ("warning_immediate.wav", None, MAX_VOLUME), - **sound_list_iq, - } - if device_type in ("tizi", "tici"): - sounds.update({ - AudibleAlert.engage: ("engage.wav", 1, MAX_VOLUME), - AudibleAlert.disengage: ("disengage.wav", 1, MAX_VOLUME), - }) - return sounds - - -sound_list = get_sound_list(HARDWARE.get_device_type()) + }) def check_selfdrive_timeout_alert(sm): ss_missing = time.monotonic() - sm.recv_time['selfdriveState'] diff --git a/selfdrive/ui/tests/test_soundd.py b/selfdrive/ui/tests/test_soundd.py index a1e18e8..a9da845 100644 --- a/selfdrive/ui/tests/test_soundd.py +++ b/selfdrive/ui/tests/test_soundd.py @@ -2,19 +2,13 @@ from cereal import car from cereal import messaging from cereal.messaging import SubMaster, PubMaster from openpilot.selfdrive.ui.soundd import SELFDRIVE_STATE_TIMEOUT, check_selfdrive_timeout_alert -from openpilot.selfdrive.ui.soundd import get_sound_list -import pytest import time AudibleAlert = car.CarControl.HUDControl.AudibleAlert class TestSoundd: - @pytest.mark.parametrize("device_type", ["mici", "tici", "tizi"]) - def test_prompt_distracted_sound(self, device_type): - assert get_sound_list(device_type)[AudibleAlert.promptDistracted][0] == "prompt_distracted.wav" - def test_check_selfdrive_timeout_alert(self): sm = SubMaster(['selfdriveState']) pm = PubMaster(['selfdriveState']) @@ -38,3 +32,4 @@ class TestSoundd: assert check_selfdrive_timeout_alert(sm) # TODO: add test with micd for checking that soundd actually outputs sounds + diff --git a/selfdrive/ui/ui_state.py b/selfdrive/ui/ui_state.py index f181763..c0cee13 100644 --- a/selfdrive/ui/ui_state.py +++ b/selfdrive/ui/ui_state.py @@ -142,15 +142,14 @@ class IQUIState: _PARAM_MIRROR = { "active_bundle": ("ModelManager_ActiveBundle", "raw"), - "blindspot": ("BlindSpot", "bool"), - "chevron_metrics": ("ChevronInfo", "raw"), + "blindspot": ("IQBlindSpotAlerts", "bool"), + "chevron_metrics": ("IQLeadReadouts", "raw"), "developer_ui": ("IQDevUIInfo", "raw"), "night_mode": ("NightMode", "bool"), - "rainbow_path": ("RainbowMode", "bool"), - "road_name_toggle": ("RoadNameToggle", "bool"), - "rocket_fuel": ("RocketFuel", "bool"), - "torque_bar": ("TorqueBar", "bool"), - "turn_signals": ("ShowTurnSignals", "bool"), + "road_name_toggle": ("IQRoadNameOverlay", "bool"), + "rocket_fuel": ("IQAccelMeter", "bool"), + "torque_bar": ("IQSteerEffortArc", "bool"), + "turn_signals": ("IQBlinkerIndicators", "bool"), "custom_interactive_timeout": ("InteractivityTimeout", "default"), "onroad_brightness_timer_param": ("OnroadScreenOffTimer", "default"), "speed_limit_mode": ("IQSpeedAssistMode", "default"), diff --git a/system/loggerd/loggerd.h b/system/loggerd/loggerd.h index e8c8c04..0b440db 100644 --- a/system/loggerd/loggerd.h +++ b/system/loggerd/loggerd.h @@ -28,12 +28,17 @@ const int SEGMENT_LENGTH = LOGGERD_TEST ? atoi(getenv("LOGGERD_SEGMENT_LENGTH")) constexpr char PRESERVE_ATTR_NAME[] = "user.preserve"; constexpr char PRESERVE_ATTR_VALUE = '1'; -// 2.5x the stock 526x330 qcamera, rounded up to even. The msm_vidc encoder rejects -// VIDIOC_S_FMT with ENOTSUPP (524) on an odd width or height, which throws out of -// encoder_thread and SIGABRTs all of encoderd -- taking fcamera/dcamera/ecamera with it. constexpr int QCAM_WIDTH = 1316; constexpr int QCAM_HEIGHT = 826; -static_assert(QCAM_WIDTH % 2 == 0 && QCAM_HEIGHT % 2 == 0, "qcamera dimensions must be even"); +constexpr int MICI_QCAM_WIDTH = 1210; +constexpr int MICI_QCAM_HEIGHT = 760; +static_assert(QCAM_WIDTH % 2 == 0 && QCAM_HEIGHT % 2 == 0 && + MICI_QCAM_WIDTH % 2 == 0 && MICI_QCAM_HEIGHT % 2 == 0, + "qcamera dimensions must be even"); + +inline bool is_mici() { + return Hardware::get_device_type() == cereal::InitData::DeviceType::MICI; +} struct EncoderSettings { cereal::EncodeIndex::Type encode_type; @@ -147,8 +152,8 @@ const EncoderInfo qcam_encoder_info = { .filename = "qcamera.ts", .cbr = true, // enforce the bitrate so upload size stays predictable (no VBR overshoot) .get_settings = [](int){return EncoderSettings::QcamEncoderSettings();}, - .frame_width = QCAM_WIDTH, - .frame_height = QCAM_HEIGHT, + .frame_width = is_mici() ? MICI_QCAM_WIDTH : QCAM_WIDTH, + .frame_height = is_mici() ? MICI_QCAM_HEIGHT : QCAM_HEIGHT, .include_audio = Params().getBool("RecordAudio"), INIT_ENCODE_FUNCTIONS(QRoadEncode), }; diff --git a/system/manager/process.py b/system/manager/process.py index 1314b11..152d8f8 100644 --- a/system/manager/process.py +++ b/system/manager/process.py @@ -154,7 +154,7 @@ class ManagerProcess(ABC): class NativeProcess(ManagerProcess): - def __init__(self, name, cwd, cmdline, should_run, enabled=True, sigkill=False): + def __init__(self, name, cwd, cmdline, should_run, enabled=True, sigkill=False, restart_if_crash=False): self.name = name self.cwd = cwd self.cmdline = cmdline @@ -162,6 +162,7 @@ class NativeProcess(ManagerProcess): self.enabled = enabled self.sigkill = sigkill self.launcher = nativelauncher + self.restart_if_crash = restart_if_crash def prepare(self) -> None: pass diff --git a/system/manager/process_config.py b/system/manager/process_config.py index 81518cb..3421565 100644 --- a/system/manager/process_config.py +++ b/system/manager/process_config.py @@ -79,7 +79,7 @@ def navrenderd_onroad(started: bool, params: Params, CP: car.CarParams) -> bool: def iqmapd_needed(params: Params) -> bool: return ( - params.get_bool("RoadNameToggle") + params.get_bool("IQRoadNameOverlay") or params.get_bool("ShowSpeedLimits") or params.get_bool("SpeedLimitController") or params.get_bool("EnableSpeedLimitControl") @@ -200,7 +200,7 @@ procs += [ BundleProcess("iqvd", "iqpilot_iqvd_private", "iqpilot_private.iqvd.iqvd", iqvd_onroad, restart_if_crash=True), # mapd - NativeProcess("mapd", "third_party/mapd_pfeiferj", ["./mapd"], mapd_onroad), + NativeProcess("mapd", "third_party/mapd_pfeiferj", ["./mapd"], mapd_onroad, restart_if_crash=True), PythonProcess("mapd_manager", "iqpilot.iq_maps.orchestrator", and_(only_offroad, not_low_power)), # locationd diff --git a/tools/iqpilot/mici_preview.py b/tools/iqpilot/mici_preview.py index d77fbf4..aef1f09 100755 --- a/tools/iqpilot/mici_preview.py +++ b/tools/iqpilot/mici_preview.py @@ -145,12 +145,12 @@ def _patch_mock_state(): mp.put("IQLaneChangeBsmDelay", False) # ── Visuals (correct param keys matching visuals.py) ───────────────────── - mp.put("BlindSpot", True) - mp.put("TorqueBar", True) - mp.put("RoadNameToggle", True) - mp.put("ShowTurnSignals", True) - mp.put("RocketFuel", False) - mp.put("ChevronInfo", 0) # 0=off + mp.put("IQBlindSpotAlerts", True) + mp.put("IQSteerEffortArc", True) + mp.put("IQRoadNameOverlay", True) + mp.put("IQBlinkerIndicators", True) + mp.put("IQAccelMeter", False) + mp.put("IQLeadReadouts", 0) # 0=off mp.put("IQDevUIInfo", 0) # 0=off mp.put("AlphaLongitudinalEnabled", False) # real param; gates ChevronInfo diff --git a/tools/lateral_maneuvers/README.md b/tools/lateral_maneuvers/README.md index 78e7b36..2802b74 100644 --- a/tools/lateral_maneuvers/README.md +++ b/tools/lateral_maneuvers/README.md @@ -7,7 +7,7 @@ Test your vehicle's lateral control tuning with this tool. The tool will test th ## Instructions -1. Check out a development branch such as `master-mici` on your comma device. +1. Check out a development branch such as `master-mici` on your device. The toggle is hidden on release branches. 2. The full maneuver suite runs at 20 and 30 mph. 3. Enable "Lateral Maneuver Mode" in Settings > Developer on the device while offroad. Alternatively, set the parameter manually: @@ -17,7 +17,7 @@ Test your vehicle's lateral control tuning with this tool. The tool will test th 4. Turn your vehicle back on. You will see "Lateral Maneuver Mode". -5. Ensure the area ahead is clear, as iqpilot will command lateral acceleration steps in this mode. Once you are ready, set ACC manually to the target speed shown on screen and let iqpilot stabilize lateral. After 2 seconds of steady straight driving, the maneuver will begin automatically. iqpilot lateral control stays engaged between maneuvers normally while waiting for the next maneuver's readiness conditions. The maneuver will be aborted and repeated if speed is out of range, steering is touched or iqpilot disengages. +5. Ensure the area ahead is clear, as IQ.Pilot will command lateral acceleration steps in this mode. Once you are ready, set ACC manually to the target speed shown on screen and let IQ.Pilot stabilize lateral. After 2 seconds of steady straight driving on a road under 250 m radius and under 6.8° of roll, the maneuver will begin automatically. IQ.Pilot lateral control stays engaged between maneuvers normally while waiting for the next maneuver's readiness conditions. The maneuver will be aborted and repeated if speed is out of range, the steering wheel or gas is touched, or IQ.Pilot disengages. 6. When the testing is complete, you'll see an alert that says "Maneuvers Finished." Complete the route by pulling over and turning off the vehicle. @@ -37,4 +37,18 @@ Test your vehicle's lateral control tuning with this tool. The tool will test th Opening report: tools/lateral_maneuvers/lateral_reports/KIA_EV6_98395b7c5b27882e_000001cc--5a73bde686.html ``` - The iqpilot `generate_report.py` also supports auto-detection of lateral sweeps in any route without `alertDebug` markers (pass `--auto`), and ranks the top-N highest-peak sweeps by speed/peak filters. See `generate_report.py --help`. + The IQ.Pilot `generate_report.py` also takes a path to a local `rlog.zst` or a directory of them, supports + auto-detection of lateral sweeps in any route without `alertDebug` markers (pass `--auto`), and ranks the + top-N highest-peak sweeps by speed/peak filters. See `generate_report.py --help`. + +## Testing the tooling without a car + +`sim_maneuvers.py` runs `lateral_maneuversd` as a real process against a synthetic steering rack and writes an +rlog that `generate_report.py` reads. Use it to verify the daemon and the report generator after changing either: + +```sh +$ python tools/lateral_maneuvers/sim_maneuvers.py --out /tmp/lat/rlog.zst +$ python tools/lateral_maneuvers/generate_report.py /tmp/lat/rlog.zst +``` + +The full suite takes about 5 minutes of wall clock; `--max-maneuvers N` stops early. diff --git a/tools/lateral_maneuvers/generate_report.py b/tools/lateral_maneuvers/generate_report.py index fbfb43f..c122203 100644 --- a/tools/lateral_maneuvers/generate_report.py +++ b/tools/lateral_maneuvers/generate_report.py @@ -1,376 +1,261 @@ #!/usr/bin/env python3 -"""Lateral maneuver compliance report — analog of tools/longitudinal_maneuvers/generate_report.py. - -Produces the "sine 0.5 Hz 30 mph" / "50% peak crossed in X.XXXs" style HTML report comma posts -on social media for steering-rack compliance comparisons. Reads any iqpilot/openpilot rlog -route, slices it into lateral maneuver windows (either by `alertDebug` markers from a scripted -maneuversd run, or by auto-detection of contiguous lat-active sweeps), and emits a 4-panel -plot per run: - - 1. Lateral accel (desired + actual, m/s²) on left axis and steering-wheel angle (deg) on - right axis. Black circle marks the time the actual lat-accel first crosses 50 % of the - desired peak in the same direction. - 2. Vehicle speed (mph) - 3. Lateral jerk (m/s³), numerically differentiated from actual lat-accel - 4. Roll (deg) from liveParameters - -Usage: - python tools/lateral_maneuvers/generate_report.py [description] - -Examples: - python tools/lateral_maneuvers/generate_report.py 1ce1b50dd82993a1\\|0000003b--a389fbdf35 - python tools/lateral_maneuvers/generate_report.py /path/to/local/rlog.zst "sine 0.5Hz 30mph" -""" import argparse import base64 import io import math +import numpy as np import os -import pprint import webbrowser from collections import defaultdict from pathlib import Path - import matplotlib.pyplot as plt -import numpy as np -from tabulate import tabulate +from openpilot.common.utils import tabulate +from cereal import car +from openpilot.common.filter_simple import FirstOrderFilter +from openpilot.selfdrive.controls.lib.latcontrol_torque import LP_FILTER_CUTOFF_HZ from openpilot.tools.lib.logreader import LogReader from openpilot.system.hardware.hw import Paths +from openpilot.common.constants import CV +from openpilot.tools.longitudinal_maneuvers.generate_report import format_car_params + +ANGLE_CONTROL = (car.CarParams.SteerControlType.angle, car.CarParams.SteerControlType.curvatureDEPRECATED) -MPS_TO_MPH = 2.23693629 -AUTO_DESIRED_LAT_ACCEL_THRESHOLD = 0.5 -AUTO_MIN_V_EGO = 5.0 -AUTO_MIN_DURATION_S = 1.5 -AUTO_GAP_S = 0.5 +def lat_accel(curvature, v): + return curvature * max(v, 1.0) ** 2 -def format_car_params(CP): - return pprint.pformat({k: v for k, v in CP.to_dict().items() if not k.endswith("DEPRECATED")}, indent=2) - - -def _series(msgs, which): - rows = [(m.logMonoTime, getattr(m, which)) for m in msgs if m.which() == which] - if not rows: - return [], [] - t, v = zip(*rows, strict=True) - return list(t), list(v) - - -def _to_relative_seconds(t_ns, t0): - return [(t - t0) / 1e9 for t in t_ns] - - -def _resample(t_src, v_src, t_dst): - if not t_src or not t_dst: - return np.zeros(len(t_dst)) - return np.interp(t_dst, t_src, v_src) - - -def _peak_crossing_time(t, desired, actual, fraction=0.5): - if len(desired) == 0: - return None, 0.0 - desired = np.asarray(desired) - actual = np.asarray(actual) - peak_idx = int(np.argmax(np.abs(desired))) - peak = desired[peak_idx] - if abs(peak) < 1e-3: - return None, peak - target = fraction * peak - prev = False - for i in range(peak_idx + 1): - crossed = (target > 0 and actual[i] >= target) or (target < 0 and actual[i] <= target) - if crossed and prev: - return t[i], peak - prev = crossed - return None, peak - - -def _slice_by_alert_debug(msgs): - out = [] - active_prev = False - description_prev = None - for msg in msgs: - if msg.which() == "alertDebug": - # Match both the longitudinal daemon ("Maneuver Active: …") and the lateral daemon - # ("Active sine …", "Active +0.5m/s² …", "Complete"). - text1 = msg.alertDebug.alertText1 - active = "Maneuver Active" in text1 or text1.startswith("Active") or text1 == "Complete" - if active and not active_prev: - if msg.alertDebug.alertText2 == description_prev: - out[-1][1].append([]) - else: - out.append((msg.alertDebug.alertText2, [[]])) - description_prev = out[-1][0] - active_prev = active - if active_prev: - out[-1][1][-1].append(msg) - return out - - -def _slice_auto(msgs): - t_cc, cc_vals = _series(msgs, "carControl") - t_cs, cs_vals = _series(msgs, "carState") - if not t_cc or not t_cs: - return [] - - v_ego = np.asarray([m.vEgo for m in cs_vals]) - curvature = np.asarray([m.actuators.curvature for m in cc_vals]) - lat_active = np.asarray([1.0 if m.latActive else 0.0 for m in cc_vals]) - - t_cc_s = np.asarray([(t - t_cc[0]) / 1e9 for t in t_cc]) - t_cs_s = np.asarray([(t - t_cc[0]) / 1e9 for t in t_cs]) - v_at_cc = np.interp(t_cc_s, t_cs_s, v_ego) - desired_lat_accel = curvature * v_at_cc ** 2 - signal = np.abs(desired_lat_accel) * lat_active * (v_at_cc > AUTO_MIN_V_EGO).astype(float) - - cycle_dt = float(np.median(np.diff(t_cc_s))) if len(t_cc_s) > 1 else 0.01 - min_frames = max(1, int(AUTO_MIN_DURATION_S / cycle_dt)) - - windows = [] - start = None - for i, s in enumerate(signal): - if s > AUTO_DESIRED_LAT_ACCEL_THRESHOLD and start is None: - start = i - elif s <= AUTO_DESIRED_LAT_ACCEL_THRESHOLD and start is not None: - if i - start > min_frames: - windows.append((t_cc[start], t_cc[i])) - start = None - if start is not None and len(signal) - start > min_frames: - windows.append((t_cc[start], t_cc[-1])) - - merged = [] - for a, b in windows: - if merged and (a - merged[-1][1]) / 1e9 < AUTO_GAP_S: - merged[-1] = (merged[-1][0], b) - else: - merged.append((a, b)) - - runs = [] - for a, b in merged: - runs.append([m for m in msgs if a <= m.logMonoTime <= b]) - if not runs: - return [] - return [("auto-detected lateral sweep", runs)] - - -def _plot_run(description, run_idx, msgs, builder, target_cross_times): - t_cc, carControl = _series(msgs, "carControl") - t_cs, carState = _series(msgs, "carState") - t_lp, livePose = _series(msgs, "livePose") - - if not (t_cc and t_cs and t_lp): - builder.append(f"

Run #{run_idx + 1}: missing required data, skipping.

\n") - return - - t0 = min(t_cc[0], t_cs[0], t_lp[0]) - t_cc_s = _to_relative_seconds(t_cc, t0) - t_cs_s = _to_relative_seconds(t_cs, t0) - t_lp_s = _to_relative_seconds(t_lp, t0) - - v_ego = np.asarray([m.vEgo for m in carState]) - steer = np.asarray([m.steeringAngleDeg for m in carState]) - - curvature = np.asarray([m.actuators.curvature for m in carControl]) - v_at_cc = _resample(t_cs_s, v_ego, t_cc_s) - desired_lat_accel = curvature * v_at_cc ** 2 - - actual_lat_accel = np.asarray([m.accelerationDevice.y for m in livePose]) - jerk = np.gradient(actual_lat_accel, t_lp_s) - - t_lpar, liveParameters = _series(msgs, "liveParameters") - if liveParameters: - t_lpar_s = _to_relative_seconds(t_lpar, t0) - roll_deg = np.asarray([math.degrees(m.roll) for m in liveParameters]) - else: - t_lpar_s = [] - roll_deg = np.asarray([]) - - desired_lat_accel_on_lp = _resample(t_cc_s, desired_lat_accel, t_lp_s) - cross_time, peak = _peak_crossing_time(t_lp_s, desired_lat_accel_on_lp, actual_lat_accel, fraction=0.5) - - title = f"Run #{run_idx + 1}" - builder.append(f"

{title}

\n") - if cross_time is not None: - builder.append(f"

50% peak, crossed in {cross_time:.3f}s

\n") - target_cross_times[description].append(cross_time) - else: - builder.append("

50% peak, not crossed

\n") - builder.append(f"

Peak desired lat accel: {peak:+.2f} m/s², " - f"avg speed: {np.mean(v_ego) * MPS_TO_MPH:.1f} mph

\n") - - plt.rcParams["font.size"] = 32 - fig = plt.figure(figsize=(28, 22)) - ax = fig.subplots(4, 1, sharex=True, gridspec_kw={"height_ratios": [5, 2, 2, 2]}) - - ax_la = ax[0] - ax_la.grid(linewidth=2) - ax_la.plot(t_cc_s, desired_lat_accel, label="desired lat accel", linewidth=4) - ax_la.plot(t_lp_s, actual_lat_accel, label="actual lat accel", linewidth=4) - ax_la.set_ylabel("Lateral Accel (m/s²)") - - ax_st = ax_la.twinx() - ax_st.plot(t_cs_s, steer, color="tab:green", label="steer angle", linewidth=4) - ax_st.set_ylabel("Steering Angle (deg)") - - lines_l, labels_l = ax_la.get_legend_handles_labels() - lines_r, labels_r = ax_st.get_legend_handles_labels() - ax_la.legend(lines_l + lines_r, labels_l + labels_r, loc="upper right", prop={"size": 22}) - - if cross_time is not None: - cross_val = float(np.interp(cross_time, t_lp_s, actual_lat_accel)) - ax_la.plot(cross_time, cross_val, marker="o", markersize=30, markeredgewidth=4, - markeredgecolor="black", markerfacecolor="None") - - ax[1].grid(linewidth=2) - ax[1].plot(t_cs_s, v_ego * MPS_TO_MPH, color="tab:blue", label="vEgo", linewidth=4) - ax[1].set_ylabel("Velocity (mph)") - ax[1].legend(loc="upper right", prop={"size": 22}) - - ax[2].grid(linewidth=2) - ax[2].plot(t_lp_s, jerk, color="tab:blue", label="actual jerk", linewidth=4) - ax[2].set_ylabel("Jerk (m/s³)") - ax[2].legend(loc="upper left", prop={"size": 22}) - - ax[3].grid(linewidth=2) - if len(roll_deg): - ax[3].plot(t_lpar_s, roll_deg, color="tab:blue", label="roll", linewidth=4) - ax[3].set_ylabel("Roll (deg)") - ax[3].legend(loc="upper right", prop={"size": 22}) - - ax[-1].set_xlabel("Time (s)") - fig.tight_layout() - - buffer = io.BytesIO() - fig.savefig(buffer, format="webp") - plt.close(fig) - buffer.seek(0) - builder.append(f"\n") - builder.append("
\n") - - -def report(platform, route, description, CP, ID, maneuvers): +def report(platform, route, _description, CP, ID, maneuvers): output_path = Path(__file__).resolve().parent / "lateral_reports" + output_fn = output_path / f"{platform}_{route.replace('/', '_').replace('|', '_')}.html" output_path.mkdir(exist_ok=True) - safe_route = route.replace("/", "_").replace("|", "_") - output_fn = output_path / f"{platform}_{safe_route}.html" - target_cross_times = defaultdict(list) builder = [ - "\n", + "\n", "

Lateral maneuver report

\n", f"

{platform}

\n", f"

{route}

\n", f"

{ID.gitCommit}, {ID.gitBranch}, {ID.gitRemote}

\n", ] - if description is not None: - builder.append(f"

Description: {description}

\n") - builder.append(f"

CarParams

{format_car_params(CP)}
\n") - builder.append("{ summary }") + if _description is not None: + builder.append(f"

Description: {_description}

\n") + builder.append(f"

CarParams

{format_car_params(CP)}
\n") + builder.append('{ summary }') # to be replaced below + for description, runs in maneuvers: + # filter incomplete runs + completed_runs = [msgs for msgs in runs + if any(m.alertDebug.alertText1 == 'Complete' for m in msgs if m.which() == 'alertDebug')] + print(f'plotting maneuver: {description}, runs: {len(completed_runs)}') + if not completed_runs: + continue + builder.append("
\n") + builder.append(f"

{description}

\n") + for run, msgs in enumerate(completed_runs): + last_active = max(m.logMonoTime for m in msgs if m.which() == 'lateralManeuverPlan' and m.valid) + msgs = [m for m in msgs if m.logMonoTime <= last_active] + t_carControl, carControl = zip(*[(m.logMonoTime, m.carControl) for m in msgs if m.which() == 'carControl'], strict=True) + t_carState, carState = zip(*[(m.logMonoTime, m.carState) for m in msgs if m.which() == 'carState'], strict=True) + t_controlsState, controlsState = zip(*[(m.logMonoTime, m.controlsState) for m in msgs if m.which() == 'controlsState'], strict=True) + t_lateralPlan, lateralPlan = zip(*[(m.logMonoTime, m.lateralManeuverPlan) for m in msgs if m.which() == 'lateralManeuverPlan' and m.valid], strict=True) + t_carOutput, carOutput = zip(*[(m.logMonoTime, m.carOutput) for m in msgs if m.which() == 'carOutput'], strict=True) - for maneuver_description, runs in maneuvers: - print(f"plotting maneuver: {maneuver_description}, runs: {len(runs)}") - builder.append("
\n") - builder.append(f"

{maneuver_description}

\n") - for run_idx, msgs in enumerate(runs): - _plot_run(maneuver_description, run_idx, msgs, builder, target_cross_times) + # make time relative seconds + t_carControl = [(t - t_carControl[0]) / 1e9 for t in t_carControl] + t_carState = [(t - t_carState[0]) / 1e9 for t in t_carState] + t_controlsState = [(t - t_controlsState[0]) / 1e9 for t in t_controlsState] + t_lateralPlan = [(t - t_lateralPlan[0]) / 1e9 for t in t_lateralPlan] + t_carOutput = [(t - t_carOutput[0]) / 1e9 for t in t_carOutput] + + # maneuver validity + latActive = [m.latActive for m in carControl] + maneuver_valid = all(latActive) and not any(cs.steeringPressed for cs in carState) + + _open = 'open' if maneuver_valid else '' + title = f'Run #{int(run)+1}' + (' (invalid maneuver!)' if not maneuver_valid else '') + + builder.append(f"

{title}

\n") + + baseline_accel = lat_accel(controlsState[0].curvature, carState[0].vEgo) + v_ego = [m.vEgo for m in carState] + cross_markers = [] + + if description.startswith(('sine', 'jitter')): + amplitude = max(abs(lat_accel(lp.desiredCurvature, v) - baseline_accel) + for lp, v in zip(lateralPlan, v_ego, strict=False)) + threshold = amplitude * 0.5 + builder.append('

50% peak') + for t, cs, v in zip(t_controlsState, controlsState, v_ego, strict=False): + actual = lat_accel(cs.curvature, v) - baseline_accel + if abs(actual) > threshold: + builder.append(f', crossed in {t:.3f}s') + cross_markers.append((t, actual + baseline_accel)) + if maneuver_valid: + target_cross_times[description].append(t) + break + else: + builder.append(', not crossed') + builder.append('

') + if maneuver_valid: + target_cross_times.setdefault(description, []) + else: + action_targets = [(0, lat_accel(lateralPlan[0].desiredCurvature, v_ego[0]) - baseline_accel)] + for i in range(1, min(len(lateralPlan), len(v_ego))): + if abs(lateralPlan[i].desiredCurvature - lateralPlan[i - 1].desiredCurvature) > 0.001: + desired = lat_accel(lateralPlan[i].desiredCurvature, v_ego[i]) - baseline_accel + action_targets.append((i, desired)) + + for j, (start_i, act_target) in enumerate(action_targets): + start_time = t_lateralPlan[start_i] + end_time = t_lateralPlan[action_targets[j + 1][0]] if j + 1 < len(action_targets) else t_controlsState[-1] + + builder.append(f'

aTarget: {round(act_target, 1)} m/s^2') + prev_crossed = False + for t, cs, v in zip(t_controlsState, controlsState, v_ego, strict=False): + if not (start_time <= t <= end_time): + continue + actual_accel = lat_accel(cs.curvature, v) - baseline_accel + crossed = (0 < act_target < actual_accel) or (0 > act_target > actual_accel) + if crossed and prev_crossed: + cross_time = t - start_time + builder.append(f', crossed in {cross_time:.3f}s') + cross_markers.append((t, act_target + baseline_accel)) + if maneuver_valid: + target_cross_times[description].append(cross_time) + break + prev_crossed = crossed + else: + builder.append(', not crossed') + builder.append('

') + if maneuver_valid: + target_cross_times.setdefault(description, []) + + plt.rcParams['font.size'] = 40 + fig = plt.figure(figsize=(30, 40)) + ax = fig.subplots(5, 1, sharex=True, gridspec_kw={'height_ratios': [5, 5, 3, 3, 3]}) + + ax[0].grid(linewidth=4) + desired_label = 'lateralManeuverPlan.desiredCurvature * vEgo^2' + desired_lat_accel = [lat_accel(m.desiredCurvature, v) for m, v in zip(lateralPlan, v_ego, strict=False)] + if description.startswith(('sine', 'jitter')): + ax[0].plot(t_lateralPlan[:len(desired_lat_accel)], desired_lat_accel, 'C1', label=desired_label, linewidth=6) + else: + t_desired = [t_lateralPlan[0]] + t_lateralPlan[:len(desired_lat_accel)] + desired_lat_accel = [baseline_accel] + desired_lat_accel + ax[0].step(t_desired, desired_lat_accel, 'C1', label=desired_label, linewidth=6, where='post') + actual_lat_accel = [lat_accel(cs.curvature, v) for cs, v in zip(controlsState, v_ego, strict=False)] + ax[0].plot(t_controlsState[:len(actual_lat_accel)], actual_lat_accel, 'g', label='controlsState.curvature * vEgo^2', linewidth=6) + ax[0].set_ylabel('Lateral Accel (m/s^2)') + for ct, cv in cross_markers: + ax[0].plot(ct, cv, marker='o', markersize=50, markeredgewidth=7, markeredgecolor='black', markerfacecolor='None') + ax[0].legend(prop={'size': 30}) + + ax[1].grid(linewidth=4) + if CP.steerControlType in ANGLE_CONTROL: + steer_field, steer_ylabel = 'steeringAngleDeg', 'Steer angle (deg)' + else: + steer_field, steer_ylabel = 'torque', 'Steer torque' + ax[1].plot(t_carControl, [getattr(m.actuators, steer_field) for m in carControl], 'C1', label=f'carControl.actuators.{steer_field}', linewidth=6) + ax[1].plot(t_carOutput, [getattr(m.actuatorsOutput, steer_field) for m in carOutput], 'g', label=f'carOutput.actuatorsOutput.{steer_field}', linewidth=6) + ax[1].set_ylabel(steer_ylabel) + ax[1].legend(prop={'size': 30}) + + ax[2].grid(linewidth=4) + ax[2].plot(t_carState, [v * CV.MS_TO_MPH for v in v_ego], label='carState.vEgo', linewidth=6) + ax[2].set_ylabel('Velocity (mph)') + ax[2].yaxis.set_major_formatter(plt.FormatStrFormatter('%.1f')) + ax[2].legend() + + t_accel = np.array(t_controlsState[:len(actual_lat_accel)]) + raw_jerk = np.gradient(actual_lat_accel, t_accel) + dt_avg = np.mean(np.diff(t_accel)) + jerk_filter = FirstOrderFilter(0.0, 1 / (2 * np.pi * LP_FILTER_CUTOFF_HZ), dt_avg) + filtered_jerk = [jerk_filter.update(j) for j in raw_jerk] + ax[3].grid(linewidth=4) + ax[3].plot(t_accel, filtered_jerk, label='d/dt(controlsState.curvature * vEgo^2)', linewidth=6) + ax[3].set_ylabel('Jerk (m/s^3)') + ax[3].legend() + + ax[4].grid(linewidth=4) + ax[4].plot(t_carControl, [math.degrees(m.orientationNED[0]) if len(m.orientationNED) == 3 else 0.0 for m in carControl], + label='carControl.orientationNED[0]', linewidth=6) + ax[4].set_ylabel('Roll (deg)') + ax[4].legend() + + ax[-1].set_xlabel("Time (s)") + fig.tight_layout() + + buffer = io.BytesIO() + fig.savefig(buffer, format='webp') + plt.close(fig) + buffer.seek(0) + builder.append(f"\n") + builder.append("
\n") summary = ["

Summary

\n"] - cols = ["maneuver", "crossed", "runs", "mean (s)", "min (s)", "max (s)"] + cols = ['maneuver', 'crossed', 'mean', 'min', 'max'] table = [] - for maneuver_description, runs in maneuvers: - times = target_cross_times[maneuver_description] - row = [maneuver_description, len(times), len(runs)] - if times: - row.extend([round(np.mean(times), 3), round(np.min(times), 3), round(np.max(times), 3)]) - table.append(row) - summary.append(tabulate(table, headers=cols, tablefmt="html", numalign="left") + "\n") + for description, times in target_cross_times.items(): + l = [description, len(times)] + if len(times): + l.extend([round(sum(times) / len(times), 2), round(min(times), 2), round(max(times), 2)]) + table.append(l) + summary.append(tabulate(table, headers=cols, tablefmt='html', numalign='left') + '\n') - sum_idx = builder.index("{ summary }") + sum_idx = builder.index('{ summary }') builder[sum_idx:sum_idx + 1] = summary with open(output_fn, "w") as f: - f.write("".join(builder)) + f.write(''.join(builder)) + print(f"\nOpening report: {output_fn}\n") webbrowser.open_new_tab(str(output_fn)) -def _rank_runs(runs, top_n, min_vego_mph, min_peak): - scored = [] - for r in runs: - t_cc, cc = _series(r, "carControl") - t_cs, cs = _series(r, "carState") - if not (t_cc and t_cs): - continue - v_ego = np.mean([m.vEgo for m in cs]) * MPS_TO_MPH - curv = np.asarray([m.actuators.curvature for m in cc]) - v_at_cc = np.interp([(t - t_cc[0]) / 1e9 for t in t_cc], - [(t - t_cc[0]) / 1e9 for t in t_cs], - [m.vEgo for m in cs]) - peak = float(np.max(np.abs(curv * v_at_cc ** 2))) - if v_ego < min_vego_mph or peak < min_peak: - continue - scored.append((peak, r)) - scored.sort(key=lambda x: -x[0]) - return [r for _, r in scored[:top_n]] if top_n > 0 else [r for _, r in scored] +def open_route(route: str) -> LogReader: + if os.path.isdir(route): + rlogs = sorted(str(p) for p in Path(route).glob("*rlog.zst")) + if not rlogs: + raise SystemExit(f"no *rlog.zst files in {route}") + print(f"loading {len(rlogs)} rlogs from {route}") + return LogReader(rlogs, only_union_types=True) + if os.path.exists(route) or '/' in route or '|' in route: + return LogReader(route, only_union_types=True) + segs = [seg for seg in os.listdir(Paths.log_root()) if route in seg] + return LogReader([os.path.join(Paths.log_root(), seg, 'rlog.zst') for seg in segs], only_union_types=True) -def main(): - parser = argparse.ArgumentParser(description="Generate lateral maneuver compliance report from a route") - parser.add_argument("route", type=str, help="Route name, segment range, local rlog path, or directory of rlogs") - parser.add_argument("description", type=str, nargs="?") - parser.add_argument("--auto", action="store_true", - help="Auto-detect lateral sweeps instead of relying on alertDebug 'Maneuver Active' markers") - parser.add_argument("--top-n", type=int, default=10, - help="Plot only the N largest-peak sweeps (0 = all). Default 10.") - parser.add_argument("--min-vego-mph", type=float, default=15.0, - help="Drop sweeps below this average speed. Default 15 mph.") - parser.add_argument("--min-peak", type=float, default=0.5, - help="Drop sweeps with peak desired lat accel below this (m/s²). Default 0.5.") +if __name__ == '__main__': + parser = argparse.ArgumentParser(description='Generate lateral maneuver report from route') + parser.add_argument('route', type=str, help='Route name, local rlog path, or directory of rlogs') + parser.add_argument('description', type=str, nargs='?') + args = parser.parse_args() - if os.path.isdir(args.route): - rlogs = sorted(p for p in Path(args.route).glob("*rlog.zst")) - if not rlogs: - raise SystemExit(f"no *rlog.zst files in {args.route}") - print(f"loading {len(rlogs)} rlogs from {args.route}") - lr = LogReader([str(p) for p in rlogs]) - elif os.path.exists(args.route): - lr = LogReader(args.route) - elif "/" in args.route or "|" in args.route: - lr = LogReader(args.route) - else: - segs = [seg for seg in os.listdir(Paths.log_root()) if args.route in seg] - lr = LogReader([os.path.join(Paths.log_root(), seg, "rlog.zst") for seg in segs]) + lr = open_route(args.route) - msgs = list(lr) - CP = next(m.carParams for m in msgs if m.which() == "carParams") - ID = next(m.initData for m in msgs if m.which() == "initData") + CP = lr.first('carParams') + ID = lr.first('initData') platform = CP.carFingerprint - print("processing report for", platform) + print('processing report for', platform) - maneuvers = [] if args.auto else _slice_by_alert_debug(msgs) - if not maneuvers: - print("no alertDebug 'Maneuver Active' windows found; auto-detecting lateral sweeps") - maneuvers = _slice_auto(msgs) + maneuvers: list[tuple[str, list[list]]] = [] + active_prev = False + description_prev = None - if not maneuvers: - print("no lateral maneuvers detected — treating the whole route as one run") - maneuvers = [("full route", [msgs])] - else: - filtered = [] - for description, runs in maneuvers: - kept = _rank_runs(runs, args.top_n, args.min_vego_mph, args.min_peak) - print(f" {description}: {len(runs)} candidate sweeps → {len(kept)} after rank/filter") - if kept: - filtered.append((description, kept)) - maneuvers = filtered or [("filtered out", [])] + for msg in lr: + if msg.which() == 'alertDebug': + active = 'Active' in msg.alertDebug.alertText1 or msg.alertDebug.alertText1 == 'Complete' + if active and not active_prev: + if msg.alertDebug.alertText2 == description_prev: + maneuvers[-1][1].append([]) + else: + maneuvers.append((msg.alertDebug.alertText2, [[]])) + description_prev = maneuvers[-1][0] + active_prev = active + + if active_prev: + maneuvers[-1][1][-1].append(msg) report(platform, args.route, args.description, CP, ID, maneuvers) - - -if __name__ == "__main__": - main() diff --git a/tools/lateral_maneuvers/lateral_maneuversd.py b/tools/lateral_maneuvers/lateral_maneuversd.py index ad398a9..cce125a 100644 --- a/tools/lateral_maneuvers/lateral_maneuversd.py +++ b/tools/lateral_maneuvers/lateral_maneuversd.py @@ -12,7 +12,7 @@ from openpilot.tools.longitudinal_maneuvers.maneuversd import Action, Maneuver a # thresholds for starting maneuvers MAX_SPEED_DEV = 0.7 # deviation in m/s -MAX_CURV = 0.002 # 500 m radius +MAX_CURV = 0.004 # 250 m radius MAX_ROLL = 0.12 # 6.8° TIMER = 2.0 # sec stable conditions before starting maneuver @@ -67,6 +67,12 @@ MANEUVERS = [ repeat=2, initial_speed=20. * CV.MPH_TO_MS, ), + Maneuver( + "jitter 20mph", + [Action([-0.5 if i % 2 == 0 else 0.5], [0.1]) for i in range(10)], + repeat=2, + initial_speed=20. * CV.MPH_TO_MS, + ), Maneuver( "step right 30mph", [Action([0.5], [1.0]), Action([-0.5], [1.5])], @@ -85,6 +91,12 @@ MANEUVERS = [ repeat=2, initial_speed=30. * CV.MPH_TO_MS, ), + Maneuver( + "jitter 30mph", + [Action([-0.5 if i % 2 == 0 else 0.5], [0.1]) for i in range(10)], + repeat=2, + initial_speed=30. * CV.MPH_TO_MS, + ), ] @@ -106,6 +118,8 @@ def main(): maneuvers = iter(MANEUVERS) maneuver = None complete_cnt = 0 + aborted_cnt = 0 + abort_reason = '' display_holdoff = 0 prev_text = '' @@ -129,8 +143,14 @@ def main(): alert_msg.alertDebug.alertText1 = 'Completed' alert_msg.alertDebug.alertText2 = maneuver.description elif maneuver is not None: - # reset maneuver on steering override or out of range speed - if sm['carState'].steeringPressed or (maneuver.active and abs(v_ego - maneuver.initial_speed) > MAX_SPEED_DEV): + # any driver input aborts the maneuver + CS = sm['carState'] + if CS.steeringPressed or CS.gasPressed: + aborted_cnt = int(1.0 / DT_MDL) + abort_reason = ('steering pressed' if CS.steeringPressed else 'gas pressed').ljust(20) + aborted = aborted_cnt > 0 + speed_out_of_range = maneuver.active and abs(v_ego - maneuver.initial_speed) > MAX_SPEED_DEV + if aborted or speed_out_of_range: maneuver.reset() roll = sm['carControl'].orientationNED[0] if len(sm['carControl'].orientationNED) == 3 else 0.0 @@ -148,6 +168,9 @@ def main(): else: alert_msg.alertDebug.alertText1 = f'Active {accel:+.1f}m/s² {max(action_remaining, 0):.1f}s' alert_msg.alertDebug.alertText2 = maneuver.description + elif aborted_cnt > 0: + aborted_cnt -= 1 + alert_msg.alertDebug.alertText1 = abort_reason elif not (abs(v_ego - maneuver.initial_speed) < MAX_SPEED_DEV and sm['carControl'].latActive): alert_msg.alertDebug.alertText1 = f'Set speed to {maneuver.initial_speed * CV.MS_TO_MPH:0.0f} mph' elif maneuver._ready_cnt > 0: diff --git a/tools/lateral_maneuvers/sim_maneuvers.py b/tools/lateral_maneuvers/sim_maneuvers.py new file mode 100755 index 0000000..22ad54f --- /dev/null +++ b/tools/lateral_maneuvers/sim_maneuvers.py @@ -0,0 +1,73 @@ +#!/usr/bin/env python3 +"""Run lateral_maneuversd against a synthetic lateral plant and write an rlog. + + ./tools/lateral_maneuvers/sim_maneuvers.py --out /tmp/lat_rlog.zst + ./tools/lateral_maneuvers/generate_report.py /tmp/lat_rlog.zst +""" +import argparse +import re +from pathlib import Path + +from openpilot.common.constants import CV +from openpilot.tools.lateral_maneuvers.lateral_maneuversd import MANEUVERS +from openpilot.tools.longitudinal_maneuvers.sim_harness import ManeuverSim, Plant + +CURV_TAU = 0.05 # controlsd curvature command tracking +RACK_WN = 8.0 # steering rack + tire natural frequency (rad/s) +RACK_ZETA = 0.7 # underdamped, so achieved curvature overshoots like a real rack +CRUISE_ACCEL = 1.2 + +SET_SPEED_RE = re.compile(r"Set speed to (\d+) mph") + + +class LateralPlant(Plant): + PLAN = 'lateralManeuverPlan' + + def __init__(self): + super().__init__(v_ego=MANEUVERS[0].initial_speed) + self.sim = None + self._rack_rate = 0.0 + self.target_speed = MANEUVERS[0].initial_speed + self._by_description = {m.description: m.initial_speed for m in MANEUVERS} + + def _update_target(self): + if self.sim is None: + return + speed = self._by_description.get(self.sim.alert2) + if speed is None: + match = SET_SPEED_RE.search(self.sim.alert1) + speed = float(match.group(1)) * CV.MPH_TO_MS if match else None + if speed is not None: + self.target_speed = speed + + def step(self, dt, plan): + self._update_target() + + err = self.target_speed - self.v_ego + self.a_ego = max(min(err / 1.0, CRUISE_ACCEL), -CRUISE_ACCEL) + self.v_ego = max(self.v_ego + self.a_ego * dt, 0.0) + + desired_curvature = float(plan.desiredCurvature) if plan is not None else 0.0 + self.curvature += (dt / (CURV_TAU + dt)) * (desired_curvature - self.curvature) + + self._rack_rate += dt * (RACK_WN ** 2 * (self.curvature - self.achieved_curvature) - 2 * RACK_ZETA * RACK_WN * self._rack_rate) + self.achieved_curvature += dt * self._rack_rate + self.lat_accel = self.achieved_curvature * max(self.v_ego, 1.0) ** 2 + + +def main(): + parser = argparse.ArgumentParser(description=__doc__) + parser.add_argument("--out", type=Path, default=Path("/tmp/lateral_maneuvers_sim/rlog.zst")) + parser.add_argument("--max-maneuvers", type=int, default=0, help="stop after N maneuvers (0 = all)") + parser.add_argument("--timeout", type=float, default=900.0) + args = parser.parse_args() + + sim = ManeuverSim("openpilot.tools.lateral_maneuvers.lateral_maneuversd", LateralPlant(), + max_maneuvers=args.max_maneuvers, timeout=args.timeout) + out = sim.run(args.out) + print(f"\nmaneuvers seen: {sim.seen_maneuvers}") + print(f"rlog: {out} ({out.stat().st_size / 1e6:.1f} MB)") + + +if __name__ == "__main__": + main() diff --git a/tools/longitudinal_maneuvers/README.md b/tools/longitudinal_maneuvers/README.md index 643af7f..28e4231 100644 --- a/tools/longitudinal_maneuvers/README.md +++ b/tools/longitudinal_maneuvers/README.md @@ -6,9 +6,9 @@ Test your vehicle's longitudinal control tuning with this tool. The tool will te ## Instructions -1. Check out a development branch such as `master` on your comma device. -2. Locate either a large empty parking lot or road devoid of any car or foot traffic. Flat, straight road is preferred. The full maneuver suite can take 1 mile or more if left running, however it is recommended to disengage openpilot between maneuvers and turn around if there is not enough space. -3. Turn off the vehicle and set this parameter which will signal to openpilot to start the longitudinal maneuver daemon: +1. Check out a development branch such as `master-mici` on your device. The toggle is hidden on release branches. +2. Locate either a large empty parking lot or road devoid of any car or foot traffic. Flat, straight road is preferred. The full maneuver suite can take 1 mile or more if left running, however it is recommended to disengage IQ.Pilot between maneuvers and turn around if there is not enough space. +3. Turn off the vehicle and enable "Longitudinal Maneuver Mode" in Settings > Developer. The toggle requires IQ.Pilot longitudinal control and only enables while offroad. Alternatively, set the parameter manually: ```sh echo -n 1 > /data/params/d/LongitudinalManeuverMode @@ -42,7 +42,19 @@ Test your vehicle's longitudinal control tuning with this tool. The tool will te plotting maneuver: creep: alternate between +1m/s^2 and -1m/s^2, runs: 2 plotting maneuver: gas step response: +1m/s^2 from 20mph, runs: 2 - Report written to /home/batman/openpilot/tools/longitudinal_maneuvers/longitudinal_reports/LEXUS_ES_TSS2_57048cfce01d9625_0000010e--5b26bc3be7.html + Report written to tools/longitudinal_maneuvers/longitudinal_reports/LEXUS_ES_TSS2_57048cfce01d9625_0000010e--5b26bc3be7.html ``` -You can reach out on [Discord](https://discord.comma.ai) if you have any questions about these instructions or the tool itself. + `generate_report.py` also takes a path to a local `rlog.zst` or a directory of them. + +## Testing the tooling without a car + +`sim_maneuvers.py` runs `maneuversd` as a real process against a synthetic powertrain and writes an rlog +that `generate_report.py` reads. Use it to verify the daemon and the report generator after changing either: + +```sh +$ python tools/longitudinal_maneuvers/sim_maneuvers.py --out /tmp/long/rlog.zst +$ python tools/longitudinal_maneuvers/generate_report.py /tmp/long/rlog.zst +``` + +The full suite takes about 4 minutes of wall clock; `--max-maneuvers N` stops early. diff --git a/tools/longitudinal_maneuvers/generate_report.py b/tools/longitudinal_maneuvers/generate_report.py index 8c16e30..32bdb5b 100755 --- a/tools/longitudinal_maneuvers/generate_report.py +++ b/tools/longitudinal_maneuvers/generate_report.py @@ -9,7 +9,7 @@ import webbrowser from collections import defaultdict from pathlib import Path import matplotlib.pyplot as plt -from tabulate import tabulate +from openpilot.common.utils import tabulate from openpilot.tools.lib.logreader import LogReader from openpilot.system.hardware.hw import Paths diff --git a/tools/longitudinal_maneuvers/sim_harness.py b/tools/longitudinal_maneuvers/sim_harness.py new file mode 100644 index 0000000..d77079f --- /dev/null +++ b/tools/longitudinal_maneuvers/sim_harness.py @@ -0,0 +1,240 @@ +#!/usr/bin/env python3 +"""Closed-loop offline harness for the maneuver daemons. + +Runs maneuversd / lateral_maneuversd as real subprocesses over msgq, drives them with a +synthetic vehicle, and records every message to an rlog that generate_report.py can read. +Used to validate the maneuver tooling without a car. +""" +import math +import os +import signal +import subprocess +import sys +import time +from pathlib import Path +from typing import NamedTuple + +import numpy as np +import zstandard as zstd + +from cereal import car, messaging +from openpilot.common.params import Params +from openpilot.common.realtime import DT_CTRL, Ratekeeper +from openpilot.common.basedir import BASEDIR + +PUB_100HZ = ('carState', 'carControl', 'carOutput', 'controlsState', 'selfdriveState') +PUB_20HZ = ('modelV2', 'livePose', 'liveParameters') +SUB = ('alertDebug', 'longitudinalPlan', 'lateralManeuverPlan') + +STEER_RATIO = 15.0 +WHEELBASE = 2.78 + + +class LongPlan(NamedTuple): + aTarget: float + shouldStop: bool + + +class LatPlan(NamedTuple): + desiredCurvature: float + + +class Plant: + """Vehicle model. Subclasses consume the daemon's plan and fill the published messages.""" + + sim = None + PLAN = 'longitudinalPlan' + + def __init__(self, v_ego: float = 0.0): + self.v_ego = v_ego + self.a_ego = 0.0 + self.curvature = 0.0 # commanded, controlsState.desiredCurvature + self.achieved_curvature = 0.0 # measured, controlsState.curvature + self.lat_accel = 0.0 + self.long_active = True + self.lat_active = True + + def step(self, dt: float, plan) -> None: + raise NotImplementedError + + def _angle(self, curvature: float) -> float: + return math.degrees(curvature * WHEELBASE * STEER_RATIO) + + def _torque(self, curvature: float) -> float: + return float(np.clip(curvature * max(self.v_ego, 1.0) ** 2 / 3.0, -1.0, 1.0)) + + def fill_car_state(self, cs) -> None: + cs.vEgo = float(self.v_ego) + cs.vEgoRaw = float(self.v_ego) + cs.vEgoCluster = float(self.v_ego) + cs.aEgo = float(self.a_ego) + cs.standstill = self.v_ego < 0.01 + cs.steeringAngleDeg = self._angle(self.achieved_curvature) + cs.cruiseState.enabled = True + cs.cruiseState.available = True + cs.cruiseState.speed = float(max(self.v_ego, 1.0)) + + def fill_car_control(self, cc) -> None: + cc.enabled = True + cc.latActive = self.lat_active + cc.longActive = self.long_active + cc.orientationNED = [0.0, 0.0, 0.0] + cc.actuators.curvature = float(self.curvature) + cc.actuators.accel = float(self.a_ego) + cc.actuators.steeringAngleDeg = self._angle(self.curvature) + cc.actuators.torque = self._torque(self.curvature) + + +class ManeuverSim: + def __init__(self, module: str, plant: Plant, fingerprint: str = "TOYOTA_SIENNA", + max_maneuvers: int = 0, timeout: float = 600.0, verbose: bool = True): + self.module = module + self.plant = plant + plant.sim = self + self.fingerprint = fingerprint + self.max_maneuvers = max_maneuvers + self.timeout = timeout + self.verbose = verbose + + self.events: list[bytes] = [] + self.alert1 = '' + self.alert2 = '' + self.seen_maneuvers: list[str] = [] + self.finished = False + + def _write_car_params(self): + CP = car.CarParams.new_message() + CP.carFingerprint = self.fingerprint + CP.brand = "toyota" + CP.openpilotLongitudinalControl = True + CP.autoResumeSng = True + CP.steerRatio = STEER_RATIO + CP.wheelbase = WHEELBASE + Params().put("CarParams", CP.to_bytes()) + return CP + + def _head_events(self, CP): + init = messaging.new_message('initData') + init.valid = True + init.initData.gitCommit = "simulated" + init.initData.gitBranch = "sim" + init.initData.gitRemote = "iqpilot-sim" + self.events.append(init.to_bytes()) + + cpm = messaging.new_message('carParams') + cpm.valid = True + cpm.carParams = CP + self.events.append(cpm.to_bytes()) + + def _launch(self): + env = dict(os.environ) + env["PYTHONPATH"] = str(BASEDIR) + os.pathsep + env.get("PYTHONPATH", "") + return subprocess.Popen([sys.executable, "-c", f"from {self.module} import main; main()"], + cwd=str(BASEDIR), env=env, start_new_session=True) + + def _on_alert(self, ad): + text1, text2 = ad.alertText1, ad.alertText2 + if (text1, text2) != (self.alert1, self.alert2): + if self.verbose: + print(f" [{time.monotonic() - self.t_start:6.1f}s] {text1!r} | {text2!r}") + if text2 and text2 not in self.seen_maneuvers: + self.seen_maneuvers.append(text2) + if text1 == 'Maneuvers Finished': + self.finished = True + self.alert1, self.alert2 = text1, text2 + + def run(self, out: Path) -> Path: + self._head_events(self._write_car_params()) + + pm = messaging.PubMaster(list(PUB_100HZ) + list(PUB_20HZ)) + socks = {s: messaging.sub_sock(s, conflate=False, timeout=0) for s in SUB} + + proc = self._launch() + self.t_start = time.monotonic() + rk = Ratekeeper(int(1.0 / DT_CTRL), print_delay_threshold=None) + + plans: dict[str, object | None] = {'longitudinalPlan': None, 'lateralManeuverPlan': None} + frame = 0 + try: + while True: + for s, sock in socks.items(): + while True: + raw = sock.receive(non_blocking=True) + if raw is None: + break + self.events.append(raw) + evt = messaging.log_from_bytes(raw) + if s == 'alertDebug': + self._on_alert(evt.alertDebug) + elif s == 'longitudinalPlan': + plans[s] = LongPlan(evt.longitudinalPlan.aTarget, evt.longitudinalPlan.shouldStop) + elif s == 'lateralManeuverPlan': + plans[s] = LatPlan(evt.lateralManeuverPlan.desiredCurvature) if evt.valid else None + + self.plant.step(DT_CTRL, plans[self.plant.PLAN]) + + for s in PUB_100HZ: + raw = self._build(s).to_bytes() + self.events.append(raw) + pm.send(s, raw) + + if frame % 5 == 0: + for s in PUB_20HZ: + raw = self._build(s).to_bytes() + self.events.append(raw) + pm.send(s, raw) + + frame += 1 + if self.finished: + break + if self.max_maneuvers and len(self.seen_maneuvers) > self.max_maneuvers: + break + if time.monotonic() - self.t_start > self.timeout: + print(" timed out") + break + rk.keep_time() + finally: + if proc.poll() is None: + os.killpg(os.getpgid(proc.pid), signal.SIGTERM) + proc.wait(timeout=5) + for sock in socks.values(): + del sock + + out.parent.mkdir(parents=True, exist_ok=True) + out.write_bytes(zstd.compress(b"".join(self.events), 10)) + return out + + def _build(self, s: str): + msg = messaging.new_message(s) + msg.valid = True + if s == 'carState': + self.plant.fill_car_state(msg.carState) + elif s == 'carControl': + self.plant.fill_car_control(msg.carControl) + elif s == 'carOutput': + msg.carOutput.actuatorsOutput.accel = float(self.plant.a_ego) + msg.carOutput.actuatorsOutput.curvature = float(self.plant.curvature) + msg.carOutput.actuatorsOutput.steeringAngleDeg = self.plant._angle(self.plant.achieved_curvature) + msg.carOutput.actuatorsOutput.torque = self.plant._torque(self.plant.achieved_curvature) + elif s == 'controlsState': + msg.controlsState.curvature = float(self.plant.achieved_curvature) + msg.controlsState.desiredCurvature = float(self.plant.curvature) + elif s == 'selfdriveState': + msg.selfdriveState.enabled = True + msg.selfdriveState.active = True + msg.selfdriveState.state = 'enabled' + elif s == 'modelV2': + msg.modelV2.frameId = 0 + msg.modelV2.action.desiredCurvature = 0.0 + elif s == 'livePose': + msg.livePose.accelerationDevice.x = float(self.plant.a_ego) + msg.livePose.accelerationDevice.y = float(self.plant.lat_accel) + msg.livePose.velocityDevice.x = float(self.plant.v_ego) + msg.livePose.inputsOK = True + msg.livePose.posenetOK = True + msg.livePose.sensorsOK = True + elif s == 'liveParameters': + msg.liveParameters.valid = True + msg.liveParameters.roll = 0.0 + msg.liveParameters.steerRatio = STEER_RATIO + return msg diff --git a/tools/longitudinal_maneuvers/sim_maneuvers.py b/tools/longitudinal_maneuvers/sim_maneuvers.py new file mode 100755 index 0000000..34e0eae --- /dev/null +++ b/tools/longitudinal_maneuvers/sim_maneuvers.py @@ -0,0 +1,51 @@ +#!/usr/bin/env python3 +"""Run maneuversd against a synthetic longitudinal plant and write an rlog. + + ./tools/longitudinal_maneuvers/sim_maneuvers.py --out /tmp/long_rlog.zst + ./tools/longitudinal_maneuvers/generate_report.py /tmp/long_rlog.zst +""" +import argparse +from pathlib import Path + +from openpilot.tools.longitudinal_maneuvers.sim_harness import ManeuverSim, Plant + +WN = 6.0 # powertrain natural frequency (rad/s) +ZETA = 0.6 # underdamped, so actual accel overshoots the target like a real car + + +class LongitudinalPlant(Plant): + def __init__(self): + super().__init__() + self.jerk = 0.0 + + def step(self, dt, plan): + a_target = float(plan.aTarget) if plan is not None else 0.0 + if plan is not None and plan.shouldStop: + a_target = min(a_target, -0.5) + + self.jerk += dt * (WN ** 2 * (a_target - self.a_ego) - 2 * ZETA * WN * self.jerk) + self.a_ego += dt * self.jerk + + self.v_ego = max(self.v_ego + self.a_ego * dt, 0.0) + if self.v_ego <= 0.0: + self.a_ego = min(self.a_ego, 0.0) + self.jerk = min(self.jerk, 0.0) + self.lat_accel = 0.0 + + +def main(): + parser = argparse.ArgumentParser(description=__doc__) + parser.add_argument("--out", type=Path, default=Path("/tmp/longitudinal_maneuvers_sim/rlog.zst")) + parser.add_argument("--max-maneuvers", type=int, default=0, help="stop after N maneuvers (0 = all)") + parser.add_argument("--timeout", type=float, default=900.0) + args = parser.parse_args() + + sim = ManeuverSim("openpilot.tools.longitudinal_maneuvers.maneuversd", LongitudinalPlant(), + max_maneuvers=args.max_maneuvers, timeout=args.timeout) + out = sim.run(args.out) + print(f"\nmaneuvers seen: {sim.seen_maneuvers}") + print(f"rlog: {out} ({out.stat().st_size / 1e6:.1f} MB)") + + +if __name__ == "__main__": + main() diff --git a/tools/sim/lib/simulated_sensors.py b/tools/sim/lib/simulated_sensors.py index 6ac7c4f..a8374a0 100644 --- a/tools/sim/lib/simulated_sensors.py +++ b/tools/sim/lib/simulated_sensors.py @@ -92,12 +92,11 @@ class SimulatedSensors: # dmonitoringd output dat = messaging.new_message('driverMonitoringState', valid=True) - dm = dat.driverMonitoringState - dm.alertLevel = log.DriverMonitoringState.AlertLevel.none - dm.activePolicy = log.DriverMonitoringState.MonitoringPolicy.vision - dm.visionPolicyState.faceDetected = True - dm.visionPolicyState.isDistracted = False - dm.visionPolicyState.awarenessPercent = 100 + dat.driverMonitoringState = { + "faceDetected": True, + "isDistracted": False, + "awarenessStatus": 1., + } self.pm.send('driverMonitoringState', dat) def send_camera_images(self, world: 'World'):