IQ.Lvbs/IQ.Pilot
8
7
Fork 2
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

IQ.Pilot Release Commit @ f2a861c

Browse Source
This commit is contained in:
IQ.Lvbs CI [bot]
2026-09-02 15:07:09 -05:00
parent b42569dbca
commit e8748fd704
5497 changed files with 316070 additions and 179848 deletions
Download Patch File Download Diff File Expand all files Collapse all files

90
.gitattributes vendored
View File

@@ -2,56 +2,52 @@
# to move existing files into LFS:
# git add --renormalize .
selfdrive/assets/icons/iq/rotate-ccw.svg -filter -diff -merge -text
selfdrive/assets/icons/iq/rotate-ccw.png -filter -diff -merge -text
selfdrive/assets/icons/iq/rotate-cw.svg -filter -diff -merge -text
selfdrive/assets/icons/iq/rotate-cw.png -filter -diff -merge -text
selfdrive/assets/icons/iq/square-parking.svg -filter -diff -merge -text
selfdrive/assets/icons/iq/square-parking.png -filter -diff -merge -text
iqpilot/selfdrive/assets/icons/iq/rotate-ccw.svg -filter -diff -merge -text
iqpilot/selfdrive/assets/icons/iq/rotate-ccw.png -filter -diff -merge -text
iqpilot/selfdrive/assets/icons/iq/rotate-cw.svg -filter -diff -merge -text
iqpilot/selfdrive/assets/icons/iq/rotate-cw.png -filter -diff -merge -text
iqpilot/selfdrive/assets/icons/iq/square-parking.svg -filter -diff -merge -text
iqpilot/selfdrive/assets/icons/iq/square-parking.png -filter -diff -merge -text
# Keep this icon in regular git (not LFS) for lightweight branding iteration.
selfdrive/assets/icons_mici/experimental_mode_mici.png -filter -diff -merge -text
selfdrive/assets/icons_mici/experimental_mode_tizi.png -filter -diff -merge -text
selfdrive/assets/icons_mici/iqdynamic_mode_mici.png -filter -diff -merge -text
selfdrive/assets/icons_mici/iqdynamic_mode_tizi.png -filter -diff -merge -text
selfdrive/assets/icons_mici/iqstandard_mode_mici.png -filter -diff -merge -text
selfdrive/assets/icons_mici/iqstandard_mode_tizi.png -filter -diff -merge -text
selfdrive/assets/icons_mici/onroad/driver_monitoring/dm_center.png -filter -diff -merge -text
selfdrive/assets/icons_mici/onroad/driver_monitoring/dm_cone.png -filter -diff -merge -text
selfdrive/assets/icons_mici/settings/konn3kt_icon.png -filter -diff -merge -text
selfdrive/assets/icons_mici/buttons/toggle_dot_enabled.png -filter -diff -merge -text
iqpilot/selfdrive/assets/icons_mici/experimental_mode_mici.png -filter -diff -merge -text
iqpilot/selfdrive/assets/icons_mici/experimental_mode_tizi.png -filter -diff -merge -text
iqpilot/selfdrive/assets/icons_mici/iqdynamic_mode_mici.png -filter -diff -merge -text
iqpilot/selfdrive/assets/icons_mici/iqdynamic_mode_tizi.png -filter -diff -merge -text
iqpilot/selfdrive/assets/icons_mici/iqstandard_mode_mici.png -filter -diff -merge -text
iqpilot/selfdrive/assets/icons_mici/iqstandard_mode_tizi.png -filter -diff -merge -text
iqpilot/selfdrive/assets/icons_mici/onroad/driver_monitoring/dm_center.png -filter -diff -merge -text
iqpilot/selfdrive/assets/icons_mici/onroad/driver_monitoring/dm_cone.png -filter -diff -merge -text
iqpilot/selfdrive/assets/icons_mici/settings/konn3kt_icon.png -filter -diff -merge -text
iqpilot/selfdrive/assets/icons_mici/buttons/toggle_dot_enabled.png -filter -diff -merge -text
iqpilot/selfdrive/assets -filter -diff -merge -text
selfdrive/assets/icons_mici/buttons/toggle_pill_enabled.png -filter -diff -merge -text
selfdrive/assets/icons_mici/offroad_alerts/green_wheel.png -filter -diff -merge -text
selfdrive/assets/icons_mici/setup/green_button.png -filter -diff -merge -text
selfdrive/assets/icons_mici/setup/green_button_pressed.png -filter -diff -merge -text
selfdrive/assets/icons_mici/setup/green_dm.png -filter -diff -merge -text
selfdrive/assets/icons_mici/setup/green_info.png -filter -diff -merge -text
selfdrive/assets/icons_mici/setup/small_slider/slider_green_rounded_rectangle.png -filter -diff -merge -text
selfdrive/assets/images/spinner_comma.png -filter -diff -merge -text
selfdrive/assets/images/k3_spinner.png -filter -diff -merge -text
selfdrive/assets/icons/camera.png -filter -diff -merge -text
selfdrive/assets/fonts/Syncopate-Regular.ttf -filter -diff -merge -text
selfdrive/assets/fonts/Tektur-Variable.ttf -filter -diff -merge -text
tools/jotpluggler/assets/bootstrap-icons.ttf -filter -diff -merge -text
# IQ assets, including icon_longitudinal.png, should stay in normal git (not LFS).
iqpilot/selfdrive/assets/** -filter -diff -merge -text
iqpilot/selfdrive/assets/icons_mici/buttons/toggle_pill_enabled.png -filter -diff -merge -text
iqpilot/selfdrive/assets/icons_mici/offroad_alerts/green_wheel.png -filter -diff -merge -text
iqpilot/selfdrive/assets/icons_mici/setup/green_button.png -filter -diff -merge -text
iqpilot/selfdrive/assets/icons_mici/setup/green_button_pressed.png -filter -diff -merge -text
iqpilot/selfdrive/assets/icons_mici/setup/green_dm.png -filter -diff -merge -text
iqpilot/selfdrive/assets/icons_mici/setup/green_info.png -filter -diff -merge -text
iqpilot/selfdrive/assets/icons_mici/setup/small_slider/slider_green_rounded_rectangle.png -filter -diff -merge -text
iqpilot/selfdrive/assets/images/spinner_comma.png -filter -diff -merge -text
iqpilot/selfdrive/assets/images/k3_spinner.png -filter -diff -merge -text
iqpilot/selfdrive/assets/icons/camera.png -filter -diff -merge -text
iqpilot/selfdrive/assets/fonts/Syncopate-Regular.ttf -filter -diff -merge -text
iqpilot/selfdrive/assets/fonts/Tektur-Variable.ttf -filter -diff -merge -text
iqpilot/tools/jotpluggler/assets/bootstrap-icons.ttf -filter -diff -merge -text
# mici UI icons stored as plain PNG — gitlab LFS server lacks these objects
selfdrive/assets/icons_mici/buttons/button_circle_pressed.png -filter -diff -merge -text
selfdrive/assets/icons_mici/buttons/button_circle_red_pressed.png -filter -diff -merge -text
selfdrive/assets/icons_mici/models.png -filter -diff -merge -text
selfdrive/assets/icons_mici/settings/brightness.png -filter -diff -merge -text
selfdrive/assets/icons_mici/settings/camera.png -filter -diff -merge -text
selfdrive/assets/icons_mici/settings/keyboard/enter.png -filter -diff -merge -text
selfdrive/assets/icons_mici/settings/keyboard/enter_disabled.png -filter -diff -merge -text
selfdrive/assets/icons_mici/settings/sd_card.png -filter -diff -merge -text
selfdrive/assets/icons_mici/settings/software.png -filter -diff -merge -text
selfdrive/assets/icons_mici/settings/trips.png -filter -diff -merge -text
selfdrive/assets/icons_mici/settings/vehicle.png -filter -diff -merge -text
selfdrive/assets/icons_mici/speedometer.png -filter -diff -merge -text
iqpilot/selfdrive/assets/icons_mici/buttons/button_circle_pressed.png -filter -diff -merge -text
iqpilot/selfdrive/assets/icons_mici/buttons/button_circle_red_pressed.png -filter -diff -merge -text
iqpilot/selfdrive/assets/icons_mici/models.png -filter -diff -merge -text
iqpilot/selfdrive/assets/icons_mici/settings/brightness.png -filter -diff -merge -text
iqpilot/selfdrive/assets/icons_mici/settings/camera.png -filter -diff -merge -text
iqpilot/selfdrive/assets/icons_mici/settings/keyboard/enter.png -filter -diff -merge -text
iqpilot/selfdrive/assets/icons_mici/settings/keyboard/enter_disabled.png -filter -diff -merge -text
iqpilot/selfdrive/assets/icons_mici/settings/sd_card.png -filter -diff -merge -text
iqpilot/selfdrive/assets/icons_mici/settings/software.png -filter -diff -merge -text
iqpilot/selfdrive/assets/icons_mici/settings/trips.png -filter -diff -merge -text
iqpilot/selfdrive/assets/icons_mici/settings/vehicle.png -filter -diff -merge -text
iqpilot/selfdrive/assets/icons_mici/speedometer.png -filter -diff -merge -text
selfdrive/assets/icons/iq/*.png -filter -diff -merge -text
iqpilot/selfdrive/assets/icons/iq/*.png -filter -diff -merge -text
artifacts/runtime_wheels/*.whl -filter -diff -merge -text

64
.gitignore vendored
View File

@@ -7,6 +7,10 @@ venv/
# perpetually dirty and sends the updater into an apply/restart loop that kills
# ble-transportd (and comma.service) every few minutes.
/.iqpilot/
/.iqpilot-package-lock-sha256
/gi
/k3_log.txt
/iqpilot/system/loggerd/encoder/v4l_decode
.ci_cache
.env
.clang-format
@@ -22,6 +26,11 @@ a.out
.hypothesis
.cache/
/iqdbc
/msgq
/openpilot
/tinygrad
/docs_site/
*.mp4
@@ -46,6 +55,7 @@ a.out
!artifacts/runtime/ble/rootfs/usr/local/venv/**/*.so
artifacts/iqpilot_private/
artifacts/iqpilot_*_private/python/iqpilot_private/**/*.cpython-*-darwin.so
artifacts/test-results/
*.a
*.clb
*.class
@@ -59,14 +69,14 @@ clcache
compile_commands.json
compare_runtime*.html
selfdrive/pandad/pandad
cereal/services.h
cereal/gen
cereal/messaging/bridge
selfdrive/ui/translations/tmp
selfdrive/car/tests/cars_dump
system/camerad/camerad
system/camerad/test/ae_gray_test
iqpilot/selfdrive/pandad/pandad
iqpilot/cereal/services.h
iqpilot/cereal/gen
iqpilot/cereal/messaging/bridge
iqpilot/selfdrive/ui/translations/tmp
iqpilot/selfdrive/car/tests/cars_dump
iqpilot/system/camerad/camerad
iqpilot/system/camerad/test/ae_gray_test
.coverage*
coverage.xml
@@ -79,13 +89,13 @@ flycheck_*
cppcheck_report.txt
comma*.sh
selfdrive/modeld/models/*.pkl
!selfdrive/modeld/models/driving_vision_tinygrad.pkl
!selfdrive/modeld/models/driving_policy_tinygrad.pkl
!selfdrive/modeld/models/dmonitoring_model_tinygrad.pkl
!selfdrive/modeld/models/driving_vision_metadata.pkl
!selfdrive/modeld/models/driving_policy_metadata.pkl
!selfdrive/modeld/models/dmonitoring_model_metadata.pkl
iqpilot/selfdrive/modeld/models/*.pkl
!iqpilot/selfdrive/modeld/models/driving_vision_tinygrad.pkl
!iqpilot/selfdrive/modeld/models/driving_policy_tinygrad.pkl
!iqpilot/selfdrive/modeld/models/dmonitoring_model_tinygrad.pkl
!iqpilot/selfdrive/modeld/models/driving_vision_metadata.pkl
!iqpilot/selfdrive/modeld/models/driving_policy_metadata.pkl
!iqpilot/selfdrive/modeld/models/dmonitoring_model_metadata.pkl
iqpilot/modeld*/thneed/compile
iqpilot/modeld*/models/*.thneed
iqpilot/modeld*/models/*.pkl
@@ -124,10 +134,6 @@ Pipfile
PLAN.md
TASK.md
### JetBrains ###
!.idea/customTargets.xml
!.idea/tools/*
!.run/*
/.antigravitycli/
/artifacts/iqpilot_alc_private/python/iqpilot_private/konn3kt/iqlvbs/_iqcrypto.cpython-312-aarch64-linux-gnu.so
/artifacts/iqpilot_alc_private/python/iqpilot_private/konn3kt/iqlvbs/alc_handshake.cpython-312-aarch64-linux-gnu.so
@@ -135,20 +141,22 @@ TASK.md
/iqpilot/selfdrive/iqlocd/iqlocd
/iqpilot/selfdrive/iqlocd/models/generated/car.cpp
/iqpilot/selfdrive/iqlocd/models/generated/car.h
/tools/scripts/debug/
/iqpilot/tools/scripts/debug/
/iqpilot/tools/scripts/plans/
!scripts/iqpilot/iqos_usrlocal_larch64.tar.zst
# pretty-build capture logs (model compile / mpc + dbc codegen stdout+stderr)
*_tinygrad.pkl.log
*_metadata.pkl.log
selfdrive/controls/lib/*_mpc_lib/gen.log
tools/cabana/dbc/*.json.log
iqpilot/selfdrive/controls/lib/*_mpc_lib/gen.log
iqpilot/tools/cabana/dbc/*.json.log
iqpilot/selfdrive/iqlocd/models/generated/
osmaps_build/
# --- local scratch/debug + large model artifacts (never commit) ---
/infinite_driver/
/konn3kt_private/
/.mull/
/_pq*.py
/bus_topo.py
/cluster_bus.py
@@ -167,8 +175,7 @@ osmaps_build/
/SUNNYPILOT_REMEDIATION_PLAN.md
/prompt.md
/iqos44_bluetooth_gamepad_spec.md
tools/mac_egpu/*.pkl
selfdrive/ui/tests/test_ui/nav_demo_report/
iqpilot/selfdrive/ui/tests/test_ui/nav_demo_report/
**/__pycache__/
*.pyc
*.mlpackage/
@@ -177,4 +184,11 @@ selfdrive/ui/tests/test_ui/nav_demo_report/
# fetcher: closed-source precompiled component — ship only the obfuscated .so
/iqpilot/models_private_src/fetcher.py
/tools/iqmacvisiond/macos/dist/
# build output: a signed .app is a 233-file sealed bundle, never track a subset
iqpilot/tools/iqmacd/macos/dist/IQ eMac.app/
iqpilot/tools/iqmacd/macos/dist/.stage/
iqpilot/tools/iqmacd/macos/dist/.DS_Store
iqpilot/tools/iqmacd/macos/dist/
iqpilot/tools/scripts/iqemac/clips/README.md

4
.lfsconfig-comma
View File

@@ -1,4 +0,0 @@
[lfs]
url = https://gitlab.com/commaai/openpilot-lfs.git/info/lfs
pushurl = ssh://git@gitlab.com/commaai/openpilot-lfs.git
locksverify = false

127
CHANGELOG.md
View File

@@ -1,127 +0,0 @@
# IQ.Pilot User Changelog
This changelog is written for everyday drivers and focuses on what you will notice on the road, as well as changes under-the-hood.
## IQ.Pilot 1.0c
**Navigate on IQ.Pilot**
Navigate on IQ.Pilot is here. Search for a destination, pick from route alternatives, and let IQ.Pilot guide you turn by turn with live rerouting when you miss an exit. Along with on-screen-maps, you can see your position, the route, and upcoming turns all at once without leaving the driving view. Speed, turn, and highway exit handling are route-aware, so IQ.Pilot knows what's coming before you do. Mapbox is included at no cost to you for enhanced online map data and routing. On supported vehicles, Navigate on IQ.Pilot will command turn signals automatically based on the route. Map Curve speed control pulls limit data from OpenStreetMaps when offline and Mapbox when available.
When approaching a highway exit, IQ.Pilot now initiates the lane change toward the exit. If your car has Blind Spot Monitoring (BSM), it can perform the lane change fully automatically, there's no blinker nudge required. Without BSM, you confirm with a brief blinker push and IQ.Pilot takes it from there.
**Speed Limit Control (SLC)**
IQ.Pilot can now read and act on speed limits from your car's dash, Mapbox, TomTom, HERE, (included at no cost to our users!) and offline maps. You pick what mode you want in settings: display only, warn you when you're over, or actually adjust your cruise speed. You also pick which source wins when they disagree (dash, Mapbox, map data, highest, or lowest reported limit). There's a look ahead setting so IQ.Pilot can start reacting to an upcoming speed change before you hit the sign to decelerate to the limit before crossing into the new speed limit.
SLC can now also raise your cruise speed automatically when the speed limit increases, not just lower it. Toggle "confirm higher speed limit" off to enable this, SLC will adjust up to a higher accepted limit with a small prompt, without any confirmation input needed.
**Camera Alerts (Speed Cameras, Red Light Cameras, and Surveillance / ALPR Cameras)**
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.
Force Stops now include "Smooth Stops" under the same toggle thank's to SpysyWeeb! With Force Stops on, all stops including model predicted stops at signs and lights now use the smooth landing law, so every stop settles gently rather than dropping in hard. The minimum force stop distance slider enforces minimum distance when configured.
IQ.Dynamic on supported Volkswagen platforms including MQB, and PQ now support blending OEM Stock Radar ACC with IQ.Dynamic to allow for a blended longitudinal experience while maintaining E2E IQ.Pilot functionality.
**Driving Models**
IQ.Pilot updated to a new default driving model, `Pop!`
IQ.Pilot also has the latest bleeding-edge models, as always, including the latest TobyRL model, NoPP model, DeeperRL model, DeepRLv3/4/5 models, OP Model 16 Deep, and all future RL models as they are released.
IQ.Pilot maintains supports for all legacy models like `Notre Dame (v1/3)`, `FarmVille`, `WD40`, etc.
**Dashcam, Live View, and Alerts**
- WebSSH now connects in under 15 seconds and connects the first time, every time.
- Live View in the Konn3kt app now processes full-resolution HDR input from the Comma 4 driver camera for a noticeably sharper, and more accurate picture. Live View performance was optimized, and microphone audio (one way) streaming is now included.
- You can fully disable dashcam recording from the Konn3kt app. Turning it off stops all recording, no logs, no video, no audio, full stop.
- Audible alerts now ramp in volume smoothly instead of cutting in abruptly for enhanced auditory alerts.
- On-road live streaming, with Two-Way Audio streaming from your IQ.Pilot devices camera feed live through the Konn3kt app. Onroad choppiness fixed, keyframe-on-demand enabled for instant camera switches, and variable network-condition-adaptive bitrate.
- Konn3kt can now take a snapshot from any camera (road, driver, or wide) on-demand, both onroad and offroad, and returns it as a JPEG instantly for a glance.
**Volkswagen**
Volkswagen support got a significant overhaul:
- Lateral and longitudinal tuning greatly improved.
- Accelerator override behavior now matches stock feel.
- MQB SnG handling improved for supported non-EPB ACC FtS vehicles.
- IQ.Pilot now supports all VW PQ and MQB CC-only and (A)CC-less cars, including CC-only PQ cars without an ADAS gateway.
- Volkswagen Passat B7 (PQ) with TRW450 now supports Stop-and-Go.
- Volkswagen MEB/MQBevo now only go on-road when in Drive and no longer in Park for 15 minutes after parking the car.
- Added Passat (PQ) model year ECU fingerprint.
- Konn3kt can now check EPS compatibility and LKAS coding status on VW MQB vehicles with a comma power.
- Konn3kt can enable LKAS coding on supported VW MQB vehicles with EPS that didn't ship with factory LKAS with a comma power.
- Volkswagen MQB/PQ now supports full Radar Blending with IQ.Dynamic for an enhanced E2E + Highway experience. (limited by OEM ACC minimum speed)
**Volkswagen MEB and MQBevo**
IQ.Pilot now **officially** supports the Volkswagen MEB and MQBevo platforms! Including the ID.4, ID.3, ID.5, Golf MK8, and Tiguan 2024+, up to model year 2026, as long as you have a compatible camera or gateway harness. Both LKAS and ACC are supported. This is the foundation of the `release-meb` branch, which is auto-synced from `release` and tailored specifically for these platforms.
**Toyota/Lexus**
Support added for Stop-and-Go and SDSU for Toyota/Lexus vehicles.
**Hyundai/Kia**
Fingerprint coverage expanded to cover more Hyundai/Kia variants that were previously unrecognized, and proper CAN-FD handling for newer HKG.
**UI — Comma 4 (mici)**
The Comma 4's offroad UI has been completely redesigned and has had major performance optimizations, it contains the same settings that BIG UI contains on Comma 3x/3 devices.
**UI — Comma 3x and Comma 3 (tizi/tici)**
The tizi/tici (Comma 3x and Comma 3) onroad and offroad UI has been fully redesigned as well, matching the new IQ.Pilot design language with a brand new home screen, status bar, settings menu's, and should be a much better experience.
**UI Improvements**
IQ.Pilot's on-road UI got a number of improvements:
- The Steering Assistance border now has a lower portion that distinguishes lateral only engagement from full engagement.
- IQLong Personality can now be cycled on-road by tapping the driver-monitoring icon on BIG UI devices. The icon color reflects the currently selected personality as well as an on screen current profile confirmation.
- IQLong mode IQ.Standard has been renamed to IQ.Chill to lessen confusion on long modes.
- IQLong mode can now be cycled on-road by tapping the nucleus icon in the top right corner on BIG UI devices. The icon changes to reflect IQ.Chill/Dynamic/Pilot.
- Fixed augmented road view calibration showing invalid calibration data on the model path / tracked lane lines by refreshing the matrix cache correctly for calibration to properly update on startup with Navigation enabled.
- Live Konn3kt accent color sync: whatever color you pick in the Konn3kt app flows to your device UI instantly.
- Revamped Branch switcher to properly switch branches, and updater has had bugfixes to fix install issues where the device claims to have updated but has not actually updated.
**IQ.OS 3.14**
IQ.OS 3.14 is bundeled with IQ.Pilot 1.0c. IQ.OS is available for all supported devices: Comma 3, Comma 3x, Comma 4, Konik A1/M, and Mr.One C3/C3(X)Lite. It's a lightweight OS based on Ubuntu 24.04, includes Bluetooth (BLE), has a much smaller install footprint, and is continuously optimized for IQ.Pilot.
- Konn3kt now stays online regardless of IQ.Pilot's status. If IQ.Pilot fails to boot, Konn3kt remains available so you can switch branches, SSH in, and recover remotely without a physical connection, including over cellular.
- Bug causing konn3kt setup time on a fresh install dropped from ~30 minutes fixed, setup dropped down to ~10 seconds.
- Automatic LocalAPI configuration in Konn3kt.
- Fixed Upstream Comma 4/3x/3 AGNOS Wi-Fi driver crash causing random crashes while driving.
- Fixed Comma 4 green-dot-matrix text aliasing issue.
- Fixed Comma 4 display calibration not showing for accurate colors.
**Updater: Pre-download Mode**
A new "Update Install Mode" setting in Software settings gives you control over how updates are applied:
- **Predownload Only** — updates download in the background but wait for you to confirm before installing.
- **Predownload + Preinstall** — downloads and installs automatically on next boot, as it was before.
**Konn3kt Services**
Konn3kt's WebApp has migrated to `app.konn3kt.com`
Connection stability between Konn3kt and IQ.Pilot (Konn3ktion) is greatly improved.
**eSIM**
eSIM detection, provisioning, and profile management groundwork is now built into the device. The app can detect whether your device has an embedded SIM, provision it, and manage profiles without a physical SIM swap. eSIM support requires a compatible data plan; Contact IQ.Pilot support in the discord for known working eSIM carriers and plans. Note: eSIM is experimental and generally requires a hotspot-style data plan or an MVNO that does not IMEI filter.

54
Jenkinsfile vendored
View File

@@ -93,7 +93,7 @@ def deviceStage(String stageName, String deviceType, List extra_env, def steps)
retry (3) {
def date = sh(script: 'date', returnStdout: true).trim();
device(device_ip, "set time", "date -s '" + date + "'")
device(device_ip, "git checkout", extra + "\n" + readFile("selfdrive/test/setup_device_ci.sh"))
device(device_ip, "git checkout", extra + "\n" + readFile("iqpilot/selfdrive/test/setup_device_ci.sh"))
}
steps.each { item ->
def name = item[0]
@@ -202,61 +202,63 @@ node {
parallel (
'onroad tests': {
deviceStage("onroad", "tizi-needs-can", ["UNSAFE=1"], [
step("build openpilot", "cd system/manager && ./build.py"),
step("build openpilot", "cd iqpilot/system/manager && ./build.py"),
step("check dirty", "release/check-dirty.sh"),
step("onroad tests", "pytest selfdrive/test/test_onroad.py -s", [timeout: 60]),
step("onroad tests", "pytest iqpilot/selfdrive/test/test_onroad.py -s", [timeout: 60]),
])
},
'HW + Unit Tests': {
deviceStage("tizi-hardware", "tizi-common", ["UNSAFE=1"], [
step("build", "cd system/manager && ./build.py"),
step("test pandad", "pytest selfdrive/pandad/tests/test_pandad.py", [diffPaths: ["panda", "selfdrive/pandad/"]]),
step("test power draw", "pytest -s system/hardware/tici/tests/test_power_draw.py"),
step("test encoder", "LD_LIBRARY_PATH=/usr/local/lib pytest system/loggerd/tests/test_encoder.py", [diffPaths: ["system/loggerd/"]]),
step("test manager", "pytest system/manager/test/test_manager.py"),
step("audit proprietary entrypoints", "python3 scripts/iqpilot/audit_proprietary_entrypoints.py --check"),
step("audit proprietary runtime", "python3 scripts/iqpilot/audit_proprietary_runtime.py"),
step("build", "cd iqpilot/system/manager && ./build.py"),
step("test pandad", "pytest iqpilot/selfdrive/pandad/tests/test_pandad.py", [diffPaths: ["panda", "iqpilot/selfdrive/pandad/"]]),
step("test power draw", "pytest -s iqpilot/system/hardware/tici/tests/test_power_draw.py"),
step("test encoder", "LD_LIBRARY_PATH=/usr/local/lib pytest iqpilot/system/loggerd/tests/test_encoder.py", [diffPaths: ["iqpilot/system/loggerd/"]]),
step("test manager", "pytest iqpilot/system/manager/test/test_manager.py"),
])
},
'loopback': {
deviceStage("loopback", "tizi-loopback", ["UNSAFE=1"], [
step("build openpilot", "cd system/manager && ./build.py"),
step("test pandad loopback", "pytest selfdrive/pandad/tests/test_pandad_loopback.py"),
step("build openpilot", "cd iqpilot/system/manager && ./build.py"),
step("test pandad loopback", "pytest iqpilot/selfdrive/pandad/tests/test_pandad_loopback.py"),
])
},
'camerad OX03C10': {
deviceStage("OX03C10", "tizi-ox03c10", ["UNSAFE=1"], [
step("build", "cd system/manager && ./build.py"),
step("test camerad", "pytest system/camerad/test/test_camerad.py", [timeout: 60]),
step("test exposure", "pytest system/camerad/test/test_exposure.py"),
step("build", "cd iqpilot/system/manager && ./build.py"),
step("test camerad", "pytest iqpilot/system/camerad/test/test_camerad.py", [timeout: 60]),
step("test exposure", "pytest iqpilot/system/camerad/test/test_exposure.py"),
])
},
'camerad OS04C10': {
deviceStage("OS04C10", "tici-os04c10", ["UNSAFE=1"], [
step("build", "cd system/manager && ./build.py"),
step("test camerad", "pytest system/camerad/test/test_camerad.py", [timeout: 60]),
step("test exposure", "pytest system/camerad/test/test_exposure.py"),
step("build", "cd iqpilot/system/manager && ./build.py"),
step("test camerad", "pytest iqpilot/system/camerad/test/test_camerad.py", [timeout: 60]),
step("test exposure", "pytest iqpilot/system/camerad/test/test_exposure.py"),
])
},
'sensord': {
deviceStage("LSM + MMC", "tizi-lsmc", ["UNSAFE=1"], [
step("build", "cd system/manager && ./build.py"),
step("test sensord", "pytest system/sensord/tests/test_sensord.py"),
step("build", "cd iqpilot/system/manager && ./build.py"),
step("test sensord", "pytest iqpilot/system/sensord/tests/test_sensord.py"),
])
},
'replay': {
deviceStage("model-replay", "tizi-replay", ["UNSAFE=1"], [
step("build", "cd system/manager && ./build.py", [diffPaths: ["selfdrive/modeld/", "tinygrad_repo", "selfdrive/test/process_replay/model_replay.py"]]),
step("model replay", "selfdrive/test/process_replay/model_replay.py", [diffPaths: ["selfdrive/modeld/", "tinygrad_repo", "selfdrive/test/process_replay/model_replay.py"]]),
step("build", "cd iqpilot/system/manager && ./build.py", [diffPaths: ["iqpilot/selfdrive/dmonitoringmodeld/", "iqpilot/selfdrive/iqmodeld/", "pyproject.toml", "iqpilot/selfdrive/test/process_replay/model_replay.py"]]),
step("model replay", "iqpilot/selfdrive/test/process_replay/model_replay.py", [diffPaths: ["iqpilot/selfdrive/dmonitoringmodeld/", "iqpilot/selfdrive/iqmodeld/", "pyproject.toml", "iqpilot/selfdrive/test/process_replay/model_replay.py"]]),
])
},
'tizi': {
deviceStage("tizi", "tizi", ["UNSAFE=1"], [
step("build openpilot", "cd system/manager && ./build.py"),
step("test pandad loopback", "SINGLE_PANDA=1 pytest selfdrive/pandad/tests/test_pandad_loopback.py"),
step("test pandad spi", "pytest selfdrive/pandad/tests/test_pandad_spi.py"),
step("test amp", "pytest system/hardware/tici/tests/test_amplifier.py"),
step("build openpilot", "cd iqpilot/system/manager && ./build.py"),
step("test pandad loopback", "SINGLE_PANDA=1 pytest iqpilot/selfdrive/pandad/tests/test_pandad_loopback.py"),
step("test pandad spi", "pytest iqpilot/selfdrive/pandad/tests/test_pandad_spi.py"),
step("test amp", "pytest iqpilot/system/hardware/tici/tests/test_amplifier.py"),
// TODO: enable once new AGNOS is available
// step("test esim", "pytest system/hardware/tici/tests/test_esim.py"),
step("test qcomgpsd", "pytest system/qcomgpsd/tests/test_qcomgpsd.py", [diffPaths: ["system/qcomgpsd/"]]),
// step("test esim", "pytest iqpilot/system/hardware/tici/tests/test_esim.py"),
step("test qcomgpsd", "pytest iqpilot/system/qcomgpsd/tests/test_qcomgpsd.py", [diffPaths: ["iqpilot/system/qcomgpsd/"]]),
])
},

4
LICENSE
View File

@@ -1,4 +1,4 @@
IQ.Lvbs License v0.1a
IQ.Lvbs License v1.8.2
Copyright (c) 2026 IQ.Lvbs LLC, a part of Project Teal Lvbs Inc. All Rights Reserved.
@@ -91,7 +91,7 @@ GOVERNING LAW
This license shall be governed by the laws of the State of Illinois, United
States of America. Any disputes arising under this license shall be subject
to the exclusive jurisdiction of the courts located in The State of Illinois, United States.
to the exclusive jurisdiction of the courts located in Henry County, Illinois.
---

107
LICENSE.md
View File

@@ -1,107 +0,0 @@
IQ.Lvbs License v0.1a
Copyright (c) 2026 IQ.Lvbs LLC, a part of Project Teal Lvbs Inc. All Rights Reserved.
DEFINITIONS
"Software" refers to IQ.Pilot, konn3kt, and all associated source code,
documentation, and assets owned by the Copyright Holder.
"Open Components" refers to portions of the Software explicitly marked as
open source.
"Proprietary Components" refers to all portions of the Software not made
available to the public in source form.
"Copyright Holder" refers to IQ.Lvbs LLC, a part of Project Teal Lvbs Inc.
GRANT OF LICENSE
Subject to the terms of this license, you are granted a limited,
non-exclusive, revocable license to:
1. View, study, and learn from the Open Components
2. Modify the Open Components for personal, internal, or open-source public use
3. Run the Software for personal, non-commercial purposes
RESTRICTIONS
You may NOT:
1. Claim ownership of any part of the Software, excluding your own
modifications that do not incorporate Proprietary Components.
2. Reverse engineer, decompile, disassemble, or in any way attempt to
circumvent the obfuscation of the Proprietary Components.
3. Use the Software or any derivative for commercial purposes without
explicit written permission from the Copyright Holder.
4. Remove or alter any copyright notices or this license.
5. Sublicense, sell, or transfer rights to the Software.
6. Use the Software and/or its source code to compete with or create a
substantially similar product.
7. Use the Software in closed source software not licensed by IQ.Lvbs LLC.
CONSEQUENCES OF VIOLATION
In the event any Restriction is violated, any product created using inspiration from, or source code from, IQ.Pilot or Konn3kt shall be subject to a licensing fee determined solely by the Copyright Holder. Additionally, the violating party hereby grants IQ.Lvbs LLC an exclusive, irrevocable, worldwide, royalty-free license to use any and all assets from the infringing product on IQ.Lvbs webpages, advertising materials, and in any other manner IQ.Lvbs sees fit.
OWNERSHIP
All rights, title, and interest in the Software remain exclusively with the
Copyright Holder. Any modifications, improvements, or derivative works you
create based on the Software are owned by the Copyright Holder. By
contributing modifications, you irrevocably assign all rights to the
Copyright Holder.
PROPRIETARY COMPONENTS
The Proprietary Components are provided in binary or obfuscated form only.
Reverse engineering, decompilation, or disassembly of Proprietary Components
is strictly prohibited. Violation of this provision entitles IQ.Lvbs LLC to
pursue all available legal remedies to protect its intellectual property and
trade secrets.
NO WARRANTY
THE SOFTWARE IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY KIND. THE COPYRIGHT
HOLDER DISCLAIMS ALL WARRANTIES, EXPRESS OR IMPLIED, INCLUDING BUT NOT
LIMITED TO MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE, AND
NON-INFRINGEMENT.
LIMITATION OF LIABILITY
IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY CLAIM, DAMAGES, OR
OTHER LIABILITY ARISING FROM THE USE OF THE SOFTWARE. THE USER ACCEPTS FULL
RESPONSIBILITY FOR ANY AND ALL LIABILITIES WHEN USING IQ.LVBS SOFTWARE.
TERMINATION
This license terminates automatically if you violate any of its terms. Upon
termination, you must destroy all copies of the Software in your possession.
The Copyright Holder reserves the right to revoke this license at any time
for any reason.
GOVERNING LAW
This license shall be governed by the laws of the State of Illinois, United
States of America. Any disputes arising under this license shall be subject
to the exclusive jurisdiction of the courts located in Henry County, Illinois.
---
For commercial licensing inquiries, contact: support@iqlvbs.com
IQ.Pilot is a fork of Stock OpenPilot maintained by Comma.ai, Inc. licensed under the following notice:
Copyright (c) 2018, Comma.ai, Inc.
Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions:
The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software.
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.

6
README.md
View File

@@ -6,9 +6,9 @@
## Running IQ.Pilot
* A modern comma, or clone device to run this software (Comma 3, 3x, 4, Konik A1/M, Mr.One C3, C3 Lite)
* One of [the supported cars](https://gitlvb.teallvbs.xyz/IQ.Lvbs/IQ.Pilot/src/branch/release/iqdbc_repo/docs/CARS.md).
* One of [the supported cars](https://gitlvb.teallvbs.xyz/IQ.Lvbs/IQ.Pilot/src/branch/release/artifacts/package_sources/iqdbc/docs/CARS.md).
* A [car harness](https://comma.ai/shop/products/car-harness) to connect to your car
#### Wondering if IQ.Pilot supports your car? IQ.Pilot supports every car [stock openpilot](https://gitlvb.teallvbs.xyz/IQ.Lvbs/IQ.Pilot/src/branch/release/iqdbc_repo/docs/CARS.md) supports!
#### Wondering if IQ.Pilot supports your car? IQ.Pilot supports every car [stock openpilot](https://gitlvb.teallvbs.xyz/IQ.Lvbs/IQ.Pilot/src/branch/release/artifacts/package_sources/iqdbc/docs/CARS.md) supports!
## Installation
#### Installing Via Installer URL:
#### Enter the following into your device custom URL box to install IQ.Pilot:
@@ -31,7 +31,7 @@
#### If you'd like to backup your previous installation as well, paste the following command below to install IQ.Pilot:
`cd .. && mv openpilot openpilot_backup_X && git clone https://git.konn3kt.com/IQ.Lvbs/IQ.Pilot -b release && cd openpilot && sudo reboot`
#### Alternatively, you can use your existing fork's built in tools to switch your branch as well:
`git remote add iqpilot https://git.konn3kt.com/IQ.Lvbs/IQ.Pilot && op switch iqpilot release`
`git remote add iqpilot https://git.konn3kt.com/IQ.Lvbs/IQ.Pilot && iq switch iqpilot release`
---

7
RELEASES.md
View File

@@ -1,7 +0,0 @@
IQ.Pilot 1.0c
========================
* Navigation;)
IQ.Pilot 1.0b
========================
* Major Refactor!

123
SConstruct
View File

@@ -6,13 +6,18 @@ import platform
import shlex
import numpy as np
import iqdbc
import msgq as msgq_package
import panda
import tinygrad
import SCons.Errors
SCons.Warnings.warningAsException(True)
# scons only auto-loads a site dir named site_scons at the repo root; ours lives under tools/,
# so replicate what _load_site_scons_dir does (sys.path for site_tools imports + run site_init)
SITE_DIR = Dir('#tools/scons').abspath
SITE_DIR = Dir('#iqpilot/tools/scons').abspath
if SITE_DIR not in sys.path:
sys.path.insert(0, SITE_DIR)
import site_init # noqa: F401
@@ -32,9 +37,19 @@ AddOption('--minimal',
action='store_false',
dest='extras',
default=os.path.exists(File('#.gitattributes').abspath), # minimal by default on release branch (where there's no LFS)
help='the minimum build to run openpilot. no tests, tools, etc.')
help='the minimum IQ.Pilot build. no tests, tools, etc.')
AddOption('--verbose', action='store_true', help='show full compiler/linker command lines instead of short build lines')
python_paths = [
Dir("#").abspath,
]
for p in reversed(python_paths):
if p not in sys.path:
sys.path.insert(0, p)
if external_pythonpath := os.environ.get("PYTHONPATH"):
python_paths += [p for p in external_pythonpath.split(os.pathsep) if p and p not in python_paths]
# Detect platform
arch = subprocess.check_output(["uname", "-m"], encoding='utf8').rstrip()
if platform.system() == "Darwin":
@@ -43,7 +58,7 @@ if platform.system() == "Darwin":
elif arch == "aarch64" and os.path.isfile('/TICI'):
arch = "larch64"
try:
from openpilot.system.hardware import HARDWARE
from iqpilot.system.hardware import HARDWARE
HARDWARE.set_power_save(False)
os.sched_setaffinity(0, range(8))
except Exception:
@@ -56,13 +71,20 @@ assert arch in [
"Darwin", # macOS arm64 (x86 not supported)
]
# ffmpeg comes from the system (brew on macOS, distro packages elsewhere) rather than
# a vendored wheel, so it always needs the static-link deps. Exported so tools/ can
# take upstream's `ffmpeg_libs` form instead of hand-listing codecs per SConscript.
ffmpeg_libs = ['avformat', 'avcodec', 'avutil', 'x264', 'z']
if arch != "Darwin":
ffmpeg_libs += ['va', 'va-drm', 'drm']
env = Environment(
ENV={
"PATH": os.environ['PATH'],
"PYTHONPATH": Dir("#").abspath + ':' + Dir(f"#third_party/acados").abspath,
"ACADOS_SOURCE_DIR": Dir("#third_party/acados").abspath,
"ACADOS_PYTHON_INTERFACE_PATH": Dir("#third_party/acados/acados_template").abspath,
"TERA_PATH": Dir("#").abspath + f"/third_party/acados/{arch}/t_renderer"
"PYTHONPATH": os.pathsep.join(python_paths + [Dir(f"#iqpilot/third_party/acados").abspath]),
"ACADOS_SOURCE_DIR": Dir("#iqpilot/third_party/acados").abspath,
"ACADOS_PYTHON_INTERFACE_PATH": Dir("#iqpilot/third_party/acados/acados_template").abspath,
"TERA_PATH": Dir("#").abspath + f"/iqpilot/third_party/acados/{arch}/t_renderer"
},
CC='clang',
CXX='clang++',
@@ -83,37 +105,38 @@ env = Environment(
CXXFLAGS=["-std=c++1z"],
CPPPATH=[
"#",
"#msgq",
"#third_party",
"#third_party/json11",
"#third_party/linux/include",
"#third_party/acados/include",
"#third_party/acados/include/blasfeo/include",
"#third_party/acados/include/hpipm/include",
"#third_party/catch2/include",
"#third_party/libyuv/include",
"#iqpilot",
iqdbc.INCLUDE_PATH,
msgq_package.INCLUDE_PATH,
panda.INCLUDE_PATH,
"#iqpilot/cereal/gen/cpp",
"#iqpilot/third_party",
"#iqpilot/third_party/json11",
"#iqpilot/third_party/linux/include",
"#iqpilot/third_party/acados/include",
"#iqpilot/third_party/acados/include/blasfeo/include",
"#iqpilot/third_party/acados/include/hpipm/include",
"#iqpilot/third_party/catch2/include",
"#iqpilot/third_party/libyuv/include",
],
LIBPATH=[
"#common",
"#msgq_repo",
"#third_party",
"#selfdrive/pandad",
"#rednose/helpers",
f"#third_party/libyuv/{arch}/lib",
f"#third_party/acados/{arch}/lib",
"#iqpilot/common",
"#iqpilot/third_party",
"#iqpilot/selfdrive/pandad",
f"#iqpilot/third_party/libyuv/{arch}/lib",
f"#iqpilot/third_party/acados/{arch}/lib",
],
RPATH=[],
CYTHONCFILESUFFIX=".cpp",
COMPILATIONDB_USE_ABSPATH=True,
REDNOSE_ROOT="#",
tools=["default", "cython", "compilation_db", "rednose_filter"],
toolpath=["#tools/scons/site_tools", "#rednose_repo/site_scons/site_tools"],
tools=["default", "cython", "compilation_db"],
toolpath=["#iqpilot/tools/scons/site_tools"],
)
# Arch-specific flags and paths
if arch == "larch64":
env.Append(CPPPATH=[
"#third_party/opencl/include",
"#iqpilot/third_party/opencl/include",
"/usr/include/aarch64-linux-gnu",
])
env.Append(LIBPATH=[
@@ -194,7 +217,8 @@ else:
np_version = SCons.Script.Value(np.__version__)
Export('envCython', 'np_version')
Export('env', 'arch')
tinygrad_dir = os.path.dirname(tinygrad.__file__)
Export('env', 'arch', 'ffmpeg_libs', 'tinygrad_dir')
# Setup cache dir
default_cache_dir = os.environ.get('SCONS_CACHE_DIR') or ('/data/scons_cache' if arch == "larch64" else '/tmp/scons_cache')
@@ -205,53 +229,44 @@ Clean(["."], cache_dir)
# ********** start building stuff **********
# Build common module
SConscript(['common/SConscript'])
SConscript(['iqpilot/common/SConscript'])
Import('_common')
common = [_common, 'json11', 'zmq']
Export('common')
# Build messaging (cereal + msgq + socketmaster + their dependencies)
# Enable swaglog include in submodules
env_swaglog = env.Clone()
env_swaglog['CXXFLAGS'].append('-DSWAGLOG="\\"common/swaglog.h\\""')
SConscript(['msgq_repo/SConscript'], exports={'env': env_swaglog})
SConscript(['iqdbc_repo/SConscript'], exports={'env': env_swaglog})
msgq = File(msgq_package.LIB_PATH)
visionipc = File(msgq_package.VISIONIPC_LIB_PATH)
msgq_python = File(msgq_package.PYTHON_LIB_PATH)
Export('msgq', 'visionipc', 'msgq_python')
SConscript(['cereal/SConscript'])
SConscript(['iqpilot/cereal/SConscript'])
Import('socketmaster', 'msgq')
Import('socketmaster')
messaging = [socketmaster, msgq, 'capnp', 'kj',]
Export('messaging')
# Build other submodules
SConscript(['panda/SConscript'])
# Build rednose library
SConscript(['rednose/SConscript'])
# Build system services
SConscript([
'system/loggerd/SConscript',
'system/proprietary_runtime/SConscript',
'iqpilot/system/loggerd/SConscript',
'iqpilot/system/proprietary_runtime/SConscript',
])
if arch == "larch64":
SConscript(['system/camerad/SConscript'])
SConscript(['iqpilot/system/camerad/SConscript'])
# Build openpilot
SConscript(['third_party/SConscript'])
SConscript(['iqpilot/third_party/SConscript'])
SConscript(['selfdrive/SConscript'])
SConscript(['iqpilot/selfdrive/SConscript'])
SConscript(['iqpilot/SConscript'])
if Dir('#tools/cabana/').exists() and GetOption('extras'):
SConscript(['tools/replay/SConscript'])
if Dir('#iqpilot/tools/cabana/').exists() and GetOption('extras'):
SConscript(['iqpilot/tools/replay/SConscript'])
if arch != "larch64":
SConscript(['tools/cabana/SConscript'])
if Dir('#tools/jotpluggler/').exists():
SConscript(['tools/jotpluggler/SConscript'])
SConscript(['iqpilot/tools/cabana/SConscript'])
if Dir('#iqpilot/tools/jotpluggler/').exists():
SConscript(['iqpilot/tools/jotpluggler/SConscript'])
env.CompilationDatabase('compile_commands.json')

1
artifacts/iqpilot_alc_private/.source_sha Normal file
View File

@@ -0,0 +1 @@
076c1d057f97b067222d74180fbb63ba5467254a

17
artifacts/iqpilot_alc_private/manifest.json
View File

@@ -1,4 +1,9 @@
{
"python/_iqclosure/_vendored.json": {
"mode": 420,
"sha256": "37517e5f3dc66819f61f5a7bb8ace1921282415f10551d2defa5c3eb0985b570",
"size": 3
},
"python/iqpilot_private/__init__.py": {
"mode": 420,
"sha256": "e3b0c44298fc1c149afbf4c8996fb92427ae41e4649b934ca495991b7852b855",
@@ -16,13 +21,13 @@
},
"python/iqpilot_private/konn3kt/iqlvbs/alc.cpython-312-aarch64-linux-gnu.so": {
"mode": 493,
"sha256": "16990383c7ba1d2f3d33e5e4c1746019101bc79bed867721b51de853a68002ed",
"size": 401688
"sha256": "268267fdfb6ae438f1b3c6f5f2c378a5b878e68357d5560f604f77d5c36a2cea",
"size": 402144
},
"python/iqpilot_private/konn3kt/iqlvbs/vehicle_state.cpython-312-aarch64-linux-gnu.so": {
"mode": 493,
"sha256": "bd060a8527d21501ce866f24d055f1c8bb03209b2730eb745b786780aace0aae",
"size": 69688
"sha256": "b3d33e86493408e5a02f2df753e1b257f003e5028480b5b040aa5a07eeed98be",
"size": 69712
},
"runtime": {
"entries": {
@@ -37,7 +42,7 @@
}
},
"signatures": {
"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=="
"python/iqpilot_private/konn3kt/iqlvbs/alc.cpython-312-aarch64-linux-gnu.so": "v0mM7HrM8Ssbm3E/TxIUefAh5aCkzbZIKDQk5yFN6kr4EgqmVyPMNkqu7kCsn4ah0fTzr7xCrhs/gXjGf+6yBQ==",
"python/iqpilot_private/konn3kt/iqlvbs/vehicle_state.cpython-312-aarch64-linux-gnu.so": "5KQhJ61bt8tNmKuegPVMnMiqEHKcnjhqMtCDv2CphlSS54L/GmsOZjieHQEcgpOIDw7n/7qz2k6iFnwJ6clLCg=="
}
}

1
artifacts/iqpilot_alc_private/manifest.json.sig Normal file
View File

@@ -0,0 +1 @@
Xr+Nn9b/ZoT2/S94B/I16aHVQI5dZZE3hGvpS6elgasC7e4JZouZvln/ue3Lh5Hg/NTJCCEStRmpEtNfgcjbAA==

1
artifacts/iqpilot_alc_private/python/_iqclosure/_vendored.json Normal file
View File

@@ -0,0 +1 @@
[]

BIN
artifacts/iqpilot_alc_private/python/iqpilot_private/konn3kt/iqlvbs/alc.cpython-312-aarch64-linux-gnu.so
View File

Binary file not shown.

BIN
artifacts/iqpilot_alc_private/python/iqpilot_private/konn3kt/iqlvbs/vehicle_state.cpython-312-aarch64-linux-gnu.so
View File

Binary file not shown.

9
artifacts/iqpilot_commander_private/manifest.json
View File

@@ -1,4 +1,9 @@
{
"python/_iqclosure/_vendored.json": {
"mode": 420,
"sha256": "37517e5f3dc66819f61f5a7bb8ace1921282415f10551d2defa5c3eb0985b570",
"size": 3
},
"python/iqpilot_private/__init__.py": {
"mode": 420,
"sha256": "e3b0c44298fc1c149afbf4c8996fb92427ae41e4649b934ca495991b7852b855",
@@ -16,7 +21,7 @@
},
"python/iqpilot_private/konn3kt/iqlvbs/iqlvbs_commander.cpython-312-aarch64-linux-gnu.so": {
"mode": 493,
"sha256": "384571df8380e5eef92806f3fdb6c9d1bc879af2ebdce9259bcfc486d506a4d0",
"sha256": "9b48803b46906943d7cb10b7e4b4f38c880f06a5c281f254a89e2254965b025f",
"size": 68080
},
"runtime": {
@@ -28,6 +33,6 @@
}
},
"signatures": {
"python/iqpilot_private/konn3kt/iqlvbs/iqlvbs_commander.cpython-312-aarch64-linux-gnu.so": "cH88ob+s+ZRJaNgbrRN2CQqCiQMBlo9yXdBvN3gteSC7OYlJ5z8OZND7eZSEfeeq2O7E8ZeZDs9WDU0WfP8DCw=="
"python/iqpilot_private/konn3kt/iqlvbs/iqlvbs_commander.cpython-312-aarch64-linux-gnu.so": "RwstZkEGexV6fia/R9BRk+6CQ9Q6SGJopSJf04UpMpaw5fRtoPjLOrDFOt9kU8GPmCmW40gaXhJK+rimnBYnBw=="
}
}

1
artifacts/iqpilot_commander_private/manifest.json.sig Normal file
View File

@@ -0,0 +1 @@
TV78NXdEYQiabAzLIqS2lsZbp5bO0y4EPGwLRUMyKeEH2BStZiFfpNPvlqSn5HGI+RjmTq+p5rRUDMNofbZGBw==

1
artifacts/iqpilot_commander_private/python/_iqclosure/_vendored.json Normal file
View File

@@ -0,0 +1 @@
[]

BIN
artifacts/iqpilot_commander_private/python/iqpilot_private/konn3kt/iqlvbs/iqlvbs_commander.cpython-312-aarch64-linux-gnu.so
View File

Binary file not shown.

577
artifacts/iqpilot_emac_private/manifest.json Normal file
View File

@@ -0,0 +1,577 @@
{
"python/_iqclosure/_vendored.json": {
"mode": 420,
"sha256": "5cae92c259782dd90e2523851ba0b5262fb4e7325f42578af7c55977e9c120b9",
"size": 3558
},
"python/_iqclosure/iqpilot/__init__.py": {
"mode": 420,
"sha256": "e3b0c44298fc1c149afbf4c8996fb92427ae41e4649b934ca495991b7852b855",
"size": 0
},
"python/_iqclosure/iqpilot/common/__init__.py": {
"mode": 420,
"sha256": "e3b0c44298fc1c149afbf4c8996fb92427ae41e4649b934ca495991b7852b855",
"size": 0
},
"python/_iqclosure/iqpilot/common/api/__init__.py": {
"mode": 420,
"sha256": "97d6e0d8cbd3e47797d734322b42d04efea4d95a2dae5a56f1baef3fd847aa0d",
"size": 939
},
"python/_iqclosure/iqpilot/common/api/base.py": {
"mode": 420,
"sha256": "be55c83869bad911ee0204e8779b4673653f7d29ae4f524c978a280bd84c7777",
"size": 3187
},
"python/_iqclosure/iqpilot/common/api/comma_connect.py": {
"mode": 420,
"sha256": "e7c2dd0b56fb8533c2cbc614e728a9ffc5ce7360524cfa62a02f5235bc23b9a8",
"size": 267
},
"python/_iqclosure/iqpilot/common/atlas_alerts.py": {
"mode": 420,
"sha256": "ef0d867f6213bb67ed6b7fd6ef4e694eb1705d9dc694163a80c8dd9572932954",
"size": 9448
},
"python/_iqclosure/iqpilot/common/basedir.py": {
"mode": 420,
"sha256": "58cfa5d6405903ee1acecc4544463bb8f47a66921b120c382143aa80d8539b28",
"size": 106
},
"python/_iqclosure/iqpilot/common/constants.py": {
"mode": 420,
"sha256": "fc439d59c24b95ed349b84f1a5862df3ea50e0891dbec4ea3cf5e1e0ccff6513",
"size": 434
},
"python/_iqclosure/iqpilot/common/file_chunker.py": {
"mode": 420,
"sha256": "0c6dfabdbf1aa737f4dc306b10d12e6dcc3a87720dbbfe23df2f40490453b06e",
"size": 1898
},
"python/_iqclosure/iqpilot/common/filter_simple.py": {
"mode": 420,
"sha256": "4b34753a3d3eea6d1578d88d21350045173a3809d38aed232875d1c5f34b4141",
"size": 1929
},
"python/_iqclosure/iqpilot/common/gpio.py": {
"mode": 420,
"sha256": "01ba8cb32a644a2df545e764ba19bf1c7a320a217f983f8bac73b45d3a6891cf",
"size": 2408
},
"python/_iqclosure/iqpilot/common/i2c.py": {
"mode": 420,
"sha256": "4af8f9da7504a548d15f18b91d71c1379cb7c05c84be031448764cc49920ce68",
"size": 2449
},
"python/_iqclosure/iqpilot/common/iq_perf.py": {
"mode": 420,
"sha256": "16b9a744fc1346d9660eab59c87c05a56015fea6bfc30317047e5f95aba2f7d2",
"size": 7218
},
"python/_iqclosure/iqpilot/common/issue_debug.py": {
"mode": 420,
"sha256": "73a10d846c735d4bcfda65f7828aa82192349b53920cb9d1b01cef8c211982fe",
"size": 1128
},
"python/_iqclosure/iqpilot/common/logging_extra.py": {
"mode": 420,
"sha256": "ce9b3b3d8498d76e1c4c0352a3e98e72759bb7f367ff8629c1329c46040595c3",
"size": 6628
},
"python/_iqclosure/iqpilot/common/realtime.py": {
"mode": 420,
"sha256": "f137ef62c1602a1f7c329731883dc518c62288bd25596891858d460ae3209ae2",
"size": 3966
},
"python/_iqclosure/iqpilot/common/spinner.py": {
"mode": 420,
"sha256": "33a2eb2aa3148492414035c5dc7150b3a69467905d7a3e58c8f65cfe70d3552c",
"size": 1371
},
"python/_iqclosure/iqpilot/common/steer_delay.py": {
"mode": 420,
"sha256": "933220f41a027fe73bfcf7af1d40a7b58d1a751dc9a4fe16ddccc66f2c58c549",
"size": 2541
},
"python/_iqclosure/iqpilot/common/swaglog.py": {
"mode": 420,
"sha256": "0a133b4ef49f80b8890f115e81273f3f3e21351ae79f92dc02abc033e6d24027",
"size": 5274
},
"python/_iqclosure/iqpilot/common/time_helpers.py": {
"mode": 420,
"sha256": "c14cda1f84149d5ffa63a2adec5ca24126f77c1412a78412e5e3e68a6fd12bcf",
"size": 474
},
"python/_iqclosure/iqpilot/common/utils.py": {
"mode": 420,
"sha256": "6475ad94bc89252398c41e9eabf7929ba5eb8fa23a08ad7545e7a67b400af9f3",
"size": 8090
},
"python/_iqclosure/iqpilot/konn3kt/__init__.py": {
"mode": 420,
"sha256": "e3b0c44298fc1c149afbf4c8996fb92427ae41e4649b934ca495991b7852b855",
"size": 0
},
"python/_iqclosure/iqpilot/konn3kt/cloud_client.py": {
"mode": 420,
"sha256": "4d58e62f8d628a8570881f2b87c82c801b46928fbb84f8dab8a81a9b55ba4e15",
"size": 493
},
"python/_iqclosure/iqpilot/konn3kt/registration.py": {
"mode": 420,
"sha256": "d8da8fa6373d54c3090b4466a975969317c9f5cd7c0ba20fd905e11ce9b95efd",
"size": 7948
},
"python/_iqclosure/iqpilot/selfdrive/__init__.py": {
"mode": 420,
"sha256": "e3b0c44298fc1c149afbf4c8996fb92427ae41e4649b934ca495991b7852b855",
"size": 0
},
"python/_iqclosure/iqpilot/selfdrive/controls/__init__.py": {
"mode": 420,
"sha256": "e3b0c44298fc1c149afbf4c8996fb92427ae41e4649b934ca495991b7852b855",
"size": 0
},
"python/_iqclosure/iqpilot/selfdrive/controls/lib/__init__.py": {
"mode": 420,
"sha256": "e3b0c44298fc1c149afbf4c8996fb92427ae41e4649b934ca495991b7852b855",
"size": 0
},
"python/_iqclosure/iqpilot/selfdrive/controls/lib/desire_helper.py": {
"mode": 420,
"sha256": "91008a050069861404b3f20977f5b8b6fdab4b3919e76e1ae95be49ce92f4ad8",
"size": 12298
},
"python/_iqclosure/iqpilot/selfdrive/controls/lib/drive_helpers.py": {
"mode": 420,
"sha256": "4246e7ac3a1c554e068a967472b2fe2793fb291d6468308badb25f5769fd1fb9",
"size": 3599
},
"python/_iqclosure/iqpilot/selfdrive/controls/lib/helpers/__init__.py": {
"mode": 420,
"sha256": "e3b0c44298fc1c149afbf4c8996fb92427ae41e4649b934ca495991b7852b855",
"size": 0
},
"python/_iqclosure/iqpilot/selfdrive/controls/lib/helpers/lane_change.py": {
"mode": 420,
"sha256": "3e7557a578a971f45e704b4386f6ac386745c8d044f45dab4b9b6cdd492a7f42",
"size": 7875
},
"python/_iqclosure/iqpilot/selfdrive/controls/lib/helpers/lane_turn.py": {
"mode": 420,
"sha256": "faa6f87b227c805e8890a91139e8d16d7874c4b99b330dda08080392ce3479ec",
"size": 4618
},
"python/_iqclosure/iqpilot/selfdrive/controls/lib/helpers/lateral_edge_guard.py": {
"mode": 420,
"sha256": "6bb6bcb8e5323480bf1b992549279a7d4c17a1bd1bd260863cf7b61b887fa735",
"size": 10227
},
"python/_iqclosure/iqpilot/selfdrive/iqmodeld/__init__.py": {
"mode": 420,
"sha256": "e3b0c44298fc1c149afbf4c8996fb92427ae41e4649b934ca495991b7852b855",
"size": 0
},
"python/_iqclosure/iqpilot/selfdrive/iqmodeld/big_catalog.py": {
"mode": 420,
"sha256": "7ec5df43ce10b47ce925467e61100123e828b8e72bdab20bef4c8ac0ffd3cc7a",
"size": 369
},
"python/_iqclosure/iqpilot/selfdrive/iqmodeld/camera.py": {
"mode": 420,
"sha256": "6d6b309a557a6f7a97e97dd0227d2ec66c215523f19c8e3fcf7256940115da5c",
"size": 2646
},
"python/_iqclosure/iqpilot/selfdrive/iqmodeld/config.py": {
"mode": 420,
"sha256": "056c744bbd9f1fe1053555d9b1fc1257d53093d69ddc94be3f05d840eb70781c",
"size": 3590
},
"python/_iqclosure/iqpilot/selfdrive/iqmodeld/daemon.py": {
"mode": 420,
"sha256": "15002cb6c81cca8c7aec572568da1e202c55777bbd21fefaf8fda36fd0d39b6e",
"size": 30295
},
"python/_iqclosure/iqpilot/selfdrive/iqmodeld/driving_action.py": {
"mode": 420,
"sha256": "863a9b18e82c7b2ab821541f498228ca7f5904ee38d5ac282c66dfaab7fdc492",
"size": 2381
},
"python/_iqclosure/iqpilot/selfdrive/iqmodeld/egpu_helpers.py": {
"mode": 420,
"sha256": "0b8f118dc45ad0c3809855800960b47c893ee121ea75d94e8569dcce46d58ce5",
"size": 7312
},
"python/_iqclosure/iqpilot/selfdrive/iqmodeld/egpu_model.py": {
"mode": 420,
"sha256": "eb3ebc7065a863d51d5bb4359d68f26fc2f1929e1d7e044377f15cf5bac7f77d",
"size": 367
},
"python/_iqclosure/iqpilot/selfdrive/iqmodeld/emac_model_meta.py": {
"mode": 420,
"sha256": "e47406eabf9c96bdf393ddaba8936fbd01755b2728ebe9413b2ab24b94de5d48",
"size": 377
},
"python/_iqclosure/iqpilot/selfdrive/iqmodeld/messaging.py": {
"mode": 420,
"sha256": "17d48331eeeaf746ee6982a9c65f28d740a15a4c34c37617115c104894d49c06",
"size": 11920
},
"python/_iqclosure/iqpilot/selfdrive/iqmodeld/metadata.py": {
"mode": 420,
"sha256": "dd1559099407687ee2dc81e180e05b9954e712c39e1a82e39c1b00b70c43374a",
"size": 3016
},
"python/_iqclosure/iqpilot/selfdrive/iqmodeld/model_bundle_downloader.py": {
"mode": 420,
"sha256": "27aade5852507baf84ed460b9446d0de4df58a4a535b82c301113040b7ba472a",
"size": 7305
},
"python/_iqclosure/iqpilot/selfdrive/iqmodeld/model_channel.py": {
"mode": 420,
"sha256": "2d706993df418501d93dc766ffe452131e565bfbc2e8412b527dc14915e92832",
"size": 1872
},
"python/_iqclosure/iqpilot/selfdrive/iqmodeld/model_warp.py": {
"mode": 420,
"sha256": "075c89f8740945b84d0c554cccf4b0198d0738c85bde52e64bab69b01e482987",
"size": 3325
},
"python/_iqclosure/iqpilot/selfdrive/iqmodeld/models/__init__.py": {
"mode": 420,
"sha256": "5865243a95ca4557d2a960302124a9898545cc3a8387e88ef569308e3d0f7077",
"size": 119
},
"python/_iqclosure/iqpilot/selfdrive/iqmodeld/models/combined_artifact.py": {
"mode": 420,
"sha256": "321e7223249dc7dba93effe8e834f225fb6067abf49da17fe9a6faeb71ba8e49",
"size": 2786
},
"python/_iqclosure/iqpilot/selfdrive/iqmodeld/models/fetcher.py": {
"mode": 420,
"sha256": "23622cdec9eed5649a05422ba19eae956ba16a63850927af4a222c3d9030a1bb",
"size": 399
},
"python/_iqclosure/iqpilot/selfdrive/iqmodeld/models/helpers.py": {
"mode": 420,
"sha256": "e4f524c7595754fd9db86724a7bd9b67524c630275ac78599ab6838d1aa7d809",
"size": 11037
},
"python/_iqclosure/iqpilot/selfdrive/iqmodeld/models/inference_state.py": {
"mode": 420,
"sha256": "bdfd1c1b538e2342b17d08aa6ebe0c041c2f721025bcd7334434d1139fe6063d",
"size": 268
},
"python/_iqclosure/iqpilot/selfdrive/iqmodeld/models/runners/__init__.py": {
"mode": 420,
"sha256": "e3b0c44298fc1c149afbf4c8996fb92427ae41e4649b934ca495991b7852b855",
"size": 0
},
"python/_iqclosure/iqpilot/selfdrive/iqmodeld/models/runners/model_runner.py": {
"mode": 420,
"sha256": "e0eb9d7bff1da89d52b09596f7e7109fc25c3b9b86a89674382a7dde9e0a4199",
"size": 8555
},
"python/_iqclosure/iqpilot/selfdrive/iqmodeld/models/runners/tinygrad/__init__.py": {
"mode": 420,
"sha256": "e3b0c44298fc1c149afbf4c8996fb92427ae41e4649b934ca495991b7852b855",
"size": 0
},
"python/_iqclosure/iqpilot/selfdrive/iqmodeld/models/runners/tinygrad/combined_split_runner.py": {
"mode": 420,
"sha256": "b7c15991f4fe9c6b4d7fb6a2a37256c142ab40cdd53f52bd3c59accc3a3618b1",
"size": 10929
},
"python/_iqclosure/iqpilot/selfdrive/iqmodeld/models/runners/tinygrad/fused_runner.py": {
"mode": 420,
"sha256": "25304fe7ae034d1bb8c9c86a2757704944539be6a0c69c322e75adc84f66e221",
"size": 7885
},
"python/_iqclosure/iqpilot/selfdrive/iqmodeld/models/runners/tinygrad/model_types.py": {
"mode": 420,
"sha256": "b68fe2c7f0013fe6780e765882b4a1ac7074834ea6d370cbca90a51d2f614091",
"size": 1981
},
"python/_iqclosure/iqpilot/selfdrive/iqmodeld/models/runners/tinygrad/supercombo_runner.py": {
"mode": 420,
"sha256": "10ae83948aba757a48a781fed90be1cce65e8853c8b67dcb7b4ec6928e312005",
"size": 14219
},
"python/_iqclosure/iqpilot/selfdrive/iqmodeld/models/runners/tinygrad/tinygrad_runner.py": {
"mode": 420,
"sha256": "58ffe0afb2cd9527d2a1d5bbebf0471af8c8210534e266ab80488a7f89bb06ba",
"size": 7684
},
"python/_iqclosure/iqpilot/selfdrive/iqmodeld/models/split_model_constants.py": {
"mode": 420,
"sha256": "1fe374d5eebef362923a5d90f1218bcd6b916eb60584d4a55f840de2ef6c4cf3",
"size": 2145
},
"python/_iqclosure/iqpilot/selfdrive/iqmodeld/parser.py": {
"mode": 420,
"sha256": "1dc47b2c167f1271c5c45d6ba0582cb906f4b59b0621b6a6d971da3f329d66e8",
"size": 9212
},
"python/_iqclosure/iqpilot/selfdrive/iqmodeld/runtime/__init__.py": {
"mode": 420,
"sha256": "e3b0c44298fc1c149afbf4c8996fb92427ae41e4649b934ca495991b7852b855",
"size": 0
},
"python/_iqclosure/iqpilot/selfdrive/iqmodeld/runtime/tinygrad.py": {
"mode": 420,
"sha256": "316988acf63f98aca83c5662ffb2c6028ef1f8b94fd8d6fcc15e96d9ac0d8181",
"size": 779
},
"python/_iqclosure/iqpilot/selfdrive/iqmodeld/temporal_state.py": {
"mode": 420,
"sha256": "06ab9349b786559037ce68d5882b54ffbe39f76737c4d19a213b251240023d4a",
"size": 5501
},
"python/_iqclosure/iqpilot/selfdrive/iqmodeld/tools/__init__.py": {
"mode": 420,
"sha256": "e3b0c44298fc1c149afbf4c8996fb92427ae41e4649b934ca495991b7852b855",
"size": 0
},
"python/_iqclosure/iqpilot/selfdrive/iqmodeld/tools/compile_supercombo.py": {
"mode": 420,
"sha256": "d8fbc18f67ea1e330130ce0b228af298919c5304d299da07eb94d557c0827d25",
"size": 17739
},
"python/_iqclosure/iqpilot/selfdrive/iqmodeld/tools/compile_warp.py": {
"mode": 420,
"sha256": "2cc299b2cfda88ee2dbc52658f145fb7840e2dfc49a7371e22fe671beed0e57f",
"size": 5436
},
"python/_iqclosure/iqpilot/selfdrive/locationd/__init__.py": {
"mode": 420,
"sha256": "e3b0c44298fc1c149afbf4c8996fb92427ae41e4649b934ca495991b7852b855",
"size": 0
},
"python/_iqclosure/iqpilot/selfdrive/locationd/calibration_helpers.py": {
"mode": 420,
"sha256": "1501058881da2720b35915f0949c2bc311020386c2549d48111ab32a34386a6c",
"size": 982
},
"python/_iqclosure/iqpilot/selfdrive/locationd/calibrationd.py": {
"mode": 420,
"sha256": "6bceebb5a15895d2b6d20a28cbd14b9112f9422e0a0cf466314a7e8779da39dc",
"size": 15698
},
"python/_iqclosure/iqpilot/selfdrive/locationd/helpers.py": {
"mode": 420,
"sha256": "be47384b3437670192f8bfd3dc679d57f0678f9f4fe34759419b0f2eb01ab117",
"size": 6828
},
"python/_iqclosure/iqpilot/selfdrive/locationd/locationd.py": {
"mode": 420,
"sha256": "8a7d7c89ea11998e418a52355a3d719ef3bf4ddfd0af13da2234d08348c25fe7",
"size": 15169
},
"python/_iqclosure/iqpilot/selfdrive/locationd/models/__init__.py": {
"mode": 420,
"sha256": "e3b0c44298fc1c149afbf4c8996fb92427ae41e4649b934ca495991b7852b855",
"size": 0
},
"python/_iqclosure/iqpilot/selfdrive/locationd/models/constants.py": {
"mode": 420,
"sha256": "276b20ca6187466efe6273e66100d92f4f54bd2b393c77329fb196a512b707bf",
"size": 1952
},
"python/_iqclosure/iqpilot/selfdrive/locationd/models/pose_kf.py": {
"mode": 420,
"sha256": "98aabe0a0b1e94f0c34af459ead9488901f08722ada3a87d6a33c00241dee211",
"size": 2930
},
"python/_iqclosure/iqpilot/selfdrive/selfdrived/__init__.py": {
"mode": 420,
"sha256": "e3b0c44298fc1c149afbf4c8996fb92427ae41e4649b934ca495991b7852b855",
"size": 0
},
"python/_iqclosure/iqpilot/selfdrive/selfdrived/alertmanager.py": {
"mode": 420,
"sha256": "c601bac30951a750d3ebf3d30f06c597bacd148210b1c405d5d3eca79bb7c4e3",
"size": 2045
},
"python/_iqclosure/iqpilot/selfdrive/selfdrived/events.py": {
"mode": 420,
"sha256": "5e37693eeced1e3ba81f03234b7b0dad33b7564826d232d288998be5fae7efd6",
"size": 37038
},
"python/_iqclosure/iqpilot/selfdrive/state_estimation/__init__.py": {
"mode": 420,
"sha256": "85a0c22d638eb8f87b00ca2ea21aa4c0c76708fe3e2f3ccc1fa8f87f0c784450",
"size": 319
},
"python/_iqclosure/iqpilot/selfdrive/state_estimation/estimator.py": {
"mode": 420,
"sha256": "59ae4dd78c8f850a5acd28d2743abe1b9d19ebcaaf9c472f95b2766a73e056c6",
"size": 12577
},
"python/_iqclosure/iqpilot/selfdrive/ui/__init__.py": {
"mode": 420,
"sha256": "e3b0c44298fc1c149afbf4c8996fb92427ae41e4649b934ca495991b7852b855",
"size": 0
},
"python/_iqclosure/iqpilot/selfdrive/ui/feedback/__init__.py": {
"mode": 420,
"sha256": "e3b0c44298fc1c149afbf4c8996fb92427ae41e4649b934ca495991b7852b855",
"size": 0
},
"python/_iqclosure/iqpilot/selfdrive/ui/feedback/feedbackd.py": {
"mode": 420,
"sha256": "7c1e6cb2c3c2c6aae91210f6cbc5c372f33c28cc3edb292ef71439b83844615d",
"size": 2615
},
"python/_iqclosure/iqpilot/system/__init__.py": {
"mode": 420,
"sha256": "e3b0c44298fc1c149afbf4c8996fb92427ae41e4649b934ca495991b7852b855",
"size": 0
},
"python/_iqclosure/iqpilot/system/camerad/__init__.py": {
"mode": 420,
"sha256": "e3b0c44298fc1c149afbf4c8996fb92427ae41e4649b934ca495991b7852b855",
"size": 0
},
"python/_iqclosure/iqpilot/system/camerad/cameras/__init__.py": {
"mode": 420,
"sha256": "e3b0c44298fc1c149afbf4c8996fb92427ae41e4649b934ca495991b7852b855",
"size": 0
},
"python/_iqclosure/iqpilot/system/camerad/cameras/nv12_info.py": {
"mode": 420,
"sha256": "90b0b00df8a7baa638940c2e73cbe2e79059af0acd19812ca6d765793ce4fda0",
"size": 829
},
"python/_iqclosure/iqpilot/system/hardware/__init__.py": {
"mode": 420,
"sha256": "889391bffca493e74bfc108cae2976a53b644caca13ab600061fd29e9622c627",
"size": 695
},
"python/_iqclosure/iqpilot/system/hardware/base.py": {
"mode": 420,
"sha256": "a56293f7c23e503ff62d0d3ed39f1831b5544a65b37378165934361a5ddcd304",
"size": 4966
},
"python/_iqclosure/iqpilot/system/hardware/hw.py": {
"mode": 420,
"sha256": "c69185d8f6482c8b021f687b793246d2e2ede853e89a88748c64b85f60711b59",
"size": 3638
},
"python/_iqclosure/iqpilot/system/hardware/pc/__init__.py": {
"mode": 420,
"sha256": "e3b0c44298fc1c149afbf4c8996fb92427ae41e4649b934ca495991b7852b855",
"size": 0
},
"python/_iqclosure/iqpilot/system/hardware/pc/hardware.py": {
"mode": 420,
"sha256": "2520895597c375ae570bd04d5f189e874c89d642f3f07bdbeb20388cb6691433",
"size": 258
},
"python/_iqclosure/iqpilot/system/hardware/tici/__init__.py": {
"mode": 420,
"sha256": "e3b0c44298fc1c149afbf4c8996fb92427ae41e4649b934ca495991b7852b855",
"size": 0
},
"python/_iqclosure/iqpilot/system/hardware/tici/amplifier.py": {
"mode": 420,
"sha256": "98f46f8705259b00efae7274186b4e68201aad467a19de4f16d5f850d10ab0b5",
"size": 7585
},
"python/_iqclosure/iqpilot/system/hardware/tici/hardware.py": {
"mode": 420,
"sha256": "f1ddffa9bdb5071a49fd65eb4b49958b60b1873e1ec70e6f3f2e2b2501abce2f",
"size": 25584
},
"python/_iqclosure/iqpilot/system/hardware/tici/iwlist.py": {
"mode": 420,
"sha256": "d22858982705531732d5a20a45a73795496e5bbd9e23028c24609a7e58ac7aed",
"size": 890
},
"python/_iqclosure/iqpilot/system/hardware/tici/lpa.py": {
"mode": 420,
"sha256": "2bffb84aef35892c4788e80b6d937bf2eebd15e226b72a82c4f39217667c1ca0",
"size": 53692
},
"python/_iqclosure/iqpilot/system/hardware/tici/pins.py": {
"mode": 420,
"sha256": "b57ba4262e6a6a79f2db15c27356acf3e54f579a7edd13b483b2e16d140aca6d",
"size": 678
},
"python/_iqclosure/iqpilot/system/hardware/tici/usb_storage.py": {
"mode": 420,
"sha256": "3352f2cf70d38e7158ef8ac20bb736dba55c73239cbdd3ab7f431dbacfb5027e",
"size": 2189
},
"python/_iqclosure/iqpilot/system/hardware/usb.py": {
"mode": 420,
"sha256": "8c5b26f3967100ce0e5af7043a47a852fb5e8311f00ac95e7a263934c38e4ec0",
"size": 8069
},
"python/_iqclosure/iqpilot/system/micd.py": {
"mode": 420,
"sha256": "1a59cbe6b72fcca5723ab56dc09f52039817299071a8d8da37f04acea58776bc",
"size": 5977
},
"python/_iqclosure/iqpilot/system/sentry.py": {
"mode": 420,
"sha256": "c3da338feb533937b6a5258ee450b6a3ff7a77b69cc9e48e31d16b875855caf3",
"size": 5229
},
"python/_iqclosure/iqpilot/system/version.py": {
"mode": 420,
"sha256": "3a6bc7520dae7944a1da6f911cd090704a463e011f39949ea6c9df51b0e8c616",
"size": 6033
},
"python/iqpilot_private/__init__.py": {
"mode": 420,
"sha256": "e3b0c44298fc1c149afbf4c8996fb92427ae41e4649b934ca495991b7852b855",
"size": 0
},
"python/iqpilot_private/emac/__init__.py": {
"mode": 420,
"sha256": "5865243a95ca4557d2a960302124a9898545cc3a8387e88ef569308e3d0f7077",
"size": 119
},
"python/iqpilot_private/emac/bulk_transport.cpython-312-aarch64-linux-gnu.so": {
"mode": 493,
"sha256": "f42a1bab373092b3010ca45b02d7ea7b27cd1e1e574c97f701129b2fad01f833",
"size": 333888
},
"python/iqpilot_private/emac/ffs_holder.cpython-312-aarch64-linux-gnu.so": {
"mode": 493,
"sha256": "306f0362e86558d33072f800b736fa71bfaf4ee5e4383c4e743b72b74724d2e3",
"size": 68064
},
"python/iqpilot_private/emac/mac_client.cpython-312-aarch64-linux-gnu.so": {
"mode": 493,
"sha256": "31339f53f093c3877e13eac90a6647cbca2de92f339c47e5006a85bcd2e3656c",
"size": 267960
},
"python/iqpilot_private/emac/mac_protocol.cpython-312-aarch64-linux-gnu.so": {
"mode": 493,
"sha256": "d5e855d1784a8ba66844639b871d2747ccce264c55359cf0e641ce7e3d66460d",
"size": 203512
},
"python/iqpilot_private/emac/maciqmodeld.cpython-312-aarch64-linux-gnu.so": {
"mode": 493,
"sha256": "34d4ef1746143ea714f84f3c325e2ddaecec6a19c544fed68d8fc4080578c73f",
"size": 399040
},
"runtime": {
"entries": {
"iqpilot_private.emac.maciqmodeld": {
"path": "python/iqpilot_private/emac/maciqmodeld.cpython-312-aarch64-linux-gnu.so",
"type": "python-module"
}
}
},
"signatures": {
"python/iqpilot_private/emac/bulk_transport.cpython-312-aarch64-linux-gnu.so": "7BtEAP11moL2lbQW5dYt1hDqJmDV1EuiEoSfJ0Tfislun2kS+/rwPiy7Ws8kjgrKLv+ZlXlWz6G5IBWk6z+aAw==",
"python/iqpilot_private/emac/ffs_holder.cpython-312-aarch64-linux-gnu.so": "Sv2riVgvm+4PDVTDLTuy1wpf15JObbP8hSAD4iiwF4RUs49GVGhulXkjzO28mLmF6tEuNDNWHRucxmuZrCKNAg==",
"python/iqpilot_private/emac/mac_client.cpython-312-aarch64-linux-gnu.so": "d4kaBPAEwe55BrQEVHWmxcVlEoQPsMVLOAYgpZVd6uiBnwWSUxl1X5nmqdKtxWSk8TFqU2xZkhfw4OOgG9JmDw==",
"python/iqpilot_private/emac/mac_protocol.cpython-312-aarch64-linux-gnu.so": "hvBkDKSMKjpxmRzhhGoRZT5WuOWLY+g3p2NIMlA7b6qYYpVaApoZNuquOerywEF1zJnwPiwpsg5QtQVe/5DfAA==",
"python/iqpilot_private/emac/maciqmodeld.cpython-312-aarch64-linux-gnu.so": "d3JAlH7P/X87II6iobeQ51melHy1vxdlXn0Nzo2DG65rcdVLM2B66YETPpwlX6Qv9NxhUlDcl/364CR+V+eyDQ=="
}
}

1
artifacts/iqpilot_emac_private/manifest.json.sig Normal file
View File

@@ -0,0 +1 @@
KylnzVsK5XC3Jq1G7bBbKa5lv+oJSlfBpBsNpWMEEUgHDyOEj7+5EeeHDOgy8Lt4LEFgEeIV2WEJW0GJVpxUCQ==

86
artifacts/iqpilot_emac_private/python/_iqclosure/_vendored.json Normal file
View File

@@ -0,0 +1,86 @@
[
"iqpilot.common.api",
"iqpilot.common.api.base",
"iqpilot.common.api.comma_connect",
"iqpilot.common.atlas_alerts",
"iqpilot.common.basedir",
"iqpilot.common.constants",
"iqpilot.common.file_chunker",
"iqpilot.common.filter_simple",
"iqpilot.common.gpio",
"iqpilot.common.i2c",
"iqpilot.common.iq_perf",
"iqpilot.common.issue_debug",
"iqpilot.common.logging_extra",
"iqpilot.common.realtime",
"iqpilot.common.spinner",
"iqpilot.common.steer_delay",
"iqpilot.common.swaglog",
"iqpilot.common.time_helpers",
"iqpilot.common.utils",
"iqpilot.konn3kt.cloud_client",
"iqpilot.konn3kt.registration",
"iqpilot.selfdrive.controls.lib.desire_helper",
"iqpilot.selfdrive.controls.lib.drive_helpers",
"iqpilot.selfdrive.controls.lib.helpers.lane_change",
"iqpilot.selfdrive.controls.lib.helpers.lane_turn",
"iqpilot.selfdrive.controls.lib.helpers.lateral_edge_guard",
"iqpilot.selfdrive.iqmodeld.big_catalog",
"iqpilot.selfdrive.iqmodeld.camera",
"iqpilot.selfdrive.iqmodeld.config",
"iqpilot.selfdrive.iqmodeld.daemon",
"iqpilot.selfdrive.iqmodeld.driving_action",
"iqpilot.selfdrive.iqmodeld.egpu_helpers",
"iqpilot.selfdrive.iqmodeld.egpu_model",
"iqpilot.selfdrive.iqmodeld.emac_model_meta",
"iqpilot.selfdrive.iqmodeld.messaging",
"iqpilot.selfdrive.iqmodeld.metadata",
"iqpilot.selfdrive.iqmodeld.model_bundle_downloader",
"iqpilot.selfdrive.iqmodeld.model_channel",
"iqpilot.selfdrive.iqmodeld.model_warp",
"iqpilot.selfdrive.iqmodeld.models",
"iqpilot.selfdrive.iqmodeld.models.combined_artifact",
"iqpilot.selfdrive.iqmodeld.models.fetcher",
"iqpilot.selfdrive.iqmodeld.models.helpers",
"iqpilot.selfdrive.iqmodeld.models.inference_state",
"iqpilot.selfdrive.iqmodeld.models.runners.model_runner",
"iqpilot.selfdrive.iqmodeld.models.runners.tinygrad.combined_split_runner",
"iqpilot.selfdrive.iqmodeld.models.runners.tinygrad.fused_runner",
"iqpilot.selfdrive.iqmodeld.models.runners.tinygrad.model_types",
"iqpilot.selfdrive.iqmodeld.models.runners.tinygrad.supercombo_runner",
"iqpilot.selfdrive.iqmodeld.models.runners.tinygrad.tinygrad_runner",
"iqpilot.selfdrive.iqmodeld.models.split_model_constants",
"iqpilot.selfdrive.iqmodeld.parser",
"iqpilot.selfdrive.iqmodeld.runtime.tinygrad",
"iqpilot.selfdrive.iqmodeld.temporal_state",
"iqpilot.selfdrive.iqmodeld.tools.compile_supercombo",
"iqpilot.selfdrive.iqmodeld.tools.compile_warp",
"iqpilot.selfdrive.locationd.calibration_helpers",
"iqpilot.selfdrive.locationd.calibrationd",
"iqpilot.selfdrive.locationd.helpers",
"iqpilot.selfdrive.locationd.locationd",
"iqpilot.selfdrive.locationd.models.constants",
"iqpilot.selfdrive.locationd.models.pose_kf",
"iqpilot.selfdrive.selfdrived.alertmanager",
"iqpilot.selfdrive.selfdrived.events",
"iqpilot.selfdrive.state_estimation",
"iqpilot.selfdrive.state_estimation.estimator",
"iqpilot.selfdrive.ui.feedback.feedbackd",
"iqpilot.system",
"iqpilot.system.camerad.cameras.nv12_info",
"iqpilot.system.hardware",
"iqpilot.system.hardware.base",
"iqpilot.system.hardware.hw",
"iqpilot.system.hardware.pc.hardware",
"iqpilot.system.hardware.tici",
"iqpilot.system.hardware.tici.amplifier",
"iqpilot.system.hardware.tici.hardware",
"iqpilot.system.hardware.tici.iwlist",
"iqpilot.system.hardware.tici.lpa",
"iqpilot.system.hardware.tici.pins",
"iqpilot.system.hardware.tici.usb_storage",
"iqpilot.system.hardware.usb",
"iqpilot.system.micd",
"iqpilot.system.sentry",
"iqpilot.system.version"
]

0
cereal/messaging/tests/__init__.py → artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/__init__.py
View File

0
common/__init__.py → artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/common/__init__.py
View File

26
artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/common/api/__init__.py Normal file
View File

@@ -0,0 +1,26 @@
import iqpilot.common.api.comma_connect
class Api:
def __init__(self, dongle_id):
self.service = iqpilot.common.api.comma_connect.CommaConnectApi(dongle_id)
def request(self, method, endpoint, **params):
return self.service.request(method, endpoint, **params)
def get(self, *args, **kwargs):
return self.service.get(*args, **kwargs)
def post(self, *args, **kwargs):
return self.service.post(*args, **kwargs)
def get_token(self, payload_extra=None, expiry_hours=1):
return self.service.get_token(payload_extra, expiry_hours)
def api_get(endpoint, method='GET', timeout=None, access_token=None, session=None, **params):
return iqpilot.common.api.comma_connect.CommaConnectApi(None).api_get(endpoint, method, timeout, access_token, session, **params)
def get_key_pair() -> tuple[str, str, str] | tuple[None, None, None]:
return iqpilot.common.api.comma_connect.CommaConnectApi(None).get_key_pair()

84
artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/common/api/base.py Normal file
View File

@@ -0,0 +1,84 @@
import jwt
import os
import requests
import unicodedata
from datetime import datetime, timedelta, UTC
from functools import lru_cache
from iqpilot.system.hardware.hw import Paths
from iqpilot.system.version import get_version
# name: jwt signature algorithm
KEYS = {"id_rsa": "RS256",
"id_ecdsa": "ES256"}
@lru_cache(maxsize=4)
def load_signing_key(private_key: str):
# PyJWT re-parses a PEM string on every encode; an RSA parse is ~40ms, so cache the key object
try:
from cryptography.hazmat.primitives.serialization import load_pem_private_key
return load_pem_private_key(private_key.encode(), password=None)
except Exception:
return private_key
class BaseApi:
def __init__(self, dongle_id, api_host, user_agent="openpilot-"):
self.dongle_id = dongle_id
self.api_host = api_host
self.user_agent = user_agent
self.jwt_algorithm, self.private_key, _ = self.get_key_pair()
def get(self, *args, **kwargs):
return self.request('GET', *args, **kwargs)
def post(self, *args, **kwargs):
return self.request('POST', *args, **kwargs)
def request(self, method, endpoint, timeout=None, access_token=None, **params):
return self.api_get(endpoint, method=method, timeout=timeout, access_token=access_token, **params)
def _get_token(self, payload_extra=None, expiry_hours=1, **extra_payload):
now = datetime.now(UTC).replace(tzinfo=None)
payload = {
'identity': self.dongle_id,
'nbf': now,
'iat': now,
'exp': now + timedelta(hours=expiry_hours),
**extra_payload
}
if payload_extra is not None:
payload.update(payload_extra)
key = load_signing_key(self.private_key) if self.private_key else self.private_key
token = jwt.encode(payload, key, algorithm=self.jwt_algorithm)
if isinstance(token, bytes):
token = token.decode('utf8')
return token
def get_token(self, payload_extra=None, expiry_hours=1):
return self._get_token(payload_extra, expiry_hours)
def remove_non_ascii_chars(self, text):
normalized_text = unicodedata.normalize('NFD', text)
ascii_encoded_text = normalized_text.encode('ascii', 'ignore')
return ascii_encoded_text.decode()
def api_get(self, endpoint, method='GET', timeout=None, access_token=None, session=None, json=None, **params):
headers = {}
if access_token is not None:
headers['Authorization'] = "JWT " + access_token
version = self.remove_non_ascii_chars(get_version())
headers['User-Agent'] = self.user_agent + version
# TODO: add session to Api
req = requests if session is None else session
return req.request(method, f"{self.api_host}/{endpoint}", timeout=timeout, headers=headers, json=json, params=params)
@staticmethod
def get_key_pair() -> tuple[str, str, str] | tuple[None, None, None]:
for key in KEYS:
if os.path.isfile(Paths.persist_root() + f'/comma/{key}') and os.path.isfile(Paths.persist_root() + f'/comma/{key}.pub'):
with open(Paths.persist_root() + f'/comma/{key}') as private, open(Paths.persist_root() + f'/comma/{key}.pub') as public:
return KEYS[key], private.read(), public.read()
return None, None, None

11
artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/common/api/comma_connect.py Normal file
View File

@@ -0,0 +1,11 @@
import os
from iqpilot.common.api.base import BaseApi
API_HOST = os.getenv('API_HOST', 'https://api-iqlabs.konn3kt.com')
class CommaConnectApi(BaseApi):
def __init__(self, dongle_id):
super().__init__(dongle_id, API_HOST)
self.user_agent = "openpilot-"

281
artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/common/atlas_alerts.py Normal file
View File

@@ -0,0 +1,281 @@
from __future__ import annotations
from abc import ABC, abstractmethod
from bisect import insort
from collections.abc import Callable, Iterable
from dataclasses import dataclass, field
from enum import IntEnum
import iqpilot.cereal.messaging as messaging
from iqpilot.cereal import car, log
from iqpilot.common.realtime import DT_CTRL
from iqpilot.system.hardware import HARDWARE
AlertSize = log.SelfdriveState.AlertSize
AlertStatus = log.SelfdriveState.AlertStatus
VisualAlert = car.CarControl.HUDControl.VisualAlert
AudibleAlert = car.CarControl.HUDControl.AudibleAlert
def _frames_for(seconds: float) -> int:
return int(seconds / DT_CTRL)
class Tier(IntEnum):
LOWEST = 0
LOWER = 1
LOW = 2
MID = 3
HIGH = 4
HIGHEST = 5
class Tags:
ENABLE = "enable"
PRE_ENABLE = "preEnable"
OVERRIDE_LATERAL = "overrideLateral"
OVERRIDE_LONGITUDINAL = "overrideLongitudinal"
NO_ENTRY = "noEntry"
WARNING = "warning"
USER_DISABLE = "userDisable"
SOFT_DISABLE = "softDisable"
IMMEDIATE_DISABLE = "immediateDisable"
PERMANENT = "permanent"
@dataclass(slots=True)
class AlertCard:
alert_text_1: str
alert_text_2: str
alert_status: log.SelfdriveState.AlertStatus
alert_size: log.SelfdriveState.AlertSize
priority: Tier
visual_alert: car.CarControl.HUDControl.VisualAlert
audible_alert: car.CarControl.HUDControl.AudibleAlert
duration: int
creation_delay: float = 0.0
alert_type: str = field(default="", init=False)
event_type: str | None = field(default=None, init=False)
def __init__(self,
alert_text_1: str,
alert_text_2: str,
alert_status: log.SelfdriveState.AlertStatus,
alert_size: log.SelfdriveState.AlertSize,
priority: Tier,
visual_alert: car.CarControl.HUDControl.VisualAlert,
audible_alert: car.CarControl.HUDControl.AudibleAlert,
duration: float,
creation_delay: float = 0.0):
self.alert_text_1 = alert_text_1
self.alert_text_2 = alert_text_2
self.alert_status = alert_status
self.alert_size = alert_size
self.priority = priority
self.visual_alert = visual_alert
self.audible_alert = audible_alert
self.duration = _frames_for(duration)
self.creation_delay = creation_delay
self.alert_type = ""
self.event_type = None
def __str__(self) -> str:
return f"{self.alert_text_1}/{self.alert_text_2} {self.priority} {self.visual_alert} {self.audible_alert}"
AlertFactory = Callable[[car.CarParams, car.CarState, messaging.SubMaster, bool, int, log.ControlsState], AlertCard]
def car_mode_entry_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> AlertCard:
del CS, sm, metric, soft_disable_time, personality
headline = "Enable Adaptive Cruise to Engage"
if CP.brand == "honda":
headline = "Enable Main Switch to Engage"
return NoEntryCard(headline)
class EventBook(ABC):
def __init__(self):
self._live_names: list[int] = []
self._latched_names: list[int] = []
self.event_counters: dict[int, int] = {}
@property
def events(self) -> list[int]:
return self._live_names
@events.setter
def events(self, values: list[int]) -> None:
self._live_names = values
@property
def static_events(self) -> list[int]:
return self._latched_names
@static_events.setter
def static_events(self, values: list[int]) -> None:
self._latched_names = values
@property
def names(self) -> list[int]:
return list(self._live_names)
def __len__(self) -> int:
return len(self._live_names)
def add(self, event_name: int, static: bool = False) -> None:
if static:
insort(self._latched_names, event_name)
insort(self._live_names, event_name)
def clear(self) -> None:
refreshed: dict[int, int] = {}
for event_name, frames_seen in self.event_counters.items():
refreshed[event_name] = frames_seen + 1 if event_name in self._live_names else 0
self.event_counters = refreshed
self._live_names = list(self._latched_names)
def contains(self, event_type: str) -> bool:
board = self.get_events_mapping()
return any(event_type in board.get(event_name, {}) for event_name in self._live_names)
def has(self, event_name: int) -> bool:
return event_name in self._live_names
def contains_in_list(self, events_list: list[int]) -> bool:
return any(event_name in self._live_names for event_name in events_list)
def remove(self, event_name: int, static: bool = False) -> None:
if static and event_name in self._latched_names:
self._latched_names.remove(event_name)
if event_name in self._live_names:
self.event_counters[event_name] = self.event_counters.get(event_name, 0) + 1
self._live_names.remove(event_name)
def add_from_msg(self, events: Iterable) -> None:
for event in events:
insort(self._live_names, event.name.raw)
def to_msg(self):
board = self.get_events_mapping()
outbound = []
for event_name in self._live_names:
msg = self.get_event_msg_type().new_message()
msg.name = event_name
for event_kind in board.get(event_name, {}):
setattr(msg, event_kind, True)
outbound.append(msg)
return outbound
def create_alerts(self, event_types: list[str], callback_args=None):
callback_args = [] if callback_args is None else callback_args
board = self.get_events_mapping()
spawned: list[AlertCard] = []
for event_name in self._live_names:
variants = board.get(event_name, {})
for event_type in event_types:
chosen = variants.get(event_type)
if chosen is None:
continue
alert = self._realize(chosen, callback_args)
age_frames = self.event_counters.get(event_name, 0) + 1
if age_frames * DT_CTRL < alert.creation_delay:
continue
alert.alert_type = f"{self.get_event_name(event_name)}/{event_type}"
alert.event_type = event_type
spawned.append(alert)
return spawned
@staticmethod
def _realize(candidate: AlertCard | AlertFactory, callback_args: list) -> AlertCard:
return candidate if isinstance(candidate, AlertCard) else candidate(*callback_args)
@abstractmethod
def get_events_mapping(self) -> dict[int, dict[str, AlertCard | AlertFactory]]:
raise NotImplementedError
@abstractmethod
def get_event_name(self, event: int) -> str:
raise NotImplementedError
@abstractmethod
def get_event_msg_type(self):
raise NotImplementedError
def _mici_reframe(primary: str, secondary: str) -> tuple[str, str, log.SelfdriveState.AlertSize]:
if HARDWARE.get_device_type() == "mici":
return secondary, primary, AlertSize.small
return primary, secondary, AlertSize.mid
class NoEntryCard(AlertCard):
def __init__(self,
alert_text_2: str,
alert_text_1: str = "IQ.Pilot Unavailable",
visual_alert: car.CarControl.HUDControl.VisualAlert = VisualAlert.none,
priority: Tier = Tier.LOW):
primary, secondary, size = _mici_reframe(alert_text_1, alert_text_2)
super().__init__(primary, secondary, AlertStatus.normal, size, priority, visual_alert, AudibleAlert.refuse, 3.0)
class GentleDisableCard(AlertCard):
def __init__(self, alert_text_2: str):
super().__init__(
"TAKE CONTROL IMMEDIATELY",
alert_text_2,
AlertStatus.userPrompt,
AlertSize.full,
Tier.MID,
VisualAlert.steerRequired,
AudibleAlert.warningSoft,
2.0,
)
class PendingDisableCard(GentleDisableCard):
def __init__(self, alert_text_2: str):
super().__init__(alert_text_2)
self.alert_text_1 = "IQ.Pilot will disengage"
class HardDisableCard(AlertCard):
def __init__(self, alert_text_2: str):
super().__init__(
"TAKE CONTROL IMMEDIATELY",
alert_text_2,
AlertStatus.critical,
AlertSize.full,
Tier.HIGHEST,
VisualAlert.steerRequired,
AudibleAlert.warningImmediate,
4.0,
)
class ChimeCard(AlertCard):
def __init__(self, audible_alert: car.CarControl.HUDControl.AudibleAlert):
super().__init__("", "", AlertStatus.normal, AlertSize.none, Tier.MID, VisualAlert.none, audible_alert, 0.2)
class BannerCard(AlertCard):
def __init__(self, alert_text_1: str, alert_text_2: str = "", duration: float = 0.2, priority: Tier = Tier.LOWER, creation_delay: float = 0.0):
size = AlertSize.mid if alert_text_2 else AlertSize.small
super().__init__(alert_text_1, alert_text_2, AlertStatus.normal, size, priority, VisualAlert.none, AudibleAlert.none, duration, creation_delay)
class BootCard(AlertCard):
def __init__(self, alert_text_1: str, alert_text_2: str = "Always keep hands on wheel and eyes on road", alert_status=AlertStatus.normal):
if HARDWARE.get_device_type() == "mici":
compact_secondary = "" if alert_text_2 == "Always keep hands on wheel and eyes on road" else alert_text_2
super().__init__(alert_text_1, compact_secondary, alert_status, AlertSize.small, Tier.LOWER, VisualAlert.none, AudibleAlert.none, 5.0)
else:
super().__init__(alert_text_1, alert_text_2, alert_status, AlertSize.mid, Tier.LOWER, VisualAlert.none, AudibleAlert.none, 5.0)
class AlertBase(AlertCard):
pass
NULL_ALERT = AlertCard("", "", AlertStatus.normal, AlertSize.none, Tier.LOWEST, VisualAlert.none, AudibleAlert.none, 0.0)

4
artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/common/basedir.py Normal file
View File

@@ -0,0 +1,4 @@
import os
BASEDIR = os.path.abspath(os.path.join(os.path.dirname(os.path.realpath(__file__)), "../.."))

0
common/constants.py → artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/common/constants.py
View File

0
common/file_chunker.py → artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/common/file_chunker.py
View File

0
common/filter_simple.py → artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/common/filter_simple.py
View File

0
common/gpio.py → artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/common/gpio.py
View File

0
common/i2c.py → artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/common/i2c.py
View File

187
artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/common/iq_perf.py Normal file
View File

@@ -0,0 +1,187 @@
#!/usr/bin/env python3
from __future__ import annotations
import time
from collections import deque
from dataclasses import dataclass
from typing import Any
import iqpilot.cereal.messaging as messaging
from iqpilot.cereal import custom
from iqpilot.common.swaglog import cloudlog
TRACE_SERVICE = "iqPerfTrace"
MAX_TRACE_SAMPLES = 16
_SHARED_PM: messaging.PubMaster | None = None
@dataclass(slots=True)
class PerfSample:
frame_id: int = 0
loop_dt_us: int = 0
update_us: int = 0
state_control_us: int = 0
publish_us: int = 0
tail_work_us: int = 0
rk_remaining_us: int = 0
stale_carcontrol_us: int = 0
stale_carcontrol_frames: int = 0
sendcan_gap_us: int = 0
model_eval_us: int = 0
model_dropped_frames: int = 0
model_backlog: int = 0
texture_decode_us: int = 0
texture_upload_us: int = 0
texture_unload_us: int = 0
texture_prune_us: int = 0
texture_consume_us: int = 0
texture_batch_size: int = 0
texture_bytes: int = 0
texture_cache_before: int = 0
texture_cache_after: int = 0
texture_unloaded: int = 0
memory_usage_percent: int = 0
gpu_usage_percent: int = 0
cpu_usage_percent: int = 0
flags: int = 0
class PerfTraceRing:
def __init__(self, size: int = MAX_TRACE_SAMPLES):
self._samples: deque[PerfSample] = deque(maxlen=size)
def push(self, sample: PerfSample) -> None:
self._samples.append(sample)
def snapshot(self) -> list[PerfSample]:
return list(self._samples)
class PerfTraceEmitter:
_SEVERITY_MAP = {
"info": custom.IQPerfTrace.Severity.info,
"warning": custom.IQPerfTrace.Severity.warning,
"error": custom.IQPerfTrace.Severity.error,
"critical": custom.IQPerfTrace.Severity.critical,
}
def __init__(self, process_name: str, pubmaster: messaging.PubMaster | None = None):
self.process_name = process_name
self._pm: messaging.PubMaster | None = pubmaster
self._last_emit_mono: dict[str, float] = {}
self._disabled = False
def _pubmaster(self) -> messaging.PubMaster:
global _SHARED_PM
if self._pm is not None:
return self._pm
if _SHARED_PM is None:
_SHARED_PM = messaging.PubMaster([TRACE_SERVICE])
self._pm = _SHARED_PM
return self._pm
@staticmethod
def _clamp_uint(value: int, bits: int) -> int:
return max(0, min(value, (1 << bits) - 1))
@staticmethod
def _clamp_int(value: int, bits: int) -> int:
lo = -(1 << (bits - 1))
hi = (1 << (bits - 1)) - 1
return max(lo, min(value, hi))
def emit(self, event_class: str, *,
severity: str = "warning",
frame_id: int = 0,
total_time_us: int = 0,
rk_remaining_us: int = 0,
batch_size: int = 0,
dropped_frames: int = 0,
backlog: int = 0,
flags: int = 0,
samples: list[PerfSample] | None = None,
missing_services: list[str] | None = None,
top_processes: list[str] | None = None,
detail: str = "",
min_interval_s: float = 0.0,
mirror_cloudlog: bool = True) -> bool:
if self._disabled:
return False
now = time.monotonic()
last_emit = self._last_emit_mono.get(event_class, 0.0)
if min_interval_s > 0.0 and (now - last_emit) < min_interval_s:
return False
self._last_emit_mono[event_class] = now
msg = messaging.new_message(TRACE_SERVICE)
trace = msg.iqPerfTrace
trace.process = self.process_name
trace.eventClass = event_class
trace.severity = self._SEVERITY_MAP.get(severity, custom.IQPerfTrace.Severity.warning)
trace.frameId = self._clamp_uint(int(frame_id), 32)
trace.totalTimeUs = self._clamp_uint(int(total_time_us), 32)
trace.rkRemainingUs = self._clamp_int(int(rk_remaining_us), 32)
trace.batchSize = self._clamp_uint(int(batch_size), 16)
trace.droppedFrames = self._clamp_uint(int(dropped_frames), 16)
trace.backlog = self._clamp_uint(int(backlog), 16)
trace.flags = self._clamp_uint(int(flags), 32)
trace.missingServices = list(missing_services or [])
trace.topProcesses = list(top_processes or [])
trace.detail = detail
trace_samples = samples or []
samples_builder = trace.init("samples", len(trace_samples))
for i, sample in enumerate(trace_samples):
builder = samples_builder[i]
builder.frameId = self._clamp_uint(int(sample.frame_id), 32)
builder.loopDtUs = self._clamp_uint(int(sample.loop_dt_us), 32)
builder.updateUs = self._clamp_uint(int(sample.update_us), 32)
builder.stateControlUs = self._clamp_uint(int(sample.state_control_us), 32)
builder.publishUs = self._clamp_uint(int(sample.publish_us), 32)
builder.tailWorkUs = self._clamp_uint(int(sample.tail_work_us), 32)
builder.rkRemainingUs = self._clamp_int(int(sample.rk_remaining_us), 32)
builder.staleCarControlUs = self._clamp_uint(int(sample.stale_carcontrol_us), 32)
builder.staleCarControlFrames = self._clamp_uint(int(sample.stale_carcontrol_frames), 16)
builder.sendcanGapUs = self._clamp_uint(int(sample.sendcan_gap_us), 32)
builder.modelEvalUs = self._clamp_uint(int(sample.model_eval_us), 32)
builder.modelDroppedFrames = self._clamp_uint(int(sample.model_dropped_frames), 16)
builder.modelBacklog = self._clamp_uint(int(sample.model_backlog), 16)
builder.textureDecodeUs = self._clamp_uint(int(sample.texture_decode_us), 32)
builder.textureUploadUs = self._clamp_uint(int(sample.texture_upload_us), 32)
builder.textureUnloadUs = self._clamp_uint(int(sample.texture_unload_us), 32)
builder.texturePruneUs = self._clamp_uint(int(sample.texture_prune_us), 32)
builder.textureConsumeUs = self._clamp_uint(int(sample.texture_consume_us), 32)
builder.textureBatchSize = self._clamp_uint(int(sample.texture_batch_size), 16)
builder.textureBytes = self._clamp_uint(int(sample.texture_bytes), 32)
builder.textureCacheBefore = self._clamp_uint(int(sample.texture_cache_before), 16)
builder.textureCacheAfter = self._clamp_uint(int(sample.texture_cache_after), 16)
builder.textureUnloaded = self._clamp_uint(int(sample.texture_unloaded), 16)
builder.memoryUsagePercent = self._clamp_uint(int(sample.memory_usage_percent), 16)
builder.gpuUsagePercent = self._clamp_uint(int(sample.gpu_usage_percent), 16)
builder.cpuUsagePercent = self._clamp_uint(int(sample.cpu_usage_percent), 16)
builder.flags = self._clamp_uint(int(sample.flags), 32)
try:
self._pubmaster().send(TRACE_SERVICE, msg)
except messaging.MultiplePublishersError:
self._disabled = True
cloudlog.error(f"iq_perf_trace disabled for {self.process_name}: duplicate publisher for {TRACE_SERVICE}")
return False
except Exception:
cloudlog.exception(f"iq_perf_trace publish failed for {self.process_name}")
return False
if mirror_cloudlog:
cloudlog.event(
"iq_perf_trace",
process=self.process_name,
event_class=event_class,
severity=severity,
frame_id=int(frame_id),
total_time_us=int(total_time_us),
dropped_frames=int(dropped_frames),
flags=int(flags),
detail=detail,
)
return True

44
artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/common/issue_debug.py Normal file
View File

@@ -0,0 +1,44 @@
import os
import threading
import time
from datetime import datetime
from pathlib import Path
from iqpilot.system.hardware import PC
from iqpilot.system.hardware.hw import Paths
DEBUG_FILENAME = "iqpilot_issue_debug.txt"
DEBUG_PATH = Path(Paths.comma_home()) / "community" / DEBUG_FILENAME if PC else Path("/data/community") / DEBUG_FILENAME
_lock = threading.Lock()
_last_log_times: dict[str, float] = {}
def log_issue(tag: str, message: str) -> None:
try:
DEBUG_PATH.parent.mkdir(parents=True, exist_ok=True)
with _lock:
with open(DEBUG_PATH, "a", encoding="utf-8") as f:
timestamp = datetime.now().strftime("%Y-%m-%d %H:%M:%S.%f")[:-3]
f.write(f"[{timestamp}] [{tag}] {message}\n")
except OSError:
pass
def log_issue_limited(key: str, tag: str, message: str, interval_sec: float = 1.0) -> None:
now = time.monotonic()
with _lock:
last = _last_log_times.get(key, 0.0)
if now - last < interval_sec:
return
_last_log_times[key] = now
log_issue(tag, message)
def clear_issue_debug_log() -> None:
try:
os.remove(DEBUG_PATH)
except OSError:
pass

0
common/logging_extra.py → artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/common/logging_extra.py
View File

134
artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/common/realtime.py Normal file
View File

@@ -0,0 +1,134 @@
"""Utilities for reading real time clocks and keeping soft real time constraints."""
import gc
import os
import sys
import time
from setproctitle import getproctitle
from iqpilot.common.utils import MovingAverage
from iqpilot.system.hardware import PC
# time step for each process
DT_CTRL = 0.01 # controlsd
DT_MDL = 0.05 # model
DT_HW = 0.5 # hardwared and manager
DT_DMON = 0.05 # driver monitoring
class Priority:
# CORE 2
# - modeld = 55
# - camerad = 54
CTRL_LOW = 51 # plannerd & radard
# CORE 3
# - pandad = 55
CTRL_HIGH = 53
def set_core_affinity(cores: list[int]) -> None:
if sys.platform == 'linux' and not PC:
os.sched_setaffinity(0, cores)
def config_realtime_process(cores: int | list[int], priority: int) -> None:
gc.disable()
if sys.platform == 'linux' and not PC:
os.sched_setscheduler(0, os.SCHED_FIFO, os.sched_param(priority))
c = cores if isinstance(cores, list) else [cores, ]
set_core_affinity(c)
def config_background_thread() -> None:
if sys.platform == 'linux' and not PC:
os.sched_setscheduler(0, os.SCHED_OTHER, os.sched_param(0))
set_core_affinity(list(range(os.cpu_count() or 1)))
def lock_memory() -> None:
"""mlockall this process so memory reclaim/compaction can't stall it. RT control
procs only (locking ui/modeld would worsen pressure). Best-effort."""
if sys.platform != 'linux' or PC:
return
try:
import ctypes
import resource
resource.setrlimit(resource.RLIMIT_MEMLOCK, (resource.RLIM_INFINITY, resource.RLIM_INFINITY))
MCL_CURRENT, MCL_FUTURE = 0x1, 0x2
libc = ctypes.CDLL("libc.so.6", use_errno=True)
if libc.mlockall(MCL_CURRENT | MCL_FUTURE) != 0:
raise OSError(ctypes.get_errno(), os.strerror(ctypes.get_errno()))
except Exception as e:
try:
from iqpilot.common.swaglog import cloudlog
cloudlog.warning(f"lock_memory (mlockall) failed: {e}")
except Exception:
pass
class Ratekeeper:
def __init__(self, rate: float, print_delay_threshold: float | None = 0.0) -> None:
"""Rate in Hz for ratekeeping. print_delay_threshold must be nonnegative."""
self._interval = 1. / rate
self._print_delay_threshold = print_delay_threshold
self._frame = 0
self._remaining = 0.0
self._process_name = getproctitle()
self._last_monitor_time = -1.
self._next_frame_time = -1.
self.avg_dt = MovingAverage(100)
self.avg_dt.add_value(self._interval)
def reset(self) -> None:
self._remaining = 0.0
self._last_monitor_time = -1.
self._next_frame_time = -1.
self.avg_dt = MovingAverage(100)
self.avg_dt.add_value(self._interval)
@property
def frame(self) -> int:
return self._frame
@property
def remaining(self) -> float:
return self._remaining
@property
def lag(self) -> float:
return max(0., -self._remaining)
@property
def lagging(self) -> bool:
expected_dt = self._interval * (1 / 0.9)
return self.avg_dt.get_average() > expected_dt
# Maintain loop rate by calling this at the end of each loop
def keep_time(self) -> bool:
lagged = self.monitor_time()
if self._remaining > 0:
time.sleep(self._remaining)
return lagged
# Monitors the cumulative lag, but does not enforce a rate
def monitor_time(self) -> bool:
if self._last_monitor_time < 0:
self._next_frame_time = time.monotonic() + self._interval
self._last_monitor_time = time.monotonic()
prev = self._last_monitor_time
self._last_monitor_time = time.monotonic()
self.avg_dt.add_value(self._last_monitor_time - prev)
lagged = False
remaining = self._next_frame_time - time.monotonic()
self._next_frame_time += self._interval
if self._print_delay_threshold is not None and remaining < -self._print_delay_threshold:
print(f"{self._process_name} lagging by {-remaining * 1000:.2f} ms")
lagged = True
self._frame += 1
self._remaining = remaining
return lagged

52
artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/common/spinner.py Normal file
View File

@@ -0,0 +1,52 @@
import os
import subprocess
from iqpilot.common.basedir import BASEDIR
class Spinner:
def __init__(self):
try:
self.spinner_proc = subprocess.Popen(["./spinner.py"],
stdin=subprocess.PIPE,
cwd=os.path.join(BASEDIR, "iqpilot", "system", "ui"),
close_fds=True)
except OSError:
self.spinner_proc = None
def __enter__(self):
return self
def update(self, spinner_text: str):
if self.spinner_proc is not None:
self.spinner_proc.stdin.write(spinner_text.encode('utf8') + b"\n")
try:
self.spinner_proc.stdin.flush()
except BrokenPipeError:
pass
def update_progress(self, cur: float, total: float):
self.update(str(round(100 * cur / total)))
def close(self):
if self.spinner_proc is not None:
self.spinner_proc.kill()
try:
self.spinner_proc.communicate(timeout=2.)
except subprocess.TimeoutExpired:
print("WARNING: failed to kill spinner")
self.spinner_proc = None
def __del__(self):
self.close()
def __exit__(self, exc_type, exc_value, traceback):
self.close()
if __name__ == "__main__":
import time
with Spinner() as s:
s.update("Spinner text")
time.sleep(5.0)
print("gone")
time.sleep(5.0)

60
artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/common/steer_delay.py Normal file
View File

@@ -0,0 +1,60 @@
"""
Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
Chooses which steer-actuator delay the lateral controllers run with: the value the
live estimator learned, or the driver's fixed software delay — gated by the
"IQLiveSteerDelay" param. The pick is mirrored into "IQSteerDelayCache" so consumers that do
not subscribe to lateralDelay can still read the current value.
"""
from iqpilot.cereal import car
from iqpilot.common.params import Params
_ENABLE_KEY = "IQLiveSteerDelay"
_FIXED_KEY = "IQSoftwareSteerDelay"
_CACHE_KEY = "IQSteerDelayCache"
def fixed_steer_delay(params, stock_delay):
"""The rack's own delay plus the driver's IQSoftwareSteerDelay offset, as the UI reports it."""
return stock_delay + float(params.get(_FIXED_KEY, return_default=True))
def resolve_steer_delay(params, stock_delay):
"""Learned lateral delay while live-learning is enabled, otherwise the driver's fixed delay."""
if not params.get_bool(_ENABLE_KEY):
return fixed_steer_delay(params, stock_delay)
return float(params.get(_CACHE_KEY, return_default=True))
def lateral_action_delay(params, car_params, live_delay):
"""Delay the lateral path should be planned against.
Angle cars honour the IQLiveSteerDelay toggle so that with live learning off the
estimate never reaches the path: lagd cross-correlates against localizer lateral
accel, so it reports whole-vehicle response (~0.36 s measured on VW MQB, 0.44 s on
Tesla) where the lookahead wants actuator delay (~0.10 s). Torque cars keep the
live estimate.
"""
if car_params.steerControlType == car.CarParams.SteerControlType.angle:
return resolve_steer_delay(params, car_params.steerActuatorDelay)
return live_delay
def cached_steer_delay():
"""Last value SteerDelayPublisher mirrored into the param — usable without a
lateralDelay subscription (e.g. at process startup)."""
return Params().get(_CACHE_KEY, return_default=True)
class SteerDelayPublisher:
"""Refreshes IQSteerDelayCache every lag message: the learned live delay when the
toggle is on, else the actuator delay plus the driver's fixed software offset."""
def __init__(self, car_params):
self._params = Params()
self._actuator_delay = car_params.steerActuatorDelay
def update(self, lag_msg):
live = self._params.get_bool(_ENABLE_KEY)
value = lag_msg.lateralDelay.lateralDelay if live else fixed_steer_delay(self._params, self._actuator_delay)
self._params.put_nonblocking(_CACHE_KEY, value)

165
artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/common/swaglog.py Normal file
View File

@@ -0,0 +1,165 @@
import logging
import os
import sys
import time
import warnings
from pathlib import Path
from logging.handlers import BaseRotatingHandler
import zmq
from iqpilot.common.logging_extra import SwagLogger, SwagFormatter, SwagLogFileFormatter
from iqpilot.system.hardware.hw import Paths
def get_file_handler():
Path(Paths.swaglog_root()).mkdir(parents=True, exist_ok=True)
base_filename = os.path.join(Paths.swaglog_root(), "swaglog")
handler = SwaglogRotatingFileHandler(base_filename)
return handler
class SwaglogRotatingFileHandler(BaseRotatingHandler):
def __init__(self, base_filename, interval=60, max_bytes=1024*256, backup_count=2500, encoding=None):
super().__init__(base_filename, mode="a", encoding=encoding, delay=True)
self.base_filename = base_filename
self.interval = interval # seconds
self.max_bytes = max_bytes
self.backup_count = backup_count
self.log_files = self.get_existing_logfiles()
log_indexes = [f.split(".")[-1] for f in self.log_files]
self.last_file_idx = max([int(i) for i in log_indexes if i.isdigit()] or [-1])
self.last_rollover = None
self.doRollover()
def _open(self):
self.last_rollover = time.monotonic()
self.last_file_idx += 1
next_filename = f"{self.base_filename}.{self.last_file_idx:010}"
stream = open(next_filename, self.mode, encoding=self.encoding)
self.log_files.insert(0, next_filename)
return stream
def get_existing_logfiles(self):
log_files = list()
base_dir = os.path.dirname(self.base_filename)
for fn in os.listdir(base_dir):
fp = os.path.join(base_dir, fn)
if fp.startswith(self.base_filename) and os.path.isfile(fp):
log_files.append(fp)
return sorted(log_files)
def shouldRollover(self, record):
size_exceeded = self.max_bytes > 0 and self.stream.tell() >= self.max_bytes
time_exceeded = self.interval > 0 and self.last_rollover + self.interval <= time.monotonic()
return size_exceeded or time_exceeded
def doRollover(self):
if self.stream:
self.stream.close()
self.stream = self._open()
if self.backup_count > 0:
while len(self.log_files) > self.backup_count:
to_delete = self.log_files.pop()
if os.path.exists(to_delete): # just being safe, should always exist
os.remove(to_delete)
class UnixDomainSocketHandler(logging.Handler):
def __init__(self, formatter):
logging.Handler.__init__(self)
self.setFormatter(formatter)
self.pid = None
self.zctx = None
self.sock = None
def __del__(self):
self.close()
def close(self):
if self.sock is not None:
self.sock.close()
if self.zctx is not None:
self.zctx.term()
def connect(self):
self.zctx = zmq.Context()
self.sock = self.zctx.socket(zmq.PUSH)
self.sock.setsockopt(zmq.LINGER, 10)
self.sock.connect(Paths.swaglog_ipc())
self.pid = os.getpid()
def emit(self, record):
if os.getpid() != self.pid:
# TODO suppresses warning about forking proc with zmq socket, fix root cause
warnings.filterwarnings("ignore", category=ResourceWarning, message="unclosed.*<zmq.*>")
self.connect()
msg = self.format(record).rstrip('\n')
# print("SEND".format(repr(msg)))
try:
s = chr(record.levelno)+msg
self.sock.send(s.encode('utf8'), zmq.NOBLOCK)
except zmq.error.Again:
# drop :/
pass
class ForwardingHandler(logging.Handler):
def __init__(self, target_logger):
super().__init__()
self.target_logger = target_logger
def emit(self, record):
self.target_logger.handle(record)
def add_file_handler(log):
"""
Function to add the file log handler to swaglog.
This can be used to store logs when logmessaged is not running.
"""
handler = get_file_handler()
handler.setFormatter(SwagLogFileFormatter(log))
log.addHandler(handler)
cloudlog = log = SwagLogger()
log.setLevel(logging.DEBUG)
class PrettyConsoleFormatter(logging.Formatter):
# StreamHandler writes to stderr, so tty-gate on that
_COLOR = sys.stderr.isatty() and os.environ.get('NO_COLOR') is None
def format(self, record):
msg = record.getMessage()
if not self._COLOR:
return f"{record.filename}: {msg}"
lvl = record.levelno
if lvl >= 50: lc, ln = "\033[1;38;5;196m", "CRIT"
elif lvl >= 40: lc, ln = "\033[1;38;5;203m", " ERR"
elif lvl >= 30: lc, ln = "\033[38;5;214m", "WARN"
elif lvl >= 20: lc, ln = "\033[38;5;110m", "info"
else: lc, ln = "\033[38;5;244m", " dbg"
body = f"\033[1;38;5;210m{msg}\033[0m" if lvl >= 40 else msg
src = "" if record.filename == "(unknown file)" else f"\033[2m{record.filename}\033[0m "
return f"{lc}{ln:>4}\033[0m {src}{body}"
outhandler = logging.StreamHandler()
outhandler.setFormatter(PrettyConsoleFormatter())
print_level = os.environ.get('LOGPRINT', 'warning')
if print_level == 'debug':
outhandler.setLevel(logging.DEBUG)
elif print_level == 'info':
outhandler.setLevel(logging.INFO)
elif print_level == 'warning':
outhandler.setLevel(logging.WARNING)
ipchandler = UnixDomainSocketHandler(SwagFormatter(log))
log.addHandler(outhandler)
# logs are sent through IPC before writing to disk to prevent disk I/O blocking
log.addHandler(ipchandler)

0
common/time_helpers.py → artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/common/time_helpers.py
View File

0
common/utils.py → artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/common/utils.py
View File

0
common/tests/__init__.py → artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/konn3kt/__init__.py
View File

17
artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/konn3kt/cloud_client.py Normal file
View File

@@ -0,0 +1,17 @@
"""
Copyright ©️ IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
"""
import os
from iqpilot.common.api.base import BaseApi
API_HOST = os.getenv('KONN3KT_API_HOST', 'https://api-iqlabs.konn3kt.com')
class Konn3ktApi(BaseApi):
def __init__(self, dongle_id):
super().__init__(dongle_id, API_HOST)
self.user_agent = "konn3kt-device-"
def get_token(self, expiry_hours=1):
return super()._get_token(expiry_hours=expiry_hours)

226
artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/konn3kt/registration.py Normal file
View File

@@ -0,0 +1,226 @@
#!/usr/bin/env python3
import os
import time
import json
import jwt
import re
import secrets
from typing import cast
from pathlib import Path
from datetime import datetime, timedelta, UTC
from iqpilot.common.api import api_get, get_key_pair
from iqpilot.common.params import Params
from iqpilot.common.spinner import Spinner
from iqpilot.system.hardware import HARDWARE, PC
from iqpilot.system.hardware.hw import Paths
from iqpilot.common.swaglog import cloudlog
UNREGISTERED_DONGLE_ID = "UnregisteredDevice"
_DONGLE_ID_RE = re.compile(r"^[a-fA-F0-9]{16}$")
IMEI_WAIT_TIMEOUT = 15.0
def _read_persist_dongle_id() -> str | None:
p = Path(Paths.persist_root()) / "comma" / "dongle_id"
try:
if not p.is_file():
return None
s = p.read_text().strip()
return s or None
except Exception:
cloudlog.exception("failed to read persist dongle_id")
return None
def get_cached_dongle_id(params: Params | None = None, prefer_readonly: bool = True) -> str | None:
ro = _read_persist_dongle_id()
if is_valid_dongle_id(ro):
ro = ro.lower()
if prefer_readonly and ro:
return ro
p = Params() if params is None else params
v = p.get("DongleId")
if v and v != UNREGISTERED_DONGLE_ID:
return v.lower() if is_valid_dongle_id(v) else v
return ro or None
def is_valid_dongle_id(dongle_id: str | None) -> bool:
return bool(dongle_id and _DONGLE_ID_RE.fullmatch(dongle_id))
def get_or_create_dongle_id(params: Params | None = None, prefer_readonly: bool = True) -> str:
p = Params() if params is None else params
dongle_id = get_cached_dongle_id(p, prefer_readonly=prefer_readonly)
if dongle_id and dongle_id != UNREGISTERED_DONGLE_ID:
return dongle_id
dongle_id = secrets.token_hex(8)
p.put("DongleId", dongle_id)
cloudlog.warning(f"generated new DongleId={dongle_id} (no readonly dongle_id found)")
return dongle_id
def ensure_dev_pairing_identity(params: Params | None = None, force_reset: bool = False) -> dict[str, str]:
p = Params() if params is None else params
persist_dir = Path(Paths.persist_root()) / "comma"
persist_dir.mkdir(parents=True, exist_ok=True)
dongle_path = persist_dir / "dongle_id"
priv_path = persist_dir / "id_rsa"
pub_path = persist_dir / "id_rsa.pub"
if force_reset:
for fp in (dongle_path, priv_path, pub_path):
try:
fp.unlink(missing_ok=True)
except Exception:
cloudlog.exception(f"failed to remove {fp}")
try:
(persist_dir / "konn3kt_prime_type").unlink(missing_ok=True)
except Exception:
pass
try:
p.remove("PrimeType")
except Exception:
pass
forced_dongle = os.getenv("KONN3KT_DEV_DONGLE_ID")
dongle_id = forced_dongle.strip().lower() if forced_dongle else None
if dongle_id and not is_valid_dongle_id(dongle_id):
cloudlog.error("KONN3KT_DEV_DONGLE_ID must be 16 hex chars")
dongle_id = None
if dongle_id is None:
existing = None
try:
existing = dongle_path.read_text().strip().lower() if dongle_path.is_file() else None
except Exception:
cloudlog.exception("failed reading existing dev dongle_id")
dongle_id = existing if is_valid_dongle_id(existing) else secrets.token_hex(8)
try:
dongle_path.write_text(dongle_id)
except Exception:
cloudlog.exception("failed writing dev dongle_id")
p.put("DongleId", dongle_id)
p.put("HardwareSerial", p.get("HardwareSerial") or f"DEV-{dongle_id}")
if force_reset or (not priv_path.is_file()) or (not pub_path.is_file()):
try:
from cryptography.hazmat.primitives import serialization
from cryptography.hazmat.primitives.asymmetric import rsa
key = rsa.generate_private_key(public_exponent=65537, key_size=2048)
priv_bytes = key.private_bytes(
encoding=serialization.Encoding.PEM,
format=serialization.PrivateFormat.TraditionalOpenSSL,
encryption_algorithm=serialization.NoEncryption(),
)
pub_bytes = key.public_key().public_bytes(
encoding=serialization.Encoding.PEM,
format=serialization.PublicFormat.SubjectPublicKeyInfo,
)
priv_path.write_bytes(priv_bytes)
pub_path.write_bytes(pub_bytes)
except Exception:
cloudlog.exception("failed generating dev RSA keys")
raise
return {
"dongle_id": dongle_id,
"serial": p.get("HardwareSerial") or f"DEV-{dongle_id}",
"persist_dir": str(persist_dir),
}
def is_registered_device() -> bool:
dongle = Params().get("DongleId")
return dongle not in (None, UNREGISTERED_DONGLE_ID)
def _normalize_imei(value: str | None) -> str:
return value or ""
def get_registration_identifiers(wait_timeout: float = IMEI_WAIT_TIMEOUT, show_spinner: bool = False) -> tuple[str, str, str]:
serial = HARDWARE.get_serial()
spinner = Spinner() if show_spinner else None
start_time = time.monotonic()
imei1: str | None = None
imei2: str | None = None
while time.monotonic() - start_time < wait_timeout:
try:
imei1, imei2 = HARDWARE.get_imei(0), HARDWARE.get_imei(1)
if imei1 or imei2:
break
except RuntimeError as e:
if "no modems" in str(e).lower():
cloudlog.warning("No cellular modem available, proceeding without IMEI")
break
cloudlog.exception("Error getting imei, trying again...")
except Exception:
cloudlog.exception("Error getting imei, trying again...")
time.sleep(1)
imei1 = _normalize_imei(imei1)
imei2 = _normalize_imei(imei2)
if not imei1 and not imei2:
cloudlog.warning(f"proceeding with serial-only registration for serial={serial}")
if spinner is not None:
spinner.update(f"registering device - serial: {serial}, IMEI: ({imei1 or None}, {imei2 or None})")
spinner.close()
return serial, imei1, imei2
def register(show_spinner=False) -> str | None:
params = Params()
dongle_id: str | None = get_cached_dongle_id(params, prefer_readonly=True)
if dongle_id in ("", UNREGISTERED_DONGLE_ID):
dongle_id = None
jwt_algo, private_key, public_key = get_key_pair()
if not public_key:
dongle_id = UNREGISTERED_DONGLE_ID
cloudlog.warning("missing public key")
elif dongle_id is None:
if show_spinner:
spinner = Spinner()
spinner.update("registering device")
serial, imei1, imei2 = get_registration_identifiers(wait_timeout=IMEI_WAIT_TIMEOUT, show_spinner=False)
backoff = 0
start_time = time.monotonic()
while True:
try:
register_token = jwt.encode({'register': True, 'exp': datetime.now(UTC).replace(tzinfo=None) + timedelta(hours=1)},
cast(str, private_key), algorithm=jwt_algo)
cloudlog.info("getting pilotauth")
cloudlog.info("getting pilotauth")
resp = api_get("v2/pilotauth/", method='POST', timeout=15,
imei=imei1, imei2=imei2, serial=serial, public_key=public_key, register_token=register_token)
if resp.status_code in (400, 402, 403):
cloudlog.info(f"Unable to register device, got {resp.status_code}")
dongle_id = UNREGISTERED_DONGLE_ID
else:
dongleauth = json.loads(resp.text)
dongle_id = dongleauth["dongle_id"]
break
except Exception:
cloudlog.exception("failed to authenticate")
backoff = min(backoff + 1, 15)
time.sleep(backoff)
if time.monotonic() - start_time > 60 and show_spinner:
spinner.update(f"registering device - serial: {serial}, IMEI: ({imei1}, {imei2})")
return UNREGISTERED_DONGLE_ID
if show_spinner:
spinner.update(f"registering device - serial: {serial}, IMEI: ({imei1 or None}, {imei2 or None})")
spinner.close()
if dongle_id:
params.put("DongleId", dongle_id)
from iqpilot.selfdrive.selfdrived.alertmanager import set_offroad_alert
set_offroad_alert("Offroad_UnregisteredHardware", False)
return dongle_id
if __name__ == "__main__":
print(register())

0
common/transformations/__init__.py → artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/__init__.py
View File

0
common/transformations/tests/__init__.py → artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/controls/__init__.py
View File

0
iqdbc_repo/iqdbc/car/body/__init__.py → artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/controls/lib/__init__.py
View File

294
artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/controls/lib/desire_helper.py Normal file
View File

@@ -0,0 +1,294 @@
"""
Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos/
"""
from __future__ import annotations
from iqpilot.cereal import car, custom, log
from iqpilot.common.constants import CV
from iqpilot.common.params import Params
from iqpilot.common.realtime import DT_MDL
from iqpilot.selfdrive.controls.lib.helpers.lane_change import (
IQLaneSwapController,
AutoLaneChangeMode,
NavExitLaneChangeController,
)
from iqpilot.selfdrive.controls.lib.helpers.lateral_edge_guard import LateralEdgeGuard
from iqpilot.selfdrive.controls.lib.helpers.lane_turn import IQNavTurnController
LaneChangeState = log.LaneChangeState
LaneChangeDirection = log.LaneChangeDirection
TurnDirection = custom.IQTurnSignalDirection
LateralEdgeBlock = custom.IQLateralEdgeBlock
NavManeuverPhase = custom.IQNavState.ManeuverPhase
LANE_CHANGE_SPEED_MIN = 20 * CV.MPH_TO_MS
LANE_CHANGE_TIME_MAX = 10.0
TURN_DESIRE_STOP_HOLD_TIME = 3.4
TURN_DESIRE_STOP_GAP_TIME = 0.2
TURN_DESIRE_STOP_CYCLE_TIME = TURN_DESIRE_STOP_HOLD_TIME + TURN_DESIRE_STOP_GAP_TIME
TURN_DESIRE_CYCLE_SPEED_MAX = 5 * CV.MPH_TO_MS
TURN_DESIRE_COMMIT_YAW_RATE = 0.08
_LANE_CHANGE_DESIRES = {
(LaneChangeDirection.none, LaneChangeState.off): log.Desire.none,
(LaneChangeDirection.none, LaneChangeState.preLaneChange): log.Desire.none,
(LaneChangeDirection.none, LaneChangeState.laneChangeStarting): log.Desire.none,
(LaneChangeDirection.none, LaneChangeState.laneChangeFinishing): log.Desire.none,
(LaneChangeDirection.left, LaneChangeState.off): log.Desire.none,
(LaneChangeDirection.left, LaneChangeState.preLaneChange): log.Desire.none,
(LaneChangeDirection.left, LaneChangeState.laneChangeStarting): log.Desire.laneChangeLeft,
(LaneChangeDirection.left, LaneChangeState.laneChangeFinishing): log.Desire.laneChangeLeft,
(LaneChangeDirection.right, LaneChangeState.off): log.Desire.none,
(LaneChangeDirection.right, LaneChangeState.preLaneChange): log.Desire.none,
(LaneChangeDirection.right, LaneChangeState.laneChangeStarting): log.Desire.laneChangeRight,
(LaneChangeDirection.right, LaneChangeState.laneChangeFinishing): log.Desire.laneChangeRight,
}
_TURN_DESIRES = {
TurnDirection.none: log.Desire.none,
TurnDirection.turnLeft: log.Desire.turnLeft,
TurnDirection.turnRight: log.Desire.turnRight,
}
_STOP_CYCLING_TURN_DESIRES = {
log.Desire.turnLeft,
log.Desire.turnRight,
}
def turn_desire(turn_direction) -> log.Desire:
return _TURN_DESIRES[getattr(turn_direction, "raw", turn_direction)]
def _direction_from_blinkers(carstate) -> int:
if carstate.leftBlinker:
return LaneChangeDirection.left
if carstate.rightBlinker:
return LaneChangeDirection.right
return LaneChangeDirection.none
def _steering_nudge_matches(carstate, direction: int) -> bool:
if not carstate.steeringPressed:
return False
return (
(direction == LaneChangeDirection.left and carstate.steeringTorque > 0) or
(direction == LaneChangeDirection.right and carstate.steeringTorque < 0)
)
def _blindspot_matches(carstate, direction: int) -> bool:
return (
(direction == LaneChangeDirection.left and carstate.leftBlindspot) or
(direction == LaneChangeDirection.right and carstate.rightBlindspot)
)
def _read_enable_bsm() -> bool:
try:
with car.CarParams.from_bytes(Params().get("CarParams")) as cp:
return bool(cp.enableBsm)
except Exception:
return False
class DesireHelper:
def __init__(self):
self.lane_change_state = LaneChangeState.off
self.lane_change_direction = LaneChangeDirection.none
self.lane_change_timer = 0.0
self.lane_change_ll_prob = 1.0
self.prev_one_blinker = False
self.prev_nav_exit_active = False
self.desire = log.Desire.none
self.alc = IQLaneSwapController(self)
self.lane_turn_controller = IQNavTurnController(self)
self.nav_exit = NavExitLaneChangeController(_read_enable_bsm())
self.lateral_edge_guard = LateralEdgeGuard()
self.lateral_edge_block = LateralEdgeBlock.none
self.lane_turn_direction = TurnDirection.none
self.nav_turn_direction = TurnDirection.none
self.turn_desire_stop_timer = 0.0
self.turn_desire_stop_active = False
self.turn_desire_cycle_input = log.Desire.none
self.turn_desire_committed = False
@staticmethod
def get_lane_change_direction(carstate):
return _direction_from_blinkers(carstate)
@staticmethod
def _nav_turn_desire(nav_state):
if nav_state is None or not getattr(nav_state, "active", False):
return TurnDirection.none
if getattr(nav_state, "maneuverPhase", NavManeuverPhase.none) != NavManeuverPhase.turnActive:
return TurnDirection.none
if not getattr(nav_state, "shouldSendTurnDesire", False):
return TurnDirection.none
return getattr(nav_state, "turnDesireDirection", TurnDirection.none)
def _clear_lane_change(self) -> None:
self.lane_change_state = LaneChangeState.off
self.lane_change_direction = LaneChangeDirection.none
def _refresh_turn_overrides(self, carstate, nav_state) -> bool:
speed_mps = carstate.vEgo
self.lane_turn_controller.update_params()
self.lane_turn_controller.update_lane_turn(
blindspot_left=carstate.leftBlindspot,
blindspot_right=carstate.rightBlindspot,
left_blinker=carstate.leftBlinker,
right_blinker=carstate.rightBlinker,
v_ego=speed_mps,
)
self.lane_turn_direction = self.lane_turn_controller.get_turn_direction()
self.nav_turn_direction = self._nav_turn_desire(nav_state)
self.nav_exit.update_params()
self.nav_exit.update(nav_state, carstate)
return bool(self.nav_exit.active)
def _reset_required(self, lateral_active: bool, nav_exit_active: bool) -> bool:
timed_out = self.lane_change_timer > LANE_CHANGE_TIME_MAX
feature_disabled = self.alc.lane_change_set_timer == AutoLaneChangeMode.OFF and not nav_exit_active
return (not lateral_active) or timed_out or feature_disabled
def _begin_from_idle(self, one_blinker: bool, nav_exit_active: bool, below_speed: bool) -> None:
if below_speed:
return
if one_blinker and not self.prev_one_blinker:
self.lane_change_state = LaneChangeState.preLaneChange
self.lane_change_direction = _direction_from_blinkers(self._last_carstate)
self.lane_change_ll_prob = 1.0
return
if nav_exit_active and not self.prev_nav_exit_active:
self.lane_change_state = LaneChangeState.preLaneChange
self.lane_change_direction = self.nav_exit.direction
self.lane_change_ll_prob = 1.0
def _refresh_requested_direction(self, one_blinker: bool, nav_exit_active: bool) -> None:
if one_blinker:
self.lane_change_direction = _direction_from_blinkers(self._last_carstate)
elif nav_exit_active:
self.lane_change_direction = self.nav_exit.direction
def _step_pre_lane_change(self, one_blinker: bool, nav_exit_active: bool, below_speed: bool) -> None:
self._refresh_requested_direction(one_blinker, nav_exit_active)
blindspot_detected = _blindspot_matches(self._last_carstate, self.lane_change_direction)
self.lateral_edge_block = self.lateral_edge_guard.block_for_direction(self.lane_change_direction)
lateral_edge_blocked = self.lateral_edge_block != LateralEdgeBlock.none
steering_ready = _steering_nudge_matches(self._last_carstate, self.lane_change_direction)
nav_auto_start = nav_exit_active and self.nav_exit.auto_allowed
self.alc.update_lane_change(blindspot_detected=blindspot_detected, brake_pressed=self._last_carstate.brakePressed)
allowed_to_launch = steering_ready or self.alc.auto_lane_change_allowed or nav_auto_start
if (not (one_blinker or nav_exit_active)) or below_speed:
self._clear_lane_change()
elif allowed_to_launch and not blindspot_detected and not lateral_edge_blocked:
self.lane_change_state = LaneChangeState.laneChangeStarting
def _step_lane_change_starting(self, lane_change_prob: float) -> None:
self.lane_change_ll_prob = max(self.lane_change_ll_prob - (2.0 * DT_MDL), 0.0)
if lane_change_prob < 0.02 and self.lane_change_ll_prob < 0.01:
self.lane_change_state = LaneChangeState.laneChangeFinishing
def _step_lane_change_finishing(self, one_blinker: bool) -> None:
self.lane_change_ll_prob = min(self.lane_change_ll_prob + DT_MDL, 1.0)
if self.lane_change_ll_prob <= 0.99:
return
self.lane_change_direction = LaneChangeDirection.none
self.lane_change_state = LaneChangeState.preLaneChange if one_blinker else LaneChangeState.off
def _advance_lane_change_machine(self, one_blinker: bool, nav_exit_active: bool, below_speed: bool, lane_change_prob: float) -> None:
if self.lane_change_state == LaneChangeState.off:
self._begin_from_idle(one_blinker, nav_exit_active, below_speed)
return
if self.lane_change_state == LaneChangeState.preLaneChange:
self._step_pre_lane_change(one_blinker, nav_exit_active, below_speed)
return
if self.lane_change_state == LaneChangeState.laneChangeStarting:
self._step_lane_change_starting(lane_change_prob)
return
if self.lane_change_state == LaneChangeState.laneChangeFinishing:
self._step_lane_change_finishing(one_blinker)
def _update_timer(self) -> None:
if self.lane_change_state in (LaneChangeState.off, LaneChangeState.preLaneChange):
self.lane_change_timer = 0.0
else:
self.lane_change_timer += DT_MDL
def _clear_turn_desire_stop_cycle(self) -> None:
self.turn_desire_stop_timer = 0.0
self.turn_desire_stop_active = False
self.turn_desire_cycle_input = log.Desire.none
self.turn_desire_committed = False
def _cycle_turn_desire_when_stopped(self, desired_output: log.Desire) -> log.Desire:
if desired_output not in _STOP_CYCLING_TURN_DESIRES:
self._clear_turn_desire_stop_cycle()
return desired_output
if desired_output != self.turn_desire_cycle_input:
self.turn_desire_stop_timer = 0.0
self.turn_desire_stop_active = False
self.turn_desire_cycle_input = desired_output
self.turn_desire_committed = False
if abs(getattr(self._last_carstate, "yawRate", 0.0)) >= TURN_DESIRE_COMMIT_YAW_RATE:
self.turn_desire_committed = True
if self.turn_desire_committed:
self.turn_desire_stop_timer = 0.0
self.turn_desire_stop_active = False
return desired_output
if self._last_carstate.vEgo > TURN_DESIRE_CYCLE_SPEED_MAX:
self.turn_desire_stop_timer = 0.0
self.turn_desire_stop_active = False
return desired_output
if not self.turn_desire_stop_active:
self.turn_desire_stop_active = True
self.turn_desire_stop_timer = 0.0
cycle_phase = self.turn_desire_stop_timer % TURN_DESIRE_STOP_CYCLE_TIME
self.turn_desire_stop_timer += DT_MDL
if cycle_phase >= TURN_DESIRE_STOP_HOLD_TIME:
return log.Desire.none
return desired_output
def _pick_desire_output(self) -> None:
desired_output = log.Desire.none
if self.nav_turn_direction != TurnDirection.none:
desired_output = turn_desire(self.nav_turn_direction)
elif self.lane_turn_direction != TurnDirection.none:
desired_output = turn_desire(self.lane_turn_direction)
else:
desired_output = _LANE_CHANGE_DESIRES[(self.lane_change_direction, self.lane_change_state)]
self.desire = self._cycle_turn_desire_when_stopped(desired_output)
def update(self, carstate, lateral_active, lane_change_prob, nav_state=None, modeldata=None, radar_state=None):
self._last_carstate = carstate
self.lateral_edge_guard.update(modeldata, carstate.vEgo, DT_MDL)
self.lateral_edge_block = LateralEdgeBlock.none
one_blinker = carstate.leftBlinker != carstate.rightBlinker
below_speed = carstate.vEgo < LANE_CHANGE_SPEED_MIN
nav_exit_active = self._refresh_turn_overrides(carstate, nav_state)
self.alc.update_params()
if self._reset_required(lateral_active, nav_exit_active):
self._clear_lane_change()
else:
self._advance_lane_change_machine(one_blinker, nav_exit_active, below_speed, lane_change_prob)
self._update_timer()
self.prev_one_blinker = one_blinker and lateral_active
self.prev_nav_exit_active = nav_exit_active
self.alc.update_state()
self._pick_desire_output()

80
artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/controls/lib/drive_helpers.py Normal file
View File

@@ -0,0 +1,80 @@
import numpy as np
from iqpilot.common.constants import ACCELERATION_DUE_TO_GRAVITY
from iqpilot.common.realtime import DT_CTRL, DT_MDL
MIN_SPEED = 1.0
CONTROL_N = 17
CAR_ROTATION_RADIUS = 0.0
# This is a turn radius smaller than most cars can achieve
MAX_CURVATURE = 0.4
MAX_VEL_ERR = 5.0 # m/s
MAX_LATERAL_JERK = 5.0 # m/s^3
MAX_LATERAL_ACCEL_NO_ROLL = 5.0 # m/s^2
MAX_LATERAL_ACCEL_NO_ROLL_OVERRIDE = 5.0 # m/s^2
DEFAULT_STOPPING_SPEED = 0.25 # m/s
def should_stop(v_ego: float, a_target: float, stopping_speed: float = DEFAULT_STOPPING_SPEED) -> bool:
return bool(v_ego < stopping_speed and a_target < 0.1)
def clamp(val, min_val, max_val):
clamped_val = float(np.clip(val, min_val, max_val))
return clamped_val, clamped_val != val
def smooth_value(val, prev_val, tau, dt=DT_MDL):
alpha = 1 - np.exp(-dt/tau) if tau > 0 else 1
return alpha * val + (1 - alpha) * prev_val
# "Model smoothing": when the policy's own predicted uncertainty (plan_stds) for the
# 1s-ahead lateral position spikes, temporarily lengthen the desiredCurvature smoothing
# time constant so a noisy/uncertain model output doesn't jerk the wheel.
MODEL_SMOOTHING_STD_LOW = 0.15 # m, plan y_std at 1s below which no extra smoothing is added
MODEL_SMOOTHING_STD_HIGH = 0.25 # m, plan y_std at 1s at/above which the full max_extra_seconds is added
MODEL_SMOOTHING_MAX_TOTAL_SEC = 0.60 # hard ceiling on base + dynamic lat smoothing seconds
def dynamic_lat_smooth_extra_seconds(y_std_1s: float, max_extra_seconds: float) -> float:
if max_extra_seconds <= 0.0:
return 0.0
return float(np.interp(y_std_1s, [MODEL_SMOOTHING_STD_LOW, MODEL_SMOOTHING_STD_HIGH], [0.0, max_extra_seconds]))
def clip_curvature(v_ego, prev_curvature, new_curvature, roll, override=False) -> tuple[float, bool]:
# This function respects ISO lateral jerk and acceleration limits + a max curvature
v_ego = max(v_ego, MIN_SPEED)
max_curvature_rate = MAX_LATERAL_JERK / (v_ego ** 2) # inexact calculation, check https://github.com/commaai/openpilot/pull/24755
new_curvature = np.clip(new_curvature,
prev_curvature - max_curvature_rate * DT_CTRL,
prev_curvature + max_curvature_rate * DT_CTRL)
max_lat_accel_no_roll = MAX_LATERAL_ACCEL_NO_ROLL_OVERRIDE if override else MAX_LATERAL_ACCEL_NO_ROLL
roll_compensation = roll * ACCELERATION_DUE_TO_GRAVITY
max_lat_accel = max_lat_accel_no_roll + roll_compensation
min_lat_accel = -max_lat_accel_no_roll + roll_compensation
new_curvature, limited_accel = clamp(new_curvature, min_lat_accel / v_ego ** 2, max_lat_accel / v_ego ** 2)
new_curvature, limited_max_curv = clamp(new_curvature, -MAX_CURVATURE, MAX_CURVATURE)
return float(new_curvature), limited_accel or limited_max_curv
def get_accel_from_plan(speeds, accels, t_idxs, action_t=DT_MDL, stopping_speed=DEFAULT_STOPPING_SPEED):
if len(speeds) == len(t_idxs):
v_now = speeds[0]
a_now = accels[0]
v_target = np.interp(action_t, t_idxs, speeds)
a_target = 2 * (v_target - v_now) / (action_t) - a_now
else:
v_now = 0.0
v_target = 0.0
a_target = 0.0
return a_target, should_stop(v_now, a_target, stopping_speed)
def curv_from_psis(psi_target, psi_rate, vego, action_t):
vego = np.clip(vego, MIN_SPEED, np.inf)
curv_from_psi = psi_target / (vego * action_t)
return 2*curv_from_psi - psi_rate / vego
def get_curvature_from_plan(yaws, yaw_rates, t_idxs, vego, action_t):
psi_target = np.interp(action_t, t_idxs, yaws)
psi_rate = yaw_rates[0]
return curv_from_psis(psi_target, psi_rate, vego, action_t)

0
iqdbc_repo/iqdbc/car/byd/__init__.py → artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/controls/lib/helpers/__init__.py
View File

293
artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/controls/lib/helpers/lane_change.py Normal file
View File

@@ -0,0 +1,293 @@
"""
Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
"""
from iqpilot.cereal import custom, log
from iqpilot.common.params import Params
from iqpilot.common.realtime import DT_MDL
NAV_EXIT_COMMIT_DISTANCE = 500.0 # m before a route exit to begin moving into the exit lane
_ManeuverType = custom.IQNavState.ManeuverType
_NavDirection = custom.NavDirection
class LaneSwapPreset:
DISABLED = -1
STEERING_NUDGE = 0
DIRECT = 1
DELAY_HALF = 2
DELAY_ONE = 3
DELAY_TWO = 4
DELAY_THREE = 5
OFF = DISABLED
NUDGE = STEERING_NUDGE
NUDGELESS = DIRECT
HALF_SECOND = DELAY_HALF
ONE_SECOND = DELAY_ONE
TWO_SECONDS = DELAY_TWO
THREE_SECONDS = DELAY_THREE
PRESET_SECONDS = {
LaneSwapPreset.DISABLED: 0.0,
LaneSwapPreset.STEERING_NUDGE: 0.0,
LaneSwapPreset.DIRECT: 0.05,
LaneSwapPreset.DELAY_HALF: 0.5,
LaneSwapPreset.DELAY_ONE: 1.0,
LaneSwapPreset.DELAY_TWO: 2.0,
LaneSwapPreset.DELAY_THREE: 3.0,
}
LANE_SWAP_SECONDS = dict(PRESET_SECONDS)
BLINDSPOT_WAIT_OFFSET = -1
class LaneSwapEngine:
def __init__(self, desire_hub):
self._hub = desire_hub
self._kv = Params()
self._mem = {
"sec": 0.0,
"tick": 0,
"gate": 0.0,
"preset": self._kv.get("IQLaneChangeTimer", return_default=True),
"bsm_hold": False,
"braked": False,
"ready": False,
"used": False,
}
self.reload_setup()
def _pull_setup(self) -> None:
self._mem["bsm_hold"] = self._kv.get_bool("IQLaneChangeBsmDelay")
self._mem["preset"] = self._kv.get("IQLaneChangeTimer", return_default=True)
def _idle_phase(self) -> bool:
return (
self._hub.lane_change_state == log.LaneChangeState.off and
self._hub.lane_change_direction == log.LaneChangeDirection.none
)
def _seconds_for_preset(self) -> float:
picked = self._mem["preset"]
return PRESET_SECONDS.get(picked, PRESET_SECONDS[LaneSwapPreset.STEERING_NUDGE])
def _auto_preset_active(self) -> bool:
picked = self._mem["preset"]
return picked not in (LaneSwapPreset.DISABLED, LaneSwapPreset.STEERING_NUDGE)
def _advance_clock(self, blindspot_now: bool) -> None:
wait_s = self._seconds_for_preset()
self._mem["gate"] = wait_s
self._mem["sec"] += DT_MDL
if self._mem["bsm_hold"] and blindspot_now and wait_s > 0.0:
if wait_s == PRESET_SECONDS[LaneSwapPreset.DIRECT]:
self._mem["sec"] = BLINDSPOT_WAIT_OFFSET
else:
self._mem["sec"] = wait_s + BLINDSPOT_WAIT_OFFSET
def _ready_to_fire(self) -> bool:
return (
self._auto_preset_active() and
(not self._mem["braked"]) and
(not self._mem["used"]) and
(self._mem["sec"] > self._mem["gate"])
)
def reload_setup(self) -> None:
self._pull_setup()
def heartbeat(self) -> None:
if (self._mem["tick"] % 50) == 0:
self._pull_setup()
self._mem["tick"] += 1
def sample(self, blindspot_now: bool = False, brake_now: bool = False, **legacy) -> None:
blindspot_now = bool(legacy.get("blindspot_detected", blindspot_now))
brake_now = bool(legacy.get("brake_pressed", brake_now))
self._mem["braked"] = self._mem["braked"] or brake_now
self._advance_clock(blindspot_now)
self._mem["ready"] = self._ready_to_fire()
def finalize(self) -> None:
started = self._hub.lane_change_state == log.LaneChangeState.laneChangeStarting
self._mem["used"] = self._mem["used"] or started
if self._idle_phase():
self._mem["sec"] = 0.0
self._mem["braked"] = False
self._mem["used"] = False
@property
def ready(self):
return self._mem["ready"]
@property
def delay(self):
return self._mem["gate"]
@property
def elapsed(self):
return self._mem["sec"]
@property
def preset(self):
return self._mem["preset"]
@preset.setter
def preset(self, value):
self._mem["preset"] = value
@property
def bsm_hold(self):
return self._mem["bsm_hold"]
@bsm_hold.setter
def bsm_hold(self, value):
self._mem["bsm_hold"] = bool(value)
@property
def braked(self):
return self._mem["braked"]
@braked.setter
def braked(self, value):
self._mem["braked"] = bool(value)
@property
def used(self):
return self._mem["used"]
@used.setter
def used(self, value):
self._mem["used"] = bool(value)
class NavExitLaneChangeController:
def __init__(self, enable_bsm: bool):
self._params = Params()
self._enable_bsm = bool(enable_bsm)
self.enabled = self._read_enabled()
self._tick = 0
self.active = False
self.direction = log.LaneChangeDirection.none
self.auto_allowed = False
def _read_enabled(self) -> bool:
try:
return self._params.get_bool("NavExitLaneChange")
except Exception:
return False
def update_params(self) -> None:
if self._tick % 50 == 0:
self.enabled = self._read_enabled()
self._tick += 1
@staticmethod
def _raw(value):
return getattr(value, "raw", value)
def update(self, nav_state, carstate) -> None:
self.active = False
self.direction = log.LaneChangeDirection.none
self.auto_allowed = False
if not self.enabled or nav_state is None or not getattr(nav_state, "active", False):
return
if not getattr(nav_state, "nextManeuverValid", False):
return
if self._raw(getattr(nav_state, "nextManeuverType", _ManeuverType.none)) != int(_ManeuverType.exit):
return
distance = float(getattr(nav_state, "nextManeuverDistance", 0.0))
if not 0.0 < distance <= NAV_EXIT_COMMIT_DISTANCE:
return
direction = self._raw(getattr(nav_state, "nextManeuverDirection", _NavDirection.none))
if direction == int(_NavDirection.left):
self.direction = log.LaneChangeDirection.left
elif direction == int(_NavDirection.right):
self.direction = log.LaneChangeDirection.right
else:
return
self.active = True
blindspot = carstate.leftBlindspot if self.direction == log.LaneChangeDirection.left else carstate.rightBlindspot
self.auto_allowed = (not blindspot) if self._enable_bsm else False
AutoLaneChangeMode = LaneSwapPreset
AUTO_LANE_CHANGE_TIMER = LANE_SWAP_SECONDS
ONE_SECOND_DELAY = BLINDSPOT_WAIT_OFFSET
class IQLaneSwapController(LaneSwapEngine):
def __init__(self, desire_helper):
super().__init__(desire_helper)
def reset(self) -> None:
self.finalize()
def update_params(self) -> None:
self.heartbeat()
def update_lane_change(self, blindspot_detected: bool, brake_pressed: bool) -> None:
self.sample(blindspot_now=blindspot_detected, brake_now=brake_pressed)
def update_state(self) -> None:
self.finalize()
@property
def lane_change_wait_timer(self):
return self.elapsed
@lane_change_wait_timer.setter
def lane_change_wait_timer(self, value):
self._mem["sec"] = float(value)
@property
def lane_change_delay(self):
return self.delay
@lane_change_delay.setter
def lane_change_delay(self, value):
self._mem["gate"] = float(value)
@property
def lane_change_set_timer(self):
return self.preset
@lane_change_set_timer.setter
def lane_change_set_timer(self, value):
self.preset = value
@property
def lane_change_bsm_delay(self):
return self.bsm_hold
@lane_change_bsm_delay.setter
def lane_change_bsm_delay(self, value):
self.bsm_hold = value
@property
def prev_brake_pressed(self):
return self.braked
@prev_brake_pressed.setter
def prev_brake_pressed(self, value):
self.braked = value
@property
def auto_lane_change_allowed(self):
return self.ready
@auto_lane_change_allowed.setter
def auto_lane_change_allowed(self, value):
self._mem["ready"] = bool(value)
@property
def prev_lane_change(self):
return self.used
@prev_lane_change.setter
def prev_lane_change(self, value):
self.used = value

157
artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/controls/lib/helpers/lane_turn.py Normal file
View File

@@ -0,0 +1,157 @@
"""
Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos/
"""
from __future__ import annotations
from dataclasses import dataclass
from iqpilot.cereal import custom
from iqpilot.common.constants import CV
from iqpilot.common.params import Params
TurnDirection = custom.IQTurnSignalDirection
TURN_TRIGGER_MPS = 20 * CV.MPH_TO_MS
TURN_SPEED_GATE_MPS = TURN_TRIGGER_MPS
LANE_CHANGE_SPEED_MIN = TURN_SPEED_GATE_MPS
@dataclass
class _TurnGateState:
active: bool = False
speed_limit_mps: float = TURN_TRIGGER_MPS
outcome: int = TurnDirection.none
refresh_tick: int = 0
def _mph_param_to_mps(raw_value) -> float:
try:
return float(raw_value) * CV.MPH_TO_MS
except (TypeError, ValueError):
return TURN_TRIGGER_MPS
def _resolve_signal_choice(speed_mps: float,
speed_limit_mps: float,
left_signal: bool,
right_signal: bool,
left_blocked: bool,
right_blocked: bool) -> int:
if speed_mps >= speed_limit_mps:
return TurnDirection.none
if left_signal and not right_signal and not left_blocked:
return TurnDirection.turnLeft
if right_signal and not left_signal and not right_blocked:
return TurnDirection.turnRight
return TurnDirection.none
class TurnSignalPlanner:
_REFRESH_STRIDE = 50
def __init__(self, desire_hub):
self._desire_hub = desire_hub
self._params = Params()
self._state = _TurnGateState()
self.reload_setup()
def _refresh_from_params(self) -> None:
requested_gate = _mph_param_to_mps(self._params.get("IQLaneTurnValue", return_default=True))
self._state.active = self._params.get_bool("IQLaneTurnDesire")
self._state.speed_limit_mps = min(TURN_TRIGGER_MPS, requested_gate)
def _consume_legacy_kwargs(self, **legacy) -> tuple[bool, bool, bool, bool, float]:
return (
bool(legacy.get("blindspot_left", False)),
bool(legacy.get("blindspot_right", False)),
bool(legacy.get("left_blinker", False)),
bool(legacy.get("right_blinker", False)),
float(legacy.get("v_ego", 0.0)),
)
def reload_setup(self):
self._refresh_from_params()
def heartbeat(self) -> None:
if self._state.refresh_tick % self._REFRESH_STRIDE == 0:
self._refresh_from_params()
self._state.refresh_tick += 1
def sample(self,
blocked_l: bool = False,
blocked_r: bool = False,
blink_l: bool = False,
blink_r: bool = False,
speed_mps: float = 0.0,
**legacy) -> None:
if legacy:
blocked_l, blocked_r, blink_l, blink_r, speed_mps = self._consume_legacy_kwargs(**legacy)
self._state.outcome = _resolve_signal_choice(speed_mps,
self._state.speed_limit_mps,
blink_l,
blink_r,
blocked_l,
blocked_r)
def output(self):
return self._state.outcome if self._state.active else TurnDirection.none
@property
def enabled(self):
return self._state.active
@enabled.setter
def enabled(self, value):
self._state.active = bool(value)
@property
def speed_gate(self):
return self._state.speed_limit_mps
@speed_gate.setter
def speed_gate(self, value):
self._state.speed_limit_mps = float(value)
@property
def turn_direction(self):
return self._state.outcome
@turn_direction.setter
def turn_direction(self, value):
self._state.outcome = value
class IQNavTurnController(TurnSignalPlanner):
def __init__(self, desire_helper):
super().__init__(desire_helper)
def read_params(self):
self.reload_setup()
def update_params(self) -> None:
self.heartbeat()
def update_lane_turn(self,
blindspot_left: bool,
blindspot_right: bool,
left_blinker: bool,
right_blinker: bool,
v_ego: float) -> None:
self.sample(blocked_l=blindspot_left,
blocked_r=blindspot_right,
blink_l=left_blinker,
blink_r=right_blinker,
speed_mps=v_ego)
def get_turn_direction(self):
return self.output()
@property
def lane_turn_value(self):
return self.speed_gate
@lane_turn_value.setter
def lane_turn_value(self, value):
self.speed_gate = value

268
artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/controls/lib/helpers/lateral_edge_guard.py Normal file
View File

@@ -0,0 +1,268 @@
"""
Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos/
"""
from __future__ import annotations
import math
from dataclasses import dataclass, replace
from enum import IntEnum
from typing import Any
from iqpilot.cereal import custom, log
from iqpilot.common.constants import CV
from iqpilot.common.params import Params
from iqpilot.common.swaglog import cloudlog
MIN_ACTIVE_SPEED_MPS = 20.0 * CV.MPH_TO_MS
MAX_VALID_ROAD_EDGE_STD_M = 1.0
EDGE_CONFIDENCE_SIGMA = 1.0
ROAD_EDGE_LOOKAHEAD_MIN_M = 5.0
ROAD_EDGE_LOOKAHEAD_MAX_M = 40.0
LANE_CENTER_OFFSET_M = 3.5
VEHICLE_LATERAL_HALF_WIDTH_M = 1.90 / 2.0
EDGE_CLEARANCE_MARGIN_M = 0.25
ADJACENT_LANE_LINE_PROB = 0.5
EGO_LANE_LINE_PROB_MIN = 0.5
MIN_MEASURED_LANE_WIDTH_M = 2.5
MAX_MEASURED_LANE_WIDTH_M = 4.5
OUTER_LANE_LINE_INDEX = (0, 3)
EGO_LANE_LINE_INDEX = (1, 2)
REQUIRED_ROAD_EDGE_DISTANCE_M = LANE_CENTER_OFFSET_M + VEHICLE_LATERAL_HALF_WIDTH_M + EDGE_CLEARANCE_MARGIN_M
BLOCK_DEBOUNCE_S = 0.30
CLEAR_DEBOUNCE_S = 0.50
UNAVAILABLE_HOLD_S = 0.50
TIMER_EPSILON_S = 1e-9
PARAM_REFRESH_FRAMES = 50
LaneChangeDirection = log.LaneChangeDirection
LateralEdgeBlock = custom.IQLateralEdgeBlock
class RoadEdgeDataState(IntEnum):
VALID = 0
UNAVAILABLE = 1
INVALID = 2
@dataclass(frozen=True, slots=True)
class RoadEdgeMeasurement:
state: RoadEdgeDataState
lateral_distance_m: float | None = None
conservative_distance_m: float | None = None
should_block: bool | None = None
@dataclass(frozen=True, slots=True)
class _SideState:
blocked: bool = False
block_timer_s: float = 0.0
clear_timer_s: float = 0.0
unavailable_timer_s: float = 0.0
fallback_reported: bool = False
def evaluate_road_edge(edge: Any, std_m: Any, direction: int,
lane_width_m: float = LANE_CENTER_OFFSET_M) -> RoadEdgeMeasurement:
if edge is None or std_m is None:
return RoadEdgeMeasurement(RoadEdgeDataState.UNAVAILABLE)
try:
xs = edge.x
ys = edge.y
count = len(xs)
y_count = len(ys)
except (AttributeError, TypeError):
return RoadEdgeMeasurement(RoadEdgeDataState.UNAVAILABLE)
if count == 0 or y_count != count:
return RoadEdgeMeasurement(RoadEdgeDataState.UNAVAILABLE)
try:
std = float(std_m)
except (TypeError, ValueError):
return RoadEdgeMeasurement(RoadEdgeDataState.INVALID)
if not math.isfinite(std) or std < 0.0 or std > MAX_VALID_ROAD_EDGE_STD_M:
return RoadEdgeMeasurement(RoadEdgeDataState.INVALID)
lateral_distance_m: float | None = None
for idx in range(count):
try:
x_m = float(xs[idx])
y_m = float(ys[idx])
except (IndexError, TypeError, ValueError):
return RoadEdgeMeasurement(RoadEdgeDataState.UNAVAILABLE)
if not math.isfinite(x_m) or not math.isfinite(y_m):
return RoadEdgeMeasurement(RoadEdgeDataState.UNAVAILABLE)
if not ROAD_EDGE_LOOKAHEAD_MIN_M <= x_m <= ROAD_EDGE_LOOKAHEAD_MAX_M:
continue
if ((direction == LaneChangeDirection.left and y_m >= 0.0) or
(direction == LaneChangeDirection.right and y_m <= 0.0)):
return RoadEdgeMeasurement(RoadEdgeDataState.INVALID)
distance_m = abs(y_m)
lateral_distance_m = distance_m if lateral_distance_m is None else min(lateral_distance_m, distance_m)
if lateral_distance_m is None:
return RoadEdgeMeasurement(RoadEdgeDataState.UNAVAILABLE)
conservative_distance_m = lateral_distance_m - EDGE_CONFIDENCE_SIGMA * std
required_distance_m = lane_width_m + VEHICLE_LATERAL_HALF_WIDTH_M + EDGE_CLEARANCE_MARGIN_M
return RoadEdgeMeasurement(
RoadEdgeDataState.VALID,
lateral_distance_m,
conservative_distance_m,
conservative_distance_m < required_distance_m,
)
def step_side_guard(state: _SideState, measurement: RoadEdgeMeasurement, speed_active: bool,
dt_s: float) -> tuple[_SideState, bool]:
if not speed_active:
return _SideState(), False
if measurement.state == RoadEdgeDataState.UNAVAILABLE:
unavailable_timer_s = state.unavailable_timer_s + dt_s
if unavailable_timer_s < UNAVAILABLE_HOLD_S - TIMER_EPSILON_S:
return _SideState(state.blocked, unavailable_timer_s=unavailable_timer_s,
fallback_reported=state.fallback_reported), False
fallback_started = not state.fallback_reported
return _SideState(unavailable_timer_s=unavailable_timer_s, fallback_reported=True), fallback_started
should_block = bool(measurement.should_block) if measurement.state == RoadEdgeDataState.VALID else False
if should_block == state.blocked:
return _SideState(blocked=state.blocked), False
if should_block:
block_timer_s = state.block_timer_s + dt_s
if block_timer_s >= BLOCK_DEBOUNCE_S - TIMER_EPSILON_S:
return _SideState(blocked=True), False
return _SideState(block_timer_s=block_timer_s), False
clear_timer_s = state.clear_timer_s + dt_s
if clear_timer_s >= CLEAR_DEBOUNCE_S - TIMER_EPSILON_S:
return _SideState(), False
return _SideState(blocked=True, clear_timer_s=clear_timer_s), False
class LateralEdgeGuard:
def __init__(self, enabled: bool | None = None) -> None:
self._params = Params() if enabled is None else None
self._param_refresh_frame = 0
self.enabled = self._read_enabled() if enabled is None else enabled
self._active = False
self._left = _SideState()
self._right = _SideState()
self.left_measurement = RoadEdgeMeasurement(RoadEdgeDataState.UNAVAILABLE)
self.right_measurement = RoadEdgeMeasurement(RoadEdgeDataState.UNAVAILABLE)
def _read_enabled(self) -> bool:
try:
return bool(self._params and self._params.get_bool("IQEdgeGuard"))
except Exception:
return False
def _refresh_enabled(self) -> None:
if self._params is not None and self._param_refresh_frame % PARAM_REFRESH_FRAMES == 0:
self.enabled = self._read_enabled()
self._param_refresh_frame += 1
def _reset(self) -> None:
self._left = _SideState()
self._right = _SideState()
self.left_measurement = RoadEdgeMeasurement(RoadEdgeDataState.UNAVAILABLE)
self.right_measurement = RoadEdgeMeasurement(RoadEdgeDataState.UNAVAILABLE)
@staticmethod
def _model_side(modeldata: Any, side_index: int) -> tuple[Any | None, Any | None]:
if modeldata is None:
return None, None
try:
edges = modeldata.roadEdges
stds = modeldata.roadEdgeStds
if len(edges) <= side_index or len(stds) <= side_index:
return None, None
return edges[side_index], stds[side_index]
except (AttributeError, TypeError):
return None, None
@staticmethod
def _lane_line_prob(modeldata: Any, index: int) -> float | None:
if modeldata is None:
return None
try:
probs = modeldata.laneLineProbs
if len(probs) <= index:
return None
value = float(probs[index])
except (AttributeError, TypeError, IndexError, ValueError):
return None
return value if math.isfinite(value) else None
@classmethod
def _adjacent_lane_visible(cls, modeldata: Any, side_index: int) -> bool:
prob = cls._lane_line_prob(modeldata, OUTER_LANE_LINE_INDEX[side_index])
return prob is not None and prob > ADJACENT_LANE_LINE_PROB
@classmethod
def _measured_lane_width(cls, modeldata: Any) -> float:
left_prob = cls._lane_line_prob(modeldata, EGO_LANE_LINE_INDEX[0])
right_prob = cls._lane_line_prob(modeldata, EGO_LANE_LINE_INDEX[1])
if left_prob is None or right_prob is None:
return LANE_CENTER_OFFSET_M
if left_prob <= EGO_LANE_LINE_PROB_MIN or right_prob <= EGO_LANE_LINE_PROB_MIN:
return LANE_CENTER_OFFSET_M
try:
lines = modeldata.laneLines
left_y = float(lines[EGO_LANE_LINE_INDEX[0]].y[0])
right_y = float(lines[EGO_LANE_LINE_INDEX[1]].y[0])
except (AttributeError, TypeError, IndexError, ValueError):
return LANE_CENTER_OFFSET_M
width = abs(right_y - left_y)
if not math.isfinite(width):
return LANE_CENTER_OFFSET_M
return min(max(width, MIN_MEASURED_LANE_WIDTH_M), MAX_MEASURED_LANE_WIDTH_M)
@staticmethod
def _apply_lane_evidence(measurement: RoadEdgeMeasurement, lane_visible: bool) -> RoadEdgeMeasurement:
if lane_visible and measurement.state == RoadEdgeDataState.VALID and measurement.should_block:
return replace(measurement, should_block=False)
return measurement
def update(self, modeldata: Any, v_ego_mps: float, dt_s: float) -> None:
self._refresh_enabled()
if not self.enabled:
if self._active:
self._reset()
self._active = False
return
if not self._active:
self._reset()
self._active = True
dt = max(float(dt_s), 0.0)
left_edge, left_std = self._model_side(modeldata, 0)
right_edge, right_std = self._model_side(modeldata, 1)
lane_width_m = self._measured_lane_width(modeldata)
self.left_measurement = self._apply_lane_evidence(
evaluate_road_edge(left_edge, left_std, LaneChangeDirection.left, lane_width_m),
self._adjacent_lane_visible(modeldata, 0))
self.right_measurement = self._apply_lane_evidence(
evaluate_road_edge(right_edge, right_std, LaneChangeDirection.right, lane_width_m),
self._adjacent_lane_visible(modeldata, 1))
speed_active = math.isfinite(v_ego_mps) and v_ego_mps >= MIN_ACTIVE_SPEED_MPS
self._left, left_fallback = step_side_guard(self._left, self.left_measurement, speed_active, dt)
self._right, right_fallback = step_side_guard(self._right, self.right_measurement, speed_active, dt)
if left_fallback:
cloudlog.warning(f"lateral edge guard: left road edge unavailable for {UNAVAILABLE_HOLD_S:.2f} s; falling back to not blocking")
if right_fallback:
cloudlog.warning(f"lateral edge guard: right road edge unavailable for {UNAVAILABLE_HOLD_S:.2f} s; falling back to not blocking")
def block_for_direction(self, direction: int) -> custom.IQLateralEdgeBlock:
if not self.enabled:
return LateralEdgeBlock.none
if direction == LaneChangeDirection.left and self._left.blocked:
return LateralEdgeBlock.left
if direction == LaneChangeDirection.right and self._right.blocked:
return LateralEdgeBlock.right
return LateralEdgeBlock.none

0
iqdbc_repo/iqdbc/car/byd/tests/__init__.py → artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/iqmodeld/__init__.py
View File

9
artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/iqmodeld/big_catalog.py Normal file
View File

@@ -0,0 +1,9 @@
"""
Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos/
"""
from iqpilot._proprietary_loader import ProprietaryModuleMissing, load_private_module
try:
load_private_module(__name__, "iqpilot_private.models.big_catalog")
except ProprietaryModuleMissing:
from iqpilot.models_private_src.big_catalog import *

70
artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/iqmodeld/camera.py Normal file
View File

@@ -0,0 +1,70 @@
"""
Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos/
"""
import numpy as np
from iqpilot.common.transformations.camera import DEVICE_CAMERAS
MAX_CAMERA_OFFSET_METERS = 0.35
class _OffsetSmoother:
def __init__(self, blend: float = 0.1):
self._blend = blend
self._value = 0.0
def step(self, target: float) -> float:
self._value = ((1.0 - self._blend) * self._value) + (self._blend * float(target))
return self._value
def _clamped_offset(raw_offset) -> float:
try:
parsed = float(raw_offset)
except (TypeError, ValueError):
parsed = 0.0
return float(np.clip(parsed, -MAX_CAMERA_OFFSET_METERS, MAX_CAMERA_OFFSET_METERS))
def _camera_profile(sm):
return DEVICE_CAMERAS[(str(sm["deviceState"].deviceType), str(sm["roadCameraState"].sensor))]
def _calibration_height(sm) -> float:
from iqpilot.selfdrive.locationd.calibrationd import HEIGHT_SANE_MIN, HEIGHT_SANE_MAX
h = sm["extrinsicsCalibration"].height[0] if sm["extrinsicsCalibration"].height else 1.22
return h if HEIGHT_SANE_MIN <= h <= HEIGHT_SANE_MAX else 1.22
def _sheared_transform(model_transform, intrinsics, height: float, lateral_offset: float):
optical_center_y = intrinsics[1, 2]
projection_bias = np.eye(3, dtype=np.float32)
projection_bias[0, 1] = lateral_offset / height
projection_bias[0, 2] = -(lateral_offset / height) * optical_center_y
return (projection_bias @ model_transform).astype(np.float32)
class CameraOffsetHelper:
def __init__(self):
self.camera_offset = 0.0
self.actual_camera_offset = 0.0
self._smoother = _OffsetSmoother()
@staticmethod
def apply_camera_offset(model_transform, intrinsics, height, offset_param):
return _sheared_transform(model_transform, intrinsics, height, offset_param)
def set_offset(self, offset):
self.camera_offset = _clamped_offset(offset)
def update(self, model_transform_main, model_transform_extra, sm, main_wide_camera, extra_uses_wide_camera=True):
self.actual_camera_offset = self._smoother.step(self.camera_offset)
camera_bundle = _camera_profile(sm)
camera_height = _calibration_height(sm)
main_intrinsics = camera_bundle.ecam.intrinsics if main_wide_camera else camera_bundle.fcam.intrinsics
extra_intrinsics = camera_bundle.ecam.intrinsics if extra_uses_wide_camera else camera_bundle.fcam.intrinsics
return (
self.apply_camera_offset(model_transform_main, main_intrinsics, camera_height, self.actual_camera_offset),
self.apply_camera_offset(model_transform_extra, extra_intrinsics, camera_height, self.actual_camera_offset),
)

133
artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/iqmodeld/config.py Normal file
View File

@@ -0,0 +1,133 @@
"""
Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos/
"""
from __future__ import annotations
import numpy as np
def index_function(index: int, max_val: float = 192, max_idx: int = 32) -> float:
return max_val * ((index / max_idx) ** 2)
def _quadratic_series(limit: float, steps: int) -> list[float]:
return [index_function(index, max_val=limit, max_idx=steps - 1) for index in range(steps)]
def _probability_window(*values: float) -> np.ndarray:
return np.asarray(values, dtype=np.float32)
def _field_group(start: int, stop: int, stride: int) -> slice:
return slice(start, stop, stride)
_IDX_COUNT = 33
_T_AXIS = _quadratic_series(10.0, _IDX_COUNT)
_X_AXIS = _quadratic_series(192.0, _IDX_COUNT)
class ModelConstants:
IDX_N = _IDX_COUNT
T_IDXS = _T_AXIS
X_IDXS = _X_AXIS
LEAD_T_IDXS = [0.0, 2.0, 4.0, 6.0, 8.0, 10.0]
LEAD_T_OFFSETS = [0.0, 2.0, 4.0]
META_T_IDXS = [2.0, 4.0, 6.0, 8.0, 10.0]
MODEL_FREQ = 20
FEATURE_LEN = 512
FULL_HISTORY_BUFFER_LEN = 99
HISTORY_BUFFER_LEN = FULL_HISTORY_BUFFER_LEN
DESIRE_LEN = 8
TRAFFIC_CONVENTION_LEN = 2
NAV_FEATURE_LEN = 256
NAV_INSTRUCTION_LEN = 150
LAT_PLANNER_STATE_LEN = 4
LATERAL_CONTROL_PARAMS_LEN = 2
PREV_DESIRED_CURV_LEN = 1
FCW_THRESHOLDS_5MS2 = _probability_window(0.05, 0.05, 0.15, 0.15, 0.15)
FCW_THRESHOLDS_3MS2 = _probability_window(0.7, 0.7)
FCW_5MS2_PROBS_WIDTH = 5
FCW_3MS2_PROBS_WIDTH = 2
DISENGAGE_WIDTH = 5
POSE_WIDTH = 6
WIDE_FROM_DEVICE_WIDTH = 3
SIM_POSE_WIDTH = 6
LEAD_WIDTH = 4
LANE_LINES_WIDTH = 2
ROAD_EDGES_WIDTH = 2
PLAN_WIDTH = 15
DESIRE_PRED_WIDTH = 8
LAT_PLANNER_SOLUTION_WIDTH = 4
DESIRED_CURV_WIDTH = 1
NUM_LANE_LINES = 4
NUM_ROAD_EDGES = 2
LEAD_TRAJ_LEN = 6
DESIRE_PRED_LEN = 4
PLAN_MHP_N = 5
LEAD_MHP_N = 2
PLAN_MHP_SELECTION = 1
LEAD_MHP_SELECTION = 3
FCW_THRESHOLD_5MS2_HIGH = 0.15
FCW_THRESHOLD_5MS2_LOW = 0.05
FCW_THRESHOLD_3MS2 = 0.7
CONFIDENCE_BUFFER_LEN = 5
RYG_GREEN = 0.01165
RYG_YELLOW = 0.06157
POLY_PATH_DEGREE = 4
class Plan:
POSITION = slice(0, 3)
VELOCITY = slice(3, 6)
ACCELERATION = slice(6, 9)
T_FROM_CURRENT_EULER = slice(9, 12)
ORIENTATION_RATE = slice(12, 15)
class Meta:
ENGAGED = _field_group(0, 1, 1)
GAS_DISENGAGE = _field_group(1, 31, 6)
BRAKE_DISENGAGE = _field_group(2, 31, 6)
STEER_OVERRIDE = _field_group(3, 31, 6)
HARD_BRAKE_3 = _field_group(4, 31, 6)
HARD_BRAKE_4 = _field_group(5, 31, 6)
HARD_BRAKE_5 = _field_group(6, 31, 6)
GAS_PRESS = _field_group(31, 55, 4)
BRAKE_PRESS = _field_group(32, 55, 4)
LEFT_BLINKER = _field_group(33, 55, 4)
RIGHT_BLINKER = _field_group(34, 55, 4)
class MetaTombRaider:
ENGAGED = _field_group(0, 1, 1)
GAS_DISENGAGE = _field_group(1, 41, 8)
BRAKE_DISENGAGE = _field_group(2, 41, 8)
STEER_OVERRIDE = _field_group(3, 41, 8)
HARD_BRAKE_3 = _field_group(4, 41, 8)
HARD_BRAKE_4 = _field_group(5, 41, 8)
HARD_BRAKE_5 = _field_group(6, 41, 8)
GAS_PRESS = _field_group(7, 41, 8)
BRAKE_PRESS = _field_group(8, 41, 8)
LEFT_BLINKER = _field_group(41, 53, 2)
RIGHT_BLINKER = _field_group(42, 53, 2)
class MetaSimPose:
ENGAGED = _field_group(0, 1, 1)
GAS_DISENGAGE = _field_group(1, 36, 7)
BRAKE_DISENGAGE = _field_group(2, 36, 7)
STEER_OVERRIDE = _field_group(3, 36, 7)
HARD_BRAKE_3 = _field_group(4, 36, 7)
HARD_BRAKE_4 = _field_group(5, 36, 7)
HARD_BRAKE_5 = _field_group(6, 36, 7)
GAS_PRESS = _field_group(7, 36, 7)
LEFT_BLINKER = _field_group(36, 48, 2)
RIGHT_BLINKER = _field_group(37, 48, 2)

767
artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/iqmodeld/daemon.py Normal file
View File

@@ -0,0 +1,767 @@
#!/usr/bin/env python3
from __future__ import annotations
import sys
import time
from dataclasses import dataclass
from typing import Any
import iqpilot.cereal.messaging as messaging
import numpy as np
from iqpilot.cereal import car, custom, log
from iqpilot.cereal.messaging import PubMaster, SubMaster
from iqpilot.cereal.visionipc import VisionStreamType
from msgq.visionipc import VisionBuf, VisionIpcClient
from iqdbc.car.car_helpers import get_demo_car_params
from setproctitle import setproctitle
from iqpilot.common.filter_simple import FirstOrderFilter
from iqpilot.common.iq_perf import PerfSample, PerfTraceEmitter, PerfTraceRing
from iqpilot.common.params import Params
from iqpilot.common.realtime import DT_MDL, config_realtime_process
from iqpilot.common.swaglog import cloudlog
from iqpilot.common.transformations.camera import DEVICE_CAMERAS
from iqpilot.common.transformations.model import get_warp_matrix
from iqpilot.selfdrive.controls.lib.desire_helper import DesireHelper
from iqpilot.selfdrive.controls.lib.drive_helpers import (
MODEL_SMOOTHING_MAX_TOTAL_SEC,
dynamic_lat_smooth_extra_seconds,
get_accel_from_plan,
smooth_value,
)
from iqpilot.selfdrive.locationd.calibration_helpers import get_calibrated_rpy
from iqpilot.system import sentry
from iqpilot.common.steer_delay import lateral_action_delay
from iqpilot.selfdrive.iqmodeld.models.helpers import get_active_bundle
from iqpilot.selfdrive.iqmodeld.models.inference_state import InferenceStateBase
from iqpilot.selfdrive.iqmodeld.models.runners.model_runner import get_model_runner
from iqpilot.selfdrive.iqmodeld.camera import CameraOffsetHelper
from iqpilot.selfdrive.iqmodeld.config import Plan
from iqpilot.selfdrive.iqmodeld.messaging import (
DrivePacketMemory,
pick_curvature,
populate_drive_messages,
populate_odometry_message,
)
from iqpilot.selfdrive.iqmodeld.metadata import select_meta_layout
try:
from iqpilot.selfdrive.iqmodeld.native.iqmodel_pyx import RoadProjector, WarpContext
except ModuleNotFoundError:
class WarpContext:
def __init__(self, *args, **kwargs):
raise ModuleNotFoundError("iqpilot.selfdrive.iqmodeld.native.iqmodel_pyx is not built")
class RoadProjector:
def __init__(self, *args, **kwargs):
raise ModuleNotFoundError("iqpilot.selfdrive.iqmodeld.native.iqmodel_pyx is not built")
PROCESS_NAME = "iqpilot.selfdrive.iqmodeld.daemon"
IQP_NAV_MODEL_INFLUENCE_ENABLED = False
TurnDirection = custom.IQTurnSignalDirection
IQMODEL_EVAL_WARN_US = int(DT_MDL * 1_000_000)
IQMODEL_EVAL_ERROR_US = IQMODEL_EVAL_WARN_US * 2
_FRAME_STARVED_BACKOFF_POLLS = 5
_FRAME_STARVED_BACKOFF_SECONDS = 0.005
_FRAME_STARVED_LOG_EVERY = 200
def _plan_y_std_1s(outputs: dict[str, np.ndarray]) -> float:
# plan_stds is (batch, IDX_N, PLAN_WIDTH); index 10 ~= 1s ahead (see ModelConstants.T_IDXS),
# POSITION is an (x, y, z) slice within PLAN_WIDTH so [1] picks the lateral (y) std.
try:
return float(outputs["plan_stds"][0, 10, Plan.POSITION][1])
except (KeyError, IndexError):
return 0.0
def _model_lat_smooth_max_sec(params: Params) -> float:
if not params.get_bool("ModelSmoothingEnabled"):
return 0.0
try:
raw = params.get("ModelLatSmoothSec", return_default=True)
raw = 0 if raw is None else int(raw)
except (ValueError, TypeError):
raw = 0
return min(max(raw, 0), 30) * 0.01
@dataclass
class CaptureStamp:
frame_id: int = 0
timestamp_sof: int = 0
timestamp_eof: int = 0
@classmethod
def from_vipc(cls, client: VisionIpcClient) -> "CaptureStamp":
return cls(client.frame_id, client.timestamp_sof, client.timestamp_eof)
@dataclass(frozen=True)
class StreamLayout:
dual_camera: bool
main_is_wide: bool
primary_stream: VisionStreamType
class ReplayLedger:
def __init__(self, tensor_shapes: dict[str, tuple[int, ...]], frame_inputs: list[str]):
self.inputs: dict[str, np.ndarray] = {}
self.archive: dict[str, np.ndarray] = {}
self.selectors: dict[str, np.ndarray] = {}
self._frame_inputs = set(frame_inputs)
self._pulse_name: str | None = None
self._pulse_memory: np.ndarray | None = None
feature_shape = tensor_shapes.get("features_buffer")
for tensor_name, tensor_shape in tensor_shapes.items():
if tensor_name in self._frame_inputs:
continue
self.inputs[tensor_name] = np.zeros(tensor_shape, dtype=np.float32)
if len(tensor_shape) != 3 or tensor_shape[1] <= 1:
continue
history_len = self._history_length(tensor_shape, feature_shape)
self.archive[tensor_name] = np.zeros((1, history_len, tensor_shape[2]), dtype=np.float32)
export_index = self._export_index(tensor_shape, history_len, feature_shape)
if export_index is not None:
self.selectors[tensor_name] = export_index
if tensor_name.startswith("desire"):
self._pulse_name = tensor_name
self._pulse_memory = np.zeros(tensor_shape[2], dtype=np.float32)
@staticmethod
def _history_length(tensor_shape: tuple[int, ...], feature_shape: tuple[int, ...] | None) -> int:
if tensor_shape[1] >= 99:
return tensor_shape[1]
if tensor_shape[1] in (24, 25) and feature_shape is not None and feature_shape[1] == 24:
return (feature_shape[1] + 1) * 4
return tensor_shape[1] * 4
@staticmethod
def _export_index(tensor_shape: tuple[int, ...], history_len: int,
feature_shape: tuple[int, ...] | None) -> np.ndarray | None:
if tensor_shape[1] in (24, 25) and feature_shape is not None and feature_shape[1] == 24:
stride = int(-history_len / tensor_shape[1])
return np.arange(stride, stride * (tensor_shape[1] + 1), stride)[::-1]
if tensor_shape[1] == 25:
skip = history_len // tensor_shape[1]
return np.arange(history_len)[-1 - (skip * (tensor_shape[1] - 1))::skip]
if tensor_shape[1] >= 99:
return np.arange(tensor_shape[1])
return None
@property
def pulse_name(self) -> str:
if self._pulse_name is None:
raise KeyError("No desire-like pulse input present in model inputs")
return self._pulse_name
def _shift_archive(self, tensor_name: str) -> np.ndarray:
history = self.archive[tensor_name]
history[0, :-1] = history[0, 1:]
return history
def inject_pulse(self, pulse_values: np.ndarray) -> None:
pulse = pulse_values.copy()
pulse[0] = 0
assert self._pulse_memory is not None
rising = np.where(pulse - self._pulse_memory > 0.99, pulse, 0)
self._pulse_memory[:] = pulse
history = self._shift_archive(self.pulse_name)
history[0, -1] = rising
exported_shape = self.inputs[self.pulse_name].shape
if history.shape[1] > exported_shape[1]:
stride = history.shape[1] // exported_shape[1]
self.inputs[self.pulse_name][:] = history[0].reshape(
exported_shape[0], exported_shape[1], stride, -1
).max(axis=2)
return
self.inputs[self.pulse_name][:] = history[0, self.selectors[self.pulse_name]]
def merge_inputs(self, fresh_inputs: dict[str, np.ndarray]) -> None:
pulse_name = self.pulse_name
for tensor_name, tensor_value in fresh_inputs.items():
if tensor_name in self.inputs and tensor_name != pulse_name:
self.inputs[tensor_name][:] = tensor_value
def note_hidden_state(self, hidden_state: np.ndarray) -> None:
if "features_buffer" not in self.archive:
return
history = self._shift_archive("features_buffer")
history[0, -1] = hidden_state[0]
self.inputs["features_buffer"][:] = history[0, self.selectors["features_buffer"]]
def note_feedback(self, tensor_name: str, values: np.ndarray, zero_export: bool = False) -> None:
if tensor_name not in self.archive:
return
history = self._shift_archive(tensor_name)
history[0, -1, :] = values[0]
exported = history[0, self.selectors[tensor_name]]
self.inputs[tensor_name][:] = 0 * exported if zero_export else exported
def _planplus_gain(vehicle_speed: float) -> float:
return 0.75 if vehicle_speed >= 25.0 else 1.0
def _merged_plan(runtime_state: "NeuralEngineState", outputs: dict[str, np.ndarray], vehicle_speed: float) -> np.ndarray:
base_plan = outputs["plan"][0]
if "planplus" not in outputs:
return base_plan
return base_plan + (runtime_state.PLANPLUS_CONTROL * _planplus_gain(vehicle_speed)) * outputs["planplus"][0]
class NeuralEngineState(InferenceStateBase):
frames: dict[str, RoadProjector]
def __init__(self, gpu_context: WarpContext):
super().__init__()
runner = get_model_runner()
bundle = get_active_bundle()
self.model_runner = runner
self.constants = runner.constants
self.generation = bundle.generation if bundle is not None else None
knob_values = {entry.key: entry.value for entry in bundle.overrides} if bundle is not None else {}
self.LAT_SMOOTH_SECONDS = float(knob_values.get("lat", ".0"))
self.LONG_SMOOTH_SECONDS = float(knob_values.get("long", ".0"))
self.MIN_LAT_CONTROL_SPEED = 0.3
self.PLANPLUS_CONTROL = 1.0
self.model_smoothing_max_extra_sec = 0.0
context_depth = 5 if runner.is_20hz else 2
self.frames = {
stream_name: RoadProjector(gpu_context, context_depth)
for stream_name in runner.vision_input_names
}
self._ledger = ReplayLedger(runner.input_shapes, runner.vision_input_names)
self.numpy_inputs = self._ledger.inputs
self.temporal_buffers = self._ledger.archive
self.temporal_idxs_map = self._ledger.selectors
@property
def mlsim(self) -> bool:
return bool(self.generation is not None and self.generation >= 11)
@property
def desire_key(self) -> str:
return self._ledger.pulse_name
def _warp_frames(self, vision_bufs: dict[str, VisionBuf],
transform_map: dict[str, np.ndarray]) -> dict[str, Any]:
return {
stream_name: self.frames[stream_name].stage(vision_bufs[stream_name], transform_map[stream_name].flatten())
for stream_name in self.model_runner.vision_input_names
}
def _run_split_model(self) -> dict[str, np.ndarray]:
if hasattr(self.model_runner, "run_vision"):
vision_packet = self.model_runner.run_vision()
self._ledger.note_hidden_state(vision_packet["hidden_state"])
self.model_runner.refresh_policy_features(self.numpy_inputs["features_buffer"])
return {**vision_packet, **self.model_runner.run_policy()}
result = self.model_runner.run_model()
if "hidden_state" in result:
self._ledger.note_hidden_state(result["hidden_state"])
return result
def _write_curvature_memory(self, outputs: dict[str, np.ndarray]) -> None:
if "desired_curvature" not in outputs:
return
feedback_slot = None
if "prev_desired_curvs" in self.numpy_inputs:
feedback_slot = "prev_desired_curvs"
elif "prev_desired_curv" in self.numpy_inputs:
feedback_slot = "prev_desired_curv"
if feedback_slot is not None:
self._ledger.note_feedback(feedback_slot, outputs["desired_curvature"], zero_export=self.mlsim)
def run(self, vision_bufs: dict[str, VisionBuf], transform_map: dict[str, np.ndarray],
fresh_inputs: dict[str, np.ndarray]) -> dict[str, np.ndarray] | None:
if not getattr(self.model_runner, "uses_opencl_warp", True):
return self.model_runner.run_fused(vision_bufs, transform_map, fresh_inputs)
self._ledger.inject_pulse(fresh_inputs[self.desire_key])
self._ledger.merge_inputs(fresh_inputs)
warped_frames = self._warp_frames(vision_bufs, transform_map)
self.model_runner.prepare_inputs(warped_frames, self.numpy_inputs, self.frames)
outputs = self._run_split_model()
self._write_curvature_memory(outputs)
return outputs
def get_action_from_model(self, outputs: dict[str, np.ndarray], previous_action: log.ModelDataV2.Action,
lat_action_t: float, long_action_t: float, vehicle_speed: float,
lat_smooth_seconds: float | None = None) -> log.ModelDataV2.Action:
if lat_smooth_seconds is None:
lat_smooth_seconds = self.LAT_SMOOTH_SECONDS
if "action" in outputs:
curvature_cmd = outputs["action"][0, 0] / (max(1.0, vehicle_speed)) ** 2
accel_cmd = outputs["action"][0, 1]
should_stop = bool(vehicle_speed < 0.3 and accel_cmd < 0.1)
accel_cmd = smooth_value(accel_cmd, previous_action.desiredAcceleration, self.LONG_SMOOTH_SECONDS)
if vehicle_speed > self.MIN_LAT_CONTROL_SPEED:
curvature_cmd = smooth_value(curvature_cmd, previous_action.desiredCurvature, lat_smooth_seconds)
else:
curvature_cmd = previous_action.desiredCurvature
return log.ModelDataV2.Action(
desiredCurvature=float(curvature_cmd),
desiredAcceleration=float(accel_cmd),
shouldStop=should_stop,
)
plan_rows = _merged_plan(self, outputs, vehicle_speed)
accel_cmd, should_stop = get_accel_from_plan(
plan_rows[:, Plan.VELOCITY][:, 0],
plan_rows[:, Plan.ACCELERATION][:, 0],
self.constants.T_IDXS,
action_t=long_action_t,
)
accel_cmd = smooth_value(accel_cmd, previous_action.desiredAcceleration, self.LONG_SMOOTH_SECONDS)
curvature_cmd = pick_curvature(outputs, plan_rows, vehicle_speed, lat_action_t, self.mlsim)
if self.generation is not None and self.generation >= 10:
if vehicle_speed > self.MIN_LAT_CONTROL_SPEED:
curvature_cmd = smooth_value(curvature_cmd, previous_action.desiredCurvature, lat_smooth_seconds)
else:
curvature_cmd = previous_action.desiredCurvature
return log.ModelDataV2.Action(
desiredCurvature=float(curvature_cmd),
desiredAcceleration=float(accel_cmd),
shouldStop=bool(should_stop),
)
class CameraIngress:
def __init__(self, gpu_context: WarpContext):
self.layout = self._discover_layout()
self._primary = VisionIpcClient("camerad", self.layout.primary_stream, True, gpu_context)
self._secondary = VisionIpcClient("camerad", VisionStreamType.VISION_STREAM_WIDE_ROAD, False, gpu_context)
while not self._primary.connect(False):
time.sleep(0.1)
while self.layout.dual_camera and not self._secondary.connect(False):
time.sleep(0.1)
cloudlog.warning(
f"connected main cam with buffer size: {self._primary.buffer_len} ({self._primary.width} x {self._primary.height})"
)
if self.layout.dual_camera:
cloudlog.warning(
f"connected extra cam with buffer size: {self._secondary.buffer_len} ({self._secondary.width} x {self._secondary.height})"
)
@staticmethod
def _discover_layout() -> StreamLayout:
while True:
available = VisionIpcClient.available_streams("camerad", block=False)
if available:
dual_camera = (
VisionStreamType.VISION_STREAM_WIDE_ROAD in available
and VisionStreamType.VISION_STREAM_ROAD in available
)
main_is_wide = VisionStreamType.VISION_STREAM_ROAD not in available
primary_stream = VisionStreamType.VISION_STREAM_WIDE_ROAD if main_is_wide else VisionStreamType.VISION_STREAM_ROAD
cloudlog.warning(
f"vision stream set up, main_wide_camera: {main_is_wide}, use_extra_client: {dual_camera}"
)
return StreamLayout(dual_camera=dual_camera, main_is_wide=main_is_wide, primary_stream=primary_stream)
time.sleep(0.1)
def pull(self) -> tuple[VisionBuf, VisionBuf, CaptureStamp, CaptureStamp] | None:
main_buf = None
wide_buf = None
main_stamp = CaptureStamp()
wide_stamp = CaptureStamp()
while main_stamp.timestamp_sof < wide_stamp.timestamp_sof + 25000000:
main_buf = self._primary.recv()
main_stamp = CaptureStamp.from_vipc(self._primary)
if main_buf is None:
return None
if not self.layout.dual_camera:
return main_buf, main_buf, main_stamp, main_stamp
while True:
wide_buf = self._secondary.recv()
wide_stamp = CaptureStamp.from_vipc(self._secondary)
if wide_buf is None or main_stamp.timestamp_sof < wide_stamp.timestamp_sof + 25000000:
break
if wide_buf is None:
return None
if abs(main_stamp.timestamp_sof - wide_stamp.timestamp_sof) > 10000000:
cloudlog.error(
f"frames out of sync! main: {main_stamp.frame_id} ({main_stamp.timestamp_sof / 1e9:.5f}),"
f" extra: {wide_stamp.frame_id} ({wide_stamp.timestamp_sof / 1e9:.5f})"
)
return main_buf, wide_buf, main_stamp, wide_stamp
class CalibrationAtlas:
def __init__(self):
self.main_warp = np.zeros((3, 3), dtype=np.float32)
self.extra_warp = np.zeros((3, 3), dtype=np.float32)
self.ready = False
self._offset_tuner = CameraOffsetHelper()
def set_offset(self, offset_value: Any) -> None:
self._offset_tuner.set_offset(offset_value)
def refresh(self, sm: SubMaster, main_is_wide: bool, dual_camera: bool) -> tuple[np.ndarray, np.ndarray, bool]:
if not (sm.seen["extrinsicsCalibration"] and sm.seen["roadCameraState"] and sm.seen["deviceState"]):
return self.main_warp, self.extra_warp, self.ready
rpy = get_calibrated_rpy(sm["extrinsicsCalibration"])
if rpy is None:
live_calib = sm["extrinsicsCalibration"]
if len(live_calib.rpyCalib) == 3:
rpy = np.array(live_calib.rpyCalib, dtype=np.float32)
else:
rpy = np.zeros(3, dtype=np.float32)
device_key = (str(sm["deviceState"].deviceType), str(sm["roadCameraState"].sensor))
device_camera = DEVICE_CAMERAS[device_key]
main_intrinsics = device_camera.ecam.intrinsics if main_is_wide else device_camera.fcam.intrinsics
extra_uses_wide_camera = dual_camera or main_is_wide
extra_intrinsics = device_camera.ecam.intrinsics if extra_uses_wide_camera else device_camera.fcam.intrinsics
self.main_warp = get_warp_matrix(rpy, main_intrinsics, False).astype(np.float32)
self.extra_warp = get_warp_matrix(rpy, extra_intrinsics, True).astype(np.float32)
self.main_warp, self.extra_warp = self._offset_tuner.update(
self.main_warp, self.extra_warp, sm, main_is_wide, extra_uses_wide_camera
)
self.ready = True
return self.main_warp, self.extra_warp, self.ready
class FrameDropMeter:
def __init__(self, model_freq: float):
self._smoother = FirstOrderFilter(0.0, 10.0, 1.0 / model_freq)
self._warm_frames = 0
self._last_frame_id = 0
def sample(self, frame_id: int) -> tuple[int, float, bool]:
dropped = max(0, frame_id - self._last_frame_id - 1)
smooth = self._smoother.update(min(dropped, 10))
if self._warm_frames < 10:
self._smoother.x = 0.0
smooth = 0.0
self._warm_frames += 1
return dropped, smooth / (1 + smooth), dropped > 0
def commit(self, frame_id: int) -> None:
self._last_frame_id = frame_id
class InferenceDaemon:
def __init__(self, demo: bool = False, channel_path: str | None = None):
cloudlog.warning("iqmodeld init")
sentry.set_tag("daemon", PROCESS_NAME)
cloudlog.bind(daemon=PROCESS_NAME)
setproctitle(PROCESS_NAME)
config_realtime_process(7, 54)
cloudlog.warning("setting up CL context")
self._gpu = WarpContext()
cloudlog.warning("CL context ready; loading model")
self._runtime = NeuralEngineState(self._gpu)
self._meta_layout = select_meta_layout()
cloudlog.warning("models loaded, iqmodeld starting")
self._channel = None
if channel_path is not None:
from iqpilot.selfdrive.iqmodeld.model_channel import ModelChannel
self._channel = ModelChannel(channel_path, create=True)
self._cameras = CameraIngress(self._gpu)
pub_services = ["iqPerfTrace"] if self._channel is not None else [
"modelV2", "drivingModelData", "cameraOdometry", "iqDriveModelData", "iqPerfTrace"]
self._pub = PubMaster(pub_services)
self._sub = SubMaster([
"deviceState", "carState", "roadCameraState", "extrinsicsCalibration",
"driverMonitoringState", "carControl", "lateralDelay", "iqNavState", "radarState",
])
self._message_memory = DrivePacketMemory()
self._params = Params()
self._frame_meter = FrameDropMeter(self._runtime.constants.MODEL_FREQ)
self._warps = CalibrationAtlas()
self._perf = PerfTraceEmitter("iqmodeld", pubmaster=self._pub)
self._perf_ring = PerfTraceRing()
self._car_params = self._load_car_params(demo)
self._long_action_delay = self._car_params.longitudinalActuatorDelay + self._runtime.LONG_SMOOTH_SECONDS
self._previous_action = log.ModelDataV2.Action()
self._desire_logic = DesireHelper()
self._lat_smooth_extra_sec = 0.0
def _load_car_params(self, demo: bool):
car_params = get_demo_car_params() if demo else messaging.log_from_bytes(
self._params.get("CarParams", block=True), car.CarParams)
cloudlog.info("iqmodeld got CarParams: %s", car_params.brand)
return car_params
def _refresh_tunables(self, tick: int) -> None:
if tick % 60 != 0:
return
from iqpilot.selfdrive.iqmodeld.egpu_helpers import egpu_selected
big_enabled = self._params.get_bool("IQEmacEnabled") or egpu_selected(self._params)
if big_enabled != (self._channel is not None):
# publish mode is fixed at startup: staying up would fight the selector for modelV2
cloudlog.warning("iqmodeld: big backend toggled, restarting to switch publish mode")
sys.exit(0)
self._runtime.lat_delay = lateral_action_delay(self._params, self._car_params, self._sub["lateralDelay"].lateralDelay)
self._runtime.PLANPLUS_CONTROL = self._params.get("PlanplusControl", return_default=True)
self._runtime.model_smoothing_max_extra_sec = _model_lat_smooth_max_sec(self._params)
self._warps.set_offset(self._params.get("CameraOffset", return_default=True))
def _traffic_side(self) -> np.ndarray:
traffic = np.zeros(2, dtype=np.float32)
traffic[int(self._sub["driverMonitoringState"].isRHD)] = 1
return traffic
def _desire_pulse(self) -> np.ndarray:
pulse = np.zeros(self._runtime.constants.DESIRE_LEN, dtype=np.float32)
desire_idx = self._desire_logic.desire
if 0 <= desire_idx < self._runtime.constants.DESIRE_LEN:
pulse[desire_idx] = 1
return pulse
def _compose_inputs(self, vehicle_speed: float, lat_horizon: float, long_horizon: float) -> dict[str, np.ndarray]:
inputs: dict[str, np.ndarray] = {
self._runtime.desire_key: self._desire_pulse(),
"traffic_convention": self._traffic_side(),
}
if "lateral_control_params" in self._runtime.numpy_inputs:
inputs["lateral_control_params"] = np.array([vehicle_speed, lat_horizon], dtype=np.float32)
if "action_t" in self._runtime.numpy_inputs:
inputs["action_t"] = np.array([lat_horizon, long_horizon], dtype=np.float32)
return inputs
def _publish(self, outputs: dict[str, np.ndarray], main_stamp: CaptureStamp, extra_stamp: CaptureStamp,
road_frame_id: int, frame_drop_ratio: float, dropped_frames: int,
execution_time: float, live_calib_seen: bool,
lat_horizon: float, long_horizon: float, vehicle_speed: float) -> None:
model_msg = messaging.new_message("modelV2")
driving_msg = messaging.new_message("drivingModelData")
pose_msg = messaging.new_message("cameraOdometry")
iq_msg = messaging.new_message("iqDriveModelData")
self._lat_smooth_extra_sec = dynamic_lat_smooth_extra_seconds(
_plan_y_std_1s(outputs), self._runtime.model_smoothing_max_extra_sec
)
lat_smooth_total_sec = min(self._runtime.LAT_SMOOTH_SECONDS + self._lat_smooth_extra_sec, MODEL_SMOOTHING_MAX_TOTAL_SEC)
action = self._runtime.get_action_from_model(
outputs, self._previous_action, lat_horizon, long_horizon, vehicle_speed, lat_smooth_total_sec
)
self._previous_action = action
populate_drive_messages(
driving_msg,
model_msg,
outputs,
action,
self._message_memory,
main_stamp.frame_id,
extra_stamp.frame_id,
road_frame_id,
frame_drop_ratio,
main_stamp.timestamp_eof,
execution_time,
live_calib_seen,
self._meta_layout,
)
desire_state = model_msg.modelV2.meta.desireState
lane_change_prob = desire_state[log.Desire.laneChangeLeft] + desire_state[log.Desire.laneChangeRight]
self._desire_logic.update(
self._sub["carState"],
self._sub["carControl"].latActive,
lane_change_prob,
self._sub["iqNavState"],
model_msg.modelV2,
self._sub["radarState"],
)
model_msg.modelV2.meta.laneChangeState = self._desire_logic.lane_change_state
model_msg.modelV2.meta.laneChangeDirection = self._desire_logic.lane_change_direction
driving_msg.drivingModelData.meta.laneChangeState = self._desire_logic.lane_change_state
driving_msg.drivingModelData.meta.laneChangeDirection = self._desire_logic.lane_change_direction
iq_msg.iqDriveModelData.turnSignalDirection = self._desire_logic.lane_turn_direction
iq_msg.iqDriveModelData.lateralEdgeBlock = self._desire_logic.lateral_edge_block
populate_odometry_message(
pose_msg,
outputs,
main_stamp.frame_id,
dropped_frames,
main_stamp.timestamp_eof,
live_calib_seen,
)
if self._channel is not None:
self._channel.write(main_stamp.frame_id, {
"source": "small",
"frame_id": main_stamp.frame_id,
"timestamp_sof": int(main_stamp.timestamp_sof),
"live_calib_seen": bool(live_calib_seen),
"model_execution_time": float(execution_time),
"msgs": {
"modelV2": model_msg.to_bytes(),
"drivingModelData": driving_msg.to_bytes(),
"cameraOdometry": pose_msg.to_bytes(),
"iqDriveModelData": iq_msg.to_bytes(),
},
})
return
self._pub.send("modelV2", model_msg)
self._pub.send("drivingModelData", driving_msg)
self._pub.send("cameraOdometry", pose_msg)
self._pub.send("iqDriveModelData", iq_msg)
def serve(self) -> None:
tick = 0
starved_polls = 0
while True:
frame_pair = self._cameras.pull()
if frame_pair is None:
starved_polls += 1
if starved_polls >= _FRAME_STARVED_BACKOFF_POLLS:
time.sleep(_FRAME_STARVED_BACKOFF_SECONDS)
if starved_polls % _FRAME_STARVED_LOG_EVERY == 0:
cloudlog.error(f"visionipc delivered no frames for {starved_polls} polls; model is not running")
continue
if starved_polls:
cloudlog.warning(f"visionipc recovered after {starved_polls} frameless polls")
starved_polls = 0
main_buf, extra_buf, main_stamp, extra_stamp = frame_pair
self._sub.update(0)
self._refresh_tunables(tick)
vehicle_speed = max(self._sub["carState"].vEgo, 0.0)
lat_horizon = self._runtime.lat_delay + self._runtime.LAT_SMOOTH_SECONDS + self._lat_smooth_extra_sec + DT_MDL
long_horizon = self._long_action_delay + DT_MDL
main_warp, extra_warp, live_calib_seen = self._warps.refresh(
self._sub, self._cameras.layout.main_is_wide, self._cameras.layout.dual_camera
)
dropped_frames, frame_drop_ratio, prepare_only = self._frame_meter.sample(main_stamp.frame_id)
vision_bufs = {
stream_name: extra_buf if "big" in stream_name else main_buf
for stream_name in self._runtime.model_runner.vision_input_names
}
warp_map = {
stream_name: extra_warp if "big" in stream_name else main_warp
for stream_name in self._runtime.model_runner.vision_input_names
}
fresh_inputs = self._compose_inputs(vehicle_speed, lat_horizon, long_horizon)
started_at = time.perf_counter()
outputs = self._runtime.run(vision_bufs, warp_map, fresh_inputs)
execution_time = time.perf_counter() - started_at
execution_us = int(execution_time * 1_000_000)
sample = PerfSample(
frame_id=main_stamp.frame_id,
model_eval_us=execution_us,
model_dropped_frames=dropped_frames,
model_backlog=max(0, dropped_frames),
)
self._perf_ring.push(sample)
if dropped_frames > 0 or execution_us >= IQMODEL_EVAL_WARN_US:
severity = "warning"
if dropped_frames > 0 or execution_us >= IQMODEL_EVAL_ERROR_US:
severity = "error"
self._perf.emit(
"iqmodeld_dropped_frames" if dropped_frames > 0 else "iqmodeld_slow_eval",
severity=severity,
frame_id=main_stamp.frame_id,
total_time_us=execution_us,
dropped_frames=dropped_frames,
backlog=max(0, dropped_frames),
samples=self._perf_ring.snapshot(),
detail=(
f"model_eval_us={execution_us} dropped_frames={dropped_frames} prepare_only={int(prepare_only)} "
f"road_frame_id={self._sub['roadCameraState'].frameId}"
),
min_interval_s=0.25,
)
if outputs is not None:
self._publish(
outputs,
main_stamp,
extra_stamp,
self._sub["roadCameraState"].frameId,
frame_drop_ratio,
dropped_frames,
execution_time,
live_calib_seen,
lat_horizon,
long_horizon,
vehicle_speed,
)
self._frame_meter.commit(main_stamp.frame_id)
tick += 1
def main(demo: bool = False, channel_path: str | None = "auto"):
if channel_path == "auto":
channel_path = None
params = Params()
from iqpilot.selfdrive.iqmodeld.egpu_helpers import egpu_selected
if params.get_bool("IQEmacEnabled") or egpu_selected(params):
from iqpilot.selfdrive.iqmodeld.model_channel import SMALL_CHANNEL
channel_path = SMALL_CHANNEL
InferenceDaemon(demo=demo, channel_path=channel_path).serve()
__all__ = [
"PROCESS_NAME",
"IQP_NAV_MODEL_INFLUENCE_ENABLED",
"TurnDirection",
"CaptureStamp",
"ReplayLedger",
"NeuralEngineState",
"CameraIngress",
"CalibrationAtlas",
"FrameDropMeter",
"InferenceDaemon",
"main",
]
if __name__ == "__main__":
try:
import argparse
parser = argparse.ArgumentParser()
parser.add_argument("--demo", action="store_true", help="Run iqmodeld in demo mode.")
args = parser.parse_args()
main(demo=args.demo)
except KeyboardInterrupt:
cloudlog.warning(f"child {PROCESS_NAME} got SIGINT")
except Exception:
sentry.capture_exception()
raise

46
artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/iqmodeld/driving_action.py Normal file
View File

@@ -0,0 +1,46 @@
"""
Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos/
"""
from __future__ import annotations
import numpy as np
from iqpilot.cereal import log
from iqpilot.selfdrive.controls.lib.drive_helpers import smooth_value
LAT_SMOOTH_SECONDS = 0.0
LONG_SMOOTH_SECONDS = 0.3
MIN_LAT_CONTROL_SPEED = 0.3
DESIRE_LEN = 8
def get_action_from_model(outputs: dict[str, np.ndarray], prev_action: log.ModelDataV2.Action,
v_ego: float, lat_action_t: float, long_action_t: float,
lat_smooth_seconds: float | None = None) -> log.ModelDataV2.Action:
if "action" in outputs:
desired_accel = float(outputs["action"][0, 1])
desired_curvature = float(outputs["action"][0, 0]) / (max(1.0, v_ego)) ** 2
should_stop = bool(v_ego < 0.3 and desired_accel < 0.1)
else:
from iqpilot.selfdrive.controls.lib.drive_helpers import get_accel_from_plan, get_curvature_from_plan
from iqpilot.selfdrive.iqmodeld.config import ModelConstants, Plan
plan = outputs["plan"][0]
desired_accel, should_stop = get_accel_from_plan(plan[:, Plan.VELOCITY][:, 0],
plan[:, Plan.ACCELERATION][:, 0],
ModelConstants.T_IDXS,
action_t=long_action_t)
desired_curvature = get_curvature_from_plan(plan[:, Plan.T_FROM_CURRENT_EULER][:, 2],
plan[:, Plan.ORIENTATION_RATE][:, 2],
ModelConstants.T_IDXS, v_ego, lat_action_t)
desired_accel, should_stop = float(desired_accel), bool(should_stop)
desired_curvature = float(desired_curvature)
desired_accel = smooth_value(desired_accel, prev_action.desiredAcceleration, LONG_SMOOTH_SECONDS)
if v_ego > MIN_LAT_CONTROL_SPEED:
lat_smooth = LAT_SMOOTH_SECONDS if lat_smooth_seconds is None else lat_smooth_seconds
desired_curvature = smooth_value(desired_curvature, prev_action.desiredCurvature, lat_smooth)
else:
desired_curvature = prev_action.desiredCurvature
return log.ModelDataV2.Action(desiredCurvature=float(desired_curvature),
desiredAcceleration=float(desired_accel),
shouldStop=should_stop)

209
artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/iqmodeld/egpu_helpers.py Normal file
View File

@@ -0,0 +1,209 @@
"""
Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos/
"""
from __future__ import annotations
import hashlib
import json
import os
from iqpilot.common.swaglog import cloudlog
import urllib.request
from pathlib import Path
from iqpilot.system.hardware.usb import egpu_dock_ready
USB_SYSFS_ROOT = "/sys/bus/usb/devices"
FIRMWARE_MIRROR = os.getenv("IQ_EGPU_FIRMWARE_MIRROR", "/data/firmware/tinygrad")
TINYGRAD_CACHE = "/data/.cache"
COMMA_LFS_BATCH_URL = "https://gitlab.com/commaai/openpilot-lfs.git/info/lfs/objects/batch"
DOWNLOAD_CHUNK = 4 * 1024 * 1024
def usbgpu_present(sysfs_root: str = USB_SYSFS_ROOT) -> bool:
return egpu_dock_ready(Path(sysfs_root))
def egpu_present_consented(params, sysfs_root: str = USB_SYSFS_ROOT) -> bool:
try:
if params is not None and params.get_bool("IQEgpuDisabled"):
return False
except Exception:
pass
return usbgpu_present(sysfs_root)
def egpu_selected(params, sysfs_root: str = USB_SYSFS_ROOT) -> bool:
try:
if params is not None and params.get_bool("IQEgpuDisabled"):
return False
if params is not None and params.get_bool("IQEgpuEnabled"):
return True
except Exception:
pass
return usbgpu_present(sysfs_root)
def resolve_backend(emac_enabled: bool, egpu_enabled: bool, egpu_present: bool = False) -> str | None:
if egpu_present:
return "egpu"
if emac_enabled:
return "emac"
if egpu_enabled:
return "egpu"
return None
def egpu_pkl_path(meta: dict) -> str:
from iqpilot.system.hardware.hw import Paths
return os.path.join(Paths.model_root(), f"egpu_{meta['key']}_{meta['sha256'][:8]}_amd_tinygrad.pkl")
def egpu_policy_pkl_path(meta: dict) -> str:
from iqpilot.system.hardware.hw import Paths
return os.path.join(Paths.model_root(), f"egpu_{meta['key']}_{meta['sha256'][:8]}_amd_policy.pkl")
def egpu_oob_pkl_path(meta: dict) -> str:
from iqpilot.system.hardware.hw import Paths
return os.path.join(Paths.model_root(), f"egpu_{meta['key']}_{meta['sha256'][:8]}_amd_policy_oob.pkl")
def onnx_cache_path(meta: dict) -> str:
from iqpilot.system.hardware.hw import Paths
return os.path.join(Paths.model_root(), f"{meta['model_name']}_{meta['sha256'][:8]}.onnx")
def _sha256_file(path: str) -> str:
digest = hashlib.sha256()
with open(path, "rb") as f:
while chunk := f.read(DOWNLOAD_CHUNK):
digest.update(chunk)
return digest.hexdigest()
def quarantine_artifact(path: str, why: str) -> None:
try:
if os.path.isfile(path):
os.replace(path, path + ".unusable")
except OSError:
try:
os.remove(path)
except OSError:
pass
def local_onnx(meta: dict) -> str | None:
path = onnx_cache_path(meta)
if not os.path.isfile(path):
return None
size = int(meta.get("download", {}).get("size", 0))
if size and os.path.getsize(path) != size:
quarantine_artifact(path, "onnx size mismatch")
return None
if _sha256_file(path) != meta["sha256"]:
quarantine_artifact(path, "onnx sha256 mismatch")
return None
return path
def resolve_download_url(download_url: str, sha256: str, size: int, timeout: float = 30.0) -> str:
if download_url.startswith("commalfs:"):
oid = download_url.split(":", 1)[1]
body = json.dumps({"operation": "download", "transfers": ["basic"],
"objects": [{"oid": oid, "size": size}]}).encode()
req = urllib.request.Request(COMMA_LFS_BATCH_URL, data=body, headers={
"Accept": "application/vnd.git-lfs+json", "Content-Type": "application/vnd.git-lfs+json"})
with urllib.request.urlopen(req, timeout=timeout) as r:
d = json.load(r)
return d["objects"][0]["actions"]["download"]["href"]
return download_url
def download_onnx(meta: dict, progress_cb=None) -> str:
from iqpilot.selfdrive.iqmodeld.egpu_model import download_descriptor
download_url, size = download_descriptor(meta)
if not download_url:
raise RuntimeError(f"model {meta['key']} has no download source; stage the onnx at {onnx_cache_path(meta)}")
path = onnx_cache_path(meta)
os.makedirs(os.path.dirname(path), exist_ok=True)
try:
from iqpilot.selfdrive.iqmodeld.model_bundle_downloader import download_hf_file
return download_hf_file(f"onnx/{meta['sha256']}.onnx", path, meta["sha256"], int(size or 0), progress_cb=progress_cb)
except Exception as e:
cloudlog.warning(f"onnx {meta['key']} unavailable from HF ({e}); falling back to {download_url.split(':', 1)[0]}")
url = resolve_download_url(download_url, meta["sha256"], size)
tmp = path + ".part"
digest = hashlib.sha256()
got = 0
with urllib.request.urlopen(url, timeout=60) as r, open(tmp, "wb") as f:
while chunk := r.read(DOWNLOAD_CHUNK):
f.write(chunk)
digest.update(chunk)
got += len(chunk)
if progress_cb is not None and size:
progress_cb(got / size)
if size and got != size:
os.remove(tmp)
raise RuntimeError(f"onnx download truncated: {got}/{size} bytes")
if digest.hexdigest() != meta["sha256"]:
os.remove(tmp)
raise RuntimeError(f"onnx sha256 mismatch for {meta['key']}")
os.replace(tmp, path)
return path
def download_precompiled(meta: dict, progress_cb=None, policy: bool = False, oob: bool = False) -> str | None:
field = "egpu_oob_artifact" if oob else "egpu_policy_artifact" if policy else "egpu_artifact"
art = meta.get(field)
if not art or not (art.get("objects") or art.get("hf_path")):
return None
from iqpilot.selfdrive.iqmodeld.model_bundle_downloader import download_hf_file, download_lfs_bundle
dest = egpu_oob_pkl_path(meta) if oob else egpu_policy_pkl_path(meta) if policy else egpu_pkl_path(meta)
if art.get("hf_path"):
try:
return download_hf_file(art["hf_path"], dest, art["sha256"], int(art.get("size", 0)), progress_cb=progress_cb)
except Exception as e:
cloudlog.warning(f"precompiled {meta['key']} unavailable from HF ({e}); trying LFS")
if not art.get("objects"):
raise
return download_lfs_bundle(art["objects"], dest, art["sha256"], int(art.get("size", 0)), progress_cb=progress_cb)
def patch_tinygrad_fetch_fw() -> None:
import pathlib
import zstandard
from tinygrad import helpers
if getattr(helpers.fetch_fw, "_iq_patched", False):
return
_orig = helpers.fetch_fw
def fetch_fw(path, name, sha256):
mirror = pathlib.Path(FIRMWARE_MIRROR) / path / name
if mirror.is_file():
blob = mirror.read_bytes()
if hashlib.sha256(blob).hexdigest() == sha256:
return blob
p = pathlib.Path(f"/lib/firmware/{path}/{name}.zst")
if p.is_file():
blob = zstandard.ZstdDecompressor().stream_reader(p.read_bytes()).read()
if hashlib.sha256(blob).hexdigest() == sha256:
return blob
blob = _orig(path, name, sha256)
# The dock's GPU firmware otherwise lives only in tinygrad's per-user download cache, which is
# a network fetch the first time a new HOME sees it; onroad the car is usually offline.
try:
mirror.parent.mkdir(parents=True, exist_ok=True)
tmp = mirror.with_suffix(mirror.suffix + ".part")
tmp.write_bytes(blob)
os.replace(tmp, mirror)
except OSError:
pass
return blob
fetch_fw._iq_patched = True
helpers.fetch_fw = fetch_fw

9
artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/iqmodeld/egpu_model.py Normal file
View File

@@ -0,0 +1,9 @@
"""
Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos/
"""
from iqpilot._proprietary_loader import ProprietaryModuleMissing, load_private_module
try:
load_private_module(__name__, "iqpilot_private.models.egpu_model")
except ProprietaryModuleMissing:
from iqpilot.models_private_src.egpu_model import *

9
artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/iqmodeld/emac_model_meta.py Normal file
View File

@@ -0,0 +1,9 @@
"""
Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos/
"""
from iqpilot._proprietary_loader import ProprietaryModuleMissing, load_private_module
try:
load_private_module(__name__, "iqpilot_private.models.emac_model_meta")
except ProprietaryModuleMissing:
from iqpilot.models_private_src.emac_model_meta import *

269
artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/iqmodeld/messaging.py Normal file
View File

@@ -0,0 +1,269 @@
from __future__ import annotations
import os
from dataclasses import dataclass, field
import capnp
import numpy as np
from iqpilot.cereal import log
from iqpilot.selfdrive.iqmodeld.models.helpers import plan_x_idxs_helper
from iqpilot.selfdrive.iqmodeld.config import ModelConstants, Plan
from iqpilot.selfdrive.controls.lib.drive_helpers import get_curvature_from_plan
SEND_RAW_PRED = os.getenv("SEND_RAW_PRED")
ConfidenceClass = log.ModelDataV2.ConfidenceClass
def pick_curvature(outputs: dict[str, np.ndarray], plan_rows: np.ndarray, vehicle_speed: float,
action_horizon: float, synthetic_lane_logic: bool) -> float:
direct_signal = None if synthetic_lane_logic else outputs.get("desired_curvature")
if direct_signal is not None:
return float(direct_signal[0, 0])
yaw_track = plan_rows[:, Plan.T_FROM_CURRENT_EULER][:, 2]
yaw_rate_track = plan_rows[:, Plan.ORIENTATION_RATE][:, 2]
return float(get_curvature_from_plan(yaw_track, yaw_rate_track, ModelConstants.T_IDXS, vehicle_speed, action_horizon))
@dataclass
class DrivePacketMemory:
disengage_rollup: np.ndarray = field(default_factory=lambda: np.zeros(
ModelConstants.CONFIDENCE_BUFFER_LEN * ModelConstants.DISENGAGE_WIDTH, dtype=np.float32))
brake_watch_5: np.ndarray = field(default_factory=lambda: np.zeros(
ModelConstants.FCW_5MS2_PROBS_WIDTH, dtype=np.float32))
brake_watch_3: np.ndarray = field(default_factory=lambda: np.zeros(
ModelConstants.FCW_3MS2_PROBS_WIDTH, dtype=np.float32))
def _assign_xyz(builder, t_points, x_track, y_track, z_track,
x_std=None, y_std=None, z_std=None) -> None:
builder.t = t_points
builder.x = x_track.tolist()
builder.y = y_track.tolist()
builder.z = z_track.tolist()
if x_std is not None:
builder.xStd = x_std.tolist()
if y_std is not None:
builder.yStd = y_std.tolist()
if z_std is not None:
builder.zStd = z_std.tolist()
def _assign_xyva(builder, t_points, x_track, y_track, v_track, a_track,
x_std=None, y_std=None, v_std=None, a_std=None) -> None:
builder.t = t_points
builder.x = x_track.tolist()
builder.y = y_track.tolist()
builder.v = v_track.tolist()
builder.a = a_track.tolist()
if x_std is not None:
builder.xStd = x_std.tolist()
if y_std is not None:
builder.yStd = y_std.tolist()
if v_std is not None:
builder.vStd = v_std.tolist()
if a_std is not None:
builder.aStd = a_std.tolist()
def fill_xyz_poly(builder, degree: int, x_track: np.ndarray, y_track: np.ndarray, z_track: np.ndarray) -> None:
stacked = np.stack([x_track, y_track, z_track], axis=1)
coeffs = np.polynomial.polynomial.polyfit(ModelConstants.T_IDXS, stacked, deg=degree)
builder.xCoefficients = coeffs[:, 0].tolist()
builder.yCoefficients = coeffs[:, 1].tolist()
builder.zCoefficients = coeffs[:, 2].tolist()
def fill_lane_line_meta(builder, lane_lines, lane_probs: list[float]) -> None:
builder.leftY = lane_lines[1].y[0]
builder.leftProb = lane_probs[1]
builder.rightY = lane_lines[2].y[0]
builder.rightProb = lane_probs[2]
def _roll_brake_watch(outputs: dict[str, np.ndarray], memory: DrivePacketMemory, meta_layout) -> bool:
memory.brake_watch_5[:-1] = memory.brake_watch_5[1:]
memory.brake_watch_5[-1] = outputs["meta"][0, meta_layout.HARD_BRAKE_5][0]
memory.brake_watch_3[:-1] = memory.brake_watch_3[1:]
memory.brake_watch_3[-1] = outputs["meta"][0, meta_layout.HARD_BRAKE_3][0]
return bool(
(memory.brake_watch_5 > ModelConstants.FCW_THRESHOLDS_5MS2).all()
and (memory.brake_watch_3 > ModelConstants.FCW_THRESHOLDS_3MS2).all()
)
def _confidence_bucket(outputs: dict[str, np.ndarray], memory: DrivePacketMemory, meta_layout, frame_id: int):
width = ModelConstants.DISENGAGE_WIDTH
if frame_id % (2 * ModelConstants.MODEL_FREQ) == 0:
brake_probs = outputs["meta"][0, meta_layout.BRAKE_DISENGAGE]
gas_probs = outputs["meta"][0, meta_layout.GAS_DISENGAGE]
steer_probs = outputs["meta"][0, meta_layout.STEER_OVERRIDE]
takeover_curve = 1 - ((1 - brake_probs) * (1 - gas_probs) * (1 - steer_probs))
independent = np.r_[takeover_curve[0], np.diff(takeover_curve) / (1 - takeover_curve[:-1])]
memory.disengage_rollup[:-width] = memory.disengage_rollup[width:]
memory.disengage_rollup[-width:] = independent
score = 0.0
for idx in range(width):
score += memory.disengage_rollup[idx * width + width - 1 - idx].item() / width
if score < ModelConstants.RYG_GREEN:
return ConfidenceClass.green
if score < ModelConstants.RYG_YELLOW:
return ConfidenceClass.yellow
return ConfidenceClass.red
def _write_plan_family(model_packet, driving_packet, outputs: dict[str, np.ndarray]) -> None:
plan_rows = outputs["plan"][0]
plan_stds = outputs["plan_stds"][0]
_assign_xyz(model_packet.position, ModelConstants.T_IDXS, *plan_rows[:, Plan.POSITION].T, *plan_stds[:, Plan.POSITION].T)
_assign_xyz(model_packet.velocity, ModelConstants.T_IDXS, *plan_rows[:, Plan.VELOCITY].T)
_assign_xyz(model_packet.acceleration, ModelConstants.T_IDXS, *plan_rows[:, Plan.ACCELERATION].T)
_assign_xyz(model_packet.orientation, ModelConstants.T_IDXS, *plan_rows[:, Plan.T_FROM_CURRENT_EULER].T)
_assign_xyz(model_packet.orientationRate, ModelConstants.T_IDXS, *plan_rows[:, Plan.ORIENTATION_RATE].T)
fill_xyz_poly(driving_packet.path, ModelConstants.POLY_PATH_DEGREE, *plan_rows[:, Plan.POSITION].T)
def _write_temporal_pose(model_packet, outputs: dict[str, np.ndarray]) -> None:
pose_packet = model_packet.temporalPoseDEPRECATED
if "sim_pose" in outputs:
half_width = ModelConstants.POSE_WIDTH // 2
pose_packet.trans = outputs["sim_pose"][0, :half_width].tolist()
pose_packet.transStd = outputs["sim_pose_stds"][0, :half_width].tolist()
pose_packet.rot = outputs["sim_pose"][0, half_width:].tolist()
pose_packet.rotStd = outputs["sim_pose_stds"][0, half_width:].tolist()
return
pose_packet.trans = outputs["plan"][0, 0, Plan.VELOCITY].tolist()
pose_packet.transStd = outputs["plan_stds"][0, 0, Plan.VELOCITY].tolist()
pose_packet.rot = outputs["plan"][0, 0, Plan.ORIENTATION_RATE].tolist()
pose_packet.rotStd = outputs["plan_stds"][0, 0, Plan.ORIENTATION_RATE].tolist()
def _write_lane_family(model_packet, driving_packet, outputs: dict[str, np.ndarray]) -> None:
time_axis = plan_x_idxs_helper(ModelConstants, Plan, outputs)
model_packet.init("laneLines", 4)
for lane_idx in range(4):
lane_builder = model_packet.laneLines[lane_idx]
_assign_xyz(
lane_builder,
time_axis,
np.array(ModelConstants.X_IDXS),
outputs["lane_lines"][0, lane_idx, :, 0],
outputs["lane_lines"][0, lane_idx, :, 1],
)
model_packet.laneLineStds = outputs["lane_lines_stds"][0, :, 0, 0].tolist()
model_packet.laneLineProbs = outputs["lane_lines_prob"][0, 1::2].tolist()
fill_lane_line_meta(driving_packet.laneLineMeta, model_packet.laneLines, model_packet.laneLineProbs)
model_packet.init("roadEdges", 2)
for edge_idx in range(2):
edge_builder = model_packet.roadEdges[edge_idx]
_assign_xyz(
edge_builder,
time_axis,
np.array(ModelConstants.X_IDXS),
outputs["road_edges"][0, edge_idx, :, 0],
outputs["road_edges"][0, edge_idx, :, 1],
)
model_packet.roadEdgeStds = outputs["road_edges_stds"][0, :, 0, 0].tolist()
def _write_leads(model_packet, outputs: dict[str, np.ndarray]) -> None:
model_packet.init("leadsV3", 3)
for lead_idx in range(3):
lead_builder = model_packet.leadsV3[lead_idx]
_assign_xyva(
lead_builder,
ModelConstants.LEAD_T_IDXS,
*outputs["lead"][0, lead_idx].T,
*outputs["lead_stds"][0, lead_idx].T,
)
lead_builder.prob = outputs["lead_prob"][0, lead_idx].tolist()
lead_builder.probTime = ModelConstants.LEAD_T_OFFSETS[lead_idx]
def _write_meta(model_packet, outputs: dict[str, np.ndarray], memory: DrivePacketMemory, meta_layout, frame_id: int) -> None:
meta = model_packet.meta
meta.desireState = outputs["desire_state"][0].reshape(-1).tolist()
meta.desirePrediction = outputs["desire_pred"][0].reshape(-1).tolist()
meta.engagedProb = outputs["meta"][0, meta_layout.ENGAGED].item()
meta.init("disengagePredictions")
pred = meta.disengagePredictions
pred.t = ModelConstants.META_T_IDXS
pred.brakeDisengageProbs = outputs["meta"][0, meta_layout.BRAKE_DISENGAGE].tolist()
pred.gasDisengageProbs = outputs["meta"][0, meta_layout.GAS_DISENGAGE].tolist()
pred.steerOverrideProbs = outputs["meta"][0, meta_layout.STEER_OVERRIDE].tolist()
pred.brake3MetersPerSecondSquaredProbs = outputs["meta"][0, meta_layout.HARD_BRAKE_3].tolist()
pred.brake4MetersPerSecondSquaredProbs = outputs["meta"][0, meta_layout.HARD_BRAKE_4].tolist()
pred.brake5MetersPerSecondSquaredProbs = outputs["meta"][0, meta_layout.HARD_BRAKE_5].tolist()
if hasattr(meta_layout, "GAS_PRESS") and hasattr(meta_layout, "BRAKE_PRESS"):
pred.gasPressProbs = outputs["meta"][0, meta_layout.GAS_PRESS].tolist()
pred.brakePressProbs = outputs["meta"][0, meta_layout.BRAKE_PRESS].tolist()
meta.hardBrakePredicted = _roll_brake_watch(outputs, memory, meta_layout)
model_packet.confidence = _confidence_bucket(outputs, memory, meta_layout, frame_id)
def populate_drive_messages(primary_msg: capnp._DynamicStructBuilder, extended_msg: capnp._DynamicStructBuilder,
outputs: dict[str, np.ndarray], action: log.ModelDataV2.Action,
memory: DrivePacketMemory, vipc_frame_id: int, vipc_frame_id_extra: int,
frame_id: int, frame_drop: float, timestamp_eof: int,
model_execution_time: float, valid: bool, meta_layout) -> None:
frame_age = frame_id - vipc_frame_id if frame_id > vipc_frame_id else 0
frame_drop_percent = frame_drop * 100
primary_msg.valid = valid
extended_msg.valid = valid
driving_packet = primary_msg.drivingModelData
driving_packet.frameId = vipc_frame_id
driving_packet.frameIdExtra = vipc_frame_id_extra
driving_packet.frameDropPerc = frame_drop_percent
driving_packet.modelExecutionTime = model_execution_time
driving_packet.action = action
model_packet = extended_msg.modelV2
model_packet.frameId = vipc_frame_id
model_packet.frameIdExtra = vipc_frame_id_extra
model_packet.frameAge = frame_age
model_packet.frameDropPerc = frame_drop_percent
model_packet.timestampEof = timestamp_eof
model_packet.modelExecutionTime = model_execution_time
model_packet.action = action
_write_plan_family(model_packet, driving_packet, outputs)
_write_temporal_pose(model_packet, outputs)
_write_lane_family(model_packet, driving_packet, outputs)
_write_leads(model_packet, outputs)
_write_meta(model_packet, outputs, memory, meta_layout, vipc_frame_id)
if SEND_RAW_PRED:
model_packet.rawPredictions = outputs["raw_pred"].tobytes()
def populate_odometry_message(msg: capnp._DynamicStructBuilder, outputs: dict[str, np.ndarray],
vipc_frame_id: int, vipc_dropped_frames: int,
timestamp_eof: int, live_calib_seen: bool) -> None:
msg.valid = live_calib_seen & (vipc_dropped_frames < 1)
odo = msg.cameraOdometry
odo.frameId = vipc_frame_id
odo.timestampEof = timestamp_eof
odo.trans = outputs["pose"][0, :3].tolist()
odo.rot = outputs["pose"][0, 3:].tolist()
odo.wideFromDeviceEuler = outputs["wide_from_device_euler"][0, :].tolist()
odo.roadTransformTrans = outputs["road_transform"][0, :3].tolist()
odo.transStd = outputs["pose_stds"][0, :3].tolist()
odo.rotStd = outputs["pose_stds"][0, 3:].tolist()
odo.wideFromDeviceEulerStd = outputs["wide_from_device_euler_stds"][0, :].tolist()
odo.roadTransformTransStd = outputs["road_transform_stds"][0, :3].tolist()
__all__ = [
"DrivePacketMemory",
"pick_curvature",
"populate_drive_messages",
"populate_odometry_message",
]

96
artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/iqmodeld/metadata.py Normal file
View File

@@ -0,0 +1,96 @@
#!/usr/bin/env python3
import codecs
import pathlib
import pickle
import sys
from collections.abc import Iterable
from typing import Any
from iqpilot.cereal import custom
from tinygrad.nn.onnx import OnnxPBParser
from iqpilot.selfdrive.iqmodeld.models.helpers import get_active_bundle
from iqpilot.selfdrive.iqmodeld.config import Meta
ModelBundle = custom.IQModelManager.ModelBundle
def _blank_proto_doc() -> dict[str, Any]:
return {"graph": {"input": [], "output": []}, "metadata_props": []}
class TelemetryEnvelopeParser(OnnxPBParser):
def _parse_ModelProto(self) -> dict:
envelope = _blank_proto_doc()
for fid, wire_type in self._parse_message(self.reader.len):
if fid == 7:
envelope["graph"] = self._parse_GraphProto()
elif fid == 14:
envelope["metadata_props"].append(self._parse_StringStringEntryProto())
else:
self.reader.skip_field(wire_type)
return envelope
def _shape_fingerprint(value_info: dict[str, Any]) -> tuple[str, tuple[int, ...]]:
resolved = []
for axis in value_info["parsed_type"].shape:
resolved.append(int(axis) if isinstance(axis, int) else 0)
return value_info["name"], tuple(resolved)
def _lookup_metadata(props: Iterable[dict[str, Any]], wanted_key: str) -> str | Any:
for entry in props:
if entry["key"] == wanted_key:
return entry["value"]
return None
class Meta20hz(Meta):
ENGAGED = slice(0, 1)
GAS_DISENGAGE = slice(1, 31, 6)
BRAKE_DISENGAGE = slice(2, 31, 6)
STEER_OVERRIDE = slice(3, 31, 6)
HARD_BRAKE_3 = slice(4, 31, 6)
HARD_BRAKE_4 = slice(5, 31, 6)
HARD_BRAKE_5 = slice(6, 31, 6)
GAS_PRESS = slice(31, 55, 4)
BRAKE_PRESS = slice(32, 55, 4)
LEFT_BLINKER = slice(33, 55, 4)
RIGHT_BLINKER = slice(34, 55, 4)
def select_meta_layout():
active_bundle = get_active_bundle()
return Meta20hz if active_bundle is not None and active_bundle.is20hz else Meta
def _decoded_slices(props: Iterable[dict[str, Any]]):
encoded = _lookup_metadata(props, "output_slices")
assert encoded is not None, "output_slices not found in metadata"
return pickle.loads(codecs.decode(encoded.encode(), "base64"))
def _graph_shape_table(graph_doc: dict[str, Any], field_name: str) -> dict[str, tuple[int, ...]]:
return dict(_shape_fingerprint(item) for item in graph_doc[field_name])
def build_metadata_record(model_path):
parsed = TelemetryEnvelopeParser(model_path).parse()
props = parsed["metadata_props"]
graph = parsed["graph"]
return {
"model_checkpoint": _lookup_metadata(props, "model_checkpoint"),
"output_slices": _decoded_slices(props),
"input_shapes": _graph_shape_table(graph, "input"),
"output_shapes": _graph_shape_table(graph, "output"),
}
if __name__ == "__main__":
model_path = pathlib.Path(sys.argv[1])
metadata_path = model_path.parent / f"{model_path.stem}_metadata.pkl"
with open(metadata_path, "wb") as handle:
pickle.dump(build_metadata_record(model_path), handle)
print(f"saved metadata to {metadata_path}")

211
artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/iqmodeld/model_bundle_downloader.py Normal file
View File

@@ -0,0 +1,211 @@
"""
Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos/
"""
from __future__ import annotations
import hashlib
import json
import os
MODELS_BASE_URLS = (
"https://git.konn3kt.com/teal/IQModels/raw/branch/main",
"https://gitlvb.teallvbs.xyz/teal/IQModels/raw/branch/main",
)
CHUNK = 4 * 1024 * 1024
HTTP_TIMEOUT_S = 60.0
STREAM_RETRIES = 6
def _requests_auth():
import importlib
for mod in ("iqpilot_private.models.git_auth", "iqpilot.models_private_src.git_auth",
"iqpilot.selfdrive.iqmodeld.models.git_auth"):
try:
return importlib.import_module(mod).get_requests_auth()
except Exception:
continue
return None
def _hf():
import importlib
for mod in ("iqpilot_private.models.git_auth", "iqpilot.selfdrive.iqmodeld.models.git_auth"):
try:
m = importlib.import_module(mod)
return m.get_hf_headers(), m.hf_resolve_url
except Exception:
continue
return None, None
def download_hf_file(hf_path: str, dst: str, sha256: str, size: int, progress_cb=None) -> str:
import requests
headers, resolve = _hf()
if resolve is None:
raise RuntimeError("no HF credentials available")
url = resolve(hf_path)
os.makedirs(os.path.dirname(dst), exist_ok=True)
tmp = dst + ".hfpart"
last_error: Exception | None = None
for _attempt in range(STREAM_RETRIES):
try:
have = os.path.getsize(tmp) if os.path.isfile(tmp) else 0
if size and have > size:
os.remove(tmp)
have = 0
if not size or have < size:
req_headers = dict(headers)
if have:
req_headers["Range"] = f"bytes={have}-"
with requests.get(url, headers=req_headers, stream=True, timeout=HTTP_TIMEOUT_S, allow_redirects=True) as r:
r.raise_for_status()
if have and r.status_code != 206:
have = 0
with open(tmp, "ab" if have else "wb") as f:
got = have
for chunk in r.iter_content(CHUNK):
f.write(chunk)
got += len(chunk)
if progress_cb is not None and size:
progress_cb(min(1.0, got / size))
digest = hashlib.sha256()
with open(tmp, "rb") as f:
for chunk in iter(lambda: f.read(CHUNK), b""):
digest.update(chunk)
if size and os.path.getsize(tmp) != size:
raise RuntimeError(f"size mismatch: {os.path.getsize(tmp)}/{size} bytes")
if sha256 and digest.hexdigest() != sha256:
os.remove(tmp)
raise RuntimeError("sha256 mismatch")
os.replace(tmp, dst)
return dst
except Exception as e:
last_error = e
raise RuntimeError(f"HF download failed: {last_error}")
def _lfs_endpoint(base_url: str) -> str:
return base_url.split("/raw/", 1)[0] + ".git/info/lfs"
def _resolve_oid(session, base_url: str, oid: str, size: int, auth):
import requests
batch = session.post(f"{_lfs_endpoint(base_url)}/objects/batch",
data=json.dumps({"operation": "download", "transfers": ["basic"],
"objects": [{"oid": oid, "size": size}]}),
headers={"Content-Type": "application/vnd.git-lfs+json",
"Accept": "application/vnd.git-lfs+json"},
auth=auth, timeout=HTTP_TIMEOUT_S)
batch.raise_for_status()
entry = batch.json()["objects"][0]
if "actions" not in entry:
raise requests.RequestException(f"LFS object unavailable: {entry.get('error', oid)}")
action = entry["actions"]["download"]
return action["href"], action.get("header", {})
def _part_path(dst: str, oid: str) -> str:
return os.path.join(dst + ".parts", oid)
def _part_complete(path: str, oid: str, size: int) -> bool:
if not os.path.isfile(path) or os.path.getsize(path) != size:
return False
digest = hashlib.sha256()
with open(path, "rb") as f:
for chunk in iter(lambda: f.read(CHUNK), b""):
digest.update(chunk)
return digest.hexdigest() == oid
def _fetch_part(session, base_url: str, obj: dict, path: str, auth, progress) -> None:
size = int(obj["size"])
have = os.path.getsize(path) if os.path.isfile(path) else 0
if have > size:
os.remove(path)
have = 0
href, headers = _resolve_oid(session, base_url, obj["oid"], size, auth)
obj_auth = None if headers.get("Authorization") else auth
# LFS parts are content-addressed (oid == sha256), so a half-written part can be resumed with a
# Range request and verified afterwards instead of being thrown away on every restart.
if have:
headers = {**headers, "Range": f"bytes={have}-"}
with session.get(href, headers=headers, stream=True, timeout=HTTP_TIMEOUT_S, auth=obj_auth) as r:
r.raise_for_status()
if have and r.status_code != 206:
have = 0
with open(path, "ab" if have else "wb") as f:
for chunk in r.iter_content(CHUNK):
f.write(chunk)
progress(len(chunk))
def download_lfs_bundle(objects: list, dst: str, sha256: str, size: int, progress_cb=None) -> str:
import requests
auth = _requests_auth()
session = requests.Session()
os.makedirs(dst + ".parts", exist_ok=True)
total = int(size) or sum(int(o["size"]) for o in objects)
done_bytes = sum(int(o["size"]) for o in objects if _part_complete(_part_path(dst, o["oid"]), o["oid"], int(o["size"])))
got = [done_bytes]
def progress(n: int) -> None:
got[0] += n
if progress_cb is not None and total:
progress_cb(min(1.0, got[0] / total))
last_error: Exception | None = None
for base_url in MODELS_BASE_URLS:
for _attempt in range(STREAM_RETRIES):
try:
for obj in objects:
path = _part_path(dst, obj["oid"])
if _part_complete(path, obj["oid"], int(obj["size"])):
continue
got[0] = done_bytes
_fetch_part(session, base_url, obj, path, auth, progress)
if not _part_complete(path, obj["oid"], int(obj["size"])):
if os.path.getsize(path) >= int(obj["size"]):
os.remove(path)
raise RuntimeError(f"part {obj['oid'][:12]} incomplete or failed verification")
done_bytes += int(obj["size"])
got[0] = done_bytes
break
except Exception as e:
last_error = e
else:
continue
break
else:
raise RuntimeError(f"model bundle download failed: {last_error}")
tmp = dst + ".part"
digest = hashlib.sha256()
with open(tmp, "wb") as out:
for obj in objects:
with open(_part_path(dst, obj["oid"]), "rb") as f:
for chunk in iter(lambda: f.read(CHUNK), b""):
out.write(chunk)
digest.update(chunk)
if total and os.path.getsize(tmp) != total:
os.remove(tmp)
raise RuntimeError(f"size mismatch: {os.path.getsize(tmp) if os.path.exists(tmp) else 0}/{total} bytes")
if sha256 and digest.hexdigest() != sha256:
os.remove(tmp)
for obj in objects:
try:
os.remove(_part_path(dst, obj["oid"]))
except OSError:
pass
raise RuntimeError("sha256 mismatch")
os.replace(tmp, dst)
for obj in objects:
try:
os.remove(_part_path(dst, obj["oid"]))
except OSError:
pass
try:
os.rmdir(dst + ".parts")
except OSError:
pass
return dst

58
artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/iqmodeld/model_channel.py Normal file
View File

@@ -0,0 +1,58 @@
"""
Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos/
"""
from __future__ import annotations
import mmap
import os
import pickle
import struct
from iqpilot.common.swaglog import cloudlog
SMALL_CHANNEL = "/dev/shm/iqpilot_smallmodel"
BIG_CHANNEL = "/dev/shm/iqpilot_bigmodel"
SHM_SIZE = 8 * 1024 * 1024
HEADER = struct.Struct("<QqQ")
class ModelChannel:
def __init__(self, path: str, create: bool):
if create:
fd = os.open(path, os.O_CREAT | os.O_RDWR, 0o600)
os.ftruncate(fd, SHM_SIZE)
else:
fd = os.open(path, os.O_RDWR)
self.mm = mmap.mmap(fd, SHM_SIZE)
os.close(fd)
if create:
self.mm[:HEADER.size] = HEADER.pack(0, -1, 0)
def write(self, frame_id: int, payload: dict) -> None:
data = pickle.dumps(payload, protocol=pickle.HIGHEST_PROTOCOL)
if HEADER.size + len(data) > SHM_SIZE:
cloudlog.error(f"model payload {len(data)} bytes exceeds shm {SHM_SIZE}, dropping frame {frame_id}")
return
seq = HEADER.unpack(self.mm[:HEADER.size])[0]
HEADER.pack_into(self.mm, 0, seq + 1, frame_id, len(data))
self.mm[HEADER.size:HEADER.size + len(data)] = data
HEADER.pack_into(self.mm, 0, seq + 2, frame_id, len(data))
def peek_frame_id(self) -> int | None:
seq, frame_id, length = HEADER.unpack(self.mm[:HEADER.size])
if seq == 0 or seq % 2 != 0 or length == 0:
return None
return frame_id
def read(self) -> tuple[int, dict] | None:
seq1, frame_id, length = HEADER.unpack(self.mm[:HEADER.size])
if seq1 == 0 or seq1 % 2 != 0 or length == 0:
return None
data = bytes(self.mm[HEADER.size:HEADER.size + length])
seq2 = HEADER.unpack(self.mm[:HEADER.size])[0]
if seq1 != seq2:
return None
try:
return frame_id, pickle.loads(data)
except Exception:
return None

80
artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/iqmodeld/model_warp.py Normal file
View File

@@ -0,0 +1,80 @@
"""
Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos/
"""
from __future__ import annotations
import os
import pickle
import numpy as np
from iqpilot.common.swaglog import cloudlog
from iqpilot.system.hardware.hw import Paths
def _load_bundle(pkl_path: str, cam_w: int, cam_h: int, frame_skip: int) -> dict:
with open(pkl_path, "rb") as f:
bundle = pickle.load(f)
if bundle.get("frame_skip") != frame_skip:
raise RuntimeError(f"frame_skip {bundle.get('frame_skip')} != {frame_skip}")
if (cam_w, cam_h) not in bundle:
raise RuntimeError(f"missing {cam_w}x{cam_h}; has {[k for k in bundle if isinstance(k, tuple)]}")
_verify_selftest(bundle, cam_w, cam_h)
return bundle
def _verify_selftest(bundle: dict, cam_w: int, cam_h: int) -> None:
want = bundle.get("selftest")
if not want:
raise RuntimeError("warp artifact predates the self-test; recompiling")
from iqpilot.system.camerad.cameras.nv12_info import get_nv12_info
from iqpilot.selfdrive.iqmodeld.tools.compile_warp import selftest_digest
nv12_size = get_nv12_info(cam_w, cam_h)[3]
got = selftest_digest(bundle[(cam_w, cam_h)], cam_w, cam_h, nv12_size)
if got != want:
raise RuntimeError(f"warp self-test {got[:12]} != {want[:12]}; artifact computes differently here")
class FrameWarp:
def __init__(self, cam_w: int, cam_h: int, frame_skip: int):
from tinygrad.tensor import Tensor
pkl_path = os.path.join(Paths.model_root(), f"emac_warp_{cam_w}x{cam_h}_tinygrad.pkl")
bundle = None
if os.path.isfile(pkl_path):
try:
bundle = _load_bundle(pkl_path, cam_w, cam_h, frame_skip)
except Exception as e:
cloudlog.warning(f"warp artifact unusable ({e}); discarding and recompiling")
os.remove(pkl_path)
if bundle is None:
cloudlog.warning(f"warp artifact missing; compiling for {cam_w}x{cam_h} (one-time)")
from iqpilot.selfdrive.iqmodeld.tools.compile_warp import compile_warp
compile_warp(cam_w, cam_h, pkl_path, frame_skip=frame_skip)
cloudlog.warning(f"warp compiled -> {pkl_path}")
bundle = _load_bundle(pkl_path, cam_w, cam_h, frame_skip)
self._jit = bundle[(cam_w, cam_h)]
self._npy = {"tfm": np.zeros((3, 3), dtype=np.float32), "big_tfm": np.zeros((3, 3), dtype=np.float32)}
self._tensors = {k: Tensor(v, device="NPY").realize() for k, v in self._npy.items()}
self._blob_cache: dict[tuple[str, int], object] = {}
self._Tensor = Tensor
def _frame_tensor(self, key: str, buf):
from tinygrad.device import Device
arr = np.frombuffer(buf.data, dtype=np.uint8)
ck = (key, arr.ctypes.data)
t = self._blob_cache.get(ck)
if t is None:
t = self._Tensor.from_blob(arr.ctypes.data, (arr.size,), dtype="uint8", device=Device.DEFAULT)
self._blob_cache[ck] = t
return t
def run(self, main_buf, extra_buf, main_tfm: np.ndarray, extra_tfm: np.ndarray) -> np.ndarray:
self._npy["tfm"][:] = main_tfm
self._npy["big_tfm"][:] = extra_tfm
warped = self._jit(tfm=self._tensors["tfm"], big_tfm=self._tensors["big_tfm"],
frame=self._frame_tensor("img", main_buf),
big_frame=self._frame_tensor("big_img", extra_buf))
return warped.numpy().astype(np.uint8, copy=False)

0
selfdrive/ui/mici/layouts/settings/network/__init__.py → artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/iqmodeld/models/__init__.py
View File

100
artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/iqmodeld/models/combined_artifact.py Normal file
View File

@@ -0,0 +1,100 @@
"""
Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos/
"""
from __future__ import annotations
import os
import re
from pathlib import Path
from iqpilot.system.hardware.hw import Paths
_MODEL_ROOT = Path(Paths.model_root())
_OVERRIDE_KEYS = (
"combinedRuntimeArtifact",
"combinedSplitArtifact",
"iqCombinedArtifact",
)
_SPLIT_ROLE_PATTERN = re.compile(r"^driving_(vision|policy|off_policy|on_policy)_(.+)_tinygrad\.pkl$")
def _bundle_models(bundle) -> list:
models = getattr(bundle, "models", None)
return list(models) if models is not None else []
def _bundle_override_map(bundle) -> dict[str, str]:
result: dict[str, str] = {}
for override in getattr(bundle, "overrides", None) or []:
key = getattr(override, "key", None)
value = getattr(override, "value", None)
if key and value:
result[str(key)] = str(value)
return result
def _artifact_name(model) -> str:
return getattr(getattr(model, "artifact", None), "fileName", "") or ""
def _split_suffixes(bundle) -> list[str]:
suffixes: list[str] = []
for model in _bundle_models(bundle):
match = _SPLIT_ROLE_PATTERN.match(_artifact_name(model))
if match:
suffixes.append(match.group(2))
return suffixes
def _derived_candidates(bundle) -> list[str]:
seen: set[str] = set()
candidates: list[str] = []
for suffix in _split_suffixes(bundle):
for candidate in (
f"driving_combined_{suffix}.pkl",
f"iqmodeld_combined_{suffix}.pkl",
):
if candidate not in seen:
seen.add(candidate)
candidates.append(candidate)
ref = getattr(bundle, "ref", None)
if ref:
short_ref = str(ref)[:8]
for candidate in (
f"driving_combined_{short_ref}.pkl",
f"iqmodeld_combined_{short_ref}.pkl",
):
if candidate not in seen:
seen.add(candidate)
candidates.append(candidate)
return candidates
def combined_split_artifact_candidates(bundle) -> list[Path]:
explicit_env = os.getenv("IQMODEL_COMBINED_PKL")
if explicit_env:
explicit_path = Path(explicit_env)
return [explicit_path if explicit_path.is_absolute() else _MODEL_ROOT / explicit_path]
overrides = _bundle_override_map(bundle)
explicit_names = [overrides[key] for key in _OVERRIDE_KEYS if key in overrides]
if explicit_names:
return [_MODEL_ROOT / name for name in explicit_names]
return [_MODEL_ROOT / name for name in _derived_candidates(bundle)]
def resolve_combined_split_artifact(bundle) -> Path | None:
for candidate in combined_split_artifact_candidates(bundle):
if candidate.is_file():
return candidate
return None
def has_combined_split_artifact(bundle) -> bool:
return resolve_combined_split_artifact(bundle) is not None

11
artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/iqmodeld/models/fetcher.py Normal file
View File

@@ -0,0 +1,11 @@
#!/usr/bin/env python3
"""
Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos/
"""
from iqpilot._proprietary_loader import ProprietaryModuleMissing, load_private_module
try:
load_private_module(__name__, "iqpilot_private.models.fetcher")
except ProprietaryModuleMissing:
from iqpilot.models_private_src.fetcher import * # noqa: F403

313
artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/iqmodeld/models/helpers.py Normal file
View File

@@ -0,0 +1,313 @@
#!/usr/bin/env python3
"""
Copyright (c) IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
"""
import json
import os
import shutil
from pathlib import Path
from iqpilot.cereal import custom
from iqpilot.common.params import Params
from iqpilot.common.swaglog import cloudlog
from iqpilot._proprietary_loader import ProprietaryModuleMissing, load_private_module
from iqpilot.system.hardware.hw import Paths
try:
load_private_module(__name__, "iqpilot_private.models.helpers")
except ProprietaryModuleMissing:
try:
from iqpilot.models_private_src.helpers import * # noqa: F403
except ImportError:
pass
ModelBundle = custom.IQModelManager.ModelBundle
Runner = custom.IQModelManager.Runner
_MODEL_ROOT = Path(Paths.model_root())
_ACTIVE_BUNDLE_KEY = "ModelManager_ActiveBundle"
_MODELS_CACHE_KEY = "ModelManager_ModelsCache"
_RUNNER_CACHE_KEY = "ModelRunnerTypeCache"
_DOWNLOAD_INDEX_KEY = "ModelManager_DownloadIndex"
_PENDING_INDEX_KEY = "ModelManager_PendingIndex"
_PENDING_MODEL_RESTORE_FILE = "/data/k3_pending_model_restore"
_STOCK_RUNNER = int(Runner.stock)
_TINYGRAD_RUNNER = int(Runner.tinygrad)
_SNPE_RUNNER = int(Runner.snpe)
_DEFAULT_MODEL_DIR = Path(__file__).resolve().parents[1] / "default_model"
_DEFAULT_BUNDLE_JSON = _DEFAULT_MODEL_DIR / "bundle.json"
_DEFAULT_BUNDLE_REF = "default"
def get_default_model_bundle(_bundles):
return None
def _coerce_runner_value(value) -> int | None:
raw = getattr(value, "raw", value)
try:
return int(raw)
except (TypeError, ValueError):
return None
def _bundle_models(bundle) -> list:
models = getattr(bundle, "models", None)
return list(models) if models is not None else []
def _bundle_needs_runtime_upgrade(bundle) -> bool:
if bundle is None:
return False
if _coerce_runner_value(getattr(bundle, "runner", None)) == _SNPE_RUNNER:
return True
for model in _bundle_models(bundle):
file_name = getattr(getattr(model, "artifact", None), "fileName", "") or ""
if file_name.endswith(".thneed"):
return True
return False
def _load_cached_manifest_bundles(params: Params):
cached = params.get(_MODELS_CACHE_KEY) or {}
bundles = []
for raw_bundle in cached.get("bundles", []):
try:
min_selector_version = int(raw_bundle.get("minimumSelectorVersion", raw_bundle.get("minimum_selector_version", 0)))
compatibility_view = dict(raw_bundle)
compatibility_view["minimumSelectorVersion"] = min_selector_version
is_compatible = globals().get("is_bundle_version_compatible")
if is_compatible is not None and not is_compatible(compatibility_view):
continue
if "short_name" in raw_bundle:
from iqpilot.selfdrive.iqmodeld.models.fetcher import ManifestDecoder
bundles.append(ManifestDecoder._decode_bundle(raw_bundle))
continue
if "internalName" in raw_bundle:
bundles.append(ModelBundle(**raw_bundle))
continue
bundle = ModelBundle()
bundle.index = int(raw_bundle["index"])
bundle.internalName = raw_bundle.get("short_name")
bundle.displayName = raw_bundle.get("display_name")
bundle.status = 0
bundle.generation = int(raw_bundle["generation"])
bundle.environment = raw_bundle["environment"]
bundle.runner = raw_bundle.get("runner", Runner.tinygrad)
bundle.is20hz = raw_bundle.get("is_20hz", False)
bundle.minimumSelectorVersion = int(min_selector_version)
bundle.ref = raw_bundle.get("ref")
bundle.overrides = []
for key, value in raw_bundle.get("overrides", {}).items():
override = custom.IQModelManager.Override()
override.key = key
override.value = value
bundle.overrides.append(override)
bundle.models = []
for raw_model in raw_bundle.get("models", []):
model = custom.IQModelManager.Model()
model.type = raw_model.get("type")
for attr_name in ("artifact", "metadata"):
raw_artifact = raw_model.get(attr_name)
if not raw_artifact:
continue
artifact = custom.IQModelManager.Artifact()
artifact.fileName = raw_artifact.get("file_name")
download_uri = custom.IQModelManager.DownloadUri()
download_uri.uri = raw_artifact.get("download_uri", {}).get("url")
download_uri.sha256 = raw_artifact.get("download_uri", {}).get("sha256")
artifact.downloadUri = download_uri
setattr(model, attr_name, artifact)
bundle.models.append(model)
bundles.append(bundle)
except Exception:
continue
return bundles
def _bundle_match_key(bundle) -> tuple[str | None, str | None, str | None]:
return (
getattr(bundle, "ref", None),
getattr(bundle, "internalName", None),
getattr(bundle, "displayName", None),
)
def _find_runtime_upgrade(bundle, params: Params, available_bundles=None):
if not _bundle_needs_runtime_upgrade(bundle):
return bundle
candidate_bundles = available_bundles if available_bundles is not None else _load_cached_manifest_bundles(params)
ref, internal_name, display_name = _bundle_match_key(bundle)
for candidate in candidate_bundles:
if getattr(candidate, "ref", None) and getattr(candidate, "ref", None) == ref:
return candidate
for candidate in candidate_bundles:
if getattr(candidate, "internalName", None) == internal_name:
return candidate
for candidate in candidate_bundles:
if getattr(candidate, "displayName", None) == display_name:
return candidate
return None
def bundle_files_ready(bundle) -> bool:
if bundle is None:
return False
for model in _bundle_models(bundle):
artifact = getattr(model, "artifact", None)
metadata = getattr(model, "metadata", None)
for file_name in (getattr(metadata, "fileName", None), getattr(artifact, "fileName", None)):
if file_name and not (_MODEL_ROOT / file_name).is_file():
return False
return True
def persist_active_bundle(params: Params, bundle) -> None:
params.put(_ACTIVE_BUNDLE_KEY, bundle.to_dict())
params.remove(_RUNNER_CACHE_KEY)
def _load_default_bundle_dict() -> dict:
return json.loads(_DEFAULT_BUNDLE_JSON.read_text())
def _default_bundle_filenames(bundle_dict: dict) -> list[str]:
names = []
for model in bundle_dict.get("models", []):
for artifact in (model.get("metadata"), model.get("artifact")):
file_name = artifact.get("fileName", "") if isinstance(artifact, dict) else ""
if file_name:
names.append(file_name)
return names
def is_default_bundle(bundle) -> bool:
return bool(bundle is not None and getattr(bundle, "ref", None) == _DEFAULT_BUNDLE_REF)
def ensure_default_model_files(bundle_dict: dict = None) -> None:
bundle_dict = bundle_dict if bundle_dict is not None else _load_default_bundle_dict()
try:
_MODEL_ROOT.mkdir(parents=True, exist_ok=True)
except OSError as e:
cloudlog.exception(f"default_model: cannot create model root: {e}")
return
for file_name in _default_bundle_filenames(bundle_dict):
src = _DEFAULT_MODEL_DIR / file_name
dst = _MODEL_ROOT / file_name
if not src.is_file():
cloudlog.error(f"default_model: shipped asset missing {src}")
continue
if dst.is_file() and dst.stat().st_size == src.stat().st_size:
continue
try:
shutil.copy2(src, dst)
cloudlog.warning(f"default_model: staged {file_name} into model root")
except OSError as e:
cloudlog.exception(f"default_model: failed staging {file_name}: {e}")
def select_default_model(params: Params = None) -> None:
params = Params() if params is None else params
bundle_dict = _load_default_bundle_dict()
ensure_default_model_files(bundle_dict)
params.remove(_DOWNLOAD_INDEX_KEY)
params.remove(_PENDING_INDEX_KEY)
params.put(_ACTIVE_BUNDLE_KEY, bundle_dict)
params.remove(_RUNNER_CACHE_KEY)
params.put(_RUNNER_CACHE_KEY, _TINYGRAD_RUNNER)
try:
if os.path.isfile(_PENDING_MODEL_RESTORE_FILE):
os.remove(_PENDING_MODEL_RESTORE_FILE)
except OSError:
pass
def seed_default_bundle_if_unset(params: Params = None) -> None:
params = Params() if params is None else params
if params.get(_ACTIVE_BUNDLE_KEY):
return
queued_download = params.get(_DOWNLOAD_INDEX_KEY)
try:
select_default_model(params)
if queued_download is not None:
params.put(_DOWNLOAD_INDEX_KEY, queued_download)
cloudlog.warning("default_model: seeded Default (CD210) as active bundle")
except Exception as e:
cloudlog.exception(f"default_model: failed to seed default bundle: {e}")
def get_runtime_bundle_upgrade(bundle, params: Params = None, available_bundles=None):
params = Params() if params is None else params
return _find_runtime_upgrade(bundle, params, available_bundles)
def get_active_bundle(params: Params = None):
params = Params() if params is None else params
try:
active_bundle = params.get(_ACTIVE_BUNDLE_KEY) or {}
if not active_bundle:
return None
is_compatible = globals().get("is_bundle_version_compatible")
if is_compatible is not None and not is_compatible(active_bundle):
return None
bundle = ModelBundle(**active_bundle)
except Exception:
return None
replacement = _find_runtime_upgrade(bundle, params)
if replacement is not None and replacement is not bundle and bundle_files_ready(replacement):
persist_active_bundle(params, replacement)
return replacement
return bundle
def get_active_model_runner(params: Params = None, force_check=False):
params = Params() if params is None else params
active_bundle = get_active_bundle(params)
if not active_bundle:
seed_default_bundle_if_unset(params)
active_bundle = get_active_bundle(params)
if not active_bundle:
if params.get(_RUNNER_CACHE_KEY) != str(_TINYGRAD_RUNNER):
params.put(_RUNNER_CACHE_KEY, _TINYGRAD_RUNNER)
return _TINYGRAD_RUNNER
cached_runner_type = params.get(_RUNNER_CACHE_KEY)
if cached_runner_type and not force_check and isinstance(cached_runner_type, str) and cached_runner_type.isdigit():
return int(cached_runner_type)
runner_type = _coerce_runner_value(active_bundle.runner)
if runner_type == _SNPE_RUNNER:
replacement = _find_runtime_upgrade(active_bundle, params)
if replacement is not None and replacement is not active_bundle and bundle_files_ready(replacement):
persist_active_bundle(params, replacement)
runner_type = _coerce_runner_value(replacement.runner)
else:
if replacement is not None and getattr(replacement, "index", None) is not None and params.get(_DOWNLOAD_INDEX_KEY) is None:
params.put(_DOWNLOAD_INDEX_KEY, int(replacement.index))
cloudlog.warning(f"Queued tinygrad migration for retired bundle {getattr(active_bundle, 'internalName', '<unknown>')}")
runner_type = _TINYGRAD_RUNNER
if cached_runner_type != runner_type:
params.put(_RUNNER_CACHE_KEY, int(runner_type))
return runner_type

9
artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/iqmodeld/models/inference_state.py Normal file
View File

@@ -0,0 +1,9 @@
"""
Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
"""
from iqpilot.common.steer_delay import cached_steer_delay
class InferenceStateBase:
def __init__(self):
self.lat_delay = cached_steer_delay()

0
iqdbc_repo/iqdbc/car/chrysler/__init__.py → artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/iqmodeld/models/runners/__init__.py
View File

231
artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/iqmodeld/models/runners/model_runner.py Normal file
View File

@@ -0,0 +1,231 @@
"""
Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
"""
import os
import pickle as _pk
from dataclasses import dataclass, field
from typing import TYPE_CHECKING, Any
import numpy as np
from iqpilot.cereal import custom
from iqpilot.common.swaglog import cloudlog
from iqpilot.system.hardware import TICI
from iqpilot.system.hardware.hw import Paths as _hw_paths
from iqpilot.selfdrive.iqmodeld.models.helpers import get_active_bundle as _fetch_bundle
from iqpilot.selfdrive.iqmodeld.models.combined_artifact import has_combined_split_artifact
# ---- runtime type surface (native OpenCL/frame handles resolve to Any off-device) ----
if TYPE_CHECKING:
from iqpilot.selfdrive.iqmodeld.native.iqmodel_pyx import GpuMemorySlot, RoadProjector
else:
def _resolve_native_types() -> tuple[Any, Any]:
try:
from iqpilot.selfdrive.iqmodeld.native.iqmodel_pyx import GpuMemorySlot as iq_clmem
from iqpilot.selfdrive.iqmodeld.native.iqmodel_pyx import RoadProjector as iq_frame
return iq_clmem, iq_frame
except (ModuleNotFoundError, ImportError):
return Any, Any
GpuMemorySlot, RoadProjector = _resolve_native_types()
NumpyDict = dict[str, np.ndarray]
ShapeDict = dict[str, tuple[int, ...]]
SliceDict = dict[str, slice]
CLMemDict = dict[str, GpuMemorySlot]
FrameDict = dict[str, RoadProjector]
ModelType = custom.IQModelManager.Model.Type
Model = custom.IQModelManager.Model
SEND_RAW_PRED = os.getenv("SEND_RAW_PRED")
CUSTOM_MODEL_PATH = _hw_paths.model_root()
_META_FIELDS = ("input_shapes", "output_slices")
USBGPU = "USBGPU" in os.environ
def _configure_accelerator():
"""Point tinygrad at the right backend. Must run before tinygrad is imported,
which is why it fires at module import."""
backend, extra = ("QCOM" if TICI else "CPU"), {}
if USBGPU:
backend, extra = "AMD", {"AMD_IFACE": "USB"}
elif TICI:
extra = {"QCOM_PRIORITY": "8"}
os.environ["DEV"] = backend
os.environ.update(extra)
_configure_accelerator()
# real metadata pkls are a few KB; anything bigger is a model artifact wrongly
# referenced as metadata (pre-fix manifests self-referenced the artifact), and
# unpickling it here double-loads the model onto the GPU
_META_MAX_BYTES = 1 << 20
def load_artifact_metadata(metadata_filename):
"""Read one artifact's metadata pkl: (input shapes, output slices)."""
try:
path = os.path.join(CUSTOM_MODEL_PATH, metadata_filename)
if os.path.getsize(path) > _META_MAX_BYTES:
cloudlog.error(f"metadata pkl {metadata_filename} is artifact-sized, refusing to unpickle it")
return tuple({} for _ in _META_FIELDS)
with open(path, 'rb') as fh:
blob = _pk.load(fh)
return tuple(blob.get(field, {}) for field in _META_FIELDS)
except Exception:
cloudlog.exception(f"unreadable metadata pkl {metadata_filename}, continuing without it")
return tuple({} for _ in _META_FIELDS)
@dataclass
class ArtifactSpec:
"""One model of the active bundle plus its unpacked metadata."""
model: Any
metadata: Any = None
input_shapes: ShapeDict = field(default_factory=dict)
output_slices: SliceDict = field(default_factory=dict)
def __post_init__(self):
self.metadata = self.model.metadata
if self.metadata:
self.input_shapes, self.output_slices = load_artifact_metadata(self.metadata.fileName)
# kept name: some runners annotate against the old alias
ModelData = ArtifactSpec
class RunnerRoot:
"""Shared root of the runner hierarchy.
Both ModelRunner and the per-model parser mixins (model_types.py) inherit
this, so the concrete `TinygradRunner(ModelRunner, *Tinygrad)` diamond keeps
one consistent parser registry + slice implementation.
"""
parser_method_dict: dict
_model_data: "ArtifactSpec | None"
def _slice_outputs(self, model_outputs):
raise NotImplementedError
class ModelRunner(RunnerRoot):
"""Base for the tinygrad/ONNX runners.
Owns the active bundle's ArtifactSpecs and the shared slice/parse plumbing;
subclasses provide input staging (prepare_inputs) and execution (_run_model).
"""
# False for fused runners, which warp + manage temporal buffers inside the JIT
uses_opencl_warp = True
def __init__(self):
active = _fetch_bundle()
if not active:
raise ValueError("runner started without an active model bundle")
self.models = {spec.type.raw: ArtifactSpec(spec) for spec in _qcom_models(active)}
self.is_20hz_3d = False
self.is_20hz = active.is20hz
self.inputs = {}
self.parser_method_dict = {}
self._model_data = None # active spec for the current operation
self._parser = self._constants = None
def _active_spec(self):
spec = self._model_data
if spec is None:
raise ValueError("Model data is not available. Ensure the model is loaded correctly.")
return spec
# views proxied straight off the active artifact spec; kept out of the class
# body (served via __getattr__) so the read surface stays data-driven
_SPEC_VIEW = frozenset(("input_shapes", "output_slices"))
def __getattr__(self, name):
if name == "constants":
return self._constants
if name == "vision_input_names":
return list(self._active_spec().input_shapes)
if name in ModelRunner._SPEC_VIEW:
return getattr(self._active_spec(), name)
raise AttributeError(name)
def prepare_inputs(self, imgs_cl, numpy_inputs, frames):
"""Stage image + numpy inputs for inference; implemented per backend."""
raise NotImplementedError
def _run_model(self):
"""Execute inference over the staged inputs; implemented per backend."""
raise NotImplementedError
def run_model(self):
# parsing happens inside each backend's _run_model
return self._run_model()
def _slice_outputs(self, model_outputs):
"""Split the flat output vector into named views per the artifact's slice table."""
sliced = {}
for tag, span in self._active_spec().output_slices.items():
sliced[tag] = model_outputs[np.newaxis, span]
if SEND_RAW_PRED:
sliced["raw_pred"] = model_outputs.copy()
return sliced
# ---- runner selection (which backend to build for the active bundle) ----------
def _qcom_models(bundle) -> list:
# usbeMac artifacts ride along in a bundle for the eGPU host; they are never
# loaded on QCOM and must not affect runner classification
return [m for m in bundle.models if m.type.raw != ModelType.usbeMac]
def _single_artifact_prefix(bundle, prefix: str) -> bool:
models = _qcom_models(bundle)
return len(models) == 1 and models[0].artifact.fileName.startswith(prefix)
def _is_fused_bundle(bundle) -> bool:
return _single_artifact_prefix(bundle, "driving_fused_")
def _is_supercombo_bundle(bundle) -> bool:
return _single_artifact_prefix(bundle, "driving_supercombo_")
def _is_split_bundle(bundle) -> bool:
present = {m.type.raw for m in _qcom_models(bundle)}
split_kinds = {ModelType.vision, ModelType.policy, ModelType.offPolicy, ModelType.onPolicy}
return not present.isdisjoint(split_kinds)
def get_model_runner() -> "ModelRunner":
"""Build the runner backend that fits the active bundle (supercombo / fused /
combined-split / split / single). Concrete runners are imported lazily so one
backend failing to load can't take down the others at import time."""
from iqpilot.selfdrive.iqmodeld.models.runners.tinygrad.tinygrad_runner import (TinygradRunner,
TinygradSplitRunner)
bundle = _fetch_bundle()
# an eMac-only bundle (no QCOM-loadable models) runs the stock default on
# device; the big host serves the bundle's precompiled artifact
if not (bundle and bundle.models and _qcom_models(bundle)):
return TinygradRunner(ModelType.supercombo)
if _is_supercombo_bundle(bundle):
from iqpilot.selfdrive.iqmodeld.models.runners.tinygrad.supercombo_runner import TinygradSupercomboRunner
return TinygradSupercomboRunner()
if _is_fused_bundle(bundle):
from iqpilot.selfdrive.iqmodeld.models.runners.tinygrad.fused_runner import TinygradFusedRunner
return TinygradFusedRunner()
if _is_split_bundle(bundle) and has_combined_split_artifact(bundle):
from iqpilot.selfdrive.iqmodeld.models.runners.tinygrad.combined_split_runner import TinygradCombinedSplitRunner
return TinygradCombinedSplitRunner()
if _is_split_bundle(bundle):
return TinygradSplitRunner()
return TinygradRunner(_qcom_models(bundle)[0].type.raw)

0
iqdbc_repo/iqdbc/car/common/__init__.py → artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/iqmodeld/models/runners/tinygrad/__init__.py
View File

245
artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/iqmodeld/models/runners/tinygrad/combined_split_runner.py Normal file
View File

@@ -0,0 +1,245 @@
"""
Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos/
"""
from __future__ import annotations
import os
import pickle
from pathlib import Path
from typing import Any
import numpy as np
from iqpilot.selfdrive.iqmodeld.models.combined_artifact import resolve_combined_split_artifact
from iqpilot.selfdrive.iqmodeld.models.helpers import get_active_bundle
from iqpilot.selfdrive.iqmodeld.models.runners.model_runner import NumpyDict, ShapeDict, SliceDict
from iqpilot.selfdrive.iqmodeld.models.runners.model_runner import ModelRunner
from iqpilot.selfdrive.iqmodeld.models.split_model_constants import SplitModelConstants
from iqpilot.selfdrive.iqmodeld.parser import PhaseParser
def _tinygrad_imports():
from tinygrad.device import Device
from tinygrad.tensor import Tensor
return Tensor, Device
def _phase_roles(meta_by_role: dict[str, dict]) -> list[str]:
return [name for name in meta_by_role if name != "vision"]
def _phase_desire_key(policy_shapes: dict[str, tuple[int, ...]]) -> str:
for key in policy_shapes:
if key.startswith("desire"):
return key
raise KeyError("No desire-like key found in policy inputs")
def _phase_image_keys(vision_shapes: dict[str, tuple[int, ...]]) -> tuple[str, str]:
names = sorted(name for name in vision_shapes if "img" in name)
road_key = next((name for name in names if "big" not in name), None)
wide_key = next((name for name in names if "big" in name), None)
if road_key is None or wide_key is None:
raise ValueError(f"Unable to resolve road/wide image keys from {list(vision_shapes)}")
return road_key, wide_key
def _base_policy_keys(policy_shapes: dict[str, tuple[int, ...]]) -> set[str]:
desired_key = _phase_desire_key(policy_shapes)
return {desired_key, "features_buffer", "traffic_convention", "action_t"}
def _slice_map(raw_blob: np.ndarray, slices: dict[str, slice]) -> NumpyDict:
return {name: raw_blob[np.newaxis, section] for name, section in slices.items() if name != "pad"}
class TinygradCombinedSplitRunner(ModelRunner):
uses_opencl_warp: bool = False
def __init__(self):
super().__init__()
self._constants = SplitModelConstants
self._parser = PhaseParser()
self._bundle = get_active_bundle()
self._artifact_path = resolve_combined_split_artifact(self._bundle)
if self._artifact_path is None:
raise FileNotFoundError("No IQ combined split artifact is available for the active bundle")
with open(self._artifact_path, "rb") as artifact:
runtime_package: dict[Any, Any] = pickle.load(artifact)
self._meta_by_role = runtime_package.get("meta_by_role", runtime_package.get("metadata", {}))
self._policy_roles = runtime_package.get("roles", _phase_roles(self._meta_by_role))
self._camera_programs = {
camera_key: spec
for camera_key, spec in runtime_package.items()
if isinstance(camera_key, tuple) and isinstance(spec, dict)
}
self._execute_bundle = runtime_package.get("execute_bundle", runtime_package.get("run_policy"))
self._frame_stride = int(runtime_package.get("frame_stride", runtime_package.get("frame_skip", 1)))
if "vision" not in self._meta_by_role:
raise ValueError("Combined split artifact is missing vision metadata")
if not self._policy_roles:
raise ValueError("Combined split artifact is missing policy roles")
if self._execute_bundle is None:
raise ValueError("Combined split artifact is missing execute_bundle")
self._vision_meta = self._meta_by_role["vision"]
self._primary_policy_meta = self._meta_by_role[self._policy_roles[0]]
self._desired_key = _phase_desire_key(self._primary_policy_meta["input_shapes"])
self._road_key, self._wide_key = _phase_image_keys(self._vision_meta["input_shapes"])
self._extra_policy_keys = [
key for key in self._primary_policy_meta["input_shapes"]
if key not in _base_policy_keys(self._primary_policy_meta["input_shapes"])
]
self._queue_tensors: dict[str, Any] | None = None
self._numpy_state: dict[str, np.ndarray] | None = None
self._camera_shape: tuple[int, int] | None = None
self._blob_cache: dict[tuple[str, int], Any] = {}
self._last_desire = np.zeros(self._primary_policy_meta["input_shapes"][self._desired_key][2], dtype=np.float32)
@property
def vision_input_names(self) -> list[str]:
return [self._road_key, self._wide_key]
@property
def input_shapes(self) -> ShapeDict:
merged: ShapeDict = dict(self._vision_meta["input_shapes"])
for role in self._policy_roles:
merged.update(self._meta_by_role[role]["input_shapes"])
return merged
@property
def output_slices(self) -> SliceDict:
merged: SliceDict = dict(self._vision_meta["output_slices"])
for role in self._policy_roles:
merged.update(self._meta_by_role[role]["output_slices"])
return merged
def prepare_inputs(self, imgs_cl, numpy_inputs, frames):
raise RuntimeError("Combined split runner manages its own warp + queue state; use run_fused()")
def _frame_blob(self, stream_name: str, buf):
Tensor, Device = _tinygrad_imports()
raw_frame = np.frombuffer(buf.data, dtype=np.uint8)
cache_key = (stream_name, raw_frame.ctypes.data)
tensor = self._blob_cache.get(cache_key)
if tensor is None:
tensor = Tensor.from_blob(raw_frame.ctypes.data, (raw_frame.size,), dtype="uint8", device=Device.DEFAULT)
self._blob_cache[cache_key] = tensor
return tensor
def _allocate_runtime_state(self, camera_width: int, camera_height: int) -> None:
if self._queue_tensors is not None and self._camera_shape == (camera_width, camera_height):
return
if (camera_width, camera_height) not in self._camera_programs:
raise RuntimeError(f"No combined split kernels available for {camera_width}x{camera_height}")
Tensor, Device = _tinygrad_imports()
vision_shapes = self._vision_meta["input_shapes"]
policy_shapes = self._primary_policy_meta["input_shapes"]
image_shape = vision_shapes[self._road_key]
frame_history = image_shape[1] // 6
queue_depth = self._frame_stride * (frame_history - 1) + 1
frame_queue_shape = (queue_depth, 6, image_shape[2], image_shape[3])
feature_shape = policy_shapes["features_buffer"]
desired_shape = policy_shapes[self._desired_key]
traffic_shape = policy_shapes["traffic_convention"]
action_shape = policy_shapes.get("action_t", traffic_shape)
numpy_state = {
"tfm": np.zeros((3, 3), dtype=np.float32),
"big_tfm": np.zeros((3, 3), dtype=np.float32),
"desire": np.zeros(desired_shape[2], dtype=np.float32),
"traffic_convention": np.zeros(traffic_shape, dtype=np.float32),
"action_t": np.zeros(action_shape, dtype=np.float32),
}
for key in self._extra_policy_keys:
numpy_state[key] = np.zeros(policy_shapes[key], dtype=np.float32)
queue_tensors = {
"img_q": Tensor(np.zeros(frame_queue_shape, dtype=np.uint8), device=Device.DEFAULT).contiguous().realize(),
"big_img_q": Tensor(np.zeros(frame_queue_shape, dtype=np.uint8), device=Device.DEFAULT).contiguous().realize(),
"feat_q": Tensor(
np.zeros((self._frame_stride * (feature_shape[1] - 1) + 1, feature_shape[0], feature_shape[2]), dtype=np.float32),
device=Device.DEFAULT,
).contiguous().realize(),
"desire_q": Tensor(
np.zeros((self._frame_stride * desired_shape[1], desired_shape[0], desired_shape[2]), dtype=np.float32),
device=Device.DEFAULT,
).contiguous().realize(),
**{name: Tensor(value, device="NPY").realize() for name, value in numpy_state.items()},
}
self._queue_tensors = queue_tensors
self._numpy_state = numpy_state
self._camera_shape = (camera_width, camera_height)
def _policy_inputs(self) -> dict[str, Any]:
assert self._queue_tensors is not None
tensor_names = ["feat_q", "desire_q", "desire", "traffic_convention", "action_t", *self._extra_policy_keys]
return {name: self._queue_tensors[name] for name in tensor_names if name in self._queue_tensors}
def _merge_policy_outputs(self, raw_outputs: tuple[Any, ...]) -> NumpyDict:
outputs = self._parser.parse_vision_outputs(
_slice_map(raw_outputs[0].numpy().flatten(), self._vision_meta["output_slices"])
)
has_on_policy = any(role == "on_policy" for role in self._policy_roles)
for role_name, tensor_out in zip(self._policy_roles, raw_outputs[1:], strict=True):
parsed = self._parser.parse_policy_outputs(
_slice_map(tensor_out.numpy().flatten(), self._meta_by_role[role_name]["output_slices"])
)
if role_name == "off_policy" and has_on_policy:
parsed.pop("plan", None)
outputs.update(parsed)
if "planplus" in outputs and "plan" in outputs:
outputs["plan"] = outputs["plan"] + outputs["planplus"]
return outputs
def run_fused(self, bufs: dict, transforms: dict[str, np.ndarray], numpy_inputs: NumpyDict) -> NumpyDict:
main_buf = bufs[self._road_key]
self._allocate_runtime_state(main_buf.width, main_buf.height)
assert self._queue_tensors is not None and self._numpy_state is not None and self._camera_shape is not None
self._numpy_state["tfm"][:] = transforms[self._road_key]
self._numpy_state["big_tfm"][:] = transforms[self._wide_key]
current_desire = numpy_inputs[self._desired_key].copy()
current_desire[0] = 0
self._numpy_state["desire"][:] = np.where(current_desire - self._last_desire > 0.99, current_desire, 0)
self._last_desire[:] = current_desire
if "traffic_convention" in numpy_inputs:
self._numpy_state["traffic_convention"][:] = numpy_inputs["traffic_convention"]
if "action_t" in numpy_inputs:
self._numpy_state["action_t"][:] = numpy_inputs["action_t"]
for key in self._extra_policy_keys:
if key in numpy_inputs:
self._numpy_state[key][:] = numpy_inputs[key]
stage_inputs = self._camera_programs[self._camera_shape].get("stage_inputs", self._camera_programs[self._camera_shape].get("warp_enqueue"))
if stage_inputs is None:
raise RuntimeError("Combined split artifact camera entry is missing stage_inputs")
staged_main, staged_wide = stage_inputs(
img_q=self._queue_tensors["img_q"],
big_img_q=self._queue_tensors["big_img_q"],
tfm=self._queue_tensors["tfm"],
big_tfm=self._queue_tensors["big_tfm"],
frame=self._frame_blob(self._road_key, bufs[self._road_key]),
big_frame=self._frame_blob(self._wide_key, bufs[self._wide_key]),
)
raw_outputs = self._execute_bundle(img=staged_main, big_img=staged_wide, **self._policy_inputs())
if not isinstance(raw_outputs, tuple):
raw_outputs = (raw_outputs,)
return self._merge_policy_outputs(raw_outputs)
def _run_model(self) -> NumpyDict:
raise RuntimeError("Combined split runner executes through run_fused()")

178
artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/iqmodeld/models/runners/tinygrad/fused_runner.py Normal file
View File

@@ -0,0 +1,178 @@
"""
Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos/
"""
from __future__ import annotations
import os
import pickle
from typing import Any
import numpy as np
from iqpilot.selfdrive.iqmodeld.models.runners.model_runner import (
CUSTOM_MODEL_PATH, NumpyDict, ShapeDict, SliceDict,
)
from iqpilot.selfdrive.iqmodeld.models.runners.model_runner import ModelRunner
from iqpilot.selfdrive.iqmodeld.models.split_model_constants import SplitModelConstants
from iqpilot.selfdrive.iqmodeld.parser import PhaseParser
def _tinygrad_imports():
from tinygrad.tensor import Tensor
from tinygrad.device import Device
return Tensor, Device
WARP_DEV = os.getenv('WARP_DEV')
class TinygradFusedRunner(ModelRunner):
uses_opencl_warp: bool = False
def __init__(self):
super().__init__()
self._constants = SplitModelConstants
self._parser = PhaseParser()
if len(self.models) != 1:
raise ValueError(f"fused bundle must have exactly one artifact, got {list(self.models)}")
self._model_data = next(iter(self.models.values()))
pkl_path = os.path.join(CUSTOM_MODEL_PATH, self._model_data.model.artifact.fileName)
with open(pkl_path, 'rb') as f:
self._fused: dict[Any, Any] = pickle.load(f)
self._vision_meta = self._fused['metadata']['vision']
self._on_meta = self._fused['metadata']['on_policy']
self._off_meta = self._fused['metadata']['off_policy']
self._run_policy = self._fused['run_policy']
self._warp_jits: dict[tuple[int, int], Any] = {k: v for k, v in self._fused.items() if isinstance(k, tuple)}
if not self._warp_jits:
raise ValueError("fused pkl has no warp JITs")
self._frame_skip: int = int(self._fused.get('frame_skip', 4))
self._queues: dict[str, Any] | None = None
self._npy_buffers: dict[str, np.ndarray] | None = None
self._cam_resolution: tuple[int, int] | None = None
self._blob_cache: dict[tuple[str, int], Any] = {}
def _frame_tensor(self, key, buf):
Tensor, Device = _tinygrad_imports()
arr = np.frombuffer(buf.data, dtype=np.uint8)
ck = (key, arr.ctypes.data)
t = self._blob_cache.get(ck)
if t is None:
t = Tensor.from_blob(arr.ctypes.data, (arr.size,), dtype='uint8', device=Device.DEFAULT)
self._blob_cache[ck] = t
return t
@property
def vision_input_names(self) -> list[str]:
return ['img', 'big_img']
@property
def input_shapes(self) -> ShapeDict:
return {**self._vision_meta['input_shapes'], **self._on_meta['input_shapes']}
@property
def output_slices(self) -> SliceDict:
merged: SliceDict = {}
for src in (self._vision_meta['output_slices'], self._on_meta['output_slices'], self._off_meta['output_slices']):
merged.update({k: v for k, v in src.items() if k != 'pad'})
return merged
def prepare_inputs(self, imgs_cl, numpy_inputs, frames):
raise RuntimeError("fused runner has no OpenCL path; use run_fused()")
def _ensure_queues(self, cam_w: int, cam_h: int) -> None:
if self._queues is not None and self._cam_resolution == (cam_w, cam_h):
return
if (cam_w, cam_h) not in self._warp_jits:
raise RuntimeError(f"no warp JIT for {cam_w}x{cam_h}; have {sorted(self._warp_jits)}")
Tensor, Device = _tinygrad_imports()
img_shape = self._vision_meta['input_shapes']['img']
fb = self._on_meta['input_shapes']['features_buffer']
dp = self._on_meta['input_shapes']['desire_pulse']
n_frames = img_shape[1] // 6
img_buf_shape = (self._frame_skip * (n_frames - 1) + 1, 6, img_shape[2], img_shape[3])
zeros_u8 = lambda shp: Tensor(np.zeros(shp, dtype=np.uint8), device=Device.DEFAULT).contiguous().realize()
zeros_f32 = lambda shp: Tensor(np.zeros(shp, dtype=np.float32), device=Device.DEFAULT).contiguous().realize()
self._queues = {
'img_q': zeros_u8(img_buf_shape),
'big_img_q': zeros_u8(img_buf_shape),
'feat_q': zeros_f32((self._frame_skip * (fb[1] - 1) + 1, fb[0], fb[2])),
'desire_q': zeros_f32((self._frame_skip * dp[1], dp[0], dp[2])),
}
on_shapes = self._on_meta['input_shapes']
captured = self._run_policy.captured
jit_shapes = {
name: tuple(int(s) for s in view.shape)
for name, (view, _vars, _dtype, _device) in zip(captured.expected_names, captured.expected_input_info)
}
def policy_input_shape(name):
shape = on_shapes.get(name, jit_shapes.get(name))
if shape is None:
raise ValueError(f"fused pkl declares no shape for policy input {name}")
return shape
self._npy_buffers = {
'desire': np.zeros(dp[2], dtype=np.float32),
'traffic_convention': np.zeros(policy_input_shape('traffic_convention'), dtype=np.float32),
'tfm': np.zeros((3, 3), dtype=np.float32),
'big_tfm': np.zeros((3, 3), dtype=np.float32),
}
if 'action_t' in jit_shapes:
self._npy_buffers['action_t'] = np.zeros(policy_input_shape('action_t'), dtype=np.float32)
self._cam_resolution = (cam_w, cam_h)
def run_fused(self, bufs: dict, transforms: dict[str, np.ndarray], numpy_inputs: NumpyDict) -> NumpyDict:
Tensor, Device = _tinygrad_imports()
main_buf = bufs['img']
self._ensure_queues(main_buf.width, main_buf.height)
assert self._queues is not None and self._npy_buffers is not None
desire_key = next((k for k in numpy_inputs if k.startswith('desire')), None)
if desire_key is not None:
self._npy_buffers['desire'][:] = numpy_inputs[desire_key]
if 'traffic_convention' in numpy_inputs:
self._npy_buffers['traffic_convention'][:] = numpy_inputs['traffic_convention']
if 'action_t' in numpy_inputs and 'action_t' in self._npy_buffers:
self._npy_buffers['action_t'][:] = numpy_inputs['action_t']
self._npy_buffers['tfm'][:] = transforms['img']
self._npy_buffers['big_tfm'][:] = transforms['big_img']
npy = lambda key: Tensor(self._npy_buffers[key], device='NPY')
frame = self._frame_tensor('img', bufs['img'])
big_frame = self._frame_tensor('big_img', bufs['big_img'])
warp_jit = self._warp_jits[self._cam_resolution]
img, big_img = warp_jit(img_q=self._queues['img_q'], big_img_q=self._queues['big_img_q'],
tfm=npy('tfm'), big_tfm=npy('big_tfm'), frame=frame, big_frame=big_frame)
policy_inputs = dict(
img=img, big_img=big_img, feat_q=self._queues['feat_q'], desire_q=self._queues['desire_q'],
desire=npy('desire'), traffic_convention=npy('traffic_convention'))
if 'action_t' in self._npy_buffers:
policy_inputs['action_t'] = npy('action_t')
vision_out_t, on_out_t, off_out_t = self._run_policy(**policy_inputs)
def _slice(tensor_out, meta) -> NumpyDict:
flat = tensor_out.numpy().flatten()
return {k: flat[np.newaxis, sl] for k, sl in meta['output_slices'].items() if k != 'pad'}
parsed: NumpyDict = {}
parsed.update(self._parser.parse_vision_outputs(_slice(vision_out_t, self._vision_meta)))
parsed.update(self._parser.parse_policy_outputs(_slice(off_out_t, self._off_meta)))
parsed.update(self._parser.parse_policy_outputs(_slice(on_out_t, self._on_meta)))
return parsed
def _run_model(self) -> NumpyDict:
raise RuntimeError("fused path goes through run_fused(), not _run_model()")

60
artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/iqmodeld/models/runners/tinygrad/model_types.py Normal file
View File

@@ -0,0 +1,60 @@
"""
Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos/
"""
from __future__ import annotations
from abc import ABC
from collections.abc import Callable
import numpy as np
from iqpilot.selfdrive.iqmodeld.models.runners.model_runner import ModelType, NumpyDict
from iqpilot.selfdrive.iqmodeld.models.runners.model_runner import RunnerRoot
from iqpilot.selfdrive.iqmodeld.parser import ArchiveParser, PhaseParser
class _ParserRole(RunnerRoot, ABC):
def _bind_parser_role(self,
selector: int,
parser_builder: Callable[[], object],
projector: Callable[[object, NumpyDict], NumpyDict]) -> None:
parser = parser_builder()
self.parser_method_dict[selector] = lambda model_blob: projector(parser, self._slice_outputs(model_blob))
def _phase_policy(parser: PhaseParser, sliced_outputs: NumpyDict) -> NumpyDict:
return parser.parse_policy_outputs(sliced_outputs)
def _phase_vision(parser: PhaseParser, sliced_outputs: NumpyDict) -> NumpyDict:
return parser.parse_vision_outputs(sliced_outputs)
def _archive_combined(parser: ArchiveParser, sliced_outputs: NumpyDict) -> NumpyDict:
return parser.parse_outputs(sliced_outputs)
class OffPolicyTinygrad(_ParserRole, ABC):
def __init__(self):
self._bind_parser_role(ModelType.offPolicy, PhaseParser, _phase_policy)
class OnPolicyTinygrad(_ParserRole, ABC):
def __init__(self):
self._bind_parser_role(ModelType.onPolicy, PhaseParser, _phase_policy)
class PolicyTinygrad(_ParserRole, ABC):
def __init__(self):
self._bind_parser_role(ModelType.policy, PhaseParser, _phase_policy)
class VisionTinygrad(_ParserRole, ABC):
def __init__(self):
self._bind_parser_role(ModelType.vision, PhaseParser, _phase_vision)
class SupercomboTinygrad(_ParserRole, ABC):
def __init__(self):
self._bind_parser_role(ModelType.supercombo, ArchiveParser, _archive_combined)

339
artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/iqmodeld/models/runners/tinygrad/supercombo_runner.py Normal file
View File

@@ -0,0 +1,339 @@
"""
Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos/
"""
from __future__ import annotations
import hashlib
import math
import os
import pickle
import re
from typing import Any
import numpy as np
from iqpilot.common.params import Params
from iqpilot.selfdrive.iqmodeld.models.runners.model_runner import CUSTOM_MODEL_PATH, NumpyDict, ShapeDict, SliceDict
from iqpilot.selfdrive.iqmodeld.models.runners.model_runner import ModelRunner
from iqpilot.selfdrive.iqmodeld.models.split_model_constants import SplitModelConstants
from iqpilot.selfdrive.iqmodeld.parser import PhaseParser
def _tinygrad_imports():
from tinygrad.tensor import Tensor
from tinygrad.device import Device
return Tensor, Device
def _captured_queue_depth(warp_jit: Any) -> int | None:
captured = getattr(warp_jit, "captured", None)
infos = getattr(captured, "expected_input_info", None)
if not infos or len(infos) < 2:
return None
view_repr = repr(infos[1][0])
dims = [int(val) for val in re.findall(r"arg=(\d+)", view_repr)]
return dims[0] if len(dims) >= 4 else None
def _captured_devices(warp_jit: Any) -> set[str]:
captured = getattr(warp_jit, "captured", None)
infos = getattr(captured, "expected_input_info", None)
if not infos:
return set()
devices: set[str] = set()
for info in infos:
if isinstance(info, tuple) and len(info) >= 4 and isinstance(info[3], str):
devices.add(info[3])
return devices
def _captured_expected_names(jit_obj: Any) -> list[str]:
captured = getattr(jit_obj, "captured", None)
names = getattr(captured, "expected_names", None)
return list(names) if names else []
def _file_sha256(path: str) -> str:
digest = hashlib.sha256()
with open(path, "rb") as f:
for chunk in iter(lambda: f.read(1024 * 1024), b""):
digest.update(chunk)
return digest.hexdigest()
def _is_jit_arg_mismatch(err: BaseException) -> bool:
return "args mismatch in JIT" in str(err)
class TinygradSupercomboRunner(ModelRunner):
uses_opencl_warp: bool = False
def __init__(self):
super().__init__()
self._constants = SplitModelConstants
self._parser = PhaseParser()
if len(self.models) != 1:
raise ValueError(f"supercombo bundle must have exactly one artifact, got {list(self.models)}")
self._model_data = next(iter(self.models.values()))
pkl_path = os.path.join(CUSTOM_MODEL_PATH, self._model_data.model.artifact.fileName)
self._pkl_path = pkl_path
self._expected_sha256 = getattr(getattr(self._model_data.model.artifact, "downloadUri", None), "sha256", "") or ""
self._verify_artifact_file()
with open(pkl_path, 'rb') as f:
self._m: dict[Any, Any] = pickle.load(f)
self._meta = self._m['metadata']
self._ish = self._meta['input_shapes']
self._slices = {k: v for k, v in self._meta['output_slices'].items() if k != 'pad'}
self._hidden_slice = self._meta['output_slices']['hidden_state']
self._run_policy = self._m['run_policy']
self._warp_jits: dict[tuple[int, int], Any] = {k: v for k, v in self._m.items() if isinstance(k, tuple)}
if not self._warp_jits:
raise ValueError("supercombo pkl has no warp JITs")
self._frame_skip = int(self._m.get('frame_skip', 4))
self._validate_warp_jits(pkl_path)
self._validate_jit_names()
self._queues: dict[str, Any] | None = None
self._npy: dict[str, np.ndarray] | None = None
self._cam: tuple[int, int] | None = None
self._prev_desire = np.zeros(self._ish['desire_pulse'][2], dtype=np.float32)
self._blob_cache: dict[tuple[str, int], Any] = {}
def _verify_artifact_file(self) -> None:
if not self._expected_sha256:
return
actual_sha256 = _file_sha256(self._pkl_path)
if actual_sha256 == self._expected_sha256:
return
try:
os.remove(self._pkl_path)
except OSError:
pass
redownload_msg = self._schedule_active_bundle_redownload()
raise RuntimeError(
"supercombo artifact SHA mismatch: "
f"expected {self._expected_sha256}, got {actual_sha256} for {self._pkl_path}. "
f"Deleted the stale cached file{redownload_msg}."
)
def _validate_warp_jits(self, pkl_path: str) -> None:
img = self._ish['img']
n_frames = img[1] // 6
expected_depth = self._frame_skip * (n_frames - 1) + 1
expected_device = os.getenv('DEV')
mismatches: list[str] = []
for cam, warp_jit in sorted(self._warp_jits.items()):
captured_depth = _captured_queue_depth(warp_jit)
captured_devices = _captured_devices(warp_jit)
if captured_depth is not None and captured_depth != expected_depth:
mismatches.append(
f"{cam[0]}x{cam[1]} queue-depth captured={captured_depth} expected={expected_depth}"
)
if expected_device and captured_devices and expected_device not in captured_devices:
mismatches.append(
f"{cam[0]}x{cam[1]} device captured={sorted(captured_devices)} expected={expected_device}"
)
if mismatches:
details = "; ".join(mismatches)
raise RuntimeError(
"supercombo warp JIT compatibility mismatch: "
f"{details}. Bundle {pkl_path} was compiled with the wrong backend, frame_skip, or queue shape; "
"re-download or rebuild this model artifact."
)
def _validate_jit_names(self) -> None:
expected_warp_names = ['big_frame', 'big_tfm', 'frame', 'tfm']
expected_policy_names = ['big_img_q', 'desire_q', 'feat_q', 'img_q', 'packed_npy_inputs', 'warped']
mismatches: list[str] = []
policy_names = sorted(_captured_expected_names(self._run_policy))
if policy_names and policy_names != expected_policy_names:
mismatches.append(f"run_policy captured={policy_names} expected={expected_policy_names}")
for cam, warp_jit in sorted(self._warp_jits.items()):
warp_names = sorted(_captured_expected_names(warp_jit))
if warp_names and warp_names != expected_warp_names:
mismatches.append(f"{cam[0]}x{cam[1]} warp captured={warp_names} expected={expected_warp_names}")
if mismatches:
details = "; ".join(mismatches)
actual_sha = None
try:
actual_sha = _file_sha256(self._pkl_path)
except OSError:
pass
if actual_sha and self._expected_sha256 and actual_sha != self._expected_sha256:
try:
os.remove(self._pkl_path)
except OSError:
pass
redownload_msg = self._schedule_active_bundle_redownload()
raise RuntimeError(
"supercombo artifact contract mismatch with stale cached SHA: "
f"{details}. Expected SHA {self._expected_sha256}, got {actual_sha}. "
f"Deleted the stale cached file{redownload_msg}."
)
raise RuntimeError(
"supercombo artifact JIT argument mismatch: "
f"{details}. This model file does not match the current IQPilot runtime contract. "
"Re-download or rebuild this model artifact."
)
def _handle_runtime_jit_mismatch(self, err: BaseException) -> None:
if not _is_jit_arg_mismatch(err):
raise err
actual_sha = None
try:
actual_sha = _file_sha256(self._pkl_path)
except OSError:
pass
if actual_sha and self._expected_sha256 and actual_sha != self._expected_sha256:
try:
os.remove(self._pkl_path)
except OSError:
pass
redownload_msg = self._schedule_active_bundle_redownload()
raise RuntimeError(
"supercombo artifact runtime JIT mismatch with stale cached SHA: "
f"expected {self._expected_sha256}, got {actual_sha} for {self._pkl_path}. "
f"Deleted the stale cached file{redownload_msg}."
) from err
raise RuntimeError(
"supercombo artifact runtime JIT mismatch: "
f"{err}. This model file does not match the current IQPilot runtime contract. "
"Re-download or rebuild this model artifact."
) from err
def _schedule_active_bundle_redownload(self) -> str:
try:
params = Params()
active_bundle = params.get("ModelManager_ActiveBundle") or {}
index = active_bundle.get("index") if isinstance(active_bundle, dict) else None
if isinstance(index, str) and index.isdigit():
index = int(index)
if isinstance(index, int) and index >= 0:
params.put("ModelManager_DownloadIndex", str(index))
params.remove("ModelRunnerTypeCache")
return "; scheduled automatic re-download of the active model"
except Exception:
pass
return "; unable to schedule automatic re-download"
def _frame_tensor(self, key: str, buf):
Tensor, Device = _tinygrad_imports()
arr = np.frombuffer(buf.data, dtype=np.uint8)
ck = (key, arr.ctypes.data)
t = self._blob_cache.get(ck)
if t is None:
t = Tensor.from_blob(arr.ctypes.data, (arr.size,), dtype='uint8', device=Device.DEFAULT)
self._blob_cache[ck] = t
return t
@property
def vision_input_names(self) -> list[str]:
return ['img', 'big_img']
@property
def input_shapes(self) -> ShapeDict:
return dict(self._ish)
@property
def output_slices(self) -> SliceDict:
return dict(self._slices)
def prepare_inputs(self, imgs_cl, numpy_inputs, frames):
raise RuntimeError("supercombo runner has no OpenCL path; use run_fused()")
def _ensure_queues(self, cam_w: int, cam_h: int) -> None:
if self._queues is not None and self._cam == (cam_w, cam_h):
return
if (cam_w, cam_h) not in self._warp_jits:
raise RuntimeError(f"no warp JIT for {cam_w}x{cam_h}; have {sorted(self._warp_jits)}")
Tensor, Device = _tinygrad_imports()
fs = self._frame_skip
img = self._ish['img']
n_frames = img[1] // 6
img_buf = (fs * (n_frames - 1) + 1, 6, img[2], img[3])
fb = self._ish['features_buffer']
dp = self._ish['desire_pulse']
tc = self._ish['traffic_convention']
at = self._ish['action_t']
zeros_u8 = lambda s: Tensor(np.zeros(s, dtype=np.uint8), device=Device.DEFAULT).contiguous().realize()
zeros_f32 = lambda s: Tensor(np.zeros(s, dtype=np.float32), device=Device.DEFAULT).contiguous().realize()
shapes = {'desire': (dp[2],), 'traffic_convention': tuple(tc), 'action_t': tuple(at), 'prev_feat': (fb[0], fb[2])}
sizes = [math.prod(s) for s in shapes.values()]
packed = np.zeros(sum(sizes), dtype=np.float32)
views = {k: v.reshape(s) for (k, s), v in zip(shapes.items(), np.split(packed, np.cumsum(sizes[:-1])), strict=True)}
self._npy = {'tfm': np.zeros((3, 3), dtype=np.float32), 'big_tfm': np.zeros((3, 3), dtype=np.float32), **views}
self._queues = {
'img_q': zeros_u8(img_buf),
'big_img_q': zeros_u8(img_buf),
'feat_q': zeros_f32((fs * fb[1], fb[0], fb[2])),
'desire_q': zeros_f32((fs * dp[1], dp[0], dp[2])),
'tfm': Tensor(self._npy['tfm'], device='NPY'),
'big_tfm': Tensor(self._npy['big_tfm'], device='NPY'),
'packed_npy_inputs': Tensor(packed, device='NPY'),
}
self._cam = (cam_w, cam_h)
def run_fused(self, bufs: dict, transforms: dict[str, np.ndarray], numpy_inputs: NumpyDict) -> NumpyDict:
Tensor, Device = _tinygrad_imports()
main_buf = bufs['img']
self._ensure_queues(main_buf.width, main_buf.height)
assert self._queues is not None and self._npy is not None
self._npy['tfm'][:] = transforms['img']
self._npy['big_tfm'][:] = transforms['big_img']
desire_key = next((k for k in numpy_inputs if k.startswith('desire')), None)
cur = numpy_inputs[desire_key].copy() if desire_key is not None else np.zeros_like(self._prev_desire)
cur[0] = 0
self._npy['desire'][:] = np.where(cur - self._prev_desire > .99, cur, 0)
self._prev_desire[:] = cur
if 'traffic_convention' in numpy_inputs:
self._npy['traffic_convention'][:] = numpy_inputs['traffic_convention']
if 'action_t' in numpy_inputs:
self._npy['action_t'][:] = numpy_inputs['action_t']
frame = self._frame_tensor('img', bufs['img'])
big_frame = self._frame_tensor('big_img', bufs['big_img'])
warp = self._warp_jits[self._cam]
try:
warped = warp(tfm=self._queues['tfm'], big_tfm=self._queues['big_tfm'], frame=frame, big_frame=big_frame)
out, = self._run_policy(warped=warped, img_q=self._queues['img_q'], big_img_q=self._queues['big_img_q'],
feat_q=self._queues['feat_q'], desire_q=self._queues['desire_q'],
packed_npy_inputs=self._queues['packed_npy_inputs'])
except Exception as err:
self._handle_runtime_jit_mismatch(err)
raise
flat = out.numpy().flatten()
self._npy['prev_feat'][:] = flat[self._hidden_slice].reshape(self._npy['prev_feat'].shape)
sliced = {k: flat[np.newaxis, sl] for k, sl in self._slices.items()}
return self._parser.parse_vision_outputs(sliced)
def _run_model(self) -> NumpyDict:
raise RuntimeError("supercombo path goes through run_fused(), not _run_model()")

190
artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/iqmodeld/models/runners/tinygrad/tinygrad_runner.py Normal file
View File

@@ -0,0 +1,190 @@
"""
Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos/
"""
from __future__ import annotations
import pickle
from dataclasses import dataclass
import numpy as np
from tinygrad.dtype import dtypes
from tinygrad.tensor import Tensor
from iqpilot.selfdrive.iqmodeld.models.runners.model_runner import (
CLMemDict,
CUSTOM_MODEL_PATH,
FrameDict,
ModelType,
NumpyDict,
ShapeDict,
SliceDict,
)
from iqpilot.selfdrive.iqmodeld.models.runners.model_runner import ModelRunner
from iqpilot.selfdrive.iqmodeld.models.runners.tinygrad.model_types import (
OffPolicyTinygrad,
OnPolicyTinygrad,
PolicyTinygrad,
SupercomboTinygrad,
VisionTinygrad,
)
from iqpilot.selfdrive.iqmodeld.models.split_model_constants import SplitModelConstants
from iqpilot.selfdrive.iqmodeld.config import ModelConstants
from iqpilot.selfdrive.iqmodeld.runtime.tinygrad import qcom_tensor_from_opencl_address
from iqpilot.system.hardware import TICI
@dataclass(frozen=True)
class _TensorShapePlan:
dtype: object
device: str
def _artifact_path(filename: str) -> str:
return f"{CUSTOM_MODEL_PATH}/{filename}"
def _load_program_blob(filename: str):
with open(_artifact_path(filename), "rb") as artifact:
try:
return pickle.load(artifact)
except FileNotFoundError as exc:
assert "/dev/kgsl-3d0" not in str(exc), "Model was built on C3 or C3X, but is being loaded on PC"
raise
def _compile_input_plan(captured) -> dict[str, _TensorShapePlan]:
plan: dict[str, _TensorShapePlan] = {}
for name, info in zip(captured.expected_names, captured.expected_input_info, strict=True):
plan[name] = _TensorShapePlan(dtype=info[2], device=info[3])
return plan
def _merge_step_outputs(output_groups: list[NumpyDict]) -> NumpyDict:
stitched: NumpyDict = {}
for payload in output_groups:
stitched.update(payload)
if "planplus" in stitched and "plan" in stitched:
stitched["plan"] = stitched["plan"] + stitched["planplus"]
return stitched
class TinygradRunner(ModelRunner, SupercomboTinygrad, PolicyTinygrad, VisionTinygrad, OffPolicyTinygrad, OnPolicyTinygrad):
def __init__(self, model_type: int = ModelType.supercombo):
ModelRunner.__init__(self)
for initializer in (SupercomboTinygrad, PolicyTinygrad, VisionTinygrad, OffPolicyTinygrad, OnPolicyTinygrad):
initializer.__init__(self)
self._constants = ModelConstants
self._model_data = self.models.get(model_type)
if self._model_data is None or self._model_data.model is None:
raise ValueError(f"Model data for type {model_type} not available.")
asset_name = self._model_data.model.artifact.fileName
assert asset_name.endswith("_tinygrad.pkl"), f"Invalid model file {asset_name} for TinygradRunner"
self.model_run = _load_program_blob(asset_name)
self._input_plan = _compile_input_plan(self.model_run.captured)
for name, spec in self._input_plan.items():
if "img" in name and spec.dtype is not dtypes.uint8:
raise ValueError(f"{asset_name}: image input {name} expects {spec.dtype}, incompatible with uint8 warp buffer")
self.input_to_dtype = {name: spec.dtype for name, spec in self._input_plan.items()}
self.input_to_device = {name: spec.device for name, spec in self._input_plan.items()}
@property
def vision_input_names(self) -> list[str]:
return [stream_name for stream_name in self.input_shapes if "img" in stream_name]
def _attach_vision_tensor(self, stream_name: str, frame_buffers: CLMemDict, frame_views: FrameDict) -> None:
spec = self._input_plan[stream_name]
frame_buffer = frame_buffers[stream_name]
if TICI:
self.inputs[stream_name] = qcom_tensor_from_opencl_address(frame_buffer.mem_address,
self.input_shapes[stream_name],
dtype=spec.dtype)
return
mirrored = frame_views[stream_name].as_numpy(frame_buffer).reshape(self.input_shapes[stream_name])
self.inputs[stream_name] = Tensor(mirrored, device=spec.device, dtype=spec.dtype).realize()
def _attach_state_tensor(self, tensor_name: str, tensor_value: np.ndarray) -> None:
spec = self._input_plan[tensor_name]
self.inputs[tensor_name] = Tensor(tensor_value, device=spec.device, dtype=spec.dtype).realize()
def prepare_vision_inputs(self, imgs_cl: CLMemDict, frames: FrameDict):
for stream_name in imgs_cl:
if stream_name not in self.inputs or not TICI:
self._attach_vision_tensor(stream_name, imgs_cl, frames)
def prepare_policy_inputs(self, numpy_inputs: NumpyDict):
for tensor_name, tensor_value in numpy_inputs.items():
self._attach_state_tensor(tensor_name, tensor_value)
def prepare_inputs(self, imgs_cl: CLMemDict, numpy_inputs: NumpyDict, frames: FrameDict) -> dict:
self.prepare_vision_inputs(imgs_cl, frames)
self.prepare_policy_inputs(numpy_inputs)
return self.inputs
def _parse_outputs(self, model_outputs: np.ndarray) -> NumpyDict:
if self._model_data is None:
raise ValueError("Model data is not available. Ensure the model is loaded correctly.")
return self.parser_method_dict[self._model_data.model.type.raw](model_outputs)
def _run_model(self) -> NumpyDict:
raw_output = self.model_run(**self.inputs).numpy().reshape(-1)
return self._parse_outputs(raw_output)
class TinygradSplitRunner(ModelRunner):
def __init__(self):
super().__init__()
self.is_20hz_3d = True
self._constants = SplitModelConstants
self.vision_runner = TinygradRunner(ModelType.vision)
self.policy_runner = TinygradRunner(ModelType.policy) if self.models.get(ModelType.policy) else None
self.off_policy_runner = TinygradRunner(ModelType.offPolicy) if self.models.get(ModelType.offPolicy) else None
self.on_policy_runner = TinygradRunner(ModelType.onPolicy) if self.models.get(ModelType.onPolicy) else None
def _policy_units(self) -> list[TinygradRunner]:
return [runner for runner in (self.policy_runner, self.off_policy_runner, self.on_policy_runner) if runner is not None]
def run_vision(self) -> NumpyDict:
return self.vision_runner.run_model()
def run_policy(self) -> NumpyDict:
return _merge_step_outputs([runner.run_model() for runner in self._policy_units()])
def refresh_policy_features(self, features_buffer: np.ndarray) -> None:
for runner in self._policy_units():
if "features_buffer" in runner._input_plan:
runner._attach_state_tensor("features_buffer", features_buffer)
def _run_model(self) -> NumpyDict:
return _merge_step_outputs([self.run_vision(), self.run_policy()])
@property
def vision_input_names(self) -> list[str]:
return list(self.vision_runner.vision_input_names)
@property
def input_shapes(self) -> ShapeDict:
composite: ShapeDict = dict(self.vision_runner.input_shapes)
for runner in self._policy_units():
composite.update(runner.input_shapes)
return composite
@property
def output_slices(self) -> SliceDict:
composite: SliceDict = dict(self.vision_runner.output_slices)
for runner in self._policy_units():
composite.update(runner.output_slices)
return composite
def prepare_inputs(self, imgs_cl: CLMemDict, numpy_inputs: NumpyDict, frames: FrameDict) -> dict:
self.vision_runner.prepare_vision_inputs(imgs_cl, frames)
assembled_inputs = dict(self.vision_runner.inputs)
for runner in self._policy_units():
runner.prepare_policy_inputs(numpy_inputs)
assembled_inputs.update(runner.inputs)
self.inputs = assembled_inputs
return assembled_inputs

89
artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/iqmodeld/models/split_model_constants.py Normal file
View File

@@ -0,0 +1,89 @@
import numpy as np
def index_function(idx, max_val=192, max_idx=32):
return max_val * ((idx/max_idx)**2)
class SplitModelConstants:
IDX_N = 33
T_IDXS = [index_function(idx, max_val=10.0) for idx in range(IDX_N)]
X_IDXS = [index_function(idx, max_val=192.0) for idx in range(IDX_N)]
LEAD_T_IDXS = [0., 2., 4., 6., 8., 10.]
LEAD_T_OFFSETS = [0., 2., 4.]
META_T_IDXS = [2., 4., 6., 8., 10.]
MODEL_FREQ = 20
HISTORY_FREQ = 5
HISTORY_LEN_SECONDS = 5
TEMPORAL_SKIP = MODEL_FREQ // HISTORY_FREQ
FULL_HISTORY_BUFFER_LEN = MODEL_FREQ * HISTORY_LEN_SECONDS
INPUT_HISTORY_BUFFER_LEN = HISTORY_FREQ * HISTORY_LEN_SECONDS
FEATURE_LEN = 512
DESIRE_LEN = 8
TRAFFIC_CONVENTION_LEN = 2
LAT_PLANNER_STATE_LEN = 4
LATERAL_CONTROL_PARAMS_LEN = 2
PREV_DESIRED_CURV_LEN = 1
FCW_THRESHOLDS_5MS2 = np.array([.05, .05, .15, .15, .15], dtype=np.float32)
FCW_THRESHOLDS_3MS2 = np.array([.7, .7], dtype=np.float32)
FCW_5MS2_PROBS_WIDTH = 5
FCW_3MS2_PROBS_WIDTH = 2
DISENGAGE_WIDTH = 5
POSE_WIDTH = 6
WIDE_FROM_DEVICE_WIDTH = 3
LEAD_WIDTH = 4
LANE_LINES_WIDTH = 2
ROAD_EDGES_WIDTH = 2
PLAN_WIDTH = 15
DESIRE_PRED_WIDTH = 8
LAT_PLANNER_SOLUTION_WIDTH = 4
DESIRED_CURV_WIDTH = 1
ACTION_WIDTH = 2
NUM_LANE_LINES = 4
NUM_ROAD_EDGES = 2
LEAD_TRAJ_LEN = 6
DESIRE_PRED_LEN = 4
PLAN_MHP_N = 5
LEAD_MHP_N = 2
PLAN_MHP_SELECTION = 1
LEAD_MHP_SELECTION = 3
FCW_THRESHOLD_5MS2_HIGH = 0.15
FCW_THRESHOLD_5MS2_LOW = 0.05
FCW_THRESHOLD_3MS2 = 0.7
CONFIDENCE_BUFFER_LEN = 5
RYG_GREEN = 0.01165
RYG_YELLOW = 0.06157
POLY_PATH_DEGREE = 4
class Plan:
POSITION = slice(0, 3)
VELOCITY = slice(3, 6)
ACCELERATION = slice(6, 9)
T_FROM_CURRENT_EULER = slice(9, 12)
ORIENTATION_RATE = slice(12, 15)
class Meta:
ENGAGED = slice(0, 1)
GAS_DISENGAGE = slice(1, 31, 6)
BRAKE_DISENGAGE = slice(2, 31, 6)
STEER_OVERRIDE = slice(3, 31, 6)
HARD_BRAKE_3 = slice(4, 31, 6)
HARD_BRAKE_4 = slice(5, 31, 6)
HARD_BRAKE_5 = slice(6, 31, 6)
GAS_PRESS = slice(31, 55, 4)
BRAKE_PRESS = slice(32, 55, 4)
LEFT_BLINKER = slice(33, 55, 4)
RIGHT_BLINKER = slice(34, 55, 4)

219
artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/iqmodeld/parser.py Normal file
View File

@@ -0,0 +1,219 @@
from __future__ import annotations
from dataclasses import dataclass
import numpy as np
from iqpilot.selfdrive.iqmodeld.models.split_model_constants import SplitModelConstants
from iqpilot.selfdrive.iqmodeld.config import ModelConstants
def safe_exp(values, out=None):
return np.exp(np.clip(values, -np.inf, 11), out=out)
def sigmoid(values):
return 1.0 / (1.0 + safe_exp(-values))
def _softmax_last(values, axis=-1):
values -= np.max(values, axis=axis, keepdims=True)
if values.dtype in (np.float32, np.float64):
safe_exp(values, out=values)
else:
values = safe_exp(values)
values /= np.sum(values, axis=axis, keepdims=True)
return values
@dataclass(frozen=True)
class _MixtureRecipe:
input_heads: int
output_heads: int
final_shape: tuple[int, ...]
class _TensorKitchen:
def __init__(self, ignore_missing: bool = False):
self.ignore_missing = ignore_missing
def _grab(self, outputs: dict[str, np.ndarray], tensor_name: str) -> np.ndarray | None:
if tensor_name not in outputs:
if not self.ignore_missing:
raise ValueError(f"Missing output {tensor_name}")
return
return outputs[tensor_name]
def categorical(self, outputs: dict[str, np.ndarray], tensor_name: str, shape=None) -> None:
raw = self._grab(outputs, tensor_name)
if raw is None:
return
if shape is not None:
raw = raw.reshape((raw.shape[0],) + shape)
outputs[tensor_name] = _softmax_last(raw, axis=-1)
def binary(self, outputs: dict[str, np.ndarray], tensor_name: str) -> None:
raw = self._grab(outputs, tensor_name)
if raw is None:
return
outputs[tensor_name] = sigmoid(raw)
def mixture(self, outputs: dict[str, np.ndarray], tensor_name: str, recipe: _MixtureRecipe) -> None:
raw = self._grab(outputs, tensor_name)
if raw is None:
return
reshaped = raw.reshape((raw.shape[0], max(recipe.input_heads, 1), -1))
value_count = (reshaped.shape[2] - recipe.output_heads) // 2
means = reshaped[:, :, :value_count]
stds = safe_exp(reshaped[:, :, value_count:2 * value_count])
if recipe.input_heads > 1:
weights = np.zeros((reshaped.shape[0], recipe.input_heads, recipe.output_heads), dtype=reshaped.dtype)
for output_idx in range(recipe.output_heads):
weights[:, :, output_idx - recipe.output_heads] = _softmax_last(
reshaped[:, :, output_idx - recipe.output_heads], axis=-1
)
if recipe.output_heads == 1:
for batch_idx in range(weights.shape[0]):
order = np.argsort(weights[batch_idx][:, 0])[::-1]
weights[batch_idx] = weights[batch_idx][order]
means[batch_idx] = means[batch_idx][order]
stds[batch_idx] = stds[batch_idx][order]
hypothesis_shape = (reshaped.shape[0], recipe.input_heads, *recipe.final_shape)
outputs[f"{tensor_name}_weights"] = weights
outputs[f"{tensor_name}_hypotheses"] = means.reshape(hypothesis_shape)
outputs[f"{tensor_name}_stds_hypotheses"] = stds.reshape(hypothesis_shape)
picked_means = np.zeros((reshaped.shape[0], recipe.output_heads, value_count), dtype=reshaped.dtype)
picked_stds = np.zeros((reshaped.shape[0], recipe.output_heads, value_count), dtype=reshaped.dtype)
for batch_idx in range(weights.shape[0]):
for output_idx in range(recipe.output_heads):
order = np.argsort(weights[batch_idx, :, output_idx])[::-1]
picked_means[batch_idx, output_idx] = means[batch_idx, order[0]]
picked_stds[batch_idx, output_idx] = stds[batch_idx, order[0]]
else:
picked_means = means
picked_stds = stds
final_shape = ((reshaped.shape[0], recipe.output_heads, *recipe.final_shape)
if recipe.output_heads > 1 else (reshaped.shape[0], *recipe.final_shape))
outputs[tensor_name] = picked_means.reshape(final_shape)
outputs[f"{tensor_name}_stds"] = picked_stds.reshape(final_shape)
class ArchiveParser(_TensorKitchen):
def __init__(self, ignore_missing: bool = False):
super().__init__(ignore_missing=ignore_missing)
self._c = ModelConstants
def _recipes(self) -> list[tuple[str, _MixtureRecipe]]:
c = self._c
return [
("plan", _MixtureRecipe(c.PLAN_MHP_N, c.PLAN_MHP_SELECTION, (c.IDX_N, c.PLAN_WIDTH))),
("lane_lines", _MixtureRecipe(0, 0, (c.NUM_LANE_LINES, c.IDX_N, c.LANE_LINES_WIDTH))),
("road_edges", _MixtureRecipe(0, 0, (c.NUM_ROAD_EDGES, c.IDX_N, c.LANE_LINES_WIDTH))),
("pose", _MixtureRecipe(0, 0, (c.POSE_WIDTH,))),
("road_transform", _MixtureRecipe(0, 0, (c.POSE_WIDTH,))),
("wide_from_device_euler", _MixtureRecipe(0, 0, (c.WIDE_FROM_DEVICE_WIDTH,))),
("lead", _MixtureRecipe(c.LEAD_MHP_N, c.LEAD_MHP_SELECTION, (c.LEAD_TRAJ_LEN, c.LEAD_WIDTH))),
]
def parse_outputs(self, outputs: dict[str, np.ndarray]) -> dict[str, np.ndarray]:
c = self._c
for tensor_name, recipe in self._recipes():
self.mixture(outputs, tensor_name, recipe)
if "sim_pose" in outputs:
self.mixture(outputs, "sim_pose", _MixtureRecipe(0, 0, (c.POSE_WIDTH,)))
if "lat_planner_solution" in outputs:
self.mixture(outputs, "lat_planner_solution", _MixtureRecipe(0, 0, (c.IDX_N, c.LAT_PLANNER_SOLUTION_WIDTH)))
if "desired_curvature" in outputs:
self.mixture(outputs, "desired_curvature", _MixtureRecipe(0, 0, (c.DESIRED_CURV_WIDTH,)))
for name in ("lead_prob", "lane_lines_prob", "meta"):
self.binary(outputs, name)
self.categorical(outputs, "desire_state", shape=(c.DESIRE_PRED_WIDTH,))
self.categorical(outputs, "desire_pred", shape=(c.DESIRE_PRED_LEN, c.DESIRE_PRED_WIDTH))
return outputs
class PhaseParser(_TensorKitchen):
def __init__(self, ignore_missing: bool = False):
super().__init__(ignore_missing=ignore_missing)
self._c = SplitModelConstants
def _has_mixture_heads(self, outputs: dict[str, np.ndarray], tensor_name: str, flat_width: int) -> bool:
raw = self._grab(outputs, tensor_name)
if raw is None:
return False
return raw.shape[1] != 2 * flat_width
def _decode_dynamic_family(self, outputs: dict[str, np.ndarray]) -> None:
c = self._c
if "lead" in outputs:
uses_heads = self._has_mixture_heads(outputs, "lead", c.LEAD_MHP_SELECTION * c.LEAD_TRAJ_LEN * c.LEAD_WIDTH)
self.mixture(outputs, "lead", _MixtureRecipe(
c.LEAD_MHP_N if uses_heads else 0,
c.LEAD_MHP_SELECTION if uses_heads else 0,
(c.LEAD_TRAJ_LEN, c.LEAD_WIDTH) if uses_heads else (c.LEAD_MHP_SELECTION, c.LEAD_TRAJ_LEN, c.LEAD_WIDTH),
))
if "plan" in outputs:
uses_heads = self._has_mixture_heads(outputs, "plan", c.IDX_N * c.PLAN_WIDTH)
self.mixture(outputs, "plan", _MixtureRecipe(
c.PLAN_MHP_N if uses_heads else 0,
c.PLAN_MHP_SELECTION if uses_heads else 0,
(c.IDX_N, c.PLAN_WIDTH),
))
if "planplus" in outputs:
self.mixture(outputs, "planplus", _MixtureRecipe(0, 0, (c.IDX_N, c.PLAN_WIDTH)))
def _decode_policy_family(self, outputs: dict[str, np.ndarray]) -> None:
c = self._c
if "action" in outputs:
self.mixture(outputs, "action", _MixtureRecipe(0, 0, (c.ACTION_WIDTH,)))
if "desired_curvature" in outputs:
self.mixture(outputs, "desired_curvature", _MixtureRecipe(0, 0, (c.DESIRED_CURV_WIDTH,)))
if "desire_pred" in outputs:
self.categorical(outputs, "desire_pred", shape=(c.DESIRE_PRED_LEN, c.DESIRE_PRED_WIDTH))
if "desire_state" in outputs:
self.categorical(outputs, "desire_state", shape=(c.DESIRE_PRED_WIDTH,))
if "lane_lines" in outputs:
self.mixture(outputs, "lane_lines", _MixtureRecipe(0, 0, (c.NUM_LANE_LINES, c.IDX_N, c.LANE_LINES_WIDTH)))
if "lane_lines_prob" in outputs:
self.binary(outputs, "lane_lines_prob")
if "lead_prob" in outputs:
self.binary(outputs, "lead_prob")
if "lat_planner_solution" in outputs:
self.mixture(outputs, "lat_planner_solution", _MixtureRecipe(0, 0, (c.IDX_N, c.LAT_PLANNER_SOLUTION_WIDTH)))
if "meta" in outputs:
self.binary(outputs, "meta")
if "road_edges" in outputs:
self.mixture(outputs, "road_edges", _MixtureRecipe(0, 0, (c.NUM_ROAD_EDGES, c.IDX_N, c.LANE_LINES_WIDTH)))
if "sim_pose" in outputs:
self.mixture(outputs, "sim_pose", _MixtureRecipe(0, 0, (c.POSE_WIDTH,)))
def parse_vision_outputs(self, outputs: dict[str, np.ndarray]) -> dict[str, np.ndarray]:
c = self._c
self.mixture(outputs, "pose", _MixtureRecipe(0, 0, (c.POSE_WIDTH,)))
self.mixture(outputs, "wide_from_device_euler", _MixtureRecipe(0, 0, (c.WIDE_FROM_DEVICE_WIDTH,)))
self.mixture(outputs, "road_transform", _MixtureRecipe(0, 0, (c.POSE_WIDTH,)))
self._decode_dynamic_family(outputs)
self._decode_policy_family(outputs)
return outputs
def parse_policy_outputs(self, outputs: dict[str, np.ndarray]) -> dict[str, np.ndarray]:
self._decode_dynamic_family(outputs)
self._decode_policy_family(outputs)
return outputs
def parse_outputs(self, outputs: dict[str, np.ndarray]) -> dict[str, np.ndarray]:
return self.parse_policy_outputs(self.parse_vision_outputs(outputs))
__all__ = [
"ArchiveParser",
"PhaseParser",
]

0
iqdbc_repo/iqdbc/car/ford/__init__.py → artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/iqmodeld/runtime/__init__.py
View File

23
artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/iqmodeld/runtime/tinygrad.py Normal file
View File

@@ -0,0 +1,23 @@
"""
Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos/
"""
from tinygrad.tensor import Tensor
from tinygrad.helpers import to_mv
_PTR_STRIDE = 8
_RAW_GPU_PTR_SLOT = 20
_RAW_GPU_PTR_VIEW_BYTES = 0x100
def _descriptor_pointer(opencl_address: int) -> int:
return to_mv(opencl_address, _PTR_STRIDE).cast("Q")[0]
def _raw_gpu_pointer(descriptor_pointer: int) -> int:
return to_mv(descriptor_pointer, _RAW_GPU_PTR_VIEW_BYTES).cast("Q")[_RAW_GPU_PTR_SLOT]
def qcom_tensor_from_opencl_address(opencl_address, shape, dtype):
descriptor_pointer = _descriptor_pointer(opencl_address)
device_pointer = _raw_gpu_pointer(descriptor_pointer)
return Tensor.from_blob(device_pointer, shape, dtype=dtype, device="QCOM")

131
artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/iqmodeld/temporal_state.py Normal file
View File

@@ -0,0 +1,131 @@
"""
Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos/
"""
from __future__ import annotations
import math
import numpy as np
DEFAULT_FRAME_SKIP = 4
MODEL_INPUT_SPEC: dict[str, tuple[tuple[int, ...], str]] = {
"img": ((1, 12, 128, 256), "uint8"),
"big_img": ((1, 12, 128, 256), "uint8"),
"desire_pulse": ((1, 25, 8), "float32"),
"traffic_convention": ((1, 2), "float32"),
"features_buffer": ((1, 24, 512), "float32"),
"action_t": ((1, 2), "float32"),
}
def spec_from_meta(meta: dict) -> dict[str, tuple[tuple[int, ...], str]] | None:
shapes = meta.get("input_shapes")
if not shapes:
return None
return {name: (tuple(shape), "uint8" if name in ("img", "big_img") else "float32")
for name, shape in shapes.items()}
class TemporalInputState:
def __init__(self, frame_skip: int, spec: dict[str, tuple[tuple[int, ...], str]] = MODEL_INPUT_SPEC):
self.frame_skip = frame_skip
img = spec["img"][0]
fb = spec["features_buffer"][0]
dp = spec["desire_pulse"][0]
self.n_frames = img[1] // 6
img_q_shape = (frame_skip * (self.n_frames - 1) + 1, 6, img[2], img[3])
self._img_shape = img
self._fb_shape = fb
self._dp_shape = dp
feat_dim = math.prod(fb[2:])
self.img_q = np.zeros(img_q_shape, dtype=np.uint8)
self.big_img_q = np.zeros(img_q_shape, dtype=np.uint8)
self.feat_q = np.zeros((frame_skip * fb[1], fb[0], feat_dim), dtype=np.float32)
self.desire_q = np.zeros((frame_skip * dp[1], dp[0], dp[2]), dtype=np.float32)
self.prev_desire = np.zeros(dp[2], dtype=np.float32)
self.prev_feat = np.zeros((fb[0], feat_dim), dtype=np.float32)
@staticmethod
def _shift_append(q: np.ndarray, new_val: np.ndarray) -> None:
q[:-1] = q[1:]
q[-1] = new_val
def push_and_materialize(self, warped: np.ndarray, desire_pulse: np.ndarray,
traffic_convention: np.ndarray, action_t: np.ndarray,
) -> dict[str, np.ndarray]:
fs = self.frame_skip
cur = desire_pulse.astype(np.float32).copy()
cur[0] = 0
pulse = np.where(cur - self.prev_desire > 0.99, cur, 0).astype(np.float32)
self.prev_desire[:] = cur
self._shift_append(self.img_q, warped[0])
self._shift_append(self.big_img_q, warped[1])
self._shift_append(self.desire_q, pulse.reshape(self._dp_shape[0], self._dp_shape[2]))
self._shift_append(self.feat_q, self.prev_feat)
dp = self._dp_shape
return {
"img": np.ascontiguousarray(self.img_q[::fs]).reshape(self._img_shape),
"big_img": np.ascontiguousarray(self.big_img_q[::fs]).reshape(self._img_shape),
"features_buffer": np.ascontiguousarray(self.feat_q[::fs]).reshape(self._fb_shape),
"desire_pulse": self.desire_q.reshape(dp[1], fs, dp[0], dp[2]).max(axis=1).reshape(dp),
"traffic_convention": traffic_convention.astype(np.float32).reshape(1, -1),
"action_t": action_t.astype(np.float32).reshape(1, -1),
}
def note_hidden_state(self, model_output: np.ndarray, hidden_slice: slice) -> None:
self.prev_feat[:] = model_output[hidden_slice].reshape(self.prev_feat.shape)
class SplitTemporalState:
def __init__(self, frame_skip: int, img_shape: tuple[int, ...],
feature_shape: tuple[int, ...], desire_shape: tuple[int, ...]):
self.frame_skip = frame_skip
self._img_shape = tuple(img_shape)
self._fb_shape = tuple(feature_shape)
self._dp_shape = tuple(desire_shape)
n_frames = img_shape[1] // 6
img_q_shape = (frame_skip * (n_frames - 1) + 1, 6, img_shape[2], img_shape[3])
self.img_q = np.zeros(img_q_shape, dtype=np.uint8)
self.big_img_q = np.zeros(img_q_shape, dtype=np.uint8)
self.feat_q = np.zeros((frame_skip * (feature_shape[1] - 1) + 1, feature_shape[0], feature_shape[2]),
dtype=np.float32)
self.desire_q = np.zeros((frame_skip * desire_shape[1], desire_shape[0], desire_shape[2]), dtype=np.float32)
self.prev_desire = np.zeros(desire_shape[2], dtype=np.float32)
def materialize_vision(self, warped: np.ndarray, desire: np.ndarray) -> dict[str, np.ndarray]:
fs = self.frame_skip
cur = desire.astype(np.float32).copy()
cur[0] = 0
pulse = np.where(cur - self.prev_desire > 0.99, cur, 0).astype(np.float32)
self.prev_desire[:] = cur
TemporalInputState._shift_append(self.img_q, warped[0])
TemporalInputState._shift_append(self.big_img_q, warped[1])
TemporalInputState._shift_append(self.desire_q, pulse.reshape(self._dp_shape[0], self._dp_shape[2]))
return {
"img": np.ascontiguousarray(self.img_q[::fs]).reshape(self._img_shape),
"big_img": np.ascontiguousarray(self.big_img_q[::fs]).reshape(self._img_shape),
}
def materialize_policy(self, vision_feature: np.ndarray, traffic_convention: np.ndarray,
action_t: np.ndarray | None = None) -> dict[str, np.ndarray]:
fs = self.frame_skip
TemporalInputState._shift_append(self.feat_q, vision_feature.reshape(self._fb_shape[0], self._fb_shape[2]))
dp = self._dp_shape
out = {
"features_buffer": np.ascontiguousarray(self.feat_q[::fs]).reshape(self._fb_shape),
"desire_pulse": self.desire_q.reshape(dp[1], fs, dp[0], dp[2]).max(axis=1).reshape(dp),
"traffic_convention": traffic_convention.astype(np.float32).reshape(1, -1),
}
if action_t is not None:
out["action_t"] = action_t.astype(np.float32).reshape(1, -1)
return out

0
iqdbc_repo/iqdbc/car/ford/tests/__init__.py → artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/iqmodeld/tools/__init__.py
View File

422
artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/iqmodeld/tools/compile_supercombo.py Normal file
View File

@@ -0,0 +1,422 @@
#!/usr/bin/env python3
"""
Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos/
"""
import argparse
import atexit
import math
import os
import pickle
import re
import tempfile
import time
from functools import partial
from collections import namedtuple
import numpy as np
def _patch_tinygrad_fetch_fw():
import hashlib
import pathlib
import zstandard
from tinygrad import helpers
_orig = helpers.fetch_fw
def fetch_fw(path, name, sha256):
p = pathlib.Path(f"/lib/firmware/{path}/{name}.zst")
if p.is_file():
blob = zstandard.ZstdDecompressor().stream_reader(p.read_bytes()).read()
if hashlib.sha256(blob).hexdigest() == sha256:
return blob
return _orig(path, name, sha256)
helpers.fetch_fw = fetch_fw
_patch_tinygrad_fetch_fw()
from tinygrad.tensor import Tensor
from tinygrad.helpers import Context
from tinygrad.device import Device
from tinygrad.engine.jit import TinyJit
NV12Frame = namedtuple("NV12Frame", ['width', 'height', 'stride', 'y_height', 'uv_height', 'size'])
WARP_INPUTS = ['tfm', 'big_tfm']
POLICY_INPUTS = ['img_q', 'big_img_q', 'feat_q', 'desire_q', 'packed_npy_inputs']
UV_SCALE_MATRIX = np.array([[0.5, 0, 0], [0, 0.5, 0], [0, 0, 1]], dtype=np.float32)
UV_SCALE_MATRIX_INV = np.linalg.inv(UV_SCALE_MATRIX)
WARP_DEV = os.getenv('WARP_DEV')
def make_random_images(keys, shape, device=None):
return {k: Tensor.randint(shape, low=0, high=256, dtype='uint8', device=device).realize() for k in keys}
def warp_perspective_tinygrad(src_flat, M_inv, dst_shape, src_shape, stride_pad, border_fill_val=None):
w_dst, h_dst = dst_shape
h_src, w_src = src_shape
x = Tensor.arange(w_dst).reshape(1, w_dst).expand(h_dst, w_dst).reshape(-1)
y = Tensor.arange(h_dst).reshape(h_dst, 1).expand(h_dst, w_dst).reshape(-1)
src_x = M_inv[0, 0] * x + M_inv[0, 1] * y + M_inv[0, 2]
src_y = M_inv[1, 0] * x + M_inv[1, 1] * y + M_inv[1, 2]
src_w = M_inv[2, 0] * x + M_inv[2, 1] * y + M_inv[2, 2]
src_x = src_x / src_w
src_y = src_y / src_w
x_round = Tensor.round(src_x)
y_round = Tensor.round(src_y)
x_nn_clipped = x_round.clip(0, w_src - 1).cast('int')
y_nn_clipped = y_round.clip(0, h_src - 1).cast('int')
idx = y_nn_clipped * (w_src + stride_pad) + x_nn_clipped
sampled = src_flat[idx]
if border_fill_val is None:
return sampled
in_bounds = ((x_round >= 0) & (x_round <= w_src - 1) &
(y_round >= 0) & (y_round <= h_src - 1)).cast(sampled.dtype)
return sampled * in_bounds + Tensor(border_fill_val, dtype=sampled.dtype) * (1 - in_bounds)
def frames_to_tensor(frames):
H = (frames.shape[0] * 2) // 3
W = frames.shape[1]
in_img1 = Tensor.cat(frames[0:H:2, 0::2],
frames[1:H:2, 0::2],
frames[0:H:2, 1::2],
frames[1:H:2, 1::2],
frames[H:H+H//4].reshape((H//2, W//2)),
frames[H+H//4:H+H//2].reshape((H//2, W//2)), dim=0).reshape((6, H//2, W//2))
return in_img1
def make_frame_prepare(nv12: NV12Frame, model_w, model_h):
cam_w, cam_h, stride, y_height, uv_height, _ = nv12
uv_offset = stride * y_height
stride_pad = stride - cam_w
def frame_prepare_tinygrad(input_frame, M_inv):
M_inv_uv = M_inv * Tensor([[1.0, 1.0, 0.5], [1.0, 1.0, 0.5], [2.0, 2.0, 1.0]], device=WARP_DEV)
uv = input_frame[uv_offset:uv_offset + uv_height * stride].reshape(uv_height, stride)
with Context(SPLIT_REDUCEOP=0):
y = warp_perspective_tinygrad(input_frame[:cam_h*stride],
M_inv, (model_w, model_h),
(cam_h, cam_w), stride_pad).realize()
u = warp_perspective_tinygrad(uv[:cam_h//2, :cam_w:2].flatten(),
M_inv_uv, (model_w//2, model_h//2),
(cam_h//2, cam_w//2), 0).realize()
v = warp_perspective_tinygrad(uv[:cam_h//2, 1:cam_w:2].flatten(),
M_inv_uv, (model_w//2, model_h//2),
(cam_h//2, cam_w//2), 0).realize()
yuv = y.cat(u).cat(v).reshape((model_h * 3 // 2, model_w))
tensor = frames_to_tensor(yuv)
return tensor
return frame_prepare_tinygrad
def make_warp_input_queues(vision_input_shapes, frame_skip, device):
img = vision_input_shapes['img'] # (1, 12, 128, 256)
n_frames = img[1] // 6
img_buf_shape = (frame_skip * (n_frames - 1) + 1, 6, img[2], img[3])
npy = {
'tfm': np.zeros((3, 3), dtype=np.float32),
'big_tfm': np.zeros((3, 3), dtype=np.float32),
}
input_queues = {
'img_q': Tensor(np.zeros(img_buf_shape, dtype=np.uint8), device=device).contiguous().realize(),
'big_img_q': Tensor(np.zeros(img_buf_shape, dtype=np.uint8), device=device).contiguous().realize(),
**{k: Tensor(v, device='NPY').realize() for k, v in npy.items()},
}
return input_queues, npy
def get_policy_npy_shapes(input_shapes):
dp = input_shapes['desire_pulse'] # (1, 25, 8)
tc = input_shapes['traffic_convention'] # (1, 2)
at = input_shapes['action_t'] # (1, 2)
fb = input_shapes['features_buffer'] # (1, 24, 512)
shapes = {'desire': (dp[2],), 'traffic_convention': tuple(tc), 'action_t': tuple(at), 'prev_feat': (fb[0], fb[2])}
return shapes, [math.prod(s) for s in shapes.values()]
def make_input_queues(input_shapes, frame_skip, device):
input_queues, npy = make_warp_input_queues(input_shapes, frame_skip, device)
fb = input_shapes['features_buffer'] # (1, 24, 512), past features only; the model appends the current frame's feature
dp = input_shapes['desire_pulse'] # (1, 25, 8)
shapes, sizes = get_policy_npy_shapes(input_shapes)
packed_npy_inputs = np.zeros(sum(sizes), dtype=np.float32)
npy.update({k: v.reshape(s) for (k, s), v in zip(shapes.items(), np.split(packed_npy_inputs, np.cumsum(sizes[:-1])), strict=True)})
input_queues.update({
'feat_q': Tensor(np.zeros((frame_skip * fb[1], fb[0], fb[2]), dtype=np.float32), device=device).contiguous().realize(),
'desire_q': Tensor(np.zeros((frame_skip * dp[1], dp[0], dp[2]), dtype=np.float32), device=device).contiguous().realize(),
'packed_npy_inputs': Tensor(packed_npy_inputs, device='NPY').realize(),
})
return input_queues, npy
def shift_and_sample(buf, new_val, sample_fn):
buf.assign(buf[1:].cat(new_val, dim=0).contiguous())
return sample_fn(buf)
def sample_skip(buf, frame_skip):
return buf[::frame_skip].contiguous().flatten(0, 1).unsqueeze(0)
def sample_desire(buf, frame_skip):
return buf.reshape(-1, frame_skip, *buf.shape[1:]).max(1).flatten(0, 1).unsqueeze(0)
def make_warp(nv12, model_w, model_h, frame_skip):
frame_prepare = make_frame_prepare(nv12, model_w, model_h)
def warp(tfm, big_tfm, frame, big_frame):
tfm = tfm.to(WARP_DEV)
big_tfm = big_tfm.to(WARP_DEV)
Tensor.realize(tfm, big_tfm)
warped_frame = frame_prepare(frame, tfm).unsqueeze(0)
warped_big_frame = frame_prepare(big_frame, big_tfm).unsqueeze(0)
return Tensor.cat(warped_frame, warped_big_frame)
return warp
def make_run_policy(model_runner, model_metadata, frame_skip):
sample_desire_fn = partial(sample_desire, frame_skip=frame_skip)
sample_skip_fn = partial(sample_skip, frame_skip=frame_skip)
npy_shapes, npy_sizes = get_policy_npy_shapes(model_metadata['input_shapes'])
def run_policy(warped, img_q, big_img_q, feat_q, desire_q, packed_npy_inputs):
packed_npy_inputs = packed_npy_inputs.to(Device.DEFAULT)
warped = warped.to(Device.DEFAULT)
Tensor.realize(packed_npy_inputs, warped)
img = shift_and_sample(img_q, warped[0:1], sample_skip_fn)
big_img = shift_and_sample(big_img_q, warped[1:2], sample_skip_fn)
desire, traffic_convention, action_t, prev_feat = (t.reshape(s) for t, s in zip(packed_npy_inputs.split(npy_sizes), npy_shapes.values(), strict=True))
desire_buf = shift_and_sample(desire_q, desire.reshape(1, 1, -1), sample_desire_fn)
feat_buf = shift_and_sample(feat_q, prev_feat.reshape(1, 1, -1), sample_skip_fn)
inputs = {
'img': img,
'big_img': big_img,
'features_buffer': feat_buf,
'desire_pulse': desire_buf,
'traffic_convention': traffic_convention,
'action_t': action_t,
}
out = next(iter(model_runner(inputs).values())).cast('float32')
return out,
return run_policy
def compile_jit(jit, make_random_inputs, input_keys, make_queues):
SEED = 42
def random_inputs_run(fn, seed, test_val=None, test_buffers=None, expect_match=True):
input_queues, npy = make_queues(Device.DEFAULT)
np.random.seed(seed)
Tensor.manual_seed(seed)
testing = test_val is not None or test_buffers is not None
n_runs = 1 if testing else 3
for i in range(n_runs):
for v in npy.values():
v[:] = np.random.randn(*v.shape).astype(v.dtype)
Device.default.synchronize()
random_inputs = make_random_inputs()
st = time.perf_counter()
outs = fn(**{k: input_queues[k] for k in input_keys}, **random_inputs)
mt = time.perf_counter()
Device.default.synchronize()
et = time.perf_counter()
print(f" [{i+1}/{n_runs}] enqueue {(mt-st)*1e3:6.2f} ms -- total {(et-st)*1e3:6.2f} ms")
if i == 0:
val = [np.copy(v.numpy()) for v in outs]
buffers = [np.copy(v.numpy().copy()) for v in input_queues.values()]
if test_val is not None:
match = all(np.array_equal(a, b) for a, b in zip(val, test_val, strict=True))
assert match == expect_match, f"outputs {'differ from' if expect_match else 'match'} baseline (seed={seed})"
if test_buffers is not None:
match = all(np.array_equal(a, b) for a, b in zip(buffers, test_buffers, strict=True))
assert match == expect_match, f"buffers {'differ from' if expect_match else 'match'} baseline (seed={seed})"
return val, buffers
print('capture + replay')
test_val, test_buffers = random_inputs_run(jit, SEED)
print('pickle round trip')
jit = pickle.loads(pickle.dumps(jit))
random_inputs_run(jit, SEED, test_val, test_buffers, expect_match=True)
random_inputs_run(jit, SEED+1, test_val, test_buffers, expect_match=False)
return jit
def _captured_devices(jit) -> set[str]:
captured = getattr(jit, 'captured', None)
infos = getattr(captured, 'expected_input_info', None)
if not infos:
return set()
devices: set[str] = set()
for info in infos:
if isinstance(info, tuple) and len(info) >= 4 and isinstance(info[3], str):
devices.add(info[3])
return devices
def _slice_outputs(model_outputs: np.ndarray, output_slices: dict[str, slice]) -> dict[str, np.ndarray]:
return {name: model_outputs[np.newaxis, tensor_slice] for name, tensor_slice in output_slices.items() if name != 'pad'}
def _validate_pose_outputs(parsed_outputs: dict[str, np.ndarray]) -> None:
from iqpilot.selfdrive.locationd.locationd import MIN_STD_SANITY_CHECK, ROTATION_SANITY_CHECK, TRANS_SANITY_CHECK
required = (
'pose', 'pose_stds', 'wide_from_device_euler', 'wide_from_device_euler_stds',
'road_transform', 'road_transform_stds',
)
missing = [name for name in required if name not in parsed_outputs]
if missing:
raise AssertionError(f"parsed supercombo outputs missing required odometry tensors: {missing}")
for name in required:
values = parsed_outputs[name]
if not np.isfinite(values).all():
raise AssertionError(f"parsed supercombo output {name} contains non-finite values")
pose = parsed_outputs['pose'][0]
pose_stds = parsed_outputs['pose_stds'][0]
road_transform_stds = parsed_outputs['road_transform_stds'][0]
wide_stds = parsed_outputs['wide_from_device_euler_stds'][0]
if pose_stds.min() <= MIN_STD_SANITY_CHECK:
raise AssertionError(f"pose_stds min {pose_stds.min()} <= {MIN_STD_SANITY_CHECK}")
if road_transform_stds.min() <= MIN_STD_SANITY_CHECK:
raise AssertionError(f"road_transform_stds min {road_transform_stds.min()} <= {MIN_STD_SANITY_CHECK}")
if wide_stds.min() <= MIN_STD_SANITY_CHECK:
raise AssertionError(f"wide_from_device_euler_stds min {wide_stds.min()} <= {MIN_STD_SANITY_CHECK}")
if np.linalg.norm(pose[:3]) > TRANS_SANITY_CHECK:
raise AssertionError(f"pose translation norm {np.linalg.norm(pose[:3])} exceeds {TRANS_SANITY_CHECK}")
if np.linalg.norm(pose[3:]) > ROTATION_SANITY_CHECK:
raise AssertionError(f"pose rotation norm {np.linalg.norm(pose[3:])} exceeds {ROTATION_SANITY_CHECK}")
if np.linalg.norm(pose_stds[:3]) > 10 * TRANS_SANITY_CHECK:
raise AssertionError(
f"pose translation std norm {np.linalg.norm(pose_stds[:3])} exceeds {10 * TRANS_SANITY_CHECK}"
)
if np.linalg.norm(pose_stds[3:]) > 10 * ROTATION_SANITY_CHECK:
raise AssertionError(
f"pose rotation std norm {np.linalg.norm(pose_stds[3:])} exceeds {10 * ROTATION_SANITY_CHECK}"
)
def validate_supercombo_release(run_policy_jit, model_runner, model_metadata, frame_skip, expected_device: str) -> None:
from iqpilot.selfdrive.iqmodeld.parser import PhaseParser
direct_fn = make_run_policy(model_runner, model_metadata, frame_skip)
parser = PhaseParser()
queue_factory = partial(make_input_queues, model_metadata['input_shapes'], frame_skip)
image_shape = model_metadata['input_shapes']['img']
jit_queues, jit_npy = queue_factory(Device.DEFAULT)
direct_queues, direct_npy = queue_factory(Device.DEFAULT)
for payload in (jit_npy, direct_npy):
for name, value in payload.items():
value[:] = 0 if value.dtype.kind in ('i', 'u') else 0.0
zero_inputs = {
'warped': Tensor(np.zeros((2, 6, *image_shape[2:]), dtype=np.uint8), device=Device.DEFAULT).realize(),
}
direct_outs, = direct_fn(**{k: direct_queues[k] for k in POLICY_INPUTS}, **zero_inputs)
jit_outs, = run_policy_jit(**{k: jit_queues[k] for k in POLICY_INPUTS}, **zero_inputs)
direct_flat = direct_outs.numpy().astype(np.float32).reshape(-1)
jit_flat = jit_outs.numpy().astype(np.float32).reshape(-1)
if not np.allclose(direct_flat, jit_flat, atol=1e-4, rtol=1e-4):
max_delta = float(np.max(np.abs(direct_flat - jit_flat)))
raise AssertionError(f"JIT supercombo output diverges from direct ONNX execution; max abs delta {max_delta}")
parsed = parser.parse_vision_outputs(_slice_outputs(jit_flat, model_metadata['output_slices']))
_validate_pose_outputs(parsed)
captured_devices = _captured_devices(run_policy_jit)
if expected_device and captured_devices and expected_device not in captured_devices:
raise AssertionError(
f"compiled run_policy backend mismatch: captured {sorted(captured_devices)} expected {expected_device}"
)
def _parse_size(s):
w, h = s.lower().split('x')
return int(w), int(h)
def read_file_chunked_to_shm(path):
from iqpilot.common.file_chunker import read_file_chunked
from iqpilot.system.hardware.hw import Paths
with tempfile.NamedTemporaryFile(prefix='compile_modeld_', dir=Paths.shm_path(), delete=False) as f:
f.write(read_file_chunked(path))
tmp_path = f.name
atexit.register(lambda: os.path.exists(tmp_path) and os.remove(tmp_path))
return tmp_path
if __name__ == "__main__":
from tinygrad.nn.onnx import OnnxRunner
from iqpilot.system.camerad.cameras.nv12_info import get_nv12_info
from iqpilot.selfdrive.iqmodeld.metadata import build_metadata_record
p = argparse.ArgumentParser()
p.add_argument('--model-size', type=_parse_size, required=True, help='model input WxH')
p.add_argument('--camera-resolutions', type=_parse_size, nargs='+', required=True,
help='camera resolutions WxH (one or more)')
p.add_argument('--onnx', required=True)
p.add_argument('--output', required=True)
p.add_argument('--frame-skip', type=int, required=True)
p.add_argument('--expected-device', default='QCOM', help='expected tinygrad backend baked into the JIT')
args = p.parse_args()
model_path = read_file_chunked_to_shm(args.onnx)
model_w, model_h = args.model_size
model_runner = OnnxRunner(model_path)
out = {
'metadata': build_metadata_record(model_path),
'frame_skip': args.frame_skip,
}
run_policy_jit = TinyJit(make_run_policy(model_runner, out['metadata'], args.frame_skip), prune=True)
make_policy_queues = partial(make_input_queues, out['metadata']['input_shapes'], args.frame_skip)
make_random_model_inputs = partial(make_random_images, keys=['warped'], shape=(2, 6, *out['metadata']['input_shapes']['img'][2:]))
out['run_policy'] = compile_jit(run_policy_jit, make_random_model_inputs, POLICY_INPUTS,
make_policy_queues)
validate_supercombo_release(out['run_policy'], model_runner, out['metadata'], args.frame_skip, args.expected_device)
for cam_w, cam_h in args.camera_resolutions:
nv12 = NV12Frame(cam_w, cam_h, *get_nv12_info(cam_w, cam_h))
make_random_warp_inputs = partial(make_random_images, keys=['frame', 'big_frame'], shape=nv12.size, device=WARP_DEV)
warp_enqueue = TinyJit(make_warp(nv12, model_w, model_h, args.frame_skip), prune=True)
make_warp_queues = partial(make_warp_input_queues, out['metadata']['input_shapes'], args.frame_skip)
out[(cam_w,cam_h)] = compile_jit(warp_enqueue, make_random_warp_inputs, WARP_INPUTS, make_warp_queues)
captured_devices = _captured_devices(out[(cam_w,cam_h)])
if args.expected_device and captured_devices and args.expected_device not in captured_devices:
raise AssertionError(
f"compiled warp backend mismatch for {cam_w}x{cam_h}: captured {sorted(captured_devices)} expected {args.expected_device}"
)
with open(args.output, "wb") as f:
pickle.dump(out, f)
print(f"Saved JITs to {args.output} ({os.path.getsize(args.output) / 1e6:.2f} MB)")

119
artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/iqmodeld/tools/compile_warp.py Normal file
View File

@@ -0,0 +1,119 @@
#!/usr/bin/env python3
"""
Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos/
Compile the backend-neutral warp-only artifact: NV12 camera frames + 3x3
transforms -> (2, 6, model_h/2, model_w/2) uint8 warped tensor, on the device
GPU (QCOM). maciqmodeld runs this locally
and feed the output to their backend, so the big model's image pipeline is
bit-identical to comma's fused pkl warp stage.
Run ON the device (needs the QCOM backend):
cd /data/openpilot && DEV=QCOM WARP_DEV=QCOM IMAGE=1 FLOAT16=1 NOLOCALS=1 JIT_BATCH_SIZE=0 \
python3 iqpilot/selfdrive/iqmodeld/tools/compile_warp.py \
--camera-resolutions 1928x1208 --output /data/models/emac_warp.pkl
The artifact is then split per-resolution into Paths.model_root().
"""
from __future__ import annotations
import argparse
import hashlib
import os
import pickle
from functools import partial
import numpy as np
SELFTEST_SEED = 20260817
from iqpilot.selfdrive.iqmodeld.temporal_state import DEFAULT_FRAME_SKIP, MODEL_INPUT_SPEC
from iqpilot.selfdrive.iqmodeld.tools.compile_supercombo import (
NV12Frame, WARP_INPUTS, compile_jit, make_random_images, make_warp, make_warp_input_queues,
)
MODEL_SIZE = (MODEL_INPUT_SPEC["img"][0][3] * 2, MODEL_INPUT_SPEC["img"][0][2] * 2) # (512, 256)
def _parse_size(s: str) -> tuple[int, int]:
w, h = s.lower().split("x")
return int(w), int(h)
def compile_warp(cam_w: int, cam_h: int, out_path: str | None = None,
frame_skip: int = DEFAULT_FRAME_SKIP) -> str:
"""Compile the warp-only QCOM JIT for one camera resolution and write the pkl.
Returns the artifact path. Callable from the workers so a fresh device
self-provisions the warp instead of erroring — needs the QCOM backend."""
# the QCOM warp env must be set before tinygrad is imported here
os.environ.setdefault("DEV", "QCOM")
os.environ.setdefault("WARP_DEV", "QCOM")
os.environ.setdefault("IMAGE", "1")
os.environ.setdefault("FLOAT16", "1")
os.environ.setdefault("NOLOCALS", "1")
os.environ.setdefault("JIT_BATCH_SIZE", "0")
from tinygrad.engine.jit import TinyJit
from iqpilot.system.camerad.cameras.nv12_info import get_nv12_info
from iqpilot.system.hardware.hw import Paths
model_w, model_h = MODEL_SIZE
input_shapes = {name: shape for name, (shape, _) in MODEL_INPUT_SPEC.items()}
nv12 = NV12Frame(cam_w, cam_h, *get_nv12_info(cam_w, cam_h))
make_random_warp_inputs = partial(make_random_images, keys=["frame", "big_frame"],
shape=nv12.size, device=os.getenv("WARP_DEV"))
warp_jit = TinyJit(make_warp(nv12, model_w, model_h, frame_skip), prune=True)
make_warp_queues = partial(make_warp_input_queues, input_shapes, frame_skip)
compiled = compile_jit(warp_jit, make_random_warp_inputs, WARP_INPUTS, make_warp_queues)
# historical artifact name: already-provisioned devices keep their warp
out_path = out_path or os.path.join(Paths.model_root(), f"emac_warp_{cam_w}x{cam_h}_tinygrad.pkl")
os.makedirs(os.path.dirname(out_path), exist_ok=True)
tmp = out_path + ".part"
bundle = {(cam_w, cam_h): compiled, "frame_skip": frame_skip, "model_size": MODEL_SIZE}
bundle["selftest"] = selftest_digest(compiled, cam_w, cam_h, nv12.size)
with open(tmp, "wb") as f:
pickle.dump(bundle, f)
os.replace(tmp, out_path) # atomic: a reader never sees a half-written pkl
return out_path
def main() -> None:
p = argparse.ArgumentParser()
p.add_argument("--camera-resolutions", type=_parse_size, nargs="+", default=[(1928, 1208)])
p.add_argument("--output", default=None)
p.add_argument("--frame-skip", type=int, default=DEFAULT_FRAME_SKIP)
args = p.parse_args()
for cam_w, cam_h in args.camera_resolutions:
out = compile_warp(cam_w, cam_h, args.output, frame_skip=args.frame_skip)
print(f"saved warp JIT to {out} ({os.path.getsize(out) / 1e6:.2f} MB)")
if __name__ == "__main__":
main()
def selftest_inputs(cam_w: int, cam_h: int, nv12_size: int):
"""A fixed synthetic frame pair and pair of matrices. Deterministic so the
digest is reproducible on the device that compiled the artifact."""
rng = np.random.default_rng(SELFTEST_SEED)
frame = rng.integers(0, 256, nv12_size, dtype=np.uint8)
big_frame = rng.integers(0, 256, nv12_size, dtype=np.uint8)
tfm = np.array([[0.7, 0.02, 300.0], [0.01, 0.7, 240.0], [0.0, 0.0, 1.0]], dtype=np.float32)
big_tfm = np.array([[0.5, 0.01, 380.0], [0.02, 0.5, 300.0], [0.0, 0.0, 1.0]], dtype=np.float32)
return frame, big_frame, tfm, big_tfm
def selftest_digest(compiled, cam_w: int, cam_h: int, nv12_size: int) -> str:
"""Hash the warp's output for a fixed input.
A warp artifact pinned to one tinygrad can still unpickle under another and
then compute silently wrong, which reaches the model as a garbage image and
looks like a bad model rather than a stale artifact. A version string cannot
see that; running it can."""
from tinygrad.tensor import Tensor
frame, big_frame, tfm, big_tfm = selftest_inputs(cam_w, cam_h, nv12_size)
dev = os.getenv("WARP_DEV") or "QCOM"
out = compiled(tfm=Tensor(tfm, device="NPY").realize(),
big_tfm=Tensor(big_tfm, device="NPY").realize(),
frame=Tensor(frame, device=dev).realize(),
big_frame=Tensor(big_frame, device=dev).realize())
return hashlib.sha256(out.numpy().astype(np.uint8).tobytes()).hexdigest()

0
iqdbc_repo/iqdbc/car/gm/__init__.py → artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/locationd/__init__.py
View File

32
artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/locationd/calibration_helpers.py Normal file
View File

@@ -0,0 +1,32 @@
import math
import numpy as np
from iqpilot.cereal import log
from iqpilot.selfdrive.locationd.calibrationd import HEIGHT_INIT, HEIGHT_SANE_MIN, HEIGHT_SANE_MAX
def get_calibrated_rpy(live_calib: log.ExtrinsicsCalibration) -> np.ndarray | None:
if live_calib.calStatus != log.ExtrinsicsCalibration.Status.calibrated:
return None
if len(live_calib.rpyCalib) != 3:
return None
calib_rpy = np.asarray(live_calib.rpyCalib, dtype=np.float32)
return calib_rpy if np.isfinite(calib_rpy).all() else None
def get_render_path_height(live_calib: log.ExtrinsicsCalibration) -> float:
if live_calib.calStatus != log.ExtrinsicsCalibration.Status.calibrated:
return float(HEIGHT_INIT[0])
if len(live_calib.height) != 1:
return float(HEIGHT_INIT[0])
height = float(live_calib.height[0])
if not math.isfinite(height):
return float(HEIGHT_INIT[0])
if not (HEIGHT_SANE_MIN <= height <= HEIGHT_SANE_MAX):
return float(HEIGHT_INIT[0])
return height

367
artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/locationd/calibrationd.py Normal file
View File

@@ -0,0 +1,367 @@
#!/usr/bin/env python3
'''
This process finds calibration values. More info on what these calibration values
are can be found here https://github.com/commaai/openpilot/tree/master/common/transformations
While the roll calibration is a real value that can be estimated, here we assume it's zero,
and the image input into the neural network is not corrected for roll.
'''
import os
import capnp
import numpy as np
from typing import NoReturn
from iqpilot.cereal import log, car
import iqpilot.cereal.messaging as messaging
from iqpilot.common.constants import CV
from iqpilot.common.params import Params
from iqpilot.common.issue_debug import log_issue_limited
from iqpilot.common.realtime import config_realtime_process
from iqpilot.common.transformations.orientation import rot_from_euler, euler_from_rot
from iqpilot.common.swaglog import cloudlog
from iqpilot.system.hardware import HARDWARE
MIN_SPEED_FILTER = 15 * CV.MPH_TO_MS
MAX_VEL_ANGLE_STD = np.radians(0.25)
MAX_YAW_RATE_FILTER = np.radians(2) # per second
MAX_HEIGHT_STD = np.exp(-3.5)
# This is at model frequency, blocks needed for efficiency
SMOOTH_CYCLES = 10
BLOCK_SIZE = 100
INPUTS_NEEDED = 5 # Minimum blocks needed for valid calibration
INPUTS_WANTED = 50 # We want a little bit more than we need for stability
MAX_ALLOWED_YAW_SPREAD = np.radians(2)
MAX_ALLOWED_PITCH_SPREAD = np.radians(4)
TICI_FAMILY_PITCH_SPREAD_RESET = np.radians(3)
RPY_INIT = np.array([0.0,0.0,0.0])
WIDE_FROM_DEVICE_EULER_INIT = np.array([0.0, 0.0, 0.0])
HEIGHT_INIT = np.array([1.22])
HEIGHT_SANE_MIN, HEIGHT_SANE_MAX = 0.9, 2.0
DEVICE_IS_TICI_FAMILY = HARDWARE.get_device_type() in ("tici", "tizi")
# These values are needed to accommodate the model frame in the narrow cam
if HARDWARE.get_device_type() == 'mici':
PITCH_LIMITS = np.array([-0.143101, 0.22235988])
else:
PITCH_LIMITS = np.array([-0.09074112085129739, 0.17])
YAW_LIMITS = np.array([-0.06912048084718224, 0.06912048084718235])
DEBUG = os.getenv("DEBUG") is not None
def is_calibration_valid(rpy: np.ndarray) -> bool:
return (PITCH_LIMITS[0] < rpy[1] < PITCH_LIMITS[1]) and (YAW_LIMITS[0] < rpy[2] < YAW_LIMITS[1])
def sanity_clip(rpy: np.ndarray) -> np.ndarray:
if np.isnan(rpy).any():
rpy = RPY_INIT
return np.array([rpy[0],
np.clip(rpy[1], PITCH_LIMITS[0] - .005, PITCH_LIMITS[1] + .005),
np.clip(rpy[2], YAW_LIMITS[0] - .005, YAW_LIMITS[1] + .005)])
def moving_avg_with_linear_decay(prev_mean: np.ndarray, new_val: np.ndarray, idx: int, block_size: float) -> np.ndarray:
return (idx*prev_mean + (block_size - idx) * new_val) / block_size
class Calibrator:
def __init__(self, param_put: bool = False):
self.param_put = param_put
self.not_car = False
self.stable_rpy = RPY_INIT.copy()
self.stable_wide_from_device_euler = WIDE_FROM_DEVICE_EULER_INIT.copy()
self.stable_height = HEIGHT_INIT.copy()
self.has_stable_snapshot = False
# Read saved calibration
self.params = Params()
calibration_params = self.params.get("CalibrationParams")
rpy_init = RPY_INIT
wide_from_device_euler = WIDE_FROM_DEVICE_EULER_INIT
height = HEIGHT_INIT
valid_blocks = 0
self.cal_status = log.ExtrinsicsCalibration.Status.uncalibrated
if param_put and calibration_params:
try:
with log.Event.from_bytes(calibration_params) as msg:
rpy_init = np.array(msg.extrinsicsCalibration.rpyCalib)
valid_blocks = msg.extrinsicsCalibration.validBlocks
wide_from_device_euler = np.array(msg.extrinsicsCalibration.wideFromDeviceEuler)
height = np.array(msg.extrinsicsCalibration.height)
except Exception:
cloudlog.exception("Error reading cached CalibrationParams")
self.reset(rpy_init, valid_blocks, wide_from_device_euler, height)
self.update_status()
# If saved calibration is immediately invalid (e.g. bad params from a previous
# bootstrap bug or device remount), auto-clear it so we recalibrate from scratch
# instead of getting permanently stuck in the "Calibration Invalid" state.
if self.cal_status == log.ExtrinsicsCalibration.Status.invalid:
cloudlog.warning("calibrationd: saved CalibrationParams are invalid, clearing and starting fresh")
if param_put:
self.params.remove("CalibrationParams")
self.reset()
self.update_status()
def _remember_stable_solution(self) -> None:
self.stable_rpy = self.rpy.copy()
self.stable_wide_from_device_euler = self.wide_from_device_euler.copy()
self.stable_height = self.height.copy()
self.has_stable_snapshot = True
def reset(self, rpy_init: np.ndarray = RPY_INIT,
valid_blocks: int = 0,
wide_from_device_euler_init: np.ndarray = WIDE_FROM_DEVICE_EULER_INIT,
height_init: np.ndarray = HEIGHT_INIT,
smooth_from: np.ndarray | None = None) -> None:
if not np.isfinite(rpy_init).all():
self.rpy = RPY_INIT.copy()
else:
self.rpy = rpy_init.copy()
if not np.isfinite(height_init).all() or len(height_init) != 1:
self.height = HEIGHT_INIT.copy()
else:
self.height = height_init.copy()
if not np.isfinite(wide_from_device_euler_init).all() or len(wide_from_device_euler_init) != 3:
self.wide_from_device_euler = WIDE_FROM_DEVICE_EULER_INIT.copy()
else:
self.wide_from_device_euler = wide_from_device_euler_init.copy()
if not np.isfinite(valid_blocks) or valid_blocks < 0:
self.valid_blocks = 0
else:
self.valid_blocks = valid_blocks
self.rpys = np.tile(self.rpy, (INPUTS_WANTED, 1))
self.wide_from_device_eulers = np.tile(self.wide_from_device_euler, (INPUTS_WANTED, 1))
self.heights = np.tile(self.height, (INPUTS_WANTED, 1))
self.idx = 0
self.block_idx = 0
self.v_ego = 0.0
if smooth_from is None:
self.old_rpy = RPY_INIT
self.old_rpy_weight = 0.0
else:
self.old_rpy = smooth_from
self.old_rpy_weight = 1.0
def get_valid_idxs(self) -> list[int]:
# exclude current block_idx from validity window
before_current = list(range(self.block_idx))
after_current = list(range(min(self.valid_blocks, self.block_idx + 1), self.valid_blocks))
return before_current + after_current
def update_status(self) -> None:
valid_idxs = self.get_valid_idxs()
if valid_idxs:
self.wide_from_device_euler = np.mean(self.wide_from_device_eulers[valid_idxs], axis=0)
self.height = np.mean(self.heights[valid_idxs], axis=0)
rpys = self.rpys[valid_idxs]
self.rpy = np.mean(rpys, axis=0)
max_rpy_calib = np.array(np.max(rpys, axis=0))
min_rpy_calib = np.array(np.min(rpys, axis=0))
self.calib_spread = np.abs(max_rpy_calib - min_rpy_calib)
else:
self.calib_spread = np.zeros(3)
if self.valid_blocks < INPUTS_NEEDED:
if self.cal_status == log.ExtrinsicsCalibration.Status.recalibrating:
self.cal_status = log.ExtrinsicsCalibration.Status.recalibrating
else:
self.cal_status = log.ExtrinsicsCalibration.Status.uncalibrated
elif is_calibration_valid(self.rpy):
self.cal_status = log.ExtrinsicsCalibration.Status.calibrated
else:
self.cal_status = log.ExtrinsicsCalibration.Status.invalid
# If spread is too high, assume mounting was changed and reset to last block.
# Make the transition smooth. Abrupt transitions are not good for feedback loop through supercombo model.
# TODO: add height spread check with smooth transition too
pitch_spread_limit = TICI_FAMILY_PITCH_SPREAD_RESET if DEVICE_IS_TICI_FAMILY else MAX_ALLOWED_PITCH_SPREAD
spread_too_high = self.calib_spread[1] > pitch_spread_limit or self.calib_spread[2] > MAX_ALLOWED_YAW_SPREAD
if self.cal_status == log.ExtrinsicsCalibration.Status.calibrated and not spread_too_high:
self._remember_stable_solution()
if spread_too_high and self.cal_status == log.ExtrinsicsCalibration.Status.calibrated:
use_stable_snapshot = DEVICE_IS_TICI_FAMILY and self.has_stable_snapshot
if use_stable_snapshot:
reset_rpy = self.stable_rpy
reset_wide = self.stable_wide_from_device_euler
reset_height = self.stable_height
else:
reset_rpy = self.rpys[self.block_idx - 1]
reset_wide = self.wide_from_device_eulers[self.block_idx - 1]
reset_height = self.heights[self.block_idx - 1]
log_issue_limited(
"calibrationd_reset_spread",
"calibration",
f"calibrationd reset unstable solution pitchSpread={self.calib_spread[1]:.6f} "
f"yawSpread={self.calib_spread[2]:.6f} pitchLimit={pitch_spread_limit:.6f} "
f"use_stable_snapshot={use_stable_snapshot} rpy={self.rpy.tolist()}",
interval_sec=0.5,
)
self.reset(reset_rpy, valid_blocks=1, wide_from_device_euler_init=reset_wide,
height_init=reset_height, smooth_from=self.stable_rpy if use_stable_snapshot else self.rpy)
self.cal_status = log.ExtrinsicsCalibration.Status.recalibrating
write_this_cycle = (self.idx == 0) and (self.block_idx % (INPUTS_WANTED//5) == 5)
if self.param_put and write_this_cycle:
self.params.put_nonblocking("CalibrationParams", self.get_msg(True).to_bytes())
def handle_v_ego(self, v_ego: float) -> None:
self.v_ego = v_ego
def get_smooth_rpy(self) -> np.ndarray:
if self.old_rpy_weight > 0:
return self.old_rpy_weight * self.old_rpy + (1.0 - self.old_rpy_weight) * self.rpy
else:
return self.rpy
def handle_cam_odom(self, trans: list[float],
rot: list[float],
wide_from_device_euler: list[float],
trans_std: list[float],
road_transform_trans: list[float],
road_transform_trans_std: list[float]) -> np.ndarray | None:
self.old_rpy_weight = max(0.0, self.old_rpy_weight - 1/SMOOTH_CYCLES)
fast_enough = self.v_ego > MIN_SPEED_FILTER
motion_speed = max(float(self.v_ego), float(trans[0]))
cam_fast_enough = motion_speed > MIN_SPEED_FILTER
yaw_ok = abs(rot[2]) < MAX_YAW_RATE_FILTER
straight_and_fast = fast_enough and cam_fast_enough and yaw_ok
angle_std_threshold = MAX_VEL_ANGLE_STD
height_std_threshold = MAX_HEIGHT_STD
rpy_certain = np.arctan2(trans_std[1], motion_speed) < angle_std_threshold
if len(road_transform_trans_std) == 3:
height_certain = road_transform_trans_std[2] < height_std_threshold
else:
height_certain = True
certain_if_calib = rpy_certain
if not (straight_and_fast and certain_if_calib):
log_issue_limited(
"calibrationd_rejected_sample",
"calibration",
f"calibrationd rejected sample vEgo={self.v_ego:.2f} trans0={trans[0]:.2f} yawRate={rot[2]:.4f} "
f"fast_enough={fast_enough} cam_fast_enough={cam_fast_enough} motion_speed={motion_speed:.2f} yaw_ok={yaw_ok} "
f"rpy_certain={rpy_certain} height_certain={height_certain} valid_blocks={self.valid_blocks} idx={self.idx}",
interval_sec=1.0,
)
return None
observed_rpy = np.array([0,
-np.arctan2(trans[2], trans[0]),
np.arctan2(trans[1], trans[0])])
new_rpy = euler_from_rot(rot_from_euler(self.get_smooth_rpy()).dot(rot_from_euler(observed_rpy)))
new_rpy = sanity_clip(new_rpy)
if len(wide_from_device_euler) == 3:
new_wide_from_device_euler = np.array(wide_from_device_euler)
else:
new_wide_from_device_euler = WIDE_FROM_DEVICE_EULER_INIT
if len(road_transform_trans) == 3 and HEIGHT_SANE_MIN <= road_transform_trans[2] <= HEIGHT_SANE_MAX:
new_height = np.array([road_transform_trans[2]])
else:
new_height = HEIGHT_INIT
self.rpys[self.block_idx] = moving_avg_with_linear_decay(self.rpys[self.block_idx], new_rpy, self.idx, float(BLOCK_SIZE))
self.wide_from_device_eulers[self.block_idx] = moving_avg_with_linear_decay(self.wide_from_device_eulers[self.block_idx],
new_wide_from_device_euler, self.idx, float(BLOCK_SIZE))
self.heights[self.block_idx] = moving_avg_with_linear_decay(self.heights[self.block_idx], new_height, self.idx, float(BLOCK_SIZE))
self.idx = (self.idx + 1) % BLOCK_SIZE
if self.idx == 0:
self.block_idx += 1
self.valid_blocks = max(self.block_idx, self.valid_blocks)
self.block_idx = self.block_idx % INPUTS_WANTED
self.update_status()
if self.idx == 0:
log_issue_limited(
"calibrationd_progress_block",
"calibration",
f"calibrationd progress status={int(self.cal_status)} valid_blocks={self.valid_blocks} "
f"calPerc={min(100 * (self.valid_blocks * BLOCK_SIZE + self.idx) // (INPUTS_NEEDED * BLOCK_SIZE), 100)} "
f"rpy={self.rpy.tolist()} spread={self.calib_spread.tolist()}",
interval_sec=0.5,
)
return new_rpy
def get_msg(self, valid: bool) -> capnp.lib.capnp._DynamicStructBuilder:
smooth_rpy = self.get_smooth_rpy()
msg = messaging.new_message('extrinsicsCalibration')
msg.valid = valid
extrinsicsCalibration = msg.extrinsicsCalibration
extrinsicsCalibration.validBlocks = self.valid_blocks
extrinsicsCalibration.calStatus = self.cal_status
extrinsicsCalibration.calPerc = min(100 * (self.valid_blocks * BLOCK_SIZE + self.idx) // (INPUTS_NEEDED * BLOCK_SIZE), 100)
extrinsicsCalibration.rpyCalib = smooth_rpy.tolist()
extrinsicsCalibration.rpyCalibSpread = self.calib_spread.tolist()
extrinsicsCalibration.wideFromDeviceEuler = self.wide_from_device_euler.tolist()
extrinsicsCalibration.height = self.height.tolist()
return msg
def send_data(self, pm: messaging.PubMaster, valid: bool) -> None:
pm.send('extrinsicsCalibration', self.get_msg(valid))
def main() -> NoReturn:
config_realtime_process([0, 1, 2, 3], 5)
pm = messaging.PubMaster(['extrinsicsCalibration'])
sm = messaging.SubMaster(['cameraOdometry', 'carState'], poll='cameraOdometry')
params_reader = Params()
CP = messaging.log_from_bytes(params_reader.get("CarParams", block=True), car.CarParams)
calibrator = Calibrator(param_put=True)
calibrator.not_car = CP.notCar
while 1:
timeout = 0 if sm.frame == -1 else 100
sm.update(timeout)
if sm.updated['cameraOdometry']:
calibrator.handle_v_ego(sm['carState'].vEgo)
new_rpy = calibrator.handle_cam_odom(sm['cameraOdometry'].trans,
sm['cameraOdometry'].rot,
sm['cameraOdometry'].wideFromDeviceEuler,
sm['cameraOdometry'].transStd,
sm['cameraOdometry'].roadTransformTrans,
sm['cameraOdometry'].roadTransformTransStd)
if DEBUG and new_rpy is not None:
print('got new rpy', new_rpy)
# 4Hz driven by cameraOdometry
if sm.frame % 5 == 0:
checks_ok = sm.all_checks()
if not checks_ok:
ft = sm.freq_tracker
recv_hz = {s: (round(1.0 / ft[s].avg_dt.get_average(), 2) if ft[s].avg_dt.count else None) for s in sm.services}
log_issue_limited(
"calibrationd_checks_failed",
"calibration",
f"calibrationd all_checks failed alive={sm.alive} freq_ok={sm.freq_ok} valid={sm.valid} "
f"seen={sm.seen} recv_hz={recv_hz}",
interval_sec=5.0,
)
calibrator.send_data(pm, checks_ok)
if __name__ == "__main__":
main()

188
artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/locationd/helpers.py Normal file
View File

@@ -0,0 +1,188 @@
import numpy as np
from typing import Any
from functools import cache
from iqpilot.cereal import log
from iqpilot.common.transformations.orientation import rot_from_euler, euler_from_rot
from iqpilot.selfdrive.locationd.calibration_helpers import get_calibrated_rpy
@cache
def fft_next_good_size(n: int) -> int:
"""
smallest composite of 2, 3, 5, 7, 11 that is >= n
inspired by pocketfft
"""
if n <= 6:
return n
best, f2 = 2 * n, 1
while f2 < best:
f23 = f2
while f23 < best:
f235 = f23
while f235 < best:
f2357 = f235
while f2357 < best:
f235711 = f2357
while f235711 < best:
best = f235711 if f235711 >= n else best
f235711 *= 11
f2357 *= 7
f235 *= 5
f23 *= 3
f2 *= 2
return best
def parabolic_peak_interp(R, max_index):
if max_index == 0 or max_index == len(R) - 1:
return max_index
y_m1, y_0, y_p1 = R[max_index - 1], R[max_index], R[max_index + 1]
offset = 0.5 * (y_p1 - y_m1) / (2 * y_0 - y_p1 - y_m1)
return max_index + offset
def rotate_cov(rot_matrix, cov_in):
return rot_matrix @ cov_in @ rot_matrix.T
def rotate_std(rot_matrix, std_in):
return np.sqrt(np.diag(rotate_cov(rot_matrix, np.diag(std_in**2))))
class NPQueue:
def __init__(self, maxlen: int, rowsize: int) -> None:
self.maxlen = maxlen
self.arr = np.empty((0, rowsize))
def __len__(self) -> int:
return len(self.arr)
def append(self, pt: list[float]) -> None:
if len(self.arr) < self.maxlen:
self.arr = np.append(self.arr, [pt], axis=0)
else:
self.arr[:-1] = self.arr[1:]
self.arr[-1] = pt
class PointBuckets:
def __init__(self, x_bounds: list[tuple[float, float]], min_points: list[float], min_points_total: int, points_per_bucket: int, rowsize: int) -> None:
self.x_bounds = x_bounds
self.buckets = {bounds: NPQueue(maxlen=points_per_bucket, rowsize=rowsize) for bounds in x_bounds}
self.buckets_min_points = dict(zip(x_bounds, min_points, strict=True))
self.min_points_total = min_points_total
def __len__(self) -> int:
return sum([len(v) for v in self.buckets.values()])
def is_valid(self) -> bool:
individual_buckets_valid = all(len(v) >= min_pts for v, min_pts in zip(self.buckets.values(), self.buckets_min_points.values(), strict=True))
total_points_valid = self.__len__() >= self.min_points_total
return individual_buckets_valid and total_points_valid
def get_valid_percent(self) -> int:
total_points_perc = min(self.__len__() / self.min_points_total * 100, 100)
individual_buckets_perc = min(min(len(v) / min_pts * 100 for v, min_pts in
zip(self.buckets.values(), self.buckets_min_points.values(), strict=True)), 100)
return int((total_points_perc + individual_buckets_perc) / 2)
def is_calculable(self) -> bool:
return all(len(v) > 0 for v in self.buckets.values())
def add_point(self, x: float, y: float) -> None:
raise NotImplementedError
def get_points(self, num_points: int | None = None) -> Any:
points = np.vstack([x.arr for x in self.buckets.values()])
if num_points is None:
return points
return points[np.random.choice(np.arange(len(points)), min(len(points), num_points), replace=False)]
def load_points(self, points: list[list[float]]) -> None:
for point in points:
self.add_point(*point)
class ParameterEstimator:
""" Base class for parameter estimators """
def reset(self) -> None:
raise NotImplementedError
def handle_log(self, t: int, which: str, msg: log.Event) -> None:
raise NotImplementedError
def get_msg(self, valid: bool, with_points: bool) -> log.Event:
raise NotImplementedError
class Measurement:
x, y, z = (property(lambda self: self.xyz[0]), property(lambda self: self.xyz[1]), property(lambda self: self.xyz[2]))
x_std, y_std, z_std = (property(lambda self: self.xyz_std[0]), property(lambda self: self.xyz_std[1]), property(lambda self: self.xyz_std[2]))
roll, pitch, yaw = x, y, z
roll_std, pitch_std, yaw_std = x_std, y_std, z_std
def __init__(self, xyz: np.ndarray, xyz_std: np.ndarray):
self.xyz: np.ndarray = xyz
self.xyz_std: np.ndarray = xyz_std
@classmethod
def from_measurement_xyz(cls, measurement: log.DeviceMotion.XYZMeasurement) -> 'Measurement':
return cls(
xyz=np.array([measurement.x, measurement.y, measurement.z]),
xyz_std=np.array([measurement.xStd, measurement.yStd, measurement.zStd])
)
class Pose:
def __init__(self, orientation: Measurement, velocity: Measurement, acceleration: Measurement, angular_velocity: Measurement):
self.orientation = orientation
self.velocity = velocity
self.acceleration = acceleration
self.angular_velocity = angular_velocity
@classmethod
def from_live_pose(cls, live_pose: log.DeviceMotion) -> 'Pose':
return Pose(
orientation=Measurement.from_measurement_xyz(live_pose.orientationNED),
velocity=Measurement.from_measurement_xyz(live_pose.velocityDevice),
acceleration=Measurement.from_measurement_xyz(live_pose.accelerationDevice),
angular_velocity=Measurement.from_measurement_xyz(live_pose.angularVelocityDevice)
)
class PoseCalibrator:
def __init__(self):
self.calib_valid = False
self.calib_from_device = np.eye(3)
def _transform_calib_from_device(self, meas: Measurement):
new_xyz = self.calib_from_device @ meas.xyz
new_xyz_std = rotate_std(self.calib_from_device, meas.xyz_std)
return Measurement(new_xyz, new_xyz_std)
def _ned_from_calib(self, orientation: Measurement):
ned_from_device = rot_from_euler(orientation.xyz)
ned_from_calib = ned_from_device @ self.calib_from_device.T
ned_from_calib_euler_meas = Measurement(euler_from_rot(ned_from_calib), np.full(3, np.nan))
return ned_from_calib_euler_meas
def build_calibrated_pose(self, pose: Pose) -> Pose:
ned_from_calib_euler = self._ned_from_calib(pose.orientation)
angular_velocity_calib = self._transform_calib_from_device(pose.angular_velocity)
acceleration_calib = self._transform_calib_from_device(pose.acceleration)
velocity_calib = self._transform_calib_from_device(pose.velocity)
return Pose(ned_from_calib_euler, velocity_calib, acceleration_calib, angular_velocity_calib)
def feed_live_calib(self, live_calib: log.ExtrinsicsCalibration):
calib_rpy = get_calibrated_rpy(live_calib)
if calib_rpy is not None:
self.calib_from_device = rot_from_euler(calib_rpy).T
self.calib_valid = True
else:
if not self.calib_valid:
self.calib_from_device = np.eye(3)
self.calib_valid = False

336
artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/locationd/locationd.py Normal file
View File

@@ -0,0 +1,336 @@
#!/usr/bin/env python3
import os
import time
import capnp
import numpy as np
from enum import Enum
from collections import defaultdict
from iqpilot.cereal import log, messaging
from iqpilot.cereal.services import SERVICE_LIST
from iqpilot.common.transformations.orientation import rot_from_euler
from iqpilot.common.realtime import config_realtime_process
from iqpilot.common.params import Params
from iqpilot.common.swaglog import cloudlog
from iqpilot.selfdrive.locationd.calibration_helpers import get_calibrated_rpy
from iqpilot.selfdrive.locationd.helpers import rotate_std
from iqpilot.selfdrive.locationd.models.pose_kf import PoseKalman, States
from iqpilot.selfdrive.locationd.models.constants import ObservationKind
ACCEL_SANITY_CHECK = 100.0 # m/s^2
ROTATION_SANITY_CHECK = 10.0 # rad/s
TRANS_SANITY_CHECK = 200.0 # m/s
CALIB_RPY_SANITY_CHECK = 0.5 # rad (+- 30 deg)
MIN_STD_SANITY_CHECK = 1e-5 # m or rad
MAX_FILTER_REWIND_TIME = 0.8 # s
MAX_SENSOR_TIME_DIFF = 0.1 # s
YAWRATE_CROSS_ERR_CHECK_FACTOR = 30
INPUT_INVALID_LIMIT = 2.0 # 1 (camodo) / 9 (sensor) bad input[s] ignored
INPUT_INVALID_RECOVERY = 10.0 # ~10 secs to resume after exceeding allowed bad inputs by one
POSENET_STD_INITIAL_VALUE = 10.0
POSENET_STD_HIST_HALF = 20
def calculate_invalid_input_decay(invalid_limit, recovery_time, frequency):
return (1 - 1 / (2 * invalid_limit)) ** (1 / (recovery_time * frequency))
def init_xyz_measurement(measurement: capnp._DynamicStructBuilder, values: np.ndarray, stds: np.ndarray, valid: bool):
assert len(values) == len(stds) == 3
measurement.x, measurement.y, measurement.z = map(float, values)
measurement.xStd, measurement.yStd, measurement.zStd = map(float, stds)
measurement.valid = valid
class HandleLogResult(Enum):
SUCCESS = 0
TIMING_INVALID = 1
INPUT_INVALID = 2
SENSOR_SOURCE_INVALID = 3
class LocationEstimator:
def __init__(self, debug: bool):
self.kf = PoseKalman(MAX_FILTER_REWIND_TIME)
self.debug = debug
self.posenet_stds = np.array([POSENET_STD_INITIAL_VALUE] * (POSENET_STD_HIST_HALF * 2))
self.car_speed = 0.0
self.camodo_yawrate_distribution = np.array([0.0, 10.0]) # mean, std
self.device_from_calib = np.eye(3)
obs_kinds = [ObservationKind.PHONE_ACCEL, ObservationKind.PHONE_GYRO, ObservationKind.CAMERA_ODO_ROTATION, ObservationKind.CAMERA_ODO_TRANSLATION]
self.observations = {kind: np.zeros(3, dtype=np.float32) for kind in obs_kinds}
self.observation_errors = {kind: np.zeros(3, dtype=np.float32) for kind in obs_kinds}
def reset(self, t: float, x_initial: np.ndarray = PoseKalman.initial_x, P_initial: np.ndarray = PoseKalman.initial_P):
self.kf.init_state(x_initial, covs=P_initial, filter_time=t)
def _validate_sensor_source(self, source: log.SensorEventData.SensorSource):
# some segments have two IMUs, ignore the second one
return source != log.SensorEventData.SensorSource.bmx055
def _validate_sensor_time(self, sensor_time: float, t: float):
# ignore empty readings
if sensor_time == 0:
return False
# sensor time and log time should be close
sensor_time_invalid = abs(sensor_time - t) > MAX_SENSOR_TIME_DIFF
if sensor_time_invalid:
cloudlog.warning("Sensor reading ignored, sensor timestamp more than 100ms off from log time")
return not sensor_time_invalid
def _validate_timestamp(self, t: float):
kf_t = self.kf.t
invalid = not np.isnan(kf_t) and (kf_t - t) > MAX_FILTER_REWIND_TIME
if invalid:
cloudlog.warning("Observation timestamp is older than the max rewind threshold of the filter")
return not invalid
def _finite_check(self, t: float, new_x: np.ndarray, new_P: np.ndarray):
all_finite = np.isfinite(new_x).all() and np.isfinite(new_P).all()
if not all_finite:
cloudlog.error("Non-finite values detected, kalman reset")
self.reset(t)
def handle_log(self, t: float, which: str, msg: capnp._DynamicStructReader) -> HandleLogResult:
new_x, new_P = None, None
if which == "accelerometer" and msg.which() == "acceleration":
sensor_time = msg.timestamp * 1e-9
if not self._validate_sensor_time(sensor_time, t) or not self._validate_timestamp(sensor_time):
return HandleLogResult.TIMING_INVALID
if not self._validate_sensor_source(msg.source):
return HandleLogResult.SENSOR_SOURCE_INVALID
v = msg.acceleration.v
meas = np.array([-v[2], -v[1], -v[0]])
if np.linalg.norm(meas) >= ACCEL_SANITY_CHECK:
return HandleLogResult.INPUT_INVALID
acc_res = self.kf.predict_and_observe(sensor_time, ObservationKind.PHONE_ACCEL, meas)
if acc_res is not None:
_, new_x, _, new_P, _, _, (acc_err,), _, _ = acc_res
self.observation_errors[ObservationKind.PHONE_ACCEL] = np.array(acc_err)
self.observations[ObservationKind.PHONE_ACCEL] = meas
elif which == "gyroscope" and msg.which() == "gyroUncalibrated":
sensor_time = msg.timestamp * 1e-9
if not self._validate_sensor_time(sensor_time, t) or not self._validate_timestamp(sensor_time):
return HandleLogResult.TIMING_INVALID
if not self._validate_sensor_source(msg.source):
return HandleLogResult.SENSOR_SOURCE_INVALID
v = msg.gyroUncalibrated.v
meas = np.array([-v[2], -v[1], -v[0]])
gyro_bias = self.kf.x[States.GYRO_BIAS]
gyro_camodo_yawrate_err = np.abs((meas[2] - gyro_bias[2]) - self.camodo_yawrate_distribution[0])
gyro_camodo_yawrate_err_threshold = YAWRATE_CROSS_ERR_CHECK_FACTOR * self.camodo_yawrate_distribution[1]
gyro_valid = gyro_camodo_yawrate_err < gyro_camodo_yawrate_err_threshold
if np.linalg.norm(meas) >= ROTATION_SANITY_CHECK or not gyro_valid:
return HandleLogResult.INPUT_INVALID
gyro_res = self.kf.predict_and_observe(sensor_time, ObservationKind.PHONE_GYRO, meas)
if gyro_res is not None:
_, new_x, _, new_P, _, _, (gyro_err,), _, _ = gyro_res
self.observation_errors[ObservationKind.PHONE_GYRO] = np.array(gyro_err)
self.observations[ObservationKind.PHONE_GYRO] = meas
elif which == "carState":
self.car_speed = abs(msg.vEgo)
elif which == "extrinsicsCalibration":
# Note that we use this message during calibration
calib = get_calibrated_rpy(msg)
if calib is None and len(msg.rpyCalib) > 0:
calib = np.array(msg.rpyCalib)
if calib is not None:
if calib.min() < -CALIB_RPY_SANITY_CHECK or calib.max() > CALIB_RPY_SANITY_CHECK:
return HandleLogResult.INPUT_INVALID
self.device_from_calib = rot_from_euler(calib)
elif which == "cameraOdometry":
if not self._validate_timestamp(t):
return HandleLogResult.TIMING_INVALID
rot_device = np.matmul(self.device_from_calib, np.array(msg.rot))
trans_device = np.matmul(self.device_from_calib, np.array(msg.trans))
if np.linalg.norm(rot_device) > ROTATION_SANITY_CHECK or np.linalg.norm(trans_device) > TRANS_SANITY_CHECK:
return HandleLogResult.INPUT_INVALID
rot_calib_std = np.array(msg.rotStd)
trans_calib_std = np.array(msg.transStd)
if rot_calib_std.min() <= MIN_STD_SANITY_CHECK or trans_calib_std.min() <= MIN_STD_SANITY_CHECK:
return HandleLogResult.INPUT_INVALID
if np.linalg.norm(rot_calib_std) > 10 * ROTATION_SANITY_CHECK or np.linalg.norm(trans_calib_std) > 10 * TRANS_SANITY_CHECK:
return HandleLogResult.INPUT_INVALID
self.posenet_stds = np.roll(self.posenet_stds, -1)
self.posenet_stds[-1] = trans_calib_std[0]
# Multiply by N to avoid to high certainty in kalman filter because of temporally correlated noise
rot_calib_std *= 10
trans_calib_std *= 2
rot_device_std = rotate_std(self.device_from_calib, rot_calib_std)
trans_device_std = rotate_std(self.device_from_calib, trans_calib_std)
rot_device_noise = rot_device_std ** 2
trans_device_noise = trans_device_std ** 2
cam_odo_rot_res = self.kf.predict_and_observe(t, ObservationKind.CAMERA_ODO_ROTATION, rot_device, np.array([np.diag(rot_device_noise)]))
cam_odo_trans_res = self.kf.predict_and_observe(t, ObservationKind.CAMERA_ODO_TRANSLATION, trans_device, np.array([np.diag(trans_device_noise)]))
self.camodo_yawrate_distribution = np.array([rot_device[2], rot_device_std[2]])
if cam_odo_rot_res is not None:
_, new_x, _, new_P, _, _, (cam_odo_rot_err,), _, _ = cam_odo_rot_res
self.observation_errors[ObservationKind.CAMERA_ODO_ROTATION] = np.array(cam_odo_rot_err)
self.observations[ObservationKind.CAMERA_ODO_ROTATION] = rot_device
if cam_odo_trans_res is not None:
_, new_x, _, new_P, _, _, (cam_odo_trans_err,), _, _ = cam_odo_trans_res
self.observation_errors[ObservationKind.CAMERA_ODO_TRANSLATION] = np.array(cam_odo_trans_err)
self.observations[ObservationKind.CAMERA_ODO_TRANSLATION] = trans_device
if new_x is not None and new_P is not None:
self._finite_check(t, new_x, new_P)
return HandleLogResult.SUCCESS
def get_msg(self, sensors_valid: bool, inputs_valid: bool, filter_valid: bool):
state, cov = self.kf.x, self.kf.P
std = np.sqrt(np.diag(cov))
orientation_ned, orientation_ned_std = state[States.NED_ORIENTATION], std[States.NED_ORIENTATION]
velocity_device, velocity_device_std = state[States.DEVICE_VELOCITY], std[States.DEVICE_VELOCITY]
angular_velocity_device, angular_velocity_device_std = state[States.ANGULAR_VELOCITY], std[States.ANGULAR_VELOCITY]
acceleration_device, acceleration_device_std = state[States.ACCELERATION], std[States.ACCELERATION]
msg = messaging.new_message("deviceMotion")
msg.valid = filter_valid
deviceMotion = msg.deviceMotion
init_xyz_measurement(deviceMotion.orientationNED, orientation_ned, orientation_ned_std, filter_valid)
init_xyz_measurement(deviceMotion.velocityDevice, velocity_device, velocity_device_std, filter_valid)
init_xyz_measurement(deviceMotion.angularVelocityDevice, angular_velocity_device, angular_velocity_device_std, filter_valid)
init_xyz_measurement(deviceMotion.accelerationDevice, acceleration_device, acceleration_device_std, filter_valid)
if self.debug:
deviceMotion.debugFilterState.value = state.tolist()
deviceMotion.debugFilterState.std = std.tolist()
deviceMotion.debugFilterState.valid = filter_valid
deviceMotion.debugFilterState.observations = [
{'kind': k, 'value': self.observations[k].tolist(), 'error': self.observation_errors[k].tolist()}
for k in self.observations.keys()
]
old_mean = np.mean(self.posenet_stds[:POSENET_STD_HIST_HALF])
new_mean = np.mean(self.posenet_stds[POSENET_STD_HIST_HALF:])
std_spike = (new_mean / old_mean) > 4.0 and new_mean > 7.0
deviceMotion.inputsOK = inputs_valid
deviceMotion.posenetOK = not std_spike or self.car_speed <= 5.0
deviceMotion.sensorsOK = sensors_valid
return msg
def sensor_all_checks(acc_msgs, gyro_msgs, sensor_valid, sensor_recv_time, sensor_alive, simulation):
cur_time = time.monotonic()
for which, msgs in [("accelerometer", acc_msgs), ("gyroscope", gyro_msgs)]:
if len(msgs) > 0:
sensor_valid[which] = msgs[-1].valid
sensor_recv_time[which] = cur_time
if not simulation:
sensor_alive[which] = (cur_time - sensor_recv_time[which]) < 0.1
else:
sensor_alive[which] = len(msgs) > 0
return all(sensor_alive.values()) and all(sensor_valid.values())
def main():
config_realtime_process([0, 1, 2, 3], 5)
DEBUG = bool(int(os.getenv("DEBUG", "0")))
SIMULATION = bool(int(os.getenv("SIMULATION", "0")))
pm = messaging.PubMaster(['deviceMotion'])
sm = messaging.SubMaster(['carState', 'extrinsicsCalibration', 'cameraOdometry'], poll='cameraOdometry')
# separate sensor sockets for efficiency
sensor_sockets = [messaging.sub_sock(which, timeout=20) for which in ['accelerometer', 'gyroscope']]
sensor_alive, sensor_valid, sensor_recv_time = defaultdict(bool), defaultdict(bool), defaultdict(float)
params = Params()
estimator = LocationEstimator(DEBUG)
filter_initialized = False
critcal_services = ["accelerometer", "gyroscope", "cameraOdometry"]
observation_input_invalid = defaultdict(int)
input_invalid_limit = {s: round(INPUT_INVALID_LIMIT * (SERVICE_LIST[s].frequency / 20.)) for s in critcal_services}
input_invalid_threshold = {s: input_invalid_limit[s] - 0.5 for s in critcal_services}
input_invalid_decay = {s: calculate_invalid_input_decay(input_invalid_limit[s], INPUT_INVALID_RECOVERY, SERVICE_LIST[s].frequency) for s in critcal_services}
initial_pose_data = params.get("LocationFilterInitialState")
if initial_pose_data is not None:
with log.Event.from_bytes(initial_pose_data) as lp_msg:
filter_state = lp_msg.deviceMotion.debugFilterState
x_initial = np.array(filter_state.value, dtype=np.float64) if len(filter_state.value) != 0 else PoseKalman.initial_x
P_initial = np.diag(np.array(filter_state.std, dtype=np.float64)) if len(filter_state.std) != 0 else PoseKalman.initial_P
estimator.reset(None, x_initial, P_initial)
while True:
sm.update()
acc_msgs, gyro_msgs = (messaging.drain_sock(sock) for sock in sensor_sockets)
if filter_initialized:
msgs = []
for msg in acc_msgs + gyro_msgs:
t, valid, which, data = msg.logMonoTime, msg.valid, msg.which(), getattr(msg, msg.which())
msgs.append((t, valid, which, data))
for which, updated in sm.updated.items():
if not updated:
continue
t, valid, data = sm.logMonoTime[which], sm.valid[which], sm[which]
msgs.append((t, valid, which, data))
for log_mono_time, valid, which, msg in sorted(msgs, key=lambda x: x[0]):
if valid:
t = log_mono_time * 1e-9
res = estimator.handle_log(t, which, msg)
if which not in critcal_services:
continue
if res == HandleLogResult.TIMING_INVALID:
cloudlog.warning(f"Observation {which} ignored due to failed timing check")
observation_input_invalid[which] += 1
elif res == HandleLogResult.INPUT_INVALID:
cloudlog.warning(f"Observation {which} ignored due to failed sanity check")
observation_input_invalid[which] += 1
elif res == HandleLogResult.SUCCESS:
observation_input_invalid[which] *= input_invalid_decay[which]
else:
filter_initialized = sm.all_checks() and sensor_all_checks(acc_msgs, gyro_msgs, sensor_valid, sensor_recv_time, sensor_alive, SIMULATION)
if sm.updated["cameraOdometry"]:
critical_service_inputs_valid = all(observation_input_invalid[s] < input_invalid_threshold[s] for s in critcal_services)
inputs_valid = sm.all_valid() and critical_service_inputs_valid
sensors_valid = sensor_all_checks(acc_msgs, gyro_msgs, sensor_valid, sensor_recv_time, sensor_alive, SIMULATION)
msg = estimator.get_msg(sensors_valid, inputs_valid, filter_initialized)
pm.send("deviceMotion", msg)
if __name__ == "__main__":
main()

0
iqdbc_repo/iqdbc/car/gm/tests/__init__.py → artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/locationd/models/__init__.py
View File

88
artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/locationd/models/constants.py Normal file
View File

@@ -0,0 +1,88 @@
class ObservationKind:
UNKNOWN = 0
NO_OBSERVATION = 1
GPS_NED = 2
ODOMETRIC_SPEED = 3
PHONE_GYRO = 4
GPS_VEL = 5
PSEUDORANGE_GPS = 6
PSEUDORANGE_RATE_GPS = 7
SPEED = 8
NO_ROT = 9
PHONE_ACCEL = 10
ORB_POINT = 11
ECEF_POS = 12
CAMERA_ODO_TRANSLATION = 13
CAMERA_ODO_ROTATION = 14
ORB_FEATURES = 15
MSCKF_TEST = 16
FEATURE_TRACK_TEST = 17
LANE_PT = 18
IMU_FRAME = 19
PSEUDORANGE_GLONASS = 20
PSEUDORANGE_RATE_GLONASS = 21
PSEUDORANGE = 22
PSEUDORANGE_RATE = 23
ECEF_VEL = 35
ECEF_ORIENTATION_FROM_GPS = 32
NO_ACCEL = 33
ORB_FEATURES_WIDE = 34
ROAD_FRAME_XY_SPEED = 24 # (x, y) [m/s]
ROAD_FRAME_YAW_RATE = 25 # [rad/s]
STEER_ANGLE = 26 # [rad]
ANGLE_OFFSET_FAST = 27 # [rad]
STIFFNESS = 28 # [-]
STEER_RATIO = 29 # [-]
ROAD_FRAME_X_SPEED = 30 # (x) [m/s]
ROAD_ROLL = 31 # [rad]
names = [
'Unknown',
'No observation',
'GPS NED',
'Odometric speed',
'Phone gyro',
'GPS velocity',
'GPS pseudorange',
'GPS pseudorange rate',
'Speed',
'No rotation',
'Phone acceleration',
'ORB point',
'ECEF pos',
'camera odometric translation',
'camera odometric rotation',
'ORB features',
'MSCKF test',
'Feature track test',
'Lane ecef point',
'imu frame eulers',
'GLONASS pseudorange',
'GLONASS pseudorange rate',
'pseudorange',
'pseudorange rate',
'Road Frame x,y speed',
'Road Frame yaw rate',
'Steer Angle',
'Fast Angle Offset',
'Stiffness',
'Steer Ratio',
'Road Frame x speed',
'Road Roll',
'ECEF orientation from GPS',
'NO accel',
'ORB features wide camera',
'ECEF_VEL',
]
@classmethod
def to_string(cls, kind):
return cls.names[kind]
SAT_OBS = [ObservationKind.PSEUDORANGE_GPS,
ObservationKind.PSEUDORANGE_RATE_GPS,
ObservationKind.PSEUDORANGE_GLONASS,
ObservationKind.PSEUDORANGE_RATE_GLONASS]

67
artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/locationd/models/pose_kf.py Normal file
View File

@@ -0,0 +1,67 @@
"""
Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
"""
import numpy as np
from iqpilot.common.transformations.orientation import euler_from_rot, rot_from_euler
from iqpilot.selfdrive.locationd.models.constants import ObservationKind
from iqpilot.selfdrive.state_estimation import EstimatorModel, ModelDefinition, StateEstimator
try:
from iqpilot.selfdrive.state_estimation.native_binding_pyx import pose_predict, pose_update
except ModuleNotFoundError:
pose_predict = None
pose_update = None
EARTH_G = 9.81
class States:
NED_ORIENTATION = slice(0, 3)
DEVICE_VELOCITY = slice(3, 6)
ANGULAR_VELOCITY = slice(6, 9)
GYRO_BIAS = slice(9, 12)
ACCELERATION = slice(12, 15)
ACCEL_BIAS = slice(15, 18)
def _transition(state: np.ndarray, dt: float, _: dict[str, float]) -> np.ndarray:
result = state.copy()
result[States.DEVICE_VELOCITY] += dt * state[States.ACCELERATION]
rotation = rot_from_euler(state[States.NED_ORIENTATION]) @ rot_from_euler(dt * state[States.ANGULAR_VELOCITY])
result[States.NED_ORIENTATION] = euler_from_rot(rotation)
return result
def _phone_acceleration(state: np.ndarray, _: dict[str, float]) -> np.ndarray:
device_from_ned = rot_from_euler(state[States.NED_ORIENTATION]).T
centripetal = np.cross(state[States.ANGULAR_VELOCITY], state[States.DEVICE_VELOCITY])
return device_from_ned @ np.array([0.0, 0.0, -EARTH_G]) + state[States.ACCELERATION] + centripetal + state[States.ACCEL_BIAS]
class PoseKalman(EstimatorModel):
name = "pose"
initial_x = np.zeros(18)
initial_P = np.diag([0.01**2] * 3 + [10**2] * 3 + [1**2] * 6 + [100**2] * 3 + [0.01**2] * 3)
Q = np.diag([0.001**2] * 3 + [0.01**2] * 3 + [0.1**2] * 3 + [(0.005 / 100)**2] * 3 + [3**2] * 3 + [0.005**2] * 3)
obs_noise = {
ObservationKind.PHONE_GYRO: np.diag([0.025**2] * 3),
ObservationKind.PHONE_ACCEL: np.diag([0.5**2] * 3),
ObservationKind.CAMERA_ODO_TRANSLATION: np.diag([0.5**2] * 3),
ObservationKind.CAMERA_ODO_ROTATION: np.diag([0.05**2] * 3),
}
def __init__(self, max_rewind_age: float):
measurements = {
ObservationKind.PHONE_GYRO: lambda state, _: state[States.ANGULAR_VELOCITY] + state[States.GYRO_BIAS],
ObservationKind.PHONE_ACCEL: _phone_acceleration,
ObservationKind.CAMERA_ODO_TRANSLATION: lambda state, _: state[States.DEVICE_VELOCITY],
ObservationKind.CAMERA_ODO_ROTATION: lambda state, _: state[States.ANGULAR_VELOCITY],
}
def native_predict(state, covariance, dt, process_noise, _):
pose_predict(state, covariance, process_noise, dt)
model = ModelDefinition(18, 18, _transition, measurements, self.Q, self.obs_noise,
native_predict=native_predict if pose_predict is not None else None, native_update=pose_update)
super().__init__(StateEstimator(model, self.initial_x, self.initial_P, max_rewind_age=max_rewind_age))

0
iqdbc_repo/iqdbc/car/honda/__init__.py → artifacts/iqpilot_emac_private/python/_iqclosure/iqpilot/selfdrive/selfdrived/__init__.py
View File

Some files were not shown because too many files have changed in this diff Show More