diff --git a/iqdbc_repo/iqdbc/car/volkswagen/carstate.py b/iqdbc_repo/iqdbc/car/volkswagen/carstate.py index a6aedf7..9015eab 100644 --- a/iqdbc_repo/iqdbc/car/volkswagen/carstate.py +++ b/iqdbc_repo/iqdbc/car/volkswagen/carstate.py @@ -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) diff --git a/iqpilot/iq_maps/update_vendor_version.py b/iqpilot/iq_maps/update_vendor_version.py index a5eb7d2..2b1675d 100755 --- a/iqpilot/iq_maps/update_vendor_version.py +++ b/iqpilot/iq_maps/update_vendor_version.py @@ -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()) diff --git a/iqpilot/iq_maps/vendor_mapd_installer.py b/iqpilot/iq_maps/vendor_mapd_installer.py index a76bcb9..1964939 100755 --- a/iqpilot/iq_maps/vendor_mapd_installer.py +++ b/iqpilot/iq_maps/vendor_mapd_installer.py @@ -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() diff --git a/iqpilot/selfdrive/controls/lib/longitudinal_planner.py b/iqpilot/selfdrive/controls/lib/longitudinal_planner.py index d12405d..8688530 100644 --- a/iqpilot/selfdrive/controls/lib/longitudinal_planner.py +++ b/iqpilot/selfdrive/controls/lib/longitudinal_planner.py @@ -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'])