mirror of
https://gitlvb.teallvbs.xyz/IQ.Lvbs/IQ.Pilot.git
synced 2026-07-24 13:02:07 +08:00
IQ.Pilot Release Commit @ 95cce10
This commit is contained in:
@@ -293,7 +293,7 @@ class CarState(CarStateBase):
|
||||
|
||||
ret.fuelGauge = pt_cp.vl["Kombi_02"]["KBI_Inhalt_Tank"] / 55.0
|
||||
ret.fuelTankLevelL = pt_cp.vl["Kombi_02"]["KBI_Inhalt_Tank"] # raw liters for konn3kt
|
||||
self._update_odometer(ret, aux_cp.vl["Kombi_02"]["KBI_Kilometerstand"])
|
||||
self._update_odometer(ret, pt_cp.vl["Kombi_02"]["KBI_Kilometerstand"])
|
||||
|
||||
self.cruise_faulted = ret.accFaulted
|
||||
self._apply_iq_private_flags(ret_iq)
|
||||
|
||||
@@ -1,94 +1,72 @@
|
||||
#!/usr/bin/env python3
|
||||
"""
|
||||
Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
|
||||
|
||||
Maintainer utility: pin a new pfeiferj/mapd release tag and refresh the checked-in
|
||||
binary hash. Not used at runtime.
|
||||
"""
|
||||
import argparse
|
||||
import os
|
||||
import re
|
||||
import sys
|
||||
|
||||
from openpilot.iqpilot.iq_maps.vendor_mapd_installer import get_file_hash
|
||||
from openpilot.common.basedir import BASEDIR
|
||||
from openpilot.iqpilot.iq_maps import VENDOR_MAPD_PATH
|
||||
from openpilot.iqpilot.iq_maps.vendor_mapd_installer import (
|
||||
VENDOR_RELEASE_TAG,
|
||||
get_file_hash,
|
||||
)
|
||||
|
||||
MAPD_HASH_PATH = os.path.join(BASEDIR, "iqpilot", "iq_maps", "tests", "mapd_hash")
|
||||
MAPD_VERSION_PATH = os.path.join(BASEDIR, "iqpilot", "iq_maps", "vendor_mapd_installer.py")
|
||||
RELEASE_SYMBOL = "VENDOR_RELEASE_TAG"
|
||||
_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")
|
||||
_TAG_ASSIGN = re.compile(rf'^{_RELEASE_SYMBOL}\s*=\s*["\'][^"\']*["\']', re.MULTILINE)
|
||||
|
||||
|
||||
def update_mapd_hash():
|
||||
mapd_hash = get_file_hash(VENDOR_MAPD_PATH)
|
||||
def rewrite_pinned_tag(new_tag: str) -> bool:
|
||||
with open(_INSTALLER_SRC) as f:
|
||||
src = f.read()
|
||||
|
||||
with open(MAPD_HASH_PATH, "w") as f:
|
||||
f.write(mapd_hash)
|
||||
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
|
||||
|
||||
print(f"Generated and updated new mapd hash to {MAPD_HASH_PATH}")
|
||||
with open(_INSTALLER_SRC, "w") as f:
|
||||
f.write(patched)
|
||||
print(f"pinned {_RELEASE_SYMBOL} -> {new_tag}")
|
||||
return True
|
||||
|
||||
|
||||
def get_current_mapd_version(path: str) -> str:
|
||||
print("[GET CURRENT MAPD VERSION]")
|
||||
with open(path) as f:
|
||||
for line in f:
|
||||
if line.strip().startswith(RELEASE_SYMBOL):
|
||||
match = re.search(rf'{RELEASE_SYMBOL}\s*=\s*[\'"]([^\'"]+)[\'"]', line)
|
||||
if match:
|
||||
ver = match.group(1)
|
||||
print(f'Current mapd version: "{ver}"')
|
||||
return ver
|
||||
else:
|
||||
print(f"[ERROR] {RELEASE_SYMBOL} line found but no quoted value detected.")
|
||||
return ""
|
||||
print(f"[ERROR] {RELEASE_SYMBOL} not found in file!")
|
||||
return ""
|
||||
def refresh_hash_file() -> None:
|
||||
digest = get_file_hash(VENDOR_MAPD_PATH)
|
||||
with open(_HASH_FILE, "w") as f:
|
||||
f.write(digest)
|
||||
print(f"wrote binary hash {digest} -> {_HASH_FILE}")
|
||||
|
||||
|
||||
def update_mapd_version(ver: str, path: str):
|
||||
print("[CHANGE CURRENT MAPD VERSION]")
|
||||
|
||||
with open(path) as f:
|
||||
lines = f.readlines()
|
||||
|
||||
found = False
|
||||
new_lines = []
|
||||
for line in lines:
|
||||
if not found and line.startswith(f"{RELEASE_SYMBOL} ="):
|
||||
new_lines.append(f'{RELEASE_SYMBOL} = "{ver}"\n')
|
||||
found = True
|
||||
new_lines.extend(lines[lines.index(line) + 1:])
|
||||
break
|
||||
else:
|
||||
new_lines.append(line)
|
||||
|
||||
if not found:
|
||||
print(f"[ERROR] {RELEASE_SYMBOL} line not found! Aborting without writing.")
|
||||
return
|
||||
|
||||
with open(path, "w") as f:
|
||||
f.writelines(new_lines)
|
||||
|
||||
print(f'New mapd version: "{ver}"')
|
||||
print("[DONE]")
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
parser = argparse.ArgumentParser(description="Update mapd version and hash")
|
||||
parser.add_argument("--new_ver", type=str, help="New mapd version")
|
||||
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:
|
||||
print("Warning: No new mapd version provided. Use --new_ver to specify")
|
||||
print("Example:")
|
||||
print(" python iqpilot/iq_maps/update_vendor_version.py --new_ver \"v1.12.0\"")
|
||||
print("Current mapd version and hash will not be updated! (aborted)")
|
||||
exit(0)
|
||||
parser.print_help()
|
||||
print(f'\ncurrently pinned: {VENDOR_RELEASE_TAG} (unchanged)')
|
||||
return 0
|
||||
|
||||
current_ver = get_current_mapd_version(MAPD_VERSION_PATH)
|
||||
new_ver = f"{args.new_ver}"
|
||||
if current_ver == new_ver:
|
||||
print(f'Proposed mapd version: "{new_ver}"')
|
||||
confirm = input("Proposed mapd version is the same as the current mapd version. Confirm? (y/n): ").upper().strip()
|
||||
if confirm != "Y":
|
||||
print("Current mapd version and hash will not be updated! (aborted)")
|
||||
exit(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
|
||||
|
||||
update_mapd_version(new_ver, MAPD_VERSION_PATH)
|
||||
update_mapd_hash()
|
||||
if not rewrite_pinned_tag(target):
|
||||
return 1
|
||||
refresh_hash_file()
|
||||
return 0
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
sys.exit(main())
|
||||
|
||||
@@ -2,16 +2,15 @@
|
||||
"""
|
||||
Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
|
||||
|
||||
Installs the `mapd` binary by Jacob Pfeifer (github.com/pfeiferj/mapd) — the binary is his work.
|
||||
Provisions the `mapd` routing binary authored by Jacob Pfeifer (github.com/pfeiferj/mapd).
|
||||
The binary itself is his work; this module only fetches, verifies and stages it on-device.
|
||||
"""
|
||||
import hashlib
|
||||
import logging
|
||||
import os
|
||||
import stat
|
||||
import time
|
||||
import traceback
|
||||
from pathlib import Path
|
||||
from urllib.request import urlopen
|
||||
|
||||
import requests
|
||||
|
||||
@@ -29,125 +28,144 @@ VENDOR_RELEASE_URL = f"https://github.com/pfeiferj/mapd/releases/download/{VENDO
|
||||
|
||||
_VERSION_PARAM = "MapdVersion"
|
||||
_HASH_FILE = os.path.join(BASEDIR, "iqpilot", "iq_maps", "tests", "mapd_hash")
|
||||
_DOWNLOAD_TIMEOUT_S = 60
|
||||
_MAX_DOWNLOAD_TRIES = 5
|
||||
_HTTP_TIMEOUT_S = 60
|
||||
_FETCH_ATTEMPTS = 5
|
||||
_NET_PROBE_ATTEMPTS = 10
|
||||
_NET_PROBE_INTERVAL_S = 2
|
||||
|
||||
|
||||
def get_file_hash(path: str) -> str:
|
||||
"""Hex SHA-256 of a file's contents."""
|
||||
"""Hex SHA-256 digest of a file on disk."""
|
||||
digest = hashlib.sha256()
|
||||
with open(path, "rb") as handle:
|
||||
return hashlib.file_digest(handle, "sha256").hexdigest()
|
||||
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)
|
||||
|
||||
|
||||
def _pinned_hash() -> str:
|
||||
try:
|
||||
with open(_HASH_FILE) as f:
|
||||
return f.read().strip()
|
||||
except OSError:
|
||||
return ""
|
||||
|
||||
|
||||
class VendorMapdInstaller:
|
||||
def __init__(self, spinner_ref: Spinner):
|
||||
self._spinner = spinner_ref
|
||||
self._params = Params()
|
||||
|
||||
# --- externally consumed surface -----------------------------------------
|
||||
def get_installed_version(self) -> str:
|
||||
return str(self._params.get(_VERSION_PARAM) or "")
|
||||
|
||||
@staticmethod
|
||||
def ensure_directories_exist() -> None:
|
||||
for d in (Paths.mapd_root(), VENDOR_MAPD_BIN_DIR):
|
||||
os.makedirs(d, exist_ok=True)
|
||||
|
||||
def download_needed(self) -> bool:
|
||||
if not os.path.exists(VENDOR_MAPD_PATH):
|
||||
return True
|
||||
if self.get_installed_version() != VENDOR_RELEASE_TAG:
|
||||
return True
|
||||
pinned = _pinned_hash()
|
||||
if not pinned:
|
||||
return False
|
||||
try:
|
||||
return get_file_hash(VENDOR_MAPD_PATH) != pinned
|
||||
except OSError:
|
||||
return True
|
||||
for directory in (Paths.mapd_root(), VENDOR_MAPD_BIN_DIR):
|
||||
os.makedirs(directory, exist_ok=True)
|
||||
|
||||
def check_and_download(self) -> None:
|
||||
if self.download_needed():
|
||||
self.fetch()
|
||||
|
||||
def fetch(self) -> None:
|
||||
self.ensure_directories_exist()
|
||||
if self._pull_binary():
|
||||
stamp_vendor_version(VENDOR_RELEASE_TAG, self._params)
|
||||
|
||||
def _pull_binary(self) -> bool:
|
||||
scratch = Path(f"{VENDOR_MAPD_PATH}.tmp")
|
||||
for attempt in range(1, _MAX_DOWNLOAD_TRIES + 1):
|
||||
try:
|
||||
resp = requests.get(VENDOR_RELEASE_URL, stream=True, timeout=_DOWNLOAD_TIMEOUT_S)
|
||||
resp.raise_for_status()
|
||||
with open(scratch, "wb") as out:
|
||||
out.write(resp.content)
|
||||
out.flush()
|
||||
os.fsync(out.fileno())
|
||||
mode = stat.S_IMODE(os.lstat(scratch).st_mode)
|
||||
os.chmod(scratch, mode | stat.S_IEXEC)
|
||||
scratch.replace(VENDOR_MAPD_PATH)
|
||||
return True
|
||||
except requests.exceptions.RequestException as e:
|
||||
self._spinner.update(f"mapd download failed ({e}); retry {attempt}/{_MAX_DOWNLOAD_TRIES}")
|
||||
time.sleep(0.5)
|
||||
scratch.unlink(missing_ok=True)
|
||||
logging.error("mapd binary download failed after %d attempts", _MAX_DOWNLOAD_TRIES)
|
||||
return False
|
||||
|
||||
def wait_for_internet_connection(self, return_on_failure: bool = False) -> bool:
|
||||
attempts = 10
|
||||
for i in range(attempts + 1):
|
||||
self._spinner.update(f"Waiting for internet connection... [{i}/{attempts}]")
|
||||
time.sleep(2)
|
||||
try:
|
||||
urlopen("https://sentry.io", timeout=10)
|
||||
return True
|
||||
except Exception as e: # noqa: BLE001
|
||||
print(f"Wait for internet failed: {e}")
|
||||
if return_on_failure and i == attempts:
|
||||
return False
|
||||
return False
|
||||
if not self._binary_up_to_date():
|
||||
self._provision()
|
||||
|
||||
def non_prebuilt_install(self) -> None:
|
||||
sm = messaging.SubMaster(["deviceState"])
|
||||
if sm["deviceState"].networkMetered:
|
||||
self._spinner.update("Can't proceed with mapd install since network is metered!")
|
||||
if self._on_metered_link():
|
||||
self._say("Metered connection detected — offline maps engine will not download here.")
|
||||
time.sleep(5)
|
||||
return
|
||||
|
||||
try:
|
||||
self.ensure_directories_exist()
|
||||
if not self.download_needed():
|
||||
self._spinner.update("Offline maps binary is ready.")
|
||||
if self._binary_up_to_date():
|
||||
self._say("Offline maps engine already present and current.")
|
||||
time.sleep(0.1)
|
||||
return
|
||||
|
||||
if self.wait_for_internet_connection(return_on_failure=True):
|
||||
self._spinner.update(f"Downloading vendor mapd [{self.get_installed_version()}] => [{VENDOR_RELEASE_TAG}].")
|
||||
if self._block_until_online():
|
||||
self._say(f"Retrieving offline maps engine [{self.get_installed_version() or 'none'}] -> [{VENDOR_RELEASE_TAG}]")
|
||||
time.sleep(0.1)
|
||||
self.check_and_download()
|
||||
self._provision()
|
||||
self._spinner.close()
|
||||
except Exception: # noqa: BLE001
|
||||
for i in range(6):
|
||||
self._spinner.update("Failed to download OSM maps won't work until properly downloaded!"
|
||||
f"Try again manually rebooting. Boot will continue in {5 - i}s...")
|
||||
time.sleep(1)
|
||||
sentry.init(sentry.SentryProject.SELFDRIVE)
|
||||
traceback.print_exc()
|
||||
sentry.capture_exception()
|
||||
except Exception as exc: # noqa: BLE001
|
||||
self._announce_failure(exc)
|
||||
|
||||
# --- internal ------------------------------------------------------------
|
||||
def _expected_hash(self) -> str:
|
||||
try:
|
||||
with open(_HASH_FILE) as f:
|
||||
return f.read().strip()
|
||||
except OSError:
|
||||
return ""
|
||||
|
||||
def _binary_up_to_date(self) -> bool:
|
||||
if not os.path.exists(VENDOR_MAPD_PATH):
|
||||
return False
|
||||
if self.get_installed_version() != VENDOR_RELEASE_TAG:
|
||||
return False
|
||||
reference = self._expected_hash()
|
||||
if not reference:
|
||||
return True
|
||||
try:
|
||||
return get_file_hash(VENDOR_MAPD_PATH) == reference
|
||||
except OSError:
|
||||
return False
|
||||
|
||||
def _provision(self) -> None:
|
||||
self.ensure_directories_exist()
|
||||
if self._retrieve_binary():
|
||||
stamp_vendor_version(VENDOR_RELEASE_TAG, self._params)
|
||||
|
||||
def _retrieve_binary(self) -> bool:
|
||||
staging = Path(f"{VENDOR_MAPD_PATH}.part")
|
||||
last_error: Exception | None = None
|
||||
for attempt in range(1, _FETCH_ATTEMPTS + 1):
|
||||
try:
|
||||
with requests.get(VENDOR_RELEASE_URL, stream=True, timeout=_HTTP_TIMEOUT_S) as resp:
|
||||
resp.raise_for_status()
|
||||
with open(staging, "wb") as out:
|
||||
for chunk in resp.iter_content(chunk_size=1 << 16):
|
||||
out.write(chunk)
|
||||
out.flush()
|
||||
os.fsync(out.fileno())
|
||||
os.chmod(staging, os.lstat(staging).st_mode | stat.S_IXUSR | stat.S_IXGRP | stat.S_IXOTH)
|
||||
staging.replace(VENDOR_MAPD_PATH)
|
||||
return True
|
||||
except requests.exceptions.RequestException as exc:
|
||||
last_error = exc
|
||||
self._say(f"offline maps fetch attempt {attempt}/{_FETCH_ATTEMPTS} did not complete ({exc})")
|
||||
time.sleep(0.5)
|
||||
staging.unlink(missing_ok=True)
|
||||
logging.error("offline maps engine could not be fetched after %d attempts: %s", _FETCH_ATTEMPTS, last_error)
|
||||
return False
|
||||
|
||||
def _on_metered_link(self) -> bool:
|
||||
sm = messaging.SubMaster(["deviceState"])
|
||||
return bool(sm["deviceState"].networkMetered)
|
||||
|
||||
def _block_until_online(self) -> bool:
|
||||
for i in range(1, _NET_PROBE_ATTEMPTS + 1):
|
||||
self._say(f"Waiting for a usable network connection... [{i}/{_NET_PROBE_ATTEMPTS}]")
|
||||
if self._link_reachable():
|
||||
return True
|
||||
time.sleep(_NET_PROBE_INTERVAL_S)
|
||||
return False
|
||||
|
||||
@staticmethod
|
||||
def _link_reachable() -> bool:
|
||||
try:
|
||||
requests.head(VENDOR_RELEASE_URL, timeout=10, allow_redirects=True)
|
||||
return True
|
||||
except requests.exceptions.RequestException as exc:
|
||||
logging.debug("network probe failed: %s", exc)
|
||||
return False
|
||||
|
||||
def _announce_failure(self, exc: Exception) -> None:
|
||||
for remaining in range(5, 0, -1):
|
||||
self._say(f"Offline maps engine unavailable; navigation stays online-only. Boot continues in {remaining}s...")
|
||||
time.sleep(1)
|
||||
logging.exception("vendor mapd install failed")
|
||||
sentry.init(sentry.SentryProject.SELFDRIVE)
|
||||
sentry.capture_exception(exc)
|
||||
|
||||
def _say(self, text: str) -> None:
|
||||
self._spinner.update(text)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
@@ -155,9 +173,9 @@ if __name__ == "__main__":
|
||||
installer = VendorMapdInstaller(spinner)
|
||||
installer.ensure_directories_exist()
|
||||
if is_prebuilt():
|
||||
spinner.update(f"[DEBUG] Prebuilt build; no vendor mapd install required. "
|
||||
f"VERSION: [{VENDOR_RELEASE_TAG}], Param [{installer.get_installed_version()}]")
|
||||
spinner.update(f"[DEBUG] Prebuilt build; vendor mapd install skipped. "
|
||||
f"target [{VENDOR_RELEASE_TAG}], param [{installer.get_installed_version()}]")
|
||||
stamp_vendor_version(VENDOR_RELEASE_TAG)
|
||||
else:
|
||||
spinner.update(f"Checking if vendor mapd is installed and valid. Prebuilt [{is_prebuilt()}]")
|
||||
spinner.update(f"Verifying vendor mapd install. prebuilt [{is_prebuilt()}]")
|
||||
installer.non_prebuilt_install()
|
||||
|
||||
@@ -26,17 +26,15 @@ SpeedLimitSource = custom.IQPlan.SpeedLimit.Source
|
||||
NavProvider = custom.IQNavState.LongitudinalProvider
|
||||
NavLongitudinalState = custom.IQNavState.LongitudinalState
|
||||
|
||||
|
||||
class LongitudinalPlannerIQ:
|
||||
def __init__(self, CP: structs.CarParams, CP_IQ: structs.IQCarParams, mpc):
|
||||
self.events_iq = IQEvents()
|
||||
self.iq_dynamic = IQDynamicController(CP, mpc)
|
||||
self.custom_stop_distance = CustomStopDistance()
|
||||
self.slc = SLCVCruise()
|
||||
self.slimit = SLCVCruise()
|
||||
self.generation = int(model_bundle.generation) if (model_bundle := get_active_bundle()) else None
|
||||
self.source = LongitudinalPlanSource.cruise
|
||||
self.e2e_alerts_helper = E2EAlertsHelper()
|
||||
|
||||
self.iqmodeloutput = E2EAlertsHelper()
|
||||
self.output_v_target = 0.
|
||||
self.output_a_target = 0.
|
||||
self.speed_limit_last = 0.
|
||||
@@ -84,17 +82,17 @@ class LongitudinalPlannerIQ:
|
||||
else:
|
||||
clocks = sm.get('clocks', None) if isinstance(sm, dict) else None
|
||||
time_validated = bool(getattr(clocks, 'timeValid', False))
|
||||
slc_v_cruise = self.slc.update(slc_apply_enabled, now, time_validated, v_cruise, v_ego, sm)
|
||||
self.iq_dynamic.set_slc_experimental_mode(self.slc.slc_experimental_mode)
|
||||
slc_v_cruise = self.slimit.update(slc_apply_enabled, now, time_validated, v_cruise, v_ego, sm)
|
||||
self.iq_dynamic.set_slc_experimental_mode(self.slimit.slc_experimental_mode)
|
||||
self.iq_dynamic.update(sm)
|
||||
# Prefer confirmed controller output for UI/planner rendering.
|
||||
# Fall back to active (policy-resolved) target/source when confirmed is unavailable.
|
||||
display_speed_limit = self.slc.slc_target if self.slc.slc_target > 0 else self.slc.slc_active_target
|
||||
display_source = self.slc.slc_source if self.slc.slc_source != "None" else self.slc.slc_active_source
|
||||
display_speed_limit = self.slimit.slc_target if self.slimit.slc_target > 0 else self.slimit.slc_active_target
|
||||
display_source = self.slimit.slc_source if self.slimit.slc_source != "None" else self.slimit.slc_active_source
|
||||
|
||||
if display_speed_limit > 0:
|
||||
self.speed_limit_last = display_speed_limit
|
||||
self.speed_limit_final_last = display_speed_limit + self.slc.slc_offset
|
||||
self.speed_limit_final_last = display_speed_limit + self.slimit.slc_offset
|
||||
elif display_source == "None":
|
||||
self.speed_limit_last = 0.0
|
||||
self.speed_limit_final_last = 0.0
|
||||
@@ -120,8 +118,8 @@ class LongitudinalPlannerIQ:
|
||||
self.output_v_target, self.output_a_target = targets[self.source]
|
||||
self.output_v_target = self._apply_force_stop(self.output_v_target, v_ego, sm, slc_apply_enabled)
|
||||
# envelope shaping only in Assist mode: info/warn must never change the plan
|
||||
self._envelope_enabled = (slc_apply_enabled and bool(getattr(self.slc, "controller_enabled", False))
|
||||
and bool(getattr(self.slc, "mode_assist", False)))
|
||||
self._envelope_enabled = (slc_apply_enabled and bool(getattr(self.slimit, "controller_enabled", False))
|
||||
and bool(getattr(self.slimit, "mode_assist", False)))
|
||||
return self.output_v_target, self.output_a_target
|
||||
|
||||
def cruise_envelope(self, v_target: float, v_ego: float, t_idxs) -> np.ndarray:
|
||||
@@ -131,12 +129,12 @@ class LongitudinalPlannerIQ:
|
||||
env = np.full(len(t_idxs), max(float(v_target), 0.0))
|
||||
if not getattr(self, "_envelope_enabled", False):
|
||||
return env
|
||||
slc = getattr(self.slc, "slc", None)
|
||||
slc = getattr(self.slimit, "slc", None)
|
||||
next_limit = float(getattr(slc, "next_speed_limit", 0.0) or 0.0)
|
||||
next_dist = float(getattr(slc, "next_speed_distance", 0.0) or 0.0)
|
||||
if next_limit <= 0.0 or next_dist <= 0.0:
|
||||
return env
|
||||
next_target = max(next_limit + float(getattr(self.slc, "slc_offset", 0.0) or 0.0), 0.0)
|
||||
next_target = max(next_limit + float(getattr(self.slimit, "slc_offset", 0.0) or 0.0), 0.0)
|
||||
if next_target >= env[0]:
|
||||
return env
|
||||
travel = np.maximum(v_ego, 1.0) * np.asarray(t_idxs)
|
||||
@@ -145,10 +143,10 @@ class LongitudinalPlannerIQ:
|
||||
|
||||
def update(self, sm: messaging.SubMaster) -> None:
|
||||
self.events_iq.clear()
|
||||
for event_name in getattr(self.slc, 'pending_events', []):
|
||||
for event_name in getattr(self.slimit, 'pending_events', []):
|
||||
self.events_iq.add(event_name)
|
||||
self.custom_stop_distance.update()
|
||||
self.e2e_alerts_helper.update(sm, self.events_iq)
|
||||
self.iqmodeloutput.update(sm, self.events_iq)
|
||||
if bool(getattr(sm["iqCarState"], "alcOverrideAlert", False)):
|
||||
self.events_iq.add(custom.IQOnroadEvent.EventName.steeringOverrideReengageAlc)
|
||||
|
||||
@@ -213,8 +211,8 @@ class LongitudinalPlannerIQ:
|
||||
# Speed Limit
|
||||
speedLimit = plan_msg.speedLimit
|
||||
resolver = speedLimit.resolver
|
||||
speed_limit = float(self.slc.slc_target if self.slc.slc_target > 0 else self.slc.slc_active_target)
|
||||
speed_limit_offset = float(self.slc.slc_offset)
|
||||
speed_limit = float(self.slimit.slc_target if self.slimit.slc_target > 0 else self.slimit.slc_active_target)
|
||||
speed_limit_offset = float(self.slimit.slc_offset)
|
||||
speed_limit_final = speed_limit + speed_limit_offset if speed_limit > 0 else 0.
|
||||
speed_limit_valid = speed_limit > 0.
|
||||
speed_limit_last_valid = self.speed_limit_last > 0.
|
||||
@@ -230,26 +228,26 @@ class LongitudinalPlannerIQ:
|
||||
resolver.source = self.speed_limit_source
|
||||
|
||||
assist = speedLimit.assist
|
||||
slc_assist_state = self.slc.assist_state
|
||||
assist.enabled = bool(self.slc.slc_target > 0 or self.slc.slc_unconfirmed > 0)
|
||||
assist.active = self.source == LongitudinalPlanSource.speedLimitAssist and self.slc.slc_target > 0
|
||||
slc_assist_state = self.slimit.assist_state
|
||||
assist.enabled = bool(self.slimit.slc_target > 0 or self.slimit.slc_unconfirmed > 0)
|
||||
assist.active = self.source == LongitudinalPlanSource.speedLimitAssist and self.slimit.slc_target > 0
|
||||
if slc_assist_state is not None:
|
||||
assist.state = slc_assist_state
|
||||
elif not assist.enabled:
|
||||
assist.state = SpeedLimitAssistState.disabled
|
||||
elif self.slc.slc_unconfirmed > 0:
|
||||
elif self.slimit.slc_unconfirmed > 0:
|
||||
assist.state = SpeedLimitAssistState.preActive
|
||||
elif assist.active:
|
||||
assist.state = SpeedLimitAssistState.active
|
||||
else:
|
||||
assist.state = SpeedLimitAssistState.inactive
|
||||
assist.vTarget = float(self.output_v_target if assist.active else 255.)
|
||||
assist.aTarget = float(self.slc.slc_a_target if assist.active else 0.)
|
||||
assist.aTarget = float(self.slimit.slc_a_target if assist.active else 0.)
|
||||
|
||||
# E2E Alerts
|
||||
e2eAlerts = plan_msg.e2eAlerts
|
||||
e2eAlerts.greenLightAlert = self.e2e_alerts_helper.queue_alert
|
||||
e2eAlerts.leadDepartAlert = self.e2e_alerts_helper.lead_alert
|
||||
e2eAlerts.greenLightAlert = self.iqmodeloutput.queue_alert
|
||||
e2eAlerts.leadDepartAlert = self.iqmodeloutput.lead_alert
|
||||
|
||||
valid = sm.all_checks(service_list=['carState', 'controlsState'])
|
||||
|
||||
|
||||
Reference in New Issue
Block a user