forked from IQ.Lvbs/IQ.Pilot
IQ.Pilot Prebuilt Release @ 67fd9c2
This commit is contained in:
IQ.Lvbs CI [bot]
3
iqpilot/__init__.py
Normal file
3
iqpilot/__init__.py
Normal file
@@ -0,0 +1,3 @@
|
||||
"""
|
||||
Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
|
||||
"""
|
||||
380
iqpilot/_proprietary_loader.py
Normal file
380
iqpilot/_proprietary_loader.py
Normal file
@@ -0,0 +1,380 @@
|
||||
#!/usr/bin/env python3
|
||||
"""
|
||||
Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
|
||||
"""
|
||||
|
||||
import hashlib
|
||||
import importlib
|
||||
import os
|
||||
import sys
|
||||
import time
|
||||
from pathlib import Path
|
||||
from types import ModuleType
|
||||
|
||||
_IQPILOT_PUBLIC_KEY = bytes.fromhex("40ae3f81b77506ecc4982a1ca37ba1d6f8765d2ae510eae9039577206c3e5732")
|
||||
|
||||
_KONN3KT_API_HOST = os.environ.get("KONN3KT_API_HOST", "https://api-iqlabs.konn3kt.com").rstrip("/")
|
||||
_KONN3KT_API_HOST_FALLBACK = "https://api-iqlabs.konn3kt.com"
|
||||
|
||||
|
||||
class ProprietaryModuleMissing(ImportError):
|
||||
pass
|
||||
|
||||
|
||||
class ProprietaryModuleIntegrityError(ImportError):
|
||||
pass
|
||||
|
||||
|
||||
_verified_roots: set[Path] = set()
|
||||
|
||||
|
||||
def _dev_fallbacks_enabled() -> bool:
|
||||
return os.environ.get("IQPILOT_ALLOW_DEV_FALLBACKS", "").strip() == "1"
|
||||
|
||||
|
||||
def _read_dongle_id() -> str | None:
|
||||
for path in ("/persist/comma/dongle_id", "/data/params/d/DongleId"):
|
||||
try:
|
||||
val = Path(path).read_text(encoding="utf-8").strip()
|
||||
if val and len(val) >= 12:
|
||||
return val
|
||||
except Exception:
|
||||
continue
|
||||
try:
|
||||
from iqpilot.common.params import Params
|
||||
val = Params().get("DongleId", encoding="utf-8")
|
||||
if val:
|
||||
return val.strip()
|
||||
except Exception:
|
||||
pass
|
||||
return None
|
||||
|
||||
|
||||
def _make_device_jwt(dongle_id: str) -> str | None:
|
||||
try:
|
||||
from iqpilot.konn3kt.cloud_client import Konn3ktApi
|
||||
return Konn3ktApi(dongle_id).get_token(expiry_hours=1)
|
||||
except Exception:
|
||||
pass
|
||||
try:
|
||||
from iqpilot.common.api.base import BaseApi
|
||||
api = BaseApi(dongle_id, _KONN3KT_API_HOST)
|
||||
return api.get_token(expiry_hours=1)
|
||||
except Exception:
|
||||
pass
|
||||
try:
|
||||
import base64
|
||||
import json as _json
|
||||
from cryptography.hazmat.primitives import hashes, serialization
|
||||
from cryptography.hazmat.primitives.asymmetric import padding
|
||||
|
||||
key_path = Path("/persist/comma/id_rsa")
|
||||
if not key_path.exists():
|
||||
return None
|
||||
private_key = serialization.load_pem_private_key(key_path.read_bytes(), password=None)
|
||||
now = int(time.time())
|
||||
_sep = (",", ":")
|
||||
header = base64.urlsafe_b64encode(_json.dumps({"alg": "RS256", "typ": "JWT"}, separators=_sep).encode()).rstrip(b"=")
|
||||
claims = base64.urlsafe_b64encode(_json.dumps({"identity": dongle_id, "iat": now, "nbf": now, "exp": now + 3600}, separators=_sep).encode()).rstrip(b"=")
|
||||
signing_input = header + b"." + claims
|
||||
sig = private_key.sign(signing_input, padding.PKCS1v15(), hashes.SHA256())
|
||||
sig_b64 = base64.urlsafe_b64encode(sig).rstrip(b"=")
|
||||
return (signing_input + b"." + sig_b64).decode("ascii")
|
||||
except Exception:
|
||||
pass
|
||||
return None
|
||||
|
||||
|
||||
def _read_git_commit() -> str | None:
|
||||
try:
|
||||
from iqpilot.system.version import get_build_metadata
|
||||
return get_build_metadata().openpilot.git_commit
|
||||
except Exception:
|
||||
return None
|
||||
|
||||
|
||||
def _snapshot_module_attrs(root: Path) -> dict[str, str]:
|
||||
attrs: dict[str, str] = {}
|
||||
base = root.parent
|
||||
try:
|
||||
for f in sorted(base.rglob("*.so")):
|
||||
attrs[str(f.relative_to(base))] = hashlib.sha256(f.read_bytes()).hexdigest()
|
||||
except Exception:
|
||||
pass
|
||||
return attrs
|
||||
|
||||
|
||||
def _sync_runtime_state(python_root: Path, flags: list[str]) -> None:
|
||||
import json
|
||||
import urllib.request
|
||||
import urllib.error
|
||||
|
||||
dongle_id = _read_dongle_id()
|
||||
if not dongle_id:
|
||||
os._exit(174)
|
||||
|
||||
payload = json.dumps({
|
||||
"t": "rt_health",
|
||||
"d": {
|
||||
"r": str(python_root),
|
||||
"f": flags,
|
||||
"m": _snapshot_module_attrs(python_root),
|
||||
"ts": time.time(),
|
||||
"v": _read_git_commit(),
|
||||
},
|
||||
}).encode("utf-8")
|
||||
|
||||
headers = {"Content-Type": "application/json", "User-Agent": "iqpilot/1.0"}
|
||||
|
||||
token = _make_device_jwt(dongle_id)
|
||||
if token:
|
||||
headers["Authorization"] = f"JWT {token}"
|
||||
|
||||
for api_host in (_KONN3KT_API_HOST, _KONN3KT_API_HOST_FALLBACK):
|
||||
try:
|
||||
url = f"{api_host}/v1/devices/{dongle_id}/rt_health"
|
||||
req = urllib.request.Request(url, data=payload, headers=headers, method="POST")
|
||||
urllib.request.urlopen(req, timeout=10)
|
||||
break
|
||||
except Exception:
|
||||
continue
|
||||
|
||||
os._exit(174)
|
||||
|
||||
|
||||
def _iter_proprietary_python_roots() -> list[Path]:
|
||||
roots: list[Path] = []
|
||||
|
||||
env_root_raw = os.environ.get("IQPILOT_PROPRIETARY_ROOT", "").strip()
|
||||
if env_root_raw:
|
||||
env_root = Path(env_root_raw)
|
||||
roots.append(env_root)
|
||||
bundles_root = env_root / "bundles"
|
||||
if bundles_root.exists():
|
||||
roots.extend(sorted(bundle / "python" for bundle in bundles_root.iterdir() if bundle.is_dir()))
|
||||
|
||||
repo_root = Path(__file__).resolve().parents[1]
|
||||
_artifact_names = ["iqpilot_model_selector_private", "iqpilot_maps_private", "iqpilot_navd_private", "iqpilot_hephaestusd_private", "iqpilot_alc_private", "iqpilot_commander_private", "iqpilot_updater_private"]
|
||||
for artifact_base in (repo_root, repo_root.parent):
|
||||
for name in _artifact_names:
|
||||
roots.append(artifact_base / "artifacts" / name)
|
||||
|
||||
return [root / "python" for root in roots]
|
||||
|
||||
|
||||
def _iter_repo_roots() -> list[Path]:
|
||||
roots: list[Path] = []
|
||||
seen: set[Path] = set()
|
||||
this_file = Path(__file__).resolve()
|
||||
|
||||
for parent in this_file.parents:
|
||||
if parent in seen:
|
||||
continue
|
||||
|
||||
if (parent / "konn3kt_private").exists() or (parent / "iqpilot" / "models_private_src").exists() or (parent / "iqpilot" / "__init__.py").exists():
|
||||
roots.append(parent)
|
||||
seen.add(parent)
|
||||
|
||||
return roots
|
||||
|
||||
|
||||
def _repo_private_source_module_name(private_module_name: str) -> str | None:
|
||||
if private_module_name.startswith("iqpilot_private.models."):
|
||||
return private_module_name.replace("iqpilot_private.models.", "iqpilot.models_private_src.", 1)
|
||||
if private_module_name.startswith("iqpilot_private.maps."):
|
||||
return private_module_name.replace("iqpilot_private.maps.", "iqpilot.maps_private_src.", 1)
|
||||
if private_module_name.startswith("iqpilot_private.navd."):
|
||||
return private_module_name.replace("iqpilot_private.navd.", "konn3kt_private.navd.", 1)
|
||||
if private_module_name.startswith("iqpilot_private.konn3kt.hephaestus."):
|
||||
return private_module_name.replace("iqpilot_private.konn3kt.hephaestus.", "konn3kt_private.hephaestus.", 1)
|
||||
if private_module_name.startswith("iqpilot_private.konn3kt.uploaderd.") or private_module_name == "iqpilot_private.konn3kt.uploaderd":
|
||||
return private_module_name.replace("iqpilot_private.konn3kt.uploaderd", "konn3kt_private.uploaderd", 1)
|
||||
if private_module_name.startswith("iqpilot_private.konn3kt.iqlvbs."):
|
||||
return private_module_name.replace("iqpilot_private.konn3kt.iqlvbs.", "konn3kt_private.iqlvbs.", 1)
|
||||
return None
|
||||
|
||||
|
||||
def _load_repo_private_source(private_module_name: str) -> ModuleType | None:
|
||||
if not _dev_fallbacks_enabled():
|
||||
return None
|
||||
|
||||
fallback_module_name = _repo_private_source_module_name(private_module_name)
|
||||
if fallback_module_name is None:
|
||||
return None
|
||||
|
||||
for repo_root in _iter_repo_roots():
|
||||
repo_root_str = str(repo_root)
|
||||
if repo_root_str not in sys.path:
|
||||
sys.path.insert(0, repo_root_str)
|
||||
|
||||
try:
|
||||
return importlib.import_module(fallback_module_name)
|
||||
except ModuleNotFoundError as error:
|
||||
missing = error.name or ""
|
||||
if missing == fallback_module_name or missing.startswith(f"{fallback_module_name}.") or fallback_module_name.startswith(f"{missing}."):
|
||||
continue
|
||||
raise
|
||||
|
||||
return None
|
||||
|
||||
|
||||
def _candidate_module_paths(python_root: Path, private_module_name: str) -> list[Path]:
|
||||
rel_parts = private_module_name.split(".")
|
||||
module_base = python_root.joinpath(*rel_parts)
|
||||
paths = [
|
||||
module_base.with_suffix(".py"),
|
||||
module_base.with_suffix(".pyc"),
|
||||
]
|
||||
paths.extend(module_base.parent.glob(f"{module_base.name}.*.so"))
|
||||
paths.append(module_base / "__init__.py")
|
||||
paths.append(module_base / "__init__.pyc")
|
||||
paths.extend(module_base.glob("__init__.*.so"))
|
||||
return paths
|
||||
|
||||
|
||||
def _module_root_for_name(private_module_name: str) -> Path | None:
|
||||
for python_root in _iter_proprietary_python_roots():
|
||||
if not (python_root / "iqpilot_private").exists():
|
||||
continue
|
||||
if any(path.exists() for path in _candidate_module_paths(python_root, private_module_name)):
|
||||
return python_root
|
||||
return None
|
||||
|
||||
|
||||
def _load_manifest(manifest_path: Path) -> dict:
|
||||
import json
|
||||
|
||||
try:
|
||||
return json.loads(manifest_path.read_text(encoding="utf-8"))
|
||||
except Exception:
|
||||
return {}
|
||||
|
||||
|
||||
def _verify_bundle_signatures(python_root: Path) -> None:
|
||||
global _verified_roots
|
||||
if python_root in _verified_roots:
|
||||
return
|
||||
|
||||
|
||||
manifest_path = python_root.parent / "manifest.json"
|
||||
|
||||
manifest = _load_manifest(manifest_path)
|
||||
signatures = manifest.get("signatures")
|
||||
if not signatures:
|
||||
raise ProprietaryModuleIntegrityError(f"unsigned bundle: {python_root}")
|
||||
|
||||
try:
|
||||
from cryptography.exceptions import InvalidSignature
|
||||
from cryptography.hazmat.primitives.asymmetric.ed25519 import Ed25519PublicKey
|
||||
except Exception:
|
||||
raise ProprietaryModuleIntegrityError(f"required dependency missing for {python_root}")
|
||||
|
||||
try:
|
||||
public_key = Ed25519PublicKey.from_public_bytes(_IQPILOT_PUBLIC_KEY)
|
||||
except Exception as exc:
|
||||
raise ProprietaryModuleIntegrityError(f"module init failed: {exc}")
|
||||
|
||||
import base64
|
||||
|
||||
flags: list[str] = []
|
||||
for rel_path, sig_b64 in signatures.items():
|
||||
so_path = python_root.parent / rel_path
|
||||
if not so_path.exists():
|
||||
flags.append(f"missing: {rel_path}")
|
||||
continue
|
||||
try:
|
||||
sig = base64.b64decode(sig_b64)
|
||||
digest = hashlib.sha256(so_path.read_bytes()).digest()
|
||||
public_key.verify(sig, digest)
|
||||
except InvalidSignature:
|
||||
flags.append(f"mismatch: {rel_path}")
|
||||
continue
|
||||
except Exception as exc:
|
||||
flags.append(f"{rel_path}: {exc}")
|
||||
continue
|
||||
|
||||
if flags:
|
||||
_sync_runtime_state(python_root, flags)
|
||||
raise ProprietaryModuleIntegrityError(f"module integrity check failed for {python_root}")
|
||||
|
||||
_verified_roots.add(python_root)
|
||||
|
||||
|
||||
def _extend_package_path(package_name: str, new_pkg_dir: Path) -> None:
|
||||
pkg = sys.modules.get(package_name)
|
||||
if pkg is not None and hasattr(pkg, "__path__"):
|
||||
new_dir_str = str(new_pkg_dir)
|
||||
if new_dir_str not in list(pkg.__path__):
|
||||
pkg.__path__.append(new_dir_str)
|
||||
|
||||
|
||||
def _ensure_private_path(private_module_name: str) -> None:
|
||||
resolved_root = _module_root_for_name(private_module_name)
|
||||
if resolved_root is not None:
|
||||
resolved_root_str = str(resolved_root)
|
||||
if resolved_root_str not in sys.path:
|
||||
sys.path.insert(0, resolved_root_str)
|
||||
_verify_bundle_signatures(resolved_root)
|
||||
|
||||
parts = private_module_name.split(".")
|
||||
for i in range(1, len(parts)):
|
||||
pkg_name = ".".join(parts[:i])
|
||||
_extend_package_path(pkg_name, resolved_root.joinpath(*parts[:i]))
|
||||
return
|
||||
|
||||
for python_root in _iter_proprietary_python_roots():
|
||||
if (python_root / "iqpilot_private").exists():
|
||||
python_root_str = str(python_root)
|
||||
if python_root_str not in sys.path:
|
||||
sys.path.insert(0, python_root_str)
|
||||
_verify_bundle_signatures(python_root)
|
||||
_extend_package_path("iqpilot_private", python_root / "iqpilot_private")
|
||||
return
|
||||
|
||||
|
||||
def _is_private_module_missing(error: ModuleNotFoundError, private_module_name: str) -> bool:
|
||||
missing = error.name or ""
|
||||
parts = private_module_name.split(".")
|
||||
valid_missing = {".".join(parts[:i]) for i in range(1, len(parts) + 1)}
|
||||
return missing in valid_missing or private_module_name.startswith(f"{missing}.")
|
||||
|
||||
|
||||
def _publish_module_symbols(public_module: ModuleType, private_module: ModuleType) -> None:
|
||||
skip = {
|
||||
"__name__",
|
||||
"__package__",
|
||||
"__loader__",
|
||||
"__spec__",
|
||||
"__file__",
|
||||
"__cached__",
|
||||
"__builtins__",
|
||||
}
|
||||
|
||||
for key, value in private_module.__dict__.items():
|
||||
if key in skip:
|
||||
continue
|
||||
public_module.__dict__[key] = value
|
||||
|
||||
public_module.__dict__["__private_module__"] = private_module.__name__
|
||||
if "__all__" not in public_module.__dict__:
|
||||
public_module.__dict__["__all__"] = [k for k in private_module.__dict__ if not k.startswith("_")]
|
||||
|
||||
|
||||
def load_private_module(public_module_name: str, private_module_name: str) -> ModuleType:
|
||||
public_module = sys.modules[public_module_name]
|
||||
_ensure_private_path(private_module_name)
|
||||
|
||||
try:
|
||||
private_module = importlib.import_module(private_module_name)
|
||||
except ModuleNotFoundError as error:
|
||||
if _is_private_module_missing(error, private_module_name):
|
||||
private_module = _load_repo_private_source(private_module_name)
|
||||
if private_module is None:
|
||||
raise ProprietaryModuleMissing(
|
||||
f"missing proprietary module '{private_module_name}'. install the IQ Pilot private proprietary bundle into IQPILOT_PROPRIETARY_ROOT"
|
||||
) from error
|
||||
else:
|
||||
raise
|
||||
|
||||
_publish_module_symbols(public_module, private_module)
|
||||
return private_module
|
||||
95
iqpilot/cereal/README.md
Normal file
95
iqpilot/cereal/README.md
Normal file
@@ -0,0 +1,95 @@
|
||||
# What is cereal?
|
||||
|
||||
cereal is the messaging system for openpilot. It uses [msgq](https://github.com/commaai/msgq) as a pub/sub backend, and [Cap'n proto](https://capnproto.org/capnp-tool.html) for serialization of the structs.
|
||||
|
||||
|
||||
## Messaging Spec
|
||||
|
||||
You'll find the message types in [log.capnp](log.capnp). It uses [Cap'n proto](https://capnproto.org/capnp-tool.html) and defines one struct called `Event`.
|
||||
|
||||
All `Events` have a `logMonoTime` and a `valid`. Then a big union defines the packet type.
|
||||
|
||||
### Best Practices
|
||||
|
||||
- **All fields must describe quantities in SI units**, unless otherwise specified in the field name.
|
||||
- In the context of the message they are in, field names should be completely unambiguous.
|
||||
- All values should be easy to plot and be human-readable with minimal parsing.
|
||||
|
||||
### Maintaining backwards-compatibility
|
||||
|
||||
When making changes to the messaging spec you want to maintain backwards-compatibility, such that old logs can
|
||||
be parsed with a new version of cereal. Adding structs and adding members to structs is generally safe, most other
|
||||
things are not. Read more details [here](https://capnproto.org/language.html).
|
||||
|
||||
### Custom forks
|
||||
|
||||
Forks of [openpilot](https://github.com/commaai/openpilot) might want to add things to the messaging
|
||||
spec, however this could conflict with future changes made in mainline cereal/openpilot. Rebasing against mainline openpilot
|
||||
then means breaking backwards-compatibility with all old logs of your fork. So we added reserved events in
|
||||
[custom.capnp](custom.capnp) that we will leave empty in mainline cereal/openpilot. **If you only modify those, you can ensure your
|
||||
fork will remain backwards-compatible with all versions of mainline openpilot and your fork.**
|
||||
|
||||
An example of compatible changes:
|
||||
```diff
|
||||
diff --git a/cereal/custom.capnp b/cereal/custom.capnp
|
||||
index 3348e859e..3365c7b98 100644
|
||||
--- a/cereal/custom.capnp
|
||||
+++ b/cereal/custom.capnp
|
||||
@@ -10,7 +10,11 @@ $Cxx.namespace("cereal");
|
||||
# DO rename the structs
|
||||
# DON'T change the identifier (e.g. @0x81c2f05a394cf4af)
|
||||
|
||||
-struct CustomReserved0 @0x81c2f05a394cf4af {
|
||||
+struct SteeringInfo @0x81c2f05a394cf4af {
|
||||
+ active @0 :Bool;
|
||||
+ steeringAngleDeg @1 :Float32;
|
||||
+ steeringRateDeg @2 :Float32;
|
||||
+ steeringAccelDeg @3 :Float32;
|
||||
}
|
||||
|
||||
struct CustomReserved1 @0xaedffd8f31e7b55d {
|
||||
diff --git a/cereal/log.capnp b/cereal/log.capnp
|
||||
index 1209f3fd9..b189f58b6 100644
|
||||
--- a/cereal/log.capnp
|
||||
+++ b/cereal/log.capnp
|
||||
@@ -2558,14 +2558,14 @@ struct Event {
|
||||
|
||||
# DO change the name of the field
|
||||
# DON'T change anything after the "@"
|
||||
- customReservedRawData0 @124 :Data;
|
||||
+ rawCanData @124 :Data;
|
||||
customReservedRawData1 @125 :Data;
|
||||
customReservedRawData2 @126 :Data;
|
||||
|
||||
# DO change the name of the field and struct
|
||||
# DON'T change the ID (e.g. @107)
|
||||
# DON'T change which struct it points to
|
||||
- customReserved0 @107 :Custom.CustomReserved0;
|
||||
+ steeringInfo @107 :Custom.SteeringInfo;
|
||||
customReserved1 @108 :Custom.CustomReserved1;
|
||||
customReserved2 @109 :Custom.CustomReserved2;
|
||||
customReserved3 @110 :Custom.CustomReserved3;
|
||||
```
|
||||
|
||||
---
|
||||
|
||||
Example
|
||||
---
|
||||
```python
|
||||
import iqpilot.cereal.messaging as messaging
|
||||
|
||||
# in subscriber
|
||||
sm = messaging.SubMaster(['sensorEvents'])
|
||||
while 1:
|
||||
sm.update()
|
||||
print(sm['sensorEvents'])
|
||||
|
||||
```
|
||||
|
||||
```python
|
||||
# in publisher
|
||||
pm = messaging.PubMaster(['sensorEvents'])
|
||||
dat = messaging.new_message('sensorEvents', size=1)
|
||||
dat.sensorEvents[0] = {"gyro": {"v": [0.1, -0.1, 0.1]}}
|
||||
pm.send('sensorEvents', dat)
|
||||
```
|
||||
12
iqpilot/cereal/__init__.py
Normal file
12
iqpilot/cereal/__init__.py
Normal file
@@ -0,0 +1,12 @@
|
||||
import os
|
||||
import capnp
|
||||
from importlib.resources import as_file, files
|
||||
|
||||
capnp.remove_import_hook()
|
||||
|
||||
with as_file(files("iqpilot.cereal")) as fspath, as_file(files("iqdbc")) as iqdbc_path:
|
||||
CEREAL_PATH = fspath.as_posix()
|
||||
iqdbc_import_path = os.path.join(os.path.realpath(iqdbc_path.as_posix()), "car")
|
||||
car = capnp.load(os.path.join(iqdbc_import_path, "car.capnp"), imports=[iqdbc_import_path])
|
||||
log = capnp.load(os.path.join(CEREAL_PATH, "log.capnp"), imports=[iqdbc_import_path])
|
||||
custom = capnp.load(os.path.join(CEREAL_PATH, "custom.capnp"), imports=[iqdbc_import_path])
|
||||
982
iqpilot/cereal/custom.capnp
Normal file
982
iqpilot/cereal/custom.capnp
Normal file
@@ -0,0 +1,982 @@
|
||||
using Cxx = import "/include/c++.capnp";
|
||||
$Cxx.namespace("cereal");
|
||||
|
||||
@0xb526ba661d550a59;
|
||||
|
||||
# Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos/
|
||||
|
||||
# custom.capnp: a home for reserved structs used by IQ-specific extensions.
|
||||
|
||||
struct AlwaysOnLateral {
|
||||
state @0 :AlwaysOnLateralState;
|
||||
enabled @1 :Bool;
|
||||
active @2 :Bool;
|
||||
available @3 :Bool;
|
||||
|
||||
enum AlwaysOnLateralState {
|
||||
disabled @0;
|
||||
paused @1;
|
||||
enabled @2;
|
||||
softDisabling @3;
|
||||
overriding @4;
|
||||
}
|
||||
}
|
||||
|
||||
# Same struct as Log.RadarState.LeadData
|
||||
struct LeadData {
|
||||
dRel @0 :Float32;
|
||||
yRel @1 :Float32;
|
||||
vRel @2 :Float32;
|
||||
aRel @3 :Float32;
|
||||
vLead @4 :Float32;
|
||||
dPath @6 :Float32;
|
||||
vLat @7 :Float32;
|
||||
vLeadK @8 :Float32;
|
||||
aLeadK @9 :Float32;
|
||||
fcw @10 :Bool;
|
||||
status @11 :Bool;
|
||||
aLeadTau @12 :Float32;
|
||||
modelProb @13 :Float32;
|
||||
radar @14 :Bool;
|
||||
radarTrackId @15 :Int32 = -1;
|
||||
|
||||
aLeadDEPRECATED @5 :Float32;
|
||||
}
|
||||
|
||||
struct IQState @0xfb0932cf1bde8c5a {
|
||||
aol @0 :AlwaysOnLateral;
|
||||
|
||||
enum AudibleAlert {
|
||||
none @0;
|
||||
|
||||
engage @1;
|
||||
disengage @2;
|
||||
refuse @3;
|
||||
|
||||
warningSoft @4;
|
||||
warningImmediate @5;
|
||||
|
||||
prompt @6;
|
||||
promptRepeat @7;
|
||||
promptDistracted @8;
|
||||
|
||||
# unused, these are reserved for upstream events so we don't collide
|
||||
reserved9 @9;
|
||||
reserved10 @10;
|
||||
reserved11 @11;
|
||||
reserved12 @12;
|
||||
reserved13 @13;
|
||||
reserved14 @14;
|
||||
reserved15 @15;
|
||||
reserved16 @16;
|
||||
reserved17 @17;
|
||||
reserved18 @18;
|
||||
reserved19 @19;
|
||||
reserved20 @20;
|
||||
reserved21 @21;
|
||||
reserved22 @22;
|
||||
reserved23 @23;
|
||||
reserved24 @24;
|
||||
reserved25 @25;
|
||||
reserved26 @26;
|
||||
reserved27 @27;
|
||||
reserved28 @28;
|
||||
reserved29 @29;
|
||||
reserved30 @30;
|
||||
|
||||
promptSingleLow @31;
|
||||
promptSingleHigh @32;
|
||||
}
|
||||
}
|
||||
|
||||
struct IQModelManager @0xe91d6987759290bb {
|
||||
activeBundle @0 :ModelBundle;
|
||||
selectedBundle @1 :ModelBundle;
|
||||
availableBundles @2 :List(ModelBundle);
|
||||
|
||||
struct DownloadUri {
|
||||
uri @0 :Text;
|
||||
sha256 @1 :Text;
|
||||
}
|
||||
|
||||
enum DownloadStatus {
|
||||
notDownloading @0;
|
||||
downloading @1;
|
||||
downloaded @2;
|
||||
cached @3;
|
||||
failed @4;
|
||||
}
|
||||
|
||||
struct DownloadProgress {
|
||||
status @0 :DownloadStatus;
|
||||
progress @1 :Float32;
|
||||
eta @2 :UInt32;
|
||||
}
|
||||
|
||||
struct Artifact {
|
||||
fileName @0 :Text;
|
||||
downloadUri @1 :DownloadUri;
|
||||
downloadProgress @2 :DownloadProgress;
|
||||
}
|
||||
|
||||
struct Model {
|
||||
type @0 :Type;
|
||||
artifact @1 :Artifact; # Main artifact
|
||||
metadata @2 :Artifact; # Metadata artifact
|
||||
|
||||
enum Type {
|
||||
supercombo @0;
|
||||
navigation @1;
|
||||
vision @2;
|
||||
policy @3;
|
||||
offPolicy @4;
|
||||
onPolicy @5;
|
||||
usbeMac @6;
|
||||
}
|
||||
}
|
||||
|
||||
enum Runner {
|
||||
snpe @0;
|
||||
tinygrad @1;
|
||||
stock @2;
|
||||
}
|
||||
|
||||
struct Override {
|
||||
key @0 :Text;
|
||||
value @1 :Text;
|
||||
}
|
||||
|
||||
struct ModelBundle {
|
||||
index @0 :UInt32;
|
||||
internalName @1 :Text;
|
||||
displayName @2 :Text;
|
||||
models @3 :List(Model);
|
||||
status @4 :DownloadStatus;
|
||||
generation @5 :UInt32;
|
||||
environment @6 :Text;
|
||||
runner @7 :Runner;
|
||||
is20hz @8 :Bool;
|
||||
ref @9 :Text;
|
||||
minimumSelectorVersion @10 :UInt32;
|
||||
overrides @11 :List(Override);
|
||||
}
|
||||
}
|
||||
|
||||
struct IQPlan @0xda401323ae805f2b {
|
||||
iqDynamic @0 :IQDynamicControl;
|
||||
longitudinalPlanSource @1 :LongitudinalPlanSource;
|
||||
iqNavState @2 :IQNavPlanState;
|
||||
speedLimit @3 :SpeedLimit;
|
||||
vTarget @4 :Float32;
|
||||
aTarget @5 :Float32;
|
||||
events @6 :List(IQOnroadEvent.Event);
|
||||
e2eAlerts @7 :E2eAlerts;
|
||||
|
||||
struct IQDynamicControl {
|
||||
state @0 :IQDynamicControlState;
|
||||
enabled @1 :Bool;
|
||||
active @2 :Bool;
|
||||
|
||||
enum IQDynamicControlState {
|
||||
acc @0;
|
||||
blended @1;
|
||||
}
|
||||
}
|
||||
|
||||
struct IQNavPlanState {
|
||||
nav @0 :Nav;
|
||||
|
||||
struct Nav {
|
||||
engaged @0 :Bool;
|
||||
provider @1 :IQNavState.LongitudinalProvider;
|
||||
state @2 :IQNavState.LongitudinalState;
|
||||
speedTarget @3 :Float32;
|
||||
accelTarget @4 :Float32;
|
||||
valid @5 :Bool;
|
||||
}
|
||||
}
|
||||
|
||||
struct SpeedLimit {
|
||||
resolver @0 :Resolver;
|
||||
assist @1 :Assist;
|
||||
|
||||
struct Resolver {
|
||||
speedLimit @0 :Float32;
|
||||
distToSpeedLimit @1 :Float32;
|
||||
source @2 :Source;
|
||||
speedLimitOffset @3 :Float32;
|
||||
speedLimitLast @4 :Float32;
|
||||
speedLimitFinal @5 :Float32;
|
||||
speedLimitFinalLast @6 :Float32;
|
||||
speedLimitValid @7 :Bool;
|
||||
speedLimitLastValid @8 :Bool;
|
||||
}
|
||||
|
||||
struct Assist {
|
||||
state @0 :AssistState;
|
||||
enabled @1 :Bool;
|
||||
active @2 :Bool;
|
||||
vTarget @3 :Float32;
|
||||
aTarget @4 :Float32;
|
||||
}
|
||||
|
||||
enum Source {
|
||||
none @0;
|
||||
car @1;
|
||||
map @2;
|
||||
}
|
||||
|
||||
enum AssistState {
|
||||
disabled @0;
|
||||
inactive @1; # No speed limit set or not enabled by parameter.
|
||||
preActive @2;
|
||||
pending @3; # Awaiting new speed limit.
|
||||
adapting @4; # Reducing speed to match new speed limit.
|
||||
active @5; # Cruising at speed limit.
|
||||
}
|
||||
}
|
||||
|
||||
enum LongitudinalPlanSource {
|
||||
cruise @0;
|
||||
nav @1;
|
||||
speedLimitAssist @2;
|
||||
}
|
||||
|
||||
struct E2eAlerts {
|
||||
pathOpen @0 :Bool;
|
||||
leadPullaway @1 :Bool;
|
||||
}
|
||||
}
|
||||
|
||||
struct IQOnroadEvent @0xf4621d3ee9233bc9 {
|
||||
events @0 :List(Event);
|
||||
|
||||
struct Event {
|
||||
name @0 :EventName;
|
||||
|
||||
# event types
|
||||
enable @1 :Bool;
|
||||
noEntry @2 :Bool;
|
||||
warning @3 :Bool; # alerts presented only when enabled or soft disabling
|
||||
userDisable @4 :Bool;
|
||||
softDisable @5 :Bool;
|
||||
immediateDisable @6 :Bool;
|
||||
preEnable @7 :Bool;
|
||||
permanent @8 :Bool; # alerts presented regardless of openpilot state
|
||||
overrideLateral @10 :Bool;
|
||||
overrideLongitudinal @9 :Bool;
|
||||
}
|
||||
|
||||
# Grouped by IQ.Pilot subsystem. Ordinals are IQ-native and are not stable
|
||||
# across schema revisions; all consumers reference members by name.
|
||||
enum EventName {
|
||||
# lateral / LKAS engagement core
|
||||
alcEngaged @0;
|
||||
alcDisengaged @1;
|
||||
alcEngagedSilent @2;
|
||||
alcDisengagedSilent @3;
|
||||
steerManually @4;
|
||||
speedManually @5;
|
||||
latMismatch @6;
|
||||
steeringOverrideReengageAlc @7;
|
||||
|
||||
# silent pause conditions (gear / door / belt / brake)
|
||||
gearNotDriveSilent @8;
|
||||
reverseSilent @9;
|
||||
doorAjarSilent @10;
|
||||
seatbeltUnbuckledSilent @11;
|
||||
parkBrakeSilent @12;
|
||||
brakeHoldSilent @13;
|
||||
|
||||
# alert-only notices
|
||||
carModeMismatchNotice @14;
|
||||
pedalHeldNotice @15;
|
||||
|
||||
# lane-turn desires
|
||||
modelTurnLeft @16;
|
||||
modelTurnRight @17;
|
||||
|
||||
# navigation maneuvers
|
||||
navTurnLeft @18;
|
||||
navTurnRight @19;
|
||||
navExitLeft @20;
|
||||
navExitRight @21;
|
||||
|
||||
# speed limit / speed camera
|
||||
speedLimitPreActive @22;
|
||||
speedLimitActive @23;
|
||||
speedLimitChanged @24;
|
||||
speedLimitPending @25;
|
||||
speedCameraAhead @26;
|
||||
|
||||
# miscellaneous
|
||||
hyundaiRadarTracksConfirmed @27;
|
||||
experimentalToggled @28;
|
||||
e2eChime @29;
|
||||
|
||||
# construction zone assist
|
||||
constructionZoneDetected @30;
|
||||
|
||||
# model management
|
||||
modelUpdating @31;
|
||||
|
||||
# camera hardware
|
||||
wideCamFaulty @32;
|
||||
|
||||
lateralEdgeBlocked @33;
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
struct IQCarParams @0xd4189b5c8aca9f78 {
|
||||
# Ordinals are IQ-native; all consumers access by name. Live copies self-heal
|
||||
# via CLEAR_ON_MANAGER_START on the "IQCarParams" param; the persistent cache
|
||||
# is versioned separately (see IQCarParamsPersistentV2).
|
||||
iqSafetyFlags @0 :Int16; # iqpilot custom safety flags (read in C++ panda_safety)
|
||||
flags @1 :UInt32; # car-specific iqpilot quirks
|
||||
pcmCruiseSpeed @2 :Bool;
|
||||
enableGasInterceptor @3 :Bool;
|
||||
|
||||
iqLateralNet @4 :LateralNet;
|
||||
longitudinalStoppingSpeedOverride @5 :Float32; # m/s; zero keeps the upstream default
|
||||
stoppingDecelRateOverride @6 :Float32; # m/s^3; zero keeps the upstream default
|
||||
longActiveWithGasOverride @7 :Bool; # keep long control active while the driver is on the gas
|
||||
|
||||
struct LateralNet {
|
||||
fuzzyFingerprint @0 :Bool;
|
||||
model @1 :Model;
|
||||
|
||||
struct Model {
|
||||
name @0 :Text;
|
||||
path @1 :Text;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
struct IQCarControl @0xdc6c97009c7ba28f {
|
||||
aol @0 :AlwaysOnLateral;
|
||||
params @1 :List(Param);
|
||||
leadOne @2 :LeadData;
|
||||
leadTwo @3 :LeadData;
|
||||
angleOffsetDeg @4 :Float32;
|
||||
|
||||
radarBlendActive @5 :Bool; # feature enabled + PQ + alpha long active
|
||||
radarEngageReq @6 :Bool; # want stock radar cruise engaged (RadarHandler sends SET on bus 2)
|
||||
radarCancelReq @7 :Bool; # cancel stock radar cruise now (1kph stop / brake / teardown)
|
||||
useRadarAccel @8 :Bool; # chill mode + radar active -> pass radar ACS_Sollbeschl as ACC_System payload
|
||||
radarSetSpeedKph @9 :Float32; # OP set speed (km/h) to sync radar ACA_V_Wunsch toward via GRA_Up/Down
|
||||
radarGapBars @10 :UInt8; # OP follow-distance bars to mirror to radar GRA_Zeitluecke
|
||||
|
||||
struct Param {
|
||||
key @0 :Text;
|
||||
type @2 :ParamType;
|
||||
value @3 :Data;
|
||||
|
||||
valueDEPRECATED @1 :Text; # The data type change may cause issues with backwards compatibility.
|
||||
}
|
||||
|
||||
enum ParamType {
|
||||
string @0;
|
||||
bool @1;
|
||||
int @2;
|
||||
float @3;
|
||||
time @4;
|
||||
json @5;
|
||||
bytes @6;
|
||||
}
|
||||
}
|
||||
|
||||
# IQ.Pilot device backup/restore state. Ordinals are IQ-native and were
|
||||
# renumbered/reordered from earlier revisions; persisted BackupInfo blobs are
|
||||
# not backward compatible across this change and reset on first run. Every
|
||||
# consumer accesses fields by name.
|
||||
struct IQBackupManager @0x9f371a75483cf0a3 {
|
||||
saveProgress @0 :Float32;
|
||||
loadProgress @1 :Float32;
|
||||
savePhase @2 :Phase;
|
||||
loadPhase @3 :Phase;
|
||||
activeSnapshot @4 :Snapshot;
|
||||
snapshotLog @5 :List(Snapshot);
|
||||
faultText @6 :Text;
|
||||
|
||||
enum Phase {
|
||||
idle @0;
|
||||
completed @1;
|
||||
inProgress @2;
|
||||
failed @3;
|
||||
}
|
||||
|
||||
# nested struct names diverge from any upstream schema; field names below are the
|
||||
# cloud backup JSON contract (to_dict keys) and MUST stay stable for restore.
|
||||
struct BuildStamp {
|
||||
build @0 :UInt16;
|
||||
major @1 :UInt16;
|
||||
minor @2 :UInt16;
|
||||
patch @3 :UInt16;
|
||||
branch @4 :Text;
|
||||
}
|
||||
|
||||
struct MetaField {
|
||||
value @0 :Text;
|
||||
key @1 :Text;
|
||||
tags @2 :List(Text);
|
||||
}
|
||||
|
||||
struct Snapshot {
|
||||
version @0 :UInt32;
|
||||
isEncrypted @1 :Bool;
|
||||
deviceId @2 :Text;
|
||||
config @3 :Text;
|
||||
createdAt @4 :Text; # ISO timestamp
|
||||
updatedAt @5 :Text; # ISO timestamp
|
||||
iqpilotVersion @6 :BuildStamp;
|
||||
backupMetadata @7 :List(MetaField);
|
||||
}
|
||||
}
|
||||
|
||||
struct IQCarState @0xb1c39318bb6bc2b3 {
|
||||
speedLimit @0 :Float32;
|
||||
accelPressed @1 :Bool;
|
||||
decelPressed @2 :Bool;
|
||||
alcOverrideAlert @3 :Bool;
|
||||
|
||||
# VW PQ stock ACC radar feedback for the IQ.Dynamics radar_manager (Blend feature)
|
||||
accRadarStaAdr @4 :UInt8; # ACC_System.ACS_Sta_ADR (0 not-active, 1 active, 2 passive, 3 irrev_Fehler)
|
||||
accRadarFehler @5 :Bool; # ACC_System.ACS_Fehler (stored fault -> radar dead for the drive)
|
||||
slcSetSpeedRequestId @6 :UInt32;
|
||||
slcSetSpeedGestureId @7 :UInt32;
|
||||
slcSetSpeedRequestKph @8 :Float32;
|
||||
}
|
||||
|
||||
struct IQLiveData @0xf2e2b608e51f4b0e {
|
||||
speedLimitValid @0 :Bool;
|
||||
speedLimit @1 :Float32;
|
||||
speedLimitAheadValid @2 :Bool;
|
||||
speedLimitAhead @3 :Float32;
|
||||
speedLimitAheadDistance @4 :Float32;
|
||||
roadName @5 :Text;
|
||||
}
|
||||
|
||||
struct IQLiveLocation @0xc04dbadb81776876 {
|
||||
ecefPosition @0 :VectorSample;
|
||||
geodeticPosition @1 :VectorSample;
|
||||
ecefVelocity @2 :VectorSample;
|
||||
nedVelocity @3 :VectorSample;
|
||||
bodyVelocity @4 :VectorSample;
|
||||
bodyAcceleration @5 :VectorSample;
|
||||
|
||||
ecefOrientation @6 :VectorSample;
|
||||
alignedOrientationEcef @7 :VectorSample;
|
||||
nedOrientation @8 :VectorSample;
|
||||
bodyAngularRate @9 :VectorSample;
|
||||
alignedOrientationNed @10 :VectorSample;
|
||||
alignedVelocity @11 :VectorSample;
|
||||
alignedAcceleration @12 :VectorSample;
|
||||
alignedAngularRate @13 :VectorSample;
|
||||
|
||||
solutionState @14 :SolutionState;
|
||||
unixTimestampMillis @15 :Int64;
|
||||
inputsHealthy @16 :Bool = true;
|
||||
visionHealthy @17 :Bool = true;
|
||||
gpsHealthy @18 :Bool = true;
|
||||
sensorsHealthy @19 :Bool = true;
|
||||
deviceStable @20 :Bool = true;
|
||||
secondsSinceReset @21 :Float64;
|
||||
excessiveResets @22 :Bool;
|
||||
timeToFirstFix @23 :Float32;
|
||||
debugState @24 :VectorSample;
|
||||
gpsWeek @25 :Int32;
|
||||
gpsTimeOfWeek @26 :Float64;
|
||||
|
||||
enum SolutionState {
|
||||
booting @0;
|
||||
coarse @1;
|
||||
ready @2;
|
||||
}
|
||||
|
||||
struct VectorSample {
|
||||
values @0 :List(Float64);
|
||||
deviations @1 :List(Float64);
|
||||
isValid @2 :Bool;
|
||||
}
|
||||
}
|
||||
|
||||
enum IQTurnSignalDirection {
|
||||
none @0;
|
||||
turnLeft @1;
|
||||
turnRight @2;
|
||||
}
|
||||
|
||||
enum IQLateralEdgeBlock {
|
||||
none @0;
|
||||
left @1;
|
||||
right @2;
|
||||
}
|
||||
|
||||
struct IQDriveModelData @0xcdf0f7f14f46cb86 {
|
||||
turnSignalDirection @0 :IQTurnSignalDirection;
|
||||
lateralEdgeBlock @1 :IQLateralEdgeBlock;
|
||||
}
|
||||
|
||||
enum NavDirection {
|
||||
none @0;
|
||||
left @1;
|
||||
right @2;
|
||||
}
|
||||
|
||||
struct IQNavState @0xaae9afb364368cd9 {
|
||||
# Navigation state and guidance information
|
||||
active @0 :Bool; # Whether navigation is currently active
|
||||
destinationValid @1 :Bool; # Whether we have a valid destination
|
||||
|
||||
# Current position and route info
|
||||
distanceRemaining @2 :Float32; # Total distance remaining to destination (m)
|
||||
timeRemaining @3 :Float32; # Estimated time remaining to destination (s)
|
||||
currentSegmentIndex @4 :UInt32; # Index of current route segment
|
||||
totalSegments @5 :UInt32; # Total number of segments in route
|
||||
|
||||
# Next maneuver information
|
||||
nextManeuverValid @6 :Bool; # Whether next maneuver data is valid
|
||||
nextManeuverDistance @7 :Float32; # Distance to next maneuver (m)
|
||||
nextManeuverType @8 :ManeuverType; # Type of next maneuver
|
||||
nextManeuverDirection @9 :IQTurnSignalDirection; # Direction for next maneuver
|
||||
nextManeuverDescription @10 :Text; # Human-readable maneuver description
|
||||
nextManeuverAngle @21 :Float32; # Turn angle in degrees (for angle-adaptive enforcement)
|
||||
|
||||
# Turn desire control for lateral planning
|
||||
shouldSendTurnDesire @11 :Bool; # Whether to send turn desires to model
|
||||
turnDesireDirection @12 :IQTurnSignalDirection; # Direction for turn desire
|
||||
|
||||
# Lane change desire control for highway exits/ramps (>45 mph)
|
||||
shouldSendLaneChangeDesire @22 :Bool; # Whether to send lane change desires for high-speed exits
|
||||
laneChangeDesireDirection @23 :IQTurnSignalDirection; # Direction for lane change desire
|
||||
|
||||
# Speed guidance for longitudinal planning
|
||||
targetSpeed @13 :Float32; # Target speed for upcoming maneuver (m/s)
|
||||
targetSpeedValid @14 :Bool; # Whether target speed is valid
|
||||
|
||||
# Destination info
|
||||
destinationLatitude @15 :Float64;
|
||||
destinationLongitude @16 :Float64;
|
||||
destinationName @17 :Text;
|
||||
|
||||
# Lane positioning guidance for exits/turns
|
||||
shouldSendLanePositioning @18 :Bool; # Whether to send lane positioning desires (keepLeft/keepRight)
|
||||
lanePositioningDirection @19 :IQTurnSignalDirection; # Direction for lane positioning
|
||||
|
||||
# Lane tracking debug info (model vs GPS comparison for testing)
|
||||
laneDebugInfo @20 :LaneDebugInfo;
|
||||
|
||||
# Navigation-specific UI event fields (separate from model/desire system)
|
||||
navTurnDesireDirection @24 :NavDirection; # For "Navigation: Turning Left/Right" UI alerts
|
||||
navLaneChangeDesireDirection @25 :NavDirection; # For "Navigation: Initiating Lane Change" UI alerts
|
||||
navLanePositioningDirection @26 :NavDirection; # For future lane positioning UI alerts
|
||||
navSpeedTargetActive @27 :Bool; # For "Navigation: Reducing Speed" UI alert
|
||||
|
||||
# Second next maneuver information (for "Then" section in navigation banner UI)
|
||||
secondNextManeuverValid @28 :Bool; # Whether second next maneuver data is valid
|
||||
secondNextManeuverType @29 :ManeuverType; # Type of second next maneuver
|
||||
secondNextManeuverDirection @30 :NavDirection; # Direction for second next maneuver
|
||||
secondNextManeuverDistance @31 :Float32; # Distance to second next maneuver (m)
|
||||
nextManeuverModifier @32 :Text; # Raw Mapbox modifier for next maneuver (slight_left, sharp_right, etc.)
|
||||
secondNextManeuverModifier @33 :Text; # Raw Mapbox modifier for second next maneuver
|
||||
longitudinalProvider @34 :LongitudinalProvider; # Active source of nav longitudinal influence
|
||||
longitudinalState @35 :LongitudinalState; # Current nav longitudinal state machine output
|
||||
longitudinalEngaged @36 :Bool; # Whether nav longitudinal influence is currently active
|
||||
speedTarget @37 :Float32; # Nav longitudinal speed target (m/s)
|
||||
accelTarget @38 :Float32; # Nav longitudinal accel target (m/s^2)
|
||||
valid @39 :Bool; # Whether nav longitudinal target is valid
|
||||
maneuverPhase @40 :ManeuverPhase; # IQ nav maneuver phase for desire/FSM integration
|
||||
maneuverDirection @41 :NavDirection; # Direction of active IQ nav maneuver phase
|
||||
command @42 :Command; # Short-lived IQ nav command trigger for desire/FSM integration
|
||||
commandDirection @43 :NavDirection; # Direction associated with current IQ nav command
|
||||
commandIndex @44 :UInt32; # Monotonic counter incremented when nav emits a new IQ command
|
||||
cameraValid @45 :Bool; # Whether a speed camera ahead is currently detected
|
||||
cameraType @46 :CameraType; # Type of the upcoming speed camera
|
||||
cameraDistance @47 :Float32; # Distance to the upcoming camera (m)
|
||||
cameraSpeedLimit @48 :Float32; # Enforced speed limit at the camera (m/s)
|
||||
timeRemainingTypical @49 :Float32; # Remaining time under typical traffic conditions (s)
|
||||
trafficDelay @50 :Float32; # Remaining delay versus typical conditions (s)
|
||||
trafficDataAge @51 :Float32; # Age of the active traffic snapshot (s)
|
||||
trafficDataValid @52 :Bool; # Whether traffic metadata is fresh enough to trust
|
||||
trafficClosureCount @53 :UInt16; # Closures reported along the active route
|
||||
trafficRouteId @54 :Text; # Stable identity of the active traffic route
|
||||
trafficSource @55 :Text; # Routing provider that supplied traffic metadata
|
||||
trafficTimeRemaining @56 :Float32; # Remaining time from the latest traffic snapshot (s)
|
||||
trafficDataTimestamp @57 :Float64; # UTC acquisition time of the active traffic snapshot
|
||||
cameraAlertId @58 :Text;
|
||||
cameraChime @59 :Bool;
|
||||
cameraTrusted @60 :Bool;
|
||||
cameraSourceAge @61 :Float32;
|
||||
mapboxSpeedLimit @62 :Float32;
|
||||
mapboxSpeedLimitValid @63 :Bool;
|
||||
|
||||
enum CameraType {
|
||||
none @0;
|
||||
fixedSpeed @1; # Fixed speed camera
|
||||
mobileSpeed @2; # Mobile/handheld speed camera
|
||||
sectionStart @3; # Average-speed (section) zone start
|
||||
sectionEnd @4; # Average-speed (section) zone end
|
||||
averageZone @5; # Within an average-speed zone
|
||||
redLight @6; # Red-light camera
|
||||
bump @7; # Speed bump
|
||||
alpr @8; # ALPR / Flock surveillance camera (DeFlock/OSM surveillance:type=ALPR)
|
||||
police @9;
|
||||
}
|
||||
|
||||
enum ManeuverType {
|
||||
none @0;
|
||||
turn @1; # Regular turn at intersection
|
||||
exit @2; # Highway exit
|
||||
merge @3; # Merge onto highway
|
||||
fork @4; # Road fork
|
||||
continueStraight @5; # Continue straight
|
||||
arrive @6; # Arrive at destination
|
||||
roundabout @7; # Enter/exit roundabout
|
||||
}
|
||||
|
||||
enum LongitudinalProvider {
|
||||
none @0;
|
||||
route @1;
|
||||
mapbox @2;
|
||||
vision @3;
|
||||
offlineOsm @4;
|
||||
camera @5;
|
||||
}
|
||||
|
||||
enum LongitudinalState {
|
||||
disabled @0;
|
||||
enabled @1;
|
||||
entering @2;
|
||||
active @3;
|
||||
leaving @4;
|
||||
overriding @5;
|
||||
}
|
||||
|
||||
enum ManeuverPhase {
|
||||
none @0;
|
||||
turnPrepare @1;
|
||||
turnActive @2;
|
||||
highwayPrepare @3;
|
||||
highwayCommit @4;
|
||||
}
|
||||
|
||||
enum Command {
|
||||
none @0;
|
||||
laneChange @1;
|
||||
}
|
||||
|
||||
struct LaneDebugInfo {
|
||||
modelLane @0 :Text; # "left", "middle", "right", "unknown"
|
||||
modelConfidence @1 :Float32; # 0.0-1.0
|
||||
gpsLane @2 :Text; # "left", "middle", "right", "unknown"
|
||||
gpsConfidence @3 :Float32; # 0.0-1.0
|
||||
lateralOffset @4 :Float32; # Meters from road centerline (negative=left, positive=right)
|
||||
gpsAccuracy @5 :Float32; # GPS position accuracy (meters)
|
||||
agreement @6 :Bool; # Do model and GPS agree?
|
||||
}
|
||||
}
|
||||
|
||||
struct IQRoadIncidentFeed @0xf2b94d5ed8504f81 {
|
||||
valid @0 :Bool;
|
||||
status @1 :Status;
|
||||
fetchedMonoTime @2 :UInt64;
|
||||
fetchedWallTime @3 :Float64;
|
||||
reports @4 :List(Report);
|
||||
|
||||
enum Status {
|
||||
idle @0;
|
||||
ok @1;
|
||||
missingKey @2;
|
||||
noLocation @3;
|
||||
networkError @4;
|
||||
invalidResponse @5;
|
||||
}
|
||||
|
||||
struct Report {
|
||||
id @0 :Text;
|
||||
latitude @1 :Float64;
|
||||
longitude @2 :Float64;
|
||||
publishedAt @3 :Float64;
|
||||
street @4 :Text;
|
||||
city @5 :Text;
|
||||
headingValid @6 :Bool;
|
||||
heading @7 :Float32;
|
||||
}
|
||||
}
|
||||
|
||||
struct IQNavRenderState @0xf6e4a54ca6c92276 {
|
||||
active @0 :Bool;
|
||||
currentLatitude @1 :Float64;
|
||||
currentLongitude @2 :Float64;
|
||||
bearingDeg @3 :Float32;
|
||||
routePolyline @4 :List(NavPoint);
|
||||
routePolylineSimplified @5 :List(NavPoint);
|
||||
nextManeuverLatitude @6 :Float64;
|
||||
nextManeuverLongitude @7 :Float64;
|
||||
nextManeuverType @8 :IQNavState.ManeuverType;
|
||||
nextManeuverDirection @9 :NavDirection;
|
||||
nextManeuverDistance @10 :Float32;
|
||||
destinationLatitude @11 :Float64;
|
||||
destinationLongitude @12 :Float64;
|
||||
zoomHint @13 :Float32;
|
||||
|
||||
struct NavPoint {
|
||||
latitude @0 :Float64;
|
||||
longitude @1 :Float64;
|
||||
}
|
||||
}
|
||||
|
||||
struct IQPerfTrace @0xa8e2e4a8c6f4d3b2 {
|
||||
process @0 :Text;
|
||||
eventClass @1 :Text;
|
||||
severity @2 :Severity;
|
||||
frameId @3 :UInt32;
|
||||
totalTimeUs @4 :UInt32;
|
||||
rkRemainingUs @5 :Int32;
|
||||
batchSize @6 :UInt16;
|
||||
droppedFrames @7 :UInt16;
|
||||
backlog @8 :UInt16;
|
||||
flags @9 :UInt32;
|
||||
samples @10 :List(Sample);
|
||||
missingServices @11 :List(Text);
|
||||
topProcesses @12 :List(Text);
|
||||
detail @13 :Text;
|
||||
|
||||
enum Severity {
|
||||
info @0;
|
||||
warning @1;
|
||||
error @2;
|
||||
critical @3;
|
||||
}
|
||||
|
||||
struct Sample {
|
||||
frameId @0 :UInt32;
|
||||
loopDtUs @1 :UInt32;
|
||||
updateUs @2 :UInt32;
|
||||
stateControlUs @3 :UInt32;
|
||||
publishUs @4 :UInt32;
|
||||
tailWorkUs @5 :UInt32;
|
||||
rkRemainingUs @6 :Int32;
|
||||
staleCarControlUs @7 :UInt32;
|
||||
staleCarControlFrames @8 :UInt16;
|
||||
sendcanGapUs @9 :UInt32;
|
||||
modelEvalUs @10 :UInt32;
|
||||
modelDroppedFrames @11 :UInt16;
|
||||
modelBacklog @12 :UInt16;
|
||||
textureDecodeUs @13 :UInt32;
|
||||
textureUploadUs @14 :UInt32;
|
||||
textureUnloadUs @15 :UInt32;
|
||||
texturePruneUs @16 :UInt32;
|
||||
textureConsumeUs @17 :UInt32;
|
||||
textureBatchSize @18 :UInt16;
|
||||
textureBytes @19 :UInt32;
|
||||
textureCacheBefore @20 :UInt16;
|
||||
textureCacheAfter @21 :UInt16;
|
||||
textureUnloaded @22 :UInt16;
|
||||
memoryUsagePercent @23 :UInt16;
|
||||
gpuUsagePercent @24 :UInt16;
|
||||
cpuUsagePercent @25 :UInt16;
|
||||
flags @26 :UInt32;
|
||||
}
|
||||
}
|
||||
|
||||
struct IQConstructionZone @0xb54d6e69da4ddc9f {
|
||||
state @0 :State;
|
||||
active @1 :Bool;
|
||||
orangeFraction @2 :Float32; # hot-orange fraction of ROI chroma samples this analysis
|
||||
secondsSinceHit @3 :Float32; # time since last frame that passed the hit threshold
|
||||
|
||||
enum State {
|
||||
inactive @0;
|
||||
pending @1; # hits seen, not yet enough persistence to enter
|
||||
active @2;
|
||||
}
|
||||
}
|
||||
|
||||
struct IQVehicleTracks @0xb877ef4b20a4ae22 {
|
||||
frameId @0 :UInt32;
|
||||
frameWidth @1 :UInt16;
|
||||
frameHeight @2 :UInt16;
|
||||
processingMs @3 :Float32;
|
||||
tracks @4 :List(Track);
|
||||
wide @5 :Bool;
|
||||
|
||||
struct Track {
|
||||
# box corners normalized [0,1] in the road-camera frame
|
||||
x1 @0 :Float32;
|
||||
y1 @1 :Float32;
|
||||
x2 @2 :Float32;
|
||||
y2 @3 :Float32;
|
||||
prob @4 :Float32;
|
||||
label @5 :Label;
|
||||
|
||||
enum Label {
|
||||
car @0;
|
||||
motorcycle @1;
|
||||
bus @2;
|
||||
truck @3;
|
||||
person @4;
|
||||
bicycle @5;
|
||||
stopSign @6;
|
||||
trafficLight @7;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
struct IQEnvironment @0xfd960244a79e2804 {
|
||||
frameId @0 :UInt32;
|
||||
offloaded @1 :Bool;
|
||||
modelValid @2 :Bool;
|
||||
objects @3 :List(Object);
|
||||
|
||||
struct Object {
|
||||
x @0 :Float32;
|
||||
y @1 :Float32;
|
||||
z @2 :Float32;
|
||||
width @3 :Float32;
|
||||
height @4 :Float32;
|
||||
length @5 :Float32;
|
||||
prob @6 :Float32;
|
||||
label @7 :Label;
|
||||
|
||||
enum Label {
|
||||
car @0;
|
||||
motorcycle @1;
|
||||
bus @2;
|
||||
truck @3;
|
||||
person @4;
|
||||
bicycle @5;
|
||||
stopSign @6;
|
||||
trafficLight @7;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
struct CustomReserved14 @0xa6e5a1ce8ca5258e {
|
||||
}
|
||||
|
||||
struct CustomReserved15 @0xdb8042111e62cc87 {
|
||||
}
|
||||
|
||||
struct CustomReserved16 @0xf59900ccf47b651a {
|
||||
}
|
||||
|
||||
# pfeiferj/mapd v2 output schema (struct ids must match the mapd binary exactly).
|
||||
struct MapdDownloadLocationDetails @0xff889853e7b0987f {
|
||||
location @0 :Text;
|
||||
totalFiles @1 :UInt32;
|
||||
downloadedFiles @2 :UInt32;
|
||||
}
|
||||
|
||||
struct MapdDownloadProgress @0xfaa35dcac85073a2 {
|
||||
active @0 :Bool;
|
||||
cancelled @1 :Bool;
|
||||
totalFiles @2 :UInt32;
|
||||
downloadedFiles @3 :UInt32;
|
||||
locations @4 :List(Text);
|
||||
locationDetails @5 :List(MapdDownloadLocationDetails);
|
||||
}
|
||||
|
||||
struct MapdPathPoint @0xd6f78acca1bc3939 {
|
||||
latitude @0 :Float64;
|
||||
longitude @1 :Float64;
|
||||
curvature @2 :Float32;
|
||||
targetVelocity @3 :Float32;
|
||||
}
|
||||
|
||||
enum MapdRoadContext {
|
||||
freeway @0;
|
||||
city @1;
|
||||
unknown @2;
|
||||
}
|
||||
|
||||
enum MapdWaySelectionType {
|
||||
current @0;
|
||||
predicted @1;
|
||||
possible @2;
|
||||
extended @3;
|
||||
fail @4;
|
||||
}
|
||||
|
||||
enum MapdInputType {
|
||||
download @0;
|
||||
setTargetLateralAccel @1;
|
||||
setSpeedLimitOffset @2;
|
||||
setSpeedLimitControl @3;
|
||||
setMapCurveSpeedControl @4;
|
||||
setVisionCurveSpeedControl @5;
|
||||
setLogLevel @6;
|
||||
setVisionCurveTargetLatA @7;
|
||||
setVisionCurveMinTargetV @8;
|
||||
reloadSettings @9;
|
||||
saveSettings @10;
|
||||
setEnableSpeed @11;
|
||||
setVisionCurveUseEnableSpeed @12;
|
||||
setMapCurveUseEnableSpeed @13;
|
||||
setSpeedLimitUseEnableSpeed @14;
|
||||
setHoldLastSeenSpeedLimit @15;
|
||||
setTargetSpeedJerk @16;
|
||||
setTargetSpeedAccel @17;
|
||||
setTargetSpeedTimeOffset @18;
|
||||
setDefaultLaneWidth @19;
|
||||
setMapCurveTargetLatA @20;
|
||||
loadDefaultSettings @21;
|
||||
loadRecommendedSettings @22;
|
||||
setSlowDownForNextSpeedLimit @23;
|
||||
setSpeedUpForNextSpeedLimit @24;
|
||||
setHoldSpeedLimitWhileChangingSetSpeed @25;
|
||||
loadPersistentSettings @26;
|
||||
cancelDownload @27;
|
||||
setLogJson @28;
|
||||
setLogSource @29;
|
||||
setExternalSpeedLimitControl @30;
|
||||
setExternalSpeedLimit @31;
|
||||
setSpeedLimitPriority @32;
|
||||
setSpeedLimitChangeRequiresAccept @33;
|
||||
acceptSpeedLimit @34;
|
||||
setPressGasToAcceptSpeedLimit @35;
|
||||
setAdjustSetSpeedToAcceptSpeedLimit @36;
|
||||
setAcceptSpeedLimitTimeout @37;
|
||||
setPressGasToOverrideSpeedLimit @38;
|
||||
}
|
||||
|
||||
struct MapdExtendedOut @0x8d12e6a08c60a1a1 {
|
||||
downloadProgress @0 :MapdDownloadProgress;
|
||||
settings @1 :Text;
|
||||
path @2 :List(MapdPathPoint);
|
||||
}
|
||||
|
||||
struct MapdIn @0xb9ceb3ea89cecc23 {
|
||||
type @0 :MapdInputType;
|
||||
float @1 :Float32;
|
||||
str @2 :Text;
|
||||
bool @3 :Bool;
|
||||
}
|
||||
|
||||
struct MapdOut @0xd615f9fe1608a3c0 {
|
||||
wayName @0 :Text;
|
||||
wayRef @1 :Text;
|
||||
roadName @2 :Text;
|
||||
speedLimit @3 :Float32;
|
||||
nextSpeedLimit @4 :Float32;
|
||||
nextSpeedLimitDistance @5 :Float32;
|
||||
hazard @6 :Text;
|
||||
nextHazard @7 :Text;
|
||||
nextHazardDistance @8 :Float32;
|
||||
advisorySpeed @9 :Float32;
|
||||
nextAdvisorySpeed @10 :Float32;
|
||||
nextAdvisorySpeedDistance @11 :Float32;
|
||||
oneWay @12 :Bool;
|
||||
lanes @13 :UInt8;
|
||||
tileLoaded @14 :Bool;
|
||||
speedLimitSuggestedSpeed @15 :Float32;
|
||||
suggestedSpeed @16 :Float32;
|
||||
estimatedRoadWidth @17 :Float32;
|
||||
roadContext @18 :MapdRoadContext;
|
||||
distanceFromWayCenter @19 :Float32;
|
||||
visionCurveSpeed @20 :Float32;
|
||||
mapCurveSpeed @21 :Float32;
|
||||
waySelectionType @22 :MapdWaySelectionType;
|
||||
speedLimitAccepted @23 :Bool;
|
||||
}
|
||||
26
iqpilot/cereal/include/c++.capnp
Normal file
26
iqpilot/cereal/include/c++.capnp
Normal file
@@ -0,0 +1,26 @@
|
||||
# Copyright (c) 2013-2014 Sandstorm Development Group, Inc. and contributors
|
||||
# Licensed under the MIT License:
|
||||
#
|
||||
# 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.
|
||||
|
||||
@0xbdf87d7bb8304e81;
|
||||
$namespace("capnp::annotations");
|
||||
|
||||
annotation namespace(file): Text;
|
||||
annotation name(field, enumerant, struct, enum, interface, method, param, group, union): Text;
|
||||
573
iqpilot/cereal/legacy.capnp
Normal file
573
iqpilot/cereal/legacy.capnp
Normal file
@@ -0,0 +1,573 @@
|
||||
using Cxx = import "/include/c++.capnp";
|
||||
$Cxx.namespace("cereal");
|
||||
|
||||
@0x80ef1ec4889c2a63;
|
||||
|
||||
# legacy.capnp: a home for deprecated structs
|
||||
|
||||
struct LogRotate @0x9811e1f38f62f2d1 {
|
||||
segmentNum @0 :Int32;
|
||||
path @1 :Text;
|
||||
}
|
||||
|
||||
struct LiveUI @0xc08240f996aefced {
|
||||
rearViewCam @0 :Bool;
|
||||
alertText1 @1 :Text;
|
||||
alertText2 @2 :Text;
|
||||
awarenessStatus @3 :Float32;
|
||||
}
|
||||
|
||||
struct UiLayoutState @0x88dcce08ad29dda0 {
|
||||
activeApp @0 :App;
|
||||
sidebarCollapsed @1 :Bool;
|
||||
mapEnabled @2 :Bool;
|
||||
mockEngaged @3 :Bool;
|
||||
|
||||
enum App @0x9917470acf94d285 {
|
||||
home @0;
|
||||
music @1;
|
||||
nav @2;
|
||||
settings @3;
|
||||
none @4;
|
||||
}
|
||||
}
|
||||
|
||||
struct OrbslamCorrection @0x8afd33dc9b35e1aa {
|
||||
correctionMonoTime @0 :UInt64;
|
||||
prePositionECEF @1 :List(Float64);
|
||||
postPositionECEF @2 :List(Float64);
|
||||
prePoseQuatECEF @3 :List(Float32);
|
||||
postPoseQuatECEF @4 :List(Float32);
|
||||
numInliers @5 :UInt32;
|
||||
}
|
||||
|
||||
struct EthernetPacket @0xa99a9d5b33cf5859 {
|
||||
pkt @0 :Data;
|
||||
ts @1 :Float32;
|
||||
}
|
||||
|
||||
struct CellInfo @0xcff7566681c277ce {
|
||||
timestamp @0 :UInt64;
|
||||
repr @1 :Text; # android toString() for now
|
||||
}
|
||||
|
||||
struct WifiScan @0xd4df5a192382ba0b {
|
||||
bssid @0 :Text;
|
||||
ssid @1 :Text;
|
||||
capabilities @2 :Text;
|
||||
frequency @3 :Int32;
|
||||
level @4 :Int32;
|
||||
timestamp @5 :Int64;
|
||||
|
||||
centerFreq0 @6 :Int32;
|
||||
centerFreq1 @7 :Int32;
|
||||
channelWidth @8 :ChannelWidth;
|
||||
operatorFriendlyName @9 :Text;
|
||||
venueName @10 :Text;
|
||||
is80211mcResponder @11 :Bool;
|
||||
passpoint @12 :Bool;
|
||||
|
||||
distanceCm @13 :Int32;
|
||||
distanceSdCm @14 :Int32;
|
||||
|
||||
enum ChannelWidth @0xcb6a279f015f6b51 {
|
||||
w20Mhz @0;
|
||||
w40Mhz @1;
|
||||
w80Mhz @2;
|
||||
w160Mhz @3;
|
||||
w80Plus80Mhz @4;
|
||||
}
|
||||
}
|
||||
|
||||
struct LiveEventData @0x94b7baa90c5c321e {
|
||||
name @0 :Text;
|
||||
value @1 :Int32;
|
||||
}
|
||||
|
||||
struct ModelData @0xb8aad62cffef28a9 {
|
||||
frameId @0 :UInt32;
|
||||
frameAge @12 :UInt32;
|
||||
frameDropPerc @13 :Float32;
|
||||
timestampEof @9 :UInt64;
|
||||
modelExecutionTime @14 :Float32;
|
||||
gpuExecutionTime @16 :Float32;
|
||||
rawPred @15 :Data;
|
||||
|
||||
path @1 :PathData;
|
||||
leftLane @2 :PathData;
|
||||
rightLane @3 :PathData;
|
||||
lead @4 :LeadData;
|
||||
freePath @6 :List(Float32);
|
||||
|
||||
settings @5 :ModelSettings;
|
||||
leadFuture @7 :LeadData;
|
||||
speed @8 :List(Float32);
|
||||
meta @10 :MetaData;
|
||||
longitudinal @11 :LongitudinalData;
|
||||
|
||||
struct PathData @0x8817eeea389e9f08 {
|
||||
points @0 :List(Float32);
|
||||
prob @1 :Float32;
|
||||
std @2 :Float32;
|
||||
stds @3 :List(Float32);
|
||||
poly @4 :List(Float32);
|
||||
validLen @5 :Float32;
|
||||
}
|
||||
|
||||
struct LeadData @0xd1c9bef96d26fa91 {
|
||||
dist @0 :Float32;
|
||||
prob @1 :Float32;
|
||||
std @2 :Float32;
|
||||
relVel @3 :Float32;
|
||||
relVelStd @4 :Float32;
|
||||
relY @5 :Float32;
|
||||
relYStd @6 :Float32;
|
||||
relA @7 :Float32;
|
||||
relAStd @8 :Float32;
|
||||
}
|
||||
|
||||
struct ModelSettings @0xa26e3710efd3e914 {
|
||||
bigBoxX @0 :UInt16;
|
||||
bigBoxY @1 :UInt16;
|
||||
bigBoxWidth @2 :UInt16;
|
||||
bigBoxHeight @3 :UInt16;
|
||||
boxProjection @4 :List(Float32);
|
||||
yuvCorrection @5 :List(Float32);
|
||||
inputTransform @6 :List(Float32);
|
||||
}
|
||||
|
||||
struct MetaData @0x9744f25fb60f2bf8 {
|
||||
engagedProb @0 :Float32;
|
||||
desirePrediction @1 :List(Float32);
|
||||
brakeDisengageProb @2 :Float32;
|
||||
gasDisengageProb @3 :Float32;
|
||||
steerOverrideProb @4 :Float32;
|
||||
desireState @5 :List(Float32);
|
||||
}
|
||||
|
||||
struct LongitudinalData @0xf98f999c6a071122 {
|
||||
distances @2 :List(Float32);
|
||||
speeds @0 :List(Float32);
|
||||
accelerations @1 :List(Float32);
|
||||
}
|
||||
}
|
||||
|
||||
struct ECEFPoint @0xc25bbbd524983447 {
|
||||
x @0 :Float64;
|
||||
y @1 :Float64;
|
||||
z @2 :Float64;
|
||||
}
|
||||
|
||||
struct ECEFPointDEPRECATED @0xe10e21168db0c7f7 {
|
||||
x @0 :Float32;
|
||||
y @1 :Float32;
|
||||
z @2 :Float32;
|
||||
}
|
||||
|
||||
struct GPSPlannerPoints @0xab54c59699f8f9f3 {
|
||||
curPosDEPRECATED @0 :ECEFPointDEPRECATED;
|
||||
pointsDEPRECATED @1 :List(ECEFPointDEPRECATED);
|
||||
curPos @6 :ECEFPoint;
|
||||
points @7 :List(ECEFPoint);
|
||||
valid @2 :Bool;
|
||||
trackName @3 :Text;
|
||||
speedLimit @4 :Float32;
|
||||
accelTarget @5 :Float32;
|
||||
}
|
||||
|
||||
struct GPSPlannerPlan @0xf5ad1d90cdc1dd6b {
|
||||
valid @0 :Bool;
|
||||
poly @1 :List(Float32);
|
||||
trackName @2 :Text;
|
||||
speed @3 :Float32;
|
||||
acceleration @4 :Float32;
|
||||
pointsDEPRECATED @5 :List(ECEFPointDEPRECATED);
|
||||
points @6 :List(ECEFPoint);
|
||||
xLookahead @7 :Float32;
|
||||
}
|
||||
|
||||
struct UiNavigationEvent @0x90c8426c3eaddd3b {
|
||||
type @0: Type;
|
||||
status @1: Status;
|
||||
distanceTo @2: Float32;
|
||||
endRoadPointDEPRECATED @3: ECEFPointDEPRECATED;
|
||||
endRoadPoint @4: ECEFPoint;
|
||||
|
||||
enum Type @0xe8db07dcf8fcea05 {
|
||||
none @0;
|
||||
laneChangeLeft @1;
|
||||
laneChangeRight @2;
|
||||
mergeLeft @3;
|
||||
mergeRight @4;
|
||||
turnLeft @5;
|
||||
turnRight @6;
|
||||
}
|
||||
|
||||
enum Status @0xb9aa88c75ef99a1f {
|
||||
none @0;
|
||||
passive @1;
|
||||
approaching @2;
|
||||
active @3;
|
||||
}
|
||||
}
|
||||
|
||||
struct LiveLocationData @0xb99b2bc7a57e8128 {
|
||||
status @0 :UInt8;
|
||||
|
||||
# 3D fix
|
||||
lat @1 :Float64;
|
||||
lon @2 :Float64;
|
||||
alt @3 :Float32; # m
|
||||
|
||||
# speed
|
||||
speed @4 :Float32; # m/s
|
||||
|
||||
# NED velocity components
|
||||
vNED @5 :List(Float32);
|
||||
|
||||
# roll, pitch, heading (x,y,z)
|
||||
roll @6 :Float32; # WRT to center of earth?
|
||||
pitch @7 :Float32; # WRT to center of earth?
|
||||
heading @8 :Float32; # WRT to north?
|
||||
|
||||
# what are these?
|
||||
wanderAngle @9 :Float32;
|
||||
trackAngle @10 :Float32;
|
||||
|
||||
# car frame -- https://upload.wikimedia.org/wikipedia/commons/f/f5/RPY_angles_of_cars.png
|
||||
|
||||
# gyro, in car frame, deg/s
|
||||
gyro @11 :List(Float32);
|
||||
|
||||
# accel, in car frame, m/s^2
|
||||
accel @12 :List(Float32);
|
||||
|
||||
accuracy @13 :Accuracy;
|
||||
|
||||
source @14 :SensorSource;
|
||||
# if we are fixing a location in the past
|
||||
fixMonoTime @15 :UInt64;
|
||||
|
||||
gpsWeek @16 :Int32;
|
||||
timeOfWeek @17 :Float64;
|
||||
|
||||
positionECEF @18 :List(Float64);
|
||||
poseQuatECEF @19 :List(Float32);
|
||||
pitchCalibration @20 :Float32;
|
||||
yawCalibration @21 :Float32;
|
||||
imuFrame @22 :List(Float32);
|
||||
|
||||
struct Accuracy @0x943dc4625473b03f {
|
||||
pNEDError @0 :List(Float32);
|
||||
vNEDError @1 :List(Float32);
|
||||
rollError @2 :Float32;
|
||||
pitchError @3 :Float32;
|
||||
headingError @4 :Float32;
|
||||
ellipsoidSemiMajorError @5 :Float32;
|
||||
ellipsoidSemiMinorError @6 :Float32;
|
||||
ellipsoidOrientationError @7 :Float32;
|
||||
}
|
||||
|
||||
enum SensorSource @0xc871d3cc252af657 {
|
||||
applanix @0;
|
||||
kalman @1;
|
||||
orbslam @2;
|
||||
timing @3;
|
||||
dummy @4;
|
||||
}
|
||||
}
|
||||
|
||||
struct OrbOdometry @0xd7700859ed1f5b76 {
|
||||
# timing first
|
||||
startMonoTime @0 :UInt64;
|
||||
endMonoTime @1 :UInt64;
|
||||
|
||||
# fundamental matrix and error
|
||||
f @2: List(Float64);
|
||||
err @3: Float64;
|
||||
|
||||
# number of inlier points
|
||||
inliers @4: Int32;
|
||||
|
||||
# for debug only
|
||||
# indexed by endMonoTime features
|
||||
# value is startMonoTime feature match
|
||||
# -1 if no match
|
||||
matches @5: List(Int16);
|
||||
}
|
||||
|
||||
struct OrbFeatures @0xcd60164a8a0159ef {
|
||||
timestampEof @0 :UInt64;
|
||||
# transposed arrays of normalized image coordinates
|
||||
# len(xs) == len(ys) == len(descriptors) * 32
|
||||
xs @1 :List(Float32);
|
||||
ys @2 :List(Float32);
|
||||
descriptors @3 :Data;
|
||||
octaves @4 :List(Int8);
|
||||
|
||||
# match index to last OrbFeatures
|
||||
# -1 if no match
|
||||
timestampLastEof @5 :UInt64;
|
||||
matches @6: List(Int16);
|
||||
}
|
||||
|
||||
struct OrbFeaturesSummary @0xd500d30c5803fa4f {
|
||||
timestampEof @0 :UInt64;
|
||||
timestampLastEof @1 :UInt64;
|
||||
|
||||
featureCount @2 :UInt16;
|
||||
matchCount @3 :UInt16;
|
||||
computeNs @4 :UInt64;
|
||||
}
|
||||
|
||||
struct OrbKeyFrame @0xc8233c0345e27e24 {
|
||||
# this is a globally unique id for the KeyFrame
|
||||
id @0: UInt64;
|
||||
|
||||
# this is the location of the KeyFrame
|
||||
pos @1: ECEFPoint;
|
||||
|
||||
# these are the features in the world
|
||||
# len(dpos) == len(descriptors) * 32
|
||||
dpos @2 :List(ECEFPoint);
|
||||
descriptors @3 :Data;
|
||||
}
|
||||
|
||||
struct KalmanOdometry @0x92e21bb7ea38793a {
|
||||
trans @0 :List(Float32); # m/s in device frame
|
||||
rot @1 :List(Float32); # rad/s in device frame
|
||||
transStd @2 :List(Float32); # std m/s in device frame
|
||||
rotStd @3 :List(Float32); # std rad/s in device frame
|
||||
}
|
||||
|
||||
struct OrbObservation @0x9b326d4e436afec7 {
|
||||
observationMonoTime @0 :UInt64;
|
||||
normalizedCoordinates @1 :List(Float32);
|
||||
locationECEF @2 :List(Float64);
|
||||
matchDistance @3: UInt32;
|
||||
}
|
||||
|
||||
struct CalibrationFeatures @0x8fdfadb254ea867a {
|
||||
frameId @0 :UInt32;
|
||||
|
||||
p0 @1 :List(Float32);
|
||||
p1 @2 :List(Float32);
|
||||
status @3 :List(Int8);
|
||||
}
|
||||
|
||||
struct NavStatus @0xbd8822120928120c {
|
||||
isNavigating @0 :Bool;
|
||||
currentAddress @1 :Address;
|
||||
|
||||
struct Address @0xce7cd672cacc7814 {
|
||||
title @0 :Text;
|
||||
lat @1 :Float64;
|
||||
lng @2 :Float64;
|
||||
house @3 :Text;
|
||||
address @4 :Text;
|
||||
street @5 :Text;
|
||||
city @6 :Text;
|
||||
state @7 :Text;
|
||||
country @8 :Text;
|
||||
}
|
||||
}
|
||||
|
||||
struct NavUpdate @0xdb98be6565516acb {
|
||||
isNavigating @0 :Bool;
|
||||
curSegment @1 :Int32;
|
||||
segments @2 :List(Segment);
|
||||
|
||||
struct LatLng @0x9eaef9187cadbb9b {
|
||||
lat @0 :Float64;
|
||||
lng @1 :Float64;
|
||||
}
|
||||
|
||||
struct Segment @0xa5b39b4fc4d7da3f {
|
||||
from @0 :LatLng;
|
||||
to @1 :LatLng;
|
||||
updateTime @2 :Int32;
|
||||
distance @3 :Int32;
|
||||
crossTime @4 :Int32;
|
||||
exitNo @5 :Int32;
|
||||
instruction @6 :Instruction;
|
||||
|
||||
parts @7 :List(LatLng);
|
||||
|
||||
enum Instruction @0xc5417a637451246f {
|
||||
turnLeft @0;
|
||||
turnRight @1;
|
||||
keepLeft @2;
|
||||
keepRight @3;
|
||||
straight @4;
|
||||
roundaboutExitNumber @5;
|
||||
roundaboutExit @6;
|
||||
roundaboutTurnLeft @7;
|
||||
unkn8 @8;
|
||||
roundaboutStraight @9;
|
||||
unkn10 @10;
|
||||
roundaboutTurnRight @11;
|
||||
unkn12 @12;
|
||||
roundaboutUturn @13;
|
||||
unkn14 @14;
|
||||
arrive @15;
|
||||
exitLeft @16;
|
||||
exitRight @17;
|
||||
unkn18 @18;
|
||||
uturn @19;
|
||||
# ...
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
struct TrafficEvent @0xacfa74a094e62626 {
|
||||
type @0 :Type;
|
||||
distance @1 :Float32;
|
||||
action @2 :Action;
|
||||
resuming @3 :Bool;
|
||||
|
||||
enum Type @0xd85d75253435bf4b {
|
||||
stopSign @0;
|
||||
lightRed @1;
|
||||
lightYellow @2;
|
||||
lightGreen @3;
|
||||
stopLight @4;
|
||||
}
|
||||
|
||||
enum Action @0xa6f6ce72165ccb49 {
|
||||
none @0;
|
||||
yield @1;
|
||||
stop @2;
|
||||
resumeReady @3;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
||||
struct AndroidGnss @0xdfdf30d03fc485bd {
|
||||
union {
|
||||
measurements @0 :Measurements;
|
||||
navigationMessage @1 :NavigationMessage;
|
||||
}
|
||||
|
||||
struct Measurements @0xa20710d4f428d6cd {
|
||||
clock @0 :Clock;
|
||||
measurements @1 :List(Measurement);
|
||||
|
||||
struct Clock @0xa0e27b453a38f450 {
|
||||
timeNanos @0 :Int64;
|
||||
hardwareClockDiscontinuityCount @1 :Int32;
|
||||
|
||||
hasTimeUncertaintyNanos @2 :Bool;
|
||||
timeUncertaintyNanos @3 :Float64;
|
||||
|
||||
hasLeapSecond @4 :Bool;
|
||||
leapSecond @5 :Int32;
|
||||
|
||||
hasFullBiasNanos @6 :Bool;
|
||||
fullBiasNanos @7 :Int64;
|
||||
|
||||
hasBiasNanos @8 :Bool;
|
||||
biasNanos @9 :Float64;
|
||||
|
||||
hasBiasUncertaintyNanos @10 :Bool;
|
||||
biasUncertaintyNanos @11 :Float64;
|
||||
|
||||
hasDriftNanosPerSecond @12 :Bool;
|
||||
driftNanosPerSecond @13 :Float64;
|
||||
|
||||
hasDriftUncertaintyNanosPerSecond @14 :Bool;
|
||||
driftUncertaintyNanosPerSecond @15 :Float64;
|
||||
}
|
||||
|
||||
struct Measurement @0xd949bf717d77614d {
|
||||
svId @0 :Int32;
|
||||
constellation @1 :Constellation;
|
||||
|
||||
timeOffsetNanos @2 :Float64;
|
||||
state @3 :Int32;
|
||||
receivedSvTimeNanos @4 :Int64;
|
||||
receivedSvTimeUncertaintyNanos @5 :Int64;
|
||||
cn0DbHz @6 :Float64;
|
||||
pseudorangeRateMetersPerSecond @7 :Float64;
|
||||
pseudorangeRateUncertaintyMetersPerSecond @8 :Float64;
|
||||
accumulatedDeltaRangeState @9 :Int32;
|
||||
accumulatedDeltaRangeMeters @10 :Float64;
|
||||
accumulatedDeltaRangeUncertaintyMeters @11 :Float64;
|
||||
|
||||
hasCarrierFrequencyHz @12 :Bool;
|
||||
carrierFrequencyHz @13 :Float32;
|
||||
hasCarrierCycles @14 :Bool;
|
||||
carrierCycles @15 :Int64;
|
||||
hasCarrierPhase @16 :Bool;
|
||||
carrierPhase @17 :Float64;
|
||||
hasCarrierPhaseUncertainty @18 :Bool;
|
||||
carrierPhaseUncertainty @19 :Float64;
|
||||
hasSnrInDb @20 :Bool;
|
||||
snrInDb @21 :Float64;
|
||||
|
||||
multipathIndicator @22 :MultipathIndicator;
|
||||
|
||||
enum Constellation @0x9ef1f3ff0deb5ffb {
|
||||
unknown @0;
|
||||
gps @1;
|
||||
sbas @2;
|
||||
glonass @3;
|
||||
qzss @4;
|
||||
beidou @5;
|
||||
galileo @6;
|
||||
}
|
||||
|
||||
enum State @0xcbb9490adce12d72 {
|
||||
unknown @0;
|
||||
codeLock @1;
|
||||
bitSync @2;
|
||||
subframeSync @3;
|
||||
towDecoded @4;
|
||||
msecAmbiguous @5;
|
||||
symbolSync @6;
|
||||
gloStringSync @7;
|
||||
gloTodDecoded @8;
|
||||
bdsD2BitSync @9;
|
||||
bdsD2SubframeSync @10;
|
||||
galE1bcCodeLock @11;
|
||||
galE1c2ndCodeLock @12;
|
||||
galE1bPageSync @13;
|
||||
sbasSync @14;
|
||||
}
|
||||
|
||||
enum MultipathIndicator @0xc04e7b6231d4caa8 {
|
||||
unknown @0;
|
||||
detected @1;
|
||||
notDetected @2;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
struct NavigationMessage @0xe2517b083095fd4e {
|
||||
type @0 :Int32;
|
||||
svId @1 :Int32;
|
||||
messageId @2 :Int32;
|
||||
submessageId @3 :Int32;
|
||||
data @4 :Data;
|
||||
status @5 :Status;
|
||||
|
||||
enum Status @0xec1ff7996b35366f {
|
||||
unknown @0;
|
||||
parityPassed @1;
|
||||
parityRebuilt @2;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
struct LidarPts @0xe3d6685d4e9d8f7a {
|
||||
r @0 :List(UInt16); # uint16 m*500.0
|
||||
theta @1 :List(UInt16); # uint16 deg*100.0
|
||||
reflect @2 :List(UInt8); # uint8 0-255
|
||||
|
||||
# For storing out of file.
|
||||
idx @3 :UInt64;
|
||||
|
||||
# For storing in file
|
||||
pkt @4 :Data;
|
||||
}
|
||||
|
||||
2795
iqpilot/cereal/log.capnp
Normal file
2795
iqpilot/cereal/log.capnp
Normal file
File diff suppressed because it is too large
Load Diff
269
iqpilot/cereal/messaging/__init__.py
Normal file
269
iqpilot/cereal/messaging/__init__.py
Normal file
@@ -0,0 +1,269 @@
|
||||
# must be built with scons
|
||||
from msgq import fake_event_handle, drain_sock_raw, MultiplePublishersError, IpcError, \
|
||||
Context, Poller, SubSocket, PubSocket, SocketEventHandle, toggle_fake_events, \
|
||||
set_fake_prefix, get_fake_prefix, delete_fake_prefix, wait_for_one_event
|
||||
import msgq
|
||||
import os
|
||||
import capnp
|
||||
import time
|
||||
|
||||
from typing import Optional, List, Union, Dict
|
||||
|
||||
from iqpilot.cereal import log
|
||||
from iqpilot.cereal.services import SERVICE_LIST
|
||||
from iqpilot.common.utils import MovingAverage
|
||||
|
||||
NO_TRAVERSAL_LIMIT = 2**64-1
|
||||
|
||||
|
||||
def pub_sock(endpoint: str) -> PubSocket:
|
||||
service = SERVICE_LIST.get(endpoint)
|
||||
segment_size = service.queue_size if service else 0
|
||||
return msgq.pub_sock(endpoint, segment_size)
|
||||
|
||||
|
||||
def sub_sock(endpoint: str, poller: Optional[Poller] = None, addr: str = "127.0.0.1",
|
||||
conflate: bool = False, timeout: Optional[int] = None) -> SubSocket:
|
||||
service = SERVICE_LIST.get(endpoint)
|
||||
segment_size = service.queue_size if service else 0
|
||||
return msgq.sub_sock(endpoint, poller=poller, addr=addr, conflate=conflate,
|
||||
timeout=timeout, segment_size=segment_size)
|
||||
|
||||
|
||||
def reset_context():
|
||||
msgq.context = Context()
|
||||
|
||||
|
||||
def log_from_bytes(dat: bytes, struct: capnp.lib.capnp._StructModule = log.Event) -> capnp.lib.capnp._DynamicStructReader:
|
||||
with struct.from_bytes(dat, traversal_limit_in_words=NO_TRAVERSAL_LIMIT) as msg:
|
||||
return msg
|
||||
|
||||
|
||||
def new_message(service: Optional[str], size: Optional[int] = None, **kwargs) -> capnp.lib.capnp._DynamicStructBuilder:
|
||||
args = {
|
||||
'valid': False,
|
||||
'logMonoTime': int(time.monotonic() * 1e9),
|
||||
**kwargs
|
||||
}
|
||||
dat = log.Event.new_message(**args)
|
||||
if service is not None:
|
||||
if size is None:
|
||||
dat.init(service)
|
||||
else:
|
||||
dat.init(service, size)
|
||||
return dat
|
||||
|
||||
|
||||
def drain_sock(sock: SubSocket, wait_for_one: bool = False) -> List[capnp.lib.capnp._DynamicStructReader]:
|
||||
"""Receive all message currently available on the queue"""
|
||||
msgs = drain_sock_raw(sock, wait_for_one=wait_for_one)
|
||||
return [log_from_bytes(m) for m in msgs]
|
||||
|
||||
|
||||
# TODO: print when we drop packets?
|
||||
def recv_sock(sock: SubSocket, wait: bool = False) -> Optional[capnp.lib.capnp._DynamicStructReader]:
|
||||
"""Same as drain sock, but only returns latest message. Consider using conflate instead."""
|
||||
dat = None
|
||||
|
||||
while 1:
|
||||
if wait and dat is None:
|
||||
recv = sock.receive()
|
||||
else:
|
||||
recv = sock.receive(non_blocking=True)
|
||||
|
||||
if recv is None: # Timeout hit
|
||||
break
|
||||
|
||||
dat = recv
|
||||
|
||||
if dat is not None:
|
||||
dat = log_from_bytes(dat)
|
||||
|
||||
return dat
|
||||
|
||||
|
||||
def recv_one(sock: SubSocket) -> Optional[capnp.lib.capnp._DynamicStructReader]:
|
||||
dat = sock.receive()
|
||||
if dat is not None:
|
||||
dat = log_from_bytes(dat)
|
||||
return dat
|
||||
|
||||
|
||||
def recv_one_or_none(sock: SubSocket) -> Optional[capnp.lib.capnp._DynamicStructReader]:
|
||||
dat = sock.receive(non_blocking=True)
|
||||
if dat is not None:
|
||||
dat = log_from_bytes(dat)
|
||||
return dat
|
||||
|
||||
|
||||
def recv_one_retry(sock: SubSocket) -> capnp.lib.capnp._DynamicStructReader:
|
||||
"""Keep receiving until we get a message"""
|
||||
while True:
|
||||
dat = sock.receive()
|
||||
if dat is not None:
|
||||
return log_from_bytes(dat)
|
||||
|
||||
|
||||
class FrequencyTracker:
|
||||
def __init__(self, service_freq: float, update_freq: float, is_poll: bool):
|
||||
freq = max(min(service_freq, update_freq), 1.)
|
||||
if is_poll:
|
||||
min_freq = max_freq = freq
|
||||
else:
|
||||
max_freq = min(freq, update_freq)
|
||||
if service_freq >= 2 * update_freq:
|
||||
min_freq = update_freq
|
||||
elif update_freq >= 2* service_freq:
|
||||
min_freq = freq
|
||||
else:
|
||||
min_freq = min(freq, freq / 2.)
|
||||
|
||||
self.min_freq = min_freq * 0.8
|
||||
self.max_freq = max_freq * 1.2
|
||||
self.avg_dt = MovingAverage(int(10 * freq))
|
||||
self.recent_avg_dt = MovingAverage(int(freq))
|
||||
self.prev_time = 0.0
|
||||
|
||||
def record_recv_time(self, cur_time: float) -> None:
|
||||
# TODO: Handle case where cur_time is less than prev_time
|
||||
if self.prev_time > 1e-5:
|
||||
dt = cur_time - self.prev_time
|
||||
|
||||
self.avg_dt.add_value(dt)
|
||||
self.recent_avg_dt.add_value(dt)
|
||||
|
||||
self.prev_time = cur_time
|
||||
|
||||
@property
|
||||
def valid(self) -> bool:
|
||||
if self.avg_dt.count == 0:
|
||||
return False
|
||||
|
||||
avg_freq = 1.0 / self.avg_dt.get_average()
|
||||
if self.min_freq <= avg_freq <= self.max_freq:
|
||||
return True
|
||||
|
||||
avg_freq_recent = 1.0 / self.recent_avg_dt.get_average()
|
||||
return self.min_freq <= avg_freq_recent <= self.max_freq
|
||||
|
||||
|
||||
class SubMaster:
|
||||
def __init__(self, services: List[str], poll: Optional[str] = None,
|
||||
ignore_alive: Optional[List[str]] = None, ignore_avg_freq: Optional[List[str]] = None,
|
||||
ignore_valid: Optional[List[str]] = None, addr: str = "127.0.0.1", frequency: Optional[float] = None):
|
||||
self.frame = -1
|
||||
self.services = services
|
||||
self.seen = {s: False for s in services}
|
||||
self.updated = {s: False for s in services}
|
||||
self.recv_time = {s: 0. for s in services}
|
||||
self.recv_frame = {s: 0 for s in services}
|
||||
self.sock = {}
|
||||
self.data = {}
|
||||
self.logMonoTime = {s: 0 for s in services}
|
||||
|
||||
# zero-frequency / on-demand services are always alive and presumed valid; all others must pass checks
|
||||
on_demand = {s: SERVICE_LIST[s].frequency <= 1e-5 for s in services}
|
||||
self.static_freq_services = set(s for s in services if not on_demand[s])
|
||||
self.alive = {s: on_demand[s] for s in services}
|
||||
self.freq_ok = {s: on_demand[s] for s in services}
|
||||
self.valid = {s: on_demand[s] for s in services}
|
||||
|
||||
self.freq_tracker: Dict[str, FrequencyTracker] = {}
|
||||
self.poller = Poller()
|
||||
polled_services = set([poll, ] if poll is not None else services)
|
||||
self.non_polled_services = set(services) - polled_services
|
||||
|
||||
self.ignore_average_freq = [] if ignore_avg_freq is None else ignore_avg_freq
|
||||
self.ignore_alive = [] if ignore_alive is None else ignore_alive
|
||||
self.ignore_valid = [] if ignore_valid is None else ignore_valid
|
||||
|
||||
self.simulation = bool(int(os.getenv("SIMULATION", "0")))
|
||||
|
||||
# if freq and poll aren't specified, assume the max to be conservative
|
||||
assert frequency is None or poll is None, "Do not specify 'frequency' - frequency of the polled service will be used."
|
||||
self.update_freq = frequency or max([SERVICE_LIST[s].frequency for s in polled_services])
|
||||
|
||||
for s in services:
|
||||
p = self.poller if s not in self.non_polled_services else None
|
||||
self.sock[s] = sub_sock(s, poller=p, addr=addr, conflate=True)
|
||||
|
||||
try:
|
||||
data = new_message(s)
|
||||
except capnp.lib.capnp.KjException:
|
||||
data = new_message(s, 0) # lists
|
||||
|
||||
self.data[s] = getattr(data.as_reader(), s)
|
||||
self.freq_tracker[s] = FrequencyTracker(SERVICE_LIST[s].frequency, self.update_freq, s == poll)
|
||||
|
||||
def __getitem__(self, s: str) -> capnp.lib.capnp._DynamicStructReader:
|
||||
return self.data[s]
|
||||
|
||||
def _check_avg_freq(self, s: str) -> bool:
|
||||
return SERVICE_LIST[s].frequency > 0.99 and (s not in self.ignore_average_freq) and (s not in self.ignore_alive)
|
||||
|
||||
def update(self, timeout: int = 100) -> None:
|
||||
msgs = []
|
||||
for sock in self.poller.poll(timeout):
|
||||
msgs.append(recv_one_or_none(sock))
|
||||
|
||||
# non-blocking receive for non-polled sockets
|
||||
for s in self.non_polled_services:
|
||||
msgs.append(recv_one_or_none(self.sock[s]))
|
||||
self.update_msgs(time.monotonic(), msgs)
|
||||
|
||||
def update_msgs(self, cur_time: float, msgs: List[capnp.lib.capnp._DynamicStructReader]) -> None:
|
||||
self.frame += 1
|
||||
self.updated = dict.fromkeys(self.services, False)
|
||||
for msg in msgs:
|
||||
if msg is None:
|
||||
continue
|
||||
|
||||
s = msg.which()
|
||||
self.seen[s] = True
|
||||
self.updated[s] = True
|
||||
|
||||
self.freq_tracker[s].record_recv_time(cur_time)
|
||||
self.recv_time[s] = cur_time
|
||||
self.recv_frame[s] = self.frame
|
||||
self.data[s] = getattr(msg, s)
|
||||
self.logMonoTime[s] = msg.logMonoTime
|
||||
self.valid[s] = msg.valid
|
||||
|
||||
for s in self.static_freq_services:
|
||||
# alive if delay is within 10x the expected frequency; checks relaxed in simulator
|
||||
self.alive[s] = (cur_time - self.recv_time[s]) < (10. / SERVICE_LIST[s].frequency) or (self.seen[s] and self.simulation)
|
||||
self.freq_ok[s] = self.freq_tracker[s].valid or self.simulation
|
||||
|
||||
def all_alive(self, service_list: Optional[List[str]] = None) -> bool:
|
||||
return all(self.alive[s] for s in (service_list or self.services) if s not in self.ignore_alive)
|
||||
|
||||
def all_freq_ok(self, service_list: Optional[List[str]] = None) -> bool:
|
||||
return all(self.freq_ok[s] for s in (service_list or self.services) if self._check_avg_freq(s))
|
||||
|
||||
def all_valid(self, service_list: Optional[List[str]] = None) -> bool:
|
||||
return all(self.valid[s] for s in (service_list or self.services) if s not in self.ignore_valid)
|
||||
|
||||
def all_checks(self, service_list: Optional[List[str]] = None) -> bool:
|
||||
return self.all_alive(service_list) and self.all_freq_ok(service_list) and self.all_valid(service_list)
|
||||
|
||||
|
||||
class PubMaster:
|
||||
def __init__(self, services: List[str]):
|
||||
self.sock = {}
|
||||
for s in services:
|
||||
self.sock[s] = pub_sock(s)
|
||||
|
||||
def send(self, s: str, dat: Union[bytes, capnp.lib.capnp._DynamicStructBuilder]) -> None:
|
||||
if not isinstance(dat, bytes):
|
||||
dat = dat.to_bytes()
|
||||
self.sock[s].send(dat)
|
||||
|
||||
def wait_for_readers_to_update(self, s: str, timeout: int, dt: float = 0.05) -> bool:
|
||||
for _ in range(int(timeout*(1./dt))):
|
||||
if self.sock[s].all_readers_updated():
|
||||
return True
|
||||
time.sleep(dt)
|
||||
return False
|
||||
|
||||
def all_readers_updated(self, s: str) -> bool:
|
||||
return self.sock[s].all_readers_updated() # type: ignore
|
||||
BIN
iqpilot/cereal/messaging/bridge
Executable file
BIN
iqpilot/cereal/messaging/bridge
Executable file
Binary file not shown.
0
iqpilot/cereal/messaging/tests/__init__.py
Normal file
0
iqpilot/cereal/messaging/tests/__init__.py
Normal file
186
iqpilot/cereal/messaging/tests/test_messaging.py
Normal file
186
iqpilot/cereal/messaging/tests/test_messaging.py
Normal file
@@ -0,0 +1,186 @@
|
||||
import os
|
||||
import capnp
|
||||
import numbers
|
||||
import random
|
||||
import threading
|
||||
import time
|
||||
from parameterized import parameterized
|
||||
|
||||
from iqpilot.cereal import log, car
|
||||
import iqpilot.cereal.messaging as messaging
|
||||
from iqpilot.cereal.services import SERVICE_LIST
|
||||
|
||||
events = [evt for evt in log.Event.schema.union_fields if evt in SERVICE_LIST.keys()]
|
||||
|
||||
def random_sock():
|
||||
return random.choice(events)
|
||||
|
||||
def random_socks(num_socks=10):
|
||||
return list({random_sock() for _ in range(num_socks)})
|
||||
|
||||
def random_bytes(length=1000):
|
||||
return bytes([random.randrange(0xFF) for _ in range(length)])
|
||||
|
||||
def zmq_sleep(t=1):
|
||||
if "ZMQ" in os.environ:
|
||||
time.sleep(t)
|
||||
|
||||
|
||||
# TODO: this should take any capnp struct and returrn a msg with random populated data
|
||||
def random_carstate():
|
||||
fields = ["vEgo", "aEgo", "brake", "steeringAngleDeg"]
|
||||
msg = messaging.new_message("carState")
|
||||
cs = msg.carState
|
||||
for f in fields:
|
||||
setattr(cs, f, random.random() * 10)
|
||||
return msg
|
||||
|
||||
# TODO: this should compare any capnp structs
|
||||
def assert_carstate(cs1, cs2):
|
||||
for f in car.CarState.schema.non_union_fields:
|
||||
# TODO: check all types
|
||||
val1, val2 = getattr(cs1, f), getattr(cs2, f)
|
||||
if isinstance(val1, numbers.Number):
|
||||
assert val1 == val2, f"{f}: sent '{val1}' vs recvd '{val2}'"
|
||||
|
||||
def delayed_send(delay, sock, dat):
|
||||
def send_func():
|
||||
sock.send(dat)
|
||||
threading.Timer(delay, send_func).start()
|
||||
|
||||
|
||||
class TestMessaging:
|
||||
def setUp(self):
|
||||
# TODO: ZMQ tests are too slow; all sleeps will need to be
|
||||
# replaced with logic to block on the necessary condition
|
||||
assert "ZMQ" not in os.environ
|
||||
|
||||
# ZMQ pub socket takes too long to die
|
||||
# sleep to prevent multiple publishers error between tests
|
||||
zmq_sleep()
|
||||
|
||||
@parameterized.expand(events)
|
||||
def test_new_message(self, evt):
|
||||
try:
|
||||
msg = messaging.new_message(evt)
|
||||
except capnp.lib.capnp.KjException:
|
||||
msg = messaging.new_message(evt, random.randrange(200))
|
||||
assert (time.monotonic() - msg.logMonoTime) < 0.1
|
||||
assert not msg.valid
|
||||
assert evt == msg.which()
|
||||
|
||||
@parameterized.expand(events)
|
||||
def test_pub_sock(self, evt):
|
||||
messaging.pub_sock(evt)
|
||||
|
||||
@parameterized.expand(events)
|
||||
def test_sub_sock(self, evt):
|
||||
messaging.sub_sock(evt)
|
||||
|
||||
@parameterized.expand([
|
||||
(messaging.drain_sock, capnp._DynamicStructReader),
|
||||
(messaging.drain_sock_raw, bytes),
|
||||
])
|
||||
def test_drain_sock(self, func, expected_type):
|
||||
sock = "carState"
|
||||
pub_sock = messaging.pub_sock(sock)
|
||||
sub_sock = messaging.sub_sock(sock, timeout=1000)
|
||||
zmq_sleep()
|
||||
|
||||
# no wait and no msgs in queue
|
||||
msgs = func(sub_sock)
|
||||
assert isinstance(msgs, list)
|
||||
assert len(msgs) == 0
|
||||
|
||||
# no wait but msgs are queued up
|
||||
num_msgs = random.randrange(3, 10)
|
||||
for _ in range(num_msgs):
|
||||
pub_sock.send(messaging.new_message(sock).to_bytes())
|
||||
time.sleep(0.1)
|
||||
msgs = func(sub_sock)
|
||||
assert isinstance(msgs, list)
|
||||
assert all(isinstance(msg, expected_type) for msg in msgs)
|
||||
assert len(msgs) == num_msgs
|
||||
|
||||
def test_recv_sock(self):
|
||||
sock = "carState"
|
||||
pub_sock = messaging.pub_sock(sock)
|
||||
sub_sock = messaging.sub_sock(sock, timeout=100)
|
||||
zmq_sleep()
|
||||
|
||||
# no wait and no msg in queue, socket should timeout
|
||||
recvd = messaging.recv_sock(sub_sock)
|
||||
assert recvd is None
|
||||
|
||||
# no wait and one msg in queue
|
||||
msg = random_carstate()
|
||||
pub_sock.send(msg.to_bytes())
|
||||
time.sleep(0.01)
|
||||
recvd = messaging.recv_sock(sub_sock)
|
||||
assert isinstance(recvd, capnp._DynamicStructReader)
|
||||
# https://github.com/python/mypy/issues/13038
|
||||
assert_carstate(msg.carState, recvd.carState)
|
||||
|
||||
def test_recv_one(self):
|
||||
sock = "carState"
|
||||
pub_sock = messaging.pub_sock(sock)
|
||||
sub_sock = messaging.sub_sock(sock, timeout=1000)
|
||||
zmq_sleep()
|
||||
|
||||
# no msg in queue, socket should timeout
|
||||
recvd = messaging.recv_one(sub_sock)
|
||||
assert recvd is None
|
||||
|
||||
# one msg in queue
|
||||
msg = random_carstate()
|
||||
pub_sock.send(msg.to_bytes())
|
||||
recvd = messaging.recv_one(sub_sock)
|
||||
assert isinstance(recvd, capnp._DynamicStructReader)
|
||||
assert_carstate(msg.carState, recvd.carState)
|
||||
|
||||
def test_recv_one_or_none(self):
|
||||
sock = "carState"
|
||||
pub_sock = messaging.pub_sock(sock)
|
||||
sub_sock = messaging.sub_sock(sock)
|
||||
zmq_sleep()
|
||||
|
||||
# no msg in queue, socket shouldn't block
|
||||
recvd = messaging.recv_one_or_none(sub_sock)
|
||||
assert recvd is None
|
||||
|
||||
# one msg in queue
|
||||
msg = random_carstate()
|
||||
pub_sock.send(msg.to_bytes())
|
||||
recvd = messaging.recv_one_or_none(sub_sock)
|
||||
assert isinstance(recvd, capnp._DynamicStructReader)
|
||||
assert_carstate(msg.carState, recvd.carState)
|
||||
|
||||
def test_recv_one_retry(self):
|
||||
sock = "carState"
|
||||
sock_timeout = 0.1
|
||||
pub_sock = messaging.pub_sock(sock)
|
||||
sub_sock = messaging.sub_sock(sock, timeout=round(sock_timeout*1000))
|
||||
zmq_sleep()
|
||||
|
||||
# this test doesn't work with ZMQ since multiprocessing interrupts it
|
||||
if "ZMQ" not in os.environ:
|
||||
# wait 5 socket timeouts and make sure it's still retrying
|
||||
result = []
|
||||
thread = threading.Thread(target=lambda: result.append(messaging.recv_one_retry(sub_sock)))
|
||||
thread.start()
|
||||
time.sleep(sock_timeout*5)
|
||||
assert thread.is_alive()
|
||||
msg = random_carstate()
|
||||
pub_sock.send(msg.to_bytes())
|
||||
thread.join(timeout=1)
|
||||
assert not thread.is_alive()
|
||||
assert_carstate(msg.carState, result[0].carState)
|
||||
|
||||
# wait 5 socket timeouts before sending
|
||||
msg = random_carstate()
|
||||
start_time = time.monotonic()
|
||||
delayed_send(sock_timeout*5, pub_sock, msg.to_bytes())
|
||||
recvd = messaging.recv_one_retry(sub_sock)
|
||||
assert (time.monotonic() - start_time) >= sock_timeout*5
|
||||
assert isinstance(recvd, capnp._DynamicStructReader)
|
||||
assert_carstate(msg.carState, recvd.carState)
|
||||
161
iqpilot/cereal/messaging/tests/test_pub_sub_master.py
Normal file
161
iqpilot/cereal/messaging/tests/test_pub_sub_master.py
Normal file
@@ -0,0 +1,161 @@
|
||||
import random
|
||||
import time
|
||||
from typing import Sized, cast
|
||||
|
||||
import iqpilot.cereal.messaging as messaging
|
||||
from iqpilot.cereal.messaging.tests.test_messaging import events, random_sock, random_socks, \
|
||||
random_bytes, random_carstate, assert_carstate, \
|
||||
zmq_sleep
|
||||
from iqpilot.cereal.services import SERVICE_LIST
|
||||
from iqpilot.common.timeout import Timeout
|
||||
|
||||
|
||||
class TestSubMaster:
|
||||
|
||||
def setup_method(self):
|
||||
# ZMQ pub socket takes too long to die
|
||||
# sleep to prevent multiple publishers error between tests
|
||||
zmq_sleep(3)
|
||||
|
||||
def test_init(self):
|
||||
sm = messaging.SubMaster(events)
|
||||
for p in [sm.updated, sm.recv_time, sm.recv_frame, sm.alive,
|
||||
sm.sock, sm.data, sm.logMonoTime, sm.valid]:
|
||||
assert len(cast(Sized, p)) == len(events)
|
||||
|
||||
def test_init_state(self):
|
||||
socks = random_socks()
|
||||
sm = messaging.SubMaster(socks)
|
||||
assert sm.frame == -1
|
||||
assert not any(sm.updated.values())
|
||||
assert not any(sm.seen.values())
|
||||
on_demand = {s: SERVICE_LIST[s].frequency <= 1e-5 for s in sm.services}
|
||||
assert all(sm.alive[s] == sm.valid[s] == sm.freq_ok[s] == on_demand[s] for s in sm.services)
|
||||
assert all(t == 0. for t in sm.recv_time.values())
|
||||
assert all(f == 0 for f in sm.recv_frame.values())
|
||||
assert all(t == 0 for t in sm.logMonoTime.values())
|
||||
|
||||
for p in [sm.updated, sm.recv_time, sm.recv_frame, sm.alive,
|
||||
sm.sock, sm.data, sm.logMonoTime, sm.valid]:
|
||||
assert len(cast(Sized, p)) == len(socks)
|
||||
|
||||
def test_getitem(self):
|
||||
sock = "carState"
|
||||
pub_sock = messaging.pub_sock(sock)
|
||||
sm = messaging.SubMaster([sock,])
|
||||
zmq_sleep()
|
||||
|
||||
msg = random_carstate()
|
||||
pub_sock.send(msg.to_bytes())
|
||||
sm.update(1000)
|
||||
assert_carstate(msg.carState, sm[sock])
|
||||
|
||||
# TODO: break this test up to individually test SubMaster.update and SubMaster.update_msgs
|
||||
def test_update(self):
|
||||
sock = "carState"
|
||||
pub_sock = messaging.pub_sock(sock)
|
||||
sm = messaging.SubMaster([sock,])
|
||||
zmq_sleep()
|
||||
|
||||
for i in range(10):
|
||||
msg = messaging.new_message(sock)
|
||||
pub_sock.send(msg.to_bytes())
|
||||
sm.update(1000)
|
||||
assert sm.frame == i
|
||||
assert all(sm.updated.values())
|
||||
|
||||
def test_update_timeout(self):
|
||||
sock = random_sock()
|
||||
sm = messaging.SubMaster([sock,])
|
||||
timeout = 100
|
||||
start_time = time.monotonic()
|
||||
with Timeout(2):
|
||||
sm.update(timeout)
|
||||
t = time.monotonic() - start_time
|
||||
assert t >= timeout/1000.
|
||||
assert not any(sm.updated.values())
|
||||
|
||||
def test_avg_frequency_checks(self):
|
||||
for poll in (True, False):
|
||||
sm = messaging.SubMaster(["modelV2", "carParams", "carState", "cameraOdometry", "extrinsicsCalibration"],
|
||||
poll=("modelV2" if poll else None),
|
||||
frequency=(20. if not poll else None))
|
||||
|
||||
checks = {
|
||||
"carState": (20, 20),
|
||||
"modelV2": (20, 20 if poll else 10),
|
||||
"cameraOdometry": (20, 10),
|
||||
"extrinsicsCalibration": (4, 4),
|
||||
"carParams": (None, None),
|
||||
"userBookmark": (None, None),
|
||||
}
|
||||
|
||||
for service, (max_freq, min_freq) in checks.items():
|
||||
if max_freq is not None:
|
||||
assert sm._check_avg_freq(service)
|
||||
assert sm.freq_tracker[service].max_freq == max_freq*1.2
|
||||
assert sm.freq_tracker[service].min_freq == min_freq*0.8
|
||||
else:
|
||||
assert not sm._check_avg_freq(service)
|
||||
|
||||
def test_alive(self):
|
||||
pass
|
||||
|
||||
def test_ignore_alive(self):
|
||||
pass
|
||||
|
||||
def test_valid(self):
|
||||
pass
|
||||
|
||||
# SubMaster should always conflate
|
||||
def test_conflate(self):
|
||||
sock = "carState"
|
||||
pub_sock = messaging.pub_sock(sock)
|
||||
sm = messaging.SubMaster([sock,])
|
||||
|
||||
n = 10
|
||||
for i in range(n+1):
|
||||
msg = messaging.new_message(sock)
|
||||
msg.carState.vEgo = i
|
||||
pub_sock.send(msg.to_bytes())
|
||||
time.sleep(0.01)
|
||||
sm.update(1000)
|
||||
assert sm[sock].vEgo == n
|
||||
|
||||
|
||||
class TestPubMaster:
|
||||
|
||||
def setup_method(self):
|
||||
# ZMQ pub socket takes too long to die
|
||||
# sleep to prevent multiple publishers error between tests
|
||||
zmq_sleep(3)
|
||||
|
||||
def test_init(self):
|
||||
messaging.PubMaster(events)
|
||||
|
||||
def test_send(self):
|
||||
socks = random_socks()
|
||||
pm = messaging.PubMaster(socks)
|
||||
sub_socks = {s: messaging.sub_sock(s, conflate=True, timeout=1000) for s in socks}
|
||||
zmq_sleep()
|
||||
|
||||
# PubMaster accepts either a capnp msg builder or bytes
|
||||
for capnp in [True, False]:
|
||||
for i in range(100):
|
||||
sock = socks[i % len(socks)]
|
||||
|
||||
if capnp:
|
||||
try:
|
||||
msg = messaging.new_message(sock)
|
||||
except Exception:
|
||||
msg = messaging.new_message(sock, random.randrange(50))
|
||||
else:
|
||||
msg = random_bytes()
|
||||
|
||||
pm.send(sock, msg)
|
||||
recvd = sub_socks[sock].receive()
|
||||
|
||||
if capnp:
|
||||
msg.clear_write_flag()
|
||||
msg = msg.to_bytes()
|
||||
assert msg == recvd, i
|
||||
27
iqpilot/cereal/messaging/tests/test_services.py
Normal file
27
iqpilot/cereal/messaging/tests/test_services.py
Normal file
@@ -0,0 +1,27 @@
|
||||
import os
|
||||
import tempfile
|
||||
from typing import Dict
|
||||
from parameterized import parameterized
|
||||
|
||||
from iqpilot.cereal import log
|
||||
import iqpilot.cereal.services as services
|
||||
from iqpilot.cereal.services import SERVICE_LIST
|
||||
|
||||
|
||||
class TestServices:
|
||||
|
||||
@parameterized.expand(SERVICE_LIST.keys())
|
||||
def test_services(self, s):
|
||||
service = SERVICE_LIST[s]
|
||||
assert service.frequency <= 104
|
||||
assert service.decimation != 0
|
||||
|
||||
def test_generated_header(self):
|
||||
with tempfile.NamedTemporaryFile(suffix=".h") as f:
|
||||
ret = os.system(f"python3 {services.__file__} > {f.name} && clang++ {f.name} -std=c++11")
|
||||
assert ret == 0, "generated services header is not valid C"
|
||||
|
||||
def test_all_services_exist_in_log_union(self):
|
||||
event_fields = set(log.Event.schema.union_fields)
|
||||
missing = sorted(s for s in SERVICE_LIST if s not in event_fields)
|
||||
assert not missing, f"services missing from log.capnp Event union: {missing}"
|
||||
222
iqpilot/cereal/messaging/tests/validate_sp_cereal_upstream.py
Executable file
222
iqpilot/cereal/messaging/tests/validate_sp_cereal_upstream.py
Executable file
@@ -0,0 +1,222 @@
|
||||
#!/usr/bin/env python3
|
||||
import argparse
|
||||
import sys
|
||||
from typing import Any, List, Tuple
|
||||
|
||||
DEBUG = False
|
||||
|
||||
|
||||
def print_debug(string: str) -> None:
|
||||
if DEBUG:
|
||||
print(string)
|
||||
|
||||
|
||||
def create_schema_instance(struct: Any, prop: Tuple[str, Any]) -> Any:
|
||||
"""
|
||||
Create a new instance of a schema type, handling different field types.
|
||||
|
||||
Args:
|
||||
struct: The Cap'n Proto schema structure
|
||||
prop: A tuple containing the field name and field metadata
|
||||
|
||||
Returns:
|
||||
A new initialized schema instance
|
||||
"""
|
||||
struct_instance = struct.new_message()
|
||||
field_name, field_metadata = prop
|
||||
|
||||
try:
|
||||
field_type = field_metadata.proto.slot.type.which()
|
||||
|
||||
# Initialize different types of fields
|
||||
if field_type in ('list', 'text', 'data'):
|
||||
struct_instance.init(field_name, 1)
|
||||
print_debug(f"Initialized list/text/data field: {field_name}")
|
||||
elif field_type in ('struct', 'object'):
|
||||
struct_instance.init(field_name)
|
||||
print_debug(f"Initialized struct/object field: {field_name}")
|
||||
|
||||
return struct_instance
|
||||
|
||||
except Exception as e:
|
||||
print(f"Error creating instance for {field_name}: {e}")
|
||||
return None
|
||||
|
||||
|
||||
def get_schema_fields(schema_struct: Any) -> List[Tuple[str, Any]]:
|
||||
"""
|
||||
Retrieve all fields from a given schema structure.
|
||||
|
||||
Args:
|
||||
schema_struct: The Cap'n Proto schema structure
|
||||
|
||||
Returns:
|
||||
A list of field names and their metadata
|
||||
"""
|
||||
try:
|
||||
# Get all fields from the schema
|
||||
schema_fields = list(schema_struct.schema.fields.items())
|
||||
|
||||
print_debug("Discovered schema fields:")
|
||||
for field_name, field_metadata in schema_fields:
|
||||
print_debug(f"- {field_name}")
|
||||
|
||||
return schema_fields
|
||||
|
||||
except Exception as e:
|
||||
print(f"Error retrieving schema fields: {e}")
|
||||
return []
|
||||
|
||||
|
||||
def generate_schema_instances(schema_struct: Any) -> List[Any]:
|
||||
"""
|
||||
Generate instances for all fields in a given schema.
|
||||
|
||||
Args:
|
||||
schema_struct: The Cap'n Proto schema structure
|
||||
|
||||
Returns:
|
||||
A list of schema instances
|
||||
"""
|
||||
schema_fields = get_schema_fields(schema_struct)
|
||||
instances = []
|
||||
|
||||
for field_prop in schema_fields:
|
||||
try:
|
||||
instance = create_schema_instance(schema_struct, field_prop)
|
||||
if instance is not None:
|
||||
instances.append(instance)
|
||||
except Exception as e:
|
||||
print(f"Skipping field due to error: {e}")
|
||||
|
||||
print(f"Generated {len(instances)} schema instances")
|
||||
return instances
|
||||
|
||||
|
||||
def persist_instances(instances: List[Any], filename: str) -> None:
|
||||
"""
|
||||
Write schema instances to a binary file.
|
||||
|
||||
Args:
|
||||
instances: List of schema instances
|
||||
filename: Output file path
|
||||
"""
|
||||
try:
|
||||
with open(filename, 'wb') as f:
|
||||
for instance in instances:
|
||||
f.write(instance.to_bytes())
|
||||
|
||||
print(f"Successfully wrote {len(instances)} instances to {filename}")
|
||||
|
||||
except Exception as e:
|
||||
print(f"Error persisting instances: {e}")
|
||||
sys.exit(1)
|
||||
|
||||
|
||||
def read_instances(filename: str, schema_type: Any) -> List[Any]:
|
||||
"""
|
||||
Read schema instances from a binary file.
|
||||
|
||||
Args:
|
||||
filename: Input file path
|
||||
schema_type: The schema type to use for reading
|
||||
|
||||
Returns:
|
||||
A list of read schema instances
|
||||
"""
|
||||
try:
|
||||
with open(filename, 'rb') as f:
|
||||
data = f.read()
|
||||
|
||||
instances = list(schema_type.read_multiple_bytes(data))
|
||||
|
||||
print(f"Read {len(instances)} instances from {filename}")
|
||||
return instances
|
||||
|
||||
except Exception as e:
|
||||
print(f"Error reading instances: {e}")
|
||||
sys.exit(1)
|
||||
|
||||
|
||||
def compare_schemas(original_instances: List[Any], read_instances: List[Any]) -> bool:
|
||||
"""
|
||||
Compare original and read-back instances to detect potential breaking changes.
|
||||
|
||||
Args:
|
||||
original_instances: List of originally generated instances
|
||||
read_instances: List of instances read back from file
|
||||
|
||||
Returns:
|
||||
Boolean indicating whether schemas appear compatible
|
||||
"""
|
||||
if len(original_instances) != len(read_instances):
|
||||
print("❌ Schema Compatibility Warning: Instance count mismatch")
|
||||
return False
|
||||
|
||||
compatible = True
|
||||
for struct in read_instances:
|
||||
try:
|
||||
getattr(struct, struct.which()) # Attempting to access the field to validate readability
|
||||
except Exception as e:
|
||||
print(f"❌ Structural change detected: {struct.which()} is not readable.\nFull error: {e}")
|
||||
compatible = False
|
||||
|
||||
return compatible
|
||||
|
||||
|
||||
def main():
|
||||
"""
|
||||
CLI entry point for schema compatibility testing.
|
||||
"""
|
||||
# Setup argument parser
|
||||
parser = argparse.ArgumentParser(
|
||||
description='Cap\'n Proto Schema Compatibility Testing Tool',
|
||||
epilog='Test schema compatibility by generating and reading back instances.'
|
||||
)
|
||||
|
||||
# Add mutually exclusive group for generation or reading mode
|
||||
mode_group = parser.add_mutually_exclusive_group(required=True)
|
||||
mode_group.add_argument('-g', '--generate', action='store_true',
|
||||
help='Generate schema instances')
|
||||
mode_group.add_argument('-r', '--read', action='store_true',
|
||||
help='Read and validate schema instances')
|
||||
|
||||
# Common arguments
|
||||
parser.add_argument('-f', '--file',
|
||||
default='schema_instances.bin',
|
||||
help='Output/input binary file (default: schema_instances.bin)')
|
||||
|
||||
# Parse arguments
|
||||
args = parser.parse_args()
|
||||
|
||||
# Import the schema dynamically
|
||||
try:
|
||||
from iqpilot.cereal import log
|
||||
schema_type = log.Event
|
||||
except ImportError:
|
||||
print("Error: Unable to import schema. Ensure 'cereal' is installed.")
|
||||
sys.exit(1)
|
||||
|
||||
# Execute based on mode
|
||||
if args.generate:
|
||||
print("🔧 Generating Schema Instances")
|
||||
instances = generate_schema_instances(schema_type)
|
||||
persist_instances(instances, args.file)
|
||||
print("✅ Instance generation complete")
|
||||
|
||||
elif args.read:
|
||||
print("🔍 Reading and Validating Schema Instances")
|
||||
generated_instances = generate_schema_instances(schema_type)
|
||||
read_back_instances = read_instances(args.file, schema_type)
|
||||
|
||||
# Compare schemas
|
||||
if compare_schemas(generated_instances, read_back_instances):
|
||||
print("✅ Schema Compatibility: No breaking changes detected")
|
||||
sys.exit(0)
|
||||
else:
|
||||
print("❌ Potential Schema Breaking Changes Detected")
|
||||
sys.exit(1)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
160
iqpilot/cereal/services.py
Executable file
160
iqpilot/cereal/services.py
Executable file
@@ -0,0 +1,160 @@
|
||||
#!/usr/bin/env python3
|
||||
from enum import IntEnum
|
||||
from typing import Optional
|
||||
|
||||
|
||||
# TODO: this should be automatically determined using the capnp schema
|
||||
class QueueSize(IntEnum):
|
||||
BIG = 10 * 1024 * 1024 # 10MB - video frames, large AI outputs
|
||||
MEDIUM = 2 * 1024 * 1024 # 2MB - high freq (CAN), livestream
|
||||
SMALL = 250 * 1024 # 250KB - most services
|
||||
|
||||
|
||||
class Service:
|
||||
def __init__(self, should_log: bool, frequency: float, decimation: Optional[int] = None,
|
||||
queue_size: QueueSize = QueueSize.SMALL):
|
||||
self.should_log = should_log
|
||||
self.frequency = frequency
|
||||
self.decimation = decimation
|
||||
self.queue_size = queue_size
|
||||
|
||||
|
||||
_services: dict[str, tuple] = {
|
||||
# service: (should_log, frequency, qlog decimation (optional))
|
||||
# note: the "EncodeIdx" packets will still be in the log
|
||||
"gyroscope": (True, 104., 104),
|
||||
"accelerometer": (True, 104., 104),
|
||||
"magnetometer": (True, 25.),
|
||||
"lightSensor": (True, 100., 100),
|
||||
"temperatureSensor": (True, 2., 200),
|
||||
"gpsNMEA": (True, 9.),
|
||||
"deviceState": (True, 2., 1),
|
||||
"touch": (True, 20., 1),
|
||||
"can": (True, 100., 2053, QueueSize.BIG), # decimation gives ~3 msgs in a full segment
|
||||
"controlsState": (True, 100., 10, QueueSize.MEDIUM),
|
||||
"selfdriveState": (True, 100., 10),
|
||||
"pandaStates": (True, 10., 1),
|
||||
"peripheralState": (True, 2., 1),
|
||||
"radarState": (True, 20., 5),
|
||||
"roadEncodeIdx": (False, 20., 1),
|
||||
"radarTracks": (True, 20.),
|
||||
"sendcan": (True, 100., 139, QueueSize.MEDIUM),
|
||||
"logMessage": (True, 0., None, QueueSize.MEDIUM),
|
||||
"errorLogMessage": (True, 0., 1, QueueSize.MEDIUM),
|
||||
"extrinsicsCalibration": (True, 4., 4),
|
||||
"lateralTorqueParameters": (True, 4., 1),
|
||||
"lateralDelay": (True, 4., 1),
|
||||
"androidLog": (True, 0.),
|
||||
"carState": (True, 100., 10),
|
||||
"carControl": (True, 100., 10),
|
||||
"carOutput": (True, 100., 10),
|
||||
"longitudinalPlan": (True, 20., 10),
|
||||
"lateralManeuverPlan": (True, 20.),
|
||||
"driverAssistance": (True, 20., 20),
|
||||
"procLog": (True, 0.5, 15, QueueSize.MEDIUM),
|
||||
"gpsLocationExternal": (True, 10., 10),
|
||||
"gpsLocation": (True, 1., 1),
|
||||
"ubloxGnss": (True, 10.),
|
||||
"qcomGnss": (True, 2.),
|
||||
"gnssMeasurements": (True, 10., 10),
|
||||
"clocks": (True, 0.1, 1),
|
||||
"ubloxRaw": (True, 20.),
|
||||
"deviceMotion": (True, 20., 4),
|
||||
"vehicleParameters": (True, 20., 5),
|
||||
"cameraOdometry": (True, 20., 10),
|
||||
"thumbnail": (True, 1 / 60., 1),
|
||||
"onroadEvents": (True, 1., 1),
|
||||
"carParams": (True, 0.02, 1),
|
||||
"roadCameraState": (True, 20., 20),
|
||||
"driverCameraState": (True, 20., 20),
|
||||
"driverEncodeIdx": (False, 20., 1),
|
||||
"driverStateV2": (True, 20., 10),
|
||||
"driverMonitoringState": (True, 20., 10),
|
||||
"wideRoadEncodeIdx": (False, 20., 1),
|
||||
"wideRoadCameraState": (True, 20., 20),
|
||||
"drivingModelData": (True, 20., 10),
|
||||
"modelV2": (True, 20., None, QueueSize.MEDIUM),
|
||||
"managerState": (True, 2., 1),
|
||||
"uploaderState": (True, 0., 1),
|
||||
"navInstruction": (True, 1., 10),
|
||||
"navRoute": (True, 0.),
|
||||
"navThumbnail": (True, 0.),
|
||||
"qRoadEncodeIdx": (False, 20.),
|
||||
"userBookmark": (True, 0., 1),
|
||||
"soundPressure": (True, 10., 10),
|
||||
"rawAudioData": (False, 20.),
|
||||
"webrtcAudioData": (False, 50.),
|
||||
"bookmarkButton": (True, 0., 1),
|
||||
"audioFeedback": (True, 0., 1),
|
||||
"roadEncodeData": (False, 20., None, QueueSize.BIG),
|
||||
"driverEncodeData": (False, 20., None, QueueSize.BIG),
|
||||
"wideRoadEncodeData": (False, 20., None, QueueSize.BIG),
|
||||
"qRoadEncodeData": (False, 20., None, QueueSize.BIG),
|
||||
|
||||
# iqpilot
|
||||
"iqModelManager": (False, 1., 1, QueueSize.MEDIUM),
|
||||
"backupManagerK3": (False, 1., 1),
|
||||
"iqCarParams": (True, 0.02, 1),
|
||||
"iqCarControl": (True, 100., 10),
|
||||
"iqCarState": (True, 100., 10),
|
||||
"iqLiveData": (True, 1., 1),
|
||||
"iqConstructionZone": (True, 2., 2),
|
||||
"iqVehicleTracks": (True, 4., 4),
|
||||
"iqEnvironment": (True, 4., 4),
|
||||
"mapdOut": (True, 20., 20, QueueSize.MEDIUM),
|
||||
"mapdExtendedOut": (False, 1., -1, QueueSize.MEDIUM),
|
||||
"mapdIn": (False, 1., -1, QueueSize.MEDIUM),
|
||||
"iqNavState": (True, 5., 10),
|
||||
"iqRoadIncidentFeed": (True, 0.2, 1),
|
||||
"iqNavRenderState": (True, 5., 10, QueueSize.MEDIUM),
|
||||
"iqState": (True, 100., 10),
|
||||
"egpuDockState": (True, 10., 10),
|
||||
"iqPlan": (True, 20., 10),
|
||||
"iqOnroadEvents": (True, 1., 1),
|
||||
"iqDriveModelData": (True, 20., None, QueueSize.MEDIUM),
|
||||
"iqLiveLocation": (True, 20.),
|
||||
"liveLocationKalman": (True, 20.),
|
||||
"iqPerfTrace": (True, 0., 1, QueueSize.SMALL),
|
||||
|
||||
# debug
|
||||
"uiDebug": (True, 0., 1),
|
||||
"testJoystick": (True, 0.),
|
||||
"alertDebug": (True, 20., 5),
|
||||
"livestreamWideRoadEncodeIdx": (False, 20.),
|
||||
"livestreamRoadEncodeIdx": (False, 20.),
|
||||
"livestreamDriverEncodeIdx": (False, 20.),
|
||||
"livestreamWideRoadEncodeData": (False, 20., None, QueueSize.MEDIUM),
|
||||
"livestreamRoadEncodeData": (False, 20., None, QueueSize.MEDIUM),
|
||||
"livestreamDriverEncodeData": (False, 20., None, QueueSize.MEDIUM),
|
||||
"customReservedRawData0": (True, 0.),
|
||||
"customReservedRawData1": (True, 0.),
|
||||
"customReservedRawData2": (True, 0.),
|
||||
}
|
||||
SERVICE_LIST = {name: Service(*vals) for
|
||||
idx, (name, vals) in enumerate(_services.items())}
|
||||
|
||||
|
||||
def build_header():
|
||||
h = ""
|
||||
h += "/* THIS IS AN AUTOGENERATED FILE, PLEASE EDIT services.py */\n"
|
||||
h += "#ifndef __SERVICES_H\n"
|
||||
h += "#define __SERVICES_H\n"
|
||||
|
||||
h += "#include <map>\n"
|
||||
h += "#include <string>\n"
|
||||
|
||||
h += "struct service { std::string name; bool should_log; float frequency; int decimation; size_t queue_size; };\n"
|
||||
h += "static std::map<std::string, service> services = {\n"
|
||||
for k, v in SERVICE_LIST.items():
|
||||
should_log = "true" if v.should_log else "false"
|
||||
decimation = -1 if v.decimation is None else v.decimation
|
||||
h += ' { "%s", {"%s", %s, %f, %d, %d}},\n' % \
|
||||
(k, k, should_log, v.frequency, decimation, v.queue_size)
|
||||
h += "};\n"
|
||||
|
||||
h += "#endif\n"
|
||||
return h
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
print(build_header())
|
||||
8
iqpilot/cereal/visionipc.py
Normal file
8
iqpilot/cereal/visionipc.py
Normal file
@@ -0,0 +1,8 @@
|
||||
from enum import IntEnum
|
||||
|
||||
|
||||
class VisionStreamType(IntEnum):
|
||||
VISION_STREAM_ROAD = 0
|
||||
VISION_STREAM_DRIVER = 1
|
||||
VISION_STREAM_WIDE_ROAD = 2
|
||||
VISION_STREAM_MAP = 3
|
||||
1
iqpilot/common/.gitignore
vendored
Normal file
1
iqpilot/common/.gitignore
vendored
Normal file
@@ -0,0 +1 @@
|
||||
*.cpp
|
||||
0
iqpilot/common/__init__.py
Normal file
0
iqpilot/common/__init__.py
Normal file
26
iqpilot/common/api/__init__.py
Normal file
26
iqpilot/common/api/__init__.py
Normal 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
iqpilot/common/api/base.py
Normal file
84
iqpilot/common/api/base.py
Normal 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
iqpilot/common/api/comma_connect.py
Normal file
11
iqpilot/common/api/comma_connect.py
Normal 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
iqpilot/common/atlas_alerts.py
Normal file
281
iqpilot/common/atlas_alerts.py
Normal 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)
|
||||
58
iqpilot/common/auto_units.py
Normal file
58
iqpilot/common/auto_units.py
Normal file
@@ -0,0 +1,58 @@
|
||||
import time
|
||||
|
||||
from iqpilot.common.params import Params
|
||||
from iqpilot.common.swaglog import cloudlog
|
||||
from iqpilot.common.geo_regions import UNKNOWN_REGION, region_for_position, region_is_metric
|
||||
|
||||
CHECK_INTERVAL = 10.0
|
||||
CONFIRMATIONS = 3
|
||||
|
||||
|
||||
class AutoUnits:
|
||||
def __init__(self, params: Params | None = None):
|
||||
self.params = params or Params()
|
||||
self._next_check = 0.0
|
||||
self._candidate = UNKNOWN_REGION
|
||||
self._confirmations = 0
|
||||
|
||||
def _position(self) -> tuple[float, float, bool]:
|
||||
from iqpilot.selfdrive.ui.lib.nav_helpers import current_or_last_gps_position
|
||||
|
||||
lat, lon, _, valid = current_or_last_gps_position(self.params)
|
||||
return lat, lon, valid
|
||||
|
||||
def update(self, now: float | None = None) -> None:
|
||||
if not self.params.get_bool("IQAutoUnits"):
|
||||
self._candidate = UNKNOWN_REGION
|
||||
self._confirmations = 0
|
||||
return
|
||||
|
||||
now = time.monotonic() if now is None else now
|
||||
if now < self._next_check:
|
||||
return
|
||||
self._next_check = now + CHECK_INTERVAL
|
||||
|
||||
lat, lon, valid = self._position()
|
||||
region = region_for_position(lat, lon) if valid else UNKNOWN_REGION
|
||||
if region == UNKNOWN_REGION:
|
||||
self._confirmations = 0
|
||||
return
|
||||
|
||||
if region != self._candidate:
|
||||
self._candidate = region
|
||||
self._confirmations = 1
|
||||
return
|
||||
|
||||
self._confirmations += 1
|
||||
if self._confirmations < CONFIRMATIONS:
|
||||
return
|
||||
|
||||
if region == self.params.get("IQAutoUnitsRegion"):
|
||||
return
|
||||
|
||||
self.params.put("IQAutoUnitsRegion", region)
|
||||
|
||||
metric = region_is_metric(region)
|
||||
if metric != self.params.get_bool("IsMetric"):
|
||||
self.params.put_bool("IsMetric", metric)
|
||||
cloudlog.warning(f"auto units: {region} detected, switching to {'km/h' if metric else 'mph'}")
|
||||
4
iqpilot/common/basedir.py
Normal file
4
iqpilot/common/basedir.py
Normal file
@@ -0,0 +1,4 @@
|
||||
import os
|
||||
|
||||
|
||||
BASEDIR = os.path.abspath(os.path.join(os.path.dirname(os.path.realpath(__file__)), "../.."))
|
||||
23
iqpilot/common/constants.py
Normal file
23
iqpilot/common/constants.py
Normal file
@@ -0,0 +1,23 @@
|
||||
import numpy as np
|
||||
|
||||
# conversions
|
||||
class CV:
|
||||
# Speed
|
||||
MPH_TO_KPH = 1.609344
|
||||
KPH_TO_MPH = 1. / MPH_TO_KPH
|
||||
MS_TO_KPH = 3.6
|
||||
KPH_TO_MS = 1. / MS_TO_KPH
|
||||
MS_TO_MPH = MS_TO_KPH * KPH_TO_MPH
|
||||
MPH_TO_MS = MPH_TO_KPH * KPH_TO_MS
|
||||
MS_TO_KNOTS = 1.9438
|
||||
KNOTS_TO_MS = 1. / MS_TO_KNOTS
|
||||
|
||||
# Angle
|
||||
DEG_TO_RAD = np.pi / 180.
|
||||
RAD_TO_DEG = 1. / DEG_TO_RAD
|
||||
|
||||
# Mass
|
||||
LB_TO_KG = 0.453592
|
||||
|
||||
|
||||
ACCELERATION_DUE_TO_GRAVITY = 9.81 # m/s^2
|
||||
55
iqpilot/common/file_chunker.py
Normal file
55
iqpilot/common/file_chunker.py
Normal file
@@ -0,0 +1,55 @@
|
||||
#!/usr/bin/env python3
|
||||
import sys
|
||||
import math
|
||||
import os
|
||||
from pathlib import Path
|
||||
|
||||
CHUNK_SIZE = 45 * 1024 * 1024 # 45MB, under GitHub's 50MB limit
|
||||
|
||||
def get_chunk_name(name, idx, num_chunks):
|
||||
return f"{name}.chunk{idx+1:02d}of{num_chunks:02d}"
|
||||
|
||||
def get_manifest_path(name):
|
||||
return f"{name}.chunkmanifest"
|
||||
|
||||
def _chunk_paths(path, num_chunks):
|
||||
return [get_manifest_path(path)] + [get_chunk_name(path, i, num_chunks) for i in range(num_chunks)]
|
||||
|
||||
def get_chunk_targets(path, file_size):
|
||||
num_chunks = math.ceil(file_size / CHUNK_SIZE)
|
||||
return _chunk_paths(path, num_chunks)
|
||||
|
||||
def chunk_file(path, targets):
|
||||
manifest_path, *chunk_paths = targets
|
||||
with open(path, 'rb') as f:
|
||||
data = f.read()
|
||||
actual_num_chunks = max(1, math.ceil(len(data) / CHUNK_SIZE))
|
||||
assert len(chunk_paths) >= actual_num_chunks, f"Allowed {len(chunk_paths)} chunks but needs at least {actual_num_chunks}, for path {path}"
|
||||
for i, chunk_path in enumerate(chunk_paths):
|
||||
with open(chunk_path, 'wb') as f:
|
||||
f.write(data[i * CHUNK_SIZE:(i + 1) * CHUNK_SIZE])
|
||||
Path(manifest_path).write_text(str(len(chunk_paths)))
|
||||
os.remove(path)
|
||||
|
||||
def get_existing_chunks(path):
|
||||
if os.path.isfile(path):
|
||||
return [path]
|
||||
if os.path.isfile(manifest := get_manifest_path(path)):
|
||||
num_chunks = int(Path(manifest).read_text().strip())
|
||||
return _chunk_paths(path, num_chunks)
|
||||
raise FileNotFoundError(path)
|
||||
|
||||
def read_file_chunked(path):
|
||||
manifest_path = get_manifest_path(path)
|
||||
if os.path.isfile(manifest_path):
|
||||
num_chunks = int(Path(manifest_path).read_text().strip())
|
||||
return b''.join(Path(get_chunk_name(path, i, num_chunks)).read_bytes() for i in range(num_chunks))
|
||||
if os.path.isfile(path):
|
||||
return Path(path).read_bytes()
|
||||
raise FileNotFoundError(path)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
path = sys.argv[1]
|
||||
chunk_paths = get_chunk_targets(path, os.path.getsize(path))
|
||||
chunk_file(path, chunk_paths)
|
||||
1
iqpilot/common/file_helpers.py
Normal file
1
iqpilot/common/file_helpers.py
Normal file
@@ -0,0 +1 @@
|
||||
from iqpilot.common.utils import CallbackReader, get_upload_stream
|
||||
71
iqpilot/common/filter_simple.py
Normal file
71
iqpilot/common/filter_simple.py
Normal file
@@ -0,0 +1,71 @@
|
||||
from collections import deque
|
||||
|
||||
import numpy as np
|
||||
|
||||
|
||||
class FirstOrderFilter:
|
||||
def __init__(self, x0, rc, dt, initialized=True):
|
||||
self.x = x0
|
||||
self.dt = dt
|
||||
self.update_alpha(rc)
|
||||
self.initialized = initialized
|
||||
|
||||
def update_alpha(self, rc):
|
||||
self.alpha = self.dt / (rc + self.dt)
|
||||
|
||||
def update(self, x):
|
||||
if self.initialized:
|
||||
self.x = (1. - self.alpha) * self.x + self.alpha * x
|
||||
else:
|
||||
self.initialized = True
|
||||
self.x = x
|
||||
return self.x
|
||||
|
||||
|
||||
class BounceFilter(FirstOrderFilter):
|
||||
def __init__(self, x0, rc, dt, initialized=True, bounce=2):
|
||||
self.velocity = FirstOrderFilter(0.0, 0.15, dt)
|
||||
self.bounce = bounce
|
||||
super().__init__(x0, rc, dt, initialized)
|
||||
|
||||
def update(self, x):
|
||||
super().update(x)
|
||||
scale = self.dt / (1.0 / 60.0) # tuned at 60 fps
|
||||
self.velocity.x += (x - self.x) * self.bounce * scale * self.dt
|
||||
self.velocity.update(0.0)
|
||||
if abs(self.velocity.x) < 1e-5:
|
||||
self.velocity.x = 0.0
|
||||
self.x += self.velocity.x
|
||||
return self.x
|
||||
|
||||
|
||||
class MyMovingAverage:
|
||||
def __init__(self, window_size, value=None):
|
||||
self.window_size = window_size
|
||||
if value is not None:
|
||||
self.values = deque([value] * window_size, maxlen=window_size)
|
||||
self.sum = value * window_size
|
||||
self.result = value
|
||||
else:
|
||||
self.values = deque(maxlen=window_size)
|
||||
self.sum = 0
|
||||
self.result = 0
|
||||
|
||||
def set(self, value):
|
||||
self.values.clear()
|
||||
self.values.append(value)
|
||||
self.sum = value
|
||||
self.result = value
|
||||
return value
|
||||
|
||||
def set_all(self, value):
|
||||
self.values = deque([value] * self.window_size, maxlen=self.window_size)
|
||||
self.sum = value * self.window_size
|
||||
self.result = value
|
||||
return value
|
||||
|
||||
def process(self, value, median=False):
|
||||
self.values.append(value)
|
||||
self.sum = sum(self.values)
|
||||
self.result = float(np.median(self.values)) if median else float(self.sum) / len(self.values)
|
||||
return self.result
|
||||
140
iqpilot/common/geo_regions.py
Normal file
140
iqpilot/common/geo_regions.py
Normal file
@@ -0,0 +1,140 @@
|
||||
MPH_REGIONS = ("US", "GB", "LR")
|
||||
METRIC_REGION = "METRIC"
|
||||
UNKNOWN_REGION = ""
|
||||
|
||||
_US_CONUS = [
|
||||
(-123.32, 49.00), (-117.03, 49.00), (-110.00, 49.00), (-104.05, 49.00), (-97.23, 49.00), (-95.15, 49.00),
|
||||
(-95.15, 49.38), (-94.82, 49.30), (-94.68, 48.77), (-93.85, 48.63), (-93.35, 48.62), (-92.72, 48.54),
|
||||
(-92.30, 48.24), (-91.55, 48.10), (-90.84, 48.24), (-89.99, 48.02), (-89.60, 48.02), (-89.10, 48.32),
|
||||
(-88.40, 48.30), (-87.00, 47.80), (-85.60, 47.15), (-84.60, 46.75), (-84.42, 46.56), (-84.30, 46.49),
|
||||
(-84.12, 46.28), (-83.90, 46.05), (-83.40, 45.75), (-82.90, 45.05), (-82.55, 44.00), (-82.42, 43.00),
|
||||
(-82.70, 42.47), (-82.93, 42.34), (-83.00, 42.33), (-83.05, 42.32), (-83.075, 42.312), (-83.13, 42.25),
|
||||
(-83.15, 42.18), (-83.11, 42.10), (-83.09, 42.02),
|
||||
(-82.50, 41.70), (-81.50, 42.00), (-80.20, 42.40), (-79.06, 42.85), (-79.05, 43.27), (-78.00, 43.45),
|
||||
(-77.00, 43.65), (-76.40, 44.10), (-75.80, 44.50), (-74.75, 45.00), (-73.35, 45.01), (-71.50, 45.01),
|
||||
(-71.29, 45.30), (-70.90, 45.30), (-70.72, 45.42), (-70.31, 45.86), (-70.05, 46.44), (-69.99, 46.70),
|
||||
(-69.24, 47.46), (-68.90, 47.20), (-68.38, 47.29), (-67.79, 47.07), (-67.78, 45.94), (-67.42, 45.60),
|
||||
(-67.03, 44.80), (-68.00, 44.30), (-69.06, 43.80), (-70.20, 43.60), (-70.80, 42.85), (-70.00, 41.90),
|
||||
(-70.00, 41.55), (-71.20, 41.30), (-72.00, 41.05), (-73.90, 40.55), (-74.20, 39.60), (-75.05, 38.45),
|
||||
(-75.90, 37.05), (-75.50, 35.20), (-78.50, 33.85), (-80.90, 32.00), (-81.40, 30.70), (-80.03, 26.80),
|
||||
(-80.15, 25.15), (-81.20, 24.55), (-82.00, 26.40), (-82.80, 27.80), (-83.00, 29.15), (-84.30, 29.90),
|
||||
(-85.30, 29.65), (-87.50, 30.25), (-89.00, 29.15), (-89.40, 28.95), (-91.30, 29.10), (-93.80, 29.65),
|
||||
(-95.00, 29.10), (-97.10, 27.80), (-97.14, 25.96), (-98.30, 26.05), (-99.10, 26.40), (-99.50, 27.60),
|
||||
(-100.40, 28.50), (-101.40, 29.77), (-102.30, 29.88), (-102.90, 29.30), (-103.30, 29.00), (-104.37, 29.56),
|
||||
(-104.68, 30.13), (-105.30, 30.80), (-105.85, 31.30), (-106.15, 31.50), (-106.30, 31.68), (-106.45, 31.755),
|
||||
(-106.53, 31.786), (-108.21, 31.783), (-108.21, 31.33), (-111.07, 31.33), (-114.72, 32.72),
|
||||
(-117.13, 32.53), (-118.40, 33.75), (-119.80, 34.40), (-120.65, 35.10), (-121.90, 36.60), (-122.52, 37.78),
|
||||
(-123.75, 39.40), (-124.20, 40.45), (-124.15, 42.00), (-124.05, 43.35), (-123.95, 46.25), (-124.75, 48.40),
|
||||
(-123.30, 48.25), (-123.15, 48.70),
|
||||
]
|
||||
|
||||
_US_ALASKA = [
|
||||
(-141.00, 70.20), (-141.00, 60.30), (-139.05, 60.35), (-137.45, 58.95), (-136.47, 59.63), (-135.03, 59.57),
|
||||
(-134.30, 58.90), (-133.40, 58.20), (-132.20, 56.90), (-130.60, 56.20), (-130.01, 54.80), (-131.80, 54.70),
|
||||
(-133.80, 55.90), (-136.60, 58.20), (-140.00, 59.70), (-145.00, 60.00), (-149.20, 59.10), (-152.30, 57.30),
|
||||
(-155.20, 55.60), (-160.00, 54.60), (-164.50, 54.40), (-162.00, 57.50), (-165.00, 60.20), (-167.50, 62.50),
|
||||
(-164.00, 64.50), (-168.10, 65.60), (-166.00, 68.30), (-161.00, 70.30), (-156.50, 71.40), (-150.00, 70.50),
|
||||
]
|
||||
|
||||
_US_ALEUTIANS_EAST = [(-180.00, 51.00), (-158.50, 51.00), (-158.50, 56.00), (-180.00, 56.00)]
|
||||
_US_ALEUTIANS_WEST = [(172.00, 51.00), (180.00, 51.00), (180.00, 54.00), (172.00, 54.00)]
|
||||
_US_HAWAII = [(-160.50, 18.80), (-154.70, 18.80), (-154.70, 22.30), (-160.50, 22.30)]
|
||||
_US_PUERTO_RICO = [(-67.35, 17.85), (-64.55, 17.85), (-64.55, 18.55), (-67.35, 18.55)]
|
||||
_US_MARIANAS = [(144.50, 13.10), (146.20, 13.10), (146.20, 20.60), (144.50, 20.60)]
|
||||
_US_SAMOA = [(-171.20, -14.60), (-168.10, -14.60), (-168.10, -11.00), (-171.20, -11.00)]
|
||||
|
||||
_GB_BRITAIN = [
|
||||
(-5.72, 50.07), (-4.20, 50.32), (-3.41, 50.62), (-2.45, 50.52), (-1.80, 50.72), (-0.90, 50.77),
|
||||
(0.58, 50.85), (1.35, 51.13), (1.38, 51.38), (1.15, 51.79), (1.35, 51.95), (1.75, 52.48),
|
||||
(1.30, 52.94), (0.49, 52.94), (0.34, 53.15), (-0.08, 53.57), (-0.08, 54.12), (-0.61, 54.49),
|
||||
(-1.18, 54.69), (-1.38, 54.91), (-1.50, 55.13), (-2.00, 55.77), (-2.52, 56.00), (-2.62, 56.28),
|
||||
(-2.47, 56.55), (-2.21, 56.96), (-2.08, 57.14), (-1.77, 57.50), (-2.00, 57.70), (-2.96, 57.68),
|
||||
(-3.90, 57.60), (-4.22, 57.48), (-4.05, 57.81), (-3.85, 58.01), (-3.65, 58.12), (-3.09, 58.44),
|
||||
(-3.01, 58.67), (-3.35, 58.62), (-3.52, 58.60), (-4.99, 58.62), (-5.05, 58.45), (-5.16, 57.90), (-5.70, 57.72),
|
||||
(-5.72, 57.28), (-5.83, 57.00), (-5.72, 56.65), (-5.47, 56.41), (-5.79, 55.60), (-5.62, 55.31),
|
||||
(-4.82, 55.64), (-4.63, 55.46), (-4.85, 55.24), (-5.12, 54.84), (-4.86, 54.63), (-4.44, 54.87),
|
||||
(-4.05, 54.83), (-3.26, 54.98), (-3.05, 54.90), (-3.50, 54.72), (-3.23, 54.07), (-3.05, 53.82),
|
||||
(-3.40, 53.34), (-3.83, 53.33), (-4.63, 53.42), (-4.72, 53.28), (-4.35, 53.12), (-4.76, 52.80),
|
||||
(-4.06, 52.72), (-4.09, 52.41), (-4.66, 52.09), (-5.31, 51.88), (-5.06, 51.70), (-4.70, 51.67),
|
||||
(-4.30, 51.62), (-3.95, 51.56), (-3.70, 51.48), (-3.17, 51.45), (-2.99, 51.55), (-2.67, 51.62),
|
||||
(-2.48, 51.72), (-2.30, 51.85), (-2.70, 51.50), (-2.98, 51.35), (-3.00, 51.20), (-3.47, 51.21),
|
||||
(-4.12, 51.21), (-4.55, 50.83), (-5.08, 50.42), (-5.48, 50.21),
|
||||
]
|
||||
|
||||
_GB_NORTHERN_IRELAND = [
|
||||
(-6.03, 54.05), (-6.28, 54.10), (-6.65, 54.17), (-6.86, 54.33), (-7.16, 54.34), (-7.31, 54.12),
|
||||
(-7.62, 54.14), (-8.00, 54.31), (-8.18, 54.47), (-8.20, 54.52), (-7.90, 54.55), (-7.85, 54.72), (-7.55, 54.75),
|
||||
(-7.44, 54.94), (-7.25, 55.06), (-6.95, 55.22), (-6.50, 55.25), (-6.25, 55.31), (-6.03, 55.22),
|
||||
(-5.43, 54.62), (-5.53, 54.24),
|
||||
]
|
||||
|
||||
_GB_ISLE_OF_MAN = [(-4.85, 54.03), (-4.30, 54.03), (-4.30, 54.42), (-4.85, 54.42)]
|
||||
_GB_CHANNEL_ISLANDS = [(-2.75, 49.15), (-1.95, 49.15), (-1.95, 49.80), (-2.75, 49.80)]
|
||||
_GB_ISLE_OF_WIGHT = [(-1.60, 50.55), (-1.05, 50.55), (-1.05, 50.80), (-1.60, 50.80)]
|
||||
_GB_OUTER_HEBRIDES = [(-7.75, 56.75), (-6.05, 56.75), (-6.05, 58.55), (-7.75, 58.55)]
|
||||
_GB_INNER_HEBRIDES = [(-7.00, 55.45), (-5.55, 55.45), (-5.55, 57.85), (-7.00, 57.85)]
|
||||
_GB_ORKNEY = [(-3.50, 58.70), (-2.35, 58.70), (-2.35, 59.45), (-3.50, 59.45)]
|
||||
_GB_SHETLAND = [(-1.85, 59.80), (-0.65, 59.80), (-0.65, 60.90), (-1.85, 60.90)]
|
||||
|
||||
_LR_LIBERIA = [
|
||||
(-11.46, 6.77), (-11.30, 6.95), (-11.16, 7.15), (-11.05, 7.40), (-10.85, 7.75), (-10.60, 8.00),
|
||||
(-10.28, 8.49), (-9.70, 8.54), (-9.35, 7.80),
|
||||
(-8.85, 7.40), (-8.48, 7.55), (-8.30, 6.90), (-7.95, 6.20), (-7.60, 5.20), (-7.40, 4.55),
|
||||
(-7.74, 4.33), (-8.46, 4.61), (-9.06, 4.97), (-9.52, 5.36), (-10.08, 5.85), (-10.40, 6.11),
|
||||
(-10.83, 6.27),
|
||||
]
|
||||
|
||||
_REGION_RINGS = {
|
||||
"US": (_US_CONUS, _US_ALASKA, _US_ALEUTIANS_EAST, _US_ALEUTIANS_WEST, _US_HAWAII, _US_PUERTO_RICO,
|
||||
_US_MARIANAS, _US_SAMOA),
|
||||
"GB": (_GB_BRITAIN, _GB_NORTHERN_IRELAND, _GB_ISLE_OF_MAN, _GB_CHANNEL_ISLANDS, _GB_ISLE_OF_WIGHT,
|
||||
_GB_OUTER_HEBRIDES, _GB_INNER_HEBRIDES, _GB_ORKNEY, _GB_SHETLAND),
|
||||
"LR": (_LR_LIBERIA,),
|
||||
}
|
||||
|
||||
|
||||
def _bounded(rings):
|
||||
out = []
|
||||
for ring in rings:
|
||||
lons = [p[0] for p in ring]
|
||||
lats = [p[1] for p in ring]
|
||||
out.append(((min(lons), min(lats), max(lons), max(lats)), ring))
|
||||
return tuple(out)
|
||||
|
||||
|
||||
_REGIONS = tuple((region, _bounded(rings)) for region, rings in _REGION_RINGS.items())
|
||||
|
||||
|
||||
def _point_in_ring(lat: float, lon: float, ring) -> bool:
|
||||
inside = False
|
||||
count = len(ring)
|
||||
j = count - 1
|
||||
for i in range(count):
|
||||
lon_i, lat_i = ring[i]
|
||||
lon_j, lat_j = ring[j]
|
||||
if (lat_i > lat) != (lat_j > lat):
|
||||
crossing = (lon_j - lon_i) * (lat - lat_i) / (lat_j - lat_i) + lon_i
|
||||
if lon < crossing:
|
||||
inside = not inside
|
||||
j = i
|
||||
return inside
|
||||
|
||||
|
||||
def valid_position(lat: float, lon: float) -> bool:
|
||||
return abs(lat) <= 90.0 and abs(lon) <= 180.0 and (abs(lat) > 1e-4 or abs(lon) > 1e-4)
|
||||
|
||||
|
||||
def region_for_position(lat: float, lon: float) -> str:
|
||||
if not valid_position(lat, lon):
|
||||
return UNKNOWN_REGION
|
||||
|
||||
for region, rings in _REGIONS:
|
||||
for (min_lon, min_lat, max_lon, max_lat), ring in rings:
|
||||
if min_lon <= lon <= max_lon and min_lat <= lat <= max_lat and _point_in_ring(lat, lon, ring):
|
||||
return region
|
||||
|
||||
return METRIC_REGION
|
||||
|
||||
|
||||
def region_is_metric(region: str) -> bool:
|
||||
return bool(region) and region not in MPH_REGIONS
|
||||
42
iqpilot/common/git.py
Normal file
42
iqpilot/common/git.py
Normal file
@@ -0,0 +1,42 @@
|
||||
from functools import cache
|
||||
import subprocess
|
||||
from iqpilot.common.utils import run_cmd, run_cmd_default
|
||||
|
||||
|
||||
@cache
|
||||
def get_commit(cwd: str | None = None, branch: str = "HEAD") -> str:
|
||||
return run_cmd_default(["git", "rev-parse", branch], cwd=cwd)
|
||||
|
||||
|
||||
@cache
|
||||
def get_commit_date(cwd: str | None = None, commit: str = "HEAD") -> str:
|
||||
return run_cmd_default(["git", "show", "--no-patch", "--format='%ct %ci'", commit], cwd=cwd)
|
||||
|
||||
|
||||
@cache
|
||||
def get_short_branch(cwd: str | None = None) -> str:
|
||||
return run_cmd_default(["git", "rev-parse", "--abbrev-ref", "HEAD"], cwd=cwd)
|
||||
|
||||
|
||||
@cache
|
||||
def get_branch(cwd: str | None = None) -> str:
|
||||
return run_cmd_default(["git", "rev-parse", "--abbrev-ref", "--symbolic-full-name", "@{u}"], cwd=cwd)
|
||||
|
||||
|
||||
@cache
|
||||
def get_origin(cwd: str | None = None) -> str:
|
||||
try:
|
||||
local_branch = run_cmd(["git", "name-rev", "--name-only", "HEAD"], cwd=cwd)
|
||||
tracking_remote = run_cmd(["git", "config", "branch." + local_branch + ".remote"], cwd=cwd)
|
||||
return run_cmd(["git", "config", "remote." + tracking_remote + ".url"], cwd=cwd)
|
||||
except subprocess.CalledProcessError: # Not on a branch, fallback
|
||||
return run_cmd_default(["git", "config", "--get", "remote.origin.url"], cwd=cwd)
|
||||
|
||||
|
||||
@cache
|
||||
def get_normalized_origin(cwd: str | None = None) -> str:
|
||||
return get_origin(cwd) \
|
||||
.replace("git@", "", 1) \
|
||||
.replace(".git", "", 1) \
|
||||
.replace("https://", "", 1) \
|
||||
.replace(":", "/", 1)
|
||||
250
iqpilot/common/git_creds.py
Normal file
250
iqpilot/common/git_creds.py
Normal file
@@ -0,0 +1,250 @@
|
||||
import base64
|
||||
import json
|
||||
import os
|
||||
import subprocess
|
||||
|
||||
from iqpilot.common.params import Params
|
||||
|
||||
PARAM = "GitAuthBlob"
|
||||
PARAMS_DIR = os.environ.get("PARAMS_DIR", "/data/params/d")
|
||||
KEY_DIR = "/data/konn3kt"
|
||||
KEY_PATH = os.path.join(KEY_DIR, "git_auth.key")
|
||||
HELPER_PATH = os.path.join(KEY_DIR, "git_credential_helper.py")
|
||||
DEFAULT_REPO_DIR = "/data/openpilot"
|
||||
CREDENTIAL_HOSTS = ("git.konn3kt.com", "gitlvb.teallvbs.xyz")
|
||||
|
||||
_HELPER_SCRIPT = '''#!/usr/bin/env python3
|
||||
import json
|
||||
import os
|
||||
import sys
|
||||
|
||||
KEY_PATH = "{key_path}"
|
||||
|
||||
|
||||
def main() -> None:
|
||||
if len(sys.argv) < 2 or sys.argv[1] != "get":
|
||||
return
|
||||
# drain git's request on stdin (terminated by a blank line)
|
||||
for line in sys.stdin:
|
||||
if not line.strip():
|
||||
break
|
||||
params_dir = os.environ.get("PARAMS_DIR", "/data/params/d")
|
||||
blob_path = os.path.join(params_dir, "GitAuthBlob")
|
||||
try:
|
||||
with open(KEY_PATH, "rb") as f:
|
||||
key = f.read().strip()
|
||||
with open(blob_path, "rb") as f:
|
||||
blob = f.read()
|
||||
if not blob:
|
||||
return
|
||||
from cryptography.fernet import Fernet
|
||||
data = json.loads(Fernet(key).decrypt(blob).decode())
|
||||
username = data.get("u", "")
|
||||
token = data.get("t", "")
|
||||
if username and token:
|
||||
sys.stdout.write("username=%s\\npassword=%s\\n" % (username, token))
|
||||
except Exception:
|
||||
return
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
'''
|
||||
|
||||
|
||||
def _params_get(name: str) -> bytes | None:
|
||||
# Params -> cereal -> iqdbc: that chain is unavailable mid-bootstrap (this module's
|
||||
# callers install those very packages), so fall back to the params file directly,
|
||||
# exactly like the embedded credential helper does.
|
||||
try:
|
||||
return Params().get(name)
|
||||
except Exception:
|
||||
try:
|
||||
with open(os.path.join(PARAMS_DIR, name), "rb") as f:
|
||||
return f.read()
|
||||
except OSError:
|
||||
return None
|
||||
|
||||
|
||||
def _params_put(name: str, value: bytes) -> None:
|
||||
try:
|
||||
Params().put(name, value)
|
||||
return
|
||||
except Exception:
|
||||
pass
|
||||
os.makedirs(PARAMS_DIR, exist_ok=True)
|
||||
tmp = os.path.join(PARAMS_DIR, f".tmp_{name}")
|
||||
fd = os.open(tmp, os.O_WRONLY | os.O_CREAT | os.O_TRUNC, 0o644)
|
||||
with os.fdopen(fd, "wb") as f:
|
||||
f.write(value)
|
||||
f.flush()
|
||||
os.fsync(f.fileno())
|
||||
os.replace(tmp, os.path.join(PARAMS_DIR, name))
|
||||
|
||||
|
||||
def _params_remove(name: str) -> None:
|
||||
try:
|
||||
Params().remove(name)
|
||||
return
|
||||
except Exception:
|
||||
pass
|
||||
try:
|
||||
os.unlink(os.path.join(PARAMS_DIR, name))
|
||||
except OSError:
|
||||
pass
|
||||
|
||||
|
||||
def _load_or_create_key() -> bytes:
|
||||
from cryptography.fernet import Fernet
|
||||
try:
|
||||
with open(KEY_PATH, "rb") as f:
|
||||
return f.read().strip()
|
||||
except FileNotFoundError:
|
||||
pass
|
||||
key = Fernet.generate_key()
|
||||
os.makedirs(KEY_DIR, exist_ok=True)
|
||||
# write atomically with restrictive perms
|
||||
tmp = KEY_PATH + ".tmp"
|
||||
fd = os.open(tmp, os.O_WRONLY | os.O_CREAT | os.O_TRUNC, 0o600)
|
||||
with os.fdopen(fd, "wb") as f:
|
||||
f.write(key)
|
||||
os.replace(tmp, KEY_PATH)
|
||||
return key
|
||||
|
||||
|
||||
def set_credentials(username: str, token: str) -> None:
|
||||
"""Encrypt and store credentials. Empty username AND token clears them."""
|
||||
username = (username or "").strip()
|
||||
token = (token or "").strip()
|
||||
if not username and not token:
|
||||
clear_credentials()
|
||||
return
|
||||
from cryptography.fernet import Fernet
|
||||
blob = Fernet(_load_or_create_key()).encrypt(
|
||||
json.dumps({"u": username, "t": token}).encode()
|
||||
)
|
||||
_params_put(PARAM, blob)
|
||||
try:
|
||||
install_credential_helper(DEFAULT_REPO_DIR)
|
||||
except Exception:
|
||||
pass
|
||||
|
||||
|
||||
def get_credentials() -> tuple[str, str] | None:
|
||||
"""Return (username, token), or None if unset / unreadable."""
|
||||
blob = _params_get(PARAM)
|
||||
if not blob:
|
||||
return None
|
||||
try:
|
||||
from cryptography.fernet import Fernet
|
||||
data = json.loads(Fernet(_load_or_create_key()).decrypt(blob).decode())
|
||||
return data.get("u", ""), data.get("t", "")
|
||||
except Exception:
|
||||
return None
|
||||
|
||||
|
||||
def clear_credentials() -> None:
|
||||
_params_remove(PARAM)
|
||||
|
||||
|
||||
def has_credentials() -> bool:
|
||||
return get_credentials() is not None
|
||||
|
||||
|
||||
def _auth_header(username: str, token: str) -> str:
|
||||
return "Authorization: Basic " + base64.b64encode(f"{username}:{token}".encode()).decode()
|
||||
|
||||
|
||||
def ssh_to_https(url: str) -> str:
|
||||
"""Convert an SSH git URL to its HTTPS equivalent. Returns url unchanged if it
|
||||
is not an SSH URL. A leading ssh. host label is dropped (ssh.host -> host)."""
|
||||
url = url.strip()
|
||||
host = path = ""
|
||||
if url.startswith("ssh://"):
|
||||
rest = url[len("ssh://"):]
|
||||
rest = rest.split("@", 1)[-1] # drop user@
|
||||
hostport, _, path = rest.partition("/")
|
||||
host = hostport.split(":", 1)[0] # drop :port
|
||||
elif url.startswith("git@") or ("@" in url and ":" in url.split("@", 1)[-1] and "://" not in url):
|
||||
rest = url.split("@", 1)[-1] # host:owner/repo.git
|
||||
host, _, path = rest.partition(":")
|
||||
else:
|
||||
return url # already https/http or unrecognised
|
||||
if host.startswith("ssh."):
|
||||
host = host[len("ssh."):]
|
||||
return f"https://{host}/{path}"
|
||||
|
||||
|
||||
def install_credential_helper(repo_dir: str = DEFAULT_REPO_DIR) -> None:
|
||||
if get_credentials() is None:
|
||||
return
|
||||
|
||||
scopes = {f"https://{host}" for host in CREDENTIAL_HOSTS}
|
||||
origin = subprocess.run(
|
||||
["git", "-C", repo_dir, "config", "--get", "remote.origin.url"],
|
||||
capture_output=True, text=True, check=False,
|
||||
).stdout.strip()
|
||||
https = ssh_to_https(origin)
|
||||
if https.startswith("https://"):
|
||||
from urllib.parse import urlsplit
|
||||
parts = urlsplit(https)
|
||||
if parts.hostname:
|
||||
scopes.add(f"{parts.scheme}://{parts.hostname}")
|
||||
|
||||
try:
|
||||
os.makedirs(KEY_DIR, exist_ok=True)
|
||||
tmp = HELPER_PATH + ".tmp"
|
||||
fd = os.open(tmp, os.O_WRONLY | os.O_CREAT | os.O_TRUNC, 0o755)
|
||||
with os.fdopen(fd, "w") as f:
|
||||
f.write(_HELPER_SCRIPT.format(key_path=KEY_PATH))
|
||||
os.replace(tmp, HELPER_PATH)
|
||||
except Exception:
|
||||
return
|
||||
|
||||
helper_cmd = f"!/usr/bin/env python3 {HELPER_PATH}"
|
||||
for scope in scopes:
|
||||
subprocess.run(
|
||||
["git", "config", "--global", f"credential.{scope}.helper", helper_cmd],
|
||||
check=False, capture_output=True,
|
||||
)
|
||||
|
||||
|
||||
def configure(repo_dir: str) -> None:
|
||||
"""Apply on-device credentials to the git repo at repo_dir before a remote op.
|
||||
|
||||
No-op when no credentials are stored. If the origin is an SSH URL it is
|
||||
rewritten in-place to the HTTPS equivalent so the Basic-auth header applies.
|
||||
The header is injected via GIT_CONFIG_* env (never persisted to .git/config).
|
||||
Idempotent."""
|
||||
creds = get_credentials()
|
||||
if creds is None:
|
||||
return
|
||||
username, token = creds
|
||||
|
||||
try:
|
||||
install_credential_helper(repo_dir)
|
||||
except Exception:
|
||||
pass
|
||||
|
||||
origin = subprocess.run(
|
||||
["git", "-C", repo_dir, "config", "--get", "remote.origin.url"],
|
||||
capture_output=True, text=True, check=False,
|
||||
).stdout.strip()
|
||||
if not origin:
|
||||
return
|
||||
|
||||
https = ssh_to_https(origin)
|
||||
if https != origin and https.startswith("https://"):
|
||||
subprocess.run(
|
||||
["git", "-C", repo_dir, "config", "remote.origin.url", https],
|
||||
check=False, capture_output=True,
|
||||
)
|
||||
|
||||
if not https.startswith("https://"):
|
||||
return # header auth only works over https
|
||||
|
||||
# scope to this exact repo URL prefix (trailing slash => component boundary)
|
||||
key = https if https.endswith("/") else https + "/"
|
||||
os.environ["GIT_CONFIG_COUNT"] = "1"
|
||||
os.environ["GIT_CONFIG_KEY_0"] = f"http.{key}.extraHeader"
|
||||
os.environ["GIT_CONFIG_VALUE_0"] = _auth_header(username, token)
|
||||
89
iqpilot/common/gpio.py
Normal file
89
iqpilot/common/gpio.py
Normal file
@@ -0,0 +1,89 @@
|
||||
import os
|
||||
import fcntl
|
||||
import ctypes
|
||||
from functools import cache
|
||||
|
||||
def gpio_init(pin: int, output: bool) -> None:
|
||||
try:
|
||||
with open(f"/sys/class/gpio/gpio{pin}/direction", 'wb') as f:
|
||||
f.write(b"out" if output else b"in")
|
||||
except Exception as e:
|
||||
print(f"Failed to set gpio {pin} direction: {e}")
|
||||
|
||||
def gpio_set(pin: int, high: bool) -> None:
|
||||
try:
|
||||
with open(f"/sys/class/gpio/gpio{pin}/value", 'wb') as f:
|
||||
f.write(b"1" if high else b"0")
|
||||
except Exception as e:
|
||||
print(f"Failed to set gpio {pin} value: {e}")
|
||||
|
||||
def gpio_read(pin: int) -> bool | None:
|
||||
val = None
|
||||
try:
|
||||
with open(f"/sys/class/gpio/gpio{pin}/value", 'rb') as f:
|
||||
val = bool(int(f.read().strip()))
|
||||
except Exception as e:
|
||||
print(f"Failed to set gpio {pin} value: {e}")
|
||||
|
||||
return val
|
||||
|
||||
def gpio_export(pin: int) -> None:
|
||||
if os.path.isdir(f"/sys/class/gpio/gpio{pin}"):
|
||||
return
|
||||
|
||||
try:
|
||||
with open("/sys/class/gpio/export", 'w') as f:
|
||||
f.write(str(pin))
|
||||
except Exception:
|
||||
print(f"Failed to export gpio {pin}")
|
||||
|
||||
@cache
|
||||
def get_irq_action(irq: int) -> list[str]:
|
||||
try:
|
||||
with open(f"/sys/kernel/irq/{irq}/actions") as f:
|
||||
actions = f.read().strip().split(',')
|
||||
return actions
|
||||
except FileNotFoundError:
|
||||
return []
|
||||
|
||||
def get_irqs_for_action(action: str) -> list[str]:
|
||||
ret = []
|
||||
with open("/proc/interrupts") as f:
|
||||
for l in f.readlines():
|
||||
irq = l.split(':')[0].strip()
|
||||
if irq.isdigit() and action in get_irq_action(irq):
|
||||
ret.append(irq)
|
||||
return ret
|
||||
|
||||
# *** gpiochip ***
|
||||
|
||||
class gpioevent_data(ctypes.Structure):
|
||||
_fields_ = [
|
||||
("timestamp", ctypes.c_uint64),
|
||||
("id", ctypes.c_uint32),
|
||||
]
|
||||
|
||||
class gpioevent_request(ctypes.Structure):
|
||||
_fields_ = [
|
||||
("lineoffset", ctypes.c_uint32),
|
||||
("handleflags", ctypes.c_uint32),
|
||||
("eventflags", ctypes.c_uint32),
|
||||
("label", ctypes.c_char * 32),
|
||||
("fd", ctypes.c_int)
|
||||
]
|
||||
|
||||
def gpiochip_get_ro_value_fd(label: str, gpiochip_id: int, pin: int) -> int:
|
||||
GPIOEVENT_REQUEST_BOTH_EDGES = 0x3
|
||||
GPIOHANDLE_REQUEST_INPUT = 0x1
|
||||
GPIO_GET_LINEEVENT_IOCTL = 0xc030b404
|
||||
|
||||
rq = gpioevent_request()
|
||||
rq.lineoffset = pin
|
||||
rq.handleflags = GPIOHANDLE_REQUEST_INPUT
|
||||
rq.eventflags = GPIOEVENT_REQUEST_BOTH_EDGES
|
||||
rq.label = label.encode('utf-8')[:31] + b'\0'
|
||||
|
||||
fd = os.open(f"/dev/gpiochip{gpiochip_id}", os.O_RDONLY)
|
||||
fcntl.ioctl(fd, GPIO_GET_LINEEVENT_IOCTL, rq)
|
||||
os.close(fd)
|
||||
return int(rq.fd)
|
||||
8
iqpilot/common/gps.py
Normal file
8
iqpilot/common/gps.py
Normal file
@@ -0,0 +1,8 @@
|
||||
from iqpilot.common.params import Params
|
||||
|
||||
|
||||
def get_gps_location_service(params: Params) -> str:
|
||||
if params.get_bool("UbloxAvailable"):
|
||||
return "gpsLocationExternal"
|
||||
else:
|
||||
return "gpsLocation"
|
||||
81
iqpilot/common/i2c.py
Normal file
81
iqpilot/common/i2c.py
Normal file
@@ -0,0 +1,81 @@
|
||||
import os
|
||||
import fcntl
|
||||
import ctypes
|
||||
|
||||
# I2C constants from /usr/include/linux/i2c-dev.h
|
||||
I2C_SLAVE = 0x0703
|
||||
I2C_SLAVE_FORCE = 0x0706
|
||||
I2C_SMBUS = 0x0720
|
||||
|
||||
# SMBus transfer types
|
||||
I2C_SMBUS_READ = 1
|
||||
I2C_SMBUS_WRITE = 0
|
||||
I2C_SMBUS_BYTE_DATA = 2
|
||||
I2C_SMBUS_I2C_BLOCK_DATA = 8
|
||||
|
||||
I2C_SMBUS_BLOCK_MAX = 32
|
||||
|
||||
|
||||
class _I2cSmbusData(ctypes.Union):
|
||||
_fields_ = [
|
||||
("byte", ctypes.c_uint8),
|
||||
("word", ctypes.c_uint16),
|
||||
("block", ctypes.c_uint8 * (I2C_SMBUS_BLOCK_MAX + 2)),
|
||||
]
|
||||
|
||||
|
||||
class _I2cSmbusIoctlData(ctypes.Structure):
|
||||
_fields_ = [
|
||||
("read_write", ctypes.c_uint8),
|
||||
("command", ctypes.c_uint8),
|
||||
("size", ctypes.c_uint32),
|
||||
("data", ctypes.POINTER(_I2cSmbusData)),
|
||||
]
|
||||
|
||||
|
||||
class SMBus:
|
||||
def __init__(self, bus: int):
|
||||
self._fd = os.open(f'/dev/i2c-{bus}', os.O_RDWR)
|
||||
|
||||
def __enter__(self) -> 'SMBus':
|
||||
return self
|
||||
|
||||
def __exit__(self, *args) -> None:
|
||||
self.close()
|
||||
|
||||
def close(self) -> None:
|
||||
if hasattr(self, '_fd') and self._fd >= 0:
|
||||
os.close(self._fd)
|
||||
self._fd = -1
|
||||
|
||||
def _set_address(self, addr: int, force: bool = False) -> None:
|
||||
ioctl_arg = I2C_SLAVE_FORCE if force else I2C_SLAVE
|
||||
fcntl.ioctl(self._fd, ioctl_arg, addr)
|
||||
|
||||
def _smbus_access(self, read_write: int, command: int, size: int, data: _I2cSmbusData) -> None:
|
||||
ioctl_data = _I2cSmbusIoctlData(read_write, command, size, ctypes.pointer(data))
|
||||
fcntl.ioctl(self._fd, I2C_SMBUS, ioctl_data)
|
||||
|
||||
def read_byte_data(self, addr: int, register: int, force: bool = False) -> int:
|
||||
self._set_address(addr, force)
|
||||
data = _I2cSmbusData()
|
||||
self._smbus_access(I2C_SMBUS_READ, register, I2C_SMBUS_BYTE_DATA, data)
|
||||
return int(data.byte)
|
||||
|
||||
def write_byte_data(self, addr: int, register: int, value: int, force: bool = False) -> None:
|
||||
self._set_address(addr, force)
|
||||
data = _I2cSmbusData()
|
||||
data.byte = value & 0xFF
|
||||
self._smbus_access(I2C_SMBUS_WRITE, register, I2C_SMBUS_BYTE_DATA, data)
|
||||
|
||||
def read_i2c_block_data(self, addr: int, register: int, length: int, force: bool = False) -> list[int]:
|
||||
self._set_address(addr, force)
|
||||
if not (0 <= length <= I2C_SMBUS_BLOCK_MAX):
|
||||
raise ValueError(f"length must be 0..{I2C_SMBUS_BLOCK_MAX}")
|
||||
|
||||
data = _I2cSmbusData()
|
||||
data.block[0] = length
|
||||
self._smbus_access(I2C_SMBUS_READ, register, I2C_SMBUS_I2C_BLOCK_DATA, data)
|
||||
read_len = int(data.block[0]) or length
|
||||
read_len = min(read_len, length)
|
||||
return [int(b) for b in data.block[1 : read_len + 1]]
|
||||
187
iqpilot/common/iq_perf.py
Normal file
187
iqpilot/common/iq_perf.py
Normal 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
iqpilot/common/issue_debug.py
Normal file
44
iqpilot/common/issue_debug.py
Normal 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
|
||||
15
iqpilot/common/k3_slc_log.py
Normal file
15
iqpilot/common/k3_slc_log.py
Normal file
@@ -0,0 +1,15 @@
|
||||
from datetime import datetime
|
||||
|
||||
from iqpilot.common.swaglog import cloudlog
|
||||
|
||||
K3_SLC_LOG_FILE = "/data/openpilot/k3_slc.txt"
|
||||
|
||||
|
||||
def k3_slc_log(message: str) -> None:
|
||||
try:
|
||||
with open(K3_SLC_LOG_FILE, "a") as f:
|
||||
timestamp = datetime.now().strftime("%Y-%m-%d %H:%M:%S.%f")[:-3]
|
||||
f.write(f"[{timestamp}] {message}\n")
|
||||
f.flush()
|
||||
except Exception as e:
|
||||
cloudlog.error(f"[K3_SLC] Failed to write debug log: {e}")
|
||||
249
iqpilot/common/logging_extra.py
Normal file
249
iqpilot/common/logging_extra.py
Normal file
@@ -0,0 +1,249 @@
|
||||
import io
|
||||
import os
|
||||
import sys
|
||||
import copy
|
||||
import json
|
||||
import time
|
||||
import uuid
|
||||
import socket
|
||||
import logging
|
||||
import traceback
|
||||
import numpy as np
|
||||
from threading import local
|
||||
from collections import OrderedDict
|
||||
from contextlib import contextmanager
|
||||
|
||||
LOG_TIMESTAMPS = "LOG_TIMESTAMPS" in os.environ
|
||||
|
||||
def json_handler(obj):
|
||||
if isinstance(obj, np.bool_):
|
||||
return bool(obj)
|
||||
# if isinstance(obj, (datetime.date, datetime.time)):
|
||||
# return obj.isoformat()
|
||||
return repr(obj)
|
||||
|
||||
def json_robust_dumps(obj):
|
||||
return json.dumps(obj, default=json_handler)
|
||||
|
||||
class NiceOrderedDict(OrderedDict):
|
||||
def __str__(self):
|
||||
return json_robust_dumps(self)
|
||||
|
||||
class SwagFormatter(logging.Formatter):
|
||||
def __init__(self, swaglogger):
|
||||
logging.Formatter.__init__(self, None, '%a %b %d %H:%M:%S %Z %Y')
|
||||
|
||||
self.swaglogger = swaglogger
|
||||
self.host = socket.gethostname()
|
||||
|
||||
def format_dict(self, record):
|
||||
record_dict = NiceOrderedDict()
|
||||
|
||||
if isinstance(record.msg, dict):
|
||||
record_dict['msg'] = record.msg
|
||||
else:
|
||||
try:
|
||||
record_dict['msg'] = record.getMessage()
|
||||
except (ValueError, TypeError):
|
||||
record_dict['msg'] = [record.msg]+record.args
|
||||
|
||||
record_dict['ctx'] = self.swaglogger.get_ctx()
|
||||
|
||||
if record.exc_info:
|
||||
record_dict['exc_info'] = self.formatException(record.exc_info)
|
||||
|
||||
record_dict['level'] = record.levelname
|
||||
record_dict['levelnum'] = record.levelno
|
||||
record_dict['name'] = record.name
|
||||
record_dict['filename'] = record.filename
|
||||
record_dict['lineno'] = record.lineno
|
||||
record_dict['pathname'] = record.pathname
|
||||
record_dict['module'] = record.module
|
||||
record_dict['funcName'] = record.funcName
|
||||
record_dict['host'] = self.host
|
||||
record_dict['process'] = record.process
|
||||
record_dict['thread'] = record.thread
|
||||
record_dict['threadName'] = record.threadName
|
||||
record_dict['created'] = record.created
|
||||
|
||||
return record_dict
|
||||
|
||||
def format(self, record):
|
||||
if self.swaglogger is None:
|
||||
raise Exception("must set swaglogger before calling format()")
|
||||
return json_robust_dumps(self.format_dict(record))
|
||||
|
||||
class SwagLogFileFormatter(SwagFormatter):
|
||||
def fix_kv(self, k, v):
|
||||
# append type to names to preserve legacy naming in logs
|
||||
# avoids overlapping key namespaces with different types
|
||||
# e.g. log.info() creates 'msg' -> 'msg$s'
|
||||
# log.event() creates 'msg.health.logMonoTime' -> 'msg.health.logMonoTime$i'
|
||||
# because overlapping namespace 'msg' caused problems
|
||||
if isinstance(v, (str, bytes)):
|
||||
k += "$s"
|
||||
elif isinstance(v, float):
|
||||
k += "$f"
|
||||
elif isinstance(v, bool):
|
||||
k += "$b"
|
||||
elif isinstance(v, int):
|
||||
k += "$i"
|
||||
elif isinstance(v, dict):
|
||||
nv = {}
|
||||
for ik, iv in v.items():
|
||||
ik, iv = self.fix_kv(ik, iv)
|
||||
nv[ik] = iv
|
||||
v = nv
|
||||
elif isinstance(v, list):
|
||||
k += "$a"
|
||||
return k, v
|
||||
|
||||
def format(self, record):
|
||||
if isinstance(record, str):
|
||||
v = json.loads(record)
|
||||
else:
|
||||
v = self.format_dict(record)
|
||||
|
||||
mk, mv = self.fix_kv('msg', v['msg'])
|
||||
del v['msg']
|
||||
v[mk] = mv
|
||||
v['id'] = uuid.uuid4().hex
|
||||
|
||||
return json_robust_dumps(v)
|
||||
|
||||
class SwagErrorFilter(logging.Filter):
|
||||
def filter(self, record):
|
||||
return record.levelno < logging.ERROR
|
||||
|
||||
def _tmpfunc():
|
||||
return 0
|
||||
|
||||
def _srcfile():
|
||||
return os.path.normcase(_tmpfunc.__code__.co_filename)
|
||||
|
||||
class SwagLogger(logging.Logger):
|
||||
def __init__(self):
|
||||
logging.Logger.__init__(self, "swaglog")
|
||||
|
||||
self.global_ctx = {}
|
||||
|
||||
self.log_local = local()
|
||||
self.log_local.ctx = {}
|
||||
|
||||
def local_ctx(self):
|
||||
try:
|
||||
return self.log_local.ctx
|
||||
except AttributeError:
|
||||
self.log_local.ctx = {}
|
||||
return self.log_local.ctx
|
||||
|
||||
def get_ctx(self):
|
||||
return dict(self.local_ctx(), **self.global_ctx)
|
||||
|
||||
@contextmanager
|
||||
def ctx(self, **kwargs):
|
||||
old_ctx = self.local_ctx()
|
||||
self.log_local.ctx = copy.copy(old_ctx) or {}
|
||||
self.log_local.ctx.update(kwargs)
|
||||
try:
|
||||
yield
|
||||
finally:
|
||||
self.log_local.ctx = old_ctx
|
||||
|
||||
def bind(self, **kwargs):
|
||||
self.local_ctx().update(kwargs)
|
||||
|
||||
def bind_global(self, **kwargs):
|
||||
self.global_ctx.update(kwargs)
|
||||
|
||||
def event(self, event, *args, **kwargs):
|
||||
evt = NiceOrderedDict()
|
||||
evt['event'] = event
|
||||
if args:
|
||||
evt['args'] = args
|
||||
evt.update(kwargs)
|
||||
if 'error' in kwargs:
|
||||
self.error(evt)
|
||||
elif 'debug' in kwargs:
|
||||
self.debug(evt)
|
||||
else:
|
||||
self.info(evt)
|
||||
|
||||
def timestamp(self, event_name):
|
||||
if LOG_TIMESTAMPS:
|
||||
t = time.monotonic()
|
||||
tstp = NiceOrderedDict()
|
||||
tstp['timestamp'] = NiceOrderedDict()
|
||||
tstp['timestamp']["event"] = event_name
|
||||
tstp['timestamp']["time"] = t*1e9
|
||||
self.debug(tstp)
|
||||
|
||||
def findCaller(self, stack_info=False, stacklevel=1):
|
||||
"""
|
||||
Find the stack frame of the caller so that we can note the source
|
||||
file name, line number and function name.
|
||||
"""
|
||||
f = sys._getframe(3)
|
||||
#On some versions of IronPython, currentframe() returns None if
|
||||
#IronPython isn't run with -X:Frames.
|
||||
if f is not None:
|
||||
f = f.f_back
|
||||
orig_f = f
|
||||
while f and stacklevel > 1:
|
||||
f = f.f_back
|
||||
stacklevel -= 1
|
||||
if not f:
|
||||
f = orig_f
|
||||
rv = "(unknown file)", 0, "(unknown function)", None
|
||||
while hasattr(f, "f_code"):
|
||||
co = f.f_code
|
||||
filename = os.path.normcase(co.co_filename)
|
||||
|
||||
if filename == _srcfile:
|
||||
f = f.f_back
|
||||
continue
|
||||
sinfo = None
|
||||
if stack_info:
|
||||
sio = io.StringIO()
|
||||
sio.write('Stack (most recent call last):\n')
|
||||
traceback.print_stack(f, file=sio)
|
||||
sinfo = sio.getvalue()
|
||||
if sinfo[-1] == '\n':
|
||||
sinfo = sinfo[:-1]
|
||||
sio.close()
|
||||
rv = (co.co_filename, f.f_lineno, co.co_name, sinfo)
|
||||
break
|
||||
return rv
|
||||
|
||||
if __name__ == "__main__":
|
||||
log = SwagLogger()
|
||||
|
||||
stdout_handler = logging.StreamHandler(sys.stdout)
|
||||
stdout_handler.setLevel(logging.INFO)
|
||||
stdout_handler.addFilter(SwagErrorFilter())
|
||||
log.addHandler(stdout_handler)
|
||||
|
||||
stderr_handler = logging.StreamHandler(sys.stderr)
|
||||
stderr_handler.setLevel(logging.ERROR)
|
||||
log.addHandler(stderr_handler)
|
||||
|
||||
log.info("asdasd %s", "a")
|
||||
log.info({'wut': 1})
|
||||
log.warning("warning")
|
||||
log.error("error")
|
||||
log.critical("critical")
|
||||
log.event("test", x="y")
|
||||
|
||||
with log.ctx():
|
||||
stdout_handler.setFormatter(SwagFormatter(log))
|
||||
stderr_handler.setFormatter(SwagFormatter(log))
|
||||
log.bind(user="some user")
|
||||
log.info("in req")
|
||||
print("")
|
||||
log.warning("warning")
|
||||
print("")
|
||||
log.error("error")
|
||||
print("")
|
||||
log.critical("critical")
|
||||
print("")
|
||||
log.event("do_req", a=1, b="c")
|
||||
45
iqpilot/common/markdown.py
Normal file
45
iqpilot/common/markdown.py
Normal file
@@ -0,0 +1,45 @@
|
||||
HTML_REPLACEMENTS = [
|
||||
(r'&', r'&'),
|
||||
(r'"', r'"'),
|
||||
]
|
||||
|
||||
def parse_markdown(text: str, tab_length: int = 2) -> str:
|
||||
lines = text.split("\n")
|
||||
output: list[str] = []
|
||||
list_level = 0
|
||||
|
||||
def end_outstanding_lists(level: int, end_level: int) -> int:
|
||||
while level > end_level:
|
||||
level -= 1
|
||||
output.append("</ul>")
|
||||
if level > 0:
|
||||
output.append("</li>")
|
||||
return end_level
|
||||
|
||||
for i, line in enumerate(lines):
|
||||
if i + 1 < len(lines) and lines[i + 1].startswith("==="): # heading
|
||||
output.append(f"<h1>{line}</h1>")
|
||||
elif line.startswith("==="):
|
||||
pass
|
||||
elif line.lstrip().startswith("* "): # list
|
||||
line_level = 1 + line.count(" " * tab_length, 0, line.index("*"))
|
||||
if list_level >= line_level:
|
||||
list_level = end_outstanding_lists(list_level, line_level)
|
||||
else:
|
||||
list_level += 1
|
||||
if list_level > 1:
|
||||
output[-1] = output[-1].replace("</li>", "")
|
||||
output.append("<ul>")
|
||||
output.append(f"<li>{line.replace('*', '', 1).lstrip()}</li>")
|
||||
else:
|
||||
list_level = end_outstanding_lists(list_level, 0)
|
||||
if len(line) > 0:
|
||||
output.append(line)
|
||||
|
||||
end_outstanding_lists(list_level, 0)
|
||||
output_str = "\n".join(output) + "\n"
|
||||
|
||||
for (fr, to) in HTML_REPLACEMENTS:
|
||||
output_str = output_str.replace(fr, to)
|
||||
|
||||
return output_str
|
||||
50
iqpilot/common/mock/__init__.py
Normal file
50
iqpilot/common/mock/__init__.py
Normal file
@@ -0,0 +1,50 @@
|
||||
"""
|
||||
Utilities for generating mock messages for testing.
|
||||
example in common/tests/test_mock.py
|
||||
"""
|
||||
|
||||
|
||||
import functools
|
||||
import threading
|
||||
from iqpilot.cereal.messaging import PubMaster
|
||||
from iqpilot.cereal.services import SERVICE_LIST
|
||||
from iqpilot.common.mock.generators import generate_deviceMotion
|
||||
from iqpilot.common.realtime import Ratekeeper
|
||||
|
||||
|
||||
MOCK_GENERATOR = {
|
||||
"deviceMotion": generate_deviceMotion
|
||||
}
|
||||
|
||||
|
||||
def generate_messages_loop(services: list[str], done: threading.Event):
|
||||
pm = PubMaster(services)
|
||||
rk = Ratekeeper(100)
|
||||
i = 0
|
||||
while not done.is_set():
|
||||
for s in services:
|
||||
should_send = i % (100/SERVICE_LIST[s].frequency) == 0
|
||||
if should_send:
|
||||
message = MOCK_GENERATOR[s]()
|
||||
pm.send(s, message)
|
||||
i += 1
|
||||
rk.keep_time()
|
||||
|
||||
|
||||
def mock_messages(services: list[str] | str):
|
||||
if isinstance(services, str):
|
||||
services = [services]
|
||||
|
||||
def decorator(func):
|
||||
@functools.wraps(func)
|
||||
def wrapper(*args, **kwargs):
|
||||
done = threading.Event()
|
||||
t = threading.Thread(target=generate_messages_loop, args=(services, done))
|
||||
t.start()
|
||||
try:
|
||||
return func(*args, **kwargs)
|
||||
finally:
|
||||
done.set()
|
||||
t.join()
|
||||
return wrapper
|
||||
return decorator
|
||||
14
iqpilot/common/mock/generators.py
Normal file
14
iqpilot/common/mock/generators.py
Normal file
@@ -0,0 +1,14 @@
|
||||
from iqpilot.cereal import messaging
|
||||
|
||||
|
||||
def generate_deviceMotion():
|
||||
msg = messaging.new_message('deviceMotion')
|
||||
meas = {'x': 0.0, 'y': 0.0, 'z': 0.0, 'xStd': 0.0, 'yStd': 0.0, 'zStd': 0.0, 'valid': True}
|
||||
msg.deviceMotion.orientationNED = meas
|
||||
msg.deviceMotion.velocityDevice = meas
|
||||
msg.deviceMotion.angularVelocityDevice = meas
|
||||
msg.deviceMotion.accelerationDevice = meas
|
||||
msg.deviceMotion.inputsOK = True
|
||||
msg.deviceMotion.posenetOK = True
|
||||
msg.deviceMotion.sensorsOK = True
|
||||
return msg
|
||||
158
iqpilot/common/params.py
Normal file
158
iqpilot/common/params.py
Normal file
@@ -0,0 +1,158 @@
|
||||
try:
|
||||
from iqpilot.common.params_pyx import Params, ParamKeyFlag, ParamKeyType, UnknownKeyName
|
||||
except ImportError:
|
||||
import datetime
|
||||
import os
|
||||
import threading
|
||||
from enum import IntEnum, IntFlag
|
||||
|
||||
class UnknownKeyName(Exception):
|
||||
pass
|
||||
|
||||
class ParamKeyFlag(IntFlag):
|
||||
# must stay in lockstep with enum ParamKeyFlag in common/params.h
|
||||
PERSISTENT = 0x02
|
||||
CLEAR_ON_MANAGER_START = 0x04
|
||||
CLEAR_ON_ONROAD_TRANSITION = 0x08
|
||||
CLEAR_ON_OFFROAD_TRANSITION = 0x10
|
||||
DONT_LOG = 0x20
|
||||
DEVELOPMENT_ONLY = 0x40
|
||||
CLEAR_ON_IGNITION_ON = 0x80
|
||||
ALL = 0xFFFFFFFF
|
||||
|
||||
class ParamKeyType(IntEnum):
|
||||
STRING = 0
|
||||
BOOL = 1
|
||||
INT = 2
|
||||
FLOAT = 3
|
||||
TIME = 4
|
||||
JSON = 5
|
||||
BYTES = 6
|
||||
|
||||
class Params:
|
||||
def __init__(self, path: str = ""):
|
||||
if path:
|
||||
root = path
|
||||
else:
|
||||
from iqpilot.system.hardware.hw import Paths
|
||||
root = Paths.params()
|
||||
self._d = os.path.join(root, os.environ.get("OPENPILOT_PREFIX", "d"))
|
||||
self._lock = threading.Lock()
|
||||
|
||||
def _p(self, key):
|
||||
if isinstance(key, bytes):
|
||||
key = key.decode()
|
||||
return os.path.join(self._d, key)
|
||||
|
||||
def check_key(self, key):
|
||||
return True
|
||||
|
||||
def get(self, key, block: bool = False, return_default: bool = False, encoding=None):
|
||||
try:
|
||||
with open(self._p(key), "rb") as f:
|
||||
dat = f.read()
|
||||
except (FileNotFoundError, NotADirectoryError, IsADirectoryError):
|
||||
return None
|
||||
if encoding is not None:
|
||||
return dat.decode(encoding)
|
||||
# params_pyx returns string-typed values decoded; default to utf-8, fall back to raw bytes
|
||||
try:
|
||||
return dat.decode("utf-8")
|
||||
except UnicodeDecodeError:
|
||||
return dat
|
||||
|
||||
def get_bool(self, key, block: bool = False) -> bool:
|
||||
try:
|
||||
with open(self._p(key), "rb") as f:
|
||||
return f.read() == b"1"
|
||||
except (FileNotFoundError, NotADirectoryError, IsADirectoryError):
|
||||
return False
|
||||
|
||||
def get_int(self, key, block: bool = False) -> int:
|
||||
value = self.get(key, block=block)
|
||||
return int(value) if value else 0
|
||||
|
||||
def get_float(self, key, block: bool = False) -> float:
|
||||
value = self.get(key, block=block)
|
||||
return float(value) if value else 0.0
|
||||
|
||||
def put(self, key, dat):
|
||||
if isinstance(dat, datetime.datetime):
|
||||
dat = dat.isoformat()
|
||||
if isinstance(dat, (int, float)):
|
||||
# Params are strings on disk and half the fleet's writers spell numeric
|
||||
# puts as put(key, int). Letting that reach f.write() raises
|
||||
# "a bytes-like object is required" -- which, when the writer sits in a
|
||||
# connection's recv loop (hephaestusd's ping handler), tears down the
|
||||
# transport on the first server ping and flaps the device offline on a
|
||||
# timer. A params write must not be able to do that: coerce losslessly.
|
||||
dat = str(dat)
|
||||
if isinstance(dat, str):
|
||||
dat = dat.encode("utf-8")
|
||||
with self._lock:
|
||||
os.makedirs(self._d, exist_ok=True)
|
||||
p = self._p(key)
|
||||
tmp = p + ".tmp"
|
||||
with open(tmp, "wb") as f:
|
||||
f.write(dat)
|
||||
f.flush()
|
||||
os.fsync(f.fileno())
|
||||
os.rename(tmp, p)
|
||||
|
||||
def put_bool(self, key, val: bool):
|
||||
self.put(key, b"1" if val else b"0")
|
||||
|
||||
def put_int(self, key, val: int):
|
||||
self.put(key, str(val))
|
||||
|
||||
def put_float(self, key, val: float):
|
||||
self.put(key, str(val))
|
||||
|
||||
def put_nonblocking(self, key, dat):
|
||||
self.put(key, dat)
|
||||
|
||||
def put_bool_nonblocking(self, key, val: bool):
|
||||
self.put_bool(key, val)
|
||||
|
||||
def put_int_nonblocking(self, key, val: int):
|
||||
self.put_int(key, val)
|
||||
|
||||
def put_float_nonblocking(self, key, val: float):
|
||||
self.put_float(key, val)
|
||||
|
||||
def remove(self, key):
|
||||
try:
|
||||
os.remove(self._p(key))
|
||||
except FileNotFoundError:
|
||||
pass
|
||||
|
||||
def clear_all(self, tx_type=None):
|
||||
pass
|
||||
|
||||
def get_param_path(self, key: str = "") -> str:
|
||||
return self._p(key) if key else self._d
|
||||
|
||||
def all_keys(self):
|
||||
try:
|
||||
return [k.encode() for k in os.listdir(self._d)]
|
||||
except FileNotFoundError:
|
||||
return []
|
||||
|
||||
assert Params
|
||||
assert ParamKeyFlag
|
||||
assert ParamKeyType
|
||||
assert UnknownKeyName
|
||||
|
||||
if __name__ == "__main__":
|
||||
import sys
|
||||
|
||||
params = Params()
|
||||
key = sys.argv[1]
|
||||
assert params.check_key(key), f"unknown param: {key}"
|
||||
|
||||
if len(sys.argv) == 3:
|
||||
val = sys.argv[2]
|
||||
print(f"SET: {key} = {val}")
|
||||
params.put(key, val)
|
||||
elif len(sys.argv) == 2:
|
||||
print(f"GET: {key} = {params.get(key)}")
|
||||
BIN
iqpilot/common/params_pyx.so
Executable file
BIN
iqpilot/common/params_pyx.so
Executable file
Binary file not shown.
57
iqpilot/common/pid.py
Normal file
57
iqpilot/common/pid.py
Normal file
@@ -0,0 +1,57 @@
|
||||
import numpy as np
|
||||
from numbers import Number
|
||||
|
||||
class PIDController:
|
||||
def __init__(self, k_p, k_i, k_d=0., pos_limit=1e308, neg_limit=-1e308, rate=100):
|
||||
self._k_p: list[list[float]] = [[0], [k_p]] if isinstance(k_p, Number) else k_p
|
||||
self._k_i: list[list[float]] = [[0], [k_i]] if isinstance(k_i, Number) else k_i
|
||||
self._k_d: list[list[float]] = [[0], [k_d]] if isinstance(k_d, Number) else k_d
|
||||
|
||||
self.set_limits(pos_limit, neg_limit)
|
||||
|
||||
self.i_dt = 1.0 / rate
|
||||
self.speed = 0.0
|
||||
|
||||
self.reset()
|
||||
|
||||
@property
|
||||
def k_p(self):
|
||||
return np.interp(self.speed, self._k_p[0], self._k_p[1])
|
||||
|
||||
@property
|
||||
def k_i(self):
|
||||
return np.interp(self.speed, self._k_i[0], self._k_i[1])
|
||||
|
||||
@property
|
||||
def k_d(self):
|
||||
return np.interp(self.speed, self._k_d[0], self._k_d[1])
|
||||
|
||||
def reset(self):
|
||||
self.p = 0.0
|
||||
self.i = 0.0
|
||||
self.d = 0.0
|
||||
self.f = 0.0
|
||||
self.control = 0
|
||||
|
||||
def set_limits(self, pos_limit, neg_limit):
|
||||
self.pos_limit = pos_limit
|
||||
self.neg_limit = neg_limit
|
||||
|
||||
def update(self, error, error_rate=0.0, speed=0.0, feedforward=0., freeze_integrator=False):
|
||||
self.speed = speed
|
||||
self.p = self.k_p * float(error)
|
||||
self.d = self.k_d * error_rate
|
||||
self.f = feedforward
|
||||
|
||||
if not freeze_integrator:
|
||||
i = self.i + self.k_i * self.i_dt * error
|
||||
|
||||
# Don't allow windup if already clipping
|
||||
test_control = self.p + i + self.d + self.f
|
||||
i_upperbound = self.i if test_control > self.pos_limit else self.pos_limit
|
||||
i_lowerbound = self.i if test_control < self.neg_limit else self.neg_limit
|
||||
self.i = np.clip(i, i_lowerbound, i_upperbound)
|
||||
|
||||
control = self.p + self.i + self.d + self.f
|
||||
self.control = np.clip(control, self.neg_limit, self.pos_limit)
|
||||
return self.control
|
||||
66
iqpilot/common/prefix.py
Normal file
66
iqpilot/common/prefix.py
Normal file
@@ -0,0 +1,66 @@
|
||||
import os
|
||||
import platform
|
||||
import shutil
|
||||
import uuid
|
||||
|
||||
|
||||
from iqpilot.common.params import Params
|
||||
from iqpilot.system.hardware import PC
|
||||
from iqpilot.system.hardware.hw import Paths
|
||||
from iqpilot.system.hardware.hw import DEFAULT_DOWNLOAD_CACHE_ROOT
|
||||
|
||||
class OpenpilotPrefix:
|
||||
def __init__(self, prefix: str | None = None, create_dirs_on_enter: bool = True, clean_dirs_on_exit: bool = True, shared_download_cache: bool = False):
|
||||
self.prefix = prefix if prefix else str(uuid.uuid4().hex[0:15])
|
||||
shm_path = "/tmp" if platform.system() == "Darwin" else "/dev/shm"
|
||||
self.msgq_path = os.path.join(shm_path, "msgq_" + self.prefix)
|
||||
self.create_dirs_on_enter = create_dirs_on_enter
|
||||
self.clean_dirs_on_exit = clean_dirs_on_exit
|
||||
self.shared_download_cache = shared_download_cache
|
||||
|
||||
def __enter__(self):
|
||||
self.original_prefix = os.environ.get('OPENPILOT_PREFIX', None)
|
||||
os.environ['OPENPILOT_PREFIX'] = self.prefix
|
||||
|
||||
if self.create_dirs_on_enter:
|
||||
self.create_dirs()
|
||||
|
||||
if self.shared_download_cache:
|
||||
os.environ["COMMA_CACHE"] = DEFAULT_DOWNLOAD_CACHE_ROOT
|
||||
|
||||
return self
|
||||
|
||||
def __exit__(self, exc_type, exc_obj, exc_tb):
|
||||
if self.clean_dirs_on_exit:
|
||||
self.clean_dirs()
|
||||
try:
|
||||
del os.environ['OPENPILOT_PREFIX']
|
||||
if self.original_prefix is not None:
|
||||
os.environ['OPENPILOT_PREFIX'] = self.original_prefix
|
||||
except KeyError:
|
||||
pass
|
||||
return False
|
||||
|
||||
def create_dirs(self):
|
||||
try:
|
||||
os.mkdir(self.msgq_path)
|
||||
except FileExistsError:
|
||||
pass
|
||||
os.makedirs(Paths.log_root(), exist_ok=True)
|
||||
|
||||
def clean_dirs(self):
|
||||
symlink_path = Params().get_param_path()
|
||||
if os.path.islink(symlink_path):
|
||||
shutil.rmtree(os.path.realpath(symlink_path), ignore_errors=True)
|
||||
try:
|
||||
os.remove(symlink_path)
|
||||
except FileNotFoundError:
|
||||
pass
|
||||
else:
|
||||
shutil.rmtree(symlink_path, ignore_errors=True)
|
||||
shutil.rmtree(self.msgq_path, ignore_errors=True)
|
||||
if PC:
|
||||
shutil.rmtree(Paths.log_root(), ignore_errors=True)
|
||||
if not os.environ.get("COMMA_CACHE", False):
|
||||
shutil.rmtree(Paths.download_cache_root(), ignore_errors=True)
|
||||
shutil.rmtree(Paths.comma_home(), ignore_errors=True)
|
||||
40
iqpilot/common/pt2.py
Normal file
40
iqpilot/common/pt2.py
Normal file
@@ -0,0 +1,40 @@
|
||||
import math
|
||||
|
||||
|
||||
class PT2Filter:
|
||||
def __init__(self, w0: float, zeta: float, dt: float):
|
||||
self.w0 = w0
|
||||
self.zeta = zeta
|
||||
self.dt = dt
|
||||
self.a1, self.a2, self.b0, self.b1, self.b2 = self._design(w0, zeta, dt)
|
||||
self.y1 = 0.0
|
||||
self.y2 = 0.0
|
||||
self.u1 = 0.0
|
||||
self.u2 = 0.0
|
||||
|
||||
@staticmethod
|
||||
def _design(w0: float, zeta: float, dt: float):
|
||||
# bilinear transform of H(s) = w0^2 / (s^2 + 2*zeta*w0*s + w0^2)
|
||||
alpha = 2.0 / dt
|
||||
a2_den = alpha**2 + (2.0 * zeta * w0 * alpha) + w0**2
|
||||
a1_den = (-2.0 * alpha**2) + (2.0 * w0**2)
|
||||
a0_den = alpha**2 - (2.0 * zeta * w0 * alpha) + w0**2
|
||||
return (a1_den / a2_den, a0_den / a2_den,
|
||||
w0**2 / a2_den, 2.0 * w0**2 / a2_den, w0**2 / a2_den)
|
||||
|
||||
def reset(self, value: float = 0.0) -> None:
|
||||
self.y1 = value
|
||||
self.y2 = value
|
||||
self.u1 = value
|
||||
self.u2 = value
|
||||
|
||||
def update(self, u: float) -> float:
|
||||
y = (-self.a1 * self.y1) - (self.a2 * self.y2) + (self.b0 * u) + (self.b1 * self.u1) + (self.b2 * self.u2)
|
||||
self.y2 = self.y1
|
||||
self.y1 = y
|
||||
self.u2 = self.u1
|
||||
self.u1 = u
|
||||
return y
|
||||
|
||||
def steady_state_steps(self) -> int:
|
||||
return math.ceil((4.0 / (self.zeta * self.w0)) / self.dt)
|
||||
134
iqpilot/common/realtime.py
Normal file
134
iqpilot/common/realtime.py
Normal 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
|
||||
54
iqpilot/common/simple_kalman.py
Normal file
54
iqpilot/common/simple_kalman.py
Normal file
@@ -0,0 +1,54 @@
|
||||
import numpy as np
|
||||
|
||||
|
||||
def get_kalman_gain(dt, A, C, Q, R, iterations=100):
|
||||
P = np.zeros_like(Q)
|
||||
for _ in range(iterations):
|
||||
P = A.dot(P).dot(A.T) + dt * Q
|
||||
S = C.dot(P).dot(C.T) + R
|
||||
K = P.dot(C.T).dot(np.linalg.inv(S))
|
||||
P = (np.eye(len(P)) - K.dot(C)).dot(P)
|
||||
return K
|
||||
|
||||
|
||||
class KF1D:
|
||||
# this EKF assumes constant covariance matrix, so calculations are much simpler
|
||||
# the Kalman gain also needs to be precomputed using the control module
|
||||
|
||||
def __init__(self, x0, A, C, K):
|
||||
self.x0_0 = x0[0][0]
|
||||
self.x1_0 = x0[1][0]
|
||||
self.A0_0 = A[0][0]
|
||||
self.A0_1 = A[0][1]
|
||||
self.A1_0 = A[1][0]
|
||||
self.A1_1 = A[1][1]
|
||||
self.C0_0 = C[0]
|
||||
self.C0_1 = C[1]
|
||||
self.K0_0 = K[0][0]
|
||||
self.K1_0 = K[1][0]
|
||||
|
||||
self.A_K_0 = self.A0_0 - self.K0_0 * self.C0_0
|
||||
self.A_K_1 = self.A0_1 - self.K0_0 * self.C0_1
|
||||
self.A_K_2 = self.A1_0 - self.K1_0 * self.C0_0
|
||||
self.A_K_3 = self.A1_1 - self.K1_0 * self.C0_1
|
||||
|
||||
# K matrix needs to be pre-computed as follow:
|
||||
# import control
|
||||
# (x, l, K) = control.dare(np.transpose(self.A), np.transpose(self.C), Q, R)
|
||||
# self.K = np.transpose(K)
|
||||
|
||||
def update(self, meas):
|
||||
#self.x = np.dot(self.A_K, self.x) + np.dot(self.K, meas)
|
||||
x0_0 = self.A_K_0 * self.x0_0 + self.A_K_1 * self.x1_0 + self.K0_0 * meas
|
||||
x1_0 = self.A_K_2 * self.x0_0 + self.A_K_3 * self.x1_0 + self.K1_0 * meas
|
||||
self.x0_0 = x0_0
|
||||
self.x1_0 = x1_0
|
||||
return [self.x0_0, self.x1_0]
|
||||
|
||||
@property
|
||||
def x(self):
|
||||
return [[self.x0_0], [self.x1_0]]
|
||||
|
||||
def set_x(self, x):
|
||||
self.x0_0 = x[0][0]
|
||||
self.x1_0 = x[1][0]
|
||||
126
iqpilot/common/slc_utilities.py
Normal file
126
iqpilot/common/slc_utilities.py
Normal file
@@ -0,0 +1,126 @@
|
||||
import math
|
||||
import numpy as np
|
||||
|
||||
try:
|
||||
import requests
|
||||
except ImportError:
|
||||
requests = None
|
||||
|
||||
from iqpilot.common.slc_variables import EARTH_RADIUS
|
||||
|
||||
|
||||
def calculate_bearing_offset(latitude, longitude, current_bearing, distance):
|
||||
"""
|
||||
Calculate new GPS coordinates given a starting point, bearing, and distance.
|
||||
Used for Mapbox API lookahead calculations.
|
||||
|
||||
Args:
|
||||
latitude: Starting latitude in degrees
|
||||
longitude: Starting longitude in degrees
|
||||
current_bearing: Bearing in degrees (0-360)
|
||||
distance: Distance to project in meters
|
||||
|
||||
Returns:
|
||||
Tuple of (new_latitude, new_longitude) in degrees
|
||||
"""
|
||||
bearing = math.radians(current_bearing)
|
||||
lat_rad = math.radians(latitude)
|
||||
lon_rad = math.radians(longitude)
|
||||
|
||||
delta = distance / EARTH_RADIUS
|
||||
|
||||
new_lat = math.asin(math.sin(lat_rad) * math.cos(delta) + math.cos(lat_rad) * math.sin(delta) * math.cos(bearing))
|
||||
new_lon = lon_rad + math.atan2(math.sin(bearing) * math.sin(delta) * math.cos(lat_rad), math.cos(delta) - math.sin(lat_rad) * math.sin(new_lat))
|
||||
return math.degrees(new_lat), math.degrees(new_lon)
|
||||
|
||||
|
||||
def calculate_distance_to_point(lat1, lon1, lat2, lon2):
|
||||
"""
|
||||
Calculate the great circle distance between two GPS points using the Haversine formula.
|
||||
|
||||
Args:
|
||||
lat1, lon1: First point coordinates in degrees
|
||||
lat2, lon2: Second point coordinates in degrees
|
||||
|
||||
Returns:
|
||||
Distance in meters
|
||||
"""
|
||||
lat1_rad = math.radians(lat1)
|
||||
lon1_rad = math.radians(lon1)
|
||||
lat2_rad = math.radians(lat2)
|
||||
lon2_rad = math.radians(lon2)
|
||||
|
||||
delta_lat = lat2_rad - lat1_rad
|
||||
delta_lon = lon2_rad - lon1_rad
|
||||
|
||||
a = (math.sin(delta_lat / 2) ** 2) + math.cos(lat1_rad) * math.cos(lat2_rad) * (math.sin(delta_lon / 2) ** 2)
|
||||
c = 2 * math.atan2(math.sqrt(a), math.sqrt(1 - a))
|
||||
|
||||
return EARTH_RADIUS * c
|
||||
|
||||
|
||||
def calculate_lane_width(lane_line1, lane_line2, road_edge=None):
|
||||
"""
|
||||
Calculate the width of a lane based on lane line positions.
|
||||
Used for speed limit filler to determine road width.
|
||||
|
||||
Args:
|
||||
lane_line1: First lane line object with x, y coordinates
|
||||
lane_line2: Second lane line object with x, y coordinates
|
||||
road_edge: Optional road edge object with x, y coordinates
|
||||
|
||||
Returns:
|
||||
Lane width in meters
|
||||
"""
|
||||
lane_line1_x = np.asarray(lane_line1.x)
|
||||
lane_line1_y = np.asarray(lane_line1.y)
|
||||
|
||||
lane_line2_x = np.asarray(lane_line2.x)
|
||||
lane_line2_y = np.asarray(lane_line2.y)
|
||||
|
||||
lane_y_interp = np.interp(lane_line2_x, lane_line1_x, lane_line1_y)
|
||||
distance_to_lane = np.median(np.abs(lane_line2_y - lane_y_interp))
|
||||
|
||||
if road_edge is None:
|
||||
return distance_to_lane
|
||||
|
||||
road_edge_x = np.asarray(road_edge.x)
|
||||
road_edge_y = np.asarray(road_edge.y)
|
||||
|
||||
edge_y_interp = np.interp(lane_line2_x, road_edge_x, road_edge_y)
|
||||
distance_to_edge = np.median(np.abs(lane_line2_y - edge_y_interp))
|
||||
|
||||
return max(distance_to_lane, distance_to_edge)
|
||||
|
||||
|
||||
def is_url_pingable(url):
|
||||
"""
|
||||
Check if a URL is accessible and responding.
|
||||
Used to verify Mapbox/Overpass API availability before making requests.
|
||||
|
||||
Args:
|
||||
url: URL to ping
|
||||
|
||||
Returns:
|
||||
Boolean indicating if URL is accessible
|
||||
"""
|
||||
if not url:
|
||||
return False
|
||||
|
||||
if requests is None:
|
||||
return False
|
||||
|
||||
if not hasattr(is_url_pingable, "session"):
|
||||
is_url_pingable.session = requests.Session()
|
||||
is_url_pingable.session.headers.update({"User-Agent": "iqpilot-ping-test/1.0"})
|
||||
|
||||
try:
|
||||
response = is_url_pingable.session.head(url, timeout=10, allow_redirects=True)
|
||||
if response.status_code in (405, 501):
|
||||
response = is_url_pingable.session.get(url, timeout=10, allow_redirects=True, stream=True)
|
||||
|
||||
is_accessible = response.ok
|
||||
response.close()
|
||||
return is_accessible
|
||||
except Exception:
|
||||
return False
|
||||
35
iqpilot/common/slc_variables.py
Normal file
35
iqpilot/common/slc_variables.py
Normal file
@@ -0,0 +1,35 @@
|
||||
# Earth radius in meters (for GPS calculations)
|
||||
EARTH_RADIUS = 6378137
|
||||
|
||||
# Mapbox API limits
|
||||
FREE_MAPBOX_REQUESTS = 100_000
|
||||
|
||||
# Speed limit offset zones for different unit systems
|
||||
# Each entry is (min_speed_ms, max_speed_ms, param_name); the param value is a
|
||||
# percent offset applied to the resolved limit (e.g. 10 -> +10%), lower bound inclusive
|
||||
|
||||
OFFSET_PERCENT_MAX = 50.0
|
||||
|
||||
OFFSET_MAP_IMPERIAL = [
|
||||
(0, 8.94, "speed_limit_offset1"), # 0-20 mph
|
||||
(8.94, 17.88, "speed_limit_offset2"), # 20-40 mph
|
||||
(17.88, float("inf"), "speed_limit_offset3"), # 40+ mph
|
||||
]
|
||||
|
||||
OFFSET_MAP_METRIC = [
|
||||
(0, 8.33, "speed_limit_offset1"), # 0-30 km/h
|
||||
(8.33, 16.67, "speed_limit_offset2"), # 30-60 km/h
|
||||
(16.67, float("inf"), "speed_limit_offset3"), # 60+ km/h
|
||||
]
|
||||
|
||||
# Speed limit filler constants
|
||||
BOUNDING_BOX_RADIUS_DEGREE = 0.1
|
||||
MAX_ENTRIES = 1_000_000
|
||||
MAX_OVERPASS_DATA_BYTES = 1_073_741_824
|
||||
MAX_OVERPASS_REQUESTS = 10_000
|
||||
METERS_PER_DEG_LAT = 111_320
|
||||
VETTING_INTERVAL_DAYS = 7
|
||||
|
||||
# Overpass API URLs
|
||||
OVERPASS_API_URL = "https://overpass-api.de/api/interpreter"
|
||||
OVERPASS_STATUS_URL = "https://overpass-api.de/api/status"
|
||||
20
iqpilot/common/speed_assist_tiers.py
Normal file
20
iqpilot/common/speed_assist_tiers.py
Normal file
@@ -0,0 +1,20 @@
|
||||
"""
|
||||
Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
|
||||
|
||||
Engagement tiers for the speed-assist feature. A tier is persisted as an integer
|
||||
under the "IQSpeedAssistMode" param; the ordinal IS the stored value and must remain
|
||||
stable (0..3), ordered by how much the tier is allowed to intervene.
|
||||
"""
|
||||
from enum import IntEnum
|
||||
|
||||
STORE_KEY = "IQSpeedAssistMode"
|
||||
|
||||
# none -> just display the limit -> highlight overspeed -> move the set speed
|
||||
SpeedAssistTier = IntEnum("SpeedAssistTier", "DISABLED ADVISORY ALERTING ACTUATING", start=0)
|
||||
|
||||
DEFAULT_TIER = SpeedAssistTier.ADVISORY
|
||||
|
||||
|
||||
def actuates_speed(tier) -> bool:
|
||||
"""Only the top tier is permitted to drive the cruise set speed."""
|
||||
return int(tier) == SpeedAssistTier.ACTUATING
|
||||
52
iqpilot/common/spinner.py
Executable file
52
iqpilot/common/spinner.py
Executable 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)
|
||||
73
iqpilot/common/stat_live.py
Normal file
73
iqpilot/common/stat_live.py
Normal file
@@ -0,0 +1,73 @@
|
||||
import numpy as np
|
||||
|
||||
class RunningStat:
|
||||
# tracks realtime mean and standard deviation without storing any data
|
||||
def __init__(self, priors=None, max_trackable=-1):
|
||||
self.max_trackable = max_trackable
|
||||
if priors is not None:
|
||||
# initialize from history
|
||||
self.M = priors[0]
|
||||
self.S = priors[1]
|
||||
self.n = priors[2]
|
||||
self.M_last = self.M
|
||||
self.S_last = self.S
|
||||
|
||||
else:
|
||||
self.reset()
|
||||
|
||||
def reset(self):
|
||||
self.M = 0.
|
||||
self.S = 0.
|
||||
self.M_last = 0.
|
||||
self.S_last = 0.
|
||||
self.n = 0
|
||||
|
||||
def push_data(self, new_data):
|
||||
# short term memory hack
|
||||
if self.max_trackable < 0 or self.n < self.max_trackable:
|
||||
self.n += 1
|
||||
if self.n == 0:
|
||||
self.M_last = new_data
|
||||
self.M = self.M_last
|
||||
self.S_last = 0.
|
||||
else:
|
||||
self.M = self.M_last + (new_data - self.M_last) / self.n
|
||||
self.S = self.S_last + (new_data - self.M_last) * (new_data - self.M)
|
||||
self.M_last = self.M
|
||||
self.S_last = self.S
|
||||
|
||||
def mean(self):
|
||||
return self.M
|
||||
|
||||
def variance(self):
|
||||
if self.n >= 2:
|
||||
return self.S / (self.n - 1.)
|
||||
else:
|
||||
return 0
|
||||
|
||||
def std(self):
|
||||
return np.sqrt(self.variance())
|
||||
|
||||
def params_to_save(self):
|
||||
return [self.M, self.S, self.n]
|
||||
|
||||
class RunningStatFilter:
|
||||
def __init__(self, raw_priors=None, filtered_priors=None, max_trackable=-1):
|
||||
self.raw_stat = RunningStat(raw_priors, -1)
|
||||
self.filtered_stat = RunningStat(filtered_priors, max_trackable)
|
||||
|
||||
def reset(self):
|
||||
self.raw_stat.reset()
|
||||
self.filtered_stat.reset()
|
||||
|
||||
def push_and_update(self, new_data):
|
||||
_std_last = self.raw_stat.std()
|
||||
self.raw_stat.push_data(new_data)
|
||||
_delta_std = self.raw_stat.std() - _std_last
|
||||
if _delta_std <= 0:
|
||||
self.filtered_stat.push_data(new_data)
|
||||
else:
|
||||
pass
|
||||
# self.filtered_stat.push_data(self.filtered_stat.mean())
|
||||
|
||||
# class SequentialBayesian():
|
||||
60
iqpilot/common/steer_delay.py
Normal file
60
iqpilot/common/steer_delay.py
Normal 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
iqpilot/common/swaglog.py
Normal file
165
iqpilot/common/swaglog.py
Normal 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)
|
||||
1
iqpilot/common/tests/.gitignore
vendored
Normal file
1
iqpilot/common/tests/.gitignore
vendored
Normal file
@@ -0,0 +1 @@
|
||||
test_common
|
||||
0
iqpilot/common/tests/__init__.py
Normal file
0
iqpilot/common/tests/__init__.py
Normal file
19
iqpilot/common/tests/test_file_helpers.py
Normal file
19
iqpilot/common/tests/test_file_helpers.py
Normal file
@@ -0,0 +1,19 @@
|
||||
import os
|
||||
from uuid import uuid4
|
||||
|
||||
from iqpilot.common.utils import atomic_write
|
||||
|
||||
|
||||
class TestFileHelpers:
|
||||
def run_atomic_write_func(self, atomic_write_func):
|
||||
path = f"/tmp/tmp{uuid4()}"
|
||||
with atomic_write_func(path) as f:
|
||||
f.write("test")
|
||||
assert not os.path.exists(path)
|
||||
|
||||
with open(path) as f:
|
||||
assert f.read() == "test"
|
||||
os.remove(path)
|
||||
|
||||
def test_atomic_write(self):
|
||||
self.run_atomic_write_func(atomic_write)
|
||||
15
iqpilot/common/tests/test_markdown.py
Normal file
15
iqpilot/common/tests/test_markdown.py
Normal file
@@ -0,0 +1,15 @@
|
||||
import os
|
||||
|
||||
from iqpilot.common.basedir import BASEDIR
|
||||
from iqpilot.common.markdown import parse_markdown
|
||||
|
||||
|
||||
class TestMarkdown:
|
||||
def test_all_release_notes(self):
|
||||
with open(os.path.join(BASEDIR, "iqpilot", "docs", "CHANGELOG.md")) as f:
|
||||
release_notes = f.read().split("\n\n")
|
||||
assert len(release_notes) > 10
|
||||
|
||||
for rn in release_notes:
|
||||
md = parse_markdown(rn)
|
||||
assert len(md) > 0
|
||||
145
iqpilot/common/tests/test_params.py
Normal file
145
iqpilot/common/tests/test_params.py
Normal file
@@ -0,0 +1,145 @@
|
||||
import pytest
|
||||
import datetime
|
||||
import os
|
||||
import threading
|
||||
import time
|
||||
import uuid
|
||||
|
||||
from iqpilot.common.params import Params, ParamKeyFlag, UnknownKeyName
|
||||
|
||||
class TestParams:
|
||||
def setup_method(self):
|
||||
self.params = Params()
|
||||
|
||||
def test_params_put_and_get(self):
|
||||
self.params.put("DongleId", "cb38263377b873ee")
|
||||
assert self.params.get("DongleId") == "cb38263377b873ee"
|
||||
|
||||
def test_params_non_ascii(self):
|
||||
st = b"\xe1\x90\xff"
|
||||
self.params.put("CarParams", st)
|
||||
assert self.params.get("CarParams") == st
|
||||
|
||||
def test_params_get_cleared_manager_start(self):
|
||||
self.params.put("CarParams", b"test")
|
||||
self.params.put("DongleId", "cb38263377b873ee")
|
||||
assert self.params.get("CarParams") == b"test"
|
||||
|
||||
undefined_param = self.params.get_param_path(uuid.uuid4().hex)
|
||||
with open(undefined_param, "w") as f:
|
||||
f.write("test")
|
||||
assert os.path.isfile(undefined_param)
|
||||
|
||||
self.params.clear_all(ParamKeyFlag.CLEAR_ON_MANAGER_START)
|
||||
assert self.params.get("CarParams") is None
|
||||
assert self.params.get("DongleId") is not None
|
||||
assert not os.path.isfile(undefined_param)
|
||||
|
||||
def test_params_two_things(self):
|
||||
self.params.put("DongleId", "bob")
|
||||
self.params.put("AthenadPid", 123)
|
||||
assert self.params.get("DongleId") == "bob"
|
||||
assert self.params.get("AthenadPid") == 123
|
||||
|
||||
def test_params_get_block(self):
|
||||
def _delayed_writer():
|
||||
time.sleep(0.1)
|
||||
self.params.put("CarParams", b"test")
|
||||
threading.Thread(target=_delayed_writer).start()
|
||||
assert self.params.get("CarParams") is None
|
||||
assert self.params.get("CarParams", block=True) == b"test"
|
||||
|
||||
def test_params_unknown_key_fails(self):
|
||||
with pytest.raises(UnknownKeyName):
|
||||
self.params.get("swag")
|
||||
|
||||
with pytest.raises(UnknownKeyName):
|
||||
self.params.get_bool("swag")
|
||||
|
||||
with pytest.raises(UnknownKeyName):
|
||||
self.params.put("swag", "abc")
|
||||
|
||||
with pytest.raises(UnknownKeyName):
|
||||
self.params.put_bool("swag", True)
|
||||
|
||||
def test_remove_not_there(self):
|
||||
assert self.params.get("CarParams") is None
|
||||
self.params.remove("CarParams")
|
||||
assert self.params.get("CarParams") is None
|
||||
|
||||
def test_get_bool(self):
|
||||
self.params.remove("IsMetric")
|
||||
assert not self.params.get_bool("IsMetric")
|
||||
|
||||
self.params.put_bool("IsMetric", True)
|
||||
assert self.params.get_bool("IsMetric")
|
||||
|
||||
self.params.put_bool("IsMetric", False)
|
||||
assert not self.params.get_bool("IsMetric")
|
||||
|
||||
self.params.put("IsMetric", True)
|
||||
assert self.params.get_bool("IsMetric")
|
||||
|
||||
self.params.put("IsMetric", False)
|
||||
assert not self.params.get_bool("IsMetric")
|
||||
|
||||
def test_navigation_disabled_default(self):
|
||||
self.params.remove("NavigationEnabled")
|
||||
assert not self.params.get_bool("NavigationEnabled")
|
||||
|
||||
def test_put_non_blocking_with_get_block(self):
|
||||
q = Params()
|
||||
def _delayed_writer():
|
||||
time.sleep(0.1)
|
||||
Params().put_nonblocking("CarParams", b"test")
|
||||
threading.Thread(target=_delayed_writer).start()
|
||||
assert q.get("CarParams") is None
|
||||
assert q.get("CarParams", True) == b"test"
|
||||
|
||||
def test_put_bool_non_blocking_with_get_block(self):
|
||||
q = Params()
|
||||
def _delayed_writer():
|
||||
time.sleep(0.1)
|
||||
Params().put_bool_nonblocking("CarParams", True)
|
||||
threading.Thread(target=_delayed_writer).start()
|
||||
assert q.get("CarParams") is None
|
||||
assert q.get("CarParams", True) == b"1"
|
||||
|
||||
def test_params_all_keys(self):
|
||||
keys = Params().all_keys()
|
||||
|
||||
# sanity checks
|
||||
assert len(keys) > 20
|
||||
assert len(keys) == len(set(keys))
|
||||
assert b"CarParams" in keys
|
||||
|
||||
def test_params_default_value(self):
|
||||
self.params.remove("LanguageSetting")
|
||||
self.params.remove("LongitudinalPersonality")
|
||||
self.params.remove("LiveParameters")
|
||||
|
||||
assert self.params.get("LanguageSetting") is None
|
||||
assert self.params.get("LanguageSetting", return_default=False) is None
|
||||
assert isinstance(self.params.get("LanguageSetting", return_default=True), str)
|
||||
assert isinstance(self.params.get("LongitudinalPersonality", return_default=True), int)
|
||||
assert self.params.get("LiveParameters") is None
|
||||
assert self.params.get("LiveParameters", return_default=True) is None
|
||||
|
||||
def test_params_get_type(self):
|
||||
# json
|
||||
self.params.put("ApiCache_FirehoseStats", {"a": 0})
|
||||
assert self.params.get("ApiCache_FirehoseStats") == {"a": 0}
|
||||
|
||||
# int
|
||||
self.params.put("BootCount", 1441)
|
||||
assert self.params.get("BootCount") == 1441
|
||||
|
||||
# bool
|
||||
self.params.put("AdbEnabled", True)
|
||||
assert self.params.get("AdbEnabled")
|
||||
assert isinstance(self.params.get("AdbEnabled"), bool)
|
||||
|
||||
# time
|
||||
now = datetime.datetime.now(datetime.UTC)
|
||||
self.params.put("InstallDate", now)
|
||||
assert self.params.get("InstallDate") == now
|
||||
64
iqpilot/common/tests/test_realtime.py
Normal file
64
iqpilot/common/tests/test_realtime.py
Normal file
@@ -0,0 +1,64 @@
|
||||
#!/usr/bin/env python3
|
||||
import pytest
|
||||
|
||||
from iqpilot.common.realtime import config_background_thread, Ratekeeper
|
||||
|
||||
|
||||
class MonotonicClock:
|
||||
def __init__(self) -> None:
|
||||
self.now = 0.
|
||||
|
||||
def advance(self, seconds: float) -> None:
|
||||
self.now += seconds
|
||||
|
||||
def __call__(self) -> float:
|
||||
return self.now
|
||||
|
||||
|
||||
def test_ratekeeper_reset_discards_accumulated_lag(monkeypatch):
|
||||
clock = MonotonicClock()
|
||||
monkeypatch.setattr("iqpilot.common.realtime.time.monotonic", clock)
|
||||
rk = Ratekeeper(100)
|
||||
|
||||
rk.monitor_time()
|
||||
clock.advance(0.075)
|
||||
rk.monitor_time()
|
||||
assert rk.remaining == pytest.approx(-0.055)
|
||||
assert rk.lag == pytest.approx(0.055)
|
||||
|
||||
rk.reset()
|
||||
assert rk.remaining == 0.
|
||||
assert rk.lag == 0.
|
||||
|
||||
rk.monitor_time()
|
||||
assert rk.remaining == pytest.approx(0.01)
|
||||
assert rk.lag == 0.
|
||||
|
||||
|
||||
def test_ratekeeper_reset_preserves_frame_count(monkeypatch):
|
||||
clock = MonotonicClock()
|
||||
monkeypatch.setattr("iqpilot.common.realtime.time.monotonic", clock)
|
||||
rk = Ratekeeper(100)
|
||||
|
||||
rk.monitor_time()
|
||||
clock.advance(0.01)
|
||||
rk.monitor_time()
|
||||
frame = rk.frame
|
||||
|
||||
rk.reset()
|
||||
assert rk.frame == frame
|
||||
|
||||
|
||||
def test_config_background_thread_restores_normal_scheduling(monkeypatch):
|
||||
calls = []
|
||||
monkeypatch.setattr("iqpilot.common.realtime.sys.platform", "linux")
|
||||
monkeypatch.setattr("iqpilot.common.realtime.PC", False)
|
||||
monkeypatch.setattr("iqpilot.common.realtime.os.cpu_count", lambda: 8)
|
||||
monkeypatch.setattr("iqpilot.common.realtime.os.SCHED_OTHER", 0, raising=False)
|
||||
monkeypatch.setattr("iqpilot.common.realtime.os.sched_param", lambda priority: priority, raising=False)
|
||||
monkeypatch.setattr("iqpilot.common.realtime.os.sched_setscheduler", lambda pid, policy, param: calls.append((pid, policy, param)), raising=False)
|
||||
monkeypatch.setattr("iqpilot.common.realtime.os.sched_setaffinity", lambda pid, cores: calls.append((pid, set(cores))), raising=False)
|
||||
|
||||
config_background_thread()
|
||||
|
||||
assert calls == [(0, 0, 0), (0, set(range(8)))]
|
||||
29
iqpilot/common/tests/test_simple_kalman.py
Normal file
29
iqpilot/common/tests/test_simple_kalman.py
Normal file
@@ -0,0 +1,29 @@
|
||||
from iqpilot.common.simple_kalman import KF1D
|
||||
|
||||
|
||||
class TestSimpleKalman:
|
||||
def setup_method(self):
|
||||
dt = 0.01
|
||||
x0_0 = 0.0
|
||||
x1_0 = 0.0
|
||||
A0_0 = 1.0
|
||||
A0_1 = dt
|
||||
A1_0 = 0.0
|
||||
A1_1 = 1.0
|
||||
C0_0 = 1.0
|
||||
C0_1 = 0.0
|
||||
K0_0 = 0.12287673
|
||||
K1_0 = 0.29666309
|
||||
|
||||
self.kf = KF1D(x0=[[x0_0], [x1_0]],
|
||||
A=[[A0_0, A0_1], [A1_0, A1_1]],
|
||||
C=[C0_0, C0_1],
|
||||
K=[[K0_0], [K1_0]])
|
||||
|
||||
def test_getter_setter(self):
|
||||
self.kf.set_x([[1.0], [1.0]])
|
||||
assert self.kf.x == [[1.0], [1.0]]
|
||||
|
||||
def test_update_returns_state(self):
|
||||
x = self.kf.update(100)
|
||||
assert x == [i[0] for i in self.kf.x]
|
||||
91
iqpilot/common/tests/test_steer_delay.py
Normal file
91
iqpilot/common/tests/test_steer_delay.py
Normal file
@@ -0,0 +1,91 @@
|
||||
"""
|
||||
Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
|
||||
"""
|
||||
import time
|
||||
|
||||
import pytest
|
||||
|
||||
import iqpilot.cereal.messaging as messaging
|
||||
from iqpilot.cereal import car
|
||||
from iqpilot.common.params import Params
|
||||
from iqpilot.common.steer_delay import (
|
||||
SteerDelayPublisher,
|
||||
cached_steer_delay,
|
||||
fixed_steer_delay,
|
||||
lateral_action_delay,
|
||||
resolve_steer_delay,
|
||||
)
|
||||
|
||||
ANGLE = car.CarParams.SteerControlType.angle
|
||||
TORQUE = car.CarParams.SteerControlType.torque
|
||||
|
||||
LIVE_DELAY = 0.4387
|
||||
RACK_DELAY = 0.10
|
||||
OFFSET = 0.05
|
||||
|
||||
|
||||
@pytest.fixture
|
||||
def params(tmp_path, monkeypatch):
|
||||
monkeypatch.setenv("PARAMS_ROOT", str(tmp_path))
|
||||
p = Params()
|
||||
p.put("IQSteerDelayCache", LIVE_DELAY)
|
||||
p.put("IQSoftwareSteerDelay", OFFSET)
|
||||
return p
|
||||
|
||||
|
||||
def _car_params(steer_control_type):
|
||||
cp = car.CarParams.new_message()
|
||||
cp.steerControlType = steer_control_type
|
||||
cp.steerActuatorDelay = RACK_DELAY
|
||||
return cp
|
||||
|
||||
|
||||
def _lateral_delay_msg(value):
|
||||
msg = messaging.new_message("lateralDelay")
|
||||
msg.lateralDelay.lateralDelay = value
|
||||
return msg.as_reader()
|
||||
|
||||
|
||||
def test_params_fixture_is_isolated_from_the_real_device(params, tmp_path):
|
||||
assert str(tmp_path) in params.get_param_path("")
|
||||
|
||||
|
||||
@pytest.mark.parametrize("live_enabled", [True, False])
|
||||
def test_torque_cars_always_use_live_delay(params, live_enabled):
|
||||
params.put_bool("IQLiveSteerDelay", live_enabled)
|
||||
assert lateral_action_delay(params, _car_params(TORQUE), LIVE_DELAY) == pytest.approx(LIVE_DELAY)
|
||||
|
||||
|
||||
def test_angle_cars_ignore_live_delay_when_self_tuning_is_off(params):
|
||||
params.put_bool("IQLiveSteerDelay", False)
|
||||
delay = lateral_action_delay(params, _car_params(ANGLE), LIVE_DELAY)
|
||||
assert delay == pytest.approx(RACK_DELAY + OFFSET)
|
||||
assert delay != pytest.approx(LIVE_DELAY)
|
||||
|
||||
|
||||
def test_angle_cars_use_cached_delay_when_self_tuning_is_on(params):
|
||||
params.put_bool("IQLiveSteerDelay", True)
|
||||
assert lateral_action_delay(params, _car_params(ANGLE), LIVE_DELAY) == pytest.approx(LIVE_DELAY)
|
||||
|
||||
|
||||
@pytest.mark.parametrize("offset", [0.05, 0.20, 0.50])
|
||||
def test_manual_offset_reaches_the_path_and_matches_what_the_ui_reports(params, offset):
|
||||
params.put_bool("IQLiveSteerDelay", False)
|
||||
params.put("IQSoftwareSteerDelay", offset)
|
||||
ui_total = RACK_DELAY + offset
|
||||
assert fixed_steer_delay(params, RACK_DELAY) == pytest.approx(ui_total)
|
||||
assert lateral_action_delay(params, _car_params(ANGLE), LIVE_DELAY) == pytest.approx(ui_total)
|
||||
|
||||
|
||||
@pytest.mark.parametrize("live_enabled", [False, True])
|
||||
def test_publisher_writes_the_value_the_resolver_reads(params, live_enabled):
|
||||
params.put_bool("IQLiveSteerDelay", live_enabled)
|
||||
params.put("IQSteerDelayCache", -1.0)
|
||||
SteerDelayPublisher(_car_params(ANGLE)).update(_lateral_delay_msg(LIVE_DELAY))
|
||||
|
||||
expected = LIVE_DELAY if live_enabled else RACK_DELAY + OFFSET
|
||||
deadline = time.monotonic() + 5.0
|
||||
while cached_steer_delay() != pytest.approx(expected) and time.monotonic() < deadline:
|
||||
time.sleep(0.01)
|
||||
assert cached_steer_delay() == pytest.approx(expected)
|
||||
assert resolve_steer_delay(params, RACK_DELAY) == pytest.approx(expected)
|
||||
63
iqpilot/common/text_window.py
Executable file
63
iqpilot/common/text_window.py
Executable file
@@ -0,0 +1,63 @@
|
||||
#!/usr/bin/env python3
|
||||
import os
|
||||
import time
|
||||
import subprocess
|
||||
from iqpilot.common.basedir import BASEDIR
|
||||
|
||||
|
||||
class TextWindow:
|
||||
def __init__(self, text):
|
||||
try:
|
||||
self.text_proc = subprocess.Popen(["./text.py", text],
|
||||
stdin=subprocess.PIPE,
|
||||
cwd=os.path.join(BASEDIR, "iqpilot", "system", "ui"),
|
||||
close_fds=True)
|
||||
except OSError:
|
||||
self.text_proc = None
|
||||
|
||||
def get_status(self):
|
||||
if self.text_proc is not None:
|
||||
self.text_proc.poll()
|
||||
return self.text_proc.returncode
|
||||
return None
|
||||
|
||||
def __enter__(self):
|
||||
return self
|
||||
|
||||
def close(self):
|
||||
if self.text_proc is not None:
|
||||
self.text_proc.terminate()
|
||||
self.text_proc = None
|
||||
|
||||
def wait_for_exit(self):
|
||||
if self.text_proc is not None:
|
||||
while True:
|
||||
if self.get_status() == 1:
|
||||
return
|
||||
time.sleep(0.1)
|
||||
|
||||
def __del__(self):
|
||||
self.close()
|
||||
|
||||
def __exit__(self, exc_type, exc_value, traceback):
|
||||
self.close()
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
text = """Traceback (most recent call last):
|
||||
File "./controlsd.py", line 608, in <module>
|
||||
main()
|
||||
File "./controlsd.py", line 604, in main
|
||||
controlsd_thread(sm, pm, logcan)
|
||||
File "./controlsd.py", line 455, in controlsd_thread
|
||||
1/0
|
||||
ZeroDivisionError: division by zero"""
|
||||
print(text)
|
||||
|
||||
with TextWindow(text) as s:
|
||||
for _ in range(100):
|
||||
if s.get_status() == 1:
|
||||
print("Got exit button")
|
||||
break
|
||||
time.sleep(0.1)
|
||||
print("gone")
|
||||
15
iqpilot/common/time_helpers.py
Normal file
15
iqpilot/common/time_helpers.py
Normal file
@@ -0,0 +1,15 @@
|
||||
import datetime
|
||||
from pathlib import Path
|
||||
|
||||
MIN_DATE = datetime.datetime(year=2025, month=2, day=21)
|
||||
|
||||
def min_date():
|
||||
# on systemd systems, the default time is the systemd build time
|
||||
systemd_path = Path("/lib/systemd/systemd")
|
||||
if systemd_path.exists():
|
||||
d = datetime.datetime.fromtimestamp(systemd_path.stat().st_mtime)
|
||||
return max(MIN_DATE, d + datetime.timedelta(days=1))
|
||||
return MIN_DATE
|
||||
|
||||
def system_time_valid():
|
||||
return datetime.datetime.now() > min_date()
|
||||
27
iqpilot/common/timeout.py
Normal file
27
iqpilot/common/timeout.py
Normal file
@@ -0,0 +1,27 @@
|
||||
import signal
|
||||
|
||||
class TimeoutException(Exception):
|
||||
pass
|
||||
|
||||
class Timeout:
|
||||
"""
|
||||
Timeout context manager.
|
||||
For example this code will raise a TimeoutException:
|
||||
with Timeout(seconds=5, error_msg="Sleep was too long"):
|
||||
time.sleep(10)
|
||||
"""
|
||||
def __init__(self, seconds, error_msg=None):
|
||||
if error_msg is None:
|
||||
error_msg = f'Timed out after {seconds} seconds'
|
||||
self.seconds = seconds
|
||||
self.error_msg = error_msg
|
||||
|
||||
def handle_timeout(self, signume, frame):
|
||||
raise TimeoutException(self.error_msg)
|
||||
|
||||
def __enter__(self):
|
||||
signal.signal(signal.SIGALRM, self.handle_timeout)
|
||||
signal.alarm(self.seconds)
|
||||
|
||||
def __exit__(self, exc_type, exc_val, exc_tb):
|
||||
signal.alarm(0)
|
||||
2
iqpilot/common/transformations/.gitignore
vendored
Normal file
2
iqpilot/common/transformations/.gitignore
vendored
Normal file
@@ -0,0 +1,2 @@
|
||||
transformations
|
||||
transformations.cpp
|
||||
70
iqpilot/common/transformations/README.md
Normal file
70
iqpilot/common/transformations/README.md
Normal file
@@ -0,0 +1,70 @@
|
||||
|
||||
Reference Frames
|
||||
------
|
||||
Many reference frames are used throughout. This
|
||||
folder contains all helper functions needed to
|
||||
transform between them. Generally this is done
|
||||
by generating a rotation matrix and multiplying.
|
||||
|
||||
|
||||
| Name | [x, y, z] | Units | Notes |
|
||||
| :-------------: |:-------------:| :-----:| :----: |
|
||||
| Geodetic | [Latitude, Longitude, Altitude] | geodetic coordinates | Sometimes used as [lon, lat, alt], avoid this frame. |
|
||||
| ECEF | [x, y, z] | meters | We use **ITRF14 (IGS14)**, NOT NAD83. <br> This is the global Mesh3D frame. |
|
||||
| NED | [North, East, Down] | meters | Relative to earth's surface, useful for visualizing. |
|
||||
| Device | [Forward, Right, Down] | meters | This is the Mesh3D local frame. <br> Relative to camera, **not imu.** <br> |
|
||||
| Calibrated | [Forward, Right, Down] | meters | This is the frame the model outputs are in. <br> More details below. <br>|
|
||||
| Car | [Forward, Right, Down] | meters | This is useful for estimating position of points on the road. <br> More details below. <br>|
|
||||
| View | [Right, Down, Forward] | meters | Like device frame, but according to camera conventions. |
|
||||
| Camera | [u, v, focal] | pixels | Like view frame, but 2d on the camera image.|
|
||||
| Normalized Camera | [u / focal, v / focal, 1] | / | |
|
||||
| Model | [u, v, focal] | pixels | The sampled rectangle of the full camera frame the model uses. |
|
||||
| Normalized Model | [u / focal, v / focal, 1] | / | |
|
||||
|
||||
|
||||
|
||||
|
||||
Orientation Conventions
|
||||
------
|
||||
Quaternions, rotation matrices and euler angles are three
|
||||
equivalent representations of orientation and all three are
|
||||
used throughout the code base.
|
||||
|
||||
For euler angles the preferred convention is [roll, pitch, yaw]
|
||||
which corresponds to rotations around the [x, y, z] axes. All
|
||||
euler angles should always be in radians or radians/s unless
|
||||
for plotting or display purposes. For quaternions the hamilton
|
||||
notations is preferred which is [q<sub>w</sub>, q<sub>x</sub>, q<sub>y</sub>, q<sub>z</sub>]. All quaternions
|
||||
should always be normalized with a strictly positive q<sub>w</sub>. **These
|
||||
quaternions are a unique representation of orientation whereas euler angles
|
||||
or rotation matrices are not.**
|
||||
|
||||
To rotate from one frame into another with euler angles the
|
||||
convention is to rotate around roll, then pitch and then yaw,
|
||||
while rotating around the rotated axes, not the original axes.
|
||||
|
||||
|
||||
Car frame
|
||||
------
|
||||
Device frame is aligned with the road-facing camera used by openpilot. However, when controlling the vehicle it is helpful to think in a reference frame aligned with the vehicle. These two reference frames can be different.
|
||||
|
||||
The orientation of car frame is defined to be aligned with the car's direction of travel and the road plane when the vehicle is driving on a flat road and not turning. The origin of car frame is defined to be directly below device frame (in car frame), such that it is on the road plane. The position and orientation of this frame is not necessarily always aligned with the direction of travel or the road plane due to suspension movements and other effects.
|
||||
|
||||
|
||||
Calibrated frame
|
||||
------
|
||||
It is helpful for openpilot's driving model to take in images that look similar when mounted differently in different cars. To achieve this we "calibrate" the images by transforming it into calibrated frame. Calibrated frame is defined to be aligned with car frame in pitch and yaw, and aligned with device frame in roll. It also has the same origin as device frame.
|
||||
|
||||
|
||||
Example
|
||||
------
|
||||
To transform global Mesh3D positions and orientations (positions_ecef, quats_ecef) into the local frame described by the
|
||||
first position and orientation from Mesh3D one would do:
|
||||
```
|
||||
ecef_from_local = rot_from_quat(quats_ecef[0])
|
||||
local_from_ecef = ecef_from_local.T
|
||||
positions_local = np.einsum('ij,kj->ki', local_from_ecef, postions_ecef - positions_ecef[0])
|
||||
rotations_global = rot_from_quat(quats_ecef)
|
||||
rotations_local = np.einsum('ij,kjl->kil', local_from_ecef, rotations_global)
|
||||
eulers_local = euler_from_rot(rotations_local)
|
||||
```
|
||||
0
iqpilot/common/transformations/__init__.py
Normal file
0
iqpilot/common/transformations/__init__.py
Normal file
179
iqpilot/common/transformations/camera.py
Normal file
179
iqpilot/common/transformations/camera.py
Normal file
@@ -0,0 +1,179 @@
|
||||
import itertools
|
||||
import numpy as np
|
||||
from dataclasses import dataclass
|
||||
|
||||
import iqpilot.common.transformations.orientation as orient
|
||||
|
||||
## -- hardcoded hardware params --
|
||||
@dataclass(frozen=True)
|
||||
class CameraConfig:
|
||||
width: int
|
||||
height: int
|
||||
focal_length: float
|
||||
|
||||
@property
|
||||
def size(self):
|
||||
return (self.width, self.height)
|
||||
|
||||
@property
|
||||
def intrinsics(self):
|
||||
# aka 'K' aka camera_frame_from_view_frame
|
||||
return np.array([
|
||||
[self.focal_length, 0.0, float(self.width)/2],
|
||||
[0.0, self.focal_length, float(self.height)/2],
|
||||
[0.0, 0.0, 1.0]
|
||||
])
|
||||
|
||||
@property
|
||||
def intrinsics_inv(self):
|
||||
# aka 'K_inv' aka view_frame_from_camera_frame
|
||||
return np.linalg.inv(self.intrinsics)
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class _NoneCameraConfig(CameraConfig):
|
||||
width: int = 0
|
||||
height: int = 0
|
||||
focal_length: float = 0
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class DeviceCameraConfig:
|
||||
fcam: CameraConfig
|
||||
dcam: CameraConfig
|
||||
ecam: CameraConfig
|
||||
|
||||
def all_cams(self):
|
||||
for cam in ['fcam', 'dcam', 'ecam']:
|
||||
if not isinstance(getattr(self, cam), _NoneCameraConfig):
|
||||
yield cam, getattr(self, cam)
|
||||
|
||||
_ar_ox_fisheye = CameraConfig(1928, 1208, 567.0) # focal length probably wrong? magnification is not consistent across frame
|
||||
_os_fisheye = CameraConfig(2688 // 2, 1520 // 2, 567.0 / 4 * 3)
|
||||
_ar_ox_config = DeviceCameraConfig(CameraConfig(1928, 1208, 2648.0), _ar_ox_fisheye, _ar_ox_fisheye)
|
||||
_os_config = DeviceCameraConfig(CameraConfig(2688 // 2, 1520 // 2, 1522.0 * 3 / 4), _os_fisheye, _os_fisheye)
|
||||
_neo_config = DeviceCameraConfig(CameraConfig(1164, 874, 910.0), CameraConfig(816, 612, 650.0), _NoneCameraConfig())
|
||||
|
||||
DEVICE_CAMERAS = {
|
||||
# A "device camera" is defined by a device type and sensor
|
||||
|
||||
# sensor type was never set on eon/neo/two
|
||||
("neo", "unknown"): _neo_config,
|
||||
# unknown here is AR0231, field was added with OX03C10 support
|
||||
("tici", "unknown"): _ar_ox_config,
|
||||
|
||||
# before deviceState.deviceType was set, assume tici AR config
|
||||
("unknown", "ar0231"): _ar_ox_config,
|
||||
("unknown", "ox03c10"): _ar_ox_config,
|
||||
|
||||
# simulator (emulates a tici)
|
||||
("pc", "unknown"): _ar_ox_config,
|
||||
}
|
||||
prods = itertools.product(('tici', 'tizi', 'mici'), (('ar0231', _ar_ox_config), ('ox03c10', _ar_ox_config), ('os04c10', _os_config)))
|
||||
DEVICE_CAMERAS.update({(d, c[0]): c[1] for d, c in prods})
|
||||
|
||||
# device/mesh : x->forward, y-> right, z->down
|
||||
# view : x->right, y->down, z->forward
|
||||
device_frame_from_view_frame = np.array([
|
||||
[ 0., 0., 1.],
|
||||
[ 1., 0., 0.],
|
||||
[ 0., 1., 0.]
|
||||
])
|
||||
view_frame_from_device_frame = device_frame_from_view_frame.T
|
||||
|
||||
|
||||
# aka 'extrinsic_matrix'
|
||||
# road : x->forward, y -> left, z->up
|
||||
def get_view_frame_from_road_frame(roll, pitch, yaw, height):
|
||||
device_from_road = orient.rot_from_euler([roll, pitch, yaw]).dot(np.diag([1, -1, -1]))
|
||||
view_from_road = view_frame_from_device_frame.dot(device_from_road)
|
||||
return np.hstack((view_from_road, [[0], [height], [0]]))
|
||||
|
||||
|
||||
|
||||
# aka 'extrinsic_matrix'
|
||||
def get_view_frame_from_calib_frame(roll, pitch, yaw, height):
|
||||
device_from_calib= orient.rot_from_euler([roll, pitch, yaw])
|
||||
view_from_calib = view_frame_from_device_frame.dot(device_from_calib)
|
||||
return np.hstack((view_from_calib, [[0], [height], [0]]))
|
||||
|
||||
|
||||
def vp_from_ke(m):
|
||||
"""
|
||||
Computes the vanishing point from the product of the intrinsic and extrinsic
|
||||
matrices C = KE.
|
||||
|
||||
The vanishing point is defined as lim x->infinity C (x, 0, 0, 1).T
|
||||
"""
|
||||
return (m[0, 0]/m[2, 0], m[1, 0]/m[2, 0])
|
||||
|
||||
|
||||
def roll_from_ke(m):
|
||||
# note: different from calibration.h/RollAnglefromKE: i think that one's just wrong
|
||||
return np.arctan2(-(m[1, 0] - m[1, 1] * m[2, 0] / m[2, 1]),
|
||||
-(m[0, 0] - m[0, 1] * m[2, 0] / m[2, 1]))
|
||||
|
||||
|
||||
def normalize(img_pts, intrinsics):
|
||||
# normalizes image coordinates
|
||||
# accepts single pt or array of pts
|
||||
intrinsics_inv = np.linalg.inv(intrinsics)
|
||||
img_pts = np.array(img_pts)
|
||||
input_shape = img_pts.shape
|
||||
img_pts = np.atleast_2d(img_pts)
|
||||
img_pts = np.hstack((img_pts, np.ones((img_pts.shape[0], 1))))
|
||||
img_pts_normalized = img_pts.dot(intrinsics_inv.T)
|
||||
img_pts_normalized[(img_pts < 0).any(axis=1)] = np.nan
|
||||
return img_pts_normalized[:, :2].reshape(input_shape)
|
||||
|
||||
|
||||
def denormalize(img_pts, intrinsics, width=np.inf, height=np.inf):
|
||||
# denormalizes image coordinates
|
||||
# accepts single pt or array of pts
|
||||
img_pts = np.array(img_pts)
|
||||
input_shape = img_pts.shape
|
||||
img_pts = np.atleast_2d(img_pts)
|
||||
img_pts = np.hstack((img_pts, np.ones((img_pts.shape[0], 1), dtype=img_pts.dtype)))
|
||||
img_pts_denormalized = img_pts.dot(intrinsics.T)
|
||||
if np.isfinite(width):
|
||||
img_pts_denormalized[img_pts_denormalized[:, 0] > width] = np.nan
|
||||
img_pts_denormalized[img_pts_denormalized[:, 0] < 0] = np.nan
|
||||
if np.isfinite(height):
|
||||
img_pts_denormalized[img_pts_denormalized[:, 1] > height] = np.nan
|
||||
img_pts_denormalized[img_pts_denormalized[:, 1] < 0] = np.nan
|
||||
return img_pts_denormalized[:, :2].reshape(input_shape)
|
||||
|
||||
|
||||
def get_calib_from_vp(vp, intrinsics):
|
||||
vp_norm = normalize(vp, intrinsics)
|
||||
yaw_calib = np.arctan(vp_norm[0])
|
||||
pitch_calib = -np.arctan(vp_norm[1]*np.cos(yaw_calib))
|
||||
roll_calib = 0
|
||||
return roll_calib, pitch_calib, yaw_calib
|
||||
|
||||
|
||||
def device_from_ecef(pos_ecef, orientation_ecef, pt_ecef):
|
||||
# device from ecef frame
|
||||
# device frame is x -> forward, y-> right, z -> down
|
||||
# accepts single pt or array of pts
|
||||
input_shape = pt_ecef.shape
|
||||
pt_ecef = np.atleast_2d(pt_ecef)
|
||||
ecef_from_device_rot = orient.rotations_from_quats(orientation_ecef)
|
||||
device_from_ecef_rot = ecef_from_device_rot.T
|
||||
pt_ecef_rel = pt_ecef - pos_ecef
|
||||
pt_device = np.einsum('jk,ik->ij', device_from_ecef_rot, pt_ecef_rel)
|
||||
return pt_device.reshape(input_shape)
|
||||
|
||||
|
||||
def img_from_device(pt_device):
|
||||
# img coordinates from pts in device frame
|
||||
# first transforms to view frame, then to img coords
|
||||
# accepts single pt or array of pts
|
||||
input_shape = pt_device.shape
|
||||
pt_device = np.atleast_2d(pt_device)
|
||||
pt_view = np.einsum('jk,ik->ij', view_frame_from_device_frame, pt_device)
|
||||
|
||||
# This function should never return negative depths
|
||||
pt_view[pt_view[:, 2] < 0] = np.nan
|
||||
|
||||
pt_img = pt_view/pt_view[:, 2:3]
|
||||
return pt_img.reshape(input_shape)[:, :2]
|
||||
|
||||
18
iqpilot/common/transformations/coordinates.py
Normal file
18
iqpilot/common/transformations/coordinates.py
Normal file
@@ -0,0 +1,18 @@
|
||||
from iqpilot.common.transformations.orientation import numpy_wrap
|
||||
from iqpilot.common.transformations.transformations import (ecef2geodetic_single,
|
||||
geodetic2ecef_single)
|
||||
from iqpilot.common.transformations.transformations import LocalCoord as LocalCoord_single
|
||||
|
||||
|
||||
class LocalCoord(LocalCoord_single):
|
||||
ecef2ned = numpy_wrap(LocalCoord_single.ecef2ned_single, (3,), (3,))
|
||||
ned2ecef = numpy_wrap(LocalCoord_single.ned2ecef_single, (3,), (3,))
|
||||
geodetic2ned = numpy_wrap(LocalCoord_single.geodetic2ned_single, (3,), (3,))
|
||||
ned2geodetic = numpy_wrap(LocalCoord_single.ned2geodetic_single, (3,), (3,))
|
||||
|
||||
|
||||
geodetic2ecef = numpy_wrap(geodetic2ecef_single, (3,), (3,))
|
||||
ecef2geodetic = numpy_wrap(ecef2geodetic_single, (3,), (3,))
|
||||
|
||||
geodetic_from_ecef = ecef2geodetic
|
||||
ecef_from_geodetic = geodetic2ecef
|
||||
70
iqpilot/common/transformations/model.py
Normal file
70
iqpilot/common/transformations/model.py
Normal file
@@ -0,0 +1,70 @@
|
||||
import numpy as np
|
||||
|
||||
from iqpilot.common.transformations.orientation import rot_from_euler
|
||||
from iqpilot.common.transformations.camera import get_view_frame_from_calib_frame, view_frame_from_device_frame, _ar_ox_fisheye
|
||||
|
||||
# segnet
|
||||
SEGNET_SIZE = (512, 384)
|
||||
|
||||
# MED model
|
||||
MEDMODEL_INPUT_SIZE = (512, 256)
|
||||
MEDMODEL_YUV_SIZE = (MEDMODEL_INPUT_SIZE[0], MEDMODEL_INPUT_SIZE[1] * 3 // 2)
|
||||
MEDMODEL_CY = 47.6
|
||||
|
||||
medmodel_fl = 910.0
|
||||
medmodel_intrinsics = np.array([
|
||||
[medmodel_fl, 0.0, 0.5 * MEDMODEL_INPUT_SIZE[0]],
|
||||
[0.0, medmodel_fl, MEDMODEL_CY],
|
||||
[0.0, 0.0, 1.0]])
|
||||
|
||||
|
||||
# BIG model
|
||||
BIGMODEL_INPUT_SIZE = (1024, 512)
|
||||
BIGMODEL_YUV_SIZE = (BIGMODEL_INPUT_SIZE[0], BIGMODEL_INPUT_SIZE[1] * 3 // 2)
|
||||
|
||||
bigmodel_fl = 910.0
|
||||
bigmodel_intrinsics = np.array([
|
||||
[bigmodel_fl, 0.0, 0.5 * BIGMODEL_INPUT_SIZE[0]],
|
||||
[0.0, bigmodel_fl, 256 + MEDMODEL_CY],
|
||||
[0.0, 0.0, 1.0]])
|
||||
|
||||
|
||||
# SBIG model (big model with the size of small model)
|
||||
SBIGMODEL_INPUT_SIZE = (512, 256)
|
||||
SBIGMODEL_YUV_SIZE = (SBIGMODEL_INPUT_SIZE[0], SBIGMODEL_INPUT_SIZE[1] * 3 // 2)
|
||||
|
||||
sbigmodel_fl = 455.0
|
||||
sbigmodel_intrinsics = np.array([
|
||||
[sbigmodel_fl, 0.0, 0.5 * SBIGMODEL_INPUT_SIZE[0]],
|
||||
[0.0, sbigmodel_fl, 0.5 * (256 + MEDMODEL_CY)],
|
||||
[0.0, 0.0, 1.0]])
|
||||
|
||||
DM_INPUT_SIZE = (1440, 960)
|
||||
dmonitoringmodel_fl = _ar_ox_fisheye.focal_length
|
||||
dmonitoringmodel_intrinsics = np.array([
|
||||
[dmonitoringmodel_fl, 0.0, DM_INPUT_SIZE[0]/2],
|
||||
[0.0, dmonitoringmodel_fl, DM_INPUT_SIZE[1]/2 - (_ar_ox_fisheye.height - DM_INPUT_SIZE[1])/2],
|
||||
[0.0, 0.0, 1.0]])
|
||||
|
||||
bigmodel_frame_from_calib_frame = np.dot(bigmodel_intrinsics,
|
||||
get_view_frame_from_calib_frame(0, 0, 0, 0))
|
||||
|
||||
|
||||
sbigmodel_frame_from_calib_frame = np.dot(sbigmodel_intrinsics,
|
||||
get_view_frame_from_calib_frame(0, 0, 0, 0))
|
||||
|
||||
medmodel_frame_from_calib_frame = np.dot(medmodel_intrinsics,
|
||||
get_view_frame_from_calib_frame(0, 0, 0, 0))
|
||||
|
||||
medmodel_frame_from_bigmodel_frame = np.dot(medmodel_intrinsics, np.linalg.inv(bigmodel_intrinsics))
|
||||
|
||||
calib_from_medmodel = np.linalg.inv(medmodel_frame_from_calib_frame[:, :3])
|
||||
calib_from_sbigmodel = np.linalg.inv(sbigmodel_frame_from_calib_frame[:, :3])
|
||||
|
||||
# This function is verified to give similar results to xx.uncommon.utils.transform_img
|
||||
def get_warp_matrix(device_from_calib_euler: np.ndarray, intrinsics: np.ndarray, bigmodel_frame: bool = False) -> np.ndarray:
|
||||
calib_from_model = calib_from_sbigmodel if bigmodel_frame else calib_from_medmodel
|
||||
device_from_calib = rot_from_euler(device_from_calib_euler)
|
||||
camera_from_calib = intrinsics @ view_frame_from_device_frame @ device_from_calib
|
||||
warp_matrix: np.ndarray = camera_from_calib @ calib_from_model
|
||||
return warp_matrix
|
||||
52
iqpilot/common/transformations/orientation.py
Normal file
52
iqpilot/common/transformations/orientation.py
Normal file
@@ -0,0 +1,52 @@
|
||||
import numpy as np
|
||||
from collections.abc import Callable
|
||||
|
||||
from iqpilot.common.transformations.transformations import (ecef_euler_from_ned_single,
|
||||
euler2quat_single,
|
||||
euler2rot_single,
|
||||
ned_euler_from_ecef_single,
|
||||
quat2euler_single,
|
||||
quat2rot_single,
|
||||
rot2euler_single,
|
||||
rot2quat_single)
|
||||
|
||||
|
||||
def numpy_wrap(function, input_shape, output_shape) -> Callable[..., np.ndarray]:
|
||||
"""Wrap a function to take either an input or list of inputs and return the correct shape"""
|
||||
def f(*inps):
|
||||
*args, inp = inps
|
||||
inp = np.array(inp)
|
||||
shape = inp.shape
|
||||
|
||||
if len(shape) == len(input_shape):
|
||||
out_shape = output_shape
|
||||
else:
|
||||
out_shape = (shape[0],) + output_shape
|
||||
|
||||
# Add empty dimension if inputs is not a list
|
||||
if len(shape) == len(input_shape):
|
||||
inp.shape = (1, ) + inp.shape
|
||||
|
||||
result = np.asarray([function(*args, i) for i in inp])
|
||||
result.shape = out_shape
|
||||
return result
|
||||
return f
|
||||
|
||||
|
||||
euler2quat = numpy_wrap(euler2quat_single, (3,), (4,))
|
||||
quat2euler = numpy_wrap(quat2euler_single, (4,), (3,))
|
||||
quat2rot = numpy_wrap(quat2rot_single, (4,), (3, 3))
|
||||
rot2quat = numpy_wrap(rot2quat_single, (3, 3), (4,))
|
||||
euler2rot = numpy_wrap(euler2rot_single, (3,), (3, 3))
|
||||
rot2euler = numpy_wrap(rot2euler_single, (3, 3), (3,))
|
||||
ecef_euler_from_ned = numpy_wrap(ecef_euler_from_ned_single, (3,), (3,))
|
||||
ned_euler_from_ecef = numpy_wrap(ned_euler_from_ecef_single, (3,), (3,))
|
||||
|
||||
quats_from_rotations = rot2quat
|
||||
quat_from_rot = rot2quat
|
||||
rotations_from_quats = quat2rot
|
||||
rot_from_quat = quat2rot
|
||||
euler_from_rot = rot2euler
|
||||
euler_from_quat = quat2euler
|
||||
rot_from_euler = euler2rot
|
||||
quat_from_euler = euler2quat
|
||||
0
iqpilot/common/transformations/tests/__init__.py
Normal file
0
iqpilot/common/transformations/tests/__init__.py
Normal file
137
iqpilot/common/transformations/tests/test_coordinates.py
Normal file
137
iqpilot/common/transformations/tests/test_coordinates.py
Normal file
@@ -0,0 +1,137 @@
|
||||
import numpy as np
|
||||
|
||||
import iqpilot.common.transformations.coordinates as coord
|
||||
|
||||
geodetic_positions = np.array([[37.7610403, -122.4778699, 115],
|
||||
[27.4840915, -68.5867592, 2380],
|
||||
[32.4916858, -113.652821, -6],
|
||||
[15.1392514, 103.6976037, 24],
|
||||
[24.2302229, 44.2835412, 1650]])
|
||||
|
||||
ecef_positions = np.array([[-2711076.55270557, -4259167.14692758, 3884579.87669935],
|
||||
[ 2068042.69652729, -5273435.40316622, 2927004.89190746],
|
||||
[-2160412.60461669, -4932588.89873832, 3406542.29652851],
|
||||
[-1458247.92550567, 5983060.87496612, 1654984.6099885 ],
|
||||
[ 4167239.10867871, 4064301.90363223, 2602234.6065749 ]])
|
||||
|
||||
ecef_positions_offset = np.array([[-2711004.46961115, -4259099.33540613, 3884605.16002147],
|
||||
[ 2068074.30639499, -5273413.78835412, 2927012.48741131],
|
||||
[-2160344.53748176, -4932586.20092211, 3406636.2962545 ],
|
||||
[-1458211.98517094, 5983151.11161276, 1655077.02698447],
|
||||
[ 4167271.20055269, 4064398.22619263, 2602238.95265847]])
|
||||
|
||||
|
||||
ned_offsets = np.array([[78.722153649976391, 24.396208657446344, 60.343017506838436],
|
||||
[10.699003365155221, 37.319278617604269, 4.1084100025050407],
|
||||
[95.282646251726959, 61.266689955574428, -25.376506058505054],
|
||||
[68.535769283630003, -56.285970011848889, -100.54840137956515],
|
||||
[-33.066609321880179, 46.549821994306861, -84.062540548335591]])
|
||||
|
||||
ecef_init_batch = np.array([2068042.69652729, -5273435.40316622, 2927004.89190746])
|
||||
ecef_positions_offset_batch = np.array([[ 2068089.41454771, -5273434.46829148, 2927074.04783672],
|
||||
[ 2068103.31628647, -5273393.92275431, 2927102.08725987],
|
||||
[ 2068108.49939636, -5273359.27047121, 2927045.07091581],
|
||||
[ 2068075.12395611, -5273381.69432566, 2927041.08207992],
|
||||
[ 2068060.72033399, -5273430.6061505, 2927094.54928305]])
|
||||
|
||||
ned_offsets_batch = np.array([[ 53.88103168, 43.83445935, -46.27488057],
|
||||
[ 93.83378995, 71.57943024, -30.23113187],
|
||||
[ 57.26725796, 89.05602684, 23.02265814],
|
||||
[ 49.71775195, 49.79767572, 17.15351015],
|
||||
[ 78.56272609, 18.53100158, -43.25290759]])
|
||||
|
||||
|
||||
class TestNED:
|
||||
def test_small_distances(self):
|
||||
start_geodetic = np.array([33.8042184, -117.888593, 0.0])
|
||||
local_coord = coord.LocalCoord.from_geodetic(start_geodetic)
|
||||
|
||||
start_ned = local_coord.geodetic2ned(start_geodetic)
|
||||
np.testing.assert_array_equal(start_ned, np.zeros(3,))
|
||||
|
||||
west_geodetic = start_geodetic + [0, -0.0005, 0]
|
||||
west_ned = local_coord.geodetic2ned(west_geodetic)
|
||||
assert np.abs(west_ned[0]) < 1e-3
|
||||
assert west_ned[1] < 0
|
||||
|
||||
southwest_geodetic = start_geodetic + [-0.0005, -0.002, 0]
|
||||
southwest_ned = local_coord.geodetic2ned(southwest_geodetic)
|
||||
assert southwest_ned[0] < 0
|
||||
assert southwest_ned[1] < 0
|
||||
|
||||
def test_ecef_geodetic(self):
|
||||
# testing single
|
||||
np.testing.assert_allclose(ecef_positions[0], coord.geodetic2ecef(geodetic_positions[0]), rtol=1e-9)
|
||||
np.testing.assert_allclose(geodetic_positions[0, :2], coord.ecef2geodetic(ecef_positions[0])[:2], rtol=1e-9)
|
||||
np.testing.assert_allclose(geodetic_positions[0, 2], coord.ecef2geodetic(ecef_positions[0])[2], rtol=1e-9, atol=1e-4)
|
||||
|
||||
np.testing.assert_allclose(geodetic_positions[:, :2], coord.ecef2geodetic(ecef_positions)[:, :2], rtol=1e-9)
|
||||
np.testing.assert_allclose(geodetic_positions[:, 2], coord.ecef2geodetic(ecef_positions)[:, 2], rtol=1e-9, atol=1e-4)
|
||||
np.testing.assert_allclose(ecef_positions, coord.geodetic2ecef(geodetic_positions), rtol=1e-9)
|
||||
|
||||
|
||||
def test_ned(self):
|
||||
for ecef_pos in ecef_positions:
|
||||
converter = coord.LocalCoord.from_ecef(ecef_pos)
|
||||
ecef_pos_moved = ecef_pos + [25, -25, 25]
|
||||
ecef_pos_moved_double_converted = converter.ned2ecef(converter.ecef2ned(ecef_pos_moved))
|
||||
np.testing.assert_allclose(ecef_pos_moved, ecef_pos_moved_double_converted, rtol=1e-9)
|
||||
|
||||
for geo_pos in geodetic_positions:
|
||||
converter = coord.LocalCoord.from_geodetic(geo_pos)
|
||||
geo_pos_moved = geo_pos + np.array([0, 0, 10])
|
||||
geo_pos_double_converted_moved = converter.ned2geodetic(converter.geodetic2ned(geo_pos) + np.array([0, 0, -10]))
|
||||
np.testing.assert_allclose(geo_pos_moved[:2], geo_pos_double_converted_moved[:2], rtol=1e-9, atol=1e-6)
|
||||
np.testing.assert_allclose(geo_pos_moved[2], geo_pos_double_converted_moved[2], rtol=1e-9, atol=1e-4)
|
||||
|
||||
def test_ned_saved_results(self):
|
||||
for i, ecef_pos in enumerate(ecef_positions):
|
||||
converter = coord.LocalCoord.from_ecef(ecef_pos)
|
||||
np.testing.assert_allclose(converter.ned2ecef(ned_offsets[i]),
|
||||
ecef_positions_offset[i],
|
||||
rtol=1e-9, atol=1e-4)
|
||||
np.testing.assert_allclose(converter.ecef2ned(ecef_positions_offset[i]),
|
||||
ned_offsets[i],
|
||||
rtol=1e-9, atol=1e-4)
|
||||
|
||||
def test_ned_batch(self):
|
||||
converter = coord.LocalCoord.from_ecef(ecef_init_batch)
|
||||
np.testing.assert_allclose(converter.ecef2ned(ecef_positions_offset_batch),
|
||||
ned_offsets_batch,
|
||||
rtol=1e-9, atol=1e-7)
|
||||
np.testing.assert_allclose(converter.ned2ecef(ned_offsets_batch),
|
||||
ecef_positions_offset_batch,
|
||||
rtol=1e-9, atol=1e-7)
|
||||
|
||||
def test_errors(self):
|
||||
# Test wrong shape/type for geodetic2ecef
|
||||
# numpy_wrap raises IndexError for scalar input
|
||||
with np.testing.assert_raises(IndexError):
|
||||
coord.geodetic2ecef(1.0)
|
||||
|
||||
with np.testing.assert_raises_regex(ValueError, "Geodetic must be size 3"):
|
||||
coord.geodetic2ecef([0, 0])
|
||||
|
||||
with np.testing.assert_raises_regex(ValueError, "Geodetic must be size 3"):
|
||||
coord.geodetic2ecef([0, 0, 0, 0])
|
||||
|
||||
with np.testing.assert_raises(TypeError):
|
||||
coord.geodetic2ecef(['a', 'b', 'c'])
|
||||
|
||||
# Test LocalCoord constructor errors
|
||||
with np.testing.assert_raises(ValueError):
|
||||
coord.LocalCoord.from_geodetic([0, 0])
|
||||
|
||||
with np.testing.assert_raises(ValueError):
|
||||
coord.LocalCoord.from_geodetic(1)
|
||||
|
||||
with np.testing.assert_raises(TypeError):
|
||||
coord.LocalCoord.from_geodetic(['a', 'b', 'c'])
|
||||
|
||||
# Test wrong shape/type for ecef2geodetic
|
||||
with np.testing.assert_raises(ValueError):
|
||||
coord.ecef2geodetic([1, 2])
|
||||
with np.testing.assert_raises(ValueError):
|
||||
coord.ecef2geodetic([1, 2, 3, 4])
|
||||
with np.testing.assert_raises(IndexError):
|
||||
coord.ecef2geodetic(1.0)
|
||||
91
iqpilot/common/transformations/tests/test_orientation.py
Normal file
91
iqpilot/common/transformations/tests/test_orientation.py
Normal file
@@ -0,0 +1,91 @@
|
||||
import numpy as np
|
||||
import pytest
|
||||
|
||||
from iqpilot.common.transformations.orientation import euler2quat, quat2euler, euler2rot, rot2euler, \
|
||||
rot2quat, quat2rot, \
|
||||
ned_euler_from_ecef
|
||||
|
||||
eulers = np.array([[ 1.46520501, 2.78688383, 2.92780854],
|
||||
[ 4.86909526, 3.60618161, 4.30648981],
|
||||
[ 3.72175965, 2.68763705, 5.43895988],
|
||||
[ 5.92306687, 5.69573614, 0.81100357],
|
||||
[ 0.67838374, 5.02402037, 2.47106426]])
|
||||
|
||||
quats = np.array([[ 0.66855182, -0.71500939, 0.19539353, 0.06017818],
|
||||
[ 0.43163717, 0.70013301, 0.28209145, 0.49389021],
|
||||
[ 0.44121991, -0.08252646, 0.34257534, 0.82532207],
|
||||
[ 0.88578382, -0.04515356, -0.32936046, 0.32383617],
|
||||
[ 0.06578165, 0.61282835, 0.07126891, 0.78424163]])
|
||||
|
||||
ecef_positions = np.array([[-2711076.55270557, -4259167.14692758, 3884579.87669935],
|
||||
[ 2068042.69652729, -5273435.40316622, 2927004.89190746],
|
||||
[-2160412.60461669, -4932588.89873832, 3406542.29652851],
|
||||
[-1458247.92550567, 5983060.87496612, 1654984.6099885 ],
|
||||
[ 4167239.10867871, 4064301.90363223, 2602234.6065749 ]])
|
||||
|
||||
ned_eulers = np.array([[ 0.46806039, -0.4881889 , 1.65697808],
|
||||
[-2.14525969, -0.36533066, 0.73813479],
|
||||
[-1.39523364, -0.58540761, -1.77376356],
|
||||
[-1.84220435, 0.61828016, -1.03310421],
|
||||
[ 2.50450101, 0.36304151, 0.33136365]])
|
||||
|
||||
|
||||
class TestOrientation:
|
||||
def test_quat_euler(self):
|
||||
for i, eul in enumerate(eulers):
|
||||
np.testing.assert_allclose(quats[i], euler2quat(eul), rtol=1e-7)
|
||||
np.testing.assert_allclose(quats[i], euler2quat(quat2euler(quats[i])), rtol=1e-6)
|
||||
for i, eul in enumerate(eulers):
|
||||
np.testing.assert_allclose(quats[i], euler2quat(list(eul)), rtol=1e-7)
|
||||
np.testing.assert_allclose(quats[i], euler2quat(quat2euler(list(quats[i]))), rtol=1e-6)
|
||||
np.testing.assert_allclose(quats, euler2quat(eulers), rtol=1e-7)
|
||||
np.testing.assert_allclose(quats, euler2quat(quat2euler(quats)), rtol=1e-6)
|
||||
|
||||
def test_rot_euler(self):
|
||||
for eul in eulers:
|
||||
np.testing.assert_allclose(euler2quat(eul), euler2quat(rot2euler(euler2rot(eul))), rtol=1e-7)
|
||||
for eul in eulers:
|
||||
np.testing.assert_allclose(euler2quat(eul), euler2quat(rot2euler(euler2rot(list(eul)))), rtol=1e-7)
|
||||
np.testing.assert_allclose(euler2quat(eulers), euler2quat(rot2euler(euler2rot(eulers))), rtol=1e-7)
|
||||
|
||||
def test_rot_quat(self):
|
||||
for quat in quats:
|
||||
np.testing.assert_allclose(quat, rot2quat(quat2rot(quat)), rtol=1e-7)
|
||||
for quat in quats:
|
||||
np.testing.assert_allclose(quat, rot2quat(quat2rot(list(quat))), rtol=1e-7)
|
||||
np.testing.assert_allclose(quats, rot2quat(quat2rot(quats)), rtol=1e-7)
|
||||
|
||||
def test_euler_ned(self):
|
||||
for i in range(len(eulers)):
|
||||
np.testing.assert_allclose(ned_eulers[i], ned_euler_from_ecef(ecef_positions[i], eulers[i]), rtol=1e-7)
|
||||
#np.testing.assert_allclose(eulers[i], ecef_euler_from_ned(ecef_positions[i], ned_eulers[i]), rtol=1e-7)
|
||||
# np.testing.assert_allclose(ned_eulers, ned_euler_from_ecef(ecef_positions, eulers), rtol=1e-7)
|
||||
|
||||
def test_inputs(self):
|
||||
with pytest.raises(ValueError):
|
||||
euler2quat([1, 2])
|
||||
|
||||
with pytest.raises(ValueError):
|
||||
quat2rot([1, 2, 3])
|
||||
|
||||
with pytest.raises(IndexError):
|
||||
rot2quat(np.zeros((2, 2)))
|
||||
|
||||
def test_euler_rot_consistency(self):
|
||||
rpy = [0.1, 0.2, 0.3]
|
||||
R = euler2rot(rpy)
|
||||
|
||||
# R -> q -> R
|
||||
q = rot2quat(R)
|
||||
R_new = quat2rot(q)
|
||||
np.testing.assert_allclose(R, R_new, atol=1e-15)
|
||||
|
||||
# q -> R -> Euler (quat2euler) -> R
|
||||
rpy_new = quat2euler(q)
|
||||
R_new2 = euler2rot(rpy_new)
|
||||
np.testing.assert_allclose(R, R_new2, atol=1e-15)
|
||||
|
||||
# R -> Euler (rot2euler) -> R
|
||||
rpy_from_rot = rot2euler(R)
|
||||
R_new3 = euler2rot(rpy_from_rot)
|
||||
np.testing.assert_allclose(R, R_new3, atol=1e-15)
|
||||
342
iqpilot/common/transformations/transformations.py
Normal file
342
iqpilot/common/transformations/transformations.py
Normal file
@@ -0,0 +1,342 @@
|
||||
import numpy as np
|
||||
|
||||
|
||||
# Constants
|
||||
a = 6378137.0
|
||||
b = 6356752.3142
|
||||
esq = 6.69437999014e-3
|
||||
e1sq = 6.73949674228e-3
|
||||
|
||||
|
||||
def geodetic2ecef_single(g):
|
||||
"""
|
||||
Convert geodetic coordinates (latitude, longitude, altitude) to ECEF.
|
||||
"""
|
||||
try:
|
||||
if len(g) != 3:
|
||||
raise ValueError("Geodetic must be size 3")
|
||||
except TypeError:
|
||||
raise ValueError("Geodetic must be a sequence of length 3") from None
|
||||
|
||||
lat, lon, alt = g
|
||||
lat = np.radians(lat)
|
||||
lon = np.radians(lon)
|
||||
xi = np.sqrt(1.0 - esq * np.sin(lat)**2)
|
||||
x = (a / xi + alt) * np.cos(lat) * np.cos(lon)
|
||||
y = (a / xi + alt) * np.cos(lat) * np.sin(lon)
|
||||
z = (a / xi * (1.0 - esq) + alt) * np.sin(lat)
|
||||
return np.array([x, y, z])
|
||||
|
||||
|
||||
def ecef2geodetic_single(e):
|
||||
"""
|
||||
Convert ECEF to geodetic coordinates using Ferrari's solution.
|
||||
"""
|
||||
x, y, z = e
|
||||
r = np.sqrt(x**2 + y**2)
|
||||
Esq = a**2 - b**2
|
||||
F = 54 * b**2 * z**2
|
||||
G = r**2 + (1 - esq) * z**2 - esq * Esq
|
||||
C = (esq**2 * F * r**2) / (G**3)
|
||||
S = np.cbrt(1 + C + np.sqrt(C**2 + 2 * C))
|
||||
P = F / (3 * (S + 1 / S + 1)**2 * G**2)
|
||||
Q = np.sqrt(1 + 2 * esq**2 * P)
|
||||
r_0 = -(P * esq * r) / (1 + Q) + np.sqrt(0.5 * a**2 * (1 + 1.0 / Q) - P * (1 - esq) * z**2 / (Q * (1 + Q)) - 0.5 * P * r**2)
|
||||
U = np.sqrt((r - esq * r_0)**2 + z**2)
|
||||
V = np.sqrt((r - esq * r_0)**2 + (1 - esq) * z**2)
|
||||
Z_0 = b**2 * z / (a * V)
|
||||
h = U * (1 - b**2 / (a * V))
|
||||
lat = np.arctan((z + e1sq * Z_0) / r)
|
||||
lon = np.arctan2(y, x)
|
||||
return np.array([np.degrees(lat), np.degrees(lon), h])
|
||||
|
||||
|
||||
def euler2quat_single(euler):
|
||||
"""
|
||||
Convert Euler angles (roll, pitch, yaw) to a quaternion.
|
||||
Rotation order: Z-Y-X (yaw, pitch, roll).
|
||||
"""
|
||||
phi, theta, psi = euler
|
||||
|
||||
c_phi, s_phi = np.cos(phi / 2), np.sin(phi / 2)
|
||||
c_theta, s_theta = np.cos(theta / 2), np.sin(theta / 2)
|
||||
c_psi, s_psi = np.cos(psi / 2), np.sin(psi / 2)
|
||||
|
||||
w = c_phi * c_theta * c_psi + s_phi * s_theta * s_psi
|
||||
x = s_phi * c_theta * c_psi - c_phi * s_theta * s_psi
|
||||
y = c_phi * s_theta * c_psi + s_phi * c_theta * s_psi
|
||||
z = c_phi * c_theta * s_psi - s_phi * s_theta * c_psi
|
||||
|
||||
if w < 0:
|
||||
return np.array([-w, -x, -y, -z])
|
||||
return np.array([w, x, y, z])
|
||||
|
||||
|
||||
def quat2euler_single(q):
|
||||
"""
|
||||
Convert a quaternion to Euler angles (roll, pitch, yaw).
|
||||
"""
|
||||
w, x, y, z = q
|
||||
gamma = np.arctan2(2 * (w * x + y * z), 1 - 2 * (x**2 + y**2))
|
||||
sin_arg = 2 * (w * y - z * x)
|
||||
sin_arg = np.clip(sin_arg, -1.0, 1.0)
|
||||
theta = np.arcsin(sin_arg)
|
||||
psi = np.arctan2(2 * (w * z + x * y), 1 - 2 * (y**2 + z**2))
|
||||
return np.array([gamma, theta, psi])
|
||||
|
||||
|
||||
def quat2rot_single(q):
|
||||
"""
|
||||
Convert a quaternion to a 3x3 rotation matrix.
|
||||
"""
|
||||
w, x, y, z = q
|
||||
xx, yy, zz = x * x, y * y, z * z
|
||||
xy, xz, yz = x * y, x * z, y * z
|
||||
wx, wy, wz = w * x, w * y, w * z
|
||||
|
||||
mat = np.array([
|
||||
[1 - 2 * (yy + zz), 2 * (xy - wz), 2 * (xz + wy)],
|
||||
[2 * (xy + wz), 1 - 2 * (xx + zz), 2 * (yz - wx)],
|
||||
[2 * (xz - wy), 2 * (yz + wx), 1 - 2 * (xx + yy)]
|
||||
])
|
||||
return mat
|
||||
|
||||
|
||||
def rot2quat_single(rot):
|
||||
"""
|
||||
Convert a 3x3 rotation matrix to a quaternion.
|
||||
"""
|
||||
trace = np.trace(rot)
|
||||
if trace > 0:
|
||||
s = 0.5 / np.sqrt(trace + 1.0)
|
||||
w = 0.25 / s
|
||||
x = (rot[2, 1] - rot[1, 2]) * s
|
||||
y = (rot[0, 2] - rot[2, 0]) * s
|
||||
z = (rot[1, 0] - rot[0, 1]) * s
|
||||
else:
|
||||
if rot[0, 0] > rot[1, 1] and rot[0, 0] > rot[2, 2]:
|
||||
s = 2.0 * np.sqrt(1.0 + rot[0, 0] - rot[1, 1] - rot[2, 2])
|
||||
w = (rot[2, 1] - rot[1, 2]) / s
|
||||
x = 0.25 * s
|
||||
y = (rot[0, 1] + rot[1, 0]) / s
|
||||
z = (rot[0, 2] + rot[2, 0]) / s
|
||||
elif rot[1, 1] > rot[2, 2]:
|
||||
s = 2.0 * np.sqrt(1.0 + rot[1, 1] - rot[0, 0] - rot[2, 2])
|
||||
w = (rot[0, 2] - rot[2, 0]) / s
|
||||
x = (rot[0, 1] + rot[1, 0]) / s
|
||||
y = 0.25 * s
|
||||
z = (rot[1, 2] + rot[2, 1]) / s
|
||||
else:
|
||||
s = 2.0 * np.sqrt(1.0 + rot[2, 2] - rot[0, 0] - rot[1, 1])
|
||||
w = (rot[1, 0] - rot[0, 1]) / s
|
||||
x = (rot[0, 2] + rot[2, 0]) / s
|
||||
y = (rot[1, 2] + rot[2, 1]) / s
|
||||
z = 0.25 * s
|
||||
|
||||
if w < 0:
|
||||
return np.array([-w, -x, -y, -z])
|
||||
return np.array([w, x, y, z])
|
||||
|
||||
|
||||
def euler2rot_single(euler):
|
||||
"""
|
||||
Convert Euler angles (roll, pitch, yaw) to a 3x3 rotation matrix.
|
||||
Rotation order: Z-Y-X (yaw, pitch, roll).
|
||||
"""
|
||||
phi, theta, psi = euler
|
||||
|
||||
cx, sx = np.cos(phi), np.sin(phi)
|
||||
cy, sy = np.cos(theta), np.sin(theta)
|
||||
cz, sz = np.cos(psi), np.sin(psi)
|
||||
|
||||
Rx = np.array([[1, 0, 0], [0, cx, -sx], [0, sx, cx]])
|
||||
Ry = np.array([[cy, 0, sy], [0, 1, 0], [-sy, 0, cy]])
|
||||
Rz = np.array([[cz, -sz, 0], [sz, cz, 0], [0, 0, 1]])
|
||||
|
||||
return Rz @ Ry @ Rx
|
||||
|
||||
|
||||
def rot2euler_single(rot):
|
||||
"""
|
||||
Convert a 3x3 rotation matrix to Euler angles (roll, pitch, yaw).
|
||||
"""
|
||||
return quat2euler_single(rot2quat_single(rot))
|
||||
|
||||
|
||||
def rot_matrix(roll, pitch, yaw):
|
||||
"""
|
||||
Create a 3x3 rotation matrix from roll, pitch, and yaw angles.
|
||||
"""
|
||||
return euler2rot_single([roll, pitch, yaw])
|
||||
|
||||
|
||||
def axis_angle_to_rot(axis, angle):
|
||||
"""
|
||||
Convert an axis-angle representation to a 3x3 rotation matrix.
|
||||
"""
|
||||
c = np.cos(angle / 2)
|
||||
s = np.sin(angle / 2)
|
||||
q = np.array([c, s*axis[0], s*axis[1], s*axis[2]])
|
||||
return quat2rot_single(q)
|
||||
|
||||
|
||||
class LocalCoord:
|
||||
"""
|
||||
A class to handle conversions between ECEF and local NED coordinates.
|
||||
"""
|
||||
def __init__(self, geodetic=None, ecef=None):
|
||||
"""
|
||||
Initialize LocalCoord with either geodetic or ECEF coordinates.
|
||||
"""
|
||||
if geodetic is not None:
|
||||
self.init_ecef = geodetic2ecef_single(geodetic)
|
||||
lat, lon, _ = geodetic
|
||||
elif ecef is not None:
|
||||
self.init_ecef = np.array(ecef)
|
||||
lat, lon, _ = ecef2geodetic_single(ecef)
|
||||
else:
|
||||
raise ValueError("Must provide geodetic or ecef")
|
||||
|
||||
lat = np.radians(lat)
|
||||
lon = np.radians(lon)
|
||||
|
||||
self.ned2ecef_matrix = np.array([
|
||||
[-np.sin(lat) * np.cos(lon), -np.sin(lon), -np.cos(lat) * np.cos(lon)],
|
||||
[-np.sin(lat) * np.sin(lon), np.cos(lon), -np.cos(lat) * np.sin(lon)],
|
||||
[np.cos(lat), 0, -np.sin(lat)]
|
||||
])
|
||||
self.ecef2ned_matrix = self.ned2ecef_matrix.T
|
||||
|
||||
@classmethod
|
||||
def from_geodetic(cls, geodetic):
|
||||
"""
|
||||
Create a LocalCoord instance from geodetic coordinates.
|
||||
"""
|
||||
return cls(geodetic=geodetic)
|
||||
|
||||
@classmethod
|
||||
def from_ecef(cls, ecef):
|
||||
"""
|
||||
Create a LocalCoord instance from ECEF coordinates.
|
||||
"""
|
||||
return cls(ecef=ecef)
|
||||
|
||||
def ecef2ned_single(self, ecef):
|
||||
"""
|
||||
Convert a single ECEF point to NED coordinates relative to the origin.
|
||||
"""
|
||||
return self.ecef2ned_matrix @ (ecef - self.init_ecef)
|
||||
|
||||
def ned2ecef_single(self, ned):
|
||||
"""
|
||||
Convert a single NED point to ECEF coordinates.
|
||||
"""
|
||||
return self.ned2ecef_matrix @ ned + self.init_ecef
|
||||
|
||||
def geodetic2ned_single(self, geodetic):
|
||||
"""
|
||||
Convert a single geodetic point to NED coordinates.
|
||||
"""
|
||||
ecef = geodetic2ecef_single(geodetic)
|
||||
return self.ecef2ned_single(ecef)
|
||||
|
||||
def ned2geodetic_single(self, ned):
|
||||
"""
|
||||
Convert a single NED point to geodetic coordinates.
|
||||
"""
|
||||
ecef = self.ned2ecef_single(ned)
|
||||
return ecef2geodetic_single(ecef)
|
||||
|
||||
@property
|
||||
def ned_from_ecef_matrix(self):
|
||||
"""
|
||||
Returns the rotation matrix from ECEF to NED coordinates.
|
||||
"""
|
||||
return self.ecef2ned_matrix
|
||||
|
||||
@property
|
||||
def ecef_from_ned_matrix(self):
|
||||
"""
|
||||
Returns the rotation matrix from NED to ECEF coordinates.
|
||||
"""
|
||||
return self.ned2ecef_matrix
|
||||
|
||||
|
||||
def ecef_euler_from_ned_single(ecef_init, ned_pose):
|
||||
"""
|
||||
Convert NED Euler angles (roll, pitch, yaw) at a given ECEF origin
|
||||
to equivalent ECEF Euler angles.
|
||||
"""
|
||||
converter = LocalCoord(ecef=ecef_init)
|
||||
zero = np.array(ecef_init)
|
||||
|
||||
x0 = converter.ned2ecef_single([1, 0, 0]) - zero
|
||||
y0 = converter.ned2ecef_single([0, 1, 0]) - zero
|
||||
z0 = converter.ned2ecef_single([0, 0, 1]) - zero
|
||||
|
||||
phi, theta, psi = ned_pose
|
||||
|
||||
x1 = axis_angle_to_rot(z0, psi) @ x0
|
||||
y1 = axis_angle_to_rot(z0, psi) @ y0
|
||||
z1 = axis_angle_to_rot(z0, psi) @ z0
|
||||
|
||||
x2 = axis_angle_to_rot(y1, theta) @ x1
|
||||
y2 = axis_angle_to_rot(y1, theta) @ y1
|
||||
z2 = axis_angle_to_rot(y1, theta) @ z1
|
||||
|
||||
x3 = axis_angle_to_rot(x2, phi) @ x2
|
||||
y3 = axis_angle_to_rot(x2, phi) @ y2
|
||||
|
||||
x0 = np.array([1.0, 0, 0])
|
||||
y0 = np.array([0, 1.0, 0])
|
||||
z0 = np.array([0, 0, 1.0])
|
||||
|
||||
psi_out = np.arctan2(np.dot(x3, y0), np.dot(x3, x0))
|
||||
theta_out = np.arctan2(-np.dot(x3, z0), np.sqrt(np.dot(x3, x0)**2 + np.dot(x3, y0)**2))
|
||||
|
||||
y2 = axis_angle_to_rot(z0, psi_out) @ y0
|
||||
z2 = axis_angle_to_rot(y2, theta_out) @ z0
|
||||
|
||||
phi_out = np.arctan2(np.dot(y3, z2), np.dot(y3, y2))
|
||||
|
||||
return np.array([phi_out, theta_out, psi_out])
|
||||
|
||||
|
||||
def ned_euler_from_ecef_single(ecef_init, ecef_pose):
|
||||
"""
|
||||
Convert ECEF Euler angles (roll, pitch, yaw) at a given ECEF origin
|
||||
to equivalent NED Euler angles.
|
||||
"""
|
||||
converter = LocalCoord(ecef=ecef_init)
|
||||
|
||||
x0 = np.array([1.0, 0, 0])
|
||||
y0 = np.array([0, 1.0, 0])
|
||||
z0 = np.array([0, 0, 1.0])
|
||||
|
||||
phi, theta, psi = ecef_pose
|
||||
|
||||
x1 = axis_angle_to_rot(z0, psi) @ x0
|
||||
y1 = axis_angle_to_rot(z0, psi) @ y0
|
||||
z1 = axis_angle_to_rot(z0, psi) @ z0
|
||||
|
||||
x2 = axis_angle_to_rot(y1, theta) @ x1
|
||||
y2 = axis_angle_to_rot(y1, theta) @ y1
|
||||
z2 = axis_angle_to_rot(y1, theta) @ z1
|
||||
|
||||
x3 = axis_angle_to_rot(x2, phi) @ x2
|
||||
y3 = axis_angle_to_rot(x2, phi) @ y2
|
||||
|
||||
zero = np.array(ecef_init)
|
||||
x0 = converter.ned2ecef_single([1, 0, 0]) - zero
|
||||
y0 = converter.ned2ecef_single([0, 1, 0]) - zero
|
||||
z0 = converter.ned2ecef_single([0, 0, 1]) - zero
|
||||
|
||||
psi_out = np.arctan2(np.dot(x3, y0), np.dot(x3, x0))
|
||||
theta_out = np.arctan2(-np.dot(x3, z0), np.sqrt(np.dot(x3, x0)**2 + np.dot(x3, y0)**2))
|
||||
|
||||
y2 = axis_angle_to_rot(z0, psi_out) @ y0
|
||||
z2 = axis_angle_to_rot(y2, theta_out) @ z0
|
||||
|
||||
phi_out = np.arctan2(np.dot(y3, z2), np.dot(y3, y2))
|
||||
|
||||
return np.array([phi_out, theta_out, psi_out])
|
||||
271
iqpilot/common/utils.py
Normal file
271
iqpilot/common/utils.py
Normal file
@@ -0,0 +1,271 @@
|
||||
import io
|
||||
import os
|
||||
import tempfile
|
||||
import contextlib
|
||||
import subprocess
|
||||
import time
|
||||
import functools
|
||||
from subprocess import Popen, PIPE, TimeoutExpired
|
||||
import zstandard as zstd
|
||||
|
||||
LOG_COMPRESSION_LEVEL = 10 # little benefit up to level 15. level ~17 is a small step change
|
||||
|
||||
class Timer:
|
||||
"""Simple lap timer for profiling sequential operations."""
|
||||
|
||||
def __init__(self):
|
||||
self._start = self._lap = time.monotonic()
|
||||
self._sections = {}
|
||||
|
||||
def lap(self, name):
|
||||
now = time.monotonic()
|
||||
self._sections[name] = now - self._lap
|
||||
self._lap = now
|
||||
|
||||
@property
|
||||
def total(self):
|
||||
return time.monotonic() - self._start
|
||||
|
||||
def fmt(self, duration):
|
||||
parts = ", ".join(f"{k}={v:.2f}s" + (f" ({duration/v:.0f}x)" if k == 'render' and v > 0 else "") for k, v in self._sections.items())
|
||||
total = self.total
|
||||
realtime = f"{duration/total:.1f}x realtime" if total > 0 else "N/A"
|
||||
return f"{duration}s in {total:.1f}s ({realtime}) | {parts}"
|
||||
|
||||
def sudo_write(val: str, path: str) -> None:
|
||||
try:
|
||||
with open(path, 'w') as f:
|
||||
f.write(str(val))
|
||||
except PermissionError:
|
||||
os.system(f"sudo chmod a+w {path}")
|
||||
try:
|
||||
with open(path, 'w') as f:
|
||||
f.write(str(val))
|
||||
except PermissionError:
|
||||
# fallback for debugfs files
|
||||
os.system(f"sudo su -c 'echo {val} > {path}'")
|
||||
|
||||
|
||||
def sudo_read(path: str) -> str:
|
||||
try:
|
||||
return subprocess.check_output(f"sudo cat {path}", shell=True, encoding='utf8').strip()
|
||||
except Exception:
|
||||
return ""
|
||||
|
||||
|
||||
class MovingAverage:
|
||||
def __init__(self, window_size: int):
|
||||
self.window_size: int = window_size
|
||||
self.buffer: list[float] = [0.0] * window_size
|
||||
self.index: int = 0
|
||||
self.count: int = 0
|
||||
self.sum: float = 0.0
|
||||
|
||||
def add_value(self, new_value: float):
|
||||
# Update the sum: subtract the value being replaced and add the new value
|
||||
self.sum -= self.buffer[self.index]
|
||||
self.buffer[self.index] = new_value
|
||||
self.sum += new_value
|
||||
|
||||
# Update the index in a circular manner
|
||||
self.index = (self.index + 1) % self.window_size
|
||||
|
||||
# Track the number of added values (for partial windows)
|
||||
self.count = min(self.count + 1, self.window_size)
|
||||
|
||||
def get_average(self) -> float:
|
||||
if self.count == 0:
|
||||
return float('nan')
|
||||
return self.sum / self.count
|
||||
|
||||
|
||||
class CallbackReader:
|
||||
"""Wraps a file, but overrides the read method to also
|
||||
call a callback function with the number of bytes read so far."""
|
||||
|
||||
def __init__(self, f, callback, *args):
|
||||
self.f = f
|
||||
self.callback = callback
|
||||
self.cb_args = args
|
||||
self.total_read = 0
|
||||
|
||||
def __getattr__(self, attr):
|
||||
return getattr(self.f, attr)
|
||||
|
||||
def read(self, *args, **kwargs):
|
||||
chunk = self.f.read(*args, **kwargs)
|
||||
self.total_read += len(chunk)
|
||||
self.callback(*self.cb_args, self.total_read)
|
||||
return chunk
|
||||
|
||||
|
||||
@contextlib.contextmanager
|
||||
def atomic_write(path: str, mode: str = 'w', buffering: int = -1, encoding: str | None = None, newline: str | None = None,
|
||||
overwrite: bool = False):
|
||||
"""Write to a file atomically using a temporary file in the same directory as the destination file."""
|
||||
dir_name = os.path.dirname(path)
|
||||
|
||||
if not overwrite and os.path.exists(path):
|
||||
raise FileExistsError(f"File '{path}' already exists. To overwrite it, set 'overwrite' to True.")
|
||||
|
||||
with tempfile.NamedTemporaryFile(mode=mode, buffering=buffering, encoding=encoding, newline=newline, dir=dir_name, delete=False) as tmp_file:
|
||||
yield tmp_file
|
||||
tmp_file_name = tmp_file.name
|
||||
os.replace(tmp_file_name, path)
|
||||
|
||||
|
||||
def get_upload_stream(filepath: str, should_compress: bool) -> tuple[io.BufferedIOBase, int]:
|
||||
if not should_compress:
|
||||
file_size = os.path.getsize(filepath)
|
||||
file_stream = open(filepath, "rb")
|
||||
return file_stream, file_size
|
||||
|
||||
# Compress the file on the fly
|
||||
compressed_stream = io.BytesIO()
|
||||
compressor = zstd.ZstdCompressor(level=LOG_COMPRESSION_LEVEL)
|
||||
|
||||
with open(filepath, "rb") as f:
|
||||
compressor.copy_stream(f, compressed_stream)
|
||||
compressed_size = compressed_stream.tell()
|
||||
compressed_stream.seek(0)
|
||||
return compressed_stream, compressed_size
|
||||
|
||||
|
||||
# remove all keys that end in DEPRECATED
|
||||
def strip_deprecated_keys(d):
|
||||
for k in list(d.keys()):
|
||||
if isinstance(k, str):
|
||||
if k.endswith('DEPRECATED'):
|
||||
d.pop(k)
|
||||
elif isinstance(d[k], dict):
|
||||
strip_deprecated_keys(d[k])
|
||||
return d
|
||||
|
||||
|
||||
def run_cmd(cmd: list[str], cwd=None, env=None) -> str:
|
||||
return subprocess.check_output(cmd, encoding='utf8', cwd=cwd, env=env).strip()
|
||||
|
||||
|
||||
def run_cmd_default(cmd: list[str], default: str = "", cwd=None, env=None) -> str:
|
||||
try:
|
||||
return run_cmd(cmd, cwd=cwd, env=env)
|
||||
except subprocess.CalledProcessError:
|
||||
return default
|
||||
|
||||
|
||||
@contextlib.contextmanager
|
||||
def managed_proc(cmd: list[str], env: dict[str, str]):
|
||||
proc = Popen(cmd, env=env, stdout=PIPE, stderr=PIPE)
|
||||
try:
|
||||
yield proc
|
||||
finally:
|
||||
if proc.poll() is None:
|
||||
proc.terminate()
|
||||
try:
|
||||
proc.wait(timeout=5)
|
||||
except TimeoutExpired:
|
||||
proc.kill()
|
||||
|
||||
|
||||
def tabulate(tabular_data, headers=(), tablefmt="simple", floatfmt="g", stralign="left", numalign=None):
|
||||
rows = [list(row) for row in tabular_data]
|
||||
|
||||
def fmt(val):
|
||||
if isinstance(val, str):
|
||||
return val
|
||||
if isinstance(val, (bool, int)):
|
||||
return str(val)
|
||||
try:
|
||||
return format(val, floatfmt)
|
||||
except (TypeError, ValueError):
|
||||
return str(val)
|
||||
|
||||
formatted = [[fmt(c) for c in row] for row in rows]
|
||||
hdrs = [str(h) for h in headers] if headers else None
|
||||
|
||||
ncols = max((len(r) for r in formatted), default=0)
|
||||
if hdrs:
|
||||
ncols = max(ncols, len(hdrs))
|
||||
if ncols == 0:
|
||||
return ""
|
||||
|
||||
for r in formatted:
|
||||
r.extend([""] * (ncols - len(r)))
|
||||
if hdrs:
|
||||
hdrs.extend([""] * (ncols - len(hdrs)))
|
||||
|
||||
widths = [0] * ncols
|
||||
if hdrs:
|
||||
for i in range(ncols):
|
||||
widths[i] = len(hdrs[i])
|
||||
for row in formatted:
|
||||
for i in range(ncols):
|
||||
widths[i] = max(widths[i], max(len(ln) for ln in row[i].split('\n')))
|
||||
|
||||
def _align(s, w):
|
||||
if stralign == "center":
|
||||
return s.center(w)
|
||||
return s.ljust(w)
|
||||
|
||||
if tablefmt == "html":
|
||||
parts = ["<table>"]
|
||||
if hdrs:
|
||||
parts.append("<thead>")
|
||||
parts.append("<tr>" + "".join(f"<th>{h}</th>" for h in hdrs) + "</tr>")
|
||||
parts.append("</thead>")
|
||||
parts.append("<tbody>")
|
||||
for row in formatted:
|
||||
parts.append("<tr>" + "".join(f"<td>{c}</td>" for c in row) + "</tr>")
|
||||
parts.append("</tbody>")
|
||||
parts.append("</table>")
|
||||
return "\n".join(parts)
|
||||
|
||||
if tablefmt == "simple_grid":
|
||||
def _sep(left, mid, right):
|
||||
return left + mid.join("─" * (w + 2) for w in widths) + right
|
||||
|
||||
top, mid_sep, bot = _sep("┌", "┬", "┐"), _sep("├", "┼", "┤"), _sep("└", "┴", "┘")
|
||||
|
||||
def _fmt_row(cells):
|
||||
split = [c.split('\n') for c in cells]
|
||||
nlines = max(len(s) for s in split)
|
||||
for s in split:
|
||||
s.extend([""] * (nlines - len(s)))
|
||||
return ["│" + "│".join(f" {_align(split[i][li], widths[i])} " for i in range(ncols)) + "│" for li in range(nlines)]
|
||||
|
||||
lines = [top]
|
||||
if hdrs:
|
||||
lines.extend(_fmt_row(hdrs))
|
||||
lines.append(mid_sep)
|
||||
for ri, row in enumerate(formatted):
|
||||
lines.extend(_fmt_row(row))
|
||||
lines.append(mid_sep if ri < len(formatted) - 1 else bot)
|
||||
return "\n".join(lines)
|
||||
|
||||
gap = " "
|
||||
lines = []
|
||||
if hdrs:
|
||||
lines.append(gap.join(h.ljust(w) for h, w in zip(hdrs, widths, strict=True)))
|
||||
lines.append(gap.join("-" * w for w in widths))
|
||||
for row in formatted:
|
||||
lines.append(gap.join(_align(row[i], widths[i]) for i in range(ncols)))
|
||||
return "\n".join(lines)
|
||||
|
||||
|
||||
def retry(attempts=3, delay=1.0, ignore_failure=False):
|
||||
def decorator(func):
|
||||
@functools.wraps(func)
|
||||
def wrapper(*args, **kwargs):
|
||||
for _ in range(attempts):
|
||||
try:
|
||||
return func(*args, **kwargs)
|
||||
except Exception:
|
||||
print(f"{func.__name__} failed, trying again")
|
||||
time.sleep(delay)
|
||||
|
||||
if ignore_failure:
|
||||
print(f"{func.__name__} failed after retry")
|
||||
else:
|
||||
raise Exception(f"{func.__name__} failed after retry")
|
||||
return wrapper
|
||||
return decorator
|
||||
1
iqpilot/common/version.h
Normal file
1
iqpilot/common/version.h
Normal file
@@ -0,0 +1 @@
|
||||
#define COMMA_VERSION "IQ.Pilot 1.0c"
|
||||
5
iqpilot/common/version.py
Normal file
5
iqpilot/common/version.py
Normal file
@@ -0,0 +1,5 @@
|
||||
from iqpilot.common.git import get_normalized_origin
|
||||
|
||||
|
||||
def get_version() -> str:
|
||||
return "IQ.Pilot 1.0c"
|
||||
321
iqpilot/docs/CHANGELOG.md
Normal file
321
iqpilot/docs/CHANGELOG.md
Normal file
@@ -0,0 +1,321 @@
|
||||
# IQ.Pilot User Changelog
|
||||
|
||||
## IQ.Pilot 1.0c Changelog
|
||||
|
||||
### Features
|
||||
|
||||
#### Navigate on IQ.Pilot
|
||||
|
||||
- Added Navigate on IQ.Pilot as a complete on-device navigation experience.
|
||||
- Added destination search with Home, Work, and Recent shortcuts.
|
||||
- Added live turn-by-turn guidance with automatic rerouting after a missed turn or route deviation.
|
||||
- Added traffic-aware online routing with automatic traffic refresh, delay tracking, closure awareness, and fresh-route metadata.
|
||||
- Added AMap destination search and routing for configured mainland China devices.
|
||||
- Added an interactive off-road map with panning and current-location display.
|
||||
- Added a split on-road map view so the camera, model path, route, current position, and upcoming maneuver remain visible together.
|
||||
- Added route-aware longitudinal planning for turns, highway exits, and highway forks.
|
||||
- Added route-aware lane-change guidance for supported highway exits.
|
||||
- Added route-commanded automatic turn signals on supported vehicles.
|
||||
- Added optional exit-lane-change assistance with Blind Spot Monitoring awareness.
|
||||
- Improved low-speed maneuver guidance so the model keeps the requested turn path while waiting and carries it through the committed turn.
|
||||
- Added route cancellation and saved-destination management from the Navigate screen in the Konn3kt app.
|
||||
|
||||
#### IQ Speed Assist
|
||||
|
||||
- Added the new IQ Speed Assist architecture.
|
||||
- Added TomTom speed-limit data alongside dashboard, Mapbox, and offline OpenStreetMap sources.
|
||||
- Added percentage-based offset zones for low, medium, and higher speed ranges.
|
||||
- Added direct integration of upcoming speed limits into the longitudinal cruise envelope for earlier and smoother reactions.
|
||||
|
||||
#### Construction Zone Assist
|
||||
|
||||
- Added optional camera-based Construction Zone Assist.
|
||||
- Added road-camera detection of bright orange work-zone barrels and markers.
|
||||
- Added an adjustable work-zone target speed with a 60 mph default.
|
||||
- Added daylight, road-speed, and active-zone state checks to reduce detections from unrelated reflective objects.
|
||||
|
||||
#### Camera Alerts
|
||||
|
||||
- Added direct Flock/ALPR hardware detection from nearby Bluetooth and Wi-Fi radio signatures.
|
||||
- Added warnings for mapped speed cameras, red-light cameras, and ALPR/Flock Safety cameras.
|
||||
- Direct radio detection works without internet access or preexisting map data, and can warn about a nearby unit before appearing in the map database.
|
||||
- Added optional speed-camera slowdown using the detected camera limit and a configurable safety factor.
|
||||
- Added optional haptic feedback on supported Hyundai/Kia/Genesis vehicles when approaching a speed camera.
|
||||
|
||||
#### IQ.Dynamic
|
||||
|
||||
- Added configurable IQ.Dynamic activation for curves, low road speeds, slower or stopped lead vehicles, and model-predicted stops.
|
||||
- Added separate road-speed, lead-speed, and model-stop timing controls.
|
||||
- Added on-device IQ.Dynamic configuration by double-tapping IQ.Dynamic in the longitudinal mode selector.
|
||||
- Added on-road IQ longitudinal-mode cycling through the nucleus icon on BIG UI devices.
|
||||
- Added optional stock-radar blending on supported Volkswagen PQ vehicles for more stable highway following.
|
||||
- Added IQ Force Stops for model-predicted stop lights and stop signs when no lead vehicle is present.
|
||||
- Added adjustable minimum stop length and stopping distance.
|
||||
|
||||
#### General Longitudinal Updates
|
||||
|
||||
- Added end-to-end cruise convergence so the vehicle returns toward the selected cruise speed as the road opens in IQ.Pilot (E2E) mode.
|
||||
- Added Smooth Stops for gentler final braking at regular and model-predicted stops.
|
||||
- Added smoother pull-away behavior and departure chimes when a lead moves or the path opens.
|
||||
- Added an optional Experimental Lead MPC mode.
|
||||
- Added earlier reactions to upcoming curves.
|
||||
|
||||
#### IQ Steering Assistance Behavior (SAB)
|
||||
|
||||
- Added a distinct lateral-only engagement border separate from full lateral-and-longitudinal engagement.
|
||||
- Added Always-On Lateral support through compatible Hyundai LFA buttons.
|
||||
- Added an optional mode that pauses steering torque when the driver takes the wheel and resumes after release.
|
||||
|
||||
#### Lane Changes
|
||||
|
||||
- Added an optional model-based lane edge guard that blocks lane changes when a road edge is detected on the target side.
|
||||
|
||||
#### Lateral Tuning
|
||||
|
||||
- Added configurable steering smoothing, slew limiting, and curvature lookahead.
|
||||
- Improved curve entry and reduced abrupt steering changes while preserving quick avoidance responses.
|
||||
- Added angle-based steering and optional torque blending for Volkswagen vehicles with ALC.
|
||||
- Added Volkswagen PQ HCA7 steering support and live-learned curvature correction on supported MEB vehicles.
|
||||
|
||||
#### Driving Models
|
||||
|
||||
- Added automatic model refresh and redownload when an installed model needs an update.
|
||||
- Added a clear Driving Model Updating state, and engagement now waits until the selected model is ready.
|
||||
|
||||
#### IQ eMac
|
||||
|
||||
- Added IQ eMac for running supported big driving models on an Apple Silicon Mac over USB.
|
||||
- Added a Big Model selector with Off, BRH, Lebowski, and RDF choices.
|
||||
- Added one-time model download and compilation on the Mac, with cached models for later drives.
|
||||
- Added automatic USB network setup with a one-time Mac administrator prompt when required.
|
||||
- Added USB AMD eGPU-dock support with automatic detection and recovery tools.
|
||||
|
||||
#### IQ eMac App
|
||||
|
||||
- Added a native Mac app for starting, monitoring, restarting, and stopping IQ eMac.
|
||||
- Added live connection, model, download, compilation, inference rate, latency, and session status.
|
||||
- Added persistent menu-bar operation with compact live statistics.
|
||||
|
||||
#### Home Screen and Off-Road UI
|
||||
|
||||
- Added a new IQ.Pilot home screen with dedicated Routes, Navigation, Video, and status views.
|
||||
- Added a selectable home-panel widget and an expanded status bar.
|
||||
- Added 60 fps BIG UI presentation and improved Comma 4 visuals.
|
||||
- Added connected Wi-Fi, vehicle state, temperature, Konn3kt status, and installed IQ.OS version displays.
|
||||
- Added automatic mph or km/h selection based on the device location.
|
||||
- Added smoother navigation, transitions, animations, and controls.
|
||||
- Added Polish and expanded translations throughout the Comma 4 UI and IQ.Pilot settings.
|
||||
|
||||
#### On-Road UI
|
||||
|
||||
- Added a glowing orb for the primary lead vehicle.
|
||||
- Added live Konn3kt accent colors across borders, lane lines, controls, and sliders.
|
||||
- Updated the acceleration bar with IQ.Pilot's teal and pink visual style.
|
||||
- Added on-road longitudinal personality selection.
|
||||
- Added a Silent Mode bell control.
|
||||
- Added Night Mode for automatic display sleep after sunset.
|
||||
- Added a gradual volume ramp for immediate warning alerts.
|
||||
- Added screen recording through Konn3kt.
|
||||
|
||||
#### Dashcam and Routes
|
||||
|
||||
- Increased dashcam (qcam) video resolution by 5x.
|
||||
- Added a master dashcam control for route logging, video, and audio.
|
||||
- Added crash-safe recording with recovery after power loss or an interrupted route.
|
||||
- Added an on-device Routes screen with drive details and upload status.
|
||||
- Added model path, steering angle, driver-monitoring state, speed, and cruise-speed overlays to the route viewer in the Konn3kt app.
|
||||
|
||||
#### Live View, Audio, and WebSSH
|
||||
|
||||
- Added live on-road video over cellular/Wi-Fi with the model path overlay.
|
||||
- Added full-resolution HDR driver-camera video on Comma 4.
|
||||
- Added microphone audio and two-way voice communication through Konn3kt.
|
||||
- Added an on-road indicator while Live View is active.
|
||||
- Added road, wide, and driver-camera snapshots.
|
||||
- Added faster camera switching, adaptive video quality, and dual-camera picture-in-picture.
|
||||
- Improved Konn3kt WebSSH connection reliability.
|
||||
|
||||
#### Konn3kt Services
|
||||
|
||||
- Konn3kt now remains available when IQ.Pilot is stopped or cannot open its main UI.
|
||||
- Added remote recovery access over Wi-Fi and cellular.
|
||||
- Added faster reconnection after network or IP-address changes.
|
||||
- Added dedicated route, log, and crash-log uploading.
|
||||
- Added Volkswagen and Tesla odometer display.
|
||||
- Added encrypted device backup and restore.
|
||||
- Added supported Volkswagen coding, diagnostic controls, and EPS flashing through Konn3kt.
|
||||
|
||||
#### Konn3kt Bluetooth Control
|
||||
|
||||
- Added direct Bluetooth control from the Konn3kt app across supported IQ.OS devices.
|
||||
- Added Bluetooth synchronization for vehicle, display, network, navigation, and driving settings.
|
||||
- Added automatic discovery and pairing without manual network configuration.
|
||||
- Added automatic Bluetooth fallback when Wi-Fi or cellular service is unavailable.
|
||||
- Added setup-stage Wi-Fi configuration, channel selection, and installation control.
|
||||
|
||||
#### Volkswagen PQ and MQB
|
||||
|
||||
- Expanded Volkswagen PQ and MQB vehicle support.
|
||||
- Added Volkswagen Passat B7/NMS and SEAT Alhambra Stop-and-Go support.
|
||||
- Added Stop-and-Go and automatic-resume improvements across supported PQ and MQB vehicles.
|
||||
- Added stock-radar blending with IQ.Dynamic on supported PQ vehicles.
|
||||
- Added automatic PQ steering-patch detection and minimum-steering-speed handling.
|
||||
- Added Volkswagen PQ firmware backup, patching, programming, and recovery tools.
|
||||
- Added PQ and MQB steering coding and compatibility checks through Konn3kt.
|
||||
- Added continued PQ and MQB lateral control during cruise faults.
|
||||
- Added an MQB Steering Lockout toggle that reduces low-speed steering torque to prevent LKAS faults on sensitive MQB vehicles.
|
||||
|
||||
#### Volkswagen MEB and MQBevo
|
||||
|
||||
- Added official Volkswagen MEB and MQBevo support.
|
||||
- Added supported Volkswagen ID.3, ID.4, ID.5, and Golf Mk8 configurations through model year 2025.
|
||||
- Added platform-specific steering, zero-speed steering, and ACC display support.
|
||||
- Added vehicle Drive/Park state handling and reliable device wake support.
|
||||
|
||||
#### Toyota and Lexus
|
||||
|
||||
- Added Toyota and Lexus Stop-and-Go support with an optional compatibility mode.
|
||||
- Added SDSU support.
|
||||
- Fixed ignition handling so the device returns off-road in Park.
|
||||
|
||||
#### Hyundai, Kia, and Genesis
|
||||
|
||||
- Added more supported Hyundai, Kia, and Genesis variants.
|
||||
- Added expanded CAN-FD, HDA2, Camera SCC, radar-track, and corner-radar support.
|
||||
- Added Auto Cruise Control and Auto Engage options on compatible vehicles.
|
||||
- Added custom steering maximum and steering-rate controls.
|
||||
- Added lane-change-specific steering-rate controls.
|
||||
- Added speed-camera haptics and Always-On Lateral support through compatible LFA buttons.
|
||||
|
||||
#### Honda, Subaru, and Tesla
|
||||
|
||||
- Added smoother final braking on supported Honda vehicles.
|
||||
- Added Subaru Creep from Standstill.
|
||||
- Added support for additional Tesla configurations and Model Y steering firmware.
|
||||
- Fixed Tesla stock-DAS cancellation so cruise speed remains available for IQ.Pilot longitudinal control.
|
||||
- Added an optional Tesla FSD/Autosteer visualization mode while IQ.Pilot steers.
|
||||
|
||||
#### IQ.OS
|
||||
|
||||
- Updated device firmware to IQ.OS 4.9.7.
|
||||
- Added support for Comma 3, Comma 3X, Comma 4, Konik A1/M, and Mr.One C3/C3 Lite.
|
||||
- Added per-unit Comma 4 display calibration and HDR camera color support.
|
||||
- Reduced Comma 3 cold-boot time from 39 seconds to 21 seconds.
|
||||
- Improved USB link recovery for IQ eMac and eGPU configurations.
|
||||
- Improved Konik A1 audio reliability.
|
||||
- Added assisted GPS acquisition through Konn3kt.
|
||||
|
||||
#### FastSleep and Power Management
|
||||
|
||||
- Added FastSleep deep standby after the vehicle is parked.
|
||||
- Added faster standby when vehicle battery voltage begins to drop.
|
||||
- Added staged low-voltage shutdown protection.
|
||||
- Kept Konn3kt recovery access available while high-power IQ.Pilot services sleep.
|
||||
- Added immediate wake when ignition or charging is detected.
|
||||
|
||||
#### Bluetooth Setup and Controller Support
|
||||
|
||||
- Added zero-touch Bluetooth onboarding with Konn3kt pairing and setup progress.
|
||||
- Added IQ.OS update confirmation through Konn3kt.
|
||||
- Kept Bluetooth controls available independently from the main IQ.Pilot process.
|
||||
|
||||
#### Network, Cellular, and eSIM
|
||||
|
||||
- Added a new Network settings experience on Comma 3 and Comma 4.
|
||||
- Added a direct Wi-Fi Disconnect action.
|
||||
- Added automatic cellular reconnection after APN changes.
|
||||
- Added SIM recovery for Comma 3X devices with a worn tray-presence switch.
|
||||
- Added experimental eSIM setup and profile management through QR or manual activation codes.
|
||||
|
||||
#### Device and Recovery Controls
|
||||
|
||||
- Added Force On-Road for a temporary ten-minute diagnostic session while parked.
|
||||
- Added Update & Reboot on the crash and recovery screen.
|
||||
- Added USB Storage mode to access device storage over USB.
|
||||
|
||||
#### Updater and Installation
|
||||
|
||||
- Added a new IQ.Pilot and IQ.OS update workflow.
|
||||
- Added Predownload Only and Predownload + Preinstall modes.
|
||||
- Added interrupted-installation recovery.
|
||||
|
||||
#### Reliability and Camera Fault Recovery
|
||||
|
||||
- Added independent recovery for navigation and map services.
|
||||
- Added automatic wide-camera fault detection so the road and driver cameras can continue operating, with an on-road warning after fallback.
|
||||
- Improved calibration recovery and protected driving services from map-service communication stalls.
|
||||
|
||||
---
|
||||
|
||||
### Technical Changes
|
||||
|
||||
- Reduced the IQ.Pilot total installation size to just 139.48 MiB.
|
||||
- Removed the legacy openpilot source mirror, compatibility directories, and obsolete source aliases.
|
||||
- Removed every Git submodule from the IQ.Pilot repository.
|
||||
- Updated release builds to vendor the exact pinned component sources and required LFS assets into a self-contained installation without nested Git repositories or private source URLs.
|
||||
- Added navigation-memory handling so important fork and exit guidance remains available when a model output briefly omits it.
|
||||
- Added on-device map rendering and route-state services designed specifically for IQ.Pilot.
|
||||
- Reduced navigation CPU, GPU, and memory overhead so the map can remain open for long drives without competing with the driving model.
|
||||
- Added full offline routing through a packaged Valhalla runtime.
|
||||
- Added offline navigation that can operate without Mapbox or an active internet connection.
|
||||
- Added support for keeping multiple offline regions installed simultaneously.
|
||||
- Added offline raster map tiles for the on-screen map, not only route calculation and road metadata.
|
||||
- Added separate Online On-Screen Maps and Offline On-Screen Maps controls.
|
||||
- Added resumable regional map downloads for unstable cellular and hotspot connections.
|
||||
- Added combined download progress for routing databases and rendered map tiles.
|
||||
- Added automatic recognition of already installed map regions.
|
||||
- Added automatic restoration of missing offline-map data.
|
||||
- Added a pinned IQ.Pilot mapd v2 fork with binary verification and automatic quarantine of incompatible mapd builds.
|
||||
- Added a hosted regional tile-bundle service with a secondary fallback source.
|
||||
- Added background tile decoding and bounded texture caching to keep map work off the UI render path.
|
||||
- Added automatic stock-radar set-speed and following-gap synchronization on supported Volkswagen vehicles.
|
||||
- Hardened Experimental Lead MPC with model-horizon validation and automatic radar-trajectory fallback when model lead data is missing, malformed, or non-finite.
|
||||
- Added Bluetooth-controller commands for testing or controlling Always-On Lateral in Joystick Mode.
|
||||
- Added the unified IQModeld runtime.
|
||||
- Added a native IQModeld bridge for current combined models and legacy split models.
|
||||
- Added fused vision-and-policy execution for supported supercombo bundles.
|
||||
- Added zero-copy camera-frame handling through the current tinygrad runner.
|
||||
- Added combined-artifact, combined-split, fused, tinygrad, and ONNX runner support under one manager.
|
||||
- Added signed and notarized Mac packaging with improved USB transport diagnostics.
|
||||
- Added stable border-crossing detection and last-known-location fallback for IQ Auto Units.
|
||||
- Added tappable branch information in the BIG UI header.
|
||||
- Fixed live language updates and several previously untranslated or malformed translations.
|
||||
- Added qlog-only route visualization, allowing the model path and driving telemetry to be viewed even when only a qlog and qcam are uploaded, and no rlog is available.
|
||||
- Hardened Qualcomm encoder polling and H.264 filtering for more reliable route recording.
|
||||
- Fixed upload queues that could stall behind a missing build-version file.
|
||||
- Fixed route-upload cache invalidation when a file is replaced at the same path, preventing stale upload state from being reused.
|
||||
- Added automatic LocalAPI deployment and configuration.
|
||||
- Fixed periodic Konn3kt disconnects caused by numeric heartbeat parameters terminating the connection writer.
|
||||
- Added authenticated BLE requests, replay protection, and a dedicated settings RPC dispatcher.
|
||||
- Added live propagation of BLE setting changes to the device UI and active IQ.Pilot services.
|
||||
- Added TRW450 ACC handling for the Volkswagen Passat B7/NMS.
|
||||
- Added MQB standstill handling for supported non-EPB ACC vehicles.
|
||||
- Added dedicated PQ radar engagement, cancellation, set-speed, acceleration, and following-gap management.
|
||||
- Added model-year ECU fingerprint checks and expanded Passat identification.
|
||||
- Added explicit Volkswagen car-readiness state handling.
|
||||
- Added on-car MLB longitudinal refinements and HCA steering-status configuration for compatible Audi and Porsche platforms.
|
||||
- Corrected Porsche Macan vehicle selection and Volkswagen-group settings presentation.
|
||||
- Added MEB camera-harness support for lateral control with stock ACC and compatible gateway-harness support for IQ.Pilot longitudinal control.
|
||||
- Added broader Hyundai/Kia/Genesis vehicle parameter and fingerprint diagnostics.
|
||||
- Added guarded Tesla vehicle-bus parsing for supported harness configurations.
|
||||
- Added kernel-level USB 3 logging support.
|
||||
- Added updated USB 3 receive equalization and VGA calibration for improved link stability.
|
||||
- Added 2 GB compressed zram swap for additional memory headroom.
|
||||
- Fixed GPS clock synchronization to interpret and apply timestamps in UTC.
|
||||
- Forced the Qualcomm camera BPS pipeline to its maximum clock for more consistent frame processing.
|
||||
- Disabled unsupported EGL zero-copy paths on Comma 4 while retaining them on compatible Comma 3 and Comma 3X hardware.
|
||||
- Added in-process audio-stream retries so an unavailable Konik A1 audio DSP does not crash-loop the alert service.
|
||||
- Added cached Konn3kt-assisted GPS data with freshness checks for faster GNSS acquisition when no local AssistNow token is configured.
|
||||
- Added measured-voltage power decisions while FastSleep is active.
|
||||
- Added an authenticated Bluetooth GATT transport and RPC dispatcher.
|
||||
- Added live remote CAN streaming from a device into Cabana through Konn3kt.
|
||||
- Updated Cabana and Jotpluggler to current upstream tool foundations while retaining Konn3kt device, route, DBC, and direct remote-stream support.
|
||||
- Added replay support for IQ.Pilot's crash-safe H.264 fragmented-MP4 recordings, including correct container seeking, decoder-delay handling, and multi-frame packet output.
|
||||
- Added a precompiled release pipeline for comma 3, comma 3X, and comma 4.
|
||||
- Added automatic runtime-package bootstrap, revision pinning, and credential reuse for fresh devices and updates.
|
||||
- Added the iq command center for setup, environment checks, builds, quality checks, package synchronization, status, branch switching, updates, service control, fast restart, and optional reboot.
|
||||
- Added an IQ.Pilot process layout built around independent model, navigation, map, uploader, backup, and perception services.
|
||||
- Added clearer on-device build, process-state, and diagnostic output.
|
||||
- Improved Panda SPI NACK handling for more reliable device-to-vehicle communication.
|
||||
- Hardened calibration by clearing invalid saved calibration, preserving stable solutions during excessive spread on supported comma 3/3X hardware, and rejecting implausible camera-height data before model or path projection.
|
||||
- Added validation for every public vehicle platform and route plus expanded deterministic and ARM64 process-replay coverage.
|
||||
5
iqpilot/iq_maps/__init__.py
Normal file
5
iqpilot/iq_maps/__init__.py
Normal file
@@ -0,0 +1,5 @@
|
||||
import os
|
||||
from iqpilot.common.basedir import BASEDIR
|
||||
|
||||
VENDOR_MAPD_BIN_DIR = os.path.join(BASEDIR, "iqpilot/third_party/mapd_pfeiferj")
|
||||
VENDOR_MAPD_PATH = os.path.join(VENDOR_MAPD_BIN_DIR, "mapd")
|
||||
419
iqpilot/iq_maps/orchestrator.py
Executable file
419
iqpilot/iq_maps/orchestrator.py
Executable file
@@ -0,0 +1,419 @@
|
||||
#!/usr/bin/env python3
|
||||
"""
|
||||
Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
|
||||
"""
|
||||
import platform
|
||||
import os
|
||||
import glob
|
||||
import shutil
|
||||
import signal
|
||||
import subprocess
|
||||
import threading
|
||||
import time
|
||||
from datetime import datetime
|
||||
|
||||
import iqpilot.cereal.messaging as messaging
|
||||
from iqpilot.cereal import custom
|
||||
from iqpilot.common.params import Params
|
||||
from iqpilot.common.realtime import Ratekeeper, config_realtime_process
|
||||
from iqpilot.common.swaglog import cloudlog
|
||||
from iqpilot.selfdrive.selfdrived.alertmanager import set_offroad_alert
|
||||
from iqpilot.system.hardware.hw import Paths
|
||||
from iqpilot.iq_maps import VENDOR_MAPD_BIN_DIR, VENDOR_MAPD_PATH
|
||||
from iqpilot.iq_maps.tile_bundle_downloader import TileBundleDownloader, region_bundle_installed
|
||||
from iqpilot.iq_maps.vendor_mapd_installer import VendorMapdInstaller
|
||||
|
||||
OfflineMapAction = custom.MapdInputType
|
||||
_region_sync_worker: threading.Thread | None = None
|
||||
|
||||
_active_proc_lock = threading.Lock()
|
||||
_active_proc: subprocess.Popen | None = None
|
||||
_shutdown = threading.Event()
|
||||
_tile_downloader: TileBundleDownloader | None = None
|
||||
|
||||
|
||||
def _vendor_fetch_pidfile() -> str:
|
||||
return os.path.join(Paths.mapd_root(), ".vendor_fetch.pid")
|
||||
|
||||
|
||||
def _pid_is_vendor_fetch(pid: int) -> bool:
|
||||
try:
|
||||
with open(f"/proc/{pid}/cmdline", "rb") as f:
|
||||
cmdline = f.read()
|
||||
except OSError:
|
||||
return False
|
||||
return VENDOR_MAPD_PATH.encode() in cmdline
|
||||
|
||||
|
||||
def _reap_orphaned_vendor_fetch() -> None:
|
||||
pidfile = _vendor_fetch_pidfile()
|
||||
try:
|
||||
with open(pidfile) as f:
|
||||
pid = int(f.read().strip())
|
||||
except (OSError, ValueError):
|
||||
return
|
||||
try:
|
||||
if _pid_is_vendor_fetch(pid):
|
||||
cloudlog.warning(f"iq_maps: reaping orphaned vendor-fetch mapd pid={pid} from a prior run")
|
||||
os.kill(pid, signal.SIGTERM)
|
||||
for _ in range(20):
|
||||
time.sleep(0.1)
|
||||
if not _pid_is_vendor_fetch(pid):
|
||||
break
|
||||
else:
|
||||
os.kill(pid, signal.SIGKILL)
|
||||
except ProcessLookupError:
|
||||
pass
|
||||
finally:
|
||||
try:
|
||||
os.remove(pidfile)
|
||||
except OSError:
|
||||
pass
|
||||
|
||||
|
||||
def _kill_active_proc() -> None:
|
||||
with _active_proc_lock:
|
||||
proc = _active_proc
|
||||
if proc is None or proc.poll() is not None:
|
||||
return
|
||||
proc.terminate()
|
||||
try:
|
||||
proc.wait(timeout=3)
|
||||
except Exception:
|
||||
proc.kill()
|
||||
try:
|
||||
proc.wait(timeout=2)
|
||||
except Exception:
|
||||
pass
|
||||
|
||||
|
||||
def _handle_shutdown_signal(signum, _frame) -> None:
|
||||
cloudlog.warning(f"iq_maps: mapd_manager received signal {signum}, cleaning up vendor-fetch subprocess")
|
||||
_shutdown.set()
|
||||
_kill_active_proc()
|
||||
if _tile_downloader is not None:
|
||||
_tile_downloader.cancel()
|
||||
worker = _region_sync_worker
|
||||
if worker is not None and worker.is_alive():
|
||||
worker.join(timeout=3)
|
||||
raise SystemExit(0)
|
||||
|
||||
|
||||
def _install_signal_handlers() -> None:
|
||||
signal.signal(signal.SIGINT, _handle_shutdown_signal)
|
||||
signal.signal(signal.SIGTERM, _handle_shutdown_signal)
|
||||
|
||||
|
||||
def ensure_vendor_runtime() -> None:
|
||||
try:
|
||||
VendorMapdInstaller().verify()
|
||||
except Exception:
|
||||
cloudlog.exception("iq_maps: vendor runtime verification failed")
|
||||
|
||||
params = Params()
|
||||
mem_params = Params("/dev/shm/params") if platform.system() != "Darwin" else params
|
||||
|
||||
|
||||
def stale_region_artifacts() -> list[str]:
|
||||
patterns = [
|
||||
f"{Paths.mapd_root()}/db",
|
||||
f"{Paths.mapd_root()}/v*"
|
||||
]
|
||||
stale_paths: list[str] = []
|
||||
for pattern in patterns:
|
||||
for match in glob.glob(pattern):
|
||||
stale_paths.append(match)
|
||||
if os.path.isdir(match):
|
||||
stale_paths.extend(glob.glob(match + '/**', recursive=True))
|
||||
if not os.path.isfile(VENDOR_MAPD_PATH):
|
||||
stale_paths.append(VENDOR_MAPD_PATH)
|
||||
return stale_paths
|
||||
|
||||
|
||||
def purge_stale_region_artifacts(stale_paths: list[str]) -> None:
|
||||
for candidate in stale_paths:
|
||||
if candidate.endswith('/') and os.path.isfile(candidate[:-1]):
|
||||
candidate = candidate[:-1]
|
||||
if os.path.islink(candidate) or os.path.isfile(candidate):
|
||||
os.remove(candidate)
|
||||
elif os.path.isdir(candidate):
|
||||
shutil.rmtree(candidate, ignore_errors=False)
|
||||
|
||||
|
||||
def _compose_region_selector(nations: list[str], states: list[str] | None = None) -> str:
|
||||
requested_paths: list[str] = []
|
||||
for state_code in (states or []):
|
||||
code = str(state_code).strip().upper()
|
||||
if code and code != "ALL":
|
||||
requested_paths.append(f"us_state.{code}")
|
||||
for nation_code in (nations or []):
|
||||
code = str(nation_code).strip().upper()
|
||||
if code:
|
||||
requested_paths.append(f"nation.{code}")
|
||||
return ",".join(requested_paths)
|
||||
|
||||
|
||||
def _fetch_tile_bundles(region_selector: str, abort_check=None) -> None:
|
||||
global _tile_downloader
|
||||
if not params.get_bool("OfflineOSMaps"):
|
||||
return
|
||||
selectors = [part for part in region_selector.split(",") if part]
|
||||
if not selectors:
|
||||
return
|
||||
try:
|
||||
_tile_downloader = TileBundleDownloader(params=params, mem_params=mem_params, abort_check=abort_check)
|
||||
_tile_downloader.download_regions(selectors)
|
||||
except Exception:
|
||||
cloudlog.exception("iq_maps: tile bundle download failed")
|
||||
finally:
|
||||
_tile_downloader = None
|
||||
|
||||
|
||||
def _drive_vendor_fetch(region_selector: str, requested_regions: dict) -> None:
|
||||
global _active_proc
|
||||
proc = None
|
||||
cancelled = False
|
||||
try:
|
||||
mem_params.put("OSMDownloadLocations", requested_regions)
|
||||
proc = subprocess.Popen([VENDOR_MAPD_PATH], cwd=VENDOR_MAPD_BIN_DIR,
|
||||
stdout=subprocess.DEVNULL, stderr=subprocess.DEVNULL,
|
||||
start_new_session=True)
|
||||
with _active_proc_lock:
|
||||
_active_proc = proc
|
||||
with open(_vendor_fetch_pidfile(), "w") as f:
|
||||
f.write(str(proc.pid))
|
||||
|
||||
pm = messaging.PubMaster(["mapdIn"])
|
||||
sm = messaging.SubMaster(["mapdExtendedOut"])
|
||||
time.sleep(4.0)
|
||||
|
||||
for _ in range(10):
|
||||
msg = messaging.new_message("mapdIn")
|
||||
msg.mapdIn.type = OfflineMapAction.download
|
||||
msg.mapdIn.str = region_selector
|
||||
pm.send("mapdIn", msg)
|
||||
time.sleep(0.2)
|
||||
|
||||
started = False
|
||||
deadline = time.monotonic() + 3600.0
|
||||
while time.monotonic() < deadline and not _shutdown.is_set():
|
||||
sm.update(500)
|
||||
dp = sm["mapdExtendedOut"].downloadProgress
|
||||
mem_params.put("OSMDownloadProgress", {
|
||||
"active": bool(dp.active),
|
||||
"total_files": int(dp.totalFiles),
|
||||
"downloaded_files": int(dp.downloadedFiles),
|
||||
})
|
||||
if dp.active:
|
||||
started = True
|
||||
elif started:
|
||||
break
|
||||
if not mem_params.get("OSMDownloadLocations"):
|
||||
cancelled = True
|
||||
cancel = messaging.new_message("mapdIn")
|
||||
cancel.mapdIn.type = OfflineMapAction.cancelDownload
|
||||
pm.send("mapdIn", cancel)
|
||||
break
|
||||
cloudlog.info(f"iq_maps: vendor map download finished for {region_selector}")
|
||||
if not cancelled and not _shutdown.is_set():
|
||||
_fetch_tile_bundles(region_selector, abort_check=lambda: _shutdown.is_set() or not mem_params.get("OSMDownloadLocations"))
|
||||
except Exception:
|
||||
cloudlog.exception("iq_maps: vendor map download failed")
|
||||
finally:
|
||||
try:
|
||||
mem_params.remove("OSMDownloadLocations")
|
||||
except Exception:
|
||||
pass
|
||||
if proc is not None:
|
||||
proc.terminate()
|
||||
try:
|
||||
proc.wait(timeout=5)
|
||||
except Exception:
|
||||
proc.kill()
|
||||
with _active_proc_lock:
|
||||
_active_proc = None
|
||||
try:
|
||||
os.remove(_vendor_fetch_pidfile())
|
||||
except OSError:
|
||||
pass
|
||||
|
||||
|
||||
def queue_region_refresh(nations: list[str], states: list[str] | None = None) -> None:
|
||||
global _region_sync_worker
|
||||
params.put("OsmDownloadedDate", str(datetime.now().timestamp()))
|
||||
params.put_bool("OsmDbUpdatesCheck", False)
|
||||
|
||||
region_selector = _compose_region_selector(nations, states)
|
||||
if not region_selector:
|
||||
cloudlog.warning("iq_maps: no region selected for offline map download")
|
||||
return
|
||||
if _region_sync_worker is not None and _region_sync_worker.is_alive():
|
||||
cloudlog.warning("iq_maps: vendor map download already in progress")
|
||||
return
|
||||
|
||||
requested_regions = {"nations": nations, "states": states or [], "paths": region_selector}
|
||||
cloudlog.info(f"iq_maps: starting vendor map download for {region_selector}")
|
||||
_region_sync_worker = threading.Thread(
|
||||
target=_drive_vendor_fetch,
|
||||
args=(region_selector, requested_regions),
|
||||
daemon=True,
|
||||
)
|
||||
_region_sync_worker.start()
|
||||
|
||||
|
||||
def normalize_region_selection(nations: list[str], states: list[str] | None = None) -> tuple[list[str], list[str]]:
|
||||
normalized_nations = list(nations)
|
||||
normalized_states = list(states or [])
|
||||
lowered_states = {entry.lower() for entry in normalized_states}
|
||||
|
||||
if "US" in normalized_nations and normalized_states and "all" not in lowered_states:
|
||||
normalized_nations = [entry for entry in normalized_nations if entry != "US"]
|
||||
elif normalized_states:
|
||||
normalized_states = [entry for entry in normalized_states if entry.lower() != "all"]
|
||||
|
||||
return normalized_nations, normalized_states
|
||||
|
||||
|
||||
_AUTO_RESTORE_INTERVAL_S = 1800.0
|
||||
_last_auto_restore_t = 0.0
|
||||
|
||||
|
||||
def region_data_missing() -> bool:
|
||||
if not params.get_bool("OsmLocal"):
|
||||
return False
|
||||
if not params.get("OsmDownloadedDate"):
|
||||
return False
|
||||
if glob.glob(f"{Paths.mapd_root()}/db") or glob.glob(f"{Paths.mapd_root()}/v*"):
|
||||
return False
|
||||
if glob.glob(f"{Paths.mapd_root()}/offline/*/*"):
|
||||
return False
|
||||
country = params.get("OsmLocationName", return_default=True)
|
||||
return bool(country)
|
||||
|
||||
|
||||
def configured_states() -> list[str]:
|
||||
try:
|
||||
states = params.get("OsmStateNames")
|
||||
if isinstance(states, bytes):
|
||||
import json as _json
|
||||
states = _json.loads(states.decode("utf-8"))
|
||||
if isinstance(states, str):
|
||||
import json as _json
|
||||
states = _json.loads(states)
|
||||
if isinstance(states, list) and states:
|
||||
return [str(s).strip().upper() for s in states if str(s).strip()]
|
||||
except Exception:
|
||||
pass
|
||||
state = params.get("OsmStateName", return_default=True)
|
||||
return [state] if state else []
|
||||
|
||||
|
||||
def maybe_auto_restore_region() -> None:
|
||||
global _last_auto_restore_t
|
||||
if not region_data_missing():
|
||||
return
|
||||
if _region_sync_worker is not None and _region_sync_worker.is_alive():
|
||||
return
|
||||
now = time.monotonic()
|
||||
if now - _last_auto_restore_t < _AUTO_RESTORE_INTERVAL_S:
|
||||
return
|
||||
_last_auto_restore_t = now
|
||||
country = params.get("OsmLocationName", return_default=True)
|
||||
states = configured_states()
|
||||
nations, states_filtered = normalize_region_selection([country], states)
|
||||
cloudlog.warning(f"iq_maps: configured offline region {country}/{states} has no data on disk; auto-restoring")
|
||||
queue_region_refresh(nations, states_filtered)
|
||||
|
||||
|
||||
_TILE_RESTORE_INTERVAL_S = 1800.0
|
||||
_last_tile_restore_t = 0.0
|
||||
_tile_only_worker: threading.Thread | None = None
|
||||
|
||||
|
||||
def _configured_region_selector() -> str:
|
||||
country = params.get("OsmLocationName", return_default=True)
|
||||
states = configured_states()
|
||||
nations, states_filtered = normalize_region_selection([country] if country else [], states)
|
||||
return _compose_region_selector(nations, states_filtered)
|
||||
|
||||
|
||||
def tile_bundles_missing() -> bool:
|
||||
if not params.get_bool("OfflineOSMaps"):
|
||||
return False
|
||||
selector = _configured_region_selector()
|
||||
if not selector:
|
||||
return False
|
||||
return any(not region_bundle_installed(part) for part in selector.split(",") if part)
|
||||
|
||||
|
||||
def maybe_restore_tile_bundles() -> None:
|
||||
global _last_tile_restore_t, _tile_only_worker
|
||||
if not tile_bundles_missing():
|
||||
return
|
||||
if _region_sync_worker is not None and _region_sync_worker.is_alive():
|
||||
return
|
||||
if _tile_only_worker is not None and _tile_only_worker.is_alive():
|
||||
return
|
||||
now = time.monotonic()
|
||||
if now - _last_tile_restore_t < _TILE_RESTORE_INTERVAL_S:
|
||||
return
|
||||
_last_tile_restore_t = now
|
||||
selector = _configured_region_selector()
|
||||
cloudlog.warning(f"iq_maps: offline map tile bundles missing for {selector}; downloading")
|
||||
_tile_only_worker = threading.Thread(
|
||||
target=_fetch_tile_bundles,
|
||||
args=(selector,),
|
||||
kwargs={"abort_check": _shutdown.is_set},
|
||||
daemon=True,
|
||||
)
|
||||
_tile_only_worker.start()
|
||||
|
||||
|
||||
def sync_osm_request_flags() -> None:
|
||||
maybe_auto_restore_region()
|
||||
maybe_restore_tile_bundles()
|
||||
if params.get_bool("OsmDbUpdatesCheck"):
|
||||
if _region_sync_worker is not None and _region_sync_worker.is_alive():
|
||||
return
|
||||
purge_stale_region_artifacts(stale_region_artifacts())
|
||||
country = params.get("OsmLocationName", return_default=True)
|
||||
states = configured_states()
|
||||
filtered_nations, filtered_states = normalize_region_selection([country], states)
|
||||
queue_region_refresh(filtered_nations, filtered_states)
|
||||
|
||||
if not mem_params.get("OSMDownloadBounds"):
|
||||
mem_params.put("OSMDownloadBounds", "")
|
||||
|
||||
if not mem_params.get("LastGPSPosition"):
|
||||
mem_params.put("LastGPSPosition", "{}")
|
||||
|
||||
|
||||
def run_loop():
|
||||
ensure_vendor_runtime()
|
||||
config_realtime_process([0, 1, 2, 3], 5)
|
||||
|
||||
rk = Ratekeeper(1, print_delay_threshold=None)
|
||||
|
||||
try:
|
||||
os.mkdir(Paths.mapd_root())
|
||||
except FileExistsError:
|
||||
pass
|
||||
except PermissionError:
|
||||
cloudlog.exception(f"iq_maps: failed to make {Paths.mapd_root()}")
|
||||
_reap_orphaned_vendor_fetch()
|
||||
_install_signal_handlers()
|
||||
|
||||
while not _shutdown.is_set():
|
||||
show_alert = stale_region_artifacts() and params.get_bool("OsmLocal")
|
||||
set_offroad_alert("Offroad_OSMUpdateRequired", show_alert, "This alert will be cleared when new maps are downloaded.")
|
||||
|
||||
sync_osm_request_flags()
|
||||
rk.keep_time()
|
||||
|
||||
|
||||
def main():
|
||||
run_loop()
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
516
iqpilot/iq_maps/tile_bundle_downloader.py
Normal file
516
iqpilot/iq_maps/tile_bundle_downloader.py
Normal file
@@ -0,0 +1,516 @@
|
||||
#!/usr/bin/env python3
|
||||
"""
|
||||
Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
|
||||
"""
|
||||
import hashlib
|
||||
import json
|
||||
import platform
|
||||
import threading
|
||||
import time
|
||||
from pathlib import Path
|
||||
|
||||
import requests
|
||||
|
||||
from iqpilot.common.params import Params
|
||||
from iqpilot.common.swaglog import cloudlog
|
||||
from iqpilot.ui.onroad.offline_tiles import offline_map_root
|
||||
|
||||
try:
|
||||
from iqpilot.iq_maps.tiles_auth import get_base_urls as _private_base_urls, get_requests_auth as _private_auth
|
||||
except Exception: # ProprietaryModuleMissing or import errors in stripped builds
|
||||
_private_base_urls = None
|
||||
_private_auth = None
|
||||
|
||||
# Tile bundles live as LFS objects in the PRIVATE repo IQ.Lvbs/iqmaps (R2 is gone).
|
||||
# Anonymous access 404s by design; devices authenticate with the embedded read-only PAT
|
||||
# carried by the closed-source updater bundle (same fetch account as the OS images).
|
||||
# Hugging Face is primary: it is CDN-served, so device downloads no longer come off the
|
||||
# gitea box's home uplink. The gitea copies stay as failover -- if HF ever suspends the
|
||||
# repo the fleet silently falls back instead of losing maps entirely.
|
||||
HF_TILE_BUNDLE_BASE_URL = "https://huggingface.co/datasets/T3vl/iqmaps/resolve/main"
|
||||
DEFAULT_TILE_BUNDLE_BASE_URL = "https://git.konn3kt.com/IQ.Lvbs/iqmaps/raw/branch/master"
|
||||
FALLBACK_TILE_BUNDLE_BASE_URL = "https://gitlvb.teallvbs.xyz/IQ.Lvbs/iqmaps/raw/branch/master"
|
||||
|
||||
# Gitea /raw NEVER returns LFS content -- it returns this pointer, and the real bytes come
|
||||
# from the LFS batch API (see _resolve_object_url).
|
||||
LFS_POINTER_MAGIC = b"version https://git-lfs"
|
||||
BASE_URL_PARAM = "OfflineTilesBaseUrl"
|
||||
PROGRESS_PARAM = "OfflineTilesDownloadProgress"
|
||||
REQUEST_PARAM = "OfflineTilesDownloadRequest"
|
||||
CHUNK_BYTES = 1 << 20
|
||||
# must match scripts/iqpilot/tile_factory/upload_bundles_lfs.py
|
||||
PART_BYTES = 90 * 1024 * 1024
|
||||
HTTP_TIMEOUT_S = 30.0
|
||||
STREAM_RETRIES = 8
|
||||
|
||||
|
||||
def candidate_base_urls(params: Params) -> list[str]:
|
||||
override = params.get(BASE_URL_PARAM)
|
||||
if isinstance(override, bytes):
|
||||
override = override.decode("utf-8", errors="ignore")
|
||||
override = (override or "").strip()
|
||||
if override:
|
||||
return [override.rstrip("/")]
|
||||
# HF first (CDN, and it keeps device traffic off the gitea box's uplink); the bundle's
|
||||
# own endpoints and the self-hosted defaults follow as failover.
|
||||
urls: list[str] = [HF_TILE_BUNDLE_BASE_URL]
|
||||
if _private_base_urls is not None:
|
||||
try:
|
||||
urls.extend(url.rstrip("/") for url in _private_base_urls())
|
||||
except Exception:
|
||||
pass
|
||||
urls.append(DEFAULT_TILE_BUNDLE_BASE_URL)
|
||||
urls.append(FALLBACK_TILE_BUNDLE_BASE_URL)
|
||||
seen: set[str] = set()
|
||||
return [u for u in urls if not (u in seen or seen.add(u))]
|
||||
|
||||
|
||||
def _is_hf(url: str) -> bool:
|
||||
return "huggingface.co" in url.lower()
|
||||
|
||||
|
||||
def _maps_auth_module():
|
||||
"""The read PAT lives in the compiled updater bundle (never in this open file)."""
|
||||
try:
|
||||
from iqpilot.system.proprietary_runtime._verified_import import import_verified_module
|
||||
return import_verified_module("iqpilot_updater_private", "iqpilot_private.updater.git_remote")
|
||||
except Exception:
|
||||
pass
|
||||
try:
|
||||
import importlib
|
||||
import os
|
||||
import sys
|
||||
root = os.path.abspath(os.path.join(os.path.dirname(__file__), "..", ".."))
|
||||
bundle_python = os.path.join(root, "artifacts", "iqpilot_updater_private", "python")
|
||||
if os.path.isdir(bundle_python):
|
||||
if bundle_python not in sys.path:
|
||||
sys.path.insert(0, bundle_python)
|
||||
return importlib.import_module("iqpilot_private.updater.git_remote")
|
||||
except Exception:
|
||||
pass
|
||||
return None
|
||||
|
||||
|
||||
def request_headers(url: str) -> dict:
|
||||
mod = _maps_auth_module()
|
||||
if mod is not None:
|
||||
if _is_hf(url):
|
||||
# HF wants a bearer token, not basic auth; a build whose bundle predates HF
|
||||
# hosting simply gets nothing here and falls through to the gitea mirrors.
|
||||
try:
|
||||
token = mod.map_tiles_hf_token()
|
||||
if token:
|
||||
return {"Authorization": f"Bearer {token}"}
|
||||
except Exception:
|
||||
pass
|
||||
return {}
|
||||
try:
|
||||
headers = mod.map_tiles_headers(url)
|
||||
if headers:
|
||||
return headers
|
||||
except Exception:
|
||||
pass
|
||||
try:
|
||||
from iqpilot.common.git_creds import get_credentials
|
||||
creds = get_credentials()
|
||||
if creds and all(creds) and "/iq.lvbs/iqmaps" in url.lower():
|
||||
import base64
|
||||
return {"Authorization": "Basic " + base64.b64encode(f"{creds[0]}:{creds[1]}".encode()).decode()}
|
||||
except Exception:
|
||||
pass
|
||||
return {}
|
||||
|
||||
|
||||
def request_auth() -> tuple[str, str] | None:
|
||||
if _private_auth is None:
|
||||
return None
|
||||
try:
|
||||
return _private_auth()
|
||||
except Exception:
|
||||
return None
|
||||
|
||||
|
||||
def _lfs_endpoint(base_url: str) -> str:
|
||||
"""<host>/<owner>/<repo>/raw/branch/<b> -> <host>/<owner>/<repo>.git/info/lfs"""
|
||||
return base_url.split("/raw/", 1)[0] + ".git/info/lfs"
|
||||
|
||||
|
||||
def _resolve_oid_url(session: requests.Session, base_url: str, oid: str, size: int,
|
||||
headers: dict) -> tuple[str, dict]:
|
||||
"""Bundles are stored as bare LFS objects addressed by oid from the index -- no pointer
|
||||
files, because committing one per part meant hundreds of concurrent commits per branch."""
|
||||
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", **headers},
|
||||
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 _resolve_object_url(session: requests.Session, url: str, headers: dict) -> tuple[str, dict]:
|
||||
"""Follow a Gitea LFS pointer to the real (pre-signed) object URL.
|
||||
|
||||
Returns the URL to stream plus any extra headers it needs. A plain host that serves the
|
||||
bytes directly (local test server, static mirror) resolves to itself unchanged."""
|
||||
probe = session.get(url, headers={**headers, "Accept-Encoding": None}, stream=True,
|
||||
timeout=HTTP_TIMEOUT_S)
|
||||
probe.raise_for_status()
|
||||
# A host that serves the bytes itself still needs the caller's auth on the real GET --
|
||||
# returning {} here sends the download out anonymous and a private host answers 401.
|
||||
if int(probe.headers.get("content-length") or 0) >= 1024:
|
||||
probe.close()
|
||||
return url, dict(headers)
|
||||
body = probe.content
|
||||
probe.close()
|
||||
if not body.startswith(LFS_POINTER_MAGIC):
|
||||
return url, dict(headers)
|
||||
|
||||
meta = dict(line.split(" ", 1) for line in body.decode().strip().splitlines() if " " in line)
|
||||
oid = meta["oid"].split(":", 1)[1]
|
||||
size = int(meta["size"])
|
||||
lfs_base = url.split("/raw/", 1)[0] + ".git/info/lfs"
|
||||
batch = session.post(f"{lfs_base}/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", **headers},
|
||||
timeout=HTTP_TIMEOUT_S)
|
||||
batch.raise_for_status()
|
||||
action = batch.json()["objects"][0]["actions"]["download"]
|
||||
return action["href"], action.get("header", {})
|
||||
|
||||
|
||||
def fetch_index(base_url: str, session: requests.Session) -> dict:
|
||||
index_url = f"{base_url}/index.json"
|
||||
headers = request_headers(index_url)
|
||||
# requests' auth= rewrites the Authorization header, so only fall back to it when the
|
||||
# closed-source bundle gave us nothing.
|
||||
response = session.get(index_url, timeout=HTTP_TIMEOUT_S, headers=headers,
|
||||
auth=None if headers else request_auth())
|
||||
response.raise_for_status()
|
||||
index = response.json()
|
||||
regions = index.get("regions")
|
||||
if not isinstance(regions, dict):
|
||||
raise ValueError("tile bundle index has no regions")
|
||||
return regions
|
||||
|
||||
|
||||
def region_bundle_dir(selector: str) -> Path:
|
||||
return offline_map_root() / "regions" / selector
|
||||
|
||||
|
||||
def region_bundle_path(selector: str) -> Path:
|
||||
return region_bundle_dir(selector) / "tiles" / "offline.mbtiles"
|
||||
|
||||
|
||||
def region_valhalla_path(selector: str) -> Path:
|
||||
# valhalla mmaps this tar in place, so it stays uncompressed on disk
|
||||
return region_bundle_dir(selector) / "valhalla" / "tiles.tar"
|
||||
|
||||
|
||||
def region_valhalla_installed(selector: str) -> bool:
|
||||
return region_valhalla_path(selector).exists()
|
||||
|
||||
|
||||
def installed_valhalla_selectors() -> list[str]:
|
||||
regions_root = offline_map_root() / "regions"
|
||||
if not regions_root.exists():
|
||||
return []
|
||||
return sorted(
|
||||
child.name for child in regions_root.iterdir()
|
||||
if child.is_dir() and (child / "valhalla" / "tiles.tar").exists()
|
||||
)
|
||||
|
||||
|
||||
def region_bundle_installed(selector: str) -> bool:
|
||||
return region_bundle_path(selector).exists()
|
||||
|
||||
|
||||
def installed_region_selectors() -> list[str]:
|
||||
regions_root = offline_map_root() / "regions"
|
||||
if not regions_root.exists():
|
||||
return []
|
||||
return sorted(
|
||||
child.name for child in regions_root.iterdir()
|
||||
if child.is_dir() and (child / "tiles" / "offline.mbtiles").exists()
|
||||
)
|
||||
|
||||
|
||||
def _hash_existing(path: Path) -> tuple["hashlib._Hash", int]:
|
||||
digest = hashlib.sha256()
|
||||
size = 0
|
||||
with open(path, "rb") as f:
|
||||
while True:
|
||||
chunk = f.read(CHUNK_BYTES)
|
||||
if not chunk:
|
||||
break
|
||||
digest.update(chunk)
|
||||
size += len(chunk)
|
||||
return digest, size
|
||||
|
||||
|
||||
def _write_manifest(selector: str, entry: dict) -> None:
|
||||
manifest = {
|
||||
"region": selector,
|
||||
"version": entry.get("version", ""),
|
||||
"mbtiles": {
|
||||
"bounds": entry.get("bounds", ""),
|
||||
"minzoom": entry.get("minzoom"),
|
||||
"maxzoom": entry.get("maxzoom"),
|
||||
"bytes": entry.get("bytes"),
|
||||
"sha256": entry.get("sha256", ""),
|
||||
},
|
||||
}
|
||||
if entry.get("day_path"):
|
||||
manifest["mbtiles_day"] = {
|
||||
"bytes": entry.get("day_bytes"),
|
||||
"sha256": entry.get("day_sha256", ""),
|
||||
}
|
||||
manifest_path = region_bundle_dir(selector) / "manifest.json"
|
||||
manifest_path.parent.mkdir(parents=True, exist_ok=True)
|
||||
manifest_path.write_text(json.dumps(manifest, indent=2))
|
||||
|
||||
|
||||
class TileBundleDownloader:
|
||||
|
||||
def __init__(self, params: Params | None = None, mem_params: Params | None = None,
|
||||
abort_check=None):
|
||||
self.params = params if params is not None else Params()
|
||||
if mem_params is not None:
|
||||
self.mem_params = mem_params
|
||||
else:
|
||||
self.mem_params = Params("/dev/shm/params") if platform.system() != "Darwin" else self.params
|
||||
self.session = requests.Session()
|
||||
self._cancelled = threading.Event()
|
||||
self._abort_check = abort_check
|
||||
|
||||
def cancel(self) -> None:
|
||||
self._cancelled.set()
|
||||
|
||||
def _should_abort(self) -> bool:
|
||||
if self._cancelled.is_set():
|
||||
return True
|
||||
if not self.mem_params.get(REQUEST_PARAM):
|
||||
self._cancelled.set()
|
||||
return True
|
||||
if self._abort_check is not None and self._abort_check():
|
||||
self._cancelled.set()
|
||||
return True
|
||||
return False
|
||||
|
||||
def _publish_progress(self, region: str, downloaded: int, total: int, active: bool) -> None:
|
||||
self.mem_params.put(PROGRESS_PARAM, {
|
||||
"active": active,
|
||||
"region": region,
|
||||
"downloaded_bytes": int(downloaded),
|
||||
"total_bytes": int(total),
|
||||
})
|
||||
|
||||
def _download_one(self, selector: str, entry: dict, base_url: str,
|
||||
progress_offset: int, progress_total: int) -> bool:
|
||||
night_path = region_bundle_path(selector)
|
||||
ok = self._download_file(
|
||||
selector, base_url, entry["path"], int(entry.get("bytes", 0)),
|
||||
str(entry.get("sha256", "")).strip().lower(), night_path,
|
||||
progress_offset, progress_total, int(entry.get("parts", 1)), entry.get("objects"),
|
||||
)
|
||||
if not ok:
|
||||
return False
|
||||
if entry.get("day_path"):
|
||||
day_ok = self._download_file(
|
||||
selector, base_url, entry["day_path"], int(entry.get("day_bytes", 0)),
|
||||
str(entry.get("day_sha256", "")).strip().lower(),
|
||||
night_path.with_name("offline_day.mbtiles"),
|
||||
progress_offset + int(entry.get("bytes", 0)), progress_total,
|
||||
int(entry.get("day_parts", 1)), entry.get("day_objects"),
|
||||
)
|
||||
if not day_ok:
|
||||
cloudlog.warning(f"iq_maps: day-style bundle failed for {selector}; night set installed")
|
||||
if entry.get("valhalla_path"):
|
||||
# routing is additive: a region whose extract is missing or corrupt must still end up
|
||||
# with a usable map rather than failing the whole download
|
||||
try:
|
||||
nav_ok = self._download_file(
|
||||
selector, base_url, entry["valhalla_path"], int(entry.get("valhalla_bytes", 0)),
|
||||
str(entry.get("valhalla_sha256", "")).strip().lower(),
|
||||
region_valhalla_path(selector),
|
||||
progress_offset + int(entry.get("bytes", 0)) + int(entry.get("day_bytes", 0)),
|
||||
progress_total, 1, entry.get("valhalla_objects"),
|
||||
)
|
||||
except Exception as exc:
|
||||
nav_ok = False
|
||||
cloudlog.warning(f"iq_maps: routing extract errored for {selector}: {exc}")
|
||||
if not nav_ok:
|
||||
cloudlog.warning(f"iq_maps: routing extract failed for {selector}; map tiles installed")
|
||||
_write_manifest(selector, entry)
|
||||
cloudlog.info(f"iq_maps: installed tile bundle {selector}")
|
||||
return True
|
||||
|
||||
def _download_file(self, selector: str, base_url: str, remote_path: str, expected_bytes: int,
|
||||
expected_sha: str, final_path: Path,
|
||||
progress_offset: int, progress_total: int, parts: int = 1,
|
||||
objects: list | None = None) -> bool:
|
||||
# Bundles are published as <name>.pNN because Cloudflare caps proxied bodies at ~100MB.
|
||||
# They stream back-to-back into ONE .part file: concatenating afterwards would need
|
||||
# double the free space, which devices do not have.
|
||||
base = f"{base_url}/{remote_path.lstrip('/')}"
|
||||
count = len(objects) if objects else parts
|
||||
if objects and not _is_hf(base_url):
|
||||
urls = [None] * count # gitea: resolved per-attempt from the oid
|
||||
else:
|
||||
# HF (and plain mirrors) serve the same chunks as ordinary .pNN files
|
||||
urls = [base] if count <= 1 else [f"{base}.p{i:02d}" for i in range(count)]
|
||||
part_path = final_path.with_name(final_path.name + ".part")
|
||||
part_path.parent.mkdir(parents=True, exist_ok=True)
|
||||
|
||||
downloaded = 0
|
||||
digest = hashlib.sha256()
|
||||
last_error: Exception | None = None
|
||||
for attempt in range(STREAM_RETRIES):
|
||||
if self._should_abort():
|
||||
cloudlog.warning(f"iq_maps: tile bundle download cancelled for {selector}")
|
||||
return False
|
||||
if attempt:
|
||||
time.sleep(min(30.0, 2.0 * attempt))
|
||||
try:
|
||||
digest = hashlib.sha256()
|
||||
resume_from = 0
|
||||
if part_path.exists():
|
||||
digest, resume_from = _hash_existing(part_path)
|
||||
if expected_bytes and resume_from > expected_bytes:
|
||||
part_path.unlink()
|
||||
digest = hashlib.sha256()
|
||||
resume_from = 0
|
||||
|
||||
# every part but the last is exactly PART_BYTES, so a byte offset maps to a part index
|
||||
first_part = resume_from // PART_BYTES if len(urls) > 1 else 0
|
||||
skip_in_part = resume_from - first_part * PART_BYTES if len(urls) > 1 else resume_from
|
||||
downloaded = resume_from
|
||||
mode = "ab" if resume_from else "wb"
|
||||
with open(part_path, mode) as f:
|
||||
for index in range(first_part, len(urls)):
|
||||
if objects and not _is_hf(base_url):
|
||||
url_headers = request_headers(base_url)
|
||||
auth = None if url_headers else request_auth()
|
||||
object_url, object_headers = _resolve_oid_url(
|
||||
self.session, base_url, objects[index]["oid"], int(objects[index]["size"]),
|
||||
url_headers)
|
||||
else:
|
||||
url = urls[index]
|
||||
url_headers = request_headers(url)
|
||||
auth = None if url_headers else request_auth()
|
||||
# Re-resolve per part: a pre-signed LFS object URL can expire mid-download.
|
||||
object_url, object_headers = _resolve_object_url(self.session, url, url_headers)
|
||||
headers = dict(object_headers)
|
||||
offset = skip_in_part if index == first_part else 0
|
||||
if offset:
|
||||
headers["Range"] = f"bytes={offset}-"
|
||||
response = self.session.get(object_url, headers=headers, stream=True,
|
||||
timeout=HTTP_TIMEOUT_S, auth=auth)
|
||||
if offset and response.status_code != 206:
|
||||
# server ignored the range: restart this whole file cleanly
|
||||
f.close()
|
||||
part_path.unlink(missing_ok=True)
|
||||
raise requests.RequestException(f"range not honoured for part {index}")
|
||||
response.raise_for_status()
|
||||
for chunk in response.iter_content(chunk_size=CHUNK_BYTES):
|
||||
if self._should_abort():
|
||||
cloudlog.warning(f"iq_maps: tile bundle download cancelled for {selector}")
|
||||
return False
|
||||
f.write(chunk)
|
||||
digest.update(chunk)
|
||||
downloaded += len(chunk)
|
||||
self._publish_progress(selector, progress_offset + downloaded, progress_total, active=True)
|
||||
break
|
||||
except requests.RequestException as exc:
|
||||
last_error = exc
|
||||
cloudlog.warning(f"iq_maps: tile bundle stream interrupted for {selector} "
|
||||
+ f"(attempt {attempt + 1}/{STREAM_RETRIES}): {exc}")
|
||||
else:
|
||||
raise requests.RequestException(f"stream failed after {STREAM_RETRIES} attempts") from last_error
|
||||
|
||||
if expected_bytes and downloaded != expected_bytes:
|
||||
cloudlog.error(f"iq_maps: tile bundle size mismatch for {selector}: {downloaded} != {expected_bytes}")
|
||||
part_path.unlink(missing_ok=True)
|
||||
return False
|
||||
if expected_sha and digest.hexdigest() != expected_sha:
|
||||
cloudlog.error(f"iq_maps: tile bundle sha256 mismatch for {selector}")
|
||||
part_path.unlink(missing_ok=True)
|
||||
return False
|
||||
|
||||
part_path.replace(final_path)
|
||||
return True
|
||||
|
||||
def download_regions(self, selectors: list[str]) -> bool:
|
||||
self._cancelled.clear()
|
||||
ok = True
|
||||
try:
|
||||
self.mem_params.put(REQUEST_PARAM, {"regions": list(selectors)})
|
||||
regions = None
|
||||
base_url = ""
|
||||
for candidate in candidate_base_urls(self.params):
|
||||
try:
|
||||
regions = fetch_index(candidate, self.session)
|
||||
base_url = candidate
|
||||
break
|
||||
except (requests.RequestException, ValueError, json.JSONDecodeError):
|
||||
cloudlog.warning(f"iq_maps: tile bundle index unavailable at {candidate}")
|
||||
if regions is None:
|
||||
cloudlog.error("iq_maps: no tile bundle host reachable")
|
||||
return False
|
||||
|
||||
wanted: list[tuple[str, dict]] = []
|
||||
for selector in selectors:
|
||||
entry = regions.get(selector)
|
||||
if entry is None:
|
||||
cloudlog.warning(f"iq_maps: no tile bundle published for {selector}")
|
||||
ok = False
|
||||
continue
|
||||
if region_bundle_installed(selector) and self._installed_matches(selector, entry):
|
||||
continue
|
||||
wanted.append((selector, entry))
|
||||
|
||||
progress_total = sum(int(entry.get("bytes", 0)) + int(entry.get("day_bytes", 0)) for _, entry in wanted)
|
||||
progress_offset = 0
|
||||
for selector, entry in wanted:
|
||||
if self._should_abort():
|
||||
return False
|
||||
try:
|
||||
if not self._download_one(selector, entry, base_url, progress_offset, progress_total):
|
||||
ok = False
|
||||
except (requests.RequestException, OSError):
|
||||
cloudlog.exception(f"iq_maps: tile bundle download failed for {selector}")
|
||||
ok = False
|
||||
progress_offset += int(entry.get("bytes", 0)) + int(entry.get("day_bytes", 0))
|
||||
return ok
|
||||
finally:
|
||||
self._publish_progress("", 0, 0, active=False)
|
||||
try:
|
||||
self.mem_params.remove(REQUEST_PARAM)
|
||||
except Exception:
|
||||
pass
|
||||
|
||||
@staticmethod
|
||||
def _installed_matches(selector: str, entry: dict) -> bool:
|
||||
manifest_path = region_bundle_dir(selector) / "manifest.json"
|
||||
try:
|
||||
manifest = json.loads(manifest_path.read_text())
|
||||
except (OSError, json.JSONDecodeError):
|
||||
return False
|
||||
installed_sha = str(manifest.get("mbtiles", {}).get("sha256", "")).strip().lower()
|
||||
expected_sha = str(entry.get("sha256", "")).strip().lower()
|
||||
if not expected_sha or installed_sha != expected_sha:
|
||||
return False
|
||||
if entry.get("day_path"):
|
||||
day_file = region_bundle_dir(selector) / "tiles" / "offline_day.mbtiles"
|
||||
installed_day = str(manifest.get("mbtiles_day", {}).get("sha256", "")).strip().lower()
|
||||
expected_day = str(entry.get("day_sha256", "")).strip().lower()
|
||||
if not day_file.exists() or installed_day != expected_day:
|
||||
return False
|
||||
return True
|
||||
10
iqpilot/iq_maps/tiles_auth.py
Normal file
10
iqpilot/iq_maps/tiles_auth.py
Normal file
@@ -0,0 +1,10 @@
|
||||
#!/usr/bin/env python3
|
||||
"""
|
||||
Copyright (c) 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.maps.git_auth")
|
||||
except ProprietaryModuleMissing:
|
||||
from iqpilot.maps_private_src.git_auth import *
|
||||
70
iqpilot/iq_maps/update_vendor_version.py
Executable file
70
iqpilot/iq_maps/update_vendor_version.py
Executable file
@@ -0,0 +1,70 @@
|
||||
#!/usr/bin/env python3
|
||||
"""
|
||||
Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
|
||||
"""
|
||||
import argparse
|
||||
import os
|
||||
import re
|
||||
import sys
|
||||
|
||||
from iqpilot.common.basedir import BASEDIR
|
||||
from iqpilot.iq_maps import VENDOR_MAPD_PATH
|
||||
from iqpilot.iq_maps.vendor_mapd_installer import (
|
||||
VENDOR_RELEASE_TAG,
|
||||
sha256_of_file,
|
||||
)
|
||||
|
||||
_RELEASE_SYMBOL = "VENDOR_RELEASE_TAG"
|
||||
_INSTALLER_SRC = os.path.join(BASEDIR, "iqpilot", "iq_maps", "vendor_mapd_installer.py")
|
||||
HASH_FILE = os.path.join(BASEDIR, "iqpilot", "iq_maps", "tests", "mapd_hash")
|
||||
_HASH_FILE = HASH_FILE
|
||||
_TAG_ASSIGN = re.compile(rf'^{_RELEASE_SYMBOL}\s*=\s*["\'][^"\']*["\']', re.MULTILINE)
|
||||
|
||||
|
||||
def rewrite_pinned_tag(new_tag: str) -> bool:
|
||||
with open(_INSTALLER_SRC) as f:
|
||||
src = f.read()
|
||||
|
||||
patched, count = _TAG_ASSIGN.subn(f'{_RELEASE_SYMBOL} = "{new_tag}"', src, count=1)
|
||||
if count != 1:
|
||||
print(f"could not locate the {_RELEASE_SYMBOL} assignment in {_INSTALLER_SRC}; nothing written")
|
||||
return False
|
||||
|
||||
with open(_INSTALLER_SRC, "w") as f:
|
||||
f.write(patched)
|
||||
print(f"pinned {_RELEASE_SYMBOL} -> {new_tag}")
|
||||
return True
|
||||
|
||||
|
||||
def refresh_hash_file() -> None:
|
||||
digest = sha256_of_file(VENDOR_MAPD_PATH)
|
||||
with open(_HASH_FILE, "w") as f:
|
||||
f.write(digest)
|
||||
print(f"wrote binary hash {digest} -> {_HASH_FILE}")
|
||||
|
||||
|
||||
def main() -> int:
|
||||
parser = argparse.ArgumentParser(description="Pin a new mapd release tag and refresh its hash")
|
||||
parser.add_argument("--new_ver", type=str, help='e.g. --new_ver "v2.1.0"')
|
||||
args = parser.parse_args()
|
||||
|
||||
if not args.new_ver:
|
||||
parser.print_help()
|
||||
print(f'\ncurrently pinned: {VENDOR_RELEASE_TAG} (unchanged)')
|
||||
return 0
|
||||
|
||||
target = args.new_ver.strip()
|
||||
if target == VENDOR_RELEASE_TAG:
|
||||
reply = input(f"{target} is already the pinned tag — re-run anyway? (y/N): ").strip().lower()
|
||||
if reply != "y":
|
||||
print("aborted; nothing changed")
|
||||
return 0
|
||||
|
||||
if not rewrite_pinned_tag(target):
|
||||
return 1
|
||||
refresh_hash_file()
|
||||
return 0
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
sys.exit(main())
|
||||
100
iqpilot/iq_maps/vendor_mapd_installer.py
Executable file
100
iqpilot/iq_maps/vendor_mapd_installer.py
Executable file
@@ -0,0 +1,100 @@
|
||||
#!/usr/bin/env python3
|
||||
"""
|
||||
Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
|
||||
"""
|
||||
import hashlib
|
||||
import os
|
||||
import sys
|
||||
|
||||
from iqpilot.common.basedir import BASEDIR
|
||||
from iqpilot.common.params import Params
|
||||
from iqpilot.common.spinner import Spinner
|
||||
from iqpilot.common.swaglog import cloudlog
|
||||
from iqpilot.iq_maps import VENDOR_MAPD_PATH
|
||||
import iqpilot.system.sentry as sentry
|
||||
|
||||
VENDOR_RELEASE_TAG = "v2.0.6-iq1"
|
||||
|
||||
_VERSION_PARAM = "MapdVersion"
|
||||
_HASH_FILE = os.path.join(BASEDIR, "iqpilot", "iq_maps", "tests", "mapd_hash")
|
||||
QUARANTINE_PATH = VENDOR_MAPD_PATH + ".quarantined"
|
||||
|
||||
|
||||
def sha256_of_file(path: str) -> str:
|
||||
digest = hashlib.sha256()
|
||||
with open(path, "rb") as handle:
|
||||
for block in iter(lambda: handle.read(1 << 20), b""):
|
||||
digest.update(block)
|
||||
return digest.hexdigest()
|
||||
|
||||
|
||||
def stamp_vendor_version(version: str, params: Params | None = None) -> None:
|
||||
(params or Params()).put(_VERSION_PARAM, version)
|
||||
|
||||
|
||||
class VendorMapdInstaller:
|
||||
def __init__(self, spinner_ref: Spinner | None = None, params: Params | None = None):
|
||||
self._spinner = spinner_ref
|
||||
self._params = params if params is not None else Params()
|
||||
|
||||
def get_installed_version(self) -> str:
|
||||
return str(self._params.get(_VERSION_PARAM) or "")
|
||||
|
||||
def verify(self) -> bool:
|
||||
expected = self._expected_hash()
|
||||
if not expected:
|
||||
cloudlog.error("iq_maps: pinned mapd hash missing, vendor binary cannot be verified")
|
||||
return False
|
||||
|
||||
if not os.path.isfile(VENDOR_MAPD_PATH):
|
||||
self._say("Offline maps engine missing; it will be restored by the next update.")
|
||||
self._params.remove(_VERSION_PARAM)
|
||||
return False
|
||||
|
||||
try:
|
||||
current = sha256_of_file(VENDOR_MAPD_PATH)
|
||||
except OSError:
|
||||
cloudlog.exception("iq_maps: vendor mapd unreadable")
|
||||
return False
|
||||
|
||||
if current == expected:
|
||||
stamp_vendor_version(VENDOR_RELEASE_TAG, self._params)
|
||||
try:
|
||||
os.remove(QUARANTINE_PATH)
|
||||
except OSError:
|
||||
pass
|
||||
self._say(f"Offline maps engine verified [{VENDOR_RELEASE_TAG}]")
|
||||
return True
|
||||
|
||||
cloudlog.error(f"iq_maps: vendor mapd hash {current[:12]} != pinned {expected[:12]}, quarantining")
|
||||
self._say("Offline maps engine failed verification; quarantined until the next update.")
|
||||
try:
|
||||
os.replace(VENDOR_MAPD_PATH, QUARANTINE_PATH)
|
||||
except OSError:
|
||||
cloudlog.exception("iq_maps: vendor mapd quarantine failed")
|
||||
return False
|
||||
self._params.remove(_VERSION_PARAM)
|
||||
try:
|
||||
raise RuntimeError(f"vendor mapd hash mismatch quarantined: {current}")
|
||||
except RuntimeError as exc:
|
||||
sentry.init(sentry.SentryProject.SELFDRIVE)
|
||||
sentry.capture_exception(exc)
|
||||
return False
|
||||
|
||||
def _expected_hash(self) -> str:
|
||||
try:
|
||||
with open(_HASH_FILE) as f:
|
||||
return f.read().strip()
|
||||
except OSError:
|
||||
return ""
|
||||
|
||||
def _say(self, text: str) -> None:
|
||||
if self._spinner is not None:
|
||||
self._spinner.update(text)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
spinner = Spinner()
|
||||
ok = VendorMapdInstaller(spinner).verify()
|
||||
spinner.close()
|
||||
sys.exit(0 if ok else 1)
|
||||
4
iqpilot/konn3kt/__init__.py
Normal file
4
iqpilot/konn3kt/__init__.py
Normal file
@@ -0,0 +1,4 @@
|
||||
#!/usr/bin/env python3
|
||||
"""
|
||||
Copyright ©️ IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
|
||||
"""
|
||||
3
iqpilot/konn3kt/canlive/__init__.py
Normal file
3
iqpilot/konn3kt/canlive/__init__.py
Normal file
@@ -0,0 +1,3 @@
|
||||
"""
|
||||
Copyright ©️ IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
|
||||
"""
|
||||
78
iqpilot/konn3kt/canlive/canlived.py
Executable file
78
iqpilot/konn3kt/canlive/canlived.py
Executable file
@@ -0,0 +1,78 @@
|
||||
#!/usr/bin/env python3
|
||||
"""
|
||||
Copyright ©️ IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
|
||||
"""
|
||||
import os
|
||||
import threading
|
||||
|
||||
from websocket import ABNF, create_connection
|
||||
|
||||
import iqpilot.cereal.messaging as messaging
|
||||
from iqpilot.common.api import Api
|
||||
from iqpilot.common.params import Params
|
||||
from iqpilot.common.swaglog import cloudlog
|
||||
CAN_SERVICES = ["can"]
|
||||
RECONNECT_MIN = 1.0
|
||||
RECONNECT_MAX = 10.0
|
||||
|
||||
def _api_host() -> str:
|
||||
host = "wss://api-iqlabs.konn3kt.com"
|
||||
host = host.rstrip("/")
|
||||
if host.startswith("https://"):
|
||||
host = "wss://" + host[len("https://"):]
|
||||
elif host.startswith("http://"):
|
||||
host = "ws://" + host[len("http://"):]
|
||||
return host
|
||||
|
||||
|
||||
def _stream_once(dongle_id: str, ws_uri: str, token: str, exit_event: threading.Event) -> None:
|
||||
ws = create_connection(ws_uri, cookie="jwt=" + token, enable_multithread=True, timeout=30.0)
|
||||
cloudlog.info("canlived: connected to %s", ws_uri)
|
||||
try:
|
||||
socks = [messaging.sub_sock(s, conflate=False, timeout=100) for s in CAN_SERVICES]
|
||||
while not exit_event.is_set():
|
||||
got_any = False
|
||||
for sock in socks:
|
||||
while True:
|
||||
raw = sock.receive(non_blocking=True)
|
||||
if raw is None:
|
||||
break
|
||||
got_any = True
|
||||
ws.send_frame(ABNF.create_frame(raw, ABNF.OPCODE_BINARY, 1))
|
||||
if not got_any:
|
||||
exit_event.wait(0.005)
|
||||
finally:
|
||||
try:
|
||||
ws.close()
|
||||
except Exception:
|
||||
pass
|
||||
|
||||
|
||||
def main(exit_event: threading.Event | None = None) -> None:
|
||||
if exit_event is None:
|
||||
exit_event = threading.Event()
|
||||
|
||||
params = Params()
|
||||
dongle_id = params.get("DongleId", encoding="utf-8")
|
||||
if not dongle_id:
|
||||
cloudlog.error("canlived: no DongleId, cannot stream")
|
||||
return
|
||||
|
||||
api = Api(dongle_id)
|
||||
host = _api_host()
|
||||
ws_uri = f"{host}/ws/can/{dongle_id}"
|
||||
|
||||
backoff = RECONNECT_MIN
|
||||
while not exit_event.is_set():
|
||||
try:
|
||||
token = api.get_token(expiry_hours=1)
|
||||
_stream_once(dongle_id, ws_uri, token, exit_event)
|
||||
backoff = RECONNECT_MIN
|
||||
except Exception as e:
|
||||
cloudlog.exception("canlived: stream error: %s", e)
|
||||
exit_event.wait(backoff)
|
||||
backoff = min(backoff * 2, RECONNECT_MAX)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
17
iqpilot/konn3kt/cloud_client.py
Normal file
17
iqpilot/konn3kt/cloud_client.py
Normal 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)
|
||||
3
iqpilot/konn3kt/common/__init__.py
Normal file
3
iqpilot/konn3kt/common/__init__.py
Normal file
@@ -0,0 +1,3 @@
|
||||
"""
|
||||
Copyright ©️ IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
|
||||
"""
|
||||
48
iqpilot/konn3kt/common/param_codec.py
Normal file
48
iqpilot/konn3kt/common/param_codec.py
Normal file
@@ -0,0 +1,48 @@
|
||||
"""
|
||||
Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
|
||||
"""
|
||||
import base64
|
||||
import gzip
|
||||
import json
|
||||
|
||||
from iqpilot.common.params import Params, ParamKeyType
|
||||
|
||||
|
||||
def encode_param(name: str, params=None, use_default: bool = False) -> bytes | None:
|
||||
params = params or Params()
|
||||
raw = params.get_default_value(name) if use_default else params.get(name)
|
||||
if raw is None:
|
||||
return None
|
||||
|
||||
ktype = params.get_type(name)
|
||||
if ktype == ParamKeyType.BYTES:
|
||||
return bytes(raw)
|
||||
if ktype == ParamKeyType.JSON:
|
||||
return json.dumps(raw).encode("utf-8")
|
||||
return str(raw).encode("utf-8")
|
||||
|
||||
|
||||
_FROM_TEXT = {
|
||||
ParamKeyType.STRING: lambda s: s,
|
||||
ParamKeyType.BOOL: lambda s: s.lower() in ("true", "1", "yes"),
|
||||
ParamKeyType.INT: int,
|
||||
ParamKeyType.FLOAT: float,
|
||||
ParamKeyType.TIME: str,
|
||||
ParamKeyType.JSON: json.loads,
|
||||
}
|
||||
|
||||
|
||||
def restore_param_from_base64(name: str, b64_data: str, compressed: bool = False) -> None:
|
||||
params = Params()
|
||||
ktype = params.get_type(name)
|
||||
|
||||
blob = base64.b64decode(b64_data)
|
||||
if compressed:
|
||||
blob = gzip.decompress(blob)
|
||||
|
||||
if ktype == ParamKeyType.BYTES:
|
||||
value = blob
|
||||
else:
|
||||
value = _FROM_TEXT.get(ktype, lambda s: s)(blob.decode("utf-8"))
|
||||
|
||||
params.put(name, value)
|
||||
226
iqpilot/konn3kt/registration.py
Executable file
226
iqpilot/konn3kt/registration.py
Executable 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())
|
||||
11
iqpilot/konn3kt/service_health.py
Normal file
11
iqpilot/konn3kt/service_health.py
Normal 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
|
||||
"""
|
||||
|
||||
def hephaestus_ready(params=None) -> bool:
|
||||
return True
|
||||
|
||||
|
||||
def hephaestus_ready_shim():
|
||||
return hephaestus_ready()
|
||||
3
iqpilot/navd/__init__.py
Normal file
3
iqpilot/navd/__init__.py
Normal file
@@ -0,0 +1,3 @@
|
||||
"""
|
||||
Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
|
||||
"""
|
||||
22
iqpilot/sab/__init__.py
Normal file
22
iqpilot/sab/__init__.py
Normal file
@@ -0,0 +1,22 @@
|
||||
"""
|
||||
Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
|
||||
"""
|
||||
from .behavior import (
|
||||
SteeringAssistanceBehavior,
|
||||
GuidanceStateMachine,
|
||||
DriverInterventionMode,
|
||||
BRANDS_WITHOUT_MAIN_CRUISE_TOGGLE,
|
||||
apply_aol_brand_overrides,
|
||||
apply_aol_experience_flags,
|
||||
read_aol_enabled_pref,
|
||||
read_joint_engagement_pref,
|
||||
read_main_cruise_pref,
|
||||
resolve_brake_intervention_mode,
|
||||
)
|
||||
|
||||
__all__ = [
|
||||
"SteeringAssistanceBehavior", "GuidanceStateMachine", "DriverInterventionMode",
|
||||
"BRANDS_WITHOUT_MAIN_CRUISE_TOGGLE", "apply_aol_brand_overrides", "apply_aol_experience_flags",
|
||||
"read_aol_enabled_pref", "read_joint_engagement_pref", "read_main_cruise_pref",
|
||||
"resolve_brake_intervention_mode",
|
||||
]
|
||||
525
iqpilot/sab/behavior.py
Normal file
525
iqpilot/sab/behavior.py
Normal file
@@ -0,0 +1,525 @@
|
||||
"""
|
||||
Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
|
||||
"""
|
||||
from dataclasses import dataclass
|
||||
from typing import Optional
|
||||
|
||||
from iqpilot.common.params import Params, UnknownKeyName
|
||||
from iqdbc.car import structs
|
||||
from iqpilot.common.realtime import DT_CTRL
|
||||
from iqdbc.safety import ALTERNATIVE_EXPERIENCE
|
||||
from iqpilot.selfdrive.selfdrived.events import ET
|
||||
from iqdbc.car.hyundai.values import HyundaiFlags, HyundaiFlagsIQ, HyundaiSafetyFlagsIQ
|
||||
from iqpilot.selfdrive.selfdrived.state import SOFT_DISABLE_TIME
|
||||
from iqpilot.cereal import log, custom
|
||||
|
||||
State = custom.AlwaysOnLateral.AlwaysOnLateralState
|
||||
|
||||
class DriverInterventionMode:
|
||||
CONTINUE = 0
|
||||
SUSPEND = 1
|
||||
CANCEL = 2
|
||||
|
||||
_FORCED_BRAKE_CANCEL = frozenset({"rivian"})
|
||||
BRANDS_WITHOUT_MAIN_CRUISE_TOGGLE = ("rivian", "tesla")
|
||||
_HYUNDAI_MAIN_CRUISE_FLAG_BRANDS = frozenset({"hyundai"})
|
||||
|
||||
_EXPERIENCE_BY_BRAKE_MODE = {
|
||||
DriverInterventionMode.CANCEL: ALTERNATIVE_EXPERIENCE.AOL_DISENGAGE_LATERAL_ON_BRAKE,
|
||||
DriverInterventionMode.SUSPEND: ALTERNATIVE_EXPERIENCE.AOL_PAUSE_LATERAL_ON_BRAKE,
|
||||
}
|
||||
|
||||
|
||||
def uses_forced_brake_cancel(CP: structs.CarParams, CP_IQ: structs.IQCarParams):
|
||||
del CP_IQ
|
||||
return CP.brand in _FORCED_BRAKE_CANCEL
|
||||
|
||||
|
||||
def read_aol_enabled_pref(params: Params):
|
||||
return params.get_bool("AolEnabled")
|
||||
|
||||
|
||||
def read_main_cruise_pref(params: Params):
|
||||
return params.get_bool("AolMainCruiseAllowed")
|
||||
|
||||
|
||||
def read_joint_engagement_pref(params: Params):
|
||||
return params.get_bool("AolUnifiedEngagementMode")
|
||||
|
||||
|
||||
def read_lateral_override_pause_pref(params: Params):
|
||||
return params.get_bool("AolPauseOnSteeringOverride")
|
||||
|
||||
|
||||
def resolve_brake_intervention_mode(CP: structs.CarParams, CP_IQ: structs.IQCarParams, params: Params):
|
||||
if uses_forced_brake_cancel(CP, CP_IQ):
|
||||
return DriverInterventionMode.CANCEL
|
||||
return params.get("AolSteeringMode", return_default=True)
|
||||
|
||||
|
||||
def apply_aol_experience_flags(CP: structs.CarParams, CP_IQ: structs.IQCarParams, params: Params):
|
||||
if not read_aol_enabled_pref(params):
|
||||
return
|
||||
CP.alternativeExperience |= ALTERNATIVE_EXPERIENCE.ENABLE_AOL
|
||||
mode = resolve_brake_intervention_mode(CP, CP_IQ, params)
|
||||
CP.alternativeExperience |= _EXPERIENCE_BY_BRAKE_MODE.get(mode, 0)
|
||||
|
||||
|
||||
def apply_aol_brand_overrides(CP: structs.CarParams, CP_IQ: structs.IQCarParams, params: Params):
|
||||
if CP.brand in _HYUNDAI_MAIN_CRUISE_FLAG_BRANDS:
|
||||
CP_IQ.flags |= HyundaiFlagsIQ.MAIN_BTN_LONG_TOGGLE.value
|
||||
CP_IQ.iqSafetyFlags |= HyundaiSafetyFlagsIQ.MAIN_BTN_LONG_TOGGLE
|
||||
|
||||
if uses_forced_brake_cancel(CP, CP_IQ):
|
||||
params.put("AolSteeringMode", DriverInterventionMode.CANCEL)
|
||||
params.put_bool("AolUnifiedEngagementMode", True)
|
||||
|
||||
if CP.brand in BRANDS_WITHOUT_MAIN_CRUISE_TOGGLE:
|
||||
params.remove("AolMainCruiseAllowed")
|
||||
|
||||
|
||||
EventName = log.OnroadEvent.EventName
|
||||
EventNameIQ = custom.IQOnroadEvent.EventName
|
||||
TORQUE_DELIVERING_STATES = (State.overriding, State.enabled, State.softDisabling)
|
||||
LATERAL_CONTROLLED_STATES = (State.paused, *TORQUE_DELIVERING_STATES)
|
||||
GUIDANCE_AVAILABLE_SIGNAL = ET.ENABLE
|
||||
GUIDANCE_GATE_BLOCK_SIGNAL = ET.NO_ENTRY
|
||||
GUIDANCE_SUPPRESSION_SIGNAL = ET.SOFT_DISABLE
|
||||
GUIDANCE_OPERATOR_OFF_SIGNAL = ET.USER_DISABLE
|
||||
GUIDANCE_HARD_CUT_SIGNAL = ET.IMMEDIATE_DISABLE
|
||||
GUIDANCE_DRIVER_OVERRIDE_SIGNAL = ET.OVERRIDE_LATERAL
|
||||
GUIDANCE_ACTIVE_ALERT = ET.WARNING
|
||||
|
||||
PAUSE_WITH_IQ_EVENTS = (
|
||||
EventNameIQ.parkBrakeSilent,
|
||||
EventNameIQ.seatbeltUnbuckledSilent,
|
||||
EventNameIQ.doorAjarSilent,
|
||||
EventNameIQ.brakeHoldSilent,
|
||||
EventNameIQ.reverseSilent,
|
||||
EventNameIQ.gearNotDriveSilent,
|
||||
)
|
||||
PAUSE_WITH_STOCK_EVENTS = (
|
||||
EventName.parkBrake,
|
||||
EventName.seatbeltNotLatched,
|
||||
EventName.doorOpen,
|
||||
EventName.brakeHold,
|
||||
EventName.reverseGear,
|
||||
EventName.wrongGear,
|
||||
)
|
||||
GEARS_ALLOW_PAUSED_SILENT = PAUSE_WITH_IQ_EVENTS
|
||||
GEARS_ALLOW_PAUSED = PAUSE_WITH_STOCK_EVENTS
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class GuidancePulse:
|
||||
wake_ping: bool
|
||||
gate_closed: bool
|
||||
cooldown_call: bool
|
||||
driver_kill: bool
|
||||
hard_cut: bool
|
||||
hands_on_wheel: bool
|
||||
hush_cut: bool
|
||||
pit_stop_ready: bool
|
||||
|
||||
|
||||
class GuidanceStateMachine:
|
||||
def __init__(self, sab):
|
||||
self.sab = sab
|
||||
self.selfdrive = sab.selfdrive
|
||||
self._sm_core = sab.selfdrive.state_machine
|
||||
self._events = sab.selfdrive.events
|
||||
self._events_iq = sab.selfdrive.events_iq
|
||||
self.state = State.disabled
|
||||
|
||||
@property
|
||||
def _parks_on_override(self) -> bool:
|
||||
return bool(self.sab.pause_on_lateral_override)
|
||||
|
||||
def _hands_on_landing(self, pulse: GuidancePulse) -> State:
|
||||
if not pulse.hands_on_wheel:
|
||||
return State.enabled
|
||||
return State.paused if self._parks_on_override else State.overriding
|
||||
|
||||
def _queue_alert_if_solo(self, alert_type: str):
|
||||
if not self.selfdrive.enabled:
|
||||
self._sm_core.current_alert_types.append(alert_type)
|
||||
|
||||
def _sees_event(self, event_type: str):
|
||||
return self._events.contains(event_type) or self._events_iq.contains(event_type)
|
||||
|
||||
def _can_take_pit_stop(self):
|
||||
return self._events.contains_in_list(PAUSE_WITH_STOCK_EVENTS) or self._events_iq.contains_in_list(PAUSE_WITH_IQ_EVENTS)
|
||||
|
||||
def _capture_pulse(self) -> GuidancePulse:
|
||||
return GuidancePulse(
|
||||
wake_ping=self._sees_event(GUIDANCE_AVAILABLE_SIGNAL),
|
||||
gate_closed=self._sees_event(GUIDANCE_GATE_BLOCK_SIGNAL),
|
||||
cooldown_call=self._sees_event(GUIDANCE_SUPPRESSION_SIGNAL),
|
||||
driver_kill=self._sees_event(GUIDANCE_OPERATOR_OFF_SIGNAL),
|
||||
hard_cut=self._sees_event(GUIDANCE_HARD_CUT_SIGNAL),
|
||||
hands_on_wheel=self._sees_event(GUIDANCE_DRIVER_OVERRIDE_SIGNAL),
|
||||
hush_cut=self._events_iq.has(EventNameIQ.alcDisengagedSilent),
|
||||
pit_stop_ready=self._can_take_pit_stop(),
|
||||
)
|
||||
|
||||
def _start_grace_period(self):
|
||||
if not self.selfdrive.enabled:
|
||||
self._sm_core.soft_disable_timer = int(SOFT_DISABLE_TIME / DT_CTRL)
|
||||
self._sm_core.current_alert_types.append(GUIDANCE_SUPPRESSION_SIGNAL)
|
||||
|
||||
def _run_global_cutoffs(self, pulse: GuidancePulse) -> Optional[object]:
|
||||
if pulse.driver_kill:
|
||||
self._sm_core.current_alert_types.append(GUIDANCE_OPERATOR_OFF_SIGNAL)
|
||||
return State.paused if pulse.hush_cut else State.disabled
|
||||
if pulse.hard_cut:
|
||||
self._queue_alert_if_solo(GUIDANCE_HARD_CUT_SIGNAL)
|
||||
return State.disabled
|
||||
return None
|
||||
|
||||
def _handle_disabled(self, pulse: GuidancePulse) -> State:
|
||||
if not pulse.wake_ping:
|
||||
return State.disabled
|
||||
if pulse.gate_closed:
|
||||
self._queue_alert_if_solo(GUIDANCE_GATE_BLOCK_SIGNAL)
|
||||
return State.paused if pulse.pit_stop_ready else State.disabled
|
||||
self._queue_alert_if_solo(GUIDANCE_AVAILABLE_SIGNAL)
|
||||
return self._hands_on_landing(pulse)
|
||||
|
||||
def _handle_enabled(self, pulse: GuidancePulse) -> State:
|
||||
forced_state = self._run_global_cutoffs(pulse)
|
||||
if forced_state is not None:
|
||||
return forced_state
|
||||
if pulse.cooldown_call:
|
||||
self._start_grace_period()
|
||||
return State.softDisabling
|
||||
if pulse.hands_on_wheel:
|
||||
if self._parks_on_override:
|
||||
return State.paused
|
||||
self._queue_alert_if_solo(GUIDANCE_DRIVER_OVERRIDE_SIGNAL)
|
||||
return State.overriding
|
||||
return State.enabled
|
||||
|
||||
def _handle_soft_disabling(self, pulse: GuidancePulse) -> State:
|
||||
forced_state = self._run_global_cutoffs(pulse)
|
||||
if forced_state is not None:
|
||||
return forced_state
|
||||
if not pulse.cooldown_call:
|
||||
return State.enabled
|
||||
if self._sm_core.soft_disable_timer > 0:
|
||||
self._queue_alert_if_solo(GUIDANCE_SUPPRESSION_SIGNAL)
|
||||
return State.softDisabling
|
||||
return State.disabled
|
||||
|
||||
def _handle_paused(self, pulse: GuidancePulse) -> State:
|
||||
forced_state = self._run_global_cutoffs(pulse)
|
||||
if forced_state is not None:
|
||||
return forced_state
|
||||
if not pulse.wake_ping:
|
||||
return State.paused
|
||||
if pulse.gate_closed:
|
||||
self._queue_alert_if_solo(GUIDANCE_GATE_BLOCK_SIGNAL)
|
||||
return State.paused
|
||||
self._queue_alert_if_solo(GUIDANCE_AVAILABLE_SIGNAL)
|
||||
return self._hands_on_landing(pulse)
|
||||
|
||||
def _handle_overriding(self, pulse: GuidancePulse) -> State:
|
||||
forced_state = self._run_global_cutoffs(pulse)
|
||||
if forced_state is not None:
|
||||
return forced_state
|
||||
if pulse.cooldown_call:
|
||||
self._start_grace_period()
|
||||
return State.softDisabling
|
||||
if pulse.hands_on_wheel:
|
||||
if self._parks_on_override:
|
||||
return State.paused
|
||||
self._sm_core.current_alert_types.append(GUIDANCE_DRIVER_OVERRIDE_SIGNAL)
|
||||
return State.overriding
|
||||
return State.enabled
|
||||
|
||||
def update(self):
|
||||
pulse = self._capture_pulse()
|
||||
handler = {
|
||||
State.disabled: self._handle_disabled,
|
||||
State.enabled: self._handle_enabled,
|
||||
State.softDisabling: self._handle_soft_disabling,
|
||||
State.paused: self._handle_paused,
|
||||
State.overriding: self._handle_overriding,
|
||||
}[self.state]
|
||||
|
||||
self.state = handler(pulse)
|
||||
enabled = self.state in LATERAL_CONTROLLED_STATES
|
||||
active = self.state in TORQUE_DELIVERING_STATES
|
||||
if active:
|
||||
self._queue_alert_if_solo(GUIDANCE_ACTIVE_ALERT)
|
||||
return enabled, active
|
||||
|
||||
_E = log.OnroadEvent.EventName
|
||||
_Q = custom.IQOnroadEvent.EventName
|
||||
_BTN = structs.CarState.ButtonEvent.Type
|
||||
_GEAR = structs.CarState.GearShifter
|
||||
|
||||
_CRUISE_SET_TAPS = frozenset((_BTN.accelCruise, _BTN.resumeCruise, _BTN.decelCruise, _BTN.setCruise))
|
||||
_LATERAL_TOGGLE_BUTTONS = (_BTN.lkas, _BTN.lfaButton)
|
||||
_HYUNDAI_LDA_MASK = HyundaiFlags.CANFD
|
||||
|
||||
_QUIET_SWAPS = (
|
||||
(_Q.seatbeltUnbuckledSilent, _E.seatbeltNotLatched, True, None),
|
||||
(_Q.doorAjarSilent, _E.doorOpen, True, None),
|
||||
(_Q.reverseSilent, _E.reverseGear, False, None),
|
||||
(_Q.parkBrakeSilent, _E.parkBrake, False, None),
|
||||
(_Q.brakeHoldSilent, _E.brakeHold, False, None),
|
||||
(_Q.gearNotDriveSilent, _E.wrongGear, False,
|
||||
lambda cs: cs.vEgo < 2.5 or cs.gearShifter == _GEAR.reverse),
|
||||
)
|
||||
|
||||
_DROP_ON_ENTRY = (_E.speedTooLow, _E.belowEngageSpeed, _E.preEnableStandstill,
|
||||
_E.manualRestart, _E.cruiseDisabled)
|
||||
_DROP_ON_EXIT = (_E.wrongCruiseMode, _E.pedalPressed, _E.buttonCancel, _E.pcmDisable)
|
||||
|
||||
|
||||
class SteeringAssistanceBehavior:
|
||||
def __init__(self, selfdrive):
|
||||
sd = selfdrive
|
||||
self.selfdrive = sd
|
||||
self.CP, self.CP_IQ, self.params = sd.CP, sd.CP_IQ, sd.params
|
||||
self.events, self.events_iq = sd.events, sd.events_iq
|
||||
|
||||
self.enabled = self.active = self.available = False
|
||||
self.pause_on_lateral_override = False
|
||||
sd.enabled_prev = False
|
||||
self.state_machine = GuidanceStateMachine(self)
|
||||
|
||||
self.disengage_on_accelerator = self.params.get_bool("DisengageOnAccelerator")
|
||||
self._apply_brand_capabilities()
|
||||
self._reload_preferences(full=True)
|
||||
|
||||
def _apply_brand_capabilities(self):
|
||||
brand = self.CP.brand
|
||||
self.no_main_cruise = brand in BRANDS_WITHOUT_MAIN_CRUISE_TOGGLE
|
||||
lda_capable = bool(self.CP.flags & _HYUNDAI_LDA_MASK) or bool(self.CP_IQ.flags & HyundaiFlagsIQ.HAS_LFA_BUTTON)
|
||||
self.hkg_allow = brand == "hyundai" and lda_capable
|
||||
|
||||
def _reload_preferences(self, full: bool = False):
|
||||
self.main_enabled_toggle = read_main_cruise_pref(self.params)
|
||||
self.unified_engagement_mode = read_joint_engagement_pref(self.params)
|
||||
self.pause_on_lateral_override = read_lateral_override_pause_pref(self.params)
|
||||
if full:
|
||||
self.enabled_toggle = read_aol_enabled_pref(self.params)
|
||||
self.steering_mode_on_brake = resolve_brake_intervention_mode(self.CP, self.CP_IQ, self.params)
|
||||
|
||||
def read_params(self):
|
||||
self._reload_preferences()
|
||||
|
||||
def _has(self, ev):
|
||||
return self.events.has(ev)
|
||||
|
||||
def _drop(self, ev):
|
||||
self.events.remove(ev)
|
||||
|
||||
def _raise(self, ev):
|
||||
self.events.add(ev)
|
||||
|
||||
def _emit(self, ev):
|
||||
self.events_iq.add(ev)
|
||||
|
||||
def _retract(self, ev):
|
||||
self.events_iq.remove(ev)
|
||||
|
||||
def _emitted(self, ev):
|
||||
return self.events_iq.contains(ev)
|
||||
|
||||
def _emitted_any(self, evs):
|
||||
return self.events_iq.contains_in_list(evs)
|
||||
|
||||
def _iq_has(self, ev):
|
||||
return self.events_iq.has(ev)
|
||||
|
||||
def _brake_without_gas(self, cs):
|
||||
prev_gas = self.selfdrive.CS_prev.gasPressed
|
||||
gas_rising_edge = cs.gasPressed and not prev_gas
|
||||
override_via_gas = gas_rising_edge and self.disengage_on_accelerator
|
||||
return self._has(_E.pedalPressed) and not override_via_gas
|
||||
|
||||
def _lateral_overridden(self):
|
||||
return self.events.contains(ET.OVERRIDE_LATERAL) or self.events_iq.contains(ET.OVERRIDE_LATERAL)
|
||||
|
||||
def _may_silently_resume(self, cs):
|
||||
suspend_on_brake = self.steering_mode_on_brake == DriverInterventionMode.SUSPEND
|
||||
if suspend_on_brake and self._brake_without_gas(cs):
|
||||
return False
|
||||
if self.pause_on_lateral_override and self._lateral_overridden():
|
||||
return False
|
||||
return not self._emitted_any(GEARS_ALLOW_PAUSED_SILENT)
|
||||
|
||||
@property
|
||||
def _long_held_two_cycles(self):
|
||||
sd = self.selfdrive
|
||||
return bool(sd.enabled_prev and sd.enabled)
|
||||
|
||||
def _uem_blocks_engage(self):
|
||||
if not self.unified_engagement_mode or self.enabled:
|
||||
return True
|
||||
return self._long_held_two_cycles
|
||||
|
||||
def _lateral_offered(self, cs):
|
||||
return bool(cs.lateralAvailable or cs.cruiseState.available or self.hkg_allow or self.CP.brand == "tesla")
|
||||
|
||||
@staticmethod
|
||||
def _main_cruise_live(cs):
|
||||
cruise = getattr(cs, 'cruiseState', None)
|
||||
if getattr(cruise, 'available', False):
|
||||
return True
|
||||
return bool(getattr(cs, 'cruiseFaultLateralMode', False))
|
||||
|
||||
def _swap_event(self, stock: int, silent: int):
|
||||
self._drop(stock)
|
||||
self._emit(silent)
|
||||
|
||||
def _flag_pause(self):
|
||||
already_held = self.state_machine.state is State.paused
|
||||
if not already_held:
|
||||
self._emit(_Q.alcDisengagedSilent)
|
||||
|
||||
def _resolve_wrong_mode(self, alert_only: bool):
|
||||
if not alert_only:
|
||||
self._drop(_E.wrongCarMode)
|
||||
elif self._has(_E.wrongCarMode):
|
||||
self._swap_event(_E.wrongCarMode, _Q.carModeMismatchNotice)
|
||||
|
||||
def _consume_joystick_aol_request(self, cs) -> str | None:
|
||||
if not self.params.get_bool("JoystickDebugMode"):
|
||||
return None
|
||||
try:
|
||||
raw = self.params.get("JoystickAolRequest")
|
||||
except UnknownKeyName:
|
||||
return None
|
||||
if not raw:
|
||||
return None
|
||||
try:
|
||||
request = raw.decode("utf-8") if isinstance(raw, (bytes, bytearray)) else str(raw)
|
||||
except Exception:
|
||||
request = ""
|
||||
try:
|
||||
self.params.remove("JoystickAolRequest")
|
||||
except UnknownKeyName:
|
||||
return None
|
||||
|
||||
verb = request.strip().lower()
|
||||
if verb not in ("enable", "disable"):
|
||||
return None
|
||||
if not getattr(cs, "started", False):
|
||||
return None
|
||||
if getattr(cs, "doorOpen", False) or getattr(cs, "seatbeltUnlatched", False):
|
||||
return None
|
||||
parked_or_reverse = getattr(cs, "gearShifter", _GEAR.unknown) in (_GEAR.park, _GEAR.reverse)
|
||||
return None if parked_or_reverse else verb
|
||||
|
||||
def _phase_joystick(self, cs):
|
||||
verb = self._consume_joystick_aol_request(cs)
|
||||
if verb is not None:
|
||||
self._emit(_Q.alcEngaged if verb == "enable" else _Q.alcDisengaged)
|
||||
|
||||
def _phase_soften_for_lateral_session(self, cs):
|
||||
if self.selfdrive.enabled or not self.enabled:
|
||||
return
|
||||
|
||||
for silent, stock, standstill_only, extra in _QUIET_SWAPS:
|
||||
if standstill_only and not cs.standstill:
|
||||
continue
|
||||
if not self._has(stock):
|
||||
continue
|
||||
if extra is not None and not extra(cs):
|
||||
continue
|
||||
self._swap_event(stock, silent)
|
||||
self._flag_pause()
|
||||
|
||||
if self.steering_mode_on_brake == DriverInterventionMode.SUSPEND and self._brake_without_gas(cs):
|
||||
self._flag_pause()
|
||||
|
||||
for chatter in _DROP_ON_ENTRY:
|
||||
self._drop(chatter)
|
||||
|
||||
_ENGAGE_TRIGGERS = (_E.pcmEnable, _E.buttonEnable)
|
||||
|
||||
def _phase_engagement(self, cs):
|
||||
long_engage = any(self._has(trig) for trig in self._ENGAGE_TRIGGERS)
|
||||
tapped_set = any(be.type in _CRUISE_SET_TAPS for be in cs.buttonEvents)
|
||||
self._resolve_wrong_mode(long_engage or tapped_set)
|
||||
|
||||
if long_engage:
|
||||
if self._brake_without_gas(cs):
|
||||
self._emit(_Q.pedalHeldNotice)
|
||||
if self._uem_blocks_engage():
|
||||
self._drop(_E.pcmEnable)
|
||||
self._drop(_E.buttonEnable)
|
||||
return
|
||||
|
||||
if self.main_enabled_toggle and self._main_cruise_live(cs) and not self._main_cruise_live(self.selfdrive.CS_prev):
|
||||
self._emit(_Q.alcEngaged)
|
||||
|
||||
def _phase_buttons(self, cs):
|
||||
kill_all = False
|
||||
long_dropped_out = self.selfdrive.enabled_prev and not self.selfdrive.enabled
|
||||
for be in cs.buttonEvents:
|
||||
if be.type == _BTN.cancel and long_dropped_out:
|
||||
self._emit(_Q.speedManually)
|
||||
if not (be.type in _LATERAL_TOGGLE_BUTTONS and be.pressed and self._lateral_offered(cs)):
|
||||
continue
|
||||
if not self.enabled:
|
||||
self._emit(_Q.alcEngaged)
|
||||
continue
|
||||
self._emit(_Q.alcDisengaged)
|
||||
if self.selfdrive.enabled:
|
||||
kill_all = True
|
||||
return kill_all
|
||||
|
||||
def _phase_availability(self, cs):
|
||||
main_off = self.main_enabled_toggle and not self._main_cruise_live(cs)
|
||||
if self.no_main_cruise or (self._lateral_offered(cs) and not main_off):
|
||||
return
|
||||
self._drop(_E.buttonEnable)
|
||||
if self.enabled:
|
||||
self._emit(_Q.alcDisengaged)
|
||||
|
||||
def _phase_brake_policy(self, cs):
|
||||
if self.steering_mode_on_brake != DriverInterventionMode.CANCEL or not self._brake_without_gas(cs):
|
||||
return
|
||||
if self.enabled:
|
||||
self._emit(_Q.alcDisengaged)
|
||||
elif self._emitted(_Q.alcEngaged):
|
||||
self._retract(_Q.alcEngaged)
|
||||
self._emit(_Q.pedalHeldNotice)
|
||||
|
||||
def _phase_resume_from_pause(self, cs):
|
||||
held = self.state_machine.state is State.paused
|
||||
if held and self._may_silently_resume(cs):
|
||||
self._emit(_Q.alcEngagedSilent)
|
||||
|
||||
def update_events(self, cs):
|
||||
self._phase_joystick(cs)
|
||||
self._phase_soften_for_lateral_session(cs)
|
||||
self._phase_engagement(cs)
|
||||
kill_all = self._phase_buttons(cs)
|
||||
self._phase_availability(cs)
|
||||
self._phase_brake_policy(cs)
|
||||
self._phase_resume_from_pause(cs)
|
||||
|
||||
for chatter in _DROP_ON_EXIT:
|
||||
self._drop(chatter)
|
||||
|
||||
if kill_all:
|
||||
self._raise(_E.buttonCancel)
|
||||
|
||||
def update(self, cs):
|
||||
if not self.enabled_toggle and not self.params.get_bool("JoystickDebugMode"):
|
||||
return
|
||||
self.update_events(cs)
|
||||
self.update_state()
|
||||
|
||||
def update_state(self):
|
||||
sd = self.selfdrive
|
||||
sd.enabled_prev = sd.enabled
|
||||
runnable = sd.initialized and not self.CP.passive
|
||||
if runnable:
|
||||
verdict = self.state_machine.update()
|
||||
self.enabled, self.active = verdict
|
||||
229
iqpilot/sab/tests/test_sab.py
Normal file
229
iqpilot/sab/tests/test_sab.py
Normal file
@@ -0,0 +1,229 @@
|
||||
"""
|
||||
Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
|
||||
"""
|
||||
|
||||
from types import SimpleNamespace
|
||||
|
||||
from iqpilot.cereal import custom
|
||||
from iqdbc.car import structs
|
||||
from iqdbc.car.hyundai.values import HyundaiFlags, HyundaiFlagsIQ
|
||||
from iqpilot.sab.behavior import SteeringAssistanceBehavior
|
||||
from iqpilot.selfdrive.selfdrived.iq_events import IQEvents
|
||||
from iqpilot.selfdrive.selfdrived.events import Events
|
||||
from iqpilot.cereal import log
|
||||
|
||||
|
||||
ButtonType = structs.CarState.ButtonEvent.Type
|
||||
EventName = log.OnroadEvent.EventName
|
||||
EventNameIQ = custom.IQOnroadEvent.EventName
|
||||
GuidanceState = custom.AlwaysOnLateral.AlwaysOnLateralState
|
||||
|
||||
|
||||
class MockParams:
|
||||
def __init__(self, main_cruise_allowed: bool = False, aol_enabled: bool = True,
|
||||
pause_on_steering_override: bool = False):
|
||||
self.main_cruise_allowed = main_cruise_allowed
|
||||
self.aol_enabled = aol_enabled
|
||||
self.pause_on_steering_override = pause_on_steering_override
|
||||
|
||||
def get_bool(self, key: str) -> bool:
|
||||
return {
|
||||
"AolEnabled": self.aol_enabled,
|
||||
"AolMainCruiseAllowed": self.main_cruise_allowed,
|
||||
"AolUnifiedEngagementMode": False,
|
||||
"AolPauseOnSteeringOverride": self.pause_on_steering_override,
|
||||
"JoystickDebugMode": False,
|
||||
}.get(key, False)
|
||||
|
||||
def get(self, key: str, return_default: bool = False):
|
||||
if key == "AolSteeringMode":
|
||||
return 0 if return_default else b"0"
|
||||
return None
|
||||
|
||||
def remove(self, key: str) -> None:
|
||||
return None
|
||||
|
||||
|
||||
def make_selfdrive(cp_flags: int, brand: str = "hyundai", main_cruise_allowed: bool = False,
|
||||
aol_enabled: bool = True, cp_iq_flags: int = 0,
|
||||
pause_on_steering_override: bool = False):
|
||||
cp = SimpleNamespace(
|
||||
brand=brand,
|
||||
flags=cp_flags,
|
||||
passive=False,
|
||||
notCar=False,
|
||||
safetyModel=structs.CarParams.SafetyModel.noOutput,
|
||||
)
|
||||
cp_iq = SimpleNamespace(flags=cp_iq_flags)
|
||||
return SimpleNamespace(
|
||||
CP=cp,
|
||||
CP_IQ=cp_iq,
|
||||
params=MockParams(main_cruise_allowed, aol_enabled, pause_on_steering_override),
|
||||
state_machine=SimpleNamespace(soft_disable_timer=0, current_alert_types=[]),
|
||||
events=Events(),
|
||||
events_iq=IQEvents(),
|
||||
CS_prev=SimpleNamespace(
|
||||
gasPressed=False,
|
||||
cruiseState=SimpleNamespace(available=False),
|
||||
lateralAvailable=False,
|
||||
),
|
||||
enabled=False,
|
||||
enabled_prev=False,
|
||||
initialized=True,
|
||||
)
|
||||
|
||||
|
||||
def make_car_state():
|
||||
return SimpleNamespace(
|
||||
started=True,
|
||||
standstill=False,
|
||||
doorOpen=False,
|
||||
seatbeltUnlatched=False,
|
||||
gearShifter=structs.CarState.GearShifter.drive,
|
||||
vEgo=0.0,
|
||||
gasPressed=False,
|
||||
brakePressed=False,
|
||||
cruiseState=SimpleNamespace(available=False),
|
||||
lateralAvailable=False,
|
||||
buttonEvents=[structs.CarState.ButtonEvent(pressed=True, type=ButtonType.lkas)],
|
||||
)
|
||||
|
||||
|
||||
def make_vw_car_state(cruise_available: bool, cruise_fault_lateral: bool = False):
|
||||
return SimpleNamespace(
|
||||
started=True,
|
||||
standstill=False,
|
||||
doorOpen=False,
|
||||
seatbeltUnlatched=False,
|
||||
gearShifter=structs.CarState.GearShifter.drive,
|
||||
vEgo=0.0,
|
||||
gasPressed=False,
|
||||
brakePressed=False,
|
||||
cruiseState=SimpleNamespace(available=cruise_available),
|
||||
lateralAvailable=cruise_available or cruise_fault_lateral,
|
||||
cruiseFaultLateralMode=cruise_fault_lateral,
|
||||
buttonEvents=[],
|
||||
)
|
||||
|
||||
|
||||
def test_hyundai_lkas_button_can_arm_guidance_before_lateral_available():
|
||||
selfdrive = make_selfdrive(0, cp_iq_flags=HyundaiFlagsIQ.HAS_LFA_BUTTON)
|
||||
guidance = SteeringAssistanceBehavior(selfdrive)
|
||||
car_state = make_car_state()
|
||||
car_state.buttonEvents = [structs.CarState.ButtonEvent(pressed=True, type=ButtonType.lfaButton)]
|
||||
|
||||
guidance.update_events(car_state)
|
||||
|
||||
assert selfdrive.events_iq.has(EventNameIQ.alcEngaged)
|
||||
|
||||
|
||||
def test_hyundai_lkas_button_stays_inactive_without_platform_support():
|
||||
selfdrive = make_selfdrive(0)
|
||||
guidance = SteeringAssistanceBehavior(selfdrive)
|
||||
|
||||
guidance.update_events(make_car_state())
|
||||
|
||||
assert not selfdrive.events_iq.has(EventNameIQ.alcEngaged)
|
||||
|
||||
|
||||
def test_main_cruise_drop_cuts_guidance_even_if_lateral_signal_stays_true():
|
||||
selfdrive = make_selfdrive(0, brand="volkswagen", main_cruise_allowed=True)
|
||||
selfdrive.CS_prev = make_vw_car_state(cruise_available=True)
|
||||
guidance = SteeringAssistanceBehavior(selfdrive)
|
||||
guidance.enabled = True
|
||||
|
||||
guidance.update_events(make_vw_car_state(cruise_available=False))
|
||||
|
||||
assert selfdrive.events_iq.has(EventNameIQ.alcDisengaged)
|
||||
|
||||
|
||||
def test_faulted_lateral_mode_does_not_force_disable_guidance():
|
||||
selfdrive = make_selfdrive(0, brand="volkswagen", main_cruise_allowed=True)
|
||||
selfdrive.CS_prev = make_vw_car_state(cruise_available=True)
|
||||
guidance = SteeringAssistanceBehavior(selfdrive)
|
||||
guidance.enabled = True
|
||||
|
||||
guidance.update_events(make_vw_car_state(cruise_available=False, cruise_fault_lateral=True))
|
||||
|
||||
assert not selfdrive.events_iq.has(EventNameIQ.alcDisengaged)
|
||||
|
||||
|
||||
def test_main_switch_rising_edge_arms_guidance_during_faulted_cruise():
|
||||
selfdrive = make_selfdrive(0, brand="volkswagen", main_cruise_allowed=True)
|
||||
selfdrive.CS_prev = make_vw_car_state(cruise_available=False, cruise_fault_lateral=False)
|
||||
guidance = SteeringAssistanceBehavior(selfdrive)
|
||||
|
||||
guidance.update_events(make_vw_car_state(cruise_available=False, cruise_fault_lateral=True))
|
||||
|
||||
assert selfdrive.events_iq.has(EventNameIQ.alcEngaged)
|
||||
|
||||
|
||||
def test_main_cruise_rising_edge_does_not_engage_when_toggle_is_off():
|
||||
selfdrive = make_selfdrive(0, brand="volkswagen", main_cruise_allowed=True, aol_enabled=False)
|
||||
selfdrive.CS_prev = make_vw_car_state(cruise_available=False)
|
||||
guidance = SteeringAssistanceBehavior(selfdrive)
|
||||
|
||||
guidance.update(make_vw_car_state(cruise_available=True))
|
||||
|
||||
assert not selfdrive.events_iq.has(EventNameIQ.alcEngaged)
|
||||
assert not guidance.active
|
||||
assert not guidance.enabled
|
||||
assert guidance.state_machine.state == custom.AlwaysOnLateral.AlwaysOnLateralState.disabled
|
||||
|
||||
|
||||
def run_cycle(guidance, selfdrive, car_state, steering_pressed: bool):
|
||||
selfdrive.events.clear()
|
||||
selfdrive.events_iq.clear()
|
||||
if steering_pressed:
|
||||
selfdrive.events.add(EventName.steerOverride)
|
||||
guidance.update(car_state)
|
||||
selfdrive.CS_prev = car_state
|
||||
|
||||
|
||||
def make_engaged_guidance(pause_on_steering_override: bool):
|
||||
selfdrive = make_selfdrive(0, brand="volkswagen", pause_on_steering_override=pause_on_steering_override)
|
||||
selfdrive.CS_prev = make_vw_car_state(cruise_available=True)
|
||||
guidance = SteeringAssistanceBehavior(selfdrive)
|
||||
guidance.enabled = True
|
||||
guidance.state_machine.state = GuidanceState.enabled
|
||||
return guidance, selfdrive
|
||||
|
||||
|
||||
def test_steering_override_parks_guidance_when_enabled():
|
||||
guidance, selfdrive = make_engaged_guidance(True)
|
||||
|
||||
run_cycle(guidance, selfdrive, make_vw_car_state(cruise_available=True), steering_pressed=True)
|
||||
|
||||
assert guidance.state_machine.state == GuidanceState.paused
|
||||
assert guidance.enabled
|
||||
assert not guidance.active
|
||||
|
||||
|
||||
def test_guidance_resumes_once_steering_is_released():
|
||||
guidance, selfdrive = make_engaged_guidance(True)
|
||||
|
||||
run_cycle(guidance, selfdrive, make_vw_car_state(cruise_available=True), steering_pressed=True)
|
||||
run_cycle(guidance, selfdrive, make_vw_car_state(cruise_available=True), steering_pressed=False)
|
||||
|
||||
assert guidance.state_machine.state == GuidanceState.enabled
|
||||
assert guidance.active
|
||||
|
||||
|
||||
def test_steering_override_keeps_torque_when_option_is_off():
|
||||
guidance, selfdrive = make_engaged_guidance(False)
|
||||
|
||||
run_cycle(guidance, selfdrive, make_vw_car_state(cruise_available=True), steering_pressed=True)
|
||||
|
||||
assert guidance.state_machine.state == GuidanceState.overriding
|
||||
assert guidance.active
|
||||
|
||||
|
||||
def test_main_cruise_rising_edge_engages_when_toggle_is_on():
|
||||
selfdrive = make_selfdrive(0, brand="volkswagen", main_cruise_allowed=True, aol_enabled=True)
|
||||
selfdrive.CS_prev = make_vw_car_state(cruise_available=False)
|
||||
guidance = SteeringAssistanceBehavior(selfdrive)
|
||||
|
||||
guidance.update(make_vw_car_state(cruise_available=True))
|
||||
|
||||
assert selfdrive.events_iq.has(EventNameIQ.alcEngaged)
|
||||
assert guidance.active
|
||||
Some files were not shown because too many files have changed in this diff Show More
Reference in New Issue
Block a user